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 
ditherAndClamp(int32_t * out,const int32_t * sums,size_t c)21 void 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 
memcpy_to_i16_from_u8(int16_t * dst,const uint8_t * src,size_t count)35 void 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 
memcpy_to_u8_from_i16(uint8_t * dst,const int16_t * src,size_t count)44 void 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 
memcpy_to_u8_from_float(uint8_t * dst,const float * src,size_t count)51 void 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 
memcpy_to_i16_from_i32(int16_t * dst,const int32_t * src,size_t count)58 void 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 
memcpy_to_i16_from_float(int16_t * dst,const float * src,size_t count)65 void 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 
memcpy_to_float_from_q4_27(float * dst,const int32_t * src,size_t count)72 void 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 
memcpy_to_float_from_i16(float * dst,const int16_t * src,size_t count)79 void 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 
memcpy_to_float_from_u8(float * dst,const uint8_t * src,size_t count)86 void 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 
memcpy_to_float_from_p24(float * dst,const uint8_t * src,size_t count)93 void 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 
memcpy_to_i16_from_p24(int16_t * dst,const uint8_t * src,size_t count)101 void memcpy_to_i16_from_p24(int16_t *dst, const uint8_t *src, size_t count)
102 {
103     while (count--) {
104 #ifdef 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 
memcpy_to_i32_from_p24(int32_t * dst,const uint8_t * src,size_t count)113 void memcpy_to_i32_from_p24(int32_t *dst, const uint8_t *src, size_t count)
114 {
115     while (count--) {
116 #ifdef 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 
memcpy_to_p24_from_i16(uint8_t * dst,const int16_t * src,size_t count)125 void memcpy_to_p24_from_i16(uint8_t *dst, const int16_t *src, size_t count)
126 {
127     while (count--) {
128 #ifdef 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 
memcpy_to_p24_from_float(uint8_t * dst,const float * src,size_t count)140 void 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 #ifdef 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 
memcpy_to_p24_from_q8_23(uint8_t * dst,const int32_t * src,size_t count)157 void 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 #ifdef 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 
memcpy_to_p24_from_i32(uint8_t * dst,const int32_t * src,size_t count)174 void 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 #ifdef 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 
memcpy_to_q8_23_from_i16(int32_t * dst,const int16_t * src,size_t count)191 void 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 
memcpy_to_q8_23_from_float_with_clamp(int32_t * dst,const float * src,size_t count)198 void 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 
memcpy_to_q8_23_from_p24(int32_t * dst,const uint8_t * src,size_t count)205 void memcpy_to_q8_23_from_p24(int32_t *dst, const uint8_t *src, size_t count)
206 {
207     while (count--) {
208 #ifdef 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 
memcpy_to_q4_27_from_float(int32_t * dst,const float * src,size_t count)217 void 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 
memcpy_to_i16_from_q8_23(int16_t * dst,const int32_t * src,size_t count)224 void 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 
memcpy_to_float_from_q8_23(float * dst,const int32_t * src,size_t count)231 void 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 
memcpy_to_i32_from_i16(int32_t * dst,const int16_t * src,size_t count)238 void 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 
memcpy_to_i32_from_float(int32_t * dst,const float * src,size_t count)245 void 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 
memcpy_to_float_from_i32(float * dst,const int32_t * src,size_t count)252 void 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 
downmix_to_mono_i16_from_stereo_i16(int16_t * dst,const int16_t * src,size_t count)259 void 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 
upmix_to_stereo_i16_from_mono_i16(int16_t * dst,const int16_t * src,size_t count)267 void 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 
downmix_to_mono_float_from_stereo_float(float * dst,const float * src,size_t frames)277 void 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 
upmix_to_stereo_float_from_mono_float(float * dst,const float * src,size_t frames)285 void 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 
nonZeroMono32(const int32_t * samples,size_t count)295 size_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 
nonZeroMono16(const int16_t * samples,size_t count)306 size_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 
nonZeroStereo32(const int32_t * frames,size_t count)317 size_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 
nonZeroStereo16(const int16_t * frames,size_t count)329 size_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 
memcpy_by_channel_mask(void * dst,uint32_t dst_mask,const void * src,uint32_t src_mask,size_t sample_size,size_t count)362 void 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 
memcpy_by_index_array(void * dst,uint32_t dst_channels,const void * src,uint32_t src_channels,const int8_t * idxary,size_t sample_size,size_t count)429 void 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 
memcpy_by_index_array_initialization(int8_t * idxary,size_t idxcount,uint32_t dst_mask,uint32_t src_mask)465 size_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 
memcpy_by_index_array_initialization_src_index(int8_t * idxary,size_t idxcount,uint32_t dst_mask,uint32_t src_mask)486 size_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 
memcpy_by_index_array_initialization_dst_index(int8_t * idxary,size_t idxcount,uint32_t dst_mask,uint32_t src_mask)508 size_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