X-Git-Url: https://gerrit.automotivelinux.org/gerrit/gitweb?a=blobdiff_plain;ds=sidebyside;f=src%2Fobd2%2Fobd2.c;h=c780fd8c21dfec3ccb2b09f91a2f59724d678c4c;hb=b2705b3ec209311506af2034e021285daf3c9649;hp=d3c0fb9e459bf55fb063d8de61ce24e79676aac6;hpb=26f5b1e5648c437e22cfcb7c87c22b40424ef198;p=apps%2Flow-level-can-service.git diff --git a/src/obd2/obd2.c b/src/obd2/obd2.c index d3c0fb9..c780fd8 100644 --- a/src/obd2/obd2.c +++ b/src/obd2/obd2.c @@ -1,6 +1,11 @@ #include -#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 @@ -9,6 +14,10 @@ #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) { @@ -23,18 +32,17 @@ DiagnosticShims diagnostic_init_shims(LogShim log, DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims, DiagnosticRequest* request, DiagnosticResponseReceived callback) { DiagnosticRequestHandle handle = { - // TODO can we copy the request? request: *request, callback: callback, success: false, completed: false }; - uint8_t payload[MAX_DIAGNOSTIC_PAYLOAD_SIZE]; + uint8_t payload[MAX_DIAGNOSTIC_PAYLOAD_SIZE] = {0}; payload[MODE_BYTE_INDEX] = request->mode; if(request->pid_length > 0) { - copy_bytes_right_aligned(&request->pid, sizeof(request->pid), - PID_BYTE_INDEX, request->pid_length, payload, sizeof(payload)); + 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], @@ -44,57 +52,33 @@ DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims, 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%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[7], + request->payload_length); + } handle.isotp_receive_handle = isotp_receive(&handle.isotp_shims, - // TODO need to either always add 0x8 or let the user specify - request->arbitration_id + 0x8, + request->arbitration_id + ARBITRATION_ID_OFFSET, NULL); - // when a can frame is received and passes to the diagnostic handle - // if we haven't successfuly sent the entire message yet, give it to the - // isottp send handle - // if we have sent it, give it to the isotp rx handle - // if we've received properly, mark this request as completed - // how do you get the result? we have it set up for callbacks but that's - // getting to be kind of awkward - // - // say it were to call a callback...what state would it need to pass? - // - // the iso-tp message received callback needs to pass the handle and the - // received message - // - // so in the obd2 library, we get a callback with an isotp message. how do - // we know what diag request i went with, and which diag callback to use? we - // could store context with the isotp handle. the problem is that context is - // self referential and on the stack, so we really can't get a pointer to - // it. - // - // the diagnostic response received callback needs to pass the diagnostic - // handle and the diag response - // - // let's say we simplify things and drop the callbacks. - // - // isotp_receive_can_frame returns an isotp handle with a complete message - // in it if one was received - // - // diagnostic_receive_can_frame returns a diaghandle if one was received - // - // so in the user code you would throw the can frame at each of your active - // diag handles and see if any return a completed message. - // - // is there any advantage to a callback approach? callbacks are useful when - // you have something that will block, bt we don't have anything that will - // block. it's async but we return immediately from each partial parsing - // attempt. - // - // argh, but will we need the callbacks when we add timers for isotp multi - // frame? - // - // what are the timers for exactly? + // 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 @@ -126,6 +110,59 @@ DiagnosticRequestHandle diagnostic_request_pid(DiagnosticShims* shims, return diagnostic_request(shims, &request, callback); } +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]; + } + + if(message->size > NEGATIVE_RESPONSE_NRC_INDEX) { + response->negative_response_code = message->payload[NEGATIVE_RESPONSE_NRC_INDEX]; + } + + response->success = false; + response->completed = true; + } + return response_was_negative; +} + +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; + if(handle->request.pid_length > 0 && message->size > 1) { + if(handle->request.pid_length == 2) { + response->pid = *(uint16_t*)&message->payload[PID_BYTE_INDEX]; + if(BYTE_ORDER == LITTLE_ENDIAN) { + response->pid = __builtin_bswap32(response->pid << 16); + } + } else { + response->pid = message->payload[PID_BYTE_INDEX]; + } + // TODO we're not currently throwing an error or anything if the PID + // doesn't match - it may be OK to leave that up to the user. + } + + 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); + } + response->success = true; + response->completed = true; + } + return response_was_positive; +} + DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims, DiagnosticRequestHandle* handle, const uint16_t arbitration_id, const uint8_t data[], const uint8_t size) { @@ -137,8 +174,6 @@ DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims, }; if(!handle->isotp_send_handle.completed) { - // TODO when completing a send, this returns...a Message? we have to - // check when the isotp_send_handle is completed, and if it is, start isotp_continue_send(&handle->isotp_shims, &handle->isotp_send_handle, arbitration_id, data, size); } else if(!handle->isotp_receive_handle.completed) { @@ -148,90 +183,42 @@ DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims, if(message.completed) { if(message.size > 0) { response.mode = message.payload[0]; - if(response.mode == NEGATIVE_RESPONSE_MODE) { - if(message.size > NEGATIVE_RESPONSE_MODE_INDEX) { - // TODO we're setting the mode to the originating - // request for the error, so the user can confirm - i - // think this is OK since we're storing the failure - // status elsewhere, but think about it. - response.mode = message.payload[NEGATIVE_RESPONSE_MODE_INDEX]; - } - - if(message.size > NEGATIVE_RESPONSE_NRC_INDEX) { - response.negative_response_code = message.payload[NEGATIVE_RESPONSE_NRC_INDEX]; - } - response.success = false; + 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 { - if(response.mode == handle->request.mode + MODE_RESPONSE_OFFSET) { - // hide the "response" version of the mode from the user - // if it matched - response.mode = handle->request.mode; - if(handle->request.pid_length > 0 && message.size > 1) { - if(handle->request.pid_length == 2) { - response.pid = *(uint16_t*)&message.payload[PID_BYTE_INDEX]; - response.pid = ntohs(response.pid); - } else { - response.pid = message.payload[PID_BYTE_INDEX]; - } - } - - uint8_t payload_index = 1 + handle->request.pid_length; - response.payload_length = message.size - payload_index; - if(response.payload_length > 0) { - memcpy(response.payload, &message.payload[payload_index], - response.payload_length); - } - response.success = true; - } else { - shims->log("Response was for a mode 0x%x request, not our mode 0x%x request", - response.mode - MODE_RESPONSE_OFFSET, - handle->request.mode); - } + shims->log("Response was for a mode 0x%x request, not our mode 0x%x request", + response.mode - MODE_RESPONSE_OFFSET, + handle->request.mode); } + } else { + shims->log("Received an empty response on arb ID 0x%x", + response.arbitration_id); } - + // TODO For now even if we got an empty repsonse or something for + // the wrong mode, we're marking this as completed - I'm not sure + // those other cases could or will ever happen in practice. + // Alternatively, we could re-init handle->isotp_receive_handle if + // the current one completed without a valid response to this + // diagnostic request. response.completed = true; - // TODO what does it mean for the handle to be successful, vs. the - // request to be successful? if we get a NRC, is that a successful - // request? - handle->success = true; handle->completed = true; - if(handle->callback != NULL) { + if(handle->completed && handle->callback != NULL) { handle->callback(&response); } } - } else { - shims->log("Handle is already completed"); + shims->log("Mode %d request to arb ID 0x%x is already completed", + handle->request.mode, handle->request.arbitration_id); } return response; } - -// TODO everything below here is for future work...not critical for now. - -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 -} - -DiagnosticRequestHandle diagnostic_request_vin(DiagnosticShims* shims, - DiagnosticVinReceived callback) { -} - -DiagnosticRequestHandle diagnostic_request_dtc(DiagnosticShims* shims, - DiagnosticTroubleCodeType dtc_type, - DiagnosticTroubleCodesReceived callback) { -} - -bool diagnostic_clear_dtc(DiagnosticShims* shims) { -} - -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 -}