#define _GNU_SOURCE /* for cpu sets */

#include <sys/time.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <time.h>

#include <sched.h>

static int migrate_to(int target_cpu)
{
	cpu_set_t cpu_set;

	CPU_ZERO(&cpu_set);
	CPU_SET(target_cpu, &cpu_set);
	return sched_setaffinity(0, sizeof(cpu_set_t), &cpu_set);
}

static int become_fifo_task(void)
{
	struct sched_param sp;
	sp.sched_priority = sched_get_priority_max(SCHED_FIFO);
	return sched_setscheduler(0, SCHED_FIFO, &sp);
}

static int suspend(double seconds)
{
	struct timespec delay;
	delay.tv_sec = (time_t) seconds;
	delay.tv_nsec = (long) ((seconds - delay.tv_sec) * 1E9);
	return nanosleep(&delay, NULL);
}

static double cputime(void)
{
	struct timespec ts;
	int err;
	err = clock_gettime(CLOCK_THREAD_CPUTIME_ID, &ts);
	if (err != 0)
		perror("clock_gettime");
	return (ts.tv_sec + 1E-9 * ts.tv_nsec);
}

static double wctime(void)
{
	struct timeval tv;
	gettimeofday(&tv, NULL);
	return (tv.tv_sec + 1E-6 * tv.tv_usec);
}

static void waste_time(double seconds)
{
	double start = cputime();

	while (start + seconds > cputime())
		;
}


int main(int argc, char** argv)
{
	int i = 1, cpu;
	double start, end;
	double delay;

	if (argc != 3) {
		fprintf(stderr, "usage: loop <cpu> <loop length (seconds)>\n");
		return 1;
	} else {
		cpu   = atoi(argv[1]);
		delay = atof(argv[2]);
	}

	if (migrate_to(cpu) != 0) {
		perror("migrate_to");
		return 1;
	}

	if (delay <= 0) {
		fprintf(stderr, "bad delay: must exceed 0 seconds\n");
		return 1;
	}

	if (become_fifo_task() != 0)
		fprintf(stderr, "cold not become SCHED_FIFO task (%m)\n");

	while (1) {
		start = wctime();
		waste_time(delay);
		end   = wctime();
		printf("%5d on P%d: loop %d took %.2f seconds \n",
		       getpid(), cpu, i++, end - start);
		suspend(delay * 4); /* about 20% utilization */
	}
	return 0;
}