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 lan966x_port *port = netdev_priv(dev); 50 struct lan966x_port_qos qos = {0}; 51 struct dcb_app app_itr; 52 53 /* Get pcp ingress mapping */ 54 for (int i = 0; i < ARRAY_SIZE(qos.pcp.map); i++) { 55 app_itr.selector = DCB_APP_SEL_PCP; 56 app_itr.protocol = i; 57 qos.pcp.map[i] = dcb_getapp(dev, &app_itr); 58 } 59 60 /* Get dscp ingress mapping */ 61 for (int i = 0; i < ARRAY_SIZE(qos.dscp.map); i++) { 62 app_itr.selector = IEEE_8021QAZ_APP_SEL_DSCP; 63 app_itr.protocol = i; 64 qos.dscp.map[i] = dcb_getapp(dev, &app_itr); 65 } 66 67 /* Get default prio */ 68 qos.default_prio = dcb_ieee_getapp_default_prio_mask(dev); 69 if (qos.default_prio) 70 qos.default_prio = fls(qos.default_prio) - 1; 71 72 /* Enable use of pcp for queue classification */ 73 if (lan966x_dcb_apptrust_contains(port->chip_port, DCB_APP_SEL_PCP)) 74 qos.pcp.enable = true; 75 76 /* Enable use of dscp for queue classification */ 77 if (lan966x_dcb_apptrust_contains(port->chip_port, IEEE_8021QAZ_APP_SEL_DSCP)) 78 qos.dscp.enable = true; 79 80 lan966x_port_qos_set(port, &qos); 81 } 82 83 /* DSCP mapping is global for all ports, so set and delete app entries are 84 * replicated for each port. 85 */ 86 static int lan966x_dcb_ieee_dscp_setdel(struct net_device *dev, 87 struct dcb_app *app, 88 int (*setdel)(struct net_device *, 89 struct dcb_app *)) 90 { 91 struct lan966x_port *port = netdev_priv(dev); 92 struct lan966x *lan966x = port->lan966x; 93 int err; 94 95 for (int i = 0; i < NUM_PHYS_PORTS; i++) { 96 port = lan966x->ports[i]; 97 if (!port) 98 continue; 99 100 err = setdel(port->dev, app); 101 if (err) 102 return err; 103 } 104 105 return 0; 106 } 107 108 static int lan966x_dcb_app_validate(struct net_device *dev, 109 const struct dcb_app *app) 110 { 111 int err = 0; 112 113 switch (app->selector) { 114 /* Default priority checks */ 115 case IEEE_8021QAZ_APP_SEL_ETHERTYPE: 116 if (app->protocol) 117 err = -EINVAL; 118 else if (app->priority >= NUM_PRIO_QUEUES) 119 err = -ERANGE; 120 break; 121 /* Dscp checks */ 122 case IEEE_8021QAZ_APP_SEL_DSCP: 123 if (app->protocol >= LAN966X_PORT_QOS_DSCP_COUNT) 124 err = -EINVAL; 125 else if (app->priority >= NUM_PRIO_QUEUES) 126 err = -ERANGE; 127 break; 128 /* Pcp checks */ 129 case DCB_APP_SEL_PCP: 130 if (app->protocol >= LAN966X_PORT_QOS_PCP_DEI_COUNT) 131 err = -EINVAL; 132 else if (app->priority >= NUM_PRIO_QUEUES) 133 err = -ERANGE; 134 break; 135 default: 136 err = -EINVAL; 137 break; 138 } 139 140 if (err) 141 netdev_err(dev, "Invalid entry: %d:%d\n", app->protocol, 142 app->priority); 143 144 return err; 145 } 146 147 static int lan966x_dcb_ieee_delapp(struct net_device *dev, struct dcb_app *app) 148 { 149 int err; 150 151 if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP) 152 err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_ieee_delapp); 153 else 154 err = dcb_ieee_delapp(dev, app); 155 156 if (err) 157 return err; 158 159 lan966x_dcb_app_update(dev); 160 161 return 0; 162 } 163 164 static int lan966x_dcb_ieee_setapp(struct net_device *dev, struct dcb_app *app) 165 { 166 struct dcb_app app_itr; 167 int err; 168 u8 prio; 169 170 err = lan966x_dcb_app_validate(dev, app); 171 if (err) 172 return err; 173 174 /* Delete current mapping, if it exists */ 175 prio = dcb_getapp(dev, app); 176 if (prio) { 177 app_itr = *app; 178 app_itr.priority = prio; 179 lan966x_dcb_ieee_delapp(dev, &app_itr); 180 } 181 182 if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP) 183 err = lan966x_dcb_ieee_dscp_setdel(dev, app, dcb_ieee_setapp); 184 else 185 err = dcb_ieee_setapp(dev, app); 186 187 if (err) 188 return err; 189 190 lan966x_dcb_app_update(dev); 191 192 return 0; 193 } 194 195 static int lan966x_dcb_apptrust_validate(struct net_device *dev, 196 u8 *selectors, 197 int nselectors) 198 { 199 for (int i = 0; i < ARRAY_SIZE(lan966x_dcb_apptrust_policies); i++) { 200 bool match; 201 202 if (lan966x_dcb_apptrust_policies[i].nselectors != nselectors) 203 continue; 204 205 match = true; 206 for (int j = 0; j < nselectors; j++) { 207 if (lan966x_dcb_apptrust_policies[i].selectors[j] != 208 *(selectors + j)) { 209 match = false; 210 break; 211 } 212 } 213 if (match) 214 return i; 215 } 216 217 netdev_err(dev, "Valid apptrust configurations are:\n"); 218 for (int i = 0; i < ARRAY_SIZE(lan966x_dcb_apptrust_names); i++) 219 pr_info("order: %s\n", lan966x_dcb_apptrust_names[i]); 220 221 return -EOPNOTSUPP; 222 } 223 224 static int lan966x_dcb_setapptrust(struct net_device *dev, 225 u8 *selectors, 226 int nselectors) 227 { 228 struct lan966x_port *port = netdev_priv(dev); 229 int idx; 230 231 idx = lan966x_dcb_apptrust_validate(dev, selectors, nselectors); 232 if (idx < 0) 233 return idx; 234 235 lan966x_port_apptrust[port->chip_port] = &lan966x_dcb_apptrust_policies[idx]; 236 lan966x_dcb_app_update(dev); 237 238 return 0; 239 } 240 241 static int lan966x_dcb_getapptrust(struct net_device *dev, u8 *selectors, 242 int *nselectors) 243 { 244 struct lan966x_port *port = netdev_priv(dev); 245 const struct lan966x_dcb_apptrust *trust; 246 247 trust = lan966x_port_apptrust[port->chip_port]; 248 249 memcpy(selectors, trust->selectors, trust->nselectors); 250 *nselectors = trust->nselectors; 251 252 return 0; 253 } 254 255 static const struct dcbnl_rtnl_ops lan966x_dcbnl_ops = { 256 .ieee_setapp = lan966x_dcb_ieee_setapp, 257 .ieee_delapp = lan966x_dcb_ieee_delapp, 258 .dcbnl_setapptrust = lan966x_dcb_setapptrust, 259 .dcbnl_getapptrust = lan966x_dcb_getapptrust, 260 }; 261 262 void lan966x_dcb_init(struct lan966x *lan966x) 263 { 264 for (int p = 0; p < lan966x->num_phys_ports; ++p) { 265 struct lan966x_port *port; 266 267 port = lan966x->ports[p]; 268 if (!port) 269 continue; 270 271 port->dev->dcbnl_ops = &lan966x_dcbnl_ops; 272 273 lan966x_port_apptrust[port->chip_port] = 274 &lan966x_dcb_apptrust_policies[LAN966X_DCB_APPTRUST_DSCP_PCP]; 275 } 276 } 277