1// SPDX-License-Identifier: GPL-2.0
2/*
3 * Intel Speed Select -- Read HFI events for OOB
4 * Copyright (c) 2022 Intel Corporation.
5 */
6
7/*
8 * This file incorporates work covered by the following copyright and
9 * permission notice:
10
11 * WPA Supplicant - driver interaction with Linux nl80211/cfg80211
12 * Copyright (c) 2003-2008, Jouni Malinen <j@w1.fi>
13 *
14 * This program is free software; you can redistribute it and/or modify
15 * it under the terms of the GNU General Public License version 2 as
16 * published by the Free Software Foundation.
17 *
18 * Alternatively, this software may be distributed under the terms of
19 * BSD license.
20 *
21 * Requires
22 * libnl-genl-3-dev
23 *
24 * For Fedora/CenOS
25 * dnf install libnl3-devel
26 * For Ubuntu
27 * apt install libnl-3-dev libnl-genl-3-dev
28 */
29
30#include <stdio.h>
31#include <stdlib.h>
32#include <stdarg.h>
33#include <string.h>
34#include <unistd.h>
35#include <fcntl.h>
36#include <sys/file.h>
37#include <sys/types.h>
38#include <sys/stat.h>
39#include <errno.h>
40#include <getopt.h>
41#include <signal.h>
42#include <netlink/genl/genl.h>
43#include <netlink/genl/family.h>
44#include <netlink/genl/ctrl.h>
45
46#include <linux/thermal.h>
47#include "isst.h"
48
49struct hfi_event_data {
50	struct nl_sock *nl_handle;
51	struct nl_cb *nl_cb;
52};
53
54struct hfi_event_data drv;
55
56static int ack_handler(struct nl_msg *msg, void *arg)
57{
58	int *err = arg;
59	*err = 0;
60	return NL_STOP;
61}
62
63static int finish_handler(struct nl_msg *msg, void *arg)
64{
65	int *ret = arg;
66	*ret = 0;
67	return NL_SKIP;
68}
69
70static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err,
71			 void *arg)
72{
73	int *ret = arg;
74	*ret = err->error;
75	return NL_SKIP;
76}
77
78static int seq_check_handler(struct nl_msg *msg, void *arg)
79{
80	return NL_OK;
81}
82
83static int send_and_recv_msgs(struct hfi_event_data *drv,
84			      struct nl_msg *msg,
85			      int (*valid_handler)(struct nl_msg *, void *),
86			      void *valid_data)
87{
88	struct nl_cb *cb;
89	int err = -ENOMEM;
90
91	cb = nl_cb_clone(drv->nl_cb);
92	if (!cb)
93		goto out;
94
95	err = nl_send_auto_complete(drv->nl_handle, msg);
96	if (err < 0)
97		goto out;
98
99	err = 1;
100
101	nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
102	nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
103	nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
104
105	if (valid_handler)
106		nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM,
107			  valid_handler, valid_data);
108
109	while (err > 0)
110		nl_recvmsgs(drv->nl_handle, cb);
111 out:
112	nl_cb_put(cb);
113	nlmsg_free(msg);
114	return err;
115}
116
117struct family_data {
118	const char *group;
119	int id;
120};
121
122static int family_handler(struct nl_msg *msg, void *arg)
123{
124	struct family_data *res = arg;
125	struct nlattr *tb[CTRL_ATTR_MAX + 1];
126	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
127	struct nlattr *mcgrp;
128	int i;
129
130	nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
131		  genlmsg_attrlen(gnlh, 0), NULL);
132	if (!tb[CTRL_ATTR_MCAST_GROUPS])
133		return NL_SKIP;
134
135	nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], i) {
136		struct nlattr *tb2[CTRL_ATTR_MCAST_GRP_MAX + 1];
137		nla_parse(tb2, CTRL_ATTR_MCAST_GRP_MAX, nla_data(mcgrp),
138			  nla_len(mcgrp), NULL);
139		if (!tb2[CTRL_ATTR_MCAST_GRP_NAME] ||
140		    !tb2[CTRL_ATTR_MCAST_GRP_ID] ||
141		    strncmp(nla_data(tb2[CTRL_ATTR_MCAST_GRP_NAME]),
142				res->group,
143				nla_len(tb2[CTRL_ATTR_MCAST_GRP_NAME])) != 0)
144			continue;
145		res->id = nla_get_u32(tb2[CTRL_ATTR_MCAST_GRP_ID]);
146		break;
147	}
148
149	return 0;
150}
151
152static int nl_get_multicast_id(struct hfi_event_data *drv,
153			       const char *family, const char *group)
154{
155	struct nl_msg *msg;
156	int ret = -1;
157	struct family_data res = { group, -ENOENT };
158
159	msg = nlmsg_alloc();
160	if (!msg)
161		return -ENOMEM;
162	genlmsg_put(msg, 0, 0, genl_ctrl_resolve(drv->nl_handle, "nlctrl"),
163		    0, 0, CTRL_CMD_GETFAMILY, 0);
164	NLA_PUT_STRING(msg, CTRL_ATTR_FAMILY_NAME, family);
165
166	ret = send_and_recv_msgs(drv, msg, family_handler, &res);
167	msg = NULL;
168	if (ret == 0)
169		ret = res.id;
170
171nla_put_failure:
172	nlmsg_free(msg);
173	return ret;
174}
175
176struct perf_cap {
177	int cpu;
178	int perf;
179	int eff;
180};
181
182static void process_hfi_event(struct perf_cap *perf_cap)
183{
184	struct isst_id id;
185
186	set_isst_id(&id, perf_cap->cpu);
187	process_level_change(&id);
188}
189
190static int handle_event(struct nl_msg *n, void *arg)
191{
192	struct nlmsghdr *nlh = nlmsg_hdr(n);
193	struct genlmsghdr *genlhdr = genlmsg_hdr(nlh);
194	struct nlattr *attrs[THERMAL_GENL_ATTR_MAX + 1];
195	int ret;
196	struct perf_cap perf_cap = {0};
197
198	ret = genlmsg_parse(nlh, 0, attrs, THERMAL_GENL_ATTR_MAX, NULL);
199
200	debug_printf("Received event %d parse_rer:%d\n", genlhdr->cmd, ret);
201	if (genlhdr->cmd == THERMAL_GENL_EVENT_CPU_CAPABILITY_CHANGE) {
202		struct nlattr *cap;
203		int j, index = 0;
204
205		debug_printf("THERMAL_GENL_EVENT_CPU_CAPABILITY_CHANGE\n");
206		nla_for_each_nested(cap, attrs[THERMAL_GENL_ATTR_CPU_CAPABILITY], j) {
207			switch (index) {
208			case 0:
209				perf_cap.cpu = nla_get_u32(cap);
210				break;
211			case 1:
212				perf_cap.perf = nla_get_u32(cap);
213				break;
214			case 2:
215				perf_cap.eff = nla_get_u32(cap);
216				break;
217			default:
218				break;
219			}
220			++index;
221			if (index == 3) {
222				index = 0;
223				process_hfi_event(&perf_cap);
224			}
225		}
226	}
227
228	return 0;
229}
230
231static int _hfi_exit;
232
233static int check_hf_suport(void)
234{
235	unsigned int eax = 0, ebx = 0, ecx = 0, edx = 0;
236
237	__cpuid(6, eax, ebx, ecx, edx);
238	if (eax & BIT(19))
239		return 1;
240
241	return 0;
242}
243
244int hfi_main(void)
245{
246	struct nl_sock *sock;
247	struct nl_cb *cb;
248	int err = 0;
249	int mcast_id;
250
251	if (!check_hf_suport()) {
252		fprintf(stderr, "CPU Doesn't support HFI\n");
253		return -1;
254	}
255
256	sock = nl_socket_alloc();
257	if (!sock) {
258		fprintf(stderr, "nl_socket_alloc failed\n");
259		return -1;
260	}
261
262	if (genl_connect(sock)) {
263		fprintf(stderr, "genl_connect(sk_event) failed\n");
264		goto free_sock;
265	}
266
267	drv.nl_handle = sock;
268	drv.nl_cb = cb = nl_cb_alloc(NL_CB_DEFAULT);
269	if (drv.nl_cb == NULL) {
270		printf("Failed to allocate netlink callbacks");
271		goto free_sock;
272	}
273
274	mcast_id = nl_get_multicast_id(&drv, THERMAL_GENL_FAMILY_NAME,
275				   THERMAL_GENL_EVENT_GROUP_NAME);
276	if (mcast_id < 0) {
277		fprintf(stderr, "nl_get_multicast_id failed\n");
278		goto free_sock;
279	}
280
281	if (nl_socket_add_membership(sock, mcast_id)) {
282		fprintf(stderr, "nl_socket_add_membership failed");
283		goto free_sock;
284	}
285
286	nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, seq_check_handler, 0);
287	nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, handle_event, NULL);
288
289	debug_printf("hfi is initialized\n");
290
291	while (!_hfi_exit && !err) {
292		err = nl_recvmsgs(sock, cb);
293		debug_printf("nl_recv_message err:%d\n", err);
294	}
295
296	return 0;
297
298	/* Netlink library doesn't have calls to dealloc cb or disconnect */
299free_sock:
300	nl_socket_free(sock);
301
302	return -1;
303}
304
305void hfi_exit(void)
306{
307	_hfi_exit = 1;
308}
309