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 #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
 113 void 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
 125 void 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
 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 #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
 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 #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
 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 #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
 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
 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
 205 void 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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 }