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 unused 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_HR_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_HR_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_qpdp(struct mlxsw_sp_port *mlxsw_sp_port,
 373                                   u8 default_prio)
 374 {
 375         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 376         char qpdp_pl[MLXSW_REG_QPDP_LEN];
 377
 378         mlxsw_reg_qpdp_pack(qpdp_pl, mlxsw_sp_port->local_port, default_prio);
 379         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpdp), qpdp_pl);
 380 }
 381
 382 static int
 383 mlxsw_sp_port_dcb_app_update_qpdpm(struct mlxsw_sp_port *mlxsw_sp_port,
 384                                    struct dcb_ieee_app_dscp_map *map)
 385 {
 386         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 387         char qpdpm_pl[MLXSW_REG_QPDPM_LEN];
 388         short int i;
 389
 390         mlxsw_reg_qpdpm_pack(qpdpm_pl, mlxsw_sp_port->local_port);
 391         for (i = 0; i < ARRAY_SIZE(map->map); ++i)
 392                 mlxsw_reg_qpdpm_dscp_pack(qpdpm_pl, i, map->map[i]);
 393         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpdpm), qpdpm_pl);
 394 }
 395
 396 static int
 397 mlxsw_sp_port_dcb_app_update_qpdsm(struct mlxsw_sp_port *mlxsw_sp_port,
 398                                    struct dcb_ieee_app_prio_map *map)
 399 {
 400         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 401         char qpdsm_pl[MLXSW_REG_QPDSM_LEN];
 402         short int i;
 403
 404         mlxsw_reg_qpdsm_pack(qpdsm_pl, mlxsw_sp_port->local_port);
 405         for (i = 0; i < ARRAY_SIZE(map->map); ++i)
 406                 mlxsw_reg_qpdsm_prio_pack(qpdsm_pl, i, map->map[i]);
 407         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpdsm), qpdsm_pl);
 408 }
 409
 410 static int mlxsw_sp_port_dcb_app_update(struct mlxsw_sp_port *mlxsw_sp_port)
 411 {
 412         struct dcb_ieee_app_prio_map prio_map;
 413         struct dcb_ieee_app_dscp_map dscp_map;
 414         u8 default_prio;
 415         bool have_dscp;
 416         int err;
 417
 418         default_prio = mlxsw_sp_port_dcb_app_default_prio(mlxsw_sp_port);
 419         err = mlxsw_sp_port_dcb_app_update_qpdp(mlxsw_sp_port, default_prio);
 420         if (err) {
 421                 netdev_err(mlxsw_sp_port->dev, "Couldn't configure port default priority\n");
 422                 return err;
 423         }
 424
 425         have_dscp = mlxsw_sp_port_dcb_app_prio_dscp_map(mlxsw_sp_port,
 426                                                         &prio_map);
 427
 428         mlxsw_sp_port_dcb_app_dscp_prio_map(mlxsw_sp_port, default_prio,
 429                                             &dscp_map);
 430         err = mlxsw_sp_port_dcb_app_update_qpdpm(mlxsw_sp_port,
 431                                                  &dscp_map);
 432         if (err) {
 433                 netdev_err(mlxsw_sp_port->dev, "Couldn't configure priority map\n");
 434                 return err;
 435         }
 436
 437         err = mlxsw_sp_port_dcb_app_update_qpdsm(mlxsw_sp_port,
 438                                                  &prio_map);
 439         if (err) {
 440                 netdev_err(mlxsw_sp_port->dev, "Couldn't configure DSCP rewrite map\n");
 441                 return err;
 442         }
 443
 444         if (!have_dscp) {
 445                 err = mlxsw_sp_port_dcb_toggle_trust(mlxsw_sp_port,
 446                                         MLXSW_REG_QPTS_TRUST_STATE_PCP);
 447                 if (err)
 448                         netdev_err(mlxsw_sp_port->dev, "Couldn't switch to trust L2\n");
 449                 return err;
 450         }
 451
 452         err = mlxsw_sp_port_dcb_toggle_trust(mlxsw_sp_port,
 453                                              MLXSW_REG_QPTS_TRUST_STATE_DSCP);
 454         if (err) {
 455                 /* A failure to set trust DSCP means that the QPDPM and QPDSM
 456                  * maps installed above are not in effect. And since we are here
 457                  * attempting to set trust DSCP, we couldn't have attempted to
 458                  * switch trust to PCP. Thus no cleanup is necessary.
 459                  */
 460                 netdev_err(mlxsw_sp_port->dev, "Couldn't switch to trust L3\n");
 461                 return err;
 462         }
 463
 464         return 0;
 465 }
 466
 467 static int mlxsw_sp_dcbnl_ieee_setapp(struct net_device *dev,
 468                                       struct dcb_app *app)
 469 {
 470         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 471         int err;
 472
 473         err = mlxsw_sp_dcbnl_app_validate(dev, app);
 474         if (err)
 475                 return err;
 476
 477         err = dcb_ieee_setapp(dev, app);
 478         if (err)
 479                 return err;
 480
 481         err = mlxsw_sp_port_dcb_app_update(mlxsw_sp_port);
 482         if (err)
 483                 goto err_update;
 484
 485         return 0;
 486
 487 err_update:
 488         dcb_ieee_delapp(dev, app);
 489         return err;
 490 }
 491
 492 static int mlxsw_sp_dcbnl_ieee_delapp(struct net_device *dev,
 493                                       struct dcb_app *app)
 494 {
 495         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 496         int err;
 497
 498         err = dcb_ieee_delapp(dev, app);
 499         if (err)
 500                 return err;
 501
 502         err = mlxsw_sp_port_dcb_app_update(mlxsw_sp_port);
 503         if (err)
 504                 netdev_err(dev, "Failed to update DCB APP configuration\n");
 505         return 0;
 506 }
 507
 508 static int mlxsw_sp_dcbnl_ieee_getmaxrate(struct net_device *dev,
 509                                           struct ieee_maxrate *maxrate)
 510 {
 511         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 512
 513         memcpy(maxrate, mlxsw_sp_port->dcb.maxrate, sizeof(*maxrate));
 514
 515         return 0;
 516 }
 517
 518 static int mlxsw_sp_dcbnl_ieee_setmaxrate(struct net_device *dev,
 519                                           struct ieee_maxrate *maxrate)
 520 {
 521         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 522         struct ieee_maxrate *my_maxrate = mlxsw_sp_port->dcb.maxrate;
 523         int err, i;
 524
 525         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
 526                 err = mlxsw_sp_port_ets_maxrate_set(mlxsw_sp_port,
 527                                                     MLXSW_REG_QEEC_HR_SUBGROUP,
 528                                                     i, 0,
 529                                                     maxrate->tc_maxrate[i], 0);
 530                 if (err) {
 531                         netdev_err(dev, "Failed to set maxrate for TC %d\n", i);
 532                         goto err_port_ets_maxrate_set;
 533                 }
 534         }
 535
 536         memcpy(mlxsw_sp_port->dcb.maxrate, maxrate, sizeof(*maxrate));
 537
 538         return 0;
 539
 540 err_port_ets_maxrate_set:
 541         for (i--; i >= 0; i--)
 542                 mlxsw_sp_port_ets_maxrate_set(mlxsw_sp_port,
 543                                               MLXSW_REG_QEEC_HR_SUBGROUP,
 544                                               i, 0,
 545                                               my_maxrate->tc_maxrate[i], 0);
 546         return err;
 547 }
 548
 549 static int mlxsw_sp_port_pfc_cnt_get(struct mlxsw_sp_port *mlxsw_sp_port,
 550                                      u8 prio)
 551 {
 552         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 553         struct ieee_pfc *my_pfc = mlxsw_sp_port->dcb.pfc;
 554         char ppcnt_pl[MLXSW_REG_PPCNT_LEN];
 555         int err;
 556
 557         mlxsw_reg_ppcnt_pack(ppcnt_pl, mlxsw_sp_port->local_port,
 558                              MLXSW_REG_PPCNT_PRIO_CNT, prio);
 559         err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(ppcnt), ppcnt_pl);
 560         if (err)
 561                 return err;
 562
 563         my_pfc->requests[prio] = mlxsw_reg_ppcnt_tx_pause_get(ppcnt_pl);
 564         my_pfc->indications[prio] = mlxsw_reg_ppcnt_rx_pause_get(ppcnt_pl);
 565
 566         return 0;
 567 }
 568
 569 static int mlxsw_sp_dcbnl_ieee_getpfc(struct net_device *dev,
 570                                       struct ieee_pfc *pfc)
 571 {
 572         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 573         int err, i;
 574
 575         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
 576                 err = mlxsw_sp_port_pfc_cnt_get(mlxsw_sp_port, i);
 577                 if (err) {
 578                         netdev_err(dev, "Failed to get PFC count for priority %d\n",
 579                                    i);
 580                         return err;
 581                 }
 582         }
 583
 584         memcpy(pfc, mlxsw_sp_port->dcb.pfc, sizeof(*pfc));
 585
 586         return 0;
 587 }
 588
 589 static int mlxsw_sp_port_pfc_set(struct mlxsw_sp_port *mlxsw_sp_port,
 590                                  struct ieee_pfc *pfc)
 591 {
 592         char pfcc_pl[MLXSW_REG_PFCC_LEN];
 593
 594         mlxsw_reg_pfcc_pack(pfcc_pl, mlxsw_sp_port->local_port);
 595         mlxsw_reg_pfcc_pprx_set(pfcc_pl, mlxsw_sp_port->link.rx_pause);
 596         mlxsw_reg_pfcc_pptx_set(pfcc_pl, mlxsw_sp_port->link.tx_pause);
 597         mlxsw_reg_pfcc_prio_pack(pfcc_pl, pfc->pfc_en);
 598
 599         return mlxsw_reg_write(mlxsw_sp_port->mlxsw_sp->core, MLXSW_REG(pfcc),
 600                                pfcc_pl);
 601 }
 602
 603 static int mlxsw_sp_dcbnl_ieee_setpfc(struct net_device *dev,
 604                                       struct ieee_pfc *pfc)
 605 {
 606         struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 607         bool pause_en = mlxsw_sp_port_is_pause_en(mlxsw_sp_port);
 608         int err;
 609
 610         if (pause_en && pfc->pfc_en) {
 611                 netdev_err(dev, "PAUSE frames already enabled on port\n");
 612                 return -EINVAL;
 613         }
 614
 615         err = __mlxsw_sp_port_headroom_set(mlxsw_sp_port, dev->mtu,
 616                                            mlxsw_sp_port->dcb.ets->prio_tc,
 617                                            pause_en, pfc);
 618         if (err) {
 619                 netdev_err(dev, "Failed to configure port's headroom for PFC\n");
 620                 return err;
 621         }
 622
 623         err = mlxsw_sp_port_pfc_set(mlxsw_sp_port, pfc);
 624         if (err) {
 625                 netdev_err(dev, "Failed to configure PFC\n");
 626                 goto err_port_pfc_set;
 627         }
 628
 629         memcpy(mlxsw_sp_port->dcb.pfc, pfc, sizeof(*pfc));
 630         mlxsw_sp_port->dcb.pfc->pfc_cap = IEEE_8021QAZ_MAX_TCS;
 631
 632         return 0;
 633
 634 err_port_pfc_set:
 635         __mlxsw_sp_port_headroom_set(mlxsw_sp_port, dev->mtu,
 636                                      mlxsw_sp_port->dcb.ets->prio_tc, pause_en,
 637                                      mlxsw_sp_port->dcb.pfc);
 638         return err;
 639 }
 640
 641 static const struct dcbnl_rtnl_ops mlxsw_sp_dcbnl_ops = {
 642         .ieee_getets            = mlxsw_sp_dcbnl_ieee_getets,
 643         .ieee_setets            = mlxsw_sp_dcbnl_ieee_setets,
 644         .ieee_getmaxrate        = mlxsw_sp_dcbnl_ieee_getmaxrate,
 645         .ieee_setmaxrate        = mlxsw_sp_dcbnl_ieee_setmaxrate,
 646         .ieee_getpfc            = mlxsw_sp_dcbnl_ieee_getpfc,
 647         .ieee_setpfc            = mlxsw_sp_dcbnl_ieee_setpfc,
 648         .ieee_setapp            = mlxsw_sp_dcbnl_ieee_setapp,
 649         .ieee_delapp            = mlxsw_sp_dcbnl_ieee_delapp,
 650
 651         .getdcbx                = mlxsw_sp_dcbnl_getdcbx,
 652         .setdcbx                = mlxsw_sp_dcbnl_setdcbx,
 653 };
 654
 655 static int mlxsw_sp_port_ets_init(struct mlxsw_sp_port *mlxsw_sp_port)
 656 {
 657         mlxsw_sp_port->dcb.ets = kzalloc(sizeof(*mlxsw_sp_port->dcb.ets),
 658                                          GFP_KERNEL);
 659         if (!mlxsw_sp_port->dcb.ets)
 660                 return -ENOMEM;
 661
 662         mlxsw_sp_port->dcb.ets->ets_cap = IEEE_8021QAZ_MAX_TCS;
 663
 664         return 0;
 665 }
 666
 667 static void mlxsw_sp_port_ets_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 668 {
 669         kfree(mlxsw_sp_port->dcb.ets);
 670 }
 671
 672 static int mlxsw_sp_port_maxrate_init(struct mlxsw_sp_port *mlxsw_sp_port)
 673 {
 674         int i;
 675
 676         mlxsw_sp_port->dcb.maxrate = kmalloc(sizeof(*mlxsw_sp_port->dcb.maxrate),
 677                                              GFP_KERNEL);
 678         if (!mlxsw_sp_port->dcb.maxrate)
 679                 return -ENOMEM;
 680
 681         for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++)
 682                 mlxsw_sp_port->dcb.maxrate->tc_maxrate[i] = MLXSW_REG_QEEC_MAS_DIS;
 683
 684         return 0;
 685 }
 686
 687 static void mlxsw_sp_port_maxrate_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 688 {
 689         kfree(mlxsw_sp_port->dcb.maxrate);
 690 }
 691
 692 static int mlxsw_sp_port_pfc_init(struct mlxsw_sp_port *mlxsw_sp_port)
 693 {
 694         mlxsw_sp_port->dcb.pfc = kzalloc(sizeof(*mlxsw_sp_port->dcb.pfc),
 695                                          GFP_KERNEL);
 696         if (!mlxsw_sp_port->dcb.pfc)
 697                 return -ENOMEM;
 698
 699         mlxsw_sp_port->dcb.pfc->pfc_cap = IEEE_8021QAZ_MAX_TCS;
 700
 701         return 0;
 702 }
 703
 704 static void mlxsw_sp_port_pfc_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 705 {
 706         kfree(mlxsw_sp_port->dcb.pfc);
 707 }
 708
 709 int mlxsw_sp_port_dcb_init(struct mlxsw_sp_port *mlxsw_sp_port)
 710 {
 711         int err;
 712
 713         err = mlxsw_sp_port_ets_init(mlxsw_sp_port);
 714         if (err)
 715                 return err;
 716         err = mlxsw_sp_port_maxrate_init(mlxsw_sp_port);
 717         if (err)
 718                 goto err_port_maxrate_init;
 719         err = mlxsw_sp_port_pfc_init(mlxsw_sp_port);
 720         if (err)
 721                 goto err_port_pfc_init;
 722
 723         mlxsw_sp_port->dcb.trust_state = MLXSW_REG_QPTS_TRUST_STATE_PCP;
 724         mlxsw_sp_port->dev->dcbnl_ops = &mlxsw_sp_dcbnl_ops;
 725
 726         return 0;
 727
 728 err_port_pfc_init:
 729         mlxsw_sp_port_maxrate_fini(mlxsw_sp_port);
 730 err_port_maxrate_init:
 731         mlxsw_sp_port_ets_fini(mlxsw_sp_port);
 732         return err;
 733 }
 734
 735 void mlxsw_sp_port_dcb_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 736 {
 737         mlxsw_sp_port_pfc_fini(mlxsw_sp_port);
 738         mlxsw_sp_port_maxrate_fini(mlxsw_sp_port);
 739         mlxsw_sp_port_ets_fini(mlxsw_sp_port);
 740 }