[PATCH v2] net: macsec SCI assignment for ES = 0

[carlos.fernandez@technica-engineering.de]

From: Carlos Fernandez <carlos.fernandez@technica-engineering.de>

According to 802.1AE standard, when ES and SC flags in TCI are zero, used
SCI should be the current active SC_RX. Current kernel does not implement
it and uses the header MAC address.

Without this patch, when ES = 0 (using a bridge or switch), header MAC
will not fit the SCI and MACSec frames will be discarted.

Signed-off-by: Carlos Fernandez <carlos.fernandez@technica-engineering.de>
---
 drivers/net/macsec.c | 28 ++++++++++++++++++++++------
 1 file changed, 22 insertions(+), 6 deletions(-)

diff --git a/drivers/net/macsec.c b/drivers/net/macsec.c
index 3427993f94f7..ccecb7eb385c 100644
--- a/drivers/net/macsec.c
+++ b/drivers/net/macsec.c
@@ -256,16 +256,31 @@ static sci_t make_sci(const u8 *addr, __be16 port)
 	return sci;
 }
 
-static sci_t macsec_frame_sci(struct macsec_eth_header *hdr, bool sci_present)
+static sci_t macsec_frame_sci(struct macsec_eth_header *hdr, bool sci_present,
+		struct macsec_rxh_data *rxd)
 {
+	struct macsec_dev *macsec_device;
 	sci_t sci;
 
-	if (sci_present)
+	if (sci_present) {
 		memcpy(&sci, hdr->secure_channel_id,
-		       sizeof(hdr->secure_channel_id));
-	else
+			sizeof(hdr->secure_channel_id));
+	} else if (0 == (hdr->tci_an & (MACSEC_TCI_ES | MACSEC_TCI_SC))) {
+		list_for_each_entry_rcu(macsec_device, &rxd->secys, secys) {
+			struct macsec_rx_sc *rx_sc;
+			struct macsec_secy *secy = &macsec_device->secy;
+
+			for_each_rxsc(secy, rx_sc) {
+				rx_sc = rx_sc ? macsec_rxsc_get(rx_sc) : NULL;
+				if (rx_sc && rx_sc->active)
+					return rx_sc->sci;
+			}
+			/* If not found, use MAC in hdr as default*/
+			sci = make_sci(hdr->eth.h_source, MACSEC_PORT_ES);
+		}
+	} else {
 		sci = make_sci(hdr->eth.h_source, MACSEC_PORT_ES);
-
+	}
 	return sci;
 }
 
@@ -1150,11 +1165,12 @@ static rx_handler_result_t macsec_handle_frame(struct sk_buff **pskb)
 
 	macsec_skb_cb(skb)->has_sci = !!(hdr->tci_an & MACSEC_TCI_SC);
 	macsec_skb_cb(skb)->assoc_num = hdr->tci_an & MACSEC_AN_MASK;
-	sci = macsec_frame_sci(hdr, macsec_skb_cb(skb)->has_sci);
 
 	rcu_read_lock();
 	rxd = macsec_data_rcu(skb->dev);
 
+	sci = macsec_frame_sci(hdr, macsec_skb_cb(skb)->has_sci, rxd);
+
 	list_for_each_entry_rcu(macsec, &rxd->secys, secys) {
 		struct macsec_rx_sc *sc = find_rx_sc(&macsec->secy, sci);
 
-- 
2.34.1