networklayer_udp.c 8.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294
  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 ServerNetworklayerUDP;
  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 ServerNetworkLayerUDP *layer;
  39. } UDPConnection;
  40. typedef struct ServerNetworkLayerUDP {
  41. UA_ConnectionConfig conf;
  42. fd_set fdset;
  43. #ifdef _WIN32
  44. UA_UInt32 serversockfd;
  45. #else
  46. UA_Int32 serversockfd;
  47. #endif
  48. UA_UInt32 port;
  49. } ServerNetworkLayerUDP;
  50. static UA_StatusCode setNonBlocking(int sockid) {
  51. #ifdef _WIN32
  52. u_long iMode = 1;
  53. if(ioctlsocket(sockid, FIONBIO, &iMode) != NO_ERROR)
  54. return UA_STATUSCODE_BADINTERNALERROR;
  55. #else
  56. int opts = fcntl(sockid,F_GETFL);
  57. if(opts < 0 || fcntl(sockid,F_SETFL,opts|O_NONBLOCK) < 0)
  58. return UA_STATUSCODE_BADINTERNALERROR;
  59. #endif
  60. return UA_STATUSCODE_GOOD;
  61. }
  62. static void setFDSet(ServerNetworkLayerUDP *layer) {
  63. FD_ZERO(&layer->fdset);
  64. FD_SET(layer->serversockfd, &layer->fdset);
  65. }
  66. // the callbacks are thread-safe if UA_MULTITHREADING is defined
  67. static void closeConnectionUDP(UDPConnection *handle) {
  68. free(handle);
  69. }
  70. void writeCallbackUDP(UDPConnection *handle, UA_ByteStringArray gather_buf);
  71. /** Accesses only the sockfd in the handle. Can be run from parallel threads. */
  72. void writeCallbackUDP(UDPConnection *handle, UA_ByteStringArray gather_buf) {
  73. UA_UInt32 total_len = 0, nWritten = 0;
  74. #ifdef _WIN32
  75. /*
  76. LPWSABUF buf = _alloca(gather_buf.stringsSize * sizeof(WSABUF));
  77. int result = 0;
  78. for(UA_UInt32 i = 0; i<gather_buf.stringsSize; i++) {
  79. buf[i].buf = (char*)gather_buf.strings[i].data;
  80. buf[i].len = gather_buf.strings[i].length;
  81. total_len += gather_buf.strings[i].length;
  82. }
  83. while(nWritten < total_len) {
  84. UA_UInt32 n = 0;
  85. do {
  86. result = WSASendto(handle->layer->serversockfd, buf, gather_buf.stringsSize ,
  87. (LPDWORD)&n, 0,
  88. handle->from, handle->fromlen,
  89. 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. */
  97. #error fixme: udp not yet implemented for windows
  98. #else
  99. struct iovec iov[gather_buf.stringsSize];
  100. for(UA_UInt32 i=0;i<gather_buf.stringsSize;i++) {
  101. iov[i] = (struct iovec) {.iov_base = gather_buf.strings[i].data,
  102. .iov_len = gather_buf.strings[i].length};
  103. total_len += gather_buf.strings[i].length;
  104. }
  105. struct sockaddr_in *sin = NULL;
  106. if (handle->from.sa_family == AF_INET) {
  107. #if defined(__GNUC__) || defined(__clang__)
  108. #pragma GCC diagnostic push
  109. #pragma GCC diagnostic ignored "-Wcast-align"
  110. #endif
  111. sin = (struct sockaddr_in *) &(handle->from);
  112. #if defined(__GNUC__) || defined(__clang__)
  113. #pragma GCC diagnostic pop
  114. #endif
  115. } else {
  116. //FIXME:
  117. return;
  118. }
  119. struct msghdr message = {.msg_name = sin, .msg_namelen = handle->fromlen, .msg_iov = iov,
  120. .msg_iovlen = gather_buf.stringsSize, .msg_control = NULL,
  121. .msg_controllen = 0, .msg_flags = 0};
  122. while (nWritten < total_len) {
  123. UA_Int32 n = 0;
  124. do {
  125. n = sendmsg(handle->layer->serversockfd, &message, 0);
  126. if(n==-1L){
  127. printf("ERROR:%i\n", errno);
  128. }
  129. } while (n == -1L && errno == EINTR);
  130. nWritten += n;
  131. }
  132. #endif
  133. }
  134. static UA_StatusCode ServerNetworkLayerUDP_start(ServerNetworkLayerUDP *layer, UA_Logger *logger) {
  135. #ifdef _WIN32
  136. WORD wVersionRequested;
  137. WSADATA wsaData;
  138. wVersionRequested = MAKEWORD(2, 2);
  139. WSAStartup(wVersionRequested, &wsaData);
  140. if((layer->serversockfd = socket(PF_INET, SOCK_DGRAM,0)) == INVALID_SOCKET) {
  141. printf("ERROR opening socket, code: %d\n", WSAGetLastError());
  142. return UA_STATUSCODE_BADINTERNALERROR;
  143. }
  144. #else
  145. if((layer->serversockfd = socket(PF_INET, SOCK_DGRAM, 0)) < 0) {
  146. perror("ERROR opening socket");
  147. return UA_STATUSCODE_BADINTERNALERROR;
  148. }
  149. #endif
  150. const struct sockaddr_in serv_addr = {
  151. .sin_family = AF_INET, .sin_addr.s_addr = INADDR_ANY,
  152. .sin_port = htons(layer->port), .sin_zero = {0}};
  153. int optval = 1;
  154. if(setsockopt(layer->serversockfd, SOL_SOCKET,
  155. SO_REUSEADDR, (const char *)&optval,
  156. sizeof(optval)) == -1) {
  157. perror("setsockopt");
  158. CLOSESOCKET(layer->serversockfd);
  159. return UA_STATUSCODE_BADINTERNALERROR;
  160. }
  161. if(bind(layer->serversockfd, (const struct sockaddr *)&serv_addr,
  162. sizeof(serv_addr)) < 0) {
  163. perror("binding");
  164. CLOSESOCKET(layer->serversockfd);
  165. return UA_STATUSCODE_BADINTERNALERROR;
  166. }
  167. setNonBlocking(layer->serversockfd);
  168. char msg[256];
  169. sprintf(msg, "Listening for UDP connections on %s:%d",
  170. inet_ntoa(serv_addr.sin_addr),
  171. ntohs(serv_addr.sin_port));
  172. UA_LOG_INFO((*logger), UA_LOGGERCATEGORY_SERVER, msg);
  173. return UA_STATUSCODE_GOOD;
  174. }
  175. static UA_Int32 ServerNetworkLayerUDP_getWork(ServerNetworkLayerUDP *layer, UA_WorkItem **workItems,
  176. UA_UInt16 timeout) {
  177. UA_WorkItem *items = NULL;
  178. setFDSet(layer);
  179. struct timeval tmptv = {0, timeout};
  180. UA_Int32 resultsize = select(layer->serversockfd+1, &layer->fdset, NULL, NULL, &tmptv);
  181. if(resultsize <= 0 || !FD_ISSET(layer->serversockfd, &layer->fdset)) {
  182. *workItems = items;
  183. return 0;
  184. }
  185. items = malloc(sizeof(UA_WorkItem)*(resultsize));
  186. // read from established sockets
  187. UA_Int32 j = 0;
  188. UA_ByteString buf = { -1, NULL};
  189. if(!buf.data) {
  190. buf.data = malloc(sizeof(UA_Byte) * layer->conf.recvBufferSize);
  191. if(!buf.data){
  192. //TODO:
  193. printf("malloc failed");
  194. }
  195. }
  196. struct sockaddr sender;
  197. socklen_t sendsize = sizeof(sender);
  198. bzero(&sender, sizeof(sender));
  199. #ifdef _WIN32
  200. buf.length = recvfrom(layer->conLinks[i].sockfd, (char *)buf.data,
  201. layer->conf.recvBufferSize, 0);
  202. //todo: fixme
  203. #else
  204. buf.length = recvfrom(layer->serversockfd, buf.data, layer->conf.recvBufferSize, 0, &sender, &sendsize);
  205. #endif
  206. if (buf.length <= 0) {
  207. } else {
  208. UDPConnection *c = malloc(sizeof(UDPConnection));
  209. if(!c)
  210. return UA_STATUSCODE_BADINTERNALERROR;
  211. c->layer = layer;
  212. c->from = sender;
  213. c->fromlen = sendsize;
  214. c->connection.state = UA_CONNECTION_OPENING;
  215. c->connection.localConf = layer->conf;
  216. c->connection.channel = NULL;
  217. c->connection.close = (void (*)(void*))closeConnectionUDP;
  218. c->connection.write = (void (*)(void*, UA_ByteStringArray))writeCallbackUDP;
  219. items[j].type = UA_WORKITEMTYPE_BINARYNETWORKMESSAGE;
  220. items[j].work.binaryNetworkMessage.message = buf;
  221. items[j].work.binaryNetworkMessage.connection = (UA_Connection*)c;
  222. buf.data = NULL;
  223. j++;
  224. }
  225. if(buf.data)
  226. free(buf.data);
  227. if(j == 0) {
  228. free(items);
  229. *workItems = NULL;
  230. } else
  231. *workItems = items;
  232. return j;
  233. }
  234. static UA_Int32 ServerNetworkLayerUDP_stop(ServerNetworkLayerUDP * layer, UA_WorkItem **workItems) {
  235. CLOSESOCKET(layer->serversockfd);
  236. return 0;
  237. }
  238. static void ServerNetworkLayerUDP_delete(ServerNetworkLayerUDP *layer) {
  239. free(layer);
  240. }
  241. UA_ServerNetworkLayer ServerNetworkLayerUDP_new(UA_ConnectionConfig conf, UA_UInt32 port){
  242. ServerNetworkLayerUDP *udplayer = malloc(sizeof(ServerNetworkLayerUDP));
  243. udplayer->conf = conf;
  244. udplayer->port = port;
  245. UA_ServerNetworkLayer nl;
  246. nl.nlHandle = udplayer;
  247. nl.start = (UA_StatusCode (*)(void*))ServerNetworkLayerUDP_start;
  248. nl.getWork = (UA_Int32 (*)(void*, UA_WorkItem**, UA_UInt16)) ServerNetworkLayerUDP_getWork;
  249. nl.stop = (UA_Int32 (*)(void*, UA_WorkItem**)) ServerNetworkLayerUDP_stop;
  250. nl.free = (void (*)(void*))ServerNetworkLayerUDP_delete;
  251. nl.discoveryUrl = NULL;
  252. return nl;
  253. }