inarp.c

/******************************************************************************
*	Copyright 2016 Foxconn
*
*	Licensed under the Apache License, Version 2.0 (the "License");
*	you may not use this file except in compliance with the License.
*	You may obtain a copy of the License at
*
*		http://www.apache.org/licenses/LICENSE-2.0
*
*	Unless required by applicable law or agreed to in writing, software
*	distributed under the License is distributed on an "AS IS" BASIS,
*	WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*	See the License for the specific language governing permissions and
*	limitations under the License.
******************************************************************************/
#define MAX_SERVER_NODE 24
#define SKU1_MAX_SERVER_NODES 24

#define ETH_ARP_FRAME_LEN (	sizeof(struct ethhdr)+\
							sizeof(struct arphdr)+\
							((ETH_ALEN+4)*2)\
							)

struct ARP_DATA {
	unsigned char src_mac[ETH_ALEN];	//source mac addr
	unsigned char src_ip[4];	//source ip addr
	unsigned char dest_mac[ETH_ALEN];	//dest mac addr
	unsigned char dest_ip[4];	//dest ip 
};
struct ETH_ARP_PACKET {
	struct ethhdr eh;
	struct arphdr arp;
	struct ARP_DATA arp_data;
};

struct ETH_ARP_PACKET *inarp_req;

static char based_mac[6] = { 0, 0, 0, 0, 0, 0 };

int send_arp_packet(int fd,
		    int ifindex,
		    struct ETH_ARP_PACKET *eth_arp,
		    __be16 ar_op,
		    unsigned char *src_mac,
		    unsigned char *src_ip,
		    unsigned char *dest_mac, unsigned char *dest_ip)
{
	int send_result = 0;
	struct ethhdr *eh = &eth_arp->eh;
	struct arphdr *arp = &eth_arp->arp;
	struct ARP_DATA *arp_data = &eth_arp->arp_data;
	/*target address */
	struct sockaddr_ll socket_address;
	/*prepare sockaddr_ll */
	/*RAW communication */
	socket_address.sll_family = PF_PACKET;
	/*we don't use a protocoll above ethernet layer
	   ->just use anything here */
	socket_address.sll_protocol = htons(ETH_P_ARP);
	/*index of the network device
	   see full code later how to retrieve it */
	socket_address.sll_ifindex = ifindex;
	/*ARP hardware identifier is ethernet */
	socket_address.sll_hatype = ARPHRD_ETHER;
	/*target is another host */
	socket_address.sll_pkttype = PACKET_OTHERHOST;
	/*address length */
	socket_address.sll_halen = ETH_ALEN;
	memcpy(socket_address.sll_addr, dest_mac, ETH_ALEN);
	/*set the frame header */
	memcpy((void *)eh->h_dest, (void *)dest_mac, ETH_ALEN);
	memcpy((void *)eh->h_source, (void *)src_mac, ETH_ALEN);
	eh->h_proto = htons(ETH_P_ARP);
	/*fill InARP request data for ethernet + ipv4 */
	arp->ar_hrd = htons(ARPHRD_ETHER);
	arp->ar_pro = htons(ETH_P_ARP);
	arp->ar_hln = ETH_ALEN;
	arp->ar_pln = 4;
	arp->ar_op = htons(ar_op);
	//fill arp ethernet mac & ipv4 info
	arp_data = (void *)(arp + 1);
	memcpy(arp_data->src_mac, (void *)src_mac, ETH_ALEN);	//source mac addr
	memcpy(arp_data->src_ip, src_ip, 4);	//source ip addr
	memcpy(arp_data->dest_mac, (void *)dest_mac, ETH_ALEN);	//dest mac addr
	memcpy(arp_data->dest_ip, dest_ip, 4);	//dest ip 
	/*send the packet */
	send_result = sendto(fd, eth_arp, ETH_ARP_FRAME_LEN, 0,
			     (struct sockaddr *)&socket_address,
			     sizeof(socket_address));
	if (send_result == -1) {
		printf("sendto: [%s]\n", strerror(errno));
	}
	return send_result;
}

void show_mac_addr(char *name, unsigned char *mac_addr)
{
	int i;
	printf("%s MAC address: ", name);
	for (i = 0; i < 6; i++) {
		printf("%.2X%c", (unsigned char)mac_addr[i],
		       (i == 5) ? '\n' : ':');
	}
	return;
}

void show_ip_addr(char *desc, unsigned char *ip_addr)
{
	int i;
	printf("%s IPv4 address: ", desc);
	for (i = 0; i < 4; i++) {
		printf("%d%c", (unsigned char)ip_addr[i],
		       (i == 3) ? '\n' : '.');
	}
}

void dump_data(char *desc, unsigned char *buffer, int length)
{
	int i;
	printf("%s & length is [%d]:\n", desc, length);
	for (i = 0; i < length; i++) {
		printf("[%02X]", 0xff & buffer[i]);
		if ((i & 0xf) == 0x7)
			printf(" - ");
		if ((i & 0xf) == 0xf)
			printf("\n");
	}
	printf("\n");
}

void get_mac_address_for_node_id(int node_id, char *mac_addr)
{
	int i;
	int add = node_id;
	int carry = 0;
	int mac;
	for (i = 5; i >= 0; i--) {
		mac = (int)based_mac[i];
		mac += (add + carry);
		add = 0;
		if (mac > 255) {
			carry = 1;
			mac -= 256;
		} else {
			carry = 0;
		}
		mac_addr[i] = (char)mac;
	}
}

//return node_id or -1 for error
int get_node_id_by_mac_address(unsigned char *target_mac_addr)
{
	char mac_addr[ETH_ALEN];
	int i;
	for (i = 0; i < MAX_SERVER_NODE; i++) {
		get_mac_address_for_node_id(i, mac_addr);
		if (0 == memcmp(target_mac_addr, mac_addr, ETH_ALEN))
			break;
	}
	if (i != MAX_SERVER_NODE) {
//              int j;
//              printf("mac_address for node[%d]:", i+1);
//              for (j=0; j<6; j++)
//                      printf("%02x:", 0xff & mac_addr[j]);
//              printf("\n");
		return i;
	} else
		return -1;
}

#if ENABLE_MONOTONIC_SYSCALL
#include <sys/syscall.h>
/* Old glibc (< 2.3.4) does not provide this constant. We use syscall
 * directly so this definition is safe. */
#ifndef CLOCK_MONOTONIC
#define CLOCK_MONOTONIC 1
#endif

unsigned FAST_FUNC monotonic_sec(void)
{
	struct timespec ts;
	get_mono(&ts);
	return ts.tv_sec;
}

#else

unsigned monotonic_sec(void)
{
	return time(NULL);
}
#endif
//query ip addr for server node w/ mac addr
//inarp "ethX" "peer mac addr(xx:xx:xx:xx:xx:xx)"
//inarp daemon to handle inarp packets
//inarp "ethX" "file for storing node list"
void InvARPServerTask(
				  /** the internal task ID of this task (in) */
			     ULONG thread_input)
{
	int fd, ret;
	/*buffer for ethernet frame */
	static unsigned char buffer[ETH_FRAME_LEN];	/* single packets are usually not bigger than 8192 bytes */
	int send_result = 0;
	static struct ifreq ifreq_buffer;

	/*our MAC address */
	static unsigned char src_mac[6];
	static unsigned char src_ip[4];
	int ifindex;
//      int InARP_SERVER;

	if (((fd = socket(AF_PACKET, SOCK_PACKET, htons(ETH_P_ARP)))) == -1) {
//      if (((fd = socket (AF_PACKET, SOCK_RAW, 0))) == -1) {
		printf("socket: [%s]\n", strerror(errno));
		exit(-1);
	}
	//local mac address
	memset(&ifreq_buffer, 0x00, sizeof(ifreq_buffer));
	strcpy(ifreq_buffer.ifr_name, "ncsi0");
	ret = ioctl(fd, SIOCGIFHWADDR, &ifreq_buffer);
	if (ret == -1) {
		printf("ioctl2: [%s]\n", strerror(errno));
		close(fd);
		exit(-1);
	}
	memcpy(src_mac, ifreq_buffer.ifr_hwaddr.sa_data, ETH_ALEN);
	show_mac_addr("ncsi0", src_mac);
	//interface index
	memset(&ifreq_buffer, 0x00, sizeof(ifreq_buffer));
	strcpy(ifreq_buffer.ifr_name, "ncsi0");
	ret = ioctl(fd, SIOCGIFINDEX, &ifreq_buffer);
	if (ret == -1) {
		printf("ioctl4: [%s]\n", strerror(errno));
		close(fd);
		exit(-1);
	}
	ifindex = ifreq_buffer.ifr_ifindex;

	int length = 0;		/*length of the received frame */
	static struct ETH_ARP_PACKET *inarp_req = (struct ETH_ARP_PACKET *)buffer;	/* single packets are usually not bigger than 8192 bytes */
	static struct ETH_ARP_PACKET inarp_resp;	/* single packets are usually not bigger than 8192 bytes */
	static struct {
		unsigned char dest_ip[4];
		struct timespec update_time;
	} node[SKU1_MAX_SERVER_NODES];
	struct timespec current_time;
	//argv[2]: file name used for storing node ip list
	//                      line format in file: <node_id> <mac_addr> <ip_addr>
	//get based address via EMS
	//set socket as async
//      fcntl(fd, F_SETFL,O_NONBLOCK|FASYNC);
	//got based address
	while (1) {
		//get local ip address
		memset(&ifreq_buffer, 0x00, sizeof(ifreq_buffer));
		strcpy(ifreq_buffer.ifr_name, "ncsi0");
		ret = ioctl(fd, SIOCGIFADDR, &ifreq_buffer);
		if (ret == -1) {
//                      printf("ioctl3: [%s]\n", strerror(errno));
			sleep(1);
			continue;
		}

		if (AF_INET == ifreq_buffer.ifr_addr.sa_family) {
			memcpy(src_ip, &ifreq_buffer.ifr_addr.sa_data[2], 4);
//                      show_ip_addr("local", (unsigned char *)&ifreq_buffer.ifr_addr.sa_data[2]);
			//show_ip_addr("local", src_ip);
		} else {
			printf("unknown address family [%d]!\n",
			       ifreq_buffer.ifr_addr.sa_family);
			sleep(1);
			continue;
		}

		memset((void *)&inarp_resp, 0, sizeof inarp_resp);
		length = recvfrom(fd, buffer, ETH_ARP_FRAME_LEN, 0, NULL, NULL);
		if (length == -1) {
			sleep(1);
		}
//              printf("length = %d\r\nfd = %d\r\n", length, fd);
		current_time.tv_sec = (unsigned)monotonic_sec();
		if (0 == memcmp(src_mac, inarp_req->eh.h_dest, ETH_ALEN)) {
			if (ntohs(inarp_req->arp.ar_op) == ARPOP_InREQUEST) {	//get a inverse arp request
//                              printf("[Rsp] InRequest\n");
				//dump_data("got InARP request packet", buffer, length); 
				//send inarp response

				printf
				    ("src mac =%02x:%02x:%02x:%02x:%02x:%02x\r\n",
				     inarp_req->arp_data.src_mac[0],
				     inarp_req->arp_data.src_mac[1],
				     inarp_req->arp_data.src_mac[2],
				     inarp_req->arp_data.src_mac[3],
				     inarp_req->arp_data.src_mac[4],
				     inarp_req->arp_data.src_mac[5]
				    );
				printf("src ip =%d:%d:%d:%d\r\n",
				       inarp_req->arp_data.src_ip[0],
				       inarp_req->arp_data.src_ip[1],
				       inarp_req->arp_data.src_ip[2],
				       inarp_req->arp_data.src_ip[3]
				    );
				int fd_1;
				if (((fd_1 =
				      socket(AF_PACKET, SOCK_RAW, 0))) == -1) {
					printf("socket: [%s]\n",
					       strerror(errno));
					exit(-1);
				}
				send_result =
				    send_arp_packet(fd_1, ifindex, &inarp_resp,
						    ARPOP_InREPLY,
						    inarp_req->arp_data.
						    dest_mac, src_ip,
						    inarp_req->arp_data.src_mac,
						    inarp_req->arp_data.src_ip);
				close(fd_1);
				if (send_result == -1) {
					printf("[Rsp] sendto: [%s]\n",
					       strerror(errno));
					sleep(1);
					continue;
				}
			}
//                      } else if (ntohs(inarp_req->arp.ar_op) == ARPOP_InREPLY) {      //get a InARP Response
//                              int node_id;
//                              printf("[Rsp] InReply\n");
//                              node_id = get_node_id_by_mac_address(inarp_req->arp_data.src_mac);
//                              if (node_id == -1) {
//                                      //invalid node
//                                      printf("%02x:%02x:%02x:%02x:%02x:%02x does not belong to this rack.",
//                                                      inarp_req->arp_data.src_mac[0],
//                                                      inarp_req->arp_data.src_mac[1],
//                                                      inarp_req->arp_data.src_mac[2],
//                                                      inarp_req->arp_data.src_mac[3],
//                                                      inarp_req->arp_data.src_mac[4],
//                                                      inarp_req->arp_data.src_mac[5]
//                                      );
//                              } else {
			//valid node id
//                                      node[node_id].update_time.tv_sec = current_time.tv_sec;
			//update or remain node's ip
//                                      if ((0 != memcmp(node[node_id].dest_ip, inarp_req->arp_data.src_ip, 4))) {
			//ip address is changed.
//                                              static char ipv4_addr[16];
//                                              printf("[Rsp] update node[%d] ip @ %u\n", node_id+1, (unsigned)current_time.tv_sec);
//                                                      show_mac_addr("[Rsp] ", inarp_req->arp_data.src_mac);
//                                                      show_ip_addr("[Rsp] ", inarp_req->arp_data.src_ip);
//                                                      memcpy(node[node_id].dest_ip, inarp_req->arp_data.src_ip, 4);
//                                              sprintf(ipv4_addr, "%d.%d.%d.%d", 
//                                                                      inarp_req->arp_data.src_ip[0],
//                                                                      inarp_req->arp_data.src_ip[1],
//                                                                      inarp_req->arp_data.src_ip[2],
//                                                                      inarp_req->arp_data.src_ip[3]);
//                                              printf("[Rsp] update node[%d] ip @ %u\n", node_id+1, (unsigned)current_time.tv_sec);
//                      }
			memset(buffer, 0, sizeof(buffer));
		}
	}
	close(fd);
	return 0;
}