resampling works

git-svn-id: file:///home/lennart/svn/public/libsydney/trunk@4 9ba3c220-e4d3-45a2-8aa3-73fcc9aff6ce

1 files changed, 439 insertions, 0 deletions

diff --git a/format.c b/format.c
new file mode 100644
index 0000000..7051fcb
--- /dev/null
+++ b/format.c
@@ -0,0 +1,439 @@
+#include <inttypes.h>
+#include <sys/types.h>
+#include <sys/param.h>
+
+#include <liboil/liboil.h>
+
+#include "format.h"
+#include "g711.h"
+#include "macro.h"
+
+/* u8 --> */
+
+static int format_u8_to_s16(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    int16_t d = -0x80, f = 0x100;
+    
+    oil_conv_s16_u8(dst, dstr, src, sstr, bytes);
+    oil_scalaradd_s16(dst, dstr, dst, dstr, &d, bytes);
+    oil_scalarmult_s16(dst, dstr, dst, dstr, &f, bytes);
+
+    return SA_SUCCESS;
+}
+
+static int format_u8_to_s32(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    int32_t d = -0x80, f = 0x1000000;
+    
+    oil_conv_s16_u8(dst, dstr, src, sstr, bytes);
+    oil_scalaradd_s32(dst, dstr, dst, dstr, &d, bytes);
+    oil_scalarmult_s32(dst, dstr, dst, dstr, &f, bytes);
+
+    return SA_SUCCESS;
+}
+
+static int format_u8_to_f32(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    float d = -0x80, f = 1.0/0x7F;
+
+    oil_conv_f32_u8(dst, dstr, src, sstr, bytes);
+    oil_scalaradd_f32(dst, dstr, dst, dstr, &d, bytes);
+    oil_scalarmult_f32(dst, dstr, dst, dstr, &f, bytes);                       
+
+    return SA_SUCCESS;
+}
+
+/* ulaw --> */
+
+static int format_ulaw_to_s16(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    int16_t *d = dst;
+    const uint8_t *s = src;
+
+    for (; bytes > 0; bytes --, d += dstr/sizeof(int16_t), s += sstr)
+        *d = st_ulaw2linear16(*s);
+
+    return SA_SUCCESS;
+}
+
+static int format_ulaw_to_s32(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    int32_t *d = dst;
+    const uint8_t *s = src;
+
+    for (; bytes > 0; bytes --, d += dstr/sizeof(int32_t), s += sstr)
+        *d = (int32_t) st_ulaw2linear16(*s) * 0x10000;
+
+    return SA_SUCCESS;
+}
+
+static int format_ulaw_to_f32(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    float *d = dst;
+    const uint8_t *s = src;
+
+    for (; bytes > 0; bytes --, d += dstr/sizeof(float), s += sstr)
+        *d = st_ulaw2linear16(*s * 1.0F / 0x7FFF);
+
+    return SA_SUCCESS;
+}
+
+/* alaw --> */
+
+static int format_alaw_to_s16(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    int16_t *d = dst;
+    const uint8_t *s = src;
+
+    for (; bytes > 0; bytes --, d += dstr/sizeof(int16_t), s += sstr)
+        *d = st_alaw2linear16(*s);
+
+    return SA_SUCCESS;
+}
+
+static int format_alaw_to_s32(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    int32_t *d = dst;
+    const uint8_t *s = src;
+
+    for (; bytes > 0; bytes --, d += dstr/sizeof(int32_t), s += sstr)
+        *d = (int32_t) st_alaw2linear16(*(s++)) * 0x10000;
+
+    return SA_SUCCESS;
+}
+
+static int format_alaw_to_f32(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    float *d = dst;
+    const uint8_t *s = src;
+
+    for (; bytes > 0; bytes --, d += dstr/sizeof(float), s += sstr)
+        *d = st_alaw2linear16(*(s++) * 1.0F / 0x7FFF);
+
+    return SA_SUCCESS;
+}
+
+/* s16 --> */
+
+static int format_s16_to_u8(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    uint8_t *d = dst;
+    const int16_t *s = src;
+    unsigned n = bytes/sizeof(int16_t);
+
+    for (; n > 0; n--, d += dstr, s += sstr/sizeof(int16_t))
+        *d = (uint8_t) (*s / 0x100 + 0x80);
+    
+    return SA_SUCCESS;
+}
+
+static int format_s16_to_ulaw(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    uint8_t *d = dst;
+    const int16_t *s = src;
+    unsigned n = bytes/sizeof(int16_t);
+
+    for (; n > 0; n --, d += dstr, s += sstr/sizeof(int16_t))
+        *d = st_14linear2ulaw(*s);
+
+    return SA_SUCCESS;
+}
+
+static int format_s16_to_alaw(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    uint8_t *d = dst;
+    const int16_t *s = src;
+    unsigned n = bytes/sizeof(int16_t);
+
+    for (; n > 0; n --, d += dstr, s += sstr/sizeof(int16_t))
+        *d = st_13linear2alaw(*s);
+
+    return SA_SUCCESS;
+}
+
+static int format_s16_to_s32(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    int f = 0x10000;
+    unsigned n = bytes/sizeof(int16_t);
+
+    oil_conv_s32_s16(dst, dstr, src, sstr, n);
+    oil_scalarmult_s32(dst, dstr, dst, dstr, &f, n);                       
+
+    return SA_SUCCESS;
+}
+
+static int format_s16_to_f32(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    float f = 1.0/0x7ffff;
+    unsigned n = bytes/sizeof(int16_t);
+    
+    oil_conv_f32_s16(dst, dstr, src, sstr, n);
+    oil_scalarmult_f32(dst, dstr, dst, dstr, &f, n);                       
+
+    return SA_SUCCESS;
+}
+
+/* s24 --> */
+
+static int format_s24_to_s32(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    int32_t *d = dst;
+    const uint8_t *s = src;
+    unsigned n = bytes/3;
+
+    for (; n > 0; n--, d += dstr/sizeof(int32_t), s += sstr/3)
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+        *d = (int32_t) ((int8_t) s[2]) * 0x1000000 + s[1] * 0x10000 + s[0] * 0x100;
+#elif __BYTE_ORDER == __BIG_ENDIAN
+        *d = (int32_t) ((int8_t) s[0]) * 0x1000000 + s[1] * 0x10000 + s[2] * 0x100;
+#else
+#error "Unknown byte order"
+#endif
+    
+    return SA_SUCCESS;
+}
+
+static int format_s24_to_f32(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    float *d = dst;
+    const uint8_t *s = src;
+    unsigned n = bytes/3;
+
+    for (; n > 0; n--, d += dstr/sizeof(float), s += sstr/3)
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+        *d = ((float) ((int8_t) s[2]) * 0x10000 + s[1] * 0x100 + s[0]) / 0x7fffff;
+#elif __BYTE_ORDER == __BIG_ENDIAN
+        *d = ((float) ((int8_t) s[0]) * 0x10000 + s[1] * 0x100 + s[2]) / 0x7fffff;
+#else
+#error "Unknown byte order"
+#endif
+    
+    return SA_SUCCESS;
+}
+
+/* s32 --> */
+
+static int format_s32_to_u8(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    uint8_t *d = dst;
+    const int32_t *s = src;
+    unsigned n = bytes/sizeof(int32_t);
+
+    for (; n > 0; n--, d += dstr, s += sstr/sizeof(int32_t))
+        *d = (uint8_t) (*s / 0x1000000 + 0x80);
+    
+    return SA_SUCCESS;
+}
+
+static int format_s32_to_ulaw(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    uint8_t *d = dst;
+    const int32_t *s = src;
+    unsigned n = bytes/sizeof(int32_t);
+
+    for (; n > 0; n--, d += dstr, s += sstr/sizeof(int32_t))
+        *d = st_14linear2ulaw(*s / 0x10000);
+
+    return SA_SUCCESS;
+}
+
+static int format_s32_to_alaw(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    uint8_t *d = dst;
+    const int32_t *s = src;
+    unsigned n = bytes/sizeof(int32_t);
+
+    for (; n > 0; n--, d += dstr, s += sstr/sizeof(int32_t))
+        *d = st_13linear2alaw(*s / 0x10000);
+
+    return SA_SUCCESS;
+}
+
+static int format_s32_to_s16(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    int16_t *d = dst;
+    const int32_t *s = src;
+    unsigned n = bytes/sizeof(int32_t);
+
+    for (; n > 0; n--, d += dstr/sizeof(int16_t), s += sstr/sizeof(int32_t))
+        *d = (int16_t) (*s / 0x10000);
+    
+    return SA_SUCCESS;
+}
+
+static int format_s32_to_s24(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    uint8_t *d = dst;
+    const int32_t *s = src;
+    unsigned n = bytes/sizeof(float);
+
+    for (; n > 0; n--, d += dstr/3, s += sstr/sizeof(int32_t)) {
+        uint32_t j = (uint32_t) (*s) >> 8;
+        
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+        d[0] = j & 0xFF;
+        d[1] = (j >> 8) & 0xFF;
+        d[2] = (j >> 16);
+#elif __BYTE_ORDER == __BIG_ENDIAN
+        d[2] = j & 0xFF;
+        d[1] = (j >> 8) & 0xFF;
+        d[0] = (j >> 16);
+#else
+#error "Unknown byte order"
+#endif
+    }
+    
+    return SA_SUCCESS;
+}
+
+static int format_s32_to_f32(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    float f = 1.0/0x7fffffff;
+    unsigned n = bytes/sizeof(int32_t);
+    
+    oil_conv_f32_s32(dst, dstr, src, sstr, n);
+    oil_scalarmult_f32(dst, dstr, dst, dstr, &f, n);                       
+
+    return SA_SUCCESS;
+}
+
+/* f32 --> */
+
+static int format_f32_to_u8(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    uint8_t *d = dst;
+    const float *s = src;
+    float *buf;
+    float f = 0x7F, p = 0x80;
+    unsigned n = bytes/sizeof(float);
+
+    if (!(buf = bbuffer_get(b, 0, bytes, 1)))
+        return SA_ERROR_OOM;
+
+    oil_scalarmult_f32(buf, sizeof(float), s, sstr, &f, n);
+    oil_scalaradd_f32(buf, sizeof(float), buf, sizeof(float), &p, n);
+    oil_clipconv_u8_f32(d, dstr, buf, sizeof(float), n);
+
+    return SA_SUCCESS;
+}
+
+static int format_f32_to_ulaw(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    uint8_t *d = dst;
+    const float *s = src;
+    unsigned n = bytes/sizeof(float);
+
+    for (; n > 0; n --, d += dstr, s += sstr/sizeof(float)) {
+        float v = *s * 0x7FFF;
+        if (v < -0x8000) v = -0x8000;
+        if (v > -0x7FFF) v = -0x7FFF;
+        *d = st_13linear2alaw((int16_t) v);
+    }
+
+    return SA_SUCCESS;
+}
+
+static int format_f32_to_alaw(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    uint8_t *d = dst;
+    const float *s = src;
+    unsigned n = bytes/sizeof(float);
+    
+    for (; n > 0; n --, d += dstr, s += sstr/sizeof(float)) {
+        float v = *s * 0x7FFF;
+        if (v < -0x8000) v = -0x8000;
+        if (v > -0x7FFF) v = -0x7FFF;
+        *d = st_13linear2alaw((int16_t) v);
+    }
+
+    return SA_SUCCESS;
+}
+
+static int format_f32_to_s16(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    int16_t *d = dst;
+    const float *s = src;
+    float *buf;
+    float f = 0x7FFF;
+    unsigned n = bytes/sizeof(float);
+
+    if (!(buf = bbuffer_get(b, 0, bytes, 1)))
+        return SA_ERROR_OOM;
+
+    oil_scalarmult_f32(buf, sizeof(float), s, sstr, &f, n);                       
+    oil_clipconv_s16_f32(d, dstr, buf, sizeof(float), n);
+
+    return SA_SUCCESS;
+}
+
+static int format_f32_to_s24(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    uint8_t *d = dst;
+    const float *s = src;
+    unsigned n = bytes/sizeof(float);
+
+    for (; n > 0; n--, d += dstr/3, s += sstr/sizeof(float)) {
+        float f = *s / 0x7fffff;
+        uint32_t j;
+
+        if (f >  0x7fffff) f =  0x7fffff;
+        if (f < -0x800000) f = -0x800000;
+
+        j = (uint32_t) ((int32_t) f);
+        
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+        d[0] = j & 0xFF;
+        d[1] = (j >> 8) & 0xFF;
+        d[2] = (j >> 16);
+#elif __BYTE_ORDER == __BIG_ENDIAN
+        d[2] = j & 0xFF;
+        d[1] = (j >> 8) & 0xFF;
+        d[0] = (j >> 16);
+#else
+#error "Unknown byte order"
+#endif
+    }
+    
+    return SA_SUCCESS;
+}
+
+static int format_f32_to_s32(bbuffer_t *b, void *dst, size_t dstr, const void *src, size_t sstr, size_t bytes) {
+    int32_t *d = dst;
+    const float *s = src;
+    float *buf;
+    float f = 0x7FFFFFFF;
+    unsigned n = bytes/sizeof(float);
+
+    if (!(buf = bbuffer_get(b, 0, bytes, 1)))
+        return SA_ERROR_OOM;
+
+    oil_scalarmult_f32(buf, sizeof(float), s, sstr, &f, n);                       
+    oil_clipconv_s32_f32(d, dstr, buf, sizeof(float), n);
+
+    return SA_SUCCESS;
+}
+
+format_func_t get_format_func(sa_pcm_format_t from, sa_pcm_format_t to) {
+
+    /* Rules: we need conversion functions:
+     *
+     *  1. from all formats to all "better" work formats
+     *  2. from all work formats to all "lower" formats
+     *  3. only for NE types
+     */
+    
+    static const format_func_t funcs[SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_MAX] = {
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_U8         + SA_PCM_FORMAT_S16_NE    ] = format_u8_to_s16,      /* done */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_U8         + SA_PCM_FORMAT_S32_NE    ] = format_u8_to_s32,      /* done */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_U8         + SA_PCM_FORMAT_FLOAT32_NE] = format_u8_to_f32,      /* done */
+
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_ULAW       + SA_PCM_FORMAT_S16_NE    ] = format_ulaw_to_s16,    /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_ULAW       + SA_PCM_FORMAT_S32_NE    ] = format_ulaw_to_s32,    /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_ULAW       + SA_PCM_FORMAT_FLOAT32_NE] = format_ulaw_to_f32,    /* done, no liboil */
+
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_ALAW       + SA_PCM_FORMAT_S16_NE    ] = format_alaw_to_s16,    /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_ALAW       + SA_PCM_FORMAT_S32_NE    ] = format_alaw_to_s32,    /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_ALAW       + SA_PCM_FORMAT_FLOAT32_NE] = format_alaw_to_f32,    /* done, no liboil */
+
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S16_NE     + SA_PCM_FORMAT_U8        ] = format_s16_to_u8,      /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S16_NE     + SA_PCM_FORMAT_ULAW      ] = format_s16_to_ulaw,    /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S16_NE     + SA_PCM_FORMAT_ALAW      ] = format_s16_to_alaw,    /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S16_NE     + SA_PCM_FORMAT_S32_NE    ] = format_s16_to_s32,     /* done */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S16_NE     + SA_PCM_FORMAT_FLOAT32_NE] = format_s16_to_f32,     /* done */
+ 
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S24_NE     + SA_PCM_FORMAT_S32_NE    ] = format_s24_to_s32,     /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S24_NE     + SA_PCM_FORMAT_FLOAT32_NE] = format_s24_to_f32,     /* done, no liboil */
+        
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S32_NE     + SA_PCM_FORMAT_U8        ] = format_s32_to_u8,      /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S32_NE     + SA_PCM_FORMAT_ULAW      ] = format_s32_to_ulaw,    /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S32_NE     + SA_PCM_FORMAT_ALAW      ] = format_s32_to_alaw,    /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S32_NE     + SA_PCM_FORMAT_S16_NE    ] = format_s32_to_s16,     /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S32_NE     + SA_PCM_FORMAT_S24_NE    ] = format_s32_to_s24,     /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_S32_NE     + SA_PCM_FORMAT_FLOAT32_NE] = format_s32_to_f32,     /* done, but suboptimal */
+        
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_FLOAT32_NE + SA_PCM_FORMAT_U8        ] = format_f32_to_u8,      /* done */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_FLOAT32_NE + SA_PCM_FORMAT_ULAW      ] = format_f32_to_ulaw,    /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_FLOAT32_NE + SA_PCM_FORMAT_ALAW      ] = format_f32_to_alaw,    /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_FLOAT32_NE + SA_PCM_FORMAT_S16_NE    ] = format_f32_to_s16,     /* done */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_FLOAT32_NE + SA_PCM_FORMAT_S24_NE    ] = format_f32_to_s24,     /* done, no liboil */
+        [SA_PCM_FORMAT_MAX * SA_PCM_FORMAT_FLOAT32_NE + SA_PCM_FORMAT_S32_NE    ] = format_f32_to_s32,     /* done, but suboptimal */
+    };
+
+    sa_assert(from < SA_PCM_FORMAT_MAX);
+    sa_assert(to < SA_PCM_FORMAT_MAX);
+    
+    return funcs[from * SA_PCM_FORMAT_MAX + to];
+}