1 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
2 /* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved */
3 
4 #include <net/devlink.h>
5 
6 #include "prestera_devlink.h"
7 #include "prestera_hw.h"
8 
9 /* All driver-specific traps must be documented in
10  * Documentation/networking/devlink/prestera.rst
11  */
12 enum {
13 	DEVLINK_PRESTERA_TRAP_ID_BASE = DEVLINK_TRAP_GENERIC_ID_MAX,
14 	DEVLINK_PRESTERA_TRAP_ID_ARP_BC,
15 	DEVLINK_PRESTERA_TRAP_ID_IS_IS,
16 	DEVLINK_PRESTERA_TRAP_ID_OSPF,
17 	DEVLINK_PRESTERA_TRAP_ID_IP_BC_MAC,
18 	DEVLINK_PRESTERA_TRAP_ID_ROUTER_MC,
19 	DEVLINK_PRESTERA_TRAP_ID_VRRP,
20 	DEVLINK_PRESTERA_TRAP_ID_DHCP,
21 	DEVLINK_PRESTERA_TRAP_ID_MAC_TO_ME,
22 	DEVLINK_PRESTERA_TRAP_ID_IPV4_OPTIONS,
23 	DEVLINK_PRESTERA_TRAP_ID_IP_DEFAULT_ROUTE,
24 	DEVLINK_PRESTERA_TRAP_ID_IP_TO_ME,
25 	DEVLINK_PRESTERA_TRAP_ID_IPV4_ICMP_REDIRECT,
26 	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_0,
27 	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_1,
28 	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_2,
29 	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_3,
30 	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_4,
31 	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_5,
32 	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_6,
33 	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_7,
34 	DEVLINK_PRESTERA_TRAP_ID_BGP,
35 	DEVLINK_PRESTERA_TRAP_ID_SSH,
36 	DEVLINK_PRESTERA_TRAP_ID_TELNET,
37 	DEVLINK_PRESTERA_TRAP_ID_ICMP,
38 	DEVLINK_PRESTERA_TRAP_ID_MET_RED,
39 	DEVLINK_PRESTERA_TRAP_ID_IP_SIP_IS_ZERO,
40 	DEVLINK_PRESTERA_TRAP_ID_IP_UC_DIP_DA_MISMATCH,
41 	DEVLINK_PRESTERA_TRAP_ID_ILLEGAL_IPV4_HDR,
42 	DEVLINK_PRESTERA_TRAP_ID_ILLEGAL_IP_ADDR,
43 	DEVLINK_PRESTERA_TRAP_ID_INVALID_SA,
44 	DEVLINK_PRESTERA_TRAP_ID_LOCAL_PORT,
45 	DEVLINK_PRESTERA_TRAP_ID_PORT_NO_VLAN,
46 	DEVLINK_PRESTERA_TRAP_ID_RXDMA_DROP,
47 };
48 
49 #define DEVLINK_PRESTERA_TRAP_NAME_ARP_BC \
50 	"arp_bc"
51 #define DEVLINK_PRESTERA_TRAP_NAME_IS_IS \
52 	"is_is"
53 #define DEVLINK_PRESTERA_TRAP_NAME_OSPF \
54 	"ospf"
55 #define DEVLINK_PRESTERA_TRAP_NAME_IP_BC_MAC \
56 	"ip_bc_mac"
57 #define DEVLINK_PRESTERA_TRAP_NAME_ROUTER_MC \
58 	"router_mc"
59 #define DEVLINK_PRESTERA_TRAP_NAME_VRRP \
60 	"vrrp"
61 #define DEVLINK_PRESTERA_TRAP_NAME_DHCP \
62 	"dhcp"
63 #define DEVLINK_PRESTERA_TRAP_NAME_MAC_TO_ME \
64 	"mac_to_me"
65 #define DEVLINK_PRESTERA_TRAP_NAME_IPV4_OPTIONS \
66 	"ipv4_options"
67 #define DEVLINK_PRESTERA_TRAP_NAME_IP_DEFAULT_ROUTE \
68 	"ip_default_route"
69 #define DEVLINK_PRESTERA_TRAP_NAME_IP_TO_ME \
70 	"ip_to_me"
71 #define DEVLINK_PRESTERA_TRAP_NAME_IPV4_ICMP_REDIRECT \
72 	"ipv4_icmp_redirect"
73 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_0 \
74 	"acl_code_0"
75 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_1 \
76 	"acl_code_1"
77 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_2 \
78 	"acl_code_2"
79 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_3 \
80 	"acl_code_3"
81 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_4 \
82 	"acl_code_4"
83 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_5 \
84 	"acl_code_5"
85 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_6 \
86 	"acl_code_6"
87 #define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_7 \
88 	"acl_code_7"
89 #define DEVLINK_PRESTERA_TRAP_NAME_BGP \
90 	"bgp"
91 #define DEVLINK_PRESTERA_TRAP_NAME_SSH \
92 	"ssh"
93 #define DEVLINK_PRESTERA_TRAP_NAME_TELNET \
94 	"telnet"
95 #define DEVLINK_PRESTERA_TRAP_NAME_ICMP \
96 	"icmp"
97 #define DEVLINK_PRESTERA_TRAP_NAME_RXDMA_DROP \
98 	"rxdma_drop"
99 #define DEVLINK_PRESTERA_TRAP_NAME_PORT_NO_VLAN \
100 	"port_no_vlan"
101 #define DEVLINK_PRESTERA_TRAP_NAME_LOCAL_PORT \
102 	"local_port"
103 #define DEVLINK_PRESTERA_TRAP_NAME_INVALID_SA \
104 	"invalid_sa"
105 #define DEVLINK_PRESTERA_TRAP_NAME_ILLEGAL_IP_ADDR \
106 	"illegal_ip_addr"
107 #define DEVLINK_PRESTERA_TRAP_NAME_ILLEGAL_IPV4_HDR \
108 	"illegal_ipv4_hdr"
109 #define DEVLINK_PRESTERA_TRAP_NAME_IP_UC_DIP_DA_MISMATCH \
110 	"ip_uc_dip_da_mismatch"
111 #define DEVLINK_PRESTERA_TRAP_NAME_IP_SIP_IS_ZERO \
112 	"ip_sip_is_zero"
113 #define DEVLINK_PRESTERA_TRAP_NAME_MET_RED \
114 	"met_red"
115 
116 struct prestera_trap {
117 	struct devlink_trap trap;
118 	u8 cpu_code;
119 };
120 
121 struct prestera_trap_item {
122 	enum devlink_trap_action action;
123 	void *trap_ctx;
124 };
125 
126 struct prestera_trap_data {
127 	struct prestera_switch *sw;
128 	struct prestera_trap_item *trap_items_arr;
129 	u32 traps_count;
130 };
131 
132 #define PRESTERA_TRAP_METADATA DEVLINK_TRAP_METADATA_TYPE_F_IN_PORT
133 
134 #define PRESTERA_TRAP_CONTROL(_id, _group_id, _action)			      \
135 	DEVLINK_TRAP_GENERIC(CONTROL, _action, _id,			      \
136 			     DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,	      \
137 			     PRESTERA_TRAP_METADATA)
138 
139 #define PRESTERA_TRAP_DRIVER_CONTROL(_id, _group_id)			      \
140 	DEVLINK_TRAP_DRIVER(CONTROL, TRAP, DEVLINK_PRESTERA_TRAP_ID_##_id,    \
141 			    DEVLINK_PRESTERA_TRAP_NAME_##_id,		      \
142 			    DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,	      \
143 			    PRESTERA_TRAP_METADATA)
144 
145 #define PRESTERA_TRAP_EXCEPTION(_id, _group_id)				      \
146 	DEVLINK_TRAP_GENERIC(EXCEPTION, TRAP, _id,			      \
147 			     DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,	      \
148 			     PRESTERA_TRAP_METADATA)
149 
150 #define PRESTERA_TRAP_DRIVER_EXCEPTION(_id, _group_id)			      \
151 	DEVLINK_TRAP_DRIVER(EXCEPTION, TRAP, DEVLINK_PRESTERA_TRAP_ID_##_id,  \
152 			    DEVLINK_PRESTERA_TRAP_NAME_##_id,		      \
153 			    DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,	      \
154 			    PRESTERA_TRAP_METADATA)
155 
156 #define PRESTERA_TRAP_DRIVER_DROP(_id, _group_id)			      \
157 	DEVLINK_TRAP_DRIVER(DROP, DROP, DEVLINK_PRESTERA_TRAP_ID_##_id,	      \
158 			    DEVLINK_PRESTERA_TRAP_NAME_##_id,		      \
159 			    DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,	      \
160 			    PRESTERA_TRAP_METADATA)
161 
162 static const struct devlink_trap_group prestera_trap_groups_arr[] = {
163 	/* No policer is associated with following groups (policerid == 0)*/
164 	DEVLINK_TRAP_GROUP_GENERIC(L2_DROPS, 0),
165 	DEVLINK_TRAP_GROUP_GENERIC(L3_DROPS, 0),
166 	DEVLINK_TRAP_GROUP_GENERIC(L3_EXCEPTIONS, 0),
167 	DEVLINK_TRAP_GROUP_GENERIC(NEIGH_DISCOVERY, 0),
168 	DEVLINK_TRAP_GROUP_GENERIC(ACL_TRAP, 0),
169 	DEVLINK_TRAP_GROUP_GENERIC(ACL_DROPS, 0),
170 	DEVLINK_TRAP_GROUP_GENERIC(ACL_SAMPLE, 0),
171 	DEVLINK_TRAP_GROUP_GENERIC(OSPF, 0),
172 	DEVLINK_TRAP_GROUP_GENERIC(STP, 0),
173 	DEVLINK_TRAP_GROUP_GENERIC(LACP, 0),
174 	DEVLINK_TRAP_GROUP_GENERIC(LLDP, 0),
175 	DEVLINK_TRAP_GROUP_GENERIC(VRRP, 0),
176 	DEVLINK_TRAP_GROUP_GENERIC(DHCP, 0),
177 	DEVLINK_TRAP_GROUP_GENERIC(BGP, 0),
178 	DEVLINK_TRAP_GROUP_GENERIC(LOCAL_DELIVERY, 0),
179 	DEVLINK_TRAP_GROUP_GENERIC(BUFFER_DROPS, 0),
180 };
181 
182 /* Initialize trap list, as well as associate CPU code with them. */
183 static struct prestera_trap prestera_trap_items_arr[] = {
184 	{
185 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ARP_BC, NEIGH_DISCOVERY),
186 		.cpu_code = 5,
187 	},
188 	{
189 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(IS_IS, LOCAL_DELIVERY),
190 		.cpu_code = 13,
191 	},
192 	{
193 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(OSPF, OSPF),
194 		.cpu_code = 16,
195 	},
196 	{
197 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(IP_BC_MAC, LOCAL_DELIVERY),
198 		.cpu_code = 19,
199 	},
200 	{
201 		.trap = PRESTERA_TRAP_CONTROL(STP, STP, TRAP),
202 		.cpu_code = 26,
203 	},
204 	{
205 		.trap = PRESTERA_TRAP_CONTROL(LACP, LACP, TRAP),
206 		.cpu_code = 27,
207 	},
208 	{
209 		.trap = PRESTERA_TRAP_CONTROL(LLDP, LLDP, TRAP),
210 		.cpu_code = 28,
211 	},
212 	{
213 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ROUTER_MC, LOCAL_DELIVERY),
214 		.cpu_code = 29,
215 	},
216 	{
217 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(VRRP, VRRP),
218 		.cpu_code = 30,
219 	},
220 	{
221 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(DHCP, DHCP),
222 		.cpu_code = 33,
223 	},
224 	{
225 		.trap = PRESTERA_TRAP_EXCEPTION(MTU_ERROR, L3_EXCEPTIONS),
226 		.cpu_code = 63,
227 	},
228 	{
229 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(MAC_TO_ME, LOCAL_DELIVERY),
230 		.cpu_code = 65,
231 	},
232 	{
233 		.trap = PRESTERA_TRAP_EXCEPTION(TTL_ERROR, L3_EXCEPTIONS),
234 		.cpu_code = 133,
235 	},
236 	{
237 		.trap = PRESTERA_TRAP_DRIVER_EXCEPTION(IPV4_OPTIONS,
238 						       L3_EXCEPTIONS),
239 		.cpu_code = 141,
240 	},
241 	{
242 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(IP_DEFAULT_ROUTE,
243 						     LOCAL_DELIVERY),
244 		.cpu_code = 160,
245 	},
246 	{
247 		.trap = PRESTERA_TRAP_CONTROL(LOCAL_ROUTE, LOCAL_DELIVERY,
248 					      TRAP),
249 		.cpu_code = 161,
250 	},
251 	{
252 		.trap = PRESTERA_TRAP_DRIVER_EXCEPTION(IPV4_ICMP_REDIRECT,
253 						       L3_EXCEPTIONS),
254 		.cpu_code = 180,
255 	},
256 	{
257 		.trap = PRESTERA_TRAP_CONTROL(ARP_RESPONSE, NEIGH_DISCOVERY,
258 					      TRAP),
259 		.cpu_code = 188,
260 	},
261 	{
262 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_0, ACL_TRAP),
263 		.cpu_code = 192,
264 	},
265 	{
266 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_1, ACL_TRAP),
267 		.cpu_code = 193,
268 	},
269 	{
270 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_2, ACL_TRAP),
271 		.cpu_code = 194,
272 	},
273 	{
274 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_3, ACL_TRAP),
275 		.cpu_code = 195,
276 	},
277 	{
278 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_4, ACL_TRAP),
279 		.cpu_code = 196,
280 	},
281 	{
282 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_5, ACL_TRAP),
283 		.cpu_code = 197,
284 	},
285 	{
286 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_6, ACL_TRAP),
287 		.cpu_code = 198,
288 	},
289 	{
290 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_7, ACL_TRAP),
291 		.cpu_code = 199,
292 	},
293 	{
294 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(BGP, BGP),
295 		.cpu_code = 206,
296 	},
297 	{
298 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(SSH, LOCAL_DELIVERY),
299 		.cpu_code = 207,
300 	},
301 	{
302 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(TELNET, LOCAL_DELIVERY),
303 		.cpu_code = 208,
304 	},
305 	{
306 		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ICMP, LOCAL_DELIVERY),
307 		.cpu_code = 209,
308 	},
309 	{
310 		.trap = PRESTERA_TRAP_DRIVER_DROP(RXDMA_DROP, BUFFER_DROPS),
311 		.cpu_code = 37,
312 	},
313 	{
314 		.trap = PRESTERA_TRAP_DRIVER_DROP(PORT_NO_VLAN, L2_DROPS),
315 		.cpu_code = 39,
316 	},
317 	{
318 		.trap = PRESTERA_TRAP_DRIVER_DROP(LOCAL_PORT, L2_DROPS),
319 		.cpu_code = 56,
320 	},
321 	{
322 		.trap = PRESTERA_TRAP_DRIVER_DROP(INVALID_SA, L2_DROPS),
323 		.cpu_code = 60,
324 	},
325 	{
326 		.trap = PRESTERA_TRAP_DRIVER_DROP(ILLEGAL_IP_ADDR, L3_DROPS),
327 		.cpu_code = 136,
328 	},
329 	{
330 		.trap = PRESTERA_TRAP_DRIVER_DROP(ILLEGAL_IPV4_HDR, L3_DROPS),
331 		.cpu_code = 137,
332 	},
333 	{
334 		.trap = PRESTERA_TRAP_DRIVER_DROP(IP_UC_DIP_DA_MISMATCH,
335 						  L3_DROPS),
336 		.cpu_code = 138,
337 	},
338 	{
339 		.trap = PRESTERA_TRAP_DRIVER_DROP(IP_SIP_IS_ZERO, L3_DROPS),
340 		.cpu_code = 145,
341 	},
342 	{
343 		.trap = PRESTERA_TRAP_DRIVER_DROP(MET_RED, BUFFER_DROPS),
344 		.cpu_code = 185,
345 	},
346 };
347 
348 static void prestera_devlink_traps_fini(struct prestera_switch *sw);
349 
350 static int prestera_drop_counter_get(struct devlink *devlink,
351 				     const struct devlink_trap *trap,
352 				     u64 *p_drops);
353 
354 static int prestera_dl_info_get(struct devlink *dl,
355 				struct devlink_info_req *req,
356 				struct netlink_ext_ack *extack)
357 {
358 	struct prestera_switch *sw = devlink_priv(dl);
359 	char buf[16];
360 	int err;
361 
362 	err = devlink_info_driver_name_put(req, PRESTERA_DRV_NAME);
363 	if (err)
364 		return err;
365 
366 	snprintf(buf, sizeof(buf), "%d.%d.%d",
367 		 sw->dev->fw_rev.maj,
368 		 sw->dev->fw_rev.min,
369 		 sw->dev->fw_rev.sub);
370 
371 	return devlink_info_version_running_put(req,
372 					       DEVLINK_INFO_VERSION_GENERIC_FW,
373 					       buf);
374 }
375 
376 static int prestera_trap_init(struct devlink *devlink,
377 			      const struct devlink_trap *trap, void *trap_ctx);
378 
379 static int prestera_trap_action_set(struct devlink *devlink,
380 				    const struct devlink_trap *trap,
381 				    enum devlink_trap_action action,
382 				    struct netlink_ext_ack *extack);
383 
384 static int prestera_devlink_traps_register(struct prestera_switch *sw);
385 
386 static const struct devlink_ops prestera_dl_ops = {
387 	.info_get = prestera_dl_info_get,
388 	.trap_init = prestera_trap_init,
389 	.trap_action_set = prestera_trap_action_set,
390 	.trap_drop_counter_get = prestera_drop_counter_get,
391 };
392 
393 struct prestera_switch *prestera_devlink_alloc(void)
394 {
395 	struct devlink *dl;
396 
397 	dl = devlink_alloc(&prestera_dl_ops, sizeof(struct prestera_switch));
398 
399 	return devlink_priv(dl);
400 }
401 
402 void prestera_devlink_free(struct prestera_switch *sw)
403 {
404 	struct devlink *dl = priv_to_devlink(sw);
405 
406 	devlink_free(dl);
407 }
408 
409 int prestera_devlink_register(struct prestera_switch *sw)
410 {
411 	struct devlink *dl = priv_to_devlink(sw);
412 	int err;
413 
414 	err = devlink_register(dl, sw->dev->dev);
415 	if (err) {
416 		dev_err(prestera_dev(sw), "devlink_register failed: %d\n", err);
417 		return err;
418 	}
419 
420 	err = prestera_devlink_traps_register(sw);
421 	if (err) {
422 		devlink_unregister(dl);
423 		dev_err(sw->dev->dev, "devlink_traps_register failed: %d\n",
424 			err);
425 		return err;
426 	}
427 
428 	return 0;
429 }
430 
431 void prestera_devlink_unregister(struct prestera_switch *sw)
432 {
433 	struct prestera_trap_data *trap_data = sw->trap_data;
434 	struct devlink *dl = priv_to_devlink(sw);
435 
436 	prestera_devlink_traps_fini(sw);
437 	devlink_unregister(dl);
438 
439 	kfree(trap_data->trap_items_arr);
440 	kfree(trap_data);
441 }
442 
443 int prestera_devlink_port_register(struct prestera_port *port)
444 {
445 	struct prestera_switch *sw = port->sw;
446 	struct devlink *dl = priv_to_devlink(sw);
447 	struct devlink_port_attrs attrs = {};
448 	int err;
449 
450 	attrs.flavour = DEVLINK_PORT_FLAVOUR_PHYSICAL;
451 	attrs.phys.port_number = port->fp_id;
452 	attrs.switch_id.id_len = sizeof(sw->id);
453 	memcpy(attrs.switch_id.id, &sw->id, attrs.switch_id.id_len);
454 
455 	devlink_port_attrs_set(&port->dl_port, &attrs);
456 
457 	err = devlink_port_register(dl, &port->dl_port, port->fp_id);
458 	if (err) {
459 		dev_err(prestera_dev(sw), "devlink_port_register failed: %d\n", err);
460 		return err;
461 	}
462 
463 	return 0;
464 }
465 
466 void prestera_devlink_port_unregister(struct prestera_port *port)
467 {
468 	devlink_port_unregister(&port->dl_port);
469 }
470 
471 void prestera_devlink_port_set(struct prestera_port *port)
472 {
473 	devlink_port_type_eth_set(&port->dl_port, port->dev);
474 }
475 
476 void prestera_devlink_port_clear(struct prestera_port *port)
477 {
478 	devlink_port_type_clear(&port->dl_port);
479 }
480 
481 struct devlink_port *prestera_devlink_get_port(struct net_device *dev)
482 {
483 	struct prestera_port *port = netdev_priv(dev);
484 
485 	return &port->dl_port;
486 }
487 
488 static int prestera_devlink_traps_register(struct prestera_switch *sw)
489 {
490 	const u32 groups_count = ARRAY_SIZE(prestera_trap_groups_arr);
491 	const u32 traps_count = ARRAY_SIZE(prestera_trap_items_arr);
492 	struct devlink *devlink = priv_to_devlink(sw);
493 	struct prestera_trap_data *trap_data;
494 	struct prestera_trap *prestera_trap;
495 	int err, i;
496 
497 	trap_data = kzalloc(sizeof(*trap_data), GFP_KERNEL);
498 	if (!trap_data)
499 		return -ENOMEM;
500 
501 	trap_data->trap_items_arr = kcalloc(traps_count,
502 					    sizeof(struct prestera_trap_item),
503 					    GFP_KERNEL);
504 	if (!trap_data->trap_items_arr) {
505 		err = -ENOMEM;
506 		goto err_trap_items_alloc;
507 	}
508 
509 	trap_data->sw = sw;
510 	trap_data->traps_count = traps_count;
511 	sw->trap_data = trap_data;
512 
513 	err = devlink_trap_groups_register(devlink, prestera_trap_groups_arr,
514 					   groups_count);
515 	if (err)
516 		goto err_groups_register;
517 
518 	for (i = 0; i < traps_count; i++) {
519 		prestera_trap = &prestera_trap_items_arr[i];
520 		err = devlink_traps_register(devlink, &prestera_trap->trap, 1,
521 					     sw);
522 		if (err)
523 			goto err_trap_register;
524 	}
525 
526 	return 0;
527 
528 err_trap_register:
529 	for (i--; i >= 0; i--) {
530 		prestera_trap = &prestera_trap_items_arr[i];
531 		devlink_traps_unregister(devlink, &prestera_trap->trap, 1);
532 	}
533 	devlink_trap_groups_unregister(devlink, prestera_trap_groups_arr,
534 				       groups_count);
535 err_groups_register:
536 	kfree(trap_data->trap_items_arr);
537 err_trap_items_alloc:
538 	kfree(trap_data);
539 	return err;
540 }
541 
542 static struct prestera_trap_item *
543 prestera_get_trap_item_by_cpu_code(struct prestera_switch *sw, u8 cpu_code)
544 {
545 	struct prestera_trap_data *trap_data = sw->trap_data;
546 	struct prestera_trap *prestera_trap;
547 	int i;
548 
549 	for (i = 0; i < trap_data->traps_count; i++) {
550 		prestera_trap = &prestera_trap_items_arr[i];
551 		if (cpu_code == prestera_trap->cpu_code)
552 			return &trap_data->trap_items_arr[i];
553 	}
554 
555 	return NULL;
556 }
557 
558 void prestera_devlink_trap_report(struct prestera_port *port,
559 				  struct sk_buff *skb, u8 cpu_code)
560 {
561 	struct prestera_trap_item *trap_item;
562 	struct devlink *devlink;
563 
564 	devlink = port->dl_port.devlink;
565 
566 	trap_item = prestera_get_trap_item_by_cpu_code(port->sw, cpu_code);
567 	if (unlikely(!trap_item))
568 		return;
569 
570 	devlink_trap_report(devlink, skb, trap_item->trap_ctx,
571 			    &port->dl_port, NULL);
572 }
573 
574 static struct prestera_trap_item *
575 prestera_devlink_trap_item_lookup(struct prestera_switch *sw, u16 trap_id)
576 {
577 	struct prestera_trap_data *trap_data = sw->trap_data;
578 	int i;
579 
580 	for (i = 0; i < ARRAY_SIZE(prestera_trap_items_arr); i++) {
581 		if (prestera_trap_items_arr[i].trap.id == trap_id)
582 			return &trap_data->trap_items_arr[i];
583 	}
584 
585 	return NULL;
586 }
587 
588 static int prestera_trap_init(struct devlink *devlink,
589 			      const struct devlink_trap *trap, void *trap_ctx)
590 {
591 	struct prestera_switch *sw = devlink_priv(devlink);
592 	struct prestera_trap_item *trap_item;
593 
594 	trap_item = prestera_devlink_trap_item_lookup(sw, trap->id);
595 	if (WARN_ON(!trap_item))
596 		return -EINVAL;
597 
598 	trap_item->trap_ctx = trap_ctx;
599 	trap_item->action = trap->init_action;
600 
601 	return 0;
602 }
603 
604 static int prestera_trap_action_set(struct devlink *devlink,
605 				    const struct devlink_trap *trap,
606 				    enum devlink_trap_action action,
607 				    struct netlink_ext_ack *extack)
608 {
609 	/* Currently, driver does not support trap action altering */
610 	return -EOPNOTSUPP;
611 }
612 
613 static int prestera_drop_counter_get(struct devlink *devlink,
614 				     const struct devlink_trap *trap,
615 				     u64 *p_drops)
616 {
617 	struct prestera_switch *sw = devlink_priv(devlink);
618 	enum prestera_hw_cpu_code_cnt_t cpu_code_type =
619 		PRESTERA_HW_CPU_CODE_CNT_TYPE_DROP;
620 	struct prestera_trap *prestera_trap =
621 		container_of(trap, struct prestera_trap, trap);
622 
623 	return prestera_hw_cpu_code_counters_get(sw, prestera_trap->cpu_code,
624 						 cpu_code_type, p_drops);
625 }
626 
627 static void prestera_devlink_traps_fini(struct prestera_switch *sw)
628 {
629 	struct devlink *dl = priv_to_devlink(sw);
630 	const struct devlink_trap *trap;
631 	int i;
632 
633 	for (i = 0; i < ARRAY_SIZE(prestera_trap_items_arr); ++i) {
634 		trap = &prestera_trap_items_arr[i].trap;
635 		devlink_traps_unregister(dl, trap, 1);
636 	}
637 
638 	devlink_trap_groups_unregister(dl, prestera_trap_groups_arr,
639 				       ARRAY_SIZE(prestera_trap_groups_arr));
640 }
641