1 files changed, 43 insertions, 3 deletions
diff --git a/src/isotp/receive.c b/src/isotp/receive.c
index 9add9d5f..b4978097 100644
--- a/src/isotp/receive.c
+++ b/src/isotp/receive.c
@@ -1,7 +1,47 @@
 #include <isotp/receive.h>
 
-void isotp_handle_single_frame(IsoTpHandler* handler,
-        const uint16_t arbitration_id, const uint8_t* data,
+void isotp_handle_single_frame(IsoTpHandler* handler, IsoTpMessage* message) {
+    isotp_complete_receive(handler, message);
+}
+
+void isotp_complete_receive(IsoTpHandler* handler, IsoTpMessage* message) {
+    handler->message_received_callback(message);
+}
+
+void isotp_receive_can_frame(IsoTpHandler* handler,
+        const uint16_t arbitration_id, const uint64_t data,
         const uint8_t length) {
-    handler->message_received_callback(arbitration_id, data, length);
+    if(arbitration_id != handler->arbitration_id){
+        return;
+    }
+
+    // TODO use CanMessage struct from canutil library - allocate payload buffer
+    // on stack, 8 bytes
+    // TODO  this function should receive uint64_t...
+    IsoTpProtocolControlInformation pci = (IsoTpProtocolControlInformation)
+            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);
+    }
+
+    IsoTpMessage message = {
+        arbitration_id: arbitration_id,
+        payload: payload,
+        size: payload_length
+    };
+
+    switch(pci) {
+        case PCI_SINGLE:
+            isotp_handle_single_frame(handler, &message);
+            break;
+        default:
+            handler->shims->log("Only single frame messages are supported");
+            break;
+    }
 }
+