1 /*
2 This software is part of pffft/pfdsp, a set of simple DSP routines.
3
4 Copyright (c) 2014, Andras Retzler <randras@sdr.hu>
5 Copyright (c) 2020 Hayati Ayguen <h_ayguen@web.de>
6 All rights reserved.
7
8 Redistribution and use in source and binary forms, with or without
9 modification, are permitted provided that the following conditions are met:
10 * Redistributions of source code must retain the above copyright
11 notice, this list of conditions and the following disclaimer.
12 * Redistributions in binary form must reproduce the above copyright
13 notice, this list of conditions and the following disclaimer in the
14 documentation and/or other materials provided with the distribution.
15 * Neither the name of the copyright holder nor the
16 names of its contributors may be used to endorse or promote products
17 derived from this software without specific prior written permission.
18
19 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 DISCLAIMED. IN NO EVENT SHALL ANDRAS RETZLER BE LIABLE FOR ANY
23 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
24 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
26 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
28 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29 */
30
31 /* include own header first, to see missing includes */
32 #include "pf_cic.h"
33 #include "fmv.h"
34
35 #include <math.h>
36 #include <stdlib.h>
37 #include <string.h>
38 #include <limits.h>
39
40
41 /*
42 ____ ___ ____ ____ ____ ____
43 / ___|_ _/ ___| | _ \| _ \ / ___|
44 | | | | | | | | | | | | |
45 | |___ | | |___ | |_| | |_| | |___
46 \____|___\____| |____/|____/ \____|
47 */
48
49 #define SINESHIFT 12
50 #define SINESIZE (1<<SINESHIFT)
51 typedef int64_t cic_dt; // data type used for integrators and combs
52 typedef struct {
53 int factor;
54 uint64_t phase;
55 float gain;
56 cic_dt ig0a, ig0b, ig1a, ig1b;
57 cic_dt comb0a, comb0b, comb1a, comb1b;
58 int16_t *sinetable;
59 } cicddc_t;
60
cicddc_init(int factor)61 void *cicddc_init(int factor) {
62 int i;
63 int sinesize2 = SINESIZE * 5/4; // 25% extra to get cosine from the same table
64 cicddc_t *s;
65 s = (cicddc_t *)malloc(sizeof(cicddc_t));
66 memset(s, 0, sizeof(cicddc_t));
67
68 float sineamp = 32767.0f;
69 s->factor = factor;
70 s->gain = 1.0f / SHRT_MAX / sineamp / factor / factor / factor; // compensate for gain of 3 integrators
71
72 s->sinetable = (int16_t *)malloc(sinesize2 * sizeof(*s->sinetable));
73 double f = 2.0*M_PI / (double)SINESIZE;
74 for(i = 0; i < sinesize2; i++) {
75 s->sinetable[i] = sineamp * cos(f * i);
76 }
77 return s;
78 }
79
cicddc_free(void * state)80 void cicddc_free(void *state) {
81 cicddc_t *s = (cicddc_t *)state;
82 free(s->sinetable);
83 free(s);
84 }
85
86
87 PF_TARGET_CLONES
cicddc_s16_c(void * state,int16_t * input,complexf * output,int outsize,float rate)88 void cicddc_s16_c(void *state, int16_t *input, complexf *output, int outsize, float rate) {
89 cicddc_t *s = (cicddc_t *)state;
90 int k;
91 int factor = s->factor;
92 cic_dt ig0a = s->ig0a, ig0b = s->ig0b, ig1a = s->ig1a, ig1b = s->ig1b;
93 cic_dt comb0a = s->comb0a, comb0b = s->comb0b, comb1a = s->comb1a, comb1b = s->comb1b;
94 uint64_t phase = s->phase, freq;
95 int16_t *sinetable = s->sinetable;
96 float gain = s->gain;
97
98 freq = rate * ((float)(1ULL << 63) * 2);
99
100 int16_t *inp = input;
101 for(k = 0; k < outsize; k++) {
102 int i;
103 cic_dt out0a, out0b, out1a, out1b;
104 cic_dt ig2a = 0, ig2b = 0; // last integrator and first comb replaced simply by sum
105 for(i = 0; i < factor; i++) {
106 cic_dt in_a, in_b;
107 int sinep = phase >> (64-SINESHIFT);
108 in_a = (int32_t)inp[i] * (int32_t)sinetable[sinep + (1<<(SINESHIFT-2))];
109 in_b = (int32_t)inp[i] * (int32_t)sinetable[sinep];
110 phase += freq;
111 /* integrators:
112 The calculations are ordered so that each integrator
113 takes a result from previous loop iteration
114 to make the code more "pipeline-friendly". */
115 ig2a += ig1a; ig2b += ig1b;
116 ig1a += ig0a; ig1b += ig0b;
117 ig0a += in_a; ig0b += in_b;
118 }
119 inp += factor;
120 // comb filters:
121 out0a = ig2a - comb0a; out0b = ig2b - comb0b;
122 comb0a = ig2a; comb0b = ig2b;
123 out1a = out0a - comb1a; out1b = out0b - comb1b;
124 comb1a = out0a; comb1b = out0b;
125
126 output[k].i = (float)out1a * gain;
127 output[k].q = (float)out1b * gain;
128 }
129
130 s->ig0a = ig0a; s->ig0b = ig0b;
131 s->ig1a = ig1a; s->ig1b = ig1b;
132 s->comb0a = comb0a; s->comb0b = comb0b;
133 s->comb1a = comb1a; s->comb1b = comb1b;
134 s->phase = phase;
135 }
136
137 PF_TARGET_CLONES
cicddc_cs16_c(void * state,int16_t * input,complexf * output,int outsize,float rate)138 void cicddc_cs16_c(void *state, int16_t *input, complexf *output, int outsize, float rate) {
139 cicddc_t *s = (cicddc_t *)state;
140 int k;
141 int factor = s->factor;
142 cic_dt ig0a = s->ig0a, ig0b = s->ig0b, ig1a = s->ig1a, ig1b = s->ig1b;
143 cic_dt comb0a = s->comb0a, comb0b = s->comb0b, comb1a = s->comb1a, comb1b = s->comb1b;
144 uint64_t phase = s->phase, freq;
145 int16_t *sinetable = s->sinetable;
146 float gain = s->gain;
147
148 freq = rate * ((float)(1ULL << 63) * 2);
149
150 int16_t *inp = input;
151 for(k = 0; k < outsize; k++) {
152 int i;
153 cic_dt out0a, out0b, out1a, out1b;
154 cic_dt ig2a = 0, ig2b = 0; // last integrator and first comb replaced simply by sum
155 for(i = 0; i < factor; i++) {
156 cic_dt in_a, in_b;
157 int32_t m_a, m_b, m_c, m_d;
158 int sinep = phase >> (64-SINESHIFT);
159 m_a = inp[2*i];
160 m_b = inp[2*i+1];
161 m_c = (int32_t)sinetable[sinep + (1<<(SINESHIFT-2))];
162 m_d = (int32_t)sinetable[sinep];
163 // complex multiplication:
164 in_a = m_a*m_c - m_b*m_d;
165 in_b = m_a*m_d + m_b*m_c;
166 phase += freq;
167 /* integrators:
168 The calculations are ordered so that each integrator
169 takes a result from previous loop iteration
170 to make the code more "pipeline-friendly". */
171 ig2a += ig1a; ig2b += ig1b;
172 ig1a += ig0a; ig1b += ig0b;
173 ig0a += in_a; ig0b += in_b;
174 }
175 inp += 2*factor;
176 // comb filters:
177 out0a = ig2a - comb0a; out0b = ig2b - comb0b;
178 comb0a = ig2a; comb0b = ig2b;
179 out1a = out0a - comb1a; out1b = out0b - comb1b;
180 comb1a = out0a; comb1b = out0b;
181
182 output[k].i = (float)out1a * gain;
183 output[k].q = (float)out1b * gain;
184 }
185
186 s->ig0a = ig0a; s->ig0b = ig0b;
187 s->ig1a = ig1a; s->ig1b = ig1b;
188 s->comb0a = comb0a; s->comb0b = comb0b;
189 s->comb1a = comb1a; s->comb1b = comb1b;
190 s->phase = phase;
191 }
192
193
194 /* This is almost copy paste from cicddc_cs16_c.
195 I'm afraid this is going to be annoying to maintain... */
196 PF_TARGET_CLONES
cicddc_cu8_c(void * state,uint8_t * input,complexf * output,int outsize,float rate)197 void cicddc_cu8_c(void *state, uint8_t *input, complexf *output, int outsize, float rate) {
198 cicddc_t *s = (cicddc_t *)state;
199 int k;
200 int factor = s->factor;
201 cic_dt ig0a = s->ig0a, ig0b = s->ig0b, ig1a = s->ig1a, ig1b = s->ig1b;
202 cic_dt comb0a = s->comb0a, comb0b = s->comb0b, comb1a = s->comb1a, comb1b = s->comb1b;
203 uint64_t phase = s->phase, freq;
204 int16_t *sinetable = s->sinetable;
205 float gain = s->gain;
206
207 freq = rate * ((float)(1ULL << 63) * 2);
208
209 uint8_t *inp = input;
210 for(k = 0; k < outsize; k++) {
211 int i;
212 cic_dt out0a, out0b, out1a, out1b;
213 cic_dt ig2a = 0, ig2b = 0; // last integrator and first comb replaced simply by sum
214 for(i = 0; i < factor; i++) {
215 cic_dt in_a, in_b;
216 int32_t m_a, m_b, m_c, m_d;
217 int sinep = phase >> (64-SINESHIFT);
218 // subtract 127.4 (good for rtl-sdr)
219 m_a = (((int32_t)inp[2*i]) << 8) - 32614;
220 m_b = (((int32_t)inp[2*i+1]) << 8) - 32614;
221 m_c = (int32_t)sinetable[sinep + (1<<(SINESHIFT-2))];
222 m_d = (int32_t)sinetable[sinep];
223 // complex multiplication:
224 in_a = m_a*m_c - m_b*m_d;
225 in_b = m_a*m_d + m_b*m_c;
226 phase += freq;
227 /* integrators:
228 The calculations are ordered so that each integrator
229 takes a result from previous loop iteration
230 to make the code more "pipeline-friendly". */
231 ig2a += ig1a; ig2b += ig1b;
232 ig1a += ig0a; ig1b += ig0b;
233 ig0a += in_a; ig0b += in_b;
234 }
235 inp += 2*factor;
236 // comb filters:
237 out0a = ig2a - comb0a; out0b = ig2b - comb0b;
238 comb0a = ig2a; comb0b = ig2b;
239 out1a = out0a - comb1a; out1b = out0b - comb1b;
240 comb1a = out0a; comb1b = out0b;
241
242 output[k].i = (float)out1a * gain;
243 output[k].q = (float)out1b * gain;
244 }
245
246 s->ig0a = ig0a; s->ig0b = ig0b;
247 s->ig1a = ig1a; s->ig1b = ig1b;
248 s->comb0a = comb0a; s->comb0b = comb0b;
249 s->comb1a = comb1a; s->comb1b = comb1b;
250 s->phase = phase;
251 }
252
253