Line data Source code
1 : #include "Utils.h"
2 : #include "../Hal.h"
3 :
4 : #include <stdarg.h>
5 : #include <stdio.h>
6 : #include <stdlib.h>
7 : #include <string.h>
8 : #include <inttypes.h>
9 :
10 0 : uint32_t rlb_reflect(uint32_t in, uint8_t bits) {
11 0 : uint32_t res = 0;
12 0 : for(uint8_t i = 0; i < bits; i++) {
13 0 : res |= (((in & ((uint32_t)1 << i)) >> i) << (bits - i - 1));
14 : }
15 0 : return(res);
16 : }
17 :
18 : // fast-ish popcount function for use in calculating LFSR feedback value
19 : // without relying on __builtin_popcount() which may or may not be available
20 : // from https://stackoverflow.com/a/51388846
21 0 : static uint8_t rlb_popcount(uint32_t in) {
22 0 : in = (in & 0x55555555UL) + ((in >> 1) & 0x55555555UL);
23 0 : in = (in & 0x33333333UL) + ((in >> 2) & 0x33333333UL);
24 0 : in = (in & 0x0F0F0F0FUL) + ((in >> 4) & 0x0F0F0F0FUL);
25 0 : in = (in & 0x00FF00FFUL) + ((in >> 8) & 0x00FF00FFUL);
26 0 : in = (in & 0x0000FFFFUL) + ((in >>16) & 0x0000FFFFUL);
27 0 : return(in);
28 : }
29 :
30 0 : void rlb_scrambler(uint8_t* data, size_t len, const uint32_t poly, const uint32_t init, bool scramble) {
31 0 : if(!poly) {
32 0 : return;
33 : }
34 :
35 : // set the inital feedback register state
36 0 : uint32_t lsfr = init;
37 :
38 : // now do the shifting
39 0 : uint8_t out = 0;
40 0 : for(size_t i = 0; i < len; i++) {
41 : // for each data byte
42 0 : for(int j = 7; j >= 0; j--) {
43 : // for each bit in each data byte, calculate the next bit
44 0 : uint8_t feedback = rlb_popcount(lsfr & poly) & 1UL;
45 0 : uint8_t inbit = (data[i] & (1UL << j)) >> j;
46 0 : uint32_t nextbit = feedback ^ inbit;
47 :
48 : // shift the feedback register
49 0 : lsfr <<= 1;
50 :
51 : // now add the next bit
52 : // when scrambling, it is the calculated next bit
53 : // when descrambling, it is the input bit
54 0 : lsfr |= scramble ? nextbit : inbit;
55 :
56 : // shift the output buffer and add the next bit
57 0 : out <<= 1;
58 0 : out |= nextbit;
59 : }
60 :
61 : // extract the new byte
62 0 : data[i] = out;
63 0 : out = 0;
64 : }
65 : }
66 :
67 0 : void rlb_hexdump(const char* level, const uint8_t* data, size_t len, uint32_t offset, uint8_t width, bool be) {
68 : #if RADIOLIB_DEBUG
69 : size_t rem_len = len;
70 : for(size_t i = 0; i < len; i+=16) {
71 : char str[80];
72 : sprintf(str, "%08" PRIx32 ": ", (uint32_t)i+offset);
73 : size_t line_len = 16;
74 : if(rem_len < line_len) {
75 : line_len = rem_len;
76 : }
77 : for(size_t j = 0; j < line_len; j+=width) {
78 : if(width > 1) {
79 : int m = 0;
80 : int step = width/2;
81 : if(be) {
82 : step *= -1;
83 : }
84 : for(int32_t k = width - 1; k >= -width + 1; k+=step) {
85 : sprintf(&str[10 + (j+m)*3], "%02x ", data[i+j+k+m]);
86 : m++;
87 : }
88 : } else {
89 : sprintf(&str[10 + (j)*3], "%02x ", data[i+j]);
90 : }
91 : }
92 : for(size_t j = line_len; j < 16; j++) {
93 : sprintf(&str[10 + j*3], " ");
94 : }
95 : str[58] = ' ';
96 :
97 : for(size_t j = 0; j < line_len; j++) {
98 : char c = data[i+j];
99 : if((c < ' ') || (c > '~')) {
100 : c = '.';
101 : }
102 : sprintf(&str[58 + j], "%c", c);
103 : }
104 : for(size_t j = line_len; j < 16; j++) {
105 : sprintf(&str[58 + j], " ");
106 : }
107 : if(level) {
108 : RADIOLIB_DEBUG_PRINT_LVL("", "%s", level);
109 : }
110 :
111 : // to print the string-ified value, use write instead of the print macro
112 : // this will prevent accidentally interpreting '%' characters as control sequences
113 : #if defined(RADIOLIB_BUILD_ARDUINO)
114 : (void)RADIOLIB_DEBUG_PORT.write(reinterpret_cast<const uint8_t*>(str), strlen(str));
115 : #else
116 : (void)fwrite(str, sizeof(str[0]), strlen(str), RADIOLIB_DEBUG_PORT);
117 : #endif
118 : RADIOLIB_DEBUG_PRINTLN();
119 : rem_len -= 16;
120 : }
121 :
122 : #else
123 : // outside of debug, this does nothing
124 : (void)level;
125 : (void)data;
126 : (void)len;
127 : (void)offset;
128 : (void)width;
129 : (void)be;
130 :
131 : #endif
132 0 : }
133 :
134 : #if RADIOLIB_DEBUG
135 : // https://github.com/esp8266/Arduino/blob/65579d29081cb8501e4d7f786747bf12e7b37da2/cores/esp8266/Print.cpp#L50
136 : size_t rlb_printf(bool ts, const char* format, ...) {
137 : va_list arg;
138 : va_start(arg, format);
139 : char temp[64];
140 : char* buffer = temp;
141 : RadioLibTime_t timestamp = rlb_time_us();
142 : unsigned long sec = timestamp/1000000UL;
143 : unsigned long usec = timestamp%1000000UL;
144 : size_t len_ts = 0;
145 : if(ts) { len_ts = snprintf(temp, sizeof(temp), "[%lu.%06lu] ", sec, usec); }
146 : size_t len_str = vsnprintf(&temp[len_ts], sizeof(temp) - len_ts, format, arg);
147 : size_t len = len_ts + len_str;
148 : va_end(arg);
149 : if (len > sizeof(temp) - 1) {
150 : buffer = new char[len + 1];
151 : if (!buffer) {
152 : return 0;
153 : }
154 : va_start(arg, format);
155 : if(ts) { len_ts = snprintf(buffer, len_ts + 1, "[%lu.%06lu] ", sec, usec); }
156 : vsnprintf(buffer + len_ts, len_str + 1, format, arg);
157 : va_end(arg);
158 : }
159 : #if defined(RADIOLIB_BUILD_ARDUINO)
160 : len = RADIOLIB_DEBUG_PORT.write(reinterpret_cast<const uint8_t*>(buffer), len);
161 : #else
162 : len = fwrite(buffer, sizeof(temp[0]), len, RADIOLIB_DEBUG_PORT);
163 : #endif
164 : if (buffer != temp) {
165 : delete[] buffer;
166 : }
167 : return len;
168 : }
169 : #endif
|
