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

source code of linux/drivers/net/ethernet/ti/am65-cpsw-qos.c