Initail commit
This commit is contained in:
tuna2134
520
etherip6.c
Normal file
520
etherip6.c
Normal file
@@ -0,0 +1,520 @@
|
||||
// SPDX-License-Identifier: GPL-2.0
|
||||
#include <linux/etherdevice.h>
|
||||
#include <linux/if_ether.h>
|
||||
#include <linux/if_link.h>
|
||||
#include <linux/in6.h>
|
||||
#include <linux/ip.h>
|
||||
#include <linux/ipv6.h>
|
||||
#include <linux/kernel.h>
|
||||
#include <linux/list.h>
|
||||
#include <linux/module.h>
|
||||
#include <linux/netdevice.h>
|
||||
#include <linux/netlink.h>
|
||||
#include <linux/percpu.h>
|
||||
#include <linux/rculist.h>
|
||||
#include <linux/rtnetlink.h>
|
||||
#include <linux/skbuff.h>
|
||||
#include <net/ip6_route.h>
|
||||
#include <net/ipv6.h>
|
||||
#include <net/net_namespace.h>
|
||||
#include <net/netns/generic.h>
|
||||
#include <net/protocol.h>
|
||||
#include <net/rtnetlink.h>
|
||||
|
||||
#include "etherip6_uapi.h"
|
||||
|
||||
#ifndef IPPROTO_ETHERIP
|
||||
#define IPPROTO_ETHERIP 97
|
||||
#endif
|
||||
|
||||
#define ETHERIP6_VERSION 3
|
||||
#define ETHERIP6_HDR htons(ETHERIP6_VERSION << 12)
|
||||
#define ETHERIP6_HLEN 2
|
||||
#define ETHERIP6_DEFAULT_HOP_LIMIT 64
|
||||
|
||||
struct etherip6_tunnel {
|
||||
struct list_head list;
|
||||
struct net_device *dev;
|
||||
struct in6_addr local;
|
||||
struct in6_addr remote;
|
||||
u32 link;
|
||||
u8 hop_limit;
|
||||
struct pcpu_sw_netstats __percpu *stats;
|
||||
atomic_long_t tx_dropped;
|
||||
atomic_long_t rx_dropped;
|
||||
};
|
||||
|
||||
struct etherip6_net {
|
||||
struct list_head tunnels;
|
||||
rwlock_t lock;
|
||||
};
|
||||
|
||||
static unsigned int etherip6_net_id;
|
||||
|
||||
static const struct nla_policy etherip6_policy[IFLA_ETHERIP6_MAX + 1] = {
|
||||
[IFLA_ETHERIP6_LOCAL] = NLA_POLICY_EXACT_LEN(sizeof(struct in6_addr)),
|
||||
[IFLA_ETHERIP6_REMOTE] = NLA_POLICY_EXACT_LEN(sizeof(struct in6_addr)),
|
||||
[IFLA_ETHERIP6_LINK] = { .type = NLA_U32 },
|
||||
[IFLA_ETHERIP6_HOP_LIMIT] = { .type = NLA_U8 },
|
||||
};
|
||||
|
||||
static struct etherip6_net *etherip6_pernet(struct net *net)
|
||||
{
|
||||
return net_generic(net, etherip6_net_id);
|
||||
}
|
||||
|
||||
static bool etherip6_addr_equal(const struct in6_addr *a,
|
||||
const struct in6_addr *b)
|
||||
{
|
||||
return ipv6_addr_equal(a, b);
|
||||
}
|
||||
|
||||
static struct etherip6_tunnel *etherip6_lookup(struct net *net,
|
||||
const struct in6_addr *local,
|
||||
const struct in6_addr *remote,
|
||||
int iif, bool running_only)
|
||||
{
|
||||
struct etherip6_net *en = etherip6_pernet(net);
|
||||
struct etherip6_tunnel *tun;
|
||||
|
||||
list_for_each_entry(tun, &en->tunnels, list) {
|
||||
if (running_only && !netif_running(tun->dev))
|
||||
continue;
|
||||
if (tun->link && tun->link != iif)
|
||||
continue;
|
||||
if (!etherip6_addr_equal(&tun->local, local))
|
||||
continue;
|
||||
if (!etherip6_addr_equal(&tun->remote, remote))
|
||||
continue;
|
||||
return tun;
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static struct etherip6_tunnel *
|
||||
etherip6_lookup_unique_rx(struct net *net, const struct in6_addr *local,
|
||||
const struct in6_addr *remote, int iif,
|
||||
bool match_local, bool match_remote)
|
||||
{
|
||||
struct etherip6_net *en = etherip6_pernet(net);
|
||||
struct etherip6_tunnel *tun, *match = NULL;
|
||||
|
||||
list_for_each_entry(tun, &en->tunnels, list) {
|
||||
if (!netif_running(tun->dev))
|
||||
continue;
|
||||
if (tun->link && tun->link != iif)
|
||||
continue;
|
||||
if (match_local && !etherip6_addr_equal(&tun->local, local))
|
||||
continue;
|
||||
if (match_remote && !etherip6_addr_equal(&tun->remote, remote))
|
||||
continue;
|
||||
if (match)
|
||||
return NULL;
|
||||
match = tun;
|
||||
}
|
||||
|
||||
return match;
|
||||
}
|
||||
|
||||
static struct etherip6_tunnel *etherip6_lookup_rx(struct net *net,
|
||||
const struct in6_addr *local,
|
||||
const struct in6_addr *remote,
|
||||
int iif)
|
||||
{
|
||||
struct etherip6_tunnel *tun;
|
||||
|
||||
tun = etherip6_lookup(net, local, remote, iif, true);
|
||||
if (tun)
|
||||
return tun;
|
||||
|
||||
tun = etherip6_lookup_unique_rx(net, local, remote, iif, false, true);
|
||||
if (tun)
|
||||
return tun;
|
||||
|
||||
tun = etherip6_lookup_unique_rx(net, local, remote, iif, true, false);
|
||||
if (tun)
|
||||
return tun;
|
||||
|
||||
return etherip6_lookup_unique_rx(net, local, remote, iif, false, false);
|
||||
}
|
||||
|
||||
static netdev_tx_t etherip6_xmit(struct sk_buff *skb, struct net_device *dev)
|
||||
{
|
||||
struct etherip6_tunnel *tun = netdev_priv(dev);
|
||||
struct net *net = dev_net(dev);
|
||||
struct dst_entry *dst;
|
||||
struct flowi6 fl6 = {};
|
||||
struct ipv6hdr *ip6h;
|
||||
struct pcpu_sw_netstats *stats;
|
||||
__be16 *etherip;
|
||||
unsigned int payload_len;
|
||||
int headroom;
|
||||
int err;
|
||||
|
||||
if (skb->len + ETHERIP6_HLEN > IPV6_MAXPLEN)
|
||||
goto tx_error;
|
||||
|
||||
fl6.flowi6_proto = IPPROTO_ETHERIP;
|
||||
fl6.flowi6_oif = tun->link;
|
||||
fl6.daddr = tun->remote;
|
||||
fl6.saddr = tun->local;
|
||||
|
||||
dst = ip6_route_output(net, NULL, &fl6);
|
||||
if (dst->error) {
|
||||
dst_release(dst);
|
||||
goto tx_error;
|
||||
}
|
||||
|
||||
headroom = LL_RESERVED_SPACE(dst->dev) + sizeof(*ip6h) + ETHERIP6_HLEN;
|
||||
err = skb_cow_head(skb, headroom);
|
||||
if (err) {
|
||||
dst_release(dst);
|
||||
goto tx_error;
|
||||
}
|
||||
|
||||
payload_len = skb->len + ETHERIP6_HLEN;
|
||||
|
||||
etherip = skb_push(skb, ETHERIP6_HLEN);
|
||||
*etherip = ETHERIP6_HDR;
|
||||
|
||||
skb_push(skb, sizeof(*ip6h));
|
||||
skb_reset_network_header(skb);
|
||||
ip6h = ipv6_hdr(skb);
|
||||
ip6_flow_hdr(ip6h, 0, 0);
|
||||
ip6h->payload_len = htons(payload_len);
|
||||
ip6h->nexthdr = IPPROTO_ETHERIP;
|
||||
ip6h->hop_limit = tun->hop_limit;
|
||||
ip6h->saddr = tun->local;
|
||||
ip6h->daddr = tun->remote;
|
||||
|
||||
skb->protocol = htons(ETH_P_IPV6);
|
||||
skb->dev = dst->dev;
|
||||
skb->ignore_df = 1;
|
||||
skb_dst_set(skb, dst);
|
||||
|
||||
stats = this_cpu_ptr(tun->stats);
|
||||
u64_stats_update_begin(&stats->syncp);
|
||||
u64_stats_inc(&stats->tx_packets);
|
||||
u64_stats_add(&stats->tx_bytes, payload_len - ETHERIP6_HLEN);
|
||||
u64_stats_update_end(&stats->syncp);
|
||||
|
||||
return ip6_local_out(net, NULL, skb) == 0 ? NETDEV_TX_OK : NETDEV_TX_OK;
|
||||
|
||||
tx_error:
|
||||
atomic_long_inc(&tun->tx_dropped);
|
||||
dev_kfree_skb(skb);
|
||||
return NETDEV_TX_OK;
|
||||
}
|
||||
|
||||
static int etherip6_open(struct net_device *dev)
|
||||
{
|
||||
netif_start_queue(dev);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int etherip6_stop(struct net_device *dev)
|
||||
{
|
||||
netif_stop_queue(dev);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void etherip6_get_stats64(struct net_device *dev,
|
||||
struct rtnl_link_stats64 *stats)
|
||||
{
|
||||
struct etherip6_tunnel *tun = netdev_priv(dev);
|
||||
|
||||
dev_fetch_sw_netstats(stats, tun->stats);
|
||||
stats->tx_dropped = atomic_long_read(&tun->tx_dropped);
|
||||
stats->rx_dropped = atomic_long_read(&tun->rx_dropped);
|
||||
}
|
||||
|
||||
static void etherip6_uninit(struct net_device *dev);
|
||||
|
||||
static const struct net_device_ops etherip6_netdev_ops = {
|
||||
.ndo_open = etherip6_open,
|
||||
.ndo_stop = etherip6_stop,
|
||||
.ndo_start_xmit = etherip6_xmit,
|
||||
.ndo_get_stats64 = etherip6_get_stats64,
|
||||
.ndo_uninit = etherip6_uninit,
|
||||
};
|
||||
|
||||
static void etherip6_setup(struct net_device *dev)
|
||||
{
|
||||
ether_setup(dev);
|
||||
dev->netdev_ops = ðerip6_netdev_ops;
|
||||
dev->needs_free_netdev = true;
|
||||
dev->type = ARPHRD_ETHER;
|
||||
dev->flags |= IFF_NOARP;
|
||||
dev->features &= ~(NETIF_F_GSO_MASK | NETIF_F_CSUM_MASK);
|
||||
dev->hw_features = 0;
|
||||
dev->vlan_features = 0;
|
||||
dev->min_mtu = ETH_MIN_MTU;
|
||||
dev->max_mtu = ETH_DATA_LEN;
|
||||
eth_hw_addr_random(dev);
|
||||
}
|
||||
|
||||
static int etherip6_validate(struct nlattr *tb[], struct nlattr *data[],
|
||||
struct netlink_ext_ack *extack)
|
||||
{
|
||||
if (!data)
|
||||
return -EINVAL;
|
||||
if (!data[IFLA_ETHERIP6_LOCAL]) {
|
||||
NL_SET_ERR_MSG(extack, "local IPv6 address is required");
|
||||
return -EINVAL;
|
||||
}
|
||||
if (!data[IFLA_ETHERIP6_REMOTE]) {
|
||||
NL_SET_ERR_MSG(extack, "remote IPv6 address is required");
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int etherip6_newlink(struct net_device *dev,
|
||||
struct rtnl_newlink_params *params,
|
||||
struct netlink_ext_ack *extack)
|
||||
{
|
||||
struct nlattr **data = params->data;
|
||||
struct etherip6_tunnel *tun = netdev_priv(dev);
|
||||
struct etherip6_net *en = etherip6_pernet(dev_net(dev));
|
||||
struct net_device *link_dev;
|
||||
struct etherip6_tunnel *old;
|
||||
int err;
|
||||
|
||||
nla_memcpy(&tun->local, data[IFLA_ETHERIP6_LOCAL], sizeof(tun->local));
|
||||
nla_memcpy(&tun->remote, data[IFLA_ETHERIP6_REMOTE], sizeof(tun->remote));
|
||||
|
||||
if (data[IFLA_ETHERIP6_LINK])
|
||||
tun->link = nla_get_u32(data[IFLA_ETHERIP6_LINK]);
|
||||
if (data[IFLA_ETHERIP6_HOP_LIMIT])
|
||||
tun->hop_limit = nla_get_u8(data[IFLA_ETHERIP6_HOP_LIMIT]);
|
||||
else
|
||||
tun->hop_limit = ETHERIP6_DEFAULT_HOP_LIMIT;
|
||||
|
||||
if (ipv6_addr_any(&tun->local) || ipv6_addr_any(&tun->remote)) {
|
||||
NL_SET_ERR_MSG(extack, "local and remote must be non-zero IPv6 addresses");
|
||||
return -EINVAL;
|
||||
}
|
||||
if (ipv6_addr_is_multicast(&tun->remote)) {
|
||||
NL_SET_ERR_MSG(extack, "remote must not be multicast");
|
||||
return -EINVAL;
|
||||
}
|
||||
if (!tun->hop_limit) {
|
||||
NL_SET_ERR_MSG(extack, "hop limit must be non-zero");
|
||||
return -EINVAL;
|
||||
}
|
||||
if (tun->link) {
|
||||
link_dev = __dev_get_by_index(dev_net(dev), tun->link);
|
||||
if (!link_dev) {
|
||||
NL_SET_ERR_MSG(extack, "link interface does not exist");
|
||||
return -ENODEV;
|
||||
}
|
||||
}
|
||||
if ((ipv6_addr_type(&tun->local) & IPV6_ADDR_LINKLOCAL ||
|
||||
ipv6_addr_type(&tun->remote) & IPV6_ADDR_LINKLOCAL) && !tun->link) {
|
||||
NL_SET_ERR_MSG(extack, "link-local endpoint requires link");
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
tun->dev = dev;
|
||||
atomic_long_set(&tun->tx_dropped, 0);
|
||||
atomic_long_set(&tun->rx_dropped, 0);
|
||||
tun->stats = netdev_alloc_pcpu_stats(struct pcpu_sw_netstats);
|
||||
if (!tun->stats)
|
||||
return -ENOMEM;
|
||||
|
||||
write_lock_bh(&en->lock);
|
||||
old = etherip6_lookup(dev_net(dev), &tun->local, &tun->remote,
|
||||
tun->link, false);
|
||||
if (old)
|
||||
err = -EEXIST;
|
||||
else
|
||||
list_add_rcu(&tun->list, &en->tunnels);
|
||||
write_unlock_bh(&en->lock);
|
||||
|
||||
if (old) {
|
||||
free_percpu(tun->stats);
|
||||
NL_SET_ERR_MSG(extack, "duplicate local/remote/link tunnel");
|
||||
return err;
|
||||
}
|
||||
|
||||
err = register_netdevice(dev);
|
||||
if (err) {
|
||||
write_lock_bh(&en->lock);
|
||||
list_del_rcu(&tun->list);
|
||||
write_unlock_bh(&en->lock);
|
||||
synchronize_rcu();
|
||||
free_percpu(tun->stats);
|
||||
return err;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void etherip6_dellink(struct net_device *dev, struct list_head *head)
|
||||
{
|
||||
unregister_netdevice_queue(dev, head);
|
||||
}
|
||||
|
||||
static void etherip6_uninit(struct net_device *dev)
|
||||
{
|
||||
struct etherip6_tunnel *tun = netdev_priv(dev);
|
||||
struct etherip6_net *en = etherip6_pernet(dev_net(dev));
|
||||
|
||||
write_lock_bh(&en->lock);
|
||||
list_del_rcu(&tun->list);
|
||||
write_unlock_bh(&en->lock);
|
||||
synchronize_rcu();
|
||||
free_percpu(tun->stats);
|
||||
}
|
||||
|
||||
static size_t etherip6_get_size(const struct net_device *dev)
|
||||
{
|
||||
return nla_total_size(sizeof(struct in6_addr)) +
|
||||
nla_total_size(sizeof(struct in6_addr)) +
|
||||
nla_total_size(sizeof(u32)) +
|
||||
nla_total_size(sizeof(u8));
|
||||
}
|
||||
|
||||
static int etherip6_fill_info(struct sk_buff *skb, const struct net_device *dev)
|
||||
{
|
||||
const struct etherip6_tunnel *tun = netdev_priv(dev);
|
||||
|
||||
if (nla_put(skb, IFLA_ETHERIP6_LOCAL, sizeof(tun->local), &tun->local) ||
|
||||
nla_put(skb, IFLA_ETHERIP6_REMOTE, sizeof(tun->remote), &tun->remote) ||
|
||||
nla_put_u32(skb, IFLA_ETHERIP6_LINK, tun->link) ||
|
||||
nla_put_u8(skb, IFLA_ETHERIP6_HOP_LIMIT, tun->hop_limit))
|
||||
return -EMSGSIZE;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static struct rtnl_link_ops etherip6_link_ops __read_mostly = {
|
||||
.kind = "etherip6",
|
||||
.priv_size = sizeof(struct etherip6_tunnel),
|
||||
.setup = etherip6_setup,
|
||||
.maxtype = IFLA_ETHERIP6_MAX,
|
||||
.policy = etherip6_policy,
|
||||
.validate = etherip6_validate,
|
||||
.newlink = etherip6_newlink,
|
||||
.dellink = etherip6_dellink,
|
||||
.get_size = etherip6_get_size,
|
||||
.fill_info = etherip6_fill_info,
|
||||
};
|
||||
|
||||
static int etherip6_rcv(struct sk_buff *skb)
|
||||
{
|
||||
struct net *net = dev_net(skb->dev);
|
||||
const struct ipv6hdr *ip6h = ipv6_hdr(skb);
|
||||
struct etherip6_tunnel *tun;
|
||||
struct pcpu_sw_netstats *stats;
|
||||
__be16 etherip;
|
||||
int iif = skb->dev->ifindex;
|
||||
|
||||
if (!pskb_may_pull(skb, ETHERIP6_HLEN))
|
||||
goto drop;
|
||||
|
||||
etherip = *(__be16 *)skb->data;
|
||||
if (etherip != ETHERIP6_HDR)
|
||||
goto drop;
|
||||
|
||||
read_lock_bh(ðerip6_pernet(net)->lock);
|
||||
tun = etherip6_lookup_rx(net, &ip6h->daddr, &ip6h->saddr, iif);
|
||||
if (tun)
|
||||
dev_hold(tun->dev);
|
||||
read_unlock_bh(ðerip6_pernet(net)->lock);
|
||||
|
||||
if (!tun)
|
||||
goto drop;
|
||||
|
||||
skb_pull(skb, ETHERIP6_HLEN);
|
||||
if (!pskb_may_pull(skb, ETH_HLEN)) {
|
||||
atomic_long_inc(&tun->rx_dropped);
|
||||
dev_put(tun->dev);
|
||||
goto drop;
|
||||
}
|
||||
|
||||
skb->dev = tun->dev;
|
||||
skb_scrub_packet(skb, true);
|
||||
skb_reset_mac_header(skb);
|
||||
skb->protocol = eth_type_trans(skb, tun->dev);
|
||||
skb->pkt_type = PACKET_HOST;
|
||||
skb_reset_network_header(skb);
|
||||
|
||||
stats = this_cpu_ptr(tun->stats);
|
||||
u64_stats_update_begin(&stats->syncp);
|
||||
u64_stats_inc(&stats->rx_packets);
|
||||
u64_stats_add(&stats->rx_bytes, skb->len);
|
||||
u64_stats_update_end(&stats->syncp);
|
||||
|
||||
netif_rx(skb);
|
||||
dev_put(tun->dev);
|
||||
return 0;
|
||||
|
||||
drop:
|
||||
kfree_skb(skb);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static const struct inet6_protocol etherip6_protocol = {
|
||||
.handler = etherip6_rcv,
|
||||
.flags = INET6_PROTO_NOPOLICY | INET6_PROTO_FINAL,
|
||||
};
|
||||
|
||||
static int __net_init etherip6_net_init(struct net *net)
|
||||
{
|
||||
struct etherip6_net *en = etherip6_pernet(net);
|
||||
|
||||
INIT_LIST_HEAD(&en->tunnels);
|
||||
rwlock_init(&en->lock);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static struct pernet_operations etherip6_net_ops = {
|
||||
.init = etherip6_net_init,
|
||||
.id = ðerip6_net_id,
|
||||
.size = sizeof(struct etherip6_net),
|
||||
};
|
||||
|
||||
static int __init etherip6_init(void)
|
||||
{
|
||||
int err;
|
||||
|
||||
err = register_pernet_subsys(ðerip6_net_ops);
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
err = inet6_add_protocol(ðerip6_protocol, IPPROTO_ETHERIP);
|
||||
if (err)
|
||||
goto err_pernet;
|
||||
|
||||
err = rtnl_link_register(ðerip6_link_ops);
|
||||
if (err)
|
||||
goto err_proto;
|
||||
|
||||
pr_info("etherip6: RFC3378 EtherIP over IPv6 loaded\n");
|
||||
return 0;
|
||||
|
||||
err_proto:
|
||||
inet6_del_protocol(ðerip6_protocol, IPPROTO_ETHERIP);
|
||||
err_pernet:
|
||||
unregister_pernet_subsys(ðerip6_net_ops);
|
||||
return err;
|
||||
}
|
||||
|
||||
static void __exit etherip6_exit(void)
|
||||
{
|
||||
rtnl_link_unregister(ðerip6_link_ops);
|
||||
inet6_del_protocol(ðerip6_protocol, IPPROTO_ETHERIP);
|
||||
unregister_pernet_subsys(ðerip6_net_ops);
|
||||
pr_info("etherip6: unloaded\n");
|
||||
}
|
||||
|
||||
module_init(etherip6_init);
|
||||
module_exit(etherip6_exit);
|
||||
|
||||
MODULE_AUTHOR("Codex");
|
||||
MODULE_DESCRIPTION("RFC3378 EtherIP tunnel endpoint over IPv6");
|
||||
MODULE_LICENSE("GPL");
|
||||
MODULE_ALIAS_RTNL_LINK("etherip6");
|
||||
Reference in New Issue
Block a user