1 files changed, 23 insertions, 68 deletions

diff --git a/src/modules/bluetooth/sbc/sbc.c b/src/modules/bluetooth/sbc/sbc.c
index 5c5c1112..512341fa 100644
--- a/src/modules/bluetooth/sbc/sbc.c
+++ b/src/modules/bluetooth/sbc/sbc.c
@@ -744,7 +744,7 @@ static int sbc_analyze_audio(struct sbc_encoder_state *state,
 
 static SBC_ALWAYS_INLINE int sbc_pack_frame_internal(
 	uint8_t *data, struct sbc_frame *frame, size_t len,
-	int frame_subbands, int frame_channels)
+	int frame_subbands, int frame_channels, int joint)
 {
 	/* Bitstream writer starts from the fourth byte */
 	uint8_t *data_ptr = data + 4;
@@ -801,63 +801,6 @@ static SBC_ALWAYS_INLINE int sbc_pack_frame_internal(
 	crc_pos = 16;
 
 	if (frame->mode == JOINT_STEREO) {
-		/* like frame->sb_sample but joint stereo */
-		int32_t sb_sample_j[16][2];
-		/* scalefactor and scale_factor in joint case */
-		uint32_t scalefactor_j[2];
-		uint8_t scale_factor_j[2];
-
-		uint8_t joint = 0;
-		frame->joint = 0;
-
-		for (sb = 0; sb < frame_subbands - 1; sb++) {
-			scale_factor_j[0] = 0;
-			scalefactor_j[0] = 2 << SCALE_OUT_BITS;
-			scale_factor_j[1] = 0;
-			scalefactor_j[1] = 2 << SCALE_OUT_BITS;
-
-			for (blk = 0; blk < frame->blocks; blk++) {
-				uint32_t tmp;
-				/* Calculate joint stereo signal */
-				sb_sample_j[blk][0] =
-					ASR(frame->sb_sample_f[blk][0][sb], 1) +
-					ASR(frame->sb_sample_f[blk][1][sb], 1);
-				sb_sample_j[blk][1] =
-					ASR(frame->sb_sample_f[blk][0][sb], 1) -
-					ASR(frame->sb_sample_f[blk][1][sb], 1);
-
-				/* calculate scale_factor_j and scalefactor_j for joint case */
-				tmp = fabs(sb_sample_j[blk][0]);
-				while (scalefactor_j[0] < tmp) {
-					scale_factor_j[0]++;
-					scalefactor_j[0] *= 2;
-				}
-				tmp = fabs(sb_sample_j[blk][1]);
-				while (scalefactor_j[1] < tmp) {
-					scale_factor_j[1]++;
-					scalefactor_j[1] *= 2;
-				}
-			}
-
-			/* decide whether to join this subband */
-			if ((frame->scale_factor[0][sb] +
-					frame->scale_factor[1][sb]) >
-					(scale_factor_j[0] +
-					scale_factor_j[1])) {
-				/* use joint stereo for this subband */
-				joint |= 1 << (frame_subbands - 1 - sb);
-				frame->joint |= 1 << sb;
-				frame->scale_factor[0][sb] = scale_factor_j[0];
-				frame->scale_factor[1][sb] = scale_factor_j[1];
-				for (blk = 0; blk < frame->blocks; blk++) {
-					frame->sb_sample_f[blk][0][sb] =
-							sb_sample_j[blk][0];
-					frame->sb_sample_f[blk][1][sb] =
-							sb_sample_j[blk][1];
-				}
-			}
-		}
-
 		PUT_BITS(data_ptr, bits_cache, bits_count,
 			joint, frame_subbands);
 		crc_header[crc_pos >> 3] = joint;
@@ -915,18 +858,23 @@ static SBC_ALWAYS_INLINE int sbc_pack_frame_internal(
 	return data_ptr - data;
 }
 
-static int sbc_pack_frame(uint8_t *data, struct sbc_frame *frame, size_t len)
+static int sbc_pack_frame(uint8_t *data, struct sbc_frame *frame, size_t len,
+								int joint)
 {
 	if (frame->subbands == 4) {
 		if (frame->channels == 1)
-			return sbc_pack_frame_internal(data, frame, len, 4, 1);
+			return sbc_pack_frame_internal(
+				data, frame, len, 4, 1, joint);
 		else
-			return sbc_pack_frame_internal(data, frame, len, 4, 2);
+			return sbc_pack_frame_internal(
+				data, frame, len, 4, 2, joint);
 	} else {
 		if (frame->channels == 1)
-			return sbc_pack_frame_internal(data, frame, len, 8, 1);
+			return sbc_pack_frame_internal(
+				data, frame, len, 8, 1, joint);
 		else
-			return sbc_pack_frame_internal(data, frame, len, 8, 2);
+			return sbc_pack_frame_internal(
+				data, frame, len, 8, 2, joint);
 	}
 }
 
@@ -1124,11 +1072,18 @@ ssize_t sbc_encode(sbc_t *sbc, const void *input, size_t input_len,
 
 	samples = sbc_analyze_audio(&priv->enc_state, &priv->frame);
 
-	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);
+	if (priv->frame.mode == JOINT_STEREO) {
+		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);
+	} 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);
+	}
 
 	if (written)
 		*written = framelen;