1 /*
2  *    Copyright (C) 2018
3  *    Matthias P. Braendli (matthias.braendli@mpb.li)
4  *
5  *    Copyright (C) 2017
6  *    Albrecht Lohofener (albrechtloh@gmx.de)
7  *
8  *    This file is based on SDR-J
9  *    Copyright (C) 2010, 2011, 2012
10  *    Jan van Katwijk (J.vanKatwijk@gmail.com)
11  *
12  *    This file is part of the welle.io.
13  *    Many of the ideas as implemented in welle.io are derived from
14  *    other work, made available through the GNU general Public License.
15  *    All copyrights of the original authors are recognized.
16  *
17  *    welle.io is free software; you can redistribute it and/or modify
18  *    it under the terms of the GNU General Public License as published by
19  *    the Free Software Foundation; either version 2 of the License, or
20  *    (at your option) any later version.
21  *
22  *    welle.io is distributed in the hope that it will be useful,
23  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
24  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
25  *    GNU General Public License for more details.
26  *
27  *    You should have received a copy of the GNU General Public License
28  *    along with welle.io; if not, write to the Free Software
29  *    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
30  *
31  */
32 
33 #ifndef MATHHELPER_H
34 #define MATHHELPER_H
35 
36 #include <complex>
37 #include <cstring>
38 
39 #define Hz(x) (x)
40 #define kHz(x) (x * 1000)
41 #define MHz(x) (kHz(x) * 1000)
42 
43 static inline float get_db_over_256(float x)
44 {
45     return 20 * log10((x + 1.0f) / 256.0f);
46 }
47 
48 static inline float l1_norm(const std::complex<float>& z)
49 {
50     return std::abs(z.real()) + std::abs(z.imag());
51 }
52 
53 static inline bool check_CRC_bits(const uint8_t *in, int size)
54 {
55     static const uint8_t crcPolynome[] = { 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 }; // MSB .. LSB
56     uint8_t b[16];
57     memset(b, 1, 16);
58 
59     for (int i = 0; i < size; i++) {
60         uint8_t d = in[i];
61         if (i >= size - 16) {
62             d ^= 1;
63         }
64 
65         if ((b[0] ^ d) == 1) {
66             for (int f = 0; f < 15; f++)
67                 b[f] = crcPolynome[f] ^ b[f + 1];
68             b[15] = 1;
69         }
70         else {
71             memmove(&b[0], &b[1], sizeof(uint8_t) * 15); // Shift
72             b[15] = 0;
73         }
74     }
75 
76     uint16_t crc = 0;
77     for (int i = 0; i < 16; i++)
78         crc |= b[i] << i;
79     return crc == 0;
80 }
81 
82 static inline bool check_crc_bytes(const uint8_t *msg, int len)
83 {
84     uint16_t accumulator = 0xFFFF;
85     const uint16_t genpoly = 0x1021;
86 
87     for (int i = 0; i < len; i++) {
88         int16_t data = msg[i] << 8;
89         for (int j = 8; j > 0; j--) {
90             if ((data ^ accumulator) & 0x8000)
91                 accumulator = ((accumulator << 1) ^ genpoly) & 0xFFFF;
92             else
93                 accumulator = (accumulator << 1) & 0xFFFF;
94             data = (data << 1) & 0xFFFF;
95         }
96     }
97     //
98     // ok, now check with the crc that is contained
99     // in the au
100     uint16_t crc = ~((msg[len] << 8) | msg[len + 1]) & 0xFFFF;
101     return (crc ^ accumulator) == 0;
102 }
103 
104 static inline uint32_t getBits(const uint8_t* d, int16_t offset, uint8_t size)
105 {
106     if (size > 32) {
107         throw std::logic_error("getBits called with size>32");
108     }
109 
110     uint32_t res = 0;
111 
112     for (int i = 0; i < size; i++) {
113         res <<= 1;
114         res |= d[offset + i];
115     }
116     return res;
117 }
118 
119 static inline uint16_t getBits_1(const uint8_t* d, int16_t offset)
120 {
121     return (d[offset] & 0x01);
122 }
123 
124 static inline uint16_t getBits_2(const uint8_t* d, int16_t offset)
125 {
126     uint16_t res = d[offset];
127     res <<= 1;
128     res |= d[offset + 1];
129     return res;
130 }
131 
132 static inline uint16_t getBits_3(const uint8_t* d, int16_t offset)
133 {
134     uint16_t res = d[offset];
135     res <<= 1;
136     res |= d[offset + 1];
137     res <<= 1;
138     res |= d[offset + 2];
139     return res;
140 }
141 
142 static inline uint16_t getBits_4(const uint8_t* d, int16_t offset)
143 {
144     uint16_t res = d[offset];
145     res <<= 1;
146     res |= d[offset + 1];
147     res <<= 1;
148     res |= d[offset + 2];
149     res <<= 1;
150     res |= d[offset + 3];
151     return res;
152 }
153 
154 static inline uint16_t getBits_5(const uint8_t* d, int16_t offset)
155 {
156     uint16_t res = d[offset];
157     res <<= 1;
158     res |= d[offset + 1];
159     res <<= 1;
160     res |= d[offset + 2];
161     res <<= 1;
162     res |= d[offset + 3];
163     res <<= 1;
164     res |= d[offset + 4];
165     return res;
166 }
167 
168 static inline uint16_t getBits_6(const uint8_t* d, int16_t offset)
169 {
170     uint16_t res = d[offset];
171     res <<= 1;
172     res |= d[offset + 1];
173     res <<= 1;
174     res |= d[offset + 2];
175     res <<= 1;
176     res |= d[offset + 3];
177     res <<= 1;
178     res |= d[offset + 4];
179     res <<= 1;
180     res |= d[offset + 5];
181     return res;
182 }
183 
184 static inline uint16_t getBits_7(const uint8_t* d, int16_t offset)
185 {
186     uint16_t res = d[offset];
187     res <<= 1;
188     res |= d[offset + 1];
189     res <<= 1;
190     res |= d[offset + 2];
191     res <<= 1;
192     res |= d[offset + 3];
193     res <<= 1;
194     res |= d[offset + 4];
195     res <<= 1;
196     res |= d[offset + 5];
197     res <<= 1;
198     res |= d[offset + 6];
199     return res;
200 }
201 
202 static inline uint16_t getBits_8(const uint8_t* d, int16_t offset)
203 {
204     uint16_t res = d[offset];
205     res <<= 1;
206     res |= d[offset + 1];
207     res <<= 1;
208     res |= d[offset + 2];
209     res <<= 1;
210     res |= d[offset + 3];
211     res <<= 1;
212     res |= d[offset + 4];
213     res <<= 1;
214     res |= d[offset + 5];
215     res <<= 1;
216     res |= d[offset + 6];
217     res <<= 1;
218     res |= d[offset + 7];
219     return res;
220 }
221 
222 #endif // MATHHELPER_H
223