void sem1Task (long a0, long a1, long a2, long a3, long a4,
long a5, long a6, long a7, long a8, long a9)
{
WIND_TCB *pTcb = taskTcb(taskIdSelf());
TEST_ASSERT(pTcb != NULL);
TEST_ASSERT(semFlush(mutexid) == ERROR &&
errno == S_semLib_INVALID_OPERATION);
TEST_ASSERT_OK(semFlush(bsemid));
TEST_MARK();
TEST_ASSERT_OK(semTake(mutexid,WAIT_FOREVER));
TEST_MARK();
TEST_ASSERT_OK(semGive(bsemid));
TEST_MARK();
TEST_ASSERT_OK(semGive(mutexid));
TEST_MARK(); /* Should not pass this mark */
}
void EventImpl::setImpl()
{
if (_auto)
{
if (semGive(_sem) != OK)
throw SystemException("cannot set event");
}
else
{
_state = true;
if (semFlush(_sem) != OK)
throw SystemException("cannot set event");
}
}
static int __wind_sem_flush(struct task_struct *curr, struct pt_regs *regs)
{
xnhandle_t handle = __xn_reg_arg1(regs);
wind_sem_t *sem;
sem = (wind_sem_t *)xnregistry_fetch(handle);
if (!sem)
return S_objLib_OBJ_ID_ERROR;
if (semFlush((SEM_ID)sem) == ERROR)
return wind_errnoget();
return 0;
}
/*
Signal a condition and wakeup the all the waiters. Note: this may be called before or after to the waiter waiting.
*/
PUBLIC void mprSignalMultiCond(MprCond *cp)
{
mprLock(cp->mutex);
#if ME_WIN_LIKE
/* Pulse event */
SetEvent(cp->cv);
ResetEvent(cp->cv);
#elif VXWORKS
/* Reset sem count and then give once. Prevents accumulation */
while (semTake(cp->cv, 0) == OK) ;
semGive(cp->cv);
semFlush(cp->cv);
#else
pthread_cond_broadcast(&cp->cv);
#endif
mprUnlock(cp->mutex);
}
/***********************************************************
*
* adcOvrFlow - Interrupt Service Task
*
* Action to take:
* 3. Update adcState, flush semaphore
*
*/
static VOID adcOvrFlow(register ADC_ID pAdcId)
{
int adcReset();
FOREVER
{
semTake(pSemAdcOvrFlow,WAIT_FOREVER); /* wait here for interrupt */
semTake(pAdcId->pAdcMutex,WAIT_FOREVER); /*Mutual Exclusion Semiphore */
pAdcId->adcState = ADC_OVERFLOW;
semGive(pAdcId->pAdcMutex); /* give Mutex back */
semFlush(pAdcId->pSemAdcStateChg);/* release any task Block on statchg for FIFO */
}
}
void
disable_task_servo(void)
{
int j;
if ( servo_enabled == 1 ) {
servo_enabled = 0;
semFlush(sm_1to1_sem);
semFlush(sm_1to2_sem);
semFlush(sm_1to3_sem);
semFlush(sm_1to4_sem);
semFlush(sm_1to5_sem);
semFlush(sm_60Hz_sem);
} else
fprintf( stderr, "task servo is not on!\n" );
}
/**
* Free the PID object
*/
PIDController::~PIDController()
{
semFlush(m_semaphore);
delete m_controlLoop;
}
STATUS
c792Init (UINT32 addr, UINT32 addr_inc, int nadc, UINT16 crateID)
{
int ii, res, rdata, errFlag = 0;
int boardID = 0;
unsigned long laddr, lladdr;
volatile struct c792_ROM_struct *rp;
/* Check for valid address */
if(addr==0) {
printf("c792Init: ERROR: Must specify a Bus (VME-based A32/A24) address for QDC 0\n");
return(ERROR);
}else if(addr < 0x00ffffff) { /* A24 Addressing */
if((addr_inc==0)||(nadc==0))
nadc = 1; /* assume only one QDC to initialize */
/* get the QDCs address */
#ifdef VXWORKS
res = sysBusToLocalAdrs(0x39,(char *)addr,(char **)&laddr);
if (res != 0) {
printf("c792Init: ERROR in sysBusToLocalAdrs(0x39,0x%x,&laddr) \n",addr);
return(ERROR);
}
#else
res = vmeBusToLocalAdrs(0x39,(char *)addr,(char **)&laddr);
if (res != 0) {
printf("c792Init: ERROR in vmeBusToLocalAdrs(0x39,0x%x,&laddr) \n",addr);
return(ERROR);
}
#endif
c792MemOffset = laddr - addr;
}else{ /* A32 Addressing */
if((addr_inc==0)||(nadc==0))
nadc = 1; /* assume only one QDC to initialize */
/* get the QDC address */
#ifdef VXWORKS
res = sysBusToLocalAdrs(0x09,(char *)addr,(char **)&laddr);
if (res != 0) {
printf("c792Init: ERROR in sysBusToLocalAdrs(0x09,0x%x,&laddr) \n",addr);
return(ERROR);
}
#else
res = vmeBusToLocalAdrs(0x09,(char *)addr,(char **)&laddr);
if (res != 0) {
printf("c792Init: ERROR in vmeBusToLocalAdrs(0x09,0x%x,&laddr) \n",addr);
return(ERROR);
}
#endif
c792MemOffset = laddr - addr;
}
lladdr = laddr;
Nc792 = 0;
for (ii=0;ii<nadc;ii++)
{
c792p[ii] = (struct c792_struct *)(laddr + ii*addr_inc);
c792pl[ii] = (struct c792_struct *)(lladdr + ii*addr_inc);
/* Check if Board exists at that address */
#ifdef VXWORKS
res = vxMemProbe((char *) &(c792p[ii]->rev),0,2,(char *)&rdata);
#else
res = vmeMemProbe((char *) &(c792p[ii]->rev),2,(char *)&rdata);
#endif
if(res < 0)
{
printf("c792Init: ERROR: No addressable board at addr=0x%x\n",(UINT32) c792p[ii]);
c792p[ii] = NULL;
/*sergey: errFlag = 1;*/
break;
}
else
{
/* Check if this is a Model 792 */
rp = (struct c792_ROM_struct *)((UINT32)c792p[ii] + C792_ROM_OFFSET);
boardID = ((c792Read(&rp->ID_3)&(0xff))<<16) +
((c792Read(&rp->ID_2)&(0xff))<<8) + (c792Read(&rp->ID_1)&(0xff));
/* set 1 for tdc, 2 for adc */
if(boardID == C775_BOARD_ID) use792[Nc792] = 1;
else if(boardID == C792_BOARD_ID) use792[Nc792] = 2;
else
{
printf(" ERROR: Board ID does not match: %d \n",boardID);
return(ERROR);
}
}
Nc792++;
#ifdef VXWORKS
printf("Initialized QDC ID %d at address 0x%08x \n",ii,(UINT32) c792p[ii]);
#else
printf("Initialized QDC ID %d at VME (USER) address 0x%08x (0x%08x) \n",ii,
(UINT32)(c792p[ii]) - c792MemOffset, (UINT32) c792p[ii]);
#endif
}
#ifdef VXWORKS
/* Initialize/Create Semephore */
if(c792Sem != 0) {
semFlush(c792Sem);
semDelete(c792Sem);
}
c792Sem = semBCreate(SEM_Q_PRIORITY,SEM_EMPTY);
if(c792Sem <= 0) {
printf("c792Init: ERROR: Unable to create Binary Semephore\n");
return(ERROR);
}
#endif
/* Disable/Clear all QDCs */
for(ii=0;ii<Nc792;ii++) {
C792_EXEC_SOFT_RESET(ii);
C792_EXEC_DATA_RESET(ii);
c792Write(&c792p[ii]->intLevel,0); /* Disable Interrupts */
c792Write(&c792p[ii]->evTrigger,0); /* Zero interrupt trigger count */
c792Write(&c792p[ii]->crateSelect,crateID); /* Set Crate ID Register */
c792Write(&c792p[ii]->bitClear2,C792_INCR_ALL_TRIG); /* Increment event count only on
accepted gates */
c792EventCount[ii] = 0; /* Initialize the Event Count */
c792EvtReadCnt[ii] = -1; /* Initialize the Read Count */
}
/* Initialize Interrupt variables */
c792IntID = -1;
c792IntRunning = FALSE;
c792IntLevel = 0;
c792IntVec = 0;
c792IntRoutine = NULL;
c792IntArg = 0;
c792IntEvCount = 0;
#ifdef VXWORKSPPC
bzero((char *)&c792Fpr,sizeof(c792Fpr));
#endif
if(Nc792 > 0) printf("c792Init: %d QDC(s) successfully initialized\n",Nc792);
if(errFlag > 0)
{
printf("c792Init: ERROR: Unable to initialize all QDC Modules, errFlag=%d\n",errFlag);
return(ERROR);
}
else
{
return(Nc792);
}
}
Timer::~Timer()
{
semFlush(m_semaphore);
}
/**
* Free the PID object
*/
RobotDrivePIDController::~RobotDrivePIDController()
{
semFlush(m_semaphore);
delete m_controlLoop;
}
STATUS FNC::Recv()
{
struct sockaddr_in fromAddr;
socklen_t fromAddrLen = sizeof fromAddr;
int n = recvfrom(m_recvSocket, (char *)&m_recvPkt, sizeof m_recvPkt, 0,
(struct sockaddr *)&fromAddr, &fromAddrLen);
if (n == -1) {
perror("FRC_NetworkCommunication::Recv::recvfrom");
return ERROR;
}
if (m_recvAddr.sin_addr.s_addr == INADDR_ANY) {
// fill in my own address based on:
// DS at 10.TE.AM.xx
// robot at 10.TE.AM.2
in_addr_t ipaddr = ntohl(fromAddr.sin_addr.s_addr);
ipaddr = (ipaddr & 0xFFFFFF00) | 0x02;
m_recvAddr.sin_addr.s_addr = htonl(ipaddr);
m_sendAddr.sin_addr.s_addr = htonl(ipaddr);
}
m_pcap->write_record(&fromAddr, &m_recvAddr, (char *)&m_recvPkt, n);
if (n != sizeof m_recvPkt) {
printf("FRC_NetworkCommunication::Recv: wrong size (%d)\n", n);
return OK;
}
uint32_t recvCRC = ntohl(m_recvPkt.crc);
m_recvPkt.crc = 0;
uint32_t calcCRC = crc32(0, Z_NULL, 0);
calcCRC = crc32(calcCRC, (uint8_t *)&m_recvPkt, sizeof m_recvPkt);
if (calcCRC != recvCRC) {
printf("FRC_NetworkCommunication::Recv: bad checksum "
"(received 0x%08x, calculated 0x%08x)\n", recvCRC, calcCRC);
return OK;
}
// unpack and copy data to m_recvData
{
// begin critical section
Synchronized sync(m_dataMutex);
// clear the work area for safety
memset(&m_recvData, 0, sizeof m_recvData);
// unpack common header elements
m_recvData.packetIndex = ntohs(m_recvPkt.ctrl.packetIndex);
m_recvData.reset = m_recvPkt.ctrl.reset;
m_recvData.notEStop = m_recvPkt.ctrl.notEStop;
m_recvData.enabled = m_recvPkt.ctrl.enabled;
m_recvData.autonomous = m_recvPkt.ctrl.autonomous;
m_recvData.fmsAttached = m_recvPkt.ctrl.fmsAttached;
m_recvData.resync = m_recvPkt.ctrl.resync;
m_recvData.test = m_recvPkt.ctrl.test;
m_recvData.checkVersions = m_recvPkt.ctrl.checkVersions;
m_recvData.dsDigitalIn = m_recvPkt.ctrl.dsDigitalIn;
m_recvData.teamID = ntohs(m_recvPkt.ctrl.teamID);
m_recvData.dsID_Alliance = m_recvPkt.ctrl.dsID_Alliance;
m_recvData.dsID_Position = m_recvPkt.ctrl.dsID_Position;
#if 0
printf("pkt %u ctrl 0x%02x dig in 0x%02x team %u position %c%c\n",
m_recvData.packetIndex, m_recvData.control,
m_recvData.dsDigitalIn, m_recvData.teamID,
m_recvData.dsID_Alliance, m_recvData.dsID_Position);
#endif
// unpack data for each of the 4 possible joysticks
m_recvData.stick0Axis1 = m_recvPkt.ctrl.stick0Axis1;
m_recvData.stick0Axis2 = m_recvPkt.ctrl.stick0Axis2;
m_recvData.stick0Axis3 = m_recvPkt.ctrl.stick0Axis3;
m_recvData.stick0Axis4 = m_recvPkt.ctrl.stick0Axis4;
m_recvData.stick0Axis5 = m_recvPkt.ctrl.stick0Axis5;
m_recvData.stick0Axis6 = m_recvPkt.ctrl.stick0Axis6;
m_recvData.stick0Buttons = ntohs(m_recvPkt.ctrl.stick0Buttons);
#if 0
printf("stick0: %02x %02x %02x %02x %02x %02x %04x\n",
m_recvData.stick0Axis1, m_recvData.stick0Axis2,
m_recvData.stick0Axis3, m_recvData.stick0Axis4,
m_recvData.stick0Axis5, m_recvData.stick0Axis6,
m_recvData.stick0Buttons);
#endif
m_recvData.stick1Axis1 = m_recvPkt.ctrl.stick1Axis1;
m_recvData.stick1Axis2 = m_recvPkt.ctrl.stick1Axis2;
m_recvData.stick1Axis3 = m_recvPkt.ctrl.stick1Axis3;
m_recvData.stick1Axis4 = m_recvPkt.ctrl.stick1Axis4;
m_recvData.stick1Axis5 = m_recvPkt.ctrl.stick1Axis5;
m_recvData.stick1Axis6 = m_recvPkt.ctrl.stick1Axis6;
m_recvData.stick1Buttons = ntohs(m_recvPkt.ctrl.stick1Buttons);
#if 0
printf("stick1: %02x %02x %02x %02x %02x %02x %04x\n",
m_recvData.stick1Axis1, m_recvData.stick1Axis2,
m_recvData.stick1Axis3, m_recvData.stick1Axis4,
m_recvData.stick1Axis5, m_recvData.stick1Axis6,
m_recvData.stick1Buttons);
#endif
m_recvData.stick2Axis1 = m_recvPkt.ctrl.stick2Axis1;
m_recvData.stick2Axis2 = m_recvPkt.ctrl.stick2Axis2;
m_recvData.stick2Axis3 = m_recvPkt.ctrl.stick2Axis3;
m_recvData.stick2Axis4 = m_recvPkt.ctrl.stick2Axis4;
m_recvData.stick2Axis5 = m_recvPkt.ctrl.stick2Axis5;
m_recvData.stick2Axis6 = m_recvPkt.ctrl.stick2Axis6;
m_recvData.stick2Buttons = ntohs(m_recvPkt.ctrl.stick2Buttons);
#if 0
printf("stick2: %02x %02x %02x %02x %02x %02x %04x\n",
m_recvData.stick2Axis1, m_recvData.stick2Axis2,
m_recvData.stick2Axis3, m_recvData.stick2Axis4,
m_recvData.stick2Axis5, m_recvData.stick2Axis6,
m_recvData.stick2Buttons);
#endif
m_recvData.stick3Axis1 = m_recvPkt.ctrl.stick3Axis1;
m_recvData.stick3Axis2 = m_recvPkt.ctrl.stick3Axis2;
m_recvData.stick3Axis3 = m_recvPkt.ctrl.stick3Axis3;
m_recvData.stick3Axis4 = m_recvPkt.ctrl.stick3Axis4;
m_recvData.stick3Axis5 = m_recvPkt.ctrl.stick3Axis5;
m_recvData.stick3Axis6 = m_recvPkt.ctrl.stick3Axis6;
m_recvData.stick3Buttons = ntohs(m_recvPkt.ctrl.stick3Buttons);
#if 0
printf("stick3: %02x %02x %02x %02x %02x %02x %04x\n",
m_recvData.stick3Axis1, m_recvData.stick3Axis2,
m_recvData.stick3Axis3, m_recvData.stick3Axis4,
m_recvData.stick3Axis5, m_recvData.stick3Axis6,
m_recvData.stick3Buttons);
#endif
// unpack the four analog input channels
m_recvData.analog1 = ntohs(m_recvPkt.ctrl.analog1);
m_recvData.analog2 = ntohs(m_recvPkt.ctrl.analog2);
m_recvData.analog3 = ntohs(m_recvPkt.ctrl.analog3);
m_recvData.analog4 = ntohs(m_recvPkt.ctrl.analog4);
#if 0
printf("analog: %04x %04x %04x %04x\n",
m_recvData.analog1, m_recvData.analog2,
m_recvData.analog3, m_recvData.analog4);
#endif
// unpack the cRIO and FPGA checksums (?)
#if (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__)
# define ntohll(x) ((uint64_t)(x))
#elif (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__)
# define ntohll(x) \
((uint64_t)((((uint64_t)(x) & 0x00000000000000ffLLU) << 56) | \
(((uint64_t)(x) & 0x000000000000ff00LLU) << 40) | \
(((uint64_t)(x) & 0x0000000000ff0000LLU) << 24) | \
(((uint64_t)(x) & 0x00000000ff000000LLU) << 8) | \
(((uint64_t)(x) & 0x000000ff00000000LLU) >> 8) | \
(((uint64_t)(x) & 0x0000ff0000000000LLU) >> 24) | \
(((uint64_t)(x) & 0x00ff000000000000LLU) >> 40) | \
(((uint64_t)(x) & 0xff00000000000000LLU) >> 56)))
#else
#error __BYTE_ORDER__ must be __ORDER_BIG_ENDIAN__ or __ORDER_LITTLE_ENDIAN__
#endif
m_recvData.cRIOChecksum = ntohll(m_recvPkt.ctrl.cRIOChecksum);
#if 0
printf("cRIO checksum: %08lx%08lx\n",
(unsigned long)(m_recvData.cRIOChecksum >> 32),
(unsigned long)(m_recvData.cRIOChecksum));
#endif
m_recvData.FPGAChecksum0 = ntohl(m_recvPkt.ctrl.FPGAChecksum0);
m_recvData.FPGAChecksum1 = ntohl(m_recvPkt.ctrl.FPGAChecksum1);
m_recvData.FPGAChecksum2 = ntohl(m_recvPkt.ctrl.FPGAChecksum2);
m_recvData.FPGAChecksum3 = ntohl(m_recvPkt.ctrl.FPGAChecksum3);
#if 0
printf("FPGA checksums: %08x %08x %08x %08x\n",
m_recvData.FPGAChecksum0, m_recvData.FPGAChecksum1,
m_recvData.FPGAChecksum2, m_recvData.FPGAChecksum3);
#endif
// unpack the DS version string
// ASCII string, mmddyyvv ("vv"="version" aka "build number")
memcpy(m_recvData.versionData, m_recvPkt.ctrl.versionData, 8);
#if 0
printf("DS version %.2s.%.2s.%.2s.%.2s\n",
&m_recvData.versionData[0], &m_recvData.versionData[2],
&m_recvData.versionData[4], &m_recvData.versionData[6]);
#endif
// copy the dynamic extensions
// we don't know the internal structure of these, so clients are
// responsible for unpacking their own data
memcpy(m_extData, m_recvPkt.extData, sizeof m_extData);
// save sender's address for reply
memcpy(&m_fromAddr, &fromAddr, sizeof m_fromAddr);
// tell DriverStation object that new data is available
if (m_dataAvailable) semFlush(m_dataAvailable);
// end critical section
}
return OK;
}
/**
* Free the PID object
*/
CAN_VPID_Controller::~CAN_VPID_Controller()
{
semFlush(m_semaphore);
delete m_controlLoop;
}
