[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 GNU Zebra; see the file COPYING.  If not, write to the Free
 * Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
 * 02111-1307, USA.
 */

#include <zebra.h>
#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"

extern struct zebra_privs_t zserv_privs;

struct {
	u_int32_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;
	}

	if (zserv_privs.change(ZPRIVS_RAISE))
		zlog_err("Can't raise privileges");
	ret = writev(kr_state.fd, iov, iovcnt);
	if (zserv_privs.change(ZPRIVS_LOWER))
		zlog_err("Can't lower privileges");

	if (ret == -1)
		zlog_err("%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;
	}

	if (zserv_privs.change(ZPRIVS_RAISE))
		zlog_err("Can't raise privileges");
	ret = writev(kr_state.fd, iov, iovcnt);
	if (zserv_privs.change(ZPRIVS_LOWER))
		zlog_err("Can't lower privileges");

	if (ret == -1)
		zlog_err("%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)))) {
			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;
			}
			if (action == RTM_ADD || action == RTM_CHANGE) {
				SET_FLAG(nhlfe->flags, NHLFE_FLAG_INSTALLED);
				SET_FLAG(nexthop->flags, NEXTHOP_FLAG_FIB);
			} else {
				UNSET_FLAG(nhlfe->flags, NHLFE_FLAG_INSTALLED);
				UNSET_FLAG(nexthop->flags, NEXTHOP_FLAG_FIB);
			}
		}
	}

	return (0);
}

int kernel_add_lsp(zebra_lsp_t *lsp)
{
	int ret;

	if (!lsp || !lsp->best_nhlfe) // unexpected
		return -1;

	UNSET_FLAG(lsp->flags, LSP_FLAG_CHANGED);
	ret = kernel_lsp_cmd(RTM_ADD, lsp);
	if (!ret)
		SET_FLAG(lsp->flags, LSP_FLAG_INSTALLED);

	return ret;
}

int kernel_upd_lsp(zebra_lsp_t *lsp)
{
	int ret;

	if (!lsp || !lsp->best_nhlfe) // unexpected
		return -1;

	UNSET_FLAG(lsp->flags, LSP_FLAG_CHANGED);
	UNSET_FLAG(lsp->flags, LSP_FLAG_INSTALLED);
	ret = kernel_lsp_cmd(RTM_CHANGE, lsp);
	if (!ret)
		SET_FLAG(lsp->flags, LSP_FLAG_INSTALLED);

	return ret;
}

int kernel_del_lsp(zebra_lsp_t *lsp)
{
	int ret;

	if (!lsp) // unexpected
		return -1;

	if (!CHECK_FLAG(lsp->flags, LSP_FLAG_INSTALLED))
		return -1;

	ret = kernel_lsp_cmd(RTM_DELETE, lsp);
	if (!ret)
		UNSET_FLAG(lsp->flags, LSP_FLAG_INSTALLED);

	return ret;
}

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