From 21b9ae1675f82a1e37ab670b7510676c4dfdb8ce Mon Sep 17 00:00:00 2001
From: Steffen Vogel <post@steffenvogel.de>
Date: Mon, 13 Aug 2018 14:40:36 +0200
Subject: [PATCH] kernel: add kernel_get_cpu_frequency()

---
 include/villas/kernel/kernel.h |  3 ++
 lib/kernel/kernel.c            | 60 ++++++++++++++++++++++++++++++++++
 2 files changed, 63 insertions(+)

diff --git a/include/villas/kernel/kernel.h b/include/villas/kernel/kernel.h
index ef9c05827..2a2e2eb50 100644
--- a/include/villas/kernel/kernel.h
+++ b/include/villas/kernel/kernel.h
@@ -90,6 +90,9 @@ int kernel_get_page_size();
 /** Get the size of a huge page in bytes. */
 int kernel_get_hugepage_size();
 
+/** Get CPU base frequency */
+int kernel_get_cpu_frequency(uint64_t *freq);
+
 /** Set SMP affinity of IRQ */
 int kernel_irq_setaffinity(unsigned irq, uintmax_t aff , uintmax_t *old );
 
diff --git a/lib/kernel/kernel.c b/lib/kernel/kernel.c
index e500eb2fc..7da8d3a77 100644
--- a/lib/kernel/kernel.c
+++ b/lib/kernel/kernel.c
@@ -281,4 +281,64 @@ int kernel_irq_setaffinity(unsigned irq, uintmax_t aff, uintmax_t *old)
 	return ret;
 }
 
+int kernel_get_cpu_frequency(uint64_t *freq)
+{
+	char *line = NULL, *sep, *end;
+	size_t len = 0;
+	double dfreq;
+	int ret;
+	FILE *f;
+
+	/* Try to get CPU frequency from cpufreq module */
+	f = fopen(SYSFS_PATH "/devices/system/cpu/cpu0/cpufreq/cpuinfo_max_freq", "r");
+	if (!f)
+		goto cpuinfo;
+
+	ret = fscanf(f, "%" PRIu64, freq);
+	fclose(f);
+	if (ret != 1)
+		return -1;
+
+	/* cpufreq reports kHz */
+	*freq = *freq * 1000;
+
+	return 0;
+
+cpuinfo:
+	/* Try to read CPU frequency from /proc/cpuinfo */
+	f = fopen(PROCFS_PATH "/cpuinfo", "r");
+	if (!f)
+		return -1; /* We give up here */
+
+	ret = -1;
+	while (getline(&line, &len, f) >= 0) {
+		if (strstr(line,  "cpu MHz") == line) {
+			ret = 0;
+			break;
+		}
+	}
+	if (ret)
+		goto out;
+
+	sep = strchr(line, ':');
+	if (!sep) {
+		ret = -1;
+		goto out;
+	}
+
+	dfreq = strtod(sep+1, &end);
+
+	if (end == sep+1) {
+		ret = -1;
+		goto out;
+	}
+
+	/* Frequency is given in MHz */
+	*freq = dfreq * 1e6;
+
+out:	fclose(f);
+	free(line);
+
+	return ret;
+}
 #endif /* __linux__ */