sbc.c

蓝牙bluez-utils-2.25.tar.gz网上很少找到这个版本的了

						slicecount += 2;
					}
				}
			}
		} while (bitcount + slicecount < frame->bitpool);
		if (bitcount + slicecount == frame->bitpool) {
			bitcount += slicecount;
			bitslice--;
		}

		for (ch = 0; ch < 2; ch++) {
			for (sb = 0; sb < frame->subbands; sb++) {
				if (bitneed[ch][sb] < bitslice + 2) {
					bits[ch][sb] = 0;
				} else {
					bits[ch][sb] = bitneed[ch][sb] - bitslice;
					if (bits[ch][sb] > 16)
						bits[ch][sb] = 16;
				}
			}
		}

		ch = 0;
		sb = 0;
		while ((bitcount < frame->bitpool) && (sb < frame->subbands)) {
			if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
				bits[ch][sb]++;
				bitcount++;
			} else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1)) {
				bits[ch][sb] = 2;
				bitcount += 2;
			}
			if (ch == 1) {
				ch = 0;
				sb++;
			} else {
				ch = 1;
			}
		}

		ch = 0;
		sb = 0;
		while ((bitcount < frame->bitpool) && (sb < frame->subbands)) {
			if (bits[ch][sb] < 16) {
				bits[ch][sb]++;
				bitcount++;
			}
			if (ch == 1) {
				ch = 0;
				sb++;
			} else {
				ch = 1;
			}
		}

	}

}

/* 
 * Unpacks a SBC frame at the beginning of the stream in data,
 * which has at most len bytes into frame.
 * Returns the length in bytes of the packed frame, or a negative
 * value on error. The error codes are:
 *
 *  -1   Data stream too short
 *  -2   Sync byte incorrect
 *  -3   CRC8 incorrect
 *  -4   Bitpool value out of bounds
 */
static int sbc_unpack_frame(const u_int8_t * data, struct sbc_frame *frame, size_t len)
{
	int consumed;
	/* Will copy the parts of the header that are relevant to crc 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, temporarily needed as array index */

	int ch, sb, blk, bit;	/* channel, subband, block and bit standard counters */
	int bits[2][8];		/* bits distribution */
	int levels[2][8];	/* levels derived from that */

	double scalefactor[2][8];	/* derived from frame->scale_factors */

	if (len < 4) {
		return -1;
	}

	if (data[0] != SBC_SYNCWORD) {
		return -2;
	}

	sf = (data[1] >> 6) & 0x03;
	switch (sf) {
	case SBC_FS_16:
		frame->sampling_frequency = 16;
		break;
	case SBC_FS_32:
		frame->sampling_frequency = 32;
		break;
	case SBC_FS_44:
		frame->sampling_frequency = 44.1;
		break;
	case SBC_FS_48:
		frame->sampling_frequency = 48;
		break;
	}

	switch ((data[1] >> 4) & 0x03) {
	case SBC_NB_4:
		frame->blocks = 4;
		break;
	case SBC_NB_8:
		frame->blocks = 8;
		break;
	case SBC_NB_12:
		frame->blocks = 12;
		break;
	case SBC_NB_16:
		frame->blocks = 16;
		break;
	}

	frame->channel_mode = (data[1] >> 2) & 0x03;
	switch (frame->channel_mode) {
	case MONO:
		frame->channels = 1;
		break;
	case DUAL_CHANNEL:	/* fall-through */
	case STEREO:
	case JOINT_STEREO:
		frame->channels = 2;
		break;
	}

	frame->allocation_method = (data[1] >> 1) & 0x01;

	frame->subbands = (data[1] & 0x01) ? 8 : 4;

	frame->bitpool = data[2];

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

	/* data[3] is crc, we're checking it later */

	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;
		} else {
			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;
	} else {
		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 / 8] >> (4 - (consumed % 8))) & 0x0F;
				crc_header[crc_pos / 8] |= frame->scale_factor[ch][sb] << (4 - (crc_pos % 8));

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

						b = (data[consumed / 8] >> (7 - (consumed % 8))) & 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;
			scalefactor[ch][sb] = 2 << frame->scale_factor[ch][sb];
		}
	}

	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] =
					    scalefactor[ch][sb] * ((frame->audio_sample[blk][ch][sb] * 2.0 + 1.0) /
								   levels[ch][sb] - 1.0);
				} 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)) {
					frame->sb_sample[blk][0][sb] =
					    frame->sb_sample[blk][0][sb] + frame->sb_sample[blk][1][sb];
					frame->sb_sample[blk][1][sb] =
					    frame->sb_sample[blk][0][sb] - 2 * frame->sb_sample[blk][1][sb];
				}
			}
		}
	}

	if (consumed % 8 != 0)
		consumed += 8 - (consumed % 8);

	return consumed / 8;
}

static void sbc_decoder_init(struct sbc_decoder_state *state, const struct sbc_frame *frame)
{
	memset(&state->S, 0, sizeof(state->S));
	memset(&state->X, 0, sizeof(state->X));
	memset(&state->V, 0, sizeof(state->V));
	memset(&state->U, 0, sizeof(state->U));
	memset(&state->W, 0, sizeof(state->W));
	state->subbands = frame->subbands;
}

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

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

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

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

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

	/* 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;