Resguardo inicial

linux-7.2.3/drivers/net/ethernet/marvell/prestera/Kconfig

@@ -0,0 +1,27 @@
+# SPDX-License-Identifier: GPL-2.0-only
+#
+# Marvell Prestera drivers configuration
+#
+
+config PRESTERA
+	tristate "Marvell Prestera Switch ASICs support"
+	depends on NET_SWITCHDEV && VLAN_8021Q
+	depends on BRIDGE || BRIDGE=n
+	select NET_DEVLINK
+	select PHYLINK
+	help
+	  This driver supports Marvell Prestera Switch ASICs family.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called prestera.
+
+config PRESTERA_PCI
+	tristate "PCI interface driver for Marvell Prestera Switch ASICs family"
+	depends on PCI && HAS_IOMEM && PRESTERA
+	default PRESTERA
+	help
+	  This is implementation of PCI interface support for Marvell Prestera
+	  Switch ASICs family.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called prestera_pci.

linux-7.2.3/drivers/net/ethernet/marvell/prestera/Makefile

@@ -0,0 +1,9 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-$(CONFIG_PRESTERA)	+= prestera.o
+prestera-objs		:= prestera_main.o prestera_hw.o prestera_dsa.o \
+			   prestera_rxtx.o prestera_devlink.o prestera_ethtool.o \
+			   prestera_switchdev.o prestera_acl.o prestera_flow.o \
+			   prestera_flower.o prestera_span.o prestera_counter.o \
+			   prestera_router.o prestera_router_hw.o prestera_matchall.o
+
+obj-$(CONFIG_PRESTERA_PCI)	+= prestera_pci.o

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera.h

@@ -0,0 +1,417 @@
+/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */
+/* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved. */
+
+#ifndef _PRESTERA_H_
+#define _PRESTERA_H_
+
+#include <linux/notifier.h>
+#include <linux/skbuff.h>
+#include <linux/workqueue.h>
+#include <linux/phylink.h>
+#include <net/devlink.h>
+#include <uapi/linux/if_ether.h>
+
+#define PRESTERA_DRV_NAME	"prestera"
+
+#define PRESTERA_DEFAULT_VID    1
+
+struct prestera_fw_rev {
+	u16 maj;
+	u16 min;
+	u16 sub;
+};
+
+struct prestera_flood_domain {
+	struct prestera_switch *sw;
+	struct list_head flood_domain_port_list;
+	u32 idx;
+};
+
+struct prestera_mdb_entry {
+	struct prestera_switch *sw;
+	struct prestera_flood_domain *flood_domain;
+	unsigned char addr[ETH_ALEN];
+	u16 vid;
+};
+
+struct prestera_flood_domain_port {
+	struct prestera_flood_domain *flood_domain;
+	struct net_device *dev;
+	struct list_head flood_domain_port_node;
+	u16 vid;
+};
+
+struct prestera_port_stats {
+	u64 good_octets_received;
+	u64 bad_octets_received;
+	u64 mac_trans_error;
+	u64 broadcast_frames_received;
+	u64 multicast_frames_received;
+	u64 frames_64_octets;
+	u64 frames_65_to_127_octets;
+	u64 frames_128_to_255_octets;
+	u64 frames_256_to_511_octets;
+	u64 frames_512_to_1023_octets;
+	u64 frames_1024_to_max_octets;
+	u64 excessive_collision;
+	u64 multicast_frames_sent;
+	u64 broadcast_frames_sent;
+	u64 fc_sent;
+	u64 fc_received;
+	u64 buffer_overrun;
+	u64 undersize;
+	u64 fragments;
+	u64 oversize;
+	u64 jabber;
+	u64 rx_error_frame_received;
+	u64 bad_crc;
+	u64 collisions;
+	u64 late_collision;
+	u64 unicast_frames_received;
+	u64 unicast_frames_sent;
+	u64 sent_multiple;
+	u64 sent_deferred;
+	u64 good_octets_sent;
+};
+
+#define PRESTERA_AP_PORT_MAX   (10)
+
+struct prestera_port_caps {
+	u64 supp_link_modes;
+	u8 supp_fec;
+	u8 type;
+	u8 transceiver;
+};
+
+struct prestera_lag {
+	struct net_device *dev;
+	struct list_head members;
+	u16 member_count;
+	u16 lag_id;
+};
+
+struct prestera_flow_block;
+
+struct prestera_port_mac_state {
+	bool valid;
+	u32 mode;
+	u32 speed;
+	bool oper;
+	u8 duplex;
+	u8 fc;
+	u8 fec;
+};
+
+struct prestera_port_phy_state {
+	u64 lmode_bmap;
+	struct {
+		bool pause;
+		bool asym_pause;
+	} remote_fc;
+	u8 mdix;
+};
+
+struct prestera_port_mac_config {
+	u32 mode;
+	u32 speed;
+	bool admin;
+	u8 inband;
+	u8 duplex;
+	u8 fec;
+};
+
+struct prestera_port_phy_config {
+	u32 mode;
+	bool admin;
+	u8 mdix;
+};
+
+struct prestera_port {
+	struct net_device *dev;
+	struct prestera_switch *sw;
+	struct prestera_flow_block *ingress_flow_block;
+	struct prestera_flow_block *egress_flow_block;
+	struct devlink_port dl_port;
+	struct list_head lag_member;
+	struct prestera_lag *lag;
+	u32 id;
+	u32 hw_id;
+	u32 dev_id;
+	u16 fp_id;
+	u16 pvid;
+	bool autoneg;
+	u64 adver_link_modes;
+	u8 adver_fec;
+	struct prestera_port_caps caps;
+	struct list_head list;
+	struct list_head vlans_list;
+	struct {
+		struct prestera_port_stats stats;
+		struct delayed_work caching_dw;
+	} cached_hw_stats;
+	struct prestera_port_mac_config cfg_mac;
+	struct prestera_port_phy_config cfg_phy;
+	struct prestera_port_mac_state state_mac;
+	struct prestera_port_phy_state state_phy;
+
+	struct phylink_config phy_config;
+	struct phylink *phy_link;
+	struct phylink_pcs phylink_pcs;
+
+	/* protects state_mac */
+	spinlock_t state_mac_lock;
+};
+
+struct prestera_device {
+	struct device *dev;
+	u8 __iomem *ctl_regs;
+	u8 __iomem *pp_regs;
+	struct prestera_fw_rev fw_rev;
+	void *priv;
+
+	/* called by device driver to handle received packets */
+	void (*recv_pkt)(struct prestera_device *dev);
+
+	/* called by device driver to pass event up to the higher layer */
+	int (*recv_msg)(struct prestera_device *dev, void *msg, size_t size);
+
+	/* called by higher layer to send request to the firmware */
+	int (*send_req)(struct prestera_device *dev, int qid, void *in_msg,
+			size_t in_size, void *out_msg, size_t out_size,
+			unsigned int wait);
+};
+
+enum prestera_event_type {
+	PRESTERA_EVENT_TYPE_UNSPEC,
+
+	PRESTERA_EVENT_TYPE_PORT,
+	PRESTERA_EVENT_TYPE_FDB,
+	PRESTERA_EVENT_TYPE_RXTX,
+
+	PRESTERA_EVENT_TYPE_MAX
+};
+
+enum prestera_rxtx_event_id {
+	PRESTERA_RXTX_EVENT_UNSPEC,
+	PRESTERA_RXTX_EVENT_RCV_PKT,
+};
+
+enum prestera_port_event_id {
+	PRESTERA_PORT_EVENT_UNSPEC,
+	PRESTERA_PORT_EVENT_MAC_STATE_CHANGED,
+};
+
+struct prestera_port_event {
+	u32 port_id;
+	union {
+		struct {
+			u32 mode;
+			u32 speed;
+			u8 oper;
+			u8 duplex;
+			u8 fc;
+			u8 fec;
+		} mac;
+		struct {
+			u64 lmode_bmap;
+			struct {
+				bool pause;
+				bool asym_pause;
+			} remote_fc;
+			u8 mdix;
+		} phy;
+	} data;
+};
+
+enum prestera_fdb_entry_type {
+	PRESTERA_FDB_ENTRY_TYPE_REG_PORT,
+	PRESTERA_FDB_ENTRY_TYPE_LAG,
+	PRESTERA_FDB_ENTRY_TYPE_MAX
+};
+
+enum prestera_fdb_event_id {
+	PRESTERA_FDB_EVENT_UNSPEC,
+	PRESTERA_FDB_EVENT_LEARNED,
+	PRESTERA_FDB_EVENT_AGED,
+};
+
+struct prestera_fdb_event {
+	enum prestera_fdb_entry_type type;
+	union {
+		u32 port_id;
+		u16 lag_id;
+	} dest;
+	u32 vid;
+	union {
+		u8 mac[ETH_ALEN];
+	} data;
+};
+
+struct prestera_event {
+	u16 id;
+	union {
+		struct prestera_port_event port_evt;
+		struct prestera_fdb_event fdb_evt;
+	};
+};
+
+enum prestera_if_type {
+	/* the interface is of port type (dev,port) */
+	PRESTERA_IF_PORT_E = 0,
+
+	/* the interface is of lag type (lag-id) */
+	PRESTERA_IF_LAG_E = 1,
+
+	/* the interface is of Vid type (vlan-id) */
+	PRESTERA_IF_VID_E = 3,
+};
+
+struct prestera_iface {
+	enum prestera_if_type type;
+	struct {
+		u32 hw_dev_num;
+		u32 port_num;
+	} dev_port;
+	u32 hw_dev_num;
+	u16 vr_id;
+	u16 lag_id;
+	u16 vlan_id;
+};
+
+struct prestera_switchdev;
+struct prestera_span;
+struct prestera_rxtx;
+struct prestera_trap_data;
+struct prestera_acl;
+
+struct prestera_switch {
+	struct prestera_device *dev;
+	struct prestera_switchdev *swdev;
+	struct prestera_rxtx *rxtx;
+	struct prestera_acl *acl;
+	struct prestera_span *span;
+	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;
+	u32 port_count;
+	u32 mtu_min;
+	u32 mtu_max;
+	u8 id;
+	struct device_node *np;
+	struct prestera_router *router;
+	struct prestera_lag *lags;
+	struct prestera_counter *counter;
+	u8 lag_member_max;
+	u8 lag_max;
+	u32 size_tbl_router_nexthop;
+};
+
+struct prestera_router {
+	struct prestera_switch *sw;
+	struct list_head vr_list;
+	struct list_head rif_entry_list;
+	struct rhashtable nh_neigh_ht;
+	struct rhashtable nexthop_group_ht;
+	struct rhashtable fib_ht;
+	struct rhashtable kern_neigh_cache_ht;
+	struct rhashtable kern_fib_cache_ht;
+	struct notifier_block inetaddr_nb;
+	struct notifier_block inetaddr_valid_nb;
+	struct notifier_block fib_nb;
+	struct notifier_block netevent_nb;
+	u8 *nhgrp_hw_state_cache; /* Bitmap cached hw state of nhs */
+	unsigned long nhgrp_hw_cache_kick; /* jiffies */
+	struct {
+		struct delayed_work dw;
+	} neighs_update;
+};
+
+struct prestera_rxtx_params {
+	bool use_sdma;
+	u32 map_addr;
+};
+
+#define prestera_dev(sw)		((sw)->dev->dev)
+
+static inline void prestera_write(const struct prestera_switch *sw,
+				  unsigned int reg, u32 val)
+{
+	writel(val, sw->dev->pp_regs + reg);
+}
+
+static inline u32 prestera_read(const struct prestera_switch *sw,
+				unsigned int reg)
+{
+	return readl(sw->dev->pp_regs + reg);
+}
+
+int prestera_device_register(struct prestera_device *dev);
+void prestera_device_unregister(struct prestera_device *dev);
+
+struct prestera_port *prestera_port_find_by_hwid(struct prestera_switch *sw,
+						 u32 dev_id, u32 hw_id);
+
+int prestera_port_autoneg_set(struct prestera_port *port, u64 link_modes);
+
+int prestera_router_init(struct prestera_switch *sw);
+void prestera_router_fini(struct prestera_switch *sw);
+
+struct prestera_port *prestera_find_port(struct prestera_switch *sw, u32 id);
+
+struct prestera_switch *prestera_switch_get(struct net_device *dev);
+
+int prestera_port_cfg_mac_read(struct prestera_port *port,
+			       struct prestera_port_mac_config *cfg);
+
+int prestera_port_cfg_mac_write(struct prestera_port *port,
+				struct prestera_port_mac_config *cfg);
+
+struct prestera_port *prestera_port_dev_lower_find(struct net_device *dev);
+
+void prestera_queue_work(struct work_struct *work);
+void prestera_queue_delayed_work(struct delayed_work *work, unsigned long delay);
+void prestera_queue_drain(void);
+
+int prestera_port_learning_set(struct prestera_port *port, bool learn_enable);
+int prestera_port_uc_flood_set(struct prestera_port *port, bool flood);
+int prestera_port_mc_flood_set(struct prestera_port *port, bool flood);
+
+int prestera_port_br_locked_set(struct prestera_port *port, bool br_locked);
+
+int prestera_port_pvid_set(struct prestera_port *port, u16 vid);
+
+bool prestera_netdev_check(const struct net_device *dev);
+
+int prestera_is_valid_mac_addr(struct prestera_port *port, const u8 *addr);
+
+bool prestera_port_is_lag_member(const struct prestera_port *port);
+int prestera_lag_id(struct prestera_switch *sw,
+		    struct net_device *lag_dev, u16 *lag_id);
+
+struct prestera_lag *prestera_lag_by_id(struct prestera_switch *sw, u16 id);
+
+u16 prestera_port_lag_id(const struct prestera_port *port);
+
+struct prestera_mdb_entry *
+prestera_mdb_entry_create(struct prestera_switch *sw,
+			  const unsigned char *addr, u16 vid);
+void prestera_mdb_entry_destroy(struct prestera_mdb_entry *mdb_entry);
+
+struct prestera_flood_domain *
+prestera_flood_domain_create(struct prestera_switch *sw);
+void prestera_flood_domain_destroy(struct prestera_flood_domain *flood_domain);
+
+int
+prestera_flood_domain_port_create(struct prestera_flood_domain *flood_domain,
+				  struct net_device *dev,
+				  u16 vid);
+void
+prestera_flood_domain_port_destroy(struct prestera_flood_domain_port *port);
+struct prestera_flood_domain_port *
+prestera_flood_domain_port_find(struct prestera_flood_domain *flood_domain,
+				struct net_device *dev, u16 vid);
+
+#endif /* _PRESTERA_H_ */

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_acl.c

@@ -0,0 +1,927 @@
+// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
+/* Copyright (c) 2020-2021 Marvell International Ltd. All rights reserved */
+
+#include <linux/rhashtable.h>
+
+#include "prestera_acl.h"
+#include "prestera_flow.h"
+#include "prestera_hw.h"
+#include "prestera.h"
+
+#define ACL_KEYMASK_SIZE	\
+	(sizeof(__be32) * __PRESTERA_ACL_RULE_MATCH_TYPE_MAX)
+
+struct prestera_acl {
+	struct prestera_switch *sw;
+	struct list_head vtcam_list;
+	struct list_head rules;
+	struct rhashtable ruleset_ht;
+	struct rhashtable acl_rule_entry_ht;
+	struct idr uid;
+};
+
+struct prestera_acl_ruleset_ht_key {
+	struct prestera_flow_block *block;
+	u32 chain_index;
+};
+
+struct prestera_acl_rule_entry {
+	struct rhash_head ht_node;
+	struct prestera_acl_rule_entry_key key;
+	u32 hw_id;
+	u32 vtcam_id;
+	struct {
+		struct {
+			u8 valid:1;
+		} accept, drop, trap;
+		struct {
+			u8 valid:1;
+			struct prestera_acl_action_police i;
+		} police;
+		struct {
+			struct prestera_acl_action_jump i;
+			u8 valid:1;
+		} jump;
+		struct {
+			u32 id;
+			struct prestera_counter_block *block;
+		} counter;
+	};
+};
+
+struct prestera_acl_ruleset {
+	struct rhash_head ht_node; /* Member of acl HT */
+	struct prestera_acl_ruleset_ht_key ht_key;
+	struct rhashtable rule_ht;
+	struct prestera_acl *acl;
+	struct {
+		u32 min;
+		u32 max;
+	} prio;
+	unsigned long rule_count;
+	refcount_t refcount;
+	void *keymask;
+	u32 vtcam_id;
+	u32 index;
+	u16 pcl_id;
+	bool offload;
+	bool ingress;
+};
+
+struct prestera_acl_vtcam {
+	struct list_head list;
+	__be32 keymask[__PRESTERA_ACL_RULE_MATCH_TYPE_MAX];
+	refcount_t refcount;
+	u32 id;
+	bool is_keymask_set;
+	u8 lookup;
+	u8 direction;
+};
+
+static const struct rhashtable_params prestera_acl_ruleset_ht_params = {
+	.key_len = sizeof(struct prestera_acl_ruleset_ht_key),
+	.key_offset = offsetof(struct prestera_acl_ruleset, ht_key),
+	.head_offset = offsetof(struct prestera_acl_ruleset, ht_node),
+	.automatic_shrinking = true,
+};
+
+static const struct rhashtable_params prestera_acl_rule_ht_params = {
+	.key_len = sizeof(unsigned long),
+	.key_offset = offsetof(struct prestera_acl_rule, cookie),
+	.head_offset = offsetof(struct prestera_acl_rule, ht_node),
+	.automatic_shrinking = true,
+};
+
+static const struct rhashtable_params __prestera_acl_rule_entry_ht_params = {
+	.key_offset  = offsetof(struct prestera_acl_rule_entry, key),
+	.head_offset = offsetof(struct prestera_acl_rule_entry, ht_node),
+	.key_len     = sizeof(struct prestera_acl_rule_entry_key),
+	.automatic_shrinking = true,
+};
+
+int prestera_acl_chain_to_client(u32 chain_index, bool ingress, u32 *client)
+{
+	static const u32 ingress_client_map[] = {
+		PRESTERA_HW_COUNTER_CLIENT_INGRESS_LOOKUP_0,
+		PRESTERA_HW_COUNTER_CLIENT_INGRESS_LOOKUP_1,
+		PRESTERA_HW_COUNTER_CLIENT_INGRESS_LOOKUP_2
+	};
+
+	if (!ingress) {
+		/* prestera supports only one chain on egress */
+		if (chain_index > 0)
+			return -EINVAL;
+
+		*client = PRESTERA_HW_COUNTER_CLIENT_EGRESS_LOOKUP;
+		return 0;
+	}
+
+	if (chain_index >= ARRAY_SIZE(ingress_client_map))
+		return -EINVAL;
+
+	*client = ingress_client_map[chain_index];
+	return 0;
+}
+
+static bool prestera_acl_chain_is_supported(u32 chain_index, bool ingress)
+{
+	if (!ingress)
+		/* prestera supports only one chain on egress */
+		return chain_index == 0;
+
+	return (chain_index & ~PRESTERA_ACL_CHAIN_MASK) == 0;
+}
+
+static struct prestera_acl_ruleset *
+prestera_acl_ruleset_create(struct prestera_acl *acl,
+			    struct prestera_flow_block *block,
+			    u32 chain_index)
+{
+	struct prestera_acl_ruleset *ruleset;
+	u32 uid = 0;
+	int err;
+
+	if (!prestera_acl_chain_is_supported(chain_index, block->ingress))
+		return ERR_PTR(-EINVAL);
+
+	ruleset = kzalloc_obj(*ruleset);
+	if (!ruleset)
+		return ERR_PTR(-ENOMEM);
+
+	ruleset->acl = acl;
+	ruleset->ingress = block->ingress;
+	ruleset->ht_key.block = block;
+	ruleset->ht_key.chain_index = chain_index;
+	refcount_set(&ruleset->refcount, 1);
+
+	err = rhashtable_init(&ruleset->rule_ht, &prestera_acl_rule_ht_params);
+	if (err)
+		goto err_rhashtable_init;
+
+	err = idr_alloc_u32(&acl->uid, NULL, &uid, U8_MAX, GFP_KERNEL);
+	if (err)
+		goto err_ruleset_create;
+
+	/* make pcl-id based on uid */
+	ruleset->pcl_id = PRESTERA_ACL_PCL_ID_MAKE((u8)uid, chain_index);
+	ruleset->index = uid;
+
+	ruleset->prio.min = UINT_MAX;
+	ruleset->prio.max = 0;
+
+	err = rhashtable_insert_fast(&acl->ruleset_ht, &ruleset->ht_node,
+				     prestera_acl_ruleset_ht_params);
+	if (err)
+		goto err_ruleset_ht_insert;
+
+	return ruleset;
+
+err_ruleset_ht_insert:
+	idr_remove(&acl->uid, uid);
+err_ruleset_create:
+	rhashtable_destroy(&ruleset->rule_ht);
+err_rhashtable_init:
+	kfree(ruleset);
+	return ERR_PTR(err);
+}
+
+int prestera_acl_ruleset_keymask_set(struct prestera_acl_ruleset *ruleset,
+				     void *keymask)
+{
+	ruleset->keymask = kmemdup(keymask, ACL_KEYMASK_SIZE, GFP_KERNEL);
+	if (!ruleset->keymask)
+		return -ENOMEM;
+
+	return 0;
+}
+
+int prestera_acl_ruleset_offload(struct prestera_acl_ruleset *ruleset)
+{
+	struct prestera_acl_iface iface;
+	u32 vtcam_id;
+	int dir;
+	int err;
+
+	dir = ruleset->ingress ?
+		PRESTERA_HW_VTCAM_DIR_INGRESS : PRESTERA_HW_VTCAM_DIR_EGRESS;
+
+	if (ruleset->offload)
+		return -EEXIST;
+
+	err = prestera_acl_vtcam_id_get(ruleset->acl,
+					ruleset->ht_key.chain_index,
+					dir,
+					ruleset->keymask, &vtcam_id);
+	if (err)
+		goto err_vtcam_create;
+
+	if (ruleset->ht_key.chain_index) {
+		/* for chain > 0, bind iface index to pcl-id to be able
+		 * to jump from any other ruleset to this one using the index.
+		 */
+		iface.index = ruleset->index;
+		iface.type = PRESTERA_ACL_IFACE_TYPE_INDEX;
+		err = prestera_hw_vtcam_iface_bind(ruleset->acl->sw, &iface,
+						   vtcam_id, ruleset->pcl_id);
+		if (err)
+			goto err_ruleset_bind;
+	}
+
+	ruleset->vtcam_id = vtcam_id;
+	ruleset->offload = true;
+	return 0;
+
+err_ruleset_bind:
+	prestera_acl_vtcam_id_put(ruleset->acl, ruleset->vtcam_id);
+err_vtcam_create:
+	return err;
+}
+
+static void prestera_acl_ruleset_destroy(struct prestera_acl_ruleset *ruleset)
+{
+	struct prestera_acl *acl = ruleset->acl;
+	u8 uid = ruleset->pcl_id & PRESTERA_ACL_KEYMASK_PCL_ID_USER;
+	int err;
+
+	rhashtable_remove_fast(&acl->ruleset_ht, &ruleset->ht_node,
+			       prestera_acl_ruleset_ht_params);
+
+	if (ruleset->offload) {
+		if (ruleset->ht_key.chain_index) {
+			struct prestera_acl_iface iface = {
+				.type = PRESTERA_ACL_IFACE_TYPE_INDEX,
+				.index = ruleset->index
+			};
+			err = prestera_hw_vtcam_iface_unbind(acl->sw, &iface,
+							     ruleset->vtcam_id);
+			WARN_ON(err);
+		}
+		WARN_ON(prestera_acl_vtcam_id_put(acl, ruleset->vtcam_id));
+	}
+
+	idr_remove(&acl->uid, uid);
+	rhashtable_destroy(&ruleset->rule_ht);
+	kfree(ruleset->keymask);
+	kfree(ruleset);
+}
+
+static struct prestera_acl_ruleset *
+__prestera_acl_ruleset_lookup(struct prestera_acl *acl,
+			      struct prestera_flow_block *block,
+			      u32 chain_index)
+{
+	struct prestera_acl_ruleset_ht_key ht_key;
+
+	memset(&ht_key, 0, sizeof(ht_key));
+	ht_key.block = block;
+	ht_key.chain_index = chain_index;
+	return rhashtable_lookup_fast(&acl->ruleset_ht, &ht_key,
+				      prestera_acl_ruleset_ht_params);
+}
+
+struct prestera_acl_ruleset *
+prestera_acl_ruleset_lookup(struct prestera_acl *acl,
+			    struct prestera_flow_block *block,
+			    u32 chain_index)
+{
+	struct prestera_acl_ruleset *ruleset;
+
+	ruleset = __prestera_acl_ruleset_lookup(acl, block, chain_index);
+	if (!ruleset)
+		return ERR_PTR(-ENOENT);
+
+	refcount_inc(&ruleset->refcount);
+	return ruleset;
+}
+
+struct prestera_acl_ruleset *
+prestera_acl_ruleset_get(struct prestera_acl *acl,
+			 struct prestera_flow_block *block,
+			 u32 chain_index)
+{
+	struct prestera_acl_ruleset *ruleset;
+
+	ruleset = __prestera_acl_ruleset_lookup(acl, block, chain_index);
+	if (ruleset) {
+		refcount_inc(&ruleset->refcount);
+		return ruleset;
+	}
+
+	return prestera_acl_ruleset_create(acl, block, chain_index);
+}
+
+void prestera_acl_ruleset_put(struct prestera_acl_ruleset *ruleset)
+{
+	if (!refcount_dec_and_test(&ruleset->refcount))
+		return;
+
+	prestera_acl_ruleset_destroy(ruleset);
+}
+
+int prestera_acl_ruleset_bind(struct prestera_acl_ruleset *ruleset,
+			      struct prestera_port *port)
+{
+	struct prestera_acl_iface iface = {
+		.type = PRESTERA_ACL_IFACE_TYPE_PORT,
+		.port = port
+	};
+
+	return prestera_hw_vtcam_iface_bind(port->sw, &iface, ruleset->vtcam_id,
+					    ruleset->pcl_id);
+}
+
+int prestera_acl_ruleset_unbind(struct prestera_acl_ruleset *ruleset,
+				struct prestera_port *port)
+{
+	struct prestera_acl_iface iface = {
+		.type = PRESTERA_ACL_IFACE_TYPE_PORT,
+		.port = port
+	};
+
+	return prestera_hw_vtcam_iface_unbind(port->sw, &iface,
+					      ruleset->vtcam_id);
+}
+
+static int prestera_acl_ruleset_block_bind(struct prestera_acl_ruleset *ruleset,
+					   struct prestera_flow_block *block)
+{
+	struct prestera_flow_block_binding *binding;
+	int err;
+
+	block->ruleset_zero = ruleset;
+	list_for_each_entry(binding, &block->binding_list, list) {
+		err = prestera_acl_ruleset_bind(ruleset, binding->port);
+		if (err)
+			goto rollback;
+	}
+	return 0;
+
+rollback:
+	list_for_each_entry_continue_reverse(binding, &block->binding_list,
+					     list)
+		err = prestera_acl_ruleset_unbind(ruleset, binding->port);
+	block->ruleset_zero = NULL;
+
+	return err;
+}
+
+static void
+prestera_acl_ruleset_block_unbind(struct prestera_acl_ruleset *ruleset,
+				  struct prestera_flow_block *block)
+{
+	struct prestera_flow_block_binding *binding;
+
+	list_for_each_entry(binding, &block->binding_list, list)
+		prestera_acl_ruleset_unbind(ruleset, binding->port);
+	block->ruleset_zero = NULL;
+}
+
+static void
+prestera_acl_ruleset_prio_refresh(struct prestera_acl *acl,
+				  struct prestera_acl_ruleset *ruleset)
+{
+	struct prestera_acl_rule *rule;
+
+	ruleset->prio.min = UINT_MAX;
+	ruleset->prio.max = 0;
+
+	list_for_each_entry(rule, &acl->rules, list) {
+		if (ruleset->ingress != rule->ruleset->ingress)
+			continue;
+		if (ruleset->ht_key.chain_index != rule->chain_index)
+			continue;
+
+		ruleset->prio.min = min(ruleset->prio.min, rule->priority);
+		ruleset->prio.max = max(ruleset->prio.max, rule->priority);
+	}
+}
+
+void
+prestera_acl_rule_keymask_pcl_id_set(struct prestera_acl_rule *rule, u16 pcl_id)
+{
+	struct prestera_acl_match *r_match = &rule->re_key.match;
+	__be16 pcl_id_mask = htons(PRESTERA_ACL_KEYMASK_PCL_ID);
+	__be16 pcl_id_key = htons(pcl_id);
+
+	rule_match_set(r_match->key, PCL_ID, pcl_id_key);
+	rule_match_set(r_match->mask, PCL_ID, pcl_id_mask);
+}
+
+struct prestera_acl_rule *
+prestera_acl_rule_lookup(struct prestera_acl_ruleset *ruleset,
+			 unsigned long cookie)
+{
+	return rhashtable_lookup_fast(&ruleset->rule_ht, &cookie,
+				      prestera_acl_rule_ht_params);
+}
+
+u32 prestera_acl_ruleset_index_get(const struct prestera_acl_ruleset *ruleset)
+{
+	return ruleset->index;
+}
+
+void prestera_acl_ruleset_prio_get(struct prestera_acl_ruleset *ruleset,
+				   u32 *prio_min, u32 *prio_max)
+{
+	*prio_min = ruleset->prio.min;
+	*prio_max = ruleset->prio.max;
+}
+
+bool prestera_acl_ruleset_is_offload(struct prestera_acl_ruleset *ruleset)
+{
+	return ruleset->offload;
+}
+
+struct prestera_acl_rule *
+prestera_acl_rule_create(struct prestera_acl_ruleset *ruleset,
+			 unsigned long cookie, u32 chain_index)
+{
+	struct prestera_acl_rule *rule;
+
+	rule = kzalloc_obj(*rule);
+	if (!rule)
+		return ERR_PTR(-ENOMEM);
+
+	rule->ruleset = ruleset;
+	rule->cookie = cookie;
+	rule->chain_index = chain_index;
+
+	refcount_inc(&ruleset->refcount);
+
+	return rule;
+}
+
+void prestera_acl_rule_priority_set(struct prestera_acl_rule *rule,
+				    u32 priority)
+{
+	rule->priority = priority;
+}
+
+void prestera_acl_rule_destroy(struct prestera_acl_rule *rule)
+{
+	if (rule->jump_ruleset)
+		/* release ruleset kept by jump action */
+		prestera_acl_ruleset_put(rule->jump_ruleset);
+
+	prestera_acl_ruleset_put(rule->ruleset);
+	kfree(rule);
+}
+
+static void prestera_acl_ruleset_prio_update(struct prestera_acl_ruleset *ruleset,
+					     u32 prio)
+{
+	ruleset->prio.min = min(ruleset->prio.min, prio);
+	ruleset->prio.max = max(ruleset->prio.max, prio);
+}
+
+int prestera_acl_rule_add(struct prestera_switch *sw,
+			  struct prestera_acl_rule *rule)
+{
+	int err;
+	struct prestera_acl_ruleset *ruleset = rule->ruleset;
+	struct prestera_flow_block *block = ruleset->ht_key.block;
+
+	/* try to add rule to hash table first */
+	err = rhashtable_insert_fast(&ruleset->rule_ht, &rule->ht_node,
+				     prestera_acl_rule_ht_params);
+	if (err)
+		goto err_ht_insert;
+
+	prestera_acl_rule_keymask_pcl_id_set(rule, ruleset->pcl_id);
+	rule->re_arg.vtcam_id = ruleset->vtcam_id;
+	rule->re_key.prio = rule->priority;
+
+	rule->re = prestera_acl_rule_entry_find(sw->acl, &rule->re_key);
+	err = WARN_ON(rule->re) ? -EEXIST : 0;
+	if (err)
+		goto err_rule_add;
+
+	rule->re = prestera_acl_rule_entry_create(sw->acl, &rule->re_key,
+						  &rule->re_arg);
+	err = !rule->re ? -EINVAL : 0;
+	if (err)
+		goto err_rule_add;
+
+	/* bind the block (all ports) to chain index 0, rest of
+	 * the chains are bound to goto action
+	 */
+	if (!ruleset->ht_key.chain_index && !ruleset->rule_count) {
+		err = prestera_acl_ruleset_block_bind(ruleset, block);
+		if (err)
+			goto err_acl_block_bind;
+	}
+
+	list_add_tail(&rule->list, &sw->acl->rules);
+	ruleset->rule_count++;
+	prestera_acl_ruleset_prio_update(ruleset, rule->priority);
+	return 0;
+
+err_acl_block_bind:
+	prestera_acl_rule_entry_destroy(sw->acl, rule->re);
+err_rule_add:
+	rule->re = NULL;
+	rhashtable_remove_fast(&ruleset->rule_ht, &rule->ht_node,
+			       prestera_acl_rule_ht_params);
+err_ht_insert:
+	return err;
+}
+
+void prestera_acl_rule_del(struct prestera_switch *sw,
+			   struct prestera_acl_rule *rule)
+{
+	struct prestera_acl_ruleset *ruleset = rule->ruleset;
+	struct prestera_flow_block *block = ruleset->ht_key.block;
+
+	rhashtable_remove_fast(&ruleset->rule_ht, &rule->ht_node,
+			       prestera_acl_rule_ht_params);
+	ruleset->rule_count--;
+	list_del(&rule->list);
+
+	prestera_acl_rule_entry_destroy(sw->acl, rule->re);
+	prestera_acl_ruleset_prio_refresh(sw->acl, ruleset);
+
+	/* unbind block (all ports) */
+	if (!ruleset->ht_key.chain_index && !ruleset->rule_count)
+		prestera_acl_ruleset_block_unbind(ruleset, block);
+}
+
+int prestera_acl_rule_get_stats(struct prestera_acl *acl,
+				struct prestera_acl_rule *rule,
+				u64 *packets, u64 *bytes, u64 *last_use)
+{
+	u64 current_packets;
+	u64 current_bytes;
+	int err;
+
+	err = prestera_counter_stats_get(acl->sw->counter,
+					 rule->re->counter.block,
+					 rule->re->counter.id,
+					 &current_packets, &current_bytes);
+	if (err)
+		return err;
+
+	*packets = current_packets;
+	*bytes = current_bytes;
+	*last_use = jiffies;
+
+	return 0;
+}
+
+struct prestera_acl_rule_entry *
+prestera_acl_rule_entry_find(struct prestera_acl *acl,
+			     struct prestera_acl_rule_entry_key *key)
+{
+	return rhashtable_lookup_fast(&acl->acl_rule_entry_ht, key,
+				      __prestera_acl_rule_entry_ht_params);
+}
+
+static int __prestera_acl_rule_entry2hw_del(struct prestera_switch *sw,
+					    struct prestera_acl_rule_entry *e)
+{
+	return prestera_hw_vtcam_rule_del(sw, e->vtcam_id, e->hw_id);
+}
+
+static int __prestera_acl_rule_entry2hw_add(struct prestera_switch *sw,
+					    struct prestera_acl_rule_entry *e)
+{
+	struct prestera_acl_hw_action_info act_hw[PRESTERA_ACL_RULE_ACTION_MAX];
+	int act_num;
+
+	memset(&act_hw, 0, sizeof(act_hw));
+	act_num = 0;
+
+	/* accept */
+	if (e->accept.valid) {
+		act_hw[act_num].id = PRESTERA_ACL_RULE_ACTION_ACCEPT;
+		act_num++;
+	}
+	/* drop */
+	if (e->drop.valid) {
+		act_hw[act_num].id = PRESTERA_ACL_RULE_ACTION_DROP;
+		act_num++;
+	}
+	/* trap */
+	if (e->trap.valid) {
+		act_hw[act_num].id = PRESTERA_ACL_RULE_ACTION_TRAP;
+		act_num++;
+	}
+	/* police */
+	if (e->police.valid) {
+		act_hw[act_num].id = PRESTERA_ACL_RULE_ACTION_POLICE;
+		act_hw[act_num].police = e->police.i;
+		act_num++;
+	}
+	/* jump */
+	if (e->jump.valid) {
+		act_hw[act_num].id = PRESTERA_ACL_RULE_ACTION_JUMP;
+		act_hw[act_num].jump = e->jump.i;
+		act_num++;
+	}
+	/* counter */
+	if (e->counter.block) {
+		act_hw[act_num].id = PRESTERA_ACL_RULE_ACTION_COUNT;
+		act_hw[act_num].count.id = e->counter.id;
+		act_num++;
+	}
+
+	return prestera_hw_vtcam_rule_add(sw, e->vtcam_id, e->key.prio,
+					  e->key.match.key, e->key.match.mask,
+					  act_hw, act_num, &e->hw_id);
+}
+
+static void
+__prestera_acl_rule_entry_act_destruct(struct prestera_switch *sw,
+				       struct prestera_acl_rule_entry *e)
+{
+	/* counter */
+	prestera_counter_put(sw->counter, e->counter.block, e->counter.id);
+	/* police */
+	if (e->police.valid)
+		prestera_hw_policer_release(sw, e->police.i.id);
+}
+
+void prestera_acl_rule_entry_destroy(struct prestera_acl *acl,
+				     struct prestera_acl_rule_entry *e)
+{
+	int ret;
+
+	rhashtable_remove_fast(&acl->acl_rule_entry_ht, &e->ht_node,
+			       __prestera_acl_rule_entry_ht_params);
+
+	ret = __prestera_acl_rule_entry2hw_del(acl->sw, e);
+	WARN_ON(ret && ret != -ENODEV);
+
+	__prestera_acl_rule_entry_act_destruct(acl->sw, e);
+	kfree(e);
+}
+
+static int
+__prestera_acl_rule_entry_act_construct(struct prestera_switch *sw,
+					struct prestera_acl_rule_entry *e,
+					struct prestera_acl_rule_entry_arg *arg)
+{
+	int err;
+
+	/* accept */
+	e->accept.valid = arg->accept.valid;
+	/* drop */
+	e->drop.valid = arg->drop.valid;
+	/* trap */
+	e->trap.valid = arg->trap.valid;
+	/* jump */
+	e->jump.valid = arg->jump.valid;
+	e->jump.i = arg->jump.i;
+	/* police */
+	if (arg->police.valid) {
+		u8 type = arg->police.ingress ? PRESTERA_POLICER_TYPE_INGRESS :
+						PRESTERA_POLICER_TYPE_EGRESS;
+
+		err = prestera_hw_policer_create(sw, type, &e->police.i.id);
+		if (err)
+			goto err_out;
+
+		err = prestera_hw_policer_sr_tcm_set(sw, e->police.i.id,
+						     arg->police.rate,
+						     arg->police.burst);
+		if (err) {
+			prestera_hw_policer_release(sw, e->police.i.id);
+			goto err_out;
+		}
+		e->police.valid = arg->police.valid;
+	}
+	/* counter */
+	if (arg->count.valid) {
+		err = prestera_counter_get(sw->counter, arg->count.client,
+					   &e->counter.block,
+					   &e->counter.id);
+		if (err)
+			goto err_out;
+	}
+
+	return 0;
+
+err_out:
+	__prestera_acl_rule_entry_act_destruct(sw, e);
+	return -EINVAL;
+}
+
+struct prestera_acl_rule_entry *
+prestera_acl_rule_entry_create(struct prestera_acl *acl,
+			       struct prestera_acl_rule_entry_key *key,
+			       struct prestera_acl_rule_entry_arg *arg)
+{
+	struct prestera_acl_rule_entry *e;
+	int err;
+
+	e = kzalloc_obj(*e);
+	if (!e)
+		goto err_kzalloc;
+
+	memcpy(&e->key, key, sizeof(*key));
+	e->vtcam_id = arg->vtcam_id;
+	err = __prestera_acl_rule_entry_act_construct(acl->sw, e, arg);
+	if (err)
+		goto err_act_construct;
+
+	err = __prestera_acl_rule_entry2hw_add(acl->sw, e);
+	if (err)
+		goto err_hw_add;
+
+	err = rhashtable_insert_fast(&acl->acl_rule_entry_ht, &e->ht_node,
+				     __prestera_acl_rule_entry_ht_params);
+	if (err)
+		goto err_ht_insert;
+
+	return e;
+
+err_ht_insert:
+	WARN_ON(__prestera_acl_rule_entry2hw_del(acl->sw, e));
+err_hw_add:
+	__prestera_acl_rule_entry_act_destruct(acl->sw, e);
+err_act_construct:
+	kfree(e);
+err_kzalloc:
+	return NULL;
+}
+
+static int __prestera_acl_vtcam_id_try_fit(struct prestera_acl *acl, u8 lookup,
+					   void *keymask, u32 *vtcam_id)
+{
+	struct prestera_acl_vtcam *vtcam;
+	int i;
+
+	list_for_each_entry(vtcam, &acl->vtcam_list, list) {
+		if (lookup != vtcam->lookup)
+			continue;
+
+		if (!keymask && !vtcam->is_keymask_set)
+			goto vtcam_found;
+
+		if (!(keymask && vtcam->is_keymask_set))
+			continue;
+
+		/* try to fit with vtcam keymask */
+		for (i = 0; i < __PRESTERA_ACL_RULE_MATCH_TYPE_MAX; i++) {
+			__be32 __keymask = ((__be32 *)keymask)[i];
+
+			if (!__keymask)
+				/* vtcam keymask in not interested */
+				continue;
+
+			if (__keymask & ~vtcam->keymask[i])
+				/* keymask does not fit the vtcam keymask */
+				break;
+		}
+
+		if (i == __PRESTERA_ACL_RULE_MATCH_TYPE_MAX)
+			/* keymask fits vtcam keymask, return it */
+			goto vtcam_found;
+	}
+
+	/* nothing is found */
+	return -ENOENT;
+
+vtcam_found:
+	refcount_inc(&vtcam->refcount);
+	*vtcam_id = vtcam->id;
+	return 0;
+}
+
+int prestera_acl_vtcam_id_get(struct prestera_acl *acl, u8 lookup, u8 dir,
+			      void *keymask, u32 *vtcam_id)
+{
+	struct prestera_acl_vtcam *vtcam;
+	u32 new_vtcam_id;
+	int err;
+
+	/* find the vtcam that suits keymask. We do not expect to have
+	 * a big number of vtcams, so, the list type for vtcam list is
+	 * fine for now
+	 */
+	list_for_each_entry(vtcam, &acl->vtcam_list, list) {
+		if (lookup != vtcam->lookup ||
+		    dir != vtcam->direction)
+			continue;
+
+		if (!keymask && !vtcam->is_keymask_set) {
+			refcount_inc(&vtcam->refcount);
+			goto vtcam_found;
+		}
+
+		if (keymask && vtcam->is_keymask_set &&
+		    !memcmp(keymask, vtcam->keymask, sizeof(vtcam->keymask))) {
+			refcount_inc(&vtcam->refcount);
+			goto vtcam_found;
+		}
+	}
+
+	/* vtcam not found, try to create new one */
+	vtcam = kzalloc_obj(*vtcam);
+	if (!vtcam)
+		return -ENOMEM;
+
+	err = prestera_hw_vtcam_create(acl->sw, lookup, keymask, &new_vtcam_id,
+				       dir);
+	if (err) {
+		kfree(vtcam);
+
+		/* cannot create new, try to fit into existing vtcam */
+		if (__prestera_acl_vtcam_id_try_fit(acl, lookup,
+						    keymask, &new_vtcam_id))
+			return err;
+
+		*vtcam_id = new_vtcam_id;
+		return 0;
+	}
+
+	vtcam->direction = dir;
+	vtcam->id = new_vtcam_id;
+	vtcam->lookup = lookup;
+	if (keymask) {
+		memcpy(vtcam->keymask, keymask, sizeof(vtcam->keymask));
+		vtcam->is_keymask_set = true;
+	}
+	refcount_set(&vtcam->refcount, 1);
+	list_add_rcu(&vtcam->list, &acl->vtcam_list);
+
+vtcam_found:
+	*vtcam_id = vtcam->id;
+	return 0;
+}
+
+int prestera_acl_vtcam_id_put(struct prestera_acl *acl, u32 vtcam_id)
+{
+	struct prestera_acl_vtcam *vtcam;
+	int err;
+
+	list_for_each_entry(vtcam, &acl->vtcam_list, list) {
+		if (vtcam_id != vtcam->id)
+			continue;
+
+		if (!refcount_dec_and_test(&vtcam->refcount))
+			return 0;
+
+		err = prestera_hw_vtcam_destroy(acl->sw, vtcam->id);
+		if (err && err != -ENODEV) {
+			refcount_set(&vtcam->refcount, 1);
+			return err;
+		}
+
+		list_del(&vtcam->list);
+		kfree(vtcam);
+		return 0;
+	}
+
+	return -ENOENT;
+}
+
+int prestera_acl_init(struct prestera_switch *sw)
+{
+	struct prestera_acl *acl;
+	int err;
+
+	acl = kzalloc_obj(*acl);
+	if (!acl)
+		return -ENOMEM;
+
+	acl->sw = sw;
+	INIT_LIST_HEAD(&acl->rules);
+	INIT_LIST_HEAD(&acl->vtcam_list);
+	idr_init(&acl->uid);
+
+	err = rhashtable_init(&acl->acl_rule_entry_ht,
+			      &__prestera_acl_rule_entry_ht_params);
+	if (err)
+		goto err_acl_rule_entry_ht_init;
+
+	err = rhashtable_init(&acl->ruleset_ht,
+			      &prestera_acl_ruleset_ht_params);
+	if (err)
+		goto err_ruleset_ht_init;
+
+	sw->acl = acl;
+
+	return 0;
+
+err_ruleset_ht_init:
+	rhashtable_destroy(&acl->acl_rule_entry_ht);
+err_acl_rule_entry_ht_init:
+	kfree(acl);
+	return err;
+}
+
+void prestera_acl_fini(struct prestera_switch *sw)
+{
+	struct prestera_acl *acl = sw->acl;
+
+	WARN_ON(!idr_is_empty(&acl->uid));
+	idr_destroy(&acl->uid);
+
+	WARN_ON(!list_empty(&acl->vtcam_list));
+	WARN_ON(!list_empty(&acl->rules));
+
+	rhashtable_destroy(&acl->ruleset_ht);
+	rhashtable_destroy(&acl->acl_rule_entry_ht);
+
+	kfree(acl);
+}

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_acl.h

@@ -0,0 +1,209 @@
+/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */
+/* Copyright (c) 2020-2021 Marvell International Ltd. All rights reserved. */
+
+#ifndef _PRESTERA_ACL_H_
+#define _PRESTERA_ACL_H_
+
+#include <linux/types.h>
+#include "prestera_counter.h"
+
+#define PRESTERA_ACL_KEYMASK_PCL_ID		0x3FF
+#define PRESTERA_ACL_KEYMASK_PCL_ID_USER			\
+	(PRESTERA_ACL_KEYMASK_PCL_ID & 0x00FF)
+#define PRESTERA_ACL_KEYMASK_PCL_ID_CHAIN			\
+	(PRESTERA_ACL_KEYMASK_PCL_ID & 0xFF00)
+#define PRESTERA_ACL_CHAIN_MASK					\
+	(PRESTERA_ACL_KEYMASK_PCL_ID >> 8)
+
+#define PRESTERA_ACL_PCL_ID_MAKE(uid, chain_id)			\
+	(((uid) & PRESTERA_ACL_KEYMASK_PCL_ID_USER) |		\
+	(((chain_id) << 8) & PRESTERA_ACL_KEYMASK_PCL_ID_CHAIN))
+
+#define rule_match_set_n(match_p, type, val_p, size)		\
+	memcpy(&(match_p)[PRESTERA_ACL_RULE_MATCH_TYPE_##type],	\
+	       val_p, size)
+#define rule_match_set(match_p, type, val)			\
+	memcpy(&(match_p)[PRESTERA_ACL_RULE_MATCH_TYPE_##type],	\
+	       &(val), sizeof(val))
+
+enum prestera_acl_match_type {
+	PRESTERA_ACL_RULE_MATCH_TYPE_PCL_ID,
+	PRESTERA_ACL_RULE_MATCH_TYPE_ETH_TYPE,
+	PRESTERA_ACL_RULE_MATCH_TYPE_ETH_DMAC_0,
+	PRESTERA_ACL_RULE_MATCH_TYPE_ETH_DMAC_1,
+	PRESTERA_ACL_RULE_MATCH_TYPE_ETH_SMAC_0,
+	PRESTERA_ACL_RULE_MATCH_TYPE_ETH_SMAC_1,
+	PRESTERA_ACL_RULE_MATCH_TYPE_IP_PROTO,
+	PRESTERA_ACL_RULE_MATCH_TYPE_SYS_PORT,
+	PRESTERA_ACL_RULE_MATCH_TYPE_SYS_DEV,
+	PRESTERA_ACL_RULE_MATCH_TYPE_IP_SRC,
+	PRESTERA_ACL_RULE_MATCH_TYPE_IP_DST,
+	PRESTERA_ACL_RULE_MATCH_TYPE_L4_PORT_SRC,
+	PRESTERA_ACL_RULE_MATCH_TYPE_L4_PORT_DST,
+	PRESTERA_ACL_RULE_MATCH_TYPE_L4_PORT_RANGE_SRC,
+	PRESTERA_ACL_RULE_MATCH_TYPE_L4_PORT_RANGE_DST,
+	PRESTERA_ACL_RULE_MATCH_TYPE_VLAN_ID,
+	PRESTERA_ACL_RULE_MATCH_TYPE_VLAN_TPID,
+	PRESTERA_ACL_RULE_MATCH_TYPE_ICMP_TYPE,
+	PRESTERA_ACL_RULE_MATCH_TYPE_ICMP_CODE,
+
+	__PRESTERA_ACL_RULE_MATCH_TYPE_MAX
+};
+
+enum prestera_acl_rule_action {
+	PRESTERA_ACL_RULE_ACTION_ACCEPT = 0,
+	PRESTERA_ACL_RULE_ACTION_DROP = 1,
+	PRESTERA_ACL_RULE_ACTION_TRAP = 2,
+	PRESTERA_ACL_RULE_ACTION_JUMP = 5,
+	PRESTERA_ACL_RULE_ACTION_COUNT = 7,
+	PRESTERA_ACL_RULE_ACTION_POLICE = 8,
+
+	PRESTERA_ACL_RULE_ACTION_MAX
+};
+
+enum {
+	PRESTERA_ACL_IFACE_TYPE_PORT,
+	PRESTERA_ACL_IFACE_TYPE_INDEX
+};
+
+struct prestera_acl_match {
+	__be32 key[__PRESTERA_ACL_RULE_MATCH_TYPE_MAX];
+	__be32 mask[__PRESTERA_ACL_RULE_MATCH_TYPE_MAX];
+};
+
+struct prestera_acl_action_jump {
+	u32 index;
+};
+
+struct prestera_acl_action_police {
+	u32 id;
+};
+
+struct prestera_acl_action_count {
+	u32 id;
+};
+
+struct prestera_acl_rule_entry_key {
+	u32 prio;
+	struct prestera_acl_match match;
+};
+
+struct prestera_acl_hw_action_info {
+	enum prestera_acl_rule_action id;
+	union {
+		struct prestera_acl_action_police police;
+		struct prestera_acl_action_count count;
+		struct prestera_acl_action_jump jump;
+	};
+};
+
+/* This struct (arg) used only to be passed as parameter for
+ * acl_rule_entry_create. Must be flat. Can contain object keys, which will be
+ * resolved to object links, before saving to acl_rule_entry struct
+ */
+struct prestera_acl_rule_entry_arg {
+	u32 vtcam_id;
+	struct {
+		struct {
+			u8 valid:1;
+		} accept, drop, trap;
+		struct {
+			struct prestera_acl_action_jump i;
+			u8 valid:1;
+		} jump;
+		struct {
+			u8 valid:1;
+			u64 rate;
+			u64 burst;
+			bool ingress;
+		} police;
+		struct {
+			u8 valid:1;
+			u32 client;
+		} count;
+	};
+};
+
+struct prestera_acl_rule {
+	struct rhash_head ht_node; /* Member of acl HT */
+	struct list_head list;
+	struct prestera_acl_ruleset *ruleset;
+	struct prestera_acl_ruleset *jump_ruleset;
+	unsigned long cookie;
+	u32 chain_index;
+	u32 priority;
+	struct prestera_acl_rule_entry_key re_key;
+	struct prestera_acl_rule_entry_arg re_arg;
+	struct prestera_acl_rule_entry *re;
+};
+
+struct prestera_acl_iface {
+	union {
+		struct prestera_port *port;
+		u32 index;
+	};
+	u8 type;
+};
+
+struct prestera_acl;
+struct prestera_switch;
+struct prestera_flow_block;
+
+int prestera_acl_init(struct prestera_switch *sw);
+void prestera_acl_fini(struct prestera_switch *sw);
+
+struct prestera_acl_rule *
+prestera_acl_rule_create(struct prestera_acl_ruleset *ruleset,
+			 unsigned long cookie, u32 chain_index);
+void prestera_acl_rule_priority_set(struct prestera_acl_rule *rule,
+				    u32 priority);
+void prestera_acl_rule_destroy(struct prestera_acl_rule *rule);
+struct prestera_acl_rule *
+prestera_acl_rule_lookup(struct prestera_acl_ruleset *ruleset,
+			 unsigned long cookie);
+int prestera_acl_rule_add(struct prestera_switch *sw,
+			  struct prestera_acl_rule *rule);
+void prestera_acl_rule_del(struct prestera_switch *sw,
+			   struct prestera_acl_rule *rule);
+int prestera_acl_rule_get_stats(struct prestera_acl *acl,
+				struct prestera_acl_rule *rule,
+				u64 *packets, u64 *bytes, u64 *last_use);
+struct prestera_acl_rule_entry *
+prestera_acl_rule_entry_find(struct prestera_acl *acl,
+			     struct prestera_acl_rule_entry_key *key);
+void prestera_acl_rule_entry_destroy(struct prestera_acl *acl,
+				     struct prestera_acl_rule_entry *e);
+struct prestera_acl_rule_entry *
+prestera_acl_rule_entry_create(struct prestera_acl *acl,
+			       struct prestera_acl_rule_entry_key *key,
+			       struct prestera_acl_rule_entry_arg *arg);
+struct prestera_acl_ruleset *
+prestera_acl_ruleset_get(struct prestera_acl *acl,
+			 struct prestera_flow_block *block,
+			 u32 chain_index);
+struct prestera_acl_ruleset *
+prestera_acl_ruleset_lookup(struct prestera_acl *acl,
+			    struct prestera_flow_block *block,
+			    u32 chain_index);
+int prestera_acl_ruleset_keymask_set(struct prestera_acl_ruleset *ruleset,
+				     void *keymask);
+bool prestera_acl_ruleset_is_offload(struct prestera_acl_ruleset *ruleset);
+int prestera_acl_ruleset_offload(struct prestera_acl_ruleset *ruleset);
+void prestera_acl_ruleset_put(struct prestera_acl_ruleset *ruleset);
+int prestera_acl_ruleset_bind(struct prestera_acl_ruleset *ruleset,
+			      struct prestera_port *port);
+int prestera_acl_ruleset_unbind(struct prestera_acl_ruleset *ruleset,
+				struct prestera_port *port);
+u32 prestera_acl_ruleset_index_get(const struct prestera_acl_ruleset *ruleset);
+void prestera_acl_ruleset_prio_get(struct prestera_acl_ruleset *ruleset,
+				   u32 *prio_min, u32 *prio_max);
+void
+prestera_acl_rule_keymask_pcl_id_set(struct prestera_acl_rule *rule,
+				     u16 pcl_id);
+
+int prestera_acl_vtcam_id_get(struct prestera_acl *acl, u8 lookup, u8 dir,
+			      void *keymask, u32 *vtcam_id);
+int prestera_acl_vtcam_id_put(struct prestera_acl *acl, u32 vtcam_id);
+int prestera_acl_chain_to_client(u32 chain_index, bool ingress, u32 *client);
+
+#endif /* _PRESTERA_ACL_H_ */

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_counter.c

@@ -0,0 +1,473 @@
+// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
+/* Copyright (c) 2021 Marvell International Ltd. All rights reserved */
+
+#include "prestera.h"
+#include "prestera_hw.h"
+#include "prestera_acl.h"
+#include "prestera_counter.h"
+
+#define COUNTER_POLL_TIME	(msecs_to_jiffies(1000))
+#define COUNTER_RESCHED_TIME	(msecs_to_jiffies(50))
+#define COUNTER_BULK_SIZE	(256)
+
+struct prestera_counter {
+	struct prestera_switch *sw;
+	struct delayed_work stats_dw;
+	struct mutex mtx;  /* protect block_list */
+	struct prestera_counter_block **block_list;
+	u32 total_read;
+	u32 block_list_len;
+	u32 curr_idx;
+	bool is_fetching;
+};
+
+struct prestera_counter_block {
+	struct list_head list;
+	u32 id;
+	u32 offset;
+	u32 num_counters;
+	u32 client;
+	struct idr counter_idr;
+	refcount_t refcnt;
+	struct mutex mtx;  /* protect stats and counter_idr */
+	struct prestera_counter_stats *stats;
+	u8 *counter_flag;
+	bool is_updating;
+	bool full;
+};
+
+enum {
+	COUNTER_FLAG_READY = 0,
+	COUNTER_FLAG_INVALID = 1
+};
+
+static bool
+prestera_counter_is_ready(struct prestera_counter_block *block, u32 id)
+{
+	return block->counter_flag[id - block->offset] == COUNTER_FLAG_READY;
+}
+
+static void prestera_counter_lock(struct prestera_counter *counter)
+{
+	mutex_lock(&counter->mtx);
+}
+
+static void prestera_counter_unlock(struct prestera_counter *counter)
+{
+	mutex_unlock(&counter->mtx);
+}
+
+static void prestera_counter_block_lock(struct prestera_counter_block *block)
+{
+	mutex_lock(&block->mtx);
+}
+
+static void prestera_counter_block_unlock(struct prestera_counter_block *block)
+{
+	mutex_unlock(&block->mtx);
+}
+
+static bool prestera_counter_block_incref(struct prestera_counter_block *block)
+{
+	return refcount_inc_not_zero(&block->refcnt);
+}
+
+static bool prestera_counter_block_decref(struct prestera_counter_block *block)
+{
+	return refcount_dec_and_test(&block->refcnt);
+}
+
+/* must be called with prestera_counter_block_lock() */
+static void prestera_counter_stats_clear(struct prestera_counter_block *block,
+					 u32 counter_id)
+{
+	memset(&block->stats[counter_id - block->offset], 0,
+	       sizeof(*block->stats));
+}
+
+static struct prestera_counter_block *
+prestera_counter_block_lookup_not_full(struct prestera_counter *counter,
+				       u32 client)
+{
+	u32 i;
+
+	prestera_counter_lock(counter);
+	for (i = 0; i < counter->block_list_len; i++) {
+		if (counter->block_list[i] &&
+		    counter->block_list[i]->client == client &&
+		    !counter->block_list[i]->full &&
+		    prestera_counter_block_incref(counter->block_list[i])) {
+			prestera_counter_unlock(counter);
+			return counter->block_list[i];
+		}
+	}
+	prestera_counter_unlock(counter);
+
+	return NULL;
+}
+
+static int prestera_counter_block_list_add(struct prestera_counter *counter,
+					   struct prestera_counter_block *block)
+{
+	struct prestera_counter_block **arr;
+	u32 i;
+
+	prestera_counter_lock(counter);
+
+	for (i = 0; i < counter->block_list_len; i++) {
+		if (counter->block_list[i])
+			continue;
+
+		counter->block_list[i] = block;
+		prestera_counter_unlock(counter);
+		return 0;
+	}
+
+	arr = krealloc(counter->block_list, (counter->block_list_len + 1) *
+		       sizeof(*counter->block_list), GFP_KERNEL);
+	if (!arr) {
+		prestera_counter_unlock(counter);
+		return -ENOMEM;
+	}
+
+	counter->block_list = arr;
+	counter->block_list[counter->block_list_len] = block;
+	counter->block_list_len++;
+	prestera_counter_unlock(counter);
+	return 0;
+}
+
+static struct prestera_counter_block *
+prestera_counter_block_get(struct prestera_counter *counter, u32 client)
+{
+	struct prestera_counter_block *block;
+	int err;
+
+	block = prestera_counter_block_lookup_not_full(counter, client);
+	if (block)
+		return block;
+
+	block = kzalloc_obj(*block);
+	if (!block)
+		return ERR_PTR(-ENOMEM);
+
+	err = prestera_hw_counter_block_get(counter->sw, client,
+					    &block->id, &block->offset,
+					    &block->num_counters);
+	if (err)
+		goto err_block;
+
+	block->stats = kzalloc_objs(*block->stats, block->num_counters);
+	if (!block->stats) {
+		err = -ENOMEM;
+		goto err_stats;
+	}
+
+	block->counter_flag = kcalloc(block->num_counters,
+				      sizeof(*block->counter_flag),
+				      GFP_KERNEL);
+	if (!block->counter_flag) {
+		err = -ENOMEM;
+		goto err_flag;
+	}
+
+	block->client = client;
+	mutex_init(&block->mtx);
+	refcount_set(&block->refcnt, 1);
+	idr_init_base(&block->counter_idr, block->offset);
+
+	err = prestera_counter_block_list_add(counter, block);
+	if (err)
+		goto err_list_add;
+
+	return block;
+
+err_list_add:
+	idr_destroy(&block->counter_idr);
+	mutex_destroy(&block->mtx);
+	kfree(block->counter_flag);
+err_flag:
+	kfree(block->stats);
+err_stats:
+	prestera_hw_counter_block_release(counter->sw, block->id);
+err_block:
+	kfree(block);
+	return ERR_PTR(err);
+}
+
+static void prestera_counter_block_put(struct prestera_counter *counter,
+				       struct prestera_counter_block *block)
+{
+	u32 i;
+
+	if (!prestera_counter_block_decref(block))
+		return;
+
+	prestera_counter_lock(counter);
+	for (i = 0; i < counter->block_list_len; i++) {
+		if (counter->block_list[i] &&
+		    counter->block_list[i]->id == block->id) {
+			counter->block_list[i] = NULL;
+			break;
+		}
+	}
+	prestera_counter_unlock(counter);
+
+	WARN_ON(!idr_is_empty(&block->counter_idr));
+
+	prestera_hw_counter_block_release(counter->sw, block->id);
+	idr_destroy(&block->counter_idr);
+	mutex_destroy(&block->mtx);
+	kfree(block->stats);
+	kfree(block);
+}
+
+static int prestera_counter_get_vacant(struct prestera_counter_block *block,
+				       u32 *id)
+{
+	int free_id;
+
+	if (block->full)
+		return -ENOSPC;
+
+	prestera_counter_block_lock(block);
+	free_id = idr_alloc_cyclic(&block->counter_idr, NULL, block->offset,
+				   block->offset + block->num_counters,
+				   GFP_KERNEL);
+	if (free_id < 0) {
+		if (free_id == -ENOSPC)
+			block->full = true;
+
+		prestera_counter_block_unlock(block);
+		return free_id;
+	}
+	*id = free_id;
+	prestera_counter_block_unlock(block);
+
+	return 0;
+}
+
+int prestera_counter_get(struct prestera_counter *counter, u32 client,
+			 struct prestera_counter_block **bl, u32 *counter_id)
+{
+	struct prestera_counter_block *block;
+	int err;
+	u32 id;
+
+get_next_block:
+	block = prestera_counter_block_get(counter, client);
+	if (IS_ERR(block))
+		return PTR_ERR(block);
+
+	err = prestera_counter_get_vacant(block, &id);
+	if (err) {
+		prestera_counter_block_put(counter, block);
+
+		if (err == -ENOSPC)
+			goto get_next_block;
+
+		return err;
+	}
+
+	prestera_counter_block_lock(block);
+	if (block->is_updating)
+		block->counter_flag[id - block->offset] = COUNTER_FLAG_INVALID;
+	prestera_counter_block_unlock(block);
+
+	*counter_id = id;
+	*bl = block;
+
+	return 0;
+}
+
+void prestera_counter_put(struct prestera_counter *counter,
+			  struct prestera_counter_block *block, u32 counter_id)
+{
+	if (!block)
+		return;
+
+	prestera_counter_block_lock(block);
+	idr_remove(&block->counter_idr, counter_id);
+	block->full = false;
+	prestera_counter_stats_clear(block, counter_id);
+	prestera_counter_block_unlock(block);
+
+	prestera_hw_counter_clear(counter->sw, block->id, counter_id);
+	prestera_counter_block_put(counter, block);
+}
+
+static u32 prestera_counter_block_idx_next(struct prestera_counter *counter,
+					   u32 curr_idx)
+{
+	u32 idx, i, start = curr_idx + 1;
+
+	prestera_counter_lock(counter);
+	for (i = 0; i < counter->block_list_len; i++) {
+		idx = (start + i) % counter->block_list_len;
+		if (!counter->block_list[idx])
+			continue;
+
+		prestera_counter_unlock(counter);
+		return idx;
+	}
+	prestera_counter_unlock(counter);
+
+	return 0;
+}
+
+static struct prestera_counter_block *
+prestera_counter_block_get_by_idx(struct prestera_counter *counter, u32 idx)
+{
+	if (idx >= counter->block_list_len)
+		return NULL;
+
+	prestera_counter_lock(counter);
+
+	if (!counter->block_list[idx] ||
+	    !prestera_counter_block_incref(counter->block_list[idx])) {
+		prestera_counter_unlock(counter);
+		return NULL;
+	}
+
+	prestera_counter_unlock(counter);
+	return counter->block_list[idx];
+}
+
+static void prestera_counter_stats_work(struct work_struct *work)
+{
+	struct delayed_work *dl_work = to_delayed_work(work);
+	struct prestera_counter *counter =
+		container_of(dl_work, struct prestera_counter, stats_dw);
+	struct prestera_counter_block *block;
+	u32 resched_time = COUNTER_POLL_TIME;
+	u32 count = COUNTER_BULK_SIZE;
+	bool done = false;
+	int err;
+	u32 i;
+
+	block = prestera_counter_block_get_by_idx(counter, counter->curr_idx);
+	if (!block) {
+		if (counter->is_fetching)
+			goto abort;
+
+		goto next;
+	}
+
+	if (!counter->is_fetching) {
+		err = prestera_hw_counter_trigger(counter->sw, block->id);
+		if (err)
+			goto abort;
+
+		prestera_counter_block_lock(block);
+		block->is_updating = true;
+		prestera_counter_block_unlock(block);
+
+		counter->is_fetching = true;
+		counter->total_read = 0;
+		resched_time = COUNTER_RESCHED_TIME;
+		goto resched;
+	}
+
+	prestera_counter_block_lock(block);
+	err = prestera_hw_counters_get(counter->sw, counter->total_read,
+				       &count, &done,
+				       &block->stats[counter->total_read]);
+	prestera_counter_block_unlock(block);
+	if (err)
+		goto abort;
+
+	counter->total_read += count;
+	if (!done || counter->total_read < block->num_counters) {
+		resched_time = COUNTER_RESCHED_TIME;
+		goto resched;
+	}
+
+	for (i = 0; i < block->num_counters; i++) {
+		if (block->counter_flag[i] == COUNTER_FLAG_INVALID) {
+			prestera_counter_block_lock(block);
+			block->counter_flag[i] = COUNTER_FLAG_READY;
+			memset(&block->stats[i], 0, sizeof(*block->stats));
+			prestera_counter_block_unlock(block);
+		}
+	}
+
+	prestera_counter_block_lock(block);
+	block->is_updating = false;
+	prestera_counter_block_unlock(block);
+
+	goto next;
+abort:
+	prestera_hw_counter_abort(counter->sw);
+next:
+	counter->is_fetching = false;
+	counter->curr_idx =
+		prestera_counter_block_idx_next(counter, counter->curr_idx);
+resched:
+	if (block)
+		prestera_counter_block_put(counter, block);
+
+	schedule_delayed_work(&counter->stats_dw, resched_time);
+}
+
+/* Can be executed without rtnl_lock().
+ * So pay attention when something changing.
+ */
+int prestera_counter_stats_get(struct prestera_counter *counter,
+			       struct prestera_counter_block *block,
+			       u32 counter_id, u64 *packets, u64 *bytes)
+{
+	if (!block || !prestera_counter_is_ready(block, counter_id)) {
+		*packets = 0;
+		*bytes = 0;
+		return 0;
+	}
+
+	prestera_counter_block_lock(block);
+	*packets = block->stats[counter_id - block->offset].packets;
+	*bytes = block->stats[counter_id - block->offset].bytes;
+
+	prestera_counter_stats_clear(block, counter_id);
+	prestera_counter_block_unlock(block);
+
+	return 0;
+}
+
+int prestera_counter_init(struct prestera_switch *sw)
+{
+	struct prestera_counter *counter;
+
+	counter = kzalloc_obj(*counter);
+	if (!counter)
+		return -ENOMEM;
+
+	counter->block_list = kzalloc_obj(*counter->block_list);
+	if (!counter->block_list) {
+		kfree(counter);
+		return -ENOMEM;
+	}
+
+	mutex_init(&counter->mtx);
+	counter->block_list_len = 1;
+	counter->sw = sw;
+	sw->counter = counter;
+
+	INIT_DELAYED_WORK(&counter->stats_dw, prestera_counter_stats_work);
+	schedule_delayed_work(&counter->stats_dw, COUNTER_POLL_TIME);
+
+	return 0;
+}
+
+void prestera_counter_fini(struct prestera_switch *sw)
+{
+	struct prestera_counter *counter = sw->counter;
+	u32 i;
+
+	cancel_delayed_work_sync(&counter->stats_dw);
+
+	for (i = 0; i < counter->block_list_len; i++)
+		WARN_ON(counter->block_list[i]);
+
+	mutex_destroy(&counter->mtx);
+	kfree(counter->block_list);
+	kfree(counter);
+}

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_counter.h

@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */
+/* Copyright (c) 2021 Marvell International Ltd. All rights reserved. */
+
+#ifndef _PRESTERA_COUNTER_H_
+#define _PRESTERA_COUNTER_H_
+
+#include <linux/types.h>
+
+struct prestera_counter_stats {
+	u64 packets;
+	u64 bytes;
+};
+
+struct prestera_switch;
+struct prestera_counter;
+struct prestera_counter_block;
+
+int prestera_counter_init(struct prestera_switch *sw);
+void prestera_counter_fini(struct prestera_switch *sw);
+
+int prestera_counter_get(struct prestera_counter *counter, u32 client,
+			 struct prestera_counter_block **block,
+			 u32 *counter_id);
+void prestera_counter_put(struct prestera_counter *counter,
+			  struct prestera_counter_block *block, u32 counter_id);
+int prestera_counter_stats_get(struct prestera_counter *counter,
+			       struct prestera_counter_block *block,
+			       u32 counter_id, u64 *packets, u64 *bytes);
+
+#endif /* _PRESTERA_COUNTER_H_ */

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_devlink.c

@@ -0,0 +1,599 @@
+// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
+/* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved */
+
+#include <net/devlink.h>
+
+#include "prestera_devlink.h"
+#include "prestera_hw.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,
+	DEVLINK_PRESTERA_TRAP_ID_MET_RED,
+	DEVLINK_PRESTERA_TRAP_ID_IP_SIP_IS_ZERO,
+	DEVLINK_PRESTERA_TRAP_ID_IP_UC_DIP_DA_MISMATCH,
+	DEVLINK_PRESTERA_TRAP_ID_ILLEGAL_IPV4_HDR,
+	DEVLINK_PRESTERA_TRAP_ID_ILLEGAL_IP_ADDR,
+	DEVLINK_PRESTERA_TRAP_ID_INVALID_SA,
+	DEVLINK_PRESTERA_TRAP_ID_LOCAL_PORT,
+	DEVLINK_PRESTERA_TRAP_ID_PORT_NO_VLAN,
+	DEVLINK_PRESTERA_TRAP_ID_RXDMA_DROP,
+};
+
+#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"
+#define DEVLINK_PRESTERA_TRAP_NAME_RXDMA_DROP \
+	"rxdma_drop"
+#define DEVLINK_PRESTERA_TRAP_NAME_PORT_NO_VLAN \
+	"port_no_vlan"
+#define DEVLINK_PRESTERA_TRAP_NAME_LOCAL_PORT \
+	"local_port"
+#define DEVLINK_PRESTERA_TRAP_NAME_INVALID_SA \
+	"invalid_sa"
+#define DEVLINK_PRESTERA_TRAP_NAME_ILLEGAL_IP_ADDR \
+	"illegal_ip_addr"
+#define DEVLINK_PRESTERA_TRAP_NAME_ILLEGAL_IPV4_HDR \
+	"illegal_ipv4_hdr"
+#define DEVLINK_PRESTERA_TRAP_NAME_IP_UC_DIP_DA_MISMATCH \
+	"ip_uc_dip_da_mismatch"
+#define DEVLINK_PRESTERA_TRAP_NAME_IP_SIP_IS_ZERO \
+	"ip_sip_is_zero"
+#define DEVLINK_PRESTERA_TRAP_NAME_MET_RED \
+	"met_red"
+
+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)
+
+#define PRESTERA_TRAP_DRIVER_DROP(_id, _group_id)			      \
+	DEVLINK_TRAP_DRIVER(DROP, DROP, 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),
+	DEVLINK_TRAP_GROUP_GENERIC(BUFFER_DROPS, 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,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_DROP(RXDMA_DROP, BUFFER_DROPS),
+		.cpu_code = 37,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_DROP(PORT_NO_VLAN, L2_DROPS),
+		.cpu_code = 39,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_DROP(LOCAL_PORT, L2_DROPS),
+		.cpu_code = 56,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_DROP(INVALID_SA, L2_DROPS),
+		.cpu_code = 60,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_DROP(ILLEGAL_IP_ADDR, L3_DROPS),
+		.cpu_code = 136,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_DROP(ILLEGAL_IPV4_HDR, L3_DROPS),
+		.cpu_code = 137,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_DROP(IP_UC_DIP_DA_MISMATCH,
+						  L3_DROPS),
+		.cpu_code = 138,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_DROP(IP_SIP_IS_ZERO, L3_DROPS),
+		.cpu_code = 145,
+	},
+	{
+		.trap = PRESTERA_TRAP_DRIVER_DROP(MET_RED, BUFFER_DROPS),
+		.cpu_code = 185,
+	},
+};
+
+static int prestera_drop_counter_get(struct devlink *devlink,
+				     const struct devlink_trap *trap,
+				     u64 *p_drops);
+
+static int prestera_dl_info_get(struct devlink *dl,
+				struct devlink_info_req *req,
+				struct netlink_ext_ack *extack)
+{
+	struct prestera_switch *sw = devlink_priv(dl);
+	char buf[16];
+
+	snprintf(buf, sizeof(buf), "%d.%d.%d",
+		 sw->dev->fw_rev.maj,
+		 sw->dev->fw_rev.min,
+		 sw->dev->fw_rev.sub);
+
+	return devlink_info_version_running_put(req,
+					       DEVLINK_INFO_VERSION_GENERIC_FW,
+					       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 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,
+	.trap_drop_counter_get = prestera_drop_counter_get,
+};
+
+struct prestera_switch *prestera_devlink_alloc(struct prestera_device *dev)
+{
+	struct devlink *dl;
+
+	dl = devlink_alloc(&prestera_dl_ops, sizeof(struct prestera_switch),
+			   dev->dev);
+	if (!dl)
+		return NULL;
+
+	return devlink_priv(dl);
+}
+
+void prestera_devlink_free(struct prestera_switch *sw)
+{
+	struct devlink *dl = priv_to_devlink(sw);
+
+	devlink_free(dl);
+}
+
+void prestera_devlink_register(struct prestera_switch *sw)
+{
+	struct devlink *dl = priv_to_devlink(sw);
+
+	devlink_register(dl);
+}
+
+void prestera_devlink_unregister(struct prestera_switch *sw)
+{
+	struct devlink *dl = priv_to_devlink(sw);
+
+	devlink_unregister(dl);
+}
+
+int prestera_devlink_port_register(struct prestera_port *port)
+{
+	struct prestera_switch *sw = port->sw;
+	struct devlink *dl = priv_to_devlink(sw);
+	struct devlink_port_attrs attrs = {};
+	int err;
+
+	attrs.flavour = DEVLINK_PORT_FLAVOUR_PHYSICAL;
+	attrs.phys.port_number = port->fp_id;
+	attrs.switch_id.id_len = sizeof(sw->id);
+	memcpy(attrs.switch_id.id, &sw->id, attrs.switch_id.id_len);
+
+	devlink_port_attrs_set(&port->dl_port, &attrs);
+
+	err = devlink_port_register(dl, &port->dl_port, port->fp_id);
+	if (err) {
+		dev_err(prestera_dev(sw), "devlink_port_register failed: %d\n", err);
+		return err;
+	}
+
+	return 0;
+}
+
+void prestera_devlink_port_unregister(struct prestera_port *port)
+{
+	devlink_port_unregister(&port->dl_port);
+}
+
+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_obj(*trap_data);
+	if (!trap_data)
+		return -ENOMEM;
+
+	trap_data->trap_items_arr = kzalloc_objs(struct prestera_trap_item,
+						 traps_count);
+	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);
+	}
+	devlink_trap_groups_unregister(devlink, prestera_trap_groups_arr,
+				       groups_count);
+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 int prestera_drop_counter_get(struct devlink *devlink,
+				     const struct devlink_trap *trap,
+				     u64 *p_drops)
+{
+	struct prestera_switch *sw = devlink_priv(devlink);
+	enum prestera_hw_cpu_code_cnt_t cpu_code_type =
+		PRESTERA_HW_CPU_CODE_CNT_TYPE_DROP;
+	struct prestera_trap *prestera_trap =
+		container_of(trap, struct prestera_trap, trap);
+
+	return prestera_hw_cpu_code_counters_get(sw, prestera_trap->cpu_code,
+						 cpu_code_type, p_drops);
+}
+
+void prestera_devlink_traps_unregister(struct prestera_switch *sw)
+{
+	struct prestera_trap_data *trap_data = sw->trap_data;
+	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));
+	kfree(trap_data->trap_items_arr);
+	kfree(trap_data);
+}

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_devlink.h

@@ -0,0 +1,23 @@
+/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */
+/* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved. */
+
+#ifndef _PRESTERA_DEVLINK_H_
+#define _PRESTERA_DEVLINK_H_
+
+#include "prestera.h"
+
+struct prestera_switch *prestera_devlink_alloc(struct prestera_device *dev);
+void prestera_devlink_free(struct prestera_switch *sw);
+
+void prestera_devlink_register(struct prestera_switch *sw);
+void prestera_devlink_unregister(struct prestera_switch *sw);
+
+int prestera_devlink_port_register(struct prestera_port *port);
+void prestera_devlink_port_unregister(struct prestera_port *port);
+
+void prestera_devlink_trap_report(struct prestera_port *port,
+				  struct sk_buff *skb, u8 cpu_code);
+int prestera_devlink_traps_register(struct prestera_switch *sw);
+void prestera_devlink_traps_unregister(struct prestera_switch *sw);
+
+#endif /* _PRESTERA_DEVLINK_H_ */

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_dsa.c

@@ -0,0 +1,107 @@
+// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
+/* Copyright (c) 2020 Marvell International Ltd. All rights reserved */
+
+#include <linux/bitfield.h>
+#include <linux/bitops.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+
+#include "prestera_dsa.h"
+
+#define PRESTERA_DSA_W0_CMD		GENMASK(31, 30)
+#define PRESTERA_DSA_W0_IS_TAGGED	BIT(29)
+#define PRESTERA_DSA_W0_DEV_NUM		GENMASK(28, 24)
+#define PRESTERA_DSA_W0_PORT_NUM	GENMASK(23, 19)
+#define PRESTERA_DSA_W0_VPT		GENMASK(15, 13)
+#define PRESTERA_DSA_W0_EXT_BIT		BIT(12)
+#define PRESTERA_DSA_W0_VID		GENMASK(11, 0)
+
+#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)
+
+#define PRESTERA_DSA_W3_VID		GENMASK(30, 27)
+#define PRESTERA_DSA_W3_DST_EPORT	GENMASK(23, 7)
+#define PRESTERA_DSA_W3_DEV_NUM		GENMASK(6, 0)
+
+#define PRESTERA_DSA_VID		GENMASK(15, 12)
+#define PRESTERA_DSA_DEV_NUM		GENMASK(11, 5)
+
+int prestera_dsa_parse(struct prestera_dsa *dsa, const u8 *dsa_buf)
+{
+	__be32 *dsa_words = (__be32 *)dsa_buf;
+	enum prestera_dsa_cmd cmd;
+	u32 words[4];
+	u32 field;
+
+	words[0] = ntohl(dsa_words[0]);
+	words[1] = ntohl(dsa_words[1]);
+	words[2] = ntohl(dsa_words[2]);
+	words[3] = ntohl(dsa_words[3]);
+
+	/* set the common parameters */
+	cmd = (enum prestera_dsa_cmd)FIELD_GET(PRESTERA_DSA_W0_CMD, words[0]);
+
+	/* only to CPU is supported */
+	if (unlikely(cmd != PRESTERA_DSA_CMD_TO_CPU))
+		return -EINVAL;
+
+	if (FIELD_GET(PRESTERA_DSA_W0_EXT_BIT, words[0]) == 0)
+		return -EINVAL;
+	if (FIELD_GET(PRESTERA_DSA_W1_EXT_BIT, words[1]) == 0)
+		return -EINVAL;
+	if (FIELD_GET(PRESTERA_DSA_W2_EXT_BIT, words[2]) == 0)
+		return -EINVAL;
+
+	field = FIELD_GET(PRESTERA_DSA_W3_VID, words[3]);
+
+	dsa->vlan.is_tagged = FIELD_GET(PRESTERA_DSA_W0_IS_TAGGED, words[0]);
+	dsa->vlan.cfi_bit = FIELD_GET(PRESTERA_DSA_W1_CFI_BIT, words[1]);
+	dsa->vlan.vpt = FIELD_GET(PRESTERA_DSA_W0_VPT, words[0]);
+	dsa->vlan.vid = FIELD_GET(PRESTERA_DSA_W0_VID, words[0]);
+	dsa->vlan.vid &= ~PRESTERA_DSA_VID;
+	dsa->vlan.vid |= FIELD_PREP(PRESTERA_DSA_VID, field);
+
+	field = FIELD_GET(PRESTERA_DSA_W3_DEV_NUM, words[3]);
+
+	dsa->hw_dev_num = FIELD_GET(PRESTERA_DSA_W0_DEV_NUM, words[0]);
+	dsa->hw_dev_num |= FIELD_PREP(PRESTERA_DSA_DEV_NUM, field);
+
+	dsa->port_num = (FIELD_GET(PRESTERA_DSA_W0_PORT_NUM, words[0]) << 0) |
+			(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;
+}
+
+int prestera_dsa_build(const struct prestera_dsa *dsa, u8 *dsa_buf)
+{
+	__be32 *dsa_words = (__be32 *)dsa_buf;
+	u32 dev_num = dsa->hw_dev_num;
+	u32 words[4] = { 0 };
+
+	words[0] |= FIELD_PREP(PRESTERA_DSA_W0_CMD, PRESTERA_DSA_CMD_FROM_CPU);
+
+	words[0] |= FIELD_PREP(PRESTERA_DSA_W0_DEV_NUM, dev_num);
+	dev_num = FIELD_GET(PRESTERA_DSA_DEV_NUM, dev_num);
+	words[3] |= FIELD_PREP(PRESTERA_DSA_W3_DEV_NUM, dev_num);
+
+	words[3] |= FIELD_PREP(PRESTERA_DSA_W3_DST_EPORT, dsa->port_num);
+
+	words[0] |= FIELD_PREP(PRESTERA_DSA_W0_EXT_BIT, 1);
+	words[1] |= FIELD_PREP(PRESTERA_DSA_W1_EXT_BIT, 1);
+	words[2] |= FIELD_PREP(PRESTERA_DSA_W2_EXT_BIT, 1);
+
+	dsa_words[0] = htonl(words[0]);
+	dsa_words[1] = htonl(words[1]);
+	dsa_words[2] = htonl(words[2]);
+	dsa_words[3] = htonl(words[3]);
+
+	return 0;
+}

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_dsa.h

@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */
+/* Copyright (c) 2020 Marvell International Ltd. All rights reserved. */
+
+#ifndef __PRESTERA_DSA_H_
+#define __PRESTERA_DSA_H_
+
+#include <linux/types.h>
+
+#define PRESTERA_DSA_HLEN	16
+
+enum prestera_dsa_cmd {
+	/* DSA command is "To CPU" */
+	PRESTERA_DSA_CMD_TO_CPU = 0,
+
+	/* DSA command is "From CPU" */
+	PRESTERA_DSA_CMD_FROM_CPU,
+};
+
+struct prestera_dsa_vlan {
+	u16 vid;
+	u8 vpt;
+	u8 cfi_bit;
+	bool is_tagged;
+};
+
+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);
+int prestera_dsa_build(const struct prestera_dsa *dsa, u8 *dsa_buf);
+
+#endif /* _PRESTERA_DSA_H_ */

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_ethtool.c

@@ -0,0 +1,797 @@
+// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
+/* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved */
+
+#include <linux/ethtool.h>
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
+
+#include "prestera_ethtool.h"
+#include "prestera.h"
+#include "prestera_hw.h"
+
+#define PRESTERA_STATS_CNT \
+	(sizeof(struct prestera_port_stats) / sizeof(u64))
+#define PRESTERA_STATS_IDX(name) \
+	(offsetof(struct prestera_port_stats, name) / sizeof(u64))
+#define PRESTERA_STATS_FIELD(name)	\
+	[PRESTERA_STATS_IDX(name)] = __stringify(name)
+
+static const char driver_kind[] = "prestera";
+
+static const struct prestera_link_mode {
+	enum ethtool_link_mode_bit_indices eth_mode;
+	u32 speed;
+	u64 pr_mask;
+	u8 duplex;
+	u8 port_type;
+} port_link_modes[PRESTERA_LINK_MODE_MAX] = {
+	[PRESTERA_LINK_MODE_10baseT_Half] = {
+		.eth_mode =  ETHTOOL_LINK_MODE_10baseT_Half_BIT,
+		.speed = 10,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_10baseT_Half,
+		.duplex = PRESTERA_PORT_DUPLEX_HALF,
+		.port_type = PRESTERA_PORT_TYPE_TP,
+	},
+	[PRESTERA_LINK_MODE_10baseT_Full] = {
+		.eth_mode =  ETHTOOL_LINK_MODE_10baseT_Full_BIT,
+		.speed = 10,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_10baseT_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_TP,
+	},
+	[PRESTERA_LINK_MODE_100baseT_Half] = {
+		.eth_mode =  ETHTOOL_LINK_MODE_100baseT_Half_BIT,
+		.speed = 100,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_100baseT_Half,
+		.duplex = PRESTERA_PORT_DUPLEX_HALF,
+		.port_type = PRESTERA_PORT_TYPE_TP,
+	},
+	[PRESTERA_LINK_MODE_100baseT_Full] = {
+		.eth_mode =  ETHTOOL_LINK_MODE_100baseT_Full_BIT,
+		.speed = 100,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_100baseT_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_TP,
+	},
+	[PRESTERA_LINK_MODE_1000baseT_Half] = {
+		.eth_mode =  ETHTOOL_LINK_MODE_1000baseT_Half_BIT,
+		.speed = 1000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_1000baseT_Half,
+		.duplex = PRESTERA_PORT_DUPLEX_HALF,
+		.port_type = PRESTERA_PORT_TYPE_TP,
+	},
+	[PRESTERA_LINK_MODE_1000baseT_Full] = {
+		.eth_mode =  ETHTOOL_LINK_MODE_1000baseT_Full_BIT,
+		.speed = 1000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_1000baseT_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_TP,
+	},
+	[PRESTERA_LINK_MODE_1000baseX_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_1000baseX_Full_BIT,
+		.speed = 1000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_1000baseX_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_FIBRE,
+	},
+	[PRESTERA_LINK_MODE_1000baseKX_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_1000baseKX_Full_BIT,
+		.speed = 1000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_1000baseKX_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_TP,
+	},
+	[PRESTERA_LINK_MODE_2500baseX_Full] = {
+		.eth_mode =  ETHTOOL_LINK_MODE_2500baseX_Full_BIT,
+		.speed = 2500,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_2500baseX_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+	},
+	[PRESTERA_LINK_MODE_10GbaseKR_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_10000baseKR_Full_BIT,
+		.speed = 10000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_10GbaseKR_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_TP,
+	},
+	[PRESTERA_LINK_MODE_10GbaseSR_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_10000baseSR_Full_BIT,
+		.speed = 10000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_10GbaseSR_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_FIBRE,
+	},
+	[PRESTERA_LINK_MODE_10GbaseLR_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_10000baseLR_Full_BIT,
+		.speed = 10000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_10GbaseLR_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_FIBRE,
+	},
+	[PRESTERA_LINK_MODE_20GbaseKR2_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_20000baseKR2_Full_BIT,
+		.speed = 20000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_20GbaseKR2_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_TP,
+	},
+	[PRESTERA_LINK_MODE_25GbaseCR_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_25000baseCR_Full_BIT,
+		.speed = 25000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_25GbaseCR_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_DA,
+	},
+	[PRESTERA_LINK_MODE_25GbaseKR_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_25000baseKR_Full_BIT,
+		.speed = 25000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_25GbaseKR_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_TP,
+	},
+	[PRESTERA_LINK_MODE_25GbaseSR_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_25000baseSR_Full_BIT,
+		.speed = 25000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_25GbaseSR_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_FIBRE,
+	},
+	[PRESTERA_LINK_MODE_40GbaseKR4_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_40000baseKR4_Full_BIT,
+		.speed = 40000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_40GbaseKR4_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_TP,
+	},
+	[PRESTERA_LINK_MODE_40GbaseCR4_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_40000baseCR4_Full_BIT,
+		.speed = 40000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_40GbaseCR4_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_DA,
+	},
+	[PRESTERA_LINK_MODE_40GbaseSR4_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_40000baseSR4_Full_BIT,
+		.speed = 40000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_40GbaseSR4_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_FIBRE,
+	},
+	[PRESTERA_LINK_MODE_50GbaseCR2_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_50000baseCR2_Full_BIT,
+		.speed = 50000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_50GbaseCR2_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_DA,
+	},
+	[PRESTERA_LINK_MODE_50GbaseKR2_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_50000baseKR2_Full_BIT,
+		.speed = 50000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_50GbaseKR2_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_TP,
+	},
+	[PRESTERA_LINK_MODE_50GbaseSR2_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_50000baseSR2_Full_BIT,
+		.speed = 50000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_50GbaseSR2_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_FIBRE,
+	},
+	[PRESTERA_LINK_MODE_100GbaseKR4_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_100000baseKR4_Full_BIT,
+		.speed = 100000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_100GbaseKR4_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_TP,
+	},
+	[PRESTERA_LINK_MODE_100GbaseSR4_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_100000baseSR4_Full_BIT,
+		.speed = 100000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_100GbaseSR4_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_FIBRE,
+	},
+	[PRESTERA_LINK_MODE_100GbaseCR4_Full] = {
+		.eth_mode = ETHTOOL_LINK_MODE_100000baseCR4_Full_BIT,
+		.speed = 100000,
+		.pr_mask = 1 << PRESTERA_LINK_MODE_100GbaseCR4_Full,
+		.duplex = PRESTERA_PORT_DUPLEX_FULL,
+		.port_type = PRESTERA_PORT_TYPE_DA,
+	}
+};
+
+static const struct prestera_fec {
+	u32 eth_fec;
+	enum ethtool_link_mode_bit_indices eth_mode;
+	u8 pr_fec;
+} port_fec_caps[PRESTERA_PORT_FEC_MAX] = {
+	[PRESTERA_PORT_FEC_OFF] = {
+		.eth_fec = ETHTOOL_FEC_OFF,
+		.eth_mode = ETHTOOL_LINK_MODE_FEC_NONE_BIT,
+		.pr_fec = 1 << PRESTERA_PORT_FEC_OFF,
+	},
+	[PRESTERA_PORT_FEC_BASER] = {
+		.eth_fec = ETHTOOL_FEC_BASER,
+		.eth_mode = ETHTOOL_LINK_MODE_FEC_BASER_BIT,
+		.pr_fec = 1 << PRESTERA_PORT_FEC_BASER,
+	},
+	[PRESTERA_PORT_FEC_RS] = {
+		.eth_fec = ETHTOOL_FEC_RS,
+		.eth_mode = ETHTOOL_LINK_MODE_FEC_RS_BIT,
+		.pr_fec = 1 << PRESTERA_PORT_FEC_RS,
+	}
+};
+
+static const struct prestera_port_type {
+	enum ethtool_link_mode_bit_indices eth_mode;
+	u8 eth_type;
+} port_types[PRESTERA_PORT_TYPE_MAX] = {
+	[PRESTERA_PORT_TYPE_NONE] = {
+		.eth_mode = __ETHTOOL_LINK_MODE_MASK_NBITS,
+		.eth_type = PORT_NONE,
+	},
+	[PRESTERA_PORT_TYPE_TP] = {
+		.eth_mode = ETHTOOL_LINK_MODE_TP_BIT,
+		.eth_type = PORT_TP,
+	},
+	[PRESTERA_PORT_TYPE_AUI] = {
+		.eth_mode = ETHTOOL_LINK_MODE_AUI_BIT,
+		.eth_type = PORT_AUI,
+	},
+	[PRESTERA_PORT_TYPE_MII] = {
+		.eth_mode = ETHTOOL_LINK_MODE_MII_BIT,
+		.eth_type = PORT_MII,
+	},
+	[PRESTERA_PORT_TYPE_FIBRE] = {
+		.eth_mode = ETHTOOL_LINK_MODE_FIBRE_BIT,
+		.eth_type = PORT_FIBRE,
+	},
+	[PRESTERA_PORT_TYPE_BNC] = {
+		.eth_mode = ETHTOOL_LINK_MODE_BNC_BIT,
+		.eth_type = PORT_BNC,
+	},
+	[PRESTERA_PORT_TYPE_DA] = {
+		.eth_mode = ETHTOOL_LINK_MODE_TP_BIT,
+		.eth_type = PORT_TP,
+	},
+	[PRESTERA_PORT_TYPE_OTHER] = {
+		.eth_mode = __ETHTOOL_LINK_MODE_MASK_NBITS,
+		.eth_type = PORT_OTHER,
+	}
+};
+
+static const char prestera_cnt_name[PRESTERA_STATS_CNT][ETH_GSTRING_LEN] = {
+	PRESTERA_STATS_FIELD(good_octets_received),
+	PRESTERA_STATS_FIELD(bad_octets_received),
+	PRESTERA_STATS_FIELD(mac_trans_error),
+	PRESTERA_STATS_FIELD(broadcast_frames_received),
+	PRESTERA_STATS_FIELD(multicast_frames_received),
+	PRESTERA_STATS_FIELD(frames_64_octets),
+	PRESTERA_STATS_FIELD(frames_65_to_127_octets),
+	PRESTERA_STATS_FIELD(frames_128_to_255_octets),
+	PRESTERA_STATS_FIELD(frames_256_to_511_octets),
+	PRESTERA_STATS_FIELD(frames_512_to_1023_octets),
+	PRESTERA_STATS_FIELD(frames_1024_to_max_octets),
+	PRESTERA_STATS_FIELD(excessive_collision),
+	PRESTERA_STATS_FIELD(multicast_frames_sent),
+	PRESTERA_STATS_FIELD(broadcast_frames_sent),
+	PRESTERA_STATS_FIELD(fc_sent),
+	PRESTERA_STATS_FIELD(fc_received),
+	PRESTERA_STATS_FIELD(buffer_overrun),
+	PRESTERA_STATS_FIELD(undersize),
+	PRESTERA_STATS_FIELD(fragments),
+	PRESTERA_STATS_FIELD(oversize),
+	PRESTERA_STATS_FIELD(jabber),
+	PRESTERA_STATS_FIELD(rx_error_frame_received),
+	PRESTERA_STATS_FIELD(bad_crc),
+	PRESTERA_STATS_FIELD(collisions),
+	PRESTERA_STATS_FIELD(late_collision),
+	PRESTERA_STATS_FIELD(unicast_frames_received),
+	PRESTERA_STATS_FIELD(unicast_frames_sent),
+	PRESTERA_STATS_FIELD(sent_multiple),
+	PRESTERA_STATS_FIELD(sent_deferred),
+	PRESTERA_STATS_FIELD(good_octets_sent),
+};
+
+static void prestera_ethtool_get_drvinfo(struct net_device *dev,
+					 struct ethtool_drvinfo *drvinfo)
+{
+	struct prestera_port *port = netdev_priv(dev);
+	struct prestera_switch *sw = port->sw;
+
+	strscpy(drvinfo->driver, driver_kind, sizeof(drvinfo->driver));
+	strscpy(drvinfo->bus_info, dev_name(prestera_dev(sw)),
+		sizeof(drvinfo->bus_info));
+	snprintf(drvinfo->fw_version, sizeof(drvinfo->fw_version),
+		 "%d.%d.%d",
+		 sw->dev->fw_rev.maj,
+		 sw->dev->fw_rev.min,
+		 sw->dev->fw_rev.sub);
+}
+
+static u8 prestera_port_type_get(struct prestera_port *port)
+{
+	if (port->caps.type < PRESTERA_PORT_TYPE_MAX)
+		return port_types[port->caps.type].eth_type;
+
+	return PORT_OTHER;
+}
+
+static int prestera_port_type_set(const struct ethtool_link_ksettings *ecmd,
+				  struct prestera_port *port)
+{
+	u32 new_mode = PRESTERA_LINK_MODE_MAX;
+	u32 type, mode;
+
+	for (type = 0; type < PRESTERA_PORT_TYPE_MAX; type++) {
+		if (port_types[type].eth_type == ecmd->base.port &&
+		    test_bit(port_types[type].eth_mode,
+			     ecmd->link_modes.supported)) {
+			break;
+		}
+	}
+
+	if (type == port->caps.type)
+		return 0;
+	if (type != port->caps.type && ecmd->base.autoneg == AUTONEG_ENABLE)
+		return -EINVAL;
+	if (type == PRESTERA_PORT_TYPE_MAX)
+		return -EOPNOTSUPP;
+
+	for (mode = 0; mode < PRESTERA_LINK_MODE_MAX; mode++) {
+		if ((port_link_modes[mode].pr_mask &
+		    port->caps.supp_link_modes) &&
+		    type == port_link_modes[mode].port_type) {
+			new_mode = mode;
+		}
+	}
+
+	if (new_mode >= PRESTERA_LINK_MODE_MAX)
+		return -EINVAL;
+
+	port->caps.type = type;
+	port->autoneg = false;
+
+	return 0;
+}
+
+static void prestera_modes_to_eth(unsigned long *eth_modes, u64 link_modes,
+				  u8 fec, u8 type)
+{
+	u32 mode;
+
+	for (mode = 0; mode < PRESTERA_LINK_MODE_MAX; mode++) {
+		if ((port_link_modes[mode].pr_mask & link_modes) == 0)
+			continue;
+
+		if (type != PRESTERA_PORT_TYPE_NONE &&
+		    port_link_modes[mode].port_type != type)
+			continue;
+
+		__set_bit(port_link_modes[mode].eth_mode, eth_modes);
+	}
+
+	for (mode = 0; mode < PRESTERA_PORT_FEC_MAX; mode++) {
+		if ((port_fec_caps[mode].pr_fec & fec) == 0)
+			continue;
+
+		__set_bit(port_fec_caps[mode].eth_mode, eth_modes);
+	}
+}
+
+static void prestera_modes_from_eth(const unsigned long *eth_modes,
+				    u64 *link_modes, u8 *fec, u8 type)
+{
+	u64 adver_modes = 0;
+	u32 fec_modes = 0;
+	u32 mode;
+
+	for (mode = 0; mode < PRESTERA_LINK_MODE_MAX; mode++) {
+		if (!test_bit(port_link_modes[mode].eth_mode, eth_modes))
+			continue;
+
+		if (port_link_modes[mode].port_type != type)
+			continue;
+
+		adver_modes |= port_link_modes[mode].pr_mask;
+	}
+
+	for (mode = 0; mode < PRESTERA_PORT_FEC_MAX; mode++) {
+		if (!test_bit(port_fec_caps[mode].eth_mode, eth_modes))
+			continue;
+
+		fec_modes |= port_fec_caps[mode].pr_fec;
+	}
+
+	*link_modes = adver_modes;
+	*fec = fec_modes;
+}
+
+static void prestera_port_supp_types_get(struct ethtool_link_ksettings *ecmd,
+					 struct prestera_port *port)
+{
+	u32 mode;
+	u8 ptype;
+
+	for (mode = 0; mode < PRESTERA_LINK_MODE_MAX; mode++) {
+		if ((port_link_modes[mode].pr_mask &
+		    port->caps.supp_link_modes) == 0)
+			continue;
+
+		ptype = port_link_modes[mode].port_type;
+		__set_bit(port_types[ptype].eth_mode,
+			  ecmd->link_modes.supported);
+	}
+}
+
+static void prestera_port_remote_cap_get(struct ethtool_link_ksettings *ecmd,
+					 struct prestera_port *port)
+{
+	struct prestera_port_phy_state *state = &port->state_phy;
+	bool asym_pause;
+	bool pause;
+	u64 bitmap;
+	int err;
+
+	err = prestera_hw_port_phy_mode_get(port, NULL, &state->lmode_bmap,
+					    &state->remote_fc.pause,
+					    &state->remote_fc.asym_pause);
+	if (err)
+		netdev_warn(port->dev, "Remote link caps get failed %d",
+			    port->caps.transceiver);
+
+	bitmap = state->lmode_bmap;
+
+	prestera_modes_to_eth(ecmd->link_modes.lp_advertising,
+			      bitmap, 0, PRESTERA_PORT_TYPE_NONE);
+
+	if (!bitmap_empty(ecmd->link_modes.lp_advertising,
+			  __ETHTOOL_LINK_MODE_MASK_NBITS)) {
+		ethtool_link_ksettings_add_link_mode(ecmd,
+						     lp_advertising,
+						     Autoneg);
+	}
+
+	pause = state->remote_fc.pause;
+	asym_pause = state->remote_fc.asym_pause;
+
+	if (pause)
+		ethtool_link_ksettings_add_link_mode(ecmd,
+						     lp_advertising,
+						     Pause);
+	if (asym_pause)
+		ethtool_link_ksettings_add_link_mode(ecmd,
+						     lp_advertising,
+						     Asym_Pause);
+}
+
+static void prestera_port_link_mode_get(struct ethtool_link_ksettings *ecmd,
+					struct prestera_port *port)
+{
+	struct prestera_port_mac_state *state = &port->state_mac;
+	u32 speed;
+	u8 duplex;
+	int err;
+
+	if (!port->state_mac.oper)
+		return;
+
+	if (state->speed == SPEED_UNKNOWN || state->duplex == DUPLEX_UNKNOWN) {
+		err = prestera_hw_port_mac_mode_get(port, NULL, &speed,
+						    &duplex, NULL);
+		if (err) {
+			state->speed = SPEED_UNKNOWN;
+			state->duplex = DUPLEX_UNKNOWN;
+		} else {
+			state->speed = speed;
+			state->duplex = duplex == PRESTERA_PORT_DUPLEX_FULL ?
+					  DUPLEX_FULL : DUPLEX_HALF;
+		}
+	}
+
+	ecmd->base.speed = port->state_mac.speed;
+	ecmd->base.duplex = port->state_mac.duplex;
+}
+
+static void prestera_port_mdix_get(struct ethtool_link_ksettings *ecmd,
+				   struct prestera_port *port)
+{
+	struct prestera_port_phy_state *state = &port->state_phy;
+
+	if (prestera_hw_port_phy_mode_get(port,
+					  &state->mdix, NULL, NULL, NULL)) {
+		netdev_warn(port->dev, "MDIX params get failed");
+		state->mdix = ETH_TP_MDI_INVALID;
+	}
+
+	ecmd->base.eth_tp_mdix = port->state_phy.mdix;
+	ecmd->base.eth_tp_mdix_ctrl = port->cfg_phy.mdix;
+}
+
+static int
+prestera_ethtool_get_link_ksettings(struct net_device *dev,
+				    struct ethtool_link_ksettings *ecmd)
+{
+	struct prestera_port *port = netdev_priv(dev);
+
+	ethtool_link_ksettings_zero_link_mode(ecmd, supported);
+	ethtool_link_ksettings_zero_link_mode(ecmd, advertising);
+	ethtool_link_ksettings_zero_link_mode(ecmd, lp_advertising);
+	ecmd->base.speed = SPEED_UNKNOWN;
+	ecmd->base.duplex = DUPLEX_UNKNOWN;
+
+	if (port->phy_link)
+		return phylink_ethtool_ksettings_get(port->phy_link, ecmd);
+
+	ecmd->base.autoneg = port->autoneg ? AUTONEG_ENABLE : AUTONEG_DISABLE;
+
+	if (port->caps.type == PRESTERA_PORT_TYPE_TP) {
+		ethtool_link_ksettings_add_link_mode(ecmd, supported, Autoneg);
+
+		if (netif_running(dev) &&
+		    (port->autoneg ||
+		     port->caps.transceiver == PRESTERA_PORT_TCVR_COPPER))
+			ethtool_link_ksettings_add_link_mode(ecmd, advertising,
+							     Autoneg);
+	}
+
+	prestera_modes_to_eth(ecmd->link_modes.supported,
+			      port->caps.supp_link_modes,
+			      port->caps.supp_fec,
+			      port->caps.type);
+
+	prestera_port_supp_types_get(ecmd, port);
+
+	if (netif_carrier_ok(dev))
+		prestera_port_link_mode_get(ecmd, port);
+
+	ecmd->base.port = prestera_port_type_get(port);
+
+	if (port->autoneg) {
+		if (netif_running(dev))
+			prestera_modes_to_eth(ecmd->link_modes.advertising,
+					      port->adver_link_modes,
+					      port->adver_fec,
+					      port->caps.type);
+
+		if (netif_carrier_ok(dev) &&
+		    port->caps.transceiver == PRESTERA_PORT_TCVR_COPPER)
+			prestera_port_remote_cap_get(ecmd, port);
+	}
+
+	if (port->caps.type == PRESTERA_PORT_TYPE_TP &&
+	    port->caps.transceiver == PRESTERA_PORT_TCVR_COPPER)
+		prestera_port_mdix_get(ecmd, port);
+
+	return 0;
+}
+
+static int prestera_port_mdix_set(const struct ethtool_link_ksettings *ecmd,
+				  struct prestera_port *port)
+{
+	if (ecmd->base.eth_tp_mdix_ctrl != ETH_TP_MDI_INVALID &&
+	    port->caps.transceiver ==  PRESTERA_PORT_TCVR_COPPER &&
+	    port->caps.type == PRESTERA_PORT_TYPE_TP) {
+		port->cfg_phy.mdix = ecmd->base.eth_tp_mdix_ctrl;
+		return prestera_hw_port_phy_mode_set(port, port->cfg_phy.admin,
+						     port->autoneg,
+						     port->cfg_phy.mode,
+						     port->adver_link_modes,
+						     port->cfg_phy.mdix);
+	}
+	return 0;
+
+}
+
+static int prestera_port_link_mode_set(struct prestera_port *port,
+				       u32 speed, u8 duplex, u8 type)
+{
+	u32 new_mode = PRESTERA_LINK_MODE_MAX;
+	u32 mode;
+	int err;
+
+	for (mode = 0; mode < PRESTERA_LINK_MODE_MAX; mode++) {
+		if (speed != SPEED_UNKNOWN &&
+		    speed != port_link_modes[mode].speed)
+			continue;
+
+		if (duplex != DUPLEX_UNKNOWN &&
+		    duplex != port_link_modes[mode].duplex)
+			continue;
+
+		if (!(port_link_modes[mode].pr_mask &
+		    port->caps.supp_link_modes))
+			continue;
+
+		if (type != port_link_modes[mode].port_type)
+			continue;
+
+		new_mode = mode;
+		break;
+	}
+
+	if (new_mode == PRESTERA_LINK_MODE_MAX)
+		return -EOPNOTSUPP;
+
+	err = prestera_hw_port_phy_mode_set(port, port->cfg_phy.admin,
+					    false, new_mode, 0,
+					    port->cfg_phy.mdix);
+	if (err)
+		return err;
+
+	port->adver_fec = BIT(PRESTERA_PORT_FEC_OFF);
+	port->adver_link_modes = 0;
+	port->cfg_phy.mode = new_mode;
+	port->autoneg = false;
+
+	return 0;
+}
+
+static int
+prestera_port_speed_duplex_set(const struct ethtool_link_ksettings *ecmd,
+			       struct prestera_port *port)
+{
+	u8 duplex = DUPLEX_UNKNOWN;
+
+	if (ecmd->base.duplex != DUPLEX_UNKNOWN)
+		duplex = ecmd->base.duplex == DUPLEX_FULL ?
+			 PRESTERA_PORT_DUPLEX_FULL : PRESTERA_PORT_DUPLEX_HALF;
+
+	return prestera_port_link_mode_set(port, ecmd->base.speed, duplex,
+					   port->caps.type);
+}
+
+static int
+prestera_ethtool_set_link_ksettings(struct net_device *dev,
+				    const struct ethtool_link_ksettings *ecmd)
+{
+	struct prestera_port *port = netdev_priv(dev);
+	u64 adver_modes;
+	u8 adver_fec;
+	int err;
+
+	if (port->phy_link)
+		return phylink_ethtool_ksettings_set(port->phy_link, ecmd);
+
+	err = prestera_port_type_set(ecmd, port);
+	if (err)
+		return err;
+
+	if (port->caps.transceiver == PRESTERA_PORT_TCVR_COPPER) {
+		err = prestera_port_mdix_set(ecmd, port);
+		if (err)
+			return err;
+	}
+
+	prestera_modes_from_eth(ecmd->link_modes.advertising, &adver_modes,
+				&adver_fec, port->caps.type);
+
+	if (ecmd->base.autoneg == AUTONEG_ENABLE)
+		err = prestera_port_autoneg_set(port, adver_modes);
+	else
+		err = prestera_port_speed_duplex_set(ecmd, port);
+
+	return err;
+}
+
+static int prestera_ethtool_get_fecparam(struct net_device *dev,
+					 struct ethtool_fecparam *fecparam)
+{
+	struct prestera_port *port = netdev_priv(dev);
+	u8 active;
+	u32 mode;
+	int err;
+
+	err = prestera_hw_port_mac_mode_get(port, NULL, NULL, NULL, &active);
+	if (err)
+		return err;
+
+	fecparam->fec = 0;
+
+	for (mode = 0; mode < PRESTERA_PORT_FEC_MAX; mode++) {
+		if ((port_fec_caps[mode].pr_fec & port->caps.supp_fec) == 0)
+			continue;
+
+		fecparam->fec |= port_fec_caps[mode].eth_fec;
+	}
+
+	if (active < PRESTERA_PORT_FEC_MAX)
+		fecparam->active_fec = port_fec_caps[active].eth_fec;
+	else
+		fecparam->active_fec = ETHTOOL_FEC_AUTO;
+
+	return 0;
+}
+
+static int prestera_ethtool_set_fecparam(struct net_device *dev,
+					 struct ethtool_fecparam *fecparam)
+{
+	struct prestera_port *port = netdev_priv(dev);
+	struct prestera_port_mac_config cfg_mac;
+	u32 mode;
+	u8 fec;
+
+	if (port->autoneg) {
+		netdev_err(dev, "FEC set is not allowed while autoneg is on\n");
+		return -EINVAL;
+	}
+
+	fec = PRESTERA_PORT_FEC_MAX;
+	for (mode = 0; mode < PRESTERA_PORT_FEC_MAX; mode++) {
+		if ((port_fec_caps[mode].eth_fec & fecparam->fec) &&
+		    (port_fec_caps[mode].pr_fec & port->caps.supp_fec)) {
+			fec = mode;
+			break;
+		}
+	}
+
+	prestera_port_cfg_mac_read(port, &cfg_mac);
+
+	if (fec == cfg_mac.fec)
+		return 0;
+
+	if (fec == PRESTERA_PORT_FEC_MAX) {
+		netdev_err(dev, "Unsupported FEC requested");
+		return -EINVAL;
+	}
+
+	cfg_mac.fec = fec;
+
+	return prestera_port_cfg_mac_write(port, &cfg_mac);
+}
+
+static int prestera_ethtool_get_sset_count(struct net_device *dev, int sset)
+{
+	switch (sset) {
+	case ETH_SS_STATS:
+		return PRESTERA_STATS_CNT;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static void prestera_ethtool_get_strings(struct net_device *dev,
+					 u32 stringset, u8 *data)
+{
+	if (stringset != ETH_SS_STATS)
+		return;
+
+	memcpy(data, prestera_cnt_name, sizeof(prestera_cnt_name));
+}
+
+static void prestera_ethtool_get_stats(struct net_device *dev,
+				       struct ethtool_stats *stats, u64 *data)
+{
+	struct prestera_port *port = netdev_priv(dev);
+	struct prestera_port_stats *port_stats;
+
+	port_stats = &port->cached_hw_stats.stats;
+
+	memcpy(data, port_stats, sizeof(*port_stats));
+}
+
+static int prestera_ethtool_nway_reset(struct net_device *dev)
+{
+	struct prestera_port *port = netdev_priv(dev);
+
+	if (netif_running(dev) &&
+	    port->caps.transceiver == PRESTERA_PORT_TCVR_COPPER &&
+	    port->caps.type == PRESTERA_PORT_TYPE_TP)
+		return prestera_hw_port_autoneg_restart(port);
+
+	return -EINVAL;
+}
+
+const struct ethtool_ops prestera_ethtool_ops = {
+	.get_drvinfo = prestera_ethtool_get_drvinfo,
+	.get_link_ksettings = prestera_ethtool_get_link_ksettings,
+	.set_link_ksettings = prestera_ethtool_set_link_ksettings,
+	.get_fecparam = prestera_ethtool_get_fecparam,
+	.set_fecparam = prestera_ethtool_set_fecparam,
+	.get_sset_count = prestera_ethtool_get_sset_count,
+	.get_strings = prestera_ethtool_get_strings,
+	.get_ethtool_stats = prestera_ethtool_get_stats,
+	.get_link = ethtool_op_get_link,
+	.nway_reset = prestera_ethtool_nway_reset
+};

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_ethtool.h

@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */
+/* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved. */
+
+#ifndef __PRESTERA_ETHTOOL_H_
+#define __PRESTERA_ETHTOOL_H_
+
+#include <linux/ethtool.h>
+
+struct prestera_port_event;
+struct prestera_port;
+
+extern const struct ethtool_ops prestera_ethtool_ops;
+
+#endif /* _PRESTERA_ETHTOOL_H_ */

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_flow.c

@@ -0,0 +1,315 @@
+// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
+/* Copyright (c) 2020 Marvell International Ltd. All rights reserved */
+
+#include <linux/kernel.h>
+#include <linux/list.h>
+
+#include "prestera.h"
+#include "prestera_acl.h"
+#include "prestera_flow.h"
+#include "prestera_flower.h"
+#include "prestera_matchall.h"
+#include "prestera_span.h"
+
+static LIST_HEAD(prestera_block_cb_list);
+
+static int prestera_flow_block_mall_cb(struct prestera_flow_block *block,
+				       struct tc_cls_matchall_offload *f)
+{
+	switch (f->command) {
+	case TC_CLSMATCHALL_REPLACE:
+		return prestera_mall_replace(block, f);
+	case TC_CLSMATCHALL_DESTROY:
+		prestera_mall_destroy(block);
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static int prestera_flow_block_flower_cb(struct prestera_flow_block *block,
+					 struct flow_cls_offload *f)
+{
+	switch (f->command) {
+	case FLOW_CLS_REPLACE:
+		return prestera_flower_replace(block, f);
+	case FLOW_CLS_DESTROY:
+		prestera_flower_destroy(block, f);
+		return 0;
+	case FLOW_CLS_STATS:
+		return prestera_flower_stats(block, f);
+	case FLOW_CLS_TMPLT_CREATE:
+		return prestera_flower_tmplt_create(block, f);
+	case FLOW_CLS_TMPLT_DESTROY:
+		prestera_flower_tmplt_destroy(block, f);
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static int prestera_flow_block_cb(enum tc_setup_type type,
+				  void *type_data, void *cb_priv)
+{
+	struct prestera_flow_block *block = cb_priv;
+
+	switch (type) {
+	case TC_SETUP_CLSFLOWER:
+		return prestera_flow_block_flower_cb(block, type_data);
+	case TC_SETUP_CLSMATCHALL:
+		return prestera_flow_block_mall_cb(block, type_data);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static void prestera_flow_block_destroy(void *cb_priv)
+{
+	struct prestera_flow_block *block = cb_priv;
+
+	prestera_flower_template_cleanup(block);
+
+	WARN_ON(!list_empty(&block->template_list));
+	WARN_ON(!list_empty(&block->binding_list));
+
+	kfree(block);
+}
+
+static struct prestera_flow_block *
+prestera_flow_block_create(struct prestera_switch *sw,
+			   struct net *net,
+			   bool ingress)
+{
+	struct prestera_flow_block *block;
+
+	block = kzalloc_obj(*block);
+	if (!block)
+		return NULL;
+
+	INIT_LIST_HEAD(&block->binding_list);
+	INIT_LIST_HEAD(&block->template_list);
+	block->net = net;
+	block->sw = sw;
+	block->mall.prio_min = UINT_MAX;
+	block->mall.prio_max = 0;
+	block->mall.bound = false;
+	block->ingress = ingress;
+
+	return block;
+}
+
+static void prestera_flow_block_release(void *cb_priv)
+{
+	struct prestera_flow_block *block = cb_priv;
+
+	prestera_flow_block_destroy(block);
+}
+
+static bool
+prestera_flow_block_is_bound(const struct prestera_flow_block *block)
+{
+	return block->ruleset_zero;
+}
+
+static struct prestera_flow_block_binding *
+prestera_flow_block_lookup(struct prestera_flow_block *block,
+			   struct prestera_port *port)
+{
+	struct prestera_flow_block_binding *binding;
+
+	list_for_each_entry(binding, &block->binding_list, list)
+		if (binding->port == port)
+			return binding;
+
+	return NULL;
+}
+
+static int prestera_flow_block_bind(struct prestera_flow_block *block,
+				    struct prestera_port *port)
+{
+	struct prestera_flow_block_binding *binding;
+	int err;
+
+	binding = kzalloc_obj(*binding);
+	if (!binding)
+		return -ENOMEM;
+
+	binding->span_id = PRESTERA_SPAN_INVALID_ID;
+	binding->port = port;
+
+	if (prestera_flow_block_is_bound(block)) {
+		err = prestera_acl_ruleset_bind(block->ruleset_zero, port);
+		if (err)
+			goto err_ruleset_bind;
+	}
+
+	list_add(&binding->list, &block->binding_list);
+	return 0;
+
+err_ruleset_bind:
+	kfree(binding);
+	return err;
+}
+
+static int prestera_flow_block_unbind(struct prestera_flow_block *block,
+				      struct prestera_port *port)
+{
+	struct prestera_flow_block_binding *binding;
+
+	binding = prestera_flow_block_lookup(block, port);
+	if (!binding)
+		return -ENOENT;
+
+	list_del(&binding->list);
+
+	if (prestera_flow_block_is_bound(block))
+		prestera_acl_ruleset_unbind(block->ruleset_zero, port);
+
+	kfree(binding);
+	return 0;
+}
+
+static struct prestera_flow_block *
+prestera_flow_block_get(struct prestera_switch *sw,
+			struct flow_block_offload *f,
+			bool *register_block,
+			bool ingress)
+{
+	struct prestera_flow_block *block;
+	struct flow_block_cb *block_cb;
+
+	block_cb = flow_block_cb_lookup(f->block,
+					prestera_flow_block_cb, sw);
+	if (!block_cb) {
+		block = prestera_flow_block_create(sw, f->net, ingress);
+		if (!block)
+			return ERR_PTR(-ENOMEM);
+
+		block_cb = flow_block_cb_alloc(prestera_flow_block_cb,
+					       sw, block,
+					       prestera_flow_block_release);
+		if (IS_ERR(block_cb)) {
+			prestera_flow_block_destroy(block);
+			return ERR_CAST(block_cb);
+		}
+
+		block->block_cb = block_cb;
+		*register_block = true;
+	} else {
+		block = flow_block_cb_priv(block_cb);
+		*register_block = false;
+	}
+
+	flow_block_cb_incref(block_cb);
+
+	return block;
+}
+
+static void prestera_flow_block_put(struct prestera_flow_block *block)
+{
+	struct flow_block_cb *block_cb = block->block_cb;
+
+	if (flow_block_cb_decref(block_cb))
+		return;
+
+	flow_block_cb_free(block_cb);
+	prestera_flow_block_destroy(block);
+}
+
+static int prestera_setup_flow_block_bind(struct prestera_port *port,
+					  struct flow_block_offload *f, bool ingress)
+{
+	struct prestera_switch *sw = port->sw;
+	struct prestera_flow_block *block;
+	struct flow_block_cb *block_cb;
+	bool register_block;
+	int err;
+
+	block = prestera_flow_block_get(sw, f, &register_block, ingress);
+	if (IS_ERR(block))
+		return PTR_ERR(block);
+
+	block_cb = block->block_cb;
+
+	err = prestera_flow_block_bind(block, port);
+	if (err)
+		goto err_block_bind;
+
+	if (register_block) {
+		flow_block_cb_add(block_cb, f);
+		list_add_tail(&block_cb->driver_list, &prestera_block_cb_list);
+	}
+
+	if (ingress)
+		port->ingress_flow_block = block;
+	else
+		port->egress_flow_block = block;
+
+	return 0;
+
+err_block_bind:
+	prestera_flow_block_put(block);
+
+	return err;
+}
+
+static void prestera_setup_flow_block_unbind(struct prestera_port *port,
+					     struct flow_block_offload *f, bool ingress)
+{
+	struct prestera_switch *sw = port->sw;
+	struct prestera_flow_block *block;
+	struct flow_block_cb *block_cb;
+	int err;
+
+	block_cb = flow_block_cb_lookup(f->block, prestera_flow_block_cb, sw);
+	if (!block_cb)
+		return;
+
+	block = flow_block_cb_priv(block_cb);
+
+	prestera_mall_destroy(block);
+
+	err = prestera_flow_block_unbind(block, port);
+	if (err)
+		goto error;
+
+	if (!flow_block_cb_decref(block_cb)) {
+		flow_block_cb_remove(block_cb, f);
+		list_del(&block_cb->driver_list);
+	}
+error:
+	if (ingress)
+		port->ingress_flow_block = NULL;
+	else
+		port->egress_flow_block = NULL;
+}
+
+static int prestera_setup_flow_block_clsact(struct prestera_port *port,
+					    struct flow_block_offload *f,
+					    bool ingress)
+{
+	f->driver_block_list = &prestera_block_cb_list;
+
+	switch (f->command) {
+	case FLOW_BLOCK_BIND:
+		return prestera_setup_flow_block_bind(port, f, ingress);
+	case FLOW_BLOCK_UNBIND:
+		prestera_setup_flow_block_unbind(port, f, ingress);
+		return 0;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+int prestera_flow_block_setup(struct prestera_port *port,
+			      struct flow_block_offload *f)
+{
+	switch (f->binder_type) {
+	case FLOW_BLOCK_BINDER_TYPE_CLSACT_INGRESS:
+		return prestera_setup_flow_block_clsact(port, f, true);
+	case FLOW_BLOCK_BINDER_TYPE_CLSACT_EGRESS:
+		return prestera_setup_flow_block_clsact(port, f, false);
+	default:
+		return -EOPNOTSUPP;
+	}
+}

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_flow.h

@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */
+/* Copyright (c) 2020 Marvell International Ltd. All rights reserved. */
+
+#ifndef _PRESTERA_FLOW_H_
+#define _PRESTERA_FLOW_H_
+
+#include <net/flow_offload.h>
+
+struct prestera_port;
+struct prestera_switch;
+
+struct prestera_flow_block_binding {
+	struct list_head list;
+	struct prestera_port *port;
+	int span_id;
+};
+
+struct prestera_flow_block {
+	struct list_head binding_list;
+	struct prestera_switch *sw;
+	struct net *net;
+	struct prestera_acl_ruleset *ruleset_zero;
+	struct flow_block_cb *block_cb;
+	struct list_head template_list;
+	struct {
+		u32 prio_min;
+		u32 prio_max;
+		bool bound;
+	} mall;
+	unsigned int rule_count;
+	bool ingress;
+};
+
+int prestera_flow_block_setup(struct prestera_port *port,
+			      struct flow_block_offload *f);
+
+#endif /* _PRESTERA_FLOW_H_ */

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_flower.c

@@ -0,0 +1,585 @@
+// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
+/* Copyright (c) 2020 Marvell International Ltd. All rights reserved */
+
+#include "prestera.h"
+#include "prestera_acl.h"
+#include "prestera_flow.h"
+#include "prestera_flower.h"
+#include "prestera_matchall.h"
+
+struct prestera_flower_template {
+	struct prestera_acl_ruleset *ruleset;
+	struct list_head list;
+	u32 chain_index;
+};
+
+static void
+prestera_flower_template_free(struct prestera_flower_template *template)
+{
+	prestera_acl_ruleset_put(template->ruleset);
+	list_del(&template->list);
+	kfree(template);
+}
+
+void prestera_flower_template_cleanup(struct prestera_flow_block *block)
+{
+	struct prestera_flower_template *template, *tmp;
+
+	/* put the reference to all rulesets kept in tmpl create */
+	list_for_each_entry_safe(template, tmp, &block->template_list, list)
+		prestera_flower_template_free(template);
+}
+
+static int
+prestera_flower_parse_goto_action(struct prestera_flow_block *block,
+				  struct prestera_acl_rule *rule,
+				  u32 chain_index,
+				  const struct flow_action_entry *act)
+{
+	struct prestera_acl_ruleset *ruleset;
+
+	if (act->chain_index <= chain_index)
+		/* we can jump only forward */
+		return -EINVAL;
+
+	if (rule->re_arg.jump.valid)
+		return -EEXIST;
+
+	ruleset = prestera_acl_ruleset_get(block->sw->acl, block,
+					   act->chain_index);
+	if (IS_ERR(ruleset))
+		return PTR_ERR(ruleset);
+
+	rule->re_arg.jump.valid = 1;
+	rule->re_arg.jump.i.index = prestera_acl_ruleset_index_get(ruleset);
+
+	rule->jump_ruleset = ruleset;
+
+	return 0;
+}
+
+static int prestera_flower_parse_actions(struct prestera_flow_block *block,
+					 struct prestera_acl_rule *rule,
+					 struct flow_action *flow_action,
+					 u32 chain_index,
+					 struct netlink_ext_ack *extack)
+{
+	const struct flow_action_entry *act;
+	int err, i;
+
+	/* whole struct (rule->re_arg) must be initialized with 0 */
+	if (!flow_action_has_entries(flow_action))
+		return 0;
+
+	if (!flow_action_mixed_hw_stats_check(flow_action, extack))
+		return -EOPNOTSUPP;
+
+	act = flow_action_first_entry_get(flow_action);
+	if (act->hw_stats & FLOW_ACTION_HW_STATS_DISABLED) {
+		/* Nothing to do */
+	} else if (act->hw_stats & FLOW_ACTION_HW_STATS_DELAYED) {
+		/* setup counter first */
+		rule->re_arg.count.valid = true;
+		err = prestera_acl_chain_to_client(chain_index, block->ingress,
+						   &rule->re_arg.count.client);
+		if (err)
+			return err;
+	} else {
+		NL_SET_ERR_MSG_MOD(extack, "Unsupported action HW stats type");
+		return -EOPNOTSUPP;
+	}
+
+	flow_action_for_each(i, act, flow_action) {
+		switch (act->id) {
+		case FLOW_ACTION_ACCEPT:
+			if (rule->re_arg.accept.valid)
+				return -EEXIST;
+
+			rule->re_arg.accept.valid = 1;
+			break;
+		case FLOW_ACTION_DROP:
+			if (rule->re_arg.drop.valid)
+				return -EEXIST;
+
+			rule->re_arg.drop.valid = 1;
+			break;
+		case FLOW_ACTION_TRAP:
+			if (rule->re_arg.trap.valid)
+				return -EEXIST;
+
+			rule->re_arg.trap.valid = 1;
+			break;
+		case FLOW_ACTION_POLICE:
+			if (rule->re_arg.police.valid)
+				return -EEXIST;
+
+			rule->re_arg.police.valid = 1;
+			rule->re_arg.police.rate =
+				act->police.rate_bytes_ps;
+			rule->re_arg.police.burst = act->police.burst;
+			rule->re_arg.police.ingress = block->ingress;
+			break;
+		case FLOW_ACTION_GOTO:
+			err = prestera_flower_parse_goto_action(block, rule,
+								chain_index,
+								act);
+			if (err)
+				return err;
+			break;
+		default:
+			NL_SET_ERR_MSG_MOD(extack, "Unsupported action");
+			pr_err("Unsupported action\n");
+			return -EOPNOTSUPP;
+		}
+	}
+
+	return 0;
+}
+
+static int prestera_flower_parse_meta(struct prestera_acl_rule *rule,
+				      struct flow_cls_offload *f,
+				      struct prestera_flow_block *block)
+{
+	struct flow_rule *f_rule = flow_cls_offload_flow_rule(f);
+	struct prestera_acl_match *r_match = &rule->re_key.match;
+	struct prestera_port *port;
+	struct net_device *ingress_dev;
+	struct flow_match_meta match;
+	__be16 key, mask;
+
+	flow_rule_match_meta(f_rule, &match);
+
+	if (match.mask->l2_miss) {
+		NL_SET_ERR_MSG_MOD(f->common.extack, "Can't match on \"l2_miss\"");
+		return -EOPNOTSUPP;
+	}
+
+	if (match.mask->ingress_ifindex != 0xFFFFFFFF) {
+		NL_SET_ERR_MSG_MOD(f->common.extack,
+				   "Unsupported ingress ifindex mask");
+		return -EINVAL;
+	}
+
+	ingress_dev = __dev_get_by_index(block->net,
+					 match.key->ingress_ifindex);
+	if (!ingress_dev) {
+		NL_SET_ERR_MSG_MOD(f->common.extack,
+				   "Can't find specified ingress port to match on");
+		return -EINVAL;
+	}
+
+	if (!prestera_netdev_check(ingress_dev)) {
+		NL_SET_ERR_MSG_MOD(f->common.extack,
+				   "Can't match on switchdev ingress port");
+		return -EINVAL;
+	}
+	port = netdev_priv(ingress_dev);
+
+	mask = htons(0x1FFF << 3);
+	key = htons(port->hw_id << 3);
+	rule_match_set(r_match->key, SYS_PORT, key);
+	rule_match_set(r_match->mask, SYS_PORT, mask);
+
+	mask = htons(0x3FF);
+	key = htons(port->dev_id);
+	rule_match_set(r_match->key, SYS_DEV, key);
+	rule_match_set(r_match->mask, SYS_DEV, mask);
+
+	return 0;
+}
+
+static int prestera_flower_parse(struct prestera_flow_block *block,
+				 struct prestera_acl_rule *rule,
+				 struct flow_cls_offload *f)
+{
+	struct flow_rule *f_rule = flow_cls_offload_flow_rule(f);
+	struct flow_dissector *dissector = f_rule->match.dissector;
+	struct prestera_acl_match *r_match = &rule->re_key.match;
+	__be16 n_proto_mask = 0;
+	__be16 n_proto_key = 0;
+	u16 addr_type = 0;
+	u8 ip_proto = 0;
+	int err;
+
+	if (dissector->used_keys &
+	    ~(BIT_ULL(FLOW_DISSECTOR_KEY_META) |
+	      BIT_ULL(FLOW_DISSECTOR_KEY_CONTROL) |
+	      BIT_ULL(FLOW_DISSECTOR_KEY_BASIC) |
+	      BIT_ULL(FLOW_DISSECTOR_KEY_ETH_ADDRS) |
+	      BIT_ULL(FLOW_DISSECTOR_KEY_IPV4_ADDRS) |
+	      BIT_ULL(FLOW_DISSECTOR_KEY_IPV6_ADDRS) |
+	      BIT_ULL(FLOW_DISSECTOR_KEY_ICMP) |
+	      BIT_ULL(FLOW_DISSECTOR_KEY_PORTS) |
+	      BIT_ULL(FLOW_DISSECTOR_KEY_PORTS_RANGE) |
+	      BIT_ULL(FLOW_DISSECTOR_KEY_VLAN))) {
+		NL_SET_ERR_MSG_MOD(f->common.extack, "Unsupported key");
+		return -EOPNOTSUPP;
+	}
+
+	prestera_acl_rule_priority_set(rule, f->common.prio);
+
+	if (flow_rule_match_key(f_rule, FLOW_DISSECTOR_KEY_META)) {
+		err = prestera_flower_parse_meta(rule, f, block);
+		if (err)
+			return err;
+	}
+
+	if (flow_rule_match_key(f_rule, FLOW_DISSECTOR_KEY_CONTROL)) {
+		struct flow_match_control match;
+
+		flow_rule_match_control(f_rule, &match);
+		addr_type = match.key->addr_type;
+
+		if (flow_rule_has_control_flags(match.mask->flags,
+						f->common.extack))
+			return -EOPNOTSUPP;
+	}
+
+	if (flow_rule_match_key(f_rule, FLOW_DISSECTOR_KEY_BASIC)) {
+		struct flow_match_basic match;
+
+		flow_rule_match_basic(f_rule, &match);
+		n_proto_key = match.key->n_proto;
+		n_proto_mask = match.mask->n_proto;
+
+		if (ntohs(match.key->n_proto) == ETH_P_ALL) {
+			n_proto_key = 0;
+			n_proto_mask = 0;
+		}
+
+		rule_match_set(r_match->key, ETH_TYPE, n_proto_key);
+		rule_match_set(r_match->mask, ETH_TYPE, n_proto_mask);
+
+		rule_match_set(r_match->key, IP_PROTO, match.key->ip_proto);
+		rule_match_set(r_match->mask, IP_PROTO, match.mask->ip_proto);
+		ip_proto = match.key->ip_proto;
+	}
+
+	if (flow_rule_match_key(f_rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
+		struct flow_match_eth_addrs match;
+
+		flow_rule_match_eth_addrs(f_rule, &match);
+
+		/* DA key, mask */
+		rule_match_set_n(r_match->key,
+				 ETH_DMAC_0, &match.key->dst[0], 4);
+		rule_match_set_n(r_match->key,
+				 ETH_DMAC_1, &match.key->dst[4], 2);
+
+		rule_match_set_n(r_match->mask,
+				 ETH_DMAC_0, &match.mask->dst[0], 4);
+		rule_match_set_n(r_match->mask,
+				 ETH_DMAC_1, &match.mask->dst[4], 2);
+
+		/* SA key, mask */
+		rule_match_set_n(r_match->key,
+				 ETH_SMAC_0, &match.key->src[0], 4);
+		rule_match_set_n(r_match->key,
+				 ETH_SMAC_1, &match.key->src[4], 2);
+
+		rule_match_set_n(r_match->mask,
+				 ETH_SMAC_0, &match.mask->src[0], 4);
+		rule_match_set_n(r_match->mask,
+				 ETH_SMAC_1, &match.mask->src[4], 2);
+	}
+
+	if (addr_type == FLOW_DISSECTOR_KEY_IPV4_ADDRS) {
+		struct flow_match_ipv4_addrs match;
+
+		flow_rule_match_ipv4_addrs(f_rule, &match);
+
+		rule_match_set(r_match->key, IP_SRC, match.key->src);
+		rule_match_set(r_match->mask, IP_SRC, match.mask->src);
+
+		rule_match_set(r_match->key, IP_DST, match.key->dst);
+		rule_match_set(r_match->mask, IP_DST, match.mask->dst);
+	}
+
+	if (flow_rule_match_key(f_rule, FLOW_DISSECTOR_KEY_PORTS)) {
+		struct flow_match_ports match;
+
+		if (ip_proto != IPPROTO_TCP && ip_proto != IPPROTO_UDP) {
+			NL_SET_ERR_MSG_MOD
+			    (f->common.extack,
+			     "Only UDP and TCP keys are supported");
+			return -EINVAL;
+		}
+
+		flow_rule_match_ports(f_rule, &match);
+
+		rule_match_set(r_match->key, L4_PORT_SRC, match.key->src);
+		rule_match_set(r_match->mask, L4_PORT_SRC, match.mask->src);
+
+		rule_match_set(r_match->key, L4_PORT_DST, match.key->dst);
+		rule_match_set(r_match->mask, L4_PORT_DST, match.mask->dst);
+	}
+
+	if (flow_rule_match_key(f_rule, FLOW_DISSECTOR_KEY_PORTS_RANGE)) {
+		struct flow_match_ports_range match;
+		__be32 tp_key, tp_mask;
+
+		flow_rule_match_ports_range(f_rule, &match);
+
+		/* src port range (min, max) */
+		tp_key = htonl(ntohs(match.key->tp_min.src) |
+			       (ntohs(match.key->tp_max.src) << 16));
+		tp_mask = htonl(ntohs(match.mask->tp_min.src) |
+				(ntohs(match.mask->tp_max.src) << 16));
+		rule_match_set(r_match->key, L4_PORT_RANGE_SRC, tp_key);
+		rule_match_set(r_match->mask, L4_PORT_RANGE_SRC, tp_mask);
+
+		/* dst port range (min, max) */
+		tp_key = htonl(ntohs(match.key->tp_min.dst) |
+			       (ntohs(match.key->tp_max.dst) << 16));
+		tp_mask = htonl(ntohs(match.mask->tp_min.dst) |
+				(ntohs(match.mask->tp_max.dst) << 16));
+		rule_match_set(r_match->key, L4_PORT_RANGE_DST, tp_key);
+		rule_match_set(r_match->mask, L4_PORT_RANGE_DST, tp_mask);
+	}
+
+	if (flow_rule_match_key(f_rule, FLOW_DISSECTOR_KEY_VLAN)) {
+		struct flow_match_vlan match;
+
+		flow_rule_match_vlan(f_rule, &match);
+
+		if (match.mask->vlan_id != 0) {
+			__be16 key = cpu_to_be16(match.key->vlan_id);
+			__be16 mask = cpu_to_be16(match.mask->vlan_id);
+
+			rule_match_set(r_match->key, VLAN_ID, key);
+			rule_match_set(r_match->mask, VLAN_ID, mask);
+		}
+
+		rule_match_set(r_match->key, VLAN_TPID, match.key->vlan_tpid);
+		rule_match_set(r_match->mask, VLAN_TPID, match.mask->vlan_tpid);
+	}
+
+	if (flow_rule_match_key(f_rule, FLOW_DISSECTOR_KEY_ICMP)) {
+		struct flow_match_icmp match;
+
+		flow_rule_match_icmp(f_rule, &match);
+
+		rule_match_set(r_match->key, ICMP_TYPE, match.key->type);
+		rule_match_set(r_match->mask, ICMP_TYPE, match.mask->type);
+
+		rule_match_set(r_match->key, ICMP_CODE, match.key->code);
+		rule_match_set(r_match->mask, ICMP_CODE, match.mask->code);
+	}
+
+	return prestera_flower_parse_actions(block, rule, &f->rule->action,
+					     f->common.chain_index,
+					     f->common.extack);
+}
+
+static int prestera_flower_prio_check(struct prestera_flow_block *block,
+				      struct flow_cls_offload *f)
+{
+	u32 mall_prio_min;
+	u32 mall_prio_max;
+	int err;
+
+	err = prestera_mall_prio_get(block, &mall_prio_min, &mall_prio_max);
+	if (err == -ENOENT)
+		/* No matchall filters installed on this chain. */
+		return 0;
+
+	if (err) {
+		NL_SET_ERR_MSG(f->common.extack, "Failed to get matchall priorities");
+		return err;
+	}
+
+	if (f->common.prio <= mall_prio_max && block->ingress) {
+		NL_SET_ERR_MSG(f->common.extack,
+			       "Failed to add in front of existing matchall rules");
+		return -EOPNOTSUPP;
+	}
+	if (f->common.prio >= mall_prio_min && !block->ingress) {
+		NL_SET_ERR_MSG(f->common.extack, "Failed to add behind of existing matchall rules");
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+int prestera_flower_prio_get(struct prestera_flow_block *block, u32 chain_index,
+			     u32 *prio_min, u32 *prio_max)
+{
+	struct prestera_acl_ruleset *ruleset;
+
+	ruleset = prestera_acl_ruleset_lookup(block->sw->acl, block, chain_index);
+	if (IS_ERR(ruleset))
+		return PTR_ERR(ruleset);
+
+	prestera_acl_ruleset_prio_get(ruleset, prio_min, prio_max);
+	return 0;
+}
+
+int prestera_flower_replace(struct prestera_flow_block *block,
+			    struct flow_cls_offload *f)
+{
+	struct prestera_acl_ruleset *ruleset;
+	struct prestera_acl *acl = block->sw->acl;
+	struct prestera_acl_rule *rule;
+	int err;
+
+	err = prestera_flower_prio_check(block, f);
+	if (err)
+		return err;
+
+	ruleset = prestera_acl_ruleset_get(acl, block, f->common.chain_index);
+	if (IS_ERR(ruleset))
+		return PTR_ERR(ruleset);
+
+	/* increments the ruleset reference */
+	rule = prestera_acl_rule_create(ruleset, f->cookie,
+					f->common.chain_index);
+	if (IS_ERR(rule)) {
+		err = PTR_ERR(rule);
+		goto err_rule_create;
+	}
+
+	err = prestera_flower_parse(block, rule, f);
+	if (err)
+		goto err_rule_add;
+
+	if (!prestera_acl_ruleset_is_offload(ruleset)) {
+		err = prestera_acl_ruleset_offload(ruleset);
+		if (err)
+			goto err_ruleset_offload;
+	}
+
+	err = prestera_acl_rule_add(block->sw, rule);
+	if (err)
+		goto err_rule_add;
+
+	prestera_acl_ruleset_put(ruleset);
+	return 0;
+
+err_ruleset_offload:
+err_rule_add:
+	prestera_acl_rule_destroy(rule);
+err_rule_create:
+	prestera_acl_ruleset_put(ruleset);
+	return err;
+}
+
+void prestera_flower_destroy(struct prestera_flow_block *block,
+			     struct flow_cls_offload *f)
+{
+	struct prestera_acl_ruleset *ruleset;
+	struct prestera_acl_rule *rule;
+
+	ruleset = prestera_acl_ruleset_lookup(block->sw->acl, block,
+					      f->common.chain_index);
+	if (IS_ERR(ruleset))
+		return;
+
+	rule = prestera_acl_rule_lookup(ruleset, f->cookie);
+	if (rule) {
+		prestera_acl_rule_del(block->sw, rule);
+		prestera_acl_rule_destroy(rule);
+	}
+	prestera_acl_ruleset_put(ruleset);
+}
+
+int prestera_flower_tmplt_create(struct prestera_flow_block *block,
+				 struct flow_cls_offload *f)
+{
+	struct prestera_flower_template *template;
+	struct prestera_acl_ruleset *ruleset;
+	struct prestera_acl_rule rule;
+	int err;
+
+	memset(&rule, 0, sizeof(rule));
+	err = prestera_flower_parse(block, &rule, f);
+	if (err)
+		return err;
+
+	template = kmalloc_obj(*template);
+	if (!template) {
+		err = -ENOMEM;
+		goto err_malloc;
+	}
+
+	prestera_acl_rule_keymask_pcl_id_set(&rule, 0);
+	ruleset = prestera_acl_ruleset_get(block->sw->acl, block,
+					   f->common.chain_index);
+	if (IS_ERR_OR_NULL(ruleset)) {
+		err = -EINVAL;
+		goto err_ruleset_get;
+	}
+
+	/* preserve keymask/template to this ruleset */
+	err = prestera_acl_ruleset_keymask_set(ruleset, rule.re_key.match.mask);
+	if (err)
+		goto err_ruleset_keymask_set;
+
+	/* skip error, as it is not possible to reject template operation,
+	 * so, keep the reference to the ruleset for rules to be added
+	 * to that ruleset later. In case of offload fail, the ruleset
+	 * will be offloaded again during adding a new rule. Also,
+	 * unlikly possble that ruleset is already offloaded at this staage.
+	 */
+	prestera_acl_ruleset_offload(ruleset);
+
+	/* keep the reference to the ruleset */
+	template->ruleset = ruleset;
+	template->chain_index = f->common.chain_index;
+	list_add_rcu(&template->list, &block->template_list);
+	return 0;
+
+err_ruleset_keymask_set:
+	prestera_acl_ruleset_put(ruleset);
+err_ruleset_get:
+	kfree(template);
+err_malloc:
+	NL_SET_ERR_MSG_MOD(f->common.extack, "Create chain template failed");
+	return err;
+}
+
+void prestera_flower_tmplt_destroy(struct prestera_flow_block *block,
+				   struct flow_cls_offload *f)
+{
+	struct prestera_flower_template *template, *tmp;
+
+	list_for_each_entry_safe(template, tmp, &block->template_list, list)
+		if (template->chain_index == f->common.chain_index) {
+			/* put the reference to the ruleset kept in create */
+			prestera_flower_template_free(template);
+			return;
+		}
+}
+
+int prestera_flower_stats(struct prestera_flow_block *block,
+			  struct flow_cls_offload *f)
+{
+	struct prestera_acl_ruleset *ruleset;
+	struct prestera_acl_rule *rule;
+	u64 packets;
+	u64 lastuse;
+	u64 bytes;
+	int err;
+
+	ruleset = prestera_acl_ruleset_lookup(block->sw->acl, block,
+					      f->common.chain_index);
+	if (IS_ERR(ruleset))
+		return PTR_ERR(ruleset);
+
+	rule = prestera_acl_rule_lookup(ruleset, f->cookie);
+	if (!rule) {
+		err = -EINVAL;
+		goto err_rule_get_stats;
+	}
+
+	err = prestera_acl_rule_get_stats(block->sw->acl, rule, &packets,
+					  &bytes, &lastuse);
+	if (err)
+		goto err_rule_get_stats;
+
+	flow_stats_update(&f->stats, bytes, packets, 0, lastuse,
+			  FLOW_ACTION_HW_STATS_DELAYED);
+
+err_rule_get_stats:
+	prestera_acl_ruleset_put(ruleset);
+	return err;
+}

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_flower.h

@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */
+/* Copyright (c) 2020-2021 Marvell International Ltd. All rights reserved. */
+
+#ifndef _PRESTERA_FLOWER_H_
+#define _PRESTERA_FLOWER_H_
+
+#include <net/pkt_cls.h>
+
+struct prestera_flow_block;
+
+int prestera_flower_replace(struct prestera_flow_block *block,
+			    struct flow_cls_offload *f);
+void prestera_flower_destroy(struct prestera_flow_block *block,
+			     struct flow_cls_offload *f);
+int prestera_flower_stats(struct prestera_flow_block *block,
+			  struct flow_cls_offload *f);
+int prestera_flower_tmplt_create(struct prestera_flow_block *block,
+				 struct flow_cls_offload *f);
+void prestera_flower_tmplt_destroy(struct prestera_flow_block *block,
+				   struct flow_cls_offload *f);
+void prestera_flower_template_cleanup(struct prestera_flow_block *block);
+int prestera_flower_prio_get(struct prestera_flow_block *block, u32 chain_index,
+			     u32 *prio_min, u32 *prio_max);
+
+#endif /* _PRESTERA_FLOWER_H_ */

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_hw.h

@@ -0,0 +1,330 @@
+/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */
+/* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved. */
+
+#ifndef _PRESTERA_HW_H_
+#define _PRESTERA_HW_H_
+
+#include <linux/types.h>
+#include "prestera_acl.h"
+
+enum prestera_accept_frm_type {
+	PRESTERA_ACCEPT_FRAME_TYPE_TAGGED,
+	PRESTERA_ACCEPT_FRAME_TYPE_UNTAGGED,
+	PRESTERA_ACCEPT_FRAME_TYPE_ALL,
+};
+
+enum prestera_fdb_flush_mode {
+	PRESTERA_FDB_FLUSH_MODE_DYNAMIC = BIT(0),
+	PRESTERA_FDB_FLUSH_MODE_STATIC = BIT(1),
+	PRESTERA_FDB_FLUSH_MODE_ALL = PRESTERA_FDB_FLUSH_MODE_DYNAMIC
+					| PRESTERA_FDB_FLUSH_MODE_STATIC,
+};
+
+enum {
+	PRESTERA_MAC_MODE_INTERNAL,
+	PRESTERA_MAC_MODE_SGMII,
+	PRESTERA_MAC_MODE_1000BASE_X,
+	PRESTERA_MAC_MODE_KR,
+	PRESTERA_MAC_MODE_KR2,
+	PRESTERA_MAC_MODE_KR4,
+	PRESTERA_MAC_MODE_CR,
+	PRESTERA_MAC_MODE_CR2,
+	PRESTERA_MAC_MODE_CR4,
+	PRESTERA_MAC_MODE_SR_LR,
+	PRESTERA_MAC_MODE_SR_LR2,
+	PRESTERA_MAC_MODE_SR_LR4,
+
+	PRESTERA_MAC_MODE_MAX
+};
+
+enum {
+	PRESTERA_LINK_MODE_10baseT_Half,
+	PRESTERA_LINK_MODE_10baseT_Full,
+	PRESTERA_LINK_MODE_100baseT_Half,
+	PRESTERA_LINK_MODE_100baseT_Full,
+	PRESTERA_LINK_MODE_1000baseT_Half,
+	PRESTERA_LINK_MODE_1000baseT_Full,
+	PRESTERA_LINK_MODE_1000baseX_Full,
+	PRESTERA_LINK_MODE_1000baseKX_Full,
+	PRESTERA_LINK_MODE_2500baseX_Full,
+	PRESTERA_LINK_MODE_10GbaseKR_Full,
+	PRESTERA_LINK_MODE_10GbaseSR_Full,
+	PRESTERA_LINK_MODE_10GbaseLR_Full,
+	PRESTERA_LINK_MODE_20GbaseKR2_Full,
+	PRESTERA_LINK_MODE_25GbaseCR_Full,
+	PRESTERA_LINK_MODE_25GbaseKR_Full,
+	PRESTERA_LINK_MODE_25GbaseSR_Full,
+	PRESTERA_LINK_MODE_40GbaseKR4_Full,
+	PRESTERA_LINK_MODE_40GbaseCR4_Full,
+	PRESTERA_LINK_MODE_40GbaseSR4_Full,
+	PRESTERA_LINK_MODE_50GbaseCR2_Full,
+	PRESTERA_LINK_MODE_50GbaseKR2_Full,
+	PRESTERA_LINK_MODE_50GbaseSR2_Full,
+	PRESTERA_LINK_MODE_100GbaseKR4_Full,
+	PRESTERA_LINK_MODE_100GbaseSR4_Full,
+	PRESTERA_LINK_MODE_100GbaseCR4_Full,
+
+	PRESTERA_LINK_MODE_MAX
+};
+
+enum {
+	PRESTERA_PORT_TYPE_NONE,
+	PRESTERA_PORT_TYPE_TP,
+	PRESTERA_PORT_TYPE_AUI,
+	PRESTERA_PORT_TYPE_MII,
+	PRESTERA_PORT_TYPE_FIBRE,
+	PRESTERA_PORT_TYPE_BNC,
+	PRESTERA_PORT_TYPE_DA,
+	PRESTERA_PORT_TYPE_OTHER,
+
+	PRESTERA_PORT_TYPE_MAX
+};
+
+enum {
+	PRESTERA_PORT_TCVR_COPPER,
+	PRESTERA_PORT_TCVR_SFP,
+
+	PRESTERA_PORT_TCVR_MAX
+};
+
+enum {
+	PRESTERA_PORT_FEC_OFF,
+	PRESTERA_PORT_FEC_BASER,
+	PRESTERA_PORT_FEC_RS,
+
+	PRESTERA_PORT_FEC_MAX
+};
+
+enum {
+	PRESTERA_PORT_DUPLEX_HALF,
+	PRESTERA_PORT_DUPLEX_FULL,
+};
+
+enum {
+	PRESTERA_STP_DISABLED,
+	PRESTERA_STP_BLOCK_LISTEN,
+	PRESTERA_STP_LEARN,
+	PRESTERA_STP_FORWARD,
+};
+
+enum {
+	PRESTERA_POLICER_TYPE_INGRESS,
+	PRESTERA_POLICER_TYPE_EGRESS
+};
+
+enum prestera_hw_cpu_code_cnt_t {
+	PRESTERA_HW_CPU_CODE_CNT_TYPE_DROP = 0,
+	PRESTERA_HW_CPU_CODE_CNT_TYPE_TRAP = 1,
+};
+
+enum prestera_hw_vtcam_direction_t {
+	PRESTERA_HW_VTCAM_DIR_INGRESS = 0,
+	PRESTERA_HW_VTCAM_DIR_EGRESS = 1,
+};
+
+enum {
+	PRESTERA_HW_COUNTER_CLIENT_INGRESS_LOOKUP_0 = 0,
+	PRESTERA_HW_COUNTER_CLIENT_INGRESS_LOOKUP_1 = 1,
+	PRESTERA_HW_COUNTER_CLIENT_INGRESS_LOOKUP_2 = 2,
+	PRESTERA_HW_COUNTER_CLIENT_EGRESS_LOOKUP = 3,
+};
+
+struct prestera_switch;
+struct prestera_port;
+struct prestera_port_stats;
+struct prestera_port_caps;
+enum prestera_event_type;
+struct prestera_event;
+
+typedef void (*prestera_event_cb_t)
+	(struct prestera_switch *sw, struct prestera_event *evt, void *arg);
+
+struct prestera_rxtx_params;
+struct prestera_acl_hw_action_info;
+struct prestera_acl_iface;
+struct prestera_counter_stats;
+struct prestera_iface;
+struct prestera_flood_domain;
+struct prestera_mdb_entry;
+struct prestera_neigh_info;
+
+/* Switch API */
+int prestera_hw_switch_init(struct prestera_switch *sw);
+void prestera_hw_switch_fini(struct prestera_switch *sw);
+int prestera_hw_switch_ageing_set(struct prestera_switch *sw, u32 ageing_ms);
+int prestera_hw_switch_mac_set(struct prestera_switch *sw, const char *mac);
+
+/* Port API */
+int prestera_hw_port_info_get(const struct prestera_port *port,
+			      u32 *dev_id, u32 *hw_id, u16 *fp_id);
+
+int prestera_hw_port_mac_mode_get(const struct prestera_port *port,
+				  u32 *mode, u32 *speed, u8 *duplex, u8 *fec);
+int prestera_hw_port_mac_mode_set(const struct prestera_port *port,
+				  bool admin, u32 mode, u8 inband,
+				  u32 speed, u8 duplex, u8 fec);
+int prestera_hw_port_phy_mode_get(const struct prestera_port *port,
+				  u8 *mdix, u64 *lmode_bmap,
+				  bool *fc_pause, bool *fc_asym);
+int prestera_hw_port_phy_mode_set(const struct prestera_port *port,
+				  bool admin, bool adv, u32 mode, u64 modes,
+				  u8 mdix);
+
+int prestera_hw_port_mtu_set(const struct prestera_port *port, u32 mtu);
+int prestera_hw_port_mtu_get(const struct prestera_port *port, u32 *mtu);
+int prestera_hw_port_mac_set(const struct prestera_port *port, const char *mac);
+int prestera_hw_port_mac_get(const struct prestera_port *port, char *mac);
+int prestera_hw_port_cap_get(const struct prestera_port *port,
+			     struct prestera_port_caps *caps);
+int prestera_hw_port_type_get(const struct prestera_port *port, u8 *type);
+int prestera_hw_port_autoneg_restart(struct prestera_port *port);
+int prestera_hw_port_stats_get(const struct prestera_port *port,
+			       struct prestera_port_stats *stats);
+int prestera_hw_port_speed_get(const struct prestera_port *port, u32 *speed);
+int prestera_hw_port_learning_set(struct prestera_port *port, bool enable);
+int prestera_hw_port_uc_flood_set(const struct prestera_port *port, bool flood);
+int prestera_hw_port_mc_flood_set(const struct prestera_port *port, bool flood);
+int prestera_hw_port_br_locked_set(const struct prestera_port *port,
+				   bool br_locked);
+int prestera_hw_port_accept_frm_type(struct prestera_port *port,
+				     enum prestera_accept_frm_type type);
+/* Vlan API */
+int prestera_hw_vlan_create(struct prestera_switch *sw, u16 vid);
+int prestera_hw_vlan_delete(struct prestera_switch *sw, u16 vid);
+int prestera_hw_vlan_port_set(struct prestera_port *port, u16 vid,
+			      bool is_member, bool untagged);
+int prestera_hw_vlan_port_vid_set(struct prestera_port *port, u16 vid);
+int prestera_hw_vlan_port_stp_set(struct prestera_port *port, u16 vid, u8 state);
+
+/* FDB API */
+int prestera_hw_fdb_add(struct prestera_port *port, const unsigned char *mac,
+			u16 vid, bool dynamic);
+int prestera_hw_fdb_del(struct prestera_port *port, const unsigned char *mac,
+			u16 vid);
+int prestera_hw_fdb_flush_port(struct prestera_port *port, u32 mode);
+int prestera_hw_fdb_flush_vlan(struct prestera_switch *sw, u16 vid, u32 mode);
+int prestera_hw_fdb_flush_port_vlan(struct prestera_port *port, u16 vid,
+				    u32 mode);
+
+/* Bridge API */
+int prestera_hw_bridge_create(struct prestera_switch *sw, u16 *bridge_id);
+int prestera_hw_bridge_delete(struct prestera_switch *sw, u16 bridge_id);
+int prestera_hw_bridge_port_add(struct prestera_port *port, u16 bridge_id);
+int prestera_hw_bridge_port_delete(struct prestera_port *port, u16 bridge_id);
+
+/* vTCAM API */
+int prestera_hw_vtcam_create(struct prestera_switch *sw,
+			     u8 lookup, const u32 *keymask, u32 *vtcam_id,
+			     enum prestera_hw_vtcam_direction_t direction);
+int prestera_hw_vtcam_rule_add(struct prestera_switch *sw, u32 vtcam_id,
+			       u32 prio, void *key, void *keymask,
+			       struct prestera_acl_hw_action_info *act,
+			       u8 n_act, u32 *rule_id);
+int prestera_hw_vtcam_rule_del(struct prestera_switch *sw,
+			       u32 vtcam_id, u32 rule_id);
+int prestera_hw_vtcam_destroy(struct prestera_switch *sw, u32 vtcam_id);
+int prestera_hw_vtcam_iface_bind(struct prestera_switch *sw,
+				 struct prestera_acl_iface *iface,
+				 u32 vtcam_id, u16 pcl_id);
+int prestera_hw_vtcam_iface_unbind(struct prestera_switch *sw,
+				   struct prestera_acl_iface *iface,
+				   u32 vtcam_id);
+
+/* Counter API */
+int prestera_hw_counter_trigger(struct prestera_switch *sw, u32 block_id);
+int prestera_hw_counter_abort(struct prestera_switch *sw);
+int prestera_hw_counters_get(struct prestera_switch *sw, u32 idx,
+			     u32 *len, bool *done,
+			     struct prestera_counter_stats *stats);
+int prestera_hw_counter_block_get(struct prestera_switch *sw,
+				  u32 client, u32 *block_id, u32 *offset,
+				  u32 *num_counters);
+int prestera_hw_counter_block_release(struct prestera_switch *sw,
+				      u32 block_id);
+int prestera_hw_counter_clear(struct prestera_switch *sw, u32 block_id,
+			      u32 counter_id);
+
+/* SPAN API */
+int prestera_hw_span_get(const struct prestera_port *port, u8 *span_id);
+int prestera_hw_span_bind(const struct prestera_port *port, u8 span_id,
+			  bool ingress);
+int prestera_hw_span_unbind(const struct prestera_port *port, bool ingress);
+int prestera_hw_span_release(struct prestera_switch *sw, u8 span_id);
+
+/* Router API */
+int prestera_hw_rif_create(struct prestera_switch *sw,
+			   struct prestera_iface *iif, u8 *mac, u16 *rif_id);
+int prestera_hw_rif_delete(struct prestera_switch *sw, u16 rif_id,
+			   struct prestera_iface *iif);
+
+/* Virtual Router API */
+int prestera_hw_vr_create(struct prestera_switch *sw, u16 *vr_id);
+int prestera_hw_vr_delete(struct prestera_switch *sw, u16 vr_id);
+
+/* LPM PI */
+int prestera_hw_lpm_add(struct prestera_switch *sw, u16 vr_id,
+			__be32 dst, u32 dst_len, u32 grp_id);
+int prestera_hw_lpm_del(struct prestera_switch *sw, u16 vr_id,
+			__be32 dst, u32 dst_len);
+
+/* NH API */
+int prestera_hw_nh_entries_set(struct prestera_switch *sw, int count,
+			       struct prestera_neigh_info *nhs, u32 grp_id);
+int prestera_hw_nhgrp_blk_get(struct prestera_switch *sw,
+			      u8 *hw_state, u32 buf_size /* Buffer in bytes */);
+int prestera_hw_nh_group_create(struct prestera_switch *sw, u16 nh_count,
+				u32 *grp_id);
+int prestera_hw_nh_group_delete(struct prestera_switch *sw, u16 nh_count,
+				u32 grp_id);
+
+/* Event handlers */
+int prestera_hw_event_handler_register(struct prestera_switch *sw,
+				       enum prestera_event_type type,
+				       prestera_event_cb_t fn,
+				       void *arg);
+void prestera_hw_event_handler_unregister(struct prestera_switch *sw,
+					  enum prestera_event_type type,
+					  prestera_event_cb_t fn);
+
+/* RX/TX */
+int prestera_hw_rxtx_init(struct prestera_switch *sw,
+			  struct prestera_rxtx_params *params);
+
+/* LAG API */
+int prestera_hw_lag_member_add(struct prestera_port *port, u16 lag_id);
+int prestera_hw_lag_member_del(struct prestera_port *port, u16 lag_id);
+int prestera_hw_lag_member_enable(struct prestera_port *port, u16 lag_id,
+				  bool enable);
+int prestera_hw_lag_fdb_add(struct prestera_switch *sw, u16 lag_id,
+			    const unsigned char *mac, u16 vid, bool dynamic);
+int prestera_hw_lag_fdb_del(struct prestera_switch *sw, u16 lag_id,
+			    const unsigned char *mac, u16 vid);
+int prestera_hw_fdb_flush_lag(struct prestera_switch *sw, u16 lag_id,
+			      u32 mode);
+int prestera_hw_fdb_flush_lag_vlan(struct prestera_switch *sw,
+				   u16 lag_id, u16 vid, u32 mode);
+
+/* HW trap/drop counters API */
+int
+prestera_hw_cpu_code_counters_get(struct prestera_switch *sw, u8 code,
+				  enum prestera_hw_cpu_code_cnt_t counter_type,
+				  u64 *packet_count);
+
+/* Policer API */
+int prestera_hw_policer_create(struct prestera_switch *sw, u8 type,
+			       u32 *policer_id);
+int prestera_hw_policer_release(struct prestera_switch *sw,
+				u32 policer_id);
+int prestera_hw_policer_sr_tcm_set(struct prestera_switch *sw,
+				   u32 policer_id, u64 cir, u32 cbs);
+
+/* Flood domain / MDB API */
+int prestera_hw_flood_domain_create(struct prestera_flood_domain *domain);
+int prestera_hw_flood_domain_destroy(struct prestera_flood_domain *domain);
+int prestera_hw_flood_domain_ports_set(struct prestera_flood_domain *domain);
+int prestera_hw_flood_domain_ports_reset(struct prestera_flood_domain *domain);
+
+int prestera_hw_mdb_create(struct prestera_mdb_entry *mdb);
+int prestera_hw_mdb_destroy(struct prestera_mdb_entry *mdb);
+
+#endif /* _PRESTERA_HW_H_ */

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_matchall.c

@@ -0,0 +1,127 @@
+// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
+/* Copyright (c) 2019-2022 Marvell International Ltd. All rights reserved */
+
+#include <linux/kernel.h>
+#include <linux/list.h>
+
+#include "prestera.h"
+#include "prestera_hw.h"
+#include "prestera_flow.h"
+#include "prestera_flower.h"
+#include "prestera_matchall.h"
+#include "prestera_span.h"
+
+static int prestera_mall_prio_check(struct prestera_flow_block *block,
+				    struct tc_cls_matchall_offload *f)
+{
+	u32 flower_prio_min;
+	u32 flower_prio_max;
+	int err;
+
+	err = prestera_flower_prio_get(block, f->common.chain_index,
+				       &flower_prio_min, &flower_prio_max);
+	if (err == -ENOENT)
+		/* No flower filters installed on this chain. */
+		return 0;
+
+	if (err) {
+		NL_SET_ERR_MSG(f->common.extack, "Failed to get flower priorities");
+		return err;
+	}
+
+	if (f->common.prio <= flower_prio_max && !block->ingress) {
+		NL_SET_ERR_MSG(f->common.extack, "Failed to add in front of existing flower rules");
+		return -EOPNOTSUPP;
+	}
+	if (f->common.prio >= flower_prio_min && block->ingress) {
+		NL_SET_ERR_MSG(f->common.extack, "Failed to add behind of existing flower rules");
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+int prestera_mall_prio_get(struct prestera_flow_block *block,
+			   u32 *prio_min, u32 *prio_max)
+{
+	if (!block->mall.bound)
+		return -ENOENT;
+
+	*prio_min = block->mall.prio_min;
+	*prio_max = block->mall.prio_max;
+	return 0;
+}
+
+static void prestera_mall_prio_update(struct prestera_flow_block *block,
+				      struct tc_cls_matchall_offload *f)
+{
+	block->mall.prio_min = min(block->mall.prio_min, f->common.prio);
+	block->mall.prio_max = max(block->mall.prio_max, f->common.prio);
+}
+
+int prestera_mall_replace(struct prestera_flow_block *block,
+			  struct tc_cls_matchall_offload *f)
+{
+	struct prestera_flow_block_binding *binding;
+	__be16 protocol = f->common.protocol;
+	struct flow_action_entry *act;
+	struct prestera_port *port;
+	int err;
+
+	if (!flow_offload_has_one_action(&f->rule->action)) {
+		NL_SET_ERR_MSG(f->common.extack,
+			       "Only singular actions are supported");
+		return -EOPNOTSUPP;
+	}
+
+	act = &f->rule->action.entries[0];
+
+	if (!prestera_netdev_check(act->dev)) {
+		NL_SET_ERR_MSG(f->common.extack,
+			       "Only Marvell Prestera port is supported");
+		return -EINVAL;
+	}
+	if (!tc_cls_can_offload_and_chain0(act->dev, &f->common))
+		return -EOPNOTSUPP;
+	if (act->id != FLOW_ACTION_MIRRED)
+		return -EOPNOTSUPP;
+	if (protocol != htons(ETH_P_ALL))
+		return -EOPNOTSUPP;
+
+	err = prestera_mall_prio_check(block, f);
+	if (err)
+		return err;
+
+	port = netdev_priv(act->dev);
+
+	list_for_each_entry(binding, &block->binding_list, list) {
+		err = prestera_span_rule_add(binding, port, block->ingress);
+		if (err == -EEXIST)
+			return err;
+		if (err)
+			goto rollback;
+	}
+
+	prestera_mall_prio_update(block, f);
+
+	block->mall.bound = true;
+	return 0;
+
+rollback:
+	list_for_each_entry_continue_reverse(binding,
+					     &block->binding_list, list)
+		prestera_span_rule_del(binding, block->ingress);
+	return err;
+}
+
+void prestera_mall_destroy(struct prestera_flow_block *block)
+{
+	struct prestera_flow_block_binding *binding;
+
+	list_for_each_entry(binding, &block->binding_list, list)
+		prestera_span_rule_del(binding, block->ingress);
+
+	block->mall.prio_min = UINT_MAX;
+	block->mall.prio_max = 0;
+	block->mall.bound = false;
+}

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_matchall.h

@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */
+/* Copyright (c) 2022 Marvell International Ltd. All rights reserved. */
+
+#ifndef _PRESTERA_MATCHALL_H_
+#define _PRESTERA_MATCHALL_H_
+
+#include <net/pkt_cls.h>
+
+struct prestera_flow_block;
+
+int prestera_mall_replace(struct prestera_flow_block *block,
+			  struct tc_cls_matchall_offload *f);
+void prestera_mall_destroy(struct prestera_flow_block *block);
+int prestera_mall_prio_get(struct prestera_flow_block *block,
+			   u32 *prio_min, u32 *prio_max);
+
+#endif /* _PRESTERA_MATCHALL_H_ */

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_pci.c

@@ -0,0 +1,982 @@
+// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
+/* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved */
+
+#include <linux/bitfield.h>
+#include <linux/circ_buf.h>
+#include <linux/device.h>
+#include <linux/firmware.h>
+#include <linux/iopoll.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+
+#include "prestera.h"
+
+#define PRESTERA_MSG_MAX_SIZE 1500
+
+#define PRESTERA_SUPP_FW_MAJ_VER	4
+#define PRESTERA_SUPP_FW_MIN_VER	1
+
+#define PRESTERA_PREV_FW_MAJ_VER	4
+#define PRESTERA_PREV_FW_MIN_VER	0
+
+#define PRESTERA_FW_PATH_FMT	"mrvl/prestera/mvsw_prestera_fw-v%u.%u.img"
+#define PRESTERA_FW_ARM64_PATH_FMT "mrvl/prestera/mvsw_prestera_fw_arm64-v%u.%u.img"
+
+#define PRESTERA_FW_HDR_MAGIC		0x351D9D06
+#define PRESTERA_FW_DL_TIMEOUT_MS	50000
+#define PRESTERA_FW_BLK_SZ		1024
+
+#define PRESTERA_FW_VER_MAJ_MUL 1000000
+#define PRESTERA_FW_VER_MIN_MUL 1000
+
+#define PRESTERA_FW_VER_MAJ(v)	((v) / PRESTERA_FW_VER_MAJ_MUL)
+
+#define PRESTERA_FW_VER_MIN(v) \
+	(((v) - (PRESTERA_FW_VER_MAJ(v) * PRESTERA_FW_VER_MAJ_MUL)) / \
+			PRESTERA_FW_VER_MIN_MUL)
+
+#define PRESTERA_FW_VER_PATCH(v) \
+	((v) - (PRESTERA_FW_VER_MAJ(v) * PRESTERA_FW_VER_MAJ_MUL) - \
+			(PRESTERA_FW_VER_MIN(v) * PRESTERA_FW_VER_MIN_MUL))
+
+enum prestera_pci_bar_t {
+	PRESTERA_PCI_BAR_FW = 2,
+	PRESTERA_PCI_BAR_PP = 4,
+};
+
+struct prestera_fw_header {
+	__be32 magic_number;
+	__be32 version_value;
+	u8 reserved[8];
+};
+
+struct prestera_ldr_regs {
+	u32 ldr_ready;
+	u32 pad1;
+
+	u32 ldr_img_size;
+	u32 ldr_ctl_flags;
+
+	u32 ldr_buf_offs;
+	u32 ldr_buf_size;
+
+	u32 ldr_buf_rd;
+	u32 pad2;
+	u32 ldr_buf_wr;
+
+	u32 ldr_status;
+};
+
+#define PRESTERA_LDR_REG_OFFSET(f)	offsetof(struct prestera_ldr_regs, f)
+
+#define PRESTERA_LDR_READY_MAGIC	0xf00dfeed
+
+#define PRESTERA_LDR_STATUS_IMG_DL	BIT(0)
+#define PRESTERA_LDR_STATUS_START_FW	BIT(1)
+#define PRESTERA_LDR_STATUS_INVALID_IMG	BIT(2)
+#define PRESTERA_LDR_STATUS_NOMEM	BIT(3)
+
+#define PRESTERA_LDR_REG_BASE(fw)	((fw)->ldr_regs)
+#define PRESTERA_LDR_REG_ADDR(fw, reg)	(PRESTERA_LDR_REG_BASE(fw) + (reg))
+
+/* fw loader registers */
+#define PRESTERA_LDR_READY_REG		PRESTERA_LDR_REG_OFFSET(ldr_ready)
+#define PRESTERA_LDR_IMG_SIZE_REG	PRESTERA_LDR_REG_OFFSET(ldr_img_size)
+#define PRESTERA_LDR_CTL_REG		PRESTERA_LDR_REG_OFFSET(ldr_ctl_flags)
+#define PRESTERA_LDR_BUF_SIZE_REG	PRESTERA_LDR_REG_OFFSET(ldr_buf_size)
+#define PRESTERA_LDR_BUF_OFFS_REG	PRESTERA_LDR_REG_OFFSET(ldr_buf_offs)
+#define PRESTERA_LDR_BUF_RD_REG		PRESTERA_LDR_REG_OFFSET(ldr_buf_rd)
+#define PRESTERA_LDR_BUF_WR_REG		PRESTERA_LDR_REG_OFFSET(ldr_buf_wr)
+#define PRESTERA_LDR_STATUS_REG		PRESTERA_LDR_REG_OFFSET(ldr_status)
+
+#define PRESTERA_LDR_CTL_DL_START	BIT(0)
+
+#define PRESTERA_EVT_QNUM_MAX	4
+
+struct prestera_fw_evtq_regs {
+	u32 rd_idx;
+	u32 pad1;
+	u32 wr_idx;
+	u32 pad2;
+	u32 offs;
+	u32 len;
+};
+
+#define PRESTERA_CMD_QNUM_MAX	4
+
+struct prestera_fw_cmdq_regs {
+	u32 req_ctl;
+	u32 req_len;
+	u32 rcv_ctl;
+	u32 rcv_len;
+	u32 offs;
+	u32 len;
+};
+
+struct prestera_fw_regs {
+	u32 fw_ready;
+	u32 cmd_offs;
+	u32 cmd_len;
+	u32 cmd_qnum;
+	u32 evt_offs;
+	u32 evt_qnum;
+
+	u32 fw_status;
+	u32 rx_status;
+
+	struct prestera_fw_cmdq_regs cmdq_list[PRESTERA_EVT_QNUM_MAX];
+	struct prestera_fw_evtq_regs evtq_list[PRESTERA_CMD_QNUM_MAX];
+};
+
+#define PRESTERA_FW_REG_OFFSET(f)	offsetof(struct prestera_fw_regs, f)
+
+#define PRESTERA_FW_READY_MAGIC		0xcafebabe
+
+/* fw registers */
+#define PRESTERA_FW_READY_REG		PRESTERA_FW_REG_OFFSET(fw_ready)
+
+#define PRESTERA_CMD_BUF_OFFS_REG	PRESTERA_FW_REG_OFFSET(cmd_offs)
+#define PRESTERA_CMD_BUF_LEN_REG	PRESTERA_FW_REG_OFFSET(cmd_len)
+#define PRESTERA_CMD_QNUM_REG		PRESTERA_FW_REG_OFFSET(cmd_qnum)
+#define PRESTERA_EVT_BUF_OFFS_REG	PRESTERA_FW_REG_OFFSET(evt_offs)
+#define PRESTERA_EVT_QNUM_REG		PRESTERA_FW_REG_OFFSET(evt_qnum)
+
+#define PRESTERA_CMDQ_REG_OFFSET(q, f)			\
+	(PRESTERA_FW_REG_OFFSET(cmdq_list) +		\
+	 (q) * sizeof(struct prestera_fw_cmdq_regs) +	\
+	 offsetof(struct prestera_fw_cmdq_regs, f))
+
+#define PRESTERA_CMDQ_REQ_CTL_REG(q)	PRESTERA_CMDQ_REG_OFFSET(q, req_ctl)
+#define PRESTERA_CMDQ_REQ_LEN_REG(q)	PRESTERA_CMDQ_REG_OFFSET(q, req_len)
+#define PRESTERA_CMDQ_RCV_CTL_REG(q)	PRESTERA_CMDQ_REG_OFFSET(q, rcv_ctl)
+#define PRESTERA_CMDQ_RCV_LEN_REG(q)	PRESTERA_CMDQ_REG_OFFSET(q, rcv_len)
+#define PRESTERA_CMDQ_OFFS_REG(q)	PRESTERA_CMDQ_REG_OFFSET(q, offs)
+#define PRESTERA_CMDQ_LEN_REG(q)	PRESTERA_CMDQ_REG_OFFSET(q, len)
+
+#define PRESTERA_FW_STATUS_REG		PRESTERA_FW_REG_OFFSET(fw_status)
+#define PRESTERA_RX_STATUS_REG		PRESTERA_FW_REG_OFFSET(rx_status)
+
+/* PRESTERA_CMD_REQ_CTL_REG flags */
+#define PRESTERA_CMD_F_REQ_SENT		BIT(0)
+#define PRESTERA_CMD_F_REPL_RCVD	BIT(1)
+
+/* PRESTERA_CMD_RCV_CTL_REG flags */
+#define PRESTERA_CMD_F_REPL_SENT	BIT(0)
+
+#define PRESTERA_FW_EVT_CTL_STATUS_MASK	GENMASK(1, 0)
+
+#define PRESTERA_FW_EVT_CTL_STATUS_ON	0
+#define PRESTERA_FW_EVT_CTL_STATUS_OFF	1
+
+#define PRESTERA_EVTQ_REG_OFFSET(q, f)			\
+	(PRESTERA_FW_REG_OFFSET(evtq_list) +		\
+	 (q) * sizeof(struct prestera_fw_evtq_regs) +	\
+	 offsetof(struct prestera_fw_evtq_regs, f))
+
+#define PRESTERA_EVTQ_RD_IDX_REG(q)	PRESTERA_EVTQ_REG_OFFSET(q, rd_idx)
+#define PRESTERA_EVTQ_WR_IDX_REG(q)	PRESTERA_EVTQ_REG_OFFSET(q, wr_idx)
+#define PRESTERA_EVTQ_OFFS_REG(q)	PRESTERA_EVTQ_REG_OFFSET(q, offs)
+#define PRESTERA_EVTQ_LEN_REG(q)	PRESTERA_EVTQ_REG_OFFSET(q, len)
+
+#define PRESTERA_FW_REG_BASE(fw)	((fw)->dev.ctl_regs)
+#define PRESTERA_FW_REG_ADDR(fw, reg)	PRESTERA_FW_REG_BASE((fw)) + (reg)
+
+#define PRESTERA_FW_CMD_DEFAULT_WAIT_MS	30000
+#define PRESTERA_FW_READY_WAIT_MS	20000
+
+#define PRESTERA_DEV_ID_AC3X_98DX_55	0xC804
+#define PRESTERA_DEV_ID_AC3X_98DX_65	0xC80C
+#define PRESTERA_DEV_ID_ALDRIN2		0xCC1E
+#define PRESTERA_DEV_ID_98DX7312M	0x981F
+#define PRESTERA_DEV_ID_98DX3500	0x9820
+#define PRESTERA_DEV_ID_98DX3501	0x9826
+#define PRESTERA_DEV_ID_98DX3510	0x9821
+#define PRESTERA_DEV_ID_98DX3520	0x9822
+
+struct prestera_fw_evtq {
+	u8 __iomem *addr;
+	size_t len;
+};
+
+struct prestera_fw_cmdq {
+	/* serialize access to dev->send_req */
+	struct mutex cmd_mtx;
+	u8 __iomem *addr;
+	size_t len;
+};
+
+struct prestera_fw {
+	struct prestera_fw_rev rev_supp;
+	const struct firmware *bin;
+	struct workqueue_struct *wq;
+	struct prestera_device dev;
+	struct pci_dev *pci_dev;
+	u8 __iomem *ldr_regs;
+	u8 __iomem *ldr_ring_buf;
+	u32 ldr_buf_len;
+	u32 ldr_wr_idx;
+	size_t cmd_mbox_len;
+	u8 __iomem *cmd_mbox;
+	struct prestera_fw_cmdq cmd_queue[PRESTERA_CMD_QNUM_MAX];
+	u8 cmd_qnum;
+	struct prestera_fw_evtq evt_queue[PRESTERA_EVT_QNUM_MAX];
+	u8 evt_qnum;
+	struct work_struct evt_work;
+	u8 __iomem *evt_buf;
+	u8 *evt_msg;
+};
+
+static int prestera_fw_load(struct prestera_fw *fw);
+
+static void prestera_fw_write(struct prestera_fw *fw, u32 reg, u32 val)
+{
+	writel(val, PRESTERA_FW_REG_ADDR(fw, reg));
+}
+
+static u32 prestera_fw_read(struct prestera_fw *fw, u32 reg)
+{
+	return readl(PRESTERA_FW_REG_ADDR(fw, reg));
+}
+
+static u32 prestera_fw_evtq_len(struct prestera_fw *fw, u8 qid)
+{
+	return fw->evt_queue[qid].len;
+}
+
+static u32 prestera_fw_evtq_avail(struct prestera_fw *fw, u8 qid)
+{
+	u32 wr_idx = prestera_fw_read(fw, PRESTERA_EVTQ_WR_IDX_REG(qid));
+	u32 rd_idx = prestera_fw_read(fw, PRESTERA_EVTQ_RD_IDX_REG(qid));
+
+	return CIRC_CNT(wr_idx, rd_idx, prestera_fw_evtq_len(fw, qid));
+}
+
+static void prestera_fw_evtq_rd_set(struct prestera_fw *fw,
+				    u8 qid, u32 idx)
+{
+	u32 rd_idx = idx & (prestera_fw_evtq_len(fw, qid) - 1);
+
+	prestera_fw_write(fw, PRESTERA_EVTQ_RD_IDX_REG(qid), rd_idx);
+}
+
+static u8 __iomem *prestera_fw_evtq_buf(struct prestera_fw *fw, u8 qid)
+{
+	return fw->evt_queue[qid].addr;
+}
+
+static u32 prestera_fw_evtq_read32(struct prestera_fw *fw, u8 qid)
+{
+	u32 rd_idx = prestera_fw_read(fw, PRESTERA_EVTQ_RD_IDX_REG(qid));
+	u32 val;
+
+	val = readl(prestera_fw_evtq_buf(fw, qid) + rd_idx);
+	prestera_fw_evtq_rd_set(fw, qid, rd_idx + 4);
+	return val;
+}
+
+static ssize_t prestera_fw_evtq_read_buf(struct prestera_fw *fw,
+					 u8 qid, void *buf, size_t len)
+{
+	u32 idx = prestera_fw_read(fw, PRESTERA_EVTQ_RD_IDX_REG(qid));
+	u8 __iomem *evtq_addr = prestera_fw_evtq_buf(fw, qid);
+	u32 *buf32 = buf;
+	int i;
+
+	for (i = 0; i < len / 4; buf32++, i++) {
+		*buf32 = readl_relaxed(evtq_addr + idx);
+		idx = (idx + 4) & (prestera_fw_evtq_len(fw, qid) - 1);
+	}
+
+	prestera_fw_evtq_rd_set(fw, qid, idx);
+
+	return i;
+}
+
+static u8 prestera_fw_evtq_pick(struct prestera_fw *fw)
+{
+	int qid;
+
+	for (qid = 0; qid < fw->evt_qnum; qid++) {
+		if (prestera_fw_evtq_avail(fw, qid) >= 4)
+			return qid;
+	}
+
+	return PRESTERA_EVT_QNUM_MAX;
+}
+
+static void prestera_fw_evt_ctl_status_set(struct prestera_fw *fw, u32 val)
+{
+	u32 status = prestera_fw_read(fw, PRESTERA_FW_STATUS_REG);
+
+	u32p_replace_bits(&status, val, PRESTERA_FW_EVT_CTL_STATUS_MASK);
+
+	prestera_fw_write(fw, PRESTERA_FW_STATUS_REG, status);
+}
+
+static void prestera_fw_evt_work_fn(struct work_struct *work)
+{
+	struct prestera_fw *fw;
+	void *msg;
+	u8 qid;
+
+	fw = container_of(work, struct prestera_fw, evt_work);
+	msg = fw->evt_msg;
+
+	prestera_fw_evt_ctl_status_set(fw, PRESTERA_FW_EVT_CTL_STATUS_OFF);
+
+	while ((qid = prestera_fw_evtq_pick(fw)) < PRESTERA_EVT_QNUM_MAX) {
+		u32 idx;
+		u32 len;
+
+		len = prestera_fw_evtq_read32(fw, qid);
+		idx = prestera_fw_read(fw, PRESTERA_EVTQ_RD_IDX_REG(qid));
+
+		WARN_ON(prestera_fw_evtq_avail(fw, qid) < len);
+
+		if (WARN_ON(len > PRESTERA_MSG_MAX_SIZE)) {
+			prestera_fw_evtq_rd_set(fw, qid, idx + len);
+			continue;
+		}
+
+		prestera_fw_evtq_read_buf(fw, qid, msg, len);
+
+		if (fw->dev.recv_msg)
+			fw->dev.recv_msg(&fw->dev, msg, len);
+	}
+
+	prestera_fw_evt_ctl_status_set(fw, PRESTERA_FW_EVT_CTL_STATUS_ON);
+}
+
+static int prestera_fw_wait_reg32(struct prestera_fw *fw, u32 reg, u32 cmp,
+				  unsigned int waitms)
+{
+	u8 __iomem *addr = PRESTERA_FW_REG_ADDR(fw, reg);
+	u32 val;
+
+	return readl_poll_timeout(addr, val, cmp == val,
+				  1 * USEC_PER_MSEC, waitms * USEC_PER_MSEC);
+}
+
+static void prestera_fw_cmdq_lock(struct prestera_fw *fw, u8 qid)
+{
+	mutex_lock(&fw->cmd_queue[qid].cmd_mtx);
+}
+
+static void prestera_fw_cmdq_unlock(struct prestera_fw *fw, u8 qid)
+{
+	mutex_unlock(&fw->cmd_queue[qid].cmd_mtx);
+}
+
+static u32 prestera_fw_cmdq_len(struct prestera_fw *fw, u8 qid)
+{
+	return fw->cmd_queue[qid].len;
+}
+
+static u8 __iomem *prestera_fw_cmdq_buf(struct prestera_fw *fw, u8 qid)
+{
+	return fw->cmd_queue[qid].addr;
+}
+
+static int prestera_fw_cmd_send(struct prestera_fw *fw, int qid,
+				void *in_msg, size_t in_size,
+				void *out_msg, size_t out_size,
+				unsigned int waitms)
+{
+	u32 ret_size;
+	int err;
+
+	if (!waitms)
+		waitms = PRESTERA_FW_CMD_DEFAULT_WAIT_MS;
+
+	if (ALIGN(in_size, 4) > prestera_fw_cmdq_len(fw, qid))
+		return -EMSGSIZE;
+
+	/* wait for finish previous reply from FW */
+	err = prestera_fw_wait_reg32(fw, PRESTERA_CMDQ_RCV_CTL_REG(qid), 0, 30);
+	if (err) {
+		dev_err(fw->dev.dev, "finish reply from FW is timed out\n");
+		return err;
+	}
+
+	prestera_fw_write(fw, PRESTERA_CMDQ_REQ_LEN_REG(qid), in_size);
+
+	memcpy_toio(prestera_fw_cmdq_buf(fw, qid), in_msg, in_size);
+
+	prestera_fw_write(fw, PRESTERA_CMDQ_REQ_CTL_REG(qid),
+			  PRESTERA_CMD_F_REQ_SENT);
+
+	/* wait for reply from FW */
+	err = prestera_fw_wait_reg32(fw, PRESTERA_CMDQ_RCV_CTL_REG(qid),
+				     PRESTERA_CMD_F_REPL_SENT, waitms);
+	if (err) {
+		dev_err(fw->dev.dev, "reply from FW is timed out\n");
+		goto cmd_exit;
+	}
+
+	ret_size = prestera_fw_read(fw, PRESTERA_CMDQ_RCV_LEN_REG(qid));
+	if (ret_size > out_size) {
+		dev_err(fw->dev.dev, "ret_size (%u) > out_len(%zu)\n",
+			ret_size, out_size);
+		err = -EMSGSIZE;
+		goto cmd_exit;
+	}
+
+	memcpy_fromio(out_msg,
+		      prestera_fw_cmdq_buf(fw, qid) + in_size, ret_size);
+
+cmd_exit:
+	prestera_fw_write(fw, PRESTERA_CMDQ_REQ_CTL_REG(qid),
+			  PRESTERA_CMD_F_REPL_RCVD);
+	return err;
+}
+
+static int prestera_fw_send_req(struct prestera_device *dev, int qid,
+				void *in_msg, size_t in_size, void *out_msg,
+				size_t out_size, unsigned int waitms)
+{
+	struct prestera_fw *fw;
+	ssize_t ret;
+
+	fw = container_of(dev, struct prestera_fw, dev);
+
+	prestera_fw_cmdq_lock(fw, qid);
+	ret = prestera_fw_cmd_send(fw, qid, in_msg, in_size, out_msg, out_size,
+				   waitms);
+	prestera_fw_cmdq_unlock(fw, qid);
+
+	return ret;
+}
+
+static int prestera_fw_init(struct prestera_fw *fw)
+{
+	u8 __iomem *base;
+	int err;
+	u8 qid;
+
+	fw->dev.send_req = prestera_fw_send_req;
+	fw->ldr_regs = fw->dev.ctl_regs;
+
+	err = prestera_fw_load(fw);
+	if (err)
+		return err;
+
+	err = prestera_fw_wait_reg32(fw, PRESTERA_FW_READY_REG,
+				     PRESTERA_FW_READY_MAGIC,
+				     PRESTERA_FW_READY_WAIT_MS);
+	if (err) {
+		dev_err(fw->dev.dev, "FW failed to start\n");
+		return err;
+	}
+
+	base = fw->dev.ctl_regs;
+
+	fw->cmd_mbox = base + prestera_fw_read(fw, PRESTERA_CMD_BUF_OFFS_REG);
+	fw->cmd_mbox_len = prestera_fw_read(fw, PRESTERA_CMD_BUF_LEN_REG);
+	fw->cmd_qnum = prestera_fw_read(fw, PRESTERA_CMD_QNUM_REG);
+
+	for (qid = 0; qid < fw->cmd_qnum; qid++) {
+		u32 offs = prestera_fw_read(fw, PRESTERA_CMDQ_OFFS_REG(qid));
+		struct prestera_fw_cmdq *cmdq = &fw->cmd_queue[qid];
+
+		cmdq->len = prestera_fw_read(fw, PRESTERA_CMDQ_LEN_REG(qid));
+		cmdq->addr = fw->cmd_mbox + offs;
+		mutex_init(&cmdq->cmd_mtx);
+	}
+
+	fw->evt_buf = base + prestera_fw_read(fw, PRESTERA_EVT_BUF_OFFS_REG);
+	fw->evt_qnum = prestera_fw_read(fw, PRESTERA_EVT_QNUM_REG);
+	fw->evt_msg = kmalloc(PRESTERA_MSG_MAX_SIZE, GFP_KERNEL);
+	if (!fw->evt_msg)
+		return -ENOMEM;
+
+	for (qid = 0; qid < fw->evt_qnum; qid++) {
+		u32 offs = prestera_fw_read(fw, PRESTERA_EVTQ_OFFS_REG(qid));
+		struct prestera_fw_evtq *evtq = &fw->evt_queue[qid];
+
+		evtq->len = prestera_fw_read(fw, PRESTERA_EVTQ_LEN_REG(qid));
+		evtq->addr = fw->evt_buf + offs;
+	}
+
+	return 0;
+}
+
+static void prestera_fw_uninit(struct prestera_fw *fw)
+{
+	kfree(fw->evt_msg);
+}
+
+static irqreturn_t prestera_pci_irq_handler(int irq, void *dev_id)
+{
+	struct prestera_fw *fw = dev_id;
+
+	if (prestera_fw_read(fw, PRESTERA_RX_STATUS_REG)) {
+		prestera_fw_write(fw, PRESTERA_RX_STATUS_REG, 0);
+
+		if (fw->dev.recv_pkt)
+			fw->dev.recv_pkt(&fw->dev);
+	}
+
+	queue_work(fw->wq, &fw->evt_work);
+
+	return IRQ_HANDLED;
+}
+
+static void prestera_ldr_write(struct prestera_fw *fw, u32 reg, u32 val)
+{
+	writel(val, PRESTERA_LDR_REG_ADDR(fw, reg));
+}
+
+static u32 prestera_ldr_read(struct prestera_fw *fw, u32 reg)
+{
+	return readl(PRESTERA_LDR_REG_ADDR(fw, reg));
+}
+
+static int prestera_ldr_wait_reg32(struct prestera_fw *fw,
+				   u32 reg, u32 cmp, unsigned int waitms)
+{
+	u8 __iomem *addr = PRESTERA_LDR_REG_ADDR(fw, reg);
+	u32 val;
+
+	return readl_poll_timeout(addr, val, cmp == val,
+				  10 * USEC_PER_MSEC, waitms * USEC_PER_MSEC);
+}
+
+static int prestera_ldr_wait_buf(struct prestera_fw *fw, size_t len)
+{
+	u8 __iomem *addr = PRESTERA_LDR_REG_ADDR(fw, PRESTERA_LDR_BUF_RD_REG);
+	u32 buf_len = fw->ldr_buf_len;
+	u32 wr_idx = fw->ldr_wr_idx;
+	u32 rd_idx;
+
+	return readl_poll_timeout(addr, rd_idx,
+				 CIRC_SPACE(wr_idx, rd_idx, buf_len) >= len,
+				 1 * USEC_PER_MSEC, 100 * USEC_PER_MSEC);
+}
+
+static int prestera_ldr_wait_dl_finish(struct prestera_fw *fw)
+{
+	u8 __iomem *addr = PRESTERA_LDR_REG_ADDR(fw, PRESTERA_LDR_STATUS_REG);
+	unsigned long mask = ~(PRESTERA_LDR_STATUS_IMG_DL);
+	u32 val;
+	int err;
+
+	err = readl_poll_timeout(addr, val, val & mask, 10 * USEC_PER_MSEC,
+				 PRESTERA_FW_DL_TIMEOUT_MS * USEC_PER_MSEC);
+	if (err) {
+		dev_err(fw->dev.dev, "Timeout to load FW img [state=%d]",
+			prestera_ldr_read(fw, PRESTERA_LDR_STATUS_REG));
+		return err;
+	}
+
+	return 0;
+}
+
+static void prestera_ldr_wr_idx_move(struct prestera_fw *fw, unsigned int n)
+{
+	fw->ldr_wr_idx = (fw->ldr_wr_idx + (n)) & (fw->ldr_buf_len - 1);
+}
+
+static void prestera_ldr_wr_idx_commit(struct prestera_fw *fw)
+{
+	prestera_ldr_write(fw, PRESTERA_LDR_BUF_WR_REG, fw->ldr_wr_idx);
+}
+
+static u8 __iomem *prestera_ldr_wr_ptr(struct prestera_fw *fw)
+{
+	return fw->ldr_ring_buf + fw->ldr_wr_idx;
+}
+
+static int prestera_ldr_send(struct prestera_fw *fw, const u8 *buf, size_t len)
+{
+	int err;
+	int i;
+
+	err = prestera_ldr_wait_buf(fw, len);
+	if (err) {
+		dev_err(fw->dev.dev, "failed wait for sending firmware\n");
+		return err;
+	}
+
+	for (i = 0; i < len; i += 4) {
+		writel_relaxed(*(u32 *)(buf + i), prestera_ldr_wr_ptr(fw));
+		prestera_ldr_wr_idx_move(fw, 4);
+	}
+
+	prestera_ldr_wr_idx_commit(fw);
+	return 0;
+}
+
+static int prestera_ldr_fw_send(struct prestera_fw *fw,
+				const char *img, u32 fw_size)
+{
+	u32 status;
+	u32 pos;
+	int err;
+
+	err = prestera_ldr_wait_reg32(fw, PRESTERA_LDR_STATUS_REG,
+				      PRESTERA_LDR_STATUS_IMG_DL,
+				      5 * MSEC_PER_SEC);
+	if (err) {
+		dev_err(fw->dev.dev, "Loader is not ready to load image\n");
+		return err;
+	}
+
+	for (pos = 0; pos < fw_size; pos += PRESTERA_FW_BLK_SZ) {
+		if (pos + PRESTERA_FW_BLK_SZ > fw_size)
+			break;
+
+		err = prestera_ldr_send(fw, img + pos, PRESTERA_FW_BLK_SZ);
+		if (err)
+			return err;
+	}
+
+	if (pos < fw_size) {
+		err = prestera_ldr_send(fw, img + pos, fw_size - pos);
+		if (err)
+			return err;
+	}
+
+	err = prestera_ldr_wait_dl_finish(fw);
+	if (err)
+		return err;
+
+	status = prestera_ldr_read(fw, PRESTERA_LDR_STATUS_REG);
+
+	switch (status) {
+	case PRESTERA_LDR_STATUS_INVALID_IMG:
+		dev_err(fw->dev.dev, "FW img has bad CRC\n");
+		return -EINVAL;
+	case PRESTERA_LDR_STATUS_NOMEM:
+		dev_err(fw->dev.dev, "Loader has no enough mem\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void prestera_fw_rev_parse(const struct prestera_fw_header *hdr,
+				  struct prestera_fw_rev *rev)
+{
+	u32 version = be32_to_cpu(hdr->version_value);
+
+	rev->maj = PRESTERA_FW_VER_MAJ(version);
+	rev->min = PRESTERA_FW_VER_MIN(version);
+	rev->sub = PRESTERA_FW_VER_PATCH(version);
+}
+
+static int prestera_fw_rev_check(struct prestera_fw *fw)
+{
+	struct prestera_fw_rev *rev = &fw->dev.fw_rev;
+
+	if (rev->maj == fw->rev_supp.maj && rev->min >= fw->rev_supp.min)
+		return 0;
+
+	dev_err(fw->dev.dev, "Driver supports FW version only '%u.%u.x'",
+		fw->rev_supp.maj, fw->rev_supp.min);
+
+	return -EINVAL;
+}
+
+static int prestera_fw_hdr_parse(struct prestera_fw *fw)
+{
+	struct prestera_fw_rev *rev = &fw->dev.fw_rev;
+	struct prestera_fw_header *hdr;
+	u32 magic;
+
+	if (fw->bin->size < sizeof(*hdr))
+		return -EINVAL;
+
+	hdr = (struct prestera_fw_header *)fw->bin->data;
+
+	magic = be32_to_cpu(hdr->magic_number);
+	if (magic != PRESTERA_FW_HDR_MAGIC) {
+		dev_err(fw->dev.dev, "FW img hdr magic is invalid");
+		return -EINVAL;
+	}
+
+	prestera_fw_rev_parse(hdr, rev);
+
+	dev_info(fw->dev.dev, "FW version '%u.%u.%u'\n",
+		 rev->maj, rev->min, rev->sub);
+
+	return prestera_fw_rev_check(fw);
+}
+
+static const char *prestera_fw_path_fmt_get(struct prestera_fw *fw)
+{
+	switch (fw->pci_dev->device) {
+	case PRESTERA_DEV_ID_98DX3500:
+	case PRESTERA_DEV_ID_98DX3501:
+	case PRESTERA_DEV_ID_98DX3510:
+	case PRESTERA_DEV_ID_98DX3520:
+		return PRESTERA_FW_ARM64_PATH_FMT;
+
+	default:
+		return PRESTERA_FW_PATH_FMT;
+	}
+}
+
+static int prestera_fw_get(struct prestera_fw *fw)
+{
+	int ver_maj = PRESTERA_SUPP_FW_MAJ_VER;
+	int ver_min = PRESTERA_SUPP_FW_MIN_VER;
+	char fw_path[128];
+	int err;
+
+pick_fw_ver:
+	snprintf(fw_path, sizeof(fw_path), prestera_fw_path_fmt_get(fw),
+		 ver_maj, ver_min);
+
+	err = request_firmware_direct(&fw->bin, fw_path, fw->dev.dev);
+	if (err) {
+		if (ver_maj != PRESTERA_PREV_FW_MAJ_VER ||
+		    ver_min != PRESTERA_PREV_FW_MIN_VER) {
+			ver_maj = PRESTERA_PREV_FW_MAJ_VER;
+			ver_min = PRESTERA_PREV_FW_MIN_VER;
+
+			dev_warn(fw->dev.dev,
+				 "missing latest %s firmware, fall-back to previous %u.%u version\n",
+				 fw_path, ver_maj, ver_min);
+
+			goto pick_fw_ver;
+		} else {
+			dev_err(fw->dev.dev, "failed to request previous firmware: %s\n",
+				fw_path);
+			return err;
+		}
+	}
+
+	dev_info(fw->dev.dev, "Loading %s ...", fw_path);
+
+	fw->rev_supp.maj = ver_maj;
+	fw->rev_supp.min = ver_min;
+	fw->rev_supp.sub = 0;
+
+	return 0;
+}
+
+static void prestera_fw_put(struct prestera_fw *fw)
+{
+	release_firmware(fw->bin);
+}
+
+static int prestera_fw_load(struct prestera_fw *fw)
+{
+	size_t hlen = sizeof(struct prestera_fw_header);
+	int err;
+
+	err = prestera_ldr_wait_reg32(fw, PRESTERA_LDR_READY_REG,
+				      PRESTERA_LDR_READY_MAGIC,
+				      5 * MSEC_PER_SEC);
+	if (err) {
+		dev_err(fw->dev.dev, "waiting for FW loader is timed out");
+		return err;
+	}
+
+	fw->ldr_ring_buf = fw->ldr_regs +
+		prestera_ldr_read(fw, PRESTERA_LDR_BUF_OFFS_REG);
+
+	fw->ldr_buf_len =
+		prestera_ldr_read(fw, PRESTERA_LDR_BUF_SIZE_REG);
+
+	fw->ldr_wr_idx = 0;
+
+	err = prestera_fw_get(fw);
+	if (err)
+		return err;
+
+	err = prestera_fw_hdr_parse(fw);
+	if (err) {
+		dev_err(fw->dev.dev, "FW image header is invalid\n");
+		goto out_release;
+	}
+
+	prestera_ldr_write(fw, PRESTERA_LDR_IMG_SIZE_REG, fw->bin->size - hlen);
+	prestera_ldr_write(fw, PRESTERA_LDR_CTL_REG, PRESTERA_LDR_CTL_DL_START);
+
+	err = prestera_ldr_fw_send(fw, fw->bin->data + hlen,
+				   fw->bin->size - hlen);
+
+out_release:
+	prestera_fw_put(fw);
+	return err;
+}
+
+static bool prestera_pci_pp_use_bar2(struct pci_dev *pdev)
+{
+	switch (pdev->device) {
+	case PRESTERA_DEV_ID_98DX7312M:
+	case PRESTERA_DEV_ID_98DX3500:
+	case PRESTERA_DEV_ID_98DX3501:
+	case PRESTERA_DEV_ID_98DX3510:
+	case PRESTERA_DEV_ID_98DX3520:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+static u32 prestera_pci_pp_bar2_offs(struct pci_dev *pdev)
+{
+	if (pci_resource_len(pdev, 2) == 0x1000000)
+		return 0x0;
+	else
+		return (pci_resource_len(pdev, 2) / 2);
+}
+
+static u32 prestera_pci_fw_bar2_offs(struct pci_dev *pdev)
+{
+	if (pci_resource_len(pdev, 2) == 0x1000000)
+		return 0x400000;
+	else
+		return 0x0;
+}
+
+static int prestera_pci_probe(struct pci_dev *pdev,
+			      const struct pci_device_id *id)
+{
+	const char *driver_name = dev_driver_string(&pdev->dev);
+	u8 __iomem *mem_addr, *pp_addr = NULL;
+	struct prestera_fw *fw;
+	int err;
+
+	err = pcim_enable_device(pdev);
+	if (err) {
+		dev_err(&pdev->dev, "pci_enable_device failed\n");
+		goto err_pci_enable_device;
+	}
+
+	err = pcim_request_all_regions(pdev, driver_name);
+	if (err) {
+		dev_err(&pdev->dev, "pcim_request_all_regions failed\n");
+		goto err_pci_request_regions;
+	}
+
+	err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(30));
+	if (err) {
+		dev_err(&pdev->dev, "fail to set DMA mask\n");
+		goto err_dma_mask;
+	}
+
+	mem_addr = pcim_iomap(pdev, 2, 0);
+	if (!mem_addr) {
+		dev_err(&pdev->dev, "pci mem ioremap failed\n");
+		err = -EIO;
+		goto err_mem_ioremap;
+	}
+
+	/* AC5X devices use second half of BAR2 */
+	if (prestera_pci_pp_use_bar2(pdev)) {
+		pp_addr = mem_addr + prestera_pci_pp_bar2_offs(pdev);
+		mem_addr = mem_addr + prestera_pci_fw_bar2_offs(pdev);
+	} else {
+		pp_addr = pcim_iomap(pdev, 4, 0);
+		if (!pp_addr) {
+			dev_err(&pdev->dev, "pp regs ioremap failed\n");
+			err = -EIO;
+			goto err_pp_ioremap;
+		}
+	}
+
+	pci_set_master(pdev);
+
+	fw = devm_kzalloc(&pdev->dev, sizeof(*fw), GFP_KERNEL);
+	if (!fw) {
+		err = -ENOMEM;
+		goto err_pci_dev_alloc;
+	}
+
+	fw->pci_dev = pdev;
+	fw->dev.ctl_regs = mem_addr;
+	fw->dev.pp_regs = pp_addr;
+	fw->dev.dev = &pdev->dev;
+
+	pci_set_drvdata(pdev, fw);
+
+	err = prestera_fw_init(fw);
+	if (err)
+		goto err_prestera_fw_init;
+
+	dev_info(fw->dev.dev, "Prestera FW is ready\n");
+
+	fw->wq = alloc_workqueue("prestera_fw_wq", WQ_HIGHPRI | WQ_PERCPU, 1);
+	if (!fw->wq) {
+		err = -ENOMEM;
+		goto err_wq_alloc;
+	}
+
+	INIT_WORK(&fw->evt_work, prestera_fw_evt_work_fn);
+
+	err = pci_alloc_irq_vectors(pdev, 1, 1, PCI_IRQ_MSI);
+	if (err < 0) {
+		dev_err(&pdev->dev, "MSI IRQ init failed\n");
+		goto err_irq_alloc;
+	}
+
+	err = request_irq(pci_irq_vector(pdev, 0), prestera_pci_irq_handler,
+			  0, driver_name, fw);
+	if (err) {
+		dev_err(&pdev->dev, "fail to request IRQ\n");
+		goto err_request_irq;
+	}
+
+	err = prestera_device_register(&fw->dev);
+	if (err)
+		goto err_prestera_dev_register;
+
+	return 0;
+
+err_prestera_dev_register:
+	free_irq(pci_irq_vector(pdev, 0), fw);
+err_request_irq:
+	pci_free_irq_vectors(pdev);
+err_irq_alloc:
+	destroy_workqueue(fw->wq);
+err_wq_alloc:
+	prestera_fw_uninit(fw);
+err_prestera_fw_init:
+err_pci_dev_alloc:
+err_pp_ioremap:
+err_mem_ioremap:
+err_dma_mask:
+err_pci_request_regions:
+err_pci_enable_device:
+	return err;
+}
+
+static void prestera_pci_remove(struct pci_dev *pdev)
+{
+	struct prestera_fw *fw = pci_get_drvdata(pdev);
+
+	prestera_device_unregister(&fw->dev);
+	free_irq(pci_irq_vector(pdev, 0), fw);
+	pci_free_irq_vectors(pdev);
+	destroy_workqueue(fw->wq);
+	prestera_fw_uninit(fw);
+}
+
+static const struct pci_device_id prestera_pci_devices[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL, PRESTERA_DEV_ID_AC3X_98DX_55) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL, PRESTERA_DEV_ID_AC3X_98DX_65) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL, PRESTERA_DEV_ID_ALDRIN2) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL, PRESTERA_DEV_ID_98DX7312M) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL, PRESTERA_DEV_ID_98DX3500) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL, PRESTERA_DEV_ID_98DX3501) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL, PRESTERA_DEV_ID_98DX3510) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL, PRESTERA_DEV_ID_98DX3520) },
+	{ }
+};
+MODULE_DEVICE_TABLE(pci, prestera_pci_devices);
+
+static struct pci_driver prestera_pci_driver = {
+	.name     = "Prestera DX",
+	.id_table = prestera_pci_devices,
+	.probe    = prestera_pci_probe,
+	.remove   = prestera_pci_remove,
+};
+module_pci_driver(prestera_pci_driver);
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_DESCRIPTION("Marvell Prestera switch PCI interface");

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_router_hw.c

@@ -0,0 +1,688 @@
+// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
+/* Copyright (c) 2019-2021 Marvell International Ltd. All rights reserved */
+
+#include <linux/rhashtable.h>
+
+#include "prestera.h"
+#include "prestera_hw.h"
+#include "prestera_router_hw.h"
+#include "prestera_acl.h"
+
+/*                                Nexthop is pointed
+ *                                to port (not rif)
+ *                                +-------+
+ *                              +>|nexthop|
+ *                              | +-------+
+ *                              |
+ *            +--+        +-----++
+ *   +------->|vr|<-+   +>|nh_grp|
+ *   |        +--+  |   | +------+
+ *   |              |   |
+ * +-+-------+   +--+---+-+
+ * |rif_entry|   |fib_node|
+ * +---------+   +--------+
+ *  Rif is        Fib - is exit point
+ *  used as
+ *  entry point
+ *  for vr in hw
+ */
+
+#define PRESTERA_NHGR_UNUSED (0)
+#define PRESTERA_NHGR_DROP (0xFFFFFFFF)
+/* Need to merge it with router_manager */
+#define PRESTERA_NH_ACTIVE_JIFFER_FILTER 3000 /* ms */
+
+static const struct rhashtable_params __prestera_fib_ht_params = {
+	.key_offset  = offsetof(struct prestera_fib_node, key),
+	.head_offset = offsetof(struct prestera_fib_node, ht_node),
+	.key_len     = sizeof(struct prestera_fib_key),
+	.automatic_shrinking = true,
+};
+
+static const struct rhashtable_params __prestera_nh_neigh_ht_params = {
+	.key_offset  = offsetof(struct prestera_nh_neigh, key),
+	.key_len     = sizeof(struct prestera_nh_neigh_key),
+	.head_offset = offsetof(struct prestera_nh_neigh, ht_node),
+};
+
+static const struct rhashtable_params __prestera_nexthop_group_ht_params = {
+	.key_offset  = offsetof(struct prestera_nexthop_group, key),
+	.key_len     = sizeof(struct prestera_nexthop_group_key),
+	.head_offset = offsetof(struct prestera_nexthop_group, ht_node),
+};
+
+static int prestera_nexthop_group_set(struct prestera_switch *sw,
+				      struct prestera_nexthop_group *nh_grp);
+static bool
+prestera_nexthop_group_util_hw_state(struct prestera_switch *sw,
+				     struct prestera_nexthop_group *nh_grp);
+static void prestera_fib_node_destroy_ht_cb(void *ptr, void *arg);
+
+/* TODO: move to router.h as macros */
+static bool prestera_nh_neigh_key_is_valid(struct prestera_nh_neigh_key *key)
+{
+	return memchr_inv(key, 0, sizeof(*key)) ? true : false;
+}
+
+int prestera_router_hw_init(struct prestera_switch *sw)
+{
+	int err;
+
+	err = rhashtable_init(&sw->router->nh_neigh_ht,
+			      &__prestera_nh_neigh_ht_params);
+	if (err)
+		goto err_nh_neigh_ht_init;
+
+	err = rhashtable_init(&sw->router->nexthop_group_ht,
+			      &__prestera_nexthop_group_ht_params);
+	if (err)
+		goto err_nexthop_grp_ht_init;
+
+	err = rhashtable_init(&sw->router->fib_ht,
+			      &__prestera_fib_ht_params);
+	if (err)
+		goto err_fib_ht_init;
+
+	INIT_LIST_HEAD(&sw->router->vr_list);
+	INIT_LIST_HEAD(&sw->router->rif_entry_list);
+
+	return 0;
+
+err_fib_ht_init:
+	rhashtable_destroy(&sw->router->nexthop_group_ht);
+err_nexthop_grp_ht_init:
+	rhashtable_destroy(&sw->router->nh_neigh_ht);
+err_nh_neigh_ht_init:
+	return 0;
+}
+
+void prestera_router_hw_fini(struct prestera_switch *sw)
+{
+	rhashtable_free_and_destroy(&sw->router->fib_ht,
+				    prestera_fib_node_destroy_ht_cb, sw);
+	WARN_ON(!list_empty(&sw->router->vr_list));
+	WARN_ON(!list_empty(&sw->router->rif_entry_list));
+	rhashtable_destroy(&sw->router->fib_ht);
+	rhashtable_destroy(&sw->router->nexthop_group_ht);
+	rhashtable_destroy(&sw->router->nh_neigh_ht);
+}
+
+static struct prestera_vr *__prestera_vr_find(struct prestera_switch *sw,
+					      u32 tb_id)
+{
+	struct prestera_vr *vr;
+
+	list_for_each_entry(vr, &sw->router->vr_list, router_node) {
+		if (vr->tb_id == tb_id)
+			return vr;
+	}
+
+	return NULL;
+}
+
+static struct prestera_vr *__prestera_vr_create(struct prestera_switch *sw,
+						u32 tb_id,
+						struct netlink_ext_ack *extack)
+{
+	struct prestera_vr *vr;
+	int err;
+
+	vr = kzalloc_obj(*vr);
+	if (!vr) {
+		err = -ENOMEM;
+		goto err_alloc_vr;
+	}
+
+	vr->tb_id = tb_id;
+
+	err = prestera_hw_vr_create(sw, &vr->hw_vr_id);
+	if (err)
+		goto err_hw_create;
+
+	list_add(&vr->router_node, &sw->router->vr_list);
+
+	return vr;
+
+err_hw_create:
+	kfree(vr);
+err_alloc_vr:
+	return ERR_PTR(err);
+}
+
+static void __prestera_vr_destroy(struct prestera_switch *sw,
+				  struct prestera_vr *vr)
+{
+	list_del(&vr->router_node);
+	prestera_hw_vr_delete(sw, vr->hw_vr_id);
+	kfree(vr);
+}
+
+static struct prestera_vr *prestera_vr_get(struct prestera_switch *sw, u32 tb_id,
+					   struct netlink_ext_ack *extack)
+{
+	struct prestera_vr *vr;
+
+	vr = __prestera_vr_find(sw, tb_id);
+	if (vr) {
+		refcount_inc(&vr->refcount);
+	} else {
+		vr = __prestera_vr_create(sw, tb_id, extack);
+		if (IS_ERR(vr))
+			return ERR_CAST(vr);
+
+		refcount_set(&vr->refcount, 1);
+	}
+
+	return vr;
+}
+
+static void prestera_vr_put(struct prestera_switch *sw, struct prestera_vr *vr)
+{
+	if (refcount_dec_and_test(&vr->refcount))
+		__prestera_vr_destroy(sw, vr);
+}
+
+/* iface is overhead struct. vr_id also can be removed. */
+static int
+__prestera_rif_entry_key_copy(const struct prestera_rif_entry_key *in,
+			      struct prestera_rif_entry_key *out)
+{
+	memset(out, 0, sizeof(*out));
+
+	switch (in->iface.type) {
+	case PRESTERA_IF_PORT_E:
+		out->iface.dev_port.hw_dev_num = in->iface.dev_port.hw_dev_num;
+		out->iface.dev_port.port_num = in->iface.dev_port.port_num;
+		break;
+	case PRESTERA_IF_LAG_E:
+		out->iface.lag_id = in->iface.lag_id;
+		break;
+	case PRESTERA_IF_VID_E:
+		out->iface.vlan_id = in->iface.vlan_id;
+		break;
+	default:
+		WARN(1, "Unsupported iface type");
+		return -EINVAL;
+	}
+
+	out->iface.type = in->iface.type;
+	return 0;
+}
+
+struct prestera_rif_entry *
+prestera_rif_entry_find(const struct prestera_switch *sw,
+			const struct prestera_rif_entry_key *k)
+{
+	struct prestera_rif_entry *rif_entry;
+	struct prestera_rif_entry_key lk; /* lookup key */
+
+	if (__prestera_rif_entry_key_copy(k, &lk))
+		return NULL;
+
+	list_for_each_entry(rif_entry, &sw->router->rif_entry_list,
+			    router_node) {
+		if (!memcmp(k, &rif_entry->key, sizeof(*k)))
+			return rif_entry;
+	}
+
+	return NULL;
+}
+
+void prestera_rif_entry_destroy(struct prestera_switch *sw,
+				struct prestera_rif_entry *e)
+{
+	struct prestera_iface iface;
+
+	list_del(&e->router_node);
+
+	memcpy(&iface, &e->key.iface, sizeof(iface));
+	iface.vr_id = e->vr->hw_vr_id;
+	prestera_hw_rif_delete(sw, e->hw_id, &iface);
+
+	prestera_vr_put(sw, e->vr);
+	kfree(e);
+}
+
+struct prestera_rif_entry *
+prestera_rif_entry_create(struct prestera_switch *sw,
+			  struct prestera_rif_entry_key *k,
+			  u32 tb_id, const unsigned char *addr)
+{
+	int err;
+	struct prestera_rif_entry *e;
+	struct prestera_iface iface;
+
+	e = kzalloc_obj(*e);
+	if (!e)
+		goto err_kzalloc;
+
+	if (__prestera_rif_entry_key_copy(k, &e->key))
+		goto err_key_copy;
+
+	e->vr = prestera_vr_get(sw, tb_id, NULL);
+	if (IS_ERR(e->vr))
+		goto err_vr_get;
+
+	memcpy(&e->addr, addr, sizeof(e->addr));
+
+	/* HW */
+	memcpy(&iface, &e->key.iface, sizeof(iface));
+	iface.vr_id = e->vr->hw_vr_id;
+	err = prestera_hw_rif_create(sw, &iface, e->addr, &e->hw_id);
+	if (err)
+		goto err_hw_create;
+
+	list_add(&e->router_node, &sw->router->rif_entry_list);
+
+	return e;
+
+err_hw_create:
+	prestera_vr_put(sw, e->vr);
+err_vr_get:
+err_key_copy:
+	kfree(e);
+err_kzalloc:
+	return NULL;
+}
+
+static void __prestera_nh_neigh_destroy(struct prestera_switch *sw,
+					struct prestera_nh_neigh *neigh)
+{
+	rhashtable_remove_fast(&sw->router->nh_neigh_ht,
+			       &neigh->ht_node,
+			       __prestera_nh_neigh_ht_params);
+	kfree(neigh);
+}
+
+static struct prestera_nh_neigh *
+__prestera_nh_neigh_create(struct prestera_switch *sw,
+			   struct prestera_nh_neigh_key *key)
+{
+	struct prestera_nh_neigh *neigh;
+	int err;
+
+	neigh = kzalloc_obj(*neigh);
+	if (!neigh)
+		goto err_kzalloc;
+
+	memcpy(&neigh->key, key, sizeof(*key));
+	neigh->info.connected = false;
+	INIT_LIST_HEAD(&neigh->nexthop_group_list);
+	err = rhashtable_insert_fast(&sw->router->nh_neigh_ht,
+				     &neigh->ht_node,
+				     __prestera_nh_neigh_ht_params);
+	if (err)
+		goto err_rhashtable_insert;
+
+	return neigh;
+
+err_rhashtable_insert:
+	kfree(neigh);
+err_kzalloc:
+	return NULL;
+}
+
+struct prestera_nh_neigh *
+prestera_nh_neigh_find(struct prestera_switch *sw,
+		       struct prestera_nh_neigh_key *key)
+{
+	struct prestera_nh_neigh *nh_neigh;
+
+	nh_neigh = rhashtable_lookup_fast(&sw->router->nh_neigh_ht,
+					  key, __prestera_nh_neigh_ht_params);
+	return nh_neigh;
+}
+
+struct prestera_nh_neigh *
+prestera_nh_neigh_get(struct prestera_switch *sw,
+		      struct prestera_nh_neigh_key *key)
+{
+	struct prestera_nh_neigh *neigh;
+
+	neigh = prestera_nh_neigh_find(sw, key);
+	if (!neigh)
+		return __prestera_nh_neigh_create(sw, key);
+
+	return neigh;
+}
+
+void prestera_nh_neigh_put(struct prestera_switch *sw,
+			   struct prestera_nh_neigh *neigh)
+{
+	if (list_empty(&neigh->nexthop_group_list))
+		__prestera_nh_neigh_destroy(sw, neigh);
+}
+
+/* Updates new prestera_neigh_info */
+int prestera_nh_neigh_set(struct prestera_switch *sw,
+			  struct prestera_nh_neigh *neigh)
+{
+	struct prestera_nh_neigh_head *nh_head;
+	struct prestera_nexthop_group *nh_grp;
+	int err;
+
+	list_for_each_entry(nh_head, &neigh->nexthop_group_list, head) {
+		nh_grp = nh_head->this;
+		err = prestera_nexthop_group_set(sw, nh_grp);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+bool prestera_nh_neigh_util_hw_state(struct prestera_switch *sw,
+				     struct prestera_nh_neigh *nh_neigh)
+{
+	bool state;
+	struct prestera_nh_neigh_head *nh_head, *tmp;
+
+	state = false;
+	list_for_each_entry_safe(nh_head, tmp,
+				 &nh_neigh->nexthop_group_list, head) {
+		state = prestera_nexthop_group_util_hw_state(sw, nh_head->this);
+		if (state)
+			goto out;
+	}
+
+out:
+	return state;
+}
+
+static struct prestera_nexthop_group *
+__prestera_nexthop_group_create(struct prestera_switch *sw,
+				struct prestera_nexthop_group_key *key)
+{
+	struct prestera_nexthop_group *nh_grp;
+	struct prestera_nh_neigh *nh_neigh;
+	int nh_cnt, err, gid;
+
+	nh_grp = kzalloc_obj(*nh_grp);
+	if (!nh_grp)
+		goto err_kzalloc;
+
+	memcpy(&nh_grp->key, key, sizeof(*key));
+	for (nh_cnt = 0; nh_cnt < PRESTERA_NHGR_SIZE_MAX; nh_cnt++) {
+		if (!prestera_nh_neigh_key_is_valid(&nh_grp->key.neigh[nh_cnt]))
+			break;
+
+		nh_neigh = prestera_nh_neigh_get(sw,
+						 &nh_grp->key.neigh[nh_cnt]);
+		if (!nh_neigh)
+			goto err_nh_neigh_get;
+
+		nh_grp->nh_neigh_head[nh_cnt].neigh = nh_neigh;
+		nh_grp->nh_neigh_head[nh_cnt].this = nh_grp;
+		list_add(&nh_grp->nh_neigh_head[nh_cnt].head,
+			 &nh_neigh->nexthop_group_list);
+	}
+
+	err = prestera_hw_nh_group_create(sw, nh_cnt, &nh_grp->grp_id);
+	if (err)
+		goto err_nh_group_create;
+
+	err = prestera_nexthop_group_set(sw, nh_grp);
+	if (err)
+		goto err_nexthop_group_set;
+
+	err = rhashtable_insert_fast(&sw->router->nexthop_group_ht,
+				     &nh_grp->ht_node,
+				     __prestera_nexthop_group_ht_params);
+	if (err)
+		goto err_ht_insert;
+
+	/* reset cache for created group */
+	gid = nh_grp->grp_id;
+	sw->router->nhgrp_hw_state_cache[gid / 8] &= ~BIT(gid % 8);
+
+	return nh_grp;
+
+err_ht_insert:
+err_nexthop_group_set:
+	prestera_hw_nh_group_delete(sw, nh_cnt, nh_grp->grp_id);
+err_nh_group_create:
+err_nh_neigh_get:
+	for (nh_cnt--; nh_cnt >= 0; nh_cnt--) {
+		list_del(&nh_grp->nh_neigh_head[nh_cnt].head);
+		prestera_nh_neigh_put(sw, nh_grp->nh_neigh_head[nh_cnt].neigh);
+	}
+
+	kfree(nh_grp);
+err_kzalloc:
+	return NULL;
+}
+
+static void
+__prestera_nexthop_group_destroy(struct prestera_switch *sw,
+				 struct prestera_nexthop_group *nh_grp)
+{
+	struct prestera_nh_neigh *nh_neigh;
+	int nh_cnt;
+
+	rhashtable_remove_fast(&sw->router->nexthop_group_ht,
+			       &nh_grp->ht_node,
+			       __prestera_nexthop_group_ht_params);
+
+	for (nh_cnt = 0; nh_cnt < PRESTERA_NHGR_SIZE_MAX; nh_cnt++) {
+		nh_neigh = nh_grp->nh_neigh_head[nh_cnt].neigh;
+		if (!nh_neigh)
+			break;
+
+		list_del(&nh_grp->nh_neigh_head[nh_cnt].head);
+		prestera_nh_neigh_put(sw, nh_neigh);
+	}
+
+	prestera_hw_nh_group_delete(sw, nh_cnt, nh_grp->grp_id);
+	kfree(nh_grp);
+}
+
+static struct prestera_nexthop_group *
+__prestera_nexthop_group_find(struct prestera_switch *sw,
+			      struct prestera_nexthop_group_key *key)
+{
+	struct prestera_nexthop_group *nh_grp;
+
+	nh_grp = rhashtable_lookup_fast(&sw->router->nexthop_group_ht,
+					key, __prestera_nexthop_group_ht_params);
+	return nh_grp;
+}
+
+static struct prestera_nexthop_group *
+prestera_nexthop_group_get(struct prestera_switch *sw,
+			   struct prestera_nexthop_group_key *key)
+{
+	struct prestera_nexthop_group *nh_grp;
+
+	nh_grp = __prestera_nexthop_group_find(sw, key);
+	if (nh_grp) {
+		refcount_inc(&nh_grp->refcount);
+	} else {
+		nh_grp = __prestera_nexthop_group_create(sw, key);
+		if (!nh_grp)
+			return ERR_PTR(-ENOMEM);
+
+		refcount_set(&nh_grp->refcount, 1);
+	}
+
+	return nh_grp;
+}
+
+static void prestera_nexthop_group_put(struct prestera_switch *sw,
+				       struct prestera_nexthop_group *nh_grp)
+{
+	if (refcount_dec_and_test(&nh_grp->refcount))
+		__prestera_nexthop_group_destroy(sw, nh_grp);
+}
+
+/* Updates with new nh_neigh's info */
+static int prestera_nexthop_group_set(struct prestera_switch *sw,
+				      struct prestera_nexthop_group *nh_grp)
+{
+	struct prestera_neigh_info info[PRESTERA_NHGR_SIZE_MAX];
+	struct prestera_nh_neigh *neigh;
+	int nh_cnt;
+
+	memset(&info[0], 0, sizeof(info));
+	for (nh_cnt = 0; nh_cnt < PRESTERA_NHGR_SIZE_MAX; nh_cnt++) {
+		neigh = nh_grp->nh_neigh_head[nh_cnt].neigh;
+		if (!neigh)
+			break;
+
+		memcpy(&info[nh_cnt], &neigh->info, sizeof(neigh->info));
+	}
+
+	return prestera_hw_nh_entries_set(sw, nh_cnt, &info[0], nh_grp->grp_id);
+}
+
+static bool
+prestera_nexthop_group_util_hw_state(struct prestera_switch *sw,
+				     struct prestera_nexthop_group *nh_grp)
+{
+	int err;
+	u32 buf_size = sw->size_tbl_router_nexthop / 8 + 1;
+	u32 gid = nh_grp->grp_id;
+	u8 *cache = sw->router->nhgrp_hw_state_cache;
+
+	/* Antijitter
+	 * Prevent situation, when we read state of nh_grp twice in short time,
+	 * and state bit is still cleared on second call. So just stuck active
+	 * state for PRESTERA_NH_ACTIVE_JIFFER_FILTER, after last occurred.
+	 */
+	if (!time_before(jiffies, sw->router->nhgrp_hw_cache_kick +
+			msecs_to_jiffies(PRESTERA_NH_ACTIVE_JIFFER_FILTER))) {
+		err = prestera_hw_nhgrp_blk_get(sw, cache, buf_size);
+		if (err) {
+			pr_err("Failed to get hw state nh_grp's");
+			return false;
+		}
+
+		sw->router->nhgrp_hw_cache_kick = jiffies;
+	}
+
+	if (cache[gid / 8] & BIT(gid % 8))
+		return true;
+
+	return false;
+}
+
+struct prestera_fib_node *
+prestera_fib_node_find(struct prestera_switch *sw, struct prestera_fib_key *key)
+{
+	struct prestera_fib_node *fib_node;
+
+	fib_node = rhashtable_lookup_fast(&sw->router->fib_ht, key,
+					  __prestera_fib_ht_params);
+	return fib_node;
+}
+
+static void __prestera_fib_node_destruct(struct prestera_switch *sw,
+					 struct prestera_fib_node *fib_node)
+{
+	struct prestera_vr *vr;
+
+	vr = fib_node->info.vr;
+	prestera_hw_lpm_del(sw, vr->hw_vr_id, fib_node->key.addr.u.ipv4,
+			    fib_node->key.prefix_len);
+	switch (fib_node->info.type) {
+	case PRESTERA_FIB_TYPE_UC_NH:
+		prestera_nexthop_group_put(sw, fib_node->info.nh_grp);
+		break;
+	case PRESTERA_FIB_TYPE_TRAP:
+		break;
+	case PRESTERA_FIB_TYPE_DROP:
+		break;
+	default:
+	      pr_err("Unknown fib_node->info.type = %d",
+		     fib_node->info.type);
+	}
+
+	prestera_vr_put(sw, vr);
+}
+
+void prestera_fib_node_destroy(struct prestera_switch *sw,
+			       struct prestera_fib_node *fib_node)
+{
+	__prestera_fib_node_destruct(sw, fib_node);
+	rhashtable_remove_fast(&sw->router->fib_ht, &fib_node->ht_node,
+			       __prestera_fib_ht_params);
+	kfree(fib_node);
+}
+
+static void prestera_fib_node_destroy_ht_cb(void *ptr, void *arg)
+{
+	struct prestera_fib_node *node = ptr;
+	struct prestera_switch *sw = arg;
+
+	__prestera_fib_node_destruct(sw, node);
+	kfree(node);
+}
+
+struct prestera_fib_node *
+prestera_fib_node_create(struct prestera_switch *sw,
+			 struct prestera_fib_key *key,
+			 enum prestera_fib_type fib_type,
+			 struct prestera_nexthop_group_key *nh_grp_key)
+{
+	struct prestera_fib_node *fib_node;
+	u32 grp_id;
+	struct prestera_vr *vr;
+	int err;
+
+	fib_node = kzalloc_obj(*fib_node);
+	if (!fib_node)
+		goto err_kzalloc;
+
+	memcpy(&fib_node->key, key, sizeof(*key));
+	fib_node->info.type = fib_type;
+
+	vr = prestera_vr_get(sw, key->tb_id, NULL);
+	if (IS_ERR(vr))
+		goto err_vr_get;
+
+	fib_node->info.vr = vr;
+
+	switch (fib_type) {
+	case PRESTERA_FIB_TYPE_TRAP:
+		grp_id = PRESTERA_NHGR_UNUSED;
+		break;
+	case PRESTERA_FIB_TYPE_DROP:
+		grp_id = PRESTERA_NHGR_DROP;
+		break;
+	case PRESTERA_FIB_TYPE_UC_NH:
+		fib_node->info.nh_grp = prestera_nexthop_group_get(sw,
+								   nh_grp_key);
+		if (IS_ERR(fib_node->info.nh_grp))
+			goto err_nh_grp_get;
+
+		grp_id = fib_node->info.nh_grp->grp_id;
+		break;
+	default:
+		pr_err("Unsupported fib_type %d", fib_type);
+		goto err_nh_grp_get;
+	}
+
+	err = prestera_hw_lpm_add(sw, vr->hw_vr_id, key->addr.u.ipv4,
+				  key->prefix_len, grp_id);
+	if (err)
+		goto err_lpm_add;
+
+	err = rhashtable_insert_fast(&sw->router->fib_ht, &fib_node->ht_node,
+				     __prestera_fib_ht_params);
+	if (err)
+		goto err_ht_insert;
+
+	return fib_node;
+
+err_ht_insert:
+	prestera_hw_lpm_del(sw, vr->hw_vr_id, key->addr.u.ipv4,
+			    key->prefix_len);
+err_lpm_add:
+	if (fib_type == PRESTERA_FIB_TYPE_UC_NH)
+		prestera_nexthop_group_put(sw, fib_node->info.nh_grp);
+err_nh_grp_get:
+	prestera_vr_put(sw, vr);
+err_vr_get:
+	kfree(fib_node);
+err_kzalloc:
+	return NULL;
+}

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_router_hw.h

@@ -0,0 +1,155 @@
+/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */
+/* Copyright (c) 2019-2021 Marvell International Ltd. All rights reserved. */
+
+#ifndef _PRESTERA_ROUTER_HW_H_
+#define _PRESTERA_ROUTER_HW_H_
+
+struct prestera_vr {
+	struct list_head router_node;
+	refcount_t refcount;
+	u32 tb_id;			/* key (kernel fib table id) */
+	u16 hw_vr_id;			/* virtual router ID */
+	u8 __pad[2];
+};
+
+struct prestera_rif_entry {
+	struct prestera_rif_entry_key {
+		struct prestera_iface iface;
+	} key;
+	struct prestera_vr *vr;
+	unsigned char addr[ETH_ALEN];
+	u16 hw_id; /* rif_id */
+	struct list_head router_node; /* ht */
+};
+
+struct prestera_ip_addr {
+	union {
+		__be32 ipv4;
+		struct in6_addr ipv6;
+	} u;
+	enum {
+		PRESTERA_IPV4 = 0,
+		PRESTERA_IPV6
+	} v;
+#define PRESTERA_IP_ADDR_PLEN(V) ((V) == PRESTERA_IPV4 ? 32 : \
+				  /* (V) == PRESTERA_IPV6 ? */ 128 /* : 0 */)
+};
+
+struct prestera_nh_neigh_key {
+	struct prestera_ip_addr addr;
+	/* Seems like rif is obsolete, because there is iface in info ?
+	 * Key can contain functional fields, or fields, which is used to
+	 * filter duplicate objects on logical level (before you pass it to
+	 * HW)... also key can be used to cover hardware restrictions.
+	 * In our case rif - is logical interface (even can be VLAN), which
+	 * is used in combination with IP address (which is also not related to
+	 * hardware nexthop) to provide logical compression of created nexthops.
+	 * You even can imagine, that rif+IPaddr is just cookie.
+	 */
+	/* struct prestera_rif *rif; */
+	/* Use just as cookie, to divide ARP domains (in order with addr) */
+	void *rif;
+};
+
+/* Used for hw call */
+struct prestera_neigh_info {
+	struct prestera_iface iface;
+	unsigned char ha[ETH_ALEN];
+	u8 connected; /* bool. indicate, if mac/oif valid */
+	u8 __pad[1];
+};
+
+/* Used to notify nh about neigh change */
+struct prestera_nh_neigh {
+	struct prestera_nh_neigh_key key;
+	struct prestera_neigh_info info;
+	struct rhash_head ht_node; /* node of prestera_vr */
+	struct list_head nexthop_group_list;
+};
+
+#define PRESTERA_NHGR_SIZE_MAX 4
+
+struct prestera_nexthop_group {
+	struct prestera_nexthop_group_key {
+		struct prestera_nh_neigh_key neigh[PRESTERA_NHGR_SIZE_MAX];
+	} key;
+	/* Store intermediate object here.
+	 * This prevent overhead kzalloc call.
+	 */
+	/* nh_neigh is used only to notify nexthop_group */
+	struct prestera_nh_neigh_head {
+		struct prestera_nexthop_group *this;
+		struct list_head head;
+		/* ptr to neigh is not necessary.
+		 * It used to prevent lookup of nh_neigh by key (n) on destroy
+		 */
+		struct prestera_nh_neigh *neigh;
+	} nh_neigh_head[PRESTERA_NHGR_SIZE_MAX];
+	struct rhash_head ht_node; /* node of prestera_vr */
+	refcount_t refcount;
+	u32 grp_id; /* hw */
+};
+
+struct prestera_fib_key {
+	struct prestera_ip_addr addr;
+	u32 prefix_len;
+	u32 tb_id;
+};
+
+struct prestera_fib_info {
+	struct prestera_vr *vr;
+	struct list_head vr_node;
+	enum prestera_fib_type {
+		PRESTERA_FIB_TYPE_INVALID = 0,
+		/* must be pointer to nh_grp id */
+		PRESTERA_FIB_TYPE_UC_NH,
+		/* It can be connected route
+		 * and will be overlapped with neighbours
+		 */
+		PRESTERA_FIB_TYPE_TRAP,
+		PRESTERA_FIB_TYPE_DROP
+	} type;
+	/* Valid only if type = UC_NH*/
+	struct prestera_nexthop_group *nh_grp;
+};
+
+struct prestera_fib_node {
+	struct rhash_head ht_node; /* node of prestera_vr */
+	struct prestera_fib_key key;
+	struct prestera_fib_info info; /* action related info */
+};
+
+struct prestera_rif_entry *
+prestera_rif_entry_find(const struct prestera_switch *sw,
+			const struct prestera_rif_entry_key *k);
+void prestera_rif_entry_destroy(struct prestera_switch *sw,
+				struct prestera_rif_entry *e);
+struct prestera_rif_entry *
+prestera_rif_entry_create(struct prestera_switch *sw,
+			  struct prestera_rif_entry_key *k,
+			  u32 tb_id, const unsigned char *addr);
+struct prestera_nh_neigh *
+prestera_nh_neigh_find(struct prestera_switch *sw,
+		       struct prestera_nh_neigh_key *key);
+struct prestera_nh_neigh *
+prestera_nh_neigh_get(struct prestera_switch *sw,
+		      struct prestera_nh_neigh_key *key);
+void prestera_nh_neigh_put(struct prestera_switch *sw,
+			   struct prestera_nh_neigh *neigh);
+int prestera_nh_neigh_set(struct prestera_switch *sw,
+			  struct prestera_nh_neigh *neigh);
+bool prestera_nh_neigh_util_hw_state(struct prestera_switch *sw,
+				     struct prestera_nh_neigh *nh_neigh);
+struct prestera_fib_node *prestera_fib_node_find(struct prestera_switch *sw,
+						 struct prestera_fib_key *key);
+void prestera_fib_node_destroy(struct prestera_switch *sw,
+			       struct prestera_fib_node *fib_node);
+struct prestera_fib_node *
+prestera_fib_node_create(struct prestera_switch *sw,
+			 struct prestera_fib_key *key,
+			 enum prestera_fib_type fib_type,
+			 struct prestera_nexthop_group_key *nh_grp_key);
+int prestera_router_hw_init(struct prestera_switch *sw);
+void prestera_router_hw_fini(struct prestera_switch *sw);
+
+#endif /* _PRESTERA_ROUTER_HW_H_ */

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_rxtx.c

@@ -0,0 +1,829 @@
+// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
+/* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved */
+
+#include <linux/bitfield.h>
+#include <linux/dmapool.h>
+#include <linux/etherdevice.h>
+#include <linux/if_vlan.h>
+#include <linux/platform_device.h>
+
+#include "prestera_dsa.h"
+#include "prestera.h"
+#include "prestera_hw.h"
+#include "prestera_rxtx.h"
+#include "prestera_devlink.h"
+
+#define PRESTERA_SDMA_WAIT_MUL		10
+
+struct prestera_sdma_desc {
+	__le32 word1;
+	__le32 word2;
+	__le32 buff;
+	__le32 next;
+} __packed __aligned(16);
+
+#define PRESTERA_SDMA_BUFF_SIZE_MAX	1544
+
+#define PRESTERA_SDMA_RX_DESC_PKT_LEN(desc) \
+	((le32_to_cpu((desc)->word2) >> 16) & GENMASK(13, 0))
+
+#define PRESTERA_SDMA_RX_DESC_OWNER(desc) \
+	((le32_to_cpu((desc)->word1) & BIT(31)) >> 31)
+
+#define PRESTERA_SDMA_RX_DESC_IS_RCVD(desc) \
+	(PRESTERA_SDMA_RX_DESC_OWNER(desc) == PRESTERA_SDMA_RX_DESC_CPU_OWN)
+
+#define PRESTERA_SDMA_RX_DESC_CPU_OWN	0
+#define PRESTERA_SDMA_RX_DESC_DMA_OWN	1
+
+#define PRESTERA_SDMA_RX_QUEUE_NUM	8
+
+#define PRESTERA_SDMA_RX_DESC_PER_Q	1000
+
+#define PRESTERA_SDMA_TX_DESC_PER_Q	1000
+#define PRESTERA_SDMA_TX_MAX_BURST	64
+
+#define PRESTERA_SDMA_TX_DESC_OWNER(desc) \
+	((le32_to_cpu((desc)->word1) & BIT(31)) >> 31)
+
+#define PRESTERA_SDMA_TX_DESC_CPU_OWN	0
+#define PRESTERA_SDMA_TX_DESC_DMA_OWN	1U
+
+#define PRESTERA_SDMA_TX_DESC_IS_SENT(desc) \
+	(PRESTERA_SDMA_TX_DESC_OWNER(desc) == PRESTERA_SDMA_TX_DESC_CPU_OWN)
+
+#define PRESTERA_SDMA_TX_DESC_LAST	BIT(20)
+#define PRESTERA_SDMA_TX_DESC_FIRST	BIT(21)
+#define PRESTERA_SDMA_TX_DESC_CALC_CRC	BIT(12)
+
+#define PRESTERA_SDMA_TX_DESC_SINGLE	\
+	(PRESTERA_SDMA_TX_DESC_FIRST | PRESTERA_SDMA_TX_DESC_LAST)
+
+#define PRESTERA_SDMA_TX_DESC_INIT	\
+	(PRESTERA_SDMA_TX_DESC_SINGLE | PRESTERA_SDMA_TX_DESC_CALC_CRC)
+
+#define PRESTERA_SDMA_RX_INTR_MASK_REG		0x2814
+#define PRESTERA_SDMA_RX_QUEUE_STATUS_REG	0x2680
+#define PRESTERA_SDMA_RX_QUEUE_DESC_REG(n)	(0x260C + (n) * 16)
+
+#define PRESTERA_SDMA_TX_QUEUE_DESC_REG		0x26C0
+#define PRESTERA_SDMA_TX_QUEUE_START_REG	0x2868
+
+struct prestera_sdma_buf {
+	struct prestera_sdma_desc *desc;
+	dma_addr_t desc_dma;
+	struct sk_buff *skb;
+	dma_addr_t buf_dma;
+	bool is_used;
+};
+
+struct prestera_rx_ring {
+	struct prestera_sdma_buf *bufs;
+	int next_rx;
+};
+
+struct prestera_tx_ring {
+	struct prestera_sdma_buf *bufs;
+	int next_tx;
+	int max_burst;
+	int burst;
+};
+
+struct prestera_sdma {
+	struct prestera_rx_ring rx_ring[PRESTERA_SDMA_RX_QUEUE_NUM];
+	struct prestera_tx_ring tx_ring;
+	struct prestera_switch *sw;
+	struct dma_pool *desc_pool;
+	struct work_struct tx_work;
+	struct napi_struct rx_napi;
+	struct net_device *napi_dev;
+	u32 map_addr;
+	u64 dma_mask;
+	/* protect SDMA with concurrent access from multiple CPUs */
+	spinlock_t tx_lock;
+};
+
+struct prestera_rxtx {
+	struct prestera_sdma sdma;
+};
+
+static int prestera_sdma_buf_init(struct prestera_sdma *sdma,
+				  struct prestera_sdma_buf *buf)
+{
+	struct prestera_sdma_desc *desc;
+	dma_addr_t dma;
+
+	desc = dma_pool_alloc(sdma->desc_pool, GFP_DMA | GFP_KERNEL, &dma);
+	if (!desc)
+		return -ENOMEM;
+
+	buf->buf_dma = DMA_MAPPING_ERROR;
+	buf->desc_dma = dma;
+	buf->desc = desc;
+	buf->skb = NULL;
+
+	return 0;
+}
+
+static u32 prestera_sdma_map(struct prestera_sdma *sdma, dma_addr_t pa)
+{
+	return sdma->map_addr + pa;
+}
+
+static void prestera_sdma_rx_desc_init(struct prestera_sdma *sdma,
+				       struct prestera_sdma_desc *desc,
+				       dma_addr_t buf)
+{
+	u32 word = le32_to_cpu(desc->word2);
+
+	u32p_replace_bits(&word, PRESTERA_SDMA_BUFF_SIZE_MAX, GENMASK(15, 0));
+	desc->word2 = cpu_to_le32(word);
+
+	desc->buff = cpu_to_le32(prestera_sdma_map(sdma, buf));
+
+	/* make sure buffer is set before reset the descriptor */
+	wmb();
+
+	desc->word1 = cpu_to_le32(0xA0000000);
+}
+
+static void prestera_sdma_rx_desc_set_next(struct prestera_sdma *sdma,
+					   struct prestera_sdma_desc *desc,
+					   dma_addr_t next)
+{
+	desc->next = cpu_to_le32(prestera_sdma_map(sdma, next));
+}
+
+static int prestera_sdma_rx_skb_alloc(struct prestera_sdma *sdma,
+				      struct prestera_sdma_buf *buf)
+{
+	struct device *dev = sdma->sw->dev->dev;
+	struct sk_buff *skb;
+	dma_addr_t dma;
+
+	skb = alloc_skb(PRESTERA_SDMA_BUFF_SIZE_MAX, GFP_DMA | GFP_ATOMIC);
+	if (!skb)
+		return -ENOMEM;
+
+	dma = dma_map_single(dev, skb->data, skb->len, DMA_FROM_DEVICE);
+	if (dma_mapping_error(dev, dma))
+		goto err_dma_map;
+
+	if (buf->skb)
+		dma_unmap_single(dev, buf->buf_dma, buf->skb->len,
+				 DMA_FROM_DEVICE);
+
+	buf->buf_dma = dma;
+	buf->skb = skb;
+
+	return 0;
+
+err_dma_map:
+	kfree_skb(skb);
+
+	return -ENOMEM;
+}
+
+static struct sk_buff *prestera_sdma_rx_skb_get(struct prestera_sdma *sdma,
+						struct prestera_sdma_buf *buf)
+{
+	dma_addr_t buf_dma = buf->buf_dma;
+	struct sk_buff *skb = buf->skb;
+	u32 len = skb->len;
+	int err;
+
+	err = prestera_sdma_rx_skb_alloc(sdma, buf);
+	if (err) {
+		buf->buf_dma = buf_dma;
+		buf->skb = skb;
+
+		skb = alloc_skb(skb->len, GFP_ATOMIC);
+		if (skb) {
+			skb_put(skb, len);
+			skb_copy_from_linear_data(buf->skb, skb->data, len);
+		}
+	}
+
+	prestera_sdma_rx_desc_init(sdma, buf->desc, buf->buf_dma);
+
+	return skb;
+}
+
+static int prestera_rxtx_process_skb(struct prestera_sdma *sdma,
+				     struct sk_buff *skb)
+{
+	struct prestera_port *port;
+	struct prestera_dsa dsa;
+	u32 hw_port, dev_id;
+	u8 cpu_code;
+	int err;
+
+	skb_pull(skb, ETH_HLEN);
+
+	/* ethertype field is part of the dsa header */
+	err = prestera_dsa_parse(&dsa, skb->data - ETH_TLEN);
+	if (err)
+		return err;
+
+	dev_id = dsa.hw_dev_num;
+	hw_port = dsa.port_num;
+
+	port = prestera_port_find_by_hwid(sdma->sw, dev_id, hw_port);
+	if (unlikely(!port)) {
+		dev_warn_ratelimited(prestera_dev(sdma->sw), "received pkt for non-existent port(%u, %u)\n",
+				     dev_id, hw_port);
+		return -ENOENT;
+	}
+
+	if (unlikely(!pskb_may_pull(skb, PRESTERA_DSA_HLEN)))
+		return -EINVAL;
+
+	/* remove DSA tag and update checksum */
+	skb_pull_rcsum(skb, PRESTERA_DSA_HLEN);
+
+	memmove(skb->data - ETH_HLEN, skb->data - ETH_HLEN - PRESTERA_DSA_HLEN,
+		ETH_ALEN * 2);
+
+	skb_push(skb, ETH_HLEN);
+
+	skb->protocol = eth_type_trans(skb, port->dev);
+
+	if (dsa.vlan.is_tagged) {
+		u16 tci = dsa.vlan.vid & VLAN_VID_MASK;
+
+		tci |= dsa.vlan.vpt << VLAN_PRIO_SHIFT;
+		if (dsa.vlan.cfi_bit)
+			tci |= VLAN_CFI_MASK;
+
+		__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;
+}
+
+static int prestera_sdma_next_rx_buf_idx(int buf_idx)
+{
+	return (buf_idx + 1) % PRESTERA_SDMA_RX_DESC_PER_Q;
+}
+
+static int prestera_sdma_rx_poll(struct napi_struct *napi, int budget)
+{
+	int qnum = PRESTERA_SDMA_RX_QUEUE_NUM;
+	unsigned int rxq_done_map = 0;
+	struct prestera_sdma *sdma;
+	struct list_head rx_list;
+	unsigned int qmask;
+	int pkts_done = 0;
+	int q;
+
+	qnum = PRESTERA_SDMA_RX_QUEUE_NUM;
+	qmask = GENMASK(qnum - 1, 0);
+
+	INIT_LIST_HEAD(&rx_list);
+
+	sdma = container_of(napi, struct prestera_sdma, rx_napi);
+
+	while (pkts_done < budget && rxq_done_map != qmask) {
+		for (q = 0; q < qnum && pkts_done < budget; q++) {
+			struct prestera_rx_ring *ring = &sdma->rx_ring[q];
+			struct prestera_sdma_desc *desc;
+			struct prestera_sdma_buf *buf;
+			int buf_idx = ring->next_rx;
+			struct sk_buff *skb;
+
+			buf = &ring->bufs[buf_idx];
+			desc = buf->desc;
+
+			if (PRESTERA_SDMA_RX_DESC_IS_RCVD(desc)) {
+				rxq_done_map &= ~BIT(q);
+			} else {
+				rxq_done_map |= BIT(q);
+				continue;
+			}
+
+			pkts_done++;
+
+			__skb_trim(buf->skb, PRESTERA_SDMA_RX_DESC_PKT_LEN(desc));
+
+			skb = prestera_sdma_rx_skb_get(sdma, buf);
+			if (!skb)
+				goto rx_next_buf;
+
+			if (unlikely(prestera_rxtx_process_skb(sdma, skb)))
+				goto rx_next_buf;
+
+			list_add_tail(&skb->list, &rx_list);
+rx_next_buf:
+			ring->next_rx = prestera_sdma_next_rx_buf_idx(buf_idx);
+		}
+	}
+
+	if (pkts_done < budget && napi_complete_done(napi, pkts_done))
+		prestera_write(sdma->sw, PRESTERA_SDMA_RX_INTR_MASK_REG,
+			       GENMASK(9, 2));
+
+	netif_receive_skb_list(&rx_list);
+
+	return pkts_done;
+}
+
+static void prestera_sdma_rx_fini(struct prestera_sdma *sdma)
+{
+	int qnum = PRESTERA_SDMA_RX_QUEUE_NUM;
+	int q, b;
+
+	/* disable all rx queues */
+	prestera_write(sdma->sw, PRESTERA_SDMA_RX_QUEUE_STATUS_REG,
+		       GENMASK(15, 8));
+
+	for (q = 0; q < qnum; q++) {
+		struct prestera_rx_ring *ring = &sdma->rx_ring[q];
+
+		if (!ring->bufs)
+			break;
+
+		for (b = 0; b < PRESTERA_SDMA_RX_DESC_PER_Q; b++) {
+			struct prestera_sdma_buf *buf = &ring->bufs[b];
+
+			if (buf->desc_dma)
+				dma_pool_free(sdma->desc_pool, buf->desc,
+					      buf->desc_dma);
+
+			if (!buf->skb)
+				continue;
+
+			if (buf->buf_dma != DMA_MAPPING_ERROR)
+				dma_unmap_single(sdma->sw->dev->dev,
+						 buf->buf_dma, buf->skb->len,
+						 DMA_FROM_DEVICE);
+			kfree_skb(buf->skb);
+		}
+	}
+}
+
+static int prestera_sdma_rx_init(struct prestera_sdma *sdma)
+{
+	int bnum = PRESTERA_SDMA_RX_DESC_PER_Q;
+	int qnum = PRESTERA_SDMA_RX_QUEUE_NUM;
+	int err;
+	int q;
+
+	/* disable all rx queues */
+	prestera_write(sdma->sw, PRESTERA_SDMA_RX_QUEUE_STATUS_REG,
+		       GENMASK(15, 8));
+
+	for (q = 0; q < qnum; q++) {
+		struct prestera_sdma_buf *head, *tail, *next, *prev;
+		struct prestera_rx_ring *ring = &sdma->rx_ring[q];
+
+		ring->bufs = kmalloc_objs(*head, bnum);
+		if (!ring->bufs)
+			return -ENOMEM;
+
+		ring->next_rx = 0;
+
+		tail = &ring->bufs[bnum - 1];
+		head = &ring->bufs[0];
+		next = head;
+		prev = next;
+
+		do {
+			err = prestera_sdma_buf_init(sdma, next);
+			if (err)
+				return err;
+
+			err = prestera_sdma_rx_skb_alloc(sdma, next);
+			if (err)
+				return err;
+
+			prestera_sdma_rx_desc_init(sdma, next->desc,
+						   next->buf_dma);
+
+			prestera_sdma_rx_desc_set_next(sdma, prev->desc,
+						       next->desc_dma);
+
+			prev = next;
+			next++;
+		} while (prev != tail);
+
+		/* join tail with head to make a circular list */
+		prestera_sdma_rx_desc_set_next(sdma, tail->desc, head->desc_dma);
+
+		prestera_write(sdma->sw, PRESTERA_SDMA_RX_QUEUE_DESC_REG(q),
+			       prestera_sdma_map(sdma, head->desc_dma));
+	}
+
+	/* make sure all rx descs are filled before enabling all rx queues */
+	wmb();
+
+	prestera_write(sdma->sw, PRESTERA_SDMA_RX_QUEUE_STATUS_REG,
+		       GENMASK(7, 0));
+
+	return 0;
+}
+
+static void prestera_sdma_tx_desc_init(struct prestera_sdma *sdma,
+				       struct prestera_sdma_desc *desc)
+{
+	desc->word1 = cpu_to_le32(PRESTERA_SDMA_TX_DESC_INIT);
+	desc->word2 = 0;
+}
+
+static void prestera_sdma_tx_desc_set_next(struct prestera_sdma *sdma,
+					   struct prestera_sdma_desc *desc,
+					   dma_addr_t next)
+{
+	desc->next = cpu_to_le32(prestera_sdma_map(sdma, next));
+}
+
+static void prestera_sdma_tx_desc_set_buf(struct prestera_sdma *sdma,
+					  struct prestera_sdma_desc *desc,
+					  dma_addr_t buf, size_t len)
+{
+	u32 word = le32_to_cpu(desc->word2);
+
+	u32p_replace_bits(&word, len + ETH_FCS_LEN, GENMASK(30, 16));
+
+	desc->buff = cpu_to_le32(prestera_sdma_map(sdma, buf));
+	desc->word2 = cpu_to_le32(word);
+}
+
+static void prestera_sdma_tx_desc_xmit(struct prestera_sdma_desc *desc)
+{
+	u32 word = le32_to_cpu(desc->word1);
+
+	word |= PRESTERA_SDMA_TX_DESC_DMA_OWN << 31;
+
+	/* make sure everything is written before enable xmit */
+	wmb();
+
+	desc->word1 = cpu_to_le32(word);
+}
+
+static int prestera_sdma_tx_buf_map(struct prestera_sdma *sdma,
+				    struct prestera_sdma_buf *buf,
+				    struct sk_buff *skb)
+{
+	struct device *dma_dev = sdma->sw->dev->dev;
+	dma_addr_t dma;
+
+	dma = dma_map_single(dma_dev, skb->data, skb->len, DMA_TO_DEVICE);
+	if (dma_mapping_error(dma_dev, dma))
+		return -ENOMEM;
+
+	buf->buf_dma = dma;
+	buf->skb = skb;
+
+	return 0;
+}
+
+static void prestera_sdma_tx_buf_unmap(struct prestera_sdma *sdma,
+				       struct prestera_sdma_buf *buf)
+{
+	struct device *dma_dev = sdma->sw->dev->dev;
+
+	dma_unmap_single(dma_dev, buf->buf_dma, buf->skb->len, DMA_TO_DEVICE);
+}
+
+static void prestera_sdma_tx_recycle_work_fn(struct work_struct *work)
+{
+	int bnum = PRESTERA_SDMA_TX_DESC_PER_Q;
+	struct prestera_tx_ring *tx_ring;
+	struct prestera_sdma *sdma;
+	int b;
+
+	sdma = container_of(work, struct prestera_sdma, tx_work);
+
+	tx_ring = &sdma->tx_ring;
+
+	for (b = 0; b < bnum; b++) {
+		struct prestera_sdma_buf *buf = &tx_ring->bufs[b];
+
+		if (!buf->is_used)
+			continue;
+
+		if (!PRESTERA_SDMA_TX_DESC_IS_SENT(buf->desc))
+			continue;
+
+		prestera_sdma_tx_buf_unmap(sdma, buf);
+		dev_consume_skb_any(buf->skb);
+		buf->skb = NULL;
+
+		/* make sure everything is cleaned up */
+		wmb();
+
+		buf->is_used = false;
+	}
+}
+
+static int prestera_sdma_tx_init(struct prestera_sdma *sdma)
+{
+	struct prestera_sdma_buf *head, *tail, *next, *prev;
+	struct prestera_tx_ring *tx_ring = &sdma->tx_ring;
+	int bnum = PRESTERA_SDMA_TX_DESC_PER_Q;
+	int err;
+
+	INIT_WORK(&sdma->tx_work, prestera_sdma_tx_recycle_work_fn);
+	spin_lock_init(&sdma->tx_lock);
+
+	tx_ring->bufs = kmalloc_objs(*head, bnum);
+	if (!tx_ring->bufs)
+		return -ENOMEM;
+
+	tail = &tx_ring->bufs[bnum - 1];
+	head = &tx_ring->bufs[0];
+	next = head;
+	prev = next;
+
+	tx_ring->max_burst = PRESTERA_SDMA_TX_MAX_BURST;
+	tx_ring->burst = tx_ring->max_burst;
+	tx_ring->next_tx = 0;
+
+	do {
+		err = prestera_sdma_buf_init(sdma, next);
+		if (err)
+			return err;
+
+		next->is_used = false;
+
+		prestera_sdma_tx_desc_init(sdma, next->desc);
+
+		prestera_sdma_tx_desc_set_next(sdma, prev->desc,
+					       next->desc_dma);
+
+		prev = next;
+		next++;
+	} while (prev != tail);
+
+	/* join tail with head to make a circular list */
+	prestera_sdma_tx_desc_set_next(sdma, tail->desc, head->desc_dma);
+
+	/* make sure descriptors are written */
+	wmb();
+
+	prestera_write(sdma->sw, PRESTERA_SDMA_TX_QUEUE_DESC_REG,
+		       prestera_sdma_map(sdma, head->desc_dma));
+
+	return 0;
+}
+
+static void prestera_sdma_tx_fini(struct prestera_sdma *sdma)
+{
+	struct prestera_tx_ring *ring = &sdma->tx_ring;
+	int bnum = PRESTERA_SDMA_TX_DESC_PER_Q;
+	int b;
+
+	cancel_work_sync(&sdma->tx_work);
+
+	if (!ring->bufs)
+		return;
+
+	for (b = 0; b < bnum; b++) {
+		struct prestera_sdma_buf *buf = &ring->bufs[b];
+
+		if (buf->desc)
+			dma_pool_free(sdma->desc_pool, buf->desc,
+				      buf->desc_dma);
+
+		if (!buf->skb)
+			continue;
+
+		dma_unmap_single(sdma->sw->dev->dev, buf->buf_dma,
+				 buf->skb->len, DMA_TO_DEVICE);
+
+		dev_consume_skb_any(buf->skb);
+	}
+}
+
+static void prestera_rxtx_handle_event(struct prestera_switch *sw,
+				       struct prestera_event *evt,
+				       void *arg)
+{
+	struct prestera_sdma *sdma = arg;
+
+	if (evt->id != PRESTERA_RXTX_EVENT_RCV_PKT)
+		return;
+
+	prestera_write(sdma->sw, PRESTERA_SDMA_RX_INTR_MASK_REG, 0);
+	napi_schedule(&sdma->rx_napi);
+}
+
+static int prestera_sdma_switch_init(struct prestera_switch *sw)
+{
+	struct prestera_sdma *sdma = &sw->rxtx->sdma;
+	struct device *dev = sw->dev->dev;
+	struct prestera_rxtx_params p;
+	int err;
+
+	p.use_sdma = true;
+
+	err = prestera_hw_rxtx_init(sw, &p);
+	if (err) {
+		dev_err(dev, "failed to init rxtx by hw\n");
+		return err;
+	}
+
+	sdma->dma_mask = dma_get_mask(dev);
+	sdma->map_addr = p.map_addr;
+	sdma->sw = sw;
+
+	sdma->desc_pool = dma_pool_create("desc_pool", dev,
+					  sizeof(struct prestera_sdma_desc),
+					  16, 0);
+	if (!sdma->desc_pool)
+		return -ENOMEM;
+
+	err = prestera_sdma_rx_init(sdma);
+	if (err) {
+		dev_err(dev, "failed to init rx ring\n");
+		goto err_rx_init;
+	}
+
+	err = prestera_sdma_tx_init(sdma);
+	if (err) {
+		dev_err(dev, "failed to init tx ring\n");
+		goto err_tx_init;
+	}
+
+	err = prestera_hw_event_handler_register(sw, PRESTERA_EVENT_TYPE_RXTX,
+						 prestera_rxtx_handle_event,
+						 sdma);
+	if (err)
+		goto err_evt_register;
+
+	sdma->napi_dev = alloc_netdev_dummy(0);
+	if (!sdma->napi_dev) {
+		dev_err(dev, "not able to initialize dummy device\n");
+		err = -ENOMEM;
+		goto err_alloc_dummy;
+	}
+
+	netif_napi_add(sdma->napi_dev, &sdma->rx_napi, prestera_sdma_rx_poll);
+	napi_enable(&sdma->rx_napi);
+
+	return 0;
+
+err_alloc_dummy:
+	prestera_hw_event_handler_unregister(sw, PRESTERA_EVENT_TYPE_RXTX,
+					     prestera_rxtx_handle_event);
+err_evt_register:
+err_tx_init:
+	prestera_sdma_tx_fini(sdma);
+err_rx_init:
+	prestera_sdma_rx_fini(sdma);
+
+	dma_pool_destroy(sdma->desc_pool);
+	return err;
+}
+
+static void prestera_sdma_switch_fini(struct prestera_switch *sw)
+{
+	struct prestera_sdma *sdma = &sw->rxtx->sdma;
+
+	napi_disable(&sdma->rx_napi);
+	netif_napi_del(&sdma->rx_napi);
+	free_netdev(sdma->napi_dev);
+	prestera_hw_event_handler_unregister(sw, PRESTERA_EVENT_TYPE_RXTX,
+					     prestera_rxtx_handle_event);
+	prestera_sdma_tx_fini(sdma);
+	prestera_sdma_rx_fini(sdma);
+	dma_pool_destroy(sdma->desc_pool);
+}
+
+static bool prestera_sdma_is_ready(struct prestera_sdma *sdma)
+{
+	return !(prestera_read(sdma->sw, PRESTERA_SDMA_TX_QUEUE_START_REG) & 1);
+}
+
+static int prestera_sdma_tx_wait(struct prestera_sdma *sdma,
+				 struct prestera_tx_ring *tx_ring)
+{
+	int tx_wait_num = PRESTERA_SDMA_WAIT_MUL * tx_ring->max_burst;
+
+	do {
+		if (prestera_sdma_is_ready(sdma))
+			return 0;
+
+		udelay(1);
+	} while (--tx_wait_num);
+
+	return -EBUSY;
+}
+
+static void prestera_sdma_tx_start(struct prestera_sdma *sdma)
+{
+	prestera_write(sdma->sw, PRESTERA_SDMA_TX_QUEUE_START_REG, 1);
+	schedule_work(&sdma->tx_work);
+}
+
+static netdev_tx_t prestera_sdma_xmit(struct prestera_sdma *sdma,
+				      struct sk_buff *skb)
+{
+	struct device *dma_dev = sdma->sw->dev->dev;
+	struct net_device *dev = skb->dev;
+	struct prestera_tx_ring *tx_ring;
+	struct prestera_sdma_buf *buf;
+	int err;
+
+	spin_lock(&sdma->tx_lock);
+
+	tx_ring = &sdma->tx_ring;
+
+	buf = &tx_ring->bufs[tx_ring->next_tx];
+	if (buf->is_used) {
+		schedule_work(&sdma->tx_work);
+		goto drop_skb;
+	}
+
+	if (unlikely(eth_skb_pad(skb)))
+		goto drop_skb_nofree;
+
+	err = prestera_sdma_tx_buf_map(sdma, buf, skb);
+	if (err)
+		goto drop_skb;
+
+	prestera_sdma_tx_desc_set_buf(sdma, buf->desc, buf->buf_dma, skb->len);
+
+	dma_sync_single_for_device(dma_dev, buf->buf_dma, skb->len,
+				   DMA_TO_DEVICE);
+
+	if (tx_ring->burst) {
+		tx_ring->burst--;
+	} else {
+		tx_ring->burst = tx_ring->max_burst;
+
+		err = prestera_sdma_tx_wait(sdma, tx_ring);
+		if (err)
+			goto drop_skb_unmap;
+	}
+
+	tx_ring->next_tx = (tx_ring->next_tx + 1) % PRESTERA_SDMA_TX_DESC_PER_Q;
+	prestera_sdma_tx_desc_xmit(buf->desc);
+	buf->is_used = true;
+
+	prestera_sdma_tx_start(sdma);
+
+	goto tx_done;
+
+drop_skb_unmap:
+	prestera_sdma_tx_buf_unmap(sdma, buf);
+drop_skb:
+	dev_consume_skb_any(skb);
+drop_skb_nofree:
+	dev->stats.tx_dropped++;
+tx_done:
+	spin_unlock(&sdma->tx_lock);
+	return NETDEV_TX_OK;
+}
+
+int prestera_rxtx_switch_init(struct prestera_switch *sw)
+{
+	struct prestera_rxtx *rxtx;
+	int err;
+
+	rxtx = kzalloc_obj(*rxtx);
+	if (!rxtx)
+		return -ENOMEM;
+
+	sw->rxtx = rxtx;
+
+	err = prestera_sdma_switch_init(sw);
+	if (err)
+		kfree(rxtx);
+
+	return err;
+}
+
+void prestera_rxtx_switch_fini(struct prestera_switch *sw)
+{
+	prestera_sdma_switch_fini(sw);
+	kfree(sw->rxtx);
+}
+
+int prestera_rxtx_port_init(struct prestera_port *port)
+{
+	port->dev->needed_headroom = PRESTERA_DSA_HLEN;
+	return 0;
+}
+
+netdev_tx_t prestera_rxtx_xmit(struct prestera_port *port, struct sk_buff *skb)
+{
+	struct prestera_dsa dsa;
+
+	dsa.hw_dev_num = port->dev_id;
+	dsa.port_num = port->hw_id;
+
+	if (skb_cow_head(skb, PRESTERA_DSA_HLEN) < 0)
+		return NET_XMIT_DROP;
+
+	skb_push(skb, PRESTERA_DSA_HLEN);
+	memmove(skb->data, skb->data + PRESTERA_DSA_HLEN, 2 * ETH_ALEN);
+
+	if (prestera_dsa_build(&dsa, skb->data + 2 * ETH_ALEN) != 0)
+		return NET_XMIT_DROP;
+
+	return prestera_sdma_xmit(&port->sw->rxtx->sdma, skb);
+}

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_rxtx.h

@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */
+/* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved. */
+
+#ifndef _PRESTERA_RXTX_H_
+#define _PRESTERA_RXTX_H_
+
+#include <linux/netdevice.h>
+
+struct prestera_switch;
+struct prestera_port;
+
+int prestera_rxtx_switch_init(struct prestera_switch *sw);
+void prestera_rxtx_switch_fini(struct prestera_switch *sw);
+
+int prestera_rxtx_port_init(struct prestera_port *port);
+
+netdev_tx_t prestera_rxtx_xmit(struct prestera_port *port, struct sk_buff *skb);
+
+#endif /* _PRESTERA_RXTX_H_ */

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_span.c

@@ -0,0 +1,191 @@
+// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
+/* Copyright (c) 2020 Marvell International Ltd. All rights reserved */
+
+#include <linux/kernel.h>
+#include <linux/list.h>
+
+#include "prestera.h"
+#include "prestera_hw.h"
+#include "prestera_acl.h"
+#include "prestera_flow.h"
+#include "prestera_span.h"
+
+struct prestera_span_entry {
+	struct list_head list;
+	struct prestera_port *port;
+	refcount_t ref_count;
+	u8 id;
+};
+
+struct prestera_span {
+	struct prestera_switch *sw;
+	struct list_head entries;
+};
+
+static struct prestera_span_entry *
+prestera_span_entry_create(struct prestera_port *port, u8 span_id)
+{
+	struct prestera_span_entry *entry;
+
+	entry = kzalloc_obj(*entry);
+	if (!entry)
+		return ERR_PTR(-ENOMEM);
+
+	refcount_set(&entry->ref_count, 1);
+	entry->port = port;
+	entry->id = span_id;
+	list_add_tail(&entry->list, &port->sw->span->entries);
+
+	return entry;
+}
+
+static void prestera_span_entry_del(struct prestera_span_entry *entry)
+{
+	list_del(&entry->list);
+	kfree(entry);
+}
+
+static struct prestera_span_entry *
+prestera_span_entry_find_by_id(struct prestera_span *span, u8 span_id)
+{
+	struct prestera_span_entry *entry;
+
+	list_for_each_entry(entry, &span->entries, list) {
+		if (entry->id == span_id)
+			return entry;
+	}
+
+	return NULL;
+}
+
+static struct prestera_span_entry *
+prestera_span_entry_find_by_port(struct prestera_span *span,
+				 struct prestera_port *port)
+{
+	struct prestera_span_entry *entry;
+
+	list_for_each_entry(entry, &span->entries, list) {
+		if (entry->port == port)
+			return entry;
+	}
+
+	return NULL;
+}
+
+static int prestera_span_get(struct prestera_port *port, u8 *span_id)
+{
+	u8 new_span_id;
+	struct prestera_switch *sw = port->sw;
+	struct prestera_span_entry *entry;
+	int err;
+
+	entry = prestera_span_entry_find_by_port(sw->span, port);
+	if (entry) {
+		refcount_inc(&entry->ref_count);
+		*span_id = entry->id;
+		return 0;
+	}
+
+	err = prestera_hw_span_get(port, &new_span_id);
+	if (err)
+		return err;
+
+	entry = prestera_span_entry_create(port, new_span_id);
+	if (IS_ERR(entry)) {
+		prestera_hw_span_release(sw, new_span_id);
+		return PTR_ERR(entry);
+	}
+
+	*span_id = new_span_id;
+	return 0;
+}
+
+static int prestera_span_put(struct prestera_switch *sw, u8 span_id)
+{
+	struct prestera_span_entry *entry;
+	int err;
+
+	entry = prestera_span_entry_find_by_id(sw->span, span_id);
+	if (!entry)
+		return -ENOENT;
+
+	if (!refcount_dec_and_test(&entry->ref_count))
+		return 0;
+
+	err = prestera_hw_span_release(sw, span_id);
+	if (err)
+		return err;
+
+	prestera_span_entry_del(entry);
+	return 0;
+}
+
+int prestera_span_rule_add(struct prestera_flow_block_binding *binding,
+			   struct prestera_port *to_port,
+			   bool ingress)
+{
+	struct prestera_switch *sw = binding->port->sw;
+	u8 span_id;
+	int err;
+
+	if (binding->span_id != PRESTERA_SPAN_INVALID_ID)
+		/* port already in mirroring */
+		return -EEXIST;
+
+	err = prestera_span_get(to_port, &span_id);
+	if (err)
+		return err;
+
+	err = prestera_hw_span_bind(binding->port, span_id, ingress);
+	if (err) {
+		prestera_span_put(sw, span_id);
+		return err;
+	}
+
+	binding->span_id = span_id;
+	return 0;
+}
+
+int prestera_span_rule_del(struct prestera_flow_block_binding *binding,
+			   bool ingress)
+{
+	int err;
+
+	if (binding->span_id == PRESTERA_SPAN_INVALID_ID)
+		return -ENOENT;
+
+	err = prestera_hw_span_unbind(binding->port, ingress);
+	if (err)
+		return err;
+
+	err = prestera_span_put(binding->port->sw, binding->span_id);
+	if (err)
+		return err;
+
+	binding->span_id = PRESTERA_SPAN_INVALID_ID;
+	return 0;
+}
+
+int prestera_span_init(struct prestera_switch *sw)
+{
+	struct prestera_span *span;
+
+	span = kzalloc_obj(*span);
+	if (!span)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&span->entries);
+
+	sw->span = span;
+	span->sw = sw;
+
+	return 0;
+}
+
+void prestera_span_fini(struct prestera_switch *sw)
+{
+	struct prestera_span *span = sw->span;
+
+	WARN_ON(!list_empty(&span->entries));
+	kfree(span);
+}

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_span.h

@@ -0,0 +1,24 @@
+/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */
+/* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved. */
+
+#ifndef _PRESTERA_SPAN_H_
+#define _PRESTERA_SPAN_H_
+
+#include <net/pkt_cls.h>
+
+#define PRESTERA_SPAN_INVALID_ID -1
+
+struct prestera_port;
+struct prestera_switch;
+struct prestera_flow_block_binding;
+
+int prestera_span_init(struct prestera_switch *sw);
+void prestera_span_fini(struct prestera_switch *sw);
+
+int prestera_span_rule_add(struct prestera_flow_block_binding *binding,
+			   struct prestera_port *to_port,
+			   bool ingress);
+int prestera_span_rule_del(struct prestera_flow_block_binding *binding,
+			   bool ingress);
+
+#endif /* _PRESTERA_SPAN_H_ */

linux-7.2.3/drivers/net/ethernet/marvell/prestera/prestera_switchdev.h

@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0 */
+/* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved. */
+
+#ifndef _PRESTERA_SWITCHDEV_H_
+#define _PRESTERA_SWITCHDEV_H_
+
+int prestera_switchdev_init(struct prestera_switch *sw);
+void prestera_switchdev_fini(struct prestera_switch *sw);
+
+int prestera_bridge_port_join(struct net_device *br_dev,
+			      struct prestera_port *port,
+			      struct netlink_ext_ack *extack);
+
+void prestera_bridge_port_leave(struct net_device *br_dev,
+				struct prestera_port *port);
+
+#endif /* _PRESTERA_SWITCHDEV_H_ */