1 // SPDX-License-Identifier: GPL-2.0 2 /* 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 11 static 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 38 static 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 56 static 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 of 62 * new gate control list 63 * - change is triggered at start of operation (minimum one operation) 64 * - operation with adjusted interval is inserted on demand to exactly 65 * meet the start of the new gate control list (optional) 66 * 67 * additionally properties are read directly after start of previous 68 * operation 69 * 70 * therefore, three operations needs to be considered for the limit 71 */ 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 87 static 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 and 110 * start of new gate control list 111 */ 112 113 /* case 1: extend cycle time for change 114 * - change duration of last operation 115 * - cycle time extension 116 */ 117 extend = tsnep_change_duration(gcl, gcl->count - 1); 118 extend += gcl->cycle_time_extension; 119 120 /* case 2: cut cycle time for change 121 * - maximum change duration 122 */ 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 be 128 * determined only after limit is known (chicken-and-egg problem) 129 */ 130 gcl->change_limit = max(extend, cut); 131 } 132 133 static 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 146 static 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 159 static 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 at 163 * start of operation 164 */ 165 if (index == 0) 166 index = gcl->count - 1; 167 else 168 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 183 static 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 203 static 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 219 static 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 229 static 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 cycle 252 * => change exactly at operation 253 */ 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 260 static 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 limit 268 * in hardware will be earlier than estimate so we are on the safe side 269 */ 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 else 276 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 near 281 * 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 else 301 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 318 static 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 else 358 curr = &adapter->gcl[0]; 359 } else { 360 curr = NULL; 361 } 362 363 for (;;) { 364 /* start timeout which discards late enable, this helps ensuring 365 * that start/change time are in the future at enable 366 */ 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 else 380 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 else 401 adapter->next_gcl = 0; 402 403 mutex_unlock(&adapter->gate_control_lock); 404 405 return 0; 406 } 407 408 static 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 427 int 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 442 int 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 457 void 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