From bc42820ca3cc370adc1d3509375767eabd336242 Mon Sep 17 00:00:00 2001
From: Midgard <midgard@zeus.ugent.be>
Date: Fri, 11 Sep 2020 21:32:23 +0200
Subject: [PATCH] Add util functions for byte (un)packing

---
 lib/obus_can.cpp  | 12 +++---------
 lib/obus_util.cpp | 29 +++++++++++++++++++++++++++++
 lib/obus_util.h   | 14 ++++++++++++++
 3 files changed, 46 insertions(+), 9 deletions(-)
 create mode 100644 lib/obus_util.cpp
 create mode 100644 lib/obus_util.h

diff --git a/lib/obus_can.cpp b/lib/obus_can.cpp
index 3d851be..34d2178 100644
--- a/lib/obus_can.cpp
+++ b/lib/obus_can.cpp
@@ -1,5 +1,6 @@
 #include <mcp2515.h>
 
+#include <obus_util.h>
 #include "obus_can.h"
 
 // Chip select for the CAN module
@@ -116,11 +117,7 @@ bool receive(struct message *msg) {
 				Serial.println(F("W Received illegal gamestatus msg: payload <7"));
 				return false;
 			}
-			msg->gamestatus.time_left   =
-				((uint32_t) receive_frame.data[1] << 0x18) |
-				((uint32_t) receive_frame.data[2] << 0x10) |
-				((uint32_t) receive_frame.data[3] << 0x08) |
-				((uint32_t) receive_frame.data[4]);
+			msg->gamestatus.time_left   = unpack_4b_into_u32(&(receive_frame.data[1]));
 			msg->gamestatus.strikes     = receive_frame.data[5];
 			msg->gamestatus.max_strikes = receive_frame.data[6];
 			break;
@@ -175,10 +172,7 @@ void send(struct message *msg) {
 			break;
 
 		case OBUS_PAYLDTYPE_GAMESTATUS:
-			send_frame.data[1] = (uint8_t) ((msg->gamestatus.time_left & 0xFF000000) >> 0x18);
-			send_frame.data[2] = (uint8_t) ((msg->gamestatus.time_left & 0x00FF0000) >> 0x10);
-			send_frame.data[3] = (uint8_t) ((msg->gamestatus.time_left & 0x0000FF00) >> 0x08);
-			send_frame.data[4] = (uint8_t) (msg->gamestatus.time_left & 0x000000FF);
+			pack_u32_into_4b(&(send_frame.data[1]), msg->gamestatus.time_left);
 			send_frame.data[5] = msg->gamestatus.strikes;
 			send_frame.data[6] = msg->gamestatus.max_strikes;
 			length = 7;
diff --git a/lib/obus_util.cpp b/lib/obus_util.cpp
new file mode 100644
index 0000000..30e7e39
--- /dev/null
+++ b/lib/obus_util.cpp
@@ -0,0 +1,29 @@
+#include "obus_util.h"
+
+
+uint32_t unpack_4b_into_u32(uint8_t *data) {
+	return
+		((uint32_t) data[0] << 0x18) |
+		((uint32_t) data[1] << 0x10) |
+		((uint32_t) data[2] << 0x08) |
+		((uint32_t) data[3]);
+}
+
+void pack_u32_into_4b(uint8_t *dest, uint32_t data) {
+	dest[0] = (uint8_t) ((data & 0xFF000000) >> 0x18);
+	dest[1] = (uint8_t) ((data & 0x00FF0000) >> 0x10);
+	dest[2] = (uint8_t) ((data & 0x0000FF00) >> 0x08);
+	dest[3] = (uint8_t)  (data & 0x000000FF);
+}
+
+
+uint16_t unpack_2b_into_u16(uint8_t *data) {
+	return
+		((uint16_t) data[0] << 0x08) |
+		((uint16_t) data[1]);
+}
+
+void pack_u16_into_2b(uint8_t *dest, uint16_t data) {
+	dest[0] = (uint8_t) ((data & 0xFF00) >> 0x08);
+	dest[1] = (uint8_t)  (data & 0x00FF);
+}
diff --git a/lib/obus_util.h b/lib/obus_util.h
new file mode 100644
index 0000000..42a8e7d
--- /dev/null
+++ b/lib/obus_util.h
@@ -0,0 +1,14 @@
+#ifndef OBUS_UTIL_H
+#define OBUS_UTIL_H
+
+#include "Arduino.h"
+
+
+uint32_t unpack_4b_into_u32(uint8_t *data);
+void pack_u32_into_4b(uint8_t *dest, uint32_t data);
+
+uint16_t unpack_2b_into_u16(uint8_t *data);
+void pack_u16_into_2b(uint8_t *dest, uint16_t data);
+
+
+#endif /* end of include guard: OBUS_UTIL_H */