Quelle  dpaa2-xsk.c   Sprache: C

// SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause)
/* Copyright 2022 NXP
 */
#include <linux/filter.h>
#include <linux/compiler.h>
#include <linux/bpf_trace.h>
#include <net/xdp.h>
#include <net/xdp_sock_drv.h>

#include "dpaa2-eth.h"

static void dpaa2_eth_setup_consume_func(struct dpaa2_eth_priv *priv,
      struct dpaa2_eth_channel *ch,
      enum dpaa2_eth_fq_type type,
      dpaa2_eth_consume_cb_t *consume)
{
 struct dpaa2_eth_fq *fq;
 int i;

 for (i = 0; i < priv->num_fqs; i++) {
  fq = &priv->fq[i];

  if (fq->type != type)
   continue;
  if (fq->channel != ch)
   continue;

  fq->consume = consume;
 }
}

static u32 dpaa2_xsk_run_xdp(struct dpaa2_eth_priv *priv,
        struct dpaa2_eth_channel *ch,
        struct dpaa2_eth_fq *rx_fq,
        struct dpaa2_fd *fd, void *vaddr)
{
 dma_addr_t addr = dpaa2_fd_get_addr(fd);
 struct bpf_prog *xdp_prog;
 struct xdp_buff *xdp_buff;
 struct dpaa2_eth_swa *swa;
 u32 xdp_act = XDP_PASS;
 int err;

 xdp_prog = READ_ONCE(ch->xdp.prog);
 if (!xdp_prog)
  goto out;

 swa = (struct dpaa2_eth_swa *)(vaddr + DPAA2_ETH_RX_HWA_SIZE +
           ch->xsk_pool->umem->headroom);
 xdp_buff = swa->xsk.xdp_buff;

 xdp_buff->data_hard_start = vaddr;
 xdp_buff->data = vaddr + dpaa2_fd_get_offset(fd);
 xdp_buff->data_end = xdp_buff->data + dpaa2_fd_get_len(fd);
 xdp_set_data_meta_invalid(xdp_buff);
 xdp_buff->rxq = &ch->xdp_rxq;

 xsk_buff_dma_sync_for_cpu(xdp_buff);
 xdp_act = bpf_prog_run_xdp(xdp_prog, xdp_buff);

 /* xdp.data pointer may have changed */
 dpaa2_fd_set_offset(fd, xdp_buff->data - vaddr);
 dpaa2_fd_set_len(fd, xdp_buff->data_end - xdp_buff->data);

 if (likely(xdp_act == XDP_REDIRECT)) {
  err = xdp_do_redirect(priv->net_dev, xdp_buff, xdp_prog);
  if (unlikely(err)) {
   ch->stats.xdp_drop++;
   dpaa2_eth_recycle_buf(priv, ch, addr);
  } else {
   ch->buf_count--;
   ch->stats.xdp_redirect++;
  }

  goto xdp_redir;
 }

 switch (xdp_act) {
 case XDP_PASS:
  break;
 case XDP_TX:
  dpaa2_eth_xdp_enqueue(priv, ch, fd, vaddr, rx_fq->flowid);
  break;
 default:
  bpf_warn_invalid_xdp_action(priv->net_dev, xdp_prog, xdp_act);
  fallthrough;
 case XDP_ABORTED:
  trace_xdp_exception(priv->net_dev, xdp_prog, xdp_act);
  fallthrough;
 case XDP_DROP:
  dpaa2_eth_recycle_buf(priv, ch, addr);
  ch->stats.xdp_drop++;
  break;
 }

xdp_redir:
 ch->xdp.res |= xdp_act;
out:
 return xdp_act;
}

/* Rx frame processing routine for the AF_XDP fast path */
static void dpaa2_xsk_rx(struct dpaa2_eth_priv *priv,
    struct dpaa2_eth_channel *ch,
    const struct dpaa2_fd *fd,
    struct dpaa2_eth_fq *fq)
{
 dma_addr_t addr = dpaa2_fd_get_addr(fd);
 u8 fd_format = dpaa2_fd_get_format(fd);
 struct rtnl_link_stats64 *percpu_stats;
 u32 fd_length = dpaa2_fd_get_len(fd);
 struct sk_buff *skb;
 void *vaddr;
 u32 xdp_act;

 trace_dpaa2_rx_xsk_fd(priv->net_dev, fd);

 vaddr = dpaa2_iova_to_virt(priv->iommu_domain, addr);
 percpu_stats = this_cpu_ptr(priv->percpu_stats);

 if (fd_format != dpaa2_fd_single) {
  WARN_ON(priv->xdp_prog);
  /* AF_XDP doesn't support any other formats */
  goto err_frame_format;
 }

 xdp_act = dpaa2_xsk_run_xdp(priv, ch, fq, (struct dpaa2_fd *)fd, vaddr);
 if (xdp_act != XDP_PASS) {
  percpu_stats->rx_packets++;
  percpu_stats->rx_bytes += dpaa2_fd_get_len(fd);
  return;
 }

 /* Build skb */
 skb = dpaa2_eth_alloc_skb(priv, ch, fd, fd_length, vaddr);
 if (!skb)
  /* Nothing else we can do, recycle the buffer and
 * drop the frame.
 */
  goto err_alloc_skb;

 /* Send the skb to the Linux networking stack */
 dpaa2_eth_receive_skb(priv, ch, fd, vaddr, fq, percpu_stats, skb);

 return;

err_alloc_skb:
 dpaa2_eth_recycle_buf(priv, ch, addr);
err_frame_format:
 percpu_stats->rx_dropped++;
}

static void dpaa2_xsk_set_bp_per_qdbin(struct dpaa2_eth_priv *priv,
           struct dpni_pools_cfg *pools_params)
{
 int curr_bp = 0, i, j;

 pools_params->pool_options = DPNI_POOL_ASSOC_QDBIN;
 for (i = 0; i < priv->num_bps; i++) {
  for (j = 0; j < priv->num_channels; j++)
   if (priv->bp[i] == priv->channel[j]->bp)
    pools_params->pools[curr_bp].priority_mask |= (1 << j);
  if (!pools_params->pools[curr_bp].priority_mask)
   continue;

  pools_params->pools[curr_bp].dpbp_id = priv->bp[i]->bpid;
  pools_params->pools[curr_bp].buffer_size = priv->rx_buf_size;
  pools_params->pools[curr_bp++].backup_pool = 0;
 }
 pools_params->num_dpbp = curr_bp;
}

static int dpaa2_xsk_disable_pool(struct net_device *dev, u16 qid)
{
 struct xsk_buff_pool *pool = xsk_get_pool_from_qid(dev, qid);
 struct dpaa2_eth_priv *priv = netdev_priv(dev);
 struct dpni_pools_cfg pools_params = { 0 };
 struct dpaa2_eth_channel *ch;
 int err;
 bool up;

 ch = priv->channel[qid];
 if (!ch->xsk_pool)
  return -EINVAL;

 up = netif_running(dev);
 if (up)
  dev_close(dev);

 xsk_pool_dma_unmap(pool, 0);
 err = xdp_rxq_info_reg_mem_model(&ch->xdp_rxq,
      MEM_TYPE_PAGE_ORDER0, NULL);
 if (err)
  netdev_err(dev, "xsk_rxq_info_reg_mem_model() failed (err = %d)\n",
      err);

 dpaa2_eth_free_dpbp(priv, ch->bp);

 ch->xsk_zc = false;
 ch->xsk_pool = NULL;
 ch->xsk_tx_pkts_sent = 0;
 ch->bp = priv->bp[DPAA2_ETH_DEFAULT_BP_IDX];

 dpaa2_eth_setup_consume_func(priv, ch, DPAA2_RX_FQ, dpaa2_eth_rx);

 dpaa2_xsk_set_bp_per_qdbin(priv, &pools_params);
 err = dpni_set_pools(priv->mc_io, 0, priv->mc_token, &pools_params);
 if (err)
  netdev_err(dev, "dpni_set_pools() failed\n");

 if (up) {
  err = dev_open(dev, NULL);
  if (err)
   return err;
 }

 return 0;
}

static int dpaa2_xsk_enable_pool(struct net_device *dev,
     struct xsk_buff_pool *pool,
     u16 qid)
{
 struct dpaa2_eth_priv *priv = netdev_priv(dev);
 struct dpni_pools_cfg pools_params = { 0 };
 struct dpaa2_eth_channel *ch;
 int err, err2;
 bool up;

 if (priv->dpni_attrs.wriop_version < DPAA2_WRIOP_VERSION(3, 0, 0)) {
  netdev_err(dev, "AF_XDP zero-copy not supported on devices <= WRIOP(3, 0, 0)\n");
  return -EOPNOTSUPP;
 }

 if (priv->dpni_attrs.num_queues > 8) {
  netdev_err(dev, "AF_XDP zero-copy not supported on DPNI with more then 8 queues\n");
  return -EOPNOTSUPP;
 }

 up = netif_running(dev);
 if (up)
  dev_close(dev);

 err = xsk_pool_dma_map(pool, priv->net_dev->dev.parent, 0);
 if (err) {
  netdev_err(dev, "xsk_pool_dma_map() failed (err = %d)\n",
      err);
  goto err_dma_unmap;
 }

 ch = priv->channel[qid];
 err = xdp_rxq_info_reg_mem_model(&ch->xdp_rxq, MEM_TYPE_XSK_BUFF_POOL, NULL);
 if (err) {
  netdev_err(dev, "xdp_rxq_info_reg_mem_model() failed (err = %d)\n", err);
  goto err_mem_model;
 }
 xsk_pool_set_rxq_info(pool, &ch->xdp_rxq);

 priv->bp[priv->num_bps] = dpaa2_eth_allocate_dpbp(priv);
 if (IS_ERR(priv->bp[priv->num_bps])) {
  err = PTR_ERR(priv->bp[priv->num_bps]);
  goto err_bp_alloc;
 }
 ch->xsk_zc = true;
 ch->xsk_pool = pool;
 ch->bp = priv->bp[priv->num_bps++];

 dpaa2_eth_setup_consume_func(priv, ch, DPAA2_RX_FQ, dpaa2_xsk_rx);

 dpaa2_xsk_set_bp_per_qdbin(priv, &pools_params);
 err = dpni_set_pools(priv->mc_io, 0, priv->mc_token, &pools_params);
 if (err) {
  netdev_err(dev, "dpni_set_pools() failed\n");
  goto err_set_pools;
 }

 if (up) {
  err = dev_open(dev, NULL);
  if (err)
   return err;
 }

 return 0;

err_set_pools:
 err2 = dpaa2_xsk_disable_pool(dev, qid);
 if (err2)
  netdev_err(dev, "dpaa2_xsk_disable_pool() failed %d\n", err2);
err_bp_alloc:
 err2 = xdp_rxq_info_reg_mem_model(&priv->channel[qid]->xdp_rxq,
       MEM_TYPE_PAGE_ORDER0, NULL);
 if (err2)
  netdev_err(dev, "xsk_rxq_info_reg_mem_model() failed with %d)\n", err2);
err_mem_model:
 xsk_pool_dma_unmap(pool, 0);
err_dma_unmap:
 if (up)
  dev_open(dev, NULL);

 return err;
}

int dpaa2_xsk_setup_pool(struct net_device *dev, struct xsk_buff_pool *pool, u16 qid)
{
 return pool ? dpaa2_xsk_enable_pool(dev, pool, qid) :
        dpaa2_xsk_disable_pool(dev, qid);
}

int dpaa2_xsk_wakeup(struct net_device *dev, u32 qid, u32 flags)
{
 struct dpaa2_eth_priv *priv = netdev_priv(dev);
 struct dpaa2_eth_channel *ch = priv->channel[qid];

 if (!priv->link_state.up)
  return -ENETDOWN;

 if (!priv->xdp_prog)
  return -EINVAL;

 if (!ch->xsk_zc)
  return -EINVAL;

 /* We do not have access to a per channel SW interrupt, so instead we
 * schedule a NAPI instance.
 */
 if (!napi_if_scheduled_mark_missed(&ch->napi))
  napi_schedule(&ch->napi);

 return 0;
}

static int dpaa2_xsk_tx_build_fd(struct dpaa2_eth_priv *priv,
     struct dpaa2_eth_channel *ch,
     struct dpaa2_fd *fd,
     struct xdp_desc *xdp_desc)
{
 struct device *dev = priv->net_dev->dev.parent;
 struct dpaa2_sg_entry *sgt;
 struct dpaa2_eth_swa *swa;
 void *sgt_buf = NULL;
 dma_addr_t sgt_addr;
 int sgt_buf_size;
 dma_addr_t addr;
 int err = 0;

 /* Prepare the HW SGT structure */
 sgt_buf_size = priv->tx_data_offset + sizeof(struct dpaa2_sg_entry);
 sgt_buf = dpaa2_eth_sgt_get(priv);
 if (unlikely(!sgt_buf))
  return -ENOMEM;
 sgt = (struct dpaa2_sg_entry *)(sgt_buf + priv->tx_data_offset);

 /* Get the address of the XSK Tx buffer */
 addr = xsk_buff_raw_get_dma(ch->xsk_pool, xdp_desc->addr);
 xsk_buff_raw_dma_sync_for_device(ch->xsk_pool, addr, xdp_desc->len);

 /* Fill in the HW SGT structure */
 dpaa2_sg_set_addr(sgt, addr);
 dpaa2_sg_set_len(sgt, xdp_desc->len);
 dpaa2_sg_set_final(sgt, true);

 /* Store the necessary info in the SGT buffer */
 swa = (struct dpaa2_eth_swa *)sgt_buf;
 swa->type = DPAA2_ETH_SWA_XSK;
 swa->xsk.sgt_size = sgt_buf_size;

 /* Separately map the SGT buffer */
 sgt_addr = dma_map_single(dev, sgt_buf, sgt_buf_size, DMA_BIDIRECTIONAL);
 if (unlikely(dma_mapping_error(dev, sgt_addr))) {
  err = -ENOMEM;
  goto sgt_map_failed;
 }

 /* Initialize FD fields */
 memset(fd, 0, sizeof(struct dpaa2_fd));
 dpaa2_fd_set_offset(fd, priv->tx_data_offset);
 dpaa2_fd_set_format(fd, dpaa2_fd_sg);
 dpaa2_fd_set_addr(fd, sgt_addr);
 dpaa2_fd_set_len(fd, xdp_desc->len);
 dpaa2_fd_set_ctrl(fd, FD_CTRL_PTA);

 return 0;

sgt_map_failed:
 dpaa2_eth_sgt_recycle(priv, sgt_buf);

 return err;
}

bool dpaa2_xsk_tx(struct dpaa2_eth_priv *priv,
    struct dpaa2_eth_channel *ch)
{
 struct xdp_desc *xdp_descs = ch->xsk_pool->tx_descs;
 struct dpaa2_eth_drv_stats *percpu_extras;
 struct rtnl_link_stats64 *percpu_stats;
 int budget = DPAA2_ETH_TX_ZC_PER_NAPI;
 int total_enqueued, enqueued;
 int retries, max_retries;
 struct dpaa2_eth_fq *fq;
 struct dpaa2_fd *fds;
 int batch, i, err;

 percpu_stats = this_cpu_ptr(priv->percpu_stats);
 percpu_extras = this_cpu_ptr(priv->percpu_extras);
 fds = (this_cpu_ptr(priv->fd))->array;

 /* Use the FQ with the same idx as the affine CPU */
 fq = &priv->fq[ch->nctx.desired_cpu];

 batch = xsk_tx_peek_release_desc_batch(ch->xsk_pool, budget);
 if (!batch)
  return false;

 /* Create a FD for each XSK frame to be sent */
 for (i = 0; i < batch; i++) {
  err = dpaa2_xsk_tx_build_fd(priv, ch, &fds[i], &xdp_descs[i]);
  if (err) {
   batch = i;
   break;
  }

  trace_dpaa2_tx_xsk_fd(priv->net_dev, &fds[i]);
 }

 /* Enqueue all the created FDs */
 max_retries = batch * DPAA2_ETH_ENQUEUE_RETRIES;
 total_enqueued = 0;
 enqueued = 0;
 retries = 0;
 while (total_enqueued < batch && retries < max_retries) {
  err = priv->enqueue(priv, fq, &fds[total_enqueued], 0,
        batch - total_enqueued, &enqueued);
  if (err == -EBUSY) {
   retries++;
   continue;
  }

  total_enqueued += enqueued;
 }
 percpu_extras->tx_portal_busy += retries;

 /* Update statistics */
 percpu_stats->tx_packets += total_enqueued;
 for (i = 0; i < total_enqueued; i++)
  percpu_stats->tx_bytes += dpaa2_fd_get_len(&fds[i]);
 for (i = total_enqueued; i < batch; i++) {
  dpaa2_eth_free_tx_fd(priv, ch, fq, &fds[i], false);
  percpu_stats->tx_errors++;
 }

 return total_enqueued == budget;
}