• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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)228 static int get_multicast_id(struct cld80211_ctx *ctx, const char *group)
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 	cld80211_send_recv_msg(ctx, nlmsg, family_handler, &res);
242 	ALOGI("%s: nlctrl family id: %d group: %s mcast_id: %d", getprogname(),
243 				   ctx->nlctrl_familyid, group, res.id);
244 	nlmsg_free(nlmsg);
245 	return res.id;
246 }
247 
248 
cld80211_add_mcast_group(struct cld80211_ctx * ctx,const char * mcgroup)249 int cld80211_add_mcast_group(struct cld80211_ctx *ctx, const char* mcgroup)
250 {
251 	if (!ctx || !mcgroup) {
252 		ALOGE("%s: ctx/mcgroup is NULL: %s", getprogname(), __func__);
253 		return 0;
254 	}
255 	int id = get_multicast_id(ctx, mcgroup);
256 	if (id < 0) {
257 		ALOGE("%s: Could not find group %s, errno: %d id: %d",
258 		      getprogname(), mcgroup, errno, id);
259 		return id;
260 	}
261 
262 	int ret = nl_socket_add_membership(ctx->sock, id);
263 	if (ret < 0) {
264 		ALOGE("%s: Could not add membership to group %s, errno: %d",
265 		      getprogname(), mcgroup, errno);
266 	}
267 
268 	return ret;
269 }
270 
271 
cld80211_remove_mcast_group(struct cld80211_ctx * ctx,const char * mcgroup)272 int cld80211_remove_mcast_group(struct cld80211_ctx *ctx, const char* mcgroup)
273 {
274 	if (!ctx || !mcgroup) {
275 		ALOGE("%s: ctx/mcgroup is NULL: %s", getprogname(), __func__);
276 		return 0;
277 	}
278 	int id = get_multicast_id(ctx, mcgroup);
279 	if (id < 0) {
280 		ALOGE("%s: Could not find group %s, errno: %d id: %d",
281 		      getprogname(), mcgroup, errno, id);
282 		return id;
283 	}
284 
285 	int ret = nl_socket_drop_membership(ctx->sock, id);
286 	if (ret < 0) {
287 		ALOGE("%s: Could not drop membership from group %s, errno: %d,"
288 		      " ret: %d", getprogname(), mcgroup, errno, ret);
289 		return ret;
290 	}
291 
292 	return 0;
293 }
294 
295 
cld80211_msg_alloc(struct cld80211_ctx * ctx,int cmd,struct nlattr ** nla_data,int pid)296 struct nl_msg *cld80211_msg_alloc(struct cld80211_ctx *ctx, int cmd,
297 				  struct nlattr **nla_data, int pid)
298 {
299 	struct nl_msg *nlmsg;
300 
301 	if (!ctx || !nla_data) {
302 		ALOGE("%s: ctx is null: %s", getprogname(), __func__);
303 		return NULL;
304 	}
305 
306 	nlmsg = nlmsg_alloc();
307 	if (nlmsg == NULL) {
308 		ALOGE("%s: Out of memory", getprogname());
309 		return NULL;
310 	}
311 
312 	genlmsg_put(nlmsg, pid, /* seq = */ 0, ctx->netlink_familyid,
313 			0, 0, cmd, /* version = */ 0);
314 
315 	*nla_data = nla_nest_start(nlmsg, CLD80211_ATTR_VENDOR_DATA);
316 	if (!nla_data)
317 		goto cleanup;
318 
319 	return nlmsg;
320 
321 cleanup:
322 	if (nlmsg)
323 		nlmsg_free(nlmsg);
324 	return NULL;
325 }
326 
327 
cld80211_send_msg(struct cld80211_ctx * ctx,struct nl_msg * nlmsg)328 int cld80211_send_msg(struct cld80211_ctx *ctx, struct nl_msg *nlmsg)
329 {
330 	int err;
331 
332 	if (!ctx || !ctx->sock || !nlmsg) {
333 		ALOGE("%s: Invalid data from client", getprogname());
334 		return -EINVAL;
335 	}
336 
337 	err = nl_send_auto_complete(ctx->sock, nlmsg);  /* send message */
338 	if (err < 0) {
339 		ALOGE("%s: failed to send msg: %d", getprogname(), err);
340 		return err;
341 	}
342 
343 	return 0;
344 }
345 
346 
cld80211_send_recv_msg(struct cld80211_ctx * ctx,struct nl_msg * nlmsg,int (* valid_handler)(struct nl_msg *,void *),void * valid_data)347 int cld80211_send_recv_msg(struct cld80211_ctx *ctx, struct nl_msg *nlmsg,
348 			   int (*valid_handler)(struct nl_msg *, void *),
349 			   void *valid_data)
350 {
351 	int err;
352 
353 	if (!ctx || !ctx->sock || !nlmsg) {
354 		ALOGE("%s: Invalid data from client", getprogname());
355 		return -EINVAL;
356 	}
357 
358 	struct nl_cb *cb = nl_cb_alloc(NL_CB_DEFAULT);
359 	if (!cb)
360 		return -ENOMEM;
361 
362 	err = nl_send_auto_complete(ctx->sock, nlmsg);  /* send message */
363 	if (err < 0)
364 		goto out;
365 
366 	err = 1;
367 
368 	nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL);
369 	nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
370 	nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
371 	nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
372 
373 	if (valid_handler)
374 		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM,
375 			  valid_handler, valid_data);
376 	else
377 		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM,
378 			  response_handler, valid_data);
379 
380 	while (err > 0) {    /* wait for reply */
381 		int res = nl_recvmsgs(ctx->sock, cb);
382 		if (res) {
383 			ALOGE("%s: cld80211: nl_recvmsgs failed: %d",
384 			      getprogname(), res);
385 		}
386 	}
387 out:
388 	nl_cb_put(cb);
389 	return err;
390 }
391 
392 
cld80211_recv(struct cld80211_ctx * ctx,int timeout,bool recv_multi_msg,int (* valid_handler)(struct nl_msg *,void *),void * cbctx)393 int cld80211_recv(struct cld80211_ctx *ctx, int timeout, bool recv_multi_msg,
394 		  int (*valid_handler)(struct nl_msg *, void *),
395 		  void *cbctx)
396 {
397 	struct pollfd pfd[2];
398 	struct nl_cb *cb;
399 	int err;
400 
401 	if (!ctx || !ctx->sock || !valid_handler) {
402 		ALOGE("%s: Invalid data from client", getprogname());
403 		return -EINVAL;
404 	}
405 
406 	cb = nl_cb_alloc(NL_CB_DEFAULT);
407 	if (!cb)
408 		return -ENOMEM;
409 
410 	memset(&pfd[0], 0, 2*sizeof(struct pollfd));
411 
412 	err = 1;
413 
414 	nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
415 	nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
416 	nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
417 	nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL);
418 	nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, valid_handler, cbctx);
419 
420 	pfd[0].fd = nl_socket_get_fd(ctx->sock);
421 	pfd[0].events = POLLIN;
422 
423 	pfd[1].fd = ctx->exit_sockets[1];
424 	pfd[1].events = POLLIN;
425 
426 	do {
427 		pfd[0].revents = 0;
428 		pfd[1].revents = 0;
429 		int result = poll(pfd, 2, timeout);
430 		if (result < 0) {
431 			ALOGE("%s: Error polling socket", getprogname());
432 		} else if (pfd[0].revents & (POLLIN | POLLHUP | POLLERR)) {
433 			cld80211_handle_event(pfd[0].revents, ctx->sock, cb);
434 			if (!recv_multi_msg)
435 				break;
436 		} else {
437 			ALOGI("%s: Exiting poll", getprogname());
438 			break;
439 		}
440 	} while (1);
441 
442 	nl_cb_put(cb);
443 	return 0;
444 }
445 
446 
cld80211_init()447 struct cld80211_ctx * cld80211_init()
448 {
449 	struct cld80211_ctx *ctx;
450 
451 	ctx = (struct cld80211_ctx *)malloc(sizeof(struct cld80211_ctx));
452 	if (ctx == NULL) {
453 		ALOGE("%s: Failed to alloc cld80211_ctx", getprogname());
454 		return NULL;
455 	}
456 	memset(ctx, 0, sizeof(struct cld80211_ctx));
457 
458 	ctx->sock = create_nl_socket(NETLINK_GENERIC);
459 	if (ctx->sock == NULL) {
460 		ALOGE("%s: Failed to create socket port", getprogname());
461 		goto cleanup;
462 	}
463 
464 	/* Set the socket buffer size */
465 	if (nl_socket_set_buffer_size(ctx->sock, SOCK_BUF_SIZE , 0) < 0) {
466 		ALOGE("%s: Could not set nl_socket RX buffer size for sock: %s",
467 		      getprogname(), strerror(errno));
468 		/* continue anyway with the default (smaller) buffer */
469 	}
470 
471 	ctx->netlink_familyid = genl_ctrl_resolve(ctx->sock, "cld80211");
472 	if (ctx->netlink_familyid < 0) {
473 		ALOGE("%s: Could not resolve cld80211 familty id",
474 		      getprogname());
475 		goto cleanup;
476 	}
477 
478 	ctx->nlctrl_familyid = genl_ctrl_resolve(ctx->sock, "nlctrl");
479 	if (ctx->nlctrl_familyid < 0) {
480 		ALOGE("%s: net link family nlctrl is not present: %d err:%d",
481 			getprogname(), ctx->nlctrl_familyid, errno);
482 		goto cleanup;
483 	}
484 
485 
486 	if (init_exit_sockets(ctx) != 0) {
487 		ALOGE("%s: Failed to initialize exit sockets", getprogname());
488 		goto cleanup;
489 	}
490 
491 	return ctx;
492 cleanup:
493 	if (ctx->sock) {
494 		nl_socket_free(ctx->sock);
495 	}
496 	free (ctx);
497 	return NULL;
498 }
499 
500 
cld80211_deinit(struct cld80211_ctx * ctx)501 void cld80211_deinit(struct cld80211_ctx *ctx)
502 {
503 	if (!ctx || !ctx->sock) {
504 		ALOGE("%s: ctx/sock is NULL", getprogname());
505 		return;
506 	}
507 	nl_socket_free(ctx->sock);
508 	cleanup_exit_sockets(ctx);
509 	free (ctx);
510 }
511