[Sound-open-firmware] [PATCH 2/4] Tone: Add channel specific volume plus other fixes

[Seppo Ingalsuo]

The mono tone level can be controlled per channel. A similar channel map
feature as in the volume component was added into volume command of tone.

The default tone frequency of 997 Hz was updated to a proper Q16.16 value.

Moved tone function pointer set to tone_new() since it is not modified
later.

Signed-off-by: Seppo Ingalsuo <seppo.ingalsuo at linux.intel.com>
---
 src/audio/tone.c | 56 ++++++++++++++++++++++++++++++++++++++++++--------------
 1 file changed, 42 insertions(+), 14 deletions(-)

diff --git a/src/audio/tone.c b/src/audio/tone.c
index f45d8be..d1c7468 100644
--- a/src/audio/tone.c
+++ b/src/audio/tone.c
@@ -56,7 +56,8 @@
 
 #define TONE_NUM_FS            13       /* Table size for 8-192 kHz range */
 #define TONE_AMPLITUDE_DEFAULT 2147484  /* -60 dB from full scale */
-#define TONE_FREQUENCY_DEFAULT 16334848 /* 997 in Q18.14 */
+#define TONE_FREQUENCY_DEFAULT 65339392 /* 997 Hz in Q16.16 */
+#define TONE_VOLUME_DEFAULT 65536       /* 1.0 in Q16.16 */
 
 static int32_t tonegen(struct tone_state *sg);
 static void tonegen_control(struct tone_state *sg);
@@ -77,10 +78,11 @@ static const int32_t tone_pi2_div_fs[TONE_NUM_FS] = {
 
 /* TODO: Remove *source when internal endpoint is possible */
 struct comp_data {
+	enum sof_ipc_chmap chan[PLATFORM_MAX_CHANNELS];
+	int32_t volume[PLATFORM_MAX_CHANNELS];
 	struct tone_state sg;
 	void (*tone_func)(struct comp_dev *dev, struct comp_buffer *sink,
 		struct comp_buffer *source, uint32_t frames);
-
 };
 
 /*
@@ -109,8 +111,12 @@ static void tone_s32_default(struct comp_dev *dev, struct comp_buffer *sink,
 				 */
 				sine_sample = tonegen(&cd->sg);
 				n -= nch;
+				/* Multiply Q1.31 sine with channel specific 
+				 * Q16.16 volume. Stream is Q1.31. */
 				for (i = 0; i < nch; i++) {
-					*dest = sine_sample;
+					*dest = sat_int32(q_mults_32x32(
+						sine_sample,
+						cd->volume[i], 31, 16, 31));
 					dest++;
 				}
 			}
@@ -122,14 +128,17 @@ static void tone_s32_default(struct comp_dev *dev, struct comp_buffer *sink,
 				n -= nch;
 				n_wrap_dest -= nch;
 				for (i = 0; i < nch; i++) {
-					*dest = sine_sample;
+					*dest = sat_int32(q_mults_32x32(
+						sine_sample,
+						cd->volume[i], 31, 16, 31));
 					dest++;
 				}
 			}
 			/* No need to check if past end_addr,
 			 * it is so just subtract buffer size.
 			 */
-			dest = (int32_t *) ((size_t) dest - sink->alloc_size);
+			dest = (int32_t *) ((size_t) dest
+				- sink->ipc_buffer.size);
 		}
 	}
 	sink->w_ptr = dest;
@@ -364,6 +373,7 @@ static struct comp_dev *tone_new(struct sof_ipc_comp *comp)
 {
 	struct comp_dev *dev;
 	struct comp_data *cd;
+	int i;
 
 	trace_tone("TNw");
 	dev = rmalloc(RZONE_RUNTIME, RFLAGS_NONE, sizeof(*dev));
@@ -381,8 +391,12 @@ static struct comp_dev *tone_new(struct sof_ipc_comp *comp)
 
 	comp_set_drvdata(dev, cd);
 	comp_set_endpoint(dev);
+	cd->tone_func = tone_s32_default;
 
+	/* Reset tone generator and set channels volumes to default */
 	tonegen_reset(&cd->sg);
+	for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
+		cd->volume[i] = TONE_VOLUME_DEFAULT;
 
 	return dev;
 }
@@ -400,6 +414,9 @@ static void tone_free(struct comp_dev *dev)
 /* set component audio stream parameters */
 static int tone_params(struct comp_dev *dev, struct stream_params *params)
 {
+	int i;
+	struct comp_data *cd = comp_get_drvdata(dev);
+
 	trace_tone("TPa");
 
 	/* Tone generator supports only S32_LE PCM format */
@@ -407,6 +424,10 @@ static int tone_params(struct comp_dev *dev, struct stream_params *params)
 		|| (params->pcm->frame_fmt != SOF_IPC_FRAME_S32_LE))
 		return -EINVAL;
 
+	/* Copy channels map that is used by volume IPC */
+	for (i = 0; i < params->pcm->channels; i++)
+		cd->chan[i] = params->pcm->channel_map[i];
+
 	/* Don't do any data transformation */
 	comp_set_sink_params(dev, params);
 
@@ -417,22 +438,31 @@ static int tone_params(struct comp_dev *dev, struct stream_params *params)
 static int tone_cmd(struct comp_dev *dev, int cmd, void *data)
 {
 	struct comp_data *cd = comp_get_drvdata(dev);
-	struct sof_ipc_comp_tone *cv;
+	struct sof_ipc_comp_tone *ct;
+	struct sof_ipc_ctrl_values *cv;
+	int i, j;
 	trace_tone("TCm");
 
 	switch (cmd) {
 	case COMP_CMD_TONE:
 		trace_tone("Tto");
-		cv = (struct sof_ipc_comp_tone *) data;
+		ct = (struct sof_ipc_comp_tone *) data;
 		/* Ignore channels while tone implementation is mono */
-		tonegen_set_f(&cd->sg, cv->frequency);
-		tonegen_set_a(&cd->sg, cv->amplitude);
-		tonegen_set_sweep(&cd->sg, cv->freq_mult, cv->ampl_mult,
-			cv->length, cv->period, cv->repeats);
-		tonegen_set_linramp(&cd->sg, cv->ramp_step);
+		tonegen_set_f(&cd->sg, ct->frequency);
+		tonegen_set_a(&cd->sg, ct->amplitude);
+		tonegen_set_sweep(&cd->sg, ct->freq_mult, ct->ampl_mult,
+			ct->length, ct->period, ct->repeats);
+		tonegen_set_linramp(&cd->sg, ct->ramp_step);
 		break;
 	case COMP_CMD_VOLUME:
 		trace_tone("TVo");
+		cv = (struct sof_ipc_ctrl_values *) data;
+		for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) {
+			for (j = 0; j < cv->num_values; j++) {
+				if (cv->values[j].channel == cd->chan[i])
+					cd->volume[i] = cv->values[j].value;
+			}
+		}
 		break;
 	case COMP_CMD_MUTE:
 		trace_tone("TMu");
@@ -496,7 +526,6 @@ static int tone_copy(struct comp_dev *dev)
 	 */
 	need_sink = cframes * sink->params.pcm->frame_size;
 	if (sink->free >= need_sink) {
-
 		/* create tone */
 		cd->tone_func(dev, sink, source, cframes);
 	}
@@ -517,7 +546,6 @@ static int tone_prepare(struct comp_dev *dev)
 	trace_value(sink->params.pcm->channels);
 	trace_value(sink->params.pcm->rate);
 
-	cd->tone_func = tone_s32_default;
 	f = tonegen_get_f(&cd->sg);
 	a = tonegen_get_a(&cd->sg);
 	if (tonegen_init(&cd->sg, sink->params.pcm->rate, f, a) < 0)
-- 
2.11.0