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