path: root/thermal-genl.c

#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;
}