networklayer_tcp.c 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350
  1. /*
  2. * This work is licensed under a Creative Commons CCZero 1.0 Universal License.
  3. * See http://creativecommons.org/publicdomain/zero/1.0/ for more information.
  4. */
  5. #ifdef WIN32
  6. #include <malloc.h>
  7. #include <winsock2.h>
  8. #include <sys/types.h>
  9. #include <Windows.h>
  10. #include <ws2tcpip.h>
  11. #define CLOSESOCKET(S) closesocket(S)
  12. #define IOCTLSOCKET ioctlsocket
  13. #else
  14. #include <sys/select.h>
  15. #include <sys/socket.h>
  16. #include <netinet/in.h>
  17. #include <sys/socketvar.h>
  18. #include <sys/ioctl.h>
  19. #include <unistd.h> // read, write, close
  20. #define CLOSESOCKET(S) close(S)
  21. #define IOCTLSOCKET ioctl
  22. #endif /* WIN32 */
  23. #include <stdlib.h> // exit
  24. #include <stdio.h>
  25. #include <errno.h> // errno, EINTR
  26. #include <memory.h> // memset
  27. #include <fcntl.h> // fcntl
  28. #include "ua_securechannel.h"
  29. #include "networklayer_tcp.h"
  30. typedef struct TCPConnection {
  31. UA_Int32 sockfd;
  32. UA_Connection connection;
  33. } TCPConnection;
  34. struct NetworklayerTCP {
  35. UA_ConnectionConfig localConf;
  36. UA_UInt32 port;
  37. fd_set fdset;
  38. UA_Int32 serversockfd;
  39. UA_Int32 highestfd;
  40. UA_UInt32 connectionsSize;
  41. TCPConnection *connections;
  42. };
  43. /** This structure is stored in the UA_Connection for callbacks into the
  44. network layer. */
  45. typedef struct TCPConnectionHandle {
  46. UA_Int32 sockfd;
  47. NetworklayerTCP *layer;
  48. } TCPConnectionHandle;
  49. NetworklayerTCP *NetworklayerTCP_new(UA_ConnectionConfig localConf, UA_UInt32 port) {
  50. NetworklayerTCP *newlayer = malloc(sizeof(NetworklayerTCP));
  51. if(newlayer == NULL)
  52. return NULL;
  53. newlayer->localConf = localConf;
  54. newlayer->port = port;
  55. newlayer->connectionsSize = 0;
  56. newlayer->connections = NULL;
  57. return newlayer;
  58. }
  59. void NetworklayerTCP_delete(NetworklayerTCP *layer) {
  60. for(UA_UInt32 index = 0;index < layer->connectionsSize;index++) {
  61. shutdown(layer->connections[index].sockfd, 2);
  62. if(layer->connections[index].connection.channel)
  63. layer->connections[index].connection.channel->connection = NULL;
  64. UA_Connection_deleteMembers(&layer->connections[index].connection);
  65. CLOSESOCKET(layer->connections[index].sockfd);
  66. }
  67. free(layer->connections);
  68. free(layer);
  69. }
  70. void closeCallback(TCPConnectionHandle *handle);
  71. void writeCallback(TCPConnectionHandle *handle, UA_ByteStringArray gather_buf);
  72. static UA_StatusCode NetworklayerTCP_add(NetworklayerTCP *layer, UA_Int32 newsockfd) {
  73. layer->connectionsSize++;
  74. layer->connections = realloc(layer->connections, sizeof(TCPConnection) * layer->connectionsSize);
  75. TCPConnection *newconnection = &layer->connections[layer->connectionsSize-1];
  76. newconnection->sockfd = newsockfd;
  77. struct TCPConnectionHandle *callbackhandle;
  78. if(!(callbackhandle = malloc(sizeof(struct TCPConnectionHandle))))
  79. return UA_STATUSCODE_BADOUTOFMEMORY;
  80. callbackhandle->layer = layer;
  81. callbackhandle->sockfd = newsockfd;
  82. UA_Connection_init(&newconnection->connection, layer->localConf, callbackhandle,
  83. (UA_Connection_closeCallback)closeCallback, (UA_Connection_writeCallback)writeCallback);
  84. return UA_STATUSCODE_GOOD;
  85. }
  86. // copy the array of connections, but _loose_ one. This does not close the
  87. // actual socket.
  88. static UA_StatusCode NetworklayerTCP_remove(NetworklayerTCP *layer, UA_Int32 sockfd) {
  89. UA_UInt32 index;
  90. for(index = 0;index < layer->connectionsSize;index++) {
  91. if(layer->connections[index].sockfd == sockfd)
  92. break;
  93. }
  94. if(index == layer->connectionsSize)
  95. return UA_STATUSCODE_BADINTERNALERROR;
  96. if(layer->connections[index].connection.channel)
  97. layer->connections[index].connection.channel->connection = NULL;
  98. UA_Connection_deleteMembers(&layer->connections[index].connection);
  99. layer->connectionsSize--;
  100. TCPConnection *newconnections;
  101. newconnections = malloc(sizeof(TCPConnection) * layer->connectionsSize);
  102. memcpy(newconnections, layer->connections, sizeof(TCPConnection) * index);
  103. memcpy(&newconnections[index], &layer->connections[index+1],
  104. sizeof(TCPConnection) * (layer->connectionsSize - index));
  105. free(layer->connections);
  106. layer->connections = newconnections;
  107. return UA_STATUSCODE_GOOD;
  108. }
  109. /** Callback function */
  110. void closeCallback(TCPConnectionHandle *handle) {
  111. shutdown(handle->sockfd,2);
  112. CLOSESOCKET(handle->sockfd);
  113. NetworklayerTCP_remove(handle->layer, handle->sockfd);
  114. }
  115. /** Callback function */
  116. void writeCallback(TCPConnectionHandle *handle, UA_ByteStringArray gather_buf) {
  117. UA_UInt32 total_len = 0;
  118. UA_UInt32 nWritten = 0;
  119. #ifdef WIN32
  120. LPWSABUF buf = _alloca(gather_buf.stringsSize * sizeof(WSABUF));
  121. int result = 0;
  122. for(UA_UInt32 i = 0; i<gather_buf.stringsSize; i++) {
  123. buf[i].buf = gather_buf.strings[i].data;
  124. buf[i].len = gather_buf.strings[i].length;
  125. total_len += gather_buf.strings[i].length;
  126. }
  127. while (nWritten < total_len) {
  128. UA_UInt32 n=0;
  129. do {
  130. result = WSASend(handle->sockfd, buf, gather_buf.stringsSize , (LPDWORD)&n, 0, NULL, NULL);
  131. if(result != 0)
  132. printf("NL_TCP_Writer - Error WSASend, code: %d \n", WSAGetLastError());
  133. } while (errno == EINTR);
  134. nWritten += n;
  135. }
  136. #else
  137. struct iovec iov[gather_buf.stringsSize];
  138. for(UA_UInt32 i=0;i<gather_buf.stringsSize;i++) {
  139. iov[i].iov_base = gather_buf.strings[i].data;
  140. iov[i].iov_len = gather_buf.strings[i].length;
  141. total_len += gather_buf.strings[i].length;
  142. }
  143. struct msghdr message = {.msg_name = NULL, .msg_namelen = 0, .msg_iov = iov,
  144. .msg_iovlen = gather_buf.stringsSize, .msg_control = NULL,
  145. .msg_controllen = 0, .msg_flags = 0};
  146. while (nWritten < total_len) {
  147. UA_Int32 n = 0;
  148. do {
  149. n = sendmsg(handle->sockfd, &message, 0);
  150. } while (n == -1L && errno == EINTR);
  151. if (n >= 0) {
  152. // TODO: handle incompletely send messages
  153. /* nWritten += n; */
  154. break;
  155. } else {
  156. // TODO: error handling
  157. break;
  158. }
  159. }
  160. #endif
  161. for(UA_UInt32 i=0;i<gather_buf.stringsSize;i++)
  162. free(gather_buf.strings[i].data);
  163. }
  164. static UA_StatusCode setNonBlocking(int sockid) {
  165. #ifdef WIN32
  166. u_long iMode = 1;
  167. int opts = IOCTLSOCKET(sockid, FIONBIO, &iMode);
  168. if (opts != NO_ERROR){
  169. printf("ioctlsocket failed with error: %ld\n", opts);
  170. return UA_STATUSCODE_BADINTERNALERROR;
  171. }
  172. return UA_STATUSCODE_GOOD;
  173. #else
  174. int opts = fcntl(sockid,F_GETFL);
  175. if (opts < 0) {
  176. perror("fcntl(F_GETFL)");
  177. return UA_STATUSCODE_BADINTERNALERROR;
  178. }
  179. opts = (opts | O_NONBLOCK);
  180. if (fcntl(sockid,F_SETFL,opts) < 0) {
  181. perror("fcntl(F_SETFL)");
  182. return UA_STATUSCODE_BADINTERNALERROR;
  183. }
  184. return UA_STATUSCODE_GOOD;
  185. #endif
  186. }
  187. void readConnection(NetworklayerTCP *layer, UA_Server *server, TCPConnection *entry) {
  188. UA_ByteString readBuffer;
  189. readBuffer.data = malloc(layer->localConf.recvBufferSize);
  190. #ifdef WIN32
  191. readBuffer.length = recv(entry->sockfd, (char *)readBuffer.data,
  192. layer->localConf.recvBufferSize, 0);
  193. #else
  194. readBuffer.length = read(entry->sockfd, readBuffer.data, layer->localConf.recvBufferSize);
  195. #endif
  196. if (errno != 0) {
  197. shutdown(entry->sockfd,2);
  198. CLOSESOCKET(entry->sockfd);
  199. NetworklayerTCP_remove(layer, entry->sockfd);
  200. } else {
  201. if(readBuffer.length>0){ //data received to process?
  202. UA_Server_processBinaryMessage(server, &entry->connection, &readBuffer);
  203. }
  204. }
  205. readBuffer.length = layer->localConf.recvBufferSize; // because this was malloc'd. Length=0 would lead to errors.
  206. UA_ByteString_deleteMembers(&readBuffer);
  207. }
  208. void worksocks(NetworklayerTCP *layer, UA_Server *server, UA_UInt32 workamount) {
  209. // accept new connections
  210. if(FD_ISSET(layer->serversockfd,&layer->fdset)) {
  211. struct sockaddr_in cli_addr;
  212. socklen_t cli_len = sizeof(cli_addr);
  213. int newsockfd = accept(layer->serversockfd, (struct sockaddr *) &cli_addr, &cli_len);
  214. if (newsockfd >= 0) {
  215. setNonBlocking(newsockfd);
  216. NetworklayerTCP_add(layer, newsockfd);
  217. }
  218. workamount--;
  219. }
  220. // read from established sockets
  221. for(UA_UInt32 i=0;i<layer->connectionsSize && workamount > 0;i++) {
  222. if(FD_ISSET(layer->connections[i].sockfd, &layer->fdset)) {
  223. int n = 0;
  224. IOCTLSOCKET(layer->connections[i].sockfd, FIONREAD, &n);
  225. if(n==0){ /* the socket has been closed by the client - remove the socket from the socket list */
  226. layer->connections[i].connection.close(layer->connections[i].connection.callbackHandle);
  227. }else{
  228. readConnection(layer, server, &layer->connections[i]);
  229. }
  230. workamount--;
  231. }
  232. }
  233. }
  234. // after every select, reset the set of sockets we want to listen on
  235. void setFDSet(NetworklayerTCP *layer) {
  236. FD_ZERO(&layer->fdset);
  237. FD_SET(layer->serversockfd, &layer->fdset);
  238. layer->highestfd = layer->serversockfd;
  239. for(UA_UInt32 i=0;i<layer->connectionsSize;i++) {
  240. FD_SET(layer->connections[i].sockfd, &layer->fdset);
  241. if(layer->connections[i].sockfd > layer->highestfd)
  242. layer->highestfd = layer->connections[i].sockfd;
  243. }
  244. layer->highestfd++;
  245. }
  246. UA_StatusCode NetworkLayerTCP_run(NetworklayerTCP *layer, UA_Server *server, struct timeval tv,
  247. void(*worker)(UA_Server*), UA_Boolean *running) {
  248. #ifdef WIN32
  249. WORD wVersionRequested;
  250. WSADATA wsaData;
  251. wVersionRequested = MAKEWORD(2, 2);
  252. WSAStartup(wVersionRequested, &wsaData);
  253. if((layer->serversockfd = socket(PF_INET, SOCK_STREAM,0)) == INVALID_SOCKET) {
  254. printf("ERROR opening socket, code: %d\n", WSAGetLastError());
  255. return UA_STATUSCODE_BADINTERNALERROR;
  256. }
  257. #else
  258. if((layer->serversockfd = socket(PF_INET, SOCK_STREAM, 0)) < 0) {
  259. perror("ERROR opening socket");
  260. return UA_STATUSCODE_BADINTERNALERROR;
  261. }
  262. #endif
  263. struct sockaddr_in serv_addr;
  264. memset((void *)&serv_addr, sizeof(serv_addr), 1);
  265. serv_addr.sin_family = AF_INET;
  266. serv_addr.sin_addr.s_addr = INADDR_ANY;
  267. serv_addr.sin_port = htons(layer->port);
  268. int optval = 1;
  269. if(setsockopt(layer->serversockfd, SOL_SOCKET, SO_REUSEADDR, (const char *)&optval, sizeof(optval)) == -1) {
  270. perror("setsockopt");
  271. CLOSESOCKET(layer->serversockfd);
  272. return UA_STATUSCODE_BADINTERNALERROR;
  273. }
  274. if(bind(layer->serversockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) {
  275. perror("binding");
  276. CLOSESOCKET(layer->serversockfd);
  277. return UA_STATUSCODE_BADINTERNALERROR;
  278. }
  279. #define MAXBACKLOG 10
  280. setNonBlocking(layer->serversockfd);
  281. listen(layer->serversockfd, MAXBACKLOG);
  282. while (*running) {
  283. setFDSet(layer);
  284. struct timeval tmptv = tv;
  285. UA_Int32 resultsize = select(layer->highestfd, &layer->fdset, NULL, NULL, &tmptv);
  286. if (resultsize <= 0) {
  287. #ifdef WIN32
  288. UA_Int32 err = (resultsize == SOCKET_ERROR) ? WSAGetLastError() : 0;
  289. switch (err) {
  290. case WSANOTINITIALISED:
  291. case WSAEFAULT:
  292. case WSAENETDOWN:
  293. case WSAEINVAL:
  294. case WSAEINTR:
  295. case WSAEINPROGRESS:
  296. case WSAENOTSOCK:
  297. #else
  298. UA_Int32 err = errno;
  299. switch (err) {
  300. case EBADF:
  301. case EINTR:
  302. case EINVAL:
  303. #endif
  304. // todo: handle errors
  305. printf("UA_Stack_msgLoop - result=%d, errno={%d,%s}\n", resultsize, errno, strerror(errno));
  306. break;
  307. default:
  308. // timeout
  309. worker(server);
  310. }
  311. } else { // activity on listener or client socket
  312. worksocks(layer, server, resultsize);
  313. worker(server);
  314. }
  315. }
  316. #ifdef WIN32
  317. WSACleanup();
  318. #endif
  319. return UA_STATUSCODE_GOOD;
  320. }