X-Git-Url: https://gerrit.automotivelinux.org/gerrit/gitweb?a=blobdiff_plain;f=src%2Fobd2%2Fobd2.c;h=5b282f16314bb9ed5bc0779067f0199b414913d2;hb=648c2f77afd1dadce1c01de3f1dc42b54da656d0;hp=3d501d6aad4389793ba52f44c9021b0d1a72a11a;hpb=84bc68873aa53e295853b461b6793b5019d1dacb;p=apps%2Flow-level-can-service.git diff --git a/src/obd2/obd2.c b/src/obd2/obd2.c index 3d501d6..5b282f1 100644 --- a/src/obd2/obd2.c +++ b/src/obd2/obd2.c @@ -1,87 +1,243 @@ #include +#include +#include +#include +#include +#include + +#define ARBITRATION_ID_OFFSET 0x8 +#define MODE_RESPONSE_OFFSET 0x40 +#define NEGATIVE_RESPONSE_MODE 0x7f +#define MAX_DIAGNOSTIC_PAYLOAD_SIZE 6 +#define MODE_BYTE_INDEX 0 +#define PID_BYTE_INDEX 1 +#define NEGATIVE_RESPONSE_MODE_INDEX 1 +#define NEGATIVE_RESPONSE_NRC_INDEX 2 + +#ifndef MAX +#define MAX(x, y) (((x) > (y)) ? (x) : (y)) +#endif DiagnosticShims diagnostic_init_shims(LogShim log, SendCanMessageShim send_can_message, SetTimerShim set_timer) { DiagnosticShims shims = { - isotp_shims: { - log: log, - send_can_message: send_can_message, - set_timer: set_timer - }, - log: log + log: log, + send_can_message: send_can_message, + set_timer: set_timer }; return shims; } +static void setup_receive_handle(DiagnosticRequestHandle* handle) { + if(handle->request.arbitration_id == OBD2_FUNCTIONAL_BROADCAST_ID) { + uint16_t response_id; + for(response_id = 0; + response_id < OBD2_FUNCTIONAL_RESPONSE_COUNT; ++response_id) { + handle->isotp_receive_handles[response_id] = isotp_receive( + &handle->isotp_shims, OBD2_FUNCTIONAL_RESPONSE_START + response_id, + NULL); + } + handle->isotp_receive_handle_count = OBD2_FUNCTIONAL_RESPONSE_COUNT; + } else { + handle->isotp_receive_handle_count = 1; + handle->isotp_receive_handles[0] = isotp_receive(&handle->isotp_shims, + handle->request.arbitration_id + ARBITRATION_ID_OFFSET, + NULL); + } +} + + DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims, DiagnosticRequest* request, DiagnosticResponseReceived callback) { - // TODO hmm, where is message_received coming from? we would need 2 layers - // of callbacks. if we do it in the obd2 library, we have to have some - // context passed to that message_received handler so we can then retreive - // the obd2 callback. there was an option question of if we should pass a - // context with that callback, and maybe this answers it. - // - // alternatively, what if don't hide isotp and allow that to also be - // injected. the user has the iso_tp message_received callback, and in that - // they call a message_received handler from obd2. - // - // in fact that makes more sense - all the diagnostic_can_frame_received - // function is going to be able to do is call the equivalent function in the - // isotp library. it may or may not have a complete ISO-TP message. huh. DiagnosticRequestHandle handle = { - // TODO why are teh shims stored as a reference in the isotp handler? - // it's just 3 pointers - isotp_handler: isotp_init(&shims->isotp_shims, request->arbitration_id, - NULL, // TODO need a callback here! - NULL, NULL), - type: 0 //DIAGNOSTIC_.... // TODO how would we know the type? - //does it matter? we were going to have a different callback + request: *request, + callback: callback, + success: false, + completed: false }; + + uint8_t payload[MAX_DIAGNOSTIC_PAYLOAD_SIZE] = {0}; + payload[MODE_BYTE_INDEX] = request->mode; + if(request->pid_length > 0) { + set_bitfield(request->pid, PID_BYTE_INDEX * CHAR_BIT, + request->pid_length * CHAR_BIT, payload, sizeof(payload)); + } + if(request->payload_length > 0) { + memcpy(&payload[PID_BYTE_INDEX + request->pid_length], + request->payload, request->payload_length); + } + + handle.isotp_shims = isotp_init_shims(shims->log, + shims->send_can_message, + shims->set_timer); + + handle.isotp_send_handle = isotp_send(&handle.isotp_shims, + request->arbitration_id, payload, + 1 + request->payload_length + request->pid_length, + NULL); + if(shims->log != NULL) { + 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", + request->arbitration_id, + request->mode, + request->pid, + request->payload[0], + request->payload[1], + request->payload[2], + request->payload[3], + request->payload[4], + request->payload[5], + request->payload[6], + request->payload_length); + } + + setup_receive_handle(&handle); + + // TODO notes on multi frame: + // TODO what are the timers for exactly? + // + // when sending multi frame, send 1 frame, wait for a response + // if it says send all, send all right away + // if it says flow control, set the time for the next send + // instead of creating a timer with an async callback, add a process_handle + // function that's called repeatedly in the main loop - if it's time to + // send, we do it. so there's a process_handle_send and receive_can_frame + // that are just called continuously from the main loop. it's a waste of a + // few cpu cycles but it may be more natural than callbacks. + // + // what woudl a timer callback look like...it would need to pass the handle + // and that's all. seems like a context void* would be able to capture all + // of the information but arg, memory allocation. look at how it's done in + // the other library again + // + return handle; } DiagnosticRequestHandle diagnostic_request_pid(DiagnosticShims* shims, - DiagnosticPidRequestType pid_request_type, uint16_t pid, - DiagnosticResponseReceived callback) { - // decide mode 0x1 / 0x22 based on pid type + DiagnosticPidRequestType pid_request_type, uint16_t arbitration_id, + uint16_t pid, DiagnosticResponseReceived callback) { DiagnosticRequest request = { + arbitration_id: arbitration_id, mode: pid_request_type == DIAGNOSTIC_STANDARD_PID ? 0x1 : 0x22, - pid: pid + pid: pid, + pid_length: pid_request_type == DIAGNOSTIC_STANDARD_PID ? 1 : 2 }; return diagnostic_request(shims, &request, callback); } -void diagnostic_receive_can_frame(DiagnosticRequestHandle* handler, - const uint16_t arbitration_id, const uint8_t data[], - const uint8_t size) { - isotp_receive_can_frame(handler->isotp_handler, arbitration_id, data, size); -} +static bool handle_negative_response(IsoTpMessage* message, + DiagnosticResponse* response, DiagnosticShims* shims) { + bool response_was_negative = false; + if(response->mode == NEGATIVE_RESPONSE_MODE) { + response_was_negative = true; + if(message->size > NEGATIVE_RESPONSE_MODE_INDEX) { + response->mode = message->payload[NEGATIVE_RESPONSE_MODE_INDEX]; + } -// TODO everything below here is for future work...not critical for now. + if(message->size > NEGATIVE_RESPONSE_NRC_INDEX) { + response->negative_response_code = message->payload[NEGATIVE_RESPONSE_NRC_INDEX]; + } -DiagnosticRequestHandle diagnostic_request_malfunction_indicator_status( - DiagnosticShims* shims, - DiagnosticMilStatusReceived callback) { - // TODO request malfunction indicator light (MIL) status - request mode 1 - // pid 1, parse first bit + response->success = false; + response->completed = true; + } + return response_was_negative; } -DiagnosticRequestHandle diagnostic_request_vin(DiagnosticShims* shims, - DiagnosticVinReceived callback) { -} +static bool handle_positive_response(DiagnosticRequestHandle* handle, + IsoTpMessage* message, DiagnosticResponse* response, + DiagnosticShims* shims) { + bool response_was_positive = false; + if(response->mode == handle->request.mode + MODE_RESPONSE_OFFSET) { + response_was_positive = true; + // hide the "response" version of the mode from the user + // if it matched + response->mode = handle->request.mode; + bool has_pid = false; + if(handle->request.pid_length > 0 && message->size > 1) { + has_pid = true; + if(handle->request.pid_length == 2) { + response->pid = get_bitfield(message->payload, message->size, + PID_BYTE_INDEX * CHAR_BIT, sizeof(uint16_t) * CHAR_BIT); + } else { + response->pid = message->payload[PID_BYTE_INDEX]; + } -DiagnosticRequestHandle diagnostic_request_dtc(DiagnosticShims* shims, - DiagnosticTroubleCodeType dtc_type, - DiagnosticTroubleCodesReceived callback) { -} + } -bool diagnostic_clear_dtc(DiagnosticShims* shims) { + uint8_t payload_index = 1 + handle->request.pid_length; + response->payload_length = MAX(0, message->size - payload_index); + if(response->payload_length > 0) { + memcpy(response->payload, &message->payload[payload_index], + response->payload_length); + } + + if((handle->request.pid_length == 0 && !has_pid) + || response->pid == handle->request.pid) { + response->success = true; + response->completed = true; + } else { + response_was_positive = false; + } + } + return response_was_positive; } -DiagnosticRequestHandle diagnostic_enumerate_pids(DiagnosticShims* shims, - DiagnosticRequest* request, DiagnosticPidEnumerationReceived callback) { - // before calling the callback, split up the received bytes into 1 or 2 byte - // chunks depending on the mode so the final pid list is actual 1 or 2 byte PIDs - // TODO request supported PIDs - request PID 0 and parse 4 bytes in response +DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims, + DiagnosticRequestHandle* handle, const uint16_t arbitration_id, + const uint8_t data[], const uint8_t size) { + + DiagnosticResponse response = { + arbitration_id: arbitration_id, + success: false, + completed: false + }; + + if(!handle->isotp_send_handle.completed) { + isotp_continue_send(&handle->isotp_shims, + &handle->isotp_send_handle, arbitration_id, data, size); + } else { + uint8_t i; + for(i = 0; i < handle->isotp_receive_handle_count; ++i) { + IsoTpMessage message = isotp_continue_receive(&handle->isotp_shims, + &handle->isotp_receive_handles[i], arbitration_id, data, size); + + // TODO as of now we're completing the handle as soon as one + // broadcast response is received....need to hang on for 100ms + if(message.completed) { + if(message.size > 0) { + response.mode = message.payload[0]; + if(handle_negative_response(&message, &response, shims)) { + shims->log("Received a negative response to mode %d on arb ID 0x%x", + response.mode, response.arbitration_id); + handle->success = true; + handle->completed = true; + } else if(handle_positive_response(handle, &message, &response, + shims)) { + shims->log("Received a positive mode %d response on arb ID 0x%x", + response.mode, response.arbitration_id); + handle->success = true; + handle->completed = true; + } else { + shims->log("Response was for a mode 0x%x request (pid 0x%x), not our mode 0x%x request (pid 0x%x)", + MAX(0, response.mode - MODE_RESPONSE_OFFSET), + response.pid, handle->request.mode, + handle->request.pid); + // TODO just leave handles open until the user decides + // to be done with it - keep a count of valid responses + // received. + } + } else { + shims->log("Received an empty response on arb ID 0x%x", + response.arbitration_id); + } + + if(handle->completed && handle->callback != NULL) { + handle->callback(&response); + } + } + } + } + return response; }