diff options
Diffstat (limited to 'thermal-sampling.c')
| -rw-r--r-- | thermal-sampling.c | 249 |
1 files changed, 249 insertions, 0 deletions
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; +} |