Add repeater protocol and device firmware features

This commit is contained in:
Lars Nolden
2026-09-21 12:30:50 +02:00
parent 73d3ddf00c
commit a11a4b3b89
41 changed files with 7791 additions and 228 deletions
+164
View File
@@ -103,6 +103,81 @@ bool wmbus_check_crc(wmbus_frame_fmt_t fmt, const uint8_t *frame, int total_len)
return false;
}
// Map a CRC-free logical byte offset to its position in the raw frame.
// Header fields can straddle a format A block boundary.
static int raw_offset(wmbus_frame_fmt_t fmt, int offset)
{
if (fmt == WMBUS_FRAME_A)
return offset < 10 ? offset : offset + 2 + 2 * ((offset - 10) / 16);
return offset + 2 * (offset / 126);
}
bool wmbus_mark_repeated(wmbus_frame_fmt_t fmt, uint8_t *frame, int total_len)
{
if (total_len < 12 || wmbus_frame_total_len(fmt, frame[0]) != total_len)
return false;
if (fmt == WMBUS_FRAME_B && total_len % 128 != 0 && total_len % 128 < 3)
return false;
int data_len = fmt == WMBUS_FRAME_A ? frame[0] + 1
: total_len - 2 * ((total_len + 127) / 128);
if (data_len <= 10 || (frame[1] != 0x44 && frame[1] != 0x46))
return false;
uint8_t ci = frame[raw_offset(fmt, 10)];
int control;
uint8_t hop_mask;
if (ci == 0x8C || ci == 0x8E) {
// Require the complete ELL and a following CI. Its contents remain
// opaque: this link control field is outside the secured payload.
int header_len = ci == 0x8C ? 3 : 11;
if (data_len <= 10 + header_len) return false;
control = raw_offset(fmt, 11);
hop_mask = 0x10;
} else {
int header_len;
switch (ci) {
// OMS uplink CI values with short / long transport headers.
case 0x67: case 0x6E: case 0x74: case 0x7A: case 0x7D:
case 0x7F: case 0x88: case 0x8A: case 0x9E: case 0xC1:
case 0xC4:
header_len = 5;
break;
case 0x68: case 0x6F: case 0x72: case 0x75: case 0x7C:
case 0x7E: case 0x87: case 0x8B: case 0x9F: case 0xC2:
case 0xC5:
header_len = 13;
break;
default:
// Do not guess through unknown or secured encapsulations.
return false;
}
if (data_len < 10 + header_len) return false;
int cf = 10 + header_len - 2;
uint8_t mode = frame[raw_offset(fmt, cf + 1)] & 0x1F;
if (mode != 0 && mode != 5) return false;
control = raw_offset(fmt, cf);
hop_mask = 0x01;
}
// Modern OMS has separate H and R bits, not a two-bit hop count.
if (frame[control] & (hop_mask | 0x02)) return false;
frame[control] |= hop_mask;
int start, count;
if (fmt == WMBUS_FRAME_A) {
start = 12 + 18 * ((control - 12) / 18);
count = total_len - start - 2;
if (count > 16) count = 16;
} else {
start = 128 * (control / 128);
count = total_len - start - 2;
if (count > 126) count = 126;
}
uint16_t crc = wmbus_crc16(frame + start, (size_t)count);
frame[start + count] = crc >> 8;
frame[start + count + 1] = crc & 0xFF;
return true;
}
uint32_t wmbus_hash(const uint8_t *data, size_t len)
{
uint32_t h = 2166136261u;
@@ -161,3 +236,92 @@ int wmbus_build_test_frame(uint8_t *out, size_t maxlen, uint8_t access_nr,
}
return total;
}
// ---- T-mode "3 of 6" codec --------------------------------------------
// nibble -> 6-bit code (EN 13757-4 table 3; inverse of the rtl-wmbus
// LOW_NIBBLE_3OUTOF6 lookup)
static const uint8_t enc3of6[16] = {
0x16, 0x0D, 0x0E, 0x0B, 0x1C, 0x19, 0x1A, 0x13,
0x2C, 0x25, 0x26, 0x23, 0x34, 0x31, 0x32, 0x29,
};
// 6-bit code -> nibble, 0xFF invalid (rtl-wmbus LOW_NIBBLE_3OUTOF6)
static const uint8_t dec3of6[64] = {
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0x03, 0xFF, 0x01, 0x02, 0xFF,
0xFF, 0xFF, 0xFF, 0x07, 0xFF, 0xFF, 0x00, 0xFF,
0xFF, 0x05, 0x06, 0xFF, 0x04, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0x0B, 0xFF, 0x09, 0x0A, 0xFF,
0xFF, 0x0F, 0xFF, 0xFF, 0x08, 0xFF, 0xFF, 0xFF,
0xFF, 0x0D, 0x0E, 0xFF, 0x0C, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
};
int wmbus_3of6_coded_len(int total_len)
{
return (total_len * 12 + 7) / 8;
}
static unsigned get6(const uint8_t *b, size_t len, size_t bitpos)
{
size_t byte = bitpos >> 3;
unsigned shift = (unsigned)(bitpos & 7);
unsigned w = (unsigned)b[byte] << 8;
if (byte + 1 < len) w |= b[byte + 1];
return (w >> (10 - shift)) & 0x3F;
}
int wmbus_3of6_decode(const uint8_t *coded, size_t coded_len,
uint8_t *out, int total_len)
{
if (total_len < 0 || (size_t)total_len * 12 > coded_len * 8) return -1;
int bad = 0;
size_t bit = 0;
for (int i = 0; i < total_len; i++) {
uint8_t hi = dec3of6[get6(coded, coded_len, bit)];
uint8_t lo = dec3of6[get6(coded, coded_len, bit + 6)];
bit += 12;
if (hi == 0xFF) { hi = 0; bad++; }
if (lo == 0xFF) { lo = 0; bad++; }
out[i] = (uint8_t)((hi << 4) | lo);
}
return bad;
}
int wmbus_3of6_first_byte(const uint8_t *coded, size_t coded_len)
{
if (coded_len < 2) return -1;
uint8_t hi = dec3of6[get6(coded, coded_len, 0)];
uint8_t lo = dec3of6[get6(coded, coded_len, 6)];
if (hi == 0xFF || lo == 0xFF) return -1;
return (hi << 4) | lo;
}
int wmbus_3of6_encode(const uint8_t *data, int total_len,
uint8_t *coded, size_t maxlen)
{
int coded_len = wmbus_3of6_coded_len(total_len);
if (total_len < 0 || (size_t)coded_len > maxlen) return -1;
uint32_t acc = 0;
int acc_bits = 0;
size_t pos = 0;
for (int i = 0; i < 2 * total_len; i++) {
uint8_t nib = (i & 1) ? (data[i >> 1] & 0x0F)
: (data[i >> 1] >> 4);
acc = (acc << 6) | enc3of6[nib];
acc_bits += 6;
while (acc_bits >= 8) {
coded[pos++] = (uint8_t)(acc >> (acc_bits - 8));
acc_bits -= 8;
}
}
if (acc_bits > 0) {
// pad the tail to the byte boundary with 01 postamble chips
uint8_t tail = (uint8_t)(acc << (8 - acc_bits));
tail |= (uint8_t)(0x55 & (0xFF >> acc_bits));
coded[pos++] = tail;
}
return (int)pos;
}