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->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 void dsa_master_get_ethtool_phy_stats(struct net_device *dev, 35 struct ethtool_stats *stats, 36 uint64_t *data) 37 { 38 struct dsa_port *cpu_dp = dev->dsa_ptr; 39 const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops; 40 struct dsa_switch *ds = cpu_dp->ds; 41 int port = cpu_dp->index; 42 int count = 0; 43 44 if (dev->phydev && !ops->get_ethtool_phy_stats) { 45 count = phy_ethtool_get_sset_count(dev->phydev); 46 if (count >= 0) 47 phy_ethtool_get_stats(dev->phydev, stats, data); 48 } else if (ops->get_sset_count && ops->get_ethtool_phy_stats) { 49 count = ops->get_sset_count(dev, ETH_SS_PHY_STATS); 50 ops->get_ethtool_phy_stats(dev, stats, data); 51 } 52 53 if (count < 0) 54 count = 0; 55 56 if (ds->ops->get_ethtool_phy_stats) 57 ds->ops->get_ethtool_phy_stats(ds, port, data + count); 58 } 59 60 static int dsa_master_get_sset_count(struct net_device *dev, int sset) 61 { 62 struct dsa_port *cpu_dp = dev->dsa_ptr; 63 const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops; 64 struct dsa_switch *ds = cpu_dp->ds; 65 int count = 0; 66 67 if (sset == ETH_SS_PHY_STATS && dev->phydev && 68 !ops->get_ethtool_phy_stats) 69 count = phy_ethtool_get_sset_count(dev->phydev); 70 else if (ops->get_sset_count) 71 count = ops->get_sset_count(dev, sset); 72 73 if (count < 0) 74 count = 0; 75 76 if (ds->ops->get_sset_count) 77 count += ds->ops->get_sset_count(ds, cpu_dp->index, sset); 78 79 return count; 80 } 81 82 static void dsa_master_get_strings(struct net_device *dev, uint32_t stringset, 83 uint8_t *data) 84 { 85 struct dsa_port *cpu_dp = dev->dsa_ptr; 86 const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops; 87 struct dsa_switch *ds = cpu_dp->ds; 88 int port = cpu_dp->index; 89 int len = ETH_GSTRING_LEN; 90 int mcount = 0, count; 91 unsigned int i; 92 uint8_t pfx[4]; 93 uint8_t *ndata; 94 95 snprintf(pfx, sizeof(pfx), "p%.2d", port); 96 /* We do not want to be NULL-terminated, since this is a prefix */ 97 pfx[sizeof(pfx) - 1] = '_'; 98 99 if (stringset == ETH_SS_PHY_STATS && dev->phydev && 100 !ops->get_ethtool_phy_stats) { 101 mcount = phy_ethtool_get_sset_count(dev->phydev); 102 if (mcount < 0) 103 mcount = 0; 104 else 105 phy_ethtool_get_strings(dev->phydev, data); 106 } else if (ops->get_sset_count && ops->get_strings) { 107 mcount = ops->get_sset_count(dev, stringset); 108 if (mcount < 0) 109 mcount = 0; 110 ops->get_strings(dev, stringset, data); 111 } 112 113 if (ds->ops->get_strings) { 114 ndata = data + mcount * len; 115 /* This function copies ETH_GSTRINGS_LEN bytes, we will mangle 116 * the output after to prepend our CPU port prefix we 117 * constructed earlier 118 */ 119 ds->ops->get_strings(ds, port, stringset, ndata); 120 count = ds->ops->get_sset_count(ds, port, stringset); 121 for (i = 0; i < count; i++) { 122 memmove(ndata + (i * len + sizeof(pfx)), 123 ndata + i * len, len - sizeof(pfx)); 124 memcpy(ndata + i * len, pfx, sizeof(pfx)); 125 } 126 } 127 } 128 129 static int dsa_master_ethtool_setup(struct net_device *dev) 130 { 131 struct dsa_port *cpu_dp = dev->dsa_ptr; 132 struct dsa_switch *ds = cpu_dp->ds; 133 struct ethtool_ops *ops; 134 135 ops = devm_kzalloc(ds->dev, sizeof(*ops), GFP_KERNEL); 136 if (!ops) 137 return -ENOMEM; 138 139 cpu_dp->orig_ethtool_ops = dev->ethtool_ops; 140 if (cpu_dp->orig_ethtool_ops) 141 memcpy(ops, cpu_dp->orig_ethtool_ops, sizeof(*ops)); 142 143 ops->get_sset_count = dsa_master_get_sset_count; 144 ops->get_ethtool_stats = dsa_master_get_ethtool_stats; 145 ops->get_strings = dsa_master_get_strings; 146 ops->get_ethtool_phy_stats = dsa_master_get_ethtool_phy_stats; 147 148 dev->ethtool_ops = ops; 149 150 return 0; 151 } 152 153 static void dsa_master_ethtool_teardown(struct net_device *dev) 154 { 155 struct dsa_port *cpu_dp = dev->dsa_ptr; 156 157 dev->ethtool_ops = cpu_dp->orig_ethtool_ops; 158 cpu_dp->orig_ethtool_ops = NULL; 159 } 160 161 int dsa_master_setup(struct net_device *dev, struct dsa_port *cpu_dp) 162 { 163 /* If we use a tagging format that doesn't have an ethertype 164 * field, make sure that all packets from this point on get 165 * sent to the tag format's receive function. 166 */ 167 wmb(); 168 169 dev->dsa_ptr = cpu_dp; 170 171 return dsa_master_ethtool_setup(dev); 172 } 173 174 void dsa_master_teardown(struct net_device *dev) 175 { 176 dsa_master_ethtool_teardown(dev); 177 178 dev->dsa_ptr = NULL; 179 180 /* If we used a tagging format that doesn't have an ethertype 181 * field, make sure that all packets from this point get sent 182 * without the tag and go through the regular receive path. 183 */ 184 wmb(); 185 } 186