cs_Utils.h

Go to the documentation of this file.

 
#pragma once
 
#include <ble/cs_Nordic.h>  // required for __get_IPSR()
#include <logging/cs_Logger.h>
 
#include <string>
 
#include "protocol/cs_ErrorCodes.h"
#include "structs/cs_PacketsInternal.h"
 
namespace CsUtils {
 
inline uint16_t convertEndian16(uint16_t val) {
    return ((val >> 8) & 0xFF) | ((val & 0xFF) << 8);
}
 
inline uint32_t convertEndian32(uint32_t val) {
    return ((val >> 24) & 0xFF) | ((val >> 8) & 0xFF00) | ((val & 0xFF00) << 8) | ((val & 0xFF) << 24);
}
 
#define CS_ROUND_UP_TO_MULTIPLE_OF(num, multiple) (((num + multiple - 1) / multiple) * multiple)
#define CS_ROUND_UP_TO_MULTIPLE_OF_POWER_OF_2(num, multiple) ((num + multiple - 1) & -multiple)
 
template <class T, size_t N>
constexpr auto ArraySize(T (&)[N]) {
    return N;
}
 
template <typename T>
void printAddress(T* arr, uint16_t len, uint8_t verbosity = SERIAL_DEBUG, bool addNewLine = true) {
    _logArray(verbosity, addNewLine, arr, len, "", "", "%02X", ":", true);
}
 
inline void print_heap([[maybe_unused]] const std::string& msg) {
    uint8_t* p = (uint8_t*)malloc(128);
    LOGd("%s %p", msg.c_str(), p);
    free(p);
}
 
inline void print_stack([[maybe_unused]] const std::string& msg) {
    void* sp;
    asm("mov %0, sp" : "=r"(sp) : :);
    LOGd("%s %p", msg.c_str(), (uint8_t*)sp);
}
 
template <typename T>
inline bool isBitSet(const T value, uint8_t bit) {
    return value & (1 << bit);
}
 
template <typename T>
inline bool setBit(T& value, uint8_t bit) {
    return value |= (1 << bit);
}
 
template <typename T>
inline bool clearBit(T& value, uint8_t bit) {
    return value &= ~(1 << bit);
}
 
template <typename T>
inline constexpr T lowestBitSet(T value) {
    // TODO: there is a faster implementation for this.
    T numBits = sizeof(T) * 8;
    for (T i = 0; i < numBits; i++) {
        if (value & 1) {
            return i;
        }
        value >>= 1;
    }
    return numBits;
}
 
inline bool isNewer(uint8_t previousValue, uint8_t newValue) {
    int8_t diff = newValue - previousValue;
    return diff > 0;
}
 
inline static cs_ret_code_t findAdvType(uint8_t type, uint8_t* advData, uint8_t advLen, cs_data_t* foundData) {
    int index       = 0;
    foundData->data = nullptr;
    foundData->len  = 0;
    while (index < advLen - 1) {
        uint8_t fieldLen  = advData[index];
        uint8_t fieldType = advData[index + 1];
        // Check if length is not 0 or larger than remaining advertisement data.
        if (fieldLen == 0 || index + 1 + fieldLen > advLen) {
            return ERR_NOT_FOUND;
        }
 
        if (fieldType == type) {
            foundData->data = &advData[index + 2];
            foundData->len  = fieldLen - 1;
            return ERR_SUCCESS;
        }
        index += fieldLen + 1;
    }
    return ERR_NOT_FOUND;
}
 
constexpr uint16_t stringLen(const char* str, uint16_t maxSize) {
    uint16_t size = 0;
    while ((str[size] != '\0') && (size < maxSize)) {
        ++size;
    }
    return size;
}
 
inline uint32_t getInterruptLevel() {
    //  return __get_IPSR() & 0x1FF;
    return __get_IPSR();
    //  0 = Thread mode
    //  1 = Reserved
    //  2 = NMI
    //  3 = HardFault
    //  4 = MemManage
    //  5 = BusFault
    //  6 = UsageFault
    //  7-10 = Reserved
    //  11 = SVCall
    //  12 = Reserved for Debug
    //  13 = Reserved
    //  14 = PendSV
    //  15 = SysTick
    //  16 = IRQ0.
    //  17 = IRQ1.
    //  18 = IRQ2.
    //  ..
    //  n+16 = IRQn
}
 
}  // namespace CsUtils