byte stuffing works, checksum works afaik, tested encoded packets

compile_commands.json

@@ -12,11 +12,11 @@
             "-Icfg",
             "-Iinc",
             "-o",
-            "build/p_idp.o",
-            "src/p_idp.cpp"
+            "build/p_serial_bus.o",
+            "src/p_serial_bus.cpp"
         ],
         "directory": "/storage/Shared/Projects/Penguinator/pi_serial_bus_manager",
-        "file": "src/p_idp.cpp"
+        "file": "src/p_serial_bus.cpp"
     },
     {
         "arguments": [
@@ -31,11 +31,11 @@
             "-Icfg",
             "-Iinc",
             "-o",
-            "build/p_serial_bus.o",
-            "src/p_serial_bus.cpp"
+            "build/p_idp.o",
+            "src/p_idp.cpp"
         ],
         "directory": "/storage/Shared/Projects/Penguinator/pi_serial_bus_manager",
-        "file": "src/p_serial_bus.cpp"
+        "file": "src/p_idp.cpp"
     },
     {
         "arguments": [

inc/p_serial_packet.h

@@ -32,11 +32,15 @@ public:
 	bool encode();
 	bool decode();
 	static ps_packet from_xy(int x, int y);
+	static ps_packet test_packet();
+#ifdef DEBUG
+	void show_packet();
+#endif
 
 private:
 	bool stuff_bytes();
 	bool destuff_bytes();
 	bool validate();
-	void calc_checksum();
+	uint8_t calc_checksum();
 };
 #endif

src/main.cpp

@@ -28,21 +28,25 @@ int main()
 	sub.connect(ZMQ_IPC_ADDR);
 	sub.setsockopt(ZMQ_SUBSCRIBE, "", 0);
 
+	zmq::message_t msg_recv;
+	zmq::message_t msg_send;
 	for (;;)
 	{
-		zmq::message_t msg;
 		int32_t x = 0;
 		int32_t y = 0;
-		if (sub.recv(&msg, 0))
+		if (sub.recv(&msg_recv, 0))
 		{
 			uint8_t xy[64];
 			memset(xy, 0, 64);
-			memcpy(xy, msg.data(), msg.size());
+			memcpy(xy, msg_recv.data(), msg_recv.size());
 			sscanf((const char*)xy, "X:%" PRId32 " Y:%" PRId32 "\n", &x, &y);
 			ps_packet xy_packet = ps_packet::from_xy(x, y);
-			for (int ind = 0; ind < xy_packet.msg_len; ind++)
+			xy_packet.encode();
+			xy_packet.show_packet();
+
+			for (size_t ind = 0; ind < xy_packet.msg_len; ind++)
 			{
-				printf("[%d]: %02x ", ind, xy_packet.data[ind]);
+				printf("[%lu]: %02x ", ind, xy_packet.data[ind]);
 			}
 			printf("\n");
 		}

src/p_serial_packet.cpp

@@ -34,7 +34,14 @@ bool ps_packet::encode()
 	bool ret = true;
 	do
 	{
-
+		uint8_t csum = calc_checksum();
+		if (!stuff_bytes())
+		{
+			ret = false;
+			break;
+		}
+		checksum = &data[msg_len - 1];
+		*checksum = csum;
 	} while (0);
 	return ret;
 }
@@ -44,11 +51,23 @@ bool ps_packet::decode()
 	bool ret = true;
 	do
 	{
+		if (!destuff_bytes())
+		{
+			ret = false;
+			break;
+		}
+
+		if (!validate())
+		{
+			ret = false;
+			break;
+		}
 
 	} while (0);
 	return ret;
 }
 
+// done after receiving a packet and destuffing it
 bool ps_packet::validate()
 {
 	bool ret = true;
@@ -58,15 +77,25 @@ bool ps_packet::validate()
 	} while (0);
 	return ret;
 }
-void ps_packet::calc_checksum()
-{}
+
+uint8_t ps_packet::calc_checksum()
+{
+	uint32_t ret = 0;
+	for (size_t ind = 0; ind < *frame_data_len; ind++)
+	{
+		ret += frame_data[ind];
+	}
+	ret &= 0xFF;
+	ret = 0xFF - ret;
+	return (uint8_t)ret;
+}
 
 bool ps_packet::stuff_bytes()
 {
 	int old_ind = 0;
 	int new_ind = 0;
-	uint8_t temp_buffer[MAX_FRAME_DATA_LEN];
-	memcpy(frame_data, temp_buffer, MAX_FRAME_DATA_LEN);
+	uint8_t temp_buffer[MAX_FRAME_DATA_LEN] = {0};
+	memcpy(temp_buffer, frame_data, MAX_FRAME_DATA_LEN);
 	msg_len = *frame_data_len + 5;
 	for (; old_ind < *frame_data_len; old_ind++)
 	{
@@ -74,11 +103,11 @@ bool ps_packet::stuff_bytes()
 		{
 			msg_len++;
 			frame_data[new_ind++] = 0x7D;
-			frame_data[new_ind] = temp_buffer[old_ind];
+			frame_data[new_ind++] = temp_buffer[old_ind] ^ 0x20;
 		}
 		else
 		{
-			frame_data[new_ind] = temp_buffer[old_ind];
+			frame_data[new_ind++] = temp_buffer[old_ind];
 		}
 	}
 	if (msg_len > MAX_MESSAGE_LEN)
@@ -105,3 +134,29 @@ ps_packet ps_packet::from_xy(int32_t x, int32_t y)
 
 	return ps_packet(MASTER_ADDR, 0x02, frame_data, 1 + sizeof(x) + sizeof(y));
 }
+
+ps_packet ps_packet::test_packet()
+{
+	uint8_t frame_data[16] = {0xFF, 0x01, 0x02, 0x03, 0x04, 0x7D, 0x7D, 0x07,
+							  0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0F, 0x7E};
+
+	return ps_packet(MASTER_ADDR, 0xFF, frame_data, 16);
+}
+
+#ifdef DEBUG
+void ps_packet::show_packet()
+{
+	printf("Start Byte:\t\t0x%02x\n", data[0]);
+	printf("Source Address:\t\t0x%02x\n", *src_addr);
+	printf("Dest Address:\t\t0x%02x\n", *dest_addr);
+	printf("Frame Data Len:\t\t0x%02x\n", *frame_data_len);
+	printf("Frame Data:\n");
+	for (size_t ind = 0; ind < msg_len - 4; ind++)
+	{
+		printf("\t\t[%2lu]: 0x%02x\n", ind, frame_data[(int)ind]);
+	}
+	printf("\n");
+	printf("Message Length: %lu\n", msg_len);
+	printf("Checksum: %0x02x\n", *checksum);
+}
+#endif