1// SPDX-License-Identifier: GPL-2.0+
2
3#include "lan966x_main.h"
4
5enum 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
13static 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
18static 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 */
26static 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
36static 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
47static 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 */
118static 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
140static 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
179static 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
196static 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
227static 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
256static 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
273static 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
287static 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
304static 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
332out:
333	return err;
334}
335
336static 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
345void 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