467 lines · c
1// SPDX-License-Identifier: GPL-2.02/* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */3 4#include "tsnep.h"5 6#include <net/pkt_sched.h>7 8/* save one operation at the end for additional operation at list change */9#define TSNEP_MAX_GCL_NUM (TSNEP_GCL_COUNT - 1)10 11static int tsnep_validate_gcl(struct tc_taprio_qopt_offload *qopt)12{13 int i;14 u64 cycle_time;15 16 if (!qopt->cycle_time)17 return -ERANGE;18 if (qopt->num_entries > TSNEP_MAX_GCL_NUM)19 return -EINVAL;20 cycle_time = 0;21 for (i = 0; i < qopt->num_entries; i++) {22 if (qopt->entries[i].command != TC_TAPRIO_CMD_SET_GATES)23 return -EINVAL;24 if (qopt->entries[i].gate_mask & ~TSNEP_GCL_MASK)25 return -EINVAL;26 if (qopt->entries[i].interval < TSNEP_GCL_MIN_INTERVAL)27 return -EINVAL;28 cycle_time += qopt->entries[i].interval;29 }30 if (qopt->cycle_time != cycle_time)31 return -EINVAL;32 if (qopt->cycle_time_extension >= qopt->cycle_time)33 return -EINVAL;34 35 return 0;36}37 38static void tsnep_write_gcl_operation(struct tsnep_gcl *gcl, int index,39 u32 properties, u32 interval, bool flush)40{41 void __iomem *addr = gcl->addr +42 sizeof(struct tsnep_gcl_operation) * index;43 44 gcl->operation[index].properties = properties;45 gcl->operation[index].interval = interval;46 47 iowrite32(properties, addr);48 iowrite32(interval, addr + sizeof(u32));49 50 if (flush) {51 /* flush write with read access */52 ioread32(addr);53 }54}55 56static u64 tsnep_change_duration(struct tsnep_gcl *gcl, int index)57{58 u64 duration;59 int count;60 61 /* change needs to be triggered one or two operations before start of62 * new gate control list63 * - change is triggered at start of operation (minimum one operation)64 * - operation with adjusted interval is inserted on demand to exactly65 * meet the start of the new gate control list (optional)66 *67 * additionally properties are read directly after start of previous68 * operation69 *70 * therefore, three operations needs to be considered for the limit71 */72 duration = 0;73 count = 3;74 while (count) {75 duration += gcl->operation[index].interval;76 77 index--;78 if (index < 0)79 index = gcl->count - 1;80 81 count--;82 }83 84 return duration;85}86 87static void tsnep_write_gcl(struct tsnep_gcl *gcl,88 struct tc_taprio_qopt_offload *qopt)89{90 int i;91 u32 properties;92 u64 extend;93 u64 cut;94 95 gcl->base_time = ktime_to_ns(qopt->base_time);96 gcl->cycle_time = qopt->cycle_time;97 gcl->cycle_time_extension = qopt->cycle_time_extension;98 99 for (i = 0; i < qopt->num_entries; i++) {100 properties = qopt->entries[i].gate_mask;101 if (i == (qopt->num_entries - 1))102 properties |= TSNEP_GCL_LAST;103 104 tsnep_write_gcl_operation(gcl, i, properties,105 qopt->entries[i].interval, true);106 }107 gcl->count = qopt->num_entries;108 109 /* calculate change limit; i.e., the time needed between enable and110 * start of new gate control list111 */112 113 /* case 1: extend cycle time for change114 * - change duration of last operation115 * - cycle time extension116 */117 extend = tsnep_change_duration(gcl, gcl->count - 1);118 extend += gcl->cycle_time_extension;119 120 /* case 2: cut cycle time for change121 * - maximum change duration122 */123 cut = 0;124 for (i = 0; i < gcl->count; i++)125 cut = max(cut, tsnep_change_duration(gcl, i));126 127 /* use maximum, because the actual case (extend or cut) can be128 * determined only after limit is known (chicken-and-egg problem)129 */130 gcl->change_limit = max(extend, cut);131}132 133static u64 tsnep_gcl_start_after(struct tsnep_gcl *gcl, u64 limit)134{135 u64 start = gcl->base_time;136 u64 n;137 138 if (start <= limit) {139 n = div64_u64(limit - start, gcl->cycle_time);140 start += (n + 1) * gcl->cycle_time;141 }142 143 return start;144}145 146static u64 tsnep_gcl_start_before(struct tsnep_gcl *gcl, u64 limit)147{148 u64 start = gcl->base_time;149 u64 n;150 151 n = div64_u64(limit - start, gcl->cycle_time);152 start += n * gcl->cycle_time;153 if (start == limit)154 start -= gcl->cycle_time;155 156 return start;157}158 159static u64 tsnep_set_gcl_change(struct tsnep_gcl *gcl, int index, u64 change,160 bool insert)161{162 /* previous operation triggers change and properties are evaluated at163 * start of operation164 */165 if (index == 0)166 index = gcl->count - 1;167 else168 index = index - 1;169 change -= gcl->operation[index].interval;170 171 /* optionally change to new list with additional operation in between */172 if (insert) {173 void __iomem *addr = gcl->addr +174 sizeof(struct tsnep_gcl_operation) * index;175 176 gcl->operation[index].properties |= TSNEP_GCL_INSERT;177 iowrite32(gcl->operation[index].properties, addr);178 }179 180 return change;181}182 183static void tsnep_clean_gcl(struct tsnep_gcl *gcl)184{185 int i;186 u32 mask = TSNEP_GCL_LAST | TSNEP_GCL_MASK;187 void __iomem *addr;188 189 /* search for insert operation and reset properties */190 for (i = 0; i < gcl->count; i++) {191 if (gcl->operation[i].properties & ~mask) {192 addr = gcl->addr +193 sizeof(struct tsnep_gcl_operation) * i;194 195 gcl->operation[i].properties &= mask;196 iowrite32(gcl->operation[i].properties, addr);197 198 break;199 }200 }201}202 203static u64 tsnep_insert_gcl_operation(struct tsnep_gcl *gcl, int ref,204 u64 change, u32 interval)205{206 u32 properties;207 208 properties = gcl->operation[ref].properties & TSNEP_GCL_MASK;209 /* change to new list directly after inserted operation */210 properties |= TSNEP_GCL_CHANGE;211 212 /* last operation of list is reserved to insert operation */213 tsnep_write_gcl_operation(gcl, TSNEP_GCL_COUNT - 1, properties,214 interval, false);215 216 return tsnep_set_gcl_change(gcl, ref, change, true);217}218 219static u64 tsnep_extend_gcl(struct tsnep_gcl *gcl, u64 start, u32 extension)220{221 int ref = gcl->count - 1;222 u32 interval = gcl->operation[ref].interval + extension;223 224 start -= gcl->operation[ref].interval;225 226 return tsnep_insert_gcl_operation(gcl, ref, start, interval);227}228 229static u64 tsnep_cut_gcl(struct tsnep_gcl *gcl, u64 start, u64 cycle_time)230{231 u64 sum = 0;232 int i;233 234 /* find operation which shall be cutted */235 for (i = 0; i < gcl->count; i++) {236 u64 sum_tmp = sum + gcl->operation[i].interval;237 u64 interval;238 239 /* sum up operations as long as cycle time is not exceeded */240 if (sum_tmp > cycle_time)241 break;242 243 /* remaining interval must be big enough for hardware */244 interval = cycle_time - sum_tmp;245 if (interval > 0 && interval < TSNEP_GCL_MIN_INTERVAL)246 break;247 248 sum = sum_tmp;249 }250 if (sum == cycle_time) {251 /* no need to cut operation itself or whole cycle252 * => change exactly at operation253 */254 return tsnep_set_gcl_change(gcl, i, start + sum, false);255 }256 return tsnep_insert_gcl_operation(gcl, i, start + sum,257 cycle_time - sum);258}259 260static int tsnep_enable_gcl(struct tsnep_adapter *adapter,261 struct tsnep_gcl *gcl, struct tsnep_gcl *curr)262{263 u64 system_time;264 u64 timeout;265 u64 limit;266 267 /* estimate timeout limit after timeout enable, actually timeout limit268 * in hardware will be earlier than estimate so we are on the safe side269 */270 tsnep_get_system_time(adapter, &system_time);271 timeout = system_time + TSNEP_GC_TIMEOUT;272 273 if (curr)274 limit = timeout + curr->change_limit;275 else276 limit = timeout;277 278 gcl->start_time = tsnep_gcl_start_after(gcl, limit);279 280 /* gate control time register is only 32bit => time shall be in the near281 * future (no driver support for far future implemented)282 */283 if ((gcl->start_time - system_time) >= U32_MAX)284 return -EAGAIN;285 286 if (curr) {287 /* change gate control list */288 u64 last;289 u64 change;290 291 last = tsnep_gcl_start_before(curr, gcl->start_time);292 if ((last + curr->cycle_time) == gcl->start_time)293 change = tsnep_cut_gcl(curr, last,294 gcl->start_time - last);295 else if (((gcl->start_time - last) <=296 curr->cycle_time_extension) ||297 ((gcl->start_time - last) <= TSNEP_GCL_MIN_INTERVAL))298 change = tsnep_extend_gcl(curr, last,299 gcl->start_time - last);300 else301 change = tsnep_cut_gcl(curr, last,302 gcl->start_time - last);303 304 WARN_ON(change <= timeout);305 gcl->change = true;306 iowrite32(change & 0xFFFFFFFF, adapter->addr + TSNEP_GC_CHANGE);307 } else {308 /* start gate control list */309 WARN_ON(gcl->start_time <= timeout);310 gcl->change = false;311 iowrite32(gcl->start_time & 0xFFFFFFFF,312 adapter->addr + TSNEP_GC_TIME);313 }314 315 return 0;316}317 318static int tsnep_taprio(struct tsnep_adapter *adapter,319 struct tc_taprio_qopt_offload *qopt)320{321 struct tsnep_gcl *gcl;322 struct tsnep_gcl *curr;323 int retval;324 325 if (!adapter->gate_control)326 return -EOPNOTSUPP;327 328 if (qopt->cmd == TAPRIO_CMD_DESTROY) {329 /* disable gate control if active */330 mutex_lock(&adapter->gate_control_lock);331 332 if (adapter->gate_control_active) {333 iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);334 adapter->gate_control_active = false;335 }336 337 mutex_unlock(&adapter->gate_control_lock);338 339 return 0;340 } else if (qopt->cmd != TAPRIO_CMD_REPLACE) {341 return -EOPNOTSUPP;342 }343 344 retval = tsnep_validate_gcl(qopt);345 if (retval)346 return retval;347 348 mutex_lock(&adapter->gate_control_lock);349 350 gcl = &adapter->gcl[adapter->next_gcl];351 tsnep_write_gcl(gcl, qopt);352 353 /* select current gate control list if active */354 if (adapter->gate_control_active) {355 if (adapter->next_gcl == 0)356 curr = &adapter->gcl[1];357 else358 curr = &adapter->gcl[0];359 } else {360 curr = NULL;361 }362 363 for (;;) {364 /* start timeout which discards late enable, this helps ensuring365 * that start/change time are in the future at enable366 */367 iowrite8(TSNEP_GC_ENABLE_TIMEOUT, adapter->addr + TSNEP_GC);368 369 retval = tsnep_enable_gcl(adapter, gcl, curr);370 if (retval) {371 mutex_unlock(&adapter->gate_control_lock);372 373 return retval;374 }375 376 /* enable gate control list */377 if (adapter->next_gcl == 0)378 iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);379 else380 iowrite8(TSNEP_GC_ENABLE_B, adapter->addr + TSNEP_GC);381 382 /* done if timeout did not happen */383 if (!(ioread32(adapter->addr + TSNEP_GC) &384 TSNEP_GC_TIMEOUT_SIGNAL))385 break;386 387 /* timeout is acknowledged with any enable */388 iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);389 390 if (curr)391 tsnep_clean_gcl(curr);392 393 /* retry because of timeout */394 }395 396 adapter->gate_control_active = true;397 398 if (adapter->next_gcl == 0)399 adapter->next_gcl = 1;400 else401 adapter->next_gcl = 0;402 403 mutex_unlock(&adapter->gate_control_lock);404 405 return 0;406}407 408static int tsnep_tc_query_caps(struct tsnep_adapter *adapter,409 struct tc_query_caps_base *base)410{411 switch (base->type) {412 case TC_SETUP_QDISC_TAPRIO: {413 struct tc_taprio_caps *caps = base->caps;414 415 if (!adapter->gate_control)416 return -EOPNOTSUPP;417 418 caps->gate_mask_per_txq = true;419 420 return 0;421 }422 default:423 return -EOPNOTSUPP;424 }425}426 427int tsnep_tc_setup(struct net_device *netdev, enum tc_setup_type type,428 void *type_data)429{430 struct tsnep_adapter *adapter = netdev_priv(netdev);431 432 switch (type) {433 case TC_QUERY_CAPS:434 return tsnep_tc_query_caps(adapter, type_data);435 case TC_SETUP_QDISC_TAPRIO:436 return tsnep_taprio(adapter, type_data);437 default:438 return -EOPNOTSUPP;439 }440}441 442int tsnep_tc_init(struct tsnep_adapter *adapter)443{444 if (!adapter->gate_control)445 return 0;446 447 /* open all gates */448 iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);449 iowrite32(TSNEP_GC_OPEN | TSNEP_GC_NEXT_OPEN, adapter->addr + TSNEP_GC);450 451 adapter->gcl[0].addr = adapter->addr + TSNEP_GCL_A;452 adapter->gcl[1].addr = adapter->addr + TSNEP_GCL_B;453 454 return 0;455}456 457void tsnep_tc_cleanup(struct tsnep_adapter *adapter)458{459 if (!adapter->gate_control)460 return;461 462 if (adapter->gate_control_active) {463 iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);464 adapter->gate_control_active = false;465 }466}467