text: /*
   * Copyright (C) 2014 Daniele Di Proietto, Luigi Rizzo,
   *	Universita` di Pisa. All rights reserved.
   *
   * Redistribution and use in source and binary forms, with or without
   * modification, are permitted provided that the following conditions
   * are met:
   *
   *   1. Redistributions of source code must retain the above copyright
   *      notice, this list of conditions and the following disclaimer.
   *   2. Redistributions in binary form must reproduce the above copyright
   *      notice, this list of conditions and the following disclaimer in the
   *      documentation and/or other materials provided with the distribution.
   *
   * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
   * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
   * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
   * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
   * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
   * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
   * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
   * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
   * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
   * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
   * SUCH DAMAGE.
   */
  
  /*
   * $FreeBSD$
   */
  #include <sys/types.h>
  #include <sys/systm.h>
  #include <sys/syslog.h>
  #include <sys/libkern.h>
  #include <sys/param.h>
  #include <sys/lock.h>
  #include <sys/mutex.h>
  #include <sys/kernel.h>
  #include <sys/malloc.h>
  #include <sys/mbuf.h>
  
  #include <net/netlink.h>
  
  MALLOC_DEFINE(M_NETLINK, "netlink", "Memory used for netlink packets");
  
  /*
   * dispatch table for various netlink handlers
   */
  struct bsd_nl_fn nlp[MAX_LINKS];
  
  int
  bsd_nl_proto_check(int proto)
  {
  	if (proto < 0 || proto >= MAX_LINKS)
  		return EINVAL;
  	return (nlp[proto].rx == NULL) ? EAFNOSUPPORT : 0;
  }
  
  /* register a new netlink family */
  int
  bsd_nl_proto_reg(struct bsd_nl_fn *x)
  {
  	uint32_t id;
  
  	if (x == NULL) {
  		printf("cannot register NULL netlink proto\n");
  		return EINVAL;
  	}
  	id = x->id;
  	if (id >= MAX_LINKS) {
  		printf("netlink proto %d too high\n", id);
  		return EINVAL;
  	}
  	if (x->rx == NULL) {
  		printf("netlink rx fn for proto %d missing\n", id);
  		return EINVAL;
  	}
  	if (nlp[id].rx != NULL) {
  		printf("netlink proto %d already registered\n", id);
  	}
  	nlp[id].rx = x->rx;
  	nlp[id].id = id;
  	return 0;
  }
  
  // XXX compare with linux validate_nla
  /*
   * Minimum length of various netlink attribute types.
   * The number of types is small so we can use this array
   * (ideally we could even go down to uint8_t, no attribute
   * is longer than 16 bytes apparently.
   */
  static const uint16_t nla_attr_minlen[NLA_TYPE_MAX+1] = {
          [NLA_U8]        = sizeof(uint8_t),
          [NLA_U16]       = sizeof(uint16_t),
          [NLA_U32]       = sizeof(uint32_t),
          [NLA_U64]       = sizeof(uint64_t),
          [NLA_MSECS]     = sizeof(uint64_t),
          [NLA_NESTED]    = NLA_HDRLEN,
          [NLA_S8]        = sizeof(int8_t),
          [NLA_S16]       = sizeof(int16_t),
          [NLA_S32]       = sizeof(int32_t),
          [NLA_S64]       = sizeof(int64_t),
  };
  
  /*
   * XXX nla_type() is really a sequence number in the policy definition.
   */
  static int
  validate_nla(const struct nlattr *nla, int maxtype,
  	const struct nla_policy *policy)
  {
  	const struct nla_policy *pt;
  	uint16_t alen = (uint16_t)nla_len(nla);
  	/* len excluding header, caller guarantees alen >= 0 */
  	uint16_t i, plen, atype = (uint16_t)nla_type(nla);
  	/* we need to cast atype due to a buggy definition of nla_type() */
  	const uint8_t *data = nla_data(nla);
  
  	if (atype > maxtype)
  		return 0; /* invalid fields are skipped */
  
  	pt = &policy[atype];
  	if (pt->type > NLA_TYPE_MAX) /* error in the policy! */
  		return EINVAL;
  
  	plen = pt->len; /* how much space is allowed by policy ? */
          switch (pt->type) {
  	case NLA_FLAG:
  		return (alen != 0) ? ERANGE : 0; /* nla error */
  
  	case NLA_NUL_STRING: /* plen excludes the NUL */
  		/*
  		 * if plen>0, that is the limit (excluding NUL),
  		 * but never more than alen.
  		 */
  		if (plen && plen+1 < alen)
  			alen = plen + 1;
  		if (alen == 0)
  			return EINVAL;
  
  		/* Search for NUL termination */
  		for (i = 0; i < alen; i++) {
  			if (data[i] == 0)
  				return 0; /* all fine */
  		}
  		return EINVAL; /* missing terminator */
  
  	case NLA_STRING: /* non terminated strings */
  		if (alen == 0) /* empty strings not allowed */
  			return ERANGE;
  		if (plen) { /* check the limit */
  			if (data[alen - 1] == 0)
  				alen--;
  			if (alen > plen)
  				return ERANGE;
  		}
  		return 0;
  
  	case NLA_BINARY:
  		return (plen && alen > plen) ? ERANGE : 0;
  
  	case NLA_NESTED_COMPAT:
  		/* plen indicates minimum alen */
  		if (alen < plen) /* too short */
  			return ERANGE;
  		if (alen < NLA_ALIGN(plen)) /* ok (including padding */
  			break;
  		/* if alen >= plen we need another header */
  		if (alen < NLA_ALIGN(plen) + NLA_HDRLEN)
  			return ERANGE;
  		nla = (const struct nlattr *)(data + NLA_ALIGN(plen));
  		if (alen < NLA_ALIGN(plen) + NLA_HDRLEN + nla_len(nla))
  			return ERANGE;
  		break;
  
  	case NLA_NESTED:
  		/* a nested attributes is allowed to be empty; if it is not,
  		 * it must have a size of at least NLA_HDRLEN.
  		 * plen is not used.
  		 */
  		if (alen == 0)
  			break;
  		/* fallthrough */
  	default:
  		if (plen) {
  			if (alen < plen)
  				return ERANGE;
  		} else if (pt->type != NLA_UNSPEC) {
  			if (alen < nla_attr_minlen[pt->type])
  				return ERANGE;
  		} /* else pass */
          }
  
  	return 0;
  }
  
  
  /*
   * Parse a sequence of attributes, storing results into tb[].
   * We accept one attribute per type (overwring tb[t] in case
   * of duplicates (note that the type-space is 2^14)
   */
  int
  bsd_nla_parse(struct nlattr **tb, int maxtype,
  	const struct nlattr *head, int len, const struct nla_policy *policy)
  {
  	/* XXX: parse attributes carefully using policy */
  	const struct nlattr * nla;
  	int err;
  
  	memset(tb, 0, sizeof(struct nlattr *) * (maxtype + 1));
  
  	for (nla = head; (const char *)nla < (const char *)head + len &&
  		nla->nla_len >= sizeof(*nla);
  	     nla = (const struct nlattr *)((const char *)nla + NLMSG_ALIGN(nla->nla_len))) {
  		uint16_t t = (uint16_t)nla_type(nla); /* cast is safe */
  		if (t <= maxtype) {
  			if (policy) {
  				err = validate_nla(nla, maxtype, policy);
  				if (err != 0)
  					return err;
  			}
  			tb[t] = (struct nlattr *)(uintptr_t)nla;
  		}
  	}
  
  	return 0;
  }
  
  /*
   * Messages are created incrementally, and a lot of routines
   * assume contiguos buffers which are a bad fit for mbufs.
   *
   * Hence we build them this way:
   * - always make sure a contiguous buffer is available,
   *   pointed by _M_BUFSTART(m)
   *   and of length _M_BUFLEN(m)
   * - if possible make the contiguous buffer part of the existing
   *   mbuf so we do not need extra allocations.
   * - if the buffer is externally allocated, at the end
   *   it must be copied and freed.
   * - we track the used space in m_pkthdr.len
   */
  
  /*
   * Some mbuf pkthdr fields have special meaning when used with netlink:
   *
   *	FIELD	MACRO		DESCRIPTION
   *
   * 	rcvif	_M_BUFSTART()	linear buffer, possibly external
   *	fibnum	_M_BUFLEN()	linear buffer length
   *	len	--		current write offset
   *	flowid	NETLINK_CB()	src_portid, 32 bit
   *	rsstype	_M_NLPROTO()	protocol (NETLINK_GENERIC etc.)
   *
   * In more detail:
   *
   * 1. netlink messages are built incrementally in a linear buffer of
   * predefined size. This is a poor match with mbuf chains,
   * which may be split and have no predefined size.
   * Hence we use two mbuf fields (see above, _M_BUFSTART() and _M_BUFLEN() )
   * for the linear buffer pointer and max len,
   * and use m_pkthdr.len to record the space used so far
   * (remember of course to copy back and fix things before sending).
   * These are abstracted by the three macros _M_BUFSTARTA)(), _M_BUFLEN() and _M_CUR()
   * _M_BUFSTART() and _M_BUFLEN() are cleared when passing the buffer back to the kernel
   *
   * 2. The protocol is mapped through _MP(m) into rsstype, and is used
   * in netlink_input()
   *
   * 3. sk_buff have a 48-byte "control buffer" (cb) to carry aux info.
   * In netlink this is wrapped by NETLINK_CB() into a pointer to a
   * struct netlink_skb_parms.
   * We need at least the portid, the source port for
   * messages coming from userspace, which in turn is used to locate the
   * destination socket when building a reply.
   * Netlink messages do not need flowid, so we store the portid there.
   * 
   */
  
  
  #define _M_BUFSTART(m)  (m)->m_pkthdr.rcvif   /* linear buffer pointer */
  #define _M_BUFLEN(m)  ((m)->m_pkthdr.fibnum)  /* max length, 16 bit */
  #define _M_CUR(m)  ((char *)_M_BUFSTART(m) + (m)->m_pkthdr.len) /* cur data ptr */
  
  #define _M_NLPROTO(m)  ((m)->m_pkthdr.rsstype)  /* netlink proto, 8 bit */
  
  #define NETLINK_CB(_m)	(*(struct netlink_skb_parms*)&((_m)->m_pkthdr.flowid))
  
  
  /*
   * nlmsg_new() is part of the public API for netlink.
   * on linux it is just alloc_skb()
   *
   * Can be called with M_NOWAIT or M_WAITOK so we handle failures.
   *
   * Here we call m_getm() and if the buffer is not contiguous (only happens
   * when it is too large) we allocate an external buffer.
   * In all case set _M_BUFLEN(m) to the requested capacity.
   *
   * The 'put' messages always update m->m_pkthdr.len
   * XXX todo: handle M_NEGATIVE_ERROR
   */
  struct mbuf *
  nlmsg_new(size_t payload, int flags)
  {
  	uint32_t l = NLMSG_SPACE(payload);
  	struct mbuf *m = m_getm(NULL, l, flags, MT_DATA);
  	/* XXX could use M_TRAILINGSPACE(m) ? */
  	int space;
  
  	if (m == NULL)
  		return m;
  
  	m->m_flags |= (flags & M_NEGATIVE_ERROR);
  	space = (m->m_flags & M_EXT) ? m->m_ext.ext_size :
                          ((m->m_flags & M_PKTHDR) ? MHLEN : MLEN);
  
  	D("got %d bytes space %d at %p", l, space, _P32(m));
  	/* we really want a contiguous buffer,
  	 * so if we have some odd length we allocate one
  	 * and store the pointer in rcvif
  	 */
  	_M_BUFSTART(m) = (l <= space) ? mtod(m, void *) :
  		malloc(l, M_NETLINK, flags|M_ZERO);
  	if (_M_BUFSTART(m)) {
  		_M_BUFLEN(m) = l;
  	} else {
  		m_freem(m);
  		m = NULL;
  	}
  	return m;
  }
  
  /*
   * put a message, return a pointer to the info
   * XXX define in terms of nlmsg_put()
   */
  struct nlmsghdr *
  nlmsg_put(struct mbuf *m, uint32_t portid, uint32_t seq,
  	int type, int payload_len, int flags)
  {
  	struct nlmsghdr *nlh;
  	int len = nlmsg_msg_size(payload_len);
  	int need = NLMSG_ALIGN(len);
  
  	if (m->m_pkthdr.len + need > _M_BUFLEN(m))
  		return NULL;
  
  	nlh = (struct nlmsghdr *)_M_CUR(m);
  	m->m_pkthdr.len += need;
  	/* initialize all fields */
  	nlh->nlmsg_type = type;
  	nlh->nlmsg_len = len; /* without padding */
  	nlh->nlmsg_flags = flags;
  	nlh->nlmsg_seq = seq;
  	nlh->nlmsg_pid = portid;
  	return nlh;
  }
  
  /*
   * create a msg with nlmsghdr; nlmsgerr; error
   * in case of error
   */
  static void
  netlink_ack(uint8_t proto, uint32_t portid, struct nlmsghdr * nlmsg, int err)
  {
  	struct mbuf *m;
  	struct nlmsghdr * repnlh;
  
  	struct nlmsgerr *errmsg;
  	int payloadsize = sizeof(*errmsg);
  
  	if (err)
  		payloadsize += nlmsg->nlmsg_len;
  
  	D("starting, err %d len %d", err, payloadsize);
  	m = nlmsg_new(payloadsize, M_WAITOK);
  	_M_NLPROTO(m) = proto; /* this is used later in netlink_input() */
  	/* XXX we don't set NETLINK_CB() */
  
  	// XXX nlmsg_put()
  	repnlh = (struct nlmsghdr *)_M_CUR(m);
  	repnlh->nlmsg_type = NLMSG_ERROR;
  	repnlh->nlmsg_flags = 0;
  	repnlh->nlmsg_seq = nlmsg->nlmsg_seq;
  	repnlh->nlmsg_pid = portid;
  	m->m_pkthdr.len += NLMSG_HDRLEN;
  
  	// XXX unclear how much i should copy
  	errmsg = (struct nlmsgerr *)_M_CUR(m);
  	errmsg->error = err;
  #if 0 // XXX check
  	m->m_pkthdr.len +=
  		NLMSG_ALIGN(err ?
  		nlmsg->nlmsg_len + sizeof(*errmsg) - sizeof(*nlmsg):
  		sizeof(*errmsg));
  	/* In case of error copy the whole message */
  	memcpy(&errmsg->msg, nlmsg, err ? nlmsg->nlmsg_len : sizeof(*nlmsg));
  #endif
  
  	nlmsg_end(m, repnlh);
  
  	/* I should call unicast, but it's the same */
  	nlmsg_reply(m, NULL);
  }
  
  /*
   * message from a netlink socket (coming from userspace)
   */
  int
  netlink_receive_packet(struct mbuf *m, struct socket *so, int proto)
  {
  	char *buf = NULL;
  	int datalen = 0, ofs = 0;
  	int pktlen = m_length(m, NULL);
  	int (*cb)(char *data) = nlp[proto].rx;
  
  	/*
  	 * Since we don't have a guaranteed linear buffer, we copy to
  	 * a locally allocared one.
  	 */
  	while (ofs + NLMSG_HDRLEN <= pktlen) {
  		struct nlmsghdr hdr, *h = &hdr;
  		int l, err = 0;
  		/* Copy the header */
  		m_copydata(m, ofs, sizeof(hdr), (caddr_t)h);
  
  		/* exit if msg too small or exceeds the packet length */
  		l = h->nlmsg_len;
  		D("ofs %d read %d, msglen %d", ofs, NLMSG_HDRLEN, l);
  		if (l < NLMSG_HDRLEN || ofs + l > pktlen) {
  			D("bad len %d ofs %d pktlen %d", l, ofs, pktlen);
  			break; /* also free if needed */
  		}
  		if (l > datalen) { /* must reallocate */
  			int old_l = datalen;
  			(void)old_l;
  			datalen = (l + 0x3ff) & ~0x3ff; /* round to 1k */
  			D("reallocate buffer %d -> %d [%d]",
  				old_l, l, datalen);
  			if (buf != NULL) {
  				free(buf, M_NETLINK);
  			}
  			// XXX can i use M_WAIT ?
  			buf = malloc(datalen, M_NETLINK, M_NOWAIT|M_ZERO);
  			if (buf == NULL) /* just return, already freed */ {
  				D("malloc failed, exit");
  				return ENOMEM;
  			}
  		}
  		ND("Copy message, including header");
  		m_copydata(m, ofs, l, buf);
  		h = (struct nlmsghdr *)buf;
  		if (h->nlmsg_flags & NLM_F_REQUEST &&
  		    h->nlmsg_type >= NLMSG_MIN_TYPE) {
  			D("msg_type %d callback %p len %d",
  				h->nlmsg_type, _P32(cb), l);
  			err = cb((void *)h);
  			D("callback returns %d", err);
  		}
  
  		if (err != EINTR && (h->nlmsg_flags & NLM_F_ACK || err != 0))
  			netlink_ack(_M_NLPROTO(m), NETLINK_CB(m).portid, h, err);
  
  		ofs += NLMSG_ALIGN(l);
  		D("ack done, now moving to %d/%d", ofs, pktlen);
  	}
  	D("done, freeing %p", _P32(buf));
  	if (buf != NULL)
  		free(buf, M_NETLINK);
  	return 0;
  }
  
  
  /*
   * netlink attribute manipulation
   */
  int
  nla_put(struct mbuf *m, int attrtype, int attrlen, const void *data)
  {
  	struct nlattr *nla;
  	size_t totlen = NLMSG_ALIGN(NLA_HDRLEN + attrlen);
  	size_t need = NLMSG_ALIGN(totlen);
  
  	if (m->m_pkthdr.len + need > _M_BUFLEN(m))
  		return (m->m_flags & M_NEGATIVE_ERROR) ? -EMSGSIZE : EMSGSIZE;
  	nla = (struct nlattr *)_M_CUR(m);
  	bzero(nla, NLA_HDRLEN);
  	nla->nla_len = totlen;
  	nla->nla_type = attrtype;
  	m->m_pkthdr.len += NLA_HDRLEN;
  	if (attrlen > 0) {
  		uint32_t pad = need - totlen;
  		bcopy(data, _M_CUR(m), attrlen);
  		m->m_pkthdr.len += attrlen;
  		if (pad) { /* add padding */
  			bzero(_M_CUR(m), pad);
  			m->m_pkthdr.len += pad;
  		}
  	}
  	return 0;
  }
  
  /*
   * add an attribute, return the starting position, used to set
   * the total length or cancel the operation
   */
  struct nlattr *
  nla_nest_start(struct mbuf *m, int attrtype)
  {
  	// XXX we should return the starting of the attribute after
  	// possibly moving
  	struct nlattr *start = (struct nlattr *)_M_CUR(m);
  
  	if (nla_put(m, attrtype, 0, NULL) < 0)
  		return NULL;
  
  	return start;
  }
  
  /*
   * update the total attribute's length
   */
  int
  nla_nest_end(struct mbuf *m, struct nlattr *start)
  {
  	start->nla_len = _M_CUR(m) - (char *)start;
  	return m->m_pkthdr.len;
  }
  
  /*
   * reset the length to the 'start' argument
   * This is defined here because _M_BUFSTART() will eventually be
   * valid only in this source.
   */
  void
  nlmsg_trim(struct mbuf *m, const void * start)
  {
  	// XXX validity checks ?
  	m->m_pkthdr.len = (char *)start - (char *)_M_BUFSTART(m);
  }
  
  /*
   * copy initial part of the attributes, reserve space
   */
  struct
  nlattr *nla_reserve(struct mbuf *m, int attrtype, int attrlen)
  {
  	size_t totlen = NLA_HDRLEN + attrlen;
  	struct nlattr * nla;
  	int space = _M_BUFLEN(m) - m->m_pkthdr.len;
  	int want = NLMSG_ALIGN(totlen);
  
  	if (space < want)
  		return NULL;
  
  	nla = (struct nlattr *)((char *)_M_BUFSTART(m) + m->m_pkthdr.len);
  	nla->nla_len = totlen;
  	nla->nla_type = attrtype;
  
  	memset((char *) nla + totlen, 0, want - totlen);
  
  	m->m_pkthdr.len += want;
  
  	return nla;
  }
  
  /*
   * update the total message length
   * Note, the copy from the external buffer is done in nlmsg_reply()
   */
  int
  nlmsg_end(struct mbuf *m, struct nlmsghdr *nlh)
  {
  	nlh->nlmsg_len = _M_CUR(m) - (char *)nlh;
  	return m->m_pkthdr.len;
  }
  
  // XXX why do we ignore the info ?
  int
  nlmsg_reply(struct mbuf *m, struct genl_info *info)
  {
  	int l = m->m_pkthdr.len;
  
  	D("have %d bytes out of %d", l, _M_BUFLEN(m));
  	if ((char *)_M_BUFSTART(m) == mtod(m, char *)) { /* simple, plain mbuf */
  		m->m_len = l; /* update this mbuf's len */
  	} else { /* external buffer */
  		m_copyback(m, 0, l, (char *)_M_BUFSTART(m));
  		D("copy data from external buffer, plen %d len %d",
  			m->m_pkthdr.len, m->m_len);
  		free(_M_BUFSTART(m), M_NETLINK);
  	};
  	/* clear extra fields */
  	_M_BUFSTART(m) = NULL;
  	_M_BUFLEN(m) = 0;
  	/* XXX we still need MP() */
  	return bsd_netlink_send_msg(NULL, m);
  }
  