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 /* gcc requires this for M_PI !? */
32 #undef __STRICT_ANSI__
33
34 /* include own header first, to see missing includes */
35 #include "pf_cic.h"
36 #include "fmv.h"
37
38 #include <math.h>
39 #include <stdlib.h>
40 #include <string.h>
41 #include <limits.h>
42
43
44 /*
45 ____ ___ ____ ____ ____ ____
46 / ___|_ _/ ___| | _ \| _ \ / ___|
47 | | | | | | | | | | | | |
48 | |___ | | |___ | |_| | |_| | |___
49 \____|___\____| |____/|____/ \____|
50 */
51
52 #define SINESHIFT 12
53 #define SINESIZE (1<<SINESHIFT)
54 typedef int64_t cic_dt; // data type used for integrators and combs
55 typedef struct {
56 int factor;
57 uint64_t phase;
58 float gain;
59 cic_dt ig0a, ig0b, ig1a, ig1b;
60 cic_dt comb0a, comb0b, comb1a, comb1b;
61 int16_t *sinetable;
62 } cicddc_t;
63
cicddc_init(int factor)64 void *cicddc_init(int factor) {
65 int i;
66 int sinesize2 = SINESIZE * 5/4; // 25% extra to get cosine from the same table
67 cicddc_t *s;
68 s = (cicddc_t *)malloc(sizeof(cicddc_t));
69 memset(s, 0, sizeof(cicddc_t));
70
71 float sineamp = 32767.0f;
72 s->factor = factor;
73 s->gain = 1.0f / SHRT_MAX / sineamp / factor / factor / factor; // compensate for gain of 3 integrators
74
75 s->sinetable = (int16_t *)malloc(sinesize2 * sizeof(*s->sinetable));
76 double f = 2.0 * M_PI / (double)SINESIZE;
77 for(i = 0; i < sinesize2; i++) {
78 s->sinetable[i] = sineamp * cos(f * i);
79 }
80 return s;
81 }
82
cicddc_free(void * state)83 void cicddc_free(void *state) {
84 cicddc_t *s = (cicddc_t *)state;
85 free(s->sinetable);
86 free(s);
87 }
88
89
90 PF_TARGET_CLONES
cicddc_s16_c(void * state,int16_t * input,complexf * output,int outsize,float rate)91 void cicddc_s16_c(void *state, int16_t *input, complexf *output, int outsize, float rate) {
92 cicddc_t *s = (cicddc_t *)state;
93 int k;
94 int factor = s->factor;
95 cic_dt ig0a = s->ig0a, ig0b = s->ig0b, ig1a = s->ig1a, ig1b = s->ig1b;
96 cic_dt comb0a = s->comb0a, comb0b = s->comb0b, comb1a = s->comb1a, comb1b = s->comb1b;
97 uint64_t phase = s->phase, freq;
98 int16_t *sinetable = s->sinetable;
99 float gain = s->gain;
100
101 freq = rate * ((float)(1ULL << 63) * 2);
102
103 int16_t *inp = input;
104 for(k = 0; k < outsize; k++) {
105 int i;
106 cic_dt out0a, out0b, out1a, out1b;
107 cic_dt ig2a = 0, ig2b = 0; // last integrator and first comb replaced simply by sum
108 for(i = 0; i < factor; i++) {
109 cic_dt in_a, in_b;
110 int sinep = phase >> (64-SINESHIFT);
111 in_a = (int32_t)inp[i] * (int32_t)sinetable[sinep + (1<<(SINESHIFT-2))];
112 in_b = (int32_t)inp[i] * (int32_t)sinetable[sinep];
113 phase += freq;
114 /* integrators:
115 The calculations are ordered so that each integrator
116 takes a result from previous loop iteration
117 to make the code more "pipeline-friendly". */
118 ig2a += ig1a; ig2b += ig1b;
119 ig1a += ig0a; ig1b += ig0b;
120 ig0a += in_a; ig0b += in_b;
121 }
122 inp += factor;
123 // comb filters:
124 out0a = ig2a - comb0a; out0b = ig2b - comb0b;
125 comb0a = ig2a; comb0b = ig2b;
126 out1a = out0a - comb1a; out1b = out0b - comb1b;
127 comb1a = out0a; comb1b = out0b;
128
129 output[k].i = (float)out1a * gain;
130 output[k].q = (float)out1b * gain;
131 }
132
133 s->ig0a = ig0a; s->ig0b = ig0b;
134 s->ig1a = ig1a; s->ig1b = ig1b;
135 s->comb0a = comb0a; s->comb0b = comb0b;
136 s->comb1a = comb1a; s->comb1b = comb1b;
137 s->phase = phase;
138 }
139
140 PF_TARGET_CLONES
cicddc_cs16_c(void * state,int16_t * input,complexf * output,int outsize,float rate)141 void cicddc_cs16_c(void *state, int16_t *input, complexf *output, int outsize, float rate) {
142 cicddc_t *s = (cicddc_t *)state;
143 int k;
144 int factor = s->factor;
145 cic_dt ig0a = s->ig0a, ig0b = s->ig0b, ig1a = s->ig1a, ig1b = s->ig1b;
146 cic_dt comb0a = s->comb0a, comb0b = s->comb0b, comb1a = s->comb1a, comb1b = s->comb1b;
147 uint64_t phase = s->phase, freq;
148 int16_t *sinetable = s->sinetable;
149 float gain = s->gain;
150
151 freq = rate * ((float)(1ULL << 63) * 2);
152
153 int16_t *inp = input;
154 for(k = 0; k < outsize; k++) {
155 int i;
156 cic_dt out0a, out0b, out1a, out1b;
157 cic_dt ig2a = 0, ig2b = 0; // last integrator and first comb replaced simply by sum
158 for(i = 0; i < factor; i++) {
159 cic_dt in_a, in_b;
160 int32_t m_a, m_b, m_c, m_d;
161 int sinep = phase >> (64-SINESHIFT);
162 m_a = inp[2*i];
163 m_b = inp[2*i+1];
164 m_c = (int32_t)sinetable[sinep + (1<<(SINESHIFT-2))];
165 m_d = (int32_t)sinetable[sinep];
166 // complex multiplication:
167 in_a = m_a*m_c - m_b*m_d;
168 in_b = m_a*m_d + m_b*m_c;
169 phase += freq;
170 /* integrators:
171 The calculations are ordered so that each integrator
172 takes a result from previous loop iteration
173 to make the code more "pipeline-friendly". */
174 ig2a += ig1a; ig2b += ig1b;
175 ig1a += ig0a; ig1b += ig0b;
176 ig0a += in_a; ig0b += in_b;
177 }
178 inp += 2*factor;
179 // comb filters:
180 out0a = ig2a - comb0a; out0b = ig2b - comb0b;
181 comb0a = ig2a; comb0b = ig2b;
182 out1a = out0a - comb1a; out1b = out0b - comb1b;
183 comb1a = out0a; comb1b = out0b;
184
185 output[k].i = (float)out1a * gain;
186 output[k].q = (float)out1b * gain;
187 }
188
189 s->ig0a = ig0a; s->ig0b = ig0b;
190 s->ig1a = ig1a; s->ig1b = ig1b;
191 s->comb0a = comb0a; s->comb0b = comb0b;
192 s->comb1a = comb1a; s->comb1b = comb1b;
193 s->phase = phase;
194 }
195
196
197 /* This is almost copy paste from cicddc_cs16_c.
198 I'm afraid this is going to be annoying to maintain... */
199 PF_TARGET_CLONES
cicddc_cu8_c(void * state,uint8_t * input,complexf * output,int outsize,float rate)200 void cicddc_cu8_c(void *state, uint8_t *input, complexf *output, int outsize, float rate) {
201 cicddc_t *s = (cicddc_t *)state;
202 int k;
203 int factor = s->factor;
204 cic_dt ig0a = s->ig0a, ig0b = s->ig0b, ig1a = s->ig1a, ig1b = s->ig1b;
205 cic_dt comb0a = s->comb0a, comb0b = s->comb0b, comb1a = s->comb1a, comb1b = s->comb1b;
206 uint64_t phase = s->phase, freq;
207 int16_t *sinetable = s->sinetable;
208 float gain = s->gain;
209
210 freq = rate * ((float)(1ULL << 63) * 2);
211
212 uint8_t *inp = input;
213 for(k = 0; k < outsize; k++) {
214 int i;
215 cic_dt out0a, out0b, out1a, out1b;
216 cic_dt ig2a = 0, ig2b = 0; // last integrator and first comb replaced simply by sum
217 for(i = 0; i < factor; i++) {
218 cic_dt in_a, in_b;
219 int32_t m_a, m_b, m_c, m_d;
220 int sinep = phase >> (64-SINESHIFT);
221 // subtract 127.4 (good for rtl-sdr)
222 m_a = (((int32_t)inp[2*i]) << 8) - 32614;
223 m_b = (((int32_t)inp[2*i+1]) << 8) - 32614;
224 m_c = (int32_t)sinetable[sinep + (1<<(SINESHIFT-2))];
225 m_d = (int32_t)sinetable[sinep];
226 // complex multiplication:
227 in_a = m_a*m_c - m_b*m_d;
228 in_b = m_a*m_d + m_b*m_c;
229 phase += freq;
230 /* integrators:
231 The calculations are ordered so that each integrator
232 takes a result from previous loop iteration
233 to make the code more "pipeline-friendly". */
234 ig2a += ig1a; ig2b += ig1b;
235 ig1a += ig0a; ig1b += ig0b;
236 ig0a += in_a; ig0b += in_b;
237 }
238 inp += 2*factor;
239 // comb filters:
240 out0a = ig2a - comb0a; out0b = ig2b - comb0b;
241 comb0a = ig2a; comb0b = ig2b;
242 out1a = out0a - comb1a; out1b = out0b - comb1b;
243 comb1a = out0a; comb1b = out0b;
244
245 output[k].i = (float)out1a * gain;
246 output[k].q = (float)out1b * gain;
247 }
248
249 s->ig0a = ig0a; s->ig0b = ig0b;
250 s->ig1a = ig1a; s->ig1b = ig1b;
251 s->comb0a = comb0a; s->comb0b = comb0b;
252 s->comb1a = comb1a; s->comb1b = comb1b;
253 s->phase = phase;
254 }
255
256