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 int prestera_drop_counter_get(struct devlink *devlink,
349 				     const struct devlink_trap *trap,
350 				     u64 *p_drops);
351 
352 static int prestera_dl_info_get(struct devlink *dl,
353 				struct devlink_info_req *req,
354 				struct netlink_ext_ack *extack)
355 {
356 	struct prestera_switch *sw = devlink_priv(dl);
357 	char buf[16];
358 
359 	snprintf(buf, sizeof(buf), "%d.%d.%d",
360 		 sw->dev->fw_rev.maj,
361 		 sw->dev->fw_rev.min,
362 		 sw->dev->fw_rev.sub);
363 
364 	return devlink_info_version_running_put(req,
365 					       DEVLINK_INFO_VERSION_GENERIC_FW,
366 					       buf);
367 }
368 
369 static int prestera_trap_init(struct devlink *devlink,
370 			      const struct devlink_trap *trap, void *trap_ctx);
371 
372 static int prestera_trap_action_set(struct devlink *devlink,
373 				    const struct devlink_trap *trap,
374 				    enum devlink_trap_action action,
375 				    struct netlink_ext_ack *extack);
376 
377 static const struct devlink_ops prestera_dl_ops = {
378 	.info_get = prestera_dl_info_get,
379 	.trap_init = prestera_trap_init,
380 	.trap_action_set = prestera_trap_action_set,
381 	.trap_drop_counter_get = prestera_drop_counter_get,
382 };
383 
384 struct prestera_switch *prestera_devlink_alloc(struct prestera_device *dev)
385 {
386 	struct devlink *dl;
387 
388 	dl = devlink_alloc(&prestera_dl_ops, sizeof(struct prestera_switch),
389 			   dev->dev);
390 
391 	return devlink_priv(dl);
392 }
393 
394 void prestera_devlink_free(struct prestera_switch *sw)
395 {
396 	struct devlink *dl = priv_to_devlink(sw);
397 
398 	devlink_free(dl);
399 }
400 
401 void prestera_devlink_register(struct prestera_switch *sw)
402 {
403 	struct devlink *dl = priv_to_devlink(sw);
404 
405 	devlink_register(dl);
406 }
407 
408 void prestera_devlink_unregister(struct prestera_switch *sw)
409 {
410 	struct devlink *dl = priv_to_devlink(sw);
411 
412 	devlink_unregister(dl);
413 }
414 
415 int prestera_devlink_port_register(struct prestera_port *port)
416 {
417 	struct prestera_switch *sw = port->sw;
418 	struct devlink *dl = priv_to_devlink(sw);
419 	struct devlink_port_attrs attrs = {};
420 	int err;
421 
422 	attrs.flavour = DEVLINK_PORT_FLAVOUR_PHYSICAL;
423 	attrs.phys.port_number = port->fp_id;
424 	attrs.switch_id.id_len = sizeof(sw->id);
425 	memcpy(attrs.switch_id.id, &sw->id, attrs.switch_id.id_len);
426 
427 	devlink_port_attrs_set(&port->dl_port, &attrs);
428 
429 	err = devlink_port_register(dl, &port->dl_port, port->fp_id);
430 	if (err) {
431 		dev_err(prestera_dev(sw), "devlink_port_register failed: %d\n", err);
432 		return err;
433 	}
434 
435 	return 0;
436 }
437 
438 void prestera_devlink_port_unregister(struct prestera_port *port)
439 {
440 	devlink_port_unregister(&port->dl_port);
441 }
442 
443 int prestera_devlink_traps_register(struct prestera_switch *sw)
444 {
445 	const u32 groups_count = ARRAY_SIZE(prestera_trap_groups_arr);
446 	const u32 traps_count = ARRAY_SIZE(prestera_trap_items_arr);
447 	struct devlink *devlink = priv_to_devlink(sw);
448 	struct prestera_trap_data *trap_data;
449 	struct prestera_trap *prestera_trap;
450 	int err, i;
451 
452 	trap_data = kzalloc(sizeof(*trap_data), GFP_KERNEL);
453 	if (!trap_data)
454 		return -ENOMEM;
455 
456 	trap_data->trap_items_arr = kcalloc(traps_count,
457 					    sizeof(struct prestera_trap_item),
458 					    GFP_KERNEL);
459 	if (!trap_data->trap_items_arr) {
460 		err = -ENOMEM;
461 		goto err_trap_items_alloc;
462 	}
463 
464 	trap_data->sw = sw;
465 	trap_data->traps_count = traps_count;
466 	sw->trap_data = trap_data;
467 
468 	err = devlink_trap_groups_register(devlink, prestera_trap_groups_arr,
469 					   groups_count);
470 	if (err)
471 		goto err_groups_register;
472 
473 	for (i = 0; i < traps_count; i++) {
474 		prestera_trap = &prestera_trap_items_arr[i];
475 		err = devlink_traps_register(devlink, &prestera_trap->trap, 1,
476 					     sw);
477 		if (err)
478 			goto err_trap_register;
479 	}
480 
481 	return 0;
482 
483 err_trap_register:
484 	for (i--; i >= 0; i--) {
485 		prestera_trap = &prestera_trap_items_arr[i];
486 		devlink_traps_unregister(devlink, &prestera_trap->trap, 1);
487 	}
488 	devlink_trap_groups_unregister(devlink, prestera_trap_groups_arr,
489 				       groups_count);
490 err_groups_register:
491 	kfree(trap_data->trap_items_arr);
492 err_trap_items_alloc:
493 	kfree(trap_data);
494 	return err;
495 }
496 
497 static struct prestera_trap_item *
498 prestera_get_trap_item_by_cpu_code(struct prestera_switch *sw, u8 cpu_code)
499 {
500 	struct prestera_trap_data *trap_data = sw->trap_data;
501 	struct prestera_trap *prestera_trap;
502 	int i;
503 
504 	for (i = 0; i < trap_data->traps_count; i++) {
505 		prestera_trap = &prestera_trap_items_arr[i];
506 		if (cpu_code == prestera_trap->cpu_code)
507 			return &trap_data->trap_items_arr[i];
508 	}
509 
510 	return NULL;
511 }
512 
513 void prestera_devlink_trap_report(struct prestera_port *port,
514 				  struct sk_buff *skb, u8 cpu_code)
515 {
516 	struct prestera_trap_item *trap_item;
517 	struct devlink *devlink;
518 
519 	devlink = port->dl_port.devlink;
520 
521 	trap_item = prestera_get_trap_item_by_cpu_code(port->sw, cpu_code);
522 	if (unlikely(!trap_item))
523 		return;
524 
525 	devlink_trap_report(devlink, skb, trap_item->trap_ctx,
526 			    &port->dl_port, NULL);
527 }
528 
529 static struct prestera_trap_item *
530 prestera_devlink_trap_item_lookup(struct prestera_switch *sw, u16 trap_id)
531 {
532 	struct prestera_trap_data *trap_data = sw->trap_data;
533 	int i;
534 
535 	for (i = 0; i < ARRAY_SIZE(prestera_trap_items_arr); i++) {
536 		if (prestera_trap_items_arr[i].trap.id == trap_id)
537 			return &trap_data->trap_items_arr[i];
538 	}
539 
540 	return NULL;
541 }
542 
543 static int prestera_trap_init(struct devlink *devlink,
544 			      const struct devlink_trap *trap, void *trap_ctx)
545 {
546 	struct prestera_switch *sw = devlink_priv(devlink);
547 	struct prestera_trap_item *trap_item;
548 
549 	trap_item = prestera_devlink_trap_item_lookup(sw, trap->id);
550 	if (WARN_ON(!trap_item))
551 		return -EINVAL;
552 
553 	trap_item->trap_ctx = trap_ctx;
554 	trap_item->action = trap->init_action;
555 
556 	return 0;
557 }
558 
559 static int prestera_trap_action_set(struct devlink *devlink,
560 				    const struct devlink_trap *trap,
561 				    enum devlink_trap_action action,
562 				    struct netlink_ext_ack *extack)
563 {
564 	/* Currently, driver does not support trap action altering */
565 	return -EOPNOTSUPP;
566 }
567 
568 static int prestera_drop_counter_get(struct devlink *devlink,
569 				     const struct devlink_trap *trap,
570 				     u64 *p_drops)
571 {
572 	struct prestera_switch *sw = devlink_priv(devlink);
573 	enum prestera_hw_cpu_code_cnt_t cpu_code_type =
574 		PRESTERA_HW_CPU_CODE_CNT_TYPE_DROP;
575 	struct prestera_trap *prestera_trap =
576 		container_of(trap, struct prestera_trap, trap);
577 
578 	return prestera_hw_cpu_code_counters_get(sw, prestera_trap->cpu_code,
579 						 cpu_code_type, p_drops);
580 }
581 
582 void prestera_devlink_traps_unregister(struct prestera_switch *sw)
583 {
584 	struct prestera_trap_data *trap_data = sw->trap_data;
585 	struct devlink *dl = priv_to_devlink(sw);
586 	const struct devlink_trap *trap;
587 	int i;
588 
589 	for (i = 0; i < ARRAY_SIZE(prestera_trap_items_arr); ++i) {
590 		trap = &prestera_trap_items_arr[i].trap;
591 		devlink_traps_unregister(dl, trap, 1);
592 	}
593 
594 	devlink_trap_groups_unregister(dl, prestera_trap_groups_arr,
595 				       ARRAY_SIZE(prestera_trap_groups_arr));
596 	kfree(trap_data->trap_items_arr);
597 	kfree(trap_data);
598 }
599