drivers/net/ethernet/engleder/tsnep_tc.c - linux - Git at Google

 // SPDX-License-Identifier: GPL-2.0
 /* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */

 #include "tsnep.h"

 #include <net/pkt_sched.h>

 /* save one operation at the end for additional operation at list change */
 #define TSNEP_MAX_GCL_NUM (TSNEP_GCL_COUNT - 1)

 static int tsnep_validate_gcl(struct tc_taprio_qopt_offload *qopt)
 {
 	int i;
 	u64 cycle_time;

 	if (!qopt->cycle_time)
 		return -ERANGE;
 	if (qopt->num_entries > TSNEP_MAX_GCL_NUM)
 		return -EINVAL;
 	cycle_time = 0;
 	for (i = 0; i < qopt->num_entries; i++) {
 		if (qopt->entries[i].command != TC_TAPRIO_CMD_SET_GATES)
 			return -EINVAL;
 		if (qopt->entries[i].gate_mask & ~TSNEP_GCL_MASK)
 			return -EINVAL;
 		if (qopt->entries[i].interval < TSNEP_GCL_MIN_INTERVAL)
 			return -EINVAL;
 		cycle_time += qopt->entries[i].interval;
 	}
 	if (qopt->cycle_time != cycle_time)
 		return -EINVAL;
 	if (qopt->cycle_time_extension >= qopt->cycle_time)
 		return -EINVAL;

 	return 0;
 }

 static void tsnep_write_gcl_operation(struct tsnep_gcl *gcl, int index,
 				      u32 properties, u32 interval, bool flush)
 {
 	void __iomem *addr = gcl->addr +
 			     sizeof(struct tsnep_gcl_operation) * index;

 	gcl->operation[index].properties = properties;
 	gcl->operation[index].interval = interval;

 	iowrite32(properties, addr);
 	iowrite32(interval, addr + sizeof(u32));

 	if (flush) {
 		/* flush write with read access */
 		ioread32(addr);
 	}
 }

 static u64 tsnep_change_duration(struct tsnep_gcl *gcl, int index)
 {
 	u64 duration;
 	int count;

 	/* change needs to be triggered one or two operations before start of
 	 * new gate control list
 	 * - change is triggered at start of operation (minimum one operation)
 	 * - operation with adjusted interval is inserted on demand to exactly
 	 *   meet the start of the new gate control list (optional)
 	 *
 	 * additionally properties are read directly after start of previous
 	 * operation
 	 *
 	 * therefore, three operations needs to be considered for the limit
 	 */
 	duration = 0;
 	count = 3;
 	while (count) {
 		duration += gcl->operation[index].interval;

 		index--;
 		if (index < 0)
 			index = gcl->count - 1;

 		count--;
 	}

 	return duration;
 }

 static void tsnep_write_gcl(struct tsnep_gcl *gcl,
 			    struct tc_taprio_qopt_offload *qopt)
 {
 	int i;
 	u32 properties;
 	u64 extend;
 	u64 cut;

 	gcl->base_time = ktime_to_ns(qopt->base_time);
 	gcl->cycle_time = qopt->cycle_time;
 	gcl->cycle_time_extension = qopt->cycle_time_extension;

 	for (i = 0; i < qopt->num_entries; i++) {
 		properties = qopt->entries[i].gate_mask;
 		if (i == (qopt->num_entries - 1))
 			properties |= TSNEP_GCL_LAST;

 		tsnep_write_gcl_operation(gcl, i, properties,
 					  qopt->entries[i].interval, true);
 	}
 	gcl->count = qopt->num_entries;

 	/* calculate change limit; i.e., the time needed between enable and
 	 * start of new gate control list
 	 */

 	/* case 1: extend cycle time for change
 	 * - change duration of last operation
 	 * - cycle time extension
 	 */
 	extend = tsnep_change_duration(gcl, gcl->count - 1);
 	extend += gcl->cycle_time_extension;

 	/* case 2: cut cycle time for change
 	 * - maximum change duration
 	 */
 	cut = 0;
 	for (i = 0; i < gcl->count; i++)
 		cut = max(cut, tsnep_change_duration(gcl, i));

 	/* use maximum, because the actual case (extend or cut) can be
 	 * determined only after limit is known (chicken-and-egg problem)
 	 */
 	gcl->change_limit = max(extend, cut);
 }

 static u64 tsnep_gcl_start_after(struct tsnep_gcl *gcl, u64 limit)
 {
 	u64 start = gcl->base_time;
 	u64 n;

 	if (start <= limit) {
 		n = div64_u64(limit - start, gcl->cycle_time);
 		start += (n + 1) * gcl->cycle_time;
 	}

 	return start;
 }

 static u64 tsnep_gcl_start_before(struct tsnep_gcl *gcl, u64 limit)
 {
 	u64 start = gcl->base_time;
 	u64 n;

 	n = div64_u64(limit - start, gcl->cycle_time);
 	start += n * gcl->cycle_time;
 	if (start == limit)
 		start -= gcl->cycle_time;

 	return start;
 }

 static u64 tsnep_set_gcl_change(struct tsnep_gcl *gcl, int index, u64 change,
 				bool insert)
 {
 	/* previous operation triggers change and properties are evaluated at
 	 * start of operation
 	 */
 	if (index == 0)
 		index = gcl->count - 1;
 	else
 		index = index - 1;
 	change -= gcl->operation[index].interval;

 	/* optionally change to new list with additional operation in between */
 	if (insert) {
 		void __iomem *addr = gcl->addr +
 				     sizeof(struct tsnep_gcl_operation) * index;

 		gcl->operation[index].properties |= TSNEP_GCL_INSERT;
 		iowrite32(gcl->operation[index].properties, addr);
 	}

 	return change;
 }

 static void tsnep_clean_gcl(struct tsnep_gcl *gcl)
 {
 	int i;
 	u32 mask = TSNEP_GCL_LAST | TSNEP_GCL_MASK;
 	void __iomem *addr;

 	/* search for insert operation and reset properties */
 	for (i = 0; i < gcl->count; i++) {
 		if (gcl->operation[i].properties & ~mask) {
 			addr = gcl->addr +
 			       sizeof(struct tsnep_gcl_operation) * i;

 			gcl->operation[i].properties &= mask;
 			iowrite32(gcl->operation[i].properties, addr);

 			break;
 		}
 	}
 }

 static u64 tsnep_insert_gcl_operation(struct tsnep_gcl *gcl, int ref,
 				      u64 change, u32 interval)
 {
 	u32 properties;

 	properties = gcl->operation[ref].properties & TSNEP_GCL_MASK;
 	/* change to new list directly after inserted operation */
 	properties |= TSNEP_GCL_CHANGE;

 	/* last operation of list is reserved to insert operation */
 	tsnep_write_gcl_operation(gcl, TSNEP_GCL_COUNT - 1, properties,
 				  interval, false);

 	return tsnep_set_gcl_change(gcl, ref, change, true);
 }

 static u64 tsnep_extend_gcl(struct tsnep_gcl *gcl, u64 start, u32 extension)
 {
 	int ref = gcl->count - 1;
 	u32 interval = gcl->operation[ref].interval + extension;

 	start -= gcl->operation[ref].interval;

 	return tsnep_insert_gcl_operation(gcl, ref, start, interval);
 }

 static u64 tsnep_cut_gcl(struct tsnep_gcl *gcl, u64 start, u64 cycle_time)
 {
 	u64 sum = 0;
 	int i;

 	/* find operation which shall be cutted */
 	for (i = 0; i < gcl->count; i++) {
 		u64 sum_tmp = sum + gcl->operation[i].interval;
 		u64 interval;

 		/* sum up operations as long as cycle time is not exceeded */
 		if (sum_tmp > cycle_time)
 			break;

 		/* remaining interval must be big enough for hardware */
 		interval = cycle_time - sum_tmp;
 		if (interval > 0 && interval < TSNEP_GCL_MIN_INTERVAL)
 			break;

 		sum = sum_tmp;
 	}
 	if (sum == cycle_time) {
 		/* no need to cut operation itself or whole cycle
 		 * => change exactly at operation
 		 */
 		return tsnep_set_gcl_change(gcl, i, start + sum, false);
 	}
 	return tsnep_insert_gcl_operation(gcl, i, start + sum,
 					  cycle_time - sum);
 }

 static int tsnep_enable_gcl(struct tsnep_adapter *adapter,
 			    struct tsnep_gcl *gcl, struct tsnep_gcl *curr)
 {
 	u64 system_time;
 	u64 timeout;
 	u64 limit;

 	/* estimate timeout limit after timeout enable, actually timeout limit
 	 * in hardware will be earlier than estimate so we are on the safe side
 	 */
 	tsnep_get_system_time(adapter, &system_time);
 	timeout = system_time + TSNEP_GC_TIMEOUT;

 	if (curr)
 		limit = timeout + curr->change_limit;
 	else
 		limit = timeout;

 	gcl->start_time = tsnep_gcl_start_after(gcl, limit);

 	/* gate control time register is only 32bit => time shall be in the near
 	 * future (no driver support for far future implemented)
 	 */
 	if ((gcl->start_time - system_time) >= U32_MAX)
 		return -EAGAIN;

 	if (curr) {
 		/* change gate control list */
 		u64 last;
 		u64 change;

 		last = tsnep_gcl_start_before(curr, gcl->start_time);
 		if ((last + curr->cycle_time) == gcl->start_time)
 			change = tsnep_cut_gcl(curr, last,
 					       gcl->start_time - last);
 		else if (((gcl->start_time - last) <=
 			  curr->cycle_time_extension) ||
 			 ((gcl->start_time - last) <= TSNEP_GCL_MIN_INTERVAL))
 			change = tsnep_extend_gcl(curr, last,
 						  gcl->start_time - last);
 		else
 			change = tsnep_cut_gcl(curr, last,
 					       gcl->start_time - last);

 		WARN_ON(change <= timeout);
 		gcl->change = true;
 		iowrite32(change & 0xFFFFFFFF, adapter->addr + TSNEP_GC_CHANGE);
 	} else {
 		/* start gate control list */
 		WARN_ON(gcl->start_time <= timeout);
 		gcl->change = false;
 		iowrite32(gcl->start_time & 0xFFFFFFFF,
 			  adapter->addr + TSNEP_GC_TIME);
 	}

 	return 0;
 }

 static int tsnep_taprio(struct tsnep_adapter *adapter,
 			struct tc_taprio_qopt_offload *qopt)
 {
 	struct tsnep_gcl *gcl;
 	struct tsnep_gcl *curr;
 	int retval;

 	if (!adapter->gate_control)
 		return -EOPNOTSUPP;

 	if (qopt->cmd == TAPRIO_CMD_DESTROY) {
 		/* disable gate control if active */
 		mutex_lock(&adapter->gate_control_lock);

 		if (adapter->gate_control_active) {
 			iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
 			adapter->gate_control_active = false;
 		}

 		mutex_unlock(&adapter->gate_control_lock);

 		return 0;
 	} else if (qopt->cmd != TAPRIO_CMD_REPLACE) {
 		return -EOPNOTSUPP;
 	}

 	retval = tsnep_validate_gcl(qopt);
 	if (retval)
 		return retval;

 	mutex_lock(&adapter->gate_control_lock);

 	gcl = &adapter->gcl[adapter->next_gcl];
 	tsnep_write_gcl(gcl, qopt);

 	/* select current gate control list if active */
 	if (adapter->gate_control_active) {
 		if (adapter->next_gcl == 0)
 			curr = &adapter->gcl[1];
 		else
 			curr = &adapter->gcl[0];
 	} else {
 		curr = NULL;
 	}

 	for (;;) {
 		/* start timeout which discards late enable, this helps ensuring
 		 * that start/change time are in the future at enable
 		 */
 		iowrite8(TSNEP_GC_ENABLE_TIMEOUT, adapter->addr + TSNEP_GC);

 		retval = tsnep_enable_gcl(adapter, gcl, curr);
 		if (retval) {
 			mutex_unlock(&adapter->gate_control_lock);

 			return retval;
 		}

 		/* enable gate control list */
 		if (adapter->next_gcl == 0)
 			iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);
 		else
 			iowrite8(TSNEP_GC_ENABLE_B, adapter->addr + TSNEP_GC);

 		/* done if timeout did not happen */
 		if (!(ioread32(adapter->addr + TSNEP_GC) &
 		      TSNEP_GC_TIMEOUT_SIGNAL))
 			break;

 		/* timeout is acknowledged with any enable */
 		iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);

 		if (curr)
 			tsnep_clean_gcl(curr);

 		/* retry because of timeout */
 	}

 	adapter->gate_control_active = true;

 	if (adapter->next_gcl == 0)
 		adapter->next_gcl = 1;
 	else
 		adapter->next_gcl = 0;

 	mutex_unlock(&adapter->gate_control_lock);

 	return 0;
 }

 static int tsnep_tc_query_caps(struct tsnep_adapter *adapter,
 			       struct tc_query_caps_base *base)
 {
 	switch (base->type) {
 	case TC_SETUP_QDISC_TAPRIO: {
 		struct tc_taprio_caps *caps = base->caps;

 		if (!adapter->gate_control)
 			return -EOPNOTSUPP;

 		caps->gate_mask_per_txq = true;

 		return 0;
 	}
 	default:
 		return -EOPNOTSUPP;
 	}
 }

 int tsnep_tc_setup(struct net_device *netdev, enum tc_setup_type type,
 		   void *type_data)
 {
 	struct tsnep_adapter *adapter = netdev_priv(netdev);

 	switch (type) {
 	case TC_QUERY_CAPS:
 		return tsnep_tc_query_caps(adapter, type_data);
 	case TC_SETUP_QDISC_TAPRIO:
 		return tsnep_taprio(adapter, type_data);
 	default:
 		return -EOPNOTSUPP;
 	}
 }

 int tsnep_tc_init(struct tsnep_adapter *adapter)
 {
 	if (!adapter->gate_control)
 		return 0;

 	/* open all gates */
 	iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
 	iowrite32(TSNEP_GC_OPEN | TSNEP_GC_NEXT_OPEN, adapter->addr + TSNEP_GC);

 	adapter->gcl[0].addr = adapter->addr + TSNEP_GCL_A;
 	adapter->gcl[1].addr = adapter->addr + TSNEP_GCL_B;

 	return 0;
 }

 void tsnep_tc_cleanup(struct tsnep_adapter *adapter)
 {
 	if (!adapter->gate_control)
 		return;

 	if (adapter->gate_control_active) {
 		iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
 		adapter->gate_control_active = false;
 	}
 }
	// SPDX-License-Identifier: GPL-2.0
	/* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */

	#include "tsnep.h"

	#include <net/pkt_sched.h>

	/* save one operation at the end for additional operation at list change */
	#define TSNEP_MAX_GCL_NUM (TSNEP_GCL_COUNT - 1)

	static int tsnep_validate_gcl(struct tc_taprio_qopt_offload *qopt)
	{
	int i;
	u64 cycle_time;

	if (!qopt->cycle_time)
	return -ERANGE;
	if (qopt->num_entries > TSNEP_MAX_GCL_NUM)
	return -EINVAL;
	cycle_time = 0;
	for (i = 0; i < qopt->num_entries; i++) {
	if (qopt->entries[i].command != TC_TAPRIO_CMD_SET_GATES)
	return -EINVAL;
	if (qopt->entries[i].gate_mask & ~TSNEP_GCL_MASK)
	return -EINVAL;
	if (qopt->entries[i].interval < TSNEP_GCL_MIN_INTERVAL)
	return -EINVAL;
	cycle_time += qopt->entries[i].interval;
	}
	if (qopt->cycle_time != cycle_time)
	return -EINVAL;
	if (qopt->cycle_time_extension >= qopt->cycle_time)
	return -EINVAL;

	return 0;
	}

	static void tsnep_write_gcl_operation(struct tsnep_gcl *gcl, int index,
	u32 properties, u32 interval, bool flush)
	{
	void __iomem *addr = gcl->addr +
	sizeof(struct tsnep_gcl_operation) * index;

	gcl->operation[index].properties = properties;
	gcl->operation[index].interval = interval;

	iowrite32(properties, addr);
	iowrite32(interval, addr + sizeof(u32));

	if (flush) {
	/* flush write with read access */
	ioread32(addr);
	}
	}

	static u64 tsnep_change_duration(struct tsnep_gcl *gcl, int index)
	{
	u64 duration;
	int count;

	/* change needs to be triggered one or two operations before start of
	* new gate control list
	* - change is triggered at start of operation (minimum one operation)
	* - operation with adjusted interval is inserted on demand to exactly
	* meet the start of the new gate control list (optional)
	*
	* additionally properties are read directly after start of previous
	* operation
	*
	* therefore, three operations needs to be considered for the limit
	*/
	duration = 0;
	count = 3;
	while (count) {
	duration += gcl->operation[index].interval;

	index--;
	if (index < 0)
	index = gcl->count - 1;

	count--;
	}

	return duration;
	}

	static void tsnep_write_gcl(struct tsnep_gcl *gcl,
	struct tc_taprio_qopt_offload *qopt)
	{
	int i;
	u32 properties;
	u64 extend;
	u64 cut;

	gcl->base_time = ktime_to_ns(qopt->base_time);
	gcl->cycle_time = qopt->cycle_time;
	gcl->cycle_time_extension = qopt->cycle_time_extension;

	for (i = 0; i < qopt->num_entries; i++) {
	properties = qopt->entries[i].gate_mask;
	if (i == (qopt->num_entries - 1))
	properties \|= TSNEP_GCL_LAST;

	tsnep_write_gcl_operation(gcl, i, properties,
	qopt->entries[i].interval, true);
	}
	gcl->count = qopt->num_entries;

	/* calculate change limit; i.e., the time needed between enable and
	* start of new gate control list
	*/

	/* case 1: extend cycle time for change
	* - change duration of last operation
	* - cycle time extension
	*/
	extend = tsnep_change_duration(gcl, gcl->count - 1);
	extend += gcl->cycle_time_extension;

	/* case 2: cut cycle time for change
	* - maximum change duration
	*/
	cut = 0;
	for (i = 0; i < gcl->count; i++)
	cut = max(cut, tsnep_change_duration(gcl, i));

	/* use maximum, because the actual case (extend or cut) can be
	* determined only after limit is known (chicken-and-egg problem)
	*/
	gcl->change_limit = max(extend, cut);
	}

	static u64 tsnep_gcl_start_after(struct tsnep_gcl *gcl, u64 limit)
	{
	u64 start = gcl->base_time;
	u64 n;

	if (start <= limit) {
	n = div64_u64(limit - start, gcl->cycle_time);
	start += (n + 1) * gcl->cycle_time;
	}

	return start;
	}

	static u64 tsnep_gcl_start_before(struct tsnep_gcl *gcl, u64 limit)
	{
	u64 start = gcl->base_time;
	u64 n;

	n = div64_u64(limit - start, gcl->cycle_time);
	start += n * gcl->cycle_time;
	if (start == limit)
	start -= gcl->cycle_time;

	return start;
	}

	static u64 tsnep_set_gcl_change(struct tsnep_gcl *gcl, int index, u64 change,
	bool insert)
	{
	/* previous operation triggers change and properties are evaluated at
	* start of operation
	*/
	if (index == 0)
	index = gcl->count - 1;
	else
	index = index - 1;
	change -= gcl->operation[index].interval;

	/* optionally change to new list with additional operation in between */
	if (insert) {
	void __iomem *addr = gcl->addr +
	sizeof(struct tsnep_gcl_operation) * index;

	gcl->operation[index].properties \|= TSNEP_GCL_INSERT;
	iowrite32(gcl->operation[index].properties, addr);
	}

	return change;
	}

	static void tsnep_clean_gcl(struct tsnep_gcl *gcl)
	{
	int i;
	u32 mask = TSNEP_GCL_LAST \| TSNEP_GCL_MASK;
	void __iomem *addr;

	/* search for insert operation and reset properties */
	for (i = 0; i < gcl->count; i++) {
	if (gcl->operation[i].properties & ~mask) {
	addr = gcl->addr +
	sizeof(struct tsnep_gcl_operation) * i;

	gcl->operation[i].properties &= mask;
	iowrite32(gcl->operation[i].properties, addr);

	break;
	}
	}
	}

	static u64 tsnep_insert_gcl_operation(struct tsnep_gcl *gcl, int ref,
	u64 change, u32 interval)
	{
	u32 properties;

	properties = gcl->operation[ref].properties & TSNEP_GCL_MASK;
	/* change to new list directly after inserted operation */
	properties \|= TSNEP_GCL_CHANGE;

	/* last operation of list is reserved to insert operation */
	tsnep_write_gcl_operation(gcl, TSNEP_GCL_COUNT - 1, properties,
	interval, false);

	return tsnep_set_gcl_change(gcl, ref, change, true);
	}

	static u64 tsnep_extend_gcl(struct tsnep_gcl *gcl, u64 start, u32 extension)
	{
	int ref = gcl->count - 1;
	u32 interval = gcl->operation[ref].interval + extension;

	start -= gcl->operation[ref].interval;

	return tsnep_insert_gcl_operation(gcl, ref, start, interval);
	}

	static u64 tsnep_cut_gcl(struct tsnep_gcl *gcl, u64 start, u64 cycle_time)
	{
	u64 sum = 0;
	int i;

	/* find operation which shall be cutted */
	for (i = 0; i < gcl->count; i++) {
	u64 sum_tmp = sum + gcl->operation[i].interval;
	u64 interval;

	/* sum up operations as long as cycle time is not exceeded */
	if (sum_tmp > cycle_time)
	break;

	/* remaining interval must be big enough for hardware */
	interval = cycle_time - sum_tmp;
	if (interval > 0 && interval < TSNEP_GCL_MIN_INTERVAL)
	break;

	sum = sum_tmp;
	}
	if (sum == cycle_time) {
	/* no need to cut operation itself or whole cycle
	* => change exactly at operation
	*/
	return tsnep_set_gcl_change(gcl, i, start + sum, false);
	}
	return tsnep_insert_gcl_operation(gcl, i, start + sum,
	cycle_time - sum);
	}

	static int tsnep_enable_gcl(struct tsnep_adapter *adapter,
	struct tsnep_gcl gcl, struct tsnep_gcl curr)
	{
	u64 system_time;
	u64 timeout;
	u64 limit;

	/* estimate timeout limit after timeout enable, actually timeout limit
	* in hardware will be earlier than estimate so we are on the safe side
	*/
	tsnep_get_system_time(adapter, &system_time);
	timeout = system_time + TSNEP_GC_TIMEOUT;

	if (curr)
	limit = timeout + curr->change_limit;
	else
	limit = timeout;

	gcl->start_time = tsnep_gcl_start_after(gcl, limit);

	/* gate control time register is only 32bit => time shall be in the near
	* future (no driver support for far future implemented)
	*/
	if ((gcl->start_time - system_time) >= U32_MAX)
	return -EAGAIN;

	if (curr) {
	/* change gate control list */
	u64 last;
	u64 change;

	last = tsnep_gcl_start_before(curr, gcl->start_time);
	if ((last + curr->cycle_time) == gcl->start_time)
	change = tsnep_cut_gcl(curr, last,
	gcl->start_time - last);
	else if (((gcl->start_time - last) <=
	curr->cycle_time_extension) \|\|
	((gcl->start_time - last) <= TSNEP_GCL_MIN_INTERVAL))
	change = tsnep_extend_gcl(curr, last,
	gcl->start_time - last);
	else
	change = tsnep_cut_gcl(curr, last,
	gcl->start_time - last);

	WARN_ON(change <= timeout);
	gcl->change = true;
	iowrite32(change & 0xFFFFFFFF, adapter->addr + TSNEP_GC_CHANGE);
	} else {
	/* start gate control list */
	WARN_ON(gcl->start_time <= timeout);
	gcl->change = false;
	iowrite32(gcl->start_time & 0xFFFFFFFF,
	adapter->addr + TSNEP_GC_TIME);
	}

	return 0;
	}

	static int tsnep_taprio(struct tsnep_adapter *adapter,
	struct tc_taprio_qopt_offload *qopt)
	{
	struct tsnep_gcl *gcl;
	struct tsnep_gcl *curr;
	int retval;

	if (!adapter->gate_control)
	return -EOPNOTSUPP;

	if (qopt->cmd == TAPRIO_CMD_DESTROY) {
	/* disable gate control if active */
	mutex_lock(&adapter->gate_control_lock);

	if (adapter->gate_control_active) {
	iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
	adapter->gate_control_active = false;
	}

	mutex_unlock(&adapter->gate_control_lock);

	return 0;
	} else if (qopt->cmd != TAPRIO_CMD_REPLACE) {
	return -EOPNOTSUPP;
	}

	retval = tsnep_validate_gcl(qopt);
	if (retval)
	return retval;

	mutex_lock(&adapter->gate_control_lock);

	gcl = &adapter->gcl[adapter->next_gcl];
	tsnep_write_gcl(gcl, qopt);

	/* select current gate control list if active */
	if (adapter->gate_control_active) {
	if (adapter->next_gcl == 0)
	curr = &adapter->gcl[1];
	else
	curr = &adapter->gcl[0];
	} else {
	curr = NULL;
	}

	for (;;) {
	/* start timeout which discards late enable, this helps ensuring
	* that start/change time are in the future at enable
	*/
	iowrite8(TSNEP_GC_ENABLE_TIMEOUT, adapter->addr + TSNEP_GC);

	retval = tsnep_enable_gcl(adapter, gcl, curr);
	if (retval) {
	mutex_unlock(&adapter->gate_control_lock);

	return retval;
	}

	/* enable gate control list */
	if (adapter->next_gcl == 0)
	iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);
	else
	iowrite8(TSNEP_GC_ENABLE_B, adapter->addr + TSNEP_GC);

	/* done if timeout did not happen */
	if (!(ioread32(adapter->addr + TSNEP_GC) &
	TSNEP_GC_TIMEOUT_SIGNAL))
	break;

	/* timeout is acknowledged with any enable */
	iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);

	if (curr)
	tsnep_clean_gcl(curr);

	/* retry because of timeout */
	}

	adapter->gate_control_active = true;

	if (adapter->next_gcl == 0)
	adapter->next_gcl = 1;
	else
	adapter->next_gcl = 0;

	mutex_unlock(&adapter->gate_control_lock);

	return 0;
	}

	static int tsnep_tc_query_caps(struct tsnep_adapter *adapter,
	struct tc_query_caps_base *base)
	{
	switch (base->type) {
	case TC_SETUP_QDISC_TAPRIO: {
	struct tc_taprio_caps *caps = base->caps;

	if (!adapter->gate_control)
	return -EOPNOTSUPP;

	caps->gate_mask_per_txq = true;

	return 0;
	}
	default:
	return -EOPNOTSUPP;
	}
	}

	int tsnep_tc_setup(struct net_device *netdev, enum tc_setup_type type,
	void *type_data)
	{
	struct tsnep_adapter *adapter = netdev_priv(netdev);

	switch (type) {
	case TC_QUERY_CAPS:
	return tsnep_tc_query_caps(adapter, type_data);
	case TC_SETUP_QDISC_TAPRIO:
	return tsnep_taprio(adapter, type_data);
	default:
	return -EOPNOTSUPP;
	}
	}

	int tsnep_tc_init(struct tsnep_adapter *adapter)
	{
	if (!adapter->gate_control)
	return 0;

	/* open all gates */
	iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
	iowrite32(TSNEP_GC_OPEN \| TSNEP_GC_NEXT_OPEN, adapter->addr + TSNEP_GC);

	adapter->gcl[0].addr = adapter->addr + TSNEP_GCL_A;
	adapter->gcl[1].addr = adapter->addr + TSNEP_GCL_B;

	return 0;
	}

	void tsnep_tc_cleanup(struct tsnep_adapter *adapter)
	{
	if (!adapter->gate_control)
	return;

	if (adapter->gate_control_active) {
	iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
	adapter->gate_control_active = false;
	}
	}