// SPDX-License-Identifier: GPL-2.0-or-later
/*
 * pi_stress - Priority Inheritance stress test
 * Copyright (C) 2006, 2007 Clark Williams <williams@redhat.com>
 */

/*
 * This program stress tests pthreads priority inheritance mutexes

 * The logic is built upon the state machine that performs the "classic_pi"
 * deadlock scenario. A state machine or "inversion group" is a group of three
 * threads as described below.
 *
 * The basic premise here is to set up a deadlock scenario and confirm that PI
 * mutexes resolve the situation. Three worker threads will be created from the
 * main thread: low, medium and high priority threads that use SCHED_FIFO as
 * their scheduling policy. The low priority thread claims a mutex and then
 * starts "working". The medium priority thread starts and preempts the low
 * priority thread. Then the high priority thread runs and attempts to claim
 * the mutex owned by the low priority thread. Without priority inheritance,
 * this will deadlock the program. With priority inheritance, the low priority
 * thread receives a priority boost, finishes it's "work" and releases the mutex
 * which allows the high priority thread to run and finish and then the medium
 * priority thread finishes.
 *
 * That's the theory, anyway...
 *
 * CW - 2006
 */

#include "pi_stress.h"

/* the length of the test */
/* default is infinite */
int duration = -1;

/* times for starting and finishing the stress test */
time_t start, finish;

/* the number of groups to create */
int ngroups;

/* the number of times a group causes a priority inversion situation */
/* default to infinite */
int inversions = -1;

/* turn on lots of prints */
int verbose;

/* turn on pi_debugging prints */
int debugging;

/* turn off all prints, default = 0 (off) */
int quiet;

/* filename for JSON output */
char jsonfile[MAX_PATH];

/* prompt to start test */
int prompt;

/* report interval */
unsigned long report_interval = (unsigned long)SEC_TO_USEC(0.75);

int shutdown;			/* global indicating we should shut down */
pthread_mutex_t shutdown_mtx;	/* associated mutex */

/* indicate if errors have occurred */
int have_errors;

/* indicated that keyboard interrupt has happened */
int interrupted;

/* force running on one cpu */
int uniprocessor;

/* lock all memory */
int lockall;

pthread_barrier_t all_threads_ready;
pthread_barrier_t all_threads_done;

cpu_set_t test_cpu_mask, admin_cpu_mask;

int policy = SCHED_FIFO;

/* scheduling attributes per thread */
struct sched_attr low_sa;
struct sched_attr med_sa;
struct sched_attr high_sa;
struct sched_attr admin_sa;

unsigned int sa_initialized;

struct group_parameters *groups;

/* number of online processors */
long num_processors;

int main(int argc, char **argv)
{
	int status;
	struct sched_param thread_param;
	int i;
	int retval = FAILURE;
	int core;
	int nthreads;

	/* Make sure we see all message, even those on stdout.  */
	setvbuf(stdout, NULL, _IONBF, 0);

	/* get the number of processors */
	num_processors = sysconf(_SC_NPROCESSORS_ONLN);

	/* calculate the number of inversion groups to run */
	ngroups = num_processors == 1 ? 1 : num_processors - 1;


	/* process command line arguments */
	rt_init(argc, argv);
	process_command_line(argc, argv);

	/* set default sched attributes */
	setup_sched_config(policy);

	/* lock memory */
	if (lockall)
		if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
			pi_error("mlockall failed\n");
			return FAILURE;
		}
	/* boost main's priority (so we keep running) :) */
	thread_param.sched_priority = admin_sa.sched_priority;
	status = pthread_setschedparam(pthread_self(), admin_sa.sched_policy,
				       &thread_param);
	if (status) {
		pi_error("%s: boosting to max priority: 0x%x\n", __func__,
			 status);
		return FAILURE;
	}
	/* block unwanted signals */
	block_signals();

	/* allocate our groups array */
	groups = calloc(ngroups, sizeof(struct group_parameters));
	if (groups == NULL) {
		pi_error("%s: failed to allocate %d groups\n", __func__,
			 ngroups);
		return FAILURE;
	}
	/* set up CPU affinity masks */
	if (set_cpu_affinity(&test_cpu_mask, &admin_cpu_mask))
		return FAILURE;

	nthreads = ngroups * NUM_TEST_THREADS + NUM_ADMIN_THREADS;

	/* set up our ready barrier */
	if (barrier_init(&all_threads_ready, NULL, nthreads,
			 "all_threads_ready"))
		return FAILURE;

	/* set up our done barrier */
	if (barrier_init(&all_threads_done, NULL, nthreads, "all_threads_done"))
		return FAILURE;

	/* create the groups */
	pi_info("Creating %d test groups\n", ngroups);
	for (core = 0; core < num_processors; core++)
		if (CPU_ISSET(core, &test_cpu_mask))
			break;
	for (i = 0; i < ngroups; i++) {
		groups[i].id = i;
		groups[i].cpu = core++;
		if (core >= num_processors)
			core = 0;
		if (create_group(&groups[i]) != SUCCESS)
			return FAILURE;
	}

	/* prompt if requested */
	if (prompt) {
		printf("Press return to start test: ");
		getchar();
	}
	/* report */
	banner();
	start = time(NULL);

	/* turn loose the threads */
	pi_info("Releasing all threads\n");
	status = pthread_barrier_wait(&all_threads_ready);
	if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
		pi_error("%s: pthread_barrier_wait(all_threads_ready): 0x%x\n",
			 __func__, status);
		set_shutdown_flag();
		return FAILURE;
	}

	reporter(NULL);

	if (!quiet) {
		fputs(DOWN_ONE, stdout);
		printf("Stopping test\n");
	}
	set_shutdown_flag();

	/* wait for all threads to notice the shutdown flag */
	if (have_errors == 0 && interrupted == 0) {
		pi_info("waiting for all threads to complete\n");
		status = pthread_barrier_wait(&all_threads_done);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error("%s: pthread_barrier_wait(all_threads_ready): 0x%x\n", __func__, status);
			return FAILURE;
		}
		pi_info("All threads terminated!\n");
		retval = SUCCESS;
	} else
		kill(0, SIGTERM);
	finish = time(NULL);
	summary();

	if (strlen(jsonfile) != 0)
		rt_write_json(jsonfile, retval, write_stats, NULL);

	if (lockall)
		munlockall();
	exit(retval);
}

int
setup_thread_attr(pthread_attr_t *attr, struct sched_attr *sa,
		  cpu_set_t *mask)
{
	int status;
	struct sched_param thread_param;

	status = pthread_attr_init(attr);
	if (status) {
		pi_error
		    ("%s: initializing thread attribute: 0x%x\n", __func__,
		     status);
		return FAILURE;
	}
	status = pthread_attr_setaffinity_np(attr, sizeof(cpu_set_t), mask);
	if (status) {
		pi_error("%s: setting affinity attribute: 0x%x\n", __func__,
			 status);
		return FAILURE;
	}

	/*
	 * The pthread API does not yet support SCHED_DEADLINE, defer the
	 * thread configuration to setup_thread()
	 */
	if (sa->sched_policy == SCHED_DEADLINE)
		return SUCCESS;

	status = pthread_attr_setschedpolicy(attr, sa->sched_policy);
	if (status) {
		pi_error
		    ("%s: setting attribute policy to %s: 0x%x\n", __func__,
		     policy_to_string(sa->sched_policy),
		     status);
		return FAILURE;
	}
	status = pthread_attr_setinheritsched(attr, PTHREAD_EXPLICIT_SCHED);
	if (status) {
		pi_error
		    ("%s: setting explicit scheduling inheritance: 0x%x\n",
		     __func__, status);
		return FAILURE;
	}
	thread_param.sched_priority = sa->sched_priority;
	status = pthread_attr_setschedparam(attr, &thread_param);
	if (status) {
		pi_error("%s: setting scheduler param: 0x%x\n", __func__,
			 status);
		return FAILURE;
	}
	return SUCCESS;
}

int set_cpu_affinity(cpu_set_t *test_mask, cpu_set_t *admin_mask)
{
	int status, i, admin_proc;
	cpu_set_t current_mask;

	/* handle uniprocessor case */
	if (num_processors == 1 || uniprocessor) {
		CPU_ZERO(admin_mask);
		CPU_ZERO(test_mask);
		CPU_SET(0, admin_mask);
		CPU_SET(0, test_mask);
		pi_info("admin and test threads running on one processor\n");
		return SUCCESS;
	}
	/*
	 * first set our main thread to run on the first
	 * scheduleable processor we can find
	 */
	status = sched_getaffinity(0, sizeof(cpu_set_t), &current_mask);
	if (status) {
		pi_error("failed getting CPU affinity mask: 0x%x\n", status);
		return FAILURE;
	}
	for (i = 0; i < num_processors; i++) {
		if (CPU_ISSET(i, &current_mask))
			break;
	}
	if (i >= num_processors) {
		pi_error("No schedulable CPU found for main!\n");
		return FAILURE;
	}
	admin_proc = i;
	CPU_ZERO(admin_mask);
	CPU_SET(admin_proc, admin_mask);
	status = sched_setaffinity(0, sizeof(cpu_set_t), admin_mask);
	if (status) {
		pi_error("%s: setting CPU affinity mask: 0x%x\n", __func__,
		      status);
		return FAILURE;
	}
	pi_info("Admin thread running on processor: %d\n", i);

	/* Set test affinity so that tests run on the non-admin processors */
	CPU_ZERO(test_mask);
	for (i = admin_proc + 1; i < num_processors; i++)
		CPU_SET(i, test_mask);

	if (admin_proc + 1 == num_processors - 1)
		pi_info("Test threads running on processor: %ld\n",
		     num_processors - 1);
	else
		pi_info("Test threads running on processors:  %d-%d\n",
		     admin_proc + 1, (int)num_processors - 1);

	return SUCCESS;
}

/* clear all watchdog counters */
void watchdog_clear(void)
{
	int i;

	for (i = 0; i < ngroups; i++)
		groups[i].watchdog = 0;
}

/* check for zero watchdog counters */
int watchdog_check(void)
{
	int i;
	int failures = 0;
	struct group_parameters *g;

	for (i = 0; i < ngroups; i++) {
		g = &groups[i];
		if (g->watchdog == 0) {
			/* don't report deadlock if group is finished */
			if (g->inversions == g->total)
				continue;
			if (++g->watchdog_hits >= WATCHDOG_LIMIT) {
				pi_error
				    ("WATCHDOG triggered: group %d is deadlocked!\n",
				     i);
				failures++;
			}
		} else
			g->watchdog_hits = 0;
	}
	return failures ? FAILURE : SUCCESS;
}

int pending_interrupt(void)
{
	sigset_t pending;

	if (sigpending(&pending) < 0) {
		pi_error("from sigpending: %s\n", strerror(errno));
		return 0;
	}

	return interrupted = sigismember(&pending, SIGINT);
}

/*
 * this routine serves two purposes:
 *   1. report progress
 *   2. check for deadlocks
 */
void *reporter(void *arg)
{
	int status;
	int end = 0;
	struct timespec ts;

	ts.tv_sec = 0;
	ts.tv_nsec = USEC_TO_NSEC(report_interval);

	tsnorm(&ts);

	if (duration >= 0)
		end = duration + time(NULL);

	/* sleep initially to let everything get up and running */
	status = clock_nanosleep(CLOCK_MONOTONIC, 0, &ts, NULL);
	if (status) {
		pi_error("from clock_nanosleep: %s\n", strerror(status));
		return NULL;
	}

	pi_debug("%s: starting report loop\n", __func__);
	pi_info("Press Control-C to stop test\nCurrent Inversions:\n");

	for (;;) {
		pthread_mutex_lock(&shutdown_mtx);
		if (shutdown) {
			pthread_mutex_unlock(&shutdown_mtx);
			break;
		}
		pthread_mutex_unlock(&shutdown_mtx);

		/* wait for our reporting interval */
		status = clock_nanosleep(CLOCK_MONOTONIC, 0, &ts, NULL);
		if (status) {
			pi_error("from clock_nanosleep: %s\n", strerror(status));
			break;
		}

		/* check for signaled shutdown */
		if (!quiet) {
			pthread_mutex_lock(&shutdown_mtx);
			if (shutdown == 0) {
				fputs(UP_ONE, stdout);
				printf("Current Inversions: %lu\n",
						total_inversions());
			}
			pthread_mutex_unlock(&shutdown_mtx);
		}

		/* if we specified a duration, see if it has expired */
		if (end && time(NULL) > end) {
			pi_info("duration reached (%d seconds)\n", duration);
			set_shutdown_flag();
			continue;
		}
		/* check for a pending SIGINT */
		if (pending_interrupt()) {
			pi_info("Keyboard Interrupt!\n");
			break;
		}
		/* check watchdog stuff */
		if ((watchdog_check())) {
			pi_error("%s stopping due to watchdog event\n", __func__);
			set_shutdown_flag();
			break;
		}
		/* clear watchdog counters */
		watchdog_clear();

	}
	pi_debug("%s: finished\n", __func__);
	set_shutdown_flag();
	return NULL;
}

int verify_cpu(int cpu)
{
	int status;
	int err;
	cpu_set_t mask;

	CPU_ZERO(&mask);

	status = sched_getaffinity(0, sizeof(cpu_set_t), &mask);
	if (status == -1) {
		err = errno;
		fprintf(stderr, "sched_getaffinity %s\n", strerror(err));
		exit(1);
	}

	if (CPU_ISSET(cpu, &mask))
		return SUCCESS;
	return FAILURE;
}

void *low_priority(void *arg)
{
	int status;
	int unbounded;
	unsigned long count = 0;
	struct group_parameters *p = (struct group_parameters *)arg;
	pthread_barrier_t *loop_barr = &p->loop_barr;
	pthread_mutex_t *loop_mtx = &p->loop_mtx;
	int *loop = &p->loop;

	allow_sigterm();

	if (verify_cpu(p->cpu) != SUCCESS) {
		pi_error("low_priority[%d]: not bound to %ld\n", p->id, p->cpu);
		return NULL;
	}

	pi_debug("low_priority[%d]: entering ready state\n", p->id);
	/* wait for all threads to be ready */
	status = pthread_barrier_wait(&all_threads_ready);
	if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
		pi_error
		    ("low_priority[%d]: pthread_barrier_wait(all_threads_ready): %x",
		     p->id, status);
		return NULL;
	}

	unbounded = (p->inversions < 0);

	pi_debug("low_priority[%d]: starting inversion loop\n", p->id);

	for (;;) {
		/*
		 * We can't set the 'loop' boolean here, because some flags
		 * may have already reached the loop_barr
		 */
		if (!unbounded && (p->total >= (unsigned int)p->inversions))
			set_shutdown_flag();

		/* Either all threads go through the loop_barr, or none do */
		pthread_mutex_lock(loop_mtx);
		if (*loop == 0) {
			pthread_mutex_unlock(loop_mtx);
			break;
		}
		pthread_mutex_unlock(loop_mtx);

		status = pthread_barrier_wait(loop_barr);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error("%s[%d]: pthread_barrier_wait(loop): %x\n",
			      __func__, p->id, status);
			return NULL;
		}

		/* Only one Thread needs to check the shutdown status */
		if (status == PTHREAD_BARRIER_SERIAL_THREAD) {
			pthread_mutex_lock(&shutdown_mtx);
			if (shutdown) {
				pthread_mutex_lock(loop_mtx);
				*loop = 0;
				pthread_mutex_unlock(loop_mtx);
			}
			pthread_mutex_unlock(&shutdown_mtx);
		}

		/* initial state */
		pi_debug("low_priority[%d]: entering start wait (%ld)\n", p->id,
		      count++);
		status = pthread_barrier_wait(&p->start_barrier);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error
			    ("low_priority[%d]: pthread_barrier_wait(start): %x\n",
			     p->id, status);
			return NULL;
		}

		pi_debug("low_priority[%d]: claiming mutex\n", p->id);
		pthread_mutex_lock(&p->mutex);
		pi_debug("low_priority[%d]: mutex locked\n", p->id);

		pi_debug("low_priority[%d]: entering locked wait\n", p->id);
		status = pthread_barrier_wait(&p->locked_barrier);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error
				("low_priority[%d]: pthread_barrier_wait(locked): %x\n",
				 p->id, status);
			/* release the mutex */
			pi_debug("low_priority[%d]: unlocking mutex\n", p->id);
			pthread_mutex_unlock(&p->mutex);
			return NULL;
		}

		/* wait for priority boost */
		pi_debug("low_priority[%d]: entering elevated wait\n", p->id);
		status = pthread_barrier_wait(&p->elevate_barrier);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error
				("low_priority[%d]: pthread_barrier_wait(elevate): %x\n",
				 p->id, status);
			/* release the mutex */
			pi_debug("low_priority[%d]: unlocking mutex\n", p->id);
			pthread_mutex_unlock(&p->mutex);
			return NULL;
		}

		/* release the mutex */
		pi_debug("low_priority[%d]: unlocking mutex\n", p->id);
		pthread_mutex_unlock(&p->mutex);

		/* finish state */
		pi_debug("low_priority[%d]: entering finish wait\n", p->id);
		status = pthread_barrier_wait(&p->finish_barrier);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error
			    ("low_priority[%d]: pthread_barrier_wait(elevate): %x\n",
			     p->id, status);
			return NULL;
		}
	}
	set_shutdown_flag();
	pi_debug("low_priority[%d]: entering done barrier\n", p->id);
	/* wait for all threads to finish */
	status = pthread_barrier_wait(&all_threads_done);
	if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
		pi_error
		    ("low_priority[%d]: pthread_barrier_wait(all_threads_done): %x",
		     p->id, status);
		return NULL;
	}
	pi_debug("low_priority[%d]: exiting\n", p->id);
	return NULL;
}

void *med_priority(void *arg)
{
	int status;
	int unbounded;
	unsigned long count = 0;
	struct group_parameters *p = (struct group_parameters *)arg;
	pthread_barrier_t *loop_barr = &p->loop_barr;
	pthread_mutex_t *loop_mtx = &p->loop_mtx;
	int *loop = &p->loop;

	allow_sigterm();

	if (verify_cpu(p->cpu) != SUCCESS) {
		pi_error("med_priority[%d]: not bound to %ld\n", p->id, p->cpu);
		return NULL;
	}

	pi_debug("med_priority[%d]: entering ready state\n", p->id);
	/* wait for all threads to be ready */
	status = pthread_barrier_wait(&all_threads_ready);
	if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
		pi_error
		    ("med_priority[%d]: pthread_barrier_wait(all_threads_ready): %x",
		     p->id, status);
		return NULL;
	}

	unbounded = (p->inversions < 0);

	pi_debug("med_priority[%d]: starting inversion loop\n", p->id);
	for (;;) {
		if (!unbounded && (p->total >= (unsigned int)p->inversions))
			set_shutdown_flag();

		/* Either all threads go through the loop_barr, or none do */
		pthread_mutex_lock(loop_mtx);
		if (*loop == 0) {
			pthread_mutex_unlock(loop_mtx);
			break;
		}
		pthread_mutex_unlock(loop_mtx);

		status = pthread_barrier_wait(loop_barr);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error("%s[%d]: pthread_barrier_wait(loop): %x\n",
			      __func__, p->id, status);
			return NULL;
		}

		/* Only one Thread needs to check the shutdown status */
		if (status == PTHREAD_BARRIER_SERIAL_THREAD) {
			pthread_mutex_lock(&shutdown_mtx);
			if (shutdown) {
				pthread_mutex_lock(loop_mtx);
				*loop = 0;
				pthread_mutex_unlock(loop_mtx);
			}
			pthread_mutex_unlock(&shutdown_mtx);
		}

		/* start state */
		pi_debug("med_priority[%d]: entering start state (%ld)\n", p->id,
		      count++);
		status = pthread_barrier_wait(&p->start_barrier);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error
			    ("med_priority[%d]: pthread_barrier_wait(start): %x",
			     p->id, status);
			return NULL;
		}
		pi_debug("med_priority[%d]: entering elevate state\n", p->id);
		status = pthread_barrier_wait(&p->elevate_barrier);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error ("med_priority[%d]: pthread_barrier_wait(elevate): %x", p->id, status);
			return NULL;
		}

		pi_debug("med_priority[%d]: entering finish state\n", p->id);
		status = pthread_barrier_wait(&p->finish_barrier);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error
			    ("med_priority[%d]: pthread_barrier_wait(finished): %x",
			     p->id, status);
			return NULL;
		}
	}
	set_shutdown_flag();

	pi_debug("med_priority[%d]: entering done barrier\n", p->id);
	/* wait for all threads to finish */
	if (have_errors == 0) {
		status = pthread_barrier_wait(&all_threads_done);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error
			    ("med_priority[%d]: pthread_barrier_wait(all_threads_done): %x",
			     p->id, status);
			return NULL;
		}
	}
	/* exit */
	pi_debug("med_priority[%d]: exiting\n", p->id);
	return NULL;
}

void *high_priority(void *arg)
{
	int status;
	int unbounded;
	unsigned long count = 0;
	struct group_parameters *p = (struct group_parameters *)arg;
	pthread_barrier_t *loop_barr = &p->loop_barr;
	pthread_mutex_t *loop_mtx = &p->loop_mtx;
	int *loop = &p->loop;
	cpu_set_t cpu_mask;
	int i;

	if (high_sa.sched_policy == SCHED_DEADLINE) {
		CPU_ZERO(&cpu_mask);
		for (i = 0; i < num_processors; i++)
			CPU_SET(i, &cpu_mask);
		status = sched_setaffinity(0, sizeof(cpu_set_t), &cpu_mask);
		if (status < 0) {
			pi_error
			    ("high_priority[%d]: set cpu affinity*dl): %x\n",
			    p->id, status);
			return NULL;
		}

		status = sched_setattr(gettid(), &high_sa, 0);
		if (status < 0) {
			pi_error
			    ("high_priority[%d]: sched_setattr(dl): %x\n",
			    p->id, status);
			return NULL;
		}
	}

	allow_sigterm();
	if (verify_cpu(p->cpu) != SUCCESS) {
		pi_error("high_priority[%d]: not bound to %ld\n", p->id, p->cpu);
		return NULL;
	}

	pi_debug("high_priority[%d]: entering ready state\n", p->id);

	/* wait for all threads to be ready */
	status = pthread_barrier_wait(&all_threads_ready);
	if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
		pi_error
		    ("high_priority[%d]: pthread_barrier_wait(all_threads_ready): %x",
		     p->id, status);
		return NULL;
	}
	unbounded = (p->inversions < 0);
	pi_debug("high_priority[%d]: starting inversion loop\n", p->id);
	for (;;) {
		if (!unbounded && (p->total >= (unsigned int)p->inversions)) {
			set_shutdown_flag();
		}

		/* Either all threads go through the loop_barr, or none do */
		pthread_mutex_lock(loop_mtx);
		if (*loop == 0) {
			pthread_mutex_unlock(loop_mtx);
			break;
		}
		pthread_mutex_unlock(loop_mtx);

		status = pthread_barrier_wait(loop_barr);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error("%s[%d]: pthread_barrier_wait(loop): %x\n",
			      __func__, p->id, status);
			return NULL;
		}

		/* Only one Thread needs to check the shutdown status */
		if (status == PTHREAD_BARRIER_SERIAL_THREAD) {
			pthread_mutex_lock(&shutdown_mtx);
			if (shutdown) {
				pthread_mutex_lock(loop_mtx);
				*loop = 0;
				pthread_mutex_unlock(loop_mtx);
			}
			pthread_mutex_unlock(&shutdown_mtx);
		}
		pi_debug("high_priority[%d]: entering start state (%d)\n", p->id,
		      count++);
		status = pthread_barrier_wait(&p->start_barrier);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error
			    ("high_priority[%d]: pthread_barrier_wait(start): %x",
			     p->id, status);
			return NULL;
		}

		pi_debug("high_priority[%d]: entering running state\n", p->id);
		status = pthread_barrier_wait(&p->locked_barrier);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error
			    ("high_priority[%d]: pthread_barrier_wait(running): %x",
			     p->id, status);
			return NULL;
		}
		pi_debug("high_priority[%d]: locking mutex\n", p->id);
		pthread_mutex_lock(&p->mutex);
		pi_debug("high_priority[%d]: got mutex\n", p->id);

		pi_debug("high_priority[%d]: unlocking mutex\n", p->id);
		pthread_mutex_unlock(&p->mutex);
		pi_debug("high_priority[%d]: entering finish state\n", p->id);

		status = pthread_barrier_wait(&p->finish_barrier);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error
			    ("high_priority[%d]: pthread_barrier_wait(finish): %x", p->id, status);
			return NULL;
		}
		/* update the group stats */
		p->total++;

		/* update the watchdog counter */
		p->watchdog++;

	}
	set_shutdown_flag();

	pi_debug("high_priority[%d]: entering done barrier\n", p->id);

	if (have_errors == 0) {
		/* wait for all threads to finish */
		status = pthread_barrier_wait(&all_threads_done);
		if (status && status != PTHREAD_BARRIER_SERIAL_THREAD) {
			pi_error
			    ("high_priority[%d]: pthread_barrier_wait(all_threads_done): %x",
			     p->id, status);
			return NULL;
		}
	}
	/* exit */
	pi_debug("high_priority[%d]: exiting\n", p->id);
	return NULL;
}

void usage(int error)
{
	printf("pi_stress V %1.2f\n", VERSION);
	printf("Usage:\n"
	       "pi_stress <options>\n\n"
	       "-d       --debug           turn on debug prints\n"
	       "-D TIME  --duration=TIME   length of test run in seconds (default is infinite)\n"
	       "                           Append 'm', 'h', or 'd'\n"
	       "                           to specify minutes, hours or days.\n"
	       "-g N     --groups=N        set the number of inversion groups\n"
	       "-h       --help            print this message\n"
	       "-i INV   --inversions=INV  number of inversions per group (default is infinite)\n"
	       "         --json=FILENAME   write final results into FILENAME, JSON formatted\n"
	       "-m       --mlockall        lock current and future memory\n"
	       "-p       --prompt          prompt before starting the test\n"
	       "-q       --quiet           suppress running output\n"
	       "-r       --rr              use SCHED_RR for test threads [SCHED_FIFO]\n"
	       "-s OPTS  --sched OPTS      scheduling options per thread type:\n"
	       "   id=[high|med|low]      select thread\n"
	       "   ,policy=[fifo,rr]       scheduling class [SCHED_FIFO, SCHED_RR]\n"
	       "     ,priority=N           scheduling priority\n"
	       "   ,policy=[deadline]      scheduling class [DEADLINE]\n"
               "     ,runtime=N\n"
	       "     ,deadline=N\n"
	       "     ,period=N\n"
	       "-u       --uniprocessor    force all threads to run on one processor\n"
	       "-v       --verbose         lots of output\n"
	       "-V       --version         print version number on output\n"
	       );
	exit(error);
}

/* block all signals (called from main) */
int block_signals(void)
{
	int status;
	sigset_t sigset;

	/* mask off all signals */
	status = sigfillset(&sigset);
	if (status) {
		pi_error("setting up full signal set %s\n", strerror(status));
		return FAILURE;
	}
	status = pthread_sigmask(SIG_BLOCK, &sigset, NULL);
	if (status) {
		pi_error("setting signal mask: %s\n", strerror(status));
		return FAILURE;
	}
	return SUCCESS;
}

/* allow SIGTERM delivery (called from worker threads) */
int allow_sigterm(void)
{
	int status;
	sigset_t sigset;

	status = sigemptyset(&sigset);
	if (status) {
		pi_error("creating empty signal set: %s\n", strerror(status));
		return FAILURE;
	}
	status = sigaddset(&sigset, SIGTERM);
	if (status) {
		pi_error("adding SIGTERM to signal set: %s\n", strerror(status));
		return FAILURE;
	}
	status = pthread_sigmask(SIG_UNBLOCK, &sigset, NULL);
	if (status) {
		pi_error("unblocking SIGTERM: %s\n", strerror(status));
		return FAILURE;
	}
	return SUCCESS;
}

/* clean up before exiting */
void set_shutdown_flag(void)
{
	pthread_mutex_lock(&shutdown_mtx);
	if (shutdown == 0) {
		/* tell anyone that's looking that we're done */
		pi_info("setting shutdown flag\n");
		shutdown = 1;
	}
	pthread_mutex_unlock(&shutdown_mtx);
}

/* set up a test group */
int initialize_group(struct group_parameters *group)
{
	int status;
	pthread_mutexattr_t mutex_attr;

	group->inversions = inversions;

	/* setup default attributes for the group mutex */
	/* (make it a PI mutex) */
	status = pthread_mutexattr_init(&mutex_attr);
	if (status) {
		pi_error("initializing mutex attribute: %s\n", strerror(status));
		return FAILURE;
	}

	/* set priority inheritance attribute for mutex */
	status = pthread_mutexattr_setprotocol(&mutex_attr,
					       PTHREAD_PRIO_INHERIT);
	if (status) {
		pi_error("setting mutex attribute policy: %s\n", strerror(status));
		return FAILURE;
	}
	/* initialize the group mutex */
	status = pthread_mutex_init(&group->mutex, &mutex_attr);
	if (status) {
		pi_error("initializing mutex: %s\n", strerror(status));
		return FAILURE;
	}

	/* initialize the group barriers */
	if (barrier_init(&group->start_barrier, NULL, NUM_TEST_THREADS,
			 "start_barrier"))
		return FAILURE;

	if (barrier_init(&group->locked_barrier, NULL, 2, "locked_barrier"))
		return FAILURE;

	if (barrier_init(&group->elevate_barrier, NULL, 2, "elevate_barrier"))
		return FAILURE;

	if (barrier_init
	    (&group->finish_barrier, NULL, NUM_TEST_THREADS, "finish_barrier"))
		return FAILURE;

	if (barrier_init(&group->loop_barr, NULL, NUM_TEST_THREADS,
			 "loop_barrier"))
		return FAILURE;

	status = pthread_mutex_init(&group->loop_mtx, NULL);
	if (status) {
		pi_error("pthread_mutex_init, status = %d\n", status);
		return FAILURE;
	}

	status = pthread_mutex_lock(&group->loop_mtx);
	if (status) {
		pi_error("pthread_mutex_lock, status = %d\n", status);
		return FAILURE;
	}

	group->loop = 1;

	status = pthread_mutex_unlock(&group->loop_mtx);
	if (status) {
		pi_error("pthread_mutex_unlock, status = %d\n", status);
		return FAILURE;
	}

	return SUCCESS;
}

/* setup and create a groups threads */
int create_group(struct group_parameters *group)
{
	int status;
	pthread_attr_t thread_attr;
	cpu_set_t mask;

	/* initialize group structure */
	status = initialize_group(group);
	if (status) {
		pi_error("initializing group %d\n", group->id);
		return FAILURE;
	}

	CPU_ZERO(&mask);
	CPU_SET(group->cpu, &mask);

	pi_debug("group %d bound to cpu %ld\n", group->id, group->cpu);

	/* start the low priority thread */
	pi_debug("creating low priority thread\n");
	if (setup_thread_attr(&thread_attr, &low_sa, &mask))
		return FAILURE;
	status = pthread_create(&group->low_tid,
				&thread_attr, low_priority, group);
	if (status != 0) {
		pi_error("creating low_priority thread: %s\n", strerror(status));
		return FAILURE;
	}

	/* create the medium priority thread */
	pi_debug("creating medium priority thread\n");
	if (setup_thread_attr(&thread_attr, &med_sa, &mask))
		return FAILURE;
	status = pthread_create(&group->med_tid,
				&thread_attr, med_priority, group);
	if (status != 0) {
		pi_error("creating med_priority thread: %s\n", strerror(status));
		return FAILURE;
	}

	/* create the high priority thread */
	pi_debug("creating high priority thread\n");
	if (setup_thread_attr(&thread_attr, &high_sa, &mask))
		return FAILURE;
	status = pthread_create(&group->high_tid,
				&thread_attr, high_priority, group);
	if (status != 0) {
		pi_error("creating high_priority thread: %s\n", strerror(status));
		set_shutdown_flag();
		return FAILURE;
	}
	return SUCCESS;
}

unsigned long parse_unsigned(const char *str)
{
	unsigned long n;
	char *p;

	errno = 0;
	n = strtoul(str, &p, 10);

	if ((errno == ERANGE && n == ULONG_MAX)
			|| (errno != 0 && n == 0)) {
		pi_error("parsing number failed: %s\n", str);
		exit(EXIT_FAILURE);
	}

	return n;
}

long parse_signed(const char *str)
{
	long n;
	char *p;

	errno = 0;
	n = strtol(str, &p, 10);

	if ((errno == ERANGE && (n == LONG_MAX || n == LONG_MIN))
			|| (errno != 0 && n == 0)) {
		pi_error("parsing number failed: %s\n", str);
		exit(EXIT_FAILURE);
	}

	return n;
}

int process_sched_line(const char *arg)
{
	char *buf, *k, *v;
	const char del[] = ",=";
	struct sched_attr sa = { 0, };
	char *id = NULL;
	int retval = SUCCESS;

	buf = strdupa(arg);

	k = strsep(&buf, del);
	while (k) {
		v = strsep(&buf, del);
		if (!v)
			break;

		if (!strcmp(k, "id"))
			id = v;
		else if (!strcmp(k, "policy"))
			sa.sched_policy = string_to_policy(v);
		else if (!strcmp(k, "nice"))
			sa.sched_nice = parse_signed(v);
		else if (!strcmp(k, "priority"))
			sa.sched_priority = parse_unsigned(v);
		else if (!strcmp(k, "runtime"))
			sa.sched_runtime = parse_unsigned(v);
		else if (!strcmp(k, "deadline"))
			sa.sched_deadline = parse_unsigned(v);
		else if (!strcmp(k, "period"))
			sa.sched_period = parse_unsigned(v);

		k = strsep(&buf, del);
	}

	if (!id)
		return FAILURE;

	/*
	 * We do not validate the options, instead we pass all garbage
	 * to the kernel and see what's happening
	 */

	if (!strcmp(id, "low")) {
		memcpy(&low_sa, &sa, sizeof(struct sched_attr));
		sa_initialized |= SA_INIT_LOW;
	} else if (!strcmp(id, "med")) {
		memcpy(&med_sa, &sa, sizeof(struct sched_attr));
		sa_initialized |= SA_INIT_MED;
	} else if (!strcmp(id, "high")) {
		memcpy(&high_sa, &sa, sizeof(struct sched_attr));
		sa_initialized |= SA_INIT_HIGH;
	} else {
		retval = FAILURE;
	}

	return retval;
}

enum option_values {
	OPT_DEBUG=1, OPT_DURATION, OPT_GROUPS, OPT_HELP, OPT_INVERSIONS,
	OPT_JSON, OPT_MLOCKALL, OPT_PROMPT, OPT_QUIET, OPT_RR, OPT_SCHED,
	OPT_UNIPROCESSOR, OPT_VERBOSE, OPT_VERSION,
};

void process_command_line(int argc, char **argv)
{
	for (;;) {
		struct option options[] = {
			{"debug",		no_argument,		NULL, OPT_DEBUG},
			{"duration",		required_argument,	NULL, OPT_DURATION},
			{"groups",		required_argument,	NULL, OPT_GROUPS},
			{"help",		no_argument,		NULL, OPT_HELP},
			{"inversions",		required_argument,	NULL, OPT_INVERSIONS},
			{"json",		required_argument,	NULL, OPT_JSON},
			{"mlockall",		no_argument,		NULL, OPT_MLOCKALL},
			{"prompt",		no_argument,		NULL, OPT_PROMPT},
			{"quiet",		no_argument,		NULL, OPT_QUIET},
			{"rr",			no_argument,		NULL, OPT_RR},
			{"sched",		required_argument,	NULL, OPT_SCHED},
			{"uniprocessor",	no_argument,		NULL, OPT_UNIPROCESSOR},
			{"verbose",		no_argument,		NULL, OPT_VERBOSE},
			{"version",		no_argument,		NULL, OPT_VERSION},
			{NULL, 0, NULL, 0},
		};

		int c = getopt_long(argc, argv, "+hD:vqi:g:rs:pdVum", options, NULL);
		if (c == -1)
			break;
		switch (c) {
		case OPT_DEBUG:
		case 'd':
			debugging = 1;
			break;
		case OPT_DURATION:
		case 'D':
			duration = parse_time_string(optarg);
			break;
		case OPT_GROUPS:
		case 'g':
			ngroups = strtol(optarg, NULL, 10);
			if (ngroups > num_processors) {
				pi_error("the number of groups cannot exceed the number of online processors (%ld)\n", num_processors);
				exit(-1);
			}
			pi_info("number of groups set to %d\n", ngroups);
			break;
		case OPT_HELP:
		case 'h':
			usage(0);
			break;
		case OPT_INVERSIONS:
		case 'i':
			inversions = strtol(optarg, NULL, 10);
			pi_info("doing %d inversion per group\n", inversions);
			break;
		case OPT_JSON:
			strncpy(jsonfile, optarg, strnlen(optarg, MAX_PATH-1));
			break;
		case OPT_MLOCKALL:
		case 'm':
			lockall = 1;
			break;
		case OPT_PROMPT:
		case 'p':
			prompt = 1;
			break;
		case OPT_QUIET:
		case 'q':
			verbose = 0;
			quiet = 1;
			break;
		case OPT_RR:
		case 'r':
			policy = SCHED_RR;
			break;
		case OPT_SCHED:
		case 's':
			if (process_sched_line(optarg))
				pi_error("ignoring invalid options '%s'\n", optarg);
			break;
		case OPT_UNIPROCESSOR:
		case 'u':
			uniprocessor = 1;
			break;
		case OPT_VERBOSE:
		case 'v':
			verbose = 1;
			quiet = 0;
			break;
		case OPT_VERSION:
		case 'V':
			printf("pi_stress v%1.2f ", VERSION);
			exit(0);
		default:
			usage(1);
			break;
		}
	}
}

/* total the number of inversions that have been performed */
unsigned long total_inversions(void)
{
	int i;
	unsigned long total = 0;

	for (i = 0; i < ngroups; i++)
		total += groups[i].total;
	return total;
}

void print_sched_attr(const char *name, struct sched_attr *sa)
{
	printf("    %6s thread", name);
	printf(" %s", policy_to_string(sa->sched_policy));

	switch (sa->sched_policy) {
	case SCHED_OTHER:
		printf(" nice %d\n", sa->sched_nice);
		break;
	case SCHED_FIFO:
	case SCHED_RR:
		printf(" priority %d\n", sa->sched_priority);
		break;
	case SCHED_DEADLINE:
		printf(" runtime %llu deadline %llu period %llu\n",
			sa->sched_runtime, sa->sched_deadline,
			sa->sched_period);
		break;
	}
}

void banner(void)
{
	if (quiet)
		return;

	printf("Starting PI Stress Test\n");
	printf("Number of thread groups: %d\n", ngroups);
	if (duration >= 0)
		printf("Duration of test run: %d seconds\n", duration);
	else
		printf("Duration of test run: infinite\n");
	if (inversions < 0)
		printf("Number of inversions per group: unlimited\n");
	else
		printf("Number of inversions per group: %d\n", inversions);
	print_sched_attr("Admin", &admin_sa);
	printf("%d groups of 3 threads will be created\n", ngroups);
	print_sched_attr("High", &high_sa);
	print_sched_attr("Med", &med_sa);
	print_sched_attr("Low", &low_sa);
	printf("\n");
}

void summary(void)
{
	time_t interval = finish - start;
	struct tm *t = gmtime(&interval);

	printf("Total inversion performed: %lu\n", total_inversions());
	printf("Test Duration: %d days, %d hours, %d minutes, %d seconds\n",
	       t->tm_yday, t->tm_hour, t->tm_min, t->tm_sec);
}

void write_stats(FILE *f, void *data)
{
	fprintf(f, "  \"inversion\": %lu\n", total_inversions());
}

int
barrier_init(pthread_barrier_t *b, const pthread_barrierattr_t *attr,
	     unsigned int count, const char *name)
{
	int status;

	status = pthread_barrier_init(b, attr, count);
	if (status != 0) {
		pi_error("barrier_init: failed to initialize: %s\n", name);
		pi_error("status = %d\n", status);
		return FAILURE;
	}

	return SUCCESS;
}

void setup_sched_attr(struct sched_attr *attr, int policy, int prio)
{
	attr->sched_policy = policy;
	attr->sched_priority = prio;
}

void setup_sched_config(int policy)
{
	int prio_min;

	prio_min = sched_get_priority_min(policy);

	if (!(sa_initialized & SA_INIT_LOW))
		setup_sched_attr(&low_sa,   policy, prio_min + 0);
	if (!(sa_initialized & SA_INIT_MED))
		setup_sched_attr(&med_sa,   policy, prio_min + 1);
	if (!(sa_initialized & SA_INIT_HIGH))
		setup_sched_attr(&high_sa,  policy, prio_min + 2);
	if (!(sa_initialized & SA_INIT_ADMIN))
		setup_sched_attr(&admin_sa, policy, prio_min + 3);
}
