mirror of
https://git.kernel.org/pub/scm/linux/kernel/git/stable/linux.git
synced 2024-08-22 17:01:14 +00:00
216 lines
4.3 KiB
C
216 lines
4.3 KiB
C
![]() |
// SPDX-License-Identifier: LGPL-2.1+
|
||
|
// Copyright (C) 2022, Linaro Ltd - Daniel Lezcano <daniel.lezcano@linaro.org>
|
||
|
#include <errno.h>
|
||
|
#include <stdio.h>
|
||
|
#include <stdlib.h>
|
||
|
#include <unistd.h>
|
||
|
|
||
|
#include <thermal.h>
|
||
|
#include "thermal_nl.h"
|
||
|
|
||
|
struct handler_args {
|
||
|
const char *group;
|
||
|
int id;
|
||
|
};
|
||
|
|
||
|
static __thread int err;
|
||
|
static __thread int done;
|
||
|
|
||
|
static int nl_seq_check_handler(struct nl_msg *msg, void *arg)
|
||
|
{
|
||
|
return NL_OK;
|
||
|
}
|
||
|
|
||
|
static int nl_error_handler(struct sockaddr_nl *nla, struct nlmsgerr *nl_err,
|
||
|
void *arg)
|
||
|
{
|
||
|
int *ret = arg;
|
||
|
|
||
|
if (ret)
|
||
|
*ret = nl_err->error;
|
||
|
|
||
|
return NL_STOP;
|
||
|
}
|
||
|
|
||
|
static int nl_finish_handler(struct nl_msg *msg, void *arg)
|
||
|
{
|
||
|
int *ret = arg;
|
||
|
|
||
|
if (ret)
|
||
|
*ret = 1;
|
||
|
|
||
|
return NL_OK;
|
||
|
}
|
||
|
|
||
|
static int nl_ack_handler(struct nl_msg *msg, void *arg)
|
||
|
{
|
||
|
int *ret = arg;
|
||
|
|
||
|
if (ret)
|
||
|
*ret = 1;
|
||
|
|
||
|
return NL_OK;
|
||
|
}
|
||
|
|
||
|
int nl_send_msg(struct nl_sock *sock, struct nl_cb *cb, struct nl_msg *msg,
|
||
|
int (*rx_handler)(struct nl_msg *, void *), void *data)
|
||
|
{
|
||
|
if (!rx_handler)
|
||
|
return THERMAL_ERROR;
|
||
|
|
||
|
err = nl_send_auto_complete(sock, msg);
|
||
|
if (err < 0)
|
||
|
return err;
|
||
|
|
||
|
nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, rx_handler, data);
|
||
|
|
||
|
err = done = 0;
|
||
|
|
||
|
while (err == 0 && done == 0)
|
||
|
nl_recvmsgs(sock, cb);
|
||
|
|
||
|
return err;
|
||
|
}
|
||
|
|
||
|
static int nl_family_handler(struct nl_msg *msg, void *arg)
|
||
|
{
|
||
|
struct handler_args *grp = arg;
|
||
|
struct nlattr *tb[CTRL_ATTR_MAX + 1];
|
||
|
struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
|
||
|
struct nlattr *mcgrp;
|
||
|
int rem_mcgrp;
|
||
|
|
||
|
nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
|
||
|
genlmsg_attrlen(gnlh, 0), NULL);
|
||
|
|
||
|
if (!tb[CTRL_ATTR_MCAST_GROUPS])
|
||
|
return THERMAL_ERROR;
|
||
|
|
||
|
nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], rem_mcgrp) {
|
||
|
|
||
|
struct nlattr *tb_mcgrp[CTRL_ATTR_MCAST_GRP_MAX + 1];
|
||
|
|
||
|
nla_parse(tb_mcgrp, CTRL_ATTR_MCAST_GRP_MAX,
|
||
|
nla_data(mcgrp), nla_len(mcgrp), NULL);
|
||
|
|
||
|
if (!tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME] ||
|
||
|
!tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID])
|
||
|
continue;
|
||
|
|
||
|
if (strncmp(nla_data(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]),
|
||
|
grp->group,
|
||
|
nla_len(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME])))
|
||
|
continue;
|
||
|
|
||
|
grp->id = nla_get_u32(tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]);
|
||
|
|
||
|
break;
|
||
|
}
|
||
|
|
||
|
return THERMAL_SUCCESS;
|
||
|
}
|
||
|
|
||
|
static int nl_get_multicast_id(struct nl_sock *sock, struct nl_cb *cb,
|
||
|
const char *family, const char *group)
|
||
|
{
|
||
|
struct nl_msg *msg;
|
||
|
int ret = 0, ctrlid;
|
||
|
struct handler_args grp = {
|
||
|
.group = group,
|
||
|
.id = -ENOENT,
|
||
|
};
|
||
|
|
||
|
msg = nlmsg_alloc();
|
||
|
if (!msg)
|
||
|
return THERMAL_ERROR;
|
||
|
|
||
|
ctrlid = genl_ctrl_resolve(sock, "nlctrl");
|
||
|
|
||
|
genlmsg_put(msg, 0, 0, ctrlid, 0, 0, CTRL_CMD_GETFAMILY, 0);
|
||
|
|
||
|
nla_put_string(msg, CTRL_ATTR_FAMILY_NAME, family);
|
||
|
|
||
|
ret = nl_send_msg(sock, cb, msg, nl_family_handler, &grp);
|
||
|
if (ret)
|
||
|
goto nla_put_failure;
|
||
|
|
||
|
ret = grp.id;
|
||
|
|
||
|
nla_put_failure:
|
||
|
nlmsg_free(msg);
|
||
|
return ret;
|
||
|
}
|
||
|
|
||
|
int nl_thermal_connect(struct nl_sock **nl_sock, struct nl_cb **nl_cb)
|
||
|
{
|
||
|
struct nl_cb *cb;
|
||
|
struct nl_sock *sock;
|
||
|
|
||
|
cb = nl_cb_alloc(NL_CB_DEFAULT);
|
||
|
if (!cb)
|
||
|
return THERMAL_ERROR;
|
||
|
|
||
|
sock = nl_socket_alloc();
|
||
|
if (!sock)
|
||
|
goto out_cb_free;
|
||
|
|
||
|
if (genl_connect(sock))
|
||
|
goto out_socket_free;
|
||
|
|
||
|
if (nl_cb_err(cb, NL_CB_CUSTOM, nl_error_handler, &err) ||
|
||
|
nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, nl_finish_handler, &done) ||
|
||
|
nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, nl_ack_handler, &done) ||
|
||
|
nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, nl_seq_check_handler, &done))
|
||
|
return THERMAL_ERROR;
|
||
|
|
||
|
*nl_sock = sock;
|
||
|
*nl_cb = cb;
|
||
|
|
||
|
return THERMAL_SUCCESS;
|
||
|
|
||
|
out_socket_free:
|
||
|
nl_socket_free(sock);
|
||
|
out_cb_free:
|
||
|
nl_cb_put(cb);
|
||
|
return THERMAL_ERROR;
|
||
|
}
|
||
|
|
||
|
void nl_thermal_disconnect(struct nl_sock *nl_sock, struct nl_cb *nl_cb)
|
||
|
{
|
||
|
nl_close(nl_sock);
|
||
|
nl_socket_free(nl_sock);
|
||
|
nl_cb_put(nl_cb);
|
||
|
}
|
||
|
|
||
|
int nl_unsubscribe_thermal(struct nl_sock *nl_sock, struct nl_cb *nl_cb,
|
||
|
const char *group)
|
||
|
{
|
||
|
int mcid;
|
||
|
|
||
|
mcid = nl_get_multicast_id(nl_sock, nl_cb, THERMAL_GENL_FAMILY_NAME,
|
||
|
group);
|
||
|
if (mcid < 0)
|
||
|
return THERMAL_ERROR;
|
||
|
|
||
|
if (nl_socket_drop_membership(nl_sock, mcid))
|
||
|
return THERMAL_ERROR;
|
||
|
|
||
|
return THERMAL_SUCCESS;
|
||
|
}
|
||
|
|
||
|
int nl_subscribe_thermal(struct nl_sock *nl_sock, struct nl_cb *nl_cb,
|
||
|
const char *group)
|
||
|
{
|
||
|
int mcid;
|
||
|
|
||
|
mcid = nl_get_multicast_id(nl_sock, nl_cb, THERMAL_GENL_FAMILY_NAME,
|
||
|
group);
|
||
|
if (mcid < 0)
|
||
|
return THERMAL_ERROR;
|
||
|
|
||
|
if (nl_socket_add_membership(nl_sock, mcid))
|
||
|
return THERMAL_ERROR;
|
||
|
|
||
|
return THERMAL_SUCCESS;
|
||
|
}
|