4 files changed, 29 insertions, 32 deletions

diff --git a/src/isotp/isotp.h b/src/isotp/isotp.h
index a204110f..619dcc69 100644
--- a/src/isotp/isotp.h
+++ b/src/isotp/isotp.h
@@ -4,6 +4,8 @@
 #include <stdint.h>
 #include <stdbool.h>
 
+#define CAN_MESSAGE_BYTE_SIZE 8
+
 #ifdef __cplusplus
 extern "C" {
 #endif
diff --git a/src/isotp/receive.c b/src/isotp/receive.c
index 51562fcc..bd94e7aa 100644
--- a/src/isotp/receive.c
+++ b/src/isotp/receive.c
@@ -9,26 +9,19 @@ void isotp_complete_receive(IsoTpHandler* handler, IsoTpMessage* message) {
 }
 
 void isotp_receive_can_frame(IsoTpHandler* handler,
-        const uint16_t arbitration_id, const uint64_t data,
-        const uint8_t length) {
-    if(arbitration_id != handler->arbitration_id){
+        const uint16_t arbitration_id, const uint8_t data[],
+        const uint8_t data_length) {
+    if(arbitration_id != handler->arbitration_id || data_length < 1) {
         return;
     }
 
-    // TODO use CanMessage struct from canutil library - allocate payload buffer
-    // on stack, 8 bytes
     IsoTpProtocolControlInformation pci = (IsoTpProtocolControlInformation)
-            getBitField(data, 0, 4, false);
+            get_nibble(data, data_length, 0);
 
-    IsoTpProtocolControlInformation pci = (IsoTpProtocolControlInformation)
-            getNibble(0, data, 64, LITTE_ENDIAN);
-
-    // TODO this is messed up! need a better API for grabbing bytes
-    uint8_t payload_length = getBitField(data, 4, 4, false);
+    uint8_t payload_length = get_nibble(data, data_length, 1);
     uint8_t payload[payload_length];
-    uint64_t flipped_data = __builtin_bswap64(data);
-    if(payload_length > 0) {
-        memcpy(payload, &(((uint8_t*)&flipped_data)[1]), payload_length);
+    if(payload_length > 0 && data_length > 0) {
+        memcpy(payload, &data[1], payload_length);
     }
 
     IsoTpMessage message = {
diff --git a/src/isotp/receive.h b/src/isotp/receive.h
index 0ff9e8f1..feca993c 100644
--- a/src/isotp/receive.h
+++ b/src/isotp/receive.h
@@ -14,7 +14,8 @@ void isotp_handle_single_frame(IsoTpHandler* handler, IsoTpMessage* message);
 void isotp_complete_receive(IsoTpHandler* handler, IsoTpMessage* message);
 
 void isotp_receive_can_frame(IsoTpHandler* handler,
-        const uint16_t arbitration_id, const uint64_t data, const uint8_t size);
+        const uint16_t arbitration_id, const uint8_t data[],
+        const uint8_t size);
 
 #ifdef __cplusplus
 }
diff --git a/src/isotp/send.c b/src/isotp/send.c
index b72d1b91..ae208eb3 100644
--- a/src/isotp/send.c
+++ b/src/isotp/send.c
@@ -1,10 +1,8 @@
 #include <isotp/send.h>
 
-#define PCI_START_BIT 0
-#define PCI_WIDTH 4
-#define PAYLOAD_LENGTH_START_BIT PCI_START_BIT + PCI_WIDTH
-#define PAYLOAD_LENGTH_WIDTH 4
-#define PAYLOAD_START_BIT PAYLOAD_LENGTH_START_BIT + PAYLOAD_LENGTH_WIDTH
+#define PCI_NIBBLE_INDEX 0
+#define PAYLOAD_LENGTH_NIBBLE_INDEX 1
+#define PAYLOAD_BYTE_INDEX 1
 
 void isotp_complete_send(IsoTpHandler* handler, IsoTpMessage* message,
         bool status) {
@@ -12,21 +10,24 @@ void isotp_complete_send(IsoTpHandler* handler, IsoTpMessage* message,
 }
 
 bool isotp_send_single_frame(IsoTpHandler* handler, IsoTpMessage* message) {
-    uint64_t data = 0;
-    setBitField(&data, PCI_SINGLE, PCI_START_BIT, PCI_WIDTH);
-    setBitField(&data, message->size, PAYLOAD_LENGTH_START_BIT, PAYLOAD_LENGTH_WIDTH);
-    // TODO need a better bitfield API to support this - use byte array instead
-    // of uint64_t and specify desired total width
-    for(int i = 0; i < message->size; i++) {
-        setBitField(&data, message->payload[i], PAYLOAD_START_BIT + i * 8, 8);
+    uint8_t can_data[CAN_MESSAGE_BYTE_SIZE] = {0};
+    if(!set_nibble(PCI_NIBBLE_INDEX, PCI_SINGLE, can_data, sizeof(can_data))) {
+        handler->shims->log("Unable to set PCI in CAN data");
+        return false;
     }
 
-    uint8_t data_array[message->size + 1];
-    for(int i = 0; i < sizeof(data_array); i++) {
-        // TODO need getByte(x) function
-        data_array[i] = getBitField(data, i * 8, 8, false);
+    if(!set_nibble(PAYLOAD_LENGTH_NIBBLE_INDEX, message->size, can_data,
+                sizeof(can_data))) {
+        handler->shims->log("Unable to set payload length in CAN data");
+        return false;
     }
-    handler->shims->send_can_message(message->arbitration_id, data_array, sizeof(data_array));
+
+    if(message->size > 0) {
+        memcpy(&can_data[1], message->payload, message->size);
+    }
+
+    handler->shims->send_can_message(message->arbitration_id, can_data,
+            1 + message->size);
     isotp_complete_send(handler, message, true);
     return true;
 }