[RFC net-next 2/6] netdevsim: support PSP VC based queue steering

Gives the PSP steering selftest something to run against without
hardware. nsim_start_xmit() already mirrors the Tx queue index onto Rx,
so all the cookie does is override that choice.

Signed-off-by: Jakub Kicinski <[email protected]>
---
 drivers/net/netdevsim/netdevsim.h |  4 ++--
 drivers/net/netdevsim/netdev.c    |  5 +++--
 drivers/net/netdevsim/psp.c       | 26 ++++++++++++++++++++++++--
 3 files changed, 29 insertions(+), 6 deletions(-)

diff --git a/drivers/net/netdevsim/netdevsim.h b/drivers/net/netdevsim/netdevsim.h
index 55aec41237b9..3b1c52e43d46 100644
--- a/drivers/net/netdevsim/netdevsim.h
+++ b/drivers/net/netdevsim/netdevsim.h
@@ -453,13 +453,13 @@ void nsim_psp_uninit(struct netdevsim *ns);
 void nsim_psp_handle_ext(struct sk_buff *skb, struct skb_ext *psp_ext);
 enum skb_drop_reason
 nsim_do_psp(struct sk_buff *skb, struct netdevsim *ns,
-	    struct netdevsim *peer_ns, struct skb_ext **psp_ext);
+	    struct netdevsim *peer_ns, struct skb_ext **psp_ext, int *rxq);
 #else
 static inline int nsim_psp_init(struct netdevsim *ns) { return 0; }
 static inline void nsim_psp_uninit(struct netdevsim *ns) {}
 static inline enum skb_drop_reason
 nsim_do_psp(struct sk_buff *skb, struct netdevsim *ns,
-	    struct netdevsim *peer_ns, struct skb_ext **psp_ext)
+	    struct netdevsim *peer_ns, struct skb_ext **psp_ext, int *rxq)
 {
 	return 0;
 }
diff --git a/drivers/net/netdevsim/netdev.c b/drivers/net/netdevsim/netdev.c
index b4a99f3ceac6..04e416d1a377 100644
--- a/drivers/net/netdevsim/netdev.c
+++ b/drivers/net/netdevsim/netdev.c
@@ -147,11 +147,12 @@ static netdev_tx_t nsim_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		peer_dev = peer_ns->netdev;
 	}
 
-	dr = nsim_do_psp(skb, ns, peer_ns, &psp_ext);
+	rxq = skb_get_queue_mapping(skb);
+
+	dr = nsim_do_psp(skb, ns, peer_ns, &psp_ext, &rxq);
 	if (dr)
 		goto out_drop_free;
 
-	rxq = skb_get_queue_mapping(skb);
 	if (rxq >= peer_dev->num_rx_queues)
 		rxq = rxq % peer_dev->num_rx_queues;
 	rq = peer_ns->rq[rxq];
diff --git a/drivers/net/netdevsim/psp.c b/drivers/net/netdevsim/psp.c
index 32d95205a8cc..5aa5078889eb 100644
--- a/drivers/net/netdevsim/psp.c
+++ b/drivers/net/netdevsim/psp.c
@@ -14,9 +14,26 @@ void nsim_psp_handle_ext(struct sk_buff *skb, struct skb_ext *psp_ext)
 		__skb_ext_set(skb, SKB_EXT_PSP, psp_ext);
 }
 
+/* Pick the Rx queue for a decapsulated frame. Devices which support
+ * PSP_VC_STEER_RX match the destination queue ID carried in the
+ * cookie ahead of consulting the RSS table.
+ */
+static int nsim_psp_steer(struct netdevsim *peer_ns, struct sk_buff *skb,
+			  int rxq)
+{
+	struct net_device *dev = peer_ns->netdev;
+	struct psp_skb_ext *pse;
+
+	pse = skb_ext_find(skb, SKB_EXT_PSP);
+	if (!pse || pse->vc_dst >= dev->real_num_rx_queues)
+		return rxq;
+
+	return pse->vc_dst;
+}
+
 enum skb_drop_reason
 nsim_do_psp(struct sk_buff *skb, struct netdevsim *ns,
-	    struct netdevsim *peer_ns, struct skb_ext **psp_ext)
+	    struct netdevsim *peer_ns, struct skb_ext **psp_ext, int *rxq)
 {
 	enum skb_drop_reason rc = 0;
 	struct psp_dev *peer_psd;
@@ -44,7 +61,8 @@ nsim_do_psp(struct sk_buff *skb, struct netdevsim *ns,
 	}
 
 	net = sock_net(skb->sk);
-	if (!psp_dev_encapsulate(net, skb, pas->tx.spi, pas->version, 0, 0)) {
+	if (!psp_dev_encapsulate(net, skb, pas->tx.spi, pas->version, 0,
+				 psp_assoc_vc_tx_get(pas))) {
 		rc = SKB_DROP_REASON_PSP_OUTPUT;
 		goto out_unlock;
 	}
@@ -73,6 +91,9 @@ nsim_do_psp(struct sk_buff *skb, struct netdevsim *ns,
 			goto out_unlock;
 		}
 
+		if (peer_psd->config.vc_steer & (1 << PSP_VC_STEER_RX))
+			*rxq = nsim_psp_steer(peer_ns, skb, *rxq);
+
 		*psp_ext = skb->extensions;
 		refcount_inc(&(*psp_ext)->refcnt);
 		skb->decrypted = 1;
@@ -216,6 +237,7 @@ static struct psp_dev_caps nsim_psp_caps = {
 		    1 << PSP_VERSION_HDR0_AES_GCM_256 |
 		    1 << PSP_VERSION_HDR0_AES_GMAC_256,
 	.assoc_drv_spc = sizeof(void *),
+	.vc_steer = true,
 };
 
 static void __nsim_psp_uninit(struct netdevsim *ns, bool teardown)
-- 
2.55.0