diff --git a/bacnet-stack/Makefile b/bacnet-stack/Makefile index 4686e153..ff371f55 100644 --- a/bacnet-stack/Makefile +++ b/bacnet-stack/Makefile @@ -96,6 +96,9 @@ error: router: library $(MAKE) -s -C demo router +router-ipv6: + $(MAKE) -B -s -C demo router-ipv6 + # Add "ports" to the build, if desired ports: atmega168 bdk-atxx4-mstp at91sam7s @echo "Built the ARM7 and AVR ports" @@ -116,4 +119,5 @@ clean: $(MAKE) -s -C lib clean $(MAKE) -s -C demo clean $(MAKE) -s -C demo/router clean + $(MAKE) -s -C demo/router-ipv6 clean $(MAKE) -s -C demo/gateway clean diff --git a/bacnet-stack/bin/bacroute.sh b/bacnet-stack/bin/bacroute.sh new file mode 100644 index 00000000..cc16156e --- /dev/null +++ b/bacnet-stack/bin/bacroute.sh @@ -0,0 +1,19 @@ +#!/bin/bash +echo Setting parameters for Router +BACNET_IP_PORT=47808 +export BACNET_IP_PORT +#BACNET_BBMD_PORT=47809 +#export BACNET_BBMD_PORT +BACNET_IFACE=${1} +export BACNET_IFACE +BACNET_IP6_PORT=47808 +export BACNET_IP6_PORT +BACNET_BIP6_BROADCAST=FF05::BAC0 +export BACNET_BIP6_BROADCAST +BACNET_BIP6_IFACE=${1} +export BACNET_BIP6_IFACE +# Network Numbers +BACNET_IP_NET=1 +export BACNET_IP_NET +BACNET_IP6_NET=2 +export BACNET_IP6_NET diff --git a/bacnet-stack/build.bat b/bacnet-stack/build.bat index edbf2a64..1de737ca 100644 --- a/bacnet-stack/build.bat +++ b/bacnet-stack/build.bat @@ -12,6 +12,7 @@ rem make BACNET_PORT=win32 BUILD=debug clean all rem Build for MinGW MS/TP rem make BACNET_PORT=win32 BACDL_DEFINE=-DBACDL_MSTP=1 clean all +rem make BACNET_PORT=win32 BACDL_DEFINE=-DBACDL_BIP6=1 clean all rem On Linux, install mingw32 and use this: rem make BACNET_PORT=win32 CC=i586-mingw32msvc-gcc AR=i586-mingw32msvc-ar clean all diff --git a/bacnet-stack/build.sh b/bacnet-stack/build.sh index cd492de7..0f35d939 100755 --- a/bacnet-stack/build.sh +++ b/bacnet-stack/build.sh @@ -14,6 +14,8 @@ make BACNET_PORT=win32 BUILD=release clean all > /dev/null # Build for MinGW MS/TP # make BACNET_PORT=win32 BACDL_DEFINE=-DBACDL_MSTP=1 clean all +# make BACNET_PORT=win32 BACDL_DEFINE=-DBACDL_BIP6=1 clean all +# make BACDL_DEFINE=-DBACDL_BIP6=1 BUILD=debug clean all # On Linux, install mingw32 and use this: # make BACNET_PORT=win32 CC=i586-mingw32msvc-gcc AR=i586-mingw32msvc-ar clean all diff --git a/bacnet-stack/demo/Makefile b/bacnet-stack/demo/Makefile index e952d58a..a452e145 100644 --- a/bacnet-stack/demo/Makefile +++ b/bacnet-stack/demo/Makefile @@ -47,7 +47,7 @@ DEBUGGING = OPTIMIZATION = -Os ifeq (${BUILD},debug) OPTIMIZATION = -O0 -DEBUGGING = -g +DEBUGGING = -g -DDEBUG_ENABLED=1 endif # put all the flags together CFLAGS := -Wall $(DEBUGGING) $(OPTIMIZATION) $(INCLUDES) $(DEFINES) @@ -109,3 +109,7 @@ error: router: $(MAKE) -s -b -C router + +router-ipv6: + $(MAKE) -b -C router-ipv6 + diff --git a/bacnet-stack/demo/handler/dlenv.c b/bacnet-stack/demo/handler/dlenv.c index 69a991bb..a392e0e1 100644 --- a/bacnet-stack/demo/handler/dlenv.c +++ b/bacnet-stack/demo/handler/dlenv.c @@ -226,6 +226,10 @@ void dlenv_maintenance_timer( * - BACNET_MAX_MASTER * - BACNET_MSTP_BAUD * - BACNET_MSTP_MAC + * - BACDL_BIP6: (BACnet/IPv6) + * - BACNET_BIP6_PORT - UDP/IP port number (0..65534) used for BACnet/IPv6 + * communications. Default is 47808 (0xBAC0). + * - BACNET_BIP6_BROADCAST - FF05::BAC0 or FF02::BAC0 or ... */ void dlenv_init( void) @@ -240,6 +244,27 @@ void dlenv_init( datalink_set(NULL); } #endif +#if defined(BACDL_BIP6) + BACNET_IP6_ADDRESS addr; + pEnv = getenv("BACNET_BIP6_BROADCAST"); + if (pEnv) { + bvlc6_address_set(&addr, + (uint16_t) strtol(pEnv, NULL, 0), 0, 0, 0, 0, 0, 0, + BIP6_MULTICAST_GROUP_ID); + bip6_set_broadcast_addr(&addr); + } else { + bvlc6_address_set(&addr, + BIP6_MULTICAST_SITE_LOCAL, 0, 0, 0, 0, 0, 0, + BIP6_MULTICAST_GROUP_ID); + bip6_set_broadcast_addr(&addr); + } + pEnv = getenv("BACNET_BIP6_PORT"); + if (pEnv) { + bip6_set_port((uint16_t) strtol(pEnv, NULL, 0)); + } else { + bip6_set_port(0xBAC0); + } +#endif #if defined(BACDL_BIP) #if defined(BIP_DEBUG) BIP_Debug = true; @@ -295,6 +320,7 @@ void dlenv_init( if (pEnv) { apdu_retries_set((uint8_t) strtol(pEnv, NULL, 0)); } + /* === Initialize the Datalink Here === */ if (!datalink_init(getenv("BACNET_IFACE"))) { exit(1); } diff --git a/bacnet-stack/demo/handler/h_bbmd6.c b/bacnet-stack/demo/handler/h_bbmd6.c new file mode 100644 index 00000000..25ce9066 --- /dev/null +++ b/bacnet-stack/demo/handler/h_bbmd6.c @@ -0,0 +1,1214 @@ +/*####COPYRIGHTBEGIN#### + ------------------------------------------- + Copyright (C) 2016 Steve Karg + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to: + The Free Software Foundation, Inc. + 59 Temple Place - Suite 330 + Boston, MA 02111-1307, USA. + + As a special exception, if other files instantiate templates or + use macros or inline functions from this file, or you compile + this file and link it with other works to produce a work based + on this file, this file does not by itself cause the resulting + work to be covered by the GNU General Public License. However + the source code for this file must still be made available in + accordance with section (3) of the GNU General Public License. + + This exception does not invalidate any other reasons why a work + based on this file might be covered by the GNU General Public + License. + ------------------------------------------- +####COPYRIGHTEND####*/ + +#include /* for standard i/o, like printing */ +#include /* for standard integer types uint8_t etc. */ +#include /* for the standard bool type. */ +#include /* for memcpy */ +#include "bacdcode.h" +#include "bip6.h" +#include "bvlc6.h" +#include "debug.h" +#include "device.h" +#include "vmac.h" +#ifndef TEST +#include "net.h" +#endif + +/** result from a client request */ +static uint16_t BVLC6_Result_Code = BVLC6_RESULT_SUCCESSFUL_COMPLETION; +/** incoming function */ +static uint8_t BVLC6_Function_Code = BVLC6_RESULT; + +/** if we are a foreign device, store the remote BBMD address/port here */ +static BACNET_IP6_ADDRESS Remote_BBMD; +#if defined(BACDL_BIP6) && BBMD6_ENABLED +/* local buffer & length for sending */ +static uint8_t BVLC6_Buffer[MAX_MPDU]; +static uint16_t BVLC6_Buffer_Len; +/* Broadcast Distribution Table */ +#ifndef MAX_BBMD6_ENTRIES +#define MAX_BBMD6_ENTRIES 128 +#endif +static BACNET_IP6_BROADCAST_DISTRIBUTION_TABLE_ENTRY BBMD_Table[MAX_BBMD6_ENTRIES]; +/* Foreign Device Table */ +#ifndef MAX_FD6_ENTRIES +#define MAX_FD6_ENTRIES 128 +#endif +static BACNET_IP6_FOREIGN_DEVICE_TABLE_ENTRY FD_Table[MAX_FD6_ENTRIES]; +#endif + +#if defined(BACDL_BIP6) && BBMD6_ENABLED +/** A timer function that is called about once a second. + * + * @param seconds - number of elapsed seconds since the last call + */ +void bbmd6_maintenance_timer( + time_t seconds) +{ + unsigned i = 0; + + for (i = 0; i < MAX_FD_ENTRIES; i++) { + if (FD_Table[i].valid) { + if (FD_Table[i].seconds_remaining) { + if (FD_Table[i].seconds_remaining < seconds) { + FD_Table[i].seconds_remaining = 0; + } else { + FD_Table[i].seconds_remaining -= seconds; + } + if (FD_Table[i].seconds_remaining == 0) { + FD_Table[i].valid = false; + } + } + } + } +} +#endif + +/** + * Sets the IPv6 source address from a VMAC address structure + * + * @param addr - IPv6 source address + * @param vmac - VMAC address that will be use for holding the IPv6 address + * + * @return true if the address was set + */ +static bool bbmd6_address_from_vmac( + BACNET_IP6_ADDRESS *addr, + struct vmac_data *vmac) +{ + bool status = false; + unsigned int i = 0; + + if (vmac && addr && (vmac->mac_len == 18)) { + for (i = 0; i < IP6_ADDRESS_MAX; i++) { + addr->address[i] = vmac->mac[i]; + } + decode_unsigned16(&vmac->mac[16], &addr->port); + status = true; + } + + return status; +} + +/** + * Sets the IPv6 source address to a VMAC address structure + * + * @param vmac - VMAC address that will be use for holding the IPv6 address + * @param addr - IPv6 source address + * + * @return true if the address was set + */ +static bool bbmd6_address_to_vmac( + struct vmac_data *vmac, + BACNET_IP6_ADDRESS *addr) +{ + bool status = false; + unsigned int i = 0; + + if (vmac && addr) { + for (i = 0; i < IP6_ADDRESS_MAX; i++) { + vmac->mac[i] = addr->address[i]; + } + encode_unsigned16(&vmac->mac[16], addr->port); + vmac->mac_len = 18; + status = true; + } + + return status; +} + +/** + * Adds an IPv6 source address and Device ID key to a VMAC address cache + * + * @param device_id - device ID used as the key-pair + * @param addr - IPv6 source address + * + * @return true if the VMAC address was added + */ +static bool bbmd6_add_vmac( + uint32_t device_id, + BACNET_IP6_ADDRESS *addr) +{ + bool status = false; + struct vmac_data *vmac; + struct vmac_data new_vmac; + + if (addr) { + vmac = VMAC_Find_By_Key(device_id); + if (vmac) { + /* already exists - replace? */ + } else if (bbmd6_address_to_vmac(&new_vmac, addr)) { + /* new entry - add it! */ + status = VMAC_Add(device_id, &new_vmac); + debug_printf("BVLC6: Adding VMAC %lu.\n", (unsigned long)device_id); + } + } + + return status; +} + +/** + * Compares the IPv6 source address to my VMAC + * + * @param addr - IPv6 source address + * + * @return true if the IPv6 from sin match me + */ +static bool bbmd6_address_match_self( + BACNET_IP6_ADDRESS *addr) +{ + BACNET_IP6_ADDRESS my_addr = {{0}}; + bool status = false; + + if (bip6_get_addr(&my_addr)) { + if (!bvlc6_address_different(&my_addr, addr)) { + status = true; + } + } + + return status; +} + +/** + * Finds the BACNET_IP6_ADDRESS for the #BACNET_ADDRESS via VMAC + * + * @param addr - IPv6 address + * @param vmac_src - VMAC address (Device ID) + * @param baddr - Points to a #BACNET_ADDRESS structure containing the + * VMAC address. + * + * @return true if the address was in the VMAC table + */ +static bool bbmd6_address_from_bacnet_address( + BACNET_IP6_ADDRESS * addr, + uint32_t * vmac_src, + BACNET_ADDRESS * baddr) +{ + struct vmac_data *vmac; + bool status = false; + uint32_t device_id = 0; + + if (addr && baddr) { + status = bvlc6_vmac_address_get(baddr, &device_id); + if (status) { + vmac = VMAC_Find_By_Key(device_id); + if (vmac) { + debug_printf("BVLC6: Found VMAC %lu (len=%u).\n", + (unsigned long)device_id, + (unsigned)vmac->mac_len); + status = bbmd6_address_from_vmac(addr, vmac); + if (vmac_src) { + *vmac_src = device_id; + } + } + } + } + + return status; +} + + + +/** + * 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 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_pdu( + BACNET_ADDRESS * dest, + BACNET_NPDU_DATA * npdu_data, + uint8_t * pdu, + unsigned pdu_len) +{ + BACNET_IP6_ADDRESS bvlc_dest = {{0}}; + uint8_t mtu[MAX_MPDU] = { 0 }; + uint16_t mtu_len = 0; + uint32_t vmac_src = 0; + uint32_t vmac_dst = 0; + + /* this datalink doesn't need to know the npdu data */ + (void) npdu_data; + /* handle various broadcasts: */ + if ((dest->net == BACNET_BROADCAST_NETWORK) || (dest->mac_len == 0)) { + /* mac_len = 0 is a broadcast address */ + /* net = 0 indicates local, net = 65535 indicates global */ + if (Remote_BBMD.port) { + /* we are a foreign device */ + bvlc6_address_copy(&bvlc_dest, &Remote_BBMD); + vmac_src = Device_Object_Instance_Number(); + mtu_len = bvlc6_encode_distribute_broadcast_to_network( + mtu, sizeof(mtu), vmac_src, pdu, pdu_len); + debug_printf("BVLC6: Sent Distribute-Broadcast-to-Network.\n"); + } else { + bip6_get_broadcast_addr(&bvlc_dest); + vmac_src = Device_Object_Instance_Number(); + mtu_len = bvlc6_encode_original_broadcast( + mtu, sizeof(mtu), vmac_src, pdu, pdu_len); + debug_printf("BVLC6: Sent Original-Broadcast-NPDU.\n"); + } + } else if ((dest->net > 0) && (dest->len == 0)) { + /* net > 0 and net < 65535 are network specific broadcast if len = 0 */ + if (dest->mac_len == 3) { + /* network specific broadcast to address */ + bbmd6_address_from_bacnet_address(&bvlc_dest, &vmac_dst, dest); + } else { + bip6_get_broadcast_addr(&bvlc_dest); + } + vmac_src = Device_Object_Instance_Number(); + mtu_len = bvlc6_encode_original_broadcast( + mtu, sizeof(mtu), vmac_src, pdu, pdu_len); + debug_printf("BVLC6: Sent Original-Broadcast-NPDU.\n"); + } else if (dest->mac_len == 3) { + /* valid unicast */ + bbmd6_address_from_bacnet_address(&bvlc_dest, &vmac_dst, dest); + debug_printf("BVLC6: Sending to VMAC %lu.\n", (unsigned long)vmac_dst); + vmac_src = Device_Object_Instance_Number(); + mtu_len = bvlc6_encode_original_unicast( + mtu, sizeof(mtu), vmac_src, vmac_dst, pdu, pdu_len); + debug_printf("BVLC6: Sent Original-Unicast-NPDU.\n"); + } else { + debug_printf("BVLC6: Send failure. Invalid Address.\n"); + return -1; + } + + return bip6_send_mpdu(&bvlc_dest, mtu, mtu_len); +} + +#if defined(BACDL_BIP6) && BBMD6_ENABLED +/** + * The send function for Broacast Distribution Table + * + * @param addr - Points to a #BACNET_IP6_ADDRESS structure containing the + * source IPv6 address. + * @param vmac_src - Source-Virtual-Address + * @param npdu - the bytes of NPDU+APDU data to send + * @param npdu_len - the number of bytes of NPDU+APDU data to send + */ +static void bbmd6_send_pdu_bdt( + uint8_t * mtu, + unsigned int mtu_len) +{ + BACNET_IP6_ADDRESS my_addr = {{0}}; + unsigned i = 0; /* loop counter */ + + if (mtu) { + bip6_get_addr(&my_addr); + for (i = 0; i < MAX_BBMD_ENTRIES; i++) { + if (BBMD_Table[i].valid) { + if (bvlc6_address_different(&my_addr, + &BBMD_Table[i].bip6_address)) { + bip6_send_mpdu(&BBMD_Table[i].bip6_address, mtu, mtu_len); + } + } + } + } +} + +/** + * The send function for Broacast Distribution Table + * + * @param addr - Points to a #BACNET_IP6_ADDRESS structure containing the + * source IPv6 address. + * @param vmac_src - Source-Virtual-Address + * @param npdu - the bytes of NPDU+APDU data to send + * @param npdu_len - the number of bytes of NPDU+APDU data to send + */ +static void bbmd6_send_pdu_fdt( + uint8_t * mtu, + unsigned int mtu_len) +{ + BACNET_IP6_ADDRESS my_addr = {{0}}; + unsigned i = 0; /* loop counter */ + + if (mtu) { + bip6_get_addr(&my_addr); + for (i = 0; i < MAX_FD_ENTRIES; i++) { + if (FD_Table[i].valid) { + if (bvlc6_address_different(&my_addr, + &FD_Table[i].bip6_address)) { + bip6_send_mpdu(&FD_Table[i].bip6_address, mtu, mtu_len); + } + } + } + } +} + +/** + * The Forward NPDU send function for Broacast Distribution Table + * + * @param addr - Points to a #BACNET_IP6_ADDRESS structure containing the + * source IPv6 address. + * @param vmac_src - Source-Virtual-Address + * @param npdu - the bytes of NPDU+APDU data to send + * @param npdu_len - the number of bytes of NPDU+APDU data to send + */ +static void bbmd6_send_forward_npdu( + BACNET_IP6_ADDRESS *address, + uint32_t vmac_src, + uint8_t * npdu, + unsigned int npdu_len) +{ + uint8_t mtu[MAX_MPDU] = { 0 }; + uint16_t mtu_len = 0; + unsigned i = 0; /* loop counter */ + + for (i = 0; i < MAX_BBMD_ENTRIES; i++) { + if (BBMD_Table[i].valid) { + if (bbmd6_address_match_self(&BBMD_Table[i].bip6_address)) { + /* don't forward to our selves */ + } else { + bip6_send_mpdu(&BBMD_Table[i].bip6_address, mtu, mtu_len); + } + } + } + for (i = 0; i < MAX_FD_ENTRIES; i++) { + if (FD_Table[i].valid) { + if (bbmd6_address_match_self(&FD_Table[i].bip6_address)) { + /* don't forward to our selves */ + } else { + bip6_send_mpdu(&FD_Table[i].bip6_address, mtu, mtu_len); + } + } + } +} + +#endif + +/** + * The Result Code send function for BACnet/IPv6 application layer + * + * @param dest_addr - Points to a #BACNET_IP6_ADDRESS structure containing the + * destination IPv6 address. + * @param vmac_src - Source-Virtual-Address + * @param result_code - BVLC result code + * + * @return Upon successful completion, returns the number of bytes sent. + * Otherwise, -1 shall be returned and errno set to indicate the error. + */ +static int bvlc6_send_result( + BACNET_IP6_ADDRESS *dest_addr, + uint32_t vmac_src, + uint16_t result_code) +{ + uint8_t mtu[MAX_MPDU] = { 0 }; + uint16_t mtu_len = 0; + + mtu_len = bvlc6_encode_result(&mtu[0], sizeof(mtu), vmac_src, result_code); + + return bip6_send_mpdu(dest_addr, mtu, mtu_len); +} + +/** + * The Address Resolution Ack send function for BACnet/IPv6 application layer + * + * @param dest_addr - Points to a #BACNET_IP6_ADDRESS structure containing the + * destination IPv6 address. + * @param vmac_src - Source-Virtual-Address + * @param vmac_dst - Destination-Virtual-Address + * + * @return Upon successful completion, returns the number of bytes sent. + * Otherwise, -1 shall be returned and errno set to indicate the error. + */ +static int bvlc6_send_address_resolution_ack( + BACNET_IP6_ADDRESS *dest_addr, + uint32_t vmac_src, + uint32_t vmac_dst) +{ + uint8_t mtu[MAX_MPDU] = { 0 }; + uint16_t mtu_len = 0; + + mtu_len = bvlc6_encode_address_resolution_ack( + &mtu[0], sizeof(mtu), + vmac_src, vmac_dst); + + return bip6_send_mpdu(dest_addr, mtu, mtu_len); +} + +/** + * The Virtual Address Resolution Ack send function for BACnet/IPv6 + * application layer + * + * @param dest_addr - Points to a #BACNET_IP6_ADDRESS structure containing the + * destination IPv6 address. + * @param vmac_src - Source-Virtual-Address + * @param vmac_dst - Destination-Virtual-Address + * + * @return Upon successful completion, returns the number of bytes sent. + * Otherwise, -1 shall be returned and errno set to indicate the error. + */ +static int bvlc6_send_virtual_address_resolution_ack( + BACNET_IP6_ADDRESS *dest_addr, + uint32_t vmac_src, + uint32_t vmac_dst) +{ + uint8_t mtu[MAX_MPDU] = { 0 }; + uint16_t mtu_len = 0; + + mtu_len = bvlc6_encode_virtual_address_resolution_ack( + &mtu[0], sizeof(mtu), + vmac_src, vmac_dst); + + return bip6_send_mpdu(dest_addr, mtu, mtu_len); +} + +/** + * Handler for Virtual-Address-Resolution + * + * @param addr - BACnet/IPv6 source address any NAK or reply back to. + * @param pdu - The received NPDU+APDU buffer. + * @param pdu_len - How many bytes in NPDU+APDU buffer. + */ +static void bbmd6_virtual_address_resolution_handler( + BACNET_IP6_ADDRESS *addr, + uint8_t * pdu, + uint16_t pdu_len) +{ + int function_len = 0; + uint32_t vmac_src = 0; + uint32_t vmac_me = 0; + + if (addr && pdu) { + debug_printf("BIP6: Received Virtual-Address-Resolution.\n"); + if (bbmd6_address_match_self(addr)) { + /* ignore messages from my IPv6 address */ + } else { + function_len = bvlc6_decode_virtual_address_resolution( + pdu, pdu_len, + &vmac_src); + if (function_len) { + bbmd6_add_vmac(vmac_src, addr); + /* The Address-Resolution-ACK message is unicast + to the B/IPv6 node that originally initiated + the Address-Resolution message. */ + vmac_me = Device_Object_Instance_Number(); + bvlc6_send_virtual_address_resolution_ack( + addr, vmac_me, vmac_src); + } + } + } +} + +/** + * Handler for Virtual-Address-Resolution-ACK + * + * @param addr - BACnet/IPv6 source address any NAK or reply back to. + * @param pdu - The received NPDU+APDU buffer. + * @param pdu_len - How many bytes in NPDU+APDU buffer. + */ +static void bbmd6_virtual_address_resolution_ack_handler( + BACNET_IP6_ADDRESS *addr, + uint8_t * pdu, + uint16_t pdu_len) +{ + int function_len = 0; + uint32_t vmac_src = 0; + uint32_t vmac_dst = 0; + + if (addr && pdu) { + debug_printf("BIP6: Received Virtual-Address-Resolution-ACK.\n"); + if (bbmd6_address_match_self(addr)) { + /* ignore messages from my IPv6 address */ + } else { + function_len = bvlc6_decode_virtual_address_resolution_ack( + pdu, pdu_len, + &vmac_src, &vmac_dst); + if (function_len) { + bbmd6_add_vmac(vmac_src, addr); + } + } + } +} + +/** + * Handler for Address-Resolution + * + * @param addr - BACnet/IPv6 source address any NAK or reply back to. + * @param pdu - The received NPDU+APDU buffer. + * @param pdu_len - How many bytes in NPDU+APDU buffer. + */ +static void bbmd6_address_resolution_handler( + BACNET_IP6_ADDRESS *addr, + uint8_t * pdu, + uint16_t pdu_len) +{ + int function_len = 0; + uint32_t vmac_src = 0; + uint32_t vmac_target = 0; + uint32_t vmac_me = 0; + + if (addr && pdu) { + debug_printf("BIP6: Received Address-Resolution.\n"); + if (bbmd6_address_match_self(addr)) { + /* ignore messages from my IPv6 address */ + } else { + function_len = bvlc6_decode_address_resolution( + pdu, pdu_len, + &vmac_src, &vmac_target); + if (function_len) { + bbmd6_add_vmac(vmac_src, addr); + vmac_me = Device_Object_Instance_Number(); + if (vmac_target == vmac_me) { + /* The Address-Resolution-ACK message is unicast + to the B/IPv6 node that originally initiated + the Address-Resolution message. */ + bvlc6_send_address_resolution_ack( + addr, vmac_me, vmac_src); + } + } + } + } +} + +/** + * Handler for Address-Resolution-ACK + * + * @param addr - BACnet/IPv6 source address any NAK or reply back to. + * @param pdu - The received NPDU+APDU buffer. + * @param pdu_len - How many bytes in NPDU+APDU buffer. + */ +static void bbmd6_address_resolution_ack_handler( + BACNET_IP6_ADDRESS *addr, + uint8_t * pdu, + uint16_t pdu_len) +{ + int function_len = 0; + uint32_t vmac_src = 0; + uint32_t vmac_dst = 0; + + if (addr && pdu) { + debug_printf("BIP6: Received Address-Resolution-ACK.\n"); + if (bbmd6_address_match_self(addr)) { + /* ignore messages from my IPv6 address */ + } else { + function_len = bvlc6_decode_address_resolution_ack( + pdu, pdu_len, + &vmac_src, &vmac_dst); + if (function_len) { + bbmd6_add_vmac(vmac_src, addr); + } + } + } +} + +/** + * Use this handler when you are not a BBMD. + * Sets the BVLC6_Function_Code in case it is needed later. + * + * @param addr - BACnet/IPv6 source address any NAK or reply back to. + * @param src - BACnet style the source address interpreted VMAC + * @param mtu - The received MTU buffer. + * @param mtu_len - How many bytes in MTU buffer. + * + * @return number of bytes offset into the NPDU for APDU, or 0 if handled + */ +static int handler_bbmd6_for_non_bbmd( + BACNET_IP6_ADDRESS *addr, + BACNET_ADDRESS * src, + uint8_t * mtu, + uint16_t mtu_len) +{ + uint16_t result_code = BVLC6_RESULT_SUCCESSFUL_COMPLETION; + uint32_t vmac_src = 0; + uint32_t vmac_dst = 0; + uint8_t message_type = 0; + uint16_t message_length = 0; + int header_len = 0; + int function_len = 0; + uint8_t * pdu = NULL; + uint16_t pdu_len = 0; + uint16_t npdu_len = 0; + bool send_result = false; + uint16_t offset = 0; + BACNET_IP6_ADDRESS fwd_address = {{0}}; + + header_len = bvlc6_decode_header(mtu, mtu_len, &message_type, + &message_length); + if (header_len == 4) { + BVLC6_Function_Code = message_type; + pdu = &mtu[header_len]; + pdu_len = mtu_len - header_len; + switch (BVLC6_Function_Code) { + case BVLC6_RESULT: + function_len = bvlc6_decode_result(pdu, pdu_len, &vmac_src, + &result_code); + if (function_len) { + BVLC6_Result_Code = result_code; + /* The Virtual MAC address table shall be updated + using the respective parameter values of the + incoming messages. */ + bbmd6_add_vmac(vmac_src, addr); + bvlc6_vmac_address_set(src, vmac_src); + debug_printf("BIP6: Received Result Code=%d\n", + BVLC6_Result_Code); + } + break; + case BVLC6_REGISTER_FOREIGN_DEVICE: + result_code = BVLC6_RESULT_REGISTER_FOREIGN_DEVICE_NAK; + send_result = true; + break; + case BVLC6_DELETE_FOREIGN_DEVICE: + result_code = BVLC6_RESULT_DELETE_FOREIGN_DEVICE_NAK; + send_result = true; + break; + case BVLC6_DISTRIBUTE_BROADCAST_TO_NETWORK: + result_code = BVLC6_RESULT_DISTRIBUTE_BROADCAST_TO_NETWORK_NAK; + send_result = true; + break; + case BVLC6_ORIGINAL_UNICAST_NPDU: + /* This message is used to send directed NPDUs to + another B/IPv6 node or router. */ + debug_printf("BIP6: Received Original-Unicast-NPDU.\n"); + if (bbmd6_address_match_self(addr)) { + /* ignore messages from my IPv6 address */ + offset = 0; + } else { + function_len = bvlc6_decode_original_unicast( + pdu, pdu_len, + &vmac_src, &vmac_dst, + NULL, 0, &npdu_len); + if (function_len) { + if (vmac_dst == Device_Object_Instance_Number()) { + /* The Virtual MAC address table shall be updated + using the respective parameter values of the + incoming messages. */ + bbmd6_add_vmac(vmac_src, addr); + bvlc6_vmac_address_set(src, vmac_src); + offset = header_len + (function_len - npdu_len); + } + } + } + break; + case BVLC6_ORIGINAL_BROADCAST_NPDU: + debug_printf("BIP6: Received Original-Broadcast-NPDU.\n"); + if (bbmd6_address_match_self(addr)) { + /* ignore messages from my IPv6 address */ + offset = 0; + } else { + function_len = bvlc6_decode_original_broadcast( + pdu, pdu_len, + &vmac_src, + NULL, 0, &npdu_len); + if (function_len) { + /* The Virtual MAC address table shall be updated + using the respective parameter values of the + incoming messages. */ + bbmd6_add_vmac(vmac_src, addr); + bvlc6_vmac_address_set(src, vmac_src); + offset = header_len + (function_len - npdu_len); + } + } + break; + case BVLC6_FORWARDED_NPDU: + debug_printf("BIP6: Received Forwarded-NPDU.\n"); + if (bbmd6_address_match_self(addr)) { + /* ignore messages from my IPv6 address */ + offset = 0; + } else { + function_len = bvlc6_decode_forwarded_npdu( + pdu, pdu_len, + &vmac_src, &fwd_address, + NULL, 0, &npdu_len); + if (function_len) { + /* The Virtual MAC address table shall be updated + using the respective parameter values of the + incoming messages. */ + bbmd6_add_vmac(vmac_src, &fwd_address); + bvlc6_vmac_address_set(src, vmac_src); + offset = header_len + (function_len - npdu_len); + } + } + break; + case BVLC6_FORWARDED_ADDRESS_RESOLUTION: + result_code = BVLC6_RESULT_ADDRESS_RESOLUTION_NAK; + send_result = true; + break; + case BVLC6_ADDRESS_RESOLUTION: + bbmd6_address_resolution_handler(addr, pdu, pdu_len); + break; + case BVLC6_ADDRESS_RESOLUTION_ACK: + bbmd6_address_resolution_ack_handler(addr, pdu, pdu_len); + break; + case BVLC6_VIRTUAL_ADDRESS_RESOLUTION: + bbmd6_virtual_address_resolution_handler(addr, pdu, pdu_len); + break; + case BVLC6_VIRTUAL_ADDRESS_RESOLUTION_ACK: + bbmd6_virtual_address_resolution_ack_handler(addr, pdu, pdu_len); + break; + case BVLC6_SECURE_BVLL: + break; + default: + break; + } + if (send_result) { + vmac_src = Device_Object_Instance_Number(); + bvlc6_send_result(addr, vmac_src, result_code); + debug_printf("BIP6: sent result code=%d\n", result_code); + } + } + + return offset; +} + +#if defined(BACDL_BIP6) && BBMD6_ENABLED +#warning FIXME: Needs ported to IPv6 +/** + * Use this handler when you are a BBMD. + * Sets the BVLC6_Function_Code in case it is needed later. + * + * @param addr - BACnet/IPv6 source address any NAK or reply back to. + * @param src - BACnet style the source address interpreted VMAC + * @param mtu - The received MTU buffer. + * @param mtu_len - How many bytes in MTU buffer. + * + * @return number of bytes offset into the NPDU for APDU, or 0 if handled + */ +static int handler_bbmd6_for_bbmd( + BACNET_IP6_ADDRESS *addr, + BACNET_ADDRESS * src, + uint8_t * mtu, + uint16_t mtu_len) +{ + uint16_t result_code = BVLC6_RESULT_SUCCESSFUL_COMPLETION; + uint32_t vmac_me = 0; + uint32_t vmac_src = 0; + uint32_t vmac_dst = 0; + uint32_t vmac_target = 0; + uint8_t message_type = 0; + uint16_t message_length = 0; + int header_len = 0; + int function_len = 0; + uint8_t * pdu = NULL; + uint16_t pdu_len = 0; + uint8_t * npdu = NULL; + uint16_t npdu_len = 0; + bool send_result = false; + uint16_t offset = 0; + BACNET_IP6_ADDRESS fwd_address = {{0}}; + + header_len = bvlc6_decode_header(mtu, mtu_len, &message_type, + &message_length); + if (header_len == 4) { + BVLC6_Function_Code = message_type; + pdu = &mtu[header_len]; + pdu_len = mtu_len - header_len; + switch (BVLC6_Function_Code) { + case BVLC6_RESULT: + function_len = bvlc6_decode_result(pdu, pdu_len, &vmac_src, + &result_code); + if (function_len) { + BVLC6_Result_Code = result_code; + /* The Virtual MAC address table shall be updated + using the respective parameter values of the + incoming messages. */ + bbmd6_add_vmac(vmac_src, addr); + bvlc6_vmac_address_set(src, vmac_src); + debug_printf("BIP6: Received Result Code=%d\n", + BVLC6_Result_Code); + } + break; + case BVLC6_REGISTER_FOREIGN_DEVICE: + result_code = BVLC6_RESULT_REGISTER_FOREIGN_DEVICE_NAK; + send_result = true; + break; + case BVLC6_DELETE_FOREIGN_DEVICE: + result_code = BVLC6_RESULT_DELETE_FOREIGN_DEVICE_NAK; + send_result = true; + break; + case BVLC6_DISTRIBUTE_BROADCAST_TO_NETWORK: + result_code = BVLC6_RESULT_DISTRIBUTE_BROADCAST_TO_NETWORK_NAK; + send_result = true; + break; + case BVLC6_ORIGINAL_UNICAST_NPDU: + /* This message is used to send directed NPDUs to + another B/IPv6 node or router. */ + debug_printf("BIP6: Received Original-Unicast-NPDU.\n"); + if (bbmd6_address_match_self(addr)) { + /* ignore messages from my IPv6 address */ + offset = 0; + } else { + function_len = bvlc6_decode_original_unicast( + pdu, pdu_len, + &vmac_src, &vmac_dst, + NULL, 0, &npdu_len); + if (function_len) { + if (vmac_dst == Device_Object_Instance_Number()) { + /* The Virtual MAC address table shall be updated + using the respective parameter values of the + incoming messages. */ + bbmd6_add_vmac(vmac_src, addr); + bvlc6_vmac_address_set(src, vmac_src); + offset = header_len + (function_len - npdu_len); + } + } + } + break; + case BVLC6_ORIGINAL_BROADCAST_NPDU: + debug_printf("BIP6: Received Original-Broadcast-NPDU.\n"); + function_len = bvlc6_decode_original_broadcast( + pdu, pdu_len, + &vmac_src, + NULL, 0, &npdu_len); + if (function_len) { + offset = header_len + (function_len - npdu_len); + npdu = &mtu[offset]; + /* Upon receipt of a BVLL Original-Broadcast-NPDU + message from the local multicast domain, a BBMD + shall construct a BVLL Forwarded-NPDU message and + unicast it to each entry in its BDT. In addition, + the constructed BVLL Forwarded-NPDU message shall + be unicast to each foreign device currently in + the BBMD's FDT */ + BVLC6_Buffer_Len = bvlc6_encode_forwarded_npdu( + &BVLC6_Buffer[0], sizeof(BVLC6_Buffer), + vmac_src, addr, + npdu, npdu_len); + bbmd6_send_pdu_bdt(&BVLC6_Buffer[0], BVLC6_Buffer_Len); + bbmd6_send_pdu_fdt(&BVLC6_Buffer[0], BVLC6_Buffer_Len); + if (!bbmd6_address_match_self(addr)) { + /* The Virtual MAC address table shall be updated + using the respective parameter values of the + incoming messages. */ + bbmd6_add_vmac(vmac_src, addr); + bvlc6_vmac_address_set(src, vmac_src); + } + } + break; + case BVLC6_FORWARDED_NPDU: + debug_printf("BIP6: Received Forwarded-NPDU.\n"); + function_len = bvlc6_decode_forwarded_npdu( + pdu, pdu_len, + &vmac_src, &fwd_address, + NULL, 0, &npdu_len); + if (function_len) { + offset = header_len + (function_len - npdu_len); + npdu = &mtu[offset]; + /* Upon receipt of a BVLL Forwarded-NPDU message + from a BBMD which is in the receiving BBMD's BDT, + a BBMD shall construct a BVLL Forwarded-NPDU and + transmit it via multicast to B/IPv6 devices in the + local multicast domain. */ + BVLC6_Buffer_Len = bvlc6_encode_forwarded_npdu( + &BVLC6_Buffer[0], sizeof(BVLC6_Buffer), + vmac_src, addr, + npdu, npdu_len); + bip6_get_broadcast_addr(&bvlc_dest); + bip6_send_mpdu(&bvlc_dest, &BVLC6_Buffer[0], BVLC6_Buffer_Len); + /* In addition, the constructed BVLL Forwarded-NPDU + message shall be unicast to each foreign device in + the BBMD's FDT. If the BBMD is unable to transmit + the Forwarded-NPDU, or the message was not received + from a BBMD which is in the receiving BBMD's BDT, + no BVLC-Result shall be returned and the message + shall be discarded. */ + bbmd6_send_pdu_fdt(&BVLC6_Buffer[0], BVLC6_Buffer_Len); + if (!bbmd6_address_match_self(addr)) { + /* The Virtual MAC address table shall be updated + using the respective parameter values of the + incoming messages. */ + bbmd6_add_vmac(vmac_src, &fwd_address); + bvlc6_vmac_address_set(src, vmac_src); + } + } + break; + case BVLC6_FORWARDED_ADDRESS_RESOLUTION: + result_code = BVLC6_RESULT_ADDRESS_RESOLUTION_NAK; + send_result = true; + break; + case BVLC6_ADDRESS_RESOLUTION: + bbmd6_address_resolution_handler(addr, pdu, pdu_len); + break; + case BVLC6_ADDRESS_RESOLUTION_ACK: + bbmd6_address_resolution_ack_handler(addr, pdu, pdu_len); + break; + case BVLC6_VIRTUAL_ADDRESS_RESOLUTION: + bbmd6_virtual_address_resolution_handler(addr, pdu, pdu_len); + break; + case BVLC6_VIRTUAL_ADDRESS_RESOLUTION_ACK: + bbmd6_virtual_address_resolution_ack_handler(addr, pdu, pdu_len); + break; + case BVLC6_SECURE_BVLL: + break; + default: + break; + } + if (send_result) { + vmac_src = Device_Object_Instance_Number(); + bvlc6_send_result(addr, vmac_src, result_code); + debug_printf("BIP6: sent result code=%d\n", result_code); + } + } + + return offset; +} +#endif + +/** + * Use this handler for BACnet/IPv6 BVLC + * + * @param addr [in] IPv6 address to send any NAK back to. + * @param src [out] returns the source address + * @param npdu [in] The received buffer. + * @param npdu_len [in] How many bytes in npdu[]. + * + * @return number of bytes offset into the NPDU for APDU, or 0 if handled + */ +int bvlc6_handler( + BACNET_IP6_ADDRESS *addr, + BACNET_ADDRESS * src, + uint8_t * npdu, + uint16_t npdu_len) +{ +#if defined(BACDL_BIP6) && BBMD6_ENABLED + return handler_bbmd6_for_bbmd(addr, src, npdu, npdu_len); +#else + return handler_bbmd6_for_non_bbmd(addr, src, npdu, npdu_len); +#endif +} + +/** Register as a foreign device with the indicated BBMD. + * @param bbmd_address - IPv4 address (long) of BBMD to register with, + * in network byte order. + * @param bbmd_port - Network port of BBMD, in network byte order + * @param time_to_live_seconds - Lease time to use when registering. + * @return Positive number (of bytes sent) on success, + * 0 if no registration request is sent, or + * -1 if registration fails. + */ +int bvlc6_register_with_bbmd( + BACNET_IP6_ADDRESS *bbmd_addr, + uint32_t vmac_src, + uint16_t time_to_live_seconds) +{ + uint8_t mtu[MAX_MPDU] = { 0 }; + uint16_t mtu_len = 0; + + mtu_len = bvlc6_encode_register_foreign_device( + &mtu[0], sizeof(mtu), vmac_src, time_to_live_seconds); + + return bip6_send_mpdu(bbmd_addr, &mtu[0], mtu_len); +} + +/** Returns the last BVLL Result we received, either as the result of a BBMD + * request we sent, or (if not a BBMD or Client), from trying to register + * as a foreign device. + * + * @return BVLC6_RESULT_SUCCESSFUL_COMPLETION on success, + * BVLC6_RESULT_REGISTER_FOREIGN_DEVICE_NAK if registration failed, + * or one of the other codes (if we are a BBMD). + */ +uint16_t bvlc6_get_last_result( + void) +{ + return BVLC6_Result_Code; +} + +/** Returns the current BVLL Function Code we are processing. + * We have to store this higher layer code for when the lower layers + * need to know what it is, especially to differentiate between + * BVLC6_ORIGINAL_UNICAST_NPDU and BVLC6_ORIGINAL_BROADCAST_NPDU. + * + * @return A BVLC6_ code, such as BVLC6_ORIGINAL_UNICAST_NPDU. + */ +uint8_t bvlc6_get_function_code( + void) +{ + return BVLC6_Function_Code; +} + +void bvlc6_init(void) +{ + VMAC_Init(); +} + +#ifdef TEST +#include +#include +#include "ctest.h" +static uint32_t Device_ID = 0; +static uint32_t Test_Device_ID = 12345; +static BACNET_IP6_ADDRESS BIP6_Addr; +static BACNET_IP6_ADDRESS Test_BIP6_Addr; +static BACNET_IP6_ADDRESS BIP6_Broadcast_Addr; +static uint8_t BIP6_MTU_Buffer[MAX_MPDU]; + +/* network stub functions */ +/** + * 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) +{ + return 0; +} + +/** + * 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) +{ + return 0; +} + +/** Return the Object Instance number for our (single) Device Object. + * This is a key function, widely invoked by the handler code, since + * it provides "our" (ie, local) address. + * + * @return The Instance number used in the BACNET_OBJECT_ID for the Device. + */ +uint32_t Device_Object_Instance_Number( + void) +{ + return Device_ID; +} + +/** + * Get the BACnet/IP address + * + * @return BACnet/IP address + */ +bool bip6_get_addr( + BACNET_IP6_ADDRESS *addr) +{ + return bvlc6_address_copy(addr, &BIP6_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); +} + +static void test_BBMD_Result( + Test * pTest) +{ + int result = 0; + uint32_t vmac_src = 0x1234; + uint16_t result_code[6] = { + BVLC6_RESULT_SUCCESSFUL_COMPLETION, + BVLC6_RESULT_ADDRESS_RESOLUTION_NAK, + BVLC6_RESULT_VIRTUAL_ADDRESS_RESOLUTION_NAK, + BVLC6_RESULT_REGISTER_FOREIGN_DEVICE_NAK, + BVLC6_RESULT_DELETE_FOREIGN_DEVICE_NAK, + BVLC6_RESULT_DISTRIBUTE_BROADCAST_TO_NETWORK_NAK + }; + uint16_t test_result_code = 0; + uint8_t test_function_code = 0; + BACNET_IP6_ADDRESS addr; + BACNET_ADDRESS src; + unsigned int i = 0; + uint8_t mtu[MAX_MPDU] = { 0 }; + uint16_t mtu_len = 0; + + bvlc6_address_set(&addr, + BIP6_MULTICAST_LINK_LOCAL, 0, 0, 0, 0, 0, 0, + BIP6_MULTICAST_GROUP_ID); + addr.port = 0xBAC0; + bvlc6_vmac_address_set(&src, vmac_src); + for (i = 0; i < 6; i++) { + mtu_len = bvlc6_encode_result(&mtu[0], sizeof(mtu), + vmac_src, result_code[i]); + result = handler_bbmd6_for_non_bbmd(&addr, &src, &mtu[0], mtu_len); + /* validate that the result is handled (0) */ + ct_test(pTest, result == 0); + test_result_code = bvlc6_get_last_result(); + ct_test(pTest, test_result_code == result_code[i]); + test_function_code = bvlc6_get_function_code(); + ct_test(pTest, test_function_code == BVLC6_RESULT); + } +} + +static void test_BBMD6( + Test * pTest) +{ + bool rc; + + /* individual tests */ + rc = ct_addTestFunction(pTest, test_BBMD_Result); + assert(rc); +} + +#ifdef TEST_BBMD6 +int main( + void) +{ + Test *pTest; + + pTest = ct_create("BACnet Broadcast Management Device IP/v6", NULL); + test_BBMD6(pTest); + /* configure output */ + ct_setStream(pTest, stdout); + ct_run(pTest); + (void) ct_report(pTest); + ct_destroy(pTest); + + return 0; +} +#endif +#endif + + diff --git a/bacnet-stack/demo/router-ipv6/Makefile b/bacnet-stack/demo/router-ipv6/Makefile new file mode 100644 index 00000000..920c6887 --- /dev/null +++ b/bacnet-stack/demo/router-ipv6/Makefile @@ -0,0 +1,53 @@ +#Makefile to build BACnet Application for the GCC port + +# tools - only if you need them. +# Most platforms have this already defined +# CC = gcc + +# Executable file name +TARGET = bacroute + +TARGET_BIN = ${TARGET}$(TARGET_EXT) + +CFLAGS += -DPRINT_ENABLED=1 + +BACNET_SOURCE_DIR = ../../src +BACNET_HANDLER_DIR = ../handler +BACNET_OBJECT_DIR = ../object + +SRC = main.c \ + $(BACNET_OBJECT_DIR)/device-client.c + +PORT_BIP6_SRC = \ + $(BACNET_PORT_DIR)/bip6.c \ + $(BACNET_SOURCE_DIR)/bvlc6.c \ + $(BACNET_HANDLER_DIR)/h_bbmd6.c \ + $(BACNET_SOURCE_DIR)/vmac.c + +SRCS = ${SRC} ${PORT_BIP6_SRC} + +OBJS = ${SRCS:.c=.o} + +all: ${BACNET_LIB_TARGET} Makefile ${TARGET_BIN} + +${TARGET_BIN}: ${OBJS} Makefile ${BACNET_LIB_TARGET} + ${CC} ${PFLAGS} ${OBJS} ${LFLAGS} -o $@ + size $@ + cp $@ ../../bin + +lib: ${BACNET_LIB_TARGET} + +${BACNET_LIB_TARGET}: + ( cd ${BACNET_LIB_DIR} ; $(MAKE) clean ; $(MAKE) ) + +.c.o: + ${CC} -c ${CFLAGS} $*.c -o $@ + +depend: + rm -f .depend + ${CC} -MM ${CFLAGS} *.c >> .depend + +clean: + rm -f core ${TARGET_BIN} ${OBJS} ${BACNET_LIB_TARGET} $(TARGET).map + +include: .depend diff --git a/bacnet-stack/demo/router-ipv6/main.c b/bacnet-stack/demo/router-ipv6/main.c new file mode 100644 index 00000000..aec58423 --- /dev/null +++ b/bacnet-stack/demo/router-ipv6/main.c @@ -0,0 +1,1247 @@ +/** +* @file +* @author Steve Karg +* @date 2016 +* @brief Simple BACnet/IP to BACnet/IPv6 router +* +* @section LICENSE +* +* Copyright (C) 2016 Steve Karg +* +* Permission is hereby granted, free of charge, to any person obtaining +* a copy of this software and associated documentation files (the +* "Software"), to deal in the Software without restriction, including +* without limitation the rights to use, copy, modify, merge, publish, +* distribute, sublicense, and/or sell copies of the Software, and to +* permit persons to whom the Software is furnished to do so, subject to +* the following conditions: +* +* The above copyright notice and this permission notice shall be included +* in all copies or substantial portions of the Software. +* +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY +* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, +* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE +* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +* +*/ +#include +#include +#include +#include +#include +#include +#include + +#include "bacdef.h" +#include "config.h" +#include "debug.h" +#include "bactext.h" +#include "bacerror.h" +#include "iam.h" +#include "arf.h" +#include "tsm.h" +#include "address.h" +#include "npdu.h" +#include "apdu.h" +#include "client.h" +#include "net.h" +#include "version.h" + +/* our datalink layers */ +#include "bvlc6.h" +#include "bip6.h" +#undef MAX_HEADER +#undef MAX_MPDU +#include "bip.h" +#include "bvlc.h" + +/** +* 6.6.1 Routing Tables +* +* By definition, a router is a device that is connected to at least +* two BACnet networks. Each attachment is through a "port." A +* "routing table" consists of the following information for each port: +* (a) the MAC address of the port's connection to its network; +* (b) the 2-octet network number of the directly connected network; +* (c) a list of network numbers reachable through the port along +* with the MAC address of the next router on the path to each +* network number and the reachability status of each such network. +* +* The "reachability status" is an implementation-dependent value +* that indicates whether the associated network is able to +* receive traffic. The reachability status shall be able to +* distinguish, at a minimum, between "permanent" failures of a route, +* such as might result from the failure of a router, and "temporary" +* unreachability due to the imposition of a congestion control +* restriction. +*/ +typedef struct _dnet { + uint8_t mac[MAX_MAC_LEN]; + uint8_t mac_len; + uint16_t net; + bool enabled; + struct _dnet *dnets; + struct _dnet *next; +} DNET; +/* The list of DNETs that our router can reach. */ +static DNET *Router_Table_Head; +/* track our directly connected ports network number */ +static uint16_t BIP_Net; +static uint16_t BIP6_Net; +/* buffer for receiving packets from the directly connected ports */ +static uint8_t BIP_Rx_Buffer[MAX_MPDU]; +static uint8_t BIP6_Rx_Buffer[MAX_MPDU]; +/* buffer for transmitting from any port */ +static uint8_t Tx_Buffer[MAX_MPDU]; +/* main loop exit control */ +static bool Exit_Requested; + +/** + * Search the router table to find a matching DNET entry + * + * @param net - network number to find a match + * @param addr - address to be filled with remote router address + * + * @return NULL if not found, or a pointer to the directly connected port. + * If addr is not NULL, the DNET entry address is copied to addr + * The caller will need to compare the sought after net with the + * returned port->net to determine if the addr is filled. + */ +static DNET *dnet_find( + uint16_t net, + BACNET_ADDRESS * addr) +{ + + DNET *port = Router_Table_Head; + DNET *dnet = NULL; + unsigned int i = 0; + + while (port != NULL) { + if (net == port->net) { + /* DNET is directly connected to the router */ + return port; + } else if (port->dnets) { + /* search router ports DNET list */ + dnet = port->dnets; + while (dnet != NULL) { + if (net == dnet->net) { + if (addr) { + addr->mac_len = dnet->mac_len; + for (i = 0; i < MAX_MAC_LEN; i++) { + addr->mac[i] = dnet->mac[i]; + } + } + return port; + } + dnet = dnet->next; + } + } + port = port->next; + } + + return NULL; +} + +static bool port_find( + uint16_t snet, + BACNET_ADDRESS *addr) +{ + DNET *port = NULL; + bool found = false; + unsigned int i = 0; + + port = Router_Table_Head; + while (port) { + if (port->net == snet) { + if (addr) { + addr->mac_len = port->mac_len; + for (i = 0; i < MAX_MAC_LEN; i++) { + addr->mac[i] = port->mac[i]; + } + } + found = true; + break; + } + port = port->next; + } + + return found; +} + +/** + * Add a directly connected port to the router table + * + * @param snet - router port SNET + * @param addr - address of port at the net to be added + */ +static void port_add( + uint16_t snet, + BACNET_ADDRESS *addr) +{ + DNET *port = NULL; + DNET *dnet = NULL; + unsigned int i = 0; + + port = dnet_find(snet, NULL); + if (!port) { + port = Router_Table_Head; + if (!port) { + /* create first port */ + port = (DNET *) calloc(1, sizeof(DNET)); + assert(port); + Router_Table_Head = port; + } else { + while (port) { + if (port->next) { + port = port->next; + } else { + /* create next port */ + dnet = (DNET *) calloc(1, sizeof(DNET)); + assert(dnet); + port->next = dnet; + port = port->next; + break; + } + } + } + port->net = snet; + if (addr) { + port->mac_len = addr->mac_len; + for (i = 0; i < MAX_MAC_LEN; i++) { + port->mac[i] = addr->mac[i]; + } + } else { + port->mac_len = 0; + } + port->enabled = true; + } +} + +/** + * Add a route to the router table + * + * @param snet - router port SNET + * @param net - net to be added + * @param addr - address of router at the net to be added + */ +static void dnet_add( + uint16_t snet, + uint16_t net, + BACNET_ADDRESS *addr) +{ + + DNET *dnet = NULL; + DNET *port = NULL; + DNET *prior_dnet = NULL; + unsigned int i = 0; + + /* make sure NETs are not repeated */ + dnet = dnet_find(net, NULL); + if (dnet) { + return; + } + /* start with the source network number table */ + port = dnet_find(snet, NULL); + if (!port) { + return; + } + dnet = port->dnets; + if (dnet == NULL) { + /* first DNET to add */ + dnet = (DNET *) calloc(1, sizeof(DNET)); + assert(dnet); + port->dnets = dnet; + if (addr) { + dnet->mac_len = addr->mac_len; + for (i = 0; i < MAX_MAC_LEN; i++) { + dnet->mac[i] = addr->mac[i]; + } + } + dnet->net = net; + dnet->enabled = true; + dnet->next = NULL; + } else { + while (dnet != NULL) { + if (dnet->net == net) { + /* make sure NETs are not repeated */ + return; + } + prior_dnet = dnet; + dnet = dnet->next; + } + /* next DNET to add */ + dnet = (DNET *) calloc(1, sizeof(DNET)); + if (addr) { + dnet->mac_len = addr->mac_len; + for (i = 0; i < MAX_MAC_LEN; i++) { + dnet->mac[i] = addr->mac[i]; + } + } + dnet->net = net; + dnet->enabled = true; + dnet->next = NULL; + prior_dnet->next = dnet; + } +} + +/** + * Free the DNET data of a route + * + * @param dnets - router info to be freed + */ +static void dnet_cleanup( + DNET * dnets) +{ + DNET *dnet = dnets; + while (dnet != NULL) { + debug_printf("DNET %u removed\n", (unsigned)dnet->net); + dnet = dnet->next; + free(dnets); + dnets = dnet; + } +} + +/** + * Initialize the a data link broadcast address + * + * @param dest - address to be filled with broadcast designator + */ +static void datalink_get_broadcast_address( + BACNET_ADDRESS * dest) +{ + if (dest) { + dest->mac_len = 0; + dest->net = BACNET_BROADCAST_NETWORK; + dest->len = 0; + } + + return; +} + +/** + * function to send a packet out the BACnet/IP and BACnet/IPv6 ports + * + * @param snet - network number of the directly connected port to send + * @param dest - address to where packet is sent + * @param npdu_data - NPCI data to control network destination + * @param pdu - protocol data unit to be sent + * @param pdu_len - number of bytes to send + * + * @return number of bytes sent + */ +static int datalink_send_pdu( + uint16_t snet, + BACNET_ADDRESS * dest, + BACNET_NPDU_DATA * npdu_data, + uint8_t * pdu, + unsigned int pdu_len) +{ + int bytes_sent = 0; + + if (snet == 0) { + debug_printf("BVLC/BVLC6 Send to DNET %u\n", (unsigned)dest->net); + bytes_sent = bvlc_send_pdu(dest, npdu_data, pdu, pdu_len); + bytes_sent = bip6_send_pdu(dest, npdu_data, pdu, pdu_len); + } else if (snet == BIP_Net) { + debug_printf("BVLC Send to DNET %u\n", (unsigned)dest->net); + bytes_sent = bvlc_send_pdu(dest, npdu_data, pdu, pdu_len); + } else if (snet == BIP6_Net) { + debug_printf("BVLC6 Send to DNET %u\n",(unsigned)dest->net); + bytes_sent = bip6_send_pdu(dest, npdu_data, pdu, pdu_len); + } + + return bytes_sent; +} + +/** Initialize an npdu_data structure with given parameters and good defaults, + * and add the Network Layer Message fields. + * The name is a misnomer, as it doesn't do any actual encoding here. + * @see npdu_encode_npdu_data for a simpler version to use when sending an + * APDU instead of a Network Layer Message. + * + * @param npdu_data [out] Returns a filled-out structure with information + * provided by the other arguments and good defaults. + * @param network_message_type [in] The type of Network Layer Message. + * @param data_expecting_reply [in] True if message should have a reply. + * @param priority [in] One of the 4 priorities defined in section 6.2.2, + * like B'11' = Life Safety message + */ +static void npdu_encode_npdu_network( + BACNET_NPDU_DATA * npdu_data, + BACNET_NETWORK_MESSAGE_TYPE network_message_type, + bool data_expecting_reply, + BACNET_MESSAGE_PRIORITY priority) +{ + if (npdu_data) { + npdu_data->data_expecting_reply = data_expecting_reply; + npdu_data->protocol_version = BACNET_PROTOCOL_VERSION; + npdu_data->network_layer_message = true; /* false if APDU */ + npdu_data->network_message_type = network_message_type; /* optional */ + npdu_data->vendor_id = 0; /* optional, if net message type is > 0x80 */ + npdu_data->priority = priority; + npdu_data->hop_count = HOP_COUNT_DEFAULT; + } +} + +/** + * Broadcast an I-am-router-to-network message + * + * @param snet - the directly connected port network number + * @param dnet - the network number we are saying we are a router to. + * If the dnet is 0, send a broadcast out each port with an + * I-Am-Router-To-Network message containing the network + * numbers of each accessible network except the networks + * reachable via the network on which the broadcast is being made. + */ +static void send_i_am_router_to_network(uint16_t snet, uint16_t net) +{ + BACNET_ADDRESS dest; + bool data_expecting_reply = false; + BACNET_NPDU_DATA npdu_data; + int pdu_len = 0; + int len = 0; + DNET *port = NULL; + DNET *dnet = NULL; + + datalink_get_broadcast_address(&dest); + npdu_encode_npdu_network(&npdu_data, + NETWORK_MESSAGE_I_AM_ROUTER_TO_NETWORK, + data_expecting_reply, MESSAGE_PRIORITY_NORMAL); + /* We don't need src information, since a message can't originate from + our downstream BACnet network. */ + pdu_len = + npdu_encode_pdu(&Tx_Buffer[0], &dest, NULL, &npdu_data); + if (net) { + len = encode_unsigned16(&Tx_Buffer[pdu_len], net); + pdu_len += len; + } else { + debug_printf("I-Am-Router-To-Network "); + /* Each router shall broadcast out each port + an I-Am-Router-To-Network message containing the network + numbers of each accessible network except the networks + reachable via the network on which the broadcast is being made. + This enables routers to build or update their routing table + entries for each of the network numbers contained in the message. + */ + port = Router_Table_Head; + while (port != NULL) { + if (port->net != snet) { + debug_printf("%u,", port->net); + len = encode_unsigned16(&Tx_Buffer[pdu_len], + port->net); + pdu_len += len; + dnet = port->dnets; + while (dnet != NULL) { + debug_printf("%u,", dnet->net); + len = encode_unsigned16(&Tx_Buffer[pdu_len], + dnet->net); + pdu_len += len; + dnet = dnet->next; + } + } + port = port->next; + } + debug_printf("from %u\n", snet); + } + datalink_send_pdu(snet, &dest, &npdu_data, &Tx_Buffer[0], pdu_len); +} + +/** Sends our Routing Table, built from our DNET[] array, as an ACK. + * There are two cases here: + * 1) We are responding to a NETWORK_MESSAGE_INIT_RT_TABLE requesting our table. + * We will normally broadcast that response. + * 2) We are ACKing the receipt of a NETWORK_MESSAGE_INIT_RT_TABLE containing a + * routing table, and then we will want to respond to that dst router. + * In that case, DNET[] should just have one entry of -1 (no routing table + * is sent). + * + * @param dest [in] If NULL, Ack will be broadcast to the local BACnet network. + * Optionally may designate a particular router destination, + * especially when ACKing receipt of this message type. + */ +void send_initialize_routing_table_ack( + uint8_t snet, + BACNET_ADDRESS * dst) +{ + BACNET_ADDRESS dest; + bool data_expecting_reply = false; + BACNET_NPDU_DATA npdu_data; + int pdu_len = 0; + int len = 0; + uint8_t count = 0; + uint8_t port_id = 1; + DNET *port = NULL; + + if (dst) { + bacnet_address_copy(&dest, dst); + } else { + datalink_get_broadcast_address(&dest); + } + npdu_encode_npdu_network(&npdu_data, + NETWORK_MESSAGE_INIT_RT_TABLE_ACK, + data_expecting_reply, MESSAGE_PRIORITY_NORMAL); + /* We don't need src information, since a message can't originate from + our downstream BACnet network. */ + pdu_len = + npdu_encode_pdu(&Tx_Buffer[0], &dest, NULL, &npdu_data); + /* First, count the number of Ports we will encode */ + port = Router_Table_Head; + while (port != NULL) { + count++; + port = port->next; + } + Tx_Buffer[pdu_len] = count; + pdu_len++; + if (count > 0) { + /* Now encode each BACNET_ROUTER_PORT. + * We will simply use a positive index for PortID, + * and have no PortInfo. + */ + port = Router_Table_Head; + while (port != NULL) { + len = encode_unsigned16(&Tx_Buffer[pdu_len], port->net); + pdu_len += len; + Tx_Buffer[pdu_len] = port_id; + pdu_len++; + port_id++; + Tx_Buffer[pdu_len] = 0; + pdu_len++; + port = port->next; + } + } + /* Now send the message */ + datalink_send_pdu(snet, &dest, &npdu_data, &Tx_Buffer[0], pdu_len); +} + +/** + * Sends a reject network message + * + * @param snet [in] Which BACnet network orginated the message. + * @param dst [in] If NULL, request will be broadcast to the local BACnet + * network. Otherwise, designates a particular router + * destination. + * @param reject_reason [in] One of the BACNET_NETWORK_REJECT_REASONS codes. + */ +void send_reject_message_to_network( + uint16_t snet, + BACNET_ADDRESS * dst, + uint8_t reject_reason, + uint16_t dnet) +{ + BACNET_ADDRESS dest; + bool data_expecting_reply = false; + BACNET_NPDU_DATA npdu_data; + int pdu_len = 0; + int len = 0; + + if (dst) { + bacnet_address_copy(&dest, dst); + } else { + datalink_get_broadcast_address(&dest); + } + npdu_encode_npdu_network(&npdu_data, + NETWORK_MESSAGE_REJECT_MESSAGE_TO_NETWORK, + data_expecting_reply, MESSAGE_PRIORITY_NORMAL); + /* We don't need src information, since a message can't originate from + our downstream BACnet network. */ + pdu_len = + npdu_encode_pdu(&Tx_Buffer[0], &dest, NULL, &npdu_data); + /* encode the reject reason */ + Tx_Buffer[pdu_len] = reject_reason; + pdu_len++; + if (dnet) { + len = encode_unsigned16(&Tx_Buffer[pdu_len], dnet); + pdu_len += len; + } + /* Now send the message */ + datalink_send_pdu(snet, &dest, &npdu_data, &Tx_Buffer[0], pdu_len); +} + +/** + * Sends a who-is-router-to-network message + * + * @param dnet [in] Which BACnet network we are seeking + */ +static void send_who_is_router_to_network( + uint16_t snet, + uint16_t dnet) +{ + BACNET_ADDRESS dest; + bool data_expecting_reply = false; + BACNET_NPDU_DATA npdu_data; + int pdu_len = 0; + int len = 0; + + datalink_get_broadcast_address(&dest); + npdu_encode_npdu_network(&npdu_data, + NETWORK_MESSAGE_WHO_IS_ROUTER_TO_NETWORK, + data_expecting_reply, MESSAGE_PRIORITY_NORMAL); + pdu_len = + npdu_encode_pdu(&Tx_Buffer[0], &dest, NULL, &npdu_data); + if (dnet) { + len = encode_unsigned16(&Tx_Buffer[pdu_len], dnet); + pdu_len += len; + } + /* Now send the message to port */ + datalink_send_pdu(snet, &dest, &npdu_data, &Tx_Buffer[0], pdu_len); +} + +/** + * Handler to manage the Who-Is-Router-To-Network Message + * + * 6.6.3.2 Who-Is-Router-To-Network + * + * When a router receives a Who-Is-Router-To-Network + * message specifying a particular network number, + * it shall search its routing table for the network number + * contained in the message. If the specified network number + * is found in its table and the port through which it is + * reachable is not the port from which the + * Who-Is-Router-To-Network message was received, the + * router shall construct an I-Am-Router-To-Network message + * containing the specified network number and send it to the node + * that generated the request using a broadcast MAC address, + * thus allowing other nodes on this network to take + * advantage of the routing information. + * + * If the network number is not found in the routing table, the router + * shall attempt to discover the next router on the path to the + * indicated destination network by generating a Who-Is-Router-To-Network + * message containing the specified destination + * network number and broadcasting it out all its ports other + * than the one from which the Who-Is-Router-To-Network message + * arrived. Two cases are possible. In case one the received + * Who-Is-Router-To-Network message was from the originating + * device. For this case, the router shall add SNET and SADR + * fields before broadcasting the subsequent Who-Is-Router-To- + * Network. This permits an I-Could-Be-Router-To-Network message + * to be directed to the originating device. The second case + * is that the received Who-Is-Router-To-Network message came + * from another router and it already contains SNET and SADR + * fields. For this case, the SNET and SADR shall be retained + * in the newly generated Who-Is-Router-To-Network message. + * + * If the Who-Is-Router-To-Network message does not specify a + * particular destination network number, the router shall + * construct an I-Am-Router-To-Network message containing a + * list of all the networks it is able to reach through other than the + * port from which the Who-Is-Router-To-Network message was + * received and transmit it in the same manner as described + * above. The message shall list all networks not flagged as + * permanently unreachable, including those that are temporarily + * unreachable due to the imposition of congestion control restrictions. + * Networks that may be reachable through a PTP + * connection shall be listed only if the connection is currently established. + * + * @param snet [in] source network port number + * @param src [in] The routing source information, if any. + * If src->net and src->len are 0, there is no routing source information. + * @param npdu_data [in] Contains a filled-out structure with information + * decoded from the NCPI and other NPDU bytes. + * @param npdu [in] Buffer containing the rest of the NPDU, following the + * bytes that have already been decoded. + * @param npdu_len [in] The length of the remaining NPDU message in npdu[]. + */ +static void who_is_router_to_network_handler( + uint16_t snet, + BACNET_ADDRESS * src, + BACNET_NPDU_DATA * npdu_data, + uint8_t * npdu, + uint16_t npdu_len) +{ + DNET *port = NULL; + uint16_t network = 0; + uint16_t len = 0; + + if (npdu) { + if (npdu_len >= 2) { + len += decode_unsigned16(&npdu[len], &network); + port = dnet_find(network, NULL); + if (port) { + /* found in my list! */ + if (port->net != snet) { + /* reachable not through the port this message received */ + send_i_am_router_to_network(snet, network); + } + } else { + /* discover the next router on the path to the network */ + port = Router_Table_Head; + while (port) { + if (port->net != snet) { + send_who_is_router_to_network(port->net, network); + } + port = port->next; + } + } + } else { + send_i_am_router_to_network(snet, 0); + } + } +} + +/** + * Handler to manage the Network Layer Control Messages received in a packet. + * This handler is called if the NCPI bit 7 indicates that this packet is a + * network layer message and there is no further DNET to pass it to. + * The NCPI has already been decoded into the npdu_data structure. + * + * @param snet [in] source network port number + * @param src [in] The routing source information, if any. + * If src->net and src->len are 0, there is no routing source information. + * @param npdu_data [in] Contains a filled-out structure with information + * decoded from the NCPI and other NPDU bytes. + * @param npdu [in] Buffer containing the rest of the NPDU, following the + * bytes that have already been decoded. + * @param npdu_len [in] The length of the remaining NPDU message in npdu[]. + */ +static void network_control_handler( + uint16_t snet, + BACNET_ADDRESS * src, + BACNET_NPDU_DATA * npdu_data, + uint8_t * npdu, + uint16_t npdu_len) +{ + uint16_t npdu_offset = 0; + uint16_t dnet = 0; + uint16_t len = 0; + const char *msg_name = NULL; + + msg_name = bactext_network_layer_msg_name(npdu_data->network_message_type); + fprintf(stderr, "Received %s\n", msg_name); + switch (npdu_data->network_message_type) { + case NETWORK_MESSAGE_WHO_IS_ROUTER_TO_NETWORK: + who_is_router_to_network_handler( + snet, src, npdu_data, npdu, npdu_len); + break; + case NETWORK_MESSAGE_I_AM_ROUTER_TO_NETWORK: + /* add its DNETs to our routing table */ + fprintf(stderr, "for Networks: "); + while (npdu_len) { + len = decode_unsigned16(&npdu[npdu_offset], &dnet); + fprintf(stderr, "%hu", dnet); + dnet_add(snet, dnet, src); + npdu_len -= len; + npdu_offset += len; + if (npdu_len) { + fprintf(stderr, ", "); + } + } + fprintf(stderr, ".\n"); + break; + case NETWORK_MESSAGE_I_COULD_BE_ROUTER_TO_NETWORK: + /* Do nothing, same as previous case. */ + break; + case NETWORK_MESSAGE_REJECT_MESSAGE_TO_NETWORK: + if (npdu_len >= 3) { + decode_unsigned16(&npdu[1], &dnet); + fprintf(stderr, "for Network:%hu\n", dnet); + switch (npdu[0]) { + case 0: + fprintf(stderr, "Reason: Other Error.\n"); + break; + case 1: + fprintf(stderr, "Reason: Network unreachable.\n"); + break; + case 2: + fprintf(stderr, "Reason: Network is busy.\n"); + break; + case 3: + fprintf(stderr, "Reason: Unknown network message type.\n"); + break; + case 4: + fprintf(stderr, "Reason: Message too long.\n"); + break; + case 5: + fprintf(stderr, "Reason: Security Error.\n"); + break; + case 6: + fprintf(stderr, "Reason: Invalid address length.\n"); + break; + default: + fprintf(stderr, "Reason: %u\n", (unsigned int)npdu[0]); + break; + } + } + break; + case NETWORK_MESSAGE_ROUTER_BUSY_TO_NETWORK: + case NETWORK_MESSAGE_ROUTER_AVAILABLE_TO_NETWORK: + /* Do nothing - don't support upstream traffic congestion control */ + break; + case NETWORK_MESSAGE_INIT_RT_TABLE: + /* If sent with Number of Ports == 0, we respond with + * NETWORK_MESSAGE_INIT_RT_TABLE_ACK and a list of all our + * reachable networks. + */ + if (npdu_len > 0) { + /* If Number of Ports is 0, broadcast our "full" table */ + if (npdu[0] == 0) { + send_initialize_routing_table_ack(snet, NULL); + } else { + /* they sent us a list */ + int net_count = npdu[0]; + while (net_count--) { + int i = 1; + /* DNET */ + decode_unsigned16(&npdu[i], &dnet); + /* update routing table */ + dnet_add(snet, dnet, src); + if (npdu[i + 3] > 0) { + /* find next NET value */ + i = npdu[i + 3] + 4; + } else { + i += 4; + } + } + send_initialize_routing_table_ack(snet, NULL); + } + break; + } + break; + case NETWORK_MESSAGE_INIT_RT_TABLE_ACK: + /* Do nothing with the routing table info, since don't support + * upstream traffic congestion control */ + break; + case NETWORK_MESSAGE_ESTABLISH_CONNECTION_TO_NETWORK: + case NETWORK_MESSAGE_DISCONNECT_CONNECTION_TO_NETWORK: + /* Do nothing - don't support PTP half-router control */ + break; + default: + /* An unrecognized message is bad; send an error response. */ + send_reject_message_to_network(snet, src, + NETWORK_REJECT_UNKNOWN_MESSAGE_TYPE, 0); + break; + } +} + +/** + * Fill the router src address with this port router, router network number, + * and the original src address. + * + * @param router_src [in] The src BACNET_ADDRESS for this routed message. + * @param snet [in] The source network port where the message came from + * @param src [in] The BACNET_ADDRESS of the message's original src. + */ +static void routed_src_address( + BACNET_ADDRESS * router_src, + uint16_t snet, + BACNET_ADDRESS * src) +{ + unsigned int i = 0; + + if (router_src && src) { + /* copy our directly connected port address */ + if (port_find(snet, router_src)) { + if (src->net) { + /* from a router - add router our table */ + dnet_add(snet, src->net, src); + /* the routed address stays the same */ + router_src->net = src->net; + router_src->len = src->len; + for (i = 0; i < MAX_MAC_LEN; i++) { + router_src->adr[i] = src->adr[i]; + } + } else { + /* from our directly connected port */ + router_src->net = snet; + router_src->len = src->mac_len; + for (i = 0; i < MAX_MAC_LEN; i++) { + router_src->adr[i] = src->mac[i]; + } + } + } + } +} + +/** + * If a BACnet NPDU is received with NPCI indicating that the message + * should be relayed by virtue of the presence of a non-broadcast + * DNET, the router shall search its routing table for the indicated + * network number. Normal routing procedures are described in 6.5. + * If, however, the network number cannot be found in the routing + * table or through the use of the Who-Is-Router-To-Network message, + * the router shall generate a Reject-Message-To-Network message and + * send it to the node that originated the BACnet NPDU. + * If the NPCI indicates either a remote or global broadcast, + * the message shall be processed as described in 6.3.2. + * + * @param src [in] The BACNET_ADDRESS of the message's source. + * @param dest [in] The BACNET_ADDRESS of the message's destination. + * @param DNET_list [in] List of our reachable downstream BACnet Network numbers. + * Normally just one valid entry; terminated with a -1 value. + * @param apdu [in] The apdu portion of the request, to be processed. + * @param apdu_len [in] The total (remaining) length of the apdu. + */ +static void routed_apdu_handler( + uint16_t snet, + BACNET_NPDU_DATA * npdu, + BACNET_ADDRESS * src, + BACNET_ADDRESS * dest, + uint8_t * apdu, + uint16_t apdu_len) +{ + DNET *port = NULL; + BACNET_ADDRESS local_dest; + BACNET_ADDRESS remote_dest; + BACNET_ADDRESS router_src; + int npdu_len = 0; + + /* for broadcast messages no search is needed */ + if (dest->net == BACNET_BROADCAST_NETWORK) { + /* A global broadcast, indicated by a DNET of X'FFFF', is sent + to all networks through all routers. Upon receipt of a message + with the global broadcast DNET network number, a router shall + decrement the Hop Count. If the Hop Count is still greater + than zero, then the router shall broadcast the message on all + directly connected networks except the network of origin, using + the broadcast MAC address appropriate for each destination network. + If the Hop Count is zero, then the router shall discard + the message. In order for the message to be disseminated globally, + the originating device shall use a broadcast MAC address + on the originating network so that all attached routers may + receive the message and propagate it further. */ + datalink_get_broadcast_address(&local_dest); + npdu->hop_count--; + routed_src_address(&router_src, snet, src); + /* encode both source and destination for broadcast */ + npdu_len = + npdu_encode_pdu(&Tx_Buffer[0], &local_dest, &router_src, npdu); + memmove(&Tx_Buffer[npdu_len], apdu, apdu_len); + /* send to my other ports */ + debug_printf("Routing a BROADCAST from %u\n", (unsigned)snet); + port = Router_Table_Head; + while (port != NULL) { + if (port->net != snet) { + datalink_send_pdu(port->net, &local_dest, npdu, + &Tx_Buffer[0], npdu_len+apdu_len); + } + port = port->next; + } + return; + } + port = dnet_find(dest->net, &remote_dest); + if (port) { + if (port->net == dest->net) { + debug_printf("Routing to Port %u\n", (unsigned)dest->net); + /* Case 1: the router is directly + connected to the network referred to by DNET. */ + /* In the first case, DNET, DADR, and Hop + Count shall be removed from the NPCI and the message shall be + sent directly to the destination device with DA set equal to + DADR. The control octet shall be adjusted accordingly to + indicate only the presence of SNET and SADR. */ + memmove(&local_dest.mac, dest->adr, MAX_MAC_LEN); + local_dest.mac_len = dest->len; + local_dest.net = 0; + npdu->hop_count--; + routed_src_address(&router_src, snet, src); + npdu_len = + npdu_encode_pdu(&Tx_Buffer[0], &local_dest, &router_src, npdu); + memmove(&Tx_Buffer[npdu_len], apdu, apdu_len); + datalink_send_pdu(port->net, &local_dest, npdu, + &Tx_Buffer[0], npdu_len+apdu_len); + } else { + debug_printf("Routing to another Router %u\n", + (unsigned)remote_dest.net); + /* Case 2: the message must be + relayed to another router for further transmission */ + /* In the second case, if the Hop Count is greater than zero, + the message shall be sent to the next router on the + path to the destination network. + If the Hop Count is zero, then the message shall be + discarded. */ + npdu->hop_count--; + routed_src_address(&router_src, snet, src); + npdu_len = + npdu_encode_pdu(&Tx_Buffer[0], &remote_dest, &router_src, npdu); + memmove(&Tx_Buffer[npdu_len], apdu, apdu_len); + datalink_send_pdu(port->net, &remote_dest, npdu, + &Tx_Buffer[0], npdu_len+apdu_len); + } + } else if (dest->net) { + debug_printf("Routing to Unknown Route %u\n", + (unsigned)dest->net); + /* Case 3: a global broadcast is required. */ + dest->mac_len = 0; + npdu->hop_count--; + /* encode both source and destination */ + routed_src_address(&router_src, snet, src); + npdu_len = + npdu_encode_pdu(&Tx_Buffer[0], dest, &router_src, npdu); + memmove(&Tx_Buffer[npdu_len], apdu, apdu_len); + datalink_send_pdu(port->net, dest, npdu, + &Tx_Buffer[0], npdu_len+apdu_len); + /* If the next router is unknown, an attempt shall be made to + identify it using a Who-Is-Router-To-Network message. */ + send_who_is_router_to_network(0, dest->net); + } +} + +/** + * Handler for the routing packets only + * + * @param src [out] Returned with routing source information if the NPDU + * has any and if this points to non-null storage for it. + * If src->net and src->len are 0 on return, there is no + * routing source information. + * This src describes the original source of the message when + * it had to be routed to reach this BACnet Device, and this + * is passed down into the apdu_handler. + * @param DNET_list [in] List of our reachable downstream BACnet Network + * numbers terminated with a -1 value. + * @param pdu [in] Buffer containing the NPDU and APDU of the received packet. + * @param pdu_len [in] The size of the received message in the pdu[] buffer. + */ +static void my_routing_npdu_handler( + uint16_t snet, + BACNET_ADDRESS * src, + uint8_t * pdu, + uint16_t pdu_len) +{ + int apdu_offset = 0; + BACNET_ADDRESS dest = { 0 }; + BACNET_NPDU_DATA npdu_data = { 0 }; + + if (!pdu) { + /* no packet */ + } else if (pdu[0] == BACNET_PROTOCOL_VERSION) { + apdu_offset = npdu_decode(&pdu[0], &dest, src, &npdu_data); + if (apdu_offset <= 0) { + fprintf(stderr, "NPDU: Decoding failed; Discarded!\n"); + } else if (npdu_data.network_layer_message) { + if ((dest.net == 0) || (dest.net == BACNET_BROADCAST_NETWORK)) { + network_control_handler(snet, src, &npdu_data, + &pdu[apdu_offset], (uint16_t) (pdu_len - apdu_offset)); + } else { + /* The DNET is set, but we don't support downstream routers, + * so we just silently drop this network layer message, + * since only routers can handle it (even if for our DNET) */ + } + } else if ((apdu_offset > 0) && (apdu_offset <= pdu_len)) { + if ((dest.net == 0) || + (dest.net == BACNET_BROADCAST_NETWORK) || + (npdu_data.hop_count > 1)) { + /* only handle the version that we know how to handle */ + /* and we are not a router, so ignore messages with + routing information cause they are not for us */ + if ((dest.net == BACNET_BROADCAST_NETWORK) && + ((pdu[apdu_offset] & 0xF0) == + PDU_TYPE_CONFIRMED_SERVICE_REQUEST)) { + /* hack for 5.4.5.1 - IDLE */ + /* ConfirmedBroadcastReceived */ + /* then enter IDLE - ignore the PDU */ + } else { + routed_apdu_handler(snet, &npdu_data, src, &dest, + &pdu[apdu_offset], + (uint16_t) (pdu_len - apdu_offset)); + } + } else { + fprintf(stderr, "NPDU: DNET=%u. Discarded!\n", + (unsigned) dest.net); + } + } + } else { + /* unsupported protocol version */ + } + + return; +} + +/** + * Initialize the BACnet/IPv6 and BACnet/IP data links + */ +static void datalink_init(void) +{ + char *pEnv = NULL; + BACNET_ADDRESS my_address = {0}; + extern bool BIP_Debug; + + /* BACnet/IP Initialization */ + BIP_Debug = true; + pEnv = getenv("BACNET_IP_PORT"); + if (pEnv) { + bip_set_port(htons((uint16_t) strtol(pEnv, NULL, 0))); + } else { + /* BIP_Port is statically initialized to 0xBAC0, + * so if it is different, then it was programmatically altered, + * and we shouldn't just stomp on it here. + * Unless it is set below 1024, since: + * "The range for well-known ports managed by the IANA is 0-1023." + */ + if (ntohs(bip_get_port()) < 1024) { + bip_set_port(htons(0xBAC0)); + } + } + if (!bip_init(getenv("BACNET_IFACE"))) { + exit(1); + } + atexit(bip_cleanup); + /* BACnet/IPv6 Initialization */ + pEnv = getenv("BACNET_BIP6_PORT"); + if (pEnv) { + bip6_set_port((uint16_t) strtol(pEnv, NULL, 0)); + } + pEnv = getenv("BACNET_BIP6_BROADCAST"); + if (pEnv) { + BACNET_IP6_ADDRESS addr; + bvlc6_address_set(&addr, + (uint16_t) strtol(pEnv, NULL, 0), 0, 0, 0, 0, 0, 0, + BIP6_MULTICAST_GROUP_ID); + bip6_set_broadcast_addr(&addr); + } + if (!bip6_init(getenv("BACNET_BIP6_IFACE"))) { + exit(1); + } + atexit(bip6_cleanup); + /* router network numbers */ + pEnv = getenv("BACNET_IP_NET"); + if (pEnv) { + BIP_Net = strtol(pEnv, NULL, 0); + } else { + BIP_Net = 1; + } + /* configure the first entry in the table - home port */ + bip_get_my_address(&my_address); + port_add(BIP_Net, &my_address); + /* BACnet/IPv6 network */ + pEnv = getenv("BACNET_IP6_NET"); + if (pEnv) { + BIP6_Net = strtol(pEnv, NULL, 0); + } else { + BIP6_Net = 2; + } + /* configure the next entry in the table */ + bip6_get_my_address(&my_address); + port_add(BIP6_Net, &my_address); +} + +/** + * Cleanup memory + * + */ +static void cleanup(void) +{ + DNET *port = NULL; + + fprintf(stderr, "Cleaning up...\n"); + /* clean up the remote networks */ + port = Router_Table_Head; + while (port != NULL) { + dnet_cleanup(port->dnets); + port = port->next; + } + /* clean up the directly connected networks */ + dnet_cleanup(Router_Table_Head); +} + +#if defined(_WIN32) +static BOOL WINAPI CtrlCHandler( + DWORD dwCtrlType) +{ + dwCtrlType = dwCtrlType; + + /* signal to main loop to exit */ + Exit_Requested = true; + while (Exit_Requested) { + Sleep(100); + } + exit(0); + + return TRUE; +} + +void control_c_hooks(void) +{ + SetConsoleMode(GetStdHandle(STD_INPUT_HANDLE), ENABLE_PROCESSED_INPUT); + SetConsoleCtrlHandler((PHANDLER_ROUTINE) CtrlCHandler, TRUE); +} +#else +static void sig_int( + int signo) +{ + (void) signo; + Exit_Requested = true; + exit(0); +} + +void signal_init( + void) +{ + signal(SIGINT, sig_int); + signal(SIGHUP, sig_int); + signal(SIGTERM, sig_int); +} + +void control_c_hooks(void) +{ + signal_init(); +} +#endif + +/** + * Main function of simple router demo. + * + * @param argc [in] Arg count. + * @param argv [in] Takes one argument: the Device Instance #. + * @return 0 on success. + */ +int main( + int argc, + char *argv[]) +{ + BACNET_ADDRESS src = { + 0 + }; /* address where message came from */ + uint16_t pdu_len = 0; + time_t last_seconds = 0; + time_t current_seconds = 0; + uint32_t elapsed_seconds = 0; + + printf("BACnet Simple IP Router Demo\n"); + printf("BACnet Stack Version %s\n", BACnet_Version); + datalink_init(); + atexit(cleanup); + control_c_hooks(); + /* configure the timeout values */ + last_seconds = time(NULL); + /* broadcast an I-Am on startup */ + printf("BACnet/IP Network: %u\n", (unsigned)BIP_Net); + send_i_am_router_to_network(BIP_Net, 0); + printf("BACnet/IPv6 Network: %u\n", (unsigned)BIP6_Net); + send_i_am_router_to_network(BIP6_Net, 0); + /* loop forever */ + for (;;) { + /* input */ + current_seconds = time(NULL); + /* returns 0 bytes on timeout */ + pdu_len = bvlc_receive(&src, &BIP_Rx_Buffer[0], MAX_MPDU, 5); + /* process */ + if (pdu_len) { + debug_printf("BACnet/IP Received packet\n"); + my_routing_npdu_handler(BIP_Net, &src, &BIP_Rx_Buffer[0], pdu_len); + } + /* returns 0 bytes on timeout */ + pdu_len = bip6_receive(&src, &BIP6_Rx_Buffer[0], MAX_MPDU, 5); + /* process */ + if (pdu_len) { + debug_printf("BACnet/IPv6 Received packet\n"); + my_routing_npdu_handler(BIP6_Net, &src, &BIP6_Rx_Buffer[0], pdu_len); + } + /* at least one second has passed */ + elapsed_seconds = (uint32_t) (current_seconds - last_seconds); + if (elapsed_seconds) { + last_seconds = current_seconds; + bvlc_maintenance_timer(elapsed_seconds); + } + if (Exit_Requested) { + break; + } + } + /* tell signal interrupts we are done */ + Exit_Requested = false; + + return 0; +} diff --git a/bacnet-stack/demo/router-ipv6/readme.txt b/bacnet-stack/demo/router-ipv6/readme.txt new file mode 100644 index 00000000..daab2349 --- /dev/null +++ b/bacnet-stack/demo/router-ipv6/readme.txt @@ -0,0 +1,22 @@ +BACnet Simple Router Demo +========================= + +The Simple Router demo connects one BACnet/IP and one BACnet/IPv6 network. +It also includes a BBMD so that Foreign Device Registration can be used +to tunnel local command line demos to BACnet/IP and BACnet IPv6 networks. + +Configuration +============= + +It uses environment variables to configure +the BACnet/IP port and BACnet/IPv6 address on Linux: + +export BACNET_IFACE=eth0 +export BACNET_BIP6_IFACE=eth0 + +Also uses these configurations, but defaults to these values if not set: +export BACNET_IP_PORT=47808 +export BACNET_BIP6_PORT=47808 +export BACNET_BIP6_BROADCAST=FF05 +export BACNET_IP_NET=1 +export BACNET_IP6_NET=2 diff --git a/bacnet-stack/include/config.h b/bacnet-stack/include/config.h index 59adef48..a687aa6d 100644 --- a/bacnet-stack/include/config.h +++ b/bacnet-stack/include/config.h @@ -30,11 +30,13 @@ /* declare a single physical layer using your compiler define. see datalink.h for possible defines. */ -#if !(defined(BACDL_ETHERNET) || defined(BACDL_ARCNET) || defined(BACDL_MSTP) || defined(BACDL_BIP) || defined(BACDL_TEST) || defined(BACDL_ALL)) +#if !(defined(BACDL_ETHERNET) || defined(BACDL_ARCNET) || \ + defined(BACDL_MSTP) || defined(BACDL_BIP) || defined(BACDL_BIP6) || \ + defined(BACDL_TEST) || defined(BACDL_ALL)) #define BACDL_BIP #endif -/* optional configuration for BACnet/IP datalink layers */ +/* optional configuration for BACnet/IP datalink layer */ #if (defined(BACDL_BIP) || defined(BACDL_ALL)) /* other BIP defines (define as 1 to enable): USE_INADDR - uses INADDR_BROADCAST for broadcast and binds using INADDR_ANY @@ -45,6 +47,13 @@ #endif #endif +/* optional configuration for BACnet/IPv6 datalink layer */ +#if defined(BACDL_BIP6) +#if !defined(BBMD6_ENABLED) +#define BBMD6_ENABLED 0 +#endif +#endif + /* Enable the Gateway (Routing) functionality here, if desired. */ #if !defined(MAX_NUM_DEVICES) #ifdef BAC_ROUTING @@ -80,8 +89,10 @@ /* #define MAX_APDU 1476 */ #if defined(BACDL_BIP) #define MAX_APDU 1476 - -/* #define MAX_APDU 128 enable this IP for testing readrange so you get the More Follows flag set */ +/* #define MAX_APDU 128 enable this IP for testing + readrange so you get the More Follows flag set */ +#elif defined(BACDL_BIP6) +#define MAX_APDU 1476 #elif defined (BACDL_ETHERNET) #if defined(BACNET_SECURITY) #define MAX_APDU 1420 diff --git a/bacnet-stack/include/datalink.h b/bacnet-stack/include/datalink.h index 77f7614d..54f084b6 100644 --- a/bacnet-stack/include/datalink.h +++ b/bacnet-stack/include/datalink.h @@ -79,6 +79,17 @@ extern void routed_get_my_address( #define datalink_get_my_address bip_get_my_address #endif +#elif defined(BACDL_BIP6) +#include "bip6.h" +#include "bvlc6.h" +#define datalink_init bip6_init +#define datalink_send_pdu bip6_send_pdu +#define datalink_receive bip6_receive +#define datalink_cleanup bip6_cleanup +#define datalink_get_broadcast_address bip6_get_broadcast_address +#define datalink_get_my_address bip6_get_my_address + + #else /* Ie, BACDL_ALL */ #include "npdu.h" diff --git a/bacnet-stack/include/vmac.h b/bacnet-stack/include/vmac.h new file mode 100644 index 00000000..a499d628 --- /dev/null +++ b/bacnet-stack/include/vmac.h @@ -0,0 +1,52 @@ +/** +* @file +* @author Steve Karg +* @date 2016 +*/ +#ifndef VMAC_H +#define VMAC_H + +#include +#include + +/* define the max MAC as big as IPv6 + port number */ +#define VMAC_MAC_MAX 18 +/** +* VMAC data structure +* +* @{ +*/ +struct vmac_data { + uint8_t mac[18]; + uint8_t mac_len; +}; +/** @} */ + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + + unsigned int VMAC_Count(void); + struct vmac_data *VMAC_Find_By_Key(uint32_t device_id); + bool VMAC_Find_By_Data(struct vmac_data *vmac, uint32_t *device_id); + bool VMAC_Add(uint32_t device_id, struct vmac_data *pVMAC); + bool VMAC_Delete(uint32_t device_id); + bool VMAC_Different( + struct vmac_data *vmac1, + struct vmac_data *vmac2); + bool VMAC_Match( + struct vmac_data *vmac1, + struct vmac_data *vmac2); + void VMAC_Cleanup(void); + void VMAC_Init(void); + +#ifdef TEST +#include "ctest.h" + void testVMAC( + Test * pTest); +#endif + +#ifdef __cplusplus +} +#endif /* __cplusplus */ +#endif diff --git a/bacnet-stack/lib/Makefile b/bacnet-stack/lib/Makefile index 503d7b1f..918f78f7 100644 --- a/bacnet-stack/lib/Makefile +++ b/bacnet-stack/lib/Makefile @@ -163,23 +163,25 @@ PORT_BIP_SRC = \ $(BACNET_CORE)/bvlc.c \ $(BACNET_CORE)/bip.c +PORT_BIP6_SRC = \ + $(BACNET_HANDLER)/h_bbmd6.c \ + $(BACNET_PORT_DIR)/bip6.c \ + $(BACNET_CORE)/vmac.c \ + $(BACNET_CORE)/bvlc6.c + PORT_ALL_SRC = \ - $(BACNET_PORT_DIR)/arcnet.c \ - $(BACNET_PORT_DIR)/dlmstp.c \ - $(BACNET_PORT_DIR)/rs485.c \ - $(BACNET_PORT_DIR)/timer.c \ - $(BACNET_CORE)/ringbuf.c \ - $(BACNET_CORE)/fifo.c \ - $(BACNET_CORE)/mstp.c \ - $(BACNET_CORE)/crc.c \ - $(BACNET_PORT_DIR)/ethernet.c \ - $(BACNET_PORT_DIR)/bip-init.c \ - $(BACNET_CORE)/bvlc.c \ - $(BACNET_CORE)/bip.c + $(PORT_ARCNET_SRC) \ + $(PORT_MSTP_SRC) \ + $(PORT_ETHERNET_SRC) \ + $(PORT_BIP_SRC) \ + $(PORT_BIP6_SRC) ifeq (${BACDL_DEFINE},-DBACDL_BIP=1) PORT_SRC = ${PORT_BIP_SRC} endif +ifeq (${BACDL_DEFINE},-DBACDL_BIP6=1) +PORT_SRC = ${PORT_BIP6_SRC} +endif ifeq (${BACDL_DEFINE},-DBACDL_MSTP=1) PORT_SRC = ${PORT_MSTP_SRC} endif diff --git a/bacnet-stack/ports/linux/bacnet_ipv6.lua b/bacnet-stack/ports/linux/bacnet_ipv6.lua new file mode 100644 index 00000000..30d93e97 --- /dev/null +++ b/bacnet-stack/ports/linux/bacnet_ipv6.lua @@ -0,0 +1,281 @@ +-- bacnet_BIPV6.lua +-- +-- Dissector for BACnet/IPv6 [135-2016 Annex X] +-- +-- Copyright 2011, Siemens Building Technolgies +-- Maintainer Philippe Goetz +-- Updated by Steve Karg +-- +-- This program is free software; you can redistribute it and/or +-- modify it under the terms of the GNU General Public License +-- as published by the Free Software Foundation; either version 2 +-- of the License, or (at your option) any later version. +-- +-- This program is distributed in the hope that it will be useful, +-- but WITHOUT ANY WARRANTY; without even the implied warranty of +-- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +-- GNU General Public License for more details. +-- +-- You should have received a copy of the GNU General Public License +-- along with this program; if not, write to the Free Software +-- Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + + +-- Add the following to the end of init.lua in the Wireshark install directory: +-- dofile("bacnet_ipv6.lua") + + +-- [in] tree,pf,buffer,offset,len +-- [out] offset, value, ti, bf +function uint_dissector(tree,pf,buffer,offset,len) + if (offset + len) <= buffer:len() then + local bf = buffer(offset,len) + local ti = nil + if tree ~= nil then + ti = tree:add(pf,bf) + end + return offset + len, bf:uint(), ti, bf + end + local ti = tree:add(pf,buffer(offset)) + ti:add_expert_info(PI_MALFORMED, PI_ERROR, "Data shortage") + error("Data shortage") +end + +function bytes_dissector(tree,pf,buffer,offset,len) + if (offset + len) <= buffer:len() then + local bf = buffer(offset,len) + local ti = nil + if tree ~= nil then + ti = tree:add(pf,bf) + end + return offset + len, bf:bytes(), ti, bf + end + local ti = tree:add(pf,buffer(offset)) + ti:add_expert_info(PI_MALFORMED, PI_ERROR, "Data shortage") + error("Data shortage") +end + +p_BACnetBIPV6 = Proto("lua_BACnetBIPV6", "B/IPv6 BACnet Virtual Link Control"); +p_BACnet_Port = Proto("BACnet-Port", "BACnet UDP Port Handoff") +p_dissector_bacnet = Dissector.get("bacnet") +p_dissector_bipv4 = Dissector.get("bvlc") + +do + p_BACnetBIPV6.init = function() + debug("p_BACnetBIPV6.init") + end + + local t_BACnetBIPV6_Types = { + [0x82]="B/IPv6 (Annex X)" + } + local t_BACnetBIPV6_Functions = { + -- Annex X, PPR3 Draft 22 + [0x00]="BVLC-Result", + [0x01]="Original-Unicast-NPDU", + [0x02]="Original-Broadcast-NPDU", + [0x03]="Address-Resolution", + [0x04]="Forwarded-Address-Resolution", + [0x05]="Address-Resolution-Ack", + [0x06]="Virtual-Address-Resolution", + [0x07]="Virtual-Address-Resolution-Ack", + [0x08]="Forwarded-NPDU", + [0x09]="Register-Foreign-Device", + [0x0A]="Delete-Foreign-Device", + [0x0B]="Secure-BVLL", + [0x0C]="Distribute-Broadcast-To-Network" + } + + local pf_BIPV6Data = ProtoField.bytes ("BIPV6.data", "B/IPV6 Data") + + p_BACnetBIPV6.fields = { + pf_BIPV6Data + } + + local t_BACnetBIPV6_dissectors = {} + + -- local dt_BIPV6_npdu = DissectorTable.new("lua_BACnetBIPV6.npdu","BIPV6 NPDU",ftypes.UINT8,base.HEX) + -- BIPV6 header fields + local pf_BIPV6 = {} + pf_BIPV6.Type = ProtoField.uint8 ("BIPV6.type","Type",base.HEX,t_BACnetBIPV6_Types) + table.insert(p_BACnetBIPV6.fields,pf_BIPV6.Type) + pf_BIPV6.Function = ProtoField.uint8 ("BIPV6.function","Function",base.HEX,t_BACnetBIPV6_Functions,0,"BIPV6 Function") + table.insert(p_BACnetBIPV6.fields,pf_BIPV6.Function) + pf_BIPV6.Length = ProtoField.uint16("BIPV6.length","BIPV6-Length",base.DEC,nil,0,"Length of BIPV6") + table.insert(p_BACnetBIPV6.fields,pf_BIPV6.Length) + pf_BIPV6.ResultCode = ProtoField.uint16("BIPV6.resultCode","Result-Code",base.DEC,nil,0,"Result Code") + table.insert(p_BACnetBIPV6.fields,pf_BIPV6.ResultCode) + pf_BIPV6.SourceVirtualAddress = ProtoField.uint24 ("BIPV6.sourceVirtualAddress","Source-Virtual-Address") + table.insert(p_BACnetBIPV6.fields,pf_BIPV6.SourceVirtualAddress) + pf_BIPV6.DestinationVirtualAddress = ProtoField.uint24 ("BIPV6.destinationVirtualAddress","Destination-Virtual-Address") + table.insert(p_BACnetBIPV6.fields,pf_BIPV6.DestinationVirtualAddress) + pf_BIPV6.OriginalSourceEffectiveAddress = ProtoField.bytes ("BIPV6.originalSourceEffectiveAddress","Original-Source-B/IPv6-Address") + table.insert(p_BACnetBIPV6.fields,pf_BIPV6.OriginalSourceEffectiveAddress) + pf_BIPV6.OriginalSourceEffectiveAddress_ip = ProtoField.ipv6 ("BIPV6.originalSourceEffectiveAddress.ip","Original-Source-B/IPv6-Address (ip)") + table.insert(p_BACnetBIPV6.fields,pf_BIPV6.OriginalSourceEffectiveAddress_ip) + pf_BIPV6.OriginalSourceEffectiveAddress_port = ProtoField.uint16 ("BIPV6.originalSourceEffectiveAddress.port","Original-Source-B/IPv6-Address (port)") + table.insert(p_BACnetBIPV6.fields,pf_BIPV6.OriginalSourceEffectiveAddress_port) + pf_BIPV6.OriginalSourceVirtualAddress = ProtoField.uint24 ("BIPV6.originalSourceVirtualAddress","Original-Source-Virtual-Address") + table.insert(p_BACnetBIPV6.fields,pf_BIPV6.OriginalSourceVirtualAddress) + pf_BIPV6.DestinationEffectiveAddress = ProtoField.bytes ("BIPV6.destinationEffectiveAddress","Original-Source-B/IPv6-Address") + table.insert(p_BACnetBIPV6.fields,pf_BIPV6.DestinationEffectiveAddress) + pf_BIPV6.DestinationEffectiveAddress_ip = ProtoField.ipv6 ("BIPV6.destinationEffectiveAddress.ip","Original-Source-B/IPv6-Address (ip)") + table.insert(p_BACnetBIPV6.fields,pf_BIPV6.DestinationEffectiveAddress_ip) + pf_BIPV6.DestinationEffectiveAddress_port = ProtoField.uint16 ("BIPV6.destinationEffectiveAddress.port","Original-Source-B/IPv6-Address (port)") + table.insert(p_BACnetBIPV6.fields,pf_BIPV6.DestinationEffectiveAddress_port) + pf_BIPV6.TimeToLive = ProtoField.uint16 ("BIPV6.timeToLive","Time-To-Live") + table.insert(p_BACnetBIPV6.fields,pf_BIPV6.TimeToLive) + + function p_BACnetBIPV6.dissector(buffer,pkt,tree) + + pkt.cols["protocol"] = "B/IPv6" + pkt.cols["info"] = "BACnet Building Automation and Control Network" + local offset = 0 + local v_BIPV6 = {} + local ti_BIPV6 = {} + ti_BIPV6.ti = tree:add(p_BACnetBIPV6,buffer(0)) + offset, v_BIPV6.Type, ti_BIPV6.Type = uint_dissector(ti_BIPV6.ti, pf_BIPV6.Type, buffer, offset, 1) + --if v_BIPV6.Type == 0x81 then + -- p_dissector_bipv4:call(buffer, pkt, tree) + -- elseif v_BIPV6 ~= 0x82 then + -- ti_BIPV6.Type:add_expert_info(PI_MALFORMED, PI_WARN, "Unknown BVLC Type") + -- return + --end + if v_BIPV6.Type ~= 0x82 then + ti_BIPV6.Type:add_expert_info(PI_MALFORMED, PI_WARN, "Unknown BVLC Type") + return + end + offset, v_BIPV6.Function, ti_BIPV6.Function = uint_dissector(ti_BIPV6.ti, pf_BIPV6.Function, buffer, offset, 1) + offset, v_BIPV6.Length, ti_BIPV6.Length = uint_dissector(ti_BIPV6.ti, pf_BIPV6.Length, buffer, offset, 2) + local BIPV6Dissector = t_BACnetBIPV6_dissectors[v_BIPV6.Function] + local npduData = nil + if BIPV6Dissector ~= nil then + pkt.cols["info"] = t_BACnetBIPV6_Functions[v_BIPV6.Function] + offset, npduData = BIPV6Dissector(buffer,pkt,ti_BIPV6.ti,offset,v_BIPV6,tree) + end + if npduData ~= nil then + if p_BACnetNPDU ~= nil then + ti_BIPV6.ti:set_len(offset) + p_BACnetNPDU.dissector:call(npduData:tvb(), pkt, tree) + offset = offset + npduData:len() + else + p_dissector_bacnet:call(npduData:tvb(), pkt, tree) + end + end + -- CLB don't show the raw data + -- if offset ~= buffer:len() then + -- ti_BIPV6.ti:add(pf_BIPV6Data,buffer(offset)) + -- end + ti_BIPV6 = nil + collectgarbage("collect") + end + + -- [0x00] BVLC-Result + t_BACnetBIPV6_dissectors[0x00] = function(buffer,pkt,tree,offset,npdu) + offset = bytes_dissector(tree,pf_BIPV6.SourceVirtualAddress,buffer,offset,3) + offset = bytes_dissector(tree,pf_BIPV6.ResultCode,buffer,offset,2) + return offset + end + + -- [0x01] Original-Unicast-NPDU + t_BACnetBIPV6_dissectors[0x01] = function(buffer,pkt,tree,offset,npdu) + offset = bytes_dissector(tree,pf_BIPV6.SourceVirtualAddress,buffer,offset,3) + offset = bytes_dissector(tree,pf_BIPV6.DestinationVirtualAddress,buffer,offset,3) + return offset, buffer(offset) + end + + -- [0x02] Original-Broadcast-NPDU + t_BACnetBIPV6_dissectors[0x02] = function(buffer,pkt,tree,offset,npdu) + offset = bytes_dissector(tree,pf_BIPV6.SourceVirtualAddress,buffer,offset,3) + return offset, buffer(offset) + end + + -- [0x03] Address-Resolution + t_BACnetBIPV6_dissectors[0x03] = function(buffer,pkt,tree,offset,npdu) + offset = bytes_dissector(tree,pf_BIPV6.SourceVirtualAddress,buffer,offset,3) + offset = bytes_dissector(tree,pf_BIPV6.DestinationVirtualAddress,buffer,offset,3) + return offset + end + + -- [0x04] Forwarded-Address-Resolution + t_BACnetBIPV6_dissectors[0x04] = function(buffer,pkt,tree,offset,npdu) + offset = bytes_dissector(tree,pf_BIPV6.OriginalSourceVirtualAddress,buffer,offset,3) + offset = bytes_dissector(tree,pf_BIPV6.DestinationVirtualAddress,buffer,offset,3) + local subtree = tree:add(pf_BIPV6.OriginalSourceEffectiveAddress,buffer(offset,18)) + offset = bytes_dissector(subtree,pf_BIPV6.OriginalSourceEffectiveAddress_ip,buffer,offset,16) + offset = uint_dissector(subtree,pf_BIPV6.OriginalSourceEffectiveAddress_port,buffer,offset,2) + return offset + end + + -- [0x05] Address-Resolution-Ack + t_BACnetBIPV6_dissectors[0x05] = function(buffer,pkt,tree,offset,npdu) + offset = bytes_dissector(tree,pf_BIPV6.SourceVirtualAddress,buffer,offset,3) + offset = bytes_dissector(tree,pf_BIPV6.DestinationVirtualAddress,buffer,offset,3) + return offset + end + + -- [0x06] Virtual-Address-Resolution + t_BACnetBIPV6_dissectors[0x06] = function(buffer,pkt,tree,offset,npdu) + offset = bytes_dissector(tree,pf_BIPV6.SourceVirtualAddress,buffer,offset,3) + return offset + end + + -- [0x07] Virtual-Address-Resolution-Ack + t_BACnetBIPV6_dissectors[0x07] = function(buffer,pkt,tree,offset,npdu) + offset = bytes_dissector(tree,pf_BIPV6.SourceVirtualAddress,buffer,offset,3) + return offset + end + + -- [0x08] Forwarded-NPDU + t_BACnetBIPV6_dissectors[0x08] = function(buffer,pkt,tree,offset,npdu) + offset = bytes_dissector(tree,pf_BIPV6.OriginalSourceVirtualAddress,buffer,offset,3) + local subtree = tree:add(pf_BIPV6.OriginalSourceEffectiveAddress,buffer(offset,18)) + offset = bytes_dissector(subtree,pf_BIPV6.OriginalSourceEffectiveAddress_ip,buffer,offset,16) + offset = uint_dissector(subtree,pf_BIPV6.OriginalSourceEffectiveAddress_port,buffer,offset,2) + return offset, buffer(offset) + end + + -- [0x09] Register-Foreign-Device + t_BACnetBIPV6_dissectors[0x09] = function(buffer,pkt,tree,offset,npdu) + offset = bytes_dissector(tree,pf_BIPV6.SourceVirtualAddress,buffer,offset,3) + offset = uint_dissector(tree,pf_BIPV6.TimeToLive,buffer,offset,2) + return offset + end + + -- [0x0A] Delete-Foreign-Device + t_BACnetBIPV6_dissectors[0x0A] = function(buffer,pkt,tree,offset,npdu) + offset = bytes_dissector(tree,pf_BIPV6.SourceVirtualAddress,buffer,offset,3) + -- FDT Entry decoding + return offset + end + + -- [0x0B] Secure-BVLL + t_BACnetBIPV6_dissectors[0x0B] = function(buffer,pkt,tree,offset,npdu) + return offset + end + + -- [0x0C] Distribute-Broadcast-To-Network + t_BACnetBIPV6_dissectors[0x0C] = function(buffer,pkt,tree,offset,npdu) + offset = bytes_dissector(tree,pf_BIPV6.OriginalSourceVirtualAddress,buffer,offset,3) + local subtree = tree:add(pf_BIPV6.OriginalSourceEffectiveAddress,buffer(offset,18)) + offset = bytes_dissector(subtree,pf_BIPV6.OriginalSourceEffectiveAddress_ip,buffer,offset,16) + offset = uint_dissector(subtree,pf_BIPV6.OriginalSourceEffectiveAddress_port,buffer,offset,2) + return offset, buffer(offset) + end + + function p_BACnet_Port.dissector(tvb, pinfo, tree) + local BIP_Type = tvb(0,1):uint() + if BIP_Type == 0x81 then + p_dissector_bipv4:call(tvb, pinfo, tree) + elseif BIP_Type == 0x82 then + p_BACnetBIPV6.dissector(tvb, pinfo, tree) + end + end + + -- load the udp.port table + local dt_udp_port = DissectorTable.get("udp.port") + -- p_dissector_bvlc = dt_udp_port:get_dissector(47808) + -- dt_udp_port:remove(47808, p_dissector_bvlc) + -- dt_udp_port:add(47808, p_BACnetBIPV6) + dt_udp_port:add(47808, p_BACnet_Port) + +end diff --git a/bacnet-stack/ports/linux/bip6.c b/bacnet-stack/ports/linux/bip6.c new file mode 100644 index 00000000..0cf37911 --- /dev/null +++ b/bacnet-stack/ports/linux/bip6.c @@ -0,0 +1,453 @@ +/*####COPYRIGHTBEGIN#### + ------------------------------------------- + Copyright (C) 2016 Steve Karg + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to: + The Free Software Foundation, Inc. + 59 Temple Place - Suite 330 + Boston, MA 02111-1307, USA. + + As a special exception, if other files instantiate templates or + use macros or inline functions from this file, or you compile + this file and link it with other works to produce a work based + on this file, this file does not by itself cause the resulting + work to be covered by the GNU General Public License. However + the source code for this file must still be made available in + accordance with section (3) of the GNU General Public License. + + This exception does not invalidate any other reasons why a work + based on this file might be covered by the GNU General Public + License. + ------------------------------------------- +####COPYRIGHTEND####*/ + +#include +#include +#include /* for standard integer types uint8_t etc. */ +#include /* for the standard bool type. */ +#include "bacdcode.h" +#include "config.h" +#include "bip6.h" +#include "debug.h" +#include "device.h" +#include "net.h" +#include + +static void debug_print_ipv6(const char *str, const struct in6_addr * addr) { + debug_printf( "BIP6: %s %02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x\n", + str, + (int)addr->s6_addr[0], (int)addr->s6_addr[1], + (int)addr->s6_addr[2], (int)addr->s6_addr[3], + (int)addr->s6_addr[4], (int)addr->s6_addr[5], + (int)addr->s6_addr[6], (int)addr->s6_addr[7], + (int)addr->s6_addr[8], (int)addr->s6_addr[9], + (int)addr->s6_addr[10], (int)addr->s6_addr[11], + (int)addr->s6_addr[12], (int)addr->s6_addr[13], + (int)addr->s6_addr[14], (int)addr->s6_addr[15]); +} + +/** @file linux/bip6.c Initializes BACnet/IPv6 interface (Linux). */ + +/* unix socket */ +static int BIP6_Socket = -1; +/* local address - filled by init functions */ +static BACNET_IP6_ADDRESS BIP6_Addr; +static BACNET_IP6_ADDRESS BIP6_Broadcast_Addr; + +/** + * Set the interface name. On Linux, ifname is the /dev/ name of the interface. + * + * @param ifname - C string for name or text address + */ +void bip6_set_interface( + char *ifname) +{ + struct ifaddrs *ifa, *ifa_tmp; + struct sockaddr_in6 *sin; + bool found = false; + + if (getifaddrs(&ifa) == -1) { + perror("BIP6: getifaddrs failed"); + exit(1); + } + ifa_tmp = ifa; + debug_printf("BIP6: seeking interface: %s\n", ifname); + while (ifa_tmp) { + if ((ifa_tmp->ifa_addr) && + (ifa_tmp->ifa_addr->sa_family == AF_INET6)) { + debug_printf("BIP6: found interface: %s\n", ifa_tmp->ifa_name); + } + if ((ifa_tmp->ifa_addr) && + (ifa_tmp->ifa_addr->sa_family == AF_INET6) && + (strcasecmp(ifa_tmp->ifa_name,ifname) == 0)) { + sin = (struct sockaddr_in6*) ifa_tmp->ifa_addr; + bvlc6_address_set(&BIP6_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])); + debug_print_ipv6(ifname, (&sin->sin6_addr)); + found = true; + break; + } + ifa_tmp = ifa_tmp->ifa_next; + } + if (!found) { + debug_printf("BIP6: unable to set interface: %s\n", ifname); + exit(1); + } +} + +/** + * 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) { + return 0; + } + /* 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); + debug_print_ipv6("Sending MPDU->", &bvlc_dest.sin6_addr); + /* Send the packet */ + return sendto(BIP6_Socket, (char *) mtu, mtu_len, 0, + (struct sockaddr *) &bvlc_dest, sizeof(bvlc_dest)); +} + +/** + * 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 */ + fd_set read_fds; + int max = 0; + struct 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; + } + FD_ZERO(&read_fds); + FD_SET(BIP6_Socket, &read_fds); + max = BIP6_Socket; + /* see if there is a packet for us */ + if (select(max + 1, &read_fds, NULL, NULL, &select_timeout) > 0) { + received_bytes = + 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) { + 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) { + return 0; + } + /* pass the packet into the BBMD handler */ + debug_print_ipv6("Received MPDU->", &sin.sin6_addr); + 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 { + npdu_len = 0; + } + } + + return npdu_len; +} + +/** Cleanup and close out the BACnet/IP services by closing the socket. + * @ingroup DLBIP6 + */ +void bip6_cleanup( + void) +{ + if (BIP6_Socket != -1) { + close(BIP6_Socket); + } + BIP6_Socket = -1; + + return; +} + +/** 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). + * + * @note For Linux, ifname is eth0, ath0, arc0, and others. + * + * @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 = 0; /* return from socket lib calls */ + struct sockaddr_in6 server = {0}; + struct in6_addr broadcast_address; + struct ipv6_mreq join_request; + int sockopt = 0; + + if (ifname) { + bip6_set_interface(ifname); + } else { + bip6_set_interface("eth0"); + } + if (BIP6_Addr.port == 0) { + bip6_set_port(0xBAC0); + } + debug_printf("BIP6: IPv6 UDP port: 0x%04X\n", htons(BIP6_Addr.port)); + 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); + } + /* assumes that the driver has already been initialized */ + BIP6_Socket = socket(AF_INET6, SOCK_DGRAM, IPPROTO_UDP); + if (BIP6_Socket < 0) + return false; + /* Allow us to use the same socket for sending and receiving */ + /* This makes sure that the src port is correct when sending */ + sockopt = 1; + status = + setsockopt(BIP6_Socket, SOL_SOCKET, SO_REUSEADDR, &sockopt, + sizeof(sockopt)); + if (status < 0) { + close(BIP6_Socket); + BIP6_Socket = -1; + return status; + } + /* allow us to send a broadcast */ + status = + setsockopt(BIP6_Socket, SOL_SOCKET, SO_BROADCAST, &sockopt, + sizeof(sockopt)); + if (status < 0) { + close(BIP6_Socket); + BIP6_Socket = -1; + return false; + } + /* 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 choose the interface */ + join_request.ipv6mr_interface = 0; + status = setsockopt(BIP6_Socket, IPPROTO_IPV6, IPV6_JOIN_GROUP, + &join_request, sizeof(join_request)); + if (status < 0) { + perror("BIP: setsockopt(IPV6_JOIN_GROUP)"); + } + + /* bind the socket to the local port number and IP address */ + server.sin6_family = AF_INET6; + server.sin6_addr = in6addr_any; + server.sin6_port = htons(BIP6_Addr.port); + status = bind(BIP6_Socket, (const void*)&server, sizeof(server)); + if (status < 0) { + perror("BIP: bind"); + close(BIP6_Socket); + BIP6_Socket = -1; + return false; + } + bvlc6_init(); + + return true; +} diff --git a/bacnet-stack/ports/win32/bip6.c b/bacnet-stack/ports/win32/bip6.c new file mode 100644 index 00000000..c36b9292 --- /dev/null +++ b/bacnet-stack/ports/win32/bip6.c @@ -0,0 +1,407 @@ +/*####COPYRIGHTBEGIN#### + ------------------------------------------- + Copyright (C) 2016 Steve Karg + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to: + The Free Software Foundation, Inc. + 59 Temple Place - Suite 330 + Boston, MA 02111-1307, USA. + + As a special exception, if other files instantiate templates or + use macros or inline functions from this file, or you compile + this file and link it with other works to produce a work based + on this file, this file does not by itself cause the resulting + work to be covered by the GNU General Public License. However + the source code for this file must still be made available in + accordance with section (3) of the GNU General Public License. + + This exception does not invalidate any other reasons why a work + based on this file might be covered by the GNU General Public + License. + ------------------------------------------- +####COPYRIGHTEND####*/ + +#include +#include +#include /* for standard integer types uint8_t etc. */ +#include /* for the standard bool type. */ +#include "bacdcode.h" +#include "config.h" +#include "bip6.h" +#include "net.h" + +/* Win32 Socket */ +static SOCKET BIP6_Socket = INVALID_SOCKET; +/* local address - filled by init functions */ +static BACNET_IP6_ADDRESS BIP6_Addr; +static BACNET_IP6_ADDRESS BIP6_Broadcast_Addr; + +/* on Windows, ifname is the IPv6 address of the interface */ +void bip6_set_interface( + char *ifname) +{ + +} + +/** + * 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 }; + + /* assumes that the driver has already been initialized */ + if (BIP6_Socket < 0) { + return 0; + } + /* load destination IP address */ + bvlc_dest.sin6_family = AF_INET6; + memcpy(&bvlc_dest.sin6_addr, &dest->address[0], IP6_ADDRESS_MAX); + bvlc_dest.sin6_port = dest->port; + /* Send the packet */ + return sendto(BIP6_Socket, (char *) mtu, mtu_len, 0, + (struct sockaddr *) &bvlc_dest, sizeof(bvlc_dest)); +} + +/** + * 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 */ + fd_set read_fds; + int max = 0; + struct 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; + } + FD_ZERO(&read_fds); + FD_SET(BIP6_Socket, &read_fds); + max = BIP6_Socket; + /* see if there is a packet for us */ + if (select(max + 1, &read_fds, NULL, NULL, &select_timeout) > 0) { + received_bytes = + 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) { + 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) { + return 0; + } + /* pass the packet into the BBMD handler */ + memcpy(&addr.address[0], &(sin.sin6_addr), 16); + memcpy(&addr.port, &sin.sin6_port, 2); + 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 { + npdu_len = 0; + } + } + + return npdu_len; +} + +/** Cleanup and close out the BACnet/IP services by closing the socket. + * @ingroup DLBIP6 + */ +void bip6_cleanup( + void) +{ + if (BIP6_Socket != -1) { + close(BIP6_Socket); + } + BIP6_Socket = -1; + WSACleanup(); + + return; +} + +static LPSTR PrintError(int ErrorCode) +{ + static char Message[1024]; + + // If this program was multithreaded, we'd want to use + // FORMAT_MESSAGE_ALLOCATE_BUFFER instead of a static buffer here. + // (And of course, free the buffer when we were done with it) + + FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS | + FORMAT_MESSAGE_MAX_WIDTH_MASK, NULL, ErrorCode, + MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), + (LPSTR) Message, 1024, NULL); + return Message; +} + +/** 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). + * + * @note For Windows, ifname is the IPv6 address of the interface. + * + * https://msdn.microsoft.com/en-us/library/windows/desktop/ms738639(v=vs.85).aspx + * + * @param ifname [in] The named interface to use for the network layer. + * + * @return True if the socket is successfully opened for BACnet/IPv6, + * else False if the socket functions fail. + */ +bool bip6_init( + char *ifname) +{ + WSADATA wd; + int i, NumSocks, RetVal, FromLen, AmountRead; + SOCKADDR_STORAGE From; + ADDRINFO Hints, *AddrInfo, *AI; + SOCKET ServSock[FD_SETSIZE]; + fd_set SockSet; + + // Ask for Winsock version 2.2. + if ((RetVal = WSAStartup(MAKEWORD(2, 2), &wd)) != 0) { + fprintf(stderr, "WSAStartup failed with error %d: %s\n", + RetVal, PrintError(RetVal)); + WSACleanup(); + return -1; + } + // By setting the AI_PASSIVE flag in the hints to getaddrinfo, we're + // indicating that we intend to use the resulting address(es) to bind + // to a socket(s) for accepting incoming connections. This means that + // when the Address parameter is NULL, getaddrinfo will return one + // entry per allowed protocol family containing the unspecified address + // for that family. + // + memset(&Hints, 0, sizeof (Hints)); + Hints.ai_family = PF_INET6; + Hints.ai_socktype = SOCK_DGRAM; + Hints.ai_flags = AI_NUMERICHOST | AI_PASSIVE; + RetVal = getaddrinfo(ifname, BIP6_Addr.port, &Hints, &AddrInfo); + if (RetVal != 0) { + fprintf(stderr, "getaddrinfo failed with error %d: %s\n", + RetVal, gai_strerror(RetVal)); + WSACleanup(); + return -1; + } + // + // Find the first matchin address getaddrinfo returned so that + // we can create a new socket and bind that address to it, + // and create a queue to listen on. + // + for (i = 0, AI = AddrInfo; AI != NULL; AI = AI->ai_next) { + // Highly unlikely, but check anyway. + if (i == FD_SETSIZE) { + printf("getaddrinfo returned more addresses than we could use.\n"); + break; + } + // only support PF_INET6. + if (AI->ai_family != PF_INET6) { + continue; + } + // only support SOCK_DGRAM. + if (AI->ai_socktype != SOCK_DGRAM) { + continue; + } + // only support IPPROTO_UDP. + if (AI->ai_protocol != IPPROTO_UDP) { + continue; + } + BIP6_Socket = socket(PF_INET6, SOCK_DGRAM, IPPROTO_UDP); + if (BIP6_Socket == INVALID_SOCKET) { + fprintf(stderr, "socket() failed with error %d: %s\n", + WSAGetLastError(), PrintError(WSAGetLastError())); + continue; + } + if ((AI->ai_family == PF_INET6) && + IN6_IS_ADDR_LINKLOCAL((IN6_ADDR *) INETADDR_ADDRESS(AI->ai_addr)) && + (((SOCKADDR_IN6 *) (AI->ai_addr))->sin6_scope_id == 0) + ) { + fprintf(stderr, + "IPv6 link local addresses should specify a scope ID!\n"); + } + /* Allow us to use the same socket for sending and receiving */ + /* This makes sure that the src port is correct when sending */ + sockopt = 1; + RetVal = + setsockopt(BIP6_Socket, SOL_SOCKET, SO_REUSEADDR, &sockopt, + sizeof(sockopt)); + if (RetVal < 0) { + closesocket(BIP6_Socket); + BIP6_Socket = INVALID_SOCKET; + continue; + } + // + // bind() associates a local address and port combination + // with the socket just created. This is most useful when + // the application is a server that has a well-known port + // that clients know about in advance. + // + if (bind(BIP6_Socket, AI->ai_addr, (int) AI->ai_addrlen) == SOCKET_ERROR) { + fprintf(stderr, "bind() failed with error %d: %s\n", + WSAGetLastError(), PrintError(WSAGetLastError())); + closesocket(ServSock[i]); + continue; + } else { + /* FIXME: store the address */ + /* https://msdn.microsoft.com/en-us/library/windows/desktop/ms740496(v=vs.85).aspx */ + } + i++; + if (BIP6_Socket != INVALID_SOCKET) { + break; + } + } + freeaddrinfo(AddrInfo); + if (BIP6_Socket == INVALID_SOCKET) { + fprintf(stderr, "Fatal error: unable to serve on any address.\n"); + WSACleanup(); + return -1; + } + + return true; +} diff --git a/bacnet-stack/ports/win32/net.h b/bacnet-stack/ports/win32/net.h index 5c53a95a..b847a3f6 100644 --- a/bacnet-stack/ports/win32/net.h +++ b/bacnet-stack/ports/win32/net.h @@ -28,13 +28,33 @@ #define WIN32_LEAN_AND_MEAN #define STRICT 1 +/* WindowsXP - minimum */ +#define _NTDDI_VERSION_FROM_WIN32_WINNT2(ver) ver##0000 +#define _NTDDI_VERSION_FROM_WIN32_WINNT(ver) _NTDDI_VERSION_FROM_WIN32_WINNT2(ver) + +#ifndef _WIN32_WINNT +# define _WIN32_WINNT 0x501 +#endif +#ifndef NTDDI_VERSION +# define NTDDI_VERSION _NTDDI_VERSION_FROM_WIN32_WINNT(_WIN32_WINNT) +#endif #include #if (!defined(USE_INADDR) || (USE_INADDR == 0)) && \ (!defined(USE_CLASSADDR) || (USE_CLASSADDR == 0)) #include #endif +#include #include +#include +#include +#include +#ifdef __MINGW32__ +#include +#else +#include +#endif + #include #ifdef _MSC_VER diff --git a/bacnet-stack/src/bvlc6.c b/bacnet-stack/src/bvlc6.c new file mode 100644 index 00000000..caf86544 --- /dev/null +++ b/bacnet-stack/src/bvlc6.c @@ -0,0 +1,2338 @@ +/*####COPYRIGHTBEGIN#### + ------------------------------------------- + Copyright (C) 2015 Steve Karg + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to: + The Free Software Foundation, Inc. + 59 Temple Place - Suite 330 + Boston, MA 02111-1307, USA. + + As a special exception, if other files instantiate templates or + use macros or inline functions from this file, or you compile + this file and link it with other works to produce a work based + on this file, this file does not by itself cause the resulting + work to be covered by the GNU General Public License. However + the source code for this file must still be made available in + accordance with section (3) of the GNU General Public License. + + This exception does not invalidate any other reasons why a work + based on this file might be covered by the GNU General Public + License. + ------------------------------------------- +####COPYRIGHTEND####*/ + +#include /* for standard integer types uint8_t etc. */ +#include /* for the standard bool type. */ +#include +#include "bacenum.h" +#include "bacdcode.h" +#include "bacint.h" +#include "bvlc6.h" +#ifndef DEBUG_ENABLED +#define DEBUG_ENABLED 0 +#endif +#include "debug.h" + +/** Encode the BVLC header + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param message_type - BVLL Messages + * @param length - number of bytes for this message type + * + * @return number of bytes encoded + */ +int bvlc6_encode_header( + uint8_t * pdu, + uint16_t pdu_size, + uint8_t message_type, + uint16_t length) +{ + int bytes_encoded = 0; + + if (pdu && (pdu_size >= 2)) { + pdu[0] = BVLL_TYPE_BACNET_IP6; + pdu[1] = message_type; + /* The 2-octet BVLC Length field is the length, in octets, + of the entire BVLL message, including the two octets of the + length field itself, most significant octet first. */ + encode_unsigned16(&pdu[2], length); + bytes_encoded = 4; + } + + return bytes_encoded; +} + +/** Decode the BVLC Result message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param message_type - BVLL Messages + * @param length - number of bytes for this message type + * + * @return number of bytes decoded + */ +int bvlc6_decode_header( + uint8_t * pdu, + uint16_t pdu_len, + uint8_t * message_type, + uint16_t * length) +{ + int bytes_consumed = 0; + + if (pdu && (pdu_len >= 4)) { + if (pdu[0] == BVLL_TYPE_BACNET_IP6) { + if (message_type) { + *message_type = pdu[1]; + } + if (length) { + decode_unsigned16(&pdu[2], length); + } + bytes_consumed = 4; + } + } + + return bytes_consumed; +} + + +/** Encode the BVLC Result message + * + * This message provides a mechanism to acknowledge the result + * of those BVLL service requests that require an acknowledgment, + * whether successful (ACK) or unsuccessful (NAK). + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param result_code - BVLC result code + * + * @return number of bytes encoded + * + * BVLC Type: 1-octet X'82' BVLL for BACnet/IPv6 + * BVLC Function: 1-octet X'00' BVLC-Result + * BVLC Length: 2-octets X'0009' Length of the BVLL message + * Source-Virtual-Address 3-octets + * Result Code: 2-octets X'0000' Successful completion + * X'0030' Address-Resolution NAK + * X'0060' Virtual-Address-Resolution NAK + * X'0090' Register-Foreign-Device NAK + * X'00A0' Delete-Foreign-Device-Table-Entry NAK + * X'00C0' Distribute-Broadcast-To-Network NAK + */ +int bvlc6_encode_result( + uint8_t * pdu, + uint16_t pdu_size, + uint32_t vmac, + uint16_t result_code) +{ + int bytes_encoded = 0; + const uint16_t length = 9; + + if (pdu && (pdu_size >= 9) && (vmac <= 0xFFFFFF)) { + bytes_encoded = bvlc6_encode_header(pdu, pdu_size, + BVLC6_RESULT, length); + if (bytes_encoded == 4) { + encode_unsigned24(&pdu[4], vmac); + encode_unsigned16(&pdu[7], result_code); + bytes_encoded = length; + } + } + + return bytes_encoded; +} + +/** Decode the BVLC Result message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param vmac - Virtual MAC address + * @param result_code - BVLC result code + * + * @return number of bytes decoded + */ +int bvlc6_decode_result( + uint8_t * pdu, + uint16_t pdu_len, + uint32_t * vmac, + uint16_t * result_code) +{ + int bytes_consumed = 0; + + if (pdu && (pdu_len >= 5)) { + if (vmac) { + decode_unsigned24(&pdu[0], vmac); + } + if (result_code) { + decode_unsigned16(&pdu[3], result_code); + } + bytes_consumed = 5; + } + + return bytes_consumed; +} + +/** Encode the BVLC Original-Unicast-NPDU message + * + * This message is used to send directed NPDUs to another B/IPv6 node + * or router. + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param vmac_src - Source-Virtual-Address + * @param vmac_dst - Destination-Virtual-Address + * @param npdu - BACnet NPDU buffer + * @param npdu_len - size of the BACnet NPDU buffer + * + * @return number of bytes encoded + * + * BVLC Type: 1-octet X'82' BVLL for BACnet/IPv6 + * BVLC Function: 1-octet X'01' Original-Unicast-NPDU + * BVLC Length: 2-octets L Length of the BVLL message + * Source-Virtual-Address: 3-octets + * Destination-Virtual-Address: 3-octets + * BACnet NPDU: Variable length + */ +int bvlc6_encode_original_unicast( + uint8_t * pdu, + uint16_t pdu_size, + uint32_t vmac_src, + uint32_t vmac_dst, + uint8_t * npdu, + uint16_t npdu_len) +{ + int bytes_encoded = 0; + uint16_t length = 10; + uint16_t i = 0; + + + length += npdu_len; + if (pdu && + (pdu_size >= length) && + (vmac_src <= 0xFFFFFF) && + (vmac_dst <= 0xFFFFFF)) { + bytes_encoded = bvlc6_encode_header(pdu, pdu_size, + BVLC6_ORIGINAL_UNICAST_NPDU, length); + if (bytes_encoded == 4) { + encode_unsigned24(&pdu[4], vmac_src); + encode_unsigned24(&pdu[7], vmac_dst); + if (npdu && length) { + for (i = 0; i < npdu_len; i++) { + pdu[10+i] = npdu[i]; + } + } + bytes_encoded = length; + } + } + + return bytes_encoded; +} + +/** Decode the BVLC Original-Unicast-NPDU message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param vmac_src - Source-Virtual-Address + * @param vmac_dst - Destination-Virtual-Address + * @param npdu - BACnet NPDU buffer + * @param npdu_size - size of the buffer for the decoded BACnet NPDU + * @param npdu_len - decoded length of the BACnet NPDU buffer + * + * @return number of bytes decoded + */ +int bvlc6_decode_original_unicast( + uint8_t * pdu, + uint16_t pdu_len, + uint32_t * vmac_src, + uint32_t * vmac_dst, + uint8_t * npdu, + uint16_t npdu_size, + uint16_t * npdu_len) +{ + int bytes_consumed = 0; + uint16_t length = 0; + uint16_t i = 0; + + if (pdu && (pdu_len >= 6)) { + if (vmac_src) { + decode_unsigned24(&pdu[0], vmac_src); + } + if (vmac_dst) { + decode_unsigned24(&pdu[3], vmac_dst); + } + length = pdu_len - 6; + if (npdu && length && (length <= npdu_size)) { + for (i = 0; i < length; i++) { + npdu[i] = pdu[6+i]; + } + } + if (npdu_len) { + *npdu_len = length; + } + bytes_consumed = pdu_len; + } + + return bytes_consumed; +} + +/** Encode the BVLC Original-Broadcast-NPDU message + * + * This message is used by B/IPv6 nodes which are not + * foreign devices to broadcast NPDUs on a B/IPv6 network. + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param vmac - Source-Virtual-Address + * @param npdu - BACnet NPDU buffer + * @param npdu_len - size of the BACnet NPDU buffer + * + * @return number of bytes encoded + * + * BVLC Type: 1-octet X'82' BVLL for BACnet/IPv6 + * BVLC Function: 1-octet X'02' Original-Broadcast-NPDU + * BVLC Length: 2-octets L Length of the BVLL message + * Source-Virtual-Address: 3-octets + * BACnet NPDU: Variable length + */ +int bvlc6_encode_original_broadcast( + uint8_t * pdu, + uint16_t pdu_size, + uint32_t vmac, + uint8_t * npdu, + uint16_t npdu_len) +{ + int bytes_encoded = 0; + uint16_t length = 7; + uint16_t i = 0; + + length += npdu_len; + if (pdu && + (pdu_size >= length) && + (vmac <= 0xFFFFFF)) { + bytes_encoded = bvlc6_encode_header(pdu, pdu_size, + BVLC6_ORIGINAL_BROADCAST_NPDU, length); + if (bytes_encoded == 4) { + encode_unsigned24(&pdu[4], vmac); + if (npdu && length) { + for (i = 0; i < npdu_len; i++) { + pdu[7+i] = npdu[i]; + } + } + bytes_encoded = length; + } + } + + return bytes_encoded; +} + +/** Decode the BVLC Original-Broadcast-NPDU message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param vmac - decoded Source-Virtual-Address + * @param npdu - buffer to copy the decoded BACnet NDPU + * @param npdu_size - size of the buffer for the decoded BACnet NPDU + * @param npdu_len - decoded length of the BACnet NPDU + * + * @return number of bytes decoded + */ +int bvlc6_decode_original_broadcast( + uint8_t * pdu, + uint16_t pdu_len, + uint32_t * vmac, + uint8_t * npdu, + uint16_t npdu_size, + uint16_t * npdu_len) +{ + int bytes_consumed = 0; + uint16_t length = 0; + uint16_t i = 0; + + if (pdu && (pdu_len >= 3)) { + if (vmac) { + decode_unsigned24(&pdu[0], vmac); + } + length = pdu_len - 3; + if (npdu && length && (length <= npdu_size)) { + for (i = 0; i < length; i++) { + npdu[i] = pdu[3+i]; + } + } + if (npdu_len) { + *npdu_len = length; + } + bytes_consumed = pdu_len; + } + + return bytes_consumed; +} + +/** Encode the BVLC Address-Resolution message + * + * This message is unicast by B/IPv6 BBMDs to determine + * the B/IPv6 address of a known virtual address belonging to + * a different multicast domain. + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param vmac_src - Source-Virtual-Address + * @param vmac_target - Target-Virtual-Address + * + * @return number of bytes encoded + * + * BVLC Type: 1-octet X'82' BVLL for BACnet/IPv6 + * BVLC Function: 1-octet X'03' Address-Resolution + * BVLC Length: 2-octets X'000A' Length of the BVLL message + * Source-Virtual-Address: 3-octets + * Target-Virtual-Address: 3-octets + */ +int bvlc6_encode_address_resolution( + uint8_t * pdu, + uint16_t pdu_size, + uint32_t vmac_src, + uint32_t vmac_target) +{ + int bytes_encoded = 0; + uint16_t length = 10; + + + if (pdu && (pdu_size >= length) && + (vmac_src <= 0xFFFFFF) && + (vmac_target <= 0xFFFFFF)) { + bytes_encoded = bvlc6_encode_header(pdu, pdu_size, + BVLC6_ADDRESS_RESOLUTION, length); + if (bytes_encoded == 4) { + encode_unsigned24(&pdu[4], vmac_src); + encode_unsigned24(&pdu[7], vmac_target); + bytes_encoded = length; + } + } + + return bytes_encoded; +} + +/** Decode the BVLC Address-Resolution message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param vmac_src - Source-Virtual-Address + * @param vmac_target - Target-Virtual-Address + * + * @return number of bytes decoded + */ +int bvlc6_decode_address_resolution( + uint8_t * pdu, + uint16_t pdu_len, + uint32_t * vmac_src, + uint32_t * vmac_target) +{ + int bytes_consumed = 0; + + if (pdu && (pdu_len >= 6)) { + if (vmac_src) { + decode_unsigned24(&pdu[0], vmac_src); + } + if (vmac_target) { + decode_unsigned24(&pdu[3], vmac_target); + } + bytes_consumed = 6; + } + + return bytes_consumed; +} + +/** Encode the BVLC Address + * + * Data link layer addressing between B/IPv6 nodes consists of a 128-bit + * IPv6 address followed by a two-octet UDP port number (both of which + * shall be transmitted with the most significant octet first). This + * address shall be referred to as a B/IPv6 address. + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param bip6_address - B/IPv6 address + * + * @return number of bytes encoded + */ +int bvlc6_encode_address( + uint8_t * pdu, + uint16_t pdu_size, + BACNET_IP6_ADDRESS * bip6_address) +{ + int bytes_encoded = 0; + uint16_t length = BIP6_ADDRESS_MAX; + unsigned i = 0; + + if (pdu && (pdu_size >= length) && bip6_address) { + for (i = 0; i < IP6_ADDRESS_MAX; i++) { + pdu[i] = bip6_address->address[i]; + } + encode_unsigned16(&pdu[IP6_ADDRESS_MAX], bip6_address->port); + bytes_encoded = length; + } + + return bytes_encoded; +} + +/** Decode the BVLC Address-Resolution message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param bip6_address - B/IPv6 address + * + * @return number of bytes decoded + */ +int bvlc6_decode_address( + uint8_t * pdu, + uint16_t pdu_len, + BACNET_IP6_ADDRESS * bip6_address) +{ + int bytes_consumed = 0; + uint16_t length = BIP6_ADDRESS_MAX; + unsigned i = 0; + + if (pdu && (pdu_len >= length) && bip6_address) { + for (i = 0; i < IP6_ADDRESS_MAX; i++) { + bip6_address->address[i] = pdu[i]; + } + decode_unsigned16(&pdu[IP6_ADDRESS_MAX], &bip6_address->port); + bytes_consumed = length; + } + + return bytes_consumed; +} + +/** Copy the BVLC Address + * + * Data link layer addressing between B/IPv6 nodes consists of a 128-bit + * IPv6 address followed by a two-octet UDP port number (both of which + * shall be transmitted with the most significant octet first). This + * address shall be referred to as a B/IPv6 address. + * + * @param dst - B/IPv6 address that will be filled with src + * @param src - B/IPv6 address that will be copied into dst + * + * @return true if the address was copied + */ +bool bvlc6_address_copy( + BACNET_IP6_ADDRESS * dst, + BACNET_IP6_ADDRESS * src) +{ + bool status = false; + unsigned int i = 0; + + if (src && dst) { + for (i = 0; i < IP6_ADDRESS_MAX; i++) { + dst->address[i] = src->address[i]; + } + dst->port = src->port; + status = true; + } + + return status; +} + +/** Compare the BVLC Address + * + * Data link layer addressing between B/IPv6 nodes consists of a 128-bit + * IPv6 address followed by a two-octet UDP port number (both of which + * shall be transmitted with the most significant octet first). This + * address shall be referred to as a B/IPv6 address. + * + * @param dst - B/IPv6 address that will be compared to src + * @param src - B/IPv6 address that will be compared to dst + * + * @return true if the addresses are different + */ +bool bvlc6_address_different( + BACNET_IP6_ADDRESS * dst, + BACNET_IP6_ADDRESS * src) +{ + bool status = false; + unsigned int i = 0; + + if (src && dst) { + for (i = 0; i < IP6_ADDRESS_MAX; i++) { + if (dst->address[i] != src->address[i]) { + status = true; + } + } + if (dst->port != src->port) { + status = true; + } + } + + return status; +} + +/** Set a BVLC Address from 16-bit group chunks + * + * Data link layer addressing between B/IPv6 nodes consists of a 128-bit + * IPv6 address followed by a two-octet UDP port number (both of which + * shall be transmitted with the most significant octet first). This + * address shall be referred to as a B/IPv6 address. + * + * @param addr - B/IPv6 address that be set + * @param addr0 - B/IPv6 address 16-bit + * @param addr1 - B/IPv6 address bytes + * @param addr2 - B/IPv6 address bytes + * @param addr3 - B/IPv6 address bytes + * @param addr4 - B/IPv6 address bytes + * @param addr5 - B/IPv6 address bytes + * @param addr6 - B/IPv6 address bytes + * @param addr7 - B/IPv6 address bytes + * + * @return true if the address is set + */ +bool bvlc6_address_set( + BACNET_IP6_ADDRESS * addr, + uint16_t addr0, + uint16_t addr1, + uint16_t addr2, + uint16_t addr3, + uint16_t addr4, + uint16_t addr5, + uint16_t addr6, + uint16_t addr7) +{ + bool status = false; + + if (addr) { + encode_unsigned16(&addr->address[0], addr0); + encode_unsigned16(&addr->address[2], addr1); + encode_unsigned16(&addr->address[4], addr2); + encode_unsigned16(&addr->address[6], addr3); + encode_unsigned16(&addr->address[8], addr4); + encode_unsigned16(&addr->address[10], addr5); + encode_unsigned16(&addr->address[12], addr6); + encode_unsigned16(&addr->address[14], addr7); + status = true; + } + + return status; +} + +/** Get a BVLC Address into 16-bit group chunks + * + * Data link layer addressing between B/IPv6 nodes consists of a 128-bit + * IPv6 address followed by a two-octet UDP port number (both of which + * shall be transmitted with the most significant octet first). This + * address shall be referred to as a B/IPv6 address. + * + * @param addr - B/IPv6 address that be set + * @param addr0 - B/IPv6 address 16-bit + * @param addr1 - B/IPv6 address bytes + * @param addr2 - B/IPv6 address bytes + * @param addr3 - B/IPv6 address bytes + * @param addr4 - B/IPv6 address bytes + * @param addr5 - B/IPv6 address bytes + * @param addr6 - B/IPv6 address bytes + * @param addr7 - B/IPv6 address bytes + * + * @return true if the address is set + */ +bool bvlc6_address_get( + BACNET_IP6_ADDRESS * addr, + uint16_t *addr0, + uint16_t *addr1, + uint16_t *addr2, + uint16_t *addr3, + uint16_t *addr4, + uint16_t *addr5, + uint16_t *addr6, + uint16_t *addr7) +{ + bool status = false; + + if (addr) { + if (addr0) { + decode_unsigned16(&addr->address[0], addr0); + } + if (addr1) { + decode_unsigned16(&addr->address[2], addr1); + } + if (addr2) { + decode_unsigned16(&addr->address[4], addr2); + } + if (addr3) { + decode_unsigned16(&addr->address[6], addr3); + } + if (addr4) { + decode_unsigned16(&addr->address[8], addr4); + } + if (addr5) { + decode_unsigned16(&addr->address[10], addr5); + } + if (addr6) { + decode_unsigned16(&addr->address[12], addr6); + } + if (addr7) { + decode_unsigned16(&addr->address[14], addr7); + } + status = true; + } + + return status; +} + +/** Set a BACnet VMAC Address from a Device ID + * + * @param addr - BACnet address that be set + * @param device_id - 22-bit device ID + * + * @return true if the address is set + */ +bool bvlc6_vmac_address_set( + BACNET_ADDRESS * addr, + uint32_t device_id) +{ + bool status = false; + + if (addr) { + encode_unsigned24(&addr->mac[0], device_id); + addr->mac_len = 3; + addr->net = 0; + addr->len = 0; + status = true; + } + + return status; +} + +/** Get a BACnet VMAC Address from a Device ID + * + * @param addr - BACnet address that be set + * @param device_id - 22-bit device ID + * + * @return true if the address is set + */ +bool bvlc6_vmac_address_get( + BACNET_ADDRESS * addr, + uint32_t *device_id) +{ + bool status = false; + + if (addr && device_id) { + if (addr->mac_len == 3) { + decode_unsigned24(&addr->mac[0], device_id); + status = true; + } + } + + return status; +} + +/** Encode the BVLC Forwarded-Address-Resolution message + * + * This message is unicast by B/IPv6 BBMDs to determine + * the B/IPv6 address of a known virtual address belonging to + * a different multicast domain. + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param vmac_src - Source-Virtual-Address + * @param vmac_target - Target-Virtual-Address + * @param bip6_address - Original-Source-B/IPv6-Address + * + * @return number of bytes encoded + * + * BVLC Type: 1-octet X'82' BVLL for BACnet/IPv6 + * BVLC Function: 1-octet X'04' Forwarded-Address-Resolution + * BVLC Length: 2-octets X'001C' Length of this message + * Original-Source-Virtual-Address: 3-octets + * Target-Virtual-Address: 3-octets + * Original-Source-B/IPv6-Address 18-octets + */ +int bvlc6_encode_forwarded_address_resolution( + uint8_t * pdu, + uint16_t pdu_size, + uint32_t vmac_src, + uint32_t vmac_target, + BACNET_IP6_ADDRESS * bip6_address) +{ + int bytes_encoded = 0; + uint16_t length = 0x001C; + uint16_t offset = 0; + + if (pdu && (pdu_size >= length) && + (vmac_src <= 0xFFFFFF) && + (vmac_target <= 0xFFFFFF) && + bip6_address) { + bytes_encoded = bvlc6_encode_header(pdu, pdu_size, + BVLC6_FORWARDED_ADDRESS_RESOLUTION, length); + if (bytes_encoded == 4) { + offset = 4; + encode_unsigned24(&pdu[offset], vmac_src); + offset += 3; + encode_unsigned24(&pdu[offset], vmac_target); + offset += 3; + bvlc6_encode_address(&pdu[offset], pdu_size-offset, bip6_address); + bytes_encoded = length; + } + } + + return bytes_encoded; +} + +/** Decode the BVLC Forwarded-Address-Resolution message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param vmac_src - Source-Virtual-Address + * @param vmac_target - Target-Virtual-Address + * @param bip6_address - Original-Source-B/IPv6-Address + * + * @return number of bytes decoded + */ +int bvlc6_decode_forwarded_address_resolution( + uint8_t * pdu, + uint16_t pdu_len, + uint32_t * vmac_src, + uint32_t * vmac_target, + BACNET_IP6_ADDRESS * bip6_address) +{ + int bytes_consumed = 0; + const uint16_t length = 3 + 3 + BIP6_ADDRESS_MAX; + uint16_t offset = 0; + + if (pdu && (pdu_len >= length)) { + if (vmac_src) { + decode_unsigned24(&pdu[offset], vmac_src); + } + offset += 3; + if (vmac_target) { + decode_unsigned24(&pdu[offset], vmac_target); + } + offset += 3; + if (bip6_address) { + bvlc6_decode_address(&pdu[offset], pdu_len-offset, bip6_address); + } + bytes_consumed = length; + } + + return bytes_consumed; +} + +/** Encode generic BVLC Address-Ack message + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param vmac_src - Source-Virtual-Address + * @param vmac_dst - Destination-Virtual-Address + * + * @return number of bytes encoded + */ +static int bvlc6_encode_address_ack( + uint8_t message_type, + uint8_t * pdu, + uint16_t pdu_size, + uint32_t vmac_src, + uint32_t vmac_dst) +{ + int bytes_encoded = 0; + const uint16_t length = 10; + uint16_t offset = 0; + + if (pdu && (pdu_size >= length) && + (vmac_src <= 0xFFFFFF) && + (vmac_dst <= 0xFFFFFF)) { + bytes_encoded = bvlc6_encode_header(pdu, pdu_size, + message_type, length); + if (bytes_encoded == 4) { + offset = 4; + encode_unsigned24(&pdu[offset], vmac_src); + offset += 3; + encode_unsigned24(&pdu[offset], vmac_dst); + bytes_encoded = length; + } + } + + return bytes_encoded; +} + +/** Encode the BVLC Address-Resolution-Ack message + * + * This message is the reply to either the Address-Resolution or + * the Forwarded-Address-Resolution messages. + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param vmac_src - Source-Virtual-Address + * @param vmac_dst - Destination-Virtual-Address + * + * @return number of bytes encoded + * + * BVLC Type: 1-octet X'82' BVLL for BACnet/IPv6 + * BVLC Function: 1-octet X'05' Address-Resolution-Ack + * BVLC Length: 2-octets X'000A' Length of the BVLL message + * Source-Virtual-Address: 3-octets + * Destination-Virtual-Address: 3-octets + */ +int bvlc6_encode_address_resolution_ack( + uint8_t * pdu, + uint16_t pdu_size, + uint32_t vmac_src, + uint32_t vmac_dst) +{ + return bvlc6_encode_address_ack( + BVLC6_ADDRESS_RESOLUTION_ACK, + pdu, pdu_size,vmac_src, vmac_dst); +} + +/** Decode the BVLC Address-Resolution-Ack message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param vmac_src - Source-Virtual-Address + * @param vmac_dst - Destination-Virtual-Address + * + * @return number of bytes decoded + */ +int bvlc6_decode_address_resolution_ack( + uint8_t * pdu, + uint16_t pdu_len, + uint32_t * vmac_src, + uint32_t * vmac_dst) +{ + int bytes_consumed = 0; + const uint16_t length = 6; + uint16_t offset = 0; + + if (pdu && (pdu_len >= length)) { + if (vmac_src) { + decode_unsigned24(&pdu[offset], vmac_src); + } + offset += 3; + if (vmac_dst) { + decode_unsigned24(&pdu[offset], vmac_dst); + } + bytes_consumed = length; + } + + return bytes_consumed; +} + +/** Encode the BVLC Virtual-Address-Resolution message + * + * This message is unicast by B/IPv6 nodes to determine the + * virtual address of a device with a known B/IPv6 address. + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param vmac_src - Source-Virtual-Address + * + * @return number of bytes encoded + * + * BVLC Type: 1-octet X'82' BVLL for BACnet/IPv6 + * BVLC Function: 1-octet X'06' Virtual-Address-Resolution + * BVLC Length: 2-octets X'0007' Length of the BVLL message + * Source-Virtual-Address: 3-octets + */ +int bvlc6_encode_virtual_address_resolution( + uint8_t * pdu, + uint16_t pdu_size, + uint32_t vmac_src) +{ + int bytes_encoded = 0; + const uint16_t length = 7; + + if (pdu && (pdu_size >= length) && + (vmac_src <= 0xFFFFFF)) { + bytes_encoded = bvlc6_encode_header(pdu, pdu_size, + BVLC6_VIRTUAL_ADDRESS_RESOLUTION, length); + if (bytes_encoded == 4) { + encode_unsigned24(&pdu[4], vmac_src); + bytes_encoded = length; + } + } + + return bytes_encoded; +} + +/** Decode the BVLC Virtual-Address-Resolution message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param vmac_src - Source-Virtual-Address + * + * @return number of bytes decoded + */ +int bvlc6_decode_virtual_address_resolution( + uint8_t * pdu, + uint16_t pdu_len, + uint32_t * vmac_src) +{ + int bytes_consumed = 0; + + if (pdu && (pdu_len >= 3)) { + if (vmac_src) { + decode_unsigned24(&pdu[0], vmac_src); + } + bytes_consumed = 3; + } + + return bytes_consumed; +} + +/** Encode the BVLC Virtual-Address-Resolution-Ack message + * + * This message is the reply to the Virtual-Address-Resolution message + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param vmac_src - Source-Virtual-Address + * @param vmac_dst - Destination-Virtual-Address + * + * @return number of bytes encoded + * + * BVLC Type: 1-octet X'82' BVLL for BACnet/IPv6 + * BVLC Function: 1-octet X'07' Address-Resolution-Ack + * BVLC Length: 2-octets X'000A' Length of the BVLL message + * Source-Virtual-Address: 3-octets + * Destination-Virtual-Address: 3-octets + */ +int bvlc6_encode_virtual_address_resolution_ack( + uint8_t * pdu, + uint16_t pdu_size, + uint32_t vmac_src, + uint32_t vmac_dst) +{ + return bvlc6_encode_address_ack( + BVLC6_VIRTUAL_ADDRESS_RESOLUTION_ACK, + pdu, pdu_size,vmac_src, vmac_dst); +} + +/** Decode the BVLC Virtual-Address-Resolution-Ack message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param vmac_src - Source-Virtual-Address + * @param vmac_dst - Destination-Virtual-Address + * + * @return number of bytes decoded + */ +int bvlc6_decode_virtual_address_resolution_ack( + uint8_t * pdu, + uint16_t pdu_len, + uint32_t * vmac_src, + uint32_t * vmac_dst) +{ + return bvlc6_decode_address_resolution_ack(pdu, pdu_len, + vmac_src, vmac_dst); +} + +/** Encode the BVLC Forwarded-NPDU message + * + * This BVLL message is used in multicast messages from a BBMD + * as well as in messages forwarded to registered foreign + * devices. It contains the source address of the original + * node as well as the original BACnet NPDU. + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param vmac_src - Original-Source-Virtual-Address + * @param bip6_address - Original-Source-B/IPv6-Address + * @param npdu - BACnet NPDU from Originating Device buffer + * @param npdu_len - size of the BACnet NPDU buffer + * + * @return number of bytes encoded + * + * BVLC Type: 1-octet X'82' BVLL for BACnet/IPv6 + * BVLC Function: 1-octet X'08' Forwarded-NPDU + * BVLC Length: 2-octets L Length of the BVLL message + * Original-Source-Virtual-Address: 3-octets + * Original-Source-B-IPv6-Address: 18-octets + * BACnet NPDU from Originating Device: N-octets (N=L-25) + */ +int bvlc6_encode_forwarded_npdu( + uint8_t * pdu, + uint16_t pdu_size, + uint32_t vmac_src, + BACNET_IP6_ADDRESS * bip6_address, + uint8_t * npdu, + uint16_t npdu_len) +{ + int bytes_encoded = 0; + uint16_t length = 1+1+2+3+BIP6_ADDRESS_MAX; + uint16_t i = 0; + uint16_t offset = 0; + + length += npdu_len; + if (pdu && + (pdu_size >= length) && + (vmac_src <= 0xFFFFFF)) { + bytes_encoded = bvlc6_encode_header(pdu, pdu_size, + BVLC6_FORWARDED_NPDU, length); + if (bytes_encoded == 4) { + offset = 4; + encode_unsigned24(&pdu[offset], vmac_src); + offset += 3; + bvlc6_encode_address(&pdu[offset], pdu_size-offset, + bip6_address); + offset += BIP6_ADDRESS_MAX; + if (npdu && length) { + for (i = 0; i < npdu_len; i++) { + pdu[offset+i] = npdu[i]; + } + } + bytes_encoded = length; + } + } + + return bytes_encoded; +} + +/** Decode the BVLC Forwarded-NPDU message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param vmac_src - Original-Source-Virtual-Address + * @param bip6_address - Original-Source-B/IPv6-Address + * @param npdu - BACnet NPDU buffer + * @param npdu_size - size of the buffer for the decoded BACnet NPDU + * @param npdu_len - decoded length of the BACnet NPDU buffer + * + * @return number of bytes decoded + */ +int bvlc6_decode_forwarded_npdu( + uint8_t * pdu, + uint16_t pdu_len, + uint32_t * vmac_src, + BACNET_IP6_ADDRESS * bip6_address, + uint8_t * npdu, + uint16_t npdu_size, + uint16_t * npdu_len) +{ + int bytes_consumed = 0; + uint16_t length = 0; + uint16_t i = 0; + const uint16_t address_len = 3+BIP6_ADDRESS_MAX; + uint16_t offset = 0; + + if (pdu && (pdu_len >= address_len)) { + if (vmac_src) { + decode_unsigned24(&pdu[offset], vmac_src); + } + offset += 3; + if (bip6_address) { + bvlc6_decode_address(&pdu[offset], pdu_len-offset, + bip6_address); + } + offset += BIP6_ADDRESS_MAX; + length = pdu_len - offset; + if (npdu && length && (length <= npdu_size)) { + for (i = 0; i < length; i++) { + npdu[i] = pdu[offset+i]; + } + } + if (npdu_len) { + *npdu_len = length; + } + bytes_consumed = pdu_len; + } + + return bytes_consumed; +} + +/** Encode the BVLC Register-Foreign-Device message + * + * This message allows a foreign device, as defined in X.4.5.1, + * to register with a BBMD for the purpose of receiving + * broadcast messages. + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param vmac_src - Source-Virtual-Address + * @param ttl_seconds - Time-to-Live T, in seconds + * + * @return number of bytes encoded + * + * BVLC Type: 1-octet X'82' BVLL for BACnet/IPv6 + * BVLC Function: 1-octet X'09' Register-Foreign-Device + * BVLC Length: 2-octets X'0009' Length of the BVLL message + * Source-Virtual-Address: 3-octets + * Time-to-Live: 2-octets T Time-to-Live T, in seconds + */ +int bvlc6_encode_register_foreign_device( + uint8_t * pdu, + uint16_t pdu_size, + uint32_t vmac_src, + uint16_t ttl_seconds) +{ + int bytes_encoded = 0; + const uint16_t length = 9; + uint16_t offset = 0; + + if (pdu && (pdu_size >= length) && + (vmac_src <= 0xFFFFFF)) { + bytes_encoded = bvlc6_encode_header(pdu, pdu_size, + BVLC6_REGISTER_FOREIGN_DEVICE, length); + if (bytes_encoded == 4) { + offset = 4; + encode_unsigned24(&pdu[offset], vmac_src); + offset += 3; + encode_unsigned16(&pdu[offset], ttl_seconds); + bytes_encoded = length; + } + } + + return bytes_encoded; +} + +/** Decode the BVLC Register-Foreign-Device message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param vmac_src - Source-Virtual-Address + * @param ttl_seconds - Time-to-Live T, in seconds + * + * @return number of bytes decoded + */ +int bvlc6_decode_register_foreign_device( + uint8_t * pdu, + uint16_t pdu_len, + uint32_t * vmac_src, + uint16_t * ttl_seconds) +{ + int bytes_consumed = 0; + const uint16_t length = 5; + uint16_t offset = 0; + + if (pdu && (pdu_len >= length)) { + if (vmac_src) { + decode_unsigned24(&pdu[offset], vmac_src); + } + offset += 3; + if (ttl_seconds) { + decode_unsigned16(&pdu[offset], ttl_seconds); + } + bytes_consumed = length; + } + + return bytes_consumed; +} + +/** Encode the BVLC Delete-Foreign-Device message + * + * This message allows a foreign device, as defined in X.4.5.1, + * to register with a BBMD for the purpose of receiving + * broadcast messages. + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param vmac_src - Source-Virtual-Address + * @param fdt_entry - FDT Entry + * + * @return number of bytes encoded + * + * BVLC Type: 1-octet X'82' BVLL for BACnet/IPv6 + * BVLC Function: 1-octet X'0A' Delete-Foreign-Device + * BVLC Length: 2-octets X'0019' Length of the BVLL message + * Source-Virtual-Address: 3-octets + * FDT Entry: 18-octets + */ +int bvlc6_encode_delete_foreign_device( + uint8_t * pdu, + uint16_t pdu_size, + uint32_t vmac_src, + BACNET_IP6_FOREIGN_DEVICE_TABLE_ENTRY * fdt_entry) +{ + int bytes_encoded = 0; + const uint16_t length = 0x0019; + uint16_t offset = 0; + + if (pdu && (pdu_size >= length) && + (vmac_src <= 0xFFFFFF)) { + bytes_encoded = bvlc6_encode_header(pdu, pdu_size, + BVLC6_DELETE_FOREIGN_DEVICE, length); + if (bytes_encoded == 4) { + offset = 4; + encode_unsigned24(&pdu[offset], vmac_src); + offset += 3; + if (fdt_entry) { + bvlc6_encode_address(&pdu[offset], pdu_size-offset, + &fdt_entry->bip6_address); + offset += BIP6_ADDRESS_MAX; + encode_unsigned16(&pdu[offset], + fdt_entry->ttl_seconds); + offset += 2; + encode_unsigned16(&pdu[offset], + fdt_entry->ttl_seconds_remaining); + bytes_encoded = length; + } + } + } + + return bytes_encoded; +} + +/** Decode the BVLC Delete-Foreign-Device message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param vmac_src - Source-Virtual-Address + * @param fdt_entry - FDT Entry + * + * @return number of bytes decoded + */ +int bvlc6_decode_delete_foreign_device( + uint8_t * pdu, + uint16_t pdu_len, + uint32_t * vmac_src, + BACNET_IP6_FOREIGN_DEVICE_TABLE_ENTRY * fdt_entry) +{ + int bytes_consumed = 0; + + const uint16_t length = BIP6_ADDRESS_MAX+3+2+2-4; + uint16_t offset = 0; + + if (pdu && (pdu_len >= length)) { + if (vmac_src) { + decode_unsigned24(&pdu[offset], vmac_src); + bytes_consumed = 3; + } + offset += 3; + if (fdt_entry) { + bvlc6_decode_address(&pdu[offset], pdu_len-offset, + &fdt_entry->bip6_address); + offset += BIP6_ADDRESS_MAX; + decode_unsigned16(&pdu[offset], + &fdt_entry->ttl_seconds); + offset += 2; + decode_unsigned16(&pdu[offset], + &fdt_entry->ttl_seconds_remaining); + bytes_consumed = length; + } + bytes_consumed = length; + } + + return bytes_consumed; +} + +/** Encode the BVLC Secure-BVLL message + * + * This message is used to secure BVLL messages that do not contain NPDUs. + * Its use is described in Clause 24. + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param sbuf - Security Wrapper buffer + * @param sbuf_len - size of the Security Wrapper buffer + * + * @return number of bytes encoded + * + * BVLC Type: 1-octet X'82' BVLL for BACnet/IPv6 + * BVLC Function: 1-octet X'0B' Secure-BVLL + * BVLC Length: 2-octets L Length of the BVLL message + * Security Wrapper: Variable length + */ +int bvlc6_encode_secure_bvll( + uint8_t * pdu, + uint16_t pdu_size, + uint8_t * sbuf, + uint16_t sbuf_len) +{ + int bytes_encoded = 0; + uint16_t length = 4; + uint16_t i = 0; + + length += sbuf_len; + if (pdu && + (pdu_size >= length)) { + bytes_encoded = bvlc6_encode_header(pdu, pdu_size, + BVLC6_SECURE_BVLL, length); + if (bytes_encoded == 4) { + if (sbuf && sbuf_len) { + for (i = 0; i < sbuf_len; i++) { + pdu[4+i] = sbuf[i]; + } + } + bytes_encoded = length; + } + } + + return bytes_encoded; +} + +/** Decode the BVLC Secure-BVLL message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param sbuf - Security Wrapper buffer + * @param sbuf_size - size of the Security Wrapper buffer + * @param sbuf_len - number of bytes decoded into the Security Wrapper buffer + * + * @return number of bytes decoded + */ +int bvlc6_decode_secure_bvll( + uint8_t * pdu, + uint16_t pdu_len, + uint8_t * sbuf, + uint16_t sbuf_size, + uint16_t * sbuf_len) +{ + int bytes_consumed = 0; + uint16_t i = 0; + + if (pdu && sbuf) { + if (sbuf_len) { + *sbuf_len = pdu_len; + } + if (pdu_len) { + for (i = 0; i < pdu_len; i++) { + sbuf[i] = pdu[i]; + } + } + bytes_consumed = pdu_len; + } + + return bytes_consumed; +} + +/** Encode the BVLC Distribute-Broadcast-To-Network message + * + * This message provides a mechanism whereby a foreign device + * shall cause a BBMD to distribute a Forwarded-NPDU + * BVLC to the local multicast domain, to all BBMD’s configured + * in the BBMD’s BDT, and to all foreign devices in the + * BBMD’s FDT. + * + * @param pdu - buffer to store the encoding + * @param pdu_size - size of the buffer to store encoding + * @param vmac - Original-Source-Virtual-Address + * @param npdu - BACnet NPDU from Originating Device buffer + * @param npdu_len - size of the BACnet NPDU buffer + * + * @return number of bytes encoded + * + * BVLC Type: 1-octet X'82' BVLL for BACnet/IPv6 + * BVLC Function: 1-octet X'0C' Original-Unicast-NPDU + * BVLC Length: 2-octets L Length of the BVLL message + * Original-Source-Virtual-Address: 3-octets + * BACnet NPDU from Originating Device: Variable length + */ +int bvlc6_encode_distribute_broadcast_to_network( + uint8_t * pdu, + uint16_t pdu_size, + uint32_t vmac, + uint8_t * npdu, + uint16_t npdu_len) +{ + int bytes_encoded = 0; + uint16_t length = 7; + uint16_t i = 0; + + + length += npdu_len; + if (pdu && + (pdu_size >= length) && + (vmac <= 0xFFFFFF)) { + bytes_encoded = bvlc6_encode_header(pdu, pdu_size, + BVLC6_DISTRIBUTE_BROADCAST_TO_NETWORK, length); + if (bytes_encoded == 4) { + encode_unsigned24(&pdu[4], vmac); + if (npdu && length) { + for (i = 0; i < npdu_len; i++) { + pdu[7+i] = npdu[i]; + } + } + bytes_encoded = length; + } + } + + return bytes_encoded; +} + +/** Decode the BVLC Original-Broadcast-NPDU message + * + * @param pdu - buffer from which to decode the message + * @param pdu_len - length of the buffer that needs decoding + * @param vmac - decoded Original-Source-Virtual-Address + * @param npdu - buffer to copy the decoded BACnet NDPU + * @param npdu_size - size of the buffer for the decoded BACnet NPDU + * @param npdu_len - decoded length of the BACnet NPDU + * + * @return number of bytes decoded + */ +int bvlc6_decode_distribute_broadcast_to_network( + uint8_t * pdu, + uint16_t pdu_len, + uint32_t * vmac, + uint8_t * npdu, + uint16_t npdu_size, + uint16_t * npdu_len) +{ + int bytes_consumed = 0; + uint16_t length = 0; + uint16_t i = 0; + + if (pdu && (pdu_len >= 3)) { + if (vmac) { + decode_unsigned24(&pdu[0], vmac); + } + length = pdu_len - 3; + if (npdu && length && (length <= npdu_size)) { + for (i = 0; i < length; i++) { + npdu[i] = pdu[3+i]; + } + } + if (npdu_len) { + *npdu_len = length; + } + bytes_consumed = pdu_len; + } + + return bytes_consumed; +} + +#ifdef TEST +#include +#include +#include "ctest.h" + +static void test_BVLC6_Address( + Test * pTest, + BACNET_IP6_ADDRESS * bip6_address_1, + BACNET_IP6_ADDRESS * bip6_address_2) +{ + unsigned i = 0; + + if (bip6_address_1 && bip6_address_2) { + ct_test(pTest, bip6_address_1->port == bip6_address_2->port); + for (i = 0; i < IP6_ADDRESS_MAX; i++) { + ct_test(pTest, bip6_address_1->address[i] == + bip6_address_2->address[i]); + } + } + + return; +} + +static int test_BVLC6_Header( + Test * pTest, + uint8_t * pdu, + uint16_t pdu_len, + uint8_t * message_type, + uint16_t * length) + +{ + int bytes_consumed = 0; + int len = 0; + + if (pdu && message_type && length) { + len = bvlc6_decode_header(pdu, pdu_len, message_type, + length); + ct_test(pTest, len == 4); + bytes_consumed = len; + } + + return bytes_consumed; +} + +static void test_BVLC6_Result_Code( + Test * pTest, + uint32_t vmac, + uint16_t result_code) +{ + uint8_t pdu[50] = { 0 }; + uint32_t test_vmac = 0; + uint16_t test_result_code = 0; + uint8_t message_type = 0; + uint16_t length = 0; + int len = 0, test_len = 0; + + len = bvlc6_encode_result(pdu, sizeof(pdu), vmac, result_code); + ct_test(pTest, len == 9); + test_len = test_BVLC6_Header(pTest, + pdu, len, &message_type, &length); + ct_test(pTest, test_len == 4); + ct_test(pTest, message_type == BVLC6_RESULT); + ct_test(pTest, length == 9); + test_len += bvlc6_decode_result(&pdu[4], length-4, + &test_vmac, &test_result_code); + ct_test(pTest, len == test_len); + ct_test(pTest, vmac == test_vmac); + ct_test(pTest, result_code == test_result_code); + len = bvlc6_encode_result(pdu, sizeof(pdu), 0xffffff+1, result_code); + ct_test(pTest, len == 0); +} + +static void test_BVLC6_Result( + Test * pTest) +{ + uint32_t vmac = 0; + uint16_t result_code[6] = { + BVLC6_RESULT_SUCCESSFUL_COMPLETION, + BVLC6_RESULT_ADDRESS_RESOLUTION_NAK, + BVLC6_RESULT_VIRTUAL_ADDRESS_RESOLUTION_NAK, + BVLC6_RESULT_REGISTER_FOREIGN_DEVICE_NAK, + BVLC6_RESULT_DELETE_FOREIGN_DEVICE_NAK, + BVLC6_RESULT_DISTRIBUTE_BROADCAST_TO_NETWORK_NAK + }; + unsigned int i = 0; + + vmac = 4194303; + for (i = 0; i < 6; i++) { + test_BVLC6_Result_Code(pTest, vmac, result_code[i]); + } +} + +static void test_BVLC6_Original_Unicast_NPDU_Message( + Test * pTest, + uint8_t * npdu, + uint16_t npdu_len, + uint32_t vmac_src, + uint32_t vmac_dst) +{ + uint8_t test_npdu[50] = { 0 }; + uint8_t pdu[60] = { 0 }; + uint32_t test_vmac_src = 0; + uint32_t test_vmac_dst = 0; + uint16_t test_npdu_len = 0; + uint8_t message_type = 0; + uint16_t length = 0; + int len = 0, msg_len = 0, test_len = 0; + uint16_t i = 0; + + len = bvlc6_encode_original_unicast(pdu, sizeof(pdu), + vmac_src, vmac_dst, + npdu, npdu_len); + msg_len = 10 + npdu_len; + ct_test(pTest, len == msg_len); + test_len = test_BVLC6_Header(pTest, + pdu, len, &message_type, &length); + ct_test(pTest, test_len == 4); + ct_test(pTest, message_type == BVLC6_ORIGINAL_UNICAST_NPDU); + ct_test(pTest, length == msg_len); + test_len += bvlc6_decode_original_unicast(&pdu[4], length-4, + &test_vmac_src, &test_vmac_dst, + test_npdu, sizeof(test_npdu), &test_npdu_len); + ct_test(pTest, len == test_len); + ct_test(pTest, msg_len == test_len); + ct_test(pTest, vmac_src == test_vmac_src); + ct_test(pTest, vmac_dst == test_vmac_dst); + ct_test(pTest, npdu_len == test_npdu_len); + for (i = 0; i < npdu_len; i++) { + ct_test(pTest, npdu[i] == test_npdu[i]); + } +} + +static void test_BVLC6_Original_Unicast_NPDU( + Test * pTest) +{ + uint8_t npdu[50] = { 0 }; + uint32_t vmac_src = 0; + uint32_t vmac_dst = 0; + uint16_t npdu_len = 0; + uint16_t i = 0; + + test_BVLC6_Original_Unicast_NPDU_Message(pTest, + npdu, npdu_len, vmac_src, vmac_dst); + /* now with some NPDU data */ + for (i = 0; i < sizeof(npdu); i++) { + npdu[i] = i; + } + npdu_len = sizeof(npdu); + vmac_src = 4194303; + vmac_dst = 4194302; + test_BVLC6_Original_Unicast_NPDU_Message(pTest, + npdu, npdu_len, vmac_src, vmac_dst); +} + +static void test_BVLC6_Original_Broadcast_NPDU_Message( + Test * pTest, + uint8_t * npdu, + uint16_t npdu_len, + uint32_t vmac) +{ + uint8_t test_npdu[50] = { 0 }; + uint8_t pdu[60] = { 0 }; + uint32_t test_vmac = 0; + uint16_t test_npdu_len = 0; + uint8_t message_type = 0; + uint16_t length = 0; + int len = 0, msg_len = 0, test_len = 0; + uint16_t i = 0; + + len = bvlc6_encode_original_broadcast(pdu, sizeof(pdu), + vmac, npdu, npdu_len); + msg_len = 7 + npdu_len; + ct_test(pTest, len == msg_len); + test_len = test_BVLC6_Header(pTest, + pdu, len, &message_type, &length); + ct_test(pTest, test_len == 4); + ct_test(pTest, message_type == BVLC6_ORIGINAL_BROADCAST_NPDU); + ct_test(pTest, length == msg_len); + test_len += bvlc6_decode_original_broadcast(&pdu[4], length-4, + &test_vmac, test_npdu, sizeof(test_npdu), &test_npdu_len); + ct_test(pTest, len == test_len); + ct_test(pTest, msg_len == test_len); + ct_test(pTest, vmac == test_vmac); + ct_test(pTest, npdu_len == test_npdu_len); + for (i = 0; i < npdu_len; i++) { + ct_test(pTest, npdu[i] == test_npdu[i]); + } +} + +static void test_BVLC6_Original_Broadcast_NPDU( + Test * pTest) +{ + uint8_t npdu[50] = { 0 }; + uint32_t vmac = 0; + uint16_t npdu_len = 0; + uint16_t i = 0; + + test_BVLC6_Original_Broadcast_NPDU_Message(pTest, + npdu, npdu_len, vmac); + /* now with some NPDU data */ + for (i = 0; i < sizeof(npdu); i++) { + npdu[i] = i; + } + npdu_len = sizeof(npdu); + vmac = 4194303; + test_BVLC6_Original_Broadcast_NPDU_Message(pTest, + npdu, npdu_len, vmac); +} + +static void test_BVLC6_Address_Resolution_Message( + Test * pTest, + uint32_t vmac_src, + uint32_t vmac_target) +{ + uint8_t pdu[60] = { 0 }; + uint32_t test_vmac_src = 0; + uint32_t test_vmac_target = 0; + uint8_t message_type = 0; + uint16_t length = 0; + int len = 0, test_len = 0; + const int msg_len = 10; + + len = bvlc6_encode_address_resolution(pdu, sizeof(pdu), + vmac_src, vmac_target); + ct_test(pTest, len == msg_len); + test_len = test_BVLC6_Header(pTest, + pdu, len, &message_type, &length); + ct_test(pTest, test_len == 4); + ct_test(pTest, message_type == BVLC6_ADDRESS_RESOLUTION); + ct_test(pTest, length == msg_len); + test_len += bvlc6_decode_address_resolution(&pdu[4], length-4, + &test_vmac_src, &test_vmac_target); + ct_test(pTest, len == test_len); + ct_test(pTest, msg_len == test_len); + ct_test(pTest, vmac_src == test_vmac_src); + ct_test(pTest, vmac_target == test_vmac_target); +} + +static void test_BVLC6_Address_Resolution( + Test * pTest) +{ + uint32_t vmac_src = 0; + uint32_t vmac_target = 0; + + test_BVLC6_Address_Resolution_Message(pTest, + vmac_src, vmac_target); + vmac_src = 4194303; + vmac_target = 4194302; + test_BVLC6_Address_Resolution_Message(pTest, + vmac_src, vmac_target); +} + +static void test_BVLC6_Forwarded_Address_Resolution_Message( + Test * pTest, + uint32_t vmac_src, + uint32_t vmac_dst, + BACNET_IP6_ADDRESS * bip6_address) +{ + BACNET_IP6_ADDRESS test_bip6_address = {{0}}; + uint8_t pdu[60] = { 0 }; + uint32_t test_vmac_src = 0; + uint32_t test_vmac_dst = 0; + uint8_t message_type = 0; + uint16_t length = 0; + int len = 0, test_len = 0; + const int msg_len = 4 + 3 + 3 + BIP6_ADDRESS_MAX; + + len = bvlc6_encode_forwarded_address_resolution( + pdu, sizeof(pdu), + vmac_src, vmac_dst, + bip6_address); + ct_test(pTest, len == msg_len); + test_len = test_BVLC6_Header(pTest, + pdu, len, &message_type, &length); + ct_test(pTest, test_len == 4); + ct_test(pTest, message_type == BVLC6_FORWARDED_ADDRESS_RESOLUTION); + ct_test(pTest, length == msg_len); + test_len += bvlc6_decode_forwarded_address_resolution( + &pdu[4], length-4, + &test_vmac_src, &test_vmac_dst, + &test_bip6_address); + ct_test(pTest, len == test_len); + ct_test(pTest, msg_len == test_len); + ct_test(pTest, vmac_src == test_vmac_src); + ct_test(pTest, vmac_dst == test_vmac_dst); + test_BVLC6_Address(pTest, bip6_address, &test_bip6_address); +} + +static void test_BVLC6_Forwarded_Address_Resolution( + Test * pTest) +{ + BACNET_IP6_ADDRESS bip6_address = {{0}}; + uint32_t vmac_src = 0; + uint32_t vmac_target = 0; + uint16_t i = 0; + + test_BVLC6_Forwarded_Address_Resolution_Message(pTest, + vmac_src, vmac_target, &bip6_address); + /* now with some address data */ + for (i = 0; i < sizeof(bip6_address.address); i++) { + bip6_address.address[i] = i; + } + bip6_address.port = 47808; + vmac_src = 4194303; + vmac_target = 4194302; + test_BVLC6_Forwarded_Address_Resolution_Message(pTest, + vmac_src, vmac_target, &bip6_address); +} + +static void test_BVLC6_Address_Resolution_Ack_Message( + Test * pTest, + uint32_t vmac_src, + uint32_t vmac_dst) +{ + uint8_t pdu[60] = { 0 }; + uint32_t test_vmac_src = 0; + uint32_t test_vmac_dst = 0; + uint8_t message_type = 0; + uint16_t length = 0; + int len = 0, test_len = 0; + const int msg_len = 10; + + len = bvlc6_encode_address_resolution_ack(pdu, sizeof(pdu), + vmac_src, vmac_dst); + ct_test(pTest, len == msg_len); + test_len = test_BVLC6_Header(pTest, + pdu, len, &message_type, &length); + ct_test(pTest, test_len == 4); + ct_test(pTest, message_type == BVLC6_ADDRESS_RESOLUTION_ACK); + ct_test(pTest, length == msg_len); + test_len += bvlc6_decode_address_resolution_ack(&pdu[4], length-4, + &test_vmac_src, &test_vmac_dst); + ct_test(pTest, len == test_len); + ct_test(pTest, msg_len == test_len); + ct_test(pTest, vmac_src == test_vmac_src); + ct_test(pTest, vmac_dst == test_vmac_dst); +} + +static void test_BVLC6_Address_Resolution_Ack( + Test * pTest) +{ + uint32_t vmac_src = 0; + uint32_t vmac_dst = 0; + + test_BVLC6_Address_Resolution_Ack_Message(pTest, + vmac_src, vmac_dst); + vmac_src = 4194303; + vmac_dst = 4194302; + test_BVLC6_Address_Resolution_Ack_Message(pTest, + vmac_src, vmac_dst); +} + +static void test_BVLC6_Virtual_Address_Resolution_Message( + Test * pTest, + uint32_t vmac_src) +{ + uint8_t pdu[60] = { 0 }; + uint32_t test_vmac_src = 0; + uint8_t message_type = 0; + uint16_t length = 0; + int len = 0, test_len = 0; + const int msg_len = 7; + + len = bvlc6_encode_virtual_address_resolution(pdu, sizeof(pdu), + vmac_src); + ct_test(pTest, len == msg_len); + test_len = test_BVLC6_Header(pTest, + pdu, len, &message_type, &length); + ct_test(pTest, test_len == 4); + ct_test(pTest, message_type == BVLC6_VIRTUAL_ADDRESS_RESOLUTION); + ct_test(pTest, length == msg_len); + test_len += bvlc6_decode_virtual_address_resolution(&pdu[4], length-4, + &test_vmac_src); + ct_test(pTest, len == test_len); + ct_test(pTest, msg_len == test_len); + ct_test(pTest, vmac_src == test_vmac_src); +} + +static void test_BVLC6_Virtual_Address_Resolution( + Test * pTest) +{ + uint32_t vmac_src = 0; + + test_BVLC6_Virtual_Address_Resolution_Message(pTest, + vmac_src); + vmac_src = 0x1234; + test_BVLC6_Virtual_Address_Resolution_Message(pTest, + vmac_src); +} + +static void test_BVLC6_Virtual_Address_Resolution_Ack_Message( + Test * pTest, + uint32_t vmac_src, + uint32_t vmac_dst) +{ + uint8_t pdu[60] = { 0 }; + uint32_t test_vmac_src = 0; + uint32_t test_vmac_dst = 0; + uint8_t message_type = 0; + uint16_t length = 0; + int len = 0, test_len = 0; + const int msg_len = 10; + + len = bvlc6_encode_virtual_address_resolution_ack(pdu, sizeof(pdu), + vmac_src, vmac_dst); + ct_test(pTest, len == msg_len); + test_len = test_BVLC6_Header(pTest, + pdu, len, &message_type, &length); + ct_test(pTest, test_len == 4); + ct_test(pTest, message_type == BVLC6_VIRTUAL_ADDRESS_RESOLUTION_ACK); + ct_test(pTest, length == msg_len); + test_len += bvlc6_decode_virtual_address_resolution_ack(&pdu[4], length-4, + &test_vmac_src, &test_vmac_dst); + ct_test(pTest, len == test_len); + ct_test(pTest, msg_len == test_len); + ct_test(pTest, vmac_src == test_vmac_src); + ct_test(pTest, vmac_dst == test_vmac_dst); +} + +static void test_BVLC6_Virtual_Address_Resolution_Ack( + Test * pTest) +{ + uint32_t vmac_src = 0; + uint32_t vmac_dst = 0; + + test_BVLC6_Virtual_Address_Resolution_Ack_Message(pTest, + vmac_src, vmac_dst); + vmac_src = 4194303; + vmac_dst = 4194302; + test_BVLC6_Virtual_Address_Resolution_Ack_Message(pTest, + vmac_src, vmac_dst); +} + +static void test_BVLC6_Forwarded_NPDU_Message( + Test * pTest, + uint8_t * npdu, + uint16_t npdu_len, + uint32_t vmac_src, + BACNET_IP6_ADDRESS * bip6_address) +{ + uint8_t test_npdu[50] = { 0 }; + uint8_t pdu[75] = { 0 }; + uint32_t test_vmac_src = 0; + BACNET_IP6_ADDRESS test_bip6_address = {{0}}; + uint16_t test_npdu_len = 0; + uint8_t message_type = 0; + uint16_t length = 0; + int len = 0, msg_len = 0, test_len = 0; + uint16_t i = 0; + + len = bvlc6_encode_forwarded_npdu(pdu, sizeof(pdu), + vmac_src, bip6_address, + npdu, npdu_len); + msg_len = 1+1+2+3+BIP6_ADDRESS_MAX+npdu_len; + ct_test(pTest, len == msg_len); + test_len = test_BVLC6_Header(pTest, + pdu, len, &message_type, &length); + ct_test(pTest, test_len == 4); + ct_test(pTest, message_type == BVLC6_FORWARDED_NPDU); + ct_test(pTest, length == msg_len); + test_len += bvlc6_decode_forwarded_npdu(&pdu[4], length-4, + &test_vmac_src, &test_bip6_address, + test_npdu, sizeof(test_npdu), &test_npdu_len); + ct_test(pTest, len == test_len); + ct_test(pTest, msg_len == test_len); + ct_test(pTest, vmac_src == test_vmac_src); + test_BVLC6_Address(pTest, bip6_address, &test_bip6_address); + ct_test(pTest, npdu_len == test_npdu_len); + for (i = 0; i < npdu_len; i++) { + ct_test(pTest, npdu[i] == test_npdu[i]); + } +} + +static void test_BVLC6_Forwarded_NPDU( + Test * pTest) +{ + uint8_t npdu[50] = { 0 }; + uint32_t vmac_src = 0; + BACNET_IP6_ADDRESS bip6_address = {{0}}; + uint16_t npdu_len = 0; + uint16_t i = 0; + + test_BVLC6_Forwarded_NPDU_Message(pTest, + npdu, npdu_len, vmac_src, &bip6_address); + for (i = 0; i < sizeof(bip6_address.address); i++) { + bip6_address.address[i] = i; + } + bip6_address.port = 47808; + /* now with some NPDU data */ + for (i = 0; i < sizeof(npdu); i++) { + npdu[i] = i; + } + npdu_len = sizeof(npdu); + vmac_src = 4194303; + test_BVLC6_Forwarded_NPDU_Message(pTest, + npdu, npdu_len, vmac_src, &bip6_address); +} + +static void test_BVLC6_Register_Foreign_Device_Message( + Test * pTest, + uint32_t vmac_src, + uint16_t ttl_seconds) +{ + uint8_t pdu[60] = { 0 }; + uint32_t test_vmac_src = 0; + uint16_t test_ttl_seconds = 0; + uint8_t message_type = 0; + uint16_t length = 0; + int len = 0, test_len = 0; + const int msg_len = 9; + + len = bvlc6_encode_register_foreign_device(pdu, sizeof(pdu), + vmac_src, ttl_seconds); + ct_test(pTest, len == msg_len); + test_len = test_BVLC6_Header(pTest, + pdu, len, &message_type, &length); + ct_test(pTest, test_len == 4); + ct_test(pTest, message_type == BVLC6_REGISTER_FOREIGN_DEVICE); + ct_test(pTest, length == msg_len); + test_len += bvlc6_decode_register_foreign_device(&pdu[4], length-4, + &test_vmac_src, &test_ttl_seconds); + ct_test(pTest, len == test_len); + ct_test(pTest, msg_len == test_len); + ct_test(pTest, vmac_src == test_vmac_src); + ct_test(pTest, ttl_seconds == test_ttl_seconds); +} + +static void test_BVLC6_Register_Foreign_Device( + Test * pTest) +{ + uint32_t vmac_src = 0; + uint16_t ttl_seconds = 0; + + test_BVLC6_Register_Foreign_Device_Message(pTest, + vmac_src, ttl_seconds); + vmac_src = 4194303; + ttl_seconds = 600; + test_BVLC6_Register_Foreign_Device_Message(pTest, + vmac_src, ttl_seconds); +} + +static void test_BVLC6_Delete_Foreign_Device_Message( + Test * pTest, + uint32_t vmac_src, + BACNET_IP6_FOREIGN_DEVICE_TABLE_ENTRY *fdt_entry) +{ + uint8_t pdu[64] = { 0 }; + uint32_t test_vmac_src = 0; + BACNET_IP6_FOREIGN_DEVICE_TABLE_ENTRY test_fdt_entry = {{{0}}}; + uint8_t message_type = 0; + uint16_t length = 0; + int len = 0, test_len = 0; + const int msg_len = 0x0019; + + len = bvlc6_encode_delete_foreign_device(pdu, sizeof(pdu), + vmac_src, fdt_entry); + ct_test(pTest, len == msg_len); + test_len = test_BVLC6_Header(pTest, + pdu, len, &message_type, &length); + ct_test(pTest, test_len == 4); + ct_test(pTest, message_type == BVLC6_DELETE_FOREIGN_DEVICE); + ct_test(pTest, length == msg_len); + test_len += bvlc6_decode_delete_foreign_device(&pdu[4], length-4, + &test_vmac_src, &test_fdt_entry); + ct_test(pTest, len == test_len); + ct_test(pTest, msg_len == test_len); + ct_test(pTest, vmac_src == test_vmac_src); + test_BVLC6_Address(pTest, &fdt_entry->bip6_address, + &test_fdt_entry.bip6_address); + ct_test(pTest, fdt_entry->ttl_seconds == test_fdt_entry.ttl_seconds); + ct_test(pTest, fdt_entry->ttl_seconds_remaining == + test_fdt_entry.ttl_seconds_remaining); +} + +static void test_BVLC6_Delete_Foreign_Device( + Test * pTest) +{ + uint32_t vmac_src = 0; + BACNET_IP6_FOREIGN_DEVICE_TABLE_ENTRY fdt_entry = {{{0}}}; + unsigned int i = 0; + + /* test with zeros */ + test_BVLC6_Delete_Foreign_Device_Message(pTest, + vmac_src, &fdt_entry); + /* test with valid values */ + vmac_src = 4194303; + for (i = 0; i < sizeof(fdt_entry.bip6_address.address); i++) { + fdt_entry.bip6_address.address[i] = i; + } + fdt_entry.bip6_address.port = 47808; + fdt_entry.ttl_seconds = 600; + fdt_entry.ttl_seconds_remaining = 42; + fdt_entry.next = NULL; + test_BVLC6_Delete_Foreign_Device_Message(pTest, + vmac_src, &fdt_entry); +} + +static void test_BVLC6_Secure_BVLL_Message( + Test * pTest, + uint8_t * sbuf, + uint16_t sbuf_len) +{ + uint8_t test_sbuf[50] = { 0 }; + uint8_t pdu[60] = { 0 }; + uint16_t test_sbuf_len = 0; + uint8_t message_type = 0; + uint16_t length = 0; + int len = 0, msg_len = 0, test_len = 0; + uint16_t i = 0; + + len = bvlc6_encode_secure_bvll(pdu, sizeof(pdu), + sbuf, sbuf_len); + msg_len = 1+1+2+sbuf_len; + ct_test(pTest, len == msg_len); + test_len = test_BVLC6_Header(pTest, + pdu, len, &message_type, &length); + ct_test(pTest, test_len == 4); + ct_test(pTest, message_type == BVLC6_SECURE_BVLL); + ct_test(pTest, length == msg_len); + test_len += bvlc6_decode_secure_bvll(&pdu[4], length-4, + test_sbuf, sizeof(test_sbuf), &test_sbuf_len); + ct_test(pTest, len == test_len); + ct_test(pTest, msg_len == test_len); + ct_test(pTest, sbuf_len == test_sbuf_len); + for (i = 0; i < sbuf_len; i++) { + ct_test(pTest, sbuf[i] == test_sbuf[i]); + } +} + +static void test_BVLC6_Secure_BVLL( + Test * pTest) +{ + uint8_t sbuf[50] = { 0 }; + uint16_t sbuf_len = 0; + uint16_t i = 0; + + test_BVLC6_Secure_BVLL_Message(pTest, + sbuf, sbuf_len); + /* now with some NPDU data */ + for (i = 0; i < sizeof(sbuf); i++) { + sbuf[i] = i; + } + sbuf_len = sizeof(sbuf); + test_BVLC6_Secure_BVLL_Message(pTest, + sbuf, sbuf_len); +} + +static void test_BVLC6_Distribute_Broadcast_To_Network_Message( + Test * pTest, + uint8_t * npdu, + uint16_t npdu_len, + uint32_t vmac) +{ + uint8_t test_npdu[50] = { 0 }; + uint8_t pdu[60] = { 0 }; + uint32_t test_vmac = 0; + uint16_t test_npdu_len = 0; + uint8_t message_type = 0; + uint16_t length = 0; + int len = 0, msg_len = 0, test_len = 0; + uint16_t i = 0; + + len = bvlc6_encode_distribute_broadcast_to_network(pdu, sizeof(pdu), + vmac, npdu, npdu_len); + msg_len = 7 + npdu_len; + ct_test(pTest, len == msg_len); + test_len = test_BVLC6_Header(pTest, + pdu, len, &message_type, &length); + ct_test(pTest, test_len == 4); + ct_test(pTest, message_type == BVLC6_DISTRIBUTE_BROADCAST_TO_NETWORK); + ct_test(pTest, length == msg_len); + test_len += bvlc6_decode_distribute_broadcast_to_network( + &pdu[4], length-4, + &test_vmac, test_npdu, sizeof(test_npdu), &test_npdu_len); + ct_test(pTest, len == test_len); + ct_test(pTest, msg_len == test_len); + ct_test(pTest, vmac == test_vmac); + ct_test(pTest, npdu_len == test_npdu_len); + for (i = 0; i < npdu_len; i++) { + ct_test(pTest, npdu[i] == test_npdu[i]); + } +} + +static void test_BVLC6_Distribute_Broadcast_To_Network( + Test * pTest) +{ + uint8_t npdu[50] = { 0 }; + uint32_t vmac = 0; + uint16_t npdu_len = 0; + uint16_t i = 0; + + test_BVLC6_Distribute_Broadcast_To_Network_Message(pTest, + npdu, npdu_len, vmac); + /* now with some NPDU data */ + for (i = 0; i < sizeof(npdu); i++) { + npdu[i] = i; + } + npdu_len = sizeof(npdu); + vmac = 4194303; + test_BVLC6_Distribute_Broadcast_To_Network_Message(pTest, + npdu, npdu_len, vmac); +} + +static void test_BVLC6_Address_Copy( + Test * pTest) +{ + unsigned int i = 0; + BACNET_IP6_ADDRESS src = {{0}}; + BACNET_IP6_ADDRESS dst = {{0}}; + bool status = false; + + /* test with zeros */ + status = bvlc6_address_copy(&dst, &src); + ct_test(pTest, status); + status = bvlc6_address_different(&dst, &src); + ct_test(pTest, !status); + /* test with valid values */ + for (i = 0; i < sizeof(src.address); i++) { + src.address[i] = 1 + i; + } + src.port = 47808; + status = bvlc6_address_copy(&dst, &src); + ct_test(pTest, status); + status = bvlc6_address_different(&dst, &src); + ct_test(pTest, !status); + /* test for different port */ + dst.port = 47809; + status = bvlc6_address_different(&dst, &src); + ct_test(pTest, status); + /* test for different address */ + dst.port = src.port; + for (i = 0; i < sizeof(src.address); i++) { + dst.address[i] = 0; + status = bvlc6_address_different(&dst, &src); + ct_test(pTest, status); + dst.address[i] = 1 + i; + } +} + +static void test_BVLC6_Address_Get_Set( + Test * pTest) +{ + uint16_t i = 0; + BACNET_IP6_ADDRESS src = {{0}}; + uint16_t group = 1; + uint16_t test_group = 0; + bool status = false; + + for (i = 0; i < 16; i++) { + status = bvlc6_address_set(&src, + group, 0, 0, 0, 0, 0, 0, 0); + ct_test(pTest, status); + status = bvlc6_address_get(&src, + &test_group, NULL, NULL, NULL, NULL, NULL, NULL, NULL); + ct_test(pTest, status); + ct_test(pTest, group == test_group); + group = group<<1; + port++; + } +} + +static void test_BVLC6_VMAC_Address_Get_Set( + Test * pTest) +{ + uint16_t i = 0; + BACNET_ADDRESS addr; + uint32_t device_id = 1; + uint32_t test_device_id = 0; + bool status = false; + + for (i = 0; i < 24; i++) { + status = bvlc6_vmac_address_set(&addr, device_id); + ct_test(pTest, status); + ct_test(pTest, addr.mac_len == 3); + ct_test(pTest, addr.net == 0); + ct_test(pTest, addr.len == 0); + status = bvlc6_vmac_address_get(&addr, &test_device_id); + ct_test(pTest, status); + ct_test(pTest, device_id == test_device_id); + device_id = device_id<<1; + } +} + +void test_BVLC6( + Test * pTest) +{ + bool rc; + + /* individual tests */ + rc = ct_addTestFunction(pTest, test_BVLC6_Result); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_Original_Unicast_NPDU); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_Original_Broadcast_NPDU); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_Address_Resolution); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_Forwarded_Address_Resolution); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_Address_Resolution_Ack); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_Virtual_Address_Resolution); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_Virtual_Address_Resolution_Ack); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_Forwarded_NPDU); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_Register_Foreign_Device); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_Delete_Foreign_Device); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_Secure_BVLL); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_Distribute_Broadcast_To_Network); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_Address_Copy); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_Address_Get_Set); + assert(rc); + rc = ct_addTestFunction(pTest, test_BVLC6_VMAC_Address_Get_Set); + assert(rc); +} + +#ifdef TEST_BVLC6 +int main( + void) +{ + Test *pTest; + + pTest = ct_create("BACnet Virtual Link Control IP/v6", NULL); + test_BVLC6(pTest); + /* configure output */ + ct_setStream(pTest, stdout); + ct_run(pTest); + (void) ct_report(pTest); + ct_destroy(pTest); + + return 0; +} +#endif /* TEST_BBMD */ +#endif /* TEST */ diff --git a/bacnet-stack/src/keylist.c b/bacnet-stack/src/keylist.c index 03b5422c..38ec66c6 100644 --- a/bacnet-stack/src/keylist.c +++ b/bacnet-stack/src/keylist.c @@ -325,7 +325,7 @@ int Keylist_Index( } -/* returns the data specified by key */ +/* returns the data specified by index */ void *Keylist_Data_Index( OS_Keylist list, int index) diff --git a/bacnet-stack/src/vmac.c b/bacnet-stack/src/vmac.c new file mode 100644 index 00000000..e92715a4 --- /dev/null +++ b/bacnet-stack/src/vmac.c @@ -0,0 +1,326 @@ +/*####COPYRIGHTBEGIN#### + ------------------------------------------- + Copyright (C) 2015 Steve Karg + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation; either version 2 + of the License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to: + The Free Software Foundation, Inc. + 59 Temple Place - Suite 330 + Boston, MA 02111-1307, USA. + + As a special exception, if other files instantiate templates or + use macros or inline functions from this file, or you compile + this file and link it with other works to produce a work based + on this file, this file does not by itself cause the resulting + work to be covered by the GNU General Public License. However + the source code for this file must still be made available in + accordance with section (3) of the GNU General Public License. + + This exception does not invalidate any other reasons why a work + based on this file might be covered by the GNU General Public + License. + ------------------------------------------- +####COPYRIGHTEND####*/ +#include +#include +#include +#include +#include +#include "config.h" +#include "bacdef.h" +#include "keylist.h" +/* me! */ +#include "vmac.h" + +/** @file + Handle VMAC address binding */ + +/* This module is used to handle the virtual MAC address binding that */ +/* occurs in BACnet for ZigBee or IPv6. */ + +/* Key List for storing the object data sorted by instance number */ +static OS_Keylist VMAC_List; + +/** + * Returns the number of VMAC in the list + */ +unsigned int VMAC_Count(void) +{ + return Keylist_Count(VMAC_List); +} + +/** + * Adds a VMAC to the list + * + * @param device_id - BACnet device object instance number + * @param src - BACnet/IPv6 address + * + * @return true if the device ID and MAC are added + */ +bool VMAC_Add(uint32_t device_id, struct vmac_data *src) +{ + bool status = false; + struct vmac_data *pVMAC = NULL; + int index = 0; + unsigned int i = 0; + + pVMAC = Keylist_Data(VMAC_List, device_id); + if (!pVMAC) { + pVMAC = calloc(1, sizeof(struct vmac_data)); + if (pVMAC) { + /* copy the MAC into the data store */ + for (i = 0; i < sizeof(pVMAC->mac); i++) { + if (i < src->mac_len) { + pVMAC->mac[i] = src->mac[i]; + } else { + break; + } + } + pVMAC->mac_len = src->mac_len; + index = Keylist_Data_Add(VMAC_List, device_id, pVMAC); + if (index >= 0) { + status = true; + printf("VMAC %u added.\n", device_id); + } + } + } + + return status; +} + +/** + * Finds a VMAC in the list by seeking the Device ID, and deletes it. + * + * @param device_id - BACnet device object instance number + * + * @return pointer to the VMAC data from the list - be sure to free() it! + */ +bool VMAC_Delete(uint32_t device_id) +{ + bool status = false; + struct vmac_data *pVMAC; + + pVMAC = Keylist_Data_Delete(VMAC_List, device_id); + if (pVMAC) { + free(pVMAC); + status = true; + } + + return status; +} + +/** + * Finds a VMAC in the list by seeking the Device ID. + * + * @param device_id - BACnet device object instance number + * + * @return pointer to the VMAC data from the list + */ +struct vmac_data *VMAC_Find_By_Key(uint32_t device_id) +{ + return Keylist_Data(VMAC_List, device_id); +} + +/** Compare the VMAC address + * + * @param vmac1 - VMAC address that will be compared to vmac2 + * @param vmac2 - VMAC address that will be compared to vmac1 + * + * @return true if the addresses are different + */ +bool VMAC_Different( + struct vmac_data *vmac1, + struct vmac_data *vmac2) +{ + bool status = false; + unsigned int i = 0; + unsigned int mac_len = VMAC_MAC_MAX; + + if (vmac1 && vmac2) { + if (vmac1->mac_len != vmac2->mac_len) { + status = true; + } else { + if (vmac1->mac_len < mac_len) { + mac_len = vmac1->mac_len; + } + for (i = 0; i < mac_len; i++) { + if (vmac1->mac[i] != vmac1->mac[i]) { + status = true; + } + } + } + } + + return status; +} + +/** Compare the VMAC address + * + * @param vmac1 - VMAC address that will be compared to vmac2 + * @param vmac2 - VMAC address that will be compared to vmac1 + * + * @return true if the addresses are the same + */ +bool VMAC_Match( + struct vmac_data *vmac1, + struct vmac_data *vmac2) +{ + bool status = false; + unsigned int i = 0; + unsigned int mac_len = VMAC_MAC_MAX; + + if (vmac1 && vmac2 && vmac1->mac_len) { + status = true; + if (vmac1->mac_len != vmac2->mac_len) { + status = false; + } else { + if (vmac1->mac_len < mac_len) { + mac_len = vmac1->mac_len; + } + for (i = 0; i < mac_len; i++) { + if (vmac1->mac[i] != vmac1->mac[i]) { + status = false; + } + } + } + } + + return status; +} + +/** + * Finds a VMAC in the list by seeking a matching VMAC address + * + * @param vmac - VMAC address that will be sought + * @param device_id - BACnet device object instance number + * + * @return true if the VMAC address was found + */ +bool VMAC_Find_By_Data(struct vmac_data *vmac, uint32_t *device_id) +{ + bool status = false; + struct vmac_data *list_vmac; + int count = 0; + int index = 0; + + count = Keylist_Count(VMAC_List); + while (count) { + index = count - 1; + list_vmac = Keylist_Data_Index(VMAC_List, index); + if (list_vmac) { + if (VMAC_Match(vmac, list_vmac)) { + if (device_id) { + *device_id = Keylist_Key(VMAC_List, index); + } + status = true; + break; + } + } + count--; + } + + return status; +} + +/** + * Cleans up the memory used by the VMAC list data + */ +void VMAC_Cleanup(void) +{ + struct vmac_data *pVMAC; + + if (VMAC_List) { + do { + pVMAC = Keylist_Data_Pop(VMAC_List); + if (pVMAC) { + free(pVMAC); + } + } while (pVMAC); + Keylist_Delete(VMAC_List); + VMAC_List = NULL; + } +} + +/** + * Initializes the VMAC list data + */ +void VMAC_Init(void) +{ + VMAC_List = Keylist_Create(); + if (VMAC_List) { + atexit(VMAC_Cleanup); + printf("VMAC List initialized.\n"); + } +} + +#ifdef TEST +#include +#include +#include "ctest.h" + +void testVMAC( + Test * pTest) +{ + uint32_t device_id = 123; + uint32_t test_device_id = 0; + struct vmac_data test_vmac_data; + struct vmac_data *pVMAC; + unsigned int i = 0; + bool status = false; + + VMAC_Init(); + for (i = 0; i < VMAC_MAC_MAX; i++) { + test_vmac_data.mac[i] = 1 + i; + } + test_vmac_data.mac_len = VMAC_MAC_MAX; + status = VMAC_Add(device_id, &test_vmac_data); + ct_test(pTest, status); + pVMAC = VMAC_Find_By_Key(0); + ct_test(pTest, pVMAC == NULL); + pVMAC = VMAC_Find_By_Key(device_id); + ct_test(pTest, pVMAC); + status = VMAC_Different(pVMAC, &test_vmac_data); + ct_test(pTest, !status); + status = VMAC_Match(pVMAC, &test_vmac_data); + ct_test(pTest, status); + status = VMAC_Find_By_Data(&test_vmac_data, &test_device_id); + ct_test(pTest, status); + ct_test(pTest, test_device_id == device_id); + status = VMAC_Delete(device_id); + ct_test(pTest, status); + pVMAC = VMAC_Find_By_Key(device_id); + ct_test(pTest, pVMAC == NULL); + VMAC_Cleanup(); +} + +#ifdef TEST_VMAC +int main( + void) +{ + Test *pTest; + bool rc; + + pTest = ct_create("BACnet VMAC", NULL); + /* individual tests */ + rc = ct_addTestFunction(pTest, testVMAC); + assert(rc); + + ct_setStream(pTest, stdout); + ct_run(pTest); + (void) ct_report(pTest); + ct_destroy(pTest); + + return 0; +} +#endif +#endif diff --git a/bacnet-stack/test.mak b/bacnet-stack/test.mak index 55aa36d5..3df7f4a4 100644 --- a/bacnet-stack/test.mak +++ b/bacnet-stack/test.mak @@ -9,10 +9,10 @@ LOGFILE = test.log -all: abort address arf awf bacapp bacdcode bacerror bacint bacstr \ +all: abort address arf awf bvlc6 bacapp bacdcode bacerror bacint bacstr \ cov crc datetime dcc event filename fifo getevent iam ihave \ indtext keylist key memcopy npdu proplist ptransfer \ - rd reject ringbuf rp rpm sbuf timesync \ + rd reject ringbuf rp rpm sbuf timesync vmac \ whohas whois wp objects lighting clean: logfile @@ -66,6 +66,11 @@ bacstr: logfile test/bacstr.mak ( ./test/bacstr >> ${LOGFILE} ) $(MAKE) -s -C test -f bacstr.mak clean +bvlc6: logfile test/bvlc6.mak + $(MAKE) -s -C test -f bvlc6.mak clean all + ( ./test/bvlc6 >> ${LOGFILE} ) + $(MAKE) -s -C test -f bvlc6.mak clean + cov: logfile test/cov.mak $(MAKE) -s -C test -f cov.mak clean all ( ./test/cov >> ${LOGFILE} ) @@ -191,6 +196,11 @@ timesync: logfile test/timesync.mak ( ./test/timesync >> ${LOGFILE} ) $(MAKE) -s -C test -f timesync.mak clean +vmac: logfile test/vmac.mak + $(MAKE) -s -C test -f vmac.mak clean all + ( ./test/vmac >> ${LOGFILE} ) + $(MAKE) -s -C test -f vmac.mak clean + whohas: logfile test/whohas.mak $(MAKE) -s -C test -f whohas.mak clean all ( ./test/whohas >> ${LOGFILE} ) diff --git a/bacnet-stack/test/bbmd6.mak b/bacnet-stack/test/bbmd6.mak new file mode 100644 index 00000000..b2e93e39 --- /dev/null +++ b/bacnet-stack/test/bbmd6.mak @@ -0,0 +1,42 @@ +#Makefile to build test case +CC = gcc +SRC_DIR = ../src +SRC_INC = ../include +DEMO_DIR = ../demo/handler +DEMO_INC = ../demo/object +INCLUDES = -I. -I$(SRC_INC) -I$(DEMO_INC) +DEFINES = -DBIG_ENDIAN=0 -DTEST -DTEST_BBMD6 + +CFLAGS = -Wall -Wmissing-prototypes $(INCLUDES) $(DEFINES) -g + +SRCS = $(SRC_DIR)/bacdcode.c \ + $(SRC_DIR)/bacint.c \ + $(SRC_DIR)/bacstr.c \ + $(SRC_DIR)/bacreal.c \ + $(SRC_DIR)/bvlc6.c \ + $(SRC_DIR)/debug.c \ + $(SRC_DIR)/keylist.c \ + $(SRC_DIR)/vmac.c \ + $(DEMO_DIR)/h_bbmd6.c \ + ctest.c + +TARGET = bbmd6 + +all: ${TARGET} + +OBJS = ${SRCS:.c=.o} + +${TARGET}: ${OBJS} + ${CC} -o $@ ${OBJS} + +.c.o: + ${CC} -c ${CFLAGS} $*.c -o $@ + +depend: + rm -f .depend + ${CC} -MM ${CFLAGS} *.c >> .depend + +clean: + rm -rf ${TARGET} $(OBJS) + +include: .depend diff --git a/bacnet-stack/test/vmac.mak b/bacnet-stack/test/vmac.mak new file mode 100644 index 00000000..f02e2b51 --- /dev/null +++ b/bacnet-stack/test/vmac.mak @@ -0,0 +1,32 @@ +#Makefile to build test case +CC = gcc +SRC_DIR = ../src +INCLUDES = -I../include -I. +DEFINES = -DBIG_ENDIAN=0 -DTEST -DTEST_VMAC + +CFLAGS = -Wall -Wmissing-prototypes $(INCLUDES) $(DEFINES) -g + +SRCS = $(SRC_DIR)/keylist.c \ + $(SRC_DIR)/vmac.c \ + ctest.c + +TARGET = vmac + +all: ${TARGET} + +OBJS = ${SRCS:.c=.o} + +${TARGET}: ${OBJS} + ${CC} -o $@ ${OBJS} + +.c.o: + ${CC} -c ${CFLAGS} $*.c -o $@ + +depend: + rm -f .depend + ${CC} -MM ${CFLAGS} *.c >> .depend + +clean: + rm -rf ${TARGET} $(OBJS) + +include: .depend