1

#include <stdlib.h>

     2

#include <unistd.h>

     3

#include <sys/mman.h>

     4

     5

#include <alchemy/task.h>

     6

#include <alchemy/timer.h>

     7

#include <alchemy/sem.h>

     8

#include <alchemy/mutex.h>

     9

#include <alchemy/cond.h>

    10

#include <alchemy/alarm.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

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

    68

/**

    69

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

    70

 */

    71

void TimerCleanup(void)

    72

{

    73

	rt_sem_delete(&CanFestival_mutex);

    74

	rt_mutex_delete(&condition_mutex);

    75

	rt_cond_delete(&timer_set);

    76

	rt_sem_delete(&control_task);

    78

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

    79

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

    80

	}

    81

	rt_task_delete(&timerloop_task);

    82

}

    83

    84

/**

    85

 * Take a semaphore

    86

 */

    87

void EnterMutex(void)

    88

{

    89

	rt_sem_p(&CanFestival_mutex, TM_INFINITE);

    90

}

    91

    92

/**

    93

 * Signaling a semaphore

    94

 */

    95

void LeaveMutex(void)

    96

{

    97

	rt_sem_v(&CanFestival_mutex);

    98

}

    99

   100

static TimerCallback_t init_callback;

   101

   102

/**

   103

 * Timer Task

   104

 */

   105

void timerloop_task_proc(void *arg)

   106

{

   107

	int ret = 0;

   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

   154

/**

   155

 * Create the Timer Task

   156

 * @param _init_callback

   157

 */

   158

void StartTimerLoop(TimerCallback_t _init_callback)

   159

{

   160

	int ret = 0;

   161

	stop_timer = 0;	

   162

	init_callback = _init_callback;

   163

   164

	char taskname[32];

   165

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

   166

   167

	/* create timerloop_task */

   168

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

   169

	if (ret) {

   170

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

   171

		return;

   172

	}

   173

   174

	/* start timerloop_task */

   175

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

   176

	if (ret) {

   177

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

   178

		goto error;

   179

	}

   180

   181

	return;

   182

   183

error:

   185

}

   186

   187

/**

   188

 * Create the CAN Receiver Task

   189

 * @param fd0 CAN port

   190

 * @param *ReceiveLoop_task CAN receiver task

   191

 * @param *ReceiveLoop_task_proc CAN receiver function

   192

 */

   193

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

   194

{	

   195

	int ret;

   196

	static int id = 0;

   197

	char taskname[32];

   198

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

   199

	id++;

   200

   201

	/* create ReceiveLoop_task */

   202

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

   203

	if (ret) {

   204

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

   205

		return;

   206

	}

   207

	/* start ReceiveLoop_task */

   208

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

   209

	if (ret) {

   210

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

   211

		return;

   212

	}

   213

	rt_sem_v(&control_task);

   214

}

   215

   216

/**

   217

 * Wait for the CAN Receiver Task end

   218

 * @param *ReceiveLoop_task CAN receiver thread

   219

 */

   220

void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_task)

   221

{

   223

	if (rt_task_join(ReceiveLoop_task) != 0){

   224

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

   225

	}

   226

	rt_task_delete(ReceiveLoop_task);

   227

}

   228

   229

/**

   230

 * Set timer for the next wakeup

   231

 * @param value

   232

 */

   233

void setTimer(TIMEVAL value)

   234

{

   235

	rt_mutex_acquire(&condition_mutex, TM_INFINITE);

   236

	last_timeout_set = value;

   237

	rt_mutex_release(&condition_mutex);

   238

	rt_cond_signal(&timer_set);

   239

}

   240

   241

/**

   242

 * Get the elapsed time since the last alarm

   243

 * @return a time in nanoseconds

   244

 */

   245

TIMEVAL getElapsedTime(void)

   246

{

   247

	RTIME res;

   248

	rt_mutex_acquire(&condition_mutex, TM_INFINITE);

   249

	last_time_read = rt_timer_read();

   250

	res = last_time_read - last_occured_alarm;

   251

	rt_mutex_release(&condition_mutex);

   252

	return res;

   253

}