ua_network_pubsub_ethernet.c 15 KB

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