ua_network_udp.c 8.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260
  1. /* This work is licensed under a Creative Commons CCZero 1.0 Universal License.
  2. * See http://creativecommons.org/publicdomain/zero/1.0/ for more information.
  3. *
  4. * Copyright 2016-2017 (c) Julius Pfrommer, Fraunhofer IOSB
  5. * Copyright 2016-2017 (c) Stefan Profanter, fortiss GmbH
  6. */
  7. #include "ua_network_udp.h"
  8. #include <stdio.h>
  9. #include <string.h> // memset
  10. /* with a space so amalgamation does not remove the includes */
  11. # include <errno.h> // errno, EINTR
  12. # include <fcntl.h> // fcntl
  13. # include <strings.h> //bzero
  14. #if defined(UA_FREERTOS)
  15. # include <lwip/udp.h>
  16. # include <lwip/tcpip.h>
  17. #else
  18. # ifndef _WRS_KERNEL
  19. # include <sys/select.h>
  20. # else
  21. # include <selectLib.h>
  22. # endif
  23. # include <netinet/in.h>
  24. # include <netinet/tcp.h>
  25. # include <sys/socketvar.h>
  26. # include <sys/ioctl.h>
  27. # include <unistd.h> // read, write, close
  28. # include <arpa/inet.h>
  29. #endif
  30. #ifdef __QNX__
  31. #include <sys/socket.h>
  32. #endif
  33. #if defined(__unix__) || (defined(__APPLE__) && defined(__MACH__))
  34. # include <sys/param.h>
  35. # if defined(BSD)
  36. # include<sys/socket.h>
  37. # endif
  38. #endif
  39. # define CLOSESOCKET(S) close(S)
  40. #define MAXBACKLOG 100
  41. #ifdef _WIN32
  42. # error udp not yet implemented for windows
  43. #endif
  44. /*****************************/
  45. /* Generic Buffer Management */
  46. /*****************************/
  47. static UA_StatusCode GetMallocedBuffer(UA_Connection *connection, size_t length, UA_ByteString *buf) {
  48. if(length > connection->remoteConf.recvBufferSize)
  49. return UA_STATUSCODE_BADCOMMUNICATIONERROR;
  50. return UA_ByteString_allocBuffer(buf, connection->remoteConf.recvBufferSize);
  51. }
  52. static void ReleaseMallocedBuffer(UA_Connection *connection, UA_ByteString *buf) {
  53. UA_ByteString_deleteMembers(buf);
  54. }
  55. /*********************/
  56. /* UDP Network Layer */
  57. /*********************/
  58. /* Forwarded to the server as a (UA_Connection) and used for callbacks back into
  59. the networklayer */
  60. typedef struct {
  61. UA_Connection connection;
  62. struct sockaddr from;
  63. socklen_t fromlen;
  64. } UDPConnection;
  65. typedef struct {
  66. UA_ConnectionConfig conf;
  67. UA_UInt16 port;
  68. fd_set fdset;
  69. UA_Int32 serversockfd;
  70. UA_Logger logger; // Set during start
  71. } ServerNetworkLayerUDP;
  72. /** Accesses only the sockfd in the handle. Can be run from parallel threads. */
  73. static UA_StatusCode sendUDP(UA_Connection *connection, UA_ByteString *buf) {
  74. UDPConnection *udpc = (UDPConnection*)connection;
  75. ServerNetworkLayerUDP *layer = (ServerNetworkLayerUDP*)connection->handle;
  76. long nWritten = 0;
  77. struct sockaddr_in *sin = NULL;
  78. if (udpc->from.sa_family == AF_INET) {
  79. #if ((__GNUC__ == 4 && __GNUC_MINOR__ >= 6) || __GNUC__ > 4 || defined(__clang__))
  80. #pragma GCC diagnostic push
  81. #pragma GCC diagnostic ignored "-Wcast-align"
  82. #endif
  83. sin = (struct sockaddr_in *) &udpc->from;
  84. #if ((__GNUC__ == 4 && __GNUC_MINOR__ >= 6) || __GNUC__ > 4 || defined(__clang__))
  85. #pragma GCC diagnostic pop
  86. #endif
  87. } else {
  88. UA_ByteString_deleteMembers(buf);
  89. return UA_STATUSCODE_BADINTERNALERROR;
  90. }
  91. while (nWritten < (long)buf->length) {
  92. long n = sendto(layer->serversockfd, buf->data, buf->length, 0,
  93. (struct sockaddr*)sin, sizeof(struct sockaddr_in));
  94. if(n == -1L) {
  95. UA_LOG_WARNING(layer->logger, UA_LOGCATEGORY_NETWORK, "UDP send error %i", errno);
  96. UA_ByteString_deleteMembers(buf);
  97. return UA_STATUSCODE_BADINTERNALERROR;
  98. }
  99. nWritten += n;
  100. }
  101. UA_ByteString_deleteMembers(buf);
  102. return UA_STATUSCODE_GOOD;
  103. }
  104. static UA_StatusCode socket_set_nonblocking(UA_Int32 sockfd) {
  105. int opts = fcntl(sockfd, F_GETFL);
  106. if(opts < 0 || fcntl(sockfd, F_SETFL, opts|O_NONBLOCK) < 0)
  107. return UA_STATUSCODE_BADINTERNALERROR;
  108. return UA_STATUSCODE_GOOD;
  109. }
  110. static void setFDSet(ServerNetworkLayerUDP *layer) {
  111. FD_ZERO(&layer->fdset);
  112. FD_SET(layer->serversockfd, &layer->fdset);
  113. }
  114. static void closeConnectionUDP(UA_Connection *handle) {
  115. UA_free(handle);
  116. }
  117. static UA_StatusCode ServerNetworkLayerUDP_start(UA_ServerNetworkLayer *nl, UA_Logger logger) {
  118. ServerNetworkLayerUDP *layer = nl->handle;
  119. layer->logger = logger;
  120. layer->serversockfd = socket(AF_INET6, SOCK_DGRAM, 0);
  121. if(layer->serversockfd < 0) {
  122. UA_LOG_WARNING(layer->logger, UA_LOGCATEGORY_NETWORK, "Error opening socket");
  123. return UA_STATUSCODE_BADINTERNALERROR;
  124. }
  125. struct sockaddr_in6 serv_addr;
  126. memset(&serv_addr, 0, sizeof(serv_addr));
  127. serv_addr.sin6_family = AF_INET6;
  128. serv_addr.sin6_port = htons(layer->port);
  129. serv_addr.sin6_addr = in6addr_any;
  130. int optval = 1;
  131. if(setsockopt(layer->serversockfd, SOL_SOCKET,
  132. SO_REUSEADDR, (const char *)&optval, sizeof(optval)) == -1) {
  133. UA_LOG_WARNING(layer->logger, UA_LOGCATEGORY_NETWORK, "Could not setsockopt");
  134. CLOSESOCKET(layer->serversockfd);
  135. return UA_STATUSCODE_BADINTERNALERROR;
  136. }
  137. if(bind(layer->serversockfd, (const struct sockaddr *)&serv_addr,
  138. sizeof(serv_addr)) < 0) {
  139. UA_LOG_WARNING(layer->logger, UA_LOGCATEGORY_NETWORK, "Could not bind the socket");
  140. CLOSESOCKET(layer->serversockfd);
  141. return UA_STATUSCODE_BADINTERNALERROR;
  142. }
  143. socket_set_nonblocking(layer->serversockfd);
  144. UA_LOG_WARNING(layer->logger, UA_LOGCATEGORY_NETWORK, "Listening for UDP connections on %s:%d",
  145. inet_ntoa(serv_addr.sin_addr), ntohs(serv_addr.sin_port));
  146. return UA_STATUSCODE_GOOD;
  147. }
  148. static size_t ServerNetworkLayerUDP_getJobs(UA_ServerNetworkLayer *nl, UA_Job **jobs, UA_UInt16 timeout) {
  149. ServerNetworkLayerUDP *layer = nl->handle;
  150. UA_Job *items = NULL;
  151. setFDSet(layer);
  152. struct timeval tmptv = {0, timeout};
  153. int resultsize = select(layer->serversockfd+1, &layer->fdset, NULL, NULL, &tmptv);
  154. if(resultsize <= 0 || !FD_ISSET(layer->serversockfd, &layer->fdset)) {
  155. *jobs = items;
  156. return 0;
  157. }
  158. items = UA_malloc(sizeof(UA_Job)*(unsigned long)resultsize);
  159. // read from established sockets
  160. size_t j = 0;
  161. UA_ByteString buf = {0, NULL};
  162. if(!buf.data) {
  163. buf.data = UA_malloc(sizeof(UA_Byte) * layer->conf.recvBufferSize);
  164. if(!buf.data)
  165. UA_LOG_WARNING(layer->logger, UA_LOGCATEGORY_NETWORK, "malloc failed");
  166. }
  167. struct sockaddr sender;
  168. socklen_t sendsize = sizeof(sender);
  169. bzero(&sender, sizeof(sender));
  170. ssize_t rec_result = recvfrom(layer->serversockfd, buf.data, layer->conf.recvBufferSize, 0, &sender, &sendsize);
  171. if (rec_result > 0) {
  172. buf.length = (size_t)rec_result;
  173. UDPConnection *c = UA_malloc(sizeof(UDPConnection));
  174. if(!c){
  175. UA_free(items);
  176. return UA_STATUSCODE_BADINTERNALERROR;
  177. }
  178. UA_Connection_init(&c->connection);
  179. c->from = sender;
  180. c->fromlen = sendsize;
  181. // c->sockfd = newsockfd;
  182. c->connection.getSendBuffer = GetMallocedBuffer;
  183. c->connection.releaseSendBuffer = ReleaseMallocedBuffer;
  184. c->connection.releaseRecvBuffer = ReleaseMallocedBuffer;
  185. c->connection.handle = layer;
  186. c->connection.send = sendUDP;
  187. c->connection.close = closeConnectionUDP;
  188. c->connection.localConf = layer->conf;
  189. c->connection.state = UA_CONNECTION_OPENING;
  190. items[j].type = UA_JOBTYPE_BINARYMESSAGE_NETWORKLAYER;
  191. items[j].job.binaryMessage.message = buf;
  192. items[j].job.binaryMessage.connection = (UA_Connection*)c;
  193. buf.data = NULL;
  194. j++;
  195. *jobs = items;
  196. } else {
  197. UA_free(items);
  198. *jobs = NULL;
  199. }
  200. if(buf.data)
  201. UA_free(buf.data);
  202. return j;
  203. }
  204. static size_t ServerNetworkLayerUDP_stop(UA_ServerNetworkLayer *nl, UA_Job **jobs) {
  205. ServerNetworkLayerUDP *layer = nl->handle;
  206. UA_LOG_INFO(layer->logger, UA_LOGCATEGORY_NETWORK,
  207. "Shutting down the UDP network layer");
  208. CLOSESOCKET(layer->serversockfd);
  209. return 0;
  210. }
  211. static void ServerNetworkLayerUDP_deleteMembers(UA_ServerNetworkLayer *nl) {
  212. ServerNetworkLayerUDP *layer = nl->handle;
  213. UA_free(layer);
  214. UA_String_deleteMembers(&nl->discoveryUrl);
  215. }
  216. UA_ServerNetworkLayer
  217. UA_ServerNetworkLayerUDP(UA_ConnectionConfig conf, UA_UInt16 port) {
  218. UA_ServerNetworkLayer nl;
  219. memset(&nl, 0, sizeof(UA_ServerNetworkLayer));
  220. ServerNetworkLayerUDP *layer = UA_malloc(sizeof(ServerNetworkLayerUDP));
  221. if(!layer)
  222. return nl;
  223. memset(layer, 0, sizeof(ServerNetworkLayerUDP));
  224. layer->conf = conf;
  225. layer->port = port;
  226. nl.handle = layer;
  227. nl.start = ServerNetworkLayerUDP_start;
  228. nl.getJobs = ServerNetworkLayerUDP_getJobs;
  229. nl.stop = ServerNetworkLayerUDP_stop;
  230. nl.deleteMembers = ServerNetworkLayerUDP_deleteMembers;
  231. return nl;
  232. }