From 0326112c68686336d7e45898933b94c5252bdf50 Mon Sep 17 00:00:00 2001 From: skarg Date: Sun, 17 Aug 2008 00:35:18 +0000 Subject: [PATCH] Created MS/TP capture tool that captures from USB->RS485 converter and sends data to SNAP Protocol for Wireshark capture. --- bacnet-stack/demo/server/main.c | 2 +- bacnet-stack/ports/linux/ethernet.c | 31 ++ bacnet-stack/ports/linux/mstpsnap.c | 321 +++++++++++ bacnet-stack/ports/linux/mstpsnap.mak | 63 +++ bacnet-stack/src/dlmstp.c | 731 ++++++++++++++++++++++++++ 5 files changed, 1147 insertions(+), 1 deletion(-) create mode 100644 bacnet-stack/ports/linux/mstpsnap.c create mode 100644 bacnet-stack/ports/linux/mstpsnap.mak create mode 100644 bacnet-stack/src/dlmstp.c diff --git a/bacnet-stack/demo/server/main.c b/bacnet-stack/demo/server/main.c index 395bc377..8472df09 100644 --- a/bacnet-stack/demo/server/main.c +++ b/bacnet-stack/demo/server/main.c @@ -188,7 +188,7 @@ int main(int argc, char *argv[]) { BACNET_ADDRESS src = { 0}; /* address where message came from */ uint16_t pdu_len = 0; - unsigned timeout = 100; /* milliseconds */ + unsigned timeout = 1000; /* milliseconds */ time_t last_seconds = 0; time_t current_seconds = 0; uint32_t elapsed_seconds = 0; diff --git a/bacnet-stack/ports/linux/ethernet.c b/bacnet-stack/ports/linux/ethernet.c index d38a4f7d..592bdf34 100644 --- a/bacnet-stack/ports/linux/ethernet.c +++ b/bacnet-stack/ports/linux/ethernet.c @@ -93,6 +93,7 @@ static int ethernet_bind( char *interface_name) { int sock_fd = -1; /* return value */ + int sockopt = 0; int uid = 0; fprintf(stderr, "ethernet: opening \"%s\"\n", interface_name); @@ -108,6 +109,8 @@ static int ethernet_bind( /* modules.conf (or in modutils/alias on Debian with update-modules) */ /* alias net-pf-17 af_packet */ /* Then follow it by: # modprobe af_packet */ + /* Note: PF_INET/SOCK_PACKET has been replaced with + PF_PACKET/(SOCK_PACKET, SOCK_DGRAM, SOCK_RAW).*/ /* Attempt to open the socket for 802.2 ethernet frames */ if ((sock_fd = socket(PF_INET, SOCK_PACKET, htons(ETH_P_802_2))) < 0) { @@ -122,6 +125,15 @@ static int ethernet_bind( "# modprobe af_packet\n"); exit(-1); } +#if 0 + /* It is very advisable to do a IP_HDRINCL call, to make sure + that the kernel knows the header is included in the data, + and doesn't insert its own header into the packet before our data */ + if (setsockopt (sock_fd, IPPROTO_IP, IP_HDRINCL, &sockopt, + sizeof(sockopt)) < 0) { + printf ("Warning: Cannot set HDRINCL!\n"); + } +#endif /* Bind the socket to an address */ eth_addr->sa_family = PF_INET; /* Clear the memory before copying */ @@ -187,6 +199,25 @@ bool ethernet_init( return ethernet_valid(); } +int ethernet_send( + uint8_t * mtu, + int mtu_len) +{ + int bytes = 0; + + /* Send the packet */ + bytes = + sendto(eth802_sockfd, &mtu, mtu_len, 0, (struct sockaddr *) ð_addr, + sizeof(struct sockaddr)); + /* did it get sent? */ + if (bytes < 0) + fprintf(stderr, "ethernet: Error sending packet: %s\n", + strerror(errno)); + + return bytes; + +} + /* function to send a packet out the 802.2 socket */ /* returns number of bytes sent on success, negative on failure */ int ethernet_send_pdu( diff --git a/bacnet-stack/ports/linux/mstpsnap.c b/bacnet-stack/ports/linux/mstpsnap.c new file mode 100644 index 00000000..18ad6f60 --- /dev/null +++ b/bacnet-stack/ports/linux/mstpsnap.c @@ -0,0 +1,321 @@ +/*####COPYRIGHTBEGIN#### + ------------------------------------------- + Copyright (C) 2008 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 +/* OS specific include*/ +#include "net.h" +/* local includes */ +#include "bytes.h" +#include "rs485.h" +#include "crc.h" +#include "mstp.h" +#include "mstptext.h" +#include "bacint.h" + +/* local port data - shared with RS-485 */ +static volatile struct mstp_port_struct_t MSTP_Port; +/* buffers needed by mstp port struct */ +static uint8_t RxBuffer[MAX_MPDU]; +static uint8_t TxBuffer[MAX_MPDU]; +static uint16_t SilenceTime; +#define INCREMENT_AND_LIMIT_UINT16(x) {if (x < 0xFFFF) x++;} +static uint16_t Timer_Silence( + void) +{ + return SilenceTime; +} +static void Timer_Silence_Reset( + void) +{ + SilenceTime = 0; +} + +static void dlmstp_millisecond_timer( + void) +{ + INCREMENT_AND_LIMIT_UINT16(SilenceTime); +} + +void *milliseconds_task( + void *pArg) +{ + struct timespec timeOut, remains; + + timeOut.tv_sec = 0; + timeOut.tv_nsec = 10000000; /* 1 milliseconds */ + + for (;;) { + nanosleep(&timeOut, &remains); + dlmstp_millisecond_timer(); + } + + return NULL; +} + +/* functions used by the MS/TP state machine to put or get data */ +uint16_t MSTP_Put_Receive( + volatile struct mstp_port_struct_t * mstp_port) +{ + (void) mstp_port; + + return 0; +} + +/* for the MS/TP state machine to use for getting data to send */ +/* Return: amount of PDU data */ +uint16_t MSTP_Get_Send( + volatile struct mstp_port_struct_t * mstp_port, + unsigned timeout) +{ /* milliseconds to wait for a packet */ + (void) mstp_port; + (void) timeout; + return 0; +} + +uint16_t MSTP_Get_Reply( + volatile struct mstp_port_struct_t * mstp_port, + unsigned timeout) +{ /* milliseconds to wait for a packet */ + (void) mstp_port; + (void) timeout; + return 0; +} + +static int network_init(const char *name, int protocol) +{ + /* check to see if we are being run as root */ + if (getuid() != 0) { + fprintf(stderr, + "Requires root priveleges.\n"); + return -1; + } + + int sockfd = socket(PF_PACKET, SOCK_RAW, htons(protocol)); + + if (sockfd == -1) { + perror("Unable to create socket"); + return sockfd; + } + + struct ifreq ifr; + + memset(&ifr, 0, sizeof(ifr)); + strncpy(ifr.ifr_name, name, strlen(name)); + + if (ioctl(sockfd, SIOCGIFINDEX, &ifr) == -1) { + perror("Unable to get interface index"); + return -1; + } + + struct sockaddr_ll sll; + + memset(&sll, 0, sizeof(sll)); + sll.sll_family = AF_PACKET; + sll.sll_ifindex = ifr.ifr_ifindex; + sll.sll_protocol = htons(protocol); + + if (bind(sockfd, (struct sockaddr *) &sll, sizeof(sll)) == -1) { + perror("Unable to bind socket"); + return -1; + } + + return sockfd; +} + +static void snap_received_packet( + volatile struct mstp_port_struct_t *mstp_port, + int sockfd) +{ + uint16_t mtu_len = 0; /* number of octets of packet saved in file */ + unsigned i = 0; /* counter */ + static uint8_t mtu[1500] = {0}; + + mtu[0] = 0; + mtu[1] = 0; + mtu[2] = 0; + mtu[3] = 0; + mtu[4] = 0; + mtu[5] = mstp_port->DestinationAddress; + mtu[6] = 0; + mtu[7] = 0; + mtu[8] = 0; + mtu[9] = 0; + mtu[10] = 0; + mtu[11]= mstp_port->SourceAddress; + /* length - 12, 13 */ + mtu[14] = 0xaa; /* DSAP for SNAP */ + mtu[15] = 0xaa; /* SSAP for SNAP */ + mtu[16] = 0x03; /* Control Field for SNAP */ + mtu[17] = 0x00; /* Organization Code: Cimetrics */ + mtu[18] = 0x10; /* Organization Code: Cimetrics */ + mtu[19] = 0x90; /* Organization Code: Cimetrics */ + mtu[20] = 0x00; /* Protocol ID */ + mtu[21] = 0x01; /* Protocol ID */ + mtu[22] = 0x00; /* delta time */ + mtu[23] = 0x00; /* delta time */ + mtu[24] = 0x80; /* unknown byte */ + mtu[25] = mstp_port->FrameType; + mtu[26] = mstp_port->DestinationAddress; + mtu[27] = mstp_port->SourceAddress; + mtu[28] = HI_BYTE(mstp_port->DataLength); + mtu[29] = LO_BYTE(mstp_port->DataLength); + mtu[30] = mstp_port->HeaderCRCActual; + mtu_len = 31; + if (mstp_port->DataLength) { + for (i = 0; i < mstp_port->DataLength; i++) { + mtu[31+i] = mstp_port->InputBuffer[i]; + } + mtu[31+mstp_port->DataLength] = mstp_port->DataCRCActualMSB; + mtu[31+mstp_port->DataLength+1] = mstp_port->DataCRCActualLSB; + mtu_len += (mstp_port->DataLength+2); + } + /* Ethernet length is data only - not address or length bytes */ + encode_unsigned16(&mtu[12], mtu_len-14); + write(sockfd,&mtu[0], mtu_len); +} + + +static void cleanup( + void) +{ +} + +#if (!defined(_WIN32)) +static void sig_int( + int signo) +{ + (void) signo; + + cleanup(); + exit(0); +} + +void signal_init( + void) +{ + signal(SIGINT, sig_int); + signal(SIGHUP, sig_int); + signal(SIGTERM, sig_int); +} +#endif + +/* simple test to packetize the data and print it */ +int main( + int argc, + char *argv[]) +{ + volatile struct mstp_port_struct_t *mstp_port; + long my_baud = 38400; + uint32_t packet_count = 0; +#if defined(_WIN32) + unsigned long hThread = 0; + uint32_t arg_value = 0; +#else + int rc = 0; + pthread_t hThread; +#endif + int sockfd = -1; + char *my_interface = "eth0"; + + /* mimic our pointer in the state machine */ + mstp_port = &MSTP_Port; + /* initialize our interface */ + if (argc > 1) { + RS485_Set_Interface(argv[1]); + } + if (argc > 2) { + my_baud = strtol(argv[2], NULL, 0); + } + if (argc > 3) { + my_interface = argv[3]; + } + sockfd = network_init(my_interface, ETH_P_ALL); + if (sockfd == -1) { + return 1; + } + RS485_Set_Baud_Rate(my_baud); + RS485_Initialize(); + MSTP_Port.InputBuffer = &RxBuffer[0]; + MSTP_Port.InputBufferSize = sizeof(RxBuffer); + MSTP_Port.OutputBuffer = &TxBuffer[0]; + MSTP_Port.OutputBufferSize = sizeof(TxBuffer); + MSTP_Port.This_Station = 127; + MSTP_Port.Nmax_info_frames = 1; + MSTP_Port.Nmax_master = 127; + MSTP_Port.SilenceTimer = Timer_Silence; + MSTP_Port.SilenceTimerReset = Timer_Silence_Reset; + MSTP_Init(mstp_port); + mstp_port->Lurking = true; + fprintf(stdout, "mstpcap: Using %s for capture at %ld bps.\n", + RS485_Interface(), (long) RS485_Get_Baud_Rate()); +#if defined(_WIN32) + hThread = _beginthread(milliseconds_task, 4096, &arg_value); + if (hThread == 0) { + fprintf(stderr, "Failed to start timer task\n"); + } + (void) SetThreadPriority(GetCurrentThread(), + THREAD_PRIORITY_TIME_CRITICAL); +#else + /* start our MilliSec task */ + rc = pthread_create(&hThread, NULL, milliseconds_task, NULL); + signal_init(); +#endif + atexit(cleanup); + /* run forever */ + for (;;) { + RS485_Check_UART_Data(mstp_port); + MSTP_Receive_Frame_FSM(mstp_port); + /* process the data portion of the frame */ + if (mstp_port->ReceivedValidFrame) { + mstp_port->ReceivedValidFrame = false; + snap_received_packet(mstp_port, sockfd); + packet_count++; + } else if (mstp_port->ReceivedInvalidFrame) { + mstp_port->ReceivedInvalidFrame = false; + fprintf(stderr, "ReceivedInvalidFrame\n"); + snap_received_packet(mstp_port, sockfd); + packet_count++; + } + if (!(packet_count % 100)) { + fprintf(stdout, "\r%hu packets", packet_count); + } + } + + return 0; +} diff --git a/bacnet-stack/ports/linux/mstpsnap.mak b/bacnet-stack/ports/linux/mstpsnap.mak new file mode 100644 index 00000000..bcf1912d --- /dev/null +++ b/bacnet-stack/ports/linux/mstpsnap.mak @@ -0,0 +1,63 @@ +#Makefile to build BACnet Application for the Linux Port + +# Compiler to use +CC = gcc +# Executable file name +TARGET = mstpsnap + +# Configure the BACnet Datalink Layer +BACDL_DEFINE = -DBACDL_MSTP +BACNET_DEFINES = -DPRINT_ENABLED=1 -DBACAPP_ALL -DBACFILE +DEFINES = $(BACNET_DEFINES) $(BACDL_DEFINE) + +# Directories +BACNET_PORT = linux +BACNET_PORT_DIR = . +BACNET_SOURCE_DIR = ../../src +BACNET_INCLUDE = ../../include + +# Compiler Setup +INCLUDES = -I$(BACNET_INCLUDE) -I$(BACNET_PORT_DIR) +ifeq (${BACNET_PORT},linux) +PFLAGS = -pthread +TARGET_BIN = ${TARGET} +LIBRARIES=-lc,-lgcc,-lrt,-lm +endif +ifeq (${BACNET_PORT},win32) +TARGET_BIN = ${TARGET}.exe +LIBRARIES=-lws2_32,-lgcc,-lm,-liphlpapi +endif +#DEBUGGING = -g +#OPTIMIZATION = -O0 +OPTIMIZATION = -Os +CFLAGS = -Wall $(DEBUGGING) $(OPTIMIZATION) $(INCLUDES) $(DEFINES) -fdata-sections -ffunction-sections +LFLAGS = -Wl,-Map=$(TARGET).map,$(LIBRARIES),--gc-sections + +SRCS = mstpsnap.c \ + ${BACNET_PORT_DIR}/rs485.c \ + ${BACNET_SOURCE_DIR}/bacint.c \ + ${BACNET_SOURCE_DIR}/mstp.c \ + ${BACNET_SOURCE_DIR}/mstptext.c \ + ${BACNET_SOURCE_DIR}/debug.c \ + ${BACNET_SOURCE_DIR}/indtext.c \ + ${BACNET_SOURCE_DIR}/crc.c + +OBJS = ${SRCS:.c=.o} + +all: ${TARGET_BIN} + size ${TARGET_BIN} + +${TARGET_BIN}: ${OBJS} + ${CC} ${PFLAGS} ${OBJS} ${LFLAGS} -o $@ + +.c.o: + ${CC} -c ${CFLAGS} $*.c -o $@ + +depend: + rm -f .depend + ${CC} -MM ${CFLAGS} *.c >> .depend + +clean: + rm -f core ${TARGET_BIN} ${OBJS} + +include: .depend diff --git a/bacnet-stack/src/dlmstp.c b/bacnet-stack/src/dlmstp.c new file mode 100644 index 00000000..0d7458d9 --- /dev/null +++ b/bacnet-stack/src/dlmstp.c @@ -0,0 +1,731 @@ +/************************************************************************** +* +* Copyright (C) 2006 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 "bacdef.h" +#include "bacaddr.h" +#include "mstp.h" +#include "dlmstp.h" +#include "rs485.h" +#include "npdu.h" +#include "bits.h" + +#define WIN32_LEAN_AND_MEAN +#define STRICT 1 +#include +#include + +/* Number of MS/TP Packets Rx/Tx */ +uint16_t MSTP_Packets = 0; + +/* packet queues */ +static DLMSTP_PACKET Receive_Packet; +static HANDLE Receive_Packet_Flag; +/* mechanism to wait for a frame in state machine */ +HANDLE Received_Frame_Flag; +static DLMSTP_PACKET Transmit_Packet; +/* local MS/TP port data - shared with RS-485 */ +volatile struct mstp_port_struct_t MSTP_Port; +/* buffers needed by mstp port struct */ +static uint8_t TxBuffer[MAX_MPDU]; +static uint8_t RxBuffer[MAX_MPDU]; +/* Timer that indicates line silence - and functions */ +static uint16_t SilenceTime; +#define INCREMENT_AND_LIMIT_UINT16(x) {if (x < 0xFFFF) x++;} +static uint16_t Timer_Silence( + void) +{ + return SilenceTime; +} +static void Timer_Silence_Reset( + void) +{ + SilenceTime = 0; +} + +void dlmstp_millisecond_timer( + void) +{ + INCREMENT_AND_LIMIT_UINT16(SilenceTime); +} + +void dlmstp_reinit( + void) +{ + /*RS485_Reinit(); */ + dlmstp_set_mac_address(DEFAULT_MAC_ADDRESS); + dlmstp_set_max_info_frames(DEFAULT_MAX_INFO_FRAMES); + dlmstp_set_max_master(DEFAULT_MAX_MASTER); +} + +void dlmstp_cleanup( + void) +{ + /* nothing to do for static buffers */ + if (Received_Frame_Flag) { + CloseHandle(Received_Frame_Flag); + } + if (Receive_Packet_Flag) { + CloseHandle(Receive_Packet_Flag); + } +} + +/* returns number of bytes sent on success, zero on failure */ +int dlmstp_send_pdu( + BACNET_ADDRESS * dest, /* destination address */ + BACNET_NPDU_DATA * npdu_data, /* network information */ + uint8_t * pdu, /* any data to be sent - may be null */ + unsigned pdu_len) +{ /* number of bytes of data */ + int bytes_sent = 0; + unsigned i = 0; + + if (!Transmit_Packet.ready) { + if (npdu_data->data_expecting_reply) { + Transmit_Packet.frame_type = + FRAME_TYPE_BACNET_DATA_EXPECTING_REPLY; + } else { + Transmit_Packet.frame_type = + FRAME_TYPE_BACNET_DATA_NOT_EXPECTING_REPLY; + } + Transmit_Packet.pdu_len = pdu_len; + for (i = 0; i < pdu_len; i++) { + Transmit_Packet.pdu[i] = pdu[i]; + } + bacnet_address_copy(&Transmit_Packet.address, dest); + bytes_sent = pdu_len + MAX_HEADER; + Transmit_Packet.ready = true; + } + + return bytes_sent; +} + +uint16_t dlmstp_receive( + BACNET_ADDRESS * src, /* source address */ + uint8_t * pdu, /* PDU data */ + uint16_t max_pdu, /* amount of space available in the PDU */ + unsigned timeout) +{ /* milliseconds to wait for a packet */ + uint16_t pdu_len = 0; + DWORD wait_status = 0; + + (void) max_pdu; + /* see if there is a packet available, and a place + to put the reply (if necessary) and process it */ + wait_status = WaitForSingleObject(Receive_Packet_Flag, timeout); + if (wait_status == WAIT_OBJECT_0) { + if (Receive_Packet.ready) { + if (Receive_Packet.pdu_len) { + MSTP_Packets++; + if (src) { + memmove(src, &Receive_Packet.address, + sizeof(Receive_Packet.address)); + } + if (pdu) { + memmove(pdu, &Receive_Packet.pdu, + sizeof(Receive_Packet.pdu)); + } + pdu_len = Receive_Packet.pdu_len; + } + Receive_Packet.ready = false; + } + } + + return pdu_len; +} + +static void dlmstp_receive_fsm_task( + void *pArg) +{ + bool received_frame; + + (void) pArg; + (void) SetThreadPriority(GetCurrentThread(), + THREAD_PRIORITY_TIME_CRITICAL); + while (TRUE) { + /* only do receive state machine while we don't have a frame */ + if ((MSTP_Port.ReceivedValidFrame == false) && + (MSTP_Port.ReceivedInvalidFrame == false)) { + do { + RS485_Check_UART_Data(&MSTP_Port); + MSTP_Receive_Frame_FSM(&MSTP_Port); + received_frame = MSTP_Port.ReceivedValidFrame || + MSTP_Port.ReceivedInvalidFrame; + if (received_frame) { + ReleaseSemaphore(Received_Frame_Flag, 1, NULL); + break; + } + } while (MSTP_Port.DataAvailable); + } + } +} + +static void dlmstp_master_fsm_task( + void *pArg) +{ + DWORD dwMilliseconds = 0; + + (void) pArg; + (void) SetThreadPriority(GetCurrentThread(), + THREAD_PRIORITY_TIME_CRITICAL); + while (TRUE) { + switch (MSTP_Port.master_state) { + case MSTP_MASTER_STATE_IDLE: + dwMilliseconds = Tno_token; + break; + case MSTP_MASTER_STATE_WAIT_FOR_REPLY: + dwMilliseconds = Treply_timeout; + break; + case MSTP_MASTER_STATE_POLL_FOR_MASTER: + dwMilliseconds = Tusage_timeout; + break; + default: + dwMilliseconds = 0; + break; + } + if (dwMilliseconds) + WaitForSingleObject(Received_Frame_Flag, dwMilliseconds); + MSTP_Master_Node_FSM(&MSTP_Port); + } +} + +static void dlmstp_millisecond_task( + void *pArg) +{ + (void) pArg; + (void) SetThreadPriority(GetCurrentThread(), + THREAD_PRIORITY_TIME_CRITICAL); + for (;;) { + dlmstp_millisecond_timer(); + Sleep(1); + } +} + +void dlmstp_fill_bacnet_address( + BACNET_ADDRESS * src, + uint8_t mstp_address) +{ + int i = 0; + + if (mstp_address == MSTP_BROADCAST_ADDRESS) { + /* mac_len = 0 if broadcast address */ + src->mac_len = 0; + src->mac[0] = 0; + } else { + src->mac_len = 1; + src->mac[0] = mstp_address; + } + /* fill with 0's starting with index 1; index 0 filled above */ + for (i = 1; i < MAX_MAC_LEN; i++) { + src->mac[i] = 0; + } + src->net = 0; + src->len = 0; + for (i = 0; i < MAX_MAC_LEN; i++) { + src->adr[i] = 0; + } +} + +/* for the MS/TP state machine to use for putting received data */ +uint16_t MSTP_Put_Receive( + volatile struct mstp_port_struct_t *mstp_port) +{ + uint16_t pdu_len = 0; + BOOL rc; + + if (!Receive_Packet.ready) { + /* bounds check - maybe this should send an abort? */ + pdu_len = mstp_port->DataLength; + if (pdu_len > sizeof(Receive_Packet.pdu)) + pdu_len = sizeof(Receive_Packet.pdu); + memmove((void *) &Receive_Packet.pdu[0], + (void *) &mstp_port->InputBuffer[0], pdu_len); + dlmstp_fill_bacnet_address(&Receive_Packet.address, + mstp_port->SourceAddress); + Receive_Packet.pdu_len = mstp_port->DataLength; + Receive_Packet.ready = true; + rc = ReleaseSemaphore(Receive_Packet_Flag, 1, NULL); + } + + return pdu_len; +} + +/* for the MS/TP state machine to use for getting data to send */ +/* Return: amount of PDU data */ +uint16_t MSTP_Get_Send( + volatile struct mstp_port_struct_t * mstp_port, + unsigned timeout) +{ /* milliseconds to wait for a packet */ + uint16_t pdu_len = 0; + uint8_t destination = 0; /* destination address */ + + (void) timeout; + if (!Transmit_Packet.ready) { + return 0; + } + /* load destination MAC address */ + if (Transmit_Packet.address.mac_len == 1) { + destination = Transmit_Packet.address.mac[0]; + } else { + return 0; + } + if ((MAX_HEADER + Transmit_Packet.pdu_len) > MAX_MPDU) { + return 0; + } + /* convert the PDU into the MSTP Frame */ + pdu_len = MSTP_Create_Frame(&mstp_port->OutputBuffer[0], /* <-- loading this */ + mstp_port->OutputBufferSize, Transmit_Packet.frame_type, destination, + mstp_port->This_Station, &Transmit_Packet.pdu[0], + Transmit_Packet.pdu_len); + Transmit_Packet.ready = false; + + return pdu_len; +} + +bool dlmstp_compare_data_expecting_reply( + uint8_t * request_pdu, + uint16_t request_pdu_len, + uint8_t src_address, + uint8_t * reply_pdu, + uint16_t reply_pdu_len, + BACNET_ADDRESS * dest_address) +{ + uint16_t offset; + /* One way to check the message is to compare NPDU + src, dest, along with the APDU type, invoke id. + Seems a bit overkill */ + struct DER_compare_t { + BACNET_NPDU_DATA npdu_data; + BACNET_ADDRESS address; + uint8_t pdu_type; + uint8_t invoke_id; + uint8_t service_choice; + }; + struct DER_compare_t request; + struct DER_compare_t reply; + + /* unused parameters */ + request_pdu_len = request_pdu_len; + reply_pdu_len = reply_pdu_len; + /* decode the request data */ + request.address.mac[0] = src_address; + request.address.mac_len = 1; + offset = + npdu_decode(&request_pdu[0], NULL, &request.address, + &request.npdu_data); + if (request.npdu_data.network_layer_message) { + return false; + } + request.pdu_type = request_pdu[offset] & 0xF0; + if (request.pdu_type != PDU_TYPE_CONFIRMED_SERVICE_REQUEST) { + return false; + } + request.invoke_id = request_pdu[offset + 2]; + /* segmented message? */ + if (request_pdu[offset] & BIT3) + request.service_choice = request_pdu[offset + 5]; + else + request.service_choice = request_pdu[offset + 3]; + /* decode the reply data */ + bacnet_address_copy(&reply.address, dest_address); + offset = + npdu_decode(&reply_pdu[0], &reply.address, NULL, &reply.npdu_data); + if (reply.npdu_data.network_layer_message) { + return false; + } + /* reply could be a lot of things: + confirmed, simple ack, abort, reject, error */ + reply.pdu_type = reply_pdu[offset] & 0xF0; + switch (reply.pdu_type) { + case PDU_TYPE_CONFIRMED_SERVICE_REQUEST: + reply.invoke_id = reply_pdu[offset + 2]; + /* segmented message? */ + if (reply_pdu[offset] & BIT3) + reply.service_choice = reply_pdu[offset + 5]; + else + reply.service_choice = reply_pdu[offset + 3]; + break; + case PDU_TYPE_SIMPLE_ACK: + reply.invoke_id = reply_pdu[offset + 1]; + reply.service_choice = reply_pdu[offset + 2]; + break; + case PDU_TYPE_COMPLEX_ACK: + reply.invoke_id = reply_pdu[offset + 1]; + /* segmented message? */ + if (reply_pdu[offset] & BIT3) + reply.service_choice = reply_pdu[offset + 4]; + else + reply.service_choice = reply_pdu[offset + 2]; + break; + case PDU_TYPE_ERROR: + reply.invoke_id = reply_pdu[offset + 1]; + reply.service_choice = reply_pdu[offset + 2]; + break; + case PDU_TYPE_REJECT: + case PDU_TYPE_ABORT: + reply.invoke_id = reply_pdu[offset + 1]; + break; + default: + return false; + } + /* these don't have service choice included */ + if ((reply.pdu_type == PDU_TYPE_REJECT) || + (reply.pdu_type == PDU_TYPE_ABORT)) { + if (request.invoke_id != reply.invoke_id) { + return false; + } + } else { + if (request.invoke_id != reply.invoke_id) { + return false; + } + if (request.service_choice != reply.service_choice) { + return false; + } + } + if (request.npdu_data.protocol_version != reply.npdu_data.protocol_version) { + return false; + } + if (request.npdu_data.priority != reply.npdu_data.priority) { + return false; + } + if (!bacnet_address_same(&request.address, &reply.address)) { + return false; + } + + return true; +} + +/* Get the reply to a DATA_EXPECTING_REPLY frame, or nothing */ +uint16_t MSTP_Get_Reply( + volatile struct mstp_port_struct_t * mstp_port, + unsigned timeout) +{ /* milliseconds to wait for a packet */ + uint16_t pdu_len = 0; /* return value */ + uint8_t destination = 0; /* destination address */ + bool matched = false; + + (void) timeout; + if (!Transmit_Packet.ready) { + return 0; + } + /* load destination MAC address */ + if (Transmit_Packet.address.mac_len == 1) { + destination = Transmit_Packet.address.mac[0]; + } else { + return 0; + } + if ((MAX_HEADER + Transmit_Packet.pdu_len) > MAX_MPDU) { + return 0; + } + /* is this the reply to the DER? */ + matched = + dlmstp_compare_data_expecting_reply(&mstp_port->InputBuffer[0], + mstp_port->DataLength, mstp_port->SourceAddress, + &Transmit_Packet.pdu[0], Transmit_Packet.pdu_len, + &Transmit_Packet.address); + if (!matched) + return 0; + /* convert the PDU into the MSTP Frame */ + pdu_len = MSTP_Create_Frame(&mstp_port->OutputBuffer[0], /* <-- loading this */ + mstp_port->OutputBufferSize, Transmit_Packet.frame_type, destination, + mstp_port->This_Station, &Transmit_Packet.pdu[0], + Transmit_Packet.pdu_len); + Transmit_Packet.ready = false; + + return pdu_len; +} + +void dlmstp_set_mac_address( + uint8_t mac_address) +{ + /* Master Nodes can only have address 0-127 */ + if (mac_address <= 127) { + MSTP_Port.This_Station = mac_address; + /* FIXME: implement your data storage */ + /* I2C_Write_Byte( + EEPROM_DEVICE_ADDRESS, + mac_address, + EEPROM_MSTP_MAC_ADDR); */ + if (mac_address > MSTP_Port.Nmax_master) + dlmstp_set_max_master(mac_address); + } + + return; +} + +uint8_t dlmstp_mac_address( + void) +{ + return MSTP_Port.This_Station; +} + +/* This parameter represents the value of the Max_Info_Frames property of */ +/* the node's Device object. The value of Max_Info_Frames specifies the */ +/* maximum number of information frames the node may send before it must */ +/* pass the token. Max_Info_Frames may have different values on different */ +/* nodes. This may be used to allocate more or less of the available link */ +/* bandwidth to particular nodes. If Max_Info_Frames is not writable in a */ +/* node, its value shall be 1. */ +void dlmstp_set_max_info_frames( + uint8_t max_info_frames) +{ + if (max_info_frames >= 1) { + MSTP_Port.Nmax_info_frames = max_info_frames; + /* FIXME: implement your data storage */ + /* I2C_Write_Byte( + EEPROM_DEVICE_ADDRESS, + (uint8_t)max_info_frames, + EEPROM_MSTP_MAX_INFO_FRAMES_ADDR); */ + } + + return; +} + +uint8_t dlmstp_max_info_frames( + void) +{ + return MSTP_Port.Nmax_info_frames; +} + +/* This parameter represents the value of the Max_Master property of the */ +/* node's Device object. The value of Max_Master specifies the highest */ +/* allowable address for master nodes. The value of Max_Master shall be */ +/* less than or equal to 127. If Max_Master is not writable in a node, */ +/* its value shall be 127. */ +void dlmstp_set_max_master( + uint8_t max_master) +{ + if (max_master <= 127) { + if (MSTP_Port.This_Station <= max_master) { + MSTP_Port.Nmax_master = max_master; + /* FIXME: implement your data storage */ + /* I2C_Write_Byte( + EEPROM_DEVICE_ADDRESS, + max_master, + EEPROM_MSTP_MAX_MASTER_ADDR); */ + } + } + + return; +} + +uint8_t dlmstp_max_master( + void) +{ + return MSTP_Port.Nmax_master; +} + +/* RS485 Baud Rate 9600, 19200, 38400, 57600, 115200 */ +void dlmstp_set_baud_rate( + uint32_t baud) +{ + RS485_Set_Baud_Rate(baud); +} + +uint32_t dlmstp_baud_rate( + void) +{ + return RS485_Get_Baud_Rate(); +} + +void dlmstp_get_my_address( + BACNET_ADDRESS * my_address) +{ + int i = 0; /* counter */ + + my_address->mac_len = 1; + my_address->mac[0] = MSTP_Port.This_Station; + my_address->net = 0; /* local only, no routing */ + my_address->len = 0; + for (i = 0; i < MAX_MAC_LEN; i++) { + my_address->adr[i] = 0; + } + + return; +} + +void dlmstp_get_broadcast_address( + BACNET_ADDRESS * dest) +{ /* destination address */ + int i = 0; /* counter */ + + if (dest) { + dest->mac_len = 1; + dest->mac[0] = MSTP_BROADCAST_ADDRESS; + dest->net = BACNET_BROADCAST_NETWORK; + dest->len = 0; /* always zero when DNET is broadcast */ + for (i = 0; i < MAX_MAC_LEN; i++) { + dest->adr[i] = 0; + } + } + + return; +} + +bool dlmstp_init( + char *ifname) +{ + unsigned long hThread = 0; + uint32_t arg_value = 0; + + /* initialize packet queue */ + Receive_Packet.ready = false; + Receive_Packet.pdu_len = 0; + Receive_Packet_Flag = CreateSemaphore(NULL, 0, 1, "dlmstpReceivePacket"); + if (Receive_Packet_Flag == NULL) + exit(1); + Received_Frame_Flag = CreateSemaphore(NULL, 0, 1, "dlsmtpReceiveFrame"); + if (Received_Frame_Flag == NULL) { + CloseHandle(Receive_Packet_Flag); + exit(1); + } + /* initialize hardware */ + /* initialize hardware */ + if (ifname) { + RS485_Set_Interface(ifname); +#if PRINT_ENABLED + fprintf(stderr, "MS/TP Interface: %s\n", ifname); +#endif + } + RS485_Initialize(); + MSTP_Port.InputBuffer = &RxBuffer[0]; + MSTP_Port.InputBufferSize = sizeof(RxBuffer); + MSTP_Port.OutputBuffer = &TxBuffer[0]; + MSTP_Port.OutputBufferSize = sizeof(TxBuffer); + MSTP_Port.SilenceTimer = Timer_Silence; + MSTP_Port.SilenceTimerReset = Timer_Silence_Reset; + MSTP_Init(&MSTP_Port); +#if 0 + uint8_t data; + + /* FIXME: implement your data storage */ + data = 64; /* I2C_Read_Byte( + EEPROM_DEVICE_ADDRESS, + EEPROM_MSTP_MAC_ADDR); */ + if (data <= 127) + MSTP_Port.This_Station = data; + else + dlmstp_set_my_address(DEFAULT_MAC_ADDRESS); + /* FIXME: implement your data storage */ + data = 127; /* I2C_Read_Byte( + EEPROM_DEVICE_ADDRESS, + EEPROM_MSTP_MAX_MASTER_ADDR); */ + if ((data <= 127) && (data >= MSTP_Port.This_Station)) + MSTP_Port.Nmax_master = data; + else + dlmstp_set_max_master(DEFAULT_MAX_MASTER); + /* FIXME: implement your data storage */ + data = 1; + /* I2C_Read_Byte( + EEPROM_DEVICE_ADDRESS, + EEPROM_MSTP_MAX_INFO_FRAMES_ADDR); */ + if (data >= 1) + MSTP_Port.Nmax_info_frames = data; + else + dlmstp_set_max_info_frames(DEFAULT_MAX_INFO_FRAMES); +#endif +#if PRINT_ENABLED + fprintf(stderr, "MS/TP MAC: %02X\n", MSTP_Port.This_Station); + fprintf(stderr, "MS/TP Max_Master: %02X\n", MSTP_Port.Nmax_master); + fprintf(stderr, "MS/TP Max_Info_Frames: %u\n", MSTP_Port.Nmax_info_frames); +#endif + /* start the threads */ + hThread = _beginthread(dlmstp_millisecond_task, 4096, &arg_value); + if (hThread == 0) { + fprintf(stderr, "Failed to start timer task\n"); + } + hThread = _beginthread(dlmstp_receive_fsm_task, 4096, &arg_value); + if (hThread == 0) { + fprintf(stderr, "Failed to start recive FSM task\n"); + } + hThread = _beginthread(dlmstp_master_fsm_task, 4096, &arg_value); + if (hThread == 0) { + fprintf(stderr, "Failed to start Master Node FSM task\n"); + } + + return true; +} + +#ifdef TEST_DLMSTP +#include + +void apdu_handler( + BACNET_ADDRESS * src, /* source address */ + uint8_t * apdu, /* APDU data */ + uint16_t pdu_len) +{ /* for confirmed messages */ + (void) src; + (void) apdu; + (void) pdu_len; +} + +/* returns a delta timestamp */ +uint32_t timestamp_ms( + void) +{ + DWORD ticks = 0, delta_ticks = 0; + static DWORD last_ticks = 0; + + ticks = GetTickCount(); + delta_ticks = + (ticks >= last_ticks ? ticks - last_ticks : MAXDWORD - last_ticks); + last_ticks = ticks; + + return delta_ticks; +} + +static char *Network_Interface = NULL; + +int main( + int argc, + char *argv[]) +{ + uint16_t pdu_len = 0; + + /* argv has the "COM4" or some other device */ + if (argc > 1) { + Network_Interface = argv[1]; + } + dlmstp_set_baud_rate(38400); + dlmstp_set_mac_address(0x05); + dlmstp_set_max_info_frames(DEFAULT_MAX_INFO_FRAMES); + dlmstp_set_max_master(DEFAULT_MAX_MASTER); + dlmstp_init(Network_Interface); + /* forever task */ + for (;;) { + pdu_len = dlmstp_receive(NULL, NULL, 0, INFINITE); +#if 0 + MSTP_Create_And_Send_Frame(&MSTP_Port, FRAME_TYPE_TEST_REQUEST, + MSTP_Port.SourceAddress, MSTP_Port.This_Station, NULL, 0); +#endif + } + + return 0; +} +#endif