sbc.c

Linux的蓝牙操作工具。配合bluez-lib使用

	consumed = 32;

	crc_header[0] = data[1];
	crc_header[1] = data[2];
	crc_pos = 16;

	if (frame->channel_mode == JOINT_STEREO) {
		if (len * 8 < consumed + frame->subbands)
			return -1;

		frame->join = 0x00;
		for (sb = 0; sb < frame->subbands - 1; sb++)
			frame->join |= ((data[4] >> (7 - sb)) & 0x01) << sb;
		if (frame->subbands == 4)
			crc_header[crc_pos / 8] = data[4] & 0xf0;
		else
			crc_header[crc_pos / 8] = data[4];

		consumed += frame->subbands;
		crc_pos += frame->subbands;
	}

	if (len * 8 < consumed + (4 * frame->subbands * frame->channels))
		return -1;

	for (ch = 0; ch < frame->channels; ch++) {
		for (sb = 0; sb < frame->subbands; sb++) {
			/* FIXME assert(consumed % 4 == 0); */
			frame->scale_factor[ch][sb] =
				(data[consumed >> 3] >> (4 - (consumed & 0x7))) & 0x0F;
			crc_header[crc_pos >> 3] |=
				frame->scale_factor[ch][sb] << (4 - (crc_pos & 0x7));

			consumed += 4;
			crc_pos += 4;
		}
	}

	if (data[3] != sbc_crc8(crc_header, crc_pos))
		return -3;

	sbc_calculate_bits(frame, bits, sf);

	for (blk = 0; blk < frame->blocks; blk++) {
		for (ch = 0; ch < frame->channels; ch++) {
			for (sb = 0; sb < frame->subbands; sb++) {
				frame->audio_sample[blk][ch][sb] = 0;
				if (bits[ch][sb] == 0)
					continue;

				for (bit = 0; bit < bits[ch][sb]; bit++) {
					int b;	/* A bit */
					if (consumed > len * 8)
						return -1;

					b = (data[consumed >> 3] >> (7 - (consumed & 0x7))) & 0x01;
					frame->audio_sample[blk][ch][sb] |=
							b << (bits[ch][sb] - bit - 1);

					consumed++;
				}
			}
		}
	}

	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->sb_sample[blk][ch][sb] =
						(((frame->audio_sample[blk][ch][sb] << 16) | 0x8000) /
							levels[ch][sb]) - 0x8000;

					frame->sb_sample[blk][ch][sb] >>= 3;

					/* Q13 */
					frame->sb_sample[blk][ch][sb] =
						(frame->sb_sample[blk][ch][sb] <<
							(frame->scale_factor[ch][sb] + 1));

				} else
					frame->sb_sample[blk][ch][sb] = 0;
			}
		}
	}

	if (frame->channel_mode == JOINT_STEREO) {
		for (blk = 0; blk < frame->blocks; blk++) {
			for (sb = 0; sb < frame->subbands; sb++) {
				if (frame->join & (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, j, k, idx;
	sbc_extended_t res;

	for (i = 0; i < 8; i++) {
		/* Shifting */
		state->offset[ch][i]--;
		if (state->offset[ch][i] < 0) {
			state->offset[ch][i] = 79;
			for (j = 0; j < 9; j++)
				state->V[ch][j+80] = state->V[ch][j];
		}
	}

	for (i = 0; i < 8; i++) {
		/* Distribute the new matrix value to the shifted position */
		SBC_FIXED_0(res);
		for (j = 0; j < 4; j++)
			MULA(res, synmatrix4[i][j], frame->sb_sample[blk][ch][j]);
		state->V[ch][state->offset[ch][i]] = SCALE4_STAGED1(res);
	}

	/* Compute the samples */
	for (idx = 0, i = 0; i < 4; i++) {
		k = (i + 4) & 0xf;
		SBC_FIXED_0(res);
		for (j = 0; j < 10; idx++) {
			MULA(res, state->V[ch][state->offset[ch][i]+j++],
					sbc_proto_4_40m0[idx]);
			MULA(res, state->V[ch][state->offset[ch][k]+j++],
					sbc_proto_4_40m1[idx]);
		}

		/* Store in output, Q0 */
		frame->pcm_sample[ch][blk * 4 + i] = SCALE4_STAGED2(res);
	}
}

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

	for (i = 0; i < 16; i++) {
		/* Shifting */
		state->offset[ch][i]--;
		if (state->offset[ch][i] < 0) {
			state->offset[ch][i] = 159;
			for (j = 0; j < 9; j++)
				state->V[ch][j+160] = state->V[ch][j];
		}
	}

	for (i = 0; i < 16; i++) {
		/* Distribute the new matrix value to the shifted position */
		SBC_FIXED_0(res);
		for (j = 0; j < 8; j++) {
			/* Q28 = Q15 * Q13 */
			MULA(res, synmatrix8[i][j], frame->sb_sample[blk][ch][j]);
		}
		/* Q10 */
		state->V[ch][state->offset[ch][i]] = SCALE8_STAGED1(res);
	}

	/* Compute the samples */
	for (idx = 0, i = 0; i < 8; i++) {
		k = (i + 8) & 0xf;
		SBC_FIXED_0(res);
		for (j = 0; j < 10; idx++) {
			MULA(res, state->V[ch][state->offset[ch][i]+j++], sbc_proto_8_80m0[idx]);
			MULA(res, state->V[ch][state->offset[ch][k]+j++], sbc_proto_8_80m1[idx]);
		}
		/* Store in output */
		frame->pcm_sample[ch][blk * 8 + i] = SCALE8_STAGED2(res); // Q0

	}
}

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

static inline void _sbc_analyze_four(const int32_t *in, int32_t *out)
{

	sbc_extended_t res;
	sbc_extended_t t[8];
	sbc_extended_t s[5];

	MUL(res, _sbc_proto_4[0], (in[8] - in[32])); /* Q18 */
	MULA(res, _sbc_proto_4[1], (in[16] - in[24]));
	t[0] = SCALE4_STAGE1(res); /* Q8 */

	MUL(res, _sbc_proto_4[2], in[1]);
	MULA(res, _sbc_proto_4[3], in[9]);
	MULA(res, _sbc_proto_4[4], in[17]);
	MULA(res, _sbc_proto_4[5], in[25]);
	MULA(res, _sbc_proto_4[6], in[33]);
	t[1] = SCALE4_STAGE1(res);

	MUL(res, _sbc_proto_4[7], in[2]);
	MULA(res, _sbc_proto_4[8], in[10]);
	MULA(res, _sbc_proto_4[9], in[18]);
	MULA(res, _sbc_proto_4[10], in[26]);
	MULA(res, _sbc_proto_4[11], in[34]);
	t[2] = SCALE4_STAGE1(res);

	MUL(res, _sbc_proto_4[12], in[3]);
	MULA(res, _sbc_proto_4[13], in[11]);
	MULA(res, _sbc_proto_4[14], in[19]);
	MULA(res, _sbc_proto_4[15], in[27]);
	MULA(res, _sbc_proto_4[16], in[35]);
	t[3] = SCALE4_STAGE1(res);

	MUL(res, _sbc_proto_4[17], in[4] + in[36]);
	MULA(res, _sbc_proto_4[18], in[12] + in[28]);
	MULA(res, _sbc_proto_4[19], in[20]);
	t[4] = SCALE4_STAGE1(res);

	MUL(res, _sbc_proto_4[16], in[5]);
	MULA(res, _sbc_proto_4[15], in[13]);
	MULA(res, _sbc_proto_4[14], in[21]);
	MULA(res, _sbc_proto_4[13], in[29]);
	MULA(res, _sbc_proto_4[12], in[37]);
	t[5] = SCALE4_STAGE1(res);

	/* don't compute t[6]... this term always multiplies
	 * with cos(pi/2) = 0 */

	MUL(res, _sbc_proto_4[6], in[7]);
	MULA(res, _sbc_proto_4[5], in[15]);
	MULA(res, _sbc_proto_4[4], in[23]);
	MULA(res, _sbc_proto_4[3], in[31]);
	MULA(res, _sbc_proto_4[2], in[39]);
	t[7] = SCALE4_STAGE1(res);

	MUL(s[0], _anamatrix4[0], t[0] + t[4]);
	MUL(s[1], _anamatrix4[2], t[2]);
	MUL(s[2], _anamatrix4[1], t[1] + t[3]);
	MULA(s[2], _anamatrix4[3], t[5]);
	MUL(s[3], _anamatrix4[3], t[1] + t[3]);
	MULA(s[3], _anamatrix4[1], - t[5] + t[7]);
	MUL(s[4], _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)
{
	int i;
	/* 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)];
	_sbc_analyze_four(state->X[ch], frame->sb_sample_f[blk][ch]);
}

static inline void _sbc_analyze_eight(const int32_t *in, int32_t *out)
{
	sbc_extended_t res;
	sbc_extended_t t[8];
	sbc_extended_t s[8];

	MUL(res,  _sbc_proto_8[0], (in[16] - in[64])); /* Q18 = Q18 * Q0 */
	MULA(res, _sbc_proto_8[1], (in[32] - in[48]));
	MULA(res, _sbc_proto_8[2], in[4]);
	MULA(res, _sbc_proto_8[3], in[20]);
	MULA(res, _sbc_proto_8[4], in[36]);
	MULA(res, _sbc_proto_8[5], in[52]);
	t[0] = SCALE8_STAGE1(res); /* Q10 */

	MUL(res,   _sbc_proto_8[6], in[2]);
	MULA(res,  _sbc_proto_8[7], in[18]);
	MULA(res,  _sbc_proto_8[8], in[34]);
	MULA(res,  _sbc_proto_8[9], in[50]);
	MULA(res, _sbc_proto_8[10], in[66]);
	t[1] = SCALE8_STAGE1(res);

	MUL(res,  _sbc_proto_8[11], in[1]);
	MULA(res, _sbc_proto_8[12], in[17]);
	MULA(res, _sbc_proto_8[13], in[33]);
	MULA(res, _sbc_proto_8[14], in[49]);
	MULA(res, _sbc_proto_8[15], in[65]);
	MULA(res, _sbc_proto_8[16], in[3]);
	MULA(res, _sbc_proto_8[17], in[19]);
	MULA(res, _sbc_proto_8[18], in[35]);
	MULA(res, _sbc_proto_8[19], in[51]);
	MULA(res, _sbc_proto_8[20], in[67]);
	t[2] = SCALE8_STAGE1(res);

	MUL(res,   _sbc_proto_8[21], in[5]);
	MULA(res,  _sbc_proto_8[22], in[21]);
	MULA(res,  _sbc_proto_8[23], in[37]);
	MULA(res,  _sbc_proto_8[24], in[53]);
	MULA(res,  _sbc_proto_8[25], in[69]);
	MULA(res, -_sbc_proto_8[15], in[15]);
	MULA(res, -_sbc_proto_8[14], in[31]);
	MULA(res, -_sbc_proto_8[13], in[47]);
	MULA(res, -_sbc_proto_8[12], in[63]);
	MULA(res, -_sbc_proto_8[11], in[79]);
	t[3] = SCALE8_STAGE1(res);

	MUL(res,   _sbc_proto_8[26], in[6]);
	MULA(res,  _sbc_proto_8[27], in[22]);
	MULA(res,  _sbc_proto_8[28], in[38]);
	MULA(res,  _sbc_proto_8[29], in[54]);
	MULA(res,  _sbc_proto_8[30], in[70]);
	MULA(res, -_sbc_proto_8[10], in[14]);
	MULA(res,  -_sbc_proto_8[9], in[30]);
	MULA(res,  -_sbc_proto_8[8], in[46]);
	MULA(res,  -_sbc_proto_8[7], in[62]);
	MULA(res,  -_sbc_proto_8[6], in[78]);
	t[4] = SCALE8_STAGE1(res);

	MUL(res,   _sbc_proto_8[31], in[7]);
	MULA(res,  _sbc_proto_8[32], in[23]);
	MULA(res,  _sbc_proto_8[33], in[39]);
	MULA(res,  _sbc_proto_8[34], in[55]);
	MULA(res,  _sbc_proto_8[35], in[71]);
	MULA(res, -_sbc_proto_8[20], in[13]);
	MULA(res, -_sbc_proto_8[19], in[29]);
	MULA(res, -_sbc_proto_8[18], in[45]);
	MULA(res, -_sbc_proto_8[17], in[61]);
	MULA(res, -_sbc_proto_8[16], in[77]);
	t[5] = SCALE8_STAGE1(res);

	MUL(res,   _sbc_proto_8[36], in[8] + in[72]);
	MULA(res,  _sbc_proto_8[37], in[24] + in[56]);
	MULA(res,  _sbc_proto_8[38], in[40]);
	MULA(res, -_sbc_proto_8[39], in[12]);
	MULA(res,  -_sbc_proto_8[5], in[28]);
	MULA(res,  -_sbc_proto_8[4], in[44]);
	MULA(res,  -_sbc_proto_8[3], in[60]);
	MULA(res,  -_sbc_proto_8[2], in[76]);
	t[6] = SCALE8_STAGE1(res);

	MUL(res,   _sbc_proto_8[35], in[9]);
	MULA(res,  _sbc_proto_8[34], in[25]);
	MULA(res,  _sbc_proto_8[33], in[41]);
	MULA(res,  _sbc_proto_8[32], in[57]);
	MULA(res,  _sbc_proto_8[31], in[73]);
	MULA(res, -_sbc_proto_8[25], in[11]);
	MULA(res, -_sbc_proto_8[24], in[27]);
	MULA(res, -_sbc_proto_8[23], in[43]);
	MULA(res, -_sbc_proto_8[22], in[59]);
	MULA(res, -_sbc_proto_8[21], in[75]);
	t[7] = SCALE8_STAGE1(res);

	MUL(s[0], _anamatrix8[0], t[0]); /* = Q14 * Q10 */
	MULA(s[0], _anamatrix8[1], t[6]);
	MUL(s[1], _anamatrix8[7], t[1]);
	MUL(s[2], _anamatrix8[2], t[2]);
	MULA(s[2], _anamatrix8[3], t[3]);
	MULA(s[2], _anamatrix8[4], t[5]);
	MULA(s[2], _anamatrix8[5], t[7]);
	MUL(s[3], _anamatrix8[6], t[4]);
	MUL(s[4], _anamatrix8[3], t[2]);
	MULA(s[4], -_anamatrix8[5], t[3]);
	MULA(s[4], -_anamatrix8[2], t[5]);
	MULA(s[4], -_anamatrix8[4], t[7]);
	MUL(s[5], _anamatrix8[4], t[2]);
	MULA(s[5], -_anamatrix8[2], t[3]);
	MULA(s[5], _anamatrix8[5], t[5]);
	MULA(s[5], _anamatrix8[3], t[7]);
	MUL(s[6], _anamatrix8[1], t[0]);
	MULA(s[6], -_anamatrix8[0], t[6]);
	MUL(s[7], _anamatrix8[5], t[2]);
	MULA(s[7], -_anamatrix8[4], t[3]);
	MULA(s[7], _anamatrix8[3], t[5]);
	MULA(s[7], -_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)
{
	int i;

	/* 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)];
	_sbc_analyze_eight(state->X[ch], frame->sb_sample_f[blk][ch]);
}

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