dpaa2-eth.c

   1// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
   2/* Copyright 2014-2016 Freescale Semiconductor Inc.
   3 * Copyright 2016-2022 NXP
   4 */
   5#include <linux/init.h>
   6#include <linux/module.h>
   7#include <linux/platform_device.h>
   8#include <linux/etherdevice.h>
   9#include <linux/of_net.h>
  10#include <linux/interrupt.h>
  11#include <linux/kthread.h>
  12#include <linux/iommu.h>
  13#include <linux/fsl/mc.h>
  14#include <linux/bpf.h>
  15#include <linux/bpf_trace.h>
  16#include <linux/fsl/ptp_qoriq.h>
  17#include <linux/ptp_classify.h>
  18#include <net/pkt_cls.h>
  19#include <net/sock.h>
  20#include <net/tso.h>
  21#include <net/xdp_sock_drv.h>
  22
  23#include "dpaa2-eth.h"
  24
  25/* CREATE_TRACE_POINTS only needs to be defined once. Other dpa files
  26 * using trace events only need to #include <trace/events/sched.h>
  27 */
  28#define CREATE_TRACE_POINTS
  29#include "dpaa2-eth-trace.h"
  30
  31MODULE_LICENSE("Dual BSD/GPL");
  32MODULE_AUTHOR("Freescale Semiconductor, Inc");
  33MODULE_DESCRIPTION("Freescale DPAA2 Ethernet Driver");
  34
  35struct ptp_qoriq *dpaa2_ptp;
  36EXPORT_SYMBOL(dpaa2_ptp);
  37
  38static void dpaa2_eth_detect_features(struct dpaa2_eth_priv *priv)
  39{
  40	priv->features = 0;
  41
  42	if (dpaa2_eth_cmp_dpni_ver(priv, DPNI_PTP_ONESTEP_VER_MAJOR,
  43				   DPNI_PTP_ONESTEP_VER_MINOR) >= 0)
  44		priv->features |= DPAA2_ETH_FEATURE_ONESTEP_CFG_DIRECT;
  45}
  46
  47static void dpaa2_update_ptp_onestep_indirect(struct dpaa2_eth_priv *priv,
  48					      u32 offset, u8 udp)
  49{
  50	struct dpni_single_step_cfg cfg;
  51
  52	cfg.en = 1;
  53	cfg.ch_update = udp;
  54	cfg.offset = offset;
  55	cfg.peer_delay = 0;
  56
  57	if (dpni_set_single_step_cfg(priv->mc_io, 0, priv->mc_token, &cfg))
  58		WARN_ONCE(1, "Failed to set single step register");
  59}
  60
  61static void dpaa2_update_ptp_onestep_direct(struct dpaa2_eth_priv *priv,
  62					    u32 offset, u8 udp)
  63{
  64	u32 val = 0;
  65
  66	val = DPAA2_PTP_SINGLE_STEP_ENABLE |
  67	       DPAA2_PTP_SINGLE_CORRECTION_OFF(offset);
  68
  69	if (udp)
  70		val |= DPAA2_PTP_SINGLE_STEP_CH;
  71
  72	if (priv->onestep_reg_base)
  73		writel(val, priv->onestep_reg_base);
  74}
  75
  76static void dpaa2_ptp_onestep_reg_update_method(struct dpaa2_eth_priv *priv)
  77{
  78	struct device *dev = priv->net_dev->dev.parent;
  79	struct dpni_single_step_cfg ptp_cfg;
  80
  81	priv->dpaa2_set_onestep_params_cb = dpaa2_update_ptp_onestep_indirect;
  82
  83	if (!(priv->features & DPAA2_ETH_FEATURE_ONESTEP_CFG_DIRECT))
  84		return;
  85
  86	if (dpni_get_single_step_cfg(priv->mc_io, 0,
  87				     priv->mc_token, &ptp_cfg)) {
  88		dev_err(dev, "dpni_get_single_step_cfg cannot retrieve onestep reg, falling back to indirect update\n");
  89		return;
  90	}
  91
  92	if (!ptp_cfg.ptp_onestep_reg_base) {
  93		dev_err(dev, "1588 onestep reg not available, falling back to indirect update\n");
  94		return;
  95	}
  96
  97	priv->onestep_reg_base = ioremap(ptp_cfg.ptp_onestep_reg_base,
  98					 sizeof(u32));
  99	if (!priv->onestep_reg_base) {
 100		dev_err(dev, "1588 onestep reg cannot be mapped, falling back to indirect update\n");
 101		return;
 102	}
 103
 104	priv->dpaa2_set_onestep_params_cb = dpaa2_update_ptp_onestep_direct;
 105}
 106
 107void *dpaa2_iova_to_virt(struct iommu_domain *domain,
 108			 dma_addr_t iova_addr)
 109{
 110	phys_addr_t phys_addr;
 111
 112	phys_addr = domain ? iommu_iova_to_phys(domain, iova_addr) : iova_addr;
 113
 114	return phys_to_virt(phys_addr);
 115}
 116
 117static void dpaa2_eth_validate_rx_csum(struct dpaa2_eth_priv *priv,
 118				       u32 fd_status,
 119				       struct sk_buff *skb)
 120{
 121	skb_checksum_none_assert(skb);
 122
 123	/* HW checksum validation is disabled, nothing to do here */
 124	if (!(priv->net_dev->features & NETIF_F_RXCSUM))
 125		return;
 126
 127	/* Read checksum validation bits */
 128	if (!((fd_status & DPAA2_FAS_L3CV) &&
 129	      (fd_status & DPAA2_FAS_L4CV)))
 130		return;
 131
 132	/* Inform the stack there's no need to compute L3/L4 csum anymore */
 133	skb->ip_summed = CHECKSUM_UNNECESSARY;
 134}
 135
 136/* Free a received FD.
 137 * Not to be used for Tx conf FDs or on any other paths.
 138 */
 139static void dpaa2_eth_free_rx_fd(struct dpaa2_eth_priv *priv,
 140				 const struct dpaa2_fd *fd,
 141				 void *vaddr)
 142{
 143	struct device *dev = priv->net_dev->dev.parent;
 144	dma_addr_t addr = dpaa2_fd_get_addr(fd);
 145	u8 fd_format = dpaa2_fd_get_format(fd);
 146	struct dpaa2_sg_entry *sgt;
 147	void *sg_vaddr;
 148	int i;
 149
 150	/* If single buffer frame, just free the data buffer */
 151	if (fd_format == dpaa2_fd_single)
 152		goto free_buf;
 153	else if (fd_format != dpaa2_fd_sg)
 154		/* We don't support any other format */
 155		return;
 156
 157	/* For S/G frames, we first need to free all SG entries
 158	 * except the first one, which was taken care of already
 159	 */
 160	sgt = vaddr + dpaa2_fd_get_offset(fd);
 161	for (i = 1; i < DPAA2_ETH_MAX_SG_ENTRIES; i++) {
 162		addr = dpaa2_sg_get_addr(&sgt[i]);
 163		sg_vaddr = dpaa2_iova_to_virt(priv->iommu_domain, addr);
 164		dma_unmap_page(dev, addr, priv->rx_buf_size,
 165			       DMA_BIDIRECTIONAL);
 166
 167		free_pages((unsigned long)sg_vaddr, 0);
 168		if (dpaa2_sg_is_final(&sgt[i]))
 169			break;
 170	}
 171
 172free_buf:
 173	free_pages((unsigned long)vaddr, 0);
 174}
 175
 176/* Build a linear skb based on a single-buffer frame descriptor */
 177static struct sk_buff *dpaa2_eth_build_linear_skb(struct dpaa2_eth_channel *ch,
 178						  const struct dpaa2_fd *fd,
 179						  void *fd_vaddr)
 180{
 181	struct sk_buff *skb = NULL;
 182	u16 fd_offset = dpaa2_fd_get_offset(fd);
 183	u32 fd_length = dpaa2_fd_get_len(fd);
 184
 185	ch->buf_count--;
 186
 187	skb = build_skb(fd_vaddr, DPAA2_ETH_RX_BUF_RAW_SIZE);
 188	if (unlikely(!skb))
 189		return NULL;
 190
 191	skb_reserve(skb, fd_offset);
 192	skb_put(skb, fd_length);
 193
 194	return skb;
 195}
 196
 197/* Build a non linear (fragmented) skb based on a S/G table */
 198static struct sk_buff *dpaa2_eth_build_frag_skb(struct dpaa2_eth_priv *priv,
 199						struct dpaa2_eth_channel *ch,
 200						struct dpaa2_sg_entry *sgt)
 201{
 202	struct sk_buff *skb = NULL;
 203	struct device *dev = priv->net_dev->dev.parent;
 204	void *sg_vaddr;
 205	dma_addr_t sg_addr;
 206	u16 sg_offset;
 207	u32 sg_length;
 208	struct page *page, *head_page;
 209	int page_offset;
 210	int i;
 211
 212	for (i = 0; i < DPAA2_ETH_MAX_SG_ENTRIES; i++) {
 213		struct dpaa2_sg_entry *sge = &sgt[i];
 214
 215		/* NOTE: We only support SG entries in dpaa2_sg_single format,
 216		 * but this is the only format we may receive from HW anyway
 217		 */
 218
 219		/* Get the address and length from the S/G entry */
 220		sg_addr = dpaa2_sg_get_addr(sge);
 221		sg_vaddr = dpaa2_iova_to_virt(priv->iommu_domain, sg_addr);
 222		dma_unmap_page(dev, sg_addr, priv->rx_buf_size,
 223			       DMA_BIDIRECTIONAL);
 224
 225		sg_length = dpaa2_sg_get_len(sge);
 226
 227		if (i == 0) {
 228			/* We build the skb around the first data buffer */
 229			skb = build_skb(sg_vaddr, DPAA2_ETH_RX_BUF_RAW_SIZE);
 230			if (unlikely(!skb)) {
 231				/* Free the first SG entry now, since we already
 232				 * unmapped it and obtained the virtual address
 233				 */
 234				free_pages((unsigned long)sg_vaddr, 0);
 235
 236				/* We still need to subtract the buffers used
 237				 * by this FD from our software counter
 238				 */
 239				while (!dpaa2_sg_is_final(&sgt[i]) &&
 240				       i < DPAA2_ETH_MAX_SG_ENTRIES)
 241					i++;
 242				break;
 243			}
 244
 245			sg_offset = dpaa2_sg_get_offset(sge);
 246			skb_reserve(skb, sg_offset);
 247			skb_put(skb, sg_length);
 248		} else {
 249			/* Rest of the data buffers are stored as skb frags */
 250			page = virt_to_page(sg_vaddr);
 251			head_page = virt_to_head_page(sg_vaddr);
 252
 253			/* Offset in page (which may be compound).
 254			 * Data in subsequent SG entries is stored from the
 255			 * beginning of the buffer, so we don't need to add the
 256			 * sg_offset.
 257			 */
 258			page_offset = ((unsigned long)sg_vaddr &
 259				(PAGE_SIZE - 1)) +
 260				(page_address(page) - page_address(head_page));
 261
 262			skb_add_rx_frag(skb, i - 1, head_page, page_offset,
 263					sg_length, priv->rx_buf_size);
 264		}
 265
 266		if (dpaa2_sg_is_final(sge))
 267			break;
 268	}
 269
 270	WARN_ONCE(i == DPAA2_ETH_MAX_SG_ENTRIES, "Final bit not set in SGT");
 271
 272	/* Count all data buffers + SG table buffer */
 273	ch->buf_count -= i + 2;
 274
 275	return skb;
 276}
 277
 278/* Free buffers acquired from the buffer pool or which were meant to
 279 * be released in the pool
 280 */
 281static void dpaa2_eth_free_bufs(struct dpaa2_eth_priv *priv, u64 *buf_array,
 282				int count, bool xsk_zc)
 283{
 284	struct device *dev = priv->net_dev->dev.parent;
 285	struct dpaa2_eth_swa *swa;
 286	struct xdp_buff *xdp_buff;
 287	void *vaddr;
 288	int i;
 289
 290	for (i = 0; i < count; i++) {
 291		vaddr = dpaa2_iova_to_virt(priv->iommu_domain, buf_array[i]);
 292
 293		if (!xsk_zc) {
 294			dma_unmap_page(dev, buf_array[i], priv->rx_buf_size,
 295				       DMA_BIDIRECTIONAL);
 296			free_pages((unsigned long)vaddr, 0);
 297		} else {
 298			swa = (struct dpaa2_eth_swa *)
 299				(vaddr + DPAA2_ETH_RX_HWA_SIZE);
 300			xdp_buff = swa->xsk.xdp_buff;
 301			xsk_buff_free(xdp_buff);
 302		}
 303	}
 304}
 305
 306void dpaa2_eth_recycle_buf(struct dpaa2_eth_priv *priv,
 307			   struct dpaa2_eth_channel *ch,
 308			   dma_addr_t addr)
 309{
 310	int retries = 0;
 311	int err;
 312
 313	ch->recycled_bufs[ch->recycled_bufs_cnt++] = addr;
 314	if (ch->recycled_bufs_cnt < DPAA2_ETH_BUFS_PER_CMD)
 315		return;
 316
 317	while ((err = dpaa2_io_service_release(ch->dpio, ch->bp->bpid,
 318					       ch->recycled_bufs,
 319					       ch->recycled_bufs_cnt)) == -EBUSY) {
 320		if (retries++ >= DPAA2_ETH_SWP_BUSY_RETRIES)
 321			break;
 322		cpu_relax();
 323	}
 324
 325	if (err) {
 326		dpaa2_eth_free_bufs(priv, ch->recycled_bufs,
 327				    ch->recycled_bufs_cnt, ch->xsk_zc);
 328		ch->buf_count -= ch->recycled_bufs_cnt;
 329	}
 330
 331	ch->recycled_bufs_cnt = 0;
 332}
 333
 334static int dpaa2_eth_xdp_flush(struct dpaa2_eth_priv *priv,
 335			       struct dpaa2_eth_fq *fq,
 336			       struct dpaa2_eth_xdp_fds *xdp_fds)
 337{
 338	int total_enqueued = 0, retries = 0, enqueued;
 339	struct dpaa2_eth_drv_stats *percpu_extras;
 340	int num_fds, err, max_retries;
 341	struct dpaa2_fd *fds;
 342
 343	percpu_extras = this_cpu_ptr(priv->percpu_extras);
 344
 345	/* try to enqueue all the FDs until the max number of retries is hit */
 346	fds = xdp_fds->fds;
 347	num_fds = xdp_fds->num;
 348	max_retries = num_fds * DPAA2_ETH_ENQUEUE_RETRIES;
 349	while (total_enqueued < num_fds && retries < max_retries) {
 350		err = priv->enqueue(priv, fq, &fds[total_enqueued],
 351				    0, num_fds - total_enqueued, &enqueued);
 352		if (err == -EBUSY) {
 353			percpu_extras->tx_portal_busy += ++retries;
 354			continue;
 355		}
 356		total_enqueued += enqueued;
 357	}
 358	xdp_fds->num = 0;
 359
 360	return total_enqueued;
 361}
 362
 363static void dpaa2_eth_xdp_tx_flush(struct dpaa2_eth_priv *priv,
 364				   struct dpaa2_eth_channel *ch,
 365				   struct dpaa2_eth_fq *fq)
 366{
 367	struct rtnl_link_stats64 *percpu_stats;
 368	struct dpaa2_fd *fds;
 369	int enqueued, i;
 370
 371	percpu_stats = this_cpu_ptr(priv->percpu_stats);
 372
 373	// enqueue the array of XDP_TX frames
 374	enqueued = dpaa2_eth_xdp_flush(priv, fq, &fq->xdp_tx_fds);
 375
 376	/* update statistics */
 377	percpu_stats->tx_packets += enqueued;
 378	fds = fq->xdp_tx_fds.fds;
 379	for (i = 0; i < enqueued; i++) {
 380		percpu_stats->tx_bytes += dpaa2_fd_get_len(&fds[i]);
 381		ch->stats.xdp_tx++;
 382	}
 383	for (i = enqueued; i < fq->xdp_tx_fds.num; i++) {
 384		dpaa2_eth_recycle_buf(priv, ch, dpaa2_fd_get_addr(&fds[i]));
 385		percpu_stats->tx_errors++;
 386		ch->stats.xdp_tx_err++;
 387	}
 388	fq->xdp_tx_fds.num = 0;
 389}
 390
 391void dpaa2_eth_xdp_enqueue(struct dpaa2_eth_priv *priv,
 392			   struct dpaa2_eth_channel *ch,
 393			   struct dpaa2_fd *fd,
 394			   void *buf_start, u16 queue_id)
 395{
 396	struct dpaa2_faead *faead;
 397	struct dpaa2_fd *dest_fd;
 398	struct dpaa2_eth_fq *fq;
 399	u32 ctrl, frc;
 400
 401	/* Mark the egress frame hardware annotation area as valid */
 402	frc = dpaa2_fd_get_frc(fd);
 403	dpaa2_fd_set_frc(fd, frc | DPAA2_FD_FRC_FAEADV);
 404	dpaa2_fd_set_ctrl(fd, DPAA2_FD_CTRL_ASAL);
 405
 406	/* Instruct hardware to release the FD buffer directly into
 407	 * the buffer pool once transmission is completed, instead of
 408	 * sending a Tx confirmation frame to us
 409	 */
 410	ctrl = DPAA2_FAEAD_A4V | DPAA2_FAEAD_A2V | DPAA2_FAEAD_EBDDV;
 411	faead = dpaa2_get_faead(buf_start, false);
 412	faead->ctrl = cpu_to_le32(ctrl);
 413	faead->conf_fqid = 0;
 414
 415	fq = &priv->fq[queue_id];
 416	dest_fd = &fq->xdp_tx_fds.fds[fq->xdp_tx_fds.num++];
 417	memcpy(dest_fd, fd, sizeof(*dest_fd));
 418
 419	if (fq->xdp_tx_fds.num < DEV_MAP_BULK_SIZE)
 420		return;
 421
 422	dpaa2_eth_xdp_tx_flush(priv, ch, fq);
 423}
 424
 425static u32 dpaa2_eth_run_xdp(struct dpaa2_eth_priv *priv,
 426			     struct dpaa2_eth_channel *ch,
 427			     struct dpaa2_eth_fq *rx_fq,
 428			     struct dpaa2_fd *fd, void *vaddr)
 429{
 430	dma_addr_t addr = dpaa2_fd_get_addr(fd);
 431	struct bpf_prog *xdp_prog;
 432	struct xdp_buff xdp;
 433	u32 xdp_act = XDP_PASS;
 434	int err, offset;
 435
 436	xdp_prog = READ_ONCE(ch->xdp.prog);
 437	if (!xdp_prog)
 438		goto out;
 439
 440	offset = dpaa2_fd_get_offset(fd) - XDP_PACKET_HEADROOM;
 441	xdp_init_buff(&xdp, DPAA2_ETH_RX_BUF_RAW_SIZE - offset, &ch->xdp_rxq);
 442	xdp_prepare_buff(&xdp, vaddr + offset, XDP_PACKET_HEADROOM,
 443			 dpaa2_fd_get_len(fd), false);
 444
 445	xdp_act = bpf_prog_run_xdp(xdp_prog, &xdp);
 446
 447	/* xdp.data pointer may have changed */
 448	dpaa2_fd_set_offset(fd, xdp.data - vaddr);
 449	dpaa2_fd_set_len(fd, xdp.data_end - xdp.data);
 450
 451	switch (xdp_act) {
 452	case XDP_PASS:
 453		break;
 454	case XDP_TX:
 455		dpaa2_eth_xdp_enqueue(priv, ch, fd, vaddr, rx_fq->flowid);
 456		break;
 457	default:
 458		bpf_warn_invalid_xdp_action(priv->net_dev, xdp_prog, xdp_act);
 459		fallthrough;
 460	case XDP_ABORTED:
 461		trace_xdp_exception(priv->net_dev, xdp_prog, xdp_act);
 462		fallthrough;
 463	case XDP_DROP:
 464		dpaa2_eth_recycle_buf(priv, ch, addr);
 465		ch->stats.xdp_drop++;
 466		break;
 467	case XDP_REDIRECT:
 468		dma_unmap_page(priv->net_dev->dev.parent, addr,
 469			       priv->rx_buf_size, DMA_BIDIRECTIONAL);
 470		ch->buf_count--;
 471
 472		/* Allow redirect use of full headroom */
 473		xdp.data_hard_start = vaddr;
 474		xdp.frame_sz = DPAA2_ETH_RX_BUF_RAW_SIZE;
 475
 476		err = xdp_do_redirect(priv->net_dev, &xdp, xdp_prog);
 477		if (unlikely(err)) {
 478			addr = dma_map_page(priv->net_dev->dev.parent,
 479					    virt_to_page(vaddr), 0,
 480					    priv->rx_buf_size, DMA_BIDIRECTIONAL);
 481			if (unlikely(dma_mapping_error(priv->net_dev->dev.parent, addr))) {
 482				free_pages((unsigned long)vaddr, 0);
 483			} else {
 484				ch->buf_count++;
 485				dpaa2_eth_recycle_buf(priv, ch, addr);
 486			}
 487			ch->stats.xdp_drop++;
 488		} else {
 489			ch->stats.xdp_redirect++;
 490		}
 491		break;
 492	}
 493
 494	ch->xdp.res |= xdp_act;
 495out:
 496	return xdp_act;
 497}
 498
 499struct sk_buff *dpaa2_eth_alloc_skb(struct dpaa2_eth_priv *priv,
 500				    struct dpaa2_eth_channel *ch,
 501				    const struct dpaa2_fd *fd, u32 fd_length,
 502				    void *fd_vaddr)
 503{
 504	u16 fd_offset = dpaa2_fd_get_offset(fd);
 505	struct sk_buff *skb = NULL;
 506	unsigned int skb_len;
 507
 508	skb_len = fd_length + dpaa2_eth_needed_headroom(NULL);
 509
 510	skb = napi_alloc_skb(&ch->napi, skb_len);
 511	if (!skb)
 512		return NULL;
 513
 514	skb_reserve(skb, dpaa2_eth_needed_headroom(NULL));
 515	skb_put(skb, fd_length);
 516
 517	memcpy(skb->data, fd_vaddr + fd_offset, fd_length);
 518
 519	return skb;
 520}
 521
 522static struct sk_buff *dpaa2_eth_copybreak(struct dpaa2_eth_channel *ch,
 523					   const struct dpaa2_fd *fd,
 524					   void *fd_vaddr)
 525{
 526	struct dpaa2_eth_priv *priv = ch->priv;
 527	u32 fd_length = dpaa2_fd_get_len(fd);
 528
 529	if (fd_length > priv->rx_copybreak)
 530		return NULL;
 531
 532	return dpaa2_eth_alloc_skb(priv, ch, fd, fd_length, fd_vaddr);
 533}
 534
 535void dpaa2_eth_receive_skb(struct dpaa2_eth_priv *priv,
 536			   struct dpaa2_eth_channel *ch,
 537			   const struct dpaa2_fd *fd, void *vaddr,
 538			   struct dpaa2_eth_fq *fq,
 539			   struct rtnl_link_stats64 *percpu_stats,
 540			   struct sk_buff *skb)
 541{
 542	struct dpaa2_fas *fas;
 543	u32 status = 0;
 544
 545	fas = dpaa2_get_fas(vaddr, false);
 546	prefetch(fas);
 547	prefetch(skb->data);
 548
 549	/* Get the timestamp value */
 550	if (priv->rx_tstamp) {
 551		struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb);
 552		__le64 *ts = dpaa2_get_ts(vaddr, false);
 553		u64 ns;
 554
 555		memset(shhwtstamps, 0, sizeof(*shhwtstamps));
 556
 557		ns = DPAA2_PTP_CLK_PERIOD_NS * le64_to_cpup(ts);
 558		shhwtstamps->hwtstamp = ns_to_ktime(ns);
 559	}
 560
 561	/* Check if we need to validate the L4 csum */
 562	if (likely(dpaa2_fd_get_frc(fd) & DPAA2_FD_FRC_FASV)) {
 563		status = le32_to_cpu(fas->status);
 564		dpaa2_eth_validate_rx_csum(priv, status, skb);
 565	}
 566
 567	skb->protocol = eth_type_trans(skb, priv->net_dev);
 568	skb_record_rx_queue(skb, fq->flowid);
 569
 570	percpu_stats->rx_packets++;
 571	percpu_stats->rx_bytes += dpaa2_fd_get_len(fd);
 572	ch->stats.bytes_per_cdan += dpaa2_fd_get_len(fd);
 573
 574	list_add_tail(&skb->list, ch->rx_list);
 575}
 576
 577/* Main Rx frame processing routine */
 578void dpaa2_eth_rx(struct dpaa2_eth_priv *priv,
 579		  struct dpaa2_eth_channel *ch,
 580		  const struct dpaa2_fd *fd,
 581		  struct dpaa2_eth_fq *fq)
 582{
 583	dma_addr_t addr = dpaa2_fd_get_addr(fd);
 584	u8 fd_format = dpaa2_fd_get_format(fd);
 585	void *vaddr;
 586	struct sk_buff *skb;
 587	struct rtnl_link_stats64 *percpu_stats;
 588	struct dpaa2_eth_drv_stats *percpu_extras;
 589	struct device *dev = priv->net_dev->dev.parent;
 590	bool recycle_rx_buf = false;
 591	void *buf_data;
 592	u32 xdp_act;
 593
 594	/* Tracing point */
 595	trace_dpaa2_rx_fd(priv->net_dev, fd);
 596
 597	vaddr = dpaa2_iova_to_virt(priv->iommu_domain, addr);
 598	dma_sync_single_for_cpu(dev, addr, priv->rx_buf_size,
 599				DMA_BIDIRECTIONAL);
 600
 601	buf_data = vaddr + dpaa2_fd_get_offset(fd);
 602	prefetch(buf_data);
 603
 604	percpu_stats = this_cpu_ptr(priv->percpu_stats);
 605	percpu_extras = this_cpu_ptr(priv->percpu_extras);
 606
 607	if (fd_format == dpaa2_fd_single) {
 608		xdp_act = dpaa2_eth_run_xdp(priv, ch, fq, (struct dpaa2_fd *)fd, vaddr);
 609		if (xdp_act != XDP_PASS) {
 610			percpu_stats->rx_packets++;
 611			percpu_stats->rx_bytes += dpaa2_fd_get_len(fd);
 612			return;
 613		}
 614
 615		skb = dpaa2_eth_copybreak(ch, fd, vaddr);
 616		if (!skb) {
 617			dma_unmap_page(dev, addr, priv->rx_buf_size,
 618				       DMA_BIDIRECTIONAL);
 619			skb = dpaa2_eth_build_linear_skb(ch, fd, vaddr);
 620		} else {
 621			recycle_rx_buf = true;
 622		}
 623	} else if (fd_format == dpaa2_fd_sg) {
 624		WARN_ON(priv->xdp_prog);
 625
 626		dma_unmap_page(dev, addr, priv->rx_buf_size,
 627			       DMA_BIDIRECTIONAL);
 628		skb = dpaa2_eth_build_frag_skb(priv, ch, buf_data);
 629		free_pages((unsigned long)vaddr, 0);
 630		percpu_extras->rx_sg_frames++;
 631		percpu_extras->rx_sg_bytes += dpaa2_fd_get_len(fd);
 632	} else {
 633		/* We don't support any other format */
 634		goto err_frame_format;
 635	}
 636
 637	if (unlikely(!skb))
 638		goto err_build_skb;
 639
 640	dpaa2_eth_receive_skb(priv, ch, fd, vaddr, fq, percpu_stats, skb);
 641
 642	if (recycle_rx_buf)
 643		dpaa2_eth_recycle_buf(priv, ch, dpaa2_fd_get_addr(fd));
 644	return;
 645
 646err_build_skb:
 647	dpaa2_eth_free_rx_fd(priv, fd, vaddr);
 648err_frame_format:
 649	percpu_stats->rx_dropped++;
 650}
 651
 652/* Processing of Rx frames received on the error FQ
 653 * We check and print the error bits and then free the frame
 654 */
 655static void dpaa2_eth_rx_err(struct dpaa2_eth_priv *priv,
 656			     struct dpaa2_eth_channel *ch,
 657			     const struct dpaa2_fd *fd,
 658			     struct dpaa2_eth_fq *fq __always_unused)
 659{
 660	struct device *dev = priv->net_dev->dev.parent;
 661	dma_addr_t addr = dpaa2_fd_get_addr(fd);
 662	u8 fd_format = dpaa2_fd_get_format(fd);
 663	struct rtnl_link_stats64 *percpu_stats;
 664	struct dpaa2_eth_trap_item *trap_item;
 665	struct dpaa2_fapr *fapr;
 666	struct sk_buff *skb;
 667	void *buf_data;
 668	void *vaddr;
 669
 670	vaddr = dpaa2_iova_to_virt(priv->iommu_domain, addr);
 671	dma_sync_single_for_cpu(dev, addr, priv->rx_buf_size,
 672				DMA_BIDIRECTIONAL);
 673
 674	buf_data = vaddr + dpaa2_fd_get_offset(fd);
 675
 676	if (fd_format == dpaa2_fd_single) {
 677		dma_unmap_page(dev, addr, priv->rx_buf_size,
 678			       DMA_BIDIRECTIONAL);
 679		skb = dpaa2_eth_build_linear_skb(ch, fd, vaddr);
 680	} else if (fd_format == dpaa2_fd_sg) {
 681		dma_unmap_page(dev, addr, priv->rx_buf_size,
 682			       DMA_BIDIRECTIONAL);
 683		skb = dpaa2_eth_build_frag_skb(priv, ch, buf_data);
 684		free_pages((unsigned long)vaddr, 0);
 685	} else {
 686		/* We don't support any other format */
 687		dpaa2_eth_free_rx_fd(priv, fd, vaddr);
 688		goto err_frame_format;
 689	}
 690
 691	fapr = dpaa2_get_fapr(vaddr, false);
 692	trap_item = dpaa2_eth_dl_get_trap(priv, fapr);
 693	if (trap_item)
 694		devlink_trap_report(priv->devlink, skb, trap_item->trap_ctx,
 695				    &priv->devlink_port, NULL);
 696	consume_skb(skb);
 697
 698err_frame_format:
 699	percpu_stats = this_cpu_ptr(priv->percpu_stats);
 700	percpu_stats->rx_errors++;
 701	ch->buf_count--;
 702}
 703
 704/* Consume all frames pull-dequeued into the store. This is the simplest way to
 705 * make sure we don't accidentally issue another volatile dequeue which would
 706 * overwrite (leak) frames already in the store.
 707 *
 708 * Observance of NAPI budget is not our concern, leaving that to the caller.
 709 */
 710static int dpaa2_eth_consume_frames(struct dpaa2_eth_channel *ch,
 711				    struct dpaa2_eth_fq **src)
 712{
 713	struct dpaa2_eth_priv *priv = ch->priv;
 714	struct dpaa2_eth_fq *fq = NULL;
 715	struct dpaa2_dq *dq;
 716	const struct dpaa2_fd *fd;
 717	int cleaned = 0, retries = 0;
 718	int is_last;
 719
 720	do {
 721		dq = dpaa2_io_store_next(ch->store, &is_last);
 722		if (unlikely(!dq)) {
 723			/* If we're here, we *must* have placed a
 724			 * volatile dequeue comnmand, so keep reading through
 725			 * the store until we get some sort of valid response
 726			 * token (either a valid frame or an "empty dequeue")
 727			 */
 728			if (retries++ >= DPAA2_ETH_SWP_BUSY_RETRIES) {
 729				netdev_err_once(priv->net_dev,
 730						"Unable to read a valid dequeue response\n");
 731				return -ETIMEDOUT;
 732			}
 733			continue;
 734		}
 735
 736		fd = dpaa2_dq_fd(dq);
 737		fq = (struct dpaa2_eth_fq *)(uintptr_t)dpaa2_dq_fqd_ctx(dq);
 738
 739		fq->consume(priv, ch, fd, fq);
 740		cleaned++;
 741		retries = 0;
 742	} while (!is_last);
 743
 744	if (!cleaned)
 745		return 0;
 746
 747	fq->stats.frames += cleaned;
 748	ch->stats.frames += cleaned;
 749	ch->stats.frames_per_cdan += cleaned;
 750
 751	/* A dequeue operation only pulls frames from a single queue
 752	 * into the store. Return the frame queue as an out param.
 753	 */
 754	if (src)
 755		*src = fq;
 756
 757	return cleaned;
 758}
 759
 760static int dpaa2_eth_ptp_parse(struct sk_buff *skb,
 761			       u8 *msgtype, u8 *twostep, u8 *udp,
 762			       u16 *correction_offset,
 763			       u16 *origintimestamp_offset)
 764{
 765	unsigned int ptp_class;
 766	struct ptp_header *hdr;
 767	unsigned int type;
 768	u8 *base;
 769
 770	ptp_class = ptp_classify_raw(skb);
 771	if (ptp_class == PTP_CLASS_NONE)
 772		return -EINVAL;
 773
 774	hdr = ptp_parse_header(skb, ptp_class);
 775	if (!hdr)
 776		return -EINVAL;
 777
 778	*msgtype = ptp_get_msgtype(hdr, ptp_class);
 779	*twostep = hdr->flag_field[0] & 0x2;
 780
 781	type = ptp_class & PTP_CLASS_PMASK;
 782	if (type == PTP_CLASS_IPV4 ||
 783	    type == PTP_CLASS_IPV6)
 784		*udp = 1;
 785	else
 786		*udp = 0;
 787
 788	base = skb_mac_header(skb);
 789	*correction_offset = (u8 *)&hdr->correction - base;
 790	*origintimestamp_offset = (u8 *)hdr + sizeof(struct ptp_header) - base;
 791
 792	return 0;
 793}
 794
 795/* Configure the egress frame annotation for timestamp update */
 796static void dpaa2_eth_enable_tx_tstamp(struct dpaa2_eth_priv *priv,
 797				       struct dpaa2_fd *fd,
 798				       void *buf_start,
 799				       struct sk_buff *skb)
 800{
 801	struct ptp_tstamp origin_timestamp;
 802	u8 msgtype, twostep, udp;
 803	struct dpaa2_faead *faead;
 804	struct dpaa2_fas *fas;
 805	struct timespec64 ts;
 806	u16 offset1, offset2;
 807	u32 ctrl, frc;
 808	__le64 *ns;
 809	u8 *data;
 810
 811	/* Mark the egress frame annotation area as valid */
 812	frc = dpaa2_fd_get_frc(fd);
 813	dpaa2_fd_set_frc(fd, frc | DPAA2_FD_FRC_FAEADV);
 814
 815	/* Set hardware annotation size */
 816	ctrl = dpaa2_fd_get_ctrl(fd);
 817	dpaa2_fd_set_ctrl(fd, ctrl | DPAA2_FD_CTRL_ASAL);
 818
 819	/* enable UPD (update prepanded data) bit in FAEAD field of
 820	 * hardware frame annotation area
 821	 */
 822	ctrl = DPAA2_FAEAD_A2V | DPAA2_FAEAD_UPDV | DPAA2_FAEAD_UPD;
 823	faead = dpaa2_get_faead(buf_start, true);
 824	faead->ctrl = cpu_to_le32(ctrl);
 825
 826	if (skb->cb[0] == TX_TSTAMP_ONESTEP_SYNC) {
 827		if (dpaa2_eth_ptp_parse(skb, &msgtype, &twostep, &udp,
 828					&offset1, &offset2) ||
 829		    msgtype != PTP_MSGTYPE_SYNC || twostep) {
 830			WARN_ONCE(1, "Bad packet for one-step timestamping\n");
 831			return;
 832		}
 833
 834		/* Mark the frame annotation status as valid */
 835		frc = dpaa2_fd_get_frc(fd);
 836		dpaa2_fd_set_frc(fd, frc | DPAA2_FD_FRC_FASV);
 837
 838		/* Mark the PTP flag for one step timestamping */
 839		fas = dpaa2_get_fas(buf_start, true);
 840		fas->status = cpu_to_le32(DPAA2_FAS_PTP);
 841
 842		dpaa2_ptp->caps.gettime64(&dpaa2_ptp->caps, &ts);
 843		ns = dpaa2_get_ts(buf_start, true);
 844		*ns = cpu_to_le64(timespec64_to_ns(&ts) /
 845				  DPAA2_PTP_CLK_PERIOD_NS);
 846
 847		/* Update current time to PTP message originTimestamp field */
 848		ns_to_ptp_tstamp(&origin_timestamp, le64_to_cpup(ns));
 849		data = skb_mac_header(skb);
 850		*(__be16 *)(data + offset2) = htons(origin_timestamp.sec_msb);
 851		*(__be32 *)(data + offset2 + 2) =
 852			htonl(origin_timestamp.sec_lsb);
 853		*(__be32 *)(data + offset2 + 6) = htonl(origin_timestamp.nsec);
 854
 855		if (priv->ptp_correction_off == offset1)
 856			return;
 857
 858		priv->dpaa2_set_onestep_params_cb(priv, offset1, udp);
 859		priv->ptp_correction_off = offset1;
 860
 861	}
 862}
 863
 864void *dpaa2_eth_sgt_get(struct dpaa2_eth_priv *priv)
 865{
 866	struct dpaa2_eth_sgt_cache *sgt_cache;
 867	void *sgt_buf = NULL;
 868	int sgt_buf_size;
 869
 870	sgt_cache = this_cpu_ptr(priv->sgt_cache);
 871	sgt_buf_size = priv->tx_data_offset +
 872		DPAA2_ETH_SG_ENTRIES_MAX * sizeof(struct dpaa2_sg_entry);
 873
 874	if (sgt_cache->count == 0)
 875		sgt_buf = napi_alloc_frag_align(sgt_buf_size, DPAA2_ETH_TX_BUF_ALIGN);
 876	else
 877		sgt_buf = sgt_cache->buf[--sgt_cache->count];
 878	if (!sgt_buf)
 879		return NULL;
 880
 881	memset(sgt_buf, 0, sgt_buf_size);
 882
 883	return sgt_buf;
 884}
 885
 886void dpaa2_eth_sgt_recycle(struct dpaa2_eth_priv *priv, void *sgt_buf)
 887{
 888	struct dpaa2_eth_sgt_cache *sgt_cache;
 889
 890	sgt_cache = this_cpu_ptr(priv->sgt_cache);
 891	if (sgt_cache->count >= DPAA2_ETH_SGT_CACHE_SIZE)
 892		skb_free_frag(sgt_buf);
 893	else
 894		sgt_cache->buf[sgt_cache->count++] = sgt_buf;
 895}
 896
 897/* Create a frame descriptor based on a fragmented skb */
 898static int dpaa2_eth_build_sg_fd(struct dpaa2_eth_priv *priv,
 899				 struct sk_buff *skb,
 900				 struct dpaa2_fd *fd,
 901				 void **swa_addr)
 902{
 903	struct device *dev = priv->net_dev->dev.parent;
 904	void *sgt_buf = NULL;
 905	dma_addr_t addr;
 906	int nr_frags = skb_shinfo(skb)->nr_frags;
 907	struct dpaa2_sg_entry *sgt;
 908	int i, err;
 909	int sgt_buf_size;
 910	struct scatterlist *scl, *crt_scl;
 911	int num_sg;
 912	int num_dma_bufs;
 913	struct dpaa2_eth_swa *swa;
 914
 915	/* Create and map scatterlist.
 916	 * We don't advertise NETIF_F_FRAGLIST, so skb_to_sgvec() will not have
 917	 * to go beyond nr_frags+1.
 918	 * Note: We don't support chained scatterlists
 919	 */
 920	if (unlikely(PAGE_SIZE / sizeof(struct scatterlist) < nr_frags + 1))
 921		return -EINVAL;
 922
 923	scl = kmalloc_array(nr_frags + 1, sizeof(struct scatterlist), GFP_ATOMIC);
 924	if (unlikely(!scl))
 925		return -ENOMEM;
 926
 927	sg_init_table(scl, nr_frags + 1);
 928	num_sg = skb_to_sgvec(skb, scl, 0, skb->len);
 929	if (unlikely(num_sg < 0)) {
 930		err = -ENOMEM;
 931		goto dma_map_sg_failed;
 932	}
 933	num_dma_bufs = dma_map_sg(dev, scl, num_sg, DMA_BIDIRECTIONAL);
 934	if (unlikely(!num_dma_bufs)) {
 935		err = -ENOMEM;
 936		goto dma_map_sg_failed;
 937	}
 938
 939	/* Prepare the HW SGT structure */
 940	sgt_buf_size = priv->tx_data_offset +
 941		       sizeof(struct dpaa2_sg_entry) *  num_dma_bufs;
 942	sgt_buf = dpaa2_eth_sgt_get(priv);
 943	if (unlikely(!sgt_buf)) {
 944		err = -ENOMEM;
 945		goto sgt_buf_alloc_failed;
 946	}
 947
 948	sgt = (struct dpaa2_sg_entry *)(sgt_buf + priv->tx_data_offset);
 949
 950	/* Fill in the HW SGT structure.
 951	 *
 952	 * sgt_buf is zeroed out, so the following fields are implicit
 953	 * in all sgt entries:
 954	 *   - offset is 0
 955	 *   - format is 'dpaa2_sg_single'
 956	 */
 957	for_each_sg(scl, crt_scl, num_dma_bufs, i) {
 958		dpaa2_sg_set_addr(&sgt[i], sg_dma_address(crt_scl));
 959		dpaa2_sg_set_len(&sgt[i], sg_dma_len(crt_scl));
 960	}
 961	dpaa2_sg_set_final(&sgt[i - 1], true);
 962
 963	/* Store the skb backpointer in the SGT buffer.
 964	 * Fit the scatterlist and the number of buffers alongside the
 965	 * skb backpointer in the software annotation area. We'll need
 966	 * all of them on Tx Conf.
 967	 */
 968	*swa_addr = (void *)sgt_buf;
 969	swa = (struct dpaa2_eth_swa *)sgt_buf;
 970	swa->type = DPAA2_ETH_SWA_SG;
 971	swa->sg.skb = skb;
 972	swa->sg.scl = scl;
 973	swa->sg.num_sg = num_sg;
 974	swa->sg.sgt_size = sgt_buf_size;
 975
 976	/* Separately map the SGT buffer */
 977	addr = dma_map_single(dev, sgt_buf, sgt_buf_size, DMA_BIDIRECTIONAL);
 978	if (unlikely(dma_mapping_error(dev, addr))) {
 979		err = -ENOMEM;
 980		goto dma_map_single_failed;
 981	}
 982	memset(fd, 0, sizeof(struct dpaa2_fd));
 983	dpaa2_fd_set_offset(fd, priv->tx_data_offset);
 984	dpaa2_fd_set_format(fd, dpaa2_fd_sg);
 985	dpaa2_fd_set_addr(fd, addr);
 986	dpaa2_fd_set_len(fd, skb->len);
 987	dpaa2_fd_set_ctrl(fd, FD_CTRL_PTA);
 988
 989	return 0;
 990
 991dma_map_single_failed:
 992	dpaa2_eth_sgt_recycle(priv, sgt_buf);
 993sgt_buf_alloc_failed:
 994	dma_unmap_sg(dev, scl, num_sg, DMA_BIDIRECTIONAL);
 995dma_map_sg_failed:
 996	kfree(scl);
 997	return err;
 998}
 999
1000/* Create a SG frame descriptor based on a linear skb.
1001 *
1002 * This function is used on the Tx path when the skb headroom is not large
1003 * enough for the HW requirements, thus instead of realloc-ing the skb we
1004 * create a SG frame descriptor with only one entry.
1005 */
1006static int dpaa2_eth_build_sg_fd_single_buf(struct dpaa2_eth_priv *priv,
1007					    struct sk_buff *skb,
1008					    struct dpaa2_fd *fd,
1009					    void **swa_addr)
1010{
1011	struct device *dev = priv->net_dev->dev.parent;
1012	struct dpaa2_sg_entry *sgt;
1013	struct dpaa2_eth_swa *swa;
1014	dma_addr_t addr, sgt_addr;
1015	void *sgt_buf = NULL;
1016	int sgt_buf_size;
1017	int err;
1018
1019	/* Prepare the HW SGT structure */
1020	sgt_buf_size = priv->tx_data_offset + sizeof(struct dpaa2_sg_entry);
1021	sgt_buf = dpaa2_eth_sgt_get(priv);
1022	if (unlikely(!sgt_buf))
1023		return -ENOMEM;
1024	sgt = (struct dpaa2_sg_entry *)(sgt_buf + priv->tx_data_offset);
1025
1026	addr = dma_map_single(dev, skb->data, skb->len, DMA_BIDIRECTIONAL);
1027	if (unlikely(dma_mapping_error(dev, addr))) {
1028		err = -ENOMEM;
1029		goto data_map_failed;
1030	}
1031
1032	/* Fill in the HW SGT structure */
1033	dpaa2_sg_set_addr(sgt, addr);
1034	dpaa2_sg_set_len(sgt, skb->len);
1035	dpaa2_sg_set_final(sgt, true);
1036
1037	/* Store the skb backpointer in the SGT buffer */
1038	*swa_addr = (void *)sgt_buf;
1039	swa = (struct dpaa2_eth_swa *)sgt_buf;
1040	swa->type = DPAA2_ETH_SWA_SINGLE;
1041	swa->single.skb = skb;
1042	swa->single.sgt_size = sgt_buf_size;
1043
1044	/* Separately map the SGT buffer */
1045	sgt_addr = dma_map_single(dev, sgt_buf, sgt_buf_size, DMA_BIDIRECTIONAL);
1046	if (unlikely(dma_mapping_error(dev, sgt_addr))) {
1047		err = -ENOMEM;
1048		goto sgt_map_failed;
1049	}
1050
1051	memset(fd, 0, sizeof(struct dpaa2_fd));
1052	dpaa2_fd_set_offset(fd, priv->tx_data_offset);
1053	dpaa2_fd_set_format(fd, dpaa2_fd_sg);
1054	dpaa2_fd_set_addr(fd, sgt_addr);
1055	dpaa2_fd_set_len(fd, skb->len);
1056	dpaa2_fd_set_ctrl(fd, FD_CTRL_PTA);
1057
1058	return 0;
1059
1060sgt_map_failed:
1061	dma_unmap_single(dev, addr, skb->len, DMA_BIDIRECTIONAL);
1062data_map_failed:
1063	dpaa2_eth_sgt_recycle(priv, sgt_buf);
1064
1065	return err;
1066}
1067
1068/* Create a frame descriptor based on a linear skb */
1069static int dpaa2_eth_build_single_fd(struct dpaa2_eth_priv *priv,
1070				     struct sk_buff *skb,
1071				     struct dpaa2_fd *fd,
1072				     void **swa_addr)
1073{
1074	struct device *dev = priv->net_dev->dev.parent;
1075	u8 *buffer_start, *aligned_start;
1076	struct dpaa2_eth_swa *swa;
1077	dma_addr_t addr;
1078
1079	buffer_start = skb->data - dpaa2_eth_needed_headroom(skb);
1080	aligned_start = PTR_ALIGN(buffer_start, DPAA2_ETH_TX_BUF_ALIGN);
1081	if (aligned_start >= skb->head)
1082		buffer_start = aligned_start;
1083	else
1084		return -ENOMEM;
1085
1086	/* Store a backpointer to the skb at the beginning of the buffer
1087	 * (in the private data area) such that we can release it
1088	 * on Tx confirm
1089	 */
1090	*swa_addr = (void *)buffer_start;
1091	swa = (struct dpaa2_eth_swa *)buffer_start;
1092	swa->type = DPAA2_ETH_SWA_SINGLE;
1093	swa->single.skb = skb;
1094
1095	addr = dma_map_single(dev, buffer_start,
1096			      skb_tail_pointer(skb) - buffer_start,
1097			      DMA_BIDIRECTIONAL);
1098	if (unlikely(dma_mapping_error(dev, addr)))
1099		return -ENOMEM;
1100
1101	memset(fd, 0, sizeof(struct dpaa2_fd));
1102	dpaa2_fd_set_addr(fd, addr);
1103	dpaa2_fd_set_offset(fd, (u16)(skb->data - buffer_start));
1104	dpaa2_fd_set_len(fd, skb->len);
1105	dpaa2_fd_set_format(fd, dpaa2_fd_single);
1106	dpaa2_fd_set_ctrl(fd, FD_CTRL_PTA);
1107
1108	return 0;
1109}
1110
1111/* FD freeing routine on the Tx path
1112 *
1113 * DMA-unmap and free FD and possibly SGT buffer allocated on Tx. The skb
1114 * back-pointed to is also freed.
1115 * This can be called either from dpaa2_eth_tx_conf() or on the error path of
1116 * dpaa2_eth_tx().
1117 */
1118void dpaa2_eth_free_tx_fd(struct dpaa2_eth_priv *priv,
1119			  struct dpaa2_eth_channel *ch,
1120			  struct dpaa2_eth_fq *fq,
1121			  const struct dpaa2_fd *fd, bool in_napi)
1122{
1123	struct device *dev = priv->net_dev->dev.parent;
1124	dma_addr_t fd_addr, sg_addr;
1125	struct sk_buff *skb = NULL;
1126	unsigned char *buffer_start;
1127	struct dpaa2_eth_swa *swa;
1128	u8 fd_format = dpaa2_fd_get_format(fd);
1129	u32 fd_len = dpaa2_fd_get_len(fd);
1130	struct dpaa2_sg_entry *sgt;
1131	int should_free_skb = 1;
1132	void *tso_hdr;
1133	int i;
1134
1135	fd_addr = dpaa2_fd_get_addr(fd);
1136	buffer_start = dpaa2_iova_to_virt(priv->iommu_domain, fd_addr);
1137	swa = (struct dpaa2_eth_swa *)buffer_start;
1138
1139	if (fd_format == dpaa2_fd_single) {
1140		if (swa->type == DPAA2_ETH_SWA_SINGLE) {
1141			skb = swa->single.skb;
1142			/* Accessing the skb buffer is safe before dma unmap,
1143			 * because we didn't map the actual skb shell.
1144			 */
1145			dma_unmap_single(dev, fd_addr,
1146					 skb_tail_pointer(skb) - buffer_start,
1147					 DMA_BIDIRECTIONAL);
1148		} else {
1149			WARN_ONCE(swa->type != DPAA2_ETH_SWA_XDP, "Wrong SWA type");
1150			dma_unmap_single(dev, fd_addr, swa->xdp.dma_size,
1151					 DMA_BIDIRECTIONAL);
1152		}
1153	} else if (fd_format == dpaa2_fd_sg) {
1154		if (swa->type == DPAA2_ETH_SWA_SG) {
1155			skb = swa->sg.skb;
1156
1157			/* Unmap the scatterlist */
1158			dma_unmap_sg(dev, swa->sg.scl, swa->sg.num_sg,
1159				     DMA_BIDIRECTIONAL);
1160			kfree(swa->sg.scl);
1161
1162			/* Unmap the SGT buffer */
1163			dma_unmap_single(dev, fd_addr, swa->sg.sgt_size,
1164					 DMA_BIDIRECTIONAL);
1165		} else if (swa->type == DPAA2_ETH_SWA_SW_TSO) {
1166			skb = swa->tso.skb;
1167
1168			sgt = (struct dpaa2_sg_entry *)(buffer_start +
1169							priv->tx_data_offset);
1170
1171			/* Unmap the SGT buffer */
1172			dma_unmap_single(dev, fd_addr, swa->tso.sgt_size,
1173					 DMA_BIDIRECTIONAL);
1174
1175			/* Unmap and free the header */
1176			tso_hdr = dpaa2_iova_to_virt(priv->iommu_domain, dpaa2_sg_get_addr(sgt));
1177			dma_unmap_single(dev, dpaa2_sg_get_addr(sgt), TSO_HEADER_SIZE,
1178					 DMA_TO_DEVICE);
1179			kfree(tso_hdr);
1180
1181			/* Unmap the other SG entries for the data */
1182			for (i = 1; i < swa->tso.num_sg; i++)
1183				dma_unmap_single(dev, dpaa2_sg_get_addr(&sgt[i]),
1184						 dpaa2_sg_get_len(&sgt[i]), DMA_TO_DEVICE);
1185
1186			if (!swa->tso.is_last_fd)
1187				should_free_skb = 0;
1188		} else if (swa->type == DPAA2_ETH_SWA_XSK) {
1189			/* Unmap the SGT Buffer */
1190			dma_unmap_single(dev, fd_addr, swa->xsk.sgt_size,
1191					 DMA_BIDIRECTIONAL);
1192		} else {
1193			skb = swa->single.skb;
1194
1195			/* Unmap the SGT Buffer */
1196			dma_unmap_single(dev, fd_addr, swa->single.sgt_size,
1197					 DMA_BIDIRECTIONAL);
1198
1199			sgt = (struct dpaa2_sg_entry *)(buffer_start +
1200							priv->tx_data_offset);
1201			sg_addr = dpaa2_sg_get_addr(sgt);
1202			dma_unmap_single(dev, sg_addr, skb->len, DMA_BIDIRECTIONAL);
1203		}
1204	} else {
1205		netdev_dbg(priv->net_dev, "Invalid FD format\n");
1206		return;
1207	}
1208
1209	if (swa->type == DPAA2_ETH_SWA_XSK) {
1210		ch->xsk_tx_pkts_sent++;
1211		dpaa2_eth_sgt_recycle(priv, buffer_start);
1212		return;
1213	}
1214
1215	if (swa->type != DPAA2_ETH_SWA_XDP && in_napi) {
1216		fq->dq_frames++;
1217		fq->dq_bytes += fd_len;
1218	}
1219
1220	if (swa->type == DPAA2_ETH_SWA_XDP) {
1221		xdp_return_frame(swa->xdp.xdpf);
1222		return;
1223	}
1224
1225	/* Get the timestamp value */
1226	if (swa->type != DPAA2_ETH_SWA_SW_TSO) {
1227		if (skb->cb[0] == TX_TSTAMP) {
1228			struct skb_shared_hwtstamps shhwtstamps;
1229			__le64 *ts = dpaa2_get_ts(buffer_start, true);
1230			u64 ns;
1231
1232			memset(&shhwtstamps, 0, sizeof(shhwtstamps));
1233
1234			ns = DPAA2_PTP_CLK_PERIOD_NS * le64_to_cpup(ts);
1235			shhwtstamps.hwtstamp = ns_to_ktime(ns);
1236			skb_tstamp_tx(skb, &shhwtstamps);
1237		} else if (skb->cb[0] == TX_TSTAMP_ONESTEP_SYNC) {
1238			mutex_unlock(&priv->onestep_tstamp_lock);
1239		}
1240	}
1241
1242	/* Free SGT buffer allocated on tx */
1243	if (fd_format != dpaa2_fd_single)
1244		dpaa2_eth_sgt_recycle(priv, buffer_start);
1245
1246	/* Move on with skb release. If we are just confirming multiple FDs
1247	 * from the same TSO skb then only the last one will need to free the
1248	 * skb.
1249	 */
1250	if (should_free_skb)
1251		napi_consume_skb(skb, in_napi);
1252}
1253
1254static int dpaa2_eth_build_gso_fd(struct dpaa2_eth_priv *priv,
1255				  struct sk_buff *skb, struct dpaa2_fd *fd,
1256				  int *num_fds, u32 *total_fds_len)
1257{
1258	struct device *dev = priv->net_dev->dev.parent;
1259	int hdr_len, total_len, data_left, fd_len;
1260	int num_sge, err, i, sgt_buf_size;
1261	struct dpaa2_fd *fd_start = fd;
1262	struct dpaa2_sg_entry *sgt;
1263	struct dpaa2_eth_swa *swa;
1264	dma_addr_t sgt_addr, addr;
1265	dma_addr_t tso_hdr_dma;
1266	unsigned int index = 0;
1267	struct tso_t tso;
1268	char *tso_hdr;
1269	void *sgt_buf;
1270
1271	/* Initialize the TSO handler, and prepare the first payload */
1272	hdr_len = tso_start(skb, &tso);
1273	*total_fds_len = 0;
1274
1275	total_len = skb->len - hdr_len;
1276	while (total_len > 0) {
1277		/* Prepare the HW SGT structure for this frame */
1278		sgt_buf = dpaa2_eth_sgt_get(priv);
1279		if (unlikely(!sgt_buf)) {
1280			netdev_err(priv->net_dev, "dpaa2_eth_sgt_get() failed\n");
1281			err = -ENOMEM;
1282			goto err_sgt_get;
1283		}
1284		sgt = (struct dpaa2_sg_entry *)(sgt_buf + priv->tx_data_offset);
1285
1286		/* Determine the data length of this frame */
1287		data_left = min_t(int, skb_shinfo(skb)->gso_size, total_len);
1288		total_len -= data_left;
1289		fd_len = data_left + hdr_len;
1290
1291		/* Prepare packet headers: MAC + IP + TCP */
1292		tso_hdr = kmalloc(TSO_HEADER_SIZE, GFP_ATOMIC);
1293		if (!tso_hdr) {
1294			err =  -ENOMEM;
1295			goto err_alloc_tso_hdr;
1296		}
1297
1298		tso_build_hdr(skb, tso_hdr, &tso, data_left, total_len == 0);
1299		tso_hdr_dma = dma_map_single(dev, tso_hdr, TSO_HEADER_SIZE, DMA_TO_DEVICE);
1300		if (dma_mapping_error(dev, tso_hdr_dma)) {
1301			netdev_err(priv->net_dev, "dma_map_single(tso_hdr) failed\n");
1302			err = -ENOMEM;
1303			goto err_map_tso_hdr;
1304		}
1305
1306		/* Setup the SG entry for the header */
1307		dpaa2_sg_set_addr(sgt, tso_hdr_dma);
1308		dpaa2_sg_set_len(sgt, hdr_len);
1309		dpaa2_sg_set_final(sgt, data_left <= 0);
1310
1311		/* Compose the SG entries for each fragment of data */
1312		num_sge = 1;
1313		while (data_left > 0) {
1314			int size;
1315
1316			/* Move to the next SG entry */
1317			sgt++;
1318			size = min_t(int, tso.size, data_left);
1319
1320			addr = dma_map_single(dev, tso.data, size, DMA_TO_DEVICE);
1321			if (dma_mapping_error(dev, addr)) {
1322				netdev_err(priv->net_dev, "dma_map_single(tso.data) failed\n");
1323				err = -ENOMEM;
1324				goto err_map_data;
1325			}
1326			dpaa2_sg_set_addr(sgt, addr);
1327			dpaa2_sg_set_len(sgt, size);
1328			dpaa2_sg_set_final(sgt, size == data_left);
1329
1330			num_sge++;
1331
1332			/* Build the data for the __next__ fragment */
1333			data_left -= size;
1334			tso_build_data(skb, &tso, size);
1335		}
1336
1337		/* Store the skb backpointer in the SGT buffer */
1338		sgt_buf_size = priv->tx_data_offset + num_sge * sizeof(struct dpaa2_sg_entry);
1339		swa = (struct dpaa2_eth_swa *)sgt_buf;
1340		swa->type = DPAA2_ETH_SWA_SW_TSO;
1341		swa->tso.skb = skb;
1342		swa->tso.num_sg = num_sge;
1343		swa->tso.sgt_size = sgt_buf_size;
1344		swa->tso.is_last_fd = total_len == 0 ? 1 : 0;
1345
1346		/* Separately map the SGT buffer */
1347		sgt_addr = dma_map_single(dev, sgt_buf, sgt_buf_size, DMA_BIDIRECTIONAL);
1348		if (unlikely(dma_mapping_error(dev, sgt_addr))) {
1349			netdev_err(priv->net_dev, "dma_map_single(sgt_buf) failed\n");
1350			err = -ENOMEM;
1351			goto err_map_sgt;
1352		}
1353
1354		/* Setup the frame descriptor */
1355		memset(fd, 0, sizeof(struct dpaa2_fd));
1356		dpaa2_fd_set_offset(fd, priv->tx_data_offset);
1357		dpaa2_fd_set_format(fd, dpaa2_fd_sg);
1358		dpaa2_fd_set_addr(fd, sgt_addr);
1359		dpaa2_fd_set_len(fd, fd_len);
1360		dpaa2_fd_set_ctrl(fd, FD_CTRL_PTA);
1361
1362		*total_fds_len += fd_len;
1363		/* Advance to the next frame descriptor */
1364		fd++;
1365		index++;
1366	}
1367
1368	*num_fds = index;
1369
1370	return 0;
1371
1372err_map_sgt:
1373err_map_data:
1374	/* Unmap all the data S/G entries for the current FD */
1375	sgt = (struct dpaa2_sg_entry *)(sgt_buf + priv->tx_data_offset);
1376	for (i = 1; i < num_sge; i++)
1377		dma_unmap_single(dev, dpaa2_sg_get_addr(&sgt[i]),
1378				 dpaa2_sg_get_len(&sgt[i]), DMA_TO_DEVICE);
1379
1380	/* Unmap the header entry */
1381	dma_unmap_single(dev, tso_hdr_dma, TSO_HEADER_SIZE, DMA_TO_DEVICE);
1382err_map_tso_hdr:
1383	kfree(tso_hdr);
1384err_alloc_tso_hdr:
1385	dpaa2_eth_sgt_recycle(priv, sgt_buf);
1386err_sgt_get:
1387	/* Free all the other FDs that were already fully created */
1388	for (i = 0; i < index; i++)
1389		dpaa2_eth_free_tx_fd(priv, NULL, NULL, &fd_start[i], false);
1390
1391	return err;
1392}
1393
1394static netdev_tx_t __dpaa2_eth_tx(struct sk_buff *skb,
1395				  struct net_device *net_dev)
1396{
1397	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
1398	int total_enqueued = 0, retries = 0, enqueued;
1399	struct dpaa2_eth_drv_stats *percpu_extras;
1400	struct rtnl_link_stats64 *percpu_stats;
1401	unsigned int needed_headroom;
1402	int num_fds = 1, max_retries;
1403	struct dpaa2_eth_fq *fq;
1404	struct netdev_queue *nq;
1405	struct dpaa2_fd *fd;
1406	u16 queue_mapping;
1407	void *swa = NULL;
1408	u8 prio = 0;
1409	int err, i;
1410	u32 fd_len;
1411
1412	percpu_stats = this_cpu_ptr(priv->percpu_stats);
1413	percpu_extras = this_cpu_ptr(priv->percpu_extras);
1414	fd = (this_cpu_ptr(priv->fd))->array;
1415
1416	needed_headroom = dpaa2_eth_needed_headroom(skb);
1417
1418	/* We'll be holding a back-reference to the skb until Tx Confirmation;
1419	 * we don't want that overwritten by a concurrent Tx with a cloned skb.
1420	 */
1421	skb = skb_unshare(skb, GFP_ATOMIC);
1422	if (unlikely(!skb)) {
1423		/* skb_unshare() has already freed the skb */
1424		percpu_stats->tx_dropped++;
1425		return NETDEV_TX_OK;
1426	}
1427
1428	/* Setup the FD fields */
1429
1430	if (skb_is_gso(skb)) {
1431		err = dpaa2_eth_build_gso_fd(priv, skb, fd, &num_fds, &fd_len);
1432		percpu_extras->tx_sg_frames += num_fds;
1433		percpu_extras->tx_sg_bytes += fd_len;
1434		percpu_extras->tx_tso_frames += num_fds;
1435		percpu_extras->tx_tso_bytes += fd_len;
1436	} else if (skb_is_nonlinear(skb)) {
1437		err = dpaa2_eth_build_sg_fd(priv, skb, fd, &swa);
1438		percpu_extras->tx_sg_frames++;
1439		percpu_extras->tx_sg_bytes += skb->len;
1440		fd_len = dpaa2_fd_get_len(fd);
1441	} else if (skb_headroom(skb) < needed_headroom) {
1442		err = dpaa2_eth_build_sg_fd_single_buf(priv, skb, fd, &swa);
1443		percpu_extras->tx_sg_frames++;
1444		percpu_extras->tx_sg_bytes += skb->len;
1445		percpu_extras->tx_converted_sg_frames++;
1446		percpu_extras->tx_converted_sg_bytes += skb->len;
1447		fd_len = dpaa2_fd_get_len(fd);
1448	} else {
1449		err = dpaa2_eth_build_single_fd(priv, skb, fd, &swa);
1450		fd_len = dpaa2_fd_get_len(fd);
1451	}
1452
1453	if (unlikely(err)) {
1454		percpu_stats->tx_dropped++;
1455		goto err_build_fd;
1456	}
1457
1458	if (swa && skb->cb[0])
1459		dpaa2_eth_enable_tx_tstamp(priv, fd, swa, skb);
1460
1461	/* Tracing point */
1462	for (i = 0; i < num_fds; i++)
1463		trace_dpaa2_tx_fd(net_dev, &fd[i]);
1464
1465	/* TxConf FQ selection relies on queue id from the stack.
1466	 * In case of a forwarded frame from another DPNI interface, we choose
1467	 * a queue affined to the same core that processed the Rx frame
1468	 */
1469	queue_mapping = skb_get_queue_mapping(skb);
1470
1471	if (net_dev->num_tc) {
1472		prio = netdev_txq_to_tc(net_dev, queue_mapping);
1473		/* Hardware interprets priority level 0 as being the highest,
1474		 * so we need to do a reverse mapping to the netdev tc index
1475		 */
1476		prio = net_dev->num_tc - prio - 1;
1477		/* We have only one FQ array entry for all Tx hardware queues
1478		 * with the same flow id (but different priority levels)
1479		 */
1480		queue_mapping %= dpaa2_eth_queue_count(priv);
1481	}
1482	fq = &priv->fq[queue_mapping];
1483	nq = netdev_get_tx_queue(net_dev, queue_mapping);
1484	netdev_tx_sent_queue(nq, fd_len);
1485
1486	/* Everything that happens after this enqueues might race with
1487	 * the Tx confirmation callback for this frame
1488	 */
1489	max_retries = num_fds * DPAA2_ETH_ENQUEUE_RETRIES;
1490	while (total_enqueued < num_fds && retries < max_retries) {
1491		err = priv->enqueue(priv, fq, &fd[total_enqueued],
1492				    prio, num_fds - total_enqueued, &enqueued);
1493		if (err == -EBUSY) {
1494			retries++;
1495			continue;
1496		}
1497
1498		total_enqueued += enqueued;
1499	}
1500	percpu_extras->tx_portal_busy += retries;
1501
1502	if (unlikely(err < 0)) {
1503		percpu_stats->tx_errors++;
1504		/* Clean up everything, including freeing the skb */
1505		dpaa2_eth_free_tx_fd(priv, NULL, fq, fd, false);
1506		netdev_tx_completed_queue(nq, 1, fd_len);
1507	} else {
1508		percpu_stats->tx_packets += total_enqueued;
1509		percpu_stats->tx_bytes += fd_len;
1510	}
1511
1512	return NETDEV_TX_OK;
1513
1514err_build_fd:
1515	dev_kfree_skb(skb);
1516
1517	return NETDEV_TX_OK;
1518}
1519
1520static void dpaa2_eth_tx_onestep_tstamp(struct work_struct *work)
1521{
1522	struct dpaa2_eth_priv *priv = container_of(work, struct dpaa2_eth_priv,
1523						   tx_onestep_tstamp);
1524	struct sk_buff *skb;
1525
1526	while (true) {
1527		skb = skb_dequeue(&priv->tx_skbs);
1528		if (!skb)
1529			return;
1530
1531		/* Lock just before TX one-step timestamping packet,
1532		 * and release the lock in dpaa2_eth_free_tx_fd when
1533		 * confirm the packet has been sent on hardware, or
1534		 * when clean up during transmit failure.
1535		 */
1536		mutex_lock(&priv->onestep_tstamp_lock);
1537		__dpaa2_eth_tx(skb, priv->net_dev);
1538	}
1539}
1540
1541static netdev_tx_t dpaa2_eth_tx(struct sk_buff *skb, struct net_device *net_dev)
1542{
1543	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
1544	u8 msgtype, twostep, udp;
1545	u16 offset1, offset2;
1546
1547	/* Utilize skb->cb[0] for timestamping request per skb */
1548	skb->cb[0] = 0;
1549
1550	if ((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) && dpaa2_ptp) {
1551		if (priv->tx_tstamp_type == HWTSTAMP_TX_ON)
1552			skb->cb[0] = TX_TSTAMP;
1553		else if (priv->tx_tstamp_type == HWTSTAMP_TX_ONESTEP_SYNC)
1554			skb->cb[0] = TX_TSTAMP_ONESTEP_SYNC;
1555	}
1556
1557	/* TX for one-step timestamping PTP Sync packet */
1558	if (skb->cb[0] == TX_TSTAMP_ONESTEP_SYNC) {
1559		if (!dpaa2_eth_ptp_parse(skb, &msgtype, &twostep, &udp,
1560					 &offset1, &offset2))
1561			if (msgtype == PTP_MSGTYPE_SYNC && twostep == 0) {
1562				skb_queue_tail(&priv->tx_skbs, skb);
1563				queue_work(priv->dpaa2_ptp_wq,
1564					   &priv->tx_onestep_tstamp);
1565				return NETDEV_TX_OK;
1566			}
1567		/* Use two-step timestamping if not one-step timestamping
1568		 * PTP Sync packet
1569		 */
1570		skb->cb[0] = TX_TSTAMP;
1571	}
1572
1573	/* TX for other packets */
1574	return __dpaa2_eth_tx(skb, net_dev);
1575}
1576
1577/* Tx confirmation frame processing routine */
1578static void dpaa2_eth_tx_conf(struct dpaa2_eth_priv *priv,
1579			      struct dpaa2_eth_channel *ch,
1580			      const struct dpaa2_fd *fd,
1581			      struct dpaa2_eth_fq *fq)
1582{
1583	struct rtnl_link_stats64 *percpu_stats;
1584	struct dpaa2_eth_drv_stats *percpu_extras;
1585	u32 fd_len = dpaa2_fd_get_len(fd);
1586	u32 fd_errors;
1587
1588	/* Tracing point */
1589	trace_dpaa2_tx_conf_fd(priv->net_dev, fd);
1590
1591	percpu_extras = this_cpu_ptr(priv->percpu_extras);
1592	percpu_extras->tx_conf_frames++;
1593	percpu_extras->tx_conf_bytes += fd_len;
1594	ch->stats.bytes_per_cdan += fd_len;
1595
1596	/* Check frame errors in the FD field */
1597	fd_errors = dpaa2_fd_get_ctrl(fd) & DPAA2_FD_TX_ERR_MASK;
1598	dpaa2_eth_free_tx_fd(priv, ch, fq, fd, true);
1599
1600	if (likely(!fd_errors))
1601		return;
1602
1603	if (net_ratelimit())
1604		netdev_dbg(priv->net_dev, "TX frame FD error: 0x%08x\n",
1605			   fd_errors);
1606
1607	percpu_stats = this_cpu_ptr(priv->percpu_stats);
1608	/* Tx-conf logically pertains to the egress path. */
1609	percpu_stats->tx_errors++;
1610}
1611
1612static int dpaa2_eth_set_rx_vlan_filtering(struct dpaa2_eth_priv *priv,
1613					   bool enable)
1614{
1615	int err;
1616
1617	err = dpni_enable_vlan_filter(priv->mc_io, 0, priv->mc_token, enable);
1618
1619	if (err) {
1620		netdev_err(priv->net_dev,
1621			   "dpni_enable_vlan_filter failed\n");
1622		return err;
1623	}
1624
1625	return 0;
1626}
1627
1628static int dpaa2_eth_set_rx_csum(struct dpaa2_eth_priv *priv, bool enable)
1629{
1630	int err;
1631
1632	err = dpni_set_offload(priv->mc_io, 0, priv->mc_token,
1633			       DPNI_OFF_RX_L3_CSUM, enable);
1634	if (err) {
1635		netdev_err(priv->net_dev,
1636			   "dpni_set_offload(RX_L3_CSUM) failed\n");
1637		return err;
1638	}
1639
1640	err = dpni_set_offload(priv->mc_io, 0, priv->mc_token,
1641			       DPNI_OFF_RX_L4_CSUM, enable);
1642	if (err) {
1643		netdev_err(priv->net_dev,
1644			   "dpni_set_offload(RX_L4_CSUM) failed\n");
1645		return err;
1646	}
1647
1648	return 0;
1649}
1650
1651static int dpaa2_eth_set_tx_csum(struct dpaa2_eth_priv *priv, bool enable)
1652{
1653	int err;
1654
1655	err = dpni_set_offload(priv->mc_io, 0, priv->mc_token,
1656			       DPNI_OFF_TX_L3_CSUM, enable);
1657	if (err) {
1658		netdev_err(priv->net_dev, "dpni_set_offload(TX_L3_CSUM) failed\n");
1659		return err;
1660	}
1661
1662	err = dpni_set_offload(priv->mc_io, 0, priv->mc_token,
1663			       DPNI_OFF_TX_L4_CSUM, enable);
1664	if (err) {
1665		netdev_err(priv->net_dev, "dpni_set_offload(TX_L4_CSUM) failed\n");
1666		return err;
1667	}
1668
1669	return 0;
1670}
1671
1672/* Perform a single release command to add buffers
1673 * to the specified buffer pool
1674 */
1675static int dpaa2_eth_add_bufs(struct dpaa2_eth_priv *priv,
1676			      struct dpaa2_eth_channel *ch)
1677{
1678	struct xdp_buff *xdp_buffs[DPAA2_ETH_BUFS_PER_CMD];
1679	struct device *dev = priv->net_dev->dev.parent;
1680	u64 buf_array[DPAA2_ETH_BUFS_PER_CMD];
1681	struct dpaa2_eth_swa *swa;
1682	struct page *page;
1683	dma_addr_t addr;
1684	int retries = 0;
1685	int i = 0, err;
1686	u32 batch;
1687
1688	/* Allocate buffers visible to WRIOP */
1689	if (!ch->xsk_zc) {
1690		for (i = 0; i < DPAA2_ETH_BUFS_PER_CMD; i++) {
1691			/* Also allocate skb shared info and alignment padding.
1692			 * There is one page for each Rx buffer. WRIOP sees
1693			 * the entire page except for a tailroom reserved for
1694			 * skb shared info
1695			 */
1696			page = dev_alloc_pages(0);
1697			if (!page)
1698				goto err_alloc;
1699
1700			addr = dma_map_page(dev, page, 0, priv->rx_buf_size,
1701					    DMA_BIDIRECTIONAL);
1702			if (unlikely(dma_mapping_error(dev, addr)))
1703				goto err_map;
1704
1705			buf_array[i] = addr;
1706
1707			/* tracing point */
1708			trace_dpaa2_eth_buf_seed(priv->net_dev,
1709						 page_address(page),
1710						 DPAA2_ETH_RX_BUF_RAW_SIZE,
1711						 addr, priv->rx_buf_size,
1712						 ch->bp->bpid);
1713		}
1714	} else if (xsk_buff_can_alloc(ch->xsk_pool, DPAA2_ETH_BUFS_PER_CMD)) {
1715		/* Allocate XSK buffers for AF_XDP fast path in batches
1716		 * of DPAA2_ETH_BUFS_PER_CMD. Bail out if the UMEM cannot
1717		 * provide enough buffers at the moment
1718		 */
1719		batch = xsk_buff_alloc_batch(ch->xsk_pool, xdp_buffs,
1720					     DPAA2_ETH_BUFS_PER_CMD);
1721		if (!batch)
1722			goto err_alloc;
1723
1724		for (i = 0; i < batch; i++) {
1725			swa = (struct dpaa2_eth_swa *)(xdp_buffs[i]->data_hard_start +
1726						       DPAA2_ETH_RX_HWA_SIZE);
1727			swa->xsk.xdp_buff = xdp_buffs[i];
1728
1729			addr = xsk_buff_xdp_get_frame_dma(xdp_buffs[i]);
1730			if (unlikely(dma_mapping_error(dev, addr)))
1731				goto err_map;
1732
1733			buf_array[i] = addr;
1734
1735			trace_dpaa2_xsk_buf_seed(priv->net_dev,
1736						 xdp_buffs[i]->data_hard_start,
1737						 DPAA2_ETH_RX_BUF_RAW_SIZE,
1738						 addr, priv->rx_buf_size,
1739						 ch->bp->bpid);
1740		}
1741	}
1742
1743release_bufs:
1744	/* In case the portal is busy, retry until successful */
1745	while ((err = dpaa2_io_service_release(ch->dpio, ch->bp->bpid,
1746					       buf_array, i)) == -EBUSY) {
1747		if (retries++ >= DPAA2_ETH_SWP_BUSY_RETRIES)
1748			break;
1749		cpu_relax();
1750	}
1751
1752	/* If release command failed, clean up and bail out;
1753	 * not much else we can do about it
1754	 */
1755	if (err) {
1756		dpaa2_eth_free_bufs(priv, buf_array, i, ch->xsk_zc);
1757		return 0;
1758	}
1759
1760	return i;
1761
1762err_map:
1763	if (!ch->xsk_zc) {
1764		__free_pages(page, 0);
1765	} else {
1766		for (; i < batch; i++)
1767			xsk_buff_free(xdp_buffs[i]);
1768	}
1769err_alloc:
1770	/* If we managed to allocate at least some buffers,
1771	 * release them to hardware
1772	 */
1773	if (i)
1774		goto release_bufs;
1775
1776	return 0;
1777}
1778
1779static int dpaa2_eth_seed_pool(struct dpaa2_eth_priv *priv,
1780			       struct dpaa2_eth_channel *ch)
1781{
1782	int i;
1783	int new_count;
1784
1785	for (i = 0; i < DPAA2_ETH_NUM_BUFS; i += DPAA2_ETH_BUFS_PER_CMD) {
1786		new_count = dpaa2_eth_add_bufs(priv, ch);
1787		ch->buf_count += new_count;
1788
1789		if (new_count < DPAA2_ETH_BUFS_PER_CMD)
1790			return -ENOMEM;
1791	}
1792
1793	return 0;
1794}
1795
1796static void dpaa2_eth_seed_pools(struct dpaa2_eth_priv *priv)
1797{
1798	struct net_device *net_dev = priv->net_dev;
1799	struct dpaa2_eth_channel *channel;
1800	int i, err = 0;
1801
1802	for (i = 0; i < priv->num_channels; i++) {
1803		channel = priv->channel[i];
1804
1805		err = dpaa2_eth_seed_pool(priv, channel);
1806
1807		/* Not much to do; the buffer pool, though not filled up,
1808		 * may still contain some buffers which would enable us
1809		 * to limp on.
1810		 */
1811		if (err)
1812			netdev_err(net_dev, "Buffer seeding failed for DPBP %d (bpid=%d)\n",
1813				   channel->bp->dev->obj_desc.id,
1814				   channel->bp->bpid);
1815	}
1816}
1817
1818/*
1819 * Drain the specified number of buffers from one of the DPNI's private buffer
1820 * pools.
1821 * @count must not exceeed DPAA2_ETH_BUFS_PER_CMD
1822 */
1823static void dpaa2_eth_drain_bufs(struct dpaa2_eth_priv *priv, int bpid,
1824				 int count)
1825{
1826	u64 buf_array[DPAA2_ETH_BUFS_PER_CMD];
1827	bool xsk_zc = false;
1828	int retries = 0;
1829	int i, ret;
1830
1831	for (i = 0; i < priv->num_channels; i++)
1832		if (priv->channel[i]->bp->bpid == bpid)
1833			xsk_zc = priv->channel[i]->xsk_zc;
1834
1835	do {
1836		ret = dpaa2_io_service_acquire(NULL, bpid, buf_array, count);
1837		if (ret < 0) {
1838			if (ret == -EBUSY &&
1839			    retries++ < DPAA2_ETH_SWP_BUSY_RETRIES)
1840				continue;
1841			netdev_err(priv->net_dev, "dpaa2_io_service_acquire() failed\n");
1842			return;
1843		}
1844		dpaa2_eth_free_bufs(priv, buf_array, ret, xsk_zc);
1845		retries = 0;
1846	} while (ret);
1847}
1848
1849static void dpaa2_eth_drain_pool(struct dpaa2_eth_priv *priv, int bpid)
1850{
1851	int i;
1852
1853	/* Drain the buffer pool */
1854	dpaa2_eth_drain_bufs(priv, bpid, DPAA2_ETH_BUFS_PER_CMD);
1855	dpaa2_eth_drain_bufs(priv, bpid, 1);
1856
1857	/* Setup to zero the buffer count of all channels which were
1858	 * using this buffer pool.
1859	 */
1860	for (i = 0; i < priv->num_channels; i++)
1861		if (priv->channel[i]->bp->bpid == bpid)
1862			priv->channel[i]->buf_count = 0;
1863}
1864
1865static void dpaa2_eth_drain_pools(struct dpaa2_eth_priv *priv)
1866{
1867	int i;
1868
1869	for (i = 0; i < priv->num_bps; i++)
1870		dpaa2_eth_drain_pool(priv, priv->bp[i]->bpid);
1871}
1872
1873/* Function is called from softirq context only, so we don't need to guard
1874 * the access to percpu count
1875 */
1876static int dpaa2_eth_refill_pool(struct dpaa2_eth_priv *priv,
1877				 struct dpaa2_eth_channel *ch)
1878{
1879	int new_count;
1880
1881	if (likely(ch->buf_count >= DPAA2_ETH_REFILL_THRESH))
1882		return 0;
1883
1884	do {
1885		new_count = dpaa2_eth_add_bufs(priv, ch);
1886		if (unlikely(!new_count)) {
1887			/* Out of memory; abort for now, we'll try later on */
1888			break;
1889		}
1890		ch->buf_count += new_count;
1891	} while (ch->buf_count < DPAA2_ETH_NUM_BUFS);
1892
1893	if (unlikely(ch->buf_count < DPAA2_ETH_NUM_BUFS))
1894		return -ENOMEM;
1895
1896	return 0;
1897}
1898
1899static void dpaa2_eth_sgt_cache_drain(struct dpaa2_eth_priv *priv)
1900{
1901	struct dpaa2_eth_sgt_cache *sgt_cache;
1902	u16 count;
1903	int k, i;
1904
1905	for_each_possible_cpu(k) {
1906		sgt_cache = per_cpu_ptr(priv->sgt_cache, k);
1907		count = sgt_cache->count;
1908
1909		for (i = 0; i < count; i++)
1910			skb_free_frag(sgt_cache->buf[i]);
1911		sgt_cache->count = 0;
1912	}
1913}
1914
1915static int dpaa2_eth_pull_channel(struct dpaa2_eth_channel *ch)
1916{
1917	int err;
1918	int dequeues = -1;
1919
1920	/* Retry while portal is busy */
1921	do {
1922		err = dpaa2_io_service_pull_channel(ch->dpio, ch->ch_id,
1923						    ch->store);
1924		dequeues++;
1925		cpu_relax();
1926	} while (err == -EBUSY && dequeues < DPAA2_ETH_SWP_BUSY_RETRIES);
1927
1928	ch->stats.dequeue_portal_busy += dequeues;
1929	if (unlikely(err))
1930		ch->stats.pull_err++;
1931
1932	return err;
1933}
1934
1935/* NAPI poll routine
1936 *
1937 * Frames are dequeued from the QMan channel associated with this NAPI context.
1938 * Rx, Tx confirmation and (if configured) Rx error frames all count
1939 * towards the NAPI budget.
1940 */
1941static int dpaa2_eth_poll(struct napi_struct *napi, int budget)
1942{
1943	struct dpaa2_eth_channel *ch;
1944	struct dpaa2_eth_priv *priv;
1945	int rx_cleaned = 0, txconf_cleaned = 0;
1946	struct dpaa2_eth_fq *fq, *txc_fq = NULL;
1947	struct netdev_queue *nq;
1948	int store_cleaned, work_done;
1949	bool work_done_zc = false;
1950	struct list_head rx_list;
1951	int retries = 0;
1952	u16 flowid;
1953	int err;
1954
1955	ch = container_of(napi, struct dpaa2_eth_channel, napi);
1956	ch->xdp.res = 0;
1957	priv = ch->priv;
1958
1959	INIT_LIST_HEAD(&rx_list);
1960	ch->rx_list = &rx_list;
1961
1962	if (ch->xsk_zc) {
1963		work_done_zc = dpaa2_xsk_tx(priv, ch);
1964		/* If we reached the XSK Tx per NAPI threshold, we're done */
1965		if (work_done_zc) {
1966			work_done = budget;
1967			goto out;
1968		}
1969	}
1970
1971	do {
1972		err = dpaa2_eth_pull_channel(ch);
1973		if (unlikely(err))
1974			break;
1975
1976		/* Refill pool if appropriate */
1977		dpaa2_eth_refill_pool(priv, ch);
1978
1979		store_cleaned = dpaa2_eth_consume_frames(ch, &fq);
1980		if (store_cleaned <= 0)
1981			break;
1982		if (fq->type == DPAA2_RX_FQ) {
1983			rx_cleaned += store_cleaned;
1984			flowid = fq->flowid;
1985		} else {
1986			txconf_cleaned += store_cleaned;
1987			/* We have a single Tx conf FQ on this channel */
1988			txc_fq = fq;
1989		}
1990
1991		/* If we either consumed the whole NAPI budget with Rx frames
1992		 * or we reached the Tx confirmations threshold, we're done.
1993		 */
1994		if (rx_cleaned >= budget ||
1995		    txconf_cleaned >= DPAA2_ETH_TXCONF_PER_NAPI) {
1996			work_done = budget;
1997			if (ch->xdp.res & XDP_REDIRECT)
1998				xdp_do_flush();
1999			goto out;
2000		}
2001	} while (store_cleaned);
2002
2003	if (ch->xdp.res & XDP_REDIRECT)
2004		xdp_do_flush();
2005
2006	/* Update NET DIM with the values for this CDAN */
2007	dpaa2_io_update_net_dim(ch->dpio, ch->stats.frames_per_cdan,
2008				ch->stats.bytes_per_cdan);
2009	ch->stats.frames_per_cdan = 0;
2010	ch->stats.bytes_per_cdan = 0;
2011
2012	/* We didn't consume the entire budget, so finish napi and
2013	 * re-enable data availability notifications
2014	 */
2015	napi_complete_done(napi, rx_cleaned);
2016	do {
2017		err = dpaa2_io_service_rearm(ch->dpio, &ch->nctx);
2018		cpu_relax();
2019	} while (err == -EBUSY && retries++ < DPAA2_ETH_SWP_BUSY_RETRIES);
2020	WARN_ONCE(err, "CDAN notifications rearm failed on core %d",
2021		  ch->nctx.desired_cpu);
2022
2023	work_done = max(rx_cleaned, 1);
2024
2025out:
2026	netif_receive_skb_list(ch->rx_list);
2027
2028	if (ch->xsk_tx_pkts_sent) {
2029		xsk_tx_completed(ch->xsk_pool, ch->xsk_tx_pkts_sent);
2030		ch->xsk_tx_pkts_sent = 0;
2031	}
2032
2033	if (txc_fq && txc_fq->dq_frames) {
2034		nq = netdev_get_tx_queue(priv->net_dev, txc_fq->flowid);
2035		netdev_tx_completed_queue(nq, txc_fq->dq_frames,
2036					  txc_fq->dq_bytes);
2037		txc_fq->dq_frames = 0;
2038		txc_fq->dq_bytes = 0;
2039	}
2040
2041	if (rx_cleaned && ch->xdp.res & XDP_TX)
2042		dpaa2_eth_xdp_tx_flush(priv, ch, &priv->fq[flowid]);
2043
2044	return work_done;
2045}
2046
2047static void dpaa2_eth_enable_ch_napi(struct dpaa2_eth_priv *priv)
2048{
2049	struct dpaa2_eth_channel *ch;
2050	int i;
2051
2052	for (i = 0; i < priv->num_channels; i++) {
2053		ch = priv->channel[i];
2054		napi_enable(&ch->napi);
2055	}
2056}
2057
2058static void dpaa2_eth_disable_ch_napi(struct dpaa2_eth_priv *priv)
2059{
2060	struct dpaa2_eth_channel *ch;
2061	int i;
2062
2063	for (i = 0; i < priv->num_channels; i++) {
2064		ch = priv->channel[i];
2065		napi_disable(&ch->napi);
2066	}
2067}
2068
2069void dpaa2_eth_set_rx_taildrop(struct dpaa2_eth_priv *priv,
2070			       bool tx_pause, bool pfc)
2071{
2072	struct dpni_taildrop td = {0};
2073	struct dpaa2_eth_fq *fq;
2074	int i, err;
2075
2076	/* FQ taildrop: threshold is in bytes, per frame queue. Enabled if
2077	 * flow control is disabled (as it might interfere with either the
2078	 * buffer pool depletion trigger for pause frames or with the group
2079	 * congestion trigger for PFC frames)
2080	 */
2081	td.enable = !tx_pause;
2082	if (priv->rx_fqtd_enabled == td.enable)
2083		goto set_cgtd;
2084
2085	td.threshold = DPAA2_ETH_FQ_TAILDROP_THRESH;
2086	td.units = DPNI_CONGESTION_UNIT_BYTES;
2087
2088	for (i = 0; i < priv->num_fqs; i++) {
2089		fq = &priv->fq[i];
2090		if (fq->type != DPAA2_RX_FQ)
2091			continue;
2092		err = dpni_set_taildrop(priv->mc_io, 0, priv->mc_token,
2093					DPNI_CP_QUEUE, DPNI_QUEUE_RX,
2094					fq->tc, fq->flowid, &td);
2095		if (err) {
2096			netdev_err(priv->net_dev,
2097				   "dpni_set_taildrop(FQ) failed\n");
2098			return;
2099		}
2100	}
2101
2102	priv->rx_fqtd_enabled = td.enable;
2103
2104set_cgtd:
2105	/* Congestion group taildrop: threshold is in frames, per group
2106	 * of FQs belonging to the same traffic class
2107	 * Enabled if general Tx pause disabled or if PFCs are enabled
2108	 * (congestion group threhsold for PFC generation is lower than the
2109	 * CG taildrop threshold, so it won't interfere with it; we also
2110	 * want frames in non-PFC enabled traffic classes to be kept in check)
2111	 */
2112	td.enable = !tx_pause || pfc;
2113	if (priv->rx_cgtd_enabled == td.enable)
2114		return;
2115
2116	td.threshold = DPAA2_ETH_CG_TAILDROP_THRESH(priv);
2117	td.units = DPNI_CONGESTION_UNIT_FRAMES;
2118	for (i = 0; i < dpaa2_eth_tc_count(priv); i++) {
2119		err = dpni_set_taildrop(priv->mc_io, 0, priv->mc_token,
2120					DPNI_CP_GROUP, DPNI_QUEUE_RX,
2121					i, 0, &td);
2122		if (err) {
2123			netdev_err(priv->net_dev,
2124				   "dpni_set_taildrop(CG) failed\n");
2125			return;
2126		}
2127	}
2128
2129	priv->rx_cgtd_enabled = td.enable;
2130}
2131
2132static int dpaa2_eth_link_state_update(struct dpaa2_eth_priv *priv)
2133{
2134	struct dpni_link_state state = {0};
2135	bool tx_pause;
2136	int err;
2137
2138	err = dpni_get_link_state(priv->mc_io, 0, priv->mc_token, &state);
2139	if (unlikely(err)) {
2140		netdev_err(priv->net_dev,
2141			   "dpni_get_link_state() failed\n");
2142		return err;
2143	}
2144
2145	/* If Tx pause frame settings have changed, we need to update
2146	 * Rx FQ taildrop configuration as well. We configure taildrop
2147	 * only when pause frame generation is disabled.
2148	 */
2149	tx_pause = dpaa2_eth_tx_pause_enabled(state.options);
2150	dpaa2_eth_set_rx_taildrop(priv, tx_pause, priv->pfc_enabled);
2151
2152	/* When we manage the MAC/PHY using phylink there is no need
2153	 * to manually update the netif_carrier.
2154	 * We can avoid locking because we are called from the "link changed"
2155	 * IRQ handler, which is the same as the "endpoint changed" IRQ handler
2156	 * (the writer to priv->mac), so we cannot race with it.
2157	 */
2158	if (dpaa2_mac_is_type_phy(priv->mac))
2159		goto out;
2160
2161	/* Chech link state; speed / duplex changes are not treated yet */
2162	if (priv->link_state.up == state.up)
2163		goto out;
2164
2165	if (state.up) {
2166		netif_carrier_on(priv->net_dev);
2167		netif_tx_start_all_queues(priv->net_dev);
2168	} else {
2169		netif_tx_stop_all_queues(priv->net_dev);
2170		netif_carrier_off(priv->net_dev);
2171	}
2172
2173	netdev_info(priv->net_dev, "Link Event: state %s\n",
2174		    state.up ? "up" : "down");
2175
2176out:
2177	priv->link_state = state;
2178
2179	return 0;
2180}
2181
2182static int dpaa2_eth_open(struct net_device *net_dev)
2183{
2184	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
2185	int err;
2186
2187	dpaa2_eth_seed_pools(priv);
2188
2189	mutex_lock(&priv->mac_lock);
2190
2191	if (!dpaa2_eth_is_type_phy(priv)) {
2192		/* We'll only start the txqs when the link is actually ready;
2193		 * make sure we don't race against the link up notification,
2194		 * which may come immediately after dpni_enable();
2195		 */
2196		netif_tx_stop_all_queues(net_dev);
2197
2198		/* Also, explicitly set carrier off, otherwise
2199		 * netif_carrier_ok() will return true and cause 'ip link show'
2200		 * to report the LOWER_UP flag, even though the link
2201		 * notification wasn't even received.
2202		 */
2203		netif_carrier_off(net_dev);
2204	}
2205	dpaa2_eth_enable_ch_napi(priv);
2206
2207	err = dpni_enable(priv->mc_io, 0, priv->mc_token);
2208	if (err < 0) {
2209		mutex_unlock(&priv->mac_lock);
2210		netdev_err(net_dev, "dpni_enable() failed\n");
2211		goto enable_err;
2212	}
2213
2214	if (dpaa2_eth_is_type_phy(priv))
2215		dpaa2_mac_start(priv->mac);
2216
2217	mutex_unlock(&priv->mac_lock);
2218
2219	return 0;
2220
2221enable_err:
2222	dpaa2_eth_disable_ch_napi(priv);
2223	dpaa2_eth_drain_pools(priv);
2224	return err;
2225}
2226
2227/* Total number of in-flight frames on ingress queues */
2228static u32 dpaa2_eth_ingress_fq_count(struct dpaa2_eth_priv *priv)
2229{
2230	struct dpaa2_eth_fq *fq;
2231	u32 fcnt = 0, bcnt = 0, total = 0;
2232	int i, err;
2233
2234	for (i = 0; i < priv->num_fqs; i++) {
2235		fq = &priv->fq[i];
2236		err = dpaa2_io_query_fq_count(NULL, fq->fqid, &fcnt, &bcnt);
2237		if (err) {
2238			netdev_warn(priv->net_dev, "query_fq_count failed");
2239			break;
2240		}
2241		total += fcnt;
2242	}
2243
2244	return total;
2245}
2246
2247static void dpaa2_eth_wait_for_ingress_fq_empty(struct dpaa2_eth_priv *priv)
2248{
2249	int retries = 10;
2250	u32 pending;
2251
2252	do {
2253		pending = dpaa2_eth_ingress_fq_count(priv);
2254		if (pending)
2255			msleep(100);
2256	} while (pending && --retries);
2257}
2258
2259#define DPNI_TX_PENDING_VER_MAJOR	7
2260#define DPNI_TX_PENDING_VER_MINOR	13
2261static void dpaa2_eth_wait_for_egress_fq_empty(struct dpaa2_eth_priv *priv)
2262{
2263	union dpni_statistics stats;
2264	int retries = 10;
2265	int err;
2266
2267	if (dpaa2_eth_cmp_dpni_ver(priv, DPNI_TX_PENDING_VER_MAJOR,
2268				   DPNI_TX_PENDING_VER_MINOR) < 0)
2269		goto out;
2270
2271	do {
2272		err = dpni_get_statistics(priv->mc_io, 0, priv->mc_token, 6,
2273					  &stats);
2274		if (err)
2275			goto out;
2276		if (stats.page_6.tx_pending_frames == 0)
2277			return;
2278	} while (--retries);
2279
2280out:
2281	msleep(500);
2282}
2283
2284static int dpaa2_eth_stop(struct net_device *net_dev)
2285{
2286	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
2287	int dpni_enabled = 0;
2288	int retries = 10;
2289
2290	mutex_lock(&priv->mac_lock);
2291
2292	if (dpaa2_eth_is_type_phy(priv)) {
2293		dpaa2_mac_stop(priv->mac);
2294	} else {
2295		netif_tx_stop_all_queues(net_dev);
2296		netif_carrier_off(net_dev);
2297	}
2298
2299	mutex_unlock(&priv->mac_lock);
2300
2301	/* On dpni_disable(), the MC firmware will:
2302	 * - stop MAC Rx and wait for all Rx frames to be enqueued to software
2303	 * - cut off WRIOP dequeues from egress FQs and wait until transmission
2304	 * of all in flight Tx frames is finished (and corresponding Tx conf
2305	 * frames are enqueued back to software)
2306	 *
2307	 * Before calling dpni_disable(), we wait for all Tx frames to arrive
2308	 * on WRIOP. After it finishes, wait until all remaining frames on Rx
2309	 * and Tx conf queues are consumed on NAPI poll.
2310	 */
2311	dpaa2_eth_wait_for_egress_fq_empty(priv);
2312
2313	do {
2314		dpni_disable(priv->mc_io, 0, priv->mc_token);
2315		dpni_is_enabled(priv->mc_io, 0, priv->mc_token, &dpni_enabled);
2316		if (dpni_enabled)
2317			/* Allow the hardware some slack */
2318			msleep(100);
2319	} while (dpni_enabled && --retries);
2320	if (!retries) {
2321		netdev_warn(net_dev, "Retry count exceeded disabling DPNI\n");
2322		/* Must go on and disable NAPI nonetheless, so we don't crash at
2323		 * the next "ifconfig up"
2324		 */
2325	}
2326
2327	dpaa2_eth_wait_for_ingress_fq_empty(priv);
2328	dpaa2_eth_disable_ch_napi(priv);
2329
2330	/* Empty the buffer pool */
2331	dpaa2_eth_drain_pools(priv);
2332
2333	/* Empty the Scatter-Gather Buffer cache */
2334	dpaa2_eth_sgt_cache_drain(priv);
2335
2336	return 0;
2337}
2338
2339static int dpaa2_eth_set_addr(struct net_device *net_dev, void *addr)
2340{
2341	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
2342	struct device *dev = net_dev->dev.parent;
2343	int err;
2344
2345	err = eth_mac_addr(net_dev, addr);
2346	if (err < 0) {
2347		dev_err(dev, "eth_mac_addr() failed (%d)\n", err);
2348		return err;
2349	}
2350
2351	err = dpni_set_primary_mac_addr(priv->mc_io, 0, priv->mc_token,
2352					net_dev->dev_addr);
2353	if (err) {
2354		dev_err(dev, "dpni_set_primary_mac_addr() failed (%d)\n", err);
2355		return err;
2356	}
2357
2358	return 0;
2359}
2360
2361/** Fill in counters maintained by the GPP driver. These may be different from
2362 * the hardware counters obtained by ethtool.
2363 */
2364static void dpaa2_eth_get_stats(struct net_device *net_dev,
2365				struct rtnl_link_stats64 *stats)
2366{
2367	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
2368	struct rtnl_link_stats64 *percpu_stats;
2369	u64 *cpustats;
2370	u64 *netstats = (u64 *)stats;
2371	int i, j;
2372	int num = sizeof(struct rtnl_link_stats64) / sizeof(u64);
2373
2374	for_each_possible_cpu(i) {
2375		percpu_stats = per_cpu_ptr(priv->percpu_stats, i);
2376		cpustats = (u64 *)percpu_stats;
2377		for (j = 0; j < num; j++)
2378			netstats[j] += cpustats[j];
2379	}
2380}
2381
2382/* Copy mac unicast addresses from @net_dev to @priv.
2383 * Its sole purpose is to make dpaa2_eth_set_rx_mode() more readable.
2384 */
2385static void dpaa2_eth_add_uc_hw_addr(const struct net_device *net_dev,
2386				     struct dpaa2_eth_priv *priv)
2387{
2388	struct netdev_hw_addr *ha;
2389	int err;
2390
2391	netdev_for_each_uc_addr(ha, net_dev) {
2392		err = dpni_add_mac_addr(priv->mc_io, 0, priv->mc_token,
2393					ha->addr);
2394		if (err)
2395			netdev_warn(priv->net_dev,
2396				    "Could not add ucast MAC %pM to the filtering table (err %d)\n",
2397				    ha->addr, err);
2398	}
2399}
2400
2401/* Copy mac multicast addresses from @net_dev to @priv
2402 * Its sole purpose is to make dpaa2_eth_set_rx_mode() more readable.
2403 */
2404static void dpaa2_eth_add_mc_hw_addr(const struct net_device *net_dev,
2405				     struct dpaa2_eth_priv *priv)
2406{
2407	struct netdev_hw_addr *ha;
2408	int err;
2409
2410	netdev_for_each_mc_addr(ha, net_dev) {
2411		err = dpni_add_mac_addr(priv->mc_io, 0, priv->mc_token,
2412					ha->addr);
2413		if (err)
2414			netdev_warn(priv->net_dev,
2415				    "Could not add mcast MAC %pM to the filtering table (err %d)\n",
2416				    ha->addr, err);
2417	}
2418}
2419
2420static int dpaa2_eth_rx_add_vid(struct net_device *net_dev,
2421				__be16 vlan_proto, u16 vid)
2422{
2423	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
2424	int err;
2425
2426	err = dpni_add_vlan_id(priv->mc_io, 0, priv->mc_token,
2427			       vid, 0, 0, 0);
2428
2429	if (err) {
2430		netdev_warn(priv->net_dev,
2431			    "Could not add the vlan id %u\n",
2432			    vid);
2433		return err;
2434	}
2435
2436	return 0;
2437}
2438
2439static int dpaa2_eth_rx_kill_vid(struct net_device *net_dev,
2440				 __be16 vlan_proto, u16 vid)
2441{
2442	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
2443	int err;
2444
2445	err = dpni_remove_vlan_id(priv->mc_io, 0, priv->mc_token, vid);
2446
2447	if (err) {
2448		netdev_warn(priv->net_dev,
2449			    "Could not remove the vlan id %u\n",
2450			    vid);
2451		return err;
2452	}
2453
2454	return 0;
2455}
2456
2457static void dpaa2_eth_set_rx_mode(struct net_device *net_dev)
2458{
2459	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
2460	int uc_count = netdev_uc_count(net_dev);
2461	int mc_count = netdev_mc_count(net_dev);
2462	u8 max_mac = priv->dpni_attrs.mac_filter_entries;
2463	u32 options = priv->dpni_attrs.options;
2464	u16 mc_token = priv->mc_token;
2465	struct fsl_mc_io *mc_io = priv->mc_io;
2466	int err;
2467
2468	/* Basic sanity checks; these probably indicate a misconfiguration */
2469	if (options & DPNI_OPT_NO_MAC_FILTER && max_mac != 0)
2470		netdev_info(net_dev,
2471			    "mac_filter_entries=%d, DPNI_OPT_NO_MAC_FILTER option must be disabled\n",
2472			    max_mac);
2473
2474	/* Force promiscuous if the uc or mc counts exceed our capabilities. */
2475	if (uc_count > max_mac) {
2476		netdev_info(net_dev,
2477			    "Unicast addr count reached %d, max allowed is %d; forcing promisc\n",
2478			    uc_count, max_mac);
2479		goto force_promisc;
2480	}
2481	if (mc_count + uc_count > max_mac) {
2482		netdev_info(net_dev,
2483			    "Unicast + multicast addr count reached %d, max allowed is %d; forcing promisc\n",
2484			    uc_count + mc_count, max_mac);
2485		goto force_mc_promisc;
2486	}
2487
2488	/* Adjust promisc settings due to flag combinations */
2489	if (net_dev->flags & IFF_PROMISC)
2490		goto force_promisc;
2491	if (net_dev->flags & IFF_ALLMULTI) {
2492		/* First, rebuild unicast filtering table. This should be done
2493		 * in promisc mode, in order to avoid frame loss while we
2494		 * progressively add entries to the table.
2495		 * We don't know whether we had been in promisc already, and
2496		 * making an MC call to find out is expensive; so set uc promisc
2497		 * nonetheless.
2498		 */
2499		err = dpni_set_unicast_promisc(mc_io, 0, mc_token, 1);
2500		if (err)
2501			netdev_warn(net_dev, "Can't set uc promisc\n");
2502
2503		/* Actual uc table reconstruction. */
2504		err = dpni_clear_mac_filters(mc_io, 0, mc_token, 1, 0);
2505		if (err)
2506			netdev_warn(net_dev, "Can't clear uc filters\n");
2507		dpaa2_eth_add_uc_hw_addr(net_dev, priv);
2508
2509		/* Finally, clear uc promisc and set mc promisc as requested. */
2510		err = dpni_set_unicast_promisc(mc_io, 0, mc_token, 0);
2511		if (err)
2512			netdev_warn(net_dev, "Can't clear uc promisc\n");
2513		goto force_mc_promisc;
2514	}
2515
2516	/* Neither unicast, nor multicast promisc will be on... eventually.
2517	 * For now, rebuild mac filtering tables while forcing both of them on.
2518	 */
2519	err = dpni_set_unicast_promisc(mc_io, 0, mc_token, 1);
2520	if (err)
2521		netdev_warn(net_dev, "Can't set uc promisc (%d)\n", err);
2522	err = dpni_set_multicast_promisc(mc_io, 0, mc_token, 1);
2523	if (err)
2524		netdev_warn(net_dev, "Can't set mc promisc (%d)\n", err);
2525
2526	/* Actual mac filtering tables reconstruction */
2527	err = dpni_clear_mac_filters(mc_io, 0, mc_token, 1, 1);
2528	if (err)
2529		netdev_warn(net_dev, "Can't clear mac filters\n");
2530	dpaa2_eth_add_mc_hw_addr(net_dev, priv);
2531	dpaa2_eth_add_uc_hw_addr(net_dev, priv);
2532
2533	/* Now we can clear both ucast and mcast promisc, without risking
2534	 * to drop legitimate frames anymore.
2535	 */
2536	err = dpni_set_unicast_promisc(mc_io, 0, mc_token, 0);
2537	if (err)
2538		netdev_warn(net_dev, "Can't clear ucast promisc\n");
2539	err = dpni_set_multicast_promisc(mc_io, 0, mc_token, 0);
2540	if (err)
2541		netdev_warn(net_dev, "Can't clear mcast promisc\n");
2542
2543	return;
2544
2545force_promisc:
2546	err = dpni_set_unicast_promisc(mc_io, 0, mc_token, 1);
2547	if (err)
2548		netdev_warn(net_dev, "Can't set ucast promisc\n");
2549force_mc_promisc:
2550	err = dpni_set_multicast_promisc(mc_io, 0, mc_token, 1);
2551	if (err)
2552		netdev_warn(net_dev, "Can't set mcast promisc\n");
2553}
2554
2555static int dpaa2_eth_set_features(struct net_device *net_dev,
2556				  netdev_features_t features)
2557{
2558	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
2559	netdev_features_t changed = features ^ net_dev->features;
2560	bool enable;
2561	int err;
2562
2563	if (changed & NETIF_F_HW_VLAN_CTAG_FILTER) {
2564		enable = !!(features & NETIF_F_HW_VLAN_CTAG_FILTER);
2565		err = dpaa2_eth_set_rx_vlan_filtering(priv, enable);
2566		if (err)
2567			return err;
2568	}
2569
2570	if (changed & NETIF_F_RXCSUM) {
2571		enable = !!(features & NETIF_F_RXCSUM);
2572		err = dpaa2_eth_set_rx_csum(priv, enable);
2573		if (err)
2574			return err;
2575	}
2576
2577	if (changed & (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM)) {
2578		enable = !!(features & (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM));
2579		err = dpaa2_eth_set_tx_csum(priv, enable);
2580		if (err)
2581			return err;
2582	}
2583
2584	return 0;
2585}
2586
2587static int dpaa2_eth_hwtstamp_set(struct net_device *dev,
2588				  struct kernel_hwtstamp_config *config,
2589				  struct netlink_ext_ack *extack)
2590{
2591	struct dpaa2_eth_priv *priv = netdev_priv(dev);
2592
2593	if (!dpaa2_ptp)
2594		return -EINVAL;
2595
2596	switch (config->tx_type) {
2597	case HWTSTAMP_TX_OFF:
2598	case HWTSTAMP_TX_ON:
2599	case HWTSTAMP_TX_ONESTEP_SYNC:
2600		priv->tx_tstamp_type = config->tx_type;
2601		break;
2602	default:
2603		return -ERANGE;
2604	}
2605
2606	if (config->rx_filter == HWTSTAMP_FILTER_NONE) {
2607		priv->rx_tstamp = false;
2608	} else {
2609		priv->rx_tstamp = true;
2610		/* TS is set for all frame types, not only those requested */
2611		config->rx_filter = HWTSTAMP_FILTER_ALL;
2612	}
2613
2614	if (priv->tx_tstamp_type == HWTSTAMP_TX_ONESTEP_SYNC)
2615		dpaa2_ptp_onestep_reg_update_method(priv);
2616
2617	return 0;
2618}
2619
2620static int dpaa2_eth_hwtstamp_get(struct net_device *dev,
2621				  struct kernel_hwtstamp_config *config)
2622{
2623	struct dpaa2_eth_priv *priv = netdev_priv(dev);
2624
2625	if (!dpaa2_ptp)
2626		return -EINVAL;
2627
2628	config->tx_type = priv->tx_tstamp_type;
2629	config->rx_filter = priv->rx_tstamp ? HWTSTAMP_FILTER_ALL :
2630			    HWTSTAMP_FILTER_NONE;
2631
2632	return 0;
2633}
2634
2635static int dpaa2_eth_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
2636{
2637	struct dpaa2_eth_priv *priv = netdev_priv(dev);
2638	int err;
2639
2640	mutex_lock(&priv->mac_lock);
2641
2642	if (dpaa2_eth_is_type_phy(priv)) {
2643		err = phylink_mii_ioctl(priv->mac->phylink, rq, cmd);
2644		mutex_unlock(&priv->mac_lock);
2645		return err;
2646	}
2647
2648	mutex_unlock(&priv->mac_lock);
2649
2650	return -EOPNOTSUPP;
2651}
2652
2653static bool xdp_mtu_valid(struct dpaa2_eth_priv *priv, int mtu)
2654{
2655	int mfl, linear_mfl;
2656
2657	mfl = DPAA2_ETH_L2_MAX_FRM(mtu);
2658	linear_mfl = priv->rx_buf_size - DPAA2_ETH_RX_HWA_SIZE -
2659		     dpaa2_eth_rx_head_room(priv) - XDP_PACKET_HEADROOM;
2660
2661	if (mfl > linear_mfl) {
2662		netdev_warn(priv->net_dev, "Maximum MTU for XDP is %d\n",
2663			    linear_mfl - VLAN_ETH_HLEN);
2664		return false;
2665	}
2666
2667	return true;
2668}
2669
2670static int dpaa2_eth_set_rx_mfl(struct dpaa2_eth_priv *priv, int mtu, bool has_xdp)
2671{
2672	int mfl, err;
2673
2674	/* We enforce a maximum Rx frame length based on MTU only if we have
2675	 * an XDP program attached (in order to avoid Rx S/G frames).
2676	 * Otherwise, we accept all incoming frames as long as they are not
2677	 * larger than maximum size supported in hardware
2678	 */
2679	if (has_xdp)
2680		mfl = DPAA2_ETH_L2_MAX_FRM(mtu);
2681	else
2682		mfl = DPAA2_ETH_MFL;
2683
2684	err = dpni_set_max_frame_length(priv->mc_io, 0, priv->mc_token, mfl);
2685	if (err) {
2686		netdev_err(priv->net_dev, "dpni_set_max_frame_length failed\n");
2687		return err;
2688	}
2689
2690	return 0;
2691}
2692
2693static int dpaa2_eth_change_mtu(struct net_device *dev, int new_mtu)
2694{
2695	struct dpaa2_eth_priv *priv = netdev_priv(dev);
2696	int err;
2697
2698	if (!priv->xdp_prog)
2699		goto out;
2700
2701	if (!xdp_mtu_valid(priv, new_mtu))
2702		return -EINVAL;
2703
2704	err = dpaa2_eth_set_rx_mfl(priv, new_mtu, true);
2705	if (err)
2706		return err;
2707
2708out:
2709	WRITE_ONCE(dev->mtu, new_mtu);
2710	return 0;
2711}
2712
2713static int dpaa2_eth_update_rx_buffer_headroom(struct dpaa2_eth_priv *priv, bool has_xdp)
2714{
2715	struct dpni_buffer_layout buf_layout = {0};
2716	int err;
2717
2718	err = dpni_get_buffer_layout(priv->mc_io, 0, priv->mc_token,
2719				     DPNI_QUEUE_RX, &buf_layout);
2720	if (err) {
2721		netdev_err(priv->net_dev, "dpni_get_buffer_layout failed\n");
2722		return err;
2723	}
2724
2725	/* Reserve extra headroom for XDP header size changes */
2726	buf_layout.data_head_room = dpaa2_eth_rx_head_room(priv) +
2727				    (has_xdp ? XDP_PACKET_HEADROOM : 0);
2728	buf_layout.options = DPNI_BUF_LAYOUT_OPT_DATA_HEAD_ROOM;
2729	err = dpni_set_buffer_layout(priv->mc_io, 0, priv->mc_token,
2730				     DPNI_QUEUE_RX, &buf_layout);
2731	if (err) {
2732		netdev_err(priv->net_dev, "dpni_set_buffer_layout failed\n");
2733		return err;
2734	}
2735
2736	return 0;
2737}
2738
2739static int dpaa2_eth_setup_xdp(struct net_device *dev, struct bpf_prog *prog)
2740{
2741	struct dpaa2_eth_priv *priv = netdev_priv(dev);
2742	struct dpaa2_eth_channel *ch;
2743	struct bpf_prog *old;
2744	bool up, need_update;
2745	int i, err;
2746
2747	if (prog && !xdp_mtu_valid(priv, dev->mtu))
2748		return -EINVAL;
2749
2750	if (prog)
2751		bpf_prog_add(prog, priv->num_channels);
2752
2753	up = netif_running(dev);
2754	need_update = (!!priv->xdp_prog != !!prog);
2755
2756	if (up)
2757		dev_close(dev);
2758
2759	/* While in xdp mode, enforce a maximum Rx frame size based on MTU.
2760	 * Also, when switching between xdp/non-xdp modes we need to reconfigure
2761	 * our Rx buffer layout. Buffer pool was drained on dpaa2_eth_stop,
2762	 * so we are sure no old format buffers will be used from now on.
2763	 */
2764	if (need_update) {
2765		err = dpaa2_eth_set_rx_mfl(priv, dev->mtu, !!prog);
2766		if (err)
2767			goto out_err;
2768		err = dpaa2_eth_update_rx_buffer_headroom(priv, !!prog);
2769		if (err)
2770			goto out_err;
2771	}
2772
2773	old = xchg(&priv->xdp_prog, prog);
2774	if (old)
2775		bpf_prog_put(old);
2776
2777	for (i = 0; i < priv->num_channels; i++) {
2778		ch = priv->channel[i];
2779		old = xchg(&ch->xdp.prog, prog);
2780		if (old)
2781			bpf_prog_put(old);
2782	}
2783
2784	if (up) {
2785		err = dev_open(dev, NULL);
2786		if (err)
2787			return err;
2788	}
2789
2790	return 0;
2791
2792out_err:
2793	if (prog)
2794		bpf_prog_sub(prog, priv->num_channels);
2795	if (up)
2796		dev_open(dev, NULL);
2797
2798	return err;
2799}
2800
2801static int dpaa2_eth_xdp(struct net_device *dev, struct netdev_bpf *xdp)
2802{
2803	switch (xdp->command) {
2804	case XDP_SETUP_PROG:
2805		return dpaa2_eth_setup_xdp(dev, xdp->prog);
2806	case XDP_SETUP_XSK_POOL:
2807		return dpaa2_xsk_setup_pool(dev, xdp->xsk.pool, xdp->xsk.queue_id);
2808	default:
2809		return -EINVAL;
2810	}
2811
2812	return 0;
2813}
2814
2815static int dpaa2_eth_xdp_create_fd(struct net_device *net_dev,
2816				   struct xdp_frame *xdpf,
2817				   struct dpaa2_fd *fd)
2818{
2819	struct device *dev = net_dev->dev.parent;
2820	unsigned int needed_headroom;
2821	struct dpaa2_eth_swa *swa;
2822	void *buffer_start, *aligned_start;
2823	dma_addr_t addr;
2824
2825	/* We require a minimum headroom to be able to transmit the frame.
2826	 * Otherwise return an error and let the original net_device handle it
2827	 */
2828	needed_headroom = dpaa2_eth_needed_headroom(NULL);
2829	if (xdpf->headroom < needed_headroom)
2830		return -EINVAL;
2831
2832	/* Setup the FD fields */
2833	memset(fd, 0, sizeof(*fd));
2834
2835	/* Align FD address, if possible */
2836	buffer_start = xdpf->data - needed_headroom;
2837	aligned_start = PTR_ALIGN(buffer_start - DPAA2_ETH_TX_BUF_ALIGN,
2838				  DPAA2_ETH_TX_BUF_ALIGN);
2839	if (aligned_start >= xdpf->data - xdpf->headroom)
2840		buffer_start = aligned_start;
2841
2842	swa = (struct dpaa2_eth_swa *)buffer_start;
2843	/* fill in necessary fields here */
2844	swa->type = DPAA2_ETH_SWA_XDP;
2845	swa->xdp.dma_size = xdpf->data + xdpf->len - buffer_start;
2846	swa->xdp.xdpf = xdpf;
2847
2848	addr = dma_map_single(dev, buffer_start,
2849			      swa->xdp.dma_size,
2850			      DMA_BIDIRECTIONAL);
2851	if (unlikely(dma_mapping_error(dev, addr)))
2852		return -ENOMEM;
2853
2854	dpaa2_fd_set_addr(fd, addr);
2855	dpaa2_fd_set_offset(fd, xdpf->data - buffer_start);
2856	dpaa2_fd_set_len(fd, xdpf->len);
2857	dpaa2_fd_set_format(fd, dpaa2_fd_single);
2858	dpaa2_fd_set_ctrl(fd, FD_CTRL_PTA);
2859
2860	return 0;
2861}
2862
2863static int dpaa2_eth_xdp_xmit(struct net_device *net_dev, int n,
2864			      struct xdp_frame **frames, u32 flags)
2865{
2866	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
2867	struct dpaa2_eth_xdp_fds *xdp_redirect_fds;
2868	struct rtnl_link_stats64 *percpu_stats;
2869	struct dpaa2_eth_fq *fq;
2870	struct dpaa2_fd *fds;
2871	int enqueued, i, err;
2872
2873	if (unlikely(flags & ~XDP_XMIT_FLAGS_MASK))
2874		return -EINVAL;
2875
2876	if (!netif_running(net_dev))
2877		return -ENETDOWN;
2878
2879	fq = &priv->fq[smp_processor_id()];
2880	xdp_redirect_fds = &fq->xdp_redirect_fds;
2881	fds = xdp_redirect_fds->fds;
2882
2883	percpu_stats = this_cpu_ptr(priv->percpu_stats);
2884
2885	/* create a FD for each xdp_frame in the list received */
2886	for (i = 0; i < n; i++) {
2887		err = dpaa2_eth_xdp_create_fd(net_dev, frames[i], &fds[i]);
2888		if (err)
2889			break;
2890	}
2891	xdp_redirect_fds->num = i;
2892
2893	/* enqueue all the frame descriptors */
2894	enqueued = dpaa2_eth_xdp_flush(priv, fq, xdp_redirect_fds);
2895
2896	/* update statistics */
2897	percpu_stats->tx_packets += enqueued;
2898	for (i = 0; i < enqueued; i++)
2899		percpu_stats->tx_bytes += dpaa2_fd_get_len(&fds[i]);
2900
2901	return enqueued;
2902}
2903
2904static int update_xps(struct dpaa2_eth_priv *priv)
2905{
2906	struct net_device *net_dev = priv->net_dev;
2907	int i, num_queues, netdev_queues;
2908	struct dpaa2_eth_fq *fq;
2909	cpumask_var_t xps_mask;
2910	int err = 0;
2911
2912	if (!alloc_cpumask_var(&xps_mask, GFP_KERNEL))
2913		return -ENOMEM;
2914
2915	num_queues = dpaa2_eth_queue_count(priv);
2916	netdev_queues = (net_dev->num_tc ? : 1) * num_queues;
2917
2918	/* The first <num_queues> entries in priv->fq array are Tx/Tx conf
2919	 * queues, so only process those
2920	 */
2921	for (i = 0; i < netdev_queues; i++) {
2922		fq = &priv->fq[i % num_queues];
2923
2924		cpumask_clear(xps_mask);
2925		cpumask_set_cpu(fq->target_cpu, xps_mask);
2926
2927		err = netif_set_xps_queue(net_dev, xps_mask, i);
2928		if (err) {
2929			netdev_warn_once(net_dev, "Error setting XPS queue\n");
2930			break;
2931		}
2932	}
2933
2934	free_cpumask_var(xps_mask);
2935	return err;
2936}
2937
2938static int dpaa2_eth_setup_mqprio(struct net_device *net_dev,
2939				  struct tc_mqprio_qopt *mqprio)
2940{
2941	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
2942	u8 num_tc, num_queues;
2943	int i;
2944
2945	mqprio->hw = TC_MQPRIO_HW_OFFLOAD_TCS;
2946	num_queues = dpaa2_eth_queue_count(priv);
2947	num_tc = mqprio->num_tc;
2948
2949	if (num_tc == net_dev->num_tc)
2950		return 0;
2951
2952	if (num_tc  > dpaa2_eth_tc_count(priv)) {
2953		netdev_err(net_dev, "Max %d traffic classes supported\n",
2954			   dpaa2_eth_tc_count(priv));
2955		return -EOPNOTSUPP;
2956	}
2957
2958	if (!num_tc) {
2959		netdev_reset_tc(net_dev);
2960		netif_set_real_num_tx_queues(net_dev, num_queues);
2961		goto out;
2962	}
2963
2964	netdev_set_num_tc(net_dev, num_tc);
2965	netif_set_real_num_tx_queues(net_dev, num_tc * num_queues);
2966
2967	for (i = 0; i < num_tc; i++)
2968		netdev_set_tc_queue(net_dev, i, num_queues, i * num_queues);
2969
2970out:
2971	update_xps(priv);
2972
2973	return 0;
2974}
2975
2976#define bps_to_mbits(rate) (div_u64((rate), 1000000) * 8)
2977
2978static int dpaa2_eth_setup_tbf(struct net_device *net_dev, struct tc_tbf_qopt_offload *p)
2979{
2980	struct tc_tbf_qopt_offload_replace_params *cfg = &p->replace_params;
2981	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
2982	struct dpni_tx_shaping_cfg tx_cr_shaper = { 0 };
2983	struct dpni_tx_shaping_cfg tx_er_shaper = { 0 };
2984	int err;
2985
2986	if (p->command == TC_TBF_STATS)
2987		return -EOPNOTSUPP;
2988
2989	/* Only per port Tx shaping */
2990	if (p->parent != TC_H_ROOT)
2991		return -EOPNOTSUPP;
2992
2993	if (p->command == TC_TBF_REPLACE) {
2994		if (cfg->max_size > DPAA2_ETH_MAX_BURST_SIZE) {
2995			netdev_err(net_dev, "burst size cannot be greater than %d\n",
2996				   DPAA2_ETH_MAX_BURST_SIZE);
2997			return -EINVAL;
2998		}
2999
3000		tx_cr_shaper.max_burst_size = cfg->max_size;
3001		/* The TBF interface is in bytes/s, whereas DPAA2 expects the
3002		 * rate in Mbits/s
3003		 */
3004		tx_cr_shaper.rate_limit = bps_to_mbits(cfg->rate.rate_bytes_ps);
3005	}
3006
3007	err = dpni_set_tx_shaping(priv->mc_io, 0, priv->mc_token, &tx_cr_shaper,
3008				  &tx_er_shaper, 0);
3009	if (err) {
3010		netdev_err(net_dev, "dpni_set_tx_shaping() = %d\n", err);
3011		return err;
3012	}
3013
3014	return 0;
3015}
3016
3017static int dpaa2_eth_setup_tc(struct net_device *net_dev,
3018			      enum tc_setup_type type, void *type_data)
3019{
3020	switch (type) {
3021	case TC_SETUP_QDISC_MQPRIO:
3022		return dpaa2_eth_setup_mqprio(net_dev, type_data);
3023	case TC_SETUP_QDISC_TBF:
3024		return dpaa2_eth_setup_tbf(net_dev, type_data);
3025	default:
3026		return -EOPNOTSUPP;
3027	}
3028}
3029
3030static const struct net_device_ops dpaa2_eth_ops = {
3031	.ndo_open = dpaa2_eth_open,
3032	.ndo_start_xmit = dpaa2_eth_tx,
3033	.ndo_stop = dpaa2_eth_stop,
3034	.ndo_set_mac_address = dpaa2_eth_set_addr,
3035	.ndo_get_stats64 = dpaa2_eth_get_stats,
3036	.ndo_set_rx_mode = dpaa2_eth_set_rx_mode,
3037	.ndo_set_features = dpaa2_eth_set_features,
3038	.ndo_eth_ioctl = dpaa2_eth_ioctl,
3039	.ndo_change_mtu = dpaa2_eth_change_mtu,
3040	.ndo_bpf = dpaa2_eth_xdp,
3041	.ndo_xdp_xmit = dpaa2_eth_xdp_xmit,
3042	.ndo_xsk_wakeup = dpaa2_xsk_wakeup,
3043	.ndo_setup_tc = dpaa2_eth_setup_tc,
3044	.ndo_vlan_rx_add_vid = dpaa2_eth_rx_add_vid,
3045	.ndo_vlan_rx_kill_vid = dpaa2_eth_rx_kill_vid,
3046	.ndo_hwtstamp_get = dpaa2_eth_hwtstamp_get,
3047	.ndo_hwtstamp_set = dpaa2_eth_hwtstamp_set,
3048};
3049
3050static void dpaa2_eth_cdan_cb(struct dpaa2_io_notification_ctx *ctx)
3051{
3052	struct dpaa2_eth_channel *ch;
3053
3054	ch = container_of(ctx, struct dpaa2_eth_channel, nctx);
3055
3056	/* Update NAPI statistics */
3057	ch->stats.cdan++;
3058
3059	/* NAPI can also be scheduled from the AF_XDP Tx path. Mark a missed
3060	 * so that it can be rescheduled again.
3061	 */
3062	if (!napi_if_scheduled_mark_missed(&ch->napi))
3063		napi_schedule(&ch->napi);
3064}
3065
3066/* Allocate and configure a DPCON object */
3067static struct fsl_mc_device *dpaa2_eth_setup_dpcon(struct dpaa2_eth_priv *priv)
3068{
3069	struct fsl_mc_device *dpcon;
3070	struct device *dev = priv->net_dev->dev.parent;
3071	int err;
3072
3073	err = fsl_mc_object_allocate(to_fsl_mc_device(dev),
3074				     FSL_MC_POOL_DPCON, &dpcon);
3075	if (err) {
3076		if (err == -ENXIO) {
3077			dev_dbg(dev, "Waiting for DPCON\n");
3078			err = -EPROBE_DEFER;
3079		} else {
3080			dev_info(dev, "Not enough DPCONs, will go on as-is\n");
3081		}
3082		return ERR_PTR(err);
3083	}
3084
3085	err = dpcon_open(priv->mc_io, 0, dpcon->obj_desc.id, &dpcon->mc_handle);
3086	if (err) {
3087		dev_err(dev, "dpcon_open() failed\n");
3088		goto free;
3089	}
3090
3091	err = dpcon_reset(priv->mc_io, 0, dpcon->mc_handle);
3092	if (err) {
3093		dev_err(dev, "dpcon_reset() failed\n");
3094		goto close;
3095	}
3096
3097	err = dpcon_enable(priv->mc_io, 0, dpcon->mc_handle);
3098	if (err) {
3099		dev_err(dev, "dpcon_enable() failed\n");
3100		goto close;
3101	}
3102
3103	return dpcon;
3104
3105close:
3106	dpcon_close(priv->mc_io, 0, dpcon->mc_handle);
3107free:
3108	fsl_mc_object_free(dpcon);
3109
3110	return ERR_PTR(err);
3111}
3112
3113static void dpaa2_eth_free_dpcon(struct dpaa2_eth_priv *priv,
3114				 struct fsl_mc_device *dpcon)
3115{
3116	dpcon_disable(priv->mc_io, 0, dpcon->mc_handle);
3117	dpcon_close(priv->mc_io, 0, dpcon->mc_handle);
3118	fsl_mc_object_free(dpcon);
3119}
3120
3121static struct dpaa2_eth_channel *dpaa2_eth_alloc_channel(struct dpaa2_eth_priv *priv)
3122{
3123	struct dpaa2_eth_channel *channel;
3124	struct dpcon_attr attr;
3125	struct device *dev = priv->net_dev->dev.parent;
3126	int err;
3127
3128	channel = kzalloc(sizeof(*channel), GFP_KERNEL);
3129	if (!channel)
3130		return NULL;
3131
3132	channel->dpcon = dpaa2_eth_setup_dpcon(priv);
3133	if (IS_ERR(channel->dpcon)) {
3134		err = PTR_ERR(channel->dpcon);
3135		goto err_setup;
3136	}
3137
3138	err = dpcon_get_attributes(priv->mc_io, 0, channel->dpcon->mc_handle,
3139				   &attr);
3140	if (err) {
3141		dev_err(dev, "dpcon_get_attributes() failed\n");
3142		goto err_get_attr;
3143	}
3144
3145	channel->dpcon_id = attr.id;
3146	channel->ch_id = attr.qbman_ch_id;
3147	channel->priv = priv;
3148
3149	return channel;
3150
3151err_get_attr:
3152	dpaa2_eth_free_dpcon(priv, channel->dpcon);
3153err_setup:
3154	kfree(channel);
3155	return ERR_PTR(err);
3156}
3157
3158static void dpaa2_eth_free_channel(struct dpaa2_eth_priv *priv,
3159				   struct dpaa2_eth_channel *channel)
3160{
3161	dpaa2_eth_free_dpcon(priv, channel->dpcon);
3162	kfree(channel);
3163}
3164
3165/* DPIO setup: allocate and configure QBMan channels, setup core affinity
3166 * and register data availability notifications
3167 */
3168static int dpaa2_eth_setup_dpio(struct dpaa2_eth_priv *priv)
3169{
3170	struct dpaa2_io_notification_ctx *nctx;
3171	struct dpaa2_eth_channel *channel;
3172	struct dpcon_notification_cfg dpcon_notif_cfg;
3173	struct device *dev = priv->net_dev->dev.parent;
3174	int i, err;
3175
3176	/* We want the ability to spread ingress traffic (RX, TX conf) to as
3177	 * many cores as possible, so we need one channel for each core
3178	 * (unless there's fewer queues than cores, in which case the extra
3179	 * channels would be wasted).
3180	 * Allocate one channel per core and register it to the core's
3181	 * affine DPIO. If not enough channels are available for all cores
3182	 * or if some cores don't have an affine DPIO, there will be no
3183	 * ingress frame processing on those cores.
3184	 */
3185	cpumask_clear(&priv->dpio_cpumask);
3186	for_each_online_cpu(i) {
3187		/* Try to allocate a channel */
3188		channel = dpaa2_eth_alloc_channel(priv);
3189		if (IS_ERR_OR_NULL(channel)) {
3190			err = PTR_ERR_OR_ZERO(channel);
3191			if (err == -EPROBE_DEFER)
3192				dev_dbg(dev, "waiting for affine channel\n");
3193			else
3194				dev_info(dev,
3195					 "No affine channel for cpu %d and above\n", i);
3196			goto err_alloc_ch;
3197		}
3198
3199		priv->channel[priv->num_channels] = channel;
3200
3201		nctx = &channel->nctx;
3202		nctx->is_cdan = 1;
3203		nctx->cb = dpaa2_eth_cdan_cb;
3204		nctx->id = channel->ch_id;
3205		nctx->desired_cpu = i;
3206
3207		/* Register the new context */
3208		channel->dpio = dpaa2_io_service_select(i);
3209		err = dpaa2_io_service_register(channel->dpio, nctx, dev);
3210		if (err) {
3211			dev_dbg(dev, "No affine DPIO for cpu %d\n", i);
3212			/* If no affine DPIO for this core, there's probably
3213			 * none available for next cores either. Signal we want
3214			 * to retry later, in case the DPIO devices weren't
3215			 * probed yet.
3216			 */
3217			err = -EPROBE_DEFER;
3218			goto err_service_reg;
3219		}
3220
3221		/* Register DPCON notification with MC */
3222		dpcon_notif_cfg.dpio_id = nctx->dpio_id;
3223		dpcon_notif_cfg.priority = 0;
3224		dpcon_notif_cfg.user_ctx = nctx->qman64;
3225		err = dpcon_set_notification(priv->mc_io, 0,
3226					     channel->dpcon->mc_handle,
3227					     &dpcon_notif_cfg);
3228		if (err) {
3229			dev_err(dev, "dpcon_set_notification failed()\n");
3230			goto err_set_cdan;
3231		}
3232
3233		/* If we managed to allocate a channel and also found an affine
3234		 * DPIO for this core, add it to the final mask
3235		 */
3236		cpumask_set_cpu(i, &priv->dpio_cpumask);
3237		priv->num_channels++;
3238
3239		/* Stop if we already have enough channels to accommodate all
3240		 * RX and TX conf queues
3241		 */
3242		if (priv->num_channels == priv->dpni_attrs.num_queues)
3243			break;
3244	}
3245
3246	return 0;
3247
3248err_set_cdan:
3249	dpaa2_io_service_deregister(channel->dpio, nctx, dev);
3250err_service_reg:
3251	dpaa2_eth_free_channel(priv, channel);
3252err_alloc_ch:
3253	if (err == -EPROBE_DEFER) {
3254		for (i = 0; i < priv->num_channels; i++) {
3255			channel = priv->channel[i];
3256			nctx = &channel->nctx;
3257			dpaa2_io_service_deregister(channel->dpio, nctx, dev);
3258			dpaa2_eth_free_channel(priv, channel);
3259		}
3260		priv->num_channels = 0;
3261		return err;
3262	}
3263
3264	if (cpumask_empty(&priv->dpio_cpumask)) {
3265		dev_err(dev, "No cpu with an affine DPIO/DPCON\n");
3266		return -ENODEV;
3267	}
3268
3269	dev_info(dev, "Cores %*pbl available for processing ingress traffic\n",
3270		 cpumask_pr_args(&priv->dpio_cpumask));
3271
3272	return 0;
3273}
3274
3275static void dpaa2_eth_free_dpio(struct dpaa2_eth_priv *priv)
3276{
3277	struct device *dev = priv->net_dev->dev.parent;
3278	struct dpaa2_eth_channel *ch;
3279	int i;
3280
3281	/* deregister CDAN notifications and free channels */
3282	for (i = 0; i < priv->num_channels; i++) {
3283		ch = priv->channel[i];
3284		dpaa2_io_service_deregister(ch->dpio, &ch->nctx, dev);
3285		dpaa2_eth_free_channel(priv, ch);
3286	}
3287}
3288
3289static struct dpaa2_eth_channel *dpaa2_eth_get_affine_channel(struct dpaa2_eth_priv *priv,
3290							      int cpu)
3291{
3292	struct device *dev = priv->net_dev->dev.parent;
3293	int i;
3294
3295	for (i = 0; i < priv->num_channels; i++)
3296		if (priv->channel[i]->nctx.desired_cpu == cpu)
3297			return priv->channel[i];
3298
3299	/* We should never get here. Issue a warning and return
3300	 * the first channel, because it's still better than nothing
3301	 */
3302	dev_warn(dev, "No affine channel found for cpu %d\n", cpu);
3303
3304	return priv->channel[0];
3305}
3306
3307static void dpaa2_eth_set_fq_affinity(struct dpaa2_eth_priv *priv)
3308{
3309	struct device *dev = priv->net_dev->dev.parent;
3310	struct dpaa2_eth_fq *fq;
3311	int rx_cpu, txc_cpu;
3312	int i;
3313
3314	/* For each FQ, pick one channel/CPU to deliver frames to.
3315	 * This may well change at runtime, either through irqbalance or
3316	 * through direct user intervention.
3317	 */
3318	rx_cpu = txc_cpu = cpumask_first(&priv->dpio_cpumask);
3319
3320	for (i = 0; i < priv->num_fqs; i++) {
3321		fq = &priv->fq[i];
3322		switch (fq->type) {
3323		case DPAA2_RX_FQ:
3324		case DPAA2_RX_ERR_FQ:
3325			fq->target_cpu = rx_cpu;
3326			rx_cpu = cpumask_next(rx_cpu, &priv->dpio_cpumask);
3327			if (rx_cpu >= nr_cpu_ids)
3328				rx_cpu = cpumask_first(&priv->dpio_cpumask);
3329			break;
3330		case DPAA2_TX_CONF_FQ:
3331			fq->target_cpu = txc_cpu;
3332			txc_cpu = cpumask_next(txc_cpu, &priv->dpio_cpumask);
3333			if (txc_cpu >= nr_cpu_ids)
3334				txc_cpu = cpumask_first(&priv->dpio_cpumask);
3335			break;
3336		default:
3337			dev_err(dev, "Unknown FQ type: %d\n", fq->type);
3338		}
3339		fq->channel = dpaa2_eth_get_affine_channel(priv, fq->target_cpu);
3340	}
3341
3342	update_xps(priv);
3343}
3344
3345static void dpaa2_eth_setup_fqs(struct dpaa2_eth_priv *priv)
3346{
3347	int i, j;
3348
3349	/* We have one TxConf FQ per Tx flow.
3350	 * The number of Tx and Rx queues is the same.
3351	 * Tx queues come first in the fq array.
3352	 */
3353	for (i = 0; i < dpaa2_eth_queue_count(priv); i++) {
3354		priv->fq[priv->num_fqs].type = DPAA2_TX_CONF_FQ;
3355		priv->fq[priv->num_fqs].consume = dpaa2_eth_tx_conf;
3356		priv->fq[priv->num_fqs++].flowid = (u16)i;
3357	}
3358
3359	for (j = 0; j < dpaa2_eth_tc_count(priv); j++) {
3360		for (i = 0; i < dpaa2_eth_queue_count(priv); i++) {
3361			priv->fq[priv->num_fqs].type = DPAA2_RX_FQ;
3362			priv->fq[priv->num_fqs].consume = dpaa2_eth_rx;
3363			priv->fq[priv->num_fqs].tc = (u8)j;
3364			priv->fq[priv->num_fqs++].flowid = (u16)i;
3365		}
3366	}
3367
3368	/* We have exactly one Rx error queue per DPNI */
3369	priv->fq[priv->num_fqs].type = DPAA2_RX_ERR_FQ;
3370	priv->fq[priv->num_fqs++].consume = dpaa2_eth_rx_err;
3371
3372	/* For each FQ, decide on which core to process incoming frames */
3373	dpaa2_eth_set_fq_affinity(priv);
3374}
3375
3376/* Allocate and configure a buffer pool */
3377struct dpaa2_eth_bp *dpaa2_eth_allocate_dpbp(struct dpaa2_eth_priv *priv)
3378{
3379	struct device *dev = priv->net_dev->dev.parent;
3380	struct fsl_mc_device *dpbp_dev;
3381	struct dpbp_attr dpbp_attrs;
3382	struct dpaa2_eth_bp *bp;
3383	int err;
3384
3385	err = fsl_mc_object_allocate(to_fsl_mc_device(dev), FSL_MC_POOL_DPBP,
3386				     &dpbp_dev);
3387	if (err) {
3388		if (err == -ENXIO)
3389			err = -EPROBE_DEFER;
3390		else
3391			dev_err(dev, "DPBP device allocation failed\n");
3392		return ERR_PTR(err);
3393	}
3394
3395	bp = kzalloc(sizeof(*bp), GFP_KERNEL);
3396	if (!bp) {
3397		err = -ENOMEM;
3398		goto err_alloc;
3399	}
3400
3401	err = dpbp_open(priv->mc_io, 0, dpbp_dev->obj_desc.id,
3402			&dpbp_dev->mc_handle);
3403	if (err) {
3404		dev_err(dev, "dpbp_open() failed\n");
3405		goto err_open;
3406	}
3407
3408	err = dpbp_reset(priv->mc_io, 0, dpbp_dev->mc_handle);
3409	if (err) {
3410		dev_err(dev, "dpbp_reset() failed\n");
3411		goto err_reset;
3412	}
3413
3414	err = dpbp_enable(priv->mc_io, 0, dpbp_dev->mc_handle);
3415	if (err) {
3416		dev_err(dev, "dpbp_enable() failed\n");
3417		goto err_enable;
3418	}
3419
3420	err = dpbp_get_attributes(priv->mc_io, 0, dpbp_dev->mc_handle,
3421				  &dpbp_attrs);
3422	if (err) {
3423		dev_err(dev, "dpbp_get_attributes() failed\n");
3424		goto err_get_attr;
3425	}
3426
3427	bp->dev = dpbp_dev;
3428	bp->bpid = dpbp_attrs.bpid;
3429
3430	return bp;
3431
3432err_get_attr:
3433	dpbp_disable(priv->mc_io, 0, dpbp_dev->mc_handle);
3434err_enable:
3435err_reset:
3436	dpbp_close(priv->mc_io, 0, dpbp_dev->mc_handle);
3437err_open:
3438	kfree(bp);
3439err_alloc:
3440	fsl_mc_object_free(dpbp_dev);
3441
3442	return ERR_PTR(err);
3443}
3444
3445static int dpaa2_eth_setup_default_dpbp(struct dpaa2_eth_priv *priv)
3446{
3447	struct dpaa2_eth_bp *bp;
3448	int i;
3449
3450	bp = dpaa2_eth_allocate_dpbp(priv);
3451	if (IS_ERR(bp))
3452		return PTR_ERR(bp);
3453
3454	priv->bp[DPAA2_ETH_DEFAULT_BP_IDX] = bp;
3455	priv->num_bps++;
3456
3457	for (i = 0; i < priv->num_channels; i++)
3458		priv->channel[i]->bp = bp;
3459
3460	return 0;
3461}
3462
3463void dpaa2_eth_free_dpbp(struct dpaa2_eth_priv *priv, struct dpaa2_eth_bp *bp)
3464{
3465	int idx_bp;
3466
3467	/* Find the index at which this BP is stored */
3468	for (idx_bp = 0; idx_bp < priv->num_bps; idx_bp++)
3469		if (priv->bp[idx_bp] == bp)
3470			break;
3471
3472	/* Drain the pool and disable the associated MC object */
3473	dpaa2_eth_drain_pool(priv, bp->bpid);
3474	dpbp_disable(priv->mc_io, 0, bp->dev->mc_handle);
3475	dpbp_close(priv->mc_io, 0, bp->dev->mc_handle);
3476	fsl_mc_object_free(bp->dev);
3477	kfree(bp);
3478
3479	/* Move the last in use DPBP over in this position */
3480	priv->bp[idx_bp] = priv->bp[priv->num_bps - 1];
3481	priv->num_bps--;
3482}
3483
3484static void dpaa2_eth_free_dpbps(struct dpaa2_eth_priv *priv)
3485{
3486	int i;
3487
3488	for (i = 0; i < priv->num_bps; i++)
3489		dpaa2_eth_free_dpbp(priv, priv->bp[i]);
3490}
3491
3492static int dpaa2_eth_set_buffer_layout(struct dpaa2_eth_priv *priv)
3493{
3494	struct device *dev = priv->net_dev->dev.parent;
3495	struct dpni_buffer_layout buf_layout = {0};
3496	u16 rx_buf_align;
3497	int err;
3498
3499	/* We need to check for WRIOP version 1.0.0, but depending on the MC
3500	 * version, this number is not always provided correctly on rev1.
3501	 * We need to check for both alternatives in this situation.
3502	 */
3503	if (priv->dpni_attrs.wriop_version == DPAA2_WRIOP_VERSION(0, 0, 0) ||
3504	    priv->dpni_attrs.wriop_version == DPAA2_WRIOP_VERSION(1, 0, 0))
3505		rx_buf_align = DPAA2_ETH_RX_BUF_ALIGN_REV1;
3506	else
3507		rx_buf_align = DPAA2_ETH_RX_BUF_ALIGN;
3508
3509	/* We need to ensure that the buffer size seen by WRIOP is a multiple
3510	 * of 64 or 256 bytes depending on the WRIOP version.
3511	 */
3512	priv->rx_buf_size = ALIGN_DOWN(DPAA2_ETH_RX_BUF_SIZE, rx_buf_align);
3513
3514	/* tx buffer */
3515	buf_layout.private_data_size = DPAA2_ETH_SWA_SIZE;
3516	buf_layout.pass_timestamp = true;
3517	buf_layout.pass_frame_status = true;
3518	buf_layout.options = DPNI_BUF_LAYOUT_OPT_PRIVATE_DATA_SIZE |
3519			     DPNI_BUF_LAYOUT_OPT_TIMESTAMP |
3520			     DPNI_BUF_LAYOUT_OPT_FRAME_STATUS;
3521	err = dpni_set_buffer_layout(priv->mc_io, 0, priv->mc_token,
3522				     DPNI_QUEUE_TX, &buf_layout);
3523	if (err) {
3524		dev_err(dev, "dpni_set_buffer_layout(TX) failed\n");
3525		return err;
3526	}
3527
3528	/* tx-confirm buffer */
3529	buf_layout.options = DPNI_BUF_LAYOUT_OPT_TIMESTAMP |
3530			     DPNI_BUF_LAYOUT_OPT_FRAME_STATUS;
3531	err = dpni_set_buffer_layout(priv->mc_io, 0, priv->mc_token,
3532				     DPNI_QUEUE_TX_CONFIRM, &buf_layout);
3533	if (err) {
3534		dev_err(dev, "dpni_set_buffer_layout(TX_CONF) failed\n");
3535		return err;
3536	}
3537
3538	/* Now that we've set our tx buffer layout, retrieve the minimum
3539	 * required tx data offset.
3540	 */
3541	err = dpni_get_tx_data_offset(priv->mc_io, 0, priv->mc_token,
3542				      &priv->tx_data_offset);
3543	if (err) {
3544		dev_err(dev, "dpni_get_tx_data_offset() failed\n");
3545		return err;
3546	}
3547
3548	if ((priv->tx_data_offset % 64) != 0)
3549		dev_warn(dev, "Tx data offset (%d) not a multiple of 64B\n",
3550			 priv->tx_data_offset);
3551
3552	/* rx buffer */
3553	buf_layout.pass_frame_status = true;
3554	buf_layout.pass_parser_result = true;
3555	buf_layout.data_align = rx_buf_align;
3556	buf_layout.data_head_room = dpaa2_eth_rx_head_room(priv);
3557	buf_layout.private_data_size = 0;
3558	buf_layout.options = DPNI_BUF_LAYOUT_OPT_PARSER_RESULT |
3559			     DPNI_BUF_LAYOUT_OPT_FRAME_STATUS |
3560			     DPNI_BUF_LAYOUT_OPT_DATA_ALIGN |
3561			     DPNI_BUF_LAYOUT_OPT_DATA_HEAD_ROOM |
3562			     DPNI_BUF_LAYOUT_OPT_TIMESTAMP;
3563	err = dpni_set_buffer_layout(priv->mc_io, 0, priv->mc_token,
3564				     DPNI_QUEUE_RX, &buf_layout);
3565	if (err) {
3566		dev_err(dev, "dpni_set_buffer_layout(RX) failed\n");
3567		return err;
3568	}
3569
3570	return 0;
3571}
3572
3573#define DPNI_ENQUEUE_FQID_VER_MAJOR	7
3574#define DPNI_ENQUEUE_FQID_VER_MINOR	9
3575
3576static inline int dpaa2_eth_enqueue_qd(struct dpaa2_eth_priv *priv,
3577				       struct dpaa2_eth_fq *fq,
3578				       struct dpaa2_fd *fd, u8 prio,
3579				       u32 num_frames __always_unused,
3580				       int *frames_enqueued)
3581{
3582	int err;
3583
3584	err = dpaa2_io_service_enqueue_qd(fq->channel->dpio,
3585					  priv->tx_qdid, prio,
3586					  fq->tx_qdbin, fd);
3587	if (!err && frames_enqueued)
3588		*frames_enqueued = 1;
3589	return err;
3590}
3591
3592static inline int dpaa2_eth_enqueue_fq_multiple(struct dpaa2_eth_priv *priv,
3593						struct dpaa2_eth_fq *fq,
3594						struct dpaa2_fd *fd,
3595						u8 prio, u32 num_frames,
3596						int *frames_enqueued)
3597{
3598	int err;
3599
3600	err = dpaa2_io_service_enqueue_multiple_fq(fq->channel->dpio,
3601						   fq->tx_fqid[prio],
3602						   fd, num_frames);
3603
3604	if (err == 0)
3605		return -EBUSY;
3606
3607	if (frames_enqueued)
3608		*frames_enqueued = err;
3609	return 0;
3610}
3611
3612static void dpaa2_eth_set_enqueue_mode(struct dpaa2_eth_priv *priv)
3613{
3614	if (dpaa2_eth_cmp_dpni_ver(priv, DPNI_ENQUEUE_FQID_VER_MAJOR,
3615				   DPNI_ENQUEUE_FQID_VER_MINOR) < 0)
3616		priv->enqueue = dpaa2_eth_enqueue_qd;
3617	else
3618		priv->enqueue = dpaa2_eth_enqueue_fq_multiple;
3619}
3620
3621static int dpaa2_eth_set_pause(struct dpaa2_eth_priv *priv)
3622{
3623	struct device *dev = priv->net_dev->dev.parent;
3624	struct dpni_link_cfg link_cfg = {0};
3625	int err;
3626
3627	/* Get the default link options so we don't override other flags */
3628	err = dpni_get_link_cfg(priv->mc_io, 0, priv->mc_token, &link_cfg);
3629	if (err) {
3630		dev_err(dev, "dpni_get_link_cfg() failed\n");
3631		return err;
3632	}
3633
3634	/* By default, enable both Rx and Tx pause frames */
3635	link_cfg.options |= DPNI_LINK_OPT_PAUSE;
3636	link_cfg.options &= ~DPNI_LINK_OPT_ASYM_PAUSE;
3637	err = dpni_set_link_cfg(priv->mc_io, 0, priv->mc_token, &link_cfg);
3638	if (err) {
3639		dev_err(dev, "dpni_set_link_cfg() failed\n");
3640		return err;
3641	}
3642
3643	priv->link_state.options = link_cfg.options;
3644
3645	return 0;
3646}
3647
3648static void dpaa2_eth_update_tx_fqids(struct dpaa2_eth_priv *priv)
3649{
3650	struct dpni_queue_id qid = {0};
3651	struct dpaa2_eth_fq *fq;
3652	struct dpni_queue queue;
3653	int i, j, err;
3654
3655	/* We only use Tx FQIDs for FQID-based enqueue, so check
3656	 * if DPNI version supports it before updating FQIDs
3657	 */
3658	if (dpaa2_eth_cmp_dpni_ver(priv, DPNI_ENQUEUE_FQID_VER_MAJOR,
3659				   DPNI_ENQUEUE_FQID_VER_MINOR) < 0)
3660		return;
3661
3662	for (i = 0; i < priv->num_fqs; i++) {
3663		fq = &priv->fq[i];
3664		if (fq->type != DPAA2_TX_CONF_FQ)
3665			continue;
3666		for (j = 0; j < dpaa2_eth_tc_count(priv); j++) {
3667			err = dpni_get_queue(priv->mc_io, 0, priv->mc_token,
3668					     DPNI_QUEUE_TX, j, fq->flowid,
3669					     &queue, &qid);
3670			if (err)
3671				goto out_err;
3672
3673			fq->tx_fqid[j] = qid.fqid;
3674			if (fq->tx_fqid[j] == 0)
3675				goto out_err;
3676		}
3677	}
3678
3679	priv->enqueue = dpaa2_eth_enqueue_fq_multiple;
3680
3681	return;
3682
3683out_err:
3684	netdev_info(priv->net_dev,
3685		    "Error reading Tx FQID, fallback to QDID-based enqueue\n");
3686	priv->enqueue = dpaa2_eth_enqueue_qd;
3687}
3688
3689/* Configure ingress classification based on VLAN PCP */
3690static int dpaa2_eth_set_vlan_qos(struct dpaa2_eth_priv *priv)
3691{
3692	struct device *dev = priv->net_dev->dev.parent;
3693	struct dpkg_profile_cfg kg_cfg = {0};
3694	struct dpni_qos_tbl_cfg qos_cfg = {0};
3695	struct dpni_rule_cfg key_params;
3696	void *dma_mem, *key, *mask;
3697	u8 key_size = 2;	/* VLAN TCI field */
3698	int i, pcp, err;
3699
3700	/* VLAN-based classification only makes sense if we have multiple
3701	 * traffic classes.
3702	 * Also, we need to extract just the 3-bit PCP field from the VLAN
3703	 * header and we can only do that by using a mask
3704	 */
3705	if (dpaa2_eth_tc_count(priv) == 1 || !dpaa2_eth_fs_mask_enabled(priv)) {
3706		dev_dbg(dev, "VLAN-based QoS classification not supported\n");
3707		return -EOPNOTSUPP;
3708	}
3709
3710	dma_mem = kzalloc(DPAA2_CLASSIFIER_DMA_SIZE, GFP_KERNEL);
3711	if (!dma_mem)
3712		return -ENOMEM;
3713
3714	kg_cfg.num_extracts = 1;
3715	kg_cfg.extracts[0].type = DPKG_EXTRACT_FROM_HDR;
3716	kg_cfg.extracts[0].extract.from_hdr.prot = NET_PROT_VLAN;
3717	kg_cfg.extracts[0].extract.from_hdr.type = DPKG_FULL_FIELD;
3718	kg_cfg.extracts[0].extract.from_hdr.field = NH_FLD_VLAN_TCI;
3719
3720	err = dpni_prepare_key_cfg(&kg_cfg, dma_mem);
3721	if (err) {
3722		dev_err(dev, "dpni_prepare_key_cfg failed\n");
3723		goto out_free_tbl;
3724	}
3725
3726	/* set QoS table */
3727	qos_cfg.default_tc = 0;
3728	qos_cfg.discard_on_miss = 0;
3729	qos_cfg.key_cfg_iova = dma_map_single(dev, dma_mem,
3730					      DPAA2_CLASSIFIER_DMA_SIZE,
3731					      DMA_TO_DEVICE);
3732	if (dma_mapping_error(dev, qos_cfg.key_cfg_iova)) {
3733		dev_err(dev, "QoS table DMA mapping failed\n");
3734		err = -ENOMEM;
3735		goto out_free_tbl;
3736	}
3737
3738	err = dpni_set_qos_table(priv->mc_io, 0, priv->mc_token, &qos_cfg);
3739	if (err) {
3740		dev_err(dev, "dpni_set_qos_table failed\n");
3741		goto out_unmap_tbl;
3742	}
3743
3744	/* Add QoS table entries */
3745	key = kzalloc(key_size * 2, GFP_KERNEL);
3746	if (!key) {
3747		err = -ENOMEM;
3748		goto out_unmap_tbl;
3749	}
3750	mask = key + key_size;
3751	*(__be16 *)mask = cpu_to_be16(VLAN_PRIO_MASK);
3752
3753	key_params.key_iova = dma_map_single(dev, key, key_size * 2,
3754					     DMA_TO_DEVICE);
3755	if (dma_mapping_error(dev, key_params.key_iova)) {
3756		dev_err(dev, "Qos table entry DMA mapping failed\n");
3757		err = -ENOMEM;
3758		goto out_free_key;
3759	}
3760
3761	key_params.mask_iova = key_params.key_iova + key_size;
3762	key_params.key_size = key_size;
3763
3764	/* We add rules for PCP-based distribution starting with highest
3765	 * priority (VLAN PCP = 7). If this DPNI doesn't have enough traffic
3766	 * classes to accommodate all priority levels, the lowest ones end up
3767	 * on TC 0 which was configured as default
3768	 */
3769	for (i = dpaa2_eth_tc_count(priv) - 1, pcp = 7; i >= 0; i--, pcp--) {
3770		*(__be16 *)key = cpu_to_be16(pcp << VLAN_PRIO_SHIFT);
3771		dma_sync_single_for_device(dev, key_params.key_iova,
3772					   key_size * 2, DMA_TO_DEVICE);
3773
3774		err = dpni_add_qos_entry(priv->mc_io, 0, priv->mc_token,
3775					 &key_params, i, i);
3776		if (err) {
3777			dev_err(dev, "dpni_add_qos_entry failed\n");
3778			dpni_clear_qos_table(priv->mc_io, 0, priv->mc_token);
3779			goto out_unmap_key;
3780		}
3781	}
3782
3783	priv->vlan_cls_enabled = true;
3784
3785	/* Table and key memory is not persistent, clean everything up after
3786	 * configuration is finished
3787	 */
3788out_unmap_key:
3789	dma_unmap_single(dev, key_params.key_iova, key_size * 2, DMA_TO_DEVICE);
3790out_free_key:
3791	kfree(key);
3792out_unmap_tbl:
3793	dma_unmap_single(dev, qos_cfg.key_cfg_iova, DPAA2_CLASSIFIER_DMA_SIZE,
3794			 DMA_TO_DEVICE);
3795out_free_tbl:
3796	kfree(dma_mem);
3797
3798	return err;
3799}
3800
3801/* Configure the DPNI object this interface is associated with */
3802static int dpaa2_eth_setup_dpni(struct fsl_mc_device *ls_dev)
3803{
3804	struct device *dev = &ls_dev->dev;
3805	struct dpaa2_eth_priv *priv;
3806	struct net_device *net_dev;
3807	int err;
3808
3809	net_dev = dev_get_drvdata(dev);
3810	priv = netdev_priv(net_dev);
3811
3812	/* get a handle for the DPNI object */
3813	err = dpni_open(priv->mc_io, 0, ls_dev->obj_desc.id, &priv->mc_token);
3814	if (err) {
3815		dev_err(dev, "dpni_open() failed\n");
3816		return err;
3817	}
3818
3819	/* Check if we can work with this DPNI object */
3820	err = dpni_get_api_version(priv->mc_io, 0, &priv->dpni_ver_major,
3821				   &priv->dpni_ver_minor);
3822	if (err) {
3823		dev_err(dev, "dpni_get_api_version() failed\n");
3824		goto close;
3825	}
3826	if (dpaa2_eth_cmp_dpni_ver(priv, DPNI_VER_MAJOR, DPNI_VER_MINOR) < 0) {
3827		dev_err(dev, "DPNI version %u.%u not supported, need >= %u.%u\n",
3828			priv->dpni_ver_major, priv->dpni_ver_minor,
3829			DPNI_VER_MAJOR, DPNI_VER_MINOR);
3830		err = -EOPNOTSUPP;
3831		goto close;
3832	}
3833
3834	ls_dev->mc_io = priv->mc_io;
3835	ls_dev->mc_handle = priv->mc_token;
3836
3837	err = dpni_reset(priv->mc_io, 0, priv->mc_token);
3838	if (err) {
3839		dev_err(dev, "dpni_reset() failed\n");
3840		goto close;
3841	}
3842
3843	err = dpni_get_attributes(priv->mc_io, 0, priv->mc_token,
3844				  &priv->dpni_attrs);
3845	if (err) {
3846		dev_err(dev, "dpni_get_attributes() failed (err=%d)\n", err);
3847		goto close;
3848	}
3849
3850	err = dpaa2_eth_set_buffer_layout(priv);
3851	if (err)
3852		goto close;
3853
3854	dpaa2_eth_set_enqueue_mode(priv);
3855
3856	/* Enable pause frame support */
3857	if (dpaa2_eth_has_pause_support(priv)) {
3858		err = dpaa2_eth_set_pause(priv);
3859		if (err)
3860			goto close;
3861	}
3862
3863	err = dpaa2_eth_set_vlan_qos(priv);
3864	if (err && err != -EOPNOTSUPP)
3865		goto close;
3866
3867	priv->cls_rules = devm_kcalloc(dev, dpaa2_eth_fs_count(priv),
3868				       sizeof(struct dpaa2_eth_cls_rule),
3869				       GFP_KERNEL);
3870	if (!priv->cls_rules) {
3871		err = -ENOMEM;
3872		goto close;
3873	}
3874
3875	return 0;
3876
3877close:
3878	dpni_close(priv->mc_io, 0, priv->mc_token);
3879
3880	return err;
3881}
3882
3883static void dpaa2_eth_free_dpni(struct dpaa2_eth_priv *priv)
3884{
3885	int err;
3886
3887	err = dpni_reset(priv->mc_io, 0, priv->mc_token);
3888	if (err)
3889		netdev_warn(priv->net_dev, "dpni_reset() failed (err %d)\n",
3890			    err);
3891
3892	dpni_close(priv->mc_io, 0, priv->mc_token);
3893}
3894
3895static int dpaa2_eth_setup_rx_flow(struct dpaa2_eth_priv *priv,
3896				   struct dpaa2_eth_fq *fq)
3897{
3898	struct device *dev = priv->net_dev->dev.parent;
3899	struct dpni_queue queue;
3900	struct dpni_queue_id qid;
3901	int err;
3902
3903	err = dpni_get_queue(priv->mc_io, 0, priv->mc_token,
3904			     DPNI_QUEUE_RX, fq->tc, fq->flowid, &queue, &qid);
3905	if (err) {
3906		dev_err(dev, "dpni_get_queue(RX) failed\n");
3907		return err;
3908	}
3909
3910	fq->fqid = qid.fqid;
3911
3912	queue.destination.id = fq->channel->dpcon_id;
3913	queue.destination.type = DPNI_DEST_DPCON;
3914	queue.destination.priority = 1;
3915	queue.user_context = (u64)(uintptr_t)fq;
3916	err = dpni_set_queue(priv->mc_io, 0, priv->mc_token,
3917			     DPNI_QUEUE_RX, fq->tc, fq->flowid,
3918			     DPNI_QUEUE_OPT_USER_CTX | DPNI_QUEUE_OPT_DEST,
3919			     &queue);
3920	if (err) {
3921		dev_err(dev, "dpni_set_queue(RX) failed\n");
3922		return err;
3923	}
3924
3925	/* xdp_rxq setup */
3926	/* only once for each channel */
3927	if (fq->tc > 0)
3928		return 0;
3929
3930	err = xdp_rxq_info_reg(&fq->channel->xdp_rxq, priv->net_dev,
3931			       fq->flowid, 0);
3932	if (err) {
3933		dev_err(dev, "xdp_rxq_info_reg failed\n");
3934		return err;
3935	}
3936
3937	err = xdp_rxq_info_reg_mem_model(&fq->channel->xdp_rxq,
3938					 MEM_TYPE_PAGE_ORDER0, NULL);
3939	if (err) {
3940		dev_err(dev, "xdp_rxq_info_reg_mem_model failed\n");
3941		xdp_rxq_info_unreg(&fq->channel->xdp_rxq);
3942		return err;
3943	}
3944
3945	return 0;
3946}
3947
3948static int dpaa2_eth_setup_tx_flow(struct dpaa2_eth_priv *priv,
3949				   struct dpaa2_eth_fq *fq)
3950{
3951	struct device *dev = priv->net_dev->dev.parent;
3952	struct dpni_queue queue;
3953	struct dpni_queue_id qid;
3954	int i, err;
3955
3956	for (i = 0; i < dpaa2_eth_tc_count(priv); i++) {
3957		err = dpni_get_queue(priv->mc_io, 0, priv->mc_token,
3958				     DPNI_QUEUE_TX, i, fq->flowid,
3959				     &queue, &qid);
3960		if (err) {
3961			dev_err(dev, "dpni_get_queue(TX) failed\n");
3962			return err;
3963		}
3964		fq->tx_fqid[i] = qid.fqid;
3965	}
3966
3967	/* All Tx queues belonging to the same flowid have the same qdbin */
3968	fq->tx_qdbin = qid.qdbin;
3969
3970	err = dpni_get_queue(priv->mc_io, 0, priv->mc_token,
3971			     DPNI_QUEUE_TX_CONFIRM, 0, fq->flowid,
3972			     &queue, &qid);
3973	if (err) {
3974		dev_err(dev, "dpni_get_queue(TX_CONF) failed\n");
3975		return err;
3976	}
3977
3978	fq->fqid = qid.fqid;
3979
3980	queue.destination.id = fq->channel->dpcon_id;
3981	queue.destination.type = DPNI_DEST_DPCON;
3982	queue.destination.priority = 0;
3983	queue.user_context = (u64)(uintptr_t)fq;
3984	err = dpni_set_queue(priv->mc_io, 0, priv->mc_token,
3985			     DPNI_QUEUE_TX_CONFIRM, 0, fq->flowid,
3986			     DPNI_QUEUE_OPT_USER_CTX | DPNI_QUEUE_OPT_DEST,
3987			     &queue);
3988	if (err) {
3989		dev_err(dev, "dpni_set_queue(TX_CONF) failed\n");
3990		return err;
3991	}
3992
3993	return 0;
3994}
3995
3996static int setup_rx_err_flow(struct dpaa2_eth_priv *priv,
3997			     struct dpaa2_eth_fq *fq)
3998{
3999	struct device *dev = priv->net_dev->dev.parent;
4000	struct dpni_queue q = { { 0 } };
4001	struct dpni_queue_id qid;
4002	u8 q_opt = DPNI_QUEUE_OPT_USER_CTX | DPNI_QUEUE_OPT_DEST;
4003	int err;
4004
4005	err = dpni_get_queue(priv->mc_io, 0, priv->mc_token,
4006			     DPNI_QUEUE_RX_ERR, 0, 0, &q, &qid);
4007	if (err) {
4008		dev_err(dev, "dpni_get_queue() failed (%d)\n", err);
4009		return err;
4010	}
4011
4012	fq->fqid = qid.fqid;
4013
4014	q.destination.id = fq->channel->dpcon_id;
4015	q.destination.type = DPNI_DEST_DPCON;
4016	q.destination.priority = 1;
4017	q.user_context = (u64)(uintptr_t)fq;
4018	err = dpni_set_queue(priv->mc_io, 0, priv->mc_token,
4019			     DPNI_QUEUE_RX_ERR, 0, 0, q_opt, &q);
4020	if (err) {
4021		dev_err(dev, "dpni_set_queue() failed (%d)\n", err);
4022		return err;
4023	}
4024
4025	return 0;
4026}
4027
4028/* Supported header fields for Rx hash distribution key */
4029static const struct dpaa2_eth_dist_fields dist_fields[] = {
4030	{
4031		/* L2 header */
4032		.rxnfc_field = RXH_L2DA,
4033		.cls_prot = NET_PROT_ETH,
4034		.cls_field = NH_FLD_ETH_DA,
4035		.id = DPAA2_ETH_DIST_ETHDST,
4036		.size = 6,
4037	}, {
4038		.cls_prot = NET_PROT_ETH,
4039		.cls_field = NH_FLD_ETH_SA,
4040		.id = DPAA2_ETH_DIST_ETHSRC,
4041		.size = 6,
4042	}, {
4043		/* This is the last ethertype field parsed:
4044		 * depending on frame format, it can be the MAC ethertype
4045		 * or the VLAN etype.
4046		 */
4047		.cls_prot = NET_PROT_ETH,
4048		.cls_field = NH_FLD_ETH_TYPE,
4049		.id = DPAA2_ETH_DIST_ETHTYPE,
4050		.size = 2,
4051	}, {
4052		/* VLAN header */
4053		.rxnfc_field = RXH_VLAN,
4054		.cls_prot = NET_PROT_VLAN,
4055		.cls_field = NH_FLD_VLAN_TCI,
4056		.id = DPAA2_ETH_DIST_VLAN,
4057		.size = 2,
4058	}, {
4059		/* IP header */
4060		.rxnfc_field = RXH_IP_SRC,
4061		.cls_prot = NET_PROT_IP,
4062		.cls_field = NH_FLD_IP_SRC,
4063		.id = DPAA2_ETH_DIST_IPSRC,
4064		.size = 4,
4065	}, {
4066		.rxnfc_field = RXH_IP_DST,
4067		.cls_prot = NET_PROT_IP,
4068		.cls_field = NH_FLD_IP_DST,
4069		.id = DPAA2_ETH_DIST_IPDST,
4070		.size = 4,
4071	}, {
4072		.rxnfc_field = RXH_L3_PROTO,
4073		.cls_prot = NET_PROT_IP,
4074		.cls_field = NH_FLD_IP_PROTO,
4075		.id = DPAA2_ETH_DIST_IPPROTO,
4076		.size = 1,
4077	}, {
4078		/* Using UDP ports, this is functionally equivalent to raw
4079		 * byte pairs from L4 header.
4080		 */
4081		.rxnfc_field = RXH_L4_B_0_1,
4082		.cls_prot = NET_PROT_UDP,
4083		.cls_field = NH_FLD_UDP_PORT_SRC,
4084		.id = DPAA2_ETH_DIST_L4SRC,
4085		.size = 2,
4086	}, {
4087		.rxnfc_field = RXH_L4_B_2_3,
4088		.cls_prot = NET_PROT_UDP,
4089		.cls_field = NH_FLD_UDP_PORT_DST,
4090		.id = DPAA2_ETH_DIST_L4DST,
4091		.size = 2,
4092	},
4093};
4094
4095/* Configure the Rx hash key using the legacy API */
4096static int dpaa2_eth_config_legacy_hash_key(struct dpaa2_eth_priv *priv, dma_addr_t key)
4097{
4098	struct device *dev = priv->net_dev->dev.parent;
4099	struct dpni_rx_tc_dist_cfg dist_cfg;
4100	int i, err = 0;
4101
4102	memset(&dist_cfg, 0, sizeof(dist_cfg));
4103
4104	dist_cfg.key_cfg_iova = key;
4105	dist_cfg.dist_size = dpaa2_eth_queue_count(priv);
4106	dist_cfg.dist_mode = DPNI_DIST_MODE_HASH;
4107
4108	for (i = 0; i < dpaa2_eth_tc_count(priv); i++) {
4109		err = dpni_set_rx_tc_dist(priv->mc_io, 0, priv->mc_token,
4110					  i, &dist_cfg);
4111		if (err) {
4112			dev_err(dev, "dpni_set_rx_tc_dist failed\n");
4113			break;
4114		}
4115	}
4116
4117	return err;
4118}
4119
4120/* Configure the Rx hash key using the new API */
4121static int dpaa2_eth_config_hash_key(struct dpaa2_eth_priv *priv, dma_addr_t key)
4122{
4123	struct device *dev = priv->net_dev->dev.parent;
4124	struct dpni_rx_dist_cfg dist_cfg;
4125	int i, err = 0;
4126
4127	memset(&dist_cfg, 0, sizeof(dist_cfg));
4128
4129	dist_cfg.key_cfg_iova = key;
4130	dist_cfg.dist_size = dpaa2_eth_queue_count(priv);
4131	dist_cfg.enable = 1;
4132
4133	for (i = 0; i < dpaa2_eth_tc_count(priv); i++) {
4134		dist_cfg.tc = i;
4135		err = dpni_set_rx_hash_dist(priv->mc_io, 0, priv->mc_token,
4136					    &dist_cfg);
4137		if (err) {
4138			dev_err(dev, "dpni_set_rx_hash_dist failed\n");
4139			break;
4140		}
4141
4142		/* If the flow steering / hashing key is shared between all
4143		 * traffic classes, install it just once
4144		 */
4145		if (priv->dpni_attrs.options & DPNI_OPT_SHARED_FS)
4146			break;
4147	}
4148
4149	return err;
4150}
4151
4152/* Configure the Rx flow classification key */
4153static int dpaa2_eth_config_cls_key(struct dpaa2_eth_priv *priv, dma_addr_t key)
4154{
4155	struct device *dev = priv->net_dev->dev.parent;
4156	struct dpni_rx_dist_cfg dist_cfg;
4157	int i, err = 0;
4158
4159	memset(&dist_cfg, 0, sizeof(dist_cfg));
4160
4161	dist_cfg.key_cfg_iova = key;
4162	dist_cfg.dist_size = dpaa2_eth_queue_count(priv);
4163	dist_cfg.enable = 1;
4164
4165	for (i = 0; i < dpaa2_eth_tc_count(priv); i++) {
4166		dist_cfg.tc = i;
4167		err = dpni_set_rx_fs_dist(priv->mc_io, 0, priv->mc_token,
4168					  &dist_cfg);
4169		if (err) {
4170			dev_err(dev, "dpni_set_rx_fs_dist failed\n");
4171			break;
4172		}
4173
4174		/* If the flow steering / hashing key is shared between all
4175		 * traffic classes, install it just once
4176		 */
4177		if (priv->dpni_attrs.options & DPNI_OPT_SHARED_FS)
4178			break;
4179	}
4180
4181	return err;
4182}
4183
4184/* Size of the Rx flow classification key */
4185int dpaa2_eth_cls_key_size(u64 fields)
4186{
4187	int i, size = 0;
4188
4189	for (i = 0; i < ARRAY_SIZE(dist_fields); i++) {
4190		if (!(fields & dist_fields[i].id))
4191			continue;
4192		size += dist_fields[i].size;
4193	}
4194
4195	return size;
4196}
4197
4198/* Offset of header field in Rx classification key */
4199int dpaa2_eth_cls_fld_off(int prot, int field)
4200{
4201	int i, off = 0;
4202
4203	for (i = 0; i < ARRAY_SIZE(dist_fields); i++) {
4204		if (dist_fields[i].cls_prot == prot &&
4205		    dist_fields[i].cls_field == field)
4206			return off;
4207		off += dist_fields[i].size;
4208	}
4209
4210	WARN_ONCE(1, "Unsupported header field used for Rx flow cls\n");
4211	return 0;
4212}
4213
4214/* Prune unused fields from the classification rule.
4215 * Used when masking is not supported
4216 */
4217void dpaa2_eth_cls_trim_rule(void *key_mem, u64 fields)
4218{
4219	int off = 0, new_off = 0;
4220	int i, size;
4221
4222	for (i = 0; i < ARRAY_SIZE(dist_fields); i++) {
4223		size = dist_fields[i].size;
4224		if (dist_fields[i].id & fields) {
4225			memcpy(key_mem + new_off, key_mem + off, size);
4226			new_off += size;
4227		}
4228		off += size;
4229	}
4230}
4231
4232/* Set Rx distribution (hash or flow classification) key
4233 * flags is a combination of RXH_ bits
4234 */
4235static int dpaa2_eth_set_dist_key(struct net_device *net_dev,
4236				  enum dpaa2_eth_rx_dist type, u64 flags)
4237{
4238	struct device *dev = net_dev->dev.parent;
4239	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
4240	struct dpkg_profile_cfg cls_cfg;
4241	u32 rx_hash_fields = 0;
4242	dma_addr_t key_iova;
4243	u8 *dma_mem;
4244	int i;
4245	int err = 0;
4246
4247	memset(&cls_cfg, 0, sizeof(cls_cfg));
4248
4249	for (i = 0; i < ARRAY_SIZE(dist_fields); i++) {
4250		struct dpkg_extract *key =
4251			&cls_cfg.extracts[cls_cfg.num_extracts];
4252
4253		/* For both Rx hashing and classification keys
4254		 * we set only the selected fields.
4255		 */
4256		if (!(flags & dist_fields[i].id))
4257			continue;
4258		if (type == DPAA2_ETH_RX_DIST_HASH)
4259			rx_hash_fields |= dist_fields[i].rxnfc_field;
4260
4261		if (cls_cfg.num_extracts >= DPKG_MAX_NUM_OF_EXTRACTS) {
4262			dev_err(dev, "error adding key extraction rule, too many rules?\n");
4263			return -E2BIG;
4264		}
4265
4266		key->type = DPKG_EXTRACT_FROM_HDR;
4267		key->extract.from_hdr.prot = dist_fields[i].cls_prot;
4268		key->extract.from_hdr.type = DPKG_FULL_FIELD;
4269		key->extract.from_hdr.field = dist_fields[i].cls_field;
4270		cls_cfg.num_extracts++;
4271	}
4272
4273	dma_mem = kzalloc(DPAA2_CLASSIFIER_DMA_SIZE, GFP_KERNEL);
4274	if (!dma_mem)
4275		return -ENOMEM;
4276
4277	err = dpni_prepare_key_cfg(&cls_cfg, dma_mem);
4278	if (err) {
4279		dev_err(dev, "dpni_prepare_key_cfg error %d\n", err);
4280		goto free_key;
4281	}
4282
4283	/* Prepare for setting the rx dist */
4284	key_iova = dma_map_single(dev, dma_mem, DPAA2_CLASSIFIER_DMA_SIZE,
4285				  DMA_TO_DEVICE);
4286	if (dma_mapping_error(dev, key_iova)) {
4287		dev_err(dev, "DMA mapping failed\n");
4288		err = -ENOMEM;
4289		goto free_key;
4290	}
4291
4292	if (type == DPAA2_ETH_RX_DIST_HASH) {
4293		if (dpaa2_eth_has_legacy_dist(priv))
4294			err = dpaa2_eth_config_legacy_hash_key(priv, key_iova);
4295		else
4296			err = dpaa2_eth_config_hash_key(priv, key_iova);
4297	} else {
4298		err = dpaa2_eth_config_cls_key(priv, key_iova);
4299	}
4300
4301	dma_unmap_single(dev, key_iova, DPAA2_CLASSIFIER_DMA_SIZE,
4302			 DMA_TO_DEVICE);
4303	if (!err && type == DPAA2_ETH_RX_DIST_HASH)
4304		priv->rx_hash_fields = rx_hash_fields;
4305
4306free_key:
4307	kfree(dma_mem);
4308	return err;
4309}
4310
4311int dpaa2_eth_set_hash(struct net_device *net_dev, u64 flags)
4312{
4313	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
4314	u64 key = 0;
4315	int i;
4316
4317	if (!dpaa2_eth_hash_enabled(priv))
4318		return -EOPNOTSUPP;
4319
4320	for (i = 0; i < ARRAY_SIZE(dist_fields); i++)
4321		if (dist_fields[i].rxnfc_field & flags)
4322			key |= dist_fields[i].id;
4323
4324	return dpaa2_eth_set_dist_key(net_dev, DPAA2_ETH_RX_DIST_HASH, key);
4325}
4326
4327int dpaa2_eth_set_cls(struct net_device *net_dev, u64 flags)
4328{
4329	return dpaa2_eth_set_dist_key(net_dev, DPAA2_ETH_RX_DIST_CLS, flags);
4330}
4331
4332static int dpaa2_eth_set_default_cls(struct dpaa2_eth_priv *priv)
4333{
4334	struct device *dev = priv->net_dev->dev.parent;
4335	int err;
4336
4337	/* Check if we actually support Rx flow classification */
4338	if (dpaa2_eth_has_legacy_dist(priv)) {
4339		dev_dbg(dev, "Rx cls not supported by current MC version\n");
4340		return -EOPNOTSUPP;
4341	}
4342
4343	if (!dpaa2_eth_fs_enabled(priv)) {
4344		dev_dbg(dev, "Rx cls disabled in DPNI options\n");
4345		return -EOPNOTSUPP;
4346	}
4347
4348	if (!dpaa2_eth_hash_enabled(priv)) {
4349		dev_dbg(dev, "Rx cls disabled for single queue DPNIs\n");
4350		return -EOPNOTSUPP;
4351	}
4352
4353	/* If there is no support for masking in the classification table,
4354	 * we don't set a default key, as it will depend on the rules
4355	 * added by the user at runtime.
4356	 */
4357	if (!dpaa2_eth_fs_mask_enabled(priv))
4358		goto out;
4359
4360	err = dpaa2_eth_set_cls(priv->net_dev, DPAA2_ETH_DIST_ALL);
4361	if (err)
4362		return err;
4363
4364out:
4365	priv->rx_cls_enabled = 1;
4366
4367	return 0;
4368}
4369
4370/* Bind the DPNI to its needed objects and resources: buffer pool, DPIOs,
4371 * frame queues and channels
4372 */
4373static int dpaa2_eth_bind_dpni(struct dpaa2_eth_priv *priv)
4374{
4375	struct dpaa2_eth_bp *bp = priv->bp[DPAA2_ETH_DEFAULT_BP_IDX];
4376	struct net_device *net_dev = priv->net_dev;
4377	struct dpni_pools_cfg pools_params = { 0 };
4378	struct device *dev = net_dev->dev.parent;
4379	struct dpni_error_cfg err_cfg;
4380	int err = 0;
4381	int i;
4382
4383	pools_params.num_dpbp = 1;
4384	pools_params.pools[0].dpbp_id = bp->dev->obj_desc.id;
4385	pools_params.pools[0].backup_pool = 0;
4386	pools_params.pools[0].buffer_size = priv->rx_buf_size;
4387	err = dpni_set_pools(priv->mc_io, 0, priv->mc_token, &pools_params);
4388	if (err) {
4389		dev_err(dev, "dpni_set_pools() failed\n");
4390		return err;
4391	}
4392
4393	/* have the interface implicitly distribute traffic based on
4394	 * the default hash key
4395	 */
4396	err = dpaa2_eth_set_hash(net_dev, DPAA2_RXH_DEFAULT);
4397	if (err && err != -EOPNOTSUPP)
4398		dev_err(dev, "Failed to configure hashing\n");
4399
4400	/* Configure the flow classification key; it includes all
4401	 * supported header fields and cannot be modified at runtime
4402	 */
4403	err = dpaa2_eth_set_default_cls(priv);
4404	if (err && err != -EOPNOTSUPP)
4405		dev_err(dev, "Failed to configure Rx classification key\n");
4406
4407	/* Configure handling of error frames */
4408	err_cfg.errors = DPAA2_FAS_RX_ERR_MASK;
4409	err_cfg.set_frame_annotation = 1;
4410	err_cfg.error_action = DPNI_ERROR_ACTION_DISCARD;
4411	err = dpni_set_errors_behavior(priv->mc_io, 0, priv->mc_token,
4412				       &err_cfg);
4413	if (err) {
4414		dev_err(dev, "dpni_set_errors_behavior failed\n");
4415		return err;
4416	}
4417
4418	/* Configure Rx and Tx conf queues to generate CDANs */
4419	for (i = 0; i < priv->num_fqs; i++) {
4420		switch (priv->fq[i].type) {
4421		case DPAA2_RX_FQ:
4422			err = dpaa2_eth_setup_rx_flow(priv, &priv->fq[i]);
4423			break;
4424		case DPAA2_TX_CONF_FQ:
4425			err = dpaa2_eth_setup_tx_flow(priv, &priv->fq[i]);
4426			break;
4427		case DPAA2_RX_ERR_FQ:
4428			err = setup_rx_err_flow(priv, &priv->fq[i]);
4429			break;
4430		default:
4431			dev_err(dev, "Invalid FQ type %d\n", priv->fq[i].type);
4432			return -EINVAL;
4433		}
4434		if (err)
4435			goto out;
4436	}
4437
4438	err = dpni_get_qdid(priv->mc_io, 0, priv->mc_token,
4439			    DPNI_QUEUE_TX, &priv->tx_qdid);
4440	if (err) {
4441		dev_err(dev, "dpni_get_qdid() failed\n");
4442		goto out;
4443	}
4444
4445	return 0;
4446
4447out:
4448	while (i--) {
4449		if (priv->fq[i].type == DPAA2_RX_FQ &&
4450		    xdp_rxq_info_is_reg(&priv->fq[i].channel->xdp_rxq))
4451			xdp_rxq_info_unreg(&priv->fq[i].channel->xdp_rxq);
4452	}
4453	return err;
4454}
4455
4456/* Allocate rings for storing incoming frame descriptors */
4457static int dpaa2_eth_alloc_rings(struct dpaa2_eth_priv *priv)
4458{
4459	struct net_device *net_dev = priv->net_dev;
4460	struct device *dev = net_dev->dev.parent;
4461	int i;
4462
4463	for (i = 0; i < priv->num_channels; i++) {
4464		priv->channel[i]->store =
4465			dpaa2_io_store_create(DPAA2_ETH_STORE_SIZE, dev);
4466		if (!priv->channel[i]->store) {
4467			netdev_err(net_dev, "dpaa2_io_store_create() failed\n");
4468			goto err_ring;
4469		}
4470	}
4471
4472	return 0;
4473
4474err_ring:
4475	for (i = 0; i < priv->num_channels; i++) {
4476		if (!priv->channel[i]->store)
4477			break;
4478		dpaa2_io_store_destroy(priv->channel[i]->store);
4479	}
4480
4481	return -ENOMEM;
4482}
4483
4484static void dpaa2_eth_free_rings(struct dpaa2_eth_priv *priv)
4485{
4486	int i;
4487
4488	for (i = 0; i < priv->num_channels; i++)
4489		dpaa2_io_store_destroy(priv->channel[i]->store);
4490}
4491
4492static int dpaa2_eth_set_mac_addr(struct dpaa2_eth_priv *priv)
4493{
4494	struct net_device *net_dev = priv->net_dev;
4495	struct device *dev = net_dev->dev.parent;
4496	u8 mac_addr[ETH_ALEN], dpni_mac_addr[ETH_ALEN];
4497	int err;
4498
4499	/* Get firmware address, if any */
4500	err = dpni_get_port_mac_addr(priv->mc_io, 0, priv->mc_token, mac_addr);
4501	if (err) {
4502		dev_err(dev, "dpni_get_port_mac_addr() failed\n");
4503		return err;
4504	}
4505
4506	/* Get DPNI attributes address, if any */
4507	err = dpni_get_primary_mac_addr(priv->mc_io, 0, priv->mc_token,
4508					dpni_mac_addr);
4509	if (err) {
4510		dev_err(dev, "dpni_get_primary_mac_addr() failed\n");
4511		return err;
4512	}
4513
4514	/* First check if firmware has any address configured by bootloader */
4515	if (!is_zero_ether_addr(mac_addr)) {
4516		/* If the DPMAC addr != DPNI addr, update it */
4517		if (!ether_addr_equal(mac_addr, dpni_mac_addr)) {
4518			err = dpni_set_primary_mac_addr(priv->mc_io, 0,
4519							priv->mc_token,
4520							mac_addr);
4521			if (err) {
4522				dev_err(dev, "dpni_set_primary_mac_addr() failed\n");
4523				return err;
4524			}
4525		}
4526		eth_hw_addr_set(net_dev, mac_addr);
4527	} else if (is_zero_ether_addr(dpni_mac_addr)) {
4528		/* No MAC address configured, fill in net_dev->dev_addr
4529		 * with a random one
4530		 */
4531		eth_hw_addr_random(net_dev);
4532		dev_dbg_once(dev, "device(s) have all-zero hwaddr, replaced with random\n");
4533
4534		err = dpni_set_primary_mac_addr(priv->mc_io, 0, priv->mc_token,
4535						net_dev->dev_addr);
4536		if (err) {
4537			dev_err(dev, "dpni_set_primary_mac_addr() failed\n");
4538			return err;
4539		}
4540
4541		/* Override NET_ADDR_RANDOM set by eth_hw_addr_random(); for all
4542		 * practical purposes, this will be our "permanent" mac address,
4543		 * at least until the next reboot. This move will also permit
4544		 * register_netdevice() to properly fill up net_dev->perm_addr.
4545		 */
4546		net_dev->addr_assign_type = NET_ADDR_PERM;
4547	} else {
4548		/* NET_ADDR_PERM is default, all we have to do is
4549		 * fill in the device addr.
4550		 */
4551		eth_hw_addr_set(net_dev, dpni_mac_addr);
4552	}
4553
4554	return 0;
4555}
4556
4557static int dpaa2_eth_netdev_init(struct net_device *net_dev)
4558{
4559	struct device *dev = net_dev->dev.parent;
4560	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
4561	u32 options = priv->dpni_attrs.options;
4562	u64 supported = 0, not_supported = 0;
4563	u8 bcast_addr[ETH_ALEN];
4564	u8 num_queues;
4565	int err;
4566
4567	net_dev->netdev_ops = &dpaa2_eth_ops;
4568	net_dev->ethtool_ops = &dpaa2_ethtool_ops;
4569
4570	err = dpaa2_eth_set_mac_addr(priv);
4571	if (err)
4572		return err;
4573
4574	/* Explicitly add the broadcast address to the MAC filtering table */
4575	eth_broadcast_addr(bcast_addr);
4576	err = dpni_add_mac_addr(priv->mc_io, 0, priv->mc_token, bcast_addr);
4577	if (err) {
4578		dev_err(dev, "dpni_add_mac_addr() failed\n");
4579		return err;
4580	}
4581
4582	/* Set MTU upper limit; lower limit is 68B (default value) */
4583	net_dev->max_mtu = DPAA2_ETH_MAX_MTU;
4584	err = dpni_set_max_frame_length(priv->mc_io, 0, priv->mc_token,
4585					DPAA2_ETH_MFL);
4586	if (err) {
4587		dev_err(dev, "dpni_set_max_frame_length() failed\n");
4588		return err;
4589	}
4590
4591	/* Set actual number of queues in the net device */
4592	num_queues = dpaa2_eth_queue_count(priv);
4593	err = netif_set_real_num_tx_queues(net_dev, num_queues);
4594	if (err) {
4595		dev_err(dev, "netif_set_real_num_tx_queues() failed\n");
4596		return err;
4597	}
4598	err = netif_set_real_num_rx_queues(net_dev, num_queues);
4599	if (err) {
4600		dev_err(dev, "netif_set_real_num_rx_queues() failed\n");
4601		return err;
4602	}
4603
4604	dpaa2_eth_detect_features(priv);
4605
4606	/* Capabilities listing */
4607	supported |= IFF_LIVE_ADDR_CHANGE;
4608
4609	if (options & DPNI_OPT_NO_MAC_FILTER)
4610		not_supported |= IFF_UNICAST_FLT;
4611	else
4612		supported |= IFF_UNICAST_FLT;
4613
4614	net_dev->priv_flags |= supported;
4615	net_dev->priv_flags &= ~not_supported;
4616	net_dev->lltx = true;
4617
4618	/* Features */
4619	net_dev->features = NETIF_F_RXCSUM |
4620			    NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
4621			    NETIF_F_SG | NETIF_F_HIGHDMA |
4622			    NETIF_F_HW_TC | NETIF_F_TSO;
4623	net_dev->gso_max_segs = DPAA2_ETH_ENQUEUE_MAX_FDS;
4624	net_dev->hw_features = net_dev->features;
4625	net_dev->xdp_features = NETDEV_XDP_ACT_BASIC |
4626				NETDEV_XDP_ACT_REDIRECT |
4627				NETDEV_XDP_ACT_NDO_XMIT;
4628	if (priv->dpni_attrs.wriop_version >= DPAA2_WRIOP_VERSION(3, 0, 0) &&
4629	    priv->dpni_attrs.num_queues <= 8)
4630		net_dev->xdp_features |= NETDEV_XDP_ACT_XSK_ZEROCOPY;
4631
4632	if (priv->dpni_attrs.vlan_filter_entries)
4633		net_dev->hw_features |= NETIF_F_HW_VLAN_CTAG_FILTER;
4634
4635	return 0;
4636}
4637
4638static int dpaa2_eth_poll_link_state(void *arg)
4639{
4640	struct dpaa2_eth_priv *priv = (struct dpaa2_eth_priv *)arg;
4641	int err;
4642
4643	while (!kthread_should_stop()) {
4644		err = dpaa2_eth_link_state_update(priv);
4645		if (unlikely(err))
4646			return err;
4647
4648		msleep(DPAA2_ETH_LINK_STATE_REFRESH);
4649	}
4650
4651	return 0;
4652}
4653
4654static int dpaa2_eth_connect_mac(struct dpaa2_eth_priv *priv)
4655{
4656	struct fsl_mc_device *dpni_dev, *dpmac_dev;
4657	struct dpaa2_mac *mac;
4658	int err;
4659
4660	dpni_dev = to_fsl_mc_device(priv->net_dev->dev.parent);
4661	dpmac_dev = fsl_mc_get_endpoint(dpni_dev, 0);
4662
4663	if (PTR_ERR(dpmac_dev) == -EPROBE_DEFER) {
4664		netdev_dbg(priv->net_dev, "waiting for mac\n");
4665		return PTR_ERR(dpmac_dev);
4666	}
4667
4668	if (IS_ERR(dpmac_dev))
4669		return 0;
4670
4671	if (dpmac_dev->dev.type != &fsl_mc_bus_dpmac_type) {
4672		err = 0;
4673		goto out_put_device;
4674	}
4675
4676	mac = kzalloc(sizeof(struct dpaa2_mac), GFP_KERNEL);
4677	if (!mac) {
4678		err = -ENOMEM;
4679		goto out_put_device;
4680	}
4681
4682	mac->mc_dev = dpmac_dev;
4683	mac->mc_io = priv->mc_io;
4684	mac->net_dev = priv->net_dev;
4685
4686	err = dpaa2_mac_open(mac);
4687	if (err)
4688		goto err_free_mac;
4689
4690	if (dpaa2_mac_is_type_phy(mac)) {
4691		err = dpaa2_mac_connect(mac);
4692		if (err) {
4693			if (err == -EPROBE_DEFER)
4694				netdev_dbg(priv->net_dev,
4695					   "could not connect to MAC\n");
4696			else
4697				netdev_err(priv->net_dev,
4698					   "Error connecting to the MAC endpoint: %pe",
4699					   ERR_PTR(err));
4700			goto err_close_mac;
4701		}
4702	}
4703
4704	mutex_lock(&priv->mac_lock);
4705	priv->mac = mac;
4706	mutex_unlock(&priv->mac_lock);
4707
4708	return 0;
4709
4710err_close_mac:
4711	dpaa2_mac_close(mac);
4712err_free_mac:
4713	kfree(mac);
4714out_put_device:
4715	put_device(&dpmac_dev->dev);
4716	return err;
4717}
4718
4719static void dpaa2_eth_disconnect_mac(struct dpaa2_eth_priv *priv)
4720{
4721	struct dpaa2_mac *mac;
4722
4723	mutex_lock(&priv->mac_lock);
4724	mac = priv->mac;
4725	priv->mac = NULL;
4726	mutex_unlock(&priv->mac_lock);
4727
4728	if (!mac)
4729		return;
4730
4731	if (dpaa2_mac_is_type_phy(mac))
4732		dpaa2_mac_disconnect(mac);
4733
4734	dpaa2_mac_close(mac);
4735	kfree(mac);
4736}
4737
4738static irqreturn_t dpni_irq0_handler_thread(int irq_num, void *arg)
4739{
4740	u32 status = ~0;
4741	struct device *dev = (struct device *)arg;
4742	struct fsl_mc_device *dpni_dev = to_fsl_mc_device(dev);
4743	struct net_device *net_dev = dev_get_drvdata(dev);
4744	struct dpaa2_eth_priv *priv = netdev_priv(net_dev);
4745	bool had_mac;
4746	int err;
4747
4748	err = dpni_get_irq_status(dpni_dev->mc_io, 0, dpni_dev->mc_handle,
4749				  DPNI_IRQ_INDEX, &status);
4750	if (unlikely(err)) {
4751		netdev_err(net_dev, "Can't get irq status (err %d)\n", err);
4752		return IRQ_HANDLED;
4753	}
4754
4755	if (status & DPNI_IRQ_EVENT_LINK_CHANGED)
4756		dpaa2_eth_link_state_update(netdev_priv(net_dev));
4757
4758	if (status & DPNI_IRQ_EVENT_ENDPOINT_CHANGED) {
4759		dpaa2_eth_set_mac_addr(netdev_priv(net_dev));
4760		dpaa2_eth_update_tx_fqids(priv);
4761
4762		/* We can avoid locking because the "endpoint changed" IRQ
4763		 * handler is the only one who changes priv->mac at runtime,
4764		 * so we are not racing with anyone.
4765		 */
4766		had_mac = !!priv->mac;
4767		if (had_mac)
4768			dpaa2_eth_disconnect_mac(priv);
4769		else
4770			dpaa2_eth_connect_mac(priv);
4771	}
4772
4773	return IRQ_HANDLED;
4774}
4775
4776static int dpaa2_eth_setup_irqs(struct fsl_mc_device *ls_dev)
4777{
4778	int err = 0;
4779	struct fsl_mc_device_irq *irq;
4780
4781	err = fsl_mc_allocate_irqs(ls_dev);
4782	if (err) {
4783		dev_err(&ls_dev->dev, "MC irqs allocation failed\n");
4784		return err;
4785	}
4786
4787	irq = ls_dev->irqs[0];
4788	err = devm_request_threaded_irq(&ls_dev->dev, irq->virq,
4789					NULL, dpni_irq0_handler_thread,
4790					IRQF_NO_SUSPEND | IRQF_ONESHOT,
4791					dev_name(&ls_dev->dev), &ls_dev->dev);
4792	if (err < 0) {
4793		dev_err(&ls_dev->dev, "devm_request_threaded_irq(): %d\n", err);
4794		goto free_mc_irq;
4795	}
4796
4797	err = dpni_set_irq_mask(ls_dev->mc_io, 0, ls_dev->mc_handle,
4798				DPNI_IRQ_INDEX, DPNI_IRQ_EVENT_LINK_CHANGED |
4799				DPNI_IRQ_EVENT_ENDPOINT_CHANGED);
4800	if (err < 0) {
4801		dev_err(&ls_dev->dev, "dpni_set_irq_mask(): %d\n", err);
4802		goto free_irq;
4803	}
4804
4805	err = dpni_set_irq_enable(ls_dev->mc_io, 0, ls_dev->mc_handle,
4806				  DPNI_IRQ_INDEX, 1);
4807	if (err < 0) {
4808		dev_err(&ls_dev->dev, "dpni_set_irq_enable(): %d\n", err);
4809		goto free_irq;
4810	}
4811
4812	return 0;
4813
4814free_irq:
4815	devm_free_irq(&ls_dev->dev, irq->virq, &ls_dev->dev);
4816free_mc_irq:
4817	fsl_mc_free_irqs(ls_dev);
4818
4819	return err;
4820}
4821
4822static void dpaa2_eth_add_ch_napi(struct dpaa2_eth_priv *priv)
4823{
4824	int i;
4825	struct dpaa2_eth_channel *ch;
4826
4827	for (i = 0; i < priv->num_channels; i++) {
4828		ch = priv->channel[i];
4829		/* NAPI weight *MUST* be a multiple of DPAA2_ETH_STORE_SIZE */
4830		netif_napi_add(priv->net_dev, &ch->napi, dpaa2_eth_poll);
4831	}
4832}
4833
4834static void dpaa2_eth_del_ch_napi(struct dpaa2_eth_priv *priv)
4835{
4836	int i;
4837	struct dpaa2_eth_channel *ch;
4838
4839	for (i = 0; i < priv->num_channels; i++) {
4840		ch = priv->channel[i];
4841		netif_napi_del(&ch->napi);
4842	}
4843}
4844
4845static void dpaa2_eth_free_rx_xdp_rxq(struct dpaa2_eth_priv *priv)
4846{
4847	int i;
4848
4849	for (i = 0; i < priv->num_fqs; i++) {
4850		if (priv->fq[i].type == DPAA2_RX_FQ &&
4851		    xdp_rxq_info_is_reg(&priv->fq[i].channel->xdp_rxq))
4852			xdp_rxq_info_unreg(&priv->fq[i].channel->xdp_rxq);
4853	}
4854}
4855
4856static int dpaa2_eth_probe(struct fsl_mc_device *dpni_dev)
4857{
4858	struct device *dev;
4859	struct net_device *net_dev = NULL;
4860	struct dpaa2_eth_priv *priv = NULL;
4861	int err = 0;
4862
4863	dev = &dpni_dev->dev;
4864
4865	/* Net device */
4866	net_dev = alloc_etherdev_mq(sizeof(*priv), DPAA2_ETH_MAX_NETDEV_QUEUES);
4867	if (!net_dev) {
4868		dev_err(dev, "alloc_etherdev_mq() failed\n");
4869		return -ENOMEM;
4870	}
4871
4872	SET_NETDEV_DEV(net_dev, dev);
4873	dev_set_drvdata(dev, net_dev);
4874
4875	priv = netdev_priv(net_dev);
4876	priv->net_dev = net_dev;
4877	SET_NETDEV_DEVLINK_PORT(net_dev, &priv->devlink_port);
4878
4879	mutex_init(&priv->mac_lock);
4880
4881	priv->iommu_domain = iommu_get_domain_for_dev(dev);
4882
4883	priv->tx_tstamp_type = HWTSTAMP_TX_OFF;
4884	priv->rx_tstamp = false;
4885
4886	priv->dpaa2_ptp_wq = alloc_workqueue("dpaa2_ptp_wq", WQ_PERCPU, 0);
4887	if (!priv->dpaa2_ptp_wq) {
4888		err = -ENOMEM;
4889		goto err_wq_alloc;
4890	}
4891
4892	INIT_WORK(&priv->tx_onestep_tstamp, dpaa2_eth_tx_onestep_tstamp);
4893	mutex_init(&priv->onestep_tstamp_lock);
4894	skb_queue_head_init(&priv->tx_skbs);
4895
4896	priv->rx_copybreak = DPAA2_ETH_DEFAULT_COPYBREAK;
4897
4898	/* Obtain a MC portal */
4899	err = fsl_mc_portal_allocate(dpni_dev, FSL_MC_IO_ATOMIC_CONTEXT_PORTAL,
4900				     &priv->mc_io);
4901	if (err) {
4902		if (err == -ENXIO) {
4903			dev_dbg(dev, "waiting for MC portal\n");
4904			err = -EPROBE_DEFER;
4905		} else {
4906			dev_err(dev, "MC portal allocation failed\n");
4907		}
4908		goto err_portal_alloc;
4909	}
4910
4911	/* MC objects initialization and configuration */
4912	err = dpaa2_eth_setup_dpni(dpni_dev);
4913	if (err)
4914		goto err_dpni_setup;
4915
4916	err = dpaa2_eth_setup_dpio(priv);
4917	if (err)
4918		goto err_dpio_setup;
4919
4920	dpaa2_eth_setup_fqs(priv);
4921
4922	err = dpaa2_eth_setup_default_dpbp(priv);
4923	if (err)
4924		goto err_dpbp_setup;
4925
4926	err = dpaa2_eth_bind_dpni(priv);
4927	if (err)
4928		goto err_bind;
4929
4930	/* Add a NAPI context for each channel */
4931	dpaa2_eth_add_ch_napi(priv);
4932
4933	/* Percpu statistics */
4934	priv->percpu_stats = alloc_percpu(*priv->percpu_stats);
4935	if (!priv->percpu_stats) {
4936		dev_err(dev, "alloc_percpu(percpu_stats) failed\n");
4937		err = -ENOMEM;
4938		goto err_alloc_percpu_stats;
4939	}
4940	priv->percpu_extras = alloc_percpu(*priv->percpu_extras);
4941	if (!priv->percpu_extras) {
4942		dev_err(dev, "alloc_percpu(percpu_extras) failed\n");
4943		err = -ENOMEM;
4944		goto err_alloc_percpu_extras;
4945	}
4946
4947	priv->sgt_cache = alloc_percpu(*priv->sgt_cache);
4948	if (!priv->sgt_cache) {
4949		dev_err(dev, "alloc_percpu(sgt_cache) failed\n");
4950		err = -ENOMEM;
4951		goto err_alloc_sgt_cache;
4952	}
4953
4954	priv->fd = alloc_percpu(*priv->fd);
4955	if (!priv->fd) {
4956		dev_err(dev, "alloc_percpu(fds) failed\n");
4957		err = -ENOMEM;
4958		goto err_alloc_fds;
4959	}
4960
4961	err = dpaa2_eth_netdev_init(net_dev);
4962	if (err)
4963		goto err_netdev_init;
4964
4965	/* Configure checksum offload based on current interface flags */
4966	err = dpaa2_eth_set_rx_csum(priv, !!(net_dev->features & NETIF_F_RXCSUM));
4967	if (err)
4968		goto err_csum;
4969
4970	err = dpaa2_eth_set_tx_csum(priv,
4971				    !!(net_dev->features & (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM)));
4972	if (err)
4973		goto err_csum;
4974
4975	err = dpaa2_eth_alloc_rings(priv);
4976	if (err)
4977		goto err_alloc_rings;
4978
4979#ifdef CONFIG_FSL_DPAA2_ETH_DCB
4980	if (dpaa2_eth_has_pause_support(priv) && priv->vlan_cls_enabled) {
4981		priv->dcbx_mode = DCB_CAP_DCBX_HOST | DCB_CAP_DCBX_VER_IEEE;
4982		net_dev->dcbnl_ops = &dpaa2_eth_dcbnl_ops;
4983	} else {
4984		dev_dbg(dev, "PFC not supported\n");
4985	}
4986#endif
4987
4988	err = dpaa2_eth_connect_mac(priv);
4989	if (err)
4990		goto err_connect_mac;
4991
4992	err = dpaa2_eth_setup_irqs(dpni_dev);
4993	if (err) {
4994		netdev_warn(net_dev, "Failed to set link interrupt, fall back to polling\n");
4995		priv->poll_thread = kthread_run(dpaa2_eth_poll_link_state, priv,
4996						"%s_poll_link", net_dev->name);
4997		if (IS_ERR(priv->poll_thread)) {
4998			dev_err(dev, "Error starting polling thread\n");
4999			goto err_poll_thread;
5000		}
5001		priv->do_link_poll = true;
5002	}
5003
5004	err = dpaa2_eth_dl_alloc(priv);
5005	if (err)
5006		goto err_dl_register;
5007
5008	err = dpaa2_eth_dl_traps_register(priv);
5009	if (err)
5010		goto err_dl_trap_register;
5011
5012	err = dpaa2_eth_dl_port_add(priv);
5013	if (err)
5014		goto err_dl_port_add;
5015
5016	net_dev->needed_headroom = DPAA2_ETH_SWA_SIZE + DPAA2_ETH_TX_BUF_ALIGN;
5017
5018	err = register_netdev(net_dev);
5019	if (err < 0) {
5020		dev_err(dev, "register_netdev() failed\n");
5021		goto err_netdev_reg;
5022	}
5023
5024#ifdef CONFIG_DEBUG_FS
5025	dpaa2_dbg_add(priv);
5026#endif
5027
5028	dpaa2_eth_dl_register(priv);
5029	dev_info(dev, "Probed interface %s\n", net_dev->name);
5030	return 0;
5031
5032err_netdev_reg:
5033	dpaa2_eth_dl_port_del(priv);
5034err_dl_port_add:
5035	dpaa2_eth_dl_traps_unregister(priv);
5036err_dl_trap_register:
5037	dpaa2_eth_dl_free(priv);
5038err_dl_register:
5039	if (priv->do_link_poll)
5040		kthread_stop(priv->poll_thread);
5041	else
5042		fsl_mc_free_irqs(dpni_dev);
5043err_poll_thread:
5044	dpaa2_eth_disconnect_mac(priv);
5045err_connect_mac:
5046	dpaa2_eth_free_rings(priv);
5047err_alloc_rings:
5048err_csum:
5049err_netdev_init:
5050	free_percpu(priv->fd);
5051err_alloc_fds:
5052	free_percpu(priv->sgt_cache);
5053err_alloc_sgt_cache:
5054	free_percpu(priv->percpu_extras);
5055err_alloc_percpu_extras:
5056	free_percpu(priv->percpu_stats);
5057err_alloc_percpu_stats:
5058	dpaa2_eth_del_ch_napi(priv);
5059	dpaa2_eth_free_rx_xdp_rxq(priv);
5060err_bind:
5061	dpaa2_eth_free_dpbps(priv);
5062err_dpbp_setup:
5063	dpaa2_eth_free_dpio(priv);
5064err_dpio_setup:
5065	dpaa2_eth_free_dpni(priv);
5066err_dpni_setup:
5067	fsl_mc_portal_free(priv->mc_io);
5068err_portal_alloc:
5069	destroy_workqueue(priv->dpaa2_ptp_wq);
5070err_wq_alloc:
5071	dev_set_drvdata(dev, NULL);
5072	free_netdev(net_dev);
5073
5074	return err;
5075}
5076
5077static void dpaa2_eth_remove(struct fsl_mc_device *ls_dev)
5078{
5079	struct device *dev;
5080	struct net_device *net_dev;
5081	struct dpaa2_eth_priv *priv;
5082
5083	dev = &ls_dev->dev;
5084	net_dev = dev_get_drvdata(dev);
5085	priv = netdev_priv(net_dev);
5086
5087	dpaa2_eth_dl_unregister(priv);
5088
5089#ifdef CONFIG_DEBUG_FS
5090	dpaa2_dbg_remove(priv);
5091#endif
5092
5093	unregister_netdev(net_dev);
5094
5095	dpaa2_eth_dl_port_del(priv);
5096	dpaa2_eth_dl_traps_unregister(priv);
5097	dpaa2_eth_dl_free(priv);
5098
5099	if (priv->do_link_poll)
5100		kthread_stop(priv->poll_thread);
5101	else
5102		fsl_mc_free_irqs(ls_dev);
5103
5104	dpaa2_eth_disconnect_mac(priv);
5105	dpaa2_eth_free_rings(priv);
5106	free_percpu(priv->fd);
5107	free_percpu(priv->sgt_cache);
5108	free_percpu(priv->percpu_stats);
5109	free_percpu(priv->percpu_extras);
5110
5111	dpaa2_eth_del_ch_napi(priv);
5112	dpaa2_eth_free_rx_xdp_rxq(priv);
5113	dpaa2_eth_free_dpbps(priv);
5114	dpaa2_eth_free_dpio(priv);
5115	dpaa2_eth_free_dpni(priv);
5116	if (priv->onestep_reg_base)
5117		iounmap(priv->onestep_reg_base);
5118
5119	fsl_mc_portal_free(priv->mc_io);
5120
5121	destroy_workqueue(priv->dpaa2_ptp_wq);
5122
5123	dev_dbg(net_dev->dev.parent, "Removed interface %s\n", net_dev->name);
5124
5125	free_netdev(net_dev);
5126}
5127
5128static const struct fsl_mc_device_id dpaa2_eth_match_id_table[] = {
5129	{
5130		.vendor = FSL_MC_VENDOR_FREESCALE,
5131		.obj_type = "dpni",
5132	},
5133	{ .vendor = 0x0 }
5134};
5135MODULE_DEVICE_TABLE(fslmc, dpaa2_eth_match_id_table);
5136
5137static struct fsl_mc_driver dpaa2_eth_driver = {
5138	.driver = {
5139		.name = KBUILD_MODNAME,
5140	},
5141	.probe = dpaa2_eth_probe,
5142	.remove = dpaa2_eth_remove,
5143	.match_id_table = dpaa2_eth_match_id_table
5144};
5145
5146static int __init dpaa2_eth_driver_init(void)
5147{
5148	int err;
5149
5150	dpaa2_eth_dbg_init();
5151	err = fsl_mc_driver_register(&dpaa2_eth_driver);
5152	if (err) {
5153		dpaa2_eth_dbg_exit();
5154		return err;
5155	}
5156
5157	return 0;
5158}
5159
5160static void __exit dpaa2_eth_driver_exit(void)
5161{
5162	dpaa2_eth_dbg_exit();
5163	fsl_mc_driver_unregister(&dpaa2_eth_driver);
5164}
5165
5166module_init(dpaa2_eth_driver_init);
5167module_exit(dpaa2_eth_driver_exit);