| 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 | { |
| 25 | memmove(hdlc->history+1, hdlc->history, sizeof(hdlc->history)-1); |
| 26 | hdlc->history[0] = bit; |
| 27 | if (ignore) |
| 28 | return -1; |
| 29 | |
| 30 | memmove(hdlc->next_outbyte+1, hdlc->next_outbyte, sizeof(hdlc->next_outbyte)-1); |
| 31 | hdlc->next_outbyte[0] = bit; |
| 32 | hdlc->num_bits++; |
| 33 | |
| 34 | if (hdlc->num_bits == 8) { |
| 35 | pbit_t out; |
| 36 | /* generate one output byte */ |
| 37 | osmo_ubit2pbit_ext(&out, 0, hdlc->next_outbyte, 0, 8, 0); |
| 38 | hdlc->num_bits = 0; |
| 39 | return out; |
| 40 | } |
| 41 | |
| 42 | return -1; |
| 43 | } |
| 44 | |
| 45 | static int process_hdlc_bit(struct hdlc_proc *hdlc, uint8_t bit) |
| 46 | { |
| 47 | int ignore = 0; |
| 48 | int out, flag = 0; |
| 49 | |
| 50 | DEBUGP("bit=%u, history_in = %s, ", bit, osmo_ubit_dump(hdlc->history, sizeof(hdlc->history))); |
| 51 | |
| 52 | switch (hdlc->state) { |
| 53 | case STATE_FLAG_WAIT_ZERO: |
| 54 | if (bit == 0) { |
| 55 | DEBUGP("F "); |
| 56 | flag = 1; |
| 57 | hdlc->state = STATE_PAYLOAD; |
| 58 | } else { |
| 59 | hdlc->state = STATE_INIT; |
| 60 | } |
| 61 | ignore = 1; |
| 62 | hdlc->num_bits = 0; |
| 63 | break; |
| 64 | default: |
| 65 | if (!memcmp(five_ones, hdlc->history, sizeof(five_ones))) { |
| 66 | if (bit == 1) { |
| 67 | //DEBUGP("F "); |
| 68 | hdlc->state = STATE_FLAG_WAIT_ZERO; |
| 69 | ignore = 1; |
| 70 | } else { |
| 71 | /* discard bit */ |
| 72 | ignore = 1; |
| 73 | } |
| 74 | } |
| 75 | break; |
| 76 | } |
| 77 | out = append_bit(hdlc, bit, ignore); |
| 78 | DEBUGP("history_out = %s", osmo_ubit_dump(hdlc->history, sizeof(hdlc->history))); |
| 79 | if (out > 0) |
| 80 | DEBUGP(", out 0x%02x\n", out); |
| 81 | else |
| 82 | DEBUGP("\n"); |
| 83 | |
| 84 | if (flag) |
| 85 | return -123; |
| 86 | else |
| 87 | return out; |
| 88 | } |
| 89 | |
| 90 | int process_raw_hdlc(struct hdlc_proc *hdlc, uint8_t *data, unsigned int len) |
| 91 | { |
| 92 | unsigned int i; |
| 93 | int out; |
| 94 | static int last_out; |
| 95 | |
| 96 | for (i = 0; i < len; i ++) { |
| 97 | out = process_hdlc_bit(hdlc, data[i]); |
| 98 | if (out == -123) { |
| 99 | /* suppress repeating Flag characters */ |
| 100 | if (last_out != out) |
| 101 | printf("\nF "); |
| 102 | last_out = out; |
| 103 | } else if (out >= 0) { |
| 104 | /* suppress 0xAA and 0x55 bit pattern */ |
| 105 | if (out != 0xaa && out != 0x55) |
| 106 | printf("%02x ", out); |
| 107 | last_out = out; |
| 108 | } |
| 109 | } |
| 110 | return 0; |
| 111 | } |