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
parse_tz_get(struct genl_info * info,struct thermal_zone ** tz)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
parse_cdev_get(struct genl_info * info,struct thermal_cdev ** cdev)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
parse_tz_get_trip(struct genl_info * info,struct thermal_zone * tz)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
parse_tz_get_temp(struct genl_info * info,struct thermal_zone * tz)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
parse_tz_get_gov(struct genl_info * info,struct thermal_zone * tz)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
handle_netlink(struct nl_cache_ops * unused,struct genl_cmd * cmd,struct genl_info * info,void * arg)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, arg);
195 break;
196
197 case THERMAL_GENL_CMD_CDEV_GET:
198 ret = parse_cdev_get(info, arg);
199 break;
200
201 case THERMAL_GENL_CMD_TZ_GET_TEMP:
202 ret = parse_tz_get_temp(info, arg);
203 break;
204
205 case THERMAL_GENL_CMD_TZ_GET_TRIP:
206 ret = parse_tz_get_trip(info, arg);
207 break;
208
209 case THERMAL_GENL_CMD_TZ_GET_GOV:
210 ret = parse_tz_get_gov(info, 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 struct cmd_param {
265 int tz_id;
266 };
267
268 typedef int (*cmd_cb_t)(struct nl_msg *, struct cmd_param *);
269
thermal_genl_tz_id_encode(struct nl_msg * msg,struct cmd_param * p)270 static int thermal_genl_tz_id_encode(struct nl_msg *msg, struct cmd_param *p)
271 {
272 if (p->tz_id >= 0 && nla_put_u32(msg, THERMAL_GENL_ATTR_TZ_ID, p->tz_id))
273 return -1;
274
275 return 0;
276 }
277
thermal_genl_auto(struct thermal_handler * th,cmd_cb_t cmd_cb,struct cmd_param * param,int cmd,int flags,void * arg)278 static thermal_error_t thermal_genl_auto(struct thermal_handler *th, cmd_cb_t cmd_cb,
279 struct cmd_param *param,
280 int cmd, int flags, void *arg)
281 {
282 thermal_error_t ret = THERMAL_ERROR;
283 struct nl_msg *msg;
284 void *hdr;
285
286 msg = nlmsg_alloc();
287 if (!msg)
288 return THERMAL_ERROR;
289
290 hdr = genlmsg_put(msg, NL_AUTO_PORT, NL_AUTO_SEQ, thermal_cmd_ops.o_id,
291 0, flags, cmd, THERMAL_GENL_VERSION);
292 if (!hdr)
293 goto out;
294
295 if (cmd_cb && cmd_cb(msg, param))
296 goto out;
297
298 if (nl_send_msg(th->sk_cmd, th->cb_cmd, msg, genl_handle_msg, arg))
299 goto out;
300
301 ret = THERMAL_SUCCESS;
302 out:
303 nlmsg_free(msg);
304
305 return ret;
306 }
307
thermal_cmd_get_tz(struct thermal_handler * th,struct thermal_zone ** tz)308 thermal_error_t thermal_cmd_get_tz(struct thermal_handler *th, struct thermal_zone **tz)
309 {
310 return thermal_genl_auto(th, NULL, NULL, THERMAL_GENL_CMD_TZ_GET_ID,
311 NLM_F_DUMP | NLM_F_ACK, tz);
312 }
313
thermal_cmd_get_cdev(struct thermal_handler * th,struct thermal_cdev ** tc)314 thermal_error_t thermal_cmd_get_cdev(struct thermal_handler *th, struct thermal_cdev **tc)
315 {
316 return thermal_genl_auto(th, NULL, NULL, THERMAL_GENL_CMD_CDEV_GET,
317 NLM_F_DUMP | NLM_F_ACK, tc);
318 }
319
thermal_cmd_get_trip(struct thermal_handler * th,struct thermal_zone * tz)320 thermal_error_t thermal_cmd_get_trip(struct thermal_handler *th, struct thermal_zone *tz)
321 {
322 struct cmd_param p = { .tz_id = tz->id };
323
324 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p,
325 THERMAL_GENL_CMD_TZ_GET_TRIP, 0, tz);
326 }
327
thermal_cmd_get_governor(struct thermal_handler * th,struct thermal_zone * tz)328 thermal_error_t thermal_cmd_get_governor(struct thermal_handler *th, struct thermal_zone *tz)
329 {
330 struct cmd_param p = { .tz_id = tz->id };
331
332 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p,
333 THERMAL_GENL_CMD_TZ_GET_GOV, 0, tz);
334 }
335
thermal_cmd_get_temp(struct thermal_handler * th,struct thermal_zone * tz)336 thermal_error_t thermal_cmd_get_temp(struct thermal_handler *th, struct thermal_zone *tz)
337 {
338 struct cmd_param p = { .tz_id = tz->id };
339
340 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p,
341 THERMAL_GENL_CMD_TZ_GET_TEMP, 0, tz);
342 }
343
thermal_cmd_exit(struct thermal_handler * th)344 thermal_error_t thermal_cmd_exit(struct thermal_handler *th)
345 {
346 if (genl_unregister_family(&thermal_cmd_ops))
347 return THERMAL_ERROR;
348
349 nl_thermal_disconnect(th->sk_cmd, th->cb_cmd);
350
351 return THERMAL_SUCCESS;
352 }
353
thermal_cmd_init(struct thermal_handler * th)354 thermal_error_t thermal_cmd_init(struct thermal_handler *th)
355 {
356 int ret;
357 int family;
358
359 if (nl_thermal_connect(&th->sk_cmd, &th->cb_cmd))
360 return THERMAL_ERROR;
361
362 ret = genl_register_family(&thermal_cmd_ops);
363 if (ret)
364 return THERMAL_ERROR;
365
366 ret = genl_ops_resolve(th->sk_cmd, &thermal_cmd_ops);
367 if (ret)
368 return THERMAL_ERROR;
369
370 family = genl_ctrl_resolve(th->sk_cmd, "nlctrl");
371 if (family != GENL_ID_CTRL)
372 return THERMAL_ERROR;
373
374 return THERMAL_SUCCESS;
375 }
376