1// SPDX-License-Identifier: LGPL-2.1+
2// Copyright (C) 2022, Linaro Ltd - Daniel Lezcano <daniel.lezcano@linaro.org>
3#include <errno.h>
4#include <stdio.h>
5#include <stdlib.h>
6#include <unistd.h>
7
8#include <thermal.h>
9#include "thermal_nl.h"
10
11struct handler_args {
12	const char *group;
13	int id;
14};
15
16static __thread int err;
17static __thread int done;
18
19static int nl_seq_check_handler(struct nl_msg *msg, void *arg)
20{
21	return NL_OK;
22}
23
24static int nl_error_handler(struct sockaddr_nl *nla, struct nlmsgerr *nl_err,
25			    void *arg)
26{
27	int *ret = arg;
28
29	if (ret)
30		*ret = nl_err->error;
31
32	return NL_STOP;
33}
34
35static int nl_finish_handler(struct nl_msg *msg, void *arg)
36{
37	int *ret = arg;
38
39	if (ret)
40		*ret = 1;
41
42	return NL_OK;
43}
44
45static int nl_ack_handler(struct nl_msg *msg, void *arg)
46{
47	int *ret = arg;
48
49	if (ret)
50		*ret = 1;
51
52	return NL_OK;
53}
54
55int nl_send_msg(struct nl_sock *sock, struct nl_cb *cb, struct nl_msg *msg,
56		int (*rx_handler)(struct nl_msg *, void *), void *data)
57{
58	if (!rx_handler)
59		return THERMAL_ERROR;
60
61	err = nl_send_auto_complete(sock, msg);
62	if (err < 0)
63		return err;
64
65	nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, rx_handler, data);
66
67	err = done = 0;
68
69	while (err == 0 && done == 0)
70		nl_recvmsgs(sock, cb);
71
72	return err;
73}
74
75static int nl_family_handler(struct nl_msg *msg, void *arg)
76{
77	struct handler_args *grp = arg;
78	struct nlattr *tb[CTRL_ATTR_MAX + 1];
79	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
80	struct nlattr *mcgrp;
81	int rem_mcgrp;
82
83	nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
84		  genlmsg_attrlen(gnlh, 0), NULL);
85
86	if (!tb[CTRL_ATTR_MCAST_GROUPS])
87		return THERMAL_ERROR;
88
89	nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], rem_mcgrp) {
90
91		struct nlattr *tb_mcgrp[CTRL_ATTR_MCAST_GRP_MAX + 1];
92
93		nla_parse(tb_mcgrp, CTRL_ATTR_MCAST_GRP_MAX,
94			  nla_data(mcgrp), nla_len(mcgrp), NULL);
95
96		if (!tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME] ||
97		    !tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID])
98			continue;
99
100		if (strncmp(nla_data(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]),
101			    grp->group,
102			    nla_len(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME])))
103			continue;
104
105		grp->id = nla_get_u32(tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]);
106
107		break;
108	}
109
110	return THERMAL_SUCCESS;
111}
112
113static int nl_get_multicast_id(struct nl_sock *sock, struct nl_cb *cb,
114			       const char *family, const char *group)
115{
116	struct nl_msg *msg;
117	int ret = 0, ctrlid;
118	struct handler_args grp = {
119		.group = group,
120		.id = -ENOENT,
121	};
122
123	msg = nlmsg_alloc();
124	if (!msg)
125		return THERMAL_ERROR;
126
127	ctrlid = genl_ctrl_resolve(sock, "nlctrl");
128
129	genlmsg_put(msg, 0, 0, ctrlid, 0, 0, CTRL_CMD_GETFAMILY, 0);
130
131	nla_put_string(msg, CTRL_ATTR_FAMILY_NAME, family);
132
133	ret = nl_send_msg(sock, cb, msg, nl_family_handler, &grp);
134	if (ret)
135		goto nla_put_failure;
136
137	ret = grp.id;
138
139nla_put_failure:
140	nlmsg_free(msg);
141	return ret;
142}
143
144int nl_thermal_connect(struct nl_sock **nl_sock, struct nl_cb **nl_cb)
145{
146	struct nl_cb *cb;
147	struct nl_sock *sock;
148
149	cb = nl_cb_alloc(NL_CB_DEFAULT);
150	if (!cb)
151		return THERMAL_ERROR;
152
153	sock = nl_socket_alloc();
154	if (!sock)
155		goto out_cb_free;
156
157	if (genl_connect(sock))
158		goto out_socket_free;
159
160	if (nl_cb_err(cb, NL_CB_CUSTOM, nl_error_handler, &err) ||
161	    nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, nl_finish_handler, &done) ||
162	    nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, nl_ack_handler, &done) ||
163	    nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, nl_seq_check_handler, &done))
164		return THERMAL_ERROR;
165
166	*nl_sock = sock;
167	*nl_cb = cb;
168
169	return THERMAL_SUCCESS;
170
171out_socket_free:
172	nl_socket_free(sock);
173out_cb_free:
174	nl_cb_put(cb);
175	return THERMAL_ERROR;
176}
177
178void nl_thermal_disconnect(struct nl_sock *nl_sock, struct nl_cb *nl_cb)
179{
180	nl_close(nl_sock);
181	nl_socket_free(nl_sock);
182	nl_cb_put(nl_cb);
183}
184
185int nl_unsubscribe_thermal(struct nl_sock *nl_sock, struct nl_cb *nl_cb,
186			   const char *group)
187{
188	int mcid;
189
190	mcid = nl_get_multicast_id(nl_sock, nl_cb, THERMAL_GENL_FAMILY_NAME,
191				   group);
192	if (mcid < 0)
193		return THERMAL_ERROR;
194
195	if (nl_socket_drop_membership(nl_sock, mcid))
196		return THERMAL_ERROR;
197
198	return THERMAL_SUCCESS;
199}
200
201int nl_subscribe_thermal(struct nl_sock *nl_sock, struct nl_cb *nl_cb,
202			 const char *group)
203{
204	int mcid;
205
206	mcid = nl_get_multicast_id(nl_sock, nl_cb, THERMAL_GENL_FAMILY_NAME,
207				   group);
208	if (mcid < 0)
209		return THERMAL_ERROR;
210
211	if (nl_socket_add_membership(nl_sock, mcid))
212		return THERMAL_ERROR;
213
214	return THERMAL_SUCCESS;
215}
216