XCP_SIM
wineth_iocp.c
1 /*
2  * BlueParrot XCP
3  *
4  * (C) 2007-2020 by Christoph Schueler <github.com/Christoph2,
5  * cpu12.gems@googlemail.com>
6  *
7  * All Rights Reserved
8  *
9  * This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License along
20  * with this program; if not, write to the Free Software Foundation, Inc.,
21  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
22  *
23  * s. FLOSS-EXCEPTION.txt
24  */
25 
26 #define _WIN32_WINNT 0x601
27 
28 #include <Mstcpip.h>
29 #include <WinSock2.h>
30 #include <Ws2tcpip.h>
31 #include <memory.h>
32 #include <stdio.h>
33 #include <stdlib.h>
34 
35 #include "mswsock.h"
36 #include "xcp.h"
37 #include "xcp_hw.h"
38 
39 #if !defined(__GNUC__)
40 #pragma comment(lib, "ws2_32.lib") // MSVC only.
41 #endif
42 
43 #define XCP_COMM_PORT (5555)
44 
45 #define DEFAULT_FAMILY PF_UNSPEC // Accept either IPv4 or IPv6
46 #define DEFAULT_SOCKTYPE SOCK_STREAM //
47 #define DEFAULT_PORT "5555"
48 
49 typedef struct tagXcpTl_ConnectionType {
50  HANDLE iocp;
51  SOCKADDR_STORAGE connectionAddress;
52  SOCKADDR_STORAGE currentAddress;
53  SOCKET boundSocket;
54  SOCKET connectedSocket;
55  bool xcpConnected;
56  bool socketConnected;
57  int socketType;
59 
60 typedef enum tagHandleType {
61  HANDLE_SOCKET,
62  HANDLE_FILE,
63  HANDLE_NAMED_PIPE,
64  HANDLE_USER,
65 } HandleType;
66 
67 typedef enum tagIoOpcode { IoAccept, IoRead, IoWrite } IoOpcode;
68 
69 typedef struct tagPerHandleData {
70  HandleType handleType;
71  HANDLE handle;
72  DWORD seqNoSend;
73  DWORD seqNoRecv;
75 
76 typedef struct tagPerIoData {
77  OVERLAPPED overlapped;
78  IoOpcode opcode;
79  SOCKET socket;
80  char buf[XCP_COMM_BUFLEN];
81  WSABUF wsabuf;
82 } PerIoData;
83 
84 int addrSize = sizeof(SOCKADDR_STORAGE);
85 
86 static XcpTl_ConnectionType XcpTl_Connection;
87 
88 void Xcp_DispatchCommand(Xcp_PduType const *const pdu);
89 
90 extern Xcp_PduType Xcp_CtoIn;
91 extern Xcp_PduType Xcp_CtoOut;
92 
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);
101 
102 static boolean Xcp_EnableSocketOption(SOCKET sock, int option) {
103 #if !defined(__STDC_VERSION__) || __STDC_VERSION__ < 199901L
104  const char enable = 1;
105 
106  if (setsockopt(sock, SOL_SOCKET, option, &enable, sizeof(int)) < 0) {
107  return XCP_FALSE;
108  }
109 #else
110  if (setsockopt(sock, SOL_SOCKET, option, &(const char){1}, sizeof(int)) < 0) {
111  return XCP_FALSE;
112  }
113 #endif
114  return XCP_TRUE;
115 }
116 
117 #define TL_WORKER_THREAD (0)
118 #define TL_ACCEPTOR_THREAD (1)
119 #define NUM_TL_THREADS (2)
120 
121 static HANDLE XcpTl_Threads[NUM_TL_THREADS];
122 static PerIoData recvOlap = {0};
123 static PerIoData sendOlap = {0};
124 
125 void XcpTl_Init(void) {
126  WSADATA wsa;
127  ADDRINFO Hints, *AddrInfo, *AI;
128  char *Address = NULL;
129  char *Port = DEFAULT_PORT;
130  SOCKET serverSockets[FD_SETSIZE];
131  int boundSocketNum = -1;
132  int ret;
133  int idx;
134  DWORD dwTimeAdjustment = 0UL, dwTimeIncrement = 0UL;
135  BOOL fAdjustmentDisabled = XCP_TRUE;
136  int one = 1;
137 
138  ZeroMemory(&XcpTl_Connection, sizeof(XcpTl_ConnectionType));
139  ZeroMemory(&serverSockets, FD_SETSIZE);
140  XcpTl_Connection.iocp = XcpTl_CreateIOCP();
141 
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);
145 
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());
150  exit(EXIT_FAILURE);
151  } else {
152  }
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);
158  if (ret != 0) {
159  XcpHw_ErrorMsg("XcpTl_Init::getaddrinfo()", WSAGetLastError());
160  WSACleanup();
161  return;
162  }
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");
166  break;
167  }
168  if ((AI->ai_family != PF_INET) && (AI->ai_family != PF_INET6)) {
169  continue;
170  }
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());
174  continue;
175  }
176 
177  XcpTl_RegisterIOCPHandle(XcpTl_Connection.iocp, (HANDLE)serverSockets[idx], (ULONG_PTR)serverSockets[idx]);
178 
179  if (bind(serverSockets[idx], AI->ai_addr, AI->ai_addrlen) == SOCKET_ERROR) {
180  XcpHw_ErrorMsg("XcpTl_Init::bind()", WSAGetLastError());
181  continue;
182  }
183  if (XcpTl_Connection.socketType == SOCK_STREAM) {
184  if (listen(serverSockets[idx], 1) == SOCKET_ERROR) {
185  XcpHw_ErrorMsg("XcpTl_Init::listen()", WSAGetLastError());
186  continue;
187  }
188  }
189  boundSocketNum = idx;
190  XcpTl_Connection.boundSocket = serverSockets[boundSocketNum];
191  break; /* Grab first address. */
192  }
193  freeaddrinfo(AddrInfo);
194  if (boundSocketNum == -1) {
195  fprintf(stderr,
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");
199  WSACleanup();
200  exit(2);
201  }
202  if (!Xcp_EnableSocketOption(XcpTl_Connection.boundSocket, SO_REUSEADDR)) {
203  XcpHw_ErrorMsg("XcpTl_Init:setsockopt(SO_REUSEADDR)", WSAGetLastError());
204  }
205 
206  setsockopt(XcpTl_Connection.boundSocket, IPPROTO_TCP, TCP_NODELAY, (const char *)&one, sizeof(one));
207 
208  recvOlap.opcode = IoRead;
209  recvOlap.wsabuf.buf = recvOlap.buf;
210  recvOlap.wsabuf.len = XCP_COMM_BUFLEN;
211 
212  sendOlap.opcode = IoWrite;
213  sendOlap.wsabuf.buf = sendOlap.buf;
214  sendOlap.wsabuf.len = XCP_COMM_BUFLEN;
215 
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);
219  } else {
220  recvOlap.opcode = IoAccept;
221  XcpTl_TriggerRecv(XCP_COMM_BUFLEN);
222  }
223 }
224 
225 void XcpTl_DeInit(void) {
226  size_t idx;
227 
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]);
235  }
236 
237  WSACleanup();
238 }
239 
240 void XcpTl_MainFunction(void) {}
241 
242 void *get_in_addr(struct sockaddr *sa) {
243  if (sa->sa_family == AF_INET) {
244  return &(((struct sockaddr_in *)sa)->sin_addr);
245  }
246  return &(((struct sockaddr_in6 *)sa)->sin6_addr);
247 }
248 
249 static DWORD WINAPI AcceptorThread(LPVOID lpParameter) {
250  int fromLen;
251  SOCKADDR_STORAGE From;
252  DWORD error;
253  int one = 1;
254 
255  XCP_UNREFERENCED_PARAMETER(lpParameter);
256 
257  XCP_FOREVER {
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) {
265  break;
266  } else {
267  XcpHw_ErrorMsg("AcceptorThread::accept()", error);
268  WSACleanup();
269  exit(1);
270  }
271  }
272  if (!Xcp_EnableSocketOption(XcpTl_Connection.connectedSocket, SO_REUSEADDR)) {
273  XcpHw_ErrorMsg("AcceptorThread::setsockopt(SO_REUSEADDR)", WSAGetLastError());
274  }
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);
280  }
281  }
282  ExitThread(0);
283 }
284 
285 void XcpTl_Send(uint8_t const *buf, uint16_t len) {
286  DWORD bytesWritten;
287  int addrLen;
288 
289  sendOlap.wsabuf.buf = (char *)buf;
290  sendOlap.wsabuf.len = len;
291  sendOlap.opcode = IoWrite;
292 
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());
300  }
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);
306  }
307  }
308 }
309 
310 void XcpTl_SaveConnection(void) {
311  CopyMemory(&XcpTl_Connection.connectionAddress, &XcpTl_Connection.currentAddress, sizeof(SOCKADDR_STORAGE));
312  XcpTl_Connection.xcpConnected = XCP_TRUE;
313 }
314 
315 void XcpTl_ReleaseConnection(void) { XcpTl_Connection.xcpConnected = XCP_FALSE; }
316 
317 bool XcpTl_VerifyConnection(void) {
318  bool res = memcmp(&XcpTl_Connection.connectionAddress, &XcpTl_Connection.currentAddress, sizeof(SOCKADDR_STORAGE)) == 0;
319  return res;
320 }
321 
322 void XcpTl_SetOptions(Xcp_OptionsType const *options) { CopyMemory(&Xcp_Options, options, sizeof(Xcp_OptionsType)); }
323 
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");
327 }
328 
332 /*
333 ** TODO: use SetFileCompletionNotificationModes() and SetFileIoOverlappedRange() on Vista and later.
334 */
335 
336 static DWORD WINAPI WorkerThread(LPVOID lpParameter) {
337  HANDLE hCompletionPort = (HANDLE)lpParameter;
338  DWORD numBytesReceived = 0;
339  ULONG_PTR CompletionKey;
340  PerIoData *iod = NULL;
341  OVERLAPPED *olap = NULL;
342  bool exitLoop = FALSE;
343  DWORD error;
344 
345  while (!exitLoop) {
346  if (GetQueuedCompletionStatus(hCompletionPort, &numBytesReceived, &CompletionKey, (LPOVERLAPPED *)&olap, INFINITE)) {
347  if ((numBytesReceived == 0) && (CompletionKey == 0)) {
348  exitLoop = XCP_TRUE;
349  } else {
350  iod = (PerIoData *)olap;
351  switch (iod->opcode) {
352  case IoAccept:
353  // printf("ACCEPT() %d %ld\n", numBytesReceived, CompletionKey);
354  // printf("\t");
355  // XcpUtl_Hexdump(recvOlap.wsabuf.buf, numBytesReceived);
356  // printf("\n");
357  // XcpTl_Connection.socketConnected = XCP_TRUE;
358  recvOlap.opcode = IoRead;
359  XcpTl_Feed(numBytesReceived);
360  XcpTl_TriggerRecv(XCP_COMM_BUFLEN);
361  break;
362  case IoRead:
363  // printf("READ() %d %ld [%d]\n", numBytesReceived, CompletionKey, recvOlap.opcode);
364  if (numBytesReceived == (DWORD)0) {
365  DBG_PRINT1("Client closed connection\n");
366  XcpTl_Connection.socketConnected = XCP_FALSE;
367  closesocket(XcpTl_Connection.connectedSocket);
368  Xcp_Disconnect();
369  }
370  XcpTl_Feed(numBytesReceived);
371  XcpTl_TriggerRecv(XCP_COMM_BUFLEN);
372  break;
373  case IoWrite:
374  // printf("WRITE() %d %ld\n", numBytesReceived, CompletionKey);
375 #if XCP_ENABLE_SLAVE_BLOCKMODE == XCP_ON
376  Xcp_UploadSingleBlock();
377 #endif /* XCP_ENABLE_SLAVE_BLOCKMODE */
378  break;
379  }
380  }
381  } else {
382  error = GetLastError();
383  if (olap == NULL) {
384  } else {
385  // Failed I/O operation.
386  // The function stores information in the variables pointed to by lpNumberOfBytes, lpCompletionKey.
387  }
388  XcpHw_ErrorMsg("WorkerThread::GetQueuedCompletionStatus()", error);
389  }
390  }
391  ExitThread(0);
392 }
393 
394 static HANDLE XcpTl_CreateIOCP(void) {
395  HANDLE handle;
396 
397  handle = CreateIoCompletionPort(INVALID_HANDLE_VALUE, NULL, (ULONG_PTR)0, 1);
398  if (handle == NULL) {
399  XcpHw_ErrorMsg("XcpTl_CreateIOCP::CreateIoCompletionPort()", WSAGetLastError());
400  exit(EXIT_FAILURE);
401  }
402  return handle;
403 }
404 
405 static bool XcpTl_RegisterIOCPHandle(HANDLE port, HANDLE object, ULONG_PTR key) {
406  HANDLE handle;
407 
408  handle = CreateIoCompletionPort(object, port, key, 0);
409  if (handle == NULL) {
410  XcpHw_ErrorMsg("XcpTl_RegisterIOCPHandle::CreateIoCompletionPort()", WSAGetLastError());
411  exit(EXIT_FAILURE);
412  }
413  return (bool)(handle == port);
414 }
415 
416 void XcpTl_PostQuitMessage(void) { PostQueuedCompletionStatus(XcpTl_Connection.iocp, 0, (ULONG_PTR)NULL, NULL); }
417 
418 static void XcpTl_TriggerRecv(DWORD numBytes) {
419  DWORD numReceived = (DWORD)0;
420  DWORD flags = (DWORD)0;
421  DWORD err = 0;
422  int addrLen;
423 
424  XCP_UNREFERENCED_PARAMETER(numBytes);
425 
426  SecureZeroMemory(&recvOlap.overlapped, sizeof(OVERLAPPED));
427 
428  if (XcpTl_Connection.socketType == SOCK_STREAM) {
429  if (XcpTl_Connection.socketConnected == XCP_FALSE) {
430  return;
431  }
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);
437  }
438  }
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());
447  }
448  }
449  }
450 }
451 
452 static void XcpTl_Feed(DWORD numBytesReceived) {
453  uint16_t dlc;
454 
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())) {
462  Xcp_CtoIn.len = dlc;
463  Xcp_CtoIn.data = (uint8_t *)(recvOlap.wsabuf.buf + XCP_TRANSPORT_LAYER_BUFFER_OFFSET);
464  Xcp_DispatchCommand(&Xcp_CtoIn);
465  }
466  if (numBytesReceived < 5) {
467  DBG_PRINT2("Error: frame to short: %ld\n", numBytesReceived);
468  } else {
469  }
470  }
471 }