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 } 216