Skip to content

Commit

Permalink
add multi-client support
Browse files Browse the repository at this point in the history
  • Loading branch information
@narspt
narspt authored Sep 7, 2022
1 parent 8c04b4f commit 11cae0f
Showing 1 changed file with 151 additions and 16 deletions.
167 changes: 151 additions & 16 deletions emulator/md380-emu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,14 +28,18 @@

#include <netinet/in.h>
#include <netdb.h>
#include <arpa/inet.h>

#include <stdbool.h>
#include <math.h>
#include <time.h>
#include <signal.h>

void *firmware;
void *sram;
void *tcram;
int serverPort = 1234;
int maxClients = 0;

//Maps the firmware image into process memory.
void mapimage(){
Expand Down Expand Up @@ -87,7 +91,8 @@ void version(){
//Prints usage info.
void usage(char *argv0){
printf("Usage: %s [OPTION]\n"
"\t-S port AMBEServer\n"
"\t-s port AMBEServer\n"
"\t-m max Enable multi-client\n"
"\n"
"\t-d Decodes AMBE\n"
"\t-e Encodes AMBE\n"
Expand Down Expand Up @@ -600,9 +605,6 @@ void ambe49toambe72(unsigned char *ambe49, unsigned char *ambe72) {






const unsigned char Interleave49Matrix[] = {
0, 3, 6, 9, 12, 15, 18, 21, 24, 27, 30, 33, 36, 39, 41, 43, 45, 47,
1, 4, 7, 10, 13, 16, 19, 22, 25, 28, 31, 34, 37, 40, 42, 44, 46, 48,
Expand All @@ -629,15 +631,56 @@ void deinterleave49(unsigned char *data) {



void clean_ambe_buffers() {
short pcm[160];
unsigned char silence[][7] = { //silence encoded without dtx
{0x98,0x02,0xB9,0x4F,0xA4,0xD3,0x80},
{0x98,0x02,0xD9,0x1E,0x44,0xC2,0x80},
{0xE8,0x02,0xB4,0x4F,0xA7,0x92,0x80},
{0xE8,0x02,0xD9,0x4E,0x07,0xD2,0x80},
{0xE8,0x02,0xB3,0x1F,0xA7,0x73,0x80},
{0xE8,0x02,0xB9,0x4E,0x47,0xC2,0x80},
{0xE8,0x02,0xD9,0x4F,0xA7,0xD2,0x80},
{0xE8,0x02,0xB4,0x1F,0xA7,0x93,0x80},
{0xE8,0x02,0xB9,0x4E,0x07,0xD2,0x80},
{0xE8,0x02,0xD9,0x4F,0xA7,0xC3,0x80}
};
for (int i=0; i < 10; i++)
decode_amb_buffer(silence[i], pcm);
}






volatile sig_atomic_t stop = 0;

void sigint_handler(int signum) {
stop = 1;
}

void ambeServer(int portNumber) {
struct sockaddr_in sa_read;
//struct hostent *hp; /* host information */

static const unsigned char AMBE_SILENCE[] = {0xAC, 0xAA, 0x40, 0x20, 0x00, 0x44, 0x40, 0x80, 0x80};

struct s_clients {
in_addr_t addr;
in_port_t port;
time_t time;
unsigned char cfg[3];
unsigned short speech_rate;
unsigned short fec_rate;
unsigned short ecmode;
bool parity_mode;
signed char gain_in;
signed char gain_out;
unsigned char sram_a[0x2000];
unsigned char sram_b[0x1000];
//unsigned char sram[0x20000];
} *clients;

static const unsigned char CFG[] = {0x25, 0x21, 0xEC};
unsigned char cfg[3];
memcpy(cfg, CFG, sizeof(CFG));
Expand All @@ -664,9 +707,19 @@ void ambeServer(int portNumber) {

printf("Listening for connections on UDP port %d\n", portNumber);

if (maxClients > 1) { //multi-client
printf("Multi-client enabled, max clients: %d\n", maxClients);
clients = calloc(maxClients, sizeof(struct s_clients));
if (clients == NULL) {
printf("Memory allocation failed.\n");
exit(1);
}
signal(SIGINT, sigint_handler);
}

//int pcmfd=open("migvoz.pcm", 0); //inject test audio

while (1) {
while (!stop) {
static unsigned char buffer[1024], outbuf[1024];
memset(buffer, 0, sizeof(buffer));

Expand All @@ -689,6 +742,51 @@ void ambeServer(int portNumber) {
continue; //ignore packet
}

int client_i = -1;
if (maxClients > 1) { //multi-client
for (int i=0; i < maxClients; i++) { //search existing client
if ((clients[i].addr == sa_read.sin_addr.s_addr) && (clients[i].port == sa_read.sin_port)) {
client_i = i; //found existing client, restore client specific data
memcpy(cfg, clients[client_i].cfg, 3);
speech_rate = clients[client_i].speech_rate;
fec_rate = clients[client_i].fec_rate;
ecmode = clients[client_i].ecmode;
parity_mode = clients[client_i].parity_mode;
gain_in = clients[client_i].gain_in;
gain_out = clients[client_i].gain_out;
memcpy((void*) 0x20000000+0xC400, clients[client_i].sram_a, 0x2000); //sram
memcpy((void*) 0x20000000+0x11000, clients[client_i].sram_b, 0x1000); //sram
//memcpy((void*) 0x20000000, clients[client_i].sram, 0x20000); //sram
break;
}
}
if (client_i < 0) { //client not found, new client
int lastused_i = 0;
for (int i=1; i < maxClients; i++) //search last used client slot
if (clients[i].time < clients[lastused_i].time)
lastused_i = i;
if (time(NULL)-clients[lastused_i].time > 2*60) { //unused for more than x seconds?
client_i = lastused_i; //found unused client slot, reset settings
if (verbosity >= 1) printf("New Client, IP:%s Port:%d Slot:%d\n", inet_ntoa(sa_read.sin_addr), ntohs(sa_read.sin_port), client_i);
memcpy(cfg, CFG, sizeof(CFG));
if ((cfg[1] & 63) == 33) {
speech_rate = 2450; fec_rate = 1150;
} else if ((cfg[1] & 63) == 34) {
speech_rate = 2450; fec_rate = 0;
} else {
speech_rate = 0; fec_rate = 0;
}
ecmode = 0x1000 | (((cfg[0] >> 3) & 1) << 11) | (((cfg[0] >> 5) & 1) << 6);
parity_mode = ((cfg[2] & 16) > 0);
gain_in = 0; gain_out = 0;
clean_ambe_buffers();
} else {
if (verbosity >= 1) printf("Unavailable Client Slots\n");
continue; //ignore packet
}
}
}

if (parity_mode) {
if (buffer[n-2] != 0x2F) { //parity field identifier
if (verbosity >= 1) printf("PARITY FIELD MISSING\n");
Expand Down Expand Up @@ -741,6 +839,7 @@ void ambeServer(int portNumber) {
ecmode = 0x1000 | (((cfg[0] >> 3) & 1) << 11) | (((cfg[0] >> 5) & 1) << 6);
parity_mode = ((cfg[2] & 16) > 0);
gain_in = 0; gain_out = 0;
clean_ambe_buffers();
if (verbosity >= 1) printf("CONTROL RESET, SPEECH_RATE:%d FEC_RATE:%d ECMODE:0x%04X PARITY:%d\n", speech_rate, fec_rate, ecmode, parity_mode);
poutbuf[0] = 0x39; //READY
pbuffer += 1; poutbuf += 1;
Expand All @@ -759,6 +858,7 @@ void ambeServer(int portNumber) {
ecmode = 0x1000 | (((cfg[0] >> 3) & 1) << 11) | (((cfg[0] >> 5) & 1) << 6);
parity_mode = ((cfg[2] & 16) > 0);
gain_in = 0; gain_out = 0;
clean_ambe_buffers();
if (verbosity >= 1) printf("CONTROL RESETSOFTCFG, SPEECH_RATE:%d FEC_RATE:%d ECMODE:0x%04X PARITY:%d\n", speech_rate, fec_rate, ecmode, parity_mode);
poutbuf[0] = 0x39; //READY
pbuffer += 7; poutbuf += 1;
Expand Down Expand Up @@ -833,6 +933,7 @@ void ambeServer(int portNumber) {
} else {
speech_rate = 0; fec_rate = 0;
}
//clean_ambe_buffers();
if (verbosity >= 1) printf("CONTROL RATEP, SPEECH_RATE:%d FEC_RATE:%d\n", speech_rate, fec_rate);
poutbuf[0] = pbuffer[0];
poutbuf[1] = 0x00;
Expand All @@ -846,6 +947,7 @@ void ambeServer(int portNumber) {
} else {
speech_rate = 0; fec_rate = 0;
}
//clean_ambe_buffers();
if (verbosity >= 1) printf("CONTROL RATET, SPEECH_RATE:%d FEC_RATE:%d\n", speech_rate, fec_rate);
poutbuf[0] = pbuffer[0];
poutbuf[1] = 0x00;
Expand Down Expand Up @@ -895,15 +997,16 @@ void ambeServer(int portNumber) {
if ((pbuffer[1] == 72) && (speech_rate == 2450) && (fec_rate == 1150)) { //72-bit AMBE with FEC
unsigned char *ambe72 = &pbuffer[2];
unsigned char ambe49[7];
if (verbosity >= 2) { printf("72-bit AMBE:\n"); dump(ambe72, 9); }
if (verbosity >= 2) { printf("72-bit AMBE (with FEC):\n"); dump(ambe72, 9); }
ambe72toambe49(ambe72, ambe49);
if (verbosity >= 2) { printf("49-bit AMBE:\n"); dump(ambe49, 7); }
if (verbosity >= 2) { printf("49-bit AMBE (deinterleaved):\n"); dump(ambe49, 7); }
decode_amb_buffer(ambe49, pcm);
}
else if ((pbuffer[1] == 49) && (speech_rate == 2450) && (fec_rate == 0)) { //49-bit AMBE without FEC
unsigned char *ambe49 = &pbuffer[2];
if (verbosity >= 2) { printf("49-bit AMBE (interleaved):\n"); dump(ambe49, 7); }
deinterleave49(ambe49);
if (verbosity >= 2) { printf("49-bit AMBE:\n"); dump(ambe49, 7); }
if (verbosity >= 2) { printf("49-bit AMBE (deinterleaved):\n"); dump(ambe49, 7); }
decode_amb_buffer(ambe49, pcm);
}
else if ((pbuffer[1] == 72) && (speech_rate == 2400) && (fec_rate == 1200)) { //DStar
Expand Down Expand Up @@ -961,9 +1064,9 @@ void ambeServer(int portNumber) {
unsigned char ambe49[7];
memset(ambe49, 0, 7);
encode_amb_buffer(ambe49, pcm, ecmode);
if (verbosity >= 2) { printf("49-bit AMBE:\n"); dump(ambe49, 7); }
if (verbosity >= 2) { printf("49-bit AMBE (deinterleaved):\n"); dump(ambe49, 7); }
ambe49toambe72(ambe49, ambe72);
if (verbosity >= 2) { printf("72-bit AMBE:\n"); dump(ambe72, 9); }
if (verbosity >= 2) { printf("72-bit AMBE (with FEC):\n"); dump(ambe72, 9); }
//memcpy(ambe72, AMBE_SILENCE, 9); //dbg
}
else if ((speech_rate == 2450) && (fec_rate == 0)) { //49-bit AMBE without FEC
Expand All @@ -974,8 +1077,9 @@ void ambeServer(int portNumber) {
poutbuf += 7;
memset(ambe49, 0, 7);
encode_amb_buffer(ambe49, pcm, ecmode);
if (verbosity >= 2) { printf("49-bit AMBE:\n"); dump(ambe49, 7); }
if (verbosity >= 2) { printf("49-bit AMBE (deinterleaved):\n"); dump(ambe49, 7); }
interleave49(ambe49);
if (verbosity >= 2) { printf("49-bit AMBE (interleaved):\n"); dump(ambe49, 7); }
}
else if ((speech_rate == 2400) && (fec_rate == 1200)) { //DStar
poutbuf[0] = 0x01; //CHAND field identifier
Expand All @@ -996,7 +1100,7 @@ void ambeServer(int portNumber) {
poutbuf[1] = 0x00; //parity byte
poutbuf += 2;
}

outbuf[0] = 0x61; //START_BYTE
outbuf[1] = (poutbuf-&outbuf[4]) >> 8; //packet length
outbuf[2] = (poutbuf-&outbuf[4]) & 0xFF; //packet length
Expand All @@ -1013,10 +1117,33 @@ void ambeServer(int portNumber) {
}
}

if (client_i >= 0) { //store client specific data
clients[client_i].addr = sa_read.sin_addr.s_addr;
clients[client_i].port = sa_read.sin_port;
clients[client_i].time = time(NULL);
memcpy(clients[client_i].cfg, cfg, 3);
clients[client_i].speech_rate = speech_rate;
clients[client_i].fec_rate = fec_rate;
clients[client_i].ecmode = ecmode;
clients[client_i].parity_mode = parity_mode;
clients[client_i].gain_in = gain_in;
clients[client_i].gain_out = gain_out;
memcpy(clients[client_i].sram_a, (void*) 0x20000000+0xC400, 0x2000); //sram
memcpy(clients[client_i].sram_b, (void*) 0x20000000+0x11000, 0x1000); //sram
//memcpy(clients[client_i].sram+0xC400, (void*) 0x20000000+0xC400, 0x2000); //dbg
//memcpy(clients[client_i].sram+0x11000, (void*) 0x20000000+0x11000, 0x1000); //dbg
//for(int i=0,d=0;i<128;i++) if(memcmp(clients[client_i].sram+(i*1024), (void*)0x20000000+(i*1024), 1024)!=0) printf(d++?",%d":"\n%d",i); //dbg
//memcpy(clients[client_i].sram, (void*) 0x20000000, 0x20000); //sram
}

} //START_BYTE

}
}
if (maxClients > 1) { //multi-client
//printf("deallocating memory...\n");
free(clients);
}
}


Expand All @@ -1034,12 +1161,12 @@ int main(int argc, char **argv){
setvbuf(stderr, NULL, _IOLBF, 0);

verb='h';
while((opt=getopt(argc,argv,"S:edVvo:i:"))!=-1){
while((opt=getopt(argc,argv,"s:m:edVvo:i:"))!=-1){
switch(opt){
/* For any flag that sets the mode, we simply set the verb char
to that mode.
*/
case 'S'://Socket
case 's'://Socket
verb=opt;
serverPort = atoi(optarg);
if (serverPort == 0) {
Expand All @@ -1048,6 +1175,14 @@ int main(int argc, char **argv){
}
break;

case 'm':
maxClients = atoi(optarg);
if (maxClients == 0) {
printf("Error, must specify max number of clients\n");
exit(1);
}
break;

case 'V'://Version
case 'd'://Decode AMBE
case 'e'://Encode AMBE
Expand Down Expand Up @@ -1097,7 +1232,7 @@ int main(int argc, char **argv){
case 'V'://Version
version();
break;
case 'S':
case 's':
ambeServer(serverPort);
break;
default:
Expand Down

0 comments on commit 11cae0f

Please sign in to comment.