350 lines · c
1// SPDX-License-Identifier: LGPL-2.1+2// Copyright (C) 2022, Linaro Ltd - Daniel Lezcano <daniel.lezcano@linaro.org>3#define _GNU_SOURCE4#include <errno.h>5#include <stdio.h>6#include <stdlib.h>7#include <unistd.h>8 9#include <thermal.h>10#include "thermal_nl.h"11 12static struct nla_policy thermal_genl_policy[THERMAL_GENL_ATTR_MAX + 1] = {13 /* Thermal zone */14 [THERMAL_GENL_ATTR_TZ] = { .type = NLA_NESTED },15 [THERMAL_GENL_ATTR_TZ_ID] = { .type = NLA_U32 },16 [THERMAL_GENL_ATTR_TZ_TEMP] = { .type = NLA_U32 },17 [THERMAL_GENL_ATTR_TZ_TRIP] = { .type = NLA_NESTED },18 [THERMAL_GENL_ATTR_TZ_TRIP_ID] = { .type = NLA_U32 },19 [THERMAL_GENL_ATTR_TZ_TRIP_TEMP] = { .type = NLA_U32 },20 [THERMAL_GENL_ATTR_TZ_TRIP_TYPE] = { .type = NLA_U32 },21 [THERMAL_GENL_ATTR_TZ_TRIP_HYST] = { .type = NLA_U32 },22 [THERMAL_GENL_ATTR_TZ_MODE] = { .type = NLA_U32 },23 [THERMAL_GENL_ATTR_TZ_CDEV_WEIGHT] = { .type = NLA_U32 },24 [THERMAL_GENL_ATTR_TZ_NAME] = { .type = NLA_STRING },25 26 /* Governor(s) */27 [THERMAL_GENL_ATTR_TZ_GOV] = { .type = NLA_NESTED },28 [THERMAL_GENL_ATTR_TZ_GOV_NAME] = { .type = NLA_STRING },29 30 /* Cooling devices */31 [THERMAL_GENL_ATTR_CDEV] = { .type = NLA_NESTED },32 [THERMAL_GENL_ATTR_CDEV_ID] = { .type = NLA_U32 },33 [THERMAL_GENL_ATTR_CDEV_CUR_STATE] = { .type = NLA_U32 },34 [THERMAL_GENL_ATTR_CDEV_MAX_STATE] = { .type = NLA_U32 },35 [THERMAL_GENL_ATTR_CDEV_NAME] = { .type = NLA_STRING },36};37 38static int parse_tz_get(struct genl_info *info, struct thermal_zone **tz)39{40 struct nlattr *attr;41 struct thermal_zone *__tz = NULL;42 size_t size = 0;43 int rem;44 45 nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_TZ], rem) {46 47 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_ID) {48 49 size++;50 51 __tz = realloc(__tz, sizeof(*__tz) * (size + 2));52 if (!__tz)53 return THERMAL_ERROR;54 55 __tz[size - 1].id = nla_get_u32(attr);56 }57 58 59 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_NAME)60 nla_strlcpy(__tz[size - 1].name, attr,61 THERMAL_NAME_LENGTH);62 }63 64 if (__tz)65 __tz[size].id = -1;66 67 *tz = __tz;68 69 return THERMAL_SUCCESS;70}71 72static int parse_cdev_get(struct genl_info *info, struct thermal_cdev **cdev)73{74 struct nlattr *attr;75 struct thermal_cdev *__cdev = NULL;76 size_t size = 0;77 int rem;78 79 nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_CDEV], rem) {80 81 if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_ID) {82 83 size++;84 85 __cdev = realloc(__cdev, sizeof(*__cdev) * (size + 2));86 if (!__cdev)87 return THERMAL_ERROR;88 89 __cdev[size - 1].id = nla_get_u32(attr);90 }91 92 if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_NAME) {93 nla_strlcpy(__cdev[size - 1].name, attr,94 THERMAL_NAME_LENGTH);95 }96 97 if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_CUR_STATE)98 __cdev[size - 1].cur_state = nla_get_u32(attr);99 100 if (nla_type(attr) == THERMAL_GENL_ATTR_CDEV_MAX_STATE)101 __cdev[size - 1].max_state = nla_get_u32(attr);102 }103 104 if (__cdev)105 __cdev[size].id = -1;106 107 *cdev = __cdev;108 109 return THERMAL_SUCCESS;110}111 112static int parse_tz_get_trip(struct genl_info *info, struct thermal_zone *tz)113{114 struct nlattr *attr;115 struct thermal_trip *__tt = NULL;116 size_t size = 0;117 int rem;118 119 nla_for_each_nested(attr, info->attrs[THERMAL_GENL_ATTR_TZ_TRIP], rem) {120 121 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_ID) {122 123 size++;124 125 __tt = realloc(__tt, sizeof(*__tt) * (size + 2));126 if (!__tt)127 return THERMAL_ERROR;128 129 __tt[size - 1].id = nla_get_u32(attr);130 }131 132 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_TYPE)133 __tt[size - 1].type = nla_get_u32(attr);134 135 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_TEMP)136 __tt[size - 1].temp = nla_get_u32(attr);137 138 if (nla_type(attr) == THERMAL_GENL_ATTR_TZ_TRIP_HYST)139 __tt[size - 1].hyst = nla_get_u32(attr);140 }141 142 if (__tt)143 __tt[size].id = -1;144 145 tz->trip = __tt;146 147 return THERMAL_SUCCESS;148}149 150static int parse_tz_get_temp(struct genl_info *info, struct thermal_zone *tz)151{152 int id = -1;153 154 if (info->attrs[THERMAL_GENL_ATTR_TZ_ID])155 id = nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_ID]);156 157 if (tz->id != id)158 return THERMAL_ERROR;159 160 if (info->attrs[THERMAL_GENL_ATTR_TZ_TEMP])161 tz->temp = nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_TEMP]);162 163 return THERMAL_SUCCESS;164}165 166static int parse_tz_get_gov(struct genl_info *info, struct thermal_zone *tz)167{168 int id = -1;169 170 if (info->attrs[THERMAL_GENL_ATTR_TZ_ID])171 id = nla_get_u32(info->attrs[THERMAL_GENL_ATTR_TZ_ID]);172 173 if (tz->id != id)174 return THERMAL_ERROR;175 176 if (info->attrs[THERMAL_GENL_ATTR_TZ_GOV_NAME]) {177 nla_strlcpy(tz->governor,178 info->attrs[THERMAL_GENL_ATTR_TZ_GOV_NAME],179 THERMAL_NAME_LENGTH);180 }181 182 return THERMAL_SUCCESS;183}184 185static int handle_netlink(struct nl_cache_ops *unused,186 struct genl_cmd *cmd,187 struct genl_info *info, void *arg)188{189 int ret;190 191 switch (cmd->c_id) {192 193 case THERMAL_GENL_CMD_TZ_GET_ID:194 ret = parse_tz_get(info, arg);195 break;196 197 case THERMAL_GENL_CMD_CDEV_GET:198 ret = parse_cdev_get(info, arg);199 break;200 201 case THERMAL_GENL_CMD_TZ_GET_TEMP:202 ret = parse_tz_get_temp(info, arg);203 break;204 205 case THERMAL_GENL_CMD_TZ_GET_TRIP:206 ret = parse_tz_get_trip(info, arg);207 break;208 209 case THERMAL_GENL_CMD_TZ_GET_GOV:210 ret = parse_tz_get_gov(info, arg);211 break;212 213 default:214 return THERMAL_ERROR;215 }216 217 return ret;218}219 220static struct genl_cmd thermal_cmds[] = {221 {222 .c_id = THERMAL_GENL_CMD_TZ_GET_ID,223 .c_name = (char *)"List thermal zones",224 .c_msg_parser = handle_netlink,225 .c_maxattr = THERMAL_GENL_ATTR_MAX,226 .c_attr_policy = thermal_genl_policy,227 },228 {229 .c_id = THERMAL_GENL_CMD_TZ_GET_GOV,230 .c_name = (char *)"Get governor",231 .c_msg_parser = handle_netlink,232 .c_maxattr = THERMAL_GENL_ATTR_MAX,233 .c_attr_policy = thermal_genl_policy,234 },235 {236 .c_id = THERMAL_GENL_CMD_TZ_GET_TEMP,237 .c_name = (char *)"Get thermal zone temperature",238 .c_msg_parser = handle_netlink,239 .c_maxattr = THERMAL_GENL_ATTR_MAX,240 .c_attr_policy = thermal_genl_policy,241 },242 {243 .c_id = THERMAL_GENL_CMD_TZ_GET_TRIP,244 .c_name = (char *)"Get thermal zone trip points",245 .c_msg_parser = handle_netlink,246 .c_maxattr = THERMAL_GENL_ATTR_MAX,247 .c_attr_policy = thermal_genl_policy,248 },249 {250 .c_id = THERMAL_GENL_CMD_CDEV_GET,251 .c_name = (char *)"Get cooling devices",252 .c_msg_parser = handle_netlink,253 .c_maxattr = THERMAL_GENL_ATTR_MAX,254 .c_attr_policy = thermal_genl_policy,255 },256};257 258static struct genl_ops thermal_cmd_ops = {259 .o_name = (char *)"thermal",260 .o_cmds = thermal_cmds,261 .o_ncmds = ARRAY_SIZE(thermal_cmds),262};263 264static thermal_error_t thermal_genl_auto(struct thermal_handler *th, int id, int cmd,265 int flags, void *arg)266{267 struct nl_msg *msg;268 void *hdr;269 270 msg = nlmsg_alloc();271 if (!msg)272 return THERMAL_ERROR;273 274 hdr = genlmsg_put(msg, NL_AUTO_PORT, NL_AUTO_SEQ, thermal_cmd_ops.o_id,275 0, flags, cmd, THERMAL_GENL_VERSION);276 if (!hdr)277 return THERMAL_ERROR;278 279 if (id >= 0 && nla_put_u32(msg, THERMAL_GENL_ATTR_TZ_ID, id))280 return THERMAL_ERROR;281 282 if (nl_send_msg(th->sk_cmd, th->cb_cmd, msg, genl_handle_msg, arg))283 return THERMAL_ERROR;284 285 nlmsg_free(msg);286 287 return THERMAL_SUCCESS;288}289 290thermal_error_t thermal_cmd_get_tz(struct thermal_handler *th, struct thermal_zone **tz)291{292 return thermal_genl_auto(th, -1, THERMAL_GENL_CMD_TZ_GET_ID,293 NLM_F_DUMP | NLM_F_ACK, tz);294}295 296thermal_error_t thermal_cmd_get_cdev(struct thermal_handler *th, struct thermal_cdev **tc)297{298 return thermal_genl_auto(th, -1, THERMAL_GENL_CMD_CDEV_GET,299 NLM_F_DUMP | NLM_F_ACK, tc);300}301 302thermal_error_t thermal_cmd_get_trip(struct thermal_handler *th, struct thermal_zone *tz)303{304 return thermal_genl_auto(th, tz->id, THERMAL_GENL_CMD_TZ_GET_TRIP,305 0, tz);306}307 308thermal_error_t thermal_cmd_get_governor(struct thermal_handler *th, struct thermal_zone *tz)309{310 return thermal_genl_auto(th, tz->id, THERMAL_GENL_CMD_TZ_GET_GOV, 0, tz);311}312 313thermal_error_t thermal_cmd_get_temp(struct thermal_handler *th, struct thermal_zone *tz)314{315 return thermal_genl_auto(th, tz->id, THERMAL_GENL_CMD_TZ_GET_TEMP, 0, tz);316}317 318thermal_error_t thermal_cmd_exit(struct thermal_handler *th)319{320 if (genl_unregister_family(&thermal_cmd_ops))321 return THERMAL_ERROR;322 323 nl_thermal_disconnect(th->sk_cmd, th->cb_cmd);324 325 return THERMAL_SUCCESS;326}327 328thermal_error_t thermal_cmd_init(struct thermal_handler *th)329{330 int ret;331 int family;332 333 if (nl_thermal_connect(&th->sk_cmd, &th->cb_cmd))334 return THERMAL_ERROR;335 336 ret = genl_register_family(&thermal_cmd_ops);337 if (ret)338 return THERMAL_ERROR;339 340 ret = genl_ops_resolve(th->sk_cmd, &thermal_cmd_ops);341 if (ret)342 return THERMAL_ERROR;343 344 family = genl_ctrl_resolve(th->sk_cmd, "nlctrl");345 if (family != GENL_ID_CTRL)346 return THERMAL_ERROR;347 348 return THERMAL_SUCCESS;349}350