1 /*
2  * Driver interaction with Linux nl80211/cfg80211
3  * Copyright (c) 2002-2015, Jouni Malinen <j@w1.fi>
4  * Copyright (c) 2003-2004, Instant802 Networks, Inc.
5  * Copyright (c) 2005-2006, Devicescape Software, Inc.
6  * Copyright (c) 2007, Johannes Berg <johannes@sipsolutions.net>
7  * Copyright (c) 2009-2010, Atheros Communications
8  * Copyright (c) 2017, The Linux Foundation. All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions are
12  * met:
13  *     * Redistributions of source code must retain the above copyright
14  *     * Redistributions in binary form must reproduce the above
15  *       copyright notice, this list of conditions and the following
16  *       disclaimer in the documentation and/or other materials provided
17  *       with the distribution.
18  *     * Neither the name of The Linux Foundation nor the names of its
19  *       contributors may be used to endorse or promote products derived
20  *       from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
23  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
24  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
25  * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
26  * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
27  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
28  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
29  * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
30  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
31  * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
32  * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE
33  *
34  */
35 
36 #include <errno.h>
37 #include <netlink/genl/family.h>
38 #include <netlink/genl/ctrl.h>
39 #include <linux/pkt_sched.h>
40 #include <unistd.h>
41 #include <log/log.h>
42 #include "cld80211_lib.h"
43 
44 #undef LOG_TAG
45 #define LOG_TAG "CLD80211"
46 #define SOCK_BUF_SIZE (256*1024)
47 
48 struct family_data {
49 	const char *group;
50 	int id;
51 };
52 
53 
create_nl_socket(int protocol)54 static struct nl_sock * create_nl_socket(int protocol)
55 {
56 	struct nl_sock *sock;
57 
58 	sock = nl_socket_alloc();
59 	if (sock == NULL) {
60 		ALOGE("%s: Failed to create NL socket, err: %d",
61 		      getprogname(), errno);
62 		return NULL;
63 	}
64 
65 	if (nl_connect(sock, protocol)) {
66 		ALOGE("%s: Could not connect sock, err: %d",
67 		      getprogname(), errno);
68 		nl_socket_free(sock);
69 		return NULL;
70 	}
71 
72 	return sock;
73 }
74 
75 
init_exit_sockets(struct cld80211_ctx * ctx)76 static int init_exit_sockets(struct cld80211_ctx *ctx)
77 {
78 	ctx->exit_sockets[0] = -1;
79 	ctx->exit_sockets[1] = -1;
80 	if (socketpair(AF_UNIX, SOCK_STREAM, 0, &ctx->exit_sockets[0]) == -1) {
81 		ALOGE("%s: Failed to create exit socket pair", getprogname());
82 		return -1;
83 	}
84 	ALOGI("%s: initialized exit socket pair", getprogname());
85 
86 	return 0;
87 }
88 
89 
cleanup_exit_sockets(struct cld80211_ctx * ctx)90 static void cleanup_exit_sockets(struct cld80211_ctx *ctx)
91 {
92 	if (ctx->exit_sockets[0] >= 0) {
93 		close(ctx->exit_sockets[0]);
94 		ctx->exit_sockets[0] = -1;
95 	}
96 
97 	if (ctx->exit_sockets[1] >= 0) {
98 		close(ctx->exit_sockets[1]);
99 		ctx->exit_sockets[1] = -1;
100 	}
101 }
102 
103 
exit_cld80211_recv(struct cld80211_ctx * ctx)104 void exit_cld80211_recv(struct cld80211_ctx *ctx)
105 {
106 	if (!ctx) {
107 		ALOGE("%s: ctx is NULL: %s", getprogname(), __func__);
108 		return;
109 	}
110 	TEMP_FAILURE_RETRY(write(ctx->exit_sockets[0], "E", 1));
111 	ALOGI("%s: Sent msg on exit sock to unblock poll()", getprogname());
112 }
113 
114 
115 /* Event handlers */
response_handler(struct nl_msg * msg,void * arg)116 static int response_handler(struct nl_msg *msg, void *arg)
117 {
118 	UNUSED(msg);
119 	UNUSED(arg);
120 	ALOGI("%s: Received nlmsg response: no callback registered;drop it",
121 	      getprogname());
122 
123 	return NL_SKIP;
124 }
125 
126 
ack_handler(struct nl_msg * msg,void * arg)127 static int ack_handler(struct nl_msg *msg, void *arg)
128 {
129 	int *err = (int *)arg;
130 	*err = 0;
131 	UNUSED(msg);
132 	return NL_STOP;
133 }
134 
135 
finish_handler(struct nl_msg * msg,void * arg)136 static int finish_handler(struct nl_msg *msg, void *arg)
137 {
138 	int *ret = (int *)arg;
139 	*ret = 0;
140 	UNUSED(msg);
141 	return NL_SKIP;
142 }
143 
144 
error_handler(struct sockaddr_nl * nla,struct nlmsgerr * err,void * arg)145 static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err,
146 			 void *arg)
147 {
148 	int *ret = (int *)arg;
149 	*ret = err->error;
150 
151 	UNUSED(nla);
152 	ALOGE("%s: error_handler received : %d", getprogname(), err->error);
153 	return NL_SKIP;
154 }
155 
156 
no_seq_check(struct nl_msg * msg,void * arg)157 static int no_seq_check(struct nl_msg *msg, void *arg)
158 {
159 	UNUSED(msg);
160 	UNUSED(arg);
161 	return NL_OK;
162 }
163 
164 
cld80211_recv_msg(struct nl_sock * sock,struct nl_cb * cb)165 int cld80211_recv_msg(struct nl_sock *sock, struct nl_cb *cb)
166 {
167 	if (!sock || !cb) {
168 		ALOGE("%s: %s is NULL", getprogname(), sock?"cb":"sock");
169 		return -EINVAL;
170 	}
171 
172 	int res = nl_recvmsgs(sock, cb);
173 	if(res)
174 		ALOGE("%s: Error :%d while reading nl msg , err: %d",
175 		      getprogname(), res, errno);
176 	return res;
177 }
178 
179 
cld80211_handle_event(int events,struct nl_sock * sock,struct nl_cb * cb)180 static void cld80211_handle_event(int events, struct nl_sock *sock,
181 				  struct nl_cb *cb)
182 {
183 	if (events & POLLERR) {
184 		ALOGE("%s: Error reading from socket", getprogname());
185 		cld80211_recv_msg(sock, cb);
186 	} else if (events & POLLHUP) {
187 		ALOGE("%s: Remote side hung up", getprogname());
188 	} else if (events & POLLIN) {
189 		cld80211_recv_msg(sock, cb);
190 	} else {
191 		ALOGE("%s: Unknown event - %0x", getprogname(), events);
192 	}
193 }
194 
195 
family_handler(struct nl_msg * msg,void * arg)196 static int family_handler(struct nl_msg *msg, void *arg)
197 {
198 	struct family_data *res = arg;
199 	struct nlattr *tb[CTRL_ATTR_MAX + 1];
200 	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
201 	struct nlattr *mcgrp;
202 	int i;
203 
204 	nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
205 			genlmsg_attrlen(gnlh, 0), NULL);
206 	if (!tb[CTRL_ATTR_MCAST_GROUPS])
207 		return NL_SKIP;
208 
209 	nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], i) {
210 		struct nlattr *tb2[CTRL_ATTR_MCAST_GRP_MAX + 1];
211 		nla_parse(tb2, CTRL_ATTR_MCAST_GRP_MAX, nla_data(mcgrp),
212 				nla_len(mcgrp), NULL);
213 
214 		if (!tb2[CTRL_ATTR_MCAST_GRP_NAME] ||
215 			!tb2[CTRL_ATTR_MCAST_GRP_ID] ||
216 			strncmp(nla_data(tb2[CTRL_ATTR_MCAST_GRP_NAME]),
217 				   res->group,
218 				   nla_len(tb2[CTRL_ATTR_MCAST_GRP_NAME])) != 0)
219 			continue;
220 		res->id = nla_get_u32(tb2[CTRL_ATTR_MCAST_GRP_ID]);
221 		break;
222 	};
223 
224 	return NL_SKIP;
225 }
226 
227 
get_multicast_id(struct cld80211_ctx * ctx,const char * group,bool sync_driver)228 static int get_multicast_id(struct cld80211_ctx *ctx, const char *group, bool sync_driver)
229 {
230 	struct family_data res = { group, -ENOENT };
231 	struct nl_msg *nlmsg = nlmsg_alloc();
232 
233 	if (!nlmsg) {
234 		return -1;
235 	}
236 
237 	genlmsg_put(nlmsg, 0, 0, ctx->nlctrl_familyid, 0, 0,
238 	            CTRL_CMD_GETFAMILY, 0);
239 	nla_put_string(nlmsg, CTRL_ATTR_FAMILY_NAME, "cld80211");
240 
241 	if (sync_driver == true) {
242 		cld80211_send_recv_msg(ctx, nlmsg, family_handler, &res);
243 		ALOGI("%s: nlctrl family id: %d group: %s mcast_id: %d", getprogname(),
244 				ctx->nlctrl_familyid, group, res.id);
245 	}
246 	nlmsg_free(nlmsg);
247 	return res.id;
248 }
249 
250 
cld80211_add_mcast_group(struct cld80211_ctx * ctx,const char * mcgroup)251 int cld80211_add_mcast_group(struct cld80211_ctx *ctx, const char* mcgroup)
252 {
253 	if (!ctx || !mcgroup) {
254 		ALOGE("%s: ctx/mcgroup is NULL: %s", getprogname(), __func__);
255 		return 0;
256 	}
257 	int id = get_multicast_id(ctx, mcgroup, true);
258 	if (id < 0) {
259 		ALOGE("%s: Could not find group %s, errno: %d id: %d",
260 		      getprogname(), mcgroup, errno, id);
261 		return id;
262 	}
263 
264 	int ret = nl_socket_add_membership(ctx->sock, id);
265 	if (ret < 0) {
266 		ALOGE("%s: Could not add membership to group %s, errno: %d",
267 		      getprogname(), mcgroup, errno);
268 	}
269 
270 	return ret;
271 }
272 
273 
cld80211_remove_mcast_group(struct cld80211_ctx * ctx,const char * mcgroup)274 int cld80211_remove_mcast_group(struct cld80211_ctx *ctx, const char* mcgroup)
275 {
276 	if (!ctx || !mcgroup) {
277 		ALOGE("%s: ctx/mcgroup is NULL: %s", getprogname(), __func__);
278 		return 0;
279 	}
280 	int id = get_multicast_id(ctx, mcgroup, false);
281 	if (id < 0) {
282 		ALOGE("%s: Could not find group %s, errno: %d id: %d",
283 		      getprogname(), mcgroup, errno, id);
284 		return id;
285 	}
286 
287 	int ret = nl_socket_drop_membership(ctx->sock, id);
288 	if (ret < 0) {
289 		ALOGE("%s: Could not drop membership from group %s, errno: %d,"
290 		      " ret: %d", getprogname(), mcgroup, errno, ret);
291 		return ret;
292 	}
293 
294 	return 0;
295 }
296 
297 
cld80211_msg_alloc(struct cld80211_ctx * ctx,int cmd,struct nlattr ** nla_data,int pid)298 struct nl_msg *cld80211_msg_alloc(struct cld80211_ctx *ctx, int cmd,
299 				  struct nlattr **nla_data, int pid)
300 {
301 	struct nl_msg *nlmsg;
302 
303 	if (!ctx || !nla_data) {
304 		ALOGE("%s: ctx is null: %s", getprogname(), __func__);
305 		return NULL;
306 	}
307 
308 	nlmsg = nlmsg_alloc();
309 	if (nlmsg == NULL) {
310 		ALOGE("%s: Out of memory", getprogname());
311 		return NULL;
312 	}
313 
314 	genlmsg_put(nlmsg, pid, /* seq = */ 0, ctx->netlink_familyid,
315 			0, 0, cmd, /* version = */ 0);
316 
317 	*nla_data = nla_nest_start(nlmsg, CLD80211_ATTR_VENDOR_DATA);
318 	if (!nla_data)
319 		goto cleanup;
320 
321 	return nlmsg;
322 
323 cleanup:
324 	if (nlmsg)
325 		nlmsg_free(nlmsg);
326 	return NULL;
327 }
328 
329 
cld80211_send_msg(struct cld80211_ctx * ctx,struct nl_msg * nlmsg)330 int cld80211_send_msg(struct cld80211_ctx *ctx, struct nl_msg *nlmsg)
331 {
332 	int err;
333 
334 	if (!ctx || !ctx->sock || !nlmsg) {
335 		ALOGE("%s: Invalid data from client", getprogname());
336 		return -EINVAL;
337 	}
338 
339 	err = nl_send_auto_complete(ctx->sock, nlmsg);  /* send message */
340 	if (err < 0) {
341 		ALOGE("%s: failed to send msg: %d", getprogname(), err);
342 		return err;
343 	}
344 
345 	return 0;
346 }
347 
348 
cld80211_send_recv_msg(struct cld80211_ctx * ctx,struct nl_msg * nlmsg,int (* valid_handler)(struct nl_msg *,void *),void * valid_data)349 int cld80211_send_recv_msg(struct cld80211_ctx *ctx, struct nl_msg *nlmsg,
350 			   int (*valid_handler)(struct nl_msg *, void *),
351 			   void *valid_data)
352 {
353 	int err;
354 
355 	if (!ctx || !ctx->sock || !nlmsg) {
356 		ALOGE("%s: Invalid data from client", getprogname());
357 		return -EINVAL;
358 	}
359 
360 	struct nl_cb *cb = nl_cb_alloc(NL_CB_DEFAULT);
361 	if (!cb)
362 		return -ENOMEM;
363 
364 	err = nl_send_auto_complete(ctx->sock, nlmsg);  /* send message */
365 	if (err < 0)
366 		goto out;
367 
368 	err = 1;
369 
370 	nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL);
371 	nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
372 	nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
373 	nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
374 
375 	if (valid_handler)
376 		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM,
377 			  valid_handler, valid_data);
378 	else
379 		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM,
380 			  response_handler, valid_data);
381 
382 	while (err > 0) {    /* wait for reply */
383 		int res = nl_recvmsgs(ctx->sock, cb);
384 		if (res) {
385 			ALOGE("%s: cld80211: nl_recvmsgs failed: %d",
386 			      getprogname(), res);
387 		}
388 	}
389 out:
390 	nl_cb_put(cb);
391 	return err;
392 }
393 
394 
cld80211_recv(struct cld80211_ctx * ctx,int timeout,bool recv_multi_msg,int (* valid_handler)(struct nl_msg *,void *),void * cbctx)395 int cld80211_recv(struct cld80211_ctx *ctx, int timeout, bool recv_multi_msg,
396 		  int (*valid_handler)(struct nl_msg *, void *),
397 		  void *cbctx)
398 {
399 	struct pollfd pfd[2];
400 	struct nl_cb *cb;
401 	int err;
402 
403 	if (!ctx || !ctx->sock || !valid_handler) {
404 		ALOGE("%s: Invalid data from client", getprogname());
405 		return -EINVAL;
406 	}
407 
408 	cb = nl_cb_alloc(NL_CB_DEFAULT);
409 	if (!cb)
410 		return -ENOMEM;
411 
412 	memset(&pfd[0], 0, 2*sizeof(struct pollfd));
413 
414 	err = 1;
415 
416 	nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
417 	nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
418 	nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
419 	nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL);
420 	nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, valid_handler, cbctx);
421 
422 	pfd[0].fd = nl_socket_get_fd(ctx->sock);
423 	pfd[0].events = POLLIN;
424 
425 	pfd[1].fd = ctx->exit_sockets[1];
426 	pfd[1].events = POLLIN;
427 
428 	do {
429 		pfd[0].revents = 0;
430 		pfd[1].revents = 0;
431 		int result = poll(pfd, 2, timeout);
432 		if (result < 0) {
433 			ALOGE("%s: Error polling socket", getprogname());
434 		} else if (pfd[0].revents & (POLLIN | POLLHUP | POLLERR)) {
435 			cld80211_handle_event(pfd[0].revents, ctx->sock, cb);
436 			if (!recv_multi_msg)
437 				break;
438 		} else {
439 			ALOGI("%s: Exiting poll", getprogname());
440 			break;
441 		}
442 	} while (1);
443 
444 	nl_cb_put(cb);
445 	return 0;
446 }
447 
448 
cld80211_init()449 struct cld80211_ctx * cld80211_init()
450 {
451 	struct cld80211_ctx *ctx;
452 
453 	ctx = (struct cld80211_ctx *)malloc(sizeof(struct cld80211_ctx));
454 	if (ctx == NULL) {
455 		ALOGE("%s: Failed to alloc cld80211_ctx", getprogname());
456 		return NULL;
457 	}
458 	memset(ctx, 0, sizeof(struct cld80211_ctx));
459 
460 	ctx->sock = create_nl_socket(NETLINK_GENERIC);
461 	if (ctx->sock == NULL) {
462 		ALOGE("%s: Failed to create socket port", getprogname());
463 		goto cleanup;
464 	}
465 
466 	/* Set the socket buffer size */
467 	if (nl_socket_set_buffer_size(ctx->sock, SOCK_BUF_SIZE , 0) < 0) {
468 		ALOGE("%s: Could not set nl_socket RX buffer size for sock: %s",
469 		      getprogname(), strerror(errno));
470 		/* continue anyway with the default (smaller) buffer */
471 	}
472 
473 	ctx->netlink_familyid = genl_ctrl_resolve(ctx->sock, "cld80211");
474 	if (ctx->netlink_familyid < 0) {
475 		ALOGE("%s: Could not resolve cld80211 familty id",
476 		      getprogname());
477 		goto cleanup;
478 	}
479 
480 	ctx->nlctrl_familyid = genl_ctrl_resolve(ctx->sock, "nlctrl");
481 	if (ctx->nlctrl_familyid < 0) {
482 		ALOGE("%s: net link family nlctrl is not present: %d err:%d",
483 			getprogname(), ctx->nlctrl_familyid, errno);
484 		goto cleanup;
485 	}
486 
487 
488 	if (init_exit_sockets(ctx) != 0) {
489 		ALOGE("%s: Failed to initialize exit sockets", getprogname());
490 		goto cleanup;
491 	}
492 
493 	return ctx;
494 cleanup:
495 	if (ctx->sock) {
496 		nl_socket_free(ctx->sock);
497 	}
498 	free (ctx);
499 	return NULL;
500 }
501 
502 
cld80211_deinit(struct cld80211_ctx * ctx)503 void cld80211_deinit(struct cld80211_ctx *ctx)
504 {
505 	if (!ctx || !ctx->sock) {
506 		ALOGE("%s: ctx/sock is NULL", getprogname());
507 		return;
508 	}
509 	nl_socket_free(ctx->sock);
510 	cleanup_exit_sockets(ctx);
511 	free (ctx);
512 }
513