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
12static 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
38static 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
72static 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
112static 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
150static 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
166static 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
185static 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
220static 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
258static 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
264static 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
290thermal_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
296thermal_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
302thermal_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
308thermal_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
313thermal_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
318thermal_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
328thermal_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