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