1 /*
2 * Copyright (C) 2015 Caleb Crome
3 * Copyright (C) 2013-2015 Intel Corporation
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 */
16
17 /*
18 * This is a general purpose sine wave generator that will stay stable
19 * for a long time, and with a little renormalization, could stay stay
20 * stable indefinitely
21 */
22
23 #include "aconfig.h"
24
25 #include <stdio.h>
26 #include <stddef.h>
27 #include <stdlib.h>
28 #include <string.h>
29 #include <math.h>
30 #include <stdint.h>
31 #include <stdbool.h>
32 #include <errno.h>
33
34 #include "gettext.h"
35 #include "common.h"
36 #include "signal.h"
37
38 /*
39 * Initialize the sine wave generator.
40 * sin_generator: gets initialized by this call.
41 * frequency: the frequency for the sine wave. must be < 0.5*sample_rate
42 * sample_rate: the sample rate...
43 * returns 0 on success, -1 on error.
44 */
sin_generator_init(struct sin_generator * sg,float magnitude,float frequency,float sample_rate)45 int sin_generator_init(struct sin_generator *sg, float magnitude,
46 float frequency, float sample_rate)
47 {
48 /* angular frequency: cycles/sec / (samp/sec) * rad/cycle = rad/samp */
49 float w = frequency / sample_rate * 2 * M_PI;
50 if (frequency >= sample_rate / 2)
51 return -1;
52 sg->phasor_real = cos(w);
53 sg->phasor_imag = sin(w);
54 sg->magnitude = magnitude;
55 sg->state_real = 0.0;
56 sg->state_imag = magnitude;
57 sg->frequency = frequency;
58 sg->sample_rate = sample_rate;
59 return 0;
60 }
61
62 /*
63 * Generates the next sample in the sine wave.
64 * should be much faster than calling a sin function
65 * if it's inlined and optimized.
66 *
67 * returns the next value. no possibility of error.
68 */
sin_generator_next_sample(struct sin_generator * sg)69 float sin_generator_next_sample(struct sin_generator *sg)
70 {
71 /* get shorthand to pointers */
72 const double pr = sg->phasor_real;
73 const double pi = sg->phasor_imag;
74 const double sr = sg->state_real;
75 const double si = sg->state_imag;
76 /* step the phasor -- complex multiply */
77 sg->state_real = sr * pr - si * pi;
78 sg->state_imag = sr * pi + pr * si;
79 /* return the input value so sine wave starts at exactly 0.0 */
80 return (float)sr;
81 }
82
83 /* fills a vector with a sine wave */
sin_generator_vfill(struct sin_generator * sg,float * buf,int n)84 void sin_generator_vfill(struct sin_generator *sg, float *buf, int n)
85 {
86 int i;
87 for (i = 0; i < n; i++)
88 *buf++ = sin_generator_next_sample(sg);
89 }
90
reorder(struct bat * bat,float * val,int frames)91 static int reorder(struct bat *bat, float *val, int frames)
92 {
93 float *new_buf = NULL;
94 int i, c, bytes;
95
96 bytes = frames * bat->channels * sizeof(float);
97
98 new_buf = (float *) malloc(bytes);
99 if (new_buf == NULL) {
100 fprintf(bat->err, _("Not enough memory.\n"));
101 return -ENOMEM;
102 }
103
104 memcpy(new_buf, val, bytes);
105 for (i = 0; i < frames; i++)
106 for (c = 0; c < bat->channels; c++)
107 val[i * bat->channels + c] =
108 new_buf[c * frames + i];
109 free(new_buf);
110
111 return 0;
112 }
113
adjust_waveform(struct bat * bat,float * val,int frames,int channels)114 static int adjust_waveform(struct bat *bat, float *val, int frames,
115 int channels)
116 {
117 int i, nsamples, max;
118 float factor, offset = 0.0;
119
120 switch (bat->format) {
121 case BAT_PCM_FORMAT_U8:
122 max = INT8_MAX;
123 offset = max; /* shift for unsigned format */
124 break;
125 case BAT_PCM_FORMAT_S16_LE:
126 max = INT16_MAX;
127 break;
128 case BAT_PCM_FORMAT_S24_3LE:
129 max = (1 << 23) - 1;
130 break;
131 case BAT_PCM_FORMAT_S32_LE:
132 max = INT32_MAX;
133 break;
134 default:
135 fprintf(bat->err, _("Invalid PCM format: %d\n"), bat->format);
136 return -EINVAL;
137 }
138
139 factor = max * RANGE_FACTOR;
140 nsamples = channels * frames;
141
142 for (i = 0; i < nsamples; i++)
143 val[i] = val[i] * factor + offset;
144
145 return 0;
146 }
147
generate_sine_wave(struct bat * bat,int frames,void * buf)148 int generate_sine_wave(struct bat *bat, int frames, void *buf)
149 {
150 int err = 0;
151 int c, nsamples;
152 float *sinus_f = NULL;
153 static struct sin_generator sg[MAX_CHANNELS];
154
155 nsamples = bat->channels * frames;
156 sinus_f = (float *) malloc(nsamples * sizeof(float));
157 if (sinus_f == NULL) {
158 fprintf(bat->err, _("Not enough memory.\n"));
159 return -ENOMEM;
160 }
161
162 for (c = 0; c < bat->channels; c++) {
163 /* initialize static struct at the first time */
164 if (sg[c].frequency != bat->target_freq[c])
165 sin_generator_init(&sg[c], 1.0, bat->target_freq[c],
166 bat->rate);
167 /* fill buffer for each channel */
168 sin_generator_vfill(&sg[c], sinus_f + c * frames, frames);
169 }
170
171 /* reorder samples to interleaved mode */
172 err = reorder(bat, sinus_f, frames);
173 if (err != 0)
174 goto exit;
175
176 /* adjust amplitude and offset of waveform */
177 err = adjust_waveform(bat, sinus_f, frames, bat->channels);
178 if (err != 0)
179 goto exit;
180
181 bat->convert_float_to_sample(sinus_f, buf, frames, bat->channels);
182
183 exit:
184 free(sinus_f);
185
186 return err;
187 }
188
189 /* generate single channel sine waveform without sample conversion */
generate_sine_wave_raw_mono(struct bat * bat,float * buf,float freq,int nsamples)190 int generate_sine_wave_raw_mono(struct bat *bat, float *buf,
191 float freq, int nsamples)
192 {
193 int err = 0;
194 struct sin_generator sg;
195
196 err = sin_generator_init(&sg, 1.0, freq, bat->rate);
197 if (err < 0)
198 return err;
199 sin_generator_vfill(&sg, buf, nsamples);
200
201 /* adjust amplitude and offset of waveform */
202 err = adjust_waveform(bat, buf, nsamples, 1);
203
204 return err;
205 }
206