drivers/net/ethernet/microchip/lan966x/lan966x_dcb.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2
   3 #include "lan966x_main.h"
   4
   5 enum lan966x_dcb_apptrust_values {
   6         LAN966X_DCB_APPTRUST_EMPTY,
   7         LAN966X_DCB_APPTRUST_DSCP,
   8         LAN966X_DCB_APPTRUST_PCP,
   9         LAN966X_DCB_APPTRUST_DSCP_PCP,
  10         __LAN966X_DCB_APPTRUST_MAX
  11 };
  12
  13 static const struct lan966x_dcb_apptrust {
  14         u8 selectors[IEEE_8021QAZ_APP_SEL_MAX + 1];
  15         int nselectors;
  16 } *lan966x_port_apptrust[NUM_PHYS_PORTS];
  17
  18 static const char *lan966x_dcb_apptrust_names[__LAN966X_DCB_APPTRUST_MAX] = {
  19         [LAN966X_DCB_APPTRUST_EMPTY]    = "empty",
  20         [LAN966X_DCB_APPTRUST_DSCP]     = "dscp",
  21         [LAN966X_DCB_APPTRUST_PCP]      = "pcp",
  22         [LAN966X_DCB_APPTRUST_DSCP_PCP] = "dscp pcp"
  23 };
  24
  25 /* Lan966x supported apptrust policies */
  26 static const struct lan966x_dcb_apptrust
  27         lan966x_dcb_apptrust_policies[__LAN966X_DCB_APPTRUST_MAX] = {
  28         /* Empty *must* be first */
  29         [LAN966X_DCB_APPTRUST_EMPTY]    = { { 0 }, 0 },
  30         [LAN966X_DCB_APPTRUST_DSCP]     = { { IEEE_8021QAZ_APP_SEL_DSCP }, 1 },
  31         [LAN966X_DCB_APPTRUST_PCP]      = { { DCB_APP_SEL_PCP }, 1 },
  32         [LAN966X_DCB_APPTRUST_DSCP_PCP] = { { IEEE_8021QAZ_APP_SEL_DSCP,
  33                                               DCB_APP_SEL_PCP }, 2 },
  34 };
  35
  36 static bool lan966x_dcb_apptrust_contains(int portno, u8 selector)
  37 {
  38         const struct lan966x_dcb_apptrust *conf = lan966x_port_apptrust[portno];
  39
  40         for (int i = 0; i < conf->nselectors; i++)
  41                 if (conf->selectors[i] == selector)
  42                         return true;
  43
  44         return false;
  45 }
  46
  47 static void lan966x_dcb_app_update(struct net_device *dev)
  48 {
  49         struct dcb_ieee_app_prio_map dscp_rewr_map = {0};
  50         struct dcb_rewr_prio_pcp_map pcp_rewr_map = {0};
  51         struct lan966x_port *port = netdev_priv(dev);
  52         struct lan966x_port_qos qos = {0};
  53         struct dcb_app app_itr;
  54         bool dscp_rewr = false;
  55         bool pcp_rewr = false;
  56
  57         /* Get pcp ingress mapping */
  58         for (int i = 0; i < ARRAY_SIZE(qos.pcp.map); i++) {
  59                 app_itr.selector = DCB_APP_SEL_PCP;
  60                 app_itr.protocol = i;
  61                 qos.pcp.map[i] = dcb_getapp(dev, &app_itr);
  62         }
  63
  64         /* Get dscp ingress mapping */
  65         for (int i = 0; i < ARRAY_SIZE(qos.dscp.map); i++) {
  66                 app_itr.selector = IEEE_8021QAZ_APP_SEL_DSCP;
  67                 app_itr.protocol = i;
  68                 qos.dscp.map[i] = dcb_getapp(dev, &app_itr);
  69         }
  70
  71         /* Get default prio */
  72         qos.default_prio = dcb_ieee_getapp_default_prio_mask(dev);
  73         if (qos.default_prio)
  74                 qos.default_prio = fls(qos.default_prio) - 1;
  75
  76         /* Get pcp rewrite mapping */
  77         dcb_getrewr_prio_pcp_mask_map(dev, &pcp_rewr_map);
  78         for (int i = 0; i < ARRAY_SIZE(pcp_rewr_map.map); i++) {
  79                 if (!pcp_rewr_map.map[i])
  80                         continue;
  81
  82                 pcp_rewr = true;
  83                 qos.pcp_rewr.map[i] = fls(pcp_rewr_map.map[i]) - 1;
  84         }
  85
  86         /* Get dscp rewrite mapping */
  87         dcb_getrewr_prio_dscp_mask_map(dev, &dscp_rewr_map);
  88         for (int i = 0; i < ARRAY_SIZE(dscp_rewr_map.map); i++) {
  89                 if (!dscp_rewr_map.map[i])
  90                         continue;
  91
  92                 dscp_rewr = true;
  93                 qos.dscp_rewr.map[i] = fls64(dscp_rewr_map.map[i]) - 1;
  94         }
  95
  96         /* Enable use of pcp for queue classification */
  97         if (lan966x_dcb_apptrust_contains(port->chip_port, DCB_APP_SEL_PCP)) {
  98                 qos.pcp.enable = true;
  99
 100                 if (pcp_rewr)
 101                         qos.pcp_rewr.enable = true;
 102         }
 103
 104         /* Enable use of dscp for queue classification */
 105         if (lan966x_dcb_apptrust_contains(port->chip_port, IEEE_8021QAZ_APP_SEL_DSCP)) {
 106                 qos.dscp.enable = true;
 107
 108                 if (dscp_rewr)
 109                         qos.dscp_rewr.enable = true;
 110         }
 111
 112         lan966x_port_qos_set(port, &qos);
 113 }
 114
 115 /* DSCP mapping is global for all ports, so set and delete app entries are
 116  * replicated for each port.
 117  */
 118 static int lan966x_dcb_ieee_dscp_setdel(struct net_device *dev,
 119                                         struct dcb_app *app,
 120                                         int (*setdel)(struct net_device *,
 121                                                       struct dcb_app *))
 122 {
 123         struct lan966x_port *port = netdev_priv(dev);
 124         struct lan966x *lan966x = port->lan966x;
 125         int err;
 126
 127         for (int i = 0; i < NUM_PHYS_PORTS; i++) {
 128                 port = lan966x->ports[i];
 129                 if (!port)
 130                         continue;
 131
 132                 err = setdel(port->dev, app);
 133                 if (err)
 134                         return err;
 135         }
 136
 137         return 0;
 138 }
 139
 140 static int lan966x_dcb_app_validate(struct net_device *dev,
 141                                     const struct dcb_app *app)
 142 {
 143         int err = 0;
 144
 145         switch (app->selector) {
 146         /* Default priority checks */
 147         case IEEE_8021QAZ_APP_SEL_ETHERTYPE:
 148                 if (app->protocol)
 149                         err = -EINVAL;
 150                 else if (app->priority >= NUM_PRIO_QUEUES)
 151                         err = -ERANGE;
 152                 break;
 153         /* Dscp checks */
 154         case IEEE_8021QAZ_APP_SEL_DSCP:
 155                 if (app->protocol >= LAN966X_PORT_QOS_DSCP_COUNT)
 156                         err = -EINVAL;
 157                 else if (app->priority >= NUM_PRIO_QUEUES)
 158                         err = -ERANGE;
 159                 break;
 160         /* Pcp checks */
 161         case DCB_APP_SEL_PCP:
 162                 if (app->protocol >= LAN966X_PORT_QOS_PCP_DEI_COUNT)
 163                         err = -EINVAL;
 164                 else if (app->priority >= NUM_PRIO_QUEUES)
 165                         err = -ERANGE;
 166                 break;
 167         default:
 168                 err = -EINVAL;
 169                 break;
 170         }
 171
 172         if (err)
 173                 netdev_err(dev, "Invalid entry: %d:%d\n", app->protocol,
 174                            app->priority);
 175
 176         return err;
 177 }
 178
 179 static int lan966x_dcb_ieee_delapp(struct net_device *dev, struct dcb_app *app)
 180 {
 181         int err;
 182
 183         if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
 184                 err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_ieee_delapp);
 185         else
 186                 err = dcb_ieee_delapp(dev, app);
 187
 188         if (err)
 189                 return err;
 190
 191         lan966x_dcb_app_update(dev);
 192
 193         return 0;
 194 }
 195
 196 static int lan966x_dcb_ieee_setapp(struct net_device *dev, struct dcb_app *app)
 197 {
 198         struct dcb_app app_itr;
 199         int err;
 200         u8 prio;
 201
 202         err = lan966x_dcb_app_validate(dev, app);
 203         if (err)
 204                 return err;
 205
 206         /* Delete current mapping, if it exists */
 207         prio = dcb_getapp(dev, app);
 208         if (prio) {
 209                 app_itr = *app;
 210                 app_itr.priority = prio;
 211                 lan966x_dcb_ieee_delapp(dev, &app_itr);
 212         }
 213
 214         if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
 215                 err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_ieee_setapp);
 216         else
 217                 err = dcb_ieee_setapp(dev, app);
 218
 219         if (err)
 220                 return err;
 221
 222         lan966x_dcb_app_update(dev);
 223
 224         return 0;
 225 }
 226
 227 static int lan966x_dcb_apptrust_validate(struct net_device *dev,
 228                                          u8 *selectors,
 229                                          int nselectors)
 230 {
 231         for (int i = 0; i < ARRAY_SIZE(lan966x_dcb_apptrust_policies); i++) {
 232                 bool match;
 233
 234                 if (lan966x_dcb_apptrust_policies[i].nselectors != nselectors)
 235                         continue;
 236
 237                 match = true;
 238                 for (int j = 0; j < nselectors; j++) {
 239                         if (lan966x_dcb_apptrust_policies[i].selectors[j] !=
 240                             *(selectors + j)) {
 241                                 match = false;
 242                                 break;
 243                         }
 244                 }
 245                 if (match)
 246                         return i;
 247         }
 248
 249         netdev_err(dev, "Valid apptrust configurations are:\n");
 250         for (int i = 0; i < ARRAY_SIZE(lan966x_dcb_apptrust_names); i++)
 251                 pr_info("order: %s\n", lan966x_dcb_apptrust_names[i]);
 252
 253         return -EOPNOTSUPP;
 254 }
 255
 256 static int lan966x_dcb_setapptrust(struct net_device *dev,
 257                                    u8 *selectors,
 258                                    int nselectors)
 259 {
 260         struct lan966x_port *port = netdev_priv(dev);
 261         int idx;
 262
 263         idx = lan966x_dcb_apptrust_validate(dev, selectors, nselectors);
 264         if (idx < 0)
 265                 return idx;
 266
 267         lan966x_port_apptrust[port->chip_port] = &lan966x_dcb_apptrust_policies[idx];
 268         lan966x_dcb_app_update(dev);
 269
 270         return 0;
 271 }
 272
 273 static int lan966x_dcb_getapptrust(struct net_device *dev, u8 *selectors,
 274                                    int *nselectors)
 275 {
 276         struct lan966x_port *port = netdev_priv(dev);
 277         const struct lan966x_dcb_apptrust *trust;
 278
 279         trust = lan966x_port_apptrust[port->chip_port];
 280
 281         memcpy(selectors, trust->selectors, trust->nselectors);
 282         *nselectors = trust->nselectors;
 283
 284         return 0;
 285 }
 286
 287 static int lan966x_dcb_delrewr(struct net_device *dev, struct dcb_app *app)
 288 {
 289         int err;
 290
 291         if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
 292                 err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_delrewr);
 293         else
 294                 err = dcb_delrewr(dev, app);
 295
 296         if (err < 0)
 297                 return err;
 298
 299         lan966x_dcb_app_update(dev);
 300
 301         return 0;
 302 }
 303
 304 static int lan966x_dcb_setrewr(struct net_device *dev, struct dcb_app *app)
 305 {
 306         struct dcb_app app_itr;
 307         u16 proto;
 308         int err;
 309
 310         err = lan966x_dcb_app_validate(dev, app);
 311         if (err)
 312                 goto out;
 313
 314         /* Delete current mapping, if it exists. */
 315         proto = dcb_getrewr(dev, app);
 316         if (proto) {
 317                 app_itr = *app;
 318                 app_itr.protocol = proto;
 319                 lan966x_dcb_delrewr(dev, &app_itr);
 320         }
 321
 322         if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
 323                 err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_setrewr);
 324         else
 325                 err = dcb_setrewr(dev, app);
 326
 327         if (err)
 328                 goto out;
 329
 330         lan966x_dcb_app_update(dev);
 331
 332 out:
 333         return err;
 334 }
 335
 336 static const struct dcbnl_rtnl_ops lan966x_dcbnl_ops = {
 337         .ieee_setapp = lan966x_dcb_ieee_setapp,
 338         .ieee_delapp = lan966x_dcb_ieee_delapp,
 339         .dcbnl_setapptrust = lan966x_dcb_setapptrust,
 340         .dcbnl_getapptrust = lan966x_dcb_getapptrust,
 341         .dcbnl_setrewr = lan966x_dcb_setrewr,
 342         .dcbnl_delrewr = lan966x_dcb_delrewr,
 343 };
 344
 345 void lan966x_dcb_init(struct lan966x *lan966x)
 346 {
 347         for (int p = 0; p < lan966x->num_phys_ports; ++p) {
 348                 struct lan966x_port *port;
 349
 350                 port = lan966x->ports[p];
 351                 if (!port)
 352                         continue;
 353
 354                 port->dev->dcbnl_ops = &lan966x_dcbnl_ops;
 355
 356                 lan966x_port_apptrust[port->chip_port] =
 357                         &lan966x_dcb_apptrust_policies[LAN966X_DCB_APPTRUST_DSCP_PCP];
 358
 359                 /* Enable DSCP classification based on classified QoS class and
 360                  * DP, for all DSCP values, for all ports.
 361                  */
 362                 lan966x_port_qos_dscp_rewr_mode_set(port,
 363                                                     LAN966X_PORT_QOS_REWR_DSCP_ALL);
 364         }
 365 }