etisserant@0: #include <stdlib.h>
etisserant@0: #include <unistd.h> 
etisserant@0: 
etisserant@0: #include <native/task.h>
etisserant@0: #include <native/timer.h>
etisserant@0: #include <native/mutex.h>
etisserant@0: #include <native/alarm.h>
etisserant@0: 
etisserant@0: #include "applicfg.h"
etisserant@0: #include "can_driver.h"
etisserant@0: #include "timer.h"
etisserant@0: 
etisserant@0: #define TIMERLOOP_TASK_CREATED        1
etisserant@0: 
etisserant@0: RT_MUTEX CanFestival_mutex;
etisserant@0: RT_TASK timerloop_task;
etisserant@0: RT_ALARM timerloop_alarm;
etisserant@0: RTIME last_time_read;
etisserant@0: RTIME last_occured_alarm;
etisserant@0: RTIME last_alarm_set;
etisserant@0: 
etisserant@0: char stop_timer=0;
etisserant@0: 
etisserant@0: void cleanup_all(void)
etisserant@0: {
etisserant@0: 	rt_task_delete(&timerloop_task);
etisserant@0: 	rt_alarm_delete(&timerloop_alarm);
etisserant@0: }
etisserant@0: void stopTimer(void)
etisserant@0: {
etisserant@0: 	stop_timer = 1;
etisserant@0: 	rt_task_unblock(&timerloop_task);
etisserant@0: }
etisserant@0: 
etisserant@0: 
etisserant@0: void EnterMutex(void)
etisserant@0: {
etisserant@0: 	rt_mutex_lock(&CanFestival_mutex, TM_INFINITE); 
etisserant@0: }
etisserant@0: 
etisserant@0: void LeaveMutex(void)
etisserant@0: {
etisserant@0: 	rt_mutex_unlock(&CanFestival_mutex);
etisserant@0: }
etisserant@0: 
etisserant@0: void timerloop_task_proc(void *arg)
etisserant@0: {
etisserant@0: 	int ret;
etisserant@0: 	do{
etisserant@0: 		do{
etisserant@0: 			last_occured_alarm = last_alarm_set;
etisserant@0: 			EnterMutex();
etisserant@0: 			TimeDispatch();
etisserant@0: 			LeaveMutex();
etisserant@0: 		}while ((ret =  rt_task_sleep_until(last_alarm_set)) == 0);
etisserant@0: 	}while (ret == -EINTR && !stop_timer );
etisserant@0: 	printf("End of TimerLoop, code %d\n",ret);
etisserant@0: }
etisserant@0: 
etisserant@0: void TimerLoop(TimerCallback_t init_callback)
etisserant@0: {
etisserant@0: 	int ret;
etisserant@0: 	stop_timer = 0;
etisserant@0: 
etisserant@0: 	ret = rt_alarm_create( &timerloop_alarm, NULL);
etisserant@0: 	if (ret) {
etisserant@0: 		printf("Failed to create timerloop_alarm, code %d\n",errno);
etisserant@0: 		goto error;
etisserant@0: 	}
etisserant@0: 	
etisserant@0: 	//create timerloop_task
etisserant@0: 	ret = rt_task_create(&timerloop_task,"timerloop",0,50,0);
etisserant@0: 	if (ret) {
etisserant@0: 		printf("Failed to create timerloop_task, code %d\n",errno);
etisserant@0: 		return;
etisserant@0: 	}
etisserant@0:  	
etisserant@0: 	getElapsedTime();
etisserant@0: 	last_alarm_set = last_time_read;
etisserant@0: 	last_occured_alarm = last_alarm_set;
etisserant@0: 	SetAlarm(NULL, 0, init_callback, 0, 0);
etisserant@0: 	// start timerloop_task
etisserant@0: 	ret = rt_task_start(&timerloop_task,&timerloop_task_proc,NULL);
etisserant@0: 	if (ret) {
etisserant@0: 		printf("Failed to start timerloop_task, code %d\n",errno);
etisserant@0: 		goto error;
etisserant@0: 	}
etisserant@0: 	
etisserant@0: 	// At first, TimeDispatch will call init_callback.
etisserant@0: 	pause();
etisserant@0: 
etisserant@0: error:
etisserant@0: 	cleanup_all();
etisserant@0: }
etisserant@0: 
etisserant@0: void ReceiveLoop_task_proc(void* arg)
etisserant@0: {
etisserant@0: 	canReceiveLoop((CAN_HANDLE)arg);
etisserant@0: }
etisserant@0: 
etisserant@0: void CreateReceiveTask(CAN_HANDLE fd0, TASK_HANDLE *ReceiveLoop_task)
etisserant@0: {
etisserant@0: 	int ret;
etisserant@0: 	static int id = 0;
etisserant@0: 	char taskname[64];
etisserant@0: 	sprintf(taskname,"canloop%d",id);
etisserant@0: 	id++;
etisserant@0: 
etisserant@0: 	//create timerloop_task
etisserant@0: 	ret = rt_task_create(ReceiveLoop_task,taskname,0,50,0);
etisserant@0: 	if (ret) {
etisserant@0: 		printf("Failed to create ReceiveLoop_task number %d, code %d\n", id, errno);
etisserant@0: 		return;
etisserant@0: 	}
etisserant@0: 	// start timerloop_task
etisserant@0: 	ret = rt_task_start(ReceiveLoop_task,&ReceiveLoop_task_proc,(void*)fd0);
etisserant@0: 	if (ret) {
etisserant@0: 		printf("Failed to start ReceiveLoop_task number %d, code %d\n", id, errno);
etisserant@0: 		return;
etisserant@0: 	}
etisserant@0: }
etisserant@0: 
etisserant@0: void WaitReceiveTaskEnd(TASK_HANDLE Thread)
etisserant@0: {
etisserant@0: 	rt_task_delete(&Thread);
etisserant@0: }
etisserant@0: #define max(a,b) a>b?a:b
etisserant@0: void setTimer(TIMEVAL value)
etisserant@0: {	
etisserant@0: 	last_alarm_set = last_time_read + value;
etisserant@0: 	rt_task_unblock(&timerloop_task);
etisserant@0: }
etisserant@0: 
etisserant@0: TIMEVAL getElapsedTime(void)
etisserant@0: {
etisserant@0: 	last_time_read = rt_timer_ticks2ns(rt_timer_read());
etisserant@0: 	return last_time_read - last_occured_alarm;
etisserant@0: }