drivers/net/dsa/microchip/ksz_dcb.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 // Copyright (c) 2024 Pengutronix, Oleksij Rempel <kernel@pengutronix.de>
   3
   4 #include <linux/dsa/ksz_common.h>
   5 #include <net/dsa.h>
   6 #include <net/dscp.h>
   7 #include <net/ieee8021q.h>
   8
   9 #include "ksz_common.h"
  10 #include "ksz_dcb.h"
  11 #include "ksz8.h"
  12
  13 #define KSZ8_REG_PORT_1_CTRL_0                  0x10
  14 #define KSZ8_PORT_DIFFSERV_ENABLE               BIT(6)
  15 #define KSZ8_PORT_802_1P_ENABLE                 BIT(5)
  16 #define KSZ8_PORT_BASED_PRIO_M                  GENMASK(4, 3)
  17
  18 #define KSZ88X3_REG_TOS_DSCP_CTRL               0x60
  19 #define KSZ8765_REG_TOS_DSCP_CTRL               0x90
  20
  21 #define KSZ9477_REG_SW_MAC_TOS_CTRL             0x033e
  22 #define KSZ9477_SW_TOS_DSCP_REMAP               BIT(0)
  23 #define KSZ9477_SW_TOS_DSCP_DEFAULT_PRIO_M      GENMASK(5, 3)
  24
  25 #define KSZ9477_REG_DIFFSERV_PRIO_MAP           0x0340
  26
  27 #define KSZ9477_REG_PORT_MRI_PRIO_CTRL          0x0801
  28 #define KSZ9477_PORT_HIGHEST_PRIO               BIT(7)
  29 #define KSZ9477_PORT_OR_PRIO                    BIT(6)
  30 #define KSZ9477_PORT_MAC_PRIO_ENABLE            BIT(4)
  31 #define KSZ9477_PORT_VLAN_PRIO_ENABLE           BIT(3)
  32 #define KSZ9477_PORT_802_1P_PRIO_ENABLE         BIT(2)
  33 #define KSZ9477_PORT_DIFFSERV_PRIO_ENABLE       BIT(1)
  34 #define KSZ9477_PORT_ACL_PRIO_ENABLE            BIT(0)
  35
  36 #define KSZ9477_REG_PORT_MRI_MAC_CTRL           0x0802
  37 #define KSZ9477_PORT_BASED_PRIO_M               GENMASK(2, 0)
  38
  39 struct ksz_apptrust_map {
  40         u8 apptrust;
  41         u8 bit;
  42 };
  43
  44 static const struct ksz_apptrust_map ksz8_apptrust_map_to_bit[] = {
  45         { DCB_APP_SEL_PCP, KSZ8_PORT_802_1P_ENABLE },
  46         { IEEE_8021QAZ_APP_SEL_DSCP, KSZ8_PORT_DIFFSERV_ENABLE },
  47 };
  48
  49 static const struct ksz_apptrust_map ksz9477_apptrust_map_to_bit[] = {
  50         { DCB_APP_SEL_PCP, KSZ9477_PORT_802_1P_PRIO_ENABLE },
  51         { IEEE_8021QAZ_APP_SEL_DSCP, KSZ9477_PORT_DIFFSERV_PRIO_ENABLE },
  52 };
  53
  54 /* ksz_supported_apptrust[] - Supported apptrust selectors and Priority Order
  55  *                            of Internal Priority Map (IPM) sources.
  56  *
  57  * This array defines the apptrust selectors supported by the hardware, where
  58  * the index within the array indicates the priority of the selector - lower
  59  * indices correspond to higher priority. This fixed priority scheme is due to
  60  * the hardware's design, which does not support configurable priority among
  61  * different priority sources.
  62  *
  63  * The priority sources, including Tail Tag, ACL, VLAN PCP and DSCP are ordered
  64  * by the hardware's fixed logic, as detailed below. The order reflects a
  65  * non-configurable precedence where certain types of priority information
  66  * override others:
  67  *
  68  * 1. Tail Tag - Highest priority, overrides ACL, VLAN PCP, and DSCP priorities.
  69  * 2. ACL - Overrides VLAN PCP and DSCP priorities.
  70  * 3. VLAN PCP - Overrides DSCP priority.
  71  * 4. DSCP - Lowest priority, does not override any other priority source.
  72  *
  73  * In this context, the array's lower index (higher priority) for
  74  * 'DCB_APP_SEL_PCP' suggests its relative priority over
  75  * 'IEEE_8021QAZ_APP_SEL_DSCP' within the system's fixed priority scheme.
  76  *
  77  * DCB_APP_SEL_PCP - Priority Code Point selector
  78  * IEEE_8021QAZ_APP_SEL_DSCP - Differentiated Services Code Point selector
  79  */
  80 static const u8 ksz_supported_apptrust[] = {
  81         DCB_APP_SEL_PCP,
  82         IEEE_8021QAZ_APP_SEL_DSCP,
  83 };
  84
  85 static const char * const ksz_supported_apptrust_variants[] = {
  86         "empty", "dscp", "pcp", "dscp pcp"
  87 };
  88
  89 static void ksz_get_default_port_prio_reg(struct ksz_device *dev, int *reg,
  90                                           u8 *mask, int *shift)
  91 {
  92         if (is_ksz8(dev)) {
  93                 *reg = KSZ8_REG_PORT_1_CTRL_0;
  94                 *mask = KSZ8_PORT_BASED_PRIO_M;
  95                 *shift = __bf_shf(KSZ8_PORT_BASED_PRIO_M);
  96         } else {
  97                 *reg = KSZ9477_REG_PORT_MRI_MAC_CTRL;
  98                 *mask = KSZ9477_PORT_BASED_PRIO_M;
  99                 *shift = __bf_shf(KSZ9477_PORT_BASED_PRIO_M);
 100         }
 101 }
 102
 103 /**
 104  * ksz_get_dscp_prio_reg - Retrieves the DSCP-to-priority-mapping register
 105  * @dev: Pointer to the KSZ switch device structure
 106  * @reg: Pointer to the register address to be set
 107  * @per_reg: Pointer to the number of DSCP values per register
 108  * @mask: Pointer to the mask to be set
 109  *
 110  * This function retrieves the DSCP to priority mapping register, the number of
 111  * DSCP values per register, and the mask to be set.
 112  */
 113 static void ksz_get_dscp_prio_reg(struct ksz_device *dev, int *reg,
 114                                   int *per_reg, u8 *mask)
 115 {
 116         if (ksz_is_ksz87xx(dev) || ksz_is_8895_family(dev)) {
 117                 *reg = KSZ8765_REG_TOS_DSCP_CTRL;
 118                 *per_reg = 4;
 119                 *mask = GENMASK(1, 0);
 120         } else if (ksz_is_ksz88x3(dev)) {
 121                 *reg = KSZ88X3_REG_TOS_DSCP_CTRL;
 122                 *per_reg = 4;
 123                 *mask = GENMASK(1, 0);
 124         } else {
 125                 *reg = KSZ9477_REG_DIFFSERV_PRIO_MAP;
 126                 *per_reg = 2;
 127                 *mask = GENMASK(2, 0);
 128         }
 129 }
 130
 131 /**
 132  * ksz_get_apptrust_map_and_reg - Retrieves the apptrust map and register
 133  * @dev: Pointer to the KSZ switch device structure
 134  * @map: Pointer to the apptrust map to be set
 135  * @reg: Pointer to the register address to be set
 136  * @mask: Pointer to the mask to be set
 137  *
 138  * This function retrieves the apptrust map and register address for the
 139  * apptrust configuration.
 140  */
 141 static void ksz_get_apptrust_map_and_reg(struct ksz_device *dev,
 142                                          const struct ksz_apptrust_map **map,
 143                                          int *reg, u8 *mask)
 144 {
 145         if (is_ksz8(dev)) {
 146                 *map = ksz8_apptrust_map_to_bit;
 147                 *reg = KSZ8_REG_PORT_1_CTRL_0;
 148                 *mask = KSZ8_PORT_DIFFSERV_ENABLE | KSZ8_PORT_802_1P_ENABLE;
 149         } else {
 150                 *map = ksz9477_apptrust_map_to_bit;
 151                 *reg = KSZ9477_REG_PORT_MRI_PRIO_CTRL;
 152                 *mask = KSZ9477_PORT_802_1P_PRIO_ENABLE |
 153                         KSZ9477_PORT_DIFFSERV_PRIO_ENABLE;
 154         }
 155 }
 156
 157 /**
 158  * ksz_port_get_default_prio - Retrieves the default priority for a port on a
 159  *                             KSZ switch
 160  * @ds: Pointer to the DSA switch structure
 161  * @port: Port number from which to get the default priority
 162  *
 163  * This function fetches the default priority for the specified port on a KSZ
 164  * switch.
 165  *
 166  * Return: The default priority of the port on success, or a negative error
 167  * code on failure.
 168  */
 169 int ksz_port_get_default_prio(struct dsa_switch *ds, int port)
 170 {
 171         struct ksz_device *dev = ds->priv;
 172         int ret, reg, shift;
 173         u8 data, mask;
 174
 175         ksz_get_default_port_prio_reg(dev, &reg, &mask, &shift);
 176
 177         ret = ksz_pread8(dev, port, reg, &data);
 178         if (ret)
 179                 return ret;
 180
 181         return (data & mask) >> shift;
 182 }
 183
 184 /**
 185  * ksz88x3_port_set_default_prio_quirks - Quirks for default priority
 186  * @dev: Pointer to the KSZ switch device structure
 187  * @port: Port number for which to set the default priority
 188  * @prio: Priority value to set
 189  *
 190  * This function implements quirks for setting the default priority on KSZ88x3
 191  * devices. On Port 2, no other priority providers are working
 192  * except of PCP. So, configuring default priority on Port 2 is not possible.
 193  * On Port 1, it is not possible to configure port priority if PCP
 194  * apptrust on Port 2 is disabled. Since we disable multiple queues on the
 195  * switch to disable PCP on Port 2, we need to ensure that the default priority
 196  * configuration on Port 1 is in agreement with the configuration on Port 2.
 197  *
 198  * Return: 0 on success, or a negative error code on failure
 199  */
 200 static int ksz88x3_port_set_default_prio_quirks(struct ksz_device *dev, int port,
 201                                                 u8 prio)
 202 {
 203         if (!prio)
 204                 return 0;
 205
 206         if (port == KSZ_PORT_2) {
 207                 dev_err(dev->dev, "Port priority configuration is not working on Port 2\n");
 208                 return -EINVAL;
 209         } else if (port == KSZ_PORT_1) {
 210                 u8 port2_data;
 211                 int ret;
 212
 213                 ret = ksz_pread8(dev, KSZ_PORT_2, KSZ8_REG_PORT_1_CTRL_0,
 214                                  &port2_data);
 215                 if (ret)
 216                         return ret;
 217
 218                 if (!(port2_data & KSZ8_PORT_802_1P_ENABLE)) {
 219                         dev_err(dev->dev, "Not possible to configure port priority on Port 1 if PCP apptrust on Port 2 is disabled\n");
 220                         return -EINVAL;
 221                 }
 222         }
 223
 224         return 0;
 225 }
 226
 227 /**
 228  * ksz_port_set_default_prio - Sets the default priority for a port on a KSZ
 229  *                             switch
 230  * @ds: Pointer to the DSA switch structure
 231  * @port: Port number for which to set the default priority
 232  * @prio: Priority value to set
 233  *
 234  * This function sets the default priority for the specified port on a KSZ
 235  * switch.
 236  *
 237  * Return: 0 on success, or a negative error code on failure.
 238  */
 239 int ksz_port_set_default_prio(struct dsa_switch *ds, int port, u8 prio)
 240 {
 241         struct ksz_device *dev = ds->priv;
 242         int reg, shift, ret;
 243         u8 mask;
 244
 245         if (prio >= dev->info->num_ipms)
 246                 return -EINVAL;
 247
 248         if (ksz_is_ksz88x3(dev)) {
 249                 ret = ksz88x3_port_set_default_prio_quirks(dev, port, prio);
 250                 if (ret)
 251                         return ret;
 252         }
 253
 254         ksz_get_default_port_prio_reg(dev, &reg, &mask, &shift);
 255
 256         return ksz_prmw8(dev, port, reg, mask, (prio << shift) & mask);
 257 }
 258
 259 /**
 260  * ksz_port_get_dscp_prio - Retrieves the priority for a DSCP value on a KSZ
 261  *                          switch
 262  * @ds: Pointer to the DSA switch structure
 263  * @port: Port number for which to get the priority
 264  * @dscp: DSCP value for which to get the priority
 265  *
 266  * This function fetches the priority value from switch global DSCP-to-priorty
 267  * mapping table for the specified DSCP value.
 268  *
 269  * Return: The priority value for the DSCP on success, or a negative error
 270  * code on failure.
 271  */
 272 int ksz_port_get_dscp_prio(struct dsa_switch *ds, int port, u8 dscp)
 273 {
 274         struct ksz_device *dev = ds->priv;
 275         int reg, per_reg, ret, shift;
 276         u8 data, mask;
 277
 278         ksz_get_dscp_prio_reg(dev, &reg, &per_reg, &mask);
 279
 280         /* If DSCP remapping is disabled, DSCP bits 3-5 are used as Internal
 281          * Priority Map (IPM)
 282          */
 283         if (!is_ksz8(dev)) {
 284                 ret = ksz_read8(dev, KSZ9477_REG_SW_MAC_TOS_CTRL, &data);
 285                 if (ret)
 286                         return ret;
 287
 288                 /* If DSCP remapping is disabled, DSCP bits 3-5 are used as
 289                  * Internal Priority Map (IPM)
 290                  */
 291                 if (!(data & KSZ9477_SW_TOS_DSCP_REMAP))
 292                         return FIELD_GET(KSZ9477_SW_TOS_DSCP_DEFAULT_PRIO_M,
 293                                          dscp);
 294         }
 295
 296         /* In case DSCP remapping is enabled, we need to write the DSCP to
 297          * priority mapping table.
 298          */
 299         reg += dscp / per_reg;
 300         ret = ksz_read8(dev, reg, &data);
 301         if (ret)
 302                 return ret;
 303
 304         shift = (dscp % per_reg) * (8 / per_reg);
 305
 306         return (data >> shift) & mask;
 307 }
 308
 309 /**
 310  * ksz_set_global_dscp_entry - Sets the global DSCP-to-priority mapping entry
 311  * @dev: Pointer to the KSZ switch device structure
 312  * @dscp: DSCP value for which to set the priority
 313  * @ipm: Priority value to set
 314  *
 315  * This function sets the global DSCP-to-priority mapping entry for the
 316  * specified DSCP value.
 317  *
 318  * Return: 0 on success, or a negative error code on failure.
 319  */
 320 static int ksz_set_global_dscp_entry(struct ksz_device *dev, u8 dscp, u8 ipm)
 321 {
 322         int reg, per_reg, shift;
 323         u8 mask;
 324
 325         ksz_get_dscp_prio_reg(dev, &reg, &per_reg, &mask);
 326
 327         shift = (dscp % per_reg) * (8 / per_reg);
 328
 329         return ksz_rmw8(dev, reg + (dscp / per_reg), mask << shift,
 330                         ipm << shift);
 331 }
 332
 333 /**
 334  * ksz_init_global_dscp_map - Initializes the global DSCP-to-priority mapping
 335  * @dev: Pointer to the KSZ switch device structure
 336  *
 337  * This function initializes the global DSCP-to-priority mapping table for the
 338  * switch.
 339  *
 340  * Return: 0 on success, or a negative error code on failure
 341  */
 342 static int ksz_init_global_dscp_map(struct ksz_device *dev)
 343 {
 344         int ret, dscp;
 345
 346         /* On KSZ9xxx variants, DSCP remapping is disabled by default.
 347          * Enable to have, predictable and reproducible behavior across
 348          * different devices.
 349          */
 350         if (!is_ksz8(dev)) {
 351                 ret = ksz_rmw8(dev, KSZ9477_REG_SW_MAC_TOS_CTRL,
 352                                KSZ9477_SW_TOS_DSCP_REMAP,
 353                                KSZ9477_SW_TOS_DSCP_REMAP);
 354                 if (ret)
 355                         return ret;
 356         }
 357
 358         for (dscp = 0; dscp < DSCP_MAX; dscp++) {
 359                 int ipm, tt;
 360
 361                 /* Map DSCP to Traffic Type, which is corresponding to the
 362                  * Internal Priority Map (IPM) in the switch.
 363                  */
 364                 if (!is_ksz8(dev)) {
 365                         ipm = ietf_dscp_to_ieee8021q_tt(dscp);
 366                 } else {
 367                         /* On KSZ8xxx variants we do not have IPM to queue
 368                          * remapping table. We need to convert DSCP to Traffic
 369                          * Type and then to queue.
 370                          */
 371                         tt = ietf_dscp_to_ieee8021q_tt(dscp);
 372                         if (tt < 0)
 373                                 return tt;
 374
 375                         ipm = ieee8021q_tt_to_tc(tt, dev->info->num_tx_queues);
 376                 }
 377
 378                 if (ipm < 0)
 379                         return ipm;
 380
 381                 ret = ksz_set_global_dscp_entry(dev, dscp, ipm);
 382         }
 383
 384         return 0;
 385 }
 386
 387 /**
 388  * ksz_port_add_dscp_prio - Adds a DSCP-to-priority mapping entry for a port on
 389  *                          a KSZ switch.
 390  * @ds: Pointer to the DSA switch structure
 391  * @port: Port number for which to add the DSCP-to-priority mapping entry
 392  * @dscp: DSCP value for which to add the priority
 393  * @prio: Priority value to set
 394  *
 395  * Return: 0 on success, or a negative error code on failure
 396  */
 397 int ksz_port_add_dscp_prio(struct dsa_switch *ds, int port, u8 dscp, u8 prio)
 398 {
 399         struct ksz_device *dev = ds->priv;
 400
 401         if (prio >= dev->info->num_ipms)
 402                 return -ERANGE;
 403
 404         return ksz_set_global_dscp_entry(dev, dscp, prio);
 405 }
 406
 407 /**
 408  * ksz_port_del_dscp_prio - Deletes a DSCP-to-priority mapping entry for a port
 409  *                          on a KSZ switch.
 410  * @ds: Pointer to the DSA switch structure
 411  * @port: Port number for which to delete the DSCP-to-priority mapping entry
 412  * @dscp: DSCP value for which to delete the priority
 413  * @prio: Priority value to delete
 414  *
 415  * Return: 0 on success, or a negative error code on failure
 416  */
 417 int ksz_port_del_dscp_prio(struct dsa_switch *ds, int port, u8 dscp, u8 prio)
 418 {
 419         struct ksz_device *dev = ds->priv;
 420         int ipm;
 421
 422         if (ksz_port_get_dscp_prio(ds, port, dscp) != prio)
 423                 return 0;
 424
 425         if (is_ksz8(dev)) {
 426                 ipm = ieee8021q_tt_to_tc(IEEE8021Q_TT_BE,
 427                                          dev->info->num_tx_queues);
 428                 if (ipm < 0)
 429                         return ipm;
 430         } else {
 431                 ipm = IEEE8021Q_TT_BE;
 432         }
 433
 434         return ksz_set_global_dscp_entry(dev, dscp, ipm);
 435 }
 436
 437 /**
 438  * ksz_apptrust_error - Prints an error message for an invalid apptrust selector
 439  * @dev: Pointer to the KSZ switch device structure
 440  *
 441  * This function prints an error message when an invalid apptrust selector is
 442  * provided.
 443  */
 444 static void ksz_apptrust_error(struct ksz_device *dev)
 445 {
 446         char supported_apptrust_variants[64];
 447         int i;
 448
 449         supported_apptrust_variants[0] = '\0';
 450         for (i = 0; i < ARRAY_SIZE(ksz_supported_apptrust_variants); i++) {
 451                 if (i > 0)
 452                         strlcat(supported_apptrust_variants, ", ",
 453                                 sizeof(supported_apptrust_variants));
 454                 strlcat(supported_apptrust_variants,
 455                         ksz_supported_apptrust_variants[i],
 456                         sizeof(supported_apptrust_variants));
 457         }
 458
 459         dev_err(dev->dev, "Invalid apptrust selector or priority order. Supported: %s\n",
 460                 supported_apptrust_variants);
 461 }
 462
 463 /**
 464  * ksz_port_set_apptrust_validate - Validates the apptrust selectors
 465  * @dev: Pointer to the KSZ switch device structure
 466  * @port: Port number for which to set the apptrust selectors
 467  * @sel: Array of apptrust selectors to validate
 468  * @nsel: Number of apptrust selectors in the array
 469  *
 470  * This function validates the apptrust selectors provided and ensures that
 471  * they are in the correct order.
 472  *
 473  * This family of switches supports two apptrust selectors: DCB_APP_SEL_PCP and
 474  * IEEE_8021QAZ_APP_SEL_DSCP. The priority order of the selectors is fixed and
 475  * cannot be changed. The order is as follows:
 476  * 1. DCB_APP_SEL_PCP - Priority Code Point selector (highest priority)
 477  * 2. IEEE_8021QAZ_APP_SEL_DSCP - Differentiated Services Code Point selector
 478  *   (lowest priority)
 479  *
 480  * Return: 0 on success, or a negative error code on failure
 481  */
 482 static int ksz_port_set_apptrust_validate(struct ksz_device *dev, int port,
 483                                           const u8 *sel, int nsel)
 484 {
 485         int i, j, found;
 486         int j_prev = 0;
 487
 488         /* Iterate through the requested selectors */
 489         for (i = 0; i < nsel; i++) {
 490                 found = 0;
 491
 492                 /* Check if the current selector is supported by the hardware */
 493                 for (j = 0; j < sizeof(ksz_supported_apptrust); j++) {
 494                         if (sel[i] != ksz_supported_apptrust[j])
 495                                 continue;
 496
 497                         found = 1;
 498
 499                         /* Ensure that no higher priority selector (lower index)
 500                          * precedes a lower priority one
 501                          */
 502                         if (i > 0 && j <= j_prev)
 503                                 goto err_sel_not_vaild;
 504
 505                         j_prev = j;
 506                         break;
 507                 }
 508
 509                 if (!found)
 510                         goto err_sel_not_vaild;
 511         }
 512
 513         return 0;
 514
 515 err_sel_not_vaild:
 516         ksz_apptrust_error(dev);
 517
 518         return -EINVAL;
 519 }
 520
 521 /**
 522  * ksz88x3_port1_apptrust_quirk - Quirk for apptrust configuration on Port 1
 523  *                                of KSZ88x3 devices
 524  * @dev: Pointer to the KSZ switch device structure
 525  * @port: Port number for which to set the apptrust selectors
 526  * @reg: Register address for the apptrust configuration
 527  * @port1_data: Data to set for the apptrust configuration
 528  *
 529  * This function implements a quirk for apptrust configuration on Port 1 of
 530  * KSZ88x3 devices. It ensures that apptrust configuration on Port 1 is not
 531  * possible if PCP apptrust on Port 2 is disabled. This is because the Port 2
 532  * seems to be permanently hardwired to PCP classification, so we need to
 533  * do Port 1 configuration always in agreement with Port 2 configuration.
 534  *
 535  * Return: 0 on success, or a negative error code on failure
 536  */
 537 static int ksz88x3_port1_apptrust_quirk(struct ksz_device *dev, int port,
 538                                         int reg, u8 port1_data)
 539 {
 540         u8 port2_data;
 541         int ret;
 542
 543         /* If no apptrust is requested for Port 1, no need to care about Port 2
 544          * configuration.
 545          */
 546         if (!(port1_data & (KSZ8_PORT_802_1P_ENABLE | KSZ8_PORT_DIFFSERV_ENABLE)))
 547                 return 0;
 548
 549         /* We got request to enable any apptrust on Port 1. To make it possible,
 550          * we need to enable multiple queues on the switch. If we enable
 551          * multiqueue support, PCP classification on Port 2 will be
 552          * automatically activated by HW.
 553          */
 554         ret = ksz_pread8(dev, KSZ_PORT_2, reg, &port2_data);
 555         if (ret)
 556                 return ret;
 557
 558         /* If KSZ8_PORT_802_1P_ENABLE bit is set on Port 2, the driver showed
 559          * the interest in PCP classification on Port 2. In this case,
 560          * multiqueue support is enabled and we can enable any apptrust on
 561          * Port 1.
 562          * If KSZ8_PORT_802_1P_ENABLE bit is not set on Port 2, the PCP
 563          * classification on Port 2 is still active, but the driver disabled
 564          * multiqueue support and made frame prioritization inactive for
 565          * all ports. In this case, we can't enable any apptrust on Port 1.
 566          */
 567         if (!(port2_data & KSZ8_PORT_802_1P_ENABLE)) {
 568                 dev_err(dev->dev, "Not possible to enable any apptrust on Port 1 if PCP apptrust on Port 2 is disabled\n");
 569                 return -EINVAL;
 570         }
 571
 572         return 0;
 573 }
 574
 575 /**
 576  * ksz88x3_port2_apptrust_quirk - Quirk for apptrust configuration on Port 2
 577  *                                of KSZ88x3 devices
 578  * @dev: Pointer to the KSZ switch device structure
 579  * @port: Port number for which to set the apptrust selectors
 580  * @reg: Register address for the apptrust configuration
 581  * @port2_data: Data to set for the apptrust configuration
 582  *
 583  * This function implements a quirk for apptrust configuration on Port 2 of
 584  * KSZ88x3 devices. It ensures that DSCP apptrust is not working on Port 2 and
 585  * that it is not possible to disable PCP on Port 2. The only way to disable PCP
 586  * on Port 2 is to disable multiple queues on the switch.
 587  *
 588  * Return: 0 on success, or a negative error code on failure
 589  */
 590 static int ksz88x3_port2_apptrust_quirk(struct ksz_device *dev, int port,
 591                                         int reg, u8 port2_data)
 592 {
 593         struct dsa_switch *ds = dev->ds;
 594         u8 port1_data;
 595         int ret;
 596
 597         /* First validate Port 2 configuration. DiffServ/DSCP is not working
 598          * on this port.
 599          */
 600         if (port2_data & KSZ8_PORT_DIFFSERV_ENABLE) {
 601                 dev_err(dev->dev, "DSCP apptrust is not working on Port 2\n");
 602                 return -EINVAL;
 603         }
 604
 605         /* If PCP support is requested, we need to enable all queues on the
 606          * switch to make PCP priority working on Port 2.
 607          */
 608         if (port2_data & KSZ8_PORT_802_1P_ENABLE)
 609                 return ksz8_all_queues_split(dev, dev->info->num_tx_queues);
 610
 611         /* We got request to disable PCP priority on Port 2.
 612          * Now, we need to compare Port 2 configuration with Port 1
 613          * configuration.
 614          */
 615         ret = ksz_pread8(dev, KSZ_PORT_1, reg, &port1_data);
 616         if (ret)
 617                 return ret;
 618
 619         /* If Port 1 has any apptrust enabled, we can't disable multiple queues
 620          * on the switch, so we can't disable PCP on Port 2.
 621          */
 622         if (port1_data & (KSZ8_PORT_802_1P_ENABLE | KSZ8_PORT_DIFFSERV_ENABLE)) {
 623                 dev_err(dev->dev, "Not possible to disable PCP on Port 2 if any apptrust is enabled on Port 1\n");
 624                 return -EINVAL;
 625         }
 626
 627         /* Now we need to ensure that default priority on Port 1 is set to 0
 628          * otherwise we can't disable multiqueue support on the switch.
 629          */
 630         ret = ksz_port_get_default_prio(ds, KSZ_PORT_1);
 631         if (ret < 0) {
 632                 return ret;
 633         } else if (ret) {
 634                 dev_err(dev->dev, "Not possible to disable PCP on Port 2 if non zero default priority is set on Port 1\n");
 635                 return -EINVAL;
 636         }
 637
 638         /* Port 1 has no apptrust or default priority set and we got request to
 639          * disable PCP on Port 2. We can disable multiqueue support to disable
 640          * PCP on Port 2.
 641          */
 642         return ksz8_all_queues_split(dev, 1);
 643 }
 644
 645 /**
 646  * ksz88x3_port_apptrust_quirk - Quirk for apptrust configuration on KSZ88x3
 647  *                             devices
 648  * @dev: Pointer to the KSZ switch device structure
 649  * @port: Port number for which to set the apptrust selectors
 650  * @reg: Register address for the apptrust configuration
 651  * @data: Data to set for the apptrust configuration
 652  *
 653  * This function implements a quirk for apptrust configuration on KSZ88x3
 654  * devices. It ensures that apptrust configuration on Port 1 and
 655  * Port 2 is done in agreement with each other.
 656  *
 657  * Return: 0 on success, or a negative error code on failure
 658  */
 659 static int ksz88x3_port_apptrust_quirk(struct ksz_device *dev, int port,
 660                                        int reg, u8 data)
 661 {
 662         if (port == KSZ_PORT_1)
 663                 return ksz88x3_port1_apptrust_quirk(dev, port, reg, data);
 664         else if (port == KSZ_PORT_2)
 665                 return ksz88x3_port2_apptrust_quirk(dev, port, reg, data);
 666
 667         return 0;
 668 }
 669
 670 /**
 671  * ksz_port_set_apptrust - Sets the apptrust selectors for a port on a KSZ
 672  *                         switch
 673  * @ds: Pointer to the DSA switch structure
 674  * @port: Port number for which to set the apptrust selectors
 675  * @sel: Array of apptrust selectors to set
 676  * @nsel: Number of apptrust selectors in the array
 677  *
 678  * This function sets the apptrust selectors for the specified port on a KSZ
 679  * switch.
 680  *
 681  * Return: 0 on success, or a negative error code on failure
 682  */
 683 int ksz_port_set_apptrust(struct dsa_switch *ds, int port,
 684                           const u8 *sel, int nsel)
 685 {
 686         const struct ksz_apptrust_map *map;
 687         struct ksz_device *dev = ds->priv;
 688         int reg, i, ret;
 689         u8 data = 0;
 690         u8 mask;
 691
 692         ret = ksz_port_set_apptrust_validate(dev, port, sel, nsel);
 693         if (ret)
 694                 return ret;
 695
 696         ksz_get_apptrust_map_and_reg(dev, &map, &reg, &mask);
 697
 698         for (i = 0; i < nsel; i++) {
 699                 int j;
 700
 701                 for (j = 0; j < ARRAY_SIZE(ksz_supported_apptrust); j++) {
 702                         if (sel[i] != ksz_supported_apptrust[j])
 703                                 continue;
 704
 705                         data |= map[j].bit;
 706                         break;
 707                 }
 708         }
 709
 710         if (ksz_is_ksz88x3(dev)) {
 711                 ret = ksz88x3_port_apptrust_quirk(dev, port, reg, data);
 712                 if (ret)
 713                         return ret;
 714         }
 715
 716         return ksz_prmw8(dev, port, reg, mask, data);
 717 }
 718
 719 /**
 720  * ksz_port_get_apptrust - Retrieves the apptrust selectors for a port on a KSZ
 721  *                         switch
 722  * @ds: Pointer to the DSA switch structure
 723  * @port: Port number for which to get the apptrust selectors
 724  * @sel: Array to store the apptrust selectors
 725  * @nsel: Number of apptrust selectors in the array
 726  *
 727  * This function fetches the apptrust selectors for the specified port on a KSZ
 728  * switch.
 729  *
 730  * Return: 0 on success, or a negative error code on failure
 731  */
 732 int ksz_port_get_apptrust(struct dsa_switch *ds, int port, u8 *sel, int *nsel)
 733 {
 734         const struct ksz_apptrust_map *map;
 735         struct ksz_device *dev = ds->priv;
 736         int reg, i, ret;
 737         u8 data;
 738         u8 mask;
 739
 740         ksz_get_apptrust_map_and_reg(dev, &map, &reg, &mask);
 741
 742         ret = ksz_pread8(dev, port, reg, &data);
 743         if (ret)
 744                 return ret;
 745
 746         *nsel = 0;
 747         for (i = 0; i < ARRAY_SIZE(ksz_supported_apptrust); i++) {
 748                 if (data & map[i].bit)
 749                         sel[(*nsel)++] = ksz_supported_apptrust[i];
 750         }
 751
 752         return 0;
 753 }
 754
 755 /**
 756  * ksz_dcb_init_port - Initializes the DCB configuration for a port on a KSZ
 757  * @dev: Pointer to the KSZ switch device structure
 758  * @port: Port number for which to initialize the DCB configuration
 759  *
 760  * This function initializes the DCB configuration for the specified port on a
 761  * KSZ switch. Particular DCB configuration is set for the port, including the
 762  * default priority and apptrust selectors.
 763  * The default priority is set to Best Effort, and the apptrust selectors are
 764  * set to all supported selectors.
 765  *
 766  * Return: 0 on success, or a negative error code on failure
 767  */
 768 int ksz_dcb_init_port(struct ksz_device *dev, int port)
 769 {
 770         const u8 ksz_default_apptrust[] = { DCB_APP_SEL_PCP };
 771         int ret, ipm;
 772
 773         if (is_ksz8(dev)) {
 774                 ipm = ieee8021q_tt_to_tc(IEEE8021Q_TT_BE,
 775                                          dev->info->num_tx_queues);
 776                 if (ipm < 0)
 777                         return ipm;
 778         } else {
 779                 ipm = IEEE8021Q_TT_BE;
 780         }
 781
 782         /* Set the default priority for the port to Best Effort */
 783         ret = ksz_port_set_default_prio(dev->ds, port, ipm);
 784         if (ret)
 785                 return ret;
 786
 787         return ksz_port_set_apptrust(dev->ds, port, ksz_default_apptrust,
 788                                      ARRAY_SIZE(ksz_default_apptrust));
 789 }
 790
 791 /**
 792  * ksz_dcb_init - Initializes the DCB configuration for a KSZ switch
 793  * @dev: Pointer to the KSZ switch device structure
 794  *
 795  * This function initializes the DCB configuration for a KSZ switch. The global
 796  * DSCP-to-priority mapping table is initialized.
 797  *
 798  * Return: 0 on success, or a negative error code on failure
 799  */
 800 int ksz_dcb_init(struct ksz_device *dev)
 801 {
 802         int ret;
 803
 804         ret = ksz_init_global_dscp_map(dev);
 805         if (ret)
 806                 return ret;
 807
 808         /* Enable 802.1p priority control on Port 2 during switch initialization.
 809          * This setup is critical for the apptrust functionality on Port 1, which
 810          * relies on the priority settings of Port 2. Note: Port 1 is naturally
 811          * configured before Port 2, necessitating this configuration order.
 812          */
 813         if (ksz_is_ksz88x3(dev))
 814                 return ksz_prmw8(dev, KSZ_PORT_2, KSZ8_REG_PORT_1_CTRL_0,
 815                                  KSZ8_PORT_802_1P_ENABLE,
 816                                  KSZ8_PORT_802_1P_ENABLE);
 817
 818         return 0;
 819 }