firmware: SX1262 GFSK wM-Bus C1 repeater, dev tooling, discovery scripts
This commit is contained in:
@@ -0,0 +1,135 @@
|
||||
#include "wmbus.h"
|
||||
|
||||
uint16_t wmbus_crc16(const uint8_t *data, size_t len)
|
||||
{
|
||||
// EN 13757-4: poly 0x3D65, init 0x0000, MSB first, final complement
|
||||
uint16_t crc = 0;
|
||||
for (size_t i = 0; i < len; i++) {
|
||||
crc ^= (uint16_t)data[i] << 8;
|
||||
for (int b = 0; b < 8; b++) {
|
||||
crc = (crc & 0x8000) ? (crc << 1) ^ 0x3D65 : (crc << 1);
|
||||
}
|
||||
}
|
||||
return ~crc;
|
||||
}
|
||||
|
||||
wmbus_frame_fmt_t wmbus_frame_format(uint8_t b0, uint8_t b1)
|
||||
{
|
||||
if (b0 != WMBUS_MODE_BYTE_A_HI) return WMBUS_FRAME_UNKNOWN;
|
||||
if (b1 == WMBUS_MODE_BYTE_A_LO) return WMBUS_FRAME_A;
|
||||
if (b1 == WMBUS_MODE_BYTE_B_LO) return WMBUS_FRAME_B;
|
||||
return WMBUS_FRAME_UNKNOWN;
|
||||
}
|
||||
|
||||
int wmbus_frame_total_len(wmbus_frame_fmt_t fmt, uint8_t lfield)
|
||||
{
|
||||
int total;
|
||||
switch (fmt) {
|
||||
case WMBUS_FRAME_A:
|
||||
// L counts C,M,A,CI,data without CRCs. Block 1 holds 9 payload
|
||||
// bytes; every further block holds up to 16, each with 2 CRC bytes.
|
||||
if (lfield < 9) return -1;
|
||||
total = 1 + lfield + 2 * (((lfield - 10 + 15) / 16) + 1);
|
||||
break;
|
||||
case WMBUS_FRAME_B:
|
||||
// L counts everything after itself including CRC bytes.
|
||||
if (lfield < 11) return -1;
|
||||
total = 1 + lfield;
|
||||
break;
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
if (total > 253) return -1; // must fit SX1262 payload with 2 mode bytes
|
||||
return total;
|
||||
}
|
||||
|
||||
bool wmbus_check_crc(wmbus_frame_fmt_t fmt, const uint8_t *frame, int total_len)
|
||||
{
|
||||
if (total_len < 12) return false;
|
||||
|
||||
if (fmt == WMBUS_FRAME_A) {
|
||||
// Block 1: 10 bytes + CRC
|
||||
if (wmbus_crc16(frame, 10) !=
|
||||
(((uint16_t)frame[10] << 8) | frame[11])) return false;
|
||||
int pos = 12;
|
||||
while (pos < total_len) {
|
||||
int n = total_len - pos - 2;
|
||||
if (n > 16) n = 16;
|
||||
if (n <= 0) return false;
|
||||
if (wmbus_crc16(frame + pos, n) !=
|
||||
(((uint16_t)frame[pos + n] << 8) | frame[pos + n + 1])) return false;
|
||||
pos += n + 2;
|
||||
}
|
||||
return pos == total_len;
|
||||
}
|
||||
|
||||
if (fmt == WMBUS_FRAME_B) {
|
||||
// Blocks 1+2 share one CRC over their concatenation. Frames longer
|
||||
// than 128 bytes carry further 128-byte chunks, each CRCed over its
|
||||
// first 126 bytes (mirrors rtl-wmbus check_calc_crc_wmbus_b_frame_type).
|
||||
const uint8_t *p = frame;
|
||||
int rem = total_len;
|
||||
while (rem) {
|
||||
if (rem >= 128) {
|
||||
if (wmbus_crc16(p, 126) !=
|
||||
(((uint16_t)p[126] << 8) | p[127])) return false;
|
||||
p += 128; rem -= 128;
|
||||
} else {
|
||||
if (rem < 3) return false;
|
||||
if (wmbus_crc16(p, rem - 2) !=
|
||||
(((uint16_t)p[rem - 2] << 8) | p[rem - 1])) return false;
|
||||
rem = 0;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
uint32_t wmbus_hash(const uint8_t *data, size_t len)
|
||||
{
|
||||
uint32_t h = 2166136261u;
|
||||
for (size_t i = 0; i < len; i++) {
|
||||
h ^= data[i];
|
||||
h *= 16777619u;
|
||||
}
|
||||
return h;
|
||||
}
|
||||
|
||||
int wmbus_build_test_frame(uint8_t *out, size_t maxlen, uint8_t access_nr)
|
||||
{
|
||||
// Format A frame: L C M(2) A(6) | CRC | CI ...data... | CRC
|
||||
// Manufacturer "SLN" = ((19)<<10)|(12<<5)|(14) = 0x4D8E (Solenos test)
|
||||
// ID 0x00000001 (BCD 00000001), version 0x01, device type 0x07 (water)
|
||||
uint8_t body[26];
|
||||
int L = 19; // C M A CI + 10 data bytes = 1+2+6+1+9... keep L=19: C(1)+M(2)+A(6)+CI(1)+data(9)
|
||||
body[0] = (uint8_t)L;
|
||||
body[1] = 0x44; // SND_NR
|
||||
body[2] = 0x8E; body[3] = 0x4D; // manufacturer, little endian
|
||||
body[4] = 0x01; body[5] = 0x00; body[6] = 0x00; body[7] = 0x00; // ID BCD LE
|
||||
body[8] = 0x01; // version
|
||||
body[9] = 0x07; // device type
|
||||
body[10] = 0x78; // CI: response, no header
|
||||
// 9 data bytes: marker + access counter
|
||||
body[11] = 0x53; body[12] = 0x4F; body[13] = 0x4C; body[14] = 0x54; // "SOLT"
|
||||
body[15] = 0x58; // "X"
|
||||
body[16] = access_nr;
|
||||
body[17] = 0xA5; body[18] = 0x5A; body[19] = access_nr ^ 0xFF;
|
||||
|
||||
// assemble with block CRCs
|
||||
int total = wmbus_frame_total_len(WMBUS_FRAME_A, (uint8_t)L);
|
||||
if (total < 0 || (size_t)total > maxlen) return -1;
|
||||
|
||||
// block 1: bytes 0..9 + crc
|
||||
for (int i = 0; i < 10; i++) out[i] = body[i];
|
||||
uint16_t c1 = wmbus_crc16(out, 10);
|
||||
out[10] = c1 >> 8; out[11] = c1 & 0xFF;
|
||||
// block 2: bytes 10..(L) of body (CI + data = L-9 bytes) + crc
|
||||
int n2 = L - 9;
|
||||
for (int i = 0; i < n2; i++) out[12 + i] = body[10 + i];
|
||||
uint16_t c2 = wmbus_crc16(out + 12, n2);
|
||||
out[12 + n2] = c2 >> 8; out[13 + n2] = c2 & 0xFF;
|
||||
|
||||
return total;
|
||||
}
|
||||
Reference in New Issue
Block a user