static int bvlc_encode_bvlc_result(uint8_t * pdu,
BACNET_BVLC_RESULT result_code)
{
if (pdu) {
pdu[0] = BVLL_TYPE_BACNET_IP;
pdu[1] = BVLC_RESULT;
/* 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], 6);
encode_unsigned16(&pdu[4], (uint16_t) result_code);
}
return 6;
}
/** Function to send a packet out the BACnet/IP socket (Annex J).
* @ingroup DLBIP
*
* @param dest [in] Destination address (may encode an IP address and port #).
* @param npdu_data [in] The NPDU header (Network) information (not used).
* @param pdu [in] Buffer of data to be sent - may be null (why?).
* @param pdu_len [in] Number of bytes in the pdu buffer.
* @return Number of bytes sent on success, negative number on failure.
*/
int bip_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 */
struct sockaddr_in bip_dest;
uint8_t mtu[MAX_MPDU] = { 0 };
int mtu_len = 0;
int bytes_sent = 0;
/* addr and port in host format */
struct in_addr address;
uint16_t port = 0;
(void) npdu_data;
/* assumes that the driver has already been initialized */
if (BIP_Socket < 0) {
return BIP_Socket;
}
mtu[0] = BVLL_TYPE_BACNET_IP;
bip_dest.sin_family = AF_INET;
if ((dest->net == BACNET_BROADCAST_NETWORK) || (dest->mac_len == 0)) {
/* broadcast */
address.s_addr = BIP_Broadcast_Address.s_addr;
port = BIP_Port;
mtu[1] = BVLC_ORIGINAL_BROADCAST_NPDU;
} else if ((dest->net > 0) && (dest->len == 0)) {
/* network specific broadcast */
if (dest->mac_len == 6) {
bip_decode_bip_address(dest, &address, &port);
} else {
address.s_addr = BIP_Broadcast_Address.s_addr;
port = BIP_Port;
}
mtu[1] = BVLC_ORIGINAL_BROADCAST_NPDU;
} else if (dest->mac_len == 6) {
bip_decode_bip_address(dest, &address, &port);
mtu[1] = BVLC_ORIGINAL_UNICAST_NPDU;
} else {
/* invalid address */
return -1;
}
bip_dest.sin_addr.s_addr = address.s_addr;
bip_dest.sin_port = port;
memset(&(bip_dest.sin_zero), '\0', 8);
mtu_len = 2;
mtu_len +=
encode_unsigned16(&mtu[mtu_len],
(uint16_t) (pdu_len + 4 /*inclusive */ ));
memcpy(&mtu[mtu_len], pdu, pdu_len);
mtu_len += pdu_len;
/* Send the packet */
bytes_sent =
sendto(BIP_Socket, (char *) mtu, mtu_len, 0,
(struct sockaddr *) &bip_dest, sizeof(struct sockaddr));
return bytes_sent;
}
/* returns number of bytes sent on success, negative on failure */
int ethernet_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 i = 0; /* counter */
int bytes = 0;
BACNET_ADDRESS src = { 0 }; /* source address for npdu */
uint8_t mtu[MAX_MPDU] = { 0 }; /* our buffer */
int mtu_len = 0;
(void) npdu_data;
/* load the BACnet address for NPDU data */
for (i = 0; i < 6; i++) {
src.mac[i] = Ethernet_MAC_Address[i];
src.mac_len++;
}
/* don't waste time if the socket is not valid */
if (!ethernet_valid()) {
return -1;
}
/* load destination ethernet MAC address */
if (dest->mac_len == 6) {
for (i = 0; i < 6; i++) {
mtu[i] = dest->mac[i];
}
} else {
return -2;
}
/* load source ethernet MAC address */
if (src.mac_len == 6) {
for (i = 0; i < 6; i++) {
mtu[6 + i] = src.mac[i];
}
} else {
return -3;
}
/* Logical PDU portion */
mtu[14] = 0x82; /* DSAP for BACnet */
mtu[15] = 0x82; /* SSAP for BACnet */
mtu[16] = 0x03; /* Control byte in header */
mtu_len = 17;
if ((mtu_len + pdu_len) > MAX_MPDU) {
return -4;
}
memcpy(&mtu[mtu_len], pdu, pdu_len);
mtu_len += pdu_len;
/* packet length - only the logical portion, not the address */
encode_unsigned16(&mtu[12], 3 + pdu_len);
/* Send the packet */
bytes = R_Ether_Write(0, mtu, mtu_len);
return bytes;
}
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 };
uint16_t max_data = 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) {
max_data = min(mstp_port->InputBufferSize, mstp_port->DataLength);
for (i = 0; i < max_data; i++) {
mtu[31 + i] = mstp_port->InputBuffer[i];
}
mtu[31 + max_data] = mstp_port->DataCRCActualMSB;
mtu[31 + max_data + 1] = mstp_port->DataCRCActualLSB;
mtu_len += (max_data + 2);
}
/* Ethernet length is data only - not address or length bytes */
encode_unsigned16(&mtu[12], mtu_len - 14);
write(sockfd, &mtu[0], mtu_len);
}
void Send_Initialize_Routing_Table(
BACNET_ADDRESS * dst,
BACNET_ROUTER_PORT * router_port_list)
{
int len = 0;
int pdu_len = 0;
int bytes_sent = 0;
BACNET_NPDU_DATA npdu_data;
uint8_t number_of_ports = 0;
BACNET_ROUTER_PORT *router_port;
uint8_t i = 0; /* counter */
npdu_encode_npdu_network(&npdu_data, NETWORK_MESSAGE_INIT_RT_TABLE, true,
MESSAGE_PRIORITY_NORMAL);
pdu_len =
npdu_encode_pdu(&Handler_Transmit_Buffer[0], NULL, NULL, &npdu_data);
/* encode the optional port_info list portion of the packet */
router_port = router_port_list;
while (router_port) {
number_of_ports++;
router_port = router_port->next;
}
Handler_Transmit_Buffer[pdu_len++] = number_of_ports;
if (number_of_ports) {
router_port = router_port_list;
while (router_port) {
len =
encode_unsigned16(&Handler_Transmit_Buffer[pdu_len],
router_port->dnet);
pdu_len += len;
Handler_Transmit_Buffer[pdu_len++] = router_port->id;
Handler_Transmit_Buffer[pdu_len++] = router_port->info_len;
for (i = 0; i < router_port->info_len; i++) {
Handler_Transmit_Buffer[pdu_len++] = router_port->info[i];
}
router_port = router_port->next;
}
}
#if PRINT_ENABLED
fprintf(stderr, "Send Initialize-Routing-Table message\n");
#endif
bytes_sent =
datalink_send_pdu(dst, &npdu_data, &Handler_Transmit_Buffer[0],
pdu_len);
#if PRINT_ENABLED
if (bytes_sent <= 0)
fprintf(stderr,
"Failed to send Initialize-Routing-Table message (%s)!\n",
strerror(errno));
#endif
}
/** Parse an ASCII string for a bacnet-address
*
* @param mac [out] BACNET_MAC_ADDRESS structure to store the results
* @param arg [in] nul terminated ASCII string to parse
*
* @return true if the address was parsed
*/
bool address_mac_from_ascii(
BACNET_MAC_ADDRESS *mac,
char *arg)
{
unsigned a[6] = {0}, p = 0;
uint16_t port = 0;
int c;
bool status = false;
if (!(mac && arg)) {
return false;
}
c = sscanf(arg, "%3u.%3u.%3u.%3u:%5u", &a[0],&a[1],&a[2],&a[3],&p);
if ((c == 4) || (c == 5)) {
mac->adr[0] = a[0];
mac->adr[1] = a[1];
mac->adr[2] = a[2];
mac->adr[3] = a[3];
if (c == 4) {
port = 0xBAC0;
} else {
port = (uint16_t)p;
}
encode_unsigned16(&mac->adr[4], port);
mac->len = 6;
status = true;
} else {
c = sscanf(arg, "%2x:%2x:%2x:%2x:%2x:%2x",
&a[0],&a[1],&a[2],&a[3],&a[4],&a[5]);
if (c == 6) {
mac->adr[0] = a[0];
mac->adr[1] = a[1];
mac->adr[2] = a[2];
mac->adr[3] = a[3];
mac->adr[4] = a[4];
mac->adr[5] = a[5];
mac->len = 6;
status = true;
} else if (c == 1) {
a[0] = (unsigned)strtol(arg, NULL, 0);
if (a[0] <= 255) {
mac->adr[0] = a[0];
mac->len = 1;
status = true;
}
}
}
return status;
}
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;
}
/* pDNET_list: list of networks for which I am a router,
terminated with -1 */
void Send_I_Am_Router_To_Network(
const int DNET_list[])
{
int len = 0;
int pdu_len = 0;
BACNET_ADDRESS dest;
int bytes_sent = 0;
BACNET_NPDU_DATA npdu_data;
uint16_t dnet = 0;
unsigned index = 0;
npdu_encode_npdu_network(&npdu_data,
NETWORK_MESSAGE_I_AM_ROUTER_TO_NETWORK, false,
MESSAGE_PRIORITY_NORMAL);
pdu_len =
npdu_encode_pdu(&Handler_Transmit_Buffer[0], NULL, NULL, &npdu_data);
/* encode the optional DNET list portion of the packet */
#if PRINT_ENABLED
fprintf(stderr, "Send I-Am-Router-To-Network message to:\n");
#endif
while (DNET_list[index] != -1) {
dnet = DNET_list[index];
len = encode_unsigned16(&Handler_Transmit_Buffer[pdu_len], dnet);
pdu_len += len;
index++;
#if PRINT_ENABLED
fprintf(stderr, "%u\n", dnet);
#endif
}
/* I-Am-Router-To-Network shall always be transmitted with
a broadcast MAC address. */
datalink_get_broadcast_address(&dest);
bytes_sent =
datalink_send_pdu(&dest, &npdu_data, &Handler_Transmit_Buffer[0],
pdu_len);
#if PRINT_ENABLED
if (bytes_sent <= 0)
fprintf(stderr,
"Failed to send I-Am-Router-To-Network message (%s)!\n",
strerror(errno));
#endif
}
static void address_parse(
BACNET_ADDRESS * dst,
int argc,
char *argv[])
{
unsigned mac[6];
unsigned port;
int count = 0;
int index = 0;
if (argc > 0) {
count =
sscanf(argv[0], "%u.%u.%u.%u:%u", &mac[0], &mac[1], &mac[2],
&mac[3], &port);
if (count == 5) {
dst->mac_len = 6;
for (index = 0; index < 4; index++) {
dst->mac[index] = mac[index];
}
encode_unsigned16(&dst->mac[4], port);
} else {
count =
sscanf(argv[0], "%x:%x:%x:%x:%x:%x", &mac[0], &mac[1], &mac[2],
&mac[3], &mac[4], &mac[5]);
dst->mac_len = count;
for (index = 0; index < MAX_MAC_LEN; index++) {
if (index < count) {
dst->mac[index] = mac[index];
} else {
dst->mac[index] = 0;
}
}
}
}
dst->net = 0;
dst->len = 0;
for (index = 0; index < MAX_MAC_LEN; index++) {
dst->adr[index] = 0;
}
}
/* find a specific router, or use -1 for limit if you want unlimited */
void Send_Who_Is_Router_To_Network(
BACNET_ADDRESS * dst,
int dnet)
{
int len = 0;
int pdu_len = 0;
int bytes_sent = 0;
BACNET_NPDU_DATA npdu_data;
npdu_encode_npdu_network(&npdu_data,
NETWORK_MESSAGE_WHO_IS_ROUTER_TO_NETWORK, false,
MESSAGE_PRIORITY_NORMAL);
/* fixme: should dnet/dlen/dadr be set in NPDU? */
pdu_len =
npdu_encode_pdu(&Handler_Transmit_Buffer[0], NULL, NULL, &npdu_data);
/* encode the optional DNET portion of the packet */
if (dnet >= 0) {
len = encode_unsigned16(&Handler_Transmit_Buffer[pdu_len], dnet);
pdu_len += len;
#if PRINT_ENABLED
fprintf(stderr, "Send Who-Is-Router-To-Network message to %u\n", dnet);
#endif
} else {
#if PRINT_ENABLED
fprintf(stderr, "Send Who-Is-Router-To-Network message\n");
#endif
}
bytes_sent =
datalink_send_pdu(dst, &npdu_data, &Handler_Transmit_Buffer[0],
pdu_len);
#if PRINT_ENABLED
if (bytes_sent <= 0)
fprintf(stderr,
"Failed to Send Who-Is-Router-To-Network Request (%s)!\n",
strerror(errno));
#endif
}
/** Encode the NPDU portion of a message to be sent, based on the npdu_data
* and associated data.
* If this is to be a Network Layer Control Message, there are probably
* more bytes which will need to be encoded following the ones encoded here.
* The Network Layer Protocol Control Information byte is described
* in section 6.2.2 of the BACnet standard.
* @param npdu [out] Buffer which will hold the encoded NPDU header bytes.
* The size isn't given, but it must be at least 2 bytes
* for the simplest case, and should always be at least 24
* bytes to accommodate the maximal case (all fields loaded).
* @param dest [in] The routing destination information if the message must
* be routed to reach its destination.
* If dest->net and dest->len are 0, there is no
* routing destination information.
* @param src [in] The routing source information if the message was routed
* from another BACnet network.
* If src->net and src->len are 0, 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.
* @param npdu_data [in] The structure which describes how the NCPI and other
* NPDU bytes should be encoded.
* @return On success, returns the number of bytes which were encoded into the
* NPDU section.
* If 0 or negative, there were problems with the data or encoding.
*/
int npdu_encode_pdu(
uint8_t * npdu,
BACNET_ADDRESS * dest,
BACNET_ADDRESS * src,
BACNET_NPDU_DATA * npdu_data)
{
int len = 1; /* return value - number of octets loaded in this function */
uint8_t i = 0; /* counter */
#if 1
if (npdu && npdu_data) {
/* protocol version */
npdu[0] = npdu_data->protocol_version;
/* initialize the control octet */
npdu[1] = 0;
/* Bit 7: 1 indicates that the NSDU conveys a network layer message. */
/* Message Type field is present. */
/* 0 indicates that the NSDU contains a BACnet APDU. */
/* Message Type field is absent. */
if (npdu_data->network_layer_message)
npdu[1] |= BIT7;
/*Bit 6: Reserved. Shall be zero. */
/*Bit 5: Destination specifier where: */
/* 0 = DNET, DLEN, DADR, and Hop Count absent */
/* 1 = DNET, DLEN, and Hop Count present */
/* DLEN = 0 denotes broadcast MAC DADR and DADR field is absent */
/* DLEN > 0 specifies length of DADR field */
if (dest && dest->net)
npdu[1] |= BIT5;
/* Bit 4: Reserved. Shall be zero. */
/* Bit 3: Source specifier where: */
/* 0 = SNET, SLEN, and SADR absent */
/* 1 = SNET, SLEN, and SADR present */
/* SLEN = 0 Invalid */
/* SLEN > 0 specifies length of SADR field */
if (src && src->net && src->len)
npdu[1] |= BIT3;
/* Bit 2: The value of this bit corresponds to the */
/* data_expecting_reply parameter in the N-UNITDATA primitives. */
/* 1 indicates that a BACnet-Confirmed-Request-PDU, */
/* a segment of a BACnet-ComplexACK-PDU, */
/* or a network layer message expecting a reply is present. */
/* 0 indicates that other than a BACnet-Confirmed-Request-PDU, */
/* a segment of a BACnet-ComplexACK-PDU, */
/* or a network layer message expecting a reply is present. */
if (npdu_data->data_expecting_reply)
npdu[1] |= BIT2;
/* Bits 1,0: Network priority where: */
/* B'11' = Life Safety message */
/* B'10' = Critical Equipment message */
/* B'01' = Urgent message */
/* B'00' = Normal message */
npdu[1] |= (npdu_data->priority & 0x03);
len = 2;
if (dest && dest->net) {
len += encode_unsigned16(&npdu[len], dest->net);
npdu[len++] = dest->len;
/* DLEN = 0 denotes broadcast MAC DADR and DADR field is absent */
/* DLEN > 0 specifies length of DADR field */
if (dest->len) {
for (i = 0; i < dest->len; i++) {
npdu[len++] = dest->adr[i];
}
}
}
if (src && src->net && src->len) { /* Only insert if valid */
len += encode_unsigned16(&npdu[len], src->net);
npdu[len++] = src->len;
/* SLEN = 0 denotes broadcast MAC SADR and SADR field is absent */
/* SLEN > 0 specifies length of SADR field */
if (src->len) {
for (i = 0; i < src->len; i++) {
npdu[len++] = src->adr[i];
}
}
}
/* The Hop Count field shall be present only if the message is */
/* destined for a remote network, i.e., if DNET is present. */
/* This is a one-octet field that is initialized to a value of 0xff. */
if (dest && dest->net) {
npdu[len] = npdu_data->hop_count;
len++;
}
if (npdu_data->network_layer_message) {
npdu[len] = npdu_data->network_message_type;
len++;
/* Message Type field contains a value in the range 0x80 - 0xFF, */
/* then a Vendor ID field shall be present */
if (npdu_data->network_message_type >= 0x80)
len += encode_unsigned16(&npdu[len], npdu_data->vendor_id);
}
}
#endif
return len;
}
/* returns bytes sent on success, negative number on failure */
int ethernet_send(
BACNET_ADDRESS * dest, /* destination address */
BACNET_ADDRESS * src, /* source 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 = 0;
uint8_t mtu[MAX_MPDU] = { 0 };
int mtu_len = 0;
int i = 0;
(void) npdu_data;
/* don't waste time if the socket is not valid */
if (Ethernet_Socket < 0) {
fprintf(stderr, "ethernet: 802.2 socket is invalid!\n");
return -1;
}
/* load destination ethernet MAC address */
if (dest->mac_len == 6) {
for (i = 0; i < 6; i++) {
mtu[mtu_len] = dest->mac[i];
mtu_len++;
}
} else {
fprintf(stderr, "ethernet: invalid destination MAC address!\n");
return -2;
}
/* load source ethernet MAC address */
if (src->mac_len == 6) {
for (i = 0; i < 6; i++) {
mtu[mtu_len] = src->mac[i];
mtu_len++;
}
} else {
fprintf(stderr, "ethernet: invalid source MAC address!\n");
return -3;
}
if ((14 + 3 + pdu_len) > MAX_MPDU) {
fprintf(stderr, "ethernet: PDU is too big to send!\n");
return -4;
}
/* packet length */
mtu_len += encode_unsigned16(&mtu[12], 3 /*DSAP,SSAP,LLC */ + pdu_len);
/* Logical PDU portion */
mtu[mtu_len++] = 0x82; /* DSAP for BACnet */
mtu[mtu_len++] = 0x82; /* SSAP for BACnet */
mtu[mtu_len++] = 0x03; /* Control byte in header */
mtu_len = 17;
if ((mtu_len + pdu_len) > MAX_MPDU) {
fprintf(stderr, "ethernet: PDU is too big to send!\n");
return -4;
}
memcpy(&mtu[mtu_len], pdu, pdu_len);
mtu_len += pdu_len;
/* packet length - only the logical portion, not the address */
encode_unsigned16(&mtu[12], 3 + pdu_len);
/* Send the packet */
bytes = send(Ethernet_Socket, (const char *) &mtu, mtu_len, 0);
/* did it get sent? */
if (bytes < 0)
fprintf(stderr, "ethernet: Error sending packet: %s\n",
strerror(errno));
return bytes;
}
/* returns number of bytes sent on success, negative on failure */
int ethernet_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 i = 0; /* counter */
int bytes = 0;
BACNET_ADDRESS src = { 0 }; /* source address for npdu */
uint8_t mtu[MAX_MPDU] = { 0 }; /* our buffer */
int mtu_len = 0;
(void) npdu_data;
/* load the BACnet address for NPDU data */
for (i = 0; i < 6; i++) {
src.mac[i] = Ethernet_MAC_Address[i];
src.mac_len++;
}
/* don't waste time if the socket is not valid */
if (eth802_sockfd < 0) {
fprintf(stderr, "ethernet: 802.2 socket is invalid!\n");
return -1;
}
/* load destination ethernet MAC address */
if (dest->mac_len == 6) {
for (i = 0; i < 6; i++) {
mtu[i] = dest->mac[i];
}
} else {
fprintf(stderr, "ethernet: invalid destination MAC address!\n");
return -2;
}
/* load source ethernet MAC address */
if (src.mac_len == 6) {
for (i = 0; i < 6; i++) {
mtu[6 + i] = src.mac[i];
}
} else {
fprintf(stderr, "ethernet: invalid source MAC address!\n");
return -3;
}
/* Logical PDU portion */
mtu[14] = 0x82; /* DSAP for BACnet */
mtu[15] = 0x82; /* SSAP for BACnet */
mtu[16] = 0x03; /* Control byte in header */
mtu_len = 17;
if ((mtu_len + pdu_len) > MAX_MPDU) {
fprintf(stderr, "ethernet: PDU is too big to send!\n");
return -4;
}
memcpy(&mtu[mtu_len], pdu, pdu_len);
mtu_len += pdu_len;
/* packet length - only the logical portion, not the address */
encode_unsigned16(&mtu[12], 3 + pdu_len);
/* 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;
}
/* returns bytes sent success, negative on failure */
int ethernet_send(
BACNET_ADDRESS * dest, /* destination address */
BACNET_ADDRESS * src, /* source address */
uint8_t * pdu, /* any data to be sent - may be null */
unsigned pdu_len /* number of bytes of data */
)
{
int bytes = 0;
uint8_t mtu[MAX_MPDU] = { 0 };
int mtu_len = 0;
int i = 0;
/* don't waste time if the socket is not valid */
if (!ethernet_valid()) {
LogError("ethernet.c: invalid 802.2 ethernet interface descriptor!\n");
return -1;
}
/* load destination ethernet MAC address */
if (dest->mac_len == 6) {
for (i = 0; i < 6; i++) {
mtu[mtu_len] = dest->mac[i];
mtu_len++;
}
} else {
LogError("ethernet.c: invalid destination MAC address!\n");
return -2;
}
/* load source ethernet MAC address */
if (src->mac_len == 6) {
for (i = 0; i < 6; i++) {
mtu[mtu_len] = src->mac[i];
mtu_len++;
}
} else {
LogError("ethernet.c: invalid source MAC address!\n");
return -3;
}
if ((14 + 3 + pdu_len) > MAX_MPDU) {
LogError("ethernet.c: PDU is too big to send!\n");
return -4;
}
/* packet length */
mtu_len += encode_unsigned16(&mtu[12], 3 /*DSAP,SSAP,LLC */ + pdu_len);
/* Logical PDU portion */
mtu[mtu_len++] = 0x82; /* DSAP for BACnet */
mtu[mtu_len++] = 0x82; /* SSAP for BACnet */
mtu[mtu_len++] = 0x03; /* Control byte in header */
memcpy(&mtu[mtu_len], pdu, pdu_len);
mtu_len += pdu_len;
/* Send the packet */
if (pcap_sendpacket(pcap_eth802_fp, mtu, mtu_len) != 0) {
/* did it get sent? */
char msgBuf[200];
sprintf(msgBuf, "ethernet.c: error sending packet: %s\n",
pcap_geterr(pcap_eth802_fp));
LogError(msgBuf);
return -5;
}
return mtu_len;
}
