diff options
author | Daniel Lezcano <daniel.lezcano@linaro.org> | 2020-05-15 12:44:20 +0000 |
---|---|---|
committer | Daniel Lezcano <daniel.lezcano@linaro.org> | 2020-05-15 12:44:20 +0000 |
commit | e725788049fe791c2e55e9efd31d8ff3a2927ac3 (patch) | |
tree | 80e0ac34af4309a77282ab958bd3c25d420620d8 |
Initial commit
Signed-off-by: Daniel Lezcano <daniel.lezcano@linaro.org>
-rw-r--r-- | Makefile | 16 | ||||
-rw-r--r-- | thermal-cmd.c | 404 | ||||
-rw-r--r-- | thermal-event.c | 261 | ||||
-rw-r--r-- | thermal-genl.c | 534 | ||||
-rw-r--r-- | thermal.h | 92 |
5 files changed, 1307 insertions, 0 deletions
diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..5d3118e --- /dev/null +++ b/Makefile @@ -0,0 +1,16 @@ +CC=gcc +CFLAGS=-g -Wall -I/usr/include/libnl3 +LDFLAGS=-lnl-genl-3 -lnl-3 +DEPS = thermal.h + +all: thermal-cmd thermal-sampling thermal-event + +thermal-cmd: thermal-cmd.c $(DEPS) + $(CC) $< $(CFLAGS) -o $@ $(LDFLAGS) + +thermal-sampling: thermal-sampling.c $(DEPS) + $(CC) $< $(CFLAGS) -o $@ $(LDFLAGS) + +thermal-event: thermal-event.c $(DEPS) + $(CC) $< $(CFLAGS) -o $@ $(LDFLAGS) + diff --git a/thermal-cmd.c b/thermal-cmd.c new file mode 100644 index 0000000..caba8f9 --- /dev/null +++ b/thermal-cmd.c @@ -0,0 +1,404 @@ +#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("Cooling device name: %s\n", nla_get_string(attr)); + } + + return 0; +} + +int show_trip(struct genl_info *info) +{ + struct nlattr *attr; + int rem; + + nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_TZ_TRIP], rem) { + + 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_ID]) + printf("Thermal zone id: %d\n", + nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_ID])); + + if (info->attrs[THERMAL_GENL_ATTR_TZ_TEMP]) + printf("Thermal zone temperature: %d\n", + nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_TEMP])); + + return 0; +} + +int show_gov(struct genl_info *info) +{ + if (info->attrs[THERMAL_GENL_ATTR_TZ_ID]) + printf("Thermal zone id: %d\n", + nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_ID])); + + if (info->attrs[THERMAL_GENL_ATTR_TZ_GOV_NAME]) + printf("Thermal zone governor: %s\n", + nla_get_string(info->attrs[THERMAL_GENL_ATTR_TZ_GOV_NAME])); + + 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_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; + + case THERMAL_GENL_CMD_TZ_GET_GOV: + printf("THERMAL_GENL_CMD_TZ_GET_GOV\n"); + show_gov(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_GOV, + .c_name = "Get governor", + .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; +}; + +int main(int argc, char *argv[]) +{ + struct nl_sock *sk_cmd; + int ret, family; + + 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; + } + + 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, 0, + THERMAL_GENL_CMD_TZ_GET_TRIP, 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; + } + + ret = thermal_genl_auto(sk_cmd, thermal_cmd_ops.o_id, 0, + 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_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, 0, + THERMAL_GENL_CMD_TZ_GET_GOV, 0); + if (ret < 0) { + fprintf(stderr, "Failed to thermal_genl_send: %d\n", ret); + return -1; + } + + return 0; +} diff --git a/thermal-event.c b/thermal-event.c new file mode 100644 index 0000000..98d645e --- /dev/null +++ b/thermal-event.c @@ -0,0 +1,261 @@ +#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_event(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_EVENT_TZ_TRIP_HIGH) { + printf("THERMAL_GENL_EVENT_TZ_TRIP_HIGH\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_TRIP_ID]) + printf("Thermal zone trip id %d\n", + nla_get_u32(attrs[THERMAL_GENL_ATTR_TZ_TRIP_ID])); + } + + if (genlhdr->cmd == THERMAL_GENL_EVENT_TZ_TRIP_LOW) { + printf("THERMAL_GENL_EVENT_TZ_TRIP_LOW\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_TRIP_ID]) + printf("Thermal zone trip id %d\n", + nla_get_u32(attrs[THERMAL_GENL_ATTR_TZ_TRIP_ID])); + } + + 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_event; + int mcid; + + nl_debug = 0; + + /* Event receiving */ + sk_event = nl_socket_alloc(); + if (!sk_event) { + fprintf(stderr, "nl_socket_alloc failed\n"); + return -1; + } + + if (genl_connect(sk_event)) { + fprintf(stderr, "genl_connect(sk_event) failed\n"); + return -1; + } + + mcid = nl_get_multicast_id(sk_event, THERMAL_GENL_FAMILY_NAME, + THERMAL_GENL_EVENT_GROUP_NAME); + if (mcid < 0) { + fprintf(stderr, "nl_get_multicast_id failed\n"); + return -1; + } + + if (nl_socket_add_membership(sk_event, 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_event, NULL); + + while (!done && !err) + nl_recvmsgs(sk_event, cb); + } + + return 0; +} diff --git a/thermal-genl.c b/thermal-genl.c new file mode 100644 index 0000000..82966c2 --- /dev/null +++ b/thermal-genl.c @@ -0,0 +1,534 @@ +#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.h b/thermal.h new file mode 100644 index 0000000..51db54d --- /dev/null +++ b/thermal.h @@ -0,0 +1,92 @@ +/* SPDX-License-Identifier: GPL-2.0 WITH Linux-syscall-note */ +#ifndef _UAPI_LINUX_THERMAL_H +#define _UAPI_LINUX_THERMAL_H + +#define THERMAL_NAME_LENGTH20 + +enum thermal_device_mode { + THERMAL_DEVICE_DISABLED = 0, + THERMAL_DEVICE_ENABLED, +}; + +enum thermal_trip_type { + THERMAL_TRIP_ACTIVE = 0, + THERMAL_TRIP_PASSIVE, + THERMAL_TRIP_HOT, + THERMAL_TRIP_CRITICAL, +}; + +/* Adding event notification support elements */ +#define THERMAL_GENL_FAMILY_NAME "thermal" +#define THERMAL_GENL_VERSION 0x01 +#define THERMAL_GENL_SAMPLING_GROUP_NAME "sampling" +#define THERMAL_GENL_EVENT_GROUP_NAME "event" + +/* Attributes of thermal_genl_family */ +enum thermal_genl_attr { + THERMAL_GENL_ATTR_UNSPEC, + THERMAL_GENL_ATTR_TZ, + THERMAL_GENL_ATTR_TZ_ID, + THERMAL_GENL_ATTR_TZ_TEMP, + THERMAL_GENL_ATTR_TZ_TRIP, + THERMAL_GENL_ATTR_TZ_TRIP_ID, + THERMAL_GENL_ATTR_TZ_TRIP_TYPE, + THERMAL_GENL_ATTR_TZ_TRIP_TEMP, + THERMAL_GENL_ATTR_TZ_TRIP_HYST, + THERMAL_GENL_ATTR_TZ_MODE, + THERMAL_GENL_ATTR_TZ_NAME, + THERMAL_GENL_ATTR_TZ_CDEV_WEIGHT, + THERMAL_GENL_ATTR_TZ_GOV, + THERMAL_GENL_ATTR_TZ_GOV_NAME, + THERMAL_GENL_ATTR_CDEV, + THERMAL_GENL_ATTR_CDEV_ID, + THERMAL_GENL_ATTR_CDEV_CUR_STATE, + THERMAL_GENL_ATTR_CDEV_MAX_STATE, + THERMAL_GENL_ATTR_CDEV_MIN_STATE, + THERMAL_GENL_ATTR_CDEV_NAME, + THERMAL_GENL_ATTR_GOV_NAME, + + __THERMAL_GENL_ATTR_MAX, +}; +#define THERMAL_GENL_ATTR_MAX (__THERMAL_GENL_ATTR_MAX - 1) + +enum thermal_genl_sampling { + THERMAL_GENL_SAMPLING_TEMP, + __THERMAL_GENL_SAMPLING_MAX, +}; +#define THERMAL_GENL_SAMPLING_MAX (__THERMAL_GENL_SAMPLING_MAX - 1) + +/* Events of thermal_genl_family */ +enum thermal_genl_event { + THERMAL_GENL_EVENT_UNSPEC, + THERMAL_GENL_EVENT_TZ_CREATE,/* Thermal zone creation */ + THERMAL_GENL_EVENT_TZ_DELETE,/* Thermal zone deletion */ + THERMAL_GENL_EVENT_TZ_DISABLE,/* Thermal zone disabed */ + THERMAL_GENL_EVENT_TZ_ENABLE,/* Thermal zone enabled */ + THERMAL_GENL_EVENT_TZ_TRIP_HIGH,/* Trip point crossed the way up */ + THERMAL_GENL_EVENT_TZ_TRIP_LOW,/* Trip point crossed the way down */ + THERMAL_GENL_EVENT_TZ_TRIP_CHANGE,/* Trip point changed */ + THERMAL_GENL_EVENT_TZ_TRIP_ADD,/* Trip point added */ + THERMAL_GENL_EVENT_TZ_TRIP_DELETE,/* Trip point deleted */ + THERMAL_GENL_EVENT_TZ_CDEV_ADD,/* Cdev bound to the thermal zone */ + THERMAL_GENL_EVENT_TZ_CDEV_DELETE,/* Cdev unbound */ + THERMAL_GENL_EVENT_TZ_CDEV_UPDATE,/* Cdev state updated */ + THERMAL_GENL_EVENT_TZ_GOV_CHANGE,/* Governor policy changed */ + __THERMAL_GENL_EVENT_MAX, +}; +#define THERMAL_GENL_EVENT_MAX (__THERMAL_GENL_EVENT_MAX - 1) + +/* Commands supported by the thermal_genl_family */ +enum thermal_genl_cmd { + THERMAL_GENL_CMD_UNSPEC, + THERMAL_GENL_CMD_TZ_GET,/* List thermal zones id */ + THERMAL_GENL_CMD_TZ_GET_TRIP,/* List of thermal trips */ + THERMAL_GENL_CMD_TZ_GET_TEMP,/* Get the thermal zone temperature */ + THERMAL_GENL_CMD_TZ_GET_GOV,/* Get the thermal zone governor */ + THERMAL_GENL_CMD_TZ_GET_MODE,/* Get the thermal zone mode */ + THERMAL_GENL_CMD_CDEV_GET,/* List of cdev id */ + __THERMAL_GENL_CMD_MAX, +}; +#define THERMAL_GENL_CMD_MAX (__THERMAL_GENL_CMD_MAX - 1) + +#endif /* _UAPI_LINUX_THERMAL_H */ |