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 9 #include <thermal.h> 10 #include "thermal_nl.h" 11 12 static 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 38 static 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 72 static 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 112 static 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 150 static 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 166 static 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 185 static 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 220 static 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 258 static 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 264 struct cmd_param { 265 int tz_id; 266 }; 267 268 typedef int (*cmd_cb_t)(struct nl_msg *, struct cmd_param *); 269 270 static int thermal_genl_tz_id_encode(struct nl_msg *msg, struct cmd_param *p) 271 { 272 if (p->tz_id >= 0 && nla_put_u32(msg, THERMAL_GENL_ATTR_TZ_ID, p->tz_id)) 273 return -1; 274 275 return 0; 276 } 277 278 static thermal_error_t thermal_genl_auto(struct thermal_handler *th, cmd_cb_t cmd_cb, 279 struct cmd_param *param, 280 int cmd, int flags, void *arg) 281 { 282 thermal_error_t ret = THERMAL_ERROR; 283 struct nl_msg *msg; 284 void *hdr; 285 286 msg = nlmsg_alloc(); 287 if (!msg) 288 return THERMAL_ERROR; 289 290 hdr = genlmsg_put(msg, NL_AUTO_PORT, NL_AUTO_SEQ, thermal_cmd_ops.o_id, 291 0, flags, cmd, THERMAL_GENL_VERSION); 292 if (!hdr) 293 goto out; 294 295 if (cmd_cb && cmd_cb(msg, param)) 296 goto out; 297 298 if (nl_send_msg(th->sk_cmd, th->cb_cmd, msg, genl_handle_msg, arg)) 299 goto out; 300 301 ret = THERMAL_SUCCESS; 302 out: 303 nlmsg_free(msg); 304 305 return ret; 306 } 307 308 thermal_error_t thermal_cmd_get_tz(struct thermal_handler *th, struct thermal_zone **tz) 309 { 310 return thermal_genl_auto(th, NULL, NULL, THERMAL_GENL_CMD_TZ_GET_ID, 311 NLM_F_DUMP | NLM_F_ACK, tz); 312 } 313 314 thermal_error_t thermal_cmd_get_cdev(struct thermal_handler *th, struct thermal_cdev **tc) 315 { 316 return thermal_genl_auto(th, NULL, NULL, THERMAL_GENL_CMD_CDEV_GET, 317 NLM_F_DUMP | NLM_F_ACK, tc); 318 } 319 320 thermal_error_t thermal_cmd_get_trip(struct thermal_handler *th, struct thermal_zone *tz) 321 { 322 struct cmd_param p = { .tz_id = tz->id }; 323 324 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p, 325 THERMAL_GENL_CMD_TZ_GET_TRIP, 0, tz); 326 } 327 328 thermal_error_t thermal_cmd_get_governor(struct thermal_handler *th, struct thermal_zone *tz) 329 { 330 struct cmd_param p = { .tz_id = tz->id }; 331 332 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p, 333 THERMAL_GENL_CMD_TZ_GET_GOV, 0, tz); 334 } 335 336 thermal_error_t thermal_cmd_get_temp(struct thermal_handler *th, struct thermal_zone *tz) 337 { 338 struct cmd_param p = { .tz_id = tz->id }; 339 340 return thermal_genl_auto(th, thermal_genl_tz_id_encode, &p, 341 THERMAL_GENL_CMD_TZ_GET_TEMP, 0, tz); 342 } 343 344 thermal_error_t thermal_cmd_exit(struct thermal_handler *th) 345 { 346 if (genl_unregister_family(&thermal_cmd_ops)) 347 return THERMAL_ERROR; 348 349 nl_thermal_disconnect(th->sk_cmd, th->cb_cmd); 350 351 return THERMAL_SUCCESS; 352 } 353 354 thermal_error_t thermal_cmd_init(struct thermal_handler *th) 355 { 356 int ret; 357 int family; 358 359 if (nl_thermal_connect(&th->sk_cmd, &th->cb_cmd)) 360 return THERMAL_ERROR; 361 362 ret = genl_register_family(&thermal_cmd_ops); 363 if (ret) 364 return THERMAL_ERROR; 365 366 ret = genl_ops_resolve(th->sk_cmd, &thermal_cmd_ops); 367 if (ret) 368 return THERMAL_ERROR; 369 370 family = genl_ctrl_resolve(th->sk_cmd, "nlctrl"); 371 if (family != GENL_ID_CTRL) 372 return THERMAL_ERROR; 373 374 return THERMAL_SUCCESS; 375 } 376