static void foregroundTask(long a1, long a2, long a3, long a4, long a5,
long a6, long a7, long a8, long a9, long a10)
{
int ret;
traceobj_enter(&trobj);
traceobj_mark(&trobj, 4);
ret = semTake(sem_id, WAIT_FOREVER);
traceobj_assert(&trobj, ret == OK);
traceobj_mark(&trobj, 5);
taskDelay(3);
traceobj_mark(&trobj, 6);
ret = taskSuspend(btid);
traceobj_assert(&trobj, ret == OK);
traceobj_mark(&trobj, 7);
traceobj_exit(&trobj);
}
/**
* Assert implementation.
* This allows breakpoints to be set on an assert.
* The users don't call this, but instead use the wpi_assert macros in Utility.h.
*/
bool wpi_assert_impl(bool conditionValue,
const char *conditionText,
const char *message,
const char *fileName,
UINT32 lineNumber,
const char *funcName)
{
if (!conditionValue)
{
// Error string buffer
char error[256];
// If an error message was specified, include it
// Build error string
if(message != NULL) {
sprintf(error, "Assertion failed: \"%s\", \"%s\" failed in %s() in %s at line %d\n",
message, conditionText, funcName, fileName, lineNumber);
} else {
sprintf(error, "Assertion failed: \"%s\" in %s() in %s at line %d\n",
conditionText, funcName, fileName, lineNumber);
}
// Print to console and send to remote dashboard
printf("\n\n>>>>%s", error);
setErrorData(error, strlen(error), 100);
wpi_handleTracing();
if (suspendOnAssertEnabled) taskSuspend(0);
}
return conditionValue;
}
/**
* Common error routines for wpi_assertEqual_impl and wpi_assertNotEqual_impl
* This should not be called directly; it should only be used by wpi_assertEqual_impl
* and wpi_assertNotEqual_impl.
*/
void wpi_assertEqual_common_impl(int valueA,
int valueB,
const char *equalityType,
const char *message,
const char *fileName,
UINT32 lineNumber,
const char *funcName)
{
// Error string buffer
char error[256];
// If an error message was specified, include it
// Build error string
if(message != NULL) {
sprintf(error, "Assertion failed: \"%s\", \"%d\" %s \"%d\" in %s() in %s at line %d\n",
message, valueA, equalityType, valueB, funcName, fileName, lineNumber);
} else {
sprintf(error, "Assertion failed: \"%d\" %s \"%d\" in %s() in %s at line %d\n",
valueA, equalityType, valueB, funcName, fileName, lineNumber);
}
// Print to console and send to remote dashboard
printf("\n\n>>>>%s", error);
setErrorData(error, strlen(error), 100);
wpi_handleTracing();
if (suspendOnAssertEnabled) taskSuspend(0);
}
void main (void)
{
/* Add your code here: create tasks, semaphores, ... */
initHardware(0);
analogInputs = semBCreate (SEM_Q_PRIORITY, SEM_FULL);
writeDisplay = semBCreate (SEM_Q_PRIORITY, SEM_FULL);
int readInputsID;
readInputsID = taskSpawn ("readInputs", 150, 0,0x1000,(FUNCPTR) readInputs,0,0,0,0,0,0,0,0,0,0);
int showInputsID;
showInputsID = taskSpawn ("showInputs", 160, 0,0x1000,(FUNCPTR) showInputs,0,0,0,0,0,0,0,0,0,0);
int readKeyboardID;
readKeyboardID = taskSpawn ("readKeyboard", 170, 0,0x1000,(FUNCPTR) readKeyboard,0,0,0,0,0,0,0,0,0,0);
int timerID;
timerID = taskSpawn ("timer", 140, 0,0x1000,(FUNCPTR) timer,0,0,0,0,0,0,0,0,0,0);
int tcpServerID;
tcpServerID = taskSpawn ("tcpServer", 200, 0,0x1000,(FUNCPTR) tcpServer,0,0,0,0,0,0,0,0,0,0);
printf("Hello World");
/* Suspend own task */
taskSuspend (0);
} /* main */
int BeagleBone::SendData(char *p) {
if (!Connected())
return 0;
taskSuspend(tid);
int writ= write(connsock, p, strlen(p));
taskResume(tid);
return writ;
}
/**
* Assert status clean implementation.
* This allows breakpoints to be set on an assert.
* This allows the fatal status to be printed.
* The users don't call this, but instead use the wpi_assertCleanStatus macro in Utility.h.
*/
void wpi_assertCleanStatus_impl(INT32 status, const char *fileName, UINT32 lineNumber, const char *funcName)
{
if (status != 0)
{
printf("\n\n>>>>%s: status == %d (0x%08X) in %s() in %s at line %d\n",
status < 0 ? "ERROR" : "WARNING", status, (UINT32)status, funcName, fileName, lineNumber);
wpi_handleTracing();
if (suspendOnAssertEnabled) taskSuspend(0);
}
}
/**
* Assert implementation.
* This allows breakpoints to be set on an assert.
* The users don't call this, but instead use the wpi_assert macro in Utility.h.
*/
void wpi_assert_impl(bool conditionValue, const char *conditionText, const char *fileName,
UINT32 lineNumber, const char *funcName)
{
if (!conditionValue)
{
printf("\n\n>>>>Assert \"%s\" failed in %s() in %s at line %d\n",
conditionText, funcName, fileName, lineNumber);
wpi_handleTracing();
if (suspendOnAssertEnabled) taskSuspend(0);
}
}
static void wpiSelfTrace()
{
INT32 priority=100;
taskPriorityGet(0, &priority);
// Lower priority than the calling task.
Task traceTask("StackTrace", (FUNCPTR)wpiStackTask, priority + 1);
traceTask.Start(taskIdSelf());
// Task to be traced must be suspended for the stack trace to work.
taskSuspend(0);
}
void autonomous() {
taskResume(velocity_task);
taskResume(leftFlywheel_task);
taskResume(rightFlywheel_task);
leftFlywheel.variables.power =29.5;
rightFlywheel.variables.power =29.5;
rightFlywheel.variables.powerRaw = ((rightFlywheel.variables.power)/3.0)*(12/FLYWHEEL_CIRCUMFERENCE)*360;
leftFlywheel.variables.powerRaw = ((leftFlywheel.variables.power)/3.0)*(12/FLYWHEEL_CIRCUMFERENCE)*360;
delay(5000);
motorSet(INTAKE,127);
motorSet(INTAKE,-127);
delay(8000);
taskSuspend(velocity_task);
taskSuspend(leftFlywheel_task);
taskSuspend(rightFlywheel_task);
leftFlywheel.variables.power =0;
rightFlywheel.variables.power =0;
rightFlywheel.variables.powerRaw =0;
leftFlywheel.variables.powerRaw = 0;
}
void main (void)
{
unsigned char tempOut;
/* Connect interrupt service routine to vector and all stuff */
intConnect (INUM_TO_IVEC(aioIntNum), my_ISR, aioIntNum);
sysIntEnablePIC (aioIRQNum);
/* Enable interrupts on the aio:
* All interrupts and interrupt from counter 1 too */
tempOut = 0x24;
sysOutByte (aioBase + intEnAddress, tempOut);
/* Start counter 1 as timer with 50 ms period
* It has a clock input of 1 MHz = 1 µs
* Therefore the load value is 0xC350 = 50000 */
tempOut = 0x74;
sysOutByte (aioBase + cntCntrlReg, tempOut);
tempOut = 0x50;
sysOutByte (aioBase + cnt1Address, tempOut);
tempOut = 0xC3;
sysOutByte (aioBase + cnt1Address, tempOut);
/* Add your code here: create tasks, semaphores, ... */
//printf("Hello World!");
/*The second task shall do the following:
* - It starts counter a as an auto-reload timer with a period of 50 ms.
* - The interrupt generated by this counter activates the task.
* This shall be done with a semaphore.
* - The task reads the analog inputs of both the potentiometer and the temperature.
* - If the values have changed compared to the previous ones, these shall be written into global
* variables (AIn).
* - A hysteresis of e.g. 10 – 20 bit is necessary due to the noise of the analog inputs.
*
* */
initHardware(0);
int readTask;
readTask = taskSpawn("tRead", 101, 0, 0x1000, (FUNCPTR) tRead,0,0,0,0,0,0,0,0,0,0);
timIntSem = semBCreate(SEM_Q_PRIORITY, SEM_EMPTY);
/* The third task shall do the following:
* - It writes the values of the global variables (AIn) onto the display.
* - This shall happen approx. each 100 ms
*/
int writeTask;
writeTask = taskSpawn("tWrite",102,0,0x1000, (FUNCPTR) tWrite,0,0,0,0,0,0,0,0,0,0);
/* Suspend own task */
taskSuspend (0); //suspended sich selber
} /* main */
void operatorControl() {
/*TaskHandle kfWriter = taskRunLoop(writeKFValues, 20);
delay(3000);*/
/*forward(100, 500);
forward(0, 500);
forward(-100, 500);
forward(0, 1000);
fclose(file);
taskSuspend(kfWriter);*/
bool running = false;
TaskHandle kfWriter = NULL;
while(1) {
if (abs(joystickGetAnalog(1, 2) > 10)) {
//forward(joystickGetAnalog(1, 2), 20);
motorSet(frontLeft, -127);
motorSet(middleLeft, -127 * 0.7143);
motorSet(backLeft, 127);
motorSet(frontRight, 127);
motorSet(middleRight, 127 * 0.7143);
motorSet(backRight, -127);
} else {
//forward(0, 20);
motorStop(backLeft);
motorStop(middleLeft);
motorStop(frontLeft);
motorStop(backRight);
motorStop(middleRight);
motorStop(frontRight);
}
if (joystickGetDigital(1, 8, JOY_DOWN)) {
if (!running) {
kfWriter = taskRunLoop(writeKFValues, 20);
delay(3000);
running = true;
}
}
if (joystickGetDigital(1, 7, JOY_DOWN)) {
if (running) {
fclose(file);
taskSuspend(kfWriter);
running = false;
}
}
delay(20);
}
}
int pauseConditionnement() {
int ret1;
int ret2;
semTake(semClapet, WAIT_FOREVER);
clapet = FERME;
semGive(semClapet);
message(CLAPET_FERME, NULL, NULL, NULL);
// On suspend la tache en erreur :
ret1 = taskSuspend(0);
return OK;
}
void Error::Set(Code code, const char* contextMessage, const char* filename, const char* function, UINT32 lineNumber, const ErrorBase* originatingObject)
{
m_code = code;
m_message = contextMessage;
m_filename = filename;
m_function = function;
m_lineNumber = lineNumber;
m_originatingObject = originatingObject;
m_timestamp = GetTime();
Report();
if (m_suspendOnErrorEnabled) taskSuspend(0);
}
void createSTBTasks(void)
{
task_conf_t *pTask = &startup_tasks[0];
TID_TYPE tid;
int i, n;
for (i = 0, n = 0; pTask->fp != NULL; i++, pTask++)
{
if (pTask->stack_size > 0)
{
#ifdef _STB_CONF_DEBUG_
dbgprint("creating Task %-8.8s, Prio=%3d, Stk=%6d fp %08x...",
pTask->name, pTask->priority, pTask->stack_size,
pTask->fp
);
#endif
tid = createTask( pTask->name,
(void (*)(void *))pTask->fp,
pTask->arg, NULL,
pTask->stack_size,
pTask->priority,
pTask->group);
if (tid == 0)
{
printf("Creating Task[-%8.8s] failed\n", pTask->name);
}
else
{
n++;
if (pTask->pTid != NULL) *pTask->pTid = tid;
if (pTask->opts & 0x01) taskSuspend(tid);
}
}
else
{
#ifdef _STB_CONF_DEBUG_
dbgprint("Initializing %-8.8s Resources", pTask->name);
#endif
((void (*)(void))pTask->fp)();
}
}
dbgprint("%3d Tasks are created", n);
return;
}
static int __wind_task_suspend(struct task_struct *curr, struct pt_regs *regs)
{
xnhandle_t handle = __xn_reg_arg1(regs);
WIND_TCB *pTcb;
if (handle)
pTcb = (WIND_TCB *)xnregistry_fetch(handle);
else
pTcb = __wind_task_current(curr);
if (!pTcb)
return S_objLib_OBJ_ID_ERROR;
if (taskSuspend((TASK_ID) pTcb) == ERROR)
return wind_errnoget();
return 0;
}
void user_defined_trap_rtn( ESFPPC *esf, REG_SET regs, int a, int b)
{
FP_CONTEXT fpContext;
long my_task_id;
CFE_EVS_LogFileCmd_t Cmd;
my_task_id = taskIdSelf();
CFE_EVS_SendEvent(24, CFE_EVS_ERROR, "Exception: task %s: vector: 0x%.08X",taskName(my_task_id),(int)esf->vecOffset);
speSave(&fpContext);
fpContext.fpcsr &= 0x000000ff;
speRestore(&fpContext);
strcpy((char *) Cmd.Payload.LogFilename,"\/cf\/log\/nvlog");
CFE_EVS_WriteLogFileCmd(&Cmd);
taskSuspend(my_task_id);
}
/**
* Assert status clean implementation.
* This allows breakpoints to be set on an assert.
* This allows the fatal status to be printed.
* The users don't call this, but instead use the wpi_assertCleanStatus macro in Utility.h.
*/
void wpi_assertCleanStatus_impl(INT32 status, const char *fileName, UINT32 lineNumber, const char *funcName)
{
if (status != 0)
{
// Error string buffers
char error[256];
char error_with_code[256];
// Build error strings
sprintf(error, "%s: status == %d (0x%08X) in %s() in %s at line %d\n",
status < 0 ? "ERROR" : "WARNING", status, (UINT32)status, funcName, fileName, lineNumber);
sprintf(error_with_code,"<Code>%d %s", status, error);
// Print to console and send to remote dashboard
printf("\n\n>>>>%s", error);
setErrorData(error_with_code, strlen(error_with_code), 100);
wpi_handleTracing();
if (suspendOnAssertEnabled) taskSuspend(0);
}
}
void sleepSTBTasks(void)
{
task_conf_t *pTask = &startup_tasks[0];
int i, n;
for (i = 0, n = 0; pTask->fp != NULL; i++, pTask++)
{
if ( ( pTask->pTid != NULL) &&
(*pTask->pTid != 0 ) &&
((pTask->opts & 0x80) == 0 ) )
{
//dbgprint("Suspending Task %-8.8s", pTask->name);
/*
* Since all error must be ignored, we can not use
* SC_Suspend Task()
*/
// TODO : verify t_suspend(*pTask->pTid);;
taskSuspend(*pTask->pTid);
}
}
return;
}
STATUS taskDestroy(
int taskId,
BOOL freeStack,
unsigned timeout,
BOOL forceDestroy
)
{
STATUS status;
int i, level;
TCB_ID tcbId;
if (INT_RESTRICT() != OK)
{
errnoSet (S_intLib_NOT_ISR_CALLABLE);
return ERROR;
}
/* Get task context */
tcbId = taskTcb(taskId);
if (tcbId == NULL)
return ERROR;
/* If task self destruct and excption lib installed */
if (tcbId == taskIdCurrent) {
/* Wait for safe to destroy */
while (tcbId->safeCount > 0)
taskUnsafe();
/* Kill it */
status = excJobAdd(
(VOIDFUNCPTR) taskDestroy,
(ARG) tcbId,
(ARG) freeStack,
(ARG) WAIT_NONE,
(ARG) FALSE,
(ARG) 0,
(ARG) 0
);
/* Block here and suspend */
while(status == OK)
taskSuspend(0);
} /* End if task self destruct and exception lib installed */
taskDestroyLoop:
/* Lock interrupts */
INT_LOCK(level);
/* Check id */
if (TASK_ID_VERIFY(tcbId) != OK)
{
/* errno set by taskIdVerify() */
/* Unlock interrupts */
INT_UNLOCK(level);
return ERROR;
}
/* Mask all signals */
if (tcbId->pSignalInfo != NULL)
tcbId->pSignalInfo->sigt_blocked = 0xffffffff;
/* Block here for safe and running locked tasks */
while ( (tcbId->safeCount > 0) ||
( (tcbId->status == TASK_READY) && (tcbId->lockCount > 0) )
)
{
/* Enter kernel mode */
kernelState = TRUE;
/* Unlock interrupts */
INT_UNLOCK(level);
/* Check if force deletion, or suicide */
if (forceDestroy || (tcbId == taskIdCurrent))
{
/* Remove protections */
tcbId->safeCount = 0;
tcbId->lockCount = 0;
/* Check if flush of safety queue is needed */
if (Q_FIRST(&tcbId->safetyQ) != NULL)
vmxPendQFlush(&tcbId->safetyQ);
/* Exit trough kernel */
vmxExit();
}
else
{
/* Not forced deletion or suicide */
/* Put task on safe queue */
if (vmxPendQPut(&tcbId->safetyQ, timeout) != OK)
{
/* Exit trough kernel */
vmxExit();
errnoSet (S_taskLib_INVALID_TIMEOUT);
return ERROR;
}
/* Exit trough kernel */
status = vmxExit();
/* Check for restart */
if (status == SIG_RESTART)
{
timeout = (sigTimeoutRecalc)(timeout);
goto taskDestroyLoop;
}
/* Check if unsuccessful */
if (status == ERROR)
{
/* timer should have set errno to S_objLib_TIMEOUT */
return ERROR;
}
} /* End else forced or suicide */
/* Lock interrupts */
INT_LOCK(level);
/* Now verify class id again */
if (TASK_ID_VERIFY(tcbId) != OK)
{
/* errno set by taskIdVerify() */
/* Unlock interrupts */
INT_UNLOCK(level);
return ERROR;
}
} /* End while blocked by safety */
/* Now only one cadidate is selected for deletion */
/* Make myself safe */
taskSafe();
/* Protet deletion cadidate */
tcbId->safeCount++;
/* Check if not suicide */
if (tcbId != taskIdCurrent)
{
/* Enter kernel mode */
kernelState = TRUE;
/* Unlock interrupts */
INT_UNLOCK(level);
/* Suspend victim */
vmxSuspend(tcbId);
/* Exit trough kernel */
vmxExit();
}
else
{
/* Unlock interrupts */
INT_UNLOCK(level);
}
/* Run deletion hooks */
for (i = 0; i < MAX_TASK_DELETE_HOOKS; i++)
if (taskDeleteHooks[i] != NULL)
(*taskDeleteHooks[i])(tcbId);
/* Lock task */
taskLock();
/* If dealloc and options dealloc stack */
if ( freeStack && (tcbId->options & TASK_OPTIONS_DEALLOC_STACK) ) {
#if (_STACK_DIR == _STACK_GROWS_DOWN)
objFree(taskClassId, tcbId->pStackEnd);
#else /* _STACK_GROWS_UP */
objFree(taskClassId, tbcId - TASK_EXTRA_BYTES);
#endif /* _STACK_DIR */
}
/* Lock interrupts */
INT_LOCK(level);
/* Invalidate id */
objCoreTerminate(&tcbId->objCore);
/* Enter kernel mode */
kernelState = TRUE;
/* Unlock interrupts */
INT_UNLOCK(level);
/* Delete task */
status = vmxDelete(tcbId);
/* Check if safe quque needs to be flushed */
if (Q_FIRST(&tcbId->safetyQ) != NULL)
vmxPendQFlush(&tcbId->safetyQ);
/* Exit trough kernel */
vmxExit();
/* Unprotect */
taskUnlock();
taskUnsafe();
return OK;
}
STATUS taskRestart(
int taskId
)
{
TCB_ID tcbId;
char *name, *rename;
int len;
unsigned priority;
int options;
char *pStackBase;
unsigned stackSize;
FUNCPTR entry;
ARG args[MAX_TASK_ARGS];
STATUS status;
if (INT_RESTRICT() != OK)
{
errnoSet(S_intLib_NOT_ISR_CALLABLE);
return ERROR;
}
/* If self restart */
if ( (taskId == 0) || (taskId == (int) taskIdCurrent) )
{
/* Task must be unsafe */
while (taskIdCurrent->safeCount > 0)
taskSafe();
/* Spawn a task that will restart this task */
taskSpawn(restartTaskName, restartTaskPriority, restartTaskOptions,
restartTaskStackSize, taskRestart, (ARG) taskIdCurrent,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0);
/* Wait for restart */
while (1)
taskSuspend(0);
} /* End if self restart */
/* Get task context */
tcbId = taskTcb(taskId);
if (tcbId == NULL)
return ERROR;
/* TASK_ID_VERIFY() already done by taskTcb() */
/* Copy task data */
priority = tcbId->priority;
options = tcbId->options;
entry = tcbId->entry;
pStackBase = tcbId->pStackBase;
stackSize = (tcbId->pStackEnd - tcbId->pStackBase) * _STACK_DIR;
taskArgGet(tcbId, pStackBase, args);
/* Copy name if needed */
name = tcbId->name;
rename = NULL;
if (name != NULL) {
len = strlen(name) + 1;
rename = malloc(len);
if (rename != NULL)
strcpy(rename, name);
name = rename;
}
/* Prevent deletion */
taskSafe();
if (taskTerminate((int) tcbId) != OK)
{
taskUnsafe();
/* errno set by taskTerminate() */
return ERROR;
}
/* Initialize task with same data */
status = taskInit(tcbId, name, priority, options, pStackBase,
stackSize, entry, args[0], args[1], args[2],
args[3], args[4], args[5], args[6], args[7],
args[8], args[9]);
if (status != OK)
{
/* errno set by taskInit() */
return ERROR;
}
/* And start it */
status = taskActivate((int) tcbId);
if (status != OK)
{
/* errno set by taskActivate() */
return ERROR;
}
/* Make me mortal */
taskUnsafe();
/* Free rename buffer if needed */
if (rename != NULL)
free(rename);
return OK;
}
void *memPartAlignedAlloc
(
FAST PART_ID partId, /* memory partition to allocate from */
unsigned nBytes, /* number of bytes to allocate */
unsigned alignment /* boundary to align to */
)
{
FAST unsigned nWords;
FAST unsigned nWordsExtra;
FAST DL_NODE * pNode;
FAST BLOCK_HDR * pHdr;
BLOCK_HDR * pNewHdr;
BLOCK_HDR origpHdr;
if (OBJ_VERIFY (partId, memPartClassId) != OK)
return (NULL);
/* get actual size to allocate; add overhead, check for minimum */
nWords = (MEM_ROUND_UP (nBytes) + sizeof (BLOCK_HDR)) >> 1;
/* check if we have an overflow. if so, set errno and return NULL */
if ((nWords << 1) < nBytes)
{
if (memPartAllocErrorRtn != NULL)
(* memPartAllocErrorRtn) (partId, nBytes);
errnoSet (S_memLib_NOT_ENOUGH_MEMORY);
if (partId->options & MEM_ALLOC_ERROR_SUSPEND_FLAG)
{
if ((taskIdCurrent->options & VX_UNBREAKABLE) == 0)
taskSuspend (0); /* suspend ourselves */
}
return (NULL);
}
if (nWords < partId->minBlockWords)
nWords = partId->minBlockWords;
/* get exclusive access to the partition */
semTake (&partId->sem, WAIT_FOREVER);
/* first fit */
pNode = DLL_FIRST (&partId->freeList);
/* We need a free block with extra room to do the alignment.
* Worst case we'll need alignment extra bytes.
*/
nWordsExtra = nWords + alignment / 2;
FOREVER
{
while (pNode != NULL)
{
/* fits if:
* - blocksize > requested size + extra room for alignment or,
* - block is already aligned and exactly the right size
*/
if ((NODE_TO_HDR (pNode)->nWords > nWordsExtra) ||
((NODE_TO_HDR (pNode)->nWords == nWords) &&
(ALIGNED (HDR_TO_BLOCK(NODE_TO_HDR(pNode)), alignment))))
break;
pNode = DLL_NEXT (pNode);
}
if (pNode == NULL)
{
semGive (&partId->sem);
if (memPartAllocErrorRtn != NULL)
(* memPartAllocErrorRtn) (partId, nBytes);
errnoSet (S_memLib_NOT_ENOUGH_MEMORY);
if (partId->options & MEM_ALLOC_ERROR_SUSPEND_FLAG)
{
if ((taskIdCurrent->options & VX_UNBREAKABLE) == 0)
taskSuspend (0); /* suspend ourselves */
}
return (NULL);
}
pHdr = NODE_TO_HDR (pNode);
origpHdr = *pHdr;
/* now we split off from this block, the amount required by the user;
* note that the piece we are giving the user is at the end of the
* block so that the remaining piece is still the piece that is
* linked in the free list; if memAlignedBlockSplit returned NULL,
* it couldn't split the block because the split would leave the
* first block too small to be hung on the free list, so we continue
* trying blocks.
*/
pNewHdr =
memAlignedBlockSplit (partId, pHdr, nWords, partId->minBlockWords,
alignment);
if (pNewHdr != NULL)
{
pHdr = pNewHdr; /* give split off block */
break;
}
pNode = DLL_NEXT (pNode);
}
/* mark the block we're giving as not free */
pHdr->free = FALSE;
#ifdef WV_INSTRUMENTATION
EVT_OBJ_4 (OBJ, partId, memPartClassId, EVENT_MEMALLOC, partId, HDR_TO_BLOCK (pHdr), 2 * (pHdr->nWords), nBytes);
#endif
/* update allocation statistics */
partId->curBlocksAllocated++;
partId->cumBlocksAllocated++;
partId->curWordsAllocated += pHdr->nWords;
partId->cumWordsAllocated += pHdr->nWords;
semGive (&partId->sem);
return ((void *) HDR_TO_BLOCK (pHdr));
}
/******************************************************************************
*
* ArrayStore_T :: ~ArrayStore_T - decommission the ArrayStore
*
* This destructor should not ever be called. It detects the error condition
* which arises if the arrayStore is deleted.
*
* RETURNS: N/A
*
* NOMANUAL
*/
ArrayStore_T :: ~ArrayStore_T ()
{
cplusLogMsg ("run-time support for C++ arrays deleted\n",
0, 0, 0, 0, 0, 0);
taskSuspend (0);
}
/**
* Pauses a running task.
* Returns true on success, false if unable to pause or the task isn't running.
*/
bool Task::Suspend()
{
return HandleError(taskSuspend(m_taskID));
}
STATUS memPartFree
(
PART_ID partId, /* memory partition to add block to */
char *pBlock /* pointer to block of memory to free */
)
{
FAST BLOCK_HDR *pHdr;
FAST unsigned nWords;
FAST BLOCK_HDR *pNextHdr;
if (ID_IS_SHARED (partId)) /* partition is shared? */
{
if (smMemPartFreeRtn == NULL)
{
errno = S_smObjLib_NOT_INITIALIZED;
return (ERROR);
}
return ((*smMemPartFreeRtn) (SM_OBJ_ID_TO_ADRS (partId), pBlock));
}
/* partition is local */
if (OBJ_VERIFY (partId, memPartClassId) != OK)
return (ERROR);
if (pBlock == NULL)
return (OK); /* ANSI C compatibility */
pHdr = BLOCK_TO_HDR (pBlock);
/* get exclusive access to the partition */
semTake (&partId->sem, WAIT_FOREVER);
/* optional check for validity of block */
if ((partId->options & MEM_BLOCK_CHECK) &&
!memPartBlockIsValid (partId, pHdr, FALSE))
{
semGive (&partId->sem); /* release mutual exclusion */
if (memPartBlockErrorRtn != NULL)
(* memPartBlockErrorRtn) (partId, pBlock, "memPartFree");
if (partId->options & MEM_BLOCK_ERROR_SUSPEND_FLAG)
{
if ((taskIdCurrent->options & VX_UNBREAKABLE) == 0)
taskSuspend (0);
}
errnoSet (S_memLib_BLOCK_ERROR);
return (ERROR);
}
#ifdef WV_INSTRUMENTATION
EVT_OBJ_3 (OBJ, partId, memPartClassId, EVENT_MEMFREE, partId, pBlock, 2 * (pHdr->nWords));
#endif
nWords = pHdr->nWords;
/* check if we can coalesce with previous block;
* if so, then we just extend the previous block,
* otherwise we have to add this as a new free block */
if (PREV_HDR (pHdr)->free)
{
pHdr->free = FALSE; /* this isn't a free block */
pHdr = PREV_HDR (pHdr); /* coalesce with prev block */
pHdr->nWords += nWords;
}
else
{
pHdr->free = TRUE; /* add new free block */
dllInsert (&partId->freeList, (DL_NODE *) NULL, HDR_TO_NODE (pHdr));
}
/* check if we can coalesce with next block;
* if so, then we can extend our block delete next block from free list */
pNextHdr = NEXT_HDR (pHdr);
if (pNextHdr->free)
{
pHdr->nWords += pNextHdr->nWords; /* coalesce with next */
dllRemove (&partId->freeList, HDR_TO_NODE (pNextHdr));
}
/* fix up prev info of whatever block is now next */
NEXT_HDR (pHdr)->pPrevHdr = pHdr;
/* adjust allocation stats */
partId->curBlocksAllocated--;
partId->curWordsAllocated -= nWords;
semGive (&partId->sem);
return (OK);
}
void
CVMthreadSuspend(CVMThreadID *t)
{
taskSuspend(t->taskID);
}
void closeTrap(){
if (partProdTid!=0){return;}
taskSuspend(partProdTid);
}
void rootTask (long a0, long a1, long a2, long a3, long a4,
long a5, long a6, long a7, long a8, long a9)
{
WIND_TCB *pTcb;
TEST_START(0);
pTcb = taskTcb(taskIdSelf());
TEST_ASSERT(pTcb != NULL);
bsemid = semBCreate(0xffffffff,0);
TEST_ASSERT(bsemid == 0 && errno == S_semLib_INVALID_QUEUE_TYPE);
bsemid = semBCreate(SEM_Q_PRIORITY,0);
TEST_ASSERT(bsemid != 0);
mutexid = semMCreate(0xffffffff);
TEST_ASSERT(mutexid == 0 && errno == S_semLib_INVALID_QUEUE_TYPE);
mutexid = semMCreate(SEM_Q_PRIORITY|SEM_DELETE_SAFE|SEM_INVERSION_SAFE);
TEST_ASSERT(mutexid != 0);
taskSpawn("Test1",
20, /* low-pri */
0,
32768,
sem1Task,
taskIdSelf(),0,0,0,0,0,0,0,0,0);
taskSpawn("Test2",
10, /* intermediate-pri */
0,
32768,
sem2Task,
taskIdSelf(),0,0,0,0,0,0,0,0,0);
/* This runs at high-pri */
TEST_MARK();
TEST_ASSERT_OK(semTake(bsemid,WAIT_FOREVER));
TEST_MARK();
TEST_ASSERT_OK(taskSuspend(0));
TEST_MARK();
TEST_ASSERT_OK(semTake(mutexid,WAIT_FOREVER));
TEST_MARK();
TEST_ASSERT_OK(semGive(mutexid));
TEST_MARK();
TEST_ASSERT(taskIsReady(taskNameToId("Test2")));
TEST_ASSERT(!taskIsSuspended(taskNameToId("Test2")));
TEST_ASSERT_OK(taskDelay(1));
TEST_MARK();
TEST_CHECK_SEQUENCE(SEQ("root",1),
SEQ("Test2",1),
SEQ("root",1),
SEQ("Test2",1),
SEQ("Test1",2),
SEQ("Test2",1),
SEQ("root",1),
SEQ("Test1",1),
SEQ("root",2),
SEQ("Test2",1),
SEQ("root",1),
END_SEQ);
TEST_FINISH();
}
static void mutex_panic (const char *msg, int status)
{
printf ("ERROR: os_mutex: %s (%d, %d) - suspending task\n", msg, status, os_getErrno ());
fflush (stdout);
taskSuspend (taskIdSelf ());
}
/**
******************************************************************************
* @brief interrupt level handling of exceptions
* @param[in] pRegs : 发生异常时寄存器值
* @param[in] excNo : 异常中断号
*
* @retval None
******************************************************************************
*/
void
excExcHandle(void* pRegs, uint32_t excNo)
{
uint32_t regs[16];
uint32_t hfsr = *(uint32_t*)HARD_FALUT_FSR;
uint8_t mfsr = *(uint8_t*)MEM_FALUT_FSR;
uint8_t bfsr = *(uint8_t*)BUS_FALUT_FSR;
uint16_t ufsr = *(uint16_t*)USGE_FALUT_FSR;
memcpy(regs, pRegs, 64);
uint32_t sp = (uint32_t)pRegs;
intCnt++;
printf("\r\n");
switch (excNo)
{
case 3:
/* 输出hfsr信息 */
printf("=====硬fault=====\r\n");
if (hfsr & 0x00000002) printf("取向量时发生硬fault\r\n");
if (hfsr & 0x40000000) printf("上访产生硬fault\r\n");
if (hfsr & 0x80000000) printf("调试产生硬fault\r\n");
break;
case 4:
/* 输出mfsr信息 */
printf("=====存储器fault=====\r\n");
if (mfsr & 0x01) printf("取指令访问违规\r\n");
if (mfsr & 0x02) printf("数据访问违规\r\n");
if (mfsr & 0x08) printf("出栈错误\r\n");
if (mfsr & 0x10) printf("入栈错误\r\n");
if (mfsr & 0x80) printf("由0x%08x处的指令引起\r\n", *(uint32_t*)(MEM_FALUT_FAR));
break;
case 5:
/* 输出bsfr信息 */
printf("=====总线fault=====\r\n");
if (bfsr & 0x01) printf("取指令访问违规\r\n");
if (bfsr & 0x02) printf("精确的数据访问违规\r\n");
if (bfsr & 0x04) printf("不精确的数据访问违规\r\n");
if (bfsr & 0x08) printf("出栈错误\r\n");
if (bfsr & 0x10) printf("入栈错误\r\n");
if (bfsr & 0x80) printf("由0x%08x处的指令引起\r\n", *(uint32_t*)(BUS_FALUT_FAR));
else printf("无法追踪违规指令\r\n");
break;
case 6:
/* 输出ufsr信息 */
printf("=====用法fault=====\r\n");
if (ufsr & 0x0001) printf("指令解码错误\r\n");
if (ufsr & 0x0002) printf("程序视图切入ARM状态\r\n");
if (ufsr & 0x0008) printf("异常返回时视图非法的加载EXC_RETURN到PC\r\n");
if (ufsr & 0x0010) printf("程序视图执行协处理器相关指令\r\n");
if (ufsr & 0x0100) printf("未对齐\r\n");
if (ufsr & 0x0200) printf("除数为零\r\n");
break;
default:
printf("=====无效的错误中断:%d=====\r\n", excNo);
intCnt--;
return;
}
printf("exception occur regs : \r\n");
printf(" r0 =%08x r1 =%08x r2 =%08x r3 =%08x \r\n",
regs[8], regs[9], regs[10], regs[11]);
printf(" r4 =%08x r5 =%08x r6 =%08x r7 =%08x \r\n",
regs[0], regs[1], regs[2], regs[3]);
printf(" r8 =%08x r9 =%08x r10 =%08x r11 =%08x \r\n",
regs[4], regs[5], regs[6], regs[7]);
printf(" sp =%08x lr =%08x pc =%08x xpsr=%08x \r\n",
sp, regs[13], regs[14], regs[15]);
TASK_ID taskid = taskIdSelf();
printf("exception occur in task: 0x%x \n\r", (uint32_t)taskid);
printf("exception task name: %-16s \n\r", taskName(taskid));
taskSuspend(taskid); /* 挂起产生异常的任务 */
time_t curtm = time(NULL);
struct tm daytime;
(void)localtime_r(&curtm, &daytime);
printf("exception occur time: %04d-%02d-%02d %02d:%02d:%02d\r\n",
daytime.tm_year + 1900, daytime.tm_mon + 1, daytime.tm_mday,
daytime.tm_hour, daytime.tm_min, daytime.tm_sec);
/* 若此时调度器关闭，则打开调度器 */
if (xTaskGetSchedulerState() == taskSCHEDULER_SUSPENDED)
{
xTaskResumeAll();
}
intCnt--;
}
void cplusTerminate ()
{
cplusLogMsg ("Unhandled C++ exception resulted in call to terminate\n",
0, 0, 0, 0, 0, 0);
taskSuspend (0);
}
