1 // SPDX-License-Identifier: LGPL-2.1+ 2 // Copyright (C) 2022, Linaro Ltd - Daniel Lezcano <daniel.lezcano@linaro.org> 3 #define _GNU_SOURCE 4 #include <errno.h> 5 #include <stdio.h> 6 #include <stdlib.h> 7 #include <unistd.h> 8 #include <limits.h> 9 10 #include <thermal.h> 11 #include "thermal_nl.h" 12 13 static struct nla_policy thermal_genl_policy[THERMAL_GENL_ATTR_MAX + 1] = { 14 /* Thermal zone */ 15 [THERMAL_GENL_ATTR_TZ] = { .type = NLA_NESTED }, 16 [THERMAL_GENL_ATTR_TZ_ID] = { .type = NLA_U32 }, 17 [THERMAL_GENL_ATTR_TZ_TEMP] = { .type = NLA_U32 }, 18 [THERMAL_GENL_ATTR_TZ_TRIP] = { .type = NLA_NESTED }, 19 [THERMAL_GENL_ATTR_TZ_TRIP_ID] = { .type = NLA_U32 }, 20 [THERMAL_GENL_ATTR_TZ_TRIP_TEMP] = { .type = NLA_U32 }, 21 [THERMAL_GENL_ATTR_TZ_TRIP_TYPE] = { .type = NLA_U32 }, 22 [THERMAL_GENL_ATTR_TZ_TRIP_HYST] = { .type = NLA_U32 }, 23 [THERMAL_GENL_ATTR_TZ_MODE] = { .type = NLA_U32 }, 24 [THERMAL_GENL_ATTR_TZ_CDEV_WEIGHT] = { .type = NLA_U32 }, 25 [THERMAL_GENL_ATTR_TZ_NAME] = { .type = NLA_STRING }, 26 27 /* Governor(s) */ 28 [THERMAL_GENL_ATTR_TZ_GOV] = { .type = NLA_NESTED }, 29 [THERMAL_GENL_ATTR_TZ_GOV_NAME] = { .type = NLA_STRING }, 30 31 /* Cooling devices */ 32 [THERMAL_GENL_ATTR_CDEV] = { .type = NLA_NESTED }, 33 [THERMAL_GENL_ATTR_CDEV_ID] = { .type = NLA_U32 }, 34 [THERMAL_GENL_ATTR_CDEV_CUR_STATE] = { .type = NLA_U32 }, 35 [THERMAL_GENL_ATTR_CDEV_MAX_STATE] = { .type = NLA_U32 }, 36 [THERMAL_GENL_ATTR_CDEV_NAME] = { .type = NLA_STRING }, 37 38 /* Thresholds */ 39 [THERMAL_GENL_ATTR_THRESHOLD] = { .type = NLA_NESTED }, 40 [THERMAL_GENL_ATTR_THRESHOLD_TEMP] = { .type = NLA_U32 }, 41 [THERMAL_GENL_ATTR_THRESHOLD_DIRECTION] = { .type = NLA_U32 }, 42 }; 43 44 static int parse_tz_get(struct genl_info *info, struct thermal_zone **tz) 45 { 46 struct nlattr *attr; 47 struct thermal_zone *__tz = NULL; 48 size_t size = 0; 49 int rem; 50 51 nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_TZ], rem) { 52 53 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_ID) { 54 55 size++; 56 57 __tz = realloc(__tz, sizeof(*__tz) * (size + 2)); 58 if (!__tz) 59 return THERMAL_ERROR; 60 61 __tz[size - 1].id = nla_get_u32(attr); 62 } 63 64 65 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_NAME) 66 nla_strlcpy(__tz[size - 1].name, attr, 67 THERMAL_NAME_LENGTH); 68 } 69 70 if (__tz) 71 __tz[size].id = -1; 72 73 *tz = __tz; 74 75 return THERMAL_SUCCESS; 76 } 77 78 static int parse_cdev_get(struct genl_info *info, struct thermal_cdev **cdev) 79 { 80 struct nlattr *attr; 81 struct thermal_cdev *__cdev = NULL; 82 size_t size = 0; 83 int rem; 84 85 nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_CDEV], rem) { 86 87 if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_ID) { 88 89 size++; 90 91 __cdev = realloc(__cdev, sizeof(*__cdev) * (size + 2)); 92 if (!__cdev) 93 return THERMAL_ERROR; 94 95 __cdev[size - 1].id = nla_get_u32(attr); 96 } 97 98 if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_NAME) { 99 nla_strlcpy(__cdev[size - 1].name, attr, 100 THERMAL_NAME_LENGTH); 101 } 102 103 if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_CUR_STATE) 104 __cdev[size - 1].cur_state = nla_get_u32(attr); 105 106 if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_MAX_STATE) 107 __cdev[size - 1].max_state = nla_get_u32(attr); 108 } 109 110 if (__cdev) 111 __cdev[size].id = -1; 112 113 *cdev = __cdev; 114 115 return THERMAL_SUCCESS; 116 } 117 118 static int parse_tz_get_trip(struct genl_info *info, struct thermal_zone *tz) 119 { 120 struct nlattr *attr; 121 struct thermal_trip *__tt = NULL; 122 size_t size = 0; 123 int rem; 124 125 nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_TZ_TRIP], rem) { 126 127 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_ID) { 128 129 size++; 130 131 __tt = realloc(__tt, sizeof(*__tt) * (size + 2)); 132 if (!__tt) 133 return THERMAL_ERROR; 134 135 __tt[size - 1].id = nla_get_u32(attr); 136 } 137 138 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_TYPE) 139 __tt[size - 1].type = nla_get_u32(attr); 140 141 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_TEMP) 142 __tt[size - 1].temp = nla_get_u32(attr); 143 144 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_HYST) 145 __tt[size - 1].hyst = nla_get_u32(attr); 146 } 147 148 if (__tt) 149 __tt[size].id = -1; 150 151 tz->trip = __tt; 152 153 return THERMAL_SUCCESS; 154 } 155 156 static int parse_tz_get_temp(struct genl_info *info, struct thermal_zone *tz) 157 { 158 int id = -1; 159 160 if (info->attrs[THERMAL_GENL_ATTR_TZ_ID]) 161 id = nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_ID]); 162 163 if (tz->id != id) 164 return THERMAL_ERROR; 165 166 if (info->attrs[THERMAL_GENL_ATTR_TZ_TEMP]) 167 tz->temp = nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_TEMP]); 168 169 return THERMAL_SUCCESS; 170 } 171 172 static int parse_tz_get_gov(struct genl_info *info, struct thermal_zone *tz) 173 { 174 int id = -1; 175 176 if (info->attrs[THERMAL_GENL_ATTR_TZ_ID]) 177 id = nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_ID]); 178 179 if (tz->id != id) 180 return THERMAL_ERROR; 181 182 if (info->attrs[THERMAL_GENL_ATTR_TZ_GOV_NAME]) { 183 nla_strlcpy(tz->governor, 184 info->attrs[THERMAL_GENL_ATTR_TZ_GOV_NAME], 185 THERMAL_NAME_LENGTH); 186 } 187 188 return THERMAL_SUCCESS; 189 } 190 191 static int parse_threshold_get(struct genl_info *info, struct thermal_zone *tz) 192 { 193 struct nlattr *attr; 194 struct thermal_threshold *__tt = NULL; 195 size_t size = 0; 196 int rem; 197 198 /* 199 * The size contains the size of the array and we want to 200 * access the last element, size - 1. 201 * 202 * The variable size is initialized to zero but it will be 203 * then incremented by the first if() statement. The message 204 * attributes are ordered, so the first if() statement will be 205 * always called before the second one. If it happens that is 206 * not the case, then it is a kernel bug. 207 */ 208 nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_THRESHOLD], rem) { 209 210 if (nla_type(attr) == THERMAL_GENL_ATTR_THRESHOLD_TEMP) { 211 212 size++; 213 214 __tt = realloc(__tt, sizeof(*__tt) * (size + 2)); 215 if (!__tt) 216 return THERMAL_ERROR; 217 218 __tt[size - 1].temperature = nla_get_u32(attr); 219 } 220 221 if (nla_type(attr) == THERMAL_GENL_ATTR_THRESHOLD_DIRECTION) 222 __tt[size - 1].direction = nla_get_u32(attr); 223 } 224 225 if (__tt) 226 __tt[size].temperature = INT_MAX; 227 228 tz->thresholds = __tt; 229 230 return THERMAL_SUCCESS; 231 } 232 233 static int handle_netlink(struct nl_cache_ops *unused, 234 struct genl_cmd *cmd, 235 struct genl_info *info, void *arg) 236 { 237 int ret; 238 239 switch (cmd->c_id) { 240 241 case THERMAL_GENL_CMD_TZ_GET_ID: 242 ret = parse_tz_get(info, arg); 243 break; 244 245 case THERMAL_GENL_CMD_CDEV_GET: 246 ret = parse_cdev_get(info, arg); 247 break; 248 249 case THERMAL_GENL_CMD_TZ_GET_TEMP: 250 ret = parse_tz_get_temp(info, arg); 251 break; 252 253 case THERMAL_GENL_CMD_TZ_GET_TRIP: 254 ret = parse_tz_get_trip(info, arg); 255 break; 256 257 case THERMAL_GENL_CMD_TZ_GET_GOV: 258 ret = parse_tz_get_gov(info, arg); 259 break; 260 261 case THERMAL_GENL_CMD_THRESHOLD_GET: 262 ret = parse_threshold_get(info, arg); 263 break; 264 265 default: 266 return THERMAL_ERROR; 267 } 268 269 return ret; 270 } 271 272 static struct genl_cmd thermal_cmds[] = { 273 { 274 .c_id = THERMAL_GENL_CMD_TZ_GET_ID, 275 .c_name = (char *)"List thermal zones", 276 .c_msg_parser = handle_netlink, 277 .c_maxattr = THERMAL_GENL_ATTR_MAX, 278 .c_attr_policy = thermal_genl_policy, 279 }, 280 { 281 .c_id = THERMAL_GENL_CMD_TZ_GET_GOV, 282 .c_name = (char *)"Get governor", 283 .c_msg_parser = handle_netlink, 284 .c_maxattr = THERMAL_GENL_ATTR_MAX, 285 .c_attr_policy = thermal_genl_policy, 286 }, 287 { 288 .c_id = THERMAL_GENL_CMD_TZ_GET_TEMP, 289 .c_name = (char *)"Get thermal zone temperature", 290 .c_msg_parser = handle_netlink, 291 .c_maxattr = THERMAL_GENL_ATTR_MAX, 292 .c_attr_policy = thermal_genl_policy, 293 }, 294 { 295 .c_id = THERMAL_GENL_CMD_TZ_GET_TRIP, 296 .c_name = (char *)"Get thermal zone trip points", 297 .c_msg_parser = handle_netlink, 298 .c_maxattr = THERMAL_GENL_ATTR_MAX, 299 .c_attr_policy = thermal_genl_policy, 300 }, 301 { 302 .c_id = THERMAL_GENL_CMD_CDEV_GET, 303 .c_name = (char *)"Get cooling devices", 304 .c_msg_parser = handle_netlink, 305 .c_maxattr = THERMAL_GENL_ATTR_MAX, 306 .c_attr_policy = thermal_genl_policy, 307 }, 308 { 309 .c_id = THERMAL_GENL_CMD_THRESHOLD_GET, 310 .c_name = (char *)"Get thresholds list", 311 .c_msg_parser = handle_netlink, 312 .c_maxattr = THERMAL_GENL_ATTR_MAX, 313 .c_attr_policy = thermal_genl_policy, 314 }, 315 { 316 .c_id = THERMAL_GENL_CMD_THRESHOLD_ADD, 317 .c_name = (char *)"Add a threshold", 318 .c_msg_parser = handle_netlink, 319 .c_maxattr = THERMAL_GENL_ATTR_MAX, 320 .c_attr_policy = thermal_genl_policy, 321 }, 322 { 323 .c_id = THERMAL_GENL_CMD_THRESHOLD_DELETE, 324 .c_name = (char *)"Delete a threshold", 325 .c_msg_parser = handle_netlink, 326 .c_maxattr = THERMAL_GENL_ATTR_MAX, 327 .c_attr_policy = thermal_genl_policy, 328 }, 329 { 330 .c_id = THERMAL_GENL_CMD_THRESHOLD_FLUSH, 331 .c_name = (char *)"Flush the thresholds", 332 .c_msg_parser = handle_netlink, 333 .c_maxattr = THERMAL_GENL_ATTR_MAX, 334 .c_attr_policy = thermal_genl_policy, 335 }, 336 }; 337 338 static struct genl_ops thermal_cmd_ops = { 339 .o_name = (char *)"thermal", 340 .o_cmds = thermal_cmds, 341 .o_ncmds = ARRAY_SIZE(thermal_cmds), 342 }; 343 344 struct cmd_param { 345 int tz_id; 346 int temp; 347 int direction; 348 }; 349 350 typedef int (*cmd_cb_t)(struct nl_msg *, struct cmd_param *); 351 352 static int thermal_genl_tz_id_encode(struct nl_msg *msg, struct cmd_param *p) 353 { 354 if (nla_put_u32(msg, THERMAL_GENL_ATTR_TZ_ID, p->tz_id)) 355 return -1; 356 357 return 0; 358 } 359 360 static int thermal_genl_threshold_encode(struct nl_msg *msg, struct cmd_param *p) 361 { 362 if (thermal_genl_tz_id_encode(msg, p)) 363 return -1; 364 365 if (nla_put_u32(msg, THERMAL_GENL_ATTR_THRESHOLD_TEMP, p->temp)) 366 return -1; 367 368 if (nla_put_u32(msg, THERMAL_GENL_ATTR_THRESHOLD_DIRECTION, p->direction)) 369 return -1; 370 371 return 0; 372 } 373 374 static thermal_error_t thermal_genl_auto(struct thermal_handler *th, cmd_cb_t cmd_cb, 375 struct cmd_param *param, 376 int cmd, int flags, void *arg) 377 { 378 thermal_error_t ret = THERMAL_ERROR; 379 struct nl_msg *msg; 380 void *hdr; 381 382 msg = nlmsg_alloc(); 383 if (!msg) 384 return THERMAL_ERROR; 385 386 hdr = genlmsg_put(msg, NL_AUTO_PORT, NL_AUTO_SEQ, thermal_cmd_ops.o_id, 387 0, flags, cmd, THERMAL_GENL_VERSION); 388 if (!hdr) 389 goto out; 390 391 if (cmd_cb && cmd_cb(msg, param)) 392 goto out; 393 394 if (nl_send_msg(th->sk_cmd, th->cb_cmd, msg, genl_handle_msg, arg)) 395 goto out; 396 397 ret = THERMAL_SUCCESS; 398 out: 399 nlmsg_free(msg); 400 401 return ret; 402 } 403 404 thermal_error_t thermal_cmd_get_tz(struct thermal_handler *th, struct thermal_zone **tz) 405 { 406 return thermal_genl_auto(th, NULL, NULL, THERMAL_GENL_CMD_TZ_GET_ID, 407 NLM_F_DUMP | NLM_F_ACK, tz); 408 } 409 410 thermal_error_t thermal_cmd_get_cdev(struct thermal_handler *th, struct thermal_cdev **tc) 411 { 412 return thermal_genl_auto(th, NULL, NULL, THERMAL_GENL_CMD_CDEV_GET, 413 NLM_F_DUMP | NLM_F_ACK, tc); 414 } 415 416 thermal_error_t thermal_cmd_get_trip(struct thermal_handler *th, struct thermal_zone *tz) 417 { 418 struct cmd_param p = { .tz_id = tz->id }; 419 420 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p, 421 THERMAL_GENL_CMD_TZ_GET_TRIP, 0, tz); 422 } 423 424 thermal_error_t thermal_cmd_get_governor(struct thermal_handler *th, struct thermal_zone *tz) 425 { 426 struct cmd_param p = { .tz_id = tz->id }; 427 428 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p, 429 THERMAL_GENL_CMD_TZ_GET_GOV, 0, tz); 430 } 431 432 thermal_error_t thermal_cmd_get_temp(struct thermal_handler *th, struct thermal_zone *tz) 433 { 434 struct cmd_param p = { .tz_id = tz->id }; 435 436 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p, 437 THERMAL_GENL_CMD_TZ_GET_TEMP, 0, tz); 438 } 439 440 thermal_error_t thermal_cmd_threshold_get(struct thermal_handler *th, 441 struct thermal_zone *tz) 442 { 443 struct cmd_param p = { .tz_id = tz->id }; 444 445 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p, 446 THERMAL_GENL_CMD_THRESHOLD_GET, 0, tz); 447 } 448 449 thermal_error_t thermal_cmd_threshold_add(struct thermal_handler *th, 450 struct thermal_zone *tz, 451 int temperature, 452 int direction) 453 { 454 struct cmd_param p = { .tz_id = tz->id, .temp = temperature, .direction = direction }; 455 456 return thermal_genl_auto(th, thermal_genl_threshold_encode, &p, 457 THERMAL_GENL_CMD_THRESHOLD_ADD, 0, tz); 458 } 459 460 thermal_error_t thermal_cmd_threshold_delete(struct thermal_handler *th, 461 struct thermal_zone *tz, 462 int temperature, 463 int direction) 464 { 465 struct cmd_param p = { .tz_id = tz->id, .temp = temperature, .direction = direction }; 466 467 return thermal_genl_auto(th, thermal_genl_threshold_encode, &p, 468 THERMAL_GENL_CMD_THRESHOLD_DELETE, 0, tz); 469 } 470 471 thermal_error_t thermal_cmd_threshold_flush(struct thermal_handler *th, 472 struct thermal_zone *tz) 473 { 474 struct cmd_param p = { .tz_id = tz->id }; 475 476 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p, 477 THERMAL_GENL_CMD_THRESHOLD_FLUSH, 0, tz); 478 } 479 480 thermal_error_t thermal_cmd_exit(struct thermal_handler *th) 481 { 482 if (genl_unregister_family(&thermal_cmd_ops)) 483 return THERMAL_ERROR; 484 485 nl_thermal_disconnect(th->sk_cmd, th->cb_cmd); 486 487 return THERMAL_SUCCESS; 488 } 489 490 thermal_error_t thermal_cmd_init(struct thermal_handler *th) 491 { 492 int ret; 493 int family; 494 495 if (nl_thermal_connect(&th->sk_cmd, &th->cb_cmd)) 496 return THERMAL_ERROR; 497 498 ret = genl_register_family(&thermal_cmd_ops); 499 if (ret) 500 return THERMAL_ERROR; 501 502 ret = genl_ops_resolve(th->sk_cmd, &thermal_cmd_ops); 503 if (ret) 504 return THERMAL_ERROR; 505 506 family = genl_ctrl_resolve(th->sk_cmd, "nlctrl"); 507 if (family != GENL_ID_CTRL) 508 return THERMAL_ERROR; 509 510 return THERMAL_SUCCESS; 511 } 512