diff --git a/examples/knx-linux/CMakeLists.txt b/examples/knx-linux/CMakeLists.txt index 0aa1f5b..fe87d43 100644 --- a/examples/knx-linux/CMakeLists.txt +++ b/examples/knx-linux/CMakeLists.txt @@ -112,6 +112,8 @@ add_executable(knx-linux ../../src/knx_facade.h ../../src/linux_platform.cpp ../../src/linux_platform.h + fdsk.cpp + fdsk.h main.cpp) target_link_libraries(knx-linux "${LIBRARIES_FROM_REFERENCES}") include_directories(../../src) diff --git a/examples/knx-linux/fdsk.cpp b/examples/knx-linux/fdsk.cpp new file mode 100644 index 0000000..6243718 --- /dev/null +++ b/examples/knx-linux/fdsk.cpp @@ -0,0 +1,189 @@ +#include "fdsk.h" + +#include +#include + +// CRC-4 generator polynom: 10011 (x^4+x+1) +const uint8_t FdskCalculator::crc4_tab[16] = +{ + 0x0, 0x3, 0x6, 0x5, 0xc, 0xf, 0xa, 0x9, + 0xb, 0x8, 0xd, 0xe, 0x7, 0x4, 0x1, 0x2 +}; + +void FdskCalculator::printFdsk(uint8_t* serialNumber, uint8_t* key) +{ + char* str = generateFdskString(serialNumber, key); + + uint8_t len = strlen(str); + printf("FDSK(len: %d): ", len); + for (int i = 0; i < 36; i++) + { + if (((i % 6) == 0) && (i!=0)) + printf("-"); + printf("%c", str[i]); + } + printf("\n"); + + delete[] str; +} + +char* FdskCalculator::generateFdskString(uint8_t* serialNumber, uint8_t* key) +{ + uint8_t buffer[6 + 16 + 1]; // 6 bytes serialnumber + 16 bytes key + 1 byte placeholder for crc-4 + memcpy(&buffer[0], serialNumber, 6); + memcpy(&buffer[6], key, 16); + buffer[22] = (crc4Array(buffer, sizeof(buffer)-1)<<4) &0xFF; + + uint8_t* outEncoded = nullptr; + toBase32(buffer, sizeof(buffer), outEncoded, false); + + return (char*)outEncoded; +} + +int FdskCalculator::ceil(float num) +{ + int inum = (int)num; + if (num == (float)inum) { + return inum; + } + return inum + 1; +} + +int FdskCalculator::toBase32(uint8_t* in, long length, uint8_t*& out, bool usePadding) +{ + char base32StandardAlphabet[] = {"ABCDEFGHIJKLMNOPQRSTUVWXYZ234567"}; + char standardPaddingChar = '='; + + int result = 0; + int count = 0; + int bufSize = 8; + int index = 0; + int size = 0; // size of temporary array + uint8_t* temp = nullptr; + + if (length < 0 || length > 268435456LL) + { + return 0; + } + + size = 8 * ceil(length / 4.0); // Calculating size of temporary array. Not very precise. + temp = new uint8_t[size]; + + if (length > 0) + { + int buffer = in[0]; + int next = 1; + int bitsLeft = 8; + + while (count < bufSize && (bitsLeft > 0 || next < length)) + { + if (bitsLeft < 5) + { + if (next < length) + { + buffer <<= 8; + buffer |= in[next] & 0xFF; + next++; + bitsLeft += 8; + } + else + { + int pad = 5 - bitsLeft; + buffer <<= pad; + bitsLeft += pad; + } + } + index = 0x1F & (buffer >> (bitsLeft -5)); + + bitsLeft -= 5; + temp[result] = (uint8_t)base32StandardAlphabet[index]; + result++; + } + } + + if (usePadding) + { + int pads = (result % 8); + if (pads > 0) + { + pads = (8 - pads); + for (int i = 0; i < pads; i++) + { + temp[result] = standardPaddingChar; + result++; + } + } + } + + out = new uint8_t[result]; + + memcpy(out, temp, result); + delete [] temp; + + return result; +} + +int FdskCalculator::fromBase32(uint8_t* in, long length, uint8_t*& out) +{ + int result = 0; // Length of the array of decoded values. + int buffer = 0; + int bitsLeft = 0; + uint8_t* temp = NULL; + + temp = new uint8_t[length]; // Allocating temporary array. + + for (int i = 0; i < length; i++) + { + uint8_t ch = in[i]; + + // ignoring some characters: ' ', '\t', '\r', '\n', '=' + if (ch == 0xA0 || ch == 0x09 || ch == 0x0A || ch == 0x0D || ch == 0x3D) + continue; + + // recovering mistyped: '0' -> 'O', '1' -> 'L', '8' -> 'B' + if (ch == 0x30) + { + ch = 0x4F; + } + else if (ch == 0x31) + { + ch = 0x4C; + } + else if (ch == 0x38) + { + ch = 0x42; + } + + + // look up one base32 symbols: from 'A' to 'Z' or from 'a' to 'z' or from '2' to '7' + if ((ch >= 0x41 && ch <= 0x5A) || (ch >= 0x61 && ch <= 0x7A)) + { + ch = ((ch & 0x1F) - 1); + } + else if (ch >= 0x32 && ch <= 0x37) + { + ch -= (0x32 - 26); + } + else { + delete [] temp; + return 0; + } + + buffer <<= 5; + buffer |= ch; + bitsLeft += 5; + if (bitsLeft >= 8) + { + temp[result] = (unsigned char)((unsigned int)(buffer >> (bitsLeft - 8)) & 0xFF); + result++; + bitsLeft -= 8; + } + } + + out = new uint8_t[result]; + memcpy(out, temp, result); + delete [] temp; + + return result; +} + diff --git a/examples/knx-linux/fdsk.h b/examples/knx-linux/fdsk.h new file mode 100644 index 0000000..2655f69 --- /dev/null +++ b/examples/knx-linux/fdsk.h @@ -0,0 +1,37 @@ +#pragma once + +#include + +class FdskCalculator +{ + public: + void printFdsk(uint8_t* serialNumber, uint8_t* key); + + private: + char* generateFdskString(uint8_t* serialNumber, uint8_t* key); + + int toBase32(uint8_t* in, long length, uint8_t*& out, bool usePadding); + int fromBase32(uint8_t* in, long length, uint8_t*& out); + + uint8_t crc4Array(uint8_t* data, uint8_t len) { + uint8_t start = 0; + for (uint8_t i = 0; i > 4; + c = crc4_tab[c ^ high4Bits]; + c = crc4_tab[c ^ low4Bits]; + + return c; + } + + int ceil(float num); + + static const uint8_t crc4_tab[16]; +}; diff --git a/examples/knx-linux/main.cpp b/examples/knx-linux/main.cpp index a5bbaa2..a680b8d 100644 --- a/examples/knx-linux/main.cpp +++ b/examples/knx-linux/main.cpp @@ -14,6 +14,8 @@ #include #include +#include "fdsk.h" + volatile sig_atomic_t loopActive = 1; void signalHandler(int sig) { @@ -46,130 +48,6 @@ long lastsend = 0; #define MIN knx.getGroupObject(3) #define RESET knx.getGroupObject(4) -int ceil(float num) { - int inum = (int)num; - if (num == (float)inum) { - return inum; - } - return inum + 1; -} - -int toBase32(uint8_t* in, long length, uint8_t*& out, bool usePadding) -{ - char base32StandardAlphabet[] = {"ABCDEFGHIJKLMNOPQRSTUVWXYZ234567"}; - char standardPaddingChar = '='; - - int result = 0; - int count = 0; - int bufSize = 8; - int index = 0; - int size = 0; // size of temporary array - uint8_t* temp = NULL; - - if (length < 0 || length > 268435456LL) - { - return 0; - } - - size = 8 * ceil(length / 4.0); // Calculating size of temporary array. Not very precise. - temp = (uint8_t*)malloc(size); // Allocating temporary array. - - if (length > 0) - { - int buffer = in[0]; - int next = 1; - int bitsLeft = 8; - - while (count < bufSize && (bitsLeft > 0 || next < length)) - { - if (bitsLeft < 5) - { - if (next < length) - { - buffer <<= 8; - buffer |= in[next] & 0xFF; - next++; - bitsLeft += 8; - } - else - { - int pad = 5 - bitsLeft; - buffer <<= pad; - bitsLeft += pad; - } - } - index = 0x1F & (buffer >> (bitsLeft -5)); - - bitsLeft -= 5; - temp[result] = (uint8_t)base32StandardAlphabet[index]; - result++; - } - } - - if (usePadding) - { - int pads = (result % 8); - if (pads > 0) - { - pads = (8 - pads); - for (int i = 0; i < pads; i++) - { - temp[result] = standardPaddingChar; - result++; - } - } - } - - out = (uint8_t*)malloc(result); - memcpy(out, temp, result); - free(temp); - - return result; -} - -int fromBase32(uint8_t* in, long length, uint8_t*& out) -{ - int result = 0; // Length of the array of decoded values. - int buffer = 0; - int bitsLeft = 0; - uint8_t* temp = NULL; - - temp = (uint8_t*)malloc(length); // Allocating temporary array. - - for (int i = 0; i < length; i++) - { - uint8_t ch = in[i]; - - // ignoring some characters: ' ', '\t', '\r', '\n', '=' - if (ch == 0xA0 || ch == 0x09 || ch == 0x0A || ch == 0x0D || ch == 0x3D) continue; - - // recovering mistyped: '0' -> 'O', '1' -> 'L', '8' -> 'B' - if (ch == 0x30) { ch = 0x4F; } else if (ch == 0x31) { ch = 0x4C; } else if (ch == 0x38) { ch = 0x42; } - - - // look up one base32 symbols: from 'A' to 'Z' or from 'a' to 'z' or from '2' to '7' - if ((ch >= 0x41 && ch <= 0x5A) || (ch >= 0x61 && ch <= 0x7A)) { ch = ((ch & 0x1F) - 1); } - else if (ch >= 0x32 && ch <= 0x37) { ch -= (0x32 - 26); } - else { free(temp); return 0; } - - buffer <<= 5; - buffer |= ch; - bitsLeft += 5; - if (bitsLeft >= 8) - { - temp[result] = (unsigned char)((unsigned int)(buffer >> (bitsLeft - 8)) & 0xFF); - result++; - bitsLeft -= 8; - } - } - - out = (uint8_t*)malloc(result); - memcpy(out, temp, result); - free(temp); - - return result; -} - void measureTemp() { long now = millis(); @@ -252,13 +130,11 @@ int main(int argc, char **argv) knx.masterReset(eraseCode, 0); } - uint8_t inPlain[] { 0x00, 0xFA, 0x01, 0x02, 0x03, 0x04, // KNX Serial - 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F}; // Key - uint8_t* outEncoded = NULL; + uint8_t serialNumber[] = { 0x00, 0xFA, 0x01, 0x02, 0x03, 0x04}; + uint8_t key[] = { 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F}; - uint8_t len = toBase32(inPlain, sizeof(inPlain), outEncoded, false); - - printf("FDSK(len: %d): %s\n", len, outEncoded); + FdskCalculator calc; + calc.printFdsk(serialNumber, key); // Prevent swapping of this process struct sched_param sp;