Commit dc1a0038 authored by Radu Pirea (NXP OSS)'s avatar Radu Pirea (NXP OSS) Committed by David S. Miller
Browse files

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: default avatarRadu Pirea (NXP OSS) <radu-nicolae.pirea@oss.nxp.com>
Signed-off-by: default avatarDavid S. Miller <davem@davemloft.net>
parent 31a99fc0

drivers/net/phy/nxp-c45-tja11xx-macsec.c

+41 −0
Original line number Diff line number Diff line
@@ -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;