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 
nl_seq_check_handler(struct nl_msg * msg,void * arg)19 static int nl_seq_check_handler(struct nl_msg *msg, void *arg)
20 {
21 	return NL_OK;
22 }
23 
nl_error_handler(struct sockaddr_nl * nla,struct nlmsgerr * nl_err,void * arg)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 
nl_finish_handler(struct nl_msg * msg,void * arg)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 
nl_ack_handler(struct nl_msg * msg,void * arg)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 
nl_send_msg(struct nl_sock * sock,struct nl_cb * cb,struct nl_msg * msg,int (* rx_handler)(struct nl_msg *,void *),void * data)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 
nl_family_handler(struct nl_msg * msg,void * arg)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 
nl_get_multicast_id(struct nl_sock * sock,struct nl_cb * cb,const char * family,const char * group)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 
nl_thermal_connect(struct nl_sock ** nl_sock,struct nl_cb ** nl_cb)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 
nl_thermal_disconnect(struct nl_sock * nl_sock,struct nl_cb * nl_cb)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 
nl_unsubscribe_thermal(struct nl_sock * nl_sock,struct nl_cb * nl_cb,const char * group)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 
nl_subscribe_thermal(struct nl_sock * nl_sock,struct nl_cb * nl_cb,const char * group)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