mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
This version runs stably on all three platforms for me. Corvus, please feel free to revert these changes if you want or have a better solution. Also only Mac tested with GCS, the rest just sit there running nicely
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1062 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124
peabody124
parent
9f2a01a78a
commit
dd598a75a4
@ -65,6 +65,8 @@
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <limits.h>
|
||||
#include <assert.h>
|
||||
|
||||
|
||||
/* Scheduler includes. */
|
||||
#include "FreeRTOS.h"
|
||||
@ -74,6 +76,23 @@
|
||||
#define MAX_NUMBER_OF_TASKS ( _POSIX_THREAD_THREADS_MAX )
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
#ifndef __DARWIN__
|
||||
#define COND_SIGNALING
|
||||
#define CHECK_TASK_RESUMES
|
||||
#define RUNNING_THREAD_MUTEX
|
||||
#define TICK_SIGNAL
|
||||
#endif
|
||||
#ifdef __CYGWIN__
|
||||
#define COND_SIGNALING
|
||||
#define CHECK_TASK_RESUMES
|
||||
#endif
|
||||
#ifdef linux
|
||||
#define COND_SIGNALING
|
||||
#define CHECK_TASK_RESUMES
|
||||
#define RUNNING_THREAD_MUTEX
|
||||
#endif
|
||||
|
||||
|
||||
/* Parameters to pass to the newly created pthread. */
|
||||
typedef struct XPARAMS
|
||||
{
|
||||
@ -85,7 +104,10 @@ typedef struct XPARAMS
|
||||
typedef struct THREAD_SUSPENSIONS
|
||||
{
|
||||
pthread_t hThread;
|
||||
pthread_cond_t * hCond;
|
||||
pthread_mutex_t * hMutex;
|
||||
xTaskHandle hTask;
|
||||
portBASE_TYPE xThreadState;
|
||||
unsigned portBASE_TYPE uxCriticalNesting;
|
||||
} xThreadState;
|
||||
/*-----------------------------------------------------------*/
|
||||
@ -93,12 +115,19 @@ typedef struct THREAD_SUSPENSIONS
|
||||
static xThreadState *pxThreads;
|
||||
static pthread_once_t hSigSetupThread = PTHREAD_ONCE_INIT;
|
||||
static pthread_attr_t xThreadAttributes;
|
||||
#ifdef RUNNING_THREAD_MUTEX
|
||||
static pthread_mutex_t xRunningThread = PTHREAD_MUTEX_INITIALIZER;
|
||||
#endif
|
||||
static pthread_mutex_t xSuspendResumeThreadMutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
static pthread_mutex_t xSingleThreadMutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
static pthread_mutex_t xSwappingThreadMutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
static pthread_t hMainThread = ( pthread_t )NULL;
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
static volatile portBASE_TYPE xSentinel = 0;
|
||||
static volatile portBASE_TYPE xRunning = pdFALSE;
|
||||
static volatile portBASE_TYPE xSuspended = pdFALSE;
|
||||
static volatile portBASE_TYPE xStarted = pdFALSE;
|
||||
static volatile portBASE_TYPE xHandover = 0;
|
||||
static volatile portBASE_TYPE xSchedulerEnd = pdFALSE;
|
||||
static volatile portBASE_TYPE xInterruptsEnabled = pdTRUE;
|
||||
static volatile portBASE_TYPE xServicingTick = pdFALSE;
|
||||
@ -110,14 +139,16 @@ static volatile unsigned portBASE_TYPE uxCriticalNesting;
|
||||
/*
|
||||
* Setup the timer to generate the tick interrupts.
|
||||
*/
|
||||
static void prvSetupTimerInterrupt( void );
|
||||
static void *prvWaitForStart( void * pvParams );
|
||||
static void prvSuspendSignalHandler(int sig);
|
||||
static void prvResumeSignalHandler(int sig);
|
||||
static void prvSetupSignalsAndSchedulerPolicy( void );
|
||||
static void prvSuspendThread( pthread_t xThreadId );
|
||||
static void prvResumeThread( pthread_t xThreadId );
|
||||
static void pauseThread( portBASE_TYPE pauseMode );
|
||||
static pthread_t prvGetThreadHandle( xTaskHandle hTask );
|
||||
#ifdef COND_SIGNALING
|
||||
static pthread_cond_t * prvGetConditionHandle( xTaskHandle hTask );
|
||||
static pthread_mutex_t * prvGetMutexHandle( xTaskHandle hTask );
|
||||
#endif
|
||||
static xTaskHandle prvGetTaskHandle( pthread_t hThread );
|
||||
static portLONG prvGetFreeThreadState( void );
|
||||
static void prvSetTaskCriticalNesting( pthread_t xThreadId, unsigned portBASE_TYPE uxNesting );
|
||||
static unsigned portBASE_TYPE prvGetTaskCriticalNesting( pthread_t xThreadId );
|
||||
@ -130,12 +161,110 @@ static void prvDeleteThread( void *xThreadId );
|
||||
void vPortYield( void );
|
||||
void vPortSystemTickHandler( int sig );
|
||||
|
||||
#define THREAD_PAUSE_CREATED 0
|
||||
#define THREAD_PAUSE_YIELD 1
|
||||
#define THREAD_PAUSE_INTERRUPT 2
|
||||
|
||||
//#define DEBUG_OUTPUT
|
||||
//#define ERROR_OUTPUT
|
||||
#ifdef DEBUG_OUTPUT
|
||||
|
||||
static pthread_mutex_t xPrintfMutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
|
||||
#define debug_printf(...) ( (real_pthread_mutex_lock( &xPrintfMutex )|1)?( \
|
||||
( \
|
||||
(NULL != (debug_task_handle = prvGetTaskHandle(pthread_self())) )? \
|
||||
(fprintf( stderr, "%20s(%li)\t%20s\t%i: ",debug_task_handle->pcTaskName,(long)pthread_self(),__func__,__LINE__)): \
|
||||
(fprintf( stderr, "%20s(%li)\t%20s\t%i: ","__unknown__",(long)pthread_self(),__func__,__LINE__)) \
|
||||
|1)?( \
|
||||
((fprintf( stderr, __VA_ARGS__ )|1)?real_pthread_mutex_unlock( &xPrintfMutex ):0) \
|
||||
):0 ):0 )
|
||||
|
||||
#define debug_error debug_printf
|
||||
|
||||
int real_pthread_mutex_lock(pthread_mutex_t* mutex) {
|
||||
return pthread_mutex_lock(mutex);
|
||||
}
|
||||
int real_pthread_mutex_unlock(pthread_mutex_t* mutex) {
|
||||
return pthread_mutex_unlock(mutex);
|
||||
}
|
||||
#define pthread_mutex_trylock(...) ( (debug_printf(" -!- pthread_mutex_trylock(%s)\n",#__VA_ARGS__)|1)?pthread_mutex_trylock(__VA_ARGS__):0 )
|
||||
#define pthread_mutex_lock(...) ( (debug_printf(" -!- pthread_mutex_lock(%s)\n",#__VA_ARGS__)|1)?pthread_mutex_lock(__VA_ARGS__):0 )
|
||||
#define pthread_mutex_unlock(...) ( (debug_printf(" -=- pthread_mutex_unlock(%s)\n",#__VA_ARGS__)|1)?pthread_mutex_unlock(__VA_ARGS__):0 )
|
||||
#define pthread_kill(thread,signal) ( (debug_printf("Sending signal %i to thread %li!\n",(int)signal,(long)thread)|1)?pthread_kill(thread,signal):0 )
|
||||
#define pthread_cond_signal( hCond ) (debug_printf( "pthread_cond_signals(%li)\r\n", *((long int *) hCond) ) ? 1 : pthread_cond_signal( hCond ) )
|
||||
#define pthread_cond_timedwait( hCond, hMutex, it ) (debug_printf( "pthread_cond_timedwait(%li,%li)\r\n", *((long int *) hCond), *((long int *) hMutex )) ? 1 : pthread_cond_timedwait( hCond, hMutex, it ) )
|
||||
#define pthread_sigmask( how, set, out ) (debug_printf( "pthread_sigmask( %i, %li )\r\n", how, *((long int*) set) ) ? 1 : pthread_sigmask( how, set, out ) )
|
||||
|
||||
#else
|
||||
#ifdef ERROR_OUTPUT
|
||||
static pthread_mutex_t xPrintfMutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
#define debug_error(...) ( (pthread_mutex_lock( &xPrintfMutex )|1)?( \
|
||||
( \
|
||||
(NULL != (debug_task_handle = prvGetTaskHandle(pthread_self())) )? \
|
||||
(fprintf( stderr, "%20s(%li)\t%20s\t%i: ",debug_task_handle->pcTaskName,(long)pthread_self(),__func__,__LINE__)): \
|
||||
(fprintf( stderr, "%20s(%li)\t%20s\t%i: ","__unknown__",(long)pthread_self(),__func__,__LINE__)) \
|
||||
|1)?( \
|
||||
((fprintf( stderr, __VA_ARGS__ )|1)?pthread_mutex_unlock( &xPrintfMutex ):0) \
|
||||
):0 ):0 )
|
||||
|
||||
#define debug_printf(...)
|
||||
#else
|
||||
#define debug_printf(...)
|
||||
#define debug_error(...)
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
||||
/*
|
||||
* Start first task is a separate function so it can be tested in isolation.
|
||||
*/
|
||||
void vPortStartFirstTask( void );
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
|
||||
typedef struct tskTaskControlBlock
|
||||
{
|
||||
volatile portSTACK_TYPE *pxTopOfStack; /*< Points to the location of the last item placed on the tasks stack. THIS MUST BE THE FIRST MEMBER OF THE STRUCT. */
|
||||
|
||||
#if ( portUSING_MPU_WRAPPERS == 1 )
|
||||
xMPU_SETTINGS xMPUSettings; /*< The MPU settings are defined as part of the port layer. THIS MUST BE THE SECOND MEMBER OF THE STRUCT. */
|
||||
#endif
|
||||
|
||||
xListItem xGenericListItem; /*< List item used to place the TCB in ready and blocked queues. */
|
||||
xListItem xEventListItem; /*< List item used to place the TCB in event lists. */
|
||||
unsigned portBASE_TYPE uxPriority; /*< The priority of the task where 0 is the lowest priority. */
|
||||
portSTACK_TYPE *pxStack; /*< Points to the start of the stack. */
|
||||
signed char pcTaskName[ configMAX_TASK_NAME_LEN ];/*< Descriptive name given to the task when created. Facilitates debugging only. */
|
||||
|
||||
#if ( portSTACK_GROWTH > 0 )
|
||||
portSTACK_TYPE *pxEndOfStack; /*< Used for stack overflow checking on architectures where the stack grows up from low memory. */
|
||||
#endif
|
||||
|
||||
#if ( portCRITICAL_NESTING_IN_TCB == 1 )
|
||||
unsigned portBASE_TYPE uxCriticalNesting;
|
||||
#endif
|
||||
|
||||
#if ( configUSE_TRACE_FACILITY == 1 )
|
||||
unsigned portBASE_TYPE uxTCBNumber; /*< This is used for tracing the scheduler and making debugging easier only. */
|
||||
#endif
|
||||
|
||||
#if ( configUSE_MUTEXES == 1 )
|
||||
unsigned portBASE_TYPE uxBasePriority; /*< The priority last assigned to the task - used by the priority inheritance mechanism. */
|
||||
#endif
|
||||
|
||||
#if ( configUSE_APPLICATION_TASK_TAG == 1 )
|
||||
pdTASK_HOOK_CODE pxTaskTag;
|
||||
#endif
|
||||
|
||||
#if ( configGENERATE_RUN_TIME_STATS == 1 )
|
||||
unsigned long ulRunTimeCounter; /*< Used for calculating how much CPU time each task is utilising. */
|
||||
#endif
|
||||
|
||||
} tskTCB;
|
||||
|
||||
tskTCB *debug_task_handle;
|
||||
|
||||
/*
|
||||
* See header file for description.
|
||||
*/
|
||||
@ -144,12 +273,14 @@ portSTACK_TYPE *pxPortInitialiseStack( portSTACK_TYPE *pxTopOfStack, pdTASK_CODE
|
||||
/* Should actually keep this struct on the stack. */
|
||||
xParams *pxThisThreadParams = pvPortMalloc( sizeof( xParams ) );
|
||||
|
||||
debug_printf("pxPortInitialiseStack\r\n");
|
||||
|
||||
(void)pthread_once( &hSigSetupThread, prvSetupSignalsAndSchedulerPolicy );
|
||||
|
||||
if ( (pthread_t)NULL == hMainThread )
|
||||
{
|
||||
hMainThread = pthread_self();
|
||||
}
|
||||
}
|
||||
|
||||
/* No need to join the threads. */
|
||||
pthread_attr_init( &xThreadAttributes );
|
||||
@ -163,21 +294,32 @@ xParams *pxThisThreadParams = pvPortMalloc( sizeof( xParams ) );
|
||||
|
||||
lIndexOfLastAddedTask = prvGetFreeThreadState();
|
||||
|
||||
/* Create the new pThread. */
|
||||
if ( 0 == pthread_mutex_lock( &xSingleThreadMutex ) )
|
||||
{
|
||||
xSentinel = 0;
|
||||
if ( 0 != pthread_create( &( pxThreads[ lIndexOfLastAddedTask ].hThread ), &xThreadAttributes, prvWaitForStart, (void *)pxThisThreadParams ) )
|
||||
{
|
||||
/* Thread create failed, signal the failure */
|
||||
pxTopOfStack = 0;
|
||||
}
|
||||
debug_printf( "Got index for new task %i\r\n", lIndexOfLastAddedTask );
|
||||
|
||||
#ifdef COND_SIGNALING
|
||||
/* Create a condition signal for this thread */
|
||||
// pthread_condattr_t condAttr;
|
||||
// assert( 0 == pthread_condattr_init( &condAttr ) );
|
||||
pxThreads[ lIndexOfLastAddedTask ].hCond = ( pthread_cond_t *) malloc( sizeof( pthread_cond_t ) );
|
||||
assert( 0 == pthread_cond_init( pxThreads[ lIndexOfLastAddedTask ].hCond , NULL ) ); //&condAttr ) );
|
||||
debug_printf("Cond: %li\r\n", *( (long int *) &pxThreads[ lIndexOfLastAddedTask ].hCond) );
|
||||
|
||||
/* Create a condition mutex for this thread */
|
||||
// pthread_mutexattr_t mutexAttr;
|
||||
// assert( 0 == pthread_mutexattr_init( &mutexAttr ) );
|
||||
// assert( 0 == pthread_mutexattr_settype( &mutexAttr, PTHREAD_MUTEX_ERRORCHECK ) );
|
||||
pxThreads[ lIndexOfLastAddedTask ].hMutex = ( pthread_mutex_t *) malloc( sizeof( pthread_mutex_t ) );
|
||||
assert( 0 == pthread_mutex_init( pxThreads[ lIndexOfLastAddedTask ].hMutex, NULL ) ); //&mutexAttr ) );
|
||||
debug_printf("Mutex: %li\r\n", *( (long int *) &pxThreads[ lIndexOfLastAddedTask ].hMutex) );
|
||||
#endif
|
||||
|
||||
/* Create a thread and store it's handle number */
|
||||
xSentinel = 0;
|
||||
assert( 0 == pthread_create( &( pxThreads[ lIndexOfLastAddedTask ].hThread ), &xThreadAttributes, prvWaitForStart, (void *)pxThisThreadParams ) );
|
||||
|
||||
/* Wait until the task suspends. */
|
||||
(void)pthread_mutex_unlock( &xSingleThreadMutex );
|
||||
while ( xSentinel == 0 );
|
||||
vPortExitCritical();
|
||||
}
|
||||
/* Wait until the task suspends. */
|
||||
while ( xSentinel == 0 );
|
||||
vPortExitCritical();
|
||||
|
||||
return pxTopOfStack;
|
||||
}
|
||||
@ -188,11 +330,17 @@ void vPortStartFirstTask( void )
|
||||
/* Initialise the critical nesting count ready for the first task. */
|
||||
uxCriticalNesting = 0;
|
||||
|
||||
debug_printf("vPortStartFirstTask\r\n");
|
||||
|
||||
/* Start the first task. */
|
||||
vPortEnableInterrupts();
|
||||
|
||||
xRunning = 1;
|
||||
|
||||
/* Start the first task. */
|
||||
prvResumeThread( prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) );
|
||||
#ifdef COND_SIGNALING
|
||||
pthread_cond_t * hCond = prvGetConditionHandle( xTaskGetCurrentTaskHandle() );
|
||||
assert( pthread_cond_signal( hCond ) == 0 );
|
||||
#endif
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
@ -202,46 +350,79 @@ void vPortStartFirstTask( void )
|
||||
portBASE_TYPE xPortStartScheduler( void )
|
||||
{
|
||||
portBASE_TYPE xResult;
|
||||
int iSignal;
|
||||
sigset_t xSignals;
|
||||
sigset_t xSignalToBlock;
|
||||
sigset_t xSignalsBlocked;
|
||||
portLONG lIndex;
|
||||
|
||||
/* Establish the signals to block before they are needed. */
|
||||
sigfillset( &xSignalToBlock );
|
||||
debug_printf( "xPortStartScheduler\r\n" );
|
||||
|
||||
/* Block until the end */
|
||||
(void)pthread_sigmask( SIG_SETMASK, &xSignalToBlock, &xSignalsBlocked );
|
||||
/* Establish the signals to block before they are needed. */
|
||||
sigemptyset( &xSignalToBlock );
|
||||
sigaddset( &xSignalToBlock, SIG_SUSPEND );
|
||||
(void)pthread_sigmask( SIG_SETMASK, &xSignalToBlock, NULL );
|
||||
|
||||
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
|
||||
{
|
||||
pxThreads[ lIndex ].uxCriticalNesting = 0;
|
||||
}
|
||||
|
||||
/* Start the timer that generates the tick ISR. Interrupts are disabled
|
||||
here already. */
|
||||
prvSetupTimerInterrupt();
|
||||
|
||||
/* Start the first task. Will not return unless all threads are killed. */
|
||||
vPortStartFirstTask();
|
||||
|
||||
/* This is the end signal we are looking for. */
|
||||
sigemptyset( &xSignals );
|
||||
sigaddset( &xSignals, SIG_RESUME );
|
||||
/* Unfortunately things are stable if we start ticking during setup. This need to be */
|
||||
/* checked careful in startup on hardware */
|
||||
usleep(1000000);
|
||||
|
||||
while ( pdTRUE != xSchedulerEnd )
|
||||
#ifdef TICK_SIGNAL
|
||||
struct itimerval itimer;
|
||||
portTickType xMicroSeconds = portTICK_RATE_MICROSECONDS;
|
||||
|
||||
debug_printf("init %li microseconds\n",(long)xMicroSeconds);
|
||||
/* Initialise the structure with the current timer information. */
|
||||
assert ( 0 == getitimer( TIMER_TYPE, &itimer ) );
|
||||
|
||||
/* Set the interval between timer events. */
|
||||
itimer.it_interval.tv_sec = 0;
|
||||
itimer.it_interval.tv_usec = xMicroSeconds;
|
||||
|
||||
/* Set the current count-down. */
|
||||
itimer.it_value.tv_sec = 0;
|
||||
itimer.it_value.tv_usec = xMicroSeconds;
|
||||
|
||||
struct sigaction sigtick;
|
||||
sigtick.sa_flags = 0;
|
||||
sigtick.sa_handler = vPortSystemTickHandler;
|
||||
sigemptyset( &sigtick.sa_mask );
|
||||
assert ( 0 == sigaction( SIG_TICK, &sigtick, NULL ) );
|
||||
|
||||
/* Set-up the timer interrupt. */
|
||||
assert ( 0 == setitimer( TIMER_TYPE, &itimer, NULL ) );
|
||||
|
||||
sigemptyset( &xSignalToBlock );
|
||||
sigaddset( &xSignalToBlock, SIG_SUSPEND );
|
||||
(void)pthread_sigmask( SIG_SETMASK, &xSignalToBlock, NULL );
|
||||
|
||||
while(1)
|
||||
{
|
||||
if ( 0 != sigwait( &xSignals, &iSignal ) )
|
||||
{
|
||||
printf( "Main thread spurious signal: %d\n", iSignal );
|
||||
}
|
||||
usleep(1000);
|
||||
sched_yield();
|
||||
}
|
||||
#else
|
||||
|
||||
struct timespec x;
|
||||
while( pdTRUE != xSchedulerEnd ) {
|
||||
x.tv_sec=0;
|
||||
x.tv_nsec=portTICK_RATE_MICROSECONDS * 1000;
|
||||
nanosleep(&x,NULL);
|
||||
// printf("."); fflush(stdout);
|
||||
vPortSystemTickHandler(SIG_TICK);
|
||||
// printf("*"); fflush(stdout);
|
||||
}
|
||||
|
||||
printf( "Cleaning Up, Exiting.\n" );
|
||||
|
||||
#endif
|
||||
debug_printf( "Cleaning Up, Exiting.\n" );
|
||||
/* Cleanup the mutexes */
|
||||
xResult = pthread_mutex_destroy( &xSuspendResumeThreadMutex );
|
||||
xResult = pthread_mutex_destroy( &xSingleThreadMutex );
|
||||
xResult = pthread_mutex_destroy( &xSwappingThreadMutex );
|
||||
vPortFree( (void *)pxThreads );
|
||||
|
||||
/* Should not get here! */
|
||||
@ -253,6 +434,8 @@ void vPortEndScheduler( void )
|
||||
{
|
||||
portBASE_TYPE xNumberOfThreads;
|
||||
portBASE_TYPE xResult;
|
||||
|
||||
|
||||
for ( xNumberOfThreads = 0; xNumberOfThreads < MAX_NUMBER_OF_TASKS; xNumberOfThreads++ )
|
||||
{
|
||||
if ( ( pthread_t )NULL != pxThreads[ xNumberOfThreads ].hThread )
|
||||
@ -271,9 +454,10 @@ portBASE_TYPE xResult;
|
||||
void vPortYieldFromISR( void )
|
||||
{
|
||||
/* Calling Yield from a Interrupt/Signal handler often doesn't work because the
|
||||
* xSingleThreadMutex is already owned by an original call to Yield. Therefore,
|
||||
* xSwappingThreadMutex is already owned by an original call to Yield. Therefore,
|
||||
* simply indicate that a yield is required soon.
|
||||
*/
|
||||
|
||||
xPendYield = pdTRUE;
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
@ -311,40 +495,118 @@ void vPortYield( void )
|
||||
{
|
||||
pthread_t xTaskToSuspend;
|
||||
pthread_t xTaskToResume;
|
||||
sigset_t xSignals;
|
||||
int retVal;
|
||||
tskTCB * oldTask, * newTask;
|
||||
|
||||
/* We must mask the suspend signal here, because otherwise there can be an */
|
||||
/* interrupt while in pthread_mutex_lock and that will cause the next thread */
|
||||
/* to deadlock when it tries to get this mutex */
|
||||
|
||||
if ( 0 == pthread_mutex_lock( &xSingleThreadMutex ) )
|
||||
{
|
||||
debug_printf( "Entering\r\n" );
|
||||
|
||||
sigemptyset( &xSignals );
|
||||
sigaddset( &xSignals, SIG_SUSPEND );
|
||||
sigaddset( &xSignals, SIG_TICK );
|
||||
pthread_sigmask( SIG_SETMASK, &xSignals, NULL );
|
||||
|
||||
oldTask = xTaskGetCurrentTaskHandle();
|
||||
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
|
||||
|
||||
retVal = pthread_mutex_trylock( &xSwappingThreadMutex );
|
||||
while( retVal != 0 ) {
|
||||
|
||||
assert( retVal == EBUSY );
|
||||
|
||||
/* If we can't get the mutex, that means an interrupt is running and we */
|
||||
/* should keep an eye out if this task should suspend so the interrupt */
|
||||
/* routine doesn't stall waiting for this task to pause */
|
||||
debug_printf( "Waiting to get swapping mutex from ISR\r\n" );
|
||||
|
||||
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
|
||||
|
||||
vTaskSwitchContext();
|
||||
|
||||
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
|
||||
if ( xTaskToSuspend != xTaskToResume )
|
||||
{
|
||||
/* Remember and switch the critical nesting. */
|
||||
prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting );
|
||||
uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume );
|
||||
/* Switch tasks. */
|
||||
prvResumeThread( xTaskToResume );
|
||||
prvSuspendThread( xTaskToSuspend );
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Yielding to self */
|
||||
(void)pthread_mutex_unlock( &xSingleThreadMutex );
|
||||
if( prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) != pthread_self() ) {
|
||||
debug_printf( "The current task isn't even us. Pausing now, deal with possible interrupt later.\r\n" );
|
||||
pauseThread( THREAD_PAUSE_YIELD );
|
||||
|
||||
/* Now we are resuming, want to be able to catch this interrupt again */
|
||||
sigemptyset( &xSignals );
|
||||
sigaddset( &xSignals, SIG_TICK );
|
||||
pthread_sigmask( SIG_SETMASK, &xSignals, NULL);
|
||||
return;
|
||||
}
|
||||
retVal = pthread_mutex_trylock( &xSwappingThreadMutex );
|
||||
}
|
||||
|
||||
/* At this point we have the lock, active task shouldn't change */
|
||||
if(xTaskToSuspend != pthread_self() ) {
|
||||
debug_printf( "The current task isn't even us, letting interrupt happen. Watch for swap.\r\n" );
|
||||
|
||||
assert( pthread_mutex_unlock( &xSwappingThreadMutex ) == 0);
|
||||
|
||||
pauseThread( THREAD_PAUSE_YIELD );
|
||||
|
||||
/* Now we are resuming, want to be able to catch this interrupt again */
|
||||
sigemptyset( &xSignals );
|
||||
sigaddset( &xSignals, SIG_TICK );
|
||||
pthread_sigmask( SIG_SETMASK, &xSignals, NULL);
|
||||
return;
|
||||
}
|
||||
|
||||
assert( xTaskToSuspend == pthread_self() ); // race condition I didn't account for
|
||||
|
||||
xStarted = pdFALSE;
|
||||
|
||||
/* Get new task then release the task switching mutex */
|
||||
vTaskSwitchContext();
|
||||
newTask = xTaskGetCurrentTaskHandle();
|
||||
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
|
||||
|
||||
if ( pthread_self() != xTaskToResume )
|
||||
{
|
||||
/* Remember and switch the critical nesting. */
|
||||
prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting );
|
||||
uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume );
|
||||
|
||||
debug_error( "Swapping From %li(%s) to %li(%s)\r\n", (long int) xTaskToSuspend, oldTask->pcTaskName, (long int) xTaskToResume, newTask->pcTaskName);
|
||||
|
||||
#ifdef COND_SIGNALING
|
||||
/* Set resume condition for specific thread */
|
||||
pthread_cond_t * hCond = prvGetConditionHandle( xTaskGetCurrentTaskHandle() );
|
||||
assert( pthread_cond_signal( hCond ) == 0 );
|
||||
#endif
|
||||
#ifdef CHECK_TASK_RESUMES
|
||||
while( xStarted == pdFALSE )
|
||||
debug_printf( "Waiting for task to resume\r\n" );
|
||||
#endif
|
||||
|
||||
debug_printf( "Detected task resuming. Pausing this task\r\n" );
|
||||
|
||||
/* Release swapping thread mutex and pause self */
|
||||
assert( pthread_mutex_unlock( &xSwappingThreadMutex ) == 0);
|
||||
pauseThread( THREAD_PAUSE_YIELD );
|
||||
}
|
||||
else {
|
||||
assert( pthread_mutex_unlock( &xSwappingThreadMutex ) == 0);
|
||||
}
|
||||
|
||||
/* Now we are resuming, want to be able to catch this interrupt again */
|
||||
sigemptyset( &xSignals );
|
||||
sigaddset( &xSignals, SIG_TICK );
|
||||
pthread_sigmask( SIG_SETMASK, &xSignals, NULL);
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
void vPortDisableInterrupts( void )
|
||||
{
|
||||
//debug_printf("\r\n");
|
||||
xInterruptsEnabled = pdFALSE;
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
void vPortEnableInterrupts( void )
|
||||
{
|
||||
//debug_printf("\r\n");
|
||||
xInterruptsEnabled = pdTRUE;
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
@ -352,6 +614,7 @@ void vPortEnableInterrupts( void )
|
||||
portBASE_TYPE xPortSetInterruptMask( void )
|
||||
{
|
||||
portBASE_TYPE xReturn = xInterruptsEnabled;
|
||||
debug_printf("\r\n");
|
||||
xInterruptsEnabled = pdFALSE;
|
||||
return xReturn;
|
||||
}
|
||||
@ -359,55 +622,36 @@ portBASE_TYPE xReturn = xInterruptsEnabled;
|
||||
|
||||
void vPortClearInterruptMask( portBASE_TYPE xMask )
|
||||
{
|
||||
debug_printf("\r\n");
|
||||
xInterruptsEnabled = xMask;
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
/*
|
||||
* Setup the systick timer to generate the tick interrupts at the required
|
||||
* frequency.
|
||||
*/
|
||||
void prvSetupTimerInterrupt( void )
|
||||
{
|
||||
struct itimerval itimer, oitimer;
|
||||
portTickType xMicroSeconds = portTICK_RATE_MICROSECONDS;
|
||||
|
||||
/* Initialise the structure with the current timer information. */
|
||||
if ( 0 == getitimer( TIMER_TYPE, &itimer ) )
|
||||
{
|
||||
/* Set the interval between timer events. */
|
||||
itimer.it_interval.tv_sec = 0;
|
||||
itimer.it_interval.tv_usec = xMicroSeconds;
|
||||
|
||||
/* Set the current count-down. */
|
||||
itimer.it_value.tv_sec = 0;
|
||||
itimer.it_value.tv_usec = xMicroSeconds;
|
||||
|
||||
/* Set-up the timer interrupt. */
|
||||
if ( 0 != setitimer( TIMER_TYPE, &itimer, &oitimer ) )
|
||||
{
|
||||
printf( "Set Timer problem.\n" );
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
printf( "Get Timer problem.\n" );
|
||||
}
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
void vPortSystemTickHandler( int sig )
|
||||
{
|
||||
pthread_t xTaskToSuspend;
|
||||
pthread_t xTaskToResume;
|
||||
|
||||
tskTCB * oldTask, * newTask;
|
||||
|
||||
debug_printf( "\r\n\r\n" );
|
||||
debug_printf( "(xInterruptsEnabled = %i, xServicingTick = %i)\r\n", (int) xInterruptsEnabled != 0, (int) xServicingTick != 0);
|
||||
if ( ( pdTRUE == xInterruptsEnabled ) && ( pdTRUE != xServicingTick ) )
|
||||
{
|
||||
if ( 0 == pthread_mutex_trylock( &xSingleThreadMutex ) )
|
||||
// debug_printf( "Checking for lock ...\r\n" );
|
||||
if ( 0 == pthread_mutex_trylock( &xSwappingThreadMutex ) )
|
||||
{
|
||||
debug_printf( "Handling\r\n");
|
||||
xServicingTick = pdTRUE;
|
||||
|
||||
|
||||
oldTask = xTaskGetCurrentTaskHandle();
|
||||
xTaskToSuspend = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
|
||||
|
||||
/* Need to set this before updating the task in case it notices before the */
|
||||
/* interrupt is set */
|
||||
xSuspended = pdFALSE;
|
||||
xStarted = pdFALSE;
|
||||
|
||||
/* Tick Increment. */
|
||||
vTaskIncrementTick();
|
||||
|
||||
@ -415,35 +659,69 @@ pthread_t xTaskToResume;
|
||||
#if ( configUSE_PREEMPTION == 1 )
|
||||
vTaskSwitchContext();
|
||||
#endif
|
||||
|
||||
newTask = xTaskGetCurrentTaskHandle();
|
||||
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
|
||||
|
||||
debug_printf( "Want %s running\r\n", newTask->pcTaskName );
|
||||
/* The only thread that can process this tick is the running thread. */
|
||||
if ( xTaskToSuspend != xTaskToResume )
|
||||
{
|
||||
/* Remember and switch the critical nesting. */
|
||||
prvSetTaskCriticalNesting( xTaskToSuspend, uxCriticalNesting );
|
||||
uxCriticalNesting = prvGetTaskCriticalNesting( xTaskToResume );
|
||||
/* Resume next task. */
|
||||
prvResumeThread( xTaskToResume );
|
||||
/* Suspend the current task. */
|
||||
prvSuspendThread( xTaskToSuspend );
|
||||
}
|
||||
|
||||
debug_printf( "Swapping From %li(%s) to %li(%s)\r\n", (long int) xTaskToSuspend, oldTask->pcTaskName, (long int) xTaskToResume, newTask->pcTaskName);
|
||||
|
||||
assert( pthread_kill( xTaskToSuspend, SIG_SUSPEND ) == 0);
|
||||
|
||||
#ifdef CHECK_TASK_RESUMES
|
||||
/* It shouldn't be possible for a second task swap to happen while waiting for this because */
|
||||
/* they can't get the xSwappingThreadMutex */
|
||||
while( xSuspended == pdFALSE )
|
||||
#endif
|
||||
{
|
||||
debug_printf( "Waiting for old task to suspend\r\n" );
|
||||
debug_printf( "Sent signal\r\n" );
|
||||
sched_yield();
|
||||
}
|
||||
debug_printf( "Suspended\r\n" );
|
||||
|
||||
#ifdef CHECK_TASK_RESUMES
|
||||
while( xStarted == pdFALSE)
|
||||
#endif
|
||||
{
|
||||
debug_printf( "Waiting for new task to resume\r\n" );
|
||||
#ifdef COND_SIGNALING
|
||||
// Set resume condition for specific thread
|
||||
pthread_cond_t * hCond = prvGetConditionHandle( xTaskGetCurrentTaskHandle() );
|
||||
assert( pthread_cond_signal( hCond ) == 0 );
|
||||
#endif
|
||||
|
||||
sched_yield();
|
||||
}
|
||||
|
||||
debug_printf( "Swapped From %li(%s) to %li(%s)\r\n", (long int) xTaskToSuspend, oldTask->pcTaskName, (long int) xTaskToResume, newTask->pcTaskName); }
|
||||
else
|
||||
{
|
||||
/* Release the lock as we are Resuming. */
|
||||
(void)pthread_mutex_unlock( &xSingleThreadMutex );
|
||||
// debug_error ("Want %s running \r\n", newTask->pcTaskName );
|
||||
}
|
||||
xServicingTick = pdFALSE;
|
||||
(void)pthread_mutex_unlock( &xSwappingThreadMutex );
|
||||
}
|
||||
else
|
||||
{
|
||||
debug_error( "Pending yield here (portYield has lock - hopefully)\r\n" );
|
||||
xPendYield = pdTRUE;
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
debug_printf( "Pending yield or here\r\n");
|
||||
xPendYield = pdTRUE;
|
||||
}
|
||||
debug_printf("Exiting\r\n");
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
@ -454,7 +732,9 @@ pthread_t xTaskToDelete;
|
||||
pthread_t xTaskToResume;
|
||||
portBASE_TYPE xResult;
|
||||
|
||||
if ( 0 == pthread_mutex_lock( &xSingleThreadMutex ) )
|
||||
printf("vPortForciblyEndThread\r\n");
|
||||
|
||||
if ( 0 == pthread_mutex_lock( &xSwappingThreadMutex ) )
|
||||
{
|
||||
xTaskToDelete = prvGetThreadHandle( hTaskToDelete );
|
||||
xTaskToResume = prvGetThreadHandle( xTaskGetCurrentTaskHandle() );
|
||||
@ -476,17 +756,19 @@ portBASE_TYPE xResult;
|
||||
xResult = pthread_cancel( xTaskToDelete );
|
||||
/* Pthread Clean-up function will note the cancellation. */
|
||||
}
|
||||
(void)pthread_mutex_unlock( &xSingleThreadMutex );
|
||||
(void)pthread_mutex_unlock( &xSwappingThreadMutex );
|
||||
}
|
||||
else
|
||||
{
|
||||
/* Resume the other thread. */
|
||||
prvResumeThread( xTaskToResume );
|
||||
/* Assert zero - I never fixed this functionality */
|
||||
assert( 0 );
|
||||
|
||||
/* Pthread Clean-up function will note the cancellation. */
|
||||
/* Release the execution. */
|
||||
uxCriticalNesting = 0;
|
||||
vPortEnableInterrupts();
|
||||
(void)pthread_mutex_unlock( &xSingleThreadMutex );
|
||||
(void)pthread_mutex_unlock( &xSwappingThreadMutex );
|
||||
/* Commit suicide */
|
||||
pthread_exit( (void *)1 );
|
||||
}
|
||||
@ -502,12 +784,38 @@ void * pParams = pxParams->pvParams;
|
||||
vPortFree( pvParams );
|
||||
|
||||
pthread_cleanup_push( prvDeleteThread, (void *)pthread_self() );
|
||||
|
||||
/* want to block suspend when not the active thread */
|
||||
sigset_t xSignals;
|
||||
sigemptyset( &xSignals );
|
||||
sigaddset( &xSignals, SIG_SUSPEND );
|
||||
sigaddset( &xSignals, SIG_TICK );
|
||||
assert( pthread_sigmask( SIG_SETMASK, &xSignals, NULL ) == 0);
|
||||
|
||||
if ( 0 == pthread_mutex_lock( &xSingleThreadMutex ) )
|
||||
{
|
||||
prvSuspendThread( pthread_self() );
|
||||
/* Because the FreeRTOS creates the TCB stack, which in this implementation */
|
||||
/* creates a thread, we need to wait until the task handle is added before */
|
||||
/* trying to pause. Must set xSentinel high so the creating task knows we're */
|
||||
/* here */
|
||||
|
||||
debug_printf("Thread started, waiting till handle is added\r\n");
|
||||
|
||||
xSentinel = 1;
|
||||
|
||||
while( prvGetTaskHandle( pthread_self() ) == NULL ){
|
||||
sched_yield();
|
||||
}
|
||||
|
||||
debug_printf("Handle added, pausing\r\n");
|
||||
|
||||
/* Want to delay briefly until we have explicit resume signal as otherwise the */
|
||||
/* current task variable might be in the wrong state */
|
||||
pauseThread( THREAD_PAUSE_CREATED );
|
||||
debug_printf("Starting first run\r\n");
|
||||
|
||||
sigemptyset( &xSignals );
|
||||
sigaddset( &xSignals, SIG_TICK );
|
||||
assert( pthread_sigmask( SIG_SETMASK, &xSignals, NULL ) == 0);
|
||||
|
||||
pvCode( pParams );
|
||||
|
||||
pthread_cleanup_pop( 1 );
|
||||
@ -515,26 +823,110 @@ void * pParams = pxParams->pvParams;
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
void pauseThread( portBASE_TYPE pauseMode )
|
||||
{
|
||||
debug_printf( "Pausing thread %li. Set xSuspended true\r\n", (long int) pthread_self() );
|
||||
xSuspended = pdTRUE;
|
||||
|
||||
#ifdef RUNNING_THREAD_MUTEX
|
||||
if( pauseMode != THREAD_PAUSE_CREATED )
|
||||
assert( 0 == pthread_mutex_unlock( &xRunningThread ) );
|
||||
#endif
|
||||
|
||||
#ifdef COND_SIGNALING
|
||||
int xResult;
|
||||
xTaskHandle hTask = prvGetTaskHandle( pthread_self() );
|
||||
pthread_cond_t * hCond = prvGetConditionHandle( hTask );
|
||||
pthread_mutex_t * hMutex = prvGetMutexHandle( hTask );
|
||||
debug_printf("Cond: %li\r\n", *( (long int *) hCond) );
|
||||
debug_printf("Mutex: %li\r\n", *( (long int *) hMutex) );
|
||||
|
||||
struct timeval tv;
|
||||
struct timespec ts;
|
||||
gettimeofday( &tv, NULL );
|
||||
ts.tv_sec = tv.tv_sec + 0;
|
||||
#endif
|
||||
|
||||
while (1) {
|
||||
if( pthread_self() == prvGetThreadHandle(xTaskGetCurrentTaskHandle() ) && xRunning )
|
||||
{
|
||||
|
||||
xStarted = pdTRUE;
|
||||
|
||||
#ifdef RUNNING_THREAD_MUTEX
|
||||
assert( 0 == pthread_mutex_lock( &xRunningThread ) );
|
||||
#endif
|
||||
debug_error("Resuming\r\n");
|
||||
return;
|
||||
}
|
||||
else {
|
||||
#ifdef COND_SIGNALING
|
||||
gettimeofday( &tv, NULL );
|
||||
ts.tv_sec = ts.tv_sec + 1;
|
||||
ts.tv_nsec = 0;
|
||||
xResult = pthread_cond_timedwait( hCond, hMutex, &ts );
|
||||
assert( xResult != EINVAL );
|
||||
#else
|
||||
/* For windows where conditional signaling is buggy */
|
||||
/* It would be wonderful to put a nanosleep here, but since its not reentrant safe */
|
||||
/* and there may be a sleep in the main code (this can be called from an ISR) we must */
|
||||
/* check this */
|
||||
if( pauseMode != THREAD_PAUSE_INTERRUPT )
|
||||
usleep(1000);
|
||||
sched_yield();
|
||||
|
||||
#endif
|
||||
// debug_error( "Checked my status\r\n" );
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void prvSuspendSignalHandler(int sig)
|
||||
{
|
||||
sigset_t xSignals;
|
||||
sigset_t xBlockSignals;
|
||||
|
||||
/* Only interested in the resume signal. */
|
||||
sigemptyset( &xSignals );
|
||||
sigaddset( &xSignals, SIG_RESUME );
|
||||
xSentinel = 1;
|
||||
/* This signal is set here instead of pauseThread because it is checked by the tick handler */
|
||||
/* which means if there were a swap it should result in a suspend interrupt */
|
||||
|
||||
debug_error( "Caught signal %i\r\n", sig );
|
||||
|
||||
/* Unlock the Single thread mutex to allow the resumed task to continue. */
|
||||
if ( 0 != pthread_mutex_unlock( &xSingleThreadMutex ) )
|
||||
{
|
||||
printf( "Releasing someone else's lock.\n" );
|
||||
#ifdef CHECK_TASK_RESUMES
|
||||
/* This would seem like a major bug, but can happen because now we send extra suspend signals */
|
||||
/* if they aren't caught */
|
||||
if( pthread_self() == prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) ) {
|
||||
debug_printf( "Marked as current task, resuming\r\n" );
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
/* Wait on the resume signal. */
|
||||
if ( 0 != sigwait( &xSignals, &sig ) )
|
||||
/* Check that we aren't suspending when we should be running. This bug would need tracking down */
|
||||
// assert( pthread_self() != prvGetThreadHandle(xTaskGetCurrentTaskHandle() ) );
|
||||
|
||||
/* Block further suspend signals. They need to go to their thread */
|
||||
sigemptyset( &xBlockSignals );
|
||||
sigaddset( &xBlockSignals, SIG_SUSPEND );
|
||||
sigaddset( &xBlockSignals, SIG_TICK );
|
||||
assert( pthread_sigmask( SIG_SETMASK, &xBlockSignals, NULL ) == 0);
|
||||
|
||||
while( pthread_self() != prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) )
|
||||
{
|
||||
printf( "SSH: Sw %d\n", sig );
|
||||
debug_printf( "Incorrectly woke up. Repausing\r\n" );
|
||||
pauseThread( THREAD_PAUSE_INTERRUPT );
|
||||
}
|
||||
|
||||
assert( pthread_self() == prvGetThreadHandle( xTaskGetCurrentTaskHandle() ) );
|
||||
|
||||
/* Old synchronization code, may still be required
|
||||
while( !xHandover );
|
||||
assert( 0 == pthread_mutex_lock( &xSingleThreadMutex ) ); */
|
||||
|
||||
/* Respond to signals again */
|
||||
sigemptyset( &xBlockSignals );
|
||||
sigaddset( &xBlockSignals, SIG_TICK );
|
||||
assert( 0 == pthread_sigmask( SIG_SETMASK, &xBlockSignals, NULL ) );
|
||||
|
||||
debug_printf( "Resuming %li from signal %i\r\n", (long int) pthread_self(), sig );
|
||||
|
||||
/* Will resume here when the SIG_RESUME signal is received. */
|
||||
/* Need to set the interrupts based on the task's critical nesting. */
|
||||
@ -546,48 +938,9 @@ sigset_t xSignals;
|
||||
{
|
||||
vPortDisableInterrupts();
|
||||
}
|
||||
debug_printf("Exit\r\n");
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
void prvSuspendThread( pthread_t xThreadId )
|
||||
{
|
||||
portBASE_TYPE xResult = pthread_mutex_lock( &xSuspendResumeThreadMutex );
|
||||
if ( 0 == xResult )
|
||||
{
|
||||
/* Set-up for the Suspend Signal handler? */
|
||||
xSentinel = 0;
|
||||
xResult = pthread_mutex_unlock( &xSuspendResumeThreadMutex );
|
||||
xResult = pthread_kill( xThreadId, SIG_SUSPEND );
|
||||
while ( ( xSentinel == 0 ) && ( pdTRUE != xServicingTick ) )
|
||||
{
|
||||
sched_yield();
|
||||
}
|
||||
}
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
void prvResumeSignalHandler(int sig)
|
||||
{
|
||||
/* Yield the Scheduler to ensure that the yielding thread completes. */
|
||||
if ( 0 == pthread_mutex_lock( &xSingleThreadMutex ) )
|
||||
{
|
||||
(void)pthread_mutex_unlock( &xSingleThreadMutex );
|
||||
}
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
void prvResumeThread( pthread_t xThreadId )
|
||||
{
|
||||
portBASE_TYPE xResult;
|
||||
if ( 0 == pthread_mutex_lock( &xSuspendResumeThreadMutex ) )
|
||||
{
|
||||
if ( pthread_self() != xThreadId )
|
||||
{
|
||||
xResult = pthread_kill( xThreadId, SIG_RESUME );
|
||||
}
|
||||
xResult = pthread_mutex_unlock( &xSuspendResumeThreadMutex );
|
||||
}
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
void prvSetupSignalsAndSchedulerPolicy( void )
|
||||
@ -602,9 +955,11 @@ int iSchedulerPriority;
|
||||
iPolicy = SCHED_FIFO;
|
||||
iResult = pthread_setschedparam( pthread_self(), iPolicy, &iSchedulerPriority ); */
|
||||
|
||||
struct sigaction sigsuspendself, sigresume, sigtick;
|
||||
struct sigaction sigsuspendself;
|
||||
portLONG lIndex;
|
||||
|
||||
|
||||
debug_printf("prvSetupSignalAndSchedulerPolicy\r\n");
|
||||
|
||||
pxThreads = ( xThreadState *)pvPortMalloc( sizeof( xThreadState ) * MAX_NUMBER_OF_TASKS );
|
||||
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
|
||||
{
|
||||
@ -612,39 +967,78 @@ portLONG lIndex;
|
||||
pxThreads[ lIndex ].hTask = ( xTaskHandle )NULL;
|
||||
pxThreads[ lIndex ].uxCriticalNesting = 0;
|
||||
}
|
||||
|
||||
|
||||
sigsuspendself.sa_flags = 0;
|
||||
sigsuspendself.sa_handler = prvSuspendSignalHandler;
|
||||
sigfillset( &sigsuspendself.sa_mask );
|
||||
sigemptyset( &sigsuspendself.sa_mask );
|
||||
|
||||
sigresume.sa_flags = 0;
|
||||
sigresume.sa_handler = prvResumeSignalHandler;
|
||||
sigfillset( &sigresume.sa_mask );
|
||||
|
||||
sigtick.sa_flags = 0;
|
||||
sigtick.sa_handler = vPortSystemTickHandler;
|
||||
sigfillset( &sigtick.sa_mask );
|
||||
|
||||
if ( 0 != sigaction( SIG_SUSPEND, &sigsuspendself, NULL ) )
|
||||
{
|
||||
printf( "Problem installing SIG_SUSPEND_SELF\n" );
|
||||
}
|
||||
if ( 0 != sigaction( SIG_RESUME, &sigresume, NULL ) )
|
||||
{
|
||||
printf( "Problem installing SIG_RESUME\n" );
|
||||
}
|
||||
if ( 0 != sigaction( SIG_TICK, &sigtick, NULL ) )
|
||||
{
|
||||
printf( "Problem installing SIG_TICK\n" );
|
||||
}
|
||||
printf( "Running as PID: %d\n", getpid() );
|
||||
assert ( 0 == sigaction( SIG_SUSPEND, &sigsuspendself, NULL ) );
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
/*-----------------------------------------------------------*/
|
||||
pthread_mutex_t * prvGetMutexHandle( xTaskHandle hTask )
|
||||
{
|
||||
pthread_mutex_t * hMutex;
|
||||
portLONG lIndex;
|
||||
|
||||
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
|
||||
{
|
||||
if ( pxThreads[ lIndex ].hTask == hTask )
|
||||
{
|
||||
hMutex = pxThreads[ lIndex ].hMutex;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return hMutex;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------*/
|
||||
xTaskHandle prvGetTaskHandle( pthread_t hThread )
|
||||
{
|
||||
xTaskHandle hTask = NULL;
|
||||
portLONG lIndex;
|
||||
|
||||
/* If not initialized yet */
|
||||
if( pxThreads == NULL ) return NULL;
|
||||
|
||||
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
|
||||
{
|
||||
if ( pxThreads[ lIndex ].hThread == hThread )
|
||||
{
|
||||
hTask = pxThreads[ lIndex ].hTask;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return hTask;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------*/
|
||||
pthread_cond_t * prvGetConditionHandle( xTaskHandle hTask )
|
||||
{
|
||||
pthread_cond_t * hCond;
|
||||
portLONG lIndex;
|
||||
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
|
||||
{
|
||||
if ( pxThreads[ lIndex ].hTask == hTask )
|
||||
{
|
||||
debug_printf( "Found condition on %i task\r\n", lIndex );
|
||||
return pxThreads[ lIndex ].hCond;
|
||||
break;
|
||||
}
|
||||
}
|
||||
printf( "Failed to get handle, pausing then recursing\r\n" );
|
||||
usleep(1000);
|
||||
return prvGetConditionHandle( hTask );
|
||||
assert(0);
|
||||
return hCond;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------*/
|
||||
pthread_t prvGetThreadHandle( xTaskHandle hTask )
|
||||
{
|
||||
pthread_t hThread = ( pthread_t )NULL;
|
||||
portLONG lIndex;
|
||||
pthread_t hThread = ( pthread_t )NULL;
|
||||
portLONG lIndex;
|
||||
|
||||
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
|
||||
{
|
||||
if ( pxThreads[ lIndex ].hTask == hTask )
|
||||
@ -660,6 +1054,7 @@ portLONG lIndex;
|
||||
portLONG prvGetFreeThreadState( void )
|
||||
{
|
||||
portLONG lIndex;
|
||||
|
||||
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
|
||||
{
|
||||
if ( pxThreads[ lIndex ].hThread == ( pthread_t )NULL )
|
||||
@ -682,6 +1077,7 @@ portLONG lIndex;
|
||||
void prvSetTaskCriticalNesting( pthread_t xThreadId, unsigned portBASE_TYPE uxNesting )
|
||||
{
|
||||
portLONG lIndex;
|
||||
|
||||
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
|
||||
{
|
||||
if ( pxThreads[ lIndex ].hThread == xThreadId )
|
||||
@ -697,6 +1093,7 @@ unsigned portBASE_TYPE prvGetTaskCriticalNesting( pthread_t xThreadId )
|
||||
{
|
||||
unsigned portBASE_TYPE uxNesting = 0;
|
||||
portLONG lIndex;
|
||||
|
||||
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
|
||||
{
|
||||
if ( pxThreads[ lIndex ].hThread == xThreadId )
|
||||
@ -712,6 +1109,7 @@ portLONG lIndex;
|
||||
void prvDeleteThread( void *xThreadId )
|
||||
{
|
||||
portLONG lIndex;
|
||||
|
||||
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
|
||||
{
|
||||
if ( pxThreads[ lIndex ].hThread == ( pthread_t )xThreadId )
|
||||
@ -734,6 +1132,8 @@ void vPortAddTaskHandle( void *pxTaskHandle )
|
||||
{
|
||||
portLONG lIndex;
|
||||
|
||||
debug_printf("vPortAddTaskHandle\r\n");
|
||||
|
||||
pxThreads[ lIndexOfLastAddedTask ].hTask = ( xTaskHandle )pxTaskHandle;
|
||||
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
|
||||
{
|
||||
@ -747,11 +1147,13 @@ portLONG lIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
usleep(10000);
|
||||
}
|
||||
/*-----------------------------------------------------------*/
|
||||
|
||||
void vPortFindTicksPerSecond( void )
|
||||
{
|
||||
|
||||
/* Needs to be reasonably high for accuracy. */
|
||||
unsigned long ulTicksPerSecond = sysconf(_SC_CLK_TCK);
|
||||
printf( "Timer Resolution for Run TimeStats is %ld ticks per second.\n", ulTicksPerSecond );
|
||||
@ -761,6 +1163,7 @@ void vPortFindTicksPerSecond( void )
|
||||
unsigned long ulPortGetTimerValue( void )
|
||||
{
|
||||
struct tms xTimes;
|
||||
|
||||
unsigned long ulTotalTime = times( &xTimes );
|
||||
/* Return the application code times.
|
||||
* The timer only increases when the application code is actually running
|
||||
|
||||
Loading…
Reference in New Issue
Block a user