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         if (app->priority >= IEEE_8021QAZ_MAX_TCS) {
 231                 netdev_err(dev, "APP entry with priority value %u is invalid\n",
 232                            app->priority);
 233                 return -EINVAL;
 234         }
 235
 236         switch (app->selector) {
 237         case IEEE_8021QAZ_APP_SEL_DSCP:
 238                 if (app->protocol >= 64) {
 239                         netdev_err(dev, "DSCP APP entry with protocol value %u is invalid\n",
 240                                    app->protocol);
 241                         return -EINVAL;
 242                 }
 243                 break;
 244
 245         case IEEE_8021QAZ_APP_SEL_ETHERTYPE:
 246                 if (app->protocol) {
 247                         netdev_err(dev, "EtherType APP entries with protocol value != 0 not supported\n");
 248                         return -EINVAL;
 249                 }
 250                 break;
 251
 252         default:
 253                 netdev_err(dev, "APP entries with selector %u not supported\n",
 254                            app->selector);
 255                 return -EINVAL;
 256         }
 257
 258         return 0;
 259 }
 260
 261 static u8
 262 mlxsw_sp_port_dcb_app_default_prio(struct mlxsw_sp_port *mlxsw_sp_port)
 263 {
 264         u8 prio_mask;
 265
 266         prio_mask = dcb_ieee_getapp_default_prio_mask(mlxsw_sp_port->dev);
 267         if (prio_mask)
 268                 /* Take the highest configured priority. */
 269                 return fls(prio_mask) - 1;
 270
 271         return 0;
 272 }
 273
 274 static void
 275 mlxsw_sp_port_dcb_app_dscp_prio_map(struct mlxsw_sp_port *mlxsw_sp_port,
 276                                     u8 default_prio,
 277                                     struct dcb_ieee_app_dscp_map *map)
 278 {
 279         int i;
 280
 281         dcb_ieee_getapp_dscp_prio_mask_map(mlxsw_sp_port->dev, map);
 282         for (i = 0; i < ARRAY_SIZE(map->map); ++i) {
 283                 if (map->map[i])
 284                         map->map[i] = fls(map->map[i]) - 1;
 285                 else
 286                         map->map[i] = default_prio;
 287         }
 288 }
 289
 290 static bool
 291 mlxsw_sp_port_dcb_app_prio_dscp_map(struct mlxsw_sp_port *mlxsw_sp_port,
 292                                     struct dcb_ieee_app_prio_map *map)
 293 {
 294         bool have_dscp = false;
 295         int i;
 296
 297         dcb_ieee_getapp_prio_dscp_mask_map(mlxsw_sp_port->dev, map);
 298         for (i = 0; i < ARRAY_SIZE(map->map); ++i) {
 299                 if (map->map[i]) {
 300                         map->map[i] = fls64(map->map[i]) - 1;
 301                         have_dscp = true;
 302                 }
 303         }
 304
 305         return have_dscp;
 306 }
 307
 308 static int
 309 mlxsw_sp_port_dcb_app_update_qpts(struct mlxsw_sp_port *mlxsw_sp_port,
 310                                   enum mlxsw_reg_qpts_trust_state ts)
 311 {
 312         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 313         char qpts_pl[MLXSW_REG_QPTS_LEN];
 314
 315         mlxsw_reg_qpts_pack(qpts_pl, mlxsw_sp_port->local_port, ts);
 316         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpts), qpts_pl);
 317 }
 318
 319 static int
 320 mlxsw_sp_port_dcb_app_update_qrwe(struct mlxsw_sp_port *mlxsw_sp_port,
 321                                   bool rewrite_dscp)
 322 {
 323         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 324         char qrwe_pl[MLXSW_REG_QRWE_LEN];
 325
 326         mlxsw_reg_qrwe_pack(qrwe_pl, mlxsw_sp_port->local_port,
 327                             false, rewrite_dscp);
 328         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qrwe), qrwe_pl);
 329 }
 330
 331 static int
 332 mlxsw_sp_port_dcb_toggle_trust(struct mlxsw_sp_port *mlxsw_sp_port,
 333                                enum mlxsw_reg_qpts_trust_state ts)
 334 {
 335         bool rewrite_dscp = ts == MLXSW_REG_QPTS_TRUST_STATE_DSCP;
 336         int err;
 337
 338         if (mlxsw_sp_port->dcb.trust_state == ts)
 339                 return 0;
 340
 341         err = mlxsw_sp_port_dcb_app_update_qpts(mlxsw_sp_port, ts);
 342         if (err)
 343                 return err;
 344
 345         err = mlxsw_sp_port_dcb_app_update_qrwe(mlxsw_sp_port, rewrite_dscp);
 346         if (err)
 347                 goto err_update_qrwe;
 348
 349         mlxsw_sp_port->dcb.trust_state = ts;
 350         return 0;
 351
 352 err_update_qrwe:
 353         mlxsw_sp_port_dcb_app_update_qpts(mlxsw_sp_port,
 354                                           mlxsw_sp_port->dcb.trust_state);
 355         return err;
 356 }
 357
 358 static int
 359 mlxsw_sp_port_dcb_app_update_qpdpm(struct mlxsw_sp_port *mlxsw_sp_port,
 360                                    struct dcb_ieee_app_dscp_map *map)
 361 {
 362         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 363         char qpdpm_pl[MLXSW_REG_QPDPM_LEN];
 364         short int i;
 365
 366         mlxsw_reg_qpdpm_pack(qpdpm_pl, mlxsw_sp_port->local_port);
 367         for (i = 0; i < ARRAY_SIZE(map->map); ++i)
 368                 mlxsw_reg_qpdpm_dscp_pack(qpdpm_pl, i, map->map[i]);
 369         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpdpm), qpdpm_pl);
 370 }
 371
 372 static int
 373 mlxsw_sp_port_dcb_app_update_qpdsm(struct mlxsw_sp_port *mlxsw_sp_port,
 374                                    struct dcb_ieee_app_prio_map *map)
 375 {
 376         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 377         char qpdsm_pl[MLXSW_REG_QPDSM_LEN];
 378         short int i;
 379
 380         mlxsw_reg_qpdsm_pack(qpdsm_pl, mlxsw_sp_port->local_port);
 381         for (i = 0; i < ARRAY_SIZE(map->map); ++i)
 382                 mlxsw_reg_qpdsm_prio_pack(qpdsm_pl, i, map->map[i]);
 383         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpdsm), qpdsm_pl);
 384 }
 385
 386 static int mlxsw_sp_port_dcb_app_update(struct mlxsw_sp_port *mlxsw_sp_port)
 387 {
 388         struct dcb_ieee_app_prio_map prio_map;
 389         struct dcb_ieee_app_dscp_map dscp_map;
 390         u8 default_prio;
 391         bool have_dscp;
 392         int err;
 393
 394         default_prio = mlxsw_sp_port_dcb_app_default_prio(mlxsw_sp_port);
 395         have_dscp = mlxsw_sp_port_dcb_app_prio_dscp_map(mlxsw_sp_port,
 396                                                         &prio_map);
 397
 398         mlxsw_sp_port_dcb_app_dscp_prio_map(mlxsw_sp_port, default_prio,
 399                                             &dscp_map);
 400         err = mlxsw_sp_port_dcb_app_update_qpdpm(mlxsw_sp_port,
 401                                                  &dscp_map);
 402         if (err) {
 403                 netdev_err(mlxsw_sp_port->dev, "Couldn't configure priority map\n");
 404                 return err;
 405         }
 406
 407         err = mlxsw_sp_port_dcb_app_update_qpdsm(mlxsw_sp_port,
 408                                                  &prio_map);
 409         if (err) {
 410                 netdev_err(mlxsw_sp_port->dev, "Couldn't configure DSCP rewrite map\n");
 411                 return err;
 412         }
 413
 414         if (!have_dscp) {
 415                 err = mlxsw_sp_port_dcb_toggle_trust(mlxsw_sp_port,
 416                                         MLXSW_REG_QPTS_TRUST_STATE_PCP);
 417                 if (err)
 418                         netdev_err(mlxsw_sp_port->dev, "Couldn't switch to trust L2\n");
 419                 return err;
 420         }
 421
 422         err = mlxsw_sp_port_dcb_toggle_trust(mlxsw_sp_port,
 423                                              MLXSW_REG_QPTS_TRUST_STATE_DSCP);
 424         if (err) {
 425                 /* A failure to set trust DSCP means that the QPDPM and QPDSM
 426                  * maps installed above are not in effect. And since we are here
 427                  * attempting to set trust DSCP, we couldn't have attempted to
 428                  * switch trust to PCP. Thus no cleanup is necessary.
 429                  */
 430                 netdev_err(mlxsw_sp_port->dev, "Couldn't switch to trust L3\n");
 431                 return err;
 432         }
 433
 434         return 0;
 435 }
 436
 437 static int mlxsw_sp_dcbnl_ieee_setapp(struct net_device *dev,
 438                                       struct dcb_app *app)
 439 {
 440         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 441         int err;
 442
 443         err = mlxsw_sp_dcbnl_app_validate(dev, app);
 444         if (err)
 445                 return err;
 446
 447         err = dcb_ieee_setapp(dev, app);
 448         if (err)
 449                 return err;
 450
 451         err = mlxsw_sp_port_dcb_app_update(mlxsw_sp_port);
 452         if (err)
 453                 goto err_update;
 454
 455         return 0;
 456
 457 err_update:
 458         dcb_ieee_delapp(dev, app);
 459         return err;
 460 }
 461
 462 static int mlxsw_sp_dcbnl_ieee_delapp(struct net_device *dev,
 463                                       struct dcb_app *app)
 464 {
 465         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 466         int err;
 467
 468         err = dcb_ieee_delapp(dev, app);
 469         if (err)
 470                 return err;
 471
 472         err = mlxsw_sp_port_dcb_app_update(mlxsw_sp_port);
 473         if (err)
 474                 netdev_err(dev, "Failed to update DCB APP configuration\n");
 475         return 0;
 476 }
 477
 478 static int mlxsw_sp_dcbnl_ieee_getmaxrate(struct net_device *dev,
 479                                           struct ieee_maxrate *maxrate)
 480 {
 481         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 482
 483         memcpy(maxrate, mlxsw_sp_port->dcb.maxrate, sizeof(*maxrate));
 484
 485         return 0;
 486 }
 487
 488 static int mlxsw_sp_dcbnl_ieee_setmaxrate(struct net_device *dev,
 489                                           struct ieee_maxrate *maxrate)
 490 {
 491         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 492         struct ieee_maxrate *my_maxrate = mlxsw_sp_port->dcb.maxrate;
 493         int err, i;
 494
 495         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
 496                 err = mlxsw_sp_port_ets_maxrate_set(mlxsw_sp_port,
 497                                                     MLXSW_REG_QEEC_HIERARCY_SUBGROUP,
 498                                                     i, 0,
 499                                                     maxrate->tc_maxrate[i]);
 500                 if (err) {
 501                         netdev_err(dev, "Failed to set maxrate for TC %d\n", i);
 502                         goto err_port_ets_maxrate_set;
 503                 }
 504         }
 505
 506         memcpy(mlxsw_sp_port->dcb.maxrate, maxrate, sizeof(*maxrate));
 507
 508         return 0;
 509
 510 err_port_ets_maxrate_set:
 511         for (i--; i >= 0; i--)
 512                 mlxsw_sp_port_ets_maxrate_set(mlxsw_sp_port,
 513                                               MLXSW_REG_QEEC_HIERARCY_SUBGROUP,
 514                                               i, 0, my_maxrate->tc_maxrate[i]);
 515         return err;
 516 }
 517
 518 static int mlxsw_sp_port_pfc_cnt_get(struct mlxsw_sp_port *mlxsw_sp_port,
 519                                      u8 prio)
 520 {
 521         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 522         struct ieee_pfc *my_pfc = mlxsw_sp_port->dcb.pfc;
 523         char ppcnt_pl[MLXSW_REG_PPCNT_LEN];
 524         int err;
 525
 526         mlxsw_reg_ppcnt_pack(ppcnt_pl, mlxsw_sp_port->local_port,
 527                              MLXSW_REG_PPCNT_PRIO_CNT, prio);
 528         err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(ppcnt), ppcnt_pl);
 529         if (err)
 530                 return err;
 531
 532         my_pfc->requests[prio] = mlxsw_reg_ppcnt_tx_pause_get(ppcnt_pl);
 533         my_pfc->indications[prio] = mlxsw_reg_ppcnt_rx_pause_get(ppcnt_pl);
 534
 535         return 0;
 536 }
 537
 538 static int mlxsw_sp_dcbnl_ieee_getpfc(struct net_device *dev,
 539                                       struct ieee_pfc *pfc)
 540 {
 541         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 542         int err, i;
 543
 544         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
 545                 err = mlxsw_sp_port_pfc_cnt_get(mlxsw_sp_port, i);
 546                 if (err) {
 547                         netdev_err(dev, "Failed to get PFC count for priority %d\n",
 548                                    i);
 549                         return err;
 550                 }
 551         }
 552
 553         memcpy(pfc, mlxsw_sp_port->dcb.pfc, sizeof(*pfc));
 554
 555         return 0;
 556 }
 557
 558 static int mlxsw_sp_port_pfc_set(struct mlxsw_sp_port *mlxsw_sp_port,
 559                                  struct ieee_pfc *pfc)
 560 {
 561         char pfcc_pl[MLXSW_REG_PFCC_LEN];
 562
 563         mlxsw_reg_pfcc_pack(pfcc_pl, mlxsw_sp_port->local_port);
 564         mlxsw_reg_pfcc_pprx_set(pfcc_pl, mlxsw_sp_port->link.rx_pause);
 565         mlxsw_reg_pfcc_pptx_set(pfcc_pl, mlxsw_sp_port->link.tx_pause);
 566         mlxsw_reg_pfcc_prio_pack(pfcc_pl, pfc->pfc_en);
 567
 568         return mlxsw_reg_write(mlxsw_sp_port->mlxsw_sp->core, MLXSW_REG(pfcc),
 569                                pfcc_pl);
 570 }
 571
 572 static int mlxsw_sp_dcbnl_ieee_setpfc(struct net_device *dev,
 573                                       struct ieee_pfc *pfc)
 574 {
 575         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 576         bool pause_en = mlxsw_sp_port_is_pause_en(mlxsw_sp_port);
 577         int err;
 578
 579         if (pause_en && pfc->pfc_en) {
 580                 netdev_err(dev, "PAUSE frames already enabled on port\n");
 581                 return -EINVAL;
 582         }
 583
 584         err = __mlxsw_sp_port_headroom_set(mlxsw_sp_port, dev->mtu,
 585                                            mlxsw_sp_port->dcb.ets->prio_tc,
 586                                            pause_en, pfc);
 587         if (err) {
 588                 netdev_err(dev, "Failed to configure port's headroom for PFC\n");
 589                 return err;
 590         }
 591
 592         err = mlxsw_sp_port_pfc_set(mlxsw_sp_port, pfc);
 593         if (err) {
 594                 netdev_err(dev, "Failed to configure PFC\n");
 595                 goto err_port_pfc_set;
 596         }
 597
 598         memcpy(mlxsw_sp_port->dcb.pfc, pfc, sizeof(*pfc));
 599         mlxsw_sp_port->dcb.pfc->pfc_cap = IEEE_8021QAZ_MAX_TCS;
 600
 601         return 0;
 602
 603 err_port_pfc_set:
 604         __mlxsw_sp_port_headroom_set(mlxsw_sp_port, dev->mtu,
 605                                      mlxsw_sp_port->dcb.ets->prio_tc, pause_en,
 606                                      mlxsw_sp_port->dcb.pfc);
 607         return err;
 608 }
 609
 610 static const struct dcbnl_rtnl_ops mlxsw_sp_dcbnl_ops = {
 611         .ieee_getets            = mlxsw_sp_dcbnl_ieee_getets,
 612         .ieee_setets            = mlxsw_sp_dcbnl_ieee_setets,
 613         .ieee_getmaxrate        = mlxsw_sp_dcbnl_ieee_getmaxrate,
 614         .ieee_setmaxrate        = mlxsw_sp_dcbnl_ieee_setmaxrate,
 615         .ieee_getpfc            = mlxsw_sp_dcbnl_ieee_getpfc,
 616         .ieee_setpfc            = mlxsw_sp_dcbnl_ieee_setpfc,
 617         .ieee_setapp            = mlxsw_sp_dcbnl_ieee_setapp,
 618         .ieee_delapp            = mlxsw_sp_dcbnl_ieee_delapp,
 619
 620         .getdcbx                = mlxsw_sp_dcbnl_getdcbx,
 621         .setdcbx                = mlxsw_sp_dcbnl_setdcbx,
 622 };
 623
 624 static int mlxsw_sp_port_ets_init(struct mlxsw_sp_port *mlxsw_sp_port)
 625 {
 626         mlxsw_sp_port->dcb.ets = kzalloc(sizeof(*mlxsw_sp_port->dcb.ets),
 627                                          GFP_KERNEL);
 628         if (!mlxsw_sp_port->dcb.ets)
 629                 return -ENOMEM;
 630
 631         mlxsw_sp_port->dcb.ets->ets_cap = IEEE_8021QAZ_MAX_TCS;
 632
 633         return 0;
 634 }
 635
 636 static void mlxsw_sp_port_ets_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 637 {
 638         kfree(mlxsw_sp_port->dcb.ets);
 639 }
 640
 641 static int mlxsw_sp_port_maxrate_init(struct mlxsw_sp_port *mlxsw_sp_port)
 642 {
 643         int i;
 644
 645         mlxsw_sp_port->dcb.maxrate = kmalloc(sizeof(*mlxsw_sp_port->dcb.maxrate),
 646                                              GFP_KERNEL);
 647         if (!mlxsw_sp_port->dcb.maxrate)
 648                 return -ENOMEM;
 649
 650         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++)
 651                 mlxsw_sp_port->dcb.maxrate->tc_maxrate[i] = MLXSW_REG_QEEC_MAS_DIS;
 652
 653         return 0;
 654 }
 655
 656 static void mlxsw_sp_port_maxrate_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 657 {
 658         kfree(mlxsw_sp_port->dcb.maxrate);
 659 }
 660
 661 static int mlxsw_sp_port_pfc_init(struct mlxsw_sp_port *mlxsw_sp_port)
 662 {
 663         mlxsw_sp_port->dcb.pfc = kzalloc(sizeof(*mlxsw_sp_port->dcb.pfc),
 664                                          GFP_KERNEL);
 665         if (!mlxsw_sp_port->dcb.pfc)
 666                 return -ENOMEM;
 667
 668         mlxsw_sp_port->dcb.pfc->pfc_cap = IEEE_8021QAZ_MAX_TCS;
 669
 670         return 0;
 671 }
 672
 673 static void mlxsw_sp_port_pfc_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 674 {
 675         kfree(mlxsw_sp_port->dcb.pfc);
 676 }
 677
 678 int mlxsw_sp_port_dcb_init(struct mlxsw_sp_port *mlxsw_sp_port)
 679 {
 680         int err;
 681
 682         err = mlxsw_sp_port_ets_init(mlxsw_sp_port);
 683         if (err)
 684                 return err;
 685         err = mlxsw_sp_port_maxrate_init(mlxsw_sp_port);
 686         if (err)
 687                 goto err_port_maxrate_init;
 688         err = mlxsw_sp_port_pfc_init(mlxsw_sp_port);
 689         if (err)
 690                 goto err_port_pfc_init;
 691
 692         mlxsw_sp_port->dcb.trust_state = MLXSW_REG_QPTS_TRUST_STATE_PCP;
 693         mlxsw_sp_port->dev->dcbnl_ops = &mlxsw_sp_dcbnl_ops;
 694
 695         return 0;
 696
 697 err_port_pfc_init:
 698         mlxsw_sp_port_maxrate_fini(mlxsw_sp_port);
 699 err_port_maxrate_init:
 700         mlxsw_sp_port_ets_fini(mlxsw_sp_port);
 701         return err;
 702 }
 703
 704 void mlxsw_sp_port_dcb_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 705 {
 706         mlxsw_sp_port_pfc_fini(mlxsw_sp_port);
 707         mlxsw_sp_port_maxrate_fini(mlxsw_sp_port);
 708         mlxsw_sp_port_ets_fini(mlxsw_sp_port);
 709 }