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[120];
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 : // at this point we need to start escaping "%" characters
98 : char* strPtr = &str[59];
99 : for(size_t j = 0; j < line_len; j++) {
100 : char c = data[i+j];
101 : if((c < ' ') || (c > '~')) {
102 : c = '.';
103 : } else if(c == '%') {
104 : *strPtr++ = '%';
105 : }
106 : sprintf(strPtr++, "%c", c);
107 :
108 : }
109 : for(size_t j = line_len; j < 16; j++) {
110 : sprintf(strPtr++, " ");
111 : }
112 : if(level) {
113 : RADIOLIB_DEBUG_PRINT_LVL("", "%s", level);
114 : }
115 : RADIOLIB_DEBUG_PRINT("%s", str);
116 : RADIOLIB_DEBUG_PRINTLN();
117 : rem_len -= 16;
118 : }
119 :
120 : #else
121 : // outside of debug, this does nothing
122 : (void)level;
123 : (void)data;
124 : (void)len;
125 : (void)offset;
126 : (void)width;
127 : (void)be;
128 :
129 : #endif
130 0 : }
131 :
132 : #if RADIOLIB_DEBUG
133 : // https://github.com/esp8266/Arduino/blob/65579d29081cb8501e4d7f786747bf12e7b37da2/cores/esp8266/Print.cpp#L50
134 : size_t rlb_printf(bool ts, const char* format, ...) {
135 : va_list arg;
136 : va_start(arg, format);
137 : char temp[64];
138 : char* buffer = temp;
139 : RadioLibTime_t timestamp = rlb_time_us();
140 : unsigned long sec = timestamp/1000000UL;
141 : unsigned long usec = timestamp%1000000UL;
142 : size_t len_ts = 0;
143 : if(ts) { len_ts = snprintf(temp, sizeof(temp), "[%lu.%06lu] ", sec, usec); }
144 : size_t len_str = vsnprintf(&temp[len_ts], sizeof(temp) - len_ts, format, arg);
145 : size_t len = len_ts + len_str;
146 : va_end(arg);
147 : if (len > sizeof(temp) - 1) {
148 : buffer = new char[len + 1];
149 : if (!buffer) {
150 : return 0;
151 : }
152 : va_start(arg, format);
153 : if(ts) { len_ts = snprintf(buffer, len_ts + 1, "[%lu.%06lu] ", sec, usec); }
154 : vsnprintf(buffer + len_ts, len_str + 1, format, arg);
155 : va_end(arg);
156 : }
157 : #if defined(RADIOLIB_BUILD_ARDUINO)
158 : len = RADIOLIB_DEBUG_PORT.write(reinterpret_cast<const uint8_t*>(buffer), len);
159 : #else
160 : len = fwrite(buffer, sizeof(temp[0]), len, RADIOLIB_DEBUG_PORT);
161 : #endif
162 : if (buffer != temp) {
163 : delete[] buffer;
164 : }
165 : return len;
166 : }
167 : #endif
|
