lldp_sender.c

/* vim: ft=c ff=unix fenc=utf-8
 * file: main.c
 */
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <unistd.h>

#include <sys/types.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <net/ethernet.h>

#include <sys/ioctl.h>
#include <net/if.h>

#include <linux/if_packet.h>

struct ph {
	int sock;
	struct ether_header eh;
};

/* 511 = maximum size for tlv body */
size_t
pack_tlv(char tlv[511], unsigned short type, unsigned short length, char *data)
{
	uint16_t nlen = htons(length & 0x1ff);
	uint16_t ntype = htons(type << 9);
	uint16_t nheader = (ntype | nlen);

	if (length >= (1 << 9)) {
		fprintf(stderr, "tlv malformed: body length > %d (value: %hd)",
				(1 << 9) - 1, length);
		return 0u;
	}
	memcpy(tlv, &nheader, 2); /* 2 == sizeof tlv header */
	if (data) {
		memcpy(&tlv[2], data, length);
	} else if (length) {
		fprintf(stderr, "tlv malformed: length=%hd, data=NULL\n", length);
	}
	return 2 + length;
}

bool
send_llpd(struct ph * restrict ph, char * restrict message)
{
	char tlv_body[511] = {}; /* 511 - max tlv body length */
	char buf[4096] = {};
	char *p = buf;
	/* ethernet header */
	memcpy(p, &ph->eh, sizeof(ph->eh));
	p += sizeof(ph->eh);

	/* required fields */
	/* lldp chassis id, size: 7 bytes */
	tlv_body[0] = '\x4'; /* subtype: mac */
	memcpy(&tlv_body[1], ph->eh.ether_shost, sizeof(ph->eh.ether_shost));
	p += pack_tlv(p, 1, 7, tlv_body); /* 1 == type of (CLASSIS ID) */
	/* lldp port id, size: 7 bytes */
	tlv_body[0] = '\x3'; /* subtype: mac */
	memcpy(&tlv_body[1], ph->eh.ether_shost, sizeof(ph->eh.ether_shost));
	p += pack_tlv(p, 2, 7, tlv_body); /* 2 == type of (PORT ID) */
	/* lldp ttl, size: 2 bytes */
	{
		short _n = htons(120);
		memcpy(tlv_body, &_n, sizeof(2)); /* 120 seconds */
	}
	p += pack_tlv(p, 3, 2, tlv_body); /* 3 == type of (TTL) */

	/* optional fields */

	if (message) {
		/* 127 = custom private field */
		p += pack_tlv(p, 127, strlen(message), message);
	}

	/* finalize */
	p += pack_tlv(p, 0, 0, NULL); /* end of tlv */

	if (send(ph->sock, buf, p - buf, 0) == -1) {
		perror("send");
		return false;
	}
	return true;
}

void
close_socket(struct ph *ph)
{
	if (ph) {
		if (ph->sock != -1) {
			close(ph->sock);
		}
		free(ph);
	}
}

struct ph *
open_socket(const char *ifname)
{
	const char lldp_multicast[6] =
			{'\x01', '\x80', '\xc2', '\x00', '\x00', '\x0e'};
	const short lldp_ether_type = htons(0x88cc);
	struct ph *ph = calloc(1, sizeof(struct ph));
	struct sockaddr_ll sa = {};
	struct ifreq req = {};

	if (!ph) {
		perror("calloc");
		return NULL;
	}

	strncpy(req.ifr_name, ifname, IFNAMSIZ);

	ph->sock = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
	if (ph->sock == -1) {
		perror("socket");
		goto err;
	}

	if (ioctl(ph->sock, SIOCGIFINDEX, &req)) {
		perror("ioctl(SIOCGIFINDEX)");
		goto err;
	}

	sa.sll_ifindex = req.ifr_ifindex;
	sa.sll_family = PF_PACKET;
	sa.sll_protocol = htons(ETH_P_ALL);

	/* for send() istead sendto() */
	if (bind(ph->sock, (struct sockaddr*)&sa, sizeof(sa))) {
		perror("bind");
		goto err;
	}

	if (ioctl(ph->sock, SIOCGIFHWADDR, &req)) {
		perror("ioctl(SIOCGIFHWADDR)");
		goto err;
	}

	ph->eh.ether_type = lldp_ether_type;
	/* set src and dst addr */
	memcpy(ph->eh.ether_shost, req.ifr_hwaddr.sa_data,
			sizeof(ph->eh.ether_shost));
	memcpy(ph->eh.ether_dhost, lldp_multicast,
			sizeof(ph->eh.ether_dhost));

	return ph;
err:
	if (ph) {
		if (ph->sock != -1)
			close(ph->sock);
		free(ph);
	}
	return NULL;
}

int
main(int argc, char *argv[])
{
	struct ph *ph = NULL;
	int c = 0;
	unsigned int interval = 30;
	char *iface = "eth0";
	char *message = "device in initial state";

	if (geteuid() != 0) {
		fprintf(stderr,
				"uid != 0. Try run under root or set SUID\n");
		fprintf(stderr,
				"HINT: chown root %s && chmod u+s %s\n",
				argv[0], argv[0]);
		return EXIT_FAILURE;
	}

	while ((c = getopt(argc, argv, "i:m:t:")) != -1) {
		switch (c) {
		case 'i':
			iface = optarg;
			break;
		case 't':
			{
				char *_n = NULL;
				interval = (int)strtoul(optarg, &_n, 10);
				if (_n && *_n) {
					fprintf(stderr, "option -t requires an integer\n");
					return EXIT_FAILURE;
				}
				break;
			}
		case 'm':
				message = optarg;
			break;
		case '?':
			if (optopt == 'i' || optopt == 'm' || optopt == 't') {
				fprintf(stderr, "option -%c requires an argument\n", optopt);
			} else {
				fprintf(stderr, "unknown option -%c\n", optopt);
			}
		default:
			fprintf(stderr,
					"usage: %s [-i <ifname>] [-t <interval>] [-m <message>]",
					argv[0]);
			return EXIT_FAILURE;
		}
	}

	fprintf(stdout, "run with -i \"%s\" -t %u -m \"%s\"\n", iface, interval, message);

	if ((ph = open_socket(iface)) != NULL) {
		while (send_llpd(ph, message)) {
			sleep(interval);
		}
	}

	return EXIT_SUCCESS;
}