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
thermal_genl_auto(struct thermal_handler * th,int id,int cmd,int flags,void * arg)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
thermal_cmd_get_tz(struct thermal_handler * th,struct thermal_zone ** tz)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
thermal_cmd_get_cdev(struct thermal_handler * th,struct thermal_cdev ** tc)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
thermal_cmd_get_trip(struct thermal_handler * th,struct thermal_zone * tz)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
thermal_cmd_get_governor(struct thermal_handler * th,struct thermal_zone * tz)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
thermal_cmd_get_temp(struct thermal_handler * th,struct thermal_zone * tz)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
thermal_cmd_exit(struct thermal_handler * th)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(th->sk_cmd, th->cb_cmd);
324
325 return THERMAL_SUCCESS;
326 }
327
thermal_cmd_init(struct thermal_handler * th)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