networklayer_udp.c 8.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282
  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. #include <stdlib.h> // malloc, free
  6. #ifdef _WIN32
  7. #include <malloc.h>
  8. #include <winsock2.h>
  9. #include <sys/types.h>
  10. #include <Windows.h>
  11. #include <ws2tcpip.h>
  12. #define CLOSESOCKET(S) closesocket(S)
  13. #else
  14. #include <strings.h> //bzero
  15. #include <sys/select.h>
  16. #include <netinet/in.h>
  17. #include <sys/socketvar.h>
  18. #include <sys/ioctl.h>
  19. #include <unistd.h> // read, write, close
  20. #include <arpa/inet.h>
  21. #define CLOSESOCKET(S) close(S)
  22. #endif
  23. #include <stdio.h>
  24. #include <errno.h> // errno, EINTR
  25. #include <fcntl.h> // fcntl
  26. #include "networklayer_udp.h" // UA_MULTITHREADING is defined in here
  27. #ifdef UA_MULTITHREADING
  28. #include <urcu/uatomic.h>
  29. #endif
  30. #define MAXBACKLOG 100
  31. struct Networklayer_UDP;
  32. /* Forwarded to the server as a (UA_Connection) and used for callbacks back into
  33. the networklayer */
  34. typedef struct {
  35. UA_Connection connection;
  36. struct sockaddr from;
  37. socklen_t fromlen;
  38. struct NetworkLayerUDP *layer;
  39. } UDPConnection;
  40. typedef struct NetworkLayerUDP {
  41. UA_ConnectionConfig conf;
  42. fd_set fdset;
  43. UA_Int32 serversockfd;
  44. UA_UInt32 port;
  45. /* We remove the connection links only in the main thread. Attach
  46. to-be-deleted links with atomic operations */
  47. struct deleteLink {
  48. UA_Int32 sockfd;
  49. struct deleteLink *next;
  50. } *deleteLinkList;
  51. } NetworkLayerUDP;
  52. static UA_StatusCode setNonBlocking(int sockid) {
  53. #ifdef _WIN32
  54. u_long iMode = 1;
  55. if(ioctlsocket(sockid, FIONBIO, &iMode) != NO_ERROR)
  56. return UA_STATUSCODE_BADINTERNALERROR;
  57. #else
  58. int opts = fcntl(sockid,F_GETFL);
  59. if(opts < 0 || fcntl(sockid,F_SETFL,opts|O_NONBLOCK) < 0)
  60. return UA_STATUSCODE_BADINTERNALERROR;
  61. #endif
  62. return UA_STATUSCODE_GOOD;
  63. }
  64. // after every select, reset the set of sockets we want to listen on
  65. static void setFDSet(NetworkLayerUDP *layer) {
  66. FD_ZERO(&layer->fdset);
  67. FD_SET(layer->serversockfd, &layer->fdset);
  68. }
  69. // the callbacks are thread-safe if UA_MULTITHREADING is defined
  70. void closeConnectionUDP(UDPConnection *handle){
  71. free(handle);
  72. }
  73. void writeCallbackUDP(UDPConnection *handle, UA_ByteStringArray gather_buf);
  74. /** Accesses only the sockfd in the handle. Can be run from parallel threads. */
  75. void writeCallbackUDP(UDPConnection *handle, UA_ByteStringArray gather_buf) {
  76. UA_UInt32 total_len = 0, nWritten = 0;
  77. #ifdef _WIN32
  78. LPWSABUF buf = _alloca(gather_buf.stringsSize * sizeof(WSABUF));
  79. int result = 0;
  80. for(UA_UInt32 i = 0; i<gather_buf.stringsSize; i++) {
  81. buf[i].buf = gather_buf.strings[i].data;
  82. buf[i].len = gather_buf.strings[i].length;
  83. total_len += gather_buf.strings[i].length;
  84. }
  85. while(nWritten < total_len) {
  86. UA_UInt32 n = 0;
  87. do {
  88. result = WSASendto(handle->sockfd, buf, gather_buf.stringsSize ,
  89. (LPDWORD)&n, 0, NULL, NULL);
  90. //FIXME:
  91. if(result != 0)
  92. printf("Error WSASend, code: %d \n", WSAGetLastError());
  93. } while(errno == EINTR);
  94. nWritten += n;
  95. }
  96. #else
  97. struct iovec iov[gather_buf.stringsSize];
  98. for(UA_UInt32 i=0;i<gather_buf.stringsSize;i++) {
  99. iov[i] = (struct iovec) {.iov_base = gather_buf.strings[i].data,
  100. .iov_len = gather_buf.strings[i].length};
  101. total_len += gather_buf.strings[i].length;
  102. }
  103. struct sockaddr_in *sin = UA_NULL;
  104. if (handle->from.sa_family == AF_INET)
  105. {
  106. sin = (struct sockaddr_in *) &(handle->from);
  107. }else{
  108. //FIXME:
  109. return;
  110. }
  111. struct msghdr message = {.msg_name = sin, .msg_namelen = handle->fromlen, .msg_iov = iov,
  112. .msg_iovlen = gather_buf.stringsSize, .msg_control = NULL,
  113. .msg_controllen = 0, .msg_flags = 0};
  114. while (nWritten < total_len) {
  115. UA_Int32 n = 0;
  116. do {
  117. n = sendmsg(handle->layer->serversockfd, &message, 0);
  118. if(n==-1L){
  119. printf("ERROR:%i\n", errno);
  120. }
  121. } while (n == -1L && errno == EINTR);
  122. nWritten += n;
  123. }
  124. #endif
  125. }
  126. UA_StatusCode NetworkLayerUDP_start(NetworkLayerUDP *layer) {
  127. #ifdef _WIN32
  128. WORD wVersionRequested;
  129. WSADATA wsaData;
  130. wVersionRequested = MAKEWORD(2, 2);
  131. WSAStartup(wVersionRequested, &wsaData);
  132. if((layer->serversockfd = socket(PF_INET, SOCK_DGRAM,0)) == INVALID_SOCKET) {
  133. printf("ERROR opening socket, code: %d\n", WSAGetLastError());
  134. return UA_STATUSCODE_BADINTERNALERROR;
  135. }
  136. #else
  137. if((layer->serversockfd = socket(PF_INET, SOCK_DGRAM, 0)) < 0) {
  138. perror("ERROR opening socket");
  139. return UA_STATUSCODE_BADINTERNALERROR;
  140. }
  141. #endif
  142. const struct sockaddr_in serv_addr = {
  143. .sin_family = AF_INET, .sin_addr.s_addr = INADDR_ANY,
  144. .sin_port = htons(layer->port), .sin_zero = {0}};
  145. int optval = 1;
  146. if(setsockopt(layer->serversockfd, SOL_SOCKET,
  147. SO_REUSEADDR, (const char *)&optval,
  148. sizeof(optval)) == -1) {
  149. perror("setsockopt");
  150. CLOSESOCKET(layer->serversockfd);
  151. return UA_STATUSCODE_BADINTERNALERROR;
  152. }
  153. if(bind(layer->serversockfd, (const struct sockaddr *)&serv_addr,
  154. sizeof(serv_addr)) < 0) {
  155. perror("binding");
  156. CLOSESOCKET(layer->serversockfd);
  157. return UA_STATUSCODE_BADINTERNALERROR;
  158. }
  159. setNonBlocking(layer->serversockfd);
  160. printf("Listening for UDP connections on %s:%d\n",
  161. inet_ntoa(serv_addr.sin_addr),
  162. ntohs(serv_addr.sin_port));
  163. return UA_STATUSCODE_GOOD;
  164. }
  165. UA_Int32 NetworkLayerUDP_getWork(NetworkLayerUDP *layer, UA_WorkItem **workItems,
  166. UA_UInt16 timeout) {
  167. UA_WorkItem *items = UA_NULL;
  168. UA_Int32 itemsCount = 0;
  169. setFDSet(layer);
  170. struct timeval tmptv = {0, timeout};
  171. UA_Int32 resultsize = select(layer->serversockfd+1, &layer->fdset, NULL, NULL, &tmptv);
  172. if(resultsize < 0) {
  173. *workItems = items;
  174. return itemsCount;
  175. }
  176. items = realloc(items, sizeof(UA_WorkItem)*(itemsCount+resultsize));
  177. // read from established sockets
  178. UA_Int32 j = itemsCount;
  179. UA_ByteString buf = { -1, NULL};
  180. if(!buf.data) {
  181. buf.data = malloc(sizeof(UA_Byte) * layer->conf.recvBufferSize);
  182. if(!buf.data){
  183. //TODO:
  184. }
  185. }
  186. struct sockaddr sender;
  187. socklen_t sendsize = sizeof(sender);
  188. bzero(&sender, sizeof(sender));
  189. #ifdef _WIN32
  190. buf.length = recvfrom(layer->conLinks[i].sockfd, (char *)buf.data,
  191. layer->conf.recvBufferSize, 0);
  192. //todo: fixme
  193. #else
  194. buf.length = recvfrom(layer->serversockfd, buf.data, layer->conf.recvBufferSize, 0, &sender, &sendsize);
  195. #endif
  196. if (buf.length <= 0) {
  197. } else {
  198. UDPConnection *c = malloc(sizeof(UDPConnection));
  199. if(!c)
  200. return UA_STATUSCODE_BADINTERNALERROR;
  201. c->layer = layer;
  202. c->from = sender;
  203. c->fromlen = sendsize;
  204. c->connection.state = UA_CONNECTION_OPENING;
  205. c->connection.localConf = layer->conf;
  206. c->connection.channel = UA_NULL;
  207. c->connection.close = (void (*)(void*))closeConnectionUDP;
  208. c->connection.write = (void (*)(void*, UA_ByteStringArray))writeCallbackUDP;
  209. items[j].type = UA_WORKITEMTYPE_BINARYNETWORKMESSAGE;
  210. items[j].work.binaryNetworkMessage.message = buf;
  211. items[j].work.binaryNetworkMessage.connection = (UA_Connection*)c;
  212. buf.data = NULL;
  213. j++;
  214. }
  215. if(buf.data)
  216. free(buf.data);
  217. if(j == 0) {
  218. free(items);
  219. *workItems = NULL;
  220. } else
  221. *workItems = items;
  222. return j;
  223. }
  224. UA_Int32 NetworkLayerUDP_stop(NetworkLayerUDP * layer, UA_WorkItem **workItems) {
  225. CLOSESOCKET(layer->serversockfd);
  226. return 0;
  227. }
  228. void NetworkLayerUDP_delete(NetworkLayerUDP *layer) {
  229. free(layer);
  230. }
  231. UA_NetworkLayer NetworkLayerUDP_new(UA_ConnectionConfig conf, UA_UInt32 port) {
  232. NetworkLayerUDP *udplayer = malloc(sizeof(NetworkLayerUDP));
  233. udplayer->conf = conf;
  234. udplayer->port = port;
  235. udplayer->deleteLinkList = UA_NULL;
  236. UA_NetworkLayer nl;
  237. nl.nlHandle = udplayer;
  238. nl.start = (UA_StatusCode (*)(void*))NetworkLayerUDP_start;
  239. nl.getWork = (UA_Int32 (*)(void*, UA_WorkItem**, UA_UInt16)) NetworkLayerUDP_getWork;
  240. nl.stop = (UA_Int32 (*)(void*, UA_WorkItem**)) NetworkLayerUDP_stop;
  241. nl.delete = (void (*)(void*))NetworkLayerUDP_delete;
  242. return nl;
  243. }