audio_utils/primitives.c

   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
  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
  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
  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
  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
  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
  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
  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
  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
  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
  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
 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
 113 void memcpy_to_p24_from_i16(uint8_t *dst, const int16_t *src, size_t count)
 114 {
 115     while (count--) {
 116 #ifdef HAVE_BIG_ENDIAN
 117         *dst++ = *src >> 8;
 118         *dst++ = *src++;
 119         *dst++ = 0;
 120 #else
 121         *dst++ = 0;
 122         *dst++ = *src;
 123         *dst++ = *src++ >> 8;
 124 #endif
 125     }
 126 }
 127
 128 void memcpy_to_p24_from_float(uint8_t *dst, const float *src, size_t count)
 129 {
 130     while (count--) {
 131         int32_t ival = clamp24_from_float(*src++);
 132
 133 #ifdef HAVE_BIG_ENDIAN
 134         *dst++ = ival >> 16;
 135         *dst++ = ival >> 8;
 136         *dst++ = ival;
 137 #else
 138         *dst++ = ival;
 139         *dst++ = ival >> 8;
 140         *dst++ = ival >> 16;
 141 #endif
 142     }
 143 }
 144
 145 void memcpy_to_p24_from_q8_23(uint8_t *dst, const int32_t *src, size_t count)
 146 {
 147     while (count--) {
 148         int32_t ival = clamp24_from_q8_23(*src++);
 149
 150 #ifdef HAVE_BIG_ENDIAN
 151         *dst++ = ival >> 16;
 152         *dst++ = ival >> 8;
 153         *dst++ = ival;
 154 #else
 155         *dst++ = ival;
 156         *dst++ = ival >> 8;
 157         *dst++ = ival >> 16;
 158 #endif
 159     }
 160 }
 161
 162 void memcpy_to_q8_23_from_i16(int32_t *dst, const int16_t *src, size_t count)
 163 {
 164     while (count--) {
 165         *dst++ = (int32_t)*src++ << 8;
 166     }
 167 }
 168
 169 void memcpy_to_q8_23_from_float_with_clamp(int32_t *dst, const float *src, size_t count)
 170 {
 171     while (count--) {
 172         *dst++ = clamp24_from_float(*src++);
 173     }
 174 }
 175
 176 void memcpy_to_q4_27_from_float(int32_t *dst, const float *src, size_t count)
 177 {
 178     while (count--) {
 179         *dst++ = clampq4_27_from_float(*src++);
 180     }
 181 }
 182
 183 void memcpy_to_i16_from_q8_23(int16_t *dst, const int32_t *src, size_t count)
 184 {
 185     while (count--) {
 186         *dst++ = clamp16(*src++ >> 8);
 187     }
 188 }
 189
 190 void memcpy_to_float_from_q8_23(float *dst, const int32_t *src, size_t count)
 191 {
 192     while (count--) {
 193         *dst++ = float_from_q8_23(*src++);
 194     }
 195 }
 196
 197 void memcpy_to_i32_from_i16(int32_t *dst, const int16_t *src, size_t count)
 198 {
 199     while (count--) {
 200         *dst++ = (int32_t)*src++ << 16;
 201     }
 202 }
 203
 204 void memcpy_to_i32_from_float(int32_t *dst, const float *src, size_t count)
 205 {
 206     while (count--) {
 207         *dst++ = clamp32_from_float(*src++);
 208     }
 209 }
 210
 211 void memcpy_to_float_from_i32(float *dst, const int32_t *src, size_t count)
 212 {
 213     while (count--) {
 214         *dst++ = float_from_i32(*src++);
 215     }
 216 }
 217
 218 void downmix_to_mono_i16_from_stereo_i16(int16_t *dst, const int16_t *src, size_t count)
 219 {
 220     while (count--) {
 221         *dst++ = (int16_t)(((int32_t)src[0] + (int32_t)src[1]) >> 1);
 222         src += 2;
 223     }
 224 }
 225
 226 void upmix_to_stereo_i16_from_mono_i16(int16_t *dst, const int16_t *src, size_t count)
 227 {
 228     while (count--) {
 229         int32_t temp = *src++;
 230         dst[0] = temp;
 231         dst[1] = temp;
 232         dst += 2;
 233     }
 234 }
 235
 236 size_t nonZeroMono32(const int32_t *samples, size_t count)
 237 {
 238     size_t nonZero = 0;
 239     while (count-- > 0) {
 240         if (*samples++ != 0) {
 241             nonZero++;
 242         }
 243     }
 244     return nonZero;
 245 }
 246
 247 size_t nonZeroMono16(const int16_t *samples, size_t count)
 248 {
 249     size_t nonZero = 0;
 250     while (count-- > 0) {
 251         if (*samples++ != 0) {
 252             nonZero++;
 253         }
 254     }
 255     return nonZero;
 256 }
 257
 258 size_t nonZeroStereo32(const int32_t *frames, size_t count)
 259 {
 260     size_t nonZero = 0;
 261     while (count-- > 0) {
 262         if (frames[0] != 0 || frames[1] != 0) {
 263             nonZero++;
 264         }
 265         frames += 2;
 266     }
 267     return nonZero;
 268 }
 269
 270 size_t nonZeroStereo16(const int16_t *frames, size_t count)
 271 {
 272     size_t nonZero = 0;
 273     while (count-- > 0) {
 274         if (frames[0] != 0 || frames[1] != 0) {
 275             nonZero++;
 276         }
 277         frames += 2;
 278     }
 279     return nonZero;
 280 }
 281
 282 /*
 283  * C macro to do channel mask copying independent of dst/src sample type.
 284  * Don't pass in any expressions for the macro arguments here.
 285  */
 286 #define copy_frame_by_mask(dst, dmask, src, smask, count, zero) \
 287 { \
 288     uint32_t bit, ormask; \
 289     while (count--) { \
 290         ormask = dmask | smask; \
 291         while (ormask) { \
 292             bit = ormask & -ormask; /* get lowest bit */ \
 293             ormask ^= bit; /* remove lowest bit */ \
 294             if (dmask & bit) { \
 295                 *dst++ = smask & bit ? *src++ : zero; \
 296             } else { /* source channel only */ \
 297                 ++src; \
 298             } \
 299         } \
 300     } \
 301 }
 302
 303 void memcpy_by_channel_mask(void *dst, uint32_t dst_mask,
 304         const void *src, uint32_t src_mask, size_t sample_size, size_t count)
 305 {
 306 #if 0
 307     /* alternate way of handling memcpy_by_channel_mask by using the idxary */
 308     int8_t idxary[32];
 309     uint32_t src_channels = popcount(src_mask);
 310     uint32_t dst_channels =
 311             memcpy_by_index_array_initialization(idxary, 32, dst_mask, src_mask);
 312
 313     memcpy_by_idxary(dst, dst_channels, src, src_channels, idxary, sample_size, count);
 314 #else
 315     if (dst_mask == src_mask) {
 316         memcpy(dst, src, sample_size * popcount(dst_mask) * count);
 317         return;
 318     }
 319     switch (sample_size) {
 320     case 1: {
 321         uint8_t *udst = (uint8_t*)dst;
 322         const uint8_t *usrc = (const uint8_t*)src;
 323
 324         copy_frame_by_mask(udst, dst_mask, usrc, src_mask, count, 0);
 325     } break;
 326     case 2: {
 327         uint16_t *udst = (uint16_t*)dst;
 328         const uint16_t *usrc = (const uint16_t*)src;
 329
 330         copy_frame_by_mask(udst, dst_mask, usrc, src_mask, count, 0);
 331     } break;
 332     case 3: { /* could be slow.  use a struct to represent 3 bytes of data. */
 333         uint8x3_t *udst = (uint8x3_t*)dst;
 334         const uint8x3_t *usrc = (const uint8x3_t*)src;
 335         static const uint8x3_t zero; /* tricky - we use this to zero out a sample */
 336
 337         copy_frame_by_mask(udst, dst_mask, usrc, src_mask, count, zero);
 338     } break;
 339     case 4: {
 340         uint32_t *udst = (uint32_t*)dst;
 341         const uint32_t *usrc = (const uint32_t*)src;
 342
 343         copy_frame_by_mask(udst, dst_mask, usrc, src_mask, count, 0);
 344     } break;
 345     default:
 346         abort(); /* illegal value */
 347         break;
 348     }
 349 #endif
 350 }
 351
 352 /*
 353  * C macro to do copying by index array, to rearrange samples
 354  * within a frame.  This is independent of src/dst sample type.
 355  * Don't pass in any expressions for the macro arguments here.
 356  */
 357 #define copy_frame_by_idx(dst, dst_channels, src, src_channels, idxary, count, zero) \
 358 { \
 359     unsigned i; \
 360     int index; \
 361     while (count--) { \
 362         for (i = 0; i < dst_channels; ++i) { \
 363             index = idxary[i]; \
 364             *dst++ = index < 0 ? zero : src[index]; \
 365         } \
 366         src += src_channels; \
 367     } \
 368 }
 369
 370 void memcpy_by_index_array(void *dst, uint32_t dst_channels,
 371         const void *src, uint32_t src_channels,
 372         const int8_t *idxary, size_t sample_size, size_t count)
 373 {
 374     switch (sample_size) {
 375     case 1: {
 376         uint8_t *udst = (uint8_t*)dst;
 377         const uint8_t *usrc = (const uint8_t*)src;
 378
 379         copy_frame_by_idx(udst, dst_channels, usrc, src_channels, idxary, count, 0);
 380     } break;
 381     case 2: {
 382         uint16_t *udst = (uint16_t*)dst;
 383         const uint16_t *usrc = (const uint16_t*)src;
 384
 385         copy_frame_by_idx(udst, dst_channels, usrc, src_channels, idxary, count, 0);
 386     } break;
 387     case 3: { /* could be slow.  use a struct to represent 3 bytes of data. */
 388         uint8x3_t *udst = (uint8x3_t*)dst;
 389         const uint8x3_t *usrc = (const uint8x3_t*)src;
 390         static const uint8x3_t zero;
 391
 392         copy_frame_by_idx(udst, dst_channels, usrc, src_channels, idxary, count, zero);
 393     } break;
 394     case 4: {
 395         uint32_t *udst = (uint32_t*)dst;
 396         const uint32_t *usrc = (const uint32_t*)src;
 397
 398         copy_frame_by_idx(udst, dst_channels, usrc, src_channels, idxary, count, 0);
 399     } break;
 400     default:
 401         abort(); /* illegal value */
 402         break;
 403     }
 404 }
 405
 406 size_t memcpy_by_index_array_initialization(int8_t *idxary, size_t idxcount,
 407         uint32_t dst_mask, uint32_t src_mask)
 408 {
 409     size_t n = 0;
 410     int srcidx = 0;
 411     uint32_t bit, ormask = src_mask | dst_mask;
 412
 413     while (ormask && n < idxcount) {
 414         bit = ormask & -ormask;          /* get lowest bit */
 415         ormask ^= bit;                   /* remove lowest bit */
 416         if (src_mask & dst_mask & bit) { /* matching channel */
 417             idxary[n++] = srcidx++;
 418         } else if (src_mask & bit) {     /* source channel only */
 419             ++srcidx;
 420         } else {                         /* destination channel only */
 421             idxary[n++] = -1;
 422         }
 423     }
 424     return n + popcount(ormask & dst_mask);
 425 }