1

/*

     2

This file is part of CanFestival, a library implementing CanOpen Stack.

     3

     4

Copyright (C): Cosateq GmbH & Co.KG

     5

               http://www.cosateq.com/

     6

               http://www.scale-rt.com/

     7

     8

See COPYING file for copyrights details.

     9

    10

This library is free software; you can redistribute it and/or

    11

modify it under the terms of the GNU Lesser General Public

    12

License as published by the Free Software Foundation; either

    13

version 2.1 of the License, or (at your option) any later version.

    14

    15

This library is distributed in the hope that it will be useful,

    16

but WITHOUT ANY WARRANTY; without even the implied warranty of

    17

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

    18

Lesser General Public License for more details.

    19

    20

You should have received a copy of the GNU Lesser General Public

    21

License along with this library; if not, write to the Free Software

    22

Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

    23

*/

    24

    25

#include <asm/current.h>

    26

    27

#include <native/task.h>

    28

#include <native/timer.h>

    29

#include <native/sem.h>

    30

#include <native/mutex.h>

    31

#include <native/cond.h>

    32

#include <native/alarm.h>

    33

    34

#include "applicfg.h"

    35

#include "can_driver.h"

    36

#include "timer.h"

    37

    38

#define TIMERLOOP_TASK_CREATED        1

    39

    40

TimerCallback_t exitall;

    41

    42

RT_MUTEX condition_mutex;

    43

RT_SEM CanFestival_mutex;

    44

RT_SEM control_task;

    45

RT_COND timer_set;

    46

RT_TASK timerloop_task;

    47

    48

RTIME last_time_read;

    49

RTIME last_occured_alarm;

    50

RTIME last_timeout_set;

    51

    52

int stop_timer = 0;

    53

    54

/**

    55

 * Init Mutex, Semaphores and Condition variable

    56

 */

    57

void TimerInit(void)

    58

{

    59

  	int ret = 0;

    60

  	char taskname[32];

    61

    62

	// lock process in to RAM

    63

  	//mlockall(MCL_CURRENT | MCL_FUTURE);

    64

    65

  	snprintf(taskname, sizeof(taskname), "S1-%d", current->pid);

    66

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

    67

    68

  	snprintf(taskname, sizeof(taskname), "S2-%d", current->pid);

    69

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

    70

    71

  	snprintf(taskname, sizeof(taskname), "M1-%d", current->pid);

    72

  	rt_mutex_create(&condition_mutex, taskname);

    73

    74

  	snprintf(taskname, sizeof(taskname), "C1-%d", current->pid);

    75

  	rt_cond_create(&timer_set, taskname);

    76

}

    77

    78

/**

    79

 * Stop Timer Task

    80

 * @param exitfunction

    81

 */

    82

void StopTimerLoop(TimerCallback_t exitfunction)

    83

{

    84

	exitall = exitfunction;

    85

	stop_timer = 1;

    86

	rt_cond_signal(&timer_set);

    87

}

    88

    89

void cleanup_all(void)

    90

{

    91

	/* normally this will fail with a non-periodic task that has already ended at this time */

    92

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

    93

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

    94

	}

    95

	rt_task_delete(&timerloop_task);

    96

}

    97

    98

/**

    99

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

   100

 */

   101

void TimerCleanup(void)

   102

{

   103

	rt_sem_delete(&CanFestival_mutex);

   104

	rt_mutex_delete(&condition_mutex);

   105

	rt_cond_delete(&timer_set);

   106

	rt_sem_delete(&control_task);

   107

   108

	/* normally this will fail with a non-periodic task that has already ended at this time */

   109

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

   110

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

   111

	}

   112

	rt_task_delete(&timerloop_task);

   113

}

   114

   115

/**

   116

 * Take a semaphore

   117

 */

   118

void EnterMutex(void)

   119

{

   120

	rt_sem_p(&CanFestival_mutex, TM_INFINITE);

   121

}

   122

   123

/**

   124

 * Signaling a semaphore

   125

 */

   126

void LeaveMutex(void)

   127

{

   128

	rt_sem_v(&CanFestival_mutex);

   129

}

   130

   131

static TimerCallback_t init_callback;

   132

   133

/**

   134

 * Timer Task

   135

 */

   136

void timerloop_task_proc(void *arg)

   137

{

   138

	int ret = 0;

   139

   140

	getElapsedTime();

   141

	last_timeout_set = 0;

   142

	last_occured_alarm = last_time_read;

   143

   144

	/* trigger first alarm */

   145

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

   146

	RTIME current_time;

   147

	RTIME real_alarm;

   148

	do{

   149

   150

		rt_mutex_acquire(&condition_mutex, TM_INFINITE);

   151

		if(last_timeout_set == TIMEVAL_MAX)

   152

		{

   153

			ret = rt_cond_wait(

   154

				&timer_set,

   155

				&condition_mutex,

   156

				TM_INFINITE

   157

				);		/* Then sleep until next message*/

   158

			rt_mutex_release(&condition_mutex);

   159

		}else{

   160

			current_time = rt_timer_read();

   161

			real_alarm = last_time_read + last_timeout_set;

   162

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

   163

				&timer_set,

   164

				&condition_mutex,

   165

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

   166

			if(ret == -ETIMEDOUT){

   167

				last_occured_alarm = real_alarm;

   168

				rt_mutex_release(&condition_mutex);

   169

				EnterMutex();

   170

				TimeDispatch();

   171

				LeaveMutex();

   172

			}else{

   173

				rt_mutex_release(&condition_mutex);

   174

			}

   175

		}

   176

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

   177

   178

	if(exitall){

   179

		EnterMutex();

   180

		exitall(NULL,0);

   181

		LeaveMutex();

   182

	}

   183

}

   184

   185

/**

   186

 * Create the Timer Task

   187

 * @param _init_callback

   188

 */

   189

void StartTimerLoop(TimerCallback_t _init_callback)

   190

{

   191

	int ret = 0;

   192

	stop_timer = 0;

   193

	init_callback = _init_callback;

   194

   195

	char taskname[32];

   196

	snprintf(taskname, sizeof(taskname), "timerloop-%d", current->pid);

   197

   198

	/* create timerloop_task */

   199

	ret = rt_task_create(&timerloop_task, taskname, 0, 50, 0); /* T_JOINABLE only in user space */

   200

	if (ret) {

   201

		printk("Failed to create timerloop_task, code %d\n",ret);

   202

		return;

   203

	}

   204

   205

	/* start timerloop_task */

   206

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

   207

	if (ret) {

   208

		printk("Failed to start timerloop_task, code %u\n",ret);

   209

		goto error;

   210

	}

   211

   212

	return;

   213

   214

error:

   215

	cleanup_all();

   216

}

   217

   218

/**

   219

 * Create the CAN Receiver Task

   220

 * @param fd0 CAN port

   221

 * @param *ReceiveLoop_task CAN receiver task

   222

 * @param *ReceiveLoop_task_proc CAN receiver function

   223

 */

   224

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

   225

{

   226

	int ret;

   227

	static int id = 0;

   228

	char taskname[32];

   229

	snprintf(taskname, sizeof(taskname), "canloop%d-%d", id, current->pid);

   230

	id++;

   231

   232

	/* create ReceiveLoop_task */

   233

	ret = rt_task_create(ReceiveLoop_task,taskname,0,50,0); /* T_JOINABLE only in user space */

   234

	if (ret) {

   235

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

   236

		return;

   237

	}

   238

   239

	/* periodic task for Xenomai kernel realtime */

   240

	rt_task_set_periodic(ReceiveLoop_task, 0, 1 * 1000 * 1000); /* 1ms */

   241

   242

	/* start ReceiveLoop_task */

   243

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

   244

	if (ret) {

   245

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

   246

		return;

   247

	}

   248

	rt_sem_v(&control_task);

   249

}

   250

   251

/**

   252

 * Wait for the CAN Receiver Task end

   253

 * @param *ReceiveLoop_task CAN receiver thread

   254

 */

   255

void WaitReceiveTaskEnd(TASK_HANDLE *ReceiveLoop_task)

   256

{

   257

	/* normally this will fail with a non-periodic task that has already ended at this time */

   258

	if (rt_task_suspend(ReceiveLoop_task) != 0){

   259

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

   260

	}

   261

	rt_task_delete(ReceiveLoop_task);

   262

}

   263

   264

/**

   265

 * Set timer for the next wakeup

   266

 * @param value

   267

 */

   268

void setTimer(TIMEVAL value)

   269

{

   270

	rt_mutex_acquire(&condition_mutex, TM_INFINITE);

   271

	last_timeout_set = value;

   272

	rt_mutex_release(&condition_mutex);

   273

	rt_cond_signal(&timer_set);

   274

}

   275

   276

/**

   277

 * Get the elapsed time since the last alarm

   278

 * @return a time in nanoseconds

   279

 */

   280

TIMEVAL getElapsedTime(void)

   281

{

   282

	RTIME res;

   283

	rt_mutex_acquire(&condition_mutex, TM_INFINITE);

   284

	last_time_read = rt_timer_read();

   285

	res = last_time_read - last_occured_alarm;

   286

	rt_mutex_release(&condition_mutex);

   287

	return res;

   288

}