Page MenuHomeFreeBSD
Differential D52308 Diff 161723 lib/virtual_oss/bt/sbc_encode.c

Changeset View

Standalone View

View Options

lib/virtual_oss/bt/sbc_encode.c

  • This file was added.
/*-
* 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);
}