1 // SPDX-License-Identifier: (GPL-2.0 OR MIT) 2 /* 3 * Microsemi SoCs FDMA driver 4 * 5 * Copyright (c) 2021 Microchip 6 * 7 * Page recycling code is mostly taken from gianfar driver. 8 */ 9 10 #include <linux/align.h> 11 #include <linux/bitops.h> 12 #include <linux/dmapool.h> 13 #include <linux/dsa/ocelot.h> 14 #include <linux/netdevice.h> 15 #include <linux/of_platform.h> 16 #include <linux/skbuff.h> 17 18 #include "ocelot_fdma.h" 19 #include "ocelot_qs.h" 20 21 DEFINE_STATIC_KEY_FALSE(ocelot_fdma_enabled); 22 23 static void ocelot_fdma_writel(struct ocelot *ocelot, u32 reg, u32 data) 24 { 25 regmap_write(ocelot->targets[FDMA], reg, data); 26 } 27 28 static u32 ocelot_fdma_readl(struct ocelot *ocelot, u32 reg) 29 { 30 u32 retval; 31 32 regmap_read(ocelot->targets[FDMA], reg, &retval); 33 34 return retval; 35 } 36 37 static dma_addr_t ocelot_fdma_idx_dma(dma_addr_t base, u16 idx) 38 { 39 return base + idx * sizeof(struct ocelot_fdma_dcb); 40 } 41 42 static u16 ocelot_fdma_dma_idx(dma_addr_t base, dma_addr_t dma) 43 { 44 return (dma - base) / sizeof(struct ocelot_fdma_dcb); 45 } 46 47 static u16 ocelot_fdma_idx_next(u16 idx, u16 ring_sz) 48 { 49 return unlikely(idx == ring_sz - 1) ? 0 : idx + 1; 50 } 51 52 static u16 ocelot_fdma_idx_prev(u16 idx, u16 ring_sz) 53 { 54 return unlikely(idx == 0) ? ring_sz - 1 : idx - 1; 55 } 56 57 static int ocelot_fdma_rx_ring_free(struct ocelot_fdma *fdma) 58 { 59 struct ocelot_fdma_rx_ring *rx_ring = &fdma->rx_ring; 60 61 if (rx_ring->next_to_use >= rx_ring->next_to_clean) 62 return OCELOT_FDMA_RX_RING_SIZE - 63 (rx_ring->next_to_use - rx_ring->next_to_clean) - 1; 64 else 65 return rx_ring->next_to_clean - rx_ring->next_to_use - 1; 66 } 67 68 static int ocelot_fdma_tx_ring_free(struct ocelot_fdma *fdma) 69 { 70 struct ocelot_fdma_tx_ring *tx_ring = &fdma->tx_ring; 71 72 if (tx_ring->next_to_use >= tx_ring->next_to_clean) 73 return OCELOT_FDMA_TX_RING_SIZE - 74 (tx_ring->next_to_use - tx_ring->next_to_clean) - 1; 75 else 76 return tx_ring->next_to_clean - tx_ring->next_to_use - 1; 77 } 78 79 static bool ocelot_fdma_tx_ring_empty(struct ocelot_fdma *fdma) 80 { 81 struct ocelot_fdma_tx_ring *tx_ring = &fdma->tx_ring; 82 83 return tx_ring->next_to_clean == tx_ring->next_to_use; 84 } 85 86 static void ocelot_fdma_activate_chan(struct ocelot *ocelot, dma_addr_t dma, 87 int chan) 88 { 89 ocelot_fdma_writel(ocelot, MSCC_FDMA_DCB_LLP(chan), dma); 90 /* Barrier to force memory writes to DCB to be completed before starting 91 * the channel. 92 */ 93 wmb(); 94 ocelot_fdma_writel(ocelot, MSCC_FDMA_CH_ACTIVATE, BIT(chan)); 95 } 96 97 static u32 ocelot_fdma_read_ch_safe(struct ocelot *ocelot) 98 { 99 return ocelot_fdma_readl(ocelot, MSCC_FDMA_CH_SAFE); 100 } 101 102 static int ocelot_fdma_wait_chan_safe(struct ocelot *ocelot, int chan) 103 { 104 u32 safe; 105 106 return readx_poll_timeout_atomic(ocelot_fdma_read_ch_safe, ocelot, safe, 107 safe & BIT(chan), 0, 108 OCELOT_FDMA_CH_SAFE_TIMEOUT_US); 109 } 110 111 static void ocelot_fdma_dcb_set_data(struct ocelot_fdma_dcb *dcb, 112 dma_addr_t dma_addr, 113 size_t size) 114 { 115 u32 offset = dma_addr & 0x3; 116 117 dcb->llp = 0; 118 dcb->datap = ALIGN_DOWN(dma_addr, 4); 119 dcb->datal = ALIGN_DOWN(size, 4); 120 dcb->stat = MSCC_FDMA_DCB_STAT_BLOCKO(offset); 121 } 122 123 static bool ocelot_fdma_rx_alloc_page(struct ocelot *ocelot, 124 struct ocelot_fdma_rx_buf *rxb) 125 { 126 dma_addr_t mapping; 127 struct page *page; 128 129 page = dev_alloc_page(); 130 if (unlikely(!page)) 131 return false; 132 133 mapping = dma_map_page(ocelot->dev, page, 0, PAGE_SIZE, 134 DMA_FROM_DEVICE); 135 if (unlikely(dma_mapping_error(ocelot->dev, mapping))) { 136 __free_page(page); 137 return false; 138 } 139 140 rxb->page = page; 141 rxb->page_offset = 0; 142 rxb->dma_addr = mapping; 143 144 return true; 145 } 146 147 static int ocelot_fdma_alloc_rx_buffs(struct ocelot *ocelot, u16 alloc_cnt) 148 { 149 struct ocelot_fdma *fdma = ocelot->fdma; 150 struct ocelot_fdma_rx_ring *rx_ring; 151 struct ocelot_fdma_rx_buf *rxb; 152 struct ocelot_fdma_dcb *dcb; 153 dma_addr_t dma_addr; 154 int ret = 0; 155 u16 idx; 156 157 rx_ring = &fdma->rx_ring; 158 idx = rx_ring->next_to_use; 159 160 while (alloc_cnt--) { 161 rxb = &rx_ring->bufs[idx]; 162 /* try reuse page */ 163 if (unlikely(!rxb->page)) { 164 if (unlikely(!ocelot_fdma_rx_alloc_page(ocelot, rxb))) { 165 dev_err_ratelimited(ocelot->dev, 166 "Failed to allocate rx\n"); 167 ret = -ENOMEM; 168 break; 169 } 170 } 171 172 dcb = &rx_ring->dcbs[idx]; 173 dma_addr = rxb->dma_addr + rxb->page_offset; 174 ocelot_fdma_dcb_set_data(dcb, dma_addr, OCELOT_FDMA_RXB_SIZE); 175 176 idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_RX_RING_SIZE); 177 /* Chain the DCB to the next one */ 178 dcb->llp = ocelot_fdma_idx_dma(rx_ring->dcbs_dma, idx); 179 } 180 181 rx_ring->next_to_use = idx; 182 rx_ring->next_to_alloc = idx; 183 184 return ret; 185 } 186 187 static bool ocelot_fdma_tx_dcb_set_skb(struct ocelot *ocelot, 188 struct ocelot_fdma_tx_buf *tx_buf, 189 struct ocelot_fdma_dcb *dcb, 190 struct sk_buff *skb) 191 { 192 dma_addr_t mapping; 193 194 mapping = dma_map_single(ocelot->dev, skb->data, skb->len, 195 DMA_TO_DEVICE); 196 if (unlikely(dma_mapping_error(ocelot->dev, mapping))) 197 return false; 198 199 dma_unmap_addr_set(tx_buf, dma_addr, mapping); 200 201 ocelot_fdma_dcb_set_data(dcb, mapping, OCELOT_FDMA_RX_SIZE); 202 tx_buf->skb = skb; 203 dcb->stat |= MSCC_FDMA_DCB_STAT_BLOCKL(skb->len); 204 dcb->stat |= MSCC_FDMA_DCB_STAT_SOF | MSCC_FDMA_DCB_STAT_EOF; 205 206 return true; 207 } 208 209 static bool ocelot_fdma_check_stop_rx(struct ocelot *ocelot) 210 { 211 u32 llp; 212 213 /* Check if the FDMA hits the DCB with LLP == NULL */ 214 llp = ocelot_fdma_readl(ocelot, MSCC_FDMA_DCB_LLP(MSCC_FDMA_XTR_CHAN)); 215 if (unlikely(llp)) 216 return false; 217 218 ocelot_fdma_writel(ocelot, MSCC_FDMA_CH_DISABLE, 219 BIT(MSCC_FDMA_XTR_CHAN)); 220 221 return true; 222 } 223 224 static void ocelot_fdma_rx_set_llp(struct ocelot_fdma_rx_ring *rx_ring) 225 { 226 struct ocelot_fdma_dcb *dcb; 227 unsigned int idx; 228 229 idx = ocelot_fdma_idx_prev(rx_ring->next_to_use, 230 OCELOT_FDMA_RX_RING_SIZE); 231 dcb = &rx_ring->dcbs[idx]; 232 dcb->llp = 0; 233 } 234 235 static void ocelot_fdma_rx_restart(struct ocelot *ocelot) 236 { 237 struct ocelot_fdma *fdma = ocelot->fdma; 238 struct ocelot_fdma_rx_ring *rx_ring; 239 const u8 chan = MSCC_FDMA_XTR_CHAN; 240 dma_addr_t new_llp, dma_base; 241 unsigned int idx; 242 u32 llp_prev; 243 int ret; 244 245 rx_ring = &fdma->rx_ring; 246 ret = ocelot_fdma_wait_chan_safe(ocelot, chan); 247 if (ret) { 248 dev_err_ratelimited(ocelot->dev, 249 "Unable to stop RX channel\n"); 250 return; 251 } 252 253 ocelot_fdma_rx_set_llp(rx_ring); 254 255 /* FDMA stopped on the last DCB that contained a NULL LLP, since 256 * we processed some DCBs in RX, there is free space, and we must set 257 * DCB_LLP to point to the next DCB 258 */ 259 llp_prev = ocelot_fdma_readl(ocelot, MSCC_FDMA_DCB_LLP_PREV(chan)); 260 dma_base = rx_ring->dcbs_dma; 261 262 /* Get the next DMA addr located after LLP == NULL DCB */ 263 idx = ocelot_fdma_dma_idx(dma_base, llp_prev); 264 idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_RX_RING_SIZE); 265 new_llp = ocelot_fdma_idx_dma(dma_base, idx); 266 267 /* Finally reactivate the channel */ 268 ocelot_fdma_activate_chan(ocelot, new_llp, chan); 269 } 270 271 static bool ocelot_fdma_add_rx_frag(struct ocelot_fdma_rx_buf *rxb, u32 stat, 272 struct sk_buff *skb, bool first) 273 { 274 int size = MSCC_FDMA_DCB_STAT_BLOCKL(stat); 275 struct page *page = rxb->page; 276 277 if (likely(first)) { 278 skb_put(skb, size); 279 } else { 280 skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, page, 281 rxb->page_offset, size, OCELOT_FDMA_RX_SIZE); 282 } 283 284 /* Try to reuse page */ 285 if (unlikely(page_ref_count(page) != 1 || page_is_pfmemalloc(page))) 286 return false; 287 288 /* Change offset to the other half */ 289 rxb->page_offset ^= OCELOT_FDMA_RX_SIZE; 290 291 page_ref_inc(page); 292 293 return true; 294 } 295 296 static void ocelot_fdma_reuse_rx_page(struct ocelot *ocelot, 297 struct ocelot_fdma_rx_buf *old_rxb) 298 { 299 struct ocelot_fdma_rx_ring *rx_ring = &ocelot->fdma->rx_ring; 300 struct ocelot_fdma_rx_buf *new_rxb; 301 302 new_rxb = &rx_ring->bufs[rx_ring->next_to_alloc]; 303 rx_ring->next_to_alloc = ocelot_fdma_idx_next(rx_ring->next_to_alloc, 304 OCELOT_FDMA_RX_RING_SIZE); 305 306 /* Copy page reference */ 307 *new_rxb = *old_rxb; 308 309 /* Sync for use by the device */ 310 dma_sync_single_range_for_device(ocelot->dev, old_rxb->dma_addr, 311 old_rxb->page_offset, 312 OCELOT_FDMA_RX_SIZE, DMA_FROM_DEVICE); 313 } 314 315 static struct sk_buff *ocelot_fdma_get_skb(struct ocelot *ocelot, u32 stat, 316 struct ocelot_fdma_rx_buf *rxb, 317 struct sk_buff *skb) 318 { 319 bool first = false; 320 321 /* Allocate skb head and data */ 322 if (likely(!skb)) { 323 void *buff_addr = page_address(rxb->page) + 324 rxb->page_offset; 325 326 skb = build_skb(buff_addr, OCELOT_FDMA_SKBFRAG_SIZE); 327 if (unlikely(!skb)) { 328 dev_err_ratelimited(ocelot->dev, 329 "build_skb failed !\n"); 330 return NULL; 331 } 332 first = true; 333 } 334 335 dma_sync_single_range_for_cpu(ocelot->dev, rxb->dma_addr, 336 rxb->page_offset, OCELOT_FDMA_RX_SIZE, 337 DMA_FROM_DEVICE); 338 339 if (ocelot_fdma_add_rx_frag(rxb, stat, skb, first)) { 340 /* Reuse the free half of the page for the next_to_alloc DCB*/ 341 ocelot_fdma_reuse_rx_page(ocelot, rxb); 342 } else { 343 /* page cannot be reused, unmap it */ 344 dma_unmap_page(ocelot->dev, rxb->dma_addr, PAGE_SIZE, 345 DMA_FROM_DEVICE); 346 } 347 348 /* clear rx buff content */ 349 rxb->page = NULL; 350 351 return skb; 352 } 353 354 static bool ocelot_fdma_receive_skb(struct ocelot *ocelot, struct sk_buff *skb) 355 { 356 struct net_device *ndev; 357 void *xfh = skb->data; 358 u64 timestamp; 359 u64 src_port; 360 361 skb_pull(skb, OCELOT_TAG_LEN); 362 363 ocelot_xfh_get_src_port(xfh, &src_port); 364 if (unlikely(src_port >= ocelot->num_phys_ports)) 365 return false; 366 367 ndev = ocelot_port_to_netdev(ocelot, src_port); 368 if (unlikely(!ndev)) 369 return false; 370 371 pskb_trim(skb, skb->len - ETH_FCS_LEN); 372 373 skb->dev = ndev; 374 skb->protocol = eth_type_trans(skb, skb->dev); 375 skb->dev->stats.rx_bytes += skb->len; 376 skb->dev->stats.rx_packets++; 377 378 if (ocelot->ptp) { 379 ocelot_xfh_get_rew_val(xfh, ×tamp); 380 ocelot_ptp_rx_timestamp(ocelot, skb, timestamp); 381 } 382 383 if (likely(!skb_defer_rx_timestamp(skb))) 384 netif_receive_skb(skb); 385 386 return true; 387 } 388 389 static int ocelot_fdma_rx_get(struct ocelot *ocelot, int budget) 390 { 391 struct ocelot_fdma *fdma = ocelot->fdma; 392 struct ocelot_fdma_rx_ring *rx_ring; 393 struct ocelot_fdma_rx_buf *rxb; 394 struct ocelot_fdma_dcb *dcb; 395 struct sk_buff *skb; 396 int work_done = 0; 397 int cleaned_cnt; 398 u32 stat; 399 u16 idx; 400 401 cleaned_cnt = ocelot_fdma_rx_ring_free(fdma); 402 rx_ring = &fdma->rx_ring; 403 skb = rx_ring->skb; 404 405 while (budget--) { 406 idx = rx_ring->next_to_clean; 407 dcb = &rx_ring->dcbs[idx]; 408 stat = dcb->stat; 409 if (MSCC_FDMA_DCB_STAT_BLOCKL(stat) == 0) 410 break; 411 412 /* New packet is a start of frame but we already got a skb set, 413 * we probably lost an EOF packet, free skb 414 */ 415 if (unlikely(skb && (stat & MSCC_FDMA_DCB_STAT_SOF))) { 416 dev_kfree_skb(skb); 417 skb = NULL; 418 } 419 420 rxb = &rx_ring->bufs[idx]; 421 /* Fetch next to clean buffer from the rx_ring */ 422 skb = ocelot_fdma_get_skb(ocelot, stat, rxb, skb); 423 if (unlikely(!skb)) 424 break; 425 426 work_done++; 427 cleaned_cnt++; 428 429 idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_RX_RING_SIZE); 430 rx_ring->next_to_clean = idx; 431 432 if (unlikely(stat & MSCC_FDMA_DCB_STAT_ABORT || 433 stat & MSCC_FDMA_DCB_STAT_PD)) { 434 dev_err_ratelimited(ocelot->dev, 435 "DCB aborted or pruned\n"); 436 dev_kfree_skb(skb); 437 skb = NULL; 438 continue; 439 } 440 441 /* We still need to process the other fragment of the packet 442 * before delivering it to the network stack 443 */ 444 if (!(stat & MSCC_FDMA_DCB_STAT_EOF)) 445 continue; 446 447 if (unlikely(!ocelot_fdma_receive_skb(ocelot, skb))) 448 dev_kfree_skb(skb); 449 450 skb = NULL; 451 } 452 453 rx_ring->skb = skb; 454 455 if (cleaned_cnt) 456 ocelot_fdma_alloc_rx_buffs(ocelot, cleaned_cnt); 457 458 return work_done; 459 } 460 461 static void ocelot_fdma_wakeup_netdev(struct ocelot *ocelot) 462 { 463 struct ocelot_port_private *priv; 464 struct ocelot_port *ocelot_port; 465 struct net_device *dev; 466 int port; 467 468 for (port = 0; port < ocelot->num_phys_ports; port++) { 469 ocelot_port = ocelot->ports[port]; 470 if (!ocelot_port) 471 continue; 472 priv = container_of(ocelot_port, struct ocelot_port_private, 473 port); 474 dev = priv->dev; 475 476 if (unlikely(netif_queue_stopped(dev))) 477 netif_wake_queue(dev); 478 } 479 } 480 481 static void ocelot_fdma_tx_cleanup(struct ocelot *ocelot, int budget) 482 { 483 struct ocelot_fdma *fdma = ocelot->fdma; 484 struct ocelot_fdma_tx_ring *tx_ring; 485 struct ocelot_fdma_tx_buf *buf; 486 unsigned int new_null_llp_idx; 487 struct ocelot_fdma_dcb *dcb; 488 bool end_of_list = false; 489 struct sk_buff *skb; 490 dma_addr_t dma; 491 u32 dcb_llp; 492 u16 ntc; 493 int ret; 494 495 tx_ring = &fdma->tx_ring; 496 497 /* Purge the TX packets that have been sent up to the NULL llp or the 498 * end of done list. 499 */ 500 while (!ocelot_fdma_tx_ring_empty(fdma)) { 501 ntc = tx_ring->next_to_clean; 502 dcb = &tx_ring->dcbs[ntc]; 503 if (!(dcb->stat & MSCC_FDMA_DCB_STAT_PD)) 504 break; 505 506 buf = &tx_ring->bufs[ntc]; 507 skb = buf->skb; 508 dma_unmap_single(ocelot->dev, dma_unmap_addr(buf, dma_addr), 509 skb->len, DMA_TO_DEVICE); 510 napi_consume_skb(skb, budget); 511 dcb_llp = dcb->llp; 512 513 /* Only update after accessing all dcb fields */ 514 tx_ring->next_to_clean = ocelot_fdma_idx_next(ntc, 515 OCELOT_FDMA_TX_RING_SIZE); 516 517 /* If we hit the NULL LLP, stop, we might need to reload FDMA */ 518 if (dcb_llp == 0) { 519 end_of_list = true; 520 break; 521 } 522 } 523 524 /* No need to try to wake if there were no TX cleaned_cnt up. */ 525 if (ocelot_fdma_tx_ring_free(fdma)) 526 ocelot_fdma_wakeup_netdev(ocelot); 527 528 /* If there is still some DCBs to be processed by the FDMA or if the 529 * pending list is empty, there is no need to restart the FDMA. 530 */ 531 if (!end_of_list || ocelot_fdma_tx_ring_empty(fdma)) 532 return; 533 534 ret = ocelot_fdma_wait_chan_safe(ocelot, MSCC_FDMA_INJ_CHAN); 535 if (ret) { 536 dev_warn(ocelot->dev, 537 "Failed to wait for TX channel to stop\n"); 538 return; 539 } 540 541 /* Set NULL LLP to be the last DCB used */ 542 new_null_llp_idx = ocelot_fdma_idx_prev(tx_ring->next_to_use, 543 OCELOT_FDMA_TX_RING_SIZE); 544 dcb = &tx_ring->dcbs[new_null_llp_idx]; 545 dcb->llp = 0; 546 547 dma = ocelot_fdma_idx_dma(tx_ring->dcbs_dma, tx_ring->next_to_clean); 548 ocelot_fdma_activate_chan(ocelot, dma, MSCC_FDMA_INJ_CHAN); 549 } 550 551 static int ocelot_fdma_napi_poll(struct napi_struct *napi, int budget) 552 { 553 struct ocelot_fdma *fdma = container_of(napi, struct ocelot_fdma, napi); 554 struct ocelot *ocelot = fdma->ocelot; 555 int work_done = 0; 556 bool rx_stopped; 557 558 ocelot_fdma_tx_cleanup(ocelot, budget); 559 560 rx_stopped = ocelot_fdma_check_stop_rx(ocelot); 561 562 work_done = ocelot_fdma_rx_get(ocelot, budget); 563 564 if (rx_stopped) 565 ocelot_fdma_rx_restart(ocelot); 566 567 if (work_done < budget) { 568 napi_complete_done(&fdma->napi, work_done); 569 ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA, 570 BIT(MSCC_FDMA_INJ_CHAN) | 571 BIT(MSCC_FDMA_XTR_CHAN)); 572 } 573 574 return work_done; 575 } 576 577 static irqreturn_t ocelot_fdma_interrupt(int irq, void *dev_id) 578 { 579 u32 ident, llp, frm, err, err_code; 580 struct ocelot *ocelot = dev_id; 581 582 ident = ocelot_fdma_readl(ocelot, MSCC_FDMA_INTR_IDENT); 583 frm = ocelot_fdma_readl(ocelot, MSCC_FDMA_INTR_FRM); 584 llp = ocelot_fdma_readl(ocelot, MSCC_FDMA_INTR_LLP); 585 586 ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_LLP, llp & ident); 587 ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_FRM, frm & ident); 588 if (frm || llp) { 589 ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA, 0); 590 napi_schedule(&ocelot->fdma->napi); 591 } 592 593 err = ocelot_fdma_readl(ocelot, MSCC_FDMA_EVT_ERR); 594 if (unlikely(err)) { 595 err_code = ocelot_fdma_readl(ocelot, MSCC_FDMA_EVT_ERR_CODE); 596 dev_err_ratelimited(ocelot->dev, 597 "Error ! chans mask: %#x, code: %#x\n", 598 err, err_code); 599 600 ocelot_fdma_writel(ocelot, MSCC_FDMA_EVT_ERR, err); 601 ocelot_fdma_writel(ocelot, MSCC_FDMA_EVT_ERR_CODE, err_code); 602 } 603 604 return IRQ_HANDLED; 605 } 606 607 static void ocelot_fdma_send_skb(struct ocelot *ocelot, 608 struct ocelot_fdma *fdma, struct sk_buff *skb) 609 { 610 struct ocelot_fdma_tx_ring *tx_ring = &fdma->tx_ring; 611 struct ocelot_fdma_tx_buf *tx_buf; 612 struct ocelot_fdma_dcb *dcb; 613 dma_addr_t dma; 614 u16 next_idx; 615 616 dcb = &tx_ring->dcbs[tx_ring->next_to_use]; 617 tx_buf = &tx_ring->bufs[tx_ring->next_to_use]; 618 if (!ocelot_fdma_tx_dcb_set_skb(ocelot, tx_buf, dcb, skb)) { 619 dev_kfree_skb_any(skb); 620 return; 621 } 622 623 next_idx = ocelot_fdma_idx_next(tx_ring->next_to_use, 624 OCELOT_FDMA_TX_RING_SIZE); 625 skb_tx_timestamp(skb); 626 627 /* If the FDMA TX chan is empty, then enqueue the DCB directly */ 628 if (ocelot_fdma_tx_ring_empty(fdma)) { 629 dma = ocelot_fdma_idx_dma(tx_ring->dcbs_dma, 630 tx_ring->next_to_use); 631 ocelot_fdma_activate_chan(ocelot, dma, MSCC_FDMA_INJ_CHAN); 632 } else { 633 /* Chain the DCBs */ 634 dcb->llp = ocelot_fdma_idx_dma(tx_ring->dcbs_dma, next_idx); 635 } 636 637 tx_ring->next_to_use = next_idx; 638 } 639 640 static int ocelot_fdma_prepare_skb(struct ocelot *ocelot, int port, u32 rew_op, 641 struct sk_buff *skb, struct net_device *dev) 642 { 643 int needed_headroom = max_t(int, OCELOT_TAG_LEN - skb_headroom(skb), 0); 644 int needed_tailroom = max_t(int, ETH_FCS_LEN - skb_tailroom(skb), 0); 645 void *ifh; 646 int err; 647 648 if (unlikely(needed_headroom || needed_tailroom || 649 skb_header_cloned(skb))) { 650 err = pskb_expand_head(skb, needed_headroom, needed_tailroom, 651 GFP_ATOMIC); 652 if (unlikely(err)) { 653 dev_kfree_skb_any(skb); 654 return 1; 655 } 656 } 657 658 err = skb_linearize(skb); 659 if (err) { 660 net_err_ratelimited("%s: skb_linearize error (%d)!\n", 661 dev->name, err); 662 dev_kfree_skb_any(skb); 663 return 1; 664 } 665 666 ifh = skb_push(skb, OCELOT_TAG_LEN); 667 skb_put(skb, ETH_FCS_LEN); 668 memset(ifh, 0, OCELOT_TAG_LEN); 669 ocelot_ifh_port_set(ifh, port, rew_op, skb_vlan_tag_get(skb)); 670 671 return 0; 672 } 673 674 int ocelot_fdma_inject_frame(struct ocelot *ocelot, int port, u32 rew_op, 675 struct sk_buff *skb, struct net_device *dev) 676 { 677 struct ocelot_fdma *fdma = ocelot->fdma; 678 int ret = NETDEV_TX_OK; 679 680 spin_lock(&fdma->tx_ring.xmit_lock); 681 682 if (ocelot_fdma_tx_ring_free(fdma) == 0) { 683 netif_stop_queue(dev); 684 ret = NETDEV_TX_BUSY; 685 goto out; 686 } 687 688 if (ocelot_fdma_prepare_skb(ocelot, port, rew_op, skb, dev)) 689 goto out; 690 691 ocelot_fdma_send_skb(ocelot, fdma, skb); 692 693 out: 694 spin_unlock(&fdma->tx_ring.xmit_lock); 695 696 return ret; 697 } 698 699 static void ocelot_fdma_free_rx_ring(struct ocelot *ocelot) 700 { 701 struct ocelot_fdma *fdma = ocelot->fdma; 702 struct ocelot_fdma_rx_ring *rx_ring; 703 struct ocelot_fdma_rx_buf *rxb; 704 u16 idx; 705 706 rx_ring = &fdma->rx_ring; 707 idx = rx_ring->next_to_clean; 708 709 /* Free the pages held in the RX ring */ 710 while (idx != rx_ring->next_to_use) { 711 rxb = &rx_ring->bufs[idx]; 712 dma_unmap_page(ocelot->dev, rxb->dma_addr, PAGE_SIZE, 713 DMA_FROM_DEVICE); 714 __free_page(rxb->page); 715 idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_RX_RING_SIZE); 716 } 717 718 if (fdma->rx_ring.skb) 719 dev_kfree_skb_any(fdma->rx_ring.skb); 720 } 721 722 static void ocelot_fdma_free_tx_ring(struct ocelot *ocelot) 723 { 724 struct ocelot_fdma *fdma = ocelot->fdma; 725 struct ocelot_fdma_tx_ring *tx_ring; 726 struct ocelot_fdma_tx_buf *txb; 727 struct sk_buff *skb; 728 u16 idx; 729 730 tx_ring = &fdma->tx_ring; 731 idx = tx_ring->next_to_clean; 732 733 while (idx != tx_ring->next_to_use) { 734 txb = &tx_ring->bufs[idx]; 735 skb = txb->skb; 736 dma_unmap_single(ocelot->dev, dma_unmap_addr(txb, dma_addr), 737 skb->len, DMA_TO_DEVICE); 738 dev_kfree_skb_any(skb); 739 idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_TX_RING_SIZE); 740 } 741 } 742 743 static int ocelot_fdma_rings_alloc(struct ocelot *ocelot) 744 { 745 struct ocelot_fdma *fdma = ocelot->fdma; 746 struct ocelot_fdma_dcb *dcbs; 747 unsigned int adjust; 748 dma_addr_t dcbs_dma; 749 int ret; 750 751 /* Create a pool of consistent memory blocks for hardware descriptors */ 752 fdma->dcbs_base = dmam_alloc_coherent(ocelot->dev, 753 OCELOT_DCBS_HW_ALLOC_SIZE, 754 &fdma->dcbs_dma_base, GFP_KERNEL); 755 if (!fdma->dcbs_base) 756 return -ENOMEM; 757 758 /* DCBs must be aligned on a 32bit boundary */ 759 dcbs = fdma->dcbs_base; 760 dcbs_dma = fdma->dcbs_dma_base; 761 if (!IS_ALIGNED(dcbs_dma, 4)) { 762 adjust = dcbs_dma & 0x3; 763 dcbs_dma = ALIGN(dcbs_dma, 4); 764 dcbs = (void *)dcbs + adjust; 765 } 766 767 /* TX queue */ 768 fdma->tx_ring.dcbs = dcbs; 769 fdma->tx_ring.dcbs_dma = dcbs_dma; 770 spin_lock_init(&fdma->tx_ring.xmit_lock); 771 772 /* RX queue */ 773 fdma->rx_ring.dcbs = dcbs + OCELOT_FDMA_TX_RING_SIZE; 774 fdma->rx_ring.dcbs_dma = dcbs_dma + OCELOT_FDMA_TX_DCB_SIZE; 775 ret = ocelot_fdma_alloc_rx_buffs(ocelot, 776 ocelot_fdma_tx_ring_free(fdma)); 777 if (ret) { 778 ocelot_fdma_free_rx_ring(ocelot); 779 return ret; 780 } 781 782 /* Set the last DCB LLP as NULL, this is normally done when restarting 783 * the RX chan, but this is for the first run 784 */ 785 ocelot_fdma_rx_set_llp(&fdma->rx_ring); 786 787 return 0; 788 } 789 790 void ocelot_fdma_netdev_init(struct ocelot *ocelot, struct net_device *dev) 791 { 792 struct ocelot_fdma *fdma = ocelot->fdma; 793 794 dev->needed_headroom = OCELOT_TAG_LEN; 795 dev->needed_tailroom = ETH_FCS_LEN; 796 797 if (fdma->ndev) 798 return; 799 800 fdma->ndev = dev; 801 netif_napi_add_weight(dev, &fdma->napi, ocelot_fdma_napi_poll, 802 OCELOT_FDMA_WEIGHT); 803 } 804 805 void ocelot_fdma_netdev_deinit(struct ocelot *ocelot, struct net_device *dev) 806 { 807 struct ocelot_fdma *fdma = ocelot->fdma; 808 809 if (fdma->ndev == dev) { 810 netif_napi_del(&fdma->napi); 811 fdma->ndev = NULL; 812 } 813 } 814 815 void ocelot_fdma_init(struct platform_device *pdev, struct ocelot *ocelot) 816 { 817 struct device *dev = ocelot->dev; 818 struct ocelot_fdma *fdma; 819 int ret; 820 821 fdma = devm_kzalloc(dev, sizeof(*fdma), GFP_KERNEL); 822 if (!fdma) 823 return; 824 825 ocelot->fdma = fdma; 826 ocelot->dev->coherent_dma_mask = DMA_BIT_MASK(32); 827 828 ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA, 0); 829 830 fdma->ocelot = ocelot; 831 fdma->irq = platform_get_irq_byname(pdev, "fdma"); 832 ret = devm_request_irq(dev, fdma->irq, ocelot_fdma_interrupt, 0, 833 dev_name(dev), ocelot); 834 if (ret) 835 goto err_free_fdma; 836 837 ret = ocelot_fdma_rings_alloc(ocelot); 838 if (ret) 839 goto err_free_irq; 840 841 static_branch_enable(&ocelot_fdma_enabled); 842 843 return; 844 845 err_free_irq: 846 devm_free_irq(dev, fdma->irq, fdma); 847 err_free_fdma: 848 devm_kfree(dev, fdma); 849 850 ocelot->fdma = NULL; 851 } 852 853 void ocelot_fdma_start(struct ocelot *ocelot) 854 { 855 struct ocelot_fdma *fdma = ocelot->fdma; 856 857 /* Reconfigure for extraction and injection using DMA */ 858 ocelot_write_rix(ocelot, QS_INJ_GRP_CFG_MODE(2), QS_INJ_GRP_CFG, 0); 859 ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(0), QS_INJ_CTRL, 0); 860 861 ocelot_write_rix(ocelot, QS_XTR_GRP_CFG_MODE(2), QS_XTR_GRP_CFG, 0); 862 863 ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_LLP, 0xffffffff); 864 ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_FRM, 0xffffffff); 865 866 ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_LLP_ENA, 867 BIT(MSCC_FDMA_INJ_CHAN) | BIT(MSCC_FDMA_XTR_CHAN)); 868 ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_FRM_ENA, 869 BIT(MSCC_FDMA_XTR_CHAN)); 870 ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA, 871 BIT(MSCC_FDMA_INJ_CHAN) | BIT(MSCC_FDMA_XTR_CHAN)); 872 873 napi_enable(&fdma->napi); 874 875 ocelot_fdma_activate_chan(ocelot, ocelot->fdma->rx_ring.dcbs_dma, 876 MSCC_FDMA_XTR_CHAN); 877 } 878 879 void ocelot_fdma_deinit(struct ocelot *ocelot) 880 { 881 struct ocelot_fdma *fdma = ocelot->fdma; 882 883 ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA, 0); 884 ocelot_fdma_writel(ocelot, MSCC_FDMA_CH_FORCEDIS, 885 BIT(MSCC_FDMA_XTR_CHAN)); 886 ocelot_fdma_writel(ocelot, MSCC_FDMA_CH_FORCEDIS, 887 BIT(MSCC_FDMA_INJ_CHAN)); 888 napi_synchronize(&fdma->napi); 889 napi_disable(&fdma->napi); 890 891 ocelot_fdma_free_rx_ring(ocelot); 892 ocelot_fdma_free_tx_ring(ocelot); 893 } 894