1 // SPDX-License-Identifier: GPL-2.0+
2 
3 #include "lan966x_main.h"
4 
5 #define LAN966X_TAPRIO_TIMEOUT_MS		1000
6 #define LAN966X_TAPRIO_ENTRIES_PER_PORT		2
7 
8 /* Minimum supported cycle time in nanoseconds */
9 #define LAN966X_TAPRIO_MIN_CYCLE_TIME_NS	NSEC_PER_USEC
10 
11 /* Maximum supported cycle time in nanoseconds */
12 #define LAN966X_TAPRIO_MAX_CYCLE_TIME_NS	(NSEC_PER_SEC - 1)
13 
14 /* Total number of TAS GCL entries */
15 #define LAN966X_TAPRIO_NUM_GCL			256
16 
17 /* TAPRIO link speeds for calculation of guard band */
18 enum lan966x_taprio_link_speed {
19 	LAN966X_TAPRIO_SPEED_NO_GB,
20 	LAN966X_TAPRIO_SPEED_10,
21 	LAN966X_TAPRIO_SPEED_100,
22 	LAN966X_TAPRIO_SPEED_1000,
23 	LAN966X_TAPRIO_SPEED_2500,
24 };
25 
26 /* TAPRIO list states */
27 enum lan966x_taprio_state {
28 	LAN966X_TAPRIO_STATE_ADMIN,
29 	LAN966X_TAPRIO_STATE_ADVANCING,
30 	LAN966X_TAPRIO_STATE_PENDING,
31 	LAN966X_TAPRIO_STATE_OPERATING,
32 	LAN966X_TAPRIO_STATE_TERMINATING,
33 	LAN966X_TAPRIO_STATE_MAX,
34 };
35 
36 /* TAPRIO GCL command */
37 enum lan966x_taprio_gcl_cmd {
38 	LAN966X_TAPRIO_GCL_CMD_SET_GATE_STATES = 0,
39 };
40 
lan966x_taprio_list_index(struct lan966x_port * port,u8 entry)41 static u32 lan966x_taprio_list_index(struct lan966x_port *port, u8 entry)
42 {
43 	return port->chip_port * LAN966X_TAPRIO_ENTRIES_PER_PORT + entry;
44 }
45 
lan966x_taprio_list_state_get(struct lan966x_port * port)46 static u32 lan966x_taprio_list_state_get(struct lan966x_port *port)
47 {
48 	struct lan966x *lan966x = port->lan966x;
49 	u32 val;
50 
51 	val = lan_rd(lan966x, QSYS_TAS_LST);
52 	return QSYS_TAS_LST_LIST_STATE_GET(val);
53 }
54 
lan966x_taprio_list_index_state_get(struct lan966x_port * port,u32 list)55 static u32 lan966x_taprio_list_index_state_get(struct lan966x_port *port,
56 					       u32 list)
57 {
58 	struct lan966x *lan966x = port->lan966x;
59 
60 	lan_rmw(QSYS_TAS_CFG_CTRL_LIST_NUM_SET(list),
61 		QSYS_TAS_CFG_CTRL_LIST_NUM,
62 		lan966x, QSYS_TAS_CFG_CTRL);
63 
64 	return lan966x_taprio_list_state_get(port);
65 }
66 
lan966x_taprio_list_state_set(struct lan966x_port * port,u32 state)67 static void lan966x_taprio_list_state_set(struct lan966x_port *port,
68 					  u32 state)
69 {
70 	struct lan966x *lan966x = port->lan966x;
71 
72 	lan_rmw(QSYS_TAS_LST_LIST_STATE_SET(state),
73 		QSYS_TAS_LST_LIST_STATE,
74 		lan966x, QSYS_TAS_LST);
75 }
76 
lan966x_taprio_list_shutdown(struct lan966x_port * port,u32 list)77 static int lan966x_taprio_list_shutdown(struct lan966x_port *port,
78 					u32 list)
79 {
80 	struct lan966x *lan966x = port->lan966x;
81 	bool pending, operating;
82 	unsigned long end;
83 	u32 state;
84 
85 	end = jiffies +  msecs_to_jiffies(LAN966X_TAPRIO_TIMEOUT_MS);
86 	/* It is required to try multiple times to set the state of list,
87 	 * because the HW can overwrite this.
88 	 */
89 	do {
90 		state = lan966x_taprio_list_state_get(port);
91 
92 		pending = false;
93 		operating = false;
94 
95 		if (state == LAN966X_TAPRIO_STATE_ADVANCING ||
96 		    state == LAN966X_TAPRIO_STATE_PENDING) {
97 			lan966x_taprio_list_state_set(port,
98 						      LAN966X_TAPRIO_STATE_ADMIN);
99 			pending = true;
100 		}
101 
102 		if (state == LAN966X_TAPRIO_STATE_OPERATING) {
103 			lan966x_taprio_list_state_set(port,
104 						      LAN966X_TAPRIO_STATE_TERMINATING);
105 			operating = true;
106 		}
107 
108 		/* If the entry was in pending and now gets in admin, then there
109 		 * is nothing else to do, so just bail out
110 		 */
111 		state = lan966x_taprio_list_state_get(port);
112 		if (pending &&
113 		    state == LAN966X_TAPRIO_STATE_ADMIN)
114 			return 0;
115 
116 		/* If the list was in operating and now is in terminating or
117 		 * admin, then is OK to exit but it needs to wait until the list
118 		 * will get in admin. It is not required to set the state
119 		 * again.
120 		 */
121 		if (operating &&
122 		    (state == LAN966X_TAPRIO_STATE_TERMINATING ||
123 		     state == LAN966X_TAPRIO_STATE_ADMIN))
124 			break;
125 
126 	} while (!time_after(jiffies, end));
127 
128 	end = jiffies + msecs_to_jiffies(LAN966X_TAPRIO_TIMEOUT_MS);
129 	do {
130 		state = lan966x_taprio_list_state_get(port);
131 		if (state == LAN966X_TAPRIO_STATE_ADMIN)
132 			break;
133 
134 	} while (!time_after(jiffies, end));
135 
136 	/* If the list was in operating mode, it could be stopped while some
137 	 * queues where closed, so make sure to restore "all-queues-open"
138 	 */
139 	if (operating) {
140 		lan_wr(QSYS_TAS_GS_CTRL_HSCH_POS_SET(port->chip_port),
141 		       lan966x, QSYS_TAS_GS_CTRL);
142 
143 		lan_wr(QSYS_TAS_GATE_STATE_TAS_GATE_STATE_SET(0xff),
144 		       lan966x, QSYS_TAS_GATE_STATE);
145 	}
146 
147 	return 0;
148 }
149 
lan966x_taprio_shutdown(struct lan966x_port * port)150 static int lan966x_taprio_shutdown(struct lan966x_port *port)
151 {
152 	u32 i, list, state;
153 	int err;
154 
155 	for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
156 		list = lan966x_taprio_list_index(port, i);
157 		state = lan966x_taprio_list_index_state_get(port, list);
158 		if (state == LAN966X_TAPRIO_STATE_ADMIN)
159 			continue;
160 
161 		err = lan966x_taprio_list_shutdown(port, list);
162 		if (err)
163 			return err;
164 	}
165 
166 	return 0;
167 }
168 
169 /* Find a suitable list for a new schedule. First priority is a list in state
170  * pending. Second priority is a list in state admin.
171  */
lan966x_taprio_find_list(struct lan966x_port * port,struct tc_taprio_qopt_offload * qopt,int * new_list,int * obs_list)172 static int lan966x_taprio_find_list(struct lan966x_port *port,
173 				    struct tc_taprio_qopt_offload *qopt,
174 				    int *new_list, int *obs_list)
175 {
176 	int state[LAN966X_TAPRIO_ENTRIES_PER_PORT];
177 	int list[LAN966X_TAPRIO_ENTRIES_PER_PORT];
178 	int err, oper = -1;
179 	u32 i;
180 
181 	*new_list = -1;
182 	*obs_list = -1;
183 
184 	/* If there is already an entry in operating mode, return this list in
185 	 * obs_list, such that when the new list will get activated the
186 	 * operating list will be stopped. In this way is possible to have
187 	 * smooth transitions between the lists
188 	 */
189 	for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
190 		list[i] = lan966x_taprio_list_index(port, i);
191 		state[i] = lan966x_taprio_list_index_state_get(port, list[i]);
192 		if (state[i] == LAN966X_TAPRIO_STATE_OPERATING)
193 			oper = list[i];
194 	}
195 
196 	for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
197 		if (state[i] == LAN966X_TAPRIO_STATE_PENDING) {
198 			err = lan966x_taprio_shutdown(port);
199 			if (err)
200 				return err;
201 
202 			*new_list = list[i];
203 			*obs_list = (oper == -1) ? *new_list : oper;
204 			return 0;
205 		}
206 	}
207 
208 	for (i = 0; i < LAN966X_TAPRIO_ENTRIES_PER_PORT; ++i) {
209 		if (state[i] == LAN966X_TAPRIO_STATE_ADMIN) {
210 			*new_list = list[i];
211 			*obs_list = (oper == -1) ? *new_list : oper;
212 			return 0;
213 		}
214 	}
215 
216 	return -ENOSPC;
217 }
218 
lan966x_taprio_check(struct tc_taprio_qopt_offload * qopt)219 static int lan966x_taprio_check(struct tc_taprio_qopt_offload *qopt)
220 {
221 	u64 total_time = 0;
222 	u32 i;
223 
224 	/* This is not supported by th HW */
225 	if (qopt->cycle_time_extension)
226 		return -EOPNOTSUPP;
227 
228 	/* There is a limited number of gcl entries that can be used, they are
229 	 * shared by all ports
230 	 */
231 	if (qopt->num_entries > LAN966X_TAPRIO_NUM_GCL)
232 		return -EINVAL;
233 
234 	/* Don't allow cycle times bigger than 1 sec or smaller than 1 usec */
235 	if (qopt->cycle_time < LAN966X_TAPRIO_MIN_CYCLE_TIME_NS ||
236 	    qopt->cycle_time > LAN966X_TAPRIO_MAX_CYCLE_TIME_NS)
237 		return -EINVAL;
238 
239 	for (i = 0; i < qopt->num_entries; ++i) {
240 		struct tc_taprio_sched_entry *entry = &qopt->entries[i];
241 
242 		/* Don't allow intervals bigger than 1 sec or smaller than 1
243 		 * usec
244 		 */
245 		if (entry->interval < LAN966X_TAPRIO_MIN_CYCLE_TIME_NS ||
246 		    entry->interval > LAN966X_TAPRIO_MAX_CYCLE_TIME_NS)
247 			return -EINVAL;
248 
249 		if (qopt->entries[i].command != TC_TAPRIO_CMD_SET_GATES)
250 			return -EINVAL;
251 
252 		total_time += qopt->entries[i].interval;
253 	}
254 
255 	/* Don't allow the total time of intervals be bigger than 1 sec */
256 	if (total_time > LAN966X_TAPRIO_MAX_CYCLE_TIME_NS)
257 		return -EINVAL;
258 
259 	/* The HW expects that the cycle time to be at least as big as sum of
260 	 * each interval of gcl
261 	 */
262 	if (qopt->cycle_time < total_time)
263 		return -EINVAL;
264 
265 	return 0;
266 }
267 
lan966x_taprio_gcl_free_get(struct lan966x_port * port,unsigned long * free_list)268 static int lan966x_taprio_gcl_free_get(struct lan966x_port *port,
269 				       unsigned long *free_list)
270 {
271 	struct lan966x *lan966x = port->lan966x;
272 	u32 num_free, state, list;
273 	u32 base, next, max_list;
274 
275 	/* By default everything is free */
276 	bitmap_fill(free_list, LAN966X_TAPRIO_NUM_GCL);
277 	num_free = LAN966X_TAPRIO_NUM_GCL;
278 
279 	/* Iterate over all gcl entries and find out which are free. And mark
280 	 * those that are not free.
281 	 */
282 	max_list = lan966x->num_phys_ports * LAN966X_TAPRIO_ENTRIES_PER_PORT;
283 	for (list = 0; list < max_list; ++list) {
284 		state = lan966x_taprio_list_index_state_get(port, list);
285 		if (state == LAN966X_TAPRIO_STATE_ADMIN)
286 			continue;
287 
288 		base = lan_rd(lan966x, QSYS_TAS_LIST_CFG);
289 		base = QSYS_TAS_LIST_CFG_LIST_BASE_ADDR_GET(base);
290 		next = base;
291 
292 		do {
293 			clear_bit(next, free_list);
294 			num_free--;
295 
296 			lan_rmw(QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM_SET(next),
297 				QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM,
298 				lan966x, QSYS_TAS_CFG_CTRL);
299 
300 			next = lan_rd(lan966x, QSYS_TAS_GCL_CT_CFG2);
301 			next = QSYS_TAS_GCL_CT_CFG2_NEXT_GCL_GET(next);
302 		} while (base != next);
303 	}
304 
305 	return num_free;
306 }
307 
lan966x_taprio_gcl_setup_entry(struct lan966x_port * port,struct tc_taprio_sched_entry * entry,u32 next_entry)308 static void lan966x_taprio_gcl_setup_entry(struct lan966x_port *port,
309 					   struct tc_taprio_sched_entry *entry,
310 					   u32 next_entry)
311 {
312 	struct lan966x *lan966x = port->lan966x;
313 
314 	/* Setup a single gcl entry */
315 	lan_wr(QSYS_TAS_GCL_CT_CFG_GATE_STATE_SET(entry->gate_mask) |
316 	       QSYS_TAS_GCL_CT_CFG_HSCH_POS_SET(port->chip_port) |
317 	       QSYS_TAS_GCL_CT_CFG_OP_TYPE_SET(LAN966X_TAPRIO_GCL_CMD_SET_GATE_STATES),
318 	       lan966x, QSYS_TAS_GCL_CT_CFG);
319 
320 	lan_wr(QSYS_TAS_GCL_CT_CFG2_PORT_PROFILE_SET(port->chip_port) |
321 	       QSYS_TAS_GCL_CT_CFG2_NEXT_GCL_SET(next_entry),
322 	       lan966x, QSYS_TAS_GCL_CT_CFG2);
323 
324 	lan_wr(entry->interval, lan966x, QSYS_TAS_GCL_TM_CFG);
325 }
326 
lan966x_taprio_gcl_setup(struct lan966x_port * port,struct tc_taprio_qopt_offload * qopt,int list)327 static int lan966x_taprio_gcl_setup(struct lan966x_port *port,
328 				    struct tc_taprio_qopt_offload *qopt,
329 				    int list)
330 {
331 	DECLARE_BITMAP(free_list, LAN966X_TAPRIO_NUM_GCL);
332 	struct lan966x *lan966x = port->lan966x;
333 	u32 i, base, next;
334 
335 	if (lan966x_taprio_gcl_free_get(port, free_list) < qopt->num_entries)
336 		return -ENOSPC;
337 
338 	/* Select list */
339 	lan_rmw(QSYS_TAS_CFG_CTRL_LIST_NUM_SET(list),
340 		QSYS_TAS_CFG_CTRL_LIST_NUM,
341 		lan966x, QSYS_TAS_CFG_CTRL);
342 
343 	/* Setup the address of the first gcl entry */
344 	base = find_first_bit(free_list, LAN966X_TAPRIO_NUM_GCL);
345 	lan_rmw(QSYS_TAS_LIST_CFG_LIST_BASE_ADDR_SET(base),
346 		QSYS_TAS_LIST_CFG_LIST_BASE_ADDR,
347 		lan966x, QSYS_TAS_LIST_CFG);
348 
349 	/* Iterate over entries and add them to the gcl list */
350 	next = base;
351 	for (i = 0; i < qopt->num_entries; ++i) {
352 		lan_rmw(QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM_SET(next),
353 			QSYS_TAS_CFG_CTRL_GCL_ENTRY_NUM,
354 			lan966x, QSYS_TAS_CFG_CTRL);
355 
356 		/* If the entry is last, point back to the start of the list */
357 		if (i == qopt->num_entries - 1)
358 			next = base;
359 		else
360 			next = find_next_bit(free_list, LAN966X_TAPRIO_NUM_GCL,
361 					     next + 1);
362 
363 		lan966x_taprio_gcl_setup_entry(port, &qopt->entries[i], next);
364 	}
365 
366 	return 0;
367 }
368 
369 /* Calculate new base_time based on cycle_time. The HW recommends to have the
370  * new base time at least 2 * cycle type + current time
371  */
lan966x_taprio_new_base_time(struct lan966x * lan966x,const u32 cycle_time,const ktime_t org_base_time,ktime_t * new_base_time)372 static void lan966x_taprio_new_base_time(struct lan966x *lan966x,
373 					 const u32 cycle_time,
374 					 const ktime_t org_base_time,
375 					 ktime_t *new_base_time)
376 {
377 	ktime_t current_time, threshold_time;
378 	struct timespec64 ts;
379 
380 	/* Get the current time and calculate the threshold_time */
381 	lan966x_ptp_gettime64(&lan966x->phc[LAN966X_PHC_PORT].info, &ts);
382 	current_time = timespec64_to_ktime(ts);
383 	threshold_time = current_time + (2 * cycle_time);
384 
385 	/* If the org_base_time is in enough in future just use it */
386 	if (org_base_time >= threshold_time) {
387 		*new_base_time = org_base_time;
388 		return;
389 	}
390 
391 	/* If the org_base_time is smaller than current_time, calculate the new
392 	 * base time as following.
393 	 */
394 	if (org_base_time <= current_time) {
395 		u64 tmp = current_time - org_base_time;
396 		u32 rem = 0;
397 
398 		if (tmp > cycle_time)
399 			div_u64_rem(tmp, cycle_time, &rem);
400 		rem = cycle_time - rem;
401 		*new_base_time = threshold_time + rem;
402 		return;
403 	}
404 
405 	/* The only left place for org_base_time is between current_time and
406 	 * threshold_time. In this case the new_base_time is calculated like
407 	 * org_base_time + 2 * cycletime
408 	 */
409 	*new_base_time = org_base_time + 2 * cycle_time;
410 }
411 
lan966x_taprio_speed_set(struct lan966x_port * port,int speed)412 int lan966x_taprio_speed_set(struct lan966x_port *port, int speed)
413 {
414 	struct lan966x *lan966x = port->lan966x;
415 	u8 taprio_speed;
416 
417 	switch (speed) {
418 	case SPEED_10:
419 		taprio_speed = LAN966X_TAPRIO_SPEED_10;
420 		break;
421 	case SPEED_100:
422 		taprio_speed = LAN966X_TAPRIO_SPEED_100;
423 		break;
424 	case SPEED_1000:
425 		taprio_speed = LAN966X_TAPRIO_SPEED_1000;
426 		break;
427 	case SPEED_2500:
428 		taprio_speed = LAN966X_TAPRIO_SPEED_2500;
429 		break;
430 	default:
431 		return -EINVAL;
432 	}
433 
434 	lan_rmw(QSYS_TAS_PROFILE_CFG_LINK_SPEED_SET(taprio_speed),
435 		QSYS_TAS_PROFILE_CFG_LINK_SPEED,
436 		lan966x, QSYS_TAS_PROFILE_CFG(port->chip_port));
437 
438 	return 0;
439 }
440 
lan966x_taprio_add(struct lan966x_port * port,struct tc_taprio_qopt_offload * qopt)441 int lan966x_taprio_add(struct lan966x_port *port,
442 		       struct tc_taprio_qopt_offload *qopt)
443 {
444 	struct lan966x *lan966x = port->lan966x;
445 	int err, new_list, obs_list;
446 	struct timespec64 ts;
447 	ktime_t base_time;
448 
449 	err = lan966x_taprio_check(qopt);
450 	if (err)
451 		return err;
452 
453 	err = lan966x_taprio_find_list(port, qopt, &new_list, &obs_list);
454 	if (err)
455 		return err;
456 
457 	err = lan966x_taprio_gcl_setup(port, qopt, new_list);
458 	if (err)
459 		return err;
460 
461 	lan966x_taprio_new_base_time(lan966x, qopt->cycle_time,
462 				     qopt->base_time, &base_time);
463 
464 	ts = ktime_to_timespec64(base_time);
465 	lan_wr(QSYS_TAS_BT_NSEC_NSEC_SET(ts.tv_nsec),
466 	       lan966x, QSYS_TAS_BT_NSEC);
467 
468 	lan_wr(lower_32_bits(ts.tv_sec),
469 	       lan966x, QSYS_TAS_BT_SEC_LSB);
470 
471 	lan_wr(QSYS_TAS_BT_SEC_MSB_SEC_MSB_SET(upper_32_bits(ts.tv_sec)),
472 	       lan966x, QSYS_TAS_BT_SEC_MSB);
473 
474 	lan_wr(qopt->cycle_time, lan966x, QSYS_TAS_CT_CFG);
475 
476 	lan_rmw(QSYS_TAS_STARTUP_CFG_OBSOLETE_IDX_SET(obs_list),
477 		QSYS_TAS_STARTUP_CFG_OBSOLETE_IDX,
478 		lan966x, QSYS_TAS_STARTUP_CFG);
479 
480 	/* Start list processing */
481 	lan_rmw(QSYS_TAS_LST_LIST_STATE_SET(LAN966X_TAPRIO_STATE_ADVANCING),
482 		QSYS_TAS_LST_LIST_STATE,
483 		lan966x, QSYS_TAS_LST);
484 
485 	return err;
486 }
487 
lan966x_taprio_del(struct lan966x_port * port)488 int lan966x_taprio_del(struct lan966x_port *port)
489 {
490 	return lan966x_taprio_shutdown(port);
491 }
492 
lan966x_taprio_init(struct lan966x * lan966x)493 void lan966x_taprio_init(struct lan966x *lan966x)
494 {
495 	int num_taprio_lists;
496 	int p;
497 
498 	lan_wr(QSYS_TAS_STM_CFG_REVISIT_DLY_SET((256 * 1000) /
499 						lan966x_ptp_get_period_ps()),
500 	       lan966x, QSYS_TAS_STM_CFG);
501 
502 	num_taprio_lists = lan966x->num_phys_ports *
503 			   LAN966X_TAPRIO_ENTRIES_PER_PORT;
504 
505 	/* For now we always use guard band on all queues */
506 	lan_rmw(QSYS_TAS_CFG_CTRL_LIST_NUM_MAX_SET(num_taprio_lists) |
507 		QSYS_TAS_CFG_CTRL_ALWAYS_GB_SCH_Q_SET(1),
508 		QSYS_TAS_CFG_CTRL_LIST_NUM_MAX |
509 		QSYS_TAS_CFG_CTRL_ALWAYS_GB_SCH_Q,
510 		lan966x, QSYS_TAS_CFG_CTRL);
511 
512 	for (p = 0; p < lan966x->num_phys_ports; p++)
513 		lan_rmw(QSYS_TAS_PROFILE_CFG_PORT_NUM_SET(p),
514 			QSYS_TAS_PROFILE_CFG_PORT_NUM,
515 			lan966x, QSYS_TAS_PROFILE_CFG(p));
516 }
517 
lan966x_taprio_deinit(struct lan966x * lan966x)518 void lan966x_taprio_deinit(struct lan966x *lan966x)
519 {
520 	int p;
521 
522 	for (p = 0; p < lan966x->num_phys_ports; ++p) {
523 		if (!lan966x->ports[p])
524 			continue;
525 
526 		lan966x_taprio_del(lan966x->ports[p]);
527 	}
528 }
529