[mirror_frr.git] / zebra / zebra_mpls_openbsd.c

/*
 * Copyright (C) 2016 by Open Source Routing.
 *
 * This file is part of GNU Zebra.
 *
 * GNU Zebra 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, or (at your option) any
 * later version.
 *
 * GNU Zebra 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>

#ifdef OPEN_BSD

#include <netmpls/mpls.h>
#include "zebra/rt.h"
#include "zebra/zebra_mpls.h"
#include "zebra/debug.h"

#include "privs.h"
#include "prefix.h"
#include "interface.h"
#include "log.h"
#include "lib_errors.h"

extern struct zebra_privs_t zserv_privs;

struct {
	uint32_t rtseq;
	int fd;
	int ioctl_fd;
} kr_state;

static int kernel_send_rtmsg_v4(int action, mpls_label_t in_label,
				zebra_nhlfe_t *nhlfe)
{
	struct iovec iov[5];
	struct rt_msghdr hdr;
	struct sockaddr_mpls sa_label_in, sa_label_out;
	struct sockaddr_in nexthop;
	int iovcnt = 0;
	int ret;

	if (IS_ZEBRA_DEBUG_KERNEL)
		zlog_debug("%s: 0x%x, label=%u", __func__, action, in_label);

	/* initialize header */
	memset(&hdr, 0, sizeof(hdr));
	hdr.rtm_version = RTM_VERSION;

	hdr.rtm_type = action;
	hdr.rtm_flags = RTF_UP;
	hdr.rtm_fmask = RTF_MPLS;
	hdr.rtm_seq = kr_state.rtseq++; /* overflow doesn't matter */
	hdr.rtm_msglen = sizeof(hdr);
	hdr.rtm_hdrlen = sizeof(struct rt_msghdr);
	hdr.rtm_priority = 0;
	/* adjust iovec */
	iov[iovcnt].iov_base = &hdr;
	iov[iovcnt++].iov_len = sizeof(hdr);

	/* in label */
	memset(&sa_label_in, 0, sizeof(sa_label_in));
	sa_label_in.smpls_len = sizeof(sa_label_in);
	sa_label_in.smpls_family = AF_MPLS;
	sa_label_in.smpls_label = htonl(in_label << MPLS_LABEL_OFFSET);
	/* adjust header */
	hdr.rtm_flags |= RTF_MPLS | RTF_MPATH;
	hdr.rtm_addrs |= RTA_DST;
	hdr.rtm_msglen += sizeof(sa_label_in);
	/* adjust iovec */
	iov[iovcnt].iov_base = &sa_label_in;
	iov[iovcnt++].iov_len = sizeof(sa_label_in);

	/* nexthop */
	memset(&nexthop, 0, sizeof(nexthop));
	nexthop.sin_len = sizeof(nexthop);
	nexthop.sin_family = AF_INET;
	nexthop.sin_addr = nhlfe->nexthop->gate.ipv4;
	/* adjust header */
	hdr.rtm_flags |= RTF_GATEWAY;
	hdr.rtm_addrs |= RTA_GATEWAY;
	hdr.rtm_msglen += sizeof(nexthop);
	/* adjust iovec */
	iov[iovcnt].iov_base = &nexthop;
	iov[iovcnt++].iov_len = sizeof(nexthop);

	/* If action is RTM_DELETE we have to get rid of MPLS infos */
	if (action != RTM_DELETE) {
		memset(&sa_label_out, 0, sizeof(sa_label_out));
		sa_label_out.smpls_len = sizeof(sa_label_out);
		sa_label_out.smpls_family = AF_MPLS;
		sa_label_out.smpls_label =
			htonl(nhlfe->nexthop->nh_label->label[0]
			      << MPLS_LABEL_OFFSET);
		/* adjust header */
		hdr.rtm_addrs |= RTA_SRC;
		hdr.rtm_flags |= RTF_MPLS;
		hdr.rtm_msglen += sizeof(sa_label_out);
		/* adjust iovec */
		iov[iovcnt].iov_base = &sa_label_out;
		iov[iovcnt++].iov_len = sizeof(sa_label_out);

		if (nhlfe->nexthop->nh_label->label[0] == MPLS_LABEL_IMPLNULL)
			hdr.rtm_mpls = MPLS_OP_POP;
		else
			hdr.rtm_mpls = MPLS_OP_SWAP;
	}

	frr_elevate_privs(&zserv_privs) {
		ret = writev(kr_state.fd, iov, iovcnt);
	}

	if (ret == -1)
		flog_err_sys(LIB_ERR_SOCKET, "%s: %s", __func__,
			     safe_strerror(errno));

	return ret;
}

#if !defined(ROUNDUP)
#define ROUNDUP(a)                                                             \
	(((a) & (sizeof(long) - 1)) ? (1 + ((a) | (sizeof(long) - 1))) : (a))
#endif

static int kernel_send_rtmsg_v6(int action, mpls_label_t in_label,
				zebra_nhlfe_t *nhlfe)
{
	struct iovec iov[5];
	struct rt_msghdr hdr;
	struct sockaddr_mpls sa_label_in, sa_label_out;
	struct pad {
		struct sockaddr_in6 addr;
		char pad[sizeof(long)]; /* thank you IPv6 */
	} nexthop;
	int iovcnt = 0;
	int ret;

	if (IS_ZEBRA_DEBUG_KERNEL)
		zlog_debug("%s: 0x%x, label=%u", __func__, action, in_label);

	/* initialize header */
	memset(&hdr, 0, sizeof(hdr));
	hdr.rtm_version = RTM_VERSION;

	hdr.rtm_type = action;
	hdr.rtm_flags = RTF_UP;
	hdr.rtm_fmask = RTF_MPLS;
	hdr.rtm_seq = kr_state.rtseq++; /* overflow doesn't matter */
	hdr.rtm_msglen = sizeof(hdr);
	hdr.rtm_hdrlen = sizeof(struct rt_msghdr);
	hdr.rtm_priority = 0;
	/* adjust iovec */
	iov[iovcnt].iov_base = &hdr;
	iov[iovcnt++].iov_len = sizeof(hdr);

	/* in label */
	memset(&sa_label_in, 0, sizeof(sa_label_in));
	sa_label_in.smpls_len = sizeof(sa_label_in);
	sa_label_in.smpls_family = AF_MPLS;
	sa_label_in.smpls_label = htonl(in_label << MPLS_LABEL_OFFSET);
	/* adjust header */
	hdr.rtm_flags |= RTF_MPLS | RTF_MPATH;
	hdr.rtm_addrs |= RTA_DST;
	hdr.rtm_msglen += sizeof(sa_label_in);
	/* adjust iovec */
	iov[iovcnt].iov_base = &sa_label_in;
	iov[iovcnt++].iov_len = sizeof(sa_label_in);

	/* nexthop */
	memset(&nexthop, 0, sizeof(nexthop));
	nexthop.addr.sin6_len = sizeof(struct sockaddr_in6);
	nexthop.addr.sin6_family = AF_INET6;
	nexthop.addr.sin6_addr = nhlfe->nexthop->gate.ipv6;
	if (IN6_IS_ADDR_LINKLOCAL(&nexthop.addr.sin6_addr)) {
		uint16_t tmp16;
		struct sockaddr_in6 *sin6 = &nexthop.addr;

		nexthop.addr.sin6_scope_id = nhlfe->nexthop->ifindex;

		memcpy(&tmp16, &sin6->sin6_addr.s6_addr[2], sizeof(tmp16));
		tmp16 = htons(sin6->sin6_scope_id);
		memcpy(&sin6->sin6_addr.s6_addr[2], &tmp16, sizeof(tmp16));
		sin6->sin6_scope_id = 0;
	}

	/* adjust header */
	hdr.rtm_flags |= RTF_GATEWAY;
	hdr.rtm_addrs |= RTA_GATEWAY;
	hdr.rtm_msglen += ROUNDUP(sizeof(struct sockaddr_in6));
	/* adjust iovec */
	iov[iovcnt].iov_base = &nexthop;
	iov[iovcnt++].iov_len = ROUNDUP(sizeof(struct sockaddr_in6));

	/* If action is RTM_DELETE we have to get rid of MPLS infos */
	if (action != RTM_DELETE) {
		memset(&sa_label_out, 0, sizeof(sa_label_out));
		sa_label_out.smpls_len = sizeof(sa_label_out);
		sa_label_out.smpls_family = AF_MPLS;
		sa_label_out.smpls_label =
			htonl(nhlfe->nexthop->nh_label->label[0]
			      << MPLS_LABEL_OFFSET);
		/* adjust header */
		hdr.rtm_addrs |= RTA_SRC;
		hdr.rtm_flags |= RTF_MPLS;
		hdr.rtm_msglen += sizeof(sa_label_out);
		/* adjust iovec */
		iov[iovcnt].iov_base = &sa_label_out;
		iov[iovcnt++].iov_len = sizeof(sa_label_out);

		if (nhlfe->nexthop->nh_label->label[0] == MPLS_LABEL_IMPLNULL)
			hdr.rtm_mpls = MPLS_OP_POP;
		else
			hdr.rtm_mpls = MPLS_OP_SWAP;
	}

	frr_elevate_privs(&zserv_privs) {
		ret = writev(kr_state.fd, iov, iovcnt);
	}

	if (ret == -1)
		flog_err_sys(LIB_ERR_SOCKET, "%s: %s", __func__,
			     safe_strerror(errno));

	return ret;
}

static int kernel_lsp_cmd(int action, zebra_lsp_t *lsp)
{
	zebra_nhlfe_t *nhlfe;
	struct nexthop *nexthop = NULL;
	unsigned int nexthop_num = 0;

	for (nhlfe = lsp->nhlfe_list; nhlfe; nhlfe = nhlfe->next) {
		nexthop = nhlfe->nexthop;
		if (!nexthop)
			continue;

		if (nexthop_num >= multipath_num)
			break;

		if (((action == RTM_ADD || action == RTM_CHANGE)
		     && (CHECK_FLAG(nhlfe->flags, NHLFE_FLAG_SELECTED)
			 && CHECK_FLAG(nexthop->flags, NEXTHOP_FLAG_ACTIVE)))
		    || (action == RTM_DELETE
			&& (CHECK_FLAG(nhlfe->flags, NHLFE_FLAG_INSTALLED)
			    && CHECK_FLAG(nexthop->flags, NEXTHOP_FLAG_FIB)))) {
			if (nhlfe->nexthop->nh_label->num_labels > 1) {
				zlog_warn(
					"%s: can't push %u labels at once "
					"(maximum is 1)",
					__func__,
					nhlfe->nexthop->nh_label->num_labels);
				continue;
			}

			nexthop_num++;

			switch (NHLFE_FAMILY(nhlfe)) {
			case AF_INET:
				kernel_send_rtmsg_v4(action, lsp->ile.in_label,
						     nhlfe);
				break;
			case AF_INET6:
				kernel_send_rtmsg_v6(action, lsp->ile.in_label,
						     nhlfe);
				break;
			default:
				break;
			}
		}
	}

	return (0);
}

enum dp_req_result kernel_add_lsp(zebra_lsp_t *lsp)
{
	int ret;

	if (!lsp || !lsp->best_nhlfe) { // unexpected
		kernel_lsp_pass_fail(lsp, DP_INSTALL_FAILURE);
		return DP_REQUEST_FAILURE;
	}

	ret = kernel_lsp_cmd(RTM_ADD, lsp);

	kernel_lsp_pass_fail(lsp,
			     (!ret) ? DP_INSTALL_SUCCESS
				    : DP_INSTALL_FAILURE);

	return DP_REQUEST_SUCCESS;
}

enum dp_req_result kernel_upd_lsp(zebra_lsp_t *lsp)
{
	int ret;

	if (!lsp || !lsp->best_nhlfe) { // unexpected
		kernel_lsp_pass_fail(lsp, DP_INSTALL_FAILURE);
		return DP_REQUEST_FAILURE;
	}

	ret = kernel_lsp_cmd(RTM_CHANGE, lsp);

	kernel_lsp_pass_fail(lsp,
			     (!ret) ? DP_INSTALL_SUCCESS
				    : DP_INSTALL_FAILURE);
	return DP_REQUEST_SUCCESS;
}

enum dp_req_result kernel_del_lsp(zebra_lsp_t *lsp)
{
	int ret;

	if (!lsp) { // unexpected
		kernel_lsp_pass_fail(lsp, DP_DELETE_FAILURE);
		return DP_REQUEST_FAILURE;
	}

	if (!CHECK_FLAG(lsp->flags, LSP_FLAG_INSTALLED)) {
		kernel_lsp_pass_fail(lsp, DP_DELETE_FAILURE);
		return DP_REQUEST_FAILURE;
	}

	ret = kernel_lsp_cmd(RTM_DELETE, lsp);

	kernel_lsp_pass_fail(lsp,
			     (!ret) ? DP_DELETE_SUCCESS
				    : DP_DELETE_FAILURE);

	return DP_REQUEST_SUCCESS;
}

static int kmpw_install(struct zebra_pw *pw)
{
	struct ifreq ifr;
	struct ifmpwreq imr;
	struct sockaddr_storage ss;
	struct sockaddr_in *sa_in = (struct sockaddr_in *)&ss;
	struct sockaddr_in6 *sa_in6 = (struct sockaddr_in6 *)&ss;

	memset(&imr, 0, sizeof(imr));
	switch (pw->type) {
	case PW_TYPE_ETHERNET:
		imr.imr_type = IMR_TYPE_ETHERNET;
		break;
	case PW_TYPE_ETHERNET_TAGGED:
		imr.imr_type = IMR_TYPE_ETHERNET_TAGGED;
		break;
	default:
		zlog_warn("%s: unhandled pseudowire type (%#X)", __func__,
			  pw->type);
		return -1;
	}

	if (pw->flags & F_PSEUDOWIRE_CWORD)
		imr.imr_flags |= IMR_FLAG_CONTROLWORD;

	/* pseudowire nexthop */
	memset(&ss, 0, sizeof(ss));
	switch (pw->af) {
	case AF_INET:
		sa_in->sin_family = AF_INET;
		sa_in->sin_len = sizeof(struct sockaddr_in);
		sa_in->sin_addr = pw->nexthop.ipv4;
		break;
	case AF_INET6:
		sa_in6->sin6_family = AF_INET6;
		sa_in6->sin6_len = sizeof(struct sockaddr_in6);
		sa_in6->sin6_addr = pw->nexthop.ipv6;
		break;
	default:
		zlog_warn("%s: unhandled pseudowire address-family (%u)",
			  __func__, pw->af);
		return -1;
	}
	memcpy(&imr.imr_nexthop, (struct sockaddr *)&ss,
	       sizeof(imr.imr_nexthop));

	/* pseudowire local/remote labels */
	imr.imr_lshim.shim_label = pw->local_label;
	imr.imr_rshim.shim_label = pw->remote_label;

	/* ioctl */
	memset(&ifr, 0, sizeof(ifr));
	strlcpy(ifr.ifr_name, pw->ifname, sizeof(ifr.ifr_name));
	ifr.ifr_data = (caddr_t)&imr;
	if (ioctl(kr_state.ioctl_fd, SIOCSETMPWCFG, &ifr) == -1) {
		flog_err_sys(LIB_ERR_SYSTEM_CALL, "ioctl SIOCSETMPWCFG: %s",
			     safe_strerror(errno));
		return -1;
	}

	return 0;
}

static int kmpw_uninstall(struct zebra_pw *pw)
{
	struct ifreq ifr;
	struct ifmpwreq imr;

	memset(&ifr, 0, sizeof(ifr));
	memset(&imr, 0, sizeof(imr));
	strlcpy(ifr.ifr_name, pw->ifname, sizeof(ifr.ifr_name));
	ifr.ifr_data = (caddr_t)&imr;
	if (ioctl(kr_state.ioctl_fd, SIOCSETMPWCFG, &ifr) == -1) {
		flog_err_sys(LIB_ERR_SYSTEM_CALL, "ioctl SIOCSETMPWCFG: %s",
			     safe_strerror(errno));
		return -1;
	}

	return 0;
}

#define MAX_RTSOCK_BUF	128 * 1024
int mpls_kernel_init(void)
{
	int rcvbuf, default_rcvbuf;
	socklen_t optlen;

	if ((kr_state.fd = socket(AF_ROUTE, SOCK_RAW, 0)) == -1) {
		zlog_warn("%s: socket", __func__);
		return -1;
	}

	if ((kr_state.ioctl_fd = socket(AF_INET, SOCK_DGRAM | SOCK_NONBLOCK, 0))
	    == -1) {
		zlog_warn("%s: ioctl socket", __func__);
		return -1;
	}

	/* grow receive buffer, don't wanna miss messages */
	optlen = sizeof(default_rcvbuf);
	if (getsockopt(kr_state.fd, SOL_SOCKET, SO_RCVBUF, &default_rcvbuf,
		       &optlen)
	    == -1)
		zlog_warn("kr_init getsockopt SOL_SOCKET SO_RCVBUF");
	else
		for (rcvbuf = MAX_RTSOCK_BUF;
		     rcvbuf > default_rcvbuf
		     && setsockopt(kr_state.fd, SOL_SOCKET, SO_RCVBUF, &rcvbuf,
				   sizeof(rcvbuf))
				== -1
		     && errno == ENOBUFS;
		     rcvbuf /= 2)
			; /* nothing */

	kr_state.rtseq = 1;

	/* register hook to install/uninstall pseudowires */
	hook_register(pw_install, kmpw_install);
	hook_register(pw_uninstall, kmpw_uninstall);

	return 0;
}

#endif /* OPEN_BSD */
Commit	Line	Data
7fc02572 RW	1	/*
	2	* Copyright (C) 2016 by Open Source Routing.
	3	*
	4	* This file is part of GNU Zebra.
	5	*
	6	* GNU Zebra 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
	8	* Free Software Foundation; either version 2, or (at your option) any
	9	* later version.
	10	*
	11	* GNU Zebra is distributed in the hope that it will be useful, but
	12	* WITHOUT ANY WARRANTY; without even the implied warranty of
	13	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
	14	* General Public License for more details.
	15	*
896014f4 DL	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
7fc02572 RW	19	*/
7fc02572 RW	20
d3e2c74a	21	#include <zebra.h>
ddfeb486 DL	22
	23	#ifdef OPEN_BSD
	24
d3e2c74a RW	25	#include <netmpls/mpls.h>
	26	#include "zebra/rt.h"
	27	#include "zebra/zebra_mpls.h"
	28	#include "zebra/debug.h"
	29
	30	#include "privs.h"
	31	#include "prefix.h"
	32	#include "interface.h"
	33	#include "log.h"
174482ef	34	#include "lib_errors.h"
d3e2c74a RW	35
	36	extern struct zebra_privs_t zserv_privs;
	37
	38	struct {
d7c0a89a	39	uint32_t rtseq;
d62a17ae	40	int fd;
d42127da	41	int ioctl_fd;
d3e2c74a RW	42	} kr_state;
d3e2c74a RW	43
d62a17ae	44	static int kernel_send_rtmsg_v4(int action, mpls_label_t in_label,
d62a17ae	45	zebra_nhlfe_t *nhlfe)
d3e2c74a	46	{
d62a17ae	47	struct iovec iov[5];
	48	struct rt_msghdr hdr;
	49	struct sockaddr_mpls sa_label_in, sa_label_out;
	50	struct sockaddr_in nexthop;
	51	int iovcnt = 0;
	52	int ret;
	53
	54	if (IS_ZEBRA_DEBUG_KERNEL)
	55	zlog_debug("%s: 0x%x, label=%u", __func__, action, in_label);
	56
	57	/* initialize header */
	58	memset(&hdr, 0, sizeof(hdr));
	59	hdr.rtm_version = RTM_VERSION;
	60
	61	hdr.rtm_type = action;
	62	hdr.rtm_flags = RTF_UP;
	63	hdr.rtm_fmask = RTF_MPLS;
	64	hdr.rtm_seq = kr_state.rtseq++; /* overflow doesn't matter */
	65	hdr.rtm_msglen = sizeof(hdr);
	66	hdr.rtm_hdrlen = sizeof(struct rt_msghdr);
	67	hdr.rtm_priority = 0;
	68	/* adjust iovec */
	69	iov[iovcnt].iov_base = &hdr;
	70	iov[iovcnt++].iov_len = sizeof(hdr);
	71
	72	/* in label */
	73	memset(&sa_label_in, 0, sizeof(sa_label_in));
	74	sa_label_in.smpls_len = sizeof(sa_label_in);
	75	sa_label_in.smpls_family = AF_MPLS;
	76	sa_label_in.smpls_label = htonl(in_label << MPLS_LABEL_OFFSET);
	77	/* adjust header */
	78	hdr.rtm_flags \|= RTF_MPLS \| RTF_MPATH;
	79	hdr.rtm_addrs \|= RTA_DST;
	80	hdr.rtm_msglen += sizeof(sa_label_in);
	81	/* adjust iovec */
	82	iov[iovcnt].iov_base = &sa_label_in;
	83	iov[iovcnt++].iov_len = sizeof(sa_label_in);
	84
	85	/* nexthop */
	86	memset(&nexthop, 0, sizeof(nexthop));
	87	nexthop.sin_len = sizeof(nexthop);
	88	nexthop.sin_family = AF_INET;
	89	nexthop.sin_addr = nhlfe->nexthop->gate.ipv4;
	90	/* adjust header */
	91	hdr.rtm_flags \|= RTF_GATEWAY;
	92	hdr.rtm_addrs \|= RTA_GATEWAY;
	93	hdr.rtm_msglen += sizeof(nexthop);
	94	/* adjust iovec */
	95	iov[iovcnt].iov_base = &nexthop;
	96	iov[iovcnt++].iov_len = sizeof(nexthop);
	97
	98	/* If action is RTM_DELETE we have to get rid of MPLS infos */
	99	if (action != RTM_DELETE) {
	100	memset(&sa_label_out, 0, sizeof(sa_label_out));
	101	sa_label_out.smpls_len = sizeof(sa_label_out);
	102	sa_label_out.smpls_family = AF_MPLS;
	103	sa_label_out.smpls_label =
	104	htonl(nhlfe->nexthop->nh_label->label[0]
	105	<< MPLS_LABEL_OFFSET);
	106	/* adjust header */
	107	hdr.rtm_addrs \|= RTA_SRC;
	108	hdr.rtm_flags \|= RTF_MPLS;
	109	hdr.rtm_msglen += sizeof(sa_label_out);
	110	/* adjust iovec */
111	iov[iovcnt].iov_base = &sa_label_out;
112	iov[iovcnt++].iov_len = sizeof(sa_label_out);
113
114	if (nhlfe->nexthop->nh_label->label[0] == MPLS_LABEL_IMPLNULL)
115	hdr.rtm_mpls = MPLS_OP_POP;
116	else
117	hdr.rtm_mpls = MPLS_OP_SWAP;
118	}
119
01b9e3fd DL	120	frr_elevate_privs(&zserv_privs) {
	121	ret = writev(kr_state.fd, iov, iovcnt);
	122	}
d62a17ae	123
d62a17ae	124	if (ret == -1)
09c866e3 QY	125	flog_err_sys(LIB_ERR_SOCKET, "%s: %s", __func__,
09c866e3 QY	126	safe_strerror(errno));
d62a17ae	127
d62a17ae	128	return ret;
e07486ce RW	129	}
	130
	131	#if !defined(ROUNDUP)
d62a17ae	132	#define ROUNDUP(a) \
d62a17ae	133	(((a) & (sizeof(long) - 1)) ? (1 + ((a) \| (sizeof(long) - 1))) : (a))
e07486ce RW	134	#endif
e07486ce RW	135
d62a17ae	136	static int kernel_send_rtmsg_v6(int action, mpls_label_t in_label,
d62a17ae	137	zebra_nhlfe_t *nhlfe)
e07486ce	138	{
d62a17ae	139	struct iovec iov[5];
	140	struct rt_msghdr hdr;
	141	struct sockaddr_mpls sa_label_in, sa_label_out;
	142	struct pad {
	143	struct sockaddr_in6 addr;
	144	char pad[sizeof(long)]; /* thank you IPv6 */
	145	} nexthop;
	146	int iovcnt = 0;
	147	int ret;
	148
	149	if (IS_ZEBRA_DEBUG_KERNEL)
	150	zlog_debug("%s: 0x%x, label=%u", __func__, action, in_label);
	151
	152	/* initialize header */
	153	memset(&hdr, 0, sizeof(hdr));
	154	hdr.rtm_version = RTM_VERSION;
	155
	156	hdr.rtm_type = action;
	157	hdr.rtm_flags = RTF_UP;
	158	hdr.rtm_fmask = RTF_MPLS;
	159	hdr.rtm_seq = kr_state.rtseq++; /* overflow doesn't matter */
	160	hdr.rtm_msglen = sizeof(hdr);
	161	hdr.rtm_hdrlen = sizeof(struct rt_msghdr);
	162	hdr.rtm_priority = 0;
	163	/* adjust iovec */
	164	iov[iovcnt].iov_base = &hdr;
	165	iov[iovcnt++].iov_len = sizeof(hdr);
	166
	167	/* in label */
	168	memset(&sa_label_in, 0, sizeof(sa_label_in));
	169	sa_label_in.smpls_len = sizeof(sa_label_in);
	170	sa_label_in.smpls_family = AF_MPLS;
	171	sa_label_in.smpls_label = htonl(in_label << MPLS_LABEL_OFFSET);
	172	/* adjust header */
	173	hdr.rtm_flags \|= RTF_MPLS \| RTF_MPATH;
	174	hdr.rtm_addrs \|= RTA_DST;
	175	hdr.rtm_msglen += sizeof(sa_label_in);
	176	/* adjust iovec */
	177	iov[iovcnt].iov_base = &sa_label_in;
	178	iov[iovcnt++].iov_len = sizeof(sa_label_in);
	179
	180	/* nexthop */
	181	memset(&nexthop, 0, sizeof(nexthop));
	182	nexthop.addr.sin6_len = sizeof(struct sockaddr_in6);
	183	nexthop.addr.sin6_family = AF_INET6;
	184	nexthop.addr.sin6_addr = nhlfe->nexthop->gate.ipv6;
	185	if (IN6_IS_ADDR_LINKLOCAL(&nexthop.addr.sin6_addr)) {
	186	uint16_t tmp16;
	187	struct sockaddr_in6 *sin6 = &nexthop.addr;
	188
	189	nexthop.addr.sin6_scope_id = nhlfe->nexthop->ifindex;
	190
	191	memcpy(&tmp16, &sin6->sin6_addr.s6_addr[2], sizeof(tmp16));
	192	tmp16 = htons(sin6->sin6_scope_id);
	193	memcpy(&sin6->sin6_addr.s6_addr[2], &tmp16, sizeof(tmp16));
	194	sin6->sin6_scope_id = 0;
	195	}
	196
	197	/* adjust header */
	198	hdr.rtm_flags \|= RTF_GATEWAY;
	199	hdr.rtm_addrs \|= RTA_GATEWAY;
	200	hdr.rtm_msglen += ROUNDUP(sizeof(struct sockaddr_in6));
	201	/* adjust iovec */
	202	iov[iovcnt].iov_base = &nexthop;
203	iov[iovcnt++].iov_len = ROUNDUP(sizeof(struct sockaddr_in6));
204
205	/* If action is RTM_DELETE we have to get rid of MPLS infos */
206	if (action != RTM_DELETE) {
207	memset(&sa_label_out, 0, sizeof(sa_label_out));
208	sa_label_out.smpls_len = sizeof(sa_label_out);
209	sa_label_out.smpls_family = AF_MPLS;
210	sa_label_out.smpls_label =
211	htonl(nhlfe->nexthop->nh_label->label[0]
212	<< MPLS_LABEL_OFFSET);
213	/* adjust header */
214	hdr.rtm_addrs \|= RTA_SRC;
215	hdr.rtm_flags \|= RTF_MPLS;
216	hdr.rtm_msglen += sizeof(sa_label_out);
217	/* adjust iovec */
218	iov[iovcnt].iov_base = &sa_label_out;
219	iov[iovcnt++].iov_len = sizeof(sa_label_out);
220
221	if (nhlfe->nexthop->nh_label->label[0] == MPLS_LABEL_IMPLNULL)
222	hdr.rtm_mpls = MPLS_OP_POP;
223	else
224	hdr.rtm_mpls = MPLS_OP_SWAP;
225	}
226
01b9e3fd DL	227	frr_elevate_privs(&zserv_privs) {
	228	ret = writev(kr_state.fd, iov, iovcnt);
	229	}
d62a17ae	230
d62a17ae	231	if (ret == -1)
09c866e3 QY	232	flog_err_sys(LIB_ERR_SOCKET, "%s: %s", __func__,
09c866e3 QY	233	safe_strerror(errno));
d62a17ae	234
d62a17ae	235	return ret;
d3e2c74a RW	236	}
d3e2c74a RW	237
d62a17ae	238	static int kernel_lsp_cmd(int action, zebra_lsp_t *lsp)
d3e2c74a	239	{
d62a17ae	240	zebra_nhlfe_t *nhlfe;
	241	struct nexthop *nexthop = NULL;
	242	unsigned int nexthop_num = 0;
	243
	244	for (nhlfe = lsp->nhlfe_list; nhlfe; nhlfe = nhlfe->next) {
	245	nexthop = nhlfe->nexthop;
	246	if (!nexthop)
	247	continue;
	248
	249	if (nexthop_num >= multipath_num)
	250	break;
	251
	252	if (((action == RTM_ADD \|\| action == RTM_CHANGE)
	253	&& (CHECK_FLAG(nhlfe->flags, NHLFE_FLAG_SELECTED)
	254	&& CHECK_FLAG(nexthop->flags, NEXTHOP_FLAG_ACTIVE)))
	255	\|\| (action == RTM_DELETE
	256	&& (CHECK_FLAG(nhlfe->flags, NHLFE_FLAG_INSTALLED)
	257	&& CHECK_FLAG(nexthop->flags, NEXTHOP_FLAG_FIB)))) {
5e8c8947 RW	258	if (nhlfe->nexthop->nh_label->num_labels > 1) {
	259	zlog_warn(
	260	"%s: can't push %u labels at once "
	261	"(maximum is 1)",
	262	__func__,
	263	nhlfe->nexthop->nh_label->num_labels);
	264	continue;
	265	}
	266
d62a17ae	267	nexthop_num++;
	268
	269	switch (NHLFE_FAMILY(nhlfe)) {
	270	case AF_INET:
	271	kernel_send_rtmsg_v4(action, lsp->ile.in_label,
	272	nhlfe);
	273	break;
	274	case AF_INET6:
	275	kernel_send_rtmsg_v6(action, lsp->ile.in_label,
	276	nhlfe);
	277	break;
	278	default:
	279	break;
	280	}
d62a17ae	281	}
	282	}
	283
	284	return (0);
d3e2c74a RW	285	}
d3e2c74a RW	286
7c5d0e18	287	enum dp_req_result kernel_add_lsp(zebra_lsp_t *lsp)
d3e2c74a	288	{
d62a17ae	289	int ret;
c4c8dec0	290
4a83e7a0	291	if (!lsp \|\| !lsp->best_nhlfe) { // unexpected
215181cb	292	kernel_lsp_pass_fail(lsp, DP_INSTALL_FAILURE);
7c5d0e18	293	return DP_REQUEST_FAILURE;
4a83e7a0	294	}
d3e2c74a	295
d62a17ae	296	ret = kernel_lsp_cmd(RTM_ADD, lsp);
c4c8dec0	297
4a83e7a0	298	kernel_lsp_pass_fail(lsp,
215181cb DS	299	(!ret) ? DP_INSTALL_SUCCESS
215181cb DS	300	: DP_INSTALL_FAILURE);
7c5d0e18 DS	301
7c5d0e18 DS	302	return DP_REQUEST_SUCCESS;
d3e2c74a RW	303	}
d3e2c74a RW	304
7c5d0e18	305	enum dp_req_result kernel_upd_lsp(zebra_lsp_t *lsp)
d3e2c74a	306	{
d62a17ae	307	int ret;
c4c8dec0	308
4a83e7a0	309	if (!lsp \|\| !lsp->best_nhlfe) { // unexpected
215181cb	310	kernel_lsp_pass_fail(lsp, DP_INSTALL_FAILURE);
7c5d0e18	311	return DP_REQUEST_FAILURE;
4a83e7a0	312	}
d3e2c74a	313
d62a17ae	314	ret = kernel_lsp_cmd(RTM_CHANGE, lsp);
c4c8dec0	315
4a83e7a0	316	kernel_lsp_pass_fail(lsp,
215181cb DS	317	(!ret) ? DP_INSTALL_SUCCESS
215181cb DS	318	: DP_INSTALL_FAILURE);
7c5d0e18	319	return DP_REQUEST_SUCCESS;
d3e2c74a RW	320	}
d3e2c74a RW	321
7c5d0e18	322	enum dp_req_result kernel_del_lsp(zebra_lsp_t *lsp)
d3e2c74a	323	{
d62a17ae	324	int ret;
c4c8dec0	325
4a83e7a0	326	if (!lsp) { // unexpected
215181cb	327	kernel_lsp_pass_fail(lsp, DP_DELETE_FAILURE);
7c5d0e18	328	return DP_REQUEST_FAILURE;
4a83e7a0	329	}
d3e2c74a	330
4a83e7a0	331	if (!CHECK_FLAG(lsp->flags, LSP_FLAG_INSTALLED)) {
215181cb	332	kernel_lsp_pass_fail(lsp, DP_DELETE_FAILURE);
7c5d0e18	333	return DP_REQUEST_FAILURE;
4a83e7a0	334	}
c4c8dec0	335
d62a17ae	336	ret = kernel_lsp_cmd(RTM_DELETE, lsp);
c4c8dec0	337
4a83e7a0	338	kernel_lsp_pass_fail(lsp,
215181cb DS	339	(!ret) ? DP_DELETE_SUCCESS
215181cb DS	340	: DP_DELETE_FAILURE);
7c5d0e18 DS	341
7c5d0e18 DS	342	return DP_REQUEST_SUCCESS;
d3e2c74a RW	343	}
d3e2c74a RW	344
d42127da RW	345	static int kmpw_install(struct zebra_pw *pw)
	346	{
	347	struct ifreq ifr;
	348	struct ifmpwreq imr;
	349	struct sockaddr_storage ss;
	350	struct sockaddr_in sa_in = (struct sockaddr_in )&ss;
	351	struct sockaddr_in6 sa_in6 = (struct sockaddr_in6 )&ss;
	352
	353	memset(&imr, 0, sizeof(imr));
	354	switch (pw->type) {
	355	case PW_TYPE_ETHERNET:
	356	imr.imr_type = IMR_TYPE_ETHERNET;
	357	break;
	358	case PW_TYPE_ETHERNET_TAGGED:
	359	imr.imr_type = IMR_TYPE_ETHERNET_TAGGED;
	360	break;
	361	default:
43e52561 QY	362	zlog_warn("%s: unhandled pseudowire type (%#X)", __func__,
43e52561 QY	363	pw->type);
d42127da RW	364	return -1;
	365	}
	366
	367	if (pw->flags & F_PSEUDOWIRE_CWORD)
	368	imr.imr_flags \|= IMR_FLAG_CONTROLWORD;
	369
	370	/* pseudowire nexthop */
	371	memset(&ss, 0, sizeof(ss));
	372	switch (pw->af) {
	373	case AF_INET:
	374	sa_in->sin_family = AF_INET;
	375	sa_in->sin_len = sizeof(struct sockaddr_in);
	376	sa_in->sin_addr = pw->nexthop.ipv4;
	377	break;
	378	case AF_INET6:
	379	sa_in6->sin6_family = AF_INET6;
	380	sa_in6->sin6_len = sizeof(struct sockaddr_in6);
	381	sa_in6->sin6_addr = pw->nexthop.ipv6;
	382	break;
	383	default:
43e52561 QY	384	zlog_warn("%s: unhandled pseudowire address-family (%u)",
43e52561 QY	385	__func__, pw->af);
d42127da RW	386	return -1;
	387	}
	388	memcpy(&imr.imr_nexthop, (struct sockaddr *)&ss,
	389	sizeof(imr.imr_nexthop));
	390
	391	/* pseudowire local/remote labels */
	392	imr.imr_lshim.shim_label = pw->local_label;
	393	imr.imr_rshim.shim_label = pw->remote_label;
	394
	395	/* ioctl */
	396	memset(&ifr, 0, sizeof(ifr));
	397	strlcpy(ifr.ifr_name, pw->ifname, sizeof(ifr.ifr_name));
	398	ifr.ifr_data = (caddr_t)&imr;
	399	if (ioctl(kr_state.ioctl_fd, SIOCSETMPWCFG, &ifr) == -1) {
09c866e3 QY	400	flog_err_sys(LIB_ERR_SYSTEM_CALL, "ioctl SIOCSETMPWCFG: %s",
09c866e3 QY	401	safe_strerror(errno));
d42127da RW	402	return -1;
	403	}
	404
	405	return 0;
	406	}
	407
	408	static int kmpw_uninstall(struct zebra_pw *pw)
	409	{
	410	struct ifreq ifr;
	411	struct ifmpwreq imr;
	412
	413	memset(&ifr, 0, sizeof(ifr));
	414	memset(&imr, 0, sizeof(imr));
	415	strlcpy(ifr.ifr_name, pw->ifname, sizeof(ifr.ifr_name));
	416	ifr.ifr_data = (caddr_t)&imr;
	417	if (ioctl(kr_state.ioctl_fd, SIOCSETMPWCFG, &ifr) == -1) {
09c866e3 QY	418	flog_err_sys(LIB_ERR_SYSTEM_CALL, "ioctl SIOCSETMPWCFG: %s",
09c866e3 QY	419	safe_strerror(errno));
d42127da RW	420	return -1;
	421	}
	422
	423	return 0;
	424	}
	425
d3e2c74a	426	#define MAX_RTSOCK_BUF 128 * 1024
d62a17ae	427	int mpls_kernel_init(void)
d3e2c74a	428	{
d62a17ae	429	int rcvbuf, default_rcvbuf;
	430	socklen_t optlen;
	431
	432	if ((kr_state.fd = socket(AF_ROUTE, SOCK_RAW, 0)) == -1) {
	433	zlog_warn("%s: socket", __func__);
	434	return -1;
	435	}
	436
d42127da RW	437	if ((kr_state.ioctl_fd = socket(AF_INET, SOCK_DGRAM \| SOCK_NONBLOCK, 0))
	438	== -1) {
	439	zlog_warn("%s: ioctl socket", __func__);
	440	return -1;
	441	}
	442
d62a17ae	443	/* grow receive buffer, don't wanna miss messages */
	444	optlen = sizeof(default_rcvbuf);
	445	if (getsockopt(kr_state.fd, SOL_SOCKET, SO_RCVBUF, &default_rcvbuf,
	446	&optlen)
	447	== -1)
	448	zlog_warn("kr_init getsockopt SOL_SOCKET SO_RCVBUF");
	449	else
	450	for (rcvbuf = MAX_RTSOCK_BUF;
	451	rcvbuf > default_rcvbuf
	452	&& setsockopt(kr_state.fd, SOL_SOCKET, SO_RCVBUF, &rcvbuf,
	453	sizeof(rcvbuf))
	454	== -1
	455	&& errno == ENOBUFS;
	456	rcvbuf /= 2)
	457	; /* nothing */
	458
	459	kr_state.rtseq = 1;
	460
d42127da RW	461	/* register hook to install/uninstall pseudowires */
	462	hook_register(pw_install, kmpw_install);
	463	hook_register(pw_uninstall, kmpw_uninstall);
	464
d62a17ae	465	return 0;
d3e2c74a	466	}
ddfeb486 DL	467
ddfeb486 DL	468	#endif /* OPEN_BSD */