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

	// lock process in to RAM

    41

  	mlockall(MCL_CURRENT | MCL_FUTURE);

    42

    43

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

    44

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

    45

    46

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

    47

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

    48

    49

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

    50

  	rt_mutex_create(&condition_mutex, taskname);

    51

    52

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

    53

  	rt_cond_create(&timer_set, taskname);

    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

void cleanup_all(void)

    68

{

    69

	if (rt_task_join(&timerloop_task) != 0){

    70

		printf("Failed to join with Timerloop task\n");

    71

	}

    72

	rt_task_delete(&timerloop_task);

    73

}

    74

    75

/**

    76

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

    77

 */

    78

void TimerCleanup(void)

    79

{

    80

	rt_sem_delete(&CanFestival_mutex);

    81

	rt_mutex_delete(&condition_mutex);

    82

	rt_cond_delete(&timer_set);

    83

	rt_sem_delete(&control_task);

    84

	if (rt_task_join(&timerloop_task) != 0){

    85

		printf("Failed to join with Timerloop task\n");

    86

	}

    87

	rt_task_delete(&timerloop_task);

    88

}

    89

    90

/**

    91

 * Take a semaphore

    92

 */

    93

void EnterMutex(void)

    94

{

    95

	rt_sem_p(&CanFestival_mutex, TM_INFINITE);

    96

}

    97

    98

/**

    99

 * Signaling a semaphore

   100

 */

   101

void LeaveMutex(void)

   102

{

   103

	rt_sem_v(&CanFestival_mutex);

   104

}

   105

   106

static TimerCallback_t init_callback;

   107

   108

/**

   109

 * Timer Task

   110

 */

   111

void timerloop_task_proc(void *arg)

   112

{

   113

	int ret = 0;

   114

   115

	getElapsedTime();

   116

	last_timeout_set = 0;

   117

	last_occured_alarm = last_time_read;

   118

   119

	/* trigger first alarm */

   120

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

   121

	RTIME current_time;

   122

	RTIME real_alarm;

   123

	do{

   124

   125

		rt_mutex_acquire(&condition_mutex, TM_INFINITE);

   126

		if(last_timeout_set == TIMEVAL_MAX)

   127

		{

   128

			ret = rt_cond_wait(

   129

				&timer_set,

   130

				&condition_mutex,

   131

				TM_INFINITE

   132

				);		/* Then sleep until next message*/

   133

			rt_mutex_release(&condition_mutex);

   134

		}else{

   135

			current_time = rt_timer_read();

   136

			real_alarm = last_time_read + last_timeout_set;

   137

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

   138

				&timer_set,

   139

				&condition_mutex,

   140

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

   141

			if(ret == -ETIMEDOUT){

   142

				last_occured_alarm = real_alarm;

   143

				rt_mutex_release(&condition_mutex);

   144

				EnterMutex();

   145

				TimeDispatch();

   146

				LeaveMutex();

   147

			}else{ 

   148

				rt_mutex_release(&condition_mutex);

   149

			}

   150

		}

   151

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

   152

   153

	if(exitall){

   154

		EnterMutex();

   155

		exitall(NULL,0);

   156

		LeaveMutex();

   157

	}

   158

}

   159

   160

/**

   161

 * Create the Timer Task

   162

 * @param _init_callback

   163

 */

   164

void StartTimerLoop(TimerCallback_t _init_callback)

   165

{

   166

	int ret = 0;

   167

	stop_timer = 0;	

   168

	init_callback = _init_callback;

   169

   170

	char taskname[32];

   171

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

   172

   173

	/* create timerloop_task */

   174

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

   175

	if (ret) {

   176

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

   177

		return;

   178

	}

   179

   180

	/* start timerloop_task */

   181

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

   182

	if (ret) {

   183

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

   184

		goto error;

   185

	}

   186

   187

	return;

   188

   189

error:

   190

	cleanup_all();

   191

}

   192

   193

/**

   194

 * Create the CAN Receiver Task

   195

 * @param fd0 CAN port

   196

 * @param *ReceiveLoop_task CAN receiver task

   197

 * @param *ReceiveLoop_task_proc CAN receiver function

   198

 */

   199

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

   200

{	

   201

	int ret;

   202

	static int id = 0;

   203

	char taskname[32];

   204

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

   205

	id++;

   206

   207

	/* create ReceiveLoop_task */

   208

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

   209

	if (ret) {

   210

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

   211

		return;

   212

	}

   213

	/* start ReceiveLoop_task */

   214

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

   215

	if (ret) {

   216

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

   217

		return;

   218

	}

   219

	rt_sem_v(&control_task);

   220

}

   221

   222

/**

   223

 * Wait for the CAN Receiver Task end

   224

 * @param *ReceiveLoop_task CAN receiver thread

   225

 */

   226

void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_task)

   227

{

   228

	if (rt_task_join(ReceiveLoop_task) != 0){

   229

		printf("Failed to join with Receive task\n");

   230

	}

   231

	rt_task_delete(ReceiveLoop_task);

   232

}

   233

   234

/**

   235

 * Set timer for the next wakeup

   236

 * @param value

   237

 */

   238

void setTimer(TIMEVAL value)

   239

{

   240

	rt_mutex_acquire(&condition_mutex, TM_INFINITE);

   241

	last_timeout_set = value;

   242

	rt_mutex_release(&condition_mutex);

   243

	rt_cond_signal(&timer_set);

   244

}

   245

   246

/**

   247

 * Get the elapsed time since the last alarm

   248

 * @return a time in nanoseconds

   249

 */

   250

TIMEVAL getElapsedTime(void)

   251

{

   252

	RTIME res;

   253

	rt_mutex_acquire(&condition_mutex, TM_INFINITE);

   254

	last_time_read = rt_timer_read();

   255

	res = last_time_read - last_occured_alarm;

   256

	rt_mutex_release(&condition_mutex);

   257

	return res;

   258

}