#if HAVE_CONFIG_H
#include <config.h>
#endif

#include <sys/types.h>
#if HAVE_SYS_UTSNAME_H
#include <sys/utsname.h>
#endif
#if HAVE_SYS_PARAM_H
#include <sys/param.h>
#endif

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#if HAVE_UNISTD_H
#include <unistd.h>
#endif

#include <libnet.h>

#define CDP_CAP_ROUTER 0x01
#define CDP_CAP_TBRIDG 0x02
#define CDP_CAP_SBRIDG 0x04
#define CDP_CAP_SWITCH 0x08
#define CDP_CAP_HOST   0x10

struct cdp_header { 
/* ethernet 802.3 header */
	unsigned char dst_addr[6] __attribute__ ((packed));
	unsigned char src_addr[6] __attribute__ ((packed));
	u_int16_t length __attribute__ ((packed));
/* LLC */
	u_int8_t dsap __attribute__ ((packed));
	u_int8_t ssap __attribute__ ((packed));
/* llc control */
	u_int8_t control __attribute__ ((packed));
	u_int8_t orgcode[3] __attribute__ ((packed));
	u_int16_t protocolId __attribute__ ((packed));
};

static struct utsname myuname;
static unsigned char mysysname[512];
static int debug=0;

#if !HAVE_VSNPRINTF
#if HAVE___VSNPRINTF
#include <stdarg.h>
/* Solaris 2.5.1 implementation of vsnprintf.. */
int __vsnprintf(char* str, size_t size, const char* format, va_list ap);
int vsnprintf(char* str, size_t size, const char* format, va_list ap)
{ 
	return __vsnprintf(str,size,format,ap);
}; 
#else
#error "Don't know how to handle vsnprintf(3) calls."
#endif
#endif

#if !HAVE_SNPRINTF
#if HAVE___VSNPRINTF
#include <stdarg.h>
int snprintf(char* str, size_t size, const char* format, ...)
{ 
	va_list ap;
	int retval;
	va_start(ap, format);
	retval=vsnprintf(str,size,format,ap);
	va_end(ap);
	return retval;
}; 
#else
#error "Don't know how to handle snprintf(3) calls."
#endif
#endif

#if !HAVE_DAEMON
/* prototype of daemon(3), defined in daemon.c */
int daemon(int,int);
#endif

int
sx_write_long(unsigned char* buffer, u_int32_t data)
{ 
#ifdef LIBNET_LIL_ENDIAN
	buffer[3]=(data>>24)&0xff;
	buffer[2]=(data>>16)&0xff;
	buffer[1]=(data>>8)&0xff;
	buffer[0]=data&0xff;
#else
	buffer[0]=(data>>24)&0xff;
	buffer[1]=(data>>16)&0xff;
	buffer[2]=(data>>8)&0xff;
	buffer[3]=data&0xff;
#endif
	return 1;
};

int
sx_write_short(unsigned char* buffer, u_int16_t data)
{ 
#ifdef LIBNET_LIL_ENDIAN
	buffer[1]=(data>>8)&0xff;
	buffer[0]=data&0xff;
#else
	buffer[0]=(data>>8)&0xff;
	buffer[1]=data&0xff;
#endif
	return 1;
};

int
cdp_buffer_init(unsigned char* buffer, int len, struct ether_addr* myether)
{ 
	memset(buffer,0,len);

	buffer[0]=0x01;
	buffer[1]=0x00;
	buffer[2]=0x0c;
	buffer[3]=buffer[4]=buffer[5]=0xcc; 

	memcpy(buffer+6,myether->ether_addr_octet,6);

	((struct cdp_header*)buffer)->dsap=0xaa;
	((struct cdp_header*)buffer)->ssap=0xaa;
	((struct cdp_header*)buffer)->control=0x03;
	((struct cdp_header*)buffer)->orgcode[2]=0x0c;
	sx_write_short((unsigned char*)&(((struct cdp_header*)buffer)->protocolId),
		htons(0x2000));

	buffer+=sizeof(struct cdp_header);

	buffer[0]=0x1; /* cdp version */
	buffer[1]=0xb4; /* cdp holdtime, 180 sec by default */
	buffer[2]=buffer[3]=0; /* checksum - will calculate later */

	return 4+sizeof(struct cdp_header);
};

int
cdp_add_device_id(unsigned char* buffer, int len)
{ 
	char hostname[128];
	gethostname(hostname,128);

	if((strlen(hostname)+4)>len) return 0;

	*(u_int16_t*)buffer=htons(0x0001); /* type=deviceId */
	*((u_int16_t*)(buffer+2))=htons(strlen(hostname)+4); /* total length */
	memcpy(buffer+4,hostname,strlen(hostname));

	return strlen(hostname)+4;
};

int
cdp_add_address(unsigned char* buffer, int len, u_int32_t addr)
{ 
	if(!addr) return 0;
	if(len<17) return 0;

	sx_write_short(buffer,htons(0x02)); 
	sx_write_short(buffer+2,htons(17)); 
	sx_write_long(buffer+4,htonl(1));
	buffer[8]=1; /* nlpid */
	buffer[9]=1; /* proto length */
	buffer[10]=0xcc; /* proto id: cc==IP */
	sx_write_short(buffer+11,htons(4));
	sx_write_long(buffer+13,addr); /* XXXX! */

	return 17;
};

int
cdp_add_interface(unsigned char* buffer, int len, char* interface)
{ 
	if(!interface) return 0;
	if(len<(strlen(interface)+4)) return 0;

	sx_write_short(buffer,htons(0x0003)); /* type=PortId */
	sx_write_short(buffer+2,htons(strlen(interface)+4)); /* totallength*/
	memcpy(buffer+4,interface,strlen(interface));

	return strlen(interface)+4;
};

int
cdp_add_capabilities(unsigned char* buffer, int len)
{ 
	if(len<8) return 0;

	sx_write_short(buffer,htons(0x0004)); /* type=Capabilities */
	sx_write_short(buffer+2,htons(8)); /* totallength*/
	sx_write_long(buffer+4,htonl(CDP_CAP_HOST)); /* no capabilities */

	return 8;
};

int
cdp_add_software_version(unsigned char* buffer, int len)
{ 
	if((strlen(mysysname)+4)>len) return 0;

	sx_write_short(buffer,htons(0x0005)); /* type=software version */
	sx_write_short(buffer+2,htons(strlen(mysysname)+4)); /* totallength*/
	memcpy(buffer+4,mysysname,strlen(mysysname));

	return strlen(mysysname)+4;
};

int 
cdp_add_platform(unsigned char* buffer, int len)
{ 
	if((strlen(myuname.machine)+4)>len) return 0;
	sx_write_short(buffer,htons(0x0006)); /* type=platform */
	sx_write_short(buffer+2,htons(strlen(myuname.machine)+4)); /* totallength*/
	memcpy(buffer+4,myuname.machine,strlen(myuname.machine));

	return strlen(myuname.machine)+4;
};

unsigned short
cdp_checksum(unsigned char *ptr, int length) {
  if (length % 2 == 0) {
    /* The doc says 'standard IP checksum', so this is what we do. */
    return libnet_ip_check((u_short *)ptr, length);
  } else {
    /* An IP checksum is not defined for an odd number of bytes... */
    /* Tricky. */
    /* Treat the last byte as an unsigned short in network order. */

    int c = ptr[length-1];
    unsigned short *sp = (unsigned short *)(&ptr[length-1]);
    unsigned short ret;

    *sp = htons(c);
    ret = libnet_ip_check((u_short *)ptr, length+1);
    ptr[length-1] = c;
    return ret;
  };
}

int
usage()
{ 
	printf("Usage: cdpd [-i interface] [-d] [-t period] [-h] [-o]\n");
	printf("\t-d - increase debug level and do not daemonise\n");
	printf("\t-i interface - interface to send cdp packets on\n");
	printf("\t\t(only ethernet interfaces supported now)\n");
	printf("\t-t period - period before packets (60 sec by default)\n");
	printf("\t-h - get this help message\n");
	printf("\t-o - run once - send only one packet and exit\n");
	printf("\t-a - try to add all iinterfaces with ip addresses configured\n");
	printf("\t\tto those added with -i option\n");
	return 0;
};

struct cdp_interface {
	struct cdp_interface* next;
	char* name;
	struct sockaddr_in address;
	struct ether_addr* eaddr;
	struct libnet_link_int* llink;
};

struct cdp_interface*
cdp_interface_always(struct cdp_interface* list, char* iface)
{
	while(list) {
		if(list->name && !strcmp(list->name,iface)) return list;
		list=list->next;
	};
	return NULL;
};

struct cdp_interface*
cdp_interface_add(struct cdp_interface** head, char* iface)
{
	struct cdp_interface* cdp;
	char ebuf[2048];

	if(!iface || !head) return NULL;

	if((cdp=cdp_interface_always(*head,iface))) return cdp;

	cdp=malloc(sizeof(struct cdp_interface));
	if(!cdp) { 
		perror("malloc");
		exit(1);
	};
	memset(cdp,0,sizeof(struct cdp_interface));

	cdp->llink=libnet_open_link_interface(iface,ebuf);
	if(!cdp->llink) { 
		printf("Can't open interface %s (%s), skipping\n",iface,ebuf);
		return NULL;
	};

	cdp->eaddr=libnet_get_hwaddr(cdp->llink,iface,ebuf);
	if(!cdp->eaddr) { 
		printf("Can't recognize hardware address of %s (%s), skipping\n",iface,
			ebuf);
		return NULL;
	};

	cdp->address.sin_addr.s_addr=htonl(libnet_get_ipaddr(cdp->llink,iface,
		ebuf));
	cdp->name=iface;

	if(!*head) { 
		*head=cdp;
	} else { 
		struct cdp_interface* b=*head;
		while(b->next) b=b->next;
		b->next=cdp;
	};
	if(debug) { 
		printf("added interface %s (%s)\n",iface,
			inet_ntoa(cdp->address.sin_addr));
	};
	return cdp;
};
	
#if (!__solaris__)
int
libnet_ifaddrlist(register struct libnet_ifaddr_list **ipaddrp,
            register char *errbuf);
#else
int
libnet_ifaddrlist(register struct libnet_ifaddr_list **ipaddrp,
            register char *errbuf)
{
	static struct libnet_ifaddr_list list={0,"le0"};
	if(!ipaddrp) { 
		snprintf(errbuf,50,"Wrong arguments");
		return -1;
	};
	*ipaddrp=&list;
	return 1;
};
#endif


int
main(int argc, char* argv[])
{ 
	char c;
	int timeout=60, ret=0;
	unsigned char buffer[1600];
	int offset;
	int once=0, ininited=0, allfaces=0;
	struct cdp_interface *ifaces=NULL;

	while((c=getopt(argc,argv,"i:dt:hoa"))!=EOF) { 
	switch(c) { 
		case 'd': debug++;
			break;
		case 'i': 
			cdp_interface_add(&ifaces,optarg);
			ininited++;
			break;
		case 't': timeout=atoi(optarg);
			if(timeout<=0) { 
				printf("wrong value to timeout - reverting to default 60 sec\n");
				timeout=60;
			};
			break;
		case 'o': once=1;
			break;
		case 'a': allfaces=1;
			break;
		default: usage();
			exit(1);
	};
	};

	if((!ifaces && !ininited) || allfaces) { 
		/* no interfaces given at commandline, so, trying to initialise
			all interfaces.. */
		struct libnet_ifaddr_list* iflist;
		char ebuf[4096];
		int intno,i;

		intno=libnet_ifaddrlist(&iflist,ebuf);
		if(intno<0) { 
			printf("Can't get interface list: %s\n",ebuf);
			exit(1);
		};
		if(!intno) { 
			printf("No interfaces found by libnet.. \n");
			exit(1);
		};

		for(i=0;i<intno;i++) { 
			cdp_interface_add(&ifaces,iflist[i].device);
		};
	};

	if(!ifaces) { 
		printf("No valid interfaces found, exiting..\n");
		exit(1);
	};

	uname(&myuname);
	snprintf(mysysname,sizeof(mysysname),"%s %s %s",
		myuname.sysname, myuname.release, myuname.version);

	if(!debug && !once) 
		daemon(0,0);

	while(1) { 
		struct cdp_interface* cifa=ifaces;
		while(cifa) { 
			offset=0;
			offset=cdp_buffer_init(buffer,sizeof(buffer),cifa->eaddr);
	
			offset+=cdp_add_device_id(buffer+offset,sizeof(buffer)-offset);
			offset+=cdp_add_address(buffer+offset,sizeof(buffer)-offset,
				cifa->address.sin_addr.s_addr);
			offset+=cdp_add_interface(buffer+offset,sizeof(buffer)-offset,
				cifa->name);
			offset+=cdp_add_capabilities(buffer+offset,sizeof(buffer)-offset);
			offset+=cdp_add_software_version(buffer+offset,
				sizeof(buffer)-offset);

			offset+=cdp_add_platform(buffer+offset,sizeof(buffer)-offset);
	
			((struct cdp_header*)buffer)->length=htons(offset-14);
		
			*(u_short*)(buffer+sizeof(struct cdp_header)+2)=cdp_checksum(
				buffer+sizeof(struct cdp_header),
				offset-sizeof(struct cdp_header));
	
			if((ret=libnet_write_link_layer(cifa->llink,cifa->name,buffer,
				offset))
				!=offset) {
				printf("wrote only %i bytes: %s\n",ret,strerror(errno));
			};
	
			if(debug>1) { 
				int i, j;
				printf("Sent over: %s, total length: %i\n", cifa->name, offset);
				for(i=0;i<offset/16;i++) { 
					printf("%4.4x ",i);
					for(j=0;j<16;j++)
						printf("%2.2x ",buffer[16*i+j]);
					for(j=0;j<8;j++) 
						if(isprint(buffer[16*i+j])) 
							printf("%c",buffer[16*i+j]);
						else 
							printf(".");
					printf(" ");
					for(j=8;j<16;j++) 
						if(isprint(buffer[16*i+j])) 
							printf("%c",buffer[16*i+j]);
						else 
							printf(".");
	
					printf("\n");
				};
				if(offset%16) { 
					i=offset/16;
	
					printf("%4.4x ",i);
					for(j=0;j<offset%16;j++)
						printf("%2.2x ",buffer[16*i+j]);
					for(j=offset%16; j<16; j++) 
						printf("   ");
					for(j=0;j<(offset%16>8?8:offset%16);j++) 
						if(isprint(buffer[16*i+j])) 
							printf("%c",buffer[16*i+j]);
						else 
							printf(".");
					printf(" ");
					for(j=8;j<offset%16;j++) 
						if(isprint(buffer[16*i+j])) 
							printf("%c",buffer[16*i+j]);
						else 
							printf(".");
	
					printf("\n");
				};
	
			};
			cifa=cifa->next;
		};  /* all interfaces done */
		if(once) return 0;
		sleep(timeout);
	};
	return 0;
};