1632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten/* 2632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten * Copyright (C) 2011 The Android Open Source Project 3632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten * 4632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten * Licensed under the Apache License, Version 2.0 (the "License"); 5632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten * you may not use this file except in compliance with the License. 6632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten * You may obtain a copy of the License at 7632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten * 8632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten * http://www.apache.org/licenses/LICENSE-2.0 9632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten * 10632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten * Unless required by applicable law or agreed to in writing, software 11632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten * distributed under the License is distributed on an "AS IS" BASIS, 12632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten * See the License for the specific language governing permissions and 14632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten * limitations under the License. 15632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten */ 16632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten 17e0ccb20d0b3ab5c686508a7b1413928320ec2d1fAndy Hung#include <cutils/bitops.h> /* for popcount() */ 18632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten#include <audio_utils/primitives.h> 19e0ccb20d0b3ab5c686508a7b1413928320ec2d1fAndy Hung#include "private/private.h" 20632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten 217ef795a9d1ac0f3f464e0e7a54aeca00bf94215bGlenn Kastenvoid ditherAndClamp(int32_t* out, const int32_t *sums, size_t c) 22632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten{ 23632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten size_t i; 24632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten for (i=0 ; i<c ; i++) { 25632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten int32_t l = *sums++; 26632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten int32_t r = *sums++; 27632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten int32_t nl = l >> 12; 28632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten int32_t nr = r >> 12; 29632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten l = clamp16(nl); 30632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten r = clamp16(nr); 31632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten *out++ = (r<<16) | (l & 0xFFFF); 32632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten } 33632e0c016c9a518b36f09988b740b3bc1199c3e4Glenn Kasten} 34ddb2e930bef3d1be1627c613fb8f3df848166ac7Glenn Kasten 35ddb2e930bef3d1be1627c613fb8f3df848166ac7Glenn Kastenvoid memcpy_to_i16_from_u8(int16_t *dst, const uint8_t *src, size_t count) 36ddb2e930bef3d1be1627c613fb8f3df848166ac7Glenn Kasten{ 37ddb2e930bef3d1be1627c613fb8f3df848166ac7Glenn Kasten dst += count; 38ddb2e930bef3d1be1627c613fb8f3df848166ac7Glenn Kasten src += count; 39ddb2e930bef3d1be1627c613fb8f3df848166ac7Glenn Kasten while (count--) { 40ddb2e930bef3d1be1627c613fb8f3df848166ac7Glenn Kasten *--dst = (int16_t)(*--src - 0x80) << 8; 41ddb2e930bef3d1be1627c613fb8f3df848166ac7Glenn Kasten } 42ddb2e930bef3d1be1627c613fb8f3df848166ac7Glenn Kasten} 437a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten 4478da2acdd7aa14fe9920d1ad3074e445332de27dGlenn Kastenvoid memcpy_to_u8_from_i16(uint8_t *dst, const int16_t *src, size_t count) 4578da2acdd7aa14fe9920d1ad3074e445332de27dGlenn Kasten{ 4678da2acdd7aa14fe9920d1ad3074e445332de27dGlenn Kasten while (count--) { 4778da2acdd7aa14fe9920d1ad3074e445332de27dGlenn Kasten *dst++ = (*src++ >> 8) + 0x80; 4878da2acdd7aa14fe9920d1ad3074e445332de27dGlenn Kasten } 4978da2acdd7aa14fe9920d1ad3074e445332de27dGlenn Kasten} 5078da2acdd7aa14fe9920d1ad3074e445332de27dGlenn Kasten 5123ef1b379caa5e641367f115d67080ed35069117Andy Hungvoid memcpy_to_u8_from_float(uint8_t *dst, const float *src, size_t count) 5223ef1b379caa5e641367f115d67080ed35069117Andy Hung{ 5323ef1b379caa5e641367f115d67080ed35069117Andy Hung while (count--) { 5423ef1b379caa5e641367f115d67080ed35069117Andy Hung *dst++ = clamp8_from_float(*src++); 5523ef1b379caa5e641367f115d67080ed35069117Andy Hung } 5623ef1b379caa5e641367f115d67080ed35069117Andy Hung} 5723ef1b379caa5e641367f115d67080ed35069117Andy Hung 585d43605288f444eb30999334642253283941fb3dGlenn Kastenvoid memcpy_to_i16_from_i32(int16_t *dst, const int32_t *src, size_t count) 595d43605288f444eb30999334642253283941fb3dGlenn Kasten{ 605d43605288f444eb30999334642253283941fb3dGlenn Kasten while (count--) { 615d43605288f444eb30999334642253283941fb3dGlenn Kasten *dst++ = *src++ >> 16; 625d43605288f444eb30999334642253283941fb3dGlenn Kasten } 635d43605288f444eb30999334642253283941fb3dGlenn Kasten} 645d43605288f444eb30999334642253283941fb3dGlenn Kasten 655d43605288f444eb30999334642253283941fb3dGlenn Kastenvoid memcpy_to_i16_from_float(int16_t *dst, const float *src, size_t count) 665d43605288f444eb30999334642253283941fb3dGlenn Kasten{ 675d43605288f444eb30999334642253283941fb3dGlenn Kasten while (count--) { 6865b5ccd2a7a7c756db2cfa627ac75d17eb0484c2Andy Hung *dst++ = clamp16_from_float(*src++); 695d43605288f444eb30999334642253283941fb3dGlenn Kasten } 705d43605288f444eb30999334642253283941fb3dGlenn Kasten} 715d43605288f444eb30999334642253283941fb3dGlenn Kasten 72b878e52fd43ea10f8d45eb29e5901ec1c8fb7278Andy Hungvoid memcpy_to_float_from_q4_27(float *dst, const int32_t *src, size_t count) 73aecb15eb579434bbc980892b3707374340e00138Andy Hung{ 74b878e52fd43ea10f8d45eb29e5901ec1c8fb7278Andy Hung while (count--) { 75d2a25cda1c23cc7092824028c49b2bce2cbb4ca2Andy Hung *dst++ = float_from_q4_27(*src++); 76aecb15eb579434bbc980892b3707374340e00138Andy Hung } 77aecb15eb579434bbc980892b3707374340e00138Andy Hung} 78aecb15eb579434bbc980892b3707374340e00138Andy Hung 79e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hungvoid memcpy_to_float_from_i16(float *dst, const int16_t *src, size_t count) 80e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung{ 81e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung while (count--) { 82e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = float_from_i16(*src++); 83e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung } 84e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung} 85e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung 8623ef1b379caa5e641367f115d67080ed35069117Andy Hungvoid memcpy_to_float_from_u8(float *dst, const uint8_t *src, size_t count) 8723ef1b379caa5e641367f115d67080ed35069117Andy Hung{ 8823ef1b379caa5e641367f115d67080ed35069117Andy Hung while (count--) { 8923ef1b379caa5e641367f115d67080ed35069117Andy Hung *dst++ = float_from_u8(*src++); 9023ef1b379caa5e641367f115d67080ed35069117Andy Hung } 9123ef1b379caa5e641367f115d67080ed35069117Andy Hung} 9223ef1b379caa5e641367f115d67080ed35069117Andy Hung 93e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hungvoid memcpy_to_float_from_p24(float *dst, const uint8_t *src, size_t count) 94e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung{ 95e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung while (count--) { 96e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = float_from_p24(src); 97e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung src += 3; 98e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung } 99e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung} 100e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung 101e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hungvoid memcpy_to_i16_from_p24(int16_t *dst, const uint8_t *src, size_t count) 102e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung{ 103e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung while (count--) { 1042af0e17cfac0445b411e018cbb0dbc7c4ed64b0bAndy Hung#if HAVE_BIG_ENDIAN 105e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = src[1] | (src[0] << 8); 106e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung#else 107e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = src[1] | (src[2] << 8); 108e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung#endif 109e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung src += 3; 11095880cb16065385b5633e2f39c7a285d84dfef98Glenn Kasten } 11195880cb16065385b5633e2f39c7a285d84dfef98Glenn Kasten} 11295880cb16065385b5633e2f39c7a285d84dfef98Glenn Kasten 11395880cb16065385b5633e2f39c7a285d84dfef98Glenn Kastenvoid memcpy_to_i32_from_p24(int32_t *dst, const uint8_t *src, size_t count) 11495880cb16065385b5633e2f39c7a285d84dfef98Glenn Kasten{ 11595880cb16065385b5633e2f39c7a285d84dfef98Glenn Kasten while (count--) { 1162af0e17cfac0445b411e018cbb0dbc7c4ed64b0bAndy Hung#if HAVE_BIG_ENDIAN 11795880cb16065385b5633e2f39c7a285d84dfef98Glenn Kasten *dst++ = (src[2] << 8) | (src[1] << 16) | (src[0] << 24); 11895880cb16065385b5633e2f39c7a285d84dfef98Glenn Kasten#else 11995880cb16065385b5633e2f39c7a285d84dfef98Glenn Kasten *dst++ = (src[0] << 8) | (src[1] << 16) | (src[2] << 24); 12095880cb16065385b5633e2f39c7a285d84dfef98Glenn Kasten#endif 12195880cb16065385b5633e2f39c7a285d84dfef98Glenn Kasten src += 3; 122e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung } 123e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung} 124e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung 125e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hungvoid memcpy_to_p24_from_i16(uint8_t *dst, const int16_t *src, size_t count) 126e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung{ 127e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung while (count--) { 1282af0e17cfac0445b411e018cbb0dbc7c4ed64b0bAndy Hung#if HAVE_BIG_ENDIAN 129e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = *src >> 8; 130e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = *src++; 131e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = 0; 132e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung#else 133e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = 0; 134e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = *src; 135e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = *src++ >> 8; 136e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung#endif 137e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung } 138e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung} 139e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung 140e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hungvoid memcpy_to_p24_from_float(uint8_t *dst, const float *src, size_t count) 141e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung{ 142e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung while (count--) { 143e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung int32_t ival = clamp24_from_float(*src++); 144e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung 1452af0e17cfac0445b411e018cbb0dbc7c4ed64b0bAndy Hung#if HAVE_BIG_ENDIAN 146e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = ival >> 16; 147e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = ival >> 8; 148e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = ival; 149e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung#else 150e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = ival; 151e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = ival >> 8; 152e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung *dst++ = ival >> 16; 153e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung#endif 154e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung } 155e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung} 156e0454e2f58e18d954a3de3fa9a9c0e1be3d56f57Andy Hung 157eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kastenvoid memcpy_to_p24_from_q8_23(uint8_t *dst, const int32_t *src, size_t count) 158eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten{ 159eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten while (count--) { 160eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten int32_t ival = clamp24_from_q8_23(*src++); 161eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten 1622af0e17cfac0445b411e018cbb0dbc7c4ed64b0bAndy Hung#if HAVE_BIG_ENDIAN 163eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten *dst++ = ival >> 16; 164eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten *dst++ = ival >> 8; 165eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten *dst++ = ival; 166eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten#else 167eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten *dst++ = ival; 168eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten *dst++ = ival >> 8; 169eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten *dst++ = ival >> 16; 170eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten#endif 171eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten } 172eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten} 173eee45154f377e6fc9face02b44ec10a831d98ef7Glenn Kasten 174daa1a00524b5628676075cf17e1699e904252303Glenn Kastenvoid memcpy_to_p24_from_i32(uint8_t *dst, const int32_t *src, size_t count) 175daa1a00524b5628676075cf17e1699e904252303Glenn Kasten{ 176daa1a00524b5628676075cf17e1699e904252303Glenn Kasten while (count--) { 177daa1a00524b5628676075cf17e1699e904252303Glenn Kasten int32_t ival = *src++ >> 8; 178daa1a00524b5628676075cf17e1699e904252303Glenn Kasten 1792af0e17cfac0445b411e018cbb0dbc7c4ed64b0bAndy Hung#if HAVE_BIG_ENDIAN 180daa1a00524b5628676075cf17e1699e904252303Glenn Kasten *dst++ = ival >> 16; 181daa1a00524b5628676075cf17e1699e904252303Glenn Kasten *dst++ = ival >> 8; 182daa1a00524b5628676075cf17e1699e904252303Glenn Kasten *dst++ = ival; 183daa1a00524b5628676075cf17e1699e904252303Glenn Kasten#else 184daa1a00524b5628676075cf17e1699e904252303Glenn Kasten *dst++ = ival; 185daa1a00524b5628676075cf17e1699e904252303Glenn Kasten *dst++ = ival >> 8; 186daa1a00524b5628676075cf17e1699e904252303Glenn Kasten *dst++ = ival >> 16; 187daa1a00524b5628676075cf17e1699e904252303Glenn Kasten#endif 188daa1a00524b5628676075cf17e1699e904252303Glenn Kasten } 189daa1a00524b5628676075cf17e1699e904252303Glenn Kasten} 190daa1a00524b5628676075cf17e1699e904252303Glenn Kasten 191d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hungvoid memcpy_to_q8_23_from_i16(int32_t *dst, const int16_t *src, size_t count) 192d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung{ 193d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung while (count--) { 194d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung *dst++ = (int32_t)*src++ << 8; 195d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung } 196d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung} 197d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung 198d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hungvoid memcpy_to_q8_23_from_float_with_clamp(int32_t *dst, const float *src, size_t count) 199d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung{ 200d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung while (count--) { 201d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung *dst++ = clamp24_from_float(*src++); 202d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung } 203d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung} 204d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung 20578ac9f8cb0d6ae6a01ad9d2329ecf18db3a1f050Haynes Mathew Georgevoid memcpy_to_q8_23_from_p24(int32_t *dst, const uint8_t *src, size_t count) 20678ac9f8cb0d6ae6a01ad9d2329ecf18db3a1f050Haynes Mathew George{ 20778ac9f8cb0d6ae6a01ad9d2329ecf18db3a1f050Haynes Mathew George while (count--) { 2082af0e17cfac0445b411e018cbb0dbc7c4ed64b0bAndy Hung#if HAVE_BIG_ENDIAN 20978ac9f8cb0d6ae6a01ad9d2329ecf18db3a1f050Haynes Mathew George *dst++ = (int8_t)src[0] << 16 | src[1] << 8 | src[2]; 21078ac9f8cb0d6ae6a01ad9d2329ecf18db3a1f050Haynes Mathew George#else 21178ac9f8cb0d6ae6a01ad9d2329ecf18db3a1f050Haynes Mathew George *dst++ = (int8_t)src[2] << 16 | src[1] << 8 | src[0]; 21278ac9f8cb0d6ae6a01ad9d2329ecf18db3a1f050Haynes Mathew George#endif 21378ac9f8cb0d6ae6a01ad9d2329ecf18db3a1f050Haynes Mathew George src += 3; 21478ac9f8cb0d6ae6a01ad9d2329ecf18db3a1f050Haynes Mathew George } 21578ac9f8cb0d6ae6a01ad9d2329ecf18db3a1f050Haynes Mathew George} 21678ac9f8cb0d6ae6a01ad9d2329ecf18db3a1f050Haynes Mathew George 217b878e52fd43ea10f8d45eb29e5901ec1c8fb7278Andy Hungvoid memcpy_to_q4_27_from_float(int32_t *dst, const float *src, size_t count) 218b878e52fd43ea10f8d45eb29e5901ec1c8fb7278Andy Hung{ 219b878e52fd43ea10f8d45eb29e5901ec1c8fb7278Andy Hung while (count--) { 220b878e52fd43ea10f8d45eb29e5901ec1c8fb7278Andy Hung *dst++ = clampq4_27_from_float(*src++); 221b878e52fd43ea10f8d45eb29e5901ec1c8fb7278Andy Hung } 222b878e52fd43ea10f8d45eb29e5901ec1c8fb7278Andy Hung} 223b878e52fd43ea10f8d45eb29e5901ec1c8fb7278Andy Hung 224d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hungvoid memcpy_to_i16_from_q8_23(int16_t *dst, const int32_t *src, size_t count) 225d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung{ 226d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung while (count--) { 227d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung *dst++ = clamp16(*src++ >> 8); 228d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung } 229d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung} 230d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung 231d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hungvoid memcpy_to_float_from_q8_23(float *dst, const int32_t *src, size_t count) 232d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung{ 233d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung while (count--) { 234d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung *dst++ = float_from_q8_23(*src++); 235d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung } 236d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung} 237d5829888ea3839910dcf7c94f3308d469c46b7faAndy Hung 2382c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hungvoid memcpy_to_i32_from_i16(int32_t *dst, const int16_t *src, size_t count) 2392c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung{ 2402c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung while (count--) { 2412c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung *dst++ = (int32_t)*src++ << 16; 2422c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung } 2432c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung} 2442c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung 2452c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hungvoid memcpy_to_i32_from_float(int32_t *dst, const float *src, size_t count) 2462c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung{ 2472c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung while (count--) { 2482c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung *dst++ = clamp32_from_float(*src++); 2492c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung } 2502c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung} 2512c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung 2522c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hungvoid memcpy_to_float_from_i32(float *dst, const int32_t *src, size_t count) 2532c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung{ 2542c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung while (count--) { 2552c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung *dst++ = float_from_i32(*src++); 2562c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung } 2572c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung} 2582c63fb60f8086000645e82cfb384cc8dfc5d1634Andy Hung 2597a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kastenvoid downmix_to_mono_i16_from_stereo_i16(int16_t *dst, const int16_t *src, size_t count) 2607a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten{ 2617a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten while (count--) { 2627a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten *dst++ = (int16_t)(((int32_t)src[0] + (int32_t)src[1]) >> 1); 2637a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten src += 2; 2647a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten } 2657a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten} 2667a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten 2677a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kastenvoid upmix_to_stereo_i16_from_mono_i16(int16_t *dst, const int16_t *src, size_t count) 2687a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten{ 2697a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten while (count--) { 2707a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten int32_t temp = *src++; 2717a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten dst[0] = temp; 2727a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten dst[1] = temp; 2737a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten dst += 2; 2747a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten } 2757a0bacaf9634d6bf0abbea9608fb15e94a675358Glenn Kasten} 276eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten 2779c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hungvoid downmix_to_mono_float_from_stereo_float(float *dst, const float *src, size_t frames) 2789c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung{ 2799c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung while (frames--) { 2809c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung *dst++ = (src[0] + src[1]) * 0.5; 2819c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung src += 2; 2829c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung } 2839c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung} 2849c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung 2859c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hungvoid upmix_to_stereo_float_from_mono_float(float *dst, const float *src, size_t frames) 2869c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung{ 2879c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung while (frames--) { 2889c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung float temp = *src++; 2899c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung dst[0] = temp; 2909c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung dst[1] = temp; 2919c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung dst += 2; 2929c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung } 2939c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung} 2949c8dd45c203ac7f9fa558b9436fa133a3026d62aAndy Hung 295eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kastensize_t nonZeroMono32(const int32_t *samples, size_t count) 296eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten{ 297eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten size_t nonZero = 0; 298eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten while (count-- > 0) { 299eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten if (*samples++ != 0) { 300eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten nonZero++; 301eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten } 302eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten } 303eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten return nonZero; 304eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten} 305eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten 306eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kastensize_t nonZeroMono16(const int16_t *samples, size_t count) 307eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten{ 308eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten size_t nonZero = 0; 309eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten while (count-- > 0) { 310eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten if (*samples++ != 0) { 311eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten nonZero++; 312eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten } 313eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten } 314eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten return nonZero; 315eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten} 316eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten 317eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kastensize_t nonZeroStereo32(const int32_t *frames, size_t count) 318eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten{ 319eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten size_t nonZero = 0; 320eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten while (count-- > 0) { 321eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten if (frames[0] != 0 || frames[1] != 0) { 322eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten nonZero++; 323eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten } 324eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten frames += 2; 325eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten } 326eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten return nonZero; 327eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten} 328eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten 329eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kastensize_t nonZeroStereo16(const int16_t *frames, size_t count) 330eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten{ 331eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten size_t nonZero = 0; 332eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten while (count-- > 0) { 333eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten if (frames[0] != 0 || frames[1] != 0) { 334eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten nonZero++; 335eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten } 336eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten frames += 2; 337eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten } 338eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten return nonZero; 339eb247dfc917ebfb02e164cdd82ce5fa2c0815369Glenn Kasten} 3403af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 3413af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung/* 3423af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung * C macro to do channel mask copying independent of dst/src sample type. 3433af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung * Don't pass in any expressions for the macro arguments here. 3443af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung */ 3453af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung#define copy_frame_by_mask(dst, dmask, src, smask, count, zero) \ 3463af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung{ \ 3473af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung uint32_t bit, ormask; \ 3482c853ace047f87e2de66bca0e653c70cdb1eed65Chih-Hung Hsieh while ((count)--) { \ 3492c853ace047f87e2de66bca0e653c70cdb1eed65Chih-Hung Hsieh ormask = (dmask) | (smask); \ 3503af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung while (ormask) { \ 3513af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung bit = ormask & -ormask; /* get lowest bit */ \ 3523af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung ormask ^= bit; /* remove lowest bit */ \ 3532c853ace047f87e2de66bca0e653c70cdb1eed65Chih-Hung Hsieh if ((dmask) & bit) { \ 3542c853ace047f87e2de66bca0e653c70cdb1eed65Chih-Hung Hsieh *(dst)++ = (smask) & bit ? *(src)++ : (zero); \ 3553af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } else { /* source channel only */ \ 3562c853ace047f87e2de66bca0e653c70cdb1eed65Chih-Hung Hsieh ++(src); \ 3573af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } \ 3583af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } \ 3593af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } \ 3603af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung} 3613af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 3623af2af2518dc5534c94285e77d39d0fc729ed917Andy Hungvoid memcpy_by_channel_mask(void *dst, uint32_t dst_mask, 3633af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung const void *src, uint32_t src_mask, size_t sample_size, size_t count) 3643af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung{ 3653af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung#if 0 3663af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung /* alternate way of handling memcpy_by_channel_mask by using the idxary */ 3673af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung int8_t idxary[32]; 3683af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung uint32_t src_channels = popcount(src_mask); 3693af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung uint32_t dst_channels = 3703af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung memcpy_by_index_array_initialization(idxary, 32, dst_mask, src_mask); 3713af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 3723af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung memcpy_by_idxary(dst, dst_channels, src, src_channels, idxary, sample_size, count); 3733af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung#else 3743af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung if (dst_mask == src_mask) { 3753af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung memcpy(dst, src, sample_size * popcount(dst_mask) * count); 3763af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung return; 3773af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } 3783af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung switch (sample_size) { 3793af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung case 1: { 3803af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung uint8_t *udst = (uint8_t*)dst; 3813af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung const uint8_t *usrc = (const uint8_t*)src; 3823af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 3833af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung copy_frame_by_mask(udst, dst_mask, usrc, src_mask, count, 0); 3843af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } break; 3853af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung case 2: { 3863af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung uint16_t *udst = (uint16_t*)dst; 3873af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung const uint16_t *usrc = (const uint16_t*)src; 3883af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 3893af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung copy_frame_by_mask(udst, dst_mask, usrc, src_mask, count, 0); 3903af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } break; 3913af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung case 3: { /* could be slow. use a struct to represent 3 bytes of data. */ 3923af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung uint8x3_t *udst = (uint8x3_t*)dst; 3933af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung const uint8x3_t *usrc = (const uint8x3_t*)src; 3943af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung static const uint8x3_t zero; /* tricky - we use this to zero out a sample */ 3953af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 3963af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung copy_frame_by_mask(udst, dst_mask, usrc, src_mask, count, zero); 3973af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } break; 3983af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung case 4: { 3993af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung uint32_t *udst = (uint32_t*)dst; 4003af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung const uint32_t *usrc = (const uint32_t*)src; 4013af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 4023af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung copy_frame_by_mask(udst, dst_mask, usrc, src_mask, count, 0); 4033af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } break; 4043af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung default: 4053af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung abort(); /* illegal value */ 4063af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung break; 4073af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } 4083af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung#endif 4093af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung} 4103af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 4113af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung/* 4123af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung * C macro to do copying by index array, to rearrange samples 4133af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung * within a frame. This is independent of src/dst sample type. 4143af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung * Don't pass in any expressions for the macro arguments here. 4153af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung */ 4163af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung#define copy_frame_by_idx(dst, dst_channels, src, src_channels, idxary, count, zero) \ 4173af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung{ \ 4183af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung unsigned i; \ 4193af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung int index; \ 4202c853ace047f87e2de66bca0e653c70cdb1eed65Chih-Hung Hsieh while ((count)--) { \ 4212c853ace047f87e2de66bca0e653c70cdb1eed65Chih-Hung Hsieh for (i = 0; i < (dst_channels); ++i) { \ 4222c853ace047f87e2de66bca0e653c70cdb1eed65Chih-Hung Hsieh index = (idxary)[i]; \ 4232c853ace047f87e2de66bca0e653c70cdb1eed65Chih-Hung Hsieh *(dst)++ = index < 0 ? (zero) : (src)[index]; \ 4243af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } \ 4252c853ace047f87e2de66bca0e653c70cdb1eed65Chih-Hung Hsieh (src) += (src_channels); \ 4263af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } \ 4273af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung} 4283af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 4293af2af2518dc5534c94285e77d39d0fc729ed917Andy Hungvoid memcpy_by_index_array(void *dst, uint32_t dst_channels, 4303af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung const void *src, uint32_t src_channels, 4313af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung const int8_t *idxary, size_t sample_size, size_t count) 4323af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung{ 4333af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung switch (sample_size) { 4343af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung case 1: { 4353af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung uint8_t *udst = (uint8_t*)dst; 4363af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung const uint8_t *usrc = (const uint8_t*)src; 4373af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 4383af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung copy_frame_by_idx(udst, dst_channels, usrc, src_channels, idxary, count, 0); 4393af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } break; 4403af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung case 2: { 4413af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung uint16_t *udst = (uint16_t*)dst; 4423af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung const uint16_t *usrc = (const uint16_t*)src; 4433af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 4443af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung copy_frame_by_idx(udst, dst_channels, usrc, src_channels, idxary, count, 0); 4453af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } break; 4463af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung case 3: { /* could be slow. use a struct to represent 3 bytes of data. */ 4473af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung uint8x3_t *udst = (uint8x3_t*)dst; 4483af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung const uint8x3_t *usrc = (const uint8x3_t*)src; 4493af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung static const uint8x3_t zero; 4503af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 4513af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung copy_frame_by_idx(udst, dst_channels, usrc, src_channels, idxary, count, zero); 4523af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } break; 4533af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung case 4: { 4543af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung uint32_t *udst = (uint32_t*)dst; 4553af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung const uint32_t *usrc = (const uint32_t*)src; 4563af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 4573af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung copy_frame_by_idx(udst, dst_channels, usrc, src_channels, idxary, count, 0); 4583af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } break; 4593af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung default: 4603af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung abort(); /* illegal value */ 4613af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung break; 4623af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } 4633af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung} 4643af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 4653af2af2518dc5534c94285e77d39d0fc729ed917Andy Hungsize_t memcpy_by_index_array_initialization(int8_t *idxary, size_t idxcount, 4663af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung uint32_t dst_mask, uint32_t src_mask) 4673af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung{ 4683af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung size_t n = 0; 4693af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung int srcidx = 0; 4703af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung uint32_t bit, ormask = src_mask | dst_mask; 4713af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung 4723af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung while (ormask && n < idxcount) { 4733af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung bit = ormask & -ormask; /* get lowest bit */ 4743af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung ormask ^= bit; /* remove lowest bit */ 4753af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung if (src_mask & dst_mask & bit) { /* matching channel */ 4763af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung idxary[n++] = srcidx++; 4773af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } else if (src_mask & bit) { /* source channel only */ 4783af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung ++srcidx; 4793af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } else { /* destination channel only */ 4803af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung idxary[n++] = -1; 4813af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } 4823af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung } 4833af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung return n + popcount(ormask & dst_mask); 4843af2af2518dc5534c94285e77d39d0fc729ed917Andy Hung} 4855a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung 4865a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hungsize_t memcpy_by_index_array_initialization_src_index(int8_t *idxary, size_t idxcount, 4875a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung uint32_t dst_mask, uint32_t src_mask) { 4885a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung size_t dst_count = popcount(dst_mask); 4895a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung if (idxcount == 0) { 4905a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung return dst_count; 4915a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung } 4925a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung if (dst_count > idxcount) { 4935a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung dst_count = idxcount; 4945a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung } 4955a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung 4965a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung size_t src_idx, dst_idx; 4975a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung for (src_idx = 0, dst_idx = 0; dst_idx < dst_count; ++dst_idx) { 4985a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung if (src_mask & 1) { 4995a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung idxary[dst_idx] = src_idx++; 5005a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung } else { 5015a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung idxary[dst_idx] = -1; 5025a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung } 5035a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung src_mask >>= 1; 5045a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung } 5055a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung return dst_idx; 5065a0d0285cdd75cd5c94e020311830d634d6587b0Andy Hung} 507291a1948093850aa87f60264680404b2b96ee327Andy Hung 508291a1948093850aa87f60264680404b2b96ee327Andy Hungsize_t memcpy_by_index_array_initialization_dst_index(int8_t *idxary, size_t idxcount, 509291a1948093850aa87f60264680404b2b96ee327Andy Hung uint32_t dst_mask, uint32_t src_mask) { 510291a1948093850aa87f60264680404b2b96ee327Andy Hung size_t src_idx, dst_idx; 511291a1948093850aa87f60264680404b2b96ee327Andy Hung size_t dst_count = __builtin_popcount(dst_mask); 512291a1948093850aa87f60264680404b2b96ee327Andy Hung size_t src_count = __builtin_popcount(src_mask); 513291a1948093850aa87f60264680404b2b96ee327Andy Hung if (idxcount == 0) { 514291a1948093850aa87f60264680404b2b96ee327Andy Hung return dst_count; 515291a1948093850aa87f60264680404b2b96ee327Andy Hung } 516291a1948093850aa87f60264680404b2b96ee327Andy Hung if (dst_count > idxcount) { 517291a1948093850aa87f60264680404b2b96ee327Andy Hung dst_count = idxcount; 518291a1948093850aa87f60264680404b2b96ee327Andy Hung } 519291a1948093850aa87f60264680404b2b96ee327Andy Hung for (src_idx = 0, dst_idx = 0; dst_idx < dst_count; ++src_idx) { 520291a1948093850aa87f60264680404b2b96ee327Andy Hung if (dst_mask & 1) { 521291a1948093850aa87f60264680404b2b96ee327Andy Hung idxary[dst_idx++] = src_idx < src_count ? (signed)src_idx : -1; 522291a1948093850aa87f60264680404b2b96ee327Andy Hung } 523291a1948093850aa87f60264680404b2b96ee327Andy Hung dst_mask >>= 1; 524291a1948093850aa87f60264680404b2b96ee327Andy Hung } 525291a1948093850aa87f60264680404b2b96ee327Andy Hung return dst_idx; 526291a1948093850aa87f60264680404b2b96ee327Andy Hung} 527