216 lines · 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 11struct handler_args {12 const char *group;13 int id;14};15 16static __thread int err;17static __thread int done;18 19static int nl_seq_check_handler(struct nl_msg *msg, void *arg)20{21 return NL_OK;22}23 24static 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 35static 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 45static 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 55int 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 75static 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 113static 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 139nla_put_failure:140 nlmsg_free(msg);141 return ret;142}143 144int 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 171out_socket_free:172 nl_socket_free(sock);173out_cb_free:174 nl_cb_put(cb);175 return THERMAL_ERROR;176}177 178void 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 185int 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 201int 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}216