static switch_status_t switch_codec2_decode(switch_codec_t *codec,
switch_codec_t *other_codec,
void *encoded_data,
uint32_t encoded_data_len,
uint32_t encoded_rate,
void *decoded_data,
uint32_t *decoded_data_len,
uint32_t *decoded_rate,
unsigned int *flag)
{
struct codec2_context *context = codec->private_info;
codec2_assert(encoded_data_len == 8 /* aligned to 8 */);
#ifdef LOG_DATA
fwrite(encoded_data, encoded_data_len, 1, context->decoder_in);
fflush(context->decoder_in);
fwrite(bits, sizeof(bits), 1, context->decoder_in_unpacked);
fflush(context->decoder_in_unpacked);
#endif
codec2_decode(context->decoder, decoded_data, encoded_data);
#ifdef LOG_DATA
fwrite(decoded_data, CODEC2_SAMPLES_PER_FRAME, 2, context->decoder_out);
fflush(context->decoder_out);
#endif
*decoded_data_len = CODEC2_SAMPLES_PER_FRAME * 2; /* 160 samples */
return SWITCH_STATUS_SUCCESS;
}
int main(int argc, char *argv[])
{
struct CODEC2 *codec2;
FILE *fin;
FILE *fout;
short *buf;
unsigned char *bits;
int nsam, nbit, i, r;
for(i=0; i<10; i++) {
r = codec2_rand();
printf("[%d] r = %d\n", i, r);
}
if (argc != 3) {
printf("usage: %s InputRawSpeechFile OutputRawSpeechFile\n", argv[0]);
exit(1);
}
if ( (fin = fopen(argv[1],"rb")) == NULL ) {
fprintf(stderr, "Error opening input speech file: %s: %s.\n",
argv[1], strerror(errno));
exit(1);
}
if ( (fout = fopen(argv[2],"wb")) == NULL ) {
fprintf(stderr, "Error opening output speech file: %s: %s.\n",
argv[2], strerror(errno));
exit(1);
}
#ifdef DUMP
dump_on("c2demo");
#endif
/* Note only one set of Codec 2 states is required for an encoder
and decoder pair. */
codec2 = codec2_create(CODEC2_MODE_1300);
nsam = codec2_samples_per_frame(codec2);
buf = (short*)malloc(nsam*sizeof(short));
nbit = codec2_bits_per_frame(codec2);
bits = (unsigned char*)malloc(nbit*sizeof(char));
while(fread(buf, sizeof(short), nsam, fin) == (size_t)nsam) {
codec2_encode(codec2, bits, buf);
codec2_decode(codec2, buf, bits);
fwrite(buf, sizeof(short), nsam, fout);
}
free(buf);
free(bits);
codec2_destroy(codec2);
fclose(fin);
fclose(fout);
return 0;
}
static void c2demo(int mode, char inputfile[], char outputfile[])
{
struct CODEC2 *codec2;
short *inbuf, *outbuf;
unsigned char *bits;
int nsam, nbit;
FILE *fin, *fout;
int frame;
PROFILE_VAR(enc_start, dec_start);
codec2 = codec2_create(mode);
nsam = codec2_samples_per_frame(codec2);
outbuf = (short*)malloc(nsam*sizeof(short));
inbuf = (short*)malloc(nsam*sizeof(short));
nbit = codec2_bits_per_frame(codec2);
bits = (unsigned char*)malloc(nbit*sizeof(char));
fin = fopen(inputfile, "rb");
if (fin == NULL) {
printf("Error opening input file: %s\n\nTerminating....\n",inputfile);
exit(1);
}
fout = fopen(outputfile, "wb");
if (fout == NULL) {
printf("Error opening output file: %s\n\nTerminating....\n",outputfile);
exit(1);
}
#ifdef DUMP
dump_on("stm32f4");
#endif
frame = 0;
while (fread(inbuf, sizeof(short), nsam, fin) == nsam) {
PROFILE_SAMPLE(enc_start);
codec2_encode(codec2, bits, inbuf);
PROFILE_SAMPLE_AND_LOG(dec_start, enc_start, " enc");
codec2_decode(codec2, outbuf, bits);
PROFILE_SAMPLE_AND_LOG2(dec_start, " dec");
PROFILE_SAMPLE_AND_LOG2(enc_start, " enc & dec");
fwrite((char*)outbuf, sizeof(short), nsam, fout);
printf("frame: %d\n", ++frame);
machdep_profile_print_logged_samples();
}
#ifdef DUMP
dump_off("sm32f4");
#endif
fclose(fin);
fclose(fout);
free(inbuf);
free(outbuf);
free(bits);
codec2_destroy(codec2);
}
static void c2speedtest(int mode, char inputfile[])
{
struct CODEC2 *codec2;
short *inbuf, *outbuf, *pinbuf, *dummy_buf;
unsigned char *bits;
int nsam, nbit, nframes;
FILE *fin;
int f, nread;
codec2 = codec2_create(mode);
nsam = codec2_samples_per_frame(codec2);
nframes = SPEED_TEST_SAMPLES/nsam;
outbuf = (short*)malloc(nsam*sizeof(short));
inbuf = (short*)malloc(SPEED_TEST_SAMPLES*sizeof(short));
nbit = codec2_bits_per_frame(codec2);
bits = (unsigned char*)malloc(nbit*sizeof(char));
dummy_buf = (short*)malloc(2*nsam*sizeof(short));
fin = fopen(inputfile, "rb");
if (fin == NULL) {
printf("Error opening input file: %s\nTerminating....\n",inputfile);
exit(1);
}
printf("reading samples ....\n");
nread = fread(inbuf, sizeof(short), SPEED_TEST_SAMPLES, fin);
if (nread != SPEED_TEST_SAMPLES) {
printf("error reading %s, %d samples reqd, %d read\n",
inputfile, SPEED_TEST_SAMPLES, nread);
}
fclose(fin);
pinbuf = inbuf;
for(f=0; f<nframes; f++) {
//printf("read ADC\n");
while(adc1_read(dummy_buf, nsam*2) == -1); /* runs at Fs = 16kHz */
//printf("Codec 2 enc\n");
GPIOD->ODR = (1 << 13);
codec2_encode(codec2, bits, pinbuf);
pinbuf += nsam;
GPIOD->ODR &= ~(1 << 13);
//printf("Codec 2 dec\n");
codec2_decode(codec2, outbuf, bits);
//printf("write to DAC\n");
while(dac1_write(dummy_buf, nsam*2) == -1); /* runs at Fs = 16kHz */
//printf(".");
}
free(inbuf);
free(outbuf);
free(bits);
codec2_destroy(codec2);
}
static int decode(struct audec_state *ads, int16_t *sampv,
size_t *sampc, const uint8_t *buf, size_t len)
{
if (!sampv || !sampc || !buf)
return EINVAL;
if (*sampc < (size_t)codec2_samples_per_frame(ads->c2))
return ENOMEM;
if (len < (size_t)codec2_bits_per_frame(ads->c2)/8)
return EPROTO;
codec2_decode(ads->c2, sampv, buf);
*sampc = codec2_samples_per_frame(ads->c2);
return 0;
}
/*! \brief decode and store in outbuf. */
static int codec2tolin_framein(struct ast_trans_pvt *pvt, struct ast_frame *f)
{
struct codec2_translator_pvt *tmp = pvt->pvt;
int x;
for (x = 0; x < f->datalen; x += CODEC2_FRAME_LEN) {
unsigned char *src = f->data.ptr + x;
int16_t *dst = pvt->outbuf.i16 + pvt->samples;
codec2_decode(tmp->state, dst, src);
pvt->samples += CODEC2_SAMPLES;
pvt->datalen += CODEC2_SAMPLES * 2;
}
return 0;
}
int main(int argc, char *argv[])
{
void *codec2;
FILE *fin;
FILE *fout;
short buf[CODEC2_SAMPLES_PER_FRAME];
unsigned char bits[BITS_SIZE];
if (argc != 3) {
printf("usage: %s InputBitFile OutputRawSpeechFile\n", argv[0]);
exit(1);
}
if (strcmp(argv[1], "-") == 0) fin = stdin;
else if ( (fin = fopen(argv[1],"rb")) == NULL ) {
fprintf(stderr, "Error opening input bit file: %s: %s.\n",
argv[1], strerror(errno));
exit(1);
}
if (strcmp(argv[2], "-") == 0) fout = stdout;
else if ( (fout = fopen(argv[2],"wb")) == NULL ) {
fprintf(stderr, "Error opening output speech file: %s: %s.\n",
argv[2], strerror(errno));
exit(1);
}
codec2 = codec2_create();
while(fread(bits, sizeof(char), BITS_SIZE, fin) == BITS_SIZE) {
codec2_decode(codec2, buf, bits);
fwrite(buf, sizeof(short), CODEC2_SAMPLES_PER_FRAME, fout);
//if this is in a pipeline, we probably don't want the usual
//buffering to occur
if (fout == stdout) fflush(stdout);
if (fin == stdin) fflush(stdin);
}
codec2_destroy(codec2);
fclose(fin);
fclose(fout);
return 0;
}
int main(int argc, char *argv[]) {
FILE *fin, *fout, *ftxt;
short *speech_out;
short *demod_in;
struct freedv *freedv;
int nin, nout, frame = 0;
struct my_callback_state my_cb_state;
struct MODEM_STATS stats;
int mode;
int sync;
int total_bits;
int total_bit_errors;
float snr_est;
int n_speech_samples;
int n_max_modem_samples;
float clock_offset;
int use_codecrx;
struct CODEC2 *c2 = NULL;
int i;
if (argc < 4) {
printf("usage: %s 1600|700|700B|2400A|2400B|800XA InputModemSpeechFile OutputSpeechRawFile [--test_frames] [--codecrx]\n", argv[0]);
printf("e.g %s 1600 hts1a_fdmdv.raw hts1a_out.raw txtLogFile\n", argv[0]);
exit(1);
}
mode = -1;
if (!strcmp(argv[1],"1600"))
mode = FREEDV_MODE_1600;
if (!strcmp(argv[1],"700"))
mode = FREEDV_MODE_700;
if (!strcmp(argv[1],"700B"))
mode = FREEDV_MODE_700B;
if (!strcmp(argv[1],"2400A"))
mode = FREEDV_MODE_2400A;
if (!strcmp(argv[1],"2400B"))
mode = FREEDV_MODE_2400B;
if (!strcmp(argv[1],"800XA"))
mode = FREEDV_MODE_800XA;
assert(mode != -1);
if (strcmp(argv[2], "-") == 0) fin = stdin;
else if ( (fin = fopen(argv[2],"rb")) == NULL ) {
fprintf(stderr, "Error opening input raw modem sample file: %s: %s.\n",
argv[2], strerror(errno));
exit(1);
}
if (strcmp(argv[3], "-") == 0) fout = stdout;
else if ( (fout = fopen(argv[3],"wb")) == NULL ) {
fprintf(stderr, "Error opening output speech sample file: %s: %s.\n",
argv[3], strerror(errno));
exit(1);
}
freedv = freedv_open(mode);
assert(freedv != NULL);
use_codecrx = 0;
if (argc > 4) {
for (i = 4; i < argc; i++) {
if (strcmp(argv[i], "--testframes") == 0) {
freedv_set_test_frames(freedv, 1);
}
if (strcmp(argv[i], "--codecrx") == 0) {
int c2_mode;
if (mode == FREEDV_MODE_700) {
c2_mode = CODEC2_MODE_700;
} else if ((mode == FREEDV_MODE_700B)|| (mode == FREEDV_MODE_800XA)) {
c2_mode = CODEC2_MODE_700B;
} else {
c2_mode = CODEC2_MODE_1300;
}
use_codecrx = 1;
c2 = codec2_create(c2_mode);
assert(c2 != NULL);
}
}
}
freedv_set_snr_squelch_thresh(freedv, -100.0);
freedv_set_squelch_en(freedv, 1);
n_speech_samples = freedv_get_n_speech_samples(freedv);
n_max_modem_samples = freedv_get_n_max_modem_samples(freedv);
speech_out = (short*)malloc(sizeof(short)*n_speech_samples);
assert(speech_out != NULL);
demod_in = (short*)malloc(sizeof(short)*n_max_modem_samples);
assert(demod_in != NULL);
ftxt = fopen("freedv_rx_log.txt","wt");
assert(ftxt != NULL);
my_cb_state.ftxt = ftxt;
freedv_set_callback_txt(freedv, &my_put_next_rx_char, NULL, &my_cb_state);
freedv_set_callback_protocol(freedv, &my_put_next_rx_proto, NULL, &my_cb_state);
freedv_set_callback_data(freedv, my_datarx, my_datatx, &my_cb_state);
/* Note we need to work out how many samples demod needs on each
call (nin). This is used to adjust for differences in the tx and rx
sample clock frequencies. Note also the number of output
speech samples is time varying (nout). */
nin = freedv_nin(freedv);
while(fread(demod_in, sizeof(short), nin, fin) == nin) {
frame++;
if (use_codecrx == 0) {
/* Use the freedv_api to do everything: speech decoding, demodulating */
nout = freedv_rx(freedv, speech_out, demod_in);
} else {
int bits_per_codec_frame = codec2_bits_per_frame(c2);
int bytes_per_codec_frame = (bits_per_codec_frame + 7) / 8;
int codec_frames = freedv_get_n_codec_bits(freedv) / bits_per_codec_frame;
int samples_per_frame = codec2_samples_per_frame(c2);
unsigned char encoded[bytes_per_codec_frame * codec_frames];
/* Use the freedv_api to demodulate only */
nout = freedv_codecrx(freedv, encoded, demod_in);
/* deccode the speech ourself (or send it to elsewhere, e.g. network) */
if (nout) {
unsigned char *enc_frame = encoded;
short *speech_frame = speech_out;
nout = 0;
for (i = 0; i < codec_frames; i++) {
codec2_decode(c2, speech_frame, enc_frame);
enc_frame += bytes_per_codec_frame;
speech_frame += samples_per_frame;
nout += samples_per_frame;
}
}
}
nin = freedv_nin(freedv);
fwrite(speech_out, sizeof(short), nout, fout);
freedv_get_modem_stats(freedv, &sync, &snr_est);
freedv_get_modem_extended_stats(freedv,&stats);
total_bit_errors = freedv_get_total_bit_errors(freedv);
clock_offset = stats.clock_offset;
/* log some side info to the txt file */
if (ftxt != NULL) {
fprintf(ftxt, "frame: %d demod sync: %d nin:%d demod snr: %3.2f dB bit errors: %d clock_offset: %f\n",
frame, sync, nin, snr_est, total_bit_errors, clock_offset);
}
/* if this is in a pipeline, we probably don't want the usual
buffering to occur */
if (fout == stdout) fflush(stdout);
if (fin == stdin) fflush(stdin);
}
if (freedv_get_test_frames(freedv)) {
total_bits = freedv_get_total_bits(freedv);
total_bit_errors = freedv_get_total_bit_errors(freedv);
fprintf(stderr, "bits: %d errors: %d BER: %3.2f\n", total_bits, total_bit_errors, (float)total_bit_errors/total_bits);
}
free(speech_out);
free(demod_in);
freedv_close(freedv);
fclose(fin);
fclose(fout);
return 0;
}
void per_frame_rx_processing(short output_buf[], /* output buf of decoded speech samples */
int *n_output_buf, /* how many samples currently in output_buf[] */
int codec_bits[], /* current frame of bits for decoder */
short input_buf[], /* input buf of modem samples input to demod */
int *n_input_buf /* how many samples currently in input_buf[] */
)
{
int sync_bit;
COMP rx_fdm[FDMDV_MAX_SAMPLES_PER_FRAME];
int rx_bits[FDMDV_BITS_PER_FRAME];
unsigned char packed_bits[BYTES_PER_CODEC_FRAME];
float rx_spec[FDMDV_NSPEC];
int i, nin_prev, bit, byte;
int next_state;
assert(*n_input_buf <= (2*FDMDV_NOM_SAMPLES_PER_FRAME));
/*
This while loop will run the demod 0, 1 (nominal) or 2 times:
0: when tx sample clock runs faster than rx, occasionally we
will run out of samples
1: normal, run decoder once, every 2nd frame output a frame of
speech samples to D/A
2: when tx sample clock runs slower than rx, occasionally we will
have enough samples to run demod twice.
With a +/- 10 Hz sample clock difference at FS=8000Hz (+/- 1250
ppm), case 0 or 1 occured about once every 30 seconds. This is
no problem for the decoded audio.
*/
while(*n_input_buf >= g_nin) {
// demod per frame processing
for(i=0; i<g_nin; i++) {
rx_fdm[i].real = (float)input_buf[i]/FDMDV_SCALE;
rx_fdm[i].imag = 0.0;
}
nin_prev = g_nin;
fdmdv_demod(fdmdv, rx_bits, &sync_bit, rx_fdm, &g_nin);
*n_input_buf -= nin_prev;
assert(*n_input_buf >= 0);
// shift input buffer
for(i=0; i<*n_input_buf; i++)
input_buf[i] = input_buf[i+nin_prev];
// compute rx spectrum & get demod stats, and update GUI plot data
fdmdv_get_rx_spectrum(fdmdv, rx_spec, rx_fdm, nin_prev);
// Average rx spectrum data using a simple IIR low pass filter
for(i = 0; i < FDMDV_NSPEC; i++)
{
g_avmag[i] = BETA * g_avmag[i] + (1.0 - BETA) * rx_spec[i];
}
fdmdv_get_demod_stats(fdmdv, &stats);
jni_update_stats(&stats, g_avmag);
count++;
/*
State machine to:
+ Mute decoded audio when out of sync. The demod is synced
when we are using the fine freq estimate and SNR is above
a thresh.
+ Decode codec bits only if we have a 0,1 sync bit
sequence. Collects two frames of demod bits to decode
one frame of codec bits.
*/
next_state = g_state;
switch (g_state) {
case 0:
/* mute output audio when out of sync */
if (*n_output_buf < 2*codec2_samples_per_frame(codec2) - N8) {
for(i=0; i<N8; i++)
output_buf[*n_output_buf + i] = 0;
*n_output_buf += N8;
}
if (!(*n_output_buf <= (2*codec2_samples_per_frame(codec2)))) {
LOGE("*n_output_buf <= (2*codec2_samples_per_frame(codec2))");
}
if ((stats.fest_coarse_fine == 1))// && (stats.snr_est > 3.0))
next_state = 1;
break;
case 1:
if (sync_bit == 0) {
next_state = 2;
/* first half of frame of codec bits */
memcpy(codec_bits, rx_bits, FDMDV_BITS_PER_FRAME*sizeof(int));
}
else
next_state = 1;
if (stats.fest_coarse_fine == 0)
next_state = 0;
break;
case 2:
next_state = 1;
if (stats.fest_coarse_fine == 0)
next_state = 0;
if (sync_bit == 1) {
/* second half of frame of codec bits */
memcpy(&codec_bits[FDMDV_BITS_PER_FRAME], rx_bits,
FDMDV_BITS_PER_FRAME*sizeof(int));
// extract data bit
int data_flag_index = codec2_get_spare_bit_index(codec2);
assert(data_flag_index != -1); // not supported for all rates
short abit = codec_bits[data_flag_index];
char ascii_out;
int n_ascii = varicode_decode(&g_varicode_dec_states,
&ascii_out, &abit, 1, 1);
assert((n_ascii == 0) || (n_ascii == 1));
if (n_ascii) {
short ashort = ascii_out;
LOGD("%c", ashort);
}
// reconstruct missing bit we steal for data bit and decode
// speech
codec2_rebuild_spare_bit(codec2, codec_bits);
/* pack bits, MSB received first */
bit = 7; byte = 0;
memset(packed_bits, 0, BYTES_PER_CODEC_FRAME);
for(i=0; i<BITS_PER_CODEC_FRAME; i++) {
packed_bits[byte] |= (codec_bits[i] << bit);
bit--;
if (bit < 0) {
bit = 7;
byte++;
}
}
assert(byte == BYTES_PER_CODEC_FRAME);
/* add decoded speech to end of output buffer */
if (*n_output_buf <= codec2_samples_per_frame(codec2)) {
codec2_decode(codec2, &output_buf[*n_output_buf], packed_bits);
*n_output_buf += codec2_samples_per_frame(codec2);
}
assert(*n_output_buf <= (2*codec2_samples_per_frame(codec2)));
}
break;
}
if (!!g_state != !!next_state) {
jni_update_sync(g_state == 0);
}
g_state = next_state;
}
}
int main (int argc, char ** argv) {
// vars for networking
int udpport;
struct sockaddr_in localsa4;
struct sockaddr_in6 localsa6;
struct sockaddr * receivefromsa = NULL; // structure used for sendto
socklen_t receivefromsa_size=0; // size of structed used for sendto
unsigned char *udpbuffer;
int udpsock;
int udpsize;
// vars for audio
int16_t *inaudiobuffer; // tempory audio buffer used when audio sampling is needed
float *inaudiobuffer_f = NULL; // used for audio samplerate conversion
float *outaudiobuffer_f = NULL; // used for audio samplerate conversion
// for SAMPLE RATE CONVERSION
SRC_STATE *src=NULL;
SRC_DATA src_data;
int src_error;
// vars for codec2
void *codec2;
int mode, nc2byte;
// other vars
int ret;
int new_ptr_audio_write;
int state;
// structure for c2encap data
c2encap c2_voice;
c2encap c2_begin, c2_end;
uint8_t *c2encap_type;
uint8_t *c2encap_begindata;
// "audio out" posix thread
pthread_t thr_audioout;
// init data
global.stereo=-1;
global.ptr_audio_write=1;
global.ptr_audio_read=0;
global.exact=0;
// We need at least 2 arguments: udpport and samplerate
if (argc < 3) {
fprintf(stderr,"Error: at least 2 arguments needed. \n");
fprintf(stderr,"Usage: %s <udp port> <samplerate> [ <audiodevice> [exact] ] \n",argv[0]);
fprintf(stderr,"Note: allowed audio samplerate are 8000, 44100 or 48000 samples/second.\n");
fprintf(stderr,"Note: use device \"\" to get list of devices.\n");
exit(-1);
}; // end if
udpport=atoi(argv[1]);
global.rate=atoi(argv[2]);
// if 1st argument exists, use it as capture device
if (argc >= 4) {
global.pa_device = argv[3];
// is there the "exact" statement?
if (argc >= 5) {
if (!strcmp(argv[4],"exact")) {
global.exact=1;
} else {
fprintf(stderr,"Error: parameter \"exact\" expected. Got %s. Ignoring! \n",argv[4]);
}; // end else - if
}; // end if
} else {
// no argument given; use default
global.pa_device = NULL;
}; // end else - if
// sample rates below 8Ksamples/sec or above 48Ksamples/sec do not make sence
if ((global.rate != 8000) && (global.rate != 44100) && (global.rate != 48000)) {
fprintf(stderr,"Error: audio samplerate should be 8000, 44100 or 48000 samples/sec! \n");
exit(-1);
}; // end if
// create network structure
if ((udpport < 0) || (udpport > 65535)) {
fprintf(stderr,"Error: UDPport number must be between 0 and 65535! \n");
exit(-1);
}; // end if
if ((IPV4ONLY) && (IPV6ONLY)) {
fprintf(stderr,"Error: internal configuration error: ipv4only and ipv6only are mutually exclusive! \n");
exit(-1);
}; // end if
// initialise UDP buffer
udpbuffer=malloc(1500); // we can receive up to 1500 octets
if (!udpbuffer) {
fprintf(stderr,"Error: could not allocate memory for udpbuffer!\n");
exit(-1);
}; // end if
// set pointers for c2encap type and c2encap begin-of-data
c2encap_type = (uint8_t*) &udpbuffer[3];
c2encap_begindata = (uint8_t*) &udpbuffer[4];
// open inbound UDP socket and bind to socket
if (IPV4ONLY) {
udpsock=socket(AF_INET,SOCK_DGRAM,0);
localsa4.sin_family=AF_INET;
localsa4.sin_port=udpport;
memset(&localsa4.sin_addr,0,sizeof(struct in_addr)); // address = "::" (all 0) -> we listen
receivefromsa=(struct sockaddr *) &localsa4;
ret=bind(udpsock, receivefromsa, sizeof(localsa4));
} else {
// IPV6 socket can handle both ipv4 and ipv6
udpsock=socket(AF_INET6,SOCK_DGRAM,0);
// if ipv6 only, set option
if (IPV6ONLY) {
int yes=1;
// make socket ipv6-only.
ret=setsockopt(udpsock,IPPROTO_IPV6, IPV6_V6ONLY, (char *)&yes,sizeof(int));
if (ret == -1) {
fprintf(stderr,"Error: IPV6ONLY option set but could not make socket ipv6 only: %d (%s)! \n",errno,strerror(errno));
return(-1);
}; // end if
}; // end if
localsa6.sin6_family=AF_INET6;
localsa6.sin6_port=htons(udpport);
localsa6.sin6_flowinfo=0; // flows not used
localsa6.sin6_scope_id=0; // we listen on all interfaces
memset(&localsa6.sin6_addr,0,sizeof(struct in6_addr)); // address = "::" (all 0) -> we listen
receivefromsa=(struct sockaddr *)&localsa6;
ret=bind(udpsock, receivefromsa, sizeof(localsa6));
}; // end else - elsif - if
if (ret < 0) {
fprintf(stderr,"Error: could not bind network-address to socket: %d (%s) \n",errno,strerror(errno));
exit(-1);
}; // end if
// start audio out thread
pthread_create (&thr_audioout, NULL, funct_audioout, (void *) &global);
// init c2encap structures
memcpy(c2_begin.header,C2ENCAP_HEAD,sizeof(C2ENCAP_HEAD));
c2_begin.header[3]=C2ENCAP_MARK_BEGIN;
memcpy(c2_begin.c2data.c2data_text3,"BEG",3);
memcpy(c2_end.header,C2ENCAP_HEAD,sizeof(C2ENCAP_HEAD));
c2_end.header[3]=C2ENCAP_MARK_END;
memcpy(c2_end.c2data.c2data_text3,"END",3);
memcpy(c2_voice.header,C2ENCAP_HEAD,sizeof(C2ENCAP_HEAD));
c2_voice.header[3]=C2ENCAP_DATA_VOICE1400;
// in the mean time, do some other things while the audio-process initialises
// init codec2
mode = CODEC2_MODE_1400;
codec2 = codec2_create (mode);
nc2byte = (codec2_bits_per_frame(codec2) + 7) >> 3; // ">>3" is same as "/8"
if (nc2byte != 7) {
fprintf(stderr,"Error: number of bytes for codec2 frames should be 7. We got %d \n",nc2byte);
}; // end if
if (codec2_samples_per_frame(codec2) != 320) {
fprintf(stderr,"Error: number of samples for codec2 frames should be 320. We got %d \n",codec2_samples_per_frame(codec2));
}; // end if
// wait for thread audio to initialise
while (!global.audioready) {
// sleep 5 ms
usleep(5000);
}; // end while
// done. Just to be sure, check "stereo" setting, should be "0" or "1"
if ((global.stereo != 0) && (global.stereo != 1)) {
fprintf(stderr,"Internal error: stereo flag not set correctly by audioout subthread! Should not happen! Exiting. \n");
exit(-1);
}; // end if
if (global.rate != 8000) {
// allocate memory for audio sample buffers (only needed when audio rate conversion is used)
inaudiobuffer=malloc(320 * sizeof(int16_t));
if (!inaudiobuffer) {
fprintf(stderr,"Error in malloc for inaudiobuffer! \n");
exit(-1);
}; // end if
inaudiobuffer_f=malloc(320 * sizeof(float));
if (!inaudiobuffer_f) {
fprintf(stderr,"Error in malloc for inaudiobuffer_f! \n");
exit(-1);
}; // end if
outaudiobuffer_f=malloc(global.numSample * sizeof(float));
if (!outaudiobuffer_f) {
fprintf(stderr,"Error in malloc for outaudiobuffer_f! \n");
exit(-1);
}; // end if
// init samplerate conversion
src = src_new(SRC_SINC_FASTEST, 1, &src_error);
if (!src) {
fprintf(stderr,"src_new failed! \n");
exit(-1);
}; // end if
src_data.data_in = inaudiobuffer_f;
src_data.data_out = outaudiobuffer_f;
src_data.input_frames = 320; // 40ms @ 8000 samples/sec
src_data.output_frames = global.numSample;
src_data.end_of_input = 0; // no further data, every 40 ms is processed on itself
if (global.rate == 48000) {
src_data.src_ratio = (float) 48000/8000;
} else {
src_data.src_ratio = (float) 44100/8000;
}; // end if
}; // end if
// init state
state=0; // state 0 = wait for start
while (FOREVER ) {
// wait for UDP packets
// read until read or error, but ignore "EINTR" errors
while (FOREVER) {
udpsize = recvfrom(udpsock, udpbuffer, 1500, 0, receivefromsa, &receivefromsa_size);
if (udpsize > 0) {
// break out if really packet received;
break;
}; // end if
// break out when not error EINTR
if (errno != EINTR) {
break;
}; // end if
}; // end while (read valid UDP packet)
if (udpsize < 0) {
// error: print message, wait 1/4 of a second and retry
fprintf(stderr,"Error receiving UDP packets: %d (%s) \n",errno, strerror(errno));
usleep(250000);
continue;
}; // end if
if (udpsize < 4) {
// should be at least 4 octets: to small, ignore it
fprintf(stderr,"Error: received UDP packet to small (size = %d). Ignoring! \n",udpsize);
continue;
}; // end if
// check beginning of frame, it should contain the c2enc signature
if (memcmp(udpbuffer,C2ENCAP_HEAD,3)) {
// signature does not match, ignore packet
continue;
}; // end if
// check size + content
// we know the udp packet is at least 4 octets, so check 4th char for type
if (*c2encap_type == C2ENCAP_MARK_BEGIN) {
if (udpsize < C2ENCAP_SIZE_MARK ) {
fprintf(stderr,"Error: received C2ENCAP BEGIN MARKER with to small size: %d octets! Ignoring\n",udpsize);
continue;
} else if (udpsize > C2ENCAP_SIZE_MARK) {
fprintf(stderr,"Warning: received C2ENCAP BEGIN MARKER with to large size: %d octets! Ignoring extra data\n",udpsize);
};
// check content
if (memcmp(c2encap_begindata,"BEG",3)) {
fprintf(stderr,"Error: RECEIVED C2ENCAP BEGIN MARKER WITH INCORRECT TEXT: 0X%02X 0X%02X 0X%02X. Ignoring frame!\n",udpbuffer[4],udpbuffer[5],udpbuffer[6]);
continue;
}; // end if
} else if (*c2encap_type == C2ENCAP_MARK_END) {
if (udpsize < C2ENCAP_SIZE_MARK ) {
fprintf(stderr,"Error: received C2ENCAP END MARKER with to small size: %d octets! Ignoring\n",udpsize);
continue;
} else if (udpsize > C2ENCAP_SIZE_MARK) {
fprintf(stderr,"Warning: received C2ENCAP END MARKER with to large size: %d octets! Ignoring extra data\n",udpsize);
};
// check content
if (memcmp(c2encap_begindata,"END",3)) {
fprintf(stderr,"Error: RECEIVED C2ENCAP BEGIN MARKER WITH INCORRECT TEXT: 0X%02X 0X%02X 0X%02X. Ignoring frame!\n",udpbuffer[4],udpbuffer[5],udpbuffer[6]);
continue;
}; // end if
} else if (*c2encap_type == C2ENCAP_DATA_VOICE1200) {
if (udpsize < C2ENCAP_SIZE_VOICE1200 ) {
fprintf(stderr,"Warning: received C2ENCAP VOICE1200 with to small size: %d octets! Ignoring\n",udpsize);
continue;
} else if (udpsize > C2ENCAP_SIZE_VOICE1200) {
fprintf(stderr,"Warning: received C2ENCAP VOICE1200 with to large size: %d octets! Ignoring extra data\n",udpsize);
};
} else if (*c2encap_type == C2ENCAP_DATA_VOICE1400) {
if (udpsize < C2ENCAP_SIZE_VOICE1400 ) {
fprintf(stderr,"Warning: received C2ENCAP VOICE1400 with to small size: %d octets! Ignoring\n",udpsize);
continue;
} else if (udpsize > C2ENCAP_SIZE_VOICE1400) {
fprintf(stderr,"Warning: received C2ENCAP VOICE1400 with to large size: %d octets! Ignoring extra data\n",udpsize);
};
} else if (*c2encap_type == C2ENCAP_DATA_VOICE2400) {
if (udpsize < C2ENCAP_SIZE_VOICE2400 ) {
fprintf(stderr,"Warning: received C2ENCAP VOICE2400 with to small size: %d octets! Ignoring\n",udpsize);
continue;
} else if (udpsize > C2ENCAP_SIZE_VOICE2400) {
fprintf(stderr,"Warning: received C2ENCAP VOICE2400 with to large size: %d octets! Ignoring extra data\n",udpsize);
};
} else {
fprintf(stderr,"Warning: received packet with unknown type of C2ENCAP type: 0X%02X. Ignoring!\n",*c2encap_type);
continue;
}; // end if
// processing from here on depends on state
if (state == 0) {
// state 0, waiting for start data
if (*c2encap_type == C2ENCAP_MARK_BEGIN) {
// received start, go to state 1
state=1;
continue;
} else {
fprintf(stderr,"Warning: received packet of type 0X%02X in state 0. Ignoring packet! \n",*c2encap_type);
continue;
}; // end if
} else if (state == 1) {
// state 1: receiving voice data, until we receive a "end" marker
if (*c2encap_type == C2ENCAP_MARK_END) {
// end received. Go back to state 0
state=0;
continue;
} else if (*c2encap_type != C2ENCAP_DATA_VOICE1400) {
fprintf(stderr,"Warning: received packet of type 0X%02X in state 1. Ignoring packet! \n",*c2encap_type);
continue;
} else {
// voice 1400 data packet. Decode and play out
// first check if there is place to store the result
new_ptr_audio_write = global.ptr_audio_write+1;
if (new_ptr_audio_write >= NUMBUFF) {
// wrap around at NUMBUFF
new_ptr_audio_write=0;
}; // end if
if (new_ptr_audio_write == global.ptr_audio_read) {
// oeps. No buffers available to write data
fputc('B',stderr);
} else {
// fputc('Q',stderr);
// decode codec2 frame
codec2_decode(codec2, global.audiobuffer[new_ptr_audio_write],c2encap_begindata);
// if not samplerate 8000, do rate conversion
if (global.rate != 8000) {
// convert int16 to float
if (!inaudiobuffer_f) {
fprintf(stderr,"Internal error: inaudiobuffer_f not initialised! \n");
exit(-1);
}; // "end if
if (!outaudiobuffer_f) {
fprintf(stderr,"Internal error: outaudiobuffer_f not initialised! \n");
exit(-1);
}; // "end if
src_short_to_float_array(global.audiobuffer[new_ptr_audio_write],inaudiobuffer_f,320);
// do conversion
ret=src_process(src,&src_data);
if (ret) {
fprintf(stderr,"Warning: samplerate conversion error %d (%s)\n",ret,src_strerror(ret));
}; // end if
// some error checking
if (src_data.output_frames_gen != global.numSample) {
fprintf(stderr,"Warning: number of frames generated by samplerateconvert should be %d, got %ld. \n",global.numSample,src_data.output_frames_gen);
}; // end if
// convert back from float to int, and store immediatly in ringbuffer
src_float_to_short_array(outaudiobuffer_f,global.audiobuffer[new_ptr_audio_write],global.numSample );
}; // end if (samplerate != 8000)
// make stereo (duplicate channels) if needed
if (global.stereo) {
int loop;
int16_t *p1, *p2;
int lastsample_m, lastsample_s;
lastsample_m = global.numSample - 1;
lastsample_s = global.numSample*2 - 1;
// codec2_decode returns a buffer of 16-bit samples, MONO
// so duplicate all samples, start with last sample, move down to sample "1" (not 0);
p1=&global.audiobuffer[new_ptr_audio_write][lastsample_s]; // last sample of buffer (320 samples stereo = 640 samples mono)
p2=&global.audiobuffer[new_ptr_audio_write][lastsample_m]; // last sample of buffer (mono)
for (loop=0; loop < lastsample_m; loop++) {
*p1 = *p2; p1--; // copy 1st octet, move down "write" pointer
*p1 = *p2; p1--; p2--; // copy 2nd time, move down both pointers
}; // end if
// last sample, just copy (no need anymore to move pointers)
*p1 = *p2;
}; // end if
// move up pointer in global vars
global.ptr_audio_write=new_ptr_audio_write;
}; // end if
}; // end if
} else {
fprintf(stderr,"Internal Error: unknow state %d in audioplay main loop. Should not happen. Exiting!!!\n",state);
exit(-1);
}; // end if
}; // end while
fprintf(stderr,"Internal Error: audioplay main application drops out of endless loop. Should not happen! \n");
exit(-1);
}; // end main application
void per_frame_rx_processing(short output_buf[], /* output buf of decoded speech samples */
int *n_output_buf, /* how many samples currently in output_buf[] */
int codec_bits[], /* current frame of bits for decoder */
short input_buf[], /* input buf of modem samples input to demod */
int *n_input_buf /* how many samples currently in input_buf[] */
)
{
int sync_bit;
COMP rx_fdm[FDMDV_MAX_SAMPLES_PER_FRAME];
int rx_bits[FDMDV_BITS_PER_FRAME];
unsigned char packed_bits[BYTES_PER_CODEC_FRAME];
int i, nin_prev, bit, byte;
int next_state;
if (!(*n_input_buf <= (2*FDMDV_NOM_SAMPLES_PER_FRAME))) {
fprintf(stderr, "Assert: *n_input_buf <= (2*FDMDV_NOM_SAMPLES_PER_FRAME)\n");
}
/*
This while loop will run the demod 0, 1 (nominal) or 2 times:
0: when tx sample clock runs faster than rx, occasionally we
will run out of samples
1: normal, run decoder once, every 2nd frame output a frame of
speech samples to D/A
2: when tx sample clock runs slower than rx, occasionally we will
have enough samples to run demod twice.
With a +/- 10 Hz sample clock difference at FS=8000Hz (+/- 1250
ppm), case 0 or 1 occured about once every 30 seconds. This is
no problem for the decoded audio.
*/
while(*n_input_buf >= g_nin) {
// demod per frame processing
for(i=0; i<g_nin; i++) {
rx_fdm[i].real = (float)input_buf[i]/FDMDV_SCALE;
rx_fdm[i].imag = 0.0;
}
nin_prev = g_nin;
fdmdv_demod(fdmdv, rx_bits, &sync_bit, rx_fdm, &g_nin);
*n_input_buf -= nin_prev;
if (!(*n_input_buf >= 0)) {
fprintf(stderr, "Assert: *n_input_buf >= 0\n");
}
// shift input buffer
for(i=0; i<*n_input_buf; i++)
input_buf[i] = input_buf[i+nin_prev];
#if 0
// compute rx spectrum & get demod stats, and update GUI plot data
fdmdv_get_rx_spectrum(fdmdv, rx_spec, rx_fdm, nin_prev);
#endif
fdmdv_get_demod_stats(fdmdv, &stats);
/*
State machine to:
+ Mute decoded audio when out of sync. The demod is synced
when we are using the fine freq estimate and SNR is above
a thresh.
+ Decode codec bits only if we have a 0,1 sync bit
sequence. Collects two frames of demod bits to decode
one frame of codec bits.
*/
next_state = g_state;
switch (g_state) {
case 0:
/* mute output audio when out of sync */
if (*n_output_buf < 2*codec2_samples_per_frame(codec2) - N8) {
for(i=0; i<N8; i++)
output_buf[*n_output_buf + i] = 0;
*n_output_buf += N8;
}
if (!(*n_output_buf <= (2*codec2_samples_per_frame(codec2)))) {
fprintf(stderr, "Assert: *n_output_buf <= (2*codec2_samples_per_frame(codec2))\n");
}
if ((stats.fest_coarse_fine == 1) && (stats.snr_est > 3.0))
next_state = 1;
break;
case 1:
if (sync_bit == 0) {
next_state = 2;
/* first half of frame of codec bits */
memcpy(codec_bits, rx_bits, FDMDV_BITS_PER_FRAME*sizeof(int));
}
else
next_state = 1;
if (stats.fest_coarse_fine == 0)
next_state = 0;
break;
case 2:
next_state = 1;
if (stats.fest_coarse_fine == 0)
next_state = 0;
if (sync_bit == 1) {
/* second half of frame of codec bits */
memcpy(&codec_bits[FDMDV_BITS_PER_FRAME], rx_bits, FDMDV_BITS_PER_FRAME*sizeof(int));
// reconstruct missing bit we steal for data bit and decode speech
codec2_rebuild_spare_bit(codec2, codec_bits);
/* pack bits, MSB received first */
bit = 7; byte = 0;
memset(packed_bits, 0, BYTES_PER_CODEC_FRAME);
for(i=0; i<BITS_PER_CODEC_FRAME; i++) {
packed_bits[byte] |= (codec_bits[i] << bit);
bit--;
if (bit < 0) {
bit = 7;
byte++;
}
}
if (!(byte == BYTES_PER_CODEC_FRAME)) {
fprintf(stderr, "Assert: byte == BYTES_PER_CODEC_FRAME\n");
}
/* add decoded speech to end of output buffer */
if (*n_output_buf <= codec2_samples_per_frame(codec2)) {
codec2_decode(codec2, &output_buf[*n_output_buf], packed_bits);
*n_output_buf += codec2_samples_per_frame(codec2);
}
if (!(*n_output_buf <= (2*codec2_samples_per_frame(codec2)))) {
fprintf(stderr, "Assert: *n_output_buf <= (2*codec2_samples_per_frame(codec2))\n");
}
}
break;
}
g_state = next_state;
}
}
int main(int argc, char *argv[])
{
int mode;
void *codec2;
FILE *fin;
FILE *fout;
FILE *fber = NULL;
short *buf;
unsigned char *bits, *prev_bits;
int nsam, nbit, nbyte, i, byte, frames, bits_proc, bit_errors, error_mode;
int nstart_bit, nend_bit, bit_rate;
int state, next_state;
float ber, r, burst_length, burst_period, burst_timer, ber_est;
unsigned char mask;
if ((argc != 4) && (argc != 5) && (argc != 6) && (argc != 7)) {
printf("basic usage.................: c2dec 3200|2400|1600|1400|1300|1200 InputBitFile OutputRawSpeechFile\n");
printf("uniform errors usage........: c2dec 3200|2400|1600|1400|1300|1200 InputBitFile OutputRawSpeechFile uniformBER startBit endBit\n");
printf("uniform error on range usage: c2dec 3200|2400|1600|1400|1300|1200 InputBitFile OutputRawSpeechFile uniformBER\n");
printf("demod BER estimate..........: c2dec 3200|2400|1600|1400|1300|1200 InputBitFile OutputRawSpeechFile BERfile\n");
printf("two state fading usage......: c2dec 3200|2400|1600|1400|1300|1200 InputBitFile OutputRawSpeechFile burstLength burstPeriod\n");
printf("e.g c2dec 1400 hts1a.c2 hts1a_1400.raw\n");
printf("e.g c2dec 1400 hts1a.c2 hts1a_1400.raw 0.9\n");
printf("e.g c2dec 1400 hts1a.c2 hts1a_1400.raw 0.99 0.9\n");
exit(1);
}
if (strcmp(argv[1],"3200") == 0)
mode = CODEC2_MODE_3200;
else if (strcmp(argv[1],"2400") == 0)
mode = CODEC2_MODE_2400;
else if (strcmp(argv[1],"1600") == 0)
mode = CODEC2_MODE_1600;
else if (strcmp(argv[1],"1400") == 0)
mode = CODEC2_MODE_1400;
else if (strcmp(argv[1],"1300") == 0)
mode = CODEC2_MODE_1300;
else if (strcmp(argv[1],"1200") == 0)
mode = CODEC2_MODE_1200;
else {
fprintf(stderr, "Error in mode: %s. Must be 3200, 2400, 1600, 1400, 1300 or 1200\n", argv[1]);
exit(1);
}
bit_rate = atoi(argv[1]);
if (strcmp(argv[2], "-") == 0) fin = stdin;
else if ( (fin = fopen(argv[2],"rb")) == NULL ) {
fprintf(stderr, "Error opening input bit file: %s: %s.\n",
argv[2], strerror(errno));
exit(1);
}
if (strcmp(argv[3], "-") == 0) fout = stdout;
else if ( (fout = fopen(argv[3],"wb")) == NULL ) {
fprintf(stderr, "Error opening output speech file: %s: %s.\n",
argv[3], strerror(errno));
exit(1);
}
error_mode = NONE;
ber = 0.0;
burst_length = burst_period = 0.0;
burst_timer = 0.0;
codec2 = codec2_create(mode);
nsam = codec2_samples_per_frame(codec2);
nbit = codec2_bits_per_frame(codec2);
buf = (short*)malloc(nsam*sizeof(short));
nbyte = (nbit + 7) / 8;
bits = (unsigned char*)malloc(nbyte*sizeof(char));
prev_bits = (unsigned char*)malloc(nbyte*sizeof(char));
frames = bit_errors = bits_proc = 0;
nstart_bit = 0;
nend_bit = nbit-1;
if (argc == 5) {
/* see if 4th argument is a valid file name */
if ( (fber = fopen(argv[4],"rb")) == NULL ) {
/* otherwise it must be BER value for uniform errors */
ber = atof(argv[4]);
error_mode = UNIFORM;
}
}
if (argc == 6) {
error_mode = TWO_STATE;
burst_length = atof(argv[4]);
burst_period = atof(argv[5]);
nstart_bit = 0;
nend_bit = 2;
state = 0;
}
if (argc == 7) {
error_mode = UNIFORM_RANGE;
ber = atof(argv[4]);
nstart_bit = atoi(argv[5]);
nend_bit = atoi(argv[6]);
fprintf(stderr, "ber: %f nstart_bit: %d nend_bit: %d\n", ber, nstart_bit, nend_bit);
state = 0;
}
assert(nend_bit <= nbit);
while(fread(bits, sizeof(char), nbyte, fin) == (size_t)nbyte) {
frames++;
// apply bit errors, MSB of byte 0 is bit 0 in frame */
if ((error_mode == UNIFORM) || (error_mode == UNIFORM_RANGE)) {
for(i=nstart_bit; i<nend_bit+1; i++) {
r = (float)rand()/RAND_MAX;
if (r < ber) {
byte = i/8;
//printf("nbyte %d nbit %d i %d byte %d bits[%d] 0x%0x ", nbyte, nbit, i, byte, byte, bits[byte]);
mask = 1 << (7 - i + byte*8);
bits[byte] ^= mask;
//printf("shift: %d mask: 0x%0x bits[%d] 0x%0x\n", 7 - i + byte*8, mask, byte, bits[byte] );
bit_errors++;
}
bits_proc++;
}
}
if (error_mode == TWO_STATE) {
burst_timer += (float)nbit/bit_rate;
fprintf(stderr, "burst_timer: %f state: %d\n", burst_timer, state);
next_state = state;
switch(state) {
case 0:
/* clear channel state - no bit errors */
if (burst_timer > (burst_period - burst_length))
next_state = 1;
break;
case 1:
/* burst error state - 50% bit error rate */
for(i=nstart_bit; i<nend_bit+1; i++) {
r = (float)rand()/RAND_MAX;
if (r < 0.5) {
byte = i/8;
bits[byte] ^= 1 << (7 - i + byte*8);
bit_errors++;
}
bits_proc++;
}
if (burst_timer > burst_period) {
burst_timer = 0.0;
next_state = 0;
}
break;
}
state = next_state;
}
if (fber != NULL) {
if (fread(&ber_est, sizeof(float), 1, fber) != 1) {
fprintf(stderr, "ran out of BER estimates!\n");
exit(1);
}
//fprintf(stderr, "ber_est: %f\n", ber_est);
}
else
ber_est = 0.0;
/* frame repeat logic */
if (ber_est > 0.15) {
//memcpy(bits, prev_bits, nbyte);
// fprintf(stderr, "repeat\n");
}
codec2_decode(codec2, buf, bits, ber_est);
fwrite(buf, sizeof(short), nsam, fout);
//if this is in a pipeline, we probably don't want the usual
//buffering to occur
if (fout == stdout) fflush(stdout);
if (fin == stdin) fflush(stdin);
memcpy(prev_bits, bits, nbyte);
}
if (error_mode)
fprintf(stderr, "actual BER: %1.3f\n", (float)bit_errors/bits_proc);
codec2_destroy(codec2);
free(buf);
free(bits);
fclose(fin);
fclose(fout);
return 0;
}
