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 | |
Harald Welte | 6daa71d | 2016-11-14 21:26:11 +0100 | [diff] [blame] | 21 | static const ubit_t flag_octet[] = { 0,1,1,1,1,1,1,0 }; |
| 22 | static const ubit_t five_ones_zero[] = { 0,1,1,1,1,1 }; |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 23 | |
Harald Welte | 6daa71d | 2016-11-14 21:26:11 +0100 | [diff] [blame] | 24 | static void append_bit_history(struct hdlc_proc *hdlc, uint8_t bit) |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 25 | { |
Harald Welte | 90fb785 | 2016-11-14 19:34:30 +0100 | [diff] [blame] | 26 | /* we always add the bit to the history */ |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 27 | memmove(hdlc->history+1, hdlc->history, sizeof(hdlc->history)-1); |
| 28 | hdlc->history[0] = bit; |
Harald Welte | 6daa71d | 2016-11-14 21:26:11 +0100 | [diff] [blame] | 29 | } |
Harald Welte | 90fb785 | 2016-11-14 19:34:30 +0100 | [diff] [blame] | 30 | |
Harald Welte | 6daa71d | 2016-11-14 21:26:11 +0100 | [diff] [blame] | 31 | static int append_bit_out(struct hdlc_proc *hdlc, uint8_t bit) |
| 32 | { |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 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; |
Harald Welte | 90bd7b6 | 2016-11-14 22:06:46 +0100 | [diff] [blame] | 39 | hdlc->num_bits = 0; |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 40 | /* generate one output byte */ |
| 41 | osmo_ubit2pbit_ext(&out, 0, hdlc->next_outbyte, 0, 8, 0); |
Harald Welte | 90bd7b6 | 2016-11-14 22:06:46 +0100 | [diff] [blame] | 42 | /* append to output buffer */ |
| 43 | OSMO_ASSERT(hdlc->out.len < sizeof(hdlc->out.buf)); |
| 44 | hdlc->out.buf[hdlc->out.len++] = out; |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 45 | return out; |
| 46 | } |
| 47 | |
| 48 | return -1; |
| 49 | } |
| 50 | |
| 51 | static int process_hdlc_bit(struct hdlc_proc *hdlc, uint8_t bit) |
| 52 | { |
Harald Welte | 6daa71d | 2016-11-14 21:26:11 +0100 | [diff] [blame] | 53 | int out = -1, flag = 0; |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 54 | |
| 55 | DEBUGP("bit=%u, history_in = %s, ", bit, osmo_ubit_dump(hdlc->history, sizeof(hdlc->history))); |
| 56 | |
Harald Welte | 6daa71d | 2016-11-14 21:26:11 +0100 | [diff] [blame] | 57 | /* always append bit to history */ |
| 58 | append_bit_history(hdlc, bit); |
| 59 | |
| 60 | if (!memcmp(flag_octet, hdlc->history, sizeof(flag_octet))) { |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 61 | hdlc->num_bits = 0; |
Harald Welte | 6daa71d | 2016-11-14 21:26:11 +0100 | [diff] [blame] | 62 | DEBUGP("S "); |
| 63 | flag = 1; |
Harald Welte | 90bd7b6 | 2016-11-14 22:06:46 +0100 | [diff] [blame] | 64 | if (hdlc->out.len) { |
| 65 | /* call output function if any frame was |
| 66 | * received before the flag octet */ |
| 67 | if (hdlc->out_cb) |
| 68 | hdlc->out_cb(hdlc->out.buf, hdlc->out.len, |
| 69 | hdlc->priv); |
| 70 | hdlc->out.len = 0; |
| 71 | } |
Harald Welte | 6daa71d | 2016-11-14 21:26:11 +0100 | [diff] [blame] | 72 | } else if (!memcmp(five_ones_zero, hdlc->history, sizeof(five_ones_zero))) { |
| 73 | /* 4.3.1 Synchronous transmission: receiver shall |
| 74 | * discard any "0" bit after five contiguous ones */ |
| 75 | DEBUGP("I "); |
| 76 | } else { |
| 77 | out = append_bit_out(hdlc, bit); |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 78 | } |
Harald Welte | 6daa71d | 2016-11-14 21:26:11 +0100 | [diff] [blame] | 79 | |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 80 | DEBUGP("history_out = %s", osmo_ubit_dump(hdlc->history, sizeof(hdlc->history))); |
Harald Welte | 6daa71d | 2016-11-14 21:26:11 +0100 | [diff] [blame] | 81 | if (out >= 0) |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 82 | DEBUGP(", out 0x%02x\n", out); |
| 83 | else |
| 84 | DEBUGP("\n"); |
| 85 | |
| 86 | if (flag) |
| 87 | return -123; |
| 88 | else |
| 89 | return out; |
| 90 | } |
| 91 | |
Harald Welte | 90bd7b6 | 2016-11-14 22:06:46 +0100 | [diff] [blame] | 92 | int process_raw_hdlc(struct hdlc_proc *hdlc, ubit_t *bits, unsigned int len) |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 93 | { |
| 94 | unsigned int i; |
| 95 | int out; |
| 96 | static int last_out; |
| 97 | |
Harald Welte | 90bd7b6 | 2016-11-14 22:06:46 +0100 | [diff] [blame] | 98 | DEBUGP("process_raw_hdlc(%s)\n", osmo_hexdump(bits, len)); |
Harald Welte | 6daa71d | 2016-11-14 21:26:11 +0100 | [diff] [blame] | 99 | |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 100 | for (i = 0; i < len; i ++) { |
Harald Welte | 90bd7b6 | 2016-11-14 22:06:46 +0100 | [diff] [blame] | 101 | out = process_hdlc_bit(hdlc, bits[i]); |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 102 | if (out == -123) { |
| 103 | /* suppress repeating Flag characters */ |
| 104 | if (last_out != out) |
| 105 | printf("\nF "); |
| 106 | last_out = out; |
| 107 | } else if (out >= 0) { |
| 108 | /* suppress 0xAA and 0x55 bit pattern */ |
Harald Welte | 6daa71d | 2016-11-14 21:26:11 +0100 | [diff] [blame] | 109 | //if (out != 0xaa && out != 0x55) |
Harald Welte | d83c513 | 2016-10-26 09:38:24 +0200 | [diff] [blame] | 110 | printf("%02x ", out); |
| 111 | last_out = out; |
| 112 | } |
| 113 | } |
| 114 | return 0; |
| 115 | } |