1 // SPDX-License-Identifier: GPL-2.0+
2 /* Microchip Sparx5 Switch driver
3  *
4  * Copyright (c) 2022 Microchip Technology Inc. and its subsidiaries.
5  */
6 
7 #include <net/dcbnl.h>
8 
9 #include "sparx5_port.h"
10 
11 enum sparx5_dcb_apptrust_values {
12 	SPARX5_DCB_APPTRUST_EMPTY,
13 	SPARX5_DCB_APPTRUST_DSCP,
14 	SPARX5_DCB_APPTRUST_PCP,
15 	SPARX5_DCB_APPTRUST_DSCP_PCP,
16 	__SPARX5_DCB_APPTRUST_MAX
17 };
18 
19 static const struct sparx5_dcb_apptrust {
20 	u8 selectors[IEEE_8021QAZ_APP_SEL_MAX + 1];
21 	int nselectors;
22 } *sparx5_port_apptrust[SPX5_PORTS];
23 
24 static const char *sparx5_dcb_apptrust_names[__SPARX5_DCB_APPTRUST_MAX] = {
25 	[SPARX5_DCB_APPTRUST_EMPTY]    = "empty",
26 	[SPARX5_DCB_APPTRUST_DSCP]     = "dscp",
27 	[SPARX5_DCB_APPTRUST_PCP]      = "pcp",
28 	[SPARX5_DCB_APPTRUST_DSCP_PCP] = "dscp pcp"
29 };
30 
31 /* Sparx5 supported apptrust policies */
32 static const struct sparx5_dcb_apptrust
33 	sparx5_dcb_apptrust_policies[__SPARX5_DCB_APPTRUST_MAX] = {
34 	/* Empty *must* be first */
35 	[SPARX5_DCB_APPTRUST_EMPTY]    = { { 0 }, 0 },
36 	[SPARX5_DCB_APPTRUST_DSCP]     = { { IEEE_8021QAZ_APP_SEL_DSCP }, 1 },
37 	[SPARX5_DCB_APPTRUST_PCP]      = { { DCB_APP_SEL_PCP }, 1 },
38 	[SPARX5_DCB_APPTRUST_DSCP_PCP] = { { IEEE_8021QAZ_APP_SEL_DSCP,
39 					     DCB_APP_SEL_PCP }, 2 },
40 };
41 
42 /* Validate app entry.
43  *
44  * Check for valid selectors and valid protocol and priority ranges.
45  */
sparx5_dcb_app_validate(struct net_device * dev,const struct dcb_app * app)46 static int sparx5_dcb_app_validate(struct net_device *dev,
47 				   const struct dcb_app *app)
48 {
49 	int err = 0;
50 
51 	switch (app->selector) {
52 	/* Default priority checks */
53 	case IEEE_8021QAZ_APP_SEL_ETHERTYPE:
54 		if (app->protocol != 0)
55 			err = -EINVAL;
56 		else if (app->priority >= SPX5_PRIOS)
57 			err = -ERANGE;
58 		break;
59 	/* Dscp checks */
60 	case IEEE_8021QAZ_APP_SEL_DSCP:
61 		if (app->protocol >= SPARX5_PORT_QOS_DSCP_COUNT)
62 			err = -EINVAL;
63 		else if (app->priority >= SPX5_PRIOS)
64 			err = -ERANGE;
65 		break;
66 	/* Pcp checks */
67 	case DCB_APP_SEL_PCP:
68 		if (app->protocol >= SPARX5_PORT_QOS_PCP_DEI_COUNT)
69 			err = -EINVAL;
70 		else if (app->priority >= SPX5_PRIOS)
71 			err = -ERANGE;
72 		break;
73 	default:
74 		err = -EINVAL;
75 		break;
76 	}
77 
78 	if (err)
79 		netdev_err(dev, "Invalid entry: %d:%d\n", app->protocol,
80 			   app->priority);
81 
82 	return err;
83 }
84 
85 /* Validate apptrust configuration.
86  *
87  * Return index of supported apptrust configuration if valid, otherwise return
88  * error.
89  */
sparx5_dcb_apptrust_validate(struct net_device * dev,u8 * selectors,int nselectors,int * err)90 static int sparx5_dcb_apptrust_validate(struct net_device *dev, u8 *selectors,
91 					int nselectors, int *err)
92 {
93 	bool match = false;
94 	int i, ii;
95 
96 	for (i = 0; i < ARRAY_SIZE(sparx5_dcb_apptrust_policies); i++) {
97 		if (sparx5_dcb_apptrust_policies[i].nselectors != nselectors)
98 			continue;
99 		match = true;
100 		for (ii = 0; ii < nselectors; ii++) {
101 			if (sparx5_dcb_apptrust_policies[i].selectors[ii] !=
102 			    *(selectors + ii)) {
103 				match = false;
104 				break;
105 			}
106 		}
107 		if (match)
108 			break;
109 	}
110 
111 	/* Requested trust configuration is not supported */
112 	if (!match) {
113 		netdev_err(dev, "Valid apptrust configurations are:\n");
114 		for (i = 0; i < ARRAY_SIZE(sparx5_dcb_apptrust_names); i++)
115 			pr_info("order: %s\n", sparx5_dcb_apptrust_names[i]);
116 		*err = -EOPNOTSUPP;
117 	}
118 
119 	return i;
120 }
121 
sparx5_dcb_apptrust_contains(int portno,u8 selector)122 static bool sparx5_dcb_apptrust_contains(int portno, u8 selector)
123 {
124 	const struct sparx5_dcb_apptrust *conf = sparx5_port_apptrust[portno];
125 	int i;
126 
127 	for (i = 0; i < conf->nselectors; i++)
128 		if (conf->selectors[i] == selector)
129 			return true;
130 
131 	return false;
132 }
133 
sparx5_dcb_app_update(struct net_device * dev)134 static int sparx5_dcb_app_update(struct net_device *dev)
135 {
136 	struct dcb_ieee_app_prio_map dscp_rewr_map = {0};
137 	struct dcb_rewr_prio_pcp_map pcp_rewr_map = {0};
138 	struct sparx5_port *port = netdev_priv(dev);
139 	struct sparx5_port_qos_dscp_map *dscp_map;
140 	struct sparx5_port_qos_pcp_map *pcp_map;
141 	struct sparx5_port_qos qos = {0};
142 	struct dcb_app app_itr = {0};
143 	int portno = port->portno;
144 	bool dscp_rewr = false;
145 	bool pcp_rewr = false;
146 	u16 dscp;
147 	int i;
148 
149 	dscp_map = &qos.dscp.map;
150 	pcp_map = &qos.pcp.map;
151 
152 	/* Get default prio. */
153 	qos.default_prio = dcb_ieee_getapp_default_prio_mask(dev);
154 	if (qos.default_prio)
155 		qos.default_prio = fls(qos.default_prio) - 1;
156 
157 	/* Get dscp ingress mapping */
158 	for (i = 0; i < ARRAY_SIZE(dscp_map->map); i++) {
159 		app_itr.selector = IEEE_8021QAZ_APP_SEL_DSCP;
160 		app_itr.protocol = i;
161 		dscp_map->map[i] = dcb_getapp(dev, &app_itr);
162 	}
163 
164 	/* Get pcp ingress mapping */
165 	for (i = 0; i < ARRAY_SIZE(pcp_map->map); i++) {
166 		app_itr.selector = DCB_APP_SEL_PCP;
167 		app_itr.protocol = i;
168 		pcp_map->map[i] = dcb_getapp(dev, &app_itr);
169 	}
170 
171 	/* Get pcp rewrite mapping */
172 	dcb_getrewr_prio_pcp_mask_map(dev, &pcp_rewr_map);
173 	for (i = 0; i < ARRAY_SIZE(pcp_rewr_map.map); i++) {
174 		if (!pcp_rewr_map.map[i])
175 			continue;
176 		pcp_rewr = true;
177 		qos.pcp_rewr.map.map[i] = fls(pcp_rewr_map.map[i]) - 1;
178 	}
179 
180 	/* Get dscp rewrite mapping */
181 	dcb_getrewr_prio_dscp_mask_map(dev, &dscp_rewr_map);
182 	for (i = 0; i < ARRAY_SIZE(dscp_rewr_map.map); i++) {
183 		if (!dscp_rewr_map.map[i])
184 			continue;
185 
186 		/* The rewrite table of the switch has 32 entries; one for each
187 		 * priority for each DP level. Currently, the rewrite map does
188 		 * not indicate DP level, so we map classified QoS class to
189 		 * classified DSCP, for each classified DP level. Rewrite of
190 		 * DSCP is only enabled, if we have active mappings.
191 		 */
192 		dscp_rewr = true;
193 		dscp = fls64(dscp_rewr_map.map[i]) - 1;
194 		qos.dscp_rewr.map.map[i] = dscp;      /* DP 0 */
195 		qos.dscp_rewr.map.map[i + 8] = dscp;  /* DP 1 */
196 		qos.dscp_rewr.map.map[i + 16] = dscp; /* DP 2 */
197 		qos.dscp_rewr.map.map[i + 24] = dscp; /* DP 3 */
198 	}
199 
200 	/* Enable use of pcp for queue classification ? */
201 	if (sparx5_dcb_apptrust_contains(portno, DCB_APP_SEL_PCP)) {
202 		qos.pcp.qos_enable = true;
203 		qos.pcp.dp_enable = qos.pcp.qos_enable;
204 		/* Enable rewrite of PCP and DEI if PCP is trusted *and* rewrite
205 		 * table is not empty.
206 		 */
207 		if (pcp_rewr)
208 			qos.pcp_rewr.enable = true;
209 	}
210 
211 	/* Enable use of dscp for queue classification ? */
212 	if (sparx5_dcb_apptrust_contains(portno, IEEE_8021QAZ_APP_SEL_DSCP)) {
213 		qos.dscp.qos_enable = true;
214 		qos.dscp.dp_enable = qos.dscp.qos_enable;
215 		if (dscp_rewr)
216 			/* Do not enable rewrite if no mappings are active, as
217 			 * classified DSCP will then be zero for all classified
218 			 * QoS class and DP combinations.
219 			 */
220 			qos.dscp_rewr.enable = true;
221 	}
222 
223 	return sparx5_port_qos_set(port, &qos);
224 }
225 
226 /* Set or delete DSCP app entry.
227  *
228  * DSCP mapping is global for all ports, so set and delete app entries are
229  * replicated for each port.
230  */
sparx5_dcb_ieee_dscp_setdel(struct net_device * dev,struct dcb_app * app,int (* setdel)(struct net_device *,struct dcb_app *))231 static int sparx5_dcb_ieee_dscp_setdel(struct net_device *dev,
232 				       struct dcb_app *app,
233 				       int (*setdel)(struct net_device *,
234 						     struct dcb_app *))
235 {
236 	struct sparx5_port *port = netdev_priv(dev);
237 	struct sparx5_port *port_itr;
238 	int err, i;
239 
240 	for (i = 0; i < SPX5_PORTS; i++) {
241 		port_itr = port->sparx5->ports[i];
242 		if (!port_itr)
243 			continue;
244 		err = setdel(port_itr->ndev, app);
245 		if (err)
246 			return err;
247 	}
248 
249 	return 0;
250 }
251 
sparx5_dcb_ieee_delapp(struct net_device * dev,struct dcb_app * app)252 static int sparx5_dcb_ieee_delapp(struct net_device *dev, struct dcb_app *app)
253 {
254 	int err;
255 
256 	if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
257 		err = sparx5_dcb_ieee_dscp_setdel(dev, app, dcb_ieee_delapp);
258 	else
259 		err = dcb_ieee_delapp(dev, app);
260 
261 	if (err < 0)
262 		return err;
263 
264 	return sparx5_dcb_app_update(dev);
265 }
266 
sparx5_dcb_ieee_setapp(struct net_device * dev,struct dcb_app * app)267 static int sparx5_dcb_ieee_setapp(struct net_device *dev, struct dcb_app *app)
268 {
269 	struct dcb_app app_itr;
270 	int err = 0;
271 	u8 prio;
272 
273 	err = sparx5_dcb_app_validate(dev, app);
274 	if (err)
275 		goto out;
276 
277 	/* Delete current mapping, if it exists */
278 	prio = dcb_getapp(dev, app);
279 	if (prio) {
280 		app_itr = *app;
281 		app_itr.priority = prio;
282 		sparx5_dcb_ieee_delapp(dev, &app_itr);
283 	}
284 
285 	if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
286 		err = sparx5_dcb_ieee_dscp_setdel(dev, app, dcb_ieee_setapp);
287 	else
288 		err = dcb_ieee_setapp(dev, app);
289 
290 	if (err)
291 		goto out;
292 
293 	sparx5_dcb_app_update(dev);
294 
295 out:
296 	return err;
297 }
298 
sparx5_dcb_setapptrust(struct net_device * dev,u8 * selectors,int nselectors)299 static int sparx5_dcb_setapptrust(struct net_device *dev, u8 *selectors,
300 				  int nselectors)
301 {
302 	struct sparx5_port *port = netdev_priv(dev);
303 	int err = 0, idx;
304 
305 	idx = sparx5_dcb_apptrust_validate(dev, selectors, nselectors, &err);
306 	if (err < 0)
307 		return err;
308 
309 	sparx5_port_apptrust[port->portno] = &sparx5_dcb_apptrust_policies[idx];
310 
311 	return sparx5_dcb_app_update(dev);
312 }
313 
sparx5_dcb_getapptrust(struct net_device * dev,u8 * selectors,int * nselectors)314 static int sparx5_dcb_getapptrust(struct net_device *dev, u8 *selectors,
315 				  int *nselectors)
316 {
317 	struct sparx5_port *port = netdev_priv(dev);
318 	const struct sparx5_dcb_apptrust *trust;
319 
320 	trust = sparx5_port_apptrust[port->portno];
321 
322 	memcpy(selectors, trust->selectors, trust->nselectors);
323 	*nselectors = trust->nselectors;
324 
325 	return 0;
326 }
327 
sparx5_dcb_delrewr(struct net_device * dev,struct dcb_app * app)328 static int sparx5_dcb_delrewr(struct net_device *dev, struct dcb_app *app)
329 {
330 	int err;
331 
332 	if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
333 		err = sparx5_dcb_ieee_dscp_setdel(dev, app, dcb_delrewr);
334 	else
335 		err = dcb_delrewr(dev, app);
336 
337 	if (err < 0)
338 		return err;
339 
340 	return sparx5_dcb_app_update(dev);
341 }
342 
sparx5_dcb_setrewr(struct net_device * dev,struct dcb_app * app)343 static int sparx5_dcb_setrewr(struct net_device *dev, struct dcb_app *app)
344 {
345 	struct dcb_app app_itr;
346 	int err = 0;
347 	u16 proto;
348 
349 	err = sparx5_dcb_app_validate(dev, app);
350 	if (err)
351 		goto out;
352 
353 	/* Delete current mapping, if it exists. */
354 	proto = dcb_getrewr(dev, app);
355 	if (proto) {
356 		app_itr = *app;
357 		app_itr.protocol = proto;
358 		sparx5_dcb_delrewr(dev, &app_itr);
359 	}
360 
361 	if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
362 		err = sparx5_dcb_ieee_dscp_setdel(dev, app, dcb_setrewr);
363 	else
364 		err = dcb_setrewr(dev, app);
365 
366 	if (err)
367 		goto out;
368 
369 	sparx5_dcb_app_update(dev);
370 
371 out:
372 	return err;
373 }
374 
375 const struct dcbnl_rtnl_ops sparx5_dcbnl_ops = {
376 	.ieee_setapp = sparx5_dcb_ieee_setapp,
377 	.ieee_delapp = sparx5_dcb_ieee_delapp,
378 	.dcbnl_setapptrust = sparx5_dcb_setapptrust,
379 	.dcbnl_getapptrust = sparx5_dcb_getapptrust,
380 	.dcbnl_setrewr = sparx5_dcb_setrewr,
381 	.dcbnl_delrewr = sparx5_dcb_delrewr,
382 };
383 
sparx5_dcb_init(struct sparx5 * sparx5)384 int sparx5_dcb_init(struct sparx5 *sparx5)
385 {
386 	struct sparx5_port *port;
387 	int i;
388 
389 	for (i = 0; i < SPX5_PORTS; i++) {
390 		port = sparx5->ports[i];
391 		if (!port)
392 			continue;
393 		port->ndev->dcbnl_ops = &sparx5_dcbnl_ops;
394 		/* Initialize [dscp, pcp] default trust */
395 		sparx5_port_apptrust[port->portno] =
396 			&sparx5_dcb_apptrust_policies
397 				[SPARX5_DCB_APPTRUST_DSCP_PCP];
398 
399 		/* Enable DSCP classification based on classified QoS class and
400 		 * DP, for all DSCP values, for all ports.
401 		 */
402 		sparx5_port_qos_dscp_rewr_mode_set(port,
403 						   SPARX5_PORT_REW_DSCP_ALL);
404 	}
405 
406 	return 0;
407 }
408