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-netlink.c | 483 +++++++++++++++++++++++++++++++++++++++++++++++------ 1 files changed, 428 insertions(+), 55 deletions(-) diff --git a/kernel/net/ncsi/ncsi-netlink.c b/kernel/net/ncsi/ncsi-netlink.c index a2f4280..c189b4c 100644 --- a/kernel/net/ncsi/ncsi-netlink.c +++ b/kernel/net/ncsi/ncsi-netlink.c @@ -1,10 +1,6 @@ +// SPDX-License-Identifier: GPL-2.0-or-later /* * Copyright Samuel Mendoza-Jonas, IBM Corporation 2018. - * - * 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> @@ -12,7 +8,6 @@ #include <linux/if_arp.h> #include <linux/rtnetlink.h> #include <linux/etherdevice.h> -#include <linux/module.h> #include <net/genetlink.h> #include <net/ncsi.h> #include <linux/skbuff.h> @@ -20,6 +15,7 @@ #include <uapi/linux/ncsi.h> #include "internal.h" +#include "ncsi-pkt.h" #include "ncsi-netlink.h" static struct genl_family ncsi_genl_family; @@ -29,6 +25,10 @@ [NCSI_ATTR_PACKAGE_LIST] = { .type = NLA_NESTED }, [NCSI_ATTR_PACKAGE_ID] = { .type = NLA_U32 }, [NCSI_ATTR_CHANNEL_ID] = { .type = NLA_U32 }, + [NCSI_ATTR_DATA] = { .type = NLA_BINARY, .len = 2048 }, + [NCSI_ATTR_MULTI_FLAG] = { .type = NLA_FLAG }, + [NCSI_ATTR_PACKAGE_MASK] = { .type = NLA_U32 }, + [NCSI_ATTR_CHANNEL_MASK] = { .type = NLA_U32 }, }; static struct ncsi_dev_priv *ndp_from_ifindex(struct net *net, u32 ifindex) @@ -68,14 +68,14 @@ nla_put_u32(skb, NCSI_CHANNEL_ATTR_LINK_STATE, m->data[2]); if (nc->state == NCSI_CHANNEL_ACTIVE) nla_put_flag(skb, NCSI_CHANNEL_ATTR_ACTIVE); - if (ndp->force_channel == nc) + if (nc == nc->package->preferred_channel) nla_put_flag(skb, NCSI_CHANNEL_ATTR_FORCED); nla_put_u32(skb, NCSI_CHANNEL_ATTR_VERSION_MAJOR, nc->version.version); nla_put_u32(skb, NCSI_CHANNEL_ATTR_VERSION_MINOR, nc->version.alpha2); nla_put_string(skb, NCSI_CHANNEL_ATTR_VERSION_STR, nc->version.fw_name); - vid_nest = nla_nest_start(skb, NCSI_CHANNEL_ATTR_VLAN_LIST); + vid_nest = nla_nest_start_noflag(skb, NCSI_CHANNEL_ATTR_VLAN_LIST); if (!vid_nest) return -ENOMEM; ncf = &nc->vlan_filter; @@ -109,19 +109,23 @@ NCSI_FOR_EACH_PACKAGE(ndp, np) { if (np->id != id) continue; - pnest = nla_nest_start(skb, NCSI_PKG_ATTR); + pnest = nla_nest_start_noflag(skb, NCSI_PKG_ATTR); if (!pnest) return -ENOMEM; - nla_put_u32(skb, NCSI_PKG_ATTR_ID, np->id); - if (ndp->force_package == np) + rc = nla_put_u32(skb, NCSI_PKG_ATTR_ID, np->id); + if (rc) { + nla_nest_cancel(skb, pnest); + return rc; + } + if ((0x1 << np->id) == ndp->package_whitelist) nla_put_flag(skb, NCSI_PKG_ATTR_FORCED); - cnest = nla_nest_start(skb, NCSI_PKG_ATTR_CHANNEL_LIST); + cnest = nla_nest_start_noflag(skb, NCSI_PKG_ATTR_CHANNEL_LIST); if (!cnest) { nla_nest_cancel(skb, pnest); return -ENOMEM; } NCSI_FOR_EACH_CHANNEL(np, nc) { - nest = nla_nest_start(skb, NCSI_CHANNEL_ATTR); + nest = nla_nest_start_noflag(skb, NCSI_CHANNEL_ATTR); if (!nest) { nla_nest_cancel(skb, cnest); nla_nest_cancel(skb, pnest); @@ -183,7 +187,7 @@ package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]); - attr = nla_nest_start(skb, NCSI_ATTR_PACKAGE_LIST); + attr = nla_nest_start_noflag(skb, NCSI_ATTR_PACKAGE_LIST); if (!attr) { kfree_skb(skb); return -EMSGSIZE; @@ -216,8 +220,8 @@ void *hdr; int rc; - rc = genlmsg_parse(cb->nlh, &ncsi_genl_family, attrs, NCSI_ATTR_MAX, - ncsi_genl_policy, NULL); + rc = genlmsg_parse_deprecated(cb->nlh, &ncsi_genl_family, attrs, NCSI_ATTR_MAX, + ncsi_genl_policy, NULL); if (rc) return rc; @@ -246,7 +250,11 @@ goto err; } - attr = nla_nest_start(skb, NCSI_ATTR_PACKAGE_LIST); + attr = nla_nest_start_noflag(skb, NCSI_ATTR_PACKAGE_LIST); + if (!attr) { + rc = -EMSGSIZE; + goto err; + } rc = ncsi_write_package_info(skb, ndp, package->id); if (rc) { nla_nest_cancel(skb, attr); @@ -289,49 +297,58 @@ package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]); package = NULL; - spin_lock_irqsave(&ndp->lock, flags); - NCSI_FOR_EACH_PACKAGE(ndp, np) if (np->id == package_id) package = np; if (!package) { /* The user has set a package that does not exist */ - spin_unlock_irqrestore(&ndp->lock, flags); return -ERANGE; } channel = NULL; - if (!info->attrs[NCSI_ATTR_CHANNEL_ID]) { - /* Allow any channel */ - channel_id = NCSI_RESERVED_CHANNEL; - } else { + if (info->attrs[NCSI_ATTR_CHANNEL_ID]) { channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]); NCSI_FOR_EACH_CHANNEL(package, nc) - if (nc->id == channel_id) + if (nc->id == channel_id) { channel = nc; + break; + } + if (!channel) { + netdev_info(ndp->ndev.dev, + "NCSI: Channel %u does not exist!\n", + channel_id); + return -ERANGE; + } } - if (channel_id != NCSI_RESERVED_CHANNEL && !channel) { - /* The user has set a channel that does not exist on this - * package - */ - spin_unlock_irqrestore(&ndp->lock, flags); - netdev_info(ndp->ndev.dev, "NCSI: Channel %u does not exist!\n", - channel_id); - return -ERANGE; - } - - ndp->force_package = package; - ndp->force_channel = channel; + spin_lock_irqsave(&ndp->lock, flags); + ndp->package_whitelist = 0x1 << package->id; + ndp->multi_package = false; spin_unlock_irqrestore(&ndp->lock, flags); - netdev_info(ndp->ndev.dev, "Set package 0x%x, channel 0x%x%s as preferred\n", - package_id, channel_id, - channel_id == NCSI_RESERVED_CHANNEL ? " (any)" : ""); + spin_lock_irqsave(&package->lock, flags); + package->multi_channel = false; + if (channel) { + package->channel_whitelist = 0x1 << channel->id; + package->preferred_channel = channel; + } else { + /* Allow any channel */ + package->channel_whitelist = UINT_MAX; + package->preferred_channel = NULL; + } + spin_unlock_irqrestore(&package->lock, flags); - /* Bounce the NCSI channel to set changes */ - ncsi_stop_dev(&ndp->ndev); - ncsi_start_dev(&ndp->ndev); + if (channel) + netdev_info(ndp->ndev.dev, + "Set package 0x%x, channel 0x%x as preferred\n", + package_id, channel_id); + else + netdev_info(ndp->ndev.dev, "Set package 0x%x as preferred\n", + package_id); + + /* Update channel configuration */ + if (!(ndp->flags & NCSI_DEV_RESET)) + ncsi_reset_dev(&ndp->ndev); return 0; } @@ -339,6 +356,7 @@ static int ncsi_clear_interface_nl(struct sk_buff *msg, struct genl_info *info) { struct ncsi_dev_priv *ndp; + struct ncsi_package *np; unsigned long flags; if (!info || !info->attrs) @@ -352,38 +370,392 @@ if (!ndp) return -ENODEV; - /* Clear any override */ + /* Reset any whitelists and disable multi mode */ spin_lock_irqsave(&ndp->lock, flags); - ndp->force_package = NULL; - ndp->force_channel = NULL; + ndp->package_whitelist = UINT_MAX; + ndp->multi_package = false; spin_unlock_irqrestore(&ndp->lock, flags); + + NCSI_FOR_EACH_PACKAGE(ndp, np) { + spin_lock_irqsave(&np->lock, flags); + np->multi_channel = false; + np->channel_whitelist = UINT_MAX; + np->preferred_channel = NULL; + spin_unlock_irqrestore(&np->lock, flags); + } netdev_info(ndp->ndev.dev, "NCSI: Cleared preferred package/channel\n"); - /* Bounce the NCSI channel to set changes */ - ncsi_stop_dev(&ndp->ndev); - ncsi_start_dev(&ndp->ndev); + /* Update channel configuration */ + if (!(ndp->flags & NCSI_DEV_RESET)) + ncsi_reset_dev(&ndp->ndev); return 0; } -static const struct genl_ops ncsi_ops[] = { +static int ncsi_send_cmd_nl(struct sk_buff *msg, struct genl_info *info) +{ + struct ncsi_dev_priv *ndp; + struct ncsi_pkt_hdr *hdr; + struct ncsi_cmd_arg nca; + unsigned char *data; + u32 package_id; + u32 channel_id; + int len, ret; + + if (!info || !info->attrs) { + ret = -EINVAL; + goto out; + } + + if (!info->attrs[NCSI_ATTR_IFINDEX]) { + ret = -EINVAL; + goto out; + } + + if (!info->attrs[NCSI_ATTR_PACKAGE_ID]) { + ret = -EINVAL; + goto out; + } + + if (!info->attrs[NCSI_ATTR_CHANNEL_ID]) { + ret = -EINVAL; + goto out; + } + + if (!info->attrs[NCSI_ATTR_DATA]) { + ret = -EINVAL; + goto out; + } + + ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)), + nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX])); + if (!ndp) { + ret = -ENODEV; + goto out; + } + + package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]); + channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]); + + if (package_id >= NCSI_MAX_PACKAGE || channel_id >= NCSI_MAX_CHANNEL) { + ret = -ERANGE; + goto out_netlink; + } + + len = nla_len(info->attrs[NCSI_ATTR_DATA]); + if (len < sizeof(struct ncsi_pkt_hdr)) { + netdev_info(ndp->ndev.dev, "NCSI: no command to send %u\n", + package_id); + ret = -EINVAL; + goto out_netlink; + } else { + data = (unsigned char *)nla_data(info->attrs[NCSI_ATTR_DATA]); + } + + hdr = (struct ncsi_pkt_hdr *)data; + + nca.ndp = ndp; + nca.package = (unsigned char)package_id; + nca.channel = (unsigned char)channel_id; + nca.type = hdr->type; + nca.req_flags = NCSI_REQ_FLAG_NETLINK_DRIVEN; + nca.info = info; + nca.payload = ntohs(hdr->length); + nca.data = data + sizeof(*hdr); + + ret = ncsi_xmit_cmd(&nca); +out_netlink: + if (ret != 0) { + netdev_err(ndp->ndev.dev, + "NCSI: Error %d sending command\n", + ret); + ncsi_send_netlink_err(ndp->ndev.dev, + info->snd_seq, + info->snd_portid, + info->nlhdr, + ret); + } +out: + return ret; +} + +int ncsi_send_netlink_rsp(struct ncsi_request *nr, + struct ncsi_package *np, + struct ncsi_channel *nc) +{ + struct sk_buff *skb; + struct net *net; + void *hdr; + int rc; + + net = dev_net(nr->rsp->dev); + + skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC); + if (!skb) + return -ENOMEM; + + hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq, + &ncsi_genl_family, 0, NCSI_CMD_SEND_CMD); + if (!hdr) { + kfree_skb(skb); + return -EMSGSIZE; + } + + nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->rsp->dev->ifindex); + if (np) + nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id); + if (nc) + nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id); + else + nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL); + + rc = nla_put(skb, NCSI_ATTR_DATA, nr->rsp->len, (void *)nr->rsp->data); + if (rc) + goto err; + + genlmsg_end(skb, hdr); + return genlmsg_unicast(net, skb, nr->snd_portid); + +err: + kfree_skb(skb); + return rc; +} + +int ncsi_send_netlink_timeout(struct ncsi_request *nr, + struct ncsi_package *np, + struct ncsi_channel *nc) +{ + struct sk_buff *skb; + struct net *net; + void *hdr; + + skb = genlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC); + if (!skb) + return -ENOMEM; + + hdr = genlmsg_put(skb, nr->snd_portid, nr->snd_seq, + &ncsi_genl_family, 0, NCSI_CMD_SEND_CMD); + if (!hdr) { + kfree_skb(skb); + return -EMSGSIZE; + } + + net = dev_net(nr->cmd->dev); + + nla_put_u32(skb, NCSI_ATTR_IFINDEX, nr->cmd->dev->ifindex); + + if (np) + nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, np->id); + else + nla_put_u32(skb, NCSI_ATTR_PACKAGE_ID, + NCSI_PACKAGE_INDEX((((struct ncsi_pkt_hdr *) + nr->cmd->data)->channel))); + + if (nc) + nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, nc->id); + else + nla_put_u32(skb, NCSI_ATTR_CHANNEL_ID, NCSI_RESERVED_CHANNEL); + + genlmsg_end(skb, hdr); + return genlmsg_unicast(net, skb, nr->snd_portid); +} + +int ncsi_send_netlink_err(struct net_device *dev, + u32 snd_seq, + u32 snd_portid, + struct nlmsghdr *nlhdr, + int err) +{ + struct nlmsghdr *nlh; + struct nlmsgerr *nle; + struct sk_buff *skb; + struct net *net; + + skb = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC); + if (!skb) + return -ENOMEM; + + net = dev_net(dev); + + nlh = nlmsg_put(skb, snd_portid, snd_seq, + NLMSG_ERROR, sizeof(*nle), 0); + nle = (struct nlmsgerr *)nlmsg_data(nlh); + nle->error = err; + memcpy(&nle->msg, nlhdr, sizeof(*nlh)); + + nlmsg_end(skb, nlh); + + return nlmsg_unicast(net->genl_sock, skb, snd_portid); +} + +static int ncsi_set_package_mask_nl(struct sk_buff *msg, + struct genl_info *info) +{ + struct ncsi_dev_priv *ndp; + unsigned long flags; + int rc; + + if (!info || !info->attrs) + return -EINVAL; + + if (!info->attrs[NCSI_ATTR_IFINDEX]) + return -EINVAL; + + if (!info->attrs[NCSI_ATTR_PACKAGE_MASK]) + return -EINVAL; + + ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)), + nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX])); + if (!ndp) + return -ENODEV; + + spin_lock_irqsave(&ndp->lock, flags); + if (nla_get_flag(info->attrs[NCSI_ATTR_MULTI_FLAG])) { + if (ndp->flags & NCSI_DEV_HWA) { + ndp->multi_package = true; + rc = 0; + } else { + netdev_err(ndp->ndev.dev, + "NCSI: Can't use multiple packages without HWA\n"); + rc = -EPERM; + } + } else { + ndp->multi_package = false; + rc = 0; + } + + if (!rc) + ndp->package_whitelist = + nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_MASK]); + spin_unlock_irqrestore(&ndp->lock, flags); + + if (!rc) { + /* Update channel configuration */ + if (!(ndp->flags & NCSI_DEV_RESET)) + ncsi_reset_dev(&ndp->ndev); + } + + return rc; +} + +static int ncsi_set_channel_mask_nl(struct sk_buff *msg, + struct genl_info *info) +{ + struct ncsi_package *np, *package; + struct ncsi_channel *nc, *channel; + u32 package_id, channel_id; + struct ncsi_dev_priv *ndp; + unsigned long flags; + + if (!info || !info->attrs) + return -EINVAL; + + if (!info->attrs[NCSI_ATTR_IFINDEX]) + return -EINVAL; + + if (!info->attrs[NCSI_ATTR_PACKAGE_ID]) + return -EINVAL; + + if (!info->attrs[NCSI_ATTR_CHANNEL_MASK]) + return -EINVAL; + + ndp = ndp_from_ifindex(get_net(sock_net(msg->sk)), + nla_get_u32(info->attrs[NCSI_ATTR_IFINDEX])); + if (!ndp) + return -ENODEV; + + package_id = nla_get_u32(info->attrs[NCSI_ATTR_PACKAGE_ID]); + package = NULL; + NCSI_FOR_EACH_PACKAGE(ndp, np) + if (np->id == package_id) { + package = np; + break; + } + if (!package) + return -ERANGE; + + spin_lock_irqsave(&package->lock, flags); + + channel = NULL; + if (info->attrs[NCSI_ATTR_CHANNEL_ID]) { + channel_id = nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_ID]); + NCSI_FOR_EACH_CHANNEL(np, nc) + if (nc->id == channel_id) { + channel = nc; + break; + } + if (!channel) { + spin_unlock_irqrestore(&package->lock, flags); + return -ERANGE; + } + netdev_dbg(ndp->ndev.dev, + "NCSI: Channel %u set as preferred channel\n", + channel->id); + } + + package->channel_whitelist = + nla_get_u32(info->attrs[NCSI_ATTR_CHANNEL_MASK]); + if (package->channel_whitelist == 0) + netdev_dbg(ndp->ndev.dev, + "NCSI: Package %u set to all channels disabled\n", + package->id); + + package->preferred_channel = channel; + + if (nla_get_flag(info->attrs[NCSI_ATTR_MULTI_FLAG])) { + package->multi_channel = true; + netdev_info(ndp->ndev.dev, + "NCSI: Multi-channel enabled on package %u\n", + package_id); + } else { + package->multi_channel = false; + } + + spin_unlock_irqrestore(&package->lock, flags); + + /* Update channel configuration */ + if (!(ndp->flags & NCSI_DEV_RESET)) + ncsi_reset_dev(&ndp->ndev); + + return 0; +} + +static const struct genl_small_ops ncsi_ops[] = { { .cmd = NCSI_CMD_PKG_INFO, - .policy = ncsi_genl_policy, + .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, .doit = ncsi_pkg_info_nl, .dumpit = ncsi_pkg_info_all_nl, .flags = 0, }, { .cmd = NCSI_CMD_SET_INTERFACE, - .policy = ncsi_genl_policy, + .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, .doit = ncsi_set_interface_nl, .flags = GENL_ADMIN_PERM, }, { .cmd = NCSI_CMD_CLEAR_INTERFACE, - .policy = ncsi_genl_policy, + .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, .doit = ncsi_clear_interface_nl, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = NCSI_CMD_SEND_CMD, + .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, + .doit = ncsi_send_cmd_nl, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = NCSI_CMD_SET_PACKAGE_MASK, + .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, + .doit = ncsi_set_package_mask_nl, + .flags = GENL_ADMIN_PERM, + }, + { + .cmd = NCSI_CMD_SET_CHANNEL_MASK, + .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, + .doit = ncsi_set_channel_mask_nl, .flags = GENL_ADMIN_PERM, }, }; @@ -392,9 +764,10 @@ .name = "NCSI", .version = 0, .maxattr = NCSI_ATTR_MAX, + .policy = ncsi_genl_policy, .module = THIS_MODULE, - .ops = ncsi_ops, - .n_ops = ARRAY_SIZE(ncsi_ops), + .small_ops = ncsi_ops, + .n_small_ops = ARRAY_SIZE(ncsi_ops), }; static int __init ncsi_init_netlink(void) -- Gitblit v1.6.2