/*

Copyright (c) 2012, Dmitry Grinberg (dmitrygr@gmail.com / http://dmitrygr.com)
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
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 COPYRIGHT HOLDERS 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 COPYRIGHT HOLDER 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 "sbcdec.h"
#include <linux/kernel.h>

#define QUALITY_MEDIUM  1   /* pretty good */
#define QUALITY_GREAT   2   /* as good as it will get without an FPU */

/* /config options begin */

#define QUALITY QUALITY_MEDIUM
/* set to cheat a bit with shifts (saves a divide per sample) */
#define SPEED_OVER_ACCURACY
/* iterator up to 180 use fastest type for your platform */
#define ITER   uint32_t

/* /config options end */

#define DEBUG_DECODING 0

#if QUALITY == QUALITY_MEDIUM

	#define CONST(x)       (x >> 16)
	#define SAMPLE_CVT(x)  (x)
	#define INSAMPLE       int16_t
	#define OUTSAMPLE      uint16_t
	#define FIXED          int16_t
	#define FIXED_S        int32_t
	#define OUT_CLIP_MAX   0x7FFF
	#define OUT_CLIP_MIN   -0x8000

	#define NUM_FRAC_BITS_PROTO 16
	#define NUM_FRAC_BITS_COS   14

#elif QUALITY == QUALITY_GREAT

	#define CONST(x)       (x)
	#define SAMPLE_CVT(x)  (x)
	#define INSAMPLE       int16_t
	#define OUTSAMPLE      uint16_t
	#define FIXED          int32_t
	#define FIXED_S        int64_t
	#define OUT_CLIP_MAX   0x7FFF
	#define OUT_CLIP_MIN   -0x8000

	#define NUM_FRAC_BITS_PROTO 32
	#define NUM_FRAC_BITS_COS   30

#else

	#error "You did not define SBC decoder synthesizer quality to use"

#endif

static FIXED gV[160];


static const FIXED proto_4_40[] =
{
	CONST(0x00000000), CONST(0x00FB7991), CONST(0x02CB3E8B), CONST(0x069FDC59),
	CONST(0x22B63DA5), CONST(0x4B583FE6), CONST(0xDD49C25B), CONST(0x069FDC59),
	CONST(0xFD34C175), CONST(0x00FB7991), CONST(0x002329CC), CONST(0x00FF11CA),
	CONST(0x053B7546), CONST(0x0191E578), CONST(0x31EAB920), CONST(0x4825E4A3),
	CONST(0xEC1F5E6D), CONST(0x083DDC80), CONST(0xFF3773A8), CONST(0x00B32807),
	CONST(0x0061C5A7), CONST(0x007A4737), CONST(0x07646684), CONST(0xF89F23A7),
	CONST(0x3F23948D), CONST(0x3F23948D), CONST(0xF89F23A7), CONST(0x07646684),
	CONST(0x007A4737), CONST(0x0061C5A7), CONST(0x00B32807), CONST(0xFF3773A8),
	CONST(0x083DDC80), CONST(0xEC1F5E6D), CONST(0x4825E4A3), CONST(0x31EAB920),
	CONST(0x0191E578), CONST(0x053B7546), CONST(0x00FF11CA), CONST(0x002329CC)
};

static const FIXED proto_8_80[] =
{
	CONST(0x00000000), CONST(0x0083D8D4), CONST(0x0172E691), CONST(0x034FD9E0),
	CONST(0x116860F5), CONST(0x259ED8EB), CONST(0xEE979F0B), CONST(0x034FD9E0),
	CONST(0xFE8D196F), CONST(0x0083D8D4), CONST(0x000A42E6), CONST(0x0089DE90),
	CONST(0x020E372C), CONST(0x02447D75), CONST(0x153E7D35), CONST(0x253844DE),
	CONST(0xF2625120), CONST(0x03EBE849), CONST(0xFF1ACF26), CONST(0x0074E5CF),
	CONST(0x00167EE3), CONST(0x0082B6EC), CONST(0x02AD6794), CONST(0x00BFA1FF),
	CONST(0x18FAB36D), CONST(0x24086BF5), CONST(0xF5FF2BF8), CONST(0x04270CA8),
	CONST(0xFF93E21B), CONST(0x0060C1E9), CONST(0x002458FC), CONST(0x0069F16C),
	CONST(0x03436717), CONST(0xFEBDD6E5), CONST(0x1C7762DF), CONST(0x221D9DE0),
	CONST(0xF950DCFC), CONST(0x0412523E), CONST(0xFFF44825), CONST(0x004AB4C5),
	CONST(0x0035FF13), CONST(0x003B1FA4), CONST(0x03C04499), CONST(0xFC4086B8),
	CONST(0x1F8E43F2), CONST(0x1F8E43F2), CONST(0xFC4086B8), CONST(0x03C04499),
	CONST(0x003B1FA4), CONST(0x0035FF13), CONST(0x004AB4C5), CONST(0xFFF44825),
	CONST(0x0412523E), CONST(0xF950DCFC), CONST(0x221D9DE0), CONST(0x1C7762DF),
	CONST(0xFEBDD6E5), CONST(0x03436717), CONST(0x0069F16C), CONST(0x002458FC),
	CONST(0x0060C1E9), CONST(0xFF93E21B), CONST(0x04270CA8), CONST(0xF5FF2BF8),
	CONST(0x24086BF5), CONST(0x18FAB36D), CONST(0x00BFA1FF), CONST(0x02AD6794),
	CONST(0x0082B6EC), CONST(0x00167EE3), CONST(0x0074E5CF), CONST(0xFF1ACF26),
	CONST(0x03EBE849), CONST(0xF2625120), CONST(0x253844DE), CONST(0x153E7D35),
	CONST(0x02447D75), CONST(0x020E372C), CONST(0x0089DE90), CONST(0x000A42E6)
};

static const FIXED costab_4[] =
{
	CONST(0x2D413CCD), CONST(0xD2BEC333), CONST(0xD2BEC333), CONST(0x2D413CCD),
	CONST(0x187DE2A7), CONST(0xC4DF2862), CONST(0x3B20D79E), CONST(0xE7821D59),
	CONST(0x00000000), CONST(0x00000000), CONST(0x00000000), CONST(0x00000000),
	CONST(0xE7821D59), CONST(0x3B20D79E), CONST(0xC4DF2862), CONST(0x187DE2A7),
	CONST(0xD2BEC333), CONST(0x2D413CCD), CONST(0x2D413CCD), CONST(0xD2BEC333),
	CONST(0xC4DF2862), CONST(0xE7821D59), CONST(0x187DE2A7), CONST(0x3B20D79E),
	CONST(0xC0000000), CONST(0xC0000000), CONST(0xC0000000), CONST(0xC0000000),
	CONST(0xC4DF2862), CONST(0xE7821D59), CONST(0x187DE2A7), CONST(0x3B20D79E)
};

static const FIXED costab_8[] =
{
	CONST(0x2D413CCD), CONST(0xD2BEC333), CONST(0xD2BEC333), CONST(0x2D413CCD),
	CONST(0x2D413CCD), CONST(0xD2BEC333), CONST(0xD2BEC333), CONST(0x2D413CCD),
	CONST(0x238E7673), CONST(0xC13AD060), CONST(0x0C7C5C1E), CONST(0x3536CC52),
	CONST(0xCAC933AE), CONST(0xF383A3E2), CONST(0x3EC52FA0), CONST(0xDC71898D),
	CONST(0x187DE2A7), CONST(0xC4DF2862), CONST(0x3B20D79E), CONST(0xE7821D59),
	CONST(0xE7821D59), CONST(0x3B20D79E), CONST(0xC4DF2862), CONST(0x187DE2A7),
	CONST(0x0C7C5C1E), CONST(0xDC71898D), CONST(0x3536CC52), CONST(0xC13AD060),
	CONST(0x3EC52FA0), CONST(0xCAC933AE), CONST(0x238E7673), CONST(0xF383A3E2),
	CONST(0x00000000), CONST(0x00000000), CONST(0x00000000), CONST(0x00000000),
	CONST(0x00000000), CONST(0x00000000), CONST(0x00000000), CONST(0x00000000),
	CONST(0xF383A3E2), CONST(0x238E7673), CONST(0xCAC933AE), CONST(0x3EC52FA0),
	CONST(0xC13AD060), CONST(0x3536CC52), CONST(0xDC71898D), CONST(0x0C7C5C1E),
	CONST(0xE7821D59), CONST(0x3B20D79E), CONST(0xC4DF2862), CONST(0x187DE2A7),
	CONST(0x187DE2A7), CONST(0xC4DF2862), CONST(0x3B20D79E), CONST(0xE7821D59),
	CONST(0xDC71898D), CONST(0x3EC52FA0), CONST(0xF383A3E2), CONST(0xCAC933AE),
	CONST(0x3536CC52), CONST(0x0C7C5C1E), CONST(0xC13AD060), CONST(0x238E7673),
	CONST(0xD2BEC333), CONST(0x2D413CCD), CONST(0x2D413CCD), CONST(0xD2BEC333),
	CONST(0xD2BEC333), CONST(0x2D413CCD), CONST(0x2D413CCD), CONST(0xD2BEC333),
	CONST(0xCAC933AE), CONST(0x0C7C5C1E), CONST(0x3EC52FA0), CONST(0x238E7673),
	CONST(0xDC71898D), CONST(0xC13AD060), CONST(0xF383A3E2), CONST(0x3536CC52),
	CONST(0xC4DF2862), CONST(0xE7821D59), CONST(0x187DE2A7), CONST(0x3B20D79E),
	CONST(0x3B20D79E), CONST(0x187DE2A7), CONST(0xE7821D59), CONST(0xC4DF2862),
	CONST(0xC13AD060), CONST(0xCAC933AE), CONST(0xDC71898D), CONST(0xF383A3E2),
	CONST(0x0C7C5C1E), CONST(0x238E7673), CONST(0x3536CC52), CONST(0x3EC52FA0),
	CONST(0xC0000000), CONST(0xC0000000), CONST(0xC0000000), CONST(0xC0000000),
	CONST(0xC0000000), CONST(0xC0000000), CONST(0xC0000000), CONST(0xC0000000),
	CONST(0xC13AD060), CONST(0xCAC933AE), CONST(0xDC71898D), CONST(0xF383A3E2),
	CONST(0x0C7C5C1E), CONST(0x238E7673), CONST(0x3536CC52), CONST(0x3EC52FA0),
	CONST(0xC4DF2862), CONST(0xE7821D59), CONST(0x187DE2A7), CONST(0x3B20D79E),
	CONST(0x3B20D79E), CONST(0x187DE2A7), CONST(0xE7821D59), CONST(0xC4DF2862),
	CONST(0xCAC933AE), CONST(0x0C7C5C1E), CONST(0x3EC52FA0), CONST(0x238E7673),
	CONST(0xDC71898D), CONST(0xC13AD060), CONST(0xF383A3E2), CONST(0x3536CC52)
};

static const int8_t loudness_4[4][4] =
{
	{ -1, 0, 0, 0 }, { -2, 0, 0, 1 },
	{ -2, 0, 0, 1 }, { -2, 0, 0, 1 }
};

static const int8_t loudness_8[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 void synth_4(OUTSAMPLE* dst, const INSAMPLE* src, FIXED* V){  /* A2DP figure 12.3 */

	ITER i, j;
	const FIXED* tabl = proto_4_40;
	const FIXED* costab = costab_4;

	/* shift */
	for(i = 79; i >= 8; i--) V[i] = V[i - 8];

	/* matrix */
	i = 8;
	do{
		FIXED_S t;
		t  = (FIXED_S)costab[0] * (FIXED_S)src[0];
		t += (FIXED_S)costab[1] * (FIXED_S)src[1];
		t += (FIXED_S)costab[2] * (FIXED_S)src[2];
		t += (FIXED_S)costab[3] * (FIXED_S)src[3];
		costab += 4;
		*V++ = t >> NUM_FRAC_BITS_COS;
	}while(--i);
	V -= 8;


	/* calculate audio samples */
	j = 4;
	do{

		OUTSAMPLE s;
		FIXED_S sample;
		sample  = (FIXED_S)V[  0] * (FIXED_S)tabl[0];
		sample += (FIXED_S)V[ 12] * (FIXED_S)tabl[1];
		sample += (FIXED_S)V[ 16] * (FIXED_S)tabl[2];
		sample += (FIXED_S)V[ 28] * (FIXED_S)tabl[3];
		sample += (FIXED_S)V[ 32] * (FIXED_S)tabl[4];
		sample += (FIXED_S)V[ 44] * (FIXED_S)tabl[5];
		sample += (FIXED_S)V[ 48] * (FIXED_S)tabl[6];
		sample += (FIXED_S)V[ 60] * (FIXED_S)tabl[7];
		sample += (FIXED_S)V[ 64] * (FIXED_S)tabl[8];
		sample += (FIXED_S)V[ 76] * (FIXED_S)tabl[9];
		tabl += 10;
		V++;

		sample >>= (NUM_FRAC_BITS_PROTO - 1 - 2); /* -2 is for the -4 we need to multiply by :) */
		sample = -sample;

		if(sample >= OUT_CLIP_MAX) sample = OUT_CLIP_MAX;
		if(sample <= OUT_CLIP_MIN) sample = OUT_CLIP_MIN;
		s = sample;

		*dst++ = s;

	}while(--j);
}

static void synth_8(OUTSAMPLE* dst, const INSAMPLE* src, FIXED* V){  /* A2DP figure 12.3 */

	ITER i, j;
	const FIXED* tabl = proto_8_80;
	const FIXED* costab = costab_8;

	/* shift */
	for(i = 159; i >= 16; i--) V[i] = V[i - 16];

	/* matrix */
	i = 16;
	do{
		FIXED_S t;
		t  = (FIXED_S)costab[0] * (FIXED_S)src[0];
		t += (FIXED_S)costab[1] * (FIXED_S)src[1];
		t += (FIXED_S)costab[2] * (FIXED_S)src[2];
		t += (FIXED_S)costab[3] * (FIXED_S)src[3];
		t += (FIXED_S)costab[4] * (FIXED_S)src[4];
		t += (FIXED_S)costab[5] * (FIXED_S)src[5];
		t += (FIXED_S)costab[6] * (FIXED_S)src[6];
		t += (FIXED_S)costab[7] * (FIXED_S)src[7];
		costab += 8;
		*V++ = t >> NUM_FRAC_BITS_COS;

	}while(--i);

	V -= 16;

	/* calculate audio samples */
	j = 8;
	do{

		OUTSAMPLE s;
		FIXED_S sample;

		sample  = (FIXED_S)V[  0] * (FIXED_S)tabl[0];
		sample += (FIXED_S)V[ 24] * (FIXED_S)tabl[1];
		sample += (FIXED_S)V[ 32] * (FIXED_S)tabl[2];
		sample += (FIXED_S)V[ 56] * (FIXED_S)tabl[3];
		sample += (FIXED_S)V[ 64] * (FIXED_S)tabl[4];
		sample += (FIXED_S)V[ 88] * (FIXED_S)tabl[5];
		sample += (FIXED_S)V[ 96] * (FIXED_S)tabl[6];
		sample += (FIXED_S)V[120] * (FIXED_S)tabl[7];
		sample += (FIXED_S)V[128] * (FIXED_S)tabl[8];
		sample += (FIXED_S)V[152] * (FIXED_S)tabl[9];
		tabl += 10;
		V++;

		sample >>= (NUM_FRAC_BITS_PROTO - 1 - 3); /* -3 is for the -8 we need to multiply by :) */
		sample = -sample;

		if(sample > OUT_CLIP_MAX) sample = OUT_CLIP_MAX;
		if(sample < OUT_CLIP_MIN) sample = OUT_CLIP_MIN;
		s = sample;

		*dst++ = s;

	}while(--j);
}

static void synth(OUTSAMPLE* dst, const INSAMPLE* src, uint8_t nBands, FIXED* V) {
	/* A2DP sigure 12.3  */

	if(nBands == 4) synth_4(dst, src, V);
	else synth_8(dst, src, V);
}

#ifdef SPEED_OVER_ACCURACY
	static inline int32_t mulshift(int32_t val, uint32_t bits) {
		/* return approximately  val / ((2^bits) - 1)  */

		static const uint32_t cooltable[] = {0, 0, 0x55555555, 0x24924925, 0x11111111, 0x08421084,
			0x04104104, 0x02040810, 0x01010101, 0x00804020, 0x00401004, 0x00200400,
			0x00100100, 0x00080040, 0x00040010, 0x00020004, 0x00010001};

		if(bits != 1) val = ((uint64_t)(uint32_t)val * (uint64_t)cooltable[bits]) >> 32;

		return val;
	}
#endif

void sbc_decoder_reset(void) {
	unsigned i;
	for(i = 0; i < sizeof(gV) / sizeof(*gV); i++) {
		gV[i] = 0;
	}
}

/**
 * We expect a mSBC header at the start of the buffer.
 */
uint32_t sbc_decode(uint8_t blocks_per_packet, uint8_t num_bits,
		    const uint8_t* buf, uint16_t len, int16_t* outPCM){

	/* convenience  */
	const uint8_t* end = buf + len;
	int16_t* outBufPtr = outPCM;
	#define left (end - buf)

	/* workspace */
	static INSAMPLE samples[16][8]; /*  We blow the stack if this is not static. */
	ITER i, j, k;
	uint32_t scaleFactors[8];
	int32_t bitneed[8];
	uint32_t bits[8];
	int32_t bitcount, slicecount, bitslice;
	uint8_t samplingRate, blocks, snr, numSubbands, bitpoolSz, bitpos = 0x80;
	int8_t max_bitneed = 0;
#ifndef SPEED_OVER_ACCURACY
	int32_t levels[8];
#endif

#if (DEBUG_DECODING == 1)
	const uint8_t *start_buf = buf;
	pr_info("%s: blocks_per_packet = %d, num_bits = %d, buf = %p, len = %d\n",
		__func__, blocks_per_packet, num_bits, buf, len);
	for (i = 0; i < len; i++) {
		pr_info("buf[%d] = 0x%02x\n", i, buf[i]);
	}
#endif

	/* look into the frame header */
	if(left < 2) goto out;      /* too short a frame header  */

	/*  use Bemote specific constants  */
	samplingRate = 0; /*  always 16000 Hz */
	blocks = blocks_per_packet;
	snr = 0;
	numSubbands = 8;
	bitpoolSz = num_bits;

	if (buf[0] != 0xAD) {
		pr_err("mSBC syncword not found\n");
		goto out;
	}
	/* skip header and process scale factors */
	buf += 4;

	/* read scale factors */
	/* pr_info("sbc_decode: read scale factors, numSubbands = %d\n", numSubbands); */
	for(i = 0; i < numSubbands; i++){

		if(bitpos == 0x80){

			scaleFactors[i] = (*buf) >> 4;
			bitpos = 0x08;
		}
		else{

			scaleFactors[i] = (*buf++) & 0x0F;
			bitpos = 0x80;
		}
	}

	/* calculate bitneed table and max_bitneed value (A2DP 12.6.3.1)  */
	if(snr){

		for(i = 0; i < numSubbands; i++){

			bitneed[i] = scaleFactors[i];
			if(bitneed[i] > max_bitneed) max_bitneed = bitneed[i];
		}
	}
	else{

		const signed char* tbl;

		if(numSubbands == 4) tbl = loudness_4[samplingRate];
		else tbl = loudness_8[samplingRate];

		for(i = 0; i < numSubbands; i++){

			if(scaleFactors[i]){

				int loudness = scaleFactors[i] - tbl[i];

				if(loudness > 0) loudness /= 2;
				bitneed[i] = loudness;
			}
			else bitneed[i] = -5;
			if(bitneed[i] > max_bitneed) max_bitneed = bitneed[i];
		}
	}

	/* fit bitslices into the bitpool */
	bitcount = 0;
	slicecount = 0;
	bitslice = max_bitneed + 1;
	/* pr_info("sbc_decode: fit bitslices into the bitpool, bitslice = %d\n", bitslice ); */
	do{
		bitslice--;
		bitcount += slicecount;
		slicecount = 0;
		for(i = 0; i < numSubbands; i++){

			if(bitneed[i] > bitslice + 1 && bitneed[i] < bitslice + 16) slicecount++;
			else if(bitneed[i] == bitslice + 1) slicecount += 2;
		}

	}while(bitcount + slicecount < bitpoolSz);

	/* distribute bits */
	for(i = 0; i < numSubbands; i++){

		if(bitneed[i] < bitslice + 2) bits[i] = 0;
		else{

			int8_t v = bitneed[i] - bitslice;
			if(v > 16) v = 16;
			bits[i] = v;
		}
	}

	/* allocate remaining bits */
	for(i = 0; i < numSubbands && bitcount < bitpoolSz; i++){

		if(bits[i] >= 2 && bits[i] < 16){

			bits[i]++;
			bitcount++;
		}
		else if(bitneed[i] == bitslice + 1 && bitpoolSz > bitcount + 1){

			bits[i] = 2;
			bitcount += 2;
		}
	}
	for(i = 0; i < numSubbands && bitcount < bitpoolSz; i++){

		if(bits[i] < 16){

			bits[i]++;
			bitcount++;
		}
	}

	/* reconstruct subband samples (A2DP 12.6.4) */
	#ifndef SPEED_OVER_ACCURACY
		for(i = 0; i < numSubbands; i++) levels[i] = (1 << bits[i]) - 1;
	#endif

	/* pr_info("sbc_decode: reconstruct subband samples, blocks = %d\n", blocks );  */
	for(j = 0; j < blocks; j++){

		for(i = 0; i < numSubbands; i++){

			if(bits[i]){

				uint32_t val = 0;
				k = bits[i];
				do{

					val <<= 1;
#if (DEBUG_DECODING == 1)
					pr_info("%s: buf = %p, offset %d\n",
						__func__, buf, buf-start_buf);
#endif
					if(*buf & bitpos) val++;
					if(!(bitpos >>= 1)){
						bitpos = 0x80;
						buf++;
					}
				}while(--k);

				val = (val << 1) | 1;
				val <<= scaleFactors[i];

				#ifdef SPEED_OVER_ACCURACY
					val = mulshift(val, bits[i]);
				#else
					val /= levels[i];
				#endif

				val -= (1 << scaleFactors[i]);

				samples[j][i] = SAMPLE_CVT(val);
			}
			else samples[j][i] = SAMPLE_CVT(0);
		}
	}

	/* synthesis  */
#if (DEBUG_DECODING == 1)
	pr_info("sbc_decode: synthesis, blocks = %d\n", blocks );
#endif
	for(j = 0; j < blocks; j++){
		synth((OUTSAMPLE*)outPCM, samples[j], numSubbands, gV);
		outPCM += numSubbands;
	}
	/* if we used a byte partially, skip the rest of it, it is "padding"  */
	if(bitpos != 0x80) buf++;
out:
#if (DEBUG_DECODING == 1)
	if(left < 0)
		pr_err("SBC: buffer over-read by %d bytes.\n", -left);
	if(left > 0)
		pr_err("SBC: buffer under-read by %d bytes.\n", left);
#endif

	return outPCM - outBufPtr;
}

uint32_t sbcDecGetNumSamples(const uint8_t bufFirstByte){

	uint8_t blocks = (bufFirstByte >> 4) & 3; /* see A2DP table 12.17  */
	uint8_t numSubbands = bufFirstByte & 1; /* see A2DP table 12.20  */

	numSubbands = numSubbands ? 8 : 4;
	blocks = (blocks + 1) << 2;

	return numSubbands * blocks;
}

uint32_t sbcDecGetPacketSize(const uint8_t bufFirstByte, uint8_t bufsecondByte){

	uint8_t blocks = (bufFirstByte >> 4) & 3; /* see A2DP table 12.17 */
	uint8_t numSubbands = bufFirstByte & 1; /* see A2DP table 12.20 */

	numSubbands = numSubbands ? 8 : 4;
	blocks = (blocks + 1) << 2;


	return 2 + /* header */
		numSubbands / 2 + /* bit allocations */
		(blocks * bufsecondByte + 7) / 8; /* data */
}