tools/lib/thermal/thermal_nl.c

   1 // SPDX-License-Identifier: LGPL-2.1+
   2 // Copyright (C) 2022, Linaro Ltd - Daniel Lezcano <daniel.lezcano@linaro.org>
   3 #include <errno.h>
   4 #include <stdio.h>
   5 #include <stdlib.h>
   6 #include <unistd.h>
   7
   8 #include <thermal.h>
   9 #include "thermal_nl.h"
  10
  11 struct handler_args {
  12         const char *group;
  13         int id;
  14 };
  15
  16 static __thread int err;
  17 static __thread int done;
  18
  19 static int nl_seq_check_handler(struct nl_msg *msg, void *arg)
  20 {
  21         return NL_OK;
  22 }
  23
  24 static int nl_error_handler(struct sockaddr_nl *nla, struct nlmsgerr *nl_err,
  25                             void *arg)
  26 {
  27         int *ret = arg;
  28
  29         if (ret)
  30                 *ret = nl_err->error;
  31
  32         return NL_STOP;
  33 }
  34
  35 static int nl_finish_handler(struct nl_msg *msg, void *arg)
  36 {
  37         int *ret = arg;
  38
  39         if (ret)
  40                 *ret = 1;
  41
  42         return NL_OK;
  43 }
  44
  45 static int nl_ack_handler(struct nl_msg *msg, void *arg)
  46 {
  47         int *ret = arg;
  48
  49         if (ret)
  50                 *ret = 1;
  51
  52         return NL_OK;
  53 }
  54
  55 int nl_send_msg(struct nl_sock *sock, struct nl_cb *cb, struct nl_msg *msg,
  56                 int (*rx_handler)(struct nl_msg *, void *), void *data)
  57 {
  58         if (!rx_handler)
  59                 return THERMAL_ERROR;
  60
  61         err = nl_send_auto_complete(sock, msg);
  62         if (err < 0)
  63                 return err;
  64
  65         nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, rx_handler, data);
  66
  67         err = done = 0;
  68
  69         while (err == 0 && done == 0)
  70                 nl_recvmsgs(sock, cb);
  71
  72         return err;
  73 }
  74
  75 static int nl_family_handler(struct nl_msg *msg, void *arg)
  76 {
  77         struct handler_args *grp = arg;
  78         struct nlattr *tb[CTRL_ATTR_MAX + 1];
  79         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
  80         struct nlattr *mcgrp;
  81         int rem_mcgrp;
  82
  83         nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
  84                   genlmsg_attrlen(gnlh, 0), NULL);
  85
  86         if (!tb[CTRL_ATTR_MCAST_GROUPS])
  87                 return THERMAL_ERROR;
  88
  89         nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], rem_mcgrp) {
  90
  91                 struct nlattr *tb_mcgrp[CTRL_ATTR_MCAST_GRP_MAX + 1];
  92
  93                 nla_parse(tb_mcgrp, CTRL_ATTR_MCAST_GRP_MAX,
  94                           nla_data(mcgrp), nla_len(mcgrp), NULL);
  95
  96                 if (!tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME] ||
  97                     !tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID])
  98                         continue;
  99
 100                 if (strncmp(nla_data(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]),
 101                             grp->group,
 102                             nla_len(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME])))
 103                         continue;
 104
 105                 grp->id = nla_get_u32(tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]);
 106
 107                 break;
 108         }
 109
 110         return THERMAL_SUCCESS;
 111 }
 112
 113 static int nl_get_multicast_id(struct nl_sock *sock, struct nl_cb *cb,
 114                                const char *family, const char *group)
 115 {
 116         struct nl_msg *msg;
 117         int ret = 0, ctrlid;
 118         struct handler_args grp = {
 119                 .group = group,
 120                 .id = -ENOENT,
 121         };
 122
 123         msg = nlmsg_alloc();
 124         if (!msg)
 125                 return THERMAL_ERROR;
 126
 127         ctrlid = genl_ctrl_resolve(sock, "nlctrl");
 128
 129         genlmsg_put(msg, 0, 0, ctrlid, 0, 0, CTRL_CMD_GETFAMILY, 0);
 130
 131         nla_put_string(msg, CTRL_ATTR_FAMILY_NAME, family);
 132
 133         ret = nl_send_msg(sock, cb, msg, nl_family_handler, &grp);
 134         if (ret)
 135                 goto nla_put_failure;
 136
 137         ret = grp.id;
 138
 139 nla_put_failure:
 140         nlmsg_free(msg);
 141         return ret;
 142 }
 143
 144 int nl_thermal_connect(struct nl_sock **nl_sock, struct nl_cb **nl_cb)
 145 {
 146         struct nl_cb *cb;
 147         struct nl_sock *sock;
 148
 149         cb = nl_cb_alloc(NL_CB_DEFAULT);
 150         if (!cb)
 151                 return THERMAL_ERROR;
 152
 153         sock = nl_socket_alloc();
 154         if (!sock)
 155                 goto out_cb_free;
 156
 157         if (genl_connect(sock))
 158                 goto out_socket_free;
 159
 160         if (nl_cb_err(cb, NL_CB_CUSTOM, nl_error_handler, &err) ||
 161             nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, nl_finish_handler, &done) ||
 162             nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, nl_ack_handler, &done) ||
 163             nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, nl_seq_check_handler, &done))
 164                 return THERMAL_ERROR;
 165
 166         *nl_sock = sock;
 167         *nl_cb = cb;
 168
 169         return THERMAL_SUCCESS;
 170
 171 out_socket_free:
 172         nl_socket_free(sock);
 173 out_cb_free:
 174         nl_cb_put(cb);
 175         return THERMAL_ERROR;
 176 }
 177
 178 void nl_thermal_disconnect(struct nl_sock *nl_sock, struct nl_cb *nl_cb)
 179 {
 180         nl_close(nl_sock);
 181         nl_socket_free(nl_sock);
 182         nl_cb_put(nl_cb);
 183 }
 184
 185 int nl_unsubscribe_thermal(struct nl_sock *nl_sock, struct nl_cb *nl_cb,
 186                            const char *group)
 187 {
 188         int mcid;
 189
 190         mcid = nl_get_multicast_id(nl_sock, nl_cb, THERMAL_GENL_FAMILY_NAME,
 191                                    group);
 192         if (mcid < 0)
 193                 return THERMAL_ERROR;
 194
 195         if (nl_socket_drop_membership(nl_sock, mcid))
 196                 return THERMAL_ERROR;
 197
 198         return THERMAL_SUCCESS;
 199 }
 200
 201 int nl_subscribe_thermal(struct nl_sock *nl_sock, struct nl_cb *nl_cb,
 202                          const char *group)
 203 {
 204         int mcid;
 205
 206         mcid = nl_get_multicast_id(nl_sock, nl_cb, THERMAL_GENL_FAMILY_NAME,
 207                                    group);
 208         if (mcid < 0)
 209                 return THERMAL_ERROR;
 210
 211         if (nl_socket_add_membership(nl_sock, mcid))
 212                 return THERMAL_ERROR;
 213
 214         return THERMAL_SUCCESS;
 215 }