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