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