1 // SPDX-License-Identifier: GPL-2.0
2 /* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com>
3  */
4 #include "sja1105.h"
5 
6 #define SJA1105_TAS_CLKSRC_DISABLED	0
7 #define SJA1105_TAS_CLKSRC_STANDALONE	1
8 #define SJA1105_TAS_CLKSRC_AS6802	2
9 #define SJA1105_TAS_CLKSRC_PTP		3
10 #define SJA1105_GATE_MASK		GENMASK_ULL(SJA1105_NUM_TC - 1, 0)
11 
12 #define work_to_sja1105_tas(d) \
13 	container_of((d), struct sja1105_tas_data, tas_work)
14 #define tas_to_sja1105(d) \
15 	container_of((d), struct sja1105_private, tas_data)
16 
17 static int sja1105_tas_set_runtime_params(struct sja1105_private *priv)
18 {
19 	struct sja1105_tas_data *tas_data = &priv->tas_data;
20 	struct sja1105_gating_config *gating_cfg = &tas_data->gating_cfg;
21 	struct dsa_switch *ds = priv->ds;
22 	s64 earliest_base_time = S64_MAX;
23 	s64 latest_base_time = 0;
24 	s64 its_cycle_time = 0;
25 	s64 max_cycle_time = 0;
26 	int port;
27 
28 	tas_data->enabled = false;
29 
30 	for (port = 0; port < ds->num_ports; port++) {
31 		const struct tc_taprio_qopt_offload *offload;
32 
33 		offload = tas_data->offload[port];
34 		if (!offload)
35 			continue;
36 
37 		tas_data->enabled = true;
38 
39 		if (max_cycle_time < offload->cycle_time)
40 			max_cycle_time = offload->cycle_time;
41 		if (latest_base_time < offload->base_time)
42 			latest_base_time = offload->base_time;
43 		if (earliest_base_time > offload->base_time) {
44 			earliest_base_time = offload->base_time;
45 			its_cycle_time = offload->cycle_time;
46 		}
47 	}
48 
49 	if (!list_empty(&gating_cfg->entries)) {
50 		tas_data->enabled = true;
51 
52 		if (max_cycle_time < gating_cfg->cycle_time)
53 			max_cycle_time = gating_cfg->cycle_time;
54 		if (latest_base_time < gating_cfg->base_time)
55 			latest_base_time = gating_cfg->base_time;
56 		if (earliest_base_time > gating_cfg->base_time) {
57 			earliest_base_time = gating_cfg->base_time;
58 			its_cycle_time = gating_cfg->cycle_time;
59 		}
60 	}
61 
62 	if (!tas_data->enabled)
63 		return 0;
64 
65 	/* Roll the earliest base time over until it is in a comparable
66 	 * time base with the latest, then compare their deltas.
67 	 * We want to enforce that all ports' base times are within
68 	 * SJA1105_TAS_MAX_DELTA 200ns cycles of one another.
69 	 */
70 	earliest_base_time = future_base_time(earliest_base_time,
71 					      its_cycle_time,
72 					      latest_base_time);
73 	while (earliest_base_time > latest_base_time)
74 		earliest_base_time -= its_cycle_time;
75 	if (latest_base_time - earliest_base_time >
76 	    sja1105_delta_to_ns(SJA1105_TAS_MAX_DELTA)) {
77 		dev_err(ds->dev,
78 			"Base times too far apart: min %llu max %llu\n",
79 			earliest_base_time, latest_base_time);
80 		return -ERANGE;
81 	}
82 
83 	tas_data->earliest_base_time = earliest_base_time;
84 	tas_data->max_cycle_time = max_cycle_time;
85 
86 	dev_dbg(ds->dev, "earliest base time %lld ns\n", earliest_base_time);
87 	dev_dbg(ds->dev, "latest base time %lld ns\n", latest_base_time);
88 	dev_dbg(ds->dev, "longest cycle time %lld ns\n", max_cycle_time);
89 
90 	return 0;
91 }
92 
93 /* Lo and behold: the egress scheduler from hell.
94  *
95  * At the hardware level, the Time-Aware Shaper holds a global linear arrray of
96  * all schedule entries for all ports. These are the Gate Control List (GCL)
97  * entries, let's call them "timeslots" for short. This linear array of
98  * timeslots is held in BLK_IDX_SCHEDULE.
99  *
100  * Then there are a maximum of 8 "execution threads" inside the switch, which
101  * iterate cyclically through the "schedule". Each "cycle" has an entry point
102  * and an exit point, both being timeslot indices in the schedule table. The
103  * hardware calls each cycle a "subschedule".
104  *
105  * Subschedule (cycle) i starts when
106  *   ptpclkval >= ptpschtm + BLK_IDX_SCHEDULE_ENTRY_POINTS[i].delta.
107  *
108  * The hardware scheduler iterates BLK_IDX_SCHEDULE with a k ranging from
109  *   k = BLK_IDX_SCHEDULE_ENTRY_POINTS[i].address to
110  *   k = BLK_IDX_SCHEDULE_PARAMS.subscheind[i]
111  *
112  * For each schedule entry (timeslot) k, the engine executes the gate control
113  * list entry for the duration of BLK_IDX_SCHEDULE[k].delta.
114  *
115  *         +---------+
116  *         |         | BLK_IDX_SCHEDULE_ENTRY_POINTS_PARAMS
117  *         +---------+
118  *              |
119  *              +-----------------+
120  *                                | .actsubsch
121  *  BLK_IDX_SCHEDULE_ENTRY_POINTS v
122  *                 +-------+-------+
123  *                 |cycle 0|cycle 1|
124  *                 +-------+-------+
125  *                   |  |      |  |
126  *  +----------------+  |      |  +-------------------------------------+
127  *  |   .subschindx     |      |             .subschindx                |
128  *  |                   |      +---------------+                        |
129  *  |          .address |        .address      |                        |
130  *  |                   |                      |                        |
131  *  |                   |                      |                        |
132  *  |  BLK_IDX_SCHEDULE v                      v                        |
133  *  |              +-------+-------+-------+-------+-------+------+     |
134  *  |              |entry 0|entry 1|entry 2|entry 3|entry 4|entry5|     |
135  *  |              +-------+-------+-------+-------+-------+------+     |
136  *  |                                  ^                    ^  ^  ^     |
137  *  |                                  |                    |  |  |     |
138  *  |        +-------------------------+                    |  |  |     |
139  *  |        |              +-------------------------------+  |  |     |
140  *  |        |              |              +-------------------+  |     |
141  *  |        |              |              |                      |     |
142  *  | +---------------------------------------------------------------+ |
143  *  | |subscheind[0]<=subscheind[1]<=subscheind[2]<=...<=subscheind[7]| |
144  *  | +---------------------------------------------------------------+ |
145  *  |        ^              ^                BLK_IDX_SCHEDULE_PARAMS    |
146  *  |        |              |                                           |
147  *  +--------+              +-------------------------------------------+
148  *
149  *  In the above picture there are two subschedules (cycles):
150  *
151  *  - cycle 0: iterates the schedule table from 0 to 2 (and back)
152  *  - cycle 1: iterates the schedule table from 3 to 5 (and back)
153  *
154  *  All other possible execution threads must be marked as unused by making
155  *  their "subschedule end index" (subscheind) equal to the last valid
156  *  subschedule's end index (in this case 5).
157  */
158 int sja1105_init_scheduling(struct sja1105_private *priv)
159 {
160 	struct sja1105_schedule_entry_points_entry *schedule_entry_points;
161 	struct sja1105_schedule_entry_points_params_entry
162 					*schedule_entry_points_params;
163 	struct sja1105_schedule_params_entry *schedule_params;
164 	struct sja1105_tas_data *tas_data = &priv->tas_data;
165 	struct sja1105_gating_config *gating_cfg = &tas_data->gating_cfg;
166 	struct sja1105_schedule_entry *schedule;
167 	struct dsa_switch *ds = priv->ds;
168 	struct sja1105_table *table;
169 	int schedule_start_idx;
170 	s64 entry_point_delta;
171 	int schedule_end_idx;
172 	int num_entries = 0;
173 	int num_cycles = 0;
174 	int cycle = 0;
175 	int i, k = 0;
176 	int port, rc;
177 
178 	rc = sja1105_tas_set_runtime_params(priv);
179 	if (rc < 0)
180 		return rc;
181 
182 	/* Discard previous Schedule Table */
183 	table = &priv->static_config.tables[BLK_IDX_SCHEDULE];
184 	if (table->entry_count) {
185 		kfree(table->entries);
186 		table->entry_count = 0;
187 	}
188 
189 	/* Discard previous Schedule Entry Points Parameters Table */
190 	table = &priv->static_config.tables[BLK_IDX_SCHEDULE_ENTRY_POINTS_PARAMS];
191 	if (table->entry_count) {
192 		kfree(table->entries);
193 		table->entry_count = 0;
194 	}
195 
196 	/* Discard previous Schedule Parameters Table */
197 	table = &priv->static_config.tables[BLK_IDX_SCHEDULE_PARAMS];
198 	if (table->entry_count) {
199 		kfree(table->entries);
200 		table->entry_count = 0;
201 	}
202 
203 	/* Discard previous Schedule Entry Points Table */
204 	table = &priv->static_config.tables[BLK_IDX_SCHEDULE_ENTRY_POINTS];
205 	if (table->entry_count) {
206 		kfree(table->entries);
207 		table->entry_count = 0;
208 	}
209 
210 	/* Figure out the dimensioning of the problem */
211 	for (port = 0; port < ds->num_ports; port++) {
212 		if (tas_data->offload[port]) {
213 			num_entries += tas_data->offload[port]->num_entries;
214 			num_cycles++;
215 		}
216 	}
217 
218 	if (!list_empty(&gating_cfg->entries)) {
219 		num_entries += gating_cfg->num_entries;
220 		num_cycles++;
221 	}
222 
223 	/* Nothing to do */
224 	if (!num_cycles)
225 		return 0;
226 
227 	/* Pre-allocate space in the static config tables */
228 
229 	/* Schedule Table */
230 	table = &priv->static_config.tables[BLK_IDX_SCHEDULE];
231 	table->entries = kcalloc(num_entries, table->ops->unpacked_entry_size,
232 				 GFP_KERNEL);
233 	if (!table->entries)
234 		return -ENOMEM;
235 	table->entry_count = num_entries;
236 	schedule = table->entries;
237 
238 	/* Schedule Points Parameters Table */
239 	table = &priv->static_config.tables[BLK_IDX_SCHEDULE_ENTRY_POINTS_PARAMS];
240 	table->entries = kcalloc(SJA1105_MAX_SCHEDULE_ENTRY_POINTS_PARAMS_COUNT,
241 				 table->ops->unpacked_entry_size, GFP_KERNEL);
242 	if (!table->entries)
243 		/* Previously allocated memory will be freed automatically in
244 		 * sja1105_static_config_free. This is true for all early
245 		 * returns below.
246 		 */
247 		return -ENOMEM;
248 	table->entry_count = SJA1105_MAX_SCHEDULE_ENTRY_POINTS_PARAMS_COUNT;
249 	schedule_entry_points_params = table->entries;
250 
251 	/* Schedule Parameters Table */
252 	table = &priv->static_config.tables[BLK_IDX_SCHEDULE_PARAMS];
253 	table->entries = kcalloc(SJA1105_MAX_SCHEDULE_PARAMS_COUNT,
254 				 table->ops->unpacked_entry_size, GFP_KERNEL);
255 	if (!table->entries)
256 		return -ENOMEM;
257 	table->entry_count = SJA1105_MAX_SCHEDULE_PARAMS_COUNT;
258 	schedule_params = table->entries;
259 
260 	/* Schedule Entry Points Table */
261 	table = &priv->static_config.tables[BLK_IDX_SCHEDULE_ENTRY_POINTS];
262 	table->entries = kcalloc(num_cycles, table->ops->unpacked_entry_size,
263 				 GFP_KERNEL);
264 	if (!table->entries)
265 		return -ENOMEM;
266 	table->entry_count = num_cycles;
267 	schedule_entry_points = table->entries;
268 
269 	/* Finally start populating the static config tables */
270 	schedule_entry_points_params->clksrc = SJA1105_TAS_CLKSRC_PTP;
271 	schedule_entry_points_params->actsubsch = num_cycles - 1;
272 
273 	for (port = 0; port < ds->num_ports; port++) {
274 		const struct tc_taprio_qopt_offload *offload;
275 		/* Relative base time */
276 		s64 rbt;
277 
278 		offload = tas_data->offload[port];
279 		if (!offload)
280 			continue;
281 
282 		schedule_start_idx = k;
283 		schedule_end_idx = k + offload->num_entries - 1;
284 		/* This is the base time expressed as a number of TAS ticks
285 		 * relative to PTPSCHTM, which we'll (perhaps improperly) call
286 		 * the operational base time.
287 		 */
288 		rbt = future_base_time(offload->base_time,
289 				       offload->cycle_time,
290 				       tas_data->earliest_base_time);
291 		rbt -= tas_data->earliest_base_time;
292 		/* UM10944.pdf 4.2.2. Schedule Entry Points table says that
293 		 * delta cannot be zero, which is shitty. Advance all relative
294 		 * base times by 1 TAS delta, so that even the earliest base
295 		 * time becomes 1 in relative terms. Then start the operational
296 		 * base time (PTPSCHTM) one TAS delta earlier than planned.
297 		 */
298 		entry_point_delta = ns_to_sja1105_delta(rbt) + 1;
299 
300 		schedule_entry_points[cycle].subschindx = cycle;
301 		schedule_entry_points[cycle].delta = entry_point_delta;
302 		schedule_entry_points[cycle].address = schedule_start_idx;
303 
304 		/* The subschedule end indices need to be
305 		 * monotonically increasing.
306 		 */
307 		for (i = cycle; i < 8; i++)
308 			schedule_params->subscheind[i] = schedule_end_idx;
309 
310 		for (i = 0; i < offload->num_entries; i++, k++) {
311 			s64 delta_ns = offload->entries[i].interval;
312 
313 			schedule[k].delta = ns_to_sja1105_delta(delta_ns);
314 			schedule[k].destports = BIT(port);
315 			schedule[k].resmedia_en = true;
316 			schedule[k].resmedia = SJA1105_GATE_MASK &
317 					~offload->entries[i].gate_mask;
318 		}
319 		cycle++;
320 	}
321 
322 	if (!list_empty(&gating_cfg->entries)) {
323 		struct sja1105_gate_entry *e;
324 
325 		/* Relative base time */
326 		s64 rbt;
327 
328 		schedule_start_idx = k;
329 		schedule_end_idx = k + gating_cfg->num_entries - 1;
330 		rbt = future_base_time(gating_cfg->base_time,
331 				       gating_cfg->cycle_time,
332 				       tas_data->earliest_base_time);
333 		rbt -= tas_data->earliest_base_time;
334 		entry_point_delta = ns_to_sja1105_delta(rbt) + 1;
335 
336 		schedule_entry_points[cycle].subschindx = cycle;
337 		schedule_entry_points[cycle].delta = entry_point_delta;
338 		schedule_entry_points[cycle].address = schedule_start_idx;
339 
340 		for (i = cycle; i < 8; i++)
341 			schedule_params->subscheind[i] = schedule_end_idx;
342 
343 		list_for_each_entry(e, &gating_cfg->entries, list) {
344 			schedule[k].delta = ns_to_sja1105_delta(e->interval);
345 			schedule[k].destports = e->rule->vl.destports;
346 			schedule[k].setvalid = true;
347 			schedule[k].txen = true;
348 			schedule[k].vlindex = e->rule->vl.sharindx;
349 			schedule[k].winstindex = e->rule->vl.sharindx;
350 			if (e->gate_state) /* Gate open */
351 				schedule[k].winst = true;
352 			else /* Gate closed */
353 				schedule[k].winend = true;
354 			k++;
355 		}
356 	}
357 
358 	return 0;
359 }
360 
361 /* Be there 2 port subschedules, each executing an arbitrary number of gate
362  * open/close events cyclically.
363  * None of those gate events must ever occur at the exact same time, otherwise
364  * the switch is known to act in exotically strange ways.
365  * However the hardware doesn't bother performing these integrity checks.
366  * So here we are with the task of validating whether the new @admin offload
367  * has any conflict with the already established TAS configuration in
368  * tas_data->offload.  We already know the other ports are in harmony with one
369  * another, otherwise we wouldn't have saved them.
370  * Each gate event executes periodically, with a period of @cycle_time and a
371  * phase given by its cycle's @base_time plus its offset within the cycle
372  * (which in turn is given by the length of the events prior to it).
373  * There are two aspects to possible collisions:
374  * - Collisions within one cycle's (actually the longest cycle's) time frame.
375  *   For that, we need to compare the cartesian product of each possible
376  *   occurrence of each event within one cycle time.
377  * - Collisions in the future. Events may not collide within one cycle time,
378  *   but if two port schedules don't have the same periodicity (aka the cycle
379  *   times aren't multiples of one another), they surely will some time in the
380  *   future (actually they will collide an infinite amount of times).
381  */
382 static bool
383 sja1105_tas_check_conflicts(struct sja1105_private *priv, int port,
384 			    const struct tc_taprio_qopt_offload *admin)
385 {
386 	struct sja1105_tas_data *tas_data = &priv->tas_data;
387 	const struct tc_taprio_qopt_offload *offload;
388 	s64 max_cycle_time, min_cycle_time;
389 	s64 delta1, delta2;
390 	s64 rbt1, rbt2;
391 	s64 stop_time;
392 	s64 t1, t2;
393 	int i, j;
394 	s32 rem;
395 
396 	offload = tas_data->offload[port];
397 	if (!offload)
398 		return false;
399 
400 	/* Check if the two cycle times are multiples of one another.
401 	 * If they aren't, then they will surely collide.
402 	 */
403 	max_cycle_time = max(offload->cycle_time, admin->cycle_time);
404 	min_cycle_time = min(offload->cycle_time, admin->cycle_time);
405 	div_s64_rem(max_cycle_time, min_cycle_time, &rem);
406 	if (rem)
407 		return true;
408 
409 	/* Calculate the "reduced" base time of each of the two cycles
410 	 * (transposed back as close to 0 as possible) by dividing to
411 	 * the cycle time.
412 	 */
413 	div_s64_rem(offload->base_time, offload->cycle_time, &rem);
414 	rbt1 = rem;
415 
416 	div_s64_rem(admin->base_time, admin->cycle_time, &rem);
417 	rbt2 = rem;
418 
419 	stop_time = max_cycle_time + max(rbt1, rbt2);
420 
421 	/* delta1 is the relative base time of each GCL entry within
422 	 * the established ports' TAS config.
423 	 */
424 	for (i = 0, delta1 = 0;
425 	     i < offload->num_entries;
426 	     delta1 += offload->entries[i].interval, i++) {
427 		/* delta2 is the relative base time of each GCL entry
428 		 * within the newly added TAS config.
429 		 */
430 		for (j = 0, delta2 = 0;
431 		     j < admin->num_entries;
432 		     delta2 += admin->entries[j].interval, j++) {
433 			/* t1 follows all possible occurrences of the
434 			 * established ports' GCL entry i within the
435 			 * first cycle time.
436 			 */
437 			for (t1 = rbt1 + delta1;
438 			     t1 <= stop_time;
439 			     t1 += offload->cycle_time) {
440 				/* t2 follows all possible occurrences
441 				 * of the newly added GCL entry j
442 				 * within the first cycle time.
443 				 */
444 				for (t2 = rbt2 + delta2;
445 				     t2 <= stop_time;
446 				     t2 += admin->cycle_time) {
447 					if (t1 == t2) {
448 						dev_warn(priv->ds->dev,
449 							 "GCL entry %d collides with entry %d of port %d\n",
450 							 j, i, port);
451 						return true;
452 					}
453 				}
454 			}
455 		}
456 	}
457 
458 	return false;
459 }
460 
461 /* Check the tc-taprio configuration on @port for conflicts with the tc-gate
462  * global subschedule. If @port is -1, check it against all ports.
463  * To reuse the sja1105_tas_check_conflicts logic without refactoring it,
464  * convert the gating configuration to a dummy tc-taprio offload structure.
465  */
466 bool sja1105_gating_check_conflicts(struct sja1105_private *priv, int port,
467 				    struct netlink_ext_ack *extack)
468 {
469 	struct sja1105_gating_config *gating_cfg = &priv->tas_data.gating_cfg;
470 	size_t num_entries = gating_cfg->num_entries;
471 	struct tc_taprio_qopt_offload *dummy;
472 	struct dsa_switch *ds = priv->ds;
473 	struct sja1105_gate_entry *e;
474 	bool conflict;
475 	int i = 0;
476 
477 	if (list_empty(&gating_cfg->entries))
478 		return false;
479 
480 	dummy = kzalloc(struct_size(dummy, entries, num_entries), GFP_KERNEL);
481 	if (!dummy) {
482 		NL_SET_ERR_MSG_MOD(extack, "Failed to allocate memory");
483 		return true;
484 	}
485 
486 	dummy->num_entries = num_entries;
487 	dummy->base_time = gating_cfg->base_time;
488 	dummy->cycle_time = gating_cfg->cycle_time;
489 
490 	list_for_each_entry(e, &gating_cfg->entries, list)
491 		dummy->entries[i++].interval = e->interval;
492 
493 	if (port != -1) {
494 		conflict = sja1105_tas_check_conflicts(priv, port, dummy);
495 	} else {
496 		for (port = 0; port < ds->num_ports; port++) {
497 			conflict = sja1105_tas_check_conflicts(priv, port,
498 							       dummy);
499 			if (conflict)
500 				break;
501 		}
502 	}
503 
504 	kfree(dummy);
505 
506 	return conflict;
507 }
508 
509 int sja1105_setup_tc_taprio(struct dsa_switch *ds, int port,
510 			    struct tc_taprio_qopt_offload *admin)
511 {
512 	struct sja1105_private *priv = ds->priv;
513 	struct sja1105_tas_data *tas_data = &priv->tas_data;
514 	int other_port, rc, i;
515 
516 	/* Can't change an already configured port (must delete qdisc first).
517 	 * Can't delete the qdisc from an unconfigured port.
518 	 */
519 	if ((!!tas_data->offload[port] && admin->cmd == TAPRIO_CMD_REPLACE) ||
520 	    (!tas_data->offload[port] && admin->cmd == TAPRIO_CMD_DESTROY))
521 		return -EINVAL;
522 
523 	if (admin->cmd == TAPRIO_CMD_DESTROY) {
524 		taprio_offload_free(tas_data->offload[port]);
525 		tas_data->offload[port] = NULL;
526 
527 		rc = sja1105_init_scheduling(priv);
528 		if (rc < 0)
529 			return rc;
530 
531 		return sja1105_static_config_reload(priv, SJA1105_SCHEDULING);
532 	} else if (admin->cmd != TAPRIO_CMD_REPLACE) {
533 		return -EOPNOTSUPP;
534 	}
535 
536 	/* The cycle time extension is the amount of time the last cycle from
537 	 * the old OPER needs to be extended in order to phase-align with the
538 	 * base time of the ADMIN when that becomes the new OPER.
539 	 * But of course our switch needs to be reset to switch-over between
540 	 * the ADMIN and the OPER configs - so much for a seamless transition.
541 	 * So don't add insult over injury and just say we don't support cycle
542 	 * time extension.
543 	 */
544 	if (admin->cycle_time_extension)
545 		return -ENOTSUPP;
546 
547 	for (i = 0; i < admin->num_entries; i++) {
548 		s64 delta_ns = admin->entries[i].interval;
549 		s64 delta_cycles = ns_to_sja1105_delta(delta_ns);
550 		bool too_long, too_short;
551 
552 		too_long = (delta_cycles >= SJA1105_TAS_MAX_DELTA);
553 		too_short = (delta_cycles == 0);
554 		if (too_long || too_short) {
555 			dev_err(priv->ds->dev,
556 				"Interval %llu too %s for GCL entry %d\n",
557 				delta_ns, too_long ? "long" : "short", i);
558 			return -ERANGE;
559 		}
560 	}
561 
562 	for (other_port = 0; other_port < ds->num_ports; other_port++) {
563 		if (other_port == port)
564 			continue;
565 
566 		if (sja1105_tas_check_conflicts(priv, other_port, admin))
567 			return -ERANGE;
568 	}
569 
570 	if (sja1105_gating_check_conflicts(priv, port, NULL)) {
571 		dev_err(ds->dev, "Conflict with tc-gate schedule\n");
572 		return -ERANGE;
573 	}
574 
575 	tas_data->offload[port] = taprio_offload_get(admin);
576 
577 	rc = sja1105_init_scheduling(priv);
578 	if (rc < 0)
579 		return rc;
580 
581 	return sja1105_static_config_reload(priv, SJA1105_SCHEDULING);
582 }
583 
584 static int sja1105_tas_check_running(struct sja1105_private *priv)
585 {
586 	struct sja1105_tas_data *tas_data = &priv->tas_data;
587 	struct dsa_switch *ds = priv->ds;
588 	struct sja1105_ptp_cmd cmd = {0};
589 	int rc;
590 
591 	rc = sja1105_ptp_commit(ds, &cmd, SPI_READ);
592 	if (rc < 0)
593 		return rc;
594 
595 	if (cmd.ptpstrtsch == 1)
596 		/* Schedule successfully started */
597 		tas_data->state = SJA1105_TAS_STATE_RUNNING;
598 	else if (cmd.ptpstopsch == 1)
599 		/* Schedule is stopped */
600 		tas_data->state = SJA1105_TAS_STATE_DISABLED;
601 	else
602 		/* Schedule is probably not configured with PTP clock source */
603 		rc = -EINVAL;
604 
605 	return rc;
606 }
607 
608 /* Write to PTPCLKCORP */
609 static int sja1105_tas_adjust_drift(struct sja1105_private *priv,
610 				    u64 correction)
611 {
612 	const struct sja1105_regs *regs = priv->info->regs;
613 	u32 ptpclkcorp = ns_to_sja1105_ticks(correction);
614 
615 	return sja1105_xfer_u32(priv, SPI_WRITE, regs->ptpclkcorp,
616 				&ptpclkcorp, NULL);
617 }
618 
619 /* Write to PTPSCHTM */
620 static int sja1105_tas_set_base_time(struct sja1105_private *priv,
621 				     u64 base_time)
622 {
623 	const struct sja1105_regs *regs = priv->info->regs;
624 	u64 ptpschtm = ns_to_sja1105_ticks(base_time);
625 
626 	return sja1105_xfer_u64(priv, SPI_WRITE, regs->ptpschtm,
627 				&ptpschtm, NULL);
628 }
629 
630 static int sja1105_tas_start(struct sja1105_private *priv)
631 {
632 	struct sja1105_tas_data *tas_data = &priv->tas_data;
633 	struct sja1105_ptp_cmd *cmd = &priv->ptp_data.cmd;
634 	struct dsa_switch *ds = priv->ds;
635 	int rc;
636 
637 	dev_dbg(ds->dev, "Starting the TAS\n");
638 
639 	if (tas_data->state == SJA1105_TAS_STATE_ENABLED_NOT_RUNNING ||
640 	    tas_data->state == SJA1105_TAS_STATE_RUNNING) {
641 		dev_err(ds->dev, "TAS already started\n");
642 		return -EINVAL;
643 	}
644 
645 	cmd->ptpstrtsch = 1;
646 	cmd->ptpstopsch = 0;
647 
648 	rc = sja1105_ptp_commit(ds, cmd, SPI_WRITE);
649 	if (rc < 0)
650 		return rc;
651 
652 	tas_data->state = SJA1105_TAS_STATE_ENABLED_NOT_RUNNING;
653 
654 	return 0;
655 }
656 
657 static int sja1105_tas_stop(struct sja1105_private *priv)
658 {
659 	struct sja1105_tas_data *tas_data = &priv->tas_data;
660 	struct sja1105_ptp_cmd *cmd = &priv->ptp_data.cmd;
661 	struct dsa_switch *ds = priv->ds;
662 	int rc;
663 
664 	dev_dbg(ds->dev, "Stopping the TAS\n");
665 
666 	if (tas_data->state == SJA1105_TAS_STATE_DISABLED) {
667 		dev_err(ds->dev, "TAS already disabled\n");
668 		return -EINVAL;
669 	}
670 
671 	cmd->ptpstopsch = 1;
672 	cmd->ptpstrtsch = 0;
673 
674 	rc = sja1105_ptp_commit(ds, cmd, SPI_WRITE);
675 	if (rc < 0)
676 		return rc;
677 
678 	tas_data->state = SJA1105_TAS_STATE_DISABLED;
679 
680 	return 0;
681 }
682 
683 /* The schedule engine and the PTP clock are driven by the same oscillator, and
684  * they run in parallel. But whilst the PTP clock can keep an absolute
685  * time-of-day, the schedule engine is only running in 'ticks' (25 ticks make
686  * up a delta, which is 200ns), and wrapping around at the end of each cycle.
687  * The schedule engine is started when the PTP clock reaches the PTPSCHTM time
688  * (in PTP domain).
689  * Because the PTP clock can be rate-corrected (accelerated or slowed down) by
690  * a software servo, and the schedule engine clock runs in parallel to the PTP
691  * clock, there is logic internal to the switch that periodically keeps the
692  * schedule engine from drifting away. The frequency with which this internal
693  * syntonization happens is the PTP clock correction period (PTPCLKCORP). It is
694  * a value also in the PTP clock domain, and is also rate-corrected.
695  * To be precise, during a correction period, there is logic to determine by
696  * how many scheduler clock ticks has the PTP clock drifted. At the end of each
697  * correction period/beginning of new one, the length of a delta is shrunk or
698  * expanded with an integer number of ticks, compared with the typical 25.
699  * So a delta lasts for 200ns (or 25 ticks) only on average.
700  * Sometimes it is longer, sometimes it is shorter. The internal syntonization
701  * logic can adjust for at most 5 ticks each 20 ticks.
702  *
703  * The first implication is that you should choose your schedule correction
704  * period to be an integer multiple of the schedule length. Preferably one.
705  * In case there are schedules of multiple ports active, then the correction
706  * period needs to be a multiple of them all. Given the restriction that the
707  * cycle times have to be multiples of one another anyway, this means the
708  * correction period can simply be the largest cycle time, hence the current
709  * choice. This way, the updates are always synchronous to the transmission
710  * cycle, and therefore predictable.
711  *
712  * The second implication is that at the beginning of a correction period, the
713  * first few deltas will be modulated in time, until the schedule engine is
714  * properly phase-aligned with the PTP clock. For this reason, you should place
715  * your best-effort traffic at the beginning of a cycle, and your
716  * time-triggered traffic afterwards.
717  *
718  * The third implication is that once the schedule engine is started, it can
719  * only adjust for so much drift within a correction period. In the servo you
720  * can only change the PTPCLKRATE, but not step the clock (PTPCLKADD). If you
721  * want to do the latter, you need to stop and restart the schedule engine,
722  * which is what the state machine handles.
723  */
724 static void sja1105_tas_state_machine(struct work_struct *work)
725 {
726 	struct sja1105_tas_data *tas_data = work_to_sja1105_tas(work);
727 	struct sja1105_private *priv = tas_to_sja1105(tas_data);
728 	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
729 	struct timespec64 base_time_ts, now_ts;
730 	struct dsa_switch *ds = priv->ds;
731 	struct timespec64 diff;
732 	s64 base_time, now;
733 	int rc = 0;
734 
735 	mutex_lock(&ptp_data->lock);
736 
737 	switch (tas_data->state) {
738 	case SJA1105_TAS_STATE_DISABLED:
739 		/* Can't do anything at all if clock is still being stepped */
740 		if (tas_data->last_op != SJA1105_PTP_ADJUSTFREQ)
741 			break;
742 
743 		rc = sja1105_tas_adjust_drift(priv, tas_data->max_cycle_time);
744 		if (rc < 0)
745 			break;
746 
747 		rc = __sja1105_ptp_gettimex(ds, &now, NULL);
748 		if (rc < 0)
749 			break;
750 
751 		/* Plan to start the earliest schedule first. The others
752 		 * will be started in hardware, by way of their respective
753 		 * entry points delta.
754 		 * Try our best to avoid fringe cases (race condition between
755 		 * ptpschtm and ptpstrtsch) by pushing the oper_base_time at
756 		 * least one second in the future from now. This is not ideal,
757 		 * but this only needs to buy us time until the
758 		 * sja1105_tas_start command below gets executed.
759 		 */
760 		base_time = future_base_time(tas_data->earliest_base_time,
761 					     tas_data->max_cycle_time,
762 					     now + 1ull * NSEC_PER_SEC);
763 		base_time -= sja1105_delta_to_ns(1);
764 
765 		rc = sja1105_tas_set_base_time(priv, base_time);
766 		if (rc < 0)
767 			break;
768 
769 		tas_data->oper_base_time = base_time;
770 
771 		rc = sja1105_tas_start(priv);
772 		if (rc < 0)
773 			break;
774 
775 		base_time_ts = ns_to_timespec64(base_time);
776 		now_ts = ns_to_timespec64(now);
777 
778 		dev_dbg(ds->dev, "OPER base time %lld.%09ld (now %lld.%09ld)\n",
779 			base_time_ts.tv_sec, base_time_ts.tv_nsec,
780 			now_ts.tv_sec, now_ts.tv_nsec);
781 
782 		break;
783 
784 	case SJA1105_TAS_STATE_ENABLED_NOT_RUNNING:
785 		if (tas_data->last_op != SJA1105_PTP_ADJUSTFREQ) {
786 			/* Clock was stepped.. bad news for TAS */
787 			sja1105_tas_stop(priv);
788 			break;
789 		}
790 
791 		/* Check if TAS has actually started, by comparing the
792 		 * scheduled start time with the SJA1105 PTP clock
793 		 */
794 		rc = __sja1105_ptp_gettimex(ds, &now, NULL);
795 		if (rc < 0)
796 			break;
797 
798 		if (now < tas_data->oper_base_time) {
799 			/* TAS has not started yet */
800 			diff = ns_to_timespec64(tas_data->oper_base_time - now);
801 			dev_dbg(ds->dev, "time to start: [%lld.%09ld]",
802 				diff.tv_sec, diff.tv_nsec);
803 			break;
804 		}
805 
806 		/* Time elapsed, what happened? */
807 		rc = sja1105_tas_check_running(priv);
808 		if (rc < 0)
809 			break;
810 
811 		if (tas_data->state != SJA1105_TAS_STATE_RUNNING)
812 			/* TAS has started */
813 			dev_err(ds->dev,
814 				"TAS not started despite time elapsed\n");
815 
816 		break;
817 
818 	case SJA1105_TAS_STATE_RUNNING:
819 		/* Clock was stepped.. bad news for TAS */
820 		if (tas_data->last_op != SJA1105_PTP_ADJUSTFREQ) {
821 			sja1105_tas_stop(priv);
822 			break;
823 		}
824 
825 		rc = sja1105_tas_check_running(priv);
826 		if (rc < 0)
827 			break;
828 
829 		if (tas_data->state != SJA1105_TAS_STATE_RUNNING)
830 			dev_err(ds->dev, "TAS surprisingly stopped\n");
831 
832 		break;
833 
834 	default:
835 		if (net_ratelimit())
836 			dev_err(ds->dev, "TAS in an invalid state (incorrect use of API)!\n");
837 	}
838 
839 	if (rc && net_ratelimit())
840 		dev_err(ds->dev, "An operation returned %d\n", rc);
841 
842 	mutex_unlock(&ptp_data->lock);
843 }
844 
845 void sja1105_tas_clockstep(struct dsa_switch *ds)
846 {
847 	struct sja1105_private *priv = ds->priv;
848 	struct sja1105_tas_data *tas_data = &priv->tas_data;
849 
850 	if (!tas_data->enabled)
851 		return;
852 
853 	tas_data->last_op = SJA1105_PTP_CLOCKSTEP;
854 	schedule_work(&tas_data->tas_work);
855 }
856 
857 void sja1105_tas_adjfreq(struct dsa_switch *ds)
858 {
859 	struct sja1105_private *priv = ds->priv;
860 	struct sja1105_tas_data *tas_data = &priv->tas_data;
861 
862 	if (!tas_data->enabled)
863 		return;
864 
865 	/* No reason to schedule the workqueue, nothing changed */
866 	if (tas_data->state == SJA1105_TAS_STATE_RUNNING)
867 		return;
868 
869 	tas_data->last_op = SJA1105_PTP_ADJUSTFREQ;
870 	schedule_work(&tas_data->tas_work);
871 }
872 
873 void sja1105_tas_setup(struct dsa_switch *ds)
874 {
875 	struct sja1105_private *priv = ds->priv;
876 	struct sja1105_tas_data *tas_data = &priv->tas_data;
877 
878 	INIT_WORK(&tas_data->tas_work, sja1105_tas_state_machine);
879 	tas_data->state = SJA1105_TAS_STATE_DISABLED;
880 	tas_data->last_op = SJA1105_PTP_NONE;
881 
882 	INIT_LIST_HEAD(&tas_data->gating_cfg.entries);
883 }
884 
885 void sja1105_tas_teardown(struct dsa_switch *ds)
886 {
887 	struct sja1105_private *priv = ds->priv;
888 	struct tc_taprio_qopt_offload *offload;
889 	int port;
890 
891 	cancel_work_sync(&priv->tas_data.tas_work);
892 
893 	for (port = 0; port < ds->num_ports; port++) {
894 		offload = priv->tas_data.offload[port];
895 		if (!offload)
896 			continue;
897 
898 		taprio_offload_free(offload);
899 	}
900 }
901