From 8ac6c7a54ed1b98d142dce24b11c6de6a1e239a5 Mon Sep 17 00:00:00 2001
From: hc <hc@nodka.com>
Date: Tue, 22 Oct 2024 10:36:11 +0000
Subject: [PATCH] 修改4g拨号为QMI,需要在系统里后台执行quectel-CM
---
kernel/net/ncsi/ncsi-rsp.c | 231 +++++++++++++++++++++++++++++++++++++++++++++++++++++----
1 files changed, 212 insertions(+), 19 deletions(-)
diff --git a/kernel/net/ncsi/ncsi-rsp.c b/kernel/net/ncsi/ncsi-rsp.c
index a43c9a4..888ccc2 100644
--- a/kernel/net/ncsi/ncsi-rsp.c
+++ b/kernel/net/ncsi/ncsi-rsp.c
@@ -1,24 +1,23 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
/*
* Copyright Gavin Shan, IBM Corporation 2016.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
*/
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
#include <linux/skbuff.h>
#include <net/ncsi.h>
#include <net/net_namespace.h>
#include <net/sock.h>
+#include <net/genetlink.h>
#include "internal.h"
#include "ncsi-pkt.h"
+#include "ncsi-netlink.h"
static int ncsi_validate_rsp_pkt(struct ncsi_request *nr,
unsigned short payload)
@@ -32,28 +31,44 @@
* before calling this function.
*/
h = (struct ncsi_rsp_pkt_hdr *)skb_network_header(nr->rsp);
- if (h->common.revision != NCSI_PKT_REVISION)
+
+ if (h->common.revision != NCSI_PKT_REVISION) {
+ netdev_dbg(nr->ndp->ndev.dev,
+ "NCSI: unsupported header revision\n");
return -EINVAL;
- if (ntohs(h->common.length) != payload)
+ }
+ if (ntohs(h->common.length) != payload) {
+ netdev_dbg(nr->ndp->ndev.dev,
+ "NCSI: payload length mismatched\n");
return -EINVAL;
+ }
/* Check on code and reason */
if (ntohs(h->code) != NCSI_PKT_RSP_C_COMPLETED ||
- ntohs(h->reason) != NCSI_PKT_RSP_R_NO_ERROR)
- return -EINVAL;
+ ntohs(h->reason) != NCSI_PKT_RSP_R_NO_ERROR) {
+ netdev_dbg(nr->ndp->ndev.dev,
+ "NCSI: non zero response/reason code %04xh, %04xh\n",
+ ntohs(h->code), ntohs(h->reason));
+ return -EPERM;
+ }
/* Validate checksum, which might be zeroes if the
* sender doesn't support checksum according to NCSI
* specification.
*/
- pchecksum = (__be32 *)((void *)(h + 1) + payload - 4);
+ pchecksum = (__be32 *)((void *)(h + 1) + ALIGN(payload, 4) - 4);
if (ntohl(*pchecksum) == 0)
return 0;
checksum = ncsi_calculate_checksum((unsigned char *)h,
sizeof(*h) + payload - 4);
- if (*pchecksum != htonl(checksum))
+
+ if (*pchecksum != htonl(checksum)) {
+ netdev_dbg(nr->ndp->ndev.dev,
+ "NCSI: checksum mismatched; recd: %08x calc: %08x\n",
+ *pchecksum, htonl(checksum));
return -EINVAL;
+ }
return 0;
}
@@ -241,7 +256,7 @@
if (!ncm->enable)
return 0;
- ncm->enable = 1;
+ ncm->enable = 0;
return 0;
}
@@ -456,7 +471,7 @@
memcpy(&ncf->addrs[index], cmd->mac, ETH_ALEN);
} else {
clear_bit(cmd->index - 1, bitmap);
- memset(&ncf->addrs[index], 0, ETH_ALEN);
+ eth_zero_addr(&ncf->addrs[index]);
}
spin_unlock_irqrestore(&nc->lock, flags);
@@ -594,6 +609,135 @@
ncm->data[0] = cmd->mode;
return 0;
+}
+
+/* Response handler for Mellanox command Get Mac Address */
+static int ncsi_rsp_handler_oem_mlx_gma(struct ncsi_request *nr)
+{
+ struct ncsi_dev_priv *ndp = nr->ndp;
+ struct net_device *ndev = ndp->ndev.dev;
+ const struct net_device_ops *ops = ndev->netdev_ops;
+ struct ncsi_rsp_oem_pkt *rsp;
+ struct sockaddr saddr;
+ int ret = 0;
+
+ /* Get the response header */
+ rsp = (struct ncsi_rsp_oem_pkt *)skb_network_header(nr->rsp);
+
+ saddr.sa_family = ndev->type;
+ ndev->priv_flags |= IFF_LIVE_ADDR_CHANGE;
+ memcpy(saddr.sa_data, &rsp->data[MLX_MAC_ADDR_OFFSET], ETH_ALEN);
+ /* Set the flag for GMA command which should only be called once */
+ ndp->gma_flag = 1;
+
+ ret = ops->ndo_set_mac_address(ndev, &saddr);
+ if (ret < 0)
+ netdev_warn(ndev, "NCSI: 'Writing mac address to device failed\n");
+
+ return ret;
+}
+
+/* Response handler for Mellanox card */
+static int ncsi_rsp_handler_oem_mlx(struct ncsi_request *nr)
+{
+ struct ncsi_rsp_oem_mlx_pkt *mlx;
+ struct ncsi_rsp_oem_pkt *rsp;
+
+ /* Get the response header */
+ rsp = (struct ncsi_rsp_oem_pkt *)skb_network_header(nr->rsp);
+ mlx = (struct ncsi_rsp_oem_mlx_pkt *)(rsp->data);
+
+ if (mlx->cmd == NCSI_OEM_MLX_CMD_GMA &&
+ mlx->param == NCSI_OEM_MLX_CMD_GMA_PARAM)
+ return ncsi_rsp_handler_oem_mlx_gma(nr);
+ return 0;
+}
+
+/* Response handler for Broadcom command Get Mac Address */
+static int ncsi_rsp_handler_oem_bcm_gma(struct ncsi_request *nr)
+{
+ struct ncsi_dev_priv *ndp = nr->ndp;
+ struct net_device *ndev = ndp->ndev.dev;
+ const struct net_device_ops *ops = ndev->netdev_ops;
+ struct ncsi_rsp_oem_pkt *rsp;
+ struct sockaddr saddr;
+ int ret = 0;
+
+ /* Get the response header */
+ rsp = (struct ncsi_rsp_oem_pkt *)skb_network_header(nr->rsp);
+
+ saddr.sa_family = ndev->type;
+ ndev->priv_flags |= IFF_LIVE_ADDR_CHANGE;
+ memcpy(saddr.sa_data, &rsp->data[BCM_MAC_ADDR_OFFSET], ETH_ALEN);
+ /* Increase mac address by 1 for BMC's address */
+ eth_addr_inc((u8 *)saddr.sa_data);
+ if (!is_valid_ether_addr((const u8 *)saddr.sa_data))
+ return -ENXIO;
+
+ /* Set the flag for GMA command which should only be called once */
+ ndp->gma_flag = 1;
+
+ ret = ops->ndo_set_mac_address(ndev, &saddr);
+ if (ret < 0)
+ netdev_warn(ndev, "NCSI: 'Writing mac address to device failed\n");
+
+ return ret;
+}
+
+/* Response handler for Broadcom card */
+static int ncsi_rsp_handler_oem_bcm(struct ncsi_request *nr)
+{
+ struct ncsi_rsp_oem_bcm_pkt *bcm;
+ struct ncsi_rsp_oem_pkt *rsp;
+
+ /* Get the response header */
+ rsp = (struct ncsi_rsp_oem_pkt *)skb_network_header(nr->rsp);
+ bcm = (struct ncsi_rsp_oem_bcm_pkt *)(rsp->data);
+
+ if (bcm->type == NCSI_OEM_BCM_CMD_GMA)
+ return ncsi_rsp_handler_oem_bcm_gma(nr);
+ return 0;
+}
+
+static struct ncsi_rsp_oem_handler {
+ unsigned int mfr_id;
+ int (*handler)(struct ncsi_request *nr);
+} ncsi_rsp_oem_handlers[] = {
+ { NCSI_OEM_MFR_MLX_ID, ncsi_rsp_handler_oem_mlx },
+ { NCSI_OEM_MFR_BCM_ID, ncsi_rsp_handler_oem_bcm }
+};
+
+/* Response handler for OEM command */
+static int ncsi_rsp_handler_oem(struct ncsi_request *nr)
+{
+ struct ncsi_rsp_oem_handler *nrh = NULL;
+ struct ncsi_rsp_oem_pkt *rsp;
+ unsigned int mfr_id, i;
+
+ /* Get the response header */
+ rsp = (struct ncsi_rsp_oem_pkt *)skb_network_header(nr->rsp);
+ mfr_id = ntohl(rsp->mfr_id);
+
+ /* Check for manufacturer id and Find the handler */
+ for (i = 0; i < ARRAY_SIZE(ncsi_rsp_oem_handlers); i++) {
+ if (ncsi_rsp_oem_handlers[i].mfr_id == mfr_id) {
+ if (ncsi_rsp_oem_handlers[i].handler)
+ nrh = &ncsi_rsp_oem_handlers[i];
+ else
+ nrh = NULL;
+
+ break;
+ }
+ }
+
+ if (!nrh) {
+ netdev_err(nr->ndp->ndev.dev, "Received unrecognized OEM packet with MFR-ID (0x%x)\n",
+ mfr_id);
+ return -ENOENT;
+ }
+
+ /* Process the packet */
+ return nrh->handler(nr);
}
static int ncsi_rsp_handler_gvi(struct ncsi_request *nr)
@@ -900,6 +1044,31 @@
return 0;
}
+static int ncsi_rsp_handler_pldm(struct ncsi_request *nr)
+{
+ return 0;
+}
+
+static int ncsi_rsp_handler_netlink(struct ncsi_request *nr)
+{
+ struct ncsi_dev_priv *ndp = nr->ndp;
+ struct ncsi_rsp_pkt *rsp;
+ struct ncsi_package *np;
+ struct ncsi_channel *nc;
+ int ret;
+
+ /* Find the package */
+ rsp = (struct ncsi_rsp_pkt *)skb_network_header(nr->rsp);
+ ncsi_find_package_and_channel(ndp, rsp->rsp.common.channel,
+ &np, &nc);
+ if (!np)
+ return -ENODEV;
+
+ ret = ncsi_send_netlink_rsp(nr, np, nc);
+
+ return ret;
+}
+
static struct ncsi_rsp_handler {
unsigned char type;
int payload;
@@ -928,13 +1097,15 @@
{ NCSI_PKT_RSP_GVI, 40, ncsi_rsp_handler_gvi },
{ NCSI_PKT_RSP_GC, 32, ncsi_rsp_handler_gc },
{ NCSI_PKT_RSP_GP, -1, ncsi_rsp_handler_gp },
- { NCSI_PKT_RSP_GCPS, 172, ncsi_rsp_handler_gcps },
- { NCSI_PKT_RSP_GNS, 172, ncsi_rsp_handler_gns },
- { NCSI_PKT_RSP_GNPTS, 172, ncsi_rsp_handler_gnpts },
+ { NCSI_PKT_RSP_GCPS, 204, ncsi_rsp_handler_gcps },
+ { NCSI_PKT_RSP_GNS, 32, ncsi_rsp_handler_gns },
+ { NCSI_PKT_RSP_GNPTS, 48, ncsi_rsp_handler_gnpts },
{ NCSI_PKT_RSP_GPS, 8, ncsi_rsp_handler_gps },
- { NCSI_PKT_RSP_OEM, 0, NULL },
- { NCSI_PKT_RSP_PLDM, 0, NULL },
- { NCSI_PKT_RSP_GPUUID, 20, ncsi_rsp_handler_gpuuid }
+ { NCSI_PKT_RSP_OEM, -1, ncsi_rsp_handler_oem },
+ { NCSI_PKT_RSP_PLDM, -1, ncsi_rsp_handler_pldm },
+ { NCSI_PKT_RSP_GPUUID, 20, ncsi_rsp_handler_gpuuid },
+ { NCSI_PKT_RSP_QPNPR, -1, ncsi_rsp_handler_pldm },
+ { NCSI_PKT_RSP_SNPR, -1, ncsi_rsp_handler_pldm }
};
int ncsi_rcv_rsp(struct sk_buff *skb, struct net_device *dev,
@@ -1002,6 +1173,17 @@
netdev_warn(ndp->ndev.dev,
"NCSI: 'bad' packet ignored for type 0x%x\n",
hdr->type);
+
+ if (nr->flags == NCSI_REQ_FLAG_NETLINK_DRIVEN) {
+ if (ret == -EPERM)
+ goto out_netlink;
+ else
+ ncsi_send_netlink_err(ndp->ndev.dev,
+ nr->snd_seq,
+ nr->snd_portid,
+ &nr->nlhdr,
+ ret);
+ }
goto out;
}
@@ -1011,6 +1193,17 @@
netdev_err(ndp->ndev.dev,
"NCSI: Handler for packet type 0x%x returned %d\n",
hdr->type, ret);
+
+out_netlink:
+ if (nr->flags == NCSI_REQ_FLAG_NETLINK_DRIVEN) {
+ ret = ncsi_rsp_handler_netlink(nr);
+ if (ret) {
+ netdev_err(ndp->ndev.dev,
+ "NCSI: Netlink handler for packet type 0x%x returned %d\n",
+ hdr->type, ret);
+ }
+ }
+
out:
ncsi_free_request(nr);
return ret;
--
Gitblit v1.6.2