drivers/net/ethernet/microchip/lan966x/lan966x_taprio.c - linux - Git at Google

 // SPDX-License-Identifier: GPL-2.0+

 #include "lan966x_main.h"

 #define LAN966X_TAPRIO_TIMEOUT_MS		1000
 #define LAN966X_TAPRIO_ENTRIES_PER_PORT		2

 /* Minimum supported cycle time in nanoseconds */
 #define LAN966X_TAPRIO_MIN_CYCLE_TIME_NS	NSEC_PER_USEC

 /* Maximum supported cycle time in nanoseconds */
 #define LAN966X_TAPRIO_MAX_CYCLE_TIME_NS	(NSEC_PER_SEC - 1)

 /* Total number of TAS GCL entries */
 #define LAN966X_TAPRIO_NUM_GCL			256

 /* TAPRIO link speeds for calculation of guard band */
 enum lan966x_taprio_link_speed {
 	LAN966X_TAPRIO_SPEED_NO_GB,
 	LAN966X_TAPRIO_SPEED_10,
 	LAN966X_TAPRIO_SPEED_100,
 	LAN966X_TAPRIO_SPEED_1000,
 	LAN966X_TAPRIO_SPEED_2500,
 };

 /* TAPRIO list states */
 enum lan966x_taprio_state {
 	LAN966X_TAPRIO_STATE_ADMIN,
 	LAN966X_TAPRIO_STATE_ADVANCING,
 	LAN966X_TAPRIO_STATE_PENDING,
 	LAN966X_TAPRIO_STATE_OPERATING,
 	LAN966X_TAPRIO_STATE_TERMINATING,
 	LAN966X_TAPRIO_STATE_MAX,
 };

 /* TAPRIO GCL command */
 enum lan966x_taprio_gcl_cmd {
 	LAN966X_TAPRIO_GCL_CMD_SET_GATE_STATES = 0,
 };

 static u32 lan966x_taprio_list_index(struct lan966x_port *port, u8 entry)
 {
 	return port->chip_port * LAN966X_TAPRIO_ENTRIES_PER_PORT + entry;
 }

 static u32 lan966x_taprio_list_state_get(struct lan966x_port *port)
 {
 	struct lan966x *lan966x = port->lan966x;
 	u32 val;

 	val = lan_rd(lan966x, QSYS_TAS_LST);
 	return QSYS_TAS_LST_LIST_STATE_GET(val);
 }

 static u32 lan966x_taprio_list_index_state_get(struct lan966x_port *port,
 					       u32 list)
 {
 	struct lan966x *lan966x = port->lan966x;

 	lan_rmw(QSYS_TAS_CFG_CTRL_LIST_NUM_SET(list),
 		QSYS_TAS_CFG_CTRL_LIST_NUM,
 		lan966x, QSYS_TAS_CFG_CTRL);

 	return lan966x_taprio_list_state_get(port);
 }

 static void lan966x_taprio_list_state_set(struct lan966x_port *port,
 					  u32 state)
 {
 	struct lan966x *lan966x = port->lan966x;

 	lan_rmw(QSYS_TAS_LST_LIST_STATE_SET(state),
 		QSYS_TAS_LST_LIST_STATE,
 		lan966x, QSYS_TAS_LST);
 }

 static int lan966x_taprio_list_shutdown(struct lan966x_port *port,
 					u32 list)
 {
 	struct lan966x *lan966x = port->lan966x;
 	bool pending, operating;
 	unsigned long end;
 	u32 state;

 	end = jiffies +  msecs_to_jiffies(LAN966X_TAPRIO_TIMEOUT_MS);
 	/* It is required to try multiple times to set the state of list,
 	 * because the HW can overwrite this.
 	 */
 	do {
 		state = lan966x_taprio_list_state_get(port);

 		pending = false;
 		operating = false;

 		if (state == LAN966X_TAPRIO_STATE_ADVANCING ||
 		    state == LAN966X_TAPRIO_STATE_PENDING) {
 			lan966x_taprio_list_state_set(port,
 						      LAN966X_TAPRIO_STATE_ADMIN);
 			pending = true;
 		}

 		if (state == LAN966X_TAPRIO_STATE_OPERATING) {
 			lan966x_taprio_list_state_set(port,
 						      LAN966X_TAPRIO_STATE_TERMINATING);
 			operating = true;
 		}

 		/* If the entry was in pending and now gets in admin, then there
 		 * is nothing else to do, so just bail out
 		 */
 		state = lan966x_taprio_list_state_get(port);
 		if (pending &&
 		    state == LAN966X_TAPRIO_STATE_ADMIN)
 			return 0;

 		/* If the list was in operating and now is in terminating or
 		 * admin, then is OK to exit but it needs to wait until the list
 		 * will get in admin. It is not required to set the state
 		 * again.
 		 */
 		if (operating &&
 		    (state == LAN966X_TAPRIO_STATE_TERMINATING ||
 		     state == LAN966X_TAPRIO_STATE_ADMIN))
 			break;

 	} while (!time_after(jiffies, end));

 	end = jiffies + msecs_to_jiffies(LAN966X_TAPRIO_TIMEOUT_MS);
 	do {
 		state = lan966x_taprio_list_state_get(port);
 		if (state == LAN966X_TAPRIO_STATE_ADMIN)
 			break;

 	} while (!time_after(jiffies, end));

 	/* If the list was in operating mode, it could be stopped while some
 	 * queues where closed, so make sure to restore "all-queues-open"
 	 */
 	if (operating) {
 		lan_wr(QSYS_TAS_GS_CTRL_HSCH_POS_SET(port->chip_port),
 		       lan966x, QSYS_TAS_GS_CTRL);

 		lan_wr(QSYS_TAS_GATE_STATE_TAS_GATE_STATE_SET(0xff),
 		       lan966x, QSYS_TAS_GATE_STATE);
 	}

 	return 0;
 }

 static int lan966x_taprio_shutdown(struct lan966x_port *port)
 {
 	u32 i, list, state;
 	int err;

 	for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
 		list = lan966x_taprio_list_index(port, i);
 		state = lan966x_taprio_list_index_state_get(port, list);
 		if (state == LAN966X_TAPRIO_STATE_ADMIN)
 			continue;

 		err = lan966x_taprio_list_shutdown(port, list);
 		if (err)
 			return err;
 	}

 	return 0;
 }

 /* Find a suitable list for a new schedule. First priority is a list in state
  * pending. Second priority is a list in state admin.
  */
 static int lan966x_taprio_find_list(struct lan966x_port *port,
 				    struct tc_taprio_qopt_offload *qopt,
 				    int *new_list, int *obs_list)
 {
 	int state[LAN966X_TAPRIO_ENTRIES_PER_PORT];
 	int list[LAN966X_TAPRIO_ENTRIES_PER_PORT];
 	int err, oper = -1;
 	u32 i;

 	*new_list = -1;
 	*obs_list = -1;

 	/* If there is already an entry in operating mode, return this list in
 	 * obs_list, such that when the new list will get activated the
 	 * operating list will be stopped. In this way is possible to have
 	 * smooth transitions between the lists
 	 */
 	for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
 		list[i] = lan966x_taprio_list_index(port, i);
 		state[i] = lan966x_taprio_list_index_state_get(port, list[i]);
 		if (state[i] == LAN966X_TAPRIO_STATE_OPERATING)
 			oper = list[i];
 	}

 	for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
 		if (state[i] == LAN966X_TAPRIO_STATE_PENDING) {
 			err = lan966x_taprio_shutdown(port);
 			if (err)
 				return err;

 			*new_list = list[i];
 			*obs_list = (oper == -1) ? *new_list : oper;
 			return 0;
 		}
 	}

 	for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
 		if (state[i] == LAN966X_TAPRIO_STATE_ADMIN) {
 			*new_list = list[i];
 			*obs_list = (oper == -1) ? *new_list : oper;
 			return 0;
 		}
 	}

 	return -ENOSPC;
 }

 static int lan966x_taprio_check(struct tc_taprio_qopt_offload *qopt)
 {
 	u64 total_time = 0;
 	u32 i;

 	/* This is not supported by th HW */
 	if (qopt->cycle_time_extension)
 		return -EOPNOTSUPP;

 	/* There is a limited number of gcl entries that can be used, they are
 	 * shared by all ports
 	 */
 	if (qopt->num_entries > LAN966X_TAPRIO_NUM_GCL)
 		return -EINVAL;

 	/* Don't allow cycle times bigger than 1 sec or smaller than 1 usec */
 	if (qopt->cycle_time < LAN966X_TAPRIO_MIN_CYCLE_TIME_NS ||
 	    qopt->cycle_time > LAN966X_TAPRIO_MAX_CYCLE_TIME_NS)
 		return -EINVAL;

 	for (i = 0; i < qopt->num_entries; ++i) {
 		struct tc_taprio_sched_entry *entry = &qopt->entries[i];

 		/* Don't allow intervals bigger than 1 sec or smaller than 1
 		 * usec
 		 */
 		if (entry->interval < LAN966X_TAPRIO_MIN_CYCLE_TIME_NS ||
 		    entry->interval > LAN966X_TAPRIO_MAX_CYCLE_TIME_NS)
 			return -EINVAL;

 		if (qopt->entries[i].command != TC_TAPRIO_CMD_SET_GATES)
 			return -EINVAL;

 		total_time += qopt->entries[i].interval;
 	}

 	/* Don't allow the total time of intervals be bigger than 1 sec */
 	if (total_time > LAN966X_TAPRIO_MAX_CYCLE_TIME_NS)
 		return -EINVAL;

 	/* The HW expects that the cycle time to be at least as big as sum of
 	 * each interval of gcl
 	 */
 	if (qopt->cycle_time < total_time)
 		return -EINVAL;

 	return 0;
 }

 static int lan966x_taprio_gcl_free_get(struct lan966x_port *port,
 				       unsigned long *free_list)
 {
 	struct lan966x *lan966x = port->lan966x;
 	u32 num_free, state, list;
 	u32 base, next, max_list;

 	/* By default everything is free */
 	bitmap_fill(free_list, LAN966X_TAPRIO_NUM_GCL);
 	num_free = LAN966X_TAPRIO_NUM_GCL;

 	/* Iterate over all gcl entries and find out which are free. And mark
 	 * those that are not free.
 	 */
 	max_list = lan966x->num_phys_ports * LAN966X_TAPRIO_ENTRIES_PER_PORT;
 	for (list = 0; list < max_list; ++list) {
 		state = lan966x_taprio_list_index_state_get(port, list);
 		if (state == LAN966X_TAPRIO_STATE_ADMIN)
 			continue;

 		base = lan_rd(lan966x, QSYS_TAS_LIST_CFG);
 		base = QSYS_TAS_LIST_CFG_LIST_BASE_ADDR_GET(base);
 		next = base;

 		do {
 			clear_bit(next, free_list);
 			num_free--;

 			lan_rmw(QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM_SET(next),
 				QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM,
 				lan966x, QSYS_TAS_CFG_CTRL);

 			next = lan_rd(lan966x, QSYS_TAS_GCL_CT_CFG2);
 			next = QSYS_TAS_GCL_CT_CFG2_NEXT_GCL_GET(next);
 		} while (base != next);
 	}

 	return num_free;
 }

 static void lan966x_taprio_gcl_setup_entry(struct lan966x_port *port,
 					   struct tc_taprio_sched_entry *entry,
 					   u32 next_entry)
 {
 	struct lan966x *lan966x = port->lan966x;

 	/* Setup a single gcl entry */
 	lan_wr(QSYS_TAS_GCL_CT_CFG_GATE_STATE_SET(entry->gate_mask) |
 	       QSYS_TAS_GCL_CT_CFG_HSCH_POS_SET(port->chip_port) |
 	       QSYS_TAS_GCL_CT_CFG_OP_TYPE_SET(LAN966X_TAPRIO_GCL_CMD_SET_GATE_STATES),
 	       lan966x, QSYS_TAS_GCL_CT_CFG);

 	lan_wr(QSYS_TAS_GCL_CT_CFG2_PORT_PROFILE_SET(port->chip_port) |
 	       QSYS_TAS_GCL_CT_CFG2_NEXT_GCL_SET(next_entry),
 	       lan966x, QSYS_TAS_GCL_CT_CFG2);

 	lan_wr(entry->interval, lan966x, QSYS_TAS_GCL_TM_CFG);
 }

 static int lan966x_taprio_gcl_setup(struct lan966x_port *port,
 				    struct tc_taprio_qopt_offload *qopt,
 				    int list)
 {
 	DECLARE_BITMAP(free_list, LAN966X_TAPRIO_NUM_GCL);
 	struct lan966x *lan966x = port->lan966x;
 	u32 i, base, next;

 	if (lan966x_taprio_gcl_free_get(port, free_list) < qopt->num_entries)
 		return -ENOSPC;

 	/* Select list */
 	lan_rmw(QSYS_TAS_CFG_CTRL_LIST_NUM_SET(list),
 		QSYS_TAS_CFG_CTRL_LIST_NUM,
 		lan966x, QSYS_TAS_CFG_CTRL);

 	/* Setup the address of the first gcl entry */
 	base = find_first_bit(free_list, LAN966X_TAPRIO_NUM_GCL);
 	lan_rmw(QSYS_TAS_LIST_CFG_LIST_BASE_ADDR_SET(base),
 		QSYS_TAS_LIST_CFG_LIST_BASE_ADDR,
 		lan966x, QSYS_TAS_LIST_CFG);

 	/* Iterate over entries and add them to the gcl list */
 	next = base;
 	for (i = 0; i < qopt->num_entries; ++i) {
 		lan_rmw(QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM_SET(next),
 			QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM,
 			lan966x, QSYS_TAS_CFG_CTRL);

 		/* If the entry is last, point back to the start of the list */
 		if (i == qopt->num_entries - 1)
 			next = base;
 		else
 			next = find_next_bit(free_list, LAN966X_TAPRIO_NUM_GCL,
 					     next + 1);

 		lan966x_taprio_gcl_setup_entry(port, &qopt->entries[i], next);
 	}

 	return 0;
 }

 /* Calculate new base_time based on cycle_time. The HW recommends to have the
  * new base time at least 2 * cycle type + current time
  */
 static void lan966x_taprio_new_base_time(struct lan966x *lan966x,
 					 const u32 cycle_time,
 					 const ktime_t org_base_time,
 					 ktime_t *new_base_time)
 {
 	ktime_t current_time, threshold_time;
 	struct timespec64 ts;

 	/* Get the current time and calculate the threshold_time */
 	lan966x_ptp_gettime64(&lan966x->phc[LAN966X_PHC_PORT].info, &ts);
 	current_time = timespec64_to_ktime(ts);
 	threshold_time = current_time + (2 * cycle_time);

 	/* If the org_base_time is in enough in future just use it */
 	if (org_base_time >= threshold_time) {
 		*new_base_time = org_base_time;
 		return;
 	}

 	/* If the org_base_time is smaller than current_time, calculate the new
 	 * base time as following.
 	 */
 	if (org_base_time <= current_time) {
 		u64 tmp = current_time - org_base_time;
 		u32 rem = 0;

 		if (tmp > cycle_time)
 			div_u64_rem(tmp, cycle_time, &rem);
 		rem = cycle_time - rem;
 		*new_base_time = threshold_time + rem;
 		return;
 	}

 	/* The only left place for org_base_time is between current_time and
 	 * threshold_time. In this case the new_base_time is calculated like
 	 * org_base_time + 2 * cycletime
 	 */
 	*new_base_time = org_base_time + 2 * cycle_time;
 }

 int lan966x_taprio_speed_set(struct lan966x_port *port, int speed)
 {
 	struct lan966x *lan966x = port->lan966x;
 	u8 taprio_speed;

 	switch (speed) {
 	case SPEED_10:
 		taprio_speed = LAN966X_TAPRIO_SPEED_10;
 		break;
 	case SPEED_100:
 		taprio_speed = LAN966X_TAPRIO_SPEED_100;
 		break;
 	case SPEED_1000:
 		taprio_speed = LAN966X_TAPRIO_SPEED_1000;
 		break;
 	case SPEED_2500:
 		taprio_speed = LAN966X_TAPRIO_SPEED_2500;
 		break;
 	default:
 		return -EINVAL;
 	}

 	lan_rmw(QSYS_TAS_PROFILE_CFG_LINK_SPEED_SET(taprio_speed),
 		QSYS_TAS_PROFILE_CFG_LINK_SPEED,
 		lan966x, QSYS_TAS_PROFILE_CFG(port->chip_port));

 	return 0;
 }

 int lan966x_taprio_add(struct lan966x_port *port,
 		       struct tc_taprio_qopt_offload *qopt)
 {
 	struct lan966x *lan966x = port->lan966x;
 	int err, new_list, obs_list;
 	struct timespec64 ts;
 	ktime_t base_time;

 	err = lan966x_taprio_check(qopt);
 	if (err)
 		return err;

 	err = lan966x_taprio_find_list(port, qopt, &new_list, &obs_list);
 	if (err)
 		return err;

 	err = lan966x_taprio_gcl_setup(port, qopt, new_list);
 	if (err)
 		return err;

 	lan966x_taprio_new_base_time(lan966x, qopt->cycle_time,
 				     qopt->base_time, &base_time);

 	ts = ktime_to_timespec64(base_time);
 	lan_wr(QSYS_TAS_BT_NSEC_NSEC_SET(ts.tv_nsec),
 	       lan966x, QSYS_TAS_BT_NSEC);

 	lan_wr(lower_32_bits(ts.tv_sec),
 	       lan966x, QSYS_TAS_BT_SEC_LSB);

 	lan_wr(QSYS_TAS_BT_SEC_MSB_SEC_MSB_SET(upper_32_bits(ts.tv_sec)),
 	       lan966x, QSYS_TAS_BT_SEC_MSB);

 	lan_wr(qopt->cycle_time, lan966x, QSYS_TAS_CT_CFG);

 	lan_rmw(QSYS_TAS_STARTUP_CFG_OBSOLETE_IDX_SET(obs_list),
 		QSYS_TAS_STARTUP_CFG_OBSOLETE_IDX,
 		lan966x, QSYS_TAS_STARTUP_CFG);

 	/* Start list processing */
 	lan_rmw(QSYS_TAS_LST_LIST_STATE_SET(LAN966X_TAPRIO_STATE_ADVANCING),
 		QSYS_TAS_LST_LIST_STATE,
 		lan966x, QSYS_TAS_LST);

 	return err;
 }

 int lan966x_taprio_del(struct lan966x_port *port)
 {
 	return lan966x_taprio_shutdown(port);
 }

 void lan966x_taprio_init(struct lan966x *lan966x)
 {
 	int num_taprio_lists;
 	int p;

 	lan_wr(QSYS_TAS_STM_CFG_REVISIT_DLY_SET((256 * 1000) /
 						lan966x_ptp_get_period_ps()),
 	       lan966x, QSYS_TAS_STM_CFG);

 	num_taprio_lists = lan966x->num_phys_ports *
 			   LAN966X_TAPRIO_ENTRIES_PER_PORT;

 	/* For now we always use guard band on all queues */
 	lan_rmw(QSYS_TAS_CFG_CTRL_LIST_NUM_MAX_SET(num_taprio_lists) |
 		QSYS_TAS_CFG_CTRL_ALWAYS_GB_SCH_Q_SET(1),
 		QSYS_TAS_CFG_CTRL_LIST_NUM_MAX |
 		QSYS_TAS_CFG_CTRL_ALWAYS_GB_SCH_Q,
 		lan966x, QSYS_TAS_CFG_CTRL);

 	for (p = 0; p < lan966x->num_phys_ports; p++)
 		lan_rmw(QSYS_TAS_PROFILE_CFG_PORT_NUM_SET(p),
 			QSYS_TAS_PROFILE_CFG_PORT_NUM,
 			lan966x, QSYS_TAS_PROFILE_CFG(p));
 }

 void lan966x_taprio_deinit(struct lan966x *lan966x)
 {
 	int p;

 	for (p = 0; p < lan966x->num_phys_ports; ++p) {
 		if (!lan966x->ports[p])
 			continue;

 		lan966x_taprio_del(lan966x->ports[p]);
 	}
 }
	// SPDX-License-Identifier: GPL-2.0+

	#include "lan966x_main.h"

	#define LAN966X_TAPRIO_TIMEOUT_MS 1000
	#define LAN966X_TAPRIO_ENTRIES_PER_PORT 2

	/* Minimum supported cycle time in nanoseconds */
	#define LAN966X_TAPRIO_MIN_CYCLE_TIME_NS NSEC_PER_USEC

	/* Maximum supported cycle time in nanoseconds */
	#define LAN966X_TAPRIO_MAX_CYCLE_TIME_NS (NSEC_PER_SEC - 1)

	/* Total number of TAS GCL entries */
	#define LAN966X_TAPRIO_NUM_GCL 256

	/* TAPRIO link speeds for calculation of guard band */
	enum lan966x_taprio_link_speed {
	LAN966X_TAPRIO_SPEED_NO_GB,
	LAN966X_TAPRIO_SPEED_10,
	LAN966X_TAPRIO_SPEED_100,
	LAN966X_TAPRIO_SPEED_1000,
	LAN966X_TAPRIO_SPEED_2500,
	};

	/* TAPRIO list states */
	enum lan966x_taprio_state {
	LAN966X_TAPRIO_STATE_ADMIN,
	LAN966X_TAPRIO_STATE_ADVANCING,
	LAN966X_TAPRIO_STATE_PENDING,
	LAN966X_TAPRIO_STATE_OPERATING,
	LAN966X_TAPRIO_STATE_TERMINATING,
	LAN966X_TAPRIO_STATE_MAX,
	};

	/* TAPRIO GCL command */
	enum lan966x_taprio_gcl_cmd {
	LAN966X_TAPRIO_GCL_CMD_SET_GATE_STATES = 0,
	};

	static u32 lan966x_taprio_list_index(struct lan966x_port *port, u8 entry)
	{
	return port->chip_port * LAN966X_TAPRIO_ENTRIES_PER_PORT + entry;
	}

	static u32 lan966x_taprio_list_state_get(struct lan966x_port *port)
	{
	struct lan966x *lan966x = port->lan966x;
	u32 val;

	val = lan_rd(lan966x, QSYS_TAS_LST);
	return QSYS_TAS_LST_LIST_STATE_GET(val);
	}

	static u32 lan966x_taprio_list_index_state_get(struct lan966x_port *port,
	u32 list)
	{
	struct lan966x *lan966x = port->lan966x;

	lan_rmw(QSYS_TAS_CFG_CTRL_LIST_NUM_SET(list),
	QSYS_TAS_CFG_CTRL_LIST_NUM,
	lan966x, QSYS_TAS_CFG_CTRL);

	return lan966x_taprio_list_state_get(port);
	}

	static void lan966x_taprio_list_state_set(struct lan966x_port *port,
	u32 state)
	{
	struct lan966x *lan966x = port->lan966x;

	lan_rmw(QSYS_TAS_LST_LIST_STATE_SET(state),
	QSYS_TAS_LST_LIST_STATE,
	lan966x, QSYS_TAS_LST);
	}

	static int lan966x_taprio_list_shutdown(struct lan966x_port *port,
	u32 list)
	{
	struct lan966x *lan966x = port->lan966x;
	bool pending, operating;
	unsigned long end;
	u32 state;

	end = jiffies + msecs_to_jiffies(LAN966X_TAPRIO_TIMEOUT_MS);
	/* It is required to try multiple times to set the state of list,
	* because the HW can overwrite this.
	*/
	do {
	state = lan966x_taprio_list_state_get(port);

	pending = false;
	operating = false;

	if (state == LAN966X_TAPRIO_STATE_ADVANCING \|\|
	state == LAN966X_TAPRIO_STATE_PENDING) {
	lan966x_taprio_list_state_set(port,
	LAN966X_TAPRIO_STATE_ADMIN);
	pending = true;
	}

	if (state == LAN966X_TAPRIO_STATE_OPERATING) {
	lan966x_taprio_list_state_set(port,
	LAN966X_TAPRIO_STATE_TERMINATING);
	operating = true;
	}

	/* If the entry was in pending and now gets in admin, then there
	* is nothing else to do, so just bail out
	*/
	state = lan966x_taprio_list_state_get(port);
	if (pending &&
	state == LAN966X_TAPRIO_STATE_ADMIN)
	return 0;

	/* If the list was in operating and now is in terminating or
	* admin, then is OK to exit but it needs to wait until the list
	* will get in admin. It is not required to set the state
	* again.
	*/
	if (operating &&
	(state == LAN966X_TAPRIO_STATE_TERMINATING \|\|
	state == LAN966X_TAPRIO_STATE_ADMIN))
	break;

	} while (!time_after(jiffies, end));

	end = jiffies + msecs_to_jiffies(LAN966X_TAPRIO_TIMEOUT_MS);
	do {
	state = lan966x_taprio_list_state_get(port);
	if (state == LAN966X_TAPRIO_STATE_ADMIN)
	break;

	} while (!time_after(jiffies, end));

	/* If the list was in operating mode, it could be stopped while some
	* queues where closed, so make sure to restore "all-queues-open"
	*/
	if (operating) {
	lan_wr(QSYS_TAS_GS_CTRL_HSCH_POS_SET(port->chip_port),
	lan966x, QSYS_TAS_GS_CTRL);

	lan_wr(QSYS_TAS_GATE_STATE_TAS_GATE_STATE_SET(0xff),
	lan966x, QSYS_TAS_GATE_STATE);
	}

	return 0;
	}

	static int lan966x_taprio_shutdown(struct lan966x_port *port)
	{
	u32 i, list, state;
	int err;

	for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
	list = lan966x_taprio_list_index(port, i);
	state = lan966x_taprio_list_index_state_get(port, list);
	if (state == LAN966X_TAPRIO_STATE_ADMIN)
	continue;

	err = lan966x_taprio_list_shutdown(port, list);
	if (err)
	return err;
	}

	return 0;
	}

	/* Find a suitable list for a new schedule. First priority is a list in state
	* pending. Second priority is a list in state admin.
	*/
	static int lan966x_taprio_find_list(struct lan966x_port *port,
	struct tc_taprio_qopt_offload *qopt,
	int new_list, int obs_list)
	{
	int state[LAN966X_TAPRIO_ENTRIES_PER_PORT];
	int list[LAN966X_TAPRIO_ENTRIES_PER_PORT];
	int err, oper = -1;
	u32 i;

	*new_list = -1;
	*obs_list = -1;

	/* If there is already an entry in operating mode, return this list in
	* obs_list, such that when the new list will get activated the
	* operating list will be stopped. In this way is possible to have
	* smooth transitions between the lists
	*/
	for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
	list[i] = lan966x_taprio_list_index(port, i);
	state[i] = lan966x_taprio_list_index_state_get(port, list[i]);
	if (state[i] == LAN966X_TAPRIO_STATE_OPERATING)
	oper = list[i];
	}

	for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
	if (state[i] == LAN966X_TAPRIO_STATE_PENDING) {
	err = lan966x_taprio_shutdown(port);
	if (err)
	return err;

	*new_list = list[i];
	obs_list = (oper == -1) ? new_list : oper;
	return 0;
	}
	}

	for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
	if (state[i] == LAN966X_TAPRIO_STATE_ADMIN) {
	*new_list = list[i];
	obs_list = (oper == -1) ? new_list : oper;
	return 0;
	}
	}

	return -ENOSPC;
	}

	static int lan966x_taprio_check(struct tc_taprio_qopt_offload *qopt)
	{
	u64 total_time = 0;
	u32 i;

	/* This is not supported by th HW */
	if (qopt->cycle_time_extension)
	return -EOPNOTSUPP;

	/* There is a limited number of gcl entries that can be used, they are
	* shared by all ports
	*/
	if (qopt->num_entries > LAN966X_TAPRIO_NUM_GCL)
	return -EINVAL;

	/* Don't allow cycle times bigger than 1 sec or smaller than 1 usec */
	if (qopt->cycle_time < LAN966X_TAPRIO_MIN_CYCLE_TIME_NS \|\|
	qopt->cycle_time > LAN966X_TAPRIO_MAX_CYCLE_TIME_NS)
	return -EINVAL;

	for (i = 0; i < qopt->num_entries; ++i) {
	struct tc_taprio_sched_entry *entry = &qopt->entries[i];

	/* Don't allow intervals bigger than 1 sec or smaller than 1
	* usec
	*/
	if (entry->interval < LAN966X_TAPRIO_MIN_CYCLE_TIME_NS \|\|
	entry->interval > LAN966X_TAPRIO_MAX_CYCLE_TIME_NS)
	return -EINVAL;

	if (qopt->entries[i].command != TC_TAPRIO_CMD_SET_GATES)
	return -EINVAL;

	total_time += qopt->entries[i].interval;
	}

	/* Don't allow the total time of intervals be bigger than 1 sec */
	if (total_time > LAN966X_TAPRIO_MAX_CYCLE_TIME_NS)
	return -EINVAL;

	/* The HW expects that the cycle time to be at least as big as sum of
	* each interval of gcl
	*/
	if (qopt->cycle_time < total_time)
	return -EINVAL;

	return 0;
	}

	static int lan966x_taprio_gcl_free_get(struct lan966x_port *port,
	unsigned long *free_list)
	{
	struct lan966x *lan966x = port->lan966x;
	u32 num_free, state, list;
	u32 base, next, max_list;

	/* By default everything is free */
	bitmap_fill(free_list, LAN966X_TAPRIO_NUM_GCL);
	num_free = LAN966X_TAPRIO_NUM_GCL;

	/* Iterate over all gcl entries and find out which are free. And mark
	* those that are not free.
	*/
	max_list = lan966x->num_phys_ports * LAN966X_TAPRIO_ENTRIES_PER_PORT;
	for (list = 0; list < max_list; ++list) {
	state = lan966x_taprio_list_index_state_get(port, list);
	if (state == LAN966X_TAPRIO_STATE_ADMIN)
	continue;

	base = lan_rd(lan966x, QSYS_TAS_LIST_CFG);
	base = QSYS_TAS_LIST_CFG_LIST_BASE_ADDR_GET(base);
	next = base;

	do {
	clear_bit(next, free_list);
	num_free--;

	lan_rmw(QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM_SET(next),
	QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM,
	lan966x, QSYS_TAS_CFG_CTRL);

	next = lan_rd(lan966x, QSYS_TAS_GCL_CT_CFG2);
	next = QSYS_TAS_GCL_CT_CFG2_NEXT_GCL_GET(next);
	} while (base != next);
	}

	return num_free;
	}

	static void lan966x_taprio_gcl_setup_entry(struct lan966x_port *port,
	struct tc_taprio_sched_entry *entry,
	u32 next_entry)
	{
	struct lan966x *lan966x = port->lan966x;

	/* Setup a single gcl entry */
	lan_wr(QSYS_TAS_GCL_CT_CFG_GATE_STATE_SET(entry->gate_mask) \|
	QSYS_TAS_GCL_CT_CFG_HSCH_POS_SET(port->chip_port) \|
	QSYS_TAS_GCL_CT_CFG_OP_TYPE_SET(LAN966X_TAPRIO_GCL_CMD_SET_GATE_STATES),
	lan966x, QSYS_TAS_GCL_CT_CFG);

	lan_wr(QSYS_TAS_GCL_CT_CFG2_PORT_PROFILE_SET(port->chip_port) \|
	QSYS_TAS_GCL_CT_CFG2_NEXT_GCL_SET(next_entry),
	lan966x, QSYS_TAS_GCL_CT_CFG2);

	lan_wr(entry->interval, lan966x, QSYS_TAS_GCL_TM_CFG);
	}

	static int lan966x_taprio_gcl_setup(struct lan966x_port *port,
	struct tc_taprio_qopt_offload *qopt,
	int list)
	{
	DECLARE_BITMAP(free_list, LAN966X_TAPRIO_NUM_GCL);
	struct lan966x *lan966x = port->lan966x;
	u32 i, base, next;

	if (lan966x_taprio_gcl_free_get(port, free_list) < qopt->num_entries)
	return -ENOSPC;

	/* Select list */
	lan_rmw(QSYS_TAS_CFG_CTRL_LIST_NUM_SET(list),
	QSYS_TAS_CFG_CTRL_LIST_NUM,
	lan966x, QSYS_TAS_CFG_CTRL);

	/* Setup the address of the first gcl entry */
	base = find_first_bit(free_list, LAN966X_TAPRIO_NUM_GCL);
	lan_rmw(QSYS_TAS_LIST_CFG_LIST_BASE_ADDR_SET(base),
	QSYS_TAS_LIST_CFG_LIST_BASE_ADDR,
	lan966x, QSYS_TAS_LIST_CFG);

	/* Iterate over entries and add them to the gcl list */
	next = base;
	for (i = 0; i < qopt->num_entries; ++i) {
	lan_rmw(QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM_SET(next),
	QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM,
	lan966x, QSYS_TAS_CFG_CTRL);

	/* If the entry is last, point back to the start of the list */
	if (i == qopt->num_entries - 1)
	next = base;
	else
	next = find_next_bit(free_list, LAN966X_TAPRIO_NUM_GCL,
	next + 1);

	lan966x_taprio_gcl_setup_entry(port, &qopt->entries[i], next);
	}

	return 0;
	}

	/* Calculate new base_time based on cycle_time. The HW recommends to have the
	* new base time at least 2 * cycle type + current time
	*/
	static void lan966x_taprio_new_base_time(struct lan966x *lan966x,
	const u32 cycle_time,
	const ktime_t org_base_time,
	ktime_t *new_base_time)
	{
	ktime_t current_time, threshold_time;
	struct timespec64 ts;

	/* Get the current time and calculate the threshold_time */
	lan966x_ptp_gettime64(&lan966x->phc[LAN966X_PHC_PORT].info, &ts);
	current_time = timespec64_to_ktime(ts);
	threshold_time = current_time + (2 * cycle_time);

	/* If the org_base_time is in enough in future just use it */
	if (org_base_time >= threshold_time) {
	*new_base_time = org_base_time;
	return;
	}

	/* If the org_base_time is smaller than current_time, calculate the new
	* base time as following.
	*/
	if (org_base_time <= current_time) {
	u64 tmp = current_time - org_base_time;
	u32 rem = 0;

	if (tmp > cycle_time)
	div_u64_rem(tmp, cycle_time, &rem);
	rem = cycle_time - rem;
	*new_base_time = threshold_time + rem;
	return;
	}

	/* The only left place for org_base_time is between current_time and
	* threshold_time. In this case the new_base_time is calculated like
	* org_base_time + 2 * cycletime
	*/
	new_base_time = org_base_time + 2 cycle_time;
	}

	int lan966x_taprio_speed_set(struct lan966x_port *port, int speed)
	{
	struct lan966x *lan966x = port->lan966x;
	u8 taprio_speed;

	switch (speed) {
	case SPEED_10:
	taprio_speed = LAN966X_TAPRIO_SPEED_10;
	break;
	case SPEED_100:
	taprio_speed = LAN966X_TAPRIO_SPEED_100;
	break;
	case SPEED_1000:
	taprio_speed = LAN966X_TAPRIO_SPEED_1000;
	break;
	case SPEED_2500:
	taprio_speed = LAN966X_TAPRIO_SPEED_2500;
	break;
	default:
	return -EINVAL;
	}

	lan_rmw(QSYS_TAS_PROFILE_CFG_LINK_SPEED_SET(taprio_speed),
	QSYS_TAS_PROFILE_CFG_LINK_SPEED,
	lan966x, QSYS_TAS_PROFILE_CFG(port->chip_port));

	return 0;
	}

	int lan966x_taprio_add(struct lan966x_port *port,
	struct tc_taprio_qopt_offload *qopt)
	{
	struct lan966x *lan966x = port->lan966x;
	int err, new_list, obs_list;
	struct timespec64 ts;
	ktime_t base_time;

	err = lan966x_taprio_check(qopt);
	if (err)
	return err;

	err = lan966x_taprio_find_list(port, qopt, &new_list, &obs_list);
	if (err)
	return err;

	err = lan966x_taprio_gcl_setup(port, qopt, new_list);
	if (err)
	return err;

	lan966x_taprio_new_base_time(lan966x, qopt->cycle_time,
	qopt->base_time, &base_time);

	ts = ktime_to_timespec64(base_time);
	lan_wr(QSYS_TAS_BT_NSEC_NSEC_SET(ts.tv_nsec),
	lan966x, QSYS_TAS_BT_NSEC);

	lan_wr(lower_32_bits(ts.tv_sec),
	lan966x, QSYS_TAS_BT_SEC_LSB);

	lan_wr(QSYS_TAS_BT_SEC_MSB_SEC_MSB_SET(upper_32_bits(ts.tv_sec)),
	lan966x, QSYS_TAS_BT_SEC_MSB);

	lan_wr(qopt->cycle_time, lan966x, QSYS_TAS_CT_CFG);

	lan_rmw(QSYS_TAS_STARTUP_CFG_OBSOLETE_IDX_SET(obs_list),
	QSYS_TAS_STARTUP_CFG_OBSOLETE_IDX,
	lan966x, QSYS_TAS_STARTUP_CFG);

	/* Start list processing */
	lan_rmw(QSYS_TAS_LST_LIST_STATE_SET(LAN966X_TAPRIO_STATE_ADVANCING),
	QSYS_TAS_LST_LIST_STATE,
	lan966x, QSYS_TAS_LST);

	return err;
	}

	int lan966x_taprio_del(struct lan966x_port *port)
	{
	return lan966x_taprio_shutdown(port);
	}

	void lan966x_taprio_init(struct lan966x *lan966x)
	{
	int num_taprio_lists;
	int p;

	lan_wr(QSYS_TAS_STM_CFG_REVISIT_DLY_SET((256 * 1000) /
	lan966x_ptp_get_period_ps()),
	lan966x, QSYS_TAS_STM_CFG);

	num_taprio_lists = lan966x->num_phys_ports *
	LAN966X_TAPRIO_ENTRIES_PER_PORT;

	/* For now we always use guard band on all queues */
	lan_rmw(QSYS_TAS_CFG_CTRL_LIST_NUM_MAX_SET(num_taprio_lists) \|
	QSYS_TAS_CFG_CTRL_ALWAYS_GB_SCH_Q_SET(1),
	QSYS_TAS_CFG_CTRL_LIST_NUM_MAX \|
	QSYS_TAS_CFG_CTRL_ALWAYS_GB_SCH_Q,
	lan966x, QSYS_TAS_CFG_CTRL);

	for (p = 0; p < lan966x->num_phys_ports; p++)
	lan_rmw(QSYS_TAS_PROFILE_CFG_PORT_NUM_SET(p),
	QSYS_TAS_PROFILE_CFG_PORT_NUM,
	lan966x, QSYS_TAS_PROFILE_CFG(p));
	}

	void lan966x_taprio_deinit(struct lan966x *lan966x)
	{
	int p;

	for (p = 0; p < lan966x->num_phys_ports; ++p) {
	if (!lan966x->ports[p])
	continue;

	lan966x_taprio_del(lan966x->ports[p]);
	}
	}