From c65cda183609116760c54f1c2ad458eef9f44d56 Mon Sep 17 00:00:00 2001

From: "Chang S. Bae" <chang.seok.bae@intel.com>

Date: Fri, 11 Feb 2022 12:27:00 -0800

Subject: [PATCH 05/14] Implement Netlink helper functions to subscribe thermal

 events

Establish Netlink socket connection in a nonblocking mode and callback

notification.

Tolerate a few times on failures from receiving events. In practice, such

delivery failure is rare. But it may indicate more fundamental issues if

it repeats. So disconnect it unless the failure is transient.

Event-specific handler will be implemented in the next patch.

Signed-off-by: Chang S. Bae <chang.seok.bae@intel.com>

---

 thermal.c | 327 +++++++++++++++++++++++++++++++++++++++++++++++++++---

 1 file changed, 313 insertions(+), 14 deletions(-)

diff --git a/thermal.c b/thermal.c

index 308bc48..16a6f39 100644

--- a/thermal.c

+++ b/thermal.c

@@ -14,43 +14,342 @@

 cpumask_t thermal_banned_cpus;

+#define INVALID_NL_FD	-1

+#define MAX_RECV_ERRS	2

+

+#define THERMAL_GENL_FAMILY_NAME	"thermal"

+#define THERMAL_GENL_EVENT_GROUP_NAME	"event"

+#define NL_FAMILY_NAME			"nlctrl"

+

+struct family_data {

+	const char *group;

+	int id;

+};

+

+static struct nl_sock *sock;

+static struct nl_cb *callback;

+

+/*

+ * return value: TRUE with an error; otherwise, FALSE

+ */

 static gboolean prepare_netlink(void)

 {

-	gboolean error = TRUE;

+	int rc;

-	log(TO_ALL, LOG_ERR, "thermal: not yet implement to alloc memory for netlink.\n");

-	return error;

+	sock = nl_socket_alloc();

+	if (!sock) {

+		log(TO_ALL, LOG_ERR, "thermal: socket allocation failed.\n");

+		return TRUE;

+	}

+

+	rc = genl_connect(sock);

+	if (rc) {

+		log(TO_ALL, LOG_ERR, "thermal: socket bind failed.\n");

+		return TRUE;

+	}

+

+	callback = nl_cb_alloc(NL_CB_DEFAULT);

+	if (!callback) {

+		log(TO_ALL, LOG_ERR, "thermal: callback allocation failed.\n");

+		return TRUE;

+	}

+

+	return FALSE;

 }

-#define NL_FAMILY_NAME	"nlctrl"

+static int handle_groupid(struct nl_msg *msg, void *arg)

+{

+	struct nlattr *attrhdr, *mcgrp, *cur_mcgrp;

+	struct nlattr *attrs[CTRL_ATTR_MAX + 1];

+	struct nla_policy *policy = NULL;

+	struct genlmsghdr *gnlhdr = NULL;

+	struct family_data *data = NULL;

+	struct nlmsghdr *msghdr = NULL;

+	int attrlen, rc, i;

+

+	if (!arg) {

+		log(TO_ALL, LOG_ERR, "thermal: group id - failed to receive argument.\n");

+		return NL_SKIP;

+	}

+	data = arg;

+

+	/* get actual netlink message header */

+	msghdr = nlmsg_hdr(msg);

+

+	/* get the start of the message payload */

+	gnlhdr = nlmsg_data(msghdr);

+

+	/* get the start of the message attribute section */

+	attrhdr = genlmsg_attrdata(gnlhdr, 0);

+

+	/* get the length of the message attribute section */

+	attrlen = genlmsg_attrlen(gnlhdr, 0);

+

+	/* create attribute index based on a stream of attributes */

+	rc = nla_parse(

+		attrs,		/* index array to be filled */

+		CTRL_ATTR_MAX,	/* the maximum acceptable attribute type */

+		attrhdr,	/* head of attribute stream */

+		attrlen,	/* length of attribute stream */

+		policy);	/* validation policy */

+	if (rc) {

+		log(TO_ALL, LOG_ERR, "thermal: group id - failed to create attributes.\n");

+		return NL_SKIP;

+	}

+

+	/* start of the multi-cast group attribute */

+	mcgrp = attrs[CTRL_ATTR_MCAST_GROUPS];

+	if (!mcgrp) {

+		log(TO_ALL, LOG_ERR, "thermal: group id - no multi-cast group attributes.\n");

+		return NL_SKIP;

+	}

+

+	/* iterate a stream of nested attributes to get the group id */

+	nla_for_each_nested(cur_mcgrp, mcgrp, i) {

+		struct nlattr *nested_attrs[CTRL_ATTR_MCAST_GRP_MAX + 1];

+		struct nlattr *name, *id;

+

+		/* get start and length of payload section */

+		attrhdr = nla_data(cur_mcgrp);

+		attrlen = nla_len(cur_mcgrp);

+

+		rc = nla_parse(nested_attrs, CTRL_ATTR_MCAST_GRP_MAX, attrhdr, attrlen, policy);

+		if (rc)

+			continue;

+

+		name = nested_attrs[CTRL_ATTR_MCAST_GRP_NAME];

+		id = nested_attrs[CTRL_ATTR_MCAST_GRP_ID];

+		if (!name || !id)

+			continue;

+

+		if (strncmp(nla_data(name), data->group, nla_len(name)) != 0)

+			continue;

+

+		data->id = nla_get_u32(id);

+		log(TO_ALL, LOG_DEBUG, "thermal: received group id (%d).\n", data->id);

+		break;

+	}

+	return NL_OK;

+}

+

+static int handle_error(struct sockaddr_nl *sk_addr __attribute__((unused)),

+			struct nlmsgerr *err, void *arg)

+{

+	if (arg) {

+		log(TO_ALL, LOG_INFO, "thermal: received a netlink error (%s).\n",

+		    nl_geterror(err->error));

+		*((int *)arg) = err->error;

+	}

+	return NL_SKIP;

+}

+

+static int handle_end(struct nl_msg *msg __attribute__((unused)), void *arg)

+{

+	if (arg)

+		*((int *)arg) = 0;

+	return NL_SKIP;

+}

+

+struct msgheader {

+	unsigned char cmd, version;

+	unsigned int port, seq;

+	int id, hdrlen, flags;

+};

 static gboolean establish_netlink(void)

 {

+	struct msgheader msghdr = { CTRL_CMD_GETFAMILY, 0, 0, 0, 0, 0, 0 };

+	struct family_data nldata = { THERMAL_GENL_EVENT_GROUP_NAME, -ENOENT };

+	struct nl_cb *cloned_callback = NULL;

+	int rc, group_id, callback_rc = 1;

+	struct nl_msg *msg = NULL;

 	gboolean error = TRUE;

+	void *hdr;

+

+	msg = nlmsg_alloc();

+	if (!msg) {

+		log(TO_ALL, LOG_ERR, "thermal: message allocation failed.\n");

+		goto err_out;

+	}

+

+	msghdr.id = genl_ctrl_resolve(sock, NL_FAMILY_NAME);

+	if (msghdr.id < 0) {

+		log(TO_ALL, LOG_ERR, "thermal: message id enumeration failed.\n");

+		goto err_out;

+	}

+

+	hdr = genlmsg_put(msg, msghdr.port, msghdr.seq, msghdr.id, msghdr.hdrlen,

+			  msghdr.flags, msghdr.cmd, msghdr.version);

+	if (!hdr) {

+		log(TO_ALL, LOG_ERR, "thermal: netlink header setup failed.\n");

+		goto err_out;

+	}

+

+	rc = nla_put_string(msg, CTRL_ATTR_FAMILY_NAME, THERMAL_GENL_FAMILY_NAME);

+	if (rc) {

+		log(TO_ALL, LOG_ERR, "thermal: message setup failed.\n");

+		goto err_out;

+	}

+

+	cloned_callback = nl_cb_clone(callback);

+	if (!cloned_callback) {

+		log(TO_ALL, LOG_ERR, "thermal: callback handle duplication failed.\n");

+		goto err_out;

+	}

+

+	rc = nl_send_auto(sock, msg);

+	if (rc < 0) {

+		log(TO_ALL, LOG_ERR, "thermal: failed to send the first message.\n");

+		goto err_out;

+	}

-	log(TO_ALL, LOG_ERR, "thermal: not yet implemented to establish netlink.\n");

+	rc = nl_cb_err(cloned_callback, NL_CB_CUSTOM, handle_error, &callback_rc);

+	if (rc) {

+		log(TO_ALL, LOG_ERR, "thermal: error callback setup failed.\n");

+		goto err_out;

+	}

+

+	rc = nl_cb_set(cloned_callback, NL_CB_ACK, NL_CB_CUSTOM, handle_end, &callback_rc);

+	if (rc) {

+		log(TO_ALL, LOG_ERR, "thermal: ack callback setup failed.\n");

+		goto err_out;

+	}

+

+	rc = nl_cb_set(cloned_callback, NL_CB_FINISH, NL_CB_CUSTOM, handle_end, &callback_rc);

+	if (rc) {

+		log(TO_ALL, LOG_ERR, "thermal: finish callback setup failed.\n");

+		goto err_out;

+	}

+

+	rc = nl_cb_set(cloned_callback, NL_CB_VALID, NL_CB_CUSTOM, handle_groupid, &nldata);

+	if (rc) {

+		log(TO_ALL, LOG_ERR, "thermal: group id callback setup failed.\n");

+		goto err_out;

+	}

+

+	while (callback_rc != 0) {

+		rc = nl_recvmsgs(sock, cloned_callback);

+		if (rc < 0) {

+			log(TO_ALL, LOG_ERR, "thermal: failed to receive messages.\n");

+			goto err_out;

+		}

+	}

+

+	group_id = nldata.id;

+	if (group_id < 0) {

+		log(TO_ALL, LOG_ERR, "thermal: invalid group_id was received.\n");

+		goto err_out;

+	}

+

+	rc = nl_socket_add_membership(sock, group_id);

+	if (rc) {

+		log(TO_ALL, LOG_ERR, "thermal: failed to join the netlink group.\n");

+		goto err_out;

+	}

+

+	error = FALSE;

+err_out:

+	nl_cb_put(cloned_callback);

+	nlmsg_free(msg);

 	return error;

 }

-static gboolean register_netlink_handler(nl_recvmsg_msg_cb_t handler __attribute__((unused)))

+static int handle_thermal_event(struct nl_msg *msg __attribute__((unused)),

+				void *arg __attribute__((unused)))

 {

-	gboolean error = TRUE;

+	log(TO_ALL, LOG_ERR, "thermal: not yet implemented to process thermal event.\n");

+	return NL_SKIP;

+}

-	log(TO_ALL, LOG_ERR, "thermal: not yet implemented to register thermal handler.\n");

-	return error;

+static int handler_for_debug(struct nl_msg *msg __attribute__((unused)),

+			     void *arg __attribute__((unused)))

+{

+	return NL_SKIP;

+}

+

+/*

+ * return value: TRUE with an error; otherwise, FALSE

+ */

+static gboolean register_netlink_handler(void)

+{

+	int rc;

+

+	rc = nl_cb_set(callback, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, handler_for_debug, NULL);

+	if (rc) {

+		log(TO_ALL, LOG_ERR, "thermal: debug handler registration failed.\n");

+		return TRUE;

+	}

+

+

+	rc = nl_cb_set(callback, NL_CB_VALID, NL_CB_CUSTOM, handle_thermal_event, NULL);

+	if (rc) {

+		log(TO_ALL, LOG_ERR, "thermal: thermal handler registration failed.\n");

+		return TRUE;

+	}

+

+	return FALSE;

 }

+/*

+ * return value: TRUE to keep the source; FALSE to disconnect.

+ */

+gboolean receive_thermal_event(gint fd __attribute__((unused)),

+			       GIOCondition condition,

+			       gpointer user_data __attribute__((unused)))

+{

+	if (condition == G_IO_IN) {

+		static unsigned int retry = 0;

+		int err;

+

+		err = nl_recvmsgs(sock, callback);

+		if (err) {

+			log(TO_ALL, LOG_ERR, "thermal: failed to receive messages (rc=%d).\n", err);

+			retry++;

+

+			/*

+			 * Pass a few failures then turn off if it keeps

+			 * failing down.

+			 */

+			if (retry <= MAX_RECV_ERRS) {

+				log(TO_ALL, LOG_ERR, "thermal: but keep the connection.\n");

+			} else {

+				log(TO_ALL, LOG_ERR, "thermal: disconnect now with %u failures.\n",

+				    retry);

+				return FALSE;

+			}

+		}

+	}

+	return TRUE;

+}

+

+/*

+ * return value: TRUE with an error; otherwise, FALSE

+ */

 static gboolean set_netlink_nonblocking(void)

 {

-	gboolean error = TRUE;

+	int rc, fd;

-	log(TO_ALL, LOG_ERR, "thermal: not yet implemented to set nonblocking socket.\n");

-	return error;

+	rc = nl_socket_set_nonblocking(sock);

+	if (rc) {

+		log(TO_ALL, LOG_ERR, "thermal: non-blocking mode setup failed.\n");

+		return TRUE;

+	}

+

+	fd = nl_socket_get_fd(sock);

+	if (fd == INVALID_NL_FD) {

+		log(TO_ALL, LOG_ERR, "thermal: file descriptor setup failed.\n");

+		return TRUE;

+	}

+

+	g_unix_fd_add(fd, G_IO_IN, receive_thermal_event, NULL);

+

+	return FALSE;

 }

 void deinit_thermal(void)

 {

-	return;

+	nl_cb_put(callback);

+	nl_socket_free(sock);

 }

 /*

@@ -68,7 +367,7 @@ gboolean init_thermal(void)

 	if (error)

 		goto err_out;

-	error = register_netlink_handler(NULL);

+	error = register_netlink_handler();

 	if (error)

 		goto err_out;

-- 

2.33.1