328 lines
12 KiB
C
328 lines
12 KiB
C
#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 and excludes every CRC byte. Block 1
|
|
// carries the L-field plus C,M,A (10 bytes) with its own CRC, so 9
|
|
// L-counted bytes. The remaining (L - 9) bytes are split into
|
|
// blocks of at most 16, each followed by 2 CRC bytes.
|
|
//
|
|
// rtl-wmbus uses `2 * ((L - 10 + 15)/16 + 1)`, i.e. floor((L+5)/16)+1
|
|
// CRC pairs. That differs from the decomposition above by one pair
|
|
// whenever L = 10 (mod 16), where it would have to place 17 bytes in
|
|
// a 16-byte block. wmbusmeters' trimCRCsFrameFormatA walks greedy
|
|
// 16-byte blocks and accepts a final block of 1..16 bytes, which
|
|
// matches the form used here.
|
|
//
|
|
// Both forms agree on every VALID length observed at this site
|
|
// (L = 54, 57, 62, 67, 87, 95, 98, 102). No valid L = 10 (mod 16)
|
|
// frame was ever captured, so this is an internal-consistency fix
|
|
// found by firmware/test/test_wmbus.c, not a fix for an observed
|
|
// interoperability failure: with the old formula such a frame would
|
|
// have been computed 2 bytes short and always failed CRC.
|
|
if (lfield < 9) return -1;
|
|
total = 1 + lfield + 2 * (1 + ((lfield - 9 + 15) / 16));
|
|
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;
|
|
}
|
|
|
|
// 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;
|
|
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,
|
|
uint8_t lfield)
|
|
{
|
|
// Format A frame: L C M(2) A(6) | CRC | CI ...data... | CRC per block
|
|
// Manufacturer "SLN" = ((19)<<10)|(12<<5)|(14) = 0x4D8E (Solenos test)
|
|
// ID 0x00000001 (BCD 00000001), version 0x01, device type 0x07 (water)
|
|
uint8_t body[256];
|
|
if (lfield < 19) lfield = 19;
|
|
if (lfield > 200) lfield = 200;
|
|
int L = lfield;
|
|
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
|
|
// data: marker + access counter + PRBS-ish padding
|
|
body[11] = 0x53; body[12] = 0x4F; body[13] = 0x4C; body[14] = 0x54; // "SOLT"
|
|
body[15] = 0x58; // "X"
|
|
body[16] = access_nr;
|
|
uint8_t x = 0xA5 ^ access_nr;
|
|
for (int i = 17; i <= L; i++) {
|
|
x = (uint8_t)(x * 197 + 31);
|
|
body[i] = x;
|
|
}
|
|
|
|
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;
|
|
// blocks 2..n: 16 data bytes + crc each (last block short)
|
|
int src = 10; // next body byte (CI onward), body holds L+1 bytes
|
|
int dst = 12;
|
|
while (src <= L) {
|
|
int n = L + 1 - src;
|
|
if (n > 16) n = 16;
|
|
for (int i = 0; i < n; i++) out[dst + i] = body[src + i];
|
|
uint16_t c = wmbus_crc16(out + dst, n);
|
|
out[dst + n] = c >> 8; out[dst + n + 1] = c & 0xFF;
|
|
src += n;
|
|
dst += n + 2;
|
|
}
|
|
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;
|
|
}
|