1 /* 2 * Handling of a master device, switching frames via its switch fabric CPU port 3 * 4 * Copyright (c) 2017 Savoir-faire Linux Inc. 5 * Vivien Didelot <vivien.didelot@savoirfairelinux.com> 6 * 7 * This program is free software; you can redistribute it and/or modify 8 * it under the terms of the GNU General Public License as published by 9 * the Free Software Foundation; either version 2 of the License, or 10 * (at your option) any later version. 11 */ 12 13 #include "dsa_priv.h" 14 15 static void dsa_master_get_ethtool_stats(struct net_device *dev, 16 struct ethtool_stats *stats, 17 uint64_t *data) 18 { 19 struct dsa_port *cpu_dp = dev->dsa_ptr; 20 const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops; 21 struct dsa_switch *ds = cpu_dp->ds; 22 int port = cpu_dp->index; 23 int count = 0; 24 25 if (ops && ops->get_sset_count && ops->get_ethtool_stats) { 26 count = ops->get_sset_count(dev, ETH_SS_STATS); 27 ops->get_ethtool_stats(dev, stats, data); 28 } 29 30 if (ds->ops->get_ethtool_stats) 31 ds->ops->get_ethtool_stats(ds, port, data + count); 32 } 33 34 static int dsa_master_get_sset_count(struct net_device *dev, int sset) 35 { 36 struct dsa_port *cpu_dp = dev->dsa_ptr; 37 const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops; 38 struct dsa_switch *ds = cpu_dp->ds; 39 int count = 0; 40 41 if (ops && ops->get_sset_count) 42 count += ops->get_sset_count(dev, sset); 43 44 if (sset == ETH_SS_STATS && ds->ops->get_sset_count) 45 count += ds->ops->get_sset_count(ds); 46 47 return count; 48 } 49 50 static void dsa_master_get_strings(struct net_device *dev, uint32_t stringset, 51 uint8_t *data) 52 { 53 struct dsa_port *cpu_dp = dev->dsa_ptr; 54 const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops; 55 struct dsa_switch *ds = cpu_dp->ds; 56 int port = cpu_dp->index; 57 int len = ETH_GSTRING_LEN; 58 int mcount = 0, count; 59 unsigned int i; 60 uint8_t pfx[4]; 61 uint8_t *ndata; 62 63 snprintf(pfx, sizeof(pfx), "p%.2d", port); 64 /* We do not want to be NULL-terminated, since this is a prefix */ 65 pfx[sizeof(pfx) - 1] = '_'; 66 67 if (ops && ops->get_sset_count && ops->get_strings) { 68 mcount = ops->get_sset_count(dev, ETH_SS_STATS); 69 ops->get_strings(dev, stringset, data); 70 } 71 72 if (stringset == ETH_SS_STATS && ds->ops->get_strings) { 73 ndata = data + mcount * len; 74 /* This function copies ETH_GSTRINGS_LEN bytes, we will mangle 75 * the output after to prepend our CPU port prefix we 76 * constructed earlier 77 */ 78 ds->ops->get_strings(ds, port, ndata); 79 count = ds->ops->get_sset_count(ds); 80 for (i = 0; i < count; i++) { 81 memmove(ndata + (i * len + sizeof(pfx)), 82 ndata + i * len, len - sizeof(pfx)); 83 memcpy(ndata + i * len, pfx, sizeof(pfx)); 84 } 85 } 86 } 87 88 static int dsa_master_ethtool_setup(struct net_device *dev) 89 { 90 struct dsa_port *cpu_dp = dev->dsa_ptr; 91 struct dsa_switch *ds = cpu_dp->ds; 92 struct ethtool_ops *ops; 93 94 ops = devm_kzalloc(ds->dev, sizeof(*ops), GFP_KERNEL); 95 if (!ops) 96 return -ENOMEM; 97 98 cpu_dp->orig_ethtool_ops = dev->ethtool_ops; 99 if (cpu_dp->orig_ethtool_ops) 100 memcpy(ops, cpu_dp->orig_ethtool_ops, sizeof(*ops)); 101 102 ops->get_sset_count = dsa_master_get_sset_count; 103 ops->get_ethtool_stats = dsa_master_get_ethtool_stats; 104 ops->get_strings = dsa_master_get_strings; 105 106 dev->ethtool_ops = ops; 107 108 return 0; 109 } 110 111 static void dsa_master_ethtool_teardown(struct net_device *dev) 112 { 113 struct dsa_port *cpu_dp = dev->dsa_ptr; 114 115 dev->ethtool_ops = cpu_dp->orig_ethtool_ops; 116 cpu_dp->orig_ethtool_ops = NULL; 117 } 118 119 int dsa_master_setup(struct net_device *dev, struct dsa_port *cpu_dp) 120 { 121 /* If we use a tagging format that doesn't have an ethertype 122 * field, make sure that all packets from this point on get 123 * sent to the tag format's receive function. 124 */ 125 wmb(); 126 127 dev->dsa_ptr = cpu_dp; 128 129 return dsa_master_ethtool_setup(dev); 130 } 131 132 void dsa_master_teardown(struct net_device *dev) 133 { 134 dsa_master_ethtool_teardown(dev); 135 136 dev->dsa_ptr = NULL; 137 138 /* If we used a tagging format that doesn't have an ethertype 139 * field, make sure that all packets from this point get sent 140 * without the tag and go through the regular receive path. 141 */ 142 wmb(); 143 } 144