ethernet/engleder/tsnep_tc.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */
0003
0004 #include "tsnep.h"
0005
0006 #include <net/pkt_sched.h>
0007
0008 /* save one operation at the end for additional operation at list change */
0009 #define TSNEP_MAX_GCL_NUM (TSNEP_GCL_COUNT - 1)
0010
0011 static int tsnep_validate_gcl(struct tc_taprio_qopt_offload *qopt)
0012 {
0013     int i;
0014     u64 cycle_time;
0015
0016     if (!qopt->cycle_time)
0017         return -ERANGE;
0018     if (qopt->num_entries > TSNEP_MAX_GCL_NUM)
0019         return -EINVAL;
0020     cycle_time = 0;
0021     for (i = 0; i < qopt->num_entries; i++) {
0022         if (qopt->entries[i].command != TC_TAPRIO_CMD_SET_GATES)
0023             return -EINVAL;
0024         if (qopt->entries[i].gate_mask & ~TSNEP_GCL_MASK)
0025             return -EINVAL;
0026         if (qopt->entries[i].interval < TSNEP_GCL_MIN_INTERVAL)
0027             return -EINVAL;
0028         cycle_time += qopt->entries[i].interval;
0029     }
0030     if (qopt->cycle_time != cycle_time)
0031         return -EINVAL;
0032     if (qopt->cycle_time_extension >= qopt->cycle_time)
0033         return -EINVAL;
0034
0035     return 0;
0036 }
0037
0038 static void tsnep_write_gcl_operation(struct tsnep_gcl *gcl, int index,
0039                       u32 properties, u32 interval, bool flush)
0040 {
0041     void __iomem *addr = gcl->addr +
0042                  sizeof(struct tsnep_gcl_operation) * index;
0043
0044     gcl->operation[index].properties = properties;
0045     gcl->operation[index].interval = interval;
0046
0047     iowrite32(properties, addr);
0048     iowrite32(interval, addr + sizeof(u32));
0049
0050     if (flush) {
0051         /* flush write with read access */
0052         ioread32(addr);
0053     }
0054 }
0055
0056 static u64 tsnep_change_duration(struct tsnep_gcl *gcl, int index)
0057 {
0058     u64 duration;
0059     int count;
0060
0061     /* change needs to be triggered one or two operations before start of
0062      * new gate control list
0063      * - change is triggered at start of operation (minimum one operation)
0064      * - operation with adjusted interval is inserted on demand to exactly
0065      *   meet the start of the new gate control list (optional)
0066      *
0067      * additionally properties are read directly after start of previous
0068      * operation
0069      *
0070      * therefore, three operations needs to be considered for the limit
0071      */
0072     duration = 0;
0073     count = 3;
0074     while (count) {
0075         duration += gcl->operation[index].interval;
0076
0077         index--;
0078         if (index < 0)
0079             index = gcl->count - 1;
0080
0081         count--;
0082     }
0083
0084     return duration;
0085 }
0086
0087 static void tsnep_write_gcl(struct tsnep_gcl *gcl,
0088                 struct tc_taprio_qopt_offload *qopt)
0089 {
0090     int i;
0091     u32 properties;
0092     u64 extend;
0093     u64 cut;
0094
0095     gcl->base_time = ktime_to_ns(qopt->base_time);
0096     gcl->cycle_time = qopt->cycle_time;
0097     gcl->cycle_time_extension = qopt->cycle_time_extension;
0098
0099     for (i = 0; i < qopt->num_entries; i++) {
0100         properties = qopt->entries[i].gate_mask;
0101         if (i == (qopt->num_entries - 1))
0102             properties |= TSNEP_GCL_LAST;
0103
0104         tsnep_write_gcl_operation(gcl, i, properties,
0105                       qopt->entries[i].interval, true);
0106     }
0107     gcl->count = qopt->num_entries;
0108
0109     /* calculate change limit; i.e., the time needed between enable and
0110      * start of new gate control list
0111      */
0112
0113     /* case 1: extend cycle time for change
0114      * - change duration of last operation
0115      * - cycle time extension
0116      */
0117     extend = tsnep_change_duration(gcl, gcl->count - 1);
0118     extend += gcl->cycle_time_extension;
0119
0120     /* case 2: cut cycle time for change
0121      * - maximum change duration
0122      */
0123     cut = 0;
0124     for (i = 0; i < gcl->count; i++)
0125         cut = max(cut, tsnep_change_duration(gcl, i));
0126
0127     /* use maximum, because the actual case (extend or cut) can be
0128      * determined only after limit is known (chicken-and-egg problem)
0129      */
0130     gcl->change_limit = max(extend, cut);
0131 }
0132
0133 static u64 tsnep_gcl_start_after(struct tsnep_gcl *gcl, u64 limit)
0134 {
0135     u64 start = gcl->base_time;
0136     u64 n;
0137
0138     if (start <= limit) {
0139         n = div64_u64(limit - start, gcl->cycle_time);
0140         start += (n + 1) * gcl->cycle_time;
0141     }
0142
0143     return start;
0144 }
0145
0146 static u64 tsnep_gcl_start_before(struct tsnep_gcl *gcl, u64 limit)
0147 {
0148     u64 start = gcl->base_time;
0149     u64 n;
0150
0151     n = div64_u64(limit - start, gcl->cycle_time);
0152     start += n * gcl->cycle_time;
0153     if (start == limit)
0154         start -= gcl->cycle_time;
0155
0156     return start;
0157 }
0158
0159 static u64 tsnep_set_gcl_change(struct tsnep_gcl *gcl, int index, u64 change,
0160                 bool insert)
0161 {
0162     /* previous operation triggers change and properties are evaluated at
0163      * start of operation
0164      */
0165     if (index == 0)
0166         index = gcl->count - 1;
0167     else
0168         index = index - 1;
0169     change -= gcl->operation[index].interval;
0170
0171     /* optionally change to new list with additional operation in between */
0172     if (insert) {
0173         void __iomem *addr = gcl->addr +
0174                      sizeof(struct tsnep_gcl_operation) * index;
0175
0176         gcl->operation[index].properties |= TSNEP_GCL_INSERT;
0177         iowrite32(gcl->operation[index].properties, addr);
0178     }
0179
0180     return change;
0181 }
0182
0183 static void tsnep_clean_gcl(struct tsnep_gcl *gcl)
0184 {
0185     int i;
0186     u32 mask = TSNEP_GCL_LAST | TSNEP_GCL_MASK;
0187     void __iomem *addr;
0188
0189     /* search for insert operation and reset properties */
0190     for (i = 0; i < gcl->count; i++) {
0191         if (gcl->operation[i].properties & ~mask) {
0192             addr = gcl->addr +
0193                    sizeof(struct tsnep_gcl_operation) * i;
0194
0195             gcl->operation[i].properties &= mask;
0196             iowrite32(gcl->operation[i].properties, addr);
0197
0198             break;
0199         }
0200     }
0201 }
0202
0203 static u64 tsnep_insert_gcl_operation(struct tsnep_gcl *gcl, int ref,
0204                       u64 change, u32 interval)
0205 {
0206     u32 properties;
0207
0208     properties = gcl->operation[ref].properties & TSNEP_GCL_MASK;
0209     /* change to new list directly after inserted operation */
0210     properties |= TSNEP_GCL_CHANGE;
0211
0212     /* last operation of list is reserved to insert operation */
0213     tsnep_write_gcl_operation(gcl, TSNEP_GCL_COUNT - 1, properties,
0214                   interval, false);
0215
0216     return tsnep_set_gcl_change(gcl, ref, change, true);
0217 }
0218
0219 static u64 tsnep_extend_gcl(struct tsnep_gcl *gcl, u64 start, u32 extension)
0220 {
0221     int ref = gcl->count - 1;
0222     u32 interval = gcl->operation[ref].interval + extension;
0223
0224     start -= gcl->operation[ref].interval;
0225
0226     return tsnep_insert_gcl_operation(gcl, ref, start, interval);
0227 }
0228
0229 static u64 tsnep_cut_gcl(struct tsnep_gcl *gcl, u64 start, u64 cycle_time)
0230 {
0231     u64 sum = 0;
0232     int i;
0233
0234     /* find operation which shall be cutted */
0235     for (i = 0; i < gcl->count; i++) {
0236         u64 sum_tmp = sum + gcl->operation[i].interval;
0237         u64 interval;
0238
0239         /* sum up operations as long as cycle time is not exceeded */
0240         if (sum_tmp > cycle_time)
0241             break;
0242
0243         /* remaining interval must be big enough for hardware */
0244         interval = cycle_time - sum_tmp;
0245         if (interval > 0 && interval < TSNEP_GCL_MIN_INTERVAL)
0246             break;
0247
0248         sum = sum_tmp;
0249     }
0250     if (sum == cycle_time) {
0251         /* no need to cut operation itself or whole cycle
0252          * => change exactly at operation
0253          */
0254         return tsnep_set_gcl_change(gcl, i, start + sum, false);
0255     }
0256     return tsnep_insert_gcl_operation(gcl, i, start + sum,
0257                       cycle_time - sum);
0258 }
0259
0260 static int tsnep_enable_gcl(struct tsnep_adapter *adapter,
0261                 struct tsnep_gcl *gcl, struct tsnep_gcl *curr)
0262 {
0263     u64 system_time;
0264     u64 timeout;
0265     u64 limit;
0266
0267     /* estimate timeout limit after timeout enable, actually timeout limit
0268      * in hardware will be earlier than estimate so we are on the safe side
0269      */
0270     tsnep_get_system_time(adapter, &system_time);
0271     timeout = system_time + TSNEP_GC_TIMEOUT;
0272
0273     if (curr)
0274         limit = timeout + curr->change_limit;
0275     else
0276         limit = timeout;
0277
0278     gcl->start_time = tsnep_gcl_start_after(gcl, limit);
0279
0280     /* gate control time register is only 32bit => time shall be in the near
0281      * future (no driver support for far future implemented)
0282      */
0283     if ((gcl->start_time - system_time) >= U32_MAX)
0284         return -EAGAIN;
0285
0286     if (curr) {
0287         /* change gate control list */
0288         u64 last;
0289         u64 change;
0290
0291         last = tsnep_gcl_start_before(curr, gcl->start_time);
0292         if ((last + curr->cycle_time) == gcl->start_time)
0293             change = tsnep_cut_gcl(curr, last,
0294                            gcl->start_time - last);
0295         else if (((gcl->start_time - last) <=
0296               curr->cycle_time_extension) ||
0297              ((gcl->start_time - last) <= TSNEP_GCL_MIN_INTERVAL))
0298             change = tsnep_extend_gcl(curr, last,
0299                           gcl->start_time - last);
0300         else
0301             change = tsnep_cut_gcl(curr, last,
0302                            gcl->start_time - last);
0303
0304         WARN_ON(change <= timeout);
0305         gcl->change = true;
0306         iowrite32(change & 0xFFFFFFFF, adapter->addr + TSNEP_GC_CHANGE);
0307     } else {
0308         /* start gate control list */
0309         WARN_ON(gcl->start_time <= timeout);
0310         gcl->change = false;
0311         iowrite32(gcl->start_time & 0xFFFFFFFF,
0312               adapter->addr + TSNEP_GC_TIME);
0313     }
0314
0315     return 0;
0316 }
0317
0318 static int tsnep_taprio(struct tsnep_adapter *adapter,
0319             struct tc_taprio_qopt_offload *qopt)
0320 {
0321     struct tsnep_gcl *gcl;
0322     struct tsnep_gcl *curr;
0323     int retval;
0324
0325     if (!adapter->gate_control)
0326         return -EOPNOTSUPP;
0327
0328     if (!qopt->enable) {
0329         /* disable gate control if active */
0330         mutex_lock(&adapter->gate_control_lock);
0331
0332         if (adapter->gate_control_active) {
0333             iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
0334             adapter->gate_control_active = false;
0335         }
0336
0337         mutex_unlock(&adapter->gate_control_lock);
0338
0339         return 0;
0340     }
0341
0342     retval = tsnep_validate_gcl(qopt);
0343     if (retval)
0344         return retval;
0345
0346     mutex_lock(&adapter->gate_control_lock);
0347
0348     gcl = &adapter->gcl[adapter->next_gcl];
0349     tsnep_write_gcl(gcl, qopt);
0350
0351     /* select current gate control list if active */
0352     if (adapter->gate_control_active) {
0353         if (adapter->next_gcl == 0)
0354             curr = &adapter->gcl[1];
0355         else
0356             curr = &adapter->gcl[0];
0357     } else {
0358         curr = NULL;
0359     }
0360
0361     for (;;) {
0362         /* start timeout which discards late enable, this helps ensuring
0363          * that start/change time are in the future at enable
0364          */
0365         iowrite8(TSNEP_GC_ENABLE_TIMEOUT, adapter->addr + TSNEP_GC);
0366
0367         retval = tsnep_enable_gcl(adapter, gcl, curr);
0368         if (retval) {
0369             mutex_unlock(&adapter->gate_control_lock);
0370
0371             return retval;
0372         }
0373
0374         /* enable gate control list */
0375         if (adapter->next_gcl == 0)
0376             iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);
0377         else
0378             iowrite8(TSNEP_GC_ENABLE_B, adapter->addr + TSNEP_GC);
0379
0380         /* done if timeout did not happen */
0381         if (!(ioread32(adapter->addr + TSNEP_GC) &
0382               TSNEP_GC_TIMEOUT_SIGNAL))
0383             break;
0384
0385         /* timeout is acknowledged with any enable */
0386         iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);
0387
0388         if (curr)
0389             tsnep_clean_gcl(curr);
0390
0391         /* retry because of timeout */
0392     }
0393
0394     adapter->gate_control_active = true;
0395
0396     if (adapter->next_gcl == 0)
0397         adapter->next_gcl = 1;
0398     else
0399         adapter->next_gcl = 0;
0400
0401     mutex_unlock(&adapter->gate_control_lock);
0402
0403     return 0;
0404 }
0405
0406 int tsnep_tc_setup(struct net_device *netdev, enum tc_setup_type type,
0407            void *type_data)
0408 {
0409     struct tsnep_adapter *adapter = netdev_priv(netdev);
0410
0411     switch (type) {
0412     case TC_SETUP_QDISC_TAPRIO:
0413         return tsnep_taprio(adapter, type_data);
0414     default:
0415         return -EOPNOTSUPP;
0416     }
0417 }
0418
0419 int tsnep_tc_init(struct tsnep_adapter *adapter)
0420 {
0421     if (!adapter->gate_control)
0422         return 0;
0423
0424     /* open all gates */
0425     iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
0426     iowrite32(TSNEP_GC_OPEN | TSNEP_GC_NEXT_OPEN, adapter->addr + TSNEP_GC);
0427
0428     adapter->gcl[0].addr = adapter->addr + TSNEP_GCL_A;
0429     adapter->gcl[1].addr = adapter->addr + TSNEP_GCL_B;
0430
0431     return 0;
0432 }
0433
0434 void tsnep_tc_cleanup(struct tsnep_adapter *adapter)
0435 {
0436     if (!adapter->gate_control)
0437         return;
0438
0439     if (adapter->gate_control_active) {
0440         iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
0441         adapter->gate_control_active = false;
0442     }
0443 }