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] = { .selectors: { 0 }, .nselectors: 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(x: qos.default_prio) - 1; |
75 | |
76 | /* Get pcp rewrite mapping */ |
77 | dcb_getrewr_prio_pcp_mask_map(dev, p_map: &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(x: pcp_rewr_map.map[i]) - 1; |
84 | } |
85 | |
86 | /* Get dscp rewrite mapping */ |
87 | dcb_getrewr_prio_dscp_mask_map(dev, p_map: &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(x: dscp_rewr_map.map[i]) - 1; |
94 | } |
95 | |
96 | /* Enable use of pcp for queue classification */ |
97 | if (lan966x_dcb_apptrust_contains(portno: 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(portno: 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: &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, format: "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, setdel: 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: &app_itr); |
212 | } |
213 | |
214 | if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP) |
215 | err = lan966x_dcb_ieee_dscp_setdel(dev, app, setdel: 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, format: "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, setdel: 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: &app_itr); |
320 | } |
321 | |
322 | if (app->selector == IEEE_8021QAZ_APP_SEL_DSCP) |
323 | err = lan966x_dcb_ieee_dscp_setdel(dev, app, setdel: 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 | |