void simulator()
{
initiateProcess();
oldSensors = getSensorValues();
/*create both message queues*/
if ((gateInQueue = msgQCreate(MAX_MESSAGES, MAX_MESSAGE_LENGTH, MSG_Q_FIFO)) == NULL)
printf("msgQCreate failed for read to gate in queue\n");
if ((gateOutQueue = msgQCreate(MAX_MESSAGES, MAX_MESSAGE_LENGTH, MSG_Q_FIFO)) == NULL)
printf("msgQCreate failed for update to gate out queue\n");
if((taskSpawn("TASK 0",70,0x100,2000,(FUNCPTR)inputWatcher,0,0,0,0,0,0,0,0,0,0))== ERROR)
{
printf("Error spawning input watcher task.\n");
}
/*TASK 1 = gate in*/
if((taskSpawn("GATE IN",70,0x100,2000,(FUNCPTR)gateIn,0,0,0,0,0,0,0,0,0,0))== ERROR)
{
printf("Error spawning gateIn task.\n");
}
/*TASK 2 = gate out*/
if((taskSpawn("GATE OUT",70,0x100,2000,(FUNCPTR)gateOut,0,0,0,0,0,0,0,0,0,0))== ERROR)
{
printf("Error spawning gateOut task.\n");
}
}
void s4_msgq2(void)
{
int taskIdOne;
int taskIdTwo;
if ((msgQueueId = msgQCreate(MAX_MESSAGES, MAX_MESSAGE_LENGTH,
MSG_Q_FIFO)) == NULL)
{
printf("msgQCreate in failed.\n");
}
if ((msgQueueIdNew = msgQCreate(MAX_MESSAGES, MAX_MESSAGE_LENGTH,
MSG_Q_FIFO)) == NULL)
{
printf("msgQCreate in failed.\n");
}
if ((taskIdOne = taskSpawn("task1", PRI, 0, 2000,
(FUNCTION)taskOne, 1, 2, 3, 4, 5)) == ERROR)
{
printf("taskSpawn taskOne failed.\n");
}
if ((taskIdTwo = taskSpawn("task2", PRI, 0, 2000,
(FUNCTION)taskTwo, 1, 2, 3, 4, 5)) == ERROR)
{
printf("taskSpawn taskTwo failed.\n");
}
}
STATUS pcmciaInit (void)
{
PCMCIA_CTRL *pCtrl = &pcmciaCtrl;
PCMCIA_CHIP *pChip = &pCtrl->chip;
PCMCIA_CARD *pCard;
PCMCIA_ADAPTER *pAdapter = NULL;
int sock;
int ix;
pcmciaMsgQId = msgQCreate (PCMCIA_MAX_MSGS, sizeof(PCMCIA_MSG), MSG_Q_FIFO);
if (pcmciaMsgQId == NULL)
return (ERROR);
pcmciadId = taskSpawn ("tPcmciad", pcmciadPriority,
pcmciadOptions, pcmciadStackSize,
(FUNCPTR) pcmciad, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
if (pcmciadId == ERROR)
return (ERROR);
for (ix = 0; ix < pcmciaAdapterNumEnt; ix++)
{
pAdapter = &pcmciaAdapter[ix];
if ((* pAdapter->initRtn) (pAdapter->ioBase,
pAdapter->intVec,
pAdapter->intLevel,
pAdapter->showRtn) == OK)
break;
}
if (ix >= pcmciaAdapterNumEnt)
return (ERROR);
semMInit (&cisMuteSem, SEM_Q_PRIORITY | SEM_DELETE_SAFE |
SEM_INVERSION_SAFE);
(void) intConnect ((VOIDFUNCPTR *)INUM_TO_IVEC(pAdapter->intVec),
(VOIDFUNCPTR)pcmciaCscIntr, 0);
if (pCtrl->socks != 0) /* if explicitely defined, use it */
pChip->socks = pCtrl->socks;
for (sock = 0; sock < pChip->socks; sock++)
{
pCard = &pCtrl->card[sock];
if ((pCard->cardStatus = (* pChip->status) (sock)) & PC_IS_CARD)
(void) cisGet (sock);
(void) (* pChip->cscPoll) (sock);
(void) (* pChip->cscOn) (sock, pChip->intLevel);
}
sysIntEnablePIC (pAdapter->intLevel);
return (OK);
}
UGL_OS_MSG_Q_ID uglOSMsgQCreate (
UGL_SIZE maxMsgs,
UGL_SIZE maxMsgSize
) {
return msgQCreate(maxMsgs, maxMsgSize, MSG_Q_FIFO);
}
/*
* =====================================================================
* Function:MsgQCreate()
* Description: This routine creates a message queue capable of holding up to maxMsgs messages,
* each up to maxMsgLength bytes long.
* The routine returns a message queue ID used to identify the created message queue
* in all subsequent calls to routines in this library.
* Input: mqname -- name of new message queue, just for Linux
* In Linux, the queue is identified by name.
* maxMsgs -- max message amount in queue
* maxMsgLength -- max size(bytes) of each message
* NOTE: Message in queue ordered by the msg_priority.
* High priority added before low ones,the same priority ordered one after one according to fifo.
* Tasks waiting for receiving messages if no available, ordered by fifo.
* Output: N/A
* Return: MsgQId on success or NULL otherwise.
*======================================================================
*/
MSG_QUEUE_ID MsgQCreate(const char* mqname, int maxMsgs, int maxMsgLength)
{
#ifdef LINUX_OS
struct mq_attr mqAttr = {0};
mqd_t* msgQId = AII_NULL;
if (mqname == AII_NULL)
{
return AII_NULL;
}
/* allocate memory in MsgQCreate
* free memory later in MsgQDelete */
msgQId = (mqd_t*)malloc(sizeof(mqd_t));
if (msgQId == AII_NULL)
{
return AII_NULL;
}
mqAttr.mq_maxmsg = maxMsgs;
mqAttr.mq_msgsize = maxMsgLength;
*msgQId = Mq_open(mqname, O_CREAT|O_EXCL|O_RDWR, 0600, (struct mq_attr*)&mqAttr);
if (*msgQId == -1)
{
free(msgQId);
return AII_NULL;
}
return msgQId;
#elif VXWORKS_OS
/* create msgQ with MSG_Q_FIFO, pending tasks in fifo order,
* the same with linux posix queue */
return msgQCreate(maxMsgs, maxMsgLength, MSG_Q_FIFO);
#endif
}
int testBoxing(){
#ifdef test
box toFill = {5,1,0,0,42};
box done;
int ret = 0;
MSG_Q_ID mid_boxing_todo = msgQCreate(10,4,0);
MSG_Q_ID mid_boxing_done = msgQCreate(10,4,0);
MSG_Q_ID mid_received_part = msgQCreate(10,4,0);
printf("Starting conditionning");
taskSpawn("testBoxing",10,0,15000,(FUNCPTR)startBoxing,0,0,0,0,0,0,0,0,0,0);
printf("Asking for batch production : \n");
printf("\tBatchNumber=%d\n",toFill.batchNumber);
printf("\tBoxSize=%d\n",toFill.size);
printf("\tPartsType=%d\n",toFill.partsType);
ret=msgQSend(mid_boxing_todo,(char*)&toFill,sizeof(toFill),WAIT_FOREVER,MSG_PRI_NORMAL);
if(ret==ERROR){
printf("[FAIL],");
printErrno(errnoGet());
printf("\n");
return;
}
printf("[OK]\n");
printf("Starting part type 1 generation...");
if (partProdTid!=0){
printf("[FAIL], Production already started.\n");
return;
}
partProdTid=taskSpawn("partProd1",5,0,10000,(FUNCPTR)startPartProd,1,0,0,0,0,0,0,0,0,0);
printf("[OK]\n");
for(;;){
printf("Waiting for boxes done...\n");
if (msgQReceive(mid_boxing_done,(char*)&done,sizeof(done),5)==-1){
return 1;
}
printf("Received :\n");
printf("\t Box size : %d\n",done.size);
printf("\t Part type : %d\n",done.partsType);
}
#else
printf("You must compile the application with 'test' defined to use this functionality!\n");
#endif
return 0;
}
initExceptionHandler()
{
/* initialize msgQ used by Master or Controller comm channel */
pMsgesToPHandlr = msgQCreate(20, sizeof(CNTLR_COMM_MSG), MSG_Q_PRIORITY);
startPhandler(PHANDLER_PRIORITY, STD_TASKOPTIONS, XSTD_STACKSIZE);
/* bring NDDS exception Pub/Sub for controller */
initialExceptionComm((void *)ExceptionPub_CallBack);
}
/*****************************************************************************
** user system initialization
*****************************************************************************/
void
user_sysinit( void )
{
printf( "\r\nCreating Queue QUEUE2" );
errno = 0;
queue2_id = msgQCreate( 1, 128, MSG_Q_PRIORITY );
if ( errno != OK )
printf( "... returned error %x\r\n", errno );
printf( "\r\nCreating Queue QUEUE3" );
errno = 0;
queue3_id = msgQCreate( 3, 128, MSG_Q_FIFO );
if ( errno != OK )
printf( "... returned error %x\r\n", errno );
printf( "\r\nCreating Semaphore SEM2" );
errno = 0;
sema42_id = semBCreate( SEM_Q_FIFO, SEM_EMPTY );
if ( errno != OK )
printf( "... returned error %x\r\n", errno );
printf( "\r\nCreating Semaphore SEM3" );
errno = 0;
sema43_id = semBCreate( SEM_Q_FIFO, SEM_EMPTY );
if ( errno != OK )
printf( "... returned error %x\r\n", errno );
/* Turn on round-robin timeslicing */
enableRoundRobin();
puts( "\r\nCreating Task 1" );
task1_id = taskSpawn( "TSK1", 10, 0, 0, task1,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0 );
puts( "\r\nCreating Task 2" );
task2_id = taskSpawn( "TSK2", 10, 0, 0, task2,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0 );
puts( "\r\nCreating Task 3" );
task3_id = taskSpawn( "TSK3", 10, 0, 0, task3,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0 );
}
/*command to execute the program*/
int main()
{
/*declare timespec for time stamp*/
struct timespec timeStamp;
timeStamp.tv_sec = 0;
timeStamp.tv_nsec = 0;
/*initialize the kernel clock to zero*/
clock_settime(CLOCK_REALTIME, &timeStamp);
/*get the current tick of the clock*/
startingTick = tickGet();
initiateProcess();
inState = WAITING;
outState = WAITING;
/*create both message queues*/
if ((inGateQueue = msgQCreate(MAX_MESSAGES, MAX_MESSAGE_LENGTH, MSG_Q_FIFO)) == NULL)
printf("msgQCreate failed for read to gate in queue\n");
if ((outGateQueue = msgQCreate(MAX_MESSAGES, MAX_MESSAGE_LENGTH, MSG_Q_FIFO)) == NULL)
printf("msgQCreate failed for update to gate out queue\n");
if((taskSpawn("INPUT_WATCHER",70,0x100,2000,(FUNCPTR)inputWatcher,0,0,0,0,0,0,0,0,0,0))== ERROR)
{
printf("Error spawning input watcher task.\n");
}
/*TASK 1 = gate in*/
if((taskSpawn("IN_GATE",70,0x100,2000,(FUNCPTR)inGate,0,0,0,0,0,0,0,0,0,0))== ERROR)
{
printf("Error spawning in gate task.\n");
}
/*TASK 2 = gate out*/
if((taskSpawn("OUT_GATE",70,0x100,2000,(FUNCPTR)outGate,0,0,0,0,0,0,0,0,0,0))== ERROR)
{
printf("Error spawning out gate task.\n");
}
}
MSG_Q_ID UDP_MsgQueueInit(void)
{
MSG_Q_ID id;
id = msgQCreate(UDP_MSG_QUEUE_NUM, UDP_MSG_QUEUE_BUF_SIZE, MSG_Q_PRIORITY);
if (id == NULL)
{
DBG_ERRO("[UDP] msgQCreate error!\n");
}
return id;
}
MSG_Q_ID startLogging(int nmsgs, int maxlen)
{
MpMisc.LogQ = msgQCreate(nmsgs, maxlen, MSG_Q_FIFO);
if (NULL == MpMisc.LogQ) {
osPrintf("cannot create LogQ! Quitting\n");
} else {
Zprintf("logQ is 0x%X\n", (int) MpMisc.LogQ, 0,0,0,0,0);
taskSpawn("Logger", logTaskPrio, 0, 8192, (FUNCPTR) printTask,
0, 0, 0,0,0,0,0,0,0,0);
}
return MpMisc.LogQ;
}
/*|><|************************************************************************
*
* Method Name: InitMessageThread
*
*
* Inputs: None
*
* Outputs: None
*
* Returns: None
*
* Logic Notes:
*
* Caveats:
*
************************************************************************|<>|*/
#ifdef WIN32 /* [ */
unsigned int __stdcall CMsgQueue::InitMessageThread(LPVOID argument1)
#elif defined(_VXWORKS) /* ] [ */
int CMsgQueue::InitMessageThread(int argument1, int arg2, int arg3, int arg4,
int arg5, int arg6, int arg7, int arg8, int arg9,
int arg10)
#elif defined(__pingtel_on_posix__) /* ] [ */
void * CMsgQueue::InitMessageThread(void * argument1)
#endif /* ] */
{
// MessageLoop is a static method requiring the 'this' object to be
// passed as an argument to gain access to internal data members.
CMsgQueue *poMsgQueue = (CMsgQueue *)argument1;
#ifdef WIN32 /* [ */
// Create the thread terminate Event object. The primary thread
// will set it when it wants this thread to cease operation.
if(!poMsgQueue->CreateThreadEvents())
{
// If Event creation fails, this is sufficient grounds to
// fail initialization. Things haven't gone as planned.
// Let's signal an Event and leave.
SetEvent (poMsgQueue->m_hMessageEvent);
return(0);
}
// Notify the primary thread that we are up
SetEvent (poMsgQueue->m_hMessageEvent);
#elif defined(_VXWORKS) /* ] [ */
const int MAX_NOTIFY_MESSAGES = MAX_NOTIFY_MESSAGES_DEFINE;
// Create a vxWorks Fifo message queue
poMsgQueue->m_ulMsgQID =
msgQCreate(MAX_NOTIFY_MESSAGES, // Max Messages that can be queued
sizeof(CMessage *), // Max bytes in a message
MSG_Q_FIFO ); // message queue options
if(poMsgQueue->m_ulMsgQID == NULL)
return(0);
#elif defined(__pingtel_on_posix__)
/* set up the OsMsgQ */
poMsgQueue->m_pMsgQ = new OsMsgQ(MAX_NOTIFY_MESSAGES_DEFINE,
OsMsgQ::DEF_MAX_MSG_LEN, OsMsgQ::Q_PRIORITY);
if(!poMsgQueue->m_pMsgQ)
return(0);
#endif /* ] */
// Enter Message Loop
poMsgQueue->MessageLoop();
return(0);
}
startLockParser(int priority, int taskoptions, int stacksize)
{
DPRINT1(-1,"sizeof(Lock_Cmd) = %d\n", sizeof(Lock_Cmd));
if (pMsgesToLockParser == NULL)
pMsgesToLockParser = msgQCreate(300,sizeof(Lock_Cmd),MSG_Q_PRIORITY);
if (pMsgesToLockParser == NULL)
{
errLogSysRet(LOGIT,debugInfo,"could not create Lock Parser MsgQ, ");
return;
}
if (taskNameToId("tLkParser") == ERROR)
taskSpawn("tLkParser",priority,0,stacksize,LkParser,pMsgesToLockParser,
2,3,4,5,6,7,8,9,10);
}
startAupdt(int priority, int taskoptions, int stacksize)
{
if (pMsgesToAupdt == NULL)
pMsgesToAupdt = msgQCreate(300,100,MSG_Q_PRIORITY);
if (pMsgesToAupdt == NULL)
{
errLogSysRet(LOGIT,debugInfo,
"could not create X Parser MsgQ, ");
return;
}
if (taskNameToId("tAupdt") == ERROR)
taskSpawn("tAupdt",priority,0,stacksize,Aupdt,pMsgesToAupdt,
2,3,4,5,6,7,8,9,10);
}
/*
* Spawn the Non-Master Tune Task
*
* Author: Greg Brissey 12/20/04
*/
int startTuneTask(int priority, int taskoptions, int stacksize)
{
if (pTuneMsgQ == NULL)
{
pTuneMsgQ = msgQCreate(10, sizeof(TUNE_MSG), MSG_Q_FIFO);
if (pTuneMsgQ == NULL)
{
errLogSysRet(LOGIT,debugInfo,
"startCntlrTuneTask: Failed to allocate pTuneMsgQ MsgQ:");
return(ERROR);
}
}
if (taskNameToId("tTune") == ERROR)
taskSpawn("tTune",priority,0,stacksize,cntlrTune,1,2,3,4,5,6,7,8,9,10);
}
static void rootTask(long a0, long a1, long a2, long a3, long a4,
long a5, long a6, long a7, long a8, long a9)
{
TASK_ID tid;
int ret, n;
traceobj_enter(&trobj);
qid = msgQCreate(NMESSAGES, sizeof(int), MSG_Q_PRIORITY);
traceobj_assert(&trobj, qid != 0);
traceobj_mark(&trobj, 3);
ret = taskPrioritySet(taskIdSelf(), 10);
traceobj_assert(&trobj, ret == OK);
traceobj_mark(&trobj, 4);
tid = taskSpawn("peerTask",
11,
0, 0, peerTask, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
traceobj_assert(&trobj, tid != ERROR);
traceobj_mark(&trobj, 5);
n = 0;
do
ret = msgQSend(qid, (char *)&messages[n], sizeof(int), NO_WAIT, MSG_PRI_URGENT);
while(n++ < NMESSAGES && ret != ERROR);
traceobj_assert(&trobj, ret == ERROR && errno == S_objLib_OBJ_UNAVAILABLE && n == NMESSAGES + 1);
traceobj_mark(&trobj, 6);
ret = taskDelay(10);
traceobj_assert(&trobj, ret == OK);
traceobj_mark(&trobj, 7);
ret = msgQNumMsgs(qid);
traceobj_assert(&trobj, ret == 0);
traceobj_verify(&trobj, tseq, sizeof(tseq) / sizeof(int));
traceobj_exit(&trobj);
}
static void rootTask(long a0, long a1, long a2, long a3, long a4,
long a5, long a6, long a7, long a8, long a9)
{
int ret, msg, n;
traceobj_enter(&trobj);
traceobj_mark(&trobj, 1);
qid = msgQCreate(NMESSAGES, sizeof(msg), MSG_Q_FIFO);
traceobj_assert(&trobj, qid != 0);
traceobj_mark(&trobj, 2);
for (msg = 0; msg < NMESSAGES; msg++) {
ret = msgQSend(qid, (char *)&msg, sizeof(msg), NO_WAIT, MSG_PRI_NORMAL);
traceobj_assert(&trobj, ret == OK);
}
traceobj_mark(&trobj, 3);
ret = msgQSend(qid, (char *)&msg, sizeof(msg), WAIT_FOREVER, MSG_PRI_URGENT);
traceobj_assert(&trobj, ret == OK);
traceobj_mark(&trobj, 4);
ret = msgQReceive(qid, (char *)&msg, sizeof(int), WAIT_FOREVER);
traceobj_assert(&trobj, ret == sizeof(int) && msg == 10);
traceobj_mark(&trobj, 5);
for (n = 1; n < NMESSAGES; n++) { /* peer task read #0 already. */
ret = msgQReceive(qid, (char *)&msg, sizeof(int), WAIT_FOREVER);
traceobj_assert(&trobj, ret == sizeof(int) && msg == n);
}
traceobj_mark(&trobj, 6);
ret = msgQReceive(qid, (char *)&msg, sizeof(int), WAIT_FOREVER);
traceobj_assert(&trobj, ret == ERROR && errno == S_objLib_OBJ_DELETED);
traceobj_mark(&trobj, 7);
traceobj_exit(&trobj);
}
/*
* Admin functions API: install of driver and then of devices.
* uninstall of devices and then of driver
* direct driver uninstall is also supported
*/
int drvInstall()
{
if (semMAdmin==0)
{//The semaphore hasn't been created yet : the driver has not been installed yet,
//let's do it
semMAdmin = semMCreate(SEM_Q_FIFO);
if(semMAdmin==0)
{
errnoSet(SEM_ERR);
return -1;
}
} else {
errnoSet(ALREADY_INSTALLED);
return -1;
}
isrmq = msgQCreate(10,4,0); //Create a msg queue with 10 msg max,
//4 byte per msg max, and msgs filled up in fifo order
if (isrmq == NULL)
{
errnoSet(MSGQ_ERR);
return -1;
}
tMsgDispatchID = taskSpawn("tMsgDispatch",0,0,1000,msgDispatch,(int)semMAdmin,0,0,0,0,0,0,0,0,0);
//This task will dispatch a msg received by the isr and sleep the rest of the time.
//It needs to be fast to prevent isr msg queue to fill up, hence the high priority
//It's also nice for tests if his priority is superior than the shell's one, which
//is set to 1
if (tMsgDispatchID==-1)
{
msgQDelete(isrmq);
errnoSet(TASK_ERR);
return -1;
}
numPilote=iosDrvInstall(0,0,devOpen,devClose,devRead,0,0); //Register device in ios
if (numPilote == -1)
{
msgQDelete(isrmq);
taskDelete(tMsgDispatchID);
errnoSet(INSTALL_ERR);
return -1;
}
return 0;
}
/**
******************************************************************************
* @brief logmsg 任务初始化
* @param[in] stacksize : 任务栈大小
*
* @retval OK : 初始化成功
* @retval ERROR : 初始化失败
******************************************************************************
*/
status_t
loglib_init(uint32_t stacksize)
{
if (the_logmsg_taskid != NULL)
{
return OK; /* already called */
}
stacksize = (stacksize == 0) ? TASK_STK_SIZE_LOGMSG : stacksize;
the_logmsg_qid = msgQCreate(MAX_MSGS);
D_ASSERT(the_logmsg_qid != NULL);
the_logmsg_taskid = taskSpawn((const signed char * const ) "LogMsg",
TASK_PRIORITY_LOGMSG, stacksize, (OSFUNCPTR) loglib_loop, 0);
D_ASSERT(the_logmsg_taskid != NULL);
return (OK);
}
static int __wind_msgq_create(struct pt_regs *regs)
{
int nb_msgs, length, flags;
wind_msgq_t *msgq;
MSG_Q_ID qid;
nb_msgs = __xn_reg_arg1(regs);
length = __xn_reg_arg2(regs);
flags = __xn_reg_arg3(regs);
msgq = (wind_msgq_t *)msgQCreate(nb_msgs, length, flags);
if (!msgq)
return wind_errnoget();
qid = msgq->handle;
return __xn_safe_copy_to_user((void __user *)__xn_reg_arg4(regs), &qid,
sizeof(qid));
}
void init(void)
{
int tacheSynchID;
mailBox = msgQCreate( 2, 4, MSG_Q_FIFO);
if (mailBox!=NULL)
printf("Boite aux lettres créée\n");
else
printf("ERREUR\n");
*GPT_IR = 0x1;
*GPT_CR = 0x1c3;
*GPT_PR = 0x0;
*GPT_OCR1 = 24000000;
// Initialisation des GPIO pour la broche GPIO9
*IOMUXC1_GPIO9=0x05;
*IOMUXC2_GPIO9=0x1B0B0;
erreur = intConnect(87, (VOIDFUNCPTR)ITserver, 0);
if (erreur !=OK)
{
printf("ERREUR intConnect\n");
taskDelete(0);
}
erreur = intEnable(87);
if (erreur != OK)
{
printf("Erreur intEnable\n");
taskDelete(0);
}
tacheSynchID = taskSpawn(tacheSynch, 100, 0, 5000, (FUNCPTR)tacheSynch, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
taskDelete(0);
}
int initConditionnement(Lot* premierLot) {
semCapteurCarton = semMCreate(SEM_Q_FIFO);
semEtatImp = semMCreate(SEM_Q_FIFO);
semCapteurPalette = semMCreate(SEM_Q_FIFO);
semEtatEmb = semMCreate(SEM_Q_FIFO);
semCompteurPalette = semMCreate(SEM_Q_FIFO);
semClapet = semMCreate(SEM_Q_FIFO);
fileConvoyage = msgQCreate(MAXFILECONVOYAGE, sizeof(Carton), MSG_Q_FIFO
| MSG_Q_INTERRUPTIBLE);
tidTraitementCarton = taskSpawn("traitement_carton", 100, 0, 128,
(FUNCPTR) traitementCarton, (int) fileConvoyage,
(int) semCapteurCarton, (int) semEtatImp, (int) semCompteurPalette,
(int) premierLot, 0, 0, 0, 0, 0);
tidTraitementPalette = taskSpawn("traitement_palette", 100, 0, 128,
(FUNCPTR) traitementPalette, (int) fileConvoyage,
(int) semCapteurPalette, (int) semEtatEmb,
(int) semCompteurPalette, 0, 0, 0, 0, 0, 0);
wdReparation = wdCreate();
return OK;
}
static int __wind_msgq_create(struct task_struct *curr, struct pt_regs *regs)
{
int nb_msgs, length, flags;
wind_msgq_t *msgq;
MSG_Q_ID qid;
if (!__xn_access_ok
(curr, VERIFY_WRITE, __xn_reg_arg4(regs), sizeof(qid)))
return -EFAULT;
nb_msgs = __xn_reg_arg1(regs);
length = __xn_reg_arg2(regs);
flags = __xn_reg_arg3(regs);
msgq = (wind_msgq_t *)msgQCreate(nb_msgs, length, flags);
if (!msgq)
return wind_errnoget();
qid = msgq->handle;
__xn_copy_to_user(curr, (void __user *)__xn_reg_arg4(regs), &qid,
sizeof(qid));
return 0;
}
//------------------------------------------------------------------------------
tOplkError timeru_addInstance(void)
{
OPLK_MEMSET(&timeruInstance_l, 0, sizeof(timeruInstance_l));
/* initialize message queue */
if ((timeruInstance_l.msgQueue = msgQCreate(TIMERU_MAX_MSGS,
sizeof(unsigned long),
MSG_Q_FIFO)) == NULL)
return kErrorTimerThreadError;
/* initialize mutexe for synchronisation */
if ((timeruInstance_l.mutex = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE |
SEM_INVERSION_SAFE)) == NULL)
return kErrorTimerThreadError;
/* create user timer task */
if ((timeruInstance_l.taskId =
taskSpawn("tTimerEplu", EPL_TASK_PRIORITY_UTIMER, 0, EPL_TASK_STACK_SIZE,
(FUNCPTR)processTask,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0)) == ERROR)
return kErrorTimerThreadError;
return kErrorOk;
}
startFidCtStatPub(int priority, int taskoptions, int stacksize)
{
/* if (pFidCtPubSem == NULL) */
if (pFidCtPubMsgQ == NULL)
{
/* char *tmpbuf = (char*) malloc(1024); see Bugzilla bug# 150 */
/* pFidCtPubSem = semBCreate(SEM_Q_FIFO,SEM_EMPTY); */
/* pFidCtPubSem = semBCreate(SEM_Q_PRIORITY,SEM_EMPTY); */
pFidCtPubMsgQ = msgQCreate(10,sizeof(FidCt_Stat),MSG_Q_PRIORITY);
/* if ( (pFidCtPubSem == NULL) ) */
if ( (pFidCtPubMsgQ == NULL) )
{
errLogSysRet(LOGIT,debugInfo,
"startConsoleStatPub: Failed to allocate pFidCtPubSem Semaphore:");
return(ERROR);
}
}
/* DPRINT1(-1,"pFidCtPubSem: 0x%lx\n",pFidCtPubSem); */
if (taskNameToId("tFidCtPub") == ERROR)
taskSpawn("tFidCtPub",priority,0,stacksize,pubFidCtStatus,1,
2,3,4,5,6,7,8,9,10);
}
STATUS excLibInit(
void
)
{
STATUS status;
/* Check if already running */
if (excLibInstalled == TRUE)
{
if (excTaskId == 0)
{
status = ERROR;
}
else
{
status = OK;
}
}
else
{
/* Create exception message queue */
excMsgQId = msgQCreate(EXC_MAX_MSGS, sizeof(EXC_MSG), MSG_Q_FIFO);
if (excMsgQId == NULL)
{
status = ERROR;
}
else
{
/* Start exception task */
excTaskId = taskSpawn(
"tExcTask",
excTaskPriority,
excTaskOptions,
excTaskStackSize,
(FUNCPTR) excTask,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0,
(ARG) 0
);
if (excTaskId == 0)
{
status = ERROR;
}
else
{
/* Mark as intalled */
excLibInstalled = TRUE;
status = OK;
}
}
}
return status;
}
OSI_LIB_HANDLE msg_queue_create(
int MaxMSGs, /* max messages that can be queued */
int MaxMsgLength, /* max bytes in a message */
int options /* Non useful */
)
{
#ifdef WINDOWS_OS
MSG_HANDLE_ENTRY_PTR MsgHandleEntryPtr;
/* Create message handle entry */
if ((MsgHandleEntryPtr=(MSG_HANDLE_ENTRY_PTR)
malloc(sizeof(MSG_HANDLE_ENTRY)))==AII_NULL)
{
goto err;
}
/* Create message buffer readable event */
if ((MsgHandleEntryPtr->ReadEnableHandle=CreateEvent(
AII_NULL,
AII_TRUE, /* Manually settable */
AII_FALSE, /* Initialized to non-signaled */
AII_NULL /* No name */
))==AII_NULL)
{
printf("CreateEvent failed (%d)\n", GetLastError());
free(MsgHandleEntryPtr);
return AII_NULL;
}
/* Create message buffer writable event */
if ((MsgHandleEntryPtr->WriteEnableHandle=CreateEvent(
AII_NULL, // default security attributes
AII_TRUE, // Manually settable
AII_TRUE, // Initialized to signaled
AII_NULL // No name
))==AII_NULL)
{
printf("CreateEvent failed (%d)\n", GetLastError());
free(MsgHandleEntryPtr);
return AII_NULL;
}
MsgHandleEntryPtr->ReadTimeout=INFINITE;
MsgHandleEntryPtr->WriteTimeout=0;
MsgHandleEntryPtr->MsgLength=MaxMsgLength*MaxMSGs+1;
MsgHandleEntryPtr->HeadIndex=0;
MsgHandleEntryPtr->RearIndex=0;
/* Allocate memory space to store message */
if ((MsgHandleEntryPtr->MsgBuffer=
malloc(MsgHandleEntryPtr->MsgLength))==AII_NULL)
{
goto err;
}
return (OSI_LIB_HANDLE)MsgHandleEntryPtr;
err:
printf("Memory limited!\n");
return AII_NULL;
#endif
#ifdef VXWORKS_OS
return (OSI_LIB_HANDLE) msgQCreate(MaxMSGs,MaxMsgLength,options);
#endif
#ifdef LINUX_OS
printf("This function is not yet available Linux!\n");
return AII_ERROR;
#endif
}
//STATUS dac3550DevCreate (char *devName, int port, int irq)
DEV_HDR* dac3550DevCreate (char *devName, int port, int irq)
{
SND_DEV *pDev;
if (DrvNum < 1)
{
errno = S_ioLib_NO_DRIVER;
return ERROR;
}
if(dac3550Dev)
return &(dac3550Dev->devHdr);
pDev = (SND_DEV *)malloc (sizeof(SND_DEV));
if (!pDev) return 0;
bzero ((char *)pDev, sizeof(SND_DEV));
if(port==0) port = (int)AT91C_BASE_SSC1;
if(irq==0) irq = AT91C_ID_SSC1;
pDev->port = port;
pDev->irq = irq;
/* pDev->dma8 = dma8;
pDev->dma16 = dma16;
*/
pDev->devSem = semBCreate (SEM_Q_FIFO, SEM_FULL);
pDev->intSem = semCCreate (SEM_Q_FIFO, 0);
pDev->bufSem = semCCreate (SEM_Q_FIFO, MAX_DMA_MSGS);
pDev->dmaQ = msgQCreate (MAX_DMA_MSGS, sizeof (DMA_MSG), MSG_Q_FIFO);
pDev->dmaIndex = 0;
if (createDmaBuffer () < 0)
{
free (pDev);
return 0;
}
if (dsp_init (pDev) < 0)
{
free (pDev);
return 0;
}
if (iosDevAdd (&pDev->devHdr, devName, DrvNum) == ERROR)
{
free ((char *)pDev);
return 0;
}
/*
pDev->tid = taskSpawn ("tSndTask", TASK_PRIORITY, TASK_OPTIONS,
TASK_STACK_SIZE, dspHelperTask, (int)pDev,
0, 0, 0, 0, 0, 0, 0, 0, 0);
if (pDev->tid == ERROR)
{
free (pDev);
return ERROR;
}
*/
intConnect (INUM_TO_IVEC ( (irq)), dspInterrupt, (int)pDev);
dac3550Dev = pDev;
return &(pDev->devHdr);
}
void state_handling_init(void)
{
INT32S config_load_flag;
StateHandlingQ = OSQCreate(state_handling_q_stack, STATE_HANDLING_QUEUE_MAX);
ApQ = msgQCreate(AP_QUEUE_MAX, AP_QUEUE_MAX, AP_QUEUE_MSG_MAX_LEN);
nvmemory_init();
config_load_flag = ap_state_config_load();
ap_state_resource_init();
ap_state_config_initial(config_load_flag);
#if ENABLE_CHECK_RTC == 1
if (config_load_flag != STATUS_FAIL)
{
ap_state_config_data_time_check();
}
#endif
ap_state_handling_calendar_init();
ap_state_handling_storage_id_set(NO_STORAGE);
{
DISPLAY_ICONSHOW icon = {216, 144, TRANSPARENT_COLOR, 0, 0};
INT32U i, *buff_ptr, color_data, cnt;
INT32U display_frame0;
display_battery_low_frame0 = (INT32U) gp_malloc_align(icon.icon_w * icon.icon_h * 2, 64);
DBG_PRINT("batteryLow_display_buf_addr = 0x%x \r\n",display_frame0);
buff_ptr = (INT32U *) display_battery_low_frame0;
color_data = 0x8C71ul|(0x8C71ul<<16);
cnt = (icon.icon_w * icon.icon_h * 2) >> 2;
for (i=0 ; i<cnt ; i++) {
*buff_ptr++ = color_data;
}
DBG_PRINT("batteryLowshow! \r\n");
{
INT32U size, read_buf;
INT16U logo_fd;
INT8U *zip_buf;
INT32U icon_buffer;
logo_fd = nv_open((INT8U*)"INSERTSDC.GPZP");
if (logo_fd != 0xFFFF) {
size = nv_rs_size_get(logo_fd);
read_buf = (INT32S) gp_malloc(size);
if (!read_buf) {
DBG_PRINT("allocate BACKGROUND.GPZP read buffer fail.[%d]\r\n", size);
}
if (nv_read(logo_fd, (INT32U) read_buf, size)) {
DBG_PRINT("Failed to read icon_buffer resource\r\n");
gp_free((void *) read_buf);
}
zip_buf = (INT8U *)read_buf;
if (gpzp_decode((INT8U *) &zip_buf[4], (INT8U *) display_battery_low_frame0) == STATUS_FAIL) {
DBG_PRINT("Failed to unzip GPZP file\r\n");
gp_free((void *) read_buf);
gp_free((void *) icon_buffer);
read_buf = 0;
icon_buffer = 0;
}
gp_free((void *) read_buf);
}
}
}
DBG_PRINT("state_handling_init OK!\r\n");
}
/* Task to be spawned by startMASSpeed. This task it to use a msgQ for
a dual purpose. #1 is to have waiting on the msgQ time out every few
seconds so we can get the current speed and put it in the NDDS structure
for sending to vnmrj. #2 is that any request coming from vnmrj will
be put into the msgQ and promptly dealt with.
Thus, the same task sitting on a single msgQ will be waiting for
requests from vnmrj and basically looping to get the current speed
and send back to vnmrj.
*/
void MASSpeed()
{
int response;
int status, mStatus;
char msgBuf[MAX_MSG_LEN];
long cycle=0;
int delay;
int len;
DPRINT(1, "*** Starting MASSpeed in Sleep State\n");
// Start up in sleep state. It will be awaken when spintype is set
MasSpeedTaskSleeping = TRUE;
msgToMASSpeedCtl = msgQCreate(MAX_MSGS, MAX_MSG_LEN, MSG_Q_FIFO);
checkModuleChange();
// Get all param values and set into pCurrentStatBlock
getAllParams();
// convert CYCLE_DELAY in sec to clock ticks
delay = clkRate * CYCLE_DELAY;
while(1) {
// Now and then, check to see if the probe module has changed.
// If it has, get all info and send back to vnmrj.
if(!MasSpeedTaskSleeping) {
if(cycle >= 20) {
checkModuleChange();
cycle = 0;
}
cycle++;
}
/* Either catch a message from vnmrj, or timeout and get the
current speed and send it.
*/
mStatus = msgQReceive(msgToMASSpeedCtl, msgBuf, MAX_MSG_LEN, delay);
// If sleeping, just continue for another cycle of delay until
// we are not sleeping. Ignore all commands that come in.
if(MasSpeedTaskSleeping)
continue;
/* Timeout of msgQReceive will return ERROR. In our case,
this simply means to get the speed and send it on.
*/
if(mStatus == ERROR) {
/* See if we need to exit this task */
if(exitMas) {
DPRINT(0, "Exiting MASSpeed Task\n");
masSpeedTaskId = 0;
close(masSpeedPort);
return;
}
sendToMASSpeed("S\n");
status = getMASSpeedResponseInt(&response);
if(status == -1 && response == -2)
errLogRet(LOGIT,debugInfo,
"*** Trying to set speed higher than limit.\n");
else if(status == -1)
errLogRet(LOGIT,debugInfo,
"*** Speed Controller had problem with 'S' command.\n");
else {
// Set the new response into the NDDS structure
pCurrentStatBlock->AcqSpinAct = response;
// Tell it we have changed a value
sendConsoleStatus();
DPRINT1(3, "MAS Current Speed = %d\n", response);
// Save current and prev for setting LED
prevMasSpeed = curMasSpeed;
curMasSpeed = response;
// Set the LED as appropriate
checkSpinLEDMas ();
}
}
/* If msgQReceive does not return ERROR, then we received a message
which we need to process.
*/
else {
processMasMessage(msgBuf);
}
}
}
