data[1] = (frame->frequency & 0x03) << 6;	data[1] |= (frame->block_mode & 0x03) << 4;	data[1] |= (frame->mode & 0x03) << 2;	data[1] |= (frame->allocation & 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->mode == MONO || frame->mode == DUAL_CHANNEL) &&			frame->bitpool > frame->subbands << 4)		return -5;	if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) &&			frame->bitpool > frame->subbands << 5)		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_f[blk][ch][sb])) {					frame->scale_factor[ch][sb]++;					scalefactor[ch][sb] *= 2;				}			}		}	}	if (frame->mode == JOINT_STEREO) {		/* like frame->sb_sample but joint stereo */		int32_t sb_sample_j[16][2];		/* scalefactor and scale_factor in joint case */		u_int32_t scalefactor_j[2];		uint8_t scale_factor_j[2];		frame->joint = 0;		for (sb = 0; sb < frame->subbands - 1; sb++) {			scale_factor_j[0] = 0;			scalefactor_j[0] = 2;			scale_factor_j[1] = 0;			scalefactor_j[1] = 2;			for (blk = 0; blk < frame->blocks; blk++) {				/* Calculate joint stereo signal */				sb_sample_j[blk][0] =					(frame->sb_sample_f[blk][0][sb] +						frame->sb_sample_f[blk][1][sb]) >> 1;				sb_sample_j[blk][1] =					(frame->sb_sample_f[blk][0][sb] -						frame->sb_sample_f[blk][1][sb]) >> 1;				/* calculate scale_factor_j and scalefactor_j for joint case */				while (scalefactor_j[0] < fabs(sb_sample_j[blk][0])) {					scale_factor_j[0]++;					scalefactor_j[0] *= 2;				}				while (scalefactor_j[1] < fabs(sb_sample_j[blk][1])) {					scale_factor_j[1]++;					scalefactor_j[1] *= 2;				}			}			/* decide whether to join this subband */			if ((scalefactor[0][sb] + scalefactor[1][sb]) >					(scalefactor_j[0] + scalefactor_j[1]) ) {				/* use joint stereo for this subband */				frame->joint |= 1 << sb;				frame->scale_factor[0][sb] = scale_factor_j[0];				frame->scale_factor[1][sb] = scale_factor_j[1];				scalefactor[0][sb] = scalefactor_j[0];				scalefactor[1][sb] = scalefactor_j[1];				for (blk = 0; blk < frame->blocks; blk++) {					frame->sb_sample_f[blk][0][sb] =							sb_sample_j[blk][0];					frame->sb_sample_f[blk][1][sb] =							sb_sample_j[blk][1];				}			}		}		data[4] = 0;		for (sb = 0; sb < frame->subbands - 1; sb++)			data[4] |= ((frame->joint >> sb) & 0x01) << (frame->subbands - 1 - sb);		crc_header[crc_pos >> 3] = data[4];		produced += frame->subbands;		crc_pos += frame->subbands;	}	for (ch = 0; ch < frame->channels; ch++) {		for (sb = 0; sb < frame->subbands; sb++) {			data[produced >> 3] <<= 4;			crc_header[crc_pos >> 3] <<= 4;			data[produced >> 3] |= frame->scale_factor[ch][sb] & 0x0F;			crc_header[crc_pos >> 3] |= frame->scale_factor[ch][sb] & 0x0F;			produced += 4;			crc_pos += 4;		}	}	/* align the last crc byte */	if (crc_pos % 8)		crc_header[crc_pos >> 3] <<= 8 - (crc_pos % 8);	data[3] = sbc_crc8(crc_header, crc_pos);	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 =						(uint16_t) ((((frame->sb_sample_f[blk][ch][sb]*levels[ch][sb]) >>									(frame->scale_factor[ch][sb] + 1)) +								levels[ch][sb]) >> 1);					audio_sample <<= 16 - bits[ch][sb];					for (bit = 0; bit < bits[ch][sb]; bit++) {						data[produced >> 3] <<= 1;						if (audio_sample & 0x8000)							data[produced >> 3] |= 0x1;						audio_sample <<= 1;						produced++;					}				}			}		}	}	/* align the last byte */	if (produced % 8) {		data[produced >> 3] <<= 8 - (produced % 8);	}	return (produced + 7) >> 3;}struct sbc_priv {	int init;	struct sbc_frame frame;	struct sbc_decoder_state dec_state;	struct sbc_encoder_state enc_state;};static void sbc_set_defaults(sbc_t *sbc, unsigned long flags){	sbc->frequency = SBC_FREQ_44100;	sbc->mode = SBC_MODE_STEREO;	sbc->subbands = SBC_SB_8;	sbc->blocks = SBC_BLK_16;	sbc->bitpool = 32;#if __BYTE_ORDER == __LITTLE_ENDIAN	sbc->endian = SBC_LE;#elif __BYTE_ORDER == __BIG_ENDIAN	sbc->endian = SBC_BE;#else#error "Unknown byte order"#endif}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_set_defaults(sbc, flags);	return 0;}int sbc_parse(sbc_t *sbc, void *input, int input_len){	return sbc_decode(sbc, input, input_len, NULL, 0, NULL);}int sbc_decode(sbc_t *sbc, void *input, int input_len, void *output,		int output_len, int *written){	struct sbc_priv *priv;	char *ptr;	int i, ch, framelen, samples;	if (!sbc && !input)		return -EIO;	priv = sbc->priv;	framelen = sbc_unpack_frame(input, &priv->frame, input_len);	if (!priv->init) {		sbc_decoder_init(&priv->dec_state, &priv->frame);		priv->init = 1;		sbc->frequency = priv->frame.frequency;		sbc->mode = priv->frame.mode;		sbc->subbands = priv->frame.subband_mode;		sbc->blocks = priv->frame.block_mode;		sbc->allocation = priv->frame.allocation;		sbc->bitpool = priv->frame.bitpool;		priv->frame.codesize = sbc_get_codesize(sbc);		priv->frame.length = sbc_get_frame_length(sbc);	}	if (!output)		return framelen;	if (written)		*written = 0;	samples = sbc_synthesize_audio(&priv->dec_state, &priv->frame);	ptr = output;	if (output_len < samples * priv->frame.channels * 2)		samples = output_len / (priv->frame.channels * 2);	for (i = 0; i < samples; i++) {		for (ch = 0; ch < priv->frame.channels; ch++) {			int16_t s;			s = priv->frame.pcm_sample[ch][i];#if __BYTE_ORDER == __LITTLE_ENDIAN			if (sbc->endian == SBC_BE) {#elif __BYTE_ORDER == __BIG_ENDIAN			if (sbc->endian == SBC_LE) {#else#error "Unknown byte order"#endif				*ptr++ = (s & 0xff00) >> 8;				*ptr++ = (s & 0x00ff);			} else {				*ptr++ = (s & 0x00ff);				*ptr++ = (s & 0xff00) >> 8;			}		}	}	if (written)		*written = samples * priv->frame.channels * 2;	return framelen;}int sbc_encode(sbc_t *sbc, void *input, int input_len, void *output,		int output_len, int *written){	struct sbc_priv *priv;	char *ptr;	int i, ch, framelen, samples;	if (!sbc && !input)		return -EIO;	priv = sbc->priv;	if (written)		*written = 0;	if (!priv->init) {		priv->frame.frequency = sbc->frequency;		priv->frame.mode = sbc->mode;		priv->frame.channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;		priv->frame.allocation = sbc->allocation;		priv->frame.subband_mode = sbc->subbands;		priv->frame.subbands = sbc->subbands ? 8 : 4;		priv->frame.block_mode = sbc->blocks;		priv->frame.blocks = 4 + (sbc->blocks * 4);		priv->frame.bitpool = sbc->bitpool;		priv->frame.codesize = sbc_get_codesize(sbc);		priv->frame.length = sbc_get_frame_length(sbc);		sbc_encoder_init(&priv->enc_state, &priv->frame);		priv->init = 1;	}	/* input must be large enough to encode a complete frame */	if (input_len < priv->frame.codesize)		return 0;	/* output must be large enough to receive the encoded frame */	if (!output || output_len < priv->frame.length)		return -ENOSPC;	ptr = input;	for (i = 0; i < priv->frame.subbands * priv->frame.blocks; i++) {		for (ch = 0; ch < priv->frame.channels; ch++) {			int16_t s;#if __BYTE_ORDER == __LITTLE_ENDIAN			if (sbc->endian == SBC_BE)#elif __BYTE_ORDER == __BIG_ENDIAN			if (sbc->endian == SBC_LE)#else#error "Unknown byte order"#endif				s = (ptr[0] & 0xff) << 8 | (ptr[1] & 0xff);			else				s = (ptr[0] & 0xff) | (ptr[1] & 0xff) << 8;			ptr += 2;			priv->frame.pcm_sample[ch][i] = s;		}	}	samples = sbc_analyze_audio(&priv->enc_state, &priv->frame);	framelen = sbc_pack_frame(output, &priv->frame, output_len);	if (written)		*written = framelen;	return samples * priv->frame.channels * 2;}void sbc_finish(sbc_t *sbc){	if (!sbc)		return;	if (sbc->priv)		free(sbc->priv);	memset(sbc, 0, sizeof(sbc_t));}int sbc_get_frame_length(sbc_t *sbc){	int ret;	uint8_t subbands, channels, blocks, joint;	struct sbc_priv *priv;	priv = sbc->priv;	if (!priv->init) {		subbands = sbc->subbands ? 8 : 4;		blocks = 4 + (sbc->blocks * 4);		channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;		joint = sbc->mode == SBC_MODE_JOINT_STEREO ? 1 : 0;	} else {		subbands = priv->frame.subbands;		blocks = priv->frame.blocks;		channels = priv->frame.channels;		joint = priv->frame.joint;	}	ret = 4 + (4 * subbands * channels) / 8;	/* This term is not always evenly divide so we round it up */	if (channels == 1)		ret += ((blocks * channels * sbc->bitpool) + 7) / 8;	else		ret += (((joint ? subbands : 0) + blocks * sbc->bitpool) + 7)			/ 8;	return ret;}int sbc_get_frame_duration(sbc_t *sbc){	uint8_t subbands, blocks;	uint16_t frequency;	struct sbc_priv *priv;	priv = sbc->priv;	if (!priv->init) {		subbands = sbc->subbands ? 8 : 4;		blocks = 4 + (sbc->blocks * 4);	} else {		subbands = priv->frame.subbands;		blocks = priv->frame.blocks;	}	switch (sbc->frequency) {	case SBC_FREQ_16000:		frequency = 16000;		break;	case SBC_FREQ_32000:		frequency = 32000;		break;	case SBC_FREQ_44100:		frequency = 44100;		break;	case SBC_FREQ_48000:		frequency = 48000;		break;	default:		return 0;	}	return (1000000 * blocks * subbands) / frequency;}int sbc_get_codesize(sbc_t *sbc){	uint8_t subbands, channels, blocks;	struct sbc_priv *priv;	priv = sbc->priv;	if (!priv->init) {		subbands = sbc->subbands ? 8 : 4;		blocks = 4 + (sbc->blocks * 4);		channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;	} else {		subbands = priv->frame.subbands;		blocks = priv->frame.blocks;		channels = priv->frame.channels;	}	return subbands * blocks * channels * 2;}int sbc_reinit(sbc_t *sbc, unsigned long flags){	struct sbc_priv *priv;	if (!sbc || !sbc->priv)		return -EIO;	priv = sbc->priv;	if (priv->init == 1)		memset(sbc->priv, 0, sizeof(struct sbc_priv));	sbc_set_defaults(sbc, flags);	return 0;}