sbc.c

这是Linux环境下的蓝牙源代码

	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_STAGED2(
			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_STAGED2( // 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;