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. #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;
  136. memset(&ifreq, 0, sizeof(struct ifreq));
  137. strncpy(ifreq.ifr_name, (char*)address->networkInterface.data,
  138. UA_MIN(address->networkInterface.length, sizeof(ifreq.ifr_name)-1));
  139. if(ioctl(sockFd, SIOCGIFINDEX, &ifreq) < 0) {
  140. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  141. "PubSub connection creation failed. Cannot get interface index.");
  142. UA_close(sockFd);
  143. UA_free(channelDataEthernet);
  144. UA_free(newChannel);
  145. return NULL;
  146. }
  147. channelDataEthernet->ifindex = ifreq.ifr_ifindex;
  148. /* determine own MAC address (source address for send) */
  149. if(ioctl(sockFd, SIOCGIFHWADDR, &ifreq) < 0) {
  150. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  151. "PubSub connection creation failed. Cannot determine own MAC address.");
  152. UA_close(sockFd);
  153. UA_free(channelDataEthernet);
  154. UA_free(newChannel);
  155. return NULL;
  156. }
  157. memcpy(channelDataEthernet->ifAddress, &ifreq.ifr_hwaddr.sa_data, ETH_ALEN);
  158. /* bind the socket to interface and ethertype */
  159. struct sockaddr_ll sll = { 0 };
  160. sll.sll_family = AF_PACKET;
  161. sll.sll_ifindex = channelDataEthernet->ifindex;
  162. sll.sll_protocol = htons(ETHERTYPE_UADP);
  163. if(UA_bind(sockFd, (struct sockaddr*)&sll, sizeof(sll)) < 0) {
  164. UA_LOG_ERROR(UA_Log_Stdout, UA_LOGCATEGORY_SERVER,
  165. "PubSub connection creation failed. Cannot bind socket.");
  166. UA_close(sockFd);
  167. UA_free(channelDataEthernet);
  168. UA_free(newChannel);
  169. return NULL;
  170. }
  171. newChannel->handle = channelDataEthernet;
  172. newChannel->state = UA_PUBSUB_CHANNEL_PUB;
  173. return newChannel;
  174. }
  175. static UA_Boolean
  176. is_multicast_address(const UA_Byte *address) {
  177. /* check if it is a unicast address */
  178. if((address[0] & 1) == 0) {
  179. return UA_FALSE;
  180. }
  181. /* and exclude broadcast addresses */
  182. for(size_t i = 0; i < ETH_ALEN; i++) {
  183. if(address[i] != 0xff)
  184. return UA_TRUE;
  185. }
  186. /* reaching this point, we know it has to be a broadcast address */
  187. return UA_FALSE;
  188. }
  189. /**
  190. * Subscribe to a given address.
  191. *
  192. * @return UA_STATUSCODE_GOOD on success
  193. */
  194. static UA_StatusCode
  195. UA_PubSubChannelEthernet_regist(UA_PubSubChannel *channel,
  196. UA_ExtensionObject *transportSettings) {
  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. }