366 lines · c
1// SPDX-License-Identifier: GPL-2.0+2 3#include "lan966x_main.h"4 5enum 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_MAX11};12 13static 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 18static 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 */26static const struct lan966x_dcb_apptrust27 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 36static 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 47static 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 are116 * replicated for each port.117 */118static 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 140static 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 179static 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 else186 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 196static 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 else217 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 227static 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 256static 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 273static 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 287static 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 else294 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 304static 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 else325 err = dcb_setrewr(dev, app);326 327 if (err)328 goto out;329 330 lan966x_dcb_app_update(dev);331 332out:333 return err;334}335 336static 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 345void 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 and360 * 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}366