Replace missing thermal-samplingHEADmaster

Signed-off-by: Daniel Lezcano <daniel.lezcano@linaro.org>

2 files changed, 249 insertions, 534 deletions

diff --git a/thermal-genl.c b/thermal-genl.c
deleted file mode 100644
index 82966c2..0000000
--- a/thermal-genl.c
+++ /dev/null
@@ -1,534 +0,0 @@
-#include <stdio.h>
-#include <errno.h>
-#include <libnl3/netlink/genl/genl.h>
-#include <libnl3/netlink/genl/mngt.h>
-#include <libnl3/netlink/genl/ctrl.h>
-#include <netlink/netlink.h>
-#include <netlink/genl/genl.h>
-#include <netlink/genl/ctrl.h>
-
-#include "thermal.h"
-
-static int nl_error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err,
-			    void *arg)
-{
-	int *ret = arg;
-
-	*ret = err->error;
-
-	printf("%s\n", __func__);
-
-	return NL_STOP;
-}
-
-static int nl_finish_handler(struct nl_msg *msg, void *arg)
-{
-	int *ret = arg;
-
-	*ret = 1;
-
-	/* printf("%s\n", __func__); */
-		
-	return NL_OK;
-}
-
-static int nl_ack_handler(struct nl_msg *msg, void *arg)
-{
-	int *ret = arg;
-
-	*ret = 1;
-
-	printf("%s\n", __func__);
-		
-	return NL_OK;
-}
-
-static int nl_overrun_handler(struct nl_msg *msg, void *arg)
-{
-	printf("%s\n", __func__);
-	
-	return NL_SKIP;
-}
-
-static int nl_skipped_handler(struct nl_msg *msg, void *arg)
-{
-	printf("%s\n", __func__);
-	
-	return NL_SKIP;
-}
-
-static int nl_seq_check_handler(struct nl_msg *msg, void *arg)
-{
-	printf("%s\n", __func__);
-	
-	return NL_OK;
-}
-
-int show_tz(struct genl_info *info)
-{
-	struct nlattr *attr;
-	int rem;
-
-	nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_TZ], rem) {
-
-		if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_ID)
-			printf("Thermal zone id: %d\n", nla_get_u32(attr));
-		
-		if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_NAME)
-			printf("Thermal zone name: %s\n",
-			       nla_get_string(attr));
-	}
-
-	return 0;
-}
-
-int show_cdev(struct genl_info *info)
-{
-	struct nlattr *attr;
-	int rem;
-
-	nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_CDEV], rem) {
-
-		if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_ID)
-			printf("Cooling device id: %d\n", nla_get_u32(attr));
-
-		if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_NAME)
-			printf("Thermal zone name: %s\n", nla_get_string(attr));
-	}
-
-	return 0;
-}
-
-int show_trip(struct genl_info *info)
-{
-	struct nlattr *attr;
-	int rem;
-
-	if (info->attrs[THERMAL_GENL_ATTR_TZ_TRIP])
-		printf("Trip zone !!!\n"); 
-
-	nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_TZ_TRIP], rem) {
-
-		if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_NAME)
-			printf("Thermal zone name: %s\n",
-			       nla_get_string(attr));
-		
-		if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_ID)
-			printf("Trip id: %d\n", nla_get_u32(attr));
-
-		if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_TYPE)
-			printf("Trip type: %d\n", nla_get_u32(attr));
-
-		if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_TEMP)
-			printf("Trip temp: %d\n", nla_get_u32(attr));
-
-		if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_HYST)
-			printf("Trip hyst: %d\n", nla_get_u32(attr));
-	}
-	
-	return 0;
-}
-
-int show_temp(struct genl_info *info)
-{
-	if (info->attrs[THERMAL_GENL_ATTR_TZ_TEMP])
-		printf("Thermal zone0 temperature: %d\n",
-		       nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_TEMP]));
-
-	return 0;
-}
-
-static struct nla_policy thermal_genl_policy[THERMAL_GENL_ATTR_MAX + 1] = {
-	/* Thermal zone */
-	[THERMAL_GENL_ATTR_TZ]                  = { .type = NLA_NESTED },
-	[THERMAL_GENL_ATTR_TZ_ID]               = { .type = NLA_U32 },
-	[THERMAL_GENL_ATTR_TZ_TEMP]             = { .type = NLA_U32 },
-	[THERMAL_GENL_ATTR_TZ_TRIP]             = { .type = NLA_NESTED },
-	[THERMAL_GENL_ATTR_TZ_TRIP_ID]          = { .type = NLA_U32 },
-	[THERMAL_GENL_ATTR_TZ_TRIP_TEMP]        = { .type = NLA_U32 },
-	[THERMAL_GENL_ATTR_TZ_TRIP_TYPE]        = { .type = NLA_U32 },
-	[THERMAL_GENL_ATTR_TZ_TRIP_HYST]        = { .type = NLA_U32 },
-	[THERMAL_GENL_ATTR_TZ_MODE]             = { .type = NLA_U32 },
-	[THERMAL_GENL_ATTR_TZ_CDEV_WEIGHT]      = { .type = NLA_U32 },
-	[THERMAL_GENL_ATTR_TZ_NAME]             = { .type = NLA_STRING },
-
-	/* Governor(s) */
-	[THERMAL_GENL_ATTR_TZ_GOV]              = { .type = NLA_NESTED },
-	[THERMAL_GENL_ATTR_TZ_GOV_NAME]         = { .type = NLA_STRING },
-
-	/* Cooling devices */
-	[THERMAL_GENL_ATTR_CDEV]                = { .type = NLA_NESTED },
-	[THERMAL_GENL_ATTR_CDEV_ID]             = { .type = NLA_U32 },
-	[THERMAL_GENL_ATTR_CDEV_CUR_STATE]      = { .type = NLA_U32 },
-	[THERMAL_GENL_ATTR_CDEV_MAX_STATE]      = { .type = NLA_U32 },
-	[THERMAL_GENL_ATTR_CDEV_MIN_STATE]      = { .type = NLA_U32 },
-	[THERMAL_GENL_ATTR_CDEV_NAME]           = { .type = NLA_STRING },
-};
-
-static int handle_sampling(struct nl_msg *n, void *arg)
-{
-	struct nlmsghdr *nlh = nlmsg_hdr(n);
-	struct genlmsghdr *genlhdr = genlmsg_hdr(nlh);
-	struct nlattr *attrs[THERMAL_GENL_ATTR_MAX + 1];
-	
-	genlmsg_parse(nlh, 0, attrs, THERMAL_GENL_ATTR_MAX, NULL);
-	
-	if (genlhdr->cmd == THERMAL_GENL_SAMPLING_TEMP) {
-		printf("THERMAL_GENL_SAMPLING_TEMP\n");
-
-		if (attrs[THERMAL_GENL_ATTR_TZ_ID])
-			printf("Thermal zone %d\n",
-			       nla_get_u32(attrs[THERMAL_GENL_ATTR_TZ_ID]));
-
-		if (attrs[THERMAL_GENL_ATTR_TZ_TEMP])
-			printf("Thermal zone temp %d\n",
-			       nla_get_u32(attrs[THERMAL_GENL_ATTR_TZ_TEMP]));
-	}
-
-	return 0;
-}
-
-static int handle_netlink(struct nl_cache_ops *unused, struct genl_cmd *cmd,
-			  struct genl_info *info, void *arg)
-{
-	switch (cmd->c_id) {
-
-	case THERMAL_GENL_CMD_TZ_GET:
-		printf("THERMAL_GENL_CMD_TZ_GET\n");
-		show_tz(info);
-		break;
-
-	case THERMAL_GENL_CMD_CDEV_GET:
-		printf("THERMAL_GENL_CMD_CDEV_GET\n");
-		show_cdev(info);
-		break;
-		
-	case THERMAL_GENL_CMD_TZ_GET_TEMP:
-		printf("THERMAL_GENL_CMD_TZ_GET_TEMP\n");
-		show_temp(info);
-		break;
-
-	case THERMAL_GENL_CMD_TZ_GET_TRIP:
-		printf("THERMAL_GENL_CMD_TZ_GET_TRIP\n");
-		show_trip(info);
-		break;
-	};
-
-	return 0;
-}
-
-static struct genl_cmd thermal_cmds[] = {
-	{
-		.c_id		= THERMAL_GENL_CMD_TZ_GET,
-		.c_name		= "List thermal zones",
-		.c_msg_parser	= handle_netlink,
-		.c_maxattr	= THERMAL_GENL_ATTR_MAX,
-		.c_attr_policy	= thermal_genl_policy,
-	},
-	{
-		.c_id		= THERMAL_GENL_CMD_TZ_GET_TEMP,
-		.c_name		= "Get thermal zone temperature",
-		.c_msg_parser	= handle_netlink,
-		.c_maxattr	= THERMAL_GENL_ATTR_MAX,
-		.c_attr_policy	= thermal_genl_policy,
-	},
-	{
-		.c_id		= THERMAL_GENL_CMD_TZ_GET_TRIP,
-		.c_name		= "Get thermal zone trip points",
-		.c_msg_parser	= handle_netlink,
-		.c_maxattr	= THERMAL_GENL_ATTR_MAX,
-		.c_attr_policy	= thermal_genl_policy,
-	},
-	{
-		.c_id		= THERMAL_GENL_CMD_CDEV_GET,
-		.c_name		= "Get cooling devices",
-		.c_msg_parser	= handle_netlink,
-		.c_maxattr	= THERMAL_GENL_ATTR_MAX,
-		.c_attr_policy	= thermal_genl_policy,
-	},
-};
-
-#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))
-
-static struct genl_ops thermal_cmd_ops = {
-	.o_name		= "thermal",
-	.o_cmds		= thermal_cmds,
-	.o_ncmds	= ARRAY_SIZE(thermal_cmds),
-};
-
-static int thermal_genl_auto(struct nl_sock *sock, int family, int id, int cmd, int flags)
-{
-	struct nl_msg *msg;
-	void *hdr;
-
-	msg = nlmsg_alloc();
-	if (!msg) {
-		fprintf(stderr, "Failed to allocate message\n");
-		return -1;
-	}
-
-	hdr = genlmsg_put(msg, NL_AUTO_PORT, NL_AUTO_SEQ, family, 0,
-			  flags, cmd, THERMAL_GENL_VERSION);
-	if (!hdr) {
-		fprintf(stderr, "Failed to set message\n");
-		return -1;
-	}
-
-	if (id >= 0 && nla_put_u32(msg, THERMAL_GENL_ATTR_TZ_ID, id)) {
-		fprintf(stderr, "Failed to set tz id\n");
-		return -1;
-	}
-
-	if (nl_send_auto_complete(sock, msg) < 0) {
-		fprintf(stderr, "Failed to nl_send_auto_complete\n");
-		return -1;
-	}
-
-	{
-		struct nl_cb *cb;
-		int err = 0, done = 0;
-
-		cb = nl_cb_alloc(NL_CB_DEFAULT);
-
-		// 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);
-		// nl_cb_set(cb, NL_CB_OVERRUN, NL_CB_CUSTOM, nl_overrun_handler, &done);
-		// nl_cb_set(cb, NL_CB_SKIPPED, NL_CB_CUSTOM, nl_skipped_handler, &done);
-		nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, nl_seq_check_handler, &done);
-		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, genl_handle_msg, NULL);
-
-		while (!done && !err)
-			nl_recvmsgs(sock, cb);
-	}
-
-	return 0;
-}
-
-struct handler_args {
-	const char *group;
-	int id;
-};
-
-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;
-
-	cb = nl_cb_alloc(NL_CB_DEFAULT);
-	if (cb == NULL)
-		return -ENOMEM;
-
-	err = nl_send_auto_complete(sock, msg);
-	if (err < 0) {
-		nl_cb_put(cb);
-		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 != NULL)
-		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;
-
-	nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
-		  genlmsg_attrlen(gnlh, 0), NULL);
-
-	if (!tb[CTRL_ATTR_MCAST_GROUPS]) {
-		fprintf(stderr, "Multicast group not found\n");
-		return -1;
-	}
-
-	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 0;
-}
-
-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,
-	};
-
-	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;
-
-	err = grp.id;
-
-nla_put_failure:
-	nlmsg_free(msg);
-	return err;
-}
-
-int main(int argc, char *argv[])
-{
-	struct nl_sock *sk_cmd;
-	struct nl_sock *sk_sample;
-	int ret, family;
-	int mcid;
-
-	nl_debug = 0;
-
-	sk_cmd = nl_socket_alloc();
-	if (!sk_cmd) {
-		perror("nl_socket_alloc");
-		return -1;
-	}
-
-	nl_socket_modify_cb(sk_cmd, NL_CB_VALID, NL_CB_CUSTOM,
-			    genl_handle_msg, NULL);
-
-	ret = genl_connect(sk_cmd);
-	if (ret) {
-		fprintf(stderr, "genl_connect: %d\n", ret);
-		return -1;
-	}
-
-	ret = genl_register_family(&thermal_cmd_ops);
-	if (ret) {
-		fprintf(stderr, "genl_register_family: %d\n", ret);
-		return -1;
-	}
-
-	ret = genl_ops_resolve(sk_cmd, &thermal_cmd_ops);
-	if (ret) {
-		fprintf(stderr, "genl_ops_resolve: %d\n", ret);
-		return -1;
-	}
-
-	family = genl_ctrl_resolve(sk_cmd, "nlctrl");
-	if (family != GENL_ID_CTRL) {
-		fprintf(stderr, "genl_ctrl_resolve: %d\n", family);
-		return -1;
-	}
-
-	goto listen;
-	
-	ret = thermal_genl_auto(sk_cmd, thermal_cmd_ops.o_id, -1,
-				THERMAL_GENL_CMD_TZ_GET,
-				NLM_F_DUMP | NLM_F_ACK);
-	if (ret < 0) {
-		fprintf(stderr, "Failed to thermal_genl_send: %d\n", ret);
-		return -1;
-	}
-
-	ret = thermal_genl_auto(sk_cmd, thermal_cmd_ops.o_id, -1,
-				THERMAL_GENL_CMD_CDEV_GET,
-				NLM_F_DUMP | NLM_F_ACK);
-	if (ret < 0) {
-		fprintf(stderr, "Failed to thermal_genl_send: %d\n", ret);
-		return -1;
-	}
-
-	ret = thermal_genl_auto(sk_cmd, thermal_cmd_ops.o_id, 1,
-				THERMAL_GENL_CMD_TZ_GET_TEMP, 0);
-	if (ret < 0) {
-		fprintf(stderr, "Failed to thermal_genl_send: %d\n", ret);
-		return -1;
-	}
-
-	ret = thermal_genl_auto(sk_cmd, thermal_cmd_ops.o_id, 1,
-				THERMAL_GENL_CMD_TZ_GET_TRIP, 0);
-	if (ret < 0) {
-		fprintf(stderr, "Failed to thermal_genl_send: %d\n", ret);
-		return -1;
-	}
-listen:
-	/* Sampling receiving */
-	sk_sample = nl_socket_alloc();
-	if (!sk_sample) {
-		fprintf(stderr, "nl_socket_alloc failed\n");
-		return -1;
-	}
-	
-	if (genl_connect(sk_sample)) {
-		fprintf(stderr, "genl_connect(sk_sample) failed\n");
-		return -1;
-	}
-
-	mcid = nl_get_multicast_id(sk_sample, THERMAL_GENL_FAMILY_NAME,
-				   THERMAL_GENL_SAMPLING_GROUP_NAME);
-	if (mcid < 0) {
-		fprintf(stderr, "nl_get_multicast_id failed\n");
-		return -1;
-	}
-
-	if (nl_socket_add_membership(sk_sample, mcid)) {
-		fprintf(stderr, "nl_socket_add_membership failed");
-		return -1;
-	}
-
-	{
-		struct nl_cb *cb;
-		int err = 0, done = 0;
-
-		cb = nl_cb_alloc(NL_CB_DEFAULT);
-
-		// 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);
-		// nl_cb_set(cb, NL_CB_OVERRUN, NL_CB_CUSTOM, nl_overrun_handler, &done);
-		// nl_cb_set(cb, NL_CB_SKIPPED, NL_CB_CUSTOM, nl_skipped_handler, &done);
-		nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, nl_seq_check_handler, &done);
-		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, handle_sampling, NULL);
-
-		while (!done && !err)
-			nl_recvmsgs(sk_sample, cb);
-	}
-	
-	return 0;
-}
diff --git a/thermal-sampling.c b/thermal-sampling.c
new file mode 100644
index 0000000..4c18811
--- /dev/null
+++ b/thermal-sampling.c
@@ -0,0 +1,249 @@
+#include <stdio.h>
+#include <errno.h>
+#include <libnl3/netlink/genl/genl.h>
+#include <libnl3/netlink/genl/mngt.h>
+#include <libnl3/netlink/genl/ctrl.h>
+#include <netlink/netlink.h>
+#include <netlink/genl/genl.h>
+#include <netlink/genl/ctrl.h>
+
+#include "thermal.h"
+
+static int nl_error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err,
+			    void *arg)
+{
+	int *ret = arg;
+
+	*ret = err->error;
+
+	printf("%s\n", __func__);
+
+	return NL_STOP;
+}
+
+static int nl_finish_handler(struct nl_msg *msg, void *arg)
+{
+	int *ret = arg;
+
+	*ret = 1;
+
+	/* printf("%s\n", __func__); */
+		
+	return NL_OK;
+}
+
+static int nl_ack_handler(struct nl_msg *msg, void *arg)
+{
+	int *ret = arg;
+
+	*ret = 1;
+
+	printf("%s\n", __func__);
+		
+	return NL_OK;
+}
+
+static int nl_overrun_handler(struct nl_msg *msg, void *arg)
+{
+	printf("%s\n", __func__);
+	
+	return NL_SKIP;
+}
+
+static int nl_skipped_handler(struct nl_msg *msg, void *arg)
+{
+	printf("%s\n", __func__);
+	
+	return NL_SKIP;
+}
+
+static int nl_seq_check_handler(struct nl_msg *msg, void *arg)
+{
+	printf("%s\n", __func__);
+	
+	return NL_OK;
+}
+
+static int handle_sampling(struct nl_msg *n, void *arg)
+{
+	struct nlmsghdr *nlh = nlmsg_hdr(n);
+	struct genlmsghdr *genlhdr = genlmsg_hdr(nlh);
+	struct nlattr *attrs[THERMAL_GENL_ATTR_MAX + 1];
+
+	genlmsg_parse(nlh, 0, attrs, THERMAL_GENL_ATTR_MAX, NULL);
+	
+	if (genlhdr->cmd == THERMAL_GENL_SAMPLING_TEMP) {
+		printf("THERMAL_GENL_SAMPLING_TEMP\n");
+
+		if (attrs[THERMAL_GENL_ATTR_TZ_ID])
+			printf("Thermal zone %d\n",
+			       nla_get_u32(attrs[THERMAL_GENL_ATTR_TZ_ID]));
+
+		if (attrs[THERMAL_GENL_ATTR_TZ_TEMP])
+			printf("Thermal zone temp %d\n",
+			       nla_get_u32(attrs[THERMAL_GENL_ATTR_TZ_TEMP]));
+	}
+
+	return 0;
+}
+
+struct handler_args {
+	const char *group;
+	int id;
+};
+
+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;
+
+	cb = nl_cb_alloc(NL_CB_DEFAULT);
+	if (cb == NULL)
+		return -ENOMEM;
+
+	err = nl_send_auto_complete(sock, msg);
+	if (err < 0) {
+		nl_cb_put(cb);
+		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 != NULL)
+		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;
+
+	nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
+		  genlmsg_attrlen(gnlh, 0), NULL);
+
+	if (!tb[CTRL_ATTR_MCAST_GROUPS]) {
+		fprintf(stderr, "Multicast group not found\n");
+		return -1;
+	}
+
+	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 0;
+}
+
+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,
+	};
+
+	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;
+
+	err = grp.id;
+
+nla_put_failure:
+	nlmsg_free(msg);
+	return err;
+}
+
+int main(int argc, char *argv[])
+{
+	struct nl_sock *sk_sample;
+	int mcid;
+
+	nl_debug = 0;
+
+	/* Sampling receiving */
+	sk_sample = nl_socket_alloc();
+	if (!sk_sample) {
+		fprintf(stderr, "nl_socket_alloc failed\n");
+		return -1;
+	}
+	
+	if (genl_connect(sk_sample)) {
+		fprintf(stderr, "genl_connect(sk_sample) failed\n");
+		return -1;
+	}
+
+	mcid = nl_get_multicast_id(sk_sample, THERMAL_GENL_FAMILY_NAME,
+				   THERMAL_GENL_SAMPLING_GROUP_NAME);
+	if (mcid < 0) {
+		fprintf(stderr, "nl_get_multicast_id failed\n");
+		return -1;
+	}
+
+	if (nl_socket_add_membership(sk_sample, mcid)) {
+		fprintf(stderr, "nl_socket_add_membership failed");
+		return -1;
+	}
+
+	{
+		struct nl_cb *cb;
+		int err = 0, done = 0;
+
+		cb = nl_cb_alloc(NL_CB_DEFAULT);
+
+		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);
+		// nl_cb_set(cb, NL_CB_OVERRUN, NL_CB_CUSTOM, nl_overrun_handler, &done);
+		// nl_cb_set(cb, NL_CB_SKIPPED, NL_CB_CUSTOM, nl_skipped_handler, &done);
+		nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, nl_seq_check_handler, &done);
+		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, handle_sampling, NULL);
+
+		while (!done && !err)
+			nl_recvmsgs(sk_sample, cb);
+	}
+	
+	return 0;
+}