kopia lustrzana https://github.com/rpp0/gr-lora
422 wiersze
14 KiB
C++
422 wiersze
14 KiB
C++
/* -*- c++ -*- */
|
|
/*
|
|
* Copyright 2017 Pieter Robyns, William Thenaers.
|
|
*
|
|
* This is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3, or (at your option)
|
|
* any later version.
|
|
*
|
|
* This software is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this software; see the file COPYING. If not, write to
|
|
* the Free Software Foundation, Inc., 51 Franklin Street,
|
|
* Boston, MA 02110-1301, USA.
|
|
*/
|
|
|
|
#ifndef UTILITIES_H
|
|
#define UTILITIES_H
|
|
|
|
#include <cstdint>
|
|
#include <string.h>
|
|
#include <iomanip>
|
|
#include <sstream>
|
|
|
|
#define MAC_CRC_SIZE 2u
|
|
#define MAX_PWR_QUEUE_SIZE 4
|
|
#define SM(value, shift, mask) (((value) << (shift)) & (mask))
|
|
#define MS(value, mask, shift) (((value) & (mask)) >> (shift))
|
|
|
|
namespace gr {
|
|
namespace lora {
|
|
static std::vector<std::string> term_colors = {
|
|
"\e[0m",
|
|
"\e[1m\e[91m",
|
|
"\e[1m\e[92m",
|
|
"\e[1m\e[93m",
|
|
"\e[1m\e[94m",
|
|
"\e[1m\e[95m",
|
|
"\e[1m\e[96m",
|
|
"\e[1m\e[97m",
|
|
"\e[1m\e[31m",
|
|
"\e[1m\e[32m",
|
|
"\e[1m\e[33m",
|
|
"\e[1m\e[34m",
|
|
"\e[1m\e[35m",
|
|
"\e[1m\e[36m"
|
|
};
|
|
|
|
/**
|
|
* \brief Wrap indices Python-like, i.e. array[wrap_index(-1, array_length)] gets the last element.
|
|
*
|
|
* \param i
|
|
* Index of array
|
|
* \param n
|
|
* Length of array
|
|
*/
|
|
inline int32_t wrap_index(int32_t i, int32_t n) {
|
|
return ((i % n) + n) % n;
|
|
}
|
|
|
|
/**
|
|
* \brief Clamp given value in the given range.
|
|
*
|
|
* \tparam T
|
|
* The type of variable to clamp.
|
|
* <br>`d`, `min` and `max` must be of this type.
|
|
* \param d
|
|
* The value to clamp.
|
|
* \param min
|
|
* The lower bound of the range.
|
|
* \param max
|
|
* The upper bound of the range.
|
|
*/
|
|
template <class T>
|
|
inline T clamp(const T d, const T min, const T max) {
|
|
const T t = d < min ? min : d;
|
|
return t > max ? max : t;
|
|
}
|
|
|
|
/**
|
|
* \brief Rotate the given bits to the left and return the result.
|
|
*
|
|
* \param bits
|
|
* The value to rotate.
|
|
* \param count
|
|
* The amount of bits to rotate (shift to left and add to right).
|
|
* \param size
|
|
* The size in bits used in `bits`.
|
|
* <BR>e.g. 1 byte given => size = 8
|
|
* <BR>e.g. only 6 bits in use => size = 6, and all bits higher than (1 << size-1) will be zeroed.
|
|
*/
|
|
inline uint32_t rotl(uint32_t bits, uint32_t count = 1u, const uint32_t size = 8u) {
|
|
const uint32_t len_mask = (1u << size) - 1u;
|
|
|
|
count %= size; // Limit bit rotate count to size
|
|
bits &= len_mask; // Limit given bits to size
|
|
|
|
return ((bits << count) & len_mask) | (bits >> (size - count));
|
|
}
|
|
|
|
/**
|
|
* \brief Return the `v` represented in a binary string.
|
|
*
|
|
* \tparam T
|
|
* The type of variable to convert.
|
|
* \param v
|
|
* The value to convert.
|
|
* \param bitwidth
|
|
* The length in bits of the given variable `v`.
|
|
*/
|
|
template <typename T>
|
|
inline std::string to_bin(const T v, const uint32_t bitwidth) {
|
|
#ifdef LSB_FIRST
|
|
const uint64_t maxpow = bitwidth ? (1ull << (bitwidth - 1)) : 0;
|
|
uint64_t mask;
|
|
|
|
std::string result = "";
|
|
|
|
for (mask = 0x1; mask <= maxpow; mask <<= 1) {
|
|
result += (v & mask) ? "1" : "0";
|
|
}
|
|
#else
|
|
uint64_t mask = bitwidth ? (1ull << bitwidth) : 0;
|
|
|
|
std::string result = "";
|
|
|
|
while(mask >>= 1) {
|
|
result += (v & mask) ? "1" : "0";
|
|
}
|
|
#endif
|
|
|
|
return result;
|
|
}
|
|
|
|
/**
|
|
* \brief Append the data in a given vector to an output stream with a comma delimiter.
|
|
*
|
|
* \tparam T
|
|
* The type of variable to append.
|
|
* \param out
|
|
* The output stream to append to.
|
|
* \param v
|
|
* The vector containing the data to append.
|
|
* \param prefix
|
|
* A prefix to include before appending the data.
|
|
* \param element_len_bits
|
|
* The length in bits of the data in `v`.
|
|
*/
|
|
template <typename T>
|
|
inline void print_vector_bin(std::ostream& out, const std::vector<T>& v, const std::string& prefix, const int element_len_bits) {
|
|
out << prefix << ": ";
|
|
|
|
for (const T& x : v)
|
|
out << to_bin(x, element_len_bits) << ", ";
|
|
|
|
out << std::endl << std::flush;
|
|
}
|
|
|
|
/**
|
|
* \brief Check whether the parity of the given binary string is even.
|
|
*
|
|
* \param word
|
|
* The string to check.
|
|
* \param even
|
|
* Check for even (`true`) or uneven (`false`) parity.
|
|
*/
|
|
inline bool check_parity_string(const std::string& word, const bool even = true) {
|
|
size_t count = 0, i = 0;
|
|
|
|
while(i < 7) {
|
|
if (word[i++] == '1')
|
|
++count;
|
|
}
|
|
|
|
return (count & 0x1) == !even;
|
|
}
|
|
|
|
/**
|
|
* \brief Check whether the parity of the given uint64_t is even.
|
|
* <BR>See https://graphics.stanford.edu/~seander/bithacks.html for more.
|
|
*
|
|
* \param word
|
|
* The uint64_t to check.
|
|
* \param even
|
|
* Check for even (`true`) or uneven (`false`) parity.
|
|
*/
|
|
inline bool check_parity(uint64_t word, const bool even = true) {
|
|
word ^= word >> 1;
|
|
word ^= word >> 2;
|
|
word = (word & 0x1111111111111111UL) * 0x1111111111111111UL;
|
|
|
|
return ((word >> 60ull) & 1ull) == !even;
|
|
}
|
|
|
|
/**
|
|
* \brief Select the bits in data given by the indices in `*indices`.
|
|
*
|
|
* \param data
|
|
* The data to select bits from.
|
|
* \param *indices
|
|
* Array with the indices to select.
|
|
* \param n
|
|
* The amount of indices.
|
|
*/
|
|
inline uint32_t select_bits(const uint32_t data, const uint8_t *indices, const uint8_t n) {
|
|
uint32_t r = 0u;
|
|
|
|
for(uint8_t i = 0u; i < n; ++i)
|
|
r |= (data & (1u << indices[i])) ? (1u << i) : 0u;
|
|
|
|
return r;
|
|
}
|
|
|
|
/**
|
|
* \brief Select a single bit from the given byte.
|
|
*
|
|
* \param v
|
|
* The byte to select from.
|
|
* \param i
|
|
* The index to select the bit from starting from the LSB.
|
|
*/
|
|
inline uint8_t bit(const uint8_t v, const uint8_t i) {
|
|
return ((v >> i) & 0x01);
|
|
}
|
|
|
|
/**
|
|
* \brief Pack the given 8 bits in a byte with: `hgfe dcba`
|
|
*
|
|
* \param a-h
|
|
* The bits to pack with the LSB first.
|
|
*/
|
|
inline uint8_t pack_byte(const uint8_t a, const uint8_t b, const uint8_t c, const uint8_t d,
|
|
const uint8_t e, const uint8_t f, const uint8_t g, const uint8_t h) {
|
|
return a | (b << 1) | (c << 2) | (d << 3) | (e << 4) | (f << 5) | (g << 6) | (h << 7);
|
|
}
|
|
|
|
/**
|
|
* \brief Pack the given 4 bits in a nibble with: `dcba`
|
|
*
|
|
* \param a-d
|
|
* The bits to pack with the LSB first.
|
|
*/
|
|
inline uint8_t pack_nibble(const uint8_t a, const uint8_t b, const uint8_t c, const uint8_t d) {
|
|
return a | (b << 1) | (c << 2) | (d << 3);
|
|
}
|
|
|
|
/**
|
|
* \brief Encode the given word with standard Hamming(7,4) and return a byte with the set parity bits.
|
|
*
|
|
* \param v
|
|
* The nibble to encode.
|
|
*/
|
|
inline uint8_t hamming_encode_soft(const uint8_t v) {
|
|
const uint8_t p1 = bit(v, 1) ^ bit(v, 2) ^ bit(v, 3);
|
|
const uint8_t p2 = bit(v, 0) ^ bit(v, 1) ^ bit(v, 2);
|
|
const uint8_t p3 = bit(v, 0) ^ bit(v, 1) ^ bit(v, 3);
|
|
const uint8_t p4 = bit(v, 0) ^ bit(v, 2) ^ bit(v, 3);
|
|
|
|
return pack_byte(p1, bit(v, 0), bit(v, 1), bit(v, 2), p2, bit(v, 3), p3, p4);
|
|
}
|
|
|
|
/**
|
|
* \brief Swap nibbles of a byte array.
|
|
*
|
|
* \param array
|
|
* Array of uint8_t bytes
|
|
* \param length
|
|
* Length of the array
|
|
*/
|
|
inline void swap_nibbles(uint8_t* array, uint32_t length) {
|
|
for(uint32_t i = 0; i < length; i++) {
|
|
array[i] = ((array[i] & 0x0f) << 4) | ((array[i] & 0xf0) >> 4);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* DEPRECATED
|
|
* \brief Hamming(8,4) decoding by constructing a Syndrome matrix LUT for XORing on parity errors.
|
|
*
|
|
* \param v
|
|
* The byte to decode.
|
|
* \return Returs a nibble containing the corrected data.
|
|
*/
|
|
static inline uint8_t hamming_decode_soft_byte(uint8_t v) {
|
|
// Precalculation
|
|
// Which bits are covered (including self)?
|
|
// p1 10110100
|
|
// p2 01111000
|
|
// p3 01100110
|
|
// p4 01010101
|
|
|
|
// Syndrome matrix = columns of "cover bits" above
|
|
/*
|
|
uint8_t H[16] = { 0u };
|
|
|
|
const uint8_t i0 = pack_nibble(1, 0, 0, 0),
|
|
i1 = pack_nibble(0, 1, 1, 1),
|
|
i2 = pack_nibble(1, 1, 1, 0),
|
|
i3 = pack_nibble(1, 1, 0, 1),
|
|
i4 = pack_nibble(0, 1, 0, 0),
|
|
i5 = pack_nibble(1, 0, 1, 1),
|
|
i6 = pack_nibble(0, 0, 1, 0),
|
|
i7 = pack_nibble(0, 0, 0, 1);
|
|
H[i0] = 0;
|
|
H[i1] = 1;
|
|
H[i2] = 2;
|
|
H[i3] = 3;
|
|
H[i4] = 4;
|
|
H[i5] = 5;
|
|
H[i6] = 6;
|
|
H[i7] = 7;
|
|
*/
|
|
|
|
static const uint8_t H[16] = { 0x0, 0x0, 0x4, 0x0, 0x6, 0x0, 0x0, 0x2,
|
|
0x7, 0x0, 0x0, 0x3, 0x0, 0x5, 0x1, 0x0 };
|
|
|
|
// Decode
|
|
// Bit positions for data bits in codeword
|
|
const uint8_t p1 = bit(v, 0),
|
|
p2 = bit(v, 4),
|
|
p3 = bit(v, 6),
|
|
p4 = bit(v, 7),
|
|
p1c = bit(v, 2) ^ bit(v, 3) ^ bit(v, 5),
|
|
p2c = bit(v, 1) ^ bit(v, 2) ^ bit(v, 3),
|
|
p3c = bit(v, 1) ^ bit(v, 2) ^ bit(v, 5),
|
|
p4c = bit(v, 1) ^ bit(v, 3) ^ bit(v, 5);
|
|
|
|
const uint8_t syndrome = pack_nibble((uint8_t)(p1 != p1c), (uint8_t)(p2 != p2c), (uint8_t)(p3 != p3c), (uint8_t)(p4 != p4c));
|
|
|
|
if (syndrome) {
|
|
v ^= 1u << H[syndrome];
|
|
}
|
|
|
|
return pack_nibble( bit(v, 1), bit(v, 2), bit(v, 3), bit(v, 5));
|
|
}
|
|
|
|
template <typename T>
|
|
inline void print_vector(std::ostream& out, const T* v, const std::string& prefix, const int size, const int element_len_bits) {
|
|
out << prefix << ": ";
|
|
|
|
for (int i = 0; i < size; i++)
|
|
out << to_bin(v[i], element_len_bits) << ", ";
|
|
|
|
out << std::endl << std::flush;
|
|
}
|
|
|
|
template <typename T>
|
|
inline void print_vector_hex(std::ostream& out, const T* v, const uint32_t size, bool endline, bool print_ascii) {
|
|
std::stringstream ss;
|
|
|
|
for (uint32_t i = 0u; i < size; i++) {
|
|
out << " " << std::hex << std::setw(2) << std::setfill('0') << (int)v[i];
|
|
if (v[i] >= ' ' && v[i] <= '~')
|
|
ss << v[i];
|
|
}
|
|
|
|
if (print_ascii)
|
|
out << " (" << ss.str() << ")";
|
|
|
|
if(endline)
|
|
out << std::endl;
|
|
|
|
out << std::flush;
|
|
}
|
|
|
|
template <typename T>
|
|
inline void print_interleave_matrix(std::ostream& out, const std::vector<T>& v, const uint32_t sf) {
|
|
uint32_t cr = v.size();
|
|
|
|
for(uint32_t i = 0; i < cr; i++)
|
|
out << "-";
|
|
out << std::endl;
|
|
|
|
out << "LSB" << std::endl;
|
|
|
|
for(int32_t i = sf-1; i >= 0; i--) {
|
|
for(int32_t j = 0; j < (int32_t)cr; j++) {
|
|
out << term_colors[wrap_index(j-i, (int32_t)sf)+1] << to_bin(v[j], sf)[i] << term_colors[0];
|
|
}
|
|
out << std::endl;
|
|
}
|
|
|
|
out << "MSB" << std::endl;
|
|
|
|
for(uint32_t i = 0; i < cr; i++)
|
|
out << "-";
|
|
out << std::endl;
|
|
|
|
out << std::flush;
|
|
}
|
|
|
|
inline bool header_checksum(const uint8_t* header) {
|
|
(void) header;
|
|
/*Found valid order for bit #0: (1, 4, 8, 9, 10, 11)
|
|
Found valid order for bit #1: (0, 2, 5, 8, 9, 10)
|
|
Found valid order for bit #2: (0, 3, 6, 9, 11)
|
|
Found valid order for bit #3: (1, 2, 3, 7, 8)
|
|
Found valid order for bit #4: (4, 5, 6, 7)*/
|
|
return true;
|
|
}
|
|
|
|
inline uint32_t dissect_packet(const void **header, uint32_t header_size, const uint8_t *buffer, uint32_t offset) {
|
|
(*header) = buffer + offset;
|
|
return offset + header_size;
|
|
}
|
|
|
|
inline uint32_t build_packet(uint8_t *buffer, uint32_t offset, const void* header, uint32_t header_size) {
|
|
memcpy(buffer + offset, header, header_size);
|
|
offset += header_size;
|
|
|
|
return offset;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
#endif /* UTILITIES_H */
|