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, 75 insertions, 0 deletions

diff --git a/src/modules/bluetooth/sbc/sbc_primitives.c b/src/modules/bluetooth/sbc/sbc_primitives.c
index 2105280e..82cd399d 100644
--- a/src/modules/bluetooth/sbc/sbc_primitives.c
+++ b/src/modules/bluetooth/sbc/sbc_primitives.c
@@ -439,6 +439,80 @@ static void sbc_calc_scalefactors(
 	}
 }
 
+static int sbc_calc_scalefactors_j(
+	int32_t sb_sample_f[16][2][8],
+	uint32_t scale_factor[2][8],
+	int blocks, int subbands)
+{
+	int blk, joint = 0;
+	int32_t tmp0, tmp1;
+	uint32_t x, y;
+
+	/* last subband does not use joint stereo */
+	int sb = subbands - 1;
+	x = 1 << SCALE_OUT_BITS;
+	y = 1 << SCALE_OUT_BITS;
+	for (blk = 0; blk < blocks; blk++) {
+		tmp0 = fabs(sb_sample_f[blk][0][sb]);
+		tmp1 = fabs(sb_sample_f[blk][1][sb]);
+		if (tmp0 != 0)
+			x |= tmp0 - 1;
+		if (tmp1 != 0)
+			y |= tmp1 - 1;
+	}
+	scale_factor[0][sb] = (31 - SCALE_OUT_BITS) - sbc_clz(x);
+	scale_factor[1][sb] = (31 - SCALE_OUT_BITS) - sbc_clz(y);
+
+	/* the rest of subbands can use joint stereo */
+	while (--sb >= 0) {
+		int32_t sb_sample_j[16][2];
+		x = 1 << SCALE_OUT_BITS;
+		y = 1 << SCALE_OUT_BITS;
+		for (blk = 0; blk < blocks; blk++) {
+			tmp0 = sb_sample_f[blk][0][sb];
+			tmp1 = sb_sample_f[blk][1][sb];
+			sb_sample_j[blk][0] = ASR(tmp0, 1) + ASR(tmp1, 1);
+			sb_sample_j[blk][1] = ASR(tmp0, 1) - ASR(tmp1, 1);
+			tmp0 = fabs(tmp0);
+			tmp1 = fabs(tmp1);
+			if (tmp0 != 0)
+				x |= tmp0 - 1;
+			if (tmp1 != 0)
+				y |= tmp1 - 1;
+		}
+		scale_factor[0][sb] = (31 - SCALE_OUT_BITS) -
+			sbc_clz(x);
+		scale_factor[1][sb] = (31 - SCALE_OUT_BITS) -
+			sbc_clz(y);
+		x = 1 << SCALE_OUT_BITS;
+		y = 1 << SCALE_OUT_BITS;
+		for (blk = 0; blk < blocks; blk++) {
+			tmp0 = fabs(sb_sample_j[blk][0]);
+			tmp1 = fabs(sb_sample_j[blk][1]);
+			if (tmp0 != 0)
+				x |= tmp0 - 1;
+			if (tmp1 != 0)
+				y |= tmp1 - 1;
+		}
+		x = (31 - SCALE_OUT_BITS) - sbc_clz(x);
+		y = (31 - SCALE_OUT_BITS) - sbc_clz(y);
+
+		/* decide whether to use joint stereo for this subband */
+		if ((scale_factor[0][sb] + scale_factor[1][sb]) > x + y) {
+			joint |= 1 << (subbands - 1 - sb);
+			scale_factor[0][sb] = x;
+			scale_factor[1][sb] = y;
+			for (blk = 0; blk < blocks; blk++) {
+				sb_sample_f[blk][0][sb] = sb_sample_j[blk][0];
+				sb_sample_f[blk][1][sb] = sb_sample_j[blk][1];
+			}
+		}
+	}
+
+	/* bitmask with the information about subbands using joint stereo */
+	return joint;
+}
+
 /*
  * Detect CPU features and setup function pointers
  */
@@ -456,6 +530,7 @@ void sbc_init_primitives(struct sbc_encoder_state *state)
 
 	/* Default implementation for scale factors calculation */
 	state->sbc_calc_scalefactors = sbc_calc_scalefactors;
+	state->sbc_calc_scalefactors_j = sbc_calc_scalefactors_j;
 	state->implementation_info = "Generic C";
 
 	/* X86/AMD64 optimizations */