From a03d4113e773bb72566759509226e9cced4c6240 Mon Sep 17 00:00:00 2001 From: Gunnar Skjold Date: Mon, 3 Jan 2022 08:23:49 +0100 Subject: [PATCH] Added checksum verification for Mbus payload --- frames/Aidon-TN-3p.raw | 4 +- frames/Kamstup-Encrypted.raw | 2 +- frames/austria.raw | 107 +++++----------------- src/AmsToMqttBridge.ino | 86 ++++++++++++++++-- src/IEC6205675.cpp | 33 ++++++- src/IEC6205675.h | 2 +- src/MbusAssembler.cpp | 57 ++++++++++++ src/MbusAssembler.h | 18 ++++ src/ams/hdlc.cpp | 167 +++++++++++++++++++++++------------ src/ams/hdlc.h | 14 +++ 10 files changed, 341 insertions(+), 149 deletions(-) create mode 100644 src/MbusAssembler.cpp create mode 100644 src/MbusAssembler.h diff --git a/frames/Aidon-TN-3p.raw b/frames/Aidon-TN-3p.raw index 203f832d..f71a8664 100644 --- a/frames/Aidon-TN-3p.raw +++ b/frames/Aidon-TN-3p.raw @@ -3,7 +3,9 @@ 7E A1 1E 41 08 83 13 EE EE E6 E7 00 0F 40 00 00 00 00 01 0D 02 02 09 06 01 01 00 02 81 FF 0A 0B 41 49 44 4F 4E 5F 56 30 30 30 31 02 02 09 06 00 00 60 01 00 FF 0A 10 37 33 35 39 39 39 32 38 39 30 34 39 37 39 39 37 02 02 09 06 00 00 60 01 07 FF 0A 04 36 35 33 34 02 03 09 06 01 00 01 07 00 FF 06 00 00 08 6C 02 02 0F 00 16 1B 02 03 09 06 01 00 02 07 00 FF 06 00 00 00 00 02 02 0F 00 16 1B 02 03 09 06 01 00 03 07 00 FF 06 00 00 00 00 02 02 0F 00 16 1D 02 03 09 06 01 00 04 07 00 FF 06 00 00 02 09 02 02 0F 00 16 1D 02 03 09 06 01 00 1F 07 00 FF 10 00 41 02 02 0F FF 16 21 02 03 09 06 01 00 33 07 00 FF 10 00 13 02 02 0F FF 16 21 02 03 09 06 01 00 47 07 00 FF 10 00 0E 02 02 0F FF 16 21 02 03 09 06 01 00 20 07 00 FF 12 08 F2 02 02 0F FF 16 23 02 03 09 06 01 00 34 07 00 FF 12 08 D1 02 02 0F FF 16 23 02 03 09 06 01 00 48 07 00 FF 12 08 E8 02 02 0F FF 16 23 8B 7E A1 8A 41 08 83 13 EB FD E6 E7 00 0F 40 00 00 00 00 01 12 02 02 09 06 01 01 00 02 81 FF 0A 0B 41 49 44 4F 4E 5F 56 30 30 30 31 02 02 09 06 00 00 60 01 00 FF 0A 10 37 33 35 39 39 39 32 38 39 30 34 39 37 39 39 37 02 02 09 06 00 00 60 01 07 FF 0A 04 36 35 33 34 02 03 09 06 01 00 01 07 00 FF 06 00 00 03 9A 02 02 0F 00 16 1B 02 03 09 06 01 00 02 07 00 FF 06 00 00 00 00 02 02 0F 00 16 1B 02 03 09 06 01 00 03 07 00 FF 06 00 00 00 00 02 02 0F 00 16 1D 02 03 09 06 01 00 04 07 00 FF 06 00 00 02 0E 02 02 0F 00 16 1D 02 03 09 06 01 00 1F 07 00 FF 10 00 11 02 02 0F FF 16 21 02 03 09 06 01 00 33 07 00 FF 10 00 10 02 02 0F FF 16 21 02 03 09 06 01 00 47 07 00 FF 10 00 0E 02 02 0F FF 16 21 02 03 09 06 01 00 20 07 00 FF 12 08 F4 02 02 0F FF 16 23 02 03 09 06 01 00 34 07 00 FF 12 08 CD 02 02 0F FF 16 23 02 03 09 06 01 00 48 07 00 FF 12 08 DC 02 02 0F FF 16 23 02 02 09 06 00 00 01 00 00 FF 09 0C 07 E5 03 18 03 08 00 00 FF 00 00 00 02 03 09 06 01 00 01 08 00 FF 06 00 47 F0 34 02 02 0F 01 16 1E 02 03 09 06 01 00 02 08 00 FF 06 00 00 00 00 02 02 0F 01 16 1E 02 03 09 06 01 00 03 08 00 FF 06 00 00 21 9E 02 02 0F 01 16 20 02 03 09 06 01 00 04 08 00 FF 06 00 08 E0 21 02 02 0F 01 16 20 57 -7E A1 8A 41 08 83 13 EB FD E6 E7 00 0F 40 00 00 00 00 +7E A1 8A 41 08 83 13 EB FD E6 E7 00 +0F +40 00 00 00 00 01 12 02 02 09 06 01 01 00 02 81 FF 0A 0B 41 49 44 4F 4E 5F 56 30 30 30 31 02 02 09 06 00 00 60 01 00 FF 0A 10 37 33 35 39 39 39 32 38 39 30 34 39 37 39 39 37 diff --git a/frames/Kamstup-Encrypted.raw b/frames/Kamstup-Encrypted.raw index cad3363c..dce0eccd 100644 --- a/frames/Kamstup-Encrypted.raw +++ b/frames/Kamstup-Encrypted.raw @@ -5,7 +5,7 @@ A1 E9 // Frame type and size DB // Encrypted 08 4B 41 4D 45 01 AC 4D 6E // System title 82 // Prefix for 2-byte length -01 D0 // Length +01 D0 // Length 464 30 // Security tag 0011 0000, 0=Compression off, 0=Unicast, 1=Encryption, 0=Authentication, 0000= Security Suite ID 00 00 A3 2F // Frame counter diff --git a/frames/austria.raw b/frames/austria.raw index 1417688c..ba1c2112 100644 --- a/frames/austria.raw +++ b/frames/austria.raw @@ -19,86 +19,27 @@ F8 // Length (248), starting from 0xDB and including end byte 00 72 00 76 // Frame counter -Some complete frames - -68 01 01 68 53 FF 00 01 67 DB 08 53 41 47 59 05 E6 D9 FD 81 F8 20 00 69 D1 4F D7 32 A2 4E 08 32 D8 38 62 C0 -91 7E 0F C3 BF 47 83 9A 1C 8F 81 D8 BC DB 8D C8 06 D6 8C B3 F2 7A 64 FF F5 AE F8 74 31 7F F0 D8 D8 30 57 57 -D7 23 C1 5A 50 23 A2 56 C5 4E 1B A3 C1 FC 75 65 75 31 4F EF D3 71 C3 E9 B4 1E CD 61 3E BF A7 27 26 A7 48 B4 -64 E3 75 B5 4A A3 57 B1 C1 8C E2 25 8F D9 14 C6 6F 9B 6B EE EF 7E 0B 3E 1C 7E 53 7F D4 A6 9D 5F 3E 5E 0B 4A -61 BA 45 8F A4 0E D5 2D 88 F3 51 76 1D 90 78 8E 0F 29 43 D4 DF 9E 05 88 26 1F C9 4A 1D F2 C2 95 84 57 A8 95 -19 EF 45 7B E8 17 CE 59 B1 78 1D 0D 82 E4 58 3F 1A 76 D2 01 CF 65 75 3C 53 97 78 C0 8A 8A 31 94 E5 15 01 81 -EB 58 E6 95 34 3D C9 46 AF FC 57 EE 5A 6D 5E 6F 6A 21 15 D1 6B 7D 4F E2 A1 83 C4 3A 81 CA 1E C9 D0 73 84 E1 -60 E5 0E 80 BC D5 58 2D B9 1A 16 - -// 19b -68 0D 0D 68 53 FF 11 01 67 CD 6B CB 69 13 53 FF 98 34 16 - -// 263b -68 01 01 68 53 FF 00 01 67 DB 08 53 41 47 59 05 -E6 D9 FD 81 F8 20 00 69 D1 50 55 28 2C E9 97 46 -82 61 19 3E 23 78 8A E6 E2 42 D1 D6 44 BA 2C 3C -55 0E 59 47 02 DC 8D D4 10 91 67 6B 76 9B F0 2F -42 BF D9 D2 FE A2 B3 AA 11 B1 BF 7B 8B B3 36 FE -7E B0 22 D7 60 10 48 1B 77 AA C2 DC 99 8D C2 C4 -5D 78 83 53 92 E8 66 44 CC 32 43 A9 E8 22 B2 0E -DF D8 39 B3 21 5B E6 A8 F1 83 5E 85 5A A3 5D 2B -92 ED 59 D7 24 2C CC 26 AB A6 0A FE 78 B0 E9 D3 -7C 6D B8 32 0F 36 C0 A0 9B A2 56 73 08 56 EE 9B -AD 7C CC F3 6B EC 13 63 55 2A 28 0E 7A 9B D9 2A -62 08 D5 9C AD E8 43 6D 7A CA 8B DD BF DB 3F E1 -88 3F 9D B9 7C 19 D3 68 8C 57 AB 82 46 4B 75 B8 -F3 9E 2C 22 06 2A 93 78 56 56 76 51 65 11 C7 12 -78 AB F2 97 97 51 2A 16 70 56 30 C9 12 00 08 BC -80 55 6E 44 51 A1 93 CD CF BA 9A DA CA 48 19 74 -E4 70 1E AD 63 99 16 - -68 0D 0D 68 53 FF 11 01 67 21 2B 32 52 74 00 40 41 90 16 - -68 01 01 68 53 FF 00 01 67 DB 08 53 -41 47 59 05 E6 D9 FD 81 F8 20 00 69 D1 51 A8 0C 89 6B 68 23 FE 94 57 3B AB 39 56 9F 26 E5 D9 A7 10 1C F3 E4 -0E 2E C6 8D 3F 0A FE 8B 54 ED AC AE 84 36 86 72 B6 AA 0D B3 94 88 C0 37 4C 75 09 53 6E 3F 44 E1 A9 28 F8 28 -7A D4 E0 65 0A FA 46 A9 08 A6 3A EE C6 20 B5 7C E8 F8 C1 92 40 84 54 2E F4 99 A9 04 86 42 9E 2F E3 D5 28 92 -99 80 EE 10 A4 96 7F BC 72 63 33 32 4E 1C FF 71 1C 4B 66 CE 48 9B 46 FF A5 36 F2 E6 FE 84 E8 38 56 65 2E 59 -79 4B 2A A1 84 B4 63 53 25 EA 02 F1 9B 50 A2 CA FB DE 22 BB E8 24 A5 70 52 F4 64 F5 93 D7 16 9D A1 90 6C F3 -04 C6 26 95 6E 60 3E C6 4A F6 BA BB AC 01 FE 23 74 26 3F 7E F1 05 BD 76 2A 7C 34 FA FC EF 1F 40 46 6E F1 6F -DA 36 75 00 E6 1A BF C2 B5 7F 34 D7 37 4C A0 6C CC A7 33 EF 9C 4A B0 E7 50 67 0A FB 85 18 25 31 67 DD 16 - -68 0D 0D 68 53 FF 11 01 67 CD 91 DA 34 7A CE 84 AD B0 16 - -68 01 01 68 53 FF 00 01 67 DB 08 53 41 47 59 05 E6 D9 -FD 81 F8 20 00 69 D1 52 7F 86 D1 DD 40 FD F7 2C 67 32 4F 5D 69 56 B0 8F FB 04 A9 E6 03 C6 A7 6B 7F 86 E7 BF -2D E6 44 09 42 BF C8 B3 92 D1 EB CA EF B2 78 56 14 F9 32 6B 8F A4 8E 44 DB 86 B8 D1 83 82 67 BC DF 3D 85 DE -42 9F FA 88 69 F4 06 C8 CB 4C A8 FF E6 7A 44 D3 29 97 90 CA 1B 22 F7 D8 8D 6E F7 69 34 1C 7F 6A B6 AA 95 96 -D0 48 95 02 0E 76 C0 BE 31 94 A1 72 66 30 E2 72 D9 82 30 1E 9A 81 DC 23 D7 AA 10 E5 91 19 AA 5B 97 F4 51 21 -17 97 E6 2E 25 D2 7C 8D E0 D8 10 19 ED B6 2A B7 29 EA 4B B1 35 F9 89 65 1A 4D 45 BE C5 3F E9 86 24 14 A3 5B -20 AD 9F 20 A5 57 9F DE 82 AD 58 4D 65 35 4D 4E 74 D2 0F EA DE 26 4F 48 AB 3D E0 91 0D 9C 1D E6 C8 2B 4F C2 -3B 53 21 2A 26 82 B2 7D 9F 93 83 91 9F 4C 0B 47 3A 48 60 7B A3 12 6D 0B 9A 40 77 88 16 - -68 0D 0D 68 53 FF 11 01 67 C7 91 CB B9 7B 23 AC 8D 7E 16 - -68 01 01 68 53 FF 00 01 67 DB 08 53 41 47 59 05 E6 D9 FD 81 F8 20 00 69 -D1 53 4F 0C 66 DD DE 97 31 C0 8B E5 2B CE C3 99 51 17 DE FE AB 9E 2F 1B 76 06 71 8D 2E 9D BF 54 6E E5 B3 7B -CE 05 34 58 22 6D 53 8F 95 6A 0A 6F EA 88 87 18 CD 29 B4 B1 99 8E 03 05 EB 1B 61 9B 36 09 99 E9 42 D4 D5 BC -E7 57 11 1E 95 FE 9B 76 CF C2 1A 52 EE 70 2B 1F 6A 52 CA 90 85 FF AB D1 F0 E5 20 90 82 3E 5E 2E 25 60 D0 FB -74 A1 7C A2 01 C2 AC 40 4A C6 0F 82 8C 0C CE 18 B8 18 1D EF 94 CC 54 16 D8 64 1F 60 B3 34 B8 0E 0C 10 56 11 -89 D6 1E 26 91 1F 85 C4 BE 55 69 96 DA D0 D6 9E 69 4A 8F 10 BF 37 97 68 55 3E 92 B1 F1 76 21 BF 34 03 54 DB -F1 2C 23 9F B7 79 02 E8 37 DD AF 15 79 70 C4 95 C9 28 90 4E 6F BC FA 52 E7 FE 27 B5 F3 6E C4 C3 C1 37 CF C9 -7C E8 B1 10 7F 6B 4D 49 88 CD 2B 60 22 51 D7 5C 5F E6 AA 0B E1 2B 16 - -68 0D 0D 68 53 FF 11 01 67 C5 B1 63 A0 24 39 F5 57 ED 16 - - - -68 01 01 68 53 FF 00 01 67 DB 08 53 41 47 59 05 E6 D9 FD 81 F8 20 00 69 D1 54 5B D2 4F 76 -37 23 43 D5 D1 C1 02 A8 E8 91 41 6D 6C F7 E5 10 1E D5 6A 1B 73 8C 54 B1 87 32 FB 85 A6 F1 80 CE 40 B9 48 5F -54 7F 1F 6A 18 F2 54 9E ED D9 18 33 DC 52 E2 14 E1 BF 65 FE 7B 58 9B 37 89 93 8D 98 AF 63 FB 90 FA A1 02 8B -80 32 BF 7D 9D 61 42 56 F0 2E 7A D1 1B B0 88 09 DC 21 F6 6E 0E 5D D3 D9 B8 66 69 96 2D 60 CC 55 2C F1 E3 A1 -5B 30 56 AB 57 FE 5D 6A 4F E4 40 CF A1 64 22 C7 4E 60 DD DA 8E 08 17 2F 49 F2 C8 0D F5 A0 ED 06 27 E0 A5 F9 -1C B2 7A 97 5E 59 0A CC C8 A9 25 AB 1F 6A B4 AD DE 8B FF AF 9A 4F 7F 44 D3 00 18 34 57 E3 15 A2 DB 4E D0 2A -20 54 61 60 47 50 B9 6C 8E D0 D4 7C 27 36 0C 89 0B 82 DD 14 2D BE A1 9C D9 DF 31 F9 BF 46 A4 14 26 4D 86 04 -28 54 44 F3 49 9C 12 CF 02 15 F9 4D B7 AB 4A B2 16 68 0D 0D 68 53 FF 11 01 67 70 74 A7 30 84 47 41 BE 50 16 -68 01 01 68 53 FF 00 01 67 DB 08 53 41 47 59 05 E6 D9 FD 81 F8 20 00 69 D1 55 59 04 1D D6 A4 96 28 33 5D A5 -A5 8F 22 2F 26 3C 77 F2 94 3C D7 ED 65 24 AA 5D 96 5E 95 71 E3 1C 99 83 40 31 A1 3F 2E BD 1F 32 A7 CE 5F 32 -59 05 55 AC C9 C0 9D 2A 59 AF 09 3A 81 61 BC DB 4D 60 CF 8D 65 BF 6E 06 E5 73 59 FE 1D 1C D3 1C 08 EC 5C 8E -57 AA 3E E6 06 6D 71 45 CE AB DC 5C 25 AC 15 09 BC A2 BD E8 12 C2 92 C9 9E D1 38 A4 02 59 38 98 38 63 3B 45 -B4 1A 20 4D 34 05 74 46 50 BE A5 87 D1 7A 5F 98 91 9A DA E9 FA 1E AA 72 10 58 3C 0A 5D 46 81 4E 57 B2 98 DF \ No newline at end of file +68 01 01 68 +53 FF 10 01 67 +DB +08 53 41 47 59 05 E6 D9 FD +81 F8 +20 +00 01 A0 E0 +0F 80 3E 37 71 +0C 07 E5 0C 1B 01 0E 00 2D 00 FF C4 02 // Frame timestamp +02 23 // 35 items +09 0C 07 E5 0C 1B 01 0E 00 2D 00 FF C4 02 // Meter timestamp +09 06 01 00 01 08 00 FF 06 00 43 3D 0A 02 02 0F 00 16 1E +09 06 01 00 02 08 00 FF 06 00 00 01 03 02 02 0F 00 16 1E +09 06 01 00 01 07 00 FF 06 00 00 01 FE 02 02 0F 00 16 1B +09 06 01 00 02 07 00 FF 06 00 00 00 00 02 02 0F 00 16 1B +09 06 01 00 20 07 00 FF 12 09 34 02 02 0F FF 16 23 +09 06 01 00 34 07 00 FF 12 09 34 02 02 0F FF 16 23 +09 06 01 00 48 07 00 FF 12 09 2D 02 02 0F FF 16 23 +09 06 01 00 1F 07 00 FF 12 00 63 02 02 0F FE 16 21 +09 06 01 00 33 07 00 FF 12 00 3F 02 02 0F FE 16 21 +09 06 01 00 47 07 00 FF 12 00 54 02 02 0F FE 16 21 +09 06 01 00 0D 07 00 FF 10 03 CF 02 02 0F FD 16 FF // Power factor +09 0C 31 37 38 32 31 30 30 31 35 31 36 35 // Meter ID +01 67 \ No newline at end of file diff --git a/src/AmsToMqttBridge.ino b/src/AmsToMqttBridge.ino index a932dfdb..9ab62130 100644 --- a/src/AmsToMqttBridge.ino +++ b/src/AmsToMqttBridge.ino @@ -39,6 +39,7 @@ ADC_MODE(ADC_VCC); #define BUF_SIZE (1024) #include "ams/hdlc.h" +#include "MbusAssembler.h" #include "IEC6205621.h" #include "IEC6205675.h" @@ -659,6 +660,7 @@ void swapWifiMode() { int len = 0; uint8_t buf[BUF_SIZE]; +MbusAssembler* ma = NULL; int currentMeterType = -1; bool readHanPort() { if(!hanSerial->available()) return false; @@ -678,8 +680,10 @@ bool readHanPort() { CosemDateTime timestamp = {0}; AmsData data; if(currentMeterType == 1) { - while(hanSerial->available()) { + int pos = HDLC_FRAME_INCOMPLETE; + while(hanSerial->available() && pos == HDLC_FRAME_INCOMPLETE) { buf[len++] = hanSerial->read(); + pos = HDLC_validate((uint8_t *) buf, len, hc, ×tamp); delay(1); } if(len > 0) { @@ -689,7 +693,33 @@ bool readHanPort() { debugI("Buffer overflow, resetting"); return false; } - int pos = HDLC_validate((uint8_t *) buf, len, hc, ×tamp); + pos = HDLC_validate((uint8_t *) buf, len, hc, ×tamp); + if(pos == MBUS_FRAME_INTERMEDIATE_SEGMENT) { + debugI("Intermediate segment"); + if(ma == NULL) { + ma = new MbusAssembler(); + } + if(ma->append((uint8_t *) buf, len) < 0) + pos = -77; + if(Debug.isActive(RemoteDebug::DEBUG)) { + debugD("Frame dump (%db):", len); + debugPrint(buf, 0, len); + } + len = 0; + return false; + } else if(pos == MBUS_FRAME_LAST_SEGMENT) { + debugI("Final segment"); + if(Debug.isActive(RemoteDebug::DEBUG)) { + debugD("Frame dump (%db):", len); + debugPrint(buf, 0, len); + } + if(ma->append((uint8_t *) buf, len) >= 0) { + len = ma->write((uint8_t *) buf); + pos = HDLC_validate((uint8_t *) buf, len, hc, ×tamp); + } else { + pos = -77; + } + } if(pos == HDLC_FRAME_INCOMPLETE) { return false; } @@ -716,13 +746,55 @@ bool readHanPort() { } } len = 0; + while(hanSerial->available()) hanSerial->read(); if(pos > 0) { - while(hanSerial->available()) hanSerial->read(); - debugI("Valid HDLC, start at %d", pos); - data = IEC6205675(((char *) (buf)) + pos, meterState.getMeterType(), timestamp); + debugI("Valid data, start at byte %d", pos); + data = IEC6205675(((char *) (buf)) + pos, meterState.getMeterType(), timestamp, hc); } else { - debugW("Invalid HDLC, returned with %d", pos); - currentMeterType = 0; + if(Debug.isActive(RemoteDebug::WARNING)) { + switch(pos) { + case HDLC_BOUNDRY_FLAG_MISSING: + debugW("Boundry flag missing"); + break; + case HDLC_HCS_ERROR: + debugW("Header checksum error"); + break; + case HDLC_FCS_ERROR: + debugW("Frame checksum error"); + break; + case HDLC_FRAME_INCOMPLETE: + debugW("Received frame is incomplete"); + break; + case HDLC_ENCRYPTION_CONFIG_MISSING: + debugI("Encryption configuration requested, initializing"); + break; + case HDLC_ENCRYPTION_AUTH_FAILED: + debugW("Decrypt authentication failed"); + break; + case HDLC_ENCRYPTION_KEY_FAILED: + debugW("Setting decryption key failed"); + break; + case HDLC_ENCRYPTION_DECRYPT_FAILED: + debugW("Decryption failed"); + break; + case MBUS_FRAME_LENGTH_NOT_EQUAL: + debugW("Frame length mismatch"); + break; + case MBUS_FRAME_INTERMEDIATE_SEGMENT: + case MBUS_FRAME_LAST_SEGMENT: + debugW("Partial frame dropped"); + break; + case HDLC_TIMESTAMP_UNKNOWN: + debugW("Frame timestamp is not correctly formatted"); + break; + case HDLC_UNKNOWN_DATA: + debugW("Unknown data format %02X", buf[0]); + currentMeterType = 0; + break; + default: + debugW("Unspecified error while reading data: %d", pos); + } + } return false; } } else { diff --git a/src/IEC6205675.cpp b/src/IEC6205675.cpp index 32369511..3b4134eb 100644 --- a/src/IEC6205675.cpp +++ b/src/IEC6205675.cpp @@ -2,7 +2,7 @@ #include "lwip/def.h" #include "Timezone.h" -IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, CosemDateTime packageTimestamp) { +IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, CosemDateTime packageTimestamp, HDLCConfig* hc) { uint32_t ui; double val; char str[64]; @@ -140,6 +140,12 @@ IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, CosemDateTime packag if(memcmp(version->str.data, "Kamstrup", 8) == 0) { meterType = AmsTypeKamstrup; } + } + } + // Try system title + if(meterType == AmsTypeUnknown && hc != NULL) { + if(memcmp(hc->system_title, "SAGY", 4)) { + meterType = AmsTypeSagemcom; } } @@ -284,6 +290,31 @@ IEC6205675::IEC6205675(const char* d, uint8_t useMeterType, CosemDateTime packag l1PowerFactor /= 100; l2PowerFactor /= 100; l3PowerFactor /= 100; + } else if(meterType == AmsTypeSagemcom) { + CosemData* meterTs = getCosemDataAt(1, ((char *) (d))); + if(meterTs != NULL) { + AmsOctetTimestamp* amst = (AmsOctetTimestamp*) meterTs; + time_t ts = getTimestamp(amst->dt); + if(meterType == AmsTypeKamstrup || meterType == AmsTypeAidon) { + this->meterTimestamp = tz.toUTC(ts); + } else { + meterTimestamp = ts; + } + } + + CosemData* mid = getCosemDataAt(58, ((char *) (d))); // TODO: Get last item + if(mid != NULL) { + switch(mid->base.type) { + case CosemTypeString: + memcpy(&meterId, mid->str.data, mid->str.length); + meterId[mid->str.length] = 0; + break; + case CosemTypeOctetString: + memcpy(&meterId, mid->oct.data, mid->oct.length); + meterId[mid->oct.length] = 0; + break; + } + } } lastUpdateMillis = millis(); diff --git a/src/IEC6205675.h b/src/IEC6205675.h index 2f16b233..7846c5e1 100644 --- a/src/IEC6205675.h +++ b/src/IEC6205675.h @@ -11,7 +11,7 @@ struct AmsOctetTimestamp { class IEC6205675 : public AmsData { public: - IEC6205675(const char* payload, uint8_t useMeterType, CosemDateTime packageTimestamp); + IEC6205675(const char* payload, uint8_t useMeterType, CosemDateTime packageTimestamp, HDLCConfig* hc); private: CosemData* getCosemDataAt(uint8_t index, const char* ptr); diff --git a/src/MbusAssembler.cpp b/src/MbusAssembler.cpp new file mode 100644 index 00000000..a74d735f --- /dev/null +++ b/src/MbusAssembler.cpp @@ -0,0 +1,57 @@ +#include "Arduino.h" +#include "MbusAssembler.h" +#include "ams/hdlc.h" + +MbusAssembler::MbusAssembler() { + buf = (uint8_t *)malloc((size_t)1024); // TODO find out from first package ? +} + +uint8_t MbusAssembler::append(const uint8_t* d, int length) { + MbusHeader* h = (MbusHeader*) d; + uint8_t* ptr = (uint8_t*) &h[1]; + + uint8_t len = h->len1; + + uint8_t control = *ptr; + ptr++; len--; + + uint8_t address = *ptr; + ptr++; len--; + + uint8_t ci = *ptr; + ptr++; len--; + + uint8_t stsap = *ptr; + ptr++; len--; + + uint8_t dtsap = *ptr; + ptr++; len--; + + uint8_t sequenceNumber = ci & 0x0F; + if(sequenceNumber == 0) { + memcpy(buf, d, length - 2); // Do not include FCS and MBUS_STOP + buf[6] = 0x10; // Mark that this is a single, complete frame + pos = length - 2; + lastSequenceNumber = 0; + return 0; + } else if(pos + len > 1024 || sequenceNumber != (lastSequenceNumber + 1)) { // TODO return error + pos = 0; + lastSequenceNumber = -1; + return -1; + } else { + if(len > length) return -1; + memcpy(buf + pos, ptr, len); + pos += len; + lastSequenceNumber = sequenceNumber; + return 0; + } + return -2; +} + +uint16_t MbusAssembler::write(const uint8_t* d) { + buf[1] = buf[2] = 0x00; + buf[pos++] = mbusChecksum(buf+4, pos-4); + buf[pos++] = MBUS_END; + memcpy((uint8_t *) d, buf, pos); + return pos; +} diff --git a/src/MbusAssembler.h b/src/MbusAssembler.h new file mode 100644 index 00000000..49be63da --- /dev/null +++ b/src/MbusAssembler.h @@ -0,0 +1,18 @@ +#ifndef _MBUS_ASSEMBLER_H +#define _MBUS_ASSEMBLER_H + +#include + +class MbusAssembler { +public: + MbusAssembler(); + uint8_t append(const uint8_t* d, int length); + uint16_t write(const uint8_t* d); + +private: + uint16_t pos = 0; + uint8_t *buf; + uint8_t lastSequenceNumber = -1; +}; + +#endif diff --git a/src/ams/hdlc.cpp b/src/ams/hdlc.cpp index 5b0bc155..d9db24ca 100644 --- a/src/ams/hdlc.cpp +++ b/src/ams/hdlc.cpp @@ -16,73 +16,125 @@ void mbus_hexdump(const uint8_t* buf, int len) { } int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTime* timestamp) { - if(length < 10) - return HDLC_FRAME_INCOMPLETE; - int len; int headersize = 3; int footersize = 1; - HDLCHeader* h = (HDLCHeader*) d; - uint8_t* ptr = (uint8_t*) &h[1]; - // Frame format type 3 - if(h->flag == HDLC_FLAG && (h->format & 0xF0) == 0xA0) { - // Length field (11 lsb of format) - len = (ntohs(h->format) & 0x7FF) + 2; - if(len > length) + + uint8_t flag = *d; + + uint8_t* ptr; + if(flag == HDLC_FLAG) { + if(length < 3) return HDLC_FRAME_INCOMPLETE; - HDLCFooter* f = (HDLCFooter*) (d + len - sizeof *f); - footersize = sizeof *f; + HDLCHeader* h = (HDLCHeader*) d; + ptr = (uint8_t*) &h[1]; - // First and last byte should be MBUS_HAN_TAG - if(h->flag != HDLC_FLAG || f->flag != HDLC_FLAG) - return HDLC_BOUNDRY_FLAG_MISSING; + // Frame format type 3 + if((h->format & 0xF0) == 0xA0) { + // Length field (11 lsb of format) + len = (ntohs(h->format) & 0x7FF) + 2; + if(len > length) + return HDLC_FRAME_INCOMPLETE; - // Verify FCS - if(ntohs(f->fcs) != crc16_x25(d + 1, len - sizeof *f - 1)) - return HDLC_FCS_ERROR; + HDLCFooter* f = (HDLCFooter*) (d + len - sizeof *f); + footersize = sizeof *f; - // Skip destination address, LSB marks last byte - while(((*ptr) & 0x01) == 0x00) { - ptr++; + // First and last byte should be MBUS_HAN_TAG + if(h->flag != HDLC_FLAG || f->flag != HDLC_FLAG) + return HDLC_BOUNDRY_FLAG_MISSING; + + // Verify FCS + if(ntohs(f->fcs) != crc16_x25(d + 1, len - sizeof *f - 1)) + return HDLC_FCS_ERROR; + + // Skip destination address, LSB marks last byte + while(((*ptr) & 0x01) == 0x00) { + ptr++; + headersize++; + } headersize++; - } - headersize++; - ptr++; - - // Skip source address, LSB marks last byte - while(((*ptr) & 0x01) == 0x00) { ptr++; + + // Skip source address, LSB marks last byte + while(((*ptr) & 0x01) == 0x00) { + ptr++; + headersize++; + } headersize++; + ptr++; + + HDLC3CtrlHcs* t3 = (HDLC3CtrlHcs*) (ptr); + headersize += 3; + + // Verify HCS + if(ntohs(t3->hcs) != crc16_x25(d + 1, ptr-d)) + return HDLC_HCS_ERROR; + + ptr += sizeof *t3; + + // Extract LLC + HDLCLLC* llc = (HDLCLLC*) ptr; + ptr += sizeof *llc; + headersize += sizeof *llc; + } else { + return HDLC_UNKNOWN_DATA; } - headersize++; - ptr++; + } else if(flag == MBUS_START) { + // https://m-bus.com/documentation-wired/06-application-layer + if(length < 4) + return HDLC_FRAME_INCOMPLETE; - HDLC3CtrlHcs* t3 = (HDLC3CtrlHcs*) (ptr); - headersize += 3; + MbusHeader* mh = (MbusHeader*) d; + if(mh->flag1 != MBUS_START || mh->flag2 != MBUS_START) + return MBUS_BOUNDRY_FLAG_MISSING; - // Verify HCS - if(ntohs(t3->hcs) != crc16_x25(d + 1, ptr-d)) - return HDLC_HCS_ERROR; + // First two bytes is 1-byte length value repeated. Only used for last segment + if(mh->len1 != mh->len2) + return MBUS_FRAME_LENGTH_NOT_EQUAL; + len = mh->len1; + ptr = (uint8_t*) &mh[1]; + headersize = 4; + footersize = 2; - ptr += sizeof *t3; - } else if(h->flag == MBUS_START) { - // TODO: Check that the two next bytes are identical + if(len == 0x00) + len = length - headersize - footersize; + // Payload can max be 255 bytes, so I think the following case is only valid for austrian meters + if(len < headersize) + len += 256; - // Ignore: Control field + Address + Flag + if((headersize + footersize + len) > length) + return HDLC_FRAME_INCOMPLETE; + + MbusFooter* mf = (MbusFooter*) (d + len + headersize); + if(mf->flag != MBUS_END) + return MBUS_BOUNDRY_FLAG_MISSING; + if(mbusChecksum(d + headersize, len) != mf->fcs) + return MBUS_CHECKSUM_ERROR; + + ptr += 2; + + // Control information field + uint8_t ci = *ptr; + + // Bits 7 6 5 4 3 2 1 0 + // 0 0 0 Finished Sequence number + uint8_t sequenceNumber = (ci & 0x0F); + if((ci & 0x10) == 0x00) { // Not finished yet + return MBUS_FRAME_INTERMEDIATE_SEGMENT; + } else if(sequenceNumber > 0) { // This is the last frame of multiple, assembly needed + return MBUS_FRAME_LAST_SEGMENT; + } + + // Skip CI, STSAP and DTSAP ptr += 3; - headersize += 3; - footersize++; + headersize += 5; // And also control and address that we didn't skip earlier, needed these for checksum. + } else { + return HDLC_UNKNOWN_DATA; } - // Extract LLC - HDLCLLC* llc = (HDLCLLC*) ptr; - ptr += sizeof *llc; - headersize += 3; - if(((*ptr) & 0xFF) == 0x0F) { // Unencrypted APDU - int i = 0; HDLCADPU* adpu = (HDLCADPU*) (ptr); ptr += sizeof *adpu; @@ -90,7 +142,7 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim CosemData* dateTime = (CosemData*) ptr; if(dateTime->base.type == CosemTypeOctetString) { if(dateTime->base.length == 0x0C) { - memcpy(timestamp, ptr+1, dateTime->base.length); + memcpy(timestamp, ptr+1, dateTime->base.length+1); } ptr += 2 + dateTime->base.length; } else if(dateTime->base.type == CosemTypeNull) { @@ -99,10 +151,10 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim } else if(dateTime->base.type == CosemTypeDateTime) { memcpy(timestamp, ptr, dateTime->base.length); } else if(dateTime->base.type == 0x0C) { // Kamstrup bug... - memcpy(timestamp, ptr, 0x0C); + memcpy(timestamp, ptr, 13); ptr += 13; } else { - return -99; + return HDLC_TIMESTAMP_UNKNOWN; } return ptr-d; @@ -132,20 +184,17 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim } else if(((*ptr) & 0xFF) == 0x82) { HDLCHeader* h = (HDLCHeader*) ptr; - // Length field + // 2-byte payload length len = (ntohs(h->format) & 0xFFFF); ptr += 3; headersize += 3; } - //len = ceil(len/16.0) * 16; // Technically GCM is 128bit blocks. This works for Austrian meters, but not Danish... if(len + headersize + footersize > length) return HDLC_FRAME_INCOMPLETE; //Serial.printf("\nL: %d : %d, %d : %d\n", length, len, headersize, footersize); - // TODO: FCS - memcpy(config->additional_authenticated_data, ptr, 1); // Security tag @@ -203,7 +252,8 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim mbedtls_gcm_free(&m_ctx); #endif - ptr += 5; // TODO: Come to this number in a proper way... + HDLCADPU* adpu = (HDLCADPU*) (ptr); + ptr += sizeof *adpu; // ADPU timestamp CosemData* dateTime = (CosemData*) ptr; @@ -221,7 +271,7 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim memcpy(timestamp, ptr, 0x0C); ptr += 13; } else { - return -99; + return HDLC_TIMESTAMP_UNKNOWN; } return ptr-d; @@ -230,3 +280,10 @@ int HDLC_validate(const uint8_t* d, int length, HDLCConfig* config, CosemDateTim // Unknown payload return HDLC_UNKNOWN_DATA; } + +uint8_t mbusChecksum(const uint8_t* p, int len) { + uint8_t ret = 0; + while(len--) + ret += *p++; + return ret; +} diff --git a/src/ams/hdlc.h b/src/ams/hdlc.h index 5e5a0983..75ea0d9d 100644 --- a/src/ams/hdlc.h +++ b/src/ams/hdlc.h @@ -14,9 +14,15 @@ #define HDLC_ENCRYPTION_AUTH_FAILED -91 #define HDLC_ENCRYPTION_KEY_FAILED -92 #define HDLC_ENCRYPTION_DECRYPT_FAILED -93 +#define HDLC_TIMESTAMP_UNKNOWN -99 #define MBUS_START 0x68 #define MBUS_END 0x16 +#define MBUS_BOUNDRY_FLAG_MISSING -1 +#define MBUS_FRAME_LENGTH_NOT_EQUAL -40 +#define MBUS_FRAME_INTERMEDIATE_SEGMENT -41 +#define MBUS_FRAME_LAST_SEGMENT -42 +#define MBUS_CHECKSUM_ERROR -3 struct HDLCConfig { uint8_t encryption_key[32]; @@ -53,6 +59,12 @@ typedef struct HDLCADPU { uint32_t id; } __attribute__((packed)) HDLCADPU; +typedef struct MbusHeader { + uint8_t flag1; + uint8_t len1; + uint8_t len2; + uint8_t flag2; +} __attribute__((packed)) MbusHeader; typedef struct MbusFooter { uint8_t fcs; @@ -126,4 +138,6 @@ typedef union { void mbus_hexdump(const uint8_t* buf, int len); int HDLC_validate(const uint8_t* d, int len, HDLCConfig* config, CosemDateTime* timestamp); +uint8_t mbusChecksum(const uint8_t* p, int len); + #endif