[PATCH v3 05/10] sbc: Add support for mSBC frame header

[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

 



---
 sbc/sbc.c |  228 +++++++++++++++++++++++++++++++++++++------------------------
 1 file changed, 138 insertions(+), 90 deletions(-)

diff --git a/sbc/sbc.c b/sbc/sbc.c
index 4bc97fc..8236122 100644
--- a/sbc/sbc.c
+++ b/sbc/sbc.c
@@ -377,8 +377,8 @@ static void sbc_calculate_bits(const struct sbc_frame *frame, int (*bits)[8])
  *  -3   CRC8 incorrect
  *  -4   Bitpool value out of bounds
  */
-static int sbc_unpack_frame(const uint8_t *data, struct sbc_frame *frame,
-								size_t len)
+static int sbc_unpack_frame_internal(const uint8_t *data,
+		struct sbc_frame *frame, size_t len)
 {
 	unsigned int consumed;
 	/* Will copy the parts of the header that are relevant to crc
@@ -393,59 +393,6 @@ static int sbc_unpack_frame(const uint8_t *data, struct sbc_frame *frame,
 	int bits[2][8];		/* bits distribution */
 	uint32_t levels[2][8];	/* levels derived from that */
 
-	if (len < 4)
-		return -1;
-
-	if (data[0] != SBC_SYNCWORD)
-		return -2;
-
-	frame->frequency = (data[1] >> 6) & 0x03;
-
-	frame->block_mode = (data[1] >> 4) & 0x03;
-	switch (frame->block_mode) {
-	case SBC_BLK_4:
-		frame->blocks = 4;
-		break;
-	case SBC_BLK_8:
-		frame->blocks = 8;
-		break;
-	case SBC_BLK_12:
-		frame->blocks = 12;
-		break;
-	case SBC_BLK_16:
-		frame->blocks = 16;
-		break;
-	}
-
-	frame->mode = (data[1] >> 2) & 0x03;
-	switch (frame->mode) {
-	case MONO:
-		frame->channels = 1;
-		break;
-	case DUAL_CHANNEL:	/* fall-through */
-	case STEREO:
-	case JOINT_STEREO:
-		frame->channels = 2;
-		break;
-	}
-
-	frame->allocation = (data[1] >> 1) & 0x01;
-
-	frame->subband_mode = (data[1] & 0x01);
-	frame->subbands = frame->subband_mode ? 8 : 4;
-
-	frame->bitpool = data[2];
-
-	if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&
-			frame->bitpool > 16 * frame->subbands)
-		return -4;
-
-	if ((frame->mode == STEREO || frame->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];
@@ -546,6 +493,90 @@ static int sbc_unpack_frame(const uint8_t *data, struct sbc_frame *frame,
 	return consumed >> 3;
 }
 
+static int sbc_unpack_frame(const uint8_t *data,
+		struct sbc_frame *frame, size_t len)
+{
+	if (len < 4)
+		return -1;
+
+	if (data[0] != SBC_SYNCWORD)
+		return -2;
+
+	frame->frequency = (data[1] >> 6) & 0x03;
+	frame->block_mode = (data[1] >> 4) & 0x03;
+
+	switch (frame->block_mode) {
+	case SBC_BLK_4:
+		frame->blocks = 4;
+		break;
+	case SBC_BLK_8:
+		frame->blocks = 8;
+		break;
+	case SBC_BLK_12:
+		frame->blocks = 12;
+		break;
+	case SBC_BLK_16:
+		frame->blocks = 16;
+		break;
+	}
+
+	frame->mode = (data[1] >> 2) & 0x03;
+
+	switch (frame->mode) {
+	case MONO:
+		frame->channels = 1;
+		break;
+	case DUAL_CHANNEL:	/* fall-through */
+	case STEREO:
+	case JOINT_STEREO:
+		frame->channels = 2;
+		break;
+	}
+
+	frame->allocation = (data[1] >> 1) & 0x01;
+
+	frame->subband_mode = (data[1] & 0x01);
+	frame->subbands = frame->subband_mode ? 8 : 4;
+
+	frame->bitpool = data[2];
+
+	if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&
+			frame->bitpool > 16 * frame->subbands)
+		return -4;
+
+	if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) &&
+			frame->bitpool > 32 * frame->subbands)
+		return -4;
+
+	return sbc_unpack_frame_internal(data, frame, len);
+}
+
+static int msbc_unpack_frame(const uint8_t *data,
+		struct sbc_frame *frame, size_t len)
+{
+	if (len < 4)
+		return -1;
+
+	if (data[0] != MSBC_SYNCWORD)
+		return -2;
+	if (data[1] != 0)
+		return -2;
+	if (data[2] != 0)
+		return -2;
+
+	frame->frequency = SBC_FREQ_16000;
+	frame->block_mode = SBC_BLK_4;
+	frame->blocks = MSBC_BLOCKS;
+	frame->allocation = LOUDNESS;
+	frame->mode = MONO;
+	frame->channels = 1;
+	frame->subband_mode = 1;
+	frame->subbands = 8;
+	frame->bitpool = 26;
+
+	return sbc_unpack_frame_internal(data, frame, len);
+}
+
 static void sbc_decoder_init(struct sbc_decoder_state *state,
 					const struct sbc_frame *frame)
 {
@@ -792,38 +823,6 @@ static SBC_ALWAYS_INLINE ssize_t sbc_pack_frame_internal(uint8_t *data,
 	uint32_t levels[2][8];	/* levels are derived from that */
 	uint32_t sb_sample_delta[2][8];
 
-	data[0] = SBC_SYNCWORD;
-
-	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 */
 
 	crc_header[0] = data[1];
@@ -891,6 +890,28 @@ static SBC_ALWAYS_INLINE ssize_t sbc_pack_frame_internal(uint8_t *data,
 static ssize_t sbc_pack_frame(uint8_t *data, struct sbc_frame *frame, size_t len,
 								int joint)
 {
+	int frame_subbands = 4;
+
+	data[0] = SBC_SYNCWORD;
+
+	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;
+
+	data[2] = frame->bitpool;
+
+	if (frame->subbands != 4)
+		frame_subbands = 8;
+
+	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;
+
 	if (frame->subbands == 4) {
 		if (frame->channels == 1)
 			return sbc_pack_frame_internal(
@@ -899,6 +920,7 @@ static ssize_t sbc_pack_frame(uint8_t *data, struct sbc_frame *frame, size_t len
 			return sbc_pack_frame_internal(
 				data, frame, len, 4, 2, joint);
 	} else {
+		data[1] |= 0x01;
 		if (frame->channels == 1)
 			return sbc_pack_frame_internal(
 				data, frame, len, 8, 1, joint);
@@ -908,6 +930,16 @@ static ssize_t sbc_pack_frame(uint8_t *data, struct sbc_frame *frame, size_t len
 	}
 }
 
+static ssize_t msbc_pack_frame(uint8_t *data, struct sbc_frame *frame,
+						size_t len, int joint)
+{
+	data[0] = MSBC_SYNCWORD;
+	data[1] = 0;
+	data[2] = 0;
+
+	return sbc_pack_frame_internal(data, frame, len, 8, 1, joint);
+}
+
 static void sbc_encoder_init(unsigned long flags,
 		struct sbc_encoder_state *state, const struct sbc_frame *frame)
 {
@@ -923,10 +955,24 @@ struct sbc_priv {
 	struct SBC_ALIGNED sbc_frame frame;
 	struct SBC_ALIGNED sbc_decoder_state dec_state;
 	struct SBC_ALIGNED sbc_encoder_state enc_state;
+	int (*unpack_frame)(const uint8_t *data, struct sbc_frame *frame,
+			size_t len);
+	ssize_t (*pack_frame)(uint8_t *data, struct sbc_frame *frame,
+			size_t len, int joint);
 };
 
 static void sbc_set_defaults(sbc_t *sbc, unsigned long flags)
 {
+	struct sbc_priv *priv = sbc->priv;
+
+	if (flags & SBC_MSBC) {
+		priv->pack_frame = msbc_pack_frame;
+		priv->unpack_frame = msbc_unpack_frame;
+	} else {
+		priv->pack_frame = sbc_pack_frame;
+		priv->unpack_frame = sbc_unpack_frame;
+	}
+
 	sbc->flags = flags;
 	sbc->frequency = SBC_FREQ_44100;
 	sbc->mode = SBC_MODE_STEREO;
@@ -980,7 +1026,7 @@ SBC_EXPORT ssize_t sbc_decode(sbc_t *sbc, const void *input, size_t input_len,
 
 	priv = sbc->priv;
 
-	framelen = sbc_unpack_frame(input, &priv->frame, input_len);
+	framelen = priv->unpack_frame(input, &priv->frame, input_len);
 
 	if (!priv->init) {
 		sbc_decoder_init(&priv->dec_state, &priv->frame);
@@ -1112,13 +1158,15 @@ SBC_EXPORT ssize_t sbc_encode(sbc_t *sbc, const void *input, size_t input_len,
 		int j = priv->enc_state.sbc_calc_scalefactors_j(
 			priv->frame.sb_sample_f, priv->frame.scale_factor,
 			priv->frame.blocks, priv->frame.subbands);
-		framelen = sbc_pack_frame(output, &priv->frame, output_len, j);
+		framelen = priv->pack_frame(output,
+				&priv->frame, output_len, j);
 	} else {
 		priv->enc_state.sbc_calc_scalefactors(
 			priv->frame.sb_sample_f, priv->frame.scale_factor,
 			priv->frame.blocks, priv->frame.channels,
 			priv->frame.subbands);
-		framelen = sbc_pack_frame(output, &priv->frame, output_len, 0);
+		framelen = priv->pack_frame(output,
+				&priv->frame, output_len, 0);
 	}
 
 	if (written)
-- 
1.7.9.5

--
To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at  http://vger.kernel.org/majordomo-info.html


[Index of Archives]     [Bluez Devel]     [Linux Wireless Networking]     [Linux Wireless Personal Area Networking]     [Linux ATH6KL]     [Linux USB Devel]     [Linux Media Drivers]     [Linux Audio Users]     [Linux Kernel]     [Linux SCSI]     [Big List of Linux Books]

  Powered by Linux