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

   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
  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
  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
  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
  78 static int seq_check_handler(struct nl_msg *msg, void *arg)
  79 {
  80         return NL_OK;
  81 }
  82
  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
 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
 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
 182 static void process_hfi_event(struct perf_cap *perf_cap)
 183 {
 184         process_level_change(perf_cap->cpu);
 185 }
 186
 187 static int handle_event(struct nl_msg *n, void *arg)
 188 {
 189         struct nlmsghdr *nlh = nlmsg_hdr(n);
 190         struct genlmsghdr *genlhdr = genlmsg_hdr(nlh);
 191         struct nlattr *attrs[THERMAL_GENL_ATTR_MAX + 1];
 192         int ret;
 193         struct perf_cap perf_cap;
 194
 195         ret = genlmsg_parse(nlh, 0, attrs, THERMAL_GENL_ATTR_MAX, NULL);
 196
 197         debug_printf("Received event %d parse_rer:%d\n", genlhdr->cmd, ret);
 198         if (genlhdr->cmd == THERMAL_GENL_EVENT_CPU_CAPABILITY_CHANGE) {
 199                 struct nlattr *cap;
 200                 int j, index = 0;
 201
 202                 debug_printf("THERMAL_GENL_EVENT_CPU_CAPABILITY_CHANGE\n");
 203                 nla_for_each_nested(cap, attrs[THERMAL_GENL_ATTR_CPU_CAPABILITY], j) {
 204                         switch (index) {
 205                         case 0:
 206                                 perf_cap.cpu = nla_get_u32(cap);
 207                                 break;
 208                         case 1:
 209                                 perf_cap.perf = nla_get_u32(cap);
 210                                 break;
 211                         case 2:
 212                                 perf_cap.eff = nla_get_u32(cap);
 213                                 break;
 214                         default:
 215                                 break;
 216                         }
 217                         ++index;
 218                         if (index == 3) {
 219                                 index = 0;
 220                                 process_hfi_event(&perf_cap);
 221                         }
 222                 }
 223         }
 224
 225         return 0;
 226 }
 227
 228 static int _hfi_exit;
 229
 230 static int check_hf_suport(void)
 231 {
 232         unsigned int eax = 0, ebx = 0, ecx = 0, edx = 0;
 233
 234         __cpuid(6, eax, ebx, ecx, edx);
 235         if (eax & BIT(19))
 236                 return 1;
 237
 238         return 0;
 239 }
 240
 241 int hfi_main(void)
 242 {
 243         struct nl_sock *sock;
 244         struct nl_cb *cb;
 245         int err = 0;
 246         int mcast_id;
 247         int no_block = 0;
 248
 249         if (!check_hf_suport()) {
 250                 fprintf(stderr, "CPU Doesn't support HFI\n");
 251                 return -1;
 252         }
 253
 254         sock = nl_socket_alloc();
 255         if (!sock) {
 256                 fprintf(stderr, "nl_socket_alloc failed\n");
 257                 return -1;
 258         }
 259
 260         if (genl_connect(sock)) {
 261                 fprintf(stderr, "genl_connect(sk_event) failed\n");
 262                 goto free_sock;
 263         }
 264
 265         drv.nl_handle = sock;
 266         drv.nl_cb = cb = nl_cb_alloc(NL_CB_DEFAULT);
 267         if (drv.nl_cb == NULL) {
 268                 printf("Failed to allocate netlink callbacks");
 269                 goto free_sock;
 270         }
 271
 272         mcast_id = nl_get_multicast_id(&drv, THERMAL_GENL_FAMILY_NAME,
 273                                    THERMAL_GENL_EVENT_GROUP_NAME);
 274         if (mcast_id < 0) {
 275                 fprintf(stderr, "nl_get_multicast_id failed\n");
 276                 goto free_sock;
 277         }
 278
 279         if (nl_socket_add_membership(sock, mcast_id)) {
 280                 fprintf(stderr, "nl_socket_add_membership failed");
 281                 goto free_sock;
 282         }
 283
 284         nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, seq_check_handler, 0);
 285         nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, handle_event, NULL);
 286
 287         if (no_block)
 288                 nl_socket_set_nonblocking(sock);
 289
 290         debug_printf("hfi is initialized\n");
 291
 292         while (!_hfi_exit && !err) {
 293                 err = nl_recvmsgs(sock, cb);
 294                 debug_printf("nl_recv_message err:%d\n", err);
 295         }
 296
 297         return 0;
 298
 299         /* Netlink library doesn't have calls to dealloc cb or disconnect */
 300 free_sock:
 301         nl_socket_free(sock);
 302
 303         return -1;
 304 }
 305
 306 void hfi_exit(void)
 307 {
 308         _hfi_exit = 1;
 309 }