From a3cab238b505fb4f06951a7826bce40d0d437f6b Mon Sep 17 00:00:00 2001 From: Max Date: Tue, 7 Nov 2017 10:41:03 -0500 Subject: [PATCH] d2460 --- op25/gr-op25_repeater/lib/CMakeLists.txt | 17 ++ op25/gr-op25_repeater/lib/d2460.cc | 360 +++++++++++++++++++++++ 2 files changed, 377 insertions(+) create mode 100644 op25/gr-op25_repeater/lib/d2460.cc diff --git a/op25/gr-op25_repeater/lib/CMakeLists.txt b/op25/gr-op25_repeater/lib/CMakeLists.txt index d35a5b4..361f266 100644 --- a/op25/gr-op25_repeater/lib/CMakeLists.txt +++ b/op25/gr-op25_repeater/lib/CMakeLists.txt @@ -97,4 +97,21 @@ target_link_libraries( GR_ADD_TEST(test_op25_repeater test-op25_repeater) +######################################################################## +# d2460 +######################################################################## +list(APPEND d2460_sources + ${CMAKE_CURRENT_SOURCE_DIR}/d2460.cc + ${CMAKE_CURRENT_SOURCE_DIR}/ambe.c + ${CMAKE_CURRENT_SOURCE_DIR}/mbelib.c + ${CMAKE_CURRENT_SOURCE_DIR}/ambe_encoder.cc + ${CMAKE_CURRENT_SOURCE_DIR}/software_imbe_decoder.cc + ${CMAKE_CURRENT_SOURCE_DIR}/imbe_decoder.cc + ${CMAKE_CURRENT_SOURCE_DIR}/p25p2_vf.cc + ${CMAKE_CURRENT_SOURCE_DIR}/rs.cc +) + +add_executable(op25-d2460 ${d2460_sources}) +target_link_libraries(op25-d2460 imbe_vocoder) + add_subdirectory(imbe_vocoder) diff --git a/op25/gr-op25_repeater/lib/d2460.cc b/op25/gr-op25_repeater/lib/d2460.cc new file mode 100644 index 0000000..655d6de --- /dev/null +++ b/op25/gr-op25_repeater/lib/d2460.cc @@ -0,0 +1,360 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include "imbe_vocoder/imbe_vocoder.h" +#include + +static const float GAIN_ADJUST=7.0; /* attenuation (dB) */ + +typedef uint16_t Uns; +static const Uns RC_OK=0; + +static const char prodid[] = "OP25 "; +static const char verstring[] = "1.0"; + +static ambe_encoder encoder; +static software_imbe_decoder software_decoder; +static p25p2_vf interleaver; +static mbe_parms cur_mp; +static mbe_parms prev_mp; + +static const Uns DV3K_START_BYTE = 0x61; +enum +{ + DV3K_CONTROL_RATEP = 0x0A, + DV3K_CONTROL_CHANFMT = 0x15, + DV3K_CONTROL_PRODID = 0x30, + DV3K_CONTROL_VERSTRING = 0x31, + DV3K_CONTROL_RESET = 0x33, + DV3K_CONTROL_READY = 0x39 +}; +static const Uns DV3K_AMBE_FIELD_CHAND = 0x01; +static const Uns DV3K_AMBE_FIELD_CMODE = 0x02; +static const Uns DV3K_AMBE_FIELD_TONE = 0x08; +static const Uns DV3K_AUDIO_FIELD_SPEECHD = 0x00; +static const Uns DV3K_AUDIO_FIELD_CMODE = 0x02; + +#pragma DATA_ALIGN(dstar_state, 2) +static Uns bitstream[72]; + +static Uns get_byte(Uns offset, Uns *p) +{ + Uns word = p[offset >> 1]; + return (offset & 1) ? (word >> 8) : (word & 0xff); +} + +static void set_byte(Uns offset, Uns *p, Uns byte) +{ + p[offset >> 1] = + (offset & 1) ? (byte << 8) | (p[offset >> 1] & 0xff) + : (p[offset >> 1] & 0xff00) | (byte & 0xff); +} + +static Uns get_word(Uns offset, Uns *p) +{ + return get_byte(offset + 1, p) | (get_byte(offset, p) << 8); +} + +static void set_word(Uns offset, Uns *p, Uns word) +{ + set_byte(offset, p, word >> 8); + set_byte(offset + 1, p, word & 0xff); +} + +static void set_cstring(Uns offset, Uns *p, const char *str) +{ + do + set_byte(offset++, p, *str); + while (*str++ != 0); +} + +static Uns pkt_check_ratep(Uns offset, Uns *p) +{ + static const Uns ratep[] = { + 0x01, 0x30, 0x07, 0x63, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x48 }; + Uns i; + for (i = 0; i < sizeof(ratep); ++i) + if (get_byte(offset + i, p) != ratep[i]) + return 0; + return 1; +} + +static void pack(Uns bits, Uns offset, Uns *p, Uns *bitstream) +{ + Uns i; + Uns byte = 0; + for (i = 0; i < bits; ++i) + { + byte |= bitstream[i] << (7 - (i & 7)); + if ((i & 7) == 7) + { + set_byte(offset++, p, byte); + byte = 0; + } + } + if (i & 7) + set_byte(offset, p, byte); +} + +static void unpack(Uns bits, Uns offset, Uns *bitstream, Uns *p) +{ + Uns i; + Uns byte; + for (i = 0; i < bits; ++i) + { + if ((i & 7) == 0) + byte = get_byte(offset++, p); + bitstream[i] = (byte >> (7 - (i & 7))) & 1; + } +} + +static int response_len = -1; + +static void bksnd(void*task, Uns bid, Uns len) +{ + response_len = len; +} + +static void vocoder_setup(void) { + encoder.set_dstar_mode(); + encoder.set_gain_adjust(GAIN_ADJUST); + encoder.set_alt_dstar_interleave(true); +} + +static void dump(unsigned char *p, ssize_t n) +{ + int i; + for (i = 0; i < n; ++i) + printf("%02x%c", p[i], i % 16 == 15 ? '\n' : ' '); + if (i % 16) + printf("\n"); +} + +static Uns pkt_process(Uns*pkt, Uns cnt) +{ + Uns bid=0; + Uns len = cnt << 1; + Uns payload_length; + Uns i; + Uns cmode = 0; + Uns tone = 0; + uint8_t codeword[72]; + int b[9]; + int K; + int rc = -1; + + if (len < 4 || cnt > 256) + goto fail; + + if (get_byte(0, pkt) != DV3K_START_BYTE) + goto fail; + + payload_length = get_word(1, pkt); + if (payload_length == 0) + goto fail; + if (4 + payload_length > len) + goto fail; + + switch (get_byte(3, pkt)) + { + case 0: + switch (get_byte(4, pkt)) + { + case DV3K_CONTROL_RATEP: + if (payload_length != 13) + goto fail; + if (!pkt_check_ratep(5, pkt)) + goto fail; + set_word(1, pkt, 1); + bksnd(NULL, bid, 3); + return RC_OK; + case DV3K_CONTROL_CHANFMT: + if (payload_length != 3) + goto fail; + if (get_word(5, pkt) != 0x0001) + goto fail; + set_word(1, pkt, 2); + set_byte(5, pkt, 0); + bksnd(NULL, bid, 3); + return RC_OK; + case DV3K_CONTROL_PRODID: + set_word(1, pkt, 8); + set_cstring(5, pkt, prodid); + bksnd(NULL, bid, 6); + return RC_OK; + case DV3K_CONTROL_VERSTRING: + set_word(1, pkt, 5); + set_cstring(5, pkt, verstring); + bksnd(NULL, bid, 5); + return RC_OK; + case DV3K_CONTROL_RESET: + if (payload_length != 1) + goto fail; + vocoder_setup(); + set_byte(4, pkt, DV3K_CONTROL_READY); + bksnd(NULL, bid, 3); + return RC_OK; + default: + goto fail; + } + case 1: + switch (payload_length) + { + case 17: + if (get_byte(18, pkt) != DV3K_AMBE_FIELD_TONE) + goto fail; + tone = get_word(19, pkt); + /* FALLTHROUGH */ + case 14: + if (get_byte(15, pkt) != DV3K_AMBE_FIELD_CMODE) + goto fail; + cmode = get_word(16, pkt); + /* FALLTHROUGH */ + case 11: + if (get_byte(4, pkt) != DV3K_AMBE_FIELD_CHAND) + goto fail; + if (get_byte(5, pkt) != 72) + goto fail; + unpack(72, 6, bitstream, pkt); + break; + default: + goto fail; + } + + for (i = 0; i < 72; i++) { + codeword[i] = bitstream[i]; + } + interleaver.decode_dstar(codeword, b, true); + if (b[0] >= 120) { + memset(6+(char*)pkt, 0, 320); // silence + // FIXME: add handling for tone case (b0=126) + } else { + rc = mbe_dequantizeAmbe2400Parms(&cur_mp, &prev_mp, b); + printf("B\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n", b[0], b[1], b[2], b[3], b[4], b[5], b[6], b[7], b[8]); + K = 12; + if (cur_mp.L <= 36) + K = int(float(cur_mp.L + 2.0) / 3.0); + software_decoder.decode_tap(cur_mp.L, K, cur_mp.w0, &cur_mp.Vl[1], &cur_mp.Ml[1]); + audio_samples *samples = software_decoder.audio(); + int16_t snd; + for (i=0; i < 160; i++) { + if (samples->size() > 0) { + snd = (int16_t)(samples->front()); + samples->pop_front(); + } else { + snd = 0; + } + set_word(6 + (i << 1), pkt, snd); + } + mbe_moveMbeParms (&cur_mp, &prev_mp); + } + + set_word(1, pkt, 322); + set_byte(3, pkt, 2); + set_byte(4, pkt, DV3K_AUDIO_FIELD_SPEECHD); + set_byte(5, pkt, 160); + bksnd(NULL, bid, 165); + return RC_OK; + case 2: + if (payload_length != 322 && payload_length != 325) + goto fail; + if (get_byte(4, pkt) != DV3K_AUDIO_FIELD_SPEECHD) + goto fail; + if (get_byte(5, pkt) != 160) + goto fail; + if (payload_length == 325) + { + if (get_byte(326, pkt) != DV3K_AUDIO_FIELD_CMODE) + goto fail; + cmode = get_word(323, pkt); + } + int16_t samples[160]; + for (i=0; i < 160; i++) { + samples[i] = (int16_t) get_word(6 + (i << 1), pkt); + } + encoder.encode(samples, codeword); + for (i = 0; i < 72; i++) { + bitstream[i] = codeword[i]; + } + set_word(1, pkt, 11); + set_byte(3, pkt, 1); + set_byte(4, pkt, DV3K_AMBE_FIELD_CHAND); + set_byte(5, pkt, 72); + pack(72, 6, pkt, bitstream); + bksnd(NULL, bid, 8); + return RC_OK; + default: + goto fail; + } + +fail: + bksnd(NULL, bid, 0); + return RC_OK; +} + +int main() +{ + int sockfd; + const ssize_t size_max = 1024; + ssize_t size_in, size_out; + char buf_in[size_max], buf_out[size_max]; + socklen_t length = sizeof(struct sockaddr_in); + struct sockaddr_in sa = { 0 }; + Uns rc; + + vocoder_setup(); + + if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) + exit(2); + + sa.sin_family = AF_INET; + sa.sin_port = htons(2460); + sa.sin_addr.s_addr = htonl(INADDR_ANY); + + if (bind(sockfd, (struct sockaddr *)&sa, sizeof(sa)) < 0) + exit(3); + + while (1) + { + if ((size_in = recvfrom(sockfd, buf_in, size_max, + 0, (struct sockaddr *)&sa, &length)) < 0) + exit(4); + + if (size_in & 1) + buf_in[size_in++] = 0; + + rc = pkt_process((Uns*)buf_in, size_in >> 1); + if (response_len <= 0) + exit(9); + + size_out = 4 + ntohs(*(short *)&buf_in[1]); + if (sendto(sockfd, buf_in, size_out, 0, (struct sockaddr *)&sa, + sizeof(struct sockaddr_in)) != size_out) + exit(7); + } + + return 0; +}