ASR_BASE
Change-Id: Icf3719cc0afe3eeb3edc7fa80a2eb5199ca9dda1
diff --git a/package/kernel/mfp/files/fp_ndisc.c b/package/kernel/mfp/files/fp_ndisc.c
new file mode 100644
index 0000000..e9e7dbc
--- /dev/null
+++ b/package/kernel/mfp/files/fp_ndisc.c
@@ -0,0 +1,257 @@
+/*
+ * 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;
+}
+
+