1 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
2 /* Copyright (c) 2016-2018 Mellanox Technologies. All rights reserved */
3 
4 #include <linux/netdevice.h>
5 #include <linux/string.h>
6 #include <linux/bitops.h>
7 #include <net/dcbnl.h>
8 
9 #include "spectrum.h"
10 #include "reg.h"
11 
12 static u8 mlxsw_sp_dcbnl_getdcbx(struct net_device __always_unused *dev)
13 {
14 	return DCB_CAP_DCBX_HOST | DCB_CAP_DCBX_VER_IEEE;
15 }
16 
17 static u8 mlxsw_sp_dcbnl_setdcbx(struct net_device __always_unused *dev,
18 				 u8 mode)
19 {
20 	return (mode != (DCB_CAP_DCBX_HOST | DCB_CAP_DCBX_VER_IEEE)) ? 1 : 0;
21 }
22 
23 static int mlxsw_sp_dcbnl_ieee_getets(struct net_device *dev,
24 				      struct ieee_ets *ets)
25 {
26 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
27 
28 	memcpy(ets, mlxsw_sp_port->dcb.ets, sizeof(*ets));
29 
30 	return 0;
31 }
32 
33 static int mlxsw_sp_port_ets_validate(struct mlxsw_sp_port *mlxsw_sp_port,
34 				      struct ieee_ets *ets)
35 {
36 	struct net_device *dev = mlxsw_sp_port->dev;
37 	bool has_ets_tc = false;
38 	int i, tx_bw_sum = 0;
39 
40 	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
41 		switch (ets->tc_tsa[i]) {
42 		case IEEE_8021QAZ_TSA_STRICT:
43 			break;
44 		case IEEE_8021QAZ_TSA_ETS:
45 			has_ets_tc = true;
46 			tx_bw_sum += ets->tc_tx_bw[i];
47 			break;
48 		default:
49 			netdev_err(dev, "Only strict priority and ETS are supported\n");
50 			return -EINVAL;
51 		}
52 
53 		if (ets->prio_tc[i] >= IEEE_8021QAZ_MAX_TCS) {
54 			netdev_err(dev, "Invalid TC\n");
55 			return -EINVAL;
56 		}
57 	}
58 
59 	if (has_ets_tc && tx_bw_sum != 100) {
60 		netdev_err(dev, "Total ETS bandwidth should equal 100\n");
61 		return -EINVAL;
62 	}
63 
64 	return 0;
65 }
66 
67 static int mlxsw_sp_port_headroom_ets_set(struct mlxsw_sp_port *mlxsw_sp_port,
68 					  struct ieee_ets *ets)
69 {
70 	struct net_device *dev = mlxsw_sp_port->dev;
71 	struct mlxsw_sp_hdroom hdroom;
72 	int prio;
73 	int err;
74 
75 	hdroom = *mlxsw_sp_port->hdroom;
76 	for (prio = 0; prio < IEEE_8021QAZ_MAX_TCS; prio++)
77 		hdroom.prios.prio[prio].ets_buf_idx = ets->prio_tc[prio];
78 	mlxsw_sp_hdroom_prios_reset_buf_idx(&hdroom);
79 	mlxsw_sp_hdroom_bufs_reset_lossiness(&hdroom);
80 	mlxsw_sp_hdroom_bufs_reset_sizes(mlxsw_sp_port, &hdroom);
81 
82 	err = mlxsw_sp_hdroom_configure(mlxsw_sp_port, &hdroom);
83 	if (err) {
84 		netdev_err(dev, "Failed to configure port's headroom\n");
85 		return err;
86 	}
87 
88 	return 0;
89 }
90 
91 static int __mlxsw_sp_dcbnl_ieee_setets(struct mlxsw_sp_port *mlxsw_sp_port,
92 					struct ieee_ets *ets)
93 {
94 	struct ieee_ets *my_ets = mlxsw_sp_port->dcb.ets;
95 	struct net_device *dev = mlxsw_sp_port->dev;
96 	int i, err;
97 
98 	/* Egress configuration. */
99 	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
100 		bool dwrr = ets->tc_tsa[i] == IEEE_8021QAZ_TSA_ETS;
101 		u8 weight = ets->tc_tx_bw[i];
102 
103 		err = mlxsw_sp_port_ets_set(mlxsw_sp_port,
104 					    MLXSW_REG_QEEC_HR_SUBGROUP, i,
105 					    0, dwrr, weight);
106 		if (err) {
107 			netdev_err(dev, "Failed to link subgroup ETS element %d to group\n",
108 				   i);
109 			goto err_port_ets_set;
110 		}
111 	}
112 
113 	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
114 		err = mlxsw_sp_port_prio_tc_set(mlxsw_sp_port, i,
115 						ets->prio_tc[i]);
116 		if (err) {
117 			netdev_err(dev, "Failed to map prio %d to TC %d\n", i,
118 				   ets->prio_tc[i]);
119 			goto err_port_prio_tc_set;
120 		}
121 	}
122 
123 	/* Ingress configuration. */
124 	err = mlxsw_sp_port_headroom_ets_set(mlxsw_sp_port, ets);
125 	if (err)
126 		goto err_port_headroom_set;
127 
128 	return 0;
129 
130 err_port_headroom_set:
131 	i = IEEE_8021QAZ_MAX_TCS;
132 err_port_prio_tc_set:
133 	for (i--; i >= 0; i--)
134 		mlxsw_sp_port_prio_tc_set(mlxsw_sp_port, i, my_ets->prio_tc[i]);
135 	i = IEEE_8021QAZ_MAX_TCS;
136 err_port_ets_set:
137 	for (i--; i >= 0; i--) {
138 		bool dwrr = my_ets->tc_tsa[i] == IEEE_8021QAZ_TSA_ETS;
139 		u8 weight = my_ets->tc_tx_bw[i];
140 
141 		err = mlxsw_sp_port_ets_set(mlxsw_sp_port,
142 					    MLXSW_REG_QEEC_HR_SUBGROUP, i,
143 					    0, dwrr, weight);
144 	}
145 	return err;
146 }
147 
148 static int mlxsw_sp_dcbnl_ieee_setets(struct net_device *dev,
149 				      struct ieee_ets *ets)
150 {
151 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
152 	int err;
153 
154 	err = mlxsw_sp_port_ets_validate(mlxsw_sp_port, ets);
155 	if (err)
156 		return err;
157 
158 	err = __mlxsw_sp_dcbnl_ieee_setets(mlxsw_sp_port, ets);
159 	if (err)
160 		return err;
161 
162 	memcpy(mlxsw_sp_port->dcb.ets, ets, sizeof(*ets));
163 	mlxsw_sp_port->dcb.ets->ets_cap = IEEE_8021QAZ_MAX_TCS;
164 
165 	return 0;
166 }
167 
168 static int mlxsw_sp_dcbnl_app_validate(struct net_device *dev,
169 				       struct dcb_app *app)
170 {
171 	int prio;
172 
173 	if (app->priority >= IEEE_8021QAZ_MAX_TCS) {
174 		netdev_err(dev, "APP entry with priority value %u is invalid\n",
175 			   app->priority);
176 		return -EINVAL;
177 	}
178 
179 	switch (app->selector) {
180 	case IEEE_8021QAZ_APP_SEL_DSCP:
181 		if (app->protocol >= 64) {
182 			netdev_err(dev, "DSCP APP entry with protocol value %u is invalid\n",
183 				   app->protocol);
184 			return -EINVAL;
185 		}
186 
187 		/* Warn about any DSCP APP entries with the same PID. */
188 		prio = fls(dcb_ieee_getapp_mask(dev, app));
189 		if (prio--) {
190 			if (prio < app->priority)
191 				netdev_warn(dev, "Choosing priority %d for DSCP %d in favor of previously-active value of %d\n",
192 					    app->priority, app->protocol, prio);
193 			else if (prio > app->priority)
194 				netdev_warn(dev, "Ignoring new priority %d for DSCP %d in favor of current value of %d\n",
195 					    app->priority, app->protocol, prio);
196 		}
197 		break;
198 
199 	case IEEE_8021QAZ_APP_SEL_ETHERTYPE:
200 		if (app->protocol) {
201 			netdev_err(dev, "EtherType APP entries with protocol value != 0 not supported\n");
202 			return -EINVAL;
203 		}
204 		break;
205 
206 	default:
207 		netdev_err(dev, "APP entries with selector %u not supported\n",
208 			   app->selector);
209 		return -EINVAL;
210 	}
211 
212 	return 0;
213 }
214 
215 static u8
216 mlxsw_sp_port_dcb_app_default_prio(struct mlxsw_sp_port *mlxsw_sp_port)
217 {
218 	u8 prio_mask;
219 
220 	prio_mask = dcb_ieee_getapp_default_prio_mask(mlxsw_sp_port->dev);
221 	if (prio_mask)
222 		/* Take the highest configured priority. */
223 		return fls(prio_mask) - 1;
224 
225 	return 0;
226 }
227 
228 static void
229 mlxsw_sp_port_dcb_app_dscp_prio_map(struct mlxsw_sp_port *mlxsw_sp_port,
230 				    u8 default_prio,
231 				    struct dcb_ieee_app_dscp_map *map)
232 {
233 	int i;
234 
235 	dcb_ieee_getapp_dscp_prio_mask_map(mlxsw_sp_port->dev, map);
236 	for (i = 0; i < ARRAY_SIZE(map->map); ++i) {
237 		if (map->map[i])
238 			map->map[i] = fls(map->map[i]) - 1;
239 		else
240 			map->map[i] = default_prio;
241 	}
242 }
243 
244 static bool
245 mlxsw_sp_port_dcb_app_prio_dscp_map(struct mlxsw_sp_port *mlxsw_sp_port,
246 				    struct dcb_ieee_app_prio_map *map)
247 {
248 	bool have_dscp = false;
249 	int i;
250 
251 	dcb_ieee_getapp_prio_dscp_mask_map(mlxsw_sp_port->dev, map);
252 	for (i = 0; i < ARRAY_SIZE(map->map); ++i) {
253 		if (map->map[i]) {
254 			map->map[i] = fls64(map->map[i]) - 1;
255 			have_dscp = true;
256 		}
257 	}
258 
259 	return have_dscp;
260 }
261 
262 static int
263 mlxsw_sp_port_dcb_app_update_qpts(struct mlxsw_sp_port *mlxsw_sp_port,
264 				  enum mlxsw_reg_qpts_trust_state ts)
265 {
266 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
267 	char qpts_pl[MLXSW_REG_QPTS_LEN];
268 
269 	mlxsw_reg_qpts_pack(qpts_pl, mlxsw_sp_port->local_port, ts);
270 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpts), qpts_pl);
271 }
272 
273 static int
274 mlxsw_sp_port_dcb_app_update_qrwe(struct mlxsw_sp_port *mlxsw_sp_port,
275 				  bool rewrite_dscp)
276 {
277 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
278 	char qrwe_pl[MLXSW_REG_QRWE_LEN];
279 
280 	mlxsw_reg_qrwe_pack(qrwe_pl, mlxsw_sp_port->local_port,
281 			    false, rewrite_dscp);
282 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qrwe), qrwe_pl);
283 }
284 
285 static int
286 mlxsw_sp_port_dcb_toggle_trust(struct mlxsw_sp_port *mlxsw_sp_port,
287 			       enum mlxsw_reg_qpts_trust_state ts)
288 {
289 	bool rewrite_dscp = ts == MLXSW_REG_QPTS_TRUST_STATE_DSCP;
290 	int err;
291 
292 	if (mlxsw_sp_port->dcb.trust_state == ts)
293 		return 0;
294 
295 	err = mlxsw_sp_port_dcb_app_update_qpts(mlxsw_sp_port, ts);
296 	if (err)
297 		return err;
298 
299 	err = mlxsw_sp_port_dcb_app_update_qrwe(mlxsw_sp_port, rewrite_dscp);
300 	if (err)
301 		goto err_update_qrwe;
302 
303 	mlxsw_sp_port->dcb.trust_state = ts;
304 	return 0;
305 
306 err_update_qrwe:
307 	mlxsw_sp_port_dcb_app_update_qpts(mlxsw_sp_port,
308 					  mlxsw_sp_port->dcb.trust_state);
309 	return err;
310 }
311 
312 static int
313 mlxsw_sp_port_dcb_app_update_qpdp(struct mlxsw_sp_port *mlxsw_sp_port,
314 				  u8 default_prio)
315 {
316 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
317 	char qpdp_pl[MLXSW_REG_QPDP_LEN];
318 
319 	mlxsw_reg_qpdp_pack(qpdp_pl, mlxsw_sp_port->local_port, default_prio);
320 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpdp), qpdp_pl);
321 }
322 
323 static int
324 mlxsw_sp_port_dcb_app_update_qpdpm(struct mlxsw_sp_port *mlxsw_sp_port,
325 				   struct dcb_ieee_app_dscp_map *map)
326 {
327 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
328 	char qpdpm_pl[MLXSW_REG_QPDPM_LEN];
329 	short int i;
330 
331 	mlxsw_reg_qpdpm_pack(qpdpm_pl, mlxsw_sp_port->local_port);
332 	for (i = 0; i < ARRAY_SIZE(map->map); ++i)
333 		mlxsw_reg_qpdpm_dscp_pack(qpdpm_pl, i, map->map[i]);
334 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpdpm), qpdpm_pl);
335 }
336 
337 static int
338 mlxsw_sp_port_dcb_app_update_qpdsm(struct mlxsw_sp_port *mlxsw_sp_port,
339 				   struct dcb_ieee_app_prio_map *map)
340 {
341 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
342 	char qpdsm_pl[MLXSW_REG_QPDSM_LEN];
343 	short int i;
344 
345 	mlxsw_reg_qpdsm_pack(qpdsm_pl, mlxsw_sp_port->local_port);
346 	for (i = 0; i < ARRAY_SIZE(map->map); ++i)
347 		mlxsw_reg_qpdsm_prio_pack(qpdsm_pl, i, map->map[i]);
348 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpdsm), qpdsm_pl);
349 }
350 
351 static int mlxsw_sp_port_dcb_app_update(struct mlxsw_sp_port *mlxsw_sp_port)
352 {
353 	struct dcb_ieee_app_prio_map prio_map;
354 	struct dcb_ieee_app_dscp_map dscp_map;
355 	u8 default_prio;
356 	bool have_dscp;
357 	int err;
358 
359 	default_prio = mlxsw_sp_port_dcb_app_default_prio(mlxsw_sp_port);
360 	err = mlxsw_sp_port_dcb_app_update_qpdp(mlxsw_sp_port, default_prio);
361 	if (err) {
362 		netdev_err(mlxsw_sp_port->dev, "Couldn't configure port default priority\n");
363 		return err;
364 	}
365 
366 	have_dscp = mlxsw_sp_port_dcb_app_prio_dscp_map(mlxsw_sp_port,
367 							&prio_map);
368 
369 	mlxsw_sp_port_dcb_app_dscp_prio_map(mlxsw_sp_port, default_prio,
370 					    &dscp_map);
371 	err = mlxsw_sp_port_dcb_app_update_qpdpm(mlxsw_sp_port,
372 						 &dscp_map);
373 	if (err) {
374 		netdev_err(mlxsw_sp_port->dev, "Couldn't configure priority map\n");
375 		return err;
376 	}
377 
378 	err = mlxsw_sp_port_dcb_app_update_qpdsm(mlxsw_sp_port,
379 						 &prio_map);
380 	if (err) {
381 		netdev_err(mlxsw_sp_port->dev, "Couldn't configure DSCP rewrite map\n");
382 		return err;
383 	}
384 
385 	if (!have_dscp) {
386 		err = mlxsw_sp_port_dcb_toggle_trust(mlxsw_sp_port,
387 					MLXSW_REG_QPTS_TRUST_STATE_PCP);
388 		if (err)
389 			netdev_err(mlxsw_sp_port->dev, "Couldn't switch to trust L2\n");
390 		return err;
391 	}
392 
393 	err = mlxsw_sp_port_dcb_toggle_trust(mlxsw_sp_port,
394 					     MLXSW_REG_QPTS_TRUST_STATE_DSCP);
395 	if (err) {
396 		/* A failure to set trust DSCP means that the QPDPM and QPDSM
397 		 * maps installed above are not in effect. And since we are here
398 		 * attempting to set trust DSCP, we couldn't have attempted to
399 		 * switch trust to PCP. Thus no cleanup is necessary.
400 		 */
401 		netdev_err(mlxsw_sp_port->dev, "Couldn't switch to trust L3\n");
402 		return err;
403 	}
404 
405 	return 0;
406 }
407 
408 static int mlxsw_sp_dcbnl_ieee_setapp(struct net_device *dev,
409 				      struct dcb_app *app)
410 {
411 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
412 	int err;
413 
414 	err = mlxsw_sp_dcbnl_app_validate(dev, app);
415 	if (err)
416 		return err;
417 
418 	err = dcb_ieee_setapp(dev, app);
419 	if (err)
420 		return err;
421 
422 	err = mlxsw_sp_port_dcb_app_update(mlxsw_sp_port);
423 	if (err)
424 		goto err_update;
425 
426 	return 0;
427 
428 err_update:
429 	dcb_ieee_delapp(dev, app);
430 	return err;
431 }
432 
433 static int mlxsw_sp_dcbnl_ieee_delapp(struct net_device *dev,
434 				      struct dcb_app *app)
435 {
436 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
437 	int err;
438 
439 	err = dcb_ieee_delapp(dev, app);
440 	if (err)
441 		return err;
442 
443 	err = mlxsw_sp_port_dcb_app_update(mlxsw_sp_port);
444 	if (err)
445 		netdev_err(dev, "Failed to update DCB APP configuration\n");
446 	return 0;
447 }
448 
449 static int mlxsw_sp_dcbnl_ieee_getmaxrate(struct net_device *dev,
450 					  struct ieee_maxrate *maxrate)
451 {
452 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
453 
454 	memcpy(maxrate, mlxsw_sp_port->dcb.maxrate, sizeof(*maxrate));
455 
456 	return 0;
457 }
458 
459 static int mlxsw_sp_dcbnl_ieee_setmaxrate(struct net_device *dev,
460 					  struct ieee_maxrate *maxrate)
461 {
462 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
463 	struct ieee_maxrate *my_maxrate = mlxsw_sp_port->dcb.maxrate;
464 	int err, i;
465 
466 	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
467 		err = mlxsw_sp_port_ets_maxrate_set(mlxsw_sp_port,
468 						    MLXSW_REG_QEEC_HR_SUBGROUP,
469 						    i, 0,
470 						    maxrate->tc_maxrate[i], 0);
471 		if (err) {
472 			netdev_err(dev, "Failed to set maxrate for TC %d\n", i);
473 			goto err_port_ets_maxrate_set;
474 		}
475 	}
476 
477 	memcpy(mlxsw_sp_port->dcb.maxrate, maxrate, sizeof(*maxrate));
478 
479 	return 0;
480 
481 err_port_ets_maxrate_set:
482 	for (i--; i >= 0; i--)
483 		mlxsw_sp_port_ets_maxrate_set(mlxsw_sp_port,
484 					      MLXSW_REG_QEEC_HR_SUBGROUP,
485 					      i, 0,
486 					      my_maxrate->tc_maxrate[i], 0);
487 	return err;
488 }
489 
490 static int mlxsw_sp_port_pfc_cnt_get(struct mlxsw_sp_port *mlxsw_sp_port,
491 				     u8 prio)
492 {
493 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
494 	struct ieee_pfc *my_pfc = mlxsw_sp_port->dcb.pfc;
495 	char ppcnt_pl[MLXSW_REG_PPCNT_LEN];
496 	int err;
497 
498 	mlxsw_reg_ppcnt_pack(ppcnt_pl, mlxsw_sp_port->local_port,
499 			     MLXSW_REG_PPCNT_PRIO_CNT, prio);
500 	err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(ppcnt), ppcnt_pl);
501 	if (err)
502 		return err;
503 
504 	my_pfc->requests[prio] = mlxsw_reg_ppcnt_tx_pause_get(ppcnt_pl);
505 	my_pfc->indications[prio] = mlxsw_reg_ppcnt_rx_pause_get(ppcnt_pl);
506 
507 	return 0;
508 }
509 
510 static int mlxsw_sp_dcbnl_ieee_getpfc(struct net_device *dev,
511 				      struct ieee_pfc *pfc)
512 {
513 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
514 	int err, i;
515 
516 	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
517 		err = mlxsw_sp_port_pfc_cnt_get(mlxsw_sp_port, i);
518 		if (err) {
519 			netdev_err(dev, "Failed to get PFC count for priority %d\n",
520 				   i);
521 			return err;
522 		}
523 	}
524 
525 	memcpy(pfc, mlxsw_sp_port->dcb.pfc, sizeof(*pfc));
526 
527 	return 0;
528 }
529 
530 static int mlxsw_sp_port_pfc_set(struct mlxsw_sp_port *mlxsw_sp_port,
531 				 struct ieee_pfc *pfc)
532 {
533 	char pfcc_pl[MLXSW_REG_PFCC_LEN];
534 
535 	mlxsw_reg_pfcc_pack(pfcc_pl, mlxsw_sp_port->local_port);
536 	mlxsw_reg_pfcc_pprx_set(pfcc_pl, mlxsw_sp_port->link.rx_pause);
537 	mlxsw_reg_pfcc_pptx_set(pfcc_pl, mlxsw_sp_port->link.tx_pause);
538 	mlxsw_reg_pfcc_prio_pack(pfcc_pl, pfc->pfc_en);
539 
540 	return mlxsw_reg_write(mlxsw_sp_port->mlxsw_sp->core, MLXSW_REG(pfcc),
541 			       pfcc_pl);
542 }
543 
544 static int mlxsw_sp_dcbnl_ieee_setpfc(struct net_device *dev,
545 				      struct ieee_pfc *pfc)
546 {
547 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
548 	bool pause_en = mlxsw_sp_port_is_pause_en(mlxsw_sp_port);
549 	struct mlxsw_sp_hdroom orig_hdroom;
550 	struct mlxsw_sp_hdroom hdroom;
551 	int prio;
552 	int err;
553 
554 	if (pause_en && pfc->pfc_en) {
555 		netdev_err(dev, "PAUSE frames already enabled on port\n");
556 		return -EINVAL;
557 	}
558 
559 	orig_hdroom = *mlxsw_sp_port->hdroom;
560 
561 	hdroom = orig_hdroom;
562 	if (pfc->pfc_en)
563 		hdroom.delay_bytes = DIV_ROUND_UP(pfc->delay, BITS_PER_BYTE);
564 	else
565 		hdroom.delay_bytes = 0;
566 
567 	for (prio = 0; prio < IEEE_8021QAZ_MAX_TCS; prio++)
568 		hdroom.prios.prio[prio].lossy = !(pfc->pfc_en & BIT(prio));
569 
570 	mlxsw_sp_hdroom_bufs_reset_lossiness(&hdroom);
571 	mlxsw_sp_hdroom_bufs_reset_sizes(mlxsw_sp_port, &hdroom);
572 
573 	err = mlxsw_sp_hdroom_configure(mlxsw_sp_port, &hdroom);
574 	if (err) {
575 		netdev_err(dev, "Failed to configure port's headroom for PFC\n");
576 		return err;
577 	}
578 
579 	err = mlxsw_sp_port_pfc_set(mlxsw_sp_port, pfc);
580 	if (err) {
581 		netdev_err(dev, "Failed to configure PFC\n");
582 		goto err_port_pfc_set;
583 	}
584 
585 	memcpy(mlxsw_sp_port->dcb.pfc, pfc, sizeof(*pfc));
586 	mlxsw_sp_port->dcb.pfc->pfc_cap = IEEE_8021QAZ_MAX_TCS;
587 
588 	return 0;
589 
590 err_port_pfc_set:
591 	mlxsw_sp_hdroom_configure(mlxsw_sp_port, &orig_hdroom);
592 	return err;
593 }
594 
595 static int mlxsw_sp_dcbnl_getbuffer(struct net_device *dev, struct dcbnl_buffer *buf)
596 {
597 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
598 	struct mlxsw_sp_hdroom *hdroom = mlxsw_sp_port->hdroom;
599 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
600 	int prio;
601 	int i;
602 
603 	buf->total_size = 0;
604 
605 	BUILD_BUG_ON(DCBX_MAX_BUFFERS > MLXSW_SP_PB_COUNT);
606 	for (i = 0; i < MLXSW_SP_PB_COUNT; i++) {
607 		u32 bytes = mlxsw_sp_cells_bytes(mlxsw_sp, hdroom->bufs.buf[i].size_cells);
608 
609 		if (i < DCBX_MAX_BUFFERS)
610 			buf->buffer_size[i] = bytes;
611 		buf->total_size += bytes;
612 	}
613 
614 	buf->total_size += mlxsw_sp_cells_bytes(mlxsw_sp, hdroom->int_buf.size_cells);
615 
616 	for (prio = 0; prio < IEEE_8021Q_MAX_PRIORITIES; prio++)
617 		buf->prio2buffer[prio] = hdroom->prios.prio[prio].buf_idx;
618 
619 	return 0;
620 }
621 
622 static int mlxsw_sp_dcbnl_setbuffer(struct net_device *dev, struct dcbnl_buffer *buf)
623 {
624 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
625 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
626 	struct mlxsw_sp_hdroom hdroom;
627 	int prio;
628 	int i;
629 
630 	hdroom = *mlxsw_sp_port->hdroom;
631 
632 	if (hdroom.mode != MLXSW_SP_HDROOM_MODE_TC) {
633 		netdev_err(dev, "The use of dcbnl_setbuffer is only allowed if egress is configured using TC\n");
634 		return -EINVAL;
635 	}
636 
637 	for (prio = 0; prio < IEEE_8021Q_MAX_PRIORITIES; prio++)
638 		hdroom.prios.prio[prio].set_buf_idx = buf->prio2buffer[prio];
639 
640 	BUILD_BUG_ON(DCBX_MAX_BUFFERS > MLXSW_SP_PB_COUNT);
641 	for (i = 0; i < DCBX_MAX_BUFFERS; i++)
642 		hdroom.bufs.buf[i].set_size_cells = mlxsw_sp_bytes_cells(mlxsw_sp,
643 									 buf->buffer_size[i]);
644 
645 	mlxsw_sp_hdroom_prios_reset_buf_idx(&hdroom);
646 	mlxsw_sp_hdroom_bufs_reset_lossiness(&hdroom);
647 	mlxsw_sp_hdroom_bufs_reset_sizes(mlxsw_sp_port, &hdroom);
648 	return mlxsw_sp_hdroom_configure(mlxsw_sp_port, &hdroom);
649 }
650 
651 static const struct dcbnl_rtnl_ops mlxsw_sp_dcbnl_ops = {
652 	.ieee_getets		= mlxsw_sp_dcbnl_ieee_getets,
653 	.ieee_setets		= mlxsw_sp_dcbnl_ieee_setets,
654 	.ieee_getmaxrate	= mlxsw_sp_dcbnl_ieee_getmaxrate,
655 	.ieee_setmaxrate	= mlxsw_sp_dcbnl_ieee_setmaxrate,
656 	.ieee_getpfc		= mlxsw_sp_dcbnl_ieee_getpfc,
657 	.ieee_setpfc		= mlxsw_sp_dcbnl_ieee_setpfc,
658 	.ieee_setapp		= mlxsw_sp_dcbnl_ieee_setapp,
659 	.ieee_delapp		= mlxsw_sp_dcbnl_ieee_delapp,
660 
661 	.getdcbx		= mlxsw_sp_dcbnl_getdcbx,
662 	.setdcbx		= mlxsw_sp_dcbnl_setdcbx,
663 
664 	.dcbnl_getbuffer	= mlxsw_sp_dcbnl_getbuffer,
665 	.dcbnl_setbuffer	= mlxsw_sp_dcbnl_setbuffer,
666 };
667 
668 static int mlxsw_sp_port_ets_init(struct mlxsw_sp_port *mlxsw_sp_port)
669 {
670 	mlxsw_sp_port->dcb.ets = kzalloc(sizeof(*mlxsw_sp_port->dcb.ets),
671 					 GFP_KERNEL);
672 	if (!mlxsw_sp_port->dcb.ets)
673 		return -ENOMEM;
674 
675 	mlxsw_sp_port->dcb.ets->ets_cap = IEEE_8021QAZ_MAX_TCS;
676 
677 	return 0;
678 }
679 
680 static void mlxsw_sp_port_ets_fini(struct mlxsw_sp_port *mlxsw_sp_port)
681 {
682 	kfree(mlxsw_sp_port->dcb.ets);
683 }
684 
685 static int mlxsw_sp_port_maxrate_init(struct mlxsw_sp_port *mlxsw_sp_port)
686 {
687 	int i;
688 
689 	mlxsw_sp_port->dcb.maxrate = kmalloc(sizeof(*mlxsw_sp_port->dcb.maxrate),
690 					     GFP_KERNEL);
691 	if (!mlxsw_sp_port->dcb.maxrate)
692 		return -ENOMEM;
693 
694 	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++)
695 		mlxsw_sp_port->dcb.maxrate->tc_maxrate[i] = MLXSW_REG_QEEC_MAS_DIS;
696 
697 	return 0;
698 }
699 
700 static void mlxsw_sp_port_maxrate_fini(struct mlxsw_sp_port *mlxsw_sp_port)
701 {
702 	kfree(mlxsw_sp_port->dcb.maxrate);
703 }
704 
705 static int mlxsw_sp_port_pfc_init(struct mlxsw_sp_port *mlxsw_sp_port)
706 {
707 	mlxsw_sp_port->dcb.pfc = kzalloc(sizeof(*mlxsw_sp_port->dcb.pfc),
708 					 GFP_KERNEL);
709 	if (!mlxsw_sp_port->dcb.pfc)
710 		return -ENOMEM;
711 
712 	mlxsw_sp_port->dcb.pfc->pfc_cap = IEEE_8021QAZ_MAX_TCS;
713 
714 	return 0;
715 }
716 
717 static void mlxsw_sp_port_pfc_fini(struct mlxsw_sp_port *mlxsw_sp_port)
718 {
719 	kfree(mlxsw_sp_port->dcb.pfc);
720 }
721 
722 int mlxsw_sp_port_dcb_init(struct mlxsw_sp_port *mlxsw_sp_port)
723 {
724 	int err;
725 
726 	err = mlxsw_sp_port_ets_init(mlxsw_sp_port);
727 	if (err)
728 		return err;
729 	err = mlxsw_sp_port_maxrate_init(mlxsw_sp_port);
730 	if (err)
731 		goto err_port_maxrate_init;
732 	err = mlxsw_sp_port_pfc_init(mlxsw_sp_port);
733 	if (err)
734 		goto err_port_pfc_init;
735 
736 	mlxsw_sp_port->dcb.trust_state = MLXSW_REG_QPTS_TRUST_STATE_PCP;
737 	mlxsw_sp_port->dev->dcbnl_ops = &mlxsw_sp_dcbnl_ops;
738 
739 	return 0;
740 
741 err_port_pfc_init:
742 	mlxsw_sp_port_maxrate_fini(mlxsw_sp_port);
743 err_port_maxrate_init:
744 	mlxsw_sp_port_ets_fini(mlxsw_sp_port);
745 	return err;
746 }
747 
748 void mlxsw_sp_port_dcb_fini(struct mlxsw_sp_port *mlxsw_sp_port)
749 {
750 	mlxsw_sp_port_pfc_fini(mlxsw_sp_port);
751 	mlxsw_sp_port_maxrate_fini(mlxsw_sp_port);
752 	mlxsw_sp_port_ets_fini(mlxsw_sp_port);
753 }
754