drivers/net/ethernet/engleder/tsnep_tc.c

   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->enable) {
 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         }
 341
 342         retval = tsnep_validate_gcl(qopt);
 343         if (retval)
 344                 return retval;
 345
 346         mutex_lock(&adapter->gate_control_lock);
 347
 348         gcl = &adapter->gcl[adapter->next_gcl];
 349         tsnep_write_gcl(gcl, qopt);
 350
 351         /* select current gate control list if active */
 352         if (adapter->gate_control_active) {
 353                 if (adapter->next_gcl == 0)
 354                         curr = &adapter->gcl[1];
 355                 else
 356                         curr = &adapter->gcl[0];
 357         } else {
 358                 curr = NULL;
 359         }
 360
 361         for (;;) {
 362                 /* start timeout which discards late enable, this helps ensuring
 363                  * that start/change time are in the future at enable
 364                  */
 365                 iowrite8(TSNEP_GC_ENABLE_TIMEOUT, adapter->addr + TSNEP_GC);
 366
 367                 retval = tsnep_enable_gcl(adapter, gcl, curr);
 368                 if (retval) {
 369                         mutex_unlock(&adapter->gate_control_lock);
 370
 371                         return retval;
 372                 }
 373
 374                 /* enable gate control list */
 375                 if (adapter->next_gcl == 0)
 376                         iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);
 377                 else
 378                         iowrite8(TSNEP_GC_ENABLE_B, adapter->addr + TSNEP_GC);
 379
 380                 /* done if timeout did not happen */
 381                 if (!(ioread32(adapter->addr + TSNEP_GC) &
 382                       TSNEP_GC_TIMEOUT_SIGNAL))
 383                         break;
 384
 385                 /* timeout is acknowledged with any enable */
 386                 iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);
 387
 388                 if (curr)
 389                         tsnep_clean_gcl(curr);
 390
 391                 /* retry because of timeout */
 392         }
 393
 394         adapter->gate_control_active = true;
 395
 396         if (adapter->next_gcl == 0)
 397                 adapter->next_gcl = 1;
 398         else
 399                 adapter->next_gcl = 0;
 400
 401         mutex_unlock(&adapter->gate_control_lock);
 402
 403         return 0;
 404 }
 405
 406 int tsnep_tc_setup(struct net_device *netdev, enum tc_setup_type type,
 407                    void *type_data)
 408 {
 409         struct tsnep_adapter *adapter = netdev_priv(netdev);
 410
 411         switch (type) {
 412         case TC_SETUP_QDISC_TAPRIO:
 413                 return tsnep_taprio(adapter, type_data);
 414         default:
 415                 return -EOPNOTSUPP;
 416         }
 417 }
 418
 419 int tsnep_tc_init(struct tsnep_adapter *adapter)
 420 {
 421         if (!adapter->gate_control)
 422                 return 0;
 423
 424         /* open all gates */
 425         iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
 426         iowrite32(TSNEP_GC_OPEN | TSNEP_GC_NEXT_OPEN, adapter->addr + TSNEP_GC);
 427
 428         adapter->gcl[0].addr = adapter->addr + TSNEP_GCL_A;
 429         adapter->gcl[1].addr = adapter->addr + TSNEP_GCL_B;
 430
 431         return 0;
 432 }
 433
 434 void tsnep_tc_cleanup(struct tsnep_adapter *adapter)
 435 {
 436         if (!adapter->gate_control)
 437                 return;
 438
 439         if (adapter->gate_control_active) {
 440                 iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
 441                 adapter->gate_control_active = false;
 442         }
 443 }