1#define _GNU_SOURCE
2#include <netlink/netlink.h>
3#include <netlink/genl/genl.h>
4#include <netlink/genl/ctrl.h>
5#include <netlink/genl/family.h>
6#include <sys/types.h>
7#include <net/if.h>
8#include <unistd.h>
9#include <fcntl.h>
10#include <linux/nl80211.h>
11
12#include "unl.h"
13
14static int unl_init(struct unl *unl)
15{
16	unl->sock = nl_socket_alloc();
17	if (!unl->sock)
18		return -1;
19
20	return 0;
21}
22
23int unl_genl_init(struct unl *unl, const char *family)
24{
25	memset(unl, 0, sizeof(*unl));
26
27	if (unl_init(unl))
28		goto error_out;
29
30	unl->hdrlen = NLMSG_ALIGN(sizeof(struct genlmsghdr));
31	unl->family_name = strdup(family);
32	if (!unl->family_name)
33		goto error;
34
35	if (genl_connect(unl->sock))
36		goto error;
37
38	if (genl_ctrl_alloc_cache(unl->sock, &unl->cache))
39		goto error;
40
41	unl->family = genl_ctrl_search_by_name(unl->cache, family);
42	if (!unl->family)
43		goto error;
44
45	return 0;
46
47error:
48	unl_free(unl);
49error_out:
50	return -1;
51}
52
53void unl_free(struct unl *unl)
54{
55	if (unl->family_name)
56		free(unl->family_name);
57
58	if (unl->sock)
59		nl_socket_free(unl->sock);
60
61	if (unl->cache)
62		nl_cache_free(unl->cache);
63
64	memset(unl, 0, sizeof(*unl));
65}
66
67static int
68ack_handler(struct nl_msg *msg, void *arg)
69{
70	int *err = arg;
71	*err = 0;
72	return NL_STOP;
73}
74
75static int
76finish_handler(struct nl_msg *msg, void *arg)
77{
78	int *err = arg;
79	*err = 0;
80	return NL_SKIP;
81}
82
83static int
84error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg)
85{
86	int *ret = arg;
87	*ret = err->error;
88	return NL_SKIP;
89}
90
91struct nl_msg *unl_genl_msg(struct unl *unl, int cmd, bool dump)
92{
93	struct nl_msg *msg;
94	int flags = 0;
95
96	msg = nlmsg_alloc();
97	if (!msg)
98		goto out;
99
100	if (dump)
101		flags |= NLM_F_DUMP;
102
103	genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ,
104		    genl_family_get_id(unl->family), 0, flags, cmd, 0);
105
106out:
107	return msg;
108}
109
110int unl_genl_request(struct unl *unl, struct nl_msg *msg, unl_cb handler, void *arg)
111{
112	struct nl_cb *cb;
113	int err;
114
115	cb = nl_cb_alloc(NL_CB_CUSTOM);
116	err = nl_send_auto_complete(unl->sock, msg);
117	if (err < 0)
118		goto out;
119
120	err = 1;
121	nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
122	nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
123	nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
124	if (handler)
125		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, handler, arg);
126
127	while (err > 0)
128		nl_recvmsgs(unl->sock, cb);
129
130out:
131	nlmsg_free(msg);
132	nl_cb_put(cb);
133	return err;
134}
135
136static int request_single_cb(struct nl_msg *msg, void *arg)
137{
138	struct nl_msg **dest = arg;
139
140	if (!*dest) {
141		nlmsg_get(msg);
142		*dest = msg;
143	}
144	return NL_SKIP;
145}
146
147int unl_genl_request_single(struct unl *unl, struct nl_msg *msg, struct nl_msg **dest)
148{
149	*dest = NULL;
150	return unl_genl_request(unl, msg, request_single_cb, dest);
151}
152
153static int no_seq_check(struct nl_msg *msg, void *arg)
154{
155	return NL_OK;
156}
157
158void unl_genl_loop(struct unl *unl, unl_cb handler, void *arg)
159{
160	struct nl_cb *cb;
161
162	cb = nl_cb_alloc(NL_CB_CUSTOM);
163	unl->loop_done = false;
164	nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL);
165	nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, handler, arg);
166
167	while (!unl->loop_done)
168		nl_recvmsgs(unl->sock, cb);
169
170	nl_cb_put(cb);
171}
172
173int unl_genl_multicast_id(struct unl *unl, const char *name)
174{
175	struct nlattr *tb[CTRL_ATTR_MCAST_GRP_MAX + 1];
176	struct nlattr *groups, *group;
177	struct nl_msg *msg;
178	int ctrlid;
179	int ret = -1;
180	int rem;
181
182	msg = nlmsg_alloc();
183	if (!msg)
184		return -1;
185
186	ctrlid = genl_ctrl_resolve(unl->sock, "nlctrl");
187	genlmsg_put(msg, 0, 0, ctrlid, 0, 0, CTRL_CMD_GETFAMILY, 0);
188	NLA_PUT_STRING(msg, CTRL_ATTR_FAMILY_NAME, unl->family_name);
189	unl_genl_request_single(unl, msg, &msg);
190	if (!msg)
191		return -1;
192
193	groups = unl_find_attr(unl, msg, CTRL_ATTR_MCAST_GROUPS);
194	if (!groups)
195		goto nla_put_failure;
196
197	nla_for_each_nested(group, groups, rem) {
198		const char *gn;
199
200		nla_parse(tb, CTRL_ATTR_MCAST_GRP_MAX, nla_data(group),
201			  nla_len(group), NULL);
202
203		if (!tb[CTRL_ATTR_MCAST_GRP_NAME] ||
204		    !tb[CTRL_ATTR_MCAST_GRP_ID])
205			continue;
206
207		gn = nla_data(tb[CTRL_ATTR_MCAST_GRP_NAME]);
208		if (strcmp(gn, name) != 0)
209			continue;
210
211		ret = nla_get_u32(tb[CTRL_ATTR_MCAST_GRP_ID]);
212		break;
213	}
214
215nla_put_failure:
216	nlmsg_free(msg);
217	return ret;
218}
219
220int unl_genl_subscribe(struct unl *unl, const char *name)
221{
222	int mcid;
223
224	mcid = unl_genl_multicast_id(unl, name);
225	if (mcid < 0)
226		return mcid;
227
228	return nl_socket_add_membership(unl->sock, mcid);
229}
230
231int unl_genl_unsubscribe(struct unl *unl, const char *name)
232{
233	int mcid;
234
235	mcid = unl_genl_multicast_id(unl, name);
236	if (mcid < 0)
237		return mcid;
238
239	return nl_socket_drop_membership(unl->sock, mcid);
240}
241
242int unl_nl80211_phy_lookup(const char *name)
243{
244	char buf[32];
245	int fd, pos;
246
247	snprintf(buf, sizeof(buf), "/sys/class/ieee80211/%s/index", name);
248
249	fd = open(buf, O_RDONLY);
250	if (fd < 0)
251		return -1;
252	pos = read(fd, buf, sizeof(buf) - 1);
253	if (pos < 0) {
254		close(fd);
255		return -1;
256	}
257	buf[pos] = '\0';
258	close(fd);
259	return atoi(buf);
260}
261
262int unl_nl80211_wdev_to_phy(struct unl *unl, int wdev)
263{
264	struct nl_msg *msg;
265	struct nlattr *attr;
266	int ret = -1;
267
268	msg = unl_genl_msg(unl, NL80211_CMD_GET_INTERFACE, false);
269	if (!msg)
270		return -1;
271
272	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, wdev);
273	if (unl_genl_request_single(unl, msg, &msg) < 0)
274		return -1;
275
276	attr = unl_find_attr(unl, msg, NL80211_ATTR_WIPHY);
277	if (!attr)
278		goto out;
279
280	ret = nla_get_u32(attr);
281out:
282nla_put_failure:
283	nlmsg_free(msg);
284	return ret;
285}
286
287
288