+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;
+}
+