#include <stdio.h>
#include <pthread.h>
#include <signal.h>
#include <sys/mman.h>

static unsigned long count;

static pthread_t id_low;
static pthread_t id_high;

#define LOW_PRIO 5
#define HIGH_PRIO 10

void init_task(int prio);

void* low_prio_task(void* dummy)
{
        struct sched_param  param;
        int policy;

        pthread_getschedparam(pthread_self(), &policy, &param);
        printf("Starting %s: policy=%d prio=%d\n",
               __FUNCTION__, policy, param.sched_priority);

	/* Switch back to primary mode */
        pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);

        while (1) {
#if 0
		pthread_set_mode_np(0, PTHREAD_PRIMARY);
#endif
		count++;
        }
        printf("Exiting %s, count=%lu\n", __FUNCTION__, count);
        return 0;
}

void* high_prio_task(void* dummy)
{
        struct timespec tstart = { .tv_sec = 0, .tv_nsec = 0};
#if 0
        struct timespec tperiod = { .tv_sec = 0, .tv_nsec = 10000000 };
#else
        struct timespec tperiod = { .tv_sec = 0, .tv_nsec = 1000000 };
#endif
        unsigned long overruns = 0;
        int tick = 0, step = 0, loop = 0, err;
        struct sched_param  param = { .sched_priority = HIGH_PRIO };
        int policy;

        pthread_getschedparam(pthread_self(), &policy, &param);
        printf("Starting %s: policy=%d prio=%d\n",
               __FUNCTION__, policy, param.sched_priority);

        err = pthread_make_periodic_np(pthread_self(), &tstart, &tperiod);
        if (err) {
		perror("pthread_make_periodic_np");
		return (void *)err;
	}

        while (1) {
                err = pthread_wait_np(&overruns);
#if 0
                if (err) {
                        err = pthread_kill(id_low, SIGUSR1);
			if (err) {
				perror("pthread_kill");
				break;
			}
                        printf("%s overruns=%lu, tick=%d, count=%lu, err=%d\n",
                               __FUNCTION__, overruns, tick, count, err);
                        loop = 0;
                        step = 2;
                }
#endif
                if  (step == 0 && loop >= 100) {
                        loop = 0;
                        step = 1;
                        init_task(LOW_PRIO);
                }
                else if (step == 1 && loop >= 500) {
                        loop = 0;
                        step = 2;
                        err = pthread_kill(id_low, SIGUSR1);
			if (err) {
				perror("pthread_kill");
				break;
			}
                        printf("SIGUSER1 to id_low: count=%lu, overruns=%lu\n",
                               count, overruns);
                }
                else if (step == 2 && loop >= 500) {
			loop = 0;
			step = 1;
                        printf("SIGUSER2 to id_low: count=%lu, overruns=%lu\n",
                               count, overruns);
			err = pthread_kill(id_low, SIGUSR2);
			if (err) {
				perror("pthread_kill");
				break;
			}
		}
                tick++;
                loop++;
        }
        printf("Exiting %s\n", __FUNCTION__);
	return 0;
}

void init_task(int prio)
{
	struct sched_param parm;
	pthread_attr_t attr;

	pthread_attr_init(&attr);
	pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
	if (prio == HIGH_PRIO)
		pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
	else
		pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
	pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
	parm.sched_priority = prio;
	pthread_attr_setschedparam(&attr, &parm);

	if (prio == HIGH_PRIO) {
		pthread_create(&id_high, &attr, high_prio_task, NULL);
		pthread_set_name_np(id_high,"high_prio");
	} else {
		pthread_create(&id_low, &attr, low_prio_task, NULL);
		pthread_set_name_np(id_low,"low_prio");
	}
}

static void suspend_signal_handler(int sig)
{
	sigset_t signal_set;

	printf("suspend signal handler\n");
	sigfillset (&signal_set);
	sigdelset (&signal_set, SIGUSR2);
	sigsuspend(&signal_set);

	return;
}

static void resume_signal_handler(int sig)
{
	printf("resume signal handler\n");
	return;
}


void init_signals(sigset_t *mask)
{
	struct sigaction sigusr1, sigusr2;

	sigemptyset(mask);
	sigaddset(mask, SIGINT);
	sigaddset(mask, SIGTERM);
	sigaddset(mask, SIGHUP);
	sigaddset(mask, SIGALRM);

	pthread_sigmask(SIG_BLOCK, mask, NULL);

	/*
	 * Install the signal handlers for suspend/resume.
	 */
	sigemptyset (&sigusr1.sa_mask);
	sigusr1.sa_flags = 0;
	sigusr1.sa_handler = suspend_signal_handler;
	sigaction(SIGUSR1, &sigusr1, NULL);

	sigemptyset (&sigusr2.sa_mask);
	sigusr2.sa_flags = 0;
	sigusr2.sa_handler = resume_signal_handler;
	sigaction(SIGUSR2, &sigusr2, NULL);

}

int main(int argc, char *argv[])
{
	sigset_t mask;
	int sig;

        mlockall(MCL_CURRENT|MCL_FUTURE);

	init_signals(&mask);

        init_task(HIGH_PRIO);

	sigwait(&mask, &sig);

        printf("Exiting %s\n", __FUNCTION__);
        return 0;
}