Implement mdo_insert_tx_tag to insert the TLV header in the ethernet
frame.
Signed-off-by: Radu Pirea (NXP OSS) <radu-nicolae.pirea@xxxxxxxxxxx>
---
Changes in v6:
- none
Changes in V5:
- removed unused defines MACSEC_TLV_CP and MACSEC_TLV_SC_ID_OFF
Changes in V4:
- removed macsec_extscs parameter
Changes in V3:
- extscs parameter renamed to macsec_extscs and improved description
Changes in V2:
- added extscs parameter to choose the TX SC selection mechanism between
and MAC SA based selection and TLV header based selection
drivers/net/phy/nxp-c45-tja11xx-macsec.c | 41 ++++++++++++++++++++++++
1 file changed, 41 insertions(+)
diff --git a/drivers/net/phy/nxp-c45-tja11xx-macsec.c b/drivers/net/phy/nxp-c45-tja11xx-macsec.c
index 4dd10ea6e06c..6ff0ceeeecd1 100644
--- 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,
@@ -1541,6 +1549,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,
@@ -1561,6 +1594,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)
@@ -1581,6 +1617,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;
--
2.34.1
