#include <math.h>
const uint8_t packet1[] {0x1B, 0x1B, 0x1B, 0x1B, 0x01, 0x01, 0x01, 0x01, 0x76, 0x05, 0x1D, 0x60, 0x22, 0x03, 0x62, 0x00, 0x62, 0x00, 0x72, 0x63, 0x01, 0x01, 0x76, 0x01, 0x01, 0x02, 0x31, 0x0B, 0x0A, 0x01, 0x44, 0x5A, 0x47, 0x00, 0x03, 0xA0, 0x86, 0xBC, 0x72, 0x62, 0x01, 0x65, 0x01, 0x0B, 0x75, 0xAE, 0x62, 0x02, 0x63, 0x75, 0xD6, 0x00, 0x76, 0x05, 0x1E, 0x60, 0x22, 0x03, 0x62, 0x00, 0x62, 0x00, 0x72, 0x63, 0x07, 0x01, 0x77, 0x01, 0x0B, 0x0A, 0x01, 0x44, 0x5A, 0x47, 0x00, 0x03, 0xA0, 0x86, 0xBC, 0x07, 0x01, 0x00, 0x62, 0x0A, 0xFF, 0xFF, 0x72, 0x62, 0x01, 0x65, 0x01, 0x0B, 0x75, 0xAE, 0x75, 0x77, 0x07, 0x01, 0x00, 0x60, 0x32, 0x01, 0x01, 0x01, 0x72, 0x62, 0x01, 0x62, 0x00, 0x62, 0x00, 0x52, 0x00, 0x04, 0x44, 0x5A, 0x47, 0x01, 0x77, 0x07, 0x01, 0x00, 0x60, 0x01, 0x00, 0xFF, 0x01, 0x72, 0x62, 0x01, 0x62, 0x00, 0x62, 0x00, 0x52, 0x00, 0x0B, 0x0A, 0x01, 0x44, 0x5A, 0x47, 0x00, 0x03, 0xA0, 0x86, 0xBC, 0x01, 0x77, 0x07, 0x01, 0x00, 0x01, 0x08, 0x00, 0xFF, 0x64, 0x1C, 0x01, 0x04, 0x72, 0x62, 0x01, 0x62, 0x00, 0x62, 0x1E, 0x52, 0xFF, 0x65, 0x01, 0x29, 0xFE, 0x45, 0x01, 0x77, 0x07, 0x01, 0x00, 0x02, 0x08, 0x00, 0xFF, 0x01, 0x72, 0x62, 0x01, 0x62, 0x00, 0x62, 0x1E, 0x52, 0xFF, 0x64, 0x05, 0x7D, 0x93, 0x01, 0x77, 0x07, 0x01, 0x00, 0x10, 0x07, 0x00, 0xFF, 0x01, 0x72, 0x62, 0x01, 0x62, 0x00, 0x62, 0x1B, 0x52, 0xFE, 0x53, 0x7C, 0x31, 0x01, 0x01, 0x01, 0x63, 0xA5, 0x2F, 0x00, 0x76, 0x05, 0x1F, 0x60, 0x22, 0x03, 0x62, 0x00, 0x62, 0x00, 0x72, 0x63, 0x02, 0x01, 0x71, 0x01, 0x63, 0x61, 0x50, 0x00, 0x00, 0x00, 0x1B, 0x1B, 0x1B, 0x1B, 0x1A, 0x02, 0x00, 0xCA};
#define SML_ARRAY 0x00
#define SML_BOOL 0x04
#define SML_INT 0x05
#define SML_UINT 0x06
#define SML_LIST 0x07
#define SML_OpenResponse 0x0101
#define SML_CloseResponse 0x0201
#define SML_GetListResponse 0x0701
const char* SML_TYPE_STR[] {
"SML_ARRAY",
"",
"",
"",
"SML_BOOL",
"SML_INT",
"SML_UINT",
"SML_LIST"
};
#define SML_TYPE_MASK 0x70
#define MAX_DEPTH 15
#define FieldType(x) *x >> 4
#define FieldTypeStr(x) SML_TYPE_STR[x]
#define FieldLength(x) *x & 0x0F
struct ListInfo {
uint8_t maxItems = 0;
uint8_t currentItem = 0;
};
void indent(size_t len) {
for (uint8_t i = 0; i < len; i++) Serial.print(" ");
}
uint64_t parseUint(const uint8_t* data, size_t len) {
uint64_t result = 0;
for (size_t i = 0; i < len; i++) result = (result << 8) + *(data++);
return result;
// Serial.println(result);
}
int64_t parseInt(const uint8_t* data, size_t len) {
int64_t result = *(data) & 0x80 ? -1 : 0;
for (size_t i = 0; i < len; i++) result = (result << 8) + *(data++);
return result;
// Serial.println(result);
}
void parseField(const uint8_t* data, size_t field_length, uint8_t field_type) {
switch (field_type) {
case SML_UINT : Serial.println(parseUint(data, field_length)); break;
case SML_INT : Serial.println(parseInt(data, field_length)); break;
default: Serial.println();
}
}
void dump(uint8_t field_type, uint8_t field_length, const uint8_t* data) {
const uint8_t* ptr = data + 1;
Serial.print("[");
for (size_t i = 0; i < field_length - 1; i++) {
Serial.printf("%s0x%02X", i ? ", " : "", *(ptr++));
}
Serial.print("] ");
parseField(data + 1, field_length - 1, field_type);
}
void parseList(const uint8_t* data, size_t len) {
ListInfo li[MAX_DEPTH];
uint8_t current_level = 0;
const uint8_t* ptr = data;
uint64_t smlMessage = 0;
uint64_t unit = 0;
int64_t scaler = 0;
while (ptr < data + len) {
uint8_t field_type = FieldType(ptr);
uint8_t field_length = FieldLength(ptr);
if (field_length == 0) field_length = 1;
if (current_level == 0 && field_type != SML_LIST) {
ptr++;
continue;
}
indent(current_level);
li[current_level].currentItem++;
Serial.printf("%d.[%d/%d] | %s (%d) ",
current_level,
li[current_level].currentItem,
current_level ? li[current_level].maxItems : 0,
FieldTypeStr(field_type),
field_type != SML_LIST ? field_length - 1 : field_length
);
if (field_type == SML_LIST) {
Serial.println();
current_level++;
li[current_level].maxItems = field_length;
ptr++;
continue;
} else {
dump(field_type, field_length, ptr);
if (current_level == 2 && li[current_level].currentItem == 1) {
// indent(current_level);
// Serial.println("SML Identifier!!!");
smlMessage = parseUint(ptr + 1, field_length - 1);
// indent(current_level);
// Serial.println(smlMessage);
indent(current_level);
switch (smlMessage) {
case 0x0101 : Serial.println("OpenResponse"); break;
case 0x0701 : Serial.println("GetListResponse"); break;
case 0x0201 : Serial.println("CloseResponse"); break;
default: Serial.println("Unbekannt");
}
}
if (smlMessage = SML_GetListResponse && current_level == 5) {
if (li[current_level].currentItem == 4) {
unit = parseUint(ptr + 1, field_length - 1);
}
if (li[current_level].currentItem == 5) {
scaler = parseInt(ptr + 1, field_length - 1);
}
if (li[current_level].currentItem == 6) {
if (field_type == SML_UINT) {
uint64_t value = parseUint(ptr + 1, field_length - 1);
double result = value * pow(10, scaler);
indent(current_level);
Serial.print(result);
switch (unit) {
case 30: Serial.println(" Wh"); break;
case 27: Serial.println(" W"); break;
default: Serial.println();
}
}
if (field_type == SML_INT) {
int64_t value = parseInt(ptr + 1, field_length - 1);
double result = value * pow(10, scaler);
indent(current_level);
Serial.print(result);
switch (unit) {
case 30: Serial.println(" Wh"); break;
case 27: Serial.println(" W"); break;
default: Serial.println();
}
}
}
}
ptr += field_length ? field_length : 1;
}
while (current_level > 0 && li[current_level].currentItem == li[current_level].maxItems) {
li[current_level].maxItems = 0;
li[current_level].currentItem = 0;
current_level--;
}
}
}
void setup() {
Serial.begin(115200);
parseList(packet1, sizeof(packet1));
}
void loop() {
delay(10); // this speeds up the simulation
}
// void parseList(const uint8_t* data, size_t len) {
// ListInfo li[MAX_DEPTH];
// uint8_t current_level = 0;
// const uint8_t* ptr = data;
// while (ptr < data + len) {
// uint8_t field_type = FieldType(ptr);
// uint8_t field_length = FieldLength(ptr);
// if (field_length == 0) field_length = 1;
// if (current_level == 0 && field_type != SML_LIST) {
// ptr++;
// continue;
// }
// indent(current_level);
// li[current_level].currentItem++;
// Serial.printf("%d.[%d/%d] | %s (%d) ",
// current_level,
// li[current_level].currentItem,
// current_level ? li[current_level].maxItems : 0,
// FieldTypeStr(field_type),
// field_type != SML_LIST ? field_length - 1 : field_length
// );
// if (field_type == SML_LIST) {
// Serial.println();
// current_level++;
// li[current_level].maxItems = field_length;
// ptr++;
// continue;
// } else {
// dump(field_type, field_length, ptr);
// ptr += field_length ? field_length : 1;
// }
// while (current_level > 0 && li[current_level].currentItem == li[current_level].maxItems) {
// li[current_level].maxItems = 0;
// li[current_level].currentItem = 0;
// current_level--;
// }
// }
// }