1 files changed, 92 insertions, 136 deletions

diff --git a/drivers/net/phy/mscc/mscc_macsec.c b/drivers/net/phy/mscc/mscc_macsec.c
index b4d3dc4068e2..4f39ba63a9a9 100644
--- a/drivers/net/phy/mscc/mscc_macsec.c
+++ b/drivers/net/phy/mscc/mscc_macsec.c
@@ -1,16 +1,16 @@
 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
 /*
- * Driver for Microsemi VSC85xx PHYs
+ * Driver for Microsemi VSC85xx PHYs - MACsec support
  *
- * Author: Nagaraju Lakkaraju
+ * Author: Antoine Tenart
  * License: Dual MIT/GPL
- * Copyright (c) 2016 Microsemi Corporation
+ * Copyright (c) 2020 Microsemi Corporation
  */
 
 #include <linux/phy.h>
 #include <dt-bindings/net/mscc-phy-vsc8531.h>
 
-#include <crypto/skcipher.h>
+#include <crypto/aes.h>
 
 #include <net/macsec.h>
 
@@ -285,7 +285,9 @@ static void vsc8584_macsec_mac_init(struct phy_device *phydev,
 				 MSCC_MAC_CFG_PKTINF_CFG_STRIP_PREAMBLE_ENA |
 				 MSCC_MAC_CFG_PKTINF_CFG_INSERT_PREAMBLE_ENA |
 				 (bank == HOST_MAC ?
-				  MSCC_MAC_CFG_PKTINF_CFG_ENABLE_TX_PADDING : 0));
+				  MSCC_MAC_CFG_PKTINF_CFG_ENABLE_TX_PADDING : 0) |
+				 (IS_ENABLED(CONFIG_NETWORK_PHY_TIMESTAMPING) ?
+				  MSCC_MAC_CFG_PKTINF_CFG_MACSEC_BYPASS_NUM_PTP_STALL_CLKS(0x8) : 0));
 
 	val = vsc8584_macsec_phy_read(phydev, bank, MSCC_MAC_CFG_MODE_CFG);
 	val &= ~MSCC_MAC_CFG_MODE_CFG_DISABLE_DIC;
@@ -383,21 +385,23 @@ static void vsc8584_macsec_flow(struct phy_device *phydev,
 	}
 
 	if (bank == MACSEC_INGR && flow->match.sci && flow->rx_sa->sc->sci) {
+		u64 sci = (__force u64)flow->rx_sa->sc->sci;
+
 		match |= MSCC_MS_SAM_MISC_MATCH_TCI(BIT(3));
 		mask |= MSCC_MS_SAM_MASK_TCI_MASK(BIT(3)) |
 			MSCC_MS_SAM_MASK_SCI_MASK;
 
 		vsc8584_macsec_phy_write(phydev, bank, MSCC_MS_SAM_MATCH_SCI_LO(idx),
-					 lower_32_bits(flow->rx_sa->sc->sci));
+					 lower_32_bits(sci));
 		vsc8584_macsec_phy_write(phydev, bank, MSCC_MS_SAM_MATCH_SCI_HI(idx),
-					 upper_32_bits(flow->rx_sa->sc->sci));
+					 upper_32_bits(sci));
 	}
 
 	if (flow->match.etype) {
 		mask |= MSCC_MS_SAM_MASK_MAC_ETYPE_MASK;
 
 		vsc8584_macsec_phy_write(phydev, bank, MSCC_MS_SAM_MAC_SA_MATCH_HI(idx),
-					 MSCC_MS_SAM_MAC_SA_MATCH_HI_ETYPE(htons(flow->etype)));
+					 MSCC_MS_SAM_MAC_SA_MATCH_HI_ETYPE((__force u32)htons(flow->etype)));
 	}
 
 	match |= MSCC_MS_SAM_MISC_MATCH_PRIORITY(flow->priority);
@@ -497,55 +501,33 @@ static u32 vsc8584_macsec_flow_context_id(struct macsec_flow *flow)
 }
 
 /* Derive the AES key to get a key for the hash autentication */
-static int vsc8584_macsec_derive_key(const u8 key[MACSEC_KEYID_LEN],
-				     u16 key_len, u8 hkey[16])
+static int vsc8584_macsec_derive_key(const u8 *key, u16 key_len, u8 hkey[16])
 {
-	struct crypto_skcipher *tfm = crypto_alloc_skcipher("ecb(aes)", 0, 0);
-	struct skcipher_request *req = NULL;
-	struct scatterlist src, dst;
-	DECLARE_CRYPTO_WAIT(wait);
-	u32 input[4] = {0};
+	const u8 input[AES_BLOCK_SIZE] = {0};
+	struct crypto_aes_ctx ctx;
 	int ret;
 
-	if (IS_ERR(tfm))
-		return PTR_ERR(tfm);
-
-	req = skcipher_request_alloc(tfm, GFP_KERNEL);
-	if (!req) {
-		ret = -ENOMEM;
-		goto out;
-	}
-
-	skcipher_request_set_callback(req, CRYPTO_TFM_REQ_MAY_BACKLOG |
-				      CRYPTO_TFM_REQ_MAY_SLEEP, crypto_req_done,
-				      &wait);
-	ret = crypto_skcipher_setkey(tfm, key, key_len);
-	if (ret < 0)
-		goto out;
-
-	sg_init_one(&src, input, 16);
-	sg_init_one(&dst, hkey, 16);
-	skcipher_request_set_crypt(req, &src, &dst, 16, NULL);
-
-	ret = crypto_wait_req(crypto_skcipher_encrypt(req), &wait);
+	ret = aes_expandkey(&ctx, key, key_len);
+	if (ret)
+		return ret;
 
-out:
-	skcipher_request_free(req);
-	crypto_free_skcipher(tfm);
-	return ret;
+	aes_encrypt(&ctx, hkey, input);
+	memzero_explicit(&ctx, sizeof(ctx));
+	return 0;
 }
 
 static int vsc8584_macsec_transformation(struct phy_device *phydev,
-					 struct macsec_flow *flow)
+					 struct macsec_flow *flow,
+					 const u8 *key)
 {
 	struct vsc8531_private *priv = phydev->priv;
 	enum macsec_bank bank = flow->bank;
 	int i, ret, index = flow->index;
 	u32 rec = 0, control = 0;
 	u8 hkey[16];
-	sci_t sci;
+	u64 sci;
 
-	ret = vsc8584_macsec_derive_key(flow->key, priv->secy->key_len, hkey);
+	ret = vsc8584_macsec_derive_key(key, priv->secy->key_len, hkey);
 	if (ret)
 		return ret;
 
@@ -581,7 +563,7 @@ static int vsc8584_macsec_transformation(struct phy_device *phydev,
 	for (i = 0; i < priv->secy->key_len / sizeof(u32); i++)
 		vsc8584_macsec_phy_write(phydev, bank,
 					 MSCC_MS_XFORM_REC(index, rec++),
-					 ((u32 *)flow->key)[i]);
+					 ((u32 *)key)[i]);
 
 	/* Set the authentication key */
 	for (i = 0; i < 4; i++)
@@ -601,7 +583,7 @@ static int vsc8584_macsec_transformation(struct phy_device *phydev,
 					 priv->secy->replay_window);
 
 	/* Set the input vectors */
-	sci = bank == MACSEC_INGR ? flow->rx_sa->sc->sci : priv->secy->sci;
+	sci = (__force u64)(bank == MACSEC_INGR ? flow->rx_sa->sc->sci : priv->secy->sci);
 	vsc8584_macsec_phy_write(phydev, bank, MSCC_MS_XFORM_REC(index, rec++),
 				 lower_32_bits(sci));
 	vsc8584_macsec_phy_write(phydev, bank, MSCC_MS_XFORM_REC(index, rec++),
@@ -653,24 +635,11 @@ static void vsc8584_macsec_free_flow(struct vsc8531_private *priv,
 	kfree(flow);
 }
 
-static int vsc8584_macsec_add_flow(struct phy_device *phydev,
-				   struct macsec_flow *flow, bool update)
+static void vsc8584_macsec_add_flow(struct phy_device *phydev,
+				    struct macsec_flow *flow)
 {
-	int ret;
-
 	flow->port = MSCC_MS_PORT_CONTROLLED;
 	vsc8584_macsec_flow(phydev, flow);
-
-	if (update)
-		return 0;
-
-	ret = vsc8584_macsec_transformation(phydev, flow);
-	if (ret) {
-		vsc8584_macsec_free_flow(phydev->priv, flow);
-		return ret;
-	}
-
-	return 0;
 }
 
 static int vsc8584_macsec_default_flows(struct phy_device *phydev)
@@ -723,14 +692,7 @@ static int __vsc8584_macsec_add_rxsa(struct macsec_context *ctx,
 {
 	struct phy_device *phydev = ctx->phydev;
 	struct vsc8531_private *priv = phydev->priv;
-
-	if (!flow) {
-		flow = vsc8584_macsec_alloc_flow(priv, MACSEC_INGR);
-		if (IS_ERR(flow))
-			return PTR_ERR(flow);
-
-		memcpy(flow->key, ctx->sa.key, priv->secy->key_len);
-	}
+	int ret;
 
 	flow->assoc_num = ctx->sa.assoc_num;
 	flow->rx_sa = ctx->sa.rx_sa;
@@ -742,22 +704,22 @@ static int __vsc8584_macsec_add_rxsa(struct macsec_context *ctx,
 	if (priv->secy->validate_frames != MACSEC_VALIDATE_DISABLED)
 		flow->match.untagged = 1;
 
-	return vsc8584_macsec_add_flow(phydev, flow, update);
+	vsc8584_macsec_add_flow(phydev, flow);
+
+	if (update)
+		return 0;
+
+	ret = vsc8584_macsec_transformation(phydev, flow, ctx->sa.key);
+	if (ret)
+		vsc8584_macsec_free_flow(phydev->priv, flow);
+
+	return ret;
 }
 
 static int __vsc8584_macsec_add_txsa(struct macsec_context *ctx,
 				     struct macsec_flow *flow, bool update)
 {
-	struct phy_device *phydev = ctx->phydev;
-	struct vsc8531_private *priv = phydev->priv;
-
-	if (!flow) {
-		flow = vsc8584_macsec_alloc_flow(priv, MACSEC_EGR);
-		if (IS_ERR(flow))
-			return PTR_ERR(flow);
-
-		memcpy(flow->key, ctx->sa.key, priv->secy->key_len);
-	}
+	int ret;
 
 	flow->assoc_num = ctx->sa.assoc_num;
 	flow->tx_sa = ctx->sa.tx_sa;
@@ -765,7 +727,16 @@ static int __vsc8584_macsec_add_txsa(struct macsec_context *ctx,
 	/* Always match untagged packets on egress */
 	flow->match.untagged = 1;
 
-	return vsc8584_macsec_add_flow(phydev, flow, update);
+	vsc8584_macsec_add_flow(ctx->phydev, flow);
+
+	if (update)
+		return 0;
+
+	ret = vsc8584_macsec_transformation(ctx->phydev, flow, ctx->sa.key);
+	if (ret)
+		vsc8584_macsec_free_flow(ctx->phydev->priv, flow);
+
+	return ret;
 }
 
 static int vsc8584_macsec_dev_open(struct macsec_context *ctx)
@@ -773,10 +744,6 @@ static int vsc8584_macsec_dev_open(struct macsec_context *ctx)
 	struct vsc8531_private *priv = ctx->phydev->priv;
 	struct macsec_flow *flow, *tmp;
 
-	/* No operation to perform before the commit step */
-	if (ctx->prepare)
-		return 0;
-
 	list_for_each_entry_safe(flow, tmp, &priv->macsec_flows, list)
 		vsc8584_macsec_flow_enable(ctx->phydev, flow);
 
@@ -788,10 +755,6 @@ static int vsc8584_macsec_dev_stop(struct macsec_context *ctx)
 	struct vsc8531_private *priv = ctx->phydev->priv;
 	struct macsec_flow *flow, *tmp;
 
-	/* No operation to perform before the commit step */
-	if (ctx->prepare)
-		return 0;
-
 	list_for_each_entry_safe(flow, tmp, &priv->macsec_flows, list)
 		vsc8584_macsec_flow_disable(ctx->phydev, flow);
 
@@ -803,12 +766,8 @@ static int vsc8584_macsec_add_secy(struct macsec_context *ctx)
 	struct vsc8531_private *priv = ctx->phydev->priv;
 	struct macsec_secy *secy = ctx->secy;
 
-	if (ctx->prepare) {
-		if (priv->secy)
-			return -EEXIST;
-
-		return 0;
-	}
+	if (priv->secy)
+		return -EEXIST;
 
 	priv->secy = secy;
 
@@ -825,10 +784,6 @@ static int vsc8584_macsec_del_secy(struct macsec_context *ctx)
 	struct vsc8531_private *priv = ctx->phydev->priv;
 	struct macsec_flow *flow, *tmp;
 
-	/* No operation to perform before the commit step */
-	if (ctx->prepare)
-		return 0;
-
 	list_for_each_entry_safe(flow, tmp, &priv->macsec_flows, list)
 		vsc8584_macsec_del_flow(ctx->phydev, flow);
 
@@ -841,10 +796,6 @@ static int vsc8584_macsec_del_secy(struct macsec_context *ctx)
 
 static int vsc8584_macsec_upd_secy(struct macsec_context *ctx)
 {
-	/* No operation to perform before the commit step */
-	if (ctx->prepare)
-		return 0;
-
 	vsc8584_macsec_del_secy(ctx);
 	return vsc8584_macsec_add_secy(ctx);
 }
@@ -865,10 +816,6 @@ static int vsc8584_macsec_del_rxsc(struct macsec_context *ctx)
 	struct vsc8531_private *priv = ctx->phydev->priv;
 	struct macsec_flow *flow, *tmp;
 
-	/* No operation to perform before the commit step */
-	if (ctx->prepare)
-		return 0;
-
 	list_for_each_entry_safe(flow, tmp, &priv->macsec_flows, list) {
 		if (flow->bank == MACSEC_INGR && flow->rx_sa &&
 		    flow->rx_sa->sc->sci == ctx->rx_sc->sci)
@@ -880,33 +827,41 @@ static int vsc8584_macsec_del_rxsc(struct macsec_context *ctx)
 
 static int vsc8584_macsec_add_rxsa(struct macsec_context *ctx)
 {
-	struct macsec_flow *flow = NULL;
-
-	if (ctx->prepare)
-		return __vsc8584_macsec_add_rxsa(ctx, flow, false);
+	struct phy_device *phydev = ctx->phydev;
+	struct vsc8531_private *priv = phydev->priv;
+	struct macsec_flow *flow;
+	int ret;
 
-	flow = vsc8584_macsec_find_flow(ctx, MACSEC_INGR);
+	flow = vsc8584_macsec_alloc_flow(priv, MACSEC_INGR);
 	if (IS_ERR(flow))
 		return PTR_ERR(flow);
 
-	vsc8584_macsec_flow_enable(ctx->phydev, flow);
+	ret = __vsc8584_macsec_add_rxsa(ctx, flow, false);
+	if (ret)
+		return ret;
+
+	vsc8584_macsec_flow_enable(phydev, flow);
 	return 0;
 }
 
 static int vsc8584_macsec_upd_rxsa(struct macsec_context *ctx)
 {
 	struct macsec_flow *flow;
+	int ret;
+
+	if (ctx->sa.update_pn)
+		return -EINVAL;
 
 	flow = vsc8584_macsec_find_flow(ctx, MACSEC_INGR);
 	if (IS_ERR(flow))
 		return PTR_ERR(flow);
 
-	if (ctx->prepare) {
-		/* Make sure the flow is disabled before updating it */
-		vsc8584_macsec_flow_disable(ctx->phydev, flow);
+	/* Make sure the flow is disabled before updating it */
+	vsc8584_macsec_flow_disable(ctx->phydev, flow);
 
-		return __vsc8584_macsec_add_rxsa(ctx, flow, true);
-	}
+	ret = __vsc8584_macsec_add_rxsa(ctx, flow, true);
+	if (ret)
+		return ret;
 
 	vsc8584_macsec_flow_enable(ctx->phydev, flow);
 	return 0;
@@ -917,11 +872,8 @@ static int vsc8584_macsec_del_rxsa(struct macsec_context *ctx)
 	struct macsec_flow *flow;
 
 	flow = vsc8584_macsec_find_flow(ctx, MACSEC_INGR);
-
 	if (IS_ERR(flow))
 		return PTR_ERR(flow);
-	if (ctx->prepare)
-		return 0;
 
 	vsc8584_macsec_del_flow(ctx->phydev, flow);
 	return 0;
@@ -929,33 +881,41 @@ static int vsc8584_macsec_del_rxsa(struct macsec_context *ctx)
 
 static int vsc8584_macsec_add_txsa(struct macsec_context *ctx)
 {
-	struct macsec_flow *flow = NULL;
-
-	if (ctx->prepare)
-		return __vsc8584_macsec_add_txsa(ctx, flow, false);
+	struct phy_device *phydev = ctx->phydev;
+	struct vsc8531_private *priv = phydev->priv;
+	struct macsec_flow *flow;
+	int ret;
 
-	flow = vsc8584_macsec_find_flow(ctx, MACSEC_EGR);
+	flow = vsc8584_macsec_alloc_flow(priv, MACSEC_EGR);
 	if (IS_ERR(flow))
 		return PTR_ERR(flow);
 
-	vsc8584_macsec_flow_enable(ctx->phydev, flow);
+	ret = __vsc8584_macsec_add_txsa(ctx, flow, false);
+	if (ret)
+		return ret;
+
+	vsc8584_macsec_flow_enable(phydev, flow);
 	return 0;
 }
 
 static int vsc8584_macsec_upd_txsa(struct macsec_context *ctx)
 {
 	struct macsec_flow *flow;
+	int ret;
+
+	if (ctx->sa.update_pn)
+		return -EINVAL;
 
 	flow = vsc8584_macsec_find_flow(ctx, MACSEC_EGR);
 	if (IS_ERR(flow))
 		return PTR_ERR(flow);
 
-	if (ctx->prepare) {
-		/* Make sure the flow is disabled before updating it */
-		vsc8584_macsec_flow_disable(ctx->phydev, flow);
+	/* Make sure the flow is disabled before updating it */
+	vsc8584_macsec_flow_disable(ctx->phydev, flow);
 
-		return __vsc8584_macsec_add_txsa(ctx, flow, true);
-	}
+	ret = __vsc8584_macsec_add_txsa(ctx, flow, true);
+	if (ret)
+		return ret;
 
 	vsc8584_macsec_flow_enable(ctx->phydev, flow);
 	return 0;
@@ -966,17 +926,14 @@ static int vsc8584_macsec_del_txsa(struct macsec_context *ctx)
 	struct macsec_flow *flow;
 
 	flow = vsc8584_macsec_find_flow(ctx, MACSEC_EGR);
-
 	if (IS_ERR(flow))
 		return PTR_ERR(flow);
-	if (ctx->prepare)
-		return 0;
 
 	vsc8584_macsec_del_flow(ctx->phydev, flow);
 	return 0;
 }
 
-static struct macsec_ops vsc8584_macsec_ops = {
+static const struct macsec_ops vsc8584_macsec_ops = {
 	.mdo_dev_open = vsc8584_macsec_dev_open,
 	.mdo_dev_stop = vsc8584_macsec_dev_stop,
 	.mdo_add_secy = vsc8584_macsec_add_secy,
@@ -999,7 +956,6 @@ int vsc8584_macsec_init(struct phy_device *phydev)
 
 	switch (phydev->phy_id & phydev->drv->phy_id_mask) {
 	case PHY_ID_VSC856X:
-	case PHY_ID_VSC8575:
 	case PHY_ID_VSC8582:
 	case PHY_ID_VSC8584:
 		INIT_LIST_HEAD(&vsc8531->macsec_flows);