1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * Texas Instruments Ethernet Switch Driver |
4 | * |
5 | * Copyright (C) 2019 Texas Instruments |
6 | */ |
7 | |
8 | #include <linux/bpf.h> |
9 | #include <linux/bpf_trace.h> |
10 | #include <linux/if_ether.h> |
11 | #include <linux/if_vlan.h> |
12 | #include <linux/kmemleak.h> |
13 | #include <linux/module.h> |
14 | #include <linux/netdevice.h> |
15 | #include <linux/net_tstamp.h> |
16 | #include <linux/of.h> |
17 | #include <linux/phy.h> |
18 | #include <linux/platform_device.h> |
19 | #include <linux/pm_runtime.h> |
20 | #include <linux/skbuff.h> |
21 | #include <net/page_pool/helpers.h> |
22 | #include <net/pkt_cls.h> |
23 | #include <net/pkt_sched.h> |
24 | |
25 | #include "cpsw.h" |
26 | #include "cpts.h" |
27 | #include "cpsw_ale.h" |
28 | #include "cpsw_priv.h" |
29 | #include "cpsw_sl.h" |
30 | #include "davinci_cpdma.h" |
31 | |
32 | #define CPTS_N_ETX_TS 4 |
33 | |
34 | int (*cpsw_slave_index)(struct cpsw_common *cpsw, struct cpsw_priv *priv); |
35 | |
36 | void cpsw_intr_enable(struct cpsw_common *cpsw) |
37 | { |
38 | writel_relaxed(0xFF, &cpsw->wr_regs->tx_en); |
39 | writel_relaxed(0xFF, &cpsw->wr_regs->rx_en); |
40 | |
41 | cpdma_ctlr_int_ctrl(ctlr: cpsw->dma, enable: true); |
42 | } |
43 | |
44 | void cpsw_intr_disable(struct cpsw_common *cpsw) |
45 | { |
46 | writel_relaxed(0, &cpsw->wr_regs->tx_en); |
47 | writel_relaxed(0, &cpsw->wr_regs->rx_en); |
48 | |
49 | cpdma_ctlr_int_ctrl(ctlr: cpsw->dma, enable: false); |
50 | } |
51 | |
52 | void cpsw_tx_handler(void *token, int len, int status) |
53 | { |
54 | struct cpsw_meta_xdp *xmeta; |
55 | struct xdp_frame *xdpf; |
56 | struct net_device *ndev; |
57 | struct netdev_queue *txq; |
58 | struct sk_buff *skb; |
59 | int ch; |
60 | |
61 | if (cpsw_is_xdpf_handle(handle: token)) { |
62 | xdpf = cpsw_handle_to_xdpf(handle: token); |
63 | xmeta = (void *)xdpf + CPSW_XMETA_OFFSET; |
64 | ndev = xmeta->ndev; |
65 | ch = xmeta->ch; |
66 | xdp_return_frame(xdpf); |
67 | } else { |
68 | skb = token; |
69 | ndev = skb->dev; |
70 | ch = skb_get_queue_mapping(skb); |
71 | cpts_tx_timestamp(ndev_to_cpsw(ndev)->cpts, skb); |
72 | dev_kfree_skb_any(skb); |
73 | } |
74 | |
75 | /* Check whether the queue is stopped due to stalled tx dma, if the |
76 | * queue is stopped then start the queue as we have free desc for tx |
77 | */ |
78 | txq = netdev_get_tx_queue(dev: ndev, index: ch); |
79 | if (unlikely(netif_tx_queue_stopped(txq))) |
80 | netif_tx_wake_queue(dev_queue: txq); |
81 | |
82 | ndev->stats.tx_packets++; |
83 | ndev->stats.tx_bytes += len; |
84 | } |
85 | |
86 | irqreturn_t cpsw_tx_interrupt(int irq, void *dev_id) |
87 | { |
88 | struct cpsw_common *cpsw = dev_id; |
89 | |
90 | writel(val: 0, addr: &cpsw->wr_regs->tx_en); |
91 | cpdma_ctlr_eoi(ctlr: cpsw->dma, CPDMA_EOI_TX); |
92 | |
93 | if (cpsw->quirk_irq) { |
94 | disable_irq_nosync(irq: cpsw->irqs_table[1]); |
95 | cpsw->tx_irq_disabled = true; |
96 | } |
97 | |
98 | napi_schedule(n: &cpsw->napi_tx); |
99 | return IRQ_HANDLED; |
100 | } |
101 | |
102 | irqreturn_t cpsw_rx_interrupt(int irq, void *dev_id) |
103 | { |
104 | struct cpsw_common *cpsw = dev_id; |
105 | |
106 | writel(val: 0, addr: &cpsw->wr_regs->rx_en); |
107 | cpdma_ctlr_eoi(ctlr: cpsw->dma, CPDMA_EOI_RX); |
108 | |
109 | if (cpsw->quirk_irq) { |
110 | disable_irq_nosync(irq: cpsw->irqs_table[0]); |
111 | cpsw->rx_irq_disabled = true; |
112 | } |
113 | |
114 | napi_schedule(n: &cpsw->napi_rx); |
115 | return IRQ_HANDLED; |
116 | } |
117 | |
118 | irqreturn_t cpsw_misc_interrupt(int irq, void *dev_id) |
119 | { |
120 | struct cpsw_common *cpsw = dev_id; |
121 | |
122 | writel(val: 0, addr: &cpsw->wr_regs->misc_en); |
123 | cpdma_ctlr_eoi(ctlr: cpsw->dma, CPDMA_EOI_MISC); |
124 | cpts_misc_interrupt(cpts: cpsw->cpts); |
125 | writel(val: 0x10, addr: &cpsw->wr_regs->misc_en); |
126 | |
127 | return IRQ_HANDLED; |
128 | } |
129 | |
130 | int cpsw_tx_mq_poll(struct napi_struct *napi_tx, int budget) |
131 | { |
132 | struct cpsw_common *cpsw = napi_to_cpsw(napi_tx); |
133 | int num_tx, cur_budget, ch; |
134 | u32 ch_map; |
135 | struct cpsw_vector *txv; |
136 | |
137 | /* process every unprocessed channel */ |
138 | ch_map = cpdma_ctrl_txchs_state(ctlr: cpsw->dma); |
139 | for (ch = 0, num_tx = 0; ch_map & 0xff; ch_map <<= 1, ch++) { |
140 | if (!(ch_map & 0x80)) |
141 | continue; |
142 | |
143 | txv = &cpsw->txv[ch]; |
144 | if (unlikely(txv->budget > budget - num_tx)) |
145 | cur_budget = budget - num_tx; |
146 | else |
147 | cur_budget = txv->budget; |
148 | |
149 | num_tx += cpdma_chan_process(chan: txv->ch, quota: cur_budget); |
150 | if (num_tx >= budget) |
151 | break; |
152 | } |
153 | |
154 | if (num_tx < budget) { |
155 | napi_complete(n: napi_tx); |
156 | writel(val: 0xff, addr: &cpsw->wr_regs->tx_en); |
157 | } |
158 | |
159 | return num_tx; |
160 | } |
161 | |
162 | int cpsw_tx_poll(struct napi_struct *napi_tx, int budget) |
163 | { |
164 | struct cpsw_common *cpsw = napi_to_cpsw(napi_tx); |
165 | int num_tx; |
166 | |
167 | num_tx = cpdma_chan_process(chan: cpsw->txv[0].ch, quota: budget); |
168 | if (num_tx < budget) { |
169 | napi_complete(n: napi_tx); |
170 | writel(val: 0xff, addr: &cpsw->wr_regs->tx_en); |
171 | if (cpsw->tx_irq_disabled) { |
172 | cpsw->tx_irq_disabled = false; |
173 | enable_irq(irq: cpsw->irqs_table[1]); |
174 | } |
175 | } |
176 | |
177 | return num_tx; |
178 | } |
179 | |
180 | int cpsw_rx_mq_poll(struct napi_struct *napi_rx, int budget) |
181 | { |
182 | struct cpsw_common *cpsw = napi_to_cpsw(napi_rx); |
183 | int num_rx, cur_budget, ch; |
184 | u32 ch_map; |
185 | struct cpsw_vector *rxv; |
186 | |
187 | /* process every unprocessed channel */ |
188 | ch_map = cpdma_ctrl_rxchs_state(ctlr: cpsw->dma); |
189 | for (ch = 0, num_rx = 0; ch_map; ch_map >>= 1, ch++) { |
190 | if (!(ch_map & 0x01)) |
191 | continue; |
192 | |
193 | rxv = &cpsw->rxv[ch]; |
194 | if (unlikely(rxv->budget > budget - num_rx)) |
195 | cur_budget = budget - num_rx; |
196 | else |
197 | cur_budget = rxv->budget; |
198 | |
199 | num_rx += cpdma_chan_process(chan: rxv->ch, quota: cur_budget); |
200 | if (num_rx >= budget) |
201 | break; |
202 | } |
203 | |
204 | if (num_rx < budget) { |
205 | napi_complete_done(n: napi_rx, work_done: num_rx); |
206 | writel(val: 0xff, addr: &cpsw->wr_regs->rx_en); |
207 | } |
208 | |
209 | return num_rx; |
210 | } |
211 | |
212 | int cpsw_rx_poll(struct napi_struct *napi_rx, int budget) |
213 | { |
214 | struct cpsw_common *cpsw = napi_to_cpsw(napi_rx); |
215 | int num_rx; |
216 | |
217 | num_rx = cpdma_chan_process(chan: cpsw->rxv[0].ch, quota: budget); |
218 | if (num_rx < budget) { |
219 | napi_complete_done(n: napi_rx, work_done: num_rx); |
220 | writel(val: 0xff, addr: &cpsw->wr_regs->rx_en); |
221 | if (cpsw->rx_irq_disabled) { |
222 | cpsw->rx_irq_disabled = false; |
223 | enable_irq(irq: cpsw->irqs_table[0]); |
224 | } |
225 | } |
226 | |
227 | return num_rx; |
228 | } |
229 | |
230 | void cpsw_rx_vlan_encap(struct sk_buff *skb) |
231 | { |
232 | struct cpsw_priv *priv = netdev_priv(dev: skb->dev); |
233 | u32 rx_vlan_encap_hdr = *((u32 *)skb->data); |
234 | struct cpsw_common *cpsw = priv->cpsw; |
235 | u16 vtag, vid, prio, pkt_type; |
236 | |
237 | /* Remove VLAN header encapsulation word */ |
238 | skb_pull(skb, CPSW_RX_VLAN_ENCAP_HDR_SIZE); |
239 | |
240 | pkt_type = (rx_vlan_encap_hdr >> |
241 | CPSW_RX_VLAN_ENCAP_HDR_PKT_TYPE_SHIFT) & |
242 | CPSW_RX_VLAN_ENCAP_HDR_PKT_TYPE_MSK; |
243 | /* Ignore unknown & Priority-tagged packets*/ |
244 | if (pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_RESERV || |
245 | pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_PRIO_TAG) |
246 | return; |
247 | |
248 | vid = (rx_vlan_encap_hdr >> |
249 | CPSW_RX_VLAN_ENCAP_HDR_VID_SHIFT) & |
250 | VLAN_VID_MASK; |
251 | /* Ignore vid 0 and pass packet as is */ |
252 | if (!vid) |
253 | return; |
254 | |
255 | /* Untag P0 packets if set for vlan */ |
256 | if (!cpsw_ale_get_vlan_p0_untag(ale: cpsw->ale, vid)) { |
257 | prio = (rx_vlan_encap_hdr >> |
258 | CPSW_RX_VLAN_ENCAP_HDR_PRIO_SHIFT) & |
259 | CPSW_RX_VLAN_ENCAP_HDR_PRIO_MSK; |
260 | |
261 | vtag = (prio << VLAN_PRIO_SHIFT) | vid; |
262 | __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlan_tci: vtag); |
263 | } |
264 | |
265 | /* strip vlan tag for VLAN-tagged packet */ |
266 | if (pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_VLAN_TAG) { |
267 | memmove(skb->data + VLAN_HLEN, skb->data, 2 * ETH_ALEN); |
268 | skb_pull(skb, VLAN_HLEN); |
269 | } |
270 | } |
271 | |
272 | void cpsw_set_slave_mac(struct cpsw_slave *slave, struct cpsw_priv *priv) |
273 | { |
274 | slave_write(slave, mac_hi(priv->mac_addr), SA_HI); |
275 | slave_write(slave, mac_lo(priv->mac_addr), SA_LO); |
276 | } |
277 | |
278 | void soft_reset(const char *module, void __iomem *reg) |
279 | { |
280 | unsigned long timeout = jiffies + HZ; |
281 | |
282 | writel_relaxed(1, reg); |
283 | do { |
284 | cpu_relax(); |
285 | } while ((readl_relaxed(reg) & 1) && time_after(timeout, jiffies)); |
286 | |
287 | WARN(readl_relaxed(reg) & 1, "failed to soft-reset %s\n" , module); |
288 | } |
289 | |
290 | void cpsw_ndo_tx_timeout(struct net_device *ndev, unsigned int txqueue) |
291 | { |
292 | struct cpsw_priv *priv = netdev_priv(dev: ndev); |
293 | struct cpsw_common *cpsw = priv->cpsw; |
294 | int ch; |
295 | |
296 | cpsw_err(priv, tx_err, "transmit timeout, restarting dma\n" ); |
297 | ndev->stats.tx_errors++; |
298 | cpsw_intr_disable(cpsw); |
299 | for (ch = 0; ch < cpsw->tx_ch_num; ch++) { |
300 | cpdma_chan_stop(chan: cpsw->txv[ch].ch); |
301 | cpdma_chan_start(chan: cpsw->txv[ch].ch); |
302 | } |
303 | |
304 | cpsw_intr_enable(cpsw); |
305 | netif_trans_update(dev: ndev); |
306 | netif_tx_wake_all_queues(dev: ndev); |
307 | } |
308 | |
309 | static int cpsw_get_common_speed(struct cpsw_common *cpsw) |
310 | { |
311 | int i, speed; |
312 | |
313 | for (i = 0, speed = 0; i < cpsw->data.slaves; i++) |
314 | if (cpsw->slaves[i].phy && cpsw->slaves[i].phy->link) |
315 | speed += cpsw->slaves[i].phy->speed; |
316 | |
317 | return speed; |
318 | } |
319 | |
320 | int cpsw_need_resplit(struct cpsw_common *cpsw) |
321 | { |
322 | int i, rlim_ch_num; |
323 | int speed, ch_rate; |
324 | |
325 | /* re-split resources only in case speed was changed */ |
326 | speed = cpsw_get_common_speed(cpsw); |
327 | if (speed == cpsw->speed || !speed) |
328 | return 0; |
329 | |
330 | cpsw->speed = speed; |
331 | |
332 | for (i = 0, rlim_ch_num = 0; i < cpsw->tx_ch_num; i++) { |
333 | ch_rate = cpdma_chan_get_rate(ch: cpsw->txv[i].ch); |
334 | if (!ch_rate) |
335 | break; |
336 | |
337 | rlim_ch_num++; |
338 | } |
339 | |
340 | /* cases not dependent on speed */ |
341 | if (!rlim_ch_num || rlim_ch_num == cpsw->tx_ch_num) |
342 | return 0; |
343 | |
344 | return 1; |
345 | } |
346 | |
347 | void cpsw_split_res(struct cpsw_common *cpsw) |
348 | { |
349 | u32 consumed_rate = 0, bigest_rate = 0; |
350 | struct cpsw_vector *txv = cpsw->txv; |
351 | int i, ch_weight, rlim_ch_num = 0; |
352 | int budget, bigest_rate_ch = 0; |
353 | u32 ch_rate, max_rate; |
354 | int ch_budget = 0; |
355 | |
356 | for (i = 0; i < cpsw->tx_ch_num; i++) { |
357 | ch_rate = cpdma_chan_get_rate(ch: txv[i].ch); |
358 | if (!ch_rate) |
359 | continue; |
360 | |
361 | rlim_ch_num++; |
362 | consumed_rate += ch_rate; |
363 | } |
364 | |
365 | if (cpsw->tx_ch_num == rlim_ch_num) { |
366 | max_rate = consumed_rate; |
367 | } else if (!rlim_ch_num) { |
368 | ch_budget = NAPI_POLL_WEIGHT / cpsw->tx_ch_num; |
369 | bigest_rate = 0; |
370 | max_rate = consumed_rate; |
371 | } else { |
372 | max_rate = cpsw->speed * 1000; |
373 | |
374 | /* if max_rate is less then expected due to reduced link speed, |
375 | * split proportionally according next potential max speed |
376 | */ |
377 | if (max_rate < consumed_rate) |
378 | max_rate *= 10; |
379 | |
380 | if (max_rate < consumed_rate) |
381 | max_rate *= 10; |
382 | |
383 | ch_budget = (consumed_rate * NAPI_POLL_WEIGHT) / max_rate; |
384 | ch_budget = (NAPI_POLL_WEIGHT - ch_budget) / |
385 | (cpsw->tx_ch_num - rlim_ch_num); |
386 | bigest_rate = (max_rate - consumed_rate) / |
387 | (cpsw->tx_ch_num - rlim_ch_num); |
388 | } |
389 | |
390 | /* split tx weight/budget */ |
391 | budget = NAPI_POLL_WEIGHT; |
392 | for (i = 0; i < cpsw->tx_ch_num; i++) { |
393 | ch_rate = cpdma_chan_get_rate(ch: txv[i].ch); |
394 | if (ch_rate) { |
395 | txv[i].budget = (ch_rate * NAPI_POLL_WEIGHT) / max_rate; |
396 | if (!txv[i].budget) |
397 | txv[i].budget++; |
398 | if (ch_rate > bigest_rate) { |
399 | bigest_rate_ch = i; |
400 | bigest_rate = ch_rate; |
401 | } |
402 | |
403 | ch_weight = (ch_rate * 100) / max_rate; |
404 | if (!ch_weight) |
405 | ch_weight++; |
406 | cpdma_chan_set_weight(ch: cpsw->txv[i].ch, weight: ch_weight); |
407 | } else { |
408 | txv[i].budget = ch_budget; |
409 | if (!bigest_rate_ch) |
410 | bigest_rate_ch = i; |
411 | cpdma_chan_set_weight(ch: cpsw->txv[i].ch, weight: 0); |
412 | } |
413 | |
414 | budget -= txv[i].budget; |
415 | } |
416 | |
417 | if (budget) |
418 | txv[bigest_rate_ch].budget += budget; |
419 | |
420 | /* split rx budget */ |
421 | budget = NAPI_POLL_WEIGHT; |
422 | ch_budget = budget / cpsw->rx_ch_num; |
423 | for (i = 0; i < cpsw->rx_ch_num; i++) { |
424 | cpsw->rxv[i].budget = ch_budget; |
425 | budget -= ch_budget; |
426 | } |
427 | |
428 | if (budget) |
429 | cpsw->rxv[0].budget += budget; |
430 | } |
431 | |
432 | int cpsw_init_common(struct cpsw_common *cpsw, void __iomem *ss_regs, |
433 | int ale_ageout, phys_addr_t desc_mem_phys, |
434 | int descs_pool_size) |
435 | { |
436 | u32 slave_offset, sliver_offset, slave_size; |
437 | struct cpsw_ale_params ale_params; |
438 | struct cpsw_platform_data *data; |
439 | struct cpdma_params dma_params; |
440 | struct device *dev = cpsw->dev; |
441 | struct device_node *cpts_node; |
442 | void __iomem *cpts_regs; |
443 | int ret = 0, i; |
444 | |
445 | data = &cpsw->data; |
446 | cpsw->rx_ch_num = 1; |
447 | cpsw->tx_ch_num = 1; |
448 | |
449 | cpsw->version = readl(addr: &cpsw->regs->id_ver); |
450 | |
451 | memset(&dma_params, 0, sizeof(dma_params)); |
452 | memset(&ale_params, 0, sizeof(ale_params)); |
453 | |
454 | switch (cpsw->version) { |
455 | case CPSW_VERSION_1: |
456 | cpsw->host_port_regs = ss_regs + CPSW1_HOST_PORT_OFFSET; |
457 | cpts_regs = ss_regs + CPSW1_CPTS_OFFSET; |
458 | cpsw->hw_stats = ss_regs + CPSW1_HW_STATS; |
459 | dma_params.dmaregs = ss_regs + CPSW1_CPDMA_OFFSET; |
460 | dma_params.txhdp = ss_regs + CPSW1_STATERAM_OFFSET; |
461 | ale_params.ale_regs = ss_regs + CPSW1_ALE_OFFSET; |
462 | slave_offset = CPSW1_SLAVE_OFFSET; |
463 | slave_size = CPSW1_SLAVE_SIZE; |
464 | sliver_offset = CPSW1_SLIVER_OFFSET; |
465 | dma_params.desc_mem_phys = 0; |
466 | break; |
467 | case CPSW_VERSION_2: |
468 | case CPSW_VERSION_3: |
469 | case CPSW_VERSION_4: |
470 | cpsw->host_port_regs = ss_regs + CPSW2_HOST_PORT_OFFSET; |
471 | cpts_regs = ss_regs + CPSW2_CPTS_OFFSET; |
472 | cpsw->hw_stats = ss_regs + CPSW2_HW_STATS; |
473 | dma_params.dmaregs = ss_regs + CPSW2_CPDMA_OFFSET; |
474 | dma_params.txhdp = ss_regs + CPSW2_STATERAM_OFFSET; |
475 | ale_params.ale_regs = ss_regs + CPSW2_ALE_OFFSET; |
476 | slave_offset = CPSW2_SLAVE_OFFSET; |
477 | slave_size = CPSW2_SLAVE_SIZE; |
478 | sliver_offset = CPSW2_SLIVER_OFFSET; |
479 | dma_params.desc_mem_phys = desc_mem_phys; |
480 | break; |
481 | default: |
482 | dev_err(dev, "unknown version 0x%08x\n" , cpsw->version); |
483 | return -ENODEV; |
484 | } |
485 | |
486 | for (i = 0; i < cpsw->data.slaves; i++) { |
487 | struct cpsw_slave *slave = &cpsw->slaves[i]; |
488 | void __iomem *regs = cpsw->regs; |
489 | |
490 | slave->slave_num = i; |
491 | slave->data = &cpsw->data.slave_data[i]; |
492 | slave->regs = regs + slave_offset; |
493 | slave->port_vlan = slave->data->dual_emac_res_vlan; |
494 | slave->mac_sl = cpsw_sl_get(device_id: "cpsw" , dev, sl_base: regs + sliver_offset); |
495 | if (IS_ERR(ptr: slave->mac_sl)) |
496 | return PTR_ERR(ptr: slave->mac_sl); |
497 | |
498 | slave_offset += slave_size; |
499 | sliver_offset += SLIVER_SIZE; |
500 | } |
501 | |
502 | ale_params.dev = dev; |
503 | ale_params.ale_ageout = ale_ageout; |
504 | ale_params.ale_ports = CPSW_ALE_PORTS_NUM; |
505 | ale_params.dev_id = "cpsw" ; |
506 | ale_params.bus_freq = cpsw->bus_freq_mhz * 1000000; |
507 | |
508 | cpsw->ale = cpsw_ale_create(params: &ale_params); |
509 | if (IS_ERR(ptr: cpsw->ale)) { |
510 | dev_err(dev, "error initializing ale engine\n" ); |
511 | return PTR_ERR(ptr: cpsw->ale); |
512 | } |
513 | |
514 | dma_params.dev = dev; |
515 | dma_params.rxthresh = dma_params.dmaregs + CPDMA_RXTHRESH; |
516 | dma_params.rxfree = dma_params.dmaregs + CPDMA_RXFREE; |
517 | dma_params.rxhdp = dma_params.txhdp + CPDMA_RXHDP; |
518 | dma_params.txcp = dma_params.txhdp + CPDMA_TXCP; |
519 | dma_params.rxcp = dma_params.txhdp + CPDMA_RXCP; |
520 | |
521 | dma_params.num_chan = data->channels; |
522 | dma_params.has_soft_reset = true; |
523 | dma_params.min_packet_size = CPSW_MIN_PACKET_SIZE; |
524 | dma_params.desc_mem_size = data->bd_ram_size; |
525 | dma_params.desc_align = 16; |
526 | dma_params.has_ext_regs = true; |
527 | dma_params.desc_hw_addr = dma_params.desc_mem_phys; |
528 | dma_params.bus_freq_mhz = cpsw->bus_freq_mhz; |
529 | dma_params.descs_pool_size = descs_pool_size; |
530 | |
531 | cpsw->dma = cpdma_ctlr_create(params: &dma_params); |
532 | if (!cpsw->dma) { |
533 | dev_err(dev, "error initializing dma\n" ); |
534 | return -ENOMEM; |
535 | } |
536 | |
537 | cpts_node = of_get_child_by_name(node: cpsw->dev->of_node, name: "cpts" ); |
538 | if (!cpts_node) |
539 | cpts_node = cpsw->dev->of_node; |
540 | |
541 | cpsw->cpts = cpts_create(dev: cpsw->dev, regs: cpts_regs, node: cpts_node, |
542 | CPTS_N_ETX_TS); |
543 | if (IS_ERR(ptr: cpsw->cpts)) { |
544 | ret = PTR_ERR(ptr: cpsw->cpts); |
545 | cpdma_ctlr_destroy(ctlr: cpsw->dma); |
546 | } |
547 | of_node_put(node: cpts_node); |
548 | |
549 | return ret; |
550 | } |
551 | |
552 | #if IS_ENABLED(CONFIG_TI_CPTS) |
553 | |
554 | static void cpsw_hwtstamp_v1(struct cpsw_priv *priv) |
555 | { |
556 | struct cpsw_common *cpsw = priv->cpsw; |
557 | struct cpsw_slave *slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)]; |
558 | u32 ts_en, seq_id; |
559 | |
560 | if (!priv->tx_ts_enabled && !priv->rx_ts_enabled) { |
561 | slave_write(slave, val: 0, CPSW1_TS_CTL); |
562 | return; |
563 | } |
564 | |
565 | seq_id = (30 << CPSW_V1_SEQ_ID_OFS_SHIFT) | ETH_P_1588; |
566 | ts_en = EVENT_MSG_BITS << CPSW_V1_MSG_TYPE_OFS; |
567 | |
568 | if (priv->tx_ts_enabled) |
569 | ts_en |= CPSW_V1_TS_TX_EN; |
570 | |
571 | if (priv->rx_ts_enabled) |
572 | ts_en |= CPSW_V1_TS_RX_EN; |
573 | |
574 | slave_write(slave, val: ts_en, CPSW1_TS_CTL); |
575 | slave_write(slave, val: seq_id, CPSW1_TS_SEQ_LTYPE); |
576 | } |
577 | |
578 | static void cpsw_hwtstamp_v2(struct cpsw_priv *priv) |
579 | { |
580 | struct cpsw_common *cpsw = priv->cpsw; |
581 | struct cpsw_slave *slave; |
582 | u32 ctrl, mtype; |
583 | |
584 | slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)]; |
585 | |
586 | ctrl = slave_read(slave, CPSW2_CONTROL); |
587 | switch (cpsw->version) { |
588 | case CPSW_VERSION_2: |
589 | ctrl &= ~CTRL_V2_ALL_TS_MASK; |
590 | |
591 | if (priv->tx_ts_enabled) |
592 | ctrl |= CTRL_V2_TX_TS_BITS; |
593 | |
594 | if (priv->rx_ts_enabled) |
595 | ctrl |= CTRL_V2_RX_TS_BITS; |
596 | break; |
597 | case CPSW_VERSION_3: |
598 | default: |
599 | ctrl &= ~CTRL_V3_ALL_TS_MASK; |
600 | |
601 | if (priv->tx_ts_enabled) |
602 | ctrl |= CTRL_V3_TX_TS_BITS; |
603 | |
604 | if (priv->rx_ts_enabled) |
605 | ctrl |= CTRL_V3_RX_TS_BITS; |
606 | break; |
607 | } |
608 | |
609 | mtype = (30 << TS_SEQ_ID_OFFSET_SHIFT) | EVENT_MSG_BITS; |
610 | |
611 | slave_write(slave, val: mtype, CPSW2_TS_SEQ_MTYPE); |
612 | slave_write(slave, val: ctrl, CPSW2_CONTROL); |
613 | writel_relaxed(ETH_P_1588, &cpsw->regs->ts_ltype); |
614 | writel_relaxed(ETH_P_8021Q, &cpsw->regs->vlan_ltype); |
615 | } |
616 | |
617 | static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr) |
618 | { |
619 | struct cpsw_priv *priv = netdev_priv(dev); |
620 | struct cpsw_common *cpsw = priv->cpsw; |
621 | struct hwtstamp_config cfg; |
622 | |
623 | if (cpsw->version != CPSW_VERSION_1 && |
624 | cpsw->version != CPSW_VERSION_2 && |
625 | cpsw->version != CPSW_VERSION_3) |
626 | return -EOPNOTSUPP; |
627 | |
628 | if (copy_from_user(to: &cfg, from: ifr->ifr_data, n: sizeof(cfg))) |
629 | return -EFAULT; |
630 | |
631 | if (cfg.tx_type != HWTSTAMP_TX_OFF && cfg.tx_type != HWTSTAMP_TX_ON) |
632 | return -ERANGE; |
633 | |
634 | switch (cfg.rx_filter) { |
635 | case HWTSTAMP_FILTER_NONE: |
636 | priv->rx_ts_enabled = 0; |
637 | break; |
638 | case HWTSTAMP_FILTER_ALL: |
639 | case HWTSTAMP_FILTER_NTP_ALL: |
640 | case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: |
641 | case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: |
642 | case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: |
643 | return -ERANGE; |
644 | case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: |
645 | case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: |
646 | case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: |
647 | case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: |
648 | case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: |
649 | case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: |
650 | case HWTSTAMP_FILTER_PTP_V2_EVENT: |
651 | case HWTSTAMP_FILTER_PTP_V2_SYNC: |
652 | case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: |
653 | priv->rx_ts_enabled = HWTSTAMP_FILTER_PTP_V2_EVENT; |
654 | cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; |
655 | break; |
656 | default: |
657 | return -ERANGE; |
658 | } |
659 | |
660 | priv->tx_ts_enabled = cfg.tx_type == HWTSTAMP_TX_ON; |
661 | |
662 | switch (cpsw->version) { |
663 | case CPSW_VERSION_1: |
664 | cpsw_hwtstamp_v1(priv); |
665 | break; |
666 | case CPSW_VERSION_2: |
667 | case CPSW_VERSION_3: |
668 | cpsw_hwtstamp_v2(priv); |
669 | break; |
670 | default: |
671 | WARN_ON(1); |
672 | } |
673 | |
674 | return copy_to_user(to: ifr->ifr_data, from: &cfg, n: sizeof(cfg)) ? -EFAULT : 0; |
675 | } |
676 | |
677 | static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr) |
678 | { |
679 | struct cpsw_common *cpsw = ndev_to_cpsw(dev); |
680 | struct cpsw_priv *priv = netdev_priv(dev); |
681 | struct hwtstamp_config cfg; |
682 | |
683 | if (cpsw->version != CPSW_VERSION_1 && |
684 | cpsw->version != CPSW_VERSION_2 && |
685 | cpsw->version != CPSW_VERSION_3) |
686 | return -EOPNOTSUPP; |
687 | |
688 | cfg.flags = 0; |
689 | cfg.tx_type = priv->tx_ts_enabled ? HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF; |
690 | cfg.rx_filter = priv->rx_ts_enabled; |
691 | |
692 | return copy_to_user(to: ifr->ifr_data, from: &cfg, n: sizeof(cfg)) ? -EFAULT : 0; |
693 | } |
694 | #else |
695 | static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr) |
696 | { |
697 | return -EOPNOTSUPP; |
698 | } |
699 | |
700 | static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr) |
701 | { |
702 | return -EOPNOTSUPP; |
703 | } |
704 | #endif /*CONFIG_TI_CPTS*/ |
705 | |
706 | int cpsw_ndo_ioctl(struct net_device *dev, struct ifreq *req, int cmd) |
707 | { |
708 | struct cpsw_priv *priv = netdev_priv(dev); |
709 | struct cpsw_common *cpsw = priv->cpsw; |
710 | int slave_no = cpsw_slave_index(cpsw, priv); |
711 | struct phy_device *phy; |
712 | |
713 | if (!netif_running(dev)) |
714 | return -EINVAL; |
715 | |
716 | phy = cpsw->slaves[slave_no].phy; |
717 | |
718 | if (!phy_has_hwtstamp(phydev: phy)) { |
719 | switch (cmd) { |
720 | case SIOCSHWTSTAMP: |
721 | return cpsw_hwtstamp_set(dev, ifr: req); |
722 | case SIOCGHWTSTAMP: |
723 | return cpsw_hwtstamp_get(dev, ifr: req); |
724 | } |
725 | } |
726 | |
727 | if (phy) |
728 | return phy_mii_ioctl(phydev: phy, ifr: req, cmd); |
729 | |
730 | return -EOPNOTSUPP; |
731 | } |
732 | |
733 | int cpsw_ndo_set_tx_maxrate(struct net_device *ndev, int queue, u32 rate) |
734 | { |
735 | struct cpsw_priv *priv = netdev_priv(dev: ndev); |
736 | struct cpsw_common *cpsw = priv->cpsw; |
737 | struct cpsw_slave *slave; |
738 | u32 min_rate; |
739 | u32 ch_rate; |
740 | int i, ret; |
741 | |
742 | ch_rate = netdev_get_tx_queue(dev: ndev, index: queue)->tx_maxrate; |
743 | if (ch_rate == rate) |
744 | return 0; |
745 | |
746 | ch_rate = rate * 1000; |
747 | min_rate = cpdma_chan_get_min_rate(ctlr: cpsw->dma); |
748 | if ((ch_rate < min_rate && ch_rate)) { |
749 | dev_err(priv->dev, "The channel rate cannot be less than %dMbps" , |
750 | min_rate); |
751 | return -EINVAL; |
752 | } |
753 | |
754 | if (rate > cpsw->speed) { |
755 | dev_err(priv->dev, "The channel rate cannot be more than 2Gbps" ); |
756 | return -EINVAL; |
757 | } |
758 | |
759 | ret = pm_runtime_resume_and_get(dev: cpsw->dev); |
760 | if (ret < 0) |
761 | return ret; |
762 | |
763 | ret = cpdma_chan_set_rate(ch: cpsw->txv[queue].ch, rate: ch_rate); |
764 | pm_runtime_put(dev: cpsw->dev); |
765 | |
766 | if (ret) |
767 | return ret; |
768 | |
769 | /* update rates for slaves tx queues */ |
770 | for (i = 0; i < cpsw->data.slaves; i++) { |
771 | slave = &cpsw->slaves[i]; |
772 | if (!slave->ndev) |
773 | continue; |
774 | |
775 | netdev_get_tx_queue(dev: slave->ndev, index: queue)->tx_maxrate = rate; |
776 | } |
777 | |
778 | cpsw_split_res(cpsw); |
779 | return ret; |
780 | } |
781 | |
782 | static int cpsw_tc_to_fifo(int tc, int num_tc) |
783 | { |
784 | if (tc == num_tc - 1) |
785 | return 0; |
786 | |
787 | return CPSW_FIFO_SHAPERS_NUM - tc; |
788 | } |
789 | |
790 | bool cpsw_shp_is_off(struct cpsw_priv *priv) |
791 | { |
792 | struct cpsw_common *cpsw = priv->cpsw; |
793 | struct cpsw_slave *slave; |
794 | u32 shift, mask, val; |
795 | |
796 | val = readl_relaxed(&cpsw->regs->ptype); |
797 | |
798 | slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)]; |
799 | shift = CPSW_FIFO_SHAPE_EN_SHIFT + 3 * slave->slave_num; |
800 | mask = 7 << shift; |
801 | val = val & mask; |
802 | |
803 | return !val; |
804 | } |
805 | |
806 | static void cpsw_fifo_shp_on(struct cpsw_priv *priv, int fifo, int on) |
807 | { |
808 | struct cpsw_common *cpsw = priv->cpsw; |
809 | struct cpsw_slave *slave; |
810 | u32 shift, mask, val; |
811 | |
812 | val = readl_relaxed(&cpsw->regs->ptype); |
813 | |
814 | slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)]; |
815 | shift = CPSW_FIFO_SHAPE_EN_SHIFT + 3 * slave->slave_num; |
816 | mask = (1 << --fifo) << shift; |
817 | val = on ? val | mask : val & ~mask; |
818 | |
819 | writel_relaxed(val, &cpsw->regs->ptype); |
820 | } |
821 | |
822 | static int cpsw_set_fifo_bw(struct cpsw_priv *priv, int fifo, int bw) |
823 | { |
824 | struct cpsw_common *cpsw = priv->cpsw; |
825 | u32 val = 0, send_pct, shift; |
826 | struct cpsw_slave *slave; |
827 | int pct = 0, i; |
828 | |
829 | if (bw > priv->shp_cfg_speed * 1000) |
830 | goto err; |
831 | |
832 | /* shaping has to stay enabled for highest fifos linearly |
833 | * and fifo bw no more then interface can allow |
834 | */ |
835 | slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)]; |
836 | send_pct = slave_read(slave, SEND_PERCENT); |
837 | for (i = CPSW_FIFO_SHAPERS_NUM; i > 0; i--) { |
838 | if (!bw) { |
839 | if (i >= fifo || !priv->fifo_bw[i]) |
840 | continue; |
841 | |
842 | dev_warn(priv->dev, "Prev FIFO%d is shaped" , i); |
843 | continue; |
844 | } |
845 | |
846 | if (!priv->fifo_bw[i] && i > fifo) { |
847 | dev_err(priv->dev, "Upper FIFO%d is not shaped" , i); |
848 | return -EINVAL; |
849 | } |
850 | |
851 | shift = (i - 1) * 8; |
852 | if (i == fifo) { |
853 | send_pct &= ~(CPSW_PCT_MASK << shift); |
854 | val = DIV_ROUND_UP(bw, priv->shp_cfg_speed * 10); |
855 | if (!val) |
856 | val = 1; |
857 | |
858 | send_pct |= val << shift; |
859 | pct += val; |
860 | continue; |
861 | } |
862 | |
863 | if (priv->fifo_bw[i]) |
864 | pct += (send_pct >> shift) & CPSW_PCT_MASK; |
865 | } |
866 | |
867 | if (pct >= 100) |
868 | goto err; |
869 | |
870 | slave_write(slave, val: send_pct, SEND_PERCENT); |
871 | priv->fifo_bw[fifo] = bw; |
872 | |
873 | dev_warn(priv->dev, "set FIFO%d bw = %d\n" , fifo, |
874 | DIV_ROUND_CLOSEST(val * priv->shp_cfg_speed, 100)); |
875 | |
876 | return 0; |
877 | err: |
878 | dev_err(priv->dev, "Bandwidth doesn't fit in tc configuration" ); |
879 | return -EINVAL; |
880 | } |
881 | |
882 | static int cpsw_set_fifo_rlimit(struct cpsw_priv *priv, int fifo, int bw) |
883 | { |
884 | struct cpsw_common *cpsw = priv->cpsw; |
885 | struct cpsw_slave *slave; |
886 | u32 tx_in_ctl_rg, val; |
887 | int ret; |
888 | |
889 | ret = cpsw_set_fifo_bw(priv, fifo, bw); |
890 | if (ret) |
891 | return ret; |
892 | |
893 | slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)]; |
894 | tx_in_ctl_rg = cpsw->version == CPSW_VERSION_1 ? |
895 | CPSW1_TX_IN_CTL : CPSW2_TX_IN_CTL; |
896 | |
897 | if (!bw) |
898 | cpsw_fifo_shp_on(priv, fifo, on: bw); |
899 | |
900 | val = slave_read(slave, offset: tx_in_ctl_rg); |
901 | if (cpsw_shp_is_off(priv)) { |
902 | /* disable FIFOs rate limited queues */ |
903 | val &= ~(0xf << CPSW_FIFO_RATE_EN_SHIFT); |
904 | |
905 | /* set type of FIFO queues to normal priority mode */ |
906 | val &= ~(3 << CPSW_FIFO_QUEUE_TYPE_SHIFT); |
907 | |
908 | /* set type of FIFO queues to be rate limited */ |
909 | if (bw) |
910 | val |= 2 << CPSW_FIFO_QUEUE_TYPE_SHIFT; |
911 | else |
912 | priv->shp_cfg_speed = 0; |
913 | } |
914 | |
915 | /* toggle a FIFO rate limited queue */ |
916 | if (bw) |
917 | val |= BIT(fifo + CPSW_FIFO_RATE_EN_SHIFT); |
918 | else |
919 | val &= ~BIT(fifo + CPSW_FIFO_RATE_EN_SHIFT); |
920 | slave_write(slave, val, offset: tx_in_ctl_rg); |
921 | |
922 | /* FIFO transmit shape enable */ |
923 | cpsw_fifo_shp_on(priv, fifo, on: bw); |
924 | return 0; |
925 | } |
926 | |
927 | /* Defaults: |
928 | * class A - prio 3 |
929 | * class B - prio 2 |
930 | * shaping for class A should be set first |
931 | */ |
932 | static int cpsw_set_cbs(struct net_device *ndev, |
933 | struct tc_cbs_qopt_offload *qopt) |
934 | { |
935 | struct cpsw_priv *priv = netdev_priv(dev: ndev); |
936 | struct cpsw_common *cpsw = priv->cpsw; |
937 | struct cpsw_slave *slave; |
938 | int prev_speed = 0; |
939 | int tc, ret, fifo; |
940 | u32 bw = 0; |
941 | |
942 | tc = netdev_txq_to_tc(dev: priv->ndev, txq: qopt->queue); |
943 | |
944 | /* enable channels in backward order, as highest FIFOs must be rate |
945 | * limited first and for compliance with CPDMA rate limited channels |
946 | * that also used in bacward order. FIFO0 cannot be rate limited. |
947 | */ |
948 | fifo = cpsw_tc_to_fifo(tc, num_tc: ndev->num_tc); |
949 | if (!fifo) { |
950 | dev_err(priv->dev, "Last tc%d can't be rate limited" , tc); |
951 | return -EINVAL; |
952 | } |
953 | |
954 | /* do nothing, it's disabled anyway */ |
955 | if (!qopt->enable && !priv->fifo_bw[fifo]) |
956 | return 0; |
957 | |
958 | /* shapers can be set if link speed is known */ |
959 | slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)]; |
960 | if (slave->phy && slave->phy->link) { |
961 | if (priv->shp_cfg_speed && |
962 | priv->shp_cfg_speed != slave->phy->speed) |
963 | prev_speed = priv->shp_cfg_speed; |
964 | |
965 | priv->shp_cfg_speed = slave->phy->speed; |
966 | } |
967 | |
968 | if (!priv->shp_cfg_speed) { |
969 | dev_err(priv->dev, "Link speed is not known" ); |
970 | return -1; |
971 | } |
972 | |
973 | ret = pm_runtime_resume_and_get(dev: cpsw->dev); |
974 | if (ret < 0) |
975 | return ret; |
976 | |
977 | bw = qopt->enable ? qopt->idleslope : 0; |
978 | ret = cpsw_set_fifo_rlimit(priv, fifo, bw); |
979 | if (ret) { |
980 | priv->shp_cfg_speed = prev_speed; |
981 | prev_speed = 0; |
982 | } |
983 | |
984 | if (bw && prev_speed) |
985 | dev_warn(priv->dev, |
986 | "Speed was changed, CBS shaper speeds are changed!" ); |
987 | |
988 | pm_runtime_put_sync(dev: cpsw->dev); |
989 | return ret; |
990 | } |
991 | |
992 | static int cpsw_set_mqprio(struct net_device *ndev, void *type_data) |
993 | { |
994 | struct tc_mqprio_qopt_offload *mqprio = type_data; |
995 | struct cpsw_priv *priv = netdev_priv(dev: ndev); |
996 | struct cpsw_common *cpsw = priv->cpsw; |
997 | int fifo, num_tc, count, offset; |
998 | struct cpsw_slave *slave; |
999 | u32 tx_prio_map = 0; |
1000 | int i, tc, ret; |
1001 | |
1002 | num_tc = mqprio->qopt.num_tc; |
1003 | if (num_tc > CPSW_TC_NUM) |
1004 | return -EINVAL; |
1005 | |
1006 | if (mqprio->mode != TC_MQPRIO_MODE_DCB) |
1007 | return -EINVAL; |
1008 | |
1009 | ret = pm_runtime_resume_and_get(dev: cpsw->dev); |
1010 | if (ret < 0) |
1011 | return ret; |
1012 | |
1013 | if (num_tc) { |
1014 | for (i = 0; i < 8; i++) { |
1015 | tc = mqprio->qopt.prio_tc_map[i]; |
1016 | fifo = cpsw_tc_to_fifo(tc, num_tc); |
1017 | tx_prio_map |= fifo << (4 * i); |
1018 | } |
1019 | |
1020 | netdev_set_num_tc(dev: ndev, num_tc); |
1021 | for (i = 0; i < num_tc; i++) { |
1022 | count = mqprio->qopt.count[i]; |
1023 | offset = mqprio->qopt.offset[i]; |
1024 | netdev_set_tc_queue(dev: ndev, tc: i, count, offset); |
1025 | } |
1026 | } |
1027 | |
1028 | if (!mqprio->qopt.hw) { |
1029 | /* restore default configuration */ |
1030 | netdev_reset_tc(dev: ndev); |
1031 | tx_prio_map = TX_PRIORITY_MAPPING; |
1032 | } |
1033 | |
1034 | priv->mqprio_hw = mqprio->qopt.hw; |
1035 | |
1036 | offset = cpsw->version == CPSW_VERSION_1 ? |
1037 | CPSW1_TX_PRI_MAP : CPSW2_TX_PRI_MAP; |
1038 | |
1039 | slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)]; |
1040 | slave_write(slave, val: tx_prio_map, offset); |
1041 | |
1042 | pm_runtime_put_sync(dev: cpsw->dev); |
1043 | |
1044 | return 0; |
1045 | } |
1046 | |
1047 | static int cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f); |
1048 | |
1049 | int cpsw_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type, |
1050 | void *type_data) |
1051 | { |
1052 | switch (type) { |
1053 | case TC_SETUP_QDISC_CBS: |
1054 | return cpsw_set_cbs(ndev, qopt: type_data); |
1055 | |
1056 | case TC_SETUP_QDISC_MQPRIO: |
1057 | return cpsw_set_mqprio(ndev, type_data); |
1058 | |
1059 | case TC_SETUP_BLOCK: |
1060 | return cpsw_qos_setup_tc_block(ndev, f: type_data); |
1061 | |
1062 | default: |
1063 | return -EOPNOTSUPP; |
1064 | } |
1065 | } |
1066 | |
1067 | void cpsw_cbs_resume(struct cpsw_slave *slave, struct cpsw_priv *priv) |
1068 | { |
1069 | int fifo, bw; |
1070 | |
1071 | for (fifo = CPSW_FIFO_SHAPERS_NUM; fifo > 0; fifo--) { |
1072 | bw = priv->fifo_bw[fifo]; |
1073 | if (!bw) |
1074 | continue; |
1075 | |
1076 | cpsw_set_fifo_rlimit(priv, fifo, bw); |
1077 | } |
1078 | } |
1079 | |
1080 | void cpsw_mqprio_resume(struct cpsw_slave *slave, struct cpsw_priv *priv) |
1081 | { |
1082 | struct cpsw_common *cpsw = priv->cpsw; |
1083 | u32 tx_prio_map = 0; |
1084 | int i, tc, fifo; |
1085 | u32 tx_prio_rg; |
1086 | |
1087 | if (!priv->mqprio_hw) |
1088 | return; |
1089 | |
1090 | for (i = 0; i < 8; i++) { |
1091 | tc = netdev_get_prio_tc_map(dev: priv->ndev, prio: i); |
1092 | fifo = CPSW_FIFO_SHAPERS_NUM - tc; |
1093 | tx_prio_map |= fifo << (4 * i); |
1094 | } |
1095 | |
1096 | tx_prio_rg = cpsw->version == CPSW_VERSION_1 ? |
1097 | CPSW1_TX_PRI_MAP : CPSW2_TX_PRI_MAP; |
1098 | |
1099 | slave_write(slave, val: tx_prio_map, offset: tx_prio_rg); |
1100 | } |
1101 | |
1102 | int cpsw_fill_rx_channels(struct cpsw_priv *priv) |
1103 | { |
1104 | struct cpsw_common *cpsw = priv->cpsw; |
1105 | struct cpsw_meta_xdp *xmeta; |
1106 | struct page_pool *pool; |
1107 | struct page *page; |
1108 | int ch_buf_num; |
1109 | int ch, i, ret; |
1110 | dma_addr_t dma; |
1111 | |
1112 | for (ch = 0; ch < cpsw->rx_ch_num; ch++) { |
1113 | pool = cpsw->page_pool[ch]; |
1114 | ch_buf_num = cpdma_chan_get_rx_buf_num(chan: cpsw->rxv[ch].ch); |
1115 | for (i = 0; i < ch_buf_num; i++) { |
1116 | page = page_pool_dev_alloc_pages(pool); |
1117 | if (!page) { |
1118 | cpsw_err(priv, ifup, "allocate rx page err\n" ); |
1119 | return -ENOMEM; |
1120 | } |
1121 | |
1122 | xmeta = page_address(page) + CPSW_XMETA_OFFSET; |
1123 | xmeta->ndev = priv->ndev; |
1124 | xmeta->ch = ch; |
1125 | |
1126 | dma = page_pool_get_dma_addr(page) + CPSW_HEADROOM_NA; |
1127 | ret = cpdma_chan_idle_submit_mapped(chan: cpsw->rxv[ch].ch, |
1128 | token: page, data: dma, |
1129 | len: cpsw->rx_packet_max, |
1130 | directed: 0); |
1131 | if (ret < 0) { |
1132 | cpsw_err(priv, ifup, |
1133 | "cannot submit page to channel %d rx, error %d\n" , |
1134 | ch, ret); |
1135 | page_pool_recycle_direct(pool, page); |
1136 | return ret; |
1137 | } |
1138 | } |
1139 | |
1140 | cpsw_info(priv, ifup, "ch %d rx, submitted %d descriptors\n" , |
1141 | ch, ch_buf_num); |
1142 | } |
1143 | |
1144 | return 0; |
1145 | } |
1146 | |
1147 | static struct page_pool *cpsw_create_page_pool(struct cpsw_common *cpsw, |
1148 | int size) |
1149 | { |
1150 | struct page_pool_params pp_params = {}; |
1151 | struct page_pool *pool; |
1152 | |
1153 | pp_params.order = 0; |
1154 | pp_params.flags = PP_FLAG_DMA_MAP; |
1155 | pp_params.pool_size = size; |
1156 | pp_params.nid = NUMA_NO_NODE; |
1157 | pp_params.dma_dir = DMA_BIDIRECTIONAL; |
1158 | pp_params.dev = cpsw->dev; |
1159 | |
1160 | pool = page_pool_create(params: &pp_params); |
1161 | if (IS_ERR(ptr: pool)) |
1162 | dev_err(cpsw->dev, "cannot create rx page pool\n" ); |
1163 | |
1164 | return pool; |
1165 | } |
1166 | |
1167 | static int cpsw_create_rx_pool(struct cpsw_common *cpsw, int ch) |
1168 | { |
1169 | struct page_pool *pool; |
1170 | int ret = 0, pool_size; |
1171 | |
1172 | pool_size = cpdma_chan_get_rx_buf_num(chan: cpsw->rxv[ch].ch); |
1173 | pool = cpsw_create_page_pool(cpsw, size: pool_size); |
1174 | if (IS_ERR(ptr: pool)) |
1175 | ret = PTR_ERR(ptr: pool); |
1176 | else |
1177 | cpsw->page_pool[ch] = pool; |
1178 | |
1179 | return ret; |
1180 | } |
1181 | |
1182 | static int cpsw_ndev_create_xdp_rxq(struct cpsw_priv *priv, int ch) |
1183 | { |
1184 | struct cpsw_common *cpsw = priv->cpsw; |
1185 | struct xdp_rxq_info *rxq; |
1186 | struct page_pool *pool; |
1187 | int ret; |
1188 | |
1189 | pool = cpsw->page_pool[ch]; |
1190 | rxq = &priv->xdp_rxq[ch]; |
1191 | |
1192 | ret = xdp_rxq_info_reg(xdp_rxq: rxq, dev: priv->ndev, queue_index: ch, napi_id: 0); |
1193 | if (ret) |
1194 | return ret; |
1195 | |
1196 | ret = xdp_rxq_info_reg_mem_model(xdp_rxq: rxq, type: MEM_TYPE_PAGE_POOL, allocator: pool); |
1197 | if (ret) |
1198 | xdp_rxq_info_unreg(xdp_rxq: rxq); |
1199 | |
1200 | return ret; |
1201 | } |
1202 | |
1203 | static void cpsw_ndev_destroy_xdp_rxq(struct cpsw_priv *priv, int ch) |
1204 | { |
1205 | struct xdp_rxq_info *rxq = &priv->xdp_rxq[ch]; |
1206 | |
1207 | if (!xdp_rxq_info_is_reg(xdp_rxq: rxq)) |
1208 | return; |
1209 | |
1210 | xdp_rxq_info_unreg(xdp_rxq: rxq); |
1211 | } |
1212 | |
1213 | void cpsw_destroy_xdp_rxqs(struct cpsw_common *cpsw) |
1214 | { |
1215 | struct net_device *ndev; |
1216 | int i, ch; |
1217 | |
1218 | for (ch = 0; ch < cpsw->rx_ch_num; ch++) { |
1219 | for (i = 0; i < cpsw->data.slaves; i++) { |
1220 | ndev = cpsw->slaves[i].ndev; |
1221 | if (!ndev) |
1222 | continue; |
1223 | |
1224 | cpsw_ndev_destroy_xdp_rxq(priv: netdev_priv(dev: ndev), ch); |
1225 | } |
1226 | |
1227 | page_pool_destroy(pool: cpsw->page_pool[ch]); |
1228 | cpsw->page_pool[ch] = NULL; |
1229 | } |
1230 | } |
1231 | |
1232 | int cpsw_create_xdp_rxqs(struct cpsw_common *cpsw) |
1233 | { |
1234 | struct net_device *ndev; |
1235 | int i, ch, ret; |
1236 | |
1237 | for (ch = 0; ch < cpsw->rx_ch_num; ch++) { |
1238 | ret = cpsw_create_rx_pool(cpsw, ch); |
1239 | if (ret) |
1240 | goto err_cleanup; |
1241 | |
1242 | /* using same page pool is allowed as no running rx handlers |
1243 | * simultaneously for both ndevs |
1244 | */ |
1245 | for (i = 0; i < cpsw->data.slaves; i++) { |
1246 | ndev = cpsw->slaves[i].ndev; |
1247 | if (!ndev) |
1248 | continue; |
1249 | |
1250 | ret = cpsw_ndev_create_xdp_rxq(priv: netdev_priv(dev: ndev), ch); |
1251 | if (ret) |
1252 | goto err_cleanup; |
1253 | } |
1254 | } |
1255 | |
1256 | return 0; |
1257 | |
1258 | err_cleanup: |
1259 | cpsw_destroy_xdp_rxqs(cpsw); |
1260 | |
1261 | return ret; |
1262 | } |
1263 | |
1264 | static int cpsw_xdp_prog_setup(struct cpsw_priv *priv, struct netdev_bpf *bpf) |
1265 | { |
1266 | struct bpf_prog *prog = bpf->prog; |
1267 | |
1268 | if (!priv->xdpi.prog && !prog) |
1269 | return 0; |
1270 | |
1271 | WRITE_ONCE(priv->xdp_prog, prog); |
1272 | |
1273 | xdp_attachment_setup(info: &priv->xdpi, bpf); |
1274 | |
1275 | return 0; |
1276 | } |
1277 | |
1278 | int cpsw_ndo_bpf(struct net_device *ndev, struct netdev_bpf *bpf) |
1279 | { |
1280 | struct cpsw_priv *priv = netdev_priv(dev: ndev); |
1281 | |
1282 | switch (bpf->command) { |
1283 | case XDP_SETUP_PROG: |
1284 | return cpsw_xdp_prog_setup(priv, bpf); |
1285 | |
1286 | default: |
1287 | return -EINVAL; |
1288 | } |
1289 | } |
1290 | |
1291 | int cpsw_xdp_tx_frame(struct cpsw_priv *priv, struct xdp_frame *xdpf, |
1292 | struct page *page, int port) |
1293 | { |
1294 | struct cpsw_common *cpsw = priv->cpsw; |
1295 | struct cpsw_meta_xdp *xmeta; |
1296 | struct cpdma_chan *txch; |
1297 | dma_addr_t dma; |
1298 | int ret; |
1299 | |
1300 | xmeta = (void *)xdpf + CPSW_XMETA_OFFSET; |
1301 | xmeta->ndev = priv->ndev; |
1302 | xmeta->ch = 0; |
1303 | txch = cpsw->txv[0].ch; |
1304 | |
1305 | if (page) { |
1306 | dma = page_pool_get_dma_addr(page); |
1307 | dma += xdpf->headroom + sizeof(struct xdp_frame); |
1308 | ret = cpdma_chan_submit_mapped(chan: txch, token: cpsw_xdpf_to_handle(xdpf), |
1309 | data: dma, len: xdpf->len, directed: port); |
1310 | } else { |
1311 | if (sizeof(*xmeta) > xdpf->headroom) |
1312 | return -EINVAL; |
1313 | |
1314 | ret = cpdma_chan_submit(chan: txch, token: cpsw_xdpf_to_handle(xdpf), |
1315 | data: xdpf->data, len: xdpf->len, directed: port); |
1316 | } |
1317 | |
1318 | if (ret) |
1319 | priv->ndev->stats.tx_dropped++; |
1320 | |
1321 | return ret; |
1322 | } |
1323 | |
1324 | int cpsw_run_xdp(struct cpsw_priv *priv, int ch, struct xdp_buff *xdp, |
1325 | struct page *page, int port, int *len) |
1326 | { |
1327 | struct cpsw_common *cpsw = priv->cpsw; |
1328 | struct net_device *ndev = priv->ndev; |
1329 | int ret = CPSW_XDP_CONSUMED; |
1330 | struct xdp_frame *xdpf; |
1331 | struct bpf_prog *prog; |
1332 | u32 act; |
1333 | |
1334 | prog = READ_ONCE(priv->xdp_prog); |
1335 | if (!prog) |
1336 | return CPSW_XDP_PASS; |
1337 | |
1338 | act = bpf_prog_run_xdp(prog, xdp); |
1339 | /* XDP prog might have changed packet data and boundaries */ |
1340 | *len = xdp->data_end - xdp->data; |
1341 | |
1342 | switch (act) { |
1343 | case XDP_PASS: |
1344 | ret = CPSW_XDP_PASS; |
1345 | goto out; |
1346 | case XDP_TX: |
1347 | xdpf = xdp_convert_buff_to_frame(xdp); |
1348 | if (unlikely(!xdpf)) |
1349 | goto drop; |
1350 | |
1351 | if (cpsw_xdp_tx_frame(priv, xdpf, page, port)) |
1352 | xdp_return_frame_rx_napi(xdpf); |
1353 | break; |
1354 | case XDP_REDIRECT: |
1355 | if (xdp_do_redirect(dev: ndev, xdp, prog)) |
1356 | goto drop; |
1357 | |
1358 | /* Have to flush here, per packet, instead of doing it in bulk |
1359 | * at the end of the napi handler. The RX devices on this |
1360 | * particular hardware is sharing a common queue, so the |
1361 | * incoming device might change per packet. |
1362 | */ |
1363 | xdp_do_flush(); |
1364 | break; |
1365 | default: |
1366 | bpf_warn_invalid_xdp_action(dev: ndev, prog, act); |
1367 | fallthrough; |
1368 | case XDP_ABORTED: |
1369 | trace_xdp_exception(dev: ndev, xdp: prog, act); |
1370 | fallthrough; /* handle aborts by dropping packet */ |
1371 | case XDP_DROP: |
1372 | ndev->stats.rx_bytes += *len; |
1373 | ndev->stats.rx_packets++; |
1374 | goto drop; |
1375 | } |
1376 | |
1377 | ndev->stats.rx_bytes += *len; |
1378 | ndev->stats.rx_packets++; |
1379 | out: |
1380 | return ret; |
1381 | drop: |
1382 | page_pool_recycle_direct(pool: cpsw->page_pool[ch], page); |
1383 | return ret; |
1384 | } |
1385 | |
1386 | static int cpsw_qos_clsflower_add_policer(struct cpsw_priv *priv, |
1387 | struct netlink_ext_ack *extack, |
1388 | struct flow_cls_offload *cls, |
1389 | u64 rate_pkt_ps) |
1390 | { |
1391 | struct flow_rule *rule = flow_cls_offload_flow_rule(flow_cmd: cls); |
1392 | struct flow_dissector *dissector = rule->match.dissector; |
1393 | static const u8 mc_mac[] = {0x01, 0x00, 0x00, 0x00, 0x00, 0x00}; |
1394 | struct flow_match_eth_addrs match; |
1395 | u32 port_id; |
1396 | int ret; |
1397 | |
1398 | if (dissector->used_keys & |
1399 | ~(BIT_ULL(FLOW_DISSECTOR_KEY_BASIC) | |
1400 | BIT_ULL(FLOW_DISSECTOR_KEY_CONTROL) | |
1401 | BIT_ULL(FLOW_DISSECTOR_KEY_ETH_ADDRS))) { |
1402 | NL_SET_ERR_MSG_MOD(extack, |
1403 | "Unsupported keys used" ); |
1404 | return -EOPNOTSUPP; |
1405 | } |
1406 | |
1407 | if (!flow_rule_match_key(rule, key: FLOW_DISSECTOR_KEY_ETH_ADDRS)) { |
1408 | NL_SET_ERR_MSG_MOD(extack, "Not matching on eth address" ); |
1409 | return -EOPNOTSUPP; |
1410 | } |
1411 | |
1412 | flow_rule_match_eth_addrs(rule, out: &match); |
1413 | |
1414 | if (!is_zero_ether_addr(addr: match.mask->src)) { |
1415 | NL_SET_ERR_MSG_MOD(extack, |
1416 | "Matching on source MAC not supported" ); |
1417 | return -EOPNOTSUPP; |
1418 | } |
1419 | |
1420 | port_id = cpsw_slave_index(priv->cpsw, priv) + 1; |
1421 | |
1422 | if (is_broadcast_ether_addr(addr: match.key->dst) && |
1423 | is_broadcast_ether_addr(addr: match.mask->dst)) { |
1424 | ret = cpsw_ale_rx_ratelimit_bc(ale: priv->cpsw->ale, port: port_id, ratelimit_pps: rate_pkt_ps); |
1425 | if (ret) |
1426 | return ret; |
1427 | |
1428 | priv->ale_bc_ratelimit.cookie = cls->cookie; |
1429 | priv->ale_bc_ratelimit.rate_packet_ps = rate_pkt_ps; |
1430 | } else if (ether_addr_equal_unaligned(addr1: match.key->dst, addr2: mc_mac) && |
1431 | ether_addr_equal_unaligned(addr1: match.mask->dst, addr2: mc_mac)) { |
1432 | ret = cpsw_ale_rx_ratelimit_mc(ale: priv->cpsw->ale, port: port_id, ratelimit_pps: rate_pkt_ps); |
1433 | if (ret) |
1434 | return ret; |
1435 | |
1436 | priv->ale_mc_ratelimit.cookie = cls->cookie; |
1437 | priv->ale_mc_ratelimit.rate_packet_ps = rate_pkt_ps; |
1438 | } else { |
1439 | NL_SET_ERR_MSG_MOD(extack, "Not supported matching key" ); |
1440 | return -EOPNOTSUPP; |
1441 | } |
1442 | |
1443 | return 0; |
1444 | } |
1445 | |
1446 | static int cpsw_qos_clsflower_policer_validate(const struct flow_action *action, |
1447 | const struct flow_action_entry *act, |
1448 | struct netlink_ext_ack *extack) |
1449 | { |
1450 | if (act->police.exceed.act_id != FLOW_ACTION_DROP) { |
1451 | NL_SET_ERR_MSG_MOD(extack, |
1452 | "Offload not supported when exceed action is not drop" ); |
1453 | return -EOPNOTSUPP; |
1454 | } |
1455 | |
1456 | if (act->police.notexceed.act_id != FLOW_ACTION_PIPE && |
1457 | act->police.notexceed.act_id != FLOW_ACTION_ACCEPT) { |
1458 | NL_SET_ERR_MSG_MOD(extack, |
1459 | "Offload not supported when conform action is not pipe or ok" ); |
1460 | return -EOPNOTSUPP; |
1461 | } |
1462 | |
1463 | if (act->police.notexceed.act_id == FLOW_ACTION_ACCEPT && |
1464 | !flow_action_is_last_entry(action, entry: act)) { |
1465 | NL_SET_ERR_MSG_MOD(extack, |
1466 | "Offload not supported when conform action is ok, but action is not last" ); |
1467 | return -EOPNOTSUPP; |
1468 | } |
1469 | |
1470 | if (act->police.rate_bytes_ps || act->police.peakrate_bytes_ps || |
1471 | act->police.avrate || act->police.overhead) { |
1472 | NL_SET_ERR_MSG_MOD(extack, |
1473 | "Offload not supported when bytes per second/peakrate/avrate/overhead is configured" ); |
1474 | return -EOPNOTSUPP; |
1475 | } |
1476 | |
1477 | return 0; |
1478 | } |
1479 | |
1480 | static int cpsw_qos_configure_clsflower(struct cpsw_priv *priv, struct flow_cls_offload *cls) |
1481 | { |
1482 | struct flow_rule *rule = flow_cls_offload_flow_rule(flow_cmd: cls); |
1483 | struct netlink_ext_ack *extack = cls->common.extack; |
1484 | const struct flow_action_entry *act; |
1485 | int i, ret; |
1486 | |
1487 | flow_action_for_each(i, act, &rule->action) { |
1488 | switch (act->id) { |
1489 | case FLOW_ACTION_POLICE: |
1490 | ret = cpsw_qos_clsflower_policer_validate(action: &rule->action, act, extack); |
1491 | if (ret) |
1492 | return ret; |
1493 | |
1494 | return cpsw_qos_clsflower_add_policer(priv, extack, cls, |
1495 | rate_pkt_ps: act->police.rate_pkt_ps); |
1496 | default: |
1497 | NL_SET_ERR_MSG_MOD(extack, "Action not supported" ); |
1498 | return -EOPNOTSUPP; |
1499 | } |
1500 | } |
1501 | return -EOPNOTSUPP; |
1502 | } |
1503 | |
1504 | static int cpsw_qos_delete_clsflower(struct cpsw_priv *priv, struct flow_cls_offload *cls) |
1505 | { |
1506 | u32 port_id = cpsw_slave_index(priv->cpsw, priv) + 1; |
1507 | |
1508 | if (cls->cookie == priv->ale_bc_ratelimit.cookie) { |
1509 | priv->ale_bc_ratelimit.cookie = 0; |
1510 | priv->ale_bc_ratelimit.rate_packet_ps = 0; |
1511 | cpsw_ale_rx_ratelimit_bc(ale: priv->cpsw->ale, port: port_id, ratelimit_pps: 0); |
1512 | } |
1513 | |
1514 | if (cls->cookie == priv->ale_mc_ratelimit.cookie) { |
1515 | priv->ale_mc_ratelimit.cookie = 0; |
1516 | priv->ale_mc_ratelimit.rate_packet_ps = 0; |
1517 | cpsw_ale_rx_ratelimit_mc(ale: priv->cpsw->ale, port: port_id, ratelimit_pps: 0); |
1518 | } |
1519 | |
1520 | return 0; |
1521 | } |
1522 | |
1523 | static int cpsw_qos_setup_tc_clsflower(struct cpsw_priv *priv, struct flow_cls_offload *cls_flower) |
1524 | { |
1525 | switch (cls_flower->command) { |
1526 | case FLOW_CLS_REPLACE: |
1527 | return cpsw_qos_configure_clsflower(priv, cls: cls_flower); |
1528 | case FLOW_CLS_DESTROY: |
1529 | return cpsw_qos_delete_clsflower(priv, cls: cls_flower); |
1530 | default: |
1531 | return -EOPNOTSUPP; |
1532 | } |
1533 | } |
1534 | |
1535 | static int cpsw_qos_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv) |
1536 | { |
1537 | struct cpsw_priv *priv = cb_priv; |
1538 | int ret; |
1539 | |
1540 | if (!tc_cls_can_offload_and_chain0(dev: priv->ndev, common: type_data)) |
1541 | return -EOPNOTSUPP; |
1542 | |
1543 | ret = pm_runtime_get_sync(dev: priv->dev); |
1544 | if (ret < 0) { |
1545 | pm_runtime_put_noidle(dev: priv->dev); |
1546 | return ret; |
1547 | } |
1548 | |
1549 | switch (type) { |
1550 | case TC_SETUP_CLSFLOWER: |
1551 | ret = cpsw_qos_setup_tc_clsflower(priv, cls_flower: type_data); |
1552 | break; |
1553 | default: |
1554 | ret = -EOPNOTSUPP; |
1555 | } |
1556 | |
1557 | pm_runtime_put(dev: priv->dev); |
1558 | return ret; |
1559 | } |
1560 | |
1561 | static LIST_HEAD(cpsw_qos_block_cb_list); |
1562 | |
1563 | static int cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f) |
1564 | { |
1565 | struct cpsw_priv *priv = netdev_priv(dev: ndev); |
1566 | |
1567 | return flow_block_cb_setup_simple(f, driver_list: &cpsw_qos_block_cb_list, |
1568 | cb: cpsw_qos_setup_tc_block_cb, |
1569 | cb_ident: priv, cb_priv: priv, ingress_only: true); |
1570 | } |
1571 | |
1572 | void cpsw_qos_clsflower_resume(struct cpsw_priv *priv) |
1573 | { |
1574 | u32 port_id = cpsw_slave_index(priv->cpsw, priv) + 1; |
1575 | |
1576 | if (priv->ale_bc_ratelimit.cookie) |
1577 | cpsw_ale_rx_ratelimit_bc(ale: priv->cpsw->ale, port: port_id, |
1578 | ratelimit_pps: priv->ale_bc_ratelimit.rate_packet_ps); |
1579 | |
1580 | if (priv->ale_mc_ratelimit.cookie) |
1581 | cpsw_ale_rx_ratelimit_mc(ale: priv->cpsw->ale, port: port_id, |
1582 | ratelimit_pps: priv->ale_mc_ratelimit.rate_packet_ps); |
1583 | } |
1584 | |