ua_network_pubsub_ethernet.c 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449
  1. /* This Source Code Form is subject to the terms of the Mozilla Public
  2. * License, v. 2.0. If a copy of the MPL was not distributed with this
  3. * file, You can obtain one at http://mozilla.org/MPL/2.0/.
  4. *
  5. * Copyright 2018 (c) Kontron Europe GmbH (Author: Rudolf Hoyler)
  6. */
  7. #include "ua_plugin_network.h"
  8. #include "ua_log_stdout.h"
  9. #include "ua_util.h"
  10. #include <netinet/ether.h>
  11. #include <linux/if_packet.h>
  12. #include "ua_network_pubsub_ethernet.h"
  13. #include "../src/ua_util_internal.h"
  14. #ifndef ETHERTYPE_UADP
  15. #define ETHERTYPE_UADP 0xb62c
  16. #endif
  17. /* Ethernet network layer specific internal data */
  18. typedef struct {
  19. int ifindex;
  20. UA_UInt16 vid;
  21. UA_Byte prio;
  22. UA_Byte ifAddress[ETH_ALEN];
  23. UA_Byte targetAddress[ETH_ALEN];
  24. } UA_PubSubChannelDataEthernet;
  25. /*
  26. * OPC-UA specification Part 14:
  27. *
  28. * "The target is a MAC address, an IP address or a registered name like a
  29. * hostname. The format of a MAC address is six groups of hexadecimal digits,
  30. * separated by hyphens (e.g. 01-23-45-67-89-ab). A system may also accept
  31. * hostnames and/or IP addresses if it provides means to resolve it to a MAC
  32. * address (e.g. DNS and Reverse-ARP)."
  33. *
  34. * We do not support currently IP addresses or hostnames.
  35. */
  36. static UA_StatusCode
  37. UA_parseHardwareAddress(UA_String* target, UA_Byte* destinationMac) {
  38. size_t curr = 0, idx = 0;
  39. for(; idx < ETH_ALEN; idx++) {
  40. UA_UInt32 value;
  41. size_t progress =
  42. UA_readNumberWithBase(&target->data[curr],
  43. target->length - curr, &value, 16);
  44. if(progress == 0 || value > (long)0xff)
  45. return UA_STATUSCODE_BADINTERNALERROR;
  46. destinationMac[idx] = (UA_Byte) value;
  47. curr += progress;
  48. if(curr == target->length)
  49. break;
  50. if(target->data[curr] != '-')
  51. return UA_STATUSCODE_BADINTERNALERROR;
  52. curr++; /* skip '-' */
  53. }
  54. if(idx != (ETH_ALEN-1))
  55. return UA_STATUSCODE_BADINTERNALERROR;
  56. return UA_STATUSCODE_GOOD;
  57. }
  58. /**
  59. * Open communication socket based on the connectionConfig.
  60. *
  61. * @return ref to created channel, NULL on error
  62. */
  63. static UA_PubSubChannel *
  64. UA_PubSubChannelEthernet_open(const UA_PubSubConnectionConfig *connectionConfig) {
  65. UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  66. "Open PubSub ethernet connection.");
  67. /* allocate and init memory for the ethernet specific internal data */
  68. UA_PubSubChannelDataEthernet* channelDataEthernet =
  69. (UA_PubSubChannelDataEthernet*) UA_calloc(1, sizeof(*channelDataEthernet));
  70. if(!channelDataEthernet) {
  71. UA_LOG_ERROR (UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  72. "PubSub Connection creation failed. Out of memory.");
  73. return NULL;
  74. }
  75. /* handle specified network address */
  76. UA_NetworkAddressUrlDataType *address;
  77. if(UA_Variant_hasScalarType(&connectionConfig->address,
  78. &UA_TYPES[UA_TYPES_NETWORKADDRESSURLDATATYPE])) {
  79. address = (UA_NetworkAddressUrlDataType *) connectionConfig->address.data;
  80. } else {
  81. UA_LOG_ERROR (UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  82. "PubSub Connection creation failed. Invalid Address.");
  83. UA_free(channelDataEthernet);
  84. return NULL;
  85. }
  86. UA_LOG_DEBUG(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Specified Interface Name = %.*s",
  87. (int) address->networkInterface.length, address->networkInterface.data);
  88. UA_LOG_DEBUG(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Specified Network Url = %.*s",
  89. (int)address->url.length, address->url.data);
  90. UA_String target;
  91. /* encode the URL and store information in internal structure */
  92. if(UA_parseEndpointUrlEthernet(&address->url, &target, &channelDataEthernet->vid,
  93. &channelDataEthernet->prio)) {
  94. UA_LOG_ERROR (UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  95. "PubSub Connection creation failed. Invalid Address URL.");
  96. UA_free(channelDataEthernet);
  97. return NULL;
  98. }
  99. /* Get a valid MAC address from target definition */
  100. if(UA_parseHardwareAddress(&target, channelDataEthernet->targetAddress) != UA_STATUSCODE_GOOD) {
  101. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  102. "PubSub Connection creation failed. Invalid destination MAC address.");
  103. UA_free(channelDataEthernet);
  104. return NULL;
  105. }
  106. /* generate a new Pub/Sub channel and open a related socket */
  107. UA_PubSubChannel *newChannel = (UA_PubSubChannel*)UA_calloc(1, sizeof(UA_PubSubChannel));
  108. if(!newChannel) {
  109. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  110. "PubSub Connection creation failed. Out of memory.");
  111. UA_free(channelDataEthernet);
  112. return NULL;
  113. }
  114. /* Open a packet socket */
  115. int sockFd = UA_socket(PF_PACKET, SOCK_RAW, 0);
  116. if(sockFd < 0) {
  117. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  118. "PubSub connection creation failed. Cannot create socket.");
  119. UA_free(channelDataEthernet);
  120. UA_free(newChannel);
  121. return NULL;
  122. }
  123. newChannel->sockfd = sockFd;
  124. /* allow the socket to be reused */
  125. int opt = 1;
  126. if(UA_setsockopt(sockFd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0) {
  127. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  128. "PubSub connection creation failed. Cannot set socket reuse.");
  129. UA_close(sockFd);
  130. UA_free(channelDataEthernet);
  131. UA_free(newChannel);
  132. return NULL;
  133. }
  134. /* get interface index */
  135. struct ifreq ifreq = { 0 };
  136. strncpy(ifreq.ifr_name, (char*)address->networkInterface.data,
  137. UA_MIN(address->networkInterface.length, sizeof(ifreq.ifr_name)-1));
  138. if(ioctl(sockFd, SIOCGIFINDEX, &ifreq) < 0) {
  139. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  140. "PubSub connection creation failed. Cannot get interface index.");
  141. UA_close(sockFd);
  142. UA_free(channelDataEthernet);
  143. UA_free(newChannel);
  144. return NULL;
  145. }
  146. channelDataEthernet->ifindex = ifreq.ifr_ifindex;
  147. /* determine own MAC address (source address for send) */
  148. if(ioctl(sockFd, SIOCGIFHWADDR, &ifreq) < 0) {
  149. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  150. "PubSub connection creation failed. Cannot determine own MAC address.");
  151. UA_close(sockFd);
  152. UA_free(channelDataEthernet);
  153. UA_free(newChannel);
  154. return NULL;
  155. }
  156. memcpy(channelDataEthernet->ifAddress, &ifreq.ifr_hwaddr.sa_data, ETH_ALEN);
  157. /* bind the socket to interface and ethertype */
  158. struct sockaddr_ll sll = { 0 };
  159. sll.sll_family = AF_PACKET;
  160. sll.sll_ifindex = channelDataEthernet->ifindex;
  161. sll.sll_protocol = htons(ETHERTYPE_UADP);
  162. if(UA_bind(sockFd, (struct sockaddr*)&sll, sizeof(sll)) < 0) {
  163. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  164. "PubSub connection creation failed. Cannot bind socket.");
  165. UA_close(sockFd);
  166. UA_free(channelDataEthernet);
  167. UA_free(newChannel);
  168. return NULL;
  169. }
  170. newChannel->handle = channelDataEthernet;
  171. newChannel->state = UA_PUBSUB_CHANNEL_PUB;
  172. return newChannel;
  173. }
  174. static UA_Boolean
  175. is_multicast_address(const UA_Byte *address) {
  176. /* check if it is a unicast address */
  177. if((address[0] & 1) == 0) {
  178. return UA_FALSE;
  179. }
  180. /* and exclude broadcast addresses */
  181. for(size_t i = 0; i < ETH_ALEN; i++) {
  182. if(address[i] != 0xff)
  183. return UA_TRUE;
  184. }
  185. /* reaching this point, we know it has to be a broadcast address */
  186. return UA_FALSE;
  187. }
  188. /**
  189. * Subscribe to a given address.
  190. *
  191. * @return UA_STATUSCODE_GOOD on success
  192. */
  193. static UA_StatusCode
  194. UA_PubSubChannelEthernet_regist(UA_PubSubChannel *channel,
  195. UA_ExtensionObject *transportSettings) {
  196. UA_PubSubChannelDataEthernet *channelDataEthernet =
  197. (UA_PubSubChannelDataEthernet *) channel->handle;
  198. if(!is_multicast_address(channelDataEthernet->targetAddress))
  199. return UA_STATUSCODE_GOOD;
  200. struct packet_mreq mreq;
  201. mreq.mr_ifindex = channelDataEthernet->ifindex;
  202. mreq.mr_type = PACKET_MR_MULTICAST;
  203. mreq.mr_alen = ETH_ALEN;
  204. memcpy(mreq.mr_address, channelDataEthernet->targetAddress, ETH_ALEN);
  205. if(UA_setsockopt(channel->sockfd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, (char*) &mreq, sizeof(mreq)) < 0) {
  206. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "PubSub Connection regist failed. %s", strerror(errno));
  207. return UA_STATUSCODE_BADINTERNALERROR;
  208. }
  209. return UA_STATUSCODE_GOOD;
  210. }
  211. /**
  212. * Remove current subscription.
  213. *
  214. * @return UA_STATUSCODE_GOOD on success
  215. */
  216. static UA_StatusCode
  217. UA_PubSubChannelEthernet_unregist(UA_PubSubChannel *channel,
  218. UA_ExtensionObject *transportSettings) {
  219. UA_PubSubChannelDataEthernet *channelDataEthernet =
  220. (UA_PubSubChannelDataEthernet *) channel->handle;
  221. if(!is_multicast_address(channelDataEthernet->targetAddress)) {
  222. return UA_STATUSCODE_GOOD;
  223. }
  224. struct packet_mreq mreq;
  225. mreq.mr_ifindex = channelDataEthernet->ifindex;
  226. mreq.mr_type = PACKET_MR_MULTICAST;
  227. mreq.mr_alen = ETH_ALEN;
  228. memcpy(mreq.mr_address, channelDataEthernet->targetAddress, ETH_ALEN);
  229. if(UA_setsockopt(channel->sockfd, SOL_PACKET, PACKET_DROP_MEMBERSHIP, (char*) &mreq, sizeof(mreq) < 0)) {
  230. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "PubSub Connection regist failed.");
  231. return UA_STATUSCODE_BADINTERNALERROR;
  232. }
  233. return UA_STATUSCODE_GOOD;
  234. }
  235. /**
  236. * Send messages to the connection defined address
  237. *
  238. * @return UA_STATUSCODE_GOOD if success
  239. */
  240. static UA_StatusCode
  241. UA_PubSubChannelEthernet_send(UA_PubSubChannel *channel,
  242. UA_ExtensionObject *transportSettings,
  243. const UA_ByteString *buf) {
  244. UA_PubSubChannelDataEthernet *channelDataEthernet =
  245. (UA_PubSubChannelDataEthernet *) channel->handle;
  246. /* Allocate a buffer for the ethernet data which contains the ethernet
  247. * header (without VLAN tag), the VLAN tag and the OPC-UA/Ethernet data. */
  248. char *bufSend, *ptrCur;
  249. size_t lenBuf;
  250. struct ether_header* ethHdr;
  251. lenBuf = sizeof(*ethHdr) + 4 + buf->length;
  252. bufSend = (char*) UA_malloc(lenBuf);
  253. ethHdr = (struct ether_header*) bufSend;
  254. /* Set (own) source MAC address */
  255. memcpy(ethHdr->ether_shost, channelDataEthernet->ifAddress, ETH_ALEN);
  256. /* Set destination MAC address */
  257. memcpy(ethHdr->ether_dhost, channelDataEthernet->targetAddress, ETH_ALEN);
  258. /* Set ethertype */
  259. /* Either VLAN or Ethernet */
  260. ptrCur = bufSend + sizeof(*ethHdr);
  261. if(channelDataEthernet->vid == 0) {
  262. ethHdr->ether_type = htons(ETHERTYPE_UADP);
  263. lenBuf -= 4; /* no VLAN tag */
  264. } else {
  265. ethHdr->ether_type = htons(ETHERTYPE_VLAN);
  266. /* set VLAN ID */
  267. UA_UInt16 vlanTag;
  268. vlanTag = (UA_UInt16) (channelDataEthernet->vid + (channelDataEthernet->prio << 13));
  269. *((UA_UInt16 *) ptrCur) = htons(vlanTag);
  270. ptrCur += sizeof(UA_UInt16);
  271. /* set Ethernet */
  272. *((UA_UInt16 *) ptrCur) = htons(ETHERTYPE_UADP);
  273. ptrCur += sizeof(UA_UInt16);
  274. }
  275. /* copy payload of ethernet message */
  276. memcpy(ptrCur, buf->data, buf->length);
  277. ssize_t rc;
  278. rc = UA_send(channel->sockfd, bufSend, lenBuf, 0);
  279. if(rc < 0) {
  280. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  281. "PubSub connection send failed. Send message failed.");
  282. UA_free(bufSend);
  283. return UA_STATUSCODE_BADINTERNALERROR;
  284. }
  285. UA_free(bufSend);
  286. return UA_STATUSCODE_GOOD;
  287. }
  288. /**
  289. * Receive messages.
  290. *
  291. * @param timeout in usec -> not used
  292. * @return
  293. */
  294. static UA_StatusCode
  295. UA_PubSubChannelEthernet_receive(UA_PubSubChannel *channel, UA_ByteString *message,
  296. UA_ExtensionObject *transportSettings, UA_UInt32 timeout) {
  297. UA_PubSubChannelDataEthernet *channelDataEthernet =
  298. (UA_PubSubChannelDataEthernet *) channel->handle;
  299. struct ether_header eth_hdr;
  300. struct msghdr msg;
  301. struct iovec iov[2];
  302. iov[0].iov_base = &eth_hdr;
  303. iov[0].iov_len = sizeof(eth_hdr);
  304. iov[1].iov_base = message->data;
  305. iov[1].iov_len = message->length;
  306. msg.msg_namelen = 0;
  307. msg.msg_iov = iov;
  308. msg.msg_iovlen = 2;
  309. msg.msg_controllen = 0;
  310. /* Sleep in a select call if a timeout was set */
  311. if(timeout > 0) {
  312. fd_set fdset;
  313. FD_ZERO(&fdset);
  314. UA_fd_set(channel->sockfd, &fdset);
  315. struct timeval tmptv = {(long int)(timeout / 1000000),
  316. (long int)(timeout % 1000000)};
  317. int resultsize = UA_select(channel->sockfd+1, &fdset, NULL, NULL, &tmptv);
  318. if(resultsize == 0) {
  319. message->length = 0;
  320. return UA_STATUSCODE_GOODNONCRITICALTIMEOUT;
  321. }
  322. if(resultsize == -1) {
  323. message->length = 0;
  324. return UA_STATUSCODE_BADINTERNALERROR;
  325. }
  326. }
  327. /* Read the current packet on the socket */
  328. ssize_t dataLen = recvmsg(channel->sockfd, &msg, 0);
  329. if(dataLen < 0) {
  330. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  331. "PubSub connection receive failed. Receive message failed.");
  332. return UA_STATUSCODE_BADINTERNALERROR;
  333. }
  334. if((size_t)dataLen < sizeof(eth_hdr)) {
  335. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  336. "PubSub connection receive failed. Packet too small.");
  337. return UA_STATUSCODE_BADINTERNALERROR;
  338. }
  339. if(dataLen == 0)
  340. return UA_STATUSCODE_GOODNODATA;
  341. /* Make sure we match our target */
  342. if(memcmp(eth_hdr.ether_dhost, channelDataEthernet->targetAddress, ETH_ALEN) != 0)
  343. return UA_STATUSCODE_GOODNODATA;
  344. /* Set the message length */
  345. message->length = (size_t)dataLen - sizeof(eth_hdr);
  346. return UA_STATUSCODE_GOOD;
  347. }
  348. /**
  349. * Close channel and free the channel data.
  350. *
  351. * @return UA_STATUSCODE_GOOD if success
  352. */
  353. static UA_StatusCode
  354. UA_PubSubChannelEthernet_close(UA_PubSubChannel *channel) {
  355. UA_close(channel->sockfd);
  356. UA_free(channel->handle);
  357. UA_free(channel);
  358. return UA_STATUSCODE_GOOD;
  359. }
  360. /**
  361. * Generate a new channel. based on the given configuration.
  362. *
  363. * @param connectionConfig connection configuration
  364. * @return ref to created channel, NULL on error
  365. */
  366. static UA_PubSubChannel *
  367. TransportLayerEthernet_addChannel(UA_PubSubConnectionConfig *connectionConfig) {
  368. UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_USERLAND, "PubSub channel requested");
  369. UA_PubSubChannel * pubSubChannel = UA_PubSubChannelEthernet_open(connectionConfig);
  370. if(pubSubChannel) {
  371. pubSubChannel->regist = UA_PubSubChannelEthernet_regist;
  372. pubSubChannel->unregist = UA_PubSubChannelEthernet_unregist;
  373. pubSubChannel->send = UA_PubSubChannelEthernet_send;
  374. pubSubChannel->receive = UA_PubSubChannelEthernet_receive;
  375. pubSubChannel->close = UA_PubSubChannelEthernet_close;
  376. pubSubChannel->connectionConfig = connectionConfig;
  377. }
  378. return pubSubChannel;
  379. }
  380. UA_PubSubTransportLayer
  381. UA_PubSubTransportLayerEthernet() {
  382. UA_PubSubTransportLayer pubSubTransportLayer;
  383. pubSubTransportLayer.transportProfileUri =
  384. UA_STRING("http://opcfoundation.org/UA-Profile/Transport/pubsub-eth-uadp");
  385. pubSubTransportLayer.createPubSubChannel = &TransportLayerEthernet_addChannel;
  386. return pubSubTransportLayer;
  387. }