forked from ~ljy/RK356X_SDK_RELEASE

blame | history | patch | commit | commitdiff
hc
2023-12-09 b22da3d8526a935aa31e086e63f60ff3246cb61c 
 kernel/net/ncsi/ncsi-rsp.c
....@@ -1,24 +1,23 @@
1
+// SPDX-License-Identifier: GPL-2.0-or-later
12 /*
23  * Copyright Gavin Shan, IBM Corporation 2016.
3
- *
4
- * This program is free software; you can redistribute it and/or modify
5
- * it under the terms of the GNU General Public License as published by
6
- * the Free Software Foundation; either version 2 of the License, or
7
- * (at your option) any later version.
84  */
95 
106 #include <linux/module.h>
117 #include <linux/kernel.h>
128 #include <linux/init.h>
139 #include <linux/netdevice.h>
10
+#include <linux/etherdevice.h>
1411 #include <linux/skbuff.h>
1512 
1613 #include <net/ncsi.h>
1714 #include <net/net_namespace.h>
1815 #include <net/sock.h>
16
+#include <net/genetlink.h>
1917 
2018 #include "internal.h"
2119 #include "ncsi-pkt.h"
20
+#include "ncsi-netlink.h"
2221 
2322 static int ncsi_validate_rsp_pkt(struct ncsi_request *nr,
2423 				 unsigned short payload)
....@@ -32,28 +31,44 @@
3231 	 * before calling this function.
3332 	 */
3433 	h = (struct ncsi_rsp_pkt_hdr *)skb_network_header(nr->rsp);
35
-	if (h->common.revision != NCSI_PKT_REVISION)
34
+
35
+	if (h->common.revision != NCSI_PKT_REVISION) {
36
+		netdev_dbg(nr->ndp->ndev.dev,
37
+			   "NCSI: unsupported header revision\n");
3638 		return -EINVAL;
37
-	if (ntohs(h->common.length) != payload)
39
+	}
40
+	if (ntohs(h->common.length) != payload) {
41
+		netdev_dbg(nr->ndp->ndev.dev,
42
+			   "NCSI: payload length mismatched\n");
3843 		return -EINVAL;
44
+	}
3945 
4046 	/* Check on code and reason */
4147 	if (ntohs(h->code) != NCSI_PKT_RSP_C_COMPLETED ||
42
-	    ntohs(h->reason) != NCSI_PKT_RSP_R_NO_ERROR)
43
-		return -EINVAL;
48
+	    ntohs(h->reason) != NCSI_PKT_RSP_R_NO_ERROR) {
49
+		netdev_dbg(nr->ndp->ndev.dev,
50
+			   "NCSI: non zero response/reason code %04xh, %04xh\n",
51
+			    ntohs(h->code), ntohs(h->reason));
52
+		return -EPERM;
53
+	}
4454 
4555 	/* Validate checksum, which might be zeroes if the
4656 	 * sender doesn't support checksum according to NCSI
4757 	 * specification.
4858 	 */
49
-	pchecksum = (__be32 *)((void *)(h + 1) + payload - 4);
59
+	pchecksum = (__be32 *)((void *)(h + 1) + ALIGN(payload, 4) - 4);
5060 	if (ntohl(*pchecksum) == 0)
5161 		return 0;
5262 
5363 	checksum = ncsi_calculate_checksum((unsigned char *)h,
5464 					   sizeof(*h) + payload - 4);
55
-	if (*pchecksum != htonl(checksum))
65
+
66
+	if (*pchecksum != htonl(checksum)) {
67
+		netdev_dbg(nr->ndp->ndev.dev,
68
+			   "NCSI: checksum mismatched; recd: %08x calc: %08x\n",
69
+			   *pchecksum, htonl(checksum));
5670 		return -EINVAL;
71
+	}
5772 
5873 	return 0;
5974 }
....@@ -241,7 +256,7 @@
241256 	if (!ncm->enable)
242257 		return 0;
243258 
244
-	ncm->enable = 1;
259
+	ncm->enable = 0;
245260 	return 0;
246261 }
247262 
....@@ -456,7 +471,7 @@
456471 		memcpy(&ncf->addrs[index], cmd->mac, ETH_ALEN);
457472 	} else {
458473 		clear_bit(cmd->index - 1, bitmap);
459
-		memset(&ncf->addrs[index], 0, ETH_ALEN);
474
+		eth_zero_addr(&ncf->addrs[index]);
460475 	}
461476 	spin_unlock_irqrestore(&nc->lock, flags);
462477 
....@@ -594,6 +609,135 @@
594609 	ncm->data[0] = cmd->mode;
595610 
596611 	return 0;
612
+}
613
+
614
+/* Response handler for Mellanox command Get Mac Address */
615
+static int ncsi_rsp_handler_oem_mlx_gma(struct ncsi_request *nr)
616
+{
617
+	struct ncsi_dev_priv *ndp = nr->ndp;
618
+	struct net_device *ndev = ndp->ndev.dev;
619
+	const struct net_device_ops *ops = ndev->netdev_ops;
620
+	struct ncsi_rsp_oem_pkt *rsp;
621
+	struct sockaddr saddr;
622
+	int ret = 0;
623
+
624
+	/* Get the response header */
625
+	rsp = (struct ncsi_rsp_oem_pkt *)skb_network_header(nr->rsp);
626
+
627
+	saddr.sa_family = ndev->type;
628
+	ndev->priv_flags |= IFF_LIVE_ADDR_CHANGE;
629
+	memcpy(saddr.sa_data, &rsp->data[MLX_MAC_ADDR_OFFSET], ETH_ALEN);
630
+	/* Set the flag for GMA command which should only be called once */
631
+	ndp->gma_flag = 1;
632
+
633
+	ret = ops->ndo_set_mac_address(ndev, &saddr);
634
+	if (ret < 0)
635
+		netdev_warn(ndev, "NCSI: 'Writing mac address to device failed\n");
636
+
637
+	return ret;
638
+}
639
+
640
+/* Response handler for Mellanox card */
641
+static int ncsi_rsp_handler_oem_mlx(struct ncsi_request *nr)
642
+{
643
+	struct ncsi_rsp_oem_mlx_pkt *mlx;
644
+	struct ncsi_rsp_oem_pkt *rsp;
645
+
646
+	/* Get the response header */
647
+	rsp = (struct ncsi_rsp_oem_pkt *)skb_network_header(nr->rsp);
648
+	mlx = (struct ncsi_rsp_oem_mlx_pkt *)(rsp->data);
649
+
650
+	if (mlx->cmd == NCSI_OEM_MLX_CMD_GMA &&
651
+	    mlx->param == NCSI_OEM_MLX_CMD_GMA_PARAM)
652
+		return ncsi_rsp_handler_oem_mlx_gma(nr);
653
+	return 0;
654
+}
655
+
656
+/* Response handler for Broadcom command Get Mac Address */
657
+static int ncsi_rsp_handler_oem_bcm_gma(struct ncsi_request *nr)
658
+{
659
+	struct ncsi_dev_priv *ndp = nr->ndp;
660
+	struct net_device *ndev = ndp->ndev.dev;
661
+	const struct net_device_ops *ops = ndev->netdev_ops;
662
+	struct ncsi_rsp_oem_pkt *rsp;
663
+	struct sockaddr saddr;
664
+	int ret = 0;
665
+
666
+	/* Get the response header */
667
+	rsp = (struct ncsi_rsp_oem_pkt *)skb_network_header(nr->rsp);
668
+
669
+	saddr.sa_family = ndev->type;
670
+	ndev->priv_flags |= IFF_LIVE_ADDR_CHANGE;
671
+	memcpy(saddr.sa_data, &rsp->data[BCM_MAC_ADDR_OFFSET], ETH_ALEN);
672
+	/* Increase mac address by 1 for BMC's address */
673
+	eth_addr_inc((u8 *)saddr.sa_data);
674
+	if (!is_valid_ether_addr((const u8 *)saddr.sa_data))
675
+		return -ENXIO;
676
+
677
+	/* Set the flag for GMA command which should only be called once */
678
+	ndp->gma_flag = 1;
679
+
680
+	ret = ops->ndo_set_mac_address(ndev, &saddr);
681
+	if (ret < 0)
682
+		netdev_warn(ndev, "NCSI: 'Writing mac address to device failed\n");
683
+
684
+	return ret;
685
+}
686
+
687
+/* Response handler for Broadcom card */
688
+static int ncsi_rsp_handler_oem_bcm(struct ncsi_request *nr)
689
+{
690
+	struct ncsi_rsp_oem_bcm_pkt *bcm;
691
+	struct ncsi_rsp_oem_pkt *rsp;
692
+
693
+	/* Get the response header */
694
+	rsp = (struct ncsi_rsp_oem_pkt *)skb_network_header(nr->rsp);
695
+	bcm = (struct ncsi_rsp_oem_bcm_pkt *)(rsp->data);
696
+
697
+	if (bcm->type == NCSI_OEM_BCM_CMD_GMA)
698
+		return ncsi_rsp_handler_oem_bcm_gma(nr);
699
+	return 0;
700
+}
701
+
702
+static struct ncsi_rsp_oem_handler {
703
+	unsigned int	mfr_id;
704
+	int		(*handler)(struct ncsi_request *nr);
705
+} ncsi_rsp_oem_handlers[] = {
706
+	{ NCSI_OEM_MFR_MLX_ID, ncsi_rsp_handler_oem_mlx },
707
+	{ NCSI_OEM_MFR_BCM_ID, ncsi_rsp_handler_oem_bcm }
708
+};
709
+
710
+/* Response handler for OEM command */
711
+static int ncsi_rsp_handler_oem(struct ncsi_request *nr)
712
+{
713
+	struct ncsi_rsp_oem_handler *nrh = NULL;
714
+	struct ncsi_rsp_oem_pkt *rsp;
715
+	unsigned int mfr_id, i;
716
+
717
+	/* Get the response header */
718
+	rsp = (struct ncsi_rsp_oem_pkt *)skb_network_header(nr->rsp);
719
+	mfr_id = ntohl(rsp->mfr_id);
720
+
721
+	/* Check for manufacturer id and Find the handler */
722
+	for (i = 0; i < ARRAY_SIZE(ncsi_rsp_oem_handlers); i++) {
723
+		if (ncsi_rsp_oem_handlers[i].mfr_id == mfr_id) {
724
+			if (ncsi_rsp_oem_handlers[i].handler)
725
+				nrh = &ncsi_rsp_oem_handlers[i];
726
+			else
727
+				nrh = NULL;
728
+
729
+			break;
730
+		}
731
+	}
732
+
733
+	if (!nrh) {
734
+		netdev_err(nr->ndp->ndev.dev, "Received unrecognized OEM packet with MFR-ID (0x%x)\n",
735
+			   mfr_id);
736
+		return -ENOENT;
737
+	}
738
+
739
+	/* Process the packet */
740
+	return nrh->handler(nr);
597741 }
598742 
599743 static int ncsi_rsp_handler_gvi(struct ncsi_request *nr)
....@@ -900,6 +1044,31 @@
9001044 	return 0;
9011045 }
9021046 
1047
+static int ncsi_rsp_handler_pldm(struct ncsi_request *nr)
1048
+{
1049
+	return 0;
1050
+}
1051
+
1052
+static int ncsi_rsp_handler_netlink(struct ncsi_request *nr)
1053
+{
1054
+	struct ncsi_dev_priv *ndp = nr->ndp;
1055
+	struct ncsi_rsp_pkt *rsp;
1056
+	struct ncsi_package *np;
1057
+	struct ncsi_channel *nc;
1058
+	int ret;
1059
+
1060
+	/* Find the package */
1061
+	rsp = (struct ncsi_rsp_pkt *)skb_network_header(nr->rsp);
1062
+	ncsi_find_package_and_channel(ndp, rsp->rsp.common.channel,
1063
+				      &np, &nc);
1064
+	if (!np)
1065
+		return -ENODEV;
1066
+
1067
+	ret = ncsi_send_netlink_rsp(nr, np, nc);
1068
+
1069
+	return ret;
1070
+}
1071
+
9031072 static struct ncsi_rsp_handler {
9041073 	unsigned char	type;
9051074 	int             payload;
....@@ -928,13 +1097,15 @@
9281097 	{ NCSI_PKT_RSP_GVI,    40, ncsi_rsp_handler_gvi     },
9291098 	{ NCSI_PKT_RSP_GC,     32, ncsi_rsp_handler_gc      },
9301099 	{ NCSI_PKT_RSP_GP,     -1, ncsi_rsp_handler_gp      },
931
-	{ NCSI_PKT_RSP_GCPS,  172, ncsi_rsp_handler_gcps    },
932
-	{ NCSI_PKT_RSP_GNS,   172, ncsi_rsp_handler_gns     },
933
-	{ NCSI_PKT_RSP_GNPTS, 172, ncsi_rsp_handler_gnpts   },
1100
+	{ NCSI_PKT_RSP_GCPS,  204, ncsi_rsp_handler_gcps    },
1101
+	{ NCSI_PKT_RSP_GNS,    32, ncsi_rsp_handler_gns     },
1102
+	{ NCSI_PKT_RSP_GNPTS,  48, ncsi_rsp_handler_gnpts   },
9341103 	{ NCSI_PKT_RSP_GPS,     8, ncsi_rsp_handler_gps     },
935
-	{ NCSI_PKT_RSP_OEM,     0, NULL                     },
936
-	{ NCSI_PKT_RSP_PLDM,    0, NULL                     },
937
-	{ NCSI_PKT_RSP_GPUUID, 20, ncsi_rsp_handler_gpuuid  }
1104
+	{ NCSI_PKT_RSP_OEM,    -1, ncsi_rsp_handler_oem     },
1105
+	{ NCSI_PKT_RSP_PLDM,   -1, ncsi_rsp_handler_pldm    },
1106
+	{ NCSI_PKT_RSP_GPUUID, 20, ncsi_rsp_handler_gpuuid  },
1107
+	{ NCSI_PKT_RSP_QPNPR,  -1, ncsi_rsp_handler_pldm    },
1108
+	{ NCSI_PKT_RSP_SNPR,   -1, ncsi_rsp_handler_pldm    }
9381109 };
9391110 
9401111 int ncsi_rcv_rsp(struct sk_buff *skb, struct net_device *dev,
....@@ -1002,6 +1173,17 @@
10021173 		netdev_warn(ndp->ndev.dev,
10031174 			    "NCSI: 'bad' packet ignored for type 0x%x\n",
10041175 			    hdr->type);
1176
+
1177
+		if (nr->flags == NCSI_REQ_FLAG_NETLINK_DRIVEN) {
1178
+			if (ret == -EPERM)
1179
+				goto out_netlink;
1180
+			else
1181
+				ncsi_send_netlink_err(ndp->ndev.dev,
1182
+						      nr->snd_seq,
1183
+						      nr->snd_portid,
1184
+						      &nr->nlhdr,
1185
+						      ret);
1186
+		}
10051187 		goto out;
10061188 	}
10071189 
....@@ -1011,6 +1193,17 @@
10111193 		netdev_err(ndp->ndev.dev,
10121194 			   "NCSI: Handler for packet type 0x%x returned %d\n",
10131195 			   hdr->type, ret);
1196
+
1197
+out_netlink:
1198
+	if (nr->flags == NCSI_REQ_FLAG_NETLINK_DRIVEN) {
1199
+		ret = ncsi_rsp_handler_netlink(nr);
1200
+		if (ret) {
1201
+			netdev_err(ndp->ndev.dev,
1202
+				   "NCSI: Netlink handler for packet type 0x%x returned %d\n",
1203
+				   hdr->type, ret);
1204
+		}
1205
+	}
1206
+
10141207 out:
10151208 	ncsi_free_request(nr);
10161209 	return ret;