rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

Getting closer on osx simulation

Browse Source
This commit is contained in:
James Cotton 2012-03-13 13:27:57 -05:00
parent a2bc7b8668
commit da294384b6
1 changed files with 73 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
flight/PiOS.osx/osx/Libraries/FreeRTOS/Source/portable/GCC/Posix/port.c
View File

@ -128,7 +128,7 @@ enum thread_status {RUNNING, STORED, STOPPED, CREATED, DESTROYED};
/* Each task maintains its own interrupt status in the critical nesting variable. */ /* Each task maintains its own interrupt status in the critical nesting variable. */
typedef struct THREAD_SUSPENSIONS typedef struct THREAD_SUSPENSIONS
{ {
pthread_t hThread; volatile pthread_t hThread;
pthread_cond_t * hCond; pthread_cond_t * hCond;
pthread_mutex_t * hMutex; pthread_mutex_t * hMutex;
xTaskHandle hTask; xTaskHandle hTask;
@ -372,7 +372,7 @@ xParams *pxThisThreadParams = pvPortMalloc( sizeof( xParams ) );
lIndexOfLastAddedTask = prvGetFreeThreadState(); lIndexOfLastAddedTask = prvGetFreeThreadState();
debug_printf( "Got index for new task %i\r\n", lIndexOfLastAddedTask ); printf( "Got index for new task %i\r\n", lIndexOfLastAddedTask );
/* Create a condition signal for this thread */ /* Create a condition signal for this thread */
pxThreads[ lIndexOfLastAddedTask ].hCond = ( pthread_cond_t *) malloc( sizeof( pthread_cond_t ) ); pxThreads[ lIndexOfLastAddedTask ].hCond = ( pthread_cond_t *) malloc( sizeof( pthread_cond_t ) );
@ -386,7 +386,7 @@ xParams *pxThisThreadParams = pvPortMalloc( sizeof( xParams ) );
/* Create a thread and store it's handle number */ /* Create a thread and store it's handle number */
xSentinel = 0; xSentinel = 0;
assert( 0 == pthread_create( &( pxThreads[ lIndexOfLastAddedTask ].hThread ), &xThreadAttributes, prvWaitForStart, (void *)pxThisThreadParams ) ); assert( 0 == pthread_create( (pthread_t *) &( pxThreads[ lIndexOfLastAddedTask ].hThread ), &xThreadAttributes, prvWaitForStart, (void *)pxThisThreadParams ) );
/* Wait until the task suspends. */ /* Wait until the task suspends. */
while ( xSentinel == 0 ); while ( xSentinel == 0 );
@ -401,10 +401,10 @@ void vPortStartFirstTask( void )
/* Initialise the critical nesting count ready for the first task. */ /* Initialise the critical nesting count ready for the first task. */
uxCriticalNesting = 0; uxCriticalNesting = 0;
debug_printf("vPortStartFirstTask\r\n"); fprintf(stdout, "vPortStartFirstTask\r\n");
/* Start the first task. */ /* Start the first task. */
vPortEnableInterrupts(); xInterruptsEnabled = pdTRUE;
xRunning = 1; xRunning = 1;
resumeThread( xTaskGetCurrentTaskHandle() ); resumeThread( xTaskGetCurrentTaskHandle() );
@ -418,7 +418,7 @@ portBASE_TYPE xPortStartScheduler( void )
{ {
portBASE_TYPE xResult; portBASE_TYPE xResult;
sigset_t xSignalToBlock; sigset_t xSignalToBlock;
portLONG lIndex; //portLONG lIndex;
fid = fopen("log.txt", "w"); fid = fopen("log.txt", "w");
@ -431,13 +431,13 @@ portLONG lIndex;
/* Block until the end */ /* Block until the end */
(void)pthread_sigmask( SIG_SETMASK, &xSignalToBlock, NULL ); (void)pthread_sigmask( SIG_SETMASK, &xSignalToBlock, NULL );
/*
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
{ {
pxThreads[ lIndex ].uxCriticalNesting = 0; pxThreads[ lIndex ].uxCriticalNesting = 0;
pxThreads[ lIndex ].status = CREATED; pxThreads[ lIndex ].status = CREATED;
} }
*/
/* Start the first task. Will not return unless all threads are killed. */ /* Start the first task. Will not return unless all threads are killed. */
vPortStartFirstTask(); vPortStartFirstTask();
@ -449,8 +449,8 @@ portLONG lIndex;
i++; i++;
if (i % 1000 == 0) if (i % 1000 == 0)
fprintf(stderr,"."); fprintf(stderr,".");
//if (i % 200 == 0) if (i % 5000 == 0)
// printTasks(); printTasks();
} }
debug_printf( "Cleaning Up, Exiting.\n" ); debug_printf( "Cleaning Up, Exiting.\n" );
@ -563,13 +563,29 @@ int irq_lock;
void vPortDisableInterrupts( void ) void vPortDisableInterrupts( void )
{ {
if (prvGetTaskHandle(pthread_self()) != NULL)
assert(prvGetThreadStatus(pthread_self()) == RUNNING);
//debug_printf("\r\n"); //debug_printf("\r\n");
maskSuspend();
irq_lock = 2;
// assert( pthread_mutex_trylock( &xIrqMutex ) == 0);
while( pthread_mutex_trylock( &xSwappingThreadMutex ) != 0)
{
// Give task schedule a chance to suspend this
unmaskSuspend();
usleep(1);
maskSuspend();
}
irq_lock = 1; irq_lock = 1;
// assert( pthread_mutex_lock( &xIrqMutex ) == 0);
xInterruptsEnabled = pdFALSE; xInterruptsEnabled = pdFALSE;
// assert( pthread_mutex_unlock( &xIrqMutex) == 0); // assert( pthread_mutex_unlock( &xIrqMutex) == 0);
assert( pthread_mutex_unlock( &xSwappingThreadMutex) == 0);
irq_lock = 0; irq_lock = 0;
// unmaskSuspend(); unmaskSuspend();
} }
/*-----------------------------------------------------------*/ /*-----------------------------------------------------------*/
@ -581,13 +597,27 @@ void vPortDisableInterrupts( void )
void vPortEnableInterrupts( void ) void vPortEnableInterrupts( void )
{ {
//debug_printf("\r\n"); //debug_printf("\r\n");
// maskSuspend(); if (prvGetTaskHandle(pthread_self()) != NULL)
irq_lock = 1; assert(prvGetThreadStatus(pthread_self()) == RUNNING);
// assert( pthread_mutex_lock( &xIrqMutex ) == 0);
maskSuspend();
irq_lock = 4;
// assert( pthread_mutex_trylock( &xIrqMutex ) == 0);
while( pthread_mutex_trylock( &xSwappingThreadMutex ) != 0)
{
// Give task schedule a chance to suspend this
unmaskSuspend();
usleep(1);
maskSuspend();
}
irq_lock = 3;
// assert( pthread_mutex_trylock( &xIrqMutex ) == 0);
xInterruptsEnabled = pdTRUE; xInterruptsEnabled = pdTRUE;
assert( pthread_mutex_unlock( &xSwappingThreadMutex) == 0);
// assert( pthread_mutex_unlock( &xIrqMutex) == 0); // assert( pthread_mutex_unlock( &xIrqMutex) == 0);
irq_lock = 0; irq_lock = 0;
// unmaskSuspend(); unmaskSuspend();
} }
/*-----------------------------------------------------------*/ /*-----------------------------------------------------------*/
@ -617,14 +647,14 @@ void vPortSystemTickHandler( int sig )
/* Tick Increment. */ /* Tick Increment. */
vTaskIncrementTick(); vTaskIncrementTick();
/*
if (pthread_mutex_trylock( &xIrqMutex ) == EBUSY) { /* if (pthread_mutex_trylock( &xIrqMutex ) == EBUSY) {
fprintf(stdout, "Systick could not block interrupts\r\n"); fprintf(stdout, "Systick could not block interrupts\r\n");
return; return;
} } */
*/
if(pthread_mutex_trylock( &xSwappingThreadMutex )== EBUSY) { if(pthread_mutex_trylock( &xSwappingThreadMutex )== EBUSY) {
//fprintf(stdout,"Can't get swapping lock for tick handler\r\n"); fprintf(stdout,"Can't get swapping lock for tick handler\r\n");
return; return;
} }
@ -695,9 +725,9 @@ portBASE_TYPE xResult;
/* Pthread Clean-up function will note the cancellation. */ /* Pthread Clean-up function will note the cancellation. */
/* Release the execution. */ /* Release the execution. */
uxCriticalNesting = 0; uxCriticalNesting = 0;
vPortEnableInterrupts();
(void)pthread_mutex_unlock( &xSwappingThreadMutex ); (void)pthread_mutex_unlock( &xSwappingThreadMutex );
prvSetThreadStatus( xTaskToDelete, DESTROYED ); prvSetThreadStatus( xTaskToDelete, DESTROYED );
// vPortEnableInterrupts();
releaseRunningSemaphore(); releaseRunningSemaphore();
/* Commit suicide */ /* Commit suicide */
pthread_exit( (void *)1 ); pthread_exit( (void *)1 );
@ -726,23 +756,21 @@ void * pParams = pxParams->pvParams;
/* trying to pause. Must set xSentinel high so the creating task knows we're */ /* trying to pause. Must set xSentinel high so the creating task knows we're */
/* here */ /* here */
//fprintf(stdout,"Thread started, waiting till handle is added\r\n"); fprintf(stdout,"Thread started, waiting till handle is added\r\n");
xSentinel = 1; xSentinel = 1;
while( prvGetTaskHandle( pthread_self() ) == NULL ){ while( prvGetTaskHandle( pthread_self() ) == NULL ){
//sched_yield(); sched_yield();
usleep(1); usleep(1);
} }
//fprintf(stdout,"Handle added, pausing\r\n"); fprintf(stdout,"Handle added%s\r\n",threadToName(pthread_self()));
/* Want to delay briefly until we have explicit resume signal as otherwise the */ /* Want to delay briefly until we have explicit resume signal as otherwise the */
/* current task variable might be in the wrong state */ /* current task variable might be in the wrong state */
/* Block further suspend signals. They need to go to their thread */ /* Block further suspend signals. They need to go to their thread */
usleep(1000);
xTaskHandle hTask = prvGetTaskHandle( pthread_self() ); xTaskHandle hTask = prvGetTaskHandle( pthread_self() );
portLONG lIndex; portLONG lIndex;
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS && pxThreads[ lIndex ].hTask != hTask; lIndex++ ); for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS && pxThreads[ lIndex ].hTask != hTask; lIndex++ );
@ -750,7 +778,14 @@ void * pParams = pxParams->pvParams;
pxThreads[lIndex].name = (char *) ((tskTCB *) hTask)->pcTaskName; pxThreads[lIndex].name = (char *) ((tskTCB *) hTask)->pcTaskName;
pauseSelf(); if(prvGetTaskHandle(pthread_self()) != xTaskGetCurrentTaskHandle())
pauseSelf();
else {
fprintf(stdout, "New thread wants to run right away %s\r\n", threadToName(pthread_self()));
printTasks();
prvSetThreadStatus(pthread_self(), RUNNING);
claimRunningSemaphore(6);
}
sigemptyset( &xSignals ); sigemptyset( &xSignals );
assert( pthread_sigmask( SIG_SETMASK, &xSignals, NULL ) == 0); assert( pthread_sigmask( SIG_SETMASK, &xSignals, NULL ) == 0);
@ -857,17 +892,15 @@ static void prvSetThreadStatus( pthread_t hThread, enum thread_status status )
{ {
portLONG lIndex; portLONG lIndex;
/* If not initialized yet */
if( pxThreads == NULL ) return;
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ ) for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS; lIndex++ )
{ {
if ( pxThreads[ lIndex ].hThread == hThread ) if ( pxThreads[ lIndex ].hThread == hThread )
{ {
pxThreads[ lIndex ].status = status; pxThreads[ lIndex ].status = status;
break; return;
} }
} }
fprintf(stderr, "Could not find thread %lx\r\n", (long int) hThread);
} }
/*-----------------------------------------------------------*/ /*-----------------------------------------------------------*/
@ -1101,13 +1134,14 @@ void pauseSelf()
struct timeval tv; struct timeval tv;
struct timespec ts; struct timespec ts;
gettimeofday( &tv, NULL ); gettimeofday( &tv, NULL );
ts.tv_sec = tv.tv_sec + 5; ts.tv_sec = tv.tv_sec + 1;
ts.tv_nsec = tv.tv_usec * 1000; ts.tv_nsec = tv.tv_usec * 1000;
xResult = pthread_cond_timedwait( hCond, hMutex, &ts ); xResult = pthread_cond_timedwait( hCond, hMutex, &ts );
assert( xResult != EINVAL ); assert( xResult != EINVAL );
if (xResult == ETIMEDOUT && pthread_self() == prvGetThreadHandle(xTaskGetCurrentTaskHandle())) { if (xResult == ETIMEDOUT && pthread_self() == prvGetThreadHandle(xTaskGetCurrentTaskHandle()) &&
pxThreads[0].status == DESTROYED) {
fprintf(stdout,"Timed out should be running %s\r\n", threadToName( pthread_self() )); fprintf(stdout,"Timed out should be running %s\r\n", threadToName( pthread_self() ));
break; break;
} }
@ -1153,7 +1187,7 @@ static int pauseOtherThread(xTaskHandle hTask)
for (int i = 0; i < MAX_ATTEMPTS; i++) { for (int i = 0; i < MAX_ATTEMPTS; i++) {
// Trigger signal handler which will call pauseSelf // Trigger signal handler which will call pauseSelf
pthread_t thread_to_supend = prvGetThreadHandle( hTask ); pthread_t thread_to_supend = prvGetThreadHandle( hTask );
fprintf(stdout, "Requesting pause of thread %s from %s.\r\n", threadToName(thread_to_supend), threadToName( pthread_self() ) ); //fprintf(stdout, "Requesting pause of thread %s from %s.\r\n", threadToName(thread_to_supend), threadToName( pthread_self() ) );
assert( pthread_kill( thread_to_supend, SIG_SUSPEND ) == 0); assert( pthread_kill( thread_to_supend, SIG_SUSPEND ) == 0);
@ -1179,8 +1213,8 @@ static int pauseOtherThread(xTaskHandle hTask)
*/ */
static void resumeThread(xTaskHandle hTask) static void resumeThread(xTaskHandle hTask)
{ {
const unsigned int MAX_TIME = 10000; // us const unsigned int MAX_TIME = 100; // us
const int MAX_ATTEMPTS = 250; const int MAX_ATTEMPTS = 20;
portLONG lIndex; portLONG lIndex;
for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS && pxThreads[ lIndex ].hTask != hTask; lIndex++ ); for ( lIndex = 0; lIndex < MAX_NUMBER_OF_TASKS && pxThreads[ lIndex ].hTask != hTask; lIndex++ );
@ -1204,7 +1238,7 @@ static void resumeThread(xTaskHandle hTask)
//fprintf(stdout, "Sending resume signal from %s to %s (%d): Try %d\r\n", threadToName(pthread_self()), threadToName(hTask), lIndex, i); //fprintf(stdout, "Sending resume signal from %s to %s (%d): Try %d\r\n", threadToName(pthread_self()), threadToName(hTask), lIndex, i);
} }
fprintf(stdout, "Railed to resume from %s to %s (%d)\r\n", threadToName(pthread_self()), threadToName(hTask), lIndex); //fprintf(stdout, "Railed to resume from %s to %s (%d)\r\n", threadToName(pthread_self()), threadToName(hTask), lIndex);
// Thread resumption timed out // Thread resumption timed out
assert(0); assert(0);
} }