applications: nrf_audio: refactor pscm_deinterleave()

Refactored pscm_deinterleave() function to use uint16_t/uint32_t
types instead of copying byte-by-byte.
It reduced time from typical 75 us to 23 us when using 2 16-bit channels
of 960 bytes each (default values in USB BIS source). The timing were
measured with pin-toggling during transmission of sine wave.
Saves around 1% of CPU time in USB BIS source case.

Signed-off-by: Ivan Iushkov <ivan.iushkov@nordicsemi.no>

Author: ivaniushkov

include/pcm_stream_channel_modifier.h

@@ -131,15 +131,16 @@ int pscm_interleave(void const *const input, size_t input_size, uint8_t channel,
 
 /**
  * @brief  De-interleave a channel from a buffer of N channels of PCM
- * @note: The de-interleaver can not be executed inplace (i.e. input != output)
+ * @note:  The de-interleaver can not be executed inplace (i.e. input != output)
  *
- * @param[in]	input				Pointer to the multi channel input buffer.
- * @param[in]	input_size			Number of bytes in input.
+ * @param[in]	input			Pointer to the multi channel input buffer.
+ *					Should be 4-bytes aligned.
+ * @param[in]	input_size		Number of bytes in input.
  * @param[in]	input_channels		Number of channels in the input buffer.
- * @param[in]	channel				Channel to de-interleave.
+ * @param[in]	channel			Channel to de-interleave.
  * @param[in]	pcm_bit_depth		Bit depth of PCM samples (8, 16, 24, or 32).
- * @param[out]	output				Pointer to the channel output.
- * @param[in]	output_size			Number of bytes in output. Must be at least
+ * @param[out]	output			Pointer to the channel output. Should be 4-bytes aligned.
+ * @param[in]	output_size		Number of bytes in output. Must be at least
  *					(input_size / output_channels).
  *
  * @return	0 if successful, error value

lib/pcm_stream_channel_modifier/pcm_stream_channel_modifier.c

@@ -262,14 +262,12 @@ int pscm_interleave(void const *const input, size_t input_size, uint8_t channel,
 int pscm_deinterleave(void const *const input, size_t input_size, uint8_t input_channels,
 		      uint8_t channel, uint8_t pcm_bit_depth, void *output, size_t output_size)
 {
-	uint8_t bytes_per_sample;
-	size_t step;
-	uint8_t *pointer_input;
-	uint8_t *pointer_output;
+	size_t bytes_to_copy;
 
 	if (input == NULL || output == NULL || input_size == 0 || channel >= input_channels ||
 	    pcm_bit_depth == 0 || pcm_bit_depth % 8 || output_size == 0 ||
-	    pcm_bit_depth > PSCM_MAX_CARRIER_BIT_DEPTH || input_channels == 0) {
+	    pcm_bit_depth > PSCM_MAX_CARRIER_BIT_DEPTH || input_channels == 0 ||
+	    !IS_ALIGNED(input, 4) || !IS_ALIGNED(output, 4)) {
 		return -EINVAL;
 	}
 
@@ -278,17 +276,41 @@ int pscm_deinterleave(void const *const input, size_t input_size, uint8_t input_
 		return -EINVAL;
 	}
 
-	bytes_per_sample = pcm_bit_depth / 8;
-	step = bytes_per_sample * (input_channels - 1);
-	pointer_input = (uint8_t *)input + (bytes_per_sample * channel);
-	pointer_output = (uint8_t *)output;
+	uint8_t bytes_per_sample = pcm_bit_depth / 8;
+	/*
+	 * Use types corresponding to pcm_bit_depth to make iterating over an array faster
+	 * when pcm_bit_depth is 16 or 32. Use uint8_t in 8/24 bits case.
+	 */
+	bytes_to_copy = input_size / input_channels;
+	if (bytes_per_sample == sizeof(uint16_t)) {
+		uint16_t *output_16 = (uint16_t *)output;
+		const uint16_t *input_16 = (const uint16_t *)input + channel;
+		uint16_t *output_16_end = output_16 + (bytes_to_copy / sizeof(uint16_t));
 
-	for (size_t i = 0; i < input_size; i += (step + bytes_per_sample)) {
-		for (size_t j = 0; j < bytes_per_sample; j++) {
-			*pointer_output++ = *pointer_input++;
+		while (output_16 < output_16_end) {
+			*output_16++ = *input_16;
+			input_16 += input_channels;
 		}
+	} else if (bytes_per_sample == sizeof(uint32_t)) {
+		uint32_t *output_32 = (uint32_t *)output;
+		const uint32_t *input_32 = (const uint32_t *)input + channel;
+		uint32_t *output_32_end = output_32 + (bytes_to_copy / sizeof(uint32_t));
 
-		pointer_input += step;
+		while (output_32 < output_32_end) {
+			*output_32++ = *input_32;
+			input_32 += input_channels;
+		}
+	} else {
+		uint8_t *output_8 = (uint8_t *)output;
+		const uint8_t *input_8 = (const uint8_t *)input + (channel * bytes_per_sample);
+		size_t step = bytes_per_sample * (input_channels - 1);
+
+		for (size_t i = 0; i < bytes_to_copy; i += bytes_per_sample) {
+			for (uint8_t j = 0; j < bytes_per_sample; j++) {
+				*output_8++ = *input_8++;
+			}
+			input_8 += step;
+		}
 	}
 
 	return 0;

tests/lib/pcm_stream_channel_modifier/src/main.c

@@ -45,10 +45,11 @@ uint8_t __aligned(4) unpadded_surround_left[] = {37, 38, 39, 40, 41, 42, 43, 44,
 uint8_t __aligned(4) unpadded_surround_right[] = {49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60};
 uint8_t stereo_split[] = {1, 13, 2, 14, 3, 15, 4,  16, 5,  17, 6,  18,
 			  7, 19, 8, 20, 9, 21, 10, 22, 11, 23, 12, 24};
-uint8_t multi_split[] = {1,  13, 25, 37, 49, 2,	 14, 26, 38, 50, 3,  15, 27, 39, 51,
-			 4,  16, 28, 40, 52, 5,	 17, 29, 41, 53, 6,  18, 30, 42, 54,
-			 7,  19, 31, 43, 55, 8,	 20, 32, 44, 56, 9,  21, 33, 45, 57,
-			 10, 22, 34, 46, 58, 11, 23, 35, 47, 59, 12, 24, 36, 48, 60};
+uint8_t __aligned(4) multi_split[] = {
+	1,  13, 25, 37, 49, 2,  14, 26, 38, 50, 3,  15, 27, 39, 51,
+	4,  16, 28, 40, 52, 5,  17, 29, 41, 53, 6,  18, 30, 42, 54,
+	7,  19, 31, 43, 55, 8,  20, 32, 44, 56, 9,  21, 33, 45, 57,
+	10, 22, 34, 46, 58, 11, 23, 35, 47, 59, 12, 24, 36, 48, 60};
 
 /* Result arrays */
 uint8_t left_zero_padded_8[] = {1, 0, 2, 0, 3, 0, 4,  0, 5,  0, 6,  0,
@@ -57,17 +58,17 @@ uint8_t right_zero_padded_8[] = {0, 13, 0, 14, 0, 15, 0, 16, 0, 17, 0, 18,
 				 0, 19, 0, 20, 0, 21, 0, 22, 0, 23, 0, 24};
 uint8_t copy_padded_8[] = {1, 1, 2, 2, 3, 3, 4,	 4,  5,	 5,  6,	 6,
 			   7, 7, 8, 8, 9, 9, 10, 10, 11, 11, 12, 12};
-uint8_t combine_8_ch2[] = {1, 13, 2, 14, 3, 15, 4,  16, 5,  17, 6,  18,
-			   7, 19, 8, 20, 9, 21, 10, 22, 11, 23, 12, 24};
+uint8_t __aligned(4) combine_8_ch2[] = {1, 13, 2, 14, 3, 15, 4,  16, 5,  17, 6,  18,
+					7, 19, 8, 20, 9, 21, 10, 22, 11, 23, 12, 24};
 
 uint8_t left_zero_padded_16[] = {1, 2, 0, 0, 3, 4,  0, 0, 5,  6,  0, 0,
 				 7, 8, 0, 0, 9, 10, 0, 0, 11, 12, 0, 0};
 uint8_t right_zero_padded_16[] = {0, 0, 1, 2, 0, 0, 3, 4,  0, 0, 5,  6,
 				  0, 0, 7, 8, 0, 0, 9, 10, 0, 0, 11, 12};
 uint8_t copy_padded_16[] = {1, 2, 1, 2, 3, 4,  3, 4,  5,  6,  5,  6,
 			    7, 8, 7, 8, 9, 10, 9, 10, 11, 12, 11, 12};
-uint8_t combine_16[] = {1, 2, 13, 14, 3, 4,  15, 16, 5,	 6,  17, 18,
-			7, 8, 19, 20, 9, 10, 21, 22, 11, 12, 23, 24};
+uint8_t __aligned(4) combine_16[] = {1, 2, 13, 14, 3, 4,  15, 16, 5,  6,  17, 18,
+					     7, 8, 19, 20, 9, 10, 21, 22, 11, 12, 23, 24};
 uint8_t stereo_split_left_16[] = {1, 13, 3, 15, 5, 17, 7, 19, 9, 21, 11, 23};
 uint8_t stereo_split_right_16[] = {2, 14, 4, 16, 6, 18, 8, 20, 10, 22, 12, 24};
 
@@ -77,8 +78,8 @@ uint8_t right_zero_padded_24[] = {0, 0, 0, 1, 2, 3, 0, 0, 0, 4,	 5,  6,
 				  0, 0, 0, 7, 8, 9, 0, 0, 0, 10, 11, 12};
 uint8_t copy_padded_24[] = {1, 2, 3, 1, 2, 3, 4,  5,  6,  4,  5,  6,
 			    7, 8, 9, 7, 8, 9, 10, 11, 12, 10, 11, 12};
-uint8_t combine_24[] = {1, 2, 3, 13, 14, 15, 4,	 5,  6,	 16, 17, 18,
-			7, 8, 9, 19, 20, 21, 10, 11, 12, 22, 23, 24};
+uint8_t __aligned(4) combine_24[] = {1, 2, 3, 13, 14, 15, 4,  5,  6,  16, 17, 18,
+					     7, 8, 9, 19, 20, 21, 10, 11, 12, 22, 23, 24};
 uint8_t stereo_split_left_24[] = {1, 13, 2, 4, 16, 5, 7, 19, 8, 10, 22, 11};
 uint8_t stereo_split_right_24[] = {14, 3, 15, 17, 6, 18, 20, 9, 21, 23, 12, 24};
 
@@ -88,8 +89,8 @@ uint8_t right_zero_padded_32[] = {0, 0, 0, 0, 1, 2, 3, 4, 0, 0,	 0,  0,
 				  5, 6, 7, 8, 0, 0, 0, 0, 9, 10, 11, 12};
 uint8_t copy_padded_32[] = {1, 2, 3, 4, 1, 2,  3,  4,  5, 6,  7,  8,
 			    5, 6, 7, 8, 9, 10, 11, 12, 9, 10, 11, 12};
-uint8_t combine_32[] = {1,  2,	3,  4,	13, 14, 15, 16, 5,  6,	7,  8,
-			17, 18, 19, 20, 9,  10, 11, 12, 21, 22, 23, 24};
+uint8_t __aligned(4) combine_32[] = {1,  2,  3,  4,  13, 14, 15, 16, 5,  6,  7,  8,
+					     17, 18, 19, 20, 9,  10, 11, 12, 21, 22, 23, 24};
 uint8_t stereo_split_left_32[] = {1, 13, 2, 14, 5, 17, 6, 18, 9, 21, 10, 22};
 uint8_t stereo_split_right_32[] = {3, 15, 4, 16, 7, 19, 8, 20, 11, 23, 12, 24};
 
@@ -160,7 +161,7 @@ ZTEST(suite_pscm_int, test_pscm_interleave_api_parameters)
 ZTEST(suite_pscm_deint, test_pscm_deinterleave_api_parameters)
 {
 	int ret;
-	uint8_t input[2], output[2];
+	uint8_t __aligned(4) input[2], output[2];
 	size_t input_size = sizeof(input);
 	size_t output_size = sizeof(output);
 	size_t test_output_size = 0;
@@ -205,6 +206,17 @@ ZTEST(suite_pscm_deint, test_pscm_deinterleave_api_parameters)
 	ret = pscm_deinterleave(input, input_size, input_channels, channel, pcm_bit_depth, &output,
 				test_output_size);
 	zassert_equal(ret, -EINVAL, "Failed de-interleave for output size too small: ret %d", ret);
+
+	uint8_t *misaligned_input = (uint8_t *)input + 1;
+	uint8_t *misaligned_output = (uint8_t *)output + 1;
+
+	ret = pscm_deinterleave(misaligned_input, input_size, input_channels, channel,
+				pcm_bit_depth, output, output_size);
+	zassert_equal(ret, -EINVAL, "Failed de-interleave for misaligned input: ret %d", ret);
+
+	ret = pscm_deinterleave(input, input_size, input_channels, channel, pcm_bit_depth,
+				misaligned_output, output_size);
+	zassert_equal(ret, -EINVAL, "Failed de-interleave for misaligned output: ret %d", ret);
 }
 
 int interleave_test(void const *const input, size_t input_size, uint8_t pcm_bit_depth,
@@ -233,7 +245,7 @@ int deinterleave_test(void const *const input, size_t input_size, uint8_t pcm_bi
 		      size_t test_result_size)
 {
 	int ret;
-	uint8_t output[TEST_PCM_DEINT_SIZE];
+	uint8_t __aligned(4) output[TEST_PCM_DEINT_SIZE];
 	size_t output_size = sizeof(output);
 
 	ret = pscm_deinterleave(input, input_size, input_channels, channel, pcm_bit_depth, &output,