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 } 310