path: root/src/dtpm/dtpm.c

#define _GNU_SOURCE
#include <stdarg.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/epoll.h>
#include <sys/stat.h>
#include <sys/timerfd.h>
#include <sys/types.h>
#include <errno.h>
#include <fcntl.h>
#include <limits.h>
#include <unistd.h>
#include <dirent.h>
#include <mntent.h>
#include <pwd.h>

#include "log.h"
#include "conf.h"
#include "mainloop.h"
#include "display.h"
#include "thermal.h"

#define MAX_EVENTS 10
#define DTPM_CHILDREN_MAX 16
#define DTPM_CONSTRAINT_MAX 1

typedef unsigned long long u64;

#define DIV_ROUND_CLOSEST(x, divisor)(                  \
{                                                       \
        typeof(x) __x = x;                              \
        typeof(divisor) __d = divisor;                  \
        (((typeof(x))-1) > 0 ||                         \
         ((typeof(divisor))-1) > 0 ||                   \
         (((__x) > 0) == ((__d) > 0))) ?                \
                (((__x) + ((__d) / 2)) / (__d)) :       \
                (((__x) - ((__d) / 2)) / (__d));        \
}                                                       \
)

struct dtpm_field {
	FILE *file;
	const char *filename;
	const char *fmt;
	union {
		u64 u64val;
		char name[MAXNAMLEN];
	};
	void *ptr;
};

struct dtpm_constraint {
	struct dtpm_field name;
	struct dtpm_field power_limit_uw;
	struct dtpm_field time_window_us;
	struct dtpm_field max_power_uw;
};

struct dtpm {
	int level;
	char *path;
	u64 power_limit;
	u64 static_power_limit;
	u64 dynamic_power_limit;
	int nr_children;
	int weight;
	struct dtpm *parent;
	struct dtpm *children[DTPM_CHILDREN_MAX];
	struct dtpm_constraint constraints[DTPM_CONSTRAINT_MAX];
	struct dtpm_field max_power_range_uw;
	struct dtpm_field power_uw;
	struct dtpm_field name;
	struct thermal_zone *tz;
};

struct cb_param {
	struct thermal_handler *th;
	struct thermal_zone *tz;
	struct dtpm_config *cfg;
	struct dtpm *root;
	struct timeval tv;
	int line;
};

struct open_param {
	struct dtpm_field *field;
	const char *mode;
};

typedef int (*filter_t)(const char *);
typedef int (*dtpm_file_cb_t)(const char *, void *);
typedef int (*dtpm_node_cb_t)(struct dtpm *, void *);

#define dtpm_power_limit(__dtpm)	(__dtpm->constraints[0].power_limit_uw.u64val)
#define dtpm_power_max(__dtpm)		(__dtpm->constraints[0].max_power_uw.u64val)
#define dtpm_power(__dtpm) 		(__dtpm->power_uw.u64val)
#define dtpm_name(__dtpm)		(__dtpm->name.name)

int dtpm_set_power_limit(struct dtpm *dtpm, u64 power_limit)
{
	struct dtpm_field *field = &dtpm->constraints[0].power_limit_uw;

	DEBUG("Setting hardware power limit to '%llu' for '%s'",
	      power_limit, dtpm_name(dtpm));

	if (lseek(fileno(field->file), 0L, SEEK_SET) < 0)
		ERROR("Failed to lseek file: %d\n", errno);

	if (fprintf(field->file, "%llu", power_limit) < 0)
		ERROR("Failed to write power limit: %d\n", errno);

	return 0;
}

static int dtpm_read_field(struct dtpm_field *field)
{
	lseek(fileno(field->file), 0L, SEEK_SET);

	return fscanf(field->file, field->fmt, field->ptr) == EOF ? -1 : 0;
}

int dtpm_balance_set_static_power_limit(struct dtpm *dtpm, u64 power_limit)
{
	struct dtpm *child;
	u64 static_power_limit;
	int i, ret;

	DEBUG("Set static power limit %llu uW to '%s'",
	      power_limit, dtpm_name(dtpm));

	if (!dtpm->nr_children) {
		dtpm->dynamic_power_limit = power_limit;
		dtpm->static_power_limit = power_limit;
		return 0;
	}

	for (i = 0; i < dtpm->nr_children; i++) {
		child = dtpm->children[i];

		static_power_limit = DIV_ROUND_CLOSEST(
			power_limit * child->weight, 1024);

		ret = dtpm_balance_set_static_power_limit(child, static_power_limit);
		if (ret) 
			return ret;
	}
	
	return 0;
}

static int dtpm_balance_rebalance_limit(struct dtpm *dtpm)
{
	if (dtpm->power_limit)
		return dtpm_balance_set_static_power_limit(dtpm, dtpm->power_limit);

	return dtpm_balance_rebalance_limit(dtpm->parent);
}

int dtpm_balance_rebalance_power(struct dtpm *dtpm)
{
	struct dtpm *donees[DTPM_CHILDREN_MAX];
	struct dtpm *donors[DTPM_CHILDREN_MAX];
	int nr_donee, nr_donor, sum_weight;
	u64 power, power_free;
	int i, j;

again:
	nr_donee = nr_donor = sum_weight = 0;
	
	/*
	 * This first pass build the list of the donees and the sum of
	 * their weights.
	 */
	for (i = 0; i < dtpm->nr_children; i++) {
		struct dtpm *child;

		child = dtpm->children[i];
		power = dtpm_power(child);

		if (power > child->dynamic_power_limit) {

			DEBUG("'%s' exceeding power consumption (%llu/%llu)",
			      dtpm_name(child), power, child->dynamic_power_limit);

			/*
			 * We have one device which is consuming more
			 * energy than allocated but actually it
			 * donated the free power it had to another
			 * device. Now it is requesting more power, so
			 * it must get back the power it gave. Reset
			 * the dynamic power limit to the static power
			 * limit and do the rebalancing again.
			 */
			if (power < dtpm->power_limit) {
				dtpm_balance_rebalance_limit(dtpm);
				goto again;
			}

			donees[nr_donee++] = child;
			sum_weight += child->weight;
			continue;
		}

		if (power < child->dynamic_power_limit)
			donors[nr_donor++] = child;
	}

	/*
	 * There are no donees, so no need to rebalance the power
	 * across the actors, this is done, bail out.
	 */
	if (!nr_donee)
		return 0;

	/*
	 * We are in a situation where there is no more free power for
	 * the donees, so we need to reduce their power.
	 */
	if (!nr_donor) {

		/*
		 * We are in the situation where all the power was
		 * rebalanced along the actors but more power is
		 * consumed, in this case we need to cap their power.
		 */
		for (i = 0; i < nr_donee; i++)
			dtpm_set_power_limit(donees[i],
					     donees[i]->dynamic_power_limit);

		return 0;
	}
	
	/*
	 * One or more actors are consuming more than the allocated
	 * power. Let's distribute the free power not used by the
	 * other actors which are below their limit.
	 */
	for (i = 0; i < nr_donor; i++) {

		struct dtpm *donor = donors[i];

                power = dtpm_power(donor);

		/*
		 * Compute the unused power ...
		 */
		power_free = donor->dynamic_power_limit - power;

		/*
		 * ... and as it will be given to the donees,
		 * remove this power from the allocated budget from
		 * the donor.
		 */
		donor->dynamic_power_limit = power;
		
		/*
		 * Redistribute the unused power to the donees.
		 */
		for (j = 0; j < nr_donee; j++) {
			donees[j]->dynamic_power_limit +=
				DIV_ROUND_CLOSEST(
					power_free * donees[j]->weight,
					sum_weight);
		}
	}

	/*
	 * Let's find out new donees and new donors
	 */
	goto again;

	/* Never reached */
	return -1;
}

int dtpm_balance_rebalance_weight(struct dtpm *dtpm)
{
	struct dtpm *child;
	u64 max_power, pmax_power = dtpm_power_max(dtpm);
	int i;

	for (i = 0; i < dtpm->nr_children; i++) {

		child = dtpm->children[i];
		max_power = dtpm_power_max(child);
		
		child->weight = DIV_ROUND_CLOSEST(max_power * 1024, pmax_power);
	}

	return 0;
}

int dtpm_node_add_child(struct dtpm *node, struct dtpm *child)
{
	int i;

	if (!node)
		return 0;

	if (node->nr_children == DTPM_CHILDREN_MAX)
		return -1;
	
	for (i = 0; i < DTPM_CHILDREN_MAX; i++) {

		if (node->children[i])
			continue;

		node->children[i] = child;
		node->nr_children++;

		dtpm_balance_rebalance_weight(node);

		return 0;
	}

	return -1;
}

struct dtpm *dtpm_node_alloc(void)
{
	struct dtpm *node;
	int i;

	node = malloc(sizeof(*node));
	if (!node)
		return NULL;

	memset(node, 0, sizeof(*node));

	node->name.filename = "name";
	node->name.fmt = "%s";
	node->name.ptr = node->name.name;

	node->power_uw.filename = "power_uw";
	node->power_uw.fmt = "%llu";
	node->power_uw.ptr = &node->power_uw.u64val;

	node->max_power_range_uw.filename = "max_power_range_uw";
	node->max_power_range_uw.fmt = "%llu";
	node->max_power_range_uw.ptr = &node->max_power_range_uw.u64val;

	for (i = 0; i < DTPM_CONSTRAINT_MAX; i++) {
		node->constraints[i].name.filename = "constraint_0_name";
		node->constraints[i].name.fmt = "%s";
		node->constraints[i].name.ptr = node->constraints[i].name.name;

		node->constraints[i].power_limit_uw.filename = "constraint_0_power_limit_uw";
		node->constraints[i].power_limit_uw.fmt = "%llu";
		node->constraints[i].power_limit_uw.ptr = &node->constraints[i].power_limit_uw.u64val;

		node->constraints[i].time_window_us.filename = "constraint_0_time_window_us";
		node->constraints[i].time_window_us.fmt = "%llu";
		node->constraints[i].time_window_us.ptr = &node->constraints[i].time_window_us.u64val;

		node->constraints[i].max_power_uw.filename = "constraint_0_max_power_uw";
		node->constraints[i].max_power_uw.fmt = "%llu";
		node->constraints[i].max_power_uw.ptr = &node->constraints[i].max_power_uw.u64val;
	}
	
	return node;
}

void dtpm_node_free(struct dtpm *node)
{
	free(node);
}

#define HASHMASK 0x00FF
#if 0
static unsigned int hash(const char* key)
{
	unsigned int i;
	unsigned int h;

	h = 1315423911;
	for (i = 0; *key; key++, i++) {
		h ^= ((h << 5) + (*key) + (h >> 2));
	}
	h &= HASHMASK;

	return h;
}
#endif

int for_each_dtpm_node(struct dtpm *node, dtpm_node_cb_t cb, void *data)
{
	int i, ret;

	ret = cb(node, data);
	
	for (i = 0; i < DTPM_CHILDREN_MAX; i++) {

		if (!node->children[i])
			continue;

		ret += for_each_dtpm_node(node->children[i], cb, data);
	}

	return ret;
}

int dtpm_read(struct dtpm *node)
{
	int i, ret;

	struct open_param dtpm_fields[] = {
		{ &node->max_power_range_uw, "r"  },
		{ &node->power_uw, "r"            },
		{ &node->name, "r"                },
		{ &node->constraints[0].name, "r"         },
		{ &node->constraints[0].power_limit_uw, "rw" },
		{ &node->constraints[0].max_power_uw, "r" },
	};

	for (i = 0; i < sizeof(dtpm_fields) / sizeof(dtpm_fields[0]); i++) {
		ret |= dtpm_read_field(dtpm_fields[i].field);
	}

	return ret < 0 ? -1 : 0;
}

int dtpm_open_field(struct dtpm *node, struct dtpm_field *field,
		    const char *mode)
{
	char path[PATH_MAX];

	sprintf(path, "%s/%s", node->path, field->filename);

	field->file = fopen(path, mode);

	return field->file ? 0 : -1;
}

void dtpm_close(struct dtpm *node)
{
	int i;

	struct open_param dtpm_fields[] = {
		{ &node->max_power_range_uw, "r"  },
		{ &node->power_uw, "r"            },
		{ &node->name, "r"                },
		{ &node->constraints[0].name, "r"         },
		{ &node->constraints[0].power_limit_uw, "rw" },
		{ &node->constraints[0].max_power_uw, "r" },
	};

	for (i = 0; i < sizeof(dtpm_fields) / sizeof(dtpm_fields[0]); i++)
		fclose(dtpm_fields[i].field->file);
}

int dtpm_open(struct dtpm *node)
{
	int i;
	int ret = 0;

	struct open_param dtpm_fields[] = {
		{ &node->max_power_range_uw, "r"  },
		{ &node->power_uw, "r"            },
		{ &node->name, "r"                },
		{ &node->constraints[0].name, "r"         },
		{ &node->constraints[0].power_limit_uw, "rw" },
		{ &node->constraints[0].max_power_uw, "r" },
	};

	for (i = 0; i < sizeof(dtpm_fields) / sizeof(dtpm_fields[0]); i++) {
		ret += dtpm_open_field(node, dtpm_fields[i].field,
				       dtpm_fields[i].mode);
	}
	
	return ret;
}

int dtpm_init(const char *dirname, struct dtpm *node)
{
	node->path = strdup(dirname);
	if (!node->path) {
		fprintf(stderr, "Failed to strdup '%s': %m\n", dirname);
		return -1;
	}

	if (dtpm_open(node) < 0) {
		fprintf(stderr, "Failed to open node '%s'\n", dirname);
		return -1;
	}

	if (dtpm_read(node) < 0) {
		fprintf(stderr, "Failed to read node information '%s'\n", dirname);
		return -1;
	}

	return 0;
}

struct dtpm *dtpm_build_tree(const char *dirname, struct dtpm *parent)
{
	DIR *dir;
	int ret = -1;
	struct dirent *direntp;
	struct dtpm *new_node;

	DEBUG("Building tree from '%s'\n", dirname);
	
        dir = opendir(dirname);
        if (!dir) {
		ERROR("Failed to open directory '%s': %m\n", dirname);
		return NULL;
	}

	new_node = dtpm_node_alloc();
	if (!new_node) {
		ERROR("Failed to allocated node\n");
		goto out_close_dir;
	}

	new_node->parent = parent;
	new_node->level = parent ? parent->level + 1 : 0;
	
	ret = dtpm_init(dirname, new_node);
	if (ret)
		goto out_dtpm_node_free;

	while ((direntp = readdir(dir)) != NULL) {

		char *subdir;
		struct dtpm *child;

		if (strncmp(direntp->d_name, "dtpm", strlen("dtpm")))
			continue;
		
		if (asprintf(&subdir, "%s/%s", dirname, direntp->d_name) == -1) {
			fprintf(stderr, "Failed to allocate string: %m\n");
			goto out_close_dir;
		}
			
		child = dtpm_build_tree(subdir, new_node);
	
		if (!child) {
			fprintf(stderr, "Failed to build '%s': %m\n", subdir);
			free(subdir);
			goto out_dtpm_node_free;
		}

		free(subdir);
			
		ret = dtpm_node_add_child(new_node, child);
		if (ret) {
			fprintf(stderr, "Failed to add new node\n");
			goto out_dtpm_node_free;
		}
	}

out_close_dir:
	closedir(dir);
out_dtpm_node_free:
	if (ret)
		dtpm_node_free(new_node);

	return ret ? NULL : new_node;
}

static int tz_create(const char *name, int tz_id, __maybe_unused void *arg)
{
	printf("Thermal zone '%s'/%d created\n", name, tz_id);

	return 0;
}

static int tz_delete(int tz_id, __maybe_unused void *arg)
{
	printf("Thermal zone %d deleted\n", tz_id);

	return 0;
}

static int tz_disable(int tz_id, __maybe_unused void *arg)
{
	printf("Thermal zone %d disabled\n", tz_id);

	return 0;
}

static int tz_enable(int tz_id, __maybe_unused void *arg)
{
	printf("Thermal zone %d enabled\n", tz_id);

	return 0;
}

static int trip_high(int tz_id, int trip_id, int temp,
		     __maybe_unused void *arg)
{
	/* struct cb_param *cb_param = arg; */
	/* struct thermal_zone *tz = cb_param->tz; */
	
	/* printf("Thermal zone %d: trip point %d (%d mC°) crossed way up with %d °C\n", */
	/* tz_id, trip_id, tz[tz_id].trip[trip_id].temp, temp); */

	return 0;
}

static int trip_low(int tz_id, int trip_id, int temp, __maybe_unused void *arg)
{
	/* struct cb_param *cb_param = arg; */
	/* struct thermal_zone *tz = cb_param->tz; */
	
	/* printf("Thermal zone %d: trip point %d (%d mC°) crossed way down with %d °C\n", */
	/*        tz_id, trip_id, tz[tz_id].trip[trip_id].temp, temp); */

	return 0;
}

static int trip_add(int tz_id, int trip_id, int type, int temp, int hyst,
		    __maybe_unused void *arg)
{
	printf("Trip point added %d: id=%d, type=%d, temp=%d, hyst=%d\n",
	       tz_id, trip_id, type, temp, hyst);

	return 0;
}

static int trip_delete(int tz_id, int trip_id, __maybe_unused void *arg)
{
	printf("Trip point deleted %d: id=%d\n", tz_id, trip_id);

	return 0;
}

static int trip_change(int tz_id, int trip_id, int type, int temp, int hyst,
		       __maybe_unused void *arg)
{
	printf("Trip point changed %d: id=%d, type=%d, temp=%d, hyst=%d\n",
	       tz_id, trip_id, type, temp, hyst);

	return 0;
}

static int cdev_add(const char *name, int cdev_id, int max_state,
		    __maybe_unused void *arg)
{
	printf("Cooling device '%s'/%d (max state=%d) added",
	       name, cdev_id, max_state);
	
	return 0;
}

static int cdev_delete(int cdev_id, __maybe_unused void *arg)
{
	printf("Cooling device %d deleted", cdev_id);

	return 0;
}

static int cdev_update(int cdev_id, int cur_state, __maybe_unused void *arg)
{
	char buffer[4096];

	snprintf(buffer, sizeof(buffer) - 1, "cdev:%d state:%d",
		 cdev_id, cur_state);
	
	display_reset_cursor(15 + cdev_id);

	display_print_line(0, 0, buffer, 0, arg);
	
	display_refresh_pad(0);

	return 0;
}

static int gov_change(int tz_id, const char *name, __maybe_unused void *arg)
{
	printf("tz %d, governor=%s\n", tz_id, name);

	return 0;
}

static struct thermal_ops ops = {
	.events.tz_create	= tz_create,
	.events.tz_delete	= tz_delete,
	.events.tz_disable	= tz_disable,
	.events.tz_enable	= tz_enable,
	.events.trip_high	= trip_high,
	.events.trip_low	= trip_low,
	.events.trip_add	= trip_add,
	.events.trip_delete	= trip_delete,
	.events.trip_change	= trip_change,
	.events.cdev_add	= cdev_add,
	.events.cdev_delete	= cdev_delete,
	.events.cdev_update	= cdev_update,
	.events.gov_change	= gov_change
};

int show_power_uw(struct dtpm *node, void *arg)
{
	struct cb_param *cb_param = arg;
	struct thermal_handler *th = cb_param->th;
	char buffer[4096];
	size_t len;
	int bold = 0;

	dtpm_read_field(&node->power_uw);
	dtpm_read_field(&node->constraints[0].power_limit_uw);
	dtpm_read_field(&node->constraints[0].max_power_uw);

	len = snprintf(buffer, sizeof(buffer) - 1, "%*s%s: power=%llu uW (max=%llu uW)",
		       node->level, "", dtpm_name(node), dtpm_power(node), dtpm_power_max(node));

	if (node->tz) {
		if (thermal_cmd_get_temp(th, node->tz) < 0)
			return -1;
		
		len += snprintf(buffer + len, sizeof(buffer) + len - 1,
				" / temp=%d mC°", node->tz->temp);
	}

	if (node->power_limit)
		len = snprintf(buffer + len, sizeof(buffer) + len - 1, " (limit=%llu uW)",
			       node->power_limit);

	if (node->power_limit && dtpm_power(node) > node->power_limit) {
		bold = 1;
		/*
		 *
		 * TODO: Fix and check power limit setting
		 * dtpm_balance_rebalance_power(node);
		 *
		 */
	}

	display_print_line(0, cb_param->line++, buffer, bold, arg);

	return 0;
}

int tz_get_temp(struct thermal_zone *tz, void *arg)
{
	struct cb_param *cb_param = arg;
	struct thermal_handler *th = cb_param->th;

	gettimeofday(&cb_param->tv, NULL);
	
	return thermal_cmd_get_temp(th, tz);
}

static int event_callback(__maybe_unused int fd, void *arg)
{
	struct cb_param *cb_param = arg;
	struct thermal_handler *th = cb_param->th;

	return thermal_events_handle(th, arg);
}

static int timer_callback(int fd, void *arg)
{
	struct cb_param *cb_param = arg;
	struct dtpm *root = cb_param->root;
	struct thermal_zone *tz = cb_param->tz;
	char buf[8];

	cb_param->line = 3;

	display_reset_cursor(0);

	for_each_thermal_zone(tz, tz_get_temp, arg);
	
	if (for_each_dtpm_node(root, show_power_uw, arg))
		return -1;

	read(fd, buf, sizeof(buf));

	display_refresh_pad(0);
	
	return 0;
}

static int dtpm_configure(struct dtpm *node, void *arg)
{
	struct cb_param *cbp = arg;
	const char *name;

	name = dtpm_conf_get_thermal_zone(cbp->cfg, dtpm_name(node));
	if (name)
		node->tz = thermal_zone_find_by_name(cbp->tz, name);

	node->power_limit = dtpm_conf_get_power_limit(cbp->cfg, dtpm_name(node));
	if (node->power_limit) {
		dtpm_balance_set_static_power_limit(node, node->power_limit);
		DEBUG("'%s' has a power limit of %llu\n",
		      dtpm_name(node), node->power_limit);
	}
	
	return 0;
}

static struct timespec sec_to_timespec(float delay)
{
	struct timespec tv = {
		.tv_sec = (time_t)delay,
		.tv_nsec = (delay - tv.tv_sec) * 1000000000
	};

	return tv;
}

int main(void)
{
	struct cb_param cb_param;
	struct dtpm *root;
	struct thermal_zone *tz;
	struct thermal_handler *th;
	struct itimerspec timer_it = { 0 };
	struct dtpm_config *cfg;
	char *config_path = NULL;
	float delay = 0.5;
	int timerfd;
	
	/*
	if (getuid()) {
		CRITICAL("Needs root privileges\n");
		return -1;
	}
	*/

	if (dtpm_log_init(LOG_DEBUG))
		fprintf(stderr, "Failed to initialize logging facility\n");
	
	cfg = dtpm_config(config_path);
	if (!cfg)
		WARN("No configuration file\n");

	root = dtpm_build_tree(dtpm_path(), NULL);
	if (!root)
		return -1;

	th = thermal_init(&ops);
	if (!th)
		return -1;

	tz = thermal_zone_discover(th);
	if (!tz)
		return -1;

	cb_param.cfg = cfg;
	cb_param.root = root;
	cb_param.th = th;
	cb_param.tz = tz;

	if (for_each_dtpm_node(root, dtpm_configure, &cb_param))
		return -1;

	if (mainloop_init())
		return -1;
	
	timerfd = timerfd_create(CLOCK_MONOTONIC, 0);
	if (timerfd < 0)
		return -1;

	timer_it.it_interval = timer_it.it_value = sec_to_timespec(delay);

	if (timerfd_settime(timerfd, 0, &timer_it, NULL) < 0)
		return -1;
	
	if (mainloop_add(timerfd, timer_callback, &cb_param))
		return -1;

	if (mainloop_add(thermal_events_fd(th), event_callback, &cb_param))
		return -1;
	
	if (display_init())
		return -1;

	return mainloop(-1);
}