Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 1 | #include <stdint.h> |
| 2 | #include <stdio.h> |
| 3 | #include <stdlib.h> |
| 4 | #include <string.h> |
| 5 | |
| 6 | #include <sys/types.h> |
| 7 | #include <sys/stat.h> |
| 8 | #include <fcntl.h> |
| 9 | |
| 10 | #include <osmocom/core/utils.h> |
| 11 | #include <osmocom/core/bits.h> |
| 12 | |
| 13 | #include "hdlc.h" |
| 14 | |
| 15 | #if 0 |
| 16 | #define DEBUGP(x, args ...) fprintf(stderr, x, ## args) |
| 17 | #else |
| 18 | #define DEBUGP(x, args ...) do {} while (0) |
| 19 | #endif |
| 20 | |
| 21 | static const ubit_t five_ones[] = { 1,1,1,1,1 }; |
| 22 | |
| 23 | static int append_bit(struct hdlc_proc *hdlc, uint8_t bit, int ignore) |
| 24 | { |
Harald Welte | 90fb785 | 2016-11-14 19:34:30 +0100 | [diff] [blame^] | 25 | /* we always add the bit to the history */ |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 26 | memmove(hdlc->history+1, hdlc->history, sizeof(hdlc->history)-1); |
| 27 | hdlc->history[0] = bit; |
Harald Welte | 90fb785 | 2016-11-14 19:34:30 +0100 | [diff] [blame^] | 28 | |
| 29 | /* if it is a to-be-discarded bit, we bail out */ |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 30 | if (ignore) |
| 31 | return -1; |
| 32 | |
| 33 | memmove(hdlc->next_outbyte+1, hdlc->next_outbyte, sizeof(hdlc->next_outbyte)-1); |
| 34 | hdlc->next_outbyte[0] = bit; |
| 35 | hdlc->num_bits++; |
| 36 | |
| 37 | if (hdlc->num_bits == 8) { |
| 38 | pbit_t out; |
| 39 | /* generate one output byte */ |
| 40 | osmo_ubit2pbit_ext(&out, 0, hdlc->next_outbyte, 0, 8, 0); |
| 41 | hdlc->num_bits = 0; |
| 42 | return out; |
| 43 | } |
| 44 | |
| 45 | return -1; |
| 46 | } |
| 47 | |
| 48 | static int process_hdlc_bit(struct hdlc_proc *hdlc, uint8_t bit) |
| 49 | { |
| 50 | int ignore = 0; |
| 51 | int out, flag = 0; |
| 52 | |
| 53 | DEBUGP("bit=%u, history_in = %s, ", bit, osmo_ubit_dump(hdlc->history, sizeof(hdlc->history))); |
| 54 | |
| 55 | switch (hdlc->state) { |
| 56 | case STATE_FLAG_WAIT_ZERO: |
Harald Welte | 90fb785 | 2016-11-14 19:34:30 +0100 | [diff] [blame^] | 57 | /* we've received 6 consecutive '1' just before and are |
| 58 | * waiting for the final '0' to end the flag character */ |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 59 | if (bit == 0) { |
Harald Welte | 90fb785 | 2016-11-14 19:34:30 +0100 | [diff] [blame^] | 60 | /* it was a zero, flag character detected */ |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 61 | DEBUGP("F "); |
| 62 | flag = 1; |
Harald Welte | 90fb785 | 2016-11-14 19:34:30 +0100 | [diff] [blame^] | 63 | /* we're now inside the payload state */ |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 64 | hdlc->state = STATE_PAYLOAD; |
| 65 | } else { |
Harald Welte | 90fb785 | 2016-11-14 19:34:30 +0100 | [diff] [blame^] | 66 | /* if we received yet another '1', we re-start |
| 67 | * from the beginning */ |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 68 | hdlc->state = STATE_INIT; |
| 69 | } |
| 70 | ignore = 1; |
| 71 | hdlc->num_bits = 0; |
| 72 | break; |
Harald Welte | 90fb785 | 2016-11-14 19:34:30 +0100 | [diff] [blame^] | 73 | case STATE_PAYLOAD: |
| 74 | case STATE_INIT: |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 75 | if (!memcmp(five_ones, hdlc->history, sizeof(five_ones))) { |
Harald Welte | 90fb785 | 2016-11-14 19:34:30 +0100 | [diff] [blame^] | 76 | /* five consecutive ones in the history */ |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 77 | if (bit == 1) { |
Harald Welte | 90fb785 | 2016-11-14 19:34:30 +0100 | [diff] [blame^] | 78 | /* one more '1' was received -> we wait |
| 79 | * for a zero at the end of the flag |
| 80 | * character 0x7E */ |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 81 | hdlc->state = STATE_FLAG_WAIT_ZERO; |
Harald Welte | 90fb785 | 2016-11-14 19:34:30 +0100 | [diff] [blame^] | 82 | /* discard bit */ |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 83 | ignore = 1; |
| 84 | } else { |
| 85 | /* discard bit */ |
| 86 | ignore = 1; |
| 87 | } |
| 88 | } |
| 89 | break; |
| 90 | } |
| 91 | out = append_bit(hdlc, bit, ignore); |
| 92 | DEBUGP("history_out = %s", osmo_ubit_dump(hdlc->history, sizeof(hdlc->history))); |
| 93 | if (out > 0) |
| 94 | DEBUGP(", out 0x%02x\n", out); |
| 95 | else |
| 96 | DEBUGP("\n"); |
| 97 | |
| 98 | if (flag) |
| 99 | return -123; |
| 100 | else |
| 101 | return out; |
| 102 | } |
| 103 | |
| 104 | int process_raw_hdlc(struct hdlc_proc *hdlc, uint8_t *data, unsigned int len) |
| 105 | { |
| 106 | unsigned int i; |
| 107 | int out; |
| 108 | static int last_out; |
| 109 | |
| 110 | for (i = 0; i < len; i ++) { |
| 111 | out = process_hdlc_bit(hdlc, data[i]); |
| 112 | if (out == -123) { |
| 113 | /* suppress repeating Flag characters */ |
| 114 | if (last_out != out) |
| 115 | printf("\nF "); |
| 116 | last_out = out; |
| 117 | } else if (out >= 0) { |
| 118 | /* suppress 0xAA and 0x55 bit pattern */ |
| 119 | if (out != 0xaa && out != 0x55) |
| 120 | printf("%02x ", out); |
| 121 | last_out = out; |
| 122 | } |
| 123 | } |
| 124 | return 0; |
| 125 | } |