diff options
-rw-r--r-- | cld80211-lib/Android.mk | 27 | ||||
-rw-r--r-- | cld80211-lib/cld80211_lib.c | 510 | ||||
-rw-r--r-- | cld80211-lib/cld80211_lib.h | 146 |
3 files changed, 683 insertions, 0 deletions
diff --git a/cld80211-lib/Android.mk b/cld80211-lib/Android.mk new file mode 100644 index 0000000..d58edb8 --- /dev/null +++ b/cld80211-lib/Android.mk @@ -0,0 +1,27 @@ +LOCAL_PATH := $(call my-dir) + +include $(CLEAR_VARS) +LOCAL_MODULE := libcld80211 +LOCAL_CLANG := true +LOCAL_MODULE_TAGS := optional +LOCAL_C_INCLUDES += $(LOCAL_PATH) \ + external/libnl/include +LOCAL_SHARED_LIBRARIES := libcutils libnl liblog +LOCAL_SRC_FILES := cld80211_lib.c +LOCAL_CFLAGS += -Wall -Werror -Wno-unused-parameter +LOCAL_COPY_HEADERS_TO := cld80211-lib +LOCAL_COPY_HEADERS := cld80211_lib.h +include $(BUILD_SHARED_LIBRARY) + +include $(CLEAR_VARS) +LOCAL_MODULE := libcld80211 +LOCAL_CLANG := true +LOCAL_MODULE_TAGS := optional +LOCAL_C_INCLUDES += $(LOCAL_PATH) \ + external/libnl/include +LOCAL_SHARED_LIBRARIES := libcutils libnl liblog +LOCAL_SRC_FILES := cld80211_lib.c +LOCAL_CFLAGS += -Wall -Werror -Wno-unused-parameter +LOCAL_COPY_HEADERS_TO := cld80211-lib +LOCAL_COPY_HEADERS := cld80211_lib.h +include $(BUILD_STATIC_LIBRARY) diff --git a/cld80211-lib/cld80211_lib.c b/cld80211-lib/cld80211_lib.c new file mode 100644 index 0000000..abec22c --- /dev/null +++ b/cld80211-lib/cld80211_lib.c @@ -0,0 +1,510 @@ +/* + * Driver interaction with Linux nl80211/cfg80211 + * Copyright (c) 2002-2015, Jouni Malinen <j@w1.fi> + * Copyright (c) 2003-2004, Instant802 Networks, Inc. + * Copyright (c) 2005-2006, Devicescape Software, Inc. + * Copyright (c) 2007, Johannes Berg <johannes@sipsolutions.net> + * Copyright (c) 2009-2010, Atheros Communications + * Copyright (c) 2017, The Linux Foundation. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * * Redistributions of source code must retain the above copyright + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of The Linux Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS + * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE + * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN + * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE + * + */ + +#include <errno.h> +#include <netlink/genl/family.h> +#include <netlink/genl/ctrl.h> +#include <linux/pkt_sched.h> +#include <unistd.h> +#include <utils/Log.h> +#include "cld80211_lib.h" + +#undef LOG_TAG +#define LOG_TAG "CLD80211" +#define SOCK_BUF_SIZE (256*1024) + +struct family_data { + const char *group; + int id; +}; + + +static struct nl_sock * create_nl_socket(int protocol) +{ + struct nl_sock *sock; + + sock = nl_socket_alloc(); + if (sock == NULL) { + ALOGE("%s: Failed to create NL socket, err: %d", + getprogname(), errno); + return NULL; + } + + if (nl_connect(sock, protocol)) { + ALOGE("%s: Could not connect sock, err: %d", + getprogname(), errno); + nl_socket_free(sock); + return NULL; + } + + return sock; +} + + +static int init_exit_sockets(struct cld80211_ctx *ctx) +{ + ctx->exit_sockets[0] = -1; + ctx->exit_sockets[1] = -1; + if (socketpair(AF_UNIX, SOCK_STREAM, 0, &ctx->exit_sockets[0]) == -1) { + ALOGE("%s: Failed to create exit socket pair", getprogname()); + return -1; + } + ALOGI("%s: initialized exit socket pair", getprogname()); + + return 0; +} + + +static void cleanup_exit_sockets(struct cld80211_ctx *ctx) +{ + if (ctx->exit_sockets[0] >= 0) { + close(ctx->exit_sockets[0]); + ctx->exit_sockets[0] = -1; + } + + if (ctx->exit_sockets[1] >= 0) { + close(ctx->exit_sockets[1]); + ctx->exit_sockets[1] = -1; + } +} + + +void exit_cld80211_recv(struct cld80211_ctx *ctx) +{ + if (!ctx) { + ALOGE("%s: ctx is NULL: %s", getprogname(), __func__); + return; + } + TEMP_FAILURE_RETRY(write(ctx->exit_sockets[0], "E", 1)); + ALOGI("%s: Sent msg on exit sock to unblock poll()", getprogname()); +} + + +/* Event handlers */ +static int response_handler(struct nl_msg *msg, void *arg) +{ + UNUSED(msg); + UNUSED(arg); + ALOGI("%s: Received nlmsg response: no callback registered;drop it", + getprogname()); + + return NL_SKIP; +} + + +static int ack_handler(struct nl_msg *msg, void *arg) +{ + int *err = (int *)arg; + *err = 0; + UNUSED(msg); + return NL_STOP; +} + + +static int finish_handler(struct nl_msg *msg, void *arg) +{ + int *ret = (int *)arg; + *ret = 0; + UNUSED(msg); + return NL_SKIP; +} + + +static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, + void *arg) +{ + int *ret = (int *)arg; + *ret = err->error; + + UNUSED(nla); + ALOGE("%s: error_handler received : %d", getprogname(), err->error); + return NL_SKIP; +} + + +static int no_seq_check(struct nl_msg *msg, void *arg) +{ + UNUSED(msg); + UNUSED(arg); + return NL_OK; +} + + +int cld80211_recv_msg(struct nl_sock *sock, struct nl_cb *cb) +{ + if (!sock || !cb) { + ALOGE("%s: %s is NULL", getprogname(), sock?"cb":"sock"); + return -EINVAL; + } + + int res = nl_recvmsgs(sock, cb); + if(res) + ALOGE("%s: Error :%d while reading nl msg , err: %d", + getprogname(), res, errno); + return res; +} + + +static void cld80211_handle_event(int events, struct nl_sock *sock, + struct nl_cb *cb) +{ + if (events & POLLERR) { + ALOGE("%s: Error reading from socket", getprogname()); + cld80211_recv_msg(sock, cb); + } else if (events & POLLHUP) { + ALOGE("%s: Remote side hung up", getprogname()); + } else if (events & POLLIN) { + cld80211_recv_msg(sock, cb); + } else { + ALOGE("%s: Unknown event - %0x", getprogname(), events); + } +} + + +static int family_handler(struct nl_msg *msg, void *arg) +{ + struct family_data *res = arg; + struct nlattr *tb[CTRL_ATTR_MAX + 1]; + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); + struct nlattr *mcgrp; + int i; + + nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0), + genlmsg_attrlen(gnlh, 0), NULL); + if (!tb[CTRL_ATTR_MCAST_GROUPS]) + return NL_SKIP; + + nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], i) { + struct nlattr *tb2[CTRL_ATTR_MCAST_GRP_MAX + 1]; + nla_parse(tb2, CTRL_ATTR_MCAST_GRP_MAX, nla_data(mcgrp), + nla_len(mcgrp), NULL); + + if (!tb2[CTRL_ATTR_MCAST_GRP_NAME] || + !tb2[CTRL_ATTR_MCAST_GRP_ID] || + strncmp(nla_data(tb2[CTRL_ATTR_MCAST_GRP_NAME]), + res->group, + nla_len(tb2[CTRL_ATTR_MCAST_GRP_NAME])) != 0) + continue; + res->id = nla_get_u32(tb2[CTRL_ATTR_MCAST_GRP_ID]); + break; + }; + + return NL_SKIP; +} + + +static int get_multicast_id(struct cld80211_ctx *ctx, const char *group) +{ + struct family_data res = { group, -ENOENT }; + struct nl_msg *nlmsg = nlmsg_alloc(); + + if (!nlmsg) { + return -1; + } + + genlmsg_put(nlmsg, 0, 0, ctx->nlctrl_familyid, 0, 0, + CTRL_CMD_GETFAMILY, 0); + nla_put_string(nlmsg, CTRL_ATTR_FAMILY_NAME, "cld80211"); + + cld80211_send_recv_msg(ctx, nlmsg, family_handler, &res); + ALOGI("%s: nlctrl family id: %d group: %s mcast_id: %d", getprogname(), + ctx->nlctrl_familyid, group, res.id); + nlmsg_free(nlmsg); + return res.id; +} + + +int cld80211_add_mcast_group(struct cld80211_ctx *ctx, const char* mcgroup) +{ + if (!ctx || !mcgroup) { + ALOGE("%s: ctx/mcgroup is NULL: %s", getprogname(), __func__); + return 0; + } + int id = get_multicast_id(ctx, mcgroup); + if (id < 0) { + ALOGE("%s: Could not find group %s, errno: %d id: %d", + getprogname(), mcgroup, errno, id); + return id; + } + + int ret = nl_socket_add_membership(ctx->sock, id); + if (ret < 0) { + ALOGE("%s: Could not add membership to group %s, errno: %d", + getprogname(), mcgroup, errno); + } + + return ret; +} + + +int cld80211_remove_mcast_group(struct cld80211_ctx *ctx, const char* mcgroup) +{ + if (!ctx || !mcgroup) { + ALOGE("%s: ctx/mcgroup is NULL: %s", getprogname(), __func__); + return 0; + } + int id = get_multicast_id(ctx, mcgroup); + if (id < 0) { + ALOGE("%s: Could not find group %s, errno: %d id: %d", + getprogname(), mcgroup, errno, id); + return id; + } + + int ret = nl_socket_drop_membership(ctx->sock, id); + if (ret < 0) { + ALOGE("%s: Could not drop membership from group %s, errno: %d," + " ret: %d", getprogname(), mcgroup, errno, ret); + return ret; + } + + return 0; +} + + +struct nl_msg *cld80211_msg_alloc(struct cld80211_ctx *ctx, int cmd, + struct nlattr **nla_data, int pid) +{ + struct nl_msg *nlmsg; + + if (!ctx || !nla_data) { + ALOGE("%s: ctx is null: %s", getprogname(), __func__); + return NULL; + } + + nlmsg = nlmsg_alloc(); + if (nlmsg == NULL) { + ALOGE("%s: Out of memory", getprogname()); + return NULL; + } + + genlmsg_put(nlmsg, pid, /* seq = */ 0, ctx->netlink_familyid, + 0, 0, cmd, /* version = */ 0); + + *nla_data = nla_nest_start(nlmsg, CLD80211_ATTR_VENDOR_DATA); + if (!nla_data) + goto cleanup; + + return nlmsg; + +cleanup: + if (nlmsg) + nlmsg_free(nlmsg); + return NULL; +} + + +int cld80211_send_msg(struct cld80211_ctx *ctx, struct nl_msg *nlmsg) +{ + int err; + + if (!ctx || !ctx->sock || !nlmsg) { + ALOGE("%s: Invalid data from client", getprogname()); + return -EINVAL; + } + + err = nl_send_auto_complete(ctx->sock, nlmsg); /* send message */ + if (err < 0) { + ALOGE("%s: failed to send msg: %d", getprogname(), err); + return err; + } + + return 0; +} + + +int cld80211_send_recv_msg(struct cld80211_ctx *ctx, struct nl_msg *nlmsg, + int (*valid_handler)(struct nl_msg *, void *), + void *valid_data) +{ + int err; + + if (!ctx || !ctx->sock || !nlmsg) { + ALOGE("%s: Invalid data from client", getprogname()); + return -EINVAL; + } + + struct nl_cb *cb = nl_cb_alloc(NL_CB_DEFAULT); + if (!cb) + return -ENOMEM; + + err = nl_send_auto_complete(ctx->sock, nlmsg); /* send message */ + if (err < 0) + goto out; + + err = 1; + + nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL); + nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err); + nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err); + nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); + + if (valid_handler) + nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, + valid_handler, valid_data); + else + nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, + response_handler, valid_data); + + while (err > 0) { /* wait for reply */ + int res = nl_recvmsgs(ctx->sock, cb); + if (res) { + ALOGE("%s: cld80211: nl_recvmsgs failed: %d", + getprogname(), res); + } + } +out: + nl_cb_put(cb); + return err; +} + + +int cld80211_recv(struct cld80211_ctx *ctx, int timeout, bool recv_multi_msg, + int (*valid_handler)(struct nl_msg *, void *), + void *cbctx) +{ + struct pollfd pfd[2]; + struct nl_cb *cb; + int err; + + if (!ctx || !ctx->sock || !valid_handler) { + ALOGE("%s: Invalid data from client", getprogname()); + return -EINVAL; + } + + cb = nl_cb_alloc(NL_CB_DEFAULT); + if (!cb) + return -ENOMEM; + + memset(&pfd[0], 0, 2*sizeof(struct pollfd)); + + err = 1; + + nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err); + nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err); + nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); + nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL); + nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, valid_handler, cbctx); + + pfd[0].fd = nl_socket_get_fd(ctx->sock); + pfd[0].events = POLLIN; + + pfd[1].fd = ctx->exit_sockets[1]; + pfd[1].events = POLLIN; + + do { + pfd[0].revents = 0; + pfd[1].revents = 0; + int result = poll(pfd, 2, timeout); + if (result < 0) { + ALOGE("%s: Error polling socket", getprogname()); + } else if (pfd[0].revents & (POLLIN | POLLHUP | POLLERR)) { + cld80211_handle_event(pfd[0].revents, ctx->sock, cb); + if (!recv_multi_msg) + break; + } else { + ALOGI("%s: Exiting poll", getprogname()); + break; + } + } while (1); + + nl_cb_put(cb); + return 0; +} + + +struct cld80211_ctx * cld80211_init() +{ + struct cld80211_ctx *ctx; + + ctx = (struct cld80211_ctx *)malloc(sizeof(struct cld80211_ctx)); + if (ctx == NULL) { + ALOGE("%s: Failed to alloc cld80211_ctx", getprogname()); + return NULL; + } + memset(ctx, 0, sizeof(struct cld80211_ctx)); + + ctx->sock = create_nl_socket(NETLINK_GENERIC); + if (ctx->sock == NULL) { + ALOGE("%s: Failed to create socket port", getprogname()); + goto cleanup; + } + + /* Set the socket buffer size */ + if (nl_socket_set_buffer_size(ctx->sock, SOCK_BUF_SIZE , 0) < 0) { + ALOGE("%s: Could not set nl_socket RX buffer size for sock: %s", + getprogname(), strerror(errno)); + /* continue anyway with the default (smaller) buffer */ + } + + ctx->netlink_familyid = genl_ctrl_resolve(ctx->sock, "cld80211"); + if (ctx->netlink_familyid < 0) { + ALOGE("%s: Could not resolve cld80211 familty id", + getprogname()); + goto cleanup; + } + + ctx->nlctrl_familyid = genl_ctrl_resolve(ctx->sock, "nlctrl"); + if (ctx->nlctrl_familyid < 0) { + ALOGE("%s: net link family nlctrl is not present: %d err:%d", + getprogname(), ctx->nlctrl_familyid, errno); + goto cleanup; + } + + + if (init_exit_sockets(ctx) != 0) { + ALOGE("%s: Failed to initialize exit sockets", getprogname()); + goto cleanup; + } + + return ctx; +cleanup: + if (ctx->sock) { + nl_socket_free(ctx->sock); + } + free (ctx); + return NULL; +} + + +void cld80211_deinit(struct cld80211_ctx *ctx) +{ + if (!ctx || !ctx->sock) { + ALOGE("%s: ctx/sock is NULL", getprogname()); + return; + } + nl_socket_free(ctx->sock); + cleanup_exit_sockets(ctx); + free (ctx); +} diff --git a/cld80211-lib/cld80211_lib.h b/cld80211-lib/cld80211_lib.h new file mode 100644 index 0000000..16b3f20 --- /dev/null +++ b/cld80211-lib/cld80211_lib.h @@ -0,0 +1,146 @@ +/* Copyright (c) 2017, The Linux Foundation. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of The Linux Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS + * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR + * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE + * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN + * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef CLD80211_LIB_H +#define CLD80211_LIB_H + +#ifdef __cplusplus +extern "C" { +#endif +#include <netlink/genl/genl.h> +#include <stdbool.h> + +#ifndef UNUSED +#define UNUSED(x) (void)(x) +#endif + +struct cld80211_ctx { + struct nl_sock *sock; + int netlink_familyid; + /* socket pair used to exit from blocking poll*/ + int exit_sockets[2]; + int sock_buf_size; + int nlctrl_familyid; +}; + +/** + * enum cld80211_attr - Driver/Application embeds the data in nlmsg with the + * help of below attributes + * CLD80211_ATTR_VENDOR_DATA: Embed all other attributes in this nested + * attribute. + * CLD80211_ATTR_DATA: Embed driver/application data in this attribute + * Any new message in future can be added as another attribute + */ +enum cld80211_attr { + CLD80211_ATTR_VENDOR_DATA = 1, + CLD80211_ATTR_DATA, + + __CLD80211_ATTR_AFTER_LAST, + CLD80211_ATTR_MAX = __CLD80211_ATTR_AFTER_LAST - 1 +}; + +/** + * Create socket of type NETLINK_GENERIC + * Retuns valid sock only if socket creation is succesful and cld80211 + * family is present, returns NULL otherwise + */ +struct cld80211_ctx *cld80211_init(); + +/** + * free the socket created in cld80211_init() + */ +void cld80211_deinit(struct cld80211_ctx *ctx); + +/** + * Allocate nl_msg and populate family and genl header details + */ +struct nl_msg *cld80211_msg_alloc(struct cld80211_ctx *ctx, int cmd, + struct nlattr **nla_data, int pid); + +/** + * Send nlmsg to driver and return; It doesn't wait for response + */ +int cld80211_send_msg(struct cld80211_ctx *ctx, struct nl_msg *nlmsg); + +/** + * Send nlmsg to driver and get response, if any + */ +int cld80211_send_recv_msg(struct cld80211_ctx *ctx, struct nl_msg *nlmsg, + int (*valid_handler)(struct nl_msg *, void *), + void *valid_data); + +/** + * Add membership for multicast group "mcgroup" to receive the messages + * sent to this group from driver + */ +int cld80211_add_mcast_group(struct cld80211_ctx *ctx, const char* mcgroup); + +/** + * Remove membership of multicast group "mcgroup" to stop receiving messages + * sent to this group from driver + */ +int cld80211_remove_mcast_group(struct cld80211_ctx *ctx, const char* mcgroup); + +/** + * Receive messages from driver on cld80211 family. Client can do + * a select()/poll() on the socket before calling this API. + * sock: nl_sock created for communication + * cb: nl callback context provided by client + * Returns corresponding errno when a failure happens while receiving nl msg + */ +int cld80211_recv_msg(struct nl_sock *sock, struct nl_cb *cb); + +/** + * Receive messages from driver on cld80211 family from the + * multicast groups subscribed + * timeout: Timeout in milliseconds for poll(); -1 is for infinite timeout. + * recv_multi_msg: Boolean flag to be sent false/true from client to indicate + * whether it wants to receive only one message or multiple + * messages from timeoutblock. + * false: Receive only one message and return + * true: Continue in the loop to receive multiple message till + * client explicitly sends exit via exit_cld80211_recv(). + * cbctx: Context provided by client, which is to be used when an + * nlmsg is received + * Returns corresponding errno when a failure happens while receiving nl msg + */ +int cld80211_recv(struct cld80211_ctx *ctx, int timeout, bool recv_multi_msg, + int (*valid_handler)(struct nl_msg *, void *), + void *cbctx); + +/** + * poll() is a blocking call on sock. Client has to unblock the poll() + * first to exit gracefully. + */ +void exit_cld80211_recv(struct cld80211_ctx *ctx); +#ifdef __cplusplus +} +#endif + +#endif |