stefan.schnitzerHere is the code (I have no time for github sry) //Arduino IDE 2.3.7
/*
RIIO_TINETZ – ESP32 – M-Bus (TINETZ Kundenschnittstelle) -> MQTT
- Reads segmented M-Bus long frames (0x68 ... 0x16), reassembles DLMS payload
- Parses General-GLO-Ciphering APDU (0xDB 0x08 ...)
- Handles sec_ctrl = 0x21 (Encryption-only, Auth not applied) by decrypting via AES-CTR (GCM-CTR)
- Extracts COSEM Data-Notification (0x0F) and publishes OBIS values to MQTT
- MQTT Watchdog toggles true/false every 30s
- Adds SSD1306 128x64 I2C OLED debug screen
- Sends M-Bus ACK (0xE5) after each valid long frame (CRC ok)
*/
#include <WiFi.h>
#include <MQTT.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "mbedtls/aes.h"
#include "mbedtls/gcm.h"
#include <math.h>
// ---------- Forward declarations (fix Arduino auto-prototype issues) ----------
struct AxdrValue;
class AxdrReader;
// ------------------------- USER CONFIG -------------------------
#define WIFI_SSID "wifissd"
#define WIFI_PASSWORD "wifipwd"
static const char* MQTT_HOST = "mqtt host ip";
static const uint16_t MQTT_PORT = mqtt host port ;
// Base topics (KEEP EXACT)
static const char* TOPIC_BASE = "tinetz/smartmeter";
static const char* TOPIC_STATUS_WD = "tinetz/smartmeter/status/watchdog";
static const char* TOPIC_DEBUG_SYS = "tinetz/smartmeter/debug/sys_title";
static const char* TOPIC_DEBUG_SC = "tinetz/smartmeter/debug/sec_ctrl";
static const char* TOPIC_DEBUG_IC = "tinetz/smartmeter/debug/invocation_counter";
static const char* TOPIC_DEBUG_DECFAIL = "tinetz/smartmeter/debug/decrypt_fail";
static const char* TOPIC_DEBUG_CIPHER = "tinetz/smartmeter/debug/cipher_hex";
// Your DML key (hex, 16 bytes / 32 hex chars)
static const char* DML_KEY_HEX = "Benutzerschnittstelle key from TINETZ ";
// ------------------------- PIN CONFIG -------------------------
// ESP32 UART2 pins
static const int MBUS_RX_PIN = 16;
static const int MBUS_TX_PIN = 17; // used for ACK 0xE5
static const uint32_t MBUS_BAUD = 2400;
// OLED I2C (SSD1306 128x64)
static const int OLED_SDA = 21;
static const int OLED_SCL = 22;
static const uint8_t OLED_ADDR = 0x3C; // change to 0x3D if needed
static const int OLED_RESET = -1;
// ------------------------- MQTT/WIFI -------------------------
WiFiClient net;
MQTTClient mqtt(4096);
// OLED object
Adafruit_SSD1306 display(128, 64, &Wire, OLED_RESET);
// ------------------------- RUNTIME / DEBUG STATE -------------------------
static uint8_t g_dmlKey[16];
static unsigned long g_lastMbusFrameMs = 0;
static unsigned long g_lastMbusByteMs = 0;
static uint32_t g_mbusFramesOk = 0;
static uint32_t g_mbusBadCrc = 0;
static uint32_t g_mbusResets = 0;
static uint32_t g_decryptFailCount = 0;
static unsigned long g_lastDecryptOkMs = 0;
static uint8_t g_lastSysTitle[8] = {0};
static uint8_t g_lastSecCtrl = 0;
static uint8_t g_lastInvCtr[4] = {0};
static String g_lastObis = "";
static String g_lastObisVal = "";
static unsigned long g_lastOledMs = 0;
static unsigned long g_lastSerialRestartMs = 0;
// ------------------------- HELPERS -------------------------
static bool hexNibble(char c, uint8_t& out) {
if (c >= '0' && c <= '9') { out = (uint8_t)(c - '0'); return true; }
if (c >= 'a' && c <= 'f') { out = (uint8_t)(10 + (c - 'a')); return true; }
if (c >= 'A' && c <= 'F') { out = (uint8_t)(10 + (c - 'A')); return true; }
return false;
}
static bool hexToBytes(const char* hex, uint8_t* out, size_t outLen) {
size_t n = strlen(hex);
if (n != outLen * 2) return false;
for (size_t i = 0; i < outLen; i++) {
uint8_t hi, lo;
if (!hexNibble(hex[2*i], hi) || !hexNibble(hex[2*i+1], lo)) return false;
out[i] = (uint8_t)((hi << 4) | lo);
}
return true;
}
static String bytesToHex(const uint8_t* b, size_t n) {
static const char* H = "0123456789ABCDEF";
String s;
s.reserve(n * 2);
for (size_t i = 0; i < n; i++) {
s += H[(b[i] >> 4) & 0x0F];
s += H[b[i] & 0x0F];
}
return s;
}
static void mqttPublish(const String& topic, const String& payload, bool retained=false, int qos=0) {
if (!mqtt.connected()) return;
mqtt.publish(topic, payload, retained, qos);
}
static void mqttPublishHex(const char* topic, const uint8_t* b, size_t n) {
mqttPublish(topic, bytesToHex(b, n), false, 0);
}
static double pow10i(int exp) {
double r = 1.0;
if (exp > 0) {
for (int i = 0; i < exp; i++) r *= 10.0;
} else if (exp < 0) {
for (int i = 0; i < (-exp); i++) r /= 10.0;
}
return r;
}
static bool allPrintableAscii(const uint8_t* b, size_t n) {
for (size_t i = 0; i < n; i++) {
if (b[i] < 0x20 || b[i] > 0x7E) return false;
}
return true;
}
static String unitName(uint8_t unitCode) {
switch (unitCode) {
case 27: return "W";
case 29: return "var";
case 30: return "Wh";
case 32: return "varh";
case 33: return "A";
case 35: return "V";
case 13: return "m3";
default: return String(unitCode);
}
}
// OBIS 6-byte -> "A-B:C.D.E.F"
static String obisToString(const uint8_t obis[6]) {
char buf[32];
snprintf(buf, sizeof(buf), "%u-%u:%u.%u.%u.%u",
obis[0], obis[1], obis[2], obis[3], obis[4], obis[5]);
return String(buf);
}
// Topic path for OBIS: "A-B/C.D.E.F"
static String obisToTopicPath(const String& obis) {
String t = obis;
t.replace(":", "/");
return t;
}
static String obisValueTopic(const String& obis) {
return String(TOPIC_BASE) + "/obis/" + obisToTopicPath(obis) + "/value";
}
static String obisUnitTopic(const String& obis) {
return String(TOPIC_BASE) + "/obis/" + obisToTopicPath(obis) + "/unit";
}
static String obisScalerTopic(const String& obis) {
return String(TOPIC_BASE) + "/obis/" + obisToTopicPath(obis) + "/scaler";
}
// ------------------------- AXDR PARSER (minimal + DateTime 0x0C) -------------------------
struct AxdrValue {
bool isNumber = false;
double number = 0.0;
bool isBytes = false;
const uint8_t* bytes = nullptr;
size_t bytesLen = 0;
bool isString = false;
String str;
};
class AxdrReader {
public:
AxdrReader(const uint8_t* data, size_t len) : p(data), n(len), pos(0) {}
bool has(size_t k=1) const { return (pos + k) <= n; }
bool readU8(uint8_t& v) { if (!has(1)) return false; v = p[pos++]; return true; }
bool peekU8(uint8_t& v) const { if (!has(1)) return false; v = p[pos]; return true; }
bool readBE16(uint16_t& v) {
if (!has(2)) return false;
v = (uint16_t)((p[pos] << 8) | p[pos+1]);
pos += 2; return true;
}
bool readBE32(uint32_t& v) {
if (!has(4)) return false;
v = ((uint32_t)p[pos] << 24) | ((uint32_t)p[pos+1] << 16) | ((uint32_t)p[pos+2] << 8) | (uint32_t)p[pos+3];
pos += 4; return true;
}
bool readBE64(uint64_t& v) {
if (!has(8)) return false;
v = 0;
for (int i = 0; i < 8; i++) v = (v << 8) | p[pos+i];
pos += 8; return true;
}
bool readBytes(const uint8_t*& b, size_t len) {
if (!has(len)) return false;
b = &p[pos];
pos += len;
return true;
}
size_t position() const { return pos; }
bool readData(AxdrValue& out, int depth=0) {
if (depth > 6) return false;
uint8_t tag;
if (!readU8(tag)) return false;
switch (tag) {
case 0x00: // null-data
out = AxdrValue();
return true;
case 0x03: { // boolean
uint8_t v; if (!readU8(v)) return false;
out.isString = true;
out.str = (v ? "true" : "false");
return true;
}
case 0x0F: { // integer (i8)
uint8_t b; if (!readU8(b)) return false;
int8_t v = (int8_t)b;
out.isNumber = true; out.number = (double)v;
return true;
}
case 0x10: { // long (i16)
uint16_t u; if (!readBE16(u)) return false;
int16_t v = (int16_t)u;
out.isNumber = true; out.number = (double)v;
return true;
}
case 0x11: { // unsigned (u8)
uint8_t v; if (!readU8(v)) return false;
out.isNumber = true; out.number = (double)v;
return true;
}
case 0x12: { // long-unsigned (u16)
uint16_t v; if (!readBE16(v)) return false;
out.isNumber = true; out.number = (double)v;
return true;
}
case 0x05: { // double-long (i32)
uint32_t u; if (!readBE32(u)) return false;
int32_t v = (int32_t)u;
out.isNumber = true; out.number = (double)v;
return true;
}
case 0x06: { // double-long-unsigned (u32)
uint32_t v; if (!readBE32(v)) return false;
out.isNumber = true; out.number = (double)v;
return true;
}
case 0x14: { // long64 (i64)
uint64_t u; if (!readBE64(u)) return false;
int64_t v = (int64_t)u;
out.isNumber = true; out.number = (double)v;
return true;
}
case 0x15: { // long64-unsigned (u64)
uint64_t v; if (!readBE64(v)) return false;
out.isNumber = true; out.number = (double)v;
return true;
}
case 0x16: { // enum (u8)
uint8_t v; if (!readU8(v)) return false;
out.isNumber = true; out.number = (double)v;
return true;
}
case 0x17: { // float32
const uint8_t* b; if (!readBytes(b, 4)) return false;
float f;
memcpy(&f, b, 4);
out.isNumber = true; out.number = (double)f;
return true;
}
case 0x18: { // float64
const uint8_t* b; if (!readBytes(b, 8)) return false;
double d;
memcpy(&d, b, 8);
out.isNumber = true; out.number = d;
return true;
}
case 0x09: { // octet-string (len u8)
uint8_t len; if (!readU8(len)) return false;
const uint8_t* b; if (!readBytes(b, len)) return false;
out.isBytes = true; out.bytes = b; out.bytesLen = len;
if (allPrintableAscii(b, len)) {
out.isString = true;
out.str = "";
out.str.reserve(len);
for (size_t i = 0; i < len; i++) out.str += (char)b[i];
}
return true;
}
case 0x0A: { // visible-string (len u8)
uint8_t len; if (!readU8(len)) return false;
const uint8_t* b; if (!readBytes(b, len)) return false;
out.isString = true;
out.str = "";
out.str.reserve(len);
for (size_t i = 0; i < len; i++) out.str += (char)b[i];
return true;
}
case 0x0C: { // date-time (fixed 12 bytes) <-- IMPORTANT for your payload
const uint8_t* b; if (!readBytes(b, 12)) return false;
out.isBytes = true; out.bytes = b; out.bytesLen = 12;
return true;
}
case 0x01: { // array (cnt u8)
uint8_t cnt; if (!readU8(cnt)) return false;
for (uint8_t i = 0; i < cnt; i++) {
AxdrValue tmp;
if (!readData(tmp, depth+1)) return false;
}
out = AxdrValue();
return true;
}
case 0x02: { // structure (cnt u8)
uint8_t cnt; if (!readU8(cnt)) return false;
for (uint8_t i = 0; i < cnt; i++) {
AxdrValue tmp;
if (!readData(tmp, depth+1)) return false;
}
out = AxdrValue();
return true;
}
default:
return false;
}
}
private:
const uint8_t* p;
size_t n;
size_t pos;
};
// Decode COSEM DateTime (12 bytes) -> ISO string (best-effort)
static String decodeCosemDateTime12(const uint8_t* b, size_t n) {
if (n != 12) return bytesToHex(b, n);
uint16_t year = (uint16_t)((b[0] << 8) | b[1]);
uint8_t month = b[2];
uint8_t day = b[3];
uint8_t hour = b[5];
uint8_t minute = b[6];
uint8_t second = b[7];
auto fix = [](uint8_t v, uint8_t fallback)->uint8_t { return (v == 0xFF ? fallback : v); };
month = fix(month, 1);
day = fix(day, 1);
hour = fix(hour, 0);
minute = fix(minute, 0);
second = fix(second, 0);
if (year == 0xFFFF) year = 1970;
char buf[32];
snprintf(buf, sizeof(buf), "%04u-%02u-%02uT%02u:%02u:%02u",
year, month, day, hour, minute, second);
return String(buf);
}
// ------------------------- DLMS DECRYPT -------------------------
static bool looksLikeDataNotification(const uint8_t* p, size_t n) {
return (n >= 1 && p[0] == 0x0F);
}
static bool readAsn1Length(const uint8_t* buf, size_t bufLen, size_t& pos, size_t& outLen) {
if (pos >= bufLen) return false;
uint8_t b = buf[pos++];
if (b <= 0x7F) { outLen = b; return true; }
if (b == 0x81) {
if (pos + 1 > bufLen) return false;
outLen = buf[pos++];
return true;
}
if (b == 0x82) {
if (pos + 2 > bufLen) return false;
outLen = ((size_t)buf[pos] << 8) | (size_t)buf[pos+1];
pos += 2;
return true;
}
return false;
}
// AES-CTR decrypt
static bool aesCtrDecrypt(const uint8_t* key16,
const uint8_t iv12[12],
uint32_t counterStart,
const uint8_t* in, size_t inLen,
uint8_t* out) {
mbedtls_aes_context aes;
mbedtls_aes_init(&aes);
if (mbedtls_aes_setkey_enc(&aes, key16, 128) != 0) {
mbedtls_aes_free(&aes);
return false;
}
uint8_t counterBlock[16];
memset(counterBlock, 0, sizeof(counterBlock));
memcpy(counterBlock, iv12, 12);
counterBlock[12] = (uint8_t)((counterStart >> 24) & 0xFF);
counterBlock[13] = (uint8_t)((counterStart >> 16) & 0xFF);
counterBlock[14] = (uint8_t)((counterStart >> 8) & 0xFF);
counterBlock[15] = (uint8_t)(counterStart & 0xFF);
size_t nc_off = 0;
uint8_t stream_block[16];
memset(stream_block, 0, sizeof(stream_block));
int rc = mbedtls_aes_crypt_ctr(&aes, inLen, &nc_off, counterBlock, stream_block, in, out);
mbedtls_aes_free(&aes);
return (rc == 0);
}
static bool aesGcmTryDecrypt(const uint8_t* key16,
const uint8_t iv12[12],
const uint8_t* aad, size_t aadLen,
const uint8_t* cipher, size_t cipherLen,
const uint8_t* tag, size_t tagLen,
uint8_t* out) {
mbedtls_gcm_context gcm;
mbedtls_gcm_init(&gcm);
if (mbedtls_gcm_setkey(&gcm, MBEDTLS_CIPHER_ID_AES, key16, 128) != 0) {
mbedtls_gcm_free(&gcm);
return false;
}
int rc = mbedtls_gcm_auth_decrypt(&gcm,
cipherLen,
iv12, 12,
aad, aadLen,
tag, tagLen,
cipher,
out);
mbedtls_gcm_free(&gcm);
return (rc == 0);
}
static bool decryptGeneralGloCiphering(const uint8_t* in, size_t inLen,
uint8_t* out, size_t outMax, size_t& outLen,
uint8_t sysTitle[8], uint8_t& secCtrl, uint8_t frameCounter[4]) {
outLen = 0;
if (inLen < 2 + 8 + 1 + 1 + 4) return false;
if (in[0] != 0xDB || in[1] != 0x08) return false;
memcpy(sysTitle, &in[2], 8);
size_t pos = 2 + 8;
size_t asnLen = 0;
if (!readAsn1Length(in, inLen, pos, asnLen)) return false;
if (pos + asnLen > inLen) return false;
if (asnLen < 5) return false;
secCtrl = in[pos++];
memcpy(frameCounter, &in[pos], 4);
pos += 4;
const uint8_t* encPayload = &in[pos];
size_t encPayloadLen = asnLen - 5;
uint8_t iv12[12];
memcpy(iv12, sysTitle, 8);
memcpy(iv12 + 8, frameCounter, 4);
bool authApplied = (secCtrl & 0x10) != 0;
bool encApplied = (secCtrl & 0x20) != 0;
if (!encApplied) return false;
// Keep debug topics unchanged
mqttPublishHex(TOPIC_DEBUG_SYS, sysTitle, 8);
mqttPublishHex(TOPIC_DEBUG_SC, &secCtrl, 1);
mqttPublishHex(TOPIC_DEBUG_IC, frameCounter, 4);
// Store for OLED
memcpy(g_lastSysTitle, sysTitle, 8);
g_lastSecCtrl = secCtrl;
memcpy(g_lastInvCtr, frameCounter, 4);
if (authApplied) {
const uint8_t aad1[1] = { secCtrl };
uint8_t aad2[5];
aad2[0] = secCtrl;
memcpy(&aad2[1], frameCounter, 4);
const size_t tagLens[2] = { 12, 16 };
const uint8_t* aads[2] = { aad1, aad2 };
const size_t aadLens[2] = { 1, 5 };
for (int tli = 0; tli < 2; tli++) {
size_t tagLen = tagLens[tli];
if (encPayloadLen <= tagLen) continue;
size_t cLen = encPayloadLen - tagLen;
const uint8_t* c = encPayload;
const uint8_t* tag = encPayload + cLen;
for (int ai = 0; ai < 2; ai++) {
if (cLen > outMax) continue;
if (aesGcmTryDecrypt(g_dmlKey, iv12, aads[ai], aadLens[ai], c, cLen, tag, tagLen, out)) {
outLen = cLen;
return true;
}
}
}
return false;
}
// A=0 => encryption-only. Try CTR with possible trailing tag stripping.
const size_t stripCandidates[3] = { 0, 12, 16 };
const uint32_t ctrCandidates[2] = { 2, 1 };
for (int si = 0; si < 3; si++) {
size_t strip = stripCandidates[si];
if (encPayloadLen <= strip) continue;
size_t cLen = encPayloadLen - strip;
const uint8_t* c = encPayload;
for (int ci = 0; ci < 2; ci++) {
uint32_t ctrStart = ctrCandidates[ci];
if (cLen > outMax) continue;
if (!aesCtrDecrypt(g_dmlKey, iv12, ctrStart, c, cLen, out)) continue;
if (looksLikeDataNotification(out, cLen)) {
outLen = cLen;
return true;
}
}
}
return false;
}
// ------------------------- DLMS PARSE + MQTT PUBLISH (ROBUST OBIS SCAN) -------------------------
static void publishObis(const uint8_t obis6[6], const AxdrValue& value, bool haveScalerUnit, int8_t scaler, uint8_t unit) {
String obisStr = obisToString(obis6);
String payload;
// Special-case: clock OBIS (0-0:1.0.0.255)
if (obisStr == "0-0:1.0.0.255") {
if (value.isBytes && value.bytesLen == 12) payload = decodeCosemDateTime12(value.bytes, value.bytesLen);
else if (value.isString) payload = value.str;
else if (value.isBytes) payload = bytesToHex(value.bytes, value.bytesLen);
else payload = "";
mqttPublish(obisValueTopic(obisStr), payload, false, 0);
g_lastObis = obisStr;
g_lastObisVal = payload;
return;
}
if (value.isNumber) {
double num = value.number;
int decimals = 0;
if (haveScalerUnit) {
num = num * pow10i((int)scaler);
if (scaler < 0) decimals = (int)(-scaler);
if (decimals > 6) decimals = 6;
}
payload = String(num, decimals);
} else if (value.isString) {
payload = value.str;
} else if (value.isBytes) {
if (allPrintableAscii(value.bytes, value.bytesLen)) {
payload = "";
payload.reserve(value.bytesLen);
for (size_t i = 0; i < value.bytesLen; i++) payload += (char)value.bytes[i];
} else if (value.bytesLen == 12) {
payload = decodeCosemDateTime12(value.bytes, value.bytesLen);
} else {
payload = bytesToHex(value.bytes, value.bytesLen);
}
} else {
payload = "";
}
mqttPublish(obisValueTopic(obisStr), payload, false, 0);
if (haveScalerUnit) {
mqttPublish(obisScalerTopic(obisStr), String((int)scaler), false, 0);
mqttPublish(obisUnitTopic(obisStr), unitName(unit), false, 0);
}
g_lastObis = obisStr;
g_lastObisVal = payload;
}
static bool tryParseScalerUnit(const uint8_t* p, size_t n, size_t& consumed, int8_t& scalerOut, uint8_t& unitOut) {
consumed = 0;
AxdrReader r(p, n);
uint8_t t;
if (!r.peekU8(t) || t != 0x02) return false;
uint8_t st, cnt;
if (!r.readU8(st) || st != 0x02) return false;
if (!r.readU8(cnt) || cnt != 2) return false;
AxdrValue s1, s2;
if (!r.readData(s1) || !r.readData(s2)) return false;
if (!s1.isNumber || !s2.isNumber) return false;
scalerOut = (int8_t)lround(s1.number);
unitOut = (uint8_t)lround(s2.number);
consumed = r.position();
return true;
}
static void parseAndPublishDlmsRobust(const uint8_t* apdu, size_t apduLen) {
if (apduLen < 1 || apdu[0] != 0x0F) return;
size_t pos = 1;
// skip long-invoke-id-and-priority (4 bytes)
if (pos + 4 > apduLen) return;
pos += 4;
// Optional date-time in Data-Notification:
// Your payload often uses 0x0C + 12 bytes (NOT 0x09 0x0C ...)
if (pos < apduLen) {
if (apdu[pos] == 0x0C) {
if (pos + 1 + 12 <= apduLen) pos += 1 + 12;
} else if (apdu[pos] == 0x09) {
if (pos + 2 <= apduLen) {
uint8_t l = apdu[pos+1];
if (pos + 2 + (size_t)l <= apduLen) pos += 2 + (size_t)l;
}
}
}
// Robust OBIS scan: look for 0x09 0x06 <6 bytes> and decode the next A-XDR value
for (size_t i = pos; i + 8 <= apduLen; ) {
if (apdu[i] == 0x09 && apdu[i+1] == 0x06) {
uint8_t obis6[6];
memcpy(obis6, &apdu[i+2], 6);
size_t valStart = i + 8;
if (valStart >= apduLen) break;
AxdrReader vr(&apdu[valStart], apduLen - valStart);
AxdrValue v;
if (!vr.readData(v)) {
// if decode fails, continue scanning a bit further
i += 2;
continue;
}
size_t consumedVal = vr.position();
bool haveSU = false;
int8_t scaler = 0;
uint8_t unit = 0;
size_t consumedSU = 0;
if (valStart + consumedVal < apduLen) {
haveSU = tryParseScalerUnit(&apdu[valStart + consumedVal],
apduLen - (valStart + consumedVal),
consumedSU, scaler, unit);
}
publishObis(obis6, v, haveSU, scaler, unit);
// advance past value (+ optional scaler/unit) so we don't re-detect inside the same block
i = valStart + consumedVal + consumedSU;
continue;
}
i++;
}
}
// ------------------------- M-BUS FRAME REASSEMBLY -------------------------
static uint8_t dlmsBuf[4096];
static size_t dlmsLen = 0;
static bool assembling = false;
static uint8_t expectedSeg = 0;
enum MbusState {
WAIT_START,
READ_LEN1,
READ_LEN2,
READ_START2,
READ_PAYLOAD,
READ_CHECKSUM,
READ_STOP
};
static MbusState mbusState = WAIT_START;
static uint8_t mbusLen1 = 0, mbusLen2 = 0;
static uint8_t framePayload[600];
static size_t framePayloadPos = 0;
static uint8_t frameChecksum = 0;
static void resetMbusParser() {
mbusState = WAIT_START;
mbusLen1 = mbusLen2 = 0;
framePayloadPos = 0;
frameChecksum = 0;
g_mbusResets++;
}
static uint8_t checksum8(const uint8_t* b, size_t n) {
uint16_t s = 0;
for (size_t i = 0; i < n; i++) s += b[i];
return (uint8_t)(s & 0xFF);
}
static void mbusSendAckE5() {
// M-Bus ACK short frame
Serial2.write((uint8_t)0xE5);
Serial2.flush(); // 1 byte @2400 baud ~4ms; helps if the other side waits for ACK
}
static void processCompleteDlmsMessage(const uint8_t* p, size_t n) {
if (n < 2 || p[0] != 0xDB || p[1] != 0x08) return;
static uint8_t plain[4096];
size_t plainLen = 0;
uint8_t sysTitle[8];
uint8_t secCtrl = 0;
uint8_t frameCounter[4];
bool ok = decryptGeneralGloCiphering(p, n, plain, sizeof(plain), plainLen, sysTitle, secCtrl, frameCounter);
mqttPublish(TOPIC_DEBUG_DECFAIL, ok ? "0" : "1", false, 0);
if (!ok) {
g_decryptFailCount++;
mqttPublish(TOPIC_DEBUG_CIPHER, bytesToHex(p, n) + " decrypt fail", false, 0);
return;
}
g_lastDecryptOkMs = millis();
parseAndPublishDlmsRobust(plain, plainLen);
}
static void handleMbusFrame(const uint8_t* payload, size_t payloadLen) {
if (payloadLen < 5) return;
uint8_t ci = payload[2];
bool fin = (ci & 0x10) != 0;
uint8_t seg = (ci & 0x0F);
const uint8_t* data = &payload[5];
size_t dataLen = payloadLen - 5;
if (!assembling) {
if (seg != 0) return;
assembling = true;
expectedSeg = 0;
dlmsLen = 0;
}
if (seg != expectedSeg) {
assembling = false;
dlmsLen = 0;
expectedSeg = 0;
return;
}
if (dlmsLen + dataLen > sizeof(dlmsBuf)) {
assembling = false;
dlmsLen = 0;
expectedSeg = 0;
return;
}
memcpy(&dlmsBuf[dlmsLen], data, dataLen);
dlmsLen += dataLen;
expectedSeg++;
if (fin) {
assembling = false;
expectedSeg = 0;
processCompleteDlmsMessage(dlmsBuf, dlmsLen);
dlmsLen = 0;
}
}
static void pollMbusUart() {
while (Serial2.available() > 0) {
uint8_t b = (uint8_t)Serial2.read();
g_lastMbusByteMs = millis();
switch (mbusState) {
case WAIT_START:
if (b == 0x68) mbusState = READ_LEN1;
break;
case READ_LEN1:
mbusLen1 = b;
mbusState = READ_LEN2;
break;
case READ_LEN2:
mbusLen2 = b;
if (mbusLen2 != mbusLen1) resetMbusParser();
else mbusState = READ_START2;
break;
case READ_START2:
if (b != 0x68) resetMbusParser();
else { framePayloadPos = 0; mbusState = READ_PAYLOAD; }
break;
case READ_PAYLOAD:
if (framePayloadPos < sizeof(framePayload)) framePayload[framePayloadPos++] = b;
else { resetMbusParser(); break; }
if (framePayloadPos >= mbusLen1) mbusState = READ_CHECKSUM;
break;
case READ_CHECKSUM:
frameChecksum = b;
mbusState = READ_STOP;
break;
case READ_STOP:
if (b == 0x16) {
uint8_t cs = checksum8(framePayload, framePayloadPos);
if (cs == frameChecksum) {
g_mbusFramesOk++;
g_lastMbusFrameMs = millis();
handleMbusFrame(framePayload, framePayloadPos);
mbusSendAckE5(); // <-- important for stability on some interfaces
} else {
g_mbusBadCrc++;
}
}
resetMbusParser();
break;
}
}
}
static void mbusHealthTick() {
// If we see no valid frames for a long time, reset UART & parser (harmless if meter is just idle)
unsigned long now = millis();
if (g_lastMbusFrameMs != 0 && (now - g_lastMbusFrameMs) > (10UL * 60UL * 1000UL)) {
if ((now - g_lastSerialRestartMs) > (60UL * 1000UL)) {
g_lastSerialRestartMs = now;
Serial2.end();
delay(50);
Serial2.begin(MBUS_BAUD, SERIAL_8E1, MBUS_RX_PIN, MBUS_TX_PIN);
resetMbusParser();
assembling = false;
expectedSeg = 0;
dlmsLen = 0;
}
}
}
// ------------------------- WATCHDOG -------------------------
static unsigned long lastWdMs = 0;
static bool wdState = false;
static void watchdogTick() {
unsigned long now = millis();
if (now - lastWdMs >= 30000UL) {
lastWdMs = now;
wdState = !wdState;
mqttPublish(TOPIC_STATUS_WD, wdState ? "true" : "false", false, 0);
}
}
// ------------------------- WIFI/MQTT CONNECT -------------------------
static void ensureWifi() {
if (WiFi.status() == WL_CONNECTED) return;
WiFi.mode(WIFI_STA);
WiFi.setSleep(false);
WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
unsigned long start = millis();
while (WiFi.status() != WL_CONNECTED && (millis() - start) < 20000UL) {
delay(250);
}
}
static void ensureMqtt() {
if (mqtt.connected()) return;
if (WiFi.status() != WL_CONNECTED) return;
String clientId = "tinetz-esp32-" + String((uint32_t)ESP.getEfuseMac(), HEX);
mqtt.begin(MQTT_HOST, MQTT_PORT, net);
if (mqtt.connect(clientId.c_str())) {
mqttPublish(TOPIC_STATUS_WD, wdState ? "true" : "false", false, 0);
}
}
// ------------------------- OLED -------------------------
static void oledInit() {
Wire.begin(OLED_SDA, OLED_SCL);
if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
// OLED optional: if not found, just continue
return;
}
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("RIIO_TINETZ");
display.println("Booting...");
display.display();
}
static void oledTick() {
unsigned long now = millis();
if (now - g_lastOledMs < 1000UL) return;
g_lastOledMs = now;
if (!display.width()) return; // not initialized (begin failed)
display.clearDisplay();
display.setCursor(0, 0);
display.setTextSize(1);
// Line 1: WiFi
if (WiFi.status() == WL_CONNECTED) {
display.print("WiFi: ");
display.println(WIFI_SSID);
} else {
display.println("WiFi: disconnected");
}
// Line 2: IP
if (WiFi.status() == WL_CONNECTED) {
display.print("IP: ");
display.println(WiFi.localIP());
} else {
display.println("IP: -");
}
// Line 3: MQTT
display.print("MQTT: ");
display.print(mqtt.connected() ? "OK " : "NO ");
display.println(MQTT_HOST);
// Line 4: MBus stats
display.print("MBus ok:");
display.print(g_mbusFramesOk);
display.print(" crc:");
display.println(g_mbusBadCrc);
// Line 5: last frame / decrypt
display.print("LastFrm:");
if (g_lastMbusFrameMs == 0) display.print("-");
else display.print((now - g_lastMbusFrameMs) / 1000UL);
display.print("s DecFail:");
display.println(g_decryptFailCount);
// Line 6: last OBIS
display.print("OBIS: ");
if (g_lastObis.length() == 0) display.println("-");
else {
String s = g_lastObis;
if (s.length() > 16) s = s.substring(0, 16);
display.println(s);
}
display.display();
}
// ------------------------- ARDUINO SETUP/LOOP -------------------------
void setup() {
Serial.begin(115200);
delay(200);
oledInit();
if (!hexToBytes(DML_KEY_HEX, g_dmlKey, sizeof(g_dmlKey))) {
Serial.println("DML key hex invalid (must be 32 hex chars).");
}
ensureWifi();
ensureMqtt();
Serial2.setRxBufferSize(2048);
Serial2.begin(MBUS_BAUD, SERIAL_8E1, MBUS_RX_PIN, MBUS_TX_PIN);
resetMbusParser();
lastWdMs = millis();
wdState = false;
mqttPublish(TOPIC_STATUS_WD, "false", false, 0);
}
void loop() {
ensureWifi();
ensureMqtt();
mqtt.loop();
pollMbusUart();
mbusHealthTick();
watchdogTick();
oledTick();
delay(2);
}