lib/thermal/thermal_nl.c

0001 // SPDX-License-Identifier: LGPL-2.1+
0002 // Copyright (C) 2022, Linaro Ltd - Daniel Lezcano <daniel.lezcano@linaro.org>
0003 #include <errno.h>
0004 #include <stdio.h>
0005 #include <stdlib.h>
0006 #include <unistd.h>
0007
0008 #include <thermal.h>
0009 #include "thermal_nl.h"
0010
0011 struct handler_args {
0012     const char *group;
0013     int id;
0014 };
0015
0016 static __thread int err;
0017 static __thread int done;
0018
0019 static int nl_seq_check_handler(struct nl_msg *msg, void *arg)
0020 {
0021     return NL_OK;
0022 }
0023
0024 static int nl_error_handler(struct sockaddr_nl *nla, struct nlmsgerr *nl_err,
0025                 void *arg)
0026 {
0027     int *ret = arg;
0028
0029     if (ret)
0030         *ret = nl_err->error;
0031
0032     return NL_STOP;
0033 }
0034
0035 static int nl_finish_handler(struct nl_msg *msg, void *arg)
0036 {
0037     int *ret = arg;
0038
0039     if (ret)
0040         *ret = 1;
0041
0042     return NL_OK;
0043 }
0044
0045 static int nl_ack_handler(struct nl_msg *msg, void *arg)
0046 {
0047     int *ret = arg;
0048
0049     if (ret)
0050         *ret = 1;
0051
0052     return NL_OK;
0053 }
0054
0055 int nl_send_msg(struct nl_sock *sock, struct nl_cb *cb, struct nl_msg *msg,
0056         int (*rx_handler)(struct nl_msg *, void *), void *data)
0057 {
0058     if (!rx_handler)
0059         return THERMAL_ERROR;
0060
0061     err = nl_send_auto_complete(sock, msg);
0062     if (err < 0)
0063         return err;
0064
0065     nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, rx_handler, data);
0066
0067     err = done = 0;
0068
0069     while (err == 0 && done == 0)
0070         nl_recvmsgs(sock, cb);
0071
0072     return err;
0073 }
0074
0075 static int nl_family_handler(struct nl_msg *msg, void *arg)
0076 {
0077     struct handler_args *grp = arg;
0078     struct nlattr *tb[CTRL_ATTR_MAX + 1];
0079     struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
0080     struct nlattr *mcgrp;
0081     int rem_mcgrp;
0082
0083     nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
0084           genlmsg_attrlen(gnlh, 0), NULL);
0085
0086     if (!tb[CTRL_ATTR_MCAST_GROUPS])
0087         return THERMAL_ERROR;
0088
0089     nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], rem_mcgrp) {
0090
0091         struct nlattr *tb_mcgrp[CTRL_ATTR_MCAST_GRP_MAX + 1];
0092
0093         nla_parse(tb_mcgrp, CTRL_ATTR_MCAST_GRP_MAX,
0094               nla_data(mcgrp), nla_len(mcgrp), NULL);
0095
0096         if (!tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME] ||
0097             !tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID])
0098             continue;
0099
0100         if (strncmp(nla_data(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]),
0101                 grp->group,
0102                 nla_len(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME])))
0103             continue;
0104
0105         grp->id = nla_get_u32(tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]);
0106
0107         break;
0108     }
0109
0110     return THERMAL_SUCCESS;
0111 }
0112
0113 static int nl_get_multicast_id(struct nl_sock *sock, struct nl_cb *cb,
0114                    const char *family, const char *group)
0115 {
0116     struct nl_msg *msg;
0117     int ret = 0, ctrlid;
0118     struct handler_args grp = {
0119         .group = group,
0120         .id = -ENOENT,
0121     };
0122
0123     msg = nlmsg_alloc();
0124     if (!msg)
0125         return THERMAL_ERROR;
0126
0127     ctrlid = genl_ctrl_resolve(sock, "nlctrl");
0128
0129     genlmsg_put(msg, 0, 0, ctrlid, 0, 0, CTRL_CMD_GETFAMILY, 0);
0130
0131     nla_put_string(msg, CTRL_ATTR_FAMILY_NAME, family);
0132
0133     ret = nl_send_msg(sock, cb, msg, nl_family_handler, &grp);
0134     if (ret)
0135         goto nla_put_failure;
0136
0137     ret = grp.id;
0138
0139 nla_put_failure:
0140     nlmsg_free(msg);
0141     return ret;
0142 }
0143
0144 int nl_thermal_connect(struct nl_sock **nl_sock, struct nl_cb **nl_cb)
0145 {
0146     struct nl_cb *cb;
0147     struct nl_sock *sock;
0148
0149     cb = nl_cb_alloc(NL_CB_DEFAULT);
0150     if (!cb)
0151         return THERMAL_ERROR;
0152
0153     sock = nl_socket_alloc();
0154     if (!sock)
0155         goto out_cb_free;
0156
0157     if (genl_connect(sock))
0158         goto out_socket_free;
0159
0160     if (nl_cb_err(cb, NL_CB_CUSTOM, nl_error_handler, &err) ||
0161         nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, nl_finish_handler, &done) ||
0162         nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, nl_ack_handler, &done) ||
0163         nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, nl_seq_check_handler, &done))
0164         return THERMAL_ERROR;
0165
0166     *nl_sock = sock;
0167     *nl_cb = cb;
0168
0169     return THERMAL_SUCCESS;
0170
0171 out_socket_free:
0172     nl_socket_free(sock);
0173 out_cb_free:
0174     nl_cb_put(cb);
0175     return THERMAL_ERROR;
0176 }
0177
0178 void nl_thermal_disconnect(struct nl_sock *nl_sock, struct nl_cb *nl_cb)
0179 {
0180     nl_close(nl_sock);
0181     nl_socket_free(nl_sock);
0182     nl_cb_put(nl_cb);
0183 }
0184
0185 int nl_unsubscribe_thermal(struct nl_sock *nl_sock, struct nl_cb *nl_cb,
0186                const char *group)
0187 {
0188     int mcid;
0189
0190     mcid = nl_get_multicast_id(nl_sock, nl_cb, THERMAL_GENL_FAMILY_NAME,
0191                    group);
0192     if (mcid < 0)
0193         return THERMAL_ERROR;
0194
0195     if (nl_socket_drop_membership(nl_sock, mcid))
0196         return THERMAL_ERROR;
0197
0198     return THERMAL_SUCCESS;
0199 }
0200
0201 int nl_subscribe_thermal(struct nl_sock *nl_sock, struct nl_cb *nl_cb,
0202              const char *group)
0203 {
0204     int mcid;
0205
0206     mcid = nl_get_multicast_id(nl_sock, nl_cb, THERMAL_GENL_FAMILY_NAME,
0207                    group);
0208     if (mcid < 0)
0209         return THERMAL_ERROR;
0210
0211     if (nl_socket_add_membership(nl_sock, mcid))
0212         return THERMAL_ERROR;
0213
0214     return THERMAL_SUCCESS;
0215 }