26 #define _WIN32_WINNT 0x601 39 #if !defined(__GNUC__) 40 #pragma comment(lib, "ws2_32.lib") // MSVC only. 43 #define XCP_COMM_PORT (5555) 45 #define DEFAULT_FAMILY PF_UNSPEC // Accept either IPv4 or IPv6 46 #define DEFAULT_SOCKTYPE SOCK_STREAM // 47 #define DEFAULT_PORT "5555" 51 SOCKADDR_STORAGE connectionAddress;
52 SOCKADDR_STORAGE currentAddress;
54 SOCKET connectedSocket;
60 typedef enum tagHandleType {
67 typedef enum tagIoOpcode { IoAccept, IoRead, IoWrite } IoOpcode;
70 HandleType handleType;
77 OVERLAPPED overlapped;
80 char buf[XCP_COMM_BUFLEN];
84 int addrSize =
sizeof(SOCKADDR_STORAGE);
88 void Xcp_DispatchCommand(Xcp_PduType
const *
const pdu);
90 extern Xcp_PduType Xcp_CtoIn;
91 extern Xcp_PduType Xcp_CtoOut;
93 static HANDLE XcpTl_CreateIOCP(
void);
94 static bool XcpTl_RegisterIOCPHandle(HANDLE port, HANDLE
object, ULONG_PTR key);
95 static DWORD WINAPI WorkerThread(LPVOID lpParameter);
96 static DWORD WINAPI AcceptorThread(LPVOID lpParameter);
97 void XcpTl_PostQuitMessage(
void);
98 static void XcpTl_TriggerRecv(DWORD numBytes);
99 static void XcpTl_Feed(DWORD numBytesReceived);
100 static boolean Xcp_EnableSocketOption(SOCKET sock,
int option);
102 static boolean Xcp_EnableSocketOption(SOCKET sock,
int option) {
103 #if !defined(__STDC_VERSION__) || __STDC_VERSION__ < 199901L 104 const char enable = 1;
106 if (setsockopt(sock, SOL_SOCKET, option, &enable,
sizeof(
int)) < 0) {
110 if (setsockopt(sock, SOL_SOCKET, option, &(
const char){1},
sizeof(int)) < 0) {
117 #define TL_WORKER_THREAD (0) 118 #define TL_ACCEPTOR_THREAD (1) 119 #define NUM_TL_THREADS (2) 121 static HANDLE XcpTl_Threads[NUM_TL_THREADS];
125 void XcpTl_Init(
void) {
127 ADDRINFO Hints, *AddrInfo, *AI;
128 char *Address = NULL;
129 char *Port = DEFAULT_PORT;
130 SOCKET serverSockets[FD_SETSIZE];
131 int boundSocketNum = -1;
134 DWORD dwTimeAdjustment = 0UL, dwTimeIncrement = 0UL;
135 BOOL fAdjustmentDisabled = XCP_TRUE;
139 ZeroMemory(&serverSockets, FD_SETSIZE);
140 XcpTl_Connection.iocp = XcpTl_CreateIOCP();
142 XcpTl_Threads[TL_WORKER_THREAD] = CreateThread(NULL, 0, WorkerThread, (LPVOID)XcpTl_Connection.iocp, 0, NULL);
143 SetThreadPriority(XcpTl_Threads[TL_WORKER_THREAD], THREAD_PRIORITY_ABOVE_NORMAL);
144 SetProcessAffinityMask(XcpTl_Threads[TL_WORKER_THREAD], 1UL);
146 ZeroMemory(&Hints,
sizeof(Hints));
147 GetSystemTimeAdjustment(&dwTimeAdjustment, &dwTimeIncrement, &fAdjustmentDisabled);
148 if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0) {
149 XcpHw_ErrorMsg(
"XcpTl_Init:WSAStartup()", WSAGetLastError());
153 XcpTl_Connection.socketType = Xcp_Options.tcp ? SOCK_STREAM : SOCK_DGRAM;
154 Hints.ai_family = Xcp_Options.ipv6 ? PF_INET6 : PF_INET;
155 Hints.ai_socktype = XcpTl_Connection.socketType;
156 Hints.ai_flags = AI_NUMERICHOST | AI_PASSIVE;
157 ret = getaddrinfo(Address, Port, &Hints, &AddrInfo);
159 XcpHw_ErrorMsg(
"XcpTl_Init::getaddrinfo()", WSAGetLastError());
163 for (idx = 0, AI = AddrInfo; AI != NULL; AI = AI->ai_next, ++idx) {
164 if (idx == FD_SETSIZE) {
165 printf(
"getaddrinfo returned more addresses than we could use.\n");
168 if ((AI->ai_family != PF_INET) && (AI->ai_family != PF_INET6)) {
171 serverSockets[idx] = WSASocket(AI->ai_family, AI->ai_socktype, AI->ai_protocol, NULL, 0, WSA_FLAG_OVERLAPPED);
172 if (serverSockets[idx] == INVALID_SOCKET) {
173 XcpHw_ErrorMsg(
"XcpTl_Init::socket()", WSAGetLastError());
177 XcpTl_RegisterIOCPHandle(XcpTl_Connection.iocp, (HANDLE)serverSockets[idx], (ULONG_PTR)serverSockets[idx]);
179 if (bind(serverSockets[idx], AI->ai_addr, AI->ai_addrlen) == SOCKET_ERROR) {
180 XcpHw_ErrorMsg(
"XcpTl_Init::bind()", WSAGetLastError());
183 if (XcpTl_Connection.socketType == SOCK_STREAM) {
184 if (listen(serverSockets[idx], 1) == SOCKET_ERROR) {
185 XcpHw_ErrorMsg(
"XcpTl_Init::listen()", WSAGetLastError());
189 boundSocketNum = idx;
190 XcpTl_Connection.boundSocket = serverSockets[boundSocketNum];
193 freeaddrinfo(AddrInfo);
194 if (boundSocketNum == -1) {
196 "Fatal error: unable to serve on any address.\nPerhaps" 197 " a server is already running on port %s / %s [%s]?\n",
198 DEFAULT_PORT, Xcp_Options.tcp ?
"TCP" :
"UDP", Xcp_Options.ipv6 ?
"IPv6" :
"IPv4");
202 if (!Xcp_EnableSocketOption(XcpTl_Connection.boundSocket, SO_REUSEADDR)) {
203 XcpHw_ErrorMsg(
"XcpTl_Init:setsockopt(SO_REUSEADDR)", WSAGetLastError());
206 setsockopt(XcpTl_Connection.boundSocket, IPPROTO_TCP, TCP_NODELAY, (
const char *)&one,
sizeof(one));
208 recvOlap.opcode = IoRead;
209 recvOlap.wsabuf.buf = recvOlap.buf;
210 recvOlap.wsabuf.len = XCP_COMM_BUFLEN;
212 sendOlap.opcode = IoWrite;
213 sendOlap.wsabuf.buf = sendOlap.buf;
214 sendOlap.wsabuf.len = XCP_COMM_BUFLEN;
216 if (XcpTl_Connection.socketType == SOCK_STREAM) {
217 XcpTl_Threads[TL_ACCEPTOR_THREAD] = CreateThread(NULL, 0, AcceptorThread, NULL, 0, NULL);
218 SetProcessAffinityMask(XcpTl_Threads[TL_ACCEPTOR_THREAD], 1UL);
220 recvOlap.opcode = IoAccept;
221 XcpTl_TriggerRecv(XCP_COMM_BUFLEN);
225 void XcpTl_DeInit(
void) {
228 XcpTl_PostQuitMessage();
229 closesocket(XcpTl_Connection.connectedSocket);
230 closesocket(XcpTl_Connection.boundSocket);
231 CloseHandle(XcpTl_Connection.iocp);
232 WaitForMultipleObjects(NUM_TL_THREADS, XcpTl_Threads, TRUE, INFINITE);
233 for (idx = 0; idx < NUM_TL_THREADS; ++idx) {
234 CloseHandle(XcpTl_Threads[idx]);
240 void XcpTl_MainFunction(
void) {}
242 void *get_in_addr(
struct sockaddr *sa) {
243 if (sa->sa_family == AF_INET) {
244 return &(((
struct sockaddr_in *)sa)->sin_addr);
246 return &(((
struct sockaddr_in6 *)sa)->sin6_addr);
249 static DWORD WINAPI AcceptorThread(LPVOID lpParameter) {
251 SOCKADDR_STORAGE From;
255 XCP_UNREFERENCED_PARAMETER(lpParameter);
258 if (!XcpTl_Connection.xcpConnected) {
259 fromLen =
sizeof(From);
260 XcpTl_Connection.connectedSocket =
261 accept(XcpTl_Connection.boundSocket, (LPSOCKADDR)&XcpTl_Connection.currentAddress, &fromLen);
262 if (XcpTl_Connection.connectedSocket == INVALID_SOCKET) {
263 error = WSAGetLastError();
264 if (error == WSAEINTR) {
267 XcpHw_ErrorMsg(
"AcceptorThread::accept()", error);
272 if (!Xcp_EnableSocketOption(XcpTl_Connection.connectedSocket, SO_REUSEADDR)) {
273 XcpHw_ErrorMsg(
"AcceptorThread::setsockopt(SO_REUSEADDR)", WSAGetLastError());
275 setsockopt(XcpTl_Connection.boundSocket, IPPROTO_TCP, TCP_NODELAY, (
const char *)&one,
sizeof(one));
276 XcpTl_Connection.socketConnected = XCP_TRUE;
277 XcpTl_RegisterIOCPHandle(XcpTl_Connection.iocp, (HANDLE)XcpTl_Connection.connectedSocket,
278 (ULONG_PTR)XcpTl_Connection.connectedSocket);
279 XcpTl_TriggerRecv(XCP_COMM_BUFLEN);
285 void XcpTl_Send(uint8_t
const *buf, uint16_t len) {
289 sendOlap.wsabuf.buf = (
char *)buf;
290 sendOlap.wsabuf.len = len;
291 sendOlap.opcode = IoWrite;
293 SecureZeroMemory(&sendOlap.overlapped,
sizeof(OVERLAPPED));
294 if (XcpTl_Connection.socketType == SOCK_DGRAM) {
295 addrLen =
sizeof(SOCKADDR_STORAGE);
296 if (WSASendTo(XcpTl_Connection.boundSocket, &sendOlap.wsabuf, 1, &bytesWritten, 0,
297 (LPSOCKADDR)&XcpTl_Connection.currentAddress, addrLen, (LPWSAOVERLAPPED)&sendOlap.overlapped,
298 NULL) == SOCKET_ERROR) {
299 XcpHw_ErrorMsg(
"XcpTl_Send:WSASendTo()", WSAGetLastError());
301 }
else if (XcpTl_Connection.socketType == SOCK_STREAM) {
302 if (WSASend(XcpTl_Connection.connectedSocket, &sendOlap.wsabuf, 1, &bytesWritten, 0, (LPWSAOVERLAPPED)&sendOlap.overlapped,
303 NULL) == SOCKET_ERROR) {
304 XcpHw_ErrorMsg(
"XcpTl_Send:WSASend()", WSAGetLastError());
305 closesocket(XcpTl_Connection.connectedSocket);
310 void XcpTl_SaveConnection(
void) {
311 CopyMemory(&XcpTl_Connection.connectionAddress, &XcpTl_Connection.currentAddress,
sizeof(SOCKADDR_STORAGE));
312 XcpTl_Connection.xcpConnected = XCP_TRUE;
315 void XcpTl_ReleaseConnection(
void) { XcpTl_Connection.xcpConnected = XCP_FALSE; }
317 bool XcpTl_VerifyConnection(
void) {
318 bool res = memcmp(&XcpTl_Connection.connectionAddress, &XcpTl_Connection.currentAddress,
sizeof(SOCKADDR_STORAGE)) == 0;
322 void XcpTl_SetOptions(Xcp_OptionsType
const *options) { CopyMemory(&Xcp_Options, options,
sizeof(Xcp_OptionsType)); }
324 void XcpTl_PrintConnectionInformation(
void) {
325 printf(
"\nXCPonEth -- Listening on port %s / %s [%s]\n", DEFAULT_PORT, Xcp_Options.tcp ?
"TCP" :
"UDP",
326 Xcp_Options.ipv6 ?
"IPv6" :
"IPv4");
336 static DWORD WINAPI WorkerThread(LPVOID lpParameter) {
337 HANDLE hCompletionPort = (HANDLE)lpParameter;
338 DWORD numBytesReceived = 0;
339 ULONG_PTR CompletionKey;
341 OVERLAPPED *olap = NULL;
342 bool exitLoop = FALSE;
346 if (GetQueuedCompletionStatus(hCompletionPort, &numBytesReceived, &CompletionKey, (LPOVERLAPPED *)&olap, INFINITE)) {
347 if ((numBytesReceived == 0) && (CompletionKey == 0)) {
351 switch (iod->opcode) {
358 recvOlap.opcode = IoRead;
359 XcpTl_Feed(numBytesReceived);
360 XcpTl_TriggerRecv(XCP_COMM_BUFLEN);
364 if (numBytesReceived == (DWORD)0) {
365 DBG_PRINT1(
"Client closed connection\n");
366 XcpTl_Connection.socketConnected = XCP_FALSE;
367 closesocket(XcpTl_Connection.connectedSocket);
370 XcpTl_Feed(numBytesReceived);
371 XcpTl_TriggerRecv(XCP_COMM_BUFLEN);
375 #if XCP_ENABLE_SLAVE_BLOCKMODE == XCP_ON 376 Xcp_UploadSingleBlock();
382 error = GetLastError();
388 XcpHw_ErrorMsg(
"WorkerThread::GetQueuedCompletionStatus()", error);
394 static HANDLE XcpTl_CreateIOCP(
void) {
397 handle = CreateIoCompletionPort(INVALID_HANDLE_VALUE, NULL, (ULONG_PTR)0, 1);
398 if (handle == NULL) {
399 XcpHw_ErrorMsg(
"XcpTl_CreateIOCP::CreateIoCompletionPort()", WSAGetLastError());
405 static bool XcpTl_RegisterIOCPHandle(HANDLE port, HANDLE
object, ULONG_PTR key) {
408 handle = CreateIoCompletionPort(
object, port, key, 0);
409 if (handle == NULL) {
410 XcpHw_ErrorMsg(
"XcpTl_RegisterIOCPHandle::CreateIoCompletionPort()", WSAGetLastError());
413 return (
bool)(handle == port);
416 void XcpTl_PostQuitMessage(
void) { PostQueuedCompletionStatus(XcpTl_Connection.iocp, 0, (ULONG_PTR)NULL, NULL); }
418 static void XcpTl_TriggerRecv(DWORD numBytes) {
419 DWORD numReceived = (DWORD)0;
420 DWORD flags = (DWORD)0;
424 XCP_UNREFERENCED_PARAMETER(numBytes);
426 SecureZeroMemory(&recvOlap.overlapped,
sizeof(OVERLAPPED));
428 if (XcpTl_Connection.socketType == SOCK_STREAM) {
429 if (XcpTl_Connection.socketConnected == XCP_FALSE) {
432 if (WSARecv(XcpTl_Connection.connectedSocket, &recvOlap.wsabuf, 1, &numReceived, &flags, (LPWSAOVERLAPPED)&recvOlap,
433 (LPWSAOVERLAPPED_COMPLETION_ROUTINE)NULL) == SOCKET_ERROR) {
434 err = WSAGetLastError();
435 if (err != WSA_IO_PENDING) {
436 XcpHw_ErrorMsg(
"XcpTl_TriggerRecv::WSARecv()", err);
439 }
else if (XcpTl_Connection.socketType == SOCK_DGRAM) {
440 addrLen =
sizeof(SOCKADDR_STORAGE);
441 if (WSARecvFrom(XcpTl_Connection.boundSocket, &recvOlap.wsabuf, 1, &numReceived, &flags,
442 (LPSOCKADDR)&XcpTl_Connection.currentAddress, &addrLen, (LPWSAOVERLAPPED)&recvOlap,
443 (LPWSAOVERLAPPED_COMPLETION_ROUTINE)NULL)) {
444 err = WSAGetLastError();
445 if (err != WSA_IO_PENDING) {
446 XcpHw_ErrorMsg(
"XcpTl_TriggerRecv:WSARecvFrom()", WSAGetLastError());
452 static void XcpTl_Feed(DWORD numBytesReceived) {
455 if (numBytesReceived > 0) {
456 #if XCP_TRANSPORT_LAYER_LENGTH_SIZE == 1 457 dlc = (uint16_t)recvOlap.wsabuf.buf[0];
458 #elif XCP_TRANSPORT_LAYER_LENGTH_SIZE == 2
459 dlc = MAKEWORD(recvOlap.wsabuf.buf[0], recvOlap.wsabuf.buf[1]);
460 #endif // XCP_TRANSPORT_LAYER_LENGTH_SIZE 461 if (!XcpTl_Connection.xcpConnected || (XcpTl_VerifyConnection())) {
463 Xcp_CtoIn.data = (uint8_t *)(recvOlap.wsabuf.buf + XCP_TRANSPORT_LAYER_BUFFER_OFFSET);
464 Xcp_DispatchCommand(&Xcp_CtoIn);
466 if (numBytesReceived < 5) {
467 DBG_PRINT2(
"Error: frame to short: %ld\n", numBytesReceived);