for (k = 0; k < 16; k++)			state->S[ch][i] += anamatrix8[i][k] * state->Y[ch][k];	}	/* Output 8 Subband Samples */	for (i = 0; i < 8; i++)		frame->sb_sample[blk][ch][i] = state->S[ch][i];}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++) {				memset(frame->sb_sample[blk][ch], 0,					sizeof(frame->sb_sample[blk][ch]));				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++) {				memset(frame->sb_sample[blk][ch], 0,					sizeof(frame->sb_sample[blk][ch]));				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(u_int8_t * data, struct sbc_frame *frame, size_t len){	int produced;	/* Will copy the header parts for CRC-8 calculation here */	u_int8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };	int crc_pos = 0;	u_int8_t sf;		/* Sampling frequency as temporary value for table lookup */	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 */	double scalefactor[2][8];	/* derived from frame->scale_factor */	if (len < 4) {		return -1;	}	/* Clear first 4 bytes of data (that's the constant length part of the SBC header) */	memset(data, 0, 4);	data[0] = SBC_SYNCWORD;	if (frame->sampling_frequency == 16) {		data[1] |= (SBC_FS_16 & 0x03) << 6;		sf = SBC_FS_16;	} else if (frame->sampling_frequency == 32) {		data[1] |= (SBC_FS_32 & 0x03) << 6;		sf = SBC_FS_32;	} else if (frame->sampling_frequency == 44.1) {		data[1] |= (SBC_FS_44 & 0x03) << 6;		sf = SBC_FS_44;	} else if (frame->sampling_frequency == 48) {		data[1] |= (SBC_FS_48 & 0x03) << 6;		sf = SBC_FS_48;	} else {		return -2;	}	switch (frame->blocks) {	case 4:		data[1] |= (SBC_NB_4 & 0x03) << 4;		break;	case 8:		data[1] |= (SBC_NB_8 & 0x03) << 4;		break;	case 12:		data[1] |= (SBC_NB_12 & 0x03) << 4;		break;	case 16:		data[1] |= (SBC_NB_16 & 0x03) << 4;		break;	default:		return -3;		break;	}	data[1] |= (frame->channel_mode & 0x03) << 2;	data[1] |= (frame->allocation_method & 0x01) << 1;	switch (frame->subbands) {	case 4:		/* Nothing to do */		break;	case 8:		data[1] |= 0x01;		break;	default:		return -4;		break;	}	data[2] = frame->bitpool;	if (((frame->channel_mode == MONO || frame->channel_mode == DUAL_CHANNEL)	     && frame->bitpool > 16 * frame->subbands)	    || ((frame->channel_mode == STEREO || frame->channel_mode == JOINT_STEREO)		&& frame->bitpool > 32 * frame->subbands)) {		return -5;	}	/* Can't fill in crc yet */	produced = 32;	crc_header[0] = data[1];	crc_header[1] = data[2];	crc_pos = 16;	for (ch = 0; ch < frame->channels; ch++) {		for (sb = 0; sb < frame->subbands; sb++) {			frame->scale_factor[ch][sb] = 0;			scalefactor[ch][sb] = 2;			for (blk = 0; blk < frame->blocks; blk++) {				while (scalefactor[ch][sb] < fabs(frame->sb_sample[blk][ch][sb])) {					frame->scale_factor[ch][sb]++;					scalefactor[ch][sb] *= 2;				}			}		}	}	if (frame->channel_mode == JOINT_STEREO) {		float sb_sample_j[16][2][7]; /* like frame->sb_sample but joint stereo */		int scalefactor_j[2][7], scale_factor_j[2][7]; /* scalefactor and scale_factor in joint case */		/* Calculate joint stereo signal */		for (sb = 0; sb < frame->subbands - 1; sb++) {			for (blk = 0; blk < frame->blocks; blk++) {				sb_sample_j[blk][0][sb] = (frame->sb_sample[blk][0][sb] 							   + frame->sb_sample[blk][1][sb]) / 2;				sb_sample_j[blk][1][sb] = (frame->sb_sample[blk][0][sb] 							   - frame->sb_sample[blk][1][sb]) / 2;			}		}		/* calculate scale_factor_j and scalefactor_j for joint case */		for (ch = 0; ch < 2; ch++) {			for (sb = 0; sb < frame->subbands - 1; sb++) {				scale_factor_j[ch][sb] = 0;				scalefactor_j[ch][sb] = 2;				for (blk = 0; blk < frame->blocks; blk++) {					while (scalefactor_j[ch][sb] < fabs(sb_sample_j[blk][ch][sb])) {						scale_factor_j[ch][sb]++;						scalefactor_j[ch][sb] *= 2;					}				}			}		}		/* decide which subbands to join */		frame->join = 0;		for (sb = 0; sb < frame->subbands - 1; sb++) {			if ( (scalefactor[0][sb] + scalefactor[1][sb]) > 			     (scalefactor_j[0][sb] + scalefactor_j[1][sb]) ) {				/* use joint stereo for this subband */				frame->join |= 1 << sb;				frame->scale_factor[0][sb] = scale_factor_j[0][sb];				frame->scale_factor[1][sb] = scale_factor_j[1][sb];				scalefactor[0][sb] = scalefactor_j[0][sb];				scalefactor[1][sb] = scalefactor_j[1][sb];				for (blk = 0; blk < frame->blocks; blk++) {					frame->sb_sample[blk][0][sb] = sb_sample_j[blk][0][sb];					frame->sb_sample[blk][1][sb] = sb_sample_j[blk][1][sb];				}			}		}  		if (len * 8 < produced + frame->subbands) {			return -1;		} else {			data[4] = 0;			for (sb = 0; sb < frame->subbands - 1; sb++) {				data[4] |= ((frame->join >> sb) & 0x01) << (7 - sb);			}			if (frame->subbands == 4) {				crc_header[crc_pos / 8] = data[4] & 0xf0;			} else {				crc_header[crc_pos / 8] = data[4];			}			produced += frame->subbands;			crc_pos += frame->subbands;		}	}	if (len * 8 < produced + (4 * frame->subbands * frame->channels)) {		return -1;	} else {		for (ch = 0; ch < frame->channels; ch++) {			for (sb = 0; sb < frame->subbands; sb++) {				if (produced % 8 == 0)					data[produced / 8] = 0;				data[produced / 8] |= ((frame->scale_factor[ch][sb] & 0x0F) << (4 - (produced % 8)));				crc_header[crc_pos / 8] |=				    ((frame->scale_factor[ch][sb] & 0x0F) << (4 - (crc_pos % 8)));				produced += 4;				crc_pos += 4;			}		}	}	data[3] = sbc_crc8(crc_header, crc_pos);	sbc_calculate_bits(frame, bits, sf);	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) {					frame->audio_sample[blk][ch][sb] =					    (u_int16_t) (((frame->sb_sample[blk][ch][sb] / scalefactor[ch][sb] +							   1.0) * levels[ch][sb]) / 2.0);				} else {					frame->audio_sample[blk][ch][sb] = 0;				}			}		}	}	for (blk = 0; blk < frame->blocks; blk++) {		for (ch = 0; ch < frame->channels; ch++) {			for (sb = 0; sb < frame->subbands; sb++) {				if (bits[ch][sb] != 0) {					for (bit = 0; bit < bits[ch][sb]; bit++) {						int b;	/* A bit */						if (produced > len * 8) {							return -1;						}						if (produced % 8 == 0) {							data[produced / 8] = 0;						}						b = ((frame->audio_sample[blk][ch][sb]) >> (bits[ch][sb] - bit -											    1)) & 0x01;						data[produced / 8] |= b << (7 - (produced % 8));						produced++;					}				}			}		}	}	if (produced % 8 != 0) {		produced += 8 - (produced % 8);	}	return produced / 8;}struct sbc_priv {	int init;	struct sbc_frame frame;	struct sbc_decoder_state dec_state;	struct sbc_encoder_state enc_state;};int sbc_init(sbc_t *sbc, unsigned long flags){	if (!sbc)		return -EIO;	memset(sbc, 0, sizeof(sbc_t));	sbc->priv = malloc(sizeof(struct sbc_priv));	if (!sbc->priv)		return -ENOMEM;	memset(sbc->priv, 0, sizeof(struct sbc_priv));	sbc->rate     = 44100;	sbc->channels = 2;	sbc->subbands = 8;	sbc->blocks   = 16;	sbc->bitpool  = 32;	return 0;}int sbc_decode(sbc_t *sbc, void *data, int count){	struct sbc_priv *priv;	char *ptr;	int i, ch, framelen, samples;	if (!sbc)		return -EIO;	priv = sbc->priv;	framelen = sbc_unpack_frame(data, &priv->frame, count);	if (!priv->init) {		sbc_decoder_init(&priv->dec_state, &priv->frame);		priv->init = 1;		sbc->rate     = priv->frame.sampling_frequency * 1000;		sbc->channels = priv->frame.channels;		sbc->subbands = priv->frame.subbands;		sbc->blocks   = priv->frame.blocks;		sbc->bitpool  = priv->frame.bitpool;	}	samples = sbc_synthesize_audio(&priv->dec_state, &priv->frame);	if (!sbc->data) {		sbc->size = samples * priv->frame.channels * 2;		sbc->data = malloc(sbc->size);	}	if (sbc->size < samples * priv->frame.channels * 2) {		sbc->size = samples * priv->frame.channels * 2;		sbc->data = realloc(sbc->data, sbc->size);	}	if (!sbc->data) {		sbc->size = 0;		return -ENOMEM;	}	ptr = sbc->data;	for (i = 0; i < samples; i++) {		for (ch = 0; ch < priv->frame.channels; ch++) {			int16_t s = (int16_t)(priv->frame.pcm_sample[ch][i]);			*ptr++ = (s & 0xff00) >> 8;			*ptr++ = (s & 0x00ff);		}	}	sbc->len = samples * priv->frame.channels * 2;	sbc->duration = (1000000 * priv->frame.subbands * priv->frame.blocks) / sbc->rate;	return framelen;}int sbc_encode(sbc_t *sbc, void *data, int count){	struct sbc_priv *priv;	char *ptr;	int i, ch, framelen, samples;	if (!sbc)		return -EIO;	priv = sbc->priv;	if (!priv->init) {		priv->frame.sampling_frequency = ((double) sbc->rate) / 1000;		priv->frame.channels = sbc->channels;		if (sbc->channels > 1)			priv->frame.channel_mode = STEREO;		else			priv->frame.channel_mode = MONO;		priv->frame.allocation_method = SNR;		priv->frame.subbands = sbc->subbands;		priv->frame.blocks   = sbc->blocks;		priv->frame.bitpool  = sbc->bitpool;		sbc_encoder_init(&priv->enc_state, &priv->frame);		priv->init = 1;	}	ptr = data;	for (i = 0; i < priv->frame.subbands * priv->frame.blocks; i++) {		for (ch = 0; ch < sbc->channels; ch++) {			//int16_t s = (ptr[0] & 0xff) << 8 | (ptr[1] & 0xff);			int16_t s = (ptr[1] & 0xff) << 8 | (ptr[2] & 0xff);			ptr += 2;			priv->frame.pcm_sample[ch][i] = ((double) s);		}	}	samples = sbc_analyze_audio(&priv->enc_state, &priv->frame);	if (!sbc->data) {		sbc->size = 1024;		sbc->data = malloc(sbc->size);	}	if (!sbc->data) {		sbc->size = 0;		return -ENOMEM;	}	framelen = sbc_pack_frame(sbc->data, &priv->frame, sbc->size);	sbc->len = framelen;	sbc->duration = (1000000 * priv->frame.subbands * priv->frame.blocks) / sbc->rate;	return samples * sbc->channels * 2;}void sbc_finish(sbc_t *sbc){	if (!sbc)		return;	if (sbc->data)		free(sbc->data);	if (sbc->priv)		free(sbc->priv);	memset(sbc, 0, sizeof(sbc_t));}