drivers/net/ethernet/mellanox/mlxsw/spectrum_qdisc.c

   1 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
   2 /* Copyright (c) 2017-2018 Mellanox Technologies. All rights reserved */
   3
   4 #include <linux/kernel.h>
   5 #include <linux/errno.h>
   6 #include <linux/netdevice.h>
   7 #include <net/pkt_cls.h>
   8 #include <net/red.h>
   9
  10 #include "spectrum.h"
  11 #include "spectrum_span.h"
  12 #include "reg.h"
  13
  14 #define MLXSW_SP_PRIO_BAND_TO_TCLASS(band) (IEEE_8021QAZ_MAX_TCS - band - 1)
  15 #define MLXSW_SP_PRIO_CHILD_TO_TCLASS(child) \
  16         MLXSW_SP_PRIO_BAND_TO_TCLASS((child - 1))
  17
  18 enum mlxsw_sp_qdisc_type {
  19         MLXSW_SP_QDISC_NO_QDISC,
  20         MLXSW_SP_QDISC_RED,
  21         MLXSW_SP_QDISC_PRIO,
  22         MLXSW_SP_QDISC_ETS,
  23         MLXSW_SP_QDISC_TBF,
  24         MLXSW_SP_QDISC_FIFO,
  25 };
  26
  27 struct mlxsw_sp_qdisc;
  28
  29 struct mlxsw_sp_qdisc_ops {
  30         enum mlxsw_sp_qdisc_type type;
  31         int (*check_params)(struct mlxsw_sp_port *mlxsw_sp_port,
  32                             void *params);
  33         int (*replace)(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
  34                        struct mlxsw_sp_qdisc *mlxsw_sp_qdisc, void *params);
  35         int (*destroy)(struct mlxsw_sp_port *mlxsw_sp_port,
  36                        struct mlxsw_sp_qdisc *mlxsw_sp_qdisc);
  37         int (*get_stats)(struct mlxsw_sp_port *mlxsw_sp_port,
  38                          struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
  39                          struct tc_qopt_offload_stats *stats_ptr);
  40         int (*get_xstats)(struct mlxsw_sp_port *mlxsw_sp_port,
  41                           struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
  42                           void *xstats_ptr);
  43         void (*clean_stats)(struct mlxsw_sp_port *mlxsw_sp_port,
  44                             struct mlxsw_sp_qdisc *mlxsw_sp_qdisc);
  45         /* unoffload - to be used for a qdisc that stops being offloaded without
  46          * being destroyed.
  47          */
  48         void (*unoffload)(struct mlxsw_sp_port *mlxsw_sp_port,
  49                           struct mlxsw_sp_qdisc *mlxsw_sp_qdisc, void *params);
  50         struct mlxsw_sp_qdisc *(*find_class)(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
  51                                              u32 parent);
  52         unsigned int num_classes;
  53 };
  54
  55 struct mlxsw_sp_qdisc {
  56         u32 handle;
  57         int tclass_num;
  58         u8 prio_bitmap;
  59         union {
  60                 struct red_stats red;
  61         } xstats_base;
  62         struct mlxsw_sp_qdisc_stats {
  63                 u64 tx_bytes;
  64                 u64 tx_packets;
  65                 u64 drops;
  66                 u64 overlimits;
  67                 u64 backlog;
  68         } stats_base;
  69
  70         struct mlxsw_sp_qdisc_ops *ops;
  71         struct mlxsw_sp_qdisc *parent;
  72         struct mlxsw_sp_qdisc *qdiscs;
  73         unsigned int num_classes;
  74 };
  75
  76 struct mlxsw_sp_qdisc_state {
  77         struct mlxsw_sp_qdisc root_qdisc;
  78
  79         /* When a PRIO or ETS are added, the invisible FIFOs in their bands are
  80          * created first. When notifications for these FIFOs arrive, it is not
  81          * known what qdisc their parent handle refers to. It could be a
  82          * newly-created PRIO that will replace the currently-offloaded one, or
  83          * it could be e.g. a RED that will be attached below it.
  84          *
  85          * As the notifications start to arrive, use them to note what the
  86          * future parent handle is, and keep track of which child FIFOs were
  87          * seen. Then when the parent is known, retroactively offload those
  88          * FIFOs.
  89          */
  90         u32 future_handle;
  91         bool future_fifos[IEEE_8021QAZ_MAX_TCS];
  92         struct mutex lock; /* Protects qdisc state. */
  93 };
  94
  95 static bool
  96 mlxsw_sp_qdisc_compare(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc, u32 handle)
  97 {
  98         return mlxsw_sp_qdisc->ops && mlxsw_sp_qdisc->handle == handle;
  99 }
 100
 101 static struct mlxsw_sp_qdisc *
 102 mlxsw_sp_qdisc_walk(struct mlxsw_sp_qdisc *qdisc,
 103                     struct mlxsw_sp_qdisc *(*pre)(struct mlxsw_sp_qdisc *,
 104                                                   void *),
 105                     void *data)
 106 {
 107         struct mlxsw_sp_qdisc *tmp;
 108         unsigned int i;
 109
 110         if (pre) {
 111                 tmp = pre(qdisc, data);
 112                 if (tmp)
 113                         return tmp;
 114         }
 115
 116         if (qdisc->ops) {
 117                 for (i = 0; i < qdisc->num_classes; i++) {
 118                         tmp = &qdisc->qdiscs[i];
 119                         if (qdisc->ops) {
 120                                 tmp = mlxsw_sp_qdisc_walk(tmp, pre, data);
 121                                 if (tmp)
 122                                         return tmp;
 123                         }
 124                 }
 125         }
 126
 127         return NULL;
 128 }
 129
 130 static struct mlxsw_sp_qdisc *
 131 mlxsw_sp_qdisc_walk_cb_find(struct mlxsw_sp_qdisc *qdisc, void *data)
 132 {
 133         u32 parent = *(u32 *)data;
 134
 135         if (qdisc->ops && TC_H_MAJ(qdisc->handle) == TC_H_MAJ(parent)) {
 136                 if (qdisc->ops->find_class)
 137                         return qdisc->ops->find_class(qdisc, parent);
 138         }
 139
 140         return NULL;
 141 }
 142
 143 static struct mlxsw_sp_qdisc *
 144 mlxsw_sp_qdisc_find(struct mlxsw_sp_port *mlxsw_sp_port, u32 parent,
 145                     bool root_only)
 146 {
 147         struct mlxsw_sp_qdisc_state *qdisc_state = mlxsw_sp_port->qdisc;
 148
 149         if (!qdisc_state)
 150                 return NULL;
 151         if (parent == TC_H_ROOT)
 152                 return &qdisc_state->root_qdisc;
 153         if (root_only)
 154                 return NULL;
 155         return mlxsw_sp_qdisc_walk(&qdisc_state->root_qdisc,
 156                                    mlxsw_sp_qdisc_walk_cb_find, &parent);
 157 }
 158
 159 static struct mlxsw_sp_qdisc *
 160 mlxsw_sp_qdisc_walk_cb_find_by_handle(struct mlxsw_sp_qdisc *qdisc, void *data)
 161 {
 162         u32 handle = *(u32 *)data;
 163
 164         if (qdisc->ops && qdisc->handle == handle)
 165                 return qdisc;
 166         return NULL;
 167 }
 168
 169 static struct mlxsw_sp_qdisc *
 170 mlxsw_sp_qdisc_find_by_handle(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle)
 171 {
 172         struct mlxsw_sp_qdisc_state *qdisc_state = mlxsw_sp_port->qdisc;
 173
 174         if (!qdisc_state)
 175                 return NULL;
 176         return mlxsw_sp_qdisc_walk(&qdisc_state->root_qdisc,
 177                                    mlxsw_sp_qdisc_walk_cb_find_by_handle,
 178                                    &handle);
 179 }
 180
 181 static void
 182 mlxsw_sp_qdisc_reduce_parent_backlog(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
 183 {
 184         struct mlxsw_sp_qdisc *tmp;
 185
 186         for (tmp = mlxsw_sp_qdisc->parent; tmp; tmp = tmp->parent)
 187                 tmp->stats_base.backlog -= mlxsw_sp_qdisc->stats_base.backlog;
 188 }
 189
 190 static int
 191 mlxsw_sp_qdisc_destroy(struct mlxsw_sp_port *mlxsw_sp_port,
 192                        struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
 193 {
 194         struct mlxsw_sp_qdisc *root_qdisc = &mlxsw_sp_port->qdisc->root_qdisc;
 195         int err_hdroom = 0;
 196         int err = 0;
 197
 198         if (!mlxsw_sp_qdisc)
 199                 return 0;
 200
 201         if (root_qdisc == mlxsw_sp_qdisc) {
 202                 struct mlxsw_sp_hdroom hdroom = *mlxsw_sp_port->hdroom;
 203
 204                 hdroom.mode = MLXSW_SP_HDROOM_MODE_DCB;
 205                 mlxsw_sp_hdroom_prios_reset_buf_idx(&hdroom);
 206                 mlxsw_sp_hdroom_bufs_reset_lossiness(&hdroom);
 207                 mlxsw_sp_hdroom_bufs_reset_sizes(mlxsw_sp_port, &hdroom);
 208                 err_hdroom = mlxsw_sp_hdroom_configure(mlxsw_sp_port, &hdroom);
 209         }
 210
 211         if (!mlxsw_sp_qdisc->ops)
 212                 return 0;
 213
 214         mlxsw_sp_qdisc_reduce_parent_backlog(mlxsw_sp_qdisc);
 215         if (mlxsw_sp_qdisc->ops->destroy)
 216                 err = mlxsw_sp_qdisc->ops->destroy(mlxsw_sp_port,
 217                                                    mlxsw_sp_qdisc);
 218         if (mlxsw_sp_qdisc->ops->clean_stats)
 219                 mlxsw_sp_qdisc->ops->clean_stats(mlxsw_sp_port, mlxsw_sp_qdisc);
 220
 221         mlxsw_sp_qdisc->handle = TC_H_UNSPEC;
 222         mlxsw_sp_qdisc->ops = NULL;
 223         mlxsw_sp_qdisc->num_classes = 0;
 224         kfree(mlxsw_sp_qdisc->qdiscs);
 225         mlxsw_sp_qdisc->qdiscs = NULL;
 226         return err_hdroom ?: err;
 227 }
 228
 229 static int mlxsw_sp_qdisc_create(struct mlxsw_sp_port *mlxsw_sp_port,
 230                                  u32 handle,
 231                                  struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 232                                  struct mlxsw_sp_qdisc_ops *ops, void *params)
 233 {
 234         struct mlxsw_sp_qdisc *root_qdisc = &mlxsw_sp_port->qdisc->root_qdisc;
 235         struct mlxsw_sp_hdroom orig_hdroom;
 236         unsigned int i;
 237         int err;
 238
 239         err = ops->check_params(mlxsw_sp_port, params);
 240         if (err)
 241                 return err;
 242
 243         if (ops->num_classes) {
 244                 mlxsw_sp_qdisc->qdiscs = kcalloc(ops->num_classes,
 245                                                  sizeof(*mlxsw_sp_qdisc->qdiscs),
 246                                                  GFP_KERNEL);
 247                 if (!mlxsw_sp_qdisc->qdiscs)
 248                         return -ENOMEM;
 249
 250                 for (i = 0; i < ops->num_classes; i++)
 251                         mlxsw_sp_qdisc->qdiscs[i].parent = mlxsw_sp_qdisc;
 252         }
 253
 254         orig_hdroom = *mlxsw_sp_port->hdroom;
 255         if (root_qdisc == mlxsw_sp_qdisc) {
 256                 struct mlxsw_sp_hdroom hdroom = orig_hdroom;
 257
 258                 hdroom.mode = MLXSW_SP_HDROOM_MODE_TC;
 259                 mlxsw_sp_hdroom_prios_reset_buf_idx(&hdroom);
 260                 mlxsw_sp_hdroom_bufs_reset_lossiness(&hdroom);
 261                 mlxsw_sp_hdroom_bufs_reset_sizes(mlxsw_sp_port, &hdroom);
 262
 263                 err = mlxsw_sp_hdroom_configure(mlxsw_sp_port, &hdroom);
 264                 if (err)
 265                         goto err_hdroom_configure;
 266         }
 267
 268         mlxsw_sp_qdisc->num_classes = ops->num_classes;
 269         mlxsw_sp_qdisc->ops = ops;
 270         mlxsw_sp_qdisc->handle = handle;
 271         err = ops->replace(mlxsw_sp_port, handle, mlxsw_sp_qdisc, params);
 272         if (err)
 273                 goto err_replace;
 274
 275         return 0;
 276
 277 err_replace:
 278         mlxsw_sp_qdisc->handle = TC_H_UNSPEC;
 279         mlxsw_sp_qdisc->ops = NULL;
 280         mlxsw_sp_qdisc->num_classes = 0;
 281         mlxsw_sp_hdroom_configure(mlxsw_sp_port, &orig_hdroom);
 282 err_hdroom_configure:
 283         kfree(mlxsw_sp_qdisc->qdiscs);
 284         mlxsw_sp_qdisc->qdiscs = NULL;
 285         return err;
 286 }
 287
 288 static int
 289 mlxsw_sp_qdisc_change(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
 290                       struct mlxsw_sp_qdisc *mlxsw_sp_qdisc, void *params)
 291 {
 292         struct mlxsw_sp_qdisc_ops *ops = mlxsw_sp_qdisc->ops;
 293         int err;
 294
 295         err = ops->check_params(mlxsw_sp_port, params);
 296         if (err)
 297                 goto unoffload;
 298
 299         err = ops->replace(mlxsw_sp_port, handle, mlxsw_sp_qdisc, params);
 300         if (err)
 301                 goto unoffload;
 302
 303         /* Check if the Qdisc changed. That includes a situation where an
 304          * invisible Qdisc replaces another one, or is being added for the
 305          * first time.
 306          */
 307         if (mlxsw_sp_qdisc->handle != handle) {
 308                 if (ops->clean_stats)
 309                         ops->clean_stats(mlxsw_sp_port, mlxsw_sp_qdisc);
 310         }
 311
 312         mlxsw_sp_qdisc->handle = handle;
 313         return 0;
 314
 315 unoffload:
 316         if (ops->unoffload)
 317                 ops->unoffload(mlxsw_sp_port, mlxsw_sp_qdisc, params);
 318
 319         mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
 320         return err;
 321 }
 322
 323 static int
 324 mlxsw_sp_qdisc_replace(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
 325                        struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 326                        struct mlxsw_sp_qdisc_ops *ops, void *params)
 327 {
 328         if (mlxsw_sp_qdisc->ops && mlxsw_sp_qdisc->ops->type != ops->type)
 329                 /* In case this location contained a different qdisc of the
 330                  * same type we can override the old qdisc configuration.
 331                  * Otherwise, we need to remove the old qdisc before setting the
 332                  * new one.
 333                  */
 334                 mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
 335
 336         if (!mlxsw_sp_qdisc->ops)
 337                 return mlxsw_sp_qdisc_create(mlxsw_sp_port, handle,
 338                                              mlxsw_sp_qdisc, ops, params);
 339         else
 340                 return mlxsw_sp_qdisc_change(mlxsw_sp_port, handle,
 341                                              mlxsw_sp_qdisc, params);
 342 }
 343
 344 static int
 345 mlxsw_sp_qdisc_get_stats(struct mlxsw_sp_port *mlxsw_sp_port,
 346                          struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 347                          struct tc_qopt_offload_stats *stats_ptr)
 348 {
 349         if (mlxsw_sp_qdisc && mlxsw_sp_qdisc->ops &&
 350             mlxsw_sp_qdisc->ops->get_stats)
 351                 return mlxsw_sp_qdisc->ops->get_stats(mlxsw_sp_port,
 352                                                       mlxsw_sp_qdisc,
 353                                                       stats_ptr);
 354
 355         return -EOPNOTSUPP;
 356 }
 357
 358 static int
 359 mlxsw_sp_qdisc_get_xstats(struct mlxsw_sp_port *mlxsw_sp_port,
 360                           struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 361                           void *xstats_ptr)
 362 {
 363         if (mlxsw_sp_qdisc && mlxsw_sp_qdisc->ops &&
 364             mlxsw_sp_qdisc->ops->get_xstats)
 365                 return mlxsw_sp_qdisc->ops->get_xstats(mlxsw_sp_port,
 366                                                       mlxsw_sp_qdisc,
 367                                                       xstats_ptr);
 368
 369         return -EOPNOTSUPP;
 370 }
 371
 372 static u64
 373 mlxsw_sp_xstats_backlog(struct mlxsw_sp_port_xstats *xstats, int tclass_num)
 374 {
 375         return xstats->backlog[tclass_num] +
 376                xstats->backlog[tclass_num + 8];
 377 }
 378
 379 static u64
 380 mlxsw_sp_xstats_tail_drop(struct mlxsw_sp_port_xstats *xstats, int tclass_num)
 381 {
 382         return xstats->tail_drop[tclass_num] +
 383                xstats->tail_drop[tclass_num + 8];
 384 }
 385
 386 static void
 387 mlxsw_sp_qdisc_bstats_per_priority_get(struct mlxsw_sp_port_xstats *xstats,
 388                                        u8 prio_bitmap, u64 *tx_packets,
 389                                        u64 *tx_bytes)
 390 {
 391         int i;
 392
 393         *tx_packets = 0;
 394         *tx_bytes = 0;
 395         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
 396                 if (prio_bitmap & BIT(i)) {
 397                         *tx_packets += xstats->tx_packets[i];
 398                         *tx_bytes += xstats->tx_bytes[i];
 399                 }
 400         }
 401 }
 402
 403 static void
 404 mlxsw_sp_qdisc_collect_tc_stats(struct mlxsw_sp_port *mlxsw_sp_port,
 405                                 struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 406                                 u64 *p_tx_bytes, u64 *p_tx_packets,
 407                                 u64 *p_drops, u64 *p_backlog)
 408 {
 409         int tclass_num = mlxsw_sp_qdisc->tclass_num;
 410         struct mlxsw_sp_port_xstats *xstats;
 411         u64 tx_bytes, tx_packets;
 412
 413         xstats = &mlxsw_sp_port->periodic_hw_stats.xstats;
 414         mlxsw_sp_qdisc_bstats_per_priority_get(xstats,
 415                                                mlxsw_sp_qdisc->prio_bitmap,
 416                                                &tx_packets, &tx_bytes);
 417
 418         *p_tx_packets += tx_packets;
 419         *p_tx_bytes += tx_bytes;
 420         *p_drops += xstats->wred_drop[tclass_num] +
 421                     mlxsw_sp_xstats_tail_drop(xstats, tclass_num);
 422         *p_backlog += mlxsw_sp_xstats_backlog(xstats, tclass_num);
 423 }
 424
 425 static void
 426 mlxsw_sp_qdisc_update_stats(struct mlxsw_sp *mlxsw_sp,
 427                             struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 428                             u64 tx_bytes, u64 tx_packets,
 429                             u64 drops, u64 backlog,
 430                             struct tc_qopt_offload_stats *stats_ptr)
 431 {
 432         struct mlxsw_sp_qdisc_stats *stats_base = &mlxsw_sp_qdisc->stats_base;
 433
 434         tx_bytes -= stats_base->tx_bytes;
 435         tx_packets -= stats_base->tx_packets;
 436         drops -= stats_base->drops;
 437         backlog -= stats_base->backlog;
 438
 439         _bstats_update(stats_ptr->bstats, tx_bytes, tx_packets);
 440         stats_ptr->qstats->drops += drops;
 441         stats_ptr->qstats->backlog += mlxsw_sp_cells_bytes(mlxsw_sp, backlog);
 442
 443         stats_base->backlog += backlog;
 444         stats_base->drops += drops;
 445         stats_base->tx_bytes += tx_bytes;
 446         stats_base->tx_packets += tx_packets;
 447 }
 448
 449 static void
 450 mlxsw_sp_qdisc_get_tc_stats(struct mlxsw_sp_port *mlxsw_sp_port,
 451                             struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 452                             struct tc_qopt_offload_stats *stats_ptr)
 453 {
 454         u64 tx_packets = 0;
 455         u64 tx_bytes = 0;
 456         u64 backlog = 0;
 457         u64 drops = 0;
 458
 459         mlxsw_sp_qdisc_collect_tc_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
 460                                         &tx_bytes, &tx_packets,
 461                                         &drops, &backlog);
 462         mlxsw_sp_qdisc_update_stats(mlxsw_sp_port->mlxsw_sp, mlxsw_sp_qdisc,
 463                                     tx_bytes, tx_packets, drops, backlog,
 464                                     stats_ptr);
 465 }
 466
 467 static int
 468 mlxsw_sp_tclass_congestion_enable(struct mlxsw_sp_port *mlxsw_sp_port,
 469                                   int tclass_num, u32 min, u32 max,
 470                                   u32 probability, bool is_wred, bool is_ecn)
 471 {
 472         char cwtpm_cmd[MLXSW_REG_CWTPM_LEN];
 473         char cwtp_cmd[MLXSW_REG_CWTP_LEN];
 474         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 475         int err;
 476
 477         mlxsw_reg_cwtp_pack(cwtp_cmd, mlxsw_sp_port->local_port, tclass_num);
 478         mlxsw_reg_cwtp_profile_pack(cwtp_cmd, MLXSW_REG_CWTP_DEFAULT_PROFILE,
 479                                     roundup(min, MLXSW_REG_CWTP_MIN_VALUE),
 480                                     roundup(max, MLXSW_REG_CWTP_MIN_VALUE),
 481                                     probability);
 482
 483         err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(cwtp), cwtp_cmd);
 484         if (err)
 485                 return err;
 486
 487         mlxsw_reg_cwtpm_pack(cwtpm_cmd, mlxsw_sp_port->local_port, tclass_num,
 488                              MLXSW_REG_CWTP_DEFAULT_PROFILE, is_wred, is_ecn);
 489
 490         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(cwtpm), cwtpm_cmd);
 491 }
 492
 493 static int
 494 mlxsw_sp_tclass_congestion_disable(struct mlxsw_sp_port *mlxsw_sp_port,
 495                                    int tclass_num)
 496 {
 497         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 498         char cwtpm_cmd[MLXSW_REG_CWTPM_LEN];
 499
 500         mlxsw_reg_cwtpm_pack(cwtpm_cmd, mlxsw_sp_port->local_port, tclass_num,
 501                              MLXSW_REG_CWTPM_RESET_PROFILE, false, false);
 502         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(cwtpm), cwtpm_cmd);
 503 }
 504
 505 static void
 506 mlxsw_sp_setup_tc_qdisc_red_clean_stats(struct mlxsw_sp_port *mlxsw_sp_port,
 507                                         struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
 508 {
 509         int tclass_num = mlxsw_sp_qdisc->tclass_num;
 510         struct mlxsw_sp_qdisc_stats *stats_base;
 511         struct mlxsw_sp_port_xstats *xstats;
 512         struct red_stats *red_base;
 513
 514         xstats = &mlxsw_sp_port->periodic_hw_stats.xstats;
 515         stats_base = &mlxsw_sp_qdisc->stats_base;
 516         red_base = &mlxsw_sp_qdisc->xstats_base.red;
 517
 518         mlxsw_sp_qdisc_bstats_per_priority_get(xstats,
 519                                                mlxsw_sp_qdisc->prio_bitmap,
 520                                                &stats_base->tx_packets,
 521                                                &stats_base->tx_bytes);
 522         red_base->prob_drop = xstats->wred_drop[tclass_num];
 523         red_base->pdrop = mlxsw_sp_xstats_tail_drop(xstats, tclass_num);
 524
 525         stats_base->overlimits = red_base->prob_drop + red_base->prob_mark;
 526         stats_base->drops = red_base->prob_drop + red_base->pdrop;
 527
 528         stats_base->backlog = 0;
 529 }
 530
 531 static int
 532 mlxsw_sp_qdisc_red_destroy(struct mlxsw_sp_port *mlxsw_sp_port,
 533                            struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
 534 {
 535         return mlxsw_sp_tclass_congestion_disable(mlxsw_sp_port,
 536                                                   mlxsw_sp_qdisc->tclass_num);
 537 }
 538
 539 static int
 540 mlxsw_sp_qdisc_red_check_params(struct mlxsw_sp_port *mlxsw_sp_port,
 541                                 void *params)
 542 {
 543         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 544         struct tc_red_qopt_offload_params *p = params;
 545
 546         if (p->min > p->max) {
 547                 dev_err(mlxsw_sp->bus_info->dev,
 548                         "spectrum: RED: min %u is bigger then max %u\n", p->min,
 549                         p->max);
 550                 return -EINVAL;
 551         }
 552         if (p->max > MLXSW_CORE_RES_GET(mlxsw_sp->core,
 553                                         GUARANTEED_SHARED_BUFFER)) {
 554                 dev_err(mlxsw_sp->bus_info->dev,
 555                         "spectrum: RED: max value %u is too big\n", p->max);
 556                 return -EINVAL;
 557         }
 558         if (p->min == 0 || p->max == 0) {
 559                 dev_err(mlxsw_sp->bus_info->dev,
 560                         "spectrum: RED: 0 value is illegal for min and max\n");
 561                 return -EINVAL;
 562         }
 563         return 0;
 564 }
 565
 566 static int
 567 mlxsw_sp_qdisc_red_replace(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
 568                            struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 569                            void *params)
 570 {
 571         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 572         struct tc_red_qopt_offload_params *p = params;
 573         int tclass_num = mlxsw_sp_qdisc->tclass_num;
 574         u32 min, max;
 575         u64 prob;
 576
 577         /* calculate probability in percentage */
 578         prob = p->probability;
 579         prob *= 100;
 580         prob = DIV_ROUND_UP(prob, 1 << 16);
 581         prob = DIV_ROUND_UP(prob, 1 << 16);
 582         min = mlxsw_sp_bytes_cells(mlxsw_sp, p->min);
 583         max = mlxsw_sp_bytes_cells(mlxsw_sp, p->max);
 584         return mlxsw_sp_tclass_congestion_enable(mlxsw_sp_port, tclass_num,
 585                                                  min, max, prob,
 586                                                  !p->is_nodrop, p->is_ecn);
 587 }
 588
 589 static void
 590 mlxsw_sp_qdisc_leaf_unoffload(struct mlxsw_sp_port *mlxsw_sp_port,
 591                               struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 592                               struct gnet_stats_queue *qstats)
 593 {
 594         u64 backlog;
 595
 596         backlog = mlxsw_sp_cells_bytes(mlxsw_sp_port->mlxsw_sp,
 597                                        mlxsw_sp_qdisc->stats_base.backlog);
 598         qstats->backlog -= backlog;
 599         mlxsw_sp_qdisc->stats_base.backlog = 0;
 600 }
 601
 602 static void
 603 mlxsw_sp_qdisc_red_unoffload(struct mlxsw_sp_port *mlxsw_sp_port,
 604                              struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 605                              void *params)
 606 {
 607         struct tc_red_qopt_offload_params *p = params;
 608
 609         mlxsw_sp_qdisc_leaf_unoffload(mlxsw_sp_port, mlxsw_sp_qdisc, p->qstats);
 610 }
 611
 612 static int
 613 mlxsw_sp_qdisc_get_red_xstats(struct mlxsw_sp_port *mlxsw_sp_port,
 614                               struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 615                               void *xstats_ptr)
 616 {
 617         struct red_stats *xstats_base = &mlxsw_sp_qdisc->xstats_base.red;
 618         int tclass_num = mlxsw_sp_qdisc->tclass_num;
 619         struct mlxsw_sp_port_xstats *xstats;
 620         struct red_stats *res = xstats_ptr;
 621         int early_drops, pdrops;
 622
 623         xstats = &mlxsw_sp_port->periodic_hw_stats.xstats;
 624
 625         early_drops = xstats->wred_drop[tclass_num] - xstats_base->prob_drop;
 626         pdrops = mlxsw_sp_xstats_tail_drop(xstats, tclass_num) -
 627                  xstats_base->pdrop;
 628
 629         res->pdrop += pdrops;
 630         res->prob_drop += early_drops;
 631
 632         xstats_base->pdrop += pdrops;
 633         xstats_base->prob_drop += early_drops;
 634         return 0;
 635 }
 636
 637 static int
 638 mlxsw_sp_qdisc_get_red_stats(struct mlxsw_sp_port *mlxsw_sp_port,
 639                              struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 640                              struct tc_qopt_offload_stats *stats_ptr)
 641 {
 642         int tclass_num = mlxsw_sp_qdisc->tclass_num;
 643         struct mlxsw_sp_qdisc_stats *stats_base;
 644         struct mlxsw_sp_port_xstats *xstats;
 645         u64 overlimits;
 646
 647         xstats = &mlxsw_sp_port->periodic_hw_stats.xstats;
 648         stats_base = &mlxsw_sp_qdisc->stats_base;
 649
 650         mlxsw_sp_qdisc_get_tc_stats(mlxsw_sp_port, mlxsw_sp_qdisc, stats_ptr);
 651         overlimits = xstats->wred_drop[tclass_num] - stats_base->overlimits;
 652
 653         stats_ptr->qstats->overlimits += overlimits;
 654         stats_base->overlimits += overlimits;
 655
 656         return 0;
 657 }
 658
 659 static struct mlxsw_sp_qdisc *
 660 mlxsw_sp_qdisc_leaf_find_class(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 661                                u32 parent)
 662 {
 663         return NULL;
 664 }
 665
 666 #define MLXSW_SP_PORT_DEFAULT_TCLASS 0
 667
 668 static struct mlxsw_sp_qdisc_ops mlxsw_sp_qdisc_ops_red = {
 669         .type = MLXSW_SP_QDISC_RED,
 670         .check_params = mlxsw_sp_qdisc_red_check_params,
 671         .replace = mlxsw_sp_qdisc_red_replace,
 672         .unoffload = mlxsw_sp_qdisc_red_unoffload,
 673         .destroy = mlxsw_sp_qdisc_red_destroy,
 674         .get_stats = mlxsw_sp_qdisc_get_red_stats,
 675         .get_xstats = mlxsw_sp_qdisc_get_red_xstats,
 676         .clean_stats = mlxsw_sp_setup_tc_qdisc_red_clean_stats,
 677         .find_class = mlxsw_sp_qdisc_leaf_find_class,
 678 };
 679
 680 static int __mlxsw_sp_setup_tc_red(struct mlxsw_sp_port *mlxsw_sp_port,
 681                                    struct tc_red_qopt_offload *p)
 682 {
 683         struct mlxsw_sp_qdisc *mlxsw_sp_qdisc;
 684
 685         mlxsw_sp_qdisc = mlxsw_sp_qdisc_find(mlxsw_sp_port, p->parent, false);
 686         if (!mlxsw_sp_qdisc)
 687                 return -EOPNOTSUPP;
 688
 689         if (p->command == TC_RED_REPLACE)
 690                 return mlxsw_sp_qdisc_replace(mlxsw_sp_port, p->handle,
 691                                               mlxsw_sp_qdisc,
 692                                               &mlxsw_sp_qdisc_ops_red,
 693                                               &p->set);
 694
 695         if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, p->handle))
 696                 return -EOPNOTSUPP;
 697
 698         switch (p->command) {
 699         case TC_RED_DESTROY:
 700                 return mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
 701         case TC_RED_XSTATS:
 702                 return mlxsw_sp_qdisc_get_xstats(mlxsw_sp_port, mlxsw_sp_qdisc,
 703                                                  p->xstats);
 704         case TC_RED_STATS:
 705                 return mlxsw_sp_qdisc_get_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
 706                                                 &p->stats);
 707         default:
 708                 return -EOPNOTSUPP;
 709         }
 710 }
 711
 712 int mlxsw_sp_setup_tc_red(struct mlxsw_sp_port *mlxsw_sp_port,
 713                           struct tc_red_qopt_offload *p)
 714 {
 715         int err;
 716
 717         mutex_lock(&mlxsw_sp_port->qdisc->lock);
 718         err = __mlxsw_sp_setup_tc_red(mlxsw_sp_port, p);
 719         mutex_unlock(&mlxsw_sp_port->qdisc->lock);
 720
 721         return err;
 722 }
 723
 724 static void
 725 mlxsw_sp_setup_tc_qdisc_leaf_clean_stats(struct mlxsw_sp_port *mlxsw_sp_port,
 726                                          struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
 727 {
 728         u64 backlog_cells = 0;
 729         u64 tx_packets = 0;
 730         u64 tx_bytes = 0;
 731         u64 drops = 0;
 732
 733         mlxsw_sp_qdisc_collect_tc_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
 734                                         &tx_bytes, &tx_packets,
 735                                         &drops, &backlog_cells);
 736
 737         mlxsw_sp_qdisc->stats_base.tx_packets = tx_packets;
 738         mlxsw_sp_qdisc->stats_base.tx_bytes = tx_bytes;
 739         mlxsw_sp_qdisc->stats_base.drops = drops;
 740         mlxsw_sp_qdisc->stats_base.backlog = 0;
 741 }
 742
 743 static int
 744 mlxsw_sp_qdisc_tbf_destroy(struct mlxsw_sp_port *mlxsw_sp_port,
 745                            struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
 746 {
 747         return mlxsw_sp_port_ets_maxrate_set(mlxsw_sp_port,
 748                                              MLXSW_REG_QEEC_HR_SUBGROUP,
 749                                              mlxsw_sp_qdisc->tclass_num, 0,
 750                                              MLXSW_REG_QEEC_MAS_DIS, 0);
 751 }
 752
 753 static int
 754 mlxsw_sp_qdisc_tbf_bs(struct mlxsw_sp_port *mlxsw_sp_port,
 755                       u32 max_size, u8 *p_burst_size)
 756 {
 757         /* TBF burst size is configured in bytes. The ASIC burst size value is
 758          * ((2 ^ bs) * 512 bits. Convert the TBF bytes to 512-bit units.
 759          */
 760         u32 bs512 = max_size / 64;
 761         u8 bs = fls(bs512);
 762
 763         if (!bs)
 764                 return -EINVAL;
 765         --bs;
 766
 767         /* Demand a power of two. */
 768         if ((1 << bs) != bs512)
 769                 return -EINVAL;
 770
 771         if (bs < mlxsw_sp_port->mlxsw_sp->lowest_shaper_bs ||
 772             bs > MLXSW_REG_QEEC_HIGHEST_SHAPER_BS)
 773                 return -EINVAL;
 774
 775         *p_burst_size = bs;
 776         return 0;
 777 }
 778
 779 static u32
 780 mlxsw_sp_qdisc_tbf_max_size(u8 bs)
 781 {
 782         return (1U << bs) * 64;
 783 }
 784
 785 static u64
 786 mlxsw_sp_qdisc_tbf_rate_kbps(struct tc_tbf_qopt_offload_replace_params *p)
 787 {
 788         /* TBF interface is in bytes/s, whereas Spectrum ASIC is configured in
 789          * Kbits/s.
 790          */
 791         return div_u64(p->rate.rate_bytes_ps, 1000) * 8;
 792 }
 793
 794 static int
 795 mlxsw_sp_qdisc_tbf_check_params(struct mlxsw_sp_port *mlxsw_sp_port,
 796                                 void *params)
 797 {
 798         struct tc_tbf_qopt_offload_replace_params *p = params;
 799         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 800         u64 rate_kbps = mlxsw_sp_qdisc_tbf_rate_kbps(p);
 801         u8 burst_size;
 802         int err;
 803
 804         if (rate_kbps >= MLXSW_REG_QEEC_MAS_DIS) {
 805                 dev_err(mlxsw_sp_port->mlxsw_sp->bus_info->dev,
 806                         "spectrum: TBF: rate of %lluKbps must be below %u\n",
 807                         rate_kbps, MLXSW_REG_QEEC_MAS_DIS);
 808                 return -EINVAL;
 809         }
 810
 811         err = mlxsw_sp_qdisc_tbf_bs(mlxsw_sp_port, p->max_size, &burst_size);
 812         if (err) {
 813                 u8 highest_shaper_bs = MLXSW_REG_QEEC_HIGHEST_SHAPER_BS;
 814
 815                 dev_err(mlxsw_sp->bus_info->dev,
 816                         "spectrum: TBF: invalid burst size of %u, must be a power of two between %u and %u",
 817                         p->max_size,
 818                         mlxsw_sp_qdisc_tbf_max_size(mlxsw_sp->lowest_shaper_bs),
 819                         mlxsw_sp_qdisc_tbf_max_size(highest_shaper_bs));
 820                 return -EINVAL;
 821         }
 822
 823         return 0;
 824 }
 825
 826 static int
 827 mlxsw_sp_qdisc_tbf_replace(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
 828                            struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 829                            void *params)
 830 {
 831         struct tc_tbf_qopt_offload_replace_params *p = params;
 832         u64 rate_kbps = mlxsw_sp_qdisc_tbf_rate_kbps(p);
 833         u8 burst_size;
 834         int err;
 835
 836         err = mlxsw_sp_qdisc_tbf_bs(mlxsw_sp_port, p->max_size, &burst_size);
 837         if (WARN_ON_ONCE(err))
 838                 /* check_params above was supposed to reject this value. */
 839                 return -EINVAL;
 840
 841         /* Configure subgroup shaper, so that both UC and MC traffic is subject
 842          * to shaping. That is unlike RED, however UC queue lengths are going to
 843          * be different than MC ones due to different pool and quota
 844          * configurations, so the configuration is not applicable. For shaper on
 845          * the other hand, subjecting the overall stream to the configured
 846          * shaper makes sense. Also note that that is what we do for
 847          * ieee_setmaxrate().
 848          */
 849         return mlxsw_sp_port_ets_maxrate_set(mlxsw_sp_port,
 850                                              MLXSW_REG_QEEC_HR_SUBGROUP,
 851                                              mlxsw_sp_qdisc->tclass_num, 0,
 852                                              rate_kbps, burst_size);
 853 }
 854
 855 static void
 856 mlxsw_sp_qdisc_tbf_unoffload(struct mlxsw_sp_port *mlxsw_sp_port,
 857                              struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 858                              void *params)
 859 {
 860         struct tc_tbf_qopt_offload_replace_params *p = params;
 861
 862         mlxsw_sp_qdisc_leaf_unoffload(mlxsw_sp_port, mlxsw_sp_qdisc, p->qstats);
 863 }
 864
 865 static int
 866 mlxsw_sp_qdisc_get_tbf_stats(struct mlxsw_sp_port *mlxsw_sp_port,
 867                              struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 868                              struct tc_qopt_offload_stats *stats_ptr)
 869 {
 870         mlxsw_sp_qdisc_get_tc_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
 871                                     stats_ptr);
 872         return 0;
 873 }
 874
 875 static struct mlxsw_sp_qdisc_ops mlxsw_sp_qdisc_ops_tbf = {
 876         .type = MLXSW_SP_QDISC_TBF,
 877         .check_params = mlxsw_sp_qdisc_tbf_check_params,
 878         .replace = mlxsw_sp_qdisc_tbf_replace,
 879         .unoffload = mlxsw_sp_qdisc_tbf_unoffload,
 880         .destroy = mlxsw_sp_qdisc_tbf_destroy,
 881         .get_stats = mlxsw_sp_qdisc_get_tbf_stats,
 882         .clean_stats = mlxsw_sp_setup_tc_qdisc_leaf_clean_stats,
 883         .find_class = mlxsw_sp_qdisc_leaf_find_class,
 884 };
 885
 886 static int __mlxsw_sp_setup_tc_tbf(struct mlxsw_sp_port *mlxsw_sp_port,
 887                                    struct tc_tbf_qopt_offload *p)
 888 {
 889         struct mlxsw_sp_qdisc *mlxsw_sp_qdisc;
 890
 891         mlxsw_sp_qdisc = mlxsw_sp_qdisc_find(mlxsw_sp_port, p->parent, false);
 892         if (!mlxsw_sp_qdisc)
 893                 return -EOPNOTSUPP;
 894
 895         if (p->command == TC_TBF_REPLACE)
 896                 return mlxsw_sp_qdisc_replace(mlxsw_sp_port, p->handle,
 897                                               mlxsw_sp_qdisc,
 898                                               &mlxsw_sp_qdisc_ops_tbf,
 899                                               &p->replace_params);
 900
 901         if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, p->handle))
 902                 return -EOPNOTSUPP;
 903
 904         switch (p->command) {
 905         case TC_TBF_DESTROY:
 906                 return mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
 907         case TC_TBF_STATS:
 908                 return mlxsw_sp_qdisc_get_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
 909                                                 &p->stats);
 910         default:
 911                 return -EOPNOTSUPP;
 912         }
 913 }
 914
 915 int mlxsw_sp_setup_tc_tbf(struct mlxsw_sp_port *mlxsw_sp_port,
 916                           struct tc_tbf_qopt_offload *p)
 917 {
 918         int err;
 919
 920         mutex_lock(&mlxsw_sp_port->qdisc->lock);
 921         err = __mlxsw_sp_setup_tc_tbf(mlxsw_sp_port, p);
 922         mutex_unlock(&mlxsw_sp_port->qdisc->lock);
 923
 924         return err;
 925 }
 926
 927 static int
 928 mlxsw_sp_qdisc_fifo_check_params(struct mlxsw_sp_port *mlxsw_sp_port,
 929                                  void *params)
 930 {
 931         return 0;
 932 }
 933
 934 static int
 935 mlxsw_sp_qdisc_fifo_replace(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
 936                             struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 937                             void *params)
 938 {
 939         return 0;
 940 }
 941
 942 static int
 943 mlxsw_sp_qdisc_get_fifo_stats(struct mlxsw_sp_port *mlxsw_sp_port,
 944                               struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
 945                               struct tc_qopt_offload_stats *stats_ptr)
 946 {
 947         mlxsw_sp_qdisc_get_tc_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
 948                                     stats_ptr);
 949         return 0;
 950 }
 951
 952 static struct mlxsw_sp_qdisc_ops mlxsw_sp_qdisc_ops_fifo = {
 953         .type = MLXSW_SP_QDISC_FIFO,
 954         .check_params = mlxsw_sp_qdisc_fifo_check_params,
 955         .replace = mlxsw_sp_qdisc_fifo_replace,
 956         .get_stats = mlxsw_sp_qdisc_get_fifo_stats,
 957         .clean_stats = mlxsw_sp_setup_tc_qdisc_leaf_clean_stats,
 958 };
 959
 960 static int __mlxsw_sp_setup_tc_fifo(struct mlxsw_sp_port *mlxsw_sp_port,
 961                                     struct tc_fifo_qopt_offload *p)
 962 {
 963         struct mlxsw_sp_qdisc_state *qdisc_state = mlxsw_sp_port->qdisc;
 964         struct mlxsw_sp_qdisc *mlxsw_sp_qdisc;
 965         unsigned int band;
 966         u32 parent_handle;
 967
 968         mlxsw_sp_qdisc = mlxsw_sp_qdisc_find(mlxsw_sp_port, p->parent, false);
 969         if (!mlxsw_sp_qdisc && p->handle == TC_H_UNSPEC) {
 970                 parent_handle = TC_H_MAJ(p->parent);
 971                 if (parent_handle != qdisc_state->future_handle) {
 972                         /* This notifications is for a different Qdisc than
 973                          * previously. Wipe the future cache.
 974                          */
 975                         memset(qdisc_state->future_fifos, 0,
 976                                sizeof(qdisc_state->future_fifos));
 977                         qdisc_state->future_handle = parent_handle;
 978                 }
 979
 980                 band = TC_H_MIN(p->parent) - 1;
 981                 if (band < IEEE_8021QAZ_MAX_TCS) {
 982                         if (p->command == TC_FIFO_REPLACE)
 983                                 qdisc_state->future_fifos[band] = true;
 984                         else if (p->command == TC_FIFO_DESTROY)
 985                                 qdisc_state->future_fifos[band] = false;
 986                 }
 987         }
 988         if (!mlxsw_sp_qdisc)
 989                 return -EOPNOTSUPP;
 990
 991         if (p->command == TC_FIFO_REPLACE) {
 992                 return mlxsw_sp_qdisc_replace(mlxsw_sp_port, p->handle,
 993                                               mlxsw_sp_qdisc,
 994                                               &mlxsw_sp_qdisc_ops_fifo, NULL);
 995         }
 996
 997         if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, p->handle))
 998                 return -EOPNOTSUPP;
 999
1000         switch (p->command) {
1001         case TC_FIFO_DESTROY:
1002                 return mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
1003         case TC_FIFO_STATS:
1004                 return mlxsw_sp_qdisc_get_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
1005                                                 &p->stats);
1006         case TC_FIFO_REPLACE: /* Handled above. */
1007                 break;
1008         }
1009
1010         return -EOPNOTSUPP;
1011 }
1012
1013 int mlxsw_sp_setup_tc_fifo(struct mlxsw_sp_port *mlxsw_sp_port,
1014                            struct tc_fifo_qopt_offload *p)
1015 {
1016         int err;
1017
1018         mutex_lock(&mlxsw_sp_port->qdisc->lock);
1019         err = __mlxsw_sp_setup_tc_fifo(mlxsw_sp_port, p);
1020         mutex_unlock(&mlxsw_sp_port->qdisc->lock);
1021
1022         return err;
1023 }
1024
1025 static int __mlxsw_sp_qdisc_ets_destroy(struct mlxsw_sp_port *mlxsw_sp_port,
1026                                         struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
1027 {
1028         int i;
1029
1030         for (i = 0; i < mlxsw_sp_qdisc->num_classes; i++) {
1031                 mlxsw_sp_port_prio_tc_set(mlxsw_sp_port, i,
1032                                           MLXSW_SP_PORT_DEFAULT_TCLASS);
1033                 mlxsw_sp_port_ets_set(mlxsw_sp_port,
1034                                       MLXSW_REG_QEEC_HR_SUBGROUP,
1035                                       i, 0, false, 0);
1036                 mlxsw_sp_qdisc_destroy(mlxsw_sp_port,
1037                                        &mlxsw_sp_qdisc->qdiscs[i]);
1038                 mlxsw_sp_qdisc->qdiscs[i].prio_bitmap = 0;
1039         }
1040
1041         return 0;
1042 }
1043
1044 static int
1045 mlxsw_sp_qdisc_prio_destroy(struct mlxsw_sp_port *mlxsw_sp_port,
1046                             struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
1047 {
1048         return __mlxsw_sp_qdisc_ets_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
1049 }
1050
1051 static int
1052 __mlxsw_sp_qdisc_ets_check_params(unsigned int nbands)
1053 {
1054         if (nbands > IEEE_8021QAZ_MAX_TCS)
1055                 return -EOPNOTSUPP;
1056
1057         return 0;
1058 }
1059
1060 static int
1061 mlxsw_sp_qdisc_prio_check_params(struct mlxsw_sp_port *mlxsw_sp_port,
1062                                  void *params)
1063 {
1064         struct tc_prio_qopt_offload_params *p = params;
1065
1066         return __mlxsw_sp_qdisc_ets_check_params(p->bands);
1067 }
1068
1069 static int
1070 __mlxsw_sp_qdisc_ets_replace(struct mlxsw_sp_port *mlxsw_sp_port,
1071                              struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1072                              u32 handle, unsigned int nbands,
1073                              const unsigned int *quanta,
1074                              const unsigned int *weights,
1075                              const u8 *priomap)
1076 {
1077         struct mlxsw_sp_qdisc_state *qdisc_state = mlxsw_sp_port->qdisc;
1078         struct mlxsw_sp_qdisc *child_qdisc;
1079         int tclass, i, band, backlog;
1080         u8 old_priomap;
1081         int err;
1082
1083         for (band = 0; band < nbands; band++) {
1084                 tclass = MLXSW_SP_PRIO_BAND_TO_TCLASS(band);
1085                 child_qdisc = &mlxsw_sp_qdisc->qdiscs[band];
1086                 old_priomap = child_qdisc->prio_bitmap;
1087                 child_qdisc->prio_bitmap = 0;
1088
1089                 err = mlxsw_sp_port_ets_set(mlxsw_sp_port,
1090                                             MLXSW_REG_QEEC_HR_SUBGROUP,
1091                                             tclass, 0, !!quanta[band],
1092                                             weights[band]);
1093                 if (err)
1094                         return err;
1095
1096                 for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
1097                         if (priomap[i] == band) {
1098                                 child_qdisc->prio_bitmap |= BIT(i);
1099                                 if (BIT(i) & old_priomap)
1100                                         continue;
1101                                 err = mlxsw_sp_port_prio_tc_set(mlxsw_sp_port,
1102                                                                 i, tclass);
1103                                 if (err)
1104                                         return err;
1105                         }
1106                 }
1107
1108                 child_qdisc->tclass_num = tclass;
1109
1110                 if (old_priomap != child_qdisc->prio_bitmap &&
1111                     child_qdisc->ops && child_qdisc->ops->clean_stats) {
1112                         backlog = child_qdisc->stats_base.backlog;
1113                         child_qdisc->ops->clean_stats(mlxsw_sp_port,
1114                                                       child_qdisc);
1115                         child_qdisc->stats_base.backlog = backlog;
1116                 }
1117
1118                 if (handle == qdisc_state->future_handle &&
1119                     qdisc_state->future_fifos[band]) {
1120                         err = mlxsw_sp_qdisc_replace(mlxsw_sp_port, TC_H_UNSPEC,
1121                                                      child_qdisc,
1122                                                      &mlxsw_sp_qdisc_ops_fifo,
1123                                                      NULL);
1124                         if (err)
1125                                 return err;
1126                 }
1127         }
1128         for (; band < IEEE_8021QAZ_MAX_TCS; band++) {
1129                 tclass = MLXSW_SP_PRIO_BAND_TO_TCLASS(band);
1130                 child_qdisc = &mlxsw_sp_qdisc->qdiscs[band];
1131                 child_qdisc->prio_bitmap = 0;
1132                 mlxsw_sp_qdisc_destroy(mlxsw_sp_port, child_qdisc);
1133                 mlxsw_sp_port_ets_set(mlxsw_sp_port,
1134                                       MLXSW_REG_QEEC_HR_SUBGROUP,
1135                                       tclass, 0, false, 0);
1136         }
1137
1138         qdisc_state->future_handle = TC_H_UNSPEC;
1139         memset(qdisc_state->future_fifos, 0, sizeof(qdisc_state->future_fifos));
1140         return 0;
1141 }
1142
1143 static int
1144 mlxsw_sp_qdisc_prio_replace(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
1145                             struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1146                             void *params)
1147 {
1148         struct tc_prio_qopt_offload_params *p = params;
1149         unsigned int zeroes[TCQ_ETS_MAX_BANDS] = {0};
1150
1151         return __mlxsw_sp_qdisc_ets_replace(mlxsw_sp_port, mlxsw_sp_qdisc,
1152                                             handle, p->bands, zeroes,
1153                                             zeroes, p->priomap);
1154 }
1155
1156 static void
1157 __mlxsw_sp_qdisc_ets_unoffload(struct mlxsw_sp_port *mlxsw_sp_port,
1158                                struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1159                                struct gnet_stats_queue *qstats)
1160 {
1161         u64 backlog;
1162
1163         backlog = mlxsw_sp_cells_bytes(mlxsw_sp_port->mlxsw_sp,
1164                                        mlxsw_sp_qdisc->stats_base.backlog);
1165         qstats->backlog -= backlog;
1166 }
1167
1168 static void
1169 mlxsw_sp_qdisc_prio_unoffload(struct mlxsw_sp_port *mlxsw_sp_port,
1170                               struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1171                               void *params)
1172 {
1173         struct tc_prio_qopt_offload_params *p = params;
1174
1175         __mlxsw_sp_qdisc_ets_unoffload(mlxsw_sp_port, mlxsw_sp_qdisc,
1176                                        p->qstats);
1177 }
1178
1179 static int
1180 mlxsw_sp_qdisc_get_prio_stats(struct mlxsw_sp_port *mlxsw_sp_port,
1181                               struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1182                               struct tc_qopt_offload_stats *stats_ptr)
1183 {
1184         struct mlxsw_sp_qdisc *tc_qdisc;
1185         u64 tx_packets = 0;
1186         u64 tx_bytes = 0;
1187         u64 backlog = 0;
1188         u64 drops = 0;
1189         int i;
1190
1191         for (i = 0; i < mlxsw_sp_qdisc->num_classes; i++) {
1192                 tc_qdisc = &mlxsw_sp_qdisc->qdiscs[i];
1193                 mlxsw_sp_qdisc_collect_tc_stats(mlxsw_sp_port, tc_qdisc,
1194                                                 &tx_bytes, &tx_packets,
1195                                                 &drops, &backlog);
1196         }
1197
1198         mlxsw_sp_qdisc_update_stats(mlxsw_sp_port->mlxsw_sp, mlxsw_sp_qdisc,
1199                                     tx_bytes, tx_packets, drops, backlog,
1200                                     stats_ptr);
1201         return 0;
1202 }
1203
1204 static void
1205 mlxsw_sp_setup_tc_qdisc_prio_clean_stats(struct mlxsw_sp_port *mlxsw_sp_port,
1206                                          struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
1207 {
1208         struct mlxsw_sp_qdisc_stats *stats_base;
1209         struct mlxsw_sp_port_xstats *xstats;
1210         struct rtnl_link_stats64 *stats;
1211         int i;
1212
1213         xstats = &mlxsw_sp_port->periodic_hw_stats.xstats;
1214         stats = &mlxsw_sp_port->periodic_hw_stats.stats;
1215         stats_base = &mlxsw_sp_qdisc->stats_base;
1216
1217         stats_base->tx_packets = stats->tx_packets;
1218         stats_base->tx_bytes = stats->tx_bytes;
1219
1220         stats_base->drops = 0;
1221         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
1222                 stats_base->drops += mlxsw_sp_xstats_tail_drop(xstats, i);
1223                 stats_base->drops += xstats->wred_drop[i];
1224         }
1225
1226         mlxsw_sp_qdisc->stats_base.backlog = 0;
1227 }
1228
1229 static struct mlxsw_sp_qdisc *
1230 mlxsw_sp_qdisc_prio_find_class(struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1231                                u32 parent)
1232 {
1233         int child_index = TC_H_MIN(parent);
1234         int band = child_index - 1;
1235
1236         if (band < 0 || band >= mlxsw_sp_qdisc->num_classes)
1237                 return NULL;
1238         return &mlxsw_sp_qdisc->qdiscs[band];
1239 }
1240
1241 static struct mlxsw_sp_qdisc_ops mlxsw_sp_qdisc_ops_prio = {
1242         .type = MLXSW_SP_QDISC_PRIO,
1243         .check_params = mlxsw_sp_qdisc_prio_check_params,
1244         .replace = mlxsw_sp_qdisc_prio_replace,
1245         .unoffload = mlxsw_sp_qdisc_prio_unoffload,
1246         .destroy = mlxsw_sp_qdisc_prio_destroy,
1247         .get_stats = mlxsw_sp_qdisc_get_prio_stats,
1248         .clean_stats = mlxsw_sp_setup_tc_qdisc_prio_clean_stats,
1249         .find_class = mlxsw_sp_qdisc_prio_find_class,
1250         .num_classes = IEEE_8021QAZ_MAX_TCS,
1251 };
1252
1253 static int
1254 mlxsw_sp_qdisc_ets_check_params(struct mlxsw_sp_port *mlxsw_sp_port,
1255                                 void *params)
1256 {
1257         struct tc_ets_qopt_offload_replace_params *p = params;
1258
1259         return __mlxsw_sp_qdisc_ets_check_params(p->bands);
1260 }
1261
1262 static int
1263 mlxsw_sp_qdisc_ets_replace(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle,
1264                            struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1265                            void *params)
1266 {
1267         struct tc_ets_qopt_offload_replace_params *p = params;
1268
1269         return __mlxsw_sp_qdisc_ets_replace(mlxsw_sp_port, mlxsw_sp_qdisc,
1270                                             handle, p->bands, p->quanta,
1271                                             p->weights, p->priomap);
1272 }
1273
1274 static void
1275 mlxsw_sp_qdisc_ets_unoffload(struct mlxsw_sp_port *mlxsw_sp_port,
1276                              struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1277                              void *params)
1278 {
1279         struct tc_ets_qopt_offload_replace_params *p = params;
1280
1281         __mlxsw_sp_qdisc_ets_unoffload(mlxsw_sp_port, mlxsw_sp_qdisc,
1282                                        p->qstats);
1283 }
1284
1285 static int
1286 mlxsw_sp_qdisc_ets_destroy(struct mlxsw_sp_port *mlxsw_sp_port,
1287                            struct mlxsw_sp_qdisc *mlxsw_sp_qdisc)
1288 {
1289         return __mlxsw_sp_qdisc_ets_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
1290 }
1291
1292 static struct mlxsw_sp_qdisc_ops mlxsw_sp_qdisc_ops_ets = {
1293         .type = MLXSW_SP_QDISC_ETS,
1294         .check_params = mlxsw_sp_qdisc_ets_check_params,
1295         .replace = mlxsw_sp_qdisc_ets_replace,
1296         .unoffload = mlxsw_sp_qdisc_ets_unoffload,
1297         .destroy = mlxsw_sp_qdisc_ets_destroy,
1298         .get_stats = mlxsw_sp_qdisc_get_prio_stats,
1299         .clean_stats = mlxsw_sp_setup_tc_qdisc_prio_clean_stats,
1300         .find_class = mlxsw_sp_qdisc_prio_find_class,
1301         .num_classes = IEEE_8021QAZ_MAX_TCS,
1302 };
1303
1304 /* Linux allows linking of Qdiscs to arbitrary classes (so long as the resulting
1305  * graph is free of cycles). These operations do not change the parent handle
1306  * though, which means it can be incomplete (if there is more than one class
1307  * where the Qdisc in question is grafted) or outright wrong (if the Qdisc was
1308  * linked to a different class and then removed from the original class).
1309  *
1310  * E.g. consider this sequence of operations:
1311  *
1312  *  # tc qdisc add dev swp1 root handle 1: prio
1313  *  # tc qdisc add dev swp1 parent 1:3 handle 13: red limit 1000000 avpkt 10000
1314  *  RED: set bandwidth to 10Mbit
1315  *  # tc qdisc link dev swp1 handle 13: parent 1:2
1316  *
1317  * At this point, both 1:2 and 1:3 have the same RED Qdisc instance as their
1318  * child. But RED will still only claim that 1:3 is its parent. If it's removed
1319  * from that band, its only parent will be 1:2, but it will continue to claim
1320  * that it is in fact 1:3.
1321  *
1322  * The notification for child Qdisc replace (e.g. TC_RED_REPLACE) comes before
1323  * the notification for parent graft (e.g. TC_PRIO_GRAFT). We take the replace
1324  * notification to offload the child Qdisc, based on its parent handle, and use
1325  * the graft operation to validate that the class where the child is actually
1326  * grafted corresponds to the parent handle. If the two don't match, we
1327  * unoffload the child.
1328  */
1329 static int
1330 __mlxsw_sp_qdisc_ets_graft(struct mlxsw_sp_port *mlxsw_sp_port,
1331                            struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1332                            u8 band, u32 child_handle)
1333 {
1334         struct mlxsw_sp_qdisc *old_qdisc;
1335         u32 parent;
1336
1337         if (band < mlxsw_sp_qdisc->num_classes &&
1338             mlxsw_sp_qdisc->qdiscs[band].handle == child_handle)
1339                 return 0;
1340
1341         if (!child_handle) {
1342                 /* This is an invisible FIFO replacing the original Qdisc.
1343                  * Ignore it--the original Qdisc's destroy will follow.
1344                  */
1345                 return 0;
1346         }
1347
1348         /* See if the grafted qdisc is already offloaded on any tclass. If so,
1349          * unoffload it.
1350          */
1351         old_qdisc = mlxsw_sp_qdisc_find_by_handle(mlxsw_sp_port,
1352                                                   child_handle);
1353         if (old_qdisc)
1354                 mlxsw_sp_qdisc_destroy(mlxsw_sp_port, old_qdisc);
1355
1356         parent = TC_H_MAKE(mlxsw_sp_qdisc->handle, band + 1);
1357         mlxsw_sp_qdisc = mlxsw_sp_qdisc->ops->find_class(mlxsw_sp_qdisc,
1358                                                          parent);
1359         if (!WARN_ON(!mlxsw_sp_qdisc))
1360                 mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
1361
1362         return -EOPNOTSUPP;
1363 }
1364
1365 static int
1366 mlxsw_sp_qdisc_prio_graft(struct mlxsw_sp_port *mlxsw_sp_port,
1367                           struct mlxsw_sp_qdisc *mlxsw_sp_qdisc,
1368                           struct tc_prio_qopt_offload_graft_params *p)
1369 {
1370         return __mlxsw_sp_qdisc_ets_graft(mlxsw_sp_port, mlxsw_sp_qdisc,
1371                                           p->band, p->child_handle);
1372 }
1373
1374 static int __mlxsw_sp_setup_tc_prio(struct mlxsw_sp_port *mlxsw_sp_port,
1375                                     struct tc_prio_qopt_offload *p)
1376 {
1377         struct mlxsw_sp_qdisc *mlxsw_sp_qdisc;
1378
1379         mlxsw_sp_qdisc = mlxsw_sp_qdisc_find(mlxsw_sp_port, p->parent, true);
1380         if (!mlxsw_sp_qdisc)
1381                 return -EOPNOTSUPP;
1382
1383         if (p->command == TC_PRIO_REPLACE)
1384                 return mlxsw_sp_qdisc_replace(mlxsw_sp_port, p->handle,
1385                                               mlxsw_sp_qdisc,
1386                                               &mlxsw_sp_qdisc_ops_prio,
1387                                               &p->replace_params);
1388
1389         if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, p->handle))
1390                 return -EOPNOTSUPP;
1391
1392         switch (p->command) {
1393         case TC_PRIO_DESTROY:
1394                 return mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
1395         case TC_PRIO_STATS:
1396                 return mlxsw_sp_qdisc_get_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
1397                                                 &p->stats);
1398         case TC_PRIO_GRAFT:
1399                 return mlxsw_sp_qdisc_prio_graft(mlxsw_sp_port, mlxsw_sp_qdisc,
1400                                                  &p->graft_params);
1401         default:
1402                 return -EOPNOTSUPP;
1403         }
1404 }
1405
1406 int mlxsw_sp_setup_tc_prio(struct mlxsw_sp_port *mlxsw_sp_port,
1407                            struct tc_prio_qopt_offload *p)
1408 {
1409         int err;
1410
1411         mutex_lock(&mlxsw_sp_port->qdisc->lock);
1412         err = __mlxsw_sp_setup_tc_prio(mlxsw_sp_port, p);
1413         mutex_unlock(&mlxsw_sp_port->qdisc->lock);
1414
1415         return err;
1416 }
1417
1418 static int __mlxsw_sp_setup_tc_ets(struct mlxsw_sp_port *mlxsw_sp_port,
1419                                    struct tc_ets_qopt_offload *p)
1420 {
1421         struct mlxsw_sp_qdisc *mlxsw_sp_qdisc;
1422
1423         mlxsw_sp_qdisc = mlxsw_sp_qdisc_find(mlxsw_sp_port, p->parent, true);
1424         if (!mlxsw_sp_qdisc)
1425                 return -EOPNOTSUPP;
1426
1427         if (p->command == TC_ETS_REPLACE)
1428                 return mlxsw_sp_qdisc_replace(mlxsw_sp_port, p->handle,
1429                                               mlxsw_sp_qdisc,
1430                                               &mlxsw_sp_qdisc_ops_ets,
1431                                               &p->replace_params);
1432
1433         if (!mlxsw_sp_qdisc_compare(mlxsw_sp_qdisc, p->handle))
1434                 return -EOPNOTSUPP;
1435
1436         switch (p->command) {
1437         case TC_ETS_DESTROY:
1438                 return mlxsw_sp_qdisc_destroy(mlxsw_sp_port, mlxsw_sp_qdisc);
1439         case TC_ETS_STATS:
1440                 return mlxsw_sp_qdisc_get_stats(mlxsw_sp_port, mlxsw_sp_qdisc,
1441                                                 &p->stats);
1442         case TC_ETS_GRAFT:
1443                 return __mlxsw_sp_qdisc_ets_graft(mlxsw_sp_port, mlxsw_sp_qdisc,
1444                                                   p->graft_params.band,
1445                                                   p->graft_params.child_handle);
1446         default:
1447                 return -EOPNOTSUPP;
1448         }
1449 }
1450
1451 int mlxsw_sp_setup_tc_ets(struct mlxsw_sp_port *mlxsw_sp_port,
1452                           struct tc_ets_qopt_offload *p)
1453 {
1454         int err;
1455
1456         mutex_lock(&mlxsw_sp_port->qdisc->lock);
1457         err = __mlxsw_sp_setup_tc_ets(mlxsw_sp_port, p);
1458         mutex_unlock(&mlxsw_sp_port->qdisc->lock);
1459
1460         return err;
1461 }
1462
1463 struct mlxsw_sp_qevent_block {
1464         struct list_head binding_list;
1465         struct list_head mall_entry_list;
1466         struct mlxsw_sp *mlxsw_sp;
1467 };
1468
1469 struct mlxsw_sp_qevent_binding {
1470         struct list_head list;
1471         struct mlxsw_sp_port *mlxsw_sp_port;
1472         u32 handle;
1473         int tclass_num;
1474         enum mlxsw_sp_span_trigger span_trigger;
1475 };
1476
1477 static LIST_HEAD(mlxsw_sp_qevent_block_cb_list);
1478
1479 static int mlxsw_sp_qevent_span_configure(struct mlxsw_sp *mlxsw_sp,
1480                                           struct mlxsw_sp_mall_entry *mall_entry,
1481                                           struct mlxsw_sp_qevent_binding *qevent_binding,
1482                                           const struct mlxsw_sp_span_agent_parms *agent_parms,
1483                                           int *p_span_id)
1484 {
1485         struct mlxsw_sp_port *mlxsw_sp_port = qevent_binding->mlxsw_sp_port;
1486         struct mlxsw_sp_span_trigger_parms trigger_parms = {};
1487         int span_id;
1488         int err;
1489
1490         err = mlxsw_sp_span_agent_get(mlxsw_sp, &span_id, agent_parms);
1491         if (err)
1492                 return err;
1493
1494         err = mlxsw_sp_span_analyzed_port_get(mlxsw_sp_port, true);
1495         if (err)
1496                 goto err_analyzed_port_get;
1497
1498         trigger_parms.span_id = span_id;
1499         trigger_parms.probability_rate = 1;
1500         err = mlxsw_sp_span_agent_bind(mlxsw_sp, qevent_binding->span_trigger, mlxsw_sp_port,
1501                                        &trigger_parms);
1502         if (err)
1503                 goto err_agent_bind;
1504
1505         err = mlxsw_sp_span_trigger_enable(mlxsw_sp_port, qevent_binding->span_trigger,
1506                                            qevent_binding->tclass_num);
1507         if (err)
1508                 goto err_trigger_enable;
1509
1510         *p_span_id = span_id;
1511         return 0;
1512
1513 err_trigger_enable:
1514         mlxsw_sp_span_agent_unbind(mlxsw_sp, qevent_binding->span_trigger, mlxsw_sp_port,
1515                                    &trigger_parms);
1516 err_agent_bind:
1517         mlxsw_sp_span_analyzed_port_put(mlxsw_sp_port, true);
1518 err_analyzed_port_get:
1519         mlxsw_sp_span_agent_put(mlxsw_sp, span_id);
1520         return err;
1521 }
1522
1523 static void mlxsw_sp_qevent_span_deconfigure(struct mlxsw_sp *mlxsw_sp,
1524                                              struct mlxsw_sp_qevent_binding *qevent_binding,
1525                                              int span_id)
1526 {
1527         struct mlxsw_sp_port *mlxsw_sp_port = qevent_binding->mlxsw_sp_port;
1528         struct mlxsw_sp_span_trigger_parms trigger_parms = {
1529                 .span_id = span_id,
1530         };
1531
1532         mlxsw_sp_span_trigger_disable(mlxsw_sp_port, qevent_binding->span_trigger,
1533                                       qevent_binding->tclass_num);
1534         mlxsw_sp_span_agent_unbind(mlxsw_sp, qevent_binding->span_trigger, mlxsw_sp_port,
1535                                    &trigger_parms);
1536         mlxsw_sp_span_analyzed_port_put(mlxsw_sp_port, true);
1537         mlxsw_sp_span_agent_put(mlxsw_sp, span_id);
1538 }
1539
1540 static int mlxsw_sp_qevent_mirror_configure(struct mlxsw_sp *mlxsw_sp,
1541                                             struct mlxsw_sp_mall_entry *mall_entry,
1542                                             struct mlxsw_sp_qevent_binding *qevent_binding)
1543 {
1544         struct mlxsw_sp_span_agent_parms agent_parms = {
1545                 .to_dev = mall_entry->mirror.to_dev,
1546         };
1547
1548         return mlxsw_sp_qevent_span_configure(mlxsw_sp, mall_entry, qevent_binding,
1549                                               &agent_parms, &mall_entry->mirror.span_id);
1550 }
1551
1552 static void mlxsw_sp_qevent_mirror_deconfigure(struct mlxsw_sp *mlxsw_sp,
1553                                                struct mlxsw_sp_mall_entry *mall_entry,
1554                                                struct mlxsw_sp_qevent_binding *qevent_binding)
1555 {
1556         mlxsw_sp_qevent_span_deconfigure(mlxsw_sp, qevent_binding, mall_entry->mirror.span_id);
1557 }
1558
1559 static int mlxsw_sp_qevent_trap_configure(struct mlxsw_sp *mlxsw_sp,
1560                                           struct mlxsw_sp_mall_entry *mall_entry,
1561                                           struct mlxsw_sp_qevent_binding *qevent_binding)
1562 {
1563         struct mlxsw_sp_span_agent_parms agent_parms = {
1564                 .session_id = MLXSW_SP_SPAN_SESSION_ID_BUFFER,
1565         };
1566         int err;
1567
1568         err = mlxsw_sp_trap_group_policer_hw_id_get(mlxsw_sp,
1569                                                     DEVLINK_TRAP_GROUP_GENERIC_ID_BUFFER_DROPS,
1570                                                     &agent_parms.policer_enable,
1571                                                     &agent_parms.policer_id);
1572         if (err)
1573                 return err;
1574
1575         return mlxsw_sp_qevent_span_configure(mlxsw_sp, mall_entry, qevent_binding,
1576                                               &agent_parms, &mall_entry->trap.span_id);
1577 }
1578
1579 static void mlxsw_sp_qevent_trap_deconfigure(struct mlxsw_sp *mlxsw_sp,
1580                                              struct mlxsw_sp_mall_entry *mall_entry,
1581                                              struct mlxsw_sp_qevent_binding *qevent_binding)
1582 {
1583         mlxsw_sp_qevent_span_deconfigure(mlxsw_sp, qevent_binding, mall_entry->trap.span_id);
1584 }
1585
1586 static int mlxsw_sp_qevent_entry_configure(struct mlxsw_sp *mlxsw_sp,
1587                                            struct mlxsw_sp_mall_entry *mall_entry,
1588                                            struct mlxsw_sp_qevent_binding *qevent_binding)
1589 {
1590         switch (mall_entry->type) {
1591         case MLXSW_SP_MALL_ACTION_TYPE_MIRROR:
1592                 return mlxsw_sp_qevent_mirror_configure(mlxsw_sp, mall_entry, qevent_binding);
1593         case MLXSW_SP_MALL_ACTION_TYPE_TRAP:
1594                 return mlxsw_sp_qevent_trap_configure(mlxsw_sp, mall_entry, qevent_binding);
1595         default:
1596                 /* This should have been validated away. */
1597                 WARN_ON(1);
1598                 return -EOPNOTSUPP;
1599         }
1600 }
1601
1602 static void mlxsw_sp_qevent_entry_deconfigure(struct mlxsw_sp *mlxsw_sp,
1603                                               struct mlxsw_sp_mall_entry *mall_entry,
1604                                               struct mlxsw_sp_qevent_binding *qevent_binding)
1605 {
1606         switch (mall_entry->type) {
1607         case MLXSW_SP_MALL_ACTION_TYPE_MIRROR:
1608                 return mlxsw_sp_qevent_mirror_deconfigure(mlxsw_sp, mall_entry, qevent_binding);
1609         case MLXSW_SP_MALL_ACTION_TYPE_TRAP:
1610                 return mlxsw_sp_qevent_trap_deconfigure(mlxsw_sp, mall_entry, qevent_binding);
1611         default:
1612                 WARN_ON(1);
1613                 return;
1614         }
1615 }
1616
1617 static int mlxsw_sp_qevent_binding_configure(struct mlxsw_sp_qevent_block *qevent_block,
1618                                              struct mlxsw_sp_qevent_binding *qevent_binding)
1619 {
1620         struct mlxsw_sp_mall_entry *mall_entry;
1621         int err;
1622
1623         list_for_each_entry(mall_entry, &qevent_block->mall_entry_list, list) {
1624                 err = mlxsw_sp_qevent_entry_configure(qevent_block->mlxsw_sp, mall_entry,
1625                                                       qevent_binding);
1626                 if (err)
1627                         goto err_entry_configure;
1628         }
1629
1630         return 0;
1631
1632 err_entry_configure:
1633         list_for_each_entry_continue_reverse(mall_entry, &qevent_block->mall_entry_list, list)
1634                 mlxsw_sp_qevent_entry_deconfigure(qevent_block->mlxsw_sp, mall_entry,
1635                                                   qevent_binding);
1636         return err;
1637 }
1638
1639 static void mlxsw_sp_qevent_binding_deconfigure(struct mlxsw_sp_qevent_block *qevent_block,
1640                                                 struct mlxsw_sp_qevent_binding *qevent_binding)
1641 {
1642         struct mlxsw_sp_mall_entry *mall_entry;
1643
1644         list_for_each_entry(mall_entry, &qevent_block->mall_entry_list, list)
1645                 mlxsw_sp_qevent_entry_deconfigure(qevent_block->mlxsw_sp, mall_entry,
1646                                                   qevent_binding);
1647 }
1648
1649 static int mlxsw_sp_qevent_block_configure(struct mlxsw_sp_qevent_block *qevent_block)
1650 {
1651         struct mlxsw_sp_qevent_binding *qevent_binding;
1652         int err;
1653
1654         list_for_each_entry(qevent_binding, &qevent_block->binding_list, list) {
1655                 err = mlxsw_sp_qevent_binding_configure(qevent_block, qevent_binding);
1656                 if (err)
1657                         goto err_binding_configure;
1658         }
1659
1660         return 0;
1661
1662 err_binding_configure:
1663         list_for_each_entry_continue_reverse(qevent_binding, &qevent_block->binding_list, list)
1664                 mlxsw_sp_qevent_binding_deconfigure(qevent_block, qevent_binding);
1665         return err;
1666 }
1667
1668 static void mlxsw_sp_qevent_block_deconfigure(struct mlxsw_sp_qevent_block *qevent_block)
1669 {
1670         struct mlxsw_sp_qevent_binding *qevent_binding;
1671
1672         list_for_each_entry(qevent_binding, &qevent_block->binding_list, list)
1673                 mlxsw_sp_qevent_binding_deconfigure(qevent_block, qevent_binding);
1674 }
1675
1676 static struct mlxsw_sp_mall_entry *
1677 mlxsw_sp_qevent_mall_entry_find(struct mlxsw_sp_qevent_block *block, unsigned long cookie)
1678 {
1679         struct mlxsw_sp_mall_entry *mall_entry;
1680
1681         list_for_each_entry(mall_entry, &block->mall_entry_list, list)
1682                 if (mall_entry->cookie == cookie)
1683                         return mall_entry;
1684
1685         return NULL;
1686 }
1687
1688 static int mlxsw_sp_qevent_mall_replace(struct mlxsw_sp *mlxsw_sp,
1689                                         struct mlxsw_sp_qevent_block *qevent_block,
1690                                         struct tc_cls_matchall_offload *f)
1691 {
1692         struct mlxsw_sp_mall_entry *mall_entry;
1693         struct flow_action_entry *act;
1694         int err;
1695
1696         /* It should not currently be possible to replace a matchall rule. So
1697          * this must be a new rule.
1698          */
1699         if (!list_empty(&qevent_block->mall_entry_list)) {
1700                 NL_SET_ERR_MSG(f->common.extack, "At most one filter supported");
1701                 return -EOPNOTSUPP;
1702         }
1703         if (f->rule->action.num_entries != 1) {
1704                 NL_SET_ERR_MSG(f->common.extack, "Only singular actions supported");
1705                 return -EOPNOTSUPP;
1706         }
1707         if (f->common.chain_index) {
1708                 NL_SET_ERR_MSG(f->common.extack, "Only chain 0 is supported");
1709                 return -EOPNOTSUPP;
1710         }
1711         if (f->common.protocol != htons(ETH_P_ALL)) {
1712                 NL_SET_ERR_MSG(f->common.extack, "Protocol matching not supported");
1713                 return -EOPNOTSUPP;
1714         }
1715
1716         act = &f->rule->action.entries[0];
1717         if (!(act->hw_stats & FLOW_ACTION_HW_STATS_DISABLED)) {
1718                 NL_SET_ERR_MSG(f->common.extack, "HW counters not supported on qevents");
1719                 return -EOPNOTSUPP;
1720         }
1721
1722         mall_entry = kzalloc(sizeof(*mall_entry), GFP_KERNEL);
1723         if (!mall_entry)
1724                 return -ENOMEM;
1725         mall_entry->cookie = f->cookie;
1726
1727         if (act->id == FLOW_ACTION_MIRRED) {
1728                 mall_entry->type = MLXSW_SP_MALL_ACTION_TYPE_MIRROR;
1729                 mall_entry->mirror.to_dev = act->dev;
1730         } else if (act->id == FLOW_ACTION_TRAP) {
1731                 mall_entry->type = MLXSW_SP_MALL_ACTION_TYPE_TRAP;
1732         } else {
1733                 NL_SET_ERR_MSG(f->common.extack, "Unsupported action");
1734                 err = -EOPNOTSUPP;
1735                 goto err_unsupported_action;
1736         }
1737
1738         list_add_tail(&mall_entry->list, &qevent_block->mall_entry_list);
1739
1740         err = mlxsw_sp_qevent_block_configure(qevent_block);
1741         if (err)
1742                 goto err_block_configure;
1743
1744         return 0;
1745
1746 err_block_configure:
1747         list_del(&mall_entry->list);
1748 err_unsupported_action:
1749         kfree(mall_entry);
1750         return err;
1751 }
1752
1753 static void mlxsw_sp_qevent_mall_destroy(struct mlxsw_sp_qevent_block *qevent_block,
1754                                          struct tc_cls_matchall_offload *f)
1755 {
1756         struct mlxsw_sp_mall_entry *mall_entry;
1757
1758         mall_entry = mlxsw_sp_qevent_mall_entry_find(qevent_block, f->cookie);
1759         if (!mall_entry)
1760                 return;
1761
1762         mlxsw_sp_qevent_block_deconfigure(qevent_block);
1763
1764         list_del(&mall_entry->list);
1765         kfree(mall_entry);
1766 }
1767
1768 static int mlxsw_sp_qevent_block_mall_cb(struct mlxsw_sp_qevent_block *qevent_block,
1769                                          struct tc_cls_matchall_offload *f)
1770 {
1771         struct mlxsw_sp *mlxsw_sp = qevent_block->mlxsw_sp;
1772
1773         switch (f->command) {
1774         case TC_CLSMATCHALL_REPLACE:
1775                 return mlxsw_sp_qevent_mall_replace(mlxsw_sp, qevent_block, f);
1776         case TC_CLSMATCHALL_DESTROY:
1777                 mlxsw_sp_qevent_mall_destroy(qevent_block, f);
1778                 return 0;
1779         default:
1780                 return -EOPNOTSUPP;
1781         }
1782 }
1783
1784 static int mlxsw_sp_qevent_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv)
1785 {
1786         struct mlxsw_sp_qevent_block *qevent_block = cb_priv;
1787
1788         switch (type) {
1789         case TC_SETUP_CLSMATCHALL:
1790                 return mlxsw_sp_qevent_block_mall_cb(qevent_block, type_data);
1791         default:
1792                 return -EOPNOTSUPP;
1793         }
1794 }
1795
1796 static struct mlxsw_sp_qevent_block *mlxsw_sp_qevent_block_create(struct mlxsw_sp *mlxsw_sp,
1797                                                                   struct net *net)
1798 {
1799         struct mlxsw_sp_qevent_block *qevent_block;
1800
1801         qevent_block = kzalloc(sizeof(*qevent_block), GFP_KERNEL);
1802         if (!qevent_block)
1803                 return NULL;
1804
1805         INIT_LIST_HEAD(&qevent_block->binding_list);
1806         INIT_LIST_HEAD(&qevent_block->mall_entry_list);
1807         qevent_block->mlxsw_sp = mlxsw_sp;
1808         return qevent_block;
1809 }
1810
1811 static void
1812 mlxsw_sp_qevent_block_destroy(struct mlxsw_sp_qevent_block *qevent_block)
1813 {
1814         WARN_ON(!list_empty(&qevent_block->binding_list));
1815         WARN_ON(!list_empty(&qevent_block->mall_entry_list));
1816         kfree(qevent_block);
1817 }
1818
1819 static void mlxsw_sp_qevent_block_release(void *cb_priv)
1820 {
1821         struct mlxsw_sp_qevent_block *qevent_block = cb_priv;
1822
1823         mlxsw_sp_qevent_block_destroy(qevent_block);
1824 }
1825
1826 static struct mlxsw_sp_qevent_binding *
1827 mlxsw_sp_qevent_binding_create(struct mlxsw_sp_port *mlxsw_sp_port, u32 handle, int tclass_num,
1828                                enum mlxsw_sp_span_trigger span_trigger)
1829 {
1830         struct mlxsw_sp_qevent_binding *binding;
1831
1832         binding = kzalloc(sizeof(*binding), GFP_KERNEL);
1833         if (!binding)
1834                 return ERR_PTR(-ENOMEM);
1835
1836         binding->mlxsw_sp_port = mlxsw_sp_port;
1837         binding->handle = handle;
1838         binding->tclass_num = tclass_num;
1839         binding->span_trigger = span_trigger;
1840         return binding;
1841 }
1842
1843 static void
1844 mlxsw_sp_qevent_binding_destroy(struct mlxsw_sp_qevent_binding *binding)
1845 {
1846         kfree(binding);
1847 }
1848
1849 static struct mlxsw_sp_qevent_binding *
1850 mlxsw_sp_qevent_binding_lookup(struct mlxsw_sp_qevent_block *block,
1851                                struct mlxsw_sp_port *mlxsw_sp_port,
1852                                u32 handle,
1853                                enum mlxsw_sp_span_trigger span_trigger)
1854 {
1855         struct mlxsw_sp_qevent_binding *qevent_binding;
1856
1857         list_for_each_entry(qevent_binding, &block->binding_list, list)
1858                 if (qevent_binding->mlxsw_sp_port == mlxsw_sp_port &&
1859                     qevent_binding->handle == handle &&
1860                     qevent_binding->span_trigger == span_trigger)
1861                         return qevent_binding;
1862         return NULL;
1863 }
1864
1865 static int mlxsw_sp_setup_tc_block_qevent_bind(struct mlxsw_sp_port *mlxsw_sp_port,
1866                                                struct flow_block_offload *f,
1867                                                enum mlxsw_sp_span_trigger span_trigger)
1868 {
1869         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
1870         struct mlxsw_sp_qevent_binding *qevent_binding;
1871         struct mlxsw_sp_qevent_block *qevent_block;
1872         struct flow_block_cb *block_cb;
1873         struct mlxsw_sp_qdisc *qdisc;
1874         bool register_block = false;
1875         int err;
1876
1877         block_cb = flow_block_cb_lookup(f->block, mlxsw_sp_qevent_block_cb, mlxsw_sp);
1878         if (!block_cb) {
1879                 qevent_block = mlxsw_sp_qevent_block_create(mlxsw_sp, f->net);
1880                 if (!qevent_block)
1881                         return -ENOMEM;
1882                 block_cb = flow_block_cb_alloc(mlxsw_sp_qevent_block_cb, mlxsw_sp, qevent_block,
1883                                                mlxsw_sp_qevent_block_release);
1884                 if (IS_ERR(block_cb)) {
1885                         mlxsw_sp_qevent_block_destroy(qevent_block);
1886                         return PTR_ERR(block_cb);
1887                 }
1888                 register_block = true;
1889         } else {
1890                 qevent_block = flow_block_cb_priv(block_cb);
1891         }
1892         flow_block_cb_incref(block_cb);
1893
1894         qdisc = mlxsw_sp_qdisc_find_by_handle(mlxsw_sp_port, f->sch->handle);
1895         if (!qdisc) {
1896                 NL_SET_ERR_MSG(f->extack, "Qdisc not offloaded");
1897                 err = -ENOENT;
1898                 goto err_find_qdisc;
1899         }
1900
1901         if (WARN_ON(mlxsw_sp_qevent_binding_lookup(qevent_block, mlxsw_sp_port, f->sch->handle,
1902                                                    span_trigger))) {
1903                 err = -EEXIST;
1904                 goto err_binding_exists;
1905         }
1906
1907         qevent_binding = mlxsw_sp_qevent_binding_create(mlxsw_sp_port, f->sch->handle,
1908                                                         qdisc->tclass_num, span_trigger);
1909         if (IS_ERR(qevent_binding)) {
1910                 err = PTR_ERR(qevent_binding);
1911                 goto err_binding_create;
1912         }
1913
1914         err = mlxsw_sp_qevent_binding_configure(qevent_block, qevent_binding);
1915         if (err)
1916                 goto err_binding_configure;
1917
1918         list_add(&qevent_binding->list, &qevent_block->binding_list);
1919
1920         if (register_block) {
1921                 flow_block_cb_add(block_cb, f);
1922                 list_add_tail(&block_cb->driver_list, &mlxsw_sp_qevent_block_cb_list);
1923         }
1924
1925         return 0;
1926
1927 err_binding_configure:
1928         mlxsw_sp_qevent_binding_destroy(qevent_binding);
1929 err_binding_create:
1930 err_binding_exists:
1931 err_find_qdisc:
1932         if (!flow_block_cb_decref(block_cb))
1933                 flow_block_cb_free(block_cb);
1934         return err;
1935 }
1936
1937 static void mlxsw_sp_setup_tc_block_qevent_unbind(struct mlxsw_sp_port *mlxsw_sp_port,
1938                                                   struct flow_block_offload *f,
1939                                                   enum mlxsw_sp_span_trigger span_trigger)
1940 {
1941         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
1942         struct mlxsw_sp_qevent_binding *qevent_binding;
1943         struct mlxsw_sp_qevent_block *qevent_block;
1944         struct flow_block_cb *block_cb;
1945
1946         block_cb = flow_block_cb_lookup(f->block, mlxsw_sp_qevent_block_cb, mlxsw_sp);
1947         if (!block_cb)
1948                 return;
1949         qevent_block = flow_block_cb_priv(block_cb);
1950
1951         qevent_binding = mlxsw_sp_qevent_binding_lookup(qevent_block, mlxsw_sp_port, f->sch->handle,
1952                                                         span_trigger);
1953         if (!qevent_binding)
1954                 return;
1955
1956         list_del(&qevent_binding->list);
1957         mlxsw_sp_qevent_binding_deconfigure(qevent_block, qevent_binding);
1958         mlxsw_sp_qevent_binding_destroy(qevent_binding);
1959
1960         if (!flow_block_cb_decref(block_cb)) {
1961                 flow_block_cb_remove(block_cb, f);
1962                 list_del(&block_cb->driver_list);
1963         }
1964 }
1965
1966 static int mlxsw_sp_setup_tc_block_qevent(struct mlxsw_sp_port *mlxsw_sp_port,
1967                                           struct flow_block_offload *f,
1968                                           enum mlxsw_sp_span_trigger span_trigger)
1969 {
1970         f->driver_block_list = &mlxsw_sp_qevent_block_cb_list;
1971
1972         switch (f->command) {
1973         case FLOW_BLOCK_BIND:
1974                 return mlxsw_sp_setup_tc_block_qevent_bind(mlxsw_sp_port, f, span_trigger);
1975         case FLOW_BLOCK_UNBIND:
1976                 mlxsw_sp_setup_tc_block_qevent_unbind(mlxsw_sp_port, f, span_trigger);
1977                 return 0;
1978         default:
1979                 return -EOPNOTSUPP;
1980         }
1981 }
1982
1983 int mlxsw_sp_setup_tc_block_qevent_early_drop(struct mlxsw_sp_port *mlxsw_sp_port,
1984                                               struct flow_block_offload *f)
1985 {
1986         return mlxsw_sp_setup_tc_block_qevent(mlxsw_sp_port, f, MLXSW_SP_SPAN_TRIGGER_EARLY_DROP);
1987 }
1988
1989 int mlxsw_sp_tc_qdisc_init(struct mlxsw_sp_port *mlxsw_sp_port)
1990 {
1991         struct mlxsw_sp_qdisc_state *qdisc_state;
1992
1993         qdisc_state = kzalloc(sizeof(*qdisc_state), GFP_KERNEL);
1994         if (!qdisc_state)
1995                 return -ENOMEM;
1996
1997         mutex_init(&qdisc_state->lock);
1998         qdisc_state->root_qdisc.prio_bitmap = 0xff;
1999         qdisc_state->root_qdisc.tclass_num = MLXSW_SP_PORT_DEFAULT_TCLASS;
2000         mlxsw_sp_port->qdisc = qdisc_state;
2001         return 0;
2002 }
2003
2004 void mlxsw_sp_tc_qdisc_fini(struct mlxsw_sp_port *mlxsw_sp_port)
2005 {
2006         mutex_destroy(&mlxsw_sp_port->qdisc->lock);
2007         kfree(mlxsw_sp_port->qdisc);
2008 }