diff --git a/lib/op25_repeater/lib/dmr_slot.h b/lib/op25_repeater/lib/dmr_slot.h index 36d9d0236..28dac3cea 100644 --- a/lib/op25_repeater/lib/dmr_slot.h +++ b/lib/op25_repeater/lib/dmr_slot.h @@ -104,7 +104,7 @@ class dmr_slot { int d_slot_mask; int d_src_id; bool d_terminated; - log_ts logts; + log_ts& logts; CBPTC19696 bptc; CDMRTrellis trellis; ezpwd::RS<255,252> rs12; // Reed-Solomon(12,9) object for Link Control decode diff --git a/lib/op25_repeater/lib/p25_crypt_algs.cc b/lib/op25_repeater/lib/p25_crypt_algs.cc index 7f7caa818..d3e6f19e6 100644 --- a/lib/op25_repeater/lib/p25_crypt_algs.cc +++ b/lib/op25_repeater/lib/p25_crypt_algs.cc @@ -33,7 +33,7 @@ p25_crypt_algs::p25_crypt_algs(log_ts& logger, int debug, int msgq_id) : logts(logger), d_debug(debug), d_msgq_id(msgq_id), - d_fr_type(FT_UNK), + d_pr_type(PT_UNK), d_algid(0x80), d_keyid(0), d_mi{0}, @@ -59,7 +59,7 @@ void p25_crypt_algs::key(uint16_t keyid, uint8_t algid, const std::vector key; }; -enum frame_type { FT_UNK = 0, FT_LDU1, FT_LDU2, FT_2V, FT_4V }; +enum frame_type { FT_UNK = 0, FT_LDU1, FT_LDU2, FT_2V, FT_4V_0, FT_4V_1, FT_4V_2, FT_4V_3 }; +enum protocol_type { PT_UNK = 0, PT_P25_PHASE1, PT_P25_PHASE2 }; class p25_crypt_algs { @@ -44,16 +45,16 @@ class p25_crypt_algs log_ts& logts; int d_debug; int d_msgq_id; - frame_type d_fr_type; + protocol_type d_pr_type; uint8_t d_algid; uint16_t d_keyid; uint8_t d_mi[9]; std::unordered_map d_keys; std::unordered_map::const_iterator d_key_iter; uint8_t adp_keystream[469]; - int d_adp_position; + uint32_t d_adp_position; - bool adp_process(packed_codeword& PCW); + bool adp_process(packed_codeword& PCW, frame_type fr_type, int voice_subframe); void adp_keystream_gen(); void adp_swap(uint8_t *S, uint32_t i, uint32_t j); @@ -62,8 +63,8 @@ class p25_crypt_algs ~p25_crypt_algs(); void key(uint16_t keyid, uint8_t algid, const std::vector &key); - bool prepare(uint8_t algid, uint16_t keyid, frame_type fr_type, uint8_t *MI); - bool process(packed_codeword& PCW); + bool prepare(uint8_t algid, uint16_t keyid, protocol_type pr_type, uint8_t *MI); + bool process(packed_codeword& PCW, frame_type fr_type, int voice_subframe); void reset(void); inline void set_debug(int debug) {d_debug = debug;} }; diff --git a/lib/op25_repeater/lib/p25p1_fdma.cc b/lib/op25_repeater/lib/p25p1_fdma.cc index 0560d2755..46fc2f09b 100644 --- a/lib/op25_repeater/lib/p25p1_fdma.cc +++ b/lib/op25_repeater/lib/p25p1_fdma.cc @@ -404,6 +404,7 @@ namespace gr { void p25p1_fdma::process_TTDU() { process_duid(framer->duid, framer->nac, NULL, 0); + reset_ess(); if ((d_do_imbe || d_do_audio_output) && (framer->duid == 0x3 || framer->duid == 0xf)) { // voice termination op25audio.send_audio_flag(op25_audio::DRAIN); @@ -654,7 +655,7 @@ namespace gr { void p25p1_fdma::process_voice(const bit_vector& A, const frame_type fr_type) { if (d_do_imbe || d_do_audio_output) { if (encrypted()) - crypt_algs.prepare(ess_algid, ess_keyid, fr_type, ess_mi); + crypt_algs.prepare(ess_algid, ess_keyid, PT_P25_PHASE1, ess_mi); for(size_t i = 0; i < nof_voice_codewords; ++i) { voice_codeword cw(voice_codeword_sz); @@ -680,7 +681,7 @@ namespace gr { if (encrypted()) { packed_codeword ciphertext; imbe_pack(ciphertext, u[0], u[1], u[2], u[3], u[4], u[5], u[6], u[7]); - audio_valid = crypt_algs.process(ciphertext); + audio_valid = crypt_algs.process(ciphertext, fr_type, i); imbe_unpack(ciphertext, u[0], u[1], u[2], u[3], u[4], u[5], u[6], u[7]); } @@ -777,6 +778,12 @@ namespace gr { qtimer.reset(); } + void p25p1_fdma::call_end() { + if (d_do_audio_output) + op25audio.send_audio_flag(op25_audio::DRAIN); + reset_ess(); + } + void p25p1_fdma::crypt_reset() { crypt_algs.reset(); } diff --git a/lib/op25_repeater/lib/p25p1_fdma.h b/lib/op25_repeater/lib/p25p1_fdma.h index 94e8007ec..3d1689db6 100644 --- a/lib/op25_repeater/lib/p25p1_fdma.h +++ b/lib/op25_repeater/lib/p25p1_fdma.h @@ -69,6 +69,7 @@ namespace gr { void process_frame(); void check_timeout(); inline bool encrypted() { return (ess_algid != 0x80); } + inline void reset_ess() { ess_algid = 0x80; memset(ess_mi, 0, sizeof(ess_mi)); } void send_msg(const std::string msg_str, long msg_type); // internal instance variables and state @@ -116,6 +117,7 @@ namespace gr { void set_debug(int debug); void set_nac(int nac); void reset_timer(); + void call_end(); void crypt_reset(); void crypt_key(uint16_t keyid, uint8_t algid, const std::vector &key); void rx_sym (const uint8_t *syms, int nsyms); diff --git a/lib/op25_repeater/lib/p25p2_isch.cc b/lib/op25_repeater/lib/p25p2_isch.cc index 2429d0e72..90dd5ae70 100644 --- a/lib/op25_repeater/lib/p25p2_isch.cc +++ b/lib/op25_repeater/lib/p25p2_isch.cc @@ -42,134 +42,135 @@ class p25p2_isch p25p2_isch::p25p2_isch(void) { - isch_map["184229d461"] = 0; - isch_map["18761451f6"] = 1; - isch_map["181ae27e2f"] = 2; - isch_map["182edffbb8"] = 3; - isch_map["18df8a7510"] = 4; - isch_map["18ebb7f087"] = 5; - isch_map["188741df5e"] = 6; - isch_map["18b37c5ac9"] = 7; - isch_map["1146a44f13"] = 8; - isch_map["117299ca84"] = 9; - isch_map["111e6fe55d"] = 10; - isch_map["112a5260ca"] = 11; - isch_map["11db07ee62"] = 12; - isch_map["11ef3a6bf5"] = 13; - isch_map["1183cc442c"] = 14; - isch_map["11b7f1c1bb"] = 15; - isch_map["1a4a2e239e"] = 16; - isch_map["1a7e13a609"] = 17; - isch_map["1a12e589d0"] = 18; - isch_map["1a26d80c47"] = 19; - isch_map["1ad78d82ef"] = 20; - isch_map["1ae3b00778"] = 21; - isch_map["1a8f4628a1"] = 22; - isch_map["1abb7bad36"] = 23; - isch_map["134ea3b8ec"] = 24; - isch_map["137a9e3d7b"] = 25; - isch_map["13166812a2"] = 26; - isch_map["1322559735"] = 27; - isch_map["13d300199d"] = 28; - isch_map["13e73d9c0a"] = 29; - isch_map["138bcbb3d3"] = 30; - isch_map["13bff63644"] = 31; - isch_map["1442f705ef"] = 32; - isch_map["1476ca8078"] = 33; - isch_map["141a3cafa1"] = 34; - isch_map["142e012a36"] = 35; - isch_map["14df54a49e"] = 36; - isch_map["14eb692109"] = 37; - isch_map["14879f0ed0"] = 38; - isch_map["14b3a28b47"] = 39; - isch_map["1d467a9e9d"] = 40; - isch_map["1d72471b0a"] = 41; - isch_map["1d1eb134d3"] = 42; - isch_map["1d2a8cb144"] = 43; - isch_map["1ddbd93fec"] = 44; - isch_map["1defe4ba7b"] = 45; - isch_map["1d831295a2"] = 46; - isch_map["1db72f1035"] = 47; - isch_map["164af0f210"] = 48; - isch_map["167ecd7787"] = 49; - isch_map["16123b585e"] = 50; - isch_map["162606ddc9"] = 51; - isch_map["16d7535361"] = 52; - isch_map["16e36ed6f6"] = 53; - isch_map["168f98f92f"] = 54; - isch_map["16bba57cb8"] = 55; - isch_map["1f4e7d6962"] = 56; - isch_map["1f7a40ecf5"] = 57; - isch_map["1f16b6c32c"] = 58; - isch_map["1f228b46bb"] = 59; - isch_map["1fd3dec813"] = 60; - isch_map["1fe7e34d84"] = 61; - isch_map["1f8b15625d"] = 62; - isch_map["1fbf28e7ca"] = 63; - isch_map["84d62c339"] = 64; - isch_map["8795f46ae"] = 65; - isch_map["815a96977"] = 66; - isch_map["82194ece0"] = 67; - isch_map["8d0c16248"] = 68; - isch_map["8e4fce7df"] = 69; - isch_map["8880ac806"] = 70; - isch_map["8bc374d91"] = 71; - isch_map["149ef584b"] = 72; - isch_map["17dd2dddc"] = 73; - isch_map["11124f205"] = 74; - isch_map["125197792"] = 75; - isch_map["1d44cf93a"] = 76; - isch_map["1e0717cad"] = 77; - isch_map["18c875374"] = 78; - isch_map["1b8bad6e3"] = 79; - isch_map["a456534c6"] = 80; - isch_map["a7158b151"] = 81; - isch_map["a1dae9e88"] = 82; - isch_map["a29931b1f"] = 83; - isch_map["ad8c695b7"] = 84; - isch_map["aecfb1020"] = 85; - isch_map["a800d3ff9"] = 86; - isch_map["ab430ba6e"] = 87; - isch_map["341e8afb4"] = 88; - isch_map["375d52a23"] = 89; - isch_map["3192305fa"] = 90; - isch_map["32d1e806d"] = 91; - isch_map["3dc4b0ec5"] = 92; - isch_map["3e8768b52"] = 93; - isch_map["38480a48b"] = 94; - isch_map["3b0bd211c"] = 95; - isch_map["44dbc12b7"] = 96; - isch_map["479819720"] = 97; - isch_map["41577b8f9"] = 98; - isch_map["4214a3d6e"] = 99; - isch_map["4d01fb3c6"] = 100; - isch_map["4e4223651"] = 101; - isch_map["488d41988"] = 102; - isch_map["4bce99c1f"] = 103; - isch_map["d493189c5"] = 104; - isch_map["d7d0c0c52"] = 105; - isch_map["d11fa238b"] = 106; - isch_map["d25c7a61c"] = 107; - isch_map["dd49228b4"] = 108; - isch_map["de0afad23"] = 109; - isch_map["d8c5982fa"] = 110; - isch_map["db864076d"] = 111; - isch_map["645bbe548"] = 112; - isch_map["6718660df"] = 113; - isch_map["61d704f06"] = 114; - isch_map["6294dca91"] = 115; - isch_map["6d8184439"] = 116; - isch_map["6ec25c1ae"] = 117; - isch_map["680d3ee77"] = 118; - isch_map["6b4ee6be0"] = 119; - isch_map["f41367e3a"] = 120; - isch_map["f750bfbad"] = 121; - isch_map["f19fdd474"] = 122; - isch_map["f2dc051e3"] = 123; - isch_map["fdc95df4b"] = 124; - isch_map["fe8a85adc"] = 125; - isch_map["f845e7505"] = 126; - isch_map["fb063f092"] = 127; + isch_map[0x184229d461ULL] = 0; + isch_map[0x18761451f6ULL] = 1; + isch_map[0x181ae27e2fULL] = 2; + isch_map[0x182edffbb8ULL] = 3; + isch_map[0x18df8a7510ULL] = 4; + isch_map[0x18ebb7f087ULL] = 5; + isch_map[0x188741df5eULL] = 6; + isch_map[0x18b37c5ac9ULL] = 7; + isch_map[0x1146a44f13ULL] = 8; + isch_map[0x117299ca84ULL] = 9; + isch_map[0x111e6fe55dULL] = 10; + isch_map[0x112a5260caULL] = 11; + isch_map[0x11db07ee62ULL] = 12; + isch_map[0x11ef3a6bf5ULL] = 13; + isch_map[0x1183cc442cULL] = 14; + isch_map[0x11b7f1c1bbULL] = 15; + isch_map[0x1a4a2e239eULL] = 16; + isch_map[0x1a7e13a609ULL] = 17; + isch_map[0x1a12e589d0ULL] = 18; + isch_map[0x1a26d80c47ULL] = 19; + isch_map[0x1ad78d82efULL] = 20; + isch_map[0x1ae3b00778ULL] = 21; + isch_map[0x1a8f4628a1ULL] = 22; + isch_map[0x1abb7bad36ULL] = 23; + isch_map[0x134ea3b8ecULL] = 24; + isch_map[0x137a9e3d7bULL] = 25; + isch_map[0x13166812a2ULL] = 26; + isch_map[0x1322559735ULL] = 27; + isch_map[0x13d300199dULL] = 28; + isch_map[0x13e73d9c0aULL] = 29; + isch_map[0x138bcbb3d3ULL] = 30; + isch_map[0x13bff63644ULL] = 31; + isch_map[0x1442f705efULL] = 32; + isch_map[0x1476ca8078ULL] = 33; + isch_map[0x141a3cafa1ULL] = 34; + isch_map[0x142e012a36ULL] = 35; + isch_map[0x14df54a49eULL] = 36; + isch_map[0x14eb692109ULL] = 37; + isch_map[0x14879f0ed0ULL] = 38; + isch_map[0x14b3a28b47ULL] = 39; + isch_map[0x1d467a9e9dULL] = 40; + isch_map[0x1d72471b0aULL] = 41; + isch_map[0x1d1eb134d3ULL] = 42; + isch_map[0x1d2a8cb144ULL] = 43; + isch_map[0x1ddbd93fecULL] = 44; + isch_map[0x1defe4ba7bULL] = 45; + isch_map[0x1d831295a2ULL] = 46; + isch_map[0x1db72f1035ULL] = 47; + isch_map[0x164af0f210ULL] = 48; + isch_map[0x167ecd7787ULL] = 49; + isch_map[0x16123b585eULL] = 50; + isch_map[0x162606ddc9ULL] = 51; + isch_map[0x16d7535361ULL] = 52; + isch_map[0x16e36ed6f6ULL] = 53; + isch_map[0x168f98f92fULL] = 54; + isch_map[0x16bba57cb8ULL] = 55; + isch_map[0x1f4e7d6962ULL] = 56; + isch_map[0x1f7a40ecf5ULL] = 57; + isch_map[0x1f16b6c32cULL] = 58; + isch_map[0x1f228b46bbULL] = 59; + isch_map[0x1fd3dec813ULL] = 60; + isch_map[0x1fe7e34d84ULL] = 61; + isch_map[0x1f8b15625dULL] = 62; + isch_map[0x1fbf28e7caULL] = 63; + isch_map[0x084d62c339ULL] = 64; + isch_map[0x08795f46aeULL] = 65; + isch_map[0x0815a96977ULL] = 66; + isch_map[0x082194ece0ULL] = 67; + isch_map[0x08d0c16248ULL] = 68; + isch_map[0x08e4fce7dfULL] = 69; + isch_map[0x08880ac806ULL] = 70; + isch_map[0x08bc374d91ULL] = 71; + isch_map[0x0149ef584bULL] = 72; + isch_map[0x017dd2dddcULL] = 73; + isch_map[0x011124f205ULL] = 74; + isch_map[0x0125197792ULL] = 75; + isch_map[0x01d44cf93aULL] = 76; + isch_map[0x01e0717cadULL] = 77; + isch_map[0x018c875374ULL] = 78; + isch_map[0x01b8bad6e3ULL] = 79; + isch_map[0x0a456534c6ULL] = 80; + isch_map[0x0a7158b151ULL] = 81; + isch_map[0x0a1dae9e88ULL] = 82; + isch_map[0x0a29931b1fULL] = 83; + isch_map[0x0ad8c695b7ULL] = 84; + isch_map[0x0aecfb1020ULL] = 85; + isch_map[0x0a800d3ff9ULL] = 86; + isch_map[0x0ab430ba6eULL] = 87; + isch_map[0x0341e8afb4ULL] = 88; + isch_map[0x0375d52a23ULL] = 89; + isch_map[0x03192305faULL] = 90; + isch_map[0x032d1e806dULL] = 91; + isch_map[0x03dc4b0ec5ULL] = 92; + isch_map[0x03e8768b52ULL] = 93; + isch_map[0x038480a48bULL] = 94; + isch_map[0x03b0bd211cULL] = 95; + isch_map[0x044dbc12b7ULL] = 96; + isch_map[0x0479819720ULL] = 97; + isch_map[0x041577b8f9ULL] = 98; + isch_map[0x04214a3d6eULL] = 99; + isch_map[0x04d01fb3c6ULL] = 100; + isch_map[0x04e4223651ULL] = 101; + isch_map[0x0488d41988ULL] = 102; + isch_map[0x04bce99c1fULL] = 103; + isch_map[0x0d493189c5ULL] = 104; + isch_map[0x0d7d0c0c52ULL] = 105; + isch_map[0x0d11fa238bULL] = 106; + isch_map[0x0d25c7a61cULL] = 107; + isch_map[0x0dd49228b4ULL] = 108; + isch_map[0x0de0afad23ULL] = 109; + isch_map[0x0d8c5982faULL] = 110; + isch_map[0x0db864076dULL] = 111; + isch_map[0x0645bbe548ULL] = 112; + isch_map[0x06718660dfULL] = 113; + isch_map[0x061d704f06ULL] = 114; + isch_map[0x06294dca91ULL] = 115; + isch_map[0x06d8184439ULL] = 116; + isch_map[0x06ec25c1aeULL] = 117; + isch_map[0x0680d3ee77ULL] = 118; + isch_map[0x06b4ee6be0ULL] = 119; + isch_map[0x0f41367e3aULL] = 120; + isch_map[0x0f750bfbadULL] = 121; + isch_map[0x0f19fdd474ULL] = 122; + isch_map[0x0f2dc051e3ULL] = 123; + isch_map[0x0fdc95df4bULL] = 124; + isch_map[0x0fe8a85adcULL] = 125; + isch_map[0x0f845e7505ULL] = 126; + isch_map[0x0fb063f092ULL] = 127; + isch_map[0x575d57f7ffULL] = -2; // S-ISCH } int16_t p25p2_isch::isch_lookup(const uint8_t dibits[]) @@ -178,16 +179,27 @@ p25p2_isch::isch_lookup(const uint8_t dibits[]) uint64_t cw = 0; for (int i=0; i<20; i++) cw = (cw << 2) + dibits[i]; - sprintf(s, "%llx", (unsigned long long)cw); - return isch_lookup(std::string(s)); + return isch_lookup(cw); } int16_t -p25p2_isch::isch_lookup(std::string codeword) +p25p2_isch::isch_lookup(uint64_t cw) { - if (codeword.compare("575d57f7ff") == 0) - return -2; - else if (isch_map.find(codeword) == isch_map.end()) - return -1; // FIXME- handle bit error(s) in codeword - else - return isch_map[codeword]; + if (isch_map.find(cw) == isch_map.end()) // Must contain bit errors if no exact match, so + { // look for codeword with smallest hamming distance + int popct, popmin = 40; + int16_t msg = -1; + for (auto it = isch_map.begin(); it != isch_map.end(); ++it) + { + popct = __builtin_popcountll(cw ^ it->first); + if ((popct <= 7) && (popct < popmin)) // Encoding is (40, 9, 16) which can correct at most 7 bit-errors + { + msg = it->second; + popmin = popct; + } + } + return msg; + } + else // No errors, so simply return the matching value + return isch_map[cw]; } + diff --git a/lib/op25_repeater/lib/p25p2_isch.h b/lib/op25_repeater/lib/p25p2_isch.h index 3279738ca..fcb6df431 100644 --- a/lib/op25_repeater/lib/p25p2_isch.h +++ b/lib/op25_repeater/lib/p25p2_isch.h @@ -30,9 +30,9 @@ class p25p2_isch { public: p25p2_isch(); // constructor - int16_t isch_lookup(std::string codeword); + int16_t isch_lookup(uint64_t cw); int16_t isch_lookup(const uint8_t dibits[]); private: - std::map isch_map; + std::map isch_map; }; #endif /* INCLUDED_P25P2_ISCH_H */ diff --git a/lib/op25_repeater/lib/p25p2_sync.cc b/lib/op25_repeater/lib/p25p2_sync.cc index a8253bd49..4ac22239f 100644 --- a/lib/op25_repeater/lib/p25p2_sync.cc +++ b/lib/op25_repeater/lib/p25p2_sync.cc @@ -28,7 +28,8 @@ p25p2_sync::p25p2_sync(void) : // constructor sync_confidence(0), _tdma_slotid(0), - packets(0) + packets(0), + d_last_rc(0) { } @@ -46,7 +47,7 @@ void p25p2_sync::check_confidence (const uint8_t dibits[]) _tdma_slotid = 0; int rc, cnt, fr, loc, chn, checkval; rc = isch.isch_lookup(dibits); - checkval = cnt = fr = loc = chn = rc; + checkval = cnt = fr = loc = chn = d_last_rc = rc; if (rc >= 0) { // I-ISCH (informational) cnt = rc & 3; rc = rc >> 2; diff --git a/lib/op25_repeater/lib/p25p2_sync.h b/lib/op25_repeater/lib/p25p2_sync.h index bf920ff40..f9b43f8b0 100644 --- a/lib/op25_repeater/lib/p25p2_sync.h +++ b/lib/op25_repeater/lib/p25p2_sync.h @@ -32,9 +32,11 @@ class p25p2_sync void check_confidence (const uint8_t dibits[]); bool in_sync(void); uint32_t tdma_slotid(void) { return _tdma_slotid; } + int last_rc(void) { return d_last_rc; } private: int32_t sync_confidence; uint32_t _tdma_slotid; uint32_t packets; + int d_last_rc; }; #endif /* INCLUDED_P25P2_SYNC_H */ diff --git a/lib/op25_repeater/lib/p25p2_tdma.cc b/lib/op25_repeater/lib/p25p2_tdma.cc index 4c4943ead..aa6ee2bcd 100644 --- a/lib/op25_repeater/lib/p25p2_tdma.cc +++ b/lib/op25_repeater/lib/p25p2_tdma.cc @@ -95,6 +95,7 @@ p25p2_tdma::p25p2_tdma(const op25_audio& udp, log_ts& logger, int slotid, int de packets(0), write_bufp(0), d_slotid(slotid), + d_tdma_slot_first_4v(-1), mbe_err_cnt(0), tone_frame(false), d_msg_queue(queue), @@ -107,6 +108,7 @@ p25p2_tdma::p25p2_tdma(const op25_audio& udp, log_ts& logger, int slotid, int de d_nac(0), d_debug(debug), burst_id(-1), + burst_type(-1), ESS_A(28,0), ESS_B(16,0), ess_keyid(0), @@ -134,6 +136,14 @@ void p25p2_tdma::set_slotid(int slotid) d_slotid = slotid; } +void p25p2_tdma::call_end() { + op25audio.send_audio_flag(op25_audio::DRAIN); + reset_ess(); + reset_vb(); + d_tdma_slot_first_4v = -1; + burst_type = -1; +} + void p25p2_tdma::crypt_reset() { crypt_algs.reset(); } @@ -175,41 +185,47 @@ p25p2_tdma::set_xormask(const char*p) { int p25p2_tdma::process_mac_pdu(const uint8_t byte_buf[], const unsigned int len, const int rs_errs) { - unsigned int opcode = (byte_buf[0] >> 5) & 0x7; - unsigned int offset = (byte_buf[0] >> 2) & 0x7; - -#if 0 - if (d_debug >= 10) { - fprintf(stderr, "%s process_mac_pdu: opcode %d len %d\n", logts.get(d_msgq_id), opcode, len); - } -#endif - - switch (opcode) - { - case 0: // MAC_SIGNAL - handle_mac_signal(byte_buf, len, rs_errs); - break; - - case 1: // MAC_PTT - handle_mac_ptt(byte_buf, len, rs_errs); - break; - - case 2: // MAC_END_PTT - handle_mac_end_ptt(byte_buf, len, rs_errs); - break; - - case 3: // MAC_IDLE - handle_mac_idle(byte_buf, len, rs_errs); - break; - - case 4: // MAC_ACTIVE - handle_mac_active(byte_buf, len, rs_errs); - break; - - case 6: // MAC_HANGTIME - handle_mac_hangtime(byte_buf, len, rs_errs); - break; - } + unsigned int offset = (byte_buf[0] >> 2) & 0x7; + unsigned int opcode = (byte_buf[0] >> 5) & 0x7; + + if (d_debug >= 10) { + fprintf(stderr, "%s process_mac_pdu: opcode %d offset %d len %d\n", logts.get(d_msgq_id), opcode, offset, len); + } + + switch (opcode) + { + case 0: // MAC_SIGNAL + handle_mac_signal(byte_buf, len, rs_errs); + break; + + case 1: // MAC_PTT + handle_mac_ptt(byte_buf, len, rs_errs); + // capture the offset field + // PT.1 has offset=1, PT.0 has offset=0 + // add offset + 1 to the current TDMA slot to get the first 4V position + // normalize TDMA slot 0-9 to ch0/ch1 slot 0-4 + d_tdma_slot_first_4v = ((sync.tdma_slotid() >> 1) + offset + 1) % 5; //TODO: handle offset > 4 + break; + + case 2: // MAC_END_PTT + handle_mac_end_ptt(byte_buf, len, rs_errs); + break; + + case 3: // MAC_IDLE + handle_mac_idle(byte_buf, len, rs_errs); + break; + + case 4: // MAC_ACTIVE + handle_mac_active(byte_buf, len, rs_errs); + // also capture the offset field here + // it can be captured directly as for non-PTT PDUs it simply stores the offset to the first 4V + d_tdma_slot_first_4v = (offset > 4) ? 0 : offset; + break; + + case 6: // MAC_HANGTIME + handle_mac_hangtime(byte_buf, len, rs_errs); + break; + } // maps sacch opcodes into phase I duid values static const int opcode_map[8] = {3, 5, 15, 15, 5, 3, 3, 3}; return opcode_map[opcode]; @@ -240,22 +256,22 @@ void p25p2_tdma::handle_mac_ptt(const uint8_t byte_buf[], const unsigned int len uint16_t grpaddr = (byte_buf[16] << 8) + byte_buf[17]; if (d_debug >= 10) { - fprintf(stderr, "%s MAC_PTT: srcaddr=%u, grpaddr=%u", logts.get(), srcaddr, grpaddr); + fprintf(stderr, "%s MAC_PTT: srcaddr=%u, grpaddr=%u, TDMA slot ID=%u", logts.get(d_msgq_id), srcaddr, grpaddr, sync.tdma_slotid()); } - for (int i = 0; i < 9; i++) { - ess_mi[i] = byte_buf[i+1]; - } - ess_algid = byte_buf[10]; - ess_keyid = (byte_buf[11] << 8) + byte_buf[12]; - if (d_debug >= 10) { + for (int i = 0; i < 9; i++) { + ess_mi[i] = byte_buf[i+1]; + } + ess_algid = byte_buf[10]; + ess_keyid = (byte_buf[11] << 8) + byte_buf[12]; + if (d_debug >= 10) { fprintf(stderr, ", algid=%x, keyid=%x, mi=%02x %02x %02x %02x %02x %02x %02x %02x %02x, rs_errs=%d\n", ess_algid, ess_keyid, ess_mi[0], ess_mi[1], ess_mi[2], ess_mi[3], ess_mi[4], ess_mi[5],ess_mi[6], ess_mi[7], ess_mi[8], rs_errs); } if (encrypted()) { - crypt_algs.prepare(ess_algid, ess_keyid, FT_4V, ess_mi); // likely not necessary because prepare() called by handle_packet() when burst_id==0 + crypt_algs.prepare(ess_algid, ess_keyid, PT_P25_PHASE2, ess_mi); } src_id = srcaddr; @@ -288,6 +304,8 @@ void p25p2_tdma::handle_mac_end_ptt(const uint8_t byte_buf[], const unsigned int op25audio.send_audio_flag(op25_audio::DRAIN); terminate_call = true; + // reset crypto parameters + reset_ess(); } void p25p2_tdma::handle_mac_idle(const uint8_t byte_buf[], const unsigned int len, const int rs_errs) @@ -504,7 +522,7 @@ int p25p2_tdma::handle_acch_frame(const uint8_t dibits[], bool fast, bool is_lcc return rc; } -void p25p2_tdma::handle_voice_frame(const uint8_t dibits[]) +void p25p2_tdma::handle_voice_frame(const uint8_t dibits[], int slot, int voice_subframe) { static const int NSAMP_OUTPUT=160; audio_samples *samples = NULL; @@ -516,6 +534,7 @@ void p25p2_tdma::handle_voice_frame(const uint8_t dibits[]) int16_t snd; int K; int rc = -1; + frame_type fr_type; // Deinterleave and figure out frame type: errs = vf.process_vcw(&errs_mp, dibits, b, u); @@ -532,8 +551,25 @@ void p25p2_tdma::handle_voice_frame(const uint8_t dibits[]) // Pass encrypted traffic through the decryption algorithms if (encrypted()) { + switch (slot) { + case 0: + fr_type = FT_4V_0; + break; + case 1: + fr_type = FT_4V_1; + break; + case 2: + fr_type = FT_4V_2; + break; + case 3: + fr_type = FT_4V_3; + break; + case 4: + fr_type = FT_2V; + break; + } vf.pack_cw(p_cw, u); - audio_valid = crypt_algs.process(p_cw); + audio_valid = crypt_algs.process(p_cw, fr_type, voice_subframe); if (!audio_valid) return; vf.unpack_cw(p_cw, u); // unpack plaintext codewords @@ -630,30 +666,58 @@ int p25p2_tdma::handle_packet(uint8_t dibits[], const uint64_t fs) return -1; const uint8_t* burstp = &dibits[10]; uint8_t xored_burst[BURST_SIZE - 10]; - int burst_type = duid.duid_lookup(duid.extract_duid(burstp)); + burst_type = duid.duid_lookup(duid.extract_duid(burstp)); if ((burst_type != 13) && (which_slot[sync.tdma_slotid()] != d_slotid)) // only permit control channel or active slot return -1; for (int i=0; i> 1; + + track_vb(); + + // update "first 4V" variable here as well as it always follows 2V + // in case we missed both PTTs + if (burst_type == 6 && sync.last_rc() >= 0) + d_tdma_slot_first_4v = (current_slot + 1) % 5; + + if (d_tdma_slot_first_4v >= 0 && sync.last_rc() >= 0) { + // now let's see if the voice frame received is the one we expected to get. + // shift the range from [0, 4] to [first_4V, first_4V+4] + if (current_slot < (int) d_tdma_slot_first_4v) + current_slot += 5; + + // translate [first_4V, first_4V+1] to the burst_id we would expect to be in this slot + current_slot -= d_tdma_slot_first_4v; + + if (current_slot != burst_id && current_slot > burst_id) { + int need_to_skip = current_slot - burst_id; + // XXX determine if the 2V frame was missed? + if (d_debug >= 10) { + fprintf(stderr, "%i voice frame(s) missing; expecting %uV_%u but got %uV_%u. ISCH rc=%d\n", need_to_skip, (burst_id == 4 ? 2 : 4), burst_id, (burst_type == 6 ? 2 : 4), current_slot, sync.last_rc()); + } + burst_id = current_slot; + } + } + handle_4V2V_ess(&xored_burst[84]); - std::string s = "{\"encrypted\": " + std::to_string((encrypted()) ? 1 : 0) + ", \"algid\": " + std::to_string(ess_algid) + ", \"keyid\": " + std::to_string(ess_keyid) + "}"; - send_msg(s, M_P25_JSON_DATA); - if ((burst_type == 0) && (burst_id == 0)) { // promote next set of encryption parameters if this is first 4V after a 2V + handle_voice_frame(&xored_burst[11], current_slot, 0); + handle_voice_frame(&xored_burst[48], current_slot, 1); + if (burst_type == 0) { + handle_voice_frame(&xored_burst[96], current_slot, 2); + handle_voice_frame(&xored_burst[133], current_slot, 3); + } else /* if (burst_type == 6) */ { + // promote next set of encryption parameters AFTER we get the full ESS & process the 2V frame ess_algid = next_algid; ess_keyid = next_keyid; memcpy(ess_mi, next_mi, sizeof(ess_mi)); - if (encrypted()) { - crypt_algs.prepare(ess_algid, ess_keyid, ((burst_type == 0) ? FT_4V : FT_2V), ess_mi); - } - } - handle_voice_frame(&xored_burst[11]); - handle_voice_frame(&xored_burst[48]); - if (burst_type == 0) { - handle_voice_frame(&xored_burst[96]); - handle_voice_frame(&xored_burst[133]); + if (encrypted()) { + crypt_algs.prepare(ess_algid, ess_keyid, PT_P25_PHASE2, ess_mi); + } + std::string s = "{\"encrypted\": " + std::to_string((encrypted()) ? 1 : 0) + ", \"algid\": " + std::to_string(ess_algid) + ", \"keyid\": " + std::to_string(ess_keyid) + "}"; + send_msg(s, M_P25_JSON_DATA); } return -1; } else if (burst_type == 3) { // scrambled sacch @@ -684,7 +748,7 @@ void p25p2_tdma::handle_4V2V_ess(const uint8_t dibits[]) int ec = 0; if (d_debug >= 10) { - fprintf(stderr, "%s %s_BURST(%d) ", logts.get(d_msgq_id), (burst_id < 4) ? "4V" : "2V", burst_id); + fprintf(stderr, "%s %s_BURST(%d) TDMA slot ID=%u ", logts.get(d_msgq_id), (burst_type == 0) ? "4V" : "2V", burst_id, sync.tdma_slotid()); } if (burst_id < 4) { @@ -716,11 +780,13 @@ void p25p2_tdma::handle_4V2V_ess(const uint8_t dibits[]) } } - if (d_debug >= 10) { + if (d_debug >= 10 && burst_id == 4) { fprintf(stderr, "ESS: algid=%x, keyid=%x, mi=%02x %02x %02x %02x %02x %02x %02x %02x %02x, rs_errs=%d\n", next_algid, next_keyid, next_mi[0], next_mi[1], next_mi[2], next_mi[3], next_mi[4], next_mi[5],next_mi[6], next_mi[7], next_mi[8], ec); + } else if (d_debug >= 10) { + fprintf(stderr, "ESS: (partial)\n"); } } diff --git a/lib/op25_repeater/lib/p25p2_tdma.h b/lib/op25_repeater/lib/p25p2_tdma.h index d5f6bd41e..a1b001412 100644 --- a/lib/op25_repeater/lib/p25p2_tdma.h +++ b/lib/op25_repeater/lib/p25p2_tdma.h @@ -45,6 +45,7 @@ class p25p2_tdma p25p2_tdma(const op25_audio& udp, log_ts& logger, int slotid, int debug, bool do_msgq, gr::msg_queue::sptr queue, std::deque &qptr, bool do_audio_output, int msgq_id = 0) ; // constructor int handle_packet(uint8_t dibits[], const uint64_t fs) ; void set_slotid(int slotid); + void call_end(); void crypt_reset(); void crypt_key(uint16_t keyid, uint8_t algid, const std::vector &key); uint8_t* tdma_xormask; @@ -67,6 +68,7 @@ class p25p2_tdma int write_bufp; char write_buf[512]; int d_slotid; + int d_tdma_slot_first_4v; mbe_parms cur_mp; mbe_parms prev_mp; mbe_parms enh_mp; @@ -88,7 +90,8 @@ class p25p2_tdma int d_nac; int d_debug; int burst_id; - inline int track_vb(int burst_type) { burst_id++; return burst_id = (burst_type == 0) ? (burst_id % 5) : 4; } + int burst_type; + inline int track_vb(void) { burst_id++; return burst_id = (burst_type == 0) ? (burst_id % 5) : 4; } inline void reset_vb(void) { burst_id = -1; } ezpwd::RS<63,35> rs28; // Reed-Solomon decoder object @@ -106,7 +109,7 @@ class p25p2_tdma p25_crypt_algs crypt_algs; int handle_acch_frame(const uint8_t dibits[], bool fast, bool is_lcch) ; - void handle_voice_frame(const uint8_t dibits[]) ; + void handle_voice_frame(const uint8_t dibits[], int slot, int voice_subframe); int process_mac_pdu(const uint8_t byte_buf[], const unsigned int len, const int rs_errs) ; void handle_mac_signal(const uint8_t byte_buf[], const unsigned int len, const int rs_errs) ; void handle_mac_ptt(const uint8_t byte_buf[], const unsigned int len, const int rs_errs) ; @@ -118,6 +121,7 @@ class p25p2_tdma void convert_abbrev_msg(const uint8_t byte_buf[], const uint16_t nac, const uint8_t mfid = 0x00); void handle_4V2V_ess(const uint8_t dibits[]); inline bool encrypted() { return (ess_algid != 0x80); } + inline void reset_ess() { ess_algid = 0x80; memset(ess_mi, 0, sizeof(ess_mi)); } void send_msg(const std::string msg_str, long msg_type); }; diff --git a/lib/op25_repeater/lib/rx_base.h b/lib/op25_repeater/lib/rx_base.h index a271fddc2..9c9b630de 100644 --- a/lib/op25_repeater/lib/rx_base.h +++ b/lib/op25_repeater/lib/rx_base.h @@ -29,6 +29,7 @@ namespace gr{ public: virtual void rx_sym(const uint8_t sym) = 0; virtual void sync_reset(void) = 0; + virtual void call_end(void) = 0; virtual void crypt_reset(void) = 0; virtual void crypt_key(uint16_t keyid, uint8_t algid, const std::vector &key) = 0; virtual void reset_timer(void) = 0; @@ -39,7 +40,7 @@ namespace gr{ virtual void set_xormask(const char* p) = 0; virtual int get_src_id(int slot) { return -1;}; virtual bool get_terminated(int slot) { return false;}; - rx_base(const char * options, log_ts& logger,int debug, int msgq_id, gr::msg_queue::sptr queue) { }; + rx_base(const char * options, log_ts& logger, int debug, int msgq_id, gr::msg_queue::sptr queue) { }; rx_base() {}; // default constructor called by derived classes virtual ~rx_base() {}; }; diff --git a/lib/op25_repeater/lib/rx_smartnet.h b/lib/op25_repeater/lib/rx_smartnet.h index 291a8d3cd..19edb9b84 100644 --- a/lib/op25_repeater/lib/rx_smartnet.h +++ b/lib/op25_repeater/lib/rx_smartnet.h @@ -63,6 +63,7 @@ namespace gr{ void rx_sym(const uint8_t sym); void sync_reset(void); void reset_timer(void); + void call_end(void) { }; void crypt_reset(void) { }; void crypt_key(uint16_t keyid, uint8_t algid, const std::vector &key) { }; void set_nac(int nac) { }; diff --git a/lib/op25_repeater/lib/rx_subchannel.h b/lib/op25_repeater/lib/rx_subchannel.h index 4792d50f2..d98716b38 100644 --- a/lib/op25_repeater/lib/rx_subchannel.h +++ b/lib/op25_repeater/lib/rx_subchannel.h @@ -47,6 +47,7 @@ namespace gr{ void rx_sym(const uint8_t sym); void sync_reset(void); void reset_timer(void) { }; + void call_end(void) { }; void crypt_reset(void) { }; void crypt_key(uint16_t keyid, uint8_t algid, const std::vector &key) { }; void set_nac(int nac) { }; diff --git a/lib/op25_repeater/lib/rx_sync.cc b/lib/op25_repeater/lib/rx_sync.cc index d3e3cb655..93bdabb92 100644 --- a/lib/op25_repeater/lib/rx_sync.cc +++ b/lib/op25_repeater/lib/rx_sync.cc @@ -95,6 +95,11 @@ void rx_sync::sync_reset(void) { reset_timer(); } +void rx_sync::call_end(void) { + p25fdma.call_end(); + p25tdma.call_end(); +} + void rx_sync::crypt_reset(void) { p25fdma.crypt_reset(); p25tdma.crypt_reset(); diff --git a/lib/op25_repeater/lib/rx_sync.h b/lib/op25_repeater/lib/rx_sync.h index a11019825..b2498b077 100644 --- a/lib/op25_repeater/lib/rx_sync.h +++ b/lib/op25_repeater/lib/rx_sync.h @@ -115,6 +115,7 @@ class rx_sync : public rx_base { void rx_sym(const uint8_t sym); void sync_reset(void); void reset_timer(void); + void call_end(void); void crypt_reset(void); void crypt_key(uint16_t keyid, uint8_t algid, const std::vector &key); void set_slot_mask(int mask); diff --git a/trunk-recorder/recorders/analog_recorder.cc b/trunk-recorder/recorders/analog_recorder.cc index 5af62cbb9..15605dcc7 100644 --- a/trunk-recorder/recorders/analog_recorder.cc +++ b/trunk-recorder/recorders/analog_recorder.cc @@ -109,12 +109,12 @@ analog_recorder::analog_recorder(Source *src, Recorder_Type type) double resampled_rate = double(initial_rate) / double(decim); #if GNURADIO_VERSION < 0x030900 - inital_lpf_taps = gr::filter::firdes::low_pass_2(1.0, samp_rate, 96000, 30000, 100, gr::filter::firdes::WIN_HANN); + inital_lpf_taps = gr::filter::firdes::low_pass_2(1.0, samp_rate, 96000, 30000, 30, gr::filter::firdes::WIN_HANN); #else - inital_lpf_taps = gr::filter::firdes::low_pass_2(1.0, samp_rate, 96000, 30000, 100, gr::fft::window::WIN_HANN); + inital_lpf_taps = gr::filter::firdes::low_pass_2(1.0, samp_rate, 96000, 30000, 30, gr::fft::window::WIN_HANN); #endif // channel_lpf_taps = gr::filter::firdes::low_pass_2(1.0, pre_channel_rate, 5000, 2000, 60); - channel_lpf_taps = gr::filter::firdes::low_pass_2(1.0, initial_rate, 4000, 1000, 100); + channel_lpf_taps = gr::filter::firdes::low_pass_2(1.0, initial_rate, 4000, 1000, 30); std::vector dest(inital_lpf_taps.begin(), inital_lpf_taps.end()); diff --git a/trunk-recorder/recorders/p25_recorder_impl.cc b/trunk-recorder/recorders/p25_recorder_impl.cc index 46c6f2608..229739f4a 100644 --- a/trunk-recorder/recorders/p25_recorder_impl.cc +++ b/trunk-recorder/recorders/p25_recorder_impl.cc @@ -13,7 +13,7 @@ p25_recorder_sptr make_p25_recorder(Source *src, Recorder_Type type) { void p25_recorder_impl::generate_arb_taps() { double arb_size = 32; - double arb_atten = 100; + double arb_atten = 30; // was originally 100 // Create a filter that covers the full bandwidth of the output signal // If rate >= 1, we need to prevent images in the output, @@ -144,7 +144,7 @@ void p25_recorder_impl::initialize_prefilter() { double sps = floor(resampled_rate / phase1_symbol_rate); double def_excess_bw = 0.2; BOOST_LOG_TRIVIAL(info) << "\t P25 Recorder ARB - Initial Rate: " << input_rate << " Resampled Rate: " << resampled_rate << " Initial Decimation: " << decim << " ARB Rate: " << arb_rate << " SPS: " << sps; - + BOOST_LOG_TRIVIAL(info) << "\t P25 Recorder Taps - lowpass: " << lowpass_filter_coeffs.size() << " bandpass: " << bandpass_filter_coeffs.size() << " cutoff: " << cutoff_filter_coeffs.size() << " arb: " << arb_taps.size(); // Squelch DB // on a trunked network where you know you will have good signal, a carrier // power squelch works well. real FM receviers use a noise squelch, where diff --git a/trunk-recorder/recorders/p25_recorder_impl.h b/trunk-recorder/recorders/p25_recorder_impl.h index f2ed5edda..d032a6767 100644 --- a/trunk-recorder/recorders/p25_recorder_impl.h +++ b/trunk-recorder/recorders/p25_recorder_impl.h @@ -160,7 +160,6 @@ class p25_recorder_impl : public p25_recorder { const double phase2_symbol_rate = 6000; std::vector arb_taps; - std::vector bandpass_filter_coeffs; std::vector lowpass_filter_coeffs; std::vector cutoff_filter_coeffs;