/*
 * Copyright (C) 2012 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#define ADK_INTERNAL
#include "fwk.h"
#include "btRFCOMM.h"
#include "btA2DP.h"
#include "btSDP.h"
#include "btL2CAP.h"
#include "dbg.h"
#include "Audio.h"
#include <string.h>
#include "coop.h"


#define UGLY_SCARY_DEBUGGING_CODE	0

//endpoint media types
	#define AVDTP_MEDIA_TYP_AUDIO		0
	#define AVDTP_MEDIA_TYP_VIDEO		1
	#define AVDTP_MEDIA_TYP_MULTIMEDIA	2

//andpoint directions
	#define AVDTP_DIR_SOURCE		0
	#define AVDTP_DIR_SINK			1


//A2DP packet header (2bytes)
	//byte 0
	#define AVDTP_HDR_MASK_TRANS	0xF0
	#define AVDTP_HDR_SHIFT_TRANS	4
	#define AVDTP_HDR_MASK_PKT_TYP	0x0C
	#define AVDTP_HDR_SHIFT_PKT_TYP	2
	#define AVDTP_HDR_MASK_MSG_TYP	0x03
	#define AVDTP_HDR_SHIFT_MSG_TYP	0
	//byte 1
	#define AVDTP_HDR_MASK_SIG_ID	0x3F
	#define AVDTP_HDR_SHIFT_SIG_ID	0

//sigId
	#define AVDTP_SIG_DISCOVER			0x01
	#define AVDTP_SIG_GET_CAPABILITIES		0x02
	#define AVDTP_SIG_SET_CONFIGURATION		0x03
	#define AVDTP_SIG_GET_CONFIGURATION		0x04
	#define AVDTP_SIG_RECONFIGURE			0x05
	#define AVDTP_SIG_OPEN				0x06
	#define AVDTP_SIG_START				0x07
	#define AVDTP_SIG_CLOSE				0x08
	#define AVDTP_SIG_SUSPEND			0x09
	#define AVDTP_SIG_ABORT				0x0A
	#define AVDTP_SIG_SECURITY_CONTROL		0x0B

//pktTyp
	#define AVDTP_PKT_TYP_SINGLE			0x00
	#define AVDTP_PKT_TYP_START			0x01
	#define AVDTP_PKT_TYP_CONTINUE			0x02
	#define AVDTP_PKT_TYP_END			0x03

//msgTyp
	#define AVDTP_MSG_TYP_CMD			0x00
	#define AVDTP_MSG_TYP_ACCEPT			0x02
	#define AVDTP_MSG_TYP_REJ			0x03

//seid info (2 bytes)
	//byte0
	#define AVDTP_SEID_NFO_MASK_SEID		0xFC
	#define AVDTP_SEID_NFO_SHIFT_SEID		2
	#define AVDTP_SEID_NFO_MASK_INUSE		0x02
	#define AVDTP_SEID_NFO_SHIFT_INUSE		1
	//byte1
	#define AVDTP_SEID_NFO_MASK_MEDIA_TYP		0xF0
	#define AVDTP_SEID_NFO_SHIFT_MEDIA_TYP		4
	#define AVDTP_SEID_NFO_MASK_TYP			0x08
	#define AVDTP_SEID_NFO_SHIFT_TYP		3


//service capability categoris
	#define AVDTP_SVC_CAT_MEDIA_TRANSPORT		1
	#define AVDTP_SVC_CAT_REPORTING			2
	#define AVDTP_SVC_CAT_RECOVERY			3
	#define AVDTP_SVC_CAT_CONTENT_PROTECTION	4
	#define AVDTP_SVC_CAT_HEADER_COMPRESSION	5
	#define AVDTP_SVC_CAT_MULTIPLEXING		6
	#define AVDTP_SVC_CAT_MEDIA_CODEC		7

//codec types
	#define A2DP_CODEC_TYP_SBC			0
	//others exist...done don't implement them

#define MY_ENDPT_ID					1		//we support just one, and this is it's ID

#define A2DP_CHAN_MODE_MONO				0
#define A2DP_CHAN_MODE_DUAL_CHANNEL			1
#define A2DP_CHAN_MODE_STEREO				2
#define A2DP_CHAN_MODE_JOINT_STEREO			3


#define DATA_STATE_ENCODE(state)			((void*)(((uint32_t)(state)) ^ 0x80000000))
#define IS_DATA_STATE(state)				(!!(((uint32_t)(state)) & 0x80000000))

typedef struct A2DPstate{

    //L2CAP data
    uint16_t aclConn;
    uint16_t remChan; //remote channel num for control channel
    uint16_t datChan; //remote channel num for data channel

    //codec data
    uint16_t samplingRate;

    //our state
    char needAudioConn;

    //link
    struct A2DPstate* next;

}A2DPstate;

static A2DPstate* conns = NULL;

//copyright notice for SBC decoder
/*
Copyright (c) 2011, Dmitry Grinberg (as published on http://dmitrygr.com)
All rights reserved.

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

* Redistributions of source code must retain the above copyright notice, this
   list of conditions and the following disclaimer. 

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 OWNER 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.
*/


#define QUALITY_LOWEST	1	//you may notice the quality reduction
#define QUALITY_MEDIUM	2	//pretty good
#define QUALITY_GREAT	3	//as good as it will get without an FPU


///config options begin

/*
	This is a rather clever little SBC decoder that I've put together 

*/

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

///config options end


#if QUALITY == QUALITY_LOWEST

	#define CONST(x)		(x >> 24)
	#define SAMPLE_CVT(x)		(x >> 8)
	#define INSAMPLE		int8_t
	#define OUTSAMPLE		uint8_t	//no point producing 16-bit samples using the 8-bit decoder
	#define FIXED			int8_t
	#define FIXED_S			int16_t
	#define OUT_CLIP_MAX		0x7F
	#define OUT_CLIP_MIN		-0x80

	#define NUM_FRAC_BITS_PROTO	8
	#define NUM_FRAC_BITS_COS	6

#elif 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 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 }
};


#if QUALITY != QUALITY_MEDIUM  //for medium we provide nice assembly routines

    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
        for(i = 0; i < 8; i++){

            FIXED_S t =    (FIXED_S)costab[0] * (FIXED_S)src[0] +
                           (FIXED_S)costab[1] * (FIXED_S)src[1] +
                           (FIXED_S)costab[2] * (FIXED_S)src[2] +
                           (FIXED_S)costab[3] * (FIXED_S)src[3];
            costab += 4;
            V[i] = t >> NUM_FRAC_BITS_COS;
        }

        //calculate audio samples
        for(j = 0; j < 4; j++){

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

            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;
            else if(sample < OUT_CLIP_MIN) sample = OUT_CLIP_MIN;
            s = sample;

            #ifndef DESKTOP
                s += 0x8000;
                s += 8;
                s >>= 4;
            #endif

            dst[j] = s;
        }
    }

    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
        for(i = 0; i < 16; i++){

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

        //calculate audio samples
        for(j = 0; j < 8; j++){

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

            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;
            else if(sample < OUT_CLIP_MIN) sample = OUT_CLIP_MIN;
            s = sample;

            #ifndef DESKTOP
                s += 0x8000;
                s += 8;
                s >>= 4;
            #endif

            dst[j] = s;
        }
    }

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

        //efficient SBC synth by Dmitry Grinberg (as published May 26, 2012)
        if(nBands == 4) synth_4(dst, src, V);
        else synth_8(dst, src, V);
    }

#else

    static void __attribute__((naked)) synth_4_med(OUTSAMPLE* dst, const INSAMPLE* src, FIXED* V, const FIXED* costabl, const FIXED* prot_Tabl){

        asm(

        //prologue {8 cycles}
            "push {r4-r7}			\n\t"

        //shifting { 9x(9+9 + 1) + 1 = 171 cycles }
            "add   r2, #128			\n\t"
            "ldmia r2!, {r4-r7}		\n\t"
            "stmia r2, {r4-r7}		\n\t"
            "subs  r2, #32			\n\t"
            "ldmia r2!, {r4-r7}		\n\t"
            "stmia r2, {r4-r7}		\n\t"
            "subs  r2, #32			\n\t"
            "ldmia r2!, {r4-r7}		\n\t"
            "stmia r2, {r4-r7}		\n\t"
            "subs  r2, #32			\n\t"
            "ldmia r2!, {r4-r7}		\n\t"
            "stmia r2, {r4-r7}		\n\t"
            "subs  r2, #32			\n\t"
            "ldmia r2!, {r4-r7}		\n\t"
            "stmia r2, {r4-r7}		\n\t"
            "subs  r2, #32			\n\t"
            "ldmia r2!, {r4-r7}		\n\t"
            "stmia r2, {r4-r7}		\n\t"
            "subs  r2, #32			\n\t"
            "ldmia r2!, {r4-r7}		\n\t"
            "stmia r2, {r4-r7}		\n\t"
            "subs  r2, #32			\n\t"
            "ldmia r2!, {r4-r7}		\n\t"
            "stmia r2, {r4-r7}		\n\t"
            "subs  r2, #32			\n\t"
            "ldmia r2!, {r4-r7}		\n\t"
            "stmia r2, {r4-r7}		\n\t"
            "subs  r2, #16			\n\t"	//r2 is right back where it started

        //matrixing
            "movs  r7, #8			\n\t"	//r7 is loop index
        "matrix_loop_4:			\n\t"
            "ldrsh r4, [r3, #0]		\n\t"
            "ldrsh r5, [r1, #0]		\n\t"
            "mul   r6, r4, r5		\n\t"
            "ldrsh r4, [r3, #2]		\n\t"
            "ldrsh r5, [r1, #2]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #4]		\n\t"
            "ldrsh r5, [r1, #4]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #6]		\n\t"
            "ldrsh r5, [r1, #6]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "adds  r3, #8			\n\t"
            "asrs  r6, #14	 		\n\t"
            "strh  r6, [r2], #2		\n\t"
            "subs  r7, #1			\n\t"
            "bne   matrix_loop_4		\n\t"
        //return r2 to point where it should
            "subs  r2, #16			\n\t"

        //sample production
            "movs  r7, #4			\n\t"	//r7 is loop index
            "ldr   r3, [sp, #16]		\n\t"	//r3 is now proto table
        "sample_loop_4:			\n\t"
            "ldrsh r4, [r3, #0]		\n\t"
            "ldrsh r5, [r2, #0]		\n\t"
            "mul   r6, r4, r5		\n\t"
            "ldrsh r4, [r3, #2]		\n\t"
            "ldrsh r5, [r2, #24]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #4]		\n\t"
            "ldrsh r5, [r2, #32]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #6]		\n\t"
            "ldrsh r5, [r2, #56]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #8]		\n\t"
            "ldrsh r5, [r2, #64]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #10]		\n\t"
            "ldrsh r5, [r2, #88]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #12]		\n\t"
            "ldrsh r5, [r2, #96]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #14]		\n\t"
            "ldrsh r5, [r2, #120]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #16]		\n\t"
            "ldrsh r5, [r2, #128]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #18]		\n\t"
            "ldrsh r5, [r2, #152]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "adds  r3, #20			\n\t"
            "adds  r2, #2			\n\t"
            "negs  r6, r6			\n\t"
            "ssat  r6, #12, r6, asr #16	\n\t"
            "add   r6, #0x0800		\n\t"
            "strh  r6, [r0], #2		\n\t"
            "subs  r7, #1			\n\t"
            "bne   sample_loop_4		\n\t"

        //cleanup
            "pop   {r4-r7}			\n\t"
            "bx    lr			\n\t"
        );
    }

    static void __attribute__((naked)) synth_8_med(OUTSAMPLE* dst, const INSAMPLE* src, FIXED* V, const FIXED* costabl, const FIXED* prot_Tabl){

        asm(

        //prologue {8 cycles}
            "push {r4-r10}			\n\t"

        //shifting { 9x(9+9 + 1) + 1 = 171 cycles }
            "add   r2, #256			\n\t"
            "ldmia r2!, {r4-r10, r12}	\n\t"
            "stmia r2, {r4-r10, r12}	\n\t"
            "subs  r2, #64			\n\t"
            "ldmia r2!, {r4-r10, r12}	\n\t"
            "stmia r2, {r4-r10, r12}	\n\t"
            "subs  r2, #64			\n\t"
            "ldmia r2!, {r4-r10, r12}	\n\t"
            "stmia r2, {r4-r10, r12}	\n\t"
            "subs  r2, #64			\n\t"
            "ldmia r2!, {r4-r10, r12}	\n\t"
            "stmia r2, {r4-r10, r12}	\n\t"
            "subs  r2, #64			\n\t"
            "ldmia r2!, {r4-r10, r12}	\n\t"
            "stmia r2, {r4-r10, r12}	\n\t"
            "subs  r2, #64			\n\t"
            "ldmia r2!, {r4-r10, r12}	\n\t"
            "stmia r2, {r4-r10, r12}	\n\t"
            "subs  r2, #64			\n\t"
            "ldmia r2!, {r4-r10, r12}	\n\t"
            "stmia r2, {r4-r10, r12}	\n\t"
            "subs  r2, #64			\n\t"
            "ldmia r2!, {r4-r10, r12}	\n\t"
            "stmia r2, {r4-r10, r12}	\n\t"
            "subs  r2, #64			\n\t"
            "ldmia r2!, {r4-r10, r12}	\n\t"
            "stmia r2, {r4-r10, r12}	\n\t"
            "subs  r2, #32			\n\t"	//r2 is right back where it started

        //matrixing
            "movs  r7, #16			\n\t"	//r7 is loop index
        "matrix_loop:			\n\t"
            "ldrsh r4, [r3, #0]		\n\t"
            "ldrsh r5, [r1, #0]		\n\t"
            "mul   r6, r4, r5		\n\t"
            "ldrsh r4, [r3, #2]		\n\t"
            "ldrsh r5, [r1, #2]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #4]		\n\t"
            "ldrsh r5, [r1, #4]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #6]		\n\t"
            "ldrsh r5, [r1, #6]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #8]		\n\t"
            "ldrsh r5, [r1, #8]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #10]		\n\t"
            "ldrsh r5, [r1, #10]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #12]		\n\t"
            "ldrsh r5, [r1, #12]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #14]		\n\t"
            "ldrsh r5, [r1, #14]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "adds  r3, #16			\n\t"
            "asrs  r6, #14	 		\n\t"
            "strh  r6, [r2], #2		\n\t"
            "subs  r7, #1			\n\t"
            "bne   matrix_loop		\n\t"
        //return r2 to point where it should
            "subs  r2, #32			\n\t"

        //sample production
            "movs  r7, #8			\n\t"	//r7 is loop index
            "ldr   r3, [sp, #28]		\n\t"	//r3 is now proto table
        "sample_loop:			\n\t"
            "ldrsh r4, [r3, #0]		\n\t"
            "ldrsh r5, [r2, #0]		\n\t"
            "mul   r6, r4, r5		\n\t"
            "ldrsh r4, [r3, #2]		\n\t"
            "ldrsh r5, [r2, #48]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #4]		\n\t"
            "ldrsh r5, [r2, #64]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #6]		\n\t"
            "ldrsh r5, [r2, #112]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #8]		\n\t"
            "ldrsh r5, [r2, #128]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #10]		\n\t"
            "ldrsh r5, [r2, #176]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #12]		\n\t"
            "ldrsh r5, [r2, #192]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #14]		\n\t"
            "ldrsh r5, [r2, #240]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #16]		\n\t"
            "ldrsh r5, [r2, #256]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "ldrsh r4, [r3, #18]		\n\t"
            "ldrsh r5, [r2, #304]		\n\t"
            "mla   r6, r4, r5, r6		\n\t"
            "adds  r3, #20			\n\t"
            "adds  r2, #2			\n\t"
            "negs  r6, r6			\n\t"
            "ssat  r6, #12, r6, asr #16	\n\t"
            "add   r6, #0x0800		\n\t"
            "strh  r6, [r0], #2		\n\t"
            "subs  r7, #1			\n\t"
            "bne   sample_loop		\n\t"

        //cleanup
            "pop   {r4-r10}			\n\t"
            "bx    lr			\n\t"
        );
    }

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

        //efficient SBC synth by Dmitry Grinberg (as published May 26, 2012)
        if(nBands == 4) synth_4_med(dst, src, V, costab_4, proto_4_40);
        else synth_8_med(dst, src, V, costab_8, proto_8_80);
    }

#endif

static void a2dpSbcPlay(const uint8_t* buf, uint16_t len){  //only supports mono (for simplicity)

    //volume
    int32_t vol = (uint32_t)getVolume();

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

    //workspace
    ITER i, j, k, ch, numChan;
    uint8_t scaleFactors[2][8];
    int8_t bitneed[2][8];
    uint8_t bits[2][8], join = 0;
    INSAMPLE samples[16][2][8];

    //audio data
    #define bufNumBuffers	3	//if less than 3, we're being suboptimal in speed.
    #define bufNumSamples	1536	//FYI: 128 samples is the maximum number of samples a single packet may have. MUST be a multiple of 8!
    static OUTSAMPLE audio[bufNumBuffers][bufNumSamples];
    static uint8_t whichBuf = 0;
    static uint16_t bufSamples = 0;

    //process the packet header
    if(left < 12) return;	//too short of a packet header
    uint8_t ver = (*buf) >> 6;
    char pad = ((*buf) >> 5) & 1;
    char ext = ((*buf) >> 4) & 1;
    uint8_t cc = (*buf++) & 0x0F;
    char mark = (*buf) >> 7;
    uint8_t pt = (*buf++) & 0x7F;
    uint16_t seq = (((uint16_t)buf[0]) << 8) | buf[1];
    uint32_t time = (((uint32_t)buf[2]) << 24) | (((uint32_t)buf[3]) << 16) | (((uint32_t)buf[4]) << 8) | buf[5];
    uint32_t src = (((uint32_t)buf[6]) << 24) | (((uint32_t)buf[7]) << 16) | (((uint32_t)buf[8]) << 8) | buf[9];
    buf += 10;

    //process the count
    if(left < 1) return;		//too short of a count
    uint8_t numFrames = *buf++;

    //process packets
    while(numFrames--){

        //process frame header
        if(left < 4) break;		//too short a frame header
        uint8_t sync = *buf++;
        uint8_t samplingRate = (*buf) >> 6;	//see A2DP table 12.16
        uint8_t blocks = ((*buf) >> 4) & 3;	//see A2DP table 12.17
        uint8_t chanMode = ((*buf) >> 2) & 3;	//see A2DP table 12.18
        uint8_t snr = ((*buf) >> 1) & 1;	//see A2DP table 12.19
        uint8_t numSubbands = (*buf++) & 1;	//see A2DP table 12.20
        uint8_t bitpoolSz = *buf++;
        uint8_t hdrCRC = *buf++;
        uint8_t bitpos = 0x80;

        numChan = (chanMode == A2DP_CHAN_MODE_MONO) ? 1 : 2;

        //process some numbers based on the tables
        numSubbands = numSubbands ? 8 : 4;
        blocks = (blocks + 1) << 2;

        //read "join" table if expected
        if(chanMode == A2DP_CHAN_MODE_JOINT_STEREO){
            join = *buf;	//we use it as a bitfield starting at top bit. 
            join >>= (8 - (numSubbands - 1));
            join <<= (8 - (numSubbands - 1));
            if(numSubbands == 8) buf++;
            else bitpos = 0x08;
        }

        //read scale factors
        for(ch = 0; ch < numChan; ch++) for(i = 0; i < numSubbands; i++){

            if(bitpos == 0x80){

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

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

        //calculate bitneed table and max_bitneed value (A2DP 12.6.3.1)
        int8_t max_bitneed[2] = {0, 0};
        for(ch = 0; ch < numChan; ch++){
            if(snr){

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

                    bitneed[ch][i] = scaleFactors[ch][i];
                    if(bitneed[ch][i] > max_bitneed[ch]) max_bitneed[ch] = bitneed[ch][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[ch][i]){

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

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

        if(chanMode == A2DP_CHAN_MODE_MONO || chanMode == A2DP_CHAN_MODE_DUAL_CHANNEL){
            for(ch = 0; ch < numChan; ch++){
                //fit bitslices into the bitpool
                int32_t bitcount = 0, slicecount = 0, bitslice = max_bitneed[ch] + 1;
                do{
                    bitslice--;
                    bitcount += slicecount;
                    slicecount = 0;
                    for(i = 0; i < numSubbands; i++){

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

                }while(bitcount + slicecount < bitpoolSz);

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

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

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

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

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

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

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

                    if(bits[ch][i] < 16){

                        bits[ch][i]++;
                        bitcount++;
                    }
                }
            }
        }
        else{
            //calculate max_bitneed value (A2DP 12.6.3.2)
            uint8_t max_bitneed_val = max_bitneed[0] > max_bitneed[1] ? max_bitneed[0] : max_bitneed[1];

            //fit bitslices into the bitpool
            int32_t bitcount = 0, slicecount = 0, bitslice = max_bitneed_val + 1;
            do{
                bitslice--;
                bitcount += slicecount;
                slicecount = 0;
                for(ch = 0; ch < 2; ch++) for(i = 0; i < numSubbands; i++){

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

            }while(bitcount + slicecount < bitpoolSz);

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

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

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

            //allocate remaining bits
            i = 0;
            ch = 0;
            while(i < numSubbands && bitcount < bitpoolSz){

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

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

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

                if(bits[ch][i] < 16){

                    bits[ch][i]++;
                    bitcount++;
                }
                if(++ch == 2){
                    ch = 0;
                    i++;
                }
            }
        }

        //reconstruct subband samples (A2DP 12.6.4)
        #ifndef SPEED_OVER_ACCURACY
            int32_t levels[2][8];
            for(ch = 0; ch < numChan; ch++) for(i = 0; i < numSubbands; i++) levels[ch][i] = (1 << bits[ch][i]) - 1;
        #endif
        for(j = 0; j < blocks; j++){

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

                if(bits[ch][i]){

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

                        val <<= 1;
                        if(*buf & bitpos) val++;
                        if(!(bitpos >>= 1)){
                            bitpos = 0x80;
                            buf++;
                        }
                    }while(--k);

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

                    #ifdef SPEED_OVER_ACCURACY
                        val >>= bits[ch][i];
                    #else
                        val /= levels[ch][i];
                    #endif

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

                    samples[j][ch][i] = SAMPLE_CVT(val);

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

            //joint processing (if needed - if not needed join will be 0)
            ITER bf = join;
            for(i = 0; i < numSubbands; i++, bf <<= 1) if(bf & 0x80){

                INSAMPLE t = samples[j][0][i];

                samples[j][0][i] += samples[j][1][i];
                samples[j][1][i] = t - samples[j][1][i];
            }
        }

        //synthesis
        static FIXED V[160] = {0, };
        for(j = 0; j < blocks; j++){

            //do the actual synthesis
            synth(audio[whichBuf] + bufSamples, samples[j][0], numSubbands, V);
            for(k = 0; k < numSubbands; k++, bufSamples++) audio[whichBuf][bufSamples] = (((int32_t)audio[whichBuf][bufSamples]) * vol) >> 8;

            //if buffer is full, enqueue it
            if(bufSamples == bufNumSamples){

                audioAddBuffer(AUDIO_BT, audio[whichBuf++], bufSamples);
                
                if(whichBuf == bufNumBuffers) whichBuf = 0;
                bufSamples = 0;
            }
        }
        //if we used a byte partially, skip the rest of it, it is "padding"
        if(bitpos != 0x80) buf++;
        if(left < 0) dbgPrintf("A2DP: buffer over-read: %d\n", left);
    }
}

static void a2dpServiceDataRx(void* service, const uint8_t* req, uint16_t reqSz){

    A2DPstate* state = service;
    uint8_t trans, pktTyp, msgTyp, sigId, eid;
    uint8_t buf[16];
    uint16_t replSz = 0;

    if(IS_DATA_STATE(service)){

        if(DATA_STATE_ENCODE(service) == conns) a2dpSbcPlay(req, reqSz);	//only latest plays data
        return;
    }

    if(reqSz < 2){

        dbgPrintf("A2DP: packet too small\n");
        return;
    }

    trans = (req[0] & AVDTP_HDR_MASK_TRANS) >> AVDTP_HDR_SHIFT_TRANS;
    pktTyp = (req[0] & AVDTP_HDR_MASK_PKT_TYP) >> AVDTP_HDR_SHIFT_PKT_TYP;
    msgTyp = (req[0] & AVDTP_HDR_MASK_MSG_TYP) >> AVDTP_HDR_SHIFT_MSG_TYP;
    sigId = (req[1] & AVDTP_HDR_MASK_SIG_ID) >> AVDTP_HDR_SHIFT_SIG_ID;
    reqSz -= 2;
    req += 2;

    #if UGLY_SCARY_DEBUGGING_CODE
        dbgPrintf("A2DP (%x) data(trans %d, pktTyp %d msgTyp %d sigId %d %ub)\n", state->remChan, trans, pktTyp, msgTyp, sigId, reqSz);
    #endif

    switch(sigId){

        case AVDTP_SIG_DISCOVER:

            if(reqSz) dbgPrintf("A2DP: unexpected further data in AVDTP_SIG_DISCOVER\n");
            //packet header
            buf[0] = (trans << AVDTP_HDR_SHIFT_TRANS) | (AVDTP_PKT_TYP_SINGLE << AVDTP_HDR_SHIFT_PKT_TYP) | (AVDTP_MSG_TYP_ACCEPT << AVDTP_HDR_SHIFT_MSG_TYP);
            buf[1] = (sigId << AVDTP_HDR_SHIFT_SIG_ID);
            //return a single SEID info structure
            buf[2] = (MY_ENDPT_ID << AVDTP_SEID_NFO_SHIFT_SEID) | (0 << AVDTP_SEID_NFO_MASK_INUSE);
            buf[3] = (AVDTP_MEDIA_TYP_AUDIO << AVDTP_SEID_NFO_SHIFT_MEDIA_TYP) | (AVDTP_DIR_SINK << AVDTP_SEID_NFO_SHIFT_TYP);
            replSz = 4;
            break;

        case AVDTP_SIG_GET_CAPABILITIES:

            if(reqSz > 1) dbgPrintf("A2DP: unexpected further data in AVDTP_SIG_GET_CAPABILITIES\n");
            if(reqSz < 1){
                dbgPrintf("A2DP: not enough data in AVDTP_SIG_GET_CAPABILITIES\n");
                break;
            }
            eid = (*req++) >> 2;
            if(eid != MY_ENDPT_ID){
                dbgPrintf("A2DP: bad EID (%d) AVDTP_SIG_GET_CAPABILITIES\n", eid);
                break;
            }
            //packet header
            buf[0] = (trans << AVDTP_HDR_SHIFT_TRANS) | (AVDTP_PKT_TYP_SINGLE << AVDTP_HDR_SHIFT_PKT_TYP) | (AVDTP_MSG_TYP_ACCEPT << AVDTP_HDR_SHIFT_MSG_TYP);
            buf[1] = (sigId << AVDTP_HDR_SHIFT_SIG_ID);
            //capability 0 - media transport
            buf[2] = AVDTP_SVC_CAT_MEDIA_TRANSPORT;
            buf[3] = 0;
            //capability 1 - SDB codec (and its info)
            buf[4] = AVDTP_SVC_CAT_MEDIA_CODEC;
            buf[5] = 6; //generic data is 2 bytes, SBC data is 4 bytes (see A2DP spec section 4.3.2)
            buf[6] = (AVDTP_MEDIA_TYP_AUDIO << 4);
            buf[7] = A2DP_CODEC_TYP_SBC;
            buf[8] = 0xFF; //spec forces us to support all of these (even though ANDROID and LINUX both do not properly support mono [try it :)  ])
            buf[9] = 0xFF; //spec forces us to support all of these
            buf[10] = 0x02; //min allowed by spec
            buf[11] = 0x50; //nice high value
            replSz = 12;
            break;

        case AVDTP_SIG_SET_CONFIGURATION:

            if(reqSz < 8){ //no valid command is shorter
                dbgPrintf("A2DP: not enough data in AVDTP_SIG_SET_CONFIGURATION\n");
                break;
            }
            eid = (*req++) >> 2;
            if(eid != MY_ENDPT_ID){
                dbgPrintf("A2DP: bad EID (%d) AVDTP_SIG_GET_CAPABILITIES\n", eid);
                break;
            }
            req++; //skip INT EID - we don't care
            reqSz -= 2;
            while(reqSz >= 2){

                uint8_t cap = *req++;
                uint8_t len = *req++;
                reqSz -= 2;

                if(len > reqSz) break;
                reqSz -= len;

                if(cap == AVDTP_SVC_CAT_MEDIA_CODEC){

                    if(*req++ != (AVDTP_MEDIA_TYP_AUDIO << 4)){
                        dbgPrintf("A2DP: requested data is not audio\n");
                        break;
                    }
                    else if(*req++ != A2DP_CODEC_TYP_SBC){
                        dbgPrintf("A2DP: requested codec is not SBC\n");
                        break;
                    }
                    else{

                        uint8_t samplingRate = (*req) >> 4;
                        uint8_t channelMode = (*req++) & 0x0F;
                        uint8_t blockLen = (*req) >> 4;
                        uint8_t subbands = (*req >> 2) & 3;
                        uint8_t allocMethod = (*req++) & 3;
                        uint8_t minBitpool = *req++;
                        uint8_t maxBitpool = *req++;

                        switch(samplingRate){
                            case 1: state->samplingRate = 48000; break;
                            case 2: state->samplingRate = 44100; break;
                            case 4: state->samplingRate = 32000; break;
                            case 8: state->samplingRate = 16000; break;
                            default:
                                dbgPrintf("A2DP: invalid sampling rate: %d\n", samplingRate);
                                goto out;
                        }
                        switch(blockLen){
                            case 1: blockLen = 16; break;
                            case 2: blockLen = 12; break;
                            case 4: blockLen = 8; break;
                            case 8: blockLen = 4; break;
                            default:
                                dbgPrintf("A2DP: invalid block len: %d\n", blockLen);
                                goto out;
                        }
                        switch(subbands){
                            case 1: subbands = 8; break;
                            case 2: subbands = 4; break;
                            default:
                                dbgPrintf("A2DP: invalid num subbands: %d\n", subbands);
                                goto out;
                        }
                        switch(allocMethod){
                            case 1: allocMethod = 0; break;
                            case 2: allocMethod = 1; break;
                            default:
                                dbgPrintf("A2DP: invalid allocMethod: %d\n", allocMethod);
                                goto out;
                        }
                        static const char* strMode[] = {NULL, "Joint Stereo", "Stereo", NULL, "Dual Channel", NULL, NULL, NULL, "Mono"};
                        dbgPrintf("A2DP:\n\t* %u %s-framed frames / block\n\t* %u subbands/frame\n\t* %u KHz sampling rate\n\t* '%s' channel mode\n\t* %u-%u bits per piece per channel\n",
                            blockLen, allocMethod ? "SNR" : "Loudness", subbands,
                            state->samplingRate, strMode[channelMode], minBitpool, maxBitpool);
                    }
                }
                else req += len;
            }
            if(reqSz){ //no valid command is shorter
                dbgPrintf("A2DP: malformed config info in AVDTP_SIG_SET_CONFIGURATION\n");
            }
            else{
                //prepare reply
                buf[0] = (trans << AVDTP_HDR_SHIFT_TRANS) | (AVDTP_PKT_TYP_SINGLE << AVDTP_HDR_SHIFT_PKT_TYP) | (AVDTP_MSG_TYP_ACCEPT << AVDTP_HDR_SHIFT_MSG_TYP);
                buf[1] = (sigId << AVDTP_HDR_SHIFT_SIG_ID);
                replSz = 2;
                #if UGLY_SCARY_DEBUGGING_CODE
                    dbgPrintf("A2DP: %u samp/sec, %db block w/%d subbands in %s mode with pool sizes %d-%d\n",
                        state->samplingRate, state->blockLen, state->subbands, state->allocTypeSNR ? "SNR" : "Loudness",
                        state->minBitpool, state->maxBitpool);
                #endif
            }
            break;

        case AVDTP_SIG_OPEN:

            if(reqSz > 1) dbgPrintf("A2DP: unexpected further data in AVDTP_SIG_OPEN\n");
            if(reqSz < 1){
                dbgPrintf("A2DP: not enough data in AVDTP_SIG_OPEN\n");
                break;
            }
            eid = (*req++) >> 2;
            if(eid != MY_ENDPT_ID){
                dbgPrintf("A2DP: bad EID (%d) AVDTP_SIG_OPEN\n", eid);
                break;
            }
            state->needAudioConn = 1;
            //packet header
            buf[0] = (trans << AVDTP_HDR_SHIFT_TRANS) | (AVDTP_PKT_TYP_SINGLE << AVDTP_HDR_SHIFT_PKT_TYP) | (AVDTP_MSG_TYP_ACCEPT << AVDTP_HDR_SHIFT_MSG_TYP);
            buf[1] = (sigId << AVDTP_HDR_SHIFT_SIG_ID);
            replSz = 2;
            break;

        case AVDTP_SIG_START:

            if(reqSz > 1) dbgPrintf("A2DP: unexpected further data in AVDTP_SIG_OPEN\n");
            if(reqSz < 1){
                dbgPrintf("A2DP: not enough data in AVDTP_SIG_OPEN\n");
                break;
            }
            eid = (*req++) >> 2;
            if(eid != MY_ENDPT_ID){
                dbgPrintf("A2DP: bad EID (%d) AVDTP_SIG_OPEN\n", eid);
                break;
            }
            audioOn(AUDIO_BT, state->samplingRate);
            //packet header
            buf[0] = (trans << AVDTP_HDR_SHIFT_TRANS) | (AVDTP_PKT_TYP_SINGLE << AVDTP_HDR_SHIFT_PKT_TYP) | (AVDTP_MSG_TYP_ACCEPT << AVDTP_HDR_SHIFT_MSG_TYP);
            buf[1] = (sigId << AVDTP_HDR_SHIFT_SIG_ID);
            replSz = 2;
            break;

        case AVDTP_SIG_CLOSE:
        case AVDTP_SIG_SUSPEND:
        case AVDTP_SIG_ABORT:

            if(reqSz > 1) dbgPrintf("A2DP: unexpected further data in AVDTP_SIG_CLOSE/ABORT/SUSPEND\n");
            if(reqSz < 1){
                dbgPrintf("A2DP: not enough data in AVDTP_SIG_CLOSE/ABORT/SUSPEND\n");
                break;
            }
            eid = (*req++) >> 2;
            if(eid != MY_ENDPT_ID){
                dbgPrintf("A2DP: bad EID (%d) AVDTP_SIG_CLOSE/ABORT/SUSPEND\n", eid);
                break;
            }
            audioOff(AUDIO_BT);
            //packet header
            buf[0] = (trans << AVDTP_HDR_SHIFT_TRANS) | (AVDTP_PKT_TYP_SINGLE << AVDTP_HDR_SHIFT_PKT_TYP) | (AVDTP_MSG_TYP_ACCEPT << AVDTP_HDR_SHIFT_MSG_TYP);
            buf[1] = (sigId << AVDTP_HDR_SHIFT_SIG_ID);
            replSz = 2;
            break;

        default:

            dbgPrintf("A2DP (%x) data(trans %d, pktTyp %d msgTyp %d sigId %d %ub)\n", state->remChan, trans, pktTyp, msgTyp, sigId, reqSz);
            dbgPrintf("DATA: ");
            while(reqSz--) dbgPrintf(" %02X", *req++);
            dbgPrintf("\n");
            break;
    }

out:
    if(replSz){
        sg_buf* sgbuf = sg_alloc();
        if(!sgbuf) return;
        if(sg_add_front(sgbuf, buf, replSz, SG_FLAG_MAKE_A_COPY)){

            l2capServiceTx(state->aclConn, state->remChan, sgbuf);
        }
        else{

            sg_free(sgbuf);
            free(sgbuf);
        }
    }
}

static void* a2dpServiceAlloc(uint16_t conn, uint16_t chan, uint16_t remChan){

    A2DPstate* state = conns;

    while(state){

        if(state->needAudioConn && state->aclConn == conn){

            state->needAudioConn = 0;
            return DATA_STATE_ENCODE(state);
        }
        state = state->next;
    }
    
    state = malloc(sizeof(A2DPstate));

    if(state){

        state->aclConn = conn;
        state->remChan = remChan;
        state->needAudioConn = 0;

        state->next = conns;
        conns = state;
    }

    return state;
}

static void a2dpServiceFree(void* service){

    if(IS_DATA_STATE(service)) return;
    
    A2DPstate *state = conns, *prev = NULL;

    while(state){

        if(state == service){

            if(prev) prev->next = state->next;
            else conns = state->next;

            dbgPrintf("A2DP: connection %d.%d closed\n", state->aclConn, state->remChan);
            if(conns) audioOn(AUDIO_BT, conns->samplingRate);

            free(state);
            return;
        }

        prev = state;
        state = state->next;
    }
}

static uint8_t sdpDescrA2DP[] =
{
        //service class ID list
        SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), 0x00, 0x01, SDP_ITEM_DESC(SDP_TYPE_ARRAY, SDP_SZ_u8), 3,
            SDP_ITEM_DESC(SDP_TYPE_UUID, SDP_SZ_2), 0x11, 0x0B,		//Audio Sink
        //ServiceId
        SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), 0x00, 0x03, SDP_ITEM_DESC(SDP_TYPE_UUID, SDP_SZ_2), 0x11, 0x0B,	//Audio Sink
        //ProtocolDescriptorList
        SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), 0x00, 0x04, SDP_ITEM_DESC(SDP_TYPE_ARRAY, SDP_SZ_u8), 16,
            SDP_ITEM_DESC(SDP_TYPE_ARRAY, SDP_SZ_u8), 6,
                SDP_ITEM_DESC(SDP_TYPE_UUID, SDP_SZ_2), 0x01, 0x00, // L2CAP
                SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), L2CAP_PSM_AVDTP >> 8, L2CAP_PSM_AVDTP & 0xFF, // AVDTP PSM
            SDP_ITEM_DESC(SDP_TYPE_ARRAY, SDP_SZ_u8), 6,
                SDP_ITEM_DESC(SDP_TYPE_UUID, SDP_SZ_2), L2CAP_PSM_AVDTP >> 8, L2CAP_PSM_AVDTP & 0xFF, // AVDTP
                SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), 0x01, 0x00,	//AVDTP version (as per spec)
        //browse group list
        SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), 0x00, 0x05, SDP_ITEM_DESC(SDP_TYPE_ARRAY, SDP_SZ_u8), 3,
            SDP_ITEM_DESC(SDP_TYPE_UUID, SDP_SZ_2), 0x10, 0x02, // Public Browse Group
        //profile descriptor list
        SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), 0x00, 0x09, SDP_ITEM_DESC(SDP_TYPE_ARRAY, SDP_SZ_u8), 8,
            SDP_ITEM_DESC(SDP_TYPE_ARRAY, SDP_SZ_u8), 6,
                SDP_ITEM_DESC(SDP_TYPE_UUID, SDP_SZ_2), 0x11, 0x0d, // Advanced Audio
                SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), 0x01, 0x00, // Version 1.0
        //name
        SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), 0x01, 0x00, SDP_ITEM_DESC(SDP_TYPE_TEXT, SDP_SZ_u8), 8, 'A', 'D', 'K', ' ', 'A', '2', 'D', 'P'
};


char btA2dpRegister(void){

    const L2capService a2dp = {L2CAP_FLAG_SUPPORT_CONNECTIONS, a2dpServiceAlloc, a2dpServiceFree, a2dpServiceDataRx};
    
    if(!l2capServiceRegister(L2CAP_PSM_AVDTP, &a2dp)) return 0;
    btSdpServiceDescriptorAdd(sdpDescrA2DP, sizeof(sdpDescrA2DP));

    return 1;
}


/* original tables


proto_4_40:
	0.00000000E+00,5.36548976E-04,1.49188357E-03,2.73370904E-03,
	3.83720193E-03,3.89205149E-03,1.86581691E-03,-3.06012286E-03,
	1.09137620E-02,2.04385087E-02,2.88757392E-02,3.21939290E-02,
	2.58767811E-02,6.13245186E-03,-2.88217274E-02,-7.76463494E-02,
	1.35593274E-01,1.94987841E-01,2.46636662E-01,2.81828203E-01,
	2.94315332E-01,2.81828203E-01,2.46636662E-01,1.94987841E-01,
	-1.35593274E-01,-7.76463494E-02,-2.88217274E-02,6.13245186E-03,
	2.58767811E-02,3.21939290E-02,2.88757392E-02,2.04385087E-02,
	-1.09137620E-02,-3.06012286E-03,1.86581691E-03,3.89205149E-03,
	3.83720193E-03,2.73370904E-03,1.49188357E-03,5.36548976E-04

proto_8_80:

	0.00000000E+00,1.56575398E-04,3.43256425E-04,5.54620202E-04,
	8.23919506E-04,1.13992507E-03,1.47640169E-03,1.78371725E-03,
	2.01182542E-03,2.10371989E-03,1.99454554E-03,1.61656283E-03,
	9.02154502E-04,-1.78805361E-04,-1.64973098E-03,-3.49717454E-03,
	5.65949473E-03,8.02941163E-03,1.04584443E-02,1.27472335E-02,
	1.46525263E-02,1.59045603E-02,1.62208471E-02,1.53184106E-02,
	1.29371806E-02,8.85757540E-03,2.92408442E-03,-4.91578024E-03,
	-1.46404076E-02,-2.61098752E-02,-3.90751381E-02,-5.31873032E-02,
	6.79989431E-02,8.29847578E-02,9.75753918E-02,1.11196689E-01,
	1.23264548E-01,1.33264415E-01,1.40753505E-01,1.45389847E-01,
	1.46955068E-01,1.45389847E-01,1.40753505E-01,1.33264415E-01,
	1.23264548E-01,1.11196689E-01,9.75753918E-02,8.29847578E-02,
	-6.79989431E-02,-5.31873032E-02,-3.90751381E-02,-2.61098752E-02,
	-1.46404076E-02,-4.91578024E-03,2.92408442E-03,8.85757540E-03,
	1.29371806E-02,1.53184106E-02,1.62208471E-02,1.59045603E-02,
	1.46525263E-02,1.27472335E-02,1.04584443E-02,8.02941163E-03,
	-5.65949473E-03,-3.49717454E-03,-1.64973098E-03,-1.78805361E-04,
	9.02154502E-04,1.61656283E-03,1.99454554E-03,2.10371989E-03,
	2.01182542E-03,1.78371725E-03,1.47640169E-03,1.13992507E-03,
	8.23919506E-04,5.54620202E-04,3.43256425E-04,1.56575398E-04


js code to convert to fixpoint:

	var xa = new Array(values here...);

	var num = 0;
	var perRow = 4;
	var L = parseInt(xa.length);

	for(i = 0; i < L; i++){
		x = xa[i];

		var neg = 0;


		if(x < 0){
		  neg = 1;
		  x = -x;
		}
		x *= (1 << 26);   //this 26 should be the number of fraction bits
		x = parseInt(x + 0.5);
		s = x >> 28
		x &= 0x0FFFFFFF;
		if(neg){

			x = x ^ 0x0FFFFFFF;
			x++;
			s ^= 0x0F;
			if(x & 0x10000000) s++;
			x &= 0x0FFFFFFF;
			s &= 0x0F;
		}

		x = x.toString(16);
		while(x.length < 7) x = "0" + x;
		x = s.toString(16) + x;
		x = x.toUpperCase();


		document.write("0x" + x);
		if(i != L - 1) document.write(",");
		if(++num == perRow){
			num = 0;
			document.write("<BR>");
		}
		else document.write(" ");
	}


js code to produce costab (adjust loop variables as needed for both table vairants)

	for(k = 0; k <= 7; k++) for(i = 0 ;i <= 3; i++){

		document.write(Math.cos((i + 0.5) * (k + 2) * Math.PI / 4) + ", ")
	}


js code to generate order tables (they are used for those strange offsets into the V array in synth_* when generating samples):

	var L = 200;
	var V = new Array(L);
	var U = new Array(80);
	var i, j;
	var nBands = 4;

	for(i = 0; i <L; i++) V[i] = i + 1;


	for(i = 0; i <= 4; i++) for(j = 0; j < nBands; j++){

		if(nBands == 4){

			U[i * 8 + j] = V[i * 16 + j];
			U[i * 8 + 4 + j] = V[i * 16 + 12 + j];
		}
		else{

			U[i * 16 + j] = V[i * 32 + j];
			U[i * 16 + 8 + j] = V[i * 32 + 24 + j];
		}
	}


	for(j = 0; j < nBands; j++) for(i = 0; i < 10; i++) document.write((U[j + nBands * i] - 1) + ",\t");


C code for reordering the proto_* tables to access order. insert table values and modify nBands as needed

	#include <stdio.h>
	#include <stdint.h>


	int32_t tabl[] =
	{
		table data here
	};


	int main(int argc, char** argv){

		int i, j;
		int nBands = 8;

		for(j = 0; j < nBands; j++) for(i = 0; i < 10; i++) printf("0x%08X, ",tabl[j + nBands * i]);
	}


*/