1 // SPDX-License-Identifier: GPL-2.0+
2
3 #include "lan966x_main.h"
4
5 enum lan966x_dcb_apptrust_values {
6 LAN966X_DCB_APPTRUST_EMPTY,
7 LAN966X_DCB_APPTRUST_DSCP,
8 LAN966X_DCB_APPTRUST_PCP,
9 LAN966X_DCB_APPTRUST_DSCP_PCP,
10 __LAN966X_DCB_APPTRUST_MAX
11 };
12
13 static const struct lan966x_dcb_apptrust {
14 u8 selectors[IEEE_8021QAZ_APP_SEL_MAX + 1];
15 int nselectors;
16 } *lan966x_port_apptrust[NUM_PHYS_PORTS];
17
18 static const char *lan966x_dcb_apptrust_names[__LAN966X_DCB_APPTRUST_MAX] = {
19 [LAN966X_DCB_APPTRUST_EMPTY] = "empty",
20 [LAN966X_DCB_APPTRUST_DSCP] = "dscp",
21 [LAN966X_DCB_APPTRUST_PCP] = "pcp",
22 [LAN966X_DCB_APPTRUST_DSCP_PCP] = "dscp pcp"
23 };
24
25 /* Lan966x supported apptrust policies */
26 static const struct lan966x_dcb_apptrust
27 lan966x_dcb_apptrust_policies[__LAN966X_DCB_APPTRUST_MAX] = {
28 /* Empty *must* be first */
29 [LAN966X_DCB_APPTRUST_EMPTY] = { { 0 }, 0 },
30 [LAN966X_DCB_APPTRUST_DSCP] = { { IEEE_8021QAZ_APP_SEL_DSCP }, 1 },
31 [LAN966X_DCB_APPTRUST_PCP] = { { DCB_APP_SEL_PCP }, 1 },
32 [LAN966X_DCB_APPTRUST_DSCP_PCP] = { { IEEE_8021QAZ_APP_SEL_DSCP,
33 DCB_APP_SEL_PCP }, 2 },
34 };
35
lan966x_dcb_apptrust_contains(int portno,u8 selector)36 static bool lan966x_dcb_apptrust_contains(int portno, u8 selector)
37 {
38 const struct lan966x_dcb_apptrust *conf = lan966x_port_apptrust[portno];
39
40 for (int i = 0; i < conf->nselectors; i++)
41 if (conf->selectors[i] == selector)
42 return true;
43
44 return false;
45 }
46
lan966x_dcb_app_update(struct net_device * dev)47 static void lan966x_dcb_app_update(struct net_device *dev)
48 {
49 struct dcb_ieee_app_prio_map dscp_rewr_map = {0};
50 struct dcb_rewr_prio_pcp_map pcp_rewr_map = {0};
51 struct lan966x_port *port = netdev_priv(dev);
52 struct lan966x_port_qos qos = {0};
53 struct dcb_app app_itr;
54 bool dscp_rewr = false;
55 bool pcp_rewr = false;
56
57 /* Get pcp ingress mapping */
58 for (int i = 0; i < ARRAY_SIZE(qos.pcp.map); i++) {
59 app_itr.selector = DCB_APP_SEL_PCP;
60 app_itr.protocol = i;
61 qos.pcp.map[i] = dcb_getapp(dev, &app_itr);
62 }
63
64 /* Get dscp ingress mapping */
65 for (int i = 0; i < ARRAY_SIZE(qos.dscp.map); i++) {
66 app_itr.selector = IEEE_8021QAZ_APP_SEL_DSCP;
67 app_itr.protocol = i;
68 qos.dscp.map[i] = dcb_getapp(dev, &app_itr);
69 }
70
71 /* Get default prio */
72 qos.default_prio = dcb_ieee_getapp_default_prio_mask(dev);
73 if (qos.default_prio)
74 qos.default_prio = fls(qos.default_prio) - 1;
75
76 /* Get pcp rewrite mapping */
77 dcb_getrewr_prio_pcp_mask_map(dev, &pcp_rewr_map);
78 for (int i = 0; i < ARRAY_SIZE(pcp_rewr_map.map); i++) {
79 if (!pcp_rewr_map.map[i])
80 continue;
81
82 pcp_rewr = true;
83 qos.pcp_rewr.map[i] = fls(pcp_rewr_map.map[i]) - 1;
84 }
85
86 /* Get dscp rewrite mapping */
87 dcb_getrewr_prio_dscp_mask_map(dev, &dscp_rewr_map);
88 for (int i = 0; i < ARRAY_SIZE(dscp_rewr_map.map); i++) {
89 if (!dscp_rewr_map.map[i])
90 continue;
91
92 dscp_rewr = true;
93 qos.dscp_rewr.map[i] = fls64(dscp_rewr_map.map[i]) - 1;
94 }
95
96 /* Enable use of pcp for queue classification */
97 if (lan966x_dcb_apptrust_contains(port->chip_port, DCB_APP_SEL_PCP)) {
98 qos.pcp.enable = true;
99
100 if (pcp_rewr)
101 qos.pcp_rewr.enable = true;
102 }
103
104 /* Enable use of dscp for queue classification */
105 if (lan966x_dcb_apptrust_contains(port->chip_port, IEEE_8021QAZ_APP_SEL_DSCP)) {
106 qos.dscp.enable = true;
107
108 if (dscp_rewr)
109 qos.dscp_rewr.enable = true;
110 }
111
112 lan966x_port_qos_set(port, &qos);
113 }
114
115 /* DSCP mapping is global for all ports, so set and delete app entries are
116 * replicated for each port.
117 */
lan966x_dcb_ieee_dscp_setdel(struct net_device * dev,struct dcb_app * app,int (* setdel)(struct net_device *,struct dcb_app *))118 static int lan966x_dcb_ieee_dscp_setdel(struct net_device *dev,
119 struct dcb_app *app,
120 int (*setdel)(struct net_device *,
121 struct dcb_app *))
122 {
123 struct lan966x_port *port = netdev_priv(dev);
124 struct lan966x *lan966x = port->lan966x;
125 int err;
126
127 for (int i = 0; i < NUM_PHYS_PORTS; i++) {
128 port = lan966x->ports[i];
129 if (!port)
130 continue;
131
132 err = setdel(port->dev, app);
133 if (err)
134 return err;
135 }
136
137 return 0;
138 }
139
lan966x_dcb_app_validate(struct net_device * dev,const struct dcb_app * app)140 static int lan966x_dcb_app_validate(struct net_device *dev,
141 const struct dcb_app *app)
142 {
143 int err = 0;
144
145 switch (app->selector) {
146 /* Default priority checks */
147 case IEEE_8021QAZ_APP_SEL_ETHERTYPE:
148 if (app->protocol)
149 err = -EINVAL;
150 else if (app->priority >= NUM_PRIO_QUEUES)
151 err = -ERANGE;
152 break;
153 /* Dscp checks */
154 case IEEE_8021QAZ_APP_SEL_DSCP:
155 if (app->protocol >= LAN966X_PORT_QOS_DSCP_COUNT)
156 err = -EINVAL;
157 else if (app->priority >= NUM_PRIO_QUEUES)
158 err = -ERANGE;
159 break;
160 /* Pcp checks */
161 case DCB_APP_SEL_PCP:
162 if (app->protocol >= LAN966X_PORT_QOS_PCP_DEI_COUNT)
163 err = -EINVAL;
164 else if (app->priority >= NUM_PRIO_QUEUES)
165 err = -ERANGE;
166 break;
167 default:
168 err = -EINVAL;
169 break;
170 }
171
172 if (err)
173 netdev_err(dev, "Invalid entry: %d:%d\n", app->protocol,
174 app->priority);
175
176 return err;
177 }
178
lan966x_dcb_ieee_delapp(struct net_device * dev,struct dcb_app * app)179 static int lan966x_dcb_ieee_delapp(struct net_device *dev, struct dcb_app *app)
180 {
181 int err;
182
183 if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
184 err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_ieee_delapp);
185 else
186 err = dcb_ieee_delapp(dev, app);
187
188 if (err)
189 return err;
190
191 lan966x_dcb_app_update(dev);
192
193 return 0;
194 }
195
lan966x_dcb_ieee_setapp(struct net_device * dev,struct dcb_app * app)196 static int lan966x_dcb_ieee_setapp(struct net_device *dev, struct dcb_app *app)
197 {
198 struct dcb_app app_itr;
199 int err;
200 u8 prio;
201
202 err = lan966x_dcb_app_validate(dev, app);
203 if (err)
204 return err;
205
206 /* Delete current mapping, if it exists */
207 prio = dcb_getapp(dev, app);
208 if (prio) {
209 app_itr = *app;
210 app_itr.priority = prio;
211 lan966x_dcb_ieee_delapp(dev, &app_itr);
212 }
213
214 if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
215 err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_ieee_setapp);
216 else
217 err = dcb_ieee_setapp(dev, app);
218
219 if (err)
220 return err;
221
222 lan966x_dcb_app_update(dev);
223
224 return 0;
225 }
226
lan966x_dcb_apptrust_validate(struct net_device * dev,u8 * selectors,int nselectors)227 static int lan966x_dcb_apptrust_validate(struct net_device *dev,
228 u8 *selectors,
229 int nselectors)
230 {
231 for (int i = 0; i < ARRAY_SIZE(lan966x_dcb_apptrust_policies); i++) {
232 bool match;
233
234 if (lan966x_dcb_apptrust_policies[i].nselectors != nselectors)
235 continue;
236
237 match = true;
238 for (int j = 0; j < nselectors; j++) {
239 if (lan966x_dcb_apptrust_policies[i].selectors[j] !=
240 *(selectors + j)) {
241 match = false;
242 break;
243 }
244 }
245 if (match)
246 return i;
247 }
248
249 netdev_err(dev, "Valid apptrust configurations are:\n");
250 for (int i = 0; i < ARRAY_SIZE(lan966x_dcb_apptrust_names); i++)
251 pr_info("order: %s\n", lan966x_dcb_apptrust_names[i]);
252
253 return -EOPNOTSUPP;
254 }
255
lan966x_dcb_setapptrust(struct net_device * dev,u8 * selectors,int nselectors)256 static int lan966x_dcb_setapptrust(struct net_device *dev,
257 u8 *selectors,
258 int nselectors)
259 {
260 struct lan966x_port *port = netdev_priv(dev);
261 int idx;
262
263 idx = lan966x_dcb_apptrust_validate(dev, selectors, nselectors);
264 if (idx < 0)
265 return idx;
266
267 lan966x_port_apptrust[port->chip_port] = &lan966x_dcb_apptrust_policies[idx];
268 lan966x_dcb_app_update(dev);
269
270 return 0;
271 }
272
lan966x_dcb_getapptrust(struct net_device * dev,u8 * selectors,int * nselectors)273 static int lan966x_dcb_getapptrust(struct net_device *dev, u8 *selectors,
274 int *nselectors)
275 {
276 struct lan966x_port *port = netdev_priv(dev);
277 const struct lan966x_dcb_apptrust *trust;
278
279 trust = lan966x_port_apptrust[port->chip_port];
280
281 memcpy(selectors, trust->selectors, trust->nselectors);
282 *nselectors = trust->nselectors;
283
284 return 0;
285 }
286
lan966x_dcb_delrewr(struct net_device * dev,struct dcb_app * app)287 static int lan966x_dcb_delrewr(struct net_device *dev, struct dcb_app *app)
288 {
289 int err;
290
291 if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
292 err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_delrewr);
293 else
294 err = dcb_delrewr(dev, app);
295
296 if (err < 0)
297 return err;
298
299 lan966x_dcb_app_update(dev);
300
301 return 0;
302 }
303
lan966x_dcb_setrewr(struct net_device * dev,struct dcb_app * app)304 static int lan966x_dcb_setrewr(struct net_device *dev, struct dcb_app *app)
305 {
306 struct dcb_app app_itr;
307 u16 proto;
308 int err;
309
310 err = lan966x_dcb_app_validate(dev, app);
311 if (err)
312 goto out;
313
314 /* Delete current mapping, if it exists. */
315 proto = dcb_getrewr(dev, app);
316 if (proto) {
317 app_itr = *app;
318 app_itr.protocol = proto;
319 lan966x_dcb_delrewr(dev, &app_itr);
320 }
321
322 if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
323 err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_setrewr);
324 else
325 err = dcb_setrewr(dev, app);
326
327 if (err)
328 goto out;
329
330 lan966x_dcb_app_update(dev);
331
332 out:
333 return err;
334 }
335
336 static const struct dcbnl_rtnl_ops lan966x_dcbnl_ops = {
337 .ieee_setapp = lan966x_dcb_ieee_setapp,
338 .ieee_delapp = lan966x_dcb_ieee_delapp,
339 .dcbnl_setapptrust = lan966x_dcb_setapptrust,
340 .dcbnl_getapptrust = lan966x_dcb_getapptrust,
341 .dcbnl_setrewr = lan966x_dcb_setrewr,
342 .dcbnl_delrewr = lan966x_dcb_delrewr,
343 };
344
lan966x_dcb_init(struct lan966x * lan966x)345 void lan966x_dcb_init(struct lan966x *lan966x)
346 {
347 for (int p = 0; p < lan966x->num_phys_ports; ++p) {
348 struct lan966x_port *port;
349
350 port = lan966x->ports[p];
351 if (!port)
352 continue;
353
354 port->dev->dcbnl_ops = &lan966x_dcbnl_ops;
355
356 lan966x_port_apptrust[port->chip_port] =
357 &lan966x_dcb_apptrust_policies[LAN966X_DCB_APPTRUST_DSCP_PCP];
358
359 /* Enable DSCP classification based on classified QoS class and
360 * DP, for all DSCP values, for all ports.
361 */
362 lan966x_port_qos_dscp_rewr_mode_set(port,
363 LAN966X_PORT_QOS_REWR_DSCP_ALL);
364 }
365 }
366