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