/* returns the number of octets in the PDU, or zero on failure */
uint16_t ethernet_receive(
BACNET_ADDRESS * src, /* source address */
uint8_t * pdu, /* PDU data */
uint16_t max_pdu, /* amount of space available in the PDU */
unsigned timeout /* number of milliseconds to wait for a packet. we ommit it due to winpcap API. */
)
{
struct pcap_pkthdr *header;
int res;
u_char *pkt_data;
uint16_t pdu_len = 0; /* return value */
/* Make sure the socket is open */
if (!ethernet_valid()) {
LogError("ethernet.c: invalid 802.2 ethernet interface descriptor!\n");
return 0;
}
/* Capture a packet */
res = pcap_next_ex(pcap_eth802_fp, &header, &pkt_data);
if (res < 0) {
char msgBuf[200];
sprintf(msgBuf, "ethernet.c: error in receiving packet: %s\n",
pcap_geterr(pcap_eth802_fp));
return 0;
} else if (res == 0)
return 0;
if (header->len == 0 || header->caplen == 0)
return 0;
/* the signature of an 802.2 BACnet packet */
if ((pkt_data[14] != 0x82) && (pkt_data[15] != 0x82)) {
/*eth_log_error("ethernet.c: Non-BACnet packet\n"); */
return 0;
}
/* copy the source address */
src->mac_len = 6;
memmove(src->mac, &pkt_data[6], 6);
/* check destination address for when */
/* the Ethernet card is in promiscious mode */
if ((memcmp(&pkt_data[0], Ethernet_MAC_Address, 6) != 0)
&& (memcmp(&pkt_data[0], Ethernet_Broadcast, 6) != 0)) {
/*eth_log_error( "ethernet.c: This packet isn't for us\n"); */
return 0;
}
(void) decode_unsigned16(&pkt_data[12], &pdu_len);
pdu_len -= 3 /* DSAP, SSAP, LLC Control */ ;
/* copy the buffer into the PDU */
if (pdu_len < max_pdu)
memmove(&pdu[0], &pkt_data[17], pdu_len);
/* ignore packets that are too large */
else
pdu_len = 0;
return pdu_len;
}
/* 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;
}
void ethernet_cleanup(
void)
{
if (ethernet_valid())
closesocket(Ethernet_Socket);
Ethernet_Socket = -1;
return;
}
void ethernet_cleanup(
void)
{
if (ethernet_valid())
close(eth802_sockfd);
eth802_sockfd = -1;
return;
}
reply_response_t wait_for_icmp_reply(int sock_fd, echo_request_t req) {
unsigned char buffer[BUFFER_LEN];
reply_response_t *res = malloc(sizeof(reply_response_t));
for (;;) {
memset(res, 0, sizeof(reply_response_t));
res->result = -1;
if (recv(sock_fd, (char *)&buffer, sizeof(buffer), 0x0) < 0) {
// If an error occured, check if it was a timeout
if (errno == EAGAIN || errno == EWOULDBLOCK) {
res->result = REPLY_TIMEOUT;
break;
} else {
printf("\nFATAL: Unknown error while reading from socket. errno = %d\n", errno);
exit(1);
}
}
// Capture the time we received the message so we can calculate the time
// elapsed and return it to the user (or timeout the echo request)
struct timeval now;
if (gettimeofday(&now, NULL) < 0) {
printf("Error getting the current time\n");
exit(1);
}
long sec_diff_in_usec = (now.tv_sec - req.sent_at.tv_sec) * 1000000;
long usec_diff = abs(sec_diff_in_usec + now.tv_usec - req.sent_at.tv_usec);
res->elapsed_time_in_ms = usec_diff / 1000.0;
if (res->elapsed_time_in_ms > MAX_WAIT_SEC * 1050) {
res->result = REPLY_TIMEOUT;
break;
}
unsigned char *packet = buffer;
if (!ethernet_valid(packet, req))
continue;
packet += ETHERNET_LEN;
if (!ipv4_valid(packet, req, res))
continue;
packet += IP_HEADER_LEN;
if (!icmp_valid(packet, req, res))
continue;
if (res->result == REPLY_UNSET)
res->result = REPLY_SUCCESS;
break;
}
return *res;
}
bool ethernet_init(
char *interface_name)
{
if (interface_name) {
get_local_hwaddr(interface_name, Ethernet_MAC_Address);
eth802_sockfd = ethernet_bind(ð_addr, interface_name);
} else {
get_local_hwaddr("eth0", Ethernet_MAC_Address);
eth802_sockfd = ethernet_bind(ð_addr, "eth0");
}
return ethernet_valid();
}
/* returns the number of octets in the PDU, or zero on failure */
uint16_t ethernet_receive(
BACNET_ADDRESS * src, /* source address */
uint8_t * pdu, /* PDU data */
uint16_t max_pdu, /* amount of space available in the PDU */
unsigned timeout)
{ /* number of milliseconds to wait for a packet */
int received_bytes;
uint8_t buf[MAX_MPDU] = { 0 }; /* data */
uint16_t pdu_len = 0; /* return value */
/* Make sure the socket is open */
if (!ethernet_valid())
return 0;
received_bytes = R_Ether_Read(0, (void *) buf);
if (received_bytes == 0)
return 0;
/* the signature of an 802.2 BACnet packet */
if ((buf[14] != 0x82) && (buf[15] != 0x82)) {
return 0;
}
/* copy the source address */
src->mac_len = 6;
memmove(src->mac, &buf[6], 6);
/* check destination address for when */
/* the Ethernet card is in promiscious mode */
if ((memcmp(&buf[0], Ethernet_MAC_Address, 6) != 0)
&& (memcmp(&buf[0], Ethernet_Broadcast, 6) != 0)) {
return 0;
}
(void) decode_unsigned16(&buf[12], &pdu_len);
pdu_len -= 3 /* DSAP, SSAP, LLC Control */ ;
/* copy the buffer into the PDU */
if (pdu_len < max_pdu)
memmove(&pdu[0], &buf[17], pdu_len);
/* ignore packets that are too large */
else
pdu_len = 0;
return pdu_len;
}
bool ethernet_init(
char *interface_name)
{
int value = 1;
(void) interface_name;
/* setup the socket */
Ethernet_Socket = socket(AF_INET, SOCK_RAW, 0);
/*Ethernet_Socket = socket(AF_INET, SOCK_STREAM, 0); */
if (Ethernet_Socket < 0)
fprintf(stderr, "ethernet: failed to bind to socket!\r\n");
Ethernet_Address.sa_family = AF_INET;
memset(Ethernet_Address.sa_data, 0, sizeof(Ethernet_Address.sa_data));
if (bind(Ethernet_Socket, &Ethernet_Address,
sizeof(Ethernet_Address)) == SOCKET_ERROR)
fprintf(stderr, "ethernet: failed to bind to socket!\r\n");
/*setsockopt(Ethernet_Socket,SOL_SOCKET,SO_802_2,(char *)&value,sizeof(value)); */
return ethernet_valid();
}
/* 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;
}
bool ethernet_init(
char *if_name)
{
PPACKET_OID_DATA pOidData;
LPADAPTER lpAdapter;
pcap_if_t *pcap_all_if;
pcap_if_t *dev;
BOOLEAN result;
CHAR str[sizeof(PACKET_OID_DATA) + 128];
int i;
char msgBuf[200];
if (ethernet_valid())
ethernet_cleanup();
/**
* Find the interface user specified
*/
/* Retrieve the device list */
if (pcap_findalldevs(&pcap_all_if, pcap_errbuf) == -1) {
sprintf(msgBuf, "ethernet.c: error in pcap_findalldevs: %s\n",
pcap_errbuf);
LogError(msgBuf);
return false;
}
/* Scan the list printing every entry */
for (dev = pcap_all_if; dev; dev = dev->next) {
if (strcmp(if_name, dev->name) == 0)
break;
}
pcap_freealldevs(pcap_all_if); /* we don't need it anymore */
if (dev == NULL) {
sprintf(msgBuf, "ethernet.c: specified interface not found: %s\n",
if_name);
LogError(msgBuf);
return false;
}
/**
* Get local MAC address
*/
ZeroMemory(str, sizeof(PACKET_OID_DATA) + 128);
lpAdapter = PacketOpenAdapter(if_name);
if (lpAdapter == NULL) {
ethernet_cleanup();
sprintf(msgBuf, "ethernet.c: error in PacketOpenAdapter(\"%s\")\n",
if_name);
LogError(msgBuf);
return false;
}
pOidData = (PPACKET_OID_DATA) str;
pOidData->Oid = OID_802_3_CURRENT_ADDRESS;
pOidData->Length = 6;
result = PacketRequest(lpAdapter, FALSE, pOidData);
if (!result) {
PacketCloseAdapter(lpAdapter);
ethernet_cleanup();
LogError("ethernet.c: error in PacketRequest()\n");
return false;
}
for (i = 0; i < 6; ++i)
Ethernet_MAC_Address[i] = pOidData->Data[i];
PacketCloseAdapter(lpAdapter);
/**
* Open interface for subsequent sending and receiving
*/
/* Open the output device */
pcap_eth802_fp = pcap_open(if_name, /* name of the device */
MAX_MPDU, /* portion of the packet to capture */
PCAP_OPENFLAG_PROMISCUOUS, /* promiscuous mode */
eth_timeout, /* read timeout */
NULL, /* authentication on the remote machine */
pcap_errbuf /* error buffer */
);
if (pcap_eth802_fp == NULL) {
PacketCloseAdapter(lpAdapter);
ethernet_cleanup();
sprintf(msgBuf,
"ethernet.c: unable to open the adapter. %s is not supported by WinPcap\n",
if_name);
LogError(msgBuf);
return false;
}
LogInfo("ethernet.c: ethernet_init() ok.\n");
atexit(ethernet_cleanup);
return ethernet_valid();
}
