1// SPDX-License-Identifier: GPL-2.0
  2
  3#define _GNU_SOURCE
  4#include <errno.h>
  5#include <stdio.h>
  6#include <stdlib.h>
  7#include <signal.h>
  8#include <sched.h>
  9#include <string.h>
 10#include <unistd.h>
 11#include <fcntl.h>
 12#include <locale.h>
 13#include <sys/types.h>
 14#include <sys/stat.h>
 15#include <sys/time.h>
 16#include <sys/resource.h>
 17#include <sys/wait.h>
 18
 19#include <bpf/bpf.h>
 20#include <bpf/libbpf.h>
 21
 22static int cstate_map_fd, pstate_map_fd;
 23
 24#define MAX_CPU			8
 25#define MAX_PSTATE_ENTRIES	5
 26#define MAX_CSTATE_ENTRIES	3
 27#define MAX_STARS		40
 28
 29#define CPUFREQ_MAX_SYSFS_PATH	"/sys/devices/system/cpu/cpu0/cpufreq/scaling_max_freq"
 30#define CPUFREQ_LOWEST_FREQ	"208000"
 31#define CPUFREQ_HIGHEST_FREQ	"12000000"
 32
 33struct cpu_stat_data {
 34	unsigned long cstate[MAX_CSTATE_ENTRIES];
 35	unsigned long pstate[MAX_PSTATE_ENTRIES];
 36};
 37
 38static struct cpu_stat_data stat_data[MAX_CPU];
 39
 40static void cpu_stat_print(void)
 41{
 42	int i, j;
 43	char state_str[sizeof("cstate-9")];
 44	struct cpu_stat_data *data;
 45
 46	/* Clear screen */
 47	printf("\033[2J");
 48
 49	/* Header */
 50	printf("\nCPU states statistics:\n");
 51	printf("%-10s ", "state(ms)");
 52
 53	for (i = 0; i < MAX_CSTATE_ENTRIES; i++) {
 54		sprintf(state_str, "cstate-%d", i);
 55		printf("%-11s ", state_str);
 56	}
 57
 58	for (i = 0; i < MAX_PSTATE_ENTRIES; i++) {
 59		sprintf(state_str, "pstate-%d", i);
 60		printf("%-11s ", state_str);
 61	}
 62
 63	printf("\n");
 64
 65	for (j = 0; j < MAX_CPU; j++) {
 66		data = &stat_data[j];
 67
 68		printf("CPU-%-6d ", j);
 69		for (i = 0; i < MAX_CSTATE_ENTRIES; i++)
 70			printf("%-11ld ", data->cstate[i] / 1000000);
 71
 72		for (i = 0; i < MAX_PSTATE_ENTRIES; i++)
 73			printf("%-11ld ", data->pstate[i] / 1000000);
 74
 75		printf("\n");
 76	}
 77}
 78
 79static void cpu_stat_update(int cstate_fd, int pstate_fd)
 80{
 81	unsigned long key, value;
 82	int c, i;
 83
 84	for (c = 0; c < MAX_CPU; c++) {
 85		for (i = 0; i < MAX_CSTATE_ENTRIES; i++) {
 86			key = c * MAX_CSTATE_ENTRIES + i;
 87			bpf_map_lookup_elem(cstate_fd, &key, &value);
 88			stat_data[c].cstate[i] = value;
 89		}
 90
 91		for (i = 0; i < MAX_PSTATE_ENTRIES; i++) {
 92			key = c * MAX_PSTATE_ENTRIES + i;
 93			bpf_map_lookup_elem(pstate_fd, &key, &value);
 94			stat_data[c].pstate[i] = value;
 95		}
 96	}
 97}
 98
 99/*
100 * This function is copied from 'idlestat' tool function
101 * idlestat_wake_all() in idlestate.c.
102 *
103 * It sets the self running task affinity to cpus one by one so can wake up
104 * the specific CPU to handle scheduling; this results in all cpus can be
105 * waken up once and produce ftrace event 'trace_cpu_idle'.
106 */
107static int cpu_stat_inject_cpu_idle_event(void)
108{
109	int rcpu, i, ret;
110	cpu_set_t cpumask;
111	cpu_set_t original_cpumask;
112
113	ret = sysconf(_SC_NPROCESSORS_CONF);
114	if (ret < 0)
115		return -1;
116
117	rcpu = sched_getcpu();
118	if (rcpu < 0)
119		return -1;
120
121	/* Keep track of the CPUs we will run on */
122	sched_getaffinity(0, sizeof(original_cpumask), &original_cpumask);
123
124	for (i = 0; i < ret; i++) {
125
126		/* Pointless to wake up ourself */
127		if (i == rcpu)
128			continue;
129
130		/* Pointless to wake CPUs we will not run on */
131		if (!CPU_ISSET(i, &original_cpumask))
132			continue;
133
134		CPU_ZERO(&cpumask);
135		CPU_SET(i, &cpumask);
136
137		sched_setaffinity(0, sizeof(cpumask), &cpumask);
138	}
139
140	/* Enable all the CPUs of the original mask */
141	sched_setaffinity(0, sizeof(original_cpumask), &original_cpumask);
142	return 0;
143}
144
145/*
146 * It's possible to have no any frequency change for long time and cannot
147 * get ftrace event 'trace_cpu_frequency' for long period, this introduces
148 * big deviation for pstate statistics.
149 *
150 * To solve this issue, below code forces to set 'scaling_max_freq' to 208MHz
151 * for triggering ftrace event 'trace_cpu_frequency' and then recovery back to
152 * the maximum frequency value 1.2GHz.
153 */
154static int cpu_stat_inject_cpu_frequency_event(void)
155{
156	int len, fd;
157
158	fd = open(CPUFREQ_MAX_SYSFS_PATH, O_WRONLY);
159	if (fd < 0) {
160		printf("failed to open scaling_max_freq, errno=%d\n", errno);
161		return fd;
162	}
163
164	len = write(fd, CPUFREQ_LOWEST_FREQ, strlen(CPUFREQ_LOWEST_FREQ));
165	if (len < 0) {
166		printf("failed to open scaling_max_freq, errno=%d\n", errno);
167		goto err;
168	}
169
170	len = write(fd, CPUFREQ_HIGHEST_FREQ, strlen(CPUFREQ_HIGHEST_FREQ));
171	if (len < 0) {
172		printf("failed to open scaling_max_freq, errno=%d\n", errno);
173		goto err;
174	}
175
176err:
177	close(fd);
178	return len;
179}
180
181static void int_exit(int sig)
182{
183	cpu_stat_inject_cpu_idle_event();
184	cpu_stat_inject_cpu_frequency_event();
185	cpu_stat_update(cstate_map_fd, pstate_map_fd);
186	cpu_stat_print();
187	exit(0);
188}
189
190int main(int argc, char **argv)
191{
192	struct bpf_link *link = NULL;
193	struct bpf_program *prog;
194	struct bpf_object *obj;
195	char filename[256];
196	int ret;
197
198	snprintf(filename, sizeof(filename), "%s_kern.o", argv[0]);
199	obj = bpf_object__open_file(filename, NULL);
200	if (libbpf_get_error(obj)) {
201		fprintf(stderr, "ERROR: opening BPF object file failed\n");
202		return 0;
203	}
204
205	prog = bpf_object__find_program_by_name(obj, "bpf_prog1");
206	if (!prog) {
207		printf("finding a prog in obj file failed\n");
208		goto cleanup;
209	}
210
211	/* load BPF program */
212	if (bpf_object__load(obj)) {
213		fprintf(stderr, "ERROR: loading BPF object file failed\n");
214		goto cleanup;
215	}
216
217	cstate_map_fd = bpf_object__find_map_fd_by_name(obj, "cstate_duration");
218	pstate_map_fd = bpf_object__find_map_fd_by_name(obj, "pstate_duration");
219	if (cstate_map_fd < 0 || pstate_map_fd < 0) {
220		fprintf(stderr, "ERROR: finding a map in obj file failed\n");
221		goto cleanup;
222	}
223
224	link = bpf_program__attach(prog);
225	if (libbpf_get_error(link)) {
226		fprintf(stderr, "ERROR: bpf_program__attach failed\n");
227		link = NULL;
228		goto cleanup;
229	}
230
231	ret = cpu_stat_inject_cpu_idle_event();
232	if (ret < 0)
233		return 1;
234
235	ret = cpu_stat_inject_cpu_frequency_event();
236	if (ret < 0)
237		return 1;
238
239	signal(SIGINT, int_exit);
240	signal(SIGTERM, int_exit);
241
242	while (1) {
243		cpu_stat_update(cstate_map_fd, pstate_map_fd);
244		cpu_stat_print();
245		sleep(5);
246	}
247
248cleanup:
249	bpf_link__destroy(link);
250	bpf_object__close(obj);
251	return 0;
252}