main.c

#include <unistd.h>
#include <linux/if_packet.h>
#include <linux/if_ether.h>
#include <strings.h>
#include <arpa/inet.h>
#include <sys/un.h>
#include <sys/socket.h>
#include <net/if.h>
#include <getopt.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <string.h>
#include <signal.h>
#include <errno.h>
#include <stdlib.h>
#include <time.h>
#include <pthread.h>
#include "tlv.h"
#include "lldp_port.h"
#include "lldp_neighbor.h"
#include "lldp_debug.h"
#include "lldp_neighbor.h"
#include "lldp_linux_framer.h"
#include "rx_sm.h"
#include "tx_sm.h"
#include "tlv.h"
#include "tlv_common.h"
#include "common_func.h"
#include "msap.h"
#include "lldp_dunchong.h"

// This is set to argv[0] on startup.
char *program;

#define LLDP_IF_NAMESIZE    32
char iface_list[IF_NAMESIZE];
int  iface_filter = 0;   /* boolean */

struct lldp_port *wifi_ports = NULL;
struct lldp_port *lldp_ports = NULL;

static void usage();
int initialize_lldp();
void handle_segfault();
void handle_hup();

void cleanupLLDP(struct lldp_port *lldp_port);

void walk_port_list(struct lldp_port *headlist) {
//    struct lldp_port *lldp_port = lldp_ports;
    struct lldp_port *lldp_port = headlist;
#if 1
    while(lldp_port != NULL) {
        lldp_printf(MSG_DEBUG, "Interface structure @ %X\n", lldp_port);
        lldp_printf(MSG_DEBUG, "\tName: %s\n", lldp_port->if_name);
        lldp_printf(MSG_DEBUG, "\tIndex: %d\n", lldp_port->if_index);
        lldp_printf(MSG_DEBUG, "\tMTU: %d\n", lldp_port->mtu);
        lldp_printf(MSG_DEBUG, "\tMAC: %X:%X:%X:%X:%X:%X\n", lldp_port->source_mac[0]
                                                            , lldp_port->source_mac[1]
                                                            , lldp_port->source_mac[2] , lldp_port->source_mac[3]
                                                            , lldp_port->source_mac[4]
                                                            , lldp_port->source_mac[5]); 
        lldp_printf(MSG_DEBUG, "\tIP: %d.%d.%d.%d\n", lldp_port->source_ipaddr[0]
                                                     , lldp_port->source_ipaddr[1]
                                                     , lldp_port->source_ipaddr[2]
                                                     , lldp_port->source_ipaddr[3]);
        lldp_port = lldp_port->next;
    }
#endif
}

extern int32_t dev_role;

int parse_args(int argc, char **argv)
{
	int exitcode = 0;
	int fork = 0;
	const char *log_file = NULL;
	char c;

	for (;;) {
		c = getopt(argc, argv, "dhqf:stxi:r:");
		if (c < 0)
			break;

		switch (c) {
			case 'i':
				iface_filter = 1;
				memcpy(iface_list, optarg, strlen(optarg));
				iface_list[LLDP_IF_NAMESIZE - 1] = '\0';
				lldp_printf(MSG_INFO, "Using interface %s\n", iface_list);
				break;
			case 'r':
				dev_role = atoi(optarg);
				break;
			case 'x':
				fork = 0;
				break;
			case 'h':
				usage();
				exitcode = -1;
				break;
			case 'd':
				if (lldp_debug_level > 0)
				  lldp_debug_level--;
				break;
			case 'f':
				log_file = optarg;
				break;
			case 'q':
				lldp_debug_level++;
				break;
#ifdef CONFIG_DEBUG_SYSLOG
			case 's':
				lldp_debug_open_syslog();
				break;
#endif /* CONFIG_DEBUG_SYSLOG */
			case 't':
				lldp_debug_timestamp++;
				break;
			default:
				usage();
				exitcode = -1;
				break;
		}
	}

	if (log_file)
	  lldp_debug_open_file(log_file);
	else
	  lldp_debug_setup_stdout();

	return exitcode;
}

void thread_rx_sm(void *ptr)
{
	int32_t socket_width = 0;
    struct timeval timeout;
    struct timeval un_timeout;
    time_t current_time = 0;
    time_t last_check = 0;
    int32_t result = 0;
	struct lldp_port *lldp_port = NULL;
	pthread_t thread_tx_id;
	int8_t *neighbors_info = NULL;

	struct eth_hdr expect_hdr, *ether_hdr;

	fd_set readfds, tmpfds;

	expect_hdr.ethertype = htons(0x88cc);
	FD_ZERO(&readfds);	/* setup select() */

	lldp_port = lldp_ports;

	while (lldp_port != NULL) {

		/* This is not the interface you are looking for ... */
		if (lldp_port->if_name == NULL) {
			lldp_printf(MSG_ERROR, "[%s %d][ERROR] Interface index %d with name  is NULL \n", __FUNCTION__, __LINE__, lldp_port->if_index);
			continue; /* Error, interface index %d with name %s is NULL */
		}

		FD_SET(lldp_port->socket, &readfds);

		if (lldp_port->socket > socket_width)
		  socket_width = lldp_port->socket;

		lldp_port->portEnabled = TRUE;

		lldp_port = lldp_port->next;
	}	

	tmpfds = readfds;

	lldp_printf(MSG_DEBUG, "[%s %d] Enter rx state machine loop ...\n",
				__FUNCTION__, __LINE__);
	while (1) {
		readfds = tmpfds;
		time(&current_time);

		/* tell select how long to wait for ... */
		timeout.tv_sec = 1;
		timeout.tv_usec = 0;

		/* Timeout after 1 second if nothing is ready */
		result = select(socket_width + 1, &readfds, NULL, NULL, &timeout);

		/* process the sockets */
		lldp_port = lldp_ports;

		while (lldp_port != NULL) {

			/* This is not the interface you are looking for ... */
			if (lldp_port->if_name == NULL) {
				lldp_printf(MSG_ERROR, "[%s %d][ERROR] Interface index %d with name  is NULL \n", 
							__FUNCTION__, __LINE__, lldp_port->if_index);
				continue; /* Error, interface index %d with name %s is NULL */
			}

			if (result > 0) {
				if (FD_ISSET(lldp_port->socket, &readfds)) {
					//lldp_printf(MSG_DEBUG, "[%s %d][DEBUG] %s is readable, recvsize %d\n", 
					//			__FUNCTION__, __LINE__, lldp_port->if_name, result);

					lldp_read(lldp_port);

					ether_hdr = (struct eth_hdr *)lldp_port->rx.frame;

					if (ether_hdr->ethertype != expect_hdr.ethertype)
						continue;

					//printf("receive frame\n");
					//show_lldp_pdu(lldp_port->rx.frame, lldp_port->rx.recvsize);

					if (lldp_port->rx.recvsize <= 0) {
						if (errno != EAGAIN && errno != ENETDOWN)
						  printf("Error: (%d): %s (%s:%d)\n", 
									  errno, strerror(errno),
									  __FUNCTION__, __LINE__);
					} else {
						/* Got an LLDP Frame %d bytes  long on %s */
						lldp_printf(MSG_INFO, "[%s %d][INFO] Got an LLDP frame %d bytes long on %s\n", 
									__FUNCTION__, __LINE__, lldp_port->rx.recvsize, lldp_port->if_name);

						//lldp_debug_hex_dump(MSG_DEBUG, lldp_port->rx.frame, lldp_port->rx.recvsize);

						/* Mark that we received a frame so the rx state machine can process it. */
						lldp_port->rx.rcvFrame = 1;
						rxStatemachineRun(lldp_port);
					}
				}
			} /* end result > 0 */

            if((result == 0) || (current_time > last_check))
				rxStatemachineRun(lldp_port);
				

			if(result < 0) {
				if(errno != EINTR) {
					lldp_printf(MSG_ERROR, "[%s %d][ERROR] %s\n", __FUNCTION__, __LINE__, strerror(errno));
				}
			}

			lldp_port = lldp_port->next;
		}	/* end while(lldp_port != NULL) */

		time(&last_check);

	}
}

void thread_tx_sm(void *ptr)
{
	struct lldp_port  *lldp_port;

	while (1) {

		//printf("Thread TX SM run...\n");
		lldp_port = lldp_ports;

		while (lldp_port) {
			txStatemachineRun(lldp_port);
			lldp_port = lldp_port->next;
		}

		sleep(1);
	}
}

void thread_gratuitous_arp(void *ptr)
{
	int i;
	struct lldp_port  *lldp_port;
	lldp_port = lldp_ports;
	uint32_t ip = 0x0a000101;
	for (i = 0; i < 512; ++i) {
		lldp_send_gratuitous_arp(lldp_port, ip);
		ip++;
	}
}

int main(int argc, char **argv)
{
    uid_t uid;
    struct timeval timeout;
    struct timeval un_timeout;
   	int fork = 0;
	const char *log_file = NULL;
	char c;

	int op = 0;
    int socket_width = 0;
    time_t last_check = 0;
    int result = 0;
	struct lldp_port *lldp_port = NULL;
	pthread_t thread_tx_id, thread_rx_id;

	pthread_t arp_id;
	fd_set readfds;
    fd_set unixfds;

	if (argc < 2) {
		usage();
		exit(0);
	}
	
	program = argv[0];

	if (parse_args(argc, argv) < 0)
		exit(0);

	get_sys_desc();
	get_sys_fqdn();
	get_wifi_interface();

	// get uid of user executing program. 
    uid = getuid();
	if (uid != 0) {
        lldp_printf(MSG_ERROR, "You must be running as root to run %s!\n", program);
        exit(0);
    }
	lldp_printf(MSG_INFO, "[%s %d] Program %s uid %d\n", __FUNCTION__, __LINE__, argv[0], uid);
	
	if (initialize_lldp() == 0)
		lldp_printf(MSG_WARNING, "[%s %d] no interface found to listen on\n", __FUNCTION__, __LINE__);
	
	if (fork) {
		if (daemon(0, 0) != 0)
			/* unable to daemonize */
			lldp_printf(MSG_WARNING, "[%s %d] Unable to daemonize\n", __FUNCTION__, __LINE__);
	}
	
	walk_port_list(wifi_ports);
	walk_port_list(lldp_ports);

	pthread_create(&thread_tx_id, NULL, (void*)thread_tx_sm, NULL);
	pthread_create(&thread_rx_id, NULL, (void*)thread_rx_sm, NULL);
	pthread_join(thread_tx_id, NULL);
	pthread_join(thread_rx_id, NULL);

#if 0
	pthread_create(&arp_id, NULL, (void*)thread_gratuitous_arp, NULL);
	pthread_join(arp_id, NULL);
#endif

	return 0;	
}

void cleanupLLDP(struct lldp_port *lldp_port)
{
	struct lldp_msap *msap_cache = NULL;

	lldp_port = lldp_ports;

	lldp_printf(MSG_INFO, "[%s %d][INFO] Recv signal, cleanup lldp resourses\n", __FUNCTION__, __LINE__);

	while (lldp_port != NULL) {

		msap_cache = lldp_port->msap_cache;

		while(msap_cache != NULL) {
			if(msap_cache->id != NULL) 
				free(msap_cache->id);

			msap_cache = msap_cache->next;
		}

		free(lldp_port->msap_cache);
		lldp_port->msap_cache = NULL;

		if (lldp_port->if_name != NULL) {
			tlvCleanupLLDP(lldp_port);
			socketCleanupLLDP(lldp_port);
		} else {
			lldp_printf(MSG_INFO, "[%s %d][ERROR] Interface with name is NULL\n", __FUNCTION__, __LINE__);
			/* Error interface with name %s is NULL */
		}
		
		lldp_port = lldp_port->next;
		
		/* clean the previous node and move up */
		free(lldp_ports);
		lldp_ports = lldp_port;
	}
	
	exit(0);
}

int initialize_lldp()
{
	int if_index = 0;
	char if_name[LLDP_IF_NAMESIZE];
	struct lldp_port *lldp_port = NULL;
	int nb_ifaces = 0;
	
	/*
	 * we need to initialize an LLDP port-per interface
	 * "lldp_port" will be changed to point at the interface currently being
	 * serviced
	 */
	for (if_index = MIN_INTERFACES; if_index < MAX_INTERFACES; if_index++) {
		if (if_indextoname(if_index, if_name) == NULL)
			continue;
		
			/* keep only the interface specified by -i option */
		if (iface_filter) {
			if (strncmp(if_name, (const char *)iface_list, LLDP_IF_NAMESIZE) != 0) {
				lldp_printf(MSG_INFO, "[%s %d]Skipping interface %s (not %s)\n",
							__FUNCTION__, __LINE__, if_name, iface_list);
				continue;	/* skipping interface */
			}
		}
		lldp_printf(MSG_INFO, "[%s %d] interface[%d] name %s\n", __FUNCTION__, __LINE__, if_index, if_name);

#if 1
		/* we do not process the lo interface */
		if (strstr(if_name, "lo") != NULL)
			continue;

		if (strncmp(if_name, "vmnet", 6) == 0)
			continue;

		if (strncmp(if_name, "wfm", 6) == 0)
			continue;

#endif
		/* create new interface struct */
		lldp_port = malloc(sizeof(struct lldp_port));
		memset(lldp_port, 0x0, sizeof(struct lldp_port));
		
		
		if (strncmp(if_name, iface_list, LLDP_IF_NAMESIZE) == 0)
			lldp_port->wanport = TRUE;

		/* add it to the global list */
		lldp_port->next = lldp_ports;
		lldp_printf(MSG_DEBUG, "[%s %d] add this interface %s to the global port list \n", __FUNCTION__, __LINE__, if_name);
		
		lldp_port->if_index = if_index;
		lldp_port->if_name = malloc(LLDP_IF_NAMESIZE);
		if (lldp_port->if_name == NULL) {
			free(lldp_port);
			lldp_port = NULL;
			continue;
		}
		
		memcpy(lldp_port->if_name, if_name, LLDP_IF_NAMESIZE);
		
		lldp_printf(MSG_INFO, "[%s %d] %s (index %d) found. Initializing...\n",
					__FUNCTION__, __LINE__, 
					lldp_port->if_name, lldp_port->if_index);


		// We want the first state to be LLDP_WAIT_PORT_OPERATIONAL, so we'll blank out everything here.
		lldp_port->portEnabled = 1;
		lldp_port->role = get_dev_role();
		
		/* initialize the socket for this interface */
		if (lldp_init_socket(lldp_port) != 0) {
			lldp_printf(MSG_ERROR, "[%s %d][ERROR] Problem initialize socket for this interface\n", __FUNCTION__, __LINE__);
			free(lldp_port->if_name);
			lldp_port->if_name = NULL;
			free(lldp_port);
			lldp_port = NULL;
			continue;
		} else {
			lldp_printf(MSG_INFO, "[%s %d]Finished initializing socket for index %d with name %s\n", 
						__FUNCTION__, __LINE__, 
						lldp_port->if_index, lldp_port->if_name);
		}
		
		nb_ifaces++;

		lldp_printf(MSG_INFO, "[%s %d]Initializing TX SM for index %d with name %s\n", 
					__FUNCTION__, __LINE__, 
					lldp_port->if_index, lldp_port->if_name);
		lldp_port->tx.state = TX_LLDP_INITIALIZE;
		txInitializeLLDP(lldp_port);

		lldp_printf(MSG_DEBUG, "[%s %d]Initializing RX SM for index %d with name %s\n", 
					__FUNCTION__, __LINE__, 
					lldp_port->if_index, lldp_port->if_name);
		lldp_port->rx.state = LLDP_WAIT_PORT_OPERATIONAL;
		rxInitializeLLDP(lldp_port);
		lldp_port->portEnabled  = 0;

		lldp_port->adminStatus  = enabledRxTx;
		
		lldp_printf(MSG_INFO, "[%s %d]Initializing TLV subsystem for index %d with name %s\n", 
					__FUNCTION__, __LINE__, 
					lldp_port->if_index, lldp_port->if_name);

		/* Initialize the TLV subsystem for this interface */
		tlvInitializeLLDP(lldp_port);
		
		lldp_printf(MSG_DEBUG, "[%s %d] To send out the first lldp frame\n", __FUNCTION__, __LINE__);
		/*  send out the first lldp frame
		 *	This allows other devices to see us right when we come up,
		 *  rather than having to wait for a full timer cycle.
		 */
		//walk_port_list();
		txChangeToState(lldp_port, TX_IDLE);
		mibConstrInfoLLDPDU(lldp_port);
		txFrame(lldp_port);
		lldp_ports = lldp_port;
	}
	
	/* Don't forget to initialize the TLV validators... */
    //initializeTLVFunctionValidators();

	signal(SIGTERM, cleanupLLDP);
	signal(SIGINT, cleanupLLDP);
	signal(SIGQUIT, cleanupLLDP);
	signal(SIGSEGV, handle_segfault);
	signal(SIGHUP, handle_hup);

	return nb_ifaces;
}

/***************************************
 *
 * Trap a segfault, and exit cleanly.
 *
 ***************************************/
void handle_segfault()
{
    fprintf(stderr, "[FATAL] SIGSEGV  (Segmentation Fault)!\n");

    fflush(stderr); fflush(stdout);
    exit(-1);
}

/***************************************
 *
 * Trap a HUP, and read location config file new.
 *
 ***************************************/
void handle_hup()
{
#ifdef USE_CONFUSE
  lldp_printf(DEBUG_NORMAL, "[INFO] SIGHUP-> read config file again!\n");
  lci_config();
#endif // USE_CONFUSE
}


static void usage(void)
{
	printf("options:\n"
			"  -d increase debugging verbosity (-dd even more)\n"
			"  -q decreate debugging verbosity (-qq even less)\n"
			"  -h show this help text\n"
			"  -t include timestamp in debug messages\n"
			"  -s log output to syslog instead of stdout\n"
			"  -f log output to debug file instead of stdout\n");
}