greg@454: #include <stdlib.h>
greg@454: #include <unistd.h>
greg@454: #include <sys/mman.h>
greg@454: 
greg@454: #include <sys/poll.h>
greg@454: #include <rtai_lxrt.h>
greg@454: 
greg@454: #include <rtai_sem.h>
greg@454: #include <pthread.h>
greg@454: #include <errno.h>
greg@454: 
greg@454: #include "applicfg.h"
greg@454: #include "can_driver.h"
Edouard@801: #include "timers.h"
greg@454: 
greg@454: #define TIMERLOOP_TASK_CREATED        1
greg@454: 
greg@454: TimerCallback_t exitall;
greg@454: 
greg@454: SEM *CanFestival_mutex;
greg@454: SEM *condition_mutex;
greg@454: SEM *control_task; 
greg@454: CND *timer_set;
greg@454: 
greg@454: // realtime task structures
greg@454: RT_TASK *timerloop_task;
greg@454: RT_TASK *Main_Task;
greg@454: 
greg@454: // linux threads id's
greg@454: static pthread_t timerloop_thr;
greg@454:  
greg@454: RTIME last_time_read;
greg@454: RTIME last_occured_alarm;
greg@454: RTIME last_timeout_set;
greg@454: 
greg@454: int stop_timer = 0;
greg@454: 
greg@454: void TimerInit(void)
greg@454: {
greg@454:     /* Init Main Task */
greg@454:     if (!(Main_Task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1))) {
greg@454:             printf("CANNOT INIT MAIN TASK\n");
greg@454:             exit(1);
greg@454:     }
greg@454:   	
greg@454:   	/* Init Mutex */
greg@454: 	CanFestival_mutex = rt_sem_init(rt_get_name(0), 1);
greg@454: 	condition_mutex = rt_typed_sem_init(rt_get_name(0), 1, RES_SEM);
greg@454: 	timer_set = rt_cond_init(rt_get_name(0));
greg@454: 	control_task = rt_sem_init(rt_get_name(0), 0);
greg@454: 	/* Set timer mode and start timer */
greg@454: 	rt_set_oneshot_mode();
greg@454: 	start_rt_timer(0);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Stop Timer Task
greg@454:  * @param exitfunction
greg@454:  */
greg@454: void StopTimerLoop(TimerCallback_t exitfunction)
greg@454: {
greg@454: 	exitall = exitfunction;
greg@454: 	stop_timer = 1;
greg@454: 	rt_cond_signal(timer_set);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Clean all Semaphores and main task
greg@454:  */
greg@454: void TimerCleanup(void)
greg@454: {
greg@454: 	/* Stop timer */
greg@454: 	stop_rt_timer();
greg@454: 
greg@454: 	/* Delete all mutex and the main task */
greg@454: 	rt_sem_delete(CanFestival_mutex);
greg@454: 	rt_sem_delete(condition_mutex);
greg@454: 	rt_sem_delete(timer_set);
greg@454: 	rt_sem_delete(control_task);
greg@454: 	rt_thread_delete(Main_Task);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Take a semaphore
greg@454:  */
greg@454: void EnterMutex(void)
greg@454: {
greg@454: 	rt_sem_wait(CanFestival_mutex);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Signaling a semaphore
greg@454:  */
greg@454: void LeaveMutex(void)
greg@454: {
greg@454: 	rt_sem_signal(CanFestival_mutex);
greg@454: }
greg@454: 
greg@454: static TimerCallback_t init_callback;
greg@454: 
greg@454: /**
greg@454:  * Timer Task
greg@454:  */
greg@454: void timerloop_task_proc(void *arg)
greg@454: {
greg@454: 	int ret = 0;
greg@454:   	// lock process in to RAM
greg@454:   	mlockall(MCL_CURRENT | MCL_FUTURE);
greg@454: 	timerloop_task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1);
greg@454: 	rt_make_hard_real_time();
greg@454: 
greg@454: 	getElapsedTime();
greg@454: 	last_timeout_set = 0;
greg@454: 	last_occured_alarm = last_time_read;
greg@454: 
greg@454: 	/* trigger first alarm */
greg@454: 	SetAlarm(NULL, 0, init_callback, 0, 0);
greg@454: 	
greg@454: 	do{
greg@454: 		RTIME real_alarm;
greg@454: 		rt_sem_wait(condition_mutex);
greg@454: 		if(last_timeout_set == TIMEVAL_MAX)
greg@454: 		{
greg@454: 			ret = rt_cond_wait(
greg@454: 				timer_set,
greg@454: 				condition_mutex);		/* Then sleep until next message*/
greg@454:  
greg@454: 			rt_sem_signal(condition_mutex);
greg@454: 		}else{
greg@454: 			real_alarm = last_time_read + last_timeout_set;
greg@454: 			ret = rt_cond_wait_until(
greg@454: 				timer_set,
greg@454: 				condition_mutex,
greg@454: 				real_alarm); /* Else, sleep until next deadline */
greg@454: 			if(ret == SEM_TIMOUT){
greg@454: 				last_occured_alarm = real_alarm;
greg@454: 				rt_sem_signal(condition_mutex);
greg@454: 				EnterMutex();
greg@454: 				TimeDispatch();
greg@454: 				LeaveMutex();
greg@454: 			}else{ 
greg@454: 				rt_sem_signal(condition_mutex);
greg@454: 			}
greg@454: 		}
greg@454: 	}while ( ret != SEM_ERR && !stop_timer);
greg@454: 	if(exitall){
greg@454: 		EnterMutex();
greg@454: 		exitall(NULL,0);
greg@454: 		LeaveMutex();
greg@454: 	}
greg@454: 	rt_make_soft_real_time();
greg@454: 	rt_thread_delete(timerloop_task);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Create the Timer Task
greg@454:  * @param _init_callback
greg@454:  */
greg@454: void StartTimerLoop(TimerCallback_t _init_callback)
greg@454: {
greg@454: 	stop_timer = 0;	
greg@454: 	init_callback = _init_callback;
greg@454: 	
greg@454: 	/* start timerloop_task ( do nothing and get blocked ) */
greg@454: 	timerloop_thr = rt_thread_create(timerloop_task_proc, NULL, 0);
greg@454: }
greg@454: 
greg@454: /* We assume that ReceiveLoop_task_proc is always the same */
greg@454: static void (*rtai_ReceiveLoop_task_proc)(CAN_PORT) = NULL;
greg@454: 
greg@454: /**
greg@454:  * Enter in realtime and start the CAN receiver loop
greg@454:  * @param port
greg@454:  */
greg@454: void rtai_canReceiveLoop(CAN_PORT port)
greg@454: {
greg@454: 	RT_TASK *current_task;
greg@454: 	mlockall(MCL_CURRENT | MCL_FUTURE);
greg@454: 	current_task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1);
greg@454: 	rt_make_hard_real_time();
greg@454: 
greg@454: 	rt_sem_signal(control_task);
greg@454: 
greg@454: 	/* Call original receive loop with port struct as a param */
greg@454: 	rtai_ReceiveLoop_task_proc(port);
greg@454: 
greg@454: 	rt_make_soft_real_time();
greg@454: 	rt_thread_delete(current_task);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Create the CAN Receiver Task
greg@454:  * @param fd0 CAN port
greg@454:  * @param *ReceiveLoop_thread CAN receiver thread
greg@454:  * @param *ReceiveLoop_task_proc CAN receiver task
greg@454:  */
greg@454: void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_thread, void* ReceiveLoop_task_proc)
greg@454: {	
greg@454: 	rtai_ReceiveLoop_task_proc = ReceiveLoop_task_proc;
greg@454: 	*ReceiveLoop_thread = rt_thread_create(rtai_canReceiveLoop, (void*)fd0, 0); 
greg@454: 	rt_sem_wait(control_task);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Wait for the CAN Receiver Task end
greg@454:  * @param *ReceiveLoop_thread CAN receiver thread
greg@454:  */
greg@454: void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_thread)
greg@454: {
greg@454: 	rt_thread_join(*ReceiveLoop_thread);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Set timer for the next wakeup
greg@454:  * @param value
greg@454:  */
greg@454: void setTimer(TIMEVAL value)
greg@454: {
greg@454: 	rt_sem_wait(condition_mutex);
etisserant@468: 	last_timeout_set = value;
greg@454: 	rt_sem_signal(condition_mutex);
greg@454: 	rt_cond_signal(timer_set);
greg@454: }
greg@454: 
greg@454: /**
greg@454:  * Get the elapsed time since the last alarm
greg@454:  * @return a time in nanoseconds
greg@454:  */
greg@454: TIMEVAL getElapsedTime(void)
greg@454: {
greg@454: 	RTIME res;
greg@454: 	rt_sem_wait(condition_mutex);
greg@454: 	last_time_read = rt_get_time();
greg@454: 	res = last_time_read - last_occured_alarm;
greg@454: 	rt_sem_signal(condition_mutex);
greg@454: 	return res;
greg@454: }