drivers/net/ethernet/freescale/dpaa/dpaa_eth.c

   1 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0-or-later
   2 /*
   3  * Copyright 2008 - 2016 Freescale Semiconductor Inc.
   4  * Copyright 2020 NXP
   5  */
   6
   7 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
   8
   9 #include <linux/init.h>
  10 #include <linux/module.h>
  11 #include <linux/of_platform.h>
  12 #include <linux/of_mdio.h>
  13 #include <linux/of_net.h>
  14 #include <linux/io.h>
  15 #include <linux/if_arp.h>
  16 #include <linux/if_vlan.h>
  17 #include <linux/icmp.h>
  18 #include <linux/ip.h>
  19 #include <linux/ipv6.h>
  20 #include <linux/udp.h>
  21 #include <linux/tcp.h>
  22 #include <linux/net.h>
  23 #include <linux/skbuff.h>
  24 #include <linux/etherdevice.h>
  25 #include <linux/if_ether.h>
  26 #include <linux/highmem.h>
  27 #include <linux/percpu.h>
  28 #include <linux/dma-mapping.h>
  29 #include <linux/sort.h>
  30 #include <linux/phy_fixed.h>
  31 #include <linux/bpf.h>
  32 #include <linux/bpf_trace.h>
  33 #include <soc/fsl/bman.h>
  34 #include <soc/fsl/qman.h>
  35 #include "fman.h"
  36 #include "fman_port.h"
  37 #include "mac.h"
  38 #include "dpaa_eth.h"
  39
  40 /* CREATE_TRACE_POINTS only needs to be defined once. Other dpaa files
  41  * using trace events only need to #include <trace/events/sched.h>
  42  */
  43 #define CREATE_TRACE_POINTS
  44 #include "dpaa_eth_trace.h"
  45
  46 static int debug = -1;
  47 module_param(debug, int, 0444);
  48 MODULE_PARM_DESC(debug, "Module/Driver verbosity level (0=none,...,16=all)");
  49
  50 static u16 tx_timeout = 1000;
  51 module_param(tx_timeout, ushort, 0444);
  52 MODULE_PARM_DESC(tx_timeout, "The Tx timeout in ms");
  53
  54 #define FM_FD_STAT_RX_ERRORS                                            \
  55         (FM_FD_ERR_DMA | FM_FD_ERR_PHYSICAL     | \
  56          FM_FD_ERR_SIZE | FM_FD_ERR_CLS_DISCARD | \
  57          FM_FD_ERR_EXTRACTION | FM_FD_ERR_NO_SCHEME     | \
  58          FM_FD_ERR_PRS_TIMEOUT | FM_FD_ERR_PRS_ILL_INSTRUCT | \
  59          FM_FD_ERR_PRS_HDR_ERR)
  60
  61 #define FM_FD_STAT_TX_ERRORS \
  62         (FM_FD_ERR_UNSUPPORTED_FORMAT | \
  63          FM_FD_ERR_LENGTH | FM_FD_ERR_DMA)
  64
  65 #define DPAA_MSG_DEFAULT (NETIF_MSG_DRV | NETIF_MSG_PROBE | \
  66                           NETIF_MSG_LINK | NETIF_MSG_IFUP | \
  67                           NETIF_MSG_IFDOWN | NETIF_MSG_HW)
  68
  69 #define DPAA_INGRESS_CS_THRESHOLD 0x10000000
  70 /* Ingress congestion threshold on FMan ports
  71  * The size in bytes of the ingress tail-drop threshold on FMan ports.
  72  * Traffic piling up above this value will be rejected by QMan and discarded
  73  * by FMan.
  74  */
  75
  76 /* Size in bytes of the FQ taildrop threshold */
  77 #define DPAA_FQ_TD 0x200000
  78
  79 #define DPAA_CS_THRESHOLD_1G 0x06000000
  80 /* Egress congestion threshold on 1G ports, range 0x1000 .. 0x10000000
  81  * The size in bytes of the egress Congestion State notification threshold on
  82  * 1G ports. The 1G dTSECs can quite easily be flooded by cores doing Tx in a
  83  * tight loop (e.g. by sending UDP datagrams at "while(1) speed"),
  84  * and the larger the frame size, the more acute the problem.
  85  * So we have to find a balance between these factors:
  86  * - avoiding the device staying congested for a prolonged time (risking
  87  *   the netdev watchdog to fire - see also the tx_timeout module param);
  88  * - affecting performance of protocols such as TCP, which otherwise
  89  *   behave well under the congestion notification mechanism;
  90  * - preventing the Tx cores from tightly-looping (as if the congestion
  91  *   threshold was too low to be effective);
  92  * - running out of memory if the CS threshold is set too high.
  93  */
  94
  95 #define DPAA_CS_THRESHOLD_10G 0x10000000
  96 /* The size in bytes of the egress Congestion State notification threshold on
  97  * 10G ports, range 0x1000 .. 0x10000000
  98  */
  99
 100 /* Largest value that the FQD's OAL field can hold */
 101 #define FSL_QMAN_MAX_OAL        127
 102
 103 /* Default alignment for start of data in an Rx FD */
 104 #ifdef CONFIG_DPAA_ERRATUM_A050385
 105 /* aligning data start to 64 avoids DMA transaction splits, unless the buffer
 106  * is crossing a 4k page boundary
 107  */
 108 #define DPAA_FD_DATA_ALIGNMENT  (fman_has_errata_a050385() ? 64 : 16)
 109 /* aligning to 256 avoids DMA transaction splits caused by 4k page boundary
 110  * crossings; also, all SG fragments except the last must have a size multiple
 111  * of 256 to avoid DMA transaction splits
 112  */
 113 #define DPAA_A050385_ALIGN 256
 114 #define DPAA_FD_RX_DATA_ALIGNMENT (fman_has_errata_a050385() ? \
 115                                    DPAA_A050385_ALIGN : 16)
 116 #else
 117 #define DPAA_FD_DATA_ALIGNMENT  16
 118 #define DPAA_FD_RX_DATA_ALIGNMENT DPAA_FD_DATA_ALIGNMENT
 119 #endif
 120
 121 /* The DPAA requires 256 bytes reserved and mapped for the SGT */
 122 #define DPAA_SGT_SIZE 256
 123
 124 /* Values for the L3R field of the FM Parse Results
 125  */
 126 /* L3 Type field: First IP Present IPv4 */
 127 #define FM_L3_PARSE_RESULT_IPV4 0x8000
 128 /* L3 Type field: First IP Present IPv6 */
 129 #define FM_L3_PARSE_RESULT_IPV6 0x4000
 130 /* Values for the L4R field of the FM Parse Results */
 131 /* L4 Type field: UDP */
 132 #define FM_L4_PARSE_RESULT_UDP  0x40
 133 /* L4 Type field: TCP */
 134 #define FM_L4_PARSE_RESULT_TCP  0x20
 135
 136 /* FD status field indicating whether the FM Parser has attempted to validate
 137  * the L4 csum of the frame.
 138  * Note that having this bit set doesn't necessarily imply that the checksum
 139  * is valid. One would have to check the parse results to find that out.
 140  */
 141 #define FM_FD_STAT_L4CV         0x00000004
 142
 143 #define DPAA_SGT_MAX_ENTRIES 16 /* maximum number of entries in SG Table */
 144 #define DPAA_BUFF_RELEASE_MAX 8 /* maximum number of buffers released at once */
 145
 146 #define FSL_DPAA_BPID_INV               0xff
 147 #define FSL_DPAA_ETH_MAX_BUF_COUNT      128
 148 #define FSL_DPAA_ETH_REFILL_THRESHOLD   80
 149
 150 #define DPAA_TX_PRIV_DATA_SIZE  16
 151 #define DPAA_PARSE_RESULTS_SIZE sizeof(struct fman_prs_result)
 152 #define DPAA_TIME_STAMP_SIZE 8
 153 #define DPAA_HASH_RESULTS_SIZE 8
 154 #define DPAA_HWA_SIZE (DPAA_PARSE_RESULTS_SIZE + DPAA_TIME_STAMP_SIZE \
 155                        + DPAA_HASH_RESULTS_SIZE)
 156 #define DPAA_RX_PRIV_DATA_DEFAULT_SIZE (DPAA_TX_PRIV_DATA_SIZE + \
 157                                         XDP_PACKET_HEADROOM - DPAA_HWA_SIZE)
 158 #ifdef CONFIG_DPAA_ERRATUM_A050385
 159 #define DPAA_RX_PRIV_DATA_A050385_SIZE (DPAA_A050385_ALIGN - DPAA_HWA_SIZE)
 160 #define DPAA_RX_PRIV_DATA_SIZE (fman_has_errata_a050385() ? \
 161                                 DPAA_RX_PRIV_DATA_A050385_SIZE : \
 162                                 DPAA_RX_PRIV_DATA_DEFAULT_SIZE)
 163 #else
 164 #define DPAA_RX_PRIV_DATA_SIZE DPAA_RX_PRIV_DATA_DEFAULT_SIZE
 165 #endif
 166
 167 #define DPAA_ETH_PCD_RXQ_NUM    128
 168
 169 #define DPAA_ENQUEUE_RETRIES    100000
 170
 171 enum port_type {RX, TX};
 172
 173 struct fm_port_fqs {
 174         struct dpaa_fq *tx_defq;
 175         struct dpaa_fq *tx_errq;
 176         struct dpaa_fq *rx_defq;
 177         struct dpaa_fq *rx_errq;
 178         struct dpaa_fq *rx_pcdq;
 179 };
 180
 181 /* All the dpa bps in use at any moment */
 182 static struct dpaa_bp *dpaa_bp_array[BM_MAX_NUM_OF_POOLS];
 183
 184 #define DPAA_BP_RAW_SIZE 4096
 185
 186 #ifdef CONFIG_DPAA_ERRATUM_A050385
 187 #define dpaa_bp_size(raw_size) (SKB_WITH_OVERHEAD(raw_size) & \
 188                                 ~(DPAA_A050385_ALIGN - 1))
 189 #else
 190 #define dpaa_bp_size(raw_size) SKB_WITH_OVERHEAD(raw_size)
 191 #endif
 192
 193 static int dpaa_max_frm;
 194
 195 static int dpaa_rx_extra_headroom;
 196
 197 #define dpaa_get_max_mtu()      \
 198         (dpaa_max_frm - (VLAN_ETH_HLEN + ETH_FCS_LEN))
 199
 200 static void dpaa_eth_cgr_set_speed(struct mac_device *mac_dev, int speed);
 201
 202 static int dpaa_netdev_init(struct net_device *net_dev,
 203                             const struct net_device_ops *dpaa_ops,
 204                             u16 tx_timeout)
 205 {
 206         struct dpaa_priv *priv = netdev_priv(net_dev);
 207         struct device *dev = net_dev->dev.parent;
 208         struct mac_device *mac_dev = priv->mac_dev;
 209         struct dpaa_percpu_priv *percpu_priv;
 210         const u8 *mac_addr;
 211         int i, err;
 212
 213         /* Although we access another CPU's private data here
 214          * we do it at initialization so it is safe
 215          */
 216         for_each_possible_cpu(i) {
 217                 percpu_priv = per_cpu_ptr(priv->percpu_priv, i);
 218                 percpu_priv->net_dev = net_dev;
 219         }
 220
 221         net_dev->netdev_ops = dpaa_ops;
 222         mac_addr = mac_dev->addr;
 223
 224         net_dev->mem_start = (unsigned long)priv->mac_dev->res->start;
 225         net_dev->mem_end = (unsigned long)priv->mac_dev->res->end;
 226
 227         net_dev->min_mtu = ETH_MIN_MTU;
 228         net_dev->max_mtu = dpaa_get_max_mtu();
 229
 230         net_dev->hw_features |= (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
 231                                  NETIF_F_LLTX | NETIF_F_RXHASH);
 232
 233         net_dev->hw_features |= NETIF_F_SG | NETIF_F_HIGHDMA;
 234         /* The kernels enables GSO automatically, if we declare NETIF_F_SG.
 235          * For conformity, we'll still declare GSO explicitly.
 236          */
 237         net_dev->features |= NETIF_F_GSO;
 238         net_dev->features |= NETIF_F_RXCSUM;
 239
 240         net_dev->priv_flags |= IFF_LIVE_ADDR_CHANGE;
 241         /* we do not want shared skbs on TX */
 242         net_dev->priv_flags &= ~IFF_TX_SKB_SHARING;
 243
 244         net_dev->features |= net_dev->hw_features;
 245         net_dev->vlan_features = net_dev->features;
 246
 247         if (is_valid_ether_addr(mac_addr)) {
 248                 memcpy(net_dev->perm_addr, mac_addr, net_dev->addr_len);
 249                 eth_hw_addr_set(net_dev, mac_addr);
 250         } else {
 251                 eth_hw_addr_random(net_dev);
 252                 err = mac_dev->change_addr(mac_dev->fman_mac,
 253                         (const enet_addr_t *)net_dev->dev_addr);
 254                 if (err) {
 255                         dev_err(dev, "Failed to set random MAC address\n");
 256                         return -EINVAL;
 257                 }
 258                 dev_info(dev, "Using random MAC address: %pM\n",
 259                          net_dev->dev_addr);
 260         }
 261
 262         net_dev->ethtool_ops = &dpaa_ethtool_ops;
 263
 264         net_dev->needed_headroom = priv->tx_headroom;
 265         net_dev->watchdog_timeo = msecs_to_jiffies(tx_timeout);
 266
 267         /* The rest of the config is filled in by the mac device already */
 268         mac_dev->phylink_config.dev = &net_dev->dev;
 269         mac_dev->phylink_config.type = PHYLINK_NETDEV;
 270         mac_dev->update_speed = dpaa_eth_cgr_set_speed;
 271         mac_dev->phylink = phylink_create(&mac_dev->phylink_config,
 272                                           dev_fwnode(mac_dev->dev),
 273                                           mac_dev->phy_if,
 274                                           mac_dev->phylink_ops);
 275         if (IS_ERR(mac_dev->phylink)) {
 276                 err = PTR_ERR(mac_dev->phylink);
 277                 dev_err_probe(dev, err, "Could not create phylink\n");
 278                 return err;
 279         }
 280
 281         /* start without the RUNNING flag, phylib controls it later */
 282         netif_carrier_off(net_dev);
 283
 284         err = register_netdev(net_dev);
 285         if (err < 0) {
 286                 dev_err(dev, "register_netdev() = %d\n", err);
 287                 phylink_destroy(mac_dev->phylink);
 288                 return err;
 289         }
 290
 291         return 0;
 292 }
 293
 294 static int dpaa_stop(struct net_device *net_dev)
 295 {
 296         struct mac_device *mac_dev;
 297         struct dpaa_priv *priv;
 298         int i, err, error;
 299
 300         priv = netdev_priv(net_dev);
 301         mac_dev = priv->mac_dev;
 302
 303         netif_tx_stop_all_queues(net_dev);
 304         /* Allow the Fman (Tx) port to process in-flight frames before we
 305          * try switching it off.
 306          */
 307         msleep(200);
 308
 309         phylink_stop(mac_dev->phylink);
 310         mac_dev->disable(mac_dev->fman_mac);
 311
 312         for (i = 0; i < ARRAY_SIZE(mac_dev->port); i++) {
 313                 error = fman_port_disable(mac_dev->port[i]);
 314                 if (error)
 315                         err = error;
 316         }
 317
 318         phylink_disconnect_phy(mac_dev->phylink);
 319         net_dev->phydev = NULL;
 320
 321         msleep(200);
 322
 323         return err;
 324 }
 325
 326 static void dpaa_tx_timeout(struct net_device *net_dev, unsigned int txqueue)
 327 {
 328         struct dpaa_percpu_priv *percpu_priv;
 329         const struct dpaa_priv  *priv;
 330
 331         priv = netdev_priv(net_dev);
 332         percpu_priv = this_cpu_ptr(priv->percpu_priv);
 333
 334         netif_crit(priv, timer, net_dev, "Transmit timeout latency: %u ms\n",
 335                    jiffies_to_msecs(jiffies - dev_trans_start(net_dev)));
 336
 337         percpu_priv->stats.tx_errors++;
 338 }
 339
 340 /* Calculates the statistics for the given device by adding the statistics
 341  * collected by each CPU.
 342  */
 343 static void dpaa_get_stats64(struct net_device *net_dev,
 344                              struct rtnl_link_stats64 *s)
 345 {
 346         int numstats = sizeof(struct rtnl_link_stats64) / sizeof(u64);
 347         struct dpaa_priv *priv = netdev_priv(net_dev);
 348         struct dpaa_percpu_priv *percpu_priv;
 349         u64 *netstats = (u64 *)s;
 350         u64 *cpustats;
 351         int i, j;
 352
 353         for_each_possible_cpu(i) {
 354                 percpu_priv = per_cpu_ptr(priv->percpu_priv, i);
 355
 356                 cpustats = (u64 *)&percpu_priv->stats;
 357
 358                 /* add stats from all CPUs */
 359                 for (j = 0; j < numstats; j++)
 360                         netstats[j] += cpustats[j];
 361         }
 362 }
 363
 364 static int dpaa_setup_tc(struct net_device *net_dev, enum tc_setup_type type,
 365                          void *type_data)
 366 {
 367         struct dpaa_priv *priv = netdev_priv(net_dev);
 368         struct tc_mqprio_qopt *mqprio = type_data;
 369         u8 num_tc;
 370         int i;
 371
 372         if (type != TC_SETUP_QDISC_MQPRIO)
 373                 return -EOPNOTSUPP;
 374
 375         mqprio->hw = TC_MQPRIO_HW_OFFLOAD_TCS;
 376         num_tc = mqprio->num_tc;
 377
 378         if (num_tc == priv->num_tc)
 379                 return 0;
 380
 381         if (!num_tc) {
 382                 netdev_reset_tc(net_dev);
 383                 goto out;
 384         }
 385
 386         if (num_tc > DPAA_TC_NUM) {
 387                 netdev_err(net_dev, "Too many traffic classes: max %d supported.\n",
 388                            DPAA_TC_NUM);
 389                 return -EINVAL;
 390         }
 391
 392         netdev_set_num_tc(net_dev, num_tc);
 393
 394         for (i = 0; i < num_tc; i++)
 395                 netdev_set_tc_queue(net_dev, i, DPAA_TC_TXQ_NUM,
 396                                     i * DPAA_TC_TXQ_NUM);
 397
 398 out:
 399         priv->num_tc = num_tc ? : 1;
 400         netif_set_real_num_tx_queues(net_dev, priv->num_tc * DPAA_TC_TXQ_NUM);
 401         return 0;
 402 }
 403
 404 static struct mac_device *dpaa_mac_dev_get(struct platform_device *pdev)
 405 {
 406         struct dpaa_eth_data *eth_data;
 407         struct device *dpaa_dev;
 408         struct mac_device *mac_dev;
 409
 410         dpaa_dev = &pdev->dev;
 411         eth_data = dpaa_dev->platform_data;
 412         if (!eth_data) {
 413                 dev_err(dpaa_dev, "eth_data missing\n");
 414                 return ERR_PTR(-ENODEV);
 415         }
 416         mac_dev = eth_data->mac_dev;
 417         if (!mac_dev) {
 418                 dev_err(dpaa_dev, "mac_dev missing\n");
 419                 return ERR_PTR(-EINVAL);
 420         }
 421
 422         return mac_dev;
 423 }
 424
 425 static int dpaa_set_mac_address(struct net_device *net_dev, void *addr)
 426 {
 427         const struct dpaa_priv *priv;
 428         struct mac_device *mac_dev;
 429         struct sockaddr old_addr;
 430         int err;
 431
 432         priv = netdev_priv(net_dev);
 433
 434         memcpy(old_addr.sa_data, net_dev->dev_addr,  ETH_ALEN);
 435
 436         err = eth_mac_addr(net_dev, addr);
 437         if (err < 0) {
 438                 netif_err(priv, drv, net_dev, "eth_mac_addr() = %d\n", err);
 439                 return err;
 440         }
 441
 442         mac_dev = priv->mac_dev;
 443
 444         err = mac_dev->change_addr(mac_dev->fman_mac,
 445                                    (const enet_addr_t *)net_dev->dev_addr);
 446         if (err < 0) {
 447                 netif_err(priv, drv, net_dev, "mac_dev->change_addr() = %d\n",
 448                           err);
 449                 /* reverting to previous address */
 450                 eth_mac_addr(net_dev, &old_addr);
 451
 452                 return err;
 453         }
 454
 455         return 0;
 456 }
 457
 458 static void dpaa_set_rx_mode(struct net_device *net_dev)
 459 {
 460         const struct dpaa_priv  *priv;
 461         int err;
 462
 463         priv = netdev_priv(net_dev);
 464
 465         if (!!(net_dev->flags & IFF_PROMISC) != priv->mac_dev->promisc) {
 466                 priv->mac_dev->promisc = !priv->mac_dev->promisc;
 467                 err = priv->mac_dev->set_promisc(priv->mac_dev->fman_mac,
 468                                                  priv->mac_dev->promisc);
 469                 if (err < 0)
 470                         netif_err(priv, drv, net_dev,
 471                                   "mac_dev->set_promisc() = %d\n",
 472                                   err);
 473         }
 474
 475         if (!!(net_dev->flags & IFF_ALLMULTI) != priv->mac_dev->allmulti) {
 476                 priv->mac_dev->allmulti = !priv->mac_dev->allmulti;
 477                 err = priv->mac_dev->set_allmulti(priv->mac_dev->fman_mac,
 478                                                   priv->mac_dev->allmulti);
 479                 if (err < 0)
 480                         netif_err(priv, drv, net_dev,
 481                                   "mac_dev->set_allmulti() = %d\n",
 482                                   err);
 483         }
 484
 485         err = priv->mac_dev->set_multi(net_dev, priv->mac_dev);
 486         if (err < 0)
 487                 netif_err(priv, drv, net_dev, "mac_dev->set_multi() = %d\n",
 488                           err);
 489 }
 490
 491 static struct dpaa_bp *dpaa_bpid2pool(int bpid)
 492 {
 493         if (WARN_ON(bpid < 0 || bpid >= BM_MAX_NUM_OF_POOLS))
 494                 return NULL;
 495
 496         return dpaa_bp_array[bpid];
 497 }
 498
 499 /* checks if this bpool is already allocated */
 500 static bool dpaa_bpid2pool_use(int bpid)
 501 {
 502         if (dpaa_bpid2pool(bpid)) {
 503                 refcount_inc(&dpaa_bp_array[bpid]->refs);
 504                 return true;
 505         }
 506
 507         return false;
 508 }
 509
 510 /* called only once per bpid by dpaa_bp_alloc_pool() */
 511 static void dpaa_bpid2pool_map(int bpid, struct dpaa_bp *dpaa_bp)
 512 {
 513         dpaa_bp_array[bpid] = dpaa_bp;
 514         refcount_set(&dpaa_bp->refs, 1);
 515 }
 516
 517 static int dpaa_bp_alloc_pool(struct dpaa_bp *dpaa_bp)
 518 {
 519         int err;
 520
 521         if (dpaa_bp->size == 0 || dpaa_bp->config_count == 0) {
 522                 pr_err("%s: Buffer pool is not properly initialized! Missing size or initial number of buffers\n",
 523                        __func__);
 524                 return -EINVAL;
 525         }
 526
 527         /* If the pool is already specified, we only create one per bpid */
 528         if (dpaa_bp->bpid != FSL_DPAA_BPID_INV &&
 529             dpaa_bpid2pool_use(dpaa_bp->bpid))
 530                 return 0;
 531
 532         if (dpaa_bp->bpid == FSL_DPAA_BPID_INV) {
 533                 dpaa_bp->pool = bman_new_pool();
 534                 if (!dpaa_bp->pool) {
 535                         pr_err("%s: bman_new_pool() failed\n",
 536                                __func__);
 537                         return -ENODEV;
 538                 }
 539
 540                 dpaa_bp->bpid = (u8)bman_get_bpid(dpaa_bp->pool);
 541         }
 542
 543         if (dpaa_bp->seed_cb) {
 544                 err = dpaa_bp->seed_cb(dpaa_bp);
 545                 if (err)
 546                         goto pool_seed_failed;
 547         }
 548
 549         dpaa_bpid2pool_map(dpaa_bp->bpid, dpaa_bp);
 550
 551         return 0;
 552
 553 pool_seed_failed:
 554         pr_err("%s: pool seeding failed\n", __func__);
 555         bman_free_pool(dpaa_bp->pool);
 556
 557         return err;
 558 }
 559
 560 /* remove and free all the buffers from the given buffer pool */
 561 static void dpaa_bp_drain(struct dpaa_bp *bp)
 562 {
 563         u8 num = 8;
 564         int ret;
 565
 566         do {
 567                 struct bm_buffer bmb[8];
 568                 int i;
 569
 570                 ret = bman_acquire(bp->pool, bmb, num);
 571                 if (ret < 0) {
 572                         if (num == 8) {
 573                                 /* we have less than 8 buffers left;
 574                                  * drain them one by one
 575                                  */
 576                                 num = 1;
 577                                 ret = 1;
 578                                 continue;
 579                         } else {
 580                                 /* Pool is fully drained */
 581                                 break;
 582                         }
 583                 }
 584
 585                 if (bp->free_buf_cb)
 586                         for (i = 0; i < num; i++)
 587                                 bp->free_buf_cb(bp, &bmb[i]);
 588         } while (ret > 0);
 589 }
 590
 591 static void dpaa_bp_free(struct dpaa_bp *dpaa_bp)
 592 {
 593         struct dpaa_bp *bp = dpaa_bpid2pool(dpaa_bp->bpid);
 594
 595         /* the mapping between bpid and dpaa_bp is done very late in the
 596          * allocation procedure; if something failed before the mapping, the bp
 597          * was not configured, therefore we don't need the below instructions
 598          */
 599         if (!bp)
 600                 return;
 601
 602         if (!refcount_dec_and_test(&bp->refs))
 603                 return;
 604
 605         if (bp->free_buf_cb)
 606                 dpaa_bp_drain(bp);
 607
 608         dpaa_bp_array[bp->bpid] = NULL;
 609         bman_free_pool(bp->pool);
 610 }
 611
 612 static void dpaa_bps_free(struct dpaa_priv *priv)
 613 {
 614         dpaa_bp_free(priv->dpaa_bp);
 615 }
 616
 617 /* Use multiple WQs for FQ assignment:
 618  *      - Tx Confirmation queues go to WQ1.
 619  *      - Rx Error and Tx Error queues go to WQ5 (giving them a better chance
 620  *        to be scheduled, in case there are many more FQs in WQ6).
 621  *      - Rx Default goes to WQ6.
 622  *      - Tx queues go to different WQs depending on their priority. Equal
 623  *        chunks of NR_CPUS queues go to WQ6 (lowest priority), WQ2, WQ1 and
 624  *        WQ0 (highest priority).
 625  * This ensures that Tx-confirmed buffers are timely released. In particular,
 626  * it avoids congestion on the Tx Confirm FQs, which can pile up PFDRs if they
 627  * are greatly outnumbered by other FQs in the system, while
 628  * dequeue scheduling is round-robin.
 629  */
 630 static inline void dpaa_assign_wq(struct dpaa_fq *fq, int idx)
 631 {
 632         switch (fq->fq_type) {
 633         case FQ_TYPE_TX_CONFIRM:
 634         case FQ_TYPE_TX_CONF_MQ:
 635                 fq->wq = 1;
 636                 break;
 637         case FQ_TYPE_RX_ERROR:
 638         case FQ_TYPE_TX_ERROR:
 639                 fq->wq = 5;
 640                 break;
 641         case FQ_TYPE_RX_DEFAULT:
 642         case FQ_TYPE_RX_PCD:
 643                 fq->wq = 6;
 644                 break;
 645         case FQ_TYPE_TX:
 646                 switch (idx / DPAA_TC_TXQ_NUM) {
 647                 case 0:
 648                         /* Low priority (best effort) */
 649                         fq->wq = 6;
 650                         break;
 651                 case 1:
 652                         /* Medium priority */
 653                         fq->wq = 2;
 654                         break;
 655                 case 2:
 656                         /* High priority */
 657                         fq->wq = 1;
 658                         break;
 659                 case 3:
 660                         /* Very high priority */
 661                         fq->wq = 0;
 662                         break;
 663                 default:
 664                         WARN(1, "Too many TX FQs: more than %d!\n",
 665                              DPAA_ETH_TXQ_NUM);
 666                 }
 667                 break;
 668         default:
 669                 WARN(1, "Invalid FQ type %d for FQID %d!\n",
 670                      fq->fq_type, fq->fqid);
 671         }
 672 }
 673
 674 static struct dpaa_fq *dpaa_fq_alloc(struct device *dev,
 675                                      u32 start, u32 count,
 676                                      struct list_head *list,
 677                                      enum dpaa_fq_type fq_type)
 678 {
 679         struct dpaa_fq *dpaa_fq;
 680         int i;
 681
 682         dpaa_fq = devm_kcalloc(dev, count, sizeof(*dpaa_fq),
 683                                GFP_KERNEL);
 684         if (!dpaa_fq)
 685                 return NULL;
 686
 687         for (i = 0; i < count; i++) {
 688                 dpaa_fq[i].fq_type = fq_type;
 689                 dpaa_fq[i].fqid = start ? start + i : 0;
 690                 list_add_tail(&dpaa_fq[i].list, list);
 691         }
 692
 693         for (i = 0; i < count; i++)
 694                 dpaa_assign_wq(dpaa_fq + i, i);
 695
 696         return dpaa_fq;
 697 }
 698
 699 static int dpaa_alloc_all_fqs(struct device *dev, struct list_head *list,
 700                               struct fm_port_fqs *port_fqs)
 701 {
 702         struct dpaa_fq *dpaa_fq;
 703         u32 fq_base, fq_base_aligned, i;
 704
 705         dpaa_fq = dpaa_fq_alloc(dev, 0, 1, list, FQ_TYPE_RX_ERROR);
 706         if (!dpaa_fq)
 707                 goto fq_alloc_failed;
 708
 709         port_fqs->rx_errq = &dpaa_fq[0];
 710
 711         dpaa_fq = dpaa_fq_alloc(dev, 0, 1, list, FQ_TYPE_RX_DEFAULT);
 712         if (!dpaa_fq)
 713                 goto fq_alloc_failed;
 714
 715         port_fqs->rx_defq = &dpaa_fq[0];
 716
 717         /* the PCD FQIDs range needs to be aligned for correct operation */
 718         if (qman_alloc_fqid_range(&fq_base, 2 * DPAA_ETH_PCD_RXQ_NUM))
 719                 goto fq_alloc_failed;
 720
 721         fq_base_aligned = ALIGN(fq_base, DPAA_ETH_PCD_RXQ_NUM);
 722
 723         for (i = fq_base; i < fq_base_aligned; i++)
 724                 qman_release_fqid(i);
 725
 726         for (i = fq_base_aligned + DPAA_ETH_PCD_RXQ_NUM;
 727              i < (fq_base + 2 * DPAA_ETH_PCD_RXQ_NUM); i++)
 728                 qman_release_fqid(i);
 729
 730         dpaa_fq = dpaa_fq_alloc(dev, fq_base_aligned, DPAA_ETH_PCD_RXQ_NUM,
 731                                 list, FQ_TYPE_RX_PCD);
 732         if (!dpaa_fq)
 733                 goto fq_alloc_failed;
 734
 735         port_fqs->rx_pcdq = &dpaa_fq[0];
 736
 737         if (!dpaa_fq_alloc(dev, 0, DPAA_ETH_TXQ_NUM, list, FQ_TYPE_TX_CONF_MQ))
 738                 goto fq_alloc_failed;
 739
 740         dpaa_fq = dpaa_fq_alloc(dev, 0, 1, list, FQ_TYPE_TX_ERROR);
 741         if (!dpaa_fq)
 742                 goto fq_alloc_failed;
 743
 744         port_fqs->tx_errq = &dpaa_fq[0];
 745
 746         dpaa_fq = dpaa_fq_alloc(dev, 0, 1, list, FQ_TYPE_TX_CONFIRM);
 747         if (!dpaa_fq)
 748                 goto fq_alloc_failed;
 749
 750         port_fqs->tx_defq = &dpaa_fq[0];
 751
 752         if (!dpaa_fq_alloc(dev, 0, DPAA_ETH_TXQ_NUM, list, FQ_TYPE_TX))
 753                 goto fq_alloc_failed;
 754
 755         return 0;
 756
 757 fq_alloc_failed:
 758         dev_err(dev, "dpaa_fq_alloc() failed\n");
 759         return -ENOMEM;
 760 }
 761
 762 static u32 rx_pool_channel;
 763 static DEFINE_SPINLOCK(rx_pool_channel_init);
 764
 765 static int dpaa_get_channel(void)
 766 {
 767         spin_lock(&rx_pool_channel_init);
 768         if (!rx_pool_channel) {
 769                 u32 pool;
 770                 int ret;
 771
 772                 ret = qman_alloc_pool(&pool);
 773
 774                 if (!ret)
 775                         rx_pool_channel = pool;
 776         }
 777         spin_unlock(&rx_pool_channel_init);
 778         if (!rx_pool_channel)
 779                 return -ENOMEM;
 780         return rx_pool_channel;
 781 }
 782
 783 static void dpaa_release_channel(void)
 784 {
 785         qman_release_pool(rx_pool_channel);
 786 }
 787
 788 static void dpaa_eth_add_channel(u16 channel, struct device *dev)
 789 {
 790         u32 pool = QM_SDQCR_CHANNELS_POOL_CONV(channel);
 791         const cpumask_t *cpus = qman_affine_cpus();
 792         struct qman_portal *portal;
 793         int cpu;
 794
 795         for_each_cpu_and(cpu, cpus, cpu_online_mask) {
 796                 portal = qman_get_affine_portal(cpu);
 797                 qman_p_static_dequeue_add(portal, pool);
 798                 qman_start_using_portal(portal, dev);
 799         }
 800 }
 801
 802 /* Congestion group state change notification callback.
 803  * Stops the device's egress queues while they are congested and
 804  * wakes them upon exiting congested state.
 805  * Also updates some CGR-related stats.
 806  */
 807 static void dpaa_eth_cgscn(struct qman_portal *qm, struct qman_cgr *cgr,
 808                            int congested)
 809 {
 810         struct dpaa_priv *priv = (struct dpaa_priv *)container_of(cgr,
 811                 struct dpaa_priv, cgr_data.cgr);
 812
 813         if (congested) {
 814                 priv->cgr_data.congestion_start_jiffies = jiffies;
 815                 netif_tx_stop_all_queues(priv->net_dev);
 816                 priv->cgr_data.cgr_congested_count++;
 817         } else {
 818                 priv->cgr_data.congested_jiffies +=
 819                         (jiffies - priv->cgr_data.congestion_start_jiffies);
 820                 netif_tx_wake_all_queues(priv->net_dev);
 821         }
 822 }
 823
 824 static int dpaa_eth_cgr_init(struct dpaa_priv *priv)
 825 {
 826         struct qm_mcc_initcgr initcgr;
 827         u32 cs_th;
 828         int err;
 829
 830         err = qman_alloc_cgrid(&priv->cgr_data.cgr.cgrid);
 831         if (err < 0) {
 832                 if (netif_msg_drv(priv))
 833                         pr_err("%s: Error %d allocating CGR ID\n",
 834                                __func__, err);
 835                 goto out_error;
 836         }
 837         priv->cgr_data.cgr.cb = dpaa_eth_cgscn;
 838
 839         /* Enable Congestion State Change Notifications and CS taildrop */
 840         memset(&initcgr, 0, sizeof(initcgr));
 841         initcgr.we_mask = cpu_to_be16(QM_CGR_WE_CSCN_EN | QM_CGR_WE_CS_THRES);
 842         initcgr.cgr.cscn_en = QM_CGR_EN;
 843
 844         /* Set different thresholds based on the configured MAC speed.
 845          * This may turn suboptimal if the MAC is reconfigured at another
 846          * speed, so MACs must call dpaa_eth_cgr_set_speed in their link_up
 847          * callback.
 848          */
 849         if (priv->mac_dev->phylink_config.mac_capabilities & MAC_10000FD)
 850                 cs_th = DPAA_CS_THRESHOLD_10G;
 851         else
 852                 cs_th = DPAA_CS_THRESHOLD_1G;
 853         qm_cgr_cs_thres_set64(&initcgr.cgr.cs_thres, cs_th, 1);
 854
 855         initcgr.we_mask |= cpu_to_be16(QM_CGR_WE_CSTD_EN);
 856         initcgr.cgr.cstd_en = QM_CGR_EN;
 857
 858         err = qman_create_cgr(&priv->cgr_data.cgr, QMAN_CGR_FLAG_USE_INIT,
 859                               &initcgr);
 860         if (err < 0) {
 861                 if (netif_msg_drv(priv))
 862                         pr_err("%s: Error %d creating CGR with ID %d\n",
 863                                __func__, err, priv->cgr_data.cgr.cgrid);
 864                 qman_release_cgrid(priv->cgr_data.cgr.cgrid);
 865                 goto out_error;
 866         }
 867         if (netif_msg_drv(priv))
 868                 pr_debug("Created CGR %d for netdev with hwaddr %pM on QMan channel %d\n",
 869                          priv->cgr_data.cgr.cgrid, priv->mac_dev->addr,
 870                          priv->cgr_data.cgr.chan);
 871
 872 out_error:
 873         return err;
 874 }
 875
 876 static void dpaa_eth_cgr_set_speed(struct mac_device *mac_dev, int speed)
 877 {
 878         struct net_device *net_dev = to_net_dev(mac_dev->phylink_config.dev);
 879         struct dpaa_priv *priv = netdev_priv(net_dev);
 880         struct qm_mcc_initcgr opts = { };
 881         u32 cs_th;
 882         int err;
 883
 884         opts.we_mask = cpu_to_be16(QM_CGR_WE_CS_THRES);
 885         switch (speed) {
 886         case SPEED_10000:
 887                 cs_th = DPAA_CS_THRESHOLD_10G;
 888                 break;
 889         case SPEED_1000:
 890         default:
 891                 cs_th = DPAA_CS_THRESHOLD_1G;
 892                 break;
 893         }
 894         qm_cgr_cs_thres_set64(&opts.cgr.cs_thres, cs_th, 1);
 895
 896         err = qman_update_cgr_safe(&priv->cgr_data.cgr, &opts);
 897         if (err)
 898                 netdev_err(net_dev, "could not update speed: %d\n", err);
 899 }
 900
 901 static inline void dpaa_setup_ingress(const struct dpaa_priv *priv,
 902                                       struct dpaa_fq *fq,
 903                                       const struct qman_fq *template)
 904 {
 905         fq->fq_base = *template;
 906         fq->net_dev = priv->net_dev;
 907
 908         fq->flags = QMAN_FQ_FLAG_NO_ENQUEUE;
 909         fq->channel = priv->channel;
 910 }
 911
 912 static inline void dpaa_setup_egress(const struct dpaa_priv *priv,
 913                                      struct dpaa_fq *fq,
 914                                      struct fman_port *port,
 915                                      const struct qman_fq *template)
 916 {
 917         fq->fq_base = *template;
 918         fq->net_dev = priv->net_dev;
 919
 920         if (port) {
 921                 fq->flags = QMAN_FQ_FLAG_TO_DCPORTAL;
 922                 fq->channel = (u16)fman_port_get_qman_channel_id(port);
 923         } else {
 924                 fq->flags = QMAN_FQ_FLAG_NO_MODIFY;
 925         }
 926 }
 927
 928 static void dpaa_fq_setup(struct dpaa_priv *priv,
 929                           const struct dpaa_fq_cbs *fq_cbs,
 930                           struct fman_port *tx_port)
 931 {
 932         int egress_cnt = 0, conf_cnt = 0, num_portals = 0, portal_cnt = 0, cpu;
 933         const cpumask_t *affine_cpus = qman_affine_cpus();
 934         u16 channels[NR_CPUS];
 935         struct dpaa_fq *fq;
 936
 937         for_each_cpu_and(cpu, affine_cpus, cpu_online_mask)
 938                 channels[num_portals++] = qman_affine_channel(cpu);
 939
 940         if (num_portals == 0)
 941                 dev_err(priv->net_dev->dev.parent,
 942                         "No Qman software (affine) channels found\n");
 943
 944         /* Initialize each FQ in the list */
 945         list_for_each_entry(fq, &priv->dpaa_fq_list, list) {
 946                 switch (fq->fq_type) {
 947                 case FQ_TYPE_RX_DEFAULT:
 948                         dpaa_setup_ingress(priv, fq, &fq_cbs->rx_defq);
 949                         break;
 950                 case FQ_TYPE_RX_ERROR:
 951                         dpaa_setup_ingress(priv, fq, &fq_cbs->rx_errq);
 952                         break;
 953                 case FQ_TYPE_RX_PCD:
 954                         if (!num_portals)
 955                                 continue;
 956                         dpaa_setup_ingress(priv, fq, &fq_cbs->rx_defq);
 957                         fq->channel = channels[portal_cnt++ % num_portals];
 958                         break;
 959                 case FQ_TYPE_TX:
 960                         dpaa_setup_egress(priv, fq, tx_port,
 961                                           &fq_cbs->egress_ern);
 962                         /* If we have more Tx queues than the number of cores,
 963                          * just ignore the extra ones.
 964                          */
 965                         if (egress_cnt < DPAA_ETH_TXQ_NUM)
 966                                 priv->egress_fqs[egress_cnt++] = &fq->fq_base;
 967                         break;
 968                 case FQ_TYPE_TX_CONF_MQ:
 969                         priv->conf_fqs[conf_cnt++] = &fq->fq_base;
 970                         fallthrough;
 971                 case FQ_TYPE_TX_CONFIRM:
 972                         dpaa_setup_ingress(priv, fq, &fq_cbs->tx_defq);
 973                         break;
 974                 case FQ_TYPE_TX_ERROR:
 975                         dpaa_setup_ingress(priv, fq, &fq_cbs->tx_errq);
 976                         break;
 977                 default:
 978                         dev_warn(priv->net_dev->dev.parent,
 979                                  "Unknown FQ type detected!\n");
 980                         break;
 981                 }
 982         }
 983
 984          /* Make sure all CPUs receive a corresponding Tx queue. */
 985         while (egress_cnt < DPAA_ETH_TXQ_NUM) {
 986                 list_for_each_entry(fq, &priv->dpaa_fq_list, list) {
 987                         if (fq->fq_type != FQ_TYPE_TX)
 988                                 continue;
 989                         priv->egress_fqs[egress_cnt++] = &fq->fq_base;
 990                         if (egress_cnt == DPAA_ETH_TXQ_NUM)
 991                                 break;
 992                 }
 993         }
 994 }
 995
 996 static inline int dpaa_tx_fq_to_id(const struct dpaa_priv *priv,
 997                                    struct qman_fq *tx_fq)
 998 {
 999         int i;
1000
1001         for (i = 0; i < DPAA_ETH_TXQ_NUM; i++)
1002                 if (priv->egress_fqs[i] == tx_fq)
1003                         return i;
1004
1005         return -EINVAL;
1006 }
1007
1008 static int dpaa_fq_init(struct dpaa_fq *dpaa_fq, bool td_enable)
1009 {
1010         const struct dpaa_priv  *priv;
1011         struct qman_fq *confq = NULL;
1012         struct qm_mcc_initfq initfq;
1013         struct device *dev;
1014         struct qman_fq *fq;
1015         int queue_id;
1016         int err;
1017
1018         priv = netdev_priv(dpaa_fq->net_dev);
1019         dev = dpaa_fq->net_dev->dev.parent;
1020
1021         if (dpaa_fq->fqid == 0)
1022                 dpaa_fq->flags |= QMAN_FQ_FLAG_DYNAMIC_FQID;
1023
1024         dpaa_fq->init = !(dpaa_fq->flags & QMAN_FQ_FLAG_NO_MODIFY);
1025
1026         err = qman_create_fq(dpaa_fq->fqid, dpaa_fq->flags, &dpaa_fq->fq_base);
1027         if (err) {
1028                 dev_err(dev, "qman_create_fq() failed\n");
1029                 return err;
1030         }
1031         fq = &dpaa_fq->fq_base;
1032
1033         if (dpaa_fq->init) {
1034                 memset(&initfq, 0, sizeof(initfq));
1035
1036                 initfq.we_mask = cpu_to_be16(QM_INITFQ_WE_FQCTRL);
1037                 /* Note: we may get to keep an empty FQ in cache */
1038                 initfq.fqd.fq_ctrl = cpu_to_be16(QM_FQCTRL_PREFERINCACHE);
1039
1040                 /* Try to reduce the number of portal interrupts for
1041                  * Tx Confirmation FQs.
1042                  */
1043                 if (dpaa_fq->fq_type == FQ_TYPE_TX_CONFIRM)
1044                         initfq.fqd.fq_ctrl |= cpu_to_be16(QM_FQCTRL_AVOIDBLOCK);
1045
1046                 /* FQ placement */
1047                 initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_DESTWQ);
1048
1049                 qm_fqd_set_destwq(&initfq.fqd, dpaa_fq->channel, dpaa_fq->wq);
1050
1051                 /* Put all egress queues in a congestion group of their own.
1052                  * Sensu stricto, the Tx confirmation queues are Rx FQs,
1053                  * rather than Tx - but they nonetheless account for the
1054                  * memory footprint on behalf of egress traffic. We therefore
1055                  * place them in the netdev's CGR, along with the Tx FQs.
1056                  */
1057                 if (dpaa_fq->fq_type == FQ_TYPE_TX ||
1058                     dpaa_fq->fq_type == FQ_TYPE_TX_CONFIRM ||
1059                     dpaa_fq->fq_type == FQ_TYPE_TX_CONF_MQ) {
1060                         initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_CGID);
1061                         initfq.fqd.fq_ctrl |= cpu_to_be16(QM_FQCTRL_CGE);
1062                         initfq.fqd.cgid = (u8)priv->cgr_data.cgr.cgrid;
1063                         /* Set a fixed overhead accounting, in an attempt to
1064                          * reduce the impact of fixed-size skb shells and the
1065                          * driver's needed headroom on system memory. This is
1066                          * especially the case when the egress traffic is
1067                          * composed of small datagrams.
1068                          * Unfortunately, QMan's OAL value is capped to an
1069                          * insufficient value, but even that is better than
1070                          * no overhead accounting at all.
1071                          */
1072                         initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_OAC);
1073                         qm_fqd_set_oac(&initfq.fqd, QM_OAC_CG);
1074                         qm_fqd_set_oal(&initfq.fqd,
1075                                        min(sizeof(struct sk_buff) +
1076                                        priv->tx_headroom,
1077                                        (size_t)FSL_QMAN_MAX_OAL));
1078                 }
1079
1080                 if (td_enable) {
1081                         initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_TDTHRESH);
1082                         qm_fqd_set_taildrop(&initfq.fqd, DPAA_FQ_TD, 1);
1083                         initfq.fqd.fq_ctrl = cpu_to_be16(QM_FQCTRL_TDE);
1084                 }
1085
1086                 if (dpaa_fq->fq_type == FQ_TYPE_TX) {
1087                         queue_id = dpaa_tx_fq_to_id(priv, &dpaa_fq->fq_base);
1088                         if (queue_id >= 0)
1089                                 confq = priv->conf_fqs[queue_id];
1090                         if (confq) {
1091                                 initfq.we_mask |=
1092                                         cpu_to_be16(QM_INITFQ_WE_CONTEXTA);
1093                         /* ContextA: OVOM=1(use contextA2 bits instead of ICAD)
1094                          *           A2V=1 (contextA A2 field is valid)
1095                          *           A0V=1 (contextA A0 field is valid)
1096                          *           B0V=1 (contextB field is valid)
1097                          * ContextA A2: EBD=1 (deallocate buffers inside FMan)
1098                          * ContextB B0(ASPID): 0 (absolute Virtual Storage ID)
1099                          */
1100                                 qm_fqd_context_a_set64(&initfq.fqd,
1101                                                        0x1e00000080000000ULL);
1102                         }
1103                 }
1104
1105                 /* Put all the ingress queues in our "ingress CGR". */
1106                 if (priv->use_ingress_cgr &&
1107                     (dpaa_fq->fq_type == FQ_TYPE_RX_DEFAULT ||
1108                      dpaa_fq->fq_type == FQ_TYPE_RX_ERROR ||
1109                      dpaa_fq->fq_type == FQ_TYPE_RX_PCD)) {
1110                         initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_CGID);
1111                         initfq.fqd.fq_ctrl |= cpu_to_be16(QM_FQCTRL_CGE);
1112                         initfq.fqd.cgid = (u8)priv->ingress_cgr.cgrid;
1113                         /* Set a fixed overhead accounting, just like for the
1114                          * egress CGR.
1115                          */
1116                         initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_OAC);
1117                         qm_fqd_set_oac(&initfq.fqd, QM_OAC_CG);
1118                         qm_fqd_set_oal(&initfq.fqd,
1119                                        min(sizeof(struct sk_buff) +
1120                                        priv->tx_headroom,
1121                                        (size_t)FSL_QMAN_MAX_OAL));
1122                 }
1123
1124                 /* Initialization common to all ingress queues */
1125                 if (dpaa_fq->flags & QMAN_FQ_FLAG_NO_ENQUEUE) {
1126                         initfq.we_mask |= cpu_to_be16(QM_INITFQ_WE_CONTEXTA);
1127                         initfq.fqd.fq_ctrl |= cpu_to_be16(QM_FQCTRL_HOLDACTIVE |
1128                                                 QM_FQCTRL_CTXASTASHING);
1129                         initfq.fqd.context_a.stashing.exclusive =
1130                                 QM_STASHING_EXCL_DATA | QM_STASHING_EXCL_CTX |
1131                                 QM_STASHING_EXCL_ANNOTATION;
1132                         qm_fqd_set_stashing(&initfq.fqd, 1, 2,
1133                                             DIV_ROUND_UP(sizeof(struct qman_fq),
1134                                                          64));
1135                 }
1136
1137                 err = qman_init_fq(fq, QMAN_INITFQ_FLAG_SCHED, &initfq);
1138                 if (err < 0) {
1139                         dev_err(dev, "qman_init_fq(%u) = %d\n",
1140                                 qman_fq_fqid(fq), err);
1141                         qman_destroy_fq(fq);
1142                         return err;
1143                 }
1144         }
1145
1146         dpaa_fq->fqid = qman_fq_fqid(fq);
1147
1148         if (dpaa_fq->fq_type == FQ_TYPE_RX_DEFAULT ||
1149             dpaa_fq->fq_type == FQ_TYPE_RX_PCD) {
1150                 err = xdp_rxq_info_reg(&dpaa_fq->xdp_rxq, dpaa_fq->net_dev,
1151                                        dpaa_fq->fqid, 0);
1152                 if (err) {
1153                         dev_err(dev, "xdp_rxq_info_reg() = %d\n", err);
1154                         return err;
1155                 }
1156
1157                 err = xdp_rxq_info_reg_mem_model(&dpaa_fq->xdp_rxq,
1158                                                  MEM_TYPE_PAGE_ORDER0, NULL);
1159                 if (err) {
1160                         dev_err(dev, "xdp_rxq_info_reg_mem_model() = %d\n",
1161                                 err);
1162                         xdp_rxq_info_unreg(&dpaa_fq->xdp_rxq);
1163                         return err;
1164                 }
1165         }
1166
1167         return 0;
1168 }
1169
1170 static int dpaa_fq_free_entry(struct device *dev, struct qman_fq *fq)
1171 {
1172         const struct dpaa_priv  *priv;
1173         struct dpaa_fq *dpaa_fq;
1174         int err, error;
1175
1176         err = 0;
1177
1178         dpaa_fq = container_of(fq, struct dpaa_fq, fq_base);
1179         priv = netdev_priv(dpaa_fq->net_dev);
1180
1181         if (dpaa_fq->init) {
1182                 err = qman_retire_fq(fq, NULL);
1183                 if (err < 0 && netif_msg_drv(priv))
1184                         dev_err(dev, "qman_retire_fq(%u) = %d\n",
1185                                 qman_fq_fqid(fq), err);
1186
1187                 error = qman_oos_fq(fq);
1188                 if (error < 0 && netif_msg_drv(priv)) {
1189                         dev_err(dev, "qman_oos_fq(%u) = %d\n",
1190                                 qman_fq_fqid(fq), error);
1191                         if (err >= 0)
1192                                 err = error;
1193                 }
1194         }
1195
1196         if ((dpaa_fq->fq_type == FQ_TYPE_RX_DEFAULT ||
1197              dpaa_fq->fq_type == FQ_TYPE_RX_PCD) &&
1198             xdp_rxq_info_is_reg(&dpaa_fq->xdp_rxq))
1199                 xdp_rxq_info_unreg(&dpaa_fq->xdp_rxq);
1200
1201         qman_destroy_fq(fq);
1202         list_del(&dpaa_fq->list);
1203
1204         return err;
1205 }
1206
1207 static int dpaa_fq_free(struct device *dev, struct list_head *list)
1208 {
1209         struct dpaa_fq *dpaa_fq, *tmp;
1210         int err, error;
1211
1212         err = 0;
1213         list_for_each_entry_safe(dpaa_fq, tmp, list, list) {
1214                 error = dpaa_fq_free_entry(dev, (struct qman_fq *)dpaa_fq);
1215                 if (error < 0 && err >= 0)
1216                         err = error;
1217         }
1218
1219         return err;
1220 }
1221
1222 static int dpaa_eth_init_tx_port(struct fman_port *port, struct dpaa_fq *errq,
1223                                  struct dpaa_fq *defq,
1224                                  struct dpaa_buffer_layout *buf_layout)
1225 {
1226         struct fman_buffer_prefix_content buf_prefix_content;
1227         struct fman_port_params params;
1228         int err;
1229
1230         memset(&params, 0, sizeof(params));
1231         memset(&buf_prefix_content, 0, sizeof(buf_prefix_content));
1232
1233         buf_prefix_content.priv_data_size = buf_layout->priv_data_size;
1234         buf_prefix_content.pass_prs_result = true;
1235         buf_prefix_content.pass_hash_result = true;
1236         buf_prefix_content.pass_time_stamp = true;
1237         buf_prefix_content.data_align = DPAA_FD_DATA_ALIGNMENT;
1238
1239         params.specific_params.non_rx_params.err_fqid = errq->fqid;
1240         params.specific_params.non_rx_params.dflt_fqid = defq->fqid;
1241
1242         err = fman_port_config(port, &params);
1243         if (err) {
1244                 pr_err("%s: fman_port_config failed\n", __func__);
1245                 return err;
1246         }
1247
1248         err = fman_port_cfg_buf_prefix_content(port, &buf_prefix_content);
1249         if (err) {
1250                 pr_err("%s: fman_port_cfg_buf_prefix_content failed\n",
1251                        __func__);
1252                 return err;
1253         }
1254
1255         err = fman_port_init(port);
1256         if (err)
1257                 pr_err("%s: fm_port_init failed\n", __func__);
1258
1259         return err;
1260 }
1261
1262 static int dpaa_eth_init_rx_port(struct fman_port *port, struct dpaa_bp *bp,
1263                                  struct dpaa_fq *errq,
1264                                  struct dpaa_fq *defq, struct dpaa_fq *pcdq,
1265                                  struct dpaa_buffer_layout *buf_layout)
1266 {
1267         struct fman_buffer_prefix_content buf_prefix_content;
1268         struct fman_port_rx_params *rx_p;
1269         struct fman_port_params params;
1270         int err;
1271
1272         memset(&params, 0, sizeof(params));
1273         memset(&buf_prefix_content, 0, sizeof(buf_prefix_content));
1274
1275         buf_prefix_content.priv_data_size = buf_layout->priv_data_size;
1276         buf_prefix_content.pass_prs_result = true;
1277         buf_prefix_content.pass_hash_result = true;
1278         buf_prefix_content.pass_time_stamp = true;
1279         buf_prefix_content.data_align = DPAA_FD_RX_DATA_ALIGNMENT;
1280
1281         rx_p = &params.specific_params.rx_params;
1282         rx_p->err_fqid = errq->fqid;
1283         rx_p->dflt_fqid = defq->fqid;
1284         if (pcdq) {
1285                 rx_p->pcd_base_fqid = pcdq->fqid;
1286                 rx_p->pcd_fqs_count = DPAA_ETH_PCD_RXQ_NUM;
1287         }
1288
1289         rx_p->ext_buf_pools.num_of_pools_used = 1;
1290         rx_p->ext_buf_pools.ext_buf_pool[0].id =  bp->bpid;
1291         rx_p->ext_buf_pools.ext_buf_pool[0].size = (u16)bp->size;
1292
1293         err = fman_port_config(port, &params);
1294         if (err) {
1295                 pr_err("%s: fman_port_config failed\n", __func__);
1296                 return err;
1297         }
1298
1299         err = fman_port_cfg_buf_prefix_content(port, &buf_prefix_content);
1300         if (err) {
1301                 pr_err("%s: fman_port_cfg_buf_prefix_content failed\n",
1302                        __func__);
1303                 return err;
1304         }
1305
1306         err = fman_port_init(port);
1307         if (err)
1308                 pr_err("%s: fm_port_init failed\n", __func__);
1309
1310         return err;
1311 }
1312
1313 static int dpaa_eth_init_ports(struct mac_device *mac_dev,
1314                                struct dpaa_bp *bp,
1315                                struct fm_port_fqs *port_fqs,
1316                                struct dpaa_buffer_layout *buf_layout,
1317                                struct device *dev)
1318 {
1319         struct fman_port *rxport = mac_dev->port[RX];
1320         struct fman_port *txport = mac_dev->port[TX];
1321         int err;
1322
1323         err = dpaa_eth_init_tx_port(txport, port_fqs->tx_errq,
1324                                     port_fqs->tx_defq, &buf_layout[TX]);
1325         if (err)
1326                 return err;
1327
1328         err = dpaa_eth_init_rx_port(rxport, bp, port_fqs->rx_errq,
1329                                     port_fqs->rx_defq, port_fqs->rx_pcdq,
1330                                     &buf_layout[RX]);
1331
1332         return err;
1333 }
1334
1335 static int dpaa_bman_release(const struct dpaa_bp *dpaa_bp,
1336                              struct bm_buffer *bmb, int cnt)
1337 {
1338         int err;
1339
1340         err = bman_release(dpaa_bp->pool, bmb, cnt);
1341         /* Should never occur, address anyway to avoid leaking the buffers */
1342         if (WARN_ON(err) && dpaa_bp->free_buf_cb)
1343                 while (cnt-- > 0)
1344                         dpaa_bp->free_buf_cb(dpaa_bp, &bmb[cnt]);
1345
1346         return cnt;
1347 }
1348
1349 static void dpaa_release_sgt_members(struct qm_sg_entry *sgt)
1350 {
1351         struct bm_buffer bmb[DPAA_BUFF_RELEASE_MAX];
1352         struct dpaa_bp *dpaa_bp;
1353         int i = 0, j;
1354
1355         memset(bmb, 0, sizeof(bmb));
1356
1357         do {
1358                 dpaa_bp = dpaa_bpid2pool(sgt[i].bpid);
1359                 if (!dpaa_bp)
1360                         return;
1361
1362                 j = 0;
1363                 do {
1364                         WARN_ON(qm_sg_entry_is_ext(&sgt[i]));
1365
1366                         bm_buffer_set64(&bmb[j], qm_sg_entry_get64(&sgt[i]));
1367
1368                         j++; i++;
1369                 } while (j < ARRAY_SIZE(bmb) &&
1370                                 !qm_sg_entry_is_final(&sgt[i - 1]) &&
1371                                 sgt[i - 1].bpid == sgt[i].bpid);
1372
1373                 dpaa_bman_release(dpaa_bp, bmb, j);
1374         } while (!qm_sg_entry_is_final(&sgt[i - 1]));
1375 }
1376
1377 static void dpaa_fd_release(const struct net_device *net_dev,
1378                             const struct qm_fd *fd)
1379 {
1380         struct qm_sg_entry *sgt;
1381         struct dpaa_bp *dpaa_bp;
1382         struct bm_buffer bmb;
1383         dma_addr_t addr;
1384         void *vaddr;
1385
1386         bmb.data = 0;
1387         bm_buffer_set64(&bmb, qm_fd_addr(fd));
1388
1389         dpaa_bp = dpaa_bpid2pool(fd->bpid);
1390         if (!dpaa_bp)
1391                 return;
1392
1393         if (qm_fd_get_format(fd) == qm_fd_sg) {
1394                 vaddr = phys_to_virt(qm_fd_addr(fd));
1395                 sgt = vaddr + qm_fd_get_offset(fd);
1396
1397                 dma_unmap_page(dpaa_bp->priv->rx_dma_dev, qm_fd_addr(fd),
1398                                DPAA_BP_RAW_SIZE, DMA_FROM_DEVICE);
1399
1400                 dpaa_release_sgt_members(sgt);
1401
1402                 addr = dma_map_page(dpaa_bp->priv->rx_dma_dev,
1403                                     virt_to_page(vaddr), 0, DPAA_BP_RAW_SIZE,
1404                                     DMA_FROM_DEVICE);
1405                 if (dma_mapping_error(dpaa_bp->priv->rx_dma_dev, addr)) {
1406                         netdev_err(net_dev, "DMA mapping failed\n");
1407                         return;
1408                 }
1409                 bm_buffer_set64(&bmb, addr);
1410         }
1411
1412         dpaa_bman_release(dpaa_bp, &bmb, 1);
1413 }
1414
1415 static void count_ern(struct dpaa_percpu_priv *percpu_priv,
1416                       const union qm_mr_entry *msg)
1417 {
1418         switch (msg->ern.rc & QM_MR_RC_MASK) {
1419         case QM_MR_RC_CGR_TAILDROP:
1420                 percpu_priv->ern_cnt.cg_tdrop++;
1421                 break;
1422         case QM_MR_RC_WRED:
1423                 percpu_priv->ern_cnt.wred++;
1424                 break;
1425         case QM_MR_RC_ERROR:
1426                 percpu_priv->ern_cnt.err_cond++;
1427                 break;
1428         case QM_MR_RC_ORPWINDOW_EARLY:
1429                 percpu_priv->ern_cnt.early_window++;
1430                 break;
1431         case QM_MR_RC_ORPWINDOW_LATE:
1432                 percpu_priv->ern_cnt.late_window++;
1433                 break;
1434         case QM_MR_RC_FQ_TAILDROP:
1435                 percpu_priv->ern_cnt.fq_tdrop++;
1436                 break;
1437         case QM_MR_RC_ORPWINDOW_RETIRED:
1438                 percpu_priv->ern_cnt.fq_retired++;
1439                 break;
1440         case QM_MR_RC_ORP_ZERO:
1441                 percpu_priv->ern_cnt.orp_zero++;
1442                 break;
1443         }
1444 }
1445
1446 /* Turn on HW checksum computation for this outgoing frame.
1447  * If the current protocol is not something we support in this regard
1448  * (or if the stack has already computed the SW checksum), we do nothing.
1449  *
1450  * Returns 0 if all goes well (or HW csum doesn't apply), and a negative value
1451  * otherwise.
1452  *
1453  * Note that this function may modify the fd->cmd field and the skb data buffer
1454  * (the Parse Results area).
1455  */
1456 static int dpaa_enable_tx_csum(struct dpaa_priv *priv,
1457                                struct sk_buff *skb,
1458                                struct qm_fd *fd,
1459                                void *parse_results)
1460 {
1461         struct fman_prs_result *parse_result;
1462         u16 ethertype = ntohs(skb->protocol);
1463         struct ipv6hdr *ipv6h = NULL;
1464         struct iphdr *iph;
1465         int retval = 0;
1466         u8 l4_proto;
1467
1468         if (skb->ip_summed != CHECKSUM_PARTIAL)
1469                 return 0;
1470
1471         /* Note: L3 csum seems to be already computed in sw, but we can't choose
1472          * L4 alone from the FM configuration anyway.
1473          */
1474
1475         /* Fill in some fields of the Parse Results array, so the FMan
1476          * can find them as if they came from the FMan Parser.
1477          */
1478         parse_result = (struct fman_prs_result *)parse_results;
1479
1480         /* If we're dealing with VLAN, get the real Ethernet type */
1481         if (ethertype == ETH_P_8021Q) {
1482                 /* We can't always assume the MAC header is set correctly
1483                  * by the stack, so reset to beginning of skb->data
1484                  */
1485                 skb_reset_mac_header(skb);
1486                 ethertype = ntohs(vlan_eth_hdr(skb)->h_vlan_encapsulated_proto);
1487         }
1488
1489         /* Fill in the relevant L3 parse result fields
1490          * and read the L4 protocol type
1491          */
1492         switch (ethertype) {
1493         case ETH_P_IP:
1494                 parse_result->l3r = cpu_to_be16(FM_L3_PARSE_RESULT_IPV4);
1495                 iph = ip_hdr(skb);
1496                 WARN_ON(!iph);
1497                 l4_proto = iph->protocol;
1498                 break;
1499         case ETH_P_IPV6:
1500                 parse_result->l3r = cpu_to_be16(FM_L3_PARSE_RESULT_IPV6);
1501                 ipv6h = ipv6_hdr(skb);
1502                 WARN_ON(!ipv6h);
1503                 l4_proto = ipv6h->nexthdr;
1504                 break;
1505         default:
1506                 /* We shouldn't even be here */
1507                 if (net_ratelimit())
1508                         netif_alert(priv, tx_err, priv->net_dev,
1509                                     "Can't compute HW csum for L3 proto 0x%x\n",
1510                                     ntohs(skb->protocol));
1511                 retval = -EIO;
1512                 goto return_error;
1513         }
1514
1515         /* Fill in the relevant L4 parse result fields */
1516         switch (l4_proto) {
1517         case IPPROTO_UDP:
1518                 parse_result->l4r = FM_L4_PARSE_RESULT_UDP;
1519                 break;
1520         case IPPROTO_TCP:
1521                 parse_result->l4r = FM_L4_PARSE_RESULT_TCP;
1522                 break;
1523         default:
1524                 if (net_ratelimit())
1525                         netif_alert(priv, tx_err, priv->net_dev,
1526                                     "Can't compute HW csum for L4 proto 0x%x\n",
1527                                     l4_proto);
1528                 retval = -EIO;
1529                 goto return_error;
1530         }
1531
1532         /* At index 0 is IPOffset_1 as defined in the Parse Results */
1533         parse_result->ip_off[0] = (u8)skb_network_offset(skb);
1534         parse_result->l4_off = (u8)skb_transport_offset(skb);
1535
1536         /* Enable L3 (and L4, if TCP or UDP) HW checksum. */
1537         fd->cmd |= cpu_to_be32(FM_FD_CMD_RPD | FM_FD_CMD_DTC);
1538
1539         /* On P1023 and similar platforms fd->cmd interpretation could
1540          * be disabled by setting CONTEXT_A bit ICMD; currently this bit
1541          * is not set so we do not need to check; in the future, if/when
1542          * using context_a we need to check this bit
1543          */
1544
1545 return_error:
1546         return retval;
1547 }
1548
1549 static int dpaa_bp_add_8_bufs(const struct dpaa_bp *dpaa_bp)
1550 {
1551         struct net_device *net_dev = dpaa_bp->priv->net_dev;
1552         struct bm_buffer bmb[8];
1553         dma_addr_t addr;
1554         struct page *p;
1555         u8 i;
1556
1557         for (i = 0; i < 8; i++) {
1558                 p = dev_alloc_pages(0);
1559                 if (unlikely(!p)) {
1560                         netdev_err(net_dev, "dev_alloc_pages() failed\n");
1561                         goto release_previous_buffs;
1562                 }
1563
1564                 addr = dma_map_page(dpaa_bp->priv->rx_dma_dev, p, 0,
1565                                     DPAA_BP_RAW_SIZE, DMA_FROM_DEVICE);
1566                 if (unlikely(dma_mapping_error(dpaa_bp->priv->rx_dma_dev,
1567                                                addr))) {
1568                         netdev_err(net_dev, "DMA map failed\n");
1569                         goto release_previous_buffs;
1570                 }
1571
1572                 bmb[i].data = 0;
1573                 bm_buffer_set64(&bmb[i], addr);
1574         }
1575
1576 release_bufs:
1577         return dpaa_bman_release(dpaa_bp, bmb, i);
1578
1579 release_previous_buffs:
1580         WARN_ONCE(1, "dpaa_eth: failed to add buffers on Rx\n");
1581
1582         bm_buffer_set64(&bmb[i], 0);
1583         /* Avoid releasing a completely null buffer; bman_release() requires
1584          * at least one buffer.
1585          */
1586         if (likely(i))
1587                 goto release_bufs;
1588
1589         return 0;
1590 }
1591
1592 static int dpaa_bp_seed(struct dpaa_bp *dpaa_bp)
1593 {
1594         int i;
1595
1596         /* Give each CPU an allotment of "config_count" buffers */
1597         for_each_possible_cpu(i) {
1598                 int *count_ptr = per_cpu_ptr(dpaa_bp->percpu_count, i);
1599                 int j;
1600
1601                 /* Although we access another CPU's counters here
1602                  * we do it at boot time so it is safe
1603                  */
1604                 for (j = 0; j < dpaa_bp->config_count; j += 8)
1605                         *count_ptr += dpaa_bp_add_8_bufs(dpaa_bp);
1606         }
1607         return 0;
1608 }
1609
1610 /* Add buffers/(pages) for Rx processing whenever bpool count falls below
1611  * REFILL_THRESHOLD.
1612  */
1613 static int dpaa_eth_refill_bpool(struct dpaa_bp *dpaa_bp, int *countptr)
1614 {
1615         int count = *countptr;
1616         int new_bufs;
1617
1618         if (unlikely(count < FSL_DPAA_ETH_REFILL_THRESHOLD)) {
1619                 do {
1620                         new_bufs = dpaa_bp_add_8_bufs(dpaa_bp);
1621                         if (unlikely(!new_bufs)) {
1622                                 /* Avoid looping forever if we've temporarily
1623                                  * run out of memory. We'll try again at the
1624                                  * next NAPI cycle.
1625                                  */
1626                                 break;
1627                         }
1628                         count += new_bufs;
1629                 } while (count < FSL_DPAA_ETH_MAX_BUF_COUNT);
1630
1631                 *countptr = count;
1632                 if (unlikely(count < FSL_DPAA_ETH_MAX_BUF_COUNT))
1633                         return -ENOMEM;
1634         }
1635
1636         return 0;
1637 }
1638
1639 static int dpaa_eth_refill_bpools(struct dpaa_priv *priv)
1640 {
1641         struct dpaa_bp *dpaa_bp;
1642         int *countptr;
1643
1644         dpaa_bp = priv->dpaa_bp;
1645         if (!dpaa_bp)
1646                 return -EINVAL;
1647         countptr = this_cpu_ptr(dpaa_bp->percpu_count);
1648
1649         return dpaa_eth_refill_bpool(dpaa_bp, countptr);
1650 }
1651
1652 /* Cleanup function for outgoing frame descriptors that were built on Tx path,
1653  * either contiguous frames or scatter/gather ones.
1654  * Skb freeing is not handled here.
1655  *
1656  * This function may be called on error paths in the Tx function, so guard
1657  * against cases when not all fd relevant fields were filled in. To avoid
1658  * reading the invalid transmission timestamp for the error paths set ts to
1659  * false.
1660  *
1661  * Return the skb backpointer, since for S/G frames the buffer containing it
1662  * gets freed here.
1663  *
1664  * No skb backpointer is set when transmitting XDP frames. Cleanup the buffer
1665  * and return NULL in this case.
1666  */
1667 static struct sk_buff *dpaa_cleanup_tx_fd(const struct dpaa_priv *priv,
1668                                           const struct qm_fd *fd, bool ts)
1669 {
1670         const enum dma_data_direction dma_dir = DMA_TO_DEVICE;
1671         struct device *dev = priv->net_dev->dev.parent;
1672         struct skb_shared_hwtstamps shhwtstamps;
1673         dma_addr_t addr = qm_fd_addr(fd);
1674         void *vaddr = phys_to_virt(addr);
1675         const struct qm_sg_entry *sgt;
1676         struct dpaa_eth_swbp *swbp;
1677         struct sk_buff *skb;
1678         u64 ns;
1679         int i;
1680
1681         if (unlikely(qm_fd_get_format(fd) == qm_fd_sg)) {
1682                 dma_unmap_page(priv->tx_dma_dev, addr,
1683                                qm_fd_get_offset(fd) + DPAA_SGT_SIZE,
1684                                dma_dir);
1685
1686                 /* The sgt buffer has been allocated with netdev_alloc_frag(),
1687                  * it's from lowmem.
1688                  */
1689                 sgt = vaddr + qm_fd_get_offset(fd);
1690
1691                 /* sgt[0] is from lowmem, was dma_map_single()-ed */
1692                 dma_unmap_single(priv->tx_dma_dev, qm_sg_addr(&sgt[0]),
1693                                  qm_sg_entry_get_len(&sgt[0]), dma_dir);
1694
1695                 /* remaining pages were mapped with skb_frag_dma_map() */
1696                 for (i = 1; (i < DPAA_SGT_MAX_ENTRIES) &&
1697                      !qm_sg_entry_is_final(&sgt[i - 1]); i++) {
1698                         WARN_ON(qm_sg_entry_is_ext(&sgt[i]));
1699
1700                         dma_unmap_page(priv->tx_dma_dev, qm_sg_addr(&sgt[i]),
1701                                        qm_sg_entry_get_len(&sgt[i]), dma_dir);
1702                 }
1703         } else {
1704                 dma_unmap_single(priv->tx_dma_dev, addr,
1705                                  qm_fd_get_offset(fd) + qm_fd_get_length(fd),
1706                                  dma_dir);
1707         }
1708
1709         swbp = (struct dpaa_eth_swbp *)vaddr;
1710         skb = swbp->skb;
1711
1712         /* No skb backpointer is set when running XDP. An xdp_frame
1713          * backpointer is saved instead.
1714          */
1715         if (!skb) {
1716                 xdp_return_frame(swbp->xdpf);
1717                 return NULL;
1718         }
1719
1720         /* DMA unmapping is required before accessing the HW provided info */
1721         if (ts && priv->tx_tstamp &&
1722             skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) {
1723                 memset(&shhwtstamps, 0, sizeof(shhwtstamps));
1724
1725                 if (!fman_port_get_tstamp(priv->mac_dev->port[TX], vaddr,
1726                                           &ns)) {
1727                         shhwtstamps.hwtstamp = ns_to_ktime(ns);
1728                         skb_tstamp_tx(skb, &shhwtstamps);
1729                 } else {
1730                         dev_warn(dev, "fman_port_get_tstamp failed!\n");
1731                 }
1732         }
1733
1734         if (qm_fd_get_format(fd) == qm_fd_sg)
1735                 /* Free the page that we allocated on Tx for the SGT */
1736                 free_pages((unsigned long)vaddr, 0);
1737
1738         return skb;
1739 }
1740
1741 static u8 rx_csum_offload(const struct dpaa_priv *priv, const struct qm_fd *fd)
1742 {
1743         /* The parser has run and performed L4 checksum validation.
1744          * We know there were no parser errors (and implicitly no
1745          * L4 csum error), otherwise we wouldn't be here.
1746          */
1747         if ((priv->net_dev->features & NETIF_F_RXCSUM) &&
1748             (be32_to_cpu(fd->status) & FM_FD_STAT_L4CV))
1749                 return CHECKSUM_UNNECESSARY;
1750
1751         /* We're here because either the parser didn't run or the L4 checksum
1752          * was not verified. This may include the case of a UDP frame with
1753          * checksum zero or an L4 proto other than TCP/UDP
1754          */
1755         return CHECKSUM_NONE;
1756 }
1757
1758 #define PTR_IS_ALIGNED(x, a) (IS_ALIGNED((unsigned long)(x), (a)))
1759
1760 /* Build a linear skb around the received buffer.
1761  * We are guaranteed there is enough room at the end of the data buffer to
1762  * accommodate the shared info area of the skb.
1763  */
1764 static struct sk_buff *contig_fd_to_skb(const struct dpaa_priv *priv,
1765                                         const struct qm_fd *fd)
1766 {
1767         ssize_t fd_off = qm_fd_get_offset(fd);
1768         dma_addr_t addr = qm_fd_addr(fd);
1769         struct dpaa_bp *dpaa_bp;
1770         struct sk_buff *skb;
1771         void *vaddr;
1772
1773         vaddr = phys_to_virt(addr);
1774         WARN_ON(!IS_ALIGNED((unsigned long)vaddr, SMP_CACHE_BYTES));
1775
1776         dpaa_bp = dpaa_bpid2pool(fd->bpid);
1777         if (!dpaa_bp)
1778                 goto free_buffer;
1779
1780         skb = build_skb(vaddr, dpaa_bp->size +
1781                         SKB_DATA_ALIGN(sizeof(struct skb_shared_info)));
1782         if (WARN_ONCE(!skb, "Build skb failure on Rx\n"))
1783                 goto free_buffer;
1784         skb_reserve(skb, fd_off);
1785         skb_put(skb, qm_fd_get_length(fd));
1786
1787         skb->ip_summed = rx_csum_offload(priv, fd);
1788
1789         return skb;
1790
1791 free_buffer:
1792         free_pages((unsigned long)vaddr, 0);
1793         return NULL;
1794 }
1795
1796 /* Build an skb with the data of the first S/G entry in the linear portion and
1797  * the rest of the frame as skb fragments.
1798  *
1799  * The page fragment holding the S/G Table is recycled here.
1800  */
1801 static struct sk_buff *sg_fd_to_skb(const struct dpaa_priv *priv,
1802                                     const struct qm_fd *fd)
1803 {
1804         ssize_t fd_off = qm_fd_get_offset(fd);
1805         dma_addr_t addr = qm_fd_addr(fd);
1806         const struct qm_sg_entry *sgt;
1807         struct page *page, *head_page;
1808         struct dpaa_bp *dpaa_bp;
1809         void *vaddr, *sg_vaddr;
1810         int frag_off, frag_len;
1811         struct sk_buff *skb;
1812         dma_addr_t sg_addr;
1813         int page_offset;
1814         unsigned int sz;
1815         int *count_ptr;
1816         int i, j;
1817
1818         vaddr = phys_to_virt(addr);
1819         WARN_ON(!IS_ALIGNED((unsigned long)vaddr, SMP_CACHE_BYTES));
1820
1821         /* Iterate through the SGT entries and add data buffers to the skb */
1822         sgt = vaddr + fd_off;
1823         skb = NULL;
1824         for (i = 0; i < DPAA_SGT_MAX_ENTRIES; i++) {
1825                 /* Extension bit is not supported */
1826                 WARN_ON(qm_sg_entry_is_ext(&sgt[i]));
1827
1828                 sg_addr = qm_sg_addr(&sgt[i]);
1829                 sg_vaddr = phys_to_virt(sg_addr);
1830                 WARN_ON(!PTR_IS_ALIGNED(sg_vaddr, SMP_CACHE_BYTES));
1831
1832                 dma_unmap_page(priv->rx_dma_dev, sg_addr,
1833                                DPAA_BP_RAW_SIZE, DMA_FROM_DEVICE);
1834
1835                 /* We may use multiple Rx pools */
1836                 dpaa_bp = dpaa_bpid2pool(sgt[i].bpid);
1837                 if (!dpaa_bp)
1838                         goto free_buffers;
1839
1840                 if (!skb) {
1841                         sz = dpaa_bp->size +
1842                                 SKB_DATA_ALIGN(sizeof(struct skb_shared_info));
1843                         skb = build_skb(sg_vaddr, sz);
1844                         if (WARN_ON(!skb))
1845                                 goto free_buffers;
1846
1847                         skb->ip_summed = rx_csum_offload(priv, fd);
1848
1849                         /* Make sure forwarded skbs will have enough space
1850                          * on Tx, if extra headers are added.
1851                          */
1852                         WARN_ON(fd_off != priv->rx_headroom);
1853                         skb_reserve(skb, fd_off);
1854                         skb_put(skb, qm_sg_entry_get_len(&sgt[i]));
1855                 } else {
1856                         /* Not the first S/G entry; all data from buffer will
1857                          * be added in an skb fragment; fragment index is offset
1858                          * by one since first S/G entry was incorporated in the
1859                          * linear part of the skb.
1860                          *
1861                          * Caution: 'page' may be a tail page.
1862                          */
1863                         page = virt_to_page(sg_vaddr);
1864                         head_page = virt_to_head_page(sg_vaddr);
1865
1866                         /* Compute offset in (possibly tail) page */
1867                         page_offset = ((unsigned long)sg_vaddr &
1868                                         (PAGE_SIZE - 1)) +
1869                                 (page_address(page) - page_address(head_page));
1870                         /* page_offset only refers to the beginning of sgt[i];
1871                          * but the buffer itself may have an internal offset.
1872                          */
1873                         frag_off = qm_sg_entry_get_off(&sgt[i]) + page_offset;
1874                         frag_len = qm_sg_entry_get_len(&sgt[i]);
1875                         /* skb_add_rx_frag() does no checking on the page; if
1876                          * we pass it a tail page, we'll end up with
1877                          * bad page accounting and eventually with segafults.
1878                          */
1879                         skb_add_rx_frag(skb, i - 1, head_page, frag_off,
1880                                         frag_len, dpaa_bp->size);
1881                 }
1882
1883                 /* Update the pool count for the current {cpu x bpool} */
1884                 count_ptr = this_cpu_ptr(dpaa_bp->percpu_count);
1885                 (*count_ptr)--;
1886
1887                 if (qm_sg_entry_is_final(&sgt[i]))
1888                         break;
1889         }
1890         WARN_ONCE(i == DPAA_SGT_MAX_ENTRIES, "No final bit on SGT\n");
1891
1892         /* free the SG table buffer */
1893         free_pages((unsigned long)vaddr, 0);
1894
1895         return skb;
1896
1897 free_buffers:
1898         /* free all the SG entries */
1899         for (j = 0; j < DPAA_SGT_MAX_ENTRIES ; j++) {
1900                 sg_addr = qm_sg_addr(&sgt[j]);
1901                 sg_vaddr = phys_to_virt(sg_addr);
1902                 /* all pages 0..i were unmaped */
1903                 if (j > i)
1904                         dma_unmap_page(priv->rx_dma_dev, qm_sg_addr(&sgt[j]),
1905                                        DPAA_BP_RAW_SIZE, DMA_FROM_DEVICE);
1906                 free_pages((unsigned long)sg_vaddr, 0);
1907                 /* counters 0..i-1 were decremented */
1908                 if (j >= i) {
1909                         dpaa_bp = dpaa_bpid2pool(sgt[j].bpid);
1910                         if (dpaa_bp) {
1911                                 count_ptr = this_cpu_ptr(dpaa_bp->percpu_count);
1912                                 (*count_ptr)--;
1913                         }
1914                 }
1915
1916                 if (qm_sg_entry_is_final(&sgt[j]))
1917                         break;
1918         }
1919         /* free the SGT fragment */
1920         free_pages((unsigned long)vaddr, 0);
1921
1922         return NULL;
1923 }
1924
1925 static int skb_to_contig_fd(struct dpaa_priv *priv,
1926                             struct sk_buff *skb, struct qm_fd *fd,
1927                             int *offset)
1928 {
1929         struct net_device *net_dev = priv->net_dev;
1930         enum dma_data_direction dma_dir;
1931         struct dpaa_eth_swbp *swbp;
1932         unsigned char *buff_start;
1933         dma_addr_t addr;
1934         int err;
1935
1936         /* We are guaranteed to have at least tx_headroom bytes
1937          * available, so just use that for offset.
1938          */
1939         fd->bpid = FSL_DPAA_BPID_INV;
1940         buff_start = skb->data - priv->tx_headroom;
1941         dma_dir = DMA_TO_DEVICE;
1942
1943         swbp = (struct dpaa_eth_swbp *)buff_start;
1944         swbp->skb = skb;
1945
1946         /* Enable L3/L4 hardware checksum computation.
1947          *
1948          * We must do this before dma_map_single(DMA_TO_DEVICE), because we may
1949          * need to write into the skb.
1950          */
1951         err = dpaa_enable_tx_csum(priv, skb, fd,
1952                                   buff_start + DPAA_TX_PRIV_DATA_SIZE);
1953         if (unlikely(err < 0)) {
1954                 if (net_ratelimit())
1955                         netif_err(priv, tx_err, net_dev, "HW csum error: %d\n",
1956                                   err);
1957                 return err;
1958         }
1959
1960         /* Fill in the rest of the FD fields */
1961         qm_fd_set_contig(fd, priv->tx_headroom, skb->len);
1962         fd->cmd |= cpu_to_be32(FM_FD_CMD_FCO);
1963
1964         /* Map the entire buffer size that may be seen by FMan, but no more */
1965         addr = dma_map_single(priv->tx_dma_dev, buff_start,
1966                               priv->tx_headroom + skb->len, dma_dir);
1967         if (unlikely(dma_mapping_error(priv->tx_dma_dev, addr))) {
1968                 if (net_ratelimit())
1969                         netif_err(priv, tx_err, net_dev, "dma_map_single() failed\n");
1970                 return -EINVAL;
1971         }
1972         qm_fd_addr_set64(fd, addr);
1973
1974         return 0;
1975 }
1976
1977 static int skb_to_sg_fd(struct dpaa_priv *priv,
1978                         struct sk_buff *skb, struct qm_fd *fd)
1979 {
1980         const enum dma_data_direction dma_dir = DMA_TO_DEVICE;
1981         const int nr_frags = skb_shinfo(skb)->nr_frags;
1982         struct net_device *net_dev = priv->net_dev;
1983         struct dpaa_eth_swbp *swbp;
1984         struct qm_sg_entry *sgt;
1985         void *buff_start;
1986         skb_frag_t *frag;
1987         dma_addr_t addr;
1988         size_t frag_len;
1989         struct page *p;
1990         int i, j, err;
1991
1992         /* get a page to store the SGTable */
1993         p = dev_alloc_pages(0);
1994         if (unlikely(!p)) {
1995                 netdev_err(net_dev, "dev_alloc_pages() failed\n");
1996                 return -ENOMEM;
1997         }
1998         buff_start = page_address(p);
1999
2000         /* Enable L3/L4 hardware checksum computation.
2001          *
2002          * We must do this before dma_map_single(DMA_TO_DEVICE), because we may
2003          * need to write into the skb.
2004          */
2005         err = dpaa_enable_tx_csum(priv, skb, fd,
2006                                   buff_start + DPAA_TX_PRIV_DATA_SIZE);
2007         if (unlikely(err < 0)) {
2008                 if (net_ratelimit())
2009                         netif_err(priv, tx_err, net_dev, "HW csum error: %d\n",
2010                                   err);
2011                 goto csum_failed;
2012         }
2013
2014         /* SGT[0] is used by the linear part */
2015         sgt = (struct qm_sg_entry *)(buff_start + priv->tx_headroom);
2016         frag_len = skb_headlen(skb);
2017         qm_sg_entry_set_len(&sgt[0], frag_len);
2018         sgt[0].bpid = FSL_DPAA_BPID_INV;
2019         sgt[0].offset = 0;
2020         addr = dma_map_single(priv->tx_dma_dev, skb->data,
2021                               skb_headlen(skb), dma_dir);
2022         if (unlikely(dma_mapping_error(priv->tx_dma_dev, addr))) {
2023                 netdev_err(priv->net_dev, "DMA mapping failed\n");
2024                 err = -EINVAL;
2025                 goto sg0_map_failed;
2026         }
2027         qm_sg_entry_set64(&sgt[0], addr);
2028
2029         /* populate the rest of SGT entries */
2030         for (i = 0; i < nr_frags; i++) {
2031                 frag = &skb_shinfo(skb)->frags[i];
2032                 frag_len = skb_frag_size(frag);
2033                 WARN_ON(!skb_frag_page(frag));
2034                 addr = skb_frag_dma_map(priv->tx_dma_dev, frag, 0,
2035                                         frag_len, dma_dir);
2036                 if (unlikely(dma_mapping_error(priv->tx_dma_dev, addr))) {
2037                         netdev_err(priv->net_dev, "DMA mapping failed\n");
2038                         err = -EINVAL;
2039                         goto sg_map_failed;
2040                 }
2041
2042                 qm_sg_entry_set_len(&sgt[i + 1], frag_len);
2043                 sgt[i + 1].bpid = FSL_DPAA_BPID_INV;
2044                 sgt[i + 1].offset = 0;
2045
2046                 /* keep the offset in the address */
2047                 qm_sg_entry_set64(&sgt[i + 1], addr);
2048         }
2049
2050         /* Set the final bit in the last used entry of the SGT */
2051         qm_sg_entry_set_f(&sgt[nr_frags], frag_len);
2052
2053         /* set fd offset to priv->tx_headroom */
2054         qm_fd_set_sg(fd, priv->tx_headroom, skb->len);
2055
2056         /* DMA map the SGT page */
2057         swbp = (struct dpaa_eth_swbp *)buff_start;
2058         swbp->skb = skb;
2059
2060         addr = dma_map_page(priv->tx_dma_dev, p, 0,
2061                             priv->tx_headroom + DPAA_SGT_SIZE, dma_dir);
2062         if (unlikely(dma_mapping_error(priv->tx_dma_dev, addr))) {
2063                 netdev_err(priv->net_dev, "DMA mapping failed\n");
2064                 err = -EINVAL;
2065                 goto sgt_map_failed;
2066         }
2067
2068         fd->bpid = FSL_DPAA_BPID_INV;
2069         fd->cmd |= cpu_to_be32(FM_FD_CMD_FCO);
2070         qm_fd_addr_set64(fd, addr);
2071
2072         return 0;
2073
2074 sgt_map_failed:
2075 sg_map_failed:
2076         for (j = 0; j < i; j++)
2077                 dma_unmap_page(priv->tx_dma_dev, qm_sg_addr(&sgt[j]),
2078                                qm_sg_entry_get_len(&sgt[j]), dma_dir);
2079 sg0_map_failed:
2080 csum_failed:
2081         free_pages((unsigned long)buff_start, 0);
2082
2083         return err;
2084 }
2085
2086 static inline int dpaa_xmit(struct dpaa_priv *priv,
2087                             struct rtnl_link_stats64 *percpu_stats,
2088                             int queue,
2089                             struct qm_fd *fd)
2090 {
2091         struct qman_fq *egress_fq;
2092         int err, i;
2093
2094         egress_fq = priv->egress_fqs[queue];
2095         if (fd->bpid == FSL_DPAA_BPID_INV)
2096                 fd->cmd |= cpu_to_be32(qman_fq_fqid(priv->conf_fqs[queue]));
2097
2098         /* Trace this Tx fd */
2099         trace_dpaa_tx_fd(priv->net_dev, egress_fq, fd);
2100
2101         for (i = 0; i < DPAA_ENQUEUE_RETRIES; i++) {
2102                 err = qman_enqueue(egress_fq, fd);
2103                 if (err != -EBUSY)
2104                         break;
2105         }
2106
2107         if (unlikely(err < 0)) {
2108                 percpu_stats->tx_fifo_errors++;
2109                 return err;
2110         }
2111
2112         percpu_stats->tx_packets++;
2113         percpu_stats->tx_bytes += qm_fd_get_length(fd);
2114
2115         return 0;
2116 }
2117
2118 #ifdef CONFIG_DPAA_ERRATUM_A050385
2119 static int dpaa_a050385_wa_skb(struct net_device *net_dev, struct sk_buff **s)
2120 {
2121         struct dpaa_priv *priv = netdev_priv(net_dev);
2122         struct sk_buff *new_skb, *skb = *s;
2123         unsigned char *start, i;
2124
2125         /* check linear buffer alignment */
2126         if (!PTR_IS_ALIGNED(skb->data, DPAA_A050385_ALIGN))
2127                 goto workaround;
2128
2129         /* linear buffers just need to have an aligned start */
2130         if (!skb_is_nonlinear(skb))
2131                 return 0;
2132
2133         /* linear data size for nonlinear skbs needs to be aligned */
2134         if (!IS_ALIGNED(skb_headlen(skb), DPAA_A050385_ALIGN))
2135                 goto workaround;
2136
2137         for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
2138                 skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
2139
2140                 /* all fragments need to have aligned start addresses */
2141                 if (!IS_ALIGNED(skb_frag_off(frag), DPAA_A050385_ALIGN))
2142                         goto workaround;
2143
2144                 /* all but last fragment need to have aligned sizes */
2145                 if (!IS_ALIGNED(skb_frag_size(frag), DPAA_A050385_ALIGN) &&
2146                     (i < skb_shinfo(skb)->nr_frags - 1))
2147                         goto workaround;
2148         }
2149
2150         return 0;
2151
2152 workaround:
2153         /* copy all the skb content into a new linear buffer */
2154         new_skb = netdev_alloc_skb(net_dev, skb->len + DPAA_A050385_ALIGN - 1 +
2155                                                 priv->tx_headroom);
2156         if (!new_skb)
2157                 return -ENOMEM;
2158
2159         /* NET_SKB_PAD bytes already reserved, adding up to tx_headroom */
2160         skb_reserve(new_skb, priv->tx_headroom - NET_SKB_PAD);
2161
2162         /* Workaround for DPAA_A050385 requires data start to be aligned */
2163         start = PTR_ALIGN(new_skb->data, DPAA_A050385_ALIGN);
2164         if (start - new_skb->data)
2165                 skb_reserve(new_skb, start - new_skb->data);
2166
2167         skb_put(new_skb, skb->len);
2168         skb_copy_bits(skb, 0, new_skb->data, skb->len);
2169         skb_copy_header(new_skb, skb);
2170         new_skb->dev = skb->dev;
2171
2172         /* Copy relevant timestamp info from the old skb to the new */
2173         if (priv->tx_tstamp) {
2174                 skb_shinfo(new_skb)->tx_flags = skb_shinfo(skb)->tx_flags;
2175                 skb_shinfo(new_skb)->hwtstamps = skb_shinfo(skb)->hwtstamps;
2176                 skb_shinfo(new_skb)->tskey = skb_shinfo(skb)->tskey;
2177                 if (skb->sk)
2178                         skb_set_owner_w(new_skb, skb->sk);
2179         }
2180
2181         /* We move the headroom when we align it so we have to reset the
2182          * network and transport header offsets relative to the new data
2183          * pointer. The checksum offload relies on these offsets.
2184          */
2185         skb_set_network_header(new_skb, skb_network_offset(skb));
2186         skb_set_transport_header(new_skb, skb_transport_offset(skb));
2187
2188         dev_kfree_skb(skb);
2189         *s = new_skb;
2190
2191         return 0;
2192 }
2193
2194 static int dpaa_a050385_wa_xdpf(struct dpaa_priv *priv,
2195                                 struct xdp_frame **init_xdpf)
2196 {
2197         struct xdp_frame *new_xdpf, *xdpf = *init_xdpf;
2198         void *new_buff, *aligned_data;
2199         struct page *p;
2200         u32 data_shift;
2201         int headroom;
2202
2203         /* Check the data alignment and make sure the headroom is large
2204          * enough to store the xdpf backpointer. Use an aligned headroom
2205          * value.
2206          *
2207          * Due to alignment constraints, we give XDP access to the full 256
2208          * byte frame headroom. If the XDP program uses all of it, copy the
2209          * data to a new buffer and make room for storing the backpointer.
2210          */
2211         if (PTR_IS_ALIGNED(xdpf->data, DPAA_FD_DATA_ALIGNMENT) &&
2212             xdpf->headroom >= priv->tx_headroom) {
2213                 xdpf->headroom = priv->tx_headroom;
2214                 return 0;
2215         }
2216
2217         /* Try to move the data inside the buffer just enough to align it and
2218          * store the xdpf backpointer. If the available headroom isn't large
2219          * enough, resort to allocating a new buffer and copying the data.
2220          */
2221         aligned_data = PTR_ALIGN_DOWN(xdpf->data, DPAA_FD_DATA_ALIGNMENT);
2222         data_shift = xdpf->data - aligned_data;
2223
2224         /* The XDP frame's headroom needs to be large enough to accommodate
2225          * shifting the data as well as storing the xdpf backpointer.
2226          */
2227         if (xdpf->headroom  >= data_shift + priv->tx_headroom) {
2228                 memmove(aligned_data, xdpf->data, xdpf->len);
2229                 xdpf->data = aligned_data;
2230                 xdpf->headroom = priv->tx_headroom;
2231                 return 0;
2232         }
2233
2234         /* The new xdp_frame is stored in the new buffer. Reserve enough space
2235          * in the headroom for storing it along with the driver's private
2236          * info. The headroom needs to be aligned to DPAA_FD_DATA_ALIGNMENT to
2237          * guarantee the data's alignment in the buffer.
2238          */
2239         headroom = ALIGN(sizeof(*new_xdpf) + priv->tx_headroom,
2240                          DPAA_FD_DATA_ALIGNMENT);
2241
2242         /* Assure the extended headroom and data don't overflow the buffer,
2243          * while maintaining the mandatory tailroom.
2244          */
2245         if (headroom + xdpf->len > DPAA_BP_RAW_SIZE -
2246                         SKB_DATA_ALIGN(sizeof(struct skb_shared_info)))
2247                 return -ENOMEM;
2248
2249         p = dev_alloc_pages(0);
2250         if (unlikely(!p))
2251                 return -ENOMEM;
2252
2253         /* Copy the data to the new buffer at a properly aligned offset */
2254         new_buff = page_address(p);
2255         memcpy(new_buff + headroom, xdpf->data, xdpf->len);
2256
2257         /* Create an XDP frame around the new buffer in a similar fashion
2258          * to xdp_convert_buff_to_frame.
2259          */
2260         new_xdpf = new_buff;
2261         new_xdpf->data = new_buff + headroom;
2262         new_xdpf->len = xdpf->len;
2263         new_xdpf->headroom = priv->tx_headroom;
2264         new_xdpf->frame_sz = DPAA_BP_RAW_SIZE;
2265         new_xdpf->mem.type = MEM_TYPE_PAGE_ORDER0;
2266
2267         /* Release the initial buffer */
2268         xdp_return_frame_rx_napi(xdpf);
2269
2270         *init_xdpf = new_xdpf;
2271         return 0;
2272 }
2273 #endif
2274
2275 static netdev_tx_t
2276 dpaa_start_xmit(struct sk_buff *skb, struct net_device *net_dev)
2277 {
2278         const int queue_mapping = skb_get_queue_mapping(skb);
2279         bool nonlinear = skb_is_nonlinear(skb);
2280         struct rtnl_link_stats64 *percpu_stats;
2281         struct dpaa_percpu_priv *percpu_priv;
2282         struct netdev_queue *txq;
2283         struct dpaa_priv *priv;
2284         struct qm_fd fd;
2285         int offset = 0;
2286         int err = 0;
2287
2288         priv = netdev_priv(net_dev);
2289         percpu_priv = this_cpu_ptr(priv->percpu_priv);
2290         percpu_stats = &percpu_priv->stats;
2291
2292         qm_fd_clear_fd(&fd);
2293
2294         if (!nonlinear) {
2295                 /* We're going to store the skb backpointer at the beginning
2296                  * of the data buffer, so we need a privately owned skb
2297                  *
2298                  * We've made sure skb is not shared in dev->priv_flags,
2299                  * we need to verify the skb head is not cloned
2300                  */
2301                 if (skb_cow_head(skb, priv->tx_headroom))
2302                         goto enomem;
2303
2304                 WARN_ON(skb_is_nonlinear(skb));
2305         }
2306
2307         /* MAX_SKB_FRAGS is equal or larger than our dpaa_SGT_MAX_ENTRIES;
2308          * make sure we don't feed FMan with more fragments than it supports.
2309          */
2310         if (unlikely(nonlinear &&
2311                      (skb_shinfo(skb)->nr_frags >= DPAA_SGT_MAX_ENTRIES))) {
2312                 /* If the egress skb contains more fragments than we support
2313                  * we have no choice but to linearize it ourselves.
2314                  */
2315                 if (__skb_linearize(skb))
2316                         goto enomem;
2317
2318                 nonlinear = skb_is_nonlinear(skb);
2319         }
2320
2321 #ifdef CONFIG_DPAA_ERRATUM_A050385
2322         if (unlikely(fman_has_errata_a050385())) {
2323                 if (dpaa_a050385_wa_skb(net_dev, &skb))
2324                         goto enomem;
2325                 nonlinear = skb_is_nonlinear(skb);
2326         }
2327 #endif
2328
2329         if (nonlinear) {
2330                 /* Just create a S/G fd based on the skb */
2331                 err = skb_to_sg_fd(priv, skb, &fd);
2332                 percpu_priv->tx_frag_skbuffs++;
2333         } else {
2334                 /* Create a contig FD from this skb */
2335                 err = skb_to_contig_fd(priv, skb, &fd, &offset);
2336         }
2337         if (unlikely(err < 0))
2338                 goto skb_to_fd_failed;
2339
2340         txq = netdev_get_tx_queue(net_dev, queue_mapping);
2341
2342         /* LLTX requires to do our own update of trans_start */
2343         txq_trans_cond_update(txq);
2344
2345         if (priv->tx_tstamp && skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) {
2346                 fd.cmd |= cpu_to_be32(FM_FD_CMD_UPD);
2347                 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
2348         }
2349
2350         if (likely(dpaa_xmit(priv, percpu_stats, queue_mapping, &fd) == 0))
2351                 return NETDEV_TX_OK;
2352
2353         dpaa_cleanup_tx_fd(priv, &fd, false);
2354 skb_to_fd_failed:
2355 enomem:
2356         percpu_stats->tx_errors++;
2357         dev_kfree_skb(skb);
2358         return NETDEV_TX_OK;
2359 }
2360
2361 static void dpaa_rx_error(struct net_device *net_dev,
2362                           const struct dpaa_priv *priv,
2363                           struct dpaa_percpu_priv *percpu_priv,
2364                           const struct qm_fd *fd,
2365                           u32 fqid)
2366 {
2367         if (net_ratelimit())
2368                 netif_err(priv, hw, net_dev, "Err FD status = 0x%08x\n",
2369                           be32_to_cpu(fd->status) & FM_FD_STAT_RX_ERRORS);
2370
2371         percpu_priv->stats.rx_errors++;
2372
2373         if (be32_to_cpu(fd->status) & FM_FD_ERR_DMA)
2374                 percpu_priv->rx_errors.dme++;
2375         if (be32_to_cpu(fd->status) & FM_FD_ERR_PHYSICAL)
2376                 percpu_priv->rx_errors.fpe++;
2377         if (be32_to_cpu(fd->status) & FM_FD_ERR_SIZE)
2378                 percpu_priv->rx_errors.fse++;
2379         if (be32_to_cpu(fd->status) & FM_FD_ERR_PRS_HDR_ERR)
2380                 percpu_priv->rx_errors.phe++;
2381
2382         dpaa_fd_release(net_dev, fd);
2383 }
2384
2385 static void dpaa_tx_error(struct net_device *net_dev,
2386                           const struct dpaa_priv *priv,
2387                           struct dpaa_percpu_priv *percpu_priv,
2388                           const struct qm_fd *fd,
2389                           u32 fqid)
2390 {
2391         struct sk_buff *skb;
2392
2393         if (net_ratelimit())
2394                 netif_warn(priv, hw, net_dev, "FD status = 0x%08x\n",
2395                            be32_to_cpu(fd->status) & FM_FD_STAT_TX_ERRORS);
2396
2397         percpu_priv->stats.tx_errors++;
2398
2399         skb = dpaa_cleanup_tx_fd(priv, fd, false);
2400         dev_kfree_skb(skb);
2401 }
2402
2403 static int dpaa_eth_poll(struct napi_struct *napi, int budget)
2404 {
2405         struct dpaa_napi_portal *np =
2406                         container_of(napi, struct dpaa_napi_portal, napi);
2407         int cleaned;
2408
2409         np->xdp_act = 0;
2410
2411         cleaned = qman_p_poll_dqrr(np->p, budget);
2412
2413         if (np->xdp_act & XDP_REDIRECT)
2414                 xdp_do_flush();
2415
2416         if (cleaned < budget) {
2417                 napi_complete_done(napi, cleaned);
2418                 qman_p_irqsource_add(np->p, QM_PIRQ_DQRI);
2419         } else if (np->down) {
2420                 qman_p_irqsource_add(np->p, QM_PIRQ_DQRI);
2421         }
2422
2423         return cleaned;
2424 }
2425
2426 static void dpaa_tx_conf(struct net_device *net_dev,
2427                          const struct dpaa_priv *priv,
2428                          struct dpaa_percpu_priv *percpu_priv,
2429                          const struct qm_fd *fd,
2430                          u32 fqid)
2431 {
2432         struct sk_buff  *skb;
2433
2434         if (unlikely(be32_to_cpu(fd->status) & FM_FD_STAT_TX_ERRORS)) {
2435                 if (net_ratelimit())
2436                         netif_warn(priv, hw, net_dev, "FD status = 0x%08x\n",
2437                                    be32_to_cpu(fd->status) &
2438                                    FM_FD_STAT_TX_ERRORS);
2439
2440                 percpu_priv->stats.tx_errors++;
2441         }
2442
2443         percpu_priv->tx_confirm++;
2444
2445         skb = dpaa_cleanup_tx_fd(priv, fd, true);
2446
2447         consume_skb(skb);
2448 }
2449
2450 static inline int dpaa_eth_napi_schedule(struct dpaa_percpu_priv *percpu_priv,
2451                                          struct qman_portal *portal, bool sched_napi)
2452 {
2453         if (sched_napi) {
2454                 /* Disable QMan IRQ and invoke NAPI */
2455                 qman_p_irqsource_remove(portal, QM_PIRQ_DQRI);
2456
2457                 percpu_priv->np.p = portal;
2458                 napi_schedule(&percpu_priv->np.napi);
2459                 percpu_priv->in_interrupt++;
2460                 return 1;
2461         }
2462         return 0;
2463 }
2464
2465 static enum qman_cb_dqrr_result rx_error_dqrr(struct qman_portal *portal,
2466                                               struct qman_fq *fq,
2467                                               const struct qm_dqrr_entry *dq,
2468                                               bool sched_napi)
2469 {
2470         struct dpaa_fq *dpaa_fq = container_of(fq, struct dpaa_fq, fq_base);
2471         struct dpaa_percpu_priv *percpu_priv;
2472         struct net_device *net_dev;
2473         struct dpaa_bp *dpaa_bp;
2474         struct dpaa_priv *priv;
2475
2476         net_dev = dpaa_fq->net_dev;
2477         priv = netdev_priv(net_dev);
2478         dpaa_bp = dpaa_bpid2pool(dq->fd.bpid);
2479         if (!dpaa_bp)
2480                 return qman_cb_dqrr_consume;
2481
2482         percpu_priv = this_cpu_ptr(priv->percpu_priv);
2483
2484         if (dpaa_eth_napi_schedule(percpu_priv, portal, sched_napi))
2485                 return qman_cb_dqrr_stop;
2486
2487         dpaa_eth_refill_bpools(priv);
2488         dpaa_rx_error(net_dev, priv, percpu_priv, &dq->fd, fq->fqid);
2489
2490         return qman_cb_dqrr_consume;
2491 }
2492
2493 static int dpaa_xdp_xmit_frame(struct net_device *net_dev,
2494                                struct xdp_frame *xdpf)
2495 {
2496         struct dpaa_priv *priv = netdev_priv(net_dev);
2497         struct rtnl_link_stats64 *percpu_stats;
2498         struct dpaa_percpu_priv *percpu_priv;
2499         struct dpaa_eth_swbp *swbp;
2500         struct netdev_queue *txq;
2501         void *buff_start;
2502         struct qm_fd fd;
2503         dma_addr_t addr;
2504         int err;
2505
2506         percpu_priv = this_cpu_ptr(priv->percpu_priv);
2507         percpu_stats = &percpu_priv->stats;
2508
2509 #ifdef CONFIG_DPAA_ERRATUM_A050385
2510         if (unlikely(fman_has_errata_a050385())) {
2511                 if (dpaa_a050385_wa_xdpf(priv, &xdpf)) {
2512                         err = -ENOMEM;
2513                         goto out_error;
2514                 }
2515         }
2516 #endif
2517
2518         if (xdpf->headroom < DPAA_TX_PRIV_DATA_SIZE) {
2519                 err = -EINVAL;
2520                 goto out_error;
2521         }
2522
2523         buff_start = xdpf->data - xdpf->headroom;
2524
2525         /* Leave empty the skb backpointer at the start of the buffer.
2526          * Save the XDP frame for easy cleanup on confirmation.
2527          */
2528         swbp = (struct dpaa_eth_swbp *)buff_start;
2529         swbp->skb = NULL;
2530         swbp->xdpf = xdpf;
2531
2532         qm_fd_clear_fd(&fd);
2533         fd.bpid = FSL_DPAA_BPID_INV;
2534         fd.cmd |= cpu_to_be32(FM_FD_CMD_FCO);
2535         qm_fd_set_contig(&fd, xdpf->headroom, xdpf->len);
2536
2537         addr = dma_map_single(priv->tx_dma_dev, buff_start,
2538                               xdpf->headroom + xdpf->len,
2539                               DMA_TO_DEVICE);
2540         if (unlikely(dma_mapping_error(priv->tx_dma_dev, addr))) {
2541                 err = -EINVAL;
2542                 goto out_error;
2543         }
2544
2545         qm_fd_addr_set64(&fd, addr);
2546
2547         /* Bump the trans_start */
2548         txq = netdev_get_tx_queue(net_dev, smp_processor_id());
2549         txq_trans_cond_update(txq);
2550
2551         err = dpaa_xmit(priv, percpu_stats, smp_processor_id(), &fd);
2552         if (err) {
2553                 dma_unmap_single(priv->tx_dma_dev, addr,
2554                                  qm_fd_get_offset(&fd) + qm_fd_get_length(&fd),
2555                                  DMA_TO_DEVICE);
2556                 goto out_error;
2557         }
2558
2559         return 0;
2560
2561 out_error:
2562         percpu_stats->tx_errors++;
2563         return err;
2564 }
2565
2566 static u32 dpaa_run_xdp(struct dpaa_priv *priv, struct qm_fd *fd, void *vaddr,
2567                         struct dpaa_fq *dpaa_fq, unsigned int *xdp_meta_len)
2568 {
2569         ssize_t fd_off = qm_fd_get_offset(fd);
2570         struct bpf_prog *xdp_prog;
2571         struct xdp_frame *xdpf;
2572         struct xdp_buff xdp;
2573         u32 xdp_act;
2574         int err;
2575
2576         xdp_prog = READ_ONCE(priv->xdp_prog);
2577         if (!xdp_prog)
2578                 return XDP_PASS;
2579
2580         xdp_init_buff(&xdp, DPAA_BP_RAW_SIZE - DPAA_TX_PRIV_DATA_SIZE,
2581                       &dpaa_fq->xdp_rxq);
2582         xdp_prepare_buff(&xdp, vaddr + fd_off - XDP_PACKET_HEADROOM,
2583                          XDP_PACKET_HEADROOM, qm_fd_get_length(fd), true);
2584
2585         /* We reserve a fixed headroom of 256 bytes under the erratum and we
2586          * offer it all to XDP programs to use. If no room is left for the
2587          * xdpf backpointer on TX, we will need to copy the data.
2588          * Disable metadata support since data realignments might be required
2589          * and the information can be lost.
2590          */
2591 #ifdef CONFIG_DPAA_ERRATUM_A050385
2592         if (unlikely(fman_has_errata_a050385())) {
2593                 xdp_set_data_meta_invalid(&xdp);
2594                 xdp.data_hard_start = vaddr;
2595                 xdp.frame_sz = DPAA_BP_RAW_SIZE;
2596         }
2597 #endif
2598
2599         xdp_act = bpf_prog_run_xdp(xdp_prog, &xdp);
2600
2601         /* Update the length and the offset of the FD */
2602         qm_fd_set_contig(fd, xdp.data - vaddr, xdp.data_end - xdp.data);
2603
2604         switch (xdp_act) {
2605         case XDP_PASS:
2606 #ifdef CONFIG_DPAA_ERRATUM_A050385
2607                 *xdp_meta_len = xdp_data_meta_unsupported(&xdp) ? 0 :
2608                                 xdp.data - xdp.data_meta;
2609 #else
2610                 *xdp_meta_len = xdp.data - xdp.data_meta;
2611 #endif
2612                 break;
2613         case XDP_TX:
2614                 /* We can access the full headroom when sending the frame
2615                  * back out
2616                  */
2617                 xdp.data_hard_start = vaddr;
2618                 xdp.frame_sz = DPAA_BP_RAW_SIZE;
2619                 xdpf = xdp_convert_buff_to_frame(&xdp);
2620                 if (unlikely(!xdpf)) {
2621                         free_pages((unsigned long)vaddr, 0);
2622                         break;
2623                 }
2624
2625                 if (dpaa_xdp_xmit_frame(priv->net_dev, xdpf))
2626                         xdp_return_frame_rx_napi(xdpf);
2627
2628                 break;
2629         case XDP_REDIRECT:
2630                 /* Allow redirect to use the full headroom */
2631                 xdp.data_hard_start = vaddr;
2632                 xdp.frame_sz = DPAA_BP_RAW_SIZE;
2633
2634                 err = xdp_do_redirect(priv->net_dev, &xdp, xdp_prog);
2635                 if (err) {
2636                         trace_xdp_exception(priv->net_dev, xdp_prog, xdp_act);
2637                         free_pages((unsigned long)vaddr, 0);
2638                 }
2639                 break;
2640         default:
2641                 bpf_warn_invalid_xdp_action(priv->net_dev, xdp_prog, xdp_act);
2642                 fallthrough;
2643         case XDP_ABORTED:
2644                 trace_xdp_exception(priv->net_dev, xdp_prog, xdp_act);
2645                 fallthrough;
2646         case XDP_DROP:
2647                 /* Free the buffer */
2648                 free_pages((unsigned long)vaddr, 0);
2649                 break;
2650         }
2651
2652         return xdp_act;
2653 }
2654
2655 static enum qman_cb_dqrr_result rx_default_dqrr(struct qman_portal *portal,
2656                                                 struct qman_fq *fq,
2657                                                 const struct qm_dqrr_entry *dq,
2658                                                 bool sched_napi)
2659 {
2660         bool ts_valid = false, hash_valid = false;
2661         struct skb_shared_hwtstamps *shhwtstamps;
2662         unsigned int skb_len, xdp_meta_len = 0;
2663         struct rtnl_link_stats64 *percpu_stats;
2664         struct dpaa_percpu_priv *percpu_priv;
2665         const struct qm_fd *fd = &dq->fd;
2666         dma_addr_t addr = qm_fd_addr(fd);
2667         struct dpaa_napi_portal *np;
2668         enum qm_fd_format fd_format;
2669         struct net_device *net_dev;
2670         u32 fd_status, hash_offset;
2671         struct qm_sg_entry *sgt;
2672         struct dpaa_bp *dpaa_bp;
2673         struct dpaa_fq *dpaa_fq;
2674         struct dpaa_priv *priv;
2675         struct sk_buff *skb;
2676         int *count_ptr;
2677         u32 xdp_act;
2678         void *vaddr;
2679         u32 hash;
2680         u64 ns;
2681
2682         dpaa_fq = container_of(fq, struct dpaa_fq, fq_base);
2683         fd_status = be32_to_cpu(fd->status);
2684         fd_format = qm_fd_get_format(fd);
2685         net_dev = dpaa_fq->net_dev;
2686         priv = netdev_priv(net_dev);
2687         dpaa_bp = dpaa_bpid2pool(dq->fd.bpid);
2688         if (!dpaa_bp)
2689                 return qman_cb_dqrr_consume;
2690
2691         /* Trace the Rx fd */
2692         trace_dpaa_rx_fd(net_dev, fq, &dq->fd);
2693
2694         percpu_priv = this_cpu_ptr(priv->percpu_priv);
2695         percpu_stats = &percpu_priv->stats;
2696         np = &percpu_priv->np;
2697
2698         if (unlikely(dpaa_eth_napi_schedule(percpu_priv, portal, sched_napi)))
2699                 return qman_cb_dqrr_stop;
2700
2701         /* Make sure we didn't run out of buffers */
2702         if (unlikely(dpaa_eth_refill_bpools(priv))) {
2703                 /* Unable to refill the buffer pool due to insufficient
2704                  * system memory. Just release the frame back into the pool,
2705                  * otherwise we'll soon end up with an empty buffer pool.
2706                  */
2707                 dpaa_fd_release(net_dev, &dq->fd);
2708                 return qman_cb_dqrr_consume;
2709         }
2710
2711         if (unlikely(fd_status & FM_FD_STAT_RX_ERRORS) != 0) {
2712                 if (net_ratelimit())
2713                         netif_warn(priv, hw, net_dev, "FD status = 0x%08x\n",
2714                                    fd_status & FM_FD_STAT_RX_ERRORS);
2715
2716                 percpu_stats->rx_errors++;
2717                 dpaa_fd_release(net_dev, fd);
2718                 return qman_cb_dqrr_consume;
2719         }
2720
2721         dma_unmap_page(dpaa_bp->priv->rx_dma_dev, addr, DPAA_BP_RAW_SIZE,
2722                        DMA_FROM_DEVICE);
2723
2724         /* prefetch the first 64 bytes of the frame or the SGT start */
2725         vaddr = phys_to_virt(addr);
2726         prefetch(vaddr + qm_fd_get_offset(fd));
2727
2728         /* The only FD types that we may receive are contig and S/G */
2729         WARN_ON((fd_format != qm_fd_contig) && (fd_format != qm_fd_sg));
2730
2731         /* Account for either the contig buffer or the SGT buffer (depending on
2732          * which case we were in) having been removed from the pool.
2733          */
2734         count_ptr = this_cpu_ptr(dpaa_bp->percpu_count);
2735         (*count_ptr)--;
2736
2737         /* Extract the timestamp stored in the headroom before running XDP */
2738         if (priv->rx_tstamp) {
2739                 if (!fman_port_get_tstamp(priv->mac_dev->port[RX], vaddr, &ns))
2740                         ts_valid = true;
2741                 else
2742                         WARN_ONCE(1, "fman_port_get_tstamp failed!\n");
2743         }
2744
2745         /* Extract the hash stored in the headroom before running XDP */
2746         if (net_dev->features & NETIF_F_RXHASH && priv->keygen_in_use &&
2747             !fman_port_get_hash_result_offset(priv->mac_dev->port[RX],
2748                                               &hash_offset)) {
2749                 hash = be32_to_cpu(*(u32 *)(vaddr + hash_offset));
2750                 hash_valid = true;
2751         }
2752
2753         if (likely(fd_format == qm_fd_contig)) {
2754                 xdp_act = dpaa_run_xdp(priv, (struct qm_fd *)fd, vaddr,
2755                                        dpaa_fq, &xdp_meta_len);
2756                 np->xdp_act |= xdp_act;
2757                 if (xdp_act != XDP_PASS) {
2758                         percpu_stats->rx_packets++;
2759                         percpu_stats->rx_bytes += qm_fd_get_length(fd);
2760                         return qman_cb_dqrr_consume;
2761                 }
2762                 skb = contig_fd_to_skb(priv, fd);
2763         } else {
2764                 /* XDP doesn't support S/G frames. Return the fragments to the
2765                  * buffer pool and release the SGT.
2766                  */
2767                 if (READ_ONCE(priv->xdp_prog)) {
2768                         WARN_ONCE(1, "S/G frames not supported under XDP\n");
2769                         sgt = vaddr + qm_fd_get_offset(fd);
2770                         dpaa_release_sgt_members(sgt);
2771                         free_pages((unsigned long)vaddr, 0);
2772                         return qman_cb_dqrr_consume;
2773                 }
2774                 skb = sg_fd_to_skb(priv, fd);
2775         }
2776         if (!skb)
2777                 return qman_cb_dqrr_consume;
2778
2779         if (xdp_meta_len)
2780                 skb_metadata_set(skb, xdp_meta_len);
2781
2782         /* Set the previously extracted timestamp */
2783         if (ts_valid) {
2784                 shhwtstamps = skb_hwtstamps(skb);
2785                 memset(shhwtstamps, 0, sizeof(*shhwtstamps));
2786                 shhwtstamps->hwtstamp = ns_to_ktime(ns);
2787         }
2788
2789         skb->protocol = eth_type_trans(skb, net_dev);
2790
2791         /* Set the previously extracted hash */
2792         if (hash_valid) {
2793                 enum pkt_hash_types type;
2794
2795                 /* if L4 exists, it was used in the hash generation */
2796                 type = be32_to_cpu(fd->status) & FM_FD_STAT_L4CV ?
2797                         PKT_HASH_TYPE_L4 : PKT_HASH_TYPE_L3;
2798                 skb_set_hash(skb, hash, type);
2799         }
2800
2801         skb_len = skb->len;
2802
2803         if (unlikely(netif_receive_skb(skb) == NET_RX_DROP)) {
2804                 percpu_stats->rx_dropped++;
2805                 return qman_cb_dqrr_consume;
2806         }
2807
2808         percpu_stats->rx_packets++;
2809         percpu_stats->rx_bytes += skb_len;
2810
2811         return qman_cb_dqrr_consume;
2812 }
2813
2814 static enum qman_cb_dqrr_result conf_error_dqrr(struct qman_portal *portal,
2815                                                 struct qman_fq *fq,
2816                                                 const struct qm_dqrr_entry *dq,
2817                                                 bool sched_napi)
2818 {
2819         struct dpaa_percpu_priv *percpu_priv;
2820         struct net_device *net_dev;
2821         struct dpaa_priv *priv;
2822
2823         net_dev = ((struct dpaa_fq *)fq)->net_dev;
2824         priv = netdev_priv(net_dev);
2825
2826         percpu_priv = this_cpu_ptr(priv->percpu_priv);
2827
2828         if (dpaa_eth_napi_schedule(percpu_priv, portal, sched_napi))
2829                 return qman_cb_dqrr_stop;
2830
2831         dpaa_tx_error(net_dev, priv, percpu_priv, &dq->fd, fq->fqid);
2832
2833         return qman_cb_dqrr_consume;
2834 }
2835
2836 static enum qman_cb_dqrr_result conf_dflt_dqrr(struct qman_portal *portal,
2837                                                struct qman_fq *fq,
2838                                                const struct qm_dqrr_entry *dq,
2839                                                bool sched_napi)
2840 {
2841         struct dpaa_percpu_priv *percpu_priv;
2842         struct net_device *net_dev;
2843         struct dpaa_priv *priv;
2844
2845         net_dev = ((struct dpaa_fq *)fq)->net_dev;
2846         priv = netdev_priv(net_dev);
2847
2848         /* Trace the fd */
2849         trace_dpaa_tx_conf_fd(net_dev, fq, &dq->fd);
2850
2851         percpu_priv = this_cpu_ptr(priv->percpu_priv);
2852
2853         if (dpaa_eth_napi_schedule(percpu_priv, portal, sched_napi))
2854                 return qman_cb_dqrr_stop;
2855
2856         dpaa_tx_conf(net_dev, priv, percpu_priv, &dq->fd, fq->fqid);
2857
2858         return qman_cb_dqrr_consume;
2859 }
2860
2861 static void egress_ern(struct qman_portal *portal,
2862                        struct qman_fq *fq,
2863                        const union qm_mr_entry *msg)
2864 {
2865         const struct qm_fd *fd = &msg->ern.fd;
2866         struct dpaa_percpu_priv *percpu_priv;
2867         const struct dpaa_priv *priv;
2868         struct net_device *net_dev;
2869         struct sk_buff *skb;
2870
2871         net_dev = ((struct dpaa_fq *)fq)->net_dev;
2872         priv = netdev_priv(net_dev);
2873         percpu_priv = this_cpu_ptr(priv->percpu_priv);
2874
2875         percpu_priv->stats.tx_dropped++;
2876         percpu_priv->stats.tx_fifo_errors++;
2877         count_ern(percpu_priv, msg);
2878
2879         skb = dpaa_cleanup_tx_fd(priv, fd, false);
2880         dev_kfree_skb_any(skb);
2881 }
2882
2883 static const struct dpaa_fq_cbs dpaa_fq_cbs = {
2884         .rx_defq = { .cb = { .dqrr = rx_default_dqrr } },
2885         .tx_defq = { .cb = { .dqrr = conf_dflt_dqrr } },
2886         .rx_errq = { .cb = { .dqrr = rx_error_dqrr } },
2887         .tx_errq = { .cb = { .dqrr = conf_error_dqrr } },
2888         .egress_ern = { .cb = { .ern = egress_ern } }
2889 };
2890
2891 static void dpaa_eth_napi_enable(struct dpaa_priv *priv)
2892 {
2893         struct dpaa_percpu_priv *percpu_priv;
2894         int i;
2895
2896         for_each_online_cpu(i) {
2897                 percpu_priv = per_cpu_ptr(priv->percpu_priv, i);
2898
2899                 percpu_priv->np.down = false;
2900                 napi_enable(&percpu_priv->np.napi);
2901         }
2902 }
2903
2904 static void dpaa_eth_napi_disable(struct dpaa_priv *priv)
2905 {
2906         struct dpaa_percpu_priv *percpu_priv;
2907         int i;
2908
2909         for_each_online_cpu(i) {
2910                 percpu_priv = per_cpu_ptr(priv->percpu_priv, i);
2911
2912                 percpu_priv->np.down = true;
2913                 napi_disable(&percpu_priv->np.napi);
2914         }
2915 }
2916
2917 static int dpaa_open(struct net_device *net_dev)
2918 {
2919         struct mac_device *mac_dev;
2920         struct dpaa_priv *priv;
2921         int err, i;
2922
2923         priv = netdev_priv(net_dev);
2924         mac_dev = priv->mac_dev;
2925         dpaa_eth_napi_enable(priv);
2926
2927         err = phylink_of_phy_connect(mac_dev->phylink,
2928                                      mac_dev->dev->of_node, 0);
2929         if (err)
2930                 goto phy_init_failed;
2931
2932         for (i = 0; i < ARRAY_SIZE(mac_dev->port); i++) {
2933                 err = fman_port_enable(mac_dev->port[i]);
2934                 if (err)
2935                         goto mac_start_failed;
2936         }
2937
2938         err = priv->mac_dev->enable(mac_dev->fman_mac);
2939         if (err < 0) {
2940                 netif_err(priv, ifup, net_dev, "mac_dev->enable() = %d\n", err);
2941                 goto mac_start_failed;
2942         }
2943         phylink_start(mac_dev->phylink);
2944
2945         netif_tx_start_all_queues(net_dev);
2946
2947         return 0;
2948
2949 mac_start_failed:
2950         for (i = 0; i < ARRAY_SIZE(mac_dev->port); i++)
2951                 fman_port_disable(mac_dev->port[i]);
2952         phylink_disconnect_phy(mac_dev->phylink);
2953
2954 phy_init_failed:
2955         dpaa_eth_napi_disable(priv);
2956
2957         return err;
2958 }
2959
2960 static int dpaa_eth_stop(struct net_device *net_dev)
2961 {
2962         struct dpaa_priv *priv;
2963         int err;
2964
2965         err = dpaa_stop(net_dev);
2966
2967         priv = netdev_priv(net_dev);
2968         dpaa_eth_napi_disable(priv);
2969
2970         return err;
2971 }
2972
2973 static bool xdp_validate_mtu(struct dpaa_priv *priv, int mtu)
2974 {
2975         int max_contig_data = priv->dpaa_bp->size - priv->rx_headroom;
2976
2977         /* We do not support S/G fragments when XDP is enabled.
2978          * Limit the MTU in relation to the buffer size.
2979          */
2980         if (mtu + VLAN_ETH_HLEN + ETH_FCS_LEN > max_contig_data) {
2981                 dev_warn(priv->net_dev->dev.parent,
2982                          "The maximum MTU for XDP is %d\n",
2983                          max_contig_data - VLAN_ETH_HLEN - ETH_FCS_LEN);
2984                 return false;
2985         }
2986
2987         return true;
2988 }
2989
2990 static int dpaa_change_mtu(struct net_device *net_dev, int new_mtu)
2991 {
2992         struct dpaa_priv *priv = netdev_priv(net_dev);
2993
2994         if (priv->xdp_prog && !xdp_validate_mtu(priv, new_mtu))
2995                 return -EINVAL;
2996
2997         net_dev->mtu = new_mtu;
2998         return 0;
2999 }
3000
3001 static int dpaa_setup_xdp(struct net_device *net_dev, struct netdev_bpf *bpf)
3002 {
3003         struct dpaa_priv *priv = netdev_priv(net_dev);
3004         struct bpf_prog *old_prog;
3005         int err;
3006         bool up;
3007
3008         /* S/G fragments are not supported in XDP-mode */
3009         if (bpf->prog && !xdp_validate_mtu(priv, net_dev->mtu)) {
3010                 NL_SET_ERR_MSG_MOD(bpf->extack, "MTU too large for XDP");
3011                 return -EINVAL;
3012         }
3013
3014         up = netif_running(net_dev);
3015
3016         if (up)
3017                 dpaa_eth_stop(net_dev);
3018
3019         old_prog = xchg(&priv->xdp_prog, bpf->prog);
3020         if (old_prog)
3021                 bpf_prog_put(old_prog);
3022
3023         if (up) {
3024                 err = dpaa_open(net_dev);
3025                 if (err) {
3026                         NL_SET_ERR_MSG_MOD(bpf->extack, "dpaa_open() failed");
3027                         return err;
3028                 }
3029         }
3030
3031         return 0;
3032 }
3033
3034 static int dpaa_xdp(struct net_device *net_dev, struct netdev_bpf *xdp)
3035 {
3036         switch (xdp->command) {
3037         case XDP_SETUP_PROG:
3038                 return dpaa_setup_xdp(net_dev, xdp);
3039         default:
3040                 return -EINVAL;
3041         }
3042 }
3043
3044 static int dpaa_xdp_xmit(struct net_device *net_dev, int n,
3045                          struct xdp_frame **frames, u32 flags)
3046 {
3047         struct xdp_frame *xdpf;
3048         int i, nxmit = 0;
3049
3050         if (unlikely(flags & ~XDP_XMIT_FLAGS_MASK))
3051                 return -EINVAL;
3052
3053         if (!netif_running(net_dev))
3054                 return -ENETDOWN;
3055
3056         for (i = 0; i < n; i++) {
3057                 xdpf = frames[i];
3058                 if (dpaa_xdp_xmit_frame(net_dev, xdpf))
3059                         break;
3060                 nxmit++;
3061         }
3062
3063         return nxmit;
3064 }
3065
3066 static int dpaa_ts_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
3067 {
3068         struct dpaa_priv *priv = netdev_priv(dev);
3069         struct hwtstamp_config config;
3070
3071         if (copy_from_user(&config, rq->ifr_data, sizeof(config)))
3072                 return -EFAULT;
3073
3074         switch (config.tx_type) {
3075         case HWTSTAMP_TX_OFF:
3076                 /* Couldn't disable rx/tx timestamping separately.
3077                  * Do nothing here.
3078                  */
3079                 priv->tx_tstamp = false;
3080                 break;
3081         case HWTSTAMP_TX_ON:
3082                 priv->mac_dev->set_tstamp(priv->mac_dev->fman_mac, true);
3083                 priv->tx_tstamp = true;
3084                 break;
3085         default:
3086                 return -ERANGE;
3087         }
3088
3089         if (config.rx_filter == HWTSTAMP_FILTER_NONE) {
3090                 /* Couldn't disable rx/tx timestamping separately.
3091                  * Do nothing here.
3092                  */
3093                 priv->rx_tstamp = false;
3094         } else {
3095                 priv->mac_dev->set_tstamp(priv->mac_dev->fman_mac, true);
3096                 priv->rx_tstamp = true;
3097                 /* TS is set for all frame types, not only those requested */
3098                 config.rx_filter = HWTSTAMP_FILTER_ALL;
3099         }
3100
3101         return copy_to_user(rq->ifr_data, &config, sizeof(config)) ?
3102                         -EFAULT : 0;
3103 }
3104
3105 static int dpaa_ioctl(struct net_device *net_dev, struct ifreq *rq, int cmd)
3106 {
3107         int ret = -EINVAL;
3108         struct dpaa_priv *priv = netdev_priv(net_dev);
3109
3110         if (cmd == SIOCGMIIREG) {
3111                 if (net_dev->phydev)
3112                         return phylink_mii_ioctl(priv->mac_dev->phylink, rq,
3113                                                  cmd);
3114         }
3115
3116         if (cmd == SIOCSHWTSTAMP)
3117                 return dpaa_ts_ioctl(net_dev, rq, cmd);
3118
3119         return ret;
3120 }
3121
3122 static const struct net_device_ops dpaa_ops = {
3123         .ndo_open = dpaa_open,
3124         .ndo_start_xmit = dpaa_start_xmit,
3125         .ndo_stop = dpaa_eth_stop,
3126         .ndo_tx_timeout = dpaa_tx_timeout,
3127         .ndo_get_stats64 = dpaa_get_stats64,
3128         .ndo_change_carrier = fixed_phy_change_carrier,
3129         .ndo_set_mac_address = dpaa_set_mac_address,
3130         .ndo_validate_addr = eth_validate_addr,
3131         .ndo_set_rx_mode = dpaa_set_rx_mode,
3132         .ndo_eth_ioctl = dpaa_ioctl,
3133         .ndo_setup_tc = dpaa_setup_tc,
3134         .ndo_change_mtu = dpaa_change_mtu,
3135         .ndo_bpf = dpaa_xdp,
3136         .ndo_xdp_xmit = dpaa_xdp_xmit,
3137 };
3138
3139 static int dpaa_napi_add(struct net_device *net_dev)
3140 {
3141         struct dpaa_priv *priv = netdev_priv(net_dev);
3142         struct dpaa_percpu_priv *percpu_priv;
3143         int cpu;
3144
3145         for_each_possible_cpu(cpu) {
3146                 percpu_priv = per_cpu_ptr(priv->percpu_priv, cpu);
3147
3148                 netif_napi_add(net_dev, &percpu_priv->np.napi, dpaa_eth_poll);
3149         }
3150
3151         return 0;
3152 }
3153
3154 static void dpaa_napi_del(struct net_device *net_dev)
3155 {
3156         struct dpaa_priv *priv = netdev_priv(net_dev);
3157         struct dpaa_percpu_priv *percpu_priv;
3158         int cpu;
3159
3160         for_each_possible_cpu(cpu) {
3161                 percpu_priv = per_cpu_ptr(priv->percpu_priv, cpu);
3162
3163                 netif_napi_del(&percpu_priv->np.napi);
3164         }
3165 }
3166
3167 static inline void dpaa_bp_free_pf(const struct dpaa_bp *bp,
3168                                    struct bm_buffer *bmb)
3169 {
3170         dma_addr_t addr = bm_buf_addr(bmb);
3171
3172         dma_unmap_page(bp->priv->rx_dma_dev, addr, DPAA_BP_RAW_SIZE,
3173                        DMA_FROM_DEVICE);
3174
3175         skb_free_frag(phys_to_virt(addr));
3176 }
3177
3178 /* Alloc the dpaa_bp struct and configure default values */
3179 static struct dpaa_bp *dpaa_bp_alloc(struct device *dev)
3180 {
3181         struct dpaa_bp *dpaa_bp;
3182
3183         dpaa_bp = devm_kzalloc(dev, sizeof(*dpaa_bp), GFP_KERNEL);
3184         if (!dpaa_bp)
3185                 return ERR_PTR(-ENOMEM);
3186
3187         dpaa_bp->bpid = FSL_DPAA_BPID_INV;
3188         dpaa_bp->percpu_count = devm_alloc_percpu(dev, *dpaa_bp->percpu_count);
3189         if (!dpaa_bp->percpu_count)
3190                 return ERR_PTR(-ENOMEM);
3191
3192         dpaa_bp->config_count = FSL_DPAA_ETH_MAX_BUF_COUNT;
3193
3194         dpaa_bp->seed_cb = dpaa_bp_seed;
3195         dpaa_bp->free_buf_cb = dpaa_bp_free_pf;
3196
3197         return dpaa_bp;
3198 }
3199
3200 /* Place all ingress FQs (Rx Default, Rx Error) in a dedicated CGR.
3201  * We won't be sending congestion notifications to FMan; for now, we just use
3202  * this CGR to generate enqueue rejections to FMan in order to drop the frames
3203  * before they reach our ingress queues and eat up memory.
3204  */
3205 static int dpaa_ingress_cgr_init(struct dpaa_priv *priv)
3206 {
3207         struct qm_mcc_initcgr initcgr;
3208         u32 cs_th;
3209         int err;
3210
3211         err = qman_alloc_cgrid(&priv->ingress_cgr.cgrid);
3212         if (err < 0) {
3213                 if (netif_msg_drv(priv))
3214                         pr_err("Error %d allocating CGR ID\n", err);
3215                 goto out_error;
3216         }
3217
3218         /* Enable CS TD, but disable Congestion State Change Notifications. */
3219         memset(&initcgr, 0, sizeof(initcgr));
3220         initcgr.we_mask = cpu_to_be16(QM_CGR_WE_CS_THRES);
3221         initcgr.cgr.cscn_en = QM_CGR_EN;
3222         cs_th = DPAA_INGRESS_CS_THRESHOLD;
3223         qm_cgr_cs_thres_set64(&initcgr.cgr.cs_thres, cs_th, 1);
3224
3225         initcgr.we_mask |= cpu_to_be16(QM_CGR_WE_CSTD_EN);
3226         initcgr.cgr.cstd_en = QM_CGR_EN;
3227
3228         /* This CGR will be associated with the SWP affined to the current CPU.
3229          * However, we'll place all our ingress FQs in it.
3230          */
3231         err = qman_create_cgr(&priv->ingress_cgr, QMAN_CGR_FLAG_USE_INIT,
3232                               &initcgr);
3233         if (err < 0) {
3234                 if (netif_msg_drv(priv))
3235                         pr_err("Error %d creating ingress CGR with ID %d\n",
3236                                err, priv->ingress_cgr.cgrid);
3237                 qman_release_cgrid(priv->ingress_cgr.cgrid);
3238                 goto out_error;
3239         }
3240         if (netif_msg_drv(priv))
3241                 pr_debug("Created ingress CGR %d for netdev with hwaddr %pM\n",
3242                          priv->ingress_cgr.cgrid, priv->mac_dev->addr);
3243
3244         priv->use_ingress_cgr = true;
3245
3246 out_error:
3247         return err;
3248 }
3249
3250 static u16 dpaa_get_headroom(struct dpaa_buffer_layout *bl,
3251                              enum port_type port)
3252 {
3253         u16 headroom;
3254
3255         /* The frame headroom must accommodate:
3256          * - the driver private data area
3257          * - parse results, hash results, timestamp if selected
3258          * If either hash results or time stamp are selected, both will
3259          * be copied to/from the frame headroom, as TS is located between PR and
3260          * HR in the IC and IC copy size has a granularity of 16bytes
3261          * (see description of FMBM_RICP and FMBM_TICP registers in DPAARM)
3262          *
3263          * Also make sure the headroom is a multiple of data_align bytes
3264          */
3265         headroom = (u16)(bl[port].priv_data_size + DPAA_HWA_SIZE);
3266
3267         if (port == RX) {
3268 #ifdef CONFIG_DPAA_ERRATUM_A050385
3269                 if (unlikely(fman_has_errata_a050385()))
3270                         headroom = XDP_PACKET_HEADROOM;
3271 #endif
3272
3273                 return ALIGN(headroom, DPAA_FD_RX_DATA_ALIGNMENT);
3274         } else {
3275                 return ALIGN(headroom, DPAA_FD_DATA_ALIGNMENT);
3276         }
3277 }
3278
3279 static int dpaa_eth_probe(struct platform_device *pdev)
3280 {
3281         struct net_device *net_dev = NULL;
3282         struct dpaa_bp *dpaa_bp = NULL;
3283         struct dpaa_fq *dpaa_fq, *tmp;
3284         struct dpaa_priv *priv = NULL;
3285         struct fm_port_fqs port_fqs;
3286         struct mac_device *mac_dev;
3287         int err = 0, channel;
3288         struct device *dev;
3289
3290         dev = &pdev->dev;
3291
3292         err = bman_is_probed();
3293         if (!err)
3294                 return -EPROBE_DEFER;
3295         if (err < 0) {
3296                 dev_err(dev, "failing probe due to bman probe error\n");
3297                 return -ENODEV;
3298         }
3299         err = qman_is_probed();
3300         if (!err)
3301                 return -EPROBE_DEFER;
3302         if (err < 0) {
3303                 dev_err(dev, "failing probe due to qman probe error\n");
3304                 return -ENODEV;
3305         }
3306         err = bman_portals_probed();
3307         if (!err)
3308                 return -EPROBE_DEFER;
3309         if (err < 0) {
3310                 dev_err(dev,
3311                         "failing probe due to bman portals probe error\n");
3312                 return -ENODEV;
3313         }
3314         err = qman_portals_probed();
3315         if (!err)
3316                 return -EPROBE_DEFER;
3317         if (err < 0) {
3318                 dev_err(dev,
3319                         "failing probe due to qman portals probe error\n");
3320                 return -ENODEV;
3321         }
3322
3323         /* Allocate this early, so we can store relevant information in
3324          * the private area
3325          */
3326         net_dev = alloc_etherdev_mq(sizeof(*priv), DPAA_ETH_TXQ_NUM);
3327         if (!net_dev) {
3328                 dev_err(dev, "alloc_etherdev_mq() failed\n");
3329                 return -ENOMEM;
3330         }
3331
3332         /* Do this here, so we can be verbose early */
3333         SET_NETDEV_DEV(net_dev, dev->parent);
3334         dev_set_drvdata(dev, net_dev);
3335
3336         priv = netdev_priv(net_dev);
3337         priv->net_dev = net_dev;
3338
3339         priv->msg_enable = netif_msg_init(debug, DPAA_MSG_DEFAULT);
3340
3341         mac_dev = dpaa_mac_dev_get(pdev);
3342         if (IS_ERR(mac_dev)) {
3343                 netdev_err(net_dev, "dpaa_mac_dev_get() failed\n");
3344                 err = PTR_ERR(mac_dev);
3345                 goto free_netdev;
3346         }
3347
3348         /* Devices used for DMA mapping */
3349         priv->rx_dma_dev = fman_port_get_device(mac_dev->port[RX]);
3350         priv->tx_dma_dev = fman_port_get_device(mac_dev->port[TX]);
3351         err = dma_coerce_mask_and_coherent(priv->rx_dma_dev, DMA_BIT_MASK(40));
3352         if (!err)
3353                 err = dma_coerce_mask_and_coherent(priv->tx_dma_dev,
3354                                                    DMA_BIT_MASK(40));
3355         if (err) {
3356                 netdev_err(net_dev, "dma_coerce_mask_and_coherent() failed\n");
3357                 goto free_netdev;
3358         }
3359
3360         /* If fsl_fm_max_frm is set to a higher value than the all-common 1500,
3361          * we choose conservatively and let the user explicitly set a higher
3362          * MTU via ifconfig. Otherwise, the user may end up with different MTUs
3363          * in the same LAN.
3364          * If on the other hand fsl_fm_max_frm has been chosen below 1500,
3365          * start with the maximum allowed.
3366          */
3367         net_dev->mtu = min(dpaa_get_max_mtu(), ETH_DATA_LEN);
3368
3369         netdev_dbg(net_dev, "Setting initial MTU on net device: %d\n",
3370                    net_dev->mtu);
3371
3372         priv->buf_layout[RX].priv_data_size = DPAA_RX_PRIV_DATA_SIZE; /* Rx */
3373         priv->buf_layout[TX].priv_data_size = DPAA_TX_PRIV_DATA_SIZE; /* Tx */
3374
3375         /* bp init */
3376         dpaa_bp = dpaa_bp_alloc(dev);
3377         if (IS_ERR(dpaa_bp)) {
3378                 err = PTR_ERR(dpaa_bp);
3379                 goto free_dpaa_bps;
3380         }
3381         /* the raw size of the buffers used for reception */
3382         dpaa_bp->raw_size = DPAA_BP_RAW_SIZE;
3383         /* avoid runtime computations by keeping the usable size here */
3384         dpaa_bp->size = dpaa_bp_size(dpaa_bp->raw_size);
3385         dpaa_bp->priv = priv;
3386
3387         err = dpaa_bp_alloc_pool(dpaa_bp);
3388         if (err < 0)
3389                 goto free_dpaa_bps;
3390         priv->dpaa_bp = dpaa_bp;
3391
3392         INIT_LIST_HEAD(&priv->dpaa_fq_list);
3393
3394         memset(&port_fqs, 0, sizeof(port_fqs));
3395
3396         err = dpaa_alloc_all_fqs(dev, &priv->dpaa_fq_list, &port_fqs);
3397         if (err < 0) {
3398                 dev_err(dev, "dpaa_alloc_all_fqs() failed\n");
3399                 goto free_dpaa_bps;
3400         }
3401
3402         priv->mac_dev = mac_dev;
3403
3404         channel = dpaa_get_channel();
3405         if (channel < 0) {
3406                 dev_err(dev, "dpaa_get_channel() failed\n");
3407                 err = channel;
3408                 goto free_dpaa_bps;
3409         }
3410
3411         priv->channel = (u16)channel;
3412
3413         /* Walk the CPUs with affine portals
3414          * and add this pool channel to each's dequeue mask.
3415          */
3416         dpaa_eth_add_channel(priv->channel, &pdev->dev);
3417
3418         dpaa_fq_setup(priv, &dpaa_fq_cbs, priv->mac_dev->port[TX]);
3419
3420         /* Create a congestion group for this netdev, with
3421          * dynamically-allocated CGR ID.
3422          * Must be executed after probing the MAC, but before
3423          * assigning the egress FQs to the CGRs.
3424          */
3425         err = dpaa_eth_cgr_init(priv);
3426         if (err < 0) {
3427                 dev_err(dev, "Error initializing CGR\n");
3428                 goto free_dpaa_bps;
3429         }
3430
3431         err = dpaa_ingress_cgr_init(priv);
3432         if (err < 0) {
3433                 dev_err(dev, "Error initializing ingress CGR\n");
3434                 goto delete_egress_cgr;
3435         }
3436
3437         /* Add the FQs to the interface, and make them active */
3438         list_for_each_entry_safe(dpaa_fq, tmp, &priv->dpaa_fq_list, list) {
3439                 err = dpaa_fq_init(dpaa_fq, false);
3440                 if (err < 0)
3441                         goto free_dpaa_fqs;
3442         }
3443
3444         priv->tx_headroom = dpaa_get_headroom(priv->buf_layout, TX);
3445         priv->rx_headroom = dpaa_get_headroom(priv->buf_layout, RX);
3446
3447         /* All real interfaces need their ports initialized */
3448         err = dpaa_eth_init_ports(mac_dev, dpaa_bp, &port_fqs,
3449                                   &priv->buf_layout[0], dev);
3450         if (err)
3451                 goto free_dpaa_fqs;
3452
3453         /* Rx traffic distribution based on keygen hashing defaults to on */
3454         priv->keygen_in_use = true;
3455
3456         priv->percpu_priv = devm_alloc_percpu(dev, *priv->percpu_priv);
3457         if (!priv->percpu_priv) {
3458                 dev_err(dev, "devm_alloc_percpu() failed\n");
3459                 err = -ENOMEM;
3460                 goto free_dpaa_fqs;
3461         }
3462
3463         priv->num_tc = 1;
3464         netif_set_real_num_tx_queues(net_dev, priv->num_tc * DPAA_TC_TXQ_NUM);
3465
3466         /* Initialize NAPI */
3467         err = dpaa_napi_add(net_dev);
3468         if (err < 0)
3469                 goto delete_dpaa_napi;
3470
3471         err = dpaa_netdev_init(net_dev, &dpaa_ops, tx_timeout);
3472         if (err < 0)
3473                 goto delete_dpaa_napi;
3474
3475         dpaa_eth_sysfs_init(&net_dev->dev);
3476
3477         netif_info(priv, probe, net_dev, "Probed interface %s\n",
3478                    net_dev->name);
3479
3480         return 0;
3481
3482 delete_dpaa_napi:
3483         dpaa_napi_del(net_dev);
3484 free_dpaa_fqs:
3485         dpaa_fq_free(dev, &priv->dpaa_fq_list);
3486         qman_delete_cgr_safe(&priv->ingress_cgr);
3487         qman_release_cgrid(priv->ingress_cgr.cgrid);
3488 delete_egress_cgr:
3489         qman_delete_cgr_safe(&priv->cgr_data.cgr);
3490         qman_release_cgrid(priv->cgr_data.cgr.cgrid);
3491 free_dpaa_bps:
3492         dpaa_bps_free(priv);
3493 free_netdev:
3494         dev_set_drvdata(dev, NULL);
3495         free_netdev(net_dev);
3496
3497         return err;
3498 }
3499
3500 static int dpaa_remove(struct platform_device *pdev)
3501 {
3502         struct net_device *net_dev;
3503         struct dpaa_priv *priv;
3504         struct device *dev;
3505         int err;
3506
3507         dev = &pdev->dev;
3508         net_dev = dev_get_drvdata(dev);
3509
3510         priv = netdev_priv(net_dev);
3511
3512         dpaa_eth_sysfs_remove(dev);
3513
3514         dev_set_drvdata(dev, NULL);
3515         unregister_netdev(net_dev);
3516         phylink_destroy(priv->mac_dev->phylink);
3517
3518         err = dpaa_fq_free(dev, &priv->dpaa_fq_list);
3519
3520         qman_delete_cgr_safe(&priv->ingress_cgr);
3521         qman_release_cgrid(priv->ingress_cgr.cgrid);
3522         qman_delete_cgr_safe(&priv->cgr_data.cgr);
3523         qman_release_cgrid(priv->cgr_data.cgr.cgrid);
3524
3525         dpaa_napi_del(net_dev);
3526
3527         dpaa_bps_free(priv);
3528
3529         free_netdev(net_dev);
3530
3531         return err;
3532 }
3533
3534 static const struct platform_device_id dpaa_devtype[] = {
3535         {
3536                 .name = "dpaa-ethernet",
3537                 .driver_data = 0,
3538         }, {
3539         }
3540 };
3541 MODULE_DEVICE_TABLE(platform, dpaa_devtype);
3542
3543 static struct platform_driver dpaa_driver = {
3544         .driver = {
3545                 .name = KBUILD_MODNAME,
3546         },
3547         .id_table = dpaa_devtype,
3548         .probe = dpaa_eth_probe,
3549         .remove = dpaa_remove
3550 };
3551
3552 static int __init dpaa_load(void)
3553 {
3554         int err;
3555
3556         pr_debug("FSL DPAA Ethernet driver\n");
3557
3558         /* initialize dpaa_eth mirror values */
3559         dpaa_rx_extra_headroom = fman_get_rx_extra_headroom();
3560         dpaa_max_frm = fman_get_max_frm();
3561
3562         err = platform_driver_register(&dpaa_driver);
3563         if (err < 0)
3564                 pr_err("Error, platform_driver_register() = %d\n", err);
3565
3566         return err;
3567 }
3568 module_init(dpaa_load);
3569
3570 static void __exit dpaa_unload(void)
3571 {
3572         platform_driver_unregister(&dpaa_driver);
3573
3574         /* Only one channel is used and needs to be released after all
3575          * interfaces are removed
3576          */
3577         dpaa_release_channel();
3578 }
3579 module_exit(dpaa_unload);
3580
3581 MODULE_LICENSE("Dual BSD/GPL");
3582 MODULE_DESCRIPTION("FSL DPAA Ethernet driver");