1

#include <stdlib.h>

     2

#include <unistd.h>

     3

#include <sys/mman.h>

     4

     5

#include <sys/poll.h>

     6

#include <rtai_lxrt.h>

     7

     8

#include <rtai_sem.h>

     9

#include <pthread.h>

    10

#include <errno.h>

    11

    12

#include "applicfg.h"

    13

#include "can_driver.h"

    14

#include "timers.h"

    15

    16

#define TIMERLOOP_TASK_CREATED        1

    17

    18

TimerCallback_t exitall;

    19

    20

SEM *CanFestival_mutex;

    21

SEM *condition_mutex;

    22

SEM *control_task; 

    23

CND *timer_set;

    24

    25

// realtime task structures

    26

RT_TASK *timerloop_task;

    27

RT_TASK *Main_Task;

    28

    29

// linux threads id's

    30

static pthread_t timerloop_thr;

    31

    32

RTIME last_time_read;

    33

RTIME last_occured_alarm;

    34

RTIME last_timeout_set;

    35

    36

int stop_timer = 0;

    37

    38

void TimerInit(void)

    39

{

    40

    /* Init Main Task */

    41

    if (!(Main_Task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1))) {

    42

            printf("CANNOT INIT MAIN TASK\n");

    43

            exit(1);

    44

    }

    45

    46

  	/* Init Mutex */

    47

	CanFestival_mutex = rt_sem_init(rt_get_name(0), 1);

    48

	condition_mutex = rt_typed_sem_init(rt_get_name(0), 1, RES_SEM);

    49

	timer_set = rt_cond_init(rt_get_name(0));

    50

	control_task = rt_sem_init(rt_get_name(0), 0);

    51

	/* Set timer mode and start timer */

    52

	rt_set_oneshot_mode();

    53

	start_rt_timer(0);

    54

}

    55

    56

/**

    57

 * Stop Timer Task

    58

 * @param exitfunction

    59

 */

    60

void StopTimerLoop(TimerCallback_t exitfunction)

    61

{

    62

	exitall = exitfunction;

    63

	stop_timer = 1;

    64

	rt_cond_signal(timer_set);

    65

}

    66

    67

/**

    68

 * Clean all Semaphores and main task

    69

 */

    70

void TimerCleanup(void)

    71

{

    72

	/* Stop timer */

    73

	stop_rt_timer();

    74

    75

	/* Delete all mutex and the main task */

    76

	rt_sem_delete(CanFestival_mutex);

    77

	rt_sem_delete(condition_mutex);

    78

	rt_sem_delete(timer_set);

    79

	rt_sem_delete(control_task);

    80

	rt_thread_delete(Main_Task);

    81

}

    82

    83

/**

    84

 * Take a semaphore

    85

 */

    86

void EnterMutex(void)

    87

{

    88

	rt_sem_wait(CanFestival_mutex);

    89

}

    90

    91

/**

    92

 * Signaling a semaphore

    93

 */

    94

void LeaveMutex(void)

    95

{

    96

	rt_sem_signal(CanFestival_mutex);

    97

}

    98

    99

static TimerCallback_t init_callback;

   100

   101

/**

   102

 * Timer Task

   103

 */

   104

void timerloop_task_proc(void *arg)

   105

{

   106

	int ret = 0;

   107

  	// lock process in to RAM

   108

  	mlockall(MCL_CURRENT | MCL_FUTURE);

   109

	timerloop_task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1);

   110

	rt_make_hard_real_time();

   111

   112

	getElapsedTime();

   113

	last_timeout_set = 0;

   114

	last_occured_alarm = last_time_read;

   115

   116

	/* trigger first alarm */

   117

	SetAlarm(NULL, 0, init_callback, 0, 0);

   118

   119

	do{

   120

		RTIME real_alarm;

   121

		rt_sem_wait(condition_mutex);

   122

		if(last_timeout_set == TIMEVAL_MAX)

   123

		{

   124

			ret = rt_cond_wait(

   125

				timer_set,

   126

				condition_mutex);		/* Then sleep until next message*/

   127

   128

			rt_sem_signal(condition_mutex);

   129

		}else{

   130

			real_alarm = last_time_read + last_timeout_set;

   131

			ret = rt_cond_wait_until(

   132

				timer_set,

   133

				condition_mutex,

   134

				real_alarm); /* Else, sleep until next deadline */

   135

			if(ret == SEM_TIMOUT){

   136

				last_occured_alarm = real_alarm;

   137

				rt_sem_signal(condition_mutex);

   138

				EnterMutex();

   139

				TimeDispatch();

   140

				LeaveMutex();

   141

			}else{ 

   142

				rt_sem_signal(condition_mutex);

   143

			}

   144

		}

   145

	}while ( ret != SEM_ERR && !stop_timer);

   146

	if(exitall){

   147

		EnterMutex();

   148

		exitall(NULL,0);

   149

		LeaveMutex();

   150

	}

   151

	rt_make_soft_real_time();

   152

	rt_thread_delete(timerloop_task);

   153

}

   154

   155

/**

   156

 * Create the Timer Task

   157

 * @param _init_callback

   158

 */

   159

void StartTimerLoop(TimerCallback_t _init_callback)

   160

{

   161

	stop_timer = 0;	

   162

	init_callback = _init_callback;

   163

   164

	/* start timerloop_task ( do nothing and get blocked ) */

   165

	timerloop_thr = rt_thread_create(timerloop_task_proc, NULL, 0);

   166

}

   167

   168

/* We assume that ReceiveLoop_task_proc is always the same */

   169

static void (*rtai_ReceiveLoop_task_proc)(CAN_PORT) = NULL;

   170

   171

/**

   172

 * Enter in realtime and start the CAN receiver loop

   173

 * @param port

   174

 */

   175

void rtai_canReceiveLoop(CAN_PORT port)

   176

{

   177

	RT_TASK *current_task;

   178

	mlockall(MCL_CURRENT | MCL_FUTURE);

   179

	current_task = rt_thread_init(rt_get_name(0), 0, 0, SCHED_FIFO, 1);

   180

	rt_make_hard_real_time();

   181

   182

	rt_sem_signal(control_task);

   183

   184

	/* Call original receive loop with port struct as a param */

   185

	rtai_ReceiveLoop_task_proc(port);

   186

   187

	rt_make_soft_real_time();

   188

	rt_thread_delete(current_task);

   189

}

   190

   191

/**

   192

 * Create the CAN Receiver Task

   193

 * @param fd0 CAN port

   194

 * @param *ReceiveLoop_thread CAN receiver thread

   195

 * @param *ReceiveLoop_task_proc CAN receiver task

   196

 */

   197

void CreateReceiveTask(CAN_PORT fd0, TASK_HANDLE *ReceiveLoop_thread, void* ReceiveLoop_task_proc)

   198

{	

   199

	rtai_ReceiveLoop_task_proc = ReceiveLoop_task_proc;

   200

	*ReceiveLoop_thread = rt_thread_create(rtai_canReceiveLoop, (void*)fd0, 0); 

   201

	rt_sem_wait(control_task);

   202

}

   203

   204

/**

   205

 * Wait for the CAN Receiver Task end

   206

 * @param *ReceiveLoop_thread CAN receiver thread

   207

 */

   208

void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_thread)

   209

{

   210

	rt_thread_join(*ReceiveLoop_thread);

   211

}

   212

   213

/**

   214

 * Set timer for the next wakeup

   215

 * @param value

   216

 */

   217

void setTimer(TIMEVAL value)

   218

{

   219

	rt_sem_wait(condition_mutex);

   220

	last_timeout_set = value;

   221

	rt_sem_signal(condition_mutex);

   222

	rt_cond_signal(timer_set);

   223

}

   224

   225

/**

   226

 * Get the elapsed time since the last alarm

   227

 * @return a time in nanoseconds

   228

 */

   229

TIMEVAL getElapsedTime(void)

   230

{

   231

	RTIME res;

   232

	rt_sem_wait(condition_mutex);

   233

	last_time_read = rt_get_time();

   234

	res = last_time_read - last_occured_alarm;

   235

	rt_sem_signal(condition_mutex);

   236

	return res;

   237

}