enetc_qos.c

// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
/* Copyright 2019 NXP */

#include "enetc.h"

#include <net/pkt_sched.h>
#include <linux/math64.h>
#include <linux/refcount.h>
#include <net/pkt_cls.h>
#include <net/tc_act/tc_gate.h>

static u16 enetc_get_max_gcl_len(struct enetc_hw *hw)
{
	return enetc_rd(hw, ENETC_QBV_PTGCAPR_OFFSET)
		& ENETC_QBV_MAX_GCL_LEN_MASK;
}

void enetc_sched_speed_set(struct net_device *ndev)
{
	struct enetc_ndev_priv *priv = netdev_priv(ndev);
	struct phy_device *phydev = ndev->phydev;
	u32 old_speed = priv->speed;
	u32 speed, pspeed;

	if (phydev->speed == old_speed)
		return;

	speed = phydev->speed;
	switch (speed) {
	case SPEED_1000:
		pspeed = ENETC_PMR_PSPEED_1000M;
		break;
	case SPEED_2500:
		pspeed = ENETC_PMR_PSPEED_2500M;
		break;
	case SPEED_100:
		pspeed = ENETC_PMR_PSPEED_100M;
		break;
	case SPEED_10:
	default:
		pspeed = ENETC_PMR_PSPEED_10M;
	}

	priv->speed = speed;
	enetc_port_wr(&priv->si->hw, ENETC_PMR,
		      (enetc_port_rd(&priv->si->hw, ENETC_PMR)
		      & (~ENETC_PMR_PSPEED_MASK))
		      | pspeed);
}

static int enetc_setup_taprio(struct net_device *ndev,
			      struct tc_taprio_qopt_offload *admin_conf)
{
	struct enetc_ndev_priv *priv = netdev_priv(ndev);
	struct enetc_cbd cbd = {.cmd = 0};
	struct tgs_gcl_conf *gcl_config;
	struct tgs_gcl_data *gcl_data;
	struct gce *gce;
	dma_addr_t dma;
	u16 data_size;
	u16 gcl_len;
	u32 tge;
	int err;
	int i;

	if (admin_conf->num_entries > enetc_get_max_gcl_len(&priv->si->hw))
		return -EINVAL;
	gcl_len = admin_conf->num_entries;

	tge = enetc_rd(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET);
	if (!admin_conf->enable) {
		enetc_wr(&priv->si->hw,
			 ENETC_QBV_PTGCR_OFFSET,
			 tge & (~ENETC_QBV_TGE));
		return 0;
	}

	if (admin_conf->cycle_time > U32_MAX ||
	    admin_conf->cycle_time_extension > U32_MAX)
		return -EINVAL;

	/* Configure the (administrative) gate control list using the
	 * control BD descriptor.
	 */
	gcl_config = &cbd.gcl_conf;

	data_size = struct_size(gcl_data, entry, gcl_len);
	gcl_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
	if (!gcl_data)
		return -ENOMEM;

	gce = (struct gce *)(gcl_data + 1);

	/* Set all gates open as default */
	gcl_config->atc = 0xff;
	gcl_config->acl_len = cpu_to_le16(gcl_len);

	if (!admin_conf->base_time) {
		gcl_data->btl =
			cpu_to_le32(enetc_rd(&priv->si->hw, ENETC_SICTR0));
		gcl_data->bth =
			cpu_to_le32(enetc_rd(&priv->si->hw, ENETC_SICTR1));
	} else {
		gcl_data->btl =
			cpu_to_le32(lower_32_bits(admin_conf->base_time));
		gcl_data->bth =
			cpu_to_le32(upper_32_bits(admin_conf->base_time));
	}

	gcl_data->ct = cpu_to_le32(admin_conf->cycle_time);
	gcl_data->cte = cpu_to_le32(admin_conf->cycle_time_extension);

	for (i = 0; i < gcl_len; i++) {
		struct tc_taprio_sched_entry *temp_entry;
		struct gce *temp_gce = gce + i;

		temp_entry = &admin_conf->entries[i];

		temp_gce->gate = (u8)temp_entry->gate_mask;
		temp_gce->period = cpu_to_le32(temp_entry->interval);
	}

	cbd.length = cpu_to_le16(data_size);
	cbd.status_flags = 0;

	dma = dma_map_single(&priv->si->pdev->dev, gcl_data,
			     data_size, DMA_TO_DEVICE);
	if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
		netdev_err(priv->si->ndev, "DMA mapping failed!\n");
		kfree(gcl_data);
		return -ENOMEM;
	}

	cbd.addr[0] = lower_32_bits(dma);
	cbd.addr[1] = upper_32_bits(dma);
	cbd.cls = BDCR_CMD_PORT_GCL;
	cbd.status_flags = 0;

	enetc_wr(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET,
		 tge | ENETC_QBV_TGE);

	err = enetc_send_cmd(priv->si, &cbd);
	if (err)
		enetc_wr(&priv->si->hw,
			 ENETC_QBV_PTGCR_OFFSET,
			 tge & (~ENETC_QBV_TGE));

	dma_unmap_single(&priv->si->pdev->dev, dma, data_size, DMA_TO_DEVICE);
	kfree(gcl_data);

	return err;
}

int enetc_setup_tc_taprio(struct net_device *ndev, void *type_data)
{
	struct tc_taprio_qopt_offload *taprio = type_data;
	struct enetc_ndev_priv *priv = netdev_priv(ndev);
	int err;
	int i;

	/* TSD and Qbv are mutually exclusive in hardware */
	for (i = 0; i < priv->num_tx_rings; i++)
		if (priv->tx_ring[i]->tsd_enable)
			return -EBUSY;

	for (i = 0; i < priv->num_tx_rings; i++)
		enetc_set_bdr_prio(&priv->si->hw,
				   priv->tx_ring[i]->index,
				   taprio->enable ? i : 0);

	err = enetc_setup_taprio(ndev, taprio);

	if (err)
		for (i = 0; i < priv->num_tx_rings; i++)
			enetc_set_bdr_prio(&priv->si->hw,
					   priv->tx_ring[i]->index,
					   taprio->enable ? 0 : i);

	return err;
}

static u32 enetc_get_cbs_enable(struct enetc_hw *hw, u8 tc)
{
	return enetc_port_rd(hw, ENETC_PTCCBSR0(tc)) & ENETC_CBSE;
}

static u8 enetc_get_cbs_bw(struct enetc_hw *hw, u8 tc)
{
	return enetc_port_rd(hw, ENETC_PTCCBSR0(tc)) & ENETC_CBS_BW_MASK;
}

int enetc_setup_tc_cbs(struct net_device *ndev, void *type_data)
{
	struct enetc_ndev_priv *priv = netdev_priv(ndev);
	struct tc_cbs_qopt_offload *cbs = type_data;
	u32 port_transmit_rate = priv->speed;
	u8 tc_nums = netdev_get_num_tc(ndev);
	struct enetc_si *si = priv->si;
	u32 hi_credit_bit, hi_credit_reg;
	u32 max_interference_size;
	u32 port_frame_max_size;
	u8 tc = cbs->queue;
	u8 prio_top, prio_next;
	int bw_sum = 0;
	u8 bw;

	prio_top = netdev_get_prio_tc_map(ndev, tc_nums - 1);
	prio_next = netdev_get_prio_tc_map(ndev, tc_nums - 2);

	/* Support highest prio and second prio tc in cbs mode */
	if (tc != prio_top && tc != prio_next)
		return -EOPNOTSUPP;

	if (!cbs->enable) {
		/* Make sure the other TC that are numerically
		 * lower than this TC have been disabled.
		 */
		if (tc == prio_top &&
		    enetc_get_cbs_enable(&si->hw, prio_next)) {
			dev_err(&ndev->dev,
				"Disable TC%d before disable TC%d\n",
				prio_next, tc);
			return -EINVAL;
		}

		enetc_port_wr(&si->hw, ENETC_PTCCBSR1(tc), 0);
		enetc_port_wr(&si->hw, ENETC_PTCCBSR0(tc), 0);

		return 0;
	}

	if (cbs->idleslope - cbs->sendslope != port_transmit_rate * 1000L ||
	    cbs->idleslope < 0 || cbs->sendslope > 0)
		return -EOPNOTSUPP;

	port_frame_max_size = ndev->mtu + VLAN_ETH_HLEN + ETH_FCS_LEN;

	bw = cbs->idleslope / (port_transmit_rate * 10UL);

	/* Make sure the other TC that are numerically
	 * higher than this TC have been enabled.
	 */
	if (tc == prio_next) {
		if (!enetc_get_cbs_enable(&si->hw, prio_top)) {
			dev_err(&ndev->dev,
				"Enable TC%d first before enable TC%d\n",
				prio_top, prio_next);
			return -EINVAL;
		}
		bw_sum += enetc_get_cbs_bw(&si->hw, prio_top);
	}

	if (bw_sum + bw >= 100) {
		dev_err(&ndev->dev,
			"The sum of all CBS Bandwidth can't exceed 100\n");
		return -EINVAL;
	}

	enetc_port_rd(&si->hw, ENETC_PTCMSDUR(tc));

	/* For top prio TC, the max_interfrence_size is maxSizedFrame.
	 *
	 * For next prio TC, the max_interfrence_size is calculated as below:
	 *
	 *      max_interference_size = M0 + Ma + Ra * M0 / (R0 - Ra)
	 *
	 *	- RA: idleSlope for AVB Class A
	 *	- R0: port transmit rate
	 *	- M0: maximum sized frame for the port
	 *	- MA: maximum sized frame for AVB Class A
	 */

	if (tc == prio_top) {
		max_interference_size = port_frame_max_size * 8;
	} else {
		u32 m0, ma, r0, ra;

		m0 = port_frame_max_size * 8;
		ma = enetc_port_rd(&si->hw, ENETC_PTCMSDUR(prio_top)) * 8;
		ra = enetc_get_cbs_bw(&si->hw, prio_top) *
			port_transmit_rate * 10000ULL;
		r0 = port_transmit_rate * 1000000ULL;
		max_interference_size = m0 + ma +
			(u32)div_u64((u64)ra * m0, r0 - ra);
	}

	/* hiCredit bits calculate by:
	 *
	 * maxSizedFrame * (idleSlope/portTxRate)
	 */
	hi_credit_bit = max_interference_size * bw / 100;

	/* hiCredit bits to hiCredit register need to calculated as:
	 *
	 * (enetClockFrequency / portTransmitRate) * 100
	 */
	hi_credit_reg = (u32)div_u64((ENETC_CLK * 100ULL) * hi_credit_bit,
				     port_transmit_rate * 1000000ULL);

	enetc_port_wr(&si->hw, ENETC_PTCCBSR1(tc), hi_credit_reg);

	/* Set bw register and enable this traffic class */
	enetc_port_wr(&si->hw, ENETC_PTCCBSR0(tc), bw | ENETC_CBSE);

	return 0;
}

int enetc_setup_tc_txtime(struct net_device *ndev, void *type_data)
{
	struct enetc_ndev_priv *priv = netdev_priv(ndev);
	struct tc_etf_qopt_offload *qopt = type_data;
	u8 tc_nums = netdev_get_num_tc(ndev);
	int tc;

	if (!tc_nums)
		return -EOPNOTSUPP;

	tc = qopt->queue;

	if (tc < 0 || tc >= priv->num_tx_rings)
		return -EINVAL;

	/* Do not support TXSTART and TX CSUM offload simutaniously */
	if (ndev->features & NETIF_F_CSUM_MASK)
		return -EBUSY;

	/* TSD and Qbv are mutually exclusive in hardware */
	if (enetc_rd(&priv->si->hw, ENETC_QBV_PTGCR_OFFSET) & ENETC_QBV_TGE)
		return -EBUSY;

	priv->tx_ring[tc]->tsd_enable = qopt->enable;
	enetc_port_wr(&priv->si->hw, ENETC_PTCTSDR(tc),
		      qopt->enable ? ENETC_TSDE : 0);

	return 0;
}

enum streamid_type {
	STREAMID_TYPE_RESERVED = 0,
	STREAMID_TYPE_NULL,
	STREAMID_TYPE_SMAC,
};

enum streamid_vlan_tagged {
	STREAMID_VLAN_RESERVED = 0,
	STREAMID_VLAN_TAGGED,
	STREAMID_VLAN_UNTAGGED,
	STREAMID_VLAN_ALL,
};

#define ENETC_PSFP_WILDCARD -1
#define HANDLE_OFFSET 100

enum forward_type {
	FILTER_ACTION_TYPE_PSFP = BIT(0),
	FILTER_ACTION_TYPE_ACL = BIT(1),
	FILTER_ACTION_TYPE_BOTH = GENMASK(1, 0),
};

/* This is for limit output type for input actions */
struct actions_fwd {
	u64 actions;
	u64 keys;	/* include the must needed keys */
	enum forward_type output;
};

struct psfp_streamfilter_counters {
	u64 matching_frames_count;
	u64 passing_frames_count;
	u64 not_passing_frames_count;
	u64 passing_sdu_count;
	u64 not_passing_sdu_count;
	u64 red_frames_count;
};

struct enetc_streamid {
	u32 index;
	union {
		u8 src_mac[6];
		u8 dst_mac[6];
	};
	u8 filtertype;
	u16 vid;
	u8 tagged;
	s32 handle;
};

struct enetc_psfp_filter {
	u32 index;
	s32 handle;
	s8 prio;
	u32 gate_id;
	s32 meter_id;
	refcount_t refcount;
	struct hlist_node node;
};

struct enetc_psfp_gate {
	u32 index;
	s8 init_ipv;
	u64 basetime;
	u64 cycletime;
	u64 cycletimext;
	u32 num_entries;
	refcount_t refcount;
	struct hlist_node node;
	struct action_gate_entry entries[0];
};

struct enetc_stream_filter {
	struct enetc_streamid sid;
	u32 sfi_index;
	u32 sgi_index;
	struct flow_stats stats;
	struct hlist_node node;
};

struct enetc_psfp {
	unsigned long dev_bitmap;
	unsigned long *psfp_sfi_bitmap;
	struct hlist_head stream_list;
	struct hlist_head psfp_filter_list;
	struct hlist_head psfp_gate_list;
	spinlock_t psfp_lock; /* spinlock for the struct enetc_psfp r/w */
};

static struct actions_fwd enetc_act_fwd[] = {
	{
		BIT(FLOW_ACTION_GATE),
		BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS),
		FILTER_ACTION_TYPE_PSFP
	},
	/* example for ACL actions */
	{
		BIT(FLOW_ACTION_DROP),
		0,
		FILTER_ACTION_TYPE_ACL
	}
};

static struct enetc_psfp epsfp = {
	.psfp_sfi_bitmap = NULL,
};

static LIST_HEAD(enetc_block_cb_list);

static inline int enetc_get_port(struct enetc_ndev_priv *priv)
{
	return priv->si->pdev->devfn & 0x7;
}

/* Stream Identity Entry Set Descriptor */
static int enetc_streamid_hw_set(struct enetc_ndev_priv *priv,
				 struct enetc_streamid *sid,
				 u8 enable)
{
	struct enetc_cbd cbd = {.cmd = 0};
	struct streamid_data *si_data;
	struct streamid_conf *si_conf;
	u16 data_size;
	dma_addr_t dma;
	int err;

	if (sid->index >= priv->psfp_cap.max_streamid)
		return -EINVAL;

	if (sid->filtertype != STREAMID_TYPE_NULL &&
	    sid->filtertype != STREAMID_TYPE_SMAC)
		return -EOPNOTSUPP;

	/* Disable operation before enable */
	cbd.index = cpu_to_le16((u16)sid->index);
	cbd.cls = BDCR_CMD_STREAM_IDENTIFY;
	cbd.status_flags = 0;

	data_size = sizeof(struct streamid_data);
	si_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
	cbd.length = cpu_to_le16(data_size);

	dma = dma_map_single(&priv->si->pdev->dev, si_data,
			     data_size, DMA_FROM_DEVICE);
	if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
		netdev_err(priv->si->ndev, "DMA mapping failed!\n");
		kfree(si_data);
		return -ENOMEM;
	}

	cbd.addr[0] = lower_32_bits(dma);
	cbd.addr[1] = upper_32_bits(dma);
	memset(si_data->dmac, 0xff, ETH_ALEN);
	si_data->vid_vidm_tg =
		cpu_to_le16(ENETC_CBDR_SID_VID_MASK
			    + ((0x3 << 14) | ENETC_CBDR_SID_VIDM));

	si_conf = &cbd.sid_set;
	/* Only one port supported for one entry, set itself */
	si_conf->iports = 1 << enetc_get_port(priv);
	si_conf->id_type = 1;
	si_conf->oui[2] = 0x0;
	si_conf->oui[1] = 0x80;
	si_conf->oui[0] = 0xC2;

	err = enetc_send_cmd(priv->si, &cbd);
	if (err)
		return -EINVAL;

	if (!enable) {
		kfree(si_data);
		return 0;
	}

	/* Enable the entry overwrite again incase space flushed by hardware */
	memset(&cbd, 0, sizeof(cbd));

	cbd.index = cpu_to_le16((u16)sid->index);
	cbd.cmd = 0;
	cbd.cls = BDCR_CMD_STREAM_IDENTIFY;
	cbd.status_flags = 0;

	si_conf->en = 0x80;
	si_conf->stream_handle = cpu_to_le32(sid->handle);
	si_conf->iports = 1 << enetc_get_port(priv);
	si_conf->id_type = sid->filtertype;
	si_conf->oui[2] = 0x0;
	si_conf->oui[1] = 0x80;
	si_conf->oui[0] = 0xC2;

	memset(si_data, 0, data_size);

	cbd.length = cpu_to_le16(data_size);

	cbd.addr[0] = lower_32_bits(dma);
	cbd.addr[1] = upper_32_bits(dma);

	/* VIDM default to be 1.
	 * VID Match. If set (b1) then the VID must match, otherwise
	 * any VID is considered a match. VIDM setting is only used
	 * when TG is set to b01.
	 */
	if (si_conf->id_type == STREAMID_TYPE_NULL) {
		ether_addr_copy(si_data->dmac, sid->dst_mac);
		si_data->vid_vidm_tg =
		cpu_to_le16((sid->vid & ENETC_CBDR_SID_VID_MASK) +
			    ((((u16)(sid->tagged) & 0x3) << 14)
			     | ENETC_CBDR_SID_VIDM));
	} else if (si_conf->id_type == STREAMID_TYPE_SMAC) {
		ether_addr_copy(si_data->smac, sid->src_mac);
		si_data->vid_vidm_tg =
		cpu_to_le16((sid->vid & ENETC_CBDR_SID_VID_MASK) +
			    ((((u16)(sid->tagged) & 0x3) << 14)
			     | ENETC_CBDR_SID_VIDM));
	}

	err = enetc_send_cmd(priv->si, &cbd);
	kfree(si_data);

	return err;
}

/* Stream Filter Instance Set Descriptor */
static int enetc_streamfilter_hw_set(struct enetc_ndev_priv *priv,
				     struct enetc_psfp_filter *sfi,
				     u8 enable)
{
	struct enetc_cbd cbd = {.cmd = 0};
	struct sfi_conf *sfi_config;

	cbd.index = cpu_to_le16(sfi->index);
	cbd.cls = BDCR_CMD_STREAM_FILTER;
	cbd.status_flags = 0x80;
	cbd.length = cpu_to_le16(1);

	sfi_config = &cbd.sfi_conf;
	if (!enable)
		goto exit;

	sfi_config->en = 0x80;

	if (sfi->handle >= 0) {
		sfi_config->stream_handle =
			cpu_to_le32(sfi->handle);
		sfi_config->sthm |= 0x80;
	}

	sfi_config->sg_inst_table_index = cpu_to_le16(sfi->gate_id);
	sfi_config->input_ports = 1 << enetc_get_port(priv);

	/* The priority value which may be matched against the
	 * frame’s priority value to determine a match for this entry.
	 */
	if (sfi->prio >= 0)
		sfi_config->multi |= (sfi->prio & 0x7) | 0x8;

	/* Filter Type. Identifies the contents of the MSDU/FM_INST_INDEX
	 * field as being either an MSDU value or an index into the Flow
	 * Meter Instance table.
	 * TODO: no limit max sdu
	 */

	if (sfi->meter_id >= 0) {
		sfi_config->fm_inst_table_index = cpu_to_le16(sfi->meter_id);
		sfi_config->multi |= 0x80;
	}

exit:
	return enetc_send_cmd(priv->si, &cbd);
}

static int enetc_streamcounter_hw_get(struct enetc_ndev_priv *priv,
				      u32 index,
				      struct psfp_streamfilter_counters *cnt)
{
	struct enetc_cbd cbd = { .cmd = 2 };
	struct sfi_counter_data *data_buf;
	dma_addr_t dma;
	u16 data_size;
	int err;

	cbd.index = cpu_to_le16((u16)index);
	cbd.cmd = 2;
	cbd.cls = BDCR_CMD_STREAM_FILTER;
	cbd.status_flags = 0;

	data_size = sizeof(struct sfi_counter_data);
	data_buf = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
	if (!data_buf)
		return -ENOMEM;

	dma = dma_map_single(&priv->si->pdev->dev, data_buf,
			     data_size, DMA_FROM_DEVICE);
	if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
		netdev_err(priv->si->ndev, "DMA mapping failed!\n");
		err = -ENOMEM;
		goto exit;
	}
	cbd.addr[0] = lower_32_bits(dma);
	cbd.addr[1] = upper_32_bits(dma);

	cbd.length = cpu_to_le16(data_size);

	err = enetc_send_cmd(priv->si, &cbd);
	if (err)
		goto exit;

	cnt->matching_frames_count =
			((u64)le32_to_cpu(data_buf->matchh) << 32)
			+ data_buf->matchl;

	cnt->not_passing_sdu_count =
			((u64)le32_to_cpu(data_buf->msdu_droph) << 32)
			+ data_buf->msdu_dropl;

	cnt->passing_sdu_count = cnt->matching_frames_count
				- cnt->not_passing_sdu_count;

	cnt->not_passing_frames_count =
		((u64)le32_to_cpu(data_buf->stream_gate_droph) << 32)
		+ le32_to_cpu(data_buf->stream_gate_dropl);

	cnt->passing_frames_count = cnt->matching_frames_count
				- cnt->not_passing_sdu_count
				- cnt->not_passing_frames_count;

	cnt->red_frames_count =
		((u64)le32_to_cpu(data_buf->flow_meter_droph) << 32)
		+ le32_to_cpu(data_buf->flow_meter_dropl);

exit:
	kfree(data_buf);
	return err;
}

static u64 get_ptp_now(struct enetc_hw *hw)
{
	u64 now_lo, now_hi, now;

	now_lo = enetc_rd(hw, ENETC_SICTR0);
	now_hi = enetc_rd(hw, ENETC_SICTR1);
	now = now_lo | now_hi << 32;

	return now;
}

static int get_start_ns(u64 now, u64 cycle, u64 *start)
{
	u64 n;

	if (!cycle)
		return -EFAULT;

	n = div64_u64(now, cycle);

	*start = (n + 1) * cycle;

	return 0;
}

/* Stream Gate Instance Set Descriptor */
static int enetc_streamgate_hw_set(struct enetc_ndev_priv *priv,
				   struct enetc_psfp_gate *sgi,
				   u8 enable)
{
	struct enetc_cbd cbd = { .cmd = 0 };
	struct sgi_table *sgi_config;
	struct sgcl_conf *sgcl_config;
	struct sgcl_data *sgcl_data;
	struct sgce *sgce;
	dma_addr_t dma;
	u16 data_size;
	int err, i;
	u64 now;

	cbd.index = cpu_to_le16(sgi->index);
	cbd.cmd = 0;
	cbd.cls = BDCR_CMD_STREAM_GCL;
	cbd.status_flags = 0x80;

	/* disable */
	if (!enable)
		return enetc_send_cmd(priv->si, &cbd);

	if (!sgi->num_entries)
		return 0;

	if (sgi->num_entries > priv->psfp_cap.max_psfp_gatelist ||
	    !sgi->cycletime)
		return -EINVAL;

	/* enable */
	sgi_config = &cbd.sgi_table;

	/* Keep open before gate list start */
	sgi_config->ocgtst = 0x80;

	sgi_config->oipv = (sgi->init_ipv < 0) ?
				0x0 : ((sgi->init_ipv & 0x7) | 0x8);

	sgi_config->en = 0x80;

	/* Basic config */
	err = enetc_send_cmd(priv->si, &cbd);
	if (err)
		return -EINVAL;

	memset(&cbd, 0, sizeof(cbd));

	cbd.index = cpu_to_le16(sgi->index);
	cbd.cmd = 1;
	cbd.cls = BDCR_CMD_STREAM_GCL;
	cbd.status_flags = 0;

	sgcl_config = &cbd.sgcl_conf;

	sgcl_config->acl_len = (sgi->num_entries - 1) & 0x3;

	data_size = struct_size(sgcl_data, sgcl, sgi->num_entries);

	sgcl_data = kzalloc(data_size, __GFP_DMA | GFP_KERNEL);
	if (!sgcl_data)
		return -ENOMEM;

	cbd.length = cpu_to_le16(data_size);

	dma = dma_map_single(&priv->si->pdev->dev,
			     sgcl_data, data_size,
			     DMA_FROM_DEVICE);
	if (dma_mapping_error(&priv->si->pdev->dev, dma)) {
		netdev_err(priv->si->ndev, "DMA mapping failed!\n");
		kfree(sgcl_data);
		return -ENOMEM;
	}

	cbd.addr[0] = lower_32_bits(dma);
	cbd.addr[1] = upper_32_bits(dma);

	sgce = &sgcl_data->sgcl[0];

	sgcl_config->agtst = 0x80;

	sgcl_data->ct = cpu_to_le32(sgi->cycletime);
	sgcl_data->cte = cpu_to_le32(sgi->cycletimext);

	if (sgi->init_ipv >= 0)
		sgcl_config->aipv = (sgi->init_ipv & 0x7) | 0x8;

	for (i = 0; i < sgi->num_entries; i++) {
		struct action_gate_entry *from = &sgi->entries[i];
		struct sgce *to = &sgce[i];

		if (from->gate_state)
			to->multi |= 0x10;

		if (from->ipv >= 0)
			to->multi |= ((from->ipv & 0x7) << 5) | 0x08;

		if (from->maxoctets >= 0) {
			to->multi |= 0x01;
			to->msdu[0] = from->maxoctets & 0xFF;
			to->msdu[1] = (from->maxoctets >> 8) & 0xFF;
			to->msdu[2] = (from->maxoctets >> 16) & 0xFF;
		}

		to->interval = cpu_to_le32(from->interval);
	}

	/* If basetime is less than now, calculate start time */
	now = get_ptp_now(&priv->si->hw);

	if (sgi->basetime < now) {
		u64 start;

		err = get_start_ns(now, sgi->cycletime, &start);
		if (err)
			goto exit;
		sgcl_data->btl = cpu_to_le32(lower_32_bits(start));
		sgcl_data->bth = cpu_to_le32(upper_32_bits(start));
	} else {
		u32 hi, lo;

		hi = upper_32_bits(sgi->basetime);
		lo = lower_32_bits(sgi->basetime);
		sgcl_data->bth = cpu_to_le32(hi);
		sgcl_data->btl = cpu_to_le32(lo);
	}

	err = enetc_send_cmd(priv->si, &cbd);

exit:
	kfree(sgcl_data);

	return err;
}

static struct enetc_stream_filter *enetc_get_stream_by_index(u32 index)
{
	struct enetc_stream_filter *f;

	hlist_for_each_entry(f, &epsfp.stream_list, node)
		if (f->sid.index == index)
			return f;

	return NULL;
}

static struct enetc_psfp_gate *enetc_get_gate_by_index(u32 index)
{
	struct enetc_psfp_gate *g;

	hlist_for_each_entry(g, &epsfp.psfp_gate_list, node)
		if (g->index == index)
			return g;

	return NULL;
}

static struct enetc_psfp_filter *enetc_get_filter_by_index(u32 index)
{
	struct enetc_psfp_filter *s;

	hlist_for_each_entry(s, &epsfp.psfp_filter_list, node)
		if (s->index == index)
			return s;

	return NULL;
}

static struct enetc_psfp_filter
	*enetc_psfp_check_sfi(struct enetc_psfp_filter *sfi)
{
	struct enetc_psfp_filter *s;

	hlist_for_each_entry(s, &epsfp.psfp_filter_list, node)
		if (s->gate_id == sfi->gate_id &&
		    s->prio == sfi->prio &&
		    s->meter_id == sfi->meter_id)
			return s;

	return NULL;
}

static int enetc_get_free_index(struct enetc_ndev_priv *priv)
{
	u32 max_size = priv->psfp_cap.max_psfp_filter;
	unsigned long index;

	index = find_first_zero_bit(epsfp.psfp_sfi_bitmap, max_size);
	if (index == max_size)
		return -1;

	return index;
}

static void stream_filter_unref(struct enetc_ndev_priv *priv, u32 index)
{
	struct enetc_psfp_filter *sfi;
	u8 z;

	sfi = enetc_get_filter_by_index(index);
	WARN_ON(!sfi);
	z = refcount_dec_and_test(&sfi->refcount);

	if (z) {
		enetc_streamfilter_hw_set(priv, sfi, false);
		hlist_del(&sfi->node);
		kfree(sfi);
		clear_bit(index, epsfp.psfp_sfi_bitmap);
	}
}

static void stream_gate_unref(struct enetc_ndev_priv *priv, u32 index)
{
	struct enetc_psfp_gate *sgi;
	u8 z;

	sgi = enetc_get_gate_by_index(index);
	WARN_ON(!sgi);
	z = refcount_dec_and_test(&sgi->refcount);
	if (z) {
		enetc_streamgate_hw_set(priv, sgi, false);
		hlist_del(&sgi->node);
		kfree(sgi);
	}
}

static void remove_one_chain(struct enetc_ndev_priv *priv,
			     struct enetc_stream_filter *filter)
{
	stream_gate_unref(priv, filter->sgi_index);
	stream_filter_unref(priv, filter->sfi_index);

	hlist_del(&filter->node);
	kfree(filter);
}

static int enetc_psfp_hw_set(struct enetc_ndev_priv *priv,
			     struct enetc_streamid *sid,
			     struct enetc_psfp_filter *sfi,
			     struct enetc_psfp_gate *sgi)
{
	int err;

	err = enetc_streamid_hw_set(priv, sid, true);
	if (err)
		return err;

	if (sfi) {
		err = enetc_streamfilter_hw_set(priv, sfi, true);
		if (err)
			goto revert_sid;
	}

	err = enetc_streamgate_hw_set(priv, sgi, true);
	if (err)
		goto revert_sfi;

	return 0;

revert_sfi:
	if (sfi)
		enetc_streamfilter_hw_set(priv, sfi, false);
revert_sid:
	enetc_streamid_hw_set(priv, sid, false);
	return err;
}

static struct actions_fwd *enetc_check_flow_actions(u64 acts,
						    unsigned int inputkeys)
{
	int i;

	for (i = 0; i < ARRAY_SIZE(enetc_act_fwd); i++)
		if (acts == enetc_act_fwd[i].actions &&
		    inputkeys & enetc_act_fwd[i].keys)
			return &enetc_act_fwd[i];

	return NULL;
}

static int enetc_psfp_parse_clsflower(struct enetc_ndev_priv *priv,
				      struct flow_cls_offload *f)
{
	struct flow_rule *rule = flow_cls_offload_flow_rule(f);
	struct netlink_ext_ack *extack = f->common.extack;
	struct enetc_stream_filter *filter, *old_filter;
	struct enetc_psfp_filter *sfi, *old_sfi;
	struct enetc_psfp_gate *sgi, *old_sgi;
	struct flow_action_entry *entry;
	struct action_gate_entry *e;
	u8 sfi_overwrite = 0;
	int entries_size;
	int i, err;

	if (f->common.chain_index >= priv->psfp_cap.max_streamid) {
		NL_SET_ERR_MSG_MOD(extack, "No Stream identify resource!");
		return -ENOSPC;
	}

	flow_action_for_each(i, entry, &rule->action)
		if (entry->id == FLOW_ACTION_GATE)
			break;

	if (entry->id != FLOW_ACTION_GATE)
		return -EINVAL;

	filter = kzalloc(sizeof(*filter), GFP_KERNEL);
	if (!filter)
		return -ENOMEM;

	filter->sid.index = f->common.chain_index;

	if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {
		struct flow_match_eth_addrs match;

		flow_rule_match_eth_addrs(rule, &match);

		if (!is_zero_ether_addr(match.mask->dst) &&
		    !is_zero_ether_addr(match.mask->src)) {
			NL_SET_ERR_MSG_MOD(extack,
					   "Cannot match on both source and destination MAC");
			err = EINVAL;
			goto free_filter;
		}

		if (!is_zero_ether_addr(match.mask->dst)) {
			if (!is_broadcast_ether_addr(match.mask->dst)) {
				NL_SET_ERR_MSG_MOD(extack,
						   "Masked matching on destination MAC not supported");
				err = EINVAL;
				goto free_filter;
			}
			ether_addr_copy(filter->sid.dst_mac, match.key->dst);
			filter->sid.filtertype = STREAMID_TYPE_NULL;
		}

		if (!is_zero_ether_addr(match.mask->src)) {
			if (!is_broadcast_ether_addr(match.mask->src)) {
				NL_SET_ERR_MSG_MOD(extack,
						   "Masked matching on source MAC not supported");
				err = EINVAL;
				goto free_filter;
			}
			ether_addr_copy(filter->sid.src_mac, match.key->src);
			filter->sid.filtertype = STREAMID_TYPE_SMAC;
		}
	} else {
		NL_SET_ERR_MSG_MOD(extack, "Unsupported, must include ETH_ADDRS");
		err = EINVAL;
		goto free_filter;
	}

	if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_VLAN)) {
		struct flow_match_vlan match;

		flow_rule_match_vlan(rule, &match);
		if (match.mask->vlan_priority) {
			if (match.mask->vlan_priority !=
			    (VLAN_PRIO_MASK >> VLAN_PRIO_SHIFT)) {
				NL_SET_ERR_MSG_MOD(extack, "Only full mask is supported for VLAN priority");
				err = -EINVAL;
				goto free_filter;
			}
		}

		if (match.mask->vlan_id) {
			if (match.mask->vlan_id != VLAN_VID_MASK) {
				NL_SET_ERR_MSG_MOD(extack, "Only full mask is supported for VLAN id");
				err = -EINVAL;
				goto free_filter;
			}

			filter->sid.vid = match.key->vlan_id;
			if (!filter->sid.vid)
				filter->sid.tagged = STREAMID_VLAN_UNTAGGED;
			else
				filter->sid.tagged = STREAMID_VLAN_TAGGED;
		}
	} else {
		filter->sid.tagged = STREAMID_VLAN_ALL;
	}

	/* parsing gate action */
	if (entry->gate.index >= priv->psfp_cap.max_psfp_gate) {
		NL_SET_ERR_MSG_MOD(extack, "No Stream Gate resource!");
		err = -ENOSPC;
		goto free_filter;
	}

	if (entry->gate.num_entries >= priv->psfp_cap.max_psfp_gatelist) {
		NL_SET_ERR_MSG_MOD(extack, "No Stream Gate resource!");
		err = -ENOSPC;
		goto free_filter;
	}

	entries_size = struct_size(sgi, entries, entry->gate.num_entries);
	sgi = kzalloc(entries_size, GFP_KERNEL);
	if (!sgi) {
		err = -ENOMEM;
		goto free_filter;
	}

	refcount_set(&sgi->refcount, 1);
	sgi->index = entry->gate.index;
	sgi->init_ipv = entry->gate.prio;
	sgi->basetime = entry->gate.basetime;
	sgi->cycletime = entry->gate.cycletime;
	sgi->num_entries = entry->gate.num_entries;

	e = sgi->entries;
	for (i = 0; i < entry->gate.num_entries; i++) {
		e[i].gate_state = entry->gate.entries[i].gate_state;
		e[i].interval = entry->gate.entries[i].interval;
		e[i].ipv = entry->gate.entries[i].ipv;
		e[i].maxoctets = entry->gate.entries[i].maxoctets;
	}

	filter->sgi_index = sgi->index;

	sfi = kzalloc(sizeof(*sfi), GFP_KERNEL);
	if (!sfi) {
		err = -ENOMEM;
		goto free_gate;
	}

	refcount_set(&sfi->refcount, 1);
	sfi->gate_id = sgi->index;

	/* flow meter not support yet */
	sfi->meter_id = ENETC_PSFP_WILDCARD;

	/* prio ref the filter prio */
	if (f->common.prio && f->common.prio <= BIT(3))
		sfi->prio = f->common.prio - 1;
	else
		sfi->prio = ENETC_PSFP_WILDCARD;

	old_sfi = enetc_psfp_check_sfi(sfi);
	if (!old_sfi) {
		int index;

		index = enetc_get_free_index(priv);
		if (sfi->handle < 0) {
			NL_SET_ERR_MSG_MOD(extack, "No Stream Filter resource!");
			err = -ENOSPC;
			goto free_sfi;
		}

		sfi->index = index;
		sfi->handle = index + HANDLE_OFFSET;
		/* Update the stream filter handle also */
		filter->sid.handle = sfi->handle;
		filter->sfi_index = sfi->index;
		sfi_overwrite = 0;
	} else {
		filter->sfi_index = old_sfi->index;
		filter->sid.handle = old_sfi->handle;
		sfi_overwrite = 1;
	}

	err = enetc_psfp_hw_set(priv, &filter->sid,
				sfi_overwrite ? NULL : sfi, sgi);
	if (err)
		goto free_sfi;

	spin_lock(&epsfp.psfp_lock);
	/* Remove the old node if exist and update with a new node */
	old_sgi = enetc_get_gate_by_index(filter->sgi_index);
	if (old_sgi) {
		refcount_set(&sgi->refcount,
			     refcount_read(&old_sgi->refcount) + 1);
		hlist_del(&old_sgi->node);
		kfree(old_sgi);
	}

	hlist_add_head(&sgi->node, &epsfp.psfp_gate_list);

	if (!old_sfi) {
		hlist_add_head(&sfi->node, &epsfp.psfp_filter_list);
		set_bit(sfi->index, epsfp.psfp_sfi_bitmap);
	} else {
		kfree(sfi);
		refcount_inc(&old_sfi->refcount);
	}

	old_filter = enetc_get_stream_by_index(filter->sid.index);
	if (old_filter)
		remove_one_chain(priv, old_filter);

	filter->stats.lastused = jiffies;
	hlist_add_head(&filter->node, &epsfp.stream_list);

	spin_unlock(&epsfp.psfp_lock);

	return 0;

free_sfi:
	kfree(sfi);
free_gate:
	kfree(sgi);
free_filter:
	kfree(filter);

	return err;
}

static int enetc_config_clsflower(struct enetc_ndev_priv *priv,
				  struct flow_cls_offload *cls_flower)
{
	struct flow_rule *rule = flow_cls_offload_flow_rule(cls_flower);
	struct netlink_ext_ack *extack = cls_flower->common.extack;
	struct flow_dissector *dissector = rule->match.dissector;
	struct flow_action *action = &rule->action;
	struct flow_action_entry *entry;
	struct actions_fwd *fwd;
	u64 actions = 0;
	int i, err;

	if (!flow_action_has_entries(action)) {
		NL_SET_ERR_MSG_MOD(extack, "At least one action is needed");
		return -EINVAL;
	}

	flow_action_for_each(i, entry, action)
		actions |= BIT(entry->id);

	fwd = enetc_check_flow_actions(actions, dissector->used_keys);
	if (!fwd) {
		NL_SET_ERR_MSG_MOD(extack, "Unsupported filter type!");
		return -EOPNOTSUPP;
	}

	if (fwd->output & FILTER_ACTION_TYPE_PSFP) {
		err = enetc_psfp_parse_clsflower(priv, cls_flower);
		if (err) {
			NL_SET_ERR_MSG_MOD(extack, "Invalid PSFP inputs");
			return err;
		}
	} else {
		NL_SET_ERR_MSG_MOD(extack, "Unsupported actions");
		return -EOPNOTSUPP;
	}

	return 0;
}

static int enetc_psfp_destroy_clsflower(struct enetc_ndev_priv *priv,
					struct flow_cls_offload *f)
{
	struct enetc_stream_filter *filter;
	struct netlink_ext_ack *extack = f->common.extack;
	int err;

	if (f->common.chain_index >= priv->psfp_cap.max_streamid) {
		NL_SET_ERR_MSG_MOD(extack, "No Stream identify resource!");
		return -ENOSPC;
	}

	filter = enetc_get_stream_by_index(f->common.chain_index);
	if (!filter)
		return -EINVAL;

	err = enetc_streamid_hw_set(priv, &filter->sid, false);
	if (err)
		return err;

	remove_one_chain(priv, filter);

	return 0;
}

static int enetc_destroy_clsflower(struct enetc_ndev_priv *priv,
				   struct flow_cls_offload *f)
{
	return enetc_psfp_destroy_clsflower(priv, f);
}

static int enetc_psfp_get_stats(struct enetc_ndev_priv *priv,
				struct flow_cls_offload *f)
{
	struct psfp_streamfilter_counters counters = {};
	struct enetc_stream_filter *filter;
	struct flow_stats stats = {};
	int err;

	filter = enetc_get_stream_by_index(f->common.chain_index);
	if (!filter)
		return -EINVAL;

	err = enetc_streamcounter_hw_get(priv, filter->sfi_index, &counters);
	if (err)
		return -EINVAL;

	spin_lock(&epsfp.psfp_lock);
	stats.pkts = counters.matching_frames_count - filter->stats.pkts;
	stats.drops = counters.not_passing_frames_count -
					filter->stats.drops;
	stats.lastused = filter->stats.lastused;
	filter->stats.pkts += stats.pkts;
	filter->stats.drops += stats.drops;
	spin_unlock(&epsfp.psfp_lock);

	flow_stats_update(&f->stats, 0x0, stats.pkts, stats.drops,
			  stats.lastused, FLOW_ACTION_HW_STATS_DELAYED);

	return 0;
}

static int enetc_setup_tc_cls_flower(struct enetc_ndev_priv *priv,
				     struct flow_cls_offload *cls_flower)
{
	switch (cls_flower->command) {
	case FLOW_CLS_REPLACE:
		return enetc_config_clsflower(priv, cls_flower);
	case FLOW_CLS_DESTROY:
		return enetc_destroy_clsflower(priv, cls_flower);
	case FLOW_CLS_STATS:
		return enetc_psfp_get_stats(priv, cls_flower);
	default:
		return -EOPNOTSUPP;
	}
}

static inline void clean_psfp_sfi_bitmap(void)
{
	bitmap_free(epsfp.psfp_sfi_bitmap);
	epsfp.psfp_sfi_bitmap = NULL;
}

static void clean_stream_list(void)
{
	struct enetc_stream_filter *s;
	struct hlist_node *tmp;

	hlist_for_each_entry_safe(s, tmp, &epsfp.stream_list, node) {
		hlist_del(&s->node);
		kfree(s);
	}
}

static void clean_sfi_list(void)
{
	struct enetc_psfp_filter *sfi;
	struct hlist_node *tmp;

	hlist_for_each_entry_safe(sfi, tmp, &epsfp.psfp_filter_list, node) {
		hlist_del(&sfi->node);
		kfree(sfi);
	}
}

static void clean_sgi_list(void)
{
	struct enetc_psfp_gate *sgi;
	struct hlist_node *tmp;

	hlist_for_each_entry_safe(sgi, tmp, &epsfp.psfp_gate_list, node) {
		hlist_del(&sgi->node);
		kfree(sgi);
	}
}

static void clean_psfp_all(void)
{
	/* Disable all list nodes and free all memory */
	clean_sfi_list();
	clean_sgi_list();
	clean_stream_list();
	epsfp.dev_bitmap = 0;
	clean_psfp_sfi_bitmap();
}

int enetc_setup_tc_block_cb(enum tc_setup_type type, void *type_data,
			    void *cb_priv)
{
	struct net_device *ndev = cb_priv;

	if (!tc_can_offload(ndev))
		return -EOPNOTSUPP;

	switch (type) {
	case TC_SETUP_CLSFLOWER:
		return enetc_setup_tc_cls_flower(netdev_priv(ndev), type_data);
	default:
		return -EOPNOTSUPP;
	}
}

int enetc_psfp_init(struct enetc_ndev_priv *priv)
{
	if (epsfp.psfp_sfi_bitmap)
		return 0;

	epsfp.psfp_sfi_bitmap = bitmap_zalloc(priv->psfp_cap.max_psfp_filter,
					      GFP_KERNEL);
	if (!epsfp.psfp_sfi_bitmap)
		return -ENOMEM;

	spin_lock_init(&epsfp.psfp_lock);

	if (list_empty(&enetc_block_cb_list))
		epsfp.dev_bitmap = 0;

	return 0;
}

int enetc_psfp_clean(struct enetc_ndev_priv *priv)
{
	if (!list_empty(&enetc_block_cb_list))
		return -EBUSY;

	clean_psfp_all();

	return 0;
}

int enetc_setup_tc_psfp(struct net_device *ndev, void *type_data)
{
	struct enetc_ndev_priv *priv = netdev_priv(ndev);
	struct flow_block_offload *f = type_data;
	int err;

	err = flow_block_cb_setup_simple(f, &enetc_block_cb_list,
					 enetc_setup_tc_block_cb,
					 ndev, ndev, true);
	if (err)
		return err;

	switch (f->command) {
	case FLOW_BLOCK_BIND:
		set_bit(enetc_get_port(priv), &epsfp.dev_bitmap);
		break;
	case FLOW_BLOCK_UNBIND:
		clear_bit(enetc_get_port(priv), &epsfp.dev_bitmap);
		if (!epsfp.dev_bitmap)
			clean_psfp_all();
		break;
	}

	return 0;
}