From abb4dbf2f97ac2229c03bec6a8e27d09f098b3c5 Mon Sep 17 00:00:00 2001 From: Nick James Kirkby <20824939+driftregion@users.noreply.github.com> Date: Sat, 9 Dec 2023 20:11:48 -0700 Subject: [PATCH] restructure source directory --- .gitignore | 5 +- .gitmodules | 2 +- BUILD | 65 +- README.md | 58 +- README_zh.md | 146 -- examples/BUILD | 0 examples/arduino_server/README.md | 7 + examples/arduino_server/main/iso14229.c | 1 + examples/arduino_server/main/iso14229.h | 1 + examples/arduino_server/main/main.ino | 107 + examples/client_multiserver/BUILD | 15 + examples/client_multiserver/main.c | 59 + examples/client_multiserver/server1.c | 1 + examples/client_multiserver/server2.c | 0 examples/esp32_server/CMakeLists.txt | 6 + examples/esp32_server/README.md | 9 + examples/esp32_server/main/CMakeLists.txt | 6 + examples/esp32_server/main/iso14229.c | 1 + examples/esp32_server/main/iso14229.h | 1 + examples/esp32_server/main/main.c | 113 + examples/s32k144/BUILD | 5 +- examples/server_minimal/BUILD | 10 + examples/server_minimal/main.c | 70 + iso14229.c | 1956 ----------------- iso14229.h | 716 ------ run_gdb.py | 53 + src/client.c | 856 ++++++++ src/client.h | 144 ++ src/config.h | 66 + .../iso14229serverbufferedwriter.h | 0 src/server.c | 944 ++++++++ src/server.h | 169 ++ src/sys.h | 30 + src/sys_arduino.h | 16 + src/sys_esp32.h | 14 + src/sys_unix.h | 12 + src/sys_win32.h | 8 + src/tp.c | 64 + src/tp.h | 128 ++ src/tp/README.md | 66 + {tp => src/tp}/isotp-c | 0 {tp => src/tp}/isotp_c.c | 26 +- {tp => src/tp}/isotp_c.h | 19 +- {tp => src/tp}/isotp_c_socketcan.c | 34 +- {tp => src/tp}/isotp_c_socketcan.h | 7 +- {tp => src/tp}/isotp_sock.c | 35 +- {tp => src/tp}/isotp_sock.h | 4 +- {tp => src/tp}/mock.c | 26 +- {tp => src/tp}/mock.h | 14 +- src/uds.h | 223 ++ src/util.c | 30 + src/util.h | 42 + test/BUILD | 16 +- test/env.c | 3 - test/test.h | 1 - test/test_fuzz_server.c | 4 +- test_iso14229.c | 875 -------- tp/BUILD | 63 - 58 files changed, 3445 insertions(+), 3907 deletions(-) create mode 100644 examples/BUILD create mode 100644 examples/arduino_server/README.md create mode 120000 examples/arduino_server/main/iso14229.c create mode 120000 examples/arduino_server/main/iso14229.h create mode 100644 examples/arduino_server/main/main.ino create mode 100644 examples/client_multiserver/BUILD create mode 100644 examples/client_multiserver/main.c create mode 100644 examples/client_multiserver/server1.c create mode 100644 examples/client_multiserver/server2.c create mode 100644 examples/esp32_server/CMakeLists.txt create mode 100644 examples/esp32_server/README.md create mode 100644 examples/esp32_server/main/CMakeLists.txt create mode 120000 examples/esp32_server/main/iso14229.c create mode 120000 examples/esp32_server/main/iso14229.h create mode 100644 examples/esp32_server/main/main.c create mode 100644 examples/server_minimal/BUILD create mode 100644 examples/server_minimal/main.c delete mode 100644 iso14229.c delete mode 100644 iso14229.h create mode 100755 run_gdb.py create mode 100644 src/client.c create mode 100644 src/client.h create mode 100644 src/config.h rename iso14229serverbufferedwriter.h => src/iso14229serverbufferedwriter.h (100%) create mode 100644 src/server.c create mode 100644 src/server.h create mode 100644 src/sys.h create mode 100644 src/sys_arduino.h create mode 100644 src/sys_esp32.h create mode 100644 src/sys_unix.h create mode 100644 src/sys_win32.h create mode 100644 src/tp.c create mode 100644 src/tp.h create mode 100644 src/tp/README.md rename {tp => src/tp}/isotp-c (100%) rename {tp => src/tp}/isotp_c.c (91%) rename {tp => src/tp}/isotp_c.h (76%) rename {tp => src/tp}/isotp_c_socketcan.c (87%) rename {tp => src/tp}/isotp_c_socketcan.h (90%) rename {tp => src/tp}/isotp_sock.c (88%) rename {tp => src/tp}/isotp_sock.h (93%) rename {tp => src/tp}/mock.c (89%) rename {tp => src/tp}/mock.h (94%) create mode 100644 src/uds.h create mode 100644 src/util.c create mode 100644 src/util.h delete mode 100644 test_iso14229.c delete mode 100644 tp/BUILD diff --git a/.gitignore b/.gitignore index 2b53068..b0a5de7 100644 --- a/.gitignore +++ b/.gitignore @@ -9,4 +9,7 @@ corpus .gdb_history fuzzer *.o -.vscode \ No newline at end of file +.vscode +**/*.cache +**/build +**/sdkconfig* diff --git a/.gitmodules b/.gitmodules index 0a9e643..d7b4425 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ [submodule "tp/isotp-c"] - path = tp/isotp-c + path = src/tp/isotp-c url = https://github.com/driftregion/isotp-c.git diff --git a/BUILD b/BUILD index d4db888..869aba6 100644 --- a/BUILD +++ b/BUILD @@ -1,37 +1,52 @@ package(default_visibility = ["//visibility:public"]) load("@hedron_compile_commands//:refresh_compile_commands.bzl", "refresh_compile_commands") +genrule( + name = "isotp_c_wrapped_c", + srcs = glob(["src/tp/isotp-c/*.c"]), + outs = ["isotp_c_wrapped.c"], + cmd = "echo '#if defined(UDS_TP) && UDS_TP==UDS_TP_ISOTP_C' >> $(OUTS) ; for f in $(SRCS); do cat $$f >> $(OUTS); done ; echo '#endif' >> $(OUTS)", +) + +genrule( + name = "isotp_c_wrapped_h", + srcs = glob(["src/tp/isotp-c/*.h"]), + outs = ["isotp_c_wrapped.h"], + cmd = "echo '#if defined(UDS_TP) && UDS_TP==UDS_TP_ISOTP_C' >> $(OUTS) ; for f in $(SRCS); do cat $$f >> $(OUTS); done ; echo '#endif' >> $(OUTS)", +) + +genrule( + name = "c_src", + srcs = glob(["src/*.c", "src/tp/*.c"]) + [ + ":isotp_c_wrapped_c", + ], + outs = ["iso14229.c"], + cmd = "(echo '#include \"iso14229.h\"'; (for f in $(SRCS); do echo; echo '#ifdef UDS_LINES'; echo \"#line 1 \\\"$$f\"\\\"; echo '#endif'; cat $$f | sed -e 's,#include \".*,,'; done)) > $(OUTS)", +) + +genrule( + name = "h_src", + srcs = glob(["src/*.h", "src/tp/*.h"]) + [ + ":isotp_c_wrapped_h", + ], + outs = ["iso14229.h"], + cmd = "echo $(SRCS); (cat ; echo '#ifndef ISO14229_H'; echo '#define ISO14229_H'; echo; echo '#ifdef __cplusplus'; echo 'extern \"C\" {'; echo '#endif'; cat src/sys.h src/sys_arduino.h src/sys_unix.h src/sys_win32.h src/sys_esp32.h src/config.h src/util.h src/tp.h src/uds.h src/client.h src/server.h $(location //:isotp_c_wrapped_h) src/tp/*.h |sed -e 's,#include \".*,,' -e 's,^#pragma once,,' ; echo '#endif'; echo '#ifdef __cplusplus'; echo '}'; echo '#endif';) > $(OUTS)", +) filegroup( - name = "iso14229_srcs", + name = "srcs", srcs = [ "iso14229.c", "iso14229.h", - "iso14229serverbufferedwriter.h", ], ) -cc_test( - name="test_all", - srcs=[ - ":iso14229_srcs", - "test_iso14229.c", - ], - deps = [ - "//tp:mock", - ], - copts=[ - "-Wall", - "-Wextra", - "-Wno-missing-field-initializers", - "-Werror", - "-Wno-unused-parameter", - ], - defines=[ - "UDS_TP=UDS_TP_CUSTOM", - "UDS_CUSTOM_MILLIS", +cc_library( + name="iso14229", + srcs = [ + "iso14229.c", + "iso14229.h", ], - size = "small", ) cc_test( @@ -43,9 +58,11 @@ cc_test( size = "small", ) + cc_library( - name="iso14229", - srcs=[":iso14229_srcs"], + name="iso14229_2", + srcs=glob(["src/**/*.c", "src/**/*.h"]), + copts=['-Isrc'], ) diff --git a/README.md b/README.md index 7448ec8..187ebe5 100644 --- a/README.md +++ b/README.md @@ -9,57 +9,37 @@ 简体中文

-iso14229 is a server and client session-layer implementation of (ISO14229-1:2013) targeting embedded systems. It is tested with [`isotp-c`](https://github.com/lishen2/isotp-c) as well as [linux kernel](https://github.com/linux-can/can-utils/blob/master/include/linux/can/isotp.h) ISO15765-2 (ISO-TP) transport layer implementations. +iso14229 is an implementation of UDS (ISO14229-1:2013) targeting embedded systems. It is tested with [`isotp-c`](https://github.com/lishen2/isotp-c) as well as [linux kernel](https://github.com/linux-can/can-utils/blob/master/include/linux/can/isotp.h) ISO15765-2 (ISO-TP) transport layer implementations. -API status: **stabilizing** +API status: **not yet stable**. -## quickstart: server +## Features -```c -#include "iso14229.h" - -static uint8_t fn(UDSServer_t *srv, UDSServerEvent_t ev, const void *arg) { - switch (ev) { - case UDS_SRV_EVT_EcuReset: { // 0x10 - UDSECUResetArgs_t *r = (UDSECUResetArgs_t *)arg; - printf("got ECUReset request of type %x\n", r->type); - return kPositiveResponse; - default: - return kServiceNotSupported; - } - } -} +- static memory allocation. does not use `malloc`, `calloc` +- highly portable. tested on arm, x86-64, ppc, ppc64 +- easy to integrate. Download `iso14229.c` and `iso14229.h` from the releases page and copy into your source tree. +- supports: + - linux + - Windows + - esp32 + - Arduino + - NXP s32k +- cares about security + - server has fuzz test, see [test/README.md](test/README.md) + - -int main() { - UDSServer_t server; - UDSServerConfig_t cfg = { - .fn = &fn, - }; - UDSServerInit(&server, &cfg); - for (;;) { - UDSServerPoll(&server); - } -} -``` +## Quick Start -## quickstart: client - -```c -// see examples/client.c -``` +See [examples](./examples). ## Preprocessor Defines | Define | Description | Valid values | | - | - | - | -| `UDS_ARCH` | Select a porting target | `UDS_ARCH_CUSTOM`, `UDS_ARCH_UNIX` | +| `UDS_SYS` | Select a porting target | `UDS_SYS_CUSTOM`, `UDS_SYS_UNIX` | | `UDS_TP` | Select a transport layer | `UDS_TP_ISOTP_C`, `UDS_TP_ISOTP_SOCKET` | | `UDS_CUSTOM_MILLIS` | Use your own `millis()` implementation | defined or not defined | -Features: -- all memory allocation is static -- architecture-independent. tested on arm, x86-64, ppc, ppc64. see [test_qemu.py](./test_qemu.py) -- has many existing unit-tests and tests are easy to extend ## supported functions (server and client ) @@ -362,9 +342,9 @@ MIT ## 0.7.0 - test refactoring. theme: test invariance across different transports and processor architectures - breaking API changes: - - rename `phys_send_id`, `phys_recv_id`, `func_send_id`, and `func_recv_id` to be consistent with the standard. Now mandatory for all UDSServerConfig_t and UDSClientConfig_t regardless of transport layer implementation - overhauled transport layer implementation - simplified client and server init + - `UDS_ARCH_` renamed to `UDS_SYS_` ## 0.6.0 - breaking API changes: diff --git a/README_zh.md b/README_zh.md index 4f10b80..8a0b02b 100644 --- a/README_zh.md +++ b/README_zh.md @@ -407,149 +407,3 @@ MIT - 初次发布 --- - - -# 开发者文档 - -## 客户端请求状态机 - -```plantuml -@startuml -title 客户端请求状态机 -note as N1 -enum { - kNoError=0, - kErrBadRequest, - kErrP2Timeout, -} ClientErr; - -static inline bool isRequestComplete() {return state==Idle;} - -while (Idle != client->state) { - receiveCAN(client); - UDSClientPoll(client); -} -end note - -state Idle -state Sending -state Sent -state SentAwaitResponse -state ProcessResponse -Idle: if (ISOTP_RET_OK == isotp_receive(...)) // Error -ProcessResponse: isotp_receive() -ProcessResponse: _ClientValidateResponse(...) -ProcessResponse: _ClientHandleResponse(...) - -Sending --> Sent: 传输层完成传输 - -Sent --> Idle : suppressPositiveResponse -Sending --> SentAwaitResponse: !suppressPositiveResponse -SentAwaitResponse -> Idle: 响应收到了 ||\np2 超时 -SentAwaitResponse --> ProcessResponse : ISOTP_RECEIVE_STATUS_FULL == link->receive_status -ProcessResponse --> Idle - -[*] -> Idle -Idle -> Sending : _SendRequest() - -@enduml -``` - -```plantuml -@startuml -title Request Lifecycle -alt normal - alt positive response - client --> client: Sending - client -> server : *Any* Service - client --> client: SentAwaitResponse: set p2 - alt 0x78 requestCorrectlyReceived-ResponsePending - server -> client : 0x3F 0x78 - client -->server : txLink idle - client --> client: SentAwaitResponse: set p2star - end - server -> client : Positive Service Response - client --> client: Idle - else negative response - server -> client !! : Negative Service Response - client --> client: Idle: RequestErrorNegativeResponse - else SID mismatch - server -> client !! : Mismatched Service Response - client --> client: Idle: RequestErrorResponseSIDMismatch - end -else unexpected response - server -> client !! : Unexpected Response - client --> client: Idle: RequestErrorUnsolicitedResponse -end -@enduml -``` - - -```plantuml -@startuml -' !pragma useVerticalIf on -title 客户端请求流程 -start - -:clientSendRequest(); -if (验证参数) then (对) -:ok; -else (不对) -:foo; -detach -endif - -:clearRequestContext(); -if (等待UDS访问) then (访问接收了,进入UDS会话) -else (时间超过20ms) -@enduml -``` - -## 服务器 0x78 requestCorrectlyReceived-ResponsePending - - -```plantuml -@startuml -client -> server : *Any* Service -server -> userServiceHandler: handler(args) -note right: Doing this will take a long time\nso I return 0x78 -userServiceHandler -> server: 0x78 -server -> client : 0x3F 0x78 -client -->server : txLink idle -server -> userServiceHandler: handler(args) -note right: actually call the long-running service -... p2* > t > p2 ... -userServiceHandler -> server : Service Response -server -> client : Service Response -@enduml -``` - -```plantuml -@startuml -' !pragma useVerticalIf on -title 0x78流程(写flash) -start - -:BufferedWriterWrite(BufferedWriter *self, const uint8_t *ibuf, uint32_t size, bool RCRRP); - -if (RCRRP) then (true) -:write to flash; -else (false) -endif -if (iBufIdx == size) then (true) - :write to pageBuffer; - :iBufIdx = 0; - :return kBufferedWriterWritePending; - :0x78 RCRRP; - detach; -else (false) - :memmove(pageBuffer + pageBufIdx, iBuf + iBufIdx, size - iBufIdx); - :write to pageBuffer; - :iBufIdx += size; - :0x01 PositiveResponse; - :0x78 RCRRP; - detach -endif - -@enduml -``` diff --git a/examples/BUILD b/examples/BUILD new file mode 100644 index 0000000..e69de29 diff --git a/examples/arduino_server/README.md b/examples/arduino_server/README.md new file mode 100644 index 0000000..901c6b1 --- /dev/null +++ b/examples/arduino_server/README.md @@ -0,0 +1,7 @@ +# Required Hardware + +- [Arduino MKR-WIFI 1010](https://store-usa.arduino.cc/products/arduino-mkr-wifi-1010) +- [MKR CAN Shield](https://store.arduino.cc/products/arduino-mkr-can-shield) + +Note that the labeling of CAN-H and CAN-L on silkscreen on the MKR CAN Shield is contradictory on the top and bottom of the board. The top is correct. + diff --git a/examples/arduino_server/main/iso14229.c b/examples/arduino_server/main/iso14229.c new file mode 120000 index 0000000..fa2cd67 --- /dev/null +++ b/examples/arduino_server/main/iso14229.c @@ -0,0 +1 @@ +../../../bazel-bin/iso14229.c \ No newline at end of file diff --git a/examples/arduino_server/main/iso14229.h b/examples/arduino_server/main/iso14229.h new file mode 120000 index 0000000..c026bd1 --- /dev/null +++ b/examples/arduino_server/main/iso14229.h @@ -0,0 +1 @@ +../../../bazel-bin/iso14229.h \ No newline at end of file diff --git a/examples/arduino_server/main/main.ino b/examples/arduino_server/main/main.ino new file mode 100644 index 0000000..0bc348f --- /dev/null +++ b/examples/arduino_server/main/main.ino @@ -0,0 +1,107 @@ +#include +#include "iso14229.h" +#include +#include + +UDSServer_t srv; +UDSISOTpC_t tp; + +int send_can(const uint32_t arb_id, const uint8_t *data, const uint8_t size, void *ud) { + CAN.beginPacket(arb_id); + CAN.write(data, size); + CAN.endPacket(); + return size; +} + +static void CANRecv(UDSISOTpC_t *tp) { + assert(tp); + uint8_t buf[8]; + int len = CAN.parsePacket(); + if (len) { + if (len > 8) { + Serial.println("CAN packet too long, truncating"); + len = 8; + } + CAN.readBytes(buf, len); + UDS_DBG_PRINT("can recv\n"); + UDS_DBG_PRINTHEX(buf, len); + if (CAN.packetId() == tp->phys_sa) { + UDS_DBG_PRINT("phys frame received\n"); + isotp_on_can_message(&tp->phys_link, buf, len); + } else if (CAN.packetId() == tp->func_sa) { + if (ISOTP_RECEIVE_STATUS_IDLE != tp->phys_link.receive_status) { + UDS_DBG_PRINT("func frame received but cannot process because link is not idle"); + return; + } + isotp_on_can_message(&tp->func_link, buf, len); + } + } +} + +extern "C" int print_impl(const char *fmt, ...); + +const UDSISOTpCConfig_t tp_cfg = { + .source_addr=0x7E8, + .target_addr=0x7E0, + .source_addr_func=0x7DF, + .target_addr_func=UDS_TP_NOOP_ADDR, + .isotp_user_send_can=send_can, + .isotp_user_get_ms=UDSMillis, + .isotp_user_debug=NULL, + .user_data=NULL, +}; + +int print_impl(const char *fmt, ...) { + char buf[256]; + va_list args; + va_start(args, fmt); + int ret = vsnprintf(buf, sizeof(buf), fmt, args); + Serial.print(buf); + va_end(args); + return ret; +} + +uint8_t fn(UDSServer_t *srv, int ev, const void *arg) { + Serial.print("Got event "); + Serial.println(ev); + switch(ev) { + case UDS_SRV_EVT_Err: + { + UDSErr_t *p_err = (UDSErr_t *)arg; + Serial.print("Err: "); + Serial.println(*p_err); + break; + } + } +} + +void setup() { + Serial.begin(9600); + while (!Serial); + + if(!UDSServerInit(&srv)) { + Serial.println("UDSServerInit failed"); + while(1); + } + + if (!UDSISOTpCInit(&tp, &tp_cfg)) { + Serial.println("UDSISOTpCInit failed"); + while(1); + } + + srv.tp = &tp.hdl; + srv.fn = fn; + + // start the CAN bus at 500 kbps + if (!CAN.begin(500E3)) { + Serial.println("Starting CAN failed!"); + while (1); + } + + Serial.println("Arduino UDS Server: Setup Complete"); +} + +void loop() { + UDSServerPoll(&srv); + CANRecv(&tp); +} diff --git a/examples/client_multiserver/BUILD b/examples/client_multiserver/BUILD new file mode 100644 index 0000000..4e5b9b7 --- /dev/null +++ b/examples/client_multiserver/BUILD @@ -0,0 +1,15 @@ +# cc_binary( +# name = "client_multiserver", +# srcs = [ +# "main.c", +# "server1.c", +# "server2.c", +# ], +# deps = [ +# "//:iso14229", +# "//:tp_mock", +# ], +# defines = [ +# "UDS_TP=UDS_TP_CUSTOM", +# ] +# ) \ No newline at end of file diff --git a/examples/client_multiserver/main.c b/examples/client_multiserver/main.c new file mode 100644 index 0000000..f859906 --- /dev/null +++ b/examples/client_multiserver/main.c @@ -0,0 +1,59 @@ +#include "iso14229.h" +#include "tp_mock.h" + +typedef struct { + bool Done; + int NumResponses; + int Step; +} Ctx_t; + +void fn(UDSClient_t *client, UDSEvent_t evt, void *evtdata, void *fndata) { + Ctx_t *ctx = (Ctx_t *)fndata; + switch (evt) { + case UDS_EVT_IDLE: + switch (ctx->Step) { + case 0: + client->options |= UDS_FUNCTIONAL; + uint16_t did_list[] = {0x1, 0x8}; + UDSSendRDBI(client, did_list, sizeof(did_list) / sizeof(did_list[0])); + ctx->Step++; + break; + default: + break; + } + break; + case UDS_EVT_RESP_RECV: + switch (ctx->Step) { + case 1: + ctx->NumResponses++; + // int err = UDSUnpackRDBIResponse(client, evtdata); + ctx->Step++; + break; + default: + break; + } + break; + } +} + +int main() { + UDSClient_t client; + Ctx_t ctx = {0}; + UDSServer_t server1, server2, server3; + + { + UDSClientConfig_t cfg = { + .tp = TPMockNew(&(TPMockCfg_t){.phys_recv_addr = 0x1, + .phys_send_addr = 0x2, + .func_recv_addr = 0x3, + .func_send_addr = 0x4}); + } + UDSClientInit(&client, &cfg); +} + +while (!ctx.Done) { + UDSClientPoll2(&client, fn, &ctx); + UDSServerPoll(&server1); + UDSServerPoll(&server2); +} +} \ No newline at end of file diff --git a/examples/client_multiserver/server1.c b/examples/client_multiserver/server1.c new file mode 100644 index 0000000..5a56d0d --- /dev/null +++ b/examples/client_multiserver/server1.c @@ -0,0 +1 @@ +#include "iso14229.h" diff --git a/examples/client_multiserver/server2.c b/examples/client_multiserver/server2.c new file mode 100644 index 0000000..e69de29 diff --git a/examples/esp32_server/CMakeLists.txt b/examples/esp32_server/CMakeLists.txt new file mode 100644 index 0000000..a246da0 --- /dev/null +++ b/examples/esp32_server/CMakeLists.txt @@ -0,0 +1,6 @@ +# The following lines of boilerplate have to be in your project's CMakeLists +# in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.5) + +include($ENV{IDF_PATH}/tools/cmake/project.cmake) +project(esp32_server) diff --git a/examples/esp32_server/README.md b/examples/esp32_server/README.md new file mode 100644 index 0000000..2808133 --- /dev/null +++ b/examples/esp32_server/README.md @@ -0,0 +1,9 @@ +# Required Hardware + +- [ESP32-C3-32S](https://docs.ai-thinker.com/_media/esp32/docs/esp-c3-32s-kit-v1.0_specification.pdf) +- [Waveshare SN65HVD230 CAN Board](https://www.waveshare.com/sn65hvd230-can-board.htm) + +# Setup + +1. download and install `esp-idf` +2. connect CAN board to ESP32 \ No newline at end of file diff --git a/examples/esp32_server/main/CMakeLists.txt b/examples/esp32_server/main/CMakeLists.txt new file mode 100644 index 0000000..6f7ca4c --- /dev/null +++ b/examples/esp32_server/main/CMakeLists.txt @@ -0,0 +1,6 @@ +idf_component_register( + SRCS "main.c" + "iso14229.c" + "iso14229.h" + INCLUDE_DIRS "." +) \ No newline at end of file diff --git a/examples/esp32_server/main/iso14229.c b/examples/esp32_server/main/iso14229.c new file mode 120000 index 0000000..fa2cd67 --- /dev/null +++ b/examples/esp32_server/main/iso14229.c @@ -0,0 +1 @@ +../../../bazel-bin/iso14229.c \ No newline at end of file diff --git a/examples/esp32_server/main/iso14229.h b/examples/esp32_server/main/iso14229.h new file mode 120000 index 0000000..c026bd1 --- /dev/null +++ b/examples/esp32_server/main/iso14229.h @@ -0,0 +1 @@ +../../../bazel-bin/iso14229.h \ No newline at end of file diff --git a/examples/esp32_server/main/main.c b/examples/esp32_server/main/main.c new file mode 100644 index 0000000..feb2dbd --- /dev/null +++ b/examples/esp32_server/main/main.c @@ -0,0 +1,113 @@ +#include "driver/gpio.h" +#include "iso14229.h" +#include +#include + +#define CAN_RX_PIN GPIO_NUM_7 +#define CAN_TX_PIN GPIO_NUM_6 +#define RED_LED_PIN GPIO_NUM_3 +#define GREEN_LED_PIN GPIO_NUM_4 +#define BLUE_LED_PIN GPIO_NUM_5 + +const char *TAG = "UDS"; + +static const twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS(); +static const twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); +static const twai_general_config_t g_config = {.mode = TWAI_MODE_NORMAL, + .tx_io = CAN_TX_PIN, + .rx_io = CAN_RX_PIN, + .clkout_io = TWAI_IO_UNUSED, + .bus_off_io = TWAI_IO_UNUSED, + .tx_queue_len = 50, + .rx_queue_len = 50, + .alerts_enabled = + TWAI_ALERT_RX_DATA | TWAI_ALERT_BUS_OFF, + .clkout_divider = 0, + .intr_flags = ESP_INTR_FLAG_LEVEL1}; + +static UDSServer_t srv; +static UDSISOTpC_t tp; + +static int send_can(const uint32_t arbitration_id, const uint8_t *data, const uint8_t size, + void *user_data) { + twai_message_t tx_msg; + tx_msg.identifier = arbitration_id; + tx_msg.data_length_code = size; + memmove(tx_msg.data, data, size); + if (ESP_OK == twai_transmit(&tx_msg, 0)) { + return size; + } else { + return -1; + } +} + +static const UDSISOTpCConfig_t tp_cfg = { + .source_addr=0x7E8, + .target_addr=0x7E0, + .source_addr_func=0x7DF, + .target_addr_func=UDS_TP_NOOP_ADDR, + .isotp_user_send_can=send_can, + .isotp_user_get_ms=UDSMillis, + .isotp_user_debug=NULL, + .user_data=NULL, +}; + +static uint8_t fn(UDSServer_t *srv, int evt, const void *data) { + ESP_LOGI(TAG, "received event %d", evt); + switch (evt) { + case UDS_SRV_EVT_WriteDataByIdent: { + UDSWDBIArgs_t *r = (UDSWDBIArgs_t *)data; + switch (r->dataId) { + case 0x0001: + ESP_LOGI(TAG, "received 0x0001"); + gpio_set_level(RED_LED_PIN, r->data[0] & 0x01); + gpio_set_level(GREEN_LED_PIN, r->data[0] & 0x02); + gpio_set_level(BLUE_LED_PIN, r->data[0] & 0x04); + break; + default: + ESP_LOGI(TAG, "received unknown data id 0x%04x", r->dataId); + break; + } + } + } + return 0; +} + +void app_main(void) { + ESP_ERROR_CHECK(twai_driver_install(&g_config, &t_config, &f_config)); + ESP_ERROR_CHECK(twai_start()); + + gpio_config_t io_conf; + io_conf.intr_type = GPIO_INTR_DISABLE; + io_conf.mode = GPIO_MODE_OUTPUT; + io_conf.pin_bit_mask = (1ULL << RED_LED_PIN) | (1ULL << GREEN_LED_PIN) | (1ULL << BLUE_LED_PIN); + io_conf.pull_down_en = 0; + io_conf.pull_up_en = 0; + gpio_config(&io_conf); + + + ESP_ERROR_CHECK(UDSServerInit(&srv)); + ESP_ERROR_CHECK(UDSISOTpCInit(&tp, &tp_cfg)); + srv.fn = fn; + srv.tp = &tp.hdl; + + for (;;) { + twai_message_t rx_msg; + if (twai_receive(&rx_msg, 0) == ESP_OK) { + if (rx_msg.identifier == tp.phys_sa) { + isotp_on_can_message(&tp.phys_link, rx_msg.data, rx_msg.data_length_code); + } else if (rx_msg.identifier == tp.func_sa) { + if (ISOTP_RECEIVE_STATUS_IDLE != tp.phys_link.receive_status) { + ESP_LOGI(TAG, "func frame received but cannot process because link is not idle"); + continue; + } + isotp_on_can_message(&tp.func_link, rx_msg.data, rx_msg.data_length_code); + } else { + ESP_LOGI(TAG, "received unknown can id 0x%03lx", rx_msg.identifier); + } + } + + UDSServerPoll(&srv); + } + +} \ No newline at end of file diff --git a/examples/s32k144/BUILD b/examples/s32k144/BUILD index 231b399..a407dd5 100644 --- a/examples/s32k144/BUILD +++ b/examples/s32k144/BUILD @@ -6,10 +6,7 @@ cc_binary( "main.c", "bsp.c", "bsp.h", - "//:iso14229_srcs", - "//tp:isotp_c_srcs", - "//tp:isotp_c.c", - "//tp:isotp_c.h", + "//:srcs", ], deps = [ ":runtime", diff --git a/examples/server_minimal/BUILD b/examples/server_minimal/BUILD new file mode 100644 index 0000000..4b2bb79 --- /dev/null +++ b/examples/server_minimal/BUILD @@ -0,0 +1,10 @@ +# cc_binary( +# name = "server_minimal", +# srcs = [ +# "main.c", +# "//:iso14229_srcs", +# ], +# deps = [ +# "//:tp_mock", +# ] +# ) \ No newline at end of file diff --git a/examples/server_minimal/main.c b/examples/server_minimal/main.c new file mode 100644 index 0000000..afc63a5 --- /dev/null +++ b/examples/server_minimal/main.c @@ -0,0 +1,70 @@ +#include "iso14229.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static UDSServer_t srv; +static bool done = false; + +void sigint_handler(int signum) { + printf("SIGINT received\n"); + done = true; +} + +static uint8_t fn(UDSServer_t *srv, UDSServerEvent_t ev, const void *arg) { + switch (ev) { + default: + printf("Unhandled event: %d\n", ev); + return kServiceNotSupported; + } +} + +static int SleepMillis(uint32_t tms) { + struct timespec ts; + int ret; + ts.tv_sec = tms / 1000; + ts.tv_nsec = (tms % 1000) * 1000000; + do { + ret = nanosleep(&ts, &ts); + } while (ret && errno == EINTR); + return ret; +} + +int main(int ac, char **av) { + UDSServerConfig_t cfg = { + .fn = fn, +#if UDS_TP == UDS_TP_ISOTP_SOCKET + .if_name = "vcan0", + .source_addr = 0x7E0, + .target_addr = 0x7E8, + .source_addr_func = 0x7DF, +#endif + }; + + struct sigaction sa; + memset(&sa, 0, sizeof(sa)); + sa.sa_handler = sigint_handler; + sigaction(SIGINT, &sa, NULL); + + if (UDSServerInit(&srv, &cfg)) { + exit(-1); + } + + printf("server up, polling . . .\n"); + while (!done) { + UDSServerPoll(&srv); +#if UDS_TP == UDS_TP_ISOTP_C + SocketCANRecv((UDSTpISOTpC_t *)srv.tp, cfg.source_addr); +#endif + SleepMillis(1); + } + printf("server exiting\n"); + UDSServerDeInit(&srv); + return 0; +} diff --git a/iso14229.c b/iso14229.c deleted file mode 100644 index 1bc1f5c..0000000 --- a/iso14229.c +++ /dev/null @@ -1,1956 +0,0 @@ -#include "iso14229.h" -#include -#include -#include -#include -#include -#include -#include -#include - -// ISO-14229-1:2013 Table 2 -#define UDS_MAX_DIAGNOSTIC_SERVICES 0x7F - -#define UDS_RESPONSE_SID_OF(request_sid) (request_sid + 0x40) -#define UDS_REQUEST_SID_OF(response_sid) (response_sid - 0x40) - -#define UDS_NEG_RESP_LEN 3U -#define UDS_0X10_REQ_LEN 2U -#define UDS_0X10_RESP_LEN 6U -#define UDS_0X11_REQ_MIN_LEN 2U -#define UDS_0X11_RESP_BASE_LEN 2U -#define UDS_0X23_REQ_MIN_LEN 4U -#define UDS_0X23_RESP_BASE_LEN 1U -#define UDS_0X22_RESP_BASE_LEN 1U -#define UDS_0X27_REQ_BASE_LEN 2U -#define UDS_0X27_RESP_BASE_LEN 2U -#define UDS_0X28_REQ_BASE_LEN 3U -#define UDS_0X28_RESP_LEN 2U -#define UDS_0X2E_REQ_BASE_LEN 3U -#define UDS_0X2E_REQ_MIN_LEN 4U -#define UDS_0X2E_RESP_LEN 3U -#define UDS_0X31_REQ_MIN_LEN 4U -#define UDS_0X31_RESP_MIN_LEN 4U -#define UDS_0X34_REQ_BASE_LEN 3U -#define UDS_0X34_RESP_BASE_LEN 2U -#define UDS_0X35_REQ_BASE_LEN 3U -#define UDS_0X35_RESP_BASE_LEN 2U -#define UDS_0X36_REQ_BASE_LEN 2U -#define UDS_0X36_RESP_BASE_LEN 2U -#define UDS_0X37_REQ_BASE_LEN 1U -#define UDS_0X37_RESP_BASE_LEN 1U -#define UDS_0X3E_REQ_MIN_LEN 2U -#define UDS_0X3E_REQ_MAX_LEN 2U -#define UDS_0X3E_RESP_LEN 2U -#define UDS_0X85_REQ_BASE_LEN 2U -#define UDS_0X85_RESP_LEN 2U - -enum UDSDiagnosticServiceId { - kSID_DIAGNOSTIC_SESSION_CONTROL = 0x10, - kSID_ECU_RESET = 0x11, - kSID_CLEAR_DIAGNOSTIC_INFORMATION = 0x14, - kSID_READ_DTC_INFORMATION = 0x19, - kSID_READ_DATA_BY_IDENTIFIER = 0x22, - kSID_READ_MEMORY_BY_ADDRESS = 0x23, - kSID_READ_SCALING_DATA_BY_IDENTIFIER = 0x24, - kSID_SECURITY_ACCESS = 0x27, - kSID_COMMUNICATION_CONTROL = 0x28, - kSID_READ_PERIODIC_DATA_BY_IDENTIFIER = 0x2A, - kSID_DYNAMICALLY_DEFINE_DATA_IDENTIFIER = 0x2C, - kSID_WRITE_DATA_BY_IDENTIFIER = 0x2E, - kSID_INPUT_CONTROL_BY_IDENTIFIER = 0x2F, - kSID_ROUTINE_CONTROL = 0x31, - kSID_REQUEST_DOWNLOAD = 0x34, - kSID_REQUEST_UPLOAD = 0x35, - kSID_TRANSFER_DATA = 0x36, - kSID_REQUEST_TRANSFER_EXIT = 0x37, - kSID_REQUEST_FILE_TRANSFER = 0x38, - kSID_WRITE_MEMORY_BY_ADDRESS = 0x3D, - kSID_TESTER_PRESENT = 0x3E, - kSID_ACCESS_TIMING_PARAMETER = 0x83, - kSID_SECURED_DATA_TRANSMISSION = 0x84, - kSID_CONTROL_DTC_SETTING = 0x85, - kSID_RESPONSE_ON_EVENT = 0x86, -}; - -// ======================================================================== -// Transports -// ======================================================================== - -/** - * @brief - * - * @param hdl - * @param info, if NULL, the default values are used: - * A_Mtype: message type (diagnostic (DEFAULT), remote diagnostic, secure diagnostic, secure - * remote diagnostic) - * A_TA_Type: application target address type (physical (DEFAULT) or functional) - * A_SA: unused - * A_TA: unused - * A_AE: unused - * @return ssize_t - */ -ssize_t UDSTpGetSendBuf(struct UDSTpHandle *hdl, uint8_t **buf) { - assert(hdl); - assert(hdl->get_send_buf); - return hdl->get_send_buf(hdl, buf); -} -ssize_t UDSTpSend(struct UDSTpHandle *hdl, const uint8_t *buf, ssize_t len, UDSSDU_t *info) { - assert(hdl); - assert(hdl->send); - return hdl->send(hdl, (uint8_t *)buf, len, info); -} - -UDSTpStatus_t UDSTpPoll(struct UDSTpHandle *hdl) { - assert(hdl); - assert(hdl->poll); - return hdl->poll(hdl); -} - -ssize_t UDSTpPeek(struct UDSTpHandle *hdl, uint8_t **buf, UDSSDU_t *info) { - assert(hdl); - assert(hdl->peek); - return hdl->peek(hdl, buf, info); -} - -const uint8_t *UDSTpGetRecvBuf(struct UDSTpHandle *hdl, size_t *p_len) { - assert(hdl); - ssize_t len = 0; - uint8_t *buf = NULL; - len = UDSTpPeek(hdl, &buf, NULL); - if (len > 0) { - if (p_len) { - *p_len = len; - } - return buf; - } else { - return NULL; - } -} - -size_t UDSTpGetRecvLen(UDSTpHandle_t *hdl) { - assert(hdl); - size_t len = 0; - UDSTpGetRecvBuf(hdl, &len); - return len; -} - -void UDSTpAckRecv(UDSTpHandle_t *hdl) { - assert(hdl); - hdl->ack_recv(hdl); -} - -// ======================================================================== -// Common -// ======================================================================== - -#if UDS_CUSTOM_MILLIS -#else -uint32_t UDSMillis(void) { -#if UDS_ARCH == UDS_ARCH_UNIX - struct timeval te; - gettimeofday(&te, NULL); - long long milliseconds = te.tv_sec * 1000LL + te.tv_usec / 1000; - return milliseconds; -#elif UDS_ARCH == UDS_ARCH_WINDOWS - struct timespec ts; - timespec_get(&ts, TIME_UTC); - long long milliseconds = ts.tv_sec * 1000LL + ts.tv_nsec / 1000000; - return milliseconds; -#else -#error "UDSMillis() undefined!" -#endif -} -#endif - -static bool UDSSecurityAccessLevelIsReserved(uint8_t securityLevel) { - securityLevel &= 0x3f; - return (0 == securityLevel || (0x43 <= securityLevel && securityLevel >= 0x5E) || - 0x7F == securityLevel); -} - -// ======================================================================== -// Server -// ======================================================================== - -static inline uint8_t NegativeResponse(UDSReq_t *r, uint8_t response_code) { - r->send_buf[0] = 0x7F; - r->send_buf[1] = r->recv_buf[0]; - r->send_buf[2] = response_code; - r->send_len = UDS_NEG_RESP_LEN; - return response_code; -} - -static inline void NoResponse(UDSReq_t *r) { r->send_len = 0; } - -static uint8_t EmitEvent(UDSServer_t *srv, UDSServerEvent_t evt, void *data) { - if (srv->fn) { - return srv->fn(srv, evt, data); - } else { - UDS_DBG_PRINT("Unhandled UDSServerEvent %d, srv.fn not installed!\n", evt); - return kGeneralReject; - } -} - -static uint8_t _0x10_DiagnosticSessionControl(UDSServer_t *srv, UDSReq_t *r) { - if (r->recv_len < UDS_0X10_REQ_LEN) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - - uint8_t sessType = r->recv_buf[1] & 0x4F; - - UDSDiagSessCtrlArgs_t args = { - .type = sessType, - .p2_ms = UDS_CLIENT_DEFAULT_P2_MS, - .p2_star_ms = UDS_CLIENT_DEFAULT_P2_STAR_MS, - }; - - uint8_t err = EmitEvent(srv, UDS_SRV_EVT_DiagSessCtrl, &args); - - if (kPositiveResponse != err) { - return NegativeResponse(r, err); - } - - srv->sessionType = sessType; - - switch (sessType) { - case kDefaultSession: - break; - case kProgrammingSession: - case kExtendedDiagnostic: - default: - srv->s3_session_timeout_timer = UDSMillis() + srv->s3_ms; - break; - } - - r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_DIAGNOSTIC_SESSION_CONTROL); - r->send_buf[1] = sessType; - - // UDS-1-2013: Table 29 - // resolution: 1ms - r->send_buf[2] = args.p2_ms >> 8; - r->send_buf[3] = args.p2_ms; - - // resolution: 10ms - r->send_buf[4] = (args.p2_star_ms / 10) >> 8; - r->send_buf[5] = args.p2_star_ms / 10; - - r->send_len = UDS_0X10_RESP_LEN; - return kPositiveResponse; -} - -static uint8_t _0x11_ECUReset(UDSServer_t *srv, UDSReq_t *r) { - uint8_t resetType = r->recv_buf[1] & 0x3F; - - if (r->recv_len < UDS_0X11_REQ_MIN_LEN) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - - UDSECUResetArgs_t args = { - .type = resetType, - .powerDownTimeMillis = UDS_SERVER_DEFAULT_POWER_DOWN_TIME_MS, - }; - - uint8_t err = EmitEvent(srv, UDS_SRV_EVT_EcuReset, &args); - - if (kPositiveResponse == err) { - srv->notReadyToReceive = true; - srv->ecuResetScheduled = resetType; - srv->ecuResetTimer = UDSMillis() + args.powerDownTimeMillis; - } else { - return NegativeResponse(r, err); - } - - r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_ECU_RESET); - r->send_buf[1] = resetType; - - if (kEnableRapidPowerShutDown == resetType) { - uint32_t powerDownTime = args.powerDownTimeMillis / 1000; - if (powerDownTime > 255) { - powerDownTime = 255; - } - r->send_buf[2] = powerDownTime; - r->send_len = UDS_0X11_RESP_BASE_LEN + 1; - } else { - r->send_len = UDS_0X11_RESP_BASE_LEN; - } - return kPositiveResponse; -} - -static uint8_t safe_copy(UDSServer_t *srv, const void *src, uint16_t count) { - if (srv == NULL) { - return kGeneralReject; - } - UDSReq_t *r = (UDSReq_t *)&srv->r; - if (count <= r->send_buf_size - r->send_len) { - memmove(r->send_buf + r->send_len, src, count); - r->send_len += count; - return kPositiveResponse; - } - return kResponseTooLong; -} - -static uint8_t _0x22_ReadDataByIdentifier(UDSServer_t *srv, UDSReq_t *r) { - uint8_t numDIDs; - uint16_t dataId = 0; - uint8_t ret = kPositiveResponse; - r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_READ_DATA_BY_IDENTIFIER); - r->send_len = 1; - - if (0 != (r->recv_len - 1) % sizeof(uint16_t)) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - - numDIDs = r->recv_len / sizeof(uint16_t); - - if (0 == numDIDs) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - - for (int did = 0; did < numDIDs; did++) { - uint16_t idx = 1 + did * 2; - dataId = (r->recv_buf[idx] << 8) + r->recv_buf[idx + 1]; - - if (r->send_len + 3 > r->send_buf_size) { - return NegativeResponse(r, kResponseTooLong); - } - uint8_t *copylocation = r->send_buf + r->send_len; - copylocation[0] = dataId >> 8; - copylocation[1] = dataId; - r->send_len += 2; - - UDSRDBIArgs_t args = { - .dataId = dataId, - .copy = safe_copy, - }; - - ret = EmitEvent(srv, UDS_SRV_EVT_ReadDataByIdent, &args); - - if (kPositiveResponse != ret) { - return NegativeResponse(r, ret); - } - } - return kPositiveResponse; -} - -/** - * @brief decode the addressAndLengthFormatIdentifier that appears in ReadMemoryByAddress (0x23), - * DynamicallyDefineDataIdentifier (0x2C), RequestDownload (0X34) - * - * @param srv - * @param buf pointer to addressAndDataLengthFormatIdentifier in recv_buf - * @param memoryAddress the decoded memory address - * @param memorySize the decoded memory size - * @return uint8_t - */ -static uint8_t decodeAddressAndLength(UDSReq_t *r, uint8_t *const buf, void **memoryAddress, - size_t *memorySize) { - assert(r); - assert(memoryAddress); - assert(memorySize); - long long unsigned int tmp = 0; - *memoryAddress = 0; - *memorySize = 0; - - assert(buf >= r->recv_buf && buf <= r->recv_buf + sizeof(r->recv_buf)); - - if (r->recv_len < 3) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - - uint8_t memorySizeLength = (buf[0] & 0xF0) >> 4; - uint8_t memoryAddressLength = buf[0] & 0x0F; - - if (memorySizeLength == 0 || memorySizeLength > sizeof(size_t)) { - return NegativeResponse(r, kRequestOutOfRange); - } - - if (memoryAddressLength == 0 || memoryAddressLength > sizeof(size_t)) { - return NegativeResponse(r, kRequestOutOfRange); - } - - if (buf + memorySizeLength + memoryAddressLength > r->recv_buf + r->recv_len) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - - for (int byteIdx = 0; byteIdx < memoryAddressLength; byteIdx++) { - long long unsigned int byte = buf[1 + byteIdx]; - uint8_t shiftBytes = memoryAddressLength - 1 - byteIdx; - tmp |= byte << (8 * shiftBytes); - } - *memoryAddress = (void *)tmp; - - for (int byteIdx = 0; byteIdx < memorySizeLength; byteIdx++) { - uint8_t byte = buf[1 + memoryAddressLength + byteIdx]; - uint8_t shiftBytes = memorySizeLength - 1 - byteIdx; - *memorySize |= (size_t)byte << (8 * shiftBytes); - } - return kPositiveResponse; -} - -static uint8_t _0x23_ReadMemoryByAddress(UDSServer_t *srv, UDSReq_t *r) { - uint8_t ret = kPositiveResponse; - void *address = 0; - size_t length = 0; - - if (r->recv_len < UDS_0X23_REQ_MIN_LEN) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - - ret = decodeAddressAndLength(r, &r->recv_buf[1], &address, &length); - if (kPositiveResponse != ret) { - return NegativeResponse(r, ret); - } - - UDSReadMemByAddrArgs_t args = { - .memAddr = address, - .memSize = length, - .copy = safe_copy, - }; - - r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_READ_MEMORY_BY_ADDRESS); - r->send_len = UDS_0X23_RESP_BASE_LEN; - ret = EmitEvent(srv, UDS_SRV_EVT_ReadMemByAddr, &args); - if (kPositiveResponse != ret) { - return NegativeResponse(r, ret); - } - if (r->send_len != UDS_0X23_RESP_BASE_LEN + length) { - return kGeneralProgrammingFailure; - } - return kPositiveResponse; -} - -static uint8_t _0x27_SecurityAccess(UDSServer_t *srv, UDSReq_t *r) { - uint8_t subFunction = r->recv_buf[1]; - uint8_t response = kPositiveResponse; - - if (UDSSecurityAccessLevelIsReserved(subFunction)) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - - r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_SECURITY_ACCESS); - r->send_buf[1] = subFunction; - r->send_len = UDS_0X27_RESP_BASE_LEN; - - // Even: sendKey - if (0 == subFunction % 2) { - uint8_t requestedLevel = subFunction - 1; - UDSSecAccessValidateKeyArgs_t args = { - .level = requestedLevel, - .key = &r->recv_buf[UDS_0X27_REQ_BASE_LEN], - .len = r->recv_len - UDS_0X27_REQ_BASE_LEN, - }; - - response = EmitEvent(srv, UDS_SRV_EVT_SecAccessValidateKey, &args); - - if (kPositiveResponse != response) { - return NegativeResponse(r, response); - } - - // "requestSeed = 0x01" identifies a fixed relationship between - // "requestSeed = 0x01" and "sendKey = 0x02" - // "requestSeed = 0x03" identifies a fixed relationship between - // "requestSeed = 0x03" and "sendKey = 0x04" - srv->securityLevel = requestedLevel; - r->send_len = UDS_0X27_RESP_BASE_LEN; - return kPositiveResponse; - } - - // Odd: requestSeed - else { - /* If a server supports security, but the requested security level is already unlocked when - a SecurityAccess ‘requestSeed’ message is received, that server shall respond with a - SecurityAccess ‘requestSeed’ positive response message service with a seed value equal to - zero (0). The server shall never send an all zero seed for a given security level that is - currently locked. The client shall use this method to determine if a server is locked for a - particular security level by checking for a non-zero seed. - */ - if (subFunction == srv->securityLevel) { - // Table 52 sends a response of length 2. Use a preprocessor define if this needs - // customizing by the user. - const uint8_t already_unlocked[] = {0x00, 0x00}; - return safe_copy(srv, already_unlocked, sizeof(already_unlocked)); - } else { - UDSSecAccessRequestSeedArgs_t args = { - .level = subFunction, - .dataRecord = &r->recv_buf[UDS_0X27_REQ_BASE_LEN], - .len = r->recv_len - UDS_0X27_REQ_BASE_LEN, - .copySeed = safe_copy, - }; - - response = EmitEvent(srv, UDS_SRV_EVT_SecAccessRequestSeed, &args); - - if (kPositiveResponse != response) { - return NegativeResponse(r, response); - } - - if (r->send_len <= UDS_0X27_RESP_BASE_LEN) { // no data was copied - return NegativeResponse(r, kGeneralProgrammingFailure); - } - return kPositiveResponse; - } - } - return NegativeResponse(r, kGeneralProgrammingFailure); -} - -static uint8_t _0x28_CommunicationControl(UDSServer_t *srv, UDSReq_t *r) { - uint8_t controlType = r->recv_buf[1] & 0x7F; - uint8_t communicationType = r->recv_buf[2]; - - if (r->recv_len < UDS_0X28_REQ_BASE_LEN) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - - UDSCommCtrlArgs_t args = { - .ctrlType = controlType, - .commType = communicationType, - }; - - uint8_t err = EmitEvent(srv, UDS_SRV_EVT_CommCtrl, &args); - if (kPositiveResponse != err) { - return NegativeResponse(r, err); - } - - r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_COMMUNICATION_CONTROL); - r->send_buf[1] = controlType; - r->send_len = UDS_0X28_RESP_LEN; - return kPositiveResponse; -} - -static uint8_t _0x2E_WriteDataByIdentifier(UDSServer_t *srv, UDSReq_t *r) { - uint16_t dataLen = 0; - uint16_t dataId = 0; - uint8_t err = kPositiveResponse; - - /* UDS-1 2013 Figure 21 Key 1 */ - if (r->recv_len < UDS_0X2E_REQ_MIN_LEN) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - - dataId = (r->recv_buf[1] << 8) + r->recv_buf[2]; - dataLen = r->recv_len - UDS_0X2E_REQ_BASE_LEN; - - UDSWDBIArgs_t args = { - .dataId = dataId, - .data = &r->recv_buf[UDS_0X2E_REQ_BASE_LEN], - .len = dataLen, - }; - - err = EmitEvent(srv, UDS_SRV_EVT_WriteDataByIdent, &args); - if (kPositiveResponse != err) { - return NegativeResponse(r, err); - } - - r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_WRITE_DATA_BY_IDENTIFIER); - r->send_buf[1] = dataId >> 8; - r->send_buf[2] = dataId; - r->send_len = UDS_0X2E_RESP_LEN; - return kPositiveResponse; -} - -static uint8_t _0x31_RoutineControl(UDSServer_t *srv, UDSReq_t *r) { - uint8_t err = kPositiveResponse; - if (r->recv_len < UDS_0X31_REQ_MIN_LEN) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - - uint8_t routineControlType = r->recv_buf[1] & 0x7F; - uint16_t routineIdentifier = (r->recv_buf[2] << 8) + r->recv_buf[3]; - - UDSRoutineCtrlArgs_t args = { - .ctrlType = routineControlType, - .id = routineIdentifier, - .optionRecord = &r->recv_buf[UDS_0X31_REQ_MIN_LEN], - .len = r->recv_len - UDS_0X31_REQ_MIN_LEN, - .copyStatusRecord = safe_copy, - }; - - r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_ROUTINE_CONTROL); - r->send_buf[1] = routineControlType; - r->send_buf[2] = routineIdentifier >> 8; - r->send_buf[3] = routineIdentifier; - r->send_len = UDS_0X31_RESP_MIN_LEN; - - switch (routineControlType) { - case kStartRoutine: - case kStopRoutine: - case kRequestRoutineResults: - err = EmitEvent(srv, UDS_SRV_EVT_RoutineCtrl, &args); - if (kPositiveResponse != err) { - return NegativeResponse(r, err); - } - break; - default: - return NegativeResponse(r, kRequestOutOfRange); - } - return kPositiveResponse; -} - -static void ResetTransfer(UDSServer_t *srv) { - assert(srv); - srv->xferBlockSequenceCounter = 1; - srv->xferByteCounter = 0; - srv->xferTotalBytes = 0; - srv->xferIsActive = false; -} - -static uint8_t _0x34_RequestDownload(UDSServer_t *srv, UDSReq_t *r) { - uint8_t err; - void *memoryAddress = 0; - size_t memorySize = 0; - - if (srv->xferIsActive) { - return NegativeResponse(r, kConditionsNotCorrect); - } - - if (r->recv_len < UDS_0X34_REQ_BASE_LEN) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - - err = decodeAddressAndLength(r, &r->recv_buf[2], &memoryAddress, &memorySize); - if (kPositiveResponse != err) { - return NegativeResponse(r, err); - } - - UDSRequestDownloadArgs_t args = { - .addr = memoryAddress, - .size = memorySize, - .dataFormatIdentifier = r->recv_buf[1], - .maxNumberOfBlockLength = UDS_SERVER_DEFAULT_XFER_DATA_MAX_BLOCKLENGTH, - }; - - err = EmitEvent(srv, UDS_SRV_EVT_RequestDownload, &args); - - if (args.maxNumberOfBlockLength < 3) { - UDS_DBG_PRINT("ERROR: maxNumberOfBlockLength too short"); - return NegativeResponse(r, kGeneralProgrammingFailure); - } - - if (kPositiveResponse != err) { - return NegativeResponse(r, err); - } - - ResetTransfer(srv); - srv->xferIsActive = true; - srv->xferTotalBytes = memorySize; - srv->xferBlockLength = args.maxNumberOfBlockLength; - - // ISO-14229-1:2013 Table 401: - uint8_t lengthFormatIdentifier = sizeof(args.maxNumberOfBlockLength) << 4; - - /* ISO-14229-1:2013 Table 396: maxNumberOfBlockLength - This parameter is used by the requestDownload positive response message to - inform the client how many data bytes (maxNumberOfBlockLength) to include in - each TransferData request message from the client. This length reflects the - complete message length, including the service identifier and the - data-parameters present in the TransferData request message. - */ - if (args.maxNumberOfBlockLength > UDS_TP_MTU) { - args.maxNumberOfBlockLength = UDS_TP_MTU; - } - - r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_REQUEST_DOWNLOAD); - r->send_buf[1] = lengthFormatIdentifier; - for (uint8_t idx = 0; idx < sizeof(args.maxNumberOfBlockLength); idx++) { - uint8_t shiftBytes = sizeof(args.maxNumberOfBlockLength) - 1 - idx; - uint8_t byte = args.maxNumberOfBlockLength >> (shiftBytes * 8); - r->send_buf[UDS_0X34_RESP_BASE_LEN + idx] = byte; - } - r->send_len = UDS_0X34_RESP_BASE_LEN + sizeof(args.maxNumberOfBlockLength); - return kPositiveResponse; -} - -static uint8_t _0x35_RequestUpload(UDSServer_t *srv, UDSReq_t *r) { - uint8_t err; - void *memoryAddress = 0; - size_t memorySize = 0; - - if (srv->xferIsActive) { - return NegativeResponse(r, kConditionsNotCorrect); - } - - if (r->recv_len < UDS_0X35_REQ_BASE_LEN) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - - err = decodeAddressAndLength(r, &r->recv_buf[2], &memoryAddress, &memorySize); - if (kPositiveResponse != err) { - return NegativeResponse(r, err); - } - - UDSRequestUploadArgs_t args = { - .addr = memoryAddress, - .size = memorySize, - .dataFormatIdentifier = r->recv_buf[1], - .maxNumberOfBlockLength = UDS_SERVER_DEFAULT_XFER_DATA_MAX_BLOCKLENGTH, - }; - - err = EmitEvent(srv, UDS_SRV_EVT_RequestUpload, &args); - - if (args.maxNumberOfBlockLength < 3) { - UDS_DBG_PRINT("ERROR: maxNumberOfBlockLength too short"); - return NegativeResponse(r, kGeneralProgrammingFailure); - } - - if (kPositiveResponse != err) { - return NegativeResponse(r, err); - } - - ResetTransfer(srv); - srv->xferIsActive = true; - srv->xferTotalBytes = memorySize; - srv->xferBlockLength = args.maxNumberOfBlockLength; - - uint8_t lengthFormatIdentifier = sizeof(args.maxNumberOfBlockLength) << 4; - - r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_REQUEST_UPLOAD); - r->send_buf[1] = lengthFormatIdentifier; - for (uint8_t idx = 0; idx < sizeof(args.maxNumberOfBlockLength); idx++) { - uint8_t shiftBytes = sizeof(args.maxNumberOfBlockLength) - 1 - idx; - uint8_t byte = args.maxNumberOfBlockLength >> (shiftBytes * 8); - r->send_buf[UDS_0X35_RESP_BASE_LEN + idx] = byte; - } - r->send_len = UDS_0X35_RESP_BASE_LEN + sizeof(args.maxNumberOfBlockLength); - return kPositiveResponse; -} - -static uint8_t _0x36_TransferData(UDSServer_t *srv, UDSReq_t *r) { - uint8_t err = kPositiveResponse; - uint16_t request_data_len = r->recv_len - UDS_0X36_REQ_BASE_LEN; - uint8_t blockSequenceCounter = 0; - - if (!srv->xferIsActive) { - return NegativeResponse(r, kUploadDownloadNotAccepted); - } - - if (r->recv_len < UDS_0X36_REQ_BASE_LEN) { - err = kIncorrectMessageLengthOrInvalidFormat; - goto fail; - } - - blockSequenceCounter = r->recv_buf[1]; - - if (!srv->RCRRP) { - if (blockSequenceCounter != srv->xferBlockSequenceCounter) { - err = kRequestSequenceError; - goto fail; - } else { - srv->xferBlockSequenceCounter++; - } - } - - if (srv->xferByteCounter + request_data_len > srv->xferTotalBytes) { - err = kTransferDataSuspended; - goto fail; - } - - { - UDSTransferDataArgs_t args = { - .data = &r->recv_buf[UDS_0X36_REQ_BASE_LEN], - .len = r->recv_len - UDS_0X36_REQ_BASE_LEN, - .maxRespLen = srv->xferBlockLength - UDS_0X36_RESP_BASE_LEN, - .copyResponse = safe_copy, - }; - - r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_TRANSFER_DATA); - r->send_buf[1] = blockSequenceCounter; - r->send_len = UDS_0X36_RESP_BASE_LEN; - - err = EmitEvent(srv, UDS_SRV_EVT_TransferData, &args); - - switch (err) { - case kPositiveResponse: - srv->xferByteCounter += request_data_len; - return kPositiveResponse; - case kRequestCorrectlyReceived_ResponsePending: - return NegativeResponse(r, kRequestCorrectlyReceived_ResponsePending); - default: - goto fail; - } - } - -fail: - ResetTransfer(srv); - return NegativeResponse(r, err); -} - -static uint8_t _0x37_RequestTransferExit(UDSServer_t *srv, UDSReq_t *r) { - uint8_t err = kPositiveResponse; - - if (!srv->xferIsActive) { - return NegativeResponse(r, kUploadDownloadNotAccepted); - } - - r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_REQUEST_TRANSFER_EXIT); - r->send_len = UDS_0X37_RESP_BASE_LEN; - - UDSRequestTransferExitArgs_t args = { - .data = &r->recv_buf[UDS_0X37_REQ_BASE_LEN], - .len = r->recv_len - UDS_0X37_REQ_BASE_LEN, - .copyResponse = safe_copy, - }; - - err = EmitEvent(srv, UDS_SRV_EVT_RequestTransferExit, &args); - - switch (err) { - case kPositiveResponse: - ResetTransfer(srv); - return kPositiveResponse; - case kRequestCorrectlyReceived_ResponsePending: - return NegativeResponse(r, kRequestCorrectlyReceived_ResponsePending); - default: - ResetTransfer(srv); - return NegativeResponse(r, err); - } -} - -static uint8_t _0x3E_TesterPresent(UDSServer_t *srv, UDSReq_t *r) { - if ((r->recv_len < UDS_0X3E_REQ_MIN_LEN) || (r->recv_len > UDS_0X3E_REQ_MAX_LEN)) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - uint8_t zeroSubFunction = r->recv_buf[1]; - - switch (zeroSubFunction) { - case 0x00: - case 0x80: - srv->s3_session_timeout_timer = UDSMillis() + srv->s3_ms; - r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_TESTER_PRESENT); - r->send_buf[1] = 0x00; - r->send_len = UDS_0X3E_RESP_LEN; - return kPositiveResponse; - default: - return NegativeResponse(r, kSubFunctionNotSupported); - } -} - -static uint8_t _0x85_ControlDTCSetting(UDSServer_t *srv, UDSReq_t *r) { - if (r->recv_len < UDS_0X85_REQ_BASE_LEN) { - return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); - } - uint8_t dtcSettingType = r->recv_buf[1] & 0x3F; - - r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_CONTROL_DTC_SETTING); - r->send_buf[1] = dtcSettingType; - r->send_len = UDS_0X85_RESP_LEN; - return kPositiveResponse; -} - -typedef uint8_t (*UDSService)(UDSServer_t *srv, UDSReq_t *r); - -/** - * @brief Get the internal service handler matching the given SID. - * @param sid - * @return pointer to UDSService or NULL if no match - */ -static UDSService getServiceForSID(uint8_t sid) { - switch (sid) { - case kSID_DIAGNOSTIC_SESSION_CONTROL: - return _0x10_DiagnosticSessionControl; - case kSID_ECU_RESET: - return _0x11_ECUReset; - case kSID_CLEAR_DIAGNOSTIC_INFORMATION: - return NULL; - case kSID_READ_DTC_INFORMATION: - return NULL; - case kSID_READ_DATA_BY_IDENTIFIER: - return _0x22_ReadDataByIdentifier; - case kSID_READ_MEMORY_BY_ADDRESS: - return _0x23_ReadMemoryByAddress; - case kSID_READ_SCALING_DATA_BY_IDENTIFIER: - return NULL; - case kSID_SECURITY_ACCESS: - return _0x27_SecurityAccess; - case kSID_COMMUNICATION_CONTROL: - return _0x28_CommunicationControl; - case kSID_READ_PERIODIC_DATA_BY_IDENTIFIER: - return NULL; - case kSID_DYNAMICALLY_DEFINE_DATA_IDENTIFIER: - return NULL; - case kSID_WRITE_DATA_BY_IDENTIFIER: - return _0x2E_WriteDataByIdentifier; - case kSID_INPUT_CONTROL_BY_IDENTIFIER: - return NULL; - case kSID_ROUTINE_CONTROL: - return _0x31_RoutineControl; - case kSID_REQUEST_DOWNLOAD: - return _0x34_RequestDownload; - case kSID_REQUEST_UPLOAD: - return _0x35_RequestUpload; - case kSID_TRANSFER_DATA: - return _0x36_TransferData; - case kSID_REQUEST_TRANSFER_EXIT: - return _0x37_RequestTransferExit; - case kSID_REQUEST_FILE_TRANSFER: - return NULL; - case kSID_WRITE_MEMORY_BY_ADDRESS: - return NULL; - case kSID_TESTER_PRESENT: - return _0x3E_TesterPresent; - case kSID_ACCESS_TIMING_PARAMETER: - return NULL; - case kSID_SECURED_DATA_TRANSMISSION: - return NULL; - case kSID_CONTROL_DTC_SETTING: - return _0x85_ControlDTCSetting; - case kSID_RESPONSE_ON_EVENT: - return NULL; - default: - UDS_DBG_PRINT("no handler for request SID %x.\n", sid); - return NULL; - } -} - -/** - * @brief Call the service if it exists, modifying the response if the spec calls for it. - * @note see UDS-1 2013 7.5.5 Pseudo code example of server response behavior - * - * @param srv - * @param addressingScheme - */ -static uint8_t evaluateServiceResponse(UDSServer_t *srv, UDSReq_t *r) { - uint8_t response = kPositiveResponse; - bool suppressResponse = false; - uint8_t sid = r->recv_buf[0]; - UDSService service = getServiceForSID(sid); - - if (NULL == service || NULL == srv->fn) { - return NegativeResponse(r, kServiceNotSupported); - } - assert(service); - assert(srv->fn); // service handler functions will call srv->fn. it must be valid - - switch (sid) { - /* CASE Service_with_sub-function */ - /* test if service with sub-function is supported */ - case kSID_DIAGNOSTIC_SESSION_CONTROL: - case kSID_ECU_RESET: - case kSID_SECURITY_ACCESS: - case kSID_COMMUNICATION_CONTROL: - case kSID_ROUTINE_CONTROL: - case kSID_TESTER_PRESENT: - case kSID_CONTROL_DTC_SETTING: { - response = service(srv, r); - - bool suppressPosRspMsgIndicationBit = r->recv_buf[1] & 0x80; - - /* test if positive response is required and if responseCode is positive 0x00 */ - if ((suppressPosRspMsgIndicationBit) && (response == kPositiveResponse) && - ( - // TODO: *not yet a NRC 0x78 response sent* - true)) { - suppressResponse = true; - } else { - suppressResponse = false; - } - break; - } - - /* CASE Service_without_sub-function */ - /* test if service without sub-function is supported */ - case kSID_READ_DATA_BY_IDENTIFIER: - case kSID_READ_MEMORY_BY_ADDRESS: - case kSID_WRITE_DATA_BY_IDENTIFIER: - case kSID_REQUEST_DOWNLOAD: - case kSID_REQUEST_UPLOAD: - case kSID_TRANSFER_DATA: - case kSID_REQUEST_TRANSFER_EXIT: { - response = service(srv, r); - break; - } - - /* CASE Service_not_implemented */ - /* shouldn't get this far as getServiceForSID(sid) will return NULL*/ - case kSID_CLEAR_DIAGNOSTIC_INFORMATION: - case kSID_READ_DTC_INFORMATION: - case kSID_READ_SCALING_DATA_BY_IDENTIFIER: - case kSID_READ_PERIODIC_DATA_BY_IDENTIFIER: - case kSID_DYNAMICALLY_DEFINE_DATA_IDENTIFIER: - case kSID_INPUT_CONTROL_BY_IDENTIFIER: - case kSID_REQUEST_FILE_TRANSFER: - case kSID_WRITE_MEMORY_BY_ADDRESS: - case kSID_ACCESS_TIMING_PARAMETER: - case kSID_SECURED_DATA_TRANSMISSION: - case kSID_RESPONSE_ON_EVENT: - default: { - response = kServiceNotSupported; - break; - } - } - - if ((UDS_A_TA_TYPE_FUNCTIONAL == r->info.A_TA_Type) && - ((kServiceNotSupported == response) || (kSubFunctionNotSupported == response) || - (kServiceNotSupportedInActiveSession == response) || - (kSubFunctionNotSupportedInActiveSession == response) || - (kRequestOutOfRange == response)) && - ( - // TODO: *not yet a NRC 0x78 response sent* - true)) { - suppressResponse = true; /* Suppress negative response message */ - NoResponse(r); - } else { - if (suppressResponse) { /* Suppress positive response message */ - NoResponse(r); - } else { /* send negative or positive response */ - ; - } - } - return response; -} - -// ======================================================================== -// Public Functions -// ======================================================================== - -/** - * @brief \~chinese 初始化服务器 \~english Initialize the server - * - * @param srv - * @param cfg - * @return int - */ -UDSErr_t UDSServerInit(UDSServer_t *srv) { - assert(srv); - memset(srv, 0, sizeof(UDSServer_t)); - srv->p2_ms = UDS_SERVER_DEFAULT_P2_MS; - srv->p2_star_ms = UDS_SERVER_DEFAULT_P2_STAR_MS; - srv->s3_ms = UDS_SERVER_DEFAULT_S3_MS; - srv->sessionType = kDefaultSession; - srv->p2_timer = UDSMillis() + srv->p2_ms; - srv->s3_session_timeout_timer = UDSMillis() + srv->s3_ms; - return UDS_OK; -} - -void UDSServerPoll(UDSServer_t *srv) { - // UDS-1-2013 Figure 38: Session Timeout (S3) - if (kDefaultSession != srv->sessionType && - UDSTimeAfter(UDSMillis(), srv->s3_session_timeout_timer)) { - EmitEvent(srv, UDS_SRV_EVT_SessionTimeout, NULL); - } - - if (srv->ecuResetScheduled && UDSTimeAfter(UDSMillis(), srv->ecuResetTimer)) { - EmitEvent(srv, UDS_SRV_EVT_DoScheduledReset, &srv->ecuResetScheduled); - } - - UDSTpStatus_t tpStatus = UDSTpPoll(srv->tp); - - UDSReq_t *r = &srv->r; - - if (srv->requestInProgress) { - if (srv->RCRRP) { - // responds only if - // 1. changed (no longer RCRRP), or - // 2. p2_timer has elapsed - uint8_t response = evaluateServiceResponse(srv, r); - if (kRequestCorrectlyReceived_ResponsePending == response) { - // it's the second time the service has responded with RCRRP - srv->notReadyToReceive = true; - } else { - // No longer RCRRP'ing - srv->RCRRP = false; - srv->notReadyToReceive = false; - - // Not a consecutive 0x78 response, use p2 instead of p2_star * 0.3 - srv->p2_timer = UDSMillis() + srv->p2_ms; - } - } - - if (UDSTimeAfter(UDSMillis(), srv->p2_timer)) { - printf("len: %ld\n", r->send_len); - ssize_t ret = UDSTpSend(srv->tp, r->send_buf, r->send_len, NULL); - // TODO test injection of transport errors: - if (ret < 0) { - UDSErr_t err = UDS_ERR_TPORT; - EmitEvent(srv, UDS_SRV_EVT_Err, &err); - UDS_DBG_PRINT("UDSTpSend failed with %ld\n", ret); - } - - if (srv->RCRRP) { - // ISO14229-2:2013 Table 4 footnote b - // min time between consecutive 0x78 responses is 0.3 * p2* - uint32_t wait_time = srv->p2_star_ms * 3 / 10; - srv->p2_timer = UDSMillis() + wait_time; - } else { - srv->p2_timer = UDSMillis() + srv->p2_ms; - UDSTpAckRecv(srv->tp); - srv->requestInProgress = false; - } - } - - } else { - if (srv->notReadyToReceive) { - return; // cannot respond to request right now - } - r->recv_len = UDSTpPeek(srv->tp, &r->recv_buf, &r->info); - r->send_buf_size = UDSTpGetSendBuf(srv->tp, &r->send_buf); - if (r->send_buf == NULL || r->recv_buf == NULL) { - UDSErr_t err = UDS_ERR_TPORT; - EmitEvent(srv, UDS_SRV_EVT_Err, &err); - UDS_DBG_PRINT("bad tport\n"); - return; - } - if (r->recv_len > 0) { - uint8_t response = evaluateServiceResponse(srv, r); - srv->requestInProgress = true; - if (kRequestCorrectlyReceived_ResponsePending == response) { - srv->RCRRP = true; - } - } - } -} - -// ======================================================================== -// Client -// ======================================================================== - -static void clearRequestContext(UDSClient_t *client) { - assert(client); - client->recv_size = 0; - client->send_size = 0; - client->state = kRequestStateIdle; - client->err = UDS_OK; -} - -UDSErr_t UDSClientInit(UDSClient_t *client) { - assert(client); - memset(client, 0, sizeof(*client)); - - client->p2_ms = UDS_CLIENT_DEFAULT_P2_MS; - client->p2_star_ms = UDS_CLIENT_DEFAULT_P2_STAR_MS; - - if (client->p2_star_ms < client->p2_ms) { - fprintf(stderr, "p2_star_ms must be >= p2_ms\n"); - client->p2_star_ms = client->p2_ms; - } - - clearRequestContext(client); - return UDS_OK; -} - -static const char *ClientStateName(enum UDSClientRequestState state) { - switch (state) { - case kRequestStateIdle: - return "Idle"; - case kRequestStateSending: - return "Sending"; - case kRequestStateAwaitSendComplete: - return "AwaitSendComplete"; - case kRequestStateAwaitResponse: - return "AwaitResponse"; - case kRequestStateProcessResponse: - return "ProcessResponse"; - default: - return "Unknown"; - } -} - -static void changeState(UDSClient_t *client, enum UDSClientRequestState state) { - printf("client state: %s (%d) -> %s (%d)\n", ClientStateName(client->state), client->state, - ClientStateName(state), state); - client->state = state; -} - -/** - * @brief Check that the response is a valid UDS response - * - * @param ctx - * @return UDSErr_t - */ -static UDSErr_t _ClientValidateResponse(const UDSClient_t *client) { - - if (client->recv_size < 1) { - return UDS_ERR_RESP_TOO_SHORT; - } - - if (0x7F == client->recv_buf[0]) { // 否定响应 - if (client->recv_size < 2) { - return UDS_ERR_RESP_TOO_SHORT; - } else if (client->send_buf[0] != client->recv_buf[1]) { - return UDS_ERR_SID_MISMATCH; - } else if (kRequestCorrectlyReceived_ResponsePending == client->recv_buf[2]) { - return UDS_OK; - } else if (client->_options_copy & UDS_NEG_RESP_IS_ERR) { - return UDS_ERR_NEG_RESP; - } else { - ; - } - } else { // 肯定响应 - if (UDS_RESPONSE_SID_OF(client->send_buf[0]) != client->recv_buf[0]) { - return UDS_ERR_SID_MISMATCH; - } - switch (client->send_buf[0]) { - case kSID_ECU_RESET: - if (client->recv_size < 2) { - return UDS_ERR_RESP_TOO_SHORT; - } else if (client->send_buf[1] != client->recv_buf[1]) { - return UDS_ERR_SUBFUNCTION_MISMATCH; - } else { - ; - } - break; - } - } - - return UDS_OK; -} - -/** - * @brief Handle validated server response - * @param client - */ -static inline void _ClientHandleResponse(UDSClient_t *client) { - if (0x7F == client->recv_buf[0]) { - if (kRequestCorrectlyReceived_ResponsePending == client->recv_buf[2]) { - UDS_DBG_PRINT("got RCRRP, setting p2 timer\n"); - client->p2_timer = UDSMillis() + client->p2_star_ms; - memset(client->recv_buf, 0, client->recv_buf_size); - client->recv_size = 0; - UDSTpAckRecv(client->tp); - changeState(client, kRequestStateAwaitResponse); - return; - } else { - ; - } - } else { - uint8_t respSid = client->recv_buf[0]; - switch (UDS_REQUEST_SID_OF(respSid)) { - case kSID_DIAGNOSTIC_SESSION_CONTROL: { - if (client->recv_size < UDS_0X10_RESP_LEN) { - UDS_DBG_PRINT("Error: SID %x response too short\n", - kSID_DIAGNOSTIC_SESSION_CONTROL); - client->err = UDS_ERR_RESP_TOO_SHORT; - UDSTpAckRecv(client->tp); - changeState(client, kRequestStateIdle); - return; - } - - if (client->_options_copy & UDS_IGNORE_SRV_TIMINGS) { - UDSTpAckRecv(client->tp); - changeState(client, kRequestStateIdle); - return; - } - - uint16_t p2 = (client->recv_buf[2] << 8) + client->recv_buf[3]; - uint32_t p2_star = ((client->recv_buf[4] << 8) + client->recv_buf[5]) * 10; - UDS_DBG_PRINT("received new timings: p2: %u, p2*: %u\n", p2, p2_star); - client->p2_ms = p2; - client->p2_star_ms = p2_star; - break; - } - default: - break; - } - } - UDSTpAckRecv(client->tp); - changeState(client, kRequestStateIdle); -} - -/** - * @brief execute the client request state machine - * @param client - */ -static void PollLowLevel(UDSClient_t *client) { - assert(client); - UDSTpStatus_t tp_status = client->tp->poll(client->tp); - switch (client->state) { - case kRequestStateIdle: { - client->options = client->defaultOptions; - break; - } - case kRequestStateSending: { - UDSTpAddr_t ta_type = client->_options_copy & UDS_FUNCTIONAL ? UDS_A_TA_TYPE_FUNCTIONAL - : UDS_A_TA_TYPE_PHYSICAL; - UDSSDU_t info = { - .A_Mtype = UDS_A_MTYPE_DIAG, - .A_TA_Type = ta_type, - }; - ssize_t ret = UDSTpSend(client->tp, client->send_buf, client->send_size, &info); - if (ret < 0) { - client->err = UDS_ERR_TPORT; - UDS_DBG_PRINT("tport err: %ld\n", ret); - } else if (0 == ret) { - UDS_DBG_PRINT("send in progress...\n"); - ; // 等待发送成功 - } else if (client->send_size == ret) { - changeState(client, kRequestStateAwaitSendComplete); - } else { - client->err = UDS_ERR_BUFSIZ; - } - break; - } - case kRequestStateAwaitSendComplete: { - if (client->_options_copy & UDS_FUNCTIONAL) { - // "The Functional addressing is applied only to single frame transmission" - // Specification of Diagnostic Communication (Diagnostic on CAN - Network Layer) - changeState(client, kRequestStateIdle); - } - if (tp_status & UDS_TP_SEND_IN_PROGRESS) { - ; // await send complete - } else { - if (client->_options_copy & UDS_SUPPRESS_POS_RESP) { - changeState(client, kRequestStateIdle); - } else { - changeState(client, kRequestStateAwaitResponse); - client->p2_timer = UDSMillis() + client->p2_ms; - } - } - break; - } - case kRequestStateAwaitResponse: { - UDSSDU_t info = {0}; - ssize_t len = UDSTpPeek(client->tp, &client->recv_buf, &info); - - if (UDS_A_TA_TYPE_FUNCTIONAL == info.A_TA_Type) { - UDSTpAckRecv(client->tp); - break; - } - if (len < 0) { - client->err = UDS_ERR_TPORT; - changeState(client, kRequestStateIdle); - } else if (0 == len) { - if (UDSTimeAfter(UDSMillis(), client->p2_timer)) { - client->err = UDS_ERR_TIMEOUT; - changeState(client, kRequestStateIdle); - } - } else { - printf("received %ld bytes\n", len); - client->recv_size = len; - changeState(client, kRequestStateProcessResponse); - } - break; - } - case kRequestStateProcessResponse: { - client->err = _ClientValidateResponse(client); - if (UDS_OK == client->err) { - _ClientHandleResponse(client); - } else { - UDSTpAckRecv(client->tp); - changeState(client, kRequestStateIdle); - } - break; - } - - default: - assert(0); - } -} - -static UDSErr_t _SendRequest(UDSClient_t *client) { - client->_options_copy = client->options; - - if (client->_options_copy & UDS_SUPPRESS_POS_RESP) { - // UDS-1:2013 8.2.2 Table 11 - client->send_buf[1] |= 0x80; - } - - changeState(client, kRequestStateSending); - PollLowLevel(client); // poll once to begin sending immediately - return UDS_OK; -} - -static UDSErr_t PreRequestCheck(UDSClient_t *client) { - if (kRequestStateIdle != client->state) { - return UDS_ERR_BUSY; - } - clearRequestContext(client); - if (client->tp == NULL) { - return UDS_ERR_TPORT; - } - ssize_t ret = UDSTpGetSendBuf(client->tp, &client->send_buf); - if (ret < 0) { - return UDS_ERR_TPORT; - } - client->send_buf_size = ret; - return UDS_OK; -} - -UDSErr_t UDSSendBytes(UDSClient_t *client, const uint8_t *data, uint16_t size) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - if (size > client->send_buf_size) { - return UDS_ERR_BUFSIZ; - } - memmove(client->send_buf, data, size); - client->send_size = size; - return _SendRequest(client); -} - -UDSErr_t UDSSendECUReset(UDSClient_t *client, UDSECUReset_t type) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - client->send_buf[0] = kSID_ECU_RESET; - client->send_buf[1] = type; - client->send_size = 2; - return _SendRequest(client); -} - -UDSErr_t UDSSendDiagSessCtrl(UDSClient_t *client, enum UDSDiagnosticSessionType mode) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - client->send_buf[0] = kSID_DIAGNOSTIC_SESSION_CONTROL; - client->send_buf[1] = mode; - client->send_size = 2; - return _SendRequest(client); -} - -UDSErr_t UDSSendCommCtrl(UDSClient_t *client, enum UDSCommunicationControlType ctrl, - enum UDSCommunicationType comm) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - client->send_buf[0] = kSID_COMMUNICATION_CONTROL; - client->send_buf[1] = ctrl; - client->send_buf[2] = comm; - client->send_size = 3; - return _SendRequest(client); -} - -UDSErr_t UDSSendTesterPresent(UDSClient_t *client) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - client->send_buf[0] = kSID_TESTER_PRESENT; - client->send_buf[1] = 0; - client->send_size = 2; - return _SendRequest(client); -} - -UDSErr_t UDSSendRDBI(UDSClient_t *client, const uint16_t *didList, - const uint16_t numDataIdentifiers) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - assert(didList); - assert(numDataIdentifiers); - client->send_buf[0] = kSID_READ_DATA_BY_IDENTIFIER; - for (int i = 0; i < numDataIdentifiers; i++) { - uint16_t offset = 1 + sizeof(uint16_t) * i; - if (offset + 2 > client->send_buf_size) { - return UDS_ERR_INVALID_ARG; - } - (client->send_buf + offset)[0] = (didList[i] & 0xFF00) >> 8; - (client->send_buf + offset)[1] = (didList[i] & 0xFF); - } - client->send_size = 1 + (numDataIdentifiers * sizeof(uint16_t)); - return _SendRequest(client); -} - -UDSErr_t UDSSendWDBI(UDSClient_t *client, uint16_t dataIdentifier, const uint8_t *data, - uint16_t size) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - assert(data); - assert(size); - client->send_buf[0] = kSID_WRITE_DATA_BY_IDENTIFIER; - if (client->send_buf_size <= 3 || size > client->send_buf_size - 3) { - return UDS_ERR_BUFSIZ; - } - client->send_buf[1] = (dataIdentifier & 0xFF00) >> 8; - client->send_buf[2] = (dataIdentifier & 0xFF); - memmove(&client->send_buf[3], data, size); - client->send_size = 3 + size; - return _SendRequest(client); -} - -/** - * @brief RoutineControl - * - * @param client - * @param type - * @param routineIdentifier - * @param data - * @param size - * @return UDSErr_t - * @addtogroup routineControl_0x31 - */ -UDSErr_t UDSSendRoutineCtrl(UDSClient_t *client, enum RoutineControlType type, - uint16_t routineIdentifier, const uint8_t *data, uint16_t size) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - client->send_buf[0] = kSID_ROUTINE_CONTROL; - client->send_buf[1] = type; - client->send_buf[2] = routineIdentifier >> 8; - client->send_buf[3] = routineIdentifier; - if (size) { - assert(data); - if (size > client->send_buf_size - UDS_0X31_REQ_MIN_LEN) { - return UDS_ERR_BUFSIZ; - } - memmove(&client->send_buf[UDS_0X31_REQ_MIN_LEN], data, size); - } else { - assert(NULL == data); - } - client->send_size = UDS_0X31_REQ_MIN_LEN + size; - return _SendRequest(client); -} - -/** - * @brief - * - * @param client - * @param dataFormatIdentifier - * @param addressAndLengthFormatIdentifier - * @param memoryAddress - * @param memorySize - * @return UDSErr_t - * @addtogroup requestDownload_0x34 - */ -UDSErr_t UDSSendRequestDownload(UDSClient_t *client, uint8_t dataFormatIdentifier, - uint8_t addressAndLengthFormatIdentifier, size_t memoryAddress, - size_t memorySize) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - uint8_t numMemorySizeBytes = (addressAndLengthFormatIdentifier & 0xF0) >> 4; - uint8_t numMemoryAddressBytes = addressAndLengthFormatIdentifier & 0x0F; - - client->send_buf[0] = kSID_REQUEST_DOWNLOAD; - client->send_buf[1] = dataFormatIdentifier; - client->send_buf[2] = addressAndLengthFormatIdentifier; - - uint8_t *ptr = &client->send_buf[UDS_0X34_REQ_BASE_LEN]; - - for (int i = numMemoryAddressBytes - 1; i >= 0; i--) { - *ptr = (memoryAddress & (0xFF << (8 * i))) >> (8 * i); - ptr++; - } - - for (int i = numMemorySizeBytes - 1; i >= 0; i--) { - *ptr = (memorySize & (0xFF << (8 * i))) >> (8 * i); - ptr++; - } - - client->send_size = UDS_0X34_REQ_BASE_LEN + numMemoryAddressBytes + numMemorySizeBytes; - return _SendRequest(client); -} - -/** - * @brief - * - * @param client - * @param dataFormatIdentifier - * @param addressAndLengthFormatIdentifier - * @param memoryAddress - * @param memorySize - * @return UDSErr_t - * @addtogroup requestDownload_0x35 - */ -UDSErr_t UDSSendRequestUpload(UDSClient_t *client, uint8_t dataFormatIdentifier, - uint8_t addressAndLengthFormatIdentifier, size_t memoryAddress, - size_t memorySize) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - uint8_t numMemorySizeBytes = (addressAndLengthFormatIdentifier & 0xF0) >> 4; - uint8_t numMemoryAddressBytes = addressAndLengthFormatIdentifier & 0x0F; - - client->send_buf[0] = kSID_REQUEST_UPLOAD; - client->send_buf[1] = dataFormatIdentifier; - client->send_buf[2] = addressAndLengthFormatIdentifier; - - uint8_t *ptr = &client->send_buf[UDS_0X35_REQ_BASE_LEN]; - - for (int i = numMemoryAddressBytes - 1; i >= 0; i--) { - *ptr = (memoryAddress & (0xFF << (8 * i))) >> (8 * i); - ptr++; - } - - for (int i = numMemorySizeBytes - 1; i >= 0; i--) { - *ptr = (memorySize & (0xFF << (8 * i))) >> (8 * i); - ptr++; - } - - client->send_size = UDS_0X35_REQ_BASE_LEN + numMemoryAddressBytes + numMemorySizeBytes; - return _SendRequest(client); -} - -/** - * @brief - * - * @param client - * @param blockSequenceCounter - * @param blockLength - * @param fd - * @return UDSErr_t - * @addtogroup transferData_0x36 - */ -UDSErr_t UDSSendTransferData(UDSClient_t *client, uint8_t blockSequenceCounter, - const uint16_t blockLength, const uint8_t *data, uint16_t size) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - assert(blockLength > 2); // blockLength must include SID and sequenceCounter - assert(size + 2 <= blockLength); // data must fit inside blockLength - 2 - client->send_buf[0] = kSID_TRANSFER_DATA; - client->send_buf[1] = blockSequenceCounter; - memmove(&client->send_buf[UDS_0X36_REQ_BASE_LEN], data, size); - UDS_DBG_PRINT("size: %d, blocklength: %d\n", size, blockLength); - client->send_size = UDS_0X36_REQ_BASE_LEN + size; - return _SendRequest(client); -} - -UDSErr_t UDSSendTransferDataStream(UDSClient_t *client, uint8_t blockSequenceCounter, - const uint16_t blockLength, FILE *fd) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - assert(blockLength > 2); // blockLength must include SID and sequenceCounter - client->send_buf[0] = kSID_TRANSFER_DATA; - client->send_buf[1] = blockSequenceCounter; - - uint16_t size = fread(&client->send_buf[2], 1, blockLength - 2, fd); - UDS_DBG_PRINT("size: %d, blocklength: %d\n", size, blockLength); - client->send_size = UDS_0X36_REQ_BASE_LEN + size; - return _SendRequest(client); -} - -/** - * @brief - * - * @param client - * @return UDSErr_t - * @addtogroup requestTransferExit_0x37 - */ -UDSErr_t UDSSendRequestTransferExit(UDSClient_t *client) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - client->send_buf[0] = kSID_REQUEST_TRANSFER_EXIT; - client->send_size = 1; - return _SendRequest(client); -} - -/** - * @brief - * - * @param client - * @param dtcSettingType - * @param data - * @param size - * @return UDSErr_t - * @addtogroup controlDTCSetting_0x85 - */ -UDSErr_t UDSCtrlDTCSetting(UDSClient_t *client, uint8_t dtcSettingType, uint8_t *data, - uint16_t size) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - if (0x00 == dtcSettingType || 0x7F == dtcSettingType || - (0x03 <= dtcSettingType && dtcSettingType <= 0x3F)) { - assert(0); // reserved vals - } - client->send_buf[0] = kSID_CONTROL_DTC_SETTING; - client->send_buf[1] = dtcSettingType; - - if (NULL == data) { - assert(size == 0); - } else { - assert(size > 0); - if (size > client->send_buf_size - 2) { - return UDS_ERR_BUFSIZ; - } - memmove(&client->send_buf[2], data, size); - } - client->send_size = 2 + size; - return _SendRequest(client); -} - -/** - * @brief - * - * @param client - * @param level - * @param data - * @param size - * @return UDSErr_t - * @addtogroup securityAccess_0x27 - */ -UDSErr_t UDSSendSecurityAccess(UDSClient_t *client, uint8_t level, uint8_t *data, uint16_t size) { - UDSErr_t err = PreRequestCheck(client); - if (err) { - return err; - } - if (UDSSecurityAccessLevelIsReserved(level)) { - return UDS_ERR_INVALID_ARG; - } - client->send_buf[0] = kSID_SECURITY_ACCESS; - client->send_buf[1] = level; - if (size) { - assert(data); - if (size > client->send_buf_size - UDS_0X27_REQ_BASE_LEN) { - return UDS_ERR_BUFSIZ; - } - } else { - assert(NULL == data); - } - - memmove(&client->send_buf[UDS_0X27_REQ_BASE_LEN], data, size); - client->send_size = UDS_0X27_REQ_BASE_LEN + size; - return _SendRequest(client); -} - -typedef struct { - uint8_t dataFormatIdentifier; - uint8_t addressAndLengthFormatIdentifier; - size_t memoryAddress; - size_t memorySize; - FILE *fd; - uint8_t blockSequenceCounter; - uint16_t blockLength; -} UDSClientDownloadSequence_t; - -static UDSSeqState_t requestDownload(UDSClient_t *client) { - UDSClientDownloadSequence_t *pL_Seq = (UDSClientDownloadSequence_t *)client->cbData; - UDSSendRequestDownload(client, pL_Seq->dataFormatIdentifier, - pL_Seq->addressAndLengthFormatIdentifier, pL_Seq->memoryAddress, - pL_Seq->memorySize); - return UDSSeqStateGotoNext; -} - -static UDSSeqState_t checkRequestDownloadResponse(UDSClient_t *client) { - UDSClientDownloadSequence_t *pL_Seq = (UDSClientDownloadSequence_t *)client->cbData; - struct RequestDownloadResponse resp = {0}; - UDSErr_t err = UDSUnpackRequestDownloadResponse(client, &resp); - if (err) { - client->err = err; - return UDSSeqStateDone; - } - pL_Seq->blockLength = resp.maxNumberOfBlockLength; - if (0 == resp.maxNumberOfBlockLength) { - client->err = UDS_ERR; - return UDSSeqStateDone; - } - return UDSSeqStateGotoNext; -} - -static UDSSeqState_t prepareToTransfer(UDSClient_t *client) { - UDSClientDownloadSequence_t *pL_Seq = (UDSClientDownloadSequence_t *)client->cbData; - pL_Seq->blockSequenceCounter = 1; - return UDSSeqStateGotoNext; -} - -static UDSSeqState_t transferData(UDSClient_t *client) { - UDSClientDownloadSequence_t *pL_Seq = (UDSClientDownloadSequence_t *)client->cbData; - if (kRequestStateIdle == client->state) { - if (ferror(pL_Seq->fd)) { - fclose(pL_Seq->fd); - client->err = UDS_ERR_FILE_IO; // 读取文件故障 - return UDSSeqStateDone; - } else if (feof(pL_Seq->fd)) { // 传完了 - return UDSSeqStateGotoNext; - } else { - UDSSendTransferDataStream(client, pL_Seq->blockSequenceCounter++, pL_Seq->blockLength, - pL_Seq->fd); - } - } - return UDSSeqStateRunning; -} - -static UDSSeqState_t requestTransferExit(UDSClient_t *client) { - UDSSendRequestTransferExit(client); - return UDSSeqStateGotoNext; -} - -UDSErr_t UDSConfigDownload(UDSClient_t *client, uint8_t dataFormatIdentifier, - uint8_t addressAndLengthFormatIdentifier, size_t memoryAddress, - size_t memorySize, FILE *fd) { - - static const UDSClientCallback callbacks[] = { - requestDownload, UDSClientAwaitIdle, checkRequestDownloadResponse, prepareToTransfer, - transferData, requestTransferExit, UDSClientAwaitIdle, NULL}; - static UDSClientDownloadSequence_t seq = {0}; - memset(&seq, 0, sizeof(seq)); - seq.blockSequenceCounter = 1; - seq.dataFormatIdentifier = dataFormatIdentifier; - seq.addressAndLengthFormatIdentifier = addressAndLengthFormatIdentifier; - seq.memoryAddress = memoryAddress; - seq.memorySize = memorySize; - seq.fd = fd; - client->cbList = callbacks; - client->cbIdx = 0; - client->cbData = &seq; - return UDS_OK; -} - -/** - * @brief - * - * @param client - * @param resp - * @return UDSErr_t - * @addtogroup securityAccess_0x27 - */ -UDSErr_t UDSUnpackSecurityAccessResponse(const UDSClient_t *client, - struct SecurityAccessResponse *resp) { - assert(client); - assert(resp); - if (UDS_RESPONSE_SID_OF(kSID_SECURITY_ACCESS) != client->recv_buf[0]) { - return UDS_ERR_SID_MISMATCH; - } - if (client->recv_size < UDS_0X27_RESP_BASE_LEN) { - return UDS_ERR_RESP_TOO_SHORT; - } - resp->securityAccessType = client->recv_buf[1]; - resp->securitySeedLength = client->recv_size - UDS_0X27_RESP_BASE_LEN; - resp->securitySeed = resp->securitySeedLength == 0 ? NULL : &client->recv_buf[2]; - return UDS_OK; -} - -/** - * @brief - * - * @param client - * @param resp - * @return UDSErr_t - * @addtogroup routineControl_0x31 - */ -UDSErr_t UDSUnpackRoutineControlResponse(const UDSClient_t *client, - struct RoutineControlResponse *resp) { - assert(client); - assert(resp); - if (UDS_RESPONSE_SID_OF(kSID_ROUTINE_CONTROL) != client->recv_buf[0]) { - return UDS_ERR_SID_MISMATCH; - } - if (client->recv_size < UDS_0X31_RESP_MIN_LEN) { - return UDS_ERR_RESP_TOO_SHORT; - } - resp->routineControlType = client->recv_buf[1]; - resp->routineIdentifier = (client->recv_buf[2] << 8) + client->recv_buf[3]; - resp->routineStatusRecordLength = client->recv_size - UDS_0X31_RESP_MIN_LEN; - resp->routineStatusRecord = - resp->routineStatusRecordLength == 0 ? NULL : &client->recv_buf[UDS_0X31_RESP_MIN_LEN]; - return UDS_OK; -} - -/** - * @brief - * - * @param client - * @param resp - * @return UDSErr_t - * @addtogroup requestDownload_0x34 - */ -UDSErr_t UDSUnpackRequestDownloadResponse(const UDSClient_t *client, - struct RequestDownloadResponse *resp) { - assert(client); - assert(resp); - if (UDS_RESPONSE_SID_OF(kSID_REQUEST_DOWNLOAD) != client->recv_buf[0]) { - return UDS_ERR_SID_MISMATCH; - } - if (client->recv_size < UDS_0X34_RESP_BASE_LEN) { - return UDS_ERR_RESP_TOO_SHORT; - } - uint8_t maxNumberOfBlockLengthSize = (client->recv_buf[1] & 0xF0) >> 4; - - if (sizeof(resp->maxNumberOfBlockLength) < maxNumberOfBlockLengthSize) { - UDS_DBG_PRINT("WARNING: sizeof(maxNumberOfBlockLength) > sizeof(size_t)"); - return UDS_ERR; - } - resp->maxNumberOfBlockLength = 0; - for (int byteIdx = 0; byteIdx < maxNumberOfBlockLengthSize; byteIdx++) { - uint8_t byte = client->recv_buf[UDS_0X34_RESP_BASE_LEN + byteIdx]; - uint8_t shiftBytes = maxNumberOfBlockLengthSize - 1 - byteIdx; - resp->maxNumberOfBlockLength |= byte << (8 * shiftBytes); - } - return UDS_OK; -} - -bool UDSClientPoll(UDSClient_t *client) { - PollLowLevel(client); - - if (client->err) { - return UDS_CLIENT_IDLE; - } - - if (kRequestStateIdle != client->state) { - return UDS_CLIENT_RUNNING; - } - - if (NULL == client->cbList) { - return UDS_CLIENT_IDLE; - } - - UDSClientCallback activeCallback = client->cbList[client->cbIdx]; - - if (NULL == activeCallback) { - return UDS_CLIENT_IDLE; - } - - UDSSeqState_t state = activeCallback(client); - - switch (state) { - case UDSSeqStateDone: - return UDS_CLIENT_IDLE; - case UDSSeqStateRunning: - return UDS_CLIENT_RUNNING; - case UDSSeqStateGotoNext: { - client->cbIdx += 1; - return UDS_CLIENT_RUNNING; - } - default: - assert(0); - return UDS_CLIENT_IDLE; - } -} - -void UDSClientPoll2(UDSClient_t *client, - int (*fn)(UDSClient_t *client, UDSEvent_t evt, void *ev_data, void *fn_data), - void *fn_data) { - UDSClientPoll(client); -} - -UDSSeqState_t UDSClientAwaitIdle(UDSClient_t *client) { - if (client->err) { - return UDSSeqStateDone; - } else if (kRequestStateIdle == client->state) { - return UDSSeqStateGotoNext; - } else { - return UDSSeqStateRunning; - } -} - -UDSErr_t UDSUnpackRDBIResponse(const uint8_t *buf, size_t buf_len, uint16_t did, uint8_t *data, - uint16_t data_size, uint16_t *offset) { - assert(buf); - assert(data); - assert(offset); - if (0 == *offset) { - *offset = UDS_0X22_RESP_BASE_LEN; - } - - if (*offset + sizeof(did) > buf_len) { - return UDS_ERR_RESP_TOO_SHORT; - } - - uint16_t theirDID = (buf[*offset] << 8) + buf[*offset + 1]; - if (theirDID != did) { - return UDS_ERR_DID_MISMATCH; - } - - if (*offset + sizeof(uint16_t) + data_size > buf_len) { - return UDS_ERR_RESP_TOO_SHORT; - } - - memmove(data, buf + *offset + sizeof(uint16_t), data_size); - - *offset += sizeof(uint16_t) + data_size; - return UDS_OK; -} diff --git a/iso14229.h b/iso14229.h deleted file mode 100644 index becaa79..0000000 --- a/iso14229.h +++ /dev/null @@ -1,716 +0,0 @@ -/** - * @file iso14229.h - * @brief ISO-14229 (UDS) server and client - * @author driftregion - * @version 0.6.0 - * @date 2022-12-08 - */ - -#ifndef ISO14229_H -#define ISO14229_H - -#ifdef __cplusplus -extern "C" { -#define _Static_assert static_assert -#endif - -#define UDS_ARCH_CUSTOM 0 -#define UDS_ARCH_UNIX 1 -#define UDS_ARCH_WINDOWS 2 - -#define UDS_TP_CUSTOM 0 // bring your own transport layer -#define UDS_TP_ISOTP_C 1 // use isotp-c -#define UDS_TP_ISOTP_SOCKET 2 // use linux ISO-TP socket - -#if !defined(UDS_ARCH) -#if defined(__unix__) || defined(__APPLE__) -#define UDS_ARCH UDS_ARCH_UNIX -#elif defined(_WIN32) -#define UDS_ARCH UDS_ARCH_WINDOWS -#else -#define UDS_ARCH UDS_ARCH_CUSTOM -#endif -#endif - -#if !defined(UDS_TP) -#if (UDS_ARCH == UDS_ARCH_UNIX) -#define UDS_TP UDS_TP_ISOTP_SOCKET -#endif -#endif - -#include -#include -#include -#include -#include - -#if (UDS_TP == UDS_TP_ISOTP_C) -#include "isotp-c/isotp.h" -#include "isotp-c/isotp_config.h" -#include "isotp-c/isotp_defines.h" -#include "isotp-c/isotp_user.h" -#elif (UDS_TP == UDS_TP_ISOTP_SOCKET) -#include -#include -#include -#include -#include -#include -#include -#include -#include -#endif - -#if (UDS_ARCH == UDS_ARCH_UNIX) -#include -#include -#include -#elif (UDS_ARCH == UDS_ARCH_WINDOWS) -#include -typedef SSIZE_T ssize_t; -#endif - -/** ISO-TP Maximum Transmissiable Unit (ISO-15764-2-2004 section 5.3.3) */ -#define UDS_ISOTP_MTU (4095) - -#ifndef UDS_TP_MTU -#define UDS_TP_MTU UDS_ISOTP_MTU -#endif - -/** Default buffer size */ -#define UDS_BUFSIZE UDS_TP_MTU - -/* -provide a debug function with -DUDS_DBG_PRINT=printf when compiling this -library -*/ -#ifndef UDS_DBG_PRINT -#define UDS_DBG_PRINT(fmt, ...) ((void)fmt) -#endif - -#define UDS_DBG_PRINTHEX(addr, len) \ - for (int i = 0; i < len; i++) { \ - UDS_DBG_PRINT("%02x,", ((uint8_t *)addr)[i]); \ - } \ - UDS_DBG_PRINT("\n"); - -typedef enum { - UDS_ERR = -1, // 通用错误 - UDS_OK = 0, // 成功 - UDS_ERR_TIMEOUT, // 请求超时 - UDS_ERR_NEG_RESP, // 否定响应 - UDS_ERR_DID_MISMATCH, // 响应DID对不上期待的DID - UDS_ERR_SID_MISMATCH, // 请求和响应SID对不上 - UDS_ERR_SUBFUNCTION_MISMATCH, // 请求和响应SubFunction对不上 - UDS_ERR_TPORT, // 传输层错误 - UDS_ERR_FILE_IO, // 文件IO错误 - UDS_ERR_RESP_TOO_SHORT, // 响应太短 - UDS_ERR_BUFSIZ, // 缓冲器不够大 - UDS_ERR_INVALID_ARG, // 参数不对、没发 - UDS_ERR_BUSY, // 正在忙、没发 -} UDSErr_t; - -typedef enum { - UDSSeqStateDone = 0, - UDSSeqStateRunning = 1, - UDSSeqStateGotoNext = 2, -} UDSSeqState_t; - -enum UDSDiagnosticSessionType { - kDefaultSession = 0x01, - kProgrammingSession = 0x02, - kExtendedDiagnostic = 0x03, - kSafetySystemDiagnostic = 0x04, -}; - -enum { - kPositiveResponse = 0, - kGeneralReject = 0x10, - kServiceNotSupported = 0x11, - kSubFunctionNotSupported = 0x12, - kIncorrectMessageLengthOrInvalidFormat = 0x13, - kResponseTooLong = 0x14, - kBusyRepeatRequest = 0x21, - kConditionsNotCorrect = 0x22, - kRequestSequenceError = 0x24, - kNoResponseFromSubnetComponent = 0x25, - kFailurePreventsExecutionOfRequestedAction = 0x26, - kRequestOutOfRange = 0x31, - kSecurityAccessDenied = 0x33, - kInvalidKey = 0x35, - kExceedNumberOfAttempts = 0x36, - kRequiredTimeDelayNotExpired = 0x37, - kUploadDownloadNotAccepted = 0x70, - kTransferDataSuspended = 0x71, - kGeneralProgrammingFailure = 0x72, - kWrongBlockSequenceCounter = 0x73, - kRequestCorrectlyReceived_ResponsePending = 0x78, - kSubFunctionNotSupportedInActiveSession = 0x7E, - kServiceNotSupportedInActiveSession = 0x7F, - kRpmTooHigh = 0x81, - kRpmTooLow = 0x82, - kEngineIsRunning = 0x83, - kEngineIsNotRunning = 0x84, - kEngineRunTimeTooLow = 0x85, - kTemperatureTooHigh = 0x86, - kTemperatureTooLow = 0x87, - kVehicleSpeedTooHigh = 0x88, - kVehicleSpeedTooLow = 0x89, - kThrottlePedalTooHigh = 0x8A, - kThrottlePedalTooLow = 0x8B, - kTransmissionRangeNotInNeutral = 0x8C, - kTransmissionRangeNotInGear = 0x8D, - kISOSAEReserved = 0x8E, - kBrakeSwitchNotClosed = 0x8F, - kShifterLeverNotInPark = 0x90, - kTorqueConverterClutchLocked = 0x91, - kVoltageTooHigh = 0x92, - kVoltageTooLow = 0x93, -}; - -/** - * @brief LEV_RT_ - * @addtogroup ecuReset_0x11 - */ -enum UDSECUResetType { - kHardReset = 1, - kKeyOffOnReset = 2, - kSoftReset = 3, - kEnableRapidPowerShutDown = 4, - kDisableRapidPowerShutDown = 5, -}; - -typedef uint8_t UDSECUReset_t; - -/** - * @addtogroup securityAccess_0x27 - */ -enum UDSSecurityAccessType { - kRequestSeed = 0x01, - kSendKey = 0x02, -}; - -/** - * @addtogroup communicationControl_0x28 - */ -enum UDSCommunicationControlType { - kEnableRxAndTx = 0, - kEnableRxAndDisableTx = 1, - kDisableRxAndEnableTx = 2, - kDisableRxAndTx = 3, -}; - -/** - * @addtogroup communicationControl_0x28 - */ -enum UDSCommunicationType { - kNormalCommunicationMessages = 0x1, - kNetworkManagementCommunicationMessages = 0x2, - kNetworkManagementCommunicationMessagesAndNormalCommunicationMessages = 0x3, -}; - -/** - * @addtogroup routineControl_0x31 - */ -enum RoutineControlType { - kStartRoutine = 1, - kStopRoutine = 2, - kRequestRoutineResults = 3, -}; - -/** - * @addtogroup controlDTCSetting_0x85 - */ -enum DTCSettingType { - kDTCSettingON = 0x01, - kDTCSettingOFF = 0x02, -}; - -enum UDSTpStatusFlags { - UDS_TP_IDLE = 0x00000000, - UDS_TP_SEND_IN_PROGRESS = 0x00000001, - UDS_TP_RECV_COMPLETE = 0x00000002, -}; - -typedef uint32_t UDSTpStatus_t; - -typedef enum { - UDS_A_MTYPE_DIAG = 0, - UDS_A_MTYPE_REMOTE_DIAG, - UDS_A_MTYPE_SECURE_DIAG, - UDS_A_MTYPE_SECURE_REMOTE_DIAG, -} UDS_A_Mtype_t; - -typedef enum { - UDS_A_TA_TYPE_PHYSICAL = 0, // unicast (1:1) - UDS_A_TA_TYPE_FUNCTIONAL, // multicast -} UDS_A_TA_Type_t; - -typedef uint8_t UDSTpAddr_t; - -/** - * @brief Service data unit (SDU) - * @details data interface between the application layer and the transport layer - */ -typedef struct { - UDS_A_Mtype_t A_Mtype; // message type (diagnostic, remote diagnostic, secure diagnostic, secure - // remote diagnostic) - uint16_t A_SA; // application source address - uint16_t A_TA; // application target address - UDS_A_TA_Type_t A_TA_Type; // application target address type (physical or functional) - uint16_t A_AE; // application layer remote address -} UDSSDU_t; - -#define UDS_TP_NOOP_ADDR (0xFFFFFFFF) - -/** - * @brief Interface to OSI layer 4 (transport layer) - * @note implementers should embed this struct at offset zero in their own transport layer handle - */ -typedef struct UDSTpHandle { - /** - * @brief Get the transport layer's send buffer - * @param hdl: pointer to transport handle - * @param buf: double pointer which will be pointed to the send buffer - * @return size of transport layer's send buffer on success, -1 on error - */ - ssize_t (*get_send_buf)(struct UDSTpHandle *hdl, uint8_t **p_buf); - - /** - * @brief Send the data in the buffer buf - * @param hdl: pointer to transport handle - * @param buf: a pointer to the data to send (this may be the buffer returned by @ref - * get_send_buf) - * @param info: pointer to SDU info (may be NULL). If NULL, implementation should send with - * physical addressing - */ - ssize_t (*send)(struct UDSTpHandle *hdl, uint8_t *buf, size_t len, UDSSDU_t *info); - - /** - * @brief Poll the transport layer. - * @param hdl: pointer to transport handle - * @note the transport layer user is responsible for calling this function periodically - * @note threaded implementations like linux isotp sockets don't need to do anything here. - * @return UDS_TP_IDLE if idle, otherwise UDS_TP_SEND_IN_PROGRESS or UDS_TP_RECV_COMPLETE - */ - UDSTpStatus_t (*poll)(struct UDSTpHandle *hdl); - - /** - * @brief Peek at the received data - * @param hdl: pointer to transport handle - * @param buf: set to the received data - * @param info: filled with SDU info by the callee if not NULL - * @return size of received data on success, -1 on error - * @note The transport will be unable to receive further data until @ref ack_recv is called - * @note The information returned by peek will not change until @ref ack_recv is called - */ - ssize_t (*peek)(struct UDSTpHandle *hdl, uint8_t **buf, UDSSDU_t *info); - - /** - * @brief Acknowledge that the received data has been processed and may be discarded - * @param hdl: pointer to transport handle - * @note: after ack_recv() is called and before new messages are received, peek must return 0. - */ - void (*ack_recv)(struct UDSTpHandle *hdl); -} UDSTpHandle_t; - -// -// Convenience functions to wrap UDSTpHandle_t -// -ssize_t UDSTpGetSendBuf(UDSTpHandle_t *hdl, uint8_t **buf); -ssize_t UDSTpSend(UDSTpHandle_t *hdl, const uint8_t *buf, ssize_t len, UDSSDU_t *info); -UDSTpStatus_t UDSTpPoll(UDSTpHandle_t *hdl); -ssize_t UDSTpPeek(struct UDSTpHandle *hdl, uint8_t **buf, UDSSDU_t *info); -const uint8_t *UDSTpGetRecvBuf(UDSTpHandle_t *hdl, size_t *len); -size_t UDSTpGetRecvLen(UDSTpHandle_t *hdl); -void UDSTpAckRecv(UDSTpHandle_t *hdl); - -// ======================================================================== -// Utility Functions -// ======================================================================== - -/* returns true if `a` is after `b` */ -static inline bool UDSTimeAfter(uint32_t a, uint32_t b) { - return ((int32_t)((int32_t)(b) - (int32_t)(a)) < 0); -} - -/** - * @brief Get time in milliseconds - * @return current time in milliseconds - */ -uint32_t UDSMillis(void); - -// ======================================================================== -// Client -// ======================================================================== - -#ifndef UDS_CLIENT_DEFAULT_P2_MS -#define UDS_CLIENT_DEFAULT_P2_MS (150U) -#endif - -#ifndef UDS_CLIENT_DEFAULT_P2_STAR_MS -#define UDS_CLIENT_DEFAULT_P2_STAR_MS (1500U) -#endif - -_Static_assert(UDS_CLIENT_DEFAULT_P2_STAR_MS > UDS_CLIENT_DEFAULT_P2_MS, ""); - -enum UDSClientRequestState { - kRequestStateIdle = 0, // 完成 - kRequestStateSending, // 传输层现在传输数据 - kRequestStateAwaitSendComplete, // 等待传输发送完成 - kRequestStateAwaitResponse, // 等待响应 - kRequestStateProcessResponse, // 处理响应 -}; - -typedef uint8_t UDSClientRequestState_t; - -enum UDSClientOptions { - UDS_SUPPRESS_POS_RESP = 0x1, // 服务器不应该发送肯定响应 - UDS_FUNCTIONAL = 0x2, // 发功能请求 - UDS_NEG_RESP_IS_ERR = 0x4, // 否定响应是属于故障 - UDS_IGNORE_SRV_TIMINGS = 0x8, // 忽略服务器给的p2和p2_star -}; - -struct UDSClient; - -typedef UDSSeqState_t (*UDSClientCallback)(struct UDSClient *client); - -typedef struct UDSClient { - uint16_t p2_ms; // p2 超时时间 - uint32_t p2_star_ms; // 0x78 p2* 超时时间 - UDSTpHandle_t *tp; - - // 内状态 - uint32_t p2_timer; - uint8_t *recv_buf; - uint8_t *send_buf; - uint16_t recv_buf_size; - uint16_t send_buf_size; - uint16_t recv_size; - uint16_t send_size; - UDSErr_t err; - UDSClientRequestState_t state; - - uint8_t options; // enum udsclientoptions - uint8_t defaultOptions; // enum udsclientoptions - // a copy of the options at the time a request is made - uint8_t _options_copy; // enum udsclientoptions - int (*fn)(struct UDSClient *, int, void *, void *); - - const UDSClientCallback *cbList; // null-terminated list of callback functions - size_t cbIdx; // index of currently active callback function - void *cbData; // a pointer to data available to callbacks - -} UDSClient_t; - -struct SecurityAccessResponse { - uint8_t securityAccessType; - const uint8_t *securitySeed; - uint16_t securitySeedLength; -}; - -struct RequestDownloadResponse { - size_t maxNumberOfBlockLength; -}; - -struct RoutineControlResponse { - uint8_t routineControlType; - uint16_t routineIdentifier; - const uint8_t *routineStatusRecord; - uint16_t routineStatusRecordLength; -}; - -UDSErr_t UDSClientInit(UDSClient_t *client); - -#define UDS_CLIENT_IDLE (0) -#define UDS_CLIENT_RUNNING (1) - -/** - * @brief poll the client (call this in a loop) - * @param client - * @return UDS_CLIENT_IDLE if idle, otherwise UDS_CLIENT_RUNNING - */ -bool UDSClientPoll(UDSClient_t *client); -void UDSClientPoll2(UDSClient_t *client, - int (*fn)(UDSClient_t *client, int evt, void *ev_data, void *fn_data), - void *fn_data); - -UDSErr_t UDSSendBytes(UDSClient_t *client, const uint8_t *data, uint16_t size); -UDSErr_t UDSSendECUReset(UDSClient_t *client, UDSECUReset_t type); -UDSErr_t UDSSendDiagSessCtrl(UDSClient_t *client, enum UDSDiagnosticSessionType mode); -UDSErr_t UDSSendSecurityAccess(UDSClient_t *client, uint8_t level, uint8_t *data, uint16_t size); -UDSErr_t UDSSendCommCtrl(UDSClient_t *client, enum UDSCommunicationControlType ctrl, - enum UDSCommunicationType comm); -UDSErr_t UDSSendRDBI(UDSClient_t *client, const uint16_t *didList, - const uint16_t numDataIdentifiers); -UDSErr_t UDSSendWDBI(UDSClient_t *client, uint16_t dataIdentifier, const uint8_t *data, - uint16_t size); -UDSErr_t UDSSendTesterPresent(UDSClient_t *client); -UDSErr_t UDSSendRoutineCtrl(UDSClient_t *client, enum RoutineControlType type, - uint16_t routineIdentifier, const uint8_t *data, uint16_t size); - -UDSErr_t UDSSendRequestDownload(UDSClient_t *client, uint8_t dataFormatIdentifier, - uint8_t addressAndLengthFormatIdentifier, size_t memoryAddress, - size_t memorySize); - -UDSErr_t UDSSendRequestUpload(UDSClient_t *client, uint8_t dataFormatIdentifier, - uint8_t addressAndLengthFormatIdentifier, size_t memoryAddress, - size_t memorySize); -UDSErr_t UDSSendTransferData(UDSClient_t *client, uint8_t blockSequenceCounter, - const uint16_t blockLength, const uint8_t *data, uint16_t size); -UDSErr_t UDSSendTransferDataStream(UDSClient_t *client, uint8_t blockSequenceCounter, - const uint16_t blockLength, FILE *fd); -UDSErr_t UDSSendRequestTransferExit(UDSClient_t *client); - -UDSErr_t UDSCtrlDTCSetting(UDSClient_t *client, uint8_t dtcSettingType, - uint8_t *dtcSettingControlOptionRecord, uint16_t len); -UDSErr_t UDSUnpackRDBIResponse(const uint8_t *buf, size_t buf_len, uint16_t did, uint8_t *data, - uint16_t size, uint16_t *offset); -UDSErr_t UDSUnpackSecurityAccessResponse(const UDSClient_t *client, - struct SecurityAccessResponse *resp); -UDSErr_t UDSUnpackRequestDownloadResponse(const UDSClient_t *client, - struct RequestDownloadResponse *resp); -UDSErr_t UDSUnpackRoutineControlResponse(const UDSClient_t *client, - struct RoutineControlResponse *resp); - -/** - * @brief Wait after request transmission for a response to be received - * @note if suppressPositiveResponse is set, this function will return - UDSSeqStateGotoNext as soon as the transport layer has completed transmission. - * - * @param client - * @param args - * @return UDSErr_t - - UDSSeqStateDone -- 流程完成 - - UDSSeqStateRunning -- 流程正在跑、还没完成 - */ -UDSSeqState_t UDSClientAwaitIdle(UDSClient_t *client); - -UDSErr_t UDSConfigDownload(UDSClient_t *client, uint8_t dataFormatIdentifier, - uint8_t addressAndLengthFormatIdentifier, size_t memoryAddress, - size_t memorySize, FILE *fd); - -// ======================================================================== -// Server -// ======================================================================== - -#ifndef UDS_SERVER_DEFAULT_POWER_DOWN_TIME_MS -#define UDS_SERVER_DEFAULT_POWER_DOWN_TIME_MS (10) -#endif - -#ifndef UDS_SERVER_DEFAULT_P2_MS -#define UDS_SERVER_DEFAULT_P2_MS (50) -#endif - -#ifndef UDS_SERVER_DEFAULT_P2_STAR_MS -#define UDS_SERVER_DEFAULT_P2_STAR_MS (2000) -#endif - -#ifndef UDS_SERVER_DEFAULT_S3_MS -#define UDS_SERVER_DEFAULT_S3_MS (3000) -#endif - -_Static_assert(0 < UDS_SERVER_DEFAULT_P2_MS && - UDS_SERVER_DEFAULT_P2_MS < UDS_SERVER_DEFAULT_P2_STAR_MS && - UDS_SERVER_DEFAULT_P2_STAR_MS < UDS_SERVER_DEFAULT_S3_MS, - ""); - -#ifndef UDS_SERVER_DEFAULT_XFER_DATA_MAX_BLOCKLENGTH -/*! ISO14229-1:2013 Table 396. This parameter is used by the requestDownload positive response -message to inform the client how many data bytes (maxNumberOfBlockLength) to include in each -TransferData request message from the client. */ -#define UDS_SERVER_DEFAULT_XFER_DATA_MAX_BLOCKLENGTH (UDS_BUFSIZE) -#endif - -enum UDSServerEvent { - UDS_SRV_EVT_DiagSessCtrl, // UDSDiagSessCtrlArgs_t * - UDS_SRV_EVT_EcuReset, // UDSECUResetArgs_t * - UDS_SRV_EVT_ReadDataByIdent, // UDSRDBIArgs_t * - UDS_SRV_EVT_ReadMemByAddr, // UDSReadMemByAddrArgs_t * - UDS_SRV_EVT_CommCtrl, // UDSCommCtrlArgs_t * - UDS_SRV_EVT_SecAccessRequestSeed, // UDSSecAccessRequestSeedArgs_t * - UDS_SRV_EVT_SecAccessValidateKey, // UDSSecAccessValidateKeyArgs_t * - UDS_SRV_EVT_WriteDataByIdent, // UDSWDBIArgs_t * - UDS_SRV_EVT_RoutineCtrl, // UDSRoutineCtrlArgs_t* - UDS_SRV_EVT_RequestDownload, // UDSRequestDownloadArgs_t* - UDS_SRV_EVT_RequestUpload, // UDSRequestUploadArgs_t * - UDS_SRV_EVT_TransferData, // UDSTransferDataArgs_t * - UDS_SRV_EVT_RequestTransferExit, // UDSRequestTransferExitArgs_t * - UDS_SRV_EVT_SessionTimeout, // NULL - UDS_SRV_EVT_DoScheduledReset, // enum UDSEcuResetType * - UDS_SRV_EVT_Err, // UDSErr_t * - UDS_EVT_IDLE, - UDS_EVT_RESP_RECV, -}; - -typedef int UDSServerEvent_t; -typedef UDSServerEvent_t UDSEvent_t; - -/** - * @brief Server request context - */ -typedef struct { - uint8_t *recv_buf; - uint8_t *send_buf; - size_t recv_len; - size_t send_len; - size_t send_buf_size; - UDSSDU_t info; -} UDSReq_t; - -typedef struct UDSServer { - UDSTpHandle_t *tp; - uint8_t (*fn)(struct UDSServer *srv, UDSServerEvent_t event, const void *arg); - - /** - * @brief \~chinese 服务器时间参数（毫秒） \~ Server time constants (milliseconds) \~ - */ - uint16_t p2_ms; // Default P2_server_max timing supported by the server for - // the activated diagnostic session. - uint32_t p2_star_ms; // Enhanced (NRC 0x78) P2_server_max supported by the - // server for the activated diagnostic session. - uint16_t s3_ms; // Session timeout - - uint8_t ecuResetScheduled; // nonzero indicates that an ECUReset has been scheduled - uint32_t ecuResetTimer; // for delaying resetting until a response - // has been sent to the client - uint32_t p2_timer; // for rate limiting server responses - uint32_t s3_session_timeout_timer; // indicates that diagnostic session has timed out - - /** - * @brief UDS-1-2013: Table 407 - 0x36 TransferData Supported negative - * response codes requires that the server keep track of whether the - * transfer is active - */ - bool xferIsActive; - // UDS-1-2013: 14.4.2.3, Table 404: The blockSequenceCounter parameter - // value starts at 0x01 - uint8_t xferBlockSequenceCounter; - size_t xferTotalBytes; // total transfer size in bytes requested by the client - size_t xferByteCounter; // total number of bytes transferred - size_t xferBlockLength; // block length (convenience for the TransferData API) - - uint8_t sessionType; // diagnostic session type (0x10) - uint8_t securityLevel; // SecurityAccess (0x27) level - - bool RCRRP; // set to true when user fn returns 0x78 and false otherwise - bool requestInProgress; // set to true when a request has been processed but the response has - // not yet been sent - - // UDS-1 2013 defines the following conditions under which the server does not - // process incoming requests: - // - not ready to receive (Table A.1 0x78) - // - not accepting request messages and not sending responses (9.3.1) - // - // when this variable is set to true, incoming ISO-TP data will not be processed. - bool notReadyToReceive; - - UDSReq_t r; -} UDSServer_t; - -// TODO: Remove -typedef struct { - uint8_t (*fn)(UDSServer_t *srv, UDSServerEvent_t event, const void *arg); - UDSTpHandle_t *tp; -} UDSServerConfig_t; - -typedef struct { - const uint8_t type; /*! requested diagnostic session type (enum UDSDiagnosticSessionType) */ - uint16_t p2_ms; /*! optional: p2 timing override */ - uint32_t p2_star_ms; /*! optional: p2* timing override */ -} UDSDiagSessCtrlArgs_t; - -typedef struct { - const uint8_t type; /**< \~chinese 客户端请求的复位类型 \~english reset type requested by client - (enum UDSECUResetType) */ - uint32_t powerDownTimeMillis; /**< when this much time has elapsed after a kPositiveResponse, a - UDS_SRV_EVT_DoScheduledReset will be issued */ -} UDSECUResetArgs_t; - -typedef struct { - const uint16_t dataId; /*! RDBI Data Identifier */ - uint8_t (*copy)(UDSServer_t *srv, const void *src, - uint16_t count); /*! function for copying data */ -} UDSRDBIArgs_t; - -typedef struct { - const void *memAddr; - const size_t memSize; - uint8_t (*copy)(UDSServer_t *srv, const void *src, - uint16_t count); /*! function for copying data */ -} UDSReadMemByAddrArgs_t; - -typedef struct { - uint8_t ctrlType; /* enum UDSCommunicationControlType */ - uint8_t commType; /* enum UDSCommunicationType */ -} UDSCommCtrlArgs_t; - -typedef struct { - const uint8_t level; /*! requested security level */ - const uint8_t *const dataRecord; /*! pointer to request data */ - const uint16_t len; /*! size of request data */ - uint8_t (*copySeed)(UDSServer_t *srv, const void *src, - uint16_t len); /*! function for copying data */ -} UDSSecAccessRequestSeedArgs_t; - -typedef struct { - const uint8_t level; /*! security level to be validated */ - const uint8_t *const key; /*! key sent by client */ - const uint16_t len; /*! length of key */ -} UDSSecAccessValidateKeyArgs_t; - -typedef struct { - const uint16_t dataId; /*! WDBI Data Identifier */ - const uint8_t *const data; /*! pointer to data */ - const uint16_t len; /*! length of data */ -} UDSWDBIArgs_t; - -typedef struct { - const uint8_t ctrlType; /*! routineControlType */ - const uint16_t id; /*! routineIdentifier */ - const uint8_t *optionRecord; /*! optional data */ - const uint16_t len; /*! length of optional data */ - uint8_t (*copyStatusRecord)(UDSServer_t *srv, const void *src, - uint16_t len); /*! function for copying response data */ -} UDSRoutineCtrlArgs_t; - -typedef struct { - const void *addr; /*! requested address */ - const size_t size; /*! requested download size */ - const uint8_t dataFormatIdentifier; /*! optional specifier for format of data */ - uint16_t maxNumberOfBlockLength; /*! optional response: inform client how many data bytes to - send in each `TransferData` request */ -} UDSRequestDownloadArgs_t; - -typedef struct { - const void *addr; /*! requested address */ - const size_t size; /*! requested download size */ - const uint8_t dataFormatIdentifier; /*! optional specifier for format of data */ - uint16_t maxNumberOfBlockLength; /*! optional response: inform client how many data bytes to - send in each `TransferData` request */ -} UDSRequestUploadArgs_t; - -typedef struct { - const uint8_t *const data; /*! transfer data */ - const uint16_t len; /*! transfer data length */ - const uint16_t maxRespLen; /*! don't send more than this many bytes with copyResponse */ - uint8_t (*copyResponse)( - UDSServer_t *srv, const void *src, - uint16_t len); /*! function for copying transfer data response data (optional) */ -} UDSTransferDataArgs_t; - -typedef struct { - const uint8_t *const data; /*! request data */ - const uint16_t len; /*! request data length */ - uint8_t (*copyResponse)(UDSServer_t *srv, const void *src, - uint16_t len); /*! function for copying response data (optional) */ -} UDSRequestTransferExitArgs_t; - -UDSErr_t UDSServerInit(UDSServer_t *srv); -void UDSServerPoll(UDSServer_t *srv); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/run_gdb.py b/run_gdb.py new file mode 100755 index 0000000..a9056cc --- /dev/null +++ b/run_gdb.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3 + +__doc__ = """ +Automates bazel, gdb, and optionally qemu to accelerate debugging + +the `bazel test` command explicitly prohibits interactive debugging, +so this script exists to automate the debugging process. +""" + +import subprocess +import argparse + +GDB = "gdb-multiarch" +QEMU = "qemu-arm" +QEMU_LDFLAGS = "/usr/arm-linux-gnueabihf/" +GDB_PORT = 1234 + +parser = argparse.ArgumentParser(description=__doc__) +parser.add_argument("test", help="The test to debug") +parser.add_argument("--config", help="bazel config", default="") +args = parser.parse_args() +gdb_args = [GDB, "-q", "-ex", f"file bazel-bin/test/{args.test}"] + +print(f"building {args.test}...") +bazel_args = ["bazel", "build", "-c", "dbg", "--copt=-g", f"//test:{args.test}"] + +subprocess.check_call(bazel_args) +procs = [] + + +if args.config: + qemu = subprocess.Popen( + [QEMU, "-g", str(GDB_PORT), + "-L", "/usr/arm-linux-gnueabihf/", + f"bazel-bin/test/{args.test}"], + ) + + if qemu.returncode is not None: + exit(qemu.returncode) + + procs.append(qemu) + gdb_args += ["-ex", f"target remote localhost:{GDB_PORT}"] + + +try: + gdb = subprocess.Popen(gdb_args) + procs.append(gdb) + gdb.wait() +except KeyboardInterrupt: + pass +finally: + [p.kill() for p in procs] + exit(gdb.returncode) diff --git a/src/client.c b/src/client.c new file mode 100644 index 0000000..1351727 --- /dev/null +++ b/src/client.c @@ -0,0 +1,856 @@ +#include "client.h" +#include "config.h" +#include "util.h" + +static void clearRequestContext(UDSClient_t *client) { + assert(client); + client->recv_size = 0; + client->send_size = 0; + client->state = kRequestStateIdle; + client->err = UDS_OK; +} + +UDSErr_t UDSClientInit(UDSClient_t *client) { + assert(client); + memset(client, 0, sizeof(*client)); + + client->p2_ms = UDS_CLIENT_DEFAULT_P2_MS; + client->p2_star_ms = UDS_CLIENT_DEFAULT_P2_STAR_MS; + + if (client->p2_star_ms < client->p2_ms) { + fprintf(stderr, "p2_star_ms must be >= p2_ms\n"); + client->p2_star_ms = client->p2_ms; + } + + clearRequestContext(client); + return UDS_OK; +} + +static const char *ClientStateName(enum UDSClientRequestState state) { + switch (state) { + case kRequestStateIdle: + return "Idle"; + case kRequestStateSending: + return "Sending"; + case kRequestStateAwaitSendComplete: + return "AwaitSendComplete"; + case kRequestStateAwaitResponse: + return "AwaitResponse"; + case kRequestStateProcessResponse: + return "ProcessResponse"; + default: + return "Unknown"; + } +} + +static void changeState(UDSClient_t *client, enum UDSClientRequestState state) { + printf("client state: %s (%d) -> %s (%d)\n", ClientStateName(client->state), client->state, + ClientStateName(state), state); + client->state = state; +} + +/** + * @brief Check that the response is a valid UDS response + * + * @param ctx + * @return UDSErr_t + */ +static UDSErr_t _ClientValidateResponse(const UDSClient_t *client) { + + if (client->recv_size < 1) { + return UDS_ERR_RESP_TOO_SHORT; + } + + if (0x7F == client->recv_buf[0]) { // 否定响应 + if (client->recv_size < 2) { + return UDS_ERR_RESP_TOO_SHORT; + } else if (client->send_buf[0] != client->recv_buf[1]) { + return UDS_ERR_SID_MISMATCH; + } else if (kRequestCorrectlyReceived_ResponsePending == client->recv_buf[2]) { + return UDS_OK; + } else if (client->_options_copy & UDS_NEG_RESP_IS_ERR) { + return UDS_ERR_NEG_RESP; + } else { + ; + } + } else { // 肯定响应 + if (UDS_RESPONSE_SID_OF(client->send_buf[0]) != client->recv_buf[0]) { + return UDS_ERR_SID_MISMATCH; + } + switch (client->send_buf[0]) { + case kSID_ECU_RESET: + if (client->recv_size < 2) { + return UDS_ERR_RESP_TOO_SHORT; + } else if (client->send_buf[1] != client->recv_buf[1]) { + return UDS_ERR_SUBFUNCTION_MISMATCH; + } else { + ; + } + break; + } + } + + return UDS_OK; +} + +/** + * @brief Handle validated server response + * @param client + */ +static inline void _ClientHandleResponse(UDSClient_t *client) { + if (0x7F == client->recv_buf[0]) { + if (kRequestCorrectlyReceived_ResponsePending == client->recv_buf[2]) { + UDS_DBG_PRINT("got RCRRP, setting p2 timer\n"); + client->p2_timer = UDSMillis() + client->p2_star_ms; + memset(client->recv_buf, 0, client->recv_buf_size); + client->recv_size = 0; + UDSTpAckRecv(client->tp); + changeState(client, kRequestStateAwaitResponse); + return; + } else { + ; + } + } else { + uint8_t respSid = client->recv_buf[0]; + switch (UDS_REQUEST_SID_OF(respSid)) { + case kSID_DIAGNOSTIC_SESSION_CONTROL: { + if (client->recv_size < UDS_0X10_RESP_LEN) { + UDS_DBG_PRINT("Error: SID %x response too short\n", + kSID_DIAGNOSTIC_SESSION_CONTROL); + client->err = UDS_ERR_RESP_TOO_SHORT; + UDSTpAckRecv(client->tp); + changeState(client, kRequestStateIdle); + return; + } + + if (client->_options_copy & UDS_IGNORE_SRV_TIMINGS) { + UDSTpAckRecv(client->tp); + changeState(client, kRequestStateIdle); + return; + } + + uint16_t p2 = (client->recv_buf[2] << 8) + client->recv_buf[3]; + uint32_t p2_star = ((client->recv_buf[4] << 8) + client->recv_buf[5]) * 10; + UDS_DBG_PRINT("received new timings: p2: %"PRIu16", p2*: %"PRIu32"\n", p2, p2_star); + client->p2_ms = p2; + client->p2_star_ms = p2_star; + break; + } + default: + break; + } + } + UDSTpAckRecv(client->tp); + changeState(client, kRequestStateIdle); +} + +/** + * @brief execute the client request state machine + * @param client + */ +static void PollLowLevel(UDSClient_t *client) { + assert(client); + UDSTpStatus_t tp_status = client->tp->poll(client->tp); + switch (client->state) { + case kRequestStateIdle: { + client->options = client->defaultOptions; + break; + } + case kRequestStateSending: { + UDSTpAddr_t ta_type = client->_options_copy & UDS_FUNCTIONAL ? UDS_A_TA_TYPE_FUNCTIONAL + : UDS_A_TA_TYPE_PHYSICAL; + UDSSDU_t info = { + .A_Mtype = UDS_A_MTYPE_DIAG, + .A_TA_Type = ta_type, + }; + ssize_t ret = UDSTpSend(client->tp, client->send_buf, client->send_size, &info); + if (ret < 0) { + client->err = UDS_ERR_TPORT; + UDS_DBG_PRINT("tport err: %zd\n", ret); + } else if (0 == ret) { + UDS_DBG_PRINT("send in progress...\n"); + ; // 等待发送成功 + } else if (client->send_size == ret) { + changeState(client, kRequestStateAwaitSendComplete); + } else { + client->err = UDS_ERR_BUFSIZ; + } + break; + } + case kRequestStateAwaitSendComplete: { + if (client->_options_copy & UDS_FUNCTIONAL) { + // "The Functional addressing is applied only to single frame transmission" + // Specification of Diagnostic Communication (Diagnostic on CAN - Network Layer) + changeState(client, kRequestStateIdle); + } + if (tp_status & UDS_TP_SEND_IN_PROGRESS) { + ; // await send complete + } else { + if (client->_options_copy & UDS_SUPPRESS_POS_RESP) { + changeState(client, kRequestStateIdle); + } else { + changeState(client, kRequestStateAwaitResponse); + client->p2_timer = UDSMillis() + client->p2_ms; + } + } + break; + } + case kRequestStateAwaitResponse: { + UDSSDU_t info = {0}; + ssize_t len = UDSTpPeek(client->tp, &client->recv_buf, &info); + + if (UDS_A_TA_TYPE_FUNCTIONAL == info.A_TA_Type) { + UDSTpAckRecv(client->tp); + break; + } + if (len < 0) { + client->err = UDS_ERR_TPORT; + changeState(client, kRequestStateIdle); + } else if (0 == len) { + if (UDSTimeAfter(UDSMillis(), client->p2_timer)) { + client->err = UDS_ERR_TIMEOUT; + changeState(client, kRequestStateIdle); + } + } else { + printf("received %zd bytes\n", len); + client->recv_size = len; + changeState(client, kRequestStateProcessResponse); + } + break; + } + case kRequestStateProcessResponse: { + client->err = _ClientValidateResponse(client); + if (UDS_OK == client->err) { + _ClientHandleResponse(client); + } else { + UDSTpAckRecv(client->tp); + changeState(client, kRequestStateIdle); + } + break; + } + + default: + assert(0); + } +} + +static UDSErr_t _SendRequest(UDSClient_t *client) { + client->_options_copy = client->options; + + if (client->_options_copy & UDS_SUPPRESS_POS_RESP) { + // UDS-1:2013 8.2.2 Table 11 + client->send_buf[1] |= 0x80; + } + + changeState(client, kRequestStateSending); + PollLowLevel(client); // poll once to begin sending immediately + return UDS_OK; +} + +static UDSErr_t PreRequestCheck(UDSClient_t *client) { + if (kRequestStateIdle != client->state) { + return UDS_ERR_BUSY; + } + clearRequestContext(client); + if (client->tp == NULL) { + return UDS_ERR_TPORT; + } + ssize_t ret = UDSTpGetSendBuf(client->tp, &client->send_buf); + if (ret < 0) { + return UDS_ERR_TPORT; + } + client->send_buf_size = ret; + return UDS_OK; +} + +UDSErr_t UDSSendBytes(UDSClient_t *client, const uint8_t *data, uint16_t size) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + if (size > client->send_buf_size) { + return UDS_ERR_BUFSIZ; + } + memmove(client->send_buf, data, size); + client->send_size = size; + return _SendRequest(client); +} + +UDSErr_t UDSSendECUReset(UDSClient_t *client, UDSECUReset_t type) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + client->send_buf[0] = kSID_ECU_RESET; + client->send_buf[1] = type; + client->send_size = 2; + return _SendRequest(client); +} + +UDSErr_t UDSSendDiagSessCtrl(UDSClient_t *client, enum UDSDiagnosticSessionType mode) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + client->send_buf[0] = kSID_DIAGNOSTIC_SESSION_CONTROL; + client->send_buf[1] = mode; + client->send_size = 2; + return _SendRequest(client); +} + +UDSErr_t UDSSendCommCtrl(UDSClient_t *client, enum UDSCommunicationControlType ctrl, + enum UDSCommunicationType comm) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + client->send_buf[0] = kSID_COMMUNICATION_CONTROL; + client->send_buf[1] = ctrl; + client->send_buf[2] = comm; + client->send_size = 3; + return _SendRequest(client); +} + +UDSErr_t UDSSendTesterPresent(UDSClient_t *client) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + client->send_buf[0] = kSID_TESTER_PRESENT; + client->send_buf[1] = 0; + client->send_size = 2; + return _SendRequest(client); +} + +UDSErr_t UDSSendRDBI(UDSClient_t *client, const uint16_t *didList, + const uint16_t numDataIdentifiers) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + assert(didList); + assert(numDataIdentifiers); + client->send_buf[0] = kSID_READ_DATA_BY_IDENTIFIER; + for (int i = 0; i < numDataIdentifiers; i++) { + uint16_t offset = 1 + sizeof(uint16_t) * i; + if (offset + 2 > client->send_buf_size) { + return UDS_ERR_INVALID_ARG; + } + (client->send_buf + offset)[0] = (didList[i] & 0xFF00) >> 8; + (client->send_buf + offset)[1] = (didList[i] & 0xFF); + } + client->send_size = 1 + (numDataIdentifiers * sizeof(uint16_t)); + return _SendRequest(client); +} + +UDSErr_t UDSSendWDBI(UDSClient_t *client, uint16_t dataIdentifier, const uint8_t *data, + uint16_t size) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + assert(data); + assert(size); + client->send_buf[0] = kSID_WRITE_DATA_BY_IDENTIFIER; + if (client->send_buf_size <= 3 || size > client->send_buf_size - 3) { + return UDS_ERR_BUFSIZ; + } + client->send_buf[1] = (dataIdentifier & 0xFF00) >> 8; + client->send_buf[2] = (dataIdentifier & 0xFF); + memmove(&client->send_buf[3], data, size); + client->send_size = 3 + size; + return _SendRequest(client); +} + +/** + * @brief RoutineControl + * + * @param client + * @param type + * @param routineIdentifier + * @param data + * @param size + * @return UDSErr_t + * @addtogroup routineControl_0x31 + */ +UDSErr_t UDSSendRoutineCtrl(UDSClient_t *client, enum RoutineControlType type, + uint16_t routineIdentifier, const uint8_t *data, uint16_t size) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + client->send_buf[0] = kSID_ROUTINE_CONTROL; + client->send_buf[1] = type; + client->send_buf[2] = routineIdentifier >> 8; + client->send_buf[3] = routineIdentifier; + if (size) { + assert(data); + if (size > client->send_buf_size - UDS_0X31_REQ_MIN_LEN) { + return UDS_ERR_BUFSIZ; + } + memmove(&client->send_buf[UDS_0X31_REQ_MIN_LEN], data, size); + } else { + assert(NULL == data); + } + client->send_size = UDS_0X31_REQ_MIN_LEN + size; + return _SendRequest(client); +} + +/** + * @brief + * + * @param client + * @param dataFormatIdentifier + * @param addressAndLengthFormatIdentifier + * @param memoryAddress + * @param memorySize + * @return UDSErr_t + * @addtogroup requestDownload_0x34 + */ +UDSErr_t UDSSendRequestDownload(UDSClient_t *client, uint8_t dataFormatIdentifier, + uint8_t addressAndLengthFormatIdentifier, size_t memoryAddress, + size_t memorySize) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + uint8_t numMemorySizeBytes = (addressAndLengthFormatIdentifier & 0xF0) >> 4; + uint8_t numMemoryAddressBytes = addressAndLengthFormatIdentifier & 0x0F; + + client->send_buf[0] = kSID_REQUEST_DOWNLOAD; + client->send_buf[1] = dataFormatIdentifier; + client->send_buf[2] = addressAndLengthFormatIdentifier; + + uint8_t *ptr = &client->send_buf[UDS_0X34_REQ_BASE_LEN]; + + for (int i = numMemoryAddressBytes - 1; i >= 0; i--) { + *ptr = (memoryAddress & (0xFF << (8 * i))) >> (8 * i); + ptr++; + } + + for (int i = numMemorySizeBytes - 1; i >= 0; i--) { + *ptr = (memorySize & (0xFF << (8 * i))) >> (8 * i); + ptr++; + } + + client->send_size = UDS_0X34_REQ_BASE_LEN + numMemoryAddressBytes + numMemorySizeBytes; + return _SendRequest(client); +} + +/** + * @brief + * + * @param client + * @param dataFormatIdentifier + * @param addressAndLengthFormatIdentifier + * @param memoryAddress + * @param memorySize + * @return UDSErr_t + * @addtogroup requestDownload_0x35 + */ +UDSErr_t UDSSendRequestUpload(UDSClient_t *client, uint8_t dataFormatIdentifier, + uint8_t addressAndLengthFormatIdentifier, size_t memoryAddress, + size_t memorySize) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + uint8_t numMemorySizeBytes = (addressAndLengthFormatIdentifier & 0xF0) >> 4; + uint8_t numMemoryAddressBytes = addressAndLengthFormatIdentifier & 0x0F; + + client->send_buf[0] = kSID_REQUEST_UPLOAD; + client->send_buf[1] = dataFormatIdentifier; + client->send_buf[2] = addressAndLengthFormatIdentifier; + + uint8_t *ptr = &client->send_buf[UDS_0X35_REQ_BASE_LEN]; + + for (int i = numMemoryAddressBytes - 1; i >= 0; i--) { + *ptr = (memoryAddress & (0xFF << (8 * i))) >> (8 * i); + ptr++; + } + + for (int i = numMemorySizeBytes - 1; i >= 0; i--) { + *ptr = (memorySize & (0xFF << (8 * i))) >> (8 * i); + ptr++; + } + + client->send_size = UDS_0X35_REQ_BASE_LEN + numMemoryAddressBytes + numMemorySizeBytes; + return _SendRequest(client); +} + +/** + * @brief + * + * @param client + * @param blockSequenceCounter + * @param blockLength + * @param fd + * @return UDSErr_t + * @addtogroup transferData_0x36 + */ +UDSErr_t UDSSendTransferData(UDSClient_t *client, uint8_t blockSequenceCounter, + const uint16_t blockLength, const uint8_t *data, uint16_t size) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + assert(blockLength > 2); // blockLength must include SID and sequenceCounter + assert(size + 2 <= blockLength); // data must fit inside blockLength - 2 + client->send_buf[0] = kSID_TRANSFER_DATA; + client->send_buf[1] = blockSequenceCounter; + memmove(&client->send_buf[UDS_0X36_REQ_BASE_LEN], data, size); + UDS_DBG_PRINT("size: %d, blocklength: %d\n", size, blockLength); + client->send_size = UDS_0X36_REQ_BASE_LEN + size; + return _SendRequest(client); +} + +UDSErr_t UDSSendTransferDataStream(UDSClient_t *client, uint8_t blockSequenceCounter, + const uint16_t blockLength, FILE *fd) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + assert(blockLength > 2); // blockLength must include SID and sequenceCounter + client->send_buf[0] = kSID_TRANSFER_DATA; + client->send_buf[1] = blockSequenceCounter; + + uint16_t size = fread(&client->send_buf[2], 1, blockLength - 2, fd); + UDS_DBG_PRINT("size: %d, blocklength: %d\n", size, blockLength); + client->send_size = UDS_0X36_REQ_BASE_LEN + size; + return _SendRequest(client); +} + +/** + * @brief + * + * @param client + * @return UDSErr_t + * @addtogroup requestTransferExit_0x37 + */ +UDSErr_t UDSSendRequestTransferExit(UDSClient_t *client) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + client->send_buf[0] = kSID_REQUEST_TRANSFER_EXIT; + client->send_size = 1; + return _SendRequest(client); +} + +/** + * @brief + * + * @param client + * @param dtcSettingType + * @param data + * @param size + * @return UDSErr_t + * @addtogroup controlDTCSetting_0x85 + */ +UDSErr_t UDSCtrlDTCSetting(UDSClient_t *client, uint8_t dtcSettingType, uint8_t *data, + uint16_t size) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + if (0x00 == dtcSettingType || 0x7F == dtcSettingType || + (0x03 <= dtcSettingType && dtcSettingType <= 0x3F)) { + assert(0); // reserved vals + } + client->send_buf[0] = kSID_CONTROL_DTC_SETTING; + client->send_buf[1] = dtcSettingType; + + if (NULL == data) { + assert(size == 0); + } else { + assert(size > 0); + if (size > client->send_buf_size - 2) { + return UDS_ERR_BUFSIZ; + } + memmove(&client->send_buf[2], data, size); + } + client->send_size = 2 + size; + return _SendRequest(client); +} + +/** + * @brief + * + * @param client + * @param level + * @param data + * @param size + * @return UDSErr_t + * @addtogroup securityAccess_0x27 + */ +UDSErr_t UDSSendSecurityAccess(UDSClient_t *client, uint8_t level, uint8_t *data, uint16_t size) { + UDSErr_t err = PreRequestCheck(client); + if (err) { + return err; + } + if (UDSSecurityAccessLevelIsReserved(level)) { + return UDS_ERR_INVALID_ARG; + } + client->send_buf[0] = kSID_SECURITY_ACCESS; + client->send_buf[1] = level; + if (size) { + assert(data); + if (size > client->send_buf_size - UDS_0X27_REQ_BASE_LEN) { + return UDS_ERR_BUFSIZ; + } + } else { + assert(NULL == data); + } + + memmove(&client->send_buf[UDS_0X27_REQ_BASE_LEN], data, size); + client->send_size = UDS_0X27_REQ_BASE_LEN + size; + return _SendRequest(client); +} + +typedef struct { + uint8_t dataFormatIdentifier; + uint8_t addressAndLengthFormatIdentifier; + size_t memoryAddress; + size_t memorySize; + FILE *fd; + uint8_t blockSequenceCounter; + uint16_t blockLength; +} UDSClientDownloadSequence_t; + +static UDSSeqState_t requestDownload(UDSClient_t *client) { + UDSClientDownloadSequence_t *pL_Seq = (UDSClientDownloadSequence_t *)client->cbData; + UDSSendRequestDownload(client, pL_Seq->dataFormatIdentifier, + pL_Seq->addressAndLengthFormatIdentifier, pL_Seq->memoryAddress, + pL_Seq->memorySize); + return UDSSeqStateGotoNext; +} + +static UDSSeqState_t checkRequestDownloadResponse(UDSClient_t *client) { + UDSClientDownloadSequence_t *pL_Seq = (UDSClientDownloadSequence_t *)client->cbData; + struct RequestDownloadResponse resp = {0}; + UDSErr_t err = UDSUnpackRequestDownloadResponse(client, &resp); + if (err) { + client->err = err; + return UDSSeqStateDone; + } + pL_Seq->blockLength = resp.maxNumberOfBlockLength; + if (0 == resp.maxNumberOfBlockLength) { + client->err = UDS_ERR; + return UDSSeqStateDone; + } + return UDSSeqStateGotoNext; +} + +static UDSSeqState_t prepareToTransfer(UDSClient_t *client) { + UDSClientDownloadSequence_t *pL_Seq = (UDSClientDownloadSequence_t *)client->cbData; + pL_Seq->blockSequenceCounter = 1; + return UDSSeqStateGotoNext; +} + +static UDSSeqState_t transferData(UDSClient_t *client) { + UDSClientDownloadSequence_t *pL_Seq = (UDSClientDownloadSequence_t *)client->cbData; + if (kRequestStateIdle == client->state) { + if (ferror(pL_Seq->fd)) { + fclose(pL_Seq->fd); + client->err = UDS_ERR_FILE_IO; // 读取文件故障 + return UDSSeqStateDone; + } else if (feof(pL_Seq->fd)) { // 传完了 + return UDSSeqStateGotoNext; + } else { + UDSSendTransferDataStream(client, pL_Seq->blockSequenceCounter++, pL_Seq->blockLength, + pL_Seq->fd); + } + } + return UDSSeqStateRunning; +} + +static UDSSeqState_t requestTransferExit(UDSClient_t *client) { + UDSSendRequestTransferExit(client); + return UDSSeqStateGotoNext; +} + +UDSErr_t UDSConfigDownload(UDSClient_t *client, uint8_t dataFormatIdentifier, + uint8_t addressAndLengthFormatIdentifier, size_t memoryAddress, + size_t memorySize, FILE *fd) { + + static const UDSClientCallback callbacks[] = { + requestDownload, UDSClientAwaitIdle, checkRequestDownloadResponse, prepareToTransfer, + transferData, requestTransferExit, UDSClientAwaitIdle, NULL}; + static UDSClientDownloadSequence_t seq = {0}; + memset(&seq, 0, sizeof(seq)); + seq.blockSequenceCounter = 1; + seq.dataFormatIdentifier = dataFormatIdentifier; + seq.addressAndLengthFormatIdentifier = addressAndLengthFormatIdentifier; + seq.memoryAddress = memoryAddress; + seq.memorySize = memorySize; + seq.fd = fd; + client->cbList = callbacks; + client->cbIdx = 0; + client->cbData = &seq; + return UDS_OK; +} + +/** + * @brief + * + * @param client + * @param resp + * @return UDSErr_t + * @addtogroup securityAccess_0x27 + */ +UDSErr_t UDSUnpackSecurityAccessResponse(const UDSClient_t *client, + struct SecurityAccessResponse *resp) { + assert(client); + assert(resp); + if (UDS_RESPONSE_SID_OF(kSID_SECURITY_ACCESS) != client->recv_buf[0]) { + return UDS_ERR_SID_MISMATCH; + } + if (client->recv_size < UDS_0X27_RESP_BASE_LEN) { + return UDS_ERR_RESP_TOO_SHORT; + } + resp->securityAccessType = client->recv_buf[1]; + resp->securitySeedLength = client->recv_size - UDS_0X27_RESP_BASE_LEN; + resp->securitySeed = resp->securitySeedLength == 0 ? NULL : &client->recv_buf[2]; + return UDS_OK; +} + +/** + * @brief + * + * @param client + * @param resp + * @return UDSErr_t + * @addtogroup routineControl_0x31 + */ +UDSErr_t UDSUnpackRoutineControlResponse(const UDSClient_t *client, + struct RoutineControlResponse *resp) { + assert(client); + assert(resp); + if (UDS_RESPONSE_SID_OF(kSID_ROUTINE_CONTROL) != client->recv_buf[0]) { + return UDS_ERR_SID_MISMATCH; + } + if (client->recv_size < UDS_0X31_RESP_MIN_LEN) { + return UDS_ERR_RESP_TOO_SHORT; + } + resp->routineControlType = client->recv_buf[1]; + resp->routineIdentifier = (client->recv_buf[2] << 8) + client->recv_buf[3]; + resp->routineStatusRecordLength = client->recv_size - UDS_0X31_RESP_MIN_LEN; + resp->routineStatusRecord = + resp->routineStatusRecordLength == 0 ? NULL : &client->recv_buf[UDS_0X31_RESP_MIN_LEN]; + return UDS_OK; +} + +/** + * @brief + * + * @param client + * @param resp + * @return UDSErr_t + * @addtogroup requestDownload_0x34 + */ +UDSErr_t UDSUnpackRequestDownloadResponse(const UDSClient_t *client, + struct RequestDownloadResponse *resp) { + assert(client); + assert(resp); + if (UDS_RESPONSE_SID_OF(kSID_REQUEST_DOWNLOAD) != client->recv_buf[0]) { + return UDS_ERR_SID_MISMATCH; + } + if (client->recv_size < UDS_0X34_RESP_BASE_LEN) { + return UDS_ERR_RESP_TOO_SHORT; + } + uint8_t maxNumberOfBlockLengthSize = (client->recv_buf[1] & 0xF0) >> 4; + + if (sizeof(resp->maxNumberOfBlockLength) < maxNumberOfBlockLengthSize) { + UDS_DBG_PRINT("WARNING: sizeof(maxNumberOfBlockLength) > sizeof(size_t)"); + return UDS_ERR; + } + resp->maxNumberOfBlockLength = 0; + for (int byteIdx = 0; byteIdx < maxNumberOfBlockLengthSize; byteIdx++) { + uint8_t byte = client->recv_buf[UDS_0X34_RESP_BASE_LEN + byteIdx]; + uint8_t shiftBytes = maxNumberOfBlockLengthSize - 1 - byteIdx; + resp->maxNumberOfBlockLength |= byte << (8 * shiftBytes); + } + return UDS_OK; +} + +bool UDSClientPoll(UDSClient_t *client) { + PollLowLevel(client); + + if (client->err) { + return UDS_CLIENT_IDLE; + } + + if (kRequestStateIdle != client->state) { + return UDS_CLIENT_RUNNING; + } + + if (NULL == client->cbList) { + return UDS_CLIENT_IDLE; + } + + UDSClientCallback activeCallback = client->cbList[client->cbIdx]; + + if (NULL == activeCallback) { + return UDS_CLIENT_IDLE; + } + + UDSSeqState_t state = activeCallback(client); + + switch (state) { + case UDSSeqStateDone: + return UDS_CLIENT_IDLE; + case UDSSeqStateRunning: + return UDS_CLIENT_RUNNING; + case UDSSeqStateGotoNext: { + client->cbIdx += 1; + return UDS_CLIENT_RUNNING; + } + default: + assert(0); + return UDS_CLIENT_IDLE; + } +} + +void UDSClientPoll2(UDSClient_t *client, + int (*fn)(UDSClient_t *client, UDSEvent_t evt, void *ev_data, void *fn_data), + void *fn_data) { + UDSClientPoll(client); +} + +UDSSeqState_t UDSClientAwaitIdle(UDSClient_t *client) { + if (client->err) { + return UDSSeqStateDone; + } else if (kRequestStateIdle == client->state) { + return UDSSeqStateGotoNext; + } else { + return UDSSeqStateRunning; + } +} + +UDSErr_t UDSUnpackRDBIResponse(const uint8_t *buf, size_t buf_len, uint16_t did, uint8_t *data, + uint16_t data_size, uint16_t *offset) { + assert(buf); + assert(data); + assert(offset); + if (0 == *offset) { + *offset = UDS_0X22_RESP_BASE_LEN; + } + + if (*offset + sizeof(did) > buf_len) { + return UDS_ERR_RESP_TOO_SHORT; + } + + uint16_t theirDID = (buf[*offset] << 8) + buf[*offset + 1]; + if (theirDID != did) { + return UDS_ERR_DID_MISMATCH; + } + + if (*offset + sizeof(uint16_t) + data_size > buf_len) { + return UDS_ERR_RESP_TOO_SHORT; + } + + memmove(data, buf + *offset + sizeof(uint16_t), data_size); + + *offset += sizeof(uint16_t) + data_size; + return UDS_OK; +} diff --git a/src/client.h b/src/client.h new file mode 100644 index 0000000..141edc6 --- /dev/null +++ b/src/client.h @@ -0,0 +1,144 @@ +#pragma once + +#include "sys.h" +#include "tp.h" +#include "uds.h" + + +enum UDSClientRequestState { + kRequestStateIdle = 0, // 完成 + kRequestStateSending, // 传输层现在传输数据 + kRequestStateAwaitSendComplete, // 等待传输发送完成 + kRequestStateAwaitResponse, // 等待响应 + kRequestStateProcessResponse, // 处理响应 +}; + +typedef uint8_t UDSClientRequestState_t; + +enum UDSClientOptions { + UDS_SUPPRESS_POS_RESP = 0x1, // 服务器不应该发送肯定响应 + UDS_FUNCTIONAL = 0x2, // 发功能请求 + UDS_NEG_RESP_IS_ERR = 0x4, // 否定响应是属于故障 + UDS_IGNORE_SRV_TIMINGS = 0x8, // 忽略服务器给的p2和p2_star +}; + +struct UDSClient; + +typedef UDSSeqState_t (*UDSClientCallback)(struct UDSClient *client); + +typedef struct UDSClient { + uint16_t p2_ms; // p2 超时时间 + uint32_t p2_star_ms; // 0x78 p2* 超时时间 + UDSTpHandle_t *tp; + + // 内状态 + uint32_t p2_timer; + uint8_t *recv_buf; + uint8_t *send_buf; + uint16_t recv_buf_size; + uint16_t send_buf_size; + uint16_t recv_size; + uint16_t send_size; + UDSErr_t err; + UDSClientRequestState_t state; + + uint8_t options; // enum udsclientoptions + uint8_t defaultOptions; // enum udsclientoptions + // a copy of the options at the time a request is made + uint8_t _options_copy; // enum udsclientoptions + int (*fn)(struct UDSClient *, int, void *, void *); + + const UDSClientCallback *cbList; // null-terminated list of callback functions + size_t cbIdx; // index of currently active callback function + void *cbData; // a pointer to data available to callbacks + +} UDSClient_t; + +struct SecurityAccessResponse { + uint8_t securityAccessType; + const uint8_t *securitySeed; + uint16_t securitySeedLength; +}; + +struct RequestDownloadResponse { + size_t maxNumberOfBlockLength; +}; + +struct RoutineControlResponse { + uint8_t routineControlType; + uint16_t routineIdentifier; + const uint8_t *routineStatusRecord; + uint16_t routineStatusRecordLength; +}; + +UDSErr_t UDSClientInit(UDSClient_t *client); + +#define UDS_CLIENT_IDLE (0) +#define UDS_CLIENT_RUNNING (1) + +/** + * @brief poll the client (call this in a loop) + * @param client + * @return UDS_CLIENT_IDLE if idle, otherwise UDS_CLIENT_RUNNING + */ +bool UDSClientPoll(UDSClient_t *client); +void UDSClientPoll2(UDSClient_t *client, + int (*fn)(UDSClient_t *client, int evt, void *ev_data, void *fn_data), + void *fn_data); + +UDSErr_t UDSSendBytes(UDSClient_t *client, const uint8_t *data, uint16_t size); +UDSErr_t UDSSendECUReset(UDSClient_t *client, UDSECUReset_t type); +UDSErr_t UDSSendDiagSessCtrl(UDSClient_t *client, enum UDSDiagnosticSessionType mode); +UDSErr_t UDSSendSecurityAccess(UDSClient_t *client, uint8_t level, uint8_t *data, uint16_t size); +UDSErr_t UDSSendCommCtrl(UDSClient_t *client, enum UDSCommunicationControlType ctrl, + enum UDSCommunicationType comm); +UDSErr_t UDSSendRDBI(UDSClient_t *client, const uint16_t *didList, + const uint16_t numDataIdentifiers); +UDSErr_t UDSSendWDBI(UDSClient_t *client, uint16_t dataIdentifier, const uint8_t *data, + uint16_t size); +UDSErr_t UDSSendTesterPresent(UDSClient_t *client); +UDSErr_t UDSSendRoutineCtrl(UDSClient_t *client, enum RoutineControlType type, + uint16_t routineIdentifier, const uint8_t *data, uint16_t size); + +UDSErr_t UDSSendRequestDownload(UDSClient_t *client, uint8_t dataFormatIdentifier, + uint8_t addressAndLengthFormatIdentifier, size_t memoryAddress, + size_t memorySize); + +UDSErr_t UDSSendRequestUpload(UDSClient_t *client, uint8_t dataFormatIdentifier, + uint8_t addressAndLengthFormatIdentifier, size_t memoryAddress, + size_t memorySize); +UDSErr_t UDSSendTransferData(UDSClient_t *client, uint8_t blockSequenceCounter, + const uint16_t blockLength, const uint8_t *data, uint16_t size); +UDSErr_t UDSSendTransferDataStream(UDSClient_t *client, uint8_t blockSequenceCounter, + const uint16_t blockLength, FILE *fd); +UDSErr_t UDSSendRequestTransferExit(UDSClient_t *client); + +UDSErr_t UDSCtrlDTCSetting(UDSClient_t *client, uint8_t dtcSettingType, + uint8_t *dtcSettingControlOptionRecord, uint16_t len); +UDSErr_t UDSUnpackRDBIResponse(const uint8_t *buf, size_t buf_len, uint16_t did, uint8_t *data, + uint16_t size, uint16_t *offset); +UDSErr_t UDSUnpackSecurityAccessResponse(const UDSClient_t *client, + struct SecurityAccessResponse *resp); +UDSErr_t UDSUnpackRequestDownloadResponse(const UDSClient_t *client, + struct RequestDownloadResponse *resp); +UDSErr_t UDSUnpackRoutineControlResponse(const UDSClient_t *client, + struct RoutineControlResponse *resp); + +/** + * @brief Wait after request transmission for a response to be received + * @note if suppressPositiveResponse is set, this function will return + UDSSeqStateGotoNext as soon as the transport layer has completed transmission. + * + * @param client + * @param args + * @return UDSErr_t + - UDSSeqStateDone -- 流程完成 + - UDSSeqStateRunning -- 流程正在跑、还没完成 + */ +UDSSeqState_t UDSClientAwaitIdle(UDSClient_t *client); + +UDSErr_t UDSConfigDownload(UDSClient_t *client, uint8_t dataFormatIdentifier, + uint8_t addressAndLengthFormatIdentifier, size_t memoryAddress, + size_t memorySize, FILE *fd); + + diff --git a/src/config.h b/src/config.h new file mode 100644 index 0000000..0f0d173 --- /dev/null +++ b/src/config.h @@ -0,0 +1,66 @@ +#pragma once + +#ifndef UDS_ENABLE_DBG_PRINT +#define UDS_ENABLE_DBG_PRINT 0 +#endif + +#ifndef UDS_ENABLE_ASSERT +#define UDS_ENABLE_ASSERT 0 +#endif + +/** ISO-TP Maximum Transmissiable Unit (ISO-15764-2-2004 section 5.3.3) */ +#define UDS_ISOTP_MTU (4095) + +#ifndef UDS_TP_MTU +#define UDS_TP_MTU UDS_ISOTP_MTU +#endif + +#ifndef UDS_CLIENT_DEFAULT_P2_MS +#define UDS_CLIENT_DEFAULT_P2_MS (150U) +#endif + +#ifndef UDS_CLIENT_DEFAULT_P2_STAR_MS +#define UDS_CLIENT_DEFAULT_P2_STAR_MS (1500U) +#endif + +_Static_assert(UDS_CLIENT_DEFAULT_P2_STAR_MS > UDS_CLIENT_DEFAULT_P2_MS, ""); + + +#ifndef UDS_SERVER_DEFAULT_POWER_DOWN_TIME_MS +#define UDS_SERVER_DEFAULT_POWER_DOWN_TIME_MS (10) +#endif + +#ifndef UDS_SERVER_DEFAULT_P2_MS +#define UDS_SERVER_DEFAULT_P2_MS (50) +#endif + +#ifndef UDS_SERVER_DEFAULT_P2_STAR_MS +#define UDS_SERVER_DEFAULT_P2_STAR_MS (2000) +#endif + +#ifndef UDS_SERVER_DEFAULT_S3_MS +#define UDS_SERVER_DEFAULT_S3_MS (3000) +#endif + +_Static_assert(0 < UDS_SERVER_DEFAULT_P2_MS && + UDS_SERVER_DEFAULT_P2_MS < UDS_SERVER_DEFAULT_P2_STAR_MS && + UDS_SERVER_DEFAULT_P2_STAR_MS < UDS_SERVER_DEFAULT_S3_MS, + ""); + +// Amount of time to wait after boot before accepting 0x27 requests. +#ifndef UDS_SERVER_0x27_BRUTE_FORCE_MITIGATION_BOOT_DELAY_MS +#define UDS_SERVER_0x27_BRUTE_FORCE_MITIGATION_BOOT_DELAY_MS (1000) +#endif + +// Amount of time to wait after an authentication failure before accepting another 0x27 request. +#ifndef UDS_SERVER_0x27_BRUTE_FORCE_MITIGATION_AUTH_FAIL_DELAY_MS +#define UDS_SERVER_0x27_BRUTE_FORCE_MITIGATION_AUTH_FAIL_DELAY_MS (1000) +#endif + +#ifndef UDS_SERVER_DEFAULT_XFER_DATA_MAX_BLOCKLENGTH +/*! ISO14229-1:2013 Table 396. This parameter is used by the requestDownload positive response +message to inform the client how many data bytes (maxNumberOfBlockLength) to include in each +TransferData request message from the client. */ +#define UDS_SERVER_DEFAULT_XFER_DATA_MAX_BLOCKLENGTH (UDS_TP_MTU) +#endif + diff --git a/iso14229serverbufferedwriter.h b/src/iso14229serverbufferedwriter.h similarity index 100% rename from iso14229serverbufferedwriter.h rename to src/iso14229serverbufferedwriter.h diff --git a/src/server.c b/src/server.c new file mode 100644 index 0000000..38146a7 --- /dev/null +++ b/src/server.c @@ -0,0 +1,944 @@ +#include "server.h" +#include "config.h" +#include "uds.h" +#include "util.h" + +static inline uint8_t NegativeResponse(UDSReq_t *r, uint8_t response_code) { + r->send_buf[0] = 0x7F; + r->send_buf[1] = r->recv_buf[0]; + r->send_buf[2] = response_code; + r->send_len = UDS_NEG_RESP_LEN; + return response_code; +} + +static inline void NoResponse(UDSReq_t *r) { r->send_len = 0; } + +static uint8_t EmitEvent(UDSServer_t *srv, UDSServerEvent_t evt, void *data) { + if (srv->fn) { + return srv->fn(srv, evt, data); + } else { + UDS_DBG_PRINT("Unhandled UDSServerEvent %d, srv.fn not installed!\n", evt); + return kGeneralReject; + } +} + +static uint8_t _0x10_DiagnosticSessionControl(UDSServer_t *srv, UDSReq_t *r) { + if (r->recv_len < UDS_0X10_REQ_LEN) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + + uint8_t sessType = r->recv_buf[1] & 0x4F; + + UDSDiagSessCtrlArgs_t args = { + .type = sessType, + .p2_ms = UDS_CLIENT_DEFAULT_P2_MS, + .p2_star_ms = UDS_CLIENT_DEFAULT_P2_STAR_MS, + }; + + uint8_t err = EmitEvent(srv, UDS_SRV_EVT_DiagSessCtrl, &args); + + if (kPositiveResponse != err) { + return NegativeResponse(r, err); + } + + srv->sessionType = sessType; + + switch (sessType) { + case kDefaultSession: + break; + case kProgrammingSession: + case kExtendedDiagnostic: + default: + srv->s3_session_timeout_timer = UDSMillis() + srv->s3_ms; + break; + } + + r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_DIAGNOSTIC_SESSION_CONTROL); + r->send_buf[1] = sessType; + + // UDS-1-2013: Table 29 + // resolution: 1ms + r->send_buf[2] = args.p2_ms >> 8; + r->send_buf[3] = args.p2_ms; + + // resolution: 10ms + r->send_buf[4] = (args.p2_star_ms / 10) >> 8; + r->send_buf[5] = args.p2_star_ms / 10; + + r->send_len = UDS_0X10_RESP_LEN; + return kPositiveResponse; +} + +static uint8_t _0x11_ECUReset(UDSServer_t *srv, UDSReq_t *r) { + uint8_t resetType = r->recv_buf[1] & 0x3F; + + if (r->recv_len < UDS_0X11_REQ_MIN_LEN) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + + UDSECUResetArgs_t args = { + .type = resetType, + .powerDownTimeMillis = UDS_SERVER_DEFAULT_POWER_DOWN_TIME_MS, + }; + + uint8_t err = EmitEvent(srv, UDS_SRV_EVT_EcuReset, &args); + + if (kPositiveResponse == err) { + srv->notReadyToReceive = true; + srv->ecuResetScheduled = resetType; + srv->ecuResetTimer = UDSMillis() + args.powerDownTimeMillis; + } else { + return NegativeResponse(r, err); + } + + r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_ECU_RESET); + r->send_buf[1] = resetType; + + if (kEnableRapidPowerShutDown == resetType) { + uint32_t powerDownTime = args.powerDownTimeMillis / 1000; + if (powerDownTime > 255) { + powerDownTime = 255; + } + r->send_buf[2] = powerDownTime; + r->send_len = UDS_0X11_RESP_BASE_LEN + 1; + } else { + r->send_len = UDS_0X11_RESP_BASE_LEN; + } + return kPositiveResponse; +} + +static uint8_t safe_copy(UDSServer_t *srv, const void *src, uint16_t count) { + if (srv == NULL) { + return kGeneralReject; + } + UDSReq_t *r = (UDSReq_t *)&srv->r; + if (count <= r->send_buf_size - r->send_len) { + memmove(r->send_buf + r->send_len, src, count); + r->send_len += count; + return kPositiveResponse; + } + return kResponseTooLong; +} + +static uint8_t _0x22_ReadDataByIdentifier(UDSServer_t *srv, UDSReq_t *r) { + uint8_t numDIDs; + uint16_t dataId = 0; + uint8_t ret = kPositiveResponse; + r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_READ_DATA_BY_IDENTIFIER); + r->send_len = 1; + + if (0 != (r->recv_len - 1) % sizeof(uint16_t)) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + + numDIDs = r->recv_len / sizeof(uint16_t); + + if (0 == numDIDs) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + + for (int did = 0; did < numDIDs; did++) { + uint16_t idx = 1 + did * 2; + dataId = (r->recv_buf[idx] << 8) + r->recv_buf[idx + 1]; + + if (r->send_len + 3 > r->send_buf_size) { + return NegativeResponse(r, kResponseTooLong); + } + uint8_t *copylocation = r->send_buf + r->send_len; + copylocation[0] = dataId >> 8; + copylocation[1] = dataId; + r->send_len += 2; + + UDSRDBIArgs_t args = { + .dataId = dataId, + .copy = safe_copy, + }; + + ret = EmitEvent(srv, UDS_SRV_EVT_ReadDataByIdent, &args); + + if (kPositiveResponse != ret) { + return NegativeResponse(r, ret); + } + } + return kPositiveResponse; +} + +/** + * @brief decode the addressAndLengthFormatIdentifier that appears in ReadMemoryByAddress (0x23), + * DynamicallyDefineDataIdentifier (0x2C), RequestDownload (0X34) + * + * @param srv + * @param buf pointer to addressAndDataLengthFormatIdentifier in recv_buf + * @param memoryAddress the decoded memory address + * @param memorySize the decoded memory size + * @return uint8_t + */ +static uint8_t decodeAddressAndLength(UDSReq_t *r, uint8_t *const buf, void **memoryAddress, + size_t *memorySize) { + assert(r); + assert(memoryAddress); + assert(memorySize); + long long unsigned int tmp = 0; + *memoryAddress = 0; + *memorySize = 0; + + assert(buf >= r->recv_buf && buf <= r->recv_buf + sizeof(r->recv_buf)); + + if (r->recv_len < 3) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + + uint8_t memorySizeLength = (buf[0] & 0xF0) >> 4; + uint8_t memoryAddressLength = buf[0] & 0x0F; + + if (memorySizeLength == 0 || memorySizeLength > sizeof(size_t)) { + return NegativeResponse(r, kRequestOutOfRange); + } + + if (memoryAddressLength == 0 || memoryAddressLength > sizeof(size_t)) { + return NegativeResponse(r, kRequestOutOfRange); + } + + if (buf + memorySizeLength + memoryAddressLength > r->recv_buf + r->recv_len) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + + for (int byteIdx = 0; byteIdx < memoryAddressLength; byteIdx++) { + long long unsigned int byte = buf[1 + byteIdx]; + uint8_t shiftBytes = memoryAddressLength - 1 - byteIdx; + tmp |= byte << (8 * shiftBytes); + } + *memoryAddress = (void *)tmp; + + for (int byteIdx = 0; byteIdx < memorySizeLength; byteIdx++) { + uint8_t byte = buf[1 + memoryAddressLength + byteIdx]; + uint8_t shiftBytes = memorySizeLength - 1 - byteIdx; + *memorySize |= (size_t)byte << (8 * shiftBytes); + } + return kPositiveResponse; +} + +static uint8_t _0x23_ReadMemoryByAddress(UDSServer_t *srv, UDSReq_t *r) { + uint8_t ret = kPositiveResponse; + void *address = 0; + size_t length = 0; + + if (r->recv_len < UDS_0X23_REQ_MIN_LEN) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + + ret = decodeAddressAndLength(r, &r->recv_buf[1], &address, &length); + if (kPositiveResponse != ret) { + return NegativeResponse(r, ret); + } + + UDSReadMemByAddrArgs_t args = { + .memAddr = address, + .memSize = length, + .copy = safe_copy, + }; + + r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_READ_MEMORY_BY_ADDRESS); + r->send_len = UDS_0X23_RESP_BASE_LEN; + ret = EmitEvent(srv, UDS_SRV_EVT_ReadMemByAddr, &args); + if (kPositiveResponse != ret) { + return NegativeResponse(r, ret); + } + if (r->send_len != UDS_0X23_RESP_BASE_LEN + length) { + return kGeneralProgrammingFailure; + } + return kPositiveResponse; +} + +static uint8_t _0x27_SecurityAccess(UDSServer_t *srv, UDSReq_t *r) { + uint8_t subFunction = r->recv_buf[1]; + uint8_t response = kPositiveResponse; + + if (UDSSecurityAccessLevelIsReserved(subFunction)) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + + if (!UDSTimeAfter(UDSMillis(), UDS_SERVER_0x27_BRUTE_FORCE_MITIGATION_BOOT_DELAY_MS)) { + return NegativeResponse(r, kRequiredTimeDelayNotExpired); + } + + if (!(UDSTimeAfter(UDSMillis(), srv->sec_access_auth_fail_timer))) { + return NegativeResponse(r, kExceedNumberOfAttempts); + } + + r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_SECURITY_ACCESS); + r->send_buf[1] = subFunction; + r->send_len = UDS_0X27_RESP_BASE_LEN; + + // Even: sendKey + if (0 == subFunction % 2) { + uint8_t requestedLevel = subFunction - 1; + UDSSecAccessValidateKeyArgs_t args = { + .level = requestedLevel, + .key = &r->recv_buf[UDS_0X27_REQ_BASE_LEN], + .len = r->recv_len - UDS_0X27_REQ_BASE_LEN, + }; + + response = EmitEvent(srv, UDS_SRV_EVT_SecAccessValidateKey, &args); + + if (kPositiveResponse != response) { + srv->sec_access_auth_fail_timer = UDSMillis() + UDS_SERVER_0x27_BRUTE_FORCE_MITIGATION_AUTH_FAIL_DELAY_MS; + return NegativeResponse(r, response); + } + + // "requestSeed = 0x01" identifies a fixed relationship between + // "requestSeed = 0x01" and "sendKey = 0x02" + // "requestSeed = 0x03" identifies a fixed relationship between + // "requestSeed = 0x03" and "sendKey = 0x04" + srv->securityLevel = requestedLevel; + r->send_len = UDS_0X27_RESP_BASE_LEN; + return kPositiveResponse; + } + + // Odd: requestSeed + else { + /* If a server supports security, but the requested security level is already unlocked when + a SecurityAccess ‘requestSeed’ message is received, that server shall respond with a + SecurityAccess ‘requestSeed’ positive response message service with a seed value equal to + zero (0). The server shall never send an all zero seed for a given security level that is + currently locked. The client shall use this method to determine if a server is locked for a + particular security level by checking for a non-zero seed. + */ + if (subFunction == srv->securityLevel) { + // Table 52 sends a response of length 2. Use a preprocessor define if this needs + // customizing by the user. + const uint8_t already_unlocked[] = {0x00, 0x00}; + return safe_copy(srv, already_unlocked, sizeof(already_unlocked)); + } else { + UDSSecAccessRequestSeedArgs_t args = { + .level = subFunction, + .dataRecord = &r->recv_buf[UDS_0X27_REQ_BASE_LEN], + .len = r->recv_len - UDS_0X27_REQ_BASE_LEN, + .copySeed = safe_copy, + }; + + response = EmitEvent(srv, UDS_SRV_EVT_SecAccessRequestSeed, &args); + + if (kPositiveResponse != response) { + return NegativeResponse(r, response); + } + + if (r->send_len <= UDS_0X27_RESP_BASE_LEN) { // no data was copied + return NegativeResponse(r, kGeneralProgrammingFailure); + } + return kPositiveResponse; + } + } + return NegativeResponse(r, kGeneralProgrammingFailure); +} + +static uint8_t _0x28_CommunicationControl(UDSServer_t *srv, UDSReq_t *r) { + uint8_t controlType = r->recv_buf[1] & 0x7F; + uint8_t communicationType = r->recv_buf[2]; + + if (r->recv_len < UDS_0X28_REQ_BASE_LEN) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + + UDSCommCtrlArgs_t args = { + .ctrlType = controlType, + .commType = communicationType, + }; + + uint8_t err = EmitEvent(srv, UDS_SRV_EVT_CommCtrl, &args); + if (kPositiveResponse != err) { + return NegativeResponse(r, err); + } + + r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_COMMUNICATION_CONTROL); + r->send_buf[1] = controlType; + r->send_len = UDS_0X28_RESP_LEN; + return kPositiveResponse; +} + +static uint8_t _0x2E_WriteDataByIdentifier(UDSServer_t *srv, UDSReq_t *r) { + uint16_t dataLen = 0; + uint16_t dataId = 0; + uint8_t err = kPositiveResponse; + + /* UDS-1 2013 Figure 21 Key 1 */ + if (r->recv_len < UDS_0X2E_REQ_MIN_LEN) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + + dataId = (r->recv_buf[1] << 8) + r->recv_buf[2]; + dataLen = r->recv_len - UDS_0X2E_REQ_BASE_LEN; + + UDSWDBIArgs_t args = { + .dataId = dataId, + .data = &r->recv_buf[UDS_0X2E_REQ_BASE_LEN], + .len = dataLen, + }; + + err = EmitEvent(srv, UDS_SRV_EVT_WriteDataByIdent, &args); + if (kPositiveResponse != err) { + return NegativeResponse(r, err); + } + + r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_WRITE_DATA_BY_IDENTIFIER); + r->send_buf[1] = dataId >> 8; + r->send_buf[2] = dataId; + r->send_len = UDS_0X2E_RESP_LEN; + return kPositiveResponse; +} + +static uint8_t _0x31_RoutineControl(UDSServer_t *srv, UDSReq_t *r) { + uint8_t err = kPositiveResponse; + if (r->recv_len < UDS_0X31_REQ_MIN_LEN) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + + uint8_t routineControlType = r->recv_buf[1] & 0x7F; + uint16_t routineIdentifier = (r->recv_buf[2] << 8) + r->recv_buf[3]; + + UDSRoutineCtrlArgs_t args = { + .ctrlType = routineControlType, + .id = routineIdentifier, + .optionRecord = &r->recv_buf[UDS_0X31_REQ_MIN_LEN], + .len = r->recv_len - UDS_0X31_REQ_MIN_LEN, + .copyStatusRecord = safe_copy, + }; + + r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_ROUTINE_CONTROL); + r->send_buf[1] = routineControlType; + r->send_buf[2] = routineIdentifier >> 8; + r->send_buf[3] = routineIdentifier; + r->send_len = UDS_0X31_RESP_MIN_LEN; + + switch (routineControlType) { + case kStartRoutine: + case kStopRoutine: + case kRequestRoutineResults: + err = EmitEvent(srv, UDS_SRV_EVT_RoutineCtrl, &args); + if (kPositiveResponse != err) { + return NegativeResponse(r, err); + } + break; + default: + return NegativeResponse(r, kRequestOutOfRange); + } + return kPositiveResponse; +} + +static void ResetTransfer(UDSServer_t *srv) { + assert(srv); + srv->xferBlockSequenceCounter = 1; + srv->xferByteCounter = 0; + srv->xferTotalBytes = 0; + srv->xferIsActive = false; +} + +static uint8_t _0x34_RequestDownload(UDSServer_t *srv, UDSReq_t *r) { + uint8_t err; + void *memoryAddress = 0; + size_t memorySize = 0; + + if (srv->xferIsActive) { + return NegativeResponse(r, kConditionsNotCorrect); + } + + if (r->recv_len < UDS_0X34_REQ_BASE_LEN) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + + err = decodeAddressAndLength(r, &r->recv_buf[2], &memoryAddress, &memorySize); + if (kPositiveResponse != err) { + return NegativeResponse(r, err); + } + + UDSRequestDownloadArgs_t args = { + .addr = memoryAddress, + .size = memorySize, + .dataFormatIdentifier = r->recv_buf[1], + .maxNumberOfBlockLength = UDS_SERVER_DEFAULT_XFER_DATA_MAX_BLOCKLENGTH, + }; + + err = EmitEvent(srv, UDS_SRV_EVT_RequestDownload, &args); + + if (args.maxNumberOfBlockLength < 3) { + UDS_DBG_PRINT("ERROR: maxNumberOfBlockLength too short"); + return NegativeResponse(r, kGeneralProgrammingFailure); + } + + if (kPositiveResponse != err) { + return NegativeResponse(r, err); + } + + ResetTransfer(srv); + srv->xferIsActive = true; + srv->xferTotalBytes = memorySize; + srv->xferBlockLength = args.maxNumberOfBlockLength; + + // ISO-14229-1:2013 Table 401: + uint8_t lengthFormatIdentifier = sizeof(args.maxNumberOfBlockLength) << 4; + + /* ISO-14229-1:2013 Table 396: maxNumberOfBlockLength + This parameter is used by the requestDownload positive response message to + inform the client how many data bytes (maxNumberOfBlockLength) to include in + each TransferData request message from the client. This length reflects the + complete message length, including the service identifier and the + data-parameters present in the TransferData request message. + */ + if (args.maxNumberOfBlockLength > UDS_TP_MTU) { + args.maxNumberOfBlockLength = UDS_TP_MTU; + } + + r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_REQUEST_DOWNLOAD); + r->send_buf[1] = lengthFormatIdentifier; + for (uint8_t idx = 0; idx < sizeof(args.maxNumberOfBlockLength); idx++) { + uint8_t shiftBytes = sizeof(args.maxNumberOfBlockLength) - 1 - idx; + uint8_t byte = args.maxNumberOfBlockLength >> (shiftBytes * 8); + r->send_buf[UDS_0X34_RESP_BASE_LEN + idx] = byte; + } + r->send_len = UDS_0X34_RESP_BASE_LEN + sizeof(args.maxNumberOfBlockLength); + return kPositiveResponse; +} + +static uint8_t _0x35_RequestUpload(UDSServer_t *srv, UDSReq_t *r) { + uint8_t err; + void *memoryAddress = 0; + size_t memorySize = 0; + + if (srv->xferIsActive) { + return NegativeResponse(r, kConditionsNotCorrect); + } + + if (r->recv_len < UDS_0X35_REQ_BASE_LEN) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + + err = decodeAddressAndLength(r, &r->recv_buf[2], &memoryAddress, &memorySize); + if (kPositiveResponse != err) { + return NegativeResponse(r, err); + } + + UDSRequestUploadArgs_t args = { + .addr = memoryAddress, + .size = memorySize, + .dataFormatIdentifier = r->recv_buf[1], + .maxNumberOfBlockLength = UDS_SERVER_DEFAULT_XFER_DATA_MAX_BLOCKLENGTH, + }; + + err = EmitEvent(srv, UDS_SRV_EVT_RequestUpload, &args); + + if (args.maxNumberOfBlockLength < 3) { + UDS_DBG_PRINT("ERROR: maxNumberOfBlockLength too short"); + return NegativeResponse(r, kGeneralProgrammingFailure); + } + + if (kPositiveResponse != err) { + return NegativeResponse(r, err); + } + + ResetTransfer(srv); + srv->xferIsActive = true; + srv->xferTotalBytes = memorySize; + srv->xferBlockLength = args.maxNumberOfBlockLength; + + uint8_t lengthFormatIdentifier = sizeof(args.maxNumberOfBlockLength) << 4; + + r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_REQUEST_UPLOAD); + r->send_buf[1] = lengthFormatIdentifier; + for (uint8_t idx = 0; idx < sizeof(args.maxNumberOfBlockLength); idx++) { + uint8_t shiftBytes = sizeof(args.maxNumberOfBlockLength) - 1 - idx; + uint8_t byte = args.maxNumberOfBlockLength >> (shiftBytes * 8); + r->send_buf[UDS_0X35_RESP_BASE_LEN + idx] = byte; + } + r->send_len = UDS_0X35_RESP_BASE_LEN + sizeof(args.maxNumberOfBlockLength); + return kPositiveResponse; +} + +static uint8_t _0x36_TransferData(UDSServer_t *srv, UDSReq_t *r) { + uint8_t err = kPositiveResponse; + uint16_t request_data_len = r->recv_len - UDS_0X36_REQ_BASE_LEN; + uint8_t blockSequenceCounter = 0; + + if (!srv->xferIsActive) { + return NegativeResponse(r, kUploadDownloadNotAccepted); + } + + if (r->recv_len < UDS_0X36_REQ_BASE_LEN) { + err = kIncorrectMessageLengthOrInvalidFormat; + goto fail; + } + + blockSequenceCounter = r->recv_buf[1]; + + if (!srv->RCRRP) { + if (blockSequenceCounter != srv->xferBlockSequenceCounter) { + err = kRequestSequenceError; + goto fail; + } else { + srv->xferBlockSequenceCounter++; + } + } + + if (srv->xferByteCounter + request_data_len > srv->xferTotalBytes) { + err = kTransferDataSuspended; + goto fail; + } + + { + UDSTransferDataArgs_t args = { + .data = &r->recv_buf[UDS_0X36_REQ_BASE_LEN], + .len = r->recv_len - UDS_0X36_REQ_BASE_LEN, + .maxRespLen = srv->xferBlockLength - UDS_0X36_RESP_BASE_LEN, + .copyResponse = safe_copy, + }; + + r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_TRANSFER_DATA); + r->send_buf[1] = blockSequenceCounter; + r->send_len = UDS_0X36_RESP_BASE_LEN; + + err = EmitEvent(srv, UDS_SRV_EVT_TransferData, &args); + + switch (err) { + case kPositiveResponse: + srv->xferByteCounter += request_data_len; + return kPositiveResponse; + case kRequestCorrectlyReceived_ResponsePending: + return NegativeResponse(r, kRequestCorrectlyReceived_ResponsePending); + default: + goto fail; + } + } + +fail: + ResetTransfer(srv); + return NegativeResponse(r, err); +} + +static uint8_t _0x37_RequestTransferExit(UDSServer_t *srv, UDSReq_t *r) { + uint8_t err = kPositiveResponse; + + if (!srv->xferIsActive) { + return NegativeResponse(r, kUploadDownloadNotAccepted); + } + + r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_REQUEST_TRANSFER_EXIT); + r->send_len = UDS_0X37_RESP_BASE_LEN; + + UDSRequestTransferExitArgs_t args = { + .data = &r->recv_buf[UDS_0X37_REQ_BASE_LEN], + .len = r->recv_len - UDS_0X37_REQ_BASE_LEN, + .copyResponse = safe_copy, + }; + + err = EmitEvent(srv, UDS_SRV_EVT_RequestTransferExit, &args); + + switch (err) { + case kPositiveResponse: + ResetTransfer(srv); + return kPositiveResponse; + case kRequestCorrectlyReceived_ResponsePending: + return NegativeResponse(r, kRequestCorrectlyReceived_ResponsePending); + default: + ResetTransfer(srv); + return NegativeResponse(r, err); + } +} + +static uint8_t _0x3E_TesterPresent(UDSServer_t *srv, UDSReq_t *r) { + if ((r->recv_len < UDS_0X3E_REQ_MIN_LEN) || (r->recv_len > UDS_0X3E_REQ_MAX_LEN)) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + uint8_t zeroSubFunction = r->recv_buf[1]; + + switch (zeroSubFunction) { + case 0x00: + case 0x80: + srv->s3_session_timeout_timer = UDSMillis() + srv->s3_ms; + r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_TESTER_PRESENT); + r->send_buf[1] = 0x00; + r->send_len = UDS_0X3E_RESP_LEN; + return kPositiveResponse; + default: + return NegativeResponse(r, kSubFunctionNotSupported); + } +} + +static uint8_t _0x85_ControlDTCSetting(UDSServer_t *srv, UDSReq_t *r) { + if (r->recv_len < UDS_0X85_REQ_BASE_LEN) { + return NegativeResponse(r, kIncorrectMessageLengthOrInvalidFormat); + } + uint8_t dtcSettingType = r->recv_buf[1] & 0x3F; + + r->send_buf[0] = UDS_RESPONSE_SID_OF(kSID_CONTROL_DTC_SETTING); + r->send_buf[1] = dtcSettingType; + r->send_len = UDS_0X85_RESP_LEN; + return kPositiveResponse; +} + +typedef uint8_t (*UDSService)(UDSServer_t *srv, UDSReq_t *r); + +/** + * @brief Get the internal service handler matching the given SID. + * @param sid + * @return pointer to UDSService or NULL if no match + */ +static UDSService getServiceForSID(uint8_t sid) { + switch (sid) { + case kSID_DIAGNOSTIC_SESSION_CONTROL: + return _0x10_DiagnosticSessionControl; + case kSID_ECU_RESET: + return _0x11_ECUReset; + case kSID_CLEAR_DIAGNOSTIC_INFORMATION: + return NULL; + case kSID_READ_DTC_INFORMATION: + return NULL; + case kSID_READ_DATA_BY_IDENTIFIER: + return _0x22_ReadDataByIdentifier; + case kSID_READ_MEMORY_BY_ADDRESS: + return _0x23_ReadMemoryByAddress; + case kSID_READ_SCALING_DATA_BY_IDENTIFIER: + return NULL; + case kSID_SECURITY_ACCESS: + return _0x27_SecurityAccess; + case kSID_COMMUNICATION_CONTROL: + return _0x28_CommunicationControl; + case kSID_READ_PERIODIC_DATA_BY_IDENTIFIER: + return NULL; + case kSID_DYNAMICALLY_DEFINE_DATA_IDENTIFIER: + return NULL; + case kSID_WRITE_DATA_BY_IDENTIFIER: + return _0x2E_WriteDataByIdentifier; + case kSID_INPUT_CONTROL_BY_IDENTIFIER: + return NULL; + case kSID_ROUTINE_CONTROL: + return _0x31_RoutineControl; + case kSID_REQUEST_DOWNLOAD: + return _0x34_RequestDownload; + case kSID_REQUEST_UPLOAD: + return _0x35_RequestUpload; + case kSID_TRANSFER_DATA: + return _0x36_TransferData; + case kSID_REQUEST_TRANSFER_EXIT: + return _0x37_RequestTransferExit; + case kSID_REQUEST_FILE_TRANSFER: + return NULL; + case kSID_WRITE_MEMORY_BY_ADDRESS: + return NULL; + case kSID_TESTER_PRESENT: + return _0x3E_TesterPresent; + case kSID_ACCESS_TIMING_PARAMETER: + return NULL; + case kSID_SECURED_DATA_TRANSMISSION: + return NULL; + case kSID_CONTROL_DTC_SETTING: + return _0x85_ControlDTCSetting; + case kSID_RESPONSE_ON_EVENT: + return NULL; + default: + UDS_DBG_PRINT("no handler for request SID %x.\n", sid); + return NULL; + } +} + +/** + * @brief Call the service if it exists, modifying the response if the spec calls for it. + * @note see UDS-1 2013 7.5.5 Pseudo code example of server response behavior + * + * @param srv + * @param addressingScheme + */ +static uint8_t evaluateServiceResponse(UDSServer_t *srv, UDSReq_t *r) { + uint8_t response = kPositiveResponse; + bool suppressResponse = false; + uint8_t sid = r->recv_buf[0]; + UDSService service = getServiceForSID(sid); + + if (NULL == service || NULL == srv->fn) { + return NegativeResponse(r, kServiceNotSupported); + } + assert(service); + assert(srv->fn); // service handler functions will call srv->fn. it must be valid + + switch (sid) { + /* CASE Service_with_sub-function */ + /* test if service with sub-function is supported */ + case kSID_DIAGNOSTIC_SESSION_CONTROL: + case kSID_ECU_RESET: + case kSID_SECURITY_ACCESS: + case kSID_COMMUNICATION_CONTROL: + case kSID_ROUTINE_CONTROL: + case kSID_TESTER_PRESENT: + case kSID_CONTROL_DTC_SETTING: { + response = service(srv, r); + + bool suppressPosRspMsgIndicationBit = r->recv_buf[1] & 0x80; + + /* test if positive response is required and if responseCode is positive 0x00 */ + if ((suppressPosRspMsgIndicationBit) && (response == kPositiveResponse) && + ( + // TODO: *not yet a NRC 0x78 response sent* + true)) { + suppressResponse = true; + } else { + suppressResponse = false; + } + break; + } + + /* CASE Service_without_sub-function */ + /* test if service without sub-function is supported */ + case kSID_READ_DATA_BY_IDENTIFIER: + case kSID_READ_MEMORY_BY_ADDRESS: + case kSID_WRITE_DATA_BY_IDENTIFIER: + case kSID_REQUEST_DOWNLOAD: + case kSID_REQUEST_UPLOAD: + case kSID_TRANSFER_DATA: + case kSID_REQUEST_TRANSFER_EXIT: { + response = service(srv, r); + break; + } + + /* CASE Service_not_implemented */ + /* shouldn't get this far as getServiceForSID(sid) will return NULL*/ + case kSID_CLEAR_DIAGNOSTIC_INFORMATION: + case kSID_READ_DTC_INFORMATION: + case kSID_READ_SCALING_DATA_BY_IDENTIFIER: + case kSID_READ_PERIODIC_DATA_BY_IDENTIFIER: + case kSID_DYNAMICALLY_DEFINE_DATA_IDENTIFIER: + case kSID_INPUT_CONTROL_BY_IDENTIFIER: + case kSID_REQUEST_FILE_TRANSFER: + case kSID_WRITE_MEMORY_BY_ADDRESS: + case kSID_ACCESS_TIMING_PARAMETER: + case kSID_SECURED_DATA_TRANSMISSION: + case kSID_RESPONSE_ON_EVENT: + default: { + response = kServiceNotSupported; + break; + } + } + + if ((UDS_A_TA_TYPE_FUNCTIONAL == r->info.A_TA_Type) && + ((kServiceNotSupported == response) || (kSubFunctionNotSupported == response) || + (kServiceNotSupportedInActiveSession == response) || + (kSubFunctionNotSupportedInActiveSession == response) || + (kRequestOutOfRange == response)) && + ( + // TODO: *not yet a NRC 0x78 response sent* + true)) { + suppressResponse = true; /* Suppress negative response message */ + NoResponse(r); + } else { + if (suppressResponse) { /* Suppress positive response message */ + NoResponse(r); + } else { /* send negative or positive response */ + ; + } + } + return response; +} + +// ======================================================================== +// Public Functions +// ======================================================================== + +/** + * @brief \~chinese 初始化服务器 \~english Initialize the server + * + * @param srv + * @param cfg + * @return int + */ +UDSErr_t UDSServerInit(UDSServer_t *srv) { + assert(srv); + memset(srv, 0, sizeof(UDSServer_t)); + srv->p2_ms = UDS_SERVER_DEFAULT_P2_MS; + srv->p2_star_ms = UDS_SERVER_DEFAULT_P2_STAR_MS; + srv->s3_ms = UDS_SERVER_DEFAULT_S3_MS; + srv->sessionType = kDefaultSession; + srv->p2_timer = UDSMillis() + srv->p2_ms; + srv->s3_session_timeout_timer = UDSMillis() + srv->s3_ms; + return UDS_OK; +} + +void UDSServerPoll(UDSServer_t *srv) { + // UDS-1-2013 Figure 38: Session Timeout (S3) + if (kDefaultSession != srv->sessionType && + UDSTimeAfter(UDSMillis(), srv->s3_session_timeout_timer)) { + EmitEvent(srv, UDS_SRV_EVT_SessionTimeout, NULL); + } + + if (srv->ecuResetScheduled && UDSTimeAfter(UDSMillis(), srv->ecuResetTimer)) { + EmitEvent(srv, UDS_SRV_EVT_DoScheduledReset, &srv->ecuResetScheduled); + } + + UDSTpStatus_t tpStatus = UDSTpPoll(srv->tp); + + UDSReq_t *r = &srv->r; + + if (srv->requestInProgress) { + if (srv->RCRRP) { + // responds only if + // 1. changed (no longer RCRRP), or + // 2. p2_timer has elapsed + uint8_t response = evaluateServiceResponse(srv, r); + if (kRequestCorrectlyReceived_ResponsePending == response) { + // it's the second time the service has responded with RCRRP + srv->notReadyToReceive = true; + } else { + // No longer RCRRP'ing + srv->RCRRP = false; + srv->notReadyToReceive = false; + + // Not a consecutive 0x78 response, use p2 instead of p2_star * 0.3 + srv->p2_timer = UDSMillis() + srv->p2_ms; + } + } + + if (UDSTimeAfter(UDSMillis(), srv->p2_timer)) { + printf("len: %zu\n", r->send_len); + ssize_t ret = UDSTpSend(srv->tp, r->send_buf, r->send_len, NULL); + // TODO test injection of transport errors: + if (ret < 0) { + UDSErr_t err = UDS_ERR_TPORT; + EmitEvent(srv, UDS_SRV_EVT_Err, &err); + UDS_DBG_PRINT("UDSTpSend failed with %zd\n", ret); + } + + if (srv->RCRRP) { + // ISO14229-2:2013 Table 4 footnote b + // min time between consecutive 0x78 responses is 0.3 * p2* + uint32_t wait_time = srv->p2_star_ms * 3 / 10; + srv->p2_timer = UDSMillis() + wait_time; + } else { + srv->p2_timer = UDSMillis() + srv->p2_ms; + UDSTpAckRecv(srv->tp); + srv->requestInProgress = false; + } + } + + } else { + if (srv->notReadyToReceive) { + return; // cannot respond to request right now + } + r->recv_len = UDSTpPeek(srv->tp, &r->recv_buf, &r->info); + r->send_buf_size = UDSTpGetSendBuf(srv->tp, &r->send_buf); + if (r->recv_len > 0) { + if (r->send_buf == NULL) { + UDS_DBG_PRINT("Send buf null\n"); + } + if (r->recv_buf == NULL) { + UDS_DBG_PRINT("Recv buf null\n"); + } + if (r->send_buf == NULL || r->recv_buf == NULL) { + UDSErr_t err = UDS_ERR_TPORT; + EmitEvent(srv, UDS_SRV_EVT_Err, &err); + UDS_DBG_PRINT("bad tport\n"); + return; + } + uint8_t response = evaluateServiceResponse(srv, r); + srv->requestInProgress = true; + if (kRequestCorrectlyReceived_ResponsePending == response) { + srv->RCRRP = true; + } + } + } +} \ No newline at end of file diff --git a/src/server.h b/src/server.h new file mode 100644 index 0000000..a741232 --- /dev/null +++ b/src/server.h @@ -0,0 +1,169 @@ +#pragma once + +#include "sys.h" +#include "tp.h" +#include "uds.h" + +/** + * @brief Server request context + */ +typedef struct { + uint8_t *recv_buf; + uint8_t *send_buf; + size_t recv_len; + size_t send_len; + size_t send_buf_size; + UDSSDU_t info; +} UDSReq_t; + +typedef struct UDSServer { + UDSTpHandle_t *tp; + uint8_t (*fn)(struct UDSServer *srv, UDSServerEvent_t event, const void *arg); + + /** + * @brief \~chinese 服务器时间参数（毫秒） \~ Server time constants (milliseconds) \~ + */ + uint16_t p2_ms; // Default P2_server_max timing supported by the server for + // the activated diagnostic session. + uint32_t p2_star_ms; // Enhanced (NRC 0x78) P2_server_max supported by the + // server for the activated diagnostic session. + uint16_t s3_ms; // Session timeout + + uint8_t ecuResetScheduled; // nonzero indicates that an ECUReset has been scheduled + uint32_t ecuResetTimer; // for delaying resetting until a response + // has been sent to the client + uint32_t p2_timer; // for rate limiting server responses + uint32_t s3_session_timeout_timer; // indicates that diagnostic session has timed out + uint32_t sec_access_auth_fail_timer; // brute-force hardening: rate limit security access requests + + /** + * @brief UDS-1-2013: Table 407 - 0x36 TransferData Supported negative + * response codes requires that the server keep track of whether the + * transfer is active + */ + bool xferIsActive; + // UDS-1-2013: 14.4.2.3, Table 404: The blockSequenceCounter parameter + // value starts at 0x01 + uint8_t xferBlockSequenceCounter; + size_t xferTotalBytes; // total transfer size in bytes requested by the client + size_t xferByteCounter; // total number of bytes transferred + size_t xferBlockLength; // block length (convenience for the TransferData API) + + uint8_t sessionType; // diagnostic session type (0x10) + uint8_t securityLevel; // SecurityAccess (0x27) level + + bool RCRRP; // set to true when user fn returns 0x78 and false otherwise + bool requestInProgress; // set to true when a request has been processed but the response has + // not yet been sent + + // UDS-1 2013 defines the following conditions under which the server does not + // process incoming requests: + // - not ready to receive (Table A.1 0x78) + // - not accepting request messages and not sending responses (9.3.1) + // + // when this variable is set to true, incoming ISO-TP data will not be processed. + bool notReadyToReceive; + + UDSReq_t r; +} UDSServer_t; + +// TODO: Remove +typedef struct { + uint8_t (*fn)(UDSServer_t *srv, UDSServerEvent_t event, const void *arg); + UDSTpHandle_t *tp; +} UDSServerConfig_t; + +typedef struct { + const uint8_t type; /*! requested diagnostic session type (enum UDSDiagnosticSessionType) */ + uint16_t p2_ms; /*! optional: p2 timing override */ + uint32_t p2_star_ms; /*! optional: p2* timing override */ +} UDSDiagSessCtrlArgs_t; + +typedef struct { + const uint8_t type; /**< \~chinese 客户端请求的复位类型 \~english reset type requested by client + (enum UDSECUResetType) */ + uint32_t powerDownTimeMillis; /**< when this much time has elapsed after a kPositiveResponse, a + UDS_SRV_EVT_DoScheduledReset will be issued */ +} UDSECUResetArgs_t; + +typedef struct { + const uint16_t dataId; /*! RDBI Data Identifier */ + uint8_t (*copy)(UDSServer_t *srv, const void *src, + uint16_t count); /*! function for copying data */ +} UDSRDBIArgs_t; + +typedef struct { + const void *memAddr; + const size_t memSize; + uint8_t (*copy)(UDSServer_t *srv, const void *src, + uint16_t count); /*! function for copying data */ +} UDSReadMemByAddrArgs_t; + +typedef struct { + uint8_t ctrlType; /* enum UDSCommunicationControlType */ + uint8_t commType; /* enum UDSCommunicationType */ +} UDSCommCtrlArgs_t; + +typedef struct { + const uint8_t level; /*! requested security level */ + const uint8_t *const dataRecord; /*! pointer to request data */ + const uint16_t len; /*! size of request data */ + uint8_t (*copySeed)(UDSServer_t *srv, const void *src, + uint16_t len); /*! function for copying data */ +} UDSSecAccessRequestSeedArgs_t; + +typedef struct { + const uint8_t level; /*! security level to be validated */ + const uint8_t *const key; /*! key sent by client */ + const uint16_t len; /*! length of key */ +} UDSSecAccessValidateKeyArgs_t; + +typedef struct { + const uint16_t dataId; /*! WDBI Data Identifier */ + const uint8_t *const data; /*! pointer to data */ + const uint16_t len; /*! length of data */ +} UDSWDBIArgs_t; + +typedef struct { + const uint8_t ctrlType; /*! routineControlType */ + const uint16_t id; /*! routineIdentifier */ + const uint8_t *optionRecord; /*! optional data */ + const uint16_t len; /*! length of optional data */ + uint8_t (*copyStatusRecord)(UDSServer_t *srv, const void *src, + uint16_t len); /*! function for copying response data */ +} UDSRoutineCtrlArgs_t; + +typedef struct { + const void *addr; /*! requested address */ + const size_t size; /*! requested download size */ + const uint8_t dataFormatIdentifier; /*! optional specifier for format of data */ + uint16_t maxNumberOfBlockLength; /*! optional response: inform client how many data bytes to + send in each `TransferData` request */ +} UDSRequestDownloadArgs_t; + +typedef struct { + const void *addr; /*! requested address */ + const size_t size; /*! requested download size */ + const uint8_t dataFormatIdentifier; /*! optional specifier for format of data */ + uint16_t maxNumberOfBlockLength; /*! optional response: inform client how many data bytes to + send in each `TransferData` request */ +} UDSRequestUploadArgs_t; + +typedef struct { + const uint8_t *const data; /*! transfer data */ + const uint16_t len; /*! transfer data length */ + const uint16_t maxRespLen; /*! don't send more than this many bytes with copyResponse */ + uint8_t (*copyResponse)( + UDSServer_t *srv, const void *src, + uint16_t len); /*! function for copying transfer data response data (optional) */ +} UDSTransferDataArgs_t; + +typedef struct { + const uint8_t *const data; /*! request data */ + const uint16_t len; /*! request data length */ + uint8_t (*copyResponse)(UDSServer_t *srv, const void *src, + uint16_t len); /*! function for copying response data (optional) */ +} UDSRequestTransferExitArgs_t; + +UDSErr_t UDSServerInit(UDSServer_t *srv); +void UDSServerPoll(UDSServer_t *srv); diff --git a/src/sys.h b/src/sys.h new file mode 100644 index 0000000..417e908 --- /dev/null +++ b/src/sys.h @@ -0,0 +1,30 @@ +#pragma once + +#define UDS_SYS_CUSTOM 0 +#define UDS_SYS_UNIX 1 +#define UDS_SYS_WINDOWS 2 +#define UDS_SYS_ARDUINO 3 +#define UDS_SYS_ESP32 4 + + +#if !defined(UDS_SYS) + +#if defined(__unix__) || defined(__APPLE__) +#define UDS_SYS UDS_SYS_UNIX +#elif defined(_WIN32) +#define UDS_SYS UDS_SYS_WINDOWS +#elif defined(ARDUINO) +#define UDS_SYS UDS_SYS_ARDUINO +#elif defined(ESP_PLATFORM) +#define UDS_SYS UDS_SYS_ESP32 +#else +#define UDS_SYS UDS_SYS_CUSTOM +#endif + +#endif + +#include "sys_unix.h" +#include "sys_win32.h" +#include "sys_arduino.h" +#include "sys_esp32.h" + diff --git a/src/sys_arduino.h b/src/sys_arduino.h new file mode 100644 index 0000000..39e6731 --- /dev/null +++ b/src/sys_arduino.h @@ -0,0 +1,16 @@ +#pragma once + +#if UDS_SYS==UDS_SYS_ARDUINO + +#include +#include +#include +#include + +#define UDS_TP UDS_TP_ISOTP_C +#define UDS_ENABLE_DBG_PRINT 1 +#define UDS_ENABLE_ASSERT 1 +int print_impl(const char *fmt, ...); +#define UDS_DBG_PRINT_IMPL print_impl + +#endif diff --git a/src/sys_esp32.h b/src/sys_esp32.h new file mode 100644 index 0000000..6a2df9f --- /dev/null +++ b/src/sys_esp32.h @@ -0,0 +1,14 @@ +#pragma once + +#if UDS_SYS==UDS_SYS_ESP32 + +#include +#include +#include + +#define UDS_TP UDS_TP_ISOTP_C +#define UDS_ENABLE_DBG_PRINT 1 +#define UDS_ENABLE_ASSERT 1 + +#endif + diff --git a/src/sys_unix.h b/src/sys_unix.h new file mode 100644 index 0000000..b2baf19 --- /dev/null +++ b/src/sys_unix.h @@ -0,0 +1,12 @@ +#pragma once + +#if UDS_SYS==UDS_SYS_UNIX + +#include +#include +#include +#include +#include +#include + +#endif diff --git a/src/sys_win32.h b/src/sys_win32.h new file mode 100644 index 0000000..25d6c43 --- /dev/null +++ b/src/sys_win32.h @@ -0,0 +1,8 @@ +#pragma once + +#if UDS_SYS==UDS_SYS_WIN32 + +#include +typedef SSIZE_T ssize_t; + +#endif diff --git a/src/tp.c b/src/tp.c new file mode 100644 index 0000000..63106d7 --- /dev/null +++ b/src/tp.c @@ -0,0 +1,64 @@ +#include "tp.h" + +/** + * @brief + * + * @param hdl + * @param info, if NULL, the default values are used: + * A_Mtype: message type (diagnostic (DEFAULT), remote diagnostic, secure diagnostic, secure + * remote diagnostic) + * A_TA_Type: application target address type (physical (DEFAULT) or functional) + * A_SA: unused + * A_TA: unused + * A_AE: unused + * @return ssize_t + */ +ssize_t UDSTpGetSendBuf(struct UDSTpHandle *hdl, uint8_t **buf) { + assert(hdl); + assert(hdl->get_send_buf); + return hdl->get_send_buf(hdl, buf); +} +ssize_t UDSTpSend(struct UDSTpHandle *hdl, const uint8_t *buf, ssize_t len, UDSSDU_t *info) { + assert(hdl); + assert(hdl->send); + return hdl->send(hdl, (uint8_t *)buf, len, info); +} + +UDSTpStatus_t UDSTpPoll(struct UDSTpHandle *hdl) { + assert(hdl); + assert(hdl->poll); + return hdl->poll(hdl); +} + +ssize_t UDSTpPeek(struct UDSTpHandle *hdl, uint8_t **buf, UDSSDU_t *info) { + assert(hdl); + assert(hdl->peek); + return hdl->peek(hdl, buf, info); +} + +const uint8_t *UDSTpGetRecvBuf(struct UDSTpHandle *hdl, size_t *p_len) { + assert(hdl); + ssize_t len = 0; + uint8_t *buf = NULL; + len = UDSTpPeek(hdl, &buf, NULL); + if (len > 0) { + if (p_len) { + *p_len = len; + } + return buf; + } else { + return NULL; + } +} + +size_t UDSTpGetRecvLen(UDSTpHandle_t *hdl) { + assert(hdl); + size_t len = 0; + UDSTpGetRecvBuf(hdl, &len); + return len; +} + +void UDSTpAckRecv(UDSTpHandle_t *hdl) { + assert(hdl); + hdl->ack_recv(hdl); +} diff --git a/src/tp.h b/src/tp.h new file mode 100644 index 0000000..966cab5 --- /dev/null +++ b/src/tp.h @@ -0,0 +1,128 @@ +#pragma once + +#include "sys.h" + +#if !defined(UDS_TP) +#if (UDS_SYS == UDS_SYS_UNIX) +#define UDS_TP UDS_TP_ISOTP_SOCKET +#endif +#endif + +#include +#include +#include +#include +#include + +#if (UDS_TP == UDS_TP_ISOTP_C) +#include "tp/isotp-c/isotp.h" +#include "tp/isotp-c/isotp_config.h" +#include "tp/isotp-c/isotp_defines.h" +#elif (UDS_TP == UDS_TP_ISOTP_SOCKET) +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif + + +enum UDSTpStatusFlags { + UDS_TP_IDLE = 0x00000000, + UDS_TP_SEND_IN_PROGRESS = 0x00000001, + UDS_TP_RECV_COMPLETE = 0x00000002, +}; + +typedef uint32_t UDSTpStatus_t; + +typedef enum { + UDS_A_MTYPE_DIAG = 0, + UDS_A_MTYPE_REMOTE_DIAG, + UDS_A_MTYPE_SECURE_DIAG, + UDS_A_MTYPE_SECURE_REMOTE_DIAG, +} UDS_A_Mtype_t; + +typedef enum { + UDS_A_TA_TYPE_PHYSICAL = 0, // unicast (1:1) + UDS_A_TA_TYPE_FUNCTIONAL, // multicast +} UDS_A_TA_Type_t; + +typedef uint8_t UDSTpAddr_t; + +/** + * @brief Service data unit (SDU) + * @details data interface between the application layer and the transport layer + */ +typedef struct { + UDS_A_Mtype_t A_Mtype; // message type (diagnostic, remote diagnostic, secure diagnostic, secure + // remote diagnostic) + uint16_t A_SA; // application source address + uint16_t A_TA; // application target address + UDS_A_TA_Type_t A_TA_Type; // application target address type (physical or functional) + uint16_t A_AE; // application layer remote address +} UDSSDU_t; + +#define UDS_TP_NOOP_ADDR (0xFFFFFFFF) + +/** + * @brief Interface to OSI layer 4 (transport layer) + * @note implementers should embed this struct at offset zero in their own transport layer handle + */ +typedef struct UDSTpHandle { + /** + * @brief Get the transport layer's send buffer + * @param hdl: pointer to transport handle + * @param buf: double pointer which will be pointed to the send buffer + * @return size of transport layer's send buffer on success, -1 on error + */ + ssize_t (*get_send_buf)(struct UDSTpHandle *hdl, uint8_t **p_buf); + + /** + * @brief Send the data in the buffer buf + * @param hdl: pointer to transport handle + * @param buf: a pointer to the data to send (this may be the buffer returned by @ref + * get_send_buf) + * @param info: pointer to SDU info (may be NULL). If NULL, implementation should send with + * physical addressing + */ + ssize_t (*send)(struct UDSTpHandle *hdl, uint8_t *buf, size_t len, UDSSDU_t *info); + + /** + * @brief Poll the transport layer. + * @param hdl: pointer to transport handle + * @note the transport layer user is responsible for calling this function periodically + * @note threaded implementations like linux isotp sockets don't need to do anything here. + * @return UDS_TP_IDLE if idle, otherwise UDS_TP_SEND_IN_PROGRESS or UDS_TP_RECV_COMPLETE + */ + UDSTpStatus_t (*poll)(struct UDSTpHandle *hdl); + + /** + * @brief Peek at the received data + * @param hdl: pointer to transport handle + * @param buf: set to the received data + * @param info: filled with SDU info by the callee if not NULL + * @return size of received data on success, -1 on error + * @note The transport will be unable to receive further data until @ref ack_recv is called + * @note The information returned by peek will not change until @ref ack_recv is called + */ + ssize_t (*peek)(struct UDSTpHandle *hdl, uint8_t **buf, UDSSDU_t *info); + + /** + * @brief Acknowledge that the received data has been processed and may be discarded + * @param hdl: pointer to transport handle + * @note: after ack_recv() is called and before new messages are received, peek must return 0. + */ + void (*ack_recv)(struct UDSTpHandle *hdl); +} UDSTpHandle_t; + +ssize_t UDSTpGetSendBuf(UDSTpHandle_t *hdl, uint8_t **buf); +ssize_t UDSTpSend(UDSTpHandle_t *hdl, const uint8_t *buf, ssize_t len, UDSSDU_t *info); +UDSTpStatus_t UDSTpPoll(UDSTpHandle_t *hdl); +ssize_t UDSTpPeek(struct UDSTpHandle *hdl, uint8_t **buf, UDSSDU_t *info); +const uint8_t *UDSTpGetRecvBuf(UDSTpHandle_t *hdl, size_t *len); +size_t UDSTpGetRecvLen(UDSTpHandle_t *hdl); +void UDSTpAckRecv(UDSTpHandle_t *hdl); diff --git a/src/tp/README.md b/src/tp/README.md new file mode 100644 index 0000000..ccba17e --- /dev/null +++ b/src/tp/README.md @@ -0,0 +1,66 @@ + +The transport layer / session layer interface is a little complex. + +- see iso14229.h `UDSTpHandle_t` + - implement all callback functions +- do sanity checks on init + +```c +#include "assert.h" +assert(cfg->source_addr != cfg->target_addr); +assert(cfg->target_addr != cfg->source_addr_func); +assert(cfg->source_addr_func != cfg->source_addr); + +assert(cfg->tp->recv); +assert(cfg->tp->send); +assert(cfg->tp->poll); + +#if foo +#elif UDS_TP == UDS_TP_ISOTP_C + assert(cfg->target_addr != cfg->source_addr_func && cfg->source_addr_func != cfg->source_addr); + UDSTpIsoTpC_t *tp = &self->tp_impl; + isotp_init_link(&tp->phys_link, cfg->target_addr, self->send_buf, self->send_buf_size, + self->recv_buf, self->recv_buf_size); + isotp_init_link(&tp->func_link, cfg->target_addr, tp->func_send_buf, sizeof(tp->func_send_buf), + tp->func_recv_buf, sizeof(tp->func_recv_buf)); + self->tp = (UDSTpHandle_t *)tp; + self->tp->poll = tp_poll; + self->tp->send = tp_send; + self->tp->recv = tp_recv; +#elif UDS_TP == UDS_TP_ISOTP_SOCKET + self->tp = (UDSTpHandle_t *)&self->tp_impl; + if (LinuxSockTpOpen(self->tp, cfg->if_name, cfg->source_addr, cfg->target_addr, + cfg->source_addr_func, cfg->target_addr)) { + return UDS_ERR; + } + +// client + +#if UDS_TP == UDS_TP_CUSTOM + assert(cfg->tp); + assert(cfg->tp->recv); + assert(cfg->tp->send); + assert(cfg->tp->poll); + client->tp = cfg->tp; +#elif UDS_TP == UDS_TP_ISOTP_C + assert(cfg->source_addr != cfg->target_addr_func && cfg->target_addr_func != cfg->target_addr); + UDSTpIsoTpC_t *tp = (UDSTpIsoTpC_t *)&client->tp_impl; + isotp_init_link(&tp->phys_link, cfg->target_addr, client->send_buf, client->send_buf_size, + client->recv_buf, client->recv_buf_size); + isotp_init_link(&tp->func_link, cfg->target_addr_func, tp->func_send_buf, + sizeof(tp->func_send_buf), tp->func_recv_buf, sizeof(tp->func_recv_buf)); + client->tp = (UDSTpHandle_t *)tp; + client->tp->poll = tp_poll; + client->tp->send = tp_send; + client->tp->recv = tp_recv; +#elif UDS_TP == UDS_TP_ISOTP_SOCKET + client->tp = (UDSTpHandle_t *)&client->tp_impl; + if (LinuxSockTpOpen(client->tp, cfg->if_name, cfg->source_addr, cfg->target_addr, + cfg->source_addr, cfg->target_addr_func)) { + return UDS_ERR; + } + assert(client->tp); +#endif + + +``` \ No newline at end of file diff --git a/tp/isotp-c b/src/tp/isotp-c similarity index 100% rename from tp/isotp-c rename to src/tp/isotp-c diff --git a/tp/isotp_c.c b/src/tp/isotp_c.c similarity index 91% rename from tp/isotp_c.c rename to src/tp/isotp_c.c index a0a44f1..05f6f76 100644 --- a/tp/isotp_c.c +++ b/src/tp/isotp_c.c @@ -1,12 +1,15 @@ + +#include +#if UDS_TP == UDS_TP_ISOTP_C + +#include "util.h" #include "tp/isotp_c.h" #include "tp/isotp-c/isotp.h" -#include -#include static UDSTpStatus_t tp_poll(UDSTpHandle_t *hdl) { assert(hdl); UDSTpStatus_t status = 0; - UDSTpISOTpC_t *impl = (UDSTpISOTpC_t *)hdl; + UDSISOTpC_t *impl = (UDSISOTpC_t *)hdl; isotp_poll(&impl->phys_link); if (impl->phys_link.send_status == ISOTP_SEND_STATUS_INPROGRESS) { status |= UDS_TP_SEND_IN_PROGRESS; @@ -42,7 +45,7 @@ int peek_link(IsoTpLink *link, uint8_t *buf, size_t bufsize, bool functional) { static ssize_t tp_peek(UDSTpHandle_t *hdl, uint8_t **p_buf, UDSSDU_t *info) { assert(hdl); assert(p_buf); - UDSTpISOTpC_t *tp = (UDSTpISOTpC_t *)hdl; + UDSISOTpC_t *tp = (UDSISOTpC_t *)hdl; if (ISOTP_RECEIVE_STATUS_FULL == tp->phys_link.receive_status) { // recv not yet acked *p_buf = tp->recv_buf; return tp->phys_link.receive_size; @@ -89,7 +92,7 @@ static ssize_t tp_peek(UDSTpHandle_t *hdl, uint8_t **p_buf, UDSSDU_t *info) { static ssize_t tp_send(UDSTpHandle_t *hdl, uint8_t *buf, size_t len, UDSSDU_t *info) { assert(hdl); ssize_t ret = -1; - UDSTpISOTpC_t *tp = (UDSTpISOTpC_t *)hdl; + UDSISOTpC_t *tp = (UDSISOTpC_t *)hdl; IsoTpLink *link = NULL; const UDSTpAddr_t ta_type = info ? info->A_TA_Type : UDS_A_TA_TYPE_PHYSICAL; const uint32_t ta = ta_type == UDS_A_TA_TYPE_PHYSICAL ? tp->phys_ta : tp->func_ta; @@ -128,17 +131,20 @@ static ssize_t tp_send(UDSTpHandle_t *hdl, uint8_t *buf, size_t len, UDSSDU_t *i static void tp_ack_recv(UDSTpHandle_t *hdl) { assert(hdl); printf("ack recv\n"); - UDSTpISOTpC_t *tp = (UDSTpISOTpC_t *)hdl; + UDSISOTpC_t *tp = (UDSISOTpC_t *)hdl; + uint16_t out_size = 0; + isotp_receive(&tp->phys_link, tp->recv_buf, sizeof(tp->recv_buf), &out_size); + } static ssize_t tp_get_send_buf(UDSTpHandle_t *hdl, uint8_t **p_buf) { assert(hdl); - UDSTpISOTpC_t *tp = (UDSTpISOTpC_t *)hdl; + UDSISOTpC_t *tp = (UDSISOTpC_t *)hdl; *p_buf = tp->send_buf; return sizeof(tp->send_buf); } -UDSErr_t UDSTpISOTpCInit(UDSTpISOTpC_t *tp, UDSTpISOTpCConfig_t *cfg) { +UDSErr_t UDSISOTpCInit(UDSISOTpC_t *tp, const UDSISOTpCConfig_t *cfg) { if (cfg == NULL || tp == NULL) { return UDS_ERR; } @@ -159,4 +165,6 @@ UDSErr_t UDSTpISOTpCInit(UDSTpISOTpC_t *tp, UDSTpISOTpCConfig_t *cfg) { sizeof(tp->recv_buf), UDSMillis, cfg->isotp_user_send_can, cfg->isotp_user_debug, cfg->user_data); return UDS_OK; -} \ No newline at end of file +} + +#endif diff --git a/tp/isotp_c.h b/src/tp/isotp_c.h similarity index 76% rename from tp/isotp_c.h rename to src/tp/isotp_c.h index 84f6f98..0226576 100644 --- a/tp/isotp_c.h +++ b/src/tp/isotp_c.h @@ -1,7 +1,10 @@ -#ifndef ISOTP_C_H -#define ISOTP_C_H +#pragma once +#if UDS_TP == UDS_TP_ISOTP_C -#include "iso14229.h" +#include "sys.h" +#include "config.h" +#include "uds.h" +#include "tp.h" #include "tp/isotp-c/isotp.h" typedef struct { @@ -12,7 +15,7 @@ typedef struct { uint8_t recv_buf[UDS_ISOTP_MTU]; uint32_t phys_sa, phys_ta; uint32_t func_sa, func_ta; -} UDSTpISOTpC_t; +} UDSISOTpC_t; typedef struct { uint32_t source_addr; @@ -25,10 +28,10 @@ typedef struct { uint32_t (*isotp_user_get_ms)(void); /* get millisecond */ void (*isotp_user_debug)(const char *message, ...); /* print debug message */ void *user_data; /* user data */ -} UDSTpISOTpCConfig_t; +} UDSISOTpCConfig_t; -UDSErr_t UDSTpISOTpCInit(UDSTpISOTpC_t *tp, UDSTpISOTpCConfig_t *cfg); +UDSErr_t UDSISOTpCInit(UDSISOTpC_t *tp, const UDSISOTpCConfig_t *cfg); -void UDSTpISOTpCDeinit(UDSTpISOTpC_t *tp); +void UDSISOTpCDeinit(UDSISOTpC_t *tp); -#endif +#endif diff --git a/tp/isotp_c_socketcan.c b/src/tp/isotp_c_socketcan.c similarity index 87% rename from tp/isotp_c_socketcan.c rename to src/tp/isotp_c_socketcan.c index 5c5ade5..1977c32 100644 --- a/tp/isotp_c_socketcan.c +++ b/src/tp/isotp_c_socketcan.c @@ -1,3 +1,5 @@ +#if defined(UDS_TP_ISOTP_C_SOCKETCAN) + #include "tp/isotp_c_socketcan.h" #include "iso14229.h" #include "tp/isotp-c/isotp_defines.h" @@ -99,7 +101,7 @@ static void SocketCANRecv(UDSTpISOTpC_t *tp) { } } -static UDSTpStatus_t tp_poll(UDSTpHandle_t *hdl) { +static UDSTpStatus_t isotp_c_socketcan_tp_poll(UDSTpHandle_t *hdl) { assert(hdl); UDSTpStatus_t status = 0; UDSTpISOTpC_t *impl = (UDSTpISOTpC_t *)hdl; @@ -111,7 +113,7 @@ static UDSTpStatus_t tp_poll(UDSTpHandle_t *hdl) { return status; } -int peek_link(IsoTpLink *link, uint8_t *buf, size_t bufsize, bool functional) { +static int isotp_c_socketcan_tp_peek_link(IsoTpLink *link, uint8_t *buf, size_t bufsize, bool functional) { assert(link); assert(buf); int ret = -1; @@ -136,7 +138,7 @@ int peek_link(IsoTpLink *link, uint8_t *buf, size_t bufsize, bool functional) { return ret; } -static ssize_t tp_peek(UDSTpHandle_t *hdl, uint8_t **p_buf, UDSSDU_t *info) { +static ssize_t isotp_c_socketcan_tp_peek(UDSTpHandle_t *hdl, uint8_t **p_buf, UDSSDU_t *info) { assert(hdl); assert(p_buf); UDSTpISOTpC_t *tp = (UDSTpISOTpC_t *)hdl; @@ -145,7 +147,7 @@ static ssize_t tp_peek(UDSTpHandle_t *hdl, uint8_t **p_buf, UDSSDU_t *info) { return tp->phys_link.receive_size; } int ret = -1; - ret = peek_link(&tp->phys_link, tp->recv_buf, sizeof(tp->recv_buf), false); + ret = isotp_c_socketcan_tp_peek_link(&tp->phys_link, tp->recv_buf, sizeof(tp->recv_buf), false); UDS_A_TA_Type_t ta_type = UDS_A_TA_TYPE_PHYSICAL; uint32_t ta = tp->phys_ta; uint32_t sa = tp->phys_sa; @@ -160,7 +162,7 @@ static ssize_t tp_peek(UDSTpHandle_t *hdl, uint8_t **p_buf, UDSSDU_t *info) { } else if (ret < 0) { goto done; } else { - ret = peek_link(&tp->func_link, tp->recv_buf, sizeof(tp->recv_buf), true); + ret = isotp_c_socketcan_tp_peek_link(&tp->func_link, tp->recv_buf, sizeof(tp->recv_buf), true); if (ret > 0) { printf("just got %d bytes on func link \n", ret); ta = tp->func_sa; @@ -190,7 +192,7 @@ static ssize_t tp_peek(UDSTpHandle_t *hdl, uint8_t **p_buf, UDSSDU_t *info) { return ret; } -static ssize_t tp_send(UDSTpHandle_t *hdl, uint8_t *buf, size_t len, UDSSDU_t *info) { +static ssize_t isotp_c_socketcan_tp_send(UDSTpHandle_t *hdl, uint8_t *buf, size_t len, UDSSDU_t *info) { assert(hdl); ssize_t ret = -1; UDSTpISOTpC_t *tp = (UDSTpISOTpC_t *)hdl; @@ -237,13 +239,15 @@ static ssize_t tp_send(UDSTpHandle_t *hdl, uint8_t *buf, size_t len, UDSSDU_t *i return ret; } -static void tp_ack_recv(UDSTpHandle_t *hdl) { +static void isotp_c_socketcan_tp_ack_recv(UDSTpHandle_t *hdl) { assert(hdl); printf("ack recv\n"); UDSTpISOTpC_t *tp = (UDSTpISOTpC_t *)hdl; + uint16_t out_size = 0; + isotp_receive(&tp->phys_link, tp->recv_buf, sizeof(tp->recv_buf), &out_size); } -static ssize_t tp_get_send_buf(UDSTpHandle_t *hdl, uint8_t **p_buf) { +static ssize_t isotp_c_socketcan_tp_get_send_buf(UDSTpHandle_t *hdl, uint8_t **p_buf) { assert(hdl); UDSTpISOTpC_t *tp = (UDSTpISOTpC_t *)hdl; *p_buf = tp->send_buf; @@ -255,11 +259,11 @@ UDSErr_t UDSTpISOTpCInit(UDSTpISOTpC_t *tp, const char *ifname, uint32_t source_ uint32_t target_addr_func) { assert(tp); assert(ifname); - tp->hdl.poll = tp_poll; - tp->hdl.send = tp_send; - tp->hdl.peek = tp_peek; - tp->hdl.ack_recv = tp_ack_recv; - tp->hdl.get_send_buf = tp_get_send_buf; + tp->hdl.poll = isotp_c_socketcan_tp_poll; + tp->hdl.send = isotp_c_socketcan_tp_send; + tp->hdl.peek = isotp_c_socketcan_tp_peek; + tp->hdl.ack_recv = isotp_c_socketcan_tp_ack_recv; + tp->hdl.get_send_buf = isotp_c_socketcan_tp_get_send_buf; tp->phys_sa = source_addr; tp->phys_ta = target_addr; tp->func_sa = source_addr_func; @@ -279,4 +283,6 @@ void UDSTpISOTpCDeinit(UDSTpISOTpC_t *tp) { assert(tp); close(tp->fd); tp->fd = -1; -} \ No newline at end of file +} + +#endif \ No newline at end of file diff --git a/tp/isotp_c_socketcan.h b/src/tp/isotp_c_socketcan.h similarity index 90% rename from tp/isotp_c_socketcan.h rename to src/tp/isotp_c_socketcan.h index 16666fa..893a62a 100644 --- a/tp/isotp_c_socketcan.h +++ b/src/tp/isotp_c_socketcan.h @@ -1,9 +1,10 @@ -#ifndef ISOTP_C_SOCKETCAN_H -#define ISOTP_C_SOCKETCAN_H +#pragma once #include "iso14229.h" #include "tp/isotp-c/isotp.h" +#if defined(UDS_TP_ISOTP_C_SOCKETCAN) + typedef struct { UDSTpHandle_t hdl; IsoTpLink phys_link; @@ -21,4 +22,4 @@ UDSErr_t UDSTpISOTpCInit(UDSTpISOTpC_t *tp, const char *ifname, uint32_t source_ uint32_t target_addr_func); void UDSTpISOTpCDeinit(UDSTpISOTpC_t *tp); -#endif \ No newline at end of file +#endif diff --git a/tp/isotp_sock.c b/src/tp/isotp_sock.c similarity index 88% rename from tp/isotp_sock.c rename to src/tp/isotp_sock.c index f642367..966542b 100644 --- a/tp/isotp_sock.c +++ b/src/tp/isotp_sock.c @@ -1,3 +1,5 @@ +#if defined(UDS_TP_ISOTP_SOCK) + #include "tp/isotp_sock.h" #include "iso14229.h" #include @@ -11,7 +13,7 @@ #include #include -static UDSTpStatus_t tp_poll(UDSTpHandle_t *hdl) { return 0; } +static UDSTpStatus_t isotp_sock_tp_poll(UDSTpHandle_t *hdl) { return 0; } static ssize_t tp_recv_once(int fd, uint8_t *buf, size_t size) { ssize_t ret = read(fd, buf, size); @@ -28,7 +30,7 @@ static ssize_t tp_recv_once(int fd, uint8_t *buf, size_t size) { return ret; } -static ssize_t tp_peek(UDSTpHandle_t *hdl, uint8_t **p_buf, UDSSDU_t *info) { +static ssize_t isotp_sock_tp_peek(UDSTpHandle_t *hdl, uint8_t **p_buf, UDSSDU_t *info) { assert(hdl); assert(p_buf); ssize_t ret = 0; @@ -78,13 +80,13 @@ static ssize_t tp_peek(UDSTpHandle_t *hdl, uint8_t **p_buf, UDSSDU_t *info) { return ret; } -static void tp_ack_recv(UDSTpHandle_t *hdl) { +static void isotp_sock_tp_ack_recv(UDSTpHandle_t *hdl) { assert(hdl); UDSTpIsoTpSock_t *impl = (UDSTpIsoTpSock_t *)hdl; impl->recv_len = 0; } -static ssize_t tp_send(UDSTpHandle_t *hdl, uint8_t *buf, size_t len, UDSSDU_t *info) { +static ssize_t isotp_sock_tp_send(UDSTpHandle_t *hdl, uint8_t *buf, size_t len, UDSSDU_t *info) { assert(hdl); ssize_t ret = -1; UDSTpIsoTpSock_t *impl = (UDSTpIsoTpSock_t *)hdl; @@ -122,7 +124,7 @@ static ssize_t tp_send(UDSTpHandle_t *hdl, uint8_t *buf, size_t len, UDSSDU_t *i return ret; } -static ssize_t tp_get_send_buf(UDSTpHandle_t *hdl, uint8_t **p_buf) { +static ssize_t isotp_sock_tp_get_send_buf(UDSTpHandle_t *hdl, uint8_t **p_buf) { assert(hdl); UDSTpIsoTpSock_t *impl = (UDSTpIsoTpSock_t *)hdl; *p_buf = impl->send_buf; @@ -180,11 +182,11 @@ static int LinuxSockBind(const char *if_name, uint32_t rxid, uint32_t txid, bool UDSErr_t UDSTpIsoTpSockInitServer(UDSTpIsoTpSock_t *tp, const char *ifname, uint32_t source_addr, uint32_t target_addr, uint32_t source_addr_func) { assert(tp); - tp->hdl.peek = tp_peek; - tp->hdl.send = tp_send; - tp->hdl.poll = tp_poll; - tp->hdl.ack_recv = tp_ack_recv; - tp->hdl.get_send_buf = tp_get_send_buf; + tp->hdl.peek = isotp_sock_tp_peek; + tp->hdl.send = isotp_sock_tp_send; + tp->hdl.poll = isotp_sock_tp_poll; + tp->hdl.ack_recv = isotp_sock_tp_ack_recv; + tp->hdl.get_send_buf = isotp_sock_tp_get_send_buf; tp->phys_sa = source_addr; tp->phys_ta = target_addr; tp->func_sa = source_addr_func; @@ -205,11 +207,11 @@ UDSErr_t UDSTpIsoTpSockInitServer(UDSTpIsoTpSock_t *tp, const char *ifname, uint UDSErr_t UDSTpIsoTpSockInitClient(UDSTpIsoTpSock_t *tp, const char *ifname, uint32_t source_addr, uint32_t target_addr, uint32_t target_addr_func) { assert(tp); - tp->hdl.peek = tp_peek; - tp->hdl.send = tp_send; - tp->hdl.poll = tp_poll; - tp->hdl.ack_recv = tp_ack_recv; - tp->hdl.get_send_buf = tp_get_send_buf; + tp->hdl.peek = isotp_sock_tp_peek; + tp->hdl.send = isotp_sock_tp_send; + tp->hdl.poll = isotp_sock_tp_poll; + tp->hdl.ack_recv = isotp_sock_tp_ack_recv; + tp->hdl.get_send_buf = isotp_sock_tp_get_send_buf; tp->func_ta = target_addr_func; tp->phys_ta = target_addr; tp->phys_sa = source_addr; @@ -236,3 +238,6 @@ void UDSTpIsoTpSockDeinit(UDSTpIsoTpSock_t *tp) { } } } + +#endif + diff --git a/tp/isotp_sock.h b/src/tp/isotp_sock.h similarity index 93% rename from tp/isotp_sock.h rename to src/tp/isotp_sock.h index ff73688..ac189f4 100644 --- a/tp/isotp_sock.h +++ b/src/tp/isotp_sock.h @@ -1,6 +1,6 @@ -#ifndef TP_ISOTP_SOCK_H -#define TP_ISOTP_SOCK_H +#if defined(UDS_TP_ISOTP_SOCK) +#pragma once #include "iso14229.h" typedef struct { diff --git a/tp/mock.c b/src/tp/mock.c similarity index 89% rename from tp/mock.c rename to src/tp/mock.c index 52d2aed..2ce0f27 100644 --- a/tp/mock.c +++ b/src/tp/mock.c @@ -1,3 +1,5 @@ +#if defined(UDS_TP_MOCK) + #include "tp/mock.h" #include "iso14229.h" #include @@ -59,7 +61,7 @@ static void NetworkPoll() { } } -static ssize_t tp_peek(struct UDSTpHandle *hdl, uint8_t **p_buf, UDSSDU_t *info) { +static ssize_t mock_tp_peek(struct UDSTpHandle *hdl, uint8_t **p_buf, UDSSDU_t *info) { assert(hdl); assert(p_buf); TPMock_t *tp = (TPMock_t *)hdl; @@ -72,7 +74,7 @@ static ssize_t tp_peek(struct UDSTpHandle *hdl, uint8_t **p_buf, UDSSDU_t *info) return tp->recv_len; } -static ssize_t tp_send(struct UDSTpHandle *hdl, uint8_t *buf, size_t len, UDSSDU_t *info) { +static ssize_t mock_tp_send(struct UDSTpHandle *hdl, uint8_t *buf, size_t len, UDSSDU_t *info) { assert(hdl); TPMock_t *tp = (TPMock_t *)hdl; if (MsgCount > NUM_MSGS) { @@ -100,13 +102,13 @@ static ssize_t tp_send(struct UDSTpHandle *hdl, uint8_t *buf, size_t len, UDSSDU return len; } -static UDSTpStatus_t tp_poll(struct UDSTpHandle *hdl) { +static UDSTpStatus_t mock_tp_poll(struct UDSTpHandle *hdl) { NetworkPoll(); // todo: make this status reflect TX time return UDS_TP_IDLE; } -static ssize_t tp_get_send_buf(struct UDSTpHandle *hdl, uint8_t **p_buf) { +static ssize_t mock_tp_get_send_buf(struct UDSTpHandle *hdl, uint8_t **p_buf) { assert(hdl); assert(p_buf); TPMock_t *tp = (TPMock_t *)hdl; @@ -114,7 +116,7 @@ static ssize_t tp_get_send_buf(struct UDSTpHandle *hdl, uint8_t **p_buf) { return sizeof(tp->send_buf); } -static void tp_ack_recv(struct UDSTpHandle *hdl) { +static void mock_tp_ack_recv(struct UDSTpHandle *hdl) { assert(hdl); TPMock_t *tp = (TPMock_t *)hdl; tp->recv_len = 0; @@ -127,11 +129,11 @@ static void TPMockAttach(TPMock_t *tp, TPMockArgs_t *args) { assert(args); assert(TPCount < MAX_NUM_TP); TPs[TPCount++] = tp; - tp->hdl.peek = tp_peek; - tp->hdl.send = tp_send; - tp->hdl.poll = tp_poll; - tp->hdl.get_send_buf = tp_get_send_buf; - tp->hdl.ack_recv = tp_ack_recv; + tp->hdl.peek = mock_tp_peek; + tp->hdl.send = mock_tp_send; + tp->hdl.poll = mock_tp_poll; + tp->hdl.get_send_buf = mock_tp_get_send_buf; + tp->hdl.ack_recv = mock_tp_ack_recv; tp->sa_func = args->sa_func; tp->sa_phys = args->sa_phys; tp->ta_func = args->ta_func; @@ -204,4 +206,6 @@ void TPMockFree(UDSTpHandle_t *tp) { TPMock_t *tpm = (TPMock_t *)tp; TPMockDetach(tpm); free(tp); -} \ No newline at end of file +} + +#endif diff --git a/tp/mock.h b/src/tp/mock.h similarity index 94% rename from tp/mock.h rename to src/tp/mock.h index 53279a6..13cff58 100644 --- a/tp/mock.h +++ b/src/tp/mock.h @@ -4,17 +4,16 @@ * @date 2023-10-21 * */ +#if defined(UDS_TP_MOCK) -#include "iso14229.h" +#pragma once -#ifdef __cplusplus -extern "C" { -#endif +#include "iso14229.h" typedef struct TPMock { UDSTpHandle_t hdl; - uint8_t recv_buf[UDS_BUFSIZE]; - uint8_t send_buf[UDS_BUFSIZE]; + uint8_t recv_buf[UDS_TP_MTU]; + uint8_t send_buf[UDS_TP_MTU]; size_t recv_len; UDSSDU_t recv_info; uint32_t sa_phys; // source address - physical messages are sent from this address @@ -64,6 +63,5 @@ void TPMockLogToStdout(void); */ void TPMockReset(void); -#ifdef __cplusplus -} #endif + diff --git a/src/uds.h b/src/uds.h new file mode 100644 index 0000000..455d11e --- /dev/null +++ b/src/uds.h @@ -0,0 +1,223 @@ +#pragma once + +enum UDSServerEvent { + UDS_SRV_EVT_DiagSessCtrl, // UDSDiagSessCtrlArgs_t * + UDS_SRV_EVT_EcuReset, // UDSECUResetArgs_t * + UDS_SRV_EVT_ReadDataByIdent, // UDSRDBIArgs_t * + UDS_SRV_EVT_ReadMemByAddr, // UDSReadMemByAddrArgs_t * + UDS_SRV_EVT_CommCtrl, // UDSCommCtrlArgs_t * + UDS_SRV_EVT_SecAccessRequestSeed, // UDSSecAccessRequestSeedArgs_t * + UDS_SRV_EVT_SecAccessValidateKey, // UDSSecAccessValidateKeyArgs_t * + UDS_SRV_EVT_WriteDataByIdent, // UDSWDBIArgs_t * + UDS_SRV_EVT_RoutineCtrl, // UDSRoutineCtrlArgs_t* + UDS_SRV_EVT_RequestDownload, // UDSRequestDownloadArgs_t* + UDS_SRV_EVT_RequestUpload, // UDSRequestUploadArgs_t * + UDS_SRV_EVT_TransferData, // UDSTransferDataArgs_t * + UDS_SRV_EVT_RequestTransferExit, // UDSRequestTransferExitArgs_t * + UDS_SRV_EVT_SessionTimeout, // NULL + UDS_SRV_EVT_DoScheduledReset, // enum UDSEcuResetType * + UDS_SRV_EVT_Err, // UDSErr_t * + UDS_EVT_IDLE, + UDS_EVT_RESP_RECV, +}; + +typedef int UDSServerEvent_t; +typedef UDSServerEvent_t UDSEvent_t; + +typedef enum { + UDS_ERR = -1, // 通用错误 + UDS_OK = 0, // 成功 + UDS_ERR_TIMEOUT, // 请求超时 + UDS_ERR_NEG_RESP, // 否定响应 + UDS_ERR_DID_MISMATCH, // 响应DID对不上期待的DID + UDS_ERR_SID_MISMATCH, // 请求和响应SID对不上 + UDS_ERR_SUBFUNCTION_MISMATCH, // 请求和响应SubFunction对不上 + UDS_ERR_TPORT, // 传输层错误 + UDS_ERR_FILE_IO, // 文件IO错误 + UDS_ERR_RESP_TOO_SHORT, // 响应太短 + UDS_ERR_BUFSIZ, // 缓冲器不够大 + UDS_ERR_INVALID_ARG, // 参数不对、没发 + UDS_ERR_BUSY, // 正在忙、没发 +} UDSErr_t; + +typedef enum { + UDSSeqStateDone = 0, + UDSSeqStateRunning = 1, + UDSSeqStateGotoNext = 2, +} UDSSeqState_t; + +enum UDSDiagnosticSessionType { + kDefaultSession = 0x01, + kProgrammingSession = 0x02, + kExtendedDiagnostic = 0x03, + kSafetySystemDiagnostic = 0x04, +}; + +enum { + kPositiveResponse = 0, + kGeneralReject = 0x10, + kServiceNotSupported = 0x11, + kSubFunctionNotSupported = 0x12, + kIncorrectMessageLengthOrInvalidFormat = 0x13, + kResponseTooLong = 0x14, + kBusyRepeatRequest = 0x21, + kConditionsNotCorrect = 0x22, + kRequestSequenceError = 0x24, + kNoResponseFromSubnetComponent = 0x25, + kFailurePreventsExecutionOfRequestedAction = 0x26, + kRequestOutOfRange = 0x31, + kSecurityAccessDenied = 0x33, + kInvalidKey = 0x35, + kExceedNumberOfAttempts = 0x36, + kRequiredTimeDelayNotExpired = 0x37, + kUploadDownloadNotAccepted = 0x70, + kTransferDataSuspended = 0x71, + kGeneralProgrammingFailure = 0x72, + kWrongBlockSequenceCounter = 0x73, + kRequestCorrectlyReceived_ResponsePending = 0x78, + kSubFunctionNotSupportedInActiveSession = 0x7E, + kServiceNotSupportedInActiveSession = 0x7F, + kRpmTooHigh = 0x81, + kRpmTooLow = 0x82, + kEngineIsRunning = 0x83, + kEngineIsNotRunning = 0x84, + kEngineRunTimeTooLow = 0x85, + kTemperatureTooHigh = 0x86, + kTemperatureTooLow = 0x87, + kVehicleSpeedTooHigh = 0x88, + kVehicleSpeedTooLow = 0x89, + kThrottlePedalTooHigh = 0x8A, + kThrottlePedalTooLow = 0x8B, + kTransmissionRangeNotInNeutral = 0x8C, + kTransmissionRangeNotInGear = 0x8D, + kISOSAEReserved = 0x8E, + kBrakeSwitchNotClosed = 0x8F, + kShifterLeverNotInPark = 0x90, + kTorqueConverterClutchLocked = 0x91, + kVoltageTooHigh = 0x92, + kVoltageTooLow = 0x93, +}; + +/** + * @brief LEV_RT_ + * @addtogroup ecuReset_0x11 + */ +enum UDSECUResetType { + kHardReset = 1, + kKeyOffOnReset = 2, + kSoftReset = 3, + kEnableRapidPowerShutDown = 4, + kDisableRapidPowerShutDown = 5, +}; + +typedef uint8_t UDSECUReset_t; + +/** + * @addtogroup securityAccess_0x27 + */ +enum UDSSecurityAccessType { + kRequestSeed = 0x01, + kSendKey = 0x02, +}; + +/** + * @addtogroup communicationControl_0x28 + */ +enum UDSCommunicationControlType { + kEnableRxAndTx = 0, + kEnableRxAndDisableTx = 1, + kDisableRxAndEnableTx = 2, + kDisableRxAndTx = 3, +}; + +/** + * @addtogroup communicationControl_0x28 + */ +enum UDSCommunicationType { + kNormalCommunicationMessages = 0x1, + kNetworkManagementCommunicationMessages = 0x2, + kNetworkManagementCommunicationMessagesAndNormalCommunicationMessages = 0x3, +}; + +/** + * @addtogroup routineControl_0x31 + */ +enum RoutineControlType { + kStartRoutine = 1, + kStopRoutine = 2, + kRequestRoutineResults = 3, +}; + +/** + * @addtogroup controlDTCSetting_0x85 + */ +enum DTCSettingType { + kDTCSettingON = 0x01, + kDTCSettingOFF = 0x02, +}; + + +// ISO-14229-1:2013 Table 2 +#define UDS_MAX_DIAGNOSTIC_SERVICES 0x7F + +#define UDS_RESPONSE_SID_OF(request_sid) (request_sid + 0x40) +#define UDS_REQUEST_SID_OF(response_sid) (response_sid - 0x40) + +#define UDS_NEG_RESP_LEN 3U +#define UDS_0X10_REQ_LEN 2U +#define UDS_0X10_RESP_LEN 6U +#define UDS_0X11_REQ_MIN_LEN 2U +#define UDS_0X11_RESP_BASE_LEN 2U +#define UDS_0X23_REQ_MIN_LEN 4U +#define UDS_0X23_RESP_BASE_LEN 1U +#define UDS_0X22_RESP_BASE_LEN 1U +#define UDS_0X27_REQ_BASE_LEN 2U +#define UDS_0X27_RESP_BASE_LEN 2U +#define UDS_0X28_REQ_BASE_LEN 3U +#define UDS_0X28_RESP_LEN 2U +#define UDS_0X2E_REQ_BASE_LEN 3U +#define UDS_0X2E_REQ_MIN_LEN 4U +#define UDS_0X2E_RESP_LEN 3U +#define UDS_0X31_REQ_MIN_LEN 4U +#define UDS_0X31_RESP_MIN_LEN 4U +#define UDS_0X34_REQ_BASE_LEN 3U +#define UDS_0X34_RESP_BASE_LEN 2U +#define UDS_0X35_REQ_BASE_LEN 3U +#define UDS_0X35_RESP_BASE_LEN 2U +#define UDS_0X36_REQ_BASE_LEN 2U +#define UDS_0X36_RESP_BASE_LEN 2U +#define UDS_0X37_REQ_BASE_LEN 1U +#define UDS_0X37_RESP_BASE_LEN 1U +#define UDS_0X3E_REQ_MIN_LEN 2U +#define UDS_0X3E_REQ_MAX_LEN 2U +#define UDS_0X3E_RESP_LEN 2U +#define UDS_0X85_REQ_BASE_LEN 2U +#define UDS_0X85_RESP_LEN 2U + +enum UDSDiagnosticServiceId { + kSID_DIAGNOSTIC_SESSION_CONTROL = 0x10, + kSID_ECU_RESET = 0x11, + kSID_CLEAR_DIAGNOSTIC_INFORMATION = 0x14, + kSID_READ_DTC_INFORMATION = 0x19, + kSID_READ_DATA_BY_IDENTIFIER = 0x22, + kSID_READ_MEMORY_BY_ADDRESS = 0x23, + kSID_READ_SCALING_DATA_BY_IDENTIFIER = 0x24, + kSID_SECURITY_ACCESS = 0x27, + kSID_COMMUNICATION_CONTROL = 0x28, + kSID_READ_PERIODIC_DATA_BY_IDENTIFIER = 0x2A, + kSID_DYNAMICALLY_DEFINE_DATA_IDENTIFIER = 0x2C, + kSID_WRITE_DATA_BY_IDENTIFIER = 0x2E, + kSID_INPUT_CONTROL_BY_IDENTIFIER = 0x2F, + kSID_ROUTINE_CONTROL = 0x31, + kSID_REQUEST_DOWNLOAD = 0x34, + kSID_REQUEST_UPLOAD = 0x35, + kSID_TRANSFER_DATA = 0x36, + kSID_REQUEST_TRANSFER_EXIT = 0x37, + kSID_REQUEST_FILE_TRANSFER = 0x38, + kSID_WRITE_MEMORY_BY_ADDRESS = 0x3D, + kSID_TESTER_PRESENT = 0x3E, + kSID_ACCESS_TIMING_PARAMETER = 0x83, + kSID_SECURED_DATA_TRANSMISSION = 0x84, + kSID_CONTROL_DTC_SETTING = 0x85, + kSID_RESPONSE_ON_EVENT = 0x86, +}; diff --git a/src/util.c b/src/util.c new file mode 100644 index 0000000..d47ba5b --- /dev/null +++ b/src/util.c @@ -0,0 +1,30 @@ +#include "util.h" + +#if UDS_CUSTOM_MILLIS +#else +uint32_t UDSMillis(void) { +#if UDS_SYS == UDS_SYS_UNIX + struct timeval te; + gettimeofday(&te, NULL); + long long milliseconds = te.tv_sec * 1000LL + te.tv_usec / 1000; + return milliseconds; +#elif UDS_SYS == UDS_SYS_WINDOWS + struct timespec ts; + timespec_get(&ts, TIME_UTC); + long long milliseconds = ts.tv_sec * 1000LL + ts.tv_nsec / 1000000; + return milliseconds; +#elif UDS_SYS == UDS_SYS_ARDUINO + return millis(); +#elif UDS_SYS == UDS_SYS_ESP32 + return esp_timer_get_time() / 1000; +#else +#error "UDSMillis() undefined!" +#endif +} +#endif + +bool UDSSecurityAccessLevelIsReserved(uint8_t securityLevel) { + securityLevel &= 0x3f; + return (0 == securityLevel || (0x43 <= securityLevel && securityLevel >= 0x5E) || + 0x7F == securityLevel); +} diff --git a/src/util.h b/src/util.h new file mode 100644 index 0000000..fda75e2 --- /dev/null +++ b/src/util.h @@ -0,0 +1,42 @@ +#pragma once + +#include "sys.h" +#include "config.h" + +#if UDS_ENABLE_ASSERT +#include +#else +#define assert(x) +#endif + +#if UDS_ENABLE_DBG_PRINT + #if defined(UDS_DBG_PRINT_IMPL) + #define UDS_DBG_PRINT UDS_DBG_PRINT_IMPL + #else + #include + #define UDS_DBG_PRINT printf + #endif +#else +#define UDS_DBG_PRINT(fmt, ...) ((void)fmt) +#endif + +#define UDS_DBG_PRINTHEX(addr, len) \ + for (int i = 0; i < len; i++) { \ + UDS_DBG_PRINT("%02x,", ((uint8_t *)addr)[i]); \ + } \ + UDS_DBG_PRINT("\n"); + + +/* returns true if `a` is after `b` */ +static inline bool UDSTimeAfter(uint32_t a, uint32_t b) { + return ((int32_t)((int32_t)(b) - (int32_t)(a)) < 0); +} + +/** + * @brief Get time in milliseconds + * @return current time in milliseconds + */ +uint32_t UDSMillis(void); + + +bool UDSSecurityAccessLevelIsReserved(uint8_t securityLevel); diff --git a/test/BUILD b/test/BUILD index 4dbb91f..7fc4cea 100644 --- a/test/BUILD +++ b/test/BUILD @@ -4,14 +4,18 @@ cc_library( "env.c", "env.h", "test.h", - "//:iso14229_srcs", - "//tp:srcs", + "//:iso14229.h", + "//:iso14229.c", ], deps = [ "@cmocka" ], defines = [ - "UDS_TP=UDS_TP_CUSTOM", + "UDS_TP_ISOTP_C_SOCKETCAN", + "UDS_TP_ISOTP_SOCK", + "UDS_TP_MOCK", "UDS_CUSTOM_MILLIS", - "UDS_DBG_PRINT=printf", + "UDS_ENABLE_DBG_PRINT", + "UDS_ENABLE_ASSERT", + "UDS_LINES", ], copts = [ "-g", ], ) @@ -110,8 +114,8 @@ sh_test( cc_library( name = "ultra_strict", srcs = [ - "//:iso14229_srcs", - "//tp:srcs", + "//:iso14229.h", + "//:iso14229.c", ], copts = [ "-Werror", diff --git a/test/env.c b/test/env.c index b104df0..f70c8d3 100644 --- a/test/env.c +++ b/test/env.c @@ -5,9 +5,6 @@ #include #include #include -#include "tp/isotp_c_socketcan.h" -#include "tp/mock.h" -#include "tp/isotp_sock.h" static UDSServer_t *registeredServer = NULL; static UDSClient_t *registeredClient = NULL; diff --git a/test/test.h b/test/test.h index a0c1445..61313d1 100644 --- a/test/test.h +++ b/test/test.h @@ -7,7 +7,6 @@ #include #include "iso14229.h" #include "test/env.h" -#include "tp/mock.h" #include #define _TEST_INT_COND(a, b, cond) \ diff --git a/test/test_fuzz_server.c b/test/test_fuzz_server.c index eeaf10b..ed6c994 100644 --- a/test/test_fuzz_server.c +++ b/test/test_fuzz_server.c @@ -18,11 +18,11 @@ typedef struct { uint16_t client_sa; uint16_t client_ta; uint8_t client_func_req; - uint8_t msg[UDS_BUFSIZE]; + uint8_t msg[UDS_TP_MTU]; } StuffToFuzz_t; static StuffToFuzz_t fuzz; -static uint8_t client_recv_buf[UDS_BUFSIZE]; +static uint8_t client_recv_buf[UDS_TP_MTU]; static uint8_t fn(UDSServer_t *srv, UDSServerEvent_t ev, const void *arg) { printf("Whoah, got event %d\n", ev); diff --git a/test_iso14229.c b/test_iso14229.c deleted file mode 100644 index 6cfd587..0000000 --- a/test_iso14229.c +++ /dev/null @@ -1,875 +0,0 @@ -#define UDS_TP UDS_TP_CUSTOM -#include -#include -#include -#include -#include -#include -#include "iso14229.h" -#include "tp/mock.h" - -#define _ASSERT_INT_COND(a, b, cond) \ - { \ - int _a = a; \ - int _b = b; \ - if (!((_a)cond(_b))) { \ - printf("%s:%d (%d %s %d)\n", __FILE__, __LINE__, _a, #cond, _b); \ - fflush(stdout); \ - assert(a cond b); \ - } \ - } - -#define ASSERT_INT_LT(a, b) _ASSERT_INT_COND(a, b, <) -#define ASSERT_INT_LE(a, b) _ASSERT_INT_COND(a, b, <=) -#define ASSERT_INT_GE(a, b) _ASSERT_INT_COND(a, b, >=) -#define ASSERT_INT_EQUAL(a, b) _ASSERT_INT_COND(a, b, ==) -#define ASSERT_INT_NE(a, b) _ASSERT_INT_COND(a, b, !=) - -#define ASSERT_PTR_EQUAL(a, b) \ - { \ - const void *_a = a; \ - const void *_b = b; \ - if ((_a) != (_b)) { \ - printf("%s:%d (%p != %p)\n", __FILE__, __LINE__, _a, _b); \ - fflush(stdout); \ - assert(a == b); \ - } \ - } - -#define ASSERT_MEMORY_EQUAL(a, b, len) \ - { \ - const uint8_t *_a = (const uint8_t *)a; \ - const uint8_t *_b = (const uint8_t *)b; \ - if (memcmp(_a, _b, len)) { \ - printf("A:"); \ - for (unsigned int i = 0; i < len; i++) { \ - printf("%02x,", _a[i]); \ - } \ - printf(" (%s)\nB:", #a); \ - for (unsigned int i = 0; i < len; i++) { \ - printf("%02x,", _b[i]); \ - } \ - printf(" (%s)\n", #b); \ - fflush(stdout); \ - assert(0); \ - } \ - } - -#define ANSI_RESET "\033[0m" -#define ANSI_BOLD "\033[1m" -#define ANSI_BRIGHT_GREEN "\033[92m" -#define ANSI_BRIGHT_MAGENTA "\033[95m" - -#define CLIENT_TARGET_ADDR (0x7E0U) -#define CLIENT_TARGET_ADDR_FUNC (0x7DFU) -#define CLIENT_SOURCE_ADDR (0x7E8U) - -#define SERVER_TARGET_ADDR (0x7E8U) -#define SERVER_SOURCE_ADDR (0x7E0U) -#define SERVER_SOURCE_ADDR_FUNC (0x7DFU) - -/* - ctx.server_tp = (struct mocktransport){.hdl = {.recv = mock_transport_recv, \ - .send = mock_transport_send, \ - .poll = mock_transport_poll}, \ - .tag = "server"}; \ - ctx.client_tp = (struct mocktransport){.hdl = {.recv = mock_transport_recv, \ - .send = mock_transport_send, \ - .poll = mock_transport_poll}, \ - .tag = "client"}; \ - UDSClientInit(&ctx.client, &(UDSClientConfig_t){.tp = &ctx.client_tp.hdl}); \ - UDSServerInit(&ctx.server, &(UDSServerConfig_t){.tp = &ctx.server_tp.hdl}); - */ - -#define SERVER_ONLY 0 -#define CLIENT_ONLY 1 - -#define _TEST_SETUP_SILENT(test_type, param_str) \ - memset(&ctx, 0, sizeof(ctx)); \ - ctx.func_name = __PRETTY_FUNCTION__; \ - if (SERVER_ONLY == test_type) { \ - UDSServerInit(&ctx.server, &(UDSServerConfig_t){ \ - .tp = TPMockNew("server"), \ - .source_addr = SERVER_SOURCE_ADDR, \ - .target_addr = SERVER_TARGET_ADDR, \ - .source_addr_func = SERVER_SOURCE_ADDR_FUNC, \ - }); \ - ctx.mock_tp = TPMockNew("mock_client"); \ - } \ - if (CLIENT_ONLY == test_type) { \ - UDSClientInit(&ctx.client, &(UDSClientConfig_t){ \ - .tp = TPMockNew("client"), \ - .target_addr = CLIENT_TARGET_ADDR, \ - .source_addr = CLIENT_SOURCE_ADDR, \ - .target_addr_func = CLIENT_TARGET_ADDR_FUNC, \ - }); \ - ctx.mock_tp = TPMockNew("mock_server"); \ - } \ - char logfilename[256] = {0}; \ - snprintf(logfilename, sizeof(logfilename), "%s%s.log", ctx.func_name, param_str); \ - TPMockLogToFile(logfilename); - -#define TEST_SETUP(test_type) \ - _TEST_SETUP_SILENT(test_type, ""); \ - printf("%s\n", ctx.func_name); - -#define TEST_SETUP_PARAMETRIZED(test_type, params_list) \ - for (size_t i = 0; i < sizeof(params_list) / sizeof(params_list[0]); i++) { \ - char _param_str[128]; \ - snprintf(_param_str, sizeof(_param_str), "%s_p_%ld_%s", ctx.func_name, i, \ - (*(char **)(&(params_list[i])))); \ - _TEST_SETUP_SILENT(test_type, _param_str); \ - printf("%s\n", _param_str); - -#define TEST_TEARDOWN_PARAMETRIZED() \ - TPMockReset(); \ - printf(ANSI_BOLD "OK [p:%ld]\n" ANSI_RESET, i); \ - } - -#define TEST_TEARDOWN() \ - { \ - TPMockReset(); \ - printf(ANSI_BOLD "OK\n" ANSI_RESET); \ - } - -// TODO: parameterize and fuzz this -#define DEFAULT_ISOTP_BUFSIZE (2048U) - -struct MockTransport { - UDSTpHandle_t hdl; - uint8_t recv_buf[DEFAULT_ISOTP_BUFSIZE]; - uint8_t send_buf[DEFAULT_ISOTP_BUFSIZE]; - uint16_t recv_size; - uint16_t send_size; - UDSTpAddr_t recv_ta_type; - UDSTpAddr_t send_ta_type; - UDSTpStatus_t status; - const char *tag; -}; - -static void printhex(const uint8_t *addr, int len) { - for (int i = 0; i < len; i++) { - printf("%02x,", addr[i]); - } - printf("\n"); -} - -// static ssize_t mock_transport_recv(UDSTpHandle_t *hdl, UDSSDU_t *msg) { -// assert(hdl); -// struct MockTransport *tp = (struct MockTransport *)hdl; -// size_t size = tp->recv_size; - -// if (msg->A_DataBufSize < size) { -// return -ENOBUFS; -// } - -// if (size) { -// memmove((void *)msg->A_Data, tp->recv_buf, size); -// tp->recv_size = 0; -// memset(tp->recv_buf, 0, sizeof(tp->recv_buf)); -// printf(ANSI_BRIGHT_MAGENTA "<-%s_tp_recv-%04d- [%02ld] ", tp->tag, UDSMillis(), size); -// printhex(msg->A_Data, size); -// printf(ANSI_RESET); -// } -// return size; -// } - -// static ssize_t mock_transport_send(UDSTpHandle_t *hdl, UDSSDU_t *msg) { -// assert(hdl); -// struct MockTransport *tp = (struct MockTransport *)hdl; -// printf(ANSI_BRIGHT_GREEN "--%s_tp_send-%04d->[%02d] ", tp->tag, UDSMillis(), msg->A_Length); -// printhex(msg->A_Data, msg->A_Length); -// printf(ANSI_RESET); -// assert(msg->A_Length); // why send zero? -// memmove(tp->send_buf, msg->A_Data, msg->A_Length); -// tp->send_size = msg->A_Length; -// return msg->A_Length; -// } - -// static UDSTpStatus_t mock_transport_poll(UDSTpHandle_t *hdl) { -// assert(hdl); -// struct MockTransport *tp = (struct MockTransport *)hdl; -// return tp->status; -// } - -typedef struct { - UDSServer_t server; - UDSClient_t client; - UDSTpHandle_t *mock_tp; - uint8_t mock_recv_buf[DEFAULT_ISOTP_BUFSIZE]; - struct MockTransport client_tp; - uint32_t time_ms; - uint32_t deadline; - int call_count; - const char *func_name; -} Ctx_t; - -Ctx_t ctx; - -uint32_t UDSMillis() { return ctx.time_ms; } - -static void poll_ctx(Ctx_t *ctx) { - if (ctx->server.tp) { - UDSServerPoll(&ctx->server); - } - if (ctx->client.tp) { - UDSClientPoll(&ctx->client); - } - ctx->time_ms++; -} - -/* - memmove(&ctx.server_tp.recv_buf, d1, sizeof(d1)); \ - ctx.server_tp.recv_ta_type = reqType; \ - ctx.server_tp.recv_size = sizeof(d1); \ - poll_ctx(&ctx); -*/ - -#define SEND_TO_SERVER(d1, reqType) \ - { \ - UDSSDU_t msg = { \ - .A_Mtype = UDS_A_MTYPE_DIAG, \ - .A_Data = d1, \ - .A_Length = sizeof(d1), \ - .A_SA = CLIENT_SOURCE_ADDR, \ - .A_TA = \ - reqType == UDS_A_TA_TYPE_PHYSICAL ? SERVER_SOURCE_ADDR : SERVER_SOURCE_ADDR_FUNC, \ - .A_TA_Type = (int)reqType, \ - }; \ - ctx.mock_tp->send(ctx.mock_tp, &msg); \ - } - -#define ASSERT_CLIENT_SENT(d1, reqType) \ - { \ - UDSSDU_t msg = { \ - .A_DataBufSize = DEFAULT_ISOTP_BUFSIZE, \ - .A_Data = ctx.mock_recv_buf, \ - }; \ - int recv_len = ctx.mock_tp->recv(ctx.mock_tp, &msg); \ - ASSERT_INT_EQUAL(recv_len, sizeof(d1)); \ - ASSERT_MEMORY_EQUAL(ctx.mock_recv_buf, d1, sizeof(d1)); \ - if (reqType == UDS_A_TA_TYPE_PHYSICAL) { \ - ASSERT_INT_EQUAL(msg.A_TA, SERVER_SOURCE_ADDR); \ - } else if (reqType == UDS_A_TA_TYPE_FUNCTIONAL) { \ - ASSERT_INT_EQUAL(msg.A_TA, SERVER_SOURCE_ADDR_FUNC); \ - } else { \ - assert(0); \ - } \ - } - -// send data to the client -static void send_to_client(const uint8_t *d1, size_t len, UDSTpAddr_t reqType) { - assert(len <= sizeof(ctx.client_tp.recv_buf)); - ctx.mock_tp->send(ctx.mock_tp, &(UDSSDU_t){ - .A_Mtype = UDS_A_MTYPE_DIAG, - .A_Data = d1, - .A_Length = len, - .A_SA = SERVER_SOURCE_ADDR, - .A_TA = SERVER_TARGET_ADDR, - .A_TA_Type = (int)reqType, - }); - // memmove(&ctx.client_tp.recv_buf, d1, len); - // ctx.client_tp.recv_ta_type = reqType; - // ctx.client_tp.recv_size = len; - poll_ctx(&ctx); -} - -#define SEND_TO_CLIENT(d1, reqType) send_to_client(d1, sizeof(d1), reqType); -/* - while (0 == ctx.server_tp.send_size) { \ - poll_ctx(&ctx); \ - ASSERT_INT_LE(ctx.time_ms, deadline); \ - } \ - */ - -// expect a server response within a timeout -#define EXPECT_RESPONSE_WITHIN_MILLIS(d1, reqType, timeout_ms) \ - { \ - uint32_t deadline = ctx.time_ms + timeout_ms + 1; \ - UDSSDU_t msg = { \ - .A_DataBufSize = DEFAULT_ISOTP_BUFSIZE, \ - .A_Data = ctx.mock_recv_buf, \ - }; \ - while (0 == ctx.mock_tp->recv(ctx.mock_tp, &msg)) { \ - poll_ctx(&ctx); \ - printf("%d, %d, %d\n", UDSMillis(), ctx.time_ms, deadline); \ - ASSERT_INT_LE(ctx.time_ms, deadline); \ - } \ - printhex(msg.A_Data, msg.A_Length); \ - ASSERT_INT_EQUAL(msg.A_Length, sizeof(d1)); \ - ASSERT_MEMORY_EQUAL(msg.A_Data, d1, sizeof(d1)); \ - ASSERT_INT_EQUAL((int)msg.A_TA_Type, reqType); \ - } - -// expect no server response within a timeout -#define EXPECT_NO_RESPONSE_FOR_MILLIS(timeout_ms) \ - { \ - uint32_t deadline = ctx.time_ms + timeout_ms; \ - while (ctx.time_ms <= deadline) { \ - poll_ctx(&ctx); \ - UDSSDU_t msg = { \ - .A_DataBufSize = DEFAULT_ISOTP_BUFSIZE, \ - .A_Data = ctx.mock_recv_buf, \ - }; \ - int resp_len = ctx.mock_tp->recv(ctx.mock_tp, &msg); \ - ASSERT_INT_EQUAL(resp_len, 0); \ - } \ - } - -void testServer0x10DiagSessCtrlIsDisabledByDefault() { - TEST_SETUP(SERVER_ONLY); - const uint8_t REQ[] = {0x10, 0x02}; - SEND_TO_SERVER(REQ, UDS_A_TA_TYPE_PHYSICAL); - const uint8_t RESP[] = {0x7f, 0x10, 0x11}; - EXPECT_RESPONSE_WITHIN_MILLIS(RESP, UDS_A_TA_TYPE_PHYSICAL, 50); - TEST_TEARDOWN(); -} - -void testServer0x10DiagSessCtrlFunctionalRequest() { - TEST_SETUP(SERVER_ONLY); - // sending a diagnostic session control request functional broadcast - const uint8_t REQ[] = {0x10, 0x03}; - SEND_TO_SERVER(REQ, UDS_A_TA_TYPE_FUNCTIONAL); - // should receive a physical response - const uint8_t RESP[] = {0x7f, 0x10, 0x11}; - EXPECT_RESPONSE_WITHIN_MILLIS(RESP, UDS_A_TA_TYPE_PHYSICAL, 50); - TEST_TEARDOWN(); -} - -uint8_t fn1_callCount = 0; -uint8_t fn1(UDSServer_t *srv, UDSServerEvent_t ev, const void *arg) { - switch (ev) { - case UDS_SRV_EVT_EcuReset: - fn1_callCount += 1; - return kPositiveResponse; - default: - ASSERT_INT_EQUAL(UDS_SRV_EVT_DoScheduledReset, ev); - return kPositiveResponse; - } -} - -// Special-case of ECU reset service -// ISO-14229-1 2013 9.3.1: -// on the behaviour of the ECU from the time following the positive response message to the ECU -// reset request: It is recommended that during this time the ECU does not accept any request -// messages and send any response messages. -void testServer0x11DoesNotSendOrReceiveMessagesAfterECUReset() { - TEST_SETUP(SERVER_ONLY); - ctx.server.fn = fn1; - - const uint8_t REQ[] = {0x11, 0x01}; - const uint8_t RESP[] = {0x51, 0x01}; - - // Sending the first ECU reset should result in a response - SEND_TO_SERVER(REQ, UDS_A_TA_TYPE_PHYSICAL); - EXPECT_RESPONSE_WITHIN_MILLIS(RESP, UDS_A_TA_TYPE_PHYSICAL, 50); - // Sending subsequent ECU reset requests should not receive any response - SEND_TO_SERVER(REQ, UDS_A_TA_TYPE_PHYSICAL); - EXPECT_NO_RESPONSE_FOR_MILLIS(5000); - - // The ECU reset handler should have been called once. - ASSERT_INT_EQUAL(fn1_callCount, 1); - TEST_TEARDOWN(); -} - -uint8_t fn2(UDSServer_t *srv, UDSServerEvent_t ev, const void *arg) { - ASSERT_INT_EQUAL(UDS_SRV_EVT_ReadDataByIdent, ev); - const uint8_t vin[] = {0x57, 0x30, 0x4C, 0x30, 0x30, 0x30, 0x30, 0x34, 0x33, - 0x4D, 0x42, 0x35, 0x34, 0x31, 0x33, 0x32, 0x36}; - const uint8_t data_0x010A[] = {0xA6, 0x66, 0x07, 0x50, 0x20, 0x1A, - 0x00, 0x63, 0x4A, 0x82, 0x7E}; - const uint8_t data_0x0110[] = {0x8C}; - - UDSRDBIArgs_t *r = (UDSRDBIArgs_t *)arg; - switch (r->dataId) { - case 0xF190: - return r->copy(srv, vin, sizeof(vin)); - case 0x010A: - return r->copy(srv, data_0x010A, sizeof(data_0x010A)); - case 0x0110: - return r->copy(srv, data_0x0110, sizeof(data_0x0110)); - default: - return kRequestOutOfRange; - } - return kPositiveResponse; -} - -void testServer0x22RDBI1() { - TEST_SETUP(SERVER_ONLY); - ctx.server.fn = fn2; - { - uint8_t REQ[] = {0x22, 0xF1, 0x90}; - SEND_TO_SERVER(REQ, UDS_A_TA_TYPE_PHYSICAL); - uint8_t RESP[] = {0x62, 0xF1, 0x90, 0x57, 0x30, 0x4C, 0x30, 0x30, 0x30, 0x30, - 0x34, 0x33, 0x4D, 0x42, 0x35, 0x34, 0x31, 0x33, 0x32, 0x36}; - EXPECT_RESPONSE_WITHIN_MILLIS(RESP, UDS_A_TA_TYPE_PHYSICAL, 50); - } - { - uint8_t REQ[] = {0x22, 0xF1, 0x91}; - SEND_TO_SERVER(REQ, UDS_A_TA_TYPE_PHYSICAL); - uint8_t RESP[] = {0x7F, 0x22, 0x31}; - EXPECT_RESPONSE_WITHIN_MILLIS(RESP, UDS_A_TA_TYPE_PHYSICAL, 50); - } - TEST_TEARDOWN(); -} - -uint8_t fn10(UDSServer_t *srv, UDSServerEvent_t ev, const void *arg) { - ASSERT_INT_EQUAL(ev, UDS_SRV_EVT_ReadMemByAddr); - UDSReadMemByAddrArgs_t *r = (UDSReadMemByAddrArgs_t *)arg; - // 1 2 3 4 5 6 7 8 - ASSERT_PTR_EQUAL(r->memAddr, (void *)0x000055555555f0c8); - ASSERT_INT_EQUAL(r->memSize, 4); - uint8_t FakeData[4] = {0x01, 0x02, 0x03, 0x04}; - return r->copy(srv, FakeData, r->memSize); -} - -void testServer0x23ReadMemoryByAddress() { - TEST_SETUP(SERVER_ONLY); - ctx.server.fn = fn10; - uint8_t REQ[] = {0x23, 0x18, 0x00, 0x00, 0x55, 0x55, 0x55, 0x55, 0xf0, 0xc8, 0x04}; - SEND_TO_SERVER(REQ, UDS_A_TA_TYPE_PHYSICAL); - uint8_t RESP[] = {0x63, 0x01, 0x02, 0x03, 0x04}; - EXPECT_RESPONSE_WITHIN_MILLIS(RESP, UDS_A_TA_TYPE_PHYSICAL, 50); - TEST_TEARDOWN(); -} - -uint8_t fn4(UDSServer_t *srv, UDSServerEvent_t ev, const void *arg) { - switch (ev) { - case UDS_SRV_EVT_SecAccessRequestSeed: { - UDSSecAccessRequestSeedArgs_t *r = (UDSSecAccessRequestSeedArgs_t *)arg; - const uint8_t seed[] = {0x36, 0x57}; - ASSERT_INT_NE(r->level, srv->securityLevel); - return r->copySeed(srv, seed, sizeof(seed)); - break; - } - case UDS_SRV_EVT_SecAccessValidateKey: { - UDSSecAccessValidateKeyArgs_t *r = (UDSSecAccessValidateKeyArgs_t *)arg; - const uint8_t expected_key[] = {0xC9, 0xA9}; - ASSERT_INT_EQUAL(r->len, sizeof(expected_key)); - ASSERT_MEMORY_EQUAL(r->key, expected_key, sizeof(expected_key)); - break; - } - default: - assert(0); - } - return kPositiveResponse; -} - -// UDS-1 2013 9.4.5.2 -// UDS-1 2013 9.4.5.3 -void testServer0x27SecurityAccess() { - TEST_SETUP(SERVER_ONLY); - ctx.server.fn = fn4; - - // the server security level after initialization should be 0 - ASSERT_INT_EQUAL(ctx.server.securityLevel, 0); - - // sending a seed request should get this response - const uint8_t SEED_REQUEST[] = {0x27, 0x01}; - const uint8_t SEED_RESPONSE[] = {0x67, 0x01, 0x36, 0x57}; - SEND_TO_SERVER(SEED_REQUEST, UDS_A_TA_TYPE_PHYSICAL); - EXPECT_RESPONSE_WITHIN_MILLIS(SEED_RESPONSE, UDS_A_TA_TYPE_PHYSICAL, 50); - - // subsequently sending an unlock request should get this response - const uint8_t UNLOCK_REQUEST[] = {0x27, 0x02, 0xC9, 0xA9}; - const uint8_t UNLOCK_RESPONSE[] = {0x67, 0x02}; - SEND_TO_SERVER(UNLOCK_REQUEST, UDS_A_TA_TYPE_PHYSICAL); - EXPECT_RESPONSE_WITHIN_MILLIS(UNLOCK_RESPONSE, UDS_A_TA_TYPE_PHYSICAL, 50); - - // sending the same seed request should now result in the "already unlocked" response - const uint8_t ALREADY_UNLOCKED_RESPONSE[] = {0x67, 0x01, 0x00, 0x00}; - SEND_TO_SERVER(SEED_REQUEST, UDS_A_TA_TYPE_PHYSICAL); - EXPECT_RESPONSE_WITHIN_MILLIS(ALREADY_UNLOCKED_RESPONSE, UDS_A_TA_TYPE_PHYSICAL, 50); - - // Additionally, the security level should now be 1 - ASSERT_INT_EQUAL(ctx.server.securityLevel, 1); - TEST_TEARDOWN(); -} - -static uint8_t ReturnRCRRP(UDSServer_t *srv, UDSServerEvent_t ev, const void *arg) { - return kRequestCorrectlyReceived_ResponsePending; -} -static uint8_t ReturnPositiveResponse(UDSServer_t *srv, UDSServerEvent_t ev, const void *arg) { - return kPositiveResponse; -} - -// ISO-14229-1 2013 Table A.1 Byte Value 0x78: requestCorrectlyReceived-ResponsePending -// "This NRC is in general supported by each diagnostic service". -void testServer0x31RCRRP() { - TEST_SETUP(SERVER_ONLY); - // When a server handler func initially returns RRCRP - ctx.server.fn = ReturnRCRRP; - - // sending a request to the server should return RCRRP - const uint8_t REQUEST[] = {0x31, 0x01, 0x12, 0x34}; - const uint8_t RCRRP[] = {0x7F, 0x31, 0x78}; - SEND_TO_SERVER(REQUEST, UDS_A_TA_TYPE_PHYSICAL); - EXPECT_RESPONSE_WITHIN_MILLIS(RCRRP, UDS_A_TA_TYPE_PHYSICAL, 50) - - // The server should again respond within p2_star ms, and keep responding - EXPECT_RESPONSE_WITHIN_MILLIS(RCRRP, UDS_A_TA_TYPE_PHYSICAL, 50) - EXPECT_RESPONSE_WITHIN_MILLIS(RCRRP, UDS_A_TA_TYPE_PHYSICAL, 50) - EXPECT_RESPONSE_WITHIN_MILLIS(RCRRP, UDS_A_TA_TYPE_PHYSICAL, 50) - EXPECT_RESPONSE_WITHIN_MILLIS(RCRRP, UDS_A_TA_TYPE_PHYSICAL, 50) - EXPECT_RESPONSE_WITHIN_MILLIS(RCRRP, UDS_A_TA_TYPE_PHYSICAL, 50) - EXPECT_RESPONSE_WITHIN_MILLIS(RCRRP, UDS_A_TA_TYPE_PHYSICAL, 50) - - // When the server handler func now returns a positive response - ctx.server.fn = ReturnPositiveResponse; - - // the server's next response should be a positive one - const uint8_t POSITIVE_RESPONSE[] = {0x71, 0x01, 0x12, 0x34}; - EXPECT_RESPONSE_WITHIN_MILLIS(POSITIVE_RESPONSE, UDS_A_TA_TYPE_PHYSICAL, 50) - - TEST_TEARDOWN(); -} - -void testServer0x34NotEnabled() { - TEST_SETUP(SERVER_ONLY); - // when no handler function is installed, sending this request to the server - const uint8_t IN[] = {0x34, 0x11, 0x33, 0x60, 0x20, 0x00, 0x00, 0xFF, 0xFF}; - SEND_TO_SERVER(IN, UDS_A_TA_TYPE_PHYSICAL); - - // should return a kServiceNotSupported response - const uint8_t OUT[] = {0x7F, 0x34, 0x11}; - EXPECT_RESPONSE_WITHIN_MILLIS(OUT, UDS_A_TA_TYPE_PHYSICAL, 50); - TEST_TEARDOWN(); -} - -uint8_t fn7(UDSServer_t *srv, UDSServerEvent_t ev, const void *arg) { - ASSERT_INT_EQUAL(ev, UDS_SRV_EVT_RequestDownload); - UDSRequestDownloadArgs_t *r = (UDSRequestDownloadArgs_t *)arg; - ASSERT_INT_EQUAL(0x11, r->dataFormatIdentifier); - ASSERT_PTR_EQUAL((void *)0x602000, r->addr); - ASSERT_INT_EQUAL(0x00FFFF, r->size); - ASSERT_INT_EQUAL(r->maxNumberOfBlockLength, UDS_SERVER_DEFAULT_XFER_DATA_MAX_BLOCKLENGTH); - r->maxNumberOfBlockLength = 0x0081; - return kPositiveResponse; -} - -void testServer0x34() { - TEST_SETUP(SERVER_ONLY); - // when a handler is installed that implements UDS-1:2013 Table 415 - ctx.server.fn = fn7; - - // sending this request to the server - uint8_t REQ[] = {0x34, 0x11, 0x33, 0x60, 0x20, 0x00, 0x00, 0xFF, 0xFF}; - SEND_TO_SERVER(REQ, UDS_A_TA_TYPE_PHYSICAL); - - // should receive a positive response matching UDS-1:2013 Table 415 - uint8_t RESP[] = {0x74, 0x20, 0x00, 0x81}; - EXPECT_RESPONSE_WITHIN_MILLIS(RESP, UDS_A_TA_TYPE_PHYSICAL, 50) - TEST_TEARDOWN(); -} - -/* UDS-1 2013 Table 72 */ -void testServer0x3ESuppressPositiveResponse() { - TEST_SETUP(SERVER_ONLY); - ctx.server.fn = ReturnPositiveResponse; - // when the suppressPositiveResponse bit is set - const uint8_t REQ[] = {0x3E, 0x80}; - // there should be no response - SEND_TO_SERVER(REQ, UDS_A_TA_TYPE_PHYSICAL); - EXPECT_NO_RESPONSE_FOR_MILLIS(5000); - TEST_TEARDOWN(); -} - -void testServer0x83DiagnosticSessionControl() { - TEST_SETUP(SERVER_ONLY); - ctx.server.fn = ReturnPositiveResponse; - // the server sessionType after initialization should be kDefaultSession. - ASSERT_INT_EQUAL(ctx.server.sessionType, kDefaultSession); - - // When the suppressPositiveResponse bit is set, there should be no response. - const uint8_t REQ[] = {0x10, 0x83}; - SEND_TO_SERVER(REQ, UDS_A_TA_TYPE_PHYSICAL); - EXPECT_NO_RESPONSE_FOR_MILLIS(5000); - // and the server sessionType should have changed - ASSERT_INT_EQUAL(ctx.server.sessionType, kExtendedDiagnostic); - TEST_TEARDOWN(); -} - -uint8_t fn9(UDSServer_t *srv, UDSServerEvent_t ev, const void *arg) { - ASSERT_INT_EQUAL(UDS_SRV_EVT_SessionTimeout, ev); - ctx.call_count++; - return kPositiveResponse; -} - -void testServerSessionTimeout() { - struct { - const char *tag; - uint8_t (*fn)(UDSServer_t *srv, UDSServerEvent_t ev, const void *arg); - uint8_t sessType; - int expectedCallCount; - } p[] = { - {.tag = "no timeout", .fn = fn9, .sessType = kDefaultSession, .expectedCallCount = 0}, - {.tag = "timeout", .fn = fn9, .sessType = kProgrammingSession, .expectedCallCount = 1}, - {.tag = "no handler", .fn = NULL, .sessType = kProgrammingSession, .expectedCallCount = 0}, - }; - TEST_SETUP_PARAMETRIZED(SERVER_ONLY, p); - ctx.server.fn = p[i].fn; - ctx.server.sessionType = p[i].sessType; - while (ctx.time_ms < 5000) - poll_ctx(&ctx); - ASSERT_INT_GE(ctx.call_count, p[i].expectedCallCount); - TEST_TEARDOWN_PARAMETRIZED(); -} - -#define POLL_UNTIL_TIME_MS(abs_time_ms) \ - while (ctx.time_ms < (abs_time_ms)) \ - poll_ctx(&ctx) - -#define POLL_FOR_MS(duration_ms) \ - { \ - uint32_t start_time_ms = ctx.time_ms; \ - while (ctx.time_ms < (start_time_ms + duration_ms)) \ - poll_ctx(&ctx); \ - } - -void testClientP2TimeoutExceeded() { - TEST_SETUP(CLIENT_ONLY); - // when sending a request that receives no response - UDSSendECUReset(&ctx.client, kHardReset); - - // before p2 ms has elapsed, the client should have no error - POLL_UNTIL_TIME_MS(UDS_CLIENT_DEFAULT_P2_MS - 10); - ASSERT_INT_EQUAL(UDS_OK, ctx.client.err); - - // after p2 ms has elapsed, the client should have a timeout error - POLL_UNTIL_TIME_MS(UDS_CLIENT_DEFAULT_P2_MS + 10); - ASSERT_INT_EQUAL(UDS_ERR_TIMEOUT, ctx.client.err); - TEST_TEARDOWN(); -} - -void testClientP2TimeoutNotExceeded() { - TEST_SETUP(CLIENT_ONLY); - // a client that sends an request - UDSSendECUReset(&ctx.client, kHardReset); - - // which receives a positive response - const uint8_t POSITIVE_RESPONSE[] = {0x51, 0x01}; - SEND_TO_CLIENT(POSITIVE_RESPONSE, UDS_A_TA_TYPE_PHYSICAL); - - POLL_UNTIL_TIME_MS(UDS_CLIENT_DEFAULT_P2_MS + 10); - // should return to the idle state - ASSERT_INT_EQUAL(kRequestStateIdle, ctx.client.state); - // and should have no error. - ASSERT_INT_EQUAL(UDS_OK, ctx.client.err); - TEST_TEARDOWN(); -} - -void testClientSuppressPositiveResponse() { - TEST_SETUP(CLIENT_ONLY); - // Setting the suppressPositiveResponse flag before sending a request - ctx.client.options |= UDS_SUPPRESS_POS_RESP; - UDSSendECUReset(&ctx.client, kHardReset); - - // and not receiving a response after approximately p2 ms - POLL_UNTIL_TIME_MS(UDS_CLIENT_DEFAULT_P2_MS + 10); - - // should not result in an error. - ASSERT_INT_EQUAL(UDS_OK, ctx.client.err); - ASSERT_INT_EQUAL(kRequestStateIdle, ctx.client.state); - TEST_TEARDOWN(); -} - -void testClientBusy() { - TEST_SETUP(CLIENT_ONLY); - // Sending a request should not return an error - ASSERT_INT_EQUAL(UDS_OK, UDSSendECUReset(&ctx.client, kHardReset)); - - // unless there is an existing unresolved request - ASSERT_INT_EQUAL(UDS_ERR_BUSY, UDSSendECUReset(&ctx.client, kHardReset)); - TEST_TEARDOWN(); -} - -void testClient0x11ECUReset() { - TEST_SETUP(CLIENT_ONLY); - const uint8_t GOOD[] = {0x51, 0x01}; - const uint8_t BAD_SID[] = {0x50, 0x01}; - const uint8_t TOO_SHORT[] = {0x51}; - const uint8_t BAD_SUBFUNC[] = {0x51, 0x02}; - const uint8_t NEG[] = {0x7F, 0x11, 0x10}; -#define CASE(d1, opt, err) \ - { \ - .tag = "resp: " #d1 ", expected_err: " #err, .resp = d1, .resp_len = sizeof(d1), \ - .options = opt, .expected_err = err \ - } - struct { - const char *tag; - const uint8_t *resp; - size_t resp_len; - uint8_t options; - UDSErr_t expected_err; - } p[] = { - CASE(GOOD, 0, UDS_OK), - CASE(BAD_SID, 0, UDS_ERR_SID_MISMATCH), - CASE(TOO_SHORT, 0, UDS_ERR_RESP_TOO_SHORT), - CASE(BAD_SUBFUNC, 0, UDS_ERR_SUBFUNCTION_MISMATCH), - CASE(NEG, 0, UDS_OK), - CASE(NEG, UDS_NEG_RESP_IS_ERR, UDS_ERR_NEG_RESP), - }; -#undef CASE - TEST_SETUP_PARAMETRIZED(CLIENT_ONLY, p); - // sending a request with these options - ctx.client.options = p[i].options; - UDSSendECUReset(&ctx.client, kHardReset); - // that receives this response - send_to_client(p[i].resp, p[i].resp_len, UDS_A_TA_TYPE_PHYSICAL); - // should return to the idle state - POLL_UNTIL_TIME_MS(UDS_CLIENT_DEFAULT_P2_MS + 10); - ASSERT_INT_EQUAL(kRequestStateIdle, ctx.client.state); - // with the expected error. - ASSERT_INT_EQUAL(p[i].expected_err, ctx.client.err); - TEST_TEARDOWN_PARAMETRIZED(); -} - -void testClient0x22RDBITxBufferTooSmall() { - TEST_SETUP(CLIENT_ONLY); - - // attempting to send a request payload of 6 bytes - uint16_t didList[] = {0x0001, 0x0002, 0x0003}; - - // which is larger than the underlying buffer - ctx.client.send_buf_size = 4; - - // should return an error - ASSERT_INT_EQUAL(UDS_ERR_INVALID_ARG, - UDSSendRDBI(&ctx.client, didList, sizeof(didList) / sizeof(didList[0]))) - - // and no data should be sent - ASSERT_INT_EQUAL(ctx.client.send_size, 0); - TEST_TEARDOWN(); -} - -void testClient0x22RDBIUnpackResponse() { - TEST_SETUP(CLIENT_ONLY); - uint8_t RESPONSE[] = {0x72, 0x12, 0x34, 0x00, 0x00, 0xAA, 0x00, 0x56, 0x78, 0xAA, 0xBB}; - UDSClient_t client; - memmove(client.recv_buf, RESPONSE, sizeof(RESPONSE)); - client.recv_size = sizeof(RESPONSE); - uint8_t buf[4]; - uint16_t offset = 0; - int err = 0; - err = UDSUnpackRDBIResponse(&client, 0x1234, buf, 4, &offset); - ASSERT_INT_EQUAL(err, UDS_OK); - uint32_t d0 = (buf[0] << 24) + (buf[1] << 16) + (buf[2] << 8) + buf[3]; - ASSERT_INT_EQUAL(d0, 0x0000AA00); - err = UDSUnpackRDBIResponse(&client, 0x1234, buf, 2, &offset); - ASSERT_INT_EQUAL(err, UDS_ERR_DID_MISMATCH); - err = UDSUnpackRDBIResponse(&client, 0x5678, buf, 20, &offset); - ASSERT_INT_EQUAL(err, UDS_ERR_RESP_TOO_SHORT); - err = UDSUnpackRDBIResponse(&client, 0x5678, buf, 2, &offset); - ASSERT_INT_EQUAL(err, UDS_OK); - uint16_t d1 = (buf[0] << 8) + buf[1]; - ASSERT_INT_EQUAL(d1, 0xAABB); - err = UDSUnpackRDBIResponse(&client, 0x5678, buf, 1, &offset); - ASSERT_INT_EQUAL(err, UDS_ERR_RESP_TOO_SHORT); - ASSERT_INT_EQUAL(offset, sizeof(RESPONSE)); - TEST_TEARDOWN(); -} - -void testClient0x31RCRRP() { - TEST_SETUP(CLIENT_ONLY); - - { // Case 1: RCRRP Timeout - // When a request is sent - UDSSendRoutineCtrl(&ctx.client, kStartRoutine, 0x1234, NULL, 0); - - // that receives an RCRRP response - const uint8_t RCRRP[] = {0x7F, 0x31, 0x78}; // RequestCorrectly-ReceievedResponsePending - SEND_TO_CLIENT(RCRRP, UDS_A_TA_TYPE_PHYSICAL); - - // that remains unresolved at a time between p2 ms and p2 star ms - POLL_FOR_MS(UDS_CLIENT_DEFAULT_P2_MS + 10); - // the client should still be pending. - ASSERT_INT_EQUAL(kRequestStateAwaitResponse, ctx.client.state) - - // after p2_star has elapsed, the client should timeout - POLL_FOR_MS(UDS_CLIENT_DEFAULT_P2_STAR_MS + 10); - ASSERT_INT_EQUAL(kRequestStateIdle, ctx.client.state) - ASSERT_INT_EQUAL(ctx.client.err, UDS_ERR_TIMEOUT); - } - - { // Case 2: Positive Response Received - // When a request is sent - UDSSendRoutineCtrl(&ctx.client, kStartRoutine, 0x1234, NULL, 0); - - // that receives an RCRRP response - const uint8_t RCRRP[] = {0x7F, 0x31, 0x78}; // RequestCorrectly-ReceievedResponsePending - SEND_TO_CLIENT(RCRRP, UDS_A_TA_TYPE_PHYSICAL); - - // that remains unresolved at a time between p2 ms and p2 star ms - POLL_FOR_MS(UDS_CLIENT_DEFAULT_P2_MS + 10); - // the client should still be pending. - ASSERT_INT_EQUAL(ctx.client.err, UDS_OK); - ASSERT_INT_EQUAL(kRequestStateAwaitResponse, ctx.client.state) - - // When the client receives a positive response from the server - const uint8_t POSITIVE_RESPONSE[] = {0x71, 0x01, 0x12, 0x34}; - SEND_TO_CLIENT(POSITIVE_RESPONSE, UDS_A_TA_TYPE_PHYSICAL); - - POLL_FOR_MS(5); - - // the client should return to the idle state with no error - ASSERT_INT_EQUAL(kRequestStateIdle, ctx.client.state) - ASSERT_INT_EQUAL(ctx.client.err, UDS_OK); - } - - TEST_TEARDOWN(); -} - -void testClient0x34RequestDownload() { - TEST_SETUP(CLIENT_ONLY); - // When RequestDownload is called with these arguments - ASSERT_INT_EQUAL(UDS_OK, UDSSendRequestDownload(&ctx.client, 0x11, 0x33, 0x602000, 0x00FFFF)); - - // the bytes sent should match UDS-1 2013 Table 415 - const uint8_t CORRECT_REQUEST[] = {0x34, 0x11, 0x33, 0x60, 0x20, 0x00, 0x00, 0xFF, 0xFF}; - ASSERT_CLIENT_SENT(CORRECT_REQUEST, UDS_A_TA_TYPE_PHYSICAL); - TEST_TEARDOWN(); -} - -void testClient0x34UDSUnpackRequestDownloadResponse() { - TEST_SETUP(CLIENT_ONLY); - struct RequestDownloadResponse resp; - - // When the following raw bytes are received - uint8_t RESPONSE[] = {0x74, 0x20, 0x00, 0x81}; - UDSClient_t client; - memmove(client.recv_buf, RESPONSE, sizeof(RESPONSE)); - client.recv_size = sizeof(RESPONSE); - - UDSErr_t err = UDSUnpackRequestDownloadResponse(&client, &resp); - - // they should unpack without error - ASSERT_INT_EQUAL(err, UDS_OK); - ASSERT_INT_EQUAL(resp.maxNumberOfBlockLength, 0x81); - TEST_TEARDOWN(); -} - -int main() { - testServer0x10DiagSessCtrlIsDisabledByDefault(); - testServer0x10DiagSessCtrlFunctionalRequest(); - testServer0x11DoesNotSendOrReceiveMessagesAfterECUReset(); - testServer0x22RDBI1(); - testServer0x23ReadMemoryByAddress(); - testServer0x27SecurityAccess(); - testServer0x31RCRRP(); - testServer0x34NotEnabled(); - testServer0x34(); - testServer0x3ESuppressPositiveResponse(); - testServer0x83DiagnosticSessionControl(); - testServerSessionTimeout(); - - testClientP2TimeoutExceeded(); - testClientP2TimeoutNotExceeded(); - testClientSuppressPositiveResponse(); - testClientBusy(); - testClient0x11ECUReset(); - testClient0x22RDBITxBufferTooSmall(); - testClient0x22RDBIUnpackResponse(); - testClient0x31RCRRP(); - testClient0x34RequestDownload(); - testClient0x34UDSUnpackRequestDownloadResponse(); -} diff --git a/tp/BUILD b/tp/BUILD deleted file mode 100644 index bf3e8bc..0000000 --- a/tp/BUILD +++ /dev/null @@ -1,63 +0,0 @@ -package(default_visibility = ["//visibility:public"]) - -cc_library( - name="mock", - srcs=[ - "mock.c", - "mock.h", - "//:iso14229.h" - ], -) - -cc_library( - name="isotp_sock", - srcs=[ - "isotp_sock.c", - "isotp_sock.h", - "//:iso14229.h" - ], -) - - -filegroup( - name="isotp_c_srcs", - srcs=[ - "isotp-c/isotp.c", - "isotp-c/isotp.h", - "isotp-c/isotp_config.h", - "isotp-c/isotp_defines.h", - ], -) - -cc_library( - name="isotp_c", - srcs=[":isotp_c_srcs"], - copts=["-Wno-unused-parameter"], -) - -cc_library( - name="isotp_c_socketcan", - srcs=[ - "isotp_c_socketcan.c", - "isotp_c_socketcan.h", - "//:iso14229.h", - ], - deps = [ - ":isotp_c", - ], -) - -filegroup( - name="srcs", - srcs = [ - "mock.c", - "mock.h", - "isotp_sock.c", - "isotp_sock.h", - "isotp_c_socketcan.c", - "isotp_c_socketcan.h", - "isotp_c.c", - "isotp_c.h", - ":isotp_c_srcs", - ] -)