From 6e6539571fd4acfeb108efb3d9654dbb097c50da Mon Sep 17 00:00:00 2001 From: max Date: Wed, 25 Sep 2013 03:33:58 +0000 Subject: [PATCH] trunking git-svn-id: http://op25.osmocom.org/svn/trunk@325 65a5c917-d112-43f1-993d-58c26a4786be --- repeater/src/lib/p25_framer.h | 2 +- .../src/lib/repeater_p25_frame_assembler.cc | 179 +++++++++++++++++- .../src/lib/repeater_p25_frame_assembler.h | 1 + 3 files changed, 179 insertions(+), 3 deletions(-) diff --git a/repeater/src/lib/p25_framer.h b/repeater/src/lib/p25_framer.h index 8434ffb..ddaf71c 100644 --- a/repeater/src/lib/p25_framer.h +++ b/repeater/src/lib/p25_framer.h @@ -39,7 +39,7 @@ public: uint32_t nac; // extracted NAC uint32_t duid; // extracted DUID bit_vector frame_body; // all bits in frame - size_t frame_size; // number of bits in frame_body + uint32_t frame_size; // number of bits in frame_body uint32_t bch_errors; // number of errors detected in bch }; diff --git a/repeater/src/lib/repeater_p25_frame_assembler.cc b/repeater/src/lib/repeater_p25_frame_assembler.cc index 0e16478..200704d 100644 --- a/repeater/src/lib/repeater_p25_frame_assembler.cc +++ b/repeater/src/lib/repeater_p25_frame_assembler.cc @@ -20,7 +20,7 @@ * Boston, MA 02111-1307, USA. */ /* - * Copyright 2010, KA1RBI + * Copyright 2010, 2011, 2012, 2013 KA1RBI */ /* * config.h is generated by configure. It contains the results @@ -62,6 +62,133 @@ repeater_make_p25_frame_assembler (const char* udp_host, int port, int debug, bo static const int MIN_IN = 1; // mininum number of input streams static const int MAX_IN = 1; // maximum number of input streams +static uint16_t crc16(uint8_t buf[], int len) { + uint32_t poly = (1<<12) + (1<<5) + (1<<0); + uint32_t crc = 0; + for(int i=0; i> (7-j)) & 1; + crc = ((crc << 1) | bit) & 0x1ffff; + if (crc & 0x10000) + crc = (crc & 0xffff) ^ poly; + } + } + crc = crc ^ 0xffff; + return crc & 0xffff; +} + +/* find_min is from wireshark/plugins/p25/packet-p25cai.c */ +/* Copyright 2008, Michael Ossmann */ +/* return the index of the lowest value in a list */ +static int +find_min(uint8_t list[], int len) +{ + int min = list[0]; + int index = 0; + int unique = 1; + int i; + + for (i = 1; i < len; i++) { + if (list[i] < min) { + min = list[i]; + index = i; + unique = 1; + } else if (list[i] == min) { + unique = 0; + } + } + /* return -1 if a minimum can't be found */ + if (!unique) + return -1; + + return index; +} + +/* count_bits is from wireshark/plugins/p25/packet-p25cai.c */ +/* Copyright 2008, Michael Ossmann */ +/* count the number of 1 bits in an int */ +static int +count_bits(unsigned int n) +{ + int i = 0; + for (i = 0; n != 0; i++) + n &= n - 1; + return i; +} + +/* adapted from wireshark/plugins/p25/packet-p25cai.c */ +/* Copyright 2008, Michael Ossmann */ +/* deinterleave and trellis1_2 decoding */ +/* tsbk_buf is assumed to be a buffer of 12 bytes */ +static int +tsbk_deinterleave(bit_vector& bv, unsigned int start, uint8_t* tsbk_buf) +{ + static const uint16_t deinterleave_tb[] = { + 0, 1, 2, 3, 52, 53, 54, 55, 100,101,102,103, 148,149,150,151, + 4, 5, 6, 7, 56, 57, 58, 59, 104,105,106,107, 152,153,154,155, + 8, 9, 10, 11, 60, 61, 62, 63, 108,109,110,111, 156,157,158,159, + 12, 13, 14, 15, 64, 65, 66, 67, 112,113,114,115, 160,161,162,163, + 16, 17, 18, 19, 68, 69, 70, 71, 116,117,118,119, 164,165,166,167, + 20, 21, 22, 23, 72, 73, 74, 75, 120,121,122,123, 168,169,170,171, + 24, 25, 26, 27, 76, 77, 78, 79, 124,125,126,127, 172,173,174,175, + 28, 29, 30, 31, 80, 81, 82, 83, 128,129,130,131, 176,177,178,179, + 32, 33, 34, 35, 84, 85, 86, 87, 132,133,134,135, 180,181,182,183, + 36, 37, 38, 39, 88, 89, 90, 91, 136,137,138,139, 184,185,186,187, + 40, 41, 42, 43, 92, 93, 94, 95, 140,141,142,143, 188,189,190,191, + 44, 45, 46, 47, 96, 97, 98, 99, 144,145,146,147, 192,193,194,195, + 48, 49, 50, 51 }; + + uint8_t hd[4]; + int b, d, j; + int state = 0; + uint8_t codeword; + uint16_t crc; + + static const uint8_t next_words[4][4] = { + {0x2, 0xC, 0x1, 0xF}, + {0xE, 0x0, 0xD, 0x3}, + {0x9, 0x7, 0xA, 0x4}, + {0x5, 0xB, 0x6, 0x8} + }; + + memset(tsbk_buf, 0, 12); + + for (b=0; b < 98*2; b += 4) { + codeword = (bv[start+deinterleave_tb[b+0]] << 3) + + (bv[start+deinterleave_tb[b+1]] << 2) + + (bv[start+deinterleave_tb[b+2]] << 1) + + bv[start+deinterleave_tb[b+3]] ; + + /* try each codeword in a row of the state transition table */ + for (j = 0; j < 4; j++) { + /* find Hamming distance for candidate */ + hd[j] = count_bits(codeword ^ next_words[state][j]); + } + /* find the dibit that matches the most codeword bits (minimum Hamming distance) */ + state = find_min(hd, 4); + /* error if minimum can't be found */ + if(state == -1) + return -1; // decode error, return failure + /* It also might be nice to report a condition where the minimum is + * non-zero, i.e. an error has been corrected. It probably shouldn't + * be a permanent failure, though. + * + * DISSECTOR_ASSERT(hd[state] == 0); + */ + + /* append dibit onto output buffer */ + d = b >> 2; // dibit ctr + if (d < 48) { + tsbk_buf[d >> 2] |= state << (6 - ((d%4) * 2)); + } + } + crc = crc16(tsbk_buf, 12); + if (crc != 0) + return -1; // trellis decode OK, but CRC error occurred + return 0; // return OK code +} + /* * The private constructor */ @@ -76,6 +203,7 @@ repeater_p25_frame_assembler::repeater_p25_frame_assembler (const char* udp_host d_debug(debug), d_do_imbe(do_imbe), d_do_output(do_output), + d_do_msgq(do_msgq), d_msg_queue(queue), symbol_queue(), framer(new p25_framer()) @@ -102,6 +230,21 @@ repeater_p25_frame_assembler::forecast(int nof_output_items, gr_vector_int &nof_ nof_samples_reqd = 3.0 * nof_output_items; std::fill(&nof_input_items_reqd[0], &nof_input_items_reqd[nof_inputs], nof_samples_reqd); } +void +repeater_p25_frame_assembler::process_tsbk(uint32_t const nac, uint8_t const tsbk_buf[]) +{ + char wbuf[12]; + if (!d_do_msgq) + return; + if (d_msg_queue->full_p()) + return; + memcpy(wbuf, tsbk_buf, 10); + wbuf[10] = (nac >> 8) & 0xff; + wbuf[11] = nac & 0xff; + gr_message_sptr msg = gr_make_message_from_string(std::string(wbuf, 12), 0, 0, 0); + d_msg_queue->insert_tail(msg); +// msg.reset(); +} int repeater_p25_frame_assembler::general_work (int noutput_items, @@ -115,7 +258,39 @@ repeater_p25_frame_assembler::general_work (int noutput_items, for (int i = 0; i < noutput_items; i++){ if(framer->rx_sym(in[i])) { // complete frame was detected if (d_debug >= 10) { - fprintf (stderr, "NAC 0x%X DUID 0x%X len %d errs %d ", framer->nac, framer->duid, framer->frame_size >> 1, framer->bch_errors); + fprintf (stderr, "NAC 0x%X DUID 0x%X len %u errs %u ", framer->nac, framer->duid, framer->frame_size >> 1, framer->bch_errors); + } + if (framer->duid == 0x07 && framer->bch_errors >= 0) { + unsigned int d, b; + int rc; + bit_vector bv1(720); + uint8_t tsbk_buf[12]; + + if (framer->frame_size > 720) { + fprintf(stderr, "warning trunk frame size %u exceeds maximum\n", framer->frame_size); + framer->frame_size = 720; + } + for (d=0, b=0; d < framer->frame_size >> 1; d++) { + if ((d+1) % 36 == 0) + continue; // skip SS + bv1[b++] = framer->frame_body[d*2]; + bv1[b++] = framer->frame_body[d*2+1]; + } + if (framer->frame_size >= 360) { + rc = tsbk_deinterleave(bv1,48+64 , tsbk_buf); + if (rc == 0) + process_tsbk(framer->nac, tsbk_buf); + } + if (framer->frame_size >= 576) { + rc = tsbk_deinterleave(bv1,48+64+196 , tsbk_buf); + if (rc == 0) + process_tsbk(framer->nac, tsbk_buf); + } + if (framer->frame_size >= 720) { + rc = tsbk_deinterleave(bv1,48+64+196+196, tsbk_buf); + if (rc == 0) + process_tsbk(framer->nac, tsbk_buf); + } } if (d_debug >= 10 && framer->duid == 0x00) { ProcHDU(framer->frame_body); diff --git a/repeater/src/lib/repeater_p25_frame_assembler.h b/repeater/src/lib/repeater_p25_frame_assembler.h index 002c5f7..8b0dc79 100644 --- a/repeater/src/lib/repeater_p25_frame_assembler.h +++ b/repeater/src/lib/repeater_p25_frame_assembler.h @@ -77,6 +77,7 @@ private: typedef std::vector bit_vector; bool header_codeword(uint64_t acc, uint32_t& nac, uint32_t& duid); void proc_voice_unit(bit_vector& frame_body) ; + void process_tsbk(uint32_t const nac, uint8_t const tsbk_buf[]); // internal instance variables and state int write_bufp; int write_sock;