| 1 |
// SPDX-License-Identifier: LGPL-2.1+ |
| 2 |
// Copyright (C) 2022, Linaro Ltd - Daniel Lezcano <[email protected]> |
| 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 |
} |