Max 5 years ago
parent e2cfbe9d1e
commit a3cab238b5
  1. 17
      op25/gr-op25_repeater/lib/CMakeLists.txt
  2. 360
      op25/gr-op25_repeater/lib/d2460.cc

@ -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)

@ -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;
}
Loading…
Cancel
Save