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, 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_length);
  73     }
  74
  75     handle.isotp_receive_handle = isotp_receive(&handle.isotp_shims,
  76             request->arbitration_id + ARBITRATION_ID_OFFSET,
  77             NULL);
  78
  79     // TODO notes on multi frame:
  80     // TODO what are the timers for exactly?
  81     //
  82     // when sending multi frame, send 1 frame, wait for a response
  83     // if it says send all, send all right away
  84     // if it says flow control, set the time for the next send
  85     // instead of creating a timer with an async callback, add a process_handle
  86     // function that's called repeatedly in the main loop - if it's time to
  87     // send, we do it. so there's a process_handle_send and receive_can_frame
  88     // that are just called continuously from the main loop. it's a waste of a
  89     // few cpu cycles but it may be more  natural than callbacks.
  90     //
  91     // what woudl a timer callback look like...it would need to pass the handle
  92     // and that's all. seems like a context void* would be able to capture all
  93     // of the information but arg, memory allocation. look at how it's done in
  94     // the other library again
  95     //
  96     return handle;
  97 }
  98
  99 DiagnosticRequestHandle diagnostic_request_pid(DiagnosticShims* shims,
 100         DiagnosticPidRequestType pid_request_type, uint16_t arbitration_id,
 101         uint16_t pid, DiagnosticResponseReceived callback) {
 102     DiagnosticRequest request = {
 103         arbitration_id: arbitration_id,
 104         mode: pid_request_type == DIAGNOSTIC_STANDARD_PID ? 0x1 : 0x22,
 105         pid: pid,
 106         pid_length: pid_request_type == DIAGNOSTIC_STANDARD_PID ? 1 : 2
 107     };
 108
 109     return diagnostic_request(shims, &request, callback);
 110 }
 111
 112 static bool handle_negative_response(IsoTpMessage* message,
 113         DiagnosticResponse* response, DiagnosticShims* shims) {
 114     bool response_was_negative = false;
 115     if(response->mode == NEGATIVE_RESPONSE_MODE) {
 116         response_was_negative = true;
 117         if(message->size > NEGATIVE_RESPONSE_MODE_INDEX) {
 118             response->mode = message->payload[NEGATIVE_RESPONSE_MODE_INDEX];
 119         }
 120
 121         if(message->size > NEGATIVE_RESPONSE_NRC_INDEX) {
 122             response->negative_response_code = message->payload[NEGATIVE_RESPONSE_NRC_INDEX];
 123         }
 124
 125         response->success = false;
 126         response->completed = true;
 127     }
 128     return response_was_negative;
 129 }
 130
 131 static bool handle_positive_response(DiagnosticRequestHandle* handle,
 132         IsoTpMessage* message, DiagnosticResponse* response,
 133         DiagnosticShims* shims) {
 134     bool response_was_positive = false;
 135     if(response->mode == handle->request.mode + MODE_RESPONSE_OFFSET) {
 136         response_was_positive = true;
 137         // hide the "response" version of the mode from the user
 138         // if it matched
 139         response->mode = handle->request.mode;
 140         if(handle->request.pid_length > 0 && message->size > 1) {
 141             if(handle->request.pid_length == 2) {
 142                 response->pid = get_bitfield(message->payload, message->size,
 143                         PID_BYTE_INDEX * CHAR_BIT, sizeof(uint16_t) * CHAR_BIT);
 144             } else {
 145                 response->pid = message->payload[PID_BYTE_INDEX];
 146             }
 147             // TODO we're not currently throwing an error or anything if the PID
 148             // doesn't match - it may be OK to leave that up to the user.
 149         }
 150
 151         uint8_t payload_index = 1 + handle->request.pid_length;
 152         response->payload_length = MAX(0, message->size - payload_index);
 153         if(response->payload_length > 0) {
 154             memcpy(response->payload, &message->payload[payload_index],
 155                     response->payload_length);
 156         }
 157         response->success = true;
 158         response->completed = true;
 159     }
 160     return response_was_positive;
 161 }
 162
 163 DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
 164         DiagnosticRequestHandle* handle, const uint16_t arbitration_id,
 165         const uint8_t data[], const uint8_t size) {
 166
 167     DiagnosticResponse response = {
 168         arbitration_id: arbitration_id,
 169         success: false,
 170         completed: false
 171     };
 172
 173     if(!handle->isotp_send_handle.completed) {
 174         isotp_continue_send(&handle->isotp_shims,
 175                 &handle->isotp_send_handle, arbitration_id, data, size);
 176     } else if(!handle->isotp_receive_handle.completed) {
 177         IsoTpMessage message = isotp_continue_receive(&handle->isotp_shims,
 178                 &handle->isotp_receive_handle, arbitration_id, data, size);
 179
 180         if(message.completed) {
 181             if(message.size > 0) {
 182                 response.mode = message.payload[0];
 183                 if(handle_negative_response(&message, &response, shims)) {
 184                     shims->log("Received a negative response to mode %d on arb ID 0x%x",
 185                             response.mode, response.arbitration_id);
 186                     handle->success = true;
 187                     handle->completed = true;
 188                 } else if(handle_positive_response(handle, &message, &response,
 189                             shims)) {
 190                     shims->log("Received a positive mode %d response on arb ID 0x%x",
 191                             response.mode, response.arbitration_id);
 192                     handle->success = true;
 193                     handle->completed = true;
 194                 } else {
 195                     shims->log("Response was for a mode 0x%x request, not our mode 0x%x request",
 196                             response.mode - MODE_RESPONSE_OFFSET,
 197                             handle->request.mode);
 198                 }
 199             } else {
 200                 shims->log("Received an empty response on arb ID 0x%x",
 201                         response.arbitration_id);
 202             }
 203             // TODO For now even if we got an empty repsonse or something for
 204             // the wrong mode, we're marking this as completed - I'm not sure
 205             // those other cases could or will ever happen in practice.
 206             // Alternatively, we could re-init handle->isotp_receive_handle if
 207             // the current one completed without a valid response to this
 208             // diagnostic request.
 209             response.completed = true;
 210             handle->completed = true;
 211
 212             if(handle->completed && handle->callback != NULL) {
 213                 handle->callback(&response);
 214             }
 215         }
 216     } else {
 217         shims->log("Mode %d request to arb ID 0x%x is already completed",
 218                 handle->request.mode, handle->request.arbitration_id);
 219     }
 220     return response;
 221 }