net: phy: nxp-c45-tja11xx: implement mdo_insert_tx_tag

Implement mdo_insert_tx_tag to insert the TLV header in the ethernet
frame.

Signed-off-by: Radu Pirea (NXP OSS) <radu-nicolae.pirea@oss.nxp.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/phy/nxp-c45-tja11xx-macsec.c b/drivers/net/phy/nxp-c45-tja11xx-macsec.c
index 40a926ffb3188aaa4fc2a64f40d3f045e441ed9c..550ef08970f444d8d0f6b7a8ac15b1051736d042 100644 (file)
--- a/drivers/net/phy/nxp-c45-tja11xx-macsec.c
+++ b/drivers/net/phy/nxp-c45-tja11xx-macsec.c
@@ -11,6 +11,7 @@
 #include <linux/module.h>
 #include <linux/phy.h>
 #include <linux/processor.h>
+#include <net/dst_metadata.h>
 #include <net/macsec.h>
 
 #include "nxp-c45-tja11xx.h"
@@ -118,6 +119,8 @@
 #define ADPTR_CNTRL                    0x0F00
 #define ADPTR_CNTRL_CONFIG_EN          BIT(14)
 #define ADPTR_CNTRL_ADPTR_EN           BIT(12)
+#define ADPTR_TX_TAG_CNTRL             0x0F0C
+#define ADPTR_TX_TAG_CNTRL_ENA         BIT(31)
 
 #define TX_SC_FLT_BASE                 0x800
 #define TX_SC_FLT_SIZE                 0x10
@@ -166,6 +169,11 @@
 #define MACSEC_INPBTS                  0x0638
 #define MACSEC_IPSNFS                  0x063C
 
+#define TJA11XX_TLV_TX_NEEDED_HEADROOM (32)
+#define TJA11XX_TLV_NEEDED_TAILROOM    (0)
+
+#define ETH_P_TJA11XX_TLV              (0x4e58)
+
 enum nxp_c45_sa_type {
        TX_SA,
        RX_SA,
@@ -1543,6 +1551,31 @@ static int nxp_c45_mdo_get_rx_sa_stats(struct macsec_context *ctx)
        return 0;
 }
 
+struct tja11xx_tlv_header {
+       struct ethhdr eth;
+       u8 subtype;
+       u8 len;
+       u8 payload[28];
+};
+
+static int nxp_c45_mdo_insert_tx_tag(struct phy_device *phydev,
+                                    struct sk_buff *skb)
+{
+       struct tja11xx_tlv_header *tlv;
+       struct ethhdr *eth;
+
+       eth = eth_hdr(skb);
+       tlv = skb_push(skb, TJA11XX_TLV_TX_NEEDED_HEADROOM);
+       memmove(tlv, eth, sizeof(*eth));
+       skb_reset_mac_header(skb);
+       tlv->eth.h_proto = htons(ETH_P_TJA11XX_TLV);
+       tlv->subtype = 1;
+       tlv->len = sizeof(tlv->payload);
+       memset(tlv->payload, 0, sizeof(tlv->payload));
+
+       return 0;
+}
+
 static const struct macsec_ops nxp_c45_macsec_ops = {
        .mdo_dev_open = nxp_c45_mdo_dev_open,
        .mdo_dev_stop = nxp_c45_mdo_dev_stop,
@@ -1563,6 +1596,9 @@ static const struct macsec_ops nxp_c45_macsec_ops = {
        .mdo_get_tx_sa_stats = nxp_c45_mdo_get_tx_sa_stats,
        .mdo_get_rx_sc_stats = nxp_c45_mdo_get_rx_sc_stats,
        .mdo_get_rx_sa_stats = nxp_c45_mdo_get_rx_sa_stats,
+       .mdo_insert_tx_tag = nxp_c45_mdo_insert_tx_tag,
+       .needed_headroom = TJA11XX_TLV_TX_NEEDED_HEADROOM,
+       .needed_tailroom = TJA11XX_TLV_NEEDED_TAILROOM,
 };
 
 int nxp_c45_macsec_config_init(struct phy_device *phydev)
@@ -1583,6 +1619,11 @@ int nxp_c45_macsec_config_init(struct phy_device *phydev)
        if (ret)
                return ret;
 
+       ret = nxp_c45_macsec_write(phydev, ADPTR_TX_TAG_CNTRL,
+                                  ADPTR_TX_TAG_CNTRL_ENA);
+       if (ret)
+               return ret;
+
        ret = nxp_c45_macsec_write(phydev, ADPTR_CNTRL, ADPTR_CNTRL_ADPTR_EN);
        if (ret)
                return ret;