From 3890f5a215aa672a18e76bad0830bde067abb960 Mon Sep 17 00:00:00 2001
From: Sam Weaver <sam.weaver@atlasied.com>
Date: Tue, 24 Sep 2024 12:38:17 -0400
Subject: [PATCH] Add support for handling non-implemented services via custom
 service event.

---
 src/server.c | 27 ++++++++++++++++++++-------
 src/server.h |  7 +++++++
 src/uds.h    |  1 +
 3 files changed, 28 insertions(+), 7 deletions(-)

diff --git a/src/server.c b/src/server.c
index bafd8ac..e7fe92b 100644
--- a/src/server.c
+++ b/src/server.c
@@ -753,10 +753,7 @@ static uint8_t evaluateServiceResponse(UDSServer_t *srv, UDSReq_t *r) {
     uint8_t sid = r->recv_buf[0];
     UDSService service = getServiceForSID(sid);
 
-    if (NULL == service || NULL == srv->fn) {
-        return NegativeResponse(r, kServiceNotSupported);
-    }
-    assert(service);
+    if (NULL == srv->fn) return NegativeResponse(r, kServiceNotSupported);
     assert(srv->fn); // service handler functions will call srv->fn. it must be valid
 
     switch (sid) {
@@ -769,6 +766,7 @@ static uint8_t evaluateServiceResponse(UDSServer_t *srv, UDSReq_t *r) {
     case kSID_ROUTINE_CONTROL:
     case kSID_TESTER_PRESENT:
     case kSID_CONTROL_DTC_SETTING: {
+        assert(service);
         response = service(srv, r);
 
         bool suppressPosRspMsgIndicationBit = r->recv_buf[1] & 0x80;
@@ -794,12 +792,12 @@ static uint8_t evaluateServiceResponse(UDSServer_t *srv, UDSReq_t *r) {
     case kSID_REQUEST_UPLOAD:
     case kSID_TRANSFER_DATA:
     case kSID_REQUEST_TRANSFER_EXIT: {
+        assert(service);
         response = service(srv, r);
         break;
     }
 
-    /* CASE Service_not_implemented */
-    /* shouldn't get this far as getServiceForSID(sid) will return NULL*/
+    /* CASE Service_optional */
     case kSID_CLEAR_DIAGNOSTIC_INFORMATION:
     case kSID_READ_DTC_INFORMATION:
     case kSID_READ_SCALING_DATA_BY_IDENTIFIER:
@@ -812,7 +810,22 @@ static uint8_t evaluateServiceResponse(UDSServer_t *srv, UDSReq_t *r) {
     case kSID_SECURED_DATA_TRANSMISSION:
     case kSID_RESPONSE_ON_EVENT:
     default: {
-        response = kServiceNotSupported;
+        if (service) {
+            response = service(srv, r);
+        } else {    /* getServiceForSID(sid) returned NULL*/
+            UDSCustomArgs_t args = {
+                .sid = sid,
+                .optionRecord = &r->recv_buf[1],
+                .len = r->recv_len - 1,
+                .copyResponse = safe_copy,
+            };
+
+            r->send_buf[0] = UDS_RESPONSE_SID_OF(sid);
+            r->send_len = 1;
+
+            response = EmitEvent(srv, UDS_SRV_EVT_CUSTOM, &args);
+            if (kPositiveResponse != response) return NegativeResponse(r, response);
+        }
         break;
     }
     }
diff --git a/src/server.h b/src/server.h
index e1e57da..bff866e 100644
--- a/src/server.h
+++ b/src/server.h
@@ -162,5 +162,12 @@ typedef struct {
                             uint16_t len); /*! function for copying response data (optional) */
 } UDSRequestTransferExitArgs_t;
 
+typedef struct {
+    const uint16_t sid;          /*! serviceIdentifier */
+    const uint8_t *optionRecord; /*! optional data */
+    const uint16_t len;          /*! length of optional data */
+    uint8_t (*copyResponse)(UDSServer_t *srv, const void *src, uint16_t len); /*! function for copying response data (optional) */
+} UDSCustomArgs_t;
+
 UDSErr_t UDSServerInit(UDSServer_t *srv);
 void UDSServerPoll(UDSServer_t *srv);
diff --git a/src/uds.h b/src/uds.h
index 269b6b2..141c74e 100644
--- a/src/uds.h
+++ b/src/uds.h
@@ -16,6 +16,7 @@ enum UDSServerEvent {
     UDS_SRV_EVT_RequestTransferExit,  // UDSRequestTransferExitArgs_t *
     UDS_SRV_EVT_SessionTimeout,       // NULL
     UDS_SRV_EVT_DoScheduledReset,     // enum UDSEcuResetType *
+    UDS_SRV_EVT_CUSTOM,               // UDSCustomArgs_t *
     UDS_SRV_EVT_Err,                  // UDSErr_t *
     UDS_EVT_IDLE,
     UDS_EVT_RESP_RECV,