/************************************************************************** * * Copyright (C) 2005-2020 Steve Karg * * SPDX-License-Identifier: GPL-2.0-or-later WITH GCC-exception-2.0 * *********************************************************************/ #include #include #include #include #include #include #include #include #include /* BACnet Stack defines - first */ #include "bacnet/bacdef.h" /* BACnet Stack API */ #include "bacnet/bacdcode.h" #include "bacnet/bacint.h" #include "bacnet/datalink/bip6.h" #include "bacnet/basic/object/device.h" #include "bacnet/basic/sys/debug.h" #include "bacnet/basic/bbmd6/h_bbmd6.h" /* Logging module registration is already done in ports/zephyr/main.c */ #include #include LOG_MODULE_DECLARE(bacnet, CONFIG_BACNETSTACK_LOG_LEVEL); #define THIS_FILE "bip6-init.c" /* zephyr socket */ static int BIP6_Socket = -1; static int BIP6_Socket_Scope_Id; /* local address - filled by init functions */ static BACNET_IP6_ADDRESS BIP6_Addr; static BACNET_IP6_ADDRESS BIP6_Broadcast_Addr; /* Used by inet6_ntoa */ #if CONFIG_BACNETSTACK_LOG_LEVEL static char ipv6_addr_str[42] = { 0 }; #else static char ipv6_addr_str[] = ""; #endif /** * @brief Return a string representation of an IPv6 address * @param a - IPv6 address * @return Pointer to global string */ static char *inet6_ntoa(struct in6_addr *a) { #if CONFIG_BACNETSTACK_LOG_LEVEL uint8_t x = 0; uint8_t d = 0; uint8_t non_zero_count = 0; /* Avoid overwhelming the logging system */ while (log_buffered_cnt()) { k_sleep(K_MSEC(1)); } for (x = 0; x < IP6_ADDRESS_MAX; x += 2) { if (a->s6_addr[x] | a->s6_addr[x + 1]) { non_zero_count++; d += snprintf( &ipv6_addr_str[d], sizeof(ipv6_addr_str), "%02X%02X", a->s6_addr[x], a->s6_addr[x + 1]); } if (x < 14) { d += snprintf(&ipv6_addr_str[d], sizeof(ipv6_addr_str), ":"); } } if (!non_zero_count) { snprintf(&ipv6_addr_str[0], sizeof(ipv6_addr_str), "undefined"); } #endif return &ipv6_addr_str[0]; } void bip6_set_interface(char *ifname) { struct net_if *interface = 0; int index = -1; int x = 0; BACNET_IP6_ADDRESS unicast = { 0 }; BACNET_IP6_ADDRESS multicast = { 0 }; unicast.port = bip6_get_port(); multicast.port = unicast.port; LOG_DBG("bip6_set_interface()"); LOG_INF("BIP6: UDP port: 0x%04X", (unsigned)unicast.port); LOG_INF("BIP6: seeking interface: %s", ifname?ifname:"NULL"); if (ifname) { index = atoi(ifname); /* if index is zero, discern between "0" and a parse error */ if (!index && strcmp(ifname, "0")) { LOG_ERR( "%s:%d - Argument must parse to an integer", THIS_FILE, __LINE__); } else { interface = net_if_get_by_index(index); if (interface) { LOG_INF("BIP6: Using interface %d", index); } else { LOG_ERR( "%s:%d - No interface at index %d", THIS_FILE, __LINE__, index); } } } if (index == -1) { LOG_INF("BIP6: No valid interface specified. Using default "); interface = net_if_get_default(); } if (interface) { BIP6_Socket_Scope_Id = net_if_get_by_iface(interface); LOG_INF("BIP6: Socket Scope ID = %d", BIP6_Socket_Scope_Id); LOG_INF("BIP6: Interface set - Configured addresses:"); for (x = 0; x < NET_IF_MAX_IPV6_ADDR; x++) { inet6_ntoa(&interface->config.ip.ipv6->unicast[x].address.in6_addr); LOG_INF(" unicast[%d]: %s", x, ipv6_addr_str); } for (x = 0; x < NET_IF_MAX_IPV6_MADDR; x++) { inet6_ntoa(&interface->config.ip.ipv6->mcast[x].address.in6_addr); LOG_INF(" multicast[%d]: %s", x, ipv6_addr_str); } if (CONFIG_BACDL_BIP6_ADDRESS_INDEX >= NET_IF_MAX_IPV6_ADDR) { LOG_ERR( "%s:%d - IPv6 address index of %d is out of range (0-%d)", THIS_FILE, __LINE__, CONFIG_BACDL_BIP6_ADDRESS_INDEX, NET_IF_MAX_IPV6_ADDR - 1); return; } LOG_INF("BIP6: Using configured index %d", CONFIG_BACDL_BIP6_ADDRESS_INDEX); memcpy( &unicast.address, &interface->config.ip.ipv6->unicast[CONFIG_BACDL_BIP6_ADDRESS_INDEX] .address.in6_addr, IP6_ADDRESS_MAX); if (net_addr_pton( AF_INET6, CONFIG_BACDL_BIP6_MCAST_ADDRESS, &multicast.address)) { LOG_ERR( "%s:%d - Failed to parse IPv6 multicast address: %s", THIS_FILE, __LINE__, CONFIG_BACDL_BIP6_MCAST_ADDRESS); } bip6_set_addr(&unicast); bip6_set_broadcast_addr(&multicast); LOG_INF( " Unicast: %s", inet6_ntoa((struct in6_addr *)&unicast.address)); LOG_INF( " Multicast: %s", inet6_ntoa((struct in6_addr *)&multicast.address)); } else { LOG_ERR("%s:%d - Failed to set interface", THIS_FILE, __LINE__); } } /** * Set the BACnet IPv6 UDP port number * * @param port - IPv6 UDP port number */ void bip6_set_port(uint16_t port) { BIP6_Addr.port = port; BIP6_Broadcast_Addr.port = port; } /** * Get the BACnet IPv6 UDP port number * * @return IPv6 UDP port number */ uint16_t bip6_get_port(void) { return BIP6_Addr.port; } /** * Get the BACnet broadcast address for my interface. * Used as dest address in messages sent as BROADCAST * * @param addr - IPv6 source address */ void bip6_get_broadcast_address(BACNET_ADDRESS *addr) { if (addr) { addr->net = BACNET_BROADCAST_NETWORK; addr->mac_len = 0; addr->len = 0; } } /** * Get the IPv6 address for my interface. Used as src address in messages sent. * * @param addr - IPv6 source address */ void bip6_get_my_address(BACNET_ADDRESS *addr) { uint32_t device_id = 0; if (addr) { device_id = Device_Object_Instance_Number(); bvlc6_vmac_address_set(addr, device_id); } } /** * Set the BACnet/IP address * * @param addr - network IPv6 address */ bool bip6_set_addr(BACNET_IP6_ADDRESS *addr) { return bvlc6_address_copy(&BIP6_Addr, addr); } /** * Get the BACnet/IP address * * @return BACnet/IP address */ bool bip6_get_addr(BACNET_IP6_ADDRESS *addr) { return bvlc6_address_copy(addr, &BIP6_Addr); } /** * Set the BACnet/IP address * * @param addr - network IPv6 address */ bool bip6_set_broadcast_addr(BACNET_IP6_ADDRESS *addr) { return bvlc6_address_copy(&BIP6_Broadcast_Addr, addr); } /** * Get the BACnet/IP address * * @return BACnet/IP address */ bool bip6_get_broadcast_addr(BACNET_IP6_ADDRESS *addr) { return bvlc6_address_copy(addr, &BIP6_Broadcast_Addr); } /** * The send function for BACnet/IPv6 driver layer * * @param dest - Points to a BACNET_IP6_ADDRESS structure containing the * destination address. * @param mtu - the bytes of data to send * @param mtu_len - the number of bytes of data to send * * @return Upon successful completion, returns the number of bytes sent. * Otherwise, -1 shall be returned and errno set to indicate the error. */ int bip6_send_mpdu(BACNET_IP6_ADDRESS *dest, uint8_t *mtu, uint16_t mtu_len) { struct sockaddr_in6 bvlc_dest = { 0 }; uint16_t addr16[8]; /* assumes that the driver has already been initialized */ if (BIP6_Socket < 0) { LOG_ERR("%s:%d - Socket not initialized!", THIS_FILE, __LINE__); return BIP6_Socket; } /* load destination IP address */ bvlc_dest.sin6_family = AF_INET6; bvlc6_address_get( dest, &addr16[0], &addr16[1], &addr16[2], &addr16[3], &addr16[4], &addr16[5], &addr16[6], &addr16[7]); bvlc_dest.sin6_addr.s6_addr16[0] = htons(addr16[0]); bvlc_dest.sin6_addr.s6_addr16[1] = htons(addr16[1]); bvlc_dest.sin6_addr.s6_addr16[2] = htons(addr16[2]); bvlc_dest.sin6_addr.s6_addr16[3] = htons(addr16[3]); bvlc_dest.sin6_addr.s6_addr16[4] = htons(addr16[4]); bvlc_dest.sin6_addr.s6_addr16[5] = htons(addr16[5]); bvlc_dest.sin6_addr.s6_addr16[6] = htons(addr16[6]); bvlc_dest.sin6_addr.s6_addr16[7] = htons(addr16[7]); bvlc_dest.sin6_port = htons(dest->port); bvlc_dest.sin6_scope_id = BIP6_Socket_Scope_Id; inet6_ntoa(&bvlc_dest.sin6_addr); LOG_DBG("BIP6: Sending MPDU to %s", ipv6_addr_str); /* Send the packet */ return zsock_sendto( BIP6_Socket, (char *)mtu, mtu_len, 0, (struct sockaddr *)&bvlc_dest, sizeof(bvlc_dest)); } /** * The common send function for BACnet/IPv6 application layer * * @param dest - Points to a #BACNET_ADDRESS structure containing the * destination address. * @param npdu_data - Points to a BACNET_NPDU_DATA structure containing the * destination network layer control flags and data. * @param pdu - the bytes of data to send * @param pdu_len - the number of bytes of data to send * @return Upon successful completion, returns the number of bytes sent. * Otherwise, -1 shall be returned and errno set to indicate the error. */ int bip6_send_pdu( BACNET_ADDRESS *dest, BACNET_NPDU_DATA *npdu_data, uint8_t *pdu, unsigned pdu_len) { return bvlc6_send_pdu(dest, npdu_data, pdu, pdu_len); } /** * @brief Generate ASCII address string from BACnet/IPv6 address * @param s - buffer to store the string * @param n - size of the buffer * @param addr - BACnet/IPv6 address */ static int bvlc6_snprintf_addr(char *s, size_t n, BACNET_IP6_ADDRESS *addr) { uint16_t addr16[8]; bvlc6_address_get( addr, &addr16[0], &addr16[1], &addr16[2], &addr16[3], &addr16[4], &addr16[5], &addr16[6], &addr16[7]); return snprintf(s, n, "%04X:%04X:%04X:%04X:%04X:%04X:%04X:%04X", addr16[0], addr16[1], addr16[2], addr16[3], addr16[4], addr16[5], addr16[6], addr16[7]); } /** * BACnet/IP Datalink Receive handler. * * @param src - returns the source address * @param npdu - returns the NPDU buffer * @param max_npdu -maximum size of the NPDU buffer * @param timeout - number of milliseconds to wait for a packet * * @return Number of bytes received, or 0 if none or timeout. */ uint16_t bip6_receive( BACNET_ADDRESS *src, uint8_t *npdu, uint16_t max_npdu, unsigned timeout) { uint16_t npdu_len = 0; /* return value */ zsock_fd_set read_fds; int max = 0; struct zsock_timeval select_timeout; struct sockaddr_in6 sin = { 0 }; BACNET_IP6_ADDRESS addr = { 0 }; socklen_t sin_len = sizeof(sin); int received_bytes = 0; int offset = 0; uint16_t i = 0; /* Make sure the socket is open */ if (BIP6_Socket < 0) { return 0; } /* we could just use a non-blocking socket, but that consumes all the CPU time. We can use a timeout; it is only supported as a select. */ if (timeout >= 1000) { select_timeout.tv_sec = timeout / 1000; select_timeout.tv_usec = 1000 * (timeout - select_timeout.tv_sec * 1000); } else { select_timeout.tv_sec = 0; select_timeout.tv_usec = 1000 * timeout; } ZSOCK_FD_ZERO(&read_fds); ZSOCK_FD_SET(BIP6_Socket, &read_fds); max = BIP6_Socket; /* see if there is a packet for us */ if (zsock_select(max + 1, &read_fds, NULL, NULL, &select_timeout) > 0) { received_bytes = zsock_recvfrom( BIP6_Socket, (char *)&npdu[0], max_npdu, 0, (struct sockaddr *)&sin, &sin_len); } else { return 0; } /* See if there is a problem */ if (received_bytes < 0) { LOG_WRN( "%s:%d - RX zsock_recvfrom() error: %d", THIS_FILE, __LINE__, received_bytes); return 0; } /* no problem, just no bytes */ if (received_bytes == 0) { return 0; } /* the signature of a BACnet/IPv6 packet */ if (npdu[0] != BVLL_TYPE_BACNET_IP6) { LOG_DBG("BIP6: not a BACnet packet. Dropped."); return 0; } /* pass the packet into the BBMD handler */ inet6_ntoa(&sin.sin6_addr); LOG_DBG("BIP6: Received MPDU from %s", ipv6_addr_str); bvlc6_address_set( &addr, ntohs(sin.sin6_addr.s6_addr16[0]), ntohs(sin.sin6_addr.s6_addr16[1]), ntohs(sin.sin6_addr.s6_addr16[2]), ntohs(sin.sin6_addr.s6_addr16[3]), ntohs(sin.sin6_addr.s6_addr16[4]), ntohs(sin.sin6_addr.s6_addr16[5]), ntohs(sin.sin6_addr.s6_addr16[6]), ntohs(sin.sin6_addr.s6_addr16[7])); addr.port = ntohs(sin.sin6_port); offset = bvlc6_handler(&addr, src, npdu, received_bytes); if (offset > 0) { npdu_len = received_bytes - offset; if (npdu_len <= max_npdu) { /* shift the buffer to return a valid NPDU */ for (i = 0; i < npdu_len; i++) { npdu[i] = npdu[offset + i]; } } else { LOG_WRN("%s:%d - NPDU dropped (too long)!", THIS_FILE, __LINE__); npdu_len = 0; } } return npdu_len; } /** Cleanup and close out the BACnet/IP services by closing the socket. * @ingroup DLBIP6 */ void bip6_cleanup(void) { LOG_DBG("bip6_cleanup();"); bvlc6_cleanup(); if (BIP6_Socket != -1) { zsock_close(BIP6_Socket); } BIP6_Socket = -1; } /** Initialize the BACnet/IP services at the given interface. * @ingroup DLBIP6 * -# Gets the local IP address and local broadcast address from the system, * and saves it into the BACnet/IPv6 data structures. * -# Opens a UDP socket * -# Configures the socket for sending and receiving * -# Configures the socket so it can send multicasts * -# Binds the socket to the local IP address at the specified port for * BACnet/IPv6 (by default, 0xBAC0 = 47808). * * @param ifname [in] The named interface to use for the network layer. * If NULL, the "eth0" interface is assigned. * @return True if the socket is successfully opened for BACnet/IP, * else False if the socket functions fail. */ bool bip6_init(char *ifname) { int status = -1; struct sockaddr_in6 server = { 0 }; struct in6_addr broadcast_address; struct ipv6_mreq join_request; const int sockopt = 1; char addr6_str[40] = ""; LOG_DBG("bip6_init()"); if (BIP6_Addr.port == 0) { bip6_set_port(0xBAC0U); } bip6_set_interface(ifname); LOG_INF("BIP6: IPv6 UDP port: 0x%04X", (unsigned)BIP6_Addr.port); bvlc6_snprintf_addr(addr6_str, sizeof(addr6_str), &BIP6_Addr); LOG_INF("BIP6: IPv6 unicast addr: %s", addr6_str); if (BIP6_Broadcast_Addr.address[0] == 0) { bvlc6_address_set( &BIP6_Broadcast_Addr, BIP6_MULTICAST_SITE_LOCAL, 0, 0, 0, 0, 0, 0, BIP6_MULTICAST_GROUP_ID); LOG_INF("BIP6: IPv6 MULTICAST_SITE_LOCAL"); } bvlc6_snprintf_addr(addr6_str, sizeof(addr6_str), &BIP6_Broadcast_Addr); LOG_INF("BIP6: IPv6 multicast addr: %s", addr6_str); /* assumes that the driver has already been initialized */ BIP6_Socket = zsock_socket(AF_INET6, SOCK_DGRAM, IPPROTO_UDP); if (BIP6_Socket < 0) { LOG_ERR("%s:%d - Failed to create socket", THIS_FILE, __LINE__); return false; } else { LOG_INF("Socket created"); } /* Allow us to use the same socket for sending and receiving */ /* This makes sure that the src port is correct when sending */ status = zsock_setsockopt( BIP6_Socket, SOL_SOCKET, SO_REUSEADDR, &sockopt, sizeof(sockopt)); if (status < 0) { LOG_ERR("BIP6: setsockopt(SO_REUSEADDR)"); } /* allow us to send a broadcast */ status = zsock_setsockopt( BIP6_Socket, SOL_SOCKET, SO_BROADCAST, &sockopt, sizeof(sockopt)); if (status < 0) { /* ignored? For compatibility? Really? */ LOG_ERR("BIP6: setsockopt(SO_BROADCAST)"); } /* subscribe to a multicast address */ memcpy( &broadcast_address.s6_addr[0], &BIP6_Broadcast_Addr.address[0], IP6_ADDRESS_MAX); memcpy( &join_request.ipv6mr_multiaddr, &broadcast_address, sizeof(struct in6_addr)); /* Let system not choose the interface */ join_request.ipv6mr_ifindex = BIP6_Socket_Scope_Id; status = setsockopt( BIP6_Socket, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &join_request, sizeof(join_request)); if (status < 0) { LOG_ERR("BIP6: setsockopt(IPV6_ADD_MEMBERSHIP)"); return false; } /* bind the socket to the local port number and IP address */ server.sin6_family = AF_INET6; #if 0 uint16_t addr16[8]; bvlc6_address_get(&BIP6_Addr, &addr16[0], &addr16[1], &addr16[2], &addr16[3], &addr16[4], &addr16[5], &addr16[6], &addr16[7]); server.sin6_addr.s6_addr16[0] = htons(addr16[0]); server.sin6_addr.s6_addr16[1] = htons(addr16[1]); server.sin6_addr.s6_addr16[2] = htons(addr16[2]); server.sin6_addr.s6_addr16[3] = htons(addr16[3]); server.sin6_addr.s6_addr16[4] = htons(addr16[4]); server.sin6_addr.s6_addr16[5] = htons(addr16[5]); server.sin6_addr.s6_addr16[6] = htons(addr16[6]); server.sin6_addr.s6_addr16[7] = htons(addr16[7]); #else server.sin6_addr = in6addr_any; #endif server.sin6_port = htons(BIP6_Addr.port); LOG_INF("BIP6: Binding to port %d", BIP6_Addr.port); status = zsock_bind(BIP6_Socket, (const struct sockaddr *)&server, sizeof(server)); if (status < 0) { zsock_close(BIP6_Socket); BIP6_Socket = -1; LOG_ERR("%s:%d - zsock_bind() failure", THIS_FILE, __LINE__); return false; } else { LOG_INF("BIP6: Socket bound. Success!"); } bvlc6_init(); return true; } /** * @brief Check if the BACnet/IPv6 socket is valid * @return True if the socket is valid, else False */ bool bip6_valid(void) { return (BIP6_Socket != -1); }