neard/tools/nfctool/netlink.c

/*
 *
 *  Near Field Communication nfctool
 *
 *  Copyright (C) 2012  Intel Corporation. All rights reserved.
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License version 2 as
 *  published by the Free Software Foundation.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <errno.h>
#include <sys/socket.h>

#include <netlink/netlink.h>
#include <netlink/genl/genl.h>
#include <netlink/genl/family.h>
#include <netlink/genl/ctrl.h>

#include <glib.h>

#include <near/nfc_copy.h>

#include "nfctool.h"
#include "netlink.h"
#include "adapter.h"

#ifdef NEED_LIBNL_COMPAT
#define nl_sock nl_handle

static inline struct nl_handle *nl_socket_alloc(void)
{
	return nl_handle_alloc();
}

static inline void nl_socket_free(struct nl_sock *h)
{
	nl_handle_destroy(h);
}

#define NLE_MISSING_ATTR	14

static inline void __nl_perror(int error, const char *s)
{
	nl_perror(s);
}
#define nl_perror __nl_perror
#endif

struct handler_args {
	const char *group;
	int id;
};

struct nlnfc_state {
	struct nl_sock *cmd_sock;
	struct nl_sock *event_sock;
	int nfc_id;
	int mcid;
};

static struct nlnfc_state *nfc_state = NULL;

static GIOChannel *nl_gio_channel = NULL;

static GHashTable *handlers = NULL;

static int nl_error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err,
			 void *arg)
{
	int *ret = arg;

	DBG("");

	*ret = err->error;

	return NL_STOP;
}

static int nl_finish_handler(struct nl_msg *msg, void *arg)
{
	int *ret = arg;

	DBG("");

	*ret = 1;

	return NL_SKIP;
}

static int nl_ack_handler(struct nl_msg *msg, void *arg)
{
	int *ret = arg;

	DBG("");

	*ret = 1;

	return NL_STOP;
}

static int nl_send_msg(struct nl_sock *sock, struct nl_msg *msg,
		       int (*rx_handler)(struct nl_msg *, void *),
		       void *data)
{
	struct nl_cb *cb;
	int err, done;

	DBG("");

	cb = nl_cb_alloc(NL_CB_DEFAULT);
	if (!cb)
		return -ENOMEM;

	err = nl_send_auto_complete(sock, msg);
	if (err < 0) {
		nl_cb_put(cb);
		print_error("%s", strerror(err));

		return err;
	}

	err = done = 0;

	nl_cb_err(cb, NL_CB_CUSTOM, nl_error_handler, &err);
	nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, nl_finish_handler, &done);
	nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, nl_ack_handler, &done);

	if (rx_handler)
		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, rx_handler, data);

	while (err == 0 && done == 0)
		nl_recvmsgs(sock, cb);

	nl_cb_put(cb);

	return err;
}

static int nl_family_handler(struct nl_msg *msg, void *arg)
{
	struct handler_args *grp = arg;
	struct nlattr *tb[CTRL_ATTR_MAX + 1];
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
	struct nlattr *mcgrp;
	int rem_mcgrp;

	DBG("");

	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], rem_mcgrp) {
		struct nlattr *tb_mcgrp[CTRL_ATTR_MCAST_GRP_MAX + 1];

		nla_parse(tb_mcgrp, CTRL_ATTR_MCAST_GRP_MAX,
			  nla_data(mcgrp), nla_len(mcgrp), NULL);

		if (!tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME] ||
		    !tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID])
			continue;
		if (strncmp(nla_data(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]),
			    grp->group,
			    nla_len(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME])))
			continue;
		grp->id = nla_get_u32(tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]);
		break;
	}

	return NL_SKIP;
}

static int nl_get_multicast_id(struct nl_sock *sock, const char *family,
				const char *group)
{
	struct nl_msg *msg;
	int err = 0, ctrlid;
	struct handler_args grp = {
		.group = group,
		.id = -ENOENT,
	};

	DBG("");

	msg = nlmsg_alloc();
	if (!msg)
		return -ENOMEM;

	ctrlid = genl_ctrl_resolve(sock, "nlctrl");

	genlmsg_put(msg, 0, 0, ctrlid, 0,
		    0, CTRL_CMD_GETFAMILY, 0);

	NLA_PUT_STRING(msg, CTRL_ATTR_FAMILY_NAME, family);

	err = nl_send_msg(sock, msg, nl_family_handler, &grp);
	if (err)
		goto nla_put_failure;

	DBG("multicast id %d", grp.id);

	err = grp.id;

nla_put_failure:
	nlmsg_free(msg);

	return err;
}

static int nl_no_seq_check_cb(struct nl_msg *n, void *arg)
{
	DBG("");

	return NL_OK;
}

static int nl_get_params_handler(struct nl_msg *n, void *arg)
{
	struct nlmsghdr *nlh = nlmsg_hdr(n);
	struct nlattr *attrs[NFC_ATTR_MAX + 1];
	struct nfc_adapter *adapter = (struct nfc_adapter *)arg;
	guint32 idx;

	DBG("");

	genlmsg_parse(nlh, 0, attrs, NFC_ATTR_MAX, NULL);

	if (!attrs[NFC_ATTR_DEVICE_INDEX]) {
		nl_perror(NLE_MISSING_ATTR, "NFC_CMD_GET_PARAMS");
		return NL_STOP;
	}

	idx = nla_get_u32(attrs[NFC_ATTR_DEVICE_INDEX]);
	if (idx != adapter->idx) {
		nl_perror(NLE_MISSING_ATTR, "NFC_CMD_GET_PARAMS");
		return NL_STOP;
	}

	adapter->param_lto = nla_get_u8(attrs[NFC_ATTR_LLC_PARAM_LTO]);
	adapter->param_rw = nla_get_u8(attrs[NFC_ATTR_LLC_PARAM_RW]);
	adapter->param_miux = nla_get_u16(attrs[NFC_ATTR_LLC_PARAM_MIUX]);

	return NL_STOP;
}

int nl_get_params(struct nfc_adapter *adapter)
{
	struct nl_msg *msg;
	void *hdr;
	int err = 0;

	DBG("");

	if (!nfc_state || nfc_state->nfc_id < 0)
		return -ENODEV;

	msg = nlmsg_alloc();
	if (!msg)
		return -ENOMEM;

	hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
			  0, NFC_CMD_LLC_GET_PARAMS, NFC_GENL_VERSION);
	if (!hdr) {
		err = -EINVAL;
		goto nla_put_failure;
	}

	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, adapter->idx);

	err = nl_send_msg(nfc_state->cmd_sock, msg, nl_get_params_handler,
			  adapter);

	DBG("nl_send_msg returns %d", err);

nla_put_failure:
	nlmsg_free(msg);

	return err;
}

int nl_set_params(struct nfc_adapter *adapter, gint32 lto, gint32 rw,
		  gint32 miux)
{
	struct nl_msg *msg;
	void *hdr;
	int err = 0;

	DBG("");

	if (lto < 0 && rw < 0 && miux < 0)
		return -EINVAL;

	if (!nfc_state || nfc_state->nfc_id < 0)
		return -ENODEV;

	msg = nlmsg_alloc();
	if (!msg)
		return -ENOMEM;

	hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
			  0, NFC_CMD_LLC_SET_PARAMS, NFC_GENL_VERSION);
	if (!hdr) {
		err = -EINVAL;
		goto nla_put_failure;
	}

	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, adapter->idx);
	if (lto >= 0)
		NLA_PUT_U8(msg, NFC_ATTR_LLC_PARAM_LTO, (guint8)lto);
	if (rw >= 0)
		NLA_PUT_U8(msg, NFC_ATTR_LLC_PARAM_RW, (guint8)rw);
	if (miux >= 0)
		NLA_PUT_U16(msg, NFC_ATTR_LLC_PARAM_MIUX, (guint16)miux);

	err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);

	DBG("nl_send_msg returns %d", err);

nla_put_failure:
	nlmsg_free(msg);

	return err;
}

static int nl_get_targets_handler(struct nl_msg *n, void *arg)
{
	struct nlmsghdr *nlh = nlmsg_hdr(n);
	struct nlattr *attrs[NFC_ATTR_MAX + 1];
	guint32 target_idx, target_type, protocols;
	struct nfc_adapter *adapter;

	DBG("");

	adapter = (struct nfc_adapter *)arg;

	genlmsg_parse(nlh, 0, attrs, NFC_ATTR_MAX, NULL);

	target_idx = nla_get_u32(attrs[NFC_ATTR_TARGET_INDEX]);
	protocols = nla_get_u32(attrs[NFC_ATTR_PROTOCOLS]);

	if (protocols & NFC_PROTO_NFC_DEP_MASK)
		target_type = TARGET_TYPE_DEVICE;
	else
		target_type = TARGET_TYPE_TAG;

	adapter_add_target(adapter, target_type, target_idx);

	return 0;
}

int nl_get_targets(struct nfc_adapter *adapter)
{
	struct nl_msg *msg;
	void *hdr;
	int err;

	DBG("");

	if (!nfc_state || nfc_state->nfc_id < 0)
		return -ENODEV;

	msg = nlmsg_alloc();
	if (!msg)
		return -ENOMEM;

	hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
			  NLM_F_DUMP, NFC_CMD_GET_TARGET, NFC_GENL_VERSION);
	if (!hdr) {
		err = -EINVAL;
		goto nla_put_failure;
	}

	err = -EMSGSIZE;

	g_slist_free(adapter->tags);
	adapter->tags = NULL;

	g_slist_free(adapter->devices);
	adapter->devices = NULL;

	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, adapter->idx);

	err = nl_send_msg(nfc_state->cmd_sock, msg,
			  nl_get_targets_handler, adapter);

	DBG("nl_send_msg returns %d", err);

nla_put_failure:
	nlmsg_free(msg);

	return err;
}

static int nl_get_devices_handler(struct nl_msg *n, void *arg)
{
	struct nlmsghdr *nlh = nlmsg_hdr(n);
	struct nlattr *attrs[NFC_ATTR_MAX + 1];
	guint32 idx, protocols = 0;
	guint8 powered = 0;
	guint8 rf_mode = NFC_RF_NONE;

	DBG("");

	genlmsg_parse(nlh, 0, attrs, NFC_ATTR_MAX, NULL);

	if (!attrs[NFC_ATTR_DEVICE_INDEX]) {
		nl_perror(NLE_MISSING_ATTR, "NFC_CMD_GET_DEVICE");
		return NL_STOP;
	}

	idx = nla_get_u32(attrs[NFC_ATTR_DEVICE_INDEX]);

	if (attrs[NFC_ATTR_PROTOCOLS])
		protocols = nla_get_u32(attrs[NFC_ATTR_PROTOCOLS]);

	if (attrs[NFC_ATTR_RF_MODE])
		rf_mode = nla_get_u8(attrs[NFC_ATTR_RF_MODE]);

	if (attrs[NFC_ATTR_DEVICE_POWERED])
		powered = nla_get_u8(attrs[NFC_ATTR_DEVICE_POWERED]);

	adapter_add(idx, protocols, powered, rf_mode);

	return NL_SKIP;
}

int nl_get_devices(void)
{
	struct nl_msg *msg;
	void *hdr;
	int err;

	DBG("");

	if (!nfc_state || nfc_state->nfc_id < 0)
		return -ENODEV;

	msg = nlmsg_alloc();
	if (!msg)
		return -ENOMEM;

	hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
			  NLM_F_DUMP, NFC_CMD_GET_DEVICE, NFC_GENL_VERSION);
	if (!hdr) {
		err = -EINVAL;
		goto out;
	}

	err = nl_send_msg(nfc_state->cmd_sock, msg, nl_get_devices_handler,
			  NULL);

	DBG("nl_send_msg returns %d", err);

out:
	nlmsg_free(msg);

	return err;
}

int nl_send_dep_link_up(guint32 idx, guint32 target_idx)
{
	struct nl_msg *msg;
	void *hdr;
	int err;

	DBG("");

	msg = nlmsg_alloc();
	if (!msg)
		return -ENOMEM;

	hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
			  NLM_F_REQUEST, NFC_CMD_DEP_LINK_UP, NFC_GENL_VERSION);
	if (!hdr) {
		err = -EINVAL;
		goto nla_put_failure;
	}

	err = -EMSGSIZE;

	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, idx);
	NLA_PUT_U32(msg, NFC_ATTR_TARGET_INDEX, target_idx);
	NLA_PUT_U8(msg, NFC_ATTR_COMM_MODE, NFC_COMM_ACTIVE);
	NLA_PUT_U8(msg, NFC_ATTR_RF_MODE, NFC_RF_INITIATOR);

	err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);

nla_put_failure:
	nlmsg_free(msg);

	return err;
}

int nl_start_poll(struct nfc_adapter *adapter, guint8 mode)
{
	struct nl_msg *msg;
	void *hdr;
	int err;
	guint32 im_protocols = 0;
	guint32 tm_protocols = 0;

	if ((mode & POLLING_MODE_INITIATOR) == POLLING_MODE_INITIATOR)
		im_protocols = adapter->protocols;

	if ((mode & POLLING_MODE_TARGET) == POLLING_MODE_TARGET)
		tm_protocols = adapter->protocols;

	DBG("IM protos 0x%x TM protos 0x%x", im_protocols, tm_protocols);

	msg = nlmsg_alloc();
	if (!msg)
		return -ENOMEM;

	hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
			  NLM_F_REQUEST, NFC_CMD_START_POLL, NFC_GENL_VERSION);
	if (!hdr) {
		err = -EINVAL;
		goto nla_put_failure;
	}

	err = -EMSGSIZE;

	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, adapter->idx);
	if (im_protocols != 0) {
		NLA_PUT_U32(msg, NFC_ATTR_IM_PROTOCOLS, im_protocols);
		NLA_PUT_U32(msg, NFC_ATTR_PROTOCOLS, im_protocols);
	}
	if (tm_protocols != 0)
		NLA_PUT_U32(msg, NFC_ATTR_TM_PROTOCOLS, tm_protocols);

	err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);

	if (err == 0)
		adapter->polling = TRUE;

nla_put_failure:
	nlmsg_free(msg);

	return err;
}

int nl_set_powered(struct nfc_adapter *adapter, bool powered)
{
	struct nl_msg *msg;
	void *hdr;
	int err;
	uint8_t cmd;

	DBG("");

	msg = nlmsg_alloc();
	if (!msg)
		return -ENOMEM;

	if (powered)
		cmd = NFC_CMD_DEV_UP;
	else
		cmd = NFC_CMD_DEV_DOWN;

	hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
			NLM_F_REQUEST, cmd, NFC_GENL_VERSION);
	if (!hdr) {
		err = -EINVAL;
		goto nla_put_failure;
	}

	err = -EMSGSIZE;

	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, adapter->idx);

	err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);

nla_put_failure:
	nlmsg_free(msg);

	return err;
}

int nl_send_sdreq(struct nfc_adapter *adapter, GSList *uris)
{
	struct nl_msg *msg;
	void *hdr;
	GSList *uri;
	struct nlattr *sdp_attr, *uri_attr;
	int err = 0, i;

	DBG("");

	if (!nfc_state || nfc_state->nfc_id < 0)
		return -ENODEV;

	msg = nlmsg_alloc();
	if (!msg)
		return -ENOMEM;

	hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
			  NLM_F_REQUEST, NFC_CMD_LLC_SDREQ, NFC_GENL_VERSION);
	if (!hdr) {
		err = -EINVAL;
		goto nla_put_failure;
	}

	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, adapter->idx);

	sdp_attr = nla_nest_start(msg, NFC_ATTR_LLC_SDP);
	if (!sdp_attr) {
		err = -ENOMEM;
		goto nla_put_failure;
	}

	i = 1;
	uri = uris;
	while (uri) {
		uri_attr = nla_nest_start(msg, i);

		if (!uri_attr) {
			err = -ENOMEM;
			goto nla_put_failure;
		}

		NLA_PUT_STRING(msg, NFC_SDP_ATTR_URI, uri->data);

		nla_nest_end(msg, uri_attr);

		uri = g_slist_next(uri);

		i++;
	}

	nla_nest_end(msg, sdp_attr);

	err = nl_send_msg(nfc_state->event_sock, msg, NULL, NULL);

	DBG("nl_send_msg returns %d", err);

nla_put_failure:
	nlmsg_free(msg);

	return err;
}

static int nl_nfc_send_sdres_event(guint32 idx, struct nlattr *sdres_attr,
							nfc_event_cb_t handler)
{
	GSList *sdres_list = NULL;
	struct nfc_snl *sdres;
	struct nlattr *attr;
	struct nlattr *sdp_attrs[NFC_SDP_ATTR_MAX + 1];
	int rem, err = 0;
	size_t uri_len;

	nla_for_each_nested(attr, sdres_attr, rem) {
		err = nla_parse_nested(sdp_attrs, NFC_SDP_ATTR_MAX,
					attr, NULL);

		if (err != 0) {
			DBG("nla_parse_nested returned %d\n", err);
			continue;
		}

		if (!sdp_attrs[NFC_SDP_ATTR_URI] ||
		    !sdp_attrs[NFC_SDP_ATTR_SAP]) {
			print_error("Missing attribute in reply");
			continue;
		}

		uri_len = nla_strlcpy(NULL, sdp_attrs[NFC_SDP_ATTR_URI], 0);

		uri_len++;

		sdres = nfctool_snl_alloc(uri_len);

		nla_strlcpy(sdres->uri, sdp_attrs[NFC_SDP_ATTR_URI], uri_len);

		sdres->sap = nla_get_u8(sdp_attrs[NFC_SDP_ATTR_SAP]);

		DBG("URI: %s, sap: %d\n", sdres->uri, sdres->sap);

		sdres_list = g_slist_append(sdres_list, sdres);
	}

	handler(NFC_EVENT_LLC_SDRES, idx, sdres_list);

	g_slist_free_full(sdres_list, (GDestroyNotify)nfctool_sdres_free);

	return err;
}

int nl_fw_download(struct nfc_adapter *adapter, gchar *fw_filename)
{
	struct nl_msg *msg;
	void *hdr;
	int err;

	DBG("");

	if (nfc_state == NULL || nfc_state->nfc_id < 0)
		return -ENODEV;

	msg = nlmsg_alloc();
	if (msg == NULL)
		return -ENOMEM;

	err = -EINVAL;

	hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
			  NLM_F_REQUEST, NFC_CMD_FW_DOWNLOAD, NFC_GENL_VERSION);
	if (hdr == NULL)
		goto nla_put_failure;

	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, adapter->idx);

	NLA_PUT_STRING(msg, NFC_ATTR_FIRMWARE_NAME, fw_filename);

	err = nl_send_msg(nfc_state->event_sock, msg, NULL, NULL);

nla_put_failure:
	nlmsg_free(msg);

	return err;
}

int nl_send_vendor_cmd(struct nfc_adapter *adapter, guint32 oui, guint32 sub_cmd,
			guint32 data_len, guint8 *data)
{
	struct nl_msg *msg;
	struct nl_data *nl_data_buffer;
	void *hdr;
	int err;

	DBG("oui 0x%08x sub_cmd 0x%08x data_len %ld", oui, sub_cmd, data_len);

	msg = nlmsg_alloc();
	if (!msg)
		return -ENOMEM;

	hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
				NLM_F_REQUEST, NFC_CMD_VENDOR, NFC_GENL_VERSION);
	if (!hdr) {
		err = -EINVAL;
		goto nla_put_failure;
	}

	err = -EMSGSIZE;

	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, adapter->idx);
	NLA_PUT_U32(msg, NFC_ATTR_VENDOR_ID, oui);
	NLA_PUT_U32(msg, NFC_ATTR_VENDOR_SUBCMD, sub_cmd);

	if (data_len > 0) {
		nl_data_buffer = nl_data_alloc(data, data_len);
		if (!nl_data_buffer) {
			err = -ENOMEM;
			goto nla_put_failure;
		}
		err = nla_put_data(msg, NFC_ATTR_VENDOR_DATA, nl_data_buffer);
	} else {
		nl_data_buffer = NULL;
		err = nla_put(msg, NFC_ATTR_VENDOR_DATA, 0, NULL);
	}
	if (err < 0)
		goto nla_fail_free_data;

	err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);

nla_fail_free_data:
	if (nl_data_buffer)
		nl_data_free(nl_data_buffer);

nla_put_failure:
	nlmsg_free(msg);

	return err;
}

static int nl_nfc_event_cb(struct nl_msg *n, void *arg)
{
	guint32 idx = INVALID_ADAPTER_IDX;
	struct nlattr *attr[NFC_ATTR_MAX + 1];
	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(n));
	guint32 cmd = gnlh->cmd;
	nfc_event_cb_t cb = NULL;

	DBG("Received cmd %d", gnlh->cmd);

	nla_parse(attr, NFC_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
		genlmsg_attrlen(gnlh, 0), NULL);

	if (attr[NFC_ATTR_DEVICE_INDEX])
		idx = nla_get_u32(attr[NFC_ATTR_DEVICE_INDEX]);

	if (handlers)
		cb = g_hash_table_lookup(handlers, GINT_TO_POINTER(cmd));

	if (!cb)
		return NL_SKIP;

	switch (cmd) {
	case NFC_EVENT_LLC_SDRES:
		DBG("Received NFC_EVENT_LLC_SDRES\n");
		nl_nfc_send_sdres_event(idx, attr[NFC_ATTR_LLC_SDP], cb);
		break;

	default:
		cb(gnlh->cmd, idx, &attr);
		break;
	}

	return NL_SKIP;
}

static gboolean nl_gio_handler(GIOChannel *channel,
			       GIOCondition cond, gpointer data)
{
	struct nl_cb *cb;
	struct nlnfc_state *state = data;

	if (cond & (G_IO_NVAL | G_IO_HUP | G_IO_ERR))
		return FALSE;

	cb = nl_cb_alloc(NL_CB_VERBOSE);
	if (!cb)
		return TRUE;

	nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, nl_no_seq_check_cb, NULL);
	nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, nl_nfc_event_cb, data);

	nl_recvmsgs(state->event_sock, cb);

	nl_cb_put(cb);

	return TRUE;
}

void nl_add_event_handler(guint8 cmd, nfc_event_cb_t cb)
{
	guint32 c = cmd;

	if (handlers)
		g_hash_table_replace(handlers, GINT_TO_POINTER(c), cb);
}

void nl_cleanup(void)
{
	if (nl_gio_channel) {
		g_io_channel_shutdown(nl_gio_channel, TRUE, NULL);
		g_io_channel_unref(nl_gio_channel);
		nl_gio_channel = NULL;
	}

	if (nfc_state) {
		if (nfc_state->cmd_sock)
			nl_socket_free(nfc_state->cmd_sock);

		if (nfc_state->event_sock)
			nl_socket_free(nfc_state->event_sock);

		g_free(nfc_state);
		nfc_state = NULL;
	}

	if (handlers)
		g_hash_table_destroy(handlers);
}

int nl_init(void)
{
	int err;
	int fd;

	DBG("");

	nfc_state = g_malloc0(sizeof(struct nlnfc_state));

	nfc_state->cmd_sock = nl_socket_alloc();
	if (!nfc_state->cmd_sock) {
		print_error("Failed to allocate NFC netlink socket");
		err = -ENOMEM;
		goto exit_err;
	}

	nfc_state->event_sock = nl_socket_alloc();
	if (!nfc_state->event_sock) {
		print_error("Failed to allocate NFC netlink socket");
		err = -ENOMEM;
		goto exit_err;
	}

	if (genl_connect(nfc_state->cmd_sock)) {
		print_error("Failed to connect to generic netlink");
		err = -ENOLINK;
		goto exit_err;
	}

	if (genl_connect(nfc_state->event_sock)) {
		print_error("Failed to connect to generic netlink");
		err = -ENOLINK;
		goto exit_err;
	}

	nfc_state->nfc_id = genl_ctrl_resolve(nfc_state->cmd_sock, "nfc");
	if (nfc_state->nfc_id < 0) {
		print_error("Unable to find NFC netlink family");
		err = -ENOENT;
		goto exit_err;
	}

	nfc_state->mcid = nl_get_multicast_id(nfc_state->cmd_sock,
					      NFC_GENL_NAME,
					      NFC_GENL_MCAST_EVENT_NAME);
	if (nfc_state->mcid <= 0) {
		print_error("Wrong mcast id %d", nfc_state->mcid);
		err = nfc_state->mcid;
		goto exit_err;
	}

	err = nl_socket_add_membership(nfc_state->event_sock, nfc_state->mcid);
	if (err) {
		print_error("Error adding nl socket to membership");
		goto exit_err;
	}

	handlers = g_hash_table_new(g_direct_hash, g_direct_equal);

	fd = nl_socket_get_fd(nfc_state->event_sock);
	nl_gio_channel = g_io_channel_unix_new(fd);
	g_io_channel_set_close_on_unref(nl_gio_channel, TRUE);

	g_io_channel_set_encoding(nl_gio_channel, NULL, NULL);
	g_io_channel_set_buffered(nl_gio_channel, FALSE);

	g_io_add_watch(nl_gio_channel,
		       G_IO_IN | G_IO_NVAL | G_IO_HUP | G_IO_ERR,
		       nl_gio_handler, nfc_state);

	return 0;

exit_err:
	nl_cleanup();

	print_error("netlink init failed");

	return err;
}