signal.c (5164B)
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 */ 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 */ 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 */ 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 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 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 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 */ 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 }