├── .gitignore ├── .travis.yml ├── LICENSE ├── Makefile ├── README.md ├── TestRunner.cpp ├── doc └── developer documentation.pdf ├── examples └── exampleDoIPServer.cpp ├── libdoipclient ├── include │ └── DoIPClient_h.h ├── src │ └── DoIPClient.cpp └── test │ └── DoIPClient_Test.cpp ├── libdoipcommon ├── include │ ├── AliveCheckTimer.h │ ├── DiagnosticMessageHandler.h │ └── DoIPGenericHeaderHandler.h ├── src │ ├── AliveCheckTimer.cpp │ ├── DiagnosticMessageHandler.cpp │ └── DoIPGenericHeaderHandler.cpp └── test │ └── DoIPGenericHeaderHandler_Test.cpp └── libdoipserver ├── include ├── DoIPConnection.h ├── DoIPServer.h ├── RoutingActivationHandler.h └── VehicleIdentificationHandler.h ├── src ├── DoIPConnection.cpp ├── DoIPServer.cpp ├── RoutingActivationHandler.cpp └── VehicleIdentificationHandler.cpp └── test ├── RoutingActivationHandler_Test.cpp └── VehicleIdentificationHandler_Test.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | runTest 34 | 35 | # Folder 36 | build/ 37 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | 3 | compiler: 4 | - g++ 5 | 6 | before_install: 7 | 8 | # save project dir in environment variable 9 | - PROJECT_DIR=$(pwd) 10 | 11 | # add toolchain repository (for g++ update) 12 | - if [ "$CXX" == "g++" ]; then sudo add-apt-repository -y ppa:ubuntu-toolchain-r/test; fi 13 | - sudo apt-get update -qq 14 | 15 | install: 16 | #install cmake 17 | - sudo apt-get install cmake 18 | 19 | #install gtest 20 | - sudo apt-get install libgtest-dev 21 | - cd /usr/src/gtest 22 | - sudo cmake CMakeLists.txt 23 | - sudo make 24 | - sudo cp libgtest.a /usr/lib 25 | - sudo cp libgtest_main.a /usr/lib 26 | 27 | # install newer compiler and set symlink to g++ version 8 28 | - if [ "$CXX" = "g++" ]; then sudo apt-get install -qq g++-8; fi 29 | - if [ "$CXX" = "g++" ]; then export CXX="g++-8"; fi 30 | - sudo ln -sf /usr/bin/g++-8 /usr/bin/g++ 31 | 32 | # check g++ version 33 | - $CXX --version 34 | - g++ --version 35 | 36 | before_script: 37 | #change to root 38 | - cd $PROJECT_DIR 39 | 40 | script: 41 | - make 42 | - ./runTest --gtest_output="xml:./testOutput.xml" 43 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "{}" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright {yyyy} {name of copyright owner} 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | CXX = g++ 2 | 3 | CPPFLAGS = -g -Wall -Wextra -std=c++11 -fsanitize=address 4 | LDFLAGS = -shared 5 | TESTFLAGS = -g -L/usr/lib -lgtest -lgtest_main -lpthread 6 | 7 | SRCPATH = src 8 | INCPATH = include 9 | BUILDPATH = build 10 | TESTPATH = test 11 | EXAMPLEPATH = examples 12 | 13 | COMMONTARGET = libdoipcommon 14 | SERVERTARGET = libdoipserver 15 | CLIENTTARGET = libdoipclient 16 | 17 | # List of all source files, separated by whitespace 18 | COMMONSOURCE = $(wildcard $(COMMONTARGET)/$(SRCPATH)/*.cpp) 19 | SERVERSOURCE = $(wildcard $(SERVERTARGET)/$(SRCPATH)/*.cpp) 20 | CLIENTSOURCE = $(wildcard $(CLIENTTARGET)/$(SRCPATH)/*.cpp) 21 | 22 | TESTSOURCE = TestRunner.cpp 23 | TESTSOURCE += $(wildcard $(COMMONTARGET)/test/*.cpp) 24 | TESTSOURCE += $(wildcard $(SERVERTARGET)/test/*.cpp) 25 | TESTSOURCE += $(wildcard $(CLIENTTARGET)/test/*.cpp) 26 | 27 | COMMONOBJS = $(patsubst $(COMMONTARGET)/$(SRCPATH)/%.cpp, $(BUILDPATH)/%.o, $(COMMONSOURCE)) 28 | SERVEROBJS = $(patsubst $(SERVERTARGET)/$(SRCPATH)/%.cpp, $(BUILDPATH)/%.o, $(SERVERSOURCE)) 29 | CLIENTOBJS = $(patsubst $(CLIENTTARGET)/$(SRCPATH)/%.cpp, $(BUILDPATH)/%.o, $(CLIENTSOURCE)) 30 | 31 | EXAMPLESERVERSOURCE = $(EXAMPLEPATH)/exampleDoIPServer.cpp 32 | 33 | .PHONY: all clean 34 | 35 | all: env $(BUILDPATH)/$(COMMONTARGET).so $(BUILDPATH)/$(SERVERTARGET).so $(BUILDPATH)/$(CLIENTTARGET).so test examples 36 | 37 | env: 38 | mkdir -p $(BUILDPATH) 39 | 40 | clean: 41 | rm -rf $(BUILDPATH)/* 42 | 43 | $(BUILDPATH)/%.o: $(COMMONTARGET)/$(SRCPATH)/%.cpp 44 | $(CXX) $(CPPFLAGS) -I $(COMMONTARGET)/$(INCPATH) -fPIC -c $< -o $@ 45 | 46 | $(BUILDPATH)/%.o: $(SERVERTARGET)/$(SRCPATH)/%.cpp 47 | $(CXX) $(CPPFLAGS) -I $(SERVERTARGET)/$(INCPATH) -I $(COMMONTARGET)/$(INCPATH) -fPIC -c $< -o $@ 48 | 49 | $(BUILDPATH)/%.o: $(CLIENTTARGET)/$(SRCPATH)/%.cpp 50 | $(CXX) $(CPPFLAGS) -I $(CLIENTTARGET)/$(INCPATH) -I $(COMMONTARGET)/$(INCPATH) -fPIC -c $< -o $@ 51 | 52 | $(BUILDPATH)/$(COMMONTARGET).so: $(COMMONOBJS) 53 | $(CXX) $(CPPFLAGS) $^ $(LDFLAGS) -o $@ 54 | 55 | $(BUILDPATH)/$(SERVERTARGET).so: $(SERVEROBJS) 56 | $(CXX) $(CPPFLAGS) $^ $(LDFLAGS) -o $@ 57 | 58 | $(BUILDPATH)/$(CLIENTTARGET).so: $(CLIENTOBJS) 59 | $(CXX) $(CPPFLAGS) $^ $(LDFLAGS) -o $@ 60 | 61 | test: 62 | $(CXX) $(CPPFLAGS) -I $(COMMONTARGET)/$(INCPATH) -I $(SERVERTARGET)/$(INCPATH) -I $(CLIENTTARGET)/$(INCPATH) $(COMMONSOURCE) $(SERVERSOURCE) $(CLIENTSOURCE) -o runTest $(TESTSOURCE) $(TESTFLAGS) 63 | 64 | examples: $(BUILDPATH)/exampleDoIPServer 65 | 66 | $(BUILDPATH)/exampleDoIPServer: $(EXAMPLESERVERSOURCE) 67 | $(CXX) $(CPPFLAGS) -I $(COMMONTARGET)/$(INCPATH) -I $(SERVERTARGET)/$(INCPATH) -o $@ $^ -ldoipserver -ldoipcommon -lpthread -L$(BUILDPATH) 68 | 69 | install: 70 | install -d /usr/lib/libdoip 71 | install -d /usr/lib/libdoip/include 72 | install build/*.so /usr/lib/libdoip 73 | install libdoipcommon/include/*.h /usr/lib/libdoip/include 74 | install libdoipserver/include/*.h /usr/lib/libdoip/include 75 | 76 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # libdoip 2 | C/C++ library for Diagnostics over IP (DoIP) 3 | 4 | ### Installing library for Diagnostics over IP 5 | 6 | 1. To install the library on the system, first get the source files with: 7 | ``` 8 | git clone https://github.com/GerritRiesch94/libdoip 9 | ``` 10 | 11 | 2. Enter the directory 'libdoip' and build the library with: 12 | ``` 13 | make 14 | ``` 15 | 16 | 3. To install the builded library into `/usr/lib/libdoip` use: 17 | ``` 18 | sudo make install 19 | ``` 20 | 21 | ### Installing Google Test Framework 22 | 23 | In order to compile the unit tests, the Google Test Framework is required to be installed on the system. 24 | 25 | 1. Install the default `libgtest-dev` package with: 26 | ``` 27 | sudo apt-get install libgtest-dev 28 | ``` 29 | 2. Enter the directory in the bash of the installed package which is by default `/usr/src/gtest` and execute: 30 | ``` 31 | sudo cmake CMakeLists.txt 32 | ``` 33 | If this command finish successfully, build the static libraries with: 34 | ``` 35 | sudo make 36 | ``` 37 | 3. If the previous step was successfully, you should have `libgtest.a` and `libgtest_main.a` in the gtest directory. 38 | Copy the libraries into the `/usr/lib` directory with: 39 | ``` 40 | sudo cp libgtest.a /usr/lib 41 | ``` 42 | and 43 | ``` 44 | sudo cp libgtest_main.a /usr/lib 45 | ``` 46 | 47 | 4. The Google Test Framework is now ready to use to compile tests. 48 | 49 | ##### Test if Google Test Framework works 50 | 51 | This section will change as soon the Makefile is ready to use with the project. 52 | 53 | 1. Assuming you have a .cpp file with the main method and a simple test you can compile the test file with: 54 | ``` 55 | g++ -o testRun testFile.cpp -g -L/usr/lib/ -lgtest -lgtest_main -lpthread 56 | ``` 57 | If this finish successfully you should have a executable called `testRun` in the directory. 58 | 59 | 2. To execute the test and receive a xml file type: 60 | ``` 61 | ./testRun --gtest_output="xml:./testOutput.xml" 62 | ``` 63 | -------------------------------------------------------------------------------- /TestRunner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char **argv) { 4 | testing::InitGoogleTest(&argc, argv); 5 | return RUN_ALL_TESTS(); 6 | } 7 | -------------------------------------------------------------------------------- /doc/developer documentation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AVL-DiTEST-DiagDev/libdoip/54ca36d584e1a3c663d8ef7bac9dbd3c7069bb03/doc/developer documentation.pdf -------------------------------------------------------------------------------- /examples/exampleDoIPServer.cpp: -------------------------------------------------------------------------------- 1 | #include "DoIPServer.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | 9 | static const unsigned short LOGICAL_ADDRESS = 0x28; 10 | 11 | DoIPServer server; 12 | unique_ptr connection(nullptr); 13 | std::vector doipReceiver; 14 | bool serverActive = false; 15 | 16 | /** 17 | * Is called when the doip library receives a diagnostic message. 18 | * @param address logical address of the ecu 19 | * @param data message which was received 20 | * @param length length of the message 21 | */ 22 | void ReceiveFromLibrary(unsigned short address, unsigned char* data, int length) { 23 | cout << "DoIP Message received from 0x" << hex << address << ": "; 24 | for(int i = 0; i < length; i++) { 25 | cout << hex << setw(2) << (int)data[i] << " "; 26 | } 27 | cout << endl; 28 | 29 | if(length > 2 && data[0] == 0x22) { 30 | cout << "-> Send diagnostic message positive response" << endl; 31 | unsigned char responseData[] = { 0x62, data[1], data[2], 0x01, 0x02, 0x03, 0x04}; 32 | connection->sendDiagnosticPayload(LOGICAL_ADDRESS, responseData, sizeof(responseData)); 33 | } else { 34 | cout << "-> Send diagnostic message negative response" << endl; 35 | unsigned char responseData[] = { 0x7F, data[0], 0x11}; 36 | connection->sendDiagnosticPayload(LOGICAL_ADDRESS, responseData, sizeof(responseData)); 37 | } 38 | 39 | 40 | } 41 | 42 | /** 43 | * Will be called when the doip library receives a diagnostic message. 44 | * The library notifies the application about the message. 45 | * Checks if there is a ecu with the logical address 46 | * @param targetAddress logical address to the ecu 47 | * @return If a positive or negative ACK should be send to the client 48 | */ 49 | bool DiagnosticMessageReceived(unsigned short targetAddress) { 50 | (void)targetAddress; 51 | unsigned char ackCode; 52 | 53 | cout << "Received Diagnostic message" << endl; 54 | 55 | //send positiv ack 56 | ackCode = 0x00; 57 | cout << "-> Send positive diagnostic message ack" << endl; 58 | connection->sendDiagnosticAck(LOGICAL_ADDRESS, true, ackCode); 59 | 60 | return true; 61 | } 62 | 63 | /** 64 | * Closes the connection of the server by ending the listener threads 65 | */ 66 | void CloseConnection() { 67 | cout << "Connection closed" << endl; 68 | //serverActive = false; 69 | } 70 | 71 | /* 72 | * Check permantly if udp message was received 73 | */ 74 | void listenUdp() { 75 | 76 | while(serverActive) { 77 | server.receiveUdpMessage(); 78 | } 79 | } 80 | 81 | /* 82 | * Check permantly if tcp message was received 83 | */ 84 | void listenTcp() { 85 | 86 | server.setupTcpSocket(); 87 | 88 | while(true) { 89 | connection = server.waitForTcpConnection(); 90 | connection->setCallback(ReceiveFromLibrary, DiagnosticMessageReceived, CloseConnection); 91 | connection->setGeneralInactivityTime(50000); 92 | 93 | while(connection->isSocketActive()) { 94 | connection->receiveTcpMessage(); 95 | } 96 | } 97 | } 98 | 99 | void ConfigureDoipServer() { 100 | // VIN needs to have a fixed length of 17 bytes. 101 | // Shorter VINs will be padded with '0' 102 | server.setVIN("FOOBAR"); 103 | server.setLogicalGatewayAddress(LOGICAL_ADDRESS); 104 | server.setGID(0); 105 | server.setFAR(0); 106 | server.setEID(0); 107 | 108 | // doipserver->setA_DoIP_Announce_Num(tempNum); 109 | // doipserver->setA_DoIP_Announce_Interval(tempInterval); 110 | 111 | } 112 | 113 | int main() { 114 | ConfigureDoipServer(); 115 | 116 | server.setupUdpSocket(); 117 | 118 | serverActive = true; 119 | doipReceiver.push_back(thread(&listenUdp)); 120 | doipReceiver.push_back(thread(&listenTcp)); 121 | 122 | server.sendVehicleAnnouncement(); 123 | 124 | doipReceiver.at(0).join(); 125 | doipReceiver.at(1).join(); 126 | return 0; 127 | } 128 | -------------------------------------------------------------------------------- /libdoipclient/include/DoIPClient_h.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef DOIPCLIENT_H 3 | #define DOIPCLIENT_H 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "DiagnosticMessageHandler.h" 13 | #include "DoIPGenericHeaderHandler.h" 14 | 15 | const int _serverPortNr=13400; 16 | const int _maxDataSize=64; 17 | 18 | 19 | class DoIPClient{ 20 | 21 | public: 22 | void startTcpConnection(); 23 | void startUdpConnection(); 24 | void sendRoutingActivationRequest(); 25 | void sendVehicleIdentificationRequest(const char* address); 26 | void receiveRoutingActivationResponse(); 27 | void receiveUdpMessage(); 28 | void receiveMessage(); 29 | void sendDiagnosticMessage(unsigned char* targetAddress, unsigned char* userData, int userDataLength); 30 | void sendAliveCheckResponse(); 31 | void setSourceAddress(unsigned char* address); 32 | void displayVIResponseInformation(); 33 | void closeTcpConnection(); 34 | void closeUdpConnection(); 35 | void reconnectServer(); 36 | 37 | int getSockFd(); 38 | int getConnected(); 39 | 40 | private: 41 | unsigned char _receivedData[_maxDataSize]; 42 | int _sockFd, _sockFd_udp, _connected; 43 | int broadcast = 1; 44 | struct sockaddr_in _serverAddr, _clientAddr; 45 | unsigned char sourceAddress [2]; 46 | 47 | unsigned char VINResult [17]; 48 | unsigned char LogicalAddressResult [2]; 49 | unsigned char EIDResult [6]; 50 | unsigned char GIDResult [6]; 51 | unsigned char FurtherActionReqResult; 52 | 53 | const std::pair* buildRoutingActivationRequest(); 54 | const std::pair* buildVehicleIdentificationRequest(); 55 | void parseVIResponseInformation(unsigned char* data); 56 | 57 | int emptyMessageCounter = 0; 58 | }; 59 | 60 | 61 | 62 | #endif /* DOIPCLIENT_H */ 63 | 64 | -------------------------------------------------------------------------------- /libdoipclient/src/DoIPClient.cpp: -------------------------------------------------------------------------------- 1 | #include "DoIPClient_h.h" 2 | 3 | /* 4 | *Set up the connection between client and server 5 | */ 6 | void DoIPClient::startTcpConnection() { 7 | 8 | const char* ipAddr = "127.0.0.1"; 9 | bool connectedFlag = false; 10 | _sockFd = socket(AF_INET,SOCK_STREAM,0); 11 | 12 | if(_sockFd>=0) 13 | { 14 | std::cout << "Client TCP-Socket created successfully" << std::endl; 15 | 16 | _serverAddr.sin_family = AF_INET; 17 | _serverAddr.sin_port = htons(_serverPortNr); 18 | inet_aton(ipAddr,&(_serverAddr.sin_addr)); 19 | 20 | while(!connectedFlag) 21 | { 22 | _connected = connect(_sockFd,(struct sockaddr *) &_serverAddr,sizeof(_serverAddr)); 23 | if(_connected!=-1) 24 | { 25 | connectedFlag = true; 26 | std::cout << "Connection to server established" << std::endl; 27 | } 28 | } 29 | } 30 | } 31 | 32 | void DoIPClient::startUdpConnection(){ 33 | 34 | _sockFd_udp = socket(AF_INET,SOCK_DGRAM, 0); 35 | 36 | if(_sockFd_udp >= 0) 37 | { 38 | std::cout << "Client-UDP-Socket created successfully" << std::endl; 39 | 40 | _serverAddr.sin_family = AF_INET; 41 | _serverAddr.sin_port = htons(_serverPortNr); 42 | _serverAddr.sin_addr.s_addr = htonl(INADDR_ANY); 43 | 44 | _clientAddr.sin_family = AF_INET; 45 | _clientAddr.sin_port = htons(_serverPortNr); 46 | _clientAddr.sin_addr.s_addr = htonl(INADDR_ANY); 47 | 48 | //binds the socket to any IP Address and the Port Number 13400 49 | bind(_sockFd_udp, (struct sockaddr *)&_clientAddr, sizeof(_clientAddr)); 50 | } 51 | } 52 | 53 | /* 54 | * closes the client-socket 55 | */ 56 | void DoIPClient::closeTcpConnection(){ 57 | close(_sockFd); 58 | } 59 | 60 | void DoIPClient::closeUdpConnection(){ 61 | close(_sockFd_udp); 62 | } 63 | 64 | void DoIPClient::reconnectServer(){ 65 | closeTcpConnection(); 66 | startTcpConnection(); 67 | } 68 | 69 | /* 70 | *Build the Routing-Activation-Request for server 71 | */ 72 | const std::pair* DoIPClient::buildRoutingActivationRequest() { 73 | 74 | std::pair * rareqWithLength= new std::pair(); 75 | int rareqLength=15; 76 | unsigned char * rareq= new unsigned char[rareqLength]; 77 | 78 | //Generic Header 79 | rareq[0]=0x02; //Protocol Version 80 | rareq[1]=0xFD; //Inverse Protocol Version 81 | rareq[2]=0x00; //Payload-Type 82 | rareq[3]=0x05; 83 | rareq[4]=0x00; //Payload-Length 84 | rareq[5]=0x00; 85 | rareq[6]=0x00; 86 | rareq[7]=0x07; 87 | 88 | //Payload-Type specific message-content 89 | rareq[8]=0x0E; //Source Address 90 | rareq[9]=0x00; 91 | rareq[10]=0x00; //Activation-Type 92 | rareq[11]=0x00; //Reserved ISO(default) 93 | rareq[12]=0x00; 94 | rareq[13]=0x00; 95 | rareq[14]=0x00; 96 | 97 | rareqWithLength->first=rareqLength; 98 | rareqWithLength->second=rareq; 99 | 100 | return rareqWithLength; 101 | } 102 | 103 | /* 104 | * Send the builded request over the tcp-connection to server 105 | */ 106 | void DoIPClient::sendRoutingActivationRequest() { 107 | 108 | const std::pair * rareqWithLength=buildRoutingActivationRequest(); 109 | write(_sockFd,rareqWithLength->second,rareqWithLength->first); 110 | } 111 | 112 | /** 113 | * Sends a diagnostic message to the server 114 | * @param targetAddress the address of the ecu which should receive the message 115 | * @param userData data that will be given to the ecu 116 | * @param userDataLength length of userData 117 | */ 118 | void DoIPClient::sendDiagnosticMessage(unsigned char* targetAddress, unsigned char* userData, int userDataLength) { 119 | unsigned short sourceAddress = 0x0E00; 120 | unsigned char* message = createDiagnosticMessage(sourceAddress, targetAddress, userData, userDataLength); 121 | 122 | write(_sockFd, message, _GenericHeaderLength + _DiagnosticMessageMinimumLength + userDataLength); 123 | } 124 | 125 | /** 126 | * Sends a alive check response containing the clients source address to the server 127 | */ 128 | void DoIPClient::sendAliveCheckResponse() { 129 | int responseLength = 2; 130 | unsigned char* message = createGenericHeader(PayloadType::ALIVECHECKRESPONSE, responseLength); 131 | message[8] = sourceAddress[0]; 132 | message[9] = sourceAddress[1]; 133 | write(_sockFd, message, _GenericHeaderLength + responseLength); 134 | } 135 | 136 | /* 137 | * Receive a message from server 138 | */ 139 | void DoIPClient::receiveMessage() { 140 | 141 | int readedBytes = recv(_sockFd,_receivedData,_maxDataSize, 0); 142 | 143 | if(!readedBytes) //if server is disconnected from client; client gets empty messages 144 | { 145 | emptyMessageCounter++; 146 | 147 | if(emptyMessageCounter == 5) 148 | { 149 | std::cout << "Received to many empty messages. Reconnect TCP connection" << std::endl; 150 | emptyMessageCounter = 0; 151 | reconnectServer(); 152 | } 153 | return; 154 | } 155 | 156 | printf("Client received: "); 157 | for(int i = 0; i < readedBytes; i++) 158 | { 159 | printf("0x%02X ", _receivedData[i]); 160 | } 161 | printf("\n "); 162 | 163 | GenericHeaderAction action = parseGenericHeader(_receivedData, readedBytes); 164 | 165 | if(action.type == PayloadType::DIAGNOSTICPOSITIVEACK || action.type == PayloadType::DIAGNOSTICNEGATIVEACK) { 166 | switch(action.type) { 167 | case PayloadType::DIAGNOSTICPOSITIVEACK: { 168 | std::cout << "Client received diagnostic message positive ack with code: "; 169 | printf("0x%02X ", _receivedData[12]); 170 | break; 171 | } 172 | case PayloadType::DIAGNOSTICNEGATIVEACK: { 173 | std::cout << "Client received diagnostic message negative ack with code: "; 174 | printf("0x%02X ", _receivedData[12]); 175 | break; 176 | } 177 | default: { 178 | std::cerr << "not handled payload type occured in receiveMessage()" << std::endl; 179 | break; 180 | } 181 | } 182 | std::cout << std::endl; 183 | } 184 | 185 | } 186 | 187 | void DoIPClient::receiveUdpMessage() { 188 | 189 | unsigned int length = sizeof(_clientAddr); 190 | 191 | int readedBytes; 192 | readedBytes = recvfrom(_sockFd_udp, _receivedData, _maxDataSize, 0, (struct sockaddr*)&_clientAddr, &length); 193 | 194 | if(PayloadType::VEHICLEIDENTRESPONSE == parseGenericHeader(_receivedData, readedBytes).type) 195 | { 196 | parseVIResponseInformation(_receivedData); 197 | } 198 | 199 | } 200 | 201 | const std::pair* DoIPClient::buildVehicleIdentificationRequest(){ 202 | 203 | std::pair * rareqWithLength= new std::pair(); 204 | int rareqLength= 8; 205 | unsigned char * rareq= new unsigned char[rareqLength]; 206 | 207 | //Generic Header 208 | rareq[0]=0x02; //Protocol Version 209 | rareq[1]=0xFD; //Inverse Protocol Version 210 | rareq[2]=0x00; //Payload-Type 211 | rareq[3]=0x01; 212 | rareq[4]=0x00; //Payload-Length 213 | rareq[5]=0x00; 214 | rareq[6]=0x00; 215 | rareq[7]=0x00; 216 | 217 | rareqWithLength->first=rareqLength; 218 | rareqWithLength->second=rareq; 219 | 220 | return rareqWithLength; 221 | 222 | } 223 | 224 | void DoIPClient::sendVehicleIdentificationRequest(const char* address){ 225 | 226 | int setAddressError = inet_aton(address,&(_serverAddr.sin_addr)); 227 | 228 | if(setAddressError != 0) 229 | { 230 | std::cout <<"Address set succesfully"<* rareqWithLength=buildVehicleIdentificationRequest(); 245 | 246 | int sendError = sendto(_sockFd_udp, rareqWithLength->second,rareqWithLength->first, 0, (struct sockaddr *) &_serverAddr, sizeof(_serverAddr)); 247 | 248 | if(sendError > 0) 249 | { 250 | std::cout << "Sending Vehicle Identification Request" << std::endl; 251 | } 252 | } 253 | 254 | /** 255 | * Sets the source address for this client 256 | * @param address source address for the client 257 | */ 258 | void DoIPClient::setSourceAddress(unsigned char* address) { 259 | sourceAddress[0] = address[0]; 260 | sourceAddress[1] = address[1]; 261 | } 262 | 263 | /* 264 | * Getter for _sockFD 265 | */ 266 | int DoIPClient::getSockFd() { 267 | return _sockFd; 268 | } 269 | 270 | /* 271 | * Getter for _connected 272 | */ 273 | int DoIPClient::getConnected() { 274 | return _connected; 275 | } 276 | 277 | void DoIPClient::parseVIResponseInformation(unsigned char* data){ 278 | 279 | //VIN 280 | int j = 0; 281 | for(int i = 8; i <= 24; i++) 282 | { 283 | VINResult[j] = data[i]; 284 | j++; 285 | } 286 | 287 | //Logical Adress 288 | j = 0; 289 | for(int i = 25; i <= 26; i++) 290 | { 291 | LogicalAddressResult[j] = data[i]; 292 | j++; 293 | } 294 | 295 | //EID 296 | j = 0; 297 | for(int i = 27; i <= 32; i++) 298 | { 299 | EIDResult[j] = data[i]; 300 | j++; 301 | } 302 | 303 | //GID 304 | j = 0; 305 | for(int i = 33; i <= 38; i++) 306 | { 307 | GIDResult[j] = data[i]; 308 | j++; 309 | } 310 | 311 | //FurtherActionRequest 312 | FurtherActionReqResult = data[39]; 313 | 314 | } 315 | 316 | void DoIPClient::displayVIResponseInformation() 317 | { 318 | //output VIN 319 | std::cout << "VIN: "; 320 | for(int i = 0; i < 17 ;i++) 321 | { 322 | std::cout << (unsigned char)(int)VINResult[i]; 323 | } 324 | std::cout << std::endl; 325 | 326 | //output LogicalAddress 327 | std::cout << "LogicalAddress: "; 328 | for(int i = 0; i < 2; i++) 329 | { 330 | printf("%02X", (int)LogicalAddressResult[i]); 331 | } 332 | std::cout << std::endl; 333 | 334 | //output EID 335 | std::cout << "EID: "; 336 | for(int i = 0; i < 6; i++) 337 | { 338 | printf("%02X", EIDResult[i]); 339 | } 340 | std::cout << std::endl; 341 | 342 | //output GID 343 | std::cout << "GID: "; 344 | for(int i = 0; i < 6; i++) 345 | { 346 | printf("%02X", (int)GIDResult[i]); 347 | } 348 | std::cout << std::endl; 349 | 350 | //output FurtherActionRequest 351 | std::cout << "FurtherActionRequest: "; 352 | printf("%02X", (int)FurtherActionReqResult); 353 | 354 | std::cout << std::endl; 355 | } 356 | -------------------------------------------------------------------------------- /libdoipclient/test/DoIPClient_Test.cpp: -------------------------------------------------------------------------------- 1 | #include "DoIPClient_h.h" 2 | #include 3 | 4 | class DoIPClient_Test : public ::testing::Test{ 5 | 6 | protected: 7 | void SetUp() override { 8 | } 9 | 10 | DoIPClient client1; 11 | }; -------------------------------------------------------------------------------- /libdoipcommon/include/AliveCheckTimer.h: -------------------------------------------------------------------------------- 1 | #ifndef ALIVECHECKTIMER_H 2 | #define ALIVECHECKTIMER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | using CloseConnectionCallback = std::function; 11 | 12 | class AliveCheckTimer { 13 | public: 14 | void setTimer(uint16_t seconds); 15 | void startTimer(); 16 | void resetTimer(); 17 | 18 | bool disabled = false; 19 | bool active = false; 20 | bool timeout = false; 21 | CloseConnectionCallback cb; 22 | 23 | ~AliveCheckTimer(); 24 | 25 | private: 26 | std::vector timerThreads; 27 | void waitForResponse(); 28 | clock_t startTime; 29 | uint16_t maxSeconds; 30 | }; 31 | 32 | #endif -------------------------------------------------------------------------------- /libdoipcommon/include/DiagnosticMessageHandler.h: -------------------------------------------------------------------------------- 1 | #ifndef DIAGNOSTICMESSAGEHANDLER_H 2 | #define DIAGNOSTICMESSAGEHANDLER_H 3 | 4 | #include "DoIPGenericHeaderHandler.h" 5 | #include 6 | 7 | using DiagnosticCallback = std::function; 8 | using DiagnosticMessageNotification = std::function; 9 | 10 | const int _DiagnosticPositiveACKLength = 5; 11 | const int _DiagnosticMessageMinimumLength = 4; 12 | 13 | const unsigned char _ValidDiagnosticMessageCode = 0x00; 14 | const unsigned char _InvalidSourceAddressCode = 0x02; 15 | const unsigned char _UnknownTargetAddressCode = 0x03; 16 | 17 | unsigned char parseDiagnosticMessage(DiagnosticCallback callback, unsigned char sourceAddress [2], unsigned char* data, int diagMessageLength); 18 | unsigned char* createDiagnosticACK(bool ackType, unsigned short sourceAddress, unsigned char targetAddress [2], unsigned char responseCode); 19 | unsigned char* createDiagnosticMessage(unsigned short sourceAddress, unsigned char targetAddress [2], unsigned char* userData, int userDataLength); 20 | 21 | 22 | #endif /* DIAGNOSTICMESSAGEHANDLER_H */ 23 | -------------------------------------------------------------------------------- /libdoipcommon/include/DoIPGenericHeaderHandler.h: -------------------------------------------------------------------------------- 1 | #ifndef DOIPGENERICHEADERHANDLER_H 2 | #define DOIPGENERICHEADERHANDLER_H 3 | 4 | #include 5 | 6 | const int _GenericHeaderLength = 8; 7 | const int _NACKLength = 1; 8 | 9 | const unsigned char _IncorrectPatternFormatCode = 0x00; 10 | const unsigned char _UnknownPayloadTypeCode = 0x01; 11 | const unsigned char _InvalidPayloadLengthCode = 0x04; 12 | 13 | enum PayloadType { 14 | NEGATIVEACK, 15 | ROUTINGACTIVATIONREQUEST, 16 | ROUTINGACTIVATIONRESPONSE, 17 | VEHICLEIDENTREQUEST, 18 | VEHICLEIDENTRESPONSE, 19 | DIAGNOSTICMESSAGE, 20 | DIAGNOSTICPOSITIVEACK, 21 | DIAGNOSTICNEGATIVEACK, 22 | ALIVECHECKRESPONSE, 23 | }; 24 | 25 | struct GenericHeaderAction { 26 | PayloadType type; 27 | unsigned char value; 28 | unsigned long payloadLength; 29 | }; 30 | 31 | GenericHeaderAction parseGenericHeader(unsigned char* data, int dataLenght); 32 | unsigned char* createGenericHeader(PayloadType type, uint32_t length); 33 | 34 | 35 | #endif /* DOIPGENERICHEADERHANDLER_H */ 36 | 37 | -------------------------------------------------------------------------------- /libdoipcommon/src/AliveCheckTimer.cpp: -------------------------------------------------------------------------------- 1 | #include "AliveCheckTimer.h" 2 | 3 | /** 4 | * Initialize and starts the alive check timer 5 | */ 6 | void AliveCheckTimer::startTimer() { 7 | if(disabled == false) { 8 | resetTimer(); 9 | timerThreads.push_back(std::thread(&AliveCheckTimer::waitForResponse, this)); 10 | } 11 | } 12 | 13 | /** 14 | * Checks if a timeout occured 15 | */ 16 | void AliveCheckTimer::waitForResponse() { 17 | while(!disabled) { 18 | double secondsPassed = (clock() - startTime) / CLOCKS_PER_SEC; 19 | if(secondsPassed >= maxSeconds) { 20 | disabled = true; 21 | timeout = true; 22 | cb(); 23 | } 24 | } 25 | } 26 | 27 | /** 28 | * Resets the timer to the current time 29 | */ 30 | void AliveCheckTimer::resetTimer() { 31 | startTime = clock(); 32 | active = true; 33 | } 34 | 35 | /** 36 | * Sets the maximum seconds to wait till a timeout occurs 37 | * @param seconds seconds till timeout 38 | */ 39 | void AliveCheckTimer::setTimer(uint16_t seconds) { 40 | maxSeconds = seconds; 41 | } 42 | 43 | AliveCheckTimer::~AliveCheckTimer() { 44 | disabled = true; 45 | if(timerThreads.size() > 0) { 46 | timerThreads.at(0).join(); 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /libdoipcommon/src/DiagnosticMessageHandler.cpp: -------------------------------------------------------------------------------- 1 | #include "DiagnosticMessageHandler.h" 2 | #include 3 | #include 4 | 5 | /** 6 | * Checks if a received Diagnostic Message is valid 7 | * @param cb callback which will be called with the user data 8 | * @param sourceAddress currently registered source address on the socket 9 | * @param data message which was received 10 | * @param diagMessageLength length of the diagnostic message 11 | */ 12 | unsigned char parseDiagnosticMessage(DiagnosticCallback callback, unsigned char sourceAddress [2], 13 | unsigned char* data, int diagMessageLength) { 14 | std::cout << "parse Diagnostic Message" << std::endl; 15 | if(diagMessageLength >= _DiagnosticMessageMinimumLength) { 16 | //Check if the received SA is registered on the socket 17 | if(data[0] != sourceAddress[0] || data[1] != sourceAddress[1]) { 18 | //SA of received message is not registered on this TCP_DATA socket 19 | return _InvalidSourceAddressCode; 20 | } 21 | 22 | std::cout << "source address valid" << std::endl; 23 | //Pass the diagnostic message to the target network/transport layer 24 | unsigned short target_address = 0; 25 | target_address |= ((unsigned short)data[2]) << 8U; 26 | target_address |= (unsigned short)data[3]; 27 | 28 | int cb_message_length = diagMessageLength - _DiagnosticMessageMinimumLength; 29 | unsigned char* cb_message = new unsigned char[cb_message_length]; 30 | 31 | for(int i = _DiagnosticMessageMinimumLength; i < diagMessageLength; i++) { 32 | cb_message[i - _DiagnosticMessageMinimumLength] = data[i]; 33 | } 34 | 35 | callback(target_address, cb_message, cb_message_length); 36 | 37 | //return positive ack code 38 | return _ValidDiagnosticMessageCode; 39 | } 40 | return _UnknownTargetAddressCode; 41 | } 42 | 43 | /** 44 | * Creates a diagnostic message positive/negative acknowledgment message 45 | * @param type defines positive/negative acknowledge type 46 | * @param sourceAddress logical address of the receiver of the previous diagnostic message 47 | * @param targetAddress logical address of the sender of the previous diagnostic message 48 | * @param responseCode positive or negative acknowledge code 49 | * @return pointer to the created diagnostic message acknowledge 50 | */ 51 | unsigned char* createDiagnosticACK(bool ackType, unsigned short sourceAddress, 52 | unsigned char targetAddress [2], unsigned char responseCode) { 53 | 54 | PayloadType type; 55 | if(ackType) 56 | type = PayloadType::DIAGNOSTICPOSITIVEACK; 57 | else 58 | type = PayloadType::DIAGNOSTICNEGATIVEACK; 59 | 60 | unsigned char* message = createGenericHeader(type, _DiagnosticPositiveACKLength); 61 | 62 | //add source address to the message 63 | message[8] = (unsigned char)((sourceAddress >> 8) & 0xFF); 64 | message[9] = (unsigned char)(sourceAddress & 0xFF); 65 | 66 | //add target address to the message 67 | message[10] = targetAddress[0]; 68 | message[11] = targetAddress[1]; 69 | 70 | //add positive or negative acknowledge code to the message 71 | message[12] = responseCode; 72 | 73 | return message; 74 | } 75 | 76 | /** 77 | * Creates a complete diagnostic message 78 | * @param sourceAddress logical address of the sender of a diagnostic message 79 | * @param targetAddress logical address of the receiver of a diagnostic message 80 | * @param userData actual diagnostic data 81 | * @param userDataLength length of diagnostic data 82 | */ 83 | unsigned char* createDiagnosticMessage(unsigned short sourceAddress, unsigned char targetAddress [2], 84 | unsigned char* userData, int userDataLength) { 85 | 86 | unsigned char* message = createGenericHeader(PayloadType::DIAGNOSTICMESSAGE, _DiagnosticMessageMinimumLength + userDataLength); 87 | 88 | //add source address to the message 89 | message[8] = (unsigned char)((sourceAddress >> 8) & 0xFF); 90 | message[9] = (unsigned char)(sourceAddress & 0xFF); 91 | 92 | //add target address to the message 93 | message[10] = targetAddress[0]; 94 | message[11] = targetAddress[1]; 95 | 96 | //add userdata to the message 97 | for(int i = 0; i < userDataLength; i++) { 98 | message[12 + i] = userData[i]; 99 | } 100 | 101 | return message; 102 | } 103 | -------------------------------------------------------------------------------- /libdoipcommon/src/DoIPGenericHeaderHandler.cpp: -------------------------------------------------------------------------------- 1 | #include "DoIPGenericHeaderHandler.h" 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | 7 | /** 8 | * Checks if the received Generic Header is valid 9 | * @param data message which was received 10 | * @param dataLenght length of the message 11 | * @return Returns a GenericHeaderAction struct, which stores the 12 | * payload type and a byte for further message processing 13 | */ 14 | GenericHeaderAction parseGenericHeader(unsigned char* data, int dataLenght) { 15 | 16 | GenericHeaderAction action; 17 | 18 | //Only check header if received data is greater or equals the set header length 19 | if(dataLenght >= _GenericHeaderLength) { 20 | 21 | //Check Generic DoIP synchronization pattern 22 | if((int)(data[1] ^ (0xFF)) != (int)data[0]) { 23 | //Return Error, Protocol Version not correct 24 | action.type = PayloadType::NEGATIVEACK; 25 | action.value = _IncorrectPatternFormatCode; 26 | action.payloadLength = 0; 27 | return action; 28 | } 29 | 30 | unsigned int payloadLength = 0; 31 | payloadLength |= (unsigned int)(data[4] << 24); 32 | payloadLength |= (unsigned int)(data[5] << 16); 33 | payloadLength |= (unsigned int)(data[6] << 8); 34 | payloadLength |= (unsigned int)(data[7] << 0); 35 | 36 | action.payloadLength = payloadLength; 37 | 38 | //Check Payload Type 39 | PayloadType messagePayloadType; 40 | if(data[2] == 0x00 && data[3] == 0x05) { //Value of RoutingActivationRequest = 0x0005 41 | messagePayloadType = PayloadType::ROUTINGACTIVATIONREQUEST; 42 | } 43 | else if(data[2] == 0x00 && data[3] == 0x04){ 44 | messagePayloadType = PayloadType::VEHICLEIDENTRESPONSE; 45 | } 46 | else if(data[2] == 0x00 && data[3] == 0x01) { //Value of Vehicle Identification Request = 0x0001 47 | messagePayloadType = PayloadType::VEHICLEIDENTREQUEST; 48 | } 49 | else if(data[2] == 0x80 && data[3] == 0x01) { //Value of Diagnose Message = 0x8001 50 | messagePayloadType = PayloadType::DIAGNOSTICMESSAGE; 51 | } 52 | else if(data[2] == 0x80 && data[3] == 0x02) { //Value of Diagnostic Message positive ack = 0x8002 53 | messagePayloadType = PayloadType::DIAGNOSTICPOSITIVEACK; 54 | } 55 | else if(data[2] == 0x80 && data[3] == 0x03) { //Value of Diagnostic Message negative ack = 0x8003 56 | messagePayloadType = PayloadType::DIAGNOSTICNEGATIVEACK; 57 | } else { 58 | //Unknown Payload Type --> Send Generic DoIP Header NACK 59 | action.type = PayloadType::NEGATIVEACK; 60 | action.value = _UnknownPayloadTypeCode; 61 | return action; 62 | } 63 | 64 | //Check Payload Type specific length 65 | switch(messagePayloadType) { 66 | case PayloadType::ROUTINGACTIVATIONREQUEST: { 67 | if(payloadLength != 7 && payloadLength != 11) { 68 | action.type = PayloadType::NEGATIVEACK; 69 | action.value = _InvalidPayloadLengthCode; 70 | return action; 71 | } 72 | break; 73 | } 74 | 75 | case PayloadType::ALIVECHECKRESPONSE: { 76 | if(payloadLength != 2) { 77 | action.type = PayloadType::NEGATIVEACK; 78 | action.value = _InvalidPayloadLengthCode; 79 | return action; 80 | } 81 | break; 82 | } 83 | 84 | case PayloadType::VEHICLEIDENTREQUEST: { 85 | if(payloadLength != 0) { 86 | action.type = PayloadType::NEGATIVEACK; 87 | action.value = _InvalidPayloadLengthCode; 88 | return action; 89 | } 90 | break; 91 | } 92 | 93 | case PayloadType::VEHICLEIDENTRESPONSE:{ 94 | if(payloadLength != 32 && payloadLength != 33) { 95 | action.type = PayloadType::NEGATIVEACK; 96 | action.value = _InvalidPayloadLengthCode; 97 | return action; 98 | } 99 | break; 100 | } 101 | 102 | case PayloadType::DIAGNOSTICMESSAGE: { 103 | if(payloadLength <= 4) { 104 | action.type = PayloadType::NEGATIVEACK; 105 | action.value = _InvalidPayloadLengthCode; 106 | return action; 107 | } 108 | break; 109 | } 110 | 111 | case PayloadType::DIAGNOSTICPOSITIVEACK: { 112 | if(payloadLength < 5) { 113 | action.type = PayloadType::NEGATIVEACK; 114 | action.value = _InvalidPayloadLengthCode; 115 | } 116 | break; 117 | } 118 | 119 | case PayloadType::DIAGNOSTICNEGATIVEACK: { 120 | if(payloadLength < 5) { 121 | action.type = PayloadType::NEGATIVEACK; 122 | action.value = _InvalidPayloadLengthCode; 123 | } 124 | break; 125 | } 126 | 127 | default: { 128 | std::cerr << "not handled payload type occured in parseGenericHeader()" << std::endl; 129 | break; 130 | } 131 | } 132 | action.type = messagePayloadType; 133 | } 134 | 135 | return action; 136 | } 137 | 138 | /** 139 | * Creates a generic header 140 | * @param type payload type which will be filled in the header 141 | * @param length length of the payload type specific message 142 | * @return header array 143 | */ 144 | unsigned char* createGenericHeader(PayloadType type, uint32_t length) { 145 | unsigned char *header = new unsigned char[8 + length]; 146 | header[0] = 0x02; 147 | header[1] = 0xFD; 148 | switch(type) { 149 | case PayloadType::ROUTINGACTIVATIONRESPONSE: { 150 | header[2] = 0x00; 151 | header[3] = 0x06; 152 | break; 153 | } 154 | 155 | case PayloadType::NEGATIVEACK: { 156 | header[2] = 0x00; 157 | header[3] = 0x00; 158 | break; 159 | } 160 | 161 | case PayloadType::VEHICLEIDENTRESPONSE:{ 162 | header[2] = 0x00; 163 | header[3] = 0x04; 164 | break; 165 | } 166 | 167 | case PayloadType::DIAGNOSTICMESSAGE: { 168 | header[2] = 0x80; 169 | header[3] = 0x01; 170 | break; 171 | } 172 | 173 | case PayloadType::DIAGNOSTICPOSITIVEACK: { 174 | header[2] = 0x80; 175 | header[3] = 0x02; 176 | break; 177 | } 178 | 179 | case PayloadType::DIAGNOSTICNEGATIVEACK: { 180 | header[2] = 0x80; 181 | header[3] = 0x03; 182 | break; 183 | } 184 | 185 | case PayloadType::ALIVECHECKRESPONSE: { 186 | header[2] = 0x00; 187 | header[3] = 0x08; 188 | break; 189 | } 190 | 191 | default: { 192 | std::cerr << "not handled payload type occured in createGenericHeader()" << std::endl; 193 | break; 194 | } 195 | } 196 | 197 | header[4] = (length >> 24) & 0xFF; 198 | header[5] = (length >> 16) & 0xFF; 199 | header[6] = (length >> 8) & 0xFF; 200 | header[7] = length & 0xFF; 201 | 202 | return header; 203 | } 204 | -------------------------------------------------------------------------------- /libdoipcommon/test/DoIPGenericHeaderHandler_Test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "DoIPGenericHeaderHandler.h" 3 | 4 | class GenericHeaderTest : public ::testing::Test { 5 | public: 6 | unsigned char* request; 7 | 8 | protected: 9 | void SetUp() override { 10 | request = new unsigned char[15]; 11 | request[0] = 0x01; 12 | request[1] = 0xFE; 13 | request[2] = 0x00; 14 | request[3] = 0x05; 15 | request[4] = 0x00; 16 | request[5] = 0x00; 17 | request[6] = 0x00; 18 | request[7] = 0x07; 19 | request[8] = 0x0F; 20 | request[9] = 0x12; 21 | request[10] = 0x00; 22 | request[11] = 0x00; 23 | request[12] = 0x00; 24 | request[13] = 0x00; 25 | request[14] = 0x00; 26 | } 27 | 28 | void TearDown() override { 29 | delete[] request; 30 | } 31 | }; 32 | 33 | /* 34 | * Checks if a wrong synchronization pattern leads to the correct response type and NACK code (0x00) 35 | */ 36 | TEST_F(GenericHeaderTest, WrongSynchronizationPattern) { 37 | //Set wrong inverse protocol version 38 | request[1] = 0x33; 39 | GenericHeaderAction action = parseGenericHeader(request, 15); 40 | ASSERT_EQ(action.type, PayloadType::NEGATIVEACK) << "returned payload type is wrong"; 41 | ASSERT_EQ(action.value, 0x00) << "returned NACK code is wrong"; 42 | 43 | //Set currently not supported protocol version 44 | request[1] = 0xFE; 45 | request[0] = 0x04; 46 | action = parseGenericHeader(request, 15); 47 | ASSERT_EQ(action.type, PayloadType::NEGATIVEACK) << "returned payload type is wrong"; 48 | ASSERT_EQ(action.value, 0x00) << "returned NACK code is wrong"; 49 | } 50 | 51 | /* 52 | * Checks if a unknown payload type leads to the correct response type and NACK code (0x01) 53 | */ 54 | TEST_F(GenericHeaderTest, UnknownPayloadType) { 55 | //Set unknown payload type (0x0010) 56 | request[3] = 0x10; 57 | GenericHeaderAction action = parseGenericHeader(request, 15); 58 | ASSERT_EQ(action.type, PayloadType::NEGATIVEACK); 59 | ASSERT_EQ(action.value, 0x01); 60 | } 61 | 62 | /* 63 | * Checks if a known payload type in the request leads to the correct payload type in action 64 | * Checks Routing Activation Request type 65 | */ 66 | TEST_F(GenericHeaderTest, KnownPayloadType_RoutingActivationRequest) { 67 | GenericHeaderAction action = parseGenericHeader(request, 15); 68 | ASSERT_EQ(action.type, PayloadType::ROUTINGACTIVATIONREQUEST); 69 | } 70 | 71 | /* 72 | * Checks if a known payload type in the request leads to the correct payload type in action 73 | * Checks Vehicle Identification Request type 74 | */ 75 | TEST_F(GenericHeaderTest, KnownPayloadType_VehicleIdentificationRequest) { 76 | //change payload type 77 | request[2] = 0x00; 78 | request[3] = 0x01; 79 | 80 | //change payload length 81 | request[7] = 0x00; 82 | 83 | GenericHeaderAction action = parseGenericHeader(request, 8); //VehidleIdentificationRequest length only 8 84 | ASSERT_EQ(action.type, PayloadType::VEHICLEIDENTREQUEST); 85 | } 86 | 87 | /* 88 | * Checks if a known payload type in the request leads to the correct payload type in action 89 | * Checks Diagnostic Message type 90 | */ 91 | TEST_F(GenericHeaderTest, KnownPayloadType_DiagnosticMessage) { 92 | request[2] = 0x80; 93 | request[3] = 0x01; 94 | GenericHeaderAction action = parseGenericHeader(request, 15); 95 | ASSERT_EQ(action.type, PayloadType::DIAGNOSTICMESSAGE); 96 | } 97 | 98 | /* 99 | * Checks if a wrong routing activation payload length return the correct response type and NACK code (0x04) 100 | */ 101 | TEST_F(GenericHeaderTest, WrongRoutingActivationLength) { 102 | request[7] = 0x08; // Use invalid routing activation payload length 8 103 | GenericHeaderAction action = parseGenericHeader(request, 20); 104 | ASSERT_EQ(action.type, PayloadType::NEGATIVEACK); 105 | ASSERT_EQ(action.value, 0x04); 106 | } 107 | 108 | /* 109 | * Checks if a valid generic header leads to the correct response type 110 | */ 111 | TEST_F(GenericHeaderTest, ValidGenericHeader) { 112 | GenericHeaderAction action = parseGenericHeader(request, 15); 113 | ASSERT_NE(action.type, PayloadType::NEGATIVEACK); 114 | } 115 | -------------------------------------------------------------------------------- /libdoipserver/include/DoIPConnection.h: -------------------------------------------------------------------------------- 1 | #ifndef DOIPCONNECTION_H 2 | #define DOIPCONNECTION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "DoIPGenericHeaderHandler.h" 13 | #include "RoutingActivationHandler.h" 14 | #include "VehicleIdentificationHandler.h" 15 | #include "DoIPGenericHeaderHandler.h" 16 | #include "RoutingActivationHandler.h" 17 | #include "DiagnosticMessageHandler.h" 18 | #include "AliveCheckTimer.h" 19 | 20 | using CloseConnectionCallback = std::function; 21 | 22 | const unsigned long _MaxDataSize = 0xFFFFFF; 23 | 24 | class DoIPConnection { 25 | 26 | public: 27 | DoIPConnection(int tcpSocket, unsigned short logicalGatewayAddress): 28 | tcpSocket(tcpSocket), logicalGatewayAddress(logicalGatewayAddress) { }; 29 | 30 | int receiveTcpMessage(); 31 | unsigned long receiveFixedNumberOfBytesFromTCP(unsigned long payloadLength, unsigned char *receivedData); 32 | 33 | void sendDiagnosticPayload(unsigned short sourceAddress, unsigned char* data, int length); 34 | bool isSocketActive() { return tcpSocket != 0; }; 35 | 36 | void triggerDisconnection(); 37 | 38 | void sendDiagnosticAck(unsigned short sourceAddress, bool ackType, unsigned char ackCode); 39 | int sendNegativeAck(unsigned char ackCode); 40 | 41 | void setCallback(DiagnosticCallback dc, DiagnosticMessageNotification dmn, CloseConnectionCallback ccb); 42 | void setGeneralInactivityTime(const uint16_t seconds); 43 | 44 | private: 45 | 46 | int tcpSocket; 47 | 48 | AliveCheckTimer aliveCheckTimer; 49 | DiagnosticCallback diag_callback; 50 | CloseConnectionCallback close_connection; 51 | DiagnosticMessageNotification notify_application; 52 | 53 | unsigned char* routedClientAddress; 54 | unsigned short logicalGatewayAddress = 0x0000; 55 | 56 | void closeSocket(); 57 | 58 | int reactOnReceivedTcpMessage(GenericHeaderAction action, unsigned long payloadLength, unsigned char *payload); 59 | 60 | int sendMessage(unsigned char* message, int messageLenght); 61 | 62 | void aliveCheckTimeout(); 63 | }; 64 | 65 | #endif /* DOIPCONNECTION_H */ 66 | -------------------------------------------------------------------------------- /libdoipserver/include/DoIPServer.h: -------------------------------------------------------------------------------- 1 | #ifndef DOIPSERVER_H 2 | #define DOIPSERVER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "DoIPGenericHeaderHandler.h" 14 | #include "RoutingActivationHandler.h" 15 | #include "VehicleIdentificationHandler.h" 16 | #include "DoIPGenericHeaderHandler.h" 17 | #include "RoutingActivationHandler.h" 18 | #include "DiagnosticMessageHandler.h" 19 | #include "AliveCheckTimer.h" 20 | #include "DoIPConnection.h" 21 | 22 | using CloseConnectionCallback = std::function; 23 | 24 | const int _ServerPort = 13400; 25 | //const unsigned long _MaxDataSize = 4294967294; 26 | //const unsigned long _MaxDataSize = 0xFFFFFF; 27 | 28 | class DoIPServer { 29 | 30 | public: 31 | DoIPServer() = default; 32 | 33 | void setupTcpSocket(); 34 | std::unique_ptr waitForTcpConnection(); 35 | void setupUdpSocket(); 36 | int receiveUdpMessage(); 37 | 38 | void closeTcpSocket(); 39 | void closeUdpSocket(); 40 | 41 | int sendVehicleAnnouncement(); 42 | 43 | void setEIDdefault(); 44 | void setVIN(std::string VINString); 45 | void setLogicalGatewayAddress(const unsigned short inputLogAdd); 46 | void setEID(const uint64_t inputEID); 47 | void setGID(const uint64_t inputGID); 48 | void setFAR(const unsigned int inputFAR); 49 | void setA_DoIP_Announce_Num(int Num); 50 | void setA_DoIP_Announce_Interval(int Interval); 51 | 52 | private: 53 | 54 | int server_socket_tcp, server_socket_udp; 55 | struct sockaddr_in serverAddress, clientAddress; 56 | unsigned char data[_MaxDataSize]; 57 | 58 | std::string VIN = "00000000000000000"; 59 | unsigned short LogicalGatewayAddress = 0x0000; 60 | unsigned char EID [6]; 61 | unsigned char GID [6] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; 62 | unsigned char FurtherActionReq = 0x00; 63 | 64 | int A_DoIP_Announce_Num = 3; //Default Value = 3 65 | int A_DoIP_Announce_Interval = 500; //Default Value = 500ms 66 | 67 | int broadcast = 1; 68 | 69 | int reactToReceivedUdpMessage(int readedBytes); 70 | 71 | int sendUdpMessage(unsigned char* message, int messageLength); 72 | 73 | void setMulticastGroup(const char* address); 74 | }; 75 | 76 | #endif /* DOIPSERVER_H */ -------------------------------------------------------------------------------- /libdoipserver/include/RoutingActivationHandler.h: -------------------------------------------------------------------------------- 1 | #ifndef ROUTINGACTIVATIONHANDLER_H 2 | #define ROUTINGACTIVATIONHANDLER_H 3 | 4 | #include 5 | #include 6 | #include "DoIPGenericHeaderHandler.h" 7 | 8 | const int _ActivationResponseLength = 9; 9 | 10 | const unsigned char _UnknownSourceAddressCode = 0x00; 11 | const unsigned char _UnsupportedRoutingTypeCode = 0x06; 12 | const unsigned char _SuccessfullyRoutedCode = 0x10; 13 | 14 | unsigned char parseRoutingActivation(unsigned char* data); 15 | unsigned char* createRoutingActivationResponse(unsigned short sourceAddress, 16 | unsigned char clientAddress[2], 17 | unsigned char responseCode); 18 | bool checkSourceAddress(uint32_t address); 19 | 20 | 21 | #endif /* ROUTINGACTIVATIONHANDLER_H */ 22 | 23 | -------------------------------------------------------------------------------- /libdoipserver/include/VehicleIdentificationHandler.h: -------------------------------------------------------------------------------- 1 | #ifndef VEHICLEIDENTIFICATIONHANDLER_H 2 | #define VEHICLEIDENTIFICATIONHANDLER_H 3 | 4 | #include "DoIPGenericHeaderHandler.h" 5 | #include 6 | 7 | 8 | unsigned char* createVehicleIdentificationResponse(std::string VIN,unsigned short LogicalAdress,unsigned char* EID,unsigned char* GID,unsigned char FurtherActionReq); 9 | 10 | const int _VIResponseLength = 32; 11 | 12 | #endif /* VEHICLEIDENTIFICATIONHANDLER_H */ 13 | 14 | -------------------------------------------------------------------------------- /libdoipserver/src/DoIPConnection.cpp: -------------------------------------------------------------------------------- 1 | #include "DoIPConnection.h" 2 | 3 | #include 4 | #include 5 | 6 | /** 7 | * Closes the connection by closing the sockets 8 | */ 9 | void DoIPConnection::aliveCheckTimeout() { 10 | std::cout << "Alive Check Timeout. Close Connection" << std::endl; 11 | closeSocket(); 12 | close_connection(); 13 | } 14 | 15 | /* 16 | * Closes the socket for this server 17 | */ 18 | void DoIPConnection::closeSocket() { 19 | close(tcpSocket); 20 | tcpSocket = 0; 21 | } 22 | 23 | /* 24 | * Receives a message from the client and calls reactToReceivedTcpMessage method 25 | * @return amount of bytes which were send back to client 26 | * or -1 if error occurred 27 | */ 28 | int DoIPConnection::receiveTcpMessage() { 29 | std::cout << "Waiting for DoIP Header..." << std::endl; 30 | unsigned char genericHeader[_GenericHeaderLength]; 31 | unsigned int readBytes = receiveFixedNumberOfBytesFromTCP(_GenericHeaderLength, genericHeader); 32 | if(readBytes == _GenericHeaderLength && !aliveCheckTimer.timeout) { 33 | std::cout << "Received DoIP Header." << std::endl; 34 | GenericHeaderAction doipHeaderAction = parseGenericHeader(genericHeader, _GenericHeaderLength); 35 | 36 | unsigned char *payload = nullptr; 37 | if(doipHeaderAction.payloadLength > 0) { 38 | std::cout << "Waiting for " << doipHeaderAction.payloadLength << " bytes of payload..." << std::endl; 39 | payload = new unsigned char[doipHeaderAction.payloadLength]; 40 | unsigned int receivedPayloadBytes = receiveFixedNumberOfBytesFromTCP(doipHeaderAction.payloadLength, payload); 41 | if(receivedPayloadBytes != doipHeaderAction.payloadLength) { 42 | closeSocket(); 43 | return 0; 44 | } 45 | std::cout << "DoIP message completely received" << std::endl; 46 | } 47 | 48 | //if alive check timouts should be possible, reset timer when message received 49 | if(aliveCheckTimer.active) { 50 | aliveCheckTimer.resetTimer(); 51 | } 52 | 53 | int sentBytes = reactOnReceivedTcpMessage(doipHeaderAction, doipHeaderAction.payloadLength, payload); 54 | 55 | return sentBytes; 56 | } else { 57 | closeSocket(); 58 | return 0; 59 | } 60 | return -1; 61 | 62 | } 63 | 64 | /** 65 | * Receive exactly payloadLength bytes from the TCP stream and put them into receivedData. 66 | * The method blocks until receivedData bytes are received or the socket is closed. 67 | * 68 | * The parameter receivedData needs to point to a readily allocated array with 69 | * at least payloadLength items. 70 | * 71 | * @return number of bytes received 72 | */ 73 | unsigned long DoIPConnection::receiveFixedNumberOfBytesFromTCP(unsigned long payloadLength, unsigned char *receivedData) { 74 | unsigned long payloadPos = 0; 75 | unsigned long remainingPayload = payloadLength; 76 | 77 | while(remainingPayload > 0) { 78 | int readBytes = recv(tcpSocket, &receivedData[payloadPos], remainingPayload, 0); 79 | if(readBytes <= 0) { 80 | return payloadPos; 81 | } 82 | payloadPos += readBytes; 83 | remainingPayload -= readBytes; 84 | } 85 | 86 | return payloadPos; 87 | } 88 | 89 | /* 90 | * Receives a message from the client and determine how to process the message 91 | * @return amount of bytes which were send back to client 92 | * or -1 if error occurred 93 | */ 94 | int DoIPConnection::reactOnReceivedTcpMessage(GenericHeaderAction action, unsigned long payloadLength, unsigned char *payload) { 95 | 96 | std::cout << "processing DoIP message..." << std::endl; 97 | int sentBytes; 98 | switch(action.type) { 99 | case PayloadType::NEGATIVEACK: { 100 | //send NACK 101 | sentBytes = sendNegativeAck(action.value); 102 | 103 | if(action.value == _IncorrectPatternFormatCode || 104 | action.value == _InvalidPayloadLengthCode) { 105 | closeSocket(); 106 | return -1; 107 | } 108 | 109 | return sentBytes; 110 | } 111 | 112 | case PayloadType::ROUTINGACTIVATIONREQUEST: { 113 | //start routing activation handler with the received message 114 | unsigned char result = parseRoutingActivation(payload); 115 | unsigned char clientAddress [2] = {payload[0], payload[1]}; 116 | 117 | unsigned char* message = createRoutingActivationResponse(logicalGatewayAddress, clientAddress, result); 118 | sentBytes = sendMessage(message, _GenericHeaderLength + _ActivationResponseLength); 119 | 120 | if(result == _UnknownSourceAddressCode || result == _UnsupportedRoutingTypeCode) { 121 | closeSocket(); 122 | return -1; 123 | } else { 124 | //Routing Activation Request was successfull, save address of the client 125 | routedClientAddress = new unsigned char[2]; 126 | routedClientAddress[0] = payload[0]; 127 | routedClientAddress[1] = payload[1]; 128 | 129 | //start alive check timer 130 | if(!aliveCheckTimer.active) { 131 | aliveCheckTimer.cb = std::bind(&DoIPConnection::aliveCheckTimeout,this); 132 | aliveCheckTimer.startTimer(); 133 | } 134 | } 135 | 136 | return sentBytes; 137 | } 138 | 139 | case PayloadType::ALIVECHECKRESPONSE: { 140 | return 0; 141 | } 142 | 143 | case PayloadType::DIAGNOSTICMESSAGE: { 144 | 145 | unsigned short target_address = 0; 146 | target_address |= ((unsigned short)payload[2]) << 8U; 147 | target_address |= (unsigned short)payload[3]; 148 | bool ack = notify_application(target_address); 149 | 150 | if(ack) 151 | parseDiagnosticMessage(diag_callback, routedClientAddress, payload, payloadLength); 152 | 153 | break; 154 | } 155 | 156 | default: { 157 | std::cerr << "Received message with unhandled payload type: " << action.type << std::endl; 158 | return -1; 159 | } 160 | } 161 | return -1; 162 | } 163 | 164 | void DoIPConnection::triggerDisconnection() { 165 | std::cout << "Application requested to disconnect Client from Server" << std::endl; 166 | closeSocket(); 167 | } 168 | 169 | /** 170 | * Sends a message back to the connected client 171 | * @param message contains generic header and payload specific content 172 | * @param messageLength length of the complete message 173 | * @return number of bytes written is returned, 174 | * or -1 if error occurred 175 | */ 176 | int DoIPConnection::sendMessage(unsigned char* message, int messageLength) { 177 | int result = write(tcpSocket, message, messageLength); 178 | return result; 179 | } 180 | 181 | /** 182 | * Sets the time in seconds after which a alive check timeout occurs. 183 | * Alive check timeouts can be deactivated when setting the seconds to 0 184 | * @param seconds time after which alive check timeout occurs 185 | */ 186 | void DoIPConnection::setGeneralInactivityTime(uint16_t seconds) { 187 | if(seconds > 0) { 188 | aliveCheckTimer.setTimer(seconds); 189 | } else { 190 | aliveCheckTimer.disabled = true; 191 | } 192 | } 193 | 194 | /* 195 | * Send diagnostic message payload to the client 196 | * @param sourceAddress logical source address (i.e. address of this server) 197 | * @param value received payload 198 | * @param length length of received payload 199 | */ 200 | void DoIPConnection::sendDiagnosticPayload(unsigned short sourceAddress, unsigned char* data, int length) { 201 | 202 | std::cout << "Sending diagnostic data: "; 203 | for(int i = 0; i < length; i++) { 204 | std::cout << std::hex << std::setw(2) << (unsigned int)data[i] << " "; 205 | } 206 | std::cout << std::endl; 207 | 208 | unsigned char* message = createDiagnosticMessage(sourceAddress, routedClientAddress, data, length); 209 | sendMessage(message, _GenericHeaderLength + _DiagnosticMessageMinimumLength + length); 210 | } 211 | 212 | /* 213 | * Set the callback function for this doip server instance 214 | * @dc Callback which sends the data of a diagnostic message to the application 215 | * @dmn Callback which notifies the application of receiving a diagnostic message 216 | * @ccb Callback for application function when the library closes the connection 217 | */ 218 | void DoIPConnection::setCallback(DiagnosticCallback dc, DiagnosticMessageNotification dmn, CloseConnectionCallback ccb) { 219 | diag_callback = dc; 220 | notify_application = dmn; 221 | close_connection = ccb; 222 | } 223 | 224 | void DoIPConnection::sendDiagnosticAck(unsigned short sourceAddress, bool ackType, unsigned char ackCode) { 225 | unsigned char data_TA [2] = { routedClientAddress[0], routedClientAddress[1] }; 226 | 227 | unsigned char* message = createDiagnosticACK(ackType, sourceAddress, data_TA, ackCode); 228 | sendMessage(message, _GenericHeaderLength + _DiagnosticPositiveACKLength); 229 | } 230 | 231 | /** 232 | * Prepares a generic header nack and sends it to the client 233 | * @param ackCode NACK-Code which will be included in the message 234 | * @return amount of bytes sended to the client 235 | */ 236 | int DoIPConnection::sendNegativeAck(unsigned char ackCode) { 237 | unsigned char* message = createGenericHeader(PayloadType::NEGATIVEACK, _NACKLength); 238 | message[8] = ackCode; 239 | int sendedBytes = sendMessage(message, _GenericHeaderLength + _NACKLength); 240 | return sendedBytes; 241 | } 242 | -------------------------------------------------------------------------------- /libdoipserver/src/DoIPServer.cpp: -------------------------------------------------------------------------------- 1 | #include "DoIPServer.h" 2 | 3 | /* 4 | * Set up a tcp socket, so the socket is ready to accept a connection 5 | */ 6 | void DoIPServer::setupTcpSocket() { 7 | 8 | server_socket_tcp = socket(AF_INET, SOCK_STREAM, 0); 9 | 10 | serverAddress.sin_family = AF_INET; 11 | serverAddress.sin_addr.s_addr = htonl(INADDR_ANY); 12 | serverAddress.sin_port = htons(_ServerPort); 13 | 14 | //binds the socket to the address and port number 15 | bind(server_socket_tcp, (struct sockaddr *)&serverAddress, sizeof(serverAddress)); 16 | } 17 | 18 | /* 19 | * Wait till a client attempts a connection and accepts it 20 | */ 21 | std::unique_ptr DoIPServer::waitForTcpConnection() { 22 | //waits till client approach to make connection 23 | listen(server_socket_tcp, 5); 24 | int tcpSocket = accept(server_socket_tcp, (struct sockaddr*) NULL, NULL); 25 | return std::unique_ptr(new DoIPConnection(tcpSocket, LogicalGatewayAddress)); 26 | } 27 | 28 | void DoIPServer::setupUdpSocket() { 29 | 30 | server_socket_udp = socket(AF_INET, SOCK_DGRAM, 0); 31 | 32 | serverAddress.sin_family = AF_INET; 33 | serverAddress.sin_addr.s_addr = htonl(INADDR_ANY); 34 | serverAddress.sin_port = htons(_ServerPort); 35 | 36 | if(server_socket_udp < 0) 37 | std::cout << "Error setting up a udp socket" << std::endl; 38 | 39 | //binds the socket to any IP Address and the Port Number 13400 40 | bind(server_socket_udp, (struct sockaddr *)&serverAddress, sizeof(serverAddress)); 41 | 42 | //setting the IP Address for Multicast 43 | setMulticastGroup("224.0.0.2"); 44 | } 45 | 46 | /* 47 | * Closes the socket for this server 48 | */ 49 | void DoIPServer::closeTcpSocket() { 50 | close(server_socket_tcp); 51 | } 52 | 53 | void DoIPServer::closeUdpSocket() { 54 | close(server_socket_udp); 55 | } 56 | 57 | /* 58 | * Receives a udp message and calls reactToReceivedUdpMessage method 59 | * @return amount of bytes which were send back to client 60 | * or -1 if error occurred 61 | */ 62 | int DoIPServer::receiveUdpMessage(){ 63 | 64 | unsigned int length = sizeof(serverAddress); 65 | int readBytes = recvfrom(server_socket_udp, data, _MaxDataSize, 0, (struct sockaddr *) &serverAddress, &length); 66 | 67 | int sentBytes = reactToReceivedUdpMessage(readBytes); 68 | 69 | return sentBytes; 70 | } 71 | 72 | 73 | /* 74 | * Receives a udp message and determine how to process the message 75 | * @return amount of bytes which were send back to client 76 | * or -1 if error occurred 77 | */ 78 | int DoIPServer::reactToReceivedUdpMessage(int readedBytes) { 79 | 80 | GenericHeaderAction action = parseGenericHeader(data, readedBytes); 81 | 82 | int sendedBytes; 83 | switch(action.type) { 84 | 85 | case PayloadType::VEHICLEIDENTRESPONSE:{ //server should not send a negative ACK if he receives the sended VehicleIdentificationAnnouncement 86 | return -1; 87 | } 88 | 89 | case PayloadType::NEGATIVEACK: { 90 | //send NACK 91 | unsigned char* message = createGenericHeader(action.type, _NACKLength); 92 | message[8] = action.value; 93 | sendedBytes = sendUdpMessage(message, _GenericHeaderLength + _NACKLength); 94 | 95 | if(action.value == _IncorrectPatternFormatCode || 96 | action.value == _InvalidPayloadLengthCode) { 97 | return -1; 98 | } else { 99 | //discard message when value 0x01, 0x02, 0x03 100 | } 101 | return sendedBytes; 102 | } 103 | 104 | case PayloadType::VEHICLEIDENTREQUEST: { 105 | unsigned char* message = createVehicleIdentificationResponse(VIN, LogicalGatewayAddress, EID, GID, FurtherActionReq); 106 | sendedBytes = sendUdpMessage(message, _GenericHeaderLength + _VIResponseLength); 107 | 108 | return sendedBytes; 109 | } 110 | 111 | default: { 112 | std::cerr << "not handled payload type occured in receiveUdpMessage()" << std::endl; 113 | return -1; 114 | } 115 | } 116 | return -1; 117 | } 118 | 119 | int DoIPServer::sendUdpMessage(unsigned char* message, int messageLength) { //sendUdpMessage after receiving a message from the client 120 | //if the server receives a message from a client, than the response should be send back to the client address and port 121 | clientAddress.sin_port = serverAddress.sin_port; 122 | clientAddress.sin_addr.s_addr = serverAddress.sin_addr.s_addr; 123 | 124 | int result = sendto(server_socket_udp, message, messageLength, 0, (struct sockaddr *)&clientAddress, sizeof(clientAddress)); 125 | return result; 126 | } 127 | 128 | void DoIPServer::setEIDdefault(){ 129 | 130 | int fd; 131 | 132 | struct ifreq ifr; 133 | const char* iface = "ens33"; //eth0 134 | unsigned char* mac; 135 | 136 | fd = socket(AF_INET, SOCK_DGRAM, 0); 137 | 138 | ifr.ifr_addr.sa_family = AF_INET; 139 | 140 | strncpy((char*)ifr.ifr_name, (const char*)iface, IFNAMSIZ-1); 141 | 142 | ioctl(fd, SIOCGIFHWADDR, &ifr); 143 | 144 | close(fd); 145 | 146 | mac = (unsigned char *)ifr.ifr_hwaddr.sa_data; 147 | 148 | //memcpy(mac, (unsigned char *)ifr.ifr_hwaddr.sa_data, 48); 149 | 150 | for(int i = 0; i < 6; i++) 151 | { 152 | EID[i] = mac[i]; 153 | } 154 | } 155 | 156 | void DoIPServer::setVIN( std::string VINString){ 157 | 158 | VIN = VINString; 159 | } 160 | 161 | void DoIPServer::setLogicalGatewayAddress(const unsigned short inputLogAdd){ 162 | LogicalGatewayAddress = inputLogAdd; 163 | } 164 | 165 | void DoIPServer::setEID(const uint64_t inputEID){ 166 | EID[0] = (inputEID >> 40) &0xFF; 167 | EID[1] = (inputEID >> 32) &0xFF; 168 | EID[2] = (inputEID >> 24) &0xFF; 169 | EID[3] = (inputEID >> 16) &0xFF; 170 | EID[4] = (inputEID >> 8) &0xFF; 171 | EID[5] = inputEID & 0xFF; 172 | } 173 | 174 | void DoIPServer::setGID(const uint64_t inputGID){ 175 | GID[0] = (inputGID >> 40) &0xFF; 176 | GID[1] = (inputGID >> 32) &0xFF; 177 | GID[2] = (inputGID >> 24) &0xFF; 178 | GID[3] = (inputGID >> 16) &0xFF; 179 | GID[4] = (inputGID >> 8) &0xFF; 180 | GID[5] = inputGID & 0xFF; 181 | } 182 | 183 | void DoIPServer::setFAR(const unsigned int inputFAR){ 184 | FurtherActionReq = inputFAR & 0xFF; 185 | } 186 | 187 | void DoIPServer::setA_DoIP_Announce_Num(int Num){ 188 | A_DoIP_Announce_Num = Num; 189 | } 190 | 191 | void DoIPServer::setA_DoIP_Announce_Interval(int Interval){ 192 | A_DoIP_Announce_Interval = Interval; 193 | } 194 | 195 | 196 | 197 | void DoIPServer::setMulticastGroup(const char* address) { 198 | 199 | int loop = 1; 200 | 201 | //set Option using the same Port for multiple Sockets 202 | int setPort = setsockopt(server_socket_udp, SOL_SOCKET, SO_REUSEADDR, &loop, sizeof(loop)); 203 | 204 | if(setPort < 0) 205 | { 206 | std::cout << "Setting Port Error" << std::endl; 207 | } 208 | 209 | 210 | struct ip_mreq mreq; 211 | 212 | mreq.imr_multiaddr.s_addr = inet_addr(address); 213 | mreq.imr_interface.s_addr = htonl(INADDR_ANY); 214 | 215 | //set Option to join Multicast Group 216 | int setGroup = setsockopt(server_socket_udp, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char*) &mreq, sizeof(mreq)); 217 | 218 | if(setGroup < 0) 219 | { 220 | std::cout <<"Setting Address Error" << std::endl; 221 | } 222 | } 223 | 224 | 225 | int DoIPServer::sendVehicleAnnouncement() { 226 | 227 | const char* address = "255.255.255.255"; 228 | 229 | //setting the destination port for the Announcement to 13401 230 | clientAddress.sin_port=htons(13401); 231 | 232 | int setAddressError = inet_aton(address,&(clientAddress.sin_addr)); 233 | 234 | 235 | if(setAddressError != 0) 236 | { 237 | std::cout <<"Broadcast Address set succesfully"< 0) 257 | { 258 | std::cout<<"Sending Vehicle Announcement"< close socket 16 | return _UnknownSourceAddressCode; 17 | } 18 | 19 | //Check if routing activation type is supported 20 | switch(data[2]) { 21 | case 0x00: { 22 | //Activation Type default 23 | break; 24 | } 25 | case 0x01: { 26 | //Activation Type WWH-OBD 27 | break; 28 | } 29 | default: { 30 | //unknown activation type 31 | //send routing activation negative response code --> close socket 32 | return _UnsupportedRoutingTypeCode; 33 | } 34 | } 35 | 36 | //if not exited before, send routing activation positive response 37 | return _SuccessfullyRoutedCode; 38 | } 39 | 40 | /** 41 | * Create the complete routing activation response, which also contains the 42 | * generic header 43 | * @param clientAddress address of the test equipment 44 | * @param responseCode routing activation response code 45 | * @return complete routing activation response 46 | */ 47 | unsigned char* createRoutingActivationResponse(unsigned short sourceAddress, unsigned char clientAddress[2], 48 | unsigned char responseCode) { 49 | 50 | unsigned char* message = createGenericHeader(PayloadType::ROUTINGACTIVATIONRESPONSE, 51 | _ActivationResponseLength); 52 | 53 | //Logical address of external test equipment 54 | message[8] = clientAddress[0]; 55 | message[9] = clientAddress[1]; 56 | 57 | //logical address of DoIP entity 58 | message[10] = (unsigned char)((sourceAddress >> 8) & 0xFF); 59 | message[11] = (unsigned char)(sourceAddress & 0xFF); 60 | 61 | //routing activation response code 62 | message[12] = responseCode; 63 | 64 | //reserved for future standardization use 65 | message[13] = 0x00; 66 | message[14] = 0x00; 67 | message[15] = 0x00; 68 | message[16] = 0x00; 69 | 70 | return message; 71 | } 72 | 73 | /** 74 | * Checks if the submitted address is valid 75 | * @param address the address which will be checked 76 | * @return true if address is valid 77 | */ 78 | bool checkSourceAddress(uint32_t address) { 79 | uint32_t minAddress = 3584; // 0x0E00 80 | uint32_t maxAddress = 4095; // 0x0FFF 81 | 82 | for(uint32_t i = minAddress; i <= maxAddress; i++) { 83 | if(address == i) { 84 | return true; 85 | } 86 | } 87 | 88 | return false; 89 | } -------------------------------------------------------------------------------- /libdoipserver/src/VehicleIdentificationHandler.cpp: -------------------------------------------------------------------------------- 1 | #include "VehicleIdentificationHandler.h" 2 | #include 3 | 4 | unsigned char* createVehicleIdentificationResponse(std::string VIN,unsigned short LogicalAddress, 5 | unsigned char* EID, unsigned char* GID, 6 | unsigned char FurtherActionReq) //also used für the Vehicle Announcement 7 | { 8 | unsigned char* message = createGenericHeader(PayloadType::VEHICLEIDENTRESPONSE, _VIResponseLength); 9 | 10 | //VIN Number 11 | unsigned int j = 0; 12 | for(int i = 8; i <= 24; i++) 13 | { 14 | if (j < VIN.length()){ 15 | message[i] = (unsigned char)VIN[j]; 16 | j++; 17 | } else { 18 | //Pad with ASCII '0' if VIN is shorter than 17 bytes 19 | message[i] = (unsigned char)'0'; 20 | } 21 | } 22 | 23 | //Logical Adress 24 | message[25] = (LogicalAddress >> 8) & 0xFF; 25 | message[26] = LogicalAddress & 0xFF; 26 | 27 | //EID 28 | j = 0; 29 | for(int i = 27; i <= 32; i++) 30 | { 31 | message[i] = EID[j]; 32 | j++; 33 | } 34 | 35 | //GID 36 | j = 0; 37 | for(int i = 33; i <= 38; i++) 38 | { 39 | message[i] = GID[j]; 40 | j++; 41 | } 42 | 43 | message[39] = FurtherActionReq; 44 | 45 | return message; 46 | } 47 | 48 | -------------------------------------------------------------------------------- /libdoipserver/test/RoutingActivationHandler_Test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "RoutingActivationHandler.h" 3 | #include 4 | 5 | class RoutingActivationTest : public ::testing::Test { 6 | public: 7 | unsigned char* request; 8 | 9 | protected: 10 | void SetUp() override { 11 | request = new unsigned char[15]; 12 | request[0] = 0x01; 13 | request[1] = 0xFE; 14 | request[2] = 0x00; 15 | request[3] = 0x05; 16 | request[4] = 0x00; 17 | request[5] = 0x00; 18 | request[6] = 0x00; 19 | request[7] = 0x07; 20 | request[8] = 0x0F; 21 | request[9] = 0x12; 22 | request[10] = 0x00; 23 | request[11] = 0x00; 24 | request[12] = 0x00; 25 | request[13] = 0x00; 26 | request[14] = 0x00; 27 | } 28 | 29 | void TearDown() override { 30 | delete[] request; 31 | } 32 | }; 33 | 34 | /* 35 | * Checks if parsing a valid address will be found in the list of valid addresses 36 | */ 37 | TEST_F(RoutingActivationTest, ValidAddressParsing) { 38 | uint32_t expectedValue = 3858; 39 | uint32_t address = 0; 40 | address |= (uint32_t)request[8] << 8; 41 | address |= (uint32_t)request[9]; 42 | ASSERT_EQ(address, expectedValue) << "Converting address to uint failed"; 43 | 44 | bool result = checkSourceAddress(address); 45 | ASSERT_TRUE(result) <<"Didnt found address"; 46 | } 47 | 48 | /* 49 | * Checks if parsing a wrong address leads to the correct negative response code (0x00) 50 | */ 51 | TEST_F(RoutingActivationTest, WrongAddressParsing) { 52 | //Set wrong address in received message 53 | request[8] = 0x0D; 54 | request[9] = 0x00; 55 | unsigned char result = parseRoutingActivation(request); 56 | ASSERT_EQ(result, 0x00) <<"Wrong address doesn't return correct response code"; 57 | } 58 | 59 | /* 60 | * Checks if parsing a unknown activation type leads to the correct negative response code (0x06) 61 | */ 62 | TEST_F(RoutingActivationTest, UnknownActivationType) { 63 | //Set wrong activation type 64 | request[10] = 0xFF; 65 | unsigned char result = parseRoutingActivation(request); 66 | ASSERT_EQ(result, 0x06); 67 | } 68 | 69 | /* 70 | * Checks if a valid routing activation request leads to the correct routing activation code (0x10); 71 | */ 72 | TEST_F(RoutingActivationTest, ValidRequest) { 73 | unsigned char result = parseRoutingActivation(request); 74 | ASSERT_EQ(result, 0x10); 75 | } 76 | -------------------------------------------------------------------------------- /libdoipserver/test/VehicleIdentificationHandler_Test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "VehicleIdentificationHandler.h" 3 | #include 4 | #include 5 | #include "DoIPServer.h" 6 | 7 | using namespace std; 8 | 9 | class VehicleIdentificationHandlerTest : public ::testing::Test { 10 | public: 11 | string matchingVIN = "MatchingVin_12345"; 12 | string shortVIN = "shortVin"; 13 | string shortVINPadded = "shortVin000000000"; 14 | unsigned char EID[6] = {0, 0, 0, 0, 0, 0}; 15 | unsigned char GID[6] = {0, 0, 0, 0, 0, 0}; 16 | unsigned char far = 0; 17 | protected: 18 | void SetUp() override { 19 | } 20 | }; 21 | 22 | /* 23 | * Checks if a VIN with 17 bytes matches correctly the input data 24 | */ 25 | TEST_F(VehicleIdentificationHandlerTest, Vin17Bytes) { 26 | // Call function under test to create message 27 | unsigned char * message = createVehicleIdentificationResponse(matchingVIN, 0, EID, GID, far); 28 | 29 | // Extract VIN from created test message 30 | char tempvin[18]; // Need 1 byte more for \0 at the end for parsing via string() 31 | for(int i=0; i<=16; i++) { 32 | tempvin[i] = message[i+8]; 33 | } 34 | tempvin[17] = 0; // Mark end of string 35 | 36 | delete[] message; 37 | 38 | // Assert that extracted VIN matches input data 39 | string expected = string(tempvin); 40 | EXPECT_EQ(matchingVIN, expected) << "Setting VIN with 17 bytes failed"; 41 | } 42 | 43 | /* 44 | * Checks if a VIN < 17 bytes is padded correctly with zero bytes 45 | */ 46 | TEST_F(VehicleIdentificationHandlerTest, VinLessThan17Bytes) { 47 | // Call function under test to create message 48 | unsigned char * message = createVehicleIdentificationResponse(shortVIN, 0, EID, GID, far); 49 | 50 | // Extract VIN from created test message 51 | char c_tempvin[18]; 52 | for(int i=0; i<=16; i++) { 53 | c_tempvin[i] = message[i+8]; 54 | } 55 | c_tempvin[17] = 0; // Mark end of string 56 | 57 | // Create expected string value 58 | string actualVin = string(c_tempvin); 59 | 60 | // Assert that extracted VIN matches input data 61 | EXPECT_EQ(shortVINPadded, actualVin) << "Setting VIN with < 17 bytes failed"; 62 | 63 | // Assert message after shortVin bytes is padded with '0' 64 | for(int i=16; i<=24; i++) { 65 | EXPECT_EQ(message[i], '0') << "VIN not correctly padded at byte: " << i; 66 | } 67 | delete[] message; 68 | } 69 | 70 | --------------------------------------------------------------------------------