T T / Mbed 2 deprecated Nucleo_IQDDS

AD9857 IQ DDS Digital Up Converter Experiment using Nucleo F401

Dependencies:   mbed

Digital Signal Processing for IQ Quadradure Modulation DDS AD9857 using Nucleo F401.

see http://ttrftech.tumblr.com/post/114310226891/

dsp.cpp@0:55201637d936, 2015-03-21 (annotated)

Committer:
edy555
Date:
Sat Mar 21 08:31:20 2015 +0000
Revision:
0:55201637d936
Child:
3:e6897d74d6bf
interpolation of fir x4 and cic x10

Who changed what in which revision?

UserRevisionLine numberNew contents of line
edy555 0:55201637d936 1 #include "mbed.h"
edy555 0:55201637d936 2 #include "dsp.h"
edy555 0:55201637d936 3
edy555 0:55201637d936 4 extern Serial pc;
edy555 0:55201637d936 5
edy555 0:55201637d936 6
edy555 0:55201637d936 7 int16_t htstat_buf[HILBERT_TRANSFORM_FIRSTATE_LENGTH];
edy555 0:55201637d936 8 int16_t cap_buf[2][CAPTURE_LEN];
edy555 0:55201637d936 9
edy555 0:55201637d936 10 int16_t fir_state[FIRSTATE_LENGTH];
edy555 0:55201637d936 11 int16_t fir_buf[CAPTURE_LEN];
edy555 0:55201637d936 12
edy555 0:55201637d936 13 int16_t cic_buf[CICBUF_LEN];
edy555 0:55201637d936 14 int16_t dma_buf[2][DMA_DATALEN];
edy555 0:55201637d936 15
edy555 0:55201637d936 16 CICState cic;
edy555 0:55201637d936 17
edy555 0:55201637d936 18 void cic_interpolate_x10(CICState *cic, uint32_t *src, int src_len, uint32_t *dst)
edy555 0:55201637d936 19 {
edy555 0:55201637d936 20 uint32_t p0 = cic->p0;
edy555 0:55201637d936 21 uint32_t s0 = cic->s0;
edy555 0:55201637d936 22 int i;
edy555 0:55201637d936 23 int j = 0;
edy555 0:55201637d936 24 for (i = 0; i < src_len; i++) {
edy555 0:55201637d936 25 uint32_t s = src[i];
edy555 0:55201637d936 26 uint32_t d0 = __SSUB16(s, p0);
edy555 0:55201637d936 27 s0 = __SADD16(s0, d0);
edy555 0:55201637d936 28 uint32_t s1 = __SADD16(s0, d0);
edy555 0:55201637d936 29 uint32_t s2 = __SADD16(s1, d0);
edy555 0:55201637d936 30 uint32_t s3 = __SADD16(s2, d0);
edy555 0:55201637d936 31 uint32_t s4 = __SADD16(s3, d0);
edy555 0:55201637d936 32 dst[j ] = s0;
edy555 0:55201637d936 33 dst[j+1] = s1;
edy555 0:55201637d936 34 dst[j+2] = s2;
edy555 0:55201637d936 35 dst[j+3] = s3;
edy555 0:55201637d936 36 dst[j+4] = s4;
edy555 0:55201637d936 37 j += 5;
edy555 0:55201637d936 38 s0 = s4;
edy555 0:55201637d936 39 s0 = __SADD16(s0, d0);
edy555 0:55201637d936 40 s1 = __SADD16(s0, d0);
edy555 0:55201637d936 41 s2 = __SADD16(s1, d0);
edy555 0:55201637d936 42 s3 = __SADD16(s2, d0);
edy555 0:55201637d936 43 s4 = __SADD16(s3, d0);
edy555 0:55201637d936 44 dst[j ] = s0;
edy555 0:55201637d936 45 dst[j+1] = s1;
edy555 0:55201637d936 46 dst[j+2] = s2;
edy555 0:55201637d936 47 dst[j+3] = s3;
edy555 0:55201637d936 48 dst[j+4] = s4;
edy555 0:55201637d936 49 j += 5;
edy555 0:55201637d936 50 s0 = s4;
edy555 0:55201637d936 51 p0 = s;
edy555 0:55201637d936 52 }
edy555 0:55201637d936 53 cic->s0 = s0;
edy555 0:55201637d936 54 cic->p0 = p0;
edy555 0:55201637d936 55 }
edy555 0:55201637d936 56
edy555 0:55201637d936 57 const int16_t fir_coeff[8][16] = {
edy555 0:55201637d936 58 { -19, -6, 102, -22, -357, 264, 1798, 2064, 630, -337, -119, 111, 15, -24, 0, 0},
edy555 0:55201637d936 59 { -13, -20, 76, 51, -311, -29, 1441, 2206, 1037, -231, -223, 96, 45, -25, -6, 0},
edy555 0:55201637d936 60 { -6, -25, 45, 96, -223, -231, 1037, 2206, 1441, -29, -311, 51, 76, -20, -13, 0},
edy555 0:55201637d936 61 { 0, -24, 15, 111, -119, -337, 630, 2064, 1798, 264, -357, -22, 102, -6, -19, 0},
edy555 0:55201637d936 62 { 0, -19, -6, 102, -22, -357, 264, 1798, 2064, 630, -337, -119, 111, 15, -24, 0},
edy555 0:55201637d936 63 { 0, -13, -20, 76, 51, -311, -29, 1441, 2206, 1037, -231, -223, 96, 45, -25, -6},
edy555 0:55201637d936 64 { 0, -6, -25, 45, 96, -223, -231, 1037, 2206, 1441, -29, -311, 51, 76, -20, -13},
edy555 0:55201637d936 65 { 0, 0, -24, 15, 111, -119, -337, 630, 2064, 1798, 264, -357, -22, 102, -6, -19}
edy555 0:55201637d936 66 };
edy555 0:55201637d936 67
edy555 0:55201637d936 68 void fir_resample_x4(uint32_t *src_state, uint32_t *dst, int dst_len)
edy555 0:55201637d936 69 {
edy555 0:55201637d936 70 uint32_t *src = src_state;
edy555 0:55201637d936 71 int index = 0;
edy555 0:55201637d936 72 int i;
edy555 0:55201637d936 73 for (i = 0; i < dst_len; i++) {
edy555 0:55201637d936 74 uint32_t *kp = (uint32_t*)fir_coeff[index];
edy555 0:55201637d936 75 uint32_t acc_i = 0;
edy555 0:55201637d936 76 uint32_t acc_q = 0;
edy555 0:55201637d936 77
edy555 0:55201637d936 78 // 8cycle x 8
edy555 0:55201637d936 79 #define CELL(n) do { \
edy555 0:55201637d936 80 uint32_t k = kp[n]; \
edy555 0:55201637d936 81 uint32_t p0 = src[n*2]; \
edy555 0:55201637d936 82 uint32_t p1 = src[n*2+1]; \
edy555 0:55201637d936 83 uint32_t i01 = __PKHTB(p1, p0, 16); \
edy555 0:55201637d936 84 uint32_t q01 = __PKHBT(p0, p1, 16); \
edy555 0:55201637d936 85 acc_i = __SMLAD(k, i01, acc_i); \
edy555 0:55201637d936 86 acc_q = __SMLAD(k, q01, acc_q); \
edy555 0:55201637d936 87 } while(0)
edy555 0:55201637d936 88
edy555 0:55201637d936 89 CELL(0); CELL(1); CELL(2); CELL(3);
edy555 0:55201637d936 90 CELL(4); CELL(5); CELL(6); CELL(7);
edy555 0:55201637d936 91 *dst++ = __SSAT16(__PKHTB(acc_i, acc_q, 16), 11);
edy555 0:55201637d936 92
edy555 0:55201637d936 93 index++;
edy555 0:55201637d936 94 if (index >= 8) {
edy555 0:55201637d936 95 index = 0;
edy555 0:55201637d936 96 src += 2;
edy555 0:55201637d936 97 }
edy555 0:55201637d936 98 }
edy555 0:55201637d936 99
edy555 0:55201637d936 100 for (i = 0; i < FIRSTATE_LENGTH/2; i++)
edy555 0:55201637d936 101 *src_state++ = *src++;
edy555 0:55201637d936 102 }
edy555 0:55201637d936 103
edy555 0:55201637d936 104 void
edy555 0:55201637d936 105 interpolate_test()
edy555 0:55201637d936 106 {
edy555 0:55201637d936 107 int freq = 1000;
edy555 0:55201637d936 108 int ampl = 3000;
edy555 0:55201637d936 109 int rate = 48000;
edy555 0:55201637d936 110 for (int i = 0; i < CAPTURE_LEN; i++){
edy555 0:55201637d936 111 cap_buf[0][i*2] = sin(2*3.141592 * i * freq / rate) * ampl; // Lch
edy555 0:55201637d936 112 cap_buf[0][i*2+1] = cos(2*3.141592 * i * freq / rate) * ampl; // Rch
edy555 0:55201637d936 113 }
edy555 0:55201637d936 114 #if 0
edy555 0:55201637d936 115 for (int i = 0; i < 48; i++){
edy555 0:55201637d936 116 pc.printf("%d, ", cap_buf[i*2]);
edy555 0:55201637d936 117 }
edy555 0:55201637d936 118 pc.printf("\n\r");
edy555 0:55201637d936 119 #endif
edy555 0:55201637d936 120 fir_resample_x4((uint32_t *)&cap_buf[0], (uint32_t*)&cic_buf, CICBUF_LEN);
edy555 0:55201637d936 121 cic_interpolate_x10(&cic, (uint32_t*)&cic_buf, CICBUF_LEN, (uint32_t*)dma_buf[0]);
edy555 0:55201637d936 122 fir_resample_x4((uint32_t *)&cap_buf[0], (uint32_t*)&cic_buf, CICBUF_LEN);
edy555 0:55201637d936 123 cic_interpolate_x10(&cic, (uint32_t*)&cic_buf, CICBUF_LEN, (uint32_t*)dma_buf[0]);
edy555 0:55201637d936 124 #if 0
edy555 0:55201637d936 125 for (int i = 0; i < 48*25/4; i++){
edy555 0:55201637d936 126 pc.printf("%d, ", cic_buf[i*2]);
edy555 0:55201637d936 127 }
edy555 0:55201637d936 128 pc.printf("\n\r");
edy555 0:55201637d936 129 #endif
edy555 0:55201637d936 130 #if 0
edy555 0:55201637d936 131 for (int i = DMA_DATALEN/2-400; i < DMA_DATALEN/2; i++){
edy555 0:55201637d936 132 pc.printf("%d, ", dma_buf[i*2]);
edy555 0:55201637d936 133 }
edy555 0:55201637d936 134 pc.printf("\n\r");
edy555 0:55201637d936 135 for (int i = 0; i < 400; i++){
edy555 0:55201637d936 136 pc.printf("%d, ", dma_buf[i*2]);
edy555 0:55201637d936 137 }
edy555 0:55201637d936 138 pc.printf("\n\r");
edy555 0:55201637d936 139 #endif
edy555 0:55201637d936 140 }
edy555 0:55201637d936 141
edy555 0:55201637d936 142 // hilbert transform by 127 taps fir
edy555 0:55201637d936 143 int16_t hilbert127_fir_coeff[32] = {
edy555 0:55201637d936 144 20848, 6917, 4112, 2897, 2212, 1768, 1454, 1219, 1036,
edy555 0:55201637d936 145 887, 764, 661, 572, 496, 429, 371, 319, 274,
edy555 0:55201637d936 146 234, 198, 167, 140, 117, 97, 79, 65, 53,
edy555 0:55201637d936 147 44, 36, 31, 28, 26
edy555 0:55201637d936 148 };
edy555 0:55201637d936 149
edy555 0:55201637d936 150 void
edy555 0:55201637d936 151 hilbert_transform(uint32_t *src, uint32_t *dst, int dst_len, int sign)
edy555 0:55201637d936 152 {
edy555 0:55201637d936 153 int j;
edy555 0:55201637d936 154 int len = sizeof htstat_buf / sizeof(uint32_t);
edy555 0:55201637d936 155 src -= len;
edy555 0:55201637d936 156 // 240 * (208 + 12) = 52800cycle = 0.62ms@84MHz
edy555 0:55201637d936 157 for (j = 0; j < dst_len; j++) {
edy555 0:55201637d936 158 int i;
edy555 0:55201637d936 159 int32_t acc = 0;
edy555 0:55201637d936 160 #define OFFSET 64
edy555 0:55201637d936 161 // 16 * (10 + 3) = 208 cycle
edy555 0:55201637d936 162 for (i = 0; i < 32; i += 2) {
edy555 0:55201637d936 163 // 10 cycle
edy555 0:55201637d936 164 uint32_t c = *(uint32_t*)&hilbert127_fir_coeff[i];
edy555 0:55201637d936 165 uint32_t a0 = src[OFFSET - i*2 - 1];
edy555 0:55201637d936 166 uint32_t a1 = src[OFFSET - i*2 - 3];
edy555 0:55201637d936 167 uint32_t b0 = src[OFFSET + i*2 + 1];
edy555 0:55201637d936 168 uint32_t b1 = src[OFFSET + i*2 + 3];
edy555 0:55201637d936 169 // fetch only R-ch (top half of word) from 2 successive samples
edy555 0:55201637d936 170 uint32_t a = __PKHTB(a1, a0, 16);
edy555 0:55201637d936 171 // and also do at symmetry samples
edy555 0:55201637d936 172 uint32_t b = __PKHTB(b1, b0, 16);
edy555 0:55201637d936 173 uint32_t d = __QSUB16(b, a);
edy555 0:55201637d936 174 acc = __SMLAD(c, d, acc);
edy555 0:55201637d936 175 }
edy555 0:55201637d936 176 acc *= sign;
edy555 0:55201637d936 177 uint32_t real = src[OFFSET];
edy555 0:55201637d936 178 *dst++ = __PKHTB(real, acc, 15);
edy555 0:55201637d936 179 src++;
edy555 0:55201637d936 180 }
edy555 0:55201637d936 181 }
edy555 0:55201637d936 182
edy555 0:55201637d936 183 void
edy555 0:55201637d936 184 hilbert_transform_save_fir_state(uint32_t *src_tail)
edy555 0:55201637d936 185 {
edy555 0:55201637d936 186 int len = sizeof htstat_buf / sizeof(uint32_t);
edy555 0:55201637d936 187 uint32_t *dst = (uint32_t*)&htstat_buf[0];
edy555 0:55201637d936 188 uint32_t *src = src_tail - len;
edy555 0:55201637d936 189 int i;
edy555 0:55201637d936 190 for (i = 0; i < len; i++) {
edy555 0:55201637d936 191 *dst++ = *src++;
edy555 0:55201637d936 192 }
edy555 0:55201637d936 193 }
edy555 0:55201637d936 194
edy555 0:55201637d936 195 const int16_t cos_sin_table[257][4] = {
edy555 0:55201637d936 196 { 0, 402, -32767, 2 },
edy555 0:55201637d936 197 { 402, 402, -32765, 8 },
edy555 0:55201637d936 198 { 804, 402, -32757, 12 },
edy555 0:55201637d936 199 { 1206, 402, -32745, 17 },
edy555 0:55201637d936 200 { 1608, 401, -32728, 23 },
edy555 0:55201637d936 201 { 2009, 401, -32705, 27 },
edy555 0:55201637d936 202 { 2410, 401, -32678, 32 },
edy555 0:55201637d936 203 { 2811, 401, -32646, 37 },
edy555 0:55201637d936 204 { 3212, 400, -32609, 42 },
edy555 0:55201637d936 205 { 3612, 399, -32567, 46 },
edy555 0:55201637d936 206 { 4011, 399, -32521, 52 },
edy555 0:55201637d936 207 { 4410, 398, -32469, 57 },
edy555 0:55201637d936 208 { 4808, 397, -32412, 61 },
edy555 0:55201637d936 209 { 5205, 397, -32351, 66 },
edy555 0:55201637d936 210 { 5602, 396, -32285, 72 },
edy555 0:55201637d936 211 { 5998, 395, -32213, 76 },
edy555 0:55201637d936 212 { 6393, 393, -32137, 80 },
edy555 0:55201637d936 213 { 6786, 393, -32057, 86 },
edy555 0:55201637d936 214 { 7179, 392, -31971, 91 },
edy555 0:55201637d936 215 { 7571, 391, -31880, 95 },
edy555 0:55201637d936 216 { 7962, 389, -31785, 100 },
edy555 0:55201637d936 217 { 8351, 388, -31685, 105 },
edy555 0:55201637d936 218 { 8739, 387, -31580, 110 },
edy555 0:55201637d936 219 { 9126, 386, -31470, 114 },
edy555 0:55201637d936 220 { 9512, 384, -31356, 119 },
edy555 0:55201637d936 221 { 9896, 382, -31237, 124 },
edy555 0:55201637d936 222 { 10278, 381, -31113, 128 },
edy555 0:55201637d936 223 { 10659, 380, -30985, 133 },
edy555 0:55201637d936 224 { 11039, 378, -30852, 138 },
edy555 0:55201637d936 225 { 11417, 376, -30714, 143 },
edy555 0:55201637d936 226 { 11793, 374, -30571, 147 },
edy555 0:55201637d936 227 { 12167, 372, -30424, 151 },
edy555 0:55201637d936 228 { 12539, 371, -30273, 156 },
edy555 0:55201637d936 229 { 12910, 369, -30117, 161 },
edy555 0:55201637d936 230 { 13279, 366, -29956, 165 },
edy555 0:55201637d936 231 { 13645, 365, -29791, 170 },
edy555 0:55201637d936 232 { 14010, 362, -29621, 174 },
edy555 0:55201637d936 233 { 14372, 360, -29447, 179 },
edy555 0:55201637d936 234 { 14732, 358, -29268, 183 },
edy555 0:55201637d936 235 { 15090, 356, -29085, 187 },
edy555 0:55201637d936 236 { 15446, 354, -28898, 192 },
edy555 0:55201637d936 237 { 15800, 351, -28706, 196 },
edy555 0:55201637d936 238 { 16151, 348, -28510, 200 },
edy555 0:55201637d936 239 { 16499, 347, -28310, 205 },
edy555 0:55201637d936 240 { 16846, 343, -28105, 209 },
edy555 0:55201637d936 241 { 17189, 341, -27896, 213 },
edy555 0:55201637d936 242 { 17530, 339, -27683, 217 },
edy555 0:55201637d936 243 { 17869, 335, -27466, 221 },
edy555 0:55201637d936 244 { 18204, 333, -27245, 226 },
edy555 0:55201637d936 245 { 18537, 331, -27019, 229 },
edy555 0:55201637d936 246 { 18868, 327, -26790, 234 },
edy555 0:55201637d936 247 { 19195, 324, -26556, 237 },
edy555 0:55201637d936 248 { 19519, 322, -26319, 242 },
edy555 0:55201637d936 249 { 19841, 318, -26077, 245 },
edy555 0:55201637d936 250 { 20159, 316, -25832, 250 },
edy555 0:55201637d936 251 { 20475, 312, -25582, 253 },
edy555 0:55201637d936 252 { 20787, 309, -25329, 257 },
edy555 0:55201637d936 253 { 21096, 307, -25072, 261 },
edy555 0:55201637d936 254 { 21403, 302, -24811, 264 },
edy555 0:55201637d936 255 { 21705, 300, -24547, 268 },
edy555 0:55201637d936 256 { 22005, 296, -24279, 272 },
edy555 0:55201637d936 257 { 22301, 293, -24007, 276 },
edy555 0:55201637d936 258 { 22594, 290, -23731, 279 },
edy555 0:55201637d936 259 { 22884, 286, -23452, 282 },
edy555 0:55201637d936 260 { 23170, 282, -23170, 286 },
edy555 0:55201637d936 261 { 23452, 279, -22884, 290 },
edy555 0:55201637d936 262 { 23731, 276, -22594, 293 },
edy555 0:55201637d936 263 { 24007, 272, -22301, 296 },
edy555 0:55201637d936 264 { 24279, 268, -22005, 300 },
edy555 0:55201637d936 265 { 24547, 264, -21705, 302 },
edy555 0:55201637d936 266 { 24811, 261, -21403, 307 },
edy555 0:55201637d936 267 { 25072, 257, -21096, 309 },
edy555 0:55201637d936 268 { 25329, 253, -20787, 312 },
edy555 0:55201637d936 269 { 25582, 250, -20475, 316 },
edy555 0:55201637d936 270 { 25832, 245, -20159, 318 },
edy555 0:55201637d936 271 { 26077, 242, -19841, 322 },
edy555 0:55201637d936 272 { 26319, 237, -19519, 324 },
edy555 0:55201637d936 273 { 26556, 234, -19195, 327 },
edy555 0:55201637d936 274 { 26790, 229, -18868, 331 },
edy555 0:55201637d936 275 { 27019, 226, -18537, 333 },
edy555 0:55201637d936 276 { 27245, 221, -18204, 335 },
edy555 0:55201637d936 277 { 27466, 217, -17869, 339 },
edy555 0:55201637d936 278 { 27683, 213, -17530, 341 },
edy555 0:55201637d936 279 { 27896, 209, -17189, 343 },
edy555 0:55201637d936 280 { 28105, 205, -16846, 347 },
edy555 0:55201637d936 281 { 28310, 200, -16499, 348 },
edy555 0:55201637d936 282 { 28510, 196, -16151, 351 },
edy555 0:55201637d936 283 { 28706, 192, -15800, 354 },
edy555 0:55201637d936 284 { 28898, 187, -15446, 356 },
edy555 0:55201637d936 285 { 29085, 183, -15090, 358 },
edy555 0:55201637d936 286 { 29268, 179, -14732, 360 },
edy555 0:55201637d936 287 { 29447, 174, -14372, 362 },
edy555 0:55201637d936 288 { 29621, 170, -14010, 365 },
edy555 0:55201637d936 289 { 29791, 165, -13645, 366 },
edy555 0:55201637d936 290 { 29956, 161, -13279, 369 },
edy555 0:55201637d936 291 { 30117, 156, -12910, 371 },
edy555 0:55201637d936 292 { 30273, 151, -12539, 372 },
edy555 0:55201637d936 293 { 30424, 147, -12167, 374 },
edy555 0:55201637d936 294 { 30571, 143, -11793, 376 },
edy555 0:55201637d936 295 { 30714, 138, -11417, 378 },
edy555 0:55201637d936 296 { 30852, 133, -11039, 380 },
edy555 0:55201637d936 297 { 30985, 128, -10659, 381 },
edy555 0:55201637d936 298 { 31113, 124, -10278, 382 },
edy555 0:55201637d936 299 { 31237, 119, -9896, 384 },
edy555 0:55201637d936 300 { 31356, 114, -9512, 386 },
edy555 0:55201637d936 301 { 31470, 110, -9126, 387 },
edy555 0:55201637d936 302 { 31580, 105, -8739, 388 },
edy555 0:55201637d936 303 { 31685, 100, -8351, 389 },
edy555 0:55201637d936 304 { 31785, 95, -7962, 391 },
edy555 0:55201637d936 305 { 31880, 91, -7571, 392 },
edy555 0:55201637d936 306 { 31971, 86, -7179, 393 },
edy555 0:55201637d936 307 { 32057, 80, -6786, 393 },
edy555 0:55201637d936 308 { 32137, 76, -6393, 395 },
edy555 0:55201637d936 309 { 32213, 72, -5998, 396 },
edy555 0:55201637d936 310 { 32285, 66, -5602, 397 },
edy555 0:55201637d936 311 { 32351, 61, -5205, 397 },
edy555 0:55201637d936 312 { 32412, 57, -4808, 398 },
edy555 0:55201637d936 313 { 32469, 52, -4410, 399 },
edy555 0:55201637d936 314 { 32521, 46, -4011, 399 },
edy555 0:55201637d936 315 { 32567, 42, -3612, 400 },
edy555 0:55201637d936 316 { 32609, 37, -3212, 401 },
edy555 0:55201637d936 317 { 32646, 32, -2811, 401 },
edy555 0:55201637d936 318 { 32678, 27, -2410, 401 },
edy555 0:55201637d936 319 { 32705, 23, -2009, 401 },
edy555 0:55201637d936 320 { 32728, 17, -1608, 402 },
edy555 0:55201637d936 321 { 32745, 12, -1206, 402 },
edy555 0:55201637d936 322 { 32757, 8, -804, 402 },
edy555 0:55201637d936 323 { 32765, 2, -402, 402 },
edy555 0:55201637d936 324 { 32767, -2, 0, 402 },
edy555 0:55201637d936 325 { 32765, -8, 402, 402 },
edy555 0:55201637d936 326 { 32757, -12, 804, 402 },
edy555 0:55201637d936 327 { 32745, -17, 1206, 402 },
edy555 0:55201637d936 328 { 32728, -23, 1608, 401 },
edy555 0:55201637d936 329 { 32705, -27, 2009, 401 },
edy555 0:55201637d936 330 { 32678, -32, 2410, 401 },
edy555 0:55201637d936 331 { 32646, -37, 2811, 401 },
edy555 0:55201637d936 332 { 32609, -42, 3212, 400 },
edy555 0:55201637d936 333 { 32567, -46, 3612, 399 },
edy555 0:55201637d936 334 { 32521, -52, 4011, 399 },
edy555 0:55201637d936 335 { 32469, -57, 4410, 398 },
edy555 0:55201637d936 336 { 32412, -61, 4808, 397 },
edy555 0:55201637d936 337 { 32351, -66, 5205, 397 },
edy555 0:55201637d936 338 { 32285, -72, 5602, 396 },
edy555 0:55201637d936 339 { 32213, -76, 5998, 395 },
edy555 0:55201637d936 340 { 32137, -80, 6393, 393 },
edy555 0:55201637d936 341 { 32057, -86, 6786, 393 },
edy555 0:55201637d936 342 { 31971, -91, 7179, 392 },
edy555 0:55201637d936 343 { 31880, -95, 7571, 391 },
edy555 0:55201637d936 344 { 31785, -100, 7962, 389 },
edy555 0:55201637d936 345 { 31685, -105, 8351, 388 },
edy555 0:55201637d936 346 { 31580, -110, 8739, 387 },
edy555 0:55201637d936 347 { 31470, -114, 9126, 386 },
edy555 0:55201637d936 348 { 31356, -119, 9512, 384 },
edy555 0:55201637d936 349 { 31237, -124, 9896, 382 },
edy555 0:55201637d936 350 { 31113, -128, 10278, 381 },
edy555 0:55201637d936 351 { 30985, -133, 10659, 380 },
edy555 0:55201637d936 352 { 30852, -138, 11039, 378 },
edy555 0:55201637d936 353 { 30714, -143, 11417, 376 },
edy555 0:55201637d936 354 { 30571, -147, 11793, 374 },
edy555 0:55201637d936 355 { 30424, -151, 12167, 372 },
edy555 0:55201637d936 356 { 30273, -156, 12539, 371 },
edy555 0:55201637d936 357 { 30117, -161, 12910, 369 },
edy555 0:55201637d936 358 { 29956, -165, 13279, 366 },
edy555 0:55201637d936 359 { 29791, -170, 13645, 365 },
edy555 0:55201637d936 360 { 29621, -174, 14010, 362 },
edy555 0:55201637d936 361 { 29447, -179, 14372, 360 },
edy555 0:55201637d936 362 { 29268, -183, 14732, 358 },
edy555 0:55201637d936 363 { 29085, -187, 15090, 356 },
edy555 0:55201637d936 364 { 28898, -192, 15446, 354 },
edy555 0:55201637d936 365 { 28706, -196, 15800, 351 },
edy555 0:55201637d936 366 { 28510, -200, 16151, 348 },
edy555 0:55201637d936 367 { 28310, -205, 16499, 347 },
edy555 0:55201637d936 368 { 28105, -209, 16846, 343 },
edy555 0:55201637d936 369 { 27896, -213, 17189, 341 },
edy555 0:55201637d936 370 { 27683, -217, 17530, 339 },
edy555 0:55201637d936 371 { 27466, -221, 17869, 335 },
edy555 0:55201637d936 372 { 27245, -226, 18204, 333 },
edy555 0:55201637d936 373 { 27019, -229, 18537, 331 },
edy555 0:55201637d936 374 { 26790, -234, 18868, 327 },
edy555 0:55201637d936 375 { 26556, -237, 19195, 324 },
edy555 0:55201637d936 376 { 26319, -242, 19519, 322 },
edy555 0:55201637d936 377 { 26077, -245, 19841, 318 },
edy555 0:55201637d936 378 { 25832, -250, 20159, 316 },
edy555 0:55201637d936 379 { 25582, -253, 20475, 312 },
edy555 0:55201637d936 380 { 25329, -257, 20787, 309 },
edy555 0:55201637d936 381 { 25072, -261, 21096, 307 },
edy555 0:55201637d936 382 { 24811, -264, 21403, 302 },
edy555 0:55201637d936 383 { 24547, -268, 21705, 300 },
edy555 0:55201637d936 384 { 24279, -272, 22005, 296 },
edy555 0:55201637d936 385 { 24007, -276, 22301, 293 },
edy555 0:55201637d936 386 { 23731, -279, 22594, 290 },
edy555 0:55201637d936 387 { 23452, -282, 22884, 286 },
edy555 0:55201637d936 388 { 23170, -286, 23170, 282 },
edy555 0:55201637d936 389 { 22884, -290, 23452, 279 },
edy555 0:55201637d936 390 { 22594, -293, 23731, 276 },
edy555 0:55201637d936 391 { 22301, -296, 24007, 272 },
edy555 0:55201637d936 392 { 22005, -300, 24279, 268 },
edy555 0:55201637d936 393 { 21705, -302, 24547, 264 },
edy555 0:55201637d936 394 { 21403, -307, 24811, 261 },
edy555 0:55201637d936 395 { 21096, -309, 25072, 257 },
edy555 0:55201637d936 396 { 20787, -312, 25329, 253 },
edy555 0:55201637d936 397 { 20475, -316, 25582, 250 },
edy555 0:55201637d936 398 { 20159, -318, 25832, 245 },
edy555 0:55201637d936 399 { 19841, -322, 26077, 242 },
edy555 0:55201637d936 400 { 19519, -324, 26319, 237 },
edy555 0:55201637d936 401 { 19195, -327, 26556, 234 },
edy555 0:55201637d936 402 { 18868, -331, 26790, 229 },
edy555 0:55201637d936 403 { 18537, -333, 27019, 226 },
edy555 0:55201637d936 404 { 18204, -335, 27245, 221 },
edy555 0:55201637d936 405 { 17869, -339, 27466, 217 },
edy555 0:55201637d936 406 { 17530, -341, 27683, 213 },
edy555 0:55201637d936 407 { 17189, -343, 27896, 209 },
edy555 0:55201637d936 408 { 16846, -347, 28105, 205 },
edy555 0:55201637d936 409 { 16499, -348, 28310, 200 },
edy555 0:55201637d936 410 { 16151, -351, 28510, 196 },
edy555 0:55201637d936 411 { 15800, -354, 28706, 192 },
edy555 0:55201637d936 412 { 15446, -356, 28898, 187 },
edy555 0:55201637d936 413 { 15090, -358, 29085, 183 },
edy555 0:55201637d936 414 { 14732, -360, 29268, 179 },
edy555 0:55201637d936 415 { 14372, -362, 29447, 174 },
edy555 0:55201637d936 416 { 14010, -365, 29621, 170 },
edy555 0:55201637d936 417 { 13645, -366, 29791, 165 },
edy555 0:55201637d936 418 { 13279, -369, 29956, 161 },
edy555 0:55201637d936 419 { 12910, -371, 30117, 156 },
edy555 0:55201637d936 420 { 12539, -372, 30273, 151 },
edy555 0:55201637d936 421 { 12167, -374, 30424, 147 },
edy555 0:55201637d936 422 { 11793, -376, 30571, 143 },
edy555 0:55201637d936 423 { 11417, -378, 30714, 138 },
edy555 0:55201637d936 424 { 11039, -380, 30852, 133 },
edy555 0:55201637d936 425 { 10659, -381, 30985, 128 },
edy555 0:55201637d936 426 { 10278, -382, 31113, 124 },
edy555 0:55201637d936 427 { 9896, -384, 31237, 119 },
edy555 0:55201637d936 428 { 9512, -386, 31356, 114 },
edy555 0:55201637d936 429 { 9126, -387, 31470, 110 },
edy555 0:55201637d936 430 { 8739, -388, 31580, 105 },
edy555 0:55201637d936 431 { 8351, -389, 31685, 100 },
edy555 0:55201637d936 432 { 7962, -391, 31785, 95 },
edy555 0:55201637d936 433 { 7571, -392, 31880, 91 },
edy555 0:55201637d936 434 { 7179, -393, 31971, 86 },
edy555 0:55201637d936 435 { 6786, -393, 32057, 80 },
edy555 0:55201637d936 436 { 6393, -395, 32137, 76 },
edy555 0:55201637d936 437 { 5998, -396, 32213, 72 },
edy555 0:55201637d936 438 { 5602, -397, 32285, 66 },
edy555 0:55201637d936 439 { 5205, -397, 32351, 61 },
edy555 0:55201637d936 440 { 4808, -398, 32412, 57 },
edy555 0:55201637d936 441 { 4410, -399, 32469, 52 },
edy555 0:55201637d936 442 { 4011, -399, 32521, 46 },
edy555 0:55201637d936 443 { 3612, -400, 32567, 42 },
edy555 0:55201637d936 444 { 3212, -401, 32609, 37 },
edy555 0:55201637d936 445 { 2811, -401, 32646, 32 },
edy555 0:55201637d936 446 { 2410, -401, 32678, 27 },
edy555 0:55201637d936 447 { 2009, -401, 32705, 23 },
edy555 0:55201637d936 448 { 1608, -402, 32728, 17 },
edy555 0:55201637d936 449 { 1206, -402, 32745, 12 },
edy555 0:55201637d936 450 { 804, -402, 32757, 8 },
edy555 0:55201637d936 451 { 402, -402, 32765, 2 },
edy555 0:55201637d936 452 { 0, -402, 32767, 0 }
edy555 0:55201637d936 453 };
edy555 0:55201637d936 454
edy555 0:55201637d936 455 static inline
edy555 0:55201637d936 456 uint32_t cos_sin(int16_t x)
edy555 0:55201637d936 457 {
edy555 0:55201637d936 458 int idx = x >> 8;
edy555 0:55201637d936 459 int mod = x & 0xff;
edy555 0:55201637d936 460 int r = __PKHBT(0x00000100, mod, 16);
edy555 0:55201637d936 461 uint32_t *e = (uint32_t *)&cos_sin_table[idx+128];
edy555 0:55201637d936 462 uint32_t c = e[0];
edy555 0:55201637d936 463 uint32_t s = e[1];
edy555 0:55201637d936 464 c = __SMUAD(r, c);
edy555 0:55201637d936 465 s = __SMUAD(r, s);
edy555 0:55201637d936 466 c >>= 8;
edy555 0:55201637d936 467 s >>= 8;
edy555 0:55201637d936 468 return __PKHBT(s, c, 16);
edy555 0:55201637d936 469 }
edy555 0:55201637d936 470
edy555 0:55201637d936 471 FMModState fmmod;
edy555 0:55201637d936 472
edy555 0:55201637d936 473 void
edy555 0:55201637d936 474 fmmod_init(FMModState *fmmod)
edy555 0:55201637d936 475 {
edy555 0:55201637d936 476 fmmod->vec = 0x7fff0000;
edy555 0:55201637d936 477 }
edy555 0:55201637d936 478
edy555 0:55201637d936 479 void
edy555 0:55201637d936 480 frequency_modulation(FMModState *fmmod, uint32_t *src, uint32_t *dst, int dst_len)
edy555 0:55201637d936 481 {
edy555 0:55201637d936 482 int j;
edy555 0:55201637d936 483 uint32_t vec = fmmod->vec;
edy555 0:55201637d936 484 for (j = 0; j < dst_len; j++) {
edy555 0:55201637d936 485 uint32_t s = *src++;
edy555 0:55201637d936 486 // fetch only R-ch (top half of word)
edy555 0:55201637d936 487 int16_t x = s >> 16;
edy555 0:55201637d936 488 uint32_t cs = cos_sin(x);
edy555 0:55201637d936 489 uint32_t real = __SMUSD(vec, cs);
edy555 0:55201637d936 490 uint32_t imag = __SMUADX(vec, cs);
edy555 0:55201637d936 491 real >>= 15;
edy555 0:55201637d936 492 imag >>= 15;
edy555 0:55201637d936 493 vec = __PKHBT(imag, real, 16);
edy555 0:55201637d936 494 *dst++ = vec;
edy555 0:55201637d936 495 }
edy555 0:55201637d936 496 #if 1
edy555 0:55201637d936 497 uint32_t mag = __SMUAD(vec, vec);
edy555 0:55201637d936 498 if (mag < 0x10000) {
edy555 0:55201637d936 499 // force initialize
edy555 0:55201637d936 500 vec = 0x7fff0000;
edy555 0:55201637d936 501 } else if (mag < 0x3ff00000) {
edy555 0:55201637d936 502 uint32_t d = __PKHBT((int16_t)(vec&0xffff) >> 12, vec >> 12, 0);
edy555 0:55201637d936 503 vec = __QADD16(vec, d);
edy555 0:55201637d936 504 }
edy555 0:55201637d936 505 #endif
edy555 0:55201637d936 506 fmmod->vec = vec;
edy555 0:55201637d936 507 }
edy555 0:55201637d936 508
edy555 0:55201637d936 509 void
edy555 0:55201637d936 510 amplitude_modulation(uint32_t *src, uint32_t *dst, int dst_len)
edy555 0:55201637d936 511 {
edy555 0:55201637d936 512 int j;
edy555 0:55201637d936 513 for (j = 0; j < dst_len; j++) {
edy555 0:55201637d936 514 uint32_t s = *src++;
edy555 0:55201637d936 515 // fetch only R-ch (top half of word) and halving it
edy555 0:55201637d936 516 int16_t x = s >> 16;
edy555 0:55201637d936 517 // add DC and set zero at quadrature
edy555 0:55201637d936 518 x /= 2;
edy555 0:55201637d936 519 x += 0x4000;
edy555 0:55201637d936 520 *dst++ = x & 0xffff;
edy555 0:55201637d936 521 }
edy555 0:55201637d936 522 }