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
11enum 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
19static const struct sparx5_dcb_apptrust {
20	u8 selectors[IEEE_8021QAZ_APP_SEL_MAX + 1];
21	int nselectors;
22} *sparx5_port_apptrust[SPX5_PORTS];
23
24static 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 */
32static 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 */
46static 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 */
90static 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
122static 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
134static 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 */
231static 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
252static 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
267static 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
295out:
296	return err;
297}
298
299static 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
314static 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
328static 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
343static 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
371out:
372	return err;
373}
374
375const 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
384int 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