sbc.c

蓝牙blue tooth sco协议栈

	/* Window by 40 coefficients */
	for (i = 0; i < 40; i++)
		state->W[ch][i] = state->U[ch][i] * sbc_proto_4_40[i] * (-4);

	/* Calculate 4 audio samples */
	for (j = 0; j < 4; j++) {
		state->X[ch][j] = 0;
		for (i = 0; i < 10; i++)
			state->X[ch][j] += state->W[ch][j + 4 * i];
	}

	/* Output 4 reconstructed Audio Samples */
	for (i = 0; i < 4; i++)
		frame->pcm_sample[ch][blk * 4 + i] = state->X[ch][i];
}

static inline void sbc_synthesize_eight(struct sbc_decoder_state *state,
				struct sbc_frame *frame, int ch, int blk)
{
	int i, j, k;

	/* Input 8 New Subband Samples */
	for (i = 0; i < 8; i++)
		state->S[ch][i] = frame->sb_sample[blk][ch][i];

	/* Shifting */
	for (i = 159; i >= 16; i--)
		state->V[ch][i] = state->V[ch][i - 16];

	/* Matrixing */
	for (k = 0; k < 16; k++) {
		state->V[ch][k] = 0;
		for (i = 0; i < 8; i++) {
			state->V[ch][k] += synmatrix8[k][i] * state->S[ch][i];
		}
	}

	/* Build a 80 values vector U */
	for (i = 0; i <= 4; i++) {
		for (j = 0; j < 8; j++) {
			state->U[ch][i * 16 + j] = state->V[ch][i * 32 + j];
			state->U[ch][i * 16 + j + 8] = state->V[ch][i * 32 + j + 24];
		}
	}

	/* Window by 80 coefficients */
	for (i = 0; i < 80; i++)
		state->W[ch][i] = state->U[ch][i] * sbc_proto_8_80[i] * (-4);

	/* Calculate 8 audio samples */
	for (j = 0; j < 8; j++) {
		state->X[ch][j] = 0;
		for (i = 0; i < 10; i++)
			state->X[ch][j] += state->W[ch][j + 8 * i];
	}

	/* Ouput 8 reconstructed Audio Samples */
	for (i = 0; i < 8; i++)
		frame->pcm_sample[ch][blk * 8 + i] = state->X[ch][i];
}

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++) {
			memset(frame->pcm_sample[ch], 0,
				sizeof(frame->pcm_sample[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++) {
			memset(frame->pcm_sample[ch], 0,
				sizeof(frame->pcm_sample[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->S, 0, sizeof(state->S));
	memset(&state->X, 0, sizeof(state->X));
	memset(&state->Y, 0, sizeof(state->Y));
	memset(&state->Z, 0, sizeof(state->Z));
	state->subbands = frame->subbands;
}

static inline void sbc_analyze_four(struct sbc_encoder_state *state,
				struct sbc_frame *frame, int ch, int blk)
{
	int i, k;

	/* Input 4 New Audio Samples */
	for (i = 39; i >= 4; i--)
		state->X[ch][i] = state->X[ch][i - 4];
	for (i = 3; i >= 0; i--)
		state->X[ch][i] = frame->pcm_sample[ch][blk * 4 + (3 - i)];

	/* Windowing by 40 coefficients */
	for (i = 0; i < 40; i++)
		state->Z[ch][i] = sbc_proto_4_40[i] * state->X[ch][i];

	/* Partial calculation */
	for (i = 0; i < 8; i++) {
		state->Y[ch][i] = 0;
		for (k = 0; k < 5; k++)
			state->Y[ch][i] += state->Z[ch][i + k * 8];
	}

	/* Calculate 4 subband samples by Matrixing */
	for (i = 0; i < 4; i++) {
		state->S[ch][i] = 0;
		for (k = 0; k < 8; k++)
			state->S[ch][i] += anamatrix4[i][k] * state->Y[ch][k];
	}

	/* Output 4 Subband Samples */
	for (i = 0; i < 4; i++)
		frame->sb_sample[blk][ch][i] = state->S[ch][i];
}

static inline void sbc_analyze_eight(struct sbc_encoder_state *state,
				struct sbc_frame *frame, int ch, int blk)
{
	int i, k;

	/* Input 8 Audio Samples */
	for (i = 79; i >= 8; i--)
		state->X[ch][i] = state->X[ch][i - 8];
	for (i = 7; i >= 0; i--)
		state->X[ch][i] = frame->pcm_sample[ch][blk * 8 + (7 - i)];

	/* Windowing by 80 coefficients */
	for (i = 0; i < 80; i++)
		state->Z[ch][i] = sbc_proto_8_80[i] * state->X[ch][i];

	/* Partial calculation */
	for (i = 0; i < 16; i++) {
		state->Y[ch][i] = 0;
		for (k = 0; k < 5; k++)
			state->Y[ch][i] += state->Z[ch][i + k * 16];
	}

	/* Calculate 8 subband samples by Matrixing */
	for (i = 0; i < 8; i++) {
		state->S[ch][i] = 0;
		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));

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

	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;

	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 = 8;
		priv->frame.blocks = 16;
		priv->frame.bitpool = 32;

		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);
			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;

	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));
}