#include <rtl.h>
#include <rtl_sched.h>
#include <time.h>
#include <pthread.h>
#include <semaphore.h>

pthread_t thread1;
pthread_t thread2;
sem_t     semaphore;


void * start_routine_clk(void *arg)
{
    struct sched_param p;
    sem_t* sem = (sem_t*)arg;

    // Delay for 1msec (Removing this delay makes program work OK)
    p.sched_priority = 1;
    pthread_setschedparam (pthread_self(), SCHED_FIFO, &p);
    pthread_make_periodic_np (pthread_self(), gethrtime(), 1000000);
    pthread_wait_np ();
  
    // Set up 2 msec period
    p.sched_priority = 1;
    pthread_setschedparam (pthread_self(), SCHED_FIFO, &p);
    pthread_make_periodic_np (pthread_self(), gethrtime(), 2000000);
  
    while (1) 
    {
	pthread_wait_np ();
	sem_post(sem);
    }
    return 0;
}

void * start_routine_main(void *arg)
{
    int count;
    struct timespec    tp;
    struct sched_param p;
    sem_t* sem = (sem_t*)arg;

    // Delay for 1msec
    p.sched_priority = 1;
    pthread_setschedparam (pthread_self(), SCHED_FIFO, &p);
    pthread_make_periodic_np (pthread_self(), gethrtime(), 1000000);
    pthread_wait_np ();

    count = 0;
    while (1) 
    {
	sem_wait(sem);

	if (++count == 500)
	{
	    count = 0;
	    clock_gettime(CLOCK_REALTIME, &tp);
	    rtl_printf("Time = %d:%d\n", tp.tv_sec, tp.tv_nsec);
	}
    }
    return 0;
}

int init_module(void) 
{
    sem_init(&semaphore, 1, 0);

    // Reversing these two lines makes program work ok
    pthread_create(&thread2, NULL, start_routine_main, (void*)&semaphore);
    pthread_create(&thread1, NULL, start_routine_clk, (void*)&semaphore);

    return 0;
}

void cleanup_module(void) 
{
    pthread_delete_np(thread1);
    pthread_delete_np(thread2);
    sem_destroy(&semaphore);
}