Quelle  lan966x_dcb.c   Sprache: C

// SPDX-License-Identifier: GPL-2.0+

#include "lan966x_main.h"

enum lan966x_dcb_apptrust_values {
 LAN966X_DCB_APPTRUST_EMPTY,
 LAN966X_DCB_APPTRUST_DSCP,
 LAN966X_DCB_APPTRUST_PCP,
 LAN966X_DCB_APPTRUST_DSCP_PCP,
 __LAN966X_DCB_APPTRUST_MAX
};

static const struct lan966x_dcb_apptrust {
 u8 selectors[IEEE_8021QAZ_APP_SEL_MAX + 1];
 int nselectors;
} *lan966x_port_apptrust[NUM_PHYS_PORTS];

static const char *lan966x_dcb_apptrust_names[__LAN966X_DCB_APPTRUST_MAX] = {
 [LAN966X_DCB_APPTRUST_EMPTY]    = "empty",
 [LAN966X_DCB_APPTRUST_DSCP]     = "dscp",
 [LAN966X_DCB_APPTRUST_PCP]      = "pcp",
 [LAN966X_DCB_APPTRUST_DSCP_PCP] = "dscp pcp"
};

/* Lan966x supported apptrust policies */
static const struct lan966x_dcb_apptrust
 lan966x_dcb_apptrust_policies[__LAN966X_DCB_APPTRUST_MAX] = {
 /* Empty *must* be first */
 [LAN966X_DCB_APPTRUST_EMPTY]    = { { 0 }, 0 },
 [LAN966X_DCB_APPTRUST_DSCP]     = { { IEEE_8021QAZ_APP_SEL_DSCP }, 1 },
 [LAN966X_DCB_APPTRUST_PCP]      = { { DCB_APP_SEL_PCP }, 1 },
 [LAN966X_DCB_APPTRUST_DSCP_PCP] = { { IEEE_8021QAZ_APP_SEL_DSCP,
           DCB_APP_SEL_PCP }, 2 },
};

static bool lan966x_dcb_apptrust_contains(int portno, u8 selector)
{
 const struct lan966x_dcb_apptrust *conf = lan966x_port_apptrust[portno];

 for (int i = 0; i < conf->nselectors; i++)
  if (conf->selectors[i] == selector)
   return true;

 return false;
}

static void lan966x_dcb_app_update(struct net_device *dev)
{
 struct dcb_ieee_app_prio_map dscp_rewr_map = {0};
 struct dcb_rewr_prio_pcp_map pcp_rewr_map = {0};
 struct lan966x_port *port = netdev_priv(dev);
 struct lan966x_port_qos qos = {0};
 struct dcb_app app_itr;
 bool dscp_rewr = false;
 bool pcp_rewr = false;

 /* Get pcp ingress mapping */
 for (int i = 0; i < ARRAY_SIZE(qos.pcp.map); i++) {
  app_itr.selector = DCB_APP_SEL_PCP;
  app_itr.protocol = i;
  qos.pcp.map[i] = dcb_getapp(dev, &app_itr);
 }

 /* Get dscp ingress mapping */
 for (int i = 0; i < ARRAY_SIZE(qos.dscp.map); i++) {
  app_itr.selector = IEEE_8021QAZ_APP_SEL_DSCP;
  app_itr.protocol = i;
  qos.dscp.map[i] = dcb_getapp(dev, &app_itr);
 }

 /* Get default prio */
 qos.default_prio = dcb_ieee_getapp_default_prio_mask(dev);
 if (qos.default_prio)
  qos.default_prio = fls(qos.default_prio) - 1;

 /* Get pcp rewrite mapping */
 dcb_getrewr_prio_pcp_mask_map(dev, &pcp_rewr_map);
 for (int i = 0; i < ARRAY_SIZE(pcp_rewr_map.map); i++) {
  if (!pcp_rewr_map.map[i])
   continue;

  pcp_rewr = true;
  qos.pcp_rewr.map[i] = fls(pcp_rewr_map.map[i]) - 1;
 }

 /* Get dscp rewrite mapping */
 dcb_getrewr_prio_dscp_mask_map(dev, &dscp_rewr_map);
 for (int i = 0; i < ARRAY_SIZE(dscp_rewr_map.map); i++) {
  if (!dscp_rewr_map.map[i])
   continue;

  dscp_rewr = true;
  qos.dscp_rewr.map[i] = fls64(dscp_rewr_map.map[i]) - 1;
 }

 /* Enable use of pcp for queue classification */
 if (lan966x_dcb_apptrust_contains(port->chip_port, DCB_APP_SEL_PCP)) {
  qos.pcp.enable = true;

  if (pcp_rewr)
   qos.pcp_rewr.enable = true;
 }

 /* Enable use of dscp for queue classification */
 if (lan966x_dcb_apptrust_contains(port->chip_port, IEEE_8021QAZ_APP_SEL_DSCP)) {
  qos.dscp.enable = true;

  if (dscp_rewr)
   qos.dscp_rewr.enable = true;
 }

 lan966x_port_qos_set(port, &qos);
}

/* DSCP mapping is global for all ports, so set and delete app entries are
 * replicated for each port.
 */
static int lan966x_dcb_ieee_dscp_setdel(struct net_device *dev,
     struct dcb_app *app,
     int (*setdel)(struct net_device *,
            struct dcb_app *))
{
 struct lan966x_port *port = netdev_priv(dev);
 struct lan966x *lan966x = port->lan966x;
 int err;

 for (int i = 0; i < NUM_PHYS_PORTS; i++) {
  port = lan966x->ports[i];
  if (!port)
   continue;

  err = setdel(port->dev, app);
  if (err)
   return err;
 }

 return 0;
}

static int lan966x_dcb_app_validate(struct net_device *dev,
        const struct dcb_app *app)
{
 int err = 0;

 switch (app->selector) {
 /* Default priority checks */
 case IEEE_8021QAZ_APP_SEL_ETHERTYPE:
  if (app->protocol)
   err = -EINVAL;
  else if (app->priority >= NUM_PRIO_QUEUES)
   err = -ERANGE;
  break;
 /* Dscp checks */
 case IEEE_8021QAZ_APP_SEL_DSCP:
  if (app->protocol >= LAN966X_PORT_QOS_DSCP_COUNT)
   err = -EINVAL;
  else if (app->priority >= NUM_PRIO_QUEUES)
   err = -ERANGE;
  break;
 /* Pcp checks */
 case DCB_APP_SEL_PCP:
  if (app->protocol >= LAN966X_PORT_QOS_PCP_DEI_COUNT)
   err = -EINVAL;
  else if (app->priority >= NUM_PRIO_QUEUES)
   err = -ERANGE;
  break;
 default:
  err = -EINVAL;
  break;
 }

 if (err)
  netdev_err(dev, "Invalid entry: %d:%d\n", app->protocol,
      app->priority);

 return err;
}

static int lan966x_dcb_ieee_delapp(struct net_device *dev, struct dcb_app *app)
{
 int err;

 if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
  err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_ieee_delapp);
 else
  err = dcb_ieee_delapp(dev, app);

 if (err)
  return err;

 lan966x_dcb_app_update(dev);

 return 0;
}

static int lan966x_dcb_ieee_setapp(struct net_device *dev, struct dcb_app *app)
{
 struct dcb_app app_itr;
 int err;
 u8 prio;

 err = lan966x_dcb_app_validate(dev, app);
 if (err)
  return err;

 /* Delete current mapping, if it exists */
 prio = dcb_getapp(dev, app);
 if (prio) {
  app_itr = *app;
  app_itr.priority = prio;
  lan966x_dcb_ieee_delapp(dev, &app_itr);
 }

 if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
  err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_ieee_setapp);
 else
  err = dcb_ieee_setapp(dev, app);

 if (err)
  return err;

 lan966x_dcb_app_update(dev);

 return 0;
}

static int lan966x_dcb_apptrust_validate(struct net_device *dev,
      u8 *selectors,
      int nselectors)
{
 for (int i = 0; i < ARRAY_SIZE(lan966x_dcb_apptrust_policies); i++) {
  bool match;

  if (lan966x_dcb_apptrust_policies[i].nselectors != nselectors)
   continue;

  match = true;
  for (int j = 0; j < nselectors; j++) {
   if (lan966x_dcb_apptrust_policies[i].selectors[j] !=
       *(selectors + j)) {
    match = false;
    break;
   }
  }
  if (match)
   return i;
 }

 netdev_err(dev, "Valid apptrust configurations are:\n");
 for (int i = 0; i < ARRAY_SIZE(lan966x_dcb_apptrust_names); i++)
  pr_info("order: %s\n", lan966x_dcb_apptrust_names[i]);

 return -EOPNOTSUPP;
}

static int lan966x_dcb_setapptrust(struct net_device *dev,
       u8 *selectors,
       int nselectors)
{
 struct lan966x_port *port = netdev_priv(dev);
 int idx;

 idx = lan966x_dcb_apptrust_validate(dev, selectors, nselectors);
 if (idx < 0)
  return idx;

 lan966x_port_apptrust[port->chip_port] = &lan966x_dcb_apptrust_policies[idx];
 lan966x_dcb_app_update(dev);

 return 0;
}

static int lan966x_dcb_getapptrust(struct net_device *dev, u8 *selectors,
       int *nselectors)
{
 struct lan966x_port *port = netdev_priv(dev);
 const struct lan966x_dcb_apptrust *trust;

 trust = lan966x_port_apptrust[port->chip_port];

 memcpy(selectors, trust->selectors, trust->nselectors);
 *nselectors = trust->nselectors;

 return 0;
}

static int lan966x_dcb_delrewr(struct net_device *dev, struct dcb_app *app)
{
 int err;

 if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
  err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_delrewr);
 else
  err = dcb_delrewr(dev, app);

 if (err < 0)
  return err;

 lan966x_dcb_app_update(dev);

 return 0;
}

static int lan966x_dcb_setrewr(struct net_device *dev, struct dcb_app *app)
{
 struct dcb_app app_itr;
 u16 proto;
 int err;

 err = lan966x_dcb_app_validate(dev, app);
 if (err)
  goto out;

 /* Delete current mapping, if it exists. */
 proto = dcb_getrewr(dev, app);
 if (proto) {
  app_itr = *app;
  app_itr.protocol = proto;
  lan966x_dcb_delrewr(dev, &app_itr);
 }

 if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP)
  err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_setrewr);
 else
  err = dcb_setrewr(dev, app);

 if (err)
  goto out;

 lan966x_dcb_app_update(dev);

out:
 return err;
}

static const struct dcbnl_rtnl_ops lan966x_dcbnl_ops = {
 .ieee_setapp = lan966x_dcb_ieee_setapp,
 .ieee_delapp = lan966x_dcb_ieee_delapp,
 .dcbnl_setapptrust = lan966x_dcb_setapptrust,
 .dcbnl_getapptrust = lan966x_dcb_getapptrust,
 .dcbnl_setrewr = lan966x_dcb_setrewr,
 .dcbnl_delrewr = lan966x_dcb_delrewr,
};

void lan966x_dcb_init(struct lan966x *lan966x)
{
 for (int p = 0; p < lan966x->num_phys_ports; ++p) {
  struct lan966x_port *port;

  port = lan966x->ports[p];
  if (!port)
   continue;

  port->dev->dcbnl_ops = &lan966x_dcbnl_ops;

  lan966x_port_apptrust[port->chip_port] =
   &lan966x_dcb_apptrust_policies[LAN966X_DCB_APPTRUST_DSCP_PCP];

  /* Enable DSCP classification based on classified QoS class and
 * DP, for all DSCP values, for all ports.
 */
  lan966x_port_qos_dscp_rewr_mode_set(port,
          LAN966X_PORT_QOS_REWR_DSCP_ALL);
 }
}