diff options
Diffstat (limited to 'add.c')
-rw-r--r-- | add.c | 55 |
1 files changed, 55 insertions, 0 deletions
@@ -0,0 +1,55 @@ +#include <liboil/liboil.h> + +#include "macro.h" +#include "add.h" + +static void add_u8(void *dst, size_t dstr, const void *src1, size_t sstr1, const void *src2, size_t sstr2, size_t bytes) { + uint8_t *d = dst; + const uint8_t *s1 = src1, *s2 = src2; + + for (; bytes > 0; bytes--, d += dstr, s1 += sstr1, s2 += sstr2) { + int16_t v = (int16_t) *s1 + (int16_t) *s2 - 0x80; + + if (v > 0xFF) v = 0xFF; + if (v < 0) v = 0; + + *d = (uint8_t) v; + } +} + +static void add_s16(void *dst, size_t dstr, const void *src1, size_t sstr1, const void *src2, size_t sstr2, size_t bytes) { + oil_vectoradd_s_s16(dst, dstr, src1, sstr1, src2, sstr2, bytes/sizeof(int16_t)); +} + +static void add_s32(void *dst, size_t dstr, const void *src1, size_t sstr1, const void *src2, size_t sstr2, size_t bytes) { + int32_t *d = dst; + const int32_t *s1 = src1, *s2 = src2; + + for (; bytes > 0; bytes--, d += dstr/sizeof(int32_t), s1 += sstr1/sizeof(int32_t), s2 += sstr2/sizeof(int32_t)) { + int64_t v = (int64_t) *s1 + (uint64_t) *s2; + + if (v > 0x7FFFFFFF) v = 0x7FFFFFFF; + if (v < -0x80000000) v = -0x80000000; + + *d = (int32_t) v; + } +} + +static void add_f32(void *dst, size_t dstr, const void *src1, size_t sstr1, const void *src2, size_t sstr2, size_t bytes) { + float a = 1, b = 1; + oil_vectoradd_f32(dst, dstr, src1, sstr1, src2, sstr2, bytes/sizeof(int32_t), &a, &b); +} + +add_func_t get_add_func(sa_pcm_format_t f) { + + static const add_func_t funcs[SA_PCM_FORMAT_MAX] = { + [SA_PCM_FORMAT_U8] = add_u8, + [SA_PCM_FORMAT_S16_NE] = add_s16, + [SA_PCM_FORMAT_S32_NE] = add_s32, + [SA_PCM_FORMAT_FLOAT32_NE] = add_f32 + }; + + sa_assert(f < SA_PCM_FORMAT_MAX); + + return funcs[f]; +} |