drivers/timers_rtai/timers_rtai.c
author Edouard Tisserant
Wed, 13 Apr 2011 16:29:59 +0200
changeset 659 1041153c5fd2
parent 468 787a54d068d6
child 801 32d146b64a35
permissions -rwxr-xr-x
Fix some side effects of EDS import, while editing afterward
454
bc000083297a - add RTAI support
greg
parents:
diff changeset
     1
#include <stdlib.h>
bc000083297a - add RTAI support
greg
parents:
diff changeset
     2
#include <unistd.h>
bc000083297a - add RTAI support
greg
parents:
diff changeset
     3
#include <sys/mman.h>
bc000083297a - add RTAI support
greg
parents:
diff changeset
     4
bc000083297a - add RTAI support
greg
parents:
diff changeset
     5
#include <sys/poll.h>
bc000083297a - add RTAI support
greg
parents:
diff changeset
     6
#include <rtai_lxrt.h>
bc000083297a - add RTAI support
greg
parents:
diff changeset
     7
bc000083297a - add RTAI support
greg
parents:
diff changeset
     8
#include <rtai_sem.h>
bc000083297a - add RTAI support
greg
parents:
diff changeset
     9
#include <pthread.h>
bc000083297a - add RTAI support
greg
parents:
diff changeset
    10
#include <errno.h>
bc000083297a - add RTAI support
greg
parents:
diff changeset
    11
bc000083297a - add RTAI support
greg
parents:
diff changeset
    12
#include "applicfg.h"
bc000083297a - add RTAI support
greg
parents:
diff changeset
    13
#include "can_driver.h"
bc000083297a - add RTAI support
greg
parents:
diff changeset
    14
#include "timer.h"
bc000083297a - add RTAI support
greg
parents:
diff changeset
    15
bc000083297a - add RTAI support
greg
parents:
diff changeset
    16
#define TIMERLOOP_TASK_CREATED        1
bc000083297a - add RTAI support
greg
parents:
diff changeset
    17
bc000083297a - add RTAI support
greg
parents:
diff changeset
    18
TimerCallback_t exitall;
bc000083297a - add RTAI support
greg
parents:
diff changeset
    19
bc000083297a - add RTAI support
greg
parents:
diff changeset
    20
SEM *CanFestival_mutex;
bc000083297a - add RTAI support
greg
parents:
diff changeset
    21
SEM *condition_mutex;
bc000083297a - add RTAI support
greg
parents:
diff changeset
    22
SEM *control_task; 
bc000083297a - add RTAI support
greg
parents:
diff changeset
    23
CND *timer_set;
bc000083297a - add RTAI support
greg
parents:
diff changeset
    24
bc000083297a - add RTAI support
greg
parents:
diff changeset
    25
// realtime task structures
bc000083297a - add RTAI support
greg
parents:
diff changeset
    26
RT_TASK *timerloop_task;
bc000083297a - add RTAI support
greg
parents:
diff changeset
    27
RT_TASK *Main_Task;
bc000083297a - add RTAI support
greg
parents:
diff changeset
    28
bc000083297a - add RTAI support
greg
parents:
diff changeset
    29
// linux threads id's
bc000083297a - add RTAI support
greg
parents:
diff changeset
    30
static pthread_t timerloop_thr;
bc000083297a - add RTAI support
greg
parents:
diff changeset
    31
 
bc000083297a - add RTAI support
greg
parents:
diff changeset
    32
RTIME last_time_read;
bc000083297a - add RTAI support
greg
parents:
diff changeset
    33
RTIME last_occured_alarm;
bc000083297a - add RTAI support
greg
parents:
diff changeset
    34
RTIME last_timeout_set;
bc000083297a - add RTAI support
greg
parents:
diff changeset
    35
bc000083297a - add RTAI support
greg
parents:
diff changeset
    36
int stop_timer = 0;
bc000083297a - add RTAI support
greg
parents:
diff changeset
    37
bc000083297a - add RTAI support
greg
parents:
diff changeset
    38
void TimerInit(void)
bc000083297a - add RTAI support
greg
parents:
diff changeset
    39
{
bc000083297a - add RTAI support
greg
parents:
diff changeset
    40
    /* Init Main Task */
bc000083297a - add RTAI support
greg
parents:
diff changeset
    41
    if (!(Main_Task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1))) {
bc000083297a - add RTAI support
greg
parents:
diff changeset
    42
            printf("CANNOT INIT MAIN TASK\n");
bc000083297a - add RTAI support
greg
parents:
diff changeset
    43
            exit(1);
bc000083297a - add RTAI support
greg
parents:
diff changeset
    44
    }
bc000083297a - add RTAI support
greg
parents:
diff changeset
    45
  	
bc000083297a - add RTAI support
greg
parents:
diff changeset
    46
  	/* Init Mutex */
bc000083297a - add RTAI support
greg
parents:
diff changeset
    47
	CanFestival_mutex = rt_sem_init(rt_get_name(0), 1);
bc000083297a - add RTAI support
greg
parents:
diff changeset
    48
	condition_mutex = rt_typed_sem_init(rt_get_name(0), 1, RES_SEM);
bc000083297a - add RTAI support
greg
parents:
diff changeset
    49
	timer_set = rt_cond_init(rt_get_name(0));
bc000083297a - add RTAI support
greg
parents:
diff changeset
    50
	control_task = rt_sem_init(rt_get_name(0), 0);
bc000083297a - add RTAI support
greg
parents:
diff changeset
    51
	/* Set timer mode and start timer */
bc000083297a - add RTAI support
greg
parents:
diff changeset
    52
	rt_set_oneshot_mode();
bc000083297a - add RTAI support
greg
parents:
diff changeset
    53
	start_rt_timer(0);
bc000083297a - add RTAI support
greg
parents:
diff changeset
    54
}
bc000083297a - add RTAI support
greg
parents:
diff changeset
    55
bc000083297a - add RTAI support
greg
parents:
diff changeset
    56
/**
bc000083297a - add RTAI support
greg
parents:
diff changeset
    57
 * Stop Timer Task
bc000083297a - add RTAI support
greg
parents:
diff changeset
    58
 * @param exitfunction
bc000083297a - add RTAI support
greg
parents:
diff changeset
    59
 */
bc000083297a - add RTAI support
greg
parents:
diff changeset
    60
void StopTimerLoop(TimerCallback_t exitfunction)
bc000083297a - add RTAI support
greg
parents:
diff changeset
    61
{
bc000083297a - add RTAI support
greg
parents:
diff changeset
    62
	exitall = exitfunction;
bc000083297a - add RTAI support
greg
parents:
diff changeset
    63
	stop_timer = 1;
bc000083297a - add RTAI support
greg
parents:
diff changeset
    64
	rt_cond_signal(timer_set);
bc000083297a - add RTAI support
greg
parents:
diff changeset
    65
}
bc000083297a - add RTAI support
greg
parents:
diff changeset
    66
bc000083297a - add RTAI support
greg
parents:
diff changeset
    67
/**
bc000083297a - add RTAI support
greg
parents:
diff changeset
    68
 * Clean all Semaphores and main task
bc000083297a - add RTAI support
greg
parents:
diff changeset
    69
 */
bc000083297a - add RTAI support
greg
parents:
diff changeset
    70
void TimerCleanup(void)
bc000083297a - add RTAI support
greg
parents:
diff changeset
    71
{
bc000083297a - add RTAI support
greg
parents:
diff changeset
    72
	/* Stop timer */
bc000083297a - add RTAI support
greg
parents:
diff changeset
    73
	stop_rt_timer();
bc000083297a - add RTAI support
greg
parents:
diff changeset
    74
bc000083297a - add RTAI support
greg
parents:
diff changeset
    75
	/* Delete all mutex and the main task */
bc000083297a - add RTAI support
greg
parents:
diff changeset
    76
	rt_sem_delete(CanFestival_mutex);
bc000083297a - add RTAI support
greg
parents:
diff changeset
    77
	rt_sem_delete(condition_mutex);
bc000083297a - add RTAI support
greg
parents:
diff changeset
    78
	rt_sem_delete(timer_set);
bc000083297a - add RTAI support
greg
parents:
diff changeset
    79
	rt_sem_delete(control_task);
bc000083297a - add RTAI support
greg
parents:
diff changeset
    80
	rt_thread_delete(Main_Task);
bc000083297a - add RTAI support
greg
parents:
diff changeset
    81
}
bc000083297a - add RTAI support
greg
parents:
diff changeset
    82
bc000083297a - add RTAI support
greg
parents:
diff changeset
    83
/**
bc000083297a - add RTAI support
greg
parents:
diff changeset
    84
 * Take a semaphore
bc000083297a - add RTAI support
greg
parents:
diff changeset
    85
 */
bc000083297a - add RTAI support
greg
parents:
diff changeset
    86
void EnterMutex(void)
bc000083297a - add RTAI support
greg
parents:
diff changeset
    87
{
bc000083297a - add RTAI support
greg
parents:
diff changeset
    88
	rt_sem_wait(CanFestival_mutex);
bc000083297a - add RTAI support
greg
parents:
diff changeset
    89
}
bc000083297a - add RTAI support
greg
parents:
diff changeset
    90
bc000083297a - add RTAI support
greg
parents:
diff changeset
    91
/**
bc000083297a - add RTAI support
greg
parents:
diff changeset
    92
 * Signaling a semaphore
bc000083297a - add RTAI support
greg
parents:
diff changeset
    93
 */
bc000083297a - add RTAI support
greg
parents:
diff changeset
    94
void LeaveMutex(void)
bc000083297a - add RTAI support
greg
parents:
diff changeset
    95
{
bc000083297a - add RTAI support
greg
parents:
diff changeset
    96
	rt_sem_signal(CanFestival_mutex);
bc000083297a - add RTAI support
greg
parents:
diff changeset
    97
}
bc000083297a - add RTAI support
greg
parents:
diff changeset
    98
bc000083297a - add RTAI support
greg
parents:
diff changeset
    99
static TimerCallback_t init_callback;
bc000083297a - add RTAI support
greg
parents:
diff changeset
   100
bc000083297a - add RTAI support
greg
parents:
diff changeset
   101
/**
bc000083297a - add RTAI support
greg
parents:
diff changeset
   102
 * Timer Task
bc000083297a - add RTAI support
greg
parents:
diff changeset
   103
 */
bc000083297a - add RTAI support
greg
parents:
diff changeset
   104
void timerloop_task_proc(void *arg)
bc000083297a - add RTAI support
greg
parents:
diff changeset
   105
{
bc000083297a - add RTAI support
greg
parents:
diff changeset
   106
	int ret = 0;
bc000083297a - add RTAI support
greg
parents:
diff changeset
   107
  	// lock process in to RAM
bc000083297a - add RTAI support
greg
parents:
diff changeset
   108
  	mlockall(MCL_CURRENT | MCL_FUTURE);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   109
	timerloop_task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   110
	rt_make_hard_real_time();
bc000083297a - add RTAI support
greg
parents:
diff changeset
   111
bc000083297a - add RTAI support
greg
parents:
diff changeset
   112
	getElapsedTime();
bc000083297a - add RTAI support
greg
parents:
diff changeset
   113
	last_timeout_set = 0;
bc000083297a - add RTAI support
greg
parents:
diff changeset
   114
	last_occured_alarm = last_time_read;
bc000083297a - add RTAI support
greg
parents:
diff changeset
   115
bc000083297a - add RTAI support
greg
parents:
diff changeset
   116
	/* trigger first alarm */
bc000083297a - add RTAI support
greg
parents:
diff changeset
   117
	SetAlarm(NULL, 0, init_callback, 0, 0);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   118
	
bc000083297a - add RTAI support
greg
parents:
diff changeset
   119
	do{
bc000083297a - add RTAI support
greg
parents:
diff changeset
   120
		RTIME real_alarm;
bc000083297a - add RTAI support
greg
parents:
diff changeset
   121
		rt_sem_wait(condition_mutex);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   122
		if(last_timeout_set == TIMEVAL_MAX)
bc000083297a - add RTAI support
greg
parents:
diff changeset
   123
		{
bc000083297a - add RTAI support
greg
parents:
diff changeset
   124
			ret = rt_cond_wait(
bc000083297a - add RTAI support
greg
parents:
diff changeset
   125
				timer_set,
bc000083297a - add RTAI support
greg
parents:
diff changeset
   126
				condition_mutex);		/* Then sleep until next message*/
bc000083297a - add RTAI support
greg
parents:
diff changeset
   127
 
bc000083297a - add RTAI support
greg
parents:
diff changeset
   128
			rt_sem_signal(condition_mutex);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   129
		}else{
bc000083297a - add RTAI support
greg
parents:
diff changeset
   130
			real_alarm = last_time_read + last_timeout_set;
bc000083297a - add RTAI support
greg
parents:
diff changeset
   131
			ret = rt_cond_wait_until(
bc000083297a - add RTAI support
greg
parents:
diff changeset
   132
				timer_set,
bc000083297a - add RTAI support
greg
parents:
diff changeset
   133
				condition_mutex,
bc000083297a - add RTAI support
greg
parents:
diff changeset
   134
				real_alarm); /* Else, sleep until next deadline */
bc000083297a - add RTAI support
greg
parents:
diff changeset
   135
			if(ret == SEM_TIMOUT){
bc000083297a - add RTAI support
greg
parents:
diff changeset
   136
				last_occured_alarm = real_alarm;
bc000083297a - add RTAI support
greg
parents:
diff changeset
   137
				rt_sem_signal(condition_mutex);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   138
				EnterMutex();
bc000083297a - add RTAI support
greg
parents:
diff changeset
   139
				TimeDispatch();
bc000083297a - add RTAI support
greg
parents:
diff changeset
   140
				LeaveMutex();
bc000083297a - add RTAI support
greg
parents:
diff changeset
   141
			}else{ 
bc000083297a - add RTAI support
greg
parents:
diff changeset
   142
				rt_sem_signal(condition_mutex);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   143
			}
bc000083297a - add RTAI support
greg
parents:
diff changeset
   144
		}
bc000083297a - add RTAI support
greg
parents:
diff changeset
   145
	}while ( ret != SEM_ERR && !stop_timer);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   146
	if(exitall){
bc000083297a - add RTAI support
greg
parents:
diff changeset
   147
		EnterMutex();
bc000083297a - add RTAI support
greg
parents:
diff changeset
   148
		exitall(NULL,0);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   149
		LeaveMutex();
bc000083297a - add RTAI support
greg
parents:
diff changeset
   150
	}
bc000083297a - add RTAI support
greg
parents:
diff changeset
   151
	rt_make_soft_real_time();
bc000083297a - add RTAI support
greg
parents:
diff changeset
   152
	rt_thread_delete(timerloop_task);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   153
}
bc000083297a - add RTAI support
greg
parents:
diff changeset
   154
bc000083297a - add RTAI support
greg
parents:
diff changeset
   155
/**
bc000083297a - add RTAI support
greg
parents:
diff changeset
   156
 * Create the Timer Task
bc000083297a - add RTAI support
greg
parents:
diff changeset
   157
 * @param _init_callback
bc000083297a - add RTAI support
greg
parents:
diff changeset
   158
 */
bc000083297a - add RTAI support
greg
parents:
diff changeset
   159
void StartTimerLoop(TimerCallback_t _init_callback)
bc000083297a - add RTAI support
greg
parents:
diff changeset
   160
{
bc000083297a - add RTAI support
greg
parents:
diff changeset
   161
	stop_timer = 0;	
bc000083297a - add RTAI support
greg
parents:
diff changeset
   162
	init_callback = _init_callback;
bc000083297a - add RTAI support
greg
parents:
diff changeset
   163
	
bc000083297a - add RTAI support
greg
parents:
diff changeset
   164
	/* start timerloop_task ( do nothing and get blocked ) */
bc000083297a - add RTAI support
greg
parents:
diff changeset
   165
	timerloop_thr = rt_thread_create(timerloop_task_proc, NULL, 0);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   166
}
bc000083297a - add RTAI support
greg
parents:
diff changeset
   167
bc000083297a - add RTAI support
greg
parents:
diff changeset
   168
/* We assume that ReceiveLoop_task_proc is always the same */
bc000083297a - add RTAI support
greg
parents:
diff changeset
   169
static void (*rtai_ReceiveLoop_task_proc)(CAN_PORT) = NULL;
bc000083297a - add RTAI support
greg
parents:
diff changeset
   170
bc000083297a - add RTAI support
greg
parents:
diff changeset
   171
/**
bc000083297a - add RTAI support
greg
parents:
diff changeset
   172
 * Enter in realtime and start the CAN receiver loop
bc000083297a - add RTAI support
greg
parents:
diff changeset
   173
 * @param port
bc000083297a - add RTAI support
greg
parents:
diff changeset
   174
 */
bc000083297a - add RTAI support
greg
parents:
diff changeset
   175
void rtai_canReceiveLoop(CAN_PORT port)
bc000083297a - add RTAI support
greg
parents:
diff changeset
   176
{
bc000083297a - add RTAI support
greg
parents:
diff changeset
   177
	RT_TASK *current_task;
bc000083297a - add RTAI support
greg
parents:
diff changeset
   178
	mlockall(MCL_CURRENT | MCL_FUTURE);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   179
	current_task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   180
	rt_make_hard_real_time();
bc000083297a - add RTAI support
greg
parents:
diff changeset
   181
bc000083297a - add RTAI support
greg
parents:
diff changeset
   182
	rt_sem_signal(control_task);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   183
bc000083297a - add RTAI support
greg
parents:
diff changeset
   184
	/* Call original receive loop with port struct as a param */
bc000083297a - add RTAI support
greg
parents:
diff changeset
   185
	rtai_ReceiveLoop_task_proc(port);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   186
bc000083297a - add RTAI support
greg
parents:
diff changeset
   187
	rt_make_soft_real_time();
bc000083297a - add RTAI support
greg
parents:
diff changeset
   188
	rt_thread_delete(current_task);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   189
}
bc000083297a - add RTAI support
greg
parents:
diff changeset
   190
bc000083297a - add RTAI support
greg
parents:
diff changeset
   191
/**
bc000083297a - add RTAI support
greg
parents:
diff changeset
   192
 * Create the CAN Receiver Task
bc000083297a - add RTAI support
greg
parents:
diff changeset
   193
 * @param fd0 CAN port
bc000083297a - add RTAI support
greg
parents:
diff changeset
   194
 * @param *ReceiveLoop_thread CAN receiver thread
bc000083297a - add RTAI support
greg
parents:
diff changeset
   195
 * @param *ReceiveLoop_task_proc CAN receiver task
bc000083297a - add RTAI support
greg
parents:
diff changeset
   196
 */
bc000083297a - add RTAI support
greg
parents:
diff changeset
   197
void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_thread, void* ReceiveLoop_task_proc)
bc000083297a - add RTAI support
greg
parents:
diff changeset
   198
{	
bc000083297a - add RTAI support
greg
parents:
diff changeset
   199
	rtai_ReceiveLoop_task_proc = ReceiveLoop_task_proc;
bc000083297a - add RTAI support
greg
parents:
diff changeset
   200
	*ReceiveLoop_thread = rt_thread_create(rtai_canReceiveLoop, (void*)fd0, 0); 
bc000083297a - add RTAI support
greg
parents:
diff changeset
   201
	rt_sem_wait(control_task);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   202
}
bc000083297a - add RTAI support
greg
parents:
diff changeset
   203
bc000083297a - add RTAI support
greg
parents:
diff changeset
   204
/**
bc000083297a - add RTAI support
greg
parents:
diff changeset
   205
 * Wait for the CAN Receiver Task end
bc000083297a - add RTAI support
greg
parents:
diff changeset
   206
 * @param *ReceiveLoop_thread CAN receiver thread
bc000083297a - add RTAI support
greg
parents:
diff changeset
   207
 */
bc000083297a - add RTAI support
greg
parents:
diff changeset
   208
void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_thread)
bc000083297a - add RTAI support
greg
parents:
diff changeset
   209
{
bc000083297a - add RTAI support
greg
parents:
diff changeset
   210
	rt_thread_join(*ReceiveLoop_thread);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   211
}
bc000083297a - add RTAI support
greg
parents:
diff changeset
   212
bc000083297a - add RTAI support
greg
parents:
diff changeset
   213
/**
bc000083297a - add RTAI support
greg
parents:
diff changeset
   214
 * Set timer for the next wakeup
bc000083297a - add RTAI support
greg
parents:
diff changeset
   215
 * @param value
bc000083297a - add RTAI support
greg
parents:
diff changeset
   216
 */
bc000083297a - add RTAI support
greg
parents:
diff changeset
   217
void setTimer(TIMEVAL value)
bc000083297a - add RTAI support
greg
parents:
diff changeset
   218
{
bc000083297a - add RTAI support
greg
parents:
diff changeset
   219
	rt_sem_wait(condition_mutex);
468
787a54d068d6 Fixed minor typos in timers_rtai.c and timers_xeno.c
etisserant
parents: 454
diff changeset
   220
	last_timeout_set = value;
454
bc000083297a - add RTAI support
greg
parents:
diff changeset
   221
	rt_sem_signal(condition_mutex);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   222
	rt_cond_signal(timer_set);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   223
}
bc000083297a - add RTAI support
greg
parents:
diff changeset
   224
bc000083297a - add RTAI support
greg
parents:
diff changeset
   225
/**
bc000083297a - add RTAI support
greg
parents:
diff changeset
   226
 * Get the elapsed time since the last alarm
bc000083297a - add RTAI support
greg
parents:
diff changeset
   227
 * @return a time in nanoseconds
bc000083297a - add RTAI support
greg
parents:
diff changeset
   228
 */
bc000083297a - add RTAI support
greg
parents:
diff changeset
   229
TIMEVAL getElapsedTime(void)
bc000083297a - add RTAI support
greg
parents:
diff changeset
   230
{
bc000083297a - add RTAI support
greg
parents:
diff changeset
   231
	RTIME res;
bc000083297a - add RTAI support
greg
parents:
diff changeset
   232
	rt_sem_wait(condition_mutex);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   233
	last_time_read = rt_get_time();
bc000083297a - add RTAI support
greg
parents:
diff changeset
   234
	res = last_time_read - last_occured_alarm;
bc000083297a - add RTAI support
greg
parents:
diff changeset
   235
	rt_sem_signal(condition_mutex);
bc000083297a - add RTAI support
greg
parents:
diff changeset
   236
	return res;
bc000083297a - add RTAI support
greg
parents:
diff changeset
   237
}