net: marvell: prestera: devlink: add traps/groups implementation

Add devlink traps registration (with corresponding groups) for
all the traffic types that driver traps to the CPU;
prestera_rxtx: report each packet trapped to the CPU (RX) to the
prestera_devlink;

Signed-off-by: Oleksandr Mazur <oleksandr.mazur@plvision.eu>
Signed-off-by: David S. Miller <davem@davemloft.net>

6 files changed, 452 insertions, 3 deletions

diff --git a/drivers/net/ethernet/marvell/prestera/prestera.h b/drivers/net/ethernet/marvell/prestera/prestera.h
index ad0f33a7e517..6353f1c67638 100644
--- a/drivers/net/ethernet/marvell/prestera/prestera.h
+++ b/drivers/net/ethernet/marvell/prestera/prestera.h
@@ -170,6 +170,7 @@ struct prestera_event {
 
 struct prestera_switchdev;
 struct prestera_rxtx;
+struct prestera_trap_data;
 
 struct prestera_switch {
 	struct prestera_device *dev;
@@ -177,6 +178,7 @@ struct prestera_switch {
 	struct prestera_rxtx *rxtx;
 	struct list_head event_handlers;
 	struct notifier_block netdev_nb;
+	struct prestera_trap_data *trap_data;
 	char base_mac[ETH_ALEN];
 	struct list_head port_list;
 	rwlock_t port_list_lock;
diff --git a/drivers/net/ethernet/marvell/prestera/prestera_devlink.c b/drivers/net/ethernet/marvell/prestera/prestera_devlink.c
index 94c185a0e2b8..f59727f050ba 100644
--- a/drivers/net/ethernet/marvell/prestera/prestera_devlink.c
+++ b/drivers/net/ethernet/marvell/prestera/prestera_devlink.c
@@ -5,6 +5,276 @@
 
 #include "prestera_devlink.h"
 
+/* All driver-specific traps must be documented in
+ * Documentation/networking/devlink/prestera.rst
+ */
+enum {
+	DEVLINK_PRESTERA_TRAP_ID_BASE = DEVLINK_TRAP_GENERIC_ID_MAX,
+	DEVLINK_PRESTERA_TRAP_ID_ARP_BC,
+	DEVLINK_PRESTERA_TRAP_ID_IS_IS,
+	DEVLINK_PRESTERA_TRAP_ID_OSPF,
+	DEVLINK_PRESTERA_TRAP_ID_IP_BC_MAC,
+	DEVLINK_PRESTERA_TRAP_ID_ROUTER_MC,
+	DEVLINK_PRESTERA_TRAP_ID_VRRP,
+	DEVLINK_PRESTERA_TRAP_ID_DHCP,
+	DEVLINK_PRESTERA_TRAP_ID_MAC_TO_ME,
+	DEVLINK_PRESTERA_TRAP_ID_IPV4_OPTIONS,
+	DEVLINK_PRESTERA_TRAP_ID_IP_DEFAULT_ROUTE,
+	DEVLINK_PRESTERA_TRAP_ID_IP_TO_ME,
+	DEVLINK_PRESTERA_TRAP_ID_IPV4_ICMP_REDIRECT,
+	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_0,
+	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_1,
+	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_2,
+	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_3,
+	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_4,
+	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_5,
+	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_6,
+	DEVLINK_PRESTERA_TRAP_ID_ACL_CODE_7,
+	DEVLINK_PRESTERA_TRAP_ID_BGP,
+	DEVLINK_PRESTERA_TRAP_ID_SSH,
+	DEVLINK_PRESTERA_TRAP_ID_TELNET,
+	DEVLINK_PRESTERA_TRAP_ID_ICMP,
+};
+
+#define DEVLINK_PRESTERA_TRAP_NAME_ARP_BC \
+	"arp_bc"
+#define DEVLINK_PRESTERA_TRAP_NAME_IS_IS \
+	"is_is"
+#define DEVLINK_PRESTERA_TRAP_NAME_OSPF \
+	"ospf"
+#define DEVLINK_PRESTERA_TRAP_NAME_IP_BC_MAC \
+	"ip_bc_mac"
+#define DEVLINK_PRESTERA_TRAP_NAME_ROUTER_MC \
+	"router_mc"
+#define DEVLINK_PRESTERA_TRAP_NAME_VRRP \
+	"vrrp"
+#define DEVLINK_PRESTERA_TRAP_NAME_DHCP \
+	"dhcp"
+#define DEVLINK_PRESTERA_TRAP_NAME_MAC_TO_ME \
+	"mac_to_me"
+#define DEVLINK_PRESTERA_TRAP_NAME_IPV4_OPTIONS \
+	"ipv4_options"
+#define DEVLINK_PRESTERA_TRAP_NAME_IP_DEFAULT_ROUTE \
+	"ip_default_route"
+#define DEVLINK_PRESTERA_TRAP_NAME_IP_TO_ME \
+	"ip_to_me"
+#define DEVLINK_PRESTERA_TRAP_NAME_IPV4_ICMP_REDIRECT \
+	"ipv4_icmp_redirect"
+#define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_0 \
+	"acl_code_0"
+#define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_1 \
+	"acl_code_1"
+#define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_2 \
+	"acl_code_2"
+#define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_3 \
+	"acl_code_3"
+#define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_4 \
+	"acl_code_4"
+#define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_5 \
+	"acl_code_5"
+#define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_6 \
+	"acl_code_6"
+#define DEVLINK_PRESTERA_TRAP_NAME_ACL_CODE_7 \
+	"acl_code_7"
+#define DEVLINK_PRESTERA_TRAP_NAME_BGP \
+	"bgp"
+#define DEVLINK_PRESTERA_TRAP_NAME_SSH \
+	"ssh"
+#define DEVLINK_PRESTERA_TRAP_NAME_TELNET \
+	"telnet"
+#define DEVLINK_PRESTERA_TRAP_NAME_ICMP \
+	"icmp"
+
+struct prestera_trap {
+	struct devlink_trap trap;
+	u8 cpu_code;
+};
+
+struct prestera_trap_item {
+	enum devlink_trap_action action;
+	void *trap_ctx;
+};
+
+struct prestera_trap_data {
+	struct prestera_switch *sw;
+	struct prestera_trap_item *trap_items_arr;
+	u32 traps_count;
+};
+
+#define PRESTERA_TRAP_METADATA DEVLINK_TRAP_METADATA_TYPE_F_IN_PORT
+
+#define PRESTERA_TRAP_CONTROL(_id, _group_id, _action)			      \
+	DEVLINK_TRAP_GENERIC(CONTROL, _action, _id,			      \
+			     DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,	      \
+			     PRESTERA_TRAP_METADATA)
+
+#define PRESTERA_TRAP_DRIVER_CONTROL(_id, _group_id)			      \
+	DEVLINK_TRAP_DRIVER(CONTROL, TRAP, DEVLINK_PRESTERA_TRAP_ID_##_id,    \
+			    DEVLINK_PRESTERA_TRAP_NAME_##_id,		      \
+			    DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,	      \
+			    PRESTERA_TRAP_METADATA)
+
+#define PRESTERA_TRAP_EXCEPTION(_id, _group_id)				      \
+	DEVLINK_TRAP_GENERIC(EXCEPTION, TRAP, _id,			      \
+			     DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,	      \
+			     PRESTERA_TRAP_METADATA)
+
+#define PRESTERA_TRAP_DRIVER_EXCEPTION(_id, _group_id)			      \
+	DEVLINK_TRAP_DRIVER(EXCEPTION, TRAP, DEVLINK_PRESTERA_TRAP_ID_##_id,  \
+			    DEVLINK_PRESTERA_TRAP_NAME_##_id,		      \
+			    DEVLINK_TRAP_GROUP_GENERIC_ID_##_group_id,	      \
+			    PRESTERA_TRAP_METADATA)
+
+static const struct devlink_trap_group prestera_trap_groups_arr[] = {
+	/* No policer is associated with following groups (policerid == 0)*/
+	DEVLINK_TRAP_GROUP_GENERIC(L2_DROPS, 0),
+	DEVLINK_TRAP_GROUP_GENERIC(L3_DROPS, 0),
+	DEVLINK_TRAP_GROUP_GENERIC(L3_EXCEPTIONS, 0),
+	DEVLINK_TRAP_GROUP_GENERIC(NEIGH_DISCOVERY, 0),
+	DEVLINK_TRAP_GROUP_GENERIC(ACL_TRAP, 0),
+	DEVLINK_TRAP_GROUP_GENERIC(ACL_DROPS, 0),
+	DEVLINK_TRAP_GROUP_GENERIC(ACL_SAMPLE, 0),
+	DEVLINK_TRAP_GROUP_GENERIC(OSPF, 0),
+	DEVLINK_TRAP_GROUP_GENERIC(STP, 0),
+	DEVLINK_TRAP_GROUP_GENERIC(LACP, 0),
+	DEVLINK_TRAP_GROUP_GENERIC(LLDP, 0),
+	DEVLINK_TRAP_GROUP_GENERIC(VRRP, 0),
+	DEVLINK_TRAP_GROUP_GENERIC(DHCP, 0),
+	DEVLINK_TRAP_GROUP_GENERIC(BGP, 0),
+	DEVLINK_TRAP_GROUP_GENERIC(LOCAL_DELIVERY, 0),
+};
+
+/* Initialize trap list, as well as associate CPU code with them. */
+static struct prestera_trap prestera_trap_items_arr[] = {
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ARP_BC, NEIGH_DISCOVERY),
+		.cpu_code = 5,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(IS_IS, LOCAL_DELIVERY),
+		.cpu_code = 13,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(OSPF, OSPF),
+		.cpu_code = 16,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(IP_BC_MAC, LOCAL_DELIVERY),
+		.cpu_code = 19,
+	},
+	{
+		.trap = PRESTERA_TRAP_CONTROL(STP, STP, TRAP),
+		.cpu_code = 26,
+	},
+	{
+		.trap = PRESTERA_TRAP_CONTROL(LACP, LACP, TRAP),
+		.cpu_code = 27,
+	},
+	{
+		.trap = PRESTERA_TRAP_CONTROL(LLDP, LLDP, TRAP),
+		.cpu_code = 28,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ROUTER_MC, LOCAL_DELIVERY),
+		.cpu_code = 29,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(VRRP, VRRP),
+		.cpu_code = 30,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(DHCP, DHCP),
+		.cpu_code = 33,
+	},
+	{
+		.trap = PRESTERA_TRAP_EXCEPTION(MTU_ERROR, L3_EXCEPTIONS),
+		.cpu_code = 63,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(MAC_TO_ME, LOCAL_DELIVERY),
+		.cpu_code = 65,
+	},
+	{
+		.trap = PRESTERA_TRAP_EXCEPTION(TTL_ERROR, L3_EXCEPTIONS),
+		.cpu_code = 133,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_EXCEPTION(IPV4_OPTIONS,
+						       L3_EXCEPTIONS),
+		.cpu_code = 141,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(IP_DEFAULT_ROUTE,
+						     LOCAL_DELIVERY),
+		.cpu_code = 160,
+	},
+	{
+		.trap = PRESTERA_TRAP_CONTROL(LOCAL_ROUTE, LOCAL_DELIVERY,
+					      TRAP),
+		.cpu_code = 161,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_EXCEPTION(IPV4_ICMP_REDIRECT,
+						       L3_EXCEPTIONS),
+		.cpu_code = 180,
+	},
+	{
+		.trap = PRESTERA_TRAP_CONTROL(ARP_RESPONSE, NEIGH_DISCOVERY,
+					      TRAP),
+		.cpu_code = 188,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_0, ACL_TRAP),
+		.cpu_code = 192,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_1, ACL_TRAP),
+		.cpu_code = 193,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_2, ACL_TRAP),
+		.cpu_code = 194,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_3, ACL_TRAP),
+		.cpu_code = 195,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_4, ACL_TRAP),
+		.cpu_code = 196,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_5, ACL_TRAP),
+		.cpu_code = 197,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_6, ACL_TRAP),
+		.cpu_code = 198,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ACL_CODE_7, ACL_TRAP),
+		.cpu_code = 199,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(BGP, BGP),
+		.cpu_code = 206,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(SSH, LOCAL_DELIVERY),
+		.cpu_code = 207,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(TELNET, LOCAL_DELIVERY),
+		.cpu_code = 208,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_CONTROL(ICMP, LOCAL_DELIVERY),
+		.cpu_code = 209,
+	},
+};
+
+static void prestera_devlink_traps_fini(struct prestera_switch *sw);
+
 static int prestera_dl_info_get(struct devlink *dl,
 				struct devlink_info_req *req,
 				struct netlink_ext_ack *extack)
@@ -27,8 +297,20 @@ static int prestera_dl_info_get(struct devlink *dl,
 					       buf);
 }
 
+static int prestera_trap_init(struct devlink *devlink,
+			      const struct devlink_trap *trap, void *trap_ctx);
+
+static int prestera_trap_action_set(struct devlink *devlink,
+				    const struct devlink_trap *trap,
+				    enum devlink_trap_action action,
+				    struct netlink_ext_ack *extack);
+
+static int prestera_devlink_traps_register(struct prestera_switch *sw);
+
 static const struct devlink_ops prestera_dl_ops = {
 	.info_get = prestera_dl_info_get,
+	.trap_init = prestera_trap_init,
+	.trap_action_set = prestera_trap_action_set,
 };
 
 struct prestera_switch *prestera_devlink_alloc(void)
@@ -53,17 +335,32 @@ int prestera_devlink_register(struct prestera_switch *sw)
 	int err;
 
 	err = devlink_register(dl, sw->dev->dev);
-	if (err)
+	if (err) {
 		dev_err(prestera_dev(sw), "devlink_register failed: %d\n", err);
+		return err;
+	}
 
-	return err;
+	err = prestera_devlink_traps_register(sw);
+	if (err) {
+		devlink_unregister(dl);
+		dev_err(sw->dev->dev, "devlink_traps_register failed: %d\n",
+			err);
+		return err;
+	}
+
+	return 0;
 }
 
 void prestera_devlink_unregister(struct prestera_switch *sw)
 {
+	struct prestera_trap_data *trap_data = sw->trap_data;
 	struct devlink *dl = priv_to_devlink(sw);
 
+	prestera_devlink_traps_fini(sw);
 	devlink_unregister(dl);
+
+	kfree(trap_data->trap_items_arr);
+	kfree(trap_data);
 }
 
 int prestera_devlink_port_register(struct prestera_port *port)
@@ -110,3 +407,141 @@ struct devlink_port *prestera_devlink_get_port(struct net_device *dev)
 
 	return &port->dl_port;
 }
+
+static int prestera_devlink_traps_register(struct prestera_switch *sw)
+{
+	const u32 groups_count = ARRAY_SIZE(prestera_trap_groups_arr);
+	const u32 traps_count = ARRAY_SIZE(prestera_trap_items_arr);
+	struct devlink *devlink = priv_to_devlink(sw);
+	struct prestera_trap_data *trap_data;
+	struct prestera_trap *prestera_trap;
+	int err, i;
+
+	trap_data = kzalloc(sizeof(*trap_data), GFP_KERNEL);
+	if (!trap_data)
+		return -ENOMEM;
+
+	trap_data->trap_items_arr = kcalloc(traps_count,
+					    sizeof(struct prestera_trap_item),
+					    GFP_KERNEL);
+	if (!trap_data->trap_items_arr) {
+		err = -ENOMEM;
+		goto err_trap_items_alloc;
+	}
+
+	trap_data->sw = sw;
+	trap_data->traps_count = traps_count;
+	sw->trap_data = trap_data;
+
+	err = devlink_trap_groups_register(devlink, prestera_trap_groups_arr,
+					   groups_count);
+	if (err)
+		goto err_groups_register;
+
+	for (i = 0; i < traps_count; i++) {
+		prestera_trap = &prestera_trap_items_arr[i];
+		err = devlink_traps_register(devlink, &prestera_trap->trap, 1,
+					     sw);
+		if (err)
+			goto err_trap_register;
+	}
+
+	return 0;
+
+err_trap_register:
+	for (i--; i >= 0; i--) {
+		prestera_trap = &prestera_trap_items_arr[i];
+		devlink_traps_unregister(devlink, &prestera_trap->trap, 1);
+	}
+err_groups_register:
+	kfree(trap_data->trap_items_arr);
+err_trap_items_alloc:
+	kfree(trap_data);
+	return err;
+}
+
+static struct prestera_trap_item *
+prestera_get_trap_item_by_cpu_code(struct prestera_switch *sw, u8 cpu_code)
+{
+	struct prestera_trap_data *trap_data = sw->trap_data;
+	struct prestera_trap *prestera_trap;
+	int i;
+
+	for (i = 0; i < trap_data->traps_count; i++) {
+		prestera_trap = &prestera_trap_items_arr[i];
+		if (cpu_code == prestera_trap->cpu_code)
+			return &trap_data->trap_items_arr[i];
+	}
+
+	return NULL;
+}
+
+void prestera_devlink_trap_report(struct prestera_port *port,
+				  struct sk_buff *skb, u8 cpu_code)
+{
+	struct prestera_trap_item *trap_item;
+	struct devlink *devlink;
+
+	devlink = port->dl_port.devlink;
+
+	trap_item = prestera_get_trap_item_by_cpu_code(port->sw, cpu_code);
+	if (unlikely(!trap_item))
+		return;
+
+	devlink_trap_report(devlink, skb, trap_item->trap_ctx,
+			    &port->dl_port, NULL);
+}
+
+static struct prestera_trap_item *
+prestera_devlink_trap_item_lookup(struct prestera_switch *sw, u16 trap_id)
+{
+	struct prestera_trap_data *trap_data = sw->trap_data;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(prestera_trap_items_arr); i++) {
+		if (prestera_trap_items_arr[i].trap.id == trap_id)
+			return &trap_data->trap_items_arr[i];
+	}
+
+	return NULL;
+}
+
+static int prestera_trap_init(struct devlink *devlink,
+			      const struct devlink_trap *trap, void *trap_ctx)
+{
+	struct prestera_switch *sw = devlink_priv(devlink);
+	struct prestera_trap_item *trap_item;
+
+	trap_item = prestera_devlink_trap_item_lookup(sw, trap->id);
+	if (WARN_ON(!trap_item))
+		return -EINVAL;
+
+	trap_item->trap_ctx = trap_ctx;
+	trap_item->action = trap->init_action;
+
+	return 0;
+}
+
+static int prestera_trap_action_set(struct devlink *devlink,
+				    const struct devlink_trap *trap,
+				    enum devlink_trap_action action,
+				    struct netlink_ext_ack *extack)
+{
+	/* Currently, driver does not support trap action altering */
+	return -EOPNOTSUPP;
+}
+
+static void prestera_devlink_traps_fini(struct prestera_switch *sw)
+{
+	struct devlink *dl = priv_to_devlink(sw);
+	const struct devlink_trap *trap;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(prestera_trap_items_arr); ++i) {
+		trap = &prestera_trap_items_arr[i].trap;
+		devlink_traps_unregister(dl, trap, 1);
+	}
+
+	devlink_trap_groups_unregister(dl, prestera_trap_groups_arr,
+				       ARRAY_SIZE(prestera_trap_groups_arr));
+}
diff --git a/drivers/net/ethernet/marvell/prestera/prestera_devlink.h b/drivers/net/ethernet/marvell/prestera/prestera_devlink.h
index 51bee9f75415..5d73aa9db897 100644
--- a/drivers/net/ethernet/marvell/prestera/prestera_devlink.h
+++ b/drivers/net/ethernet/marvell/prestera/prestera_devlink.h
@@ -20,4 +20,7 @@ void prestera_devlink_port_clear(struct prestera_port *port);
 
 struct devlink_port *prestera_devlink_get_port(struct net_device *dev);
 
+void prestera_devlink_trap_report(struct prestera_port *port,
+				  struct sk_buff *skb, u8 cpu_code);
+
 #endif /* _PRESTERA_DEVLINK_H_ */
diff --git a/drivers/net/ethernet/marvell/prestera/prestera_dsa.c b/drivers/net/ethernet/marvell/prestera/prestera_dsa.c
index a5e01c7a307b..b7e89c0ca5c0 100644
--- a/drivers/net/ethernet/marvell/prestera/prestera_dsa.c
+++ b/drivers/net/ethernet/marvell/prestera/prestera_dsa.c
@@ -19,6 +19,7 @@
 #define PRESTERA_DSA_W1_EXT_BIT		BIT(31)
 #define PRESTERA_DSA_W1_CFI_BIT		BIT(30)
 #define PRESTERA_DSA_W1_PORT_NUM	GENMASK(11, 10)
+#define PRESTERA_DSA_W1_MASK_CPU_CODE	GENMASK(7, 0)
 
 #define PRESTERA_DSA_W2_EXT_BIT		BIT(31)
 #define PRESTERA_DSA_W2_PORT_NUM	BIT(20)
@@ -74,6 +75,8 @@ int prestera_dsa_parse(struct prestera_dsa *dsa, const u8 *dsa_buf)
 			(FIELD_GET(PRESTERA_DSA_W1_PORT_NUM, words[1]) << 5) |
 			(FIELD_GET(PRESTERA_DSA_W2_PORT_NUM, words[2]) << 7);
 
+	dsa->cpu_code = FIELD_GET(PRESTERA_DSA_W1_MASK_CPU_CODE, words[1]);
+
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/marvell/prestera/prestera_dsa.h b/drivers/net/ethernet/marvell/prestera/prestera_dsa.h
index 67018629bdd2..c99342f475cf 100644
--- a/drivers/net/ethernet/marvell/prestera/prestera_dsa.h
+++ b/drivers/net/ethernet/marvell/prestera/prestera_dsa.h
@@ -27,6 +27,7 @@ struct prestera_dsa {
 	struct prestera_dsa_vlan vlan;
 	u32 hw_dev_num;
 	u32 port_num;
+	u8 cpu_code;
 };
 
 int prestera_dsa_parse(struct prestera_dsa *dsa, const u8 *dsa_buf);
diff --git a/drivers/net/ethernet/marvell/prestera/prestera_rxtx.c b/drivers/net/ethernet/marvell/prestera/prestera_rxtx.c
index 2a13c318048c..73d2eba5262f 100644
--- a/drivers/net/ethernet/marvell/prestera/prestera_rxtx.c
+++ b/drivers/net/ethernet/marvell/prestera/prestera_rxtx.c
@@ -14,6 +14,7 @@
 #include "prestera.h"
 #include "prestera_hw.h"
 #include "prestera_rxtx.h"
+#include "prestera_devlink.h"
 
 #define PRESTERA_SDMA_WAIT_MUL		10
 
@@ -214,9 +215,10 @@ static struct sk_buff *prestera_sdma_rx_skb_get(struct prestera_sdma *sdma,
 static int prestera_rxtx_process_skb(struct prestera_sdma *sdma,
 				     struct sk_buff *skb)
 {
-	const struct prestera_port *port;
+	struct prestera_port *port;
 	struct prestera_dsa dsa;
 	u32 hw_port, dev_id;
+	u8 cpu_code;
 	int err;
 
 	skb_pull(skb, ETH_HLEN);
@@ -259,6 +261,9 @@ static int prestera_rxtx_process_skb(struct prestera_sdma *sdma,
 		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), tci);
 	}
 
+	cpu_code = dsa.cpu_code;
+	prestera_devlink_trap_report(port, skb, cpu_code);
+
 	return 0;
 }