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