| /* |
| * 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; |
| } |
| |
| |