src/isotp/receive.c

   1 #include <isotp/receive.h>
   2 #include <isotp/send.h>
   3 #include <bitfield/bitfield.h>
   4 #include <string.h>
   5 #include <stdlib.h>
   6
   7 #define ARBITRATION_ID_OFFSET 0x8
   8
   9 static void isotp_complete_receive(IsoTpReceiveHandle* handle, IsoTpMessage* message) {
  10     if(handle->message_received_callback != NULL) {
  11         handle->message_received_callback(message);
  12     }
  13 }
  14
  15 bool isotp_handle_single_frame(IsoTpReceiveHandle* handle, IsoTpMessage* message) {
  16     isotp_complete_receive(handle, message);
  17     return true;
  18 }
  19
  20 bool isotp_handle_multi_frame(IsoTpReceiveHandle* handle, IsoTpMessage* message) {
  21     // call this once all consecutive frames have been received
  22     isotp_complete_receive(handle, message);
  23     return true;
  24 }
  25
  26 bool isotp_send_flow_control_frame(IsoTpShims* shims, IsoTpMessage* message) {
  27     uint8_t can_data[CAN_MESSAGE_BYTE_SIZE] = {0};
  28
  29     if(!set_nibble(PCI_NIBBLE_INDEX, PCI_FLOW_CONTROL_FRAME, can_data, sizeof(can_data))) {
  30         shims->log("Unable to set PCI in CAN data");
  31         return false;
  32     }
  33
  34     shims->send_can_message(message->arbitration_id - ARBITRATION_ID_OFFSET, can_data,
  35             shims->frame_padding ? 8 : 1 + message->size);
  36     return true;
  37 }
  38
  39
  40 IsoTpReceiveHandle isotp_receive(IsoTpShims* shims,
  41         const uint32_t arbitration_id, IsoTpMessageReceivedHandler callback) {
  42     IsoTpReceiveHandle handle = {
  43         success: false,
  44         completed: false,
  45         arbitration_id: arbitration_id,
  46         message_received_callback: callback
  47     };
  48
  49     return handle;
  50 }
  51
  52 IsoTpMessage isotp_continue_receive(IsoTpShims* shims,
  53         IsoTpReceiveHandle* handle, const uint32_t arbitration_id,
  54         const uint8_t data[], const uint8_t size) {
  55     IsoTpMessage message = {
  56         arbitration_id: arbitration_id,
  57         completed: false,
  58         multi_frame: false,
  59         payload: {0},
  60         size: 0
  61     };
  62
  63     if(size < 1) {
  64         return message;
  65     }
  66
  67     if(handle->arbitration_id != arbitration_id) {
  68         if(shims->log != NULL)  {
  69             // You may turn this on for debugging, but in normal operation it's
  70             // very noisy if you are passing all received CAN messages to this
  71             // handler.
  72             /* shims->log("The arb ID 0x%x doesn't match the expected rx ID 0x%x", */
  73                     /* arbitration_id, handle->arbitration_id); */
  74         }
  75         return message;
  76     }
  77
  78     IsoTpProtocolControlInformation pci = (IsoTpProtocolControlInformation)
  79             get_nibble(data, size, 0);
  80
  81     // TODO this is set up to handle rx a response with a payload, but not to
  82     // handle flow control responses for multi frame messages that we're in the
  83     // process of sending
  84
  85     switch(pci) {
  86         case PCI_SINGLE: {
  87             uint8_t payload_length = get_nibble(data, size, 1);
  88
  89             if(payload_length > 0) {
  90                 memcpy(message.payload, &data[1], payload_length);
  91             }
  92
  93             message.size = payload_length;
  94             message.completed = true;
  95             handle->success = true;
  96             handle->completed = true;
  97             isotp_handle_single_frame(handle, &message);
  98             break;
  99         }
 100         //If multi-frame, then the payload length is contained in the 12
 101         //bits following the first nibble of Byte 0.
 102         case PCI_FIRST_FRAME: {
 103             uint16_t payload_length = (get_nibble(data, size, 1) << 8) + get_byte(data, size, 1);
 104
 105             if(payload_length > OUR_MAX_ISO_TP_MESSAGE_SIZE) {
 106                 shims->log("Multi-frame response too large for receive buffer.");
 107                 break;
 108             }
 109
 110             //Need to allocate memory for the combination of multi-frame
 111             //messages. That way we don't have to allocate 4k of memory
 112             //for each multi-frame response.
 113             uint8_t* combined_payload = NULL;
 114             combined_payload = (uint8_t*)malloc(sizeof(uint8_t)*payload_length);
 115
 116             if(combined_payload == NULL) {
 117                 shims->log("Unable to allocate memory for multi-frame response.");
 118                 break;
 119             }
 120
 121             memcpy(combined_payload, &data[2], CAN_MESSAGE_BYTE_SIZE - 2);
 122             handle->receive_buffer = combined_payload;
 123             handle->received_buffer_size = CAN_MESSAGE_BYTE_SIZE - 2;
 124             handle->incoming_message_size = payload_length;
 125
 126             message.multi_frame = true;
 127             handle->success = false;
 128             handle->completed = false;
 129             isotp_send_flow_control_frame(shims, &message);
 130             break;
 131         }
 132         case PCI_CONSECUTIVE_FRAME: {
 133             uint8_t start_index = handle->received_buffer_size;
 134             uint8_t remaining_bytes = handle->incoming_message_size - start_index;
 135             message.multi_frame = true;
 136
 137             if(remaining_bytes > 7) {
 138                 memcpy(&handle->receive_buffer[start_index], &data[1], CAN_MESSAGE_BYTE_SIZE - 1);
 139                 handle->received_buffer_size = start_index + 7;
 140             } else {
 141                 memcpy(&handle->receive_buffer[start_index], &data[1], remaining_bytes);
 142                 handle->received_buffer_size = start_index + remaining_bytes;
 143
 144                 if(handle->received_buffer_size != handle->incoming_message_size){
 145                     free(handle->receive_buffer);
 146                     handle->success = false;
 147                     shims->log("Error capturing all bytes of multi-frame. Freeing memory.");
 148                 } else {
 149                     memcpy(message.payload,&handle->receive_buffer[0],handle->incoming_message_size);
 150                     free(handle->receive_buffer);
 151                     message.size = handle->incoming_message_size;
 152                     message.completed = true;
 153                     shims->log("Successfully captured all of multi-frame. Freeing memory.");
 154
 155                     handle->success = true;
 156                     handle->completed = true;
 157                     isotp_handle_multi_frame(handle, &message);
 158                 }
 159             }
 160             break;
 161         }
 162         default:
 163             break;
 164     }
 165     return message;
 166 }