drivers/timers_kernel_xeno/timers_kernel_xeno.c
author edouard
Sat, 13 Nov 2010 23:08:34 +0100
changeset 633 2c43383a1d57
parent 629 b9274b595650
child 801 32d146b64a35
permissions -rw-r--r--
Fix mingw32 cross build for can_peak_win32 and can_tcp_win32 server.
629
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
     1
/*
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
     2
This file is part of CanFestival, a library implementing CanOpen Stack.
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
     3
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
     4
Copyright (C): Cosateq GmbH & Co.KG
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
     5
               http://www.cosateq.com/
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
     6
               http://www.scale-rt.com/
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
     7
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
     8
See COPYING file for copyrights details.
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
     9
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    10
This library is free software; you can redistribute it and/or
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    11
modify it under the terms of the GNU Lesser General Public
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    12
License as published by the Free Software Foundation; either
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    13
version 2.1 of the License, or (at your option) any later version.
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    14
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    15
This library is distributed in the hope that it will be useful,
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    16
but WITHOUT ANY WARRANTY; without even the implied warranty of
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    17
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    18
Lesser General Public License for more details.
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    19
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    20
You should have received a copy of the GNU Lesser General Public
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    21
License along with this library; if not, write to the Free Software
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    22
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    23
*/
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    24
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    25
#include <asm/current.h>
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    26
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    27
#include <native/task.h>
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    28
#include <native/timer.h>
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    29
#include <native/sem.h>
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    30
#include <native/mutex.h>
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    31
#include <native/cond.h>
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    32
#include <native/alarm.h>
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    33
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    34
#include "applicfg.h"
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    35
#include "can_driver.h"
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    36
#include "timer.h"
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    37
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    38
#define TIMERLOOP_TASK_CREATED        1
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    39
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    40
TimerCallback_t exitall;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    41
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    42
RT_MUTEX condition_mutex;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    43
RT_SEM CanFestival_mutex;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    44
RT_SEM control_task;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    45
RT_COND timer_set;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    46
RT_TASK timerloop_task;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    47
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    48
RTIME last_time_read;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    49
RTIME last_occured_alarm;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    50
RTIME last_timeout_set;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    51
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    52
int stop_timer = 0;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    53
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    54
/**
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    55
 * Init Mutex, Semaphores and Condition variable
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    56
 */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    57
void TimerInit(void)
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    58
{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    59
  	int ret = 0;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    60
  	char taskname[32];
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    61
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    62
	// lock process in to RAM
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    63
  	//mlockall(MCL_CURRENT | MCL_FUTURE);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    64
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    65
  	snprintf(taskname, sizeof(taskname), "S1-%d", current->pid);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    66
	rt_sem_create(&CanFestival_mutex, taskname, 1, S_FIFO);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    67
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    68
  	snprintf(taskname, sizeof(taskname), "S2-%d", current->pid);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    69
  	rt_sem_create(&control_task, taskname, 0, S_FIFO);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    70
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    71
  	snprintf(taskname, sizeof(taskname), "M1-%d", current->pid);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    72
  	rt_mutex_create(&condition_mutex, taskname);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    73
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    74
  	snprintf(taskname, sizeof(taskname), "C1-%d", current->pid);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    75
  	rt_cond_create(&timer_set, taskname);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    76
}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    77
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    78
/**
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    79
 * Stop Timer Task
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    80
 * @param exitfunction
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    81
 */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    82
void StopTimerLoop(TimerCallback_t exitfunction)
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    83
{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    84
	exitall = exitfunction;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    85
	stop_timer = 1;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    86
	rt_cond_signal(&timer_set);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    87
}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    88
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    89
void cleanup_all(void)
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    90
{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    91
	/* normally this will fail with a non-periodic task that has already ended at this time */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    92
	if (rt_task_suspend(&timerloop_task) != 0){
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    93
		printk("Failed to join with Timerloop task\n");
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    94
	}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    95
	rt_task_delete(&timerloop_task);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    96
}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    97
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    98
/**
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
    99
 * Clean all Semaphores, mutex, condition variable and main task
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   100
 */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   101
void TimerCleanup(void)
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   102
{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   103
	rt_sem_delete(&CanFestival_mutex);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   104
	rt_mutex_delete(&condition_mutex);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   105
	rt_cond_delete(&timer_set);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   106
	rt_sem_delete(&control_task);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   107
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   108
	/* normally this will fail with a non-periodic task that has already ended at this time */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   109
	if (rt_task_suspend(&timerloop_task) != 0){
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   110
		printk("Failed to join with Timerloop task\n");
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   111
	}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   112
	rt_task_delete(&timerloop_task);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   113
}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   114
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   115
/**
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   116
 * Take a semaphore
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   117
 */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   118
void EnterMutex(void)
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   119
{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   120
	rt_sem_p(&CanFestival_mutex, TM_INFINITE);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   121
}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   122
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   123
/**
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   124
 * Signaling a semaphore
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   125
 */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   126
void LeaveMutex(void)
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   127
{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   128
	rt_sem_v(&CanFestival_mutex);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   129
}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   130
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   131
static TimerCallback_t init_callback;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   132
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   133
/**
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   134
 * Timer Task
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   135
 */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   136
void timerloop_task_proc(void *arg)
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   137
{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   138
	int ret = 0;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   139
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   140
	getElapsedTime();
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   141
	last_timeout_set = 0;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   142
	last_occured_alarm = last_time_read;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   143
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   144
	/* trigger first alarm */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   145
	SetAlarm(NULL, 0, init_callback, 0, 0);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   146
	RTIME current_time;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   147
	RTIME real_alarm;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   148
	do{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   149
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   150
		rt_mutex_acquire(&condition_mutex, TM_INFINITE);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   151
		if(last_timeout_set == TIMEVAL_MAX)
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   152
		{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   153
			ret = rt_cond_wait(
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   154
				&timer_set,
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   155
				&condition_mutex,
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   156
				TM_INFINITE
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   157
				);		/* Then sleep until next message*/
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   158
			rt_mutex_release(&condition_mutex);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   159
		}else{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   160
			current_time = rt_timer_read();
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   161
			real_alarm = last_time_read + last_timeout_set;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   162
			ret = rt_cond_wait( /* sleep until next deadline */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   163
				&timer_set,
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   164
				&condition_mutex,
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   165
				(real_alarm - current_time)); /* else alarm consider expired */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   166
			if(ret == -ETIMEDOUT){
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   167
				last_occured_alarm = real_alarm;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   168
				rt_mutex_release(&condition_mutex);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   169
				EnterMutex();
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   170
				TimeDispatch();
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   171
				LeaveMutex();
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   172
			}else{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   173
				rt_mutex_release(&condition_mutex);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   174
			}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   175
		}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   176
	}while ((ret == 0 || ret == -EINTR || ret == -ETIMEDOUT) && !stop_timer);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   177
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   178
	if(exitall){
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   179
		EnterMutex();
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   180
		exitall(NULL,0);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   181
		LeaveMutex();
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   182
	}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   183
}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   184
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   185
/**
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   186
 * Create the Timer Task
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   187
 * @param _init_callback
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   188
 */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   189
void StartTimerLoop(TimerCallback_t _init_callback)
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   190
{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   191
	int ret = 0;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   192
	stop_timer = 0;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   193
	init_callback = _init_callback;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   194
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   195
	char taskname[32];
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   196
	snprintf(taskname, sizeof(taskname), "timerloop-%d", current->pid);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   197
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   198
	/* create timerloop_task */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   199
	ret = rt_task_create(&timerloop_task, taskname, 0, 50, 0); /* T_JOINABLE only in user space */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   200
	if (ret) {
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   201
		printk("Failed to create timerloop_task, code %d\n",ret);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   202
		return;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   203
	}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   204
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   205
	/* start timerloop_task */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   206
	ret = rt_task_start(&timerloop_task,&timerloop_task_proc,NULL);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   207
	if (ret) {
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   208
		printk("Failed to start timerloop_task, code %u\n",ret);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   209
		goto error;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   210
	}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   211
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   212
	return;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   213
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   214
error:
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   215
	cleanup_all();
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   216
}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   217
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   218
/**
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   219
 * Create the CAN Receiver Task
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   220
 * @param fd0 CAN port
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   221
 * @param *ReceiveLoop_task CAN receiver task
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   222
 * @param *ReceiveLoop_task_proc CAN receiver function
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   223
 */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   224
void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_task, void* ReceiveLoop_task_proc)
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   225
{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   226
	int ret;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   227
	static int id = 0;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   228
	char taskname[32];
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   229
	snprintf(taskname, sizeof(taskname), "canloop%d-%d", id, current->pid);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   230
	id++;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   231
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   232
	/* create ReceiveLoop_task */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   233
	ret = rt_task_create(ReceiveLoop_task,taskname,0,50,0); /* T_JOINABLE only in user space */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   234
	if (ret) {
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   235
		printk("Failed to create ReceiveLoop_task number %d, code %d\n", id, ret);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   236
		return;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   237
	}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   238
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   239
	/* periodic task for Xenomai kernel realtime */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   240
	rt_task_set_periodic(ReceiveLoop_task, 0, 1 * 1000 * 1000); /* 1ms */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   241
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   242
	/* start ReceiveLoop_task */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   243
	ret = rt_task_start(ReceiveLoop_task, ReceiveLoop_task_proc,(void*)fd0);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   244
	if (ret) {
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   245
		printk("Failed to start ReceiveLoop_task number %d, code %d\n", id, ret);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   246
		return;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   247
	}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   248
	rt_sem_v(&control_task);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   249
}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   250
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   251
/**
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   252
 * Wait for the CAN Receiver Task end
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   253
 * @param *ReceiveLoop_task CAN receiver thread
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   254
 */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   255
void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_task)
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   256
{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   257
	/* normally this will fail with a non-periodic task that has already ended at this time */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   258
	if (rt_task_suspend(ReceiveLoop_task) != 0){
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   259
		printk("Failed to join with Receive task\n");
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   260
	}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   261
	rt_task_delete(ReceiveLoop_task);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   262
}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   263
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   264
/**
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   265
 * Set timer for the next wakeup
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   266
 * @param value
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   267
 */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   268
void setTimer(TIMEVAL value)
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   269
{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   270
	rt_mutex_acquire(&condition_mutex, TM_INFINITE);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   271
	last_timeout_set = value;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   272
	rt_mutex_release(&condition_mutex);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   273
	rt_cond_signal(&timer_set);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   274
}
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   275
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   276
/**
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   277
 * Get the elapsed time since the last alarm
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   278
 * @return a time in nanoseconds
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   279
 */
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   280
TIMEVAL getElapsedTime(void)
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   281
{
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   282
	RTIME res;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   283
	rt_mutex_acquire(&condition_mutex, TM_INFINITE);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   284
	last_time_read = rt_timer_read();
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   285
	res = last_time_read - last_occured_alarm;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   286
	rt_mutex_release(&condition_mutex);
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   287
	return res;
b9274b595650 CosateQ contribution.
edouard
parents:
diff changeset
   288
}