drivers/net/ethernet/mscc/ocelot_fdma.c

   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, &timestamp);
 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 }