309 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Intel Speed Select -- Read HFI events for OOB4 * Copyright (c) 2022 Intel Corporation.5 */6 7/*8 * This file incorporates work covered by the following copyright and9 * permission notice:10 11 * WPA Supplicant - driver interaction with Linux nl80211/cfg8021112 * Copyright (c) 2003-2008, Jouni Malinen <j@w1.fi>13 *14 * This program is free software; you can redistribute it and/or modify15 * it under the terms of the GNU General Public License version 2 as16 * published by the Free Software Foundation.17 *18 * Alternatively, this software may be distributed under the terms of19 * BSD license.20 *21 * Requires22 * libnl-genl-3-dev23 *24 * For Fedora/CenOS25 * dnf install libnl3-devel26 * For Ubuntu27 * apt install libnl-3-dev libnl-genl-3-dev28 */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