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, rx_handler: nl_family_handler, data: &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 | |