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