1 // SPDX-License-Identifier: GPL-2.0
3 * Intel Speed Select -- Read HFI events for OOB
4 * Copyright (c) 2022 Intel Corporation.
8 * This file incorporates work covered by the following copyright and
11 * WPA Supplicant - driver interaction with Linux nl80211/cfg80211
12 * Copyright (c) 2003-2008, Jouni Malinen <j@w1.fi>
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.
18 * Alternatively, this software may be distributed under the terms of
25 * dnf install libnl3-devel
27 * apt install libnl-3-dev libnl-genl-3-dev
37 #include <sys/types.h>
42 #include <netlink/genl/genl.h>
43 #include <netlink/genl/family.h>
44 #include <netlink/genl/ctrl.h>
46 #include <linux/thermal.h>
49 struct hfi_event_data {
50 struct nl_sock *nl_handle;
54 struct hfi_event_data drv;
56 static int ack_handler(struct nl_msg *msg, void *arg)
63 static int finish_handler(struct nl_msg *msg, void *arg)
70 static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err,
78 static int seq_check_handler(struct nl_msg *msg, void *arg)
83 static int send_and_recv_msgs(struct hfi_event_data *drv,
85 int (*valid_handler)(struct nl_msg *, void *),
91 cb = nl_cb_clone(drv->nl_cb);
95 err = nl_send_auto_complete(drv->nl_handle, msg);
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);
106 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM,
107 valid_handler, valid_data);
110 nl_recvmsgs(drv->nl_handle, cb);
122 static int family_handler(struct nl_msg *msg, void *arg)
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;
130 nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
131 genlmsg_attrlen(gnlh, 0), NULL);
132 if (!tb[CTRL_ATTR_MCAST_GROUPS])
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]),
143 nla_len(tb2[CTRL_ATTR_MCAST_GRP_NAME])) != 0)
145 res->id = nla_get_u32(tb2[CTRL_ATTR_MCAST_GRP_ID]);
152 static int nl_get_multicast_id(struct hfi_event_data *drv,
153 const char *family, const char *group)
157 struct family_data res = { group, -ENOENT };
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);
166 ret = send_and_recv_msgs(drv, msg, family_handler, &res);
182 static void process_hfi_event(struct perf_cap *perf_cap)
186 set_isst_id(&id, perf_cap->cpu);
187 process_level_change(&id);
190 static int handle_event(struct nl_msg *n, void *arg)
192 struct nlmsghdr *nlh = nlmsg_hdr(n);
193 struct genlmsghdr *genlhdr = genlmsg_hdr(nlh);
194 struct nlattr *attrs[THERMAL_GENL_ATTR_MAX + 1];
196 struct perf_cap perf_cap = {0};
198 ret = genlmsg_parse(nlh, 0, attrs, THERMAL_GENL_ATTR_MAX, NULL);
200 debug_printf("Received event %d parse_rer:%d\n", genlhdr->cmd, ret);
201 if (genlhdr->cmd == THERMAL_GENL_EVENT_CPU_CAPABILITY_CHANGE) {
205 debug_printf("THERMAL_GENL_EVENT_CPU_CAPABILITY_CHANGE\n");
206 nla_for_each_nested(cap, attrs[THERMAL_GENL_ATTR_CPU_CAPABILITY], j) {
209 perf_cap.cpu = nla_get_u32(cap);
212 perf_cap.perf = nla_get_u32(cap);
215 perf_cap.eff = nla_get_u32(cap);
223 process_hfi_event(&perf_cap);
231 static int _hfi_exit;
233 static int check_hf_suport(void)
235 unsigned int eax = 0, ebx = 0, ecx = 0, edx = 0;
237 __cpuid(6, eax, ebx, ecx, edx);
246 struct nl_sock *sock;
251 if (!check_hf_suport()) {
252 fprintf(stderr, "CPU Doesn't support HFI\n");
256 sock = nl_socket_alloc();
258 fprintf(stderr, "nl_socket_alloc failed\n");
262 if (genl_connect(sock)) {
263 fprintf(stderr, "genl_connect(sk_event) failed\n");
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");
274 mcast_id = nl_get_multicast_id(&drv, THERMAL_GENL_FAMILY_NAME,
275 THERMAL_GENL_EVENT_GROUP_NAME);
277 fprintf(stderr, "nl_get_multicast_id failed\n");
281 if (nl_socket_add_membership(sock, mcast_id)) {
282 fprintf(stderr, "nl_socket_add_membership failed");
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);
289 debug_printf("hfi is initialized\n");
291 while (!_hfi_exit && !err) {
292 err = nl_recvmsgs(sock, cb);
293 debug_printf("nl_recv_message err:%d\n", err);
298 /* Netlink library doesn't have calls to dealloc cb or disconnect */
300 nl_socket_free(sock);