Add PA setup and FIFO code (still not plumbed into the main program)

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)
 

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 <assert.h>
+#include <pthread.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+
+#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;
+}

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);
+
+
+
+
+

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 <portaudio.h>
+#include <samplerate.h>
+#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:
+ */
+