Merge pull request #3428 from toanju/networkd/brvlan

networkd: add support to configure VLAN on bridge ports

9 files changed, 431 insertions, 2 deletions

diff --git a/src/basic/macro.h b/src/basic/macro.h
index e41aa4260f..6b2aeb933f 100644
--- a/src/basic/macro.h
+++ b/src/basic/macro.h
@@ -89,6 +89,15 @@
 #define UNIQ_T(x, uniq) CONCATENATE(__unique_prefix_, CONCATENATE(x, uniq))
 #define UNIQ __COUNTER__
 
+/* builtins */
+#if __SIZEOF_INT__ == 4
+#define BUILTIN_FFS_U32(x) __builtin_ffs(x);
+#elif __SIZEOF_LONG__ == 4
+#define BUILTIN_FFS_U32(x) __builtin_ffsl(x);
+#else
+#error "neither int nor long are four bytes long?!?"
+#endif
+
 /* Rounds up */
 
 #define ALIGN4(l) (((l) + 3) & ~3)
diff --git a/src/basic/missing.h b/src/basic/missing.h
index 51dafcaca9..8b977871e9 100644
--- a/src/basic/missing.h
+++ b/src/basic/missing.h
@@ -759,6 +759,14 @@ struct btrfs_ioctl_quota_ctl_args {
 #define IFLA_BRIDGE_MAX (__IFLA_BRIDGE_MAX - 1)
 #endif
 
+#ifndef BRIDGE_VLAN_INFO_RANGE_BEGIN
+#define BRIDGE_VLAN_INFO_RANGE_BEGIN (1<<3) /* VLAN is start of vlan range */
+#endif
+
+#ifndef BRIDGE_VLAN_INFO_RANGE_END
+#define BRIDGE_VLAN_INFO_RANGE_END (1<<4) /* VLAN is end of vlan range */
+#endif
+
 #if !HAVE_DECL_IFLA_BR_VLAN_DEFAULT_PVID
 #define IFLA_BR_UNSPEC 0
 #define IFLA_BR_FORWARD_DELAY 1
diff --git a/src/network/networkd-brvlan.c b/src/network/networkd-brvlan.c
new file mode 100644
index 0000000000..77c08d090c
--- /dev/null
+++ b/src/network/networkd-brvlan.c
@@ -0,0 +1,335 @@
+/***
+  This file is part of systemd.
+
+  Copyright (C) 2016 BISDN GmbH. All rights reserved.
+
+  systemd is free software; you can redistribute it and/or modify it
+  under the terms of the GNU Lesser General Public License as published by
+  the Free Software Foundation; either version 2.1 of the License, or
+  (at your option) any later version.
+
+  systemd is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public License
+  along with systemd; If not, see <http://www.gnu.org/licenses/>.
+***/
+
+#include <netinet/in.h>
+#include <linux/if_bridge.h>
+#include <stdbool.h>
+
+#include "alloc-util.h"
+#include "conf-parser.h"
+#include "netlink-util.h"
+#include "networkd-brvlan.h"
+#include "networkd.h"
+#include "parse-util.h"
+#include "vlan-util.h"
+
+static bool is_bit_set(unsigned bit, uint32_t scope) {
+        assert(bit < sizeof(scope)*8);
+        return scope & (1 << bit);
+}
+
+static inline void set_bit(unsigned nr, uint32_t *addr) {
+        if (nr < BRIDGE_VLAN_BITMAP_MAX)
+                addr[nr / 32] |= (((uint32_t) 1) << (nr % 32));
+}
+
+static inline int is_vid_valid(unsigned vid) {
+        if (vid > VLANID_MAX || vid == 0)
+                return -EINVAL;
+        return 0;
+}
+
+static int find_next_bit(int i, uint32_t x) {
+        int j;
+
+        if (i >= 32)
+                return -1;
+
+        /* find first bit */
+        if (i < 0)
+                return BUILTIN_FFS_U32(x);
+
+        /* mask off prior finds to get next */
+        j = __builtin_ffs(x >> i);
+        return j ? j + i : 0;
+}
+
+static int append_vlan_info_data(Link *const link, sd_netlink_message *req, uint16_t pvid, const uint32_t *br_vid_bitmap, const uint32_t *br_untagged_bitmap) {
+        struct bridge_vlan_info br_vlan;
+        int i, j, k, r, done, cnt;
+        uint16_t begin, end;
+        bool untagged;
+
+        assert(link);
+        assert(req);
+        assert(br_vid_bitmap);
+        assert(br_untagged_bitmap);
+
+        i = cnt = -1;
+
+        begin = end = UINT16_MAX;
+        for (k = 0; k < BRIDGE_VLAN_BITMAP_LEN; k++) {
+                unsigned base_bit;
+                uint32_t vid_map = br_vid_bitmap[k];
+                uint32_t untagged_map = br_untagged_bitmap[k];
+
+                base_bit = k * 32;
+                i = -1;
+                done = 0;
+                do {
+                        j = find_next_bit(i, vid_map);
+                        if (j > 0) {
+                                /* first hit of any bit */
+                                if (begin == UINT16_MAX && end == UINT16_MAX) {
+                                        begin = end = j - 1 + base_bit;
+                                        untagged = is_bit_set(j - 1, untagged_map);
+                                        goto next;
+                                }
+
+                                /* this bit is a continuation of prior bits */
+                                if (j - 2 + base_bit == end && untagged == is_bit_set(j - 1, untagged_map) && (uint16_t)j - 1 + base_bit != pvid && (uint16_t)begin != pvid) {
+                                        end++;
+                                        goto next;
+                                }
+                        } else
+                                done = 1;
+
+                        if (begin != UINT16_MAX) {
+                                cnt++;
+                                if (done && k < BRIDGE_VLAN_BITMAP_LEN - 1)
+                                        break;
+
+                                br_vlan.flags = 0;
+                                if (untagged)
+                                        br_vlan.flags |= BRIDGE_VLAN_INFO_UNTAGGED;
+
+                                if (begin == end) {
+                                        br_vlan.vid = begin;
+
+                                        if (begin == pvid)
+                                                br_vlan.flags |= BRIDGE_VLAN_INFO_PVID;
+
+                                        r = sd_netlink_message_append_data(req, IFLA_BRIDGE_VLAN_INFO, &br_vlan, sizeof(br_vlan));
+                                        if (r < 0)
+                                                return log_link_error_errno(link, r, "Could not append IFLA_BRIDGE_VLAN_INFO attribute: %m");
+                                } else {
+                                        br_vlan.vid = begin;
+                                        br_vlan.flags |= BRIDGE_VLAN_INFO_RANGE_BEGIN;
+
+                                        r = sd_netlink_message_append_data(req, IFLA_BRIDGE_VLAN_INFO, &br_vlan, sizeof(br_vlan));
+                                        if (r < 0)
+                                                return log_link_error_errno(link, r, "Could not append IFLA_BRIDGE_VLAN_INFO attribute: %m");
+
+                                        br_vlan.vid = end;
+                                        br_vlan.flags &= ~BRIDGE_VLAN_INFO_RANGE_BEGIN;
+                                        br_vlan.flags |= BRIDGE_VLAN_INFO_RANGE_END;
+
+                                        r = sd_netlink_message_append_data(req, IFLA_BRIDGE_VLAN_INFO, &br_vlan, sizeof(br_vlan));
+                                        if (r < 0)
+                                                return log_link_error_errno(link, r, "Could not append IFLA_BRIDGE_VLAN_INFO attribute: %m");
+                                }
+
+                                if (done)
+                                        break;
+                        }
+                        if (j > 0) {
+                                begin = end = j - 1 + base_bit;
+                                untagged = is_bit_set(j - 1, untagged_map);
+                        }
+
+                next:
+                        i = j;
+                } while(!done);
+        }
+        if (!cnt)
+                return -EINVAL;
+
+        return cnt;
+}
+
+static int set_brvlan_handler(sd_netlink *rtnl, sd_netlink_message *m, void *userdata) {
+        Link *link = userdata;
+        int r;
+
+        assert(link);
+
+        r = sd_netlink_message_get_errno(m);
+        if (r < 0 && r != -EEXIST)
+                log_link_error_errno(link, r, "Could not add VLAN to bridge port: %m");
+
+        return 1;
+}
+
+int br_vlan_configure(Link *link, uint16_t pvid, uint32_t *br_vid_bitmap, uint32_t *br_untagged_bitmap) {
+        _cleanup_(sd_netlink_message_unrefp) sd_netlink_message *req = NULL;
+        int r;
+        uint16_t flags;
+        sd_netlink *rtnl;
+
+        assert(link);
+        assert(link->manager);
+        assert(br_vid_bitmap);
+        assert(br_untagged_bitmap);
+        assert(link->network);
+
+        /* pvid might not be in br_vid_bitmap yet */
+        if (pvid)
+                set_bit(pvid, br_vid_bitmap);
+
+        rtnl = link->manager->rtnl;
+
+        /* create new RTM message */
+        r = sd_rtnl_message_new_link(rtnl, &req, RTM_SETLINK, link->ifindex);
+        if (r < 0)
+                return log_link_error_errno(link, r, "Could not allocate RTM_SETLINK message: %m");
+
+        r = sd_rtnl_message_link_set_family(req, PF_BRIDGE);
+        if (r < 0)
+                return log_link_error_errno(link, r, "Could not set message family: %m");
+
+        r = sd_netlink_message_open_container(req, IFLA_AF_SPEC);
+        if (r < 0)
+                return log_link_error_errno(link, r, "Could not open IFLA_AF_SPEC container: %m");
+
+        /* master needs flag self */
+        if (!link->network->bridge) {
+                flags = BRIDGE_FLAGS_SELF;
+                sd_netlink_message_append_data(req, IFLA_BRIDGE_FLAGS, &flags, sizeof(uint16_t));
+        }
+
+        /* add vlan info */
+        r = append_vlan_info_data(link, req, pvid, br_vid_bitmap, br_untagged_bitmap);
+        if (r < 0)
+                return log_link_error_errno(link, r, "Could not append VLANs: %m");
+
+        r = sd_netlink_message_close_container(req);
+        if (r < 0)
+                return log_link_error_errno(link, r, "Could not close IFLA_AF_SPEC container: %m");
+
+        /* send message to the kernel */
+        r = sd_netlink_call_async(rtnl, req, set_brvlan_handler, link, 0, NULL);
+        if (r < 0)
+                return log_link_error_errno(link, r, "Could not send rtnetlink message: %m");
+
+        return 0;
+}
+
+static int parse_vid_range(const char *rvalue, uint16_t *vid, uint16_t *vid_end) {
+        int r;
+        char *p;
+        char *_rvalue = NULL;
+        uint16_t _vid = UINT16_MAX;
+        uint16_t _vid_end = UINT16_MAX;
+
+        assert(rvalue);
+        assert(vid);
+        assert(vid_end);
+
+        _rvalue = strdupa(rvalue);
+        p = strchr(_rvalue, '-');
+        if (p) {
+                *p = '\0';
+                p++;
+                r = parse_vlanid(_rvalue, &_vid);
+                if (r < 0)
+                        return r;
+
+                if (!_vid)
+                        return -ERANGE;
+
+                r = parse_vlanid(p, &_vid_end);
+                if (r < 0)
+                        return r;
+
+                if (!_vid_end)
+                        return -ERANGE;
+        } else {
+                r = parse_vlanid(_rvalue, &_vid);
+                if (r < 0)
+                        return r;
+
+                if (!_vid)
+                        return -ERANGE;
+        }
+
+        *vid = _vid;
+        *vid_end = _vid_end;
+        return r;
+}
+
+int config_parse_brvlan_vlan(const char *unit, const char *filename,
+                             unsigned line, const char *section,
+                             unsigned section_line, const char *lvalue,
+                             int ltype, const char *rvalue, void *data,
+                             void *userdata) {
+        Network *network = userdata;
+        int r;
+        uint16_t vid, vid_end;
+
+        assert(filename);
+        assert(section);
+        assert(lvalue);
+        assert(rvalue);
+        assert(data);
+
+        r = parse_vid_range(rvalue, &vid, &vid_end);
+        if (r < 0) {
+                log_syntax(unit, LOG_ERR, filename, line, r, "Failed to parse VLAN, ignoring: %s", rvalue);
+                return 0;
+        }
+
+        if (UINT16_MAX == vid_end)
+                set_bit(vid++, network->br_vid_bitmap);
+        else {
+                if (vid >= vid_end) {
+                        log_syntax(unit, LOG_ERR, filename, line, 0, "Invalid VLAN range, ignoring %s", rvalue);
+                        return 0;
+                }
+                for (; vid <= vid_end; vid++)
+                        set_bit(vid, network->br_vid_bitmap);
+        }
+        return 0;
+}
+
+int config_parse_brvlan_untagged(const char *unit, const char *filename,
+                                 unsigned line, const char *section,
+                                 unsigned section_line, const char *lvalue,
+                                 int ltype, const char *rvalue, void *data,
+                                 void *userdata) {
+        Network *network = userdata;
+        int r;
+        uint16_t vid, vid_end;
+
+        assert(filename);
+        assert(section);
+        assert(lvalue);
+        assert(rvalue);
+        assert(data);
+
+        r = parse_vid_range(rvalue, &vid, &vid_end);
+        if (r < 0) {
+                log_syntax(unit, LOG_ERR, filename, line, r, "Could not parse VLAN: %s", rvalue);
+                return 0;
+        }
+
+        if (UINT16_MAX == vid_end) {
+                set_bit(vid, network->br_vid_bitmap);
+                set_bit(vid, network->br_untagged_bitmap);
+        } else {
+                if (vid >= vid_end) {
+                        log_syntax(unit, LOG_ERR, filename, line, 0, "Invalid VLAN range, ignoring %s", rvalue);
+                        return 0;
+                }
+                for (; vid <= vid_end; vid++) {
+                        set_bit(vid, network->br_vid_bitmap);
+                        set_bit(vid, network->br_untagged_bitmap);
+                }
+        }
+        return 0;
+}
diff --git a/src/network/networkd-brvlan.h b/src/network/networkd-brvlan.h
new file mode 100644
index 0000000000..6aa6883bfc
--- /dev/null
+++ b/src/network/networkd-brvlan.h
@@ -0,0 +1,29 @@
+#pragma once
+
+/***
+  This file is part of systemd.
+
+  Copyright (C) 2016 BISDN GmbH. All rights reserved.
+
+  systemd is free software; you can redistribute it and/or modify it
+  under the terms of the GNU Lesser General Public License as published by
+  the Free Software Foundation; either version 2.1 of the License, or
+  (at your option) any later version.
+
+  systemd is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public License
+  along with systemd; If not, see <http://www.gnu.org/licenses/>.
+***/
+
+#include <stdint.h>
+
+typedef struct Link Link;
+
+int br_vlan_configure(Link *link, uint16_t pvid, uint32_t *br_vid_bitmap, uint32_t *br_untagged_bitmap);
+
+int config_parse_brvlan_vlan(const char *unit, const char *filename, unsigned line, const char *section, unsigned section_line, const char *lvalue, int ltype, const char *rvalue, void *data, void *userdata);
+int config_parse_brvlan_untagged(const char *unit, const char *filename, unsigned line, const char *section, unsigned section_line, const char *lvalue, int ltype, const char *rvalue, void *data, void *userdata);
diff --git a/src/network/networkd-link.c b/src/network/networkd-link.c
index 6746d88fc8..dce5c2be6e 100644
--- a/src/network/networkd-link.c
+++ b/src/network/networkd-link.c
@@ -396,7 +396,7 @@ static int link_update_flags(Link *link, sd_netlink_message *m) {
 static int link_new(Manager *manager, sd_netlink_message *message, Link **ret) {
         _cleanup_link_unref_ Link *link = NULL;
         uint16_t type;
-        const char *ifname;
+        const char *ifname, *kind = NULL;
         int r, ifindex;
         unsigned short iftype;
 
@@ -404,6 +404,15 @@ static int link_new(Manager *manager, sd_netlink_message *message, Link **ret) {
         assert(message);
         assert(ret);
 
+        /* check for link kind */
+        r = sd_netlink_message_enter_container(message, IFLA_LINKINFO);
+        if (r == 0) {
+                (void)sd_netlink_message_read_string(message, IFLA_INFO_KIND, &kind);
+                r = sd_netlink_message_exit_container(message);
+                if (r < 0)
+                        return r;
+        }
+
         r = sd_netlink_message_get_type(message, &type);
         if (r < 0)
                 return r;
@@ -438,6 +447,12 @@ static int link_new(Manager *manager, sd_netlink_message *message, Link **ret) {
         if (!link->ifname)
                 return -ENOMEM;
 
+        if (kind) {
+                link->kind = strdup(kind);
+                if (!link->kind)
+                        return -ENOMEM;
+        }
+
         r = sd_netlink_message_read_ether_addr(message, IFLA_ADDRESS, &link->mac);
         if (r < 0)
                 log_link_debug_errno(link, r, "MAC address not found for new device, continuing without");
@@ -515,6 +530,9 @@ static void link_free(Link *link) {
 
         free(link->ifname);
 
+        if (link->kind)
+                free(link->kind);
+
         (void)unlink(link->state_file);
         free(link->state_file);
 
@@ -1096,6 +1114,16 @@ int link_address_remove_handler(sd_netlink *rtnl, sd_netlink_message *m, void *u
         return 1;
 }
 
+static int link_set_bridge_vlan(Link *link) {
+        int r = 0;
+
+        r = br_vlan_configure(link, link->network->pvid, link->network->br_vid_bitmap, link->network->br_untagged_bitmap);
+        if (r < 0)
+                log_link_error_errno(link, r, "Failed to assign VLANs to bridge port: %m");
+
+        return r;
+}
+
 static int link_set_bridge_fdb(Link *link) {
         FdbEntry *fdb_entry;
         int r = 0;
@@ -1978,6 +2006,12 @@ static int link_joined(Link *link) {
                         log_link_error_errno(link, r, "Could not set bridge message: %m");
         }
 
+        if (link->network->bridge || NETDEV_KIND_BRIDGE == netdev_kind_from_string(link->kind)) {
+                r = link_set_bridge_vlan(link);
+                if (r < 0)
+                        log_link_error_errno(link, r, "Could not set bridge vlan: %m");
+        }
+
         return link_enter_set_addresses(link);
 }
 
diff --git a/src/network/networkd-link.h b/src/network/networkd-link.h
index 7db94e79e8..2809b1fe0b 100644
--- a/src/network/networkd-link.h
+++ b/src/network/networkd-link.h
@@ -68,6 +68,7 @@ typedef struct Link {
 
         int ifindex;
         char *ifname;
+        char *kind;
         unsigned short iftype;
         char *state_file;
         struct ether_addr mac;
diff --git a/src/network/networkd-network-gperf.gperf b/src/network/networkd-network-gperf.gperf
index d9f5b95cdf..0b0aa58f67 100644
--- a/src/network/networkd-network-gperf.gperf
+++ b/src/network/networkd-network-gperf.gperf
@@ -4,6 +4,7 @@
 #include "networkd.h"
 #include "networkd-conf.h"
 #include "network-internal.h"
+#include "vlan-util.h"
 %}
 struct ConfigPerfItem;
 %null_strings
@@ -112,6 +113,9 @@ Bridge.AllowPortToBeRoot,               config_parse_bool,
 Bridge.UnicastFlood,                    config_parse_bool,                              0,                             offsetof(Network, unicast_flood)
 BridgeFDB.MACAddress,                   config_parse_fdb_hwaddr,                        0,                             0
 BridgeFDB.VLANId,                       config_parse_fdb_vlan_id,                       0,                             0
+BridgeVLAN.PVID,                        config_parse_vlanid,                            0,                             offsetof(Network, pvid)
+BridgeVLAN.VLAN,                        config_parse_brvlan_vlan,                       0,                             0
+BridgeVLAN.EgressUntagged,              config_parse_brvlan_untagged,                   0,                             0
 /* backwards compatibility: do not add new entries to this section */
 Network.IPv4LL,                         config_parse_ipv4ll,                            0,                             offsetof(Network, link_local)
 DHCPv4.UseDNS,                          config_parse_bool,                              0,                             offsetof(Network, dhcp_use_dns)
diff --git a/src/network/networkd-network.c b/src/network/networkd-network.c
index c03c0f0bed..84bdf75b38 100644
--- a/src/network/networkd-network.c
+++ b/src/network/networkd-network.c
@@ -147,7 +147,8 @@ static int network_load_one(Manager *manager, const char *filename) {
                          "DHCPServer\0"
                          "IPv6AcceptRA\0"
                          "Bridge\0"
-                         "BridgeFDB\0",
+                         "BridgeFDB\0"
+                         "BridgeVLAN\0",
                          config_item_perf_lookup, network_network_gperf_lookup,
                          false, false, true, network);
         if (r < 0)
diff --git a/src/network/networkd-network.h b/src/network/networkd-network.h
index a49748d1b1..38688cc400 100644
--- a/src/network/networkd-network.h
+++ b/src/network/networkd-network.h
@@ -28,6 +28,7 @@
 #include "resolve-util.h"
 
 #include "networkd-address.h"
+#include "networkd-brvlan.h"
 #include "networkd-fdb.h"
 #include "networkd-lldp-tx.h"
 #include "networkd-netdev.h"
@@ -37,6 +38,9 @@
 #define DHCP_ROUTE_METRIC 1024
 #define IPV4LL_ROUTE_METRIC 2048
 
+#define BRIDGE_VLAN_BITMAP_MAX 4096
+#define BRIDGE_VLAN_BITMAP_LEN (BRIDGE_VLAN_BITMAP_MAX / 32)
+
 typedef enum DCHPClientIdentifier {
         DHCP_CLIENT_ID_MAC,
         DHCP_CLIENT_ID_DUID,
@@ -146,6 +150,10 @@ struct Network {
         bool unicast_flood;
         unsigned cost;
 
+        uint16_t pvid;
+        uint32_t br_vid_bitmap[BRIDGE_VLAN_BITMAP_LEN];
+        uint32_t br_untagged_bitmap[BRIDGE_VLAN_BITMAP_LEN];
+
         AddressFamilyBoolean ip_forward;
         bool ip_masquerade;