sbc_calculate_bits(frame, bits);	for (ch = 0; ch < frame->channels; ch++) {		for (sb = 0; sb < frame->subbands; sb++)			levels[ch][sb] = (1 << bits[ch][sb]) - 1;	}	for (blk = 0; blk < frame->blocks; blk++) {		for (ch = 0; ch < frame->channels; ch++) {			for (sb = 0; sb < frame->subbands; sb++) {				if (levels[ch][sb] > 0) {					audio_sample = 0;					for (bit = 0; bit < bits[ch][sb]; bit++) {						if (consumed > len * 8)							return -1;						if ((data[consumed >> 3] >> (7 - (consumed & 0x7))) & 0x01)							audio_sample |= 1 << (bits[ch][sb] - bit - 1);						consumed++;					}					frame->sb_sample[blk][ch][sb] =						(((audio_sample << 1) | 1) << frame->scale_factor[ch][sb]) /						levels[ch][sb] - (1 << frame->scale_factor[ch][sb]);				} else					frame->sb_sample[blk][ch][sb] = 0;			}		}	}	if (frame->mode == JOINT_STEREO) {		for (blk = 0; blk < frame->blocks; blk++) {			for (sb = 0; sb < frame->subbands; sb++) {				if (frame->joint & (0x01 << sb)) {					temp = frame->sb_sample[blk][0][sb] +						frame->sb_sample[blk][1][sb];					frame->sb_sample[blk][1][sb] =						frame->sb_sample[blk][0][sb] -						frame->sb_sample[blk][1][sb];					frame->sb_sample[blk][0][sb] = temp;				}			}		}	}	if ((consumed & 0x7) != 0)		consumed += 8 - (consumed & 0x7);	return consumed >> 3;}static void sbc_decoder_init(struct sbc_decoder_state *state,				const struct sbc_frame *frame){	int i, ch;	memset(state->V, 0, sizeof(state->V));	state->subbands = frame->subbands;	for (ch = 0; ch < 2; ch++)		for (i = 0; i < frame->subbands * 2; i++)			state->offset[ch][i] = (10 * i + 10);}static inline void sbc_synthesize_four(struct sbc_decoder_state *state,				struct sbc_frame *frame, int ch, int blk){	int i, k, idx;	int32_t *v = state->V[ch];	int *offset = state->offset[ch];	for (i = 0; i < 8; i++) {		/* Shifting */		offset[i]--;		if (offset[i] < 0) {			offset[i] = 79;			memcpy(v + 80, v, 9 * sizeof(*v));		}		/* Distribute the new matrix value to the shifted position */		v[offset[i]] = SCALE4_STAGED1(			MULA(synmatrix4[i][0], frame->sb_sample[blk][ch][0],			MULA(synmatrix4[i][1], frame->sb_sample[blk][ch][1],			MULA(synmatrix4[i][2], frame->sb_sample[blk][ch][2],			MUL (synmatrix4[i][3], frame->sb_sample[blk][ch][3])))));	}	/* Compute the samples */	for (idx = 0, i = 0; i < 4; i++, idx += 5) {		k = (i + 4) & 0xf;		/* Store in output, Q0 */		frame->pcm_sample[ch][blk * 4 + i] = SCALE4_STAGED1(			MULA(v[offset[i] + 0], sbc_proto_4_40m0[idx + 0],			MULA(v[offset[k] + 1], sbc_proto_4_40m1[idx + 0],			MULA(v[offset[i] + 2], sbc_proto_4_40m0[idx + 1],			MULA(v[offset[k] + 3], sbc_proto_4_40m1[idx + 1],			MULA(v[offset[i] + 4], sbc_proto_4_40m0[idx + 2],			MULA(v[offset[k] + 5], sbc_proto_4_40m1[idx + 2],			MULA(v[offset[i] + 6], sbc_proto_4_40m0[idx + 3],			MULA(v[offset[k] + 7], sbc_proto_4_40m1[idx + 3],			MULA(v[offset[i] + 8], sbc_proto_4_40m0[idx + 4],			MUL( v[offset[k] + 9], sbc_proto_4_40m1[idx + 4])))))))))));	}}static inline void sbc_synthesize_eight(struct sbc_decoder_state *state,				struct sbc_frame *frame, int ch, int blk){	int i, j, k, idx;	int *offset = state->offset[ch];	for (i = 0; i < 16; i++) {		/* Shifting */		offset[i]--;		if (offset[i] < 0) {			offset[i] = 159;			for (j = 0; j < 9; j++)				state->V[ch][j + 160] = state->V[ch][j];		}		/* Distribute the new matrix value to the shifted position */		state->V[ch][offset[i]] = SCALE8_STAGED1(			MULA(synmatrix8[i][0], frame->sb_sample[blk][ch][0],			MULA(synmatrix8[i][1], frame->sb_sample[blk][ch][1],			MULA(synmatrix8[i][2], frame->sb_sample[blk][ch][2],			MULA(synmatrix8[i][3], frame->sb_sample[blk][ch][3],			MULA(synmatrix8[i][4], frame->sb_sample[blk][ch][4],			MULA(synmatrix8[i][5], frame->sb_sample[blk][ch][5],			MULA(synmatrix8[i][6], frame->sb_sample[blk][ch][6],			MUL( synmatrix8[i][7], frame->sb_sample[blk][ch][7])))))))));	}	/* Compute the samples */	for (idx = 0, i = 0; i < 8; i++, idx += 5) {		k = (i + 8) & 0xf;		/* Store in output */		frame->pcm_sample[ch][blk * 8 + i] = SCALE8_STAGED1( // Q0			MULA(state->V[ch][offset[i] + 0], sbc_proto_8_80m0[idx + 0],			MULA(state->V[ch][offset[k] + 1], sbc_proto_8_80m1[idx + 0],			MULA(state->V[ch][offset[i] + 2], sbc_proto_8_80m0[idx + 1],			MULA(state->V[ch][offset[k] + 3], sbc_proto_8_80m1[idx + 1],			MULA(state->V[ch][offset[i] + 4], sbc_proto_8_80m0[idx + 2],			MULA(state->V[ch][offset[k] + 5], sbc_proto_8_80m1[idx + 2],			MULA(state->V[ch][offset[i] + 6], sbc_proto_8_80m0[idx + 3],			MULA(state->V[ch][offset[k] + 7], sbc_proto_8_80m1[idx + 3],			MULA(state->V[ch][offset[i] + 8], sbc_proto_8_80m0[idx + 4],			MUL( state->V[ch][offset[k] + 9], sbc_proto_8_80m1[idx + 4])))))))))));	}}static int sbc_synthesize_audio(struct sbc_decoder_state *state,				struct sbc_frame *frame){	int ch, blk;	switch (frame->subbands) {	case 4:		for (ch = 0; ch < frame->channels; ch++) {			for (blk = 0; blk < frame->blocks; blk++)				sbc_synthesize_four(state, frame, ch, blk);		}		return frame->blocks * 4;	case 8:		for (ch = 0; ch < frame->channels; ch++) {			for (blk = 0; blk < frame->blocks; blk++)				sbc_synthesize_eight(state, frame, ch, blk);		}		return frame->blocks * 8;	default:		return -EIO;	}}static void sbc_encoder_init(struct sbc_encoder_state *state,				const struct sbc_frame *frame){	memset(&state->X, 0, sizeof(state->X));	state->subbands = frame->subbands;	state->position[0] = state->position[1] = 9 * frame->subbands;}static inline void _sbc_analyze_four(const int32_t *in, int32_t *out){	sbc_fixed_t t[8], s[5];	t[0] = SCALE4_STAGE1( /* Q8 */		MULA(_sbc_proto_4[0], in[8] - in[32], /* Q18 */		MUL( _sbc_proto_4[1], in[16] - in[24])));	t[1] = SCALE4_STAGE1(		MULA(_sbc_proto_4[2], in[1],		MULA(_sbc_proto_4[3], in[9],		MULA(_sbc_proto_4[4], in[17],		MULA(_sbc_proto_4[5], in[25],		MUL( _sbc_proto_4[6], in[33]))))));	t[2] = SCALE4_STAGE1(		MULA(_sbc_proto_4[7], in[2],		MULA(_sbc_proto_4[8], in[10],		MULA(_sbc_proto_4[9], in[18],		MULA(_sbc_proto_4[10], in[26],		MUL( _sbc_proto_4[11], in[34]))))));	t[3] = SCALE4_STAGE1(		MULA(_sbc_proto_4[12], in[3],		MULA(_sbc_proto_4[13], in[11],		MULA(_sbc_proto_4[14], in[19],		MULA(_sbc_proto_4[15], in[27],		MUL( _sbc_proto_4[16], in[35]))))));	t[4] = SCALE4_STAGE1(		MULA(_sbc_proto_4[17], in[4] + in[36],		MULA(_sbc_proto_4[18], in[12] + in[28],		MUL( _sbc_proto_4[19], in[20]))));	t[5] = SCALE4_STAGE1(		MULA(_sbc_proto_4[16], in[5],		MULA(_sbc_proto_4[15], in[13],		MULA(_sbc_proto_4[14], in[21],		MULA(_sbc_proto_4[13], in[29],		MUL( _sbc_proto_4[12], in[37]))))));	/* don't compute t[6]... this term always multiplies	 * with cos(pi/2) = 0 */	t[7] = SCALE4_STAGE1(		MULA(_sbc_proto_4[6], in[7],		MULA(_sbc_proto_4[5], in[15],		MULA(_sbc_proto_4[4], in[23],		MULA(_sbc_proto_4[3], in[31],		MUL( _sbc_proto_4[2], in[39]))))));	s[0] = MUL( _anamatrix4[0], t[0] + t[4]);	s[1] = MUL( _anamatrix4[2], t[2]);	s[2] = MULA(_anamatrix4[1], t[1] + t[3],		MUL(_anamatrix4[3], t[5]));	s[3] = MULA(_anamatrix4[3], t[1] + t[3],		MUL(_anamatrix4[1], -t[5] + t[7]));	s[4] = MUL( _anamatrix4[3], t[7]);	out[0] = SCALE4_STAGE2( s[0] + s[1] + s[2] + s[4]); /* Q0 */	out[1] = SCALE4_STAGE2(-s[0] + s[1] + s[3]);	out[2] = SCALE4_STAGE2(-s[0] + s[1] - s[3]);	out[3] = SCALE4_STAGE2( s[0] + s[1] - s[2] - s[4]);}static inline void sbc_analyze_four(struct sbc_encoder_state *state,				struct sbc_frame *frame, int ch, int blk){	int32_t *x = &state->X[ch][state->position[ch]];	int16_t *pcm = &frame->pcm_sample[ch][blk * 4];	/* Input 4 Audio Samples */	x[40] = x[0] = pcm[3];	x[41] = x[1] = pcm[2];	x[42] = x[2] = pcm[1];	x[43] = x[3] = pcm[0];	_sbc_analyze_four(x, frame->sb_sample_f[blk][ch]);	state->position[ch] -= 4;	if (state->position[ch] < 0)		state->position[ch] = 36;}static inline void _sbc_analyze_eight(const int32_t *in, int32_t *out){	sbc_fixed_t t[8], s[8];	t[0] = SCALE8_STAGE1( /* Q10 */		MULA(_sbc_proto_8[0], (in[16] - in[64]), /* Q18 = Q18 * Q0 */		MULA(_sbc_proto_8[1], (in[32] - in[48]),		MULA(_sbc_proto_8[2], in[4],		MULA(_sbc_proto_8[3], in[20],		MULA(_sbc_proto_8[4], in[36],		MUL( _sbc_proto_8[5], in[52])))))));	t[1] = SCALE8_STAGE1(		MULA(_sbc_proto_8[6], in[2],		MULA(_sbc_proto_8[7], in[18],		MULA(_sbc_proto_8[8], in[34],		MULA(_sbc_proto_8[9], in[50],		MUL(_sbc_proto_8[10], in[66]))))));	t[2] = SCALE8_STAGE1(		MULA(_sbc_proto_8[11], in[1],		MULA(_sbc_proto_8[12], in[17],		MULA(_sbc_proto_8[13], in[33],		MULA(_sbc_proto_8[14], in[49],		MULA(_sbc_proto_8[15], in[65],		MULA(_sbc_proto_8[16], in[3],		MULA(_sbc_proto_8[17], in[19],		MULA(_sbc_proto_8[18], in[35],		MULA(_sbc_proto_8[19], in[51],		MUL( _sbc_proto_8[20], in[67])))))))))));	t[3] = SCALE8_STAGE1(		MULA( _sbc_proto_8[21], in[5],		MULA( _sbc_proto_8[22], in[21],		MULA( _sbc_proto_8[23], in[37],		MULA( _sbc_proto_8[24], in[53],		MULA( _sbc_proto_8[25], in[69],		MULA(-_sbc_proto_8[15], in[15],		MULA(-_sbc_proto_8[14], in[31],		MULA(-_sbc_proto_8[13], in[47],		MULA(-_sbc_proto_8[12], in[63],		MUL( -_sbc_proto_8[11], in[79])))))))))));	t[4] = SCALE8_STAGE1(		MULA( _sbc_proto_8[26], in[6],		MULA( _sbc_proto_8[27], in[22],		MULA( _sbc_proto_8[28], in[38],		MULA( _sbc_proto_8[29], in[54],		MULA( _sbc_proto_8[30], in[70],		MULA(-_sbc_proto_8[10], in[14],		MULA(-_sbc_proto_8[9], in[30],		MULA(-_sbc_proto_8[8], in[46],		MULA(-_sbc_proto_8[7], in[62],		MUL( -_sbc_proto_8[6], in[78])))))))))));	t[5] = SCALE8_STAGE1(		MULA( _sbc_proto_8[31], in[7],		MULA( _sbc_proto_8[32], in[23],		MULA( _sbc_proto_8[33], in[39],		MULA( _sbc_proto_8[34], in[55],		MULA( _sbc_proto_8[35], in[71],		MULA(-_sbc_proto_8[20], in[13],		MULA(-_sbc_proto_8[19], in[29],		MULA(-_sbc_proto_8[18], in[45],		MULA(-_sbc_proto_8[17], in[61],		MUL( -_sbc_proto_8[16], in[77])))))))))));	t[6] = SCALE8_STAGE1(		MULA( _sbc_proto_8[36], (in[8] + in[72]),		MULA( _sbc_proto_8[37], (in[24] + in[56]),		MULA( _sbc_proto_8[38], in[40],		MULA(-_sbc_proto_8[39], in[12],		MULA(-_sbc_proto_8[5], in[28],		MULA(-_sbc_proto_8[4], in[44],		MULA(-_sbc_proto_8[3], in[60],		MUL( -_sbc_proto_8[2], in[76])))))))));	t[7] = SCALE8_STAGE1(		MULA( _sbc_proto_8[35], in[9],		MULA( _sbc_proto_8[34], in[25],		MULA( _sbc_proto_8[33], in[41],		MULA( _sbc_proto_8[32], in[57],		MULA( _sbc_proto_8[31], in[73],		MULA(-_sbc_proto_8[25], in[11],		MULA(-_sbc_proto_8[24], in[27],		MULA(-_sbc_proto_8[23], in[43],		MULA(-_sbc_proto_8[22], in[59],		MUL( -_sbc_proto_8[21], in[75])))))))))));	s[0] = MULA(  _anamatrix8[0], t[0],		MUL(  _anamatrix8[1], t[6]));	s[1] = MUL(   _anamatrix8[7], t[1]);	s[2] = MULA(  _anamatrix8[2], t[2],		MULA( _anamatrix8[3], t[3],		MULA( _anamatrix8[4], t[5],		MUL(  _anamatrix8[5], t[7]))));	s[3] = MUL(   _anamatrix8[6], t[4]);	s[4] = MULA(  _anamatrix8[3], t[2],		MULA(-_anamatrix8[5], t[3],		MULA(-_anamatrix8[2], t[5],		MUL( -_anamatrix8[4], t[7]))));	s[5] = MULA(  _anamatrix8[4], t[2],		MULA(-_anamatrix8[2], t[3],		MULA( _anamatrix8[5], t[5],		MUL(  _anamatrix8[3], t[7]))));	s[6] = MULA(  _anamatrix8[1], t[0],		MUL( -_anamatrix8[0], t[6]));	s[7] = MULA(  _anamatrix8[5], t[2],		MULA(-_anamatrix8[4], t[3],		MULA( _anamatrix8[3], t[5],		MUL( -_anamatrix8[2], t[7]))));	out[0] = SCALE8_STAGE2( s[0] + s[1] + s[2] + s[3]);	out[1] = SCALE8_STAGE2( s[1] - s[3] + s[4] + s[6]);	out[2] = SCALE8_STAGE2( s[1] - s[3] + s[5] - s[6]);	out[3] = SCALE8_STAGE2(-s[0] + s[1] + s[3] + s[7]);	out[4] = SCALE8_STAGE2(-s[0] + s[1] + s[3] - s[7]);	out[5] = SCALE8_STAGE2( s[1] - s[3] - s[5] - s[6]);	out[6] = SCALE8_STAGE2( s[1] - s[3] - s[4] + s[6]);	out[7] = SCALE8_STAGE2( s[0] + s[1] - s[2] + s[3]);}static inline void sbc_analyze_eight(struct sbc_encoder_state *state,					struct sbc_frame *frame, int ch,					int blk){	int32_t *x = &state->X[ch][state->position[ch]];	int16_t *pcm = &frame->pcm_sample[ch][blk * 8];	/* Input 8 Audio Samples */	x[80] = x[0] = pcm[7];	x[81] = x[1] = pcm[6];	x[82] = x[2] = pcm[5];	x[83] = x[3] = pcm[4];	x[84] = x[4] = pcm[3];	x[85] = x[5] = pcm[2];	x[86] = x[6] = pcm[1];	x[87] = x[7] = pcm[0];	_sbc_analyze_eight(x, frame->sb_sample_f[blk][ch]);	state->position[ch] -= 8;	if (state->position[ch] < 0)		state->position[ch] = 72;}static int sbc_analyze_audio(struct sbc_encoder_state *state,				struct sbc_frame *frame){	int ch, blk;	switch (frame->subbands) {	case 4:		for (ch = 0; ch < frame->channels; ch++)			for (blk = 0; blk < frame->blocks; blk++)				sbc_analyze_four(state, frame, ch, blk);		return frame->blocks * 4;	case 8:		for (ch = 0; ch < frame->channels; ch++)			for (blk = 0; blk < frame->blocks; blk++)				sbc_analyze_eight(state, frame, ch, blk);		return frame->blocks * 8;	default:		return -EIO;	}}/* * Packs the SBC frame from frame into the memory at data. At most len * bytes will be used, should more memory be needed an appropriate * error code will be returned. Returns the length of the packed frame * on success or a negative value on error. * * The error codes are: * -1 Not enough memory reserved * -2 Unsupported sampling rate * -3 Unsupported number of blocks * -4 Unsupported number of subbands * -5 Bitpool value out of bounds * -99 not implemented */static int sbc_pack_frame(uint8_t *data, struct sbc_frame *frame, size_t len){	int produced;	/* Will copy the header parts for CRC-8 calculation here */	uint8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };	int crc_pos = 0;	uint16_t audio_sample;	int ch, sb, blk, bit;	/* channel, subband, block and bit counters */	int bits[2][8];		/* bits distribution */	int levels[2][8];	/* levels are derived from that */	u_int32_t scalefactor[2][8];	/* derived from frame->scale_factor */	data[0] = SBC_SYNCWORD;