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 <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;

struct dtpm_field {
	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_node {
	int level;
	char *path;
	struct dtpm_node *parent;
	struct dtpm_node *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_node *root;
	int line;
};

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

static int dtpm_file_rw(const char *filename, int rw, const char *fmt, ...)
{
	const char *mode = rw == O_RDONLY ? "r" : "w";
 	FILE *f;
        va_list args;
	int ret;

	f = fopen(filename, mode);
	if (!f) {
		ERROR("Failed to open %s:%m\n", filename);
		return -1;
	}

	va_start (args, fmt);
	ret = rw == O_RDONLY ? vfscanf(f, fmt, args) : vfprintf(f, fmt, args);
	va_end(args);

	fclose(f);

	return ret == EOF;
}

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

	if (!node)
		return 0;
	
	for (i = 0; i < DTPM_CHILDREN_MAX; i++) {

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

		node->children[i] = child;

		return 0;
	}

	return -1;
}

struct dtpm_node *dtpm_node_alloc(void)
{
	struct dtpm_node *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 *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 *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;
}

static int dtpm_read_field(struct dtpm_node *node, struct dtpm_field *field)
{
	char path[PATH_MAX];

	sprintf(path, "%s/%s", node->path, field->filename);
	
	return dtpm_file_rw(path, O_RDONLY, field->fmt, field->ptr);
}

int dtpm_read(struct dtpm_node *node)
{
	int i;
	int ret = 0;
	
	ret += dtpm_read_field(node, &node->max_power_range_uw);
	ret += dtpm_read_field(node, &node->power_uw);
	ret += dtpm_read_field(node, &node->name);

	for (i = 0; i < DTPM_CONSTRAINT_MAX; i++) {
		ret += dtpm_read_field(node, &node->constraints[i].name);
		ret += dtpm_read_field(node, &node->constraints[i].power_limit_uw);
		ret += dtpm_read_field(node, &node->constraints[i].max_power_uw);
	}

	return ret;
}

int dtpm_init(const char *dirname, struct dtpm_node *node)
{
	node->path = strdup(dirname);
	if (!node->path) {
		fprintf(stderr, "Failed to strdup '%s': %m\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_node *dtpm_build_tree(const char *dirname, struct dtpm_node *parent)
{
	DIR *dir;
	int ret = -1;
	struct dirent *direntp;
	struct dtpm_node *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_node *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)
{
	struct cb_param *cb_param = 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 *node, void *arg)
{
	struct cb_param *cb_param = arg;
	struct thermal_handler *th = cb_param->th;
	char buffer[4096];
	size_t len;

	dtpm_read_field(node, &node->power_uw);

	len = snprintf(buffer, sizeof(buffer) - 1, "%*s%s: power=%llu uW", node->level, "",
		       node->name.name, node->power_uw.u64val);

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

	display_print_line(0, cb_param->line++, buffer, 0, 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;

	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_node *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_bind_thermal_zone(struct dtpm_node *node, void *arg)
{
	struct cb_param *cbp = arg;
	struct thermal_zone *tz;
	const char *name;

	name = dtpm_get_thermal_zone(cbp->cfg, node->name.ptr);
	if (!name)
		return 0;

	tz = thermal_zone_find_by_name(cbp->tz, name);
	if (!tz)
		return 0;

	node->tz = tz;
	
	DEBUG("'%s' belongs to the thermal zone '%s'\n",
	      (char *)node->name.ptr, name);

	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_node *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;
	}
	*/

	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_bind_thermal_zone, &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);
}