d2460

2 files changed, 377 insertions, 0 deletions

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 <stdio.h>
+#include <time.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <ctype.h>
+#include <errno.h>
+#include <termios.h>
+#include <string.h>
+#include <fcntl.h>
+
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/select.h>
+#include <sys/socket.h>
+#include <sys/param.h>
+
+#include <netinet/in.h>
+
+#include <stdint.h>
+
+#include <mbelib.h>
+#include <ambe.h>
+#include <p25p2_vf.h>
+#include <imbe_decoder.h>
+#include <software_imbe_decoder.h>
+#include "imbe_vocoder/imbe_vocoder.h"
+#include <ambe_encoder.h>
+
+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;
+}