dali/bacnet_stack
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

indented using indent.sh script to get uniform looking code for release

Browse Source
This commit is contained in:
skarg
2013-03-13 22:17:13 +00:00
parent 2de46521b8
commit ba3242aafd
120 changed files with 3382 additions and 3299 deletions
Download Patch File Download Diff File Expand all files Collapse all files
bacnet-stack/demo/router/ipmodule.c
+338 -311
View File
@@ -21,9 +21,10 @@ Copyright (C) 2012 Andriy Sukhynyuk, Vasyl Tkhir, Andriy Ivasiv
#include "bacint.h"
#ifdef TEST_PACKET
uint8_t test_packet[]={0x81, 0x0a, 0x00, 0x16, /* BVLC header */
0x01, 0x24, 0x00, 0x01, 0x01, 0x0b, 0xff, /* NPDU */
0x00, 0x03, 0x01, 0x0c, 0x0c, 0x00, 0x00, 0x00, 0x02, 0x19, 0x55}; /* APDU */
uint8_t test_packet[] = { 0x81, 0x0a, 0x00, 0x16, /* BVLC header */
0x01, 0x24, 0x00, 0x01, 0x01, 0x0b, 0xff, /* NPDU */
0x00, 0x03, 0x01, 0x0c, 0x0c, 0x00, 0x00, 0x00, 0x02, 0x19, 0x55
}; /* APDU */
#endif
extern int get_local_address_ioctl(
@@ -31,345 +32,371 @@ extern int get_local_address_ioctl(
struct in_addr *addr,
int request);
void* dl_ip_thread(void *pArgs) {
MSGBOX_ID msgboxid;
BACMSG msg_storage, *bacmsg = NULL;
MSG_DATA *msg_data;
ROUTER_PORT *port = (ROUTER_PORT*)pArgs;
IP_DATA ip_data; /* port specific parameters */
BACNET_ADDRESS address = { 0 };
int status;
uint8_t shutdown = 0;
/* initialize router port */
if (!dl_ip_init(port, &ip_data)) {
port->state = INIT_FAILED;
return NULL;
}
/* allocate buffer */
ip_data.max_buff = MAX_BIP_MPDU;
ip_data.buff = (uint8_t*)malloc(ip_data.max_buff);
if (ip_data.buff == NULL) {
port->state = INIT_FAILED;
return NULL;
}
msgboxid = create_msgbox();
if (msgboxid == INVALID_MSGBOX_ID) {
PRINT(ERROR, "Error: Failed to create message box");
port->state = INIT_FAILED;
return NULL;
}
port->port_id = msgboxid;
port->state = RUNNING;
while (!shutdown) {
/* check for incoming messages */
bacmsg = recv_from_msgbox(port->port_id, &msg_storage);
if (bacmsg) {
switch(bacmsg->type) {
case DATA: {
msg_data = (MSG_DATA*)bacmsg->data;
memmove(&address.net, &msg_data->dest.net, 2);
memmove(&address.mac_len, &msg_data->dest.len, 1);
memmove(&address.mac[0], &msg_data->dest.adr[0], MAX_MAC_LEN);
dl_ip_send(&ip_data, &address, msg_data->pdu, msg_data->pdu_len);
check_data(msg_data);
break;
}
case SERVICE: {
switch(bacmsg->subtype) {
case SHUTDOWN:
del_msgbox(port->port_id);
shutdown = 1;
break;
default:
break;
}
break;
}
default:
break;
}
} else {
status = dl_ip_recv(&ip_data, &msg_data, &address, 1000);
if (status > 0) {
memmove(&msg_data->src.len, &address.mac_len, 1);
memmove(&msg_data->src.adr[0], &address.mac[0], MAX_MAC_LEN);
msg_storage.origin = port->port_id;
msg_storage.type = DATA;
msg_storage.data = msg_data;
if (!send_to_msgbox(port->main_id, &msg_storage)) {
free_data(msg_data);
}
}
}
}
/* cleanup procedure */
dl_ip_cleanup(&ip_data);
port->state = FINISHED;
return NULL;
}
bool dl_ip_init(ROUTER_PORT *port,
IP_DATA *ip_data) {
struct sockaddr_in sin;
int socket_opt = 0;
int status = 0; /* for error checking */
/* setup port for later use */
ip_data->port = htons(port->params.bip_params.port);
/* get local address */
status = get_local_address_ioctl(port->iface, &ip_data->local_addr, SIOCGIFADDR);
if (status < 0) {
return false;
}
/* get broadcast address */
status = get_local_address_ioctl(port->iface, &ip_data->broadcast_addr, SIOCGIFBRDADDR);
if (status < 0) {
return false;
}
ip_data->socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (ip_data->socket < 0)
return false;
/* setup socket options */
socket_opt = 1;
status = setsockopt(ip_data->socket, SOL_SOCKET, SO_REUSEADDR, &socket_opt, sizeof(socket_opt));
if (status < 0) {
close(ip_data->socket);
return false;
}
status = setsockopt(ip_data->socket, SOL_SOCKET, SO_BROADCAST, &socket_opt, sizeof(socket_opt));
if (status < 0) {
close(ip_data->socket);
return false;
}
/* bind the socket to the local port number */
sin.sin_family = AF_INET;
sin.sin_addr.s_addr = htonl(INADDR_ANY);
sin.sin_port = ip_data->port;
memset(&sin.sin_zero, '\0', sizeof(sin.sin_zero));
status = bind(ip_data->socket, (const struct sockaddr *) &sin, sizeof(struct sockaddr));
if (status < 0) {
close(ip_data->socket);
return false;
}
/* add BIP address to router port structure */
memcpy(&port->route_info.mac[0], &ip_data->local_addr.s_addr, 4);
memcpy(&port->route_info.mac[4], &port->params.bip_params.port, 2);
port->route_info.mac_len = 6;
PRINT(INFO, "Interface: %s\n", port->iface);
PRINT(INFO, "IP Address: %s\n", inet_ntoa(ip_data->local_addr));
PRINT(INFO, "IP Broadcast Address: %s\n", inet_ntoa(ip_data->broadcast_addr));
PRINT(INFO, "UDP Port: 0x%04X [%hu]\n", (port->params.bip_params.port),
(port->params.bip_params.port));
return true;
}
int dl_ip_send(IP_DATA *data,
BACNET_ADDRESS *dest,
uint8_t *pdu,
unsigned pdu_len)
void *dl_ip_thread(
void *pArgs)
{
struct sockaddr_in bip_dest = { 0 };
int buff_len = 0;
int bytes_sent = 0;
MSGBOX_ID msgboxid;
BACMSG msg_storage, *bacmsg = NULL;
MSG_DATA *msg_data;
ROUTER_PORT *port = (ROUTER_PORT *) pArgs;
IP_DATA ip_data; /* port specific parameters */
BACNET_ADDRESS address = { 0 };
int status;
uint8_t shutdown = 0;
if (data->socket < 0)
return -1;
/* initialize router port */
if (!dl_ip_init(port, &ip_data)) {
port->state = INIT_FAILED;
return NULL;
}
data->buff[0] = BVLL_TYPE_BACNET_IP;
bip_dest.sin_family = AF_INET;
if (dest->net == BACNET_BROADCAST_NETWORK) {
/* broadcast */
bip_dest.sin_addr.s_addr = data->broadcast_addr.s_addr;
bip_dest.sin_port = data->port;
data->buff[1] = BVLC_ORIGINAL_BROADCAST_NPDU;
} else if (dest->mac_len == 6) {
memcpy(&bip_dest.sin_addr.s_addr, &dest->mac[0], 4);
memcpy(&bip_dest.sin_port, &dest->mac[4], 2);
data->buff[1] = BVLC_ORIGINAL_UNICAST_NPDU;
} else {
/* invalid address */
return -1;
}
/* allocate buffer */
ip_data.max_buff = MAX_BIP_MPDU;
ip_data.buff = (uint8_t *) malloc(ip_data.max_buff);
buff_len = 2;
buff_len +=
encode_unsigned16(&data->buff[buff_len],
(uint16_t) (pdu_len + 4 /*inclusive*/ ));
memcpy(&data->buff[buff_len], pdu, pdu_len);
buff_len += pdu_len;
if (ip_data.buff == NULL) {
port->state = INIT_FAILED;
return NULL;
}
/* send the packet */
bytes_sent =
sendto(data->socket, (char *) data->buff, buff_len, 0,
(struct sockaddr *) &bip_dest, sizeof(struct sockaddr));
msgboxid = create_msgbox();
if (msgboxid == INVALID_MSGBOX_ID) {
PRINT(ERROR, "Error: Failed to create message box");
port->state = INIT_FAILED;
return NULL;
}
PRINT(DEBUG, "send to %s\n", inet_ntoa(bip_dest.sin_addr));
port->port_id = msgboxid;
port->state = RUNNING;
return bytes_sent;
while (!shutdown) {
/* check for incoming messages */
bacmsg = recv_from_msgbox(port->port_id, &msg_storage);
if (bacmsg) {
switch (bacmsg->type) {
case DATA:{
msg_data = (MSG_DATA *) bacmsg->data;
memmove(&address.net, &msg_data->dest.net, 2);
memmove(&address.mac_len, &msg_data->dest.len, 1);
memmove(&address.mac[0], &msg_data->dest.adr[0],
MAX_MAC_LEN);
dl_ip_send(&ip_data, &address, msg_data->pdu,
msg_data->pdu_len);
check_data(msg_data);
break;
}
case SERVICE:{
switch (bacmsg->subtype) {
case SHUTDOWN:
del_msgbox(port->port_id);
shutdown = 1;
break;
default:
break;
}
break;
}
default:
break;
}
} else {
status = dl_ip_recv(&ip_data, &msg_data, &address, 1000);
if (status > 0) {
memmove(&msg_data->src.len, &address.mac_len, 1);
memmove(&msg_data->src.adr[0], &address.mac[0], MAX_MAC_LEN);
msg_storage.origin = port->port_id;
msg_storage.type = DATA;
msg_storage.data = msg_data;
if (!send_to_msgbox(port->main_id, &msg_storage)) {
free_data(msg_data);
}
}
}
}
/* cleanup procedure */
dl_ip_cleanup(&ip_data);
port->state = FINISHED;
return NULL;
}
bool dl_ip_init(
ROUTER_PORT * port,
IP_DATA * ip_data)
{
struct sockaddr_in sin;
int socket_opt = 0;
int status = 0; /* for error checking */
/* setup port for later use */
ip_data->port = htons(port->params.bip_params.port);
/* get local address */
status =
get_local_address_ioctl(port->iface, &ip_data->local_addr,
SIOCGIFADDR);
if (status < 0) {
return false;
}
/* get broadcast address */
status =
get_local_address_ioctl(port->iface, &ip_data->broadcast_addr,
SIOCGIFBRDADDR);
if (status < 0) {
return false;
}
ip_data->socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (ip_data->socket < 0)
return false;
/* setup socket options */
socket_opt = 1;
status =
setsockopt(ip_data->socket, SOL_SOCKET, SO_REUSEADDR, &socket_opt,
sizeof(socket_opt));
if (status < 0) {
close(ip_data->socket);
return false;
}
status =
setsockopt(ip_data->socket, SOL_SOCKET, SO_BROADCAST, &socket_opt,
sizeof(socket_opt));
if (status < 0) {
close(ip_data->socket);
return false;
}
/* bind the socket to the local port number */
sin.sin_family = AF_INET;
sin.sin_addr.s_addr = htonl(INADDR_ANY);
sin.sin_port = ip_data->port;
memset(&sin.sin_zero, '\0', sizeof(sin.sin_zero));
status =
bind(ip_data->socket, (const struct sockaddr *) &sin,
sizeof(struct sockaddr));
if (status < 0) {
close(ip_data->socket);
return false;
}
/* add BIP address to router port structure */
memcpy(&port->route_info.mac[0], &ip_data->local_addr.s_addr, 4);
memcpy(&port->route_info.mac[4], &port->params.bip_params.port, 2);
port->route_info.mac_len = 6;
PRINT(INFO, "Interface: %s\n", port->iface);
PRINT(INFO, "IP Address: %s\n", inet_ntoa(ip_data->local_addr));
PRINT(INFO, "IP Broadcast Address: %s\n",
inet_ntoa(ip_data->broadcast_addr));
PRINT(INFO, "UDP Port: 0x%04X [%hu]\n", (port->params.bip_params.port),
(port->params.bip_params.port));
return true;
}
int dl_ip_send(
IP_DATA * data,
BACNET_ADDRESS * dest,
uint8_t * pdu,
unsigned pdu_len)
{
struct sockaddr_in bip_dest = { 0 };
int buff_len = 0;
int bytes_sent = 0;
if (data->socket < 0)
return -1;
data->buff[0] = BVLL_TYPE_BACNET_IP;
bip_dest.sin_family = AF_INET;
if (dest->net == BACNET_BROADCAST_NETWORK) {
/* broadcast */
bip_dest.sin_addr.s_addr = data->broadcast_addr.s_addr;
bip_dest.sin_port = data->port;
data->buff[1] = BVLC_ORIGINAL_BROADCAST_NPDU;
} else if (dest->mac_len == 6) {
memcpy(&bip_dest.sin_addr.s_addr, &dest->mac[0], 4);
memcpy(&bip_dest.sin_port, &dest->mac[4], 2);
data->buff[1] = BVLC_ORIGINAL_UNICAST_NPDU;
} else {
/* invalid address */
return -1;
}
buff_len = 2;
buff_len +=
encode_unsigned16(&data->buff[buff_len],
(uint16_t) (pdu_len + 4 /*inclusive */ ));
memcpy(&data->buff[buff_len], pdu, pdu_len);
buff_len += pdu_len;
/* send the packet */
bytes_sent =
sendto(data->socket, (char *) data->buff, buff_len, 0,
(struct sockaddr *) &bip_dest, sizeof(struct sockaddr));
PRINT(DEBUG, "send to %s\n", inet_ntoa(bip_dest.sin_addr));
return bytes_sent;
}
int dl_ip_recv(
IP_DATA *data,
MSG_DATA **msg_data,
BACNET_ADDRESS *src,
unsigned timeout)
IP_DATA * data,
MSG_DATA ** msg_data,
BACNET_ADDRESS * src,
unsigned timeout)
{
int received_bytes = 0;
uint16_t buff_len = 0; /* return value */
fd_set read_fds;
struct timeval select_timeout;
struct sockaddr_in sin = { 0 };
socklen_t sin_len = sizeof(sin);
int received_bytes = 0;
uint16_t buff_len = 0; /* return value */
fd_set read_fds;
struct timeval select_timeout;
struct sockaddr_in sin = { 0 };
socklen_t sin_len = sizeof(sin);
/* make sure the socket is open */
if (data->socket < 0)
return 0;
/* make sure the socket is open */
if (data->socket < 0)
return 0;
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;
}
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(data->socket, &read_fds);
FD_ZERO(&read_fds);
FD_SET(data->socket, &read_fds);
#ifdef TEST_PACKET
received_bytes = sizeof(test_packet);
memmove(data->buff, &test_packet, received_bytes);
sin.sin_addr.s_addr = 0x7E1D40A; sin.sin_port = 0xC0BA;
received_bytes = sizeof(test_packet);
memmove(data->buff, &test_packet, received_bytes);
sin.sin_addr.s_addr = 0x7E1D40A;
sin.sin_port = 0xC0BA;
#else
int ret = select(data->socket + 1, &read_fds, NULL, NULL, &select_timeout);
/* see if there is a packet for us */
if (ret > 0)
received_bytes =
recvfrom(data->socket, (char *) &data->buff[0], data->max_buff, 0,
(struct sockaddr *) &sin, &sin_len);
else
return 0;
int ret = select(data->socket + 1, &read_fds, NULL, NULL, &select_timeout);
/* see if there is a packet for us */
if (ret > 0)
received_bytes =
recvfrom(data->socket, (char *) &data->buff[0], data->max_buff, 0,
(struct sockaddr *) &sin, &sin_len);
else
return 0;
#endif
PRINT(DEBUG, "received from %s\n", inet_ntoa(sin.sin_addr));
PRINT(DEBUG, "received from %s\n", inet_ntoa(sin.sin_addr));
/* check for errors */
if (received_bytes <= 0) {
return 0;
}
/* check for errors */
if (received_bytes <= 0) {
return 0;
}
/* the signature of a BACnet/IP packet */
if (data->buff[0] != BVLL_TYPE_BACNET_IP)
return 0;
/* the signature of a BACnet/IP packet */
if (data->buff[0] != BVLL_TYPE_BACNET_IP)
return 0;
switch (data->buff[1]) {
case BVLC_ORIGINAL_UNICAST_NPDU:
case BVLC_ORIGINAL_BROADCAST_NPDU: {
if ((sin.sin_addr.s_addr == data->local_addr.s_addr) &&
(sin.sin_port == data->port)) {
buff_len = 0;
switch (data->buff[1]) {
case BVLC_ORIGINAL_UNICAST_NPDU:
case BVLC_ORIGINAL_BROADCAST_NPDU:{
if ((sin.sin_addr.s_addr == data->local_addr.s_addr) &&
(sin.sin_port == data->port)) {
buff_len = 0;
PRINT(DEBUG, "BIP: src is me. Discarded!\n");
PRINT(DEBUG, "BIP: src is me. Discarded!\n");
} else {
src->mac_len = 6;
memcpy(&src->mac[0], &sin.sin_addr.s_addr, 4);
memcpy(&src->mac[4], &sin.sin_port, 2);
} else {
src->mac_len = 6;
memcpy(&src->mac[0], &sin.sin_addr.s_addr, 4);
memcpy(&src->mac[4], &sin.sin_port, 2);
(void) decode_unsigned16(&data->buff[2], &buff_len);
/* subtract off the BVLC header */
buff_len -= 4;
if (buff_len < data->max_buff) {
/* allocate data message stucture */
(*msg_data) = (MSG_DATA*)malloc(sizeof(MSG_DATA));
(*msg_data)->pdu_len = buff_len;
(*msg_data)->pdu = (uint8_t*)malloc((*msg_data)->pdu_len);
/* fill up data message structure */
memmove(&(*msg_data)->pdu[0], &data->buff[4], (*msg_data)->pdu_len);
memmove(&(*msg_data)->src, src, sizeof(BACNET_ADDRESS));
}
/* ignore packets that are too large */
else {
buff_len = 0;
(void) decode_unsigned16(&data->buff[2], &buff_len);
/* subtract off the BVLC header */
buff_len -= 4;
if (buff_len < data->max_buff) {
/* allocate data message stucture */
(*msg_data) = (MSG_DATA *) malloc(sizeof(MSG_DATA));
(*msg_data)->pdu_len = buff_len;
(*msg_data)->pdu =
(uint8_t *) malloc((*msg_data)->pdu_len);
/* fill up data message structure */
memmove(&(*msg_data)->pdu[0], &data->buff[4],
(*msg_data)->pdu_len);
memmove(&(*msg_data)->src, src,
sizeof(BACNET_ADDRESS));
}
/* ignore packets that are too large */
else {
buff_len = 0;
PRINT(ERROR, "BIP: PDU too large. Discarded!.\n");
PRINT(ERROR, "BIP: PDU too large. Discarded!.\n");
}
}
}
break;
}
}
}
break;
case BVLC_FORWARDED_NPDU: {
memcpy(&sin.sin_addr.s_addr, &data->buff[4], 4);
memcpy(&sin.sin_port, &data->buff[8], 2);
if ((sin.sin_addr.s_addr == data->local_addr.s_addr) &&
(sin.sin_port == data->port)) {
buff_len = 0;
} else {
src->mac_len = 6;
memcpy(&src->mac[0], &sin.sin_addr.s_addr, 4);
memcpy(&src->mac[4], &sin.sin_port, 2);
case BVLC_FORWARDED_NPDU:{
memcpy(&sin.sin_addr.s_addr, &data->buff[4], 4);
memcpy(&sin.sin_port, &data->buff[8], 2);
if ((sin.sin_addr.s_addr == data->local_addr.s_addr) &&
(sin.sin_port == data->port)) {
buff_len = 0;
} else {
src->mac_len = 6;
memcpy(&src->mac[0], &sin.sin_addr.s_addr, 4);
memcpy(&src->mac[4], &sin.sin_port, 2);
(void) decode_unsigned16(&data->buff[2], &buff_len);
/* subtract off the BVLC header */
buff_len -= 10;
if (buff_len < data->max_buff) {
/* allocate data message stucture */
(*msg_data) = (MSG_DATA*)malloc(sizeof(MSG_DATA));
(*msg_data)->pdu_len = buff_len;
(*msg_data)->pdu = (uint8_t*)malloc((*msg_data)->pdu_len);
/* fill up data message structure */
memmove(&(*msg_data)->pdu, &data->buff[4+6], (*msg_data)->pdu_len);
memmove(&(*msg_data)->src, src, sizeof(BACNET_ADDRESS));
} else {
/* ignore packets that are too large */
buff_len = 0;
}
}
}
break;
default:
(void) decode_unsigned16(&data->buff[2], &buff_len);
/* subtract off the BVLC header */
buff_len -= 10;
if (buff_len < data->max_buff) {
/* allocate data message stucture */
(*msg_data) = (MSG_DATA *) malloc(sizeof(MSG_DATA));
(*msg_data)->pdu_len = buff_len;
(*msg_data)->pdu =
(uint8_t *) malloc((*msg_data)->pdu_len);
/* fill up data message structure */
memmove(&(*msg_data)->pdu, &data->buff[4 + 6],
(*msg_data)->pdu_len);
memmove(&(*msg_data)->src, src,
sizeof(BACNET_ADDRESS));
} else {
/* ignore packets that are too large */
buff_len = 0;
}
}
}
break;
default:
PRINT(ERROR, "BIP: BVLC discarded!\n");
PRINT(ERROR, "BIP: BVLC discarded!\n");
break;
}
return buff_len;
break;
}
return buff_len;
}
void dl_ip_cleanup(
IP_DATA *ip_data) {
/* free buffer */
if (ip_data->buff)
free(ip_data->buff);
/* close socket */
if (ip_data->socket > 0)
close(ip_data->socket);
return;
IP_DATA * ip_data)
{
/* free buffer */
if (ip_data->buff)
free(ip_data->buff);
/* close socket */
if (ip_data->socket > 0)
close(ip_data->socket);
return;
}
bacnet-stack/demo/router/ipmodule.h
+21 -21
View File
@@ -26,36 +26,36 @@ Copyright (C) 2012 Andriy Sukhynyuk, Vasyl Tkhir, Andriy Ivasiv
#define MAX_BIP_PDU (MAX_NPDU + MAX_BIP_APDU)
#define MAX_BIP_MPDU (MAX_HEADER + MAX_BIP_PDU)
typedef struct ip_data {
int socket;
uint16_t port;
struct in_addr local_addr;
struct in_addr broadcast_addr;
uint8_t *buff;
uint16_t max_buff;
} IP_DATA;
typedef struct ip_data {
int socket;
uint16_t port;
struct in_addr local_addr;
struct in_addr broadcast_addr;
uint8_t *buff;
uint16_t max_buff;
} IP_DATA;
void* dl_ip_thread(
void *pArgs);
void *dl_ip_thread(
void *pArgs);
bool dl_ip_init(
ROUTER_PORT *port,
IP_DATA *data);
ROUTER_PORT * port,
IP_DATA * data);
int dl_ip_send(
IP_DATA *data,
BACNET_ADDRESS *dest,
uint8_t *pdu,
unsigned pdu_len);
IP_DATA * data,
BACNET_ADDRESS * dest,
uint8_t * pdu,
unsigned pdu_len);
int dl_ip_recv(
IP_DATA *data,
MSG_DATA **msg, /* on recieve fill up message */
BACNET_ADDRESS *src,
unsigned timeout);
IP_DATA * data,
MSG_DATA ** msg, /* on recieve fill up message */
BACNET_ADDRESS * src,
unsigned timeout);
void dl_ip_cleanup(
IP_DATA *data);
IP_DATA * data);
#endif /* end of UDPMODULE_H */
bacnet-stack/demo/router/main.c
+695 -625
View File
File diff suppressed because it is too large Load Diff
bacnet-stack/demo/router/msgqueue.c
+53 -46
View File
@@ -23,73 +23,80 @@ Copyright (C) 2012 Andriy Sukhynyuk, Vasyl Tkhir, Andriy Ivasiv
pthread_mutex_t msg_lock = PTHREAD_MUTEX_INITIALIZER;
MSGBOX_ID create_msgbox() {
MSGBOX_ID msgboxid;
MSGBOX_ID create_msgbox(
)
{
MSGBOX_ID msgboxid;
msgboxid = msgget(IPC_PRIVATE, 0666 | IPC_CREAT);
if (msgboxid == INVALID_MSGBOX_ID) {
return INVALID_MSGBOX_ID;
}
msgboxid = msgget(IPC_PRIVATE, 0666 | IPC_CREAT);
if (msgboxid == INVALID_MSGBOX_ID) {
return INVALID_MSGBOX_ID;
}
return msgboxid;
return msgboxid;
}
bool send_to_msgbox(
MSGBOX_ID dest,
BACMSG *msg) {
MSGBOX_ID dest,
BACMSG * msg)
{
int err;
int err;
err = msgsnd(dest, msg, sizeof(BACMSG), 0);
if (err) {
return false;
}
return true;
err = msgsnd(dest, msg, sizeof(BACMSG), 0);
if (err) {
return false;
}
return true;
}
BACMSG* recv_from_msgbox(
MSGBOX_ID src,
BACMSG *msg) {
BACMSG *recv_from_msgbox(
MSGBOX_ID src,
BACMSG * msg)
{
int recv_bytes;
int recv_bytes;
recv_bytes = msgrcv(src, msg, sizeof(BACMSG), 0, IPC_NOWAIT);
if (recv_bytes > 0) {
return msg;
} else {
return NULL;
}
recv_bytes = msgrcv(src, msg, sizeof(BACMSG), 0, IPC_NOWAIT);
if (recv_bytes > 0) {
return msg;
} else {
return NULL;
}
}
void del_msgbox(
MSGBOX_ID msgboxid) {
MSGBOX_ID msgboxid)
{
if (msgboxid == INVALID_MSGBOX_ID)
return;
else
msgctl(msgboxid, IPC_RMID, NULL);
if (msgboxid == INVALID_MSGBOX_ID)
return;
else
msgctl(msgboxid, IPC_RMID, NULL);
}
void free_data(
MSG_DATA *data) {
MSG_DATA * data)
{
if (data->pdu) {
free(data->pdu);
data->pdu = NULL;
}
if (data) {
free(data);
data = NULL;
}
if (data->pdu) {
free(data->pdu);
data->pdu = NULL;
}
if (data) {
free(data);
data = NULL;
}
}
void check_data(
MSG_DATA *data) {
MSG_DATA * data)
{
/* lock and decrement messages reference count */
pthread_mutex_lock(&msg_lock);
if (--data->ref_count == 0) {
free_data(data);
}
pthread_mutex_unlock(&msg_lock);
/* lock and decrement messages reference count */
pthread_mutex_lock(&msg_lock);
if (--data->ref_count == 0) {
free_data(data);
}
pthread_mutex_unlock(&msg_lock);
}
bacnet-stack/demo/router/msgqueue.h
+25 -24
View File
@@ -32,54 +32,55 @@ extern pthread_mutex_t msg_lock;
typedef int MSGBOX_ID;
typedef enum {
DATA,
SERVICE
DATA,
SERVICE
} MSGTYPE;
typedef enum {
SHUTDOWN,
CHG_IP,
CHG_MAC
SHUTDOWN,
CHG_IP,
CHG_MAC
} MSGSUBTYPE;
typedef struct _message {
MSGBOX_ID origin;
MSGTYPE type;
MSGSUBTYPE subtype;
void *data;
/* add timestamp */
MSGBOX_ID origin;
MSGTYPE type;
MSGSUBTYPE subtype;
void *data;
/* add timestamp */
} BACMSG;
/* specific message type data structures */
typedef struct _msg_data {
BACNET_ADDRESS dest;
BACNET_ADDRESS src;
uint8_t *pdu;
uint16_t pdu_len;
uint8_t ref_count;
BACNET_ADDRESS dest;
BACNET_ADDRESS src;
uint8_t *pdu;
uint16_t pdu_len;
uint8_t ref_count;
} MSG_DATA;
MSGBOX_ID create_msgbox();
MSGBOX_ID create_msgbox(
);
/* returns sent byte count */
bool send_to_msgbox(
MSGBOX_ID dest,
BACMSG *msg);
MSGBOX_ID dest,
BACMSG * msg);
/* returns received message */
BACMSG* recv_from_msgbox(
MSGBOX_ID src,
BACMSG *msg);
BACMSG *recv_from_msgbox(
MSGBOX_ID src,
BACMSG * msg);
void del_msgbox(
MSGBOX_ID msgboxid);
MSGBOX_ID msgboxid);
/* free message data structure */
void free_data(
MSG_DATA *data);
MSG_DATA * data);
/* check message reference counter and delete data if needed */
void check_data(
MSG_DATA *data);
MSG_DATA * data);
#endif /* end of MSGQUEUE_H */
bacnet-stack/demo/router/mstpmodule.c
+113 -114
View File
@@ -29,140 +29,139 @@ Copyright (C) 2012 Andriy Sukhynyuk, Vasyl Tkhir, Andriy Ivasiv
#define mstp_thread_debug(...)
#endif
void* dl_mstp_thread(void* pArgs) {
void *dl_mstp_thread(
void *pArgs)
{
ROUTER_PORT *port = (ROUTER_PORT*)pArgs;
struct mstp_port_struct_t mstp_port = {(MSTP_RECEIVE_STATE)0};
volatile SHARED_MSTP_DATA shared_port_data = {0};
uint16_t pdu_len;
uint8_t shutdown = 0;
ROUTER_PORT *port = (ROUTER_PORT *) pArgs;
struct mstp_port_struct_t mstp_port = { (MSTP_RECEIVE_STATE) 0 };
volatile SHARED_MSTP_DATA shared_port_data = { 0 };
uint16_t pdu_len;
uint8_t shutdown = 0;
shared_port_data.Treply_timeout = 260;
shared_port_data.MSTP_Packets = 0;
shared_port_data.Tusage_timeout = 50;
shared_port_data.RS485_Handle = -1;
shared_port_data.RS485_Baud = B38400;
shared_port_data.RS485MOD = 0;
shared_port_data.Treply_timeout = 260;
shared_port_data.MSTP_Packets = 0;
shared_port_data.Tusage_timeout = 50;
shared_port_data.RS485_Handle = -1;
shared_port_data.RS485_Baud = B38400;
shared_port_data.RS485MOD = 0;
switch(port->params.mstp_params.databits){
case 5:
shared_port_data.RS485MOD = CS5;
break;
case 6:
shared_port_data.RS485MOD = CS6;
break;
case 7:
shared_port_data.RS485MOD = CS7;
break;
default:
shared_port_data.RS485MOD = CS8;
break;
}
switch (port->params.mstp_params.databits) {
case 5:
shared_port_data.RS485MOD = CS5;
break;
case 6:
shared_port_data.RS485MOD = CS6;
break;
case 7:
shared_port_data.RS485MOD = CS7;
break;
default:
shared_port_data.RS485MOD = CS8;
break;
}
switch(port->params.mstp_params.parity){
case PARITY_EVEN:
shared_port_data.RS485MOD |= PARENB;
break;
case PARITY_ODD:
shared_port_data.RS485MOD |= PARENB | PARODD;
break;
default:
break;
}
switch (port->params.mstp_params.parity) {
case PARITY_EVEN:
shared_port_data.RS485MOD |= PARENB;
break;
case PARITY_ODD:
shared_port_data.RS485MOD |= PARENB | PARODD;
break;
default:
break;
}
if(port->params.mstp_params.stopbits == 2)
shared_port_data.RS485MOD |= CSTOPB;
if (port->params.mstp_params.stopbits == 2)
shared_port_data.RS485MOD |= CSTOPB;
mstp_port.UserData = (void*) &shared_port_data;
dlmstp_set_baud_rate(&mstp_port, port->params.mstp_params.baudrate);
dlmstp_set_mac_address(&mstp_port, port->route_info.mac[0]);
dlmstp_set_max_info_frames(&mstp_port, port->params.mstp_params.max_frames);
dlmstp_set_max_master(&mstp_port, port->params.mstp_params.max_master);
if(!dlmstp_init(&mstp_port, port->iface))
printf("MSTP %s init failed. Stop.\n", port->iface);
mstp_port.UserData = (void *) &shared_port_data;
dlmstp_set_baud_rate(&mstp_port, port->params.mstp_params.baudrate);
dlmstp_set_mac_address(&mstp_port, port->route_info.mac[0]);
dlmstp_set_max_info_frames(&mstp_port,
port->params.mstp_params.max_frames);
dlmstp_set_max_master(&mstp_port, port->params.mstp_params.max_master);
if (!dlmstp_init(&mstp_port, port->iface))
printf("MSTP %s init failed. Stop.\n", port->iface);
port->port_id = create_msgbox();
if (port->port_id == INVALID_MSGBOX_ID) {
port->state = INIT_FAILED;
return NULL;
}
port->port_id = create_msgbox();
if (port->port_id == INVALID_MSGBOX_ID) {
port->state = INIT_FAILED;
return NULL;
}
port->state = RUNNING;
port->state = RUNNING;
while (!shutdown) {
/* message loop */
BACMSG msg_storage, * bacmsg;
MSG_DATA * msg_data;
while (!shutdown) {
/* message loop */
BACMSG msg_storage, *bacmsg;
MSG_DATA *msg_data;
bacmsg = recv_from_msgbox(port->port_id, &msg_storage);
bacmsg = recv_from_msgbox(port->port_id, &msg_storage);
if(bacmsg)
{
switch(bacmsg->type)
{
case DATA:
msg_data = (MSG_DATA*) bacmsg->data;
if (bacmsg) {
switch (bacmsg->type) {
case DATA:
msg_data = (MSG_DATA *) bacmsg->data;
if(msg_data->dest.net == BACNET_BROADCAST_NETWORK)
{
dlmstp_get_broadcast_address(&(msg_data->dest));
}
else
{
msg_data->dest.mac[0] = msg_data->dest.adr[0];
msg_data->dest.mac_len = 1;
}
if (msg_data->dest.net == BACNET_BROADCAST_NETWORK) {
dlmstp_get_broadcast_address(&(msg_data->dest));
} else {
msg_data->dest.mac[0] = msg_data->dest.adr[0];
msg_data->dest.mac_len = 1;
}
dlmstp_send_pdu(&mstp_port, &(msg_data->dest), msg_data->pdu, msg_data->pdu_len);
dlmstp_send_pdu(&mstp_port, &(msg_data->dest),
msg_data->pdu, msg_data->pdu_len);
check_data(msg_data);
check_data(msg_data);
break;
case SERVICE:
switch(bacmsg->subtype) {
case SHUTDOWN:
shutdown = 1;
break;
default:
break;
}
break;
default:
continue;
break;
}
}
else
{
pdu_len = dlmstp_receive(&mstp_port, NULL, NULL, 0, 1000);
break;
case SERVICE:
switch (bacmsg->subtype) {
case SHUTDOWN:
shutdown = 1;
break;
default:
break;
}
break;
default:
continue;
break;
}
} else {
pdu_len = dlmstp_receive(&mstp_port, NULL, NULL, 0, 1000);
if(pdu_len > 0)
{
msg_data = (MSG_DATA*)malloc(sizeof(MSG_DATA));
memmove(&(msg_data->src), (const void*)&(shared_port_data.Receive_Packet.address), sizeof(shared_port_data.Receive_Packet.address));
msg_data->src.adr[0] = msg_data->src.mac[0];
msg_data->src.len = 1;
msg_data->pdu = (uint8_t*)malloc(pdu_len);
memmove(msg_data->pdu, (const void*)&(shared_port_data.Receive_Packet.pdu), pdu_len);
msg_data->pdu_len = pdu_len;
if (pdu_len > 0) {
msg_data = (MSG_DATA *) malloc(sizeof(MSG_DATA));
memmove(&(msg_data->src),
(const void *) &(shared_port_data.Receive_Packet.address),
sizeof(shared_port_data.Receive_Packet.address));
msg_data->src.adr[0] = msg_data->src.mac[0];
msg_data->src.len = 1;
msg_data->pdu = (uint8_t *) malloc(pdu_len);
memmove(msg_data->pdu,
(const void *) &(shared_port_data.Receive_Packet.pdu),
pdu_len);
msg_data->pdu_len = pdu_len;
msg_storage.type = DATA;
msg_storage.subtype = (MSGSUBTYPE)0;
msg_storage.origin = port->port_id;
msg_storage.data = msg_data;
msg_storage.type = DATA;
msg_storage.subtype = (MSGSUBTYPE) 0;
msg_storage.origin = port->port_id;
msg_storage.data = msg_data;
if(!send_to_msgbox(port->main_id, &msg_storage))
{
free_data(msg_data);
}
}
}
}
if (!send_to_msgbox(port->main_id, &msg_storage)) {
free_data(msg_data);
}
}
}
}
dlmstp_cleanup(&mstp_port);
port->state = FINISHED;
dlmstp_cleanup(&mstp_port);
port->state = FINISHED;
return NULL;
return NULL;
}
bacnet-stack/demo/router/mstpmodule.h
+2 -2
View File
@@ -19,7 +19,7 @@ Copyright (C) 2012 Andriy Sukhynyuk, Vasyl Tkhir, Andriy Ivasiv
#include "portthread.h"
void* dl_mstp_thread(
void* pArgs);
void *dl_mstp_thread(
void *pArgs);
#endif /* end of MSTPMODULE_H */
bacnet-stack/demo/router/network_layer.c
+263 -247
View File
@@ -21,291 +21,307 @@ Copyright (C) 2012 Andriy Sukhynyuk, Vasyl Tkhir, Andriy Ivasiv
#include "bacint.h"
uint16_t process_network_message(
BACMSG *msg,
MSG_DATA *data,
uint8_t **buff) {
BACMSG * msg,
MSG_DATA * data,
uint8_t ** buff)
{
BACNET_NPDU_DATA npdu_data;
ROUTER_PORT *srcport;
ROUTER_PORT *destport;
uint16_t net;
uint8_t error_code;
int16_t buff_len = 0;
int apdu_offset;
int apdu_len;
BACNET_NPDU_DATA npdu_data;
ROUTER_PORT *srcport;
ROUTER_PORT *destport;
uint16_t net;
uint8_t error_code;
int16_t buff_len = 0;
int apdu_offset;
int apdu_len;
memmove(data, msg->data, sizeof(MSG_DATA));
memmove(data, msg->data, sizeof(MSG_DATA));
apdu_offset = npdu_decode(data->pdu, &data->dest, NULL, &npdu_data);
apdu_len = data->pdu_len - apdu_offset;
apdu_offset = npdu_decode(data->pdu, &data->dest, NULL, &npdu_data);
apdu_len = data->pdu_len - apdu_offset;
srcport = find_snet(msg->origin);
data->src.net = srcport->route_info.net;
srcport = find_snet(msg->origin);
data->src.net = srcport->route_info.net;
switch (npdu_data.network_message_type) {
switch (npdu_data.network_message_type) {
case NETWORK_MESSAGE_WHO_IS_ROUTER_TO_NETWORK:
PRINT(INFO, "Recieved Who-Is-Router-To-Network message\n");
if (apdu_len) {
/* if NET specified */
decode_unsigned16(&data->pdu[apdu_offset], &net);
if (srcport->route_info.net == net) {
PRINT(INFO, "Message discarded: NET directly connected\n");
return -2;
}
case NETWORK_MESSAGE_WHO_IS_ROUTER_TO_NETWORK:
PRINT(INFO, "Recieved Who-Is-Router-To-Network message\n");
if (apdu_len) {
/* if NET specified */
decode_unsigned16(&data->pdu[apdu_offset], &net);
if (srcport->route_info.net == net) {
PRINT(INFO, "Message discarded: NET directly connected\n");
return -2;
}
destport = find_dnet(net, NULL); /* see if NET can be reached */
if (destport) {
/* if TRUE send reply */
PRINT(INFO, "Sending I-Am-Router-To-Network message\n");
buff_len = create_network_message(NETWORK_MESSAGE_I_AM_ROUTER_TO_NETWORK,
data, buff, &net);
} else {
data->dest.net = net; /* NET to look for */
return -1; /* else initiate NET search procedure */
}
} else {
/* if NET is omitted (message sent with -1) */
PRINT(INFO, "Sending I-Am-Router-To-Network message\n");
buff_len = create_network_message(NETWORK_MESSAGE_I_AM_ROUTER_TO_NETWORK,
data, buff, NULL);
}
destport = find_dnet(net, NULL); /* see if NET can be reached */
if (destport) {
/* if TRUE send reply */
PRINT(INFO, "Sending I-Am-Router-To-Network message\n");
buff_len =
create_network_message
(NETWORK_MESSAGE_I_AM_ROUTER_TO_NETWORK, data, buff,
&net);
} else {
data->dest.net = net; /* NET to look for */
return -1; /* else initiate NET search procedure */
}
} else {
/* if NET is omitted (message sent with -1) */
PRINT(INFO, "Sending I-Am-Router-To-Network message\n");
buff_len =
create_network_message
(NETWORK_MESSAGE_I_AM_ROUTER_TO_NETWORK, data, buff, NULL);
}
break;
break;
case NETWORK_MESSAGE_I_AM_ROUTER_TO_NETWORK:
{
PRINT(INFO, "Recieved I-Am-Router-To-Network message\n");
int net_count = apdu_len / 2;
int i;
for (i = 0; i < net_count; i++) {
decode_unsigned16(&data->pdu[apdu_offset+2*i], &net); /* decode received NET values */
add_dnet(&srcport->route_info, net, data->src); /* and update routing table */
}
break;
}
case NETWORK_MESSAGE_REJECT_MESSAGE_TO_NETWORK:
{
/* first octet of the message contains rejection reason */
/* next two octets contain NET (can be decoded for additional info on error) */
error_code = data->pdu[apdu_offset];
switch (error_code) {
case 0:
PRINT(ERROR, "Error!\n");
break;
case 1:
PRINT(ERROR, "Error: Network unreachable\n");
break;
case 2:
PRINT(ERROR, "Error: Network is busy\n");
break;
case 3:
PRINT(ERROR, "Error: Unknown network message type\n");
break;
case 4:
PRINT(ERROR, "Error: Message too long\n");
break;
}
break;
}
case NETWORK_MESSAGE_INIT_RT_TABLE:
PRINT(INFO, "Recieved Initialize-Routing-Table message\n");
if (data->pdu[apdu_offset] > 0) {
int net_count = data->pdu[apdu_offset];
while (net_count--) {
int i = 1;
decode_unsigned16(&data->pdu[apdu_offset+i], &net); /* decode received NET values */
add_dnet(&srcport->route_info, net, data->src); /* and update routing table */
if (data->pdu[apdu_offset+i+3] > 0) /* find next NET value */
i = data->pdu[apdu_offset+i+3] + 4;
else
i = i + 4;
}
buff_len = create_network_message(NETWORK_MESSAGE_INIT_RT_TABLE_ACK, data, buff, NULL);
} else
buff_len = create_network_message(NETWORK_MESSAGE_INIT_RT_TABLE_ACK, data, buff, &buff);
break;
case NETWORK_MESSAGE_I_AM_ROUTER_TO_NETWORK:
{
PRINT(INFO, "Recieved I-Am-Router-To-Network message\n");
int net_count = apdu_len / 2;
int i;
for (i = 0; i < net_count; i++) {
decode_unsigned16(&data->pdu[apdu_offset + 2 * i], &net); /* decode received NET values */
add_dnet(&srcport->route_info, net, data->src); /* and update routing table */
}
break;
}
case NETWORK_MESSAGE_REJECT_MESSAGE_TO_NETWORK:
{
/* first octet of the message contains rejection reason */
/* next two octets contain NET (can be decoded for additional info on error) */
error_code = data->pdu[apdu_offset];
switch (error_code) {
case 0:
PRINT(ERROR, "Error!\n");
break;
case 1:
PRINT(ERROR, "Error: Network unreachable\n");
break;
case 2:
PRINT(ERROR, "Error: Network is busy\n");
break;
case 3:
PRINT(ERROR, "Error: Unknown network message type\n");
break;
case 4:
PRINT(ERROR, "Error: Message too long\n");
break;
}
break;
}
case NETWORK_MESSAGE_INIT_RT_TABLE:
PRINT(INFO, "Recieved Initialize-Routing-Table message\n");
if (data->pdu[apdu_offset] > 0) {
int net_count = data->pdu[apdu_offset];
while (net_count--) {
int i = 1;
decode_unsigned16(&data->pdu[apdu_offset + i], &net); /* decode received NET values */
add_dnet(&srcport->route_info, net, data->src); /* and update routing table */
if (data->pdu[apdu_offset + i + 3] > 0) /* find next NET value */
i = data->pdu[apdu_offset + i + 3] + 4;
else
i = i + 4;
}
buff_len =
create_network_message(NETWORK_MESSAGE_INIT_RT_TABLE_ACK,
data, buff, NULL);
} else
buff_len =
create_network_message(NETWORK_MESSAGE_INIT_RT_TABLE_ACK,
data, buff, &buff);
break;
case NETWORK_MESSAGE_INIT_RT_TABLE_ACK:
PRINT(INFO, "Recieved Initialize-Routing-Table-Ack message\n");
if (data->pdu[apdu_offset] > 0) {
int net_count = data->pdu[apdu_offset];
while (net_count--) {
int i = 1;
decode_unsigned16(&data->pdu[apdu_offset+i], &net); /* decode received NET values */
add_dnet(&srcport->route_info, net, data->src); /* and update routing table */
if (data->pdu[apdu_offset+i+3] > 0) /* find next NET value */
i = data->pdu[apdu_offset+i+3] + 4;
else
i = i + 4;
}
}
break;
case NETWORK_MESSAGE_INIT_RT_TABLE_ACK:
PRINT(INFO, "Recieved Initialize-Routing-Table-Ack message\n");
if (data->pdu[apdu_offset] > 0) {
int net_count = data->pdu[apdu_offset];
while (net_count--) {
int i = 1;
decode_unsigned16(&data->pdu[apdu_offset + i], &net); /* decode received NET values */
add_dnet(&srcport->route_info, net, data->src); /* and update routing table */
if (data->pdu[apdu_offset + i + 3] > 0) /* find next NET value */
i = data->pdu[apdu_offset + i + 3] + 4;
else
i = i + 4;
}
}
break;
case NETWORK_MESSAGE_INVALID:
case NETWORK_MESSAGE_I_COULD_BE_ROUTER_TO_NETWORK:
case NETWORK_MESSAGE_ROUTER_BUSY_TO_NETWORK:
case NETWORK_MESSAGE_ROUTER_AVAILABLE_TO_NETWORK:
case NETWORK_MESSAGE_ESTABLISH_CONNECTION_TO_NETWORK:
case NETWORK_MESSAGE_DISCONNECT_CONNECTION_TO_NETWORK:
/* hell if I know what to do with these messages */
break;
case NETWORK_MESSAGE_INVALID:
case NETWORK_MESSAGE_I_COULD_BE_ROUTER_TO_NETWORK:
case NETWORK_MESSAGE_ROUTER_BUSY_TO_NETWORK:
case NETWORK_MESSAGE_ROUTER_AVAILABLE_TO_NETWORK:
case NETWORK_MESSAGE_ESTABLISH_CONNECTION_TO_NETWORK:
case NETWORK_MESSAGE_DISCONNECT_CONNECTION_TO_NETWORK:
/* hell if I know what to do with these messages */
break;
default:
PRINT(ERROR, "Error: Message unsupported\n");
break;
}
default:
PRINT(ERROR, "Error: Message unsupported\n");
break;
}
return buff_len;
return buff_len;
}
uint16_t create_network_message(
BACNET_NETWORK_MESSAGE_TYPE network_message_type,
MSG_DATA *data,
uint8_t **buff,
void *val) {
BACNET_NETWORK_MESSAGE_TYPE network_message_type,
MSG_DATA * data,
uint8_t ** buff,
void *val)
{
int16_t buff_len;
bool data_expecting_reply = false;
BACNET_NPDU_DATA npdu_data;
int16_t buff_len;
bool data_expecting_reply = false;
BACNET_NPDU_DATA npdu_data;
if (network_message_type == NETWORK_MESSAGE_INIT_RT_TABLE)
data_expecting_reply = true;
init_npdu(&npdu_data, network_message_type, data_expecting_reply);
if (network_message_type == NETWORK_MESSAGE_INIT_RT_TABLE)
data_expecting_reply = true;
init_npdu(&npdu_data, network_message_type, data_expecting_reply);
*buff = (uint8_t*)malloc(128); /* resolve different length */
*buff = (uint8_t *) malloc(128); /* resolve different length */
/* manual destination setup for Init-RT-Table-Ack message */
data->dest.net = BACNET_BROADCAST_NETWORK;
buff_len = npdu_encode_pdu(*buff, &data->dest, NULL, &npdu_data);
/* manual destination setup for Init-RT-Table-Ack message */
data->dest.net = BACNET_BROADCAST_NETWORK;
buff_len = npdu_encode_pdu(*buff, &data->dest, NULL, &npdu_data);
switch (network_message_type) {
switch (network_message_type) {
case NETWORK_MESSAGE_WHO_IS_ROUTER_TO_NETWORK:
if (val != NULL) {
uint8_t * valptr = (uint8_t*)val;
uint16_t val16 = (valptr[0]) + (valptr[1] << 8);
buff_len += encode_unsigned16(*buff+buff_len, val16);
}
break;
case NETWORK_MESSAGE_WHO_IS_ROUTER_TO_NETWORK:
if (val != NULL) {
uint8_t *valptr = (uint8_t *) val;
uint16_t val16 = (valptr[0]) + (valptr[1] << 8);
buff_len += encode_unsigned16(*buff + buff_len, val16);
}
break;
case NETWORK_MESSAGE_I_AM_ROUTER_TO_NETWORK:
if (val != NULL) {
uint8_t * valptr = (uint8_t*)val;
uint16_t val16 = (valptr[0]) + (valptr[1] << 8);
buff_len += encode_unsigned16(*buff+buff_len, val16);
}
else {
ROUTER_PORT *port = head;
DNET *dnet;
while (port != NULL) {
if (port->route_info.net != data->src.net) {
buff_len += encode_unsigned16(*buff+buff_len, port->route_info.net);
dnet = port->route_info.dnets;
while (dnet != NULL) {
buff_len += encode_unsigned16(*buff+buff_len, dnet->net);
dnet = dnet->next;
}
port = port->next;
} else {
dnet = port->route_info.dnets;
while (dnet != NULL) {
buff_len += encode_unsigned16(*buff+buff_len, dnet->net);
dnet = dnet->next;
}
port = port->next;
}
}
}
break;
case NETWORK_MESSAGE_I_AM_ROUTER_TO_NETWORK:
if (val != NULL) {
uint8_t *valptr = (uint8_t *) val;
uint16_t val16 = (valptr[0]) + (valptr[1] << 8);
buff_len += encode_unsigned16(*buff + buff_len, val16);
} else {
ROUTER_PORT *port = head;
DNET *dnet;
while (port != NULL) {
if (port->route_info.net != data->src.net) {
buff_len +=
encode_unsigned16(*buff + buff_len,
port->route_info.net);
dnet = port->route_info.dnets;
while (dnet != NULL) {
buff_len +=
encode_unsigned16(*buff + buff_len, dnet->net);
dnet = dnet->next;
}
port = port->next;
} else {
dnet = port->route_info.dnets;
while (dnet != NULL) {
buff_len +=
encode_unsigned16(*buff + buff_len, dnet->net);
dnet = dnet->next;
}
port = port->next;
}
}
}
break;
case NETWORK_MESSAGE_REJECT_MESSAGE_TO_NETWORK:
{
uint8_t * valptr = (uint8_t*)val;
uint16_t val16 = (valptr[0]) + (valptr[1] << 8);
buff_len += encode_unsigned16(*buff + buff_len, val16);
break;
}
case NETWORK_MESSAGE_INIT_RT_TABLE:
case NETWORK_MESSAGE_INIT_RT_TABLE_ACK:
if ((uint8_t*)val) {
(*buff)[buff_len++] = (uint8_t) port_count;
case NETWORK_MESSAGE_REJECT_MESSAGE_TO_NETWORK:
{
uint8_t *valptr = (uint8_t *) val;
uint16_t val16 = (valptr[0]) + (valptr[1] << 8);
buff_len += encode_unsigned16(*buff + buff_len, val16);
break;
}
case NETWORK_MESSAGE_INIT_RT_TABLE:
case NETWORK_MESSAGE_INIT_RT_TABLE_ACK:
if ((uint8_t *) val) {
(*buff)[buff_len++] = (uint8_t) port_count;
if (port_count > 0) {
ROUTER_PORT *port = head;
uint8_t portID = 1;
if (port_count > 0) {
ROUTER_PORT *port = head;
uint8_t portID = 1;
while (port != NULL) {
buff_len += encode_unsigned16(*buff+buff_len, port->route_info.net);
(*buff)[buff_len++] = portID++;
(*buff)[buff_len++] = 0;
port = port->next;
}
}
} else
(*buff)[buff_len++] = (uint8_t) 0;
break;
while (port != NULL) {
buff_len +=
encode_unsigned16(*buff + buff_len,
port->route_info.net);
(*buff)[buff_len++] = portID++;
(*buff)[buff_len++] = 0;
port = port->next;
}
}
} else
(*buff)[buff_len++] = (uint8_t) 0;
break;
case NETWORK_MESSAGE_INVALID:
case NETWORK_MESSAGE_I_COULD_BE_ROUTER_TO_NETWORK:
case NETWORK_MESSAGE_ROUTER_BUSY_TO_NETWORK:
case NETWORK_MESSAGE_ROUTER_AVAILABLE_TO_NETWORK:
case NETWORK_MESSAGE_ESTABLISH_CONNECTION_TO_NETWORK:
case NETWORK_MESSAGE_DISCONNECT_CONNECTION_TO_NETWORK:
/* hell if I know what to do with these messages */
break;
}
case NETWORK_MESSAGE_INVALID:
case NETWORK_MESSAGE_I_COULD_BE_ROUTER_TO_NETWORK:
case NETWORK_MESSAGE_ROUTER_BUSY_TO_NETWORK:
case NETWORK_MESSAGE_ROUTER_AVAILABLE_TO_NETWORK:
case NETWORK_MESSAGE_ESTABLISH_CONNECTION_TO_NETWORK:
case NETWORK_MESSAGE_DISCONNECT_CONNECTION_TO_NETWORK:
/* hell if I know what to do with these messages */
break;
}
return buff_len;
return buff_len;
}
void send_network_message(
BACNET_NETWORK_MESSAGE_TYPE network_message_type,
MSG_DATA *data,
uint8_t **buff,
void *val) {
BACNET_NETWORK_MESSAGE_TYPE network_message_type,
MSG_DATA * data,
uint8_t ** buff,
void *val)
{
BACMSG msg;
ROUTER_PORT *port = head;
int16_t buff_len;
BACMSG msg;
ROUTER_PORT *port = head;
int16_t buff_len;
if (!data) {
data = (MSG_DATA*)malloc(sizeof(MSG_DATA));
data->dest.net = BACNET_BROADCAST_NETWORK;
}
if (!data) {
data = (MSG_DATA *) malloc(sizeof(MSG_DATA));
data->dest.net = BACNET_BROADCAST_NETWORK;
}
buff_len = create_network_message(network_message_type, data, buff, val);
buff_len = create_network_message(network_message_type, data, buff, val);
/* form network message */
data->pdu = *buff;
data->pdu_len = buff_len;
msg.origin = head->main_id;
msg.type = DATA;
msg.data = data;
/* form network message */
data->pdu = *buff;
data->pdu_len = buff_len;
msg.origin = head->main_id;
msg.type = DATA;
msg.data = data;
data->ref_count = port_count;
while (port != NULL) {
if (port->state == FINISHED) {
port = port->next;
continue;
}
send_to_msgbox(port->port_id, &msg);
port = port->next;
}
data->ref_count = port_count;
while (port != NULL) {
if (port->state == FINISHED) {
port = port->next;
continue;
}
send_to_msgbox(port->port_id, &msg);
port = port->next;
}
}
void init_npdu(
BACNET_NPDU_DATA *npdu_data,
BACNET_NETWORK_MESSAGE_TYPE network_message_type,
bool data_expecting_reply) {
BACNET_NPDU_DATA * npdu_data,
BACNET_NETWORK_MESSAGE_TYPE network_message_type,
bool data_expecting_reply)
{
if (npdu_data) {
npdu_data->data_expecting_reply = data_expecting_reply;
npdu_data->protocol_version = BACNET_PROTOCOL_VERSION;
npdu_data->network_layer_message = true;
npdu_data->network_message_type = network_message_type;
npdu_data->vendor_id = 0;
npdu_data->priority = MESSAGE_PRIORITY_NORMAL;
npdu_data->hop_count = HOP_COUNT_DEFAULT;
}
if (npdu_data) {
npdu_data->data_expecting_reply = data_expecting_reply;
npdu_data->protocol_version = BACNET_PROTOCOL_VERSION;
npdu_data->network_layer_message = true;
npdu_data->network_message_type = network_message_type;
npdu_data->vendor_id = 0;
npdu_data->priority = MESSAGE_PRIORITY_NORMAL;
npdu_data->hop_count = HOP_COUNT_DEFAULT;
}
}
bacnet-stack/demo/router/network_layer.h
+14 -14
View File
@@ -29,25 +29,25 @@ Copyright (C) 2012 Andriy Sukhynyuk, Vasyl Tkhir, Andriy Ivasiv
#include "portthread.h"
uint16_t process_network_message(
BACMSG *msg,
MSG_DATA *data,
uint8_t **buff);
BACMSG * msg,
MSG_DATA * data,
uint8_t ** buff);
uint16_t create_network_message(
BACNET_NETWORK_MESSAGE_TYPE network_message_type,
MSG_DATA *data,
uint8_t **buff,
void *val);
BACNET_NETWORK_MESSAGE_TYPE network_message_type,
MSG_DATA * data,
uint8_t ** buff,
void *val);
void send_network_message(
BACNET_NETWORK_MESSAGE_TYPE network_message_type,
MSG_DATA *data,
uint8_t **buff,
void *val);
BACNET_NETWORK_MESSAGE_TYPE network_message_type,
MSG_DATA * data,
uint8_t ** buff,
void *val);
void init_npdu(
BACNET_NPDU_DATA *npdu_data,
BACNET_NETWORK_MESSAGE_TYPE network_message_type,
bool data_expecting_reply);
BACNET_NPDU_DATA * npdu_data,
BACNET_NETWORK_MESSAGE_TYPE network_message_type,
bool data_expecting_reply);
#endif /* end of NETWORK_LAYER_H */
bacnet-stack/demo/router/portthread.c
+76 -72
View File
@@ -19,98 +19,102 @@ Copyright (C) 2012 Andriy Sukhynyuk, Vasyl Tkhir, Andriy Ivasiv
#include <string.h>
#include "portthread.h"
ROUTER_PORT* find_snet(
MSGBOX_ID id) {
ROUTER_PORT *find_snet(
MSGBOX_ID id)
{
ROUTER_PORT *port = head;
ROUTER_PORT *port = head;
while (port != NULL) {
if (port->port_id == id)
return port;
port = port->next;
}
while (port != NULL) {
if (port->port_id == id)
return port;
port = port->next;
}
return NULL;
return NULL;
}
ROUTER_PORT* find_dnet(
uint16_t net,
BACNET_ADDRESS *addr) {
ROUTER_PORT *find_dnet(
uint16_t net,
BACNET_ADDRESS * addr)
{
ROUTER_PORT *port = head;
DNET *dnet;
ROUTER_PORT *port = head;
DNET *dnet;
/* for broadcast messages no search is needed */
if (net == BACNET_BROADCAST_NETWORK)
return port;
/* for broadcast messages no search is needed */
if (net == BACNET_BROADCAST_NETWORK)
return port;
while(port != NULL) {
while (port != NULL) {
/* check if DNET is directly connected to the router */
if (net == port->route_info.net)
return port;
/* check if DNET is directly connected to the router */
if (net == port->route_info.net)
return port;
/* else search router ports DNET list */
else if (port->route_info.dnets) {
dnet = port->route_info.dnets;
while (dnet != NULL) {
if (net == dnet->net) {
if (addr) {
memmove(&addr->len, &dnet->mac_len, 1);
memmove(&addr->adr[0], &dnet->mac[0], MAX_MAC_LEN);
}
return port;
}
dnet = dnet->next;
}
}
port = port->next;
}
/* else search router ports DNET list */
else if (port->route_info.dnets) {
dnet = port->route_info.dnets;
while (dnet != NULL) {
if (net == dnet->net) {
if (addr) {
memmove(&addr->len, &dnet->mac_len, 1);
memmove(&addr->adr[0], &dnet->mac[0], MAX_MAC_LEN);
}
return port;
}
dnet = dnet->next;
}
}
port = port->next;
}
return NULL;
return NULL;
}
void add_dnet(
RT_ENTRY *route_info,
uint16_t net,
BACNET_ADDRESS addr) {
RT_ENTRY * route_info,
uint16_t net,
BACNET_ADDRESS addr)
{
DNET *dnet = route_info->dnets;
DNET *tmp;
DNET *dnet = route_info->dnets;
DNET *tmp;
if (dnet == NULL) {
route_info->dnets = (DNET*)malloc(sizeof(DNET));
memmove(&route_info->dnets->mac_len, &addr.len, 1);
memmove(&route_info->dnets->mac[0], &addr.adr[0], MAX_MAC_LEN);
route_info->dnets->net = net;
route_info->dnets->state = true;
route_info->dnets->next = NULL;
} else {
if (dnet == NULL) {
route_info->dnets = (DNET *) malloc(sizeof(DNET));
memmove(&route_info->dnets->mac_len, &addr.len, 1);
memmove(&route_info->dnets->mac[0], &addr.adr[0], MAX_MAC_LEN);
route_info->dnets->net = net;
route_info->dnets->state = true;
route_info->dnets->next = NULL;
} else {
while (dnet != NULL) {
if (dnet->net == net) /* make sure NETs are not repeated */
return;
tmp = dnet;
dnet = dnet->next;
}
while (dnet != NULL) {
if (dnet->net == net) /* make sure NETs are not repeated */
return;
tmp = dnet;
dnet = dnet->next;
}
dnet = (DNET*)malloc(sizeof(DNET));
memmove(&dnet->mac_len, &addr.len, 1);
memmove(&dnet->mac[0], &addr.adr[0], MAX_MAC_LEN);
dnet->net = net;
dnet->state = true;
dnet->next = NULL;
tmp->next = dnet;
}
dnet = (DNET *) malloc(sizeof(DNET));
memmove(&dnet->mac_len, &addr.len, 1);
memmove(&dnet->mac[0], &addr.adr[0], MAX_MAC_LEN);
dnet->net = net;
dnet->state = true;
dnet->next = NULL;
tmp->next = dnet;
}
}
void cleanup_dnets(
DNET *dnets) {
DNET * dnets)
{
DNET *dnet = dnets;
while(dnet != NULL) {
dnet = dnet->next;
free(dnets);
dnets = dnet;
}
DNET *dnet = dnets;
while (dnet != NULL) {
dnet = dnet->next;
free(dnets);
dnets = dnet;
}
}
bacnet-stack/demo/router/portthread.h
+51 -50
View File
@@ -36,90 +36,91 @@ Copyright (C) 2012 Andriy Sukhynyuk, Vasyl Tkhir, Andriy Ivasiv
#endif
typedef enum {
BIP = 1,
MSTP = 2
BIP = 1,
MSTP = 2
} DL_TYPE;
typedef enum {
INIT,
INIT_FAILED,
RUNNING,
FINISHED
INIT,
INIT_FAILED,
RUNNING,
FINISHED
} PORT_STATE;
/* router port thread function */
typedef void* (*PORT_FUNC)(
void*);
typedef void *(
*PORT_FUNC) (
void *);
typedef enum {
PARITY_NONE,
PARITY_EVEN,
PARITY_ODD
}PARITY;
PARITY_NONE,
PARITY_EVEN,
PARITY_ODD
} PARITY;
/* port specific parameters */
typedef union _port_params {
struct {
uint16_t port;
} bip_params;
struct {
uint32_t baudrate;
PARITY parity;
uint8_t databits;
uint8_t stopbits;
uint8_t max_master;
uint8_t max_frames;
} mstp_params;
struct {
uint16_t port;
} bip_params;
struct {
uint32_t baudrate;
PARITY parity;
uint8_t databits;
uint8_t stopbits;
uint8_t max_master;
uint8_t max_frames;
} mstp_params;
} PORT_PARAMS;
/* list node for reacheble networks */
typedef struct _dnet {
uint8_t mac[MAX_MAC_LEN];
uint8_t mac_len;
uint16_t net;
bool state; /* enabled or disabled */
struct _dnet *next;
uint8_t mac[MAX_MAC_LEN];
uint8_t mac_len;
uint16_t net;
bool state; /* enabled or disabled */
struct _dnet *next;
} DNET;
/* information for routing table */
typedef struct _routing_table_entry {
uint8_t mac[MAX_MAC_LEN];
uint8_t mac_len;
uint16_t net;
DNET *dnets;
uint8_t mac[MAX_MAC_LEN];
uint8_t mac_len;
uint16_t net;
DNET *dnets;
} RT_ENTRY;
typedef struct _port {
DL_TYPE type;
PORT_STATE state;
MSGBOX_ID main_id; /* same for every router port */
MSGBOX_ID port_id; /* different for every router port */
char *iface;
PORT_FUNC func;
RT_ENTRY route_info;
PORT_PARAMS params;
struct _port *next; /* pointer to next list node */
DL_TYPE type;
PORT_STATE state;
MSGBOX_ID main_id; /* same for every router port */
MSGBOX_ID port_id; /* different for every router port */
char *iface;
PORT_FUNC func;
RT_ENTRY route_info;
PORT_PARAMS params;
struct _port *next; /* pointer to next list node */
} ROUTER_PORT;
extern ROUTER_PORT *head;
extern int port_count;
/* get recieving router port */
ROUTER_PORT* find_snet(
MSGBOX_ID id);
ROUTER_PORT *find_snet(
MSGBOX_ID id);
/* get sending router port */
ROUTER_PORT* find_dnet(
uint16_t net,
BACNET_ADDRESS *addr);
ROUTER_PORT *find_dnet(
uint16_t net,
BACNET_ADDRESS * addr);
/* add reacheble network for specified router port */
void add_dnet(
RT_ENTRY *route_info,
uint16_t net,
BACNET_ADDRESS addr);
RT_ENTRY * route_info,
uint16_t net,
BACNET_ADDRESS addr);
void cleanup_dnets(
DNET *dnets);
DNET * dnets);
#endif /* end of PORTTHREAD_H */