Re: [PATCH v3 5/8] net: dsa: hellcreek: Add TAPRIO offloading support

The switch has support for the 802.1Qbv Time Aware Shaper (TAS). Traffic
schedules may be configured individually on each front port. Each port has eight
egress queues. The traffic is mapped to a traffic class respectively via the PCP
field of a VLAN tagged frame.

The TAPRIO Qdisc already implements that. Therefore, this interface can simply
be reused. Add .port_setup_tc() accordingly.

The activation of a schedule on a port is split into two parts:

 * Programming the necessary gate control list (GCL)
 * Setup hrtimer for starting the schedule

The hardware supports starting a schedule up to eight seconds in the future. The
TAPRIO interface provides an absolute base time. Therefore, hrtimers are
leveraged.

Signed-off-by: Kurt Kanzenbach <kurt@linutronix.de>
---
 drivers/net/dsa/hirschmann/hellcreek.c | 294 +++++++++++++++++++++++++
 drivers/net/dsa/hirschmann/hellcreek.h |  21 ++
 2 files changed, 315 insertions(+)

diff --git a/drivers/net/dsa/hirschmann/hellcreek.c b/drivers/net/dsa/hirschmann/hellcreek.c
index 745ca60342b4..e5b54f42c635 100644
--- a/drivers/net/dsa/hirschmann/hellcreek.c
+++ b/drivers/net/dsa/hirschmann/hellcreek.c

@@ -22,7 +22,9 @@
 #include <linux/spinlock.h>
 #include <linux/delay.h>
 #include <linux/ktime.h>
+#include <linux/time.h>
 #include <net/dsa.h>
+#include <net/pkt_sched.h>
 
 #include "hellcreek.h"
 #include "hellcreek_ptp.h"

@@ -153,6 +155,15 @@ static void hellcreek_select_vlan(struct hellcreek *hellcreek, int vid,
 	hellcreek_write(hellcreek, val, HR_VIDCFG);
 }
 
+static void hellcreek_select_tgd(struct hellcreek *hellcreek, int port)
+{
+	u16 val = 0;
+
+	val |= port << TR_TGDSEL_TDGSEL_SHIFT;
+
+	hellcreek_write(hellcreek, val, TR_TGDSEL);
+}
+
 static int hellcreek_wait_until_ready(struct hellcreek *hellcreek)
 {
 	u16 val;

@@ -958,6 +969,24 @@ static void __hellcreek_setup_tc_identity_mapping(struct hellcreek *hellcreek)
 	}
 }
 
+static void hellcreek_setup_tc_mapping(struct hellcreek *hellcreek,
+				       struct net_device *netdev)
+{
+	int i, j;
+
+	/* Setup mapping between traffic classes and port queues. */
+	for (i = 0; i < netdev_get_num_tc(netdev); ++i) {
+		for (j = 0; j < netdev->tc_to_txq[i].count; ++j) {
+			const int queue = j + netdev->tc_to_txq[i].offset;
+
+			hellcreek_select_prio(hellcreek, i);
+			hellcreek_write(hellcreek,
+					queue << HR_PRTCCFG_PCP_TC_MAP_SHIFT,
+					HR_PRTCCFG);
+		}
+	}
+}
+
 static void hellcreek_setup_tc_identity_mapping(struct hellcreek *hellcreek)
 {
 	unsigned long flags;

@@ -1081,6 +1110,267 @@ static void hellcreek_phylink_validate(struct dsa_switch *ds, int port,
 		   __ETHTOOL_LINK_MODE_MASK_NBITS);
 }
 
+static void hellcreek_setup_gcl(struct hellcreek *hellcreek, int port,
+				const struct hellcreek_schedule *schedule)
+{
+	size_t i;
+
+	for (i = 1; i <= schedule->num_entries; ++i) {
+		const struct hellcreek_gcl_entry *cur, *initial, *next;
+		u16 data;
+		u8 gates;
+
+		cur	= &schedule->entries[i - 1];
+		initial = &schedule->entries[0];
+		next	= &schedule->entries[i];
+
+		if (i == schedule->num_entries)
+			gates = initial->gate_states ^
+				cur->gate_states;
+		else
+			gates = next->gate_states ^
+				cur->gate_states;
+
+		data = gates;
+		if (cur->overrun_ignore)
+			data |= TR_GCLDAT_GCLOVRI;
+
+		if (i == schedule->num_entries)
+			data |= TR_GCLDAT_GCLWRLAST;
+
+		/* Gates states */
+		hellcreek_write(hellcreek, data, TR_GCLDAT);
+
+		/* Time intervall */
+		hellcreek_write(hellcreek,
+				cur->interval & 0x0000ffff,
+				TR_GCLTIL);
+		hellcreek_write(hellcreek,
+				(cur->interval & 0xffff0000) >> 16,
+				TR_GCLTIH);
+
+		/* Commit entry */
+		data = ((i - 1) << TR_GCLCMD_GCLWRADR_SHIFT) |
+			(initial->gate_states <<
+			 TR_GCLCMD_INIT_GATE_STATES_SHIFT);
+		hellcreek_write(hellcreek, data, TR_GCLCMD);
+	}
+}
+
+static void hellcreek_set_cycle_time(struct hellcreek *hellcreek,
+				     const struct hellcreek_schedule *schedule)
+{
+	u32 cycle_time = schedule->cycle_time;
+
+	hellcreek_write(hellcreek, cycle_time & 0x0000ffff, TR_CTWRL);
+	hellcreek_write(hellcreek, (cycle_time & 0xffff0000) >> 16, TR_CTWRH);
+}
+
+static void hellcreek_start_schedule(struct hellcreek *hellcreek,
+				     ktime_t start_time)
+{
+	struct timespec64 ts = ktime_to_timespec64(start_time);
+
+	/* Start can be only 8 seconds in the future */
+	ts.tv_sec %= 8;
+
+	/* Start schedule at this point of time */
+	hellcreek_write(hellcreek, ts.tv_nsec & 0x0000ffff, TR_ESTWRL);
+	hellcreek_write(hellcreek, (ts.tv_nsec & 0xffff0000) >> 16, TR_ESTWRH);
+
+	/* Arm timer, set seconds and switch schedule */
+	hellcreek_write(hellcreek, TR_ESTCMD_ESTARM | TR_ESTCMD_ESTSWCFG |
+		     ((ts.tv_sec & TR_ESTCMD_ESTSEC_MASK) <<
+		      TR_ESTCMD_ESTSEC_SHIFT), TR_ESTCMD);
+}
+
+static struct hellcreek_schedule *hellcreek_taprio_to_schedule(
+	const struct tc_taprio_qopt_offload *taprio)

+
+	spin_unlock_irqrestore(&hellcreek->reg_lock, flags);
+
+	return 0;
+}
+
+static int hellcreek_port_del_schedule(struct dsa_switch *ds, int port)
+{
+	struct hellcreek *hellcreek = ds->priv;
+	struct hellcreek_port *hellcreek_port;
+	unsigned long flags;
+
+	hellcreek_port = &hellcreek->ports[port];
+
+	dev_dbg(hellcreek->dev, "Remove traffic schedule on port %d\n", port);
+
+	/* First cancel timer */
+	hrtimer_cancel(&hellcreek_port->cycle_start_timer);
+
+	spin_lock_irqsave(&hellcreek->reg_lock, flags);
+
+	if (hellcreek_port->current_schedule) {
+		kfree(hellcreek_port->current_schedule->entries);
+		kfree(hellcreek_port->current_schedule);
+		hellcreek_port->current_schedule = NULL;
+	}
+
+	/* Then select port */
+	hellcreek_select_tgd(hellcreek, port);
+
+	/* Revert tc mapping */
+	__hellcreek_setup_tc_identity_mapping(hellcreek);
+
+	/* Disable gating and return to regular switching flow */
+	hellcreek_write(hellcreek, 0xff << TR_TGDCTRL_ADMINGATESTATES_SHIFT,
+			TR_TGDCTRL);
+
+	spin_unlock_irqrestore(&hellcreek->reg_lock, flags);
+
+	return 0;
+}
+
+static int hellcreek_port_setup_tc(struct dsa_switch *ds, int port,
+				   enum tc_setup_type type, void *type_data)
+{
+	const struct tc_taprio_qopt_offload *taprio = type_data;
+
+	if (type != TC_SETUP_QDISC_TAPRIO)
+		return -EOPNOTSUPP;
+
+	if (taprio->enable)
+		return hellcreek_port_set_schedule(ds, port, taprio);
+
+	return hellcreek_port_del_schedule(ds, port);
+}
+
 static const struct dsa_switch_ops hellcreek_ds_ops = {
 	.get_tag_protocol    = hellcreek_get_tag_protocol,
 	.setup		     = hellcreek_setup,

@@ -1104,6 +1394,7 @@ static const struct dsa_switch_ops hellcreek_ds_ops = {
 	.port_hwtstamp_get   = hellcreek_port_hwtstamp_get,
 	.port_txtstamp	     = hellcreek_port_txtstamp,
 	.port_rxtstamp	     = hellcreek_port_rxtstamp,
+	.port_setup_tc	     = hellcreek_port_setup_tc,
 	.get_ts_info	     = hellcreek_get_ts_info,
 };
 

@@ -1135,6 +1426,9 @@ static int hellcreek_probe(struct platform_device *pdev)
 		if (!port->counter_values)
 			return -ENOMEM;
 
+		hrtimer_init(&port->cycle_start_timer, CLOCK_TAI,
+			     HRTIMER_MODE_ABS);
+		port->cycle_start_timer.function = hellcreek_set_schedule;
 		port->hellcreek = hellcreek;
 		port->port	= i;
 	}

diff --git a/drivers/net/dsa/hirschmann/hellcreek.h b/drivers/net/dsa/hirschmann/hellcreek.h
index 1d3de72a48a5..d3d1a1144857 100644
--- a/drivers/net/dsa/hirschmann/hellcreek.h
+++ b/drivers/net/dsa/hirschmann/hellcreek.h

@@ -16,6 +16,7 @@
 #include <linux/ptp_clock_kernel.h>
 #include <linux/timecounter.h>
 #include <linux/spinlock.h>
+#include <linux/hrtimer.h>
 #include <net/dsa.h>
 
 /* Ports:

@@ -210,6 +211,20 @@ struct hellcreek_counter {
 	const char *name;
 };
 
+struct hellcreek_gcl_entry {
+	u32 interval;
+	u8 gate_states;
+	bool overrun_ignore;
+};
+
+struct hellcreek_schedule {
+	struct hellcreek_gcl_entry *entries;
+	size_t num_entries;
+	ktime_t base_time;
+	u32 cycle_time;
+	int port;
+};
+
 struct hellcreek;
 
 /* State flags for hellcreek_port_hwtstamp::state */

@@ -236,6 +251,8 @@ struct hellcreek_port_hwtstamp {
 
 struct hellcreek_port {
 	struct hellcreek *hellcreek;
+	struct hellcreek_schedule *current_schedule;
+	struct hrtimer cycle_start_timer;
 	int port;
 	u16 ptcfg;		/* ptcfg shadow */
 	u64 *counter_values;

@@ -273,4 +290,8 @@ struct hellcreek {
 	size_t fdb_entries;
 };
 
+#define hrtimer_to_hellcreek_port(timer)		\
+	container_of(timer, struct hellcreek_port,	\
+		     cycle_start_timer)
+
 #endif /* _HELLCREEK_H_ */

-- 
2.20.1