projects/linux·blob master · refs

511 lines · 13.6 KiB

1// SPDX-License-Identifier: LGPL-2.1+
2// Copyright (C) 2022, Linaro Ltd - Daniel Lezcano <[email protected]>
3#define _GNU_SOURCE
4#include <errno.h>
5#include <stdio.h>
6#include <stdlib.h>
7#include <unistd.h>
8#include <limits.h>
9
10#include <thermal.h>
11#include "thermal_nl.h"
12
13static struct nla_policy thermal_genl_policy[THERMAL_GENL_ATTR_MAX + 1] = {
14 /* Thermal zone */
15 [THERMAL_GENL_ATTR_TZ] = { .type = NLA_NESTED },
16 [THERMAL_GENL_ATTR_TZ_ID] = { .type = NLA_U32 },
17 [THERMAL_GENL_ATTR_TZ_TEMP] = { .type = NLA_U32 },
18 [THERMAL_GENL_ATTR_TZ_TRIP] = { .type = NLA_NESTED },
19 [THERMAL_GENL_ATTR_TZ_TRIP_ID] = { .type = NLA_U32 },
20 [THERMAL_GENL_ATTR_TZ_TRIP_TEMP] = { .type = NLA_U32 },
21 [THERMAL_GENL_ATTR_TZ_TRIP_TYPE] = { .type = NLA_U32 },
22 [THERMAL_GENL_ATTR_TZ_TRIP_HYST] = { .type = NLA_U32 },
23 [THERMAL_GENL_ATTR_TZ_MODE] = { .type = NLA_U32 },
24 [THERMAL_GENL_ATTR_TZ_CDEV_WEIGHT] = { .type = NLA_U32 },
25 [THERMAL_GENL_ATTR_TZ_NAME] = { .type = NLA_STRING },
26
27 /* Governor(s) */
28 [THERMAL_GENL_ATTR_TZ_GOV] = { .type = NLA_NESTED },
29 [THERMAL_GENL_ATTR_TZ_GOV_NAME] = { .type = NLA_STRING },
30
31 /* Cooling devices */
32 [THERMAL_GENL_ATTR_CDEV] = { .type = NLA_NESTED },
33 [THERMAL_GENL_ATTR_CDEV_ID] = { .type = NLA_U32 },
34 [THERMAL_GENL_ATTR_CDEV_CUR_STATE] = { .type = NLA_U32 },
35 [THERMAL_GENL_ATTR_CDEV_MAX_STATE] = { .type = NLA_U32 },
36 [THERMAL_GENL_ATTR_CDEV_NAME] = { .type = NLA_STRING },
37
38 /* Thresholds */
39 [THERMAL_GENL_ATTR_THRESHOLD] = { .type = NLA_NESTED },
40 [THERMAL_GENL_ATTR_THRESHOLD_TEMP] = { .type = NLA_U32 },
41 [THERMAL_GENL_ATTR_THRESHOLD_DIRECTION] = { .type = NLA_U32 },
42};
43
44static int parse_tz_get(struct genl_info *info, struct thermal_zone **tz)
45{
46 struct nlattr *attr;
47 struct thermal_zone *__tz = NULL;
48 size_t size = 0;
49 int rem;
50
51 nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_TZ], rem) {
52
53 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_ID) {
54
55 size++;
56
57 __tz = realloc(__tz, sizeof(*__tz) * (size + 2));
58 if (!__tz)
59 return THERMAL_ERROR;
60
61 __tz[size - 1].id = nla_get_u32(attr);
62 }
63
64
65 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_NAME)
66 nla_strlcpy(__tz[size - 1].name, attr,
67 THERMAL_NAME_LENGTH);
68 }
69
70 if (__tz)
71 __tz[size].id = -1;
72
73 *tz = __tz;
74
75 return THERMAL_SUCCESS;
76}
77
78static int parse_cdev_get(struct genl_info *info, struct thermal_cdev **cdev)
79{
80 struct nlattr *attr;
81 struct thermal_cdev *__cdev = NULL;
82 size_t size = 0;
83 int rem;
84
85 nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_CDEV], rem) {
86
87 if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_ID) {
88
89 size++;
90
91 __cdev = realloc(__cdev, sizeof(*__cdev) * (size + 2));
92 if (!__cdev)
93 return THERMAL_ERROR;
94
95 __cdev[size - 1].id = nla_get_u32(attr);
96 }
97
98 if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_NAME) {
99 nla_strlcpy(__cdev[size - 1].name, attr,
100 THERMAL_NAME_LENGTH);
101 }
102
103 if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_CUR_STATE)
104 __cdev[size - 1].cur_state = nla_get_u32(attr);
105
106 if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_MAX_STATE)
107 __cdev[size - 1].max_state = nla_get_u32(attr);
108 }
109
110 if (__cdev)
111 __cdev[size].id = -1;
112
113 *cdev = __cdev;
114
115 return THERMAL_SUCCESS;
116}
117
118static int parse_tz_get_trip(struct genl_info *info, struct thermal_zone *tz)
119{
120 struct nlattr *attr;
121 struct thermal_trip *__tt = NULL;
122 size_t size = 0;
123 int rem;
124
125 nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_TZ_TRIP], rem) {
126
127 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_ID) {
128
129 size++;
130
131 __tt = realloc(__tt, sizeof(*__tt) * (size + 2));
132 if (!__tt)
133 return THERMAL_ERROR;
134
135 __tt[size - 1].id = nla_get_u32(attr);
136 }
137
138 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_TYPE)
139 __tt[size - 1].type = nla_get_u32(attr);
140
141 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_TEMP)
142 __tt[size - 1].temp = nla_get_u32(attr);
143
144 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_HYST)
145 __tt[size - 1].hyst = nla_get_u32(attr);
146 }
147
148 if (__tt)
149 __tt[size].id = -1;
150
151 tz->trip = __tt;
152
153 return THERMAL_SUCCESS;
154}
155
156static int parse_tz_get_temp(struct genl_info *info, struct thermal_zone *tz)
157{
158 int id = -1;
159
160 if (info->attrs[THERMAL_GENL_ATTR_TZ_ID])
161 id = nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_ID]);
162
163 if (tz->id != id)
164 return THERMAL_ERROR;
165
166 if (info->attrs[THERMAL_GENL_ATTR_TZ_TEMP])
167 tz->temp = nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_TEMP]);
168
169 return THERMAL_SUCCESS;
170}
171
172static int parse_tz_get_gov(struct genl_info *info, struct thermal_zone *tz)
173{
174 int id = -1;
175
176 if (info->attrs[THERMAL_GENL_ATTR_TZ_ID])
177 id = nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_ID]);
178
179 if (tz->id != id)
180 return THERMAL_ERROR;
181
182 if (info->attrs[THERMAL_GENL_ATTR_TZ_GOV_NAME]) {
183 nla_strlcpy(tz->governor,
184 info->attrs[THERMAL_GENL_ATTR_TZ_GOV_NAME],
185 THERMAL_NAME_LENGTH);
186 }
187
188 return THERMAL_SUCCESS;
189}
190
191static int parse_threshold_get(struct genl_info *info, struct thermal_zone *tz)
192{
193 struct nlattr *attr;
194 struct thermal_threshold *__tt = NULL;
195 size_t size = 0;
196 int rem;
197
198 /*
199 * The size contains the size of the array and we want to
200 * access the last element, size - 1.
201 *
202 * The variable size is initialized to zero but it will be
203 * then incremented by the first if() statement. The message
204 * attributes are ordered, so the first if() statement will be
205 * always called before the second one. If it happens that is
206 * not the case, then it is a kernel bug.
207 */
208 nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_THRESHOLD], rem) {
209
210 if (nla_type(attr) == THERMAL_GENL_ATTR_THRESHOLD_TEMP) {
211
212 size++;
213
214 __tt = realloc(__tt, sizeof(*__tt) * (size + 2));
215 if (!__tt)
216 return THERMAL_ERROR;
217
218 __tt[size - 1].temperature = nla_get_u32(attr);
219 }
220
221 if (nla_type(attr) == THERMAL_GENL_ATTR_THRESHOLD_DIRECTION)
222 __tt[size - 1].direction = nla_get_u32(attr);
223 }
224
225 if (__tt)
226 __tt[size].temperature = INT_MAX;
227
228 tz->thresholds = __tt;
229
230 return THERMAL_SUCCESS;
231}
232
233static int handle_netlink(struct nl_cache_ops *unused,
234 struct genl_cmd *cmd,
235 struct genl_info *info, void *arg)
236{
237 int ret;
238
239 switch (cmd->c_id) {
240
241 case THERMAL_GENL_CMD_TZ_GET_ID:
242 ret = parse_tz_get(info, arg);
243 break;
244
245 case THERMAL_GENL_CMD_CDEV_GET:
246 ret = parse_cdev_get(info, arg);
247 break;
248
249 case THERMAL_GENL_CMD_TZ_GET_TEMP:
250 ret = parse_tz_get_temp(info, arg);
251 break;
252
253 case THERMAL_GENL_CMD_TZ_GET_TRIP:
254 ret = parse_tz_get_trip(info, arg);
255 break;
256
257 case THERMAL_GENL_CMD_TZ_GET_GOV:
258 ret = parse_tz_get_gov(info, arg);
259 break;
260
261 case THERMAL_GENL_CMD_THRESHOLD_GET:
262 ret = parse_threshold_get(info, arg);
263 break;
264
265 default:
266 return THERMAL_ERROR;
267 }
268
269 return ret;
270}
271
272static struct genl_cmd thermal_cmds[] = {
273 {
274 .c_id = THERMAL_GENL_CMD_TZ_GET_ID,
275 .c_name = (char *)"List thermal zones",
276 .c_msg_parser = handle_netlink,
277 .c_maxattr = THERMAL_GENL_ATTR_MAX,
278 .c_attr_policy = thermal_genl_policy,
279 },
280 {
281 .c_id = THERMAL_GENL_CMD_TZ_GET_GOV,
282 .c_name = (char *)"Get governor",
283 .c_msg_parser = handle_netlink,
284 .c_maxattr = THERMAL_GENL_ATTR_MAX,
285 .c_attr_policy = thermal_genl_policy,
286 },
287 {
288 .c_id = THERMAL_GENL_CMD_TZ_GET_TEMP,
289 .c_name = (char *)"Get thermal zone temperature",
290 .c_msg_parser = handle_netlink,
291 .c_maxattr = THERMAL_GENL_ATTR_MAX,
292 .c_attr_policy = thermal_genl_policy,
293 },
294 {
295 .c_id = THERMAL_GENL_CMD_TZ_GET_TRIP,
296 .c_name = (char *)"Get thermal zone trip points",
297 .c_msg_parser = handle_netlink,
298 .c_maxattr = THERMAL_GENL_ATTR_MAX,
299 .c_attr_policy = thermal_genl_policy,
300 },
301 {
302 .c_id = THERMAL_GENL_CMD_CDEV_GET,
303 .c_name = (char *)"Get cooling devices",
304 .c_msg_parser = handle_netlink,
305 .c_maxattr = THERMAL_GENL_ATTR_MAX,
306 .c_attr_policy = thermal_genl_policy,
307 },
308 {
309 .c_id = THERMAL_GENL_CMD_THRESHOLD_GET,
310 .c_name = (char *)"Get thresholds list",
311 .c_msg_parser = handle_netlink,
312 .c_maxattr = THERMAL_GENL_ATTR_MAX,
313 .c_attr_policy = thermal_genl_policy,
314 },
315 {
316 .c_id = THERMAL_GENL_CMD_THRESHOLD_ADD,
317 .c_name = (char *)"Add a threshold",
318 .c_msg_parser = handle_netlink,
319 .c_maxattr = THERMAL_GENL_ATTR_MAX,
320 .c_attr_policy = thermal_genl_policy,
321 },
322 {
323 .c_id = THERMAL_GENL_CMD_THRESHOLD_DELETE,
324 .c_name = (char *)"Delete a threshold",
325 .c_msg_parser = handle_netlink,
326 .c_maxattr = THERMAL_GENL_ATTR_MAX,
327 .c_attr_policy = thermal_genl_policy,
328 },
329 {
330 .c_id = THERMAL_GENL_CMD_THRESHOLD_FLUSH,
331 .c_name = (char *)"Flush the thresholds",
332 .c_msg_parser = handle_netlink,
333 .c_maxattr = THERMAL_GENL_ATTR_MAX,
334 .c_attr_policy = thermal_genl_policy,
335 },
336};
337
338static struct genl_ops thermal_cmd_ops = {
339 .o_name = (char *)"thermal",
340 .o_cmds = thermal_cmds,
341 .o_ncmds = ARRAY_SIZE(thermal_cmds),
342};
343
344struct cmd_param {
345 int tz_id;
346 int temp;
347 int direction;
348};
349
350typedef int (*cmd_cb_t)(struct nl_msg *, struct cmd_param *);
351
352static int thermal_genl_tz_id_encode(struct nl_msg *msg, struct cmd_param *p)
353{
354 if (nla_put_u32(msg, THERMAL_GENL_ATTR_TZ_ID, p->tz_id))
355 return -1;
356
357 return 0;
358}
359
360static int thermal_genl_threshold_encode(struct nl_msg *msg, struct cmd_param *p)
361{
362 if (thermal_genl_tz_id_encode(msg, p))
363 return -1;
364
365 if (nla_put_u32(msg, THERMAL_GENL_ATTR_THRESHOLD_TEMP, p->temp))
366 return -1;
367
368 if (nla_put_u32(msg, THERMAL_GENL_ATTR_THRESHOLD_DIRECTION, p->direction))
369 return -1;
370
371 return 0;
372}
373
374static thermal_error_t thermal_genl_auto(struct thermal_handler *th, cmd_cb_t cmd_cb,
375 struct cmd_param *param,
376 int cmd, int flags, void *arg)
377{
378 thermal_error_t ret = THERMAL_ERROR;
379 struct nl_msg *msg;
380 void *hdr;
381
382 msg = nlmsg_alloc();
383 if (!msg)
384 return THERMAL_ERROR;
385
386 hdr = genlmsg_put(msg, NL_AUTO_PORT, NL_AUTO_SEQ, thermal_cmd_ops.o_id,
387 0, flags, cmd, THERMAL_GENL_VERSION);
388 if (!hdr)
389 goto out;
390
391 if (cmd_cb && cmd_cb(msg, param))
392 goto out;
393
394 if (nl_send_msg(th->sk_cmd, th->cb_cmd, msg, genl_handle_msg, arg))
395 goto out;
396
397 ret = THERMAL_SUCCESS;
398out:
399 nlmsg_free(msg);
400
401 return ret;
402}
403
404thermal_error_t thermal_cmd_get_tz(struct thermal_handler *th, struct thermal_zone **tz)
405{
406 return thermal_genl_auto(th, NULL, NULL, THERMAL_GENL_CMD_TZ_GET_ID,
407 NLM_F_DUMP | NLM_F_ACK, tz);
408}
409
410thermal_error_t thermal_cmd_get_cdev(struct thermal_handler *th, struct thermal_cdev **tc)
411{
412 return thermal_genl_auto(th, NULL, NULL, THERMAL_GENL_CMD_CDEV_GET,
413 NLM_F_DUMP | NLM_F_ACK, tc);
414}
415
416thermal_error_t thermal_cmd_get_trip(struct thermal_handler *th, struct thermal_zone *tz)
417{
418 struct cmd_param p = { .tz_id = tz->id };
419
420 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p,
421 THERMAL_GENL_CMD_TZ_GET_TRIP, 0, tz);
422}
423
424thermal_error_t thermal_cmd_get_governor(struct thermal_handler *th, struct thermal_zone *tz)
425{
426 struct cmd_param p = { .tz_id = tz->id };
427
428 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p,
429 THERMAL_GENL_CMD_TZ_GET_GOV, 0, tz);
430}
431
432thermal_error_t thermal_cmd_get_temp(struct thermal_handler *th, struct thermal_zone *tz)
433{
434 struct cmd_param p = { .tz_id = tz->id };
435
436 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p,
437 THERMAL_GENL_CMD_TZ_GET_TEMP, 0, tz);
438}
439
440thermal_error_t thermal_cmd_threshold_get(struct thermal_handler *th,
441 struct thermal_zone *tz)
442{
443 struct cmd_param p = { .tz_id = tz->id };
444
445 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p,
446 THERMAL_GENL_CMD_THRESHOLD_GET, 0, tz);
447}
448
449thermal_error_t thermal_cmd_threshold_add(struct thermal_handler *th,
450 struct thermal_zone *tz,
451 int temperature,
452 int direction)
453{
454 struct cmd_param p = { .tz_id = tz->id, .temp = temperature, .direction = direction };
455
456 return thermal_genl_auto(th, thermal_genl_threshold_encode, &p,
457 THERMAL_GENL_CMD_THRESHOLD_ADD, 0, tz);
458}
459
460thermal_error_t thermal_cmd_threshold_delete(struct thermal_handler *th,
461 struct thermal_zone *tz,
462 int temperature,
463 int direction)
464{
465 struct cmd_param p = { .tz_id = tz->id, .temp = temperature, .direction = direction };
466
467 return thermal_genl_auto(th, thermal_genl_threshold_encode, &p,
468 THERMAL_GENL_CMD_THRESHOLD_DELETE, 0, tz);
469}
470
471thermal_error_t thermal_cmd_threshold_flush(struct thermal_handler *th,
472 struct thermal_zone *tz)
473{
474 struct cmd_param p = { .tz_id = tz->id };
475
476 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p,
477 THERMAL_GENL_CMD_THRESHOLD_FLUSH, 0, tz);
478}
479
480thermal_error_t thermal_cmd_exit(struct thermal_handler *th)
481{
482 if (genl_unregister_family(&thermal_cmd_ops))
483 return THERMAL_ERROR;
484
485 nl_thermal_disconnect(th->sk_cmd, th->cb_cmd);
486
487 return THERMAL_SUCCESS;
488}
489
490thermal_error_t thermal_cmd_init(struct thermal_handler *th)
491{
492 int ret;
493 int family;
494
495 if (nl_thermal_connect(&th->sk_cmd, &th->cb_cmd))
496 return THERMAL_ERROR;
497
498 ret = genl_register_family(&thermal_cmd_ops);
499 if (ret)
500 return THERMAL_ERROR;
501
502 ret = genl_ops_resolve(th->sk_cmd, &thermal_cmd_ops);
503 if (ret)
504 return THERMAL_ERROR;
505
506 family = genl_ctrl_resolve(th->sk_cmd, "nlctrl");
507 if (family != GENL_ID_CTRL)
508 return THERMAL_ERROR;
509
510 return THERMAL_SUCCESS;
511}