Quelle  thermal_nl.c   Sprache: 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;
}