1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* Copyright (c) 2012 GCT Semiconductor, Inc. All rights reserved. */ |
3 | |
4 | #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt |
5 | |
6 | #include <linux/export.h> |
7 | #include <linux/mutex.h> |
8 | #include <linux/etherdevice.h> |
9 | #include <linux/netlink.h> |
10 | #include <asm/byteorder.h> |
11 | #include <net/sock.h> |
12 | |
13 | #include "netlink_k.h" |
14 | |
15 | static DEFINE_MUTEX(netlink_mutex); |
16 | |
17 | #define ND_MAX_GROUP 30 |
18 | #define ND_IFINDEX_LEN sizeof(int) |
19 | #define ND_NLMSG_SPACE(len) (NLMSG_SPACE(len) + ND_IFINDEX_LEN) |
20 | #define ND_NLMSG_DATA(nlh) ((void *)((char *)NLMSG_DATA(nlh) + \ |
21 | ND_IFINDEX_LEN)) |
22 | #define ND_NLMSG_S_LEN(len) ((len) + ND_IFINDEX_LEN) |
23 | #define ND_NLMSG_R_LEN(nlh) ((nlh)->nlmsg_len - ND_IFINDEX_LEN) |
24 | #define ND_NLMSG_IFIDX(nlh) NLMSG_DATA(nlh) |
25 | #define ND_MAX_MSG_LEN (1024 * 32) |
26 | |
27 | static void (*rcv_cb)(struct net_device *dev, u16 type, void *msg, int len); |
28 | |
29 | static void netlink_rcv_cb(struct sk_buff *skb) |
30 | { |
31 | struct nlmsghdr *nlh; |
32 | struct net_device *dev; |
33 | u32 mlen; |
34 | void *msg; |
35 | int ifindex; |
36 | |
37 | if (!rcv_cb) { |
38 | pr_err("nl cb - unregistered\n" ); |
39 | return; |
40 | } |
41 | |
42 | if (skb->len < NLMSG_HDRLEN) { |
43 | pr_err("nl cb - invalid skb length\n" ); |
44 | return; |
45 | } |
46 | |
47 | nlh = (struct nlmsghdr *)skb->data; |
48 | |
49 | if (skb->len < nlh->nlmsg_len || nlh->nlmsg_len > ND_MAX_MSG_LEN) { |
50 | pr_err("nl cb - invalid length (%d,%d)\n" , |
51 | skb->len, nlh->nlmsg_len); |
52 | return; |
53 | } |
54 | |
55 | memcpy(&ifindex, ND_NLMSG_IFIDX(nlh), ND_IFINDEX_LEN); |
56 | msg = ND_NLMSG_DATA(nlh); |
57 | mlen = ND_NLMSG_R_LEN(nlh); |
58 | |
59 | dev = dev_get_by_index(net: &init_net, ifindex); |
60 | if (dev) { |
61 | rcv_cb(dev, nlh->nlmsg_type, msg, mlen); |
62 | dev_put(dev); |
63 | } else { |
64 | pr_err("nl cb - dev (%d) not found\n" , ifindex); |
65 | } |
66 | } |
67 | |
68 | static void netlink_rcv(struct sk_buff *skb) |
69 | { |
70 | mutex_lock(&netlink_mutex); |
71 | netlink_rcv_cb(skb); |
72 | mutex_unlock(lock: &netlink_mutex); |
73 | } |
74 | |
75 | struct sock *netlink_init(int unit, |
76 | void (*cb)(struct net_device *dev, u16 type, |
77 | void *msg, int len)) |
78 | { |
79 | struct sock *sock; |
80 | struct netlink_kernel_cfg cfg = { |
81 | .input = netlink_rcv, |
82 | }; |
83 | |
84 | sock = netlink_kernel_create(net: &init_net, unit, cfg: &cfg); |
85 | |
86 | if (sock) |
87 | rcv_cb = cb; |
88 | |
89 | return sock; |
90 | } |
91 | |
92 | int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len, |
93 | struct net_device *dev) |
94 | { |
95 | static u32 seq; |
96 | struct sk_buff *skb = NULL; |
97 | struct nlmsghdr *nlh; |
98 | int ret = 0; |
99 | |
100 | if (group > ND_MAX_GROUP) |
101 | return -EINVAL; |
102 | |
103 | if (!netlink_has_listeners(sk: sock, group: group + 1)) |
104 | return -ESRCH; |
105 | |
106 | skb = alloc_skb(NLMSG_SPACE(len), GFP_ATOMIC); |
107 | if (!skb) |
108 | return -ENOMEM; |
109 | |
110 | seq++; |
111 | |
112 | nlh = nlmsg_put(skb, portid: 0, seq, type, payload: len, flags: 0); |
113 | memcpy(NLMSG_DATA(nlh), msg, len); |
114 | NETLINK_CB(skb).portid = 0; |
115 | NETLINK_CB(skb).dst_group = 0; |
116 | |
117 | ret = netlink_broadcast(ssk: sock, skb, portid: 0, group: group + 1, GFP_ATOMIC); |
118 | if (!ret) |
119 | return len; |
120 | |
121 | if (ret != -ESRCH) |
122 | netdev_err(dev, format: "nl broadcast g=%d, t=%d, l=%d, r=%d\n" , |
123 | group, type, len, ret); |
124 | else if (netlink_has_listeners(sk: sock, group: group + 1)) |
125 | return -EAGAIN; |
126 | |
127 | return ret; |
128 | } |
129 | |