diff options
Diffstat (limited to 'src/libthermal/thermal-genl.c')
| -rw-r--r-- | src/libthermal/thermal-genl.c | 534 |
1 files changed, 534 insertions, 0 deletions
diff --git a/src/libthermal/thermal-genl.c b/src/libthermal/thermal-genl.c new file mode 100644 index 0000000..82966c2 --- /dev/null +++ b/src/libthermal/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; +} |