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

   1 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
   2 /* Copyright (c) 2016-2018 Mellanox Technologies. All rights reserved */
   3
   4 #include <linux/netdevice.h>
   5 #include <linux/string.h>
   6 #include <linux/bitops.h>
   7 #include <net/dcbnl.h>
   8
   9 #include "spectrum.h"
  10 #include "reg.h"
  11
  12 static u8 mlxsw_sp_dcbnl_getdcbx(struct net_device __always_unused *dev)
  13 {
  14         return DCB_CAP_DCBX_HOST | DCB_CAP_DCBX_VER_IEEE;
  15 }
  16
  17 static u8 mlxsw_sp_dcbnl_setdcbx(struct net_device __always_unused *dev,
  18                                  u8 mode)
  19 {
  20         return (mode != (DCB_CAP_DCBX_HOST | DCB_CAP_DCBX_VER_IEEE)) ? 1 : 0;
  21 }
  22
  23 static int mlxsw_sp_dcbnl_ieee_getets(struct net_device *dev,
  24                                       struct ieee_ets *ets)
  25 {
  26         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
  27
  28         memcpy(ets, mlxsw_sp_port->dcb.ets, sizeof(*ets));
  29
  30         return 0;
  31 }
  32
  33 static int mlxsw_sp_port_ets_validate(struct mlxsw_sp_port *mlxsw_sp_port,
  34                                       struct ieee_ets *ets)
  35 {
  36         struct net_device *dev = mlxsw_sp_port->dev;
  37         bool has_ets_tc = false;
  38         int i, tx_bw_sum = 0;
  39
  40         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
  41                 switch (ets->tc_tsa[i]) {
  42                 case IEEE_8021QAZ_TSA_STRICT:
  43                         break;
  44                 case IEEE_8021QAZ_TSA_ETS:
  45                         has_ets_tc = true;
  46                         tx_bw_sum += ets->tc_tx_bw[i];
  47                         break;
  48                 default:
  49                         netdev_err(dev, "Only strict priority and ETS are supported\n");
  50                         return -EINVAL;
  51                 }
  52
  53                 if (ets->prio_tc[i] >= IEEE_8021QAZ_MAX_TCS) {
  54                         netdev_err(dev, "Invalid TC\n");
  55                         return -EINVAL;
  56                 }
  57         }
  58
  59         if (has_ets_tc && tx_bw_sum != 100) {
  60                 netdev_err(dev, "Total ETS bandwidth should equal 100\n");
  61                 return -EINVAL;
  62         }
  63
  64         return 0;
  65 }
  66
  67 static int mlxsw_sp_port_pg_prio_map(struct mlxsw_sp_port *mlxsw_sp_port,
  68                                      u8 *prio_tc)
  69 {
  70         char pptb_pl[MLXSW_REG_PPTB_LEN];
  71         int i;
  72
  73         mlxsw_reg_pptb_pack(pptb_pl, mlxsw_sp_port->local_port);
  74         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++)
  75                 mlxsw_reg_pptb_prio_to_buff_pack(pptb_pl, i, prio_tc[i]);
  76
  77         return mlxsw_reg_write(mlxsw_sp_port->mlxsw_sp->core, MLXSW_REG(pptb),
  78                                pptb_pl);
  79 }
  80
  81 static bool mlxsw_sp_ets_has_pg(u8 *prio_tc, u8 pg)
  82 {
  83         int i;
  84
  85         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++)
  86                 if (prio_tc[i] == pg)
  87                         return true;
  88         return false;
  89 }
  90
  91 static int mlxsw_sp_port_pg_destroy(struct mlxsw_sp_port *mlxsw_sp_port,
  92                                     u8 *old_prio_tc, u8 *new_prio_tc)
  93 {
  94         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
  95         char pbmc_pl[MLXSW_REG_PBMC_LEN];
  96         int err, i;
  97
  98         mlxsw_reg_pbmc_pack(pbmc_pl, mlxsw_sp_port->local_port, 0, 0);
  99         err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(pbmc), pbmc_pl);
 100         if (err)
 101                 return err;
 102
 103         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
 104                 u8 pg = old_prio_tc[i];
 105
 106                 if (!mlxsw_sp_ets_has_pg(new_prio_tc, pg))
 107                         mlxsw_reg_pbmc_lossy_buffer_pack(pbmc_pl, pg, 0);
 108         }
 109
 110         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(pbmc), pbmc_pl);
 111 }
 112
 113 static int mlxsw_sp_port_headroom_set(struct mlxsw_sp_port *mlxsw_sp_port,
 114                                       struct ieee_ets *ets)
 115 {
 116         bool pause_en = mlxsw_sp_port_is_pause_en(mlxsw_sp_port);
 117         struct ieee_ets *my_ets = mlxsw_sp_port->dcb.ets;
 118         struct net_device *dev = mlxsw_sp_port->dev;
 119         int err;
 120
 121         /* Create the required PGs, but don't destroy existing ones, as
 122          * traffic is still directed to them.
 123          */
 124         err = __mlxsw_sp_port_headroom_set(mlxsw_sp_port, dev->mtu,
 125                                            ets->prio_tc, pause_en,
 126                                            mlxsw_sp_port->dcb.pfc);
 127         if (err) {
 128                 netdev_err(dev, "Failed to configure port's headroom\n");
 129                 return err;
 130         }
 131
 132         err = mlxsw_sp_port_pg_prio_map(mlxsw_sp_port, ets->prio_tc);
 133         if (err) {
 134                 netdev_err(dev, "Failed to set PG-priority mapping\n");
 135                 goto err_port_prio_pg_map;
 136         }
 137
 138         err = mlxsw_sp_port_pg_destroy(mlxsw_sp_port, my_ets->prio_tc,
 139                                        ets->prio_tc);
 140         if (err)
 141                 netdev_warn(dev, "Failed to remove ununsed PGs\n");
 142
 143         return 0;
 144
 145 err_port_prio_pg_map:
 146         mlxsw_sp_port_pg_destroy(mlxsw_sp_port, ets->prio_tc, my_ets->prio_tc);
 147         return err;
 148 }
 149
 150 static int __mlxsw_sp_dcbnl_ieee_setets(struct mlxsw_sp_port *mlxsw_sp_port,
 151                                         struct ieee_ets *ets)
 152 {
 153         struct ieee_ets *my_ets = mlxsw_sp_port->dcb.ets;
 154         struct net_device *dev = mlxsw_sp_port->dev;
 155         int i, err;
 156
 157         /* Egress configuration. */
 158         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
 159                 bool dwrr = ets->tc_tsa[i] == IEEE_8021QAZ_TSA_ETS;
 160                 u8 weight = ets->tc_tx_bw[i];
 161
 162                 err = mlxsw_sp_port_ets_set(mlxsw_sp_port,
 163                                             MLXSW_REG_QEEC_HIERARCY_SUBGROUP, i,
 164                                             0, dwrr, weight);
 165                 if (err) {
 166                         netdev_err(dev, "Failed to link subgroup ETS element %d to group\n",
 167                                    i);
 168                         goto err_port_ets_set;
 169                 }
 170         }
 171
 172         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
 173                 err = mlxsw_sp_port_prio_tc_set(mlxsw_sp_port, i,
 174                                                 ets->prio_tc[i]);
 175                 if (err) {
 176                         netdev_err(dev, "Failed to map prio %d to TC %d\n", i,
 177                                    ets->prio_tc[i]);
 178                         goto err_port_prio_tc_set;
 179                 }
 180         }
 181
 182         /* Ingress configuration. */
 183         err = mlxsw_sp_port_headroom_set(mlxsw_sp_port, ets);
 184         if (err)
 185                 goto err_port_headroom_set;
 186
 187         return 0;
 188
 189 err_port_headroom_set:
 190         i = IEEE_8021QAZ_MAX_TCS;
 191 err_port_prio_tc_set:
 192         for (i--; i >= 0; i--)
 193                 mlxsw_sp_port_prio_tc_set(mlxsw_sp_port, i, my_ets->prio_tc[i]);
 194         i = IEEE_8021QAZ_MAX_TCS;
 195 err_port_ets_set:
 196         for (i--; i >= 0; i--) {
 197                 bool dwrr = my_ets->tc_tsa[i] == IEEE_8021QAZ_TSA_ETS;
 198                 u8 weight = my_ets->tc_tx_bw[i];
 199
 200                 err = mlxsw_sp_port_ets_set(mlxsw_sp_port,
 201                                             MLXSW_REG_QEEC_HIERARCY_SUBGROUP, i,
 202                                             0, dwrr, weight);
 203         }
 204         return err;
 205 }
 206
 207 static int mlxsw_sp_dcbnl_ieee_setets(struct net_device *dev,
 208                                       struct ieee_ets *ets)
 209 {
 210         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 211         int err;
 212
 213         err = mlxsw_sp_port_ets_validate(mlxsw_sp_port, ets);
 214         if (err)
 215                 return err;
 216
 217         err = __mlxsw_sp_dcbnl_ieee_setets(mlxsw_sp_port, ets);
 218         if (err)
 219                 return err;
 220
 221         memcpy(mlxsw_sp_port->dcb.ets, ets, sizeof(*ets));
 222         mlxsw_sp_port->dcb.ets->ets_cap = IEEE_8021QAZ_MAX_TCS;
 223
 224         return 0;
 225 }
 226
 227 static int mlxsw_sp_dcbnl_app_validate(struct net_device *dev,
 228                                        struct dcb_app *app)
 229 {
 230         int prio;
 231
 232         if (app->priority >= IEEE_8021QAZ_MAX_TCS) {
 233                 netdev_err(dev, "APP entry with priority value %u is invalid\n",
 234                            app->priority);
 235                 return -EINVAL;
 236         }
 237
 238         switch (app->selector) {
 239         case IEEE_8021QAZ_APP_SEL_DSCP:
 240                 if (app->protocol >= 64) {
 241                         netdev_err(dev, "DSCP APP entry with protocol value %u is invalid\n",
 242                                    app->protocol);
 243                         return -EINVAL;
 244                 }
 245
 246                 /* Warn about any DSCP APP entries with the same PID. */
 247                 prio = fls(dcb_ieee_getapp_mask(dev, app));
 248                 if (prio--) {
 249                         if (prio < app->priority)
 250                                 netdev_warn(dev, "Choosing priority %d for DSCP %d in favor of previously-active value of %d\n",
 251                                             app->priority, app->protocol, prio);
 252                         else if (prio > app->priority)
 253                                 netdev_warn(dev, "Ignoring new priority %d for DSCP %d in favor of current value of %d\n",
 254                                             app->priority, app->protocol, prio);
 255                 }
 256                 break;
 257
 258         case IEEE_8021QAZ_APP_SEL_ETHERTYPE:
 259                 if (app->protocol) {
 260                         netdev_err(dev, "EtherType APP entries with protocol value != 0 not supported\n");
 261                         return -EINVAL;
 262                 }
 263                 break;
 264
 265         default:
 266                 netdev_err(dev, "APP entries with selector %u not supported\n",
 267                            app->selector);
 268                 return -EINVAL;
 269         }
 270
 271         return 0;
 272 }
 273
 274 static u8
 275 mlxsw_sp_port_dcb_app_default_prio(struct mlxsw_sp_port *mlxsw_sp_port)
 276 {
 277         u8 prio_mask;
 278
 279         prio_mask = dcb_ieee_getapp_default_prio_mask(mlxsw_sp_port->dev);
 280         if (prio_mask)
 281                 /* Take the highest configured priority. */
 282                 return fls(prio_mask) - 1;
 283
 284         return 0;
 285 }
 286
 287 static void
 288 mlxsw_sp_port_dcb_app_dscp_prio_map(struct mlxsw_sp_port *mlxsw_sp_port,
 289                                     u8 default_prio,
 290                                     struct dcb_ieee_app_dscp_map *map)
 291 {
 292         int i;
 293
 294         dcb_ieee_getapp_dscp_prio_mask_map(mlxsw_sp_port->dev, map);
 295         for (i = 0; i < ARRAY_SIZE(map->map); ++i) {
 296                 if (map->map[i])
 297                         map->map[i] = fls(map->map[i]) - 1;
 298                 else
 299                         map->map[i] = default_prio;
 300         }
 301 }
 302
 303 static bool
 304 mlxsw_sp_port_dcb_app_prio_dscp_map(struct mlxsw_sp_port *mlxsw_sp_port,
 305                                     struct dcb_ieee_app_prio_map *map)
 306 {
 307         bool have_dscp = false;
 308         int i;
 309
 310         dcb_ieee_getapp_prio_dscp_mask_map(mlxsw_sp_port->dev, map);
 311         for (i = 0; i < ARRAY_SIZE(map->map); ++i) {
 312                 if (map->map[i]) {
 313                         map->map[i] = fls64(map->map[i]) - 1;
 314                         have_dscp = true;
 315                 }
 316         }
 317
 318         return have_dscp;
 319 }
 320
 321 static int
 322 mlxsw_sp_port_dcb_app_update_qpts(struct mlxsw_sp_port *mlxsw_sp_port,
 323                                   enum mlxsw_reg_qpts_trust_state ts)
 324 {
 325         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 326         char qpts_pl[MLXSW_REG_QPTS_LEN];
 327
 328         mlxsw_reg_qpts_pack(qpts_pl, mlxsw_sp_port->local_port, ts);
 329         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpts), qpts_pl);
 330 }
 331
 332 static int
 333 mlxsw_sp_port_dcb_app_update_qrwe(struct mlxsw_sp_port *mlxsw_sp_port,
 334                                   bool rewrite_dscp)
 335 {
 336         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 337         char qrwe_pl[MLXSW_REG_QRWE_LEN];
 338
 339         mlxsw_reg_qrwe_pack(qrwe_pl, mlxsw_sp_port->local_port,
 340                             false, rewrite_dscp);
 341         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qrwe), qrwe_pl);
 342 }
 343
 344 static int
 345 mlxsw_sp_port_dcb_toggle_trust(struct mlxsw_sp_port *mlxsw_sp_port,
 346                                enum mlxsw_reg_qpts_trust_state ts)
 347 {
 348         bool rewrite_dscp = ts == MLXSW_REG_QPTS_TRUST_STATE_DSCP;
 349         int err;
 350
 351         if (mlxsw_sp_port->dcb.trust_state == ts)
 352                 return 0;
 353
 354         err = mlxsw_sp_port_dcb_app_update_qpts(mlxsw_sp_port, ts);
 355         if (err)
 356                 return err;
 357
 358         err = mlxsw_sp_port_dcb_app_update_qrwe(mlxsw_sp_port, rewrite_dscp);
 359         if (err)
 360                 goto err_update_qrwe;
 361
 362         mlxsw_sp_port->dcb.trust_state = ts;
 363         return 0;
 364
 365 err_update_qrwe:
 366         mlxsw_sp_port_dcb_app_update_qpts(mlxsw_sp_port,
 367                                           mlxsw_sp_port->dcb.trust_state);
 368         return err;
 369 }
 370
 371 static int
 372 mlxsw_sp_port_dcb_app_update_qpdpm(struct mlxsw_sp_port *mlxsw_sp_port,
 373                                    struct dcb_ieee_app_dscp_map *map)
 374 {
 375         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 376         char qpdpm_pl[MLXSW_REG_QPDPM_LEN];
 377         short int i;
 378
 379         mlxsw_reg_qpdpm_pack(qpdpm_pl, mlxsw_sp_port->local_port);
 380         for (i = 0; i < ARRAY_SIZE(map->map); ++i)
 381                 mlxsw_reg_qpdpm_dscp_pack(qpdpm_pl, i, map->map[i]);
 382         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpdpm), qpdpm_pl);
 383 }
 384
 385 static int
 386 mlxsw_sp_port_dcb_app_update_qpdsm(struct mlxsw_sp_port *mlxsw_sp_port,
 387                                    struct dcb_ieee_app_prio_map *map)
 388 {
 389         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 390         char qpdsm_pl[MLXSW_REG_QPDSM_LEN];
 391         short int i;
 392
 393         mlxsw_reg_qpdsm_pack(qpdsm_pl, mlxsw_sp_port->local_port);
 394         for (i = 0; i < ARRAY_SIZE(map->map); ++i)
 395                 mlxsw_reg_qpdsm_prio_pack(qpdsm_pl, i, map->map[i]);
 396         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpdsm), qpdsm_pl);
 397 }
 398
 399 static int mlxsw_sp_port_dcb_app_update(struct mlxsw_sp_port *mlxsw_sp_port)
 400 {
 401         struct dcb_ieee_app_prio_map prio_map;
 402         struct dcb_ieee_app_dscp_map dscp_map;
 403         u8 default_prio;
 404         bool have_dscp;
 405         int err;
 406
 407         default_prio = mlxsw_sp_port_dcb_app_default_prio(mlxsw_sp_port);
 408         have_dscp = mlxsw_sp_port_dcb_app_prio_dscp_map(mlxsw_sp_port,
 409                                                         &prio_map);
 410
 411         if (!have_dscp) {
 412                 err = mlxsw_sp_port_dcb_toggle_trust(mlxsw_sp_port,
 413                                         MLXSW_REG_QPTS_TRUST_STATE_PCP);
 414                 if (err)
 415                         netdev_err(mlxsw_sp_port->dev, "Couldn't switch to trust L2\n");
 416                 return err;
 417         }
 418
 419         mlxsw_sp_port_dcb_app_dscp_prio_map(mlxsw_sp_port, default_prio,
 420                                             &dscp_map);
 421         err = mlxsw_sp_port_dcb_app_update_qpdpm(mlxsw_sp_port,
 422                                                  &dscp_map);
 423         if (err) {
 424                 netdev_err(mlxsw_sp_port->dev, "Couldn't configure priority map\n");
 425                 return err;
 426         }
 427
 428         err = mlxsw_sp_port_dcb_app_update_qpdsm(mlxsw_sp_port,
 429                                                  &prio_map);
 430         if (err) {
 431                 netdev_err(mlxsw_sp_port->dev, "Couldn't configure DSCP rewrite map\n");
 432                 return err;
 433         }
 434
 435         err = mlxsw_sp_port_dcb_toggle_trust(mlxsw_sp_port,
 436                                              MLXSW_REG_QPTS_TRUST_STATE_DSCP);
 437         if (err) {
 438                 /* A failure to set trust DSCP means that the QPDPM and QPDSM
 439                  * maps installed above are not in effect. And since we are here
 440                  * attempting to set trust DSCP, we couldn't have attempted to
 441                  * switch trust to PCP. Thus no cleanup is necessary.
 442                  */
 443                 netdev_err(mlxsw_sp_port->dev, "Couldn't switch to trust L3\n");
 444                 return err;
 445         }
 446
 447         return 0;
 448 }
 449
 450 static int mlxsw_sp_dcbnl_ieee_setapp(struct net_device *dev,
 451                                       struct dcb_app *app)
 452 {
 453         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 454         int err;
 455
 456         err = mlxsw_sp_dcbnl_app_validate(dev, app);
 457         if (err)
 458                 return err;
 459
 460         err = dcb_ieee_setapp(dev, app);
 461         if (err)
 462                 return err;
 463
 464         err = mlxsw_sp_port_dcb_app_update(mlxsw_sp_port);
 465         if (err)
 466                 goto err_update;
 467
 468         return 0;
 469
 470 err_update:
 471         dcb_ieee_delapp(dev, app);
 472         return err;
 473 }
 474
 475 static int mlxsw_sp_dcbnl_ieee_delapp(struct net_device *dev,
 476                                       struct dcb_app *app)
 477 {
 478         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 479         int err;
 480
 481         err = dcb_ieee_delapp(dev, app);
 482         if (err)
 483                 return err;
 484
 485         err = mlxsw_sp_port_dcb_app_update(mlxsw_sp_port);
 486         if (err)
 487                 netdev_err(dev, "Failed to update DCB APP configuration\n");
 488         return 0;
 489 }
 490
 491 static int mlxsw_sp_dcbnl_ieee_getmaxrate(struct net_device *dev,
 492                                           struct ieee_maxrate *maxrate)
 493 {
 494         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 495
 496         memcpy(maxrate, mlxsw_sp_port->dcb.maxrate, sizeof(*maxrate));
 497
 498         return 0;
 499 }
 500
 501 static int mlxsw_sp_dcbnl_ieee_setmaxrate(struct net_device *dev,
 502                                           struct ieee_maxrate *maxrate)
 503 {
 504         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 505         struct ieee_maxrate *my_maxrate = mlxsw_sp_port->dcb.maxrate;
 506         int err, i;
 507
 508         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
 509                 err = mlxsw_sp_port_ets_maxrate_set(mlxsw_sp_port,
 510                                                     MLXSW_REG_QEEC_HIERARCY_SUBGROUP,
 511                                                     i, 0,
 512                                                     maxrate->tc_maxrate[i]);
 513                 if (err) {
 514                         netdev_err(dev, "Failed to set maxrate for TC %d\n", i);
 515                         goto err_port_ets_maxrate_set;
 516                 }
 517         }
 518
 519         memcpy(mlxsw_sp_port->dcb.maxrate, maxrate, sizeof(*maxrate));
 520
 521         return 0;
 522
 523 err_port_ets_maxrate_set:
 524         for (i--; i >= 0; i--)
 525                 mlxsw_sp_port_ets_maxrate_set(mlxsw_sp_port,
 526                                               MLXSW_REG_QEEC_HIERARCY_SUBGROUP,
 527                                               i, 0, my_maxrate->tc_maxrate[i]);
 528         return err;
 529 }
 530
 531 static int mlxsw_sp_port_pfc_cnt_get(struct mlxsw_sp_port *mlxsw_sp_port,
 532                                      u8 prio)
 533 {
 534         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 535         struct ieee_pfc *my_pfc = mlxsw_sp_port->dcb.pfc;
 536         char ppcnt_pl[MLXSW_REG_PPCNT_LEN];
 537         int err;
 538
 539         mlxsw_reg_ppcnt_pack(ppcnt_pl, mlxsw_sp_port->local_port,
 540                              MLXSW_REG_PPCNT_PRIO_CNT, prio);
 541         err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(ppcnt), ppcnt_pl);
 542         if (err)
 543                 return err;
 544
 545         my_pfc->requests[prio] = mlxsw_reg_ppcnt_tx_pause_get(ppcnt_pl);
 546         my_pfc->indications[prio] = mlxsw_reg_ppcnt_rx_pause_get(ppcnt_pl);
 547
 548         return 0;
 549 }
 550
 551 static int mlxsw_sp_dcbnl_ieee_getpfc(struct net_device *dev,
 552                                       struct ieee_pfc *pfc)
 553 {
 554         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 555         int err, i;
 556
 557         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
 558                 err = mlxsw_sp_port_pfc_cnt_get(mlxsw_sp_port, i);
 559                 if (err) {
 560                         netdev_err(dev, "Failed to get PFC count for priority %d\n",
 561                                    i);
 562                         return err;
 563                 }
 564         }
 565
 566         memcpy(pfc, mlxsw_sp_port->dcb.pfc, sizeof(*pfc));
 567
 568         return 0;
 569 }
 570
 571 static int mlxsw_sp_port_pfc_set(struct mlxsw_sp_port *mlxsw_sp_port,
 572                                  struct ieee_pfc *pfc)
 573 {
 574         char pfcc_pl[MLXSW_REG_PFCC_LEN];
 575
 576         mlxsw_reg_pfcc_pack(pfcc_pl, mlxsw_sp_port->local_port);
 577         mlxsw_reg_pfcc_pprx_set(pfcc_pl, mlxsw_sp_port->link.rx_pause);
 578         mlxsw_reg_pfcc_pptx_set(pfcc_pl, mlxsw_sp_port->link.tx_pause);
 579         mlxsw_reg_pfcc_prio_pack(pfcc_pl, pfc->pfc_en);
 580
 581         return mlxsw_reg_write(mlxsw_sp_port->mlxsw_sp->core, MLXSW_REG(pfcc),
 582                                pfcc_pl);
 583 }
 584
 585 static int mlxsw_sp_dcbnl_ieee_setpfc(struct net_device *dev,
 586                                       struct ieee_pfc *pfc)
 587 {
 588         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 589         bool pause_en = mlxsw_sp_port_is_pause_en(mlxsw_sp_port);
 590         int err;
 591
 592         if (pause_en && pfc->pfc_en) {
 593                 netdev_err(dev, "PAUSE frames already enabled on port\n");
 594                 return -EINVAL;
 595         }
 596
 597         err = __mlxsw_sp_port_headroom_set(mlxsw_sp_port, dev->mtu,
 598                                            mlxsw_sp_port->dcb.ets->prio_tc,
 599                                            pause_en, pfc);
 600         if (err) {
 601                 netdev_err(dev, "Failed to configure port's headroom for PFC\n");
 602                 return err;
 603         }
 604
 605         err = mlxsw_sp_port_pfc_set(mlxsw_sp_port, pfc);
 606         if (err) {
 607                 netdev_err(dev, "Failed to configure PFC\n");
 608                 goto err_port_pfc_set;
 609         }
 610
 611         memcpy(mlxsw_sp_port->dcb.pfc, pfc, sizeof(*pfc));
 612         mlxsw_sp_port->dcb.pfc->pfc_cap = IEEE_8021QAZ_MAX_TCS;
 613
 614         return 0;
 615
 616 err_port_pfc_set:
 617         __mlxsw_sp_port_headroom_set(mlxsw_sp_port, dev->mtu,
 618                                      mlxsw_sp_port->dcb.ets->prio_tc, pause_en,
 619                                      mlxsw_sp_port->dcb.pfc);
 620         return err;
 621 }
 622
 623 static const struct dcbnl_rtnl_ops mlxsw_sp_dcbnl_ops = {
 624         .ieee_getets            = mlxsw_sp_dcbnl_ieee_getets,
 625         .ieee_setets            = mlxsw_sp_dcbnl_ieee_setets,
 626         .ieee_getmaxrate        = mlxsw_sp_dcbnl_ieee_getmaxrate,
 627         .ieee_setmaxrate        = mlxsw_sp_dcbnl_ieee_setmaxrate,
 628         .ieee_getpfc            = mlxsw_sp_dcbnl_ieee_getpfc,
 629         .ieee_setpfc            = mlxsw_sp_dcbnl_ieee_setpfc,
 630         .ieee_setapp            = mlxsw_sp_dcbnl_ieee_setapp,
 631         .ieee_delapp            = mlxsw_sp_dcbnl_ieee_delapp,
 632
 633         .getdcbx                = mlxsw_sp_dcbnl_getdcbx,
 634         .setdcbx                = mlxsw_sp_dcbnl_setdcbx,
 635 };
 636
 637 static int mlxsw_sp_port_ets_init(struct mlxsw_sp_port *mlxsw_sp_port)
 638 {
 639         mlxsw_sp_port->dcb.ets = kzalloc(sizeof(*mlxsw_sp_port->dcb.ets),
 640                                          GFP_KERNEL);
 641         if (!mlxsw_sp_port->dcb.ets)
 642                 return -ENOMEM;
 643
 644         mlxsw_sp_port->dcb.ets->ets_cap = IEEE_8021QAZ_MAX_TCS;
 645
 646         return 0;
 647 }
 648
 649 static void mlxsw_sp_port_ets_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 650 {
 651         kfree(mlxsw_sp_port->dcb.ets);
 652 }
 653
 654 static int mlxsw_sp_port_maxrate_init(struct mlxsw_sp_port *mlxsw_sp_port)
 655 {
 656         int i;
 657
 658         mlxsw_sp_port->dcb.maxrate = kmalloc(sizeof(*mlxsw_sp_port->dcb.maxrate),
 659                                              GFP_KERNEL);
 660         if (!mlxsw_sp_port->dcb.maxrate)
 661                 return -ENOMEM;
 662
 663         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++)
 664                 mlxsw_sp_port->dcb.maxrate->tc_maxrate[i] = MLXSW_REG_QEEC_MAS_DIS;
 665
 666         return 0;
 667 }
 668
 669 static void mlxsw_sp_port_maxrate_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 670 {
 671         kfree(mlxsw_sp_port->dcb.maxrate);
 672 }
 673
 674 static int mlxsw_sp_port_pfc_init(struct mlxsw_sp_port *mlxsw_sp_port)
 675 {
 676         mlxsw_sp_port->dcb.pfc = kzalloc(sizeof(*mlxsw_sp_port->dcb.pfc),
 677                                          GFP_KERNEL);
 678         if (!mlxsw_sp_port->dcb.pfc)
 679                 return -ENOMEM;
 680
 681         mlxsw_sp_port->dcb.pfc->pfc_cap = IEEE_8021QAZ_MAX_TCS;
 682
 683         return 0;
 684 }
 685
 686 static void mlxsw_sp_port_pfc_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 687 {
 688         kfree(mlxsw_sp_port->dcb.pfc);
 689 }
 690
 691 int mlxsw_sp_port_dcb_init(struct mlxsw_sp_port *mlxsw_sp_port)
 692 {
 693         int err;
 694
 695         err = mlxsw_sp_port_ets_init(mlxsw_sp_port);
 696         if (err)
 697                 return err;
 698         err = mlxsw_sp_port_maxrate_init(mlxsw_sp_port);
 699         if (err)
 700                 goto err_port_maxrate_init;
 701         err = mlxsw_sp_port_pfc_init(mlxsw_sp_port);
 702         if (err)
 703                 goto err_port_pfc_init;
 704
 705         mlxsw_sp_port->dcb.trust_state = MLXSW_REG_QPTS_TRUST_STATE_PCP;
 706         mlxsw_sp_port->dev->dcbnl_ops = &mlxsw_sp_dcbnl_ops;
 707
 708         return 0;
 709
 710 err_port_pfc_init:
 711         mlxsw_sp_port_maxrate_fini(mlxsw_sp_port);
 712 err_port_maxrate_init:
 713         mlxsw_sp_port_ets_fini(mlxsw_sp_port);
 714         return err;
 715 }
 716
 717 void mlxsw_sp_port_dcb_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 718 {
 719         mlxsw_sp_port_pfc_fini(mlxsw_sp_port);
 720         mlxsw_sp_port_maxrate_fini(mlxsw_sp_port);
 721         mlxsw_sp_port_ets_fini(mlxsw_sp_port);
 722 }