//
//  bt_a2dp_sink.c
//
//  Bluetooth Protocol Stack - A2DP (Advanced Audio Distribution Profile) Audio Sink application
//  Copyright (C) 2017-2019 Toyohiko Togashi tog001@nifty.com
//
//
//  This program is free software; you can redistribute it and/or modify it under the terms of the
//  GNU General Public License as published by the Free Software Foundation; either version 3
//  of the License, or (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
//  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
//  See the GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License along with this program.
//  If not, see <http://www.gnu.org/licenses/>
//
//
//  Reference:
//      Bluetooth SIG (www.bluetooth.com)
//          Adopted Specifications
//          Traditional Profiles
//              Advanced Audio Distribution Profile (A2DP) 1.3
//
//  Update history
//  ---------- ----- -------------------------------------------
//  2017.03.10 v0.0  First cording
//  2018.02.10 v0.0  Debug print (DEBUG_CPU_RATE)
//  2018.03.02 v0.0  Disconnect state
//  2018.10.31 v0.4b Bug fix (Over the 1024 samples/MTU, support 48kHz sampling frequency)
//  2019.01.09 v0.4X Performance tune for STM32F4
//

//#include <math.h>						// cos()
#include <sys/time.h>
#include <string.h>
#include "bt_a2dp_sink.h"
#include "bt_avdtp_acceptor.h"
#include "bt_spec_avdtp.h"
#include "bt_spec_a2dp.h"
#ifdef __ARM_ARCH
#define ARM_MATH_CM4
#define __FPU_PRESENT 1
#include "arm_math.h"
#endif

//#define DEBUG                       	// Enable a debug message
//#define DEBUG_CPU_RATE  1000        	// Debug mode

#define MAX(a,b)    ((a) > (b) ? (a) : (b))
#define MIN(a,b)    ((a) < (b) ? (a) : (b))

//
//  Debug
//
#ifdef DEBUG
#include <stdio.h>                      // Debug
#define DEBUGP(...)     printf(__VA_ARGS__)
static void dumpConfig(struct bt_avdtp_serviceCapabilitie *param, int len) {
	while(len > 0){
		int l;
		int i;

		DEBUGP("A2DP: dumpConfig: Category=%d\n", param->serviceCategory);
		l = param->LOSC;
		switch (param->serviceCategory){
		case AVDTP_SERVICE_CATEGORY_MediaCodec:
		{
			union bt_a2dp_CodecSpecificInformationElements *p;

			DEBUGP("A2DP:  MediaCodec: type=%d(%s) codec=%d\n",
					param->mediaCodec.mediaType,
					param->mediaCodec.mediaType == AVDTP_MediaType_Audio ? "Audio" : "Video?",
					param->mediaCodec.mediaCodecType);
			p = (union bt_a2dp_CodecSpecificInformationElements *)&(param->mediaCodec.mediaCodecSpecific);
			if (param->mediaCodec.mediaCodecType == A2DP_AudioCodecID_SBC) {
				DEBUGP("A2DP:  SBC: Freq=%s,%s,%s,%s ",
						p->SBC.info1 & A2DP_SBC_SamplingFrequency_16000Hz ? "16k"   : "",
						p->SBC.info1 & A2DP_SBC_SamplingFrequency_32000Hz ? "32k"   : "",
						p->SBC.info1 & A2DP_SBC_SamplingFrequency_44100Hz ? "44.1k" : "",
						p->SBC.info1 & A2DP_SBC_SamplingFrequency_48000Hz ? "48k"   : "");
				DEBUGP("Mode=%s,%s,%s,%s ",
						p->SBC.info1 & A2DP_SBC_ChannelMode_MONO         ? "Mono"   : "",
						p->SBC.info1 & A2DP_SBC_ChannelMode_DUAL_CHANNEL ? "Dual"   : "",
						p->SBC.info1 & A2DP_SBC_ChannelMode_STEREO       ? "Stereo" : "",
						p->SBC.info1 & A2DP_SBC_ChannelMode_JOINT_STEREO ? "Joint"  : "");
				DEBUGP("Block=%s,%s,%s,%s ",
						p->SBC.info2 & A2DP_SBC_BlockLength_4  ? "4"  : "",
						p->SBC.info2 & A2DP_SBC_BlockLength_8  ? "8"  : "",
						p->SBC.info2 & A2DP_SBC_BlockLength_12 ? "12" : "",
						p->SBC.info2 & A2DP_SBC_BlockLength_16 ? "16" : "");
				DEBUGP("Subband=%s,%s ",
						p->SBC.info2 & A2DP_SBC_Subbands_4 ? "4" : "",
						p->SBC.info2 & A2DP_SBC_Subbands_8 ? "8" : "");
				DEBUGP("Alloc=%s,%s ",
						p->SBC.info2 & A2DP_SBC_AllocationMethod_SNR      ? "SNR" : "",
						p->SBC.info2 & A2DP_SBC_AllocationMethod_Loudness ? "Loudness" : "");
				DEBUGP("BitPool=%d,%d\n", p->SBC.minimumBitpoolValue, p->SBC.maximumBitpoolValue);
			} else {
				DEBUGP("A2DP:  Spec=");
				for(i = 0; i < l; i++) {
					DEBUGP("%02x ", param->mediaCodec.mediaCodecSpecific[i]);
				}
				DEBUGP("\n");
			}
			break;
		}
		case AVDTP_SERVICE_CATEGORY_MediaTransport:
			DEBUGP("A2DP:  Media Transport\n");
			break;
		case AVDTP_SERVICE_CATEGORY_Reporting:
			DEBUGP("A2DP:  Reporting\n");
			break;
		case AVDTP_SERVICE_CATEGORY_DelayReporting:
			DEBUGP("A2DP:  Delay Reporting\n");
			break;
		default:
			DEBUGP("A2DP:  Service=");
			for(i = 0; i < l; i++) {
				DEBUGP("%02x ", param->body[i]);
			}
			DEBUGP("\n");
			break;
		}
		param = (struct bt_avdtp_serviceCapabilitie *)&(param->body[l]);
		len -= (AVDTP_serviceCapabilitie_HeadSize + l);
	}
	return;
}
#else
#define DEBUGP(...)
#define dumpConfig(a,b)
#endif

//
//	Control block
//
struct bt_a2dp_endPoint	bt_a2dp_endPoint[BT_A2DP_SINK_MAX_ENDPOINT];

//
//	Local subroutines
//  	Decoder of SBC codec
//		Reference: A2DP document section 12 Appendix B: Technical Specification of SBC
//
static int getBitStream(unsigned char *str, int len) {
//  const int mask[17] = { 0,
    static int mask[17] = { 0,		// -6.56ms(STM32F446RET/@1152)
        0b0000000000000001,
        0b0000000000000011,
        0b0000000000000111,
        0b0000000000001111,
        0b0000000000011111,
        0b0000000000111111,
        0b0000000001111111,
        0b0000000011111111,
        0b0000000111111111,
        0b0000001111111111,
        0b0000011111111111,
        0b0000111111111111,
        0b0001111111111111,
        0b0011111111111111,
        0b0111111111111111,
        0b1111111111111111,
    };
    static unsigned char    *base;
    static int              now;
    int             a;
    int             s;
    unsigned char   *p;
    long            l;

    if (str) {
        base = str;
        now  = 0;
    }
    p = base + (now / 8);
    s = 24 - (now % 8) - len;
    l = (*p << 16) | (*(p+1) << 8) | *(p+2);
    a = (l >> s) & mask[len];
    now += len;
    return(a);
}

//static int sbc2pcm(short pcm[][2], int pcmDim, unsigned char *sbc, int sbcLen){		// DEL 2018.10.31
static int sbc2pcm(short pcm[][2], int pcmDim, int *samplingFrequency, int *channels,	// ADD 2018.10.31  out
                   unsigned char *sbc, int sbcLen){										// ADD 2018.10.31  in
    struct audio_frame {
        struct frame_header {
//          uint8_t syncword;    // DEL 2018.02.10
            uint8_t flags;
            uint8_t bitpool;
            uint8_t crc_check;
            uint8_t join[0];
        }       frame_header;
        uint8_t scale_factor[0];      // 4bit
        uint8_t audio_samples[0];     // 1..16bit
    };
#define FLAGS_SAMPLING_FREQUENCY    0b11000000
#define FLAGS_BLOCKS                0b00110000
#define FLAGS_CHANNEL_MODE          0b00001100
#define FLAGS_ALLOCATION_METHOD     0b00000010
#define FLAGS_SUBBANDS              0b00000001
#define FS_16kHz        0b00000000
#define FS_32kHz        0b01000000
#define FS_44_1kHz      0b10000000
#define FS_48kHz        0b11000000
#define BLOCKS_4        0b00000000
#define BLOCKS_8        0b00010000
#define BLOCKS_12       0b00100000
#define BLOCKS_16       0b00110000
#define MONO            0b00000000
#define DUAL            0b00000100
#define STEREO          0b00001000
#define JOINT_STEREO    0b00001100
#define LOUDNESS        0b00000000
#define SNR             0b00000010
#define SUBBAND4        0b00000000
#define SUBBAND8        0b00000001
    int pc;
    struct audio_frame *q;

    // Skip at the sync byte
    while(*sbc != 0x9c) {
    	sbc++;
    	sbcLen--;
    }
    sbc++;
	sbcLen--;
	q  = (struct audio_frame *)sbc;
    pc = 0;

    // Decoding of frames
    while(sbcLen > 0) {
        const int offset4[4][4] = {
            {-1, 0, 0, 0},
            {-2, 0, 0, 1},
            {-2, 0, 0, 1},
            {-2, 0, 0, 1},
        };
        const int offset8[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},
        };
        int sampling_frequency;
//      int	blocks;
        int	channel_mode;
        int	allocation_method;
//      int	subbands;
        int	bitpool;
//      int	crc_check;
//      int	fs;
        int	nrof_blocks;
        int	nrof_channels;
        int	nrof_subbands;
        int join[8];
        int	sb,ch,blk;
        int	bitneed[2][8];
        int max_bitneed;
        int bitcount;
        int slicecount;
        int bitslice;
        int	bits[2][8];
//      long levels[2][8];
//      int rfa;
        int	scale_factor[2][8];
        unsigned short audio_samples[16][2][8];
#ifdef CONV1
        long  sb_sample[16][2][8];
#else
        float sb_sample[16][2][8];
#endif

        switch(q->frame_header.flags & FLAGS_SAMPLING_FREQUENCY) {
            case FS_16kHz:  sampling_frequency = 0; break;
            case FS_32kHz:  sampling_frequency = 1; break;
            case FS_44_1kHz:sampling_frequency = 2; break;
            default:        sampling_frequency = 3; break;
        }
        switch(q->frame_header.flags & FLAGS_BLOCKS) {
            case BLOCKS_4:  nrof_blocks = 4;  break;
            case BLOCKS_8:  nrof_blocks = 8;  break;
            case BLOCKS_12: nrof_blocks = 12; break;
            default:        nrof_blocks = 16; break;
        }
        channel_mode = q->frame_header.flags & FLAGS_CHANNEL_MODE;
        switch(channel_mode) {
            case MONO:      nrof_channels = 1; break;
            case DUAL:      nrof_channels = 2; break;
            case STEREO:    nrof_channels = 2; break;
            default:        nrof_channels = 2; break;
        }
        allocation_method = q->frame_header.flags & FLAGS_ALLOCATION_METHOD;
        switch(q->frame_header.flags & FLAGS_SUBBANDS) {
            case SUBBAND4:  nrof_subbands = 4; break;
            default:        nrof_subbands = 8; break;
        }
        bitpool   = q->frame_header.bitpool;
//      crc_check = q->frame_header.crc_check;
        getBitStream(&(q->frame_header.join[0]), 0);
//      rfa = 0;
        if (channel_mode == JOINT_STEREO) {
            for(sb = 0; sb < (nrof_subbands - 1); sb++) {
                join[sb] = getBitStream(NULL, 1);
            }
//          rfa = getBitStream(NULL, 1);
            getBitStream(NULL, 1);
        }
        for(ch = 0; ch < nrof_channels; ch++) {
            for(sb = 0; sb < nrof_subbands; sb++) {
                scale_factor[ch][sb] = getBitStream(NULL, 4);
            }
        }
//		DEBUGP("A2DP: %d %d %d %d %d %d %d\n", sampling_frequency, nrof_blocks, nrof_channels, nrof_subbands, bitpool, sbcLen, pc);

        // 12.6.3 Bit Allocation

        // 12.6.3.1 Mono and Dual_Channel Bit Allocation
        if ((channel_mode == MONO) || (channel_mode == DUAL)) {
            if (allocation_method == SNR) {
                for (sb = 0; sb < nrof_subbands; sb++) {
                    bitneed[ch][sb] = scale_factor[ch][sb];
                }
            } else {
                for (sb = 0; sb < nrof_subbands; sb++) {
                    if (scale_factor[ch][sb] == 0) {
                        bitneed[ch][sb] = -5;
                    } else {
                        int loudness;

                        if (nrof_subbands == 4) {
                            loudness = scale_factor[ch][sb] - offset4[sampling_frequency][sb];
                        } else {
                            loudness = scale_factor[ch][sb] - offset8[sampling_frequency][sb];
                        }
                        if (loudness > 0) {
                            bitneed[ch][sb] = loudness / 2;
                        } else {
                            bitneed[ch][sb] = loudness;
                        }
                    }
                }
            }

            // Then the maximum bitneed index is searched for
            max_bitneed = 0;
            for (sb = 0; sb < nrof_subbands; sb++) {
                if (bitneed[ch][sb] > max_bitneed) {
                    max_bitneed = bitneed[ch][sb];
                }
            }

            // Next, an iterative process finds out how many bitslices fit into the bitpool.
            bitcount   = 0;
            slicecount = 0;
            bitslice   = max_bitneed + 1; /* init just above the largest sf */
            do {
                bitslice--;
                bitcount  += slicecount;
                slicecount = 0;
                for (sb = 0; sb < nrof_subbands; 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 < bitpool);
            if ((bitcount + slicecount) == bitpool) {
                bitcount += slicecount;
            }

            // Thereafter, bits are distributed until the last bitslice is reached.
            for (sb = 0; sb < nrof_subbands; sb++) {
                if (bitneed[ch][sb] < (bitslice + 2)) {
                    bits[ch][sb] = 0;
                } else {
                    bits[ch][sb] = MIN(bitneed[ch][sb] - bitslice, 16);
                }
            }

            // The remaining bits are allocated starting at subband 0.
            sb = 0;
            while ((bitcount < bitpool) && (sb < nrof_subbands)) {
                if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
                    bits[ch][sb]++;
                    bitcount++;
                } else if((bitneed[ch][sb] == bitslice+1) && (bitpool > bitcount+1)) {
                    bits[ch][sb] = 2;
                    bitcount    += 2;
                }
                sb++;
            }
            sb=0;
            while((bitcount < bitpool) && (sb < nrof_subbands)) {
                if (bits[ch][sb] < 16) {
                    bits[ch][sb]++;
                    bitcount++;
                }
                sb++;
            }

            // 12.6.3.2 Stereo and Joint_Stereo Bit Allocation
        } else {
            if (allocation_method == SNR) {
                for (ch = 0; ch < 2; ch++) {
                    for (sb = 0; sb < nrof_subbands; sb++) {
                        bitneed[ch][sb] = scale_factor[ch][sb];
                    }
                }
            } else {
                for (ch = 0; ch < 2; ch++) {
                    for (sb = 0; sb < nrof_subbands; sb++) {
                        if (scale_factor[ch][sb] == 0) {
                            bitneed[ch][sb] = -5;
                        } else {
                        	int loudness;

                            if (nrof_subbands == 4) {
                                loudness = scale_factor[ch][sb] - offset4[sampling_frequency][sb];
                            } else {
                                loudness = scale_factor[ch][sb] - offset8[sampling_frequency][sb];
                            }
                            if (loudness > 0) {
                                bitneed[ch][sb] = loudness / 2;
                            } else {
                                bitneed[ch][sb] = loudness;
                            }
                        }
                    }
                }
            }

            // Then the maximum bitneed index is searched for.
            max_bitneed = 0;
            for (ch = 0; ch < 2; ch++) {
                for (sb = 0; sb < nrof_subbands; sb++) {
                    if (bitneed[ch][sb] > max_bitneed) {
                        max_bitneed = bitneed[ch][sb];
                    }
                }
            }

            // Next, an iterative process finds out how many bitslices fit into the bitpool.
            bitcount   = 0;
            slicecount = 0;
            bitslice   = max_bitneed + 1; /* init just above the largest sf */
            do {
                bitslice--;
                bitcount += slicecount;
                slicecount = 0;
                for (ch = 0; ch < 2; ch++) {
                    for (sb = 0; sb < nrof_subbands; 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) < bitpool);
            if ((bitcount + slicecount) == bitpool) {
                bitcount += slicecount;
                bitslice--;
            }

            // Thereafter bits are distributed until the last bitslice is reached.
            for (ch = 0; ch < 2; ch++) {
                for (sb = 0; sb < nrof_subbands; sb++) {
                    if (bitneed[ch][sb] < (bitslice + 2)) {
                        bits[ch][sb] = 0;
                    } else {
                        bits[ch][sb] = MIN(bitneed[ch][sb] - bitslice, 16);
                    }
                }
            }

            // The remaining bits are allocated starting with subband 0 of the first channel.
            ch = 0;
            sb = 0;
            while ((bitcount < bitpool) && (sb < nrof_subbands)) {
                if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
                    bits[ch][sb]++;
                    bitcount++;
                } else if ((bitneed[ch][sb] == (bitslice + 1)) && (bitpool > (bitcount + 1))) {
                    bits[ch][sb] = 2;
                    bitcount    += 2;
                }
                if (ch == 1) {
                    ch = 0;
                    sb++;
                } else {
                    ch = 1;
                }
            }
            ch = 0;
            sb = 0;
            while ((bitcount < bitpool) && (sb < nrof_subbands)) {
                bits[ch][sb]++;
                bitcount++;
                if (ch == 1) {
                    ch = 0;
                    sb++;
                } else {
                    ch = 1;
                }
            }
        }

        // Extracting a audio samples from raw data
        for (blk = 0; blk < nrof_blocks; blk++) {					// 7.085ms(STM32F446RET/@1152)
            for (ch = 0; ch < nrof_channels; ch++) {
                for (sb = 0; sb < nrof_subbands; sb++) {
                    audio_samples[blk][ch][sb] = getBitStream(NULL, bits[ch][sb]);
                }
            }
        }

        // 12.6.4 Reconstruction of the Subband Samples
//      for (ch = 0; ch < nrof_channels; ch++) {
//          for (sb = 0; sb < nrof_subbands; sb++) {
//              levels[ch][sb] = pow(2.0, bits[ch][sb]) - 1;
//          }
//      }
        for (blk = 0; blk < nrof_blocks; blk++) {					// 0.45ms(STM32F446RET/@1152)
            for (ch = 0; ch < nrof_channels; ch++) {
                for (sb = 0; sb < nrof_subbands; sb++) {
//                  if (levels[ch][sb] > 0) {
//                      sb_sample[blk][ch][sb] = scale_factor[ch][sb]
//                                             * ((audio_samples[blk][ch][sb] * 2 + 1) / levels[ch][sb] - 1.0);
//                  } else {
//                      sb_sample[blk][ch][sb] = 0;
//                  }
//                  const float pow2[17] = {  // pow(2, i)
                    static float pow2[17] = {  // pow(2, i)  -5.7ms(F446RET)
                            1,	// [0]
                            2,	// [1]
                            4,	// [2]
                            8,	// [3]
                           16,	// [4]
                           32,	// [5]
                           64,	// [6]
                          128,	// [7]
                          256,	// [8]
                          512,	// [9]
                         1024,	// [10]
                         2048,	// [11]
                         4096,	// [12]
                         8192,	// [13]
                        16384,	// [14]
                        32768,	// [15]
                        65536,	// [16]
                    };
                    float scalefactor;
                    float levels;

                    scalefactor = pow2[scale_factor[ch][sb] + 1];
//                  scalefactor = 1 << (scale_factor[ch][sb] + 1);
                    levels      = pow2[bits[ch][sb]] - 1;
//                  levels      = (1 << pow2[bits[ch][sb]) - 1;
                    if (levels > 0) {
                        sb_sample[blk][ch][sb] = scalefactor
                                               * (((float)audio_samples[blk][ch][sb] * 2.0f + 1.0f) / levels - 1.0f);
                    } else {
                        sb_sample[blk][ch][sb] = 0;
                    }
                }
            }
        }

        // 12.6.5 Joint Processing
        for (blk = 0; blk < nrof_blocks; blk++) {
            for (sb = 0; sb < nrof_subbands; sb++) {
                if ((channel_mode == JOINT_STEREO) && (join[sb] == 1)) {
                    sb_sample[blk][0][sb] = sb_sample[blk][0][sb] + sb_sample[blk][1][sb];
                    sb_sample[blk][1][sb] = sb_sample[blk][0][sb] - 2 * sb_sample[blk][1][sb];
                }
            }
        }

        // 12.6.6 Synthesis Filter
        {
//          const float pi = 3.1415926;
//          float S[8];
            static float V[2][160];

            if (nrof_subbands == 4) {
                for (blk = 0; blk < nrof_blocks; blk++) {
                    for (ch = 0; ch < nrof_channels; ch++) {
//                      const float N[8][4] = {    // N[k][i] = cos((i + 0.5) * (k + 2) * pi / 4.0)
                        static float N[8][4] = {   // Performance tune
                            {  0.707107f, -0.707107f, -0.707107f,  0.707107f},
                            {  0.382683f, -0.923879f,  0.923880f, -0.382684f},
                            { -0.000000f,  0.000000f, -0.000000f,  0.000000f},
                            { -0.382683f,  0.923880f, -0.923879f,  0.382683f},
                            { -0.707107f,  0.707107f,  0.707107f, -0.707106f},
                            { -0.923880f, -0.382684f,  0.382683f,  0.923879f},
                            { -1.000000f, -1.000000f, -1.000000f, -1.000000f},
                            { -0.923879f, -0.382683f,  0.382684f,  0.923880f},
                        };
//                      const float D4[40] = {		// filter coeffs table 12-23 multiplied by -4
                        static float D4[40] = {		// Performance tune
                        		 -0.000000e+00f, -4.292392e-03f, -1.193507e-02f, -2.186967e-02f,
                        		 -3.069762e-02f, -3.113641e-02f, -1.492654e-02f,  2.448098e-02f,
                        		 -8.731010e-02f, -1.635081e-01f, -2.310059e-01f, -2.575514e-01f,
                        		 -2.070142e-01f, -4.905961e-02f,  2.305738e-01f,  6.211708e-01f,
                        		 -1.084746e+00f, -1.559903e+00f, -1.973093e+00f, -2.254626e+00f,
                        		 -2.354523e+00f, -2.254626e+00f, -1.973093e+00f, -1.559903e+00f,
                        		  1.084746e+00f,  6.211708e-01f,  2.305738e-01f, -4.905961e-02f,
                        		 -2.070142e-01f, -2.575514e-01f, -2.310059e-01f, -1.635081e-01f,
                        		  8.731010e-02f,  2.448098e-02f, -1.492654e-02f, -3.113641e-02f,
                        		 -3.069762e-02f, -2.186967e-02f, -1.193507e-02f, -4.292392e-03f,
                        };
                        float   U[40];
                        float   W[40];
                        float   X[4];
                        int     i;
                        int     j;
                        int     k;

                        // Input 4 New Subband Samples
//                      for(i = 0; i < 4; i++) {
//                          S[i] = sb_sample[blk][ch][i];
//                      }

                        // Shifting
//                      for (i = 79; i >= 8; --i) {
//                          V[ch][i] = V[ch][i - 8];
//                      }
//                      memmove(&(V[ch][8]), &(V[ch][0]), sizeof(float)*(80-8));
						for (i = (80-8-8); i >= 0; i -= 8) {
							memcpy(&(V[ch][i+8]), &(V[ch][i]), sizeof(float)*8);
						}

                        // Matrixing
                        for (k = 0; k <= 7; k++) {
                            V[ch][k] = 0;
                            for (i = 0; i <= 3; i++) {
//                              V[ch][k] += cos((i + 0.5) * (k + 2) * pi / 4.0) * S[i];
//                              V[ch][k] += cos((i + 0.5) * (k + 2) * pi / 4.0) * sb_sample[blk][ch][i];
                                V[ch][k] += (N[k][i] * sb_sample[blk][ch][i]);
                            }
                        }

                        // Build a 40 values vector U
                        for (i = 0; i <= 4; i++) {
                            for (j = 0; j <= 3; j++) {
                                U[i * 8 + j]     = V[ch][i * 16 + j];
                                U[i * 8 + j + 4] = V[ch][i * 16 + j + 12];
                            }
                        }

                        // Window by 40 coefficients, Produce vector W
                        for (i = 0; i <= 39; i++) {
                            W[i] = U[i] * D4[i];
                        }

                        // Calculate 4 audio samples
                        for (j = 0; j <= 3; j++) {
                            X[j] = 0;
                            for (i = 0; i <= 9; i++) {
                                X[j] += W[j + 4 * i];
                            }
                        }

                        // Output
                        for (j = 0; j <= 3; j++) {
                            pcm[pc+j][ch] = (short)X[j];
                        }
#ifdef DEL											// DEL START 2018.10.31
                        if (nrof_channels == 1) {
                            for (j = 0; j <= 3; j++) {
                                pcm[pc+j][1] = X[j];
                            }
                        }
#endif												// DEL END 2018.10.31
                    }
                    pc += 4;
//                  if ((pc + 4) > pcmDim) {		// DEL 2018.10.31
                    if ((pc + 4) >= pcmDim) {		// ADD 2018.10.31
                        break;
                    }
                }

            } else { // subband == 8
            	for (blk = 0; blk < nrof_blocks; blk++) {
                    for (ch = 0; ch < nrof_channels; ch++) {
//                      const float N[16][8] = {    // N[k][i] = cos((i + 0.5) * (k + 4) * pi / 8.0)
                        static float N[16][8] = {   // -5.6ms(STM32F446RET)
                        		{  0.707107f, -0.707107f, -0.707107f,  0.707107f,  0.707107f, -0.707107f, -0.707107f,  0.707107f,},  // 0
                        		{  0.555570f, -0.980785f,  0.195090f,  0.831470f, -0.831470f, -0.195090f,  0.980785f, -0.555571f,},  // 1
                        		{  0.382683f, -0.923879f,  0.923880f, -0.382684f, -0.382683f,  0.923879f, -0.923880f,  0.382684f,},  // 2
                        		{  0.195090f, -0.555570f,  0.831470f, -0.980785f,  0.980785f, -0.831470f,  0.555571f, -0.195091f,},  // 3
                        		{ -0.000000f,  0.000000f, -0.000000f,  0.000000f, -0.000000f,  0.000000f, -0.000001f,  0.000001f,},  // 4
                        		{ -0.195090f,  0.555570f, -0.831470f,  0.980785f, -0.980785f,  0.831469f, -0.555570f,  0.195090f,},  // 5
                        		{ -0.382683f,  0.923880f, -0.923879f,  0.382683f,  0.382684f, -0.923880f,  0.923879f, -0.382683f,},  // 6
                        		{ -0.555570f,  0.980785f, -0.195090f, -0.831470f,  0.831469f,  0.195091f, -0.980785f,  0.555569f,},  // 7
                        		{ -0.707107f,  0.707107f,  0.707107f, -0.707106f, -0.707107f,  0.707106f,  0.707107f, -0.707106f,},  // 8
                        		{ -0.831470f,  0.195090f,  0.980785f,  0.555571f, -0.555570f, -0.980785f, -0.195091f,  0.831469f,},  // 9
                        		{ -0.923880f, -0.382684f,  0.382683f,  0.923879f,  0.923880f,  0.382684f, -0.382683f, -0.923879f,},  // 10
                        		{ -0.980785f, -0.831470f, -0.555571f, -0.195091f,  0.195090f,  0.555569f,  0.831469f,  0.980785f,},  // 11
                        		{ -1.000000f, -1.000000f, -1.000000f, -1.000000f, -1.000000f, -1.000000f, -1.000000f, -1.000000f,},  // 12
                        		{ -0.980785f, -0.831469f, -0.555570f, -0.195090f,  0.195091f,  0.555571f,  0.831470f,  0.980786f,},  // 13
                        		{ -0.923879f, -0.382683f,  0.382684f,  0.923880f,  0.923879f,  0.382682f, -0.382685f, -0.923880f,},  // 14
                        		{ -0.831470f,  0.195091f,  0.980785f,  0.555570f, -0.555571f, -0.980785f, -0.195089f,  0.831470f,},  // 15
                        };
//                      const float D8[80] = {	// filter coeffs table 12-24 multiplied by -8
                        static float D8[80] = {	// -3.3ms(STM32F446RET)
                        		 -0.000000e+00f, -1.252603e-03f, -2.746051e-03f, -4.436962e-03f,
                        		 -6.591356e-03f, -9.119401e-03f, -1.181121e-02f, -1.426974e-02f,
                        		 -1.609460e-02f, -1.682976e-02f, -1.595636e-02f, -1.293250e-02f,
                        		 -7.217236e-03f,  1.430443e-03f,  1.319785e-02f,  2.797740e-02f,
                        		 -4.527596e-02f, -6.423529e-02f, -8.366755e-02f, -1.019779e-01f,
                        		 -1.172202e-01f, -1.272365e-01f, -1.297668e-01f, -1.225473e-01f,
                        		 -1.034974e-01f, -7.086060e-02f, -2.339268e-02f,  3.932624e-02f,
                        		  1.171233e-01f,  2.088790e-01f,  3.126011e-01f,  4.254984e-01f,
                        		 -5.439916e-01f, -6.638781e-01f, -7.806031e-01f, -8.895735e-01f,
                        		 -9.861164e-01f, -1.066115e+00f, -1.126028e+00f, -1.163119e+00f,
                        		 -1.175641e+00f, -1.163119e+00f, -1.126028e+00f, -1.066115e+00f,
                        		 -9.861164e-01f, -8.895735e-01f, -7.806031e-01f, -6.638781e-01f,
                        		  5.439916e-01f,  4.254984e-01f,  3.126011e-01f,  2.088790e-01f,
                        		  1.171233e-01f,  3.932624e-02f, -2.339268e-02f, -7.086060e-02f,
                        		 -1.034974e-01f, -1.225473e-01f, -1.297668e-01f, -1.272365e-01f,
                        		 -1.172202e-01f, -1.019779e-01f, -8.366755e-02f, -6.423529e-02f,
                        		  4.527596e-02f,  2.797740e-02f,  1.319785e-02f,  1.430443e-03f,
                        		 -7.217236e-03f, -1.293250e-02f, -1.595636e-02f, -1.682976e-02f,
                        		 -1.609460e-02f, -1.426974e-02f, -1.181121e-02f, -9.119401e-03f,
                        		 -6.591356e-03f, -4.436962e-03f, -2.746051e-03f, -1.252603e-03f,
                        };
                        float   U[80];
                        float   W[80];
                        float   X[8];
                        int     i;
                        int     j;
                        int     k;

                        // Input 8 New Subband Samples
//                      for(i = 0; i < 8; i++) {
//                          S[i] = sb_sample[blk][ch][i];
//                      }

#ifdef __ARM_ARCH
                        // Shifting													// 6.4ms(STM32F407VGT/@1024)
//                      for (i = 159; i >= 16; --i) {								// 6.8ms(STM32F446RET/@1152)
//                          V[ch][i] = V[ch][i - 16];
//                      }
//                      memmove(&(V[ch][16]), &(V[ch][0]), sizeof(float)*(160-16));	// -0.0ms(STM32F446RET/@1024)
                        for (i = (160-16-16); i >= 0; i -= 16) {	                // -5.2ms(STM32F446RET/@1024)
                            memcpy(&(V[ch][i+16]), &(V[ch][i]), sizeof(float)*16);
                        }
#else
                        for (i = (160-16-16); i >= 0; i -= 16) {
//                        	arm_copy_f32(&(V[ch][i]), &(V[ch][i+16]), 16);						// -4.82ms(STM32F446RET/@1152) 1.98ms
                        	arm_copy_q31((q31_t *)&(V[ch][i]), (q31_t *)&(V[ch][i+16]), 16);	// -5.35ms(STM32F446RET/@1152) 1.45ms
                        }
#endif

#ifdef __ARM_ARCH
                        // Matrixing												// 2.2ms(STM32F407VGT/@1024)
                        for (k = 0; k < 16; k++) {									// 0.96ms(STM32F446RET/@1152)
                            V[ch][k] = 0;
                            for (i = 0; i < 8; i++) {
//                              V[ch][k] += cos((i + 0.5) * (k + 4) * pi / 8.0) * S[i];
//                              V[ch][k] += cos((i + 0.5) * (k + 4) * pi / 8.0) * sb_sample[blk][ch][i];
                                V[ch][k] += (N[k][i] * sb_sample[blk][ch][i]);
                            }
                        }
#else
						for (k = 0; k < 16; k++) {									// +1.62ms(STM32F446RET/@1152)
                        	arm_dot_prod_f32(N[k], sb_sample[blk][ch], 8, &(V[ch][k]));
                        }
#endif

#ifdef __ARM_ARCH
                        // Build a 80 values vector U								// 0.5ms(STM32F407VGT/@1024)
                        for (i = 0; i < 5; i++) {									// 0.4ms(STM32F446RET/@1152)
                            for (j = 0; j < 8; j++) {
                                U[i*16 + j]     = V[ch][i*32 + j];
                                U[i*16 + j + 8] = V[ch][i*32 + j + 24];
                            }
                        }
#else
                        for (i = 0; i < 5; i++) {									// +0.79ms(STM32F446RET/@1152)
                            for (j = 0; j < 8; j++) {
                                U[(i<<4) + j    ] = V[ch][(i<<5) + j     ];
                                U[(i<<4) + j + 8] = V[ch][(i<<5) + j + 24];
                            }
                        }
#endif

#ifdef __ARM_ARCH
                        // Window by 80 coefficients, Produce vector W
                        for (i = 0; i < 80; i++) {									// 2.1ms(STM32F446RET/@1152)
                            W[i] = U[i] * D8[i];
                        }
#else
                        arm_mult_f32(U, D8, W, 80);									// -0.43ms(STM32F446RET/@1152)
#endif

#ifndef __ARM_ARCH
                        // Calculate 8 audio samples
                        for (j = 0; j < 8; j++) {									// 2.5ms(STM32F446RET/@1152)
                            X[j] = 0;
                            for (i = 0; i < 10; i++) {
                                X[j] += W[j + 8 * i];
                            }
                        }
#else
                        for (j = 0; j < 8; j++) {									// -0.029ms(STM32F446RET/@1152)
                            X[j] = W[j]
								 + W[j+8]
								 + W[j+16]
								 + W[j+24]
								 + W[j+32]
								 + W[j+40]
								 + W[j+48]
								 + W[j+56]
								 + W[j+64]
								 + W[j+72];
                        }
#endif

                        // Output													// 0.4ms(STM32F407VGT/@1024)
                        for (j = 0; j < 8; j++) {
#ifndef DEL
                            pcm[pc+j][ch] = (short)X[j];
#else
                        	if (X[j] < 32767) {
                        		if (X[j] > -32768) {
                                    pcm[pc+j][ch] = X[j];
                        		} else {
                                    pcm[pc+j][ch] = -32768;
                        		}
                        	} else {
                                pcm[pc+j][ch] = 32767;
                        	}
#endif
                        }
#ifdef DEL											// DEL START 2018.10.31
                        if (nrof_channels == 1) {	// Copy L->R
                            for (j = 0; j < 8; j++) {
                                pcm[pc+j][1] = X[j];
                            }
                        }
#endif												// DEL END 2018.10.31
                    }
                    pc += 8;
//                  if ((pc + 8) > pcmDim) {		// DEL 2018.10.31
                    if ((pc + 8) >= pcmDim) {		// ADD 2018.10.31
                        break;
                    }
                }
            }
        }
        {
            int join_length;
            int scale_factor_length;
            int audio_samples_length;
            int total_length;
            int frame_length;

            if (channel_mode == JOINT_STEREO) {
                join_length = nrof_subbands + 1;
            } else {
                join_length = 0;
            }
            scale_factor_length  = 4 * nrof_subbands * nrof_channels;
            audio_samples_length = nrof_blocks * bitpool;
//          total_length         = 32 + join_length + scale_factor_length + audio_samples_length;   // DEL 2018.02.10
            total_length         = 31 + join_length + scale_factor_length + audio_samples_length;   // ADD 2018.02.10
            frame_length         = total_length / 8 + (total_length % 8 ? 1 : 0);
            q = (struct audio_frame *)((unsigned char *)q + frame_length);
            sbcLen -= frame_length;
//          DEBUGP("A2DP: frame=%d rem=%d pc=%d\n", frame_length, sbcLen, pc);
        }

        {													// ADD START 2018.10.31  Return values
			const int sf[4] = {16000, 3200, 44100, 48000};

			*samplingFrequency = sf[sampling_frequency];
			*channels          = nrof_channels;
        }													// ADD END 2018.10.31
    }
    return(pc);
}

//
//	AVDTP User own code
//
int bt_avdt_discover(unsigned char param[][2]) {
	int i;

	DEBUGP("A2DP: bt_avdt_discover\n");
	for(i = 0; i < BT_A2DP_SINK_MAX_ENDPOINT; i++) {
		param[i][0] = ((i+1) << AVDTP_SEID_Shift) | bt_a2dp_endPoint[i].inUse;
		param[i][1] = AVDTP_MediaType_Audio | AVDTP_TSEP_isSNK;
	}
	return(i * 2);
}
int bt_avdt_getCapabilities(int seid, struct bt_avdtp_serviceCapabilitie *param, int max) {
	union bt_a2dp_CodecSpecificInformationElements *p;

	DEBUGP("A2DP: bt_avdt_getCapabilities: %d\n", seid);
	seid--;
	param->serviceCategory           = AVDTP_SERVICE_CATEGORY_MediaCodec;
	param->LOSC                      = sizeof(param->mediaCodec) + sizeof(p->SBC);
	param->mediaCodec.mediaType      = AVDTP_MediaType_Audio;
	param->mediaCodec.mediaCodecType = A2DP_AudioCodecID_SBC;
	p = (union bt_a2dp_CodecSpecificInformationElements *)&(param->mediaCodec.mediaCodecSpecific);
	p->SBC.info1 = 0
				 | A2DP_SBC_SamplingFrequency_16000Hz
				 | A2DP_SBC_SamplingFrequency_32000Hz
				 | A2DP_SBC_SamplingFrequency_44100Hz
				 | A2DP_SBC_SamplingFrequency_48000Hz
				 | A2DP_SBC_ChannelMode_MONO
				 | A2DP_SBC_ChannelMode_DUAL_CHANNEL
				 | A2DP_SBC_ChannelMode_STEREO
				 | A2DP_SBC_ChannelMode_JOINT_STEREO
				 ;
	p->SBC.info2 = A2DP_SBC_BlockLength_4
			     | A2DP_SBC_BlockLength_8
			     | A2DP_SBC_BlockLength_12
			     | A2DP_SBC_BlockLength_16
			     | A2DP_SBC_Subbands_4
			     | A2DP_SBC_Subbands_8
			     | A2DP_SBC_AllocationMethod_SNR
			     | A2DP_SBC_AllocationMethod_Loudness
			     ;
	p->SBC.minimumBitpoolValue = 2;
	p->SBC.maximumBitpoolValue = 53;
	dumpConfig(param, AVDTP_serviceCapabilitie_HeadSize + param->LOSC);	// DEBUG
	return(AVDTP_serviceCapabilitie_HeadSize + param->LOSC);
}


int bt_avdt_setConfiguration(int seid, int int_seid, struct bt_avdtp_serviceCapabilitie *param, int len) {
	int rc;

	DEBUGP("A2DP: bt_avdt_setConfiguration: %d\n", seid);
	seid--;
	if ((seid < 0) && (seid >= BT_A2DP_SINK_MAX_ENDPOINT)) {
		rc = -AVDTP_ERROR_CODE_BAD_ACP_SEID;
	} else if (len > sizeof(bt_a2dp_endPoint[seid])) {
		rc = -AVDTP_ERROR_CODE_BAD_LENGTH;
	} else if (bt_a2dp_endPoint[seid].inUse == AVDTP_InUse) {
		rc = -AVDTP_ERROR_CODE_SEP_IN_USE;
	} else {
		bt_a2dp_endPoint[seid].inUse     = AVDTP_InUse;
		bt_a2dp_endPoint[seid].intSeid   = int_seid;
		bt_a2dp_endPoint[seid].configLen = len;
		memcpy(bt_a2dp_endPoint[seid].config, param, len);
		rc = len;
		dumpConfig(param, len);		// DEBUG
	}
	return(rc);
}

int bt_avdt_setReconfiguration(int seid, struct bt_avdtp_serviceCapabilitie *param, int len) {
	int rc;

	DEBUGP("A2DP: bt_avdt_setReconfiguration: %d\n", seid);
	seid--;
	if ((seid < 0) && (seid >= BT_A2DP_SINK_MAX_ENDPOINT)) {
		rc = -AVDTP_ERROR_CODE_BAD_ACP_SEID;
	} else if (len > sizeof(bt_a2dp_endPoint[seid])) {
		rc = -AVDTP_ERROR_CODE_BAD_LENGTH;
	} else if (bt_a2dp_endPoint[seid].inUse != AVDTP_InUse) {
		rc = -AVDTP_ERROR_CODE_SEP_NOT_IN_USE;
	} else {
		bt_a2dp_endPoint[seid].configLen = len;
		memcpy(bt_a2dp_endPoint[seid].config, param, len);
		rc = len;
		dumpConfig(param, len);		// DEBUG
	}
	return(rc);
}

int bt_avdt_getConfiguration(int seid, struct bt_avdtp_serviceCapabilitie *param, int max) {
	int rc;

	DEBUGP("A2DP: bt_avdt_getConfiguration: %d\n", seid);
	seid--;
	if ((seid >= 0) && (seid < BT_A2DP_SINK_MAX_ENDPOINT)) {
		if (bt_a2dp_endPoint[seid].inUse) {
			int l;

			l = MIN(bt_a2dp_endPoint[seid].configLen, max);
			memcpy(param, bt_a2dp_endPoint[seid].config, l);
			rc = l;
		} else {
			rc = -AVDTP_ERROR_CODE_SEP_NOT_IN_USE;
		}
	} else {
		rc = -AVDTP_ERROR_CODE_BAD_ACP_SEID;
	}
	return(rc);
}

int bt_avdt_open(int seid) {
	int rc;

	DEBUGP("A2DP: bt_avdt_open: %d\n", seid);
	seid--;
	if ((seid < 0) && (seid >= BT_A2DP_SINK_MAX_ENDPOINT)) {
		rc = -AVDTP_ERROR_CODE_BAD_ACP_SEID;
	} else if (bt_a2dp_endPoint[seid].inUse != AVDTP_InUse) {
		rc = -AVDTP_ERROR_CODE_BAD_ACP_SEID;
	} else {
//		bt_a2dp_endPoint[seid].event = A2DP_EVENT_STOP;		// DEL 2018.03.02
		bt_a2dp_endPoint[seid].state = A2DP_STATE_STOP;		// ADD 2018.03.02
															// ADD START 2018.10.31
		if (!bt_a2dp_endPoint[seid].samplingFrequency) {
			bt_a2dp_endPoint[seid].samplingFrequency = BT_A2DP_SINK_DEFAULT_SAMPLING;
		}
		if (!bt_a2dp_endPoint[seid].bitDepth) {
			bt_a2dp_endPoint[seid].bitDepth = BT_A2DP_SINK_DEFAULT_BIT_DEPTH;
		}
		if (!bt_a2dp_endPoint[seid].channels) {
			bt_a2dp_endPoint[seid].channels = BT_A2DP_SINK_DEFAULT_CHANNELS;
		}
															// ADD END 2018.10.31
		rc = 0;
	}
	return(rc);
}

int bt_avdt_start(int seid) {
	int rc;

	DEBUGP("A2DP: bt_avdt_start: %d\n", seid);
	seid--;
	if ((seid < 0) && (seid >= BT_A2DP_SINK_MAX_ENDPOINT)) {
		rc = -AVDTP_ERROR_CODE_BAD_ACP_SEID;
	} else if (bt_a2dp_endPoint[seid].inUse != AVDTP_InUse) {
		rc = -AVDTP_ERROR_CODE_BAD_ACP_SEID;
	} else {
//		bt_a2dp_endPoint[seid].event = A2DP_EVENT_START;	// DEL 2018.03.02
		bt_a2dp_endPoint[seid].state = A2DP_STATE_PLAY;		// ADD 2018.03.02
		rc = 0;
	}
	return(rc);
}

int bt_avdt_suspend(int seid) {
	int rc;

	DEBUGP("A2DP: bt_avdt_suspend: %d\n", seid);
	seid--;
	if ((seid < 0) && (seid >= BT_A2DP_SINK_MAX_ENDPOINT)) {
		rc = -AVDTP_ERROR_CODE_BAD_ACP_SEID;
	} else if (bt_a2dp_endPoint[seid].inUse != AVDTP_InUse) {
		rc = -AVDTP_ERROR_CODE_BAD_ACP_SEID;
	} else {
//		bt_a2dp_endPoint[seid].event = A2DP_EVENT_PAUSE;	// DEL 2018.03.02
		bt_a2dp_endPoint[seid].state = A2DP_STATE_STOP;		// ADD 2018.03.02
		rc = 0;
	}
	return(rc);
}

int bt_avdt_close(int seid) {
	int rc;

	DEBUGP("A2DP: bt_avdt_close: %d\n", seid);
	seid--;
	if ((seid < 0) && (seid >= BT_A2DP_SINK_MAX_ENDPOINT)) {
		rc = -AVDTP_ERROR_CODE_BAD_ACP_SEID;
	} else if (bt_a2dp_endPoint[seid].inUse != AVDTP_InUse) {
		rc = -AVDTP_ERROR_CODE_BAD_ACP_SEID;
	} else {
//		bt_a2dp_endPoint[seid].event = A2DP_EVENT_STOP;			// DEL 2018.03.02
		bt_a2dp_endPoint[seid].state = A2DP_STATE_DISCONNECT;	// ADD 2018.03.02
		bt_a2dp_endPoint[seid].inUse = AVDTP_NotInUse;
		rc = 0;
	}
	return(rc);
}

int bt_avdt_abort(int seid) {
	DEBUGP("A2DP: bt_avdt_abort: %d\n", seid);
	seid--;
	if ((seid >= 0) && (seid < BT_A2DP_SINK_MAX_ENDPOINT)) {
//		bt_a2dp_endPoint[seid].event = A2DP_EVENT_STOP;			// DEL 2018.03.02
		bt_a2dp_endPoint[seid].state = A2DP_STATE_DISCONNECT;	// ADD 2018.03.02
		bt_a2dp_endPoint[seid].inUse = AVDTP_NotInUse;
	}
	return(0);
}

int gg;	// DEBUG

int bt_avdt_mediaRecieve(int seid, unsigned char *buff, int len){
//	DEBUGP("bt_avdt_mediaRecieve: %d %d\n", seid, len);
	seid--;
	if ((seid >= 0) && (seid < BT_A2DP_SINK_MAX_ENDPOINT)) {
		struct bt_a2dp_endPoint *p;
		int 	i;
		int 	j;
		int 	k;
		int 	l;
		short	pcmData[BT_A2DP_SINK_TEMP_BUFFER_SIZE][2];		// ADD 2018.10.31
		int		sf;												// ADD 2018.10.31
		int		ch;												// ADD 2018.10.31
		float	s;												// ADD 2018.10.31

#ifdef DEBUG_CPU_RATE
		struct timeval tp1;
		struct timeval tp2;
		static struct timeval tp3;
		static int sumPeriod  = 0;
		static int sumCpu     = 0;
		static int sumPcmSize = 0;
		static int sumCount   = 0;
		static int sumLen     = 0;
		static int sumSf      = 0;
		static int sumCh      = 0;

		gettimeofday(&tp1, NULL);
#endif

#ifdef DEL														// DEL START 2018.10.31
		p = &bt_a2dp_endPoint[seid];
		i = p->pcmWritePos % BT_A2DP_SINK_BUFFER_NUM;
		j = p->pcmLen[i];
		k = sbc2pcm(&(p->pcmBuffer[i][j]), BT_A2DP_SINK_BUFFER_SIZE, buff, len);	// Buffer over flow -> Plus one element

		j += k;
		l = j - BT_A2DP_SINK_BUFFER_SIZE;
		if (l >= 0) {
			p->pcmLen[i] = BT_A2DP_SINK_BUFFER_SIZE;
			j = l;
			(p->pcmWritePos)++;     // Not reset
			if (++i >= BT_A2DP_SINK_BUFFER_NUM) {
				memcpy(p->pcmBuffer[0], p->pcmBuffer[BT_A2DP_SINK_BUFFER_NUM], sizeof(short)*2*l);
				i = 0;
			}
		}
		p->pcmLen[i] = j;

#else																// DEL END & ADD START 2018.10.31
		p = &bt_a2dp_endPoint[seid];
		k = sbc2pcm(pcmData, BT_A2DP_SINK_TEMP_BUFFER_SIZE, &sf, &ch, buff, len);

		// Copy from pcmData to endpoint buffer
		//    Converting a sampling frequency
		//    Converting a mono/stereo
		s = (float)sf / (float)(p->samplingFrequency);		// Sampling frequency convert scale
		l = 0;												// Source start position
		while (k > l){
			static int	targetNumber;
			static int	targetPosition;
			static int	targetSize;
			static int	targetLen;
			static int	sourceLen;

			targetNumber   = p->pcmWritePos % BT_A2DP_SINK_BUFFER_NUM;
			targetPosition = p->pcmLen[targetNumber];
			targetSize     = BT_A2DP_SINK_BUFFER_SIZE - targetPosition;
			sourceLen      = k - l;
			targetLen      = (int)((float)sourceLen / s);
			if (targetLen > targetSize) {
				targetLen = targetSize;
				sourceLen = (int)((float)targetLen * s);
			}

			if (ch == 2) {
				for(i = 0; i < targetLen; i++) {
					j = (int)((float)i * s);
					p->pcmBuffer[targetNumber][targetPosition+i][0] = pcmData[l+j][0];
					p->pcmBuffer[targetNumber][targetPosition+i][1] = pcmData[l+j][1];
				}
			} else {
				for(i = 0; i < targetLen; i++) {
					j = (int)((float)i * s);
					p->pcmBuffer[targetNumber][targetPosition+i][0] = pcmData[l+j][0];
					p->pcmBuffer[targetNumber][targetPosition+i][1] = pcmData[l+j][0];
				}
			}
			l += sourceLen;
			p->pcmLen[targetNumber] += targetLen;;
			if (p->pcmLen[targetNumber] >= BT_A2DP_SINK_BUFFER_SIZE) {
				p->pcmWritePos++;
				p->pcmLen[p->pcmWritePos % BT_A2DP_SINK_BUFFER_NUM] = 0;
			}
		}
#endif																// ADD END 2018.10.31

#ifdef DEBUG_CPU_RATE
		gettimeofday(&tp2, NULL);
		sumCpu     += (tp2.tv_usec > tp1.tv_usec) ? (tp2.tv_usec - tp1.tv_usec) : (1000000 - tp1.tv_usec + tp2.tv_usec);
		sumPeriod  += (tp1.tv_usec > tp3.tv_usec) ? (tp1.tv_usec - tp3.tv_usec) : (1000000 - tp3.tv_usec + tp1.tv_usec);
		sumPcmSize += k;
		sumLen     += len;
		sumSf      += sf;
		sumCh      += ch;
		sumCount++;
		if (sumCount >= DEBUG_CPU_RATE) {
			DEBUGP("A2DP: SBC: Perform Period=%dus CPU=%dus PCM#=%d len=%d %dHz ch=%d <avg%d>\n", sumPeriod/sumCount, sumCpu/sumCount, sumPcmSize/sumCount, sumLen/sumCount, sumSf/sumCount, sumCh/sumCount, sumCount);
			sumPeriod  = 0;
			sumCpu     = 0;
			sumPcmSize = 0;
			sumLen     = 0;
			sumSf      = 0;
			sumCh      = 0;
			sumCount   = 0;
		}
  	    tp3.tv_sec  = tp1.tv_sec;
  	    tp3.tv_usec = tp1.tv_usec;
#endif

	}
	return(0);
}
