1+ #include "math.h"
2+
3+ uint64_t subghz_protocol_blocks_reverse_key (uint64_t key , uint8_t bit_count ) {
4+ uint64_t reverse_key = 0 ;
5+ for (uint8_t i = 0 ; i < bit_count ; i ++ ) {
6+ reverse_key = reverse_key << 1 | bit_read (key , i );
7+ }
8+ return reverse_key ;
9+ }
10+
11+ uint8_t subghz_protocol_blocks_get_parity (uint64_t key , uint8_t bit_count ) {
12+ uint8_t parity = 0 ;
13+ for (uint8_t i = 0 ; i < bit_count ; i ++ ) {
14+ parity += bit_read (key , i );
15+ }
16+ return parity & 0x01 ;
17+ }
18+
19+ uint8_t subghz_protocol_blocks_crc4 (
20+ uint8_t const message [],
21+ size_t size ,
22+ uint8_t polynomial ,
23+ uint8_t init ) {
24+ uint8_t remainder = init << 4 ; // LSBs are unused
25+ uint8_t poly = polynomial << 4 ;
26+ uint8_t bit ;
27+
28+ while (size -- ) {
29+ remainder ^= * message ++ ;
30+ for (bit = 0 ; bit < 8 ; bit ++ ) {
31+ if (remainder & 0x80 ) {
32+ remainder = (remainder << 1 ) ^ poly ;
33+ } else {
34+ remainder = (remainder << 1 );
35+ }
36+ }
37+ }
38+ return remainder >> 4 & 0x0f ; // discard the LSBs
39+ }
40+
41+ uint8_t subghz_protocol_blocks_crc7 (
42+ uint8_t const message [],
43+ size_t size ,
44+ uint8_t polynomial ,
45+ uint8_t init ) {
46+ uint8_t remainder = init << 1 ; // LSB is unused
47+ uint8_t poly = polynomial << 1 ;
48+
49+ for (size_t byte = 0 ; byte < size ; ++ byte ) {
50+ remainder ^= message [byte ];
51+ for (uint8_t bit = 0 ; bit < 8 ; ++ bit ) {
52+ if (remainder & 0x80 ) {
53+ remainder = (remainder << 1 ) ^ poly ;
54+ } else {
55+ remainder = (remainder << 1 );
56+ }
57+ }
58+ }
59+ return remainder >> 1 & 0x7f ; // discard the LSB
60+ }
61+
62+ uint8_t subghz_protocol_blocks_crc8 (
63+ uint8_t const message [],
64+ size_t size ,
65+ uint8_t polynomial ,
66+ uint8_t init ) {
67+ uint8_t remainder = init ;
68+
69+ for (size_t byte = 0 ; byte < size ; ++ byte ) {
70+ remainder ^= message [byte ];
71+ for (uint8_t bit = 0 ; bit < 8 ; ++ bit ) {
72+ if (remainder & 0x80 ) {
73+ remainder = (remainder << 1 ) ^ polynomial ;
74+ } else {
75+ remainder = (remainder << 1 );
76+ }
77+ }
78+ }
79+ return remainder ;
80+ }
81+
82+ uint8_t subghz_protocol_blocks_crc8le (
83+ uint8_t const message [],
84+ size_t size ,
85+ uint8_t polynomial ,
86+ uint8_t init ) {
87+ uint8_t remainder = subghz_protocol_blocks_reverse_key (init , 8 );
88+ polynomial = subghz_protocol_blocks_reverse_key (polynomial , 8 );
89+
90+ for (size_t byte = 0 ; byte < size ; ++ byte ) {
91+ remainder ^= message [byte ];
92+ for (uint8_t bit = 0 ; bit < 8 ; ++ bit ) {
93+ if (remainder & 1 ) {
94+ remainder = (remainder >> 1 ) ^ polynomial ;
95+ } else {
96+ remainder = (remainder >> 1 );
97+ }
98+ }
99+ }
100+ return remainder ;
101+ }
102+
103+ uint16_t subghz_protocol_blocks_crc16lsb (
104+ uint8_t const message [],
105+ size_t size ,
106+ uint16_t polynomial ,
107+ uint16_t init ) {
108+ uint16_t remainder = init ;
109+
110+ for (size_t byte = 0 ; byte < size ; ++ byte ) {
111+ remainder ^= message [byte ];
112+ for (uint8_t bit = 0 ; bit < 8 ; ++ bit ) {
113+ if (remainder & 1 ) {
114+ remainder = (remainder >> 1 ) ^ polynomial ;
115+ } else {
116+ remainder = (remainder >> 1 );
117+ }
118+ }
119+ }
120+ return remainder ;
121+ }
122+
123+ uint16_t subghz_protocol_blocks_crc16 (
124+ uint8_t const message [],
125+ size_t size ,
126+ uint16_t polynomial ,
127+ uint16_t init ) {
128+ uint16_t remainder = init ;
129+
130+ for (size_t byte = 0 ; byte < size ; ++ byte ) {
131+ remainder ^= message [byte ] << 8 ;
132+ for (uint8_t bit = 0 ; bit < 8 ; ++ bit ) {
133+ if (remainder & 0x8000 ) {
134+ remainder = (remainder << 1 ) ^ polynomial ;
135+ } else {
136+ remainder = (remainder << 1 );
137+ }
138+ }
139+ }
140+ return remainder ;
141+ }
142+
143+ uint8_t subghz_protocol_blocks_lfsr_digest8 (
144+ uint8_t const message [],
145+ size_t size ,
146+ uint8_t gen ,
147+ uint8_t key ) {
148+ uint8_t sum = 0 ;
149+ for (size_t byte = 0 ; byte < size ; ++ byte ) {
150+ uint8_t data = message [byte ];
151+ for (int i = 7 ; i >= 0 ; -- i ) {
152+ // XOR key into sum if data bit is set
153+ if ((data >> i ) & 1 ) sum ^= key ;
154+
155+ // roll the key right (actually the LSB is dropped here)
156+ // and apply the gen (needs to include the dropped LSB as MSB)
157+ if (key & 1 )
158+ key = (key >> 1 ) ^ gen ;
159+ else
160+ key = (key >> 1 );
161+ }
162+ }
163+ return sum ;
164+ }
165+
166+ uint8_t subghz_protocol_blocks_lfsr_digest8_reflect (
167+ uint8_t const message [],
168+ size_t size ,
169+ uint8_t gen ,
170+ uint8_t key ) {
171+ uint8_t sum = 0 ;
172+ // Process message from last byte to first byte (reflected)
173+ for (int byte = size - 1 ; byte >= 0 ; -- byte ) {
174+ uint8_t data = message [byte ];
175+ // Process individual bits of each byte (reflected)
176+ for (uint8_t i = 0 ; i < 8 ; ++ i ) {
177+ // XOR key into sum if data bit is set
178+ if ((data >> i ) & 1 ) {
179+ sum ^= key ;
180+ }
181+
182+ // roll the key left (actually the LSB is dropped here)
183+ // and apply the gen (needs to include the dropped lsb as MSB)
184+ if (key & 0x80 )
185+ key = (key << 1 ) ^ gen ;
186+ else
187+ key = (key << 1 );
188+ }
189+ }
190+ return sum ;
191+ }
192+
193+ uint16_t subghz_protocol_blocks_lfsr_digest16 (
194+ uint8_t const message [],
195+ size_t size ,
196+ uint16_t gen ,
197+ uint16_t key ) {
198+ uint16_t sum = 0 ;
199+ for (size_t byte = 0 ; byte < size ; ++ byte ) {
200+ uint8_t data = message [byte ];
201+ for (int8_t i = 7 ; i >= 0 ; -- i ) {
202+ // if data bit is set then xor with key
203+ if ((data >> i ) & 1 ) sum ^= key ;
204+
205+ // roll the key right (actually the LSB is dropped here)
206+ // and apply the gen (needs to include the dropped LSB as MSB)
207+ if (key & 1 )
208+ key = (key >> 1 ) ^ gen ;
209+ else
210+ key = (key >> 1 );
211+ }
212+ }
213+ return sum ;
214+ }
215+
216+ uint8_t subghz_protocol_blocks_add_bytes (uint8_t const message [], size_t size ) {
217+ uint32_t result = 0 ;
218+ for (size_t i = 0 ; i < size ; ++ i ) {
219+ result += message [i ];
220+ }
221+ return (uint8_t )result ;
222+ }
223+
224+ uint8_t subghz_protocol_blocks_parity8 (uint8_t byte ) {
225+ byte ^= byte >> 4 ;
226+ byte &= 0xf ;
227+ return (0x6996 >> byte ) & 1 ;
228+ }
229+
230+ uint8_t subghz_protocol_blocks_parity_bytes (uint8_t const message [], size_t size ) {
231+ uint8_t result = 0 ;
232+ for (size_t i = 0 ; i < size ; ++ i ) {
233+ result ^= subghz_protocol_blocks_parity8 (message [i ]);
234+ }
235+ return result ;
236+ }
237+
238+ uint8_t subghz_protocol_blocks_xor_bytes (uint8_t const message [], size_t size ) {
239+ uint8_t result = 0 ;
240+ for (size_t i = 0 ; i < size ; ++ i ) {
241+ result ^= message [i ];
242+ }
243+ return result ;
244+ }
0 commit comments