Audio: TDFB: Track sound direction of arrival

src/audio/CMakeLists.txt

@@ -180,10 +180,10 @@ set(volume_sources volume/volume.c volume/volume_generic.c)
 set(src_sources src/src.c src/src_generic.c)
 set(asrc_sources asrc/asrc.c asrc/asrc_farrow.c asrc/asrc_farrow_generic.c)
 set(eq-fir_sources eq_fir/eq_fir.c eq_fir/eq_fir_generic.c)
-set(eq-iir_sources eq_iir/eq_iir.c eq_iir/iir.c)
+set(eq-iir_sources eq_iir/eq_iir.c)
 set(dcblock_sources dcblock/dcblock.c dcblock/dcblock_generic.c)
 set(crossover_sources crossover/crossover.c crossover/crossover_generic.c)
-set(tdfb_sources tdfb/tdfb.c tdfb/tdfb_generic.c)
+set(tdfb_sources tdfb/tdfb.c tdfb/tdfb_generic.c tdfb/tdfb_direction.c)
 set(drc_sources drc/drc.c drc/drc_generic.c drc/drc_math_generic.c)
 set(multiband_drc_sources multiband_drc/multiband_drc.c multiband_drc/multiband_drc_generic.c crossover/crossover.c crossover/crossover_generic.c drc/drc.c drc/drc_generic.c drc/drc_math_generic.c)
 

src/audio/Kconfig

@@ -387,6 +387,9 @@ endif # COMP_ASRC
 config COMP_TDFB
 	bool "TDFB component"
 	select MATH_FIR
+	select MATH_IIR_DF2T
+	select SQRT_FIXED
+	select CORDIC_FIXED
 	default y
 	help
 	  Select for time domain fixed beamformer (TDFB) component. The

src/audio/crossover/crossover.c

@@ -11,13 +11,13 @@
 #include <sof/audio/ipc-config.h>
 #include <sof/audio/crossover/crossover.h>
 #include <sof/audio/crossover/crossover_algorithm.h>
-#include <sof/audio/eq_iir/iir.h>
 #include <sof/common.h>
 #include <sof/debug/panic.h>
 #include <sof/ipc/msg.h>
 #include <sof/lib/alloc.h>
 #include <sof/lib/memory.h>
 #include <sof/lib/uuid.h>
+#include <sof/math/iir_df2t.h>
 #include <sof/list.h>
 #include <sof/platform.h>
 #include <sof/string.h>

src/audio/crossover/crossover_generic.c

@@ -6,10 +6,10 @@
 
 #include <stdint.h>
 #include <sof/audio/format.h>
-#include <sof/audio/eq_iir/iir.h>
 #include <sof/audio/component.h>
 #include <sof/audio/format.h>
 #include <sof/audio/crossover/crossover.h>
+#include <sof/math/iir_df2t.h>
 
 /*
  * \brief Splits x into two based on the coefficients set in the lp

src/audio/eq_iir/CMakeLists.txt

@@ -1,3 +1,3 @@
 # SPDX-License-Identifier: BSD-3-Clause
 
-add_local_sources(sof eq_iir.c iir.c)
+add_local_sources(sof eq_iir.c)

src/audio/eq_iir/eq_iir.c

@@ -9,7 +9,6 @@
 #include <sof/audio/component.h>
 #include <sof/audio/buffer.h>
 #include <sof/audio/eq_iir/eq_iir.h>
-#include <sof/audio/eq_iir/iir.h>
 #include <sof/audio/format.h>
 #include <sof/audio/pipeline.h>
 #include <sof/audio/ipc-config.h>

src/audio/multiband_drc/multiband_drc_generic.c

@@ -7,7 +7,6 @@
 #include <stdint.h>
 #include <sof/audio/drc/drc_algorithm.h>
 #include <sof/audio/format.h>
-#include <sof/audio/eq_iir/iir.h>
 #include <sof/audio/multiband_drc/multiband_drc.h>
 #include <sof/math/iir_df2t.h>
 

src/audio/tdfb/CMakeLists.txt

@@ -1,3 +1,3 @@
 # SPDX-License-Identifier: BSD-3-Clause
 
-add_local_sources(sof tdfb.c tdfb_generic.c tdfb_hifiep.c tdfb_hifi3.c)
+add_local_sources(sof tdfb.c tdfb_generic.c tdfb_hifiep.c tdfb_hifi3.c tdfb_direction.c)

src/audio/tdfb/tdfb.c

@@ -4,6 +4,11 @@
 //
 // Author: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>
 
+/* Note: The script tools/tune/tdfb/example_all.sh can be used to re-calculate
+ * all the beamformer topology data files if need. It also creates the additional
+ * data files for simulated tests with testbench. Matlab or Octave is needed.
+ */
+
 #include <ipc/control.h>
 #include <ipc/stream.h>
 #include <ipc/topology.h>
@@ -179,6 +184,17 @@ static int16_t *tdfb_filter_seek(struct sof_tdfb_config *config, int num_filters
 	return coefp;
 }
 
+static int wrap_180(int a)
+{
+	if (a > 180)
+		return ((a + 180) % 360) - 180;
+
+	if (a < -180)
+		return 180 - ((180 - a) % 360);
+
+	return a;
+}
+
 static int tdfb_init_coef(struct tdfb_comp_data *cd, int source_nch,
 			  int sink_nch)
 {
@@ -275,15 +291,10 @@ static int tdfb_init_coef(struct tdfb_comp_data *cd, int source_nch,
 	/* Skip to requested coefficient set */
 	min_delta = 360;
 	min_delta_idx = 0;
-	target_az = cd->az_value * config->angle_enum_mult + config->angle_enum_offs;
-	if (target_az > 180)
-		target_az -= 360;
-
-	if (target_az < -180)
-		target_az += 360;
+	target_az = wrap_180(cd->az_value * config->angle_enum_mult + config->angle_enum_offs);
 
 	for (i = 0; i < config->num_angles; i++) {
-		delta = ABS(target_az - cd->filter_angles[i].azimuth);
+		delta = ABS(target_az - wrap_180(cd->filter_angles[i].azimuth));
 		if (delta < min_delta) {
 			min_delta = delta;
 			min_delta_idx = i;
@@ -479,7 +490,7 @@ static void tdfb_free(struct comp_dev *dev)
 	ipc_msg_free(cd->msg);
 	tdfb_free_delaylines(cd);
 	comp_data_blob_handler_free(cd->model_handler);
-
+	tdfb_direction_free(cd);
 	rfree(cd->ctrl_data);
 	rfree(cd);
 	rfree(dev);
@@ -653,11 +664,6 @@ static int tdfb_cmd(struct comp_dev *dev, int cmd, void *data,
 	return -EINVAL;
 }
 
-/* Placeholder function for sound direction estimate */
-static void update_direction_of_arrival(struct tdfb_comp_data *cd)
-{
-}
-
 static void tdfb_process(struct comp_dev *dev, struct comp_buffer *source,
 			 struct comp_buffer *sink, int frames,
 			 uint32_t source_bytes, uint32_t sink_bytes)
@@ -673,6 +679,11 @@ static void tdfb_process(struct comp_dev *dev, struct comp_buffer *source,
 	/* calc new free and available */
 	comp_update_buffer_consume(source, source_bytes);
 	comp_update_buffer_produce(sink, sink_bytes);
+
+	/* Update sound direction estimate */
+	tdfb_direction_estimate(cd, frames, source->stream.channels);
+	comp_dbg(dev, "tdfb_dint %u %d %d %d", cd->direction.trigger, cd->direction.level,
+		 (int32_t)(cd->direction.level_ambient >> 32), cd->direction.az_slow);
 }
 
 /* copy and process stream data from source to sink buffers */
@@ -720,6 +731,7 @@ static int tdfb_copy(struct comp_dev *dev)
 	 * optimized filter function loads the successive input samples from
 	 * internal delay line with a 64 bit load operation.
 	 */
+	cl.frames = MIN(cl.frames, cd->max_frames);
 	if (cl.frames >= 2) {
 		n = (cl.frames >> 1) << 1;
 
@@ -729,10 +741,11 @@ static int tdfb_copy(struct comp_dev *dev)
 			     n * cl.sink_frame_bytes);
 	}
 
-	/* TODO: Update direction of arrival estimate */
-	update_direction_of_arrival(cd);
-	if (cd->direction_updates && cd->direction_change)
+	if (cd->direction_updates && cd->direction_change) {
 		send_get_ctl_ipc(dev);
+		cd->direction_change = false;
+		comp_dbg(dev, "tdfb_dupd %d %d", cd->az_value_estimate, cd->direction.az_slow);
+	}
 
 	return 0;
 }
@@ -761,19 +774,40 @@ static int tdfb_prepare(struct comp_dev *dev)
 
 	/* Initialize filter */
 	cd->config = comp_get_data_blob(cd->model_handler, NULL, NULL);
-	if (cd->config) {
-		ret = tdfb_setup(cd, sourceb->stream.channels, sinkb->stream.channels);
-		if (ret < 0) {
-			comp_err(dev, "tdfb_prepare() error: tdfb_setup failed.");
-			goto err;
-		}
+	if (!cd->config) {
+		ret = -EINVAL;
+		goto err;
+	}
 
-		/* Clear in/out buffers */
-		memset(cd->in, 0, TDFB_IN_BUF_LENGTH * sizeof(int32_t));
-		memset(cd->out, 0, TDFB_IN_BUF_LENGTH * sizeof(int32_t));
+	ret = tdfb_setup(cd, sourceb->stream.channels, sinkb->stream.channels);
+	if (ret < 0) {
+		comp_err(dev, "tdfb_prepare() error: tdfb_setup failed.");
+		goto err;
+	}
 
-		ret = set_func(dev);
-		return ret;
+	/* Clear in/out buffers */
+	memset(cd->in, 0, TDFB_IN_BUF_LENGTH * sizeof(int32_t));
+	memset(cd->out, 0, TDFB_IN_BUF_LENGTH * sizeof(int32_t));
+
+	ret = set_func(dev);
+	if (ret)
+		goto err;
+
+	/* The max. amount of processing need to be limited for sound direction
+	 * processing. Max frames is used in tdfb_direction_init() and copy().
+	 */
+	cd->max_frames = Q_MULTSR_32X32(dev->frames, TDFB_MAX_FRAMES_MULT_Q14, 0, 14, 0);
+	comp_info(dev, "dev_frames = %d, max_frames = %d", dev->frames, cd->max_frames);
+
+	/* Initialize tracking */
+	ret = tdfb_direction_init(cd, sourceb->stream.rate, sourceb->stream.channels);
+	if (!ret) {
+		comp_info(dev, "max_lag = %d, xcorr_size = %d",
+			  cd->direction.max_lag, cd->direction.d_size);
+		comp_info(dev, "line_array = %d, a_step = %d, a_offs = %d",
+			  (int)cd->direction.line_array, cd->config->angle_enum_mult,
+			  cd->config->angle_enum_offs);
+		return 0;
 	}
 
 err: