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 } 366