x86/intel-speed-select/hfi-events.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Intel Speed Select -- Read HFI events for OOB
0004  * Copyright (c) 2022 Intel Corporation.
0005  */
0006
0007 /*
0008  * This file incorporates work covered by the following copyright and
0009  * permission notice:
0010
0011  * WPA Supplicant - driver interaction with Linux nl80211/cfg80211
0012  * Copyright (c) 2003-2008, Jouni Malinen <j@w1.fi>
0013  *
0014  * This program is free software; you can redistribute it and/or modify
0015  * it under the terms of the GNU General Public License version 2 as
0016  * published by the Free Software Foundation.
0017  *
0018  * Alternatively, this software may be distributed under the terms of
0019  * BSD license.
0020  *
0021  * Requires
0022  * libnl-genl-3-dev
0023  *
0024  * For Fedora/CenOS
0025  * dnf install libnl3-devel
0026  * For Ubuntu
0027  * apt install libnl-3-dev libnl-genl-3-dev
0028  */
0029
0030 #include <stdio.h>
0031 #include <stdlib.h>
0032 #include <stdarg.h>
0033 #include <string.h>
0034 #include <unistd.h>
0035 #include <fcntl.h>
0036 #include <sys/file.h>
0037 #include <sys/types.h>
0038 #include <sys/stat.h>
0039 #include <errno.h>
0040 #include <getopt.h>
0041 #include <signal.h>
0042 #include <netlink/genl/genl.h>
0043 #include <netlink/genl/family.h>
0044 #include <netlink/genl/ctrl.h>
0045
0046 #include <linux/thermal.h>
0047 #include "isst.h"
0048
0049 struct hfi_event_data {
0050     struct nl_sock *nl_handle;
0051     struct nl_cb *nl_cb;
0052 };
0053
0054 struct hfi_event_data drv;
0055
0056 static int ack_handler(struct nl_msg *msg, void *arg)
0057 {
0058     int *err = arg;
0059     *err = 0;
0060     return NL_STOP;
0061 }
0062
0063 static int finish_handler(struct nl_msg *msg, void *arg)
0064 {
0065     int *ret = arg;
0066     *ret = 0;
0067     return NL_SKIP;
0068 }
0069
0070 static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err,
0071              void *arg)
0072 {
0073     int *ret = arg;
0074     *ret = err->error;
0075     return NL_SKIP;
0076 }
0077
0078 static int seq_check_handler(struct nl_msg *msg, void *arg)
0079 {
0080     return NL_OK;
0081 }
0082
0083 static int send_and_recv_msgs(struct hfi_event_data *drv,
0084                   struct nl_msg *msg,
0085                   int (*valid_handler)(struct nl_msg *, void *),
0086                   void *valid_data)
0087 {
0088     struct nl_cb *cb;
0089     int err = -ENOMEM;
0090
0091     cb = nl_cb_clone(drv->nl_cb);
0092     if (!cb)
0093         goto out;
0094
0095     err = nl_send_auto_complete(drv->nl_handle, msg);
0096     if (err < 0)
0097         goto out;
0098
0099     err = 1;
0100
0101     nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
0102     nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
0103     nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
0104
0105     if (valid_handler)
0106         nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM,
0107               valid_handler, valid_data);
0108
0109     while (err > 0)
0110         nl_recvmsgs(drv->nl_handle, cb);
0111  out:
0112     nl_cb_put(cb);
0113     nlmsg_free(msg);
0114     return err;
0115 }
0116
0117 struct family_data {
0118     const char *group;
0119     int id;
0120 };
0121
0122 static int family_handler(struct nl_msg *msg, void *arg)
0123 {
0124     struct family_data *res = arg;
0125     struct nlattr *tb[CTRL_ATTR_MAX + 1];
0126     struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
0127     struct nlattr *mcgrp;
0128     int i;
0129
0130     nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
0131           genlmsg_attrlen(gnlh, 0), NULL);
0132     if (!tb[CTRL_ATTR_MCAST_GROUPS])
0133         return NL_SKIP;
0134
0135     nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], i) {
0136         struct nlattr *tb2[CTRL_ATTR_MCAST_GRP_MAX + 1];
0137         nla_parse(tb2, CTRL_ATTR_MCAST_GRP_MAX, nla_data(mcgrp),
0138               nla_len(mcgrp), NULL);
0139         if (!tb2[CTRL_ATTR_MCAST_GRP_NAME] ||
0140             !tb2[CTRL_ATTR_MCAST_GRP_ID] ||
0141             strncmp(nla_data(tb2[CTRL_ATTR_MCAST_GRP_NAME]),
0142                 res->group,
0143                 nla_len(tb2[CTRL_ATTR_MCAST_GRP_NAME])) != 0)
0144             continue;
0145         res->id = nla_get_u32(tb2[CTRL_ATTR_MCAST_GRP_ID]);
0146         break;
0147     }
0148
0149     return 0;
0150 }
0151
0152 static int nl_get_multicast_id(struct hfi_event_data *drv,
0153                    const char *family, const char *group)
0154 {
0155     struct nl_msg *msg;
0156     int ret = -1;
0157     struct family_data res = { group, -ENOENT };
0158
0159     msg = nlmsg_alloc();
0160     if (!msg)
0161         return -ENOMEM;
0162     genlmsg_put(msg, 0, 0, genl_ctrl_resolve(drv->nl_handle, "nlctrl"),
0163             0, 0, CTRL_CMD_GETFAMILY, 0);
0164     NLA_PUT_STRING(msg, CTRL_ATTR_FAMILY_NAME, family);
0165
0166     ret = send_and_recv_msgs(drv, msg, family_handler, &res);
0167     msg = NULL;
0168     if (ret == 0)
0169         ret = res.id;
0170
0171 nla_put_failure:
0172     nlmsg_free(msg);
0173     return ret;
0174 }
0175
0176 struct perf_cap {
0177     int cpu;
0178     int perf;
0179     int eff;
0180 };
0181
0182 static void process_hfi_event(struct perf_cap *perf_cap)
0183 {
0184     process_level_change(perf_cap->cpu);
0185 }
0186
0187 static int handle_event(struct nl_msg *n, void *arg)
0188 {
0189     struct nlmsghdr *nlh = nlmsg_hdr(n);
0190     struct genlmsghdr *genlhdr = genlmsg_hdr(nlh);
0191     struct nlattr *attrs[THERMAL_GENL_ATTR_MAX + 1];
0192     int ret;
0193     struct perf_cap perf_cap = {0};
0194
0195     ret = genlmsg_parse(nlh, 0, attrs, THERMAL_GENL_ATTR_MAX, NULL);
0196
0197     debug_printf("Received event %d parse_rer:%d\n", genlhdr->cmd, ret);
0198     if (genlhdr->cmd == THERMAL_GENL_EVENT_CPU_CAPABILITY_CHANGE) {
0199         struct nlattr *cap;
0200         int j, index = 0;
0201
0202         debug_printf("THERMAL_GENL_EVENT_CPU_CAPABILITY_CHANGE\n");
0203         nla_for_each_nested(cap, attrs[THERMAL_GENL_ATTR_CPU_CAPABILITY], j) {
0204             switch (index) {
0205             case 0:
0206                 perf_cap.cpu = nla_get_u32(cap);
0207                 break;
0208             case 1:
0209                 perf_cap.perf = nla_get_u32(cap);
0210                 break;
0211             case 2:
0212                 perf_cap.eff = nla_get_u32(cap);
0213                 break;
0214             default:
0215                 break;
0216             }
0217             ++index;
0218             if (index == 3) {
0219                 index = 0;
0220                 process_hfi_event(&perf_cap);
0221             }
0222         }
0223     }
0224
0225     return 0;
0226 }
0227
0228 static int _hfi_exit;
0229
0230 static int check_hf_suport(void)
0231 {
0232     unsigned int eax = 0, ebx = 0, ecx = 0, edx = 0;
0233
0234     __cpuid(6, eax, ebx, ecx, edx);
0235     if (eax & BIT(19))
0236         return 1;
0237
0238     return 0;
0239 }
0240
0241 int hfi_main(void)
0242 {
0243     struct nl_sock *sock;
0244     struct nl_cb *cb;
0245     int err = 0;
0246     int mcast_id;
0247     int no_block = 0;
0248
0249     if (!check_hf_suport()) {
0250         fprintf(stderr, "CPU Doesn't support HFI\n");
0251         return -1;
0252     }
0253
0254     sock = nl_socket_alloc();
0255     if (!sock) {
0256         fprintf(stderr, "nl_socket_alloc failed\n");
0257         return -1;
0258     }
0259
0260     if (genl_connect(sock)) {
0261         fprintf(stderr, "genl_connect(sk_event) failed\n");
0262         goto free_sock;
0263     }
0264
0265     drv.nl_handle = sock;
0266     drv.nl_cb = cb = nl_cb_alloc(NL_CB_DEFAULT);
0267     if (drv.nl_cb == NULL) {
0268         printf("Failed to allocate netlink callbacks");
0269         goto free_sock;
0270     }
0271
0272     mcast_id = nl_get_multicast_id(&drv, THERMAL_GENL_FAMILY_NAME,
0273                    THERMAL_GENL_EVENT_GROUP_NAME);
0274     if (mcast_id < 0) {
0275         fprintf(stderr, "nl_get_multicast_id failed\n");
0276         goto free_sock;
0277     }
0278
0279     if (nl_socket_add_membership(sock, mcast_id)) {
0280         fprintf(stderr, "nl_socket_add_membership failed");
0281         goto free_sock;
0282     }
0283
0284     nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, seq_check_handler, 0);
0285     nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, handle_event, NULL);
0286
0287     if (no_block)
0288         nl_socket_set_nonblocking(sock);
0289
0290     debug_printf("hfi is initialized\n");
0291
0292     while (!_hfi_exit && !err) {
0293         err = nl_recvmsgs(sock, cb);
0294         debug_printf("nl_recv_message err:%d\n", err);
0295     }
0296
0297     return 0;
0298
0299     /* Netlink library doesn't have calls to dealloc cb or disconnect */
0300 free_sock:
0301     nl_socket_free(sock);
0302
0303     return -1;
0304 }
0305
0306 void hfi_exit(void)
0307 {
0308     _hfi_exit = 1;
0309 }