net/dsa/master.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Handling of a master device, switching frames via its switch fabric CPU port
   4  *
   5  * Copyright (c) 2017 Savoir-faire Linux Inc.
   6  *      Vivien Didelot <vivien.didelot@savoirfairelinux.com>
   7  */
   8
   9 #include "dsa_priv.h"
  10
  11 static void dsa_master_get_ethtool_stats(struct net_device *dev,
  12                                          struct ethtool_stats *stats,
  13                                          uint64_t *data)
  14 {
  15         struct dsa_port *cpu_dp = dev->dsa_ptr;
  16         const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops;
  17         struct dsa_switch *ds = cpu_dp->ds;
  18         int port = cpu_dp->index;
  19         int count = 0;
  20
  21         if (ops->get_sset_count && ops->get_ethtool_stats) {
  22                 count = ops->get_sset_count(dev, ETH_SS_STATS);
  23                 ops->get_ethtool_stats(dev, stats, data);
  24         }
  25
  26         if (ds->ops->get_ethtool_stats)
  27                 ds->ops->get_ethtool_stats(ds, port, data + count);
  28 }
  29
  30 static void dsa_master_get_ethtool_phy_stats(struct net_device *dev,
  31                                              struct ethtool_stats *stats,
  32                                              uint64_t *data)
  33 {
  34         struct dsa_port *cpu_dp = dev->dsa_ptr;
  35         const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops;
  36         struct dsa_switch *ds = cpu_dp->ds;
  37         int port = cpu_dp->index;
  38         int count = 0;
  39
  40         if (dev->phydev && !ops->get_ethtool_phy_stats) {
  41                 count = phy_ethtool_get_sset_count(dev->phydev);
  42                 if (count >= 0)
  43                         phy_ethtool_get_stats(dev->phydev, stats, data);
  44         } else if (ops->get_sset_count && ops->get_ethtool_phy_stats) {
  45                 count = ops->get_sset_count(dev, ETH_SS_PHY_STATS);
  46                 ops->get_ethtool_phy_stats(dev, stats, data);
  47         }
  48
  49         if (count < 0)
  50                 count = 0;
  51
  52         if (ds->ops->get_ethtool_phy_stats)
  53                 ds->ops->get_ethtool_phy_stats(ds, port, data + count);
  54 }
  55
  56 static int dsa_master_get_sset_count(struct net_device *dev, int sset)
  57 {
  58         struct dsa_port *cpu_dp = dev->dsa_ptr;
  59         const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops;
  60         struct dsa_switch *ds = cpu_dp->ds;
  61         int count = 0;
  62
  63         if (sset == ETH_SS_PHY_STATS && dev->phydev &&
  64             !ops->get_ethtool_phy_stats)
  65                 count = phy_ethtool_get_sset_count(dev->phydev);
  66         else if (ops->get_sset_count)
  67                 count = ops->get_sset_count(dev, sset);
  68
  69         if (count < 0)
  70                 count = 0;
  71
  72         if (ds->ops->get_sset_count)
  73                 count += ds->ops->get_sset_count(ds, cpu_dp->index, sset);
  74
  75         return count;
  76 }
  77
  78 static void dsa_master_get_strings(struct net_device *dev, uint32_t stringset,
  79                                    uint8_t *data)
  80 {
  81         struct dsa_port *cpu_dp = dev->dsa_ptr;
  82         const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops;
  83         struct dsa_switch *ds = cpu_dp->ds;
  84         int port = cpu_dp->index;
  85         int len = ETH_GSTRING_LEN;
  86         int mcount = 0, count;
  87         unsigned int i;
  88         uint8_t pfx[4];
  89         uint8_t *ndata;
  90
  91         snprintf(pfx, sizeof(pfx), "p%.2d", port);
  92         /* We do not want to be NULL-terminated, since this is a prefix */
  93         pfx[sizeof(pfx) - 1] = '_';
  94
  95         if (stringset == ETH_SS_PHY_STATS && dev->phydev &&
  96             !ops->get_ethtool_phy_stats) {
  97                 mcount = phy_ethtool_get_sset_count(dev->phydev);
  98                 if (mcount < 0)
  99                         mcount = 0;
 100                 else
 101                         phy_ethtool_get_strings(dev->phydev, data);
 102         } else if (ops->get_sset_count && ops->get_strings) {
 103                 mcount = ops->get_sset_count(dev, stringset);
 104                 if (mcount < 0)
 105                         mcount = 0;
 106                 ops->get_strings(dev, stringset, data);
 107         }
 108
 109         if (ds->ops->get_strings) {
 110                 ndata = data + mcount * len;
 111                 /* This function copies ETH_GSTRINGS_LEN bytes, we will mangle
 112                  * the output after to prepend our CPU port prefix we
 113                  * constructed earlier
 114                  */
 115                 ds->ops->get_strings(ds, port, stringset, ndata);
 116                 count = ds->ops->get_sset_count(ds, port, stringset);
 117                 for (i = 0; i < count; i++) {
 118                         memmove(ndata + (i * len + sizeof(pfx)),
 119                                 ndata + i * len, len - sizeof(pfx));
 120                         memcpy(ndata + i * len, pfx, sizeof(pfx));
 121                 }
 122         }
 123 }
 124
 125 static int dsa_master_get_phys_port_name(struct net_device *dev,
 126                                          char *name, size_t len)
 127 {
 128         struct dsa_port *cpu_dp = dev->dsa_ptr;
 129
 130         if (snprintf(name, len, "p%d", cpu_dp->index) >= len)
 131                 return -EINVAL;
 132
 133         return 0;
 134 }
 135
 136 static int dsa_master_ethtool_setup(struct net_device *dev)
 137 {
 138         struct dsa_port *cpu_dp = dev->dsa_ptr;
 139         struct dsa_switch *ds = cpu_dp->ds;
 140         struct ethtool_ops *ops;
 141
 142         ops = devm_kzalloc(ds->dev, sizeof(*ops), GFP_KERNEL);
 143         if (!ops)
 144                 return -ENOMEM;
 145
 146         cpu_dp->orig_ethtool_ops = dev->ethtool_ops;
 147         if (cpu_dp->orig_ethtool_ops)
 148                 memcpy(ops, cpu_dp->orig_ethtool_ops, sizeof(*ops));
 149
 150         ops->get_sset_count = dsa_master_get_sset_count;
 151         ops->get_ethtool_stats = dsa_master_get_ethtool_stats;
 152         ops->get_strings = dsa_master_get_strings;
 153         ops->get_ethtool_phy_stats = dsa_master_get_ethtool_phy_stats;
 154
 155         dev->ethtool_ops = ops;
 156
 157         return 0;
 158 }
 159
 160 static void dsa_master_ethtool_teardown(struct net_device *dev)
 161 {
 162         struct dsa_port *cpu_dp = dev->dsa_ptr;
 163
 164         dev->ethtool_ops = cpu_dp->orig_ethtool_ops;
 165         cpu_dp->orig_ethtool_ops = NULL;
 166 }
 167
 168 static int dsa_master_ndo_setup(struct net_device *dev)
 169 {
 170         struct dsa_port *cpu_dp = dev->dsa_ptr;
 171         struct dsa_switch *ds = cpu_dp->ds;
 172         struct net_device_ops *ops;
 173
 174         if (dev->netdev_ops->ndo_get_phys_port_name)
 175                 return 0;
 176
 177         ops = devm_kzalloc(ds->dev, sizeof(*ops), GFP_KERNEL);
 178         if (!ops)
 179                 return -ENOMEM;
 180
 181         cpu_dp->orig_ndo_ops = dev->netdev_ops;
 182         if (cpu_dp->orig_ndo_ops)
 183                 memcpy(ops, cpu_dp->orig_ndo_ops, sizeof(*ops));
 184
 185         ops->ndo_get_phys_port_name = dsa_master_get_phys_port_name;
 186
 187         dev->netdev_ops  = ops;
 188
 189         return 0;
 190 }
 191
 192 static void dsa_master_ndo_teardown(struct net_device *dev)
 193 {
 194         struct dsa_port *cpu_dp = dev->dsa_ptr;
 195
 196         dev->netdev_ops = cpu_dp->orig_ndo_ops;
 197         cpu_dp->orig_ndo_ops = NULL;
 198 }
 199
 200 static ssize_t tagging_show(struct device *d, struct device_attribute *attr,
 201                             char *buf)
 202 {
 203         struct net_device *dev = to_net_dev(d);
 204         struct dsa_port *cpu_dp = dev->dsa_ptr;
 205
 206         return sprintf(buf, "%s\n",
 207                        dsa_tag_protocol_to_str(cpu_dp->tag_ops));
 208 }
 209 static DEVICE_ATTR_RO(tagging);
 210
 211 static struct attribute *dsa_slave_attrs[] = {
 212         &dev_attr_tagging.attr,
 213         NULL
 214 };
 215
 216 static const struct attribute_group dsa_group = {
 217         .name   = "dsa",
 218         .attrs  = dsa_slave_attrs,
 219 };
 220
 221 static void dsa_master_set_mtu(struct net_device *dev, struct dsa_port *cpu_dp)
 222 {
 223         unsigned int mtu = ETH_DATA_LEN + cpu_dp->tag_ops->overhead;
 224         int err;
 225
 226         rtnl_lock();
 227         if (mtu <= dev->max_mtu) {
 228                 err = dev_set_mtu(dev, mtu);
 229                 if (err)
 230                         netdev_dbg(dev, "Unable to set MTU to include for DSA overheads\n");
 231         }
 232         rtnl_unlock();
 233 }
 234
 235 static void dsa_master_reset_mtu(struct net_device *dev)
 236 {
 237         int err;
 238
 239         rtnl_lock();
 240         err = dev_set_mtu(dev, ETH_DATA_LEN);
 241         if (err)
 242                 netdev_dbg(dev,
 243                            "Unable to reset MTU to exclude DSA overheads\n");
 244         rtnl_unlock();
 245 }
 246
 247 static struct lock_class_key dsa_master_addr_list_lock_key;
 248
 249 int dsa_master_setup(struct net_device *dev, struct dsa_port *cpu_dp)
 250 {
 251         int ret;
 252
 253         dsa_master_set_mtu(dev,  cpu_dp);
 254
 255         /* If we use a tagging format that doesn't have an ethertype
 256          * field, make sure that all packets from this point on get
 257          * sent to the tag format's receive function.
 258          */
 259         wmb();
 260
 261         dev->dsa_ptr = cpu_dp;
 262         lockdep_set_class(&dev->addr_list_lock,
 263                           &dsa_master_addr_list_lock_key);
 264
 265         ret = dsa_master_ethtool_setup(dev);
 266         if (ret)
 267                 return ret;
 268
 269         ret = dsa_master_ndo_setup(dev);
 270         if (ret)
 271                 goto out_err_ethtool_teardown;
 272
 273         ret = sysfs_create_group(&dev->dev.kobj, &dsa_group);
 274         if (ret)
 275                 goto out_err_ndo_teardown;
 276
 277         return ret;
 278
 279 out_err_ndo_teardown:
 280         dsa_master_ndo_teardown(dev);
 281 out_err_ethtool_teardown:
 282         dsa_master_ethtool_teardown(dev);
 283         return ret;
 284 }
 285
 286 void dsa_master_teardown(struct net_device *dev)
 287 {
 288         sysfs_remove_group(&dev->dev.kobj, &dsa_group);
 289         dsa_master_ndo_teardown(dev);
 290         dsa_master_ethtool_teardown(dev);
 291         dsa_master_reset_mtu(dev);
 292
 293         dev->dsa_ptr = NULL;
 294
 295         /* If we used a tagging format that doesn't have an ethertype
 296          * field, make sure that all packets from this point get sent
 297          * without the tag and go through the regular receive path.
 298          */
 299         wmb();
 300 }