src/obd2/obd2.c

   1 #include <obd2/obd2.h>
   2 #include <bitfield/bitfield.h>
   3 #include <string.h>
   4 #include <limits.h>
   5 #include <stddef.h>
   6 #include <sys/param.h>
   7
   8 #define ARBITRATION_ID_OFFSET 0x8
   9 #define MODE_RESPONSE_OFFSET 0x40
  10 #define NEGATIVE_RESPONSE_MODE 0x7f
  11 #define MAX_DIAGNOSTIC_PAYLOAD_SIZE 6
  12 #define MODE_BYTE_INDEX 0
  13 #define PID_BYTE_INDEX 1
  14 #define NEGATIVE_RESPONSE_MODE_INDEX 1
  15 #define NEGATIVE_RESPONSE_NRC_INDEX 2
  16
  17 #ifndef MAX
  18 #define MAX(x, y) (((x) > (y)) ? (x) : (y))
  19 #endif
  20
  21 DiagnosticShims diagnostic_init_shims(LogShim log,
  22         SendCanMessageShim send_can_message,
  23         SetTimerShim set_timer) {
  24     DiagnosticShims shims = {
  25         log: log,
  26         send_can_message: send_can_message,
  27         set_timer: set_timer
  28     };
  29     return shims;
  30 }
  31
  32 DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims,
  33         DiagnosticRequest* request, DiagnosticResponseReceived callback) {
  34     DiagnosticRequestHandle handle = {
  35         request: *request,
  36         callback: callback,
  37         success: false,
  38         completed: false
  39     };
  40
  41     uint8_t payload[MAX_DIAGNOSTIC_PAYLOAD_SIZE] = {0};
  42     payload[MODE_BYTE_INDEX] = request->mode;
  43     if(request->pid_length > 0) {
  44         set_bitfield(request->pid, PID_BYTE_INDEX * CHAR_BIT,
  45                 request->pid_length * CHAR_BIT, payload, sizeof(payload));
  46     }
  47     if(request->payload_length > 0) {
  48         memcpy(&payload[PID_BYTE_INDEX + request->pid_length],
  49                 request->payload, request->payload_length);
  50     }
  51
  52     handle.isotp_shims = isotp_init_shims(shims->log,
  53             shims->send_can_message,
  54             shims->set_timer);
  55
  56     handle.isotp_send_handle = isotp_send(&handle.isotp_shims,
  57             request->arbitration_id, payload,
  58             1 + request->payload_length + request->pid_length,
  59             NULL);
  60     if(shims->log != NULL) {
  61         shims->log("Sending diagnostic request: arb_id: 0x%02x, mode: 0x%x, pid: 0x%x, payload: 0x%02x%02x%02x%02x%02x%02x%02x%02x, size: %d\r\n",
  62                 request->arbitration_id,
  63                 request->mode,
  64                 request->pid,
  65                 request->payload[0],
  66                 request->payload[1],
  67                 request->payload[2],
  68                 request->payload[3],
  69                 request->payload[4],
  70                 request->payload[5],
  71                 request->payload[6],
  72                 request->payload[7],
  73                 request->payload_length);
  74     }
  75
  76     handle.isotp_receive_handle = isotp_receive(&handle.isotp_shims,
  77             request->arbitration_id + ARBITRATION_ID_OFFSET,
  78             NULL);
  79
  80     // TODO notes on multi frame:
  81     // TODO what are the timers for exactly?
  82     //
  83     // when sending multi frame, send 1 frame, wait for a response
  84     // if it says send all, send all right away
  85     // if it says flow control, set the time for the next send
  86     // instead of creating a timer with an async callback, add a process_handle
  87     // function that's called repeatedly in the main loop - if it's time to
  88     // send, we do it. so there's a process_handle_send and receive_can_frame
  89     // that are just called continuously from the main loop. it's a waste of a
  90     // few cpu cycles but it may be more  natural than callbacks.
  91     //
  92     // what woudl a timer callback look like...it would need to pass the handle
  93     // and that's all. seems like a context void* would be able to capture all
  94     // of the information but arg, memory allocation. look at how it's done in
  95     // the other library again
  96     //
  97     return handle;
  98 }
  99
 100 DiagnosticRequestHandle diagnostic_request_pid(DiagnosticShims* shims,
 101         DiagnosticPidRequestType pid_request_type, uint16_t arbitration_id,
 102         uint16_t pid, DiagnosticResponseReceived callback) {
 103     DiagnosticRequest request = {
 104         arbitration_id: arbitration_id,
 105         mode: pid_request_type == DIAGNOSTIC_STANDARD_PID ? 0x1 : 0x22,
 106         pid: pid,
 107         pid_length: pid_request_type == DIAGNOSTIC_STANDARD_PID ? 1 : 2
 108     };
 109
 110     return diagnostic_request(shims, &request, callback);
 111 }
 112
 113 static bool handle_negative_response(IsoTpMessage* message,
 114         DiagnosticResponse* response, DiagnosticShims* shims) {
 115     bool response_was_negative = false;
 116     if(response->mode == NEGATIVE_RESPONSE_MODE) {
 117         response_was_negative = true;
 118         if(message->size > NEGATIVE_RESPONSE_MODE_INDEX) {
 119             response->mode = message->payload[NEGATIVE_RESPONSE_MODE_INDEX];
 120         }
 121
 122         if(message->size > NEGATIVE_RESPONSE_NRC_INDEX) {
 123             response->negative_response_code = message->payload[NEGATIVE_RESPONSE_NRC_INDEX];
 124         }
 125
 126         response->success = false;
 127         response->completed = true;
 128     }
 129     return response_was_negative;
 130 }
 131
 132 static bool handle_positive_response(DiagnosticRequestHandle* handle,
 133         IsoTpMessage* message, DiagnosticResponse* response,
 134         DiagnosticShims* shims) {
 135     bool response_was_positive = false;
 136     if(response->mode == handle->request.mode + MODE_RESPONSE_OFFSET) {
 137         response_was_positive = true;
 138         // hide the "response" version of the mode from the user
 139         // if it matched
 140         response->mode = handle->request.mode;
 141         if(handle->request.pid_length > 0 && message->size > 1) {
 142             if(handle->request.pid_length == 2) {
 143                 response->pid = *(uint16_t*)&message->payload[PID_BYTE_INDEX];
 144                 if(BYTE_ORDER == LITTLE_ENDIAN) {
 145                     response->pid = __builtin_bswap32(response->pid << 16);
 146                 }
 147             } else {
 148                 response->pid = message->payload[PID_BYTE_INDEX];
 149             }
 150             // TODO we're not currently throwing an error or anything if the PID
 151             // doesn't match - it may be OK to leave that up to the user.
 152         }
 153
 154         uint8_t payload_index = 1 + handle->request.pid_length;
 155         response->payload_length = MAX(0, message->size - payload_index);
 156         if(response->payload_length > 0) {
 157             memcpy(response->payload, &message->payload[payload_index],
 158                     response->payload_length);
 159         }
 160         response->success = true;
 161         response->completed = true;
 162     }
 163     return response_was_positive;
 164 }
 165
 166 DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
 167         DiagnosticRequestHandle* handle, const uint16_t arbitration_id,
 168         const uint8_t data[], const uint8_t size) {
 169
 170     DiagnosticResponse response = {
 171         arbitration_id: arbitration_id,
 172         success: false,
 173         completed: false
 174     };
 175
 176     if(!handle->isotp_send_handle.completed) {
 177         isotp_continue_send(&handle->isotp_shims,
 178                 &handle->isotp_send_handle, arbitration_id, data, size);
 179     } else if(!handle->isotp_receive_handle.completed) {
 180         IsoTpMessage message = isotp_continue_receive(&handle->isotp_shims,
 181                 &handle->isotp_receive_handle, arbitration_id, data, size);
 182
 183         if(message.completed) {
 184             if(message.size > 0) {
 185                 response.mode = message.payload[0];
 186                 if(handle_negative_response(&message, &response, shims)) {
 187                     shims->log("Received a negative response to mode %d on arb ID 0x%x",
 188                             response.mode, response.arbitration_id);
 189                     handle->success = true;
 190                     handle->completed = true;
 191                 } else if(handle_positive_response(handle, &message, &response,
 192                             shims)) {
 193                     shims->log("Received a positive mode %d response on arb ID 0x%x",
 194                             response.mode, response.arbitration_id);
 195                     handle->success = true;
 196                     handle->completed = true;
 197                 } else {
 198                     shims->log("Response was for a mode 0x%x request, not our mode 0x%x request",
 199                             response.mode - MODE_RESPONSE_OFFSET,
 200                             handle->request.mode);
 201                 }
 202             } else {
 203                 shims->log("Received an empty response on arb ID 0x%x",
 204                         response.arbitration_id);
 205             }
 206             // TODO For now even if we got an empty repsonse or something for
 207             // the wrong mode, we're marking this as completed - I'm not sure
 208             // those other cases could or will ever happen in practice.
 209             // Alternatively, we could re-init handle->isotp_receive_handle if
 210             // the current one completed without a valid response to this
 211             // diagnostic request.
 212             response.completed = true;
 213             handle->completed = true;
 214
 215             if(handle->completed && handle->callback != NULL) {
 216                 handle->callback(&response);
 217             }
 218         }
 219     } else {
 220         shims->log("Mode %d request to arb ID 0x%x is already completed",
 221                 handle->request.mode, handle->request.arbitration_id);
 222     }
 223     return response;
 224 }