package/kernel/mfp/files/fp_ndisc.c

/*
 *	Fastpath Ndisc
 *
 *	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.
 */

#include <net/ndisc.h>
#include <net/ipv6.h>
#include <net/addrconf.h>
#include <net/ip6_route.h>

#include "fp_common.h"
#include "fp_device.h"
#include "fp_ndisc.h"

#define IN6ADDR_LINKLOCAL_ADDR	\
		{ { { 0xfe,0x80,0,0,0,0,0,0,0,0,0,0,0,0,0,1 } } }

const struct in6_addr in6addr_linklocal_addr = IN6ADDR_LINKLOCAL_ADDR;

static struct nd_opt_hdr *fpnd_next_option(struct nd_opt_hdr *cur,
					    struct nd_opt_hdr *end)
{
	int type;
	if (!cur || !end || cur >= end)
		return NULL;
	type = cur->nd_opt_type;
	do {
		cur = ((void *)cur) + (cur->nd_opt_len << 3);
	} while (cur < end && cur->nd_opt_type != type);
	return cur <= end && cur->nd_opt_type == type ? cur : NULL;
}

static inline bool fpnd_icmp6_type_eq(struct sk_buff *skb, u32 type)
{
	struct ipv6hdr *iph = (struct ipv6hdr *)skb->data;
	struct icmp6hdr *icmph = (struct icmp6hdr *)(iph + 1);

	if (likely(iph->nexthdr != IPPROTO_ICMPV6 ||
		   icmph->icmp6_code != 0 ||
		   icmph->icmp6_type != type)) {

		return false;
	}

	return true;
}

bool fpnd_is_ra(struct sk_buff *skb)
{
	return fpnd_icmp6_type_eq(skb, NDISC_ROUTER_ADVERTISEMENT);
}

bool fpnd_is_rs(struct sk_buff *skb)
{
	return fpnd_icmp6_type_eq(skb, NDISC_ROUTER_SOLICITATION);
}

static void fpnd_set_pref(struct fp_net_device *src, u8 *opt, int len)
{
	struct prefix_info *pinfo;
	__u32 valid_lft;
	__u32 prefered_lft;
	int addr_type;

	pinfo = (struct prefix_info *) opt;

	if (len < sizeof(struct prefix_info)) {
		pr_debug("prefix option too short\n");
		return;
	}

	addr_type = ipv6_addr_type(&pinfo->prefix);

	if (addr_type & (IPV6_ADDR_MULTICAST | IPV6_ADDR_LINKLOCAL))
		return;

	valid_lft = ntohl(pinfo->valid);
	prefered_lft = ntohl(pinfo->prefered);

	if (prefered_lft > valid_lft) {
		pr_debug("prefix option has invalid lifetime\n");
		return;
	}

	src->prefixlen = pinfo->prefix_len;
	memcpy(&src->gb6addr, &pinfo->prefix, sizeof(struct in6_addr));
	fpdev_set_gb6(src);

	pr_debug("prefix for dev (%s) is (%pI6c) len (%d), sending to USB\n",
		src->dev->name, &pinfo->prefix, pinfo->prefix_len);
}

void fpnd_process_ra(struct net_device *src, struct sk_buff *skb)
{
	struct fp_net_device *fpdev;
	struct ndisc_options ndopts;
	struct nd_opt_hdr *p;
	int optlen;
	struct ipv6hdr *iph = (struct ipv6hdr *)skb->data;
	struct ra_msg *ra_msg = (struct ra_msg *)(iph + 1);

	__u8 *opt = (__u8 *)(ra_msg + 1);

	optlen = (skb->tail - (u8 *)ra_msg) - sizeof(struct ra_msg);

	if (!(ipv6_addr_type(&iph->saddr) & IPV6_ADDR_LINKLOCAL)) {
		pr_debug("source address is not link-local\n");
		return;
	}

	if (optlen < 0) {
		pr_debug("packet too short\n");
		return;
	}

	if (!ndisc_parse_options(skb->dev, opt, optlen, &ndopts)) {
		pr_debug("invalid ND options\n");
		return;
	}

	fpdev = fpdev_get_if(src);
	if (unlikely(!fpdev))
		return;

	if (ndopts.nd_opts_mtu) {
		struct mtu_option *mtuinfo = (struct mtu_option *) ndopts.nd_opts_mtu;

		fpdev->mtu = ntohl(mtuinfo->mtu);
		fpdev_set_mtu(fpdev);
	}

	if (ndopts.nd_opts_pi) {
		for (p = ndopts.nd_opts_pi;
		     p;
		     p = fpnd_next_option(p, ndopts.nd_opts_pi_end)) {
			fpnd_set_pref(fpdev, (u8 *)p, (p->nd_opt_len) << 3);
		}
	}

	fpdev_put(fpdev);
}

static void fp_ip6_nd_hdr(struct sk_buff *skb,
			  const struct in6_addr *saddr,
			  const struct in6_addr *daddr,
			  int hop_limit, int len)
{
	struct ipv6hdr *hdr;

	skb_push(skb, sizeof(*hdr));
	skb_reset_network_header(skb);
	hdr = ipv6_hdr(skb);

	ip6_flow_hdr(hdr, 0, 0);

	hdr->payload_len = htons(len);
	hdr->nexthdr = IPPROTO_ICMPV6;
	hdr->hop_limit = hop_limit;

	hdr->saddr = *saddr;
	hdr->daddr = *daddr;
}

static struct sk_buff *fp_ndisc_alloc_skb(struct net_device *dev,
				       int len)
{
	int hlen = LL_RESERVED_SPACE(dev);
	int tlen = dev->needed_tailroom;
	struct sock *sk = dev_net(dev)->ipv6.ndisc_sk;
	struct sk_buff *skb;

	skb = alloc_skb(hlen + sizeof(struct ipv6hdr) + len + tlen, GFP_ATOMIC);
	if (!skb) {
		printk("ndisc: %s failed to allocate an skb\n", __func__);
		return NULL;
	}

	skb->protocol = htons(ETH_P_IPV6);
	skb->dev = dev;

	skb_reserve(skb, hlen + sizeof(struct ipv6hdr));
	skb_reset_transport_header(skb);

	/* Manually assign socket ownership as we avoid calling
	 * sock_alloc_send_pskb() to bypass wmem buffer limits
	 */
	skb_set_owner_w(skb, sk);

	return skb;
}

int fpnd_process_rs(struct sk_buff *skb)
{
	struct sk_buff *nskb;
	struct icmp6hdr *icmp6h;
	struct ra_msg *msg;
	struct prefix_info *prefix;
	struct mtu_option *mtu;
	struct fp_net_device *fpdev;
	int err;

	fpdev = fpdev_get_ccinet();
	if (unlikely(!fpdev))
		return 0;

	nskb = fp_ndisc_alloc_skb(fpdev->dev, sizeof(*msg) + sizeof(struct prefix_info) + sizeof(struct mtu_option));
	if (!nskb) {
		fpdev_put(fpdev);
		return 0;
	}

	msg = (struct ra_msg *)skb_put(nskb, sizeof(*msg));
	*msg = (struct ra_msg) {
		.icmph = {
			.icmp6_type = NDISC_ROUTER_ADVERTISEMENT,
			.icmp6_hop_limit = 0xFF,
			.icmp6_rt_lifetime = htons(0xFFFF),
		},
	};

	prefix = (struct prefix_info *)skb_put(nskb, sizeof(struct prefix_info));
	prefix->type = ND_OPT_PREFIX_INFO;
	prefix->length = 4;
	prefix->prefix_len = fpdev->prefixlen;
	prefix->autoconf = 1;
	prefix->valid = htonl(0xFFFFFFFF);
	prefix->prefered = htonl(0xFFFFFFFF);
	memcpy(&prefix->prefix, &fpdev->gb6addr, sizeof(struct in6_addr));

	mtu = (struct mtu_option *)skb_put(nskb, sizeof(struct mtu_option));
	mtu->type = ND_OPT_MTU;
	mtu->length = 1;
	mtu->mtu = htonl(fpdev->mtu);

	icmp6h = icmp6_hdr(nskb);
	icmp6h->icmp6_cksum = csum_ipv6_magic(&in6addr_linklocal_addr, &fpdev->ll6addr, nskb->len,
							 IPPROTO_ICMPV6,
							 csum_partial(icmp6h,
							 nskb->len, 0));

	fp_ip6_nd_hdr(nskb, &in6addr_linklocal_addr, &fpdev->ll6addr, 0xFF, nskb->len);
	err = netif_rx(nskb);
	fpdev_put(fpdev);

	if(!err) {
		dev_kfree_skb_any(skb);
		return 1;
	}

	return 0;
}