1

#include <stdlib.h>

     2

#include <unistd.h>

     3

#include <sys/mman.h>

     4

     5

#include <native/task.h>

     6

#include <native/timer.h>

     7

#include <native/sem.h>

     8

#include <native/mutex.h>

     9

#include <native/cond.h>

    10

#include <native/alarm.h>

    11

    12

#include "applicfg.h"

    13

#include "can_driver.h"

    14

#include "timer.h"

    15

    16

#define TIMERLOOP_TASK_CREATED        1

    17

    18

TimerCallback_t exitall;

    19

    20

RT_MUTEX condition_mutex;

    21

RT_SEM CanFestival_mutex;

    22

RT_SEM control_task; 

    23

RT_COND timer_set;	

    24

RT_TASK timerloop_task;

    25

    26

RTIME last_time_read;

    27

RTIME last_occured_alarm;

    28

RTIME last_timeout_set;

    29

    30

int stop_timer = 0;

    31

    32

/**

    33

 * Init Mutex, Semaphores and Condition variable

    34

 */

    35

void TimerInit(void)

    36

{

    37

  	int ret = 0;

    38

  	char taskname[32];

    39

    40

  	mlockall(MCL_CURRENT | MCL_FUTURE);

    41

    42

  	snprintf(taskname, sizeof(taskname), "S1-%d", getpid());

    43

	rt_sem_create(&CanFestival_mutex, taskname, 1, S_FIFO);

    44

    45

  	snprintf(taskname, sizeof(taskname), "S2-%d", getpid());

    46

  	rt_sem_create(&control_task, taskname, 0, S_FIFO);

    47

    48

  	snprintf(taskname, sizeof(taskname), "M1-%d", getpid());

    49

  	rt_mutex_create(&condition_mutex, taskname);

    50

    51

  	snprintf(taskname, sizeof(taskname), "C1-%d", getpid());

    52

  	rt_cond_create(&timer_set, taskname);

    53

}

    54

    55

/**

    56

 * Stop Timer Task

    57

 * @param exitfunction

    58

 */

    59

void StopTimerLoop(TimerCallback_t exitfunction)

    60

{

    61

	exitall = exitfunction;

    62

	stop_timer = 1;

    63

	rt_cond_signal(&timer_set);

    64

}

    65

    66

void cleanup_all(void)

    67

{

    68

	rt_task_delete(&timerloop_task);

    69

}

    70

    71

/**

    72

 * Clean all Semaphores, mutex, condition variable and main task

    73

 */

    74

void TimerCleanup(void)

    75

{

    76

	rt_sem_delete(&CanFestival_mutex);

    77

	rt_mutex_delete(&condition_mutex);

    78

	rt_cond_delete(&timer_set);

    79

	rt_sem_delete(&control_task);

    80

}

    81

    82

/**

    83

 * Take a semaphore

    84

 */

    85

void EnterMutex(void)

    86

{

    87

	rt_sem_p(&CanFestival_mutex, TM_INFINITE);

    88

}

    89

    90

/**

    91

 * Signaling a semaphore

    92

 */

    93

void LeaveMutex(void)

    94

{

    95

	rt_sem_v(&CanFestival_mutex);

    96

}

    97

    98

static TimerCallback_t init_callback;

    99

   100

/**

   101

 * Timer Task

   102

 */

   103

void timerloop_task_proc(void *arg)

   104

{

   105

	int ret = 0;

   106

  	// lock process in to RAM

   107

  	mlockall(MCL_CURRENT | MCL_FUTURE);

   108

   109

	getElapsedTime();

   110

	last_timeout_set = 0;

   111

	last_occured_alarm = last_time_read;

   112

   113

	/* trigger first alarm */

   114

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

   115

	RTIME current_time;

   116

	RTIME real_alarm;

   117

	do{

   118

   119

		rt_mutex_acquire(&condition_mutex, TM_INFINITE);

   120

		if(last_timeout_set == TIMEVAL_MAX)

   121

		{

   122

			ret = rt_cond_wait(

   123

				&timer_set,

   124

				&condition_mutex,

   125

				TM_INFINITE

   126

				);		/* Then sleep until next message*/

   127

			rt_mutex_release(&condition_mutex);

   128

		}else{

   129

			current_time = rt_timer_read();

   130

			real_alarm = last_time_read + last_timeout_set;

   131

			ret = rt_cond_wait( /* sleep until next deadline */

   132

				&timer_set,

   133

				&condition_mutex,

   134

				(real_alarm - current_time)); /* else alarm consider expired */   

   135

			if(ret = -ETIMEDOUT){

   136

				last_occured_alarm = real_alarm;

   137

				rt_mutex_release(&condition_mutex);

   138

				EnterMutex();

   139

				TimeDispatch();

   140

				LeaveMutex();

   141

			}else{ 

   142

				rt_mutex_release(&condition_mutex);

   143

			}

   144

		}

   145

	}while ((ret == 0 || ret == -EINTR || ret == -ETIMEDOUT) && !stop_timer);

   146

   147

	if(exitall){

   148

		EnterMutex();

   149

		exitall(NULL,0);

   150

		LeaveMutex();

   151

	}

   152

   153

	rt_task_delete(&timerloop_task);

   154

}

   155

   156

/**

   157

 * Create the Timer Task

   158

 * @param _init_callback

   159

 */

   160

void StartTimerLoop(TimerCallback_t _init_callback)

   161

{

   162

	int ret = 0;

   163

	stop_timer = 0;	

   164

	init_callback = _init_callback;

   165

   166

	char taskname[32];

   167

	snprintf(taskname, sizeof(taskname), "timerloop-%d", getpid());

   168

   169

	/* create timerloop_task */

   170

	ret = rt_task_create(&timerloop_task, taskname, 0, 50, 0);

   171

	if (ret) {

   172

		printf("Failed to create timerloop_task, code %d\n",errno);

   173

		return;

   174

	}

   175

   176

	/* start timerloop_task */

   177

	ret = rt_task_start(&timerloop_task,&timerloop_task_proc,NULL);

   178

	if (ret) {

   179

		printf("Failed to start timerloop_task, code %u\n",errno);

   180

		goto error;

   181

	}

   182

   183

	return;

   184

   185

error:

   186

	cleanup_all();

   187

}

   188

   189

/**

   190

 * Create the CAN Receiver Task

   191

 * @param fd0 CAN port

   192

 * @param *ReceiveLoop_task CAN receiver task

   193

 * @param *ReceiveLoop_task_proc CAN receiver function

   194

 */

   195

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

   196

{	

   197

	int ret;

   198

	static int id = 0;

   199

	char taskname[32];

   200

	snprintf(taskname, sizeof(taskname), "canloop%d-%d", id, getpid());

   201

	id++;

   202

   203

	/* create ReceiveLoop_task */

   204

	ret = rt_task_create(ReceiveLoop_task,taskname,0,50,0);

   205

	if (ret) {

   206

		printf("Failed to create ReceiveLoop_task number %d, code %d\n", id, errno);

   207

		return;

   208

	}

   209

	/* start ReceiveLoop_task */

   210

	ret = rt_task_start(ReceiveLoop_task, ReceiveLoop_task_proc,(void*)fd0);

   211

	if (ret) {

   212

		printf("Failed to start ReceiveLoop_task number %d, code %d\n", id, errno);

   213

		return;

   214

	}

   215

	rt_sem_v(&control_task);

   216

}

   217

   218

/**

   219

 * Wait for the CAN Receiver Task end

   220

 * @param *ReceiveLoop_task CAN receiver thread

   221

 */

   222

void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_task)

   223

{

   224

	rt_task_delete(ReceiveLoop_task);

   225

}

   226

   227

/**

   228

 * Set timer for the next wakeup

   229

 * @param value

   230

 */

   231

void setTimer(TIMEVAL value)

   232

{

   233

	rt_mutex_acquire(&condition_mutex, TM_INFINITE);

   234

	last_timeout_set = (value == TIMEVAL_MAX) ? TIMEVAL_MAX : value;

   235

	rt_mutex_release(&condition_mutex);

   236

	rt_cond_signal(&timer_set);

   237

}

   238

   239

/**

   240

 * Get the elapsed time since the last alarm

   241

 * @return a time in nanoseconds

   242

 */

   243

TIMEVAL getElapsedTime(void)

   244

{

   245

	RTIME res;

   246

	rt_mutex_acquire(&condition_mutex, TM_INFINITE);

   247

	last_time_read = rt_timer_read();

   248

	res = last_time_read - last_occured_alarm;

   249

	rt_mutex_release(&condition_mutex);

   250

	return res;

   251

}