drivers/net/ethernet/microchip/lan966x/lan966x_fdma.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2
   3 #include "lan966x_main.h"
   4
   5 static int lan966x_fdma_channel_active(struct lan966x *lan966x)
   6 {
   7         return lan_rd(lan966x, FDMA_CH_ACTIVE);
   8 }
   9
  10 static struct page *lan966x_fdma_rx_alloc_page(struct lan966x_rx *rx,
  11                                                struct lan966x_db *db)
  12 {
  13         struct lan966x *lan966x = rx->lan966x;
  14         dma_addr_t dma_addr;
  15         struct page *page;
  16
  17         page = dev_alloc_pages(rx->page_order);
  18         if (unlikely(!page))
  19                 return NULL;
  20
  21         dma_addr = dma_map_page(lan966x->dev, page, 0,
  22                                 PAGE_SIZE << rx->page_order,
  23                                 DMA_FROM_DEVICE);
  24         if (unlikely(dma_mapping_error(lan966x->dev, dma_addr)))
  25                 goto free_page;
  26
  27         db->dataptr = dma_addr;
  28
  29         return page;
  30
  31 free_page:
  32         __free_pages(page, rx->page_order);
  33         return NULL;
  34 }
  35
  36 static void lan966x_fdma_rx_free_pages(struct lan966x_rx *rx)
  37 {
  38         struct lan966x *lan966x = rx->lan966x;
  39         struct lan966x_rx_dcb *dcb;
  40         struct lan966x_db *db;
  41         int i, j;
  42
  43         for (i = 0; i < FDMA_DCB_MAX; ++i) {
  44                 dcb = &rx->dcbs[i];
  45
  46                 for (j = 0; j < FDMA_RX_DCB_MAX_DBS; ++j) {
  47                         db = &dcb->db[j];
  48                         dma_unmap_single(lan966x->dev,
  49                                          (dma_addr_t)db->dataptr,
  50                                          PAGE_SIZE << rx->page_order,
  51                                          DMA_FROM_DEVICE);
  52                         __free_pages(rx->page[i][j], rx->page_order);
  53                 }
  54         }
  55 }
  56
  57 static void lan966x_fdma_rx_add_dcb(struct lan966x_rx *rx,
  58                                     struct lan966x_rx_dcb *dcb,
  59                                     u64 nextptr)
  60 {
  61         struct lan966x_db *db;
  62         int i;
  63
  64         for (i = 0; i < FDMA_RX_DCB_MAX_DBS; ++i) {
  65                 db = &dcb->db[i];
  66                 db->status = FDMA_DCB_STATUS_INTR;
  67         }
  68
  69         dcb->nextptr = FDMA_DCB_INVALID_DATA;
  70         dcb->info = FDMA_DCB_INFO_DATAL(PAGE_SIZE << rx->page_order);
  71
  72         rx->last_entry->nextptr = nextptr;
  73         rx->last_entry = dcb;
  74 }
  75
  76 static int lan966x_fdma_rx_alloc(struct lan966x_rx *rx)
  77 {
  78         struct lan966x *lan966x = rx->lan966x;
  79         struct lan966x_rx_dcb *dcb;
  80         struct lan966x_db *db;
  81         struct page *page;
  82         int i, j;
  83         int size;
  84
  85         /* calculate how many pages are needed to allocate the dcbs */
  86         size = sizeof(struct lan966x_rx_dcb) * FDMA_DCB_MAX;
  87         size = ALIGN(size, PAGE_SIZE);
  88
  89         rx->dcbs = dma_alloc_coherent(lan966x->dev, size, &rx->dma, GFP_KERNEL);
  90         if (!rx->dcbs)
  91                 return -ENOMEM;
  92
  93         rx->last_entry = rx->dcbs;
  94         rx->db_index = 0;
  95         rx->dcb_index = 0;
  96
  97         /* Now for each dcb allocate the dbs */
  98         for (i = 0; i < FDMA_DCB_MAX; ++i) {
  99                 dcb = &rx->dcbs[i];
 100                 dcb->info = 0;
 101
 102                 /* For each db allocate a page and map it to the DB dataptr. */
 103                 for (j = 0; j < FDMA_RX_DCB_MAX_DBS; ++j) {
 104                         db = &dcb->db[j];
 105                         page = lan966x_fdma_rx_alloc_page(rx, db);
 106                         if (!page)
 107                                 return -ENOMEM;
 108
 109                         db->status = 0;
 110                         rx->page[i][j] = page;
 111                 }
 112
 113                 lan966x_fdma_rx_add_dcb(rx, dcb, rx->dma + sizeof(*dcb) * i);
 114         }
 115
 116         return 0;
 117 }
 118
 119 static void lan966x_fdma_rx_free(struct lan966x_rx *rx)
 120 {
 121         struct lan966x *lan966x = rx->lan966x;
 122         u32 size;
 123
 124         /* Now it is possible to do the cleanup of dcb */
 125         size = sizeof(struct lan966x_tx_dcb) * FDMA_DCB_MAX;
 126         size = ALIGN(size, PAGE_SIZE);
 127         dma_free_coherent(lan966x->dev, size, rx->dcbs, rx->dma);
 128 }
 129
 130 static void lan966x_fdma_rx_start(struct lan966x_rx *rx)
 131 {
 132         struct lan966x *lan966x = rx->lan966x;
 133         u32 mask;
 134
 135         /* When activating a channel, first is required to write the first DCB
 136          * address and then to activate it
 137          */
 138         lan_wr(lower_32_bits((u64)rx->dma), lan966x,
 139                FDMA_DCB_LLP(rx->channel_id));
 140         lan_wr(upper_32_bits((u64)rx->dma), lan966x,
 141                FDMA_DCB_LLP1(rx->channel_id));
 142
 143         lan_wr(FDMA_CH_CFG_CH_DCB_DB_CNT_SET(FDMA_RX_DCB_MAX_DBS) |
 144                FDMA_CH_CFG_CH_INTR_DB_EOF_ONLY_SET(1) |
 145                FDMA_CH_CFG_CH_INJ_PORT_SET(0) |
 146                FDMA_CH_CFG_CH_MEM_SET(1),
 147                lan966x, FDMA_CH_CFG(rx->channel_id));
 148
 149         /* Start fdma */
 150         lan_rmw(FDMA_PORT_CTRL_XTR_STOP_SET(0),
 151                 FDMA_PORT_CTRL_XTR_STOP,
 152                 lan966x, FDMA_PORT_CTRL(0));
 153
 154         /* Enable interrupts */
 155         mask = lan_rd(lan966x, FDMA_INTR_DB_ENA);
 156         mask = FDMA_INTR_DB_ENA_INTR_DB_ENA_GET(mask);
 157         mask |= BIT(rx->channel_id);
 158         lan_rmw(FDMA_INTR_DB_ENA_INTR_DB_ENA_SET(mask),
 159                 FDMA_INTR_DB_ENA_INTR_DB_ENA,
 160                 lan966x, FDMA_INTR_DB_ENA);
 161
 162         /* Activate the channel */
 163         lan_rmw(FDMA_CH_ACTIVATE_CH_ACTIVATE_SET(BIT(rx->channel_id)),
 164                 FDMA_CH_ACTIVATE_CH_ACTIVATE,
 165                 lan966x, FDMA_CH_ACTIVATE);
 166 }
 167
 168 static void lan966x_fdma_rx_disable(struct lan966x_rx *rx)
 169 {
 170         struct lan966x *lan966x = rx->lan966x;
 171         u32 val;
 172
 173         /* Disable the channel */
 174         lan_rmw(FDMA_CH_DISABLE_CH_DISABLE_SET(BIT(rx->channel_id)),
 175                 FDMA_CH_DISABLE_CH_DISABLE,
 176                 lan966x, FDMA_CH_DISABLE);
 177
 178         readx_poll_timeout_atomic(lan966x_fdma_channel_active, lan966x,
 179                                   val, !(val & BIT(rx->channel_id)),
 180                                   READL_SLEEP_US, READL_TIMEOUT_US);
 181
 182         lan_rmw(FDMA_CH_DB_DISCARD_DB_DISCARD_SET(BIT(rx->channel_id)),
 183                 FDMA_CH_DB_DISCARD_DB_DISCARD,
 184                 lan966x, FDMA_CH_DB_DISCARD);
 185 }
 186
 187 static void lan966x_fdma_rx_reload(struct lan966x_rx *rx)
 188 {
 189         struct lan966x *lan966x = rx->lan966x;
 190
 191         lan_rmw(FDMA_CH_RELOAD_CH_RELOAD_SET(BIT(rx->channel_id)),
 192                 FDMA_CH_RELOAD_CH_RELOAD,
 193                 lan966x, FDMA_CH_RELOAD);
 194 }
 195
 196 static void lan966x_fdma_tx_add_dcb(struct lan966x_tx *tx,
 197                                     struct lan966x_tx_dcb *dcb)
 198 {
 199         dcb->nextptr = FDMA_DCB_INVALID_DATA;
 200         dcb->info = 0;
 201 }
 202
 203 static int lan966x_fdma_tx_alloc(struct lan966x_tx *tx)
 204 {
 205         struct lan966x *lan966x = tx->lan966x;
 206         struct lan966x_tx_dcb *dcb;
 207         struct lan966x_db *db;
 208         int size;
 209         int i, j;
 210
 211         tx->dcbs_buf = kcalloc(FDMA_DCB_MAX, sizeof(struct lan966x_tx_dcb_buf),
 212                                GFP_KERNEL);
 213         if (!tx->dcbs_buf)
 214                 return -ENOMEM;
 215
 216         /* calculate how many pages are needed to allocate the dcbs */
 217         size = sizeof(struct lan966x_tx_dcb) * FDMA_DCB_MAX;
 218         size = ALIGN(size, PAGE_SIZE);
 219         tx->dcbs = dma_alloc_coherent(lan966x->dev, size, &tx->dma, GFP_KERNEL);
 220         if (!tx->dcbs)
 221                 goto out;
 222
 223         /* Now for each dcb allocate the db */
 224         for (i = 0; i < FDMA_DCB_MAX; ++i) {
 225                 dcb = &tx->dcbs[i];
 226
 227                 for (j = 0; j < FDMA_TX_DCB_MAX_DBS; ++j) {
 228                         db = &dcb->db[j];
 229                         db->dataptr = 0;
 230                         db->status = 0;
 231                 }
 232
 233                 lan966x_fdma_tx_add_dcb(tx, dcb);
 234         }
 235
 236         return 0;
 237
 238 out:
 239         kfree(tx->dcbs_buf);
 240         return -ENOMEM;
 241 }
 242
 243 static void lan966x_fdma_tx_free(struct lan966x_tx *tx)
 244 {
 245         struct lan966x *lan966x = tx->lan966x;
 246         int size;
 247
 248         kfree(tx->dcbs_buf);
 249
 250         size = sizeof(struct lan966x_tx_dcb) * FDMA_DCB_MAX;
 251         size = ALIGN(size, PAGE_SIZE);
 252         dma_free_coherent(lan966x->dev, size, tx->dcbs, tx->dma);
 253 }
 254
 255 static void lan966x_fdma_tx_activate(struct lan966x_tx *tx)
 256 {
 257         struct lan966x *lan966x = tx->lan966x;
 258         u32 mask;
 259
 260         /* When activating a channel, first is required to write the first DCB
 261          * address and then to activate it
 262          */
 263         lan_wr(lower_32_bits((u64)tx->dma), lan966x,
 264                FDMA_DCB_LLP(tx->channel_id));
 265         lan_wr(upper_32_bits((u64)tx->dma), lan966x,
 266                FDMA_DCB_LLP1(tx->channel_id));
 267
 268         lan_wr(FDMA_CH_CFG_CH_DCB_DB_CNT_SET(FDMA_TX_DCB_MAX_DBS) |
 269                FDMA_CH_CFG_CH_INTR_DB_EOF_ONLY_SET(1) |
 270                FDMA_CH_CFG_CH_INJ_PORT_SET(0) |
 271                FDMA_CH_CFG_CH_MEM_SET(1),
 272                lan966x, FDMA_CH_CFG(tx->channel_id));
 273
 274         /* Start fdma */
 275         lan_rmw(FDMA_PORT_CTRL_INJ_STOP_SET(0),
 276                 FDMA_PORT_CTRL_INJ_STOP,
 277                 lan966x, FDMA_PORT_CTRL(0));
 278
 279         /* Enable interrupts */
 280         mask = lan_rd(lan966x, FDMA_INTR_DB_ENA);
 281         mask = FDMA_INTR_DB_ENA_INTR_DB_ENA_GET(mask);
 282         mask |= BIT(tx->channel_id);
 283         lan_rmw(FDMA_INTR_DB_ENA_INTR_DB_ENA_SET(mask),
 284                 FDMA_INTR_DB_ENA_INTR_DB_ENA,
 285                 lan966x, FDMA_INTR_DB_ENA);
 286
 287         /* Activate the channel */
 288         lan_rmw(FDMA_CH_ACTIVATE_CH_ACTIVATE_SET(BIT(tx->channel_id)),
 289                 FDMA_CH_ACTIVATE_CH_ACTIVATE,
 290                 lan966x, FDMA_CH_ACTIVATE);
 291 }
 292
 293 static void lan966x_fdma_tx_disable(struct lan966x_tx *tx)
 294 {
 295         struct lan966x *lan966x = tx->lan966x;
 296         u32 val;
 297
 298         /* Disable the channel */
 299         lan_rmw(FDMA_CH_DISABLE_CH_DISABLE_SET(BIT(tx->channel_id)),
 300                 FDMA_CH_DISABLE_CH_DISABLE,
 301                 lan966x, FDMA_CH_DISABLE);
 302
 303         readx_poll_timeout_atomic(lan966x_fdma_channel_active, lan966x,
 304                                   val, !(val & BIT(tx->channel_id)),
 305                                   READL_SLEEP_US, READL_TIMEOUT_US);
 306
 307         lan_rmw(FDMA_CH_DB_DISCARD_DB_DISCARD_SET(BIT(tx->channel_id)),
 308                 FDMA_CH_DB_DISCARD_DB_DISCARD,
 309                 lan966x, FDMA_CH_DB_DISCARD);
 310
 311         tx->activated = false;
 312         tx->last_in_use = -1;
 313 }
 314
 315 static void lan966x_fdma_tx_reload(struct lan966x_tx *tx)
 316 {
 317         struct lan966x *lan966x = tx->lan966x;
 318
 319         /* Write the registers to reload the channel */
 320         lan_rmw(FDMA_CH_RELOAD_CH_RELOAD_SET(BIT(tx->channel_id)),
 321                 FDMA_CH_RELOAD_CH_RELOAD,
 322                 lan966x, FDMA_CH_RELOAD);
 323 }
 324
 325 static void lan966x_fdma_wakeup_netdev(struct lan966x *lan966x)
 326 {
 327         struct lan966x_port *port;
 328         int i;
 329
 330         for (i = 0; i < lan966x->num_phys_ports; ++i) {
 331                 port = lan966x->ports[i];
 332                 if (!port)
 333                         continue;
 334
 335                 if (netif_queue_stopped(port->dev))
 336                         netif_wake_queue(port->dev);
 337         }
 338 }
 339
 340 static void lan966x_fdma_stop_netdev(struct lan966x *lan966x)
 341 {
 342         struct lan966x_port *port;
 343         int i;
 344
 345         for (i = 0; i < lan966x->num_phys_ports; ++i) {
 346                 port = lan966x->ports[i];
 347                 if (!port)
 348                         continue;
 349
 350                 netif_stop_queue(port->dev);
 351         }
 352 }
 353
 354 static void lan966x_fdma_tx_clear_buf(struct lan966x *lan966x, int weight)
 355 {
 356         struct lan966x_tx *tx = &lan966x->tx;
 357         struct lan966x_tx_dcb_buf *dcb_buf;
 358         struct lan966x_db *db;
 359         unsigned long flags;
 360         bool clear = false;
 361         int i;
 362
 363         spin_lock_irqsave(&lan966x->tx_lock, flags);
 364         for (i = 0; i < FDMA_DCB_MAX; ++i) {
 365                 dcb_buf = &tx->dcbs_buf[i];
 366
 367                 if (!dcb_buf->used)
 368                         continue;
 369
 370                 db = &tx->dcbs[i].db[0];
 371                 if (!(db->status & FDMA_DCB_STATUS_DONE))
 372                         continue;
 373
 374                 dcb_buf->dev->stats.tx_packets++;
 375                 dcb_buf->dev->stats.tx_bytes += dcb_buf->skb->len;
 376
 377                 dcb_buf->used = false;
 378                 dma_unmap_single(lan966x->dev,
 379                                  dcb_buf->dma_addr,
 380                                  dcb_buf->skb->len,
 381                                  DMA_TO_DEVICE);
 382                 if (!dcb_buf->ptp)
 383                         dev_kfree_skb_any(dcb_buf->skb);
 384
 385                 clear = true;
 386         }
 387
 388         if (clear)
 389                 lan966x_fdma_wakeup_netdev(lan966x);
 390
 391         spin_unlock_irqrestore(&lan966x->tx_lock, flags);
 392 }
 393
 394 static bool lan966x_fdma_rx_more_frames(struct lan966x_rx *rx)
 395 {
 396         struct lan966x_db *db;
 397
 398         /* Check if there is any data */
 399         db = &rx->dcbs[rx->dcb_index].db[rx->db_index];
 400         if (unlikely(!(db->status & FDMA_DCB_STATUS_DONE)))
 401                 return false;
 402
 403         return true;
 404 }
 405
 406 static struct sk_buff *lan966x_fdma_rx_get_frame(struct lan966x_rx *rx)
 407 {
 408         struct lan966x *lan966x = rx->lan966x;
 409         u64 src_port, timestamp;
 410         struct lan966x_db *db;
 411         struct sk_buff *skb;
 412         struct page *page;
 413
 414         /* Get the received frame and unmap it */
 415         db = &rx->dcbs[rx->dcb_index].db[rx->db_index];
 416         page = rx->page[rx->dcb_index][rx->db_index];
 417
 418         dma_sync_single_for_cpu(lan966x->dev, (dma_addr_t)db->dataptr,
 419                                 FDMA_DCB_STATUS_BLOCKL(db->status),
 420                                 DMA_FROM_DEVICE);
 421
 422         skb = build_skb(page_address(page), PAGE_SIZE << rx->page_order);
 423         if (unlikely(!skb))
 424                 goto unmap_page;
 425
 426         skb_put(skb, FDMA_DCB_STATUS_BLOCKL(db->status));
 427
 428         lan966x_ifh_get_src_port(skb->data, &src_port);
 429         lan966x_ifh_get_timestamp(skb->data, &timestamp);
 430
 431         if (WARN_ON(src_port >= lan966x->num_phys_ports))
 432                 goto free_skb;
 433
 434         dma_unmap_single_attrs(lan966x->dev, (dma_addr_t)db->dataptr,
 435                                PAGE_SIZE << rx->page_order, DMA_FROM_DEVICE,
 436                                DMA_ATTR_SKIP_CPU_SYNC);
 437
 438         skb->dev = lan966x->ports[src_port]->dev;
 439         skb_pull(skb, IFH_LEN * sizeof(u32));
 440
 441         if (likely(!(skb->dev->features & NETIF_F_RXFCS)))
 442                 skb_trim(skb, skb->len - ETH_FCS_LEN);
 443
 444         lan966x_ptp_rxtstamp(lan966x, skb, timestamp);
 445         skb->protocol = eth_type_trans(skb, skb->dev);
 446
 447         if (lan966x->bridge_mask & BIT(src_port)) {
 448                 skb->offload_fwd_mark = 1;
 449
 450                 skb_reset_network_header(skb);
 451                 if (!lan966x_hw_offload(lan966x, src_port, skb))
 452                         skb->offload_fwd_mark = 0;
 453         }
 454
 455         skb->dev->stats.rx_bytes += skb->len;
 456         skb->dev->stats.rx_packets++;
 457
 458         return skb;
 459
 460 free_skb:
 461         kfree_skb(skb);
 462 unmap_page:
 463         dma_unmap_single_attrs(lan966x->dev, (dma_addr_t)db->dataptr,
 464                                PAGE_SIZE << rx->page_order, DMA_FROM_DEVICE,
 465                                DMA_ATTR_SKIP_CPU_SYNC);
 466         __free_pages(page, rx->page_order);
 467
 468         return NULL;
 469 }
 470
 471 static int lan966x_fdma_napi_poll(struct napi_struct *napi, int weight)
 472 {
 473         struct lan966x *lan966x = container_of(napi, struct lan966x, napi);
 474         struct lan966x_rx *rx = &lan966x->rx;
 475         int dcb_reload = rx->dcb_index;
 476         struct lan966x_rx_dcb *old_dcb;
 477         struct lan966x_db *db;
 478         struct sk_buff *skb;
 479         struct page *page;
 480         int counter = 0;
 481         u64 nextptr;
 482
 483         lan966x_fdma_tx_clear_buf(lan966x, weight);
 484
 485         /* Get all received skb */
 486         while (counter < weight) {
 487                 if (!lan966x_fdma_rx_more_frames(rx))
 488                         break;
 489
 490                 skb = lan966x_fdma_rx_get_frame(rx);
 491
 492                 rx->page[rx->dcb_index][rx->db_index] = NULL;
 493                 rx->dcb_index++;
 494                 rx->dcb_index &= FDMA_DCB_MAX - 1;
 495
 496                 if (!skb)
 497                         break;
 498
 499                 napi_gro_receive(&lan966x->napi, skb);
 500                 counter++;
 501         }
 502
 503         /* Allocate new pages and map them */
 504         while (dcb_reload != rx->dcb_index) {
 505                 db = &rx->dcbs[dcb_reload].db[rx->db_index];
 506                 page = lan966x_fdma_rx_alloc_page(rx, db);
 507                 if (unlikely(!page))
 508                         break;
 509                 rx->page[dcb_reload][rx->db_index] = page;
 510
 511                 old_dcb = &rx->dcbs[dcb_reload];
 512                 dcb_reload++;
 513                 dcb_reload &= FDMA_DCB_MAX - 1;
 514
 515                 nextptr = rx->dma + ((unsigned long)old_dcb -
 516                                      (unsigned long)rx->dcbs);
 517                 lan966x_fdma_rx_add_dcb(rx, old_dcb, nextptr);
 518                 lan966x_fdma_rx_reload(rx);
 519         }
 520
 521         if (counter < weight && napi_complete_done(napi, counter))
 522                 lan_wr(0xff, lan966x, FDMA_INTR_DB_ENA);
 523
 524         return counter;
 525 }
 526
 527 irqreturn_t lan966x_fdma_irq_handler(int irq, void *args)
 528 {
 529         struct lan966x *lan966x = args;
 530         u32 db, err, err_type;
 531
 532         db = lan_rd(lan966x, FDMA_INTR_DB);
 533         err = lan_rd(lan966x, FDMA_INTR_ERR);
 534
 535         if (db) {
 536                 lan_wr(0, lan966x, FDMA_INTR_DB_ENA);
 537                 lan_wr(db, lan966x, FDMA_INTR_DB);
 538
 539                 napi_schedule(&lan966x->napi);
 540         }
 541
 542         if (err) {
 543                 err_type = lan_rd(lan966x, FDMA_ERRORS);
 544
 545                 WARN(1, "Unexpected error: %d, error_type: %d\n", err, err_type);
 546
 547                 lan_wr(err, lan966x, FDMA_INTR_ERR);
 548                 lan_wr(err_type, lan966x, FDMA_ERRORS);
 549         }
 550
 551         return IRQ_HANDLED;
 552 }
 553
 554 static int lan966x_fdma_get_next_dcb(struct lan966x_tx *tx)
 555 {
 556         struct lan966x_tx_dcb_buf *dcb_buf;
 557         int i;
 558
 559         for (i = 0; i < FDMA_DCB_MAX; ++i) {
 560                 dcb_buf = &tx->dcbs_buf[i];
 561                 if (!dcb_buf->used && i != tx->last_in_use)
 562                         return i;
 563         }
 564
 565         return -1;
 566 }
 567
 568 int lan966x_fdma_xmit(struct sk_buff *skb, __be32 *ifh, struct net_device *dev)
 569 {
 570         struct lan966x_port *port = netdev_priv(dev);
 571         struct lan966x *lan966x = port->lan966x;
 572         struct lan966x_tx_dcb_buf *next_dcb_buf;
 573         struct lan966x_tx_dcb *next_dcb, *dcb;
 574         struct lan966x_tx *tx = &lan966x->tx;
 575         struct lan966x_db *next_db;
 576         int needed_headroom;
 577         int needed_tailroom;
 578         dma_addr_t dma_addr;
 579         int next_to_use;
 580         int err;
 581
 582         /* Get next index */
 583         next_to_use = lan966x_fdma_get_next_dcb(tx);
 584         if (next_to_use < 0) {
 585                 netif_stop_queue(dev);
 586                 return NETDEV_TX_BUSY;
 587         }
 588
 589         if (skb_put_padto(skb, ETH_ZLEN)) {
 590                 dev->stats.tx_dropped++;
 591                 return NETDEV_TX_OK;
 592         }
 593
 594         /* skb processing */
 595         needed_headroom = max_t(int, IFH_LEN * sizeof(u32) - skb_headroom(skb), 0);
 596         needed_tailroom = max_t(int, ETH_FCS_LEN - skb_tailroom(skb), 0);
 597         if (needed_headroom || needed_tailroom || skb_header_cloned(skb)) {
 598                 err = pskb_expand_head(skb, needed_headroom, needed_tailroom,
 599                                        GFP_ATOMIC);
 600                 if (unlikely(err)) {
 601                         dev->stats.tx_dropped++;
 602                         err = NETDEV_TX_OK;
 603                         goto release;
 604                 }
 605         }
 606
 607         skb_tx_timestamp(skb);
 608         skb_push(skb, IFH_LEN * sizeof(u32));
 609         memcpy(skb->data, ifh, IFH_LEN * sizeof(u32));
 610         skb_put(skb, 4);
 611
 612         dma_addr = dma_map_single(lan966x->dev, skb->data, skb->len,
 613                                   DMA_TO_DEVICE);
 614         if (dma_mapping_error(lan966x->dev, dma_addr)) {
 615                 dev->stats.tx_dropped++;
 616                 err = NETDEV_TX_OK;
 617                 goto release;
 618         }
 619
 620         /* Setup next dcb */
 621         next_dcb = &tx->dcbs[next_to_use];
 622         next_dcb->nextptr = FDMA_DCB_INVALID_DATA;
 623
 624         next_db = &next_dcb->db[0];
 625         next_db->dataptr = dma_addr;
 626         next_db->status = FDMA_DCB_STATUS_SOF |
 627                           FDMA_DCB_STATUS_EOF |
 628                           FDMA_DCB_STATUS_INTR |
 629                           FDMA_DCB_STATUS_BLOCKO(0) |
 630                           FDMA_DCB_STATUS_BLOCKL(skb->len);
 631
 632         /* Fill up the buffer */
 633         next_dcb_buf = &tx->dcbs_buf[next_to_use];
 634         next_dcb_buf->skb = skb;
 635         next_dcb_buf->dma_addr = dma_addr;
 636         next_dcb_buf->used = true;
 637         next_dcb_buf->ptp = false;
 638         next_dcb_buf->dev = dev;
 639
 640         if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP &&
 641             LAN966X_SKB_CB(skb)->rew_op == IFH_REW_OP_TWO_STEP_PTP)
 642                 next_dcb_buf->ptp = true;
 643
 644         if (likely(lan966x->tx.activated)) {
 645                 /* Connect current dcb to the next db */
 646                 dcb = &tx->dcbs[tx->last_in_use];
 647                 dcb->nextptr = tx->dma + (next_to_use *
 648                                           sizeof(struct lan966x_tx_dcb));
 649
 650                 lan966x_fdma_tx_reload(tx);
 651         } else {
 652                 /* Because it is first time, then just activate */
 653                 lan966x->tx.activated = true;
 654                 lan966x_fdma_tx_activate(tx);
 655         }
 656
 657         /* Move to next dcb because this last in use */
 658         tx->last_in_use = next_to_use;
 659
 660         return NETDEV_TX_OK;
 661
 662 release:
 663         if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP &&
 664             LAN966X_SKB_CB(skb)->rew_op == IFH_REW_OP_TWO_STEP_PTP)
 665                 lan966x_ptp_txtstamp_release(port, skb);
 666
 667         dev_kfree_skb_any(skb);
 668         return err;
 669 }
 670
 671 static int lan966x_fdma_get_max_mtu(struct lan966x *lan966x)
 672 {
 673         int max_mtu = 0;
 674         int i;
 675
 676         for (i = 0; i < lan966x->num_phys_ports; ++i) {
 677                 struct lan966x_port *port;
 678                 int mtu;
 679
 680                 port = lan966x->ports[i];
 681                 if (!port)
 682                         continue;
 683
 684                 mtu = lan_rd(lan966x, DEV_MAC_MAXLEN_CFG(port->chip_port));
 685                 if (mtu > max_mtu)
 686                         max_mtu = mtu;
 687         }
 688
 689         return max_mtu;
 690 }
 691
 692 static int lan966x_qsys_sw_status(struct lan966x *lan966x)
 693 {
 694         return lan_rd(lan966x, QSYS_SW_STATUS(CPU_PORT));
 695 }
 696
 697 static int lan966x_fdma_reload(struct lan966x *lan966x, int new_mtu)
 698 {
 699         dma_addr_t rx_dma;
 700         void *rx_dcbs;
 701         u32 size;
 702         int err;
 703
 704         /* Store these for later to free them */
 705         rx_dma = lan966x->rx.dma;
 706         rx_dcbs = lan966x->rx.dcbs;
 707
 708         napi_synchronize(&lan966x->napi);
 709         napi_disable(&lan966x->napi);
 710         lan966x_fdma_stop_netdev(lan966x);
 711
 712         lan966x_fdma_rx_disable(&lan966x->rx);
 713         lan966x_fdma_rx_free_pages(&lan966x->rx);
 714         lan966x->rx.page_order = round_up(new_mtu, PAGE_SIZE) / PAGE_SIZE - 1;
 715         err = lan966x_fdma_rx_alloc(&lan966x->rx);
 716         if (err)
 717                 goto restore;
 718         lan966x_fdma_rx_start(&lan966x->rx);
 719
 720         size = sizeof(struct lan966x_rx_dcb) * FDMA_DCB_MAX;
 721         size = ALIGN(size, PAGE_SIZE);
 722         dma_free_coherent(lan966x->dev, size, rx_dcbs, rx_dma);
 723
 724         lan966x_fdma_wakeup_netdev(lan966x);
 725         napi_enable(&lan966x->napi);
 726
 727         return err;
 728 restore:
 729         lan966x->rx.dma = rx_dma;
 730         lan966x->rx.dcbs = rx_dcbs;
 731         lan966x_fdma_rx_start(&lan966x->rx);
 732
 733         return err;
 734 }
 735
 736 int lan966x_fdma_change_mtu(struct lan966x *lan966x)
 737 {
 738         int max_mtu;
 739         int err;
 740         u32 val;
 741
 742         max_mtu = lan966x_fdma_get_max_mtu(lan966x);
 743         max_mtu += IFH_LEN * sizeof(u32);
 744         max_mtu += SKB_DATA_ALIGN(sizeof(struct skb_shared_info));
 745         max_mtu += VLAN_HLEN * 2;
 746
 747         if (round_up(max_mtu, PAGE_SIZE) / PAGE_SIZE - 1 ==
 748             lan966x->rx.page_order)
 749                 return 0;
 750
 751         /* Disable the CPU port */
 752         lan_rmw(QSYS_SW_PORT_MODE_PORT_ENA_SET(0),
 753                 QSYS_SW_PORT_MODE_PORT_ENA,
 754                 lan966x, QSYS_SW_PORT_MODE(CPU_PORT));
 755
 756         /* Flush the CPU queues */
 757         readx_poll_timeout(lan966x_qsys_sw_status, lan966x,
 758                            val, !(QSYS_SW_STATUS_EQ_AVAIL_GET(val)),
 759                            READL_SLEEP_US, READL_TIMEOUT_US);
 760
 761         /* Add a sleep in case there are frames between the queues and the CPU
 762          * port
 763          */
 764         usleep_range(1000, 2000);
 765
 766         err = lan966x_fdma_reload(lan966x, max_mtu);
 767
 768         /* Enable back the CPU port */
 769         lan_rmw(QSYS_SW_PORT_MODE_PORT_ENA_SET(1),
 770                 QSYS_SW_PORT_MODE_PORT_ENA,
 771                 lan966x,  QSYS_SW_PORT_MODE(CPU_PORT));
 772
 773         return err;
 774 }
 775
 776 void lan966x_fdma_netdev_init(struct lan966x *lan966x, struct net_device *dev)
 777 {
 778         if (lan966x->fdma_ndev)
 779                 return;
 780
 781         lan966x->fdma_ndev = dev;
 782         netif_napi_add(dev, &lan966x->napi, lan966x_fdma_napi_poll);
 783         napi_enable(&lan966x->napi);
 784 }
 785
 786 void lan966x_fdma_netdev_deinit(struct lan966x *lan966x, struct net_device *dev)
 787 {
 788         if (lan966x->fdma_ndev == dev) {
 789                 netif_napi_del(&lan966x->napi);
 790                 lan966x->fdma_ndev = NULL;
 791         }
 792 }
 793
 794 int lan966x_fdma_init(struct lan966x *lan966x)
 795 {
 796         int err;
 797
 798         if (!lan966x->fdma)
 799                 return 0;
 800
 801         lan966x->rx.lan966x = lan966x;
 802         lan966x->rx.channel_id = FDMA_XTR_CHANNEL;
 803         lan966x->tx.lan966x = lan966x;
 804         lan966x->tx.channel_id = FDMA_INJ_CHANNEL;
 805         lan966x->tx.last_in_use = -1;
 806
 807         err = lan966x_fdma_rx_alloc(&lan966x->rx);
 808         if (err)
 809                 return err;
 810
 811         err = lan966x_fdma_tx_alloc(&lan966x->tx);
 812         if (err) {
 813                 lan966x_fdma_rx_free(&lan966x->rx);
 814                 return err;
 815         }
 816
 817         lan966x_fdma_rx_start(&lan966x->rx);
 818
 819         return 0;
 820 }
 821
 822 void lan966x_fdma_deinit(struct lan966x *lan966x)
 823 {
 824         if (!lan966x->fdma)
 825                 return;
 826
 827         lan966x_fdma_rx_disable(&lan966x->rx);
 828         lan966x_fdma_tx_disable(&lan966x->tx);
 829
 830         napi_synchronize(&lan966x->napi);
 831         napi_disable(&lan966x->napi);
 832
 833         lan966x_fdma_rx_free_pages(&lan966x->rx);
 834         lan966x_fdma_rx_free(&lan966x->rx);
 835         lan966x_fdma_tx_free(&lan966x->tx);
 836 }