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         struct isst_id id;
 185
 186         set_isst_id(&id, perf_cap->cpu);
 187         process_level_change(&id);
 188 }
 189
 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
 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
 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
 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 */
 299 free_sock:
 300         nl_socket_free(sock);
 301
 302         return -1;
 303 }
 304
 305 void hfi_exit(void)
 306 {
 307         _hfi_exit = 1;
 308 }