From 6c653ca2428989e31f67eec58045eb38c20bfe94 Mon Sep 17 00:00:00 2001
From: Roger Quadros <rogerq@ti.com>
Date: Thu, 14 Oct 2021 14:26:55 +0530
Subject: [PATCH] net: ethernet: ti: icssg_prueth: add TAPRIO offload support
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

ICSSG dual-emac f/w supports Enhanced Scheduled Traffic (EST – defined
in P802.1Qbv/D2.2 that later got included in IEEE 802.1Q-2018)
configuration. EST allows express queue traffic to be scheduled
(placed) on the wire at specific repeatable time intervals. In
Linux kernel, EST configuration is done through tc command and
the taprio scheduler in the net core implements a software only
scheduler (SCH_TAPRIO). If the NIC is capable of EST configuration,
user indicate "flag 2" in the command which is then parsed by
taprio scheduler in net core and indicate that the command is to
be offloaded to h/w. taprio then offloads the command to the
driver by calling ndo_setup_tc() ndo ops. This patch implements
ndo_setup_tc() to offload EST configuration to ICSSG.

[vigneshr@ti.com:
Remove cycle time limitations
Add support to specify future basetime
Use readx_poll_timeouts where possible
Fix checkpatch and build warnings]

Signed-off-by: Roger Quadros <rogerq@ti.com>
Signed-off-by: Vignesh Raghavendra <vigneshr@ti.com>
---
 drivers/net/ethernet/ti/Makefile           |   2 +-
 drivers/net/ethernet/ti/icssg_prueth.c     |   5 +
 drivers/net/ethernet/ti/icssg_prueth.h     |   4 +
 drivers/net/ethernet/ti/icssg_qos.c        | 298 +++++++++++++++++++++
 drivers/net/ethernet/ti/icssg_qos.h        | 120 +++++++++
 drivers/net/ethernet/ti/icssg_switch_map.h |   2 +
 6 files changed, 430 insertions(+), 1 deletion(-)
 create mode 100644 drivers/net/ethernet/ti/icssg_qos.c
 create mode 100644 drivers/net/ethernet/ti/icssg_qos.h

diff --git a/drivers/net/ethernet/ti/Makefile b/drivers/net/ethernet/ti/Makefile
index 5b4cc0a3e3f8..e136e9837075 100644
--- a/drivers/net/ethernet/ti/Makefile
+++ b/drivers/net/ethernet/ti/Makefile
@@ -31,4 +31,4 @@ obj-$(CONFIG_TI_K3_AM65_CPTS) += am65-cpts.o
 obj-$(CONFIG_TI_ICSS_IEP) += icss_iep.o
 
 obj-$(CONFIG_TI_ICSSG_PRUETH) += icssg-prueth.o
-icssg-prueth-y := icssg_prueth.o icssg_classifier.o icssg_ethtool.o icssg_queues.o icssg_config.o k3-cppi-desc-pool.o icssg_mii_cfg.o icssg_switchdev.o
+icssg-prueth-y := icssg_prueth.o icssg_classifier.o icssg_ethtool.o icssg_queues.o icssg_config.o k3-cppi-desc-pool.o icssg_mii_cfg.o icssg_switchdev.o icssg_qos.o
diff --git a/drivers/net/ethernet/ti/icssg_prueth.c b/drivers/net/ethernet/ti/icssg_prueth.c
index 8421272fad8a..193ec64f1341 100644
--- a/drivers/net/ethernet/ti/icssg_prueth.c
+++ b/drivers/net/ethernet/ti/icssg_prueth.c
@@ -1733,6 +1733,8 @@ static int emac_ndo_open(struct net_device *ndev)
 		napi_enable(&emac->tx_chns[i].napi_tx);
 	napi_enable(&emac->napi_rx);
 
+	icssg_qos_init(ndev);
+
 	/* Get attached phy details */
 	phy_attached_info(emac->phydev);
 
@@ -1813,6 +1815,8 @@ static int emac_ndo_stop(struct net_device *ndev)
 	int rx_flow = emac->is_sr1 ?
 			PRUETH_RX_FLOW_DATA_SR1 : PRUETH_RX_FLOW_DATA_SR2;
 
+	icssg_qos_cleanup(ndev);
+
 	/* inform the upper layers. */
 	netif_tx_stop_all_queues(ndev);
 
@@ -2073,6 +2077,7 @@ static const struct net_device_ops emac_netdev_ops = {
 	.ndo_set_rx_mode = emac_ndo_set_rx_mode,
 	.ndo_do_ioctl = emac_ndo_ioctl,
 	.ndo_get_devlink_port = emac_ndo_get_devlink_port,
+	.ndo_setup_tc = icssg_qos_ndo_setup_tc,
 };
 
 /* get emac_port corresponding to eth_node name */
diff --git a/drivers/net/ethernet/ti/icssg_prueth.h b/drivers/net/ethernet/ti/icssg_prueth.h
index 2aeca9c1e4db..2ae732fec2c6 100644
--- a/drivers/net/ethernet/ti/icssg_prueth.h
+++ b/drivers/net/ethernet/ti/icssg_prueth.h
@@ -36,6 +36,7 @@
 #include "icssg_config.h"
 #include "icss_iep.h"
 #include "icssg_switch_map.h"
+#include "icssg_qos.h"
 
 #define ICSS_SLICE0	0
 #define ICSS_SLICE1	1
@@ -188,6 +189,9 @@ struct prueth_emac {
 	bool offload_fwd_mark;
 	struct devlink_port devlink_port;
 	int port_vlan;
+
+	struct prueth_qos qos;
+	struct work_struct ts_work;
 };
 
 /**
diff --git a/drivers/net/ethernet/ti/icssg_qos.c b/drivers/net/ethernet/ti/icssg_qos.c
new file mode 100644
index 000000000000..a9086e2e374d
--- /dev/null
+++ b/drivers/net/ethernet/ti/icssg_qos.c
@@ -0,0 +1,298 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Texas Instruments ICSSG PRUETH QoS submodule
+ * Copyright (C) 2020 Texas Instruments Incorporated - http://www.ti.com/
+ */
+
+#include <linux/printk.h>
+#include "icssg_prueth.h"
+#include "icssg_switch_map.h"
+
+static void icssg_qos_tas_init(struct net_device *ndev);
+
+void icssg_qos_init(struct net_device *ndev)
+{
+	icssg_qos_tas_init(ndev);
+
+	/* IET init goes here */
+}
+
+void icssg_qos_cleanup(struct net_device *ndev)
+{
+	/* IET cleanup goes here */
+}
+
+static void tas_update_fw_list_pointers(struct prueth_emac *emac)
+{
+	struct tas_config *tas = &emac->qos.tas.config;
+
+	if ((readb(tas->active_list)) == TAS_LIST0) {
+		tas->firmware_active_list = emac->dram.va + TAS_GATE_MASK_LIST0;
+		tas->firmware_shadow_list = emac->dram.va + TAS_GATE_MASK_LIST1;
+	} else {
+		tas->firmware_active_list = emac->dram.va + TAS_GATE_MASK_LIST1;
+		tas->firmware_shadow_list = emac->dram.va + TAS_GATE_MASK_LIST0;
+	}
+}
+
+static void tas_update_maxsdu_table(struct prueth_emac *emac)
+{
+	struct tas_config *tas = &emac->qos.tas.config;
+	u16 *max_sdu_tbl_ptr;
+	u8 gate_idx;
+
+	/* update the maxsdu table */
+	max_sdu_tbl_ptr = emac->dram.va + TAS_QUEUE_MAX_SDU_LIST;
+
+	for (gate_idx = 0; gate_idx < TAS_MAX_NUM_QUEUES; gate_idx++)
+		writew(tas->max_sdu_table.max_sdu[gate_idx], &max_sdu_tbl_ptr[gate_idx]);
+}
+
+static void tas_reset(struct prueth_emac *emac)
+{
+	struct tas_config *tas = &emac->qos.tas.config;
+	int i;
+
+	for (i = 0; i < TAS_MAX_NUM_QUEUES; i++)
+		tas->max_sdu_table.max_sdu[i] = 2048;
+
+	tas_update_maxsdu_table(emac);
+
+	writeb(TAS_LIST0, tas->active_list);
+
+	memset_io(tas->firmware_active_list, 0, sizeof(*tas->firmware_active_list));
+	memset_io(tas->firmware_shadow_list, 0, sizeof(*tas->firmware_shadow_list));
+}
+
+static int tas_set_state(struct prueth_emac *emac, enum tas_state state)
+{
+	struct tas_config *tas = &emac->qos.tas.config;
+	int ret;
+
+	if (tas->state == state)
+		return 0;
+
+	switch (state) {
+	case TAS_STATE_RESET:
+		tas_reset(emac);
+		ret = emac_set_port_state(emac, ICSSG_EMAC_PORT_TAS_RESET);
+		tas->state = TAS_STATE_RESET;
+		break;
+	case TAS_STATE_ENABLE:
+		ret = emac_set_port_state(emac, ICSSG_EMAC_PORT_TAS_ENABLE);
+		tas->state = TAS_STATE_ENABLE;
+		break;
+	case TAS_STATE_DISABLE:
+		ret = emac_set_port_state(emac, ICSSG_EMAC_PORT_TAS_DISABLE);
+		tas->state = TAS_STATE_DISABLE;
+		break;
+	default:
+		netdev_err(emac->ndev, "%s: unsupported state\n", __func__);
+		ret = -EINVAL;
+		break;
+	}
+
+	if (ret)
+		netdev_err(emac->ndev, "TAS set state failed %d\n", ret);
+	return ret;
+}
+
+static int tas_set_trigger_list_change(struct prueth_emac *emac)
+{
+	struct tc_taprio_qopt_offload *admin_list = emac->qos.tas.taprio_admin;
+	struct tas_config *tas = &emac->qos.tas.config;
+	struct ptp_system_timestamp sts;
+	u32 change_cycle_count;
+	u32 cycle_time;
+	u64 base_time;
+	u64 cur_time;
+
+	cycle_time = admin_list->cycle_time - 4; /* -4ns to compensate for IEP wraparound time */
+	base_time = admin_list->base_time;
+	cur_time = prueth_iep_gettime(emac, &sts);
+
+	if (base_time > cur_time)
+		change_cycle_count = DIV_ROUND_UP_ULL(base_time - cur_time, cycle_time);
+	else
+		change_cycle_count = 1;
+
+	writel(cycle_time, emac->dram.va + TAS_ADMIN_CYCLE_TIME);
+	writel(change_cycle_count, emac->dram.va + TAS_CONFIG_CHANGE_CYCLE_COUNT);
+	writeb(admin_list->num_entries, emac->dram.va + TAS_ADMIN_LIST_LENGTH);
+
+	/* config_change cleared by f/w to ack reception of new shadow list */
+	writeb(1, &tas->config_list->config_change);
+	/* config_pending cleared by f/w when new shadow list is copied to active list */
+	writeb(1, &tas->config_list->config_pending);
+
+	return emac_set_port_state(emac, ICSSG_EMAC_PORT_TAS_TRIGGER);
+}
+
+static int tas_update_oper_list(struct prueth_emac *emac)
+{
+	struct tas_config *tas = &emac->qos.tas.config;
+	struct tc_taprio_qopt_offload *admin_list = emac->qos.tas.taprio_admin;
+	int ret;
+	u8 win_idx, gate_idx, val;
+	u32 tas_acc_gate_close_time = 0;
+
+	tas_update_fw_list_pointers(emac);
+
+	for (win_idx = 0; win_idx < admin_list->num_entries; win_idx++) {
+		tas->firmware_shadow_list->gate_mask_list[win_idx] = admin_list->entries[win_idx].gate_mask;
+		tas_acc_gate_close_time += admin_list->entries[win_idx].interval;
+
+		/* extend last entry till end of cycle time */
+		if (win_idx == admin_list->num_entries - 1)
+			tas->firmware_shadow_list->window_end_time_list[win_idx] = admin_list->cycle_time;
+		else
+			tas->firmware_shadow_list->window_end_time_list[win_idx] = tas_acc_gate_close_time;
+	}
+
+	/* clear remaining entries */
+	for (win_idx = admin_list->num_entries; win_idx < TAS_MAX_CMD_LISTS; win_idx++) {
+		tas->firmware_shadow_list->gate_mask_list[win_idx] = 0;
+		tas->firmware_shadow_list->window_end_time_list[win_idx] = 0;
+	}
+
+	/* update the Array of gate close time for each queue in each window */
+	for (win_idx = 0 ; win_idx < admin_list->num_entries; win_idx++) {
+		/* On Linux, only PRUETH_MAX_TX_QUEUES are supported per port */
+		for (gate_idx = 0; gate_idx < PRUETH_MAX_TX_QUEUES; gate_idx++) {
+			u32 gate_close_time = 0;
+
+			if (tas->firmware_shadow_list->gate_mask_list[win_idx] & BIT(gate_idx))
+				gate_close_time = tas->firmware_shadow_list->window_end_time_list[win_idx];
+
+			tas->firmware_shadow_list->gate_close_time_list[win_idx][gate_idx] = gate_close_time;
+		}
+	}
+
+	/* tell f/w to swap active & shadow list */
+	ret = tas_set_trigger_list_change(emac);
+	if (ret) {
+		netdev_err(emac->ndev, "failed to swap f/w config list: %d\n", ret);
+		return ret;
+	}
+
+	/* Wait for completion */
+	ret = readb_poll_timeout(&tas->config_list->config_change, val, !val,
+				 USEC_PER_MSEC, 10 * USEC_PER_MSEC);
+	if (ret) {
+		netdev_err(emac->ndev, "TAS list change completion time out\n");
+		return ret;
+	}
+
+	tas_update_fw_list_pointers(emac);
+
+	return 0;
+}
+
+static int emac_set_taprio(struct prueth_emac *emac)
+{
+	int ret;
+	struct tc_taprio_qopt_offload *taprio = emac->qos.tas.taprio_admin;
+
+	if (!taprio->enable)
+		return tas_set_state(emac, TAS_STATE_DISABLE);
+
+	ret = tas_update_oper_list(emac);
+	if (ret)
+		return ret;
+
+	return tas_set_state(emac, TAS_STATE_ENABLE);
+}
+
+static void emac_cp_taprio(struct tc_taprio_qopt_offload *from,
+			   struct tc_taprio_qopt_offload *to)
+{
+	int i;
+
+	*to = *from;
+	for (i = 0; i < from->num_entries; i++)
+		to->entries[i] = from->entries[i];
+}
+
+static int emac_setup_taprio(struct net_device *ndev, struct tc_taprio_qopt_offload *taprio)
+{
+	struct prueth_emac *emac = netdev_priv(ndev);
+	struct tc_taprio_qopt_offload *est_new;
+	int ret, win_idx;
+
+	if (!netif_running(ndev)) {
+		netdev_err(ndev, "interface is down, link speed unknown\n");
+		return -ENETDOWN;
+	}
+
+	if (taprio->cycle_time_extension) {
+		netdev_err(ndev, "Failed to set cycle time extension");
+		return -EOPNOTSUPP;
+	}
+
+	if (taprio->num_entries == 0 ||
+	    taprio->num_entries > TAS_MAX_CMD_LISTS) {
+		netdev_err(ndev, "unsupported num_entries %ld in taprio config\n",
+			   taprio->num_entries);
+		return -EINVAL;
+	}
+
+	/* If any time_interval is 0 in between the list, then exit */
+	for (win_idx = 0; win_idx < taprio->num_entries; win_idx++) {
+		if (taprio->entries[win_idx].interval == 0) {
+			netdev_err(ndev, "0 interval in taprio config not supported\n");
+			return -EINVAL;
+		}
+	}
+
+	if (emac->qos.tas.taprio_admin)
+		devm_kfree(&ndev->dev, emac->qos.tas.taprio_admin);
+
+	est_new = devm_kzalloc(&ndev->dev,
+			       struct_size(est_new, entries, taprio->num_entries),
+			       GFP_KERNEL);
+	emac_cp_taprio(taprio, est_new);
+	emac->qos.tas.taprio_admin = est_new;
+	ret = emac_set_taprio(emac);
+	if (ret)
+		devm_kfree(&ndev->dev, est_new);
+
+	return ret;
+}
+
+int icssg_qos_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type,
+			   void *type_data)
+{
+	struct prueth_emac *emac = netdev_priv(ndev);
+
+	if (emac->prueth->is_sr1)
+		return -EOPNOTSUPP;
+
+	switch (type) {
+	case TC_SETUP_QDISC_TAPRIO:
+		return emac_setup_taprio(ndev, type_data);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static void icssg_qos_tas_init(struct net_device *ndev)
+{
+	struct prueth_emac *emac = netdev_priv(ndev);
+	struct tas_config *tas = &emac->qos.tas.config;
+	bool need_setup = false;
+
+	if (emac->prueth->is_sr1)
+		return;
+
+	if (tas->state == TAS_STATE_ENABLE)
+		need_setup = true;
+
+	tas->config_list = emac->dram.va + TAS_CONFIG_CHANGE_TIME;
+	tas->active_list = emac->dram.va + TAS_ACTIVE_LIST_INDEX;
+
+	tas_update_fw_list_pointers(emac);
+
+	tas_set_state(emac, TAS_STATE_RESET);
+
+	if (need_setup)
+		emac_set_taprio(emac);
+}
diff --git a/drivers/net/ethernet/ti/icssg_qos.h b/drivers/net/ethernet/ti/icssg_qos.h
new file mode 100644
index 000000000000..76edc121797f
--- /dev/null
+++ b/drivers/net/ethernet/ti/icssg_qos.h
@@ -0,0 +1,120 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (C) 2020 Texas Instruments Incorporated - http://www.ti.com/
+ */
+
+#ifndef __NET_TI_ICSSG_QOS_H
+#define __NET_TI_ICSSG_QOS_H
+
+#include <linux/atomic.h>
+#include <linux/netdevice.h>
+#include <net/pkt_sched.h>
+
+/**
+ * Maximum number of gate command entries in each list.
+ */
+#define TAS_MAX_CMD_LISTS   (16)
+
+/**
+ * Maximum number of transmit queues supported by implementation
+ */
+#define TAS_MAX_NUM_QUEUES  (8)
+
+/**
+ * Minimum cycle time supported by implementation (in ns)
+ */
+#define TAS_MIN_CYCLE_TIME  (1000000)
+
+/**
+ * Minimum TAS window duration supported by implementation (in ns)
+ */
+#define TAS_MIN_WINDOW_DURATION  (10000)
+
+/**
+ * List number 0 or 1. Also the value at memory location TAS_ACTIVE_LIST_INDEX
+ */
+enum tas_list_num {
+	TAS_LIST0 = 0,
+	TAS_LIST1 = 1
+};
+
+/**
+ * state of TAS in f/w
+ */
+enum tas_state {
+	/* PRU's are idle */
+	TAS_STATE_DISABLE = 0,
+	/* Enable TAS */
+	TAS_STATE_ENABLE = 1,
+	/* Firmware will reset the state machine */
+	TAS_STATE_RESET = 2,
+};
+
+/**
+ * Config state machine variables. See IEEE Std 802.1Q-2018 8.6.8.4
+ */
+struct tas_config_list {
+	/* New list is copied at this time */
+	u64 config_change_time;
+	/* config change error counter, incremented if
+	 * admin->BaseTime < current time and TAS_enabled is true
+	 */
+	u32 config_change_error_counter;
+	/* True if list update is pending */
+	u8 config_pending;
+	/* Set to true when application trigger updating of admin list
+	 * to active list, cleared when configChangeTime is updated
+	 */
+	u8 config_change;
+};
+
+/**
+ * Max SDU table. See IEEE Std 802.1Q-2018 12.29.1.1
+ */
+struct tas_max_sdu_table {
+	u16 max_sdu[TAS_MAX_NUM_QUEUES];
+};
+
+/**
+ * TAS List Structure based on firmware memory map
+ */
+struct tas_firmware_list {
+	/* window gate mask list */
+	u8 gate_mask_list[TAS_MAX_CMD_LISTS];
+	/* window end time list */
+	u32 window_end_time_list[TAS_MAX_CMD_LISTS];
+	/* Array of gate close time for each queue in each window */
+	u32 gate_close_time_list[TAS_MAX_CMD_LISTS][TAS_MAX_NUM_QUEUES];
+};
+
+/**
+ * Main Time Aware Shaper Handle
+ */
+struct tas_config {
+	enum tas_state state;
+	struct tas_max_sdu_table max_sdu_table;
+	/* Config change variables */
+	struct __iomem tas_config_list * config_list;
+	/* Whether list 1 or list 2 is the operating list */
+	u8 __iomem *active_list;
+	/* active List pointer, used by firmware */
+	struct __iomem tas_firmware_list * firmware_active_list;
+	/* shadow List pointer, used by driver */
+	struct __iomem tas_firmware_list * firmware_shadow_list;
+};
+
+struct prueth_qos_tas {
+	struct tc_taprio_qopt_offload *taprio_admin;
+	struct tc_taprio_qopt_offload *taprio_oper;
+	struct tas_config config;
+};
+
+struct prueth_qos {
+	/* IET data structure goes here */
+	struct prueth_qos_tas tas;
+};
+
+void icssg_qos_init(struct net_device *ndev);
+void icssg_qos_cleanup(struct net_device *ndev);
+int icssg_qos_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type,
+			   void *type_data);
+#endif /* __NET_TI_ICSSG_QOS_H */
diff --git a/drivers/net/ethernet/ti/icssg_switch_map.h b/drivers/net/ethernet/ti/icssg_switch_map.h
index 0d16544aa096..f53f5633cf42 100644
--- a/drivers/net/ethernet/ti/icssg_switch_map.h
+++ b/drivers/net/ethernet/ti/icssg_switch_map.h
@@ -162,6 +162,8 @@
 #define HOST_RX_Q_PRE_CONTEXT_OFFSET                       0x0684
 /*Buffer for 8 FDB entries to be added by 'Add Multiple FDB entries IOCTL*/
 #define FDB_CMD_BUFFER                                     0x0894
+/*TAS queue max sdu length list*/
+#define TAS_QUEUE_MAX_SDU_LIST                             0x08FA
 /*Used by FW to generate random number with the SEED value*/
 #define HD_RAND_SEED_OFFSET                                0x0934
 
