diff --git a/Makefile.in b/Makefile.in index 9f928591..26f00470 100644 --- a/Makefile.in +++ b/Makefile.in @@ -5,7 +5,7 @@ SRCS= dna.c server.c client.c peers.c ciphers.c responses.c packetformats.c data rhizome.c rhizome_http.c rhizome_bundle.c rhizome_database.c rhizome_crypto.c \ rhizome_packetformats.c rhizome_fetch.c sqlite3.c encode.c sha2.c randombytes.c \ overlay_broadcast.c dna_identity.c commandline.c serval_packetvisualise.c \ - trans_cache.c keyring.c vomp.c pa_phone.c + trans_cache.c keyring.c vomp.c pa_phone.c fifo.c OBJS= $(SRCS:.c=.o) diff --git a/fifo.c b/fifo.c new file mode 100644 index 00000000..1b1530ce --- /dev/null +++ b/fifo.c @@ -0,0 +1,183 @@ +/* + * This is a simple FIFO implementation using a circular buffer. + * + * Heavily inspired by http://lwn.net/Articles/101808/ + * + * Could probably generalise in a similar fashion to sys/queue.h + * + */ + +#include +#include +#include +#include +#include + +#define min(a, b) \ + ({ __typeof__ (a) _a = (a); \ + __typeof__ (b) _b = (b); \ + _a < _b ? _a : _b; }) + +struct fifo { + unsigned int rdidx; + unsigned int wridx; + unsigned int size; + unsigned int len; + uint8_t buffer[0]; +}; + +/* + * fifo_alloc - allocates a new FIFO + * @size: the size of the internal buffer. + * + */ +struct fifo * +fifo_alloc(unsigned int size) { + struct fifo *fifo; + + if ((fifo = malloc(sizeof(struct fifo) + size)) == NULL) + return NULL; + + fifo->rdidx = fifo->wridx = 0; + fifo->size = size; + fifo->len = 0; + + return fifo; +} + +/* + * fifo_free - frees the FIFO + * @fifo: the fifo to be freed. + */ +void +fifo_free(struct fifo *fifo) { + free(fifo); +} + +/* + * fifo_reset - removes the entire FIFO contents + * @fifo: the fifo to be emptied. + */ +void +fifo_reset(struct fifo *fifo) { + + fifo->rdidx = fifo->wridx = 0; + fifo->len = 0; +} + +/* + * fifo_put - puts some data into the FIFO + * @fifo: the fifo to be used. + * @buffer: the data to be added. + * @len: the length of the data to be added. + * + * This function copies at most 'len' bytes from the 'buffer' into + * the FIFO depending on the free space, and returns the number of + * bytes copied. + */ +unsigned int +fifo_put(struct fifo *fifo, uint8_t *buffer, unsigned int len) { + unsigned int total, remaining; + + total = remaining = min(len, fifo->size - fifo->len); + while (remaining > 0) { + unsigned int l = min(remaining, fifo->size - fifo->wridx); + memcpy(fifo->buffer + fifo->wridx, buffer, l); + fifo->wridx += l; + fifo->wridx %= fifo->size; + fifo->len += l; + buffer += l; + remaining -= l; + } + + return total; +} + +/* + * fifo_get - gets some data from the FIFO + * @fifo: the fifo to be used. + * @buffer: where the data must be copied. + * @len: the size of the destination buffer. + * + * This function copies at most 'len' bytes from the FIFO into the + * 'buffer' and returns the number of copied bytes. + */ +unsigned int +fifo_get(struct fifo *fifo, uint8_t *buffer, unsigned int len) { + unsigned int total, remaining; + + total = remaining = min(len, fifo->len); + while (remaining > 0) { + unsigned int l = min(remaining, fifo->size - fifo->rdidx); + memcpy(buffer, fifo->buffer + fifo->rdidx, l); + fifo->rdidx += l; + fifo->rdidx %= fifo->size; + fifo->len -= l; + buffer += l; + remaining -= l; + } + + return total; +} + +/* + * fifo_unget - puts some data into the FIFO head + * @fifo: the fifo to be used. + * @buffer: the data to be added. + * @len: the length of the data to be added. + * + * This function copies at most 'len' bytes from the 'buffer' into + * the FIFO depending on the free space, and returns the number of + * bytes copied. + */ +unsigned int +fifo_unget(struct fifo *fifo, uint8_t *buffer, unsigned int len) { + unsigned int total, remaining, l; + int dst; + + total = remaining = min(len, fifo->size - fifo->len); + + /* Index to start putting data back */ + dst = fifo->rdidx - len; + while (dst < 0) + dst += fifo->size; + + while (remaining > 0) { + l = min(remaining, fifo->size - dst); + memcpy(fifo->buffer + dst, buffer, l); + + fifo->len += l; + buffer += l; + remaining -= l; + } + + fifo->rdidx = dst; + + return total; +} + +/* + * fifo_avail - returns the number of bytes available for reading in the FIFO + * @fifo: the fifo to be used. + */ +unsigned int +fifo_avail(struct fifo *fifo) { + unsigned int result; + + result = fifo->len; + + return result; +} + +/* + * fifo_space - returns the number of bytes available for writing in the FIFO + * @fifo: the fifo to be used. + */ +unsigned int +fifo_space(struct fifo *fifo) { + unsigned int result; + + result = fifo->size - fifo->len; + + return result; +} diff --git a/fifo.h b/fifo.h new file mode 100644 index 00000000..cb989921 --- /dev/null +++ b/fifo.h @@ -0,0 +1,13 @@ +struct fifo *fifo_alloc(unsigned int size); +void fifo_free(struct fifo *fifo); +void fifo_reset(struct fifo *fifo); +unsigned int fifo_put(struct fifo *fifo, uint8_t *buffer, unsigned int len); +unsigned int fifo_get(struct fifo *fifo, uint8_t *buffer, unsigned int len); +unsigned int fifo_unget(struct fifo *fifo, uint8_t *buffer, unsigned int len); +unsigned int fifo_avail(struct fifo *fifo); +unsigned int fifo_space(struct fifo *fifo); + + + + + diff --git a/pa_phone.c b/pa_phone.c index d0766f63..8dbef1fc 100644 --- a/pa_phone.c +++ b/pa_phone.c @@ -1,90 +1,220 @@ #ifdef WITH_PORTAUDIO -#include "serval.h" + +#include "codec2.h" +#define SPAN_DECLARE(x) x +#include "echo.h" +#include "fifo.h" #include +#include +#include "serval.h" -struct private_data { - int foo; -}; +/* Defines */ +#define MIN(x, y) ((x) > (y) ? y : x) +#define MAX(x, y) ((x) < (y) ? y : x) -struct private_data pd; -PaStream *stream=NULL; +#define CODEC2_BYTES_PER_FRAME ((CODEC2_BITS_PER_FRAME + 7) / 8) + +/* Prototypes */ +typedef struct { + SRC_STATE *src; + + pthread_mutex_t mtx; /* Mutex for frobbing queues */ + + /* Incoming samples after decompression + * Written with result of recvfrom + codec2_decode + * Read by sample rate converter + */ + struct fifo *incoming; + int incoverflow; + + /* Samples after rate conversion + * Written by sample rate converter + * Read by PA callback + */ + struct fifo *incrate; + int underrun; + + /* Outgoing samples + * Written by PA callback + * Read by codec2_encode + sendto + */ + struct fifo *outgoing; + + int overrun; + + echo_can_state_t *echocan; /* Echo canceller state */ + void *codec2; /* Codec2 state */ + +} PaCtx; + +/* Declarations */ + +/* Prototypes */ +void runstream(PaCtx *ctx, int netfd, struct sockaddr *send_addr, socklen_t addrlen); +void freectx(PaCtx *ctx); /* This routine will be called by the PortAudio engine when audio is needed. - It may called at interrupt level on some machines so don't do anything - that could mess up the system like calling malloc() or free(). -*/ -static int paphoneCallback( const void *inputBuffer, void *outputBuffer, - unsigned long framesPerBuffer, - const PaStreamCallbackTimeInfo* timeInfo, - PaStreamCallbackFlags statusFlags, - void *userData ) -{ - /* Cast data passed through stream to our structure. */ - struct private_data *data = (struct private_data*)userData; - uint16_t *out = (uint16_t*)outputBuffer; - uint16_t *in = (uint16_t*)inputBuffer; - unsigned int i; +** It may called at interrupt level on some machines so don't do anything +** that could mess up the system like calling malloc() or free(). +*/ +static int +patestCallback(const void *inputBuffer, void *outputBuffer, + unsigned long framesPerBuffer, + const PaStreamCallbackTimeInfo* timeInfo, + PaStreamCallbackFlags statusFlags, + void *userData) { + PaCtx *ctx; + int16_t *in, *out; + int avail, amt; + + ctx = (PaCtx *)userData; + out = (int16_t *)outputBuffer; + in = (int16_t *)inputBuffer; - /* Add recorded audio to ring buffer */ - /* Play audio from ring buffer. - XXX - Special case for DTMF tones. - DTMF is: - 1209 1336 1477 1633 - 697 1 2 3 A - 770 4 5 6 B - 852 7 8 9 C - 941 * 0 # D - */ + pthread_mutex_lock(&ctx->mtx); + + amt = framesPerBuffer * sizeof(out[0]); + + /* Copy out samples to be played */ + if ((avail = fifo_get(ctx->incrate, (uint8_t *)out, amt)) < amt) { + /* Zero out samples there are no data for */ + bzero(out + (avail / sizeof(out[0])), amt - avail); + ctx->underrun += (amt - avail) / sizeof(out[0]); + } + + /* Copy in samples to be recorded */ + if ((avail = fifo_put(ctx->outgoing, (uint8_t *)in, amt)) < amt) { + /* Zero out samples there are no data for */ + bzero(in + (avail / sizeof(out[0])), amt - avail); + ctx->overrun += (amt - avail) / sizeof(out[0]); + } - return 0; +#if 1 + /* Run the echo canceller */ + for (int ofs = 0; ofs < framesPerBuffer; ofs++) + out[ofs] = echo_can_update(ctx->echocan, in[ofs], out[ofs]); +#endif + pthread_mutex_unlock(&ctx->mtx); + + return paContinue; } -int paphone_setup() -{ - PaError err; - err = Pa_Initialize(); - if( err != paNoError ) goto error; +PaCtx * +pa_phone_setup(void) { + PaCtx *ctx; + int err, err2; + err = paNoError; + err2 = 0; + + if ((ctx = calloc(1, sizeof(PaCtx))) == NULL) { + WHY("Unable to allocate PA context"); + err2 = 1; + goto error; + } + + /* Init mutex */ + if (pthread_mutex_init(&ctx->mtx, NULL) != 0) { + WHY("Unable to init mutex: %s\n", strerror(errno)); + err2 = 1; + goto error; + } + + /* Allocate FIFOs */ + i = IN_FRAMES * 10 * sizeof(int16_t); + printf("Allocating %d byte FIFOs\n", i); + + if ((ctx->incoming = fifo_alloc(i)) == NULL) { + WHY("Unable to allocate incoming FIFO\n"); + err2 = 1; + goto error; + } + + if ((ctx->incrate = fifo_alloc(i)) == NULL) { + WHY("Unable to allocate incoming SRC FIFO\n"); + err2 = 1; + goto error; + } + + if ((ctx->outgoing = fifo_alloc(i)) == NULL) { + WHY("Unable to allocate outgoing FIFO\n"); + err2 = 1; + goto error; + } + + + /* Init sample rate converter */ + if ((ctx->src = src_new(SRC_SINC_BEST_QUALITY, 1, &srcerr)) == NULL) { + WHY("Unable to init sample rate converter: %d\n", srcerr); + err2 = 1; + goto error; + } + + /* Init echo canceller */ + if ((ctx->echocan = echo_can_init(ECHO_LEN, ADAPT_MODE)) == NULL) { + WHY("Unable to init echo canceller\n"); + err2 = 1; + goto error; + } + + /* Init codec2 */ + if ((ctx->codec2 = codec2_create()) == NULL) { + WHY("Unable to init codec2\n"); + err2 = 1; + goto error; + } + + /* Initialize Port Audio library */ + if ((err = Pa_Initialize()) != paNoError) + goto error; + + /* Open an audio I/O stream. */ + if ((err = Pa_OpenDefaultStream(&stream, + 1, /* input channels */ + 1, /* output channels */ + paInt16, + SAMPLE_RATE, + IN_FRAMES, /* frames per buffer */ + patestCallback, + &ctx)) != paNoError) + goto error; - /* Open an audio I/O stream. */ - err = Pa_OpenDefaultStream( &stream, - 1, /* one input channel */ - 1, /* one output channel */ - paInt16, /* sample format */ - 8000, - 8000/40, /* frames per buffer, i.e. the number - of sample frames that PortAudio will - request from the callback. Many apps - may want to use - paFramesPerBufferUnspecified, which - tells PortAudio to pick the best, - possibly changing, buffer size.*/ - paphoneCallback, /* this is your callback function */ - &pd ); /*This is a pointer that will be passed to - your callback*/ - if( err != paNoError ) goto error; + /* Start stream */ + if ((err = Pa_StartStream(stream)) != paNoError) + goto error; - err = Pa_StartStream( stream ); - if( err != paNoError ) goto error; + /* Close down stream, PA, etc */ +/* XXX: hangs in pthread_join on Ubuntu 10.04 */ +#ifndef linux + if ((err = Pa_StopStream(stream)) != paNoError) + goto error; +#endif - return 0; - error: - return WHYF( "PortAudio error: %s\n", Pa_GetErrorText( err ) ); -} + /* Do stuff */ -int paphone_cleanup() -{ - PaError err; - err = Pa_StopStream( stream ); - if( err != paNoError ) goto error; - err = Pa_CloseStream( stream ); - if( err != paNoError ) goto error; + if ((err = Pa_CloseStream(stream)) != paNoError) + goto error; - error: - err = Pa_Terminate(); - if( err != paNoError ) - return WHYF( "PortAudio error: %s\n", Pa_GetErrorText( err ) ); - return 0; + error: + Pa_Terminate(); + + /* Free things */ + freectx(&ctx); + if (netfd != -1) + close(netfd); + + if (err != paNoError) + WHY("Port audio error: %s\n", Pa_GetErrorText(err)); + + return NULL; } #endif + + +/* + * Local variables: + * c-basic-offset: 2 + * End: + */ +