drivers/net/ethernet/stmicro/stmmac/stmmac_tc.c

   1 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
   2 /*
   3  * Copyright (c) 2018 Synopsys, Inc. and/or its affiliates.
   4  * stmmac TC Handling (HW only)
   5  */
   6
   7 #include <net/pkt_cls.h>
   8 #include <net/tc_act/tc_gact.h>
   9 #include "common.h"
  10 #include "dwmac4.h"
  11 #include "dwmac5.h"
  12 #include "stmmac.h"
  13
  14 static void tc_fill_all_pass_entry(struct stmmac_tc_entry *entry)
  15 {
  16         memset(entry, 0, sizeof(*entry));
  17         entry->in_use = true;
  18         entry->is_last = true;
  19         entry->is_frag = false;
  20         entry->prio = ~0x0;
  21         entry->handle = 0;
  22         entry->val.match_data = 0x0;
  23         entry->val.match_en = 0x0;
  24         entry->val.af = 1;
  25         entry->val.dma_ch_no = 0x0;
  26 }
  27
  28 static struct stmmac_tc_entry *tc_find_entry(struct stmmac_priv *priv,
  29                                              struct tc_cls_u32_offload *cls,
  30                                              bool free)
  31 {
  32         struct stmmac_tc_entry *entry, *first = NULL, *dup = NULL;
  33         u32 loc = cls->knode.handle;
  34         int i;
  35
  36         for (i = 0; i < priv->tc_entries_max; i++) {
  37                 entry = &priv->tc_entries[i];
  38                 if (!entry->in_use && !first && free)
  39                         first = entry;
  40                 if ((entry->handle == loc) && !free && !entry->is_frag)
  41                         dup = entry;
  42         }
  43
  44         if (dup)
  45                 return dup;
  46         if (first) {
  47                 first->handle = loc;
  48                 first->in_use = true;
  49
  50                 /* Reset HW values */
  51                 memset(&first->val, 0, sizeof(first->val));
  52         }
  53
  54         return first;
  55 }
  56
  57 static int tc_fill_actions(struct stmmac_tc_entry *entry,
  58                            struct stmmac_tc_entry *frag,
  59                            struct tc_cls_u32_offload *cls)
  60 {
  61         struct stmmac_tc_entry *action_entry = entry;
  62         const struct tc_action *act;
  63         struct tcf_exts *exts;
  64         int i;
  65
  66         exts = cls->knode.exts;
  67         if (!tcf_exts_has_actions(exts))
  68                 return -EINVAL;
  69         if (frag)
  70                 action_entry = frag;
  71
  72         tcf_exts_for_each_action(i, act, exts) {
  73                 /* Accept */
  74                 if (is_tcf_gact_ok(act)) {
  75                         action_entry->val.af = 1;
  76                         break;
  77                 }
  78                 /* Drop */
  79                 if (is_tcf_gact_shot(act)) {
  80                         action_entry->val.rf = 1;
  81                         break;
  82                 }
  83
  84                 /* Unsupported */
  85                 return -EINVAL;
  86         }
  87
  88         return 0;
  89 }
  90
  91 static int tc_fill_entry(struct stmmac_priv *priv,
  92                          struct tc_cls_u32_offload *cls)
  93 {
  94         struct stmmac_tc_entry *entry, *frag = NULL;
  95         struct tc_u32_sel *sel = cls->knode.sel;
  96         u32 off, data, mask, real_off, rem;
  97         u32 prio = cls->common.prio;
  98         int ret;
  99
 100         /* Only 1 match per entry */
 101         if (sel->nkeys <= 0 || sel->nkeys > 1)
 102                 return -EINVAL;
 103
 104         off = sel->keys[0].off << sel->offshift;
 105         data = sel->keys[0].val;
 106         mask = sel->keys[0].mask;
 107
 108         switch (ntohs(cls->common.protocol)) {
 109         case ETH_P_ALL:
 110                 break;
 111         case ETH_P_IP:
 112                 off += ETH_HLEN;
 113                 break;
 114         default:
 115                 return -EINVAL;
 116         }
 117
 118         if (off > priv->tc_off_max)
 119                 return -EINVAL;
 120
 121         real_off = off / 4;
 122         rem = off % 4;
 123
 124         entry = tc_find_entry(priv, cls, true);
 125         if (!entry)
 126                 return -EINVAL;
 127
 128         if (rem) {
 129                 frag = tc_find_entry(priv, cls, true);
 130                 if (!frag) {
 131                         ret = -EINVAL;
 132                         goto err_unuse;
 133                 }
 134
 135                 entry->frag_ptr = frag;
 136                 entry->val.match_en = (mask << (rem * 8)) &
 137                         GENMASK(31, rem * 8);
 138                 entry->val.match_data = (data << (rem * 8)) &
 139                         GENMASK(31, rem * 8);
 140                 entry->val.frame_offset = real_off;
 141                 entry->prio = prio;
 142
 143                 frag->val.match_en = (mask >> (rem * 8)) &
 144                         GENMASK(rem * 8 - 1, 0);
 145                 frag->val.match_data = (data >> (rem * 8)) &
 146                         GENMASK(rem * 8 - 1, 0);
 147                 frag->val.frame_offset = real_off + 1;
 148                 frag->prio = prio;
 149                 frag->is_frag = true;
 150         } else {
 151                 entry->frag_ptr = NULL;
 152                 entry->val.match_en = mask;
 153                 entry->val.match_data = data;
 154                 entry->val.frame_offset = real_off;
 155                 entry->prio = prio;
 156         }
 157
 158         ret = tc_fill_actions(entry, frag, cls);
 159         if (ret)
 160                 goto err_unuse;
 161
 162         return 0;
 163
 164 err_unuse:
 165         if (frag)
 166                 frag->in_use = false;
 167         entry->in_use = false;
 168         return ret;
 169 }
 170
 171 static void tc_unfill_entry(struct stmmac_priv *priv,
 172                             struct tc_cls_u32_offload *cls)
 173 {
 174         struct stmmac_tc_entry *entry;
 175
 176         entry = tc_find_entry(priv, cls, false);
 177         if (!entry)
 178                 return;
 179
 180         entry->in_use = false;
 181         if (entry->frag_ptr) {
 182                 entry = entry->frag_ptr;
 183                 entry->is_frag = false;
 184                 entry->in_use = false;
 185         }
 186 }
 187
 188 static int tc_config_knode(struct stmmac_priv *priv,
 189                            struct tc_cls_u32_offload *cls)
 190 {
 191         int ret;
 192
 193         ret = tc_fill_entry(priv, cls);
 194         if (ret)
 195                 return ret;
 196
 197         ret = stmmac_rxp_config(priv, priv->hw->pcsr, priv->tc_entries,
 198                         priv->tc_entries_max);
 199         if (ret)
 200                 goto err_unfill;
 201
 202         return 0;
 203
 204 err_unfill:
 205         tc_unfill_entry(priv, cls);
 206         return ret;
 207 }
 208
 209 static int tc_delete_knode(struct stmmac_priv *priv,
 210                            struct tc_cls_u32_offload *cls)
 211 {
 212         int ret;
 213
 214         /* Set entry and fragments as not used */
 215         tc_unfill_entry(priv, cls);
 216
 217         ret = stmmac_rxp_config(priv, priv->hw->pcsr, priv->tc_entries,
 218                         priv->tc_entries_max);
 219         if (ret)
 220                 return ret;
 221
 222         return 0;
 223 }
 224
 225 static int tc_setup_cls_u32(struct stmmac_priv *priv,
 226                             struct tc_cls_u32_offload *cls)
 227 {
 228         switch (cls->command) {
 229         case TC_CLSU32_REPLACE_KNODE:
 230                 tc_unfill_entry(priv, cls);
 231                 /* Fall through */
 232         case TC_CLSU32_NEW_KNODE:
 233                 return tc_config_knode(priv, cls);
 234         case TC_CLSU32_DELETE_KNODE:
 235                 return tc_delete_knode(priv, cls);
 236         default:
 237                 return -EOPNOTSUPP;
 238         }
 239 }
 240
 241 static int tc_init(struct stmmac_priv *priv)
 242 {
 243         struct dma_features *dma_cap = &priv->dma_cap;
 244         unsigned int count;
 245
 246         if (!dma_cap->frpsel)
 247                 return -EINVAL;
 248
 249         switch (dma_cap->frpbs) {
 250         case 0x0:
 251                 priv->tc_off_max = 64;
 252                 break;
 253         case 0x1:
 254                 priv->tc_off_max = 128;
 255                 break;
 256         case 0x2:
 257                 priv->tc_off_max = 256;
 258                 break;
 259         default:
 260                 return -EINVAL;
 261         }
 262
 263         switch (dma_cap->frpes) {
 264         case 0x0:
 265                 count = 64;
 266                 break;
 267         case 0x1:
 268                 count = 128;
 269                 break;
 270         case 0x2:
 271                 count = 256;
 272                 break;
 273         default:
 274                 return -EINVAL;
 275         }
 276
 277         /* Reserve one last filter which lets all pass */
 278         priv->tc_entries_max = count;
 279         priv->tc_entries = devm_kcalloc(priv->device,
 280                         count, sizeof(*priv->tc_entries), GFP_KERNEL);
 281         if (!priv->tc_entries)
 282                 return -ENOMEM;
 283
 284         tc_fill_all_pass_entry(&priv->tc_entries[count - 1]);
 285
 286         dev_info(priv->device, "Enabling HW TC (entries=%d, max_off=%d)\n",
 287                         priv->tc_entries_max, priv->tc_off_max);
 288         return 0;
 289 }
 290
 291 static int tc_setup_cbs(struct stmmac_priv *priv,
 292                         struct tc_cbs_qopt_offload *qopt)
 293 {
 294         u32 tx_queues_count = priv->plat->tx_queues_to_use;
 295         u32 queue = qopt->queue;
 296         u32 ptr, speed_div;
 297         u32 mode_to_use;
 298         u64 value;
 299         int ret;
 300
 301         /* Queue 0 is not AVB capable */
 302         if (queue <= 0 || queue >= tx_queues_count)
 303                 return -EINVAL;
 304         if (!priv->dma_cap.av)
 305                 return -EOPNOTSUPP;
 306         if (priv->speed != SPEED_100 && priv->speed != SPEED_1000)
 307                 return -EOPNOTSUPP;
 308
 309         mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use;
 310         if (mode_to_use == MTL_QUEUE_DCB && qopt->enable) {
 311                 ret = stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_AVB);
 312                 if (ret)
 313                         return ret;
 314
 315                 priv->plat->tx_queues_cfg[queue].mode_to_use = MTL_QUEUE_AVB;
 316         } else if (!qopt->enable) {
 317                 return stmmac_dma_qmode(priv, priv->ioaddr, queue, MTL_QUEUE_DCB);
 318         }
 319
 320         /* Port Transmit Rate and Speed Divider */
 321         ptr = (priv->speed == SPEED_100) ? 4 : 8;
 322         speed_div = (priv->speed == SPEED_100) ? 100000 : 1000000;
 323
 324         /* Final adjustments for HW */
 325         value = div_s64(qopt->idleslope * 1024ll * ptr, speed_div);
 326         priv->plat->tx_queues_cfg[queue].idle_slope = value & GENMASK(31, 0);
 327
 328         value = div_s64(-qopt->sendslope * 1024ll * ptr, speed_div);
 329         priv->plat->tx_queues_cfg[queue].send_slope = value & GENMASK(31, 0);
 330
 331         value = qopt->hicredit * 1024ll * 8;
 332         priv->plat->tx_queues_cfg[queue].high_credit = value & GENMASK(31, 0);
 333
 334         value = qopt->locredit * 1024ll * 8;
 335         priv->plat->tx_queues_cfg[queue].low_credit = value & GENMASK(31, 0);
 336
 337         ret = stmmac_config_cbs(priv, priv->hw,
 338                                 priv->plat->tx_queues_cfg[queue].send_slope,
 339                                 priv->plat->tx_queues_cfg[queue].idle_slope,
 340                                 priv->plat->tx_queues_cfg[queue].high_credit,
 341                                 priv->plat->tx_queues_cfg[queue].low_credit,
 342                                 queue);
 343         if (ret)
 344                 return ret;
 345
 346         dev_info(priv->device, "CBS queue %d: send %d, idle %d, hi %d, lo %d\n",
 347                         queue, qopt->sendslope, qopt->idleslope,
 348                         qopt->hicredit, qopt->locredit);
 349         return 0;
 350 }
 351
 352 const struct stmmac_tc_ops dwmac510_tc_ops = {
 353         .init = tc_init,
 354         .setup_cls_u32 = tc_setup_cls_u32,
 355         .setup_cbs = tc_setup_cbs,
 356 };