1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* Texas Instruments K3 AM65 Ethernet QoS submodule |
3 | * Copyright (C) 2020 Texas Instruments Incorporated - http://www.ti.com/ |
4 | * |
5 | * quality of service module includes: |
6 | * Enhanced Scheduler Traffic (EST - P802.1Qbv/D2.2) |
7 | * Interspersed Express Traffic (IET - P802.3br/D2.0) |
8 | */ |
9 | |
10 | #include <linux/pm_runtime.h> |
11 | #include <linux/math.h> |
12 | #include <linux/time.h> |
13 | #include <linux/units.h> |
14 | #include <net/pkt_cls.h> |
15 | |
16 | #include "am65-cpsw-nuss.h" |
17 | #include "am65-cpsw-qos.h" |
18 | #include "am65-cpts.h" |
19 | #include "cpsw_ale.h" |
20 | |
21 | #define TO_MBPS(x) DIV_ROUND_UP((x), BYTES_PER_MBIT) |
22 | |
23 | enum timer_act { |
24 | TACT_PROG, /* need program timer */ |
25 | TACT_NEED_STOP, /* need stop first */ |
26 | TACT_SKIP_PROG, /* just buffer can be updated */ |
27 | }; |
28 | |
29 | static void am65_cpsw_iet_change_preemptible_tcs(struct am65_cpsw_port *port, u8 preemptible_tcs); |
30 | |
31 | static u32 |
32 | am65_cpsw_qos_tx_rate_calc(u32 rate_mbps, unsigned long bus_freq) |
33 | { |
34 | u32 ir; |
35 | |
36 | bus_freq /= 1000000; |
37 | ir = DIV_ROUND_UP(((u64)rate_mbps * 32768), bus_freq); |
38 | return ir; |
39 | } |
40 | |
41 | static void am65_cpsw_tx_pn_shaper_reset(struct am65_cpsw_port *port) |
42 | { |
43 | int prio; |
44 | |
45 | for (prio = 0; prio < AM65_CPSW_PN_FIFO_PRIO_NUM; prio++) { |
46 | writel(val: 0, addr: port->port_base + AM65_CPSW_PN_REG_PRI_CIR(prio)); |
47 | writel(val: 0, addr: port->port_base + AM65_CPSW_PN_REG_PRI_EIR(prio)); |
48 | } |
49 | } |
50 | |
51 | static void am65_cpsw_tx_pn_shaper_apply(struct am65_cpsw_port *port) |
52 | { |
53 | struct am65_cpsw_mqprio *p_mqprio = &port->qos.mqprio; |
54 | struct am65_cpsw_common *common = port->common; |
55 | struct tc_mqprio_qopt_offload *mqprio; |
56 | bool enable, shaper_susp = false; |
57 | u32 rate_mbps; |
58 | int tc, prio; |
59 | |
60 | mqprio = &p_mqprio->mqprio_hw; |
61 | /* takes care of no link case as well */ |
62 | if (p_mqprio->max_rate_total > port->qos.link_speed) |
63 | shaper_susp = true; |
64 | |
65 | am65_cpsw_tx_pn_shaper_reset(port); |
66 | |
67 | enable = p_mqprio->shaper_en && !shaper_susp; |
68 | if (!enable) |
69 | return; |
70 | |
71 | /* Rate limit is specified per Traffic Class but |
72 | * for CPSW, rate limit can be applied per priority |
73 | * at port FIFO. |
74 | * |
75 | * We have assigned the same priority (TCn) to all queues |
76 | * of a Traffic Class so they share the same shaper |
77 | * bandwidth. |
78 | */ |
79 | for (tc = 0; tc < mqprio->qopt.num_tc; tc++) { |
80 | prio = tc; |
81 | |
82 | rate_mbps = TO_MBPS(mqprio->min_rate[tc]); |
83 | rate_mbps = am65_cpsw_qos_tx_rate_calc(rate_mbps, |
84 | bus_freq: common->bus_freq); |
85 | writel(val: rate_mbps, |
86 | addr: port->port_base + AM65_CPSW_PN_REG_PRI_CIR(prio)); |
87 | |
88 | rate_mbps = 0; |
89 | |
90 | if (mqprio->max_rate[tc]) { |
91 | rate_mbps = mqprio->max_rate[tc] - mqprio->min_rate[tc]; |
92 | rate_mbps = TO_MBPS(rate_mbps); |
93 | rate_mbps = am65_cpsw_qos_tx_rate_calc(rate_mbps, |
94 | bus_freq: common->bus_freq); |
95 | } |
96 | |
97 | writel(val: rate_mbps, |
98 | addr: port->port_base + AM65_CPSW_PN_REG_PRI_EIR(prio)); |
99 | } |
100 | } |
101 | |
102 | static int am65_cpsw_mqprio_verify_shaper(struct am65_cpsw_port *port, |
103 | struct tc_mqprio_qopt_offload *mqprio) |
104 | { |
105 | struct am65_cpsw_mqprio *p_mqprio = &port->qos.mqprio; |
106 | struct netlink_ext_ack *extack = mqprio->extack; |
107 | u64 min_rate_total = 0, max_rate_total = 0; |
108 | u32 min_rate_msk = 0, max_rate_msk = 0; |
109 | bool has_min_rate, has_max_rate; |
110 | int num_tc, i; |
111 | |
112 | if (!(mqprio->flags & TC_MQPRIO_F_SHAPER)) |
113 | return 0; |
114 | |
115 | if (mqprio->shaper != TC_MQPRIO_SHAPER_BW_RATE) |
116 | return 0; |
117 | |
118 | has_min_rate = !!(mqprio->flags & TC_MQPRIO_F_MIN_RATE); |
119 | has_max_rate = !!(mqprio->flags & TC_MQPRIO_F_MAX_RATE); |
120 | |
121 | if (!has_min_rate && has_max_rate) { |
122 | NL_SET_ERR_MSG_MOD(extack, "min_rate is required with max_rate" ); |
123 | return -EOPNOTSUPP; |
124 | } |
125 | |
126 | if (!has_min_rate) |
127 | return 0; |
128 | |
129 | num_tc = mqprio->qopt.num_tc; |
130 | |
131 | for (i = num_tc - 1; i >= 0; i--) { |
132 | u32 ch_msk; |
133 | |
134 | if (mqprio->min_rate[i]) |
135 | min_rate_msk |= BIT(i); |
136 | min_rate_total += mqprio->min_rate[i]; |
137 | |
138 | if (has_max_rate) { |
139 | if (mqprio->max_rate[i]) |
140 | max_rate_msk |= BIT(i); |
141 | max_rate_total += mqprio->max_rate[i]; |
142 | |
143 | if (!mqprio->min_rate[i] && mqprio->max_rate[i]) { |
144 | NL_SET_ERR_MSG_FMT_MOD(extack, |
145 | "TX tc%d rate max>0 but min=0" , |
146 | i); |
147 | return -EINVAL; |
148 | } |
149 | |
150 | if (mqprio->max_rate[i] && |
151 | mqprio->max_rate[i] < mqprio->min_rate[i]) { |
152 | NL_SET_ERR_MSG_FMT_MOD(extack, |
153 | "TX tc%d rate min(%llu)>max(%llu)" , |
154 | i, mqprio->min_rate[i], |
155 | mqprio->max_rate[i]); |
156 | return -EINVAL; |
157 | } |
158 | } |
159 | |
160 | ch_msk = GENMASK(num_tc - 1, i); |
161 | if ((min_rate_msk & BIT(i)) && (min_rate_msk ^ ch_msk)) { |
162 | NL_SET_ERR_MSG_FMT_MOD(extack, |
163 | "Min rate must be set sequentially hi->lo tx_rate_msk%x" , |
164 | min_rate_msk); |
165 | return -EINVAL; |
166 | } |
167 | |
168 | if ((max_rate_msk & BIT(i)) && (max_rate_msk ^ ch_msk)) { |
169 | NL_SET_ERR_MSG_FMT_MOD(extack, |
170 | "Max rate must be set sequentially hi->lo tx_rate_msk%x" , |
171 | max_rate_msk); |
172 | return -EINVAL; |
173 | } |
174 | } |
175 | |
176 | min_rate_total = TO_MBPS(min_rate_total); |
177 | max_rate_total = TO_MBPS(max_rate_total); |
178 | |
179 | p_mqprio->shaper_en = true; |
180 | p_mqprio->max_rate_total = max_t(u64, min_rate_total, max_rate_total); |
181 | |
182 | return 0; |
183 | } |
184 | |
185 | static void am65_cpsw_reset_tc_mqprio(struct net_device *ndev) |
186 | { |
187 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
188 | struct am65_cpsw_mqprio *p_mqprio = &port->qos.mqprio; |
189 | |
190 | p_mqprio->shaper_en = false; |
191 | p_mqprio->max_rate_total = 0; |
192 | |
193 | am65_cpsw_tx_pn_shaper_reset(port); |
194 | netdev_reset_tc(dev: ndev); |
195 | |
196 | /* Reset all Queue priorities to 0 */ |
197 | writel(val: 0, addr: port->port_base + AM65_CPSW_PN_REG_TX_PRI_MAP); |
198 | |
199 | am65_cpsw_iet_change_preemptible_tcs(port, preemptible_tcs: 0); |
200 | } |
201 | |
202 | static int am65_cpsw_setup_mqprio(struct net_device *ndev, void *type_data) |
203 | { |
204 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
205 | struct am65_cpsw_mqprio *p_mqprio = &port->qos.mqprio; |
206 | struct tc_mqprio_qopt_offload *mqprio = type_data; |
207 | struct am65_cpsw_common *common = port->common; |
208 | struct tc_mqprio_qopt *qopt = &mqprio->qopt; |
209 | int i, tc, offset, count, prio, ret; |
210 | u8 num_tc = qopt->num_tc; |
211 | u32 tx_prio_map = 0; |
212 | |
213 | memcpy(&p_mqprio->mqprio_hw, mqprio, sizeof(*mqprio)); |
214 | |
215 | ret = pm_runtime_get_sync(dev: common->dev); |
216 | if (ret < 0) { |
217 | pm_runtime_put_noidle(dev: common->dev); |
218 | return ret; |
219 | } |
220 | |
221 | if (!num_tc) { |
222 | am65_cpsw_reset_tc_mqprio(ndev); |
223 | ret = 0; |
224 | goto exit_put; |
225 | } |
226 | |
227 | ret = am65_cpsw_mqprio_verify_shaper(port, mqprio); |
228 | if (ret) |
229 | goto exit_put; |
230 | |
231 | netdev_set_num_tc(dev: ndev, num_tc); |
232 | |
233 | /* Multiple Linux priorities can map to a Traffic Class |
234 | * A Traffic Class can have multiple contiguous Queues, |
235 | * Queues get mapped to Channels (thread_id), |
236 | * if not VLAN tagged, thread_id is used as packet_priority |
237 | * if VLAN tagged. VLAN priority is used as packet_priority |
238 | * packet_priority gets mapped to header_priority in p0_rx_pri_map, |
239 | * header_priority gets mapped to switch_priority in pn_tx_pri_map. |
240 | * As p0_rx_pri_map is left at defaults (0x76543210), we can |
241 | * assume that Queue_n gets mapped to header_priority_n. We can then |
242 | * set the switch priority in pn_tx_pri_map. |
243 | */ |
244 | |
245 | for (tc = 0; tc < num_tc; tc++) { |
246 | prio = tc; |
247 | |
248 | /* For simplicity we assign the same priority (TCn) to |
249 | * all queues of a Traffic Class. |
250 | */ |
251 | for (i = qopt->offset[tc]; i < qopt->offset[tc] + qopt->count[tc]; i++) |
252 | tx_prio_map |= prio << (4 * i); |
253 | |
254 | count = qopt->count[tc]; |
255 | offset = qopt->offset[tc]; |
256 | netdev_set_tc_queue(dev: ndev, tc, count, offset); |
257 | } |
258 | |
259 | writel(val: tx_prio_map, addr: port->port_base + AM65_CPSW_PN_REG_TX_PRI_MAP); |
260 | |
261 | am65_cpsw_tx_pn_shaper_apply(port); |
262 | am65_cpsw_iet_change_preemptible_tcs(port, preemptible_tcs: mqprio->preemptible_tcs); |
263 | |
264 | exit_put: |
265 | pm_runtime_put(dev: common->dev); |
266 | |
267 | return ret; |
268 | } |
269 | |
270 | static int am65_cpsw_iet_set_verify_timeout_count(struct am65_cpsw_port *port) |
271 | { |
272 | int verify_time_ms = port->qos.iet.verify_time_ms; |
273 | u32 val; |
274 | |
275 | /* The number of wireside clocks contained in the verify |
276 | * timeout counter. The default is 0x1312d0 |
277 | * (10ms at 125Mhz in 1G mode). |
278 | */ |
279 | val = 125 * HZ_PER_MHZ; /* assuming 125MHz wireside clock */ |
280 | |
281 | val /= MILLIHZ_PER_HZ; /* count per ms timeout */ |
282 | val *= verify_time_ms; /* count for timeout ms */ |
283 | |
284 | if (val > AM65_CPSW_PN_MAC_VERIFY_CNT_MASK) |
285 | return -EINVAL; |
286 | |
287 | writel(val, addr: port->port_base + AM65_CPSW_PN_REG_IET_VERIFY); |
288 | |
289 | return 0; |
290 | } |
291 | |
292 | static int am65_cpsw_iet_verify_wait(struct am65_cpsw_port *port) |
293 | { |
294 | u32 ctrl, status; |
295 | int try; |
296 | |
297 | try = 20; |
298 | do { |
299 | /* Reset the verify state machine by writing 1 |
300 | * to LINKFAIL |
301 | */ |
302 | ctrl = readl(addr: port->port_base + AM65_CPSW_PN_REG_IET_CTRL); |
303 | ctrl |= AM65_CPSW_PN_IET_MAC_LINKFAIL; |
304 | writel(val: ctrl, addr: port->port_base + AM65_CPSW_PN_REG_IET_CTRL); |
305 | |
306 | /* Clear MAC_LINKFAIL bit to start Verify. */ |
307 | ctrl = readl(addr: port->port_base + AM65_CPSW_PN_REG_IET_CTRL); |
308 | ctrl &= ~AM65_CPSW_PN_IET_MAC_LINKFAIL; |
309 | writel(val: ctrl, addr: port->port_base + AM65_CPSW_PN_REG_IET_CTRL); |
310 | |
311 | msleep(msecs: port->qos.iet.verify_time_ms); |
312 | |
313 | status = readl(addr: port->port_base + AM65_CPSW_PN_REG_IET_STATUS); |
314 | if (status & AM65_CPSW_PN_MAC_VERIFIED) |
315 | return 0; |
316 | |
317 | if (status & AM65_CPSW_PN_MAC_VERIFY_FAIL) { |
318 | netdev_dbg(port->ndev, |
319 | "MAC Merge verify failed, trying again\n" ); |
320 | continue; |
321 | } |
322 | |
323 | if (status & AM65_CPSW_PN_MAC_RESPOND_ERR) { |
324 | netdev_dbg(port->ndev, "MAC Merge respond error\n" ); |
325 | return -ENODEV; |
326 | } |
327 | |
328 | if (status & AM65_CPSW_PN_MAC_VERIFY_ERR) { |
329 | netdev_dbg(port->ndev, "MAC Merge verify error\n" ); |
330 | return -ENODEV; |
331 | } |
332 | } while (try-- > 0); |
333 | |
334 | netdev_dbg(port->ndev, "MAC Merge verify timeout\n" ); |
335 | return -ETIMEDOUT; |
336 | } |
337 | |
338 | static void am65_cpsw_iet_set_preempt_mask(struct am65_cpsw_port *port, u8 preemptible_tcs) |
339 | { |
340 | u32 val; |
341 | |
342 | val = readl(addr: port->port_base + AM65_CPSW_PN_REG_IET_CTRL); |
343 | val &= ~AM65_CPSW_PN_IET_MAC_PREMPT_MASK; |
344 | val |= AM65_CPSW_PN_IET_MAC_SET_PREEMPT(preemptible_tcs); |
345 | writel(val, addr: port->port_base + AM65_CPSW_PN_REG_IET_CTRL); |
346 | } |
347 | |
348 | /* enable common IET_ENABLE only if at least 1 port has rx IET enabled. |
349 | * UAPI doesn't allow tx enable without rx enable. |
350 | */ |
351 | void am65_cpsw_iet_common_enable(struct am65_cpsw_common *common) |
352 | { |
353 | struct am65_cpsw_port *port; |
354 | bool rx_enable = false; |
355 | u32 val; |
356 | int i; |
357 | |
358 | for (i = 0; i < common->port_num; i++) { |
359 | port = &common->ports[i]; |
360 | val = readl(addr: port->port_base + AM65_CPSW_PN_REG_CTL); |
361 | rx_enable = !!(val & AM65_CPSW_PN_CTL_IET_PORT_EN); |
362 | if (rx_enable) |
363 | break; |
364 | } |
365 | |
366 | val = readl(addr: common->cpsw_base + AM65_CPSW_REG_CTL); |
367 | |
368 | if (rx_enable) |
369 | val |= AM65_CPSW_CTL_IET_EN; |
370 | else |
371 | val &= ~AM65_CPSW_CTL_IET_EN; |
372 | |
373 | writel(val, addr: common->cpsw_base + AM65_CPSW_REG_CTL); |
374 | common->iet_enabled = rx_enable; |
375 | } |
376 | |
377 | /* CPSW does not have an IRQ to notify changes to the MAC Merge TX status |
378 | * (active/inactive), but the preemptible traffic classes should only be |
379 | * committed to hardware once TX is active. Resort to polling. |
380 | */ |
381 | void am65_cpsw_iet_commit_preemptible_tcs(struct am65_cpsw_port *port) |
382 | { |
383 | u8 preemptible_tcs; |
384 | int err; |
385 | u32 val; |
386 | |
387 | if (port->qos.link_speed == SPEED_UNKNOWN) |
388 | return; |
389 | |
390 | val = readl(addr: port->port_base + AM65_CPSW_PN_REG_CTL); |
391 | if (!(val & AM65_CPSW_PN_CTL_IET_PORT_EN)) |
392 | return; |
393 | |
394 | /* update common IET enable */ |
395 | am65_cpsw_iet_common_enable(port->common); |
396 | |
397 | /* update verify count */ |
398 | err = am65_cpsw_iet_set_verify_timeout_count(port); |
399 | if (err) { |
400 | netdev_err(dev: port->ndev, format: "couldn't set verify count: %d\n" , err); |
401 | return; |
402 | } |
403 | |
404 | val = readl(addr: port->port_base + AM65_CPSW_PN_REG_IET_CTRL); |
405 | if (!(val & AM65_CPSW_PN_IET_MAC_DISABLEVERIFY)) { |
406 | err = am65_cpsw_iet_verify_wait(port); |
407 | if (err) |
408 | return; |
409 | } |
410 | |
411 | preemptible_tcs = port->qos.iet.preemptible_tcs; |
412 | am65_cpsw_iet_set_preempt_mask(port, preemptible_tcs); |
413 | } |
414 | |
415 | static void am65_cpsw_iet_change_preemptible_tcs(struct am65_cpsw_port *port, u8 preemptible_tcs) |
416 | { |
417 | struct am65_cpsw_ndev_priv *priv = am65_ndev_to_priv(port->ndev); |
418 | |
419 | port->qos.iet.preemptible_tcs = preemptible_tcs; |
420 | mutex_lock(&priv->mm_lock); |
421 | am65_cpsw_iet_commit_preemptible_tcs(port); |
422 | mutex_unlock(lock: &priv->mm_lock); |
423 | } |
424 | |
425 | static void am65_cpsw_iet_link_state_update(struct net_device *ndev) |
426 | { |
427 | struct am65_cpsw_ndev_priv *priv = am65_ndev_to_priv(ndev); |
428 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
429 | |
430 | mutex_lock(&priv->mm_lock); |
431 | am65_cpsw_iet_commit_preemptible_tcs(port); |
432 | mutex_unlock(lock: &priv->mm_lock); |
433 | } |
434 | |
435 | static int am65_cpsw_port_est_enabled(struct am65_cpsw_port *port) |
436 | { |
437 | return port->qos.est_oper || port->qos.est_admin; |
438 | } |
439 | |
440 | static void am65_cpsw_est_enable(struct am65_cpsw_common *common, int enable) |
441 | { |
442 | u32 val; |
443 | |
444 | val = readl(addr: common->cpsw_base + AM65_CPSW_REG_CTL); |
445 | |
446 | if (enable) |
447 | val |= AM65_CPSW_CTL_EST_EN; |
448 | else |
449 | val &= ~AM65_CPSW_CTL_EST_EN; |
450 | |
451 | writel(val, addr: common->cpsw_base + AM65_CPSW_REG_CTL); |
452 | common->est_enabled = enable; |
453 | } |
454 | |
455 | static void am65_cpsw_port_est_enable(struct am65_cpsw_port *port, int enable) |
456 | { |
457 | u32 val; |
458 | |
459 | val = readl(addr: port->port_base + AM65_CPSW_PN_REG_CTL); |
460 | if (enable) |
461 | val |= AM65_CPSW_PN_CTL_EST_PORT_EN; |
462 | else |
463 | val &= ~AM65_CPSW_PN_CTL_EST_PORT_EN; |
464 | |
465 | writel(val, addr: port->port_base + AM65_CPSW_PN_REG_CTL); |
466 | } |
467 | |
468 | /* target new EST RAM buffer, actual toggle happens after cycle completion */ |
469 | static void am65_cpsw_port_est_assign_buf_num(struct net_device *ndev, |
470 | int buf_num) |
471 | { |
472 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
473 | u32 val; |
474 | |
475 | val = readl(addr: port->port_base + AM65_CPSW_PN_REG_EST_CTL); |
476 | if (buf_num) |
477 | val |= AM65_CPSW_PN_EST_BUFSEL; |
478 | else |
479 | val &= ~AM65_CPSW_PN_EST_BUFSEL; |
480 | |
481 | writel(val, addr: port->port_base + AM65_CPSW_PN_REG_EST_CTL); |
482 | } |
483 | |
484 | /* am65_cpsw_port_est_is_swapped() - Indicate if h/w is transitioned |
485 | * admin -> oper or not |
486 | * |
487 | * Return true if already transitioned. i.e oper is equal to admin and buf |
488 | * numbers match (est_oper->buf match with est_admin->buf). |
489 | * false if before transition. i.e oper is not equal to admin, (i.e a |
490 | * previous admin command is waiting to be transitioned to oper state |
491 | * and est_oper->buf not match with est_oper->buf). |
492 | */ |
493 | static int am65_cpsw_port_est_is_swapped(struct net_device *ndev, int *oper, |
494 | int *admin) |
495 | { |
496 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
497 | u32 val; |
498 | |
499 | val = readl(addr: port->port_base + AM65_CPSW_PN_REG_FIFO_STATUS); |
500 | *oper = !!(val & AM65_CPSW_PN_FST_EST_BUFACT); |
501 | |
502 | val = readl(addr: port->port_base + AM65_CPSW_PN_REG_EST_CTL); |
503 | *admin = !!(val & AM65_CPSW_PN_EST_BUFSEL); |
504 | |
505 | return *admin == *oper; |
506 | } |
507 | |
508 | /* am65_cpsw_port_est_get_free_buf_num() - Get free buffer number for |
509 | * Admin to program the new schedule. |
510 | * |
511 | * Logic as follows:- |
512 | * If oper is same as admin, return the other buffer (!oper) as the admin |
513 | * buffer. If oper is not the same, driver let the current oper to continue |
514 | * as it is in the process of transitioning from admin -> oper. So keep the |
515 | * oper by selecting the same oper buffer by writing to EST_BUFSEL bit in |
516 | * EST CTL register. In the second iteration they will match and code returns. |
517 | * The actual buffer to write command is selected later before it is ready |
518 | * to update the schedule. |
519 | */ |
520 | static int am65_cpsw_port_est_get_free_buf_num(struct net_device *ndev) |
521 | { |
522 | int oper, admin; |
523 | int roll = 2; |
524 | |
525 | while (roll--) { |
526 | if (am65_cpsw_port_est_is_swapped(ndev, oper: &oper, admin: &admin)) |
527 | return !oper; |
528 | |
529 | /* admin is not set, so hinder transition as it's not allowed |
530 | * to touch memory in-flight, by targeting same oper buf. |
531 | */ |
532 | am65_cpsw_port_est_assign_buf_num(ndev, buf_num: oper); |
533 | |
534 | dev_info(&ndev->dev, |
535 | "Prev. EST admin cycle is in transit %d -> %d\n" , |
536 | oper, admin); |
537 | } |
538 | |
539 | return admin; |
540 | } |
541 | |
542 | static void am65_cpsw_admin_to_oper(struct net_device *ndev) |
543 | { |
544 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
545 | |
546 | devm_kfree(dev: &ndev->dev, p: port->qos.est_oper); |
547 | |
548 | port->qos.est_oper = port->qos.est_admin; |
549 | port->qos.est_admin = NULL; |
550 | } |
551 | |
552 | static void am65_cpsw_port_est_get_buf_num(struct net_device *ndev, |
553 | struct am65_cpsw_est *est_new) |
554 | { |
555 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
556 | u32 val; |
557 | |
558 | val = readl(addr: port->port_base + AM65_CPSW_PN_REG_EST_CTL); |
559 | val &= ~AM65_CPSW_PN_EST_ONEBUF; |
560 | writel(val, addr: port->port_base + AM65_CPSW_PN_REG_EST_CTL); |
561 | |
562 | est_new->buf = am65_cpsw_port_est_get_free_buf_num(ndev); |
563 | |
564 | /* rolled buf num means changed buf while configuring */ |
565 | if (port->qos.est_oper && port->qos.est_admin && |
566 | est_new->buf == port->qos.est_oper->buf) |
567 | am65_cpsw_admin_to_oper(ndev); |
568 | } |
569 | |
570 | static void am65_cpsw_est_set(struct net_device *ndev, int enable) |
571 | { |
572 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
573 | struct am65_cpsw_common *common = port->common; |
574 | int common_enable = 0; |
575 | int i; |
576 | |
577 | am65_cpsw_port_est_enable(port, enable); |
578 | |
579 | for (i = 0; i < common->port_num; i++) |
580 | common_enable |= am65_cpsw_port_est_enabled(port: &common->ports[i]); |
581 | |
582 | common_enable |= enable; |
583 | am65_cpsw_est_enable(common, enable: common_enable); |
584 | } |
585 | |
586 | /* This update is supposed to be used in any routine before getting real state |
587 | * of admin -> oper transition, particularly it's supposed to be used in some |
588 | * generic routine for providing real state to Taprio Qdisc. |
589 | */ |
590 | static void am65_cpsw_est_update_state(struct net_device *ndev) |
591 | { |
592 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
593 | int oper, admin; |
594 | |
595 | if (!port->qos.est_admin) |
596 | return; |
597 | |
598 | if (!am65_cpsw_port_est_is_swapped(ndev, oper: &oper, admin: &admin)) |
599 | return; |
600 | |
601 | am65_cpsw_admin_to_oper(ndev); |
602 | } |
603 | |
604 | /* Fetch command count it's number of bytes in Gigabit mode or nibbles in |
605 | * 10/100Mb mode. So, having speed and time in ns, recalculate ns to number of |
606 | * bytes/nibbles that can be sent while transmission on given speed. |
607 | */ |
608 | static int am65_est_cmd_ns_to_cnt(u64 ns, int link_speed) |
609 | { |
610 | u64 temp; |
611 | |
612 | temp = ns * link_speed; |
613 | if (link_speed < SPEED_1000) |
614 | temp <<= 1; |
615 | |
616 | return DIV_ROUND_UP(temp, 8 * 1000); |
617 | } |
618 | |
619 | static void __iomem *am65_cpsw_est_set_sched_cmds(void __iomem *addr, |
620 | int fetch_cnt, |
621 | int fetch_allow) |
622 | { |
623 | u32 prio_mask, cmd_fetch_cnt, cmd; |
624 | |
625 | do { |
626 | if (fetch_cnt > AM65_CPSW_FETCH_CNT_MAX) { |
627 | fetch_cnt -= AM65_CPSW_FETCH_CNT_MAX; |
628 | cmd_fetch_cnt = AM65_CPSW_FETCH_CNT_MAX; |
629 | } else { |
630 | cmd_fetch_cnt = fetch_cnt; |
631 | /* fetch count can't be less than 16? */ |
632 | if (cmd_fetch_cnt && cmd_fetch_cnt < 16) |
633 | cmd_fetch_cnt = 16; |
634 | |
635 | fetch_cnt = 0; |
636 | } |
637 | |
638 | prio_mask = fetch_allow & AM65_CPSW_FETCH_ALLOW_MSK; |
639 | cmd = (cmd_fetch_cnt << AM65_CPSW_FETCH_CNT_OFFSET) | prio_mask; |
640 | |
641 | writel(val: cmd, addr); |
642 | addr += 4; |
643 | } while (fetch_cnt); |
644 | |
645 | return addr; |
646 | } |
647 | |
648 | static int am65_cpsw_est_calc_cmd_num(struct net_device *ndev, |
649 | struct tc_taprio_qopt_offload *taprio, |
650 | int link_speed) |
651 | { |
652 | int i, cmd_cnt, cmd_sum = 0; |
653 | u32 fetch_cnt; |
654 | |
655 | for (i = 0; i < taprio->num_entries; i++) { |
656 | if (taprio->entries[i].command != TC_TAPRIO_CMD_SET_GATES) { |
657 | dev_err(&ndev->dev, "Only SET command is supported" ); |
658 | return -EINVAL; |
659 | } |
660 | |
661 | fetch_cnt = am65_est_cmd_ns_to_cnt(ns: taprio->entries[i].interval, |
662 | link_speed); |
663 | |
664 | cmd_cnt = DIV_ROUND_UP(fetch_cnt, AM65_CPSW_FETCH_CNT_MAX); |
665 | if (!cmd_cnt) |
666 | cmd_cnt++; |
667 | |
668 | cmd_sum += cmd_cnt; |
669 | |
670 | if (!fetch_cnt) |
671 | break; |
672 | } |
673 | |
674 | return cmd_sum; |
675 | } |
676 | |
677 | static int am65_cpsw_est_check_scheds(struct net_device *ndev, |
678 | struct am65_cpsw_est *est_new) |
679 | { |
680 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
681 | int cmd_num; |
682 | |
683 | cmd_num = am65_cpsw_est_calc_cmd_num(ndev, taprio: &est_new->taprio, |
684 | link_speed: port->qos.link_speed); |
685 | if (cmd_num < 0) |
686 | return cmd_num; |
687 | |
688 | if (cmd_num > AM65_CPSW_FETCH_RAM_CMD_NUM / 2) { |
689 | dev_err(&ndev->dev, "No fetch RAM" ); |
690 | return -ENOMEM; |
691 | } |
692 | |
693 | return 0; |
694 | } |
695 | |
696 | static void am65_cpsw_est_set_sched_list(struct net_device *ndev, |
697 | struct am65_cpsw_est *est_new) |
698 | { |
699 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
700 | u32 fetch_cnt, fetch_allow, all_fetch_allow = 0; |
701 | void __iomem *ram_addr, *max_ram_addr; |
702 | struct tc_taprio_sched_entry *entry; |
703 | int i, ram_size; |
704 | |
705 | ram_addr = port->fetch_ram_base; |
706 | ram_size = AM65_CPSW_FETCH_RAM_CMD_NUM * 2; |
707 | ram_addr += est_new->buf * ram_size; |
708 | |
709 | max_ram_addr = ram_size + ram_addr; |
710 | for (i = 0; i < est_new->taprio.num_entries; i++) { |
711 | entry = &est_new->taprio.entries[i]; |
712 | |
713 | fetch_cnt = am65_est_cmd_ns_to_cnt(ns: entry->interval, |
714 | link_speed: port->qos.link_speed); |
715 | fetch_allow = entry->gate_mask; |
716 | if (fetch_allow > AM65_CPSW_FETCH_ALLOW_MAX) |
717 | dev_dbg(&ndev->dev, "fetch_allow > 8 bits: %d\n" , |
718 | fetch_allow); |
719 | |
720 | ram_addr = am65_cpsw_est_set_sched_cmds(addr: ram_addr, fetch_cnt, |
721 | fetch_allow); |
722 | |
723 | if (!fetch_cnt && i < est_new->taprio.num_entries - 1) { |
724 | dev_info(&ndev->dev, |
725 | "next scheds after %d have no impact" , i + 1); |
726 | break; |
727 | } |
728 | |
729 | all_fetch_allow |= fetch_allow; |
730 | } |
731 | |
732 | /* end cmd, enabling non-timed queues for potential over cycle time */ |
733 | if (ram_addr < max_ram_addr) |
734 | writel(val: ~all_fetch_allow & AM65_CPSW_FETCH_ALLOW_MSK, addr: ram_addr); |
735 | } |
736 | |
737 | /* |
738 | * Enable ESTf periodic output, set cycle start time and interval. |
739 | */ |
740 | static int am65_cpsw_timer_set(struct net_device *ndev, |
741 | struct am65_cpsw_est *est_new) |
742 | { |
743 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
744 | struct am65_cpsw_common *common = port->common; |
745 | struct am65_cpts *cpts = common->cpts; |
746 | struct am65_cpts_estf_cfg cfg; |
747 | |
748 | cfg.ns_period = est_new->taprio.cycle_time; |
749 | cfg.ns_start = est_new->taprio.base_time; |
750 | |
751 | return am65_cpts_estf_enable(cpts, idx: port->port_id - 1, cfg: &cfg); |
752 | } |
753 | |
754 | static void am65_cpsw_timer_stop(struct net_device *ndev) |
755 | { |
756 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
757 | struct am65_cpts *cpts = port->common->cpts; |
758 | |
759 | am65_cpts_estf_disable(cpts, idx: port->port_id - 1); |
760 | } |
761 | |
762 | static enum timer_act am65_cpsw_timer_act(struct net_device *ndev, |
763 | struct am65_cpsw_est *est_new) |
764 | { |
765 | struct tc_taprio_qopt_offload *taprio_oper, *taprio_new; |
766 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
767 | struct am65_cpts *cpts = port->common->cpts; |
768 | u64 cur_time; |
769 | s64 diff; |
770 | |
771 | if (!port->qos.est_oper) |
772 | return TACT_PROG; |
773 | |
774 | taprio_new = &est_new->taprio; |
775 | taprio_oper = &port->qos.est_oper->taprio; |
776 | |
777 | if (taprio_new->cycle_time != taprio_oper->cycle_time) |
778 | return TACT_NEED_STOP; |
779 | |
780 | /* in order to avoid timer reset get base_time form oper taprio */ |
781 | if (!taprio_new->base_time && taprio_oper) |
782 | taprio_new->base_time = taprio_oper->base_time; |
783 | |
784 | if (taprio_new->base_time == taprio_oper->base_time) |
785 | return TACT_SKIP_PROG; |
786 | |
787 | /* base times are cycle synchronized */ |
788 | diff = taprio_new->base_time - taprio_oper->base_time; |
789 | diff = diff < 0 ? -diff : diff; |
790 | if (diff % taprio_new->cycle_time) |
791 | return TACT_NEED_STOP; |
792 | |
793 | cur_time = am65_cpts_ns_gettime(cpts); |
794 | if (taprio_new->base_time <= cur_time + taprio_new->cycle_time) |
795 | return TACT_SKIP_PROG; |
796 | |
797 | /* TODO: Admin schedule at future time is not currently supported */ |
798 | return TACT_NEED_STOP; |
799 | } |
800 | |
801 | static void am65_cpsw_stop_est(struct net_device *ndev) |
802 | { |
803 | am65_cpsw_est_set(ndev, enable: 0); |
804 | am65_cpsw_timer_stop(ndev); |
805 | } |
806 | |
807 | static void am65_cpsw_taprio_destroy(struct net_device *ndev) |
808 | { |
809 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
810 | |
811 | am65_cpsw_stop_est(ndev); |
812 | |
813 | devm_kfree(dev: &ndev->dev, p: port->qos.est_admin); |
814 | devm_kfree(dev: &ndev->dev, p: port->qos.est_oper); |
815 | |
816 | port->qos.est_oper = NULL; |
817 | port->qos.est_admin = NULL; |
818 | |
819 | am65_cpsw_reset_tc_mqprio(ndev); |
820 | } |
821 | |
822 | static void am65_cpsw_cp_taprio(struct tc_taprio_qopt_offload *from, |
823 | struct tc_taprio_qopt_offload *to) |
824 | { |
825 | int i; |
826 | |
827 | *to = *from; |
828 | for (i = 0; i < from->num_entries; i++) |
829 | to->entries[i] = from->entries[i]; |
830 | } |
831 | |
832 | static int am65_cpsw_taprio_replace(struct net_device *ndev, |
833 | struct tc_taprio_qopt_offload *taprio) |
834 | { |
835 | struct am65_cpsw_common *common = am65_ndev_to_common(ndev); |
836 | struct netlink_ext_ack *extack = taprio->mqprio.extack; |
837 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
838 | struct am65_cpts *cpts = common->cpts; |
839 | struct am65_cpsw_est *est_new; |
840 | int ret, tact; |
841 | |
842 | if (!netif_running(dev: ndev)) { |
843 | NL_SET_ERR_MSG_MOD(extack, "interface is down, link speed unknown" ); |
844 | return -ENETDOWN; |
845 | } |
846 | |
847 | if (common->pf_p0_rx_ptype_rrobin) { |
848 | NL_SET_ERR_MSG_MOD(extack, |
849 | "p0-rx-ptype-rrobin flag conflicts with taprio qdisc" ); |
850 | return -EINVAL; |
851 | } |
852 | |
853 | if (port->qos.link_speed == SPEED_UNKNOWN) |
854 | return -ENOLINK; |
855 | |
856 | if (taprio->cycle_time_extension) { |
857 | NL_SET_ERR_MSG_MOD(extack, |
858 | "cycle time extension not supported" ); |
859 | return -EOPNOTSUPP; |
860 | } |
861 | |
862 | est_new = devm_kzalloc(dev: &ndev->dev, |
863 | struct_size(est_new, taprio.entries, taprio->num_entries), |
864 | GFP_KERNEL); |
865 | if (!est_new) |
866 | return -ENOMEM; |
867 | |
868 | ret = am65_cpsw_setup_mqprio(ndev, type_data: &taprio->mqprio); |
869 | if (ret) |
870 | return ret; |
871 | |
872 | am65_cpsw_cp_taprio(from: taprio, to: &est_new->taprio); |
873 | |
874 | am65_cpsw_est_update_state(ndev); |
875 | |
876 | ret = am65_cpsw_est_check_scheds(ndev, est_new); |
877 | if (ret < 0) |
878 | goto fail; |
879 | |
880 | tact = am65_cpsw_timer_act(ndev, est_new); |
881 | if (tact == TACT_NEED_STOP) { |
882 | NL_SET_ERR_MSG_MOD(extack, |
883 | "Can't toggle estf timer, stop taprio first" ); |
884 | ret = -EINVAL; |
885 | goto fail; |
886 | } |
887 | |
888 | if (tact == TACT_PROG) |
889 | am65_cpsw_timer_stop(ndev); |
890 | |
891 | if (!est_new->taprio.base_time) |
892 | est_new->taprio.base_time = am65_cpts_ns_gettime(cpts); |
893 | |
894 | am65_cpsw_port_est_get_buf_num(ndev, est_new); |
895 | am65_cpsw_est_set_sched_list(ndev, est_new); |
896 | am65_cpsw_port_est_assign_buf_num(ndev, buf_num: est_new->buf); |
897 | |
898 | am65_cpsw_est_set(ndev, enable: 1); |
899 | |
900 | if (tact == TACT_PROG) { |
901 | ret = am65_cpsw_timer_set(ndev, est_new); |
902 | if (ret) { |
903 | NL_SET_ERR_MSG_MOD(extack, |
904 | "Failed to set cycle time" ); |
905 | goto fail; |
906 | } |
907 | } |
908 | |
909 | devm_kfree(dev: &ndev->dev, p: port->qos.est_admin); |
910 | port->qos.est_admin = est_new; |
911 | am65_cpsw_iet_change_preemptible_tcs(port, preemptible_tcs: taprio->mqprio.preemptible_tcs); |
912 | |
913 | return 0; |
914 | |
915 | fail: |
916 | am65_cpsw_reset_tc_mqprio(ndev); |
917 | devm_kfree(dev: &ndev->dev, p: est_new); |
918 | return ret; |
919 | } |
920 | |
921 | static void am65_cpsw_est_link_up(struct net_device *ndev, int link_speed) |
922 | { |
923 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
924 | ktime_t cur_time; |
925 | s64 delta; |
926 | |
927 | if (!am65_cpsw_port_est_enabled(port)) |
928 | return; |
929 | |
930 | if (port->qos.link_down_time) { |
931 | cur_time = ktime_get(); |
932 | delta = ktime_us_delta(later: cur_time, earlier: port->qos.link_down_time); |
933 | if (delta > USEC_PER_SEC) { |
934 | dev_err(&ndev->dev, |
935 | "Link has been lost too long, stopping TAS" ); |
936 | goto purge_est; |
937 | } |
938 | } |
939 | |
940 | return; |
941 | |
942 | purge_est: |
943 | am65_cpsw_taprio_destroy(ndev); |
944 | } |
945 | |
946 | static int am65_cpsw_setup_taprio(struct net_device *ndev, void *type_data) |
947 | { |
948 | struct tc_taprio_qopt_offload *taprio = type_data; |
949 | int err = 0; |
950 | |
951 | switch (taprio->cmd) { |
952 | case TAPRIO_CMD_REPLACE: |
953 | err = am65_cpsw_taprio_replace(ndev, taprio); |
954 | break; |
955 | case TAPRIO_CMD_DESTROY: |
956 | am65_cpsw_taprio_destroy(ndev); |
957 | break; |
958 | default: |
959 | err = -EOPNOTSUPP; |
960 | } |
961 | |
962 | return err; |
963 | } |
964 | |
965 | static int am65_cpsw_tc_query_caps(struct net_device *ndev, void *type_data) |
966 | { |
967 | struct tc_query_caps_base *base = type_data; |
968 | |
969 | switch (base->type) { |
970 | case TC_SETUP_QDISC_MQPRIO: { |
971 | struct tc_mqprio_caps *caps = base->caps; |
972 | |
973 | caps->validate_queue_counts = true; |
974 | |
975 | return 0; |
976 | } |
977 | |
978 | case TC_SETUP_QDISC_TAPRIO: { |
979 | struct tc_taprio_caps *caps = base->caps; |
980 | |
981 | caps->gate_mask_per_txq = true; |
982 | |
983 | return 0; |
984 | } |
985 | default: |
986 | return -EOPNOTSUPP; |
987 | } |
988 | } |
989 | |
990 | static int am65_cpsw_qos_clsflower_add_policer(struct am65_cpsw_port *port, |
991 | struct netlink_ext_ack *extack, |
992 | struct flow_cls_offload *cls, |
993 | u64 rate_pkt_ps) |
994 | { |
995 | struct flow_rule *rule = flow_cls_offload_flow_rule(flow_cmd: cls); |
996 | struct flow_dissector *dissector = rule->match.dissector; |
997 | static const u8 mc_mac[] = {0x01, 0x00, 0x00, 0x00, 0x00, 0x00}; |
998 | struct am65_cpsw_qos *qos = &port->qos; |
999 | struct flow_match_eth_addrs match; |
1000 | int ret; |
1001 | |
1002 | if (dissector->used_keys & |
1003 | ~(BIT_ULL(FLOW_DISSECTOR_KEY_BASIC) | |
1004 | BIT_ULL(FLOW_DISSECTOR_KEY_CONTROL) | |
1005 | BIT_ULL(FLOW_DISSECTOR_KEY_ETH_ADDRS))) { |
1006 | NL_SET_ERR_MSG_MOD(extack, |
1007 | "Unsupported keys used" ); |
1008 | return -EOPNOTSUPP; |
1009 | } |
1010 | |
1011 | if (!flow_rule_match_key(rule, key: FLOW_DISSECTOR_KEY_ETH_ADDRS)) { |
1012 | NL_SET_ERR_MSG_MOD(extack, "Not matching on eth address" ); |
1013 | return -EOPNOTSUPP; |
1014 | } |
1015 | |
1016 | flow_rule_match_eth_addrs(rule, out: &match); |
1017 | |
1018 | if (!is_zero_ether_addr(addr: match.mask->src)) { |
1019 | NL_SET_ERR_MSG_MOD(extack, |
1020 | "Matching on source MAC not supported" ); |
1021 | return -EOPNOTSUPP; |
1022 | } |
1023 | |
1024 | if (is_broadcast_ether_addr(addr: match.key->dst) && |
1025 | is_broadcast_ether_addr(addr: match.mask->dst)) { |
1026 | ret = cpsw_ale_rx_ratelimit_bc(ale: port->common->ale, port: port->port_id, ratelimit_pps: rate_pkt_ps); |
1027 | if (ret) |
1028 | return ret; |
1029 | |
1030 | qos->ale_bc_ratelimit.cookie = cls->cookie; |
1031 | qos->ale_bc_ratelimit.rate_packet_ps = rate_pkt_ps; |
1032 | } else if (ether_addr_equal_unaligned(addr1: match.key->dst, addr2: mc_mac) && |
1033 | ether_addr_equal_unaligned(addr1: match.mask->dst, addr2: mc_mac)) { |
1034 | ret = cpsw_ale_rx_ratelimit_mc(ale: port->common->ale, port: port->port_id, ratelimit_pps: rate_pkt_ps); |
1035 | if (ret) |
1036 | return ret; |
1037 | |
1038 | qos->ale_mc_ratelimit.cookie = cls->cookie; |
1039 | qos->ale_mc_ratelimit.rate_packet_ps = rate_pkt_ps; |
1040 | } else { |
1041 | NL_SET_ERR_MSG_MOD(extack, "Not supported matching key" ); |
1042 | return -EOPNOTSUPP; |
1043 | } |
1044 | |
1045 | return 0; |
1046 | } |
1047 | |
1048 | static int am65_cpsw_qos_clsflower_policer_validate(const struct flow_action *action, |
1049 | const struct flow_action_entry *act, |
1050 | struct netlink_ext_ack *extack) |
1051 | { |
1052 | if (act->police.exceed.act_id != FLOW_ACTION_DROP) { |
1053 | NL_SET_ERR_MSG_MOD(extack, |
1054 | "Offload not supported when exceed action is not drop" ); |
1055 | return -EOPNOTSUPP; |
1056 | } |
1057 | |
1058 | if (act->police.notexceed.act_id != FLOW_ACTION_PIPE && |
1059 | act->police.notexceed.act_id != FLOW_ACTION_ACCEPT) { |
1060 | NL_SET_ERR_MSG_MOD(extack, |
1061 | "Offload not supported when conform action is not pipe or ok" ); |
1062 | return -EOPNOTSUPP; |
1063 | } |
1064 | |
1065 | if (act->police.notexceed.act_id == FLOW_ACTION_ACCEPT && |
1066 | !flow_action_is_last_entry(action, entry: act)) { |
1067 | NL_SET_ERR_MSG_MOD(extack, |
1068 | "Offload not supported when conform action is ok, but action is not last" ); |
1069 | return -EOPNOTSUPP; |
1070 | } |
1071 | |
1072 | if (act->police.rate_bytes_ps || act->police.peakrate_bytes_ps || |
1073 | act->police.avrate || act->police.overhead) { |
1074 | NL_SET_ERR_MSG_MOD(extack, |
1075 | "Offload not supported when bytes per second/peakrate/avrate/overhead is configured" ); |
1076 | return -EOPNOTSUPP; |
1077 | } |
1078 | |
1079 | return 0; |
1080 | } |
1081 | |
1082 | static int am65_cpsw_qos_configure_clsflower(struct am65_cpsw_port *port, |
1083 | struct flow_cls_offload *cls) |
1084 | { |
1085 | struct flow_rule *rule = flow_cls_offload_flow_rule(flow_cmd: cls); |
1086 | struct netlink_ext_ack *extack = cls->common.extack; |
1087 | const struct flow_action_entry *act; |
1088 | int i, ret; |
1089 | |
1090 | flow_action_for_each(i, act, &rule->action) { |
1091 | switch (act->id) { |
1092 | case FLOW_ACTION_POLICE: |
1093 | ret = am65_cpsw_qos_clsflower_policer_validate(action: &rule->action, act, extack); |
1094 | if (ret) |
1095 | return ret; |
1096 | |
1097 | return am65_cpsw_qos_clsflower_add_policer(port, extack, cls, |
1098 | rate_pkt_ps: act->police.rate_pkt_ps); |
1099 | default: |
1100 | NL_SET_ERR_MSG_MOD(extack, |
1101 | "Action not supported" ); |
1102 | return -EOPNOTSUPP; |
1103 | } |
1104 | } |
1105 | return -EOPNOTSUPP; |
1106 | } |
1107 | |
1108 | static int am65_cpsw_qos_delete_clsflower(struct am65_cpsw_port *port, struct flow_cls_offload *cls) |
1109 | { |
1110 | struct am65_cpsw_qos *qos = &port->qos; |
1111 | |
1112 | if (cls->cookie == qos->ale_bc_ratelimit.cookie) { |
1113 | qos->ale_bc_ratelimit.cookie = 0; |
1114 | qos->ale_bc_ratelimit.rate_packet_ps = 0; |
1115 | cpsw_ale_rx_ratelimit_bc(ale: port->common->ale, port: port->port_id, ratelimit_pps: 0); |
1116 | } |
1117 | |
1118 | if (cls->cookie == qos->ale_mc_ratelimit.cookie) { |
1119 | qos->ale_mc_ratelimit.cookie = 0; |
1120 | qos->ale_mc_ratelimit.rate_packet_ps = 0; |
1121 | cpsw_ale_rx_ratelimit_mc(ale: port->common->ale, port: port->port_id, ratelimit_pps: 0); |
1122 | } |
1123 | |
1124 | return 0; |
1125 | } |
1126 | |
1127 | static int am65_cpsw_qos_setup_tc_clsflower(struct am65_cpsw_port *port, |
1128 | struct flow_cls_offload *cls_flower) |
1129 | { |
1130 | switch (cls_flower->command) { |
1131 | case FLOW_CLS_REPLACE: |
1132 | return am65_cpsw_qos_configure_clsflower(port, cls: cls_flower); |
1133 | case FLOW_CLS_DESTROY: |
1134 | return am65_cpsw_qos_delete_clsflower(port, cls: cls_flower); |
1135 | default: |
1136 | return -EOPNOTSUPP; |
1137 | } |
1138 | } |
1139 | |
1140 | static int am65_cpsw_qos_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv) |
1141 | { |
1142 | struct am65_cpsw_port *port = cb_priv; |
1143 | |
1144 | if (!tc_cls_can_offload_and_chain0(dev: port->ndev, common: type_data)) |
1145 | return -EOPNOTSUPP; |
1146 | |
1147 | switch (type) { |
1148 | case TC_SETUP_CLSFLOWER: |
1149 | return am65_cpsw_qos_setup_tc_clsflower(port, cls_flower: type_data); |
1150 | default: |
1151 | return -EOPNOTSUPP; |
1152 | } |
1153 | } |
1154 | |
1155 | static LIST_HEAD(am65_cpsw_qos_block_cb_list); |
1156 | |
1157 | static int am65_cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f) |
1158 | { |
1159 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
1160 | |
1161 | return flow_block_cb_setup_simple(f, driver_list: &am65_cpsw_qos_block_cb_list, |
1162 | cb: am65_cpsw_qos_setup_tc_block_cb, |
1163 | cb_ident: port, cb_priv: port, ingress_only: true); |
1164 | } |
1165 | |
1166 | static void |
1167 | am65_cpsw_qos_tx_p0_rate_apply(struct am65_cpsw_common *common, |
1168 | int tx_ch, u32 rate_mbps) |
1169 | { |
1170 | struct am65_cpsw_host *host = am65_common_get_host(common); |
1171 | u32 ch_cir; |
1172 | int i; |
1173 | |
1174 | ch_cir = am65_cpsw_qos_tx_rate_calc(rate_mbps, bus_freq: common->bus_freq); |
1175 | writel(val: ch_cir, addr: host->port_base + AM65_CPSW_PN_REG_PRI_CIR(tx_ch)); |
1176 | |
1177 | /* update rates for every port tx queues */ |
1178 | for (i = 0; i < common->port_num; i++) { |
1179 | struct net_device *ndev = common->ports[i].ndev; |
1180 | |
1181 | if (!ndev) |
1182 | continue; |
1183 | netdev_get_tx_queue(dev: ndev, index: tx_ch)->tx_maxrate = rate_mbps; |
1184 | } |
1185 | } |
1186 | |
1187 | int am65_cpsw_qos_ndo_tx_p0_set_maxrate(struct net_device *ndev, |
1188 | int queue, u32 rate_mbps) |
1189 | { |
1190 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
1191 | struct am65_cpsw_common *common = port->common; |
1192 | struct am65_cpsw_tx_chn *tx_chn; |
1193 | u32 ch_rate, tx_ch_rate_msk_new; |
1194 | u32 ch_msk = 0; |
1195 | int ret; |
1196 | |
1197 | dev_dbg(common->dev, "apply TX%d rate limiting %uMbps tx_rate_msk%x\n" , |
1198 | queue, rate_mbps, common->tx_ch_rate_msk); |
1199 | |
1200 | if (common->pf_p0_rx_ptype_rrobin) { |
1201 | dev_err(common->dev, "TX Rate Limiting failed - rrobin mode\n" ); |
1202 | return -EINVAL; |
1203 | } |
1204 | |
1205 | ch_rate = netdev_get_tx_queue(dev: ndev, index: queue)->tx_maxrate; |
1206 | if (ch_rate == rate_mbps) |
1207 | return 0; |
1208 | |
1209 | ret = pm_runtime_get_sync(dev: common->dev); |
1210 | if (ret < 0) { |
1211 | pm_runtime_put_noidle(dev: common->dev); |
1212 | return ret; |
1213 | } |
1214 | ret = 0; |
1215 | |
1216 | tx_ch_rate_msk_new = common->tx_ch_rate_msk; |
1217 | if (rate_mbps && !(tx_ch_rate_msk_new & BIT(queue))) { |
1218 | tx_ch_rate_msk_new |= BIT(queue); |
1219 | ch_msk = GENMASK(common->tx_ch_num - 1, queue); |
1220 | ch_msk = tx_ch_rate_msk_new ^ ch_msk; |
1221 | } else if (!rate_mbps) { |
1222 | tx_ch_rate_msk_new &= ~BIT(queue); |
1223 | ch_msk = queue ? GENMASK(queue - 1, 0) : 0; |
1224 | ch_msk = tx_ch_rate_msk_new & ch_msk; |
1225 | } |
1226 | |
1227 | if (ch_msk) { |
1228 | dev_err(common->dev, "TX rate limiting has to be enabled sequentially hi->lo tx_rate_msk:%x tx_rate_msk_new:%x\n" , |
1229 | common->tx_ch_rate_msk, tx_ch_rate_msk_new); |
1230 | ret = -EINVAL; |
1231 | goto exit_put; |
1232 | } |
1233 | |
1234 | tx_chn = &common->tx_chns[queue]; |
1235 | tx_chn->rate_mbps = rate_mbps; |
1236 | common->tx_ch_rate_msk = tx_ch_rate_msk_new; |
1237 | |
1238 | if (!common->usage_count) |
1239 | /* will be applied on next netif up */ |
1240 | goto exit_put; |
1241 | |
1242 | am65_cpsw_qos_tx_p0_rate_apply(common, tx_ch: queue, rate_mbps); |
1243 | |
1244 | exit_put: |
1245 | pm_runtime_put(dev: common->dev); |
1246 | return ret; |
1247 | } |
1248 | |
1249 | void am65_cpsw_qos_tx_p0_rate_init(struct am65_cpsw_common *common) |
1250 | { |
1251 | struct am65_cpsw_host *host = am65_common_get_host(common); |
1252 | int tx_ch; |
1253 | |
1254 | for (tx_ch = 0; tx_ch < common->tx_ch_num; tx_ch++) { |
1255 | struct am65_cpsw_tx_chn *tx_chn = &common->tx_chns[tx_ch]; |
1256 | u32 ch_cir; |
1257 | |
1258 | if (!tx_chn->rate_mbps) |
1259 | continue; |
1260 | |
1261 | ch_cir = am65_cpsw_qos_tx_rate_calc(rate_mbps: tx_chn->rate_mbps, |
1262 | bus_freq: common->bus_freq); |
1263 | writel(val: ch_cir, |
1264 | addr: host->port_base + AM65_CPSW_PN_REG_PRI_CIR(tx_ch)); |
1265 | } |
1266 | } |
1267 | |
1268 | int am65_cpsw_qos_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type, |
1269 | void *type_data) |
1270 | { |
1271 | switch (type) { |
1272 | case TC_QUERY_CAPS: |
1273 | return am65_cpsw_tc_query_caps(ndev, type_data); |
1274 | case TC_SETUP_QDISC_TAPRIO: |
1275 | return am65_cpsw_setup_taprio(ndev, type_data); |
1276 | case TC_SETUP_QDISC_MQPRIO: |
1277 | return am65_cpsw_setup_mqprio(ndev, type_data); |
1278 | case TC_SETUP_BLOCK: |
1279 | return am65_cpsw_qos_setup_tc_block(ndev, f: type_data); |
1280 | default: |
1281 | return -EOPNOTSUPP; |
1282 | } |
1283 | } |
1284 | |
1285 | void am65_cpsw_qos_link_up(struct net_device *ndev, int link_speed) |
1286 | { |
1287 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
1288 | |
1289 | port->qos.link_speed = link_speed; |
1290 | am65_cpsw_tx_pn_shaper_apply(port); |
1291 | am65_cpsw_iet_link_state_update(ndev); |
1292 | |
1293 | am65_cpsw_est_link_up(ndev, link_speed); |
1294 | port->qos.link_down_time = 0; |
1295 | } |
1296 | |
1297 | void am65_cpsw_qos_link_down(struct net_device *ndev) |
1298 | { |
1299 | struct am65_cpsw_port *port = am65_ndev_to_port(ndev); |
1300 | |
1301 | port->qos.link_speed = SPEED_UNKNOWN; |
1302 | am65_cpsw_tx_pn_shaper_apply(port); |
1303 | am65_cpsw_iet_link_state_update(ndev); |
1304 | |
1305 | if (!port->qos.link_down_time) |
1306 | port->qos.link_down_time = ktime_get(); |
1307 | } |
1308 | |