More messagepack

2025-12-02 22:11:48 +00:00 · 2016-05-16 12:02:45 +02:00
parent 61fdb57b8f
commit e959d9a8ec
7 changed files with 277 additions and 89 deletions
--- a/firmware/common/msgpack.cpp
+++ b/firmware/common/msgpack.cpp
@@ -22,8 +22,6 @@

 #include "msgpack.hpp"

-#include <cstring>
-
 bool MsgPack::get_bool(const void * buffer, const bool inc, bool * value) {
 	uint8_t v;
 	
@@ -55,6 +53,26 @@ bool MsgPack::get_raw_word(const void * buffer, const bool inc, uint16_t * word)
 	return true;
 }

+bool MsgPack::get_u8(const void * buffer, const bool inc, uint8_t * value) {
+	uint8_t v;
+	
+	if (seek_ptr >= buffer_size) return false;	// End of buffer
+	
+	v = ((uint8_t*)buffer)[seek_ptr];
+
+	if (!(v & 0x80))
+		*value = ((uint8_t*)buffer)[seek_ptr];	// Fixnum
+	else if (v == MSGPACK_TYPE_U8)
+		*value = ((uint8_t*)buffer)[seek_ptr + 1];	// u8
+	else
+		return false;		// Value isn't a u8 or fixnum
+	
+	if (inc) seek_ptr++;
+	return true;
+}
+
+// TODO: Typecheck function
+
 bool MsgPack::get_u16(const void * buffer, const bool inc, uint16_t * value) {
 	uint8_t byte;
 	
@@ -76,20 +94,7 @@ bool MsgPack::get_s32(const void * buffer, const bool inc, int32_t * value) {
 	return true;
 }

-bool MsgPack::get_s64(const void * buffer, const bool inc, int64_t * value) {
-	uint8_t byte;
-	
-	if ((seek_ptr + 3) >= buffer_size) return false;	// End of buffer
-	if ((get_raw_byte(buffer, true, &byte)) && (byte != MSGPACK_TYPE_S64)) return false;		// Value isn't a s64
-	*value = ((int64_t)((uint8_t*)buffer)[seek_ptr] << 56) | ((int64_t)((uint8_t*)buffer)[seek_ptr + 1] << 48) |
-				((int64_t)((uint8_t*)buffer)[seek_ptr + 2] << 40) | ((int64_t)((uint8_t*)buffer)[seek_ptr + 3] << 32) |
-				(((uint8_t*)buffer)[seek_ptr + 4] << 24) | (((uint8_t*)buffer)[seek_ptr + 5] << 16) |
-				(((uint8_t*)buffer)[seek_ptr + 6] << 8) | ((uint8_t*)buffer)[seek_ptr + 7];
-	if (inc) seek_ptr += 8;
-	return true;
-}
-
-bool MsgPack::get_chars(const void * buffer, const bool inc, char * value) {
+bool MsgPack::get_string(const void * buffer, const bool inc, std::string& value) {
 	size_t length;
 	uint8_t byte;
 	
@@ -100,11 +105,11 @@ bool MsgPack::get_chars(const void * buffer, const bool inc, char * value) {

 	if (byte == MSGPACK_TYPE_STR8) {
 		if (!get_raw_byte(buffer, true, (uint8_t*)&length)) return false;		// Couldn't get str8 length
-	} else {
+	} else if (byte == MSGPACK_TYPE_STR16) {
 		if (!get_raw_word(buffer, true, (uint16_t*)&length)) return false;		// Couldn't get str16 length
 	}
 	
-	memcpy(value, ((uint8_t*)buffer), length);
+	memcpy(&value[0], ((uint8_t*)buffer), length); //std::string(

 	if (inc) seek_ptr += length;
 	return true;
@@ -112,34 +117,61 @@ bool MsgPack::get_chars(const void * buffer, const bool inc, char * value) {

 bool MsgPack::init_search(const void * buffer, const size_t size) {
 	uint8_t byte;
-	uint16_t map_size;	// Unused for now
+	uint16_t map_size;
 	
 	if (!size) return false;
 	buffer_size = size;
 	seek_ptr = 0;
 	if ((get_raw_byte(buffer, true, &byte)) && (byte != MSGPACK_TYPE_MAP16)) return false;		// First record isn't a map16
 	if (!get_raw_word(buffer, true, &map_size)) return false;		// Couldn't get map16 size
+	if (!map_size) return false;
 	
 	return true;
 }

 bool MsgPack::skip(const void * buffer) {
-	uint8_t byte;
+	uint8_t byte, c;
 	size_t length;
 	
 	if (!get_raw_byte(buffer, true, &byte)) return false;		// Couldn't get type
 	
+	if (!(byte & 0x80)) return true;			// Positive fixnum, already skipped by get_raw_byte
+	if ((byte & 0xE0) == 0xE0) return true;		// Negative fixnum, already skipped by get_raw_byte
+	if ((byte & 0xE0) == 0xA0) {				// Fixstr
+		seek_ptr += (byte & 0x1F);
+		return true;
+	}
+	if ((byte & 0xF0) == 0x80) {				// Fixmap
+		length = (byte & 0x0F) * 2;
+		for (c = 0; c < length; c++)
+			skip(buffer);
+		return true;
+	}
+	if ((byte & 0xF0) == 0x90) {				// Fixarray
+		length = byte & 0x0F;
+		for (c = 0; c < length; c++)
+			skip(buffer);
+		return true;
+	}
+	
 	switch (byte) {
+		case MSGPACK_NIL:
 		case MSGPACK_FALSE:
-		case MSGPACK_TRUE:
-			return true;	// Already skipped by get_raw_byte
+		case MSGPACK_TRUE:		// Already skipped by get_raw_byte
+			break;
+		case MSGPACK_TYPE_U8:
+		case MSGPACK_TYPE_S8:
+			seek_ptr++;
 			break;
 		case MSGPACK_TYPE_U16:
+		case MSGPACK_TYPE_S16:
 			seek_ptr += 2;
 			break;
+		case MSGPACK_TYPE_U32:
 		case MSGPACK_TYPE_S32:
 			seek_ptr += 4;
 			break;
+		case MSGPACK_TYPE_U64:
 		case MSGPACK_TYPE_S64:
 			seek_ptr += 8;
 			break;
@@ -152,11 +184,23 @@ bool MsgPack::skip(const void * buffer) {
 			if (!get_raw_word(buffer, true, (uint16_t*)&length)) return false;		// Couldn't get str16 length
 			seek_ptr += length;
 			break;
+		
+		case MSGPACK_TYPE_ARR16:
+			if (!get_raw_word(buffer, true, (uint16_t*)&length)) return false;		// Couldn't get arr16 length
+			for (c = 0; c < length; c++)
+				skip(buffer);
+			break;
+			
+		case MSGPACK_TYPE_MAP16:
+			if (!get_raw_word(buffer, true, (uint16_t*)&length)) return false;		// Couldn't get map16 length
+			for (c = 0; c < (length * 2); c++)
+				skip(buffer);
+			break;
 			
 		default:
 			return false;	// Type unsupported
 	}
-
+	
 	return true;
 }

@@ -164,48 +208,134 @@ bool MsgPack::search_key(const void * buffer, const MsgPack::RecID record_id) {
 	uint8_t byte;
 	uint16_t key;
 	
-	while (get_raw_byte(buffer, true, &byte)) {
-		if (byte == MSGPACK_TYPE_U16) {
-			if (!get_u16(buffer, true, &key)) return false;	// Couldn't get key
-			if (key == record_id) return true;	// Found record
-			if (!skip(buffer)) return false;	// Can't skip to next key
-		} else {
-			return false;		// Key wasn't a U16
-		}
+	while (get_raw_byte(buffer, false, &byte)) {
+		if (!get_u16(buffer, true, &key)) return false;	// Couldn't get key
+		if (key == record_id) return true;				// Found record
+		if (!skip(buffer)) return false;				// Can't skip to next key
 	};
 	return false;
 }

-bool MsgPack::msgpack_get(const void * buffer, const size_t size, const MsgPack::RecID record_id, bool * value) {
+bool MsgPack::msgpack_get(const void * buffer, const size_t size, const RecID record_id, bool * value) {
 	init_search(buffer, size);
 	if (!search_key(buffer, record_id)) return false;	// Record not found
-		*value = true;
-	return true;
 	if (!get_bool(buffer, false, value)) return false;	// Value isn't a bool
 	
 	return true;
 }

-bool MsgPack::msgpack_get(const void * buffer, size_t size, RecID record_id, int32_t * value) {
-	init_search(buffer, size);
+bool MsgPack::msgpack_get(const void * buffer, const size_t size, const RecID record_id, uint8_t * value) {
+	if (!init_search(buffer, size)) return false;
 	if (!search_key(buffer, record_id)) return false;	// Record not found
-	if (!get_s32(buffer, false, value)) return false;	// Value isn't a s32
+	if (!get_u8(buffer, false, value)) return false;	// Value isn't a u8
 	
 	return true;
 }

-bool MsgPack::msgpack_get(const void * buffer, size_t size, RecID record_id, int64_t * value) {
+bool MsgPack::msgpack_get(const void * buffer, const size_t size, const RecID record_id, int64_t * value) {
+	uint8_t byte;
+	
 	init_search(buffer, size);
 	if (!search_key(buffer, record_id)) return false;	// Record not found
-	if (!get_s64(buffer, false, value)) return false;	// Value isn't a s64
+	
+	if ((seek_ptr + 3) >= buffer_size) return false;	// End of buffer
+	if ((get_raw_byte(buffer, true, &byte)) && (byte != MSGPACK_TYPE_S64)) return false;		// Value isn't a s64
+	*value = ((int64_t)((uint8_t*)buffer)[seek_ptr] << 56) | ((int64_t)((uint8_t*)buffer)[seek_ptr + 1] << 48) |
+				((int64_t)((uint8_t*)buffer)[seek_ptr + 2] << 40) | ((int64_t)((uint8_t*)buffer)[seek_ptr + 3] << 32) |
+				(((uint8_t*)buffer)[seek_ptr + 4] << 24) | (((uint8_t*)buffer)[seek_ptr + 5] << 16) |
+				(((uint8_t*)buffer)[seek_ptr + 6] << 8) | ((uint8_t*)buffer)[seek_ptr + 7];
 	
 	return true;
 }

-bool MsgPack::msgpack_get(const void * buffer, size_t size, RecID record_id, char * value) {
+bool MsgPack::msgpack_get(const void * buffer, const size_t size, const RecID record_id, std::string& value) {
 	init_search(buffer, size);
 	if (!search_key(buffer, record_id)) return false;	// Record not found
-	if (!get_chars(buffer, false, value)) return false;	// Value isn't a char array
+	if (!get_string(buffer, false, value)) return false;	// Value isn't a char array
 	
 	return true;
 }
+
+
+
+void MsgPack::msgpack_init(const void * buffer, size_t * ptr) {
+	((uint8_t*)buffer)[0] = MSGPACK_TYPE_MAP16;
+	((uint8_t*)buffer)[1] = 0;
+	((uint8_t*)buffer)[2] = 0;
+	
+	*ptr = 3;
+}
+
+void MsgPack::add_key(const void * buffer, size_t * ptr, const RecID record_id) {
+	uint16_t key;
+	
+	((uint8_t*)buffer)[(*ptr)++] = MSGPACK_TYPE_U16;
+	((uint8_t*)buffer)[(*ptr)++] = record_id >> 8;
+	((uint8_t*)buffer)[(*ptr)++] = record_id & 0xFF;
+	
+	// Auto-inc MAP16 size which should be at the beginning of the buffer
+	
+	key = (((uint8_t*)buffer)[1] << 8) | ((uint8_t*)buffer)[2];
+	key++;
+	
+	((uint8_t*)buffer)[1] = key >> 8;
+	((uint8_t*)buffer)[2] = key & 0xFF;
+}
+
+void MsgPack::msgpack_add(const void * buffer, size_t * ptr, const RecID record_id, bool value) {
+	add_key(buffer, ptr, record_id);
+	
+	if (value)
+		((uint8_t*)buffer)[(*ptr)++] = MSGPACK_TRUE;
+	else
+		((uint8_t*)buffer)[(*ptr)++] = MSGPACK_FALSE;
+}
+
+void MsgPack::msgpack_add(const void * buffer, size_t * ptr, const RecID record_id, uint8_t value) {
+	add_key(buffer, ptr, record_id);
+	
+	if (value < 128) {
+		((uint8_t*)buffer)[(*ptr)++] = value;
+	} else {
+		((uint8_t*)buffer)[(*ptr)++] = MSGPACK_TYPE_U8;
+		((uint8_t*)buffer)[(*ptr)++] = value;
+	}
+}
+
+void MsgPack::msgpack_add(const void * buffer, size_t * ptr, const RecID record_id, int64_t value) {
+	uint8_t c;
+	
+	add_key(buffer, ptr, record_id);
+	
+	((uint8_t*)buffer)[(*ptr)++] = MSGPACK_TYPE_S64;
+	
+	for (c = 0; c < 8; c++)
+		((uint8_t*)buffer)[(*ptr)++] = (value >> (8 * (7 - c))) & 0xFF;
+}
+
+bool MsgPack::msgpack_add(const void * buffer, size_t * ptr, const RecID record_id, std::string value) {
+	uint8_t c;
+	size_t length;
+	
+	add_key(buffer, ptr, record_id);
+	
+	length = value.size();
+	
+	if (length < 32) {
+		((uint8_t*)buffer)[(*ptr)++] = length | 0xA0;			// Fixstr
+	} else if ((length >= 32) && (length < 256)) {
+		((uint8_t*)buffer)[(*ptr)++] = MSGPACK_TYPE_STR8;
+		((uint8_t*)buffer)[(*ptr)++] = length;
+	} else if ((length >= 256) && (length < 65536)) {
+		((uint8_t*)buffer)[(*ptr)++] = MSGPACK_TYPE_STR16;
+		((uint8_t*)buffer)[(*ptr)++] = length >> 8;
+		((uint8_t*)buffer)[(*ptr)++] = length & 0xFF;
+	} else {
+		return false;
+	}
+	
+	for (c = 0; c < length; c++)
+		((uint8_t*)buffer)[(*ptr)++] = value[c];
+		
+	return true;
+}
--- a/firmware/common/msgpack.hpp
+++ b/firmware/common/msgpack.hpp
@@ -25,40 +25,70 @@

 #include "ui.hpp"
 #include <memory>
+#include <cstring>

-#define MSGPACK_TYPE_U16	0xcd
-#define MSGPACK_TYPE_U32	0xce
-#define MSGPACK_TYPE_S32	0xd2
-#define MSGPACK_TYPE_S64	0xd3
-#define MSGPACK_TYPE_STR8	0xd9
-#define MSGPACK_TYPE_STR16	0xda
+#define MSGPACK_NIL			0xC0

-#define MSGPACK_TYPE_MAP16	0xde
+#define MSGPACK_FALSE		0xC2
+#define MSGPACK_TRUE		0xC3

-#define MSGPACK_FALSE		0xc2
-#define MSGPACK_TRUE		0xc3
+#define MSGPACK_TYPE_F32	0xCA
+#define MSGPACK_TYPE_F64	0xCB
+
+#define MSGPACK_TYPE_U8		0xCC
+#define MSGPACK_TYPE_U16	0xCD
+#define MSGPACK_TYPE_U32	0xCE
+#define MSGPACK_TYPE_U64	0xCF
+
+#define MSGPACK_TYPE_S8		0xD0
+#define MSGPACK_TYPE_S16	0xD1
+#define MSGPACK_TYPE_S32	0xD2
+#define MSGPACK_TYPE_S64	0xD3
+
+#define MSGPACK_TYPE_STR8	0xD9
+#define MSGPACK_TYPE_STR16	0xDA
+#define MSGPACK_TYPE_STR32	0xDB
+
+#define MSGPACK_TYPE_ARR16	0xDC
+#define MSGPACK_TYPE_ARR32	0xDD
+
+#define MSGPACK_TYPE_MAP16	0xDE
+#define MSGPACK_TYPE_MAP32	0xDF

 class MsgPack {
 public:

 	enum RecID {
-		FrequencyList = 0,
-		TestList = 1
+		TestListA = 0,
+		TestListB = 1,
+		TestListC = 2,
+		TestListD = 3,
+		TestListE = 4
 	};

-	bool msgpack_get(const void * buffer, const size_t size, const MsgPack::RecID record_id, bool * value);
-	bool msgpack_get(const void * buffer, const size_t size, const RecID record_id, int32_t * value);
+	// Read
+	bool msgpack_get(const void * buffer, const size_t size, const RecID record_id, bool * value);
+	bool msgpack_get(const void * buffer, const size_t size, const RecID record_id, uint8_t * value);
 	bool msgpack_get(const void * buffer, const size_t size, const RecID record_id, int64_t * value);
-	bool msgpack_get(const void * buffer, const size_t size, const RecID record_id, char * value);
+	bool msgpack_get(const void * buffer, const size_t size, const RecID record_id, std::string& value);
+	
+	// Write
+	void msgpack_init(const void * buffer, size_t * ptr);
+	void msgpack_add(const void * buffer, size_t * ptr, const RecID record_id, bool value);
+	void msgpack_add(const void * buffer, size_t * ptr, const RecID record_id, uint8_t value);
+	void msgpack_add(const void * buffer, size_t * ptr, const RecID record_id, int64_t value);
+	bool msgpack_add(const void * buffer, size_t * ptr, const RecID record_id, std::string value);

 private:
 	bool get_raw_byte(const void * buffer, const bool inc, uint8_t * byte);
 	bool get_raw_word(const void * buffer, const bool inc, uint16_t * word);
 	bool get_bool(const void * buffer, const bool inc, bool * value);
+	bool get_u8(const void * buffer, const bool inc, uint8_t * value);
 	bool get_u16(const void * buffer, const bool inc, uint16_t * value);
 	bool get_s32(const void * buffer, const bool inc, int32_t * value);
-	bool get_s64(const void * buffer, const bool inc, int64_t * value);
-	bool get_chars(const void * buffer, const bool inc, char * value);
+	bool get_string(const void * buffer, const bool inc, std::string& value);
+	
+	void add_key(const void * buffer, size_t * ptr, const RecID record_id);
 	
 	bool init_search(const void * buffer, const size_t size);
 	bool search_key(const void * buffer, const RecID record_id);
--- a/firmware/common/ui_widget.cpp
+++ b/firmware/common/ui_widget.cpp
@@ -326,6 +326,7 @@ void BigFrequency::paint(Painter& painter) {
 	char digits[7];
 	char digit;
 	Coord digit_x, digit_y;
+	ui::Color segment_color;
 	
 	const auto rect = screen_rect();
 	
@@ -333,7 +334,7 @@ void BigFrequency::paint(Painter& painter) {
 	painter.fill_rectangle({{0, rect.pos.y}, {240, 52}}, ui::Color::black());
 	
 	if (!_frequency) {
-		for (i = 0; i < 7; i++)
+		for (i = 0; i < 7; i++)		// ----.------
 			digits[i] = 10;
 	} else {
 		_frequency /= 1000;			// GMMM.KKKuuu
@@ -351,6 +352,8 @@ void BigFrequency::paint(Painter& painter) {
 				break;
 		}
 	}
+	
+	segment_color = style().foreground;

 	// Draw
 	digit_x = rect.pos.x;		// 7 * 32 + 8 = 232 (4 px margins)
@@ -358,18 +361,18 @@ void BigFrequency::paint(Painter& painter) {
 		digit = digits[i];
 		digit_y = rect.pos.y;
 		if (digit < 16) {
-			digit_def = big_segment_font[digit];
-			if (digit_def & 0x01) painter.fill_rectangle({{digit_x + 4, digit_y}, 		{20, 4}}, 	ui::Color::white());
-			if (digit_def & 0x02) painter.fill_rectangle({{digit_x + 24, digit_y + 4}, 	{4, 20}}, 	ui::Color::white());
-			if (digit_def & 0x04) painter.fill_rectangle({{digit_x + 24, digit_y + 28}, {4, 20}}, 	ui::Color::white());
-			if (digit_def & 0x08) painter.fill_rectangle({{digit_x + 4, digit_y + 48}, 	{20, 4}}, 	ui::Color::white());
-			if (digit_def & 0x10) painter.fill_rectangle({{digit_x, digit_y + 28}, 		{4, 20}}, 	ui::Color::white());
-			if (digit_def & 0x20) painter.fill_rectangle({{digit_x, digit_y + 4}, 		{4, 20}}, 	ui::Color::white());
-			if (digit_def & 0x40) painter.fill_rectangle({{digit_x + 4, digit_y + 24}, 	{20, 4}}, 	ui::Color::white());
+			digit_def = big_segment_font[(uint8_t)digit];
+			if (digit_def & 0x01) painter.fill_rectangle({{digit_x + 4, digit_y}, 		{20, 4}}, 	segment_color);
+			if (digit_def & 0x02) painter.fill_rectangle({{digit_x + 24, digit_y + 4}, 	{4, 20}}, 	segment_color);
+			if (digit_def & 0x04) painter.fill_rectangle({{digit_x + 24, digit_y + 28}, {4, 20}}, 	segment_color);
+			if (digit_def & 0x08) painter.fill_rectangle({{digit_x + 4, digit_y + 48}, 	{20, 4}}, 	segment_color);
+			if (digit_def & 0x10) painter.fill_rectangle({{digit_x, digit_y + 28}, 		{4, 20}}, 	segment_color);
+			if (digit_def & 0x20) painter.fill_rectangle({{digit_x, digit_y + 4}, 		{4, 20}}, 	segment_color);
+			if (digit_def & 0x40) painter.fill_rectangle({{digit_x + 4, digit_y + 24}, 	{20, 4}}, 	segment_color);
 		}
 		if (i == 3) {
 			// Dot
-			painter.fill_rectangle({{digit_x + 34, digit_y + 48}, {4, 4}}, ui::Color::white());
+			painter.fill_rectangle({{digit_x + 34, digit_y + 48}, {4, 4}}, segment_color);
 			digit_x += 40;
 		} else {
 			digit_x += 32;
@@ -401,7 +404,7 @@ void ProgressBar::paint(Painter& painter) {

 	v_sized = (rect.size.w * _value) / 100;

-	painter.fill_rectangle({rect.pos, {v_sized, rect.size.h}}, ui::Color::green());
+	painter.fill_rectangle({rect.pos, {v_sized, rect.size.h}}, style().foreground);
 	painter.fill_rectangle({{rect.pos.x + v_sized, rect.pos.y}, {rect.size.w - v_sized, rect.size.h}}, s.background);
 	
 	painter.draw_rectangle(rect, s.foreground);