[mirror_frr.git] / vrrpd / vrrp_packet.c

/*
 * VRRPD packet crafting
 * Copyright (C) 2018 Cumulus Networks, Inc.
 *               Quentin Young
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
 * Software Foundation; either version 2 of the License, or (at your option)
 * any later version.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; see the file COPYING; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
 */
#include <zebra.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/ip6.h>

#include "lib/checksum.h"
#include "lib/ipaddr.h"
#include "lib/memory.h"

#include "vrrp_packet.h"

/* clang-format off */
const char *vrrp_packet_names[16] = {
	[0] = "Unknown",
	[VRRP_TYPE_ADVERTISEMENT] = "ADVERTISEMENT",
	[2] = "Unknown",
	[3] = "Unknown",
	[4] = "Unknown",
	[5] = "Unknown",
	[6] = "Unknown",
	[7] = "Unknown",
	[8] = "Unknown",
	[9] = "Unknown",
	[10] = "Unknown",
	[11] = "Unknown",
	[12] = "Unknown",
	[13] = "Unknown",
	[14] = "Unknown",
	[15] = "Unknown",
};
/* clang-format on */

ssize_t vrrp_pkt_build(struct vrrp_pkt **pkt, uint8_t vrid, uint8_t prio,
		       uint16_t max_adver_int, uint8_t numip,
		       struct ipaddr **ips)
{
	bool v6 = IS_IPADDR_V6(ips[0]);

	size_t addrsz = v6 ? sizeof(struct in6_addr) : sizeof(struct in_addr);
	size_t pktsize = VRRP_PKT_SIZE(v6 ? AF_INET6 : AF_INET, numip);
	*pkt = XCALLOC(MTYPE_TMP, pktsize);

	(*pkt)->hdr.vertype |= VRRP_VERSION << 4;
	(*pkt)->hdr.vertype |= VRRP_TYPE_ADVERTISEMENT;
	(*pkt)->hdr.vrid = vrid;
	(*pkt)->hdr.priority = prio;
	(*pkt)->hdr.naddr = numip;
	(*pkt)->hdr.v3.adver_int = htons(max_adver_int);

	uint8_t *aptr = (void *)(*pkt)->addrs;

	for (int i = 0; i < numip; i++) {
		memcpy(aptr, &ips[i]->ip.addr, addrsz);
		aptr += addrsz;
	}
	(*pkt)->hdr.chksum = 0;

	/* FIXME: v6 checksum */
	uint16_t chksum = in_cksum(*pkt, pktsize);
	(*pkt)->hdr.chksum = htons(chksum);

	return pktsize;
}

size_t vrrp_pkt_dump(char *buf, size_t buflen, struct vrrp_pkt *pkt)
{
	if (buflen < 1)
		return 0;

	char tmpbuf[BUFSIZ];
	size_t rs = 0;
	struct vrrp_hdr *hdr = &pkt->hdr;

	buf[0] = 0x00;
	snprintf(tmpbuf, sizeof(tmpbuf), "Version: %u\n", (hdr->vertype >> 4));
	rs += strlcat(buf, tmpbuf, buflen);
	snprintf(tmpbuf, sizeof(tmpbuf), "Type: %u (%s)\n",
		 (hdr->vertype & 0x0F),
		 vrrp_packet_names[(hdr->vertype & 0x0F)]);
	rs += strlcat(buf, tmpbuf, buflen);
	snprintf(tmpbuf, sizeof(tmpbuf), "VRID: %u\n", hdr->vrid);
	rs += strlcat(buf, tmpbuf, buflen);
	snprintf(tmpbuf, sizeof(tmpbuf), "Priority: %u\n", hdr->priority);
	rs += strlcat(buf, tmpbuf, buflen);
	snprintf(tmpbuf, sizeof(tmpbuf), "Count IPvX: %u\n", hdr->naddr);
	rs += strlcat(buf, tmpbuf, buflen);
	snprintf(tmpbuf, sizeof(tmpbuf), "Max Adver Int: %u\n",
		 ntohs(hdr->v3.adver_int));
	rs += strlcat(buf, tmpbuf, buflen);
	snprintf(tmpbuf, sizeof(tmpbuf), "Checksum: %x\n", ntohs(hdr->chksum));
	rs += strlcat(buf, tmpbuf, buflen);

	return rs;
}

ssize_t vrrp_parse_datagram(int family, struct msghdr *m, size_t read,
			    struct vrrp_pkt **pkt, char *errmsg,
			    size_t errmsg_len)
{
	/* Source (MAC & IP), Dest (MAC & IP) TTL validation done by kernel */
	size_t addrsz = (family == AF_INET) ? sizeof(struct in_addr)
					    : sizeof(struct in6_addr);

	size_t pktsize;
	uint8_t *buf = m->msg_iov->iov_base;

#define VRRP_PKT_VCHECK(cond, _f, ...)                                         \
	do {                                                                   \
		if (!(cond)) {                                                 \
			if (errmsg)                                            \
				snprintf(errmsg, errmsg_len, (_f),             \
					 ##__VA_ARGS__);                       \
			return -1;                                             \
		}                                                              \
	} while (0)

	/* IPvX header check */

	if (family == AF_INET) {
		VRRP_PKT_VCHECK(
			read >= sizeof(struct ip),
			"Datagram not large enough to contain IP header");

		struct ip *ip = (struct ip *)buf;

		/* IP total length check */
		VRRP_PKT_VCHECK(
			ntohs(ip->ip_len) == read,
			"IPv4 packet length field does not match # received bytes; %u != %lu",
			ntohs(ip->ip_len), read);

		/* TTL check */
		VRRP_PKT_VCHECK(ip->ip_ttl == 255,
				"IPv4 TTL is %" PRIu8 "; should be 255",
				ip->ip_ttl);

		*pkt = (struct vrrp_pkt *)(buf + (ip->ip_hl << 2));
		pktsize = read - (ip->ip_hl << 2);

		/* IP empty packet check */
		VRRP_PKT_VCHECK(pktsize > 0, "IPv4 packet has no payload");
	} else if (family == AF_INET6) {
		struct cmsghdr *c;
		for (c = CMSG_FIRSTHDR(m); c != NULL; CMSG_NXTHDR(m, c)) {
			if (c->cmsg_level == IPPROTO_IPV6
			    && c->cmsg_type == IPV6_HOPLIMIT)
				break;
		}

		VRRP_PKT_VCHECK(!!c, "IPv6 Hop Limit not received");

		uint8_t *hoplimit = CMSG_DATA(c);
		VRRP_PKT_VCHECK(*hoplimit == 255,
				"IPv6 Hop Limit is %" PRIu8 "; should be 255",
				*hoplimit);

		*pkt = (struct vrrp_pkt *)buf;
		pktsize = read;
	} else {
		assert(!"Unknown address family");
	}

	/* Size check */
	size_t minsize = (family == AF_INET) ? VRRP_MIN_PKT_SIZE_V4
					     : VRRP_MIN_PKT_SIZE_V6;
	size_t maxsize = (family == AF_INET) ? VRRP_MAX_PKT_SIZE_V4
					     : VRRP_MAX_PKT_SIZE_V6;
	VRRP_PKT_VCHECK(pktsize >= minsize,
			"VRRP packet is undersized (%lu < %lu)", pktsize,
			VRRP_MIN_PKT_SIZE);
	VRRP_PKT_VCHECK(pktsize <= maxsize,
			"VRRP packet is oversized (%lu > %lu)", pktsize,
			VRRP_MAX_PKT_SIZE);
	/* Version check */
	VRRP_PKT_VCHECK(((*pkt)->hdr.vertype >> 4) != 2, "VRPPv2 unsupported");
	VRRP_PKT_VCHECK(((*pkt)->hdr.vertype >> 4) == 3, "Bad version %u",
			(*pkt)->hdr.vertype >> 4);
	/* Type check */
	VRRP_PKT_VCHECK(((*pkt)->hdr.vertype & 0x0F) == 1, "Bad type %u",
			(*pkt)->hdr.vertype & 0x0f);
	/* Priority check */
	VRRP_PKT_VCHECK((*pkt)->hdr.priority == 255
				|| (*pkt)->hdr.priority == 0,
			"Bad priority %u", (*pkt)->hdr.priority);
	/* # addresses check */
	size_t ves = VRRP_PKT_SIZE(family, (*pkt)->hdr.naddr);
	VRRP_PKT_VCHECK(pktsize == ves, "Packet has incorrect # addresses");
	/* FIXME: checksum check */
	/* ... */

	/* Addresses check */
	char vbuf[INET6_ADDRSTRLEN];
	uint8_t *p = (uint8_t *)(*pkt)->addrs;
	for (uint8_t i = 0; i < (*pkt)->hdr.naddr; i++) {
		VRRP_PKT_VCHECK(inet_ntop(family, p, vbuf, sizeof(vbuf)),
				"Bad IP address, #%u", i);
		p += addrsz;
	}

	/* Everything checks out */
	return pktsize;
}
Commit	Line	Data
5435a2bf QY	1	/*
	2	* VRRPD packet crafting
	3	* Copyright (C) 2018 Cumulus Networks, Inc.
	4	* Quentin Young
	5	*
	6	* This program is free software; you can redistribute it and/or modify it
	7	* under the terms of the GNU General Public License as published by the Free
	8	* Software Foundation; either version 2 of the License, or (at your option)
	9	* any later version.
	10	*
	11	* This program is distributed in the hope that it will be useful, but WITHOUT
	12	* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
	13	* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
	14	* more details.
	15	*
	16	* You should have received a copy of the GNU General Public License along
	17	* with this program; see the file COPYING; if not, write to the Free Software
	18	* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
	19	*/
	20	#include <zebra.h>
91188ca6 QY	21	#include <netinet/in.h>
	22	#include <netinet/ip.h>
	23	#include <netinet/ip6.h>
5435a2bf	24
3eca3857	25	#include "lib/checksum.h"
91188ca6 QY	26	#include "lib/ipaddr.h"
91188ca6 QY	27	#include "lib/memory.h"
5435a2bf QY	28
	29	#include "vrrp_packet.h"
	30
91188ca6 QY	31	/* clang-format off */
	32	const char *vrrp_packet_names[16] = {
	33	[0] = "Unknown",
	34	[VRRP_TYPE_ADVERTISEMENT] = "ADVERTISEMENT",
	35	[2] = "Unknown",
	36	[3] = "Unknown",
	37	[4] = "Unknown",
	38	[5] = "Unknown",
	39	[6] = "Unknown",
	40	[7] = "Unknown",
	41	[8] = "Unknown",
	42	[9] = "Unknown",
	43	[10] = "Unknown",
	44	[11] = "Unknown",
	45	[12] = "Unknown",
	46	[13] = "Unknown",
	47	[14] = "Unknown",
	48	[15] = "Unknown",
	49	};
	50	/* clang-format on */
	51
3eca3857	52	ssize_t vrrp_pkt_build(struct vrrp_pkt **pkt, uint8_t vrid, uint8_t prio,
862f2f37 QY	53	uint16_t max_adver_int, uint8_t numip,
862f2f37 QY	54	struct ipaddr **ips)
5435a2bf	55	{
862f2f37	56	bool v6 = IS_IPADDR_V6(ips[0]);
3eca3857	57
5435a2bf	58	size_t addrsz = v6 ? sizeof(struct in6_addr) : sizeof(struct in_addr);
91188ca6	59	size_t pktsize = VRRP_PKT_SIZE(v6 ? AF_INET6 : AF_INET, numip);
3eca3857 QY	60	*pkt = XCALLOC(MTYPE_TMP, pktsize);
3eca3857 QY	61
3eca3857 QY	62	(*pkt)->hdr.vertype \|= VRRP_VERSION << 4;
	63	(*pkt)->hdr.vertype \|= VRRP_TYPE_ADVERTISEMENT;
	64	(*pkt)->hdr.vrid = vrid;
	65	(*pkt)->hdr.priority = prio;
	66	(*pkt)->hdr.naddr = numip;
	67	(*pkt)->hdr.v3.adver_int = htons(max_adver_int);
	68
862f2f37 QY	69	uint8_t aptr = (void )(*pkt)->addrs;
862f2f37 QY	70
3eca3857	71	for (int i = 0; i < numip; i++) {
862f2f37 QY	72	memcpy(aptr, &ips[i]->ip.addr, addrsz);
862f2f37 QY	73	aptr += addrsz;
3eca3857 QY	74	}
	75	(*pkt)->hdr.chksum = 0;
	76
91188ca6	77	/* FIXME: v6 checksum */
3eca3857 QY	78	uint16_t chksum = in_cksum(*pkt, pktsize);
	79	(*pkt)->hdr.chksum = htons(chksum);
	80
	81	return pktsize;
5435a2bf	82	}
91188ca6 QY	83
	84	size_t vrrp_pkt_dump(char buf, size_t buflen, struct vrrp_pkt pkt)
	85	{
	86	if (buflen < 1)
	87	return 0;
	88
	89	char tmpbuf[BUFSIZ];
	90	size_t rs = 0;
	91	struct vrrp_hdr *hdr = &pkt->hdr;
	92
	93	buf[0] = 0x00;
	94	snprintf(tmpbuf, sizeof(tmpbuf), "Version: %u\n", (hdr->vertype >> 4));
	95	rs += strlcat(buf, tmpbuf, buflen);
	96	snprintf(tmpbuf, sizeof(tmpbuf), "Type: %u (%s)\n",
	97	(hdr->vertype & 0x0F),
	98	vrrp_packet_names[(hdr->vertype & 0x0F)]);
	99	rs += strlcat(buf, tmpbuf, buflen);
	100	snprintf(tmpbuf, sizeof(tmpbuf), "VRID: %u\n", hdr->vrid);
	101	rs += strlcat(buf, tmpbuf, buflen);
	102	snprintf(tmpbuf, sizeof(tmpbuf), "Priority: %u\n", hdr->priority);
	103	rs += strlcat(buf, tmpbuf, buflen);
	104	snprintf(tmpbuf, sizeof(tmpbuf), "Count IPvX: %u\n", hdr->naddr);
	105	rs += strlcat(buf, tmpbuf, buflen);
	106	snprintf(tmpbuf, sizeof(tmpbuf), "Max Adver Int: %u\n",
	107	ntohs(hdr->v3.adver_int));
	108	rs += strlcat(buf, tmpbuf, buflen);
	109	snprintf(tmpbuf, sizeof(tmpbuf), "Checksum: %x\n", ntohs(hdr->chksum));
	110	rs += strlcat(buf, tmpbuf, buflen);
	111
	112	return rs;
	113	}
	114
	115	ssize_t vrrp_parse_datagram(int family, struct msghdr *m, size_t read,
	116	struct vrrp_pkt *pkt, char errmsg,
	117	size_t errmsg_len)
	118	{
	119	/* Source (MAC & IP), Dest (MAC & IP) TTL validation done by kernel */
	120	size_t addrsz = (family == AF_INET) ? sizeof(struct in_addr)
	121	: sizeof(struct in6_addr);
	122
	123	size_t pktsize;
	124	uint8_t *buf = m->msg_iov->iov_base;
	125
	126	#define VRRP_PKT_VCHECK(cond, _f, ...) \
	127	do { \
	128	if (!(cond)) { \
	129	if (errmsg) \
	130	snprintf(errmsg, errmsg_len, (_f), \
	131	##__VA_ARGS__); \
	132	return -1; \
	133	} \
	134	} while (0)
	135
	136	/* IPvX header check */
	137
	138	if (family == AF_INET) {
	139	VRRP_PKT_VCHECK(
	140	read >= sizeof(struct ip),
	141	"Datagram not large enough to contain IP header");
	142
	143	struct ip ip = (struct ip )buf;
	144
	145	/* IP total length check */
	146	VRRP_PKT_VCHECK(
147	ntohs(ip->ip_len) == read,
148	"IPv4 packet length field does not match # received bytes; %u != %lu",
149	ntohs(ip->ip_len), read);
150
151	/* TTL check */
dad18a2f QY	152	VRRP_PKT_VCHECK(ip->ip_ttl == 255,
	153	"IPv4 TTL is %" PRIu8 "; should be 255",
	154	ip->ip_ttl);
91188ca6 QY	155
	156	pkt = (struct vrrp_pkt )(buf + (ip->ip_hl << 2));
	157	pktsize = read - (ip->ip_hl << 2);
	158
	159	/* IP empty packet check */
	160	VRRP_PKT_VCHECK(pktsize > 0, "IPv4 packet has no payload");
	161	} else if (family == AF_INET6) {
	162	struct cmsghdr *c;
	163	for (c = CMSG_FIRSTHDR(m); c != NULL; CMSG_NXTHDR(m, c)) {
	164	if (c->cmsg_level == IPPROTO_IPV6
	165	&& c->cmsg_type == IPV6_HOPLIMIT)
	166	break;
	167	}
	168
	169	VRRP_PKT_VCHECK(!!c, "IPv6 Hop Limit not received");
	170
	171	uint8_t *hoplimit = CMSG_DATA(c);
dad18a2f QY	172	VRRP_PKT_VCHECK(*hoplimit == 255,
	173	"IPv6 Hop Limit is %" PRIu8 "; should be 255",
	174	*hoplimit);
91188ca6 QY	175
	176	pkt = (struct vrrp_pkt )buf;
	177	pktsize = read;
	178	} else {
	179	assert(!"Unknown address family");
	180	}
	181
	182	/* Size check */
	183	size_t minsize = (family == AF_INET) ? VRRP_MIN_PKT_SIZE_V4
	184	: VRRP_MIN_PKT_SIZE_V6;
	185	size_t maxsize = (family == AF_INET) ? VRRP_MAX_PKT_SIZE_V4
	186	: VRRP_MAX_PKT_SIZE_V6;
	187	VRRP_PKT_VCHECK(pktsize >= minsize,
	188	"VRRP packet is undersized (%lu < %lu)", pktsize,
	189	VRRP_MIN_PKT_SIZE);
	190	VRRP_PKT_VCHECK(pktsize <= maxsize,
	191	"VRRP packet is oversized (%lu > %lu)", pktsize,
	192	VRRP_MAX_PKT_SIZE);
	193	/* Version check */
	194	VRRP_PKT_VCHECK(((*pkt)->hdr.vertype >> 4) != 2, "VRPPv2 unsupported");
	195	VRRP_PKT_VCHECK(((*pkt)->hdr.vertype >> 4) == 3, "Bad version %u",
	196	(*pkt)->hdr.vertype >> 4);
	197	/* Type check */
	198	VRRP_PKT_VCHECK(((*pkt)->hdr.vertype & 0x0F) == 1, "Bad type %u",
	199	(*pkt)->hdr.vertype & 0x0f);
	200	/* Priority check */
	201	VRRP_PKT_VCHECK((*pkt)->hdr.priority == 255
	202	\|\| (*pkt)->hdr.priority == 0,
	203	"Bad priority %u", (*pkt)->hdr.priority);
	204	/* # addresses check */
	205	size_t ves = VRRP_PKT_SIZE(family, (*pkt)->hdr.naddr);
	206	VRRP_PKT_VCHECK(pktsize == ves, "Packet has incorrect # addresses");
	207	/* FIXME: checksum check */
	208	/* ... */
	209
	210	/* Addresses check */
	211	char vbuf[INET6_ADDRSTRLEN];
	212	uint8_t p = (uint8_t )(*pkt)->addrs;
	213	for (uint8_t i = 0; i < (*pkt)->hdr.naddr; i++) {
	214	VRRP_PKT_VCHECK(inet_ntop(family, p, vbuf, sizeof(vbuf)),
	215	"Bad IP address, #%u", i);
	216	p += addrsz;
	217	}
	218
	219	/* Everything checks out */
	220	return pktsize;
	221	}