1 // SPDX-License-Identifier: LGPL-2.1+
2 // Copyright (C) 2022, Linaro Ltd - Daniel Lezcano <daniel.lezcano@linaro.org>
9 #include "thermal_nl.h"
16 static __thread int err;
17 static __thread int done;
19 static int nl_seq_check_handler(struct nl_msg *msg, void *arg)
24 static int nl_error_handler(struct sockaddr_nl *nla, struct nlmsgerr *nl_err,
35 static int nl_finish_handler(struct nl_msg *msg, void *arg)
45 static int nl_ack_handler(struct nl_msg *msg, void *arg)
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)
61 err = nl_send_auto_complete(sock, msg);
65 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, rx_handler, data);
69 while (err == 0 && done == 0)
70 nl_recvmsgs(sock, cb);
75 static int nl_family_handler(struct nl_msg *msg, void *arg)
77 struct handler_args *grp = arg;
78 struct nlattr *tb[CTRL_ATTR_MAX + 1];
79 struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
83 nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
84 genlmsg_attrlen(gnlh, 0), NULL);
86 if (!tb[CTRL_ATTR_MCAST_GROUPS])
89 nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], rem_mcgrp) {
91 struct nlattr *tb_mcgrp[CTRL_ATTR_MCAST_GRP_MAX + 1];
93 nla_parse(tb_mcgrp, CTRL_ATTR_MCAST_GRP_MAX,
94 nla_data(mcgrp), nla_len(mcgrp), NULL);
96 if (!tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME] ||
97 !tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID])
100 if (strncmp(nla_data(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]),
102 nla_len(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME])))
105 grp->id = nla_get_u32(tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]);
110 return THERMAL_SUCCESS;
113 static int nl_get_multicast_id(struct nl_sock *sock, struct nl_cb *cb,
114 const char *family, const char *group)
118 struct handler_args grp = {
125 return THERMAL_ERROR;
127 ctrlid = genl_ctrl_resolve(sock, "nlctrl");
129 genlmsg_put(msg, 0, 0, ctrlid, 0, 0, CTRL_CMD_GETFAMILY, 0);
131 nla_put_string(msg, CTRL_ATTR_FAMILY_NAME, family);
133 ret = nl_send_msg(sock, cb, msg, nl_family_handler, &grp);
135 goto nla_put_failure;
144 int nl_thermal_connect(struct nl_sock **nl_sock, struct nl_cb **nl_cb)
147 struct nl_sock *sock;
149 cb = nl_cb_alloc(NL_CB_DEFAULT);
151 return THERMAL_ERROR;
153 sock = nl_socket_alloc();
157 if (genl_connect(sock))
158 goto out_socket_free;
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;
169 return THERMAL_SUCCESS;
172 nl_socket_free(sock);
175 return THERMAL_ERROR;
178 void nl_thermal_disconnect(struct nl_sock *nl_sock, struct nl_cb *nl_cb)
181 nl_socket_free(nl_sock);
185 int nl_unsubscribe_thermal(struct nl_sock *nl_sock, struct nl_cb *nl_cb,
190 mcid = nl_get_multicast_id(nl_sock, nl_cb, THERMAL_GENL_FAMILY_NAME,
193 return THERMAL_ERROR;
195 if (nl_socket_drop_membership(nl_sock, mcid))
196 return THERMAL_ERROR;
198 return THERMAL_SUCCESS;
201 int nl_subscribe_thermal(struct nl_sock *nl_sock, struct nl_cb *nl_cb,
206 mcid = nl_get_multicast_id(nl_sock, nl_cb, THERMAL_GENL_FAMILY_NAME,
209 return THERMAL_ERROR;
211 if (nl_socket_add_membership(nl_sock, mcid))
212 return THERMAL_ERROR;
214 return THERMAL_SUCCESS;