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