diff options
Diffstat (limited to 'lib/virtual_oss/bt/sbc_encode.c')
| -rw-r--r-- | lib/virtual_oss/bt/sbc_encode.c | 701 |
1 files changed, 701 insertions, 0 deletions
diff --git a/lib/virtual_oss/bt/sbc_encode.c b/lib/virtual_oss/bt/sbc_encode.c new file mode 100644 index 000000000000..153f2e69e76e --- /dev/null +++ b/lib/virtual_oss/bt/sbc_encode.c @@ -0,0 +1,701 @@ +/*- + * Copyright (c) 2015 Nathanial Sloss <nathanialsloss@yahoo.com.au> + * + * This software is dedicated to the memory of - + * Baron James Anlezark (Barry) - 1 Jan 1949 - 13 May 2012. + * + * Barry was a man who loved his music. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS + * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED + * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS + * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <sys/cdefs.h> +#include <sys/types.h> +#include <sys/param.h> +#include <sys/endian.h> +#include <sys/uio.h> + +#include <stdio.h> +#include <errno.h> +#include <stdbool.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <math.h> + +#include "sbc_coeffs.h" +#include "bt.h" + +#define SYNCWORD 0x9c +#define ABS(x) (((x) < 0) ? -(x) : (x)) +#define BIT30 (1U << 30) +#define BM(x) ((1LL << (x)) - 1LL) + +/* Loudness offset allocations. */ +static const int loudnessoffset8[4][8] = { + {-2, 0, 0, 0, 0, 0, 0, 1}, + {-3, 0, 0, 0, 0, 0, 1, 2}, + {-4, 0, 0, 0, 0, 0, 1, 2}, + {-4, 0, 0, 0, 0, 0, 1, 2}, +}; + +static const int loudnessoffset4[4][4] = { + {-1, 0, 0, 0}, + {-2, 0, 0, 1}, + {-2, 0, 0, 1}, + {-2, 0, 0, 1}, +}; + +static uint8_t +calc_scalefactors_joint(struct sbc_encode *sbc) +{ + float sb_j[16][2]; + uint32_t x; + uint32_t y; + uint8_t block; + uint8_t joint; + uint8_t sb; + uint8_t lz; + + joint = 0; + for (sb = 0; sb != sbc->bands - 1; sb++) { + for (block = 0; block < sbc->blocks; block++) { + sb_j[block][0] = (sbc->samples[block][0][sb] + + sbc->samples[block][1][sb]) / 2.0f; + sb_j[block][1] = (sbc->samples[block][0][sb] - + sbc->samples[block][1][sb]) / 2.0f; + } + + x = 1 << 15; + y = 1 << 15; + for (block = 0; block < sbc->blocks; block++) { + x |= (uint32_t)ABS(sb_j[block][0]); + y |= (uint32_t)ABS(sb_j[block][1]); + } + + lz = 1; + while (!(x & BIT30)) { + lz++; + x <<= 1; + } + x = 16 - lz; + + lz = 1; + while (!(y & BIT30)) { + lz++; + y <<= 1; + } + y = 16 - lz; + + if ((sbc->scalefactor[0][sb] + sbc->scalefactor[1][sb]) > x + y) { + joint |= 1 << (sbc->bands - sb - 1); + sbc->scalefactor[0][sb] = x; + sbc->scalefactor[1][sb] = y; + for (block = 0; block < sbc->blocks; block++) { + sbc->samples[block][0][sb] = sb_j[block][0]; + sbc->samples[block][1][sb] = sb_j[block][1]; + } + } + } + return (joint); +} + +static void +calc_scalefactors(struct sbc_encode *sbc) +{ + uint8_t block; + uint8_t ch; + uint8_t sb; + + for (ch = 0; ch != sbc->channels; ch++) { + for (sb = 0; sb != sbc->bands; sb++) { + uint32_t x = 1 << 15; + uint8_t lx = 1; + + for (block = 0; block != sbc->blocks; block++) + x |= (uint32_t)ABS(sbc->samples[block][ch][sb]); + + while (!(x & BIT30)) { + lx++; + x <<= 1; + } + sbc->scalefactor[ch][sb] = 16 - lx; + } + } +} + +static void +calc_bitneed(struct bt_config *cfg) +{ + struct sbc_encode *sbc = cfg->handle.sbc_enc; + int32_t bitneed[2][8]; + int32_t max_bitneed, bitcount; + int32_t slicecount, bitslice; + int32_t loudness; + int ch, sb, start_chan = 0; + + if (cfg->chmode == MODE_DUAL) + sbc->channels = 1; + +next_chan: + max_bitneed = 0; + bitcount = 0; + slicecount = 0; + + if (cfg->allocm == ALLOC_SNR) { + for (ch = start_chan; ch < sbc->channels; ch++) { + for (sb = 0; sb < sbc->bands; sb++) { + bitneed[ch][sb] = sbc->scalefactor[ch][sb]; + + if (bitneed[ch][sb] > max_bitneed) + max_bitneed = bitneed[ch][sb]; + } + } + } else { + for (ch = start_chan; ch < sbc->channels; ch++) { + for (sb = 0; sb < sbc->bands; sb++) { + if (sbc->scalefactor[ch][sb] == 0) { + bitneed[ch][sb] = -5; + } else { + if (sbc->bands == 8) { + loudness = sbc->scalefactor[ch][sb] - + loudnessoffset8[cfg->freq][sb]; + } else { + loudness = sbc->scalefactor[ch][sb] - + loudnessoffset4[cfg->freq][sb]; + } + if (loudness > 0) + bitneed[ch][sb] = loudness / 2; + else + bitneed[ch][sb] = loudness; + } + if (bitneed[ch][sb] > max_bitneed) + max_bitneed = bitneed[ch][sb]; + } + } + } + + slicecount = bitcount = 0; + bitslice = max_bitneed + 1; + do { + bitslice--; + bitcount += slicecount; + slicecount = 0; + for (ch = start_chan; ch < sbc->channels; ch++) { + for (sb = 0; sb < sbc->bands; sb++) { + if ((bitneed[ch][sb] > bitslice + 1) && + (bitneed[ch][sb] < bitslice + 16)) + slicecount++; + else if (bitneed[ch][sb] == bitslice + 1) + slicecount += 2; + } + } + } while (bitcount + slicecount < cfg->bitpool); + + /* check if exactly one more fits */ + if (bitcount + slicecount == cfg->bitpool) { + bitcount += slicecount; + bitslice--; + } + for (ch = start_chan; ch < sbc->channels; ch++) { + for (sb = 0; sb < sbc->bands; sb++) { + if (bitneed[ch][sb] < bitslice + 2) { + sbc->bits[ch][sb] = 0; + } else { + sbc->bits[ch][sb] = bitneed[ch][sb] - bitslice; + if (sbc->bits[ch][sb] > 16) + sbc->bits[ch][sb] = 16; + } + } + } + + if (cfg->chmode == MODE_DUAL) + ch = start_chan; + else + ch = 0; + sb = 0; + while (bitcount < cfg->bitpool && sb < sbc->bands) { + if ((sbc->bits[ch][sb] >= 2) && (sbc->bits[ch][sb] < 16)) { + sbc->bits[ch][sb]++; + bitcount++; + } else if ((bitneed[ch][sb] == bitslice + 1) && + (cfg->bitpool > bitcount + 1)) { + sbc->bits[ch][sb] = 2; + bitcount += 2; + } + if (sbc->channels == 1 || start_chan == 1) + sb++; + else if (ch == 1) { + ch = 0; + sb++; + } else + ch = 1; + } + + if (cfg->chmode == MODE_DUAL) + ch = start_chan; + else + ch = 0; + sb = 0; + while (bitcount < cfg->bitpool && sb < sbc->bands) { + if (sbc->bits[ch][sb] < 16) { + sbc->bits[ch][sb]++; + bitcount++; + } + if (sbc->channels == 1 || start_chan == 1) + sb++; + else if (ch == 1) { + ch = 0; + sb++; + } else + ch = 1; + } + + if (cfg->chmode == MODE_DUAL && start_chan == 0) { + start_chan = 1; + sbc->channels = 2; + goto next_chan; + } +} + +static void +sbc_store_bits_crc(struct sbc_encode *sbc, uint32_t numbits, uint32_t value) +{ + uint32_t off = sbc->bitoffset; + + while (numbits-- && off != sbc->maxoffset) { + if (value & (1 << numbits)) { + sbc->data[off / 8] |= 1 << ((7 - off) & 7); + sbc->crc ^= 0x80; + } + sbc->crc *= 2; + if (sbc->crc & 0x100) + sbc->crc ^= 0x11d; /* CRC-8 polynomial */ + + off++; + } + sbc->bitoffset = off; +} + +static int +sbc_encode(struct bt_config *cfg) +{ + struct sbc_encode *sbc = cfg->handle.sbc_enc; + const int16_t *input = sbc->music_data; + float delta[2][8]; + float levels[2][8]; + float mask[2][8]; + float S; + float *X; + float Z[80]; + float Y[80]; + float audioout; + int16_t left[8]; + int16_t right[8]; + int16_t *data; + int numsamples; + int i; + int k; + int block; + int chan; + int sb; + + for (block = 0; block < sbc->blocks; block++) { + + for (i = 0; i < sbc->bands; i++) { + left[i] = *input++; + if (sbc->channels == 2) + right[i] = *input++; + } + + for (chan = 0; chan < sbc->channels; chan++) { + + /* select right or left channel */ + if (chan == 0) { + X = sbc->left; + data = left; + } else { + X = sbc->right; + data = right; + } + + /* shift up old data */ + for (i = (sbc->bands * 10) - 1; i > sbc->bands - 1; i--) + X[i] = X[i - sbc->bands]; + k = 0; + for (i = sbc->bands - 1; i >= 0; i--) + X[i] = data[k++]; + for (i = 0; i < sbc->bands * 10; i++) { + if (sbc->bands == 8) + Z[i] = sbc_coeffs8[i] * X[i]; + else + Z[i] = sbc_coeffs4[i] * X[i]; + } + for (i = 0; i < sbc->bands * 2; i++) { + Y[i] = 0; + for (k = 0; k < 5; k++) + Y[i] += Z[i + k * sbc->bands * 2]; + } + for (i = 0; i < sbc->bands; i++) { + S = 0; + for (k = 0; k < sbc->bands * 2; k++) { + if (sbc->bands == 8) { + S += cosdata8[i][k] * Y[k]; + } else { + S += cosdata4[i][k] * Y[k]; + } + } + sbc->samples[block][chan][i] = S * (1 << 15); + } + } + } + + calc_scalefactors(sbc); + + if (cfg->chmode == MODE_JOINT) + sbc->join = calc_scalefactors_joint(sbc); + else + sbc->join = 0; + + calc_bitneed(cfg); + + for (chan = 0; chan < sbc->channels; chan++) { + for (sb = 0; sb < sbc->bands; sb++) { + if (sbc->bits[chan][sb] == 0) + continue; + mask[chan][sb] = BM(sbc->bits[chan][sb]); + levels[chan][sb] = mask[chan][sb] * + (1LL << (15 - sbc->scalefactor[chan][sb])); + delta[chan][sb] = + (1LL << (sbc->scalefactor[chan][sb] + 16)); + } + } + + numsamples = 0; + for (block = 0; block < sbc->blocks; block++) { + for (chan = 0; chan < sbc->channels; chan++) { + for (sb = 0; sb < sbc->bands; sb++) { + if (sbc->bits[chan][sb] == 0) + continue; + audioout = (levels[chan][sb] * + (delta[chan][sb] + sbc->samples[block][chan][sb])); + audioout /= (1LL << 32); + + audioout = roundf(audioout); + + /* range check */ + if (audioout > mask[chan][sb]) + audioout = mask[chan][sb]; + + sbc->output[numsamples++] = audioout; + } + } + } + return (numsamples); +} + +static void +sbc_decode(struct bt_config *cfg) +{ + struct sbc_encode *sbc = cfg->handle.sbc_enc; + float delta[2][8]; + float levels[2][8]; + float audioout; + float *X; + float *V; + float left[160]; + float right[160]; + float U[160]; + float W[160]; + float S[8]; + int position; + int block; + int chan; + int sb; + int i; + int k; + + for (chan = 0; chan < sbc->channels; chan++) { + for (sb = 0; sb < sbc->bands; sb++) { + levels[chan][sb] = (1 << sbc->bits[chan][sb]) - 1; + delta[chan][sb] = (1 << sbc->scalefactor[chan][sb]); + } + } + + i = 0; + for (block = 0; block < sbc->blocks; block++) { + for (chan = 0; chan < sbc->channels; chan++) { + for (sb = 0; sb < sbc->bands; sb++) { + if (sbc->bits[chan][sb] == 0) { + audioout = 0; + } else { + audioout = + ((((sbc->output[i] * 2.0f) + 1.0f) * delta[chan][sb]) / + levels[chan][sb]) - delta[chan][sb]; + } + sbc->output[i++] = audioout; + } + } + } + + if (cfg->chmode == MODE_JOINT) { + i = 0; + while (i < (sbc->blocks * sbc->bands * sbc->channels)) { + for (sb = 0; sb < sbc->bands; sb++) { + if (sbc->join & (1 << (sbc->bands - sb - 1))) { + audioout = sbc->output[i]; + sbc->output[i] = (2.0f * sbc->output[i]) + + (2.0f * sbc->output[i + sbc->bands]); + sbc->output[i + sbc->bands] = + (2.0f * audioout) - + (2.0f * sbc->output[i + sbc->bands]); + sbc->output[i] /= 2.0f; + sbc->output[i + sbc->bands] /= 2.0f; + } + i++; + } + i += sbc->bands; + } + } + position = 0; + for (block = 0; block < sbc->blocks; block++) { + for (chan = 0; chan < sbc->channels; chan++) { + /* select right or left channel */ + if (chan == 0) { + X = left; + V = sbc->left; + } else { + X = right; + V = sbc->right; + } + for (i = 0; i < sbc->bands; i++) + S[i] = sbc->output[position++]; + + for (i = (sbc->bands * 20) - 1; i >= (sbc->bands * 2); i--) + V[i] = V[i - (sbc->bands * 2)]; + for (k = 0; k < sbc->bands * 2; k++) { + float vk = 0; + for (i = 0; i < sbc->bands; i++) { + if (sbc->bands == 8) { + vk += cosdecdata8[i][k] * S[i]; + } else { + vk += cosdecdata4[i][k] * S[i]; + } + } + V[k] = vk; + } + for (i = 0; i <= 4; i++) { + for (k = 0; k < sbc->bands; k++) { + U[(i * sbc->bands * 2) + k] = + V[(i * sbc->bands * 4) + k]; + U[(i * sbc->bands + * 2) + sbc->bands + k] = + V[(i * sbc->bands * 4) + + (sbc->bands * 3) + k]; + } + } + for (i = 0; i < sbc->bands * 10; i++) { + if (sbc->bands == 4) { + W[i] = U[i] * (sbc_coeffs4[i] * -4.0f); + } else if (sbc->bands == 8) { + W[i] = U[i] * (sbc_coeffs8[i] * -8.0f); + } else { + W[i] = 0; + } + } + + for (k = 0; k < sbc->bands; k++) { + unsigned int offset = k + (block * sbc->bands); + + X[offset] = 0; + for (i = 0; i < 10; i++) { + X[offset] += W[k + (i * sbc->bands)]; + } + + if (X[offset] > 32767.0) + X[offset] = 32767.0; + else if (X[offset] < -32767.0) + X[offset] = -32767.0; + } + } + } + + for (i = 0, k = 0; k != (sbc->blocks * sbc->bands); k++) { + sbc->music_data[i++] = left[k]; + if (sbc->channels == 2) + sbc->music_data[i++] = right[k]; + } +} + +size_t +sbc_encode_frame(struct bt_config *cfg) +{ + struct sbc_encode *sbc = cfg->handle.sbc_enc; + uint8_t config; + uint8_t block; + uint8_t chan; + uint8_t sb; + uint8_t j; + uint8_t i; + + config = (cfg->freq << 6) | (cfg->blocks << 4) | + (cfg->chmode << 2) | (cfg->allocm << 1) | cfg->bands; + + sbc_encode(cfg); + + /* set initial CRC */ + sbc->crc = 0x5e; + + /* reset data position and size */ + sbc->bitoffset = 0; + sbc->maxoffset = sizeof(sbc->data) * 8; + + sbc_store_bits_crc(sbc, 8, SYNCWORD); + sbc_store_bits_crc(sbc, 8, config); + sbc_store_bits_crc(sbc, 8, cfg->bitpool); + + /* skip 8-bit CRC */ + sbc->bitoffset += 8; + + if (cfg->chmode == MODE_JOINT) { + if (sbc->bands == 8) + sbc_store_bits_crc(sbc, 8, sbc->join); + else if (sbc->bands == 4) + sbc_store_bits_crc(sbc, 4, sbc->join); + } + for (i = 0; i < sbc->channels; i++) { + for (j = 0; j < sbc->bands; j++) + sbc_store_bits_crc(sbc, 4, sbc->scalefactor[i][j]); + } + + /* store 8-bit CRC */ + sbc->data[3] = (sbc->crc & 0xFF); + + i = 0; + for (block = 0; block < sbc->blocks; block++) { + for (chan = 0; chan < sbc->channels; chan++) { + for (sb = 0; sb < sbc->bands; sb++) { + if (sbc->bits[chan][sb] == 0) + continue; + + sbc_store_bits_crc(sbc, sbc->bits[chan][sb], sbc->output[i++]); + } + } + } + return ((sbc->bitoffset + 7) / 8); +} + +static uint32_t +sbc_load_bits_crc(struct sbc_encode *sbc, uint32_t numbits) +{ + uint32_t off = sbc->bitoffset; + uint32_t value = 0; + + while (numbits-- && off != sbc->maxoffset) { + if (sbc->rem_data_ptr[off / 8] & (1 << ((7 - off) & 7))) { + value |= (1 << numbits); + sbc->crc ^= 0x80; + } + sbc->crc *= 2; + if (sbc->crc & 0x100) + sbc->crc ^= 0x11d; /* CRC-8 polynomial */ + + off++; + } + sbc->bitoffset = off; + return (value); +} + +size_t +sbc_decode_frame(struct bt_config *cfg, int bits) +{ + struct sbc_encode *sbc = cfg->handle.sbc_enc; + uint8_t config; + uint8_t block; + uint8_t chan; + uint8_t sb; + uint8_t j; + uint8_t i; + + sbc->rem_off = 0; + sbc->rem_len = 0; + + config = (cfg->freq << 6) | (cfg->blocks << 4) | + (cfg->chmode << 2) | (cfg->allocm << 1) | cfg->bands; + + /* set initial CRC */ + sbc->crc = 0x5e; + + /* reset data position and size */ + sbc->bitoffset = 0; + sbc->maxoffset = bits; + + /* verify SBC header */ + if (sbc->maxoffset < (8 * 4)) + return (0); + if (sbc_load_bits_crc(sbc, 8) != SYNCWORD) + return (0); + if (sbc_load_bits_crc(sbc, 8) != config) + return (0); + cfg->bitpool = sbc_load_bits_crc(sbc, 8); + + (void)sbc_load_bits_crc(sbc, 8);/* CRC */ + + if (cfg->chmode == MODE_JOINT) { + if (sbc->bands == 8) + sbc->join = sbc_load_bits_crc(sbc, 8); + else if (sbc->bands == 4) + sbc->join = sbc_load_bits_crc(sbc, 4); + else + sbc->join = 0; + } else { + sbc->join = 0; + } + + for (i = 0; i < sbc->channels; i++) { + for (j = 0; j < sbc->bands; j++) + sbc->scalefactor[i][j] = sbc_load_bits_crc(sbc, 4); + } + + calc_bitneed(cfg); + + i = 0; + for (block = 0; block < sbc->blocks; block++) { + for (chan = 0; chan < sbc->channels; chan++) { + for (sb = 0; sb < sbc->bands; sb++) { + if (sbc->bits[chan][sb] == 0) { + i++; + continue; + } + sbc->output[i++] = + sbc_load_bits_crc(sbc, sbc->bits[chan][sb]); + } + } + } + + sbc_decode(cfg); + + sbc->rem_off = 0; + sbc->rem_len = sbc->blocks * sbc->channels * sbc->bands; + + return ((sbc->bitoffset + 7) / 8); +} |