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