1 | // SPDX-License-Identifier: LGPL-2.1+ |
2 | // Copyright (C) 2022, Linaro Ltd - Daniel Lezcano <daniel.lezcano@linaro.org> |
3 | #define _GNU_SOURCE |
4 | #include <errno.h> |
5 | #include <stdio.h> |
6 | #include <stdlib.h> |
7 | #include <unistd.h> |
8 | |
9 | #include <thermal.h> |
10 | #include "thermal_nl.h" |
11 | |
12 | static struct nla_policy thermal_genl_policy[THERMAL_GENL_ATTR_MAX + 1] = { |
13 | /* Thermal zone */ |
14 | [THERMAL_GENL_ATTR_TZ] = { .type = NLA_NESTED }, |
15 | [THERMAL_GENL_ATTR_TZ_ID] = { .type = NLA_U32 }, |
16 | [THERMAL_GENL_ATTR_TZ_TEMP] = { .type = NLA_U32 }, |
17 | [THERMAL_GENL_ATTR_TZ_TRIP] = { .type = NLA_NESTED }, |
18 | [THERMAL_GENL_ATTR_TZ_TRIP_ID] = { .type = NLA_U32 }, |
19 | [THERMAL_GENL_ATTR_TZ_TRIP_TEMP] = { .type = NLA_U32 }, |
20 | [THERMAL_GENL_ATTR_TZ_TRIP_TYPE] = { .type = NLA_U32 }, |
21 | [THERMAL_GENL_ATTR_TZ_TRIP_HYST] = { .type = NLA_U32 }, |
22 | [THERMAL_GENL_ATTR_TZ_MODE] = { .type = NLA_U32 }, |
23 | [THERMAL_GENL_ATTR_TZ_CDEV_WEIGHT] = { .type = NLA_U32 }, |
24 | [THERMAL_GENL_ATTR_TZ_NAME] = { .type = NLA_STRING }, |
25 | |
26 | /* Governor(s) */ |
27 | [THERMAL_GENL_ATTR_TZ_GOV] = { .type = NLA_NESTED }, |
28 | [THERMAL_GENL_ATTR_TZ_GOV_NAME] = { .type = NLA_STRING }, |
29 | |
30 | /* Cooling devices */ |
31 | [THERMAL_GENL_ATTR_CDEV] = { .type = NLA_NESTED }, |
32 | [THERMAL_GENL_ATTR_CDEV_ID] = { .type = NLA_U32 }, |
33 | [THERMAL_GENL_ATTR_CDEV_CUR_STATE] = { .type = NLA_U32 }, |
34 | [THERMAL_GENL_ATTR_CDEV_MAX_STATE] = { .type = NLA_U32 }, |
35 | [THERMAL_GENL_ATTR_CDEV_NAME] = { .type = NLA_STRING }, |
36 | }; |
37 | |
38 | static int parse_tz_get(struct genl_info *info, struct thermal_zone **tz) |
39 | { |
40 | struct nlattr *attr; |
41 | struct thermal_zone *__tz = NULL; |
42 | size_t size = 0; |
43 | int rem; |
44 | |
45 | nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_TZ], rem) { |
46 | |
47 | if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_ID) { |
48 | |
49 | size++; |
50 | |
51 | __tz = realloc(__tz, sizeof(*__tz) * (size + 2)); |
52 | if (!__tz) |
53 | return THERMAL_ERROR; |
54 | |
55 | __tz[size - 1].id = nla_get_u32(attr); |
56 | } |
57 | |
58 | |
59 | if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_NAME) |
60 | nla_strlcpy(__tz[size - 1].name, attr, |
61 | THERMAL_NAME_LENGTH); |
62 | } |
63 | |
64 | if (__tz) |
65 | __tz[size].id = -1; |
66 | |
67 | *tz = __tz; |
68 | |
69 | return THERMAL_SUCCESS; |
70 | } |
71 | |
72 | static int parse_cdev_get(struct genl_info *info, struct thermal_cdev **cdev) |
73 | { |
74 | struct nlattr *attr; |
75 | struct thermal_cdev *__cdev = NULL; |
76 | size_t size = 0; |
77 | int rem; |
78 | |
79 | nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_CDEV], rem) { |
80 | |
81 | if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_ID) { |
82 | |
83 | size++; |
84 | |
85 | __cdev = realloc(__cdev, sizeof(*__cdev) * (size + 2)); |
86 | if (!__cdev) |
87 | return THERMAL_ERROR; |
88 | |
89 | __cdev[size - 1].id = nla_get_u32(attr); |
90 | } |
91 | |
92 | if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_NAME) { |
93 | nla_strlcpy(__cdev[size - 1].name, attr, |
94 | THERMAL_NAME_LENGTH); |
95 | } |
96 | |
97 | if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_CUR_STATE) |
98 | __cdev[size - 1].cur_state = nla_get_u32(attr); |
99 | |
100 | if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_MAX_STATE) |
101 | __cdev[size - 1].max_state = nla_get_u32(attr); |
102 | } |
103 | |
104 | if (__cdev) |
105 | __cdev[size].id = -1; |
106 | |
107 | *cdev = __cdev; |
108 | |
109 | return THERMAL_SUCCESS; |
110 | } |
111 | |
112 | static int parse_tz_get_trip(struct genl_info *info, struct thermal_zone *tz) |
113 | { |
114 | struct nlattr *attr; |
115 | struct thermal_trip *__tt = NULL; |
116 | size_t size = 0; |
117 | int rem; |
118 | |
119 | nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_TZ_TRIP], rem) { |
120 | |
121 | if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_ID) { |
122 | |
123 | size++; |
124 | |
125 | __tt = realloc(__tt, sizeof(*__tt) * (size + 2)); |
126 | if (!__tt) |
127 | return THERMAL_ERROR; |
128 | |
129 | __tt[size - 1].id = nla_get_u32(attr); |
130 | } |
131 | |
132 | if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_TYPE) |
133 | __tt[size - 1].type = nla_get_u32(attr); |
134 | |
135 | if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_TEMP) |
136 | __tt[size - 1].temp = nla_get_u32(attr); |
137 | |
138 | if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_HYST) |
139 | __tt[size - 1].hyst = nla_get_u32(attr); |
140 | } |
141 | |
142 | if (__tt) |
143 | __tt[size].id = -1; |
144 | |
145 | tz->trip = __tt; |
146 | |
147 | return THERMAL_SUCCESS; |
148 | } |
149 | |
150 | static int parse_tz_get_temp(struct genl_info *info, struct thermal_zone *tz) |
151 | { |
152 | int id = -1; |
153 | |
154 | if (info->attrs[THERMAL_GENL_ATTR_TZ_ID]) |
155 | id = nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_ID]); |
156 | |
157 | if (tz->id != id) |
158 | return THERMAL_ERROR; |
159 | |
160 | if (info->attrs[THERMAL_GENL_ATTR_TZ_TEMP]) |
161 | tz->temp = nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_TEMP]); |
162 | |
163 | return THERMAL_SUCCESS; |
164 | } |
165 | |
166 | static int parse_tz_get_gov(struct genl_info *info, struct thermal_zone *tz) |
167 | { |
168 | int id = -1; |
169 | |
170 | if (info->attrs[THERMAL_GENL_ATTR_TZ_ID]) |
171 | id = nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_ID]); |
172 | |
173 | if (tz->id != id) |
174 | return THERMAL_ERROR; |
175 | |
176 | if (info->attrs[THERMAL_GENL_ATTR_TZ_GOV_NAME]) { |
177 | nla_strlcpy(tz->governor, |
178 | info->attrs[THERMAL_GENL_ATTR_TZ_GOV_NAME], |
179 | THERMAL_NAME_LENGTH); |
180 | } |
181 | |
182 | return THERMAL_SUCCESS; |
183 | } |
184 | |
185 | static int handle_netlink(struct nl_cache_ops *unused, |
186 | struct genl_cmd *cmd, |
187 | struct genl_info *info, void *arg) |
188 | { |
189 | int ret; |
190 | |
191 | switch (cmd->c_id) { |
192 | |
193 | case THERMAL_GENL_CMD_TZ_GET_ID: |
194 | ret = parse_tz_get(info, tz: arg); |
195 | break; |
196 | |
197 | case THERMAL_GENL_CMD_CDEV_GET: |
198 | ret = parse_cdev_get(info, cdev: arg); |
199 | break; |
200 | |
201 | case THERMAL_GENL_CMD_TZ_GET_TEMP: |
202 | ret = parse_tz_get_temp(info, tz: arg); |
203 | break; |
204 | |
205 | case THERMAL_GENL_CMD_TZ_GET_TRIP: |
206 | ret = parse_tz_get_trip(info, tz: arg); |
207 | break; |
208 | |
209 | case THERMAL_GENL_CMD_TZ_GET_GOV: |
210 | ret = parse_tz_get_gov(info, tz: arg); |
211 | break; |
212 | |
213 | default: |
214 | return THERMAL_ERROR; |
215 | } |
216 | |
217 | return ret; |
218 | } |
219 | |
220 | static struct genl_cmd thermal_cmds[] = { |
221 | { |
222 | .c_id = THERMAL_GENL_CMD_TZ_GET_ID, |
223 | .c_name = (char *)"List thermal zones" , |
224 | .c_msg_parser = handle_netlink, |
225 | .c_maxattr = THERMAL_GENL_ATTR_MAX, |
226 | .c_attr_policy = thermal_genl_policy, |
227 | }, |
228 | { |
229 | .c_id = THERMAL_GENL_CMD_TZ_GET_GOV, |
230 | .c_name = (char *)"Get governor" , |
231 | .c_msg_parser = handle_netlink, |
232 | .c_maxattr = THERMAL_GENL_ATTR_MAX, |
233 | .c_attr_policy = thermal_genl_policy, |
234 | }, |
235 | { |
236 | .c_id = THERMAL_GENL_CMD_TZ_GET_TEMP, |
237 | .c_name = (char *)"Get thermal zone temperature" , |
238 | .c_msg_parser = handle_netlink, |
239 | .c_maxattr = THERMAL_GENL_ATTR_MAX, |
240 | .c_attr_policy = thermal_genl_policy, |
241 | }, |
242 | { |
243 | .c_id = THERMAL_GENL_CMD_TZ_GET_TRIP, |
244 | .c_name = (char *)"Get thermal zone trip points" , |
245 | .c_msg_parser = handle_netlink, |
246 | .c_maxattr = THERMAL_GENL_ATTR_MAX, |
247 | .c_attr_policy = thermal_genl_policy, |
248 | }, |
249 | { |
250 | .c_id = THERMAL_GENL_CMD_CDEV_GET, |
251 | .c_name = (char *)"Get cooling devices" , |
252 | .c_msg_parser = handle_netlink, |
253 | .c_maxattr = THERMAL_GENL_ATTR_MAX, |
254 | .c_attr_policy = thermal_genl_policy, |
255 | }, |
256 | }; |
257 | |
258 | static struct genl_ops thermal_cmd_ops = { |
259 | .o_name = (char *)"thermal" , |
260 | .o_cmds = thermal_cmds, |
261 | .o_ncmds = ARRAY_SIZE(thermal_cmds), |
262 | }; |
263 | |
264 | static thermal_error_t thermal_genl_auto(struct thermal_handler *th, int id, int cmd, |
265 | int flags, void *arg) |
266 | { |
267 | struct nl_msg *msg; |
268 | void *hdr; |
269 | |
270 | msg = nlmsg_alloc(); |
271 | if (!msg) |
272 | return THERMAL_ERROR; |
273 | |
274 | hdr = genlmsg_put(msg, NL_AUTO_PORT, NL_AUTO_SEQ, thermal_cmd_ops.o_id, |
275 | 0, flags, cmd, THERMAL_GENL_VERSION); |
276 | if (!hdr) |
277 | return THERMAL_ERROR; |
278 | |
279 | if (id >= 0 && nla_put_u32(msg, THERMAL_GENL_ATTR_TZ_ID, id)) |
280 | return THERMAL_ERROR; |
281 | |
282 | if (nl_send_msg(th->sk_cmd, th->cb_cmd, msg, genl_handle_msg, arg)) |
283 | return THERMAL_ERROR; |
284 | |
285 | nlmsg_free(msg); |
286 | |
287 | return THERMAL_SUCCESS; |
288 | } |
289 | |
290 | thermal_error_t thermal_cmd_get_tz(struct thermal_handler *th, struct thermal_zone **tz) |
291 | { |
292 | return thermal_genl_auto(th, -1, THERMAL_GENL_CMD_TZ_GET_ID, |
293 | NLM_F_DUMP | NLM_F_ACK, tz); |
294 | } |
295 | |
296 | thermal_error_t thermal_cmd_get_cdev(struct thermal_handler *th, struct thermal_cdev **tc) |
297 | { |
298 | return thermal_genl_auto(th, -1, THERMAL_GENL_CMD_CDEV_GET, |
299 | NLM_F_DUMP | NLM_F_ACK, tc); |
300 | } |
301 | |
302 | thermal_error_t thermal_cmd_get_trip(struct thermal_handler *th, struct thermal_zone *tz) |
303 | { |
304 | return thermal_genl_auto(th, tz->id, THERMAL_GENL_CMD_TZ_GET_TRIP, |
305 | 0, tz); |
306 | } |
307 | |
308 | thermal_error_t thermal_cmd_get_governor(struct thermal_handler *th, struct thermal_zone *tz) |
309 | { |
310 | return thermal_genl_auto(th, tz->id, THERMAL_GENL_CMD_TZ_GET_GOV, 0, tz); |
311 | } |
312 | |
313 | thermal_error_t thermal_cmd_get_temp(struct thermal_handler *th, struct thermal_zone *tz) |
314 | { |
315 | return thermal_genl_auto(th, tz->id, THERMAL_GENL_CMD_TZ_GET_TEMP, 0, tz); |
316 | } |
317 | |
318 | thermal_error_t thermal_cmd_exit(struct thermal_handler *th) |
319 | { |
320 | if (genl_unregister_family(&thermal_cmd_ops)) |
321 | return THERMAL_ERROR; |
322 | |
323 | nl_thermal_disconnect(nl_sock: th->sk_cmd, nl_cb: th->cb_cmd); |
324 | |
325 | return THERMAL_SUCCESS; |
326 | } |
327 | |
328 | thermal_error_t thermal_cmd_init(struct thermal_handler *th) |
329 | { |
330 | int ret; |
331 | int family; |
332 | |
333 | if (nl_thermal_connect(&th->sk_cmd, &th->cb_cmd)) |
334 | return THERMAL_ERROR; |
335 | |
336 | ret = genl_register_family(&thermal_cmd_ops); |
337 | if (ret) |
338 | return THERMAL_ERROR; |
339 | |
340 | ret = genl_ops_resolve(th->sk_cmd, &thermal_cmd_ops); |
341 | if (ret) |
342 | return THERMAL_ERROR; |
343 | |
344 | family = genl_ctrl_resolve(th->sk_cmd, "nlctrl" ); |
345 | if (family != GENL_ID_CTRL) |
346 | return THERMAL_ERROR; |
347 | |
348 | return THERMAL_SUCCESS; |
349 | } |
350 | |