Test basic rx of a single frame message.

2 files changed, 23 insertions, 8 deletions

diff --git a/src/isotp/isotp.c b/src/isotp/isotp.c
index 597ffe9d..af9c2631 100644
--- a/src/isotp/isotp.c
+++ b/src/isotp/isotp.c
@@ -8,7 +8,8 @@ const uint8_t ISO_TP_DEFAULT_RESPONSE_TIMEOUT = 100;
 const bool ISO_TP_DEFAULT_FRAME_PADDING_STATUS = true;
 
 void isotp_receive_can_frame(IsoTpHandler* handler,
-        const uint16_t arbitration_id, const uint8_t* data, const uint8_t length) {
+        const uint16_t arbitration_id, const uint64_t data,
+        const uint8_t length) {
     if(arbitration_id != handler->arbitration_id){
         return;
     }
@@ -17,11 +18,20 @@ void isotp_receive_can_frame(IsoTpHandler* handler,
     // on stack, 8 bytes
     // TODO  this function should receive uint64_t...
     IsoTpProtocolControlInformation pci = (IsoTpProtocolControlInformation)
-            getBitField((uint64_t)data, 0, 2, false);
+            getBitField(data, 0, 4, false);
+
+    // TODO this is messed up! need a better API for grabbing bytes
+    uint8_t payload_length = getBitField(data, 4, 4, false);
+    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);
+    }
 
     switch(pci) {
         case PCI_SINGLE:
-            isotp_handle_single_frame(handler, arbitration_id, data, length);
+            isotp_handle_single_frame(handler, arbitration_id, payload,
+                    payload_length);
             break;
         default:
             handler->shims->log("Only single frame messages are supported");
@@ -29,7 +39,7 @@ void isotp_receive_can_frame(IsoTpHandler* handler,
     }
 }
 
-bool isotp_send(const uint8_t* payload, uint16_t payload_size) {
+bool isotp_send(const uint8_t* payload, uint16_t size) {
      // we determine if it's single/multi frame and start the send
 }
 
@@ -67,8 +77,13 @@ IsoTpHandler isotp_init(IsoTpShims* shims, uint16_t arbitration_id,
 // TODO this would be better as a "isotp_message_to_string"
 void log_isotp_message(const uint16_t arbitration_id,
         const uint8_t* payload, const uint16_t size) {
-    debug("ID: 0x%02x, Payload:", arbitration_id);
-    for(int i = 0; i < size; i++) {
-        debug("0x%x", payload[i]);
+    debug("ID: 0x%02x", arbitration_id);
+    if(size > 0) {
+        debug("Payload:");
+        for(int i = 0; i < size; i++) {
+            debug("0x%x", payload[i]);
+        }
+    }  else {
+        debug("(no payload)");
     }
 }
diff --git a/src/isotp/isotp.h b/src/isotp/isotp.h
index 089ecbc0..f1719548 100644
--- a/src/isotp/isotp.h
+++ b/src/isotp/isotp.h
@@ -94,7 +94,7 @@ void isotp_set_timeout(IsoTpHandler* handler, uint16_t timeout_ms);
 bool isotp_send(const uint8_t* payload, uint16_t payload_size);
 
 void isotp_receive_can_frame(IsoTpHandler* handler,
-        const uint16_t arbitration_id, const uint8_t* data,
+        const uint16_t arbitration_id, const uint64_t data,
         const uint8_t length);
 
 void isotp_destroy(IsoTpHandler* handler);