summaryrefslogtreecommitdiffstats
path: root/tools/lib/thermal/thermal_nl.c
diff options
context:
space:
mode:
Diffstat (limited to 'tools/lib/thermal/thermal_nl.c')
-rw-r--r--tools/lib/thermal/thermal_nl.c215
1 files changed, 215 insertions, 0 deletions
diff --git a/tools/lib/thermal/thermal_nl.c b/tools/lib/thermal/thermal_nl.c
new file mode 100644
index 000000000..b05cf9569
--- /dev/null
+++ b/tools/lib/thermal/thermal_nl.c
@@ -0,0 +1,215 @@
+// SPDX-License-Identifier: LGPL-2.1+
+// Copyright (C) 2022, Linaro Ltd - Daniel Lezcano <daniel.lezcano@linaro.org>
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <thermal.h>
+#include "thermal_nl.h"
+
+struct handler_args {
+ const char *group;
+ int id;
+};
+
+static __thread int err;
+static __thread int done;
+
+static int nl_seq_check_handler(struct nl_msg *msg, void *arg)
+{
+ return NL_OK;
+}
+
+static int nl_error_handler(struct sockaddr_nl *nla, struct nlmsgerr *nl_err,
+ void *arg)
+{
+ int *ret = arg;
+
+ if (ret)
+ *ret = nl_err->error;
+
+ return NL_STOP;
+}
+
+static int nl_finish_handler(struct nl_msg *msg, void *arg)
+{
+ int *ret = arg;
+
+ if (ret)
+ *ret = 1;
+
+ return NL_OK;
+}
+
+static int nl_ack_handler(struct nl_msg *msg, void *arg)
+{
+ int *ret = arg;
+
+ if (ret)
+ *ret = 1;
+
+ return NL_OK;
+}
+
+int nl_send_msg(struct nl_sock *sock, struct nl_cb *cb, struct nl_msg *msg,
+ int (*rx_handler)(struct nl_msg *, void *), void *data)
+{
+ if (!rx_handler)
+ return THERMAL_ERROR;
+
+ err = nl_send_auto_complete(sock, msg);
+ if (err < 0)
+ return err;
+
+ nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, rx_handler, data);
+
+ err = done = 0;
+
+ while (err == 0 && done == 0)
+ nl_recvmsgs(sock, 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])
+ return THERMAL_ERROR;
+
+ 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 THERMAL_SUCCESS;
+}
+
+static int nl_get_multicast_id(struct nl_sock *sock, struct nl_cb *cb,
+ const char *family, const char *group)
+{
+ struct nl_msg *msg;
+ int ret = 0, ctrlid;
+ struct handler_args grp = {
+ .group = group,
+ .id = -ENOENT,
+ };
+
+ msg = nlmsg_alloc();
+ if (!msg)
+ return THERMAL_ERROR;
+
+ 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);
+
+ ret = nl_send_msg(sock, cb, msg, nl_family_handler, &grp);
+ if (ret)
+ goto nla_put_failure;
+
+ ret = grp.id;
+
+nla_put_failure:
+ nlmsg_free(msg);
+ return ret;
+}
+
+int nl_thermal_connect(struct nl_sock **nl_sock, struct nl_cb **nl_cb)
+{
+ struct nl_cb *cb;
+ struct nl_sock *sock;
+
+ cb = nl_cb_alloc(NL_CB_DEFAULT);
+ if (!cb)
+ return THERMAL_ERROR;
+
+ sock = nl_socket_alloc();
+ if (!sock)
+ goto out_cb_free;
+
+ if (genl_connect(sock))
+ goto out_socket_free;
+
+ if (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_SEQ_CHECK, NL_CB_CUSTOM, nl_seq_check_handler, &done))
+ return THERMAL_ERROR;
+
+ *nl_sock = sock;
+ *nl_cb = cb;
+
+ return THERMAL_SUCCESS;
+
+out_socket_free:
+ nl_socket_free(sock);
+out_cb_free:
+ nl_cb_put(cb);
+ return THERMAL_ERROR;
+}
+
+void nl_thermal_disconnect(struct nl_sock *nl_sock, struct nl_cb *nl_cb)
+{
+ nl_close(nl_sock);
+ nl_socket_free(nl_sock);
+ nl_cb_put(nl_cb);
+}
+
+int nl_unsubscribe_thermal(struct nl_sock *nl_sock, struct nl_cb *nl_cb,
+ const char *group)
+{
+ int mcid;
+
+ mcid = nl_get_multicast_id(nl_sock, nl_cb, THERMAL_GENL_FAMILY_NAME,
+ group);
+ if (mcid < 0)
+ return THERMAL_ERROR;
+
+ if (nl_socket_drop_membership(nl_sock, mcid))
+ return THERMAL_ERROR;
+
+ return THERMAL_SUCCESS;
+}
+
+int nl_subscribe_thermal(struct nl_sock *nl_sock, struct nl_cb *nl_cb,
+ const char *group)
+{
+ int mcid;
+
+ mcid = nl_get_multicast_id(nl_sock, nl_cb, THERMAL_GENL_FAMILY_NAME,
+ group);
+ if (mcid < 0)
+ return THERMAL_ERROR;
+
+ if (nl_socket_add_membership(nl_sock, mcid))
+ return THERMAL_ERROR;
+
+ return THERMAL_SUCCESS;
+}