Commit | Line | Data |
---|---|---|
f2f23566 VD |
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 | { | |
2f657a60 | 19 | struct dsa_port *cpu_dp = dev->dsa_ptr; |
7ec764ee VD |
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; | |
f2f23566 VD |
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) | |
7ec764ee | 31 | ds->ops->get_ethtool_stats(ds, port, data + count); |
f2f23566 VD |
32 | } |
33 | ||
34 | static int dsa_master_get_sset_count(struct net_device *dev, int sset) | |
35 | { | |
2f657a60 | 36 | struct dsa_port *cpu_dp = dev->dsa_ptr; |
7ec764ee VD |
37 | const struct ethtool_ops *ops = cpu_dp->orig_ethtool_ops; |
38 | struct dsa_switch *ds = cpu_dp->ds; | |
f2f23566 VD |
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 | { | |
2f657a60 | 53 | struct dsa_port *cpu_dp = dev->dsa_ptr; |
7ec764ee VD |
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; | |
f2f23566 VD |
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 | ||
7ec764ee | 63 | snprintf(pfx, sizeof(pfx), "p%.2d", port); |
f2f23566 VD |
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 | */ | |
7ec764ee | 78 | ds->ops->get_strings(ds, port, ndata); |
f2f23566 VD |
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 | ||
17a22fcf | 88 | static int dsa_master_ethtool_setup(struct net_device *dev) |
f2f23566 | 89 | { |
2f657a60 | 90 | struct dsa_port *cpu_dp = dev->dsa_ptr; |
7ec764ee | 91 | struct dsa_switch *ds = cpu_dp->ds; |
f2f23566 VD |
92 | struct ethtool_ops *ops; |
93 | ||
94 | ops = devm_kzalloc(ds->dev, sizeof(*ops), GFP_KERNEL); | |
95 | if (!ops) | |
96 | return -ENOMEM; | |
97 | ||
7ec764ee VD |
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)); | |
f2f23566 VD |
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 | ||
17a22fcf | 111 | static void dsa_master_ethtool_teardown(struct net_device *dev) |
f2f23566 | 112 | { |
2f657a60 | 113 | struct dsa_port *cpu_dp = dev->dsa_ptr; |
f2f23566 | 114 | |
7ec764ee VD |
115 | dev->ethtool_ops = cpu_dp->orig_ethtool_ops; |
116 | cpu_dp->orig_ethtool_ops = NULL; | |
f2f23566 | 117 | } |
17a22fcf VD |
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 | } |