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 
49 struct hfi_event_data {
50 	struct nl_sock *nl_handle;
51 	struct nl_cb *nl_cb;
52 };
53 
54 struct hfi_event_data drv;
55 
ack_handler(struct nl_msg * msg,void * arg)56 static int ack_handler(struct nl_msg *msg, void *arg)
57 {
58 	int *err = arg;
59 	*err = 0;
60 	return NL_STOP;
61 }
62 
finish_handler(struct nl_msg * msg,void * arg)63 static int finish_handler(struct nl_msg *msg, void *arg)
64 {
65 	int *ret = arg;
66 	*ret = 0;
67 	return NL_SKIP;
68 }
69 
error_handler(struct sockaddr_nl * nla,struct nlmsgerr * err,void * arg)70 static 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 
seq_check_handler(struct nl_msg * msg,void * arg)78 static int seq_check_handler(struct nl_msg *msg, void *arg)
79 {
80 	return NL_OK;
81 }
82 
send_and_recv_msgs(struct hfi_event_data * drv,struct nl_msg * msg,int (* valid_handler)(struct nl_msg *,void *),void * valid_data)83 static 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 
117 struct family_data {
118 	const char *group;
119 	int id;
120 };
121 
family_handler(struct nl_msg * msg,void * arg)122 static 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 
nl_get_multicast_id(struct hfi_event_data * drv,const char * family,const char * group)152 static 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 
171 nla_put_failure:
172 	nlmsg_free(msg);
173 	return ret;
174 }
175 
176 struct perf_cap {
177 	int cpu;
178 	int perf;
179 	int eff;
180 };
181 
process_hfi_event(struct perf_cap * perf_cap)182 static 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 
handle_event(struct nl_msg * n,void * arg)190 static 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 
231 static int _hfi_exit;
232 
check_hf_suport(void)233 static 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 
hfi_main(void)244 int 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 */
299 free_sock:
300 	nl_socket_free(sock);
301 
302 	return -1;
303 }
304 
hfi_exit(void)305 void hfi_exit(void)
306 {
307 	_hfi_exit = 1;
308 }
309