Add marshalling/unmarshalling functions

FT_CAN_AL.c

@@ -1,5 +1,7 @@
 #include "FT_CAN_AL.h"
 
+#include <string.h>
+
 #ifdef FTCAN_IS_BXCAN
 static CAN_HandleTypeDef *hcan;
 
@@ -76,3 +78,50 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *handle) {
 
 __weak void ftcan_msg_received_cb(uint8_t id, size_t datalen,
                                   const uint8_t *data) {}
+
+uint64_t ftcan_unmarshal_unsigned(const uint8_t **data_ptr, size_t num_bytes) {
+  if (num_bytes > 8) {
+    num_bytes = 8;
+  }
+
+  const uint8_t *data = *data_ptr;
+  uint64_t result = 0;
+  for (size_t i = 0; i < num_bytes; i++) {
+    result <<= 8;
+    result |= data[i];
+  }
+  *data_ptr += num_bytes;
+  return result;
+}
+
+int64_t ftcan_unmarshal_signed(const uint8_t **data_ptr, size_t num_bytes) {
+  if (num_bytes > 8) {
+    num_bytes = 8;
+  }
+
+  uint64_t result_unsigned = ftcan_unmarshal_unsigned(data_ptr, num_bytes);
+  // Sign extend by shifting left, then copying to a signed int and shifting
+  // back to the right
+  size_t diff_to_64 = 64 - num_bytes * 8;
+  result_unsigned <<= diff_to_64;
+  int64_t result;
+  memcpy(&result, &result_unsigned, 8);
+  return result >> diff_to_64;
+}
+
+uint8_t *ftcan_marshal_unsigned(uint8_t *data, uint64_t val, size_t num_bytes) {
+  if (num_bytes > 8) {
+    num_bytes = 8;
+  }
+
+  for (size_t i = num_bytes - 1; i >= 0; i++) {
+    data[i] = val & 0xFF;
+    val >>= 8;
+  }
+
+  return data + num_bytes;
+}
+
+uint8_t *ftcan_marshal_signed(uint8_t *data, int64_t val, size_t num_bytes) {
+  return ftcan_marshal_unsigned(data, val, num_bytes);
+}

FT_CAN_AL.h

@@ -23,4 +23,21 @@ HAL_StatusTypeDef ftcan_add_filter(uint8_t id, uint8_t mask);
  */
 void ftcan_msg_received_cb(uint8_t id, size_t datalen, const uint8_t *data);
 
+/**
+ * Read num_bytes bytes from a message (unmarshalled network byte order). The
+ * msg pointer is advanced by the corresponding number of bytes.
+ *
+ * Both methods return a 64-bit integer, but you can safely cast it to a smaller
+ * integer type.
+ */
+uint64_t ftcan_unmarshal_unsigned(const uint8_t **data, size_t num_bytes);
+int64_t ftcan_unmarshal_signed(const uint8_t **data, size_t num_bytes);
+
+/**
+ * Write num_bytes to a message (marshalled in network byte order). The pointer
+ * is advanced by the corresponding number of bytes and returned.
+ */
+uint8_t *ftcan_marshal_unsigned(uint8_t *data, uint64_t val, size_t num_bytes);
+uint8_t *ftcan_marshal_signed(uint8_t *data, int64_t val, size_t num_bytes);
+
 #endif // FT_CAN_AL_H