sbc: new 'sbc_calc_scalefactors_j' function added to sbc primitives

The code for scale factors calculation with joint stereo support has
been moved to a separate function. It can get platform-specific
SIMD optimizations later for best possible performance.

But even this change in C code improves performance because of the
use of __builtin_clz() instead of loops similar to what was done
to sbc_calc_scalefactors earlier. Also technically it does loop
unrolling by processing two channels at once, which might be either
good or bad for performance (if the registers pressure is increased
and more data is spilled to memory). But the benchmark from 32-bit
x86 system (pentium-m) shows that it got clearly faster:

$ time ./sbcenc.old -b53 -s8 -j test.au > /dev/null

real    0m1.868s
user    0m1.808s
sys     0m0.048s

$ time ./sbcenc.new -b53 -s8 -j test.au > /dev/null

real    0m1.742s
user    0m1.668s
sys     0m0.064s

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;