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 sparx5_port *port = netdev_priv(dev);
137	struct sparx5_port_qos_dscp_map *dscp_map;
138	struct sparx5_port_qos_pcp_map *pcp_map;
139	struct sparx5_port_qos qos = {0};
140	struct dcb_app app_itr = {0};
141	int portno = port->portno;
142	int i;
143
144	dscp_map = &qos.dscp.map;
145	pcp_map = &qos.pcp.map;
146
147	/* Get default prio. */
148	qos.default_prio = dcb_ieee_getapp_default_prio_mask(dev);
149	if (qos.default_prio)
150		qos.default_prio = fls(qos.default_prio) - 1;
151
152	/* Get dscp ingress mapping */
153	for (i = 0; i < ARRAY_SIZE(dscp_map->map); i++) {
154		app_itr.selector = IEEE_8021QAZ_APP_SEL_DSCP;
155		app_itr.protocol = i;
156		dscp_map->map[i] = dcb_getapp(dev, &app_itr);
157	}
158
159	/* Get pcp ingress mapping */
160	for (i = 0; i < ARRAY_SIZE(pcp_map->map); i++) {
161		app_itr.selector = DCB_APP_SEL_PCP;
162		app_itr.protocol = i;
163		pcp_map->map[i] = dcb_getapp(dev, &app_itr);
164	}
165
166	/* Enable use of pcp for queue classification ? */
167	if (sparx5_dcb_apptrust_contains(portno, DCB_APP_SEL_PCP)) {
168		qos.pcp.qos_enable = true;
169		qos.pcp.dp_enable = qos.pcp.qos_enable;
170	}
171
172	/* Enable use of dscp for queue classification ? */
173	if (sparx5_dcb_apptrust_contains(portno, IEEE_8021QAZ_APP_SEL_DSCP)) {
174		qos.dscp.qos_enable = true;
175		qos.dscp.dp_enable = qos.dscp.qos_enable;
176	}
177
178	return sparx5_port_qos_set(port, &qos);
179}
180
181/* Set or delete dscp app entry.
182 *
183 * Dscp mapping is global for all ports, so set and delete app entries are
184 * replicated for each port.
185 */
186static int sparx5_dcb_ieee_dscp_setdel_app(struct net_device *dev,
187					   struct dcb_app *app, bool del)
188{
189	struct sparx5_port *port = netdev_priv(dev);
190	struct dcb_app apps[SPX5_PORTS];
191	struct sparx5_port *port_itr;
192	int err, i;
193
194	for (i = 0; i < SPX5_PORTS; i++) {
195		port_itr = port->sparx5->ports[i];
196		if (!port_itr)
197			continue;
198		memcpy(&apps[i], app, sizeof(struct dcb_app));
199		if (del)
200			err = dcb_ieee_delapp(port_itr->ndev, &apps[i]);
201		else
202			err = dcb_ieee_setapp(port_itr->ndev, &apps[i]);
203		if (err)
204			return err;
205	}
206
207	return 0;
208}
209
210static int sparx5_dcb_ieee_setapp(struct net_device *dev, struct dcb_app *app)
211{
212	struct dcb_app app_itr;
213	int err = 0;
214	u8 prio;
215
216	err = sparx5_dcb_app_validate(dev, app);
217	if (err)
218		goto out;
219
220	/* Delete current mapping, if it exists */
221	prio = dcb_getapp(dev, app);
222	if (prio) {
223		app_itr = *app;
224		app_itr.priority = prio;
225		dcb_ieee_delapp(dev, &app_itr);
226	}
227
228	if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
229		err = sparx5_dcb_ieee_dscp_setdel_app(dev, app, false);
230	else
231		err = dcb_ieee_setapp(dev, app);
232
233	if (err)
234		goto out;
235
236	sparx5_dcb_app_update(dev);
237
238out:
239	return err;
240}
241
242static int sparx5_dcb_ieee_delapp(struct net_device *dev, struct dcb_app *app)
243{
244	int err;
245
246	if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
247		err = sparx5_dcb_ieee_dscp_setdel_app(dev, app, true);
248	else
249		err = dcb_ieee_delapp(dev, app);
250
251	if (err < 0)
252		return err;
253
254	return sparx5_dcb_app_update(dev);
255}
256
257static int sparx5_dcb_setapptrust(struct net_device *dev, u8 *selectors,
258				  int nselectors)
259{
260	struct sparx5_port *port = netdev_priv(dev);
261	int err = 0, idx;
262
263	idx = sparx5_dcb_apptrust_validate(dev, selectors, nselectors, &err);
264	if (err < 0)
265		return err;
266
267	sparx5_port_apptrust[port->portno] = &sparx5_dcb_apptrust_policies[idx];
268
269	return sparx5_dcb_app_update(dev);
270}
271
272static int sparx5_dcb_getapptrust(struct net_device *dev, u8 *selectors,
273				  int *nselectors)
274{
275	struct sparx5_port *port = netdev_priv(dev);
276	const struct sparx5_dcb_apptrust *trust;
277
278	trust = sparx5_port_apptrust[port->portno];
279
280	memcpy(selectors, trust->selectors, trust->nselectors);
281	*nselectors = trust->nselectors;
282
283	return 0;
284}
285
286const struct dcbnl_rtnl_ops sparx5_dcbnl_ops = {
287	.ieee_setapp = sparx5_dcb_ieee_setapp,
288	.ieee_delapp = sparx5_dcb_ieee_delapp,
289	.dcbnl_setapptrust = sparx5_dcb_setapptrust,
290	.dcbnl_getapptrust = sparx5_dcb_getapptrust,
291};
292
293int sparx5_dcb_init(struct sparx5 *sparx5)
294{
295	struct sparx5_port *port;
296	int i;
297
298	for (i = 0; i < SPX5_PORTS; i++) {
299		port = sparx5->ports[i];
300		if (!port)
301			continue;
302		port->ndev->dcbnl_ops = &sparx5_dcbnl_ops;
303		/* Initialize [dscp, pcp] default trust */
304		sparx5_port_apptrust[port->portno] =
305			&sparx5_dcb_apptrust_policies
306				[SPARX5_DCB_APPTRUST_DSCP_PCP];
307	}
308
309	return 0;
310}