tools/lib/thermal/commands.c

   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 }