projects / openwrt / openwrt.git / blobdiff
? search:
summary | shortlog | log | commit | commitdiff | tree
raw | inline | side by side
swconfig: simplify init code
[openwrt/openwrt.git] / target / linux / generic / files / drivers / net / phy / swconfig.c
diff --git a/target/linux/generic/files/drivers/net/phy/swconfig.c b/target/linux/generic/files/drivers/net/phy/swconfig.c
index dea8e78b79b4fd61da2a32427d92811c77648206..4bfe64fbd0dc62a3007802eaeb1b4fef8e6cc956 100644 (file)
--- a/target/linux/generic/files/drivers/net/phy/swconfig.c
+++ b/target/linux/generic/files/drivers/net/phy/swconfig.c
@@ -23,24 +23,22 @@
 #include <linux/capability.h>
 #include <linux/skbuff.h>
 #include <linux/switch.h>
 #include <linux/capability.h>
 #include <linux/skbuff.h>
 #include <linux/switch.h>
+#include <linux/of.h>
+#include <linux/version.h>
 
 
-//#define DEBUG 1
-#ifdef DEBUG
-#define DPRINTF(format, ...) printk("%s: " format, __func__, ##__VA_ARGS__)
-#else
-#define DPRINTF(...) do {} while(0)
-#endif
+#define SWCONFIG_DEVNAME       "switch%d"
+
+#include "swconfig_leds.c"
 
 MODULE_AUTHOR("Felix Fietkau <nbd@openwrt.org>");
 MODULE_LICENSE("GPL");
 
 
 MODULE_AUTHOR("Felix Fietkau <nbd@openwrt.org>");
 MODULE_LICENSE("GPL");
 
-static int swdev_id = 0;
+static int swdev_id;
 static struct list_head swdevs;
 static struct list_head swdevs;
-static spinlock_t swdevs_lock = SPIN_LOCK_UNLOCKED;
+static DEFINE_SPINLOCK(swdevs_lock);
 struct swconfig_callback;
 
 struct swconfig_callback;
 
-struct swconfig_callback
-{
+struct swconfig_callback {
        struct sk_buff *msg;
        struct genlmsghdr *hdr;
        struct genl_info *info;
        struct sk_buff *msg;
        struct genlmsghdr *hdr;
        struct genl_info *info;
@@ -59,23 +57,26 @@ struct swconfig_callback
 /* defaults */
 
 static int
 /* defaults */
 
 static int
-swconfig_get_vlan_ports(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
+swconfig_get_vlan_ports(struct switch_dev *dev, const struct switch_attr *attr,
+                       struct switch_val *val)
 {
        int ret;
        if (val->port_vlan >= dev->vlans)
                return -EINVAL;
 
 {
        int ret;
        if (val->port_vlan >= dev->vlans)
                return -EINVAL;
 
-       if (!dev->get_vlan_ports)
+       if (!dev->ops->get_vlan_ports)
                return -EOPNOTSUPP;
 
                return -EOPNOTSUPP;
 
-       ret = dev->get_vlan_ports(dev, val);
+       ret = dev->ops->get_vlan_ports(dev, val);
        return ret;
 }
 
 static int
        return ret;
 }
 
 static int
-swconfig_set_vlan_ports(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
+swconfig_set_vlan_ports(struct switch_dev *dev, const struct switch_attr *attr,
+                       struct switch_val *val)
 {
        struct switch_port *ports = val->value.ports;
 {
        struct switch_port *ports = val->value.ports;
+       const struct switch_dev_ops *ops = dev->ops;
        int i;
 
        if (val->port_vlan >= dev->vlans)
        int i;
 
        if (val->port_vlan >= dev->vlans)
@@ -85,62 +86,93 @@ swconfig_set_vlan_ports(struct switch_dev *dev, const struct switch_attr *attr,
        if (val->len > dev->ports)
                return -EINVAL;
 
        if (val->len > dev->ports)
                return -EINVAL;
 
-       if (!dev->set_vlan_ports)
+       if (!ops->set_vlan_ports)
                return -EOPNOTSUPP;
 
        for (i = 0; i < val->len; i++) {
                if (ports[i].id >= dev->ports)
                        return -EINVAL;
 
                return -EOPNOTSUPP;
 
        for (i = 0; i < val->len; i++) {
                if (ports[i].id >= dev->ports)
                        return -EINVAL;
 
-               if (dev->set_port_pvid && !(ports[i].flags & (1 << SWITCH_PORT_FLAG_TAGGED)))
-                       dev->set_port_pvid(dev, ports[i].id, val->port_vlan);
+               if (ops->set_port_pvid &&
+                   !(ports[i].flags & (1 << SWITCH_PORT_FLAG_TAGGED)))
+                       ops->set_port_pvid(dev, ports[i].id, val->port_vlan);
        }
 
        }
 
-       return dev->set_vlan_ports(dev, val);
+       return ops->set_vlan_ports(dev, val);
 }
 
 static int
 }
 
 static int
-swconfig_set_pvid(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
+swconfig_set_pvid(struct switch_dev *dev, const struct switch_attr *attr,
+                       struct switch_val *val)
 {
        if (val->port_vlan >= dev->ports)
                return -EINVAL;
 
 {
        if (val->port_vlan >= dev->ports)
                return -EINVAL;
 
-       if (!dev->set_port_pvid)
+       if (!dev->ops->set_port_pvid)
                return -EOPNOTSUPP;
 
                return -EOPNOTSUPP;
 
-       return dev->set_port_pvid(dev, val->port_vlan, val->value.i);
+       return dev->ops->set_port_pvid(dev, val->port_vlan, val->value.i);
 }
 
 static int
 }
 
 static int
-swconfig_get_pvid(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
+swconfig_get_pvid(struct switch_dev *dev, const struct switch_attr *attr,
+                       struct switch_val *val)
 {
        if (val->port_vlan >= dev->ports)
                return -EINVAL;
 
 {
        if (val->port_vlan >= dev->ports)
                return -EINVAL;
 
-       if (!dev->get_port_pvid)
+       if (!dev->ops->get_port_pvid)
+               return -EOPNOTSUPP;
+
+       return dev->ops->get_port_pvid(dev, val->port_vlan, &val->value.i);
+}
+
+static int
+swconfig_set_link(struct switch_dev *dev, const struct switch_attr *attr,
+                       struct switch_val *val)
+{
+       if (!dev->ops->set_port_link)
+               return -EOPNOTSUPP;
+
+       return dev->ops->set_port_link(dev, val->port_vlan, val->value.link);
+}
+
+static int
+swconfig_get_link(struct switch_dev *dev, const struct switch_attr *attr,
+                       struct switch_val *val)
+{
+       struct switch_port_link *link = val->value.link;
+
+       if (val->port_vlan >= dev->ports)
+               return -EINVAL;
+
+       if (!dev->ops->get_port_link)
                return -EOPNOTSUPP;
 
                return -EOPNOTSUPP;
 
-       return dev->get_port_pvid(dev, val->port_vlan, &val->value.i);
+       memset(link, 0, sizeof(*link));
+       return dev->ops->get_port_link(dev, val->port_vlan, link);
 }
 
 static int
 }
 
 static int
-swconfig_apply_config(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
+swconfig_apply_config(struct switch_dev *dev, const struct switch_attr *attr,
+                       struct switch_val *val)
 {
        /* don't complain if not supported by the switch driver */
 {
        /* don't complain if not supported by the switch driver */
-       if (!dev->apply_config)
+       if (!dev->ops->apply_config)
                return 0;
 
                return 0;
 
-       return dev->apply_config(dev);
+       return dev->ops->apply_config(dev);
 }
 
 static int
 }
 
 static int
-swconfig_reset_switch(struct switch_dev *dev, const struct switch_attr *attr, struct switch_val *val)
+swconfig_reset_switch(struct switch_dev *dev, const struct switch_attr *attr,
+                       struct switch_val *val)
 {
        /* don't complain if not supported by the switch driver */
 {
        /* don't complain if not supported by the switch driver */
-       if (!dev->reset_switch)
+       if (!dev->ops->reset_switch)
                return 0;
 
                return 0;
 
-       return dev->reset_switch(dev);
+       return dev->ops->reset_switch(dev);
 }
 
 enum global_defaults {
 }
 
 enum global_defaults {
@@ -154,6 +186,7 @@ enum vlan_defaults {
 
 enum port_defaults {
        PORT_PVID,
 
 enum port_defaults {
        PORT_PVID,
+       PORT_LINK,
 };
 
 static struct switch_attr default_global[] = {
 };
 
 static struct switch_attr default_global[] = {
@@ -178,6 +211,13 @@ static struct switch_attr default_port[] = {
                .description = "Primary VLAN ID",
                .set = swconfig_set_pvid,
                .get = swconfig_get_pvid,
                .description = "Primary VLAN ID",
                .set = swconfig_set_pvid,
                .get = swconfig_get_pvid,
+       },
+       [PORT_LINK] = {
+               .type = SWITCH_TYPE_LINK,
+               .name = "link",
+               .description = "Get port link information",
+               .set = swconfig_set_link,
+               .get = swconfig_get_link,
        }
 };
 
        }
 };
 
@@ -191,19 +231,37 @@ static struct switch_attr default_vlan[] = {
        },
 };
 
        },
 };
 
+static const struct switch_attr *
+swconfig_find_attr_by_name(const struct switch_attrlist *alist,
+                               const char *name)
+{
+       int i;
+
+       for (i = 0; i < alist->n_attr; i++)
+               if (strcmp(name, alist->attr[i].name) == 0)
+                       return &alist->attr[i];
+
+       return NULL;
+}
 
 static void swconfig_defaults_init(struct switch_dev *dev)
 {
 
 static void swconfig_defaults_init(struct switch_dev *dev)
 {
+       const struct switch_dev_ops *ops = dev->ops;
+
        dev->def_global = 0;
        dev->def_vlan = 0;
        dev->def_port = 0;
 
        dev->def_global = 0;
        dev->def_vlan = 0;
        dev->def_port = 0;
 
-       if (dev->get_vlan_ports || dev->set_vlan_ports)
+       if (ops->get_vlan_ports || ops->set_vlan_ports)
                set_bit(VLAN_PORTS, &dev->def_vlan);
 
                set_bit(VLAN_PORTS, &dev->def_vlan);
 
-       if (dev->get_port_pvid || dev->set_port_pvid)
+       if (ops->get_port_pvid || ops->set_port_pvid)
                set_bit(PORT_PVID, &dev->def_port);
 
                set_bit(PORT_PVID, &dev->def_port);
 
+       if (ops->get_port_link &&
+           !swconfig_find_attr_by_name(&ops->attr_port, "link"))
+               set_bit(PORT_LINK, &dev->def_port);
+
        /* always present, can be no-op */
        set_bit(GLOBAL_APPLY, &dev->def_global);
        set_bit(GLOBAL_RESET, &dev->def_global);
        /* always present, can be no-op */
        set_bit(GLOBAL_APPLY, &dev->def_global);
        set_bit(GLOBAL_RESET, &dev->def_global);
@@ -234,6 +292,12 @@ static const struct nla_policy port_policy[SWITCH_PORT_ATTR_MAX+1] = {
        [SWITCH_PORT_FLAG_TAGGED] = { .type = NLA_FLAG },
 };
 
        [SWITCH_PORT_FLAG_TAGGED] = { .type = NLA_FLAG },
 };
 
+static struct nla_policy link_policy[SWITCH_LINK_ATTR_MAX] = {
+       [SWITCH_LINK_FLAG_DUPLEX] = { .type = NLA_FLAG },
+       [SWITCH_LINK_FLAG_ANEG] = { .type = NLA_FLAG },
+       [SWITCH_LINK_SPEED] = { .type = NLA_U32 },
+};
+
 static inline void
 swconfig_lock(void)
 {
 static inline void
 swconfig_lock(void)
 {
@@ -266,9 +330,9 @@ swconfig_get_dev(struct genl_info *info)
                break;
        }
        if (dev)
                break;
        }
        if (dev)
-               spin_lock(&dev->lock);
+               mutex_lock(&dev->sw_mutex);
        else
        else
-               DPRINTF("device %d not found\n", id);
+               pr_debug("device %d not found\n", id);
        swconfig_unlock();
 done:
        return dev;
        swconfig_unlock();
 done:
        return dev;
@@ -277,7 +341,7 @@ done:
 static inline void
 swconfig_put_dev(struct switch_dev *dev)
 {
 static inline void
 swconfig_put_dev(struct switch_dev *dev)
 {
-       spin_unlock(&dev->lock);
+       mutex_unlock(&dev->sw_mutex);
 }
 
 static int
 }
 
 static int
@@ -289,19 +353,24 @@ swconfig_dump_attr(struct swconfig_callback *cb, void *arg)
        int id = cb->args[0];
        void *hdr;
 
        int id = cb->args[0];
        void *hdr;
 
-       hdr = genlmsg_put(msg, info->snd_pid, info->snd_seq, &switch_fam,
+       hdr = genlmsg_put(msg, info->snd_portid, info->snd_seq, &switch_fam,
                        NLM_F_MULTI, SWITCH_CMD_NEW_ATTR);
        if (IS_ERR(hdr))
                return -1;
 
                        NLM_F_MULTI, SWITCH_CMD_NEW_ATTR);
        if (IS_ERR(hdr))
                return -1;
 
-       NLA_PUT_U32(msg, SWITCH_ATTR_OP_ID, id);
-       NLA_PUT_U32(msg, SWITCH_ATTR_OP_TYPE, op->type);
-       NLA_PUT_STRING(msg, SWITCH_ATTR_OP_NAME, op->name);
+       if (nla_put_u32(msg, SWITCH_ATTR_OP_ID, id))
+               goto nla_put_failure;
+       if (nla_put_u32(msg, SWITCH_ATTR_OP_TYPE, op->type))
+               goto nla_put_failure;
+       if (nla_put_string(msg, SWITCH_ATTR_OP_NAME, op->name))
+               goto nla_put_failure;
        if (op->description)
        if (op->description)
-               NLA_PUT_STRING(msg, SWITCH_ATTR_OP_DESCRIPTION,
-                       op->description);
+               if (nla_put_string(msg, SWITCH_ATTR_OP_DESCRIPTION,
+                       op->description))
+                       goto nla_put_failure;
 
 
-       return genlmsg_end(msg, hdr);
+       genlmsg_end(msg, hdr);
+       return msg->len;
 nla_put_failure:
        genlmsg_cancel(msg, hdr);
        return -EMSGSIZE;
 nla_put_failure:
        genlmsg_cancel(msg, hdr);
        return -EMSGSIZE;
@@ -335,7 +404,7 @@ swconfig_send_multipart(struct swconfig_callback *cb, void *arg)
                        if (cb->close(cb, arg) < 0)
                                goto error;
                }
                        if (cb->close(cb, arg) < 0)
                                goto error;
                }
-               err = genlmsg_unicast(cb->msg, info->snd_pid);
+               err = genlmsg_reply(cb->msg, info);
                cb->msg = NULL;
                if (err < 0)
                        goto error;
                cb->msg = NULL;
                if (err < 0)
                        goto error;
@@ -369,21 +438,21 @@ swconfig_list_attrs(struct sk_buff *skb, struct genl_info *info)
        if (!dev)
                return -EINVAL;
 
        if (!dev)
                return -EINVAL;
 
-       switch(hdr->cmd) {
+       switch (hdr->cmd) {
        case SWITCH_CMD_LIST_GLOBAL:
        case SWITCH_CMD_LIST_GLOBAL:
-               alist = &dev->attr_global;
+               alist = &dev->ops->attr_global;
                def_list = default_global;
                def_active = &dev->def_global;
                n_def = ARRAY_SIZE(default_global);
                break;
        case SWITCH_CMD_LIST_VLAN:
                def_list = default_global;
                def_active = &dev->def_global;
                n_def = ARRAY_SIZE(default_global);
                break;
        case SWITCH_CMD_LIST_VLAN:
-               alist = &dev->attr_vlan;
+               alist = &dev->ops->attr_vlan;
                def_list = default_vlan;
                def_active = &dev->def_vlan;
                n_def = ARRAY_SIZE(default_vlan);
                break;
        case SWITCH_CMD_LIST_PORT:
                def_list = default_vlan;
                def_active = &dev->def_vlan;
                n_def = ARRAY_SIZE(default_vlan);
                break;
        case SWITCH_CMD_LIST_PORT:
-               alist = &dev->attr_port;
+               alist = &dev->ops->attr_port;
                def_list = default_port;
                def_active = &dev->def_port;
                n_def = ARRAY_SIZE(default_port);
                def_list = default_port;
                def_active = &dev->def_port;
                n_def = ARRAY_SIZE(default_port);
@@ -419,7 +488,7 @@ swconfig_list_attrs(struct sk_buff *skb, struct genl_info *info)
        if (!cb.msg)
                return 0;
 
        if (!cb.msg)
                return 0;
 
-       return genlmsg_unicast(cb.msg, info->snd_pid);
+       return genlmsg_reply(cb.msg, info);
 
 error:
        if (cb.msg)
 
 error:
        if (cb.msg)
@@ -446,17 +515,17 @@ swconfig_lookup_attr(struct switch_dev *dev, struct genl_info *info,
        if (!info->attrs[SWITCH_ATTR_OP_ID])
                goto done;
 
        if (!info->attrs[SWITCH_ATTR_OP_ID])
                goto done;
 
-       switch(hdr->cmd) {
+       switch (hdr->cmd) {
        case SWITCH_CMD_SET_GLOBAL:
        case SWITCH_CMD_GET_GLOBAL:
        case SWITCH_CMD_SET_GLOBAL:
        case SWITCH_CMD_GET_GLOBAL:
-               alist = &dev->attr_global;
+               alist = &dev->ops->attr_global;
                def_list = default_global;
                def_active = &dev->def_global;
                n_def = ARRAY_SIZE(default_global);
                break;
        case SWITCH_CMD_SET_VLAN:
        case SWITCH_CMD_GET_VLAN:
                def_list = default_global;
                def_active = &dev->def_global;
                n_def = ARRAY_SIZE(default_global);
                break;
        case SWITCH_CMD_SET_VLAN:
        case SWITCH_CMD_GET_VLAN:
-               alist = &dev->attr_vlan;
+               alist = &dev->ops->attr_vlan;
                def_list = default_vlan;
                def_active = &dev->def_vlan;
                n_def = ARRAY_SIZE(default_vlan);
                def_list = default_vlan;
                def_active = &dev->def_vlan;
                n_def = ARRAY_SIZE(default_vlan);
@@ -468,7 +537,7 @@ swconfig_lookup_attr(struct switch_dev *dev, struct genl_info *info,
                break;
        case SWITCH_CMD_SET_PORT:
        case SWITCH_CMD_GET_PORT:
                break;
        case SWITCH_CMD_SET_PORT:
        case SWITCH_CMD_GET_PORT:
-               alist = &dev->attr_port;
+               alist = &dev->ops->attr_port;
                def_list = default_port;
                def_active = &dev->def_port;
                n_def = ARRAY_SIZE(default_port);
                def_list = default_port;
                def_active = &dev->def_port;
                n_def = ARRAY_SIZE(default_port);
@@ -505,7 +574,7 @@ swconfig_lookup_attr(struct switch_dev *dev, struct genl_info *info,
 
 done:
        if (!attr)
 
 done:
        if (!attr)
-               DPRINTF("attribute lookup failed\n");
+               pr_debug("attribute lookup failed\n");
        val->attr = attr;
        return attr;
 }
        val->attr = attr;
        return attr;
 }
@@ -541,6 +610,22 @@ swconfig_parse_ports(struct sk_buff *msg, struct nlattr *head,
        return 0;
 }
 
        return 0;
 }
 
+static int
+swconfig_parse_link(struct sk_buff *msg, struct nlattr *nla,
+                   struct switch_port_link *link)
+{
+       struct nlattr *tb[SWITCH_LINK_ATTR_MAX + 1];
+
+       if (nla_parse_nested(tb, SWITCH_LINK_ATTR_MAX, nla, link_policy))
+               return -EINVAL;
+
+       link->duplex = !!tb[SWITCH_LINK_FLAG_DUPLEX];
+       link->aneg = !!tb[SWITCH_LINK_FLAG_ANEG];
+       link->speed = nla_get_u32(tb[SWITCH_LINK_SPEED]);
+
+       return 0;
+}
+
 static int
 swconfig_set_attr(struct sk_buff *skb, struct genl_info *info)
 {
 static int
 swconfig_set_attr(struct sk_buff *skb, struct genl_info *info)
 {
@@ -559,7 +644,7 @@ swconfig_set_attr(struct sk_buff *skb, struct genl_info *info)
                goto error;
 
        val.attr = attr;
                goto error;
 
        val.attr = attr;
-       switch(attr->type) {
+       switch (attr->type) {
        case SWITCH_TYPE_NOVAL:
                break;
        case SWITCH_TYPE_INT:
        case SWITCH_TYPE_NOVAL:
                break;
        case SWITCH_TYPE_INT:
@@ -582,7 +667,23 @@ swconfig_set_attr(struct sk_buff *skb, struct genl_info *info)
                /* TODO: implement multipart? */
                if (info->attrs[SWITCH_ATTR_OP_VALUE_PORTS]) {
                        err = swconfig_parse_ports(skb,
                /* TODO: implement multipart? */
                if (info->attrs[SWITCH_ATTR_OP_VALUE_PORTS]) {
                        err = swconfig_parse_ports(skb,
-                               info->attrs[SWITCH_ATTR_OP_VALUE_PORTS], &val, dev->ports);
+                               info->attrs[SWITCH_ATTR_OP_VALUE_PORTS],
+                               &val, dev->ports);
+                       if (err < 0)
+                               goto error;
+               } else {
+                       val.len = 0;
+                       err = 0;
+               }
+               break;
+       case SWITCH_TYPE_LINK:
+               val.value.link = &dev->linkbuf;
+               memset(&dev->linkbuf, 0, sizeof(struct switch_port_link));
+
+               if (info->attrs[SWITCH_ATTR_OP_VALUE_LINK]) {
+                       err = swconfig_parse_link(skb,
+                                                 info->attrs[SWITCH_ATTR_OP_VALUE_LINK],
+                                                 val.value.link);
                        if (err < 0)
                                goto error;
                } else {
                        if (err < 0)
                                goto error;
                } else {
@@ -624,9 +725,12 @@ swconfig_send_port(struct swconfig_callback *cb, void *arg)
        if (!p)
                goto error;
 
        if (!p)
                goto error;
 
-       NLA_PUT_U32(cb->msg, SWITCH_PORT_ID, port->id);
-       if (port->flags & (1 << SWITCH_PORT_FLAG_TAGGED))
-               NLA_PUT_FLAG(cb->msg, SWITCH_PORT_FLAG_TAGGED);
+       if (nla_put_u32(cb->msg, SWITCH_PORT_ID, port->id))
+               goto nla_put_failure;
+       if (port->flags & (1 << SWITCH_PORT_FLAG_TAGGED)) {
+               if (nla_put_flag(cb->msg, SWITCH_PORT_FLAG_TAGGED))
+                       goto nla_put_failure;
+       }
 
        nla_nest_end(cb->msg, p);
        return 0;
 
        nla_nest_end(cb->msg, p);
        return 0;
@@ -670,6 +774,53 @@ done:
        return err;
 }
 
        return err;
 }
 
+static int
+swconfig_send_link(struct sk_buff *msg, struct genl_info *info, int attr,
+                  const struct switch_port_link *link)
+{
+       struct nlattr *p = NULL;
+       int err = 0;
+
+       p = nla_nest_start(msg, attr);
+       if (link->link) {
+               if (nla_put_flag(msg, SWITCH_LINK_FLAG_LINK))
+                       goto nla_put_failure;
+       }
+       if (link->duplex) {
+               if (nla_put_flag(msg, SWITCH_LINK_FLAG_DUPLEX))
+                       goto nla_put_failure;
+       }
+       if (link->aneg) {
+               if (nla_put_flag(msg, SWITCH_LINK_FLAG_ANEG))
+                       goto nla_put_failure;
+       }
+       if (link->tx_flow) {
+               if (nla_put_flag(msg, SWITCH_LINK_FLAG_TX_FLOW))
+                       goto nla_put_failure;
+       }
+       if (link->rx_flow) {
+               if (nla_put_flag(msg, SWITCH_LINK_FLAG_RX_FLOW))
+                       goto nla_put_failure;
+       }
+       if (nla_put_u32(msg, SWITCH_LINK_SPEED, link->speed))
+               goto nla_put_failure;
+       if (link->eee & ADVERTISED_100baseT_Full) {
+               if (nla_put_flag(msg, SWITCH_LINK_FLAG_EEE_100BASET))
+                       goto nla_put_failure;
+       }
+       if (link->eee & ADVERTISED_1000baseT_Full) {
+               if (nla_put_flag(msg, SWITCH_LINK_FLAG_EEE_1000BASET))
+                       goto nla_put_failure;
+       }
+       nla_nest_end(msg, p);
+
+       return err;
+
+nla_put_failure:
+       nla_nest_cancel(msg, p);
+       return -1;
+}
+
 static int
 swconfig_get_attr(struct sk_buff *skb, struct genl_info *info)
 {
 static int
 swconfig_get_attr(struct sk_buff *skb, struct genl_info *info)
 {
@@ -694,6 +845,9 @@ swconfig_get_attr(struct sk_buff *skb, struct genl_info *info)
                val.value.ports = dev->portbuf;
                memset(dev->portbuf, 0,
                        sizeof(struct switch_port) * dev->ports);
                val.value.ports = dev->portbuf;
                memset(dev->portbuf, 0,
                        sizeof(struct switch_port) * dev->ports);
+       } else if (attr->type == SWITCH_TYPE_LINK) {
+               val.value.link = &dev->linkbuf;
+               memset(&dev->linkbuf, 0, sizeof(struct switch_port_link));
        }
 
        err = attr->get(dev, attr, &val);
        }
 
        err = attr->get(dev, attr, &val);
@@ -704,17 +858,19 @@ swconfig_get_attr(struct sk_buff *skb, struct genl_info *info)
        if (!msg)
                goto error;
 
        if (!msg)
                goto error;
 
-       hdr = genlmsg_put(msg, info->snd_pid, info->snd_seq, &switch_fam,
+       hdr = genlmsg_put(msg, info->snd_portid, info->snd_seq, &switch_fam,
                        0, cmd);
        if (IS_ERR(hdr))
                goto nla_put_failure;
 
                        0, cmd);
        if (IS_ERR(hdr))
                goto nla_put_failure;
 
-       switch(attr->type) {
+       switch (attr->type) {
        case SWITCH_TYPE_INT:
        case SWITCH_TYPE_INT:
-               NLA_PUT_U32(msg, SWITCH_ATTR_OP_VALUE_INT, val.value.i);
+               if (nla_put_u32(msg, SWITCH_ATTR_OP_VALUE_INT, val.value.i))
+                       goto nla_put_failure;
                break;
        case SWITCH_TYPE_STRING:
                break;
        case SWITCH_TYPE_STRING:
-               NLA_PUT_STRING(msg, SWITCH_ATTR_OP_VALUE_STR, val.value.s);
+               if (nla_put_string(msg, SWITCH_ATTR_OP_VALUE_STR, val.value.s))
+                       goto nla_put_failure;
                break;
        case SWITCH_TYPE_PORTS:
                err = swconfig_send_ports(&msg, info,
                break;
        case SWITCH_TYPE_PORTS:
                err = swconfig_send_ports(&msg, info,
@@ -722,17 +878,24 @@ swconfig_get_attr(struct sk_buff *skb, struct genl_info *info)
                if (err < 0)
                        goto nla_put_failure;
                break;
                if (err < 0)
                        goto nla_put_failure;
                break;
+       case SWITCH_TYPE_LINK:
+               err = swconfig_send_link(msg, info,
+                                        SWITCH_ATTR_OP_VALUE_LINK, val.value.link);
+               if (err < 0)
+                       goto nla_put_failure;
+               break;
        default:
        default:
-               DPRINTF("invalid type in attribute\n");
+               pr_debug("invalid type in attribute\n");
                err = -EINVAL;
                goto error;
        }
                err = -EINVAL;
                goto error;
        }
-       err = genlmsg_end(msg, hdr);
+       genlmsg_end(msg, hdr);
+       err = msg->len;
        if (err < 0)
                goto nla_put_failure;
 
        swconfig_put_dev(dev);
        if (err < 0)
                goto nla_put_failure;
 
        swconfig_put_dev(dev);
-       return genlmsg_unicast(msg, info->snd_pid);
+       return genlmsg_reply(msg, info);
 
 nla_put_failure:
        if (msg)
 
 nla_put_failure:
        if (msg)
@@ -748,21 +911,50 @@ static int
 swconfig_send_switch(struct sk_buff *msg, u32 pid, u32 seq, int flags,
                const struct switch_dev *dev)
 {
 swconfig_send_switch(struct sk_buff *msg, u32 pid, u32 seq, int flags,
                const struct switch_dev *dev)
 {
+       struct nlattr *p = NULL, *m = NULL;
        void *hdr;
        void *hdr;
+       int i;
 
        hdr = genlmsg_put(msg, pid, seq, &switch_fam, flags,
                        SWITCH_CMD_NEW_ATTR);
        if (IS_ERR(hdr))
                return -1;
 
 
        hdr = genlmsg_put(msg, pid, seq, &switch_fam, flags,
                        SWITCH_CMD_NEW_ATTR);
        if (IS_ERR(hdr))
                return -1;
 
-       NLA_PUT_U32(msg, SWITCH_ATTR_ID, dev->id);
-       NLA_PUT_STRING(msg, SWITCH_ATTR_NAME, dev->name);
-       NLA_PUT_STRING(msg, SWITCH_ATTR_DEV_NAME, dev->devname);
-       NLA_PUT_U32(msg, SWITCH_ATTR_VLANS, dev->vlans);
-       NLA_PUT_U32(msg, SWITCH_ATTR_PORTS, dev->ports);
-       NLA_PUT_U32(msg, SWITCH_ATTR_CPU_PORT, dev->cpu_port);
+       if (nla_put_u32(msg, SWITCH_ATTR_ID, dev->id))
+               goto nla_put_failure;
+       if (nla_put_string(msg, SWITCH_ATTR_DEV_NAME, dev->devname))
+               goto nla_put_failure;
+       if (nla_put_string(msg, SWITCH_ATTR_ALIAS, dev->alias))
+               goto nla_put_failure;
+       if (nla_put_string(msg, SWITCH_ATTR_NAME, dev->name))
+               goto nla_put_failure;
+       if (nla_put_u32(msg, SWITCH_ATTR_VLANS, dev->vlans))
+               goto nla_put_failure;
+       if (nla_put_u32(msg, SWITCH_ATTR_PORTS, dev->ports))
+               goto nla_put_failure;
+       if (nla_put_u32(msg, SWITCH_ATTR_CPU_PORT, dev->cpu_port))
+               goto nla_put_failure;
 
 
-       return genlmsg_end(msg, hdr);
+       m = nla_nest_start(msg, SWITCH_ATTR_PORTMAP);
+       if (!m)
+               goto nla_put_failure;
+       for (i = 0; i < dev->ports; i++) {
+               p = nla_nest_start(msg, SWITCH_ATTR_PORTS);
+               if (!p)
+                       continue;
+               if (dev->portmap[i].s) {
+                       if (nla_put_string(msg, SWITCH_PORTMAP_SEGMENT,
+                                               dev->portmap[i].s))
+                               goto nla_put_failure;
+                       if (nla_put_u32(msg, SWITCH_PORTMAP_VIRT,
+                                               dev->portmap[i].virt))
+                               goto nla_put_failure;
+               }
+               nla_nest_end(msg, p);
+       }
+       nla_nest_end(msg, m);
+       genlmsg_end(msg, hdr);
+       return msg->len;
 nla_put_failure:
        genlmsg_cancel(msg, hdr);
        return -EMSGSIZE;
 nla_put_failure:
        genlmsg_cancel(msg, hdr);
        return -EMSGSIZE;
@@ -779,7 +971,7 @@ static int swconfig_dump_switches(struct sk_buff *skb,
        list_for_each_entry(dev, &swdevs, dev_list) {
                if (++idx <= start)
                        continue;
        list_for_each_entry(dev, &swdevs, dev_list) {
                if (++idx <= start)
                        continue;
-               if (swconfig_send_switch(skb, NETLINK_CB(cb->skb).pid,
+               if (swconfig_send_switch(skb, NETLINK_CB(cb->skb).portid,
                                cb->nlh->nlmsg_seq, NLM_F_MULTI,
                                dev) < 0)
                        break;
                                cb->nlh->nlmsg_seq, NLM_F_MULTI,
                                dev) < 0)
                        break;
@@ -850,30 +1042,115 @@ static struct genl_ops swconfig_ops[] = {
        }
 };
 
        }
 };
 
+#ifdef CONFIG_OF
+void
+of_switch_load_portmap(struct switch_dev *dev)
+{
+       struct device_node *port;
+
+       if (!dev->of_node)
+               return;
+
+       for_each_child_of_node(dev->of_node, port) {
+               const __be32 *prop;
+               const char *segment;
+               int size, phys;
+
+               if (!of_device_is_compatible(port, "swconfig,port"))
+                       continue;
+
+               if (of_property_read_string(port, "swconfig,segment", &segment))
+                       continue;
+
+               prop = of_get_property(port, "swconfig,portmap", &size);
+               if (!prop)
+                       continue;
+
+               if (size != (2 * sizeof(*prop))) {
+                       pr_err("%s: failed to parse port mapping\n",
+                                       port->name);
+                       continue;
+               }
+
+               phys = be32_to_cpup(prop++);
+               if ((phys < 0) | (phys >= dev->ports)) {
+                       pr_err("%s: physical port index out of range\n",
+                                       port->name);
+                       continue;
+               }
+
+               dev->portmap[phys].s = kstrdup(segment, GFP_KERNEL);
+               dev->portmap[phys].virt = be32_to_cpup(prop);
+               pr_debug("Found port: %s, physical: %d, virtual: %d\n",
+                       segment, phys, dev->portmap[phys].virt);
+       }
+}
+#endif
+
 int
 register_switch(struct switch_dev *dev, struct net_device *netdev)
 {
 int
 register_switch(struct switch_dev *dev, struct net_device *netdev)
 {
+       struct switch_dev *sdev;
+       const int max_switches = 8 * sizeof(unsigned long);
+       unsigned long in_use = 0;
+       int err;
+       int i;
+
        INIT_LIST_HEAD(&dev->dev_list);
        if (netdev) {
                dev->netdev = netdev;
        INIT_LIST_HEAD(&dev->dev_list);
        if (netdev) {
                dev->netdev = netdev;
-               if (!dev->devname)
-                       dev->devname = netdev->name;
+               if (!dev->alias)
+                       dev->alias = netdev->name;
        }
        }
-       BUG_ON(!dev->devname);
+       BUG_ON(!dev->alias);
 
        if (dev->ports > 0) {
 
        if (dev->ports > 0) {
-               dev->portbuf = kzalloc(sizeof(struct switch_port) * dev->ports,
-                               GFP_KERNEL);
+               dev->portbuf = kzalloc(sizeof(struct switch_port) *
+                               dev->ports, GFP_KERNEL);
                if (!dev->portbuf)
                        return -ENOMEM;
                if (!dev->portbuf)
                        return -ENOMEM;
+               dev->portmap = kzalloc(sizeof(struct switch_portmap) *
+                               dev->ports, GFP_KERNEL);
+               if (!dev->portmap) {
+                       kfree(dev->portbuf);
+                       return -ENOMEM;
+               }
        }
        }
-       dev->id = ++swdev_id;
        swconfig_defaults_init(dev);
        swconfig_defaults_init(dev);
-       spin_lock_init(&dev->lock);
+       mutex_init(&dev->sw_mutex);
        swconfig_lock();
        swconfig_lock();
-       list_add(&dev->dev_list, &swdevs);
+       dev->id = ++swdev_id;
+
+       list_for_each_entry(sdev, &swdevs, dev_list) {
+               if (!sscanf(sdev->devname, SWCONFIG_DEVNAME, &i))
+                       continue;
+               if (i < 0 || i > max_switches)
+                       continue;
+
+               set_bit(i, &in_use);
+       }
+       i = find_first_zero_bit(&in_use, max_switches);
+
+       if (i == max_switches) {
+               swconfig_unlock();
+               return -ENFILE;
+       }
+
+#ifdef CONFIG_OF
+       if (dev->ports)
+               of_switch_load_portmap(dev);
+#endif
+
+       /* fill device name */
+       snprintf(dev->devname, IFNAMSIZ, SWCONFIG_DEVNAME, i);
+
+       list_add_tail(&dev->dev_list, &swdevs);
        swconfig_unlock();
 
        swconfig_unlock();
 
+       err = swconfig_create_led_trigger(dev);
+       if (err)
+               return err;
+
        return 0;
 }
 EXPORT_SYMBOL_GPL(register_switch);
        return 0;
 }
 EXPORT_SYMBOL_GPL(register_switch);
@@ -881,12 +1158,13 @@ EXPORT_SYMBOL_GPL(register_switch);
 void
 unregister_switch(struct switch_dev *dev)
 {
 void
 unregister_switch(struct switch_dev *dev)
 {
+       swconfig_destroy_led_trigger(dev);
        kfree(dev->portbuf);
        kfree(dev->portbuf);
-       spin_lock(&dev->lock);
+       mutex_lock(&dev->sw_mutex);
        swconfig_lock();
        list_del(&dev->dev_list);
        swconfig_unlock();
        swconfig_lock();
        list_del(&dev->dev_list);
        swconfig_unlock();
-       spin_unlock(&dev->lock);
+       mutex_unlock(&dev->sw_mutex);
 }
 EXPORT_SYMBOL_GPL(unregister_switch);
 
 }
 EXPORT_SYMBOL_GPL(unregister_switch);
 
@@ -894,24 +1172,9 @@ EXPORT_SYMBOL_GPL(unregister_switch);
 static int __init
 swconfig_init(void)
 {
 static int __init
 swconfig_init(void)
 {
-       int i, err;
-
        INIT_LIST_HEAD(&swdevs);
        INIT_LIST_HEAD(&swdevs);
-       err = genl_register_family(&switch_fam);
-       if (err)
-               return err;
-
-       for (i = 0; i < ARRAY_SIZE(swconfig_ops); i++) {
-               err = genl_register_ops(&switch_fam, &swconfig_ops[i]);
-               if (err)
-                       goto unregister;
-       }
-
-       return 0;
-
-unregister:
-       genl_unregister_family(&switch_fam);
-       return err;
+       
+       return genl_register_family_with_ops(&switch_fam, swconfig_ops);
 }
 
 static void __exit
 }
 
 static void __exit
OpenWrt Source Repository