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 int no_block = 0;
251
252 if (!check_hf_suport()) {
253 fprintf(stderr, "CPU Doesn't support HFI\n");
254 return -1;
255 }
256
257 sock = nl_socket_alloc();
258 if (!sock) {
259 fprintf(stderr, "nl_socket_alloc failed\n");
260 return -1;
261 }
262
263 if (genl_connect(sock)) {
264 fprintf(stderr, "genl_connect(sk_event) failed\n");
265 goto free_sock;
266 }
267
268 drv.nl_handle = sock;
269 drv.nl_cb = cb = nl_cb_alloc(NL_CB_DEFAULT);
270 if (drv.nl_cb == NULL) {
271 printf("Failed to allocate netlink callbacks");
272 goto free_sock;
273 }
274
275 mcast_id = nl_get_multicast_id(&drv, THERMAL_GENL_FAMILY_NAME,
276 THERMAL_GENL_EVENT_GROUP_NAME);
277 if (mcast_id < 0) {
278 fprintf(stderr, "nl_get_multicast_id failed\n");
279 goto free_sock;
280 }
281
282 if (nl_socket_add_membership(sock, mcast_id)) {
283 fprintf(stderr, "nl_socket_add_membership failed");
284 goto free_sock;
285 }
286
287 nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, seq_check_handler, 0);
288 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, handle_event, NULL);
289
290 if (no_block)
291 nl_socket_set_nonblocking(sock);
292
293 debug_printf("hfi is initialized\n");
294
295 while (!_hfi_exit && !err) {
296 err = nl_recvmsgs(sock, cb);
297 debug_printf("nl_recv_message err:%d\n", err);
298 }
299
300 return 0;
301
302 /* Netlink library doesn't have calls to dealloc cb or disconnect */
303 free_sock:
304 nl_socket_free(sock);
305
306 return -1;
307 }
308
hfi_exit(void)309 void hfi_exit(void)
310 {
311 _hfi_exit = 1;
312 }
313
