[PATCH v2 5/7] sound: function to generate sine wave

[Heinrich Schuchardt]

Add function sound_create_sine_wave().

Signed-off-by: Heinrich Schuchardt <heinrich.schuchardt at canonical.com>
---
v2:
	remove stray printf()
---
 drivers/sound/sound.c | 37 +++++++++++++++++++++++++++++++++++++
 include/sound.h       | 12 ++++++++++++
 2 files changed, 49 insertions(+)

diff --git a/drivers/sound/sound.c b/drivers/sound/sound.c
index c0fc50c99d..abf9bb6de2 100644
--- a/drivers/sound/sound.c
+++ b/drivers/sound/sound.c
@@ -8,6 +8,43 @@
 #include <log.h>
 #include <sound.h>
 
+/* Amplitude between 1 and 32767 */
+#define AMP 16384
+#define AMP2PI ((int)(6.28318530718 * AMP))
+
+void sound_create_sine_wave(uint sample_rate, unsigned short *data, int size,
+			    uint freq, uint channels)
+{
+	int v, x = 0, y = AMP;
+
+	if (freq >= 0 && freq < sample_rate / 8) {
+		v = (AMP2PI * freq) / sample_rate;
+		/* tan(x) = x + x^3/3 + ... */
+		v += ((v * v) / AMP * v) / (3 * AMP);
+	} else {
+		v = 0;
+	}
+
+	for (int i = 0; i < size - (2 * channels - 1);) {
+		int s, dx, dy;
+
+		dx = (v * y) / AMP;
+		dy = -((v * x) / AMP);
+		x += dx;
+		y += dy;
+
+		/* Normalize radius: (1+x)^2 ~ 1+2x, for small x */
+		s = AMP * AMP - x * x - y * y;
+		s /= 2 * AMP;
+		s += AMP;
+		x = (s * x) / AMP;
+		y = (s * y) / AMP;
+
+		for (int j = 0; size && j < channels; ++j, i += 2)
+			*data++ = x;
+	}
+}
+
 void sound_create_square_wave(uint sample_rate, unsigned short *data, int size,
 			      uint freq, uint channels)
 {
diff --git a/include/sound.h b/include/sound.h
index dab9ea186e..cf9c3e8fb7 100644
--- a/include/sound.h
+++ b/include/sound.h
@@ -34,6 +34,18 @@ struct sound_uc_priv {
 	int setup_done;
 };
 
+/**
+ * Generates sine wave sound data for 1 second
+ *
+ * @sample_rate: Sample rate in Hz
+ * @data: data buffer pointer
+ * @size: size of the buffer in bytes
+ * @freq: frequency of the wave
+ * @channels: Number of channels to use
+ */
+void sound_create_sine_wave(uint sample_rate, unsigned short *data, int size,
+			    uint freq, uint channels);
+
 /**
  * Generates square wave sound data for 1 second
  *
-- 
2.37.2