1/*
2 * Copyright (C) 2011 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 *      http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17#include <cutils/bitops.h>  /* for popcount() */
18#include <audio_utils/primitives.h>
19#include "private/private.h"
20
21void ditherAndClamp(int32_t* out, const int32_t *sums, size_t c)
22{
23    size_t i;
24    for (i=0 ; i<c ; i++) {
25        int32_t l = *sums++;
26        int32_t r = *sums++;
27        int32_t nl = l >> 12;
28        int32_t nr = r >> 12;
29        l = clamp16(nl);
30        r = clamp16(nr);
31        *out++ = (r<<16) | (l & 0xFFFF);
32    }
33}
34
35void memcpy_to_i16_from_u8(int16_t *dst, const uint8_t *src, size_t count)
36{
37    dst += count;
38    src += count;
39    while (count--) {
40        *--dst = (int16_t)(*--src - 0x80) << 8;
41    }
42}
43
44void memcpy_to_u8_from_i16(uint8_t *dst, const int16_t *src, size_t count)
45{
46    while (count--) {
47        *dst++ = (*src++ >> 8) + 0x80;
48    }
49}
50
51void memcpy_to_u8_from_float(uint8_t *dst, const float *src, size_t count)
52{
53    while (count--) {
54        *dst++ = clamp8_from_float(*src++);
55    }
56}
57
58void memcpy_to_i16_from_i32(int16_t *dst, const int32_t *src, size_t count)
59{
60    while (count--) {
61        *dst++ = *src++ >> 16;
62    }
63}
64
65void memcpy_to_i16_from_float(int16_t *dst, const float *src, size_t count)
66{
67    while (count--) {
68        *dst++ = clamp16_from_float(*src++);
69    }
70}
71
72void memcpy_to_float_from_q4_27(float *dst, const int32_t *src, size_t count)
73{
74    while (count--) {
75        *dst++ = float_from_q4_27(*src++);
76    }
77}
78
79void memcpy_to_float_from_i16(float *dst, const int16_t *src, size_t count)
80{
81    while (count--) {
82        *dst++ = float_from_i16(*src++);
83    }
84}
85
86void memcpy_to_float_from_u8(float *dst, const uint8_t *src, size_t count)
87{
88    while (count--) {
89        *dst++ = float_from_u8(*src++);
90    }
91}
92
93void memcpy_to_float_from_p24(float *dst, const uint8_t *src, size_t count)
94{
95    while (count--) {
96        *dst++ = float_from_p24(src);
97        src += 3;
98    }
99}
100
101void memcpy_to_i16_from_p24(int16_t *dst, const uint8_t *src, size_t count)
102{
103    while (count--) {
104#if HAVE_BIG_ENDIAN
105        *dst++ = src[1] | (src[0] << 8);
106#else
107        *dst++ = src[1] | (src[2] << 8);
108#endif
109        src += 3;
110    }
111}
112
113void memcpy_to_i32_from_p24(int32_t *dst, const uint8_t *src, size_t count)
114{
115    while (count--) {
116#if HAVE_BIG_ENDIAN
117        *dst++ = (src[2] << 8) | (src[1] << 16) | (src[0] << 24);
118#else
119        *dst++ = (src[0] << 8) | (src[1] << 16) | (src[2] << 24);
120#endif
121        src += 3;
122    }
123}
124
125void memcpy_to_p24_from_i16(uint8_t *dst, const int16_t *src, size_t count)
126{
127    while (count--) {
128#if HAVE_BIG_ENDIAN
129        *dst++ = *src >> 8;
130        *dst++ = *src++;
131        *dst++ = 0;
132#else
133        *dst++ = 0;
134        *dst++ = *src;
135        *dst++ = *src++ >> 8;
136#endif
137    }
138}
139
140void memcpy_to_p24_from_float(uint8_t *dst, const float *src, size_t count)
141{
142    while (count--) {
143        int32_t ival = clamp24_from_float(*src++);
144
145#if HAVE_BIG_ENDIAN
146        *dst++ = ival >> 16;
147        *dst++ = ival >> 8;
148        *dst++ = ival;
149#else
150        *dst++ = ival;
151        *dst++ = ival >> 8;
152        *dst++ = ival >> 16;
153#endif
154    }
155}
156
157void memcpy_to_p24_from_q8_23(uint8_t *dst, const int32_t *src, size_t count)
158{
159    while (count--) {
160        int32_t ival = clamp24_from_q8_23(*src++);
161
162#if HAVE_BIG_ENDIAN
163        *dst++ = ival >> 16;
164        *dst++ = ival >> 8;
165        *dst++ = ival;
166#else
167        *dst++ = ival;
168        *dst++ = ival >> 8;
169        *dst++ = ival >> 16;
170#endif
171    }
172}
173
174void memcpy_to_p24_from_i32(uint8_t *dst, const int32_t *src, size_t count)
175{
176    while (count--) {
177        int32_t ival = *src++ >> 8;
178
179#if HAVE_BIG_ENDIAN
180        *dst++ = ival >> 16;
181        *dst++ = ival >> 8;
182        *dst++ = ival;
183#else
184        *dst++ = ival;
185        *dst++ = ival >> 8;
186        *dst++ = ival >> 16;
187#endif
188    }
189}
190
191void memcpy_to_q8_23_from_i16(int32_t *dst, const int16_t *src, size_t count)
192{
193    while (count--) {
194        *dst++ = (int32_t)*src++ << 8;
195    }
196}
197
198void memcpy_to_q8_23_from_float_with_clamp(int32_t *dst, const float *src, size_t count)
199{
200    while (count--) {
201        *dst++ = clamp24_from_float(*src++);
202    }
203}
204
205void memcpy_to_q8_23_from_p24(int32_t *dst, const uint8_t *src, size_t count)
206{
207    while (count--) {
208#if HAVE_BIG_ENDIAN
209        *dst++ = (int8_t)src[0] << 16 | src[1] << 8 | src[2];
210#else
211        *dst++ = (int8_t)src[2] << 16 | src[1] << 8 | src[0];
212#endif
213        src += 3;
214    }
215}
216
217void memcpy_to_q4_27_from_float(int32_t *dst, const float *src, size_t count)
218{
219    while (count--) {
220        *dst++ = clampq4_27_from_float(*src++);
221    }
222}
223
224void memcpy_to_i16_from_q8_23(int16_t *dst, const int32_t *src, size_t count)
225{
226    while (count--) {
227        *dst++ = clamp16(*src++ >> 8);
228    }
229}
230
231void memcpy_to_float_from_q8_23(float *dst, const int32_t *src, size_t count)
232{
233    while (count--) {
234        *dst++ = float_from_q8_23(*src++);
235    }
236}
237
238void memcpy_to_i32_from_i16(int32_t *dst, const int16_t *src, size_t count)
239{
240    while (count--) {
241        *dst++ = (int32_t)*src++ << 16;
242    }
243}
244
245void memcpy_to_i32_from_float(int32_t *dst, const float *src, size_t count)
246{
247    while (count--) {
248        *dst++ = clamp32_from_float(*src++);
249    }
250}
251
252void memcpy_to_float_from_i32(float *dst, const int32_t *src, size_t count)
253{
254    while (count--) {
255        *dst++ = float_from_i32(*src++);
256    }
257}
258
259void downmix_to_mono_i16_from_stereo_i16(int16_t *dst, const int16_t *src, size_t count)
260{
261    while (count--) {
262        *dst++ = (int16_t)(((int32_t)src[0] + (int32_t)src[1]) >> 1);
263        src += 2;
264    }
265}
266
267void upmix_to_stereo_i16_from_mono_i16(int16_t *dst, const int16_t *src, size_t count)
268{
269    while (count--) {
270        int32_t temp = *src++;
271        dst[0] = temp;
272        dst[1] = temp;
273        dst += 2;
274    }
275}
276
277void downmix_to_mono_float_from_stereo_float(float *dst, const float *src, size_t frames)
278{
279    while (frames--) {
280        *dst++ = (src[0] + src[1]) * 0.5;
281        src += 2;
282    }
283}
284
285void upmix_to_stereo_float_from_mono_float(float *dst, const float *src, size_t frames)
286{
287    while (frames--) {
288        float temp = *src++;
289        dst[0] = temp;
290        dst[1] = temp;
291        dst += 2;
292    }
293}
294
295size_t nonZeroMono32(const int32_t *samples, size_t count)
296{
297    size_t nonZero = 0;
298    while (count-- > 0) {
299        if (*samples++ != 0) {
300            nonZero++;
301        }
302    }
303    return nonZero;
304}
305
306size_t nonZeroMono16(const int16_t *samples, size_t count)
307{
308    size_t nonZero = 0;
309    while (count-- > 0) {
310        if (*samples++ != 0) {
311            nonZero++;
312        }
313    }
314    return nonZero;
315}
316
317size_t nonZeroStereo32(const int32_t *frames, size_t count)
318{
319    size_t nonZero = 0;
320    while (count-- > 0) {
321        if (frames[0] != 0 || frames[1] != 0) {
322            nonZero++;
323        }
324        frames += 2;
325    }
326    return nonZero;
327}
328
329size_t nonZeroStereo16(const int16_t *frames, size_t count)
330{
331    size_t nonZero = 0;
332    while (count-- > 0) {
333        if (frames[0] != 0 || frames[1] != 0) {
334            nonZero++;
335        }
336        frames += 2;
337    }
338    return nonZero;
339}
340
341/*
342 * C macro to do channel mask copying independent of dst/src sample type.
343 * Don't pass in any expressions for the macro arguments here.
344 */
345#define copy_frame_by_mask(dst, dmask, src, smask, count, zero) \
346{ \
347    uint32_t bit, ormask; \
348    while ((count)--) { \
349        ormask = (dmask) | (smask); \
350        while (ormask) { \
351            bit = ormask & -ormask; /* get lowest bit */ \
352            ormask ^= bit; /* remove lowest bit */ \
353            if ((dmask) & bit) { \
354                *(dst)++ = (smask) & bit ? *(src)++ : (zero); \
355            } else { /* source channel only */ \
356                ++(src); \
357            } \
358        } \
359    } \
360}
361
362void memcpy_by_channel_mask(void *dst, uint32_t dst_mask,
363        const void *src, uint32_t src_mask, size_t sample_size, size_t count)
364{
365#if 0
366    /* alternate way of handling memcpy_by_channel_mask by using the idxary */
367    int8_t idxary[32];
368    uint32_t src_channels = popcount(src_mask);
369    uint32_t dst_channels =
370            memcpy_by_index_array_initialization(idxary, 32, dst_mask, src_mask);
371
372    memcpy_by_idxary(dst, dst_channels, src, src_channels, idxary, sample_size, count);
373#else
374    if (dst_mask == src_mask) {
375        memcpy(dst, src, sample_size * popcount(dst_mask) * count);
376        return;
377    }
378    switch (sample_size) {
379    case 1: {
380        uint8_t *udst = (uint8_t*)dst;
381        const uint8_t *usrc = (const uint8_t*)src;
382
383        copy_frame_by_mask(udst, dst_mask, usrc, src_mask, count, 0);
384    } break;
385    case 2: {
386        uint16_t *udst = (uint16_t*)dst;
387        const uint16_t *usrc = (const uint16_t*)src;
388
389        copy_frame_by_mask(udst, dst_mask, usrc, src_mask, count, 0);
390    } break;
391    case 3: { /* could be slow.  use a struct to represent 3 bytes of data. */
392        uint8x3_t *udst = (uint8x3_t*)dst;
393        const uint8x3_t *usrc = (const uint8x3_t*)src;
394        static const uint8x3_t zero; /* tricky - we use this to zero out a sample */
395
396        copy_frame_by_mask(udst, dst_mask, usrc, src_mask, count, zero);
397    } break;
398    case 4: {
399        uint32_t *udst = (uint32_t*)dst;
400        const uint32_t *usrc = (const uint32_t*)src;
401
402        copy_frame_by_mask(udst, dst_mask, usrc, src_mask, count, 0);
403    } break;
404    default:
405        abort(); /* illegal value */
406        break;
407    }
408#endif
409}
410
411/*
412 * C macro to do copying by index array, to rearrange samples
413 * within a frame.  This is independent of src/dst sample type.
414 * Don't pass in any expressions for the macro arguments here.
415 */
416#define copy_frame_by_idx(dst, dst_channels, src, src_channels, idxary, count, zero) \
417{ \
418    unsigned i; \
419    int index; \
420    while ((count)--) { \
421        for (i = 0; i < (dst_channels); ++i) { \
422            index = (idxary)[i]; \
423            *(dst)++ = index < 0 ? (zero) : (src)[index]; \
424        } \
425        (src) += (src_channels); \
426    } \
427}
428
429void memcpy_by_index_array(void *dst, uint32_t dst_channels,
430        const void *src, uint32_t src_channels,
431        const int8_t *idxary, size_t sample_size, size_t count)
432{
433    switch (sample_size) {
434    case 1: {
435        uint8_t *udst = (uint8_t*)dst;
436        const uint8_t *usrc = (const uint8_t*)src;
437
438        copy_frame_by_idx(udst, dst_channels, usrc, src_channels, idxary, count, 0);
439    } break;
440    case 2: {
441        uint16_t *udst = (uint16_t*)dst;
442        const uint16_t *usrc = (const uint16_t*)src;
443
444        copy_frame_by_idx(udst, dst_channels, usrc, src_channels, idxary, count, 0);
445    } break;
446    case 3: { /* could be slow.  use a struct to represent 3 bytes of data. */
447        uint8x3_t *udst = (uint8x3_t*)dst;
448        const uint8x3_t *usrc = (const uint8x3_t*)src;
449        static const uint8x3_t zero;
450
451        copy_frame_by_idx(udst, dst_channels, usrc, src_channels, idxary, count, zero);
452    } break;
453    case 4: {
454        uint32_t *udst = (uint32_t*)dst;
455        const uint32_t *usrc = (const uint32_t*)src;
456
457        copy_frame_by_idx(udst, dst_channels, usrc, src_channels, idxary, count, 0);
458    } break;
459    default:
460        abort(); /* illegal value */
461        break;
462    }
463}
464
465size_t memcpy_by_index_array_initialization(int8_t *idxary, size_t idxcount,
466        uint32_t dst_mask, uint32_t src_mask)
467{
468    size_t n = 0;
469    int srcidx = 0;
470    uint32_t bit, ormask = src_mask | dst_mask;
471
472    while (ormask && n < idxcount) {
473        bit = ormask & -ormask;          /* get lowest bit */
474        ormask ^= bit;                   /* remove lowest bit */
475        if (src_mask & dst_mask & bit) { /* matching channel */
476            idxary[n++] = srcidx++;
477        } else if (src_mask & bit) {     /* source channel only */
478            ++srcidx;
479        } else {                         /* destination channel only */
480            idxary[n++] = -1;
481        }
482    }
483    return n + popcount(ormask & dst_mask);
484}
485
486size_t memcpy_by_index_array_initialization_src_index(int8_t *idxary, size_t idxcount,
487        uint32_t dst_mask, uint32_t src_mask) {
488    size_t dst_count = popcount(dst_mask);
489    if (idxcount == 0) {
490        return dst_count;
491    }
492    if (dst_count > idxcount) {
493        dst_count = idxcount;
494    }
495
496    size_t src_idx, dst_idx;
497    for (src_idx = 0, dst_idx = 0; dst_idx < dst_count; ++dst_idx) {
498        if (src_mask & 1) {
499            idxary[dst_idx] = src_idx++;
500        } else {
501            idxary[dst_idx] = -1;
502        }
503        src_mask >>= 1;
504    }
505    return dst_idx;
506}
507
508size_t memcpy_by_index_array_initialization_dst_index(int8_t *idxary, size_t idxcount,
509        uint32_t dst_mask, uint32_t src_mask) {
510    size_t src_idx, dst_idx;
511    size_t dst_count = __builtin_popcount(dst_mask);
512    size_t src_count = __builtin_popcount(src_mask);
513    if (idxcount == 0) {
514        return dst_count;
515    }
516    if (dst_count > idxcount) {
517        dst_count = idxcount;
518    }
519    for (src_idx = 0, dst_idx = 0; dst_idx < dst_count; ++src_idx) {
520        if (dst_mask & 1) {
521            idxary[dst_idx++] = src_idx < src_count ? (signed)src_idx : -1;
522        }
523        dst_mask >>= 1;
524    }
525    return dst_idx;
526}
527