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