op25
/
op25
3
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

d2460

Browse Source
max
Max 5 years ago
parent e2cfbe9d1e
commit a3cab238b5
2 changed files with 377 additions and 0 deletions
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Show Stats Download Patch File Download Diff File
  1. 17
      op25/gr-op25_repeater/lib/CMakeLists.txt
  2. 360
      op25/gr-op25_repeater/lib/d2460.cc

17
op25/gr-op25_repeater/lib/CMakeLists.txt
Unescape View File

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

360
op25/gr-op25_repeater/lib/d2460.cc
Unescape View File

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