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