tpmode(int port) { char ttyName[16]; char chrStr[80],replyStr[80]; int len,i; sprintf(ttyName,FpgaFormatStr,port); printf("opening dev: '%s'\n",ttyName); sfd = open(ttyName, O_RDWR, 0); if(sfd < 1) { perror("open:"); printf("failed to open: '%s'\n",ttyName); return(-1); } sprintf(ttyName,FpgaFormatStr,1); sprintf(ttyName,FpgaFormatStr,2); sprintf(ttyName,FpgaFormatStr,3); while(1) { printf("string to issue: "); fioRdString (STD_IN, &chrStr, 80); printf("sending: '%s'\n",chrStr); len = strlen(chrStr); for(i=0;i<len;i++) printf("char: '%c', 0x%x\n",chrStr[i],chrStr[i]); printf("strlen: %d\n",len); chrStr[len] = 13; chrStr[len+1] = 10; chrStr[len+2] = 0; write(sfd,chrStr,len+2); fioRdString(sfd,replyStr,80); len = strlen(replyStr); taskDelay(calcSysClkTicks(500)); /* 1/2 sec, taskDelay(30); */ printf("reply: %d bytes '%s' \n",len,replyStr); } }
STATIC s32 use_smem_test(int size) { u32* pret = (u32*)bsp_smalloc(size, MEM_ICC_DDR_POOL); u32* praw = pret; u32 rawsize = size; if(pret) { mem_print_dbg("sucess BSP_Malloc addr:%x size:%d", pret, size); while(size > 0) { size -= sizeof(u32); *pret = ACORE_MAGIC_NUM; pret++; } taskDelay(200); size = rawsize; pret = praw; while(size > 0) { size -= sizeof(u32); if(*pret != ACORE_MAGIC_NUM) { mem_print_error("fail! Use smalloc fail addr:%x size:%d", pret, size); return MEM_TEST_ERROR; } pret++; } bsp_sfree(praw); return MEM_TEST_OK; } else { mem_print_error("fail! BSP_Malloc addr:%x size:%d", pret, size); return MEM_TEST_ERROR; } }
/*TASK 0*/ void inputWatcher() { char message[MAX_MESSAGE_LENGTH]; while (1) { newSensors = getSensorValues(); /*read the sensors*/ if(newSensors != oldSensors){ /*create message and send it through the queue*/ sprintf(message, "CHANGE %d", newSensors); if((msgQSend(gateInQueue, message, MAX_MESSAGE_LENGTH, WAIT_FOREVER, MSG_PRI_NORMAL)) == ERROR) printf("msgQSend to gate in failed\n"); /*both tasks wont work together - even with this message queue*/ if((msgQSend(gateOutQueue, message, MAX_MESSAGE_LENGTH, WAIT_FOREVER, MSG_PRI_NORMAL)) == ERROR) printf("msgQSend to gate out failed\n"); } /* if(newSensors != oldSensors){ gateIn(); gateOut(); } */ taskDelay(6); /*Delay 6/60 -> 1/10 of a second*/ oldSensors = newSensors; /*update the sensors*/ } }
/*************************************************************************** * rapiModeSet - Set new RAPI mode * * int rapiId RAPI id * int mode New RAPI mode * * RETURNS: * RAPI_OK on succes, or RAPI_ERR. */ int rapiModeSet(int rapiId, int mode) { int i; RAPI_HEADER *pRapiHeader; if(rapiId < 0 || rapiId >= cRapi) /* Valid control module index ? */ return(RAPI_ERR); if(mode < 0 || mode > 7) /* Mode within valid range ? */ return(RAPI_ERR); pRapiHeader = rapiModule[rapiId].pRapiHeader; /* Get pointer to RAPI_HEADER */ pRapiHeader->ctrlReg = (pRapiHeader->ctrlReg & ~0x07) | mode; /* Read current status reg, mask mode, insert new mode and write ctrl reg */ #if 0 for(i=0; i < mSecToTick(1000); i++) /* Wait a while for new mode to show up */ #else for(i=0; i < 10000000; i++) /* Wait a while for new mode to show up */ #endif { if((pRapiHeader->statReg & 0x07) == mode) /* New mode show up in status reg ? */ { #ifdef DEBUG printf("Waited %d laps for mode to be set\n", i); /* Try to get a grip on the time frame involved here ;-) */ #endif return(RAPI_OK); /* Allright, it's set ! */ } taskDelay(1); /* (is it one millisecond ??) */ /* Sleep a short while bewteen status checking (nanosleep ??) */ } return(RAPI_ERR); /* We didn't succeed in setting new mode */ }
pollTest() { int i; char Ichar; SIO_CHAN *pSioChan, *pSioChan2; /* serial I/O channel */ char *text = "0123456789abcdefghijklmnopqurstuwxyz"; pSioChan = masterFpgaSerialChanGet(0); /* ns16550InitChannel(pSioChan); */ for (i = 0; i < 4; i++) { printf("output: %c, 0x%x\n",text[i],text[i]); while (sioPollOutput (pSioChan, text[i]) == EAGAIN); taskDelay(calcSysClkTicks(17)); /* taskDelay(1); */ if ( sioPollInput (pSioChan, &Ichar) != EAGAIN) printf("got: '%c', 0x%x\n",Ichar,Ichar); } /* for (i = 0; i < 4; i++) { while (sioPollInput (pSioChan, &Ichar) == EAGAIN); printf("got: %c\n",Ichar); } */ }
/* * Runs the user operator control code. This function will be started in its own task with the * default priority and stack size whenever the robot is enabled via the Field Management System * or the VEX Competition Switch in the operator control mode. If the robot is disabled or * communications is lost, the operator control task will be stopped by the kernel. Re-enabling * the robot will restart the task, not resume it from where it left off. * * If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will * run the operator control task. Be warned that this will also occur if the VEX Cortex is * tethered directly to a computer via the USB A to A cable without any VEX Joystick attached. * * Code running in this task can take almost any action, as the VEX Joystick is available and * the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly * recommended to give other tasks (including system tasks such as updating LCDs) time to run. * * This task should never exit; it should end with some kind of infinite loop, even if empty. */ void operatorControl() { // start IR detection task taskCreate(vTaskFilter, 2048, NULL, TASK_PRIORITY_DEFAULT); // start PI controller taskCreate(vTaskPIController, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT); //start polling the UltraSonic sensor taskCreate(vTaskUltraSonic, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT); // show the detected IR levels /*short leftIR_l, centerIR_l, rightIR_l; while(1) { if(mutexTake(irDetectorMutex, 10)) { leftIR_l = leftIR; centerIR_l = centerIR; rightIR_l = rightIR; mutexGive(irDetectorMutex); } printf("Left: %d Center: %d Right: %d \n", leftIR_l, centerIR_l, rightIR_l); taskDelay(100); }*/ // advance 1000, reverse 1000, every 2 seconds int multiplier = 1; while(1) { // take PI Mutex mutexTake(piSetpointMutex, -1); leftMotor.position += 1000*multiplier; rightMotor.position += 1000*multiplier; mutexGive(piSetpointMutex); multiplier = -1*multiplier; taskDelay(2000); } }
/**************************************************************************** cs4281_wrote_ac97() : write a word to the cs4281 address space step-1: write ACCAD (Command Address Register) 46C h step-2: write ACCDA (Command Data Register) 470 h step-3: write ACCTL (Control Register) 460 h step-4: read ACCTL, DCV should be reset and [460h] = 07h step-5: if DCV not cleared, error *****************************************************************************/ int cs4281_write_ac97(UINT32 offset, UINT32 value ) { UINT32 count, status; /* write to the actual AC97 register */ writel(offset - BA0_AC97_RESET, CS4281_pBA0 + BA0_ACCAD); writel(value, CS4281_pBA0 + BA0_ACCDA); writel(ACCTL_DCV | ACCTL_VFRM |ACCTL_ESYN, CS4281_pBA0 + BA0_ACCTL); /* Wait for write to finish ... */ for(count=0; count<10; count ++) { taskDelay(25); /*udelay(25);*/ /* check if write complete */ status = readl(CS4281_pBA0 + BA0_ACCTL); if(!(status & ACCTL_DCV)) break; } if(status & ACCTL_DCV) return 1; return 0; }
int fadcFirmwareZeroSRAM (int id) { int ii, value=0, value_1=0, value_2=0; int ErrorCount=0, stopPrint=0; if(id==0) id=fadcID[0]; if((id<=0) || (id>21) || (FAp[id] == NULL)) { printf("%s: ERROR : ADC in slot %d is not initialized \n", __FUNCTION__,id); return ERROR; } /* set address = 0; allow increment on mem2 access */ FALOCK; vmeWrite32(&FAp[id]->mem_adr, 0x0000 | FA_MEM_ADR_INCR_MEM2); for( ii = 0; ii < 0x80000; ii++) /* write ZERO to entire memory */ { vmeWrite32(&FAp[id]->mem1_data, 0); vmeWrite32(&FAp[id]->mem2_data, 0); } /* reset address = 0; allow increment on mem2 access */ vmeWrite32(&FAp[id]->mem_adr, 0x0000 | FA_MEM_ADR_INCR_MEM2); FAUNLOCK; /* read and test expected memory data */ for( ii = 0; ii < 0x80000; ii++) { FALOCK; value_1 = vmeRead32(&FAp[id]->mem1_data); value_2 = vmeRead32(&FAp[id]->mem2_data); FAUNLOCK; if( (value_1 != 0) || (value_2 != 0) ) { ErrorCount++; FALOCK; value = vmeRead32(&FAp[id]->mem_adr) & 0xFFFFF; FAUNLOCK; if(!stopPrint) { printf("%s: ERROR: FADC %2d address = %8X mem1 read = %8X mem2 read = %8X\n", __FUNCTION__,id, value, value_1, value_2); taskDelay(1); /* wait */ } if(ErrorCount==80) { printf("%s: Further errors for FADC %2d will not be displayed\n", __FUNCTION__,id); stopPrint=1; } } } if(ErrorCount) return ERROR; return OK; }
int fadcFirmwareVerifyDownload (int id) { unsigned int ArraySize; unsigned int ByteCount, ByteIndex, ByteNumber; unsigned int ExpWord32Bits, SramAdr, RdWord32Bits; int ErrorCount=0, stopPrint=0; int value; if(id==0) id=fadcID[0]; if((id<=0) || (id>21) || (FAp[id] == NULL)) { printf("%s: ERROR : ADC in slot %d is not initialized \n", __FUNCTION__,id); return ERROR; } if(MSC_loaded != 1) { printf("%s: ERROR : Firmware was not loaded\n", __FUNCTION__); return ERROR; } /* ArraySize = sizeof(MSC_ARRAY); */ ArraySize = MSC_arraySize; ByteIndex = 0; SramAdr = 0; /* write SRAM address register */ /* start at 0 and increment address after read from mem1 data register */ FALOCK; vmeWrite32(&FAp[id]->mem_adr, 0x0000 | FA_MEM_ADR_INCR_MEM1); value = vmeRead32(&FAp[id]->mem_adr); FAUNLOCK; #ifdef DEBUG printf("%s: FADC %2d memory address at start of read = 0x%08x\n\n", __FUNCTION__,id,value); #endif taskDelay(2); /* wait */ for (ByteCount = 0; ByteCount < ArraySize; ByteCount += 4) { /* get expected value */ ExpWord32Bits = 0; for (ByteNumber = 0; ByteNumber < 4; ++ByteNumber) { ExpWord32Bits = (MSC_ARRAY[ByteIndex] << (8 * ByteNumber)) | ExpWord32Bits; ++ByteIndex; } /* read 32-bit data word from mem1 data register */ FALOCK; RdWord32Bits = (unsigned int)vmeRead32(&FAp[id]->mem1_data); FAUNLOCK; #ifdef DEBUG if(ByteCount<40) printf("RdWord32Bits = 0x%08x\n",RdWord32Bits); #endif /* test if read value = expected value */ if (RdWord32Bits != ExpWord32Bits) { ErrorCount++; if(!stopPrint) printf("%s: ERROR: FADC %2d ByteCount %8d Expect %08X Read %08X\n", __FUNCTION__,id, ByteCount, ExpWord32Bits, RdWord32Bits); if( ErrorCount==80 ) { printf("%s: Further errors for FADC %2d will not be displayed\n", __FUNCTION__,id); /* getchar(); */ stopPrint=1; } /* if( ErrorCount>1000 ) */ /* stopPrint=1; */ } } FALOCK; value = vmeRead32(&FAp[id]->mem_adr); FAUNLOCK; #ifdef DEBUG printf("%s: memory address after read = 0x%08x\n\n", __FUNCTION__,value); #endif if(ErrorCount) printf("%s: ErrorCount = %d\n",__FUNCTION__,ErrorCount); taskDelay(1); /* wait */ if(ErrorCount) return ERROR; return OK; }
int fadcFirmwareLoad(int id, int chip, int pFlag) { if(id==0) id=fadcID[0]; if((id<=0) || (id>21) || (FAp[id] == NULL)) { printf("%s: ERROR : ADC in slot %d is not initialized \n", __FUNCTION__,id); return ERROR; } if(chip<0 || chip>2) { printf("%s: ERROR: Invalid chip parameter %d\n", __FUNCTION__,chip); return ERROR; } /* Perform a hardware and software reset */ FALOCK; vmeWrite32(&FAp[id]->reset, 0xFFFF); FAUNLOCK; taskDelay(60); /* Check if FADC is Ready */ if(fadcFirmwareTestReady(id, 60, pFlag) != OK) { printf("%s: ERROR: FADC %2d not ready after reset\n", __FUNCTION__,id); return ERROR; } /* Data to SRAM */ printf("%s: Loading SRAM with data \n",__FUNCTION__); fadcFirmwareDownloadConfigData(id); if(fadcFirmwareVerifyDownload(id) != OK) { printf("%s: ERROR: FADC %2d Failed data verification at SRAM\n", __FUNCTION__,id); return ERROR; } /* SRAM TO PROM */ taskDelay(1); printf("%s: Loading PROM with SRAM data \n",__FUNCTION__); FALOCK; if(chip==FADC_FIRMWARE_LX110) vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_SRAM_TO_PROM1); else if(chip==FADC_FIRMWARE_FX70T) vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_SRAM_TO_PROM2); FAUNLOCK; taskDelay(1); if(fadcFirmwareTestReady(id, 60000, pFlag) != OK) /* Wait til it's done */ { printf("%s: ERROR: FADC %2d ready timeout SRAM -> PROM\n", __FUNCTION__,id); return ERROR; } /* PROM TO SRAM (For verification) */ printf("%s: Loading SRAM with PROM data \n",__FUNCTION__); fadcFirmwareZeroSRAM(id); FALOCK; if(chip==FADC_FIRMWARE_LX110) vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_PROM1_TO_SRAM); else if(chip==FADC_FIRMWARE_FX70T) vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_PROM2_TO_SRAM); FAUNLOCK; taskDelay(1); if(fadcFirmwareTestReady(id, 60000, pFlag) != OK) /* Wait til it's done */ { printf("%s: ERROR: FADC %2d ready timeout PROM -> SRAM\n", __FUNCTION__,id); return ERROR; } /* Compare SRAM to Data Array */ printf("%s: Verifying data \n",__FUNCTION__); if(fadcFirmwareVerifyDownload(id) != OK) { printf("%s: ERROR: FADC %d PROM data not verified\n", __FUNCTION__,id); return ERROR; } /* PROM to FPGA (Reboot FPGA) */ printf("%s: Rebooting FPGA \n",__FUNCTION__); FALOCK; if(chip==FADC_FIRMWARE_LX110) vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_REBOOT_FPGA1); else if(chip==FADC_FIRMWARE_FX70T) vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_REBOOT_FPGA2); FAUNLOCK; taskDelay(1); if(fadcFirmwareTestReady(id, 60000, pFlag) != OK) /* Wait til it's done */ { printf("%s: ERROR: FADC %2d ready timeout PROM -> FPGA\n", __FUNCTION__,id); return ERROR; } printf("%s: Done programming FADC %2d\n", __FUNCTION__,id); return OK; }
/***************************************************************** 功能:控制CFG脚,发起初始化操作 输入:无 输出:无 返回:无 *****************************************************************/ LOCAL VOID StartDownLoadFpga_ALTERA() { SetCfgPin_ALTERA(0); taskDelay(1); SetCfgPin_ALTERA(1); }
/** * Runs autonomous code. \n * Contains code to run our autonomous plans. \n */ void autonomous() { if (badPlan1) { //Lift cube motorSet(6, 127); motorSet(7, 127); motorSet(8, 127); taskDelay(2000); motorSet(6, 0); motorSet(7, 0); motorSet(8, 0); //Turns robot //Positive number = right; Negative number = left. int Y1 = 127; int X1 = 0; int X2 = 0; motorSet(2, Y1 + X2 + X1); motorSet(3, Y1 - X2 - X1); motorSet(4, Y1 + X2 - X1); motorSet(5, Y1 - X2 + X1); taskDelay(800); //Go forward X2 = -127; Y1 = 0; X1 = 0; motorSet(2, Y1 + X2 + X1); motorSet(3, Y1 - X2 - X1); motorSet(4, Y1 + X2 - X1); motorSet(5, Y1 - X2 + X1); taskDelay(2000); //Stop motorSet(2, 0); motorSet(5, 0); motorSet(3, 0); motorSet(4, 0); //Drop Cube motorSet(6, -127); motorSet(7, -127); motorSet(8, -127); taskDelay(1500); //Stop dropping motorSet(6, 0); motorSet(7, 0); motorSet(8, 0); //Go back X2 = 127; Y1 = 0; X1 = 0; motorSet(2, Y1 + X2 + X1); motorSet(3, Y1 - X2 - X1); motorSet(4, Y1 + X2 - X1); motorSet(5, Y1 - X2 + X1); taskDelay(1000); //Stop motorSet(2, 0); motorSet(5, 0); motorSet(3, 0); motorSet(4, 0); } else if (goodPlan1) { //Raise Lift motorSet(6, 127); motorSet(7, 127); motorSet(8, 127); taskDelay(150); //Stop lifting motorSet(6, 0); motorSet(7, 0); motorSet(8, 0); //Go forward int X2 = -127; int Y1 = 0; int X1 = 0; motorSet(2, Y1 + X2 + X1); motorSet(3, Y1 - X2 - X1); motorSet(4, Y1 + X2 - X1); motorSet(5, Y1 - X2 + X1); taskDelay(1100); //Stop going forward motorSet(2, 0); motorSet(3, 0); motorSet(4, 0); motorSet(5, 0); //Lift cube & stop lifting cube motorSet(6, 127); motorSet(7, 127); motorSet(8, 127); taskDelay(2000); motorSet(6, 0); motorSet(7, 0); motorSet(8, 0); /* //Turn left Y1 = -127; X1 = 0; X2 = 0; motorSet(2, Y1 + X2 + X1); motorSet(3, Y1 - X2 - X1); motorSet(4, Y1 + X2 - X1); motorSet(5, Y1 - X2 + X1); taskDelay(800); //Stop turning left motorSet(2, 0); motorSet(3, 0); motorSet(4, 0); motorSet(5, 0); //Go forward X2 = -127; Y1 = 0; X1 = 0; motorSet(2, Y1 + X2 + X1); motorSet(3, Y1 - X2 - X1); motorSet(4, Y1 + X2 - X1); motorSet(5, Y1 - X2 + X1); taskDelay(1100); //Stop going forward motorSet(2, 0); motorSet(3, 0); motorSet(4, 0); motorSet(5, 0); //Turn left Y1 = -127; X1 = 0; X2 = 0; motorSet(2, Y1 + X2 + X1); motorSet(3, Y1 - X2 - X1); motorSet(4, Y1 + X2 - X1); motorSet(5, Y1 - X2 + X1); taskDelay(1100); //Stop turning left motorSet(2, 0); motorSet(3, 0); motorSet(4, 0); motorSet(5, 0); //Go forward X2 = -127; Y1 = 0; X1 = 0; motorSet(2, Y1 + X2 + X1); motorSet(3, Y1 - X2 - X1); motorSet(4, Y1 + X2 - X1); motorSet(5, Y1 - X2 + X1); taskDelay(2200); //Stop going forward motorSet(2, 0); motorSet(3, 0); motorSet(4, 0); motorSet(5, 0); //Drop Cube motorSet(6, -127); motorSet(7, -127); motorSet(8, -127); taskDelay(1000); //Stop dropping motorSet(6, 0); motorSet(7, 0); motorSet(8, 0); */ } }
STATUS shellParserControl ( UINT32 remoteEvent, /* Starting or stopping a connection? */ UINT32 sessionId, /* Unique identifier for each session */ UINT32 slaveFd /* File descriptor for character i/o */ ) { if ((taskNameToId ("tShell")) == ERROR) /* Shell not started yet. */ return (ERROR); if (remoteEvent == REMOTE_START) { /* Handle a new telnet or rlogin session. */ if (remoteId != 0) /* Failed request - only one session allowed. */ return (ERROR); if (!shellLock (TRUE)) /* Shell is not available. */ { fdprintf (slaveFd, "The shell is currently in use.\n"); return (ERROR); } /* Let the user try to login */ if (shellLogin (slaveFd) != OK) { shellLock (FALSE); return (ERROR); } /* setup the slave device to act like a terminal */ (void) ioctl (slaveFd, FIOOPTIONS, OPT_TERMINAL); shellLogoutInstall ((FUNCPTR) telnetdExit, sessionId); /* get the shell's standard I/O fd's so we can restore them later */ shellInFd = ioGlobalStdGet (STD_IN); shellOutFd = ioGlobalStdGet (STD_OUT); shellErrFd = ioGlobalStdGet (STD_ERR); /* set shell's standard I/O to new device; add extra logging device */ shellOrigStdSet (STD_IN, slaveFd); shellOrigStdSet (STD_OUT, slaveFd); shellOrigStdSet (STD_ERR, slaveFd); logFdAdd (slaveFd); logFdFromRlogin = slaveFd; /* store new fd for logFdSet() */ /* Store the session identifier. */ remoteId = sessionId; /* notify the shell we have started a remote session */ shellIsRemoteConnectedSet (TRUE); printErr ("\ntelnetd: This system *IN USE* via telnet.\n"); /* Prevent network denial of service attacks by waiting a second */ taskDelay (sysClkRateGet() / 2); /* Restart the shell to access the redirected file descriptors. */ excJobAdd (shellRestart, TRUE, 0, 0, 0, 0, 0); return (OK); } else if (remoteEvent == REMOTE_STOP) { /* * End an active telnet or rlogin session. This event occurs * after the server closes the socket. */ if (remoteId != sessionId) /* Unknown remote session. */ return (ERROR); shellLogoutInstall ((FUNCPTR) NULL, 0); /* remove logout function */ if (logFdFromRlogin != NONE) { logFdDelete (logFdFromRlogin); /* cancel extra log device */ logFdFromRlogin = NONE; /* reset fd */ } shellOrigStdSet (STD_IN, shellInFd); /* restore shell's stnd I/O */ shellOrigStdSet (STD_OUT, shellOutFd); shellOrigStdSet (STD_ERR, shellErrFd); shellLock (FALSE); /* unlock shell */ /* * For typical remote sessions, restoring the standard I/O * descriptors is enough to reconnect the shell to the console * because closing the pty device will cause the shell to unblock * from its read() and use the restored descriptors. However, * problems can occur upon logout if the remote user has disabled * the line editor and/or put the pty device in raw mode, so the * shell is restarted in all cases. */ remoteId = 0; /* Allow a new session. */ /* notify the shell we have ended a remote session */ shellIsRemoteConnectedSet (FALSE); excJobAdd (shellRestart, FALSE, 0, 0, 0, 0, 0); return (OK); } return (ERROR); /* Ignore unknown control operations. */ }
int32 OS_mkfs (char *address, char *devname, char *volname, uint32 blocksize, uint32 numblocks) { int i; int status; char local_volname[OS_MAX_PATH_LEN]; uint32 ReturnCode; BLK_DEV *ramDev; device_t xbd; #ifdef USE_VXWORKS_ATA_DRIVER BLK_DEV *ataBlkDev; #endif if ( devname == NULL || volname == NULL ) return OS_FS_ERR_INVALID_POINTER; /* ** Find an open entry in the Volume Table */ for (i = 0; i < NUM_TABLE_ENTRIES; i++) { if (OS_VolumeTable[i].FreeFlag == TRUE && OS_VolumeTable[i].IsMounted == FALSE && strcmp(OS_VolumeTable[i].DeviceName, devname) == 0) break; } if (i >= NUM_TABLE_ENTRIES) return OS_FS_ERR_DEVICE_NOT_FREE; /* ** Now enter the info in the table */ OS_VolumeTable[i].FreeFlag = FALSE; strcpy(OS_VolumeTable[i].VolumeName, volname); OS_VolumeTable[i].BlockSize = blocksize; /* ** Note we don't know the mount point/physical device name yet */ strcpy(local_volname,volname); if (OS_VolumeTable[i].VolumeType == RAM_DISK) { printf("OSAL: Making a RAM disk at: 0x%08X\n",(unsigned long)address ); /* ** Create the ram disk device ** The 32 is the number of blocks per track. ** Other values dont seem to work here */ ramDev = ramDevCreate (address, blocksize , 32 , numblocks, 0); if (ramDev == NULL) { ReturnCode = OS_FS_ERR_DRIVE_NOT_CREATED; } /* ** Connect the ram drive to the xbd block device */ xbd = xbdBlkDevCreate(ramDev,local_volname); if (xbd == NULLDEV) { ReturnCode = OS_FS_ERR_DRIVE_NOT_CREATED; } else { /* ** Delay to allow the XBD operation to complete */ (void) taskDelay(100); /* ** Determine the vxWorks device name and store it ** in the Physical Device field of the volume table. ** It will be used for determining the path in OS_NameChange */ strcat(local_volname,":0"); strncpy(OS_VolumeTable[i].PhysDevName, local_volname, 32 ); /* ** Call the dos format routine */ status = dosFsVolFormat(OS_VolumeTable[i].PhysDevName, DOS_OPT_BLANK, NULL); if ( status == -1 ) { printf("OSAL: dosFsVolFormat failed. Errno = %d\n",errnoGet()); ReturnCode = OS_FS_ERR_DRIVE_NOT_CREATED; } else { ReturnCode = OS_FS_SUCCESS; } } } else if (OS_VolumeTable[i].VolumeType == FS_BASED) { /* ** FS_BASED will map the cFE to an already mounted filesystem */ /* ** now enter the info in the table */ OS_VolumeTable[i].FreeFlag = FALSE; strcpy(OS_VolumeTable[i].VolumeName, volname); OS_VolumeTable[i].BlockSize = blocksize; ReturnCode = OS_FS_SUCCESS; } #ifdef USE_VXWORKS_ATA_DRIVER /* ** Format an ATA disk ** This code requires an ATA driver in the BSP, so it must be ** left out of the compilation BSPs without. */ /* --------------------------------------------- */ else if (OS_VolumeTable[i].VolumeType == ATA_DISK) { printf("OSAL: Formatting a FLASH DISK\n"); /* ** Create the Flash disk device */ if( (ataBlkDev = ataDevCreate( 0, 0, 0, 0)) == NULL) { printf("OSAL: Error Creating flash disk device.\n"); ReturnCode = OS_FS_ERR_DRIVE_NOT_CREATED; } else { printf("OSAL: FLASH device initialized.\n"); /* ** Attach to the XBD layer */ xbd = xbdBlkDevCreate(ataBlkDev,local_volname); if (xbd == NULLDEV) { printf("OSAL: Error Creating XBD device on FLASH disk.\n"); ReturnCode = OS_FS_ERR_DRIVE_NOT_CREATED; } else { /* ** Delay to allow the XBD operation to complete */ (void) taskDelay(100); /* ** Determine the vxWorks device name and store it ** in the Physical Device field of the volume table. ** It will be used for determining the path in OS_NameChange */ status = OS_GetPhysDeviceName(OS_VolumeTable[i].PhysDevName, local_volname); if ( status == OS_FS_ERROR ) { ReturnCode = OS_FS_ERR_DRIVE_NOT_CREATED; } else { /* ** Format the Device with the DOS file system */ if ( dosFsVolFormat(OS_VolumeTable[i].PhysDevName,DOS_OPT_BLANK,NULL) == ERROR ) { printf("OSAL: DOS format error on flash disk.\n"); ReturnCode = OS_FS_ERR_DRIVE_NOT_CREATED; } else { ReturnCode = OS_FS_SUCCESS; } } } } }/* if ATA_DISK */ #endif else { /* ** VolumeType is something else that is not supported right now */ ReturnCode = OS_FS_ERROR; } return ReturnCode; } /* end OS_mkfs */
int main(int argc, char *argv[]) { int stat; printf("\nJLAB TS Tests\n"); printf("----------------------------\n"); vmeOpenDefaultWindows(); /* Setup Address and data modes for DMA transfers * * vmeDmaConfig(addrType, dataType, sstMode); * * addrType = 0 (A16) 1 (A24) 2 (A32) * dataType = 0 (D16) 1 (D32) 2 (BLK32) 3 (MBLK) 4 (2eVME) 5 (2eSST) * sstMode = 0 (SST160) 1 (SST267) 2 (SST320) */ vmeDmaConfig(2,5,1); /* INIT dmaPList */ dmaPFreeAll(); vmeIN = dmaPCreate("vmeIN",1024,500,0); vmeOUT = dmaPCreate("vmeOUT",0,0,0); dmaPStatsAll(); dmaPReInitAll(); tsReload(); /* gefVmeSetDebugFlags(vmeHdl,0x0); */ /* Set the TS structure pointer */ tsPartInit(1,(21<<19),TS_READOUT_EXT_POLL,0); if(tsCheckAddresses()==ERROR) goto CLOSE; tsPartLoadTriggerTable(); tsPartSetBlockBufferLevel(10); tsSetBlockLevel(1); stat = tsPartIntConnect(mytsISR, 0); if (stat != OK) { printf("ERROR: tsIntConnect failed \n"); goto CLOSE; } else { printf("INFO: Attached TS Interrupt\n"); } tsSetTriggerSource(6); // Pulser = 5, GTP/Ext/FP = 6 tsSetFPInput(0); tsSetGenInput(0xffff); tsSetGTPInput(0x0); tsPartSetFPInput(1,2,3); tsPartSetExtInput(1,2,3,4,5); tsPartSetGTPInput(1,2,3,4,5); /* tsSetBusySource(TS_BUSY_LOOPBACK,1); */ tsSetBusySource(0,1); /* tsSetBlockBufferLevel(1); */ tsClockReset(); taskDelay(1); tsTrigLinkReset(); taskDelay(1); tsSyncReset(); taskDelay(1); tsStatus(); printf("Hit enter to start triggers\n"); getchar(); tsPartIntEnable(0); tsStatus(); /* #define SOFTTRIG */ #ifdef SOFTTRIG tsSetRandomTrigger(1,0x7); taskDelay(10); tsSoftTrig(1,0x1,0x700,0); #endif printf("Hit any key to Disable TID and exit.\n"); getchar(); tsStatus(); tsPrintScalers(1); tsPrintScalers(2); tsPrintScalers(3); #ifdef SOFTTRIG /* No more soft triggers */ /* tidSoftTrig(0x0,0x8888,0); */ tsSoftTrig(1,0,0x700,0); tsDisableRandomTrigger(); #endif tsPartIntDisable(); tsPartIntDisconnect(); CLOSE: vmeCloseDefaultWindows(); exit(0); }
void hang() { liftTaskDelete(); // engage the winch digitalWrite(winchPiston, 1); // give it a chance to engage taskDelay(100); int leftTicks = analogRead(potLiftLeft); int rightTicks = analogRead(potLiftRight); int changesArrayLength = STALL_TIME/HANG_DT; int changes[changesArrayLength]; // -1 indicates no data fillArray(changes, changesArrayLength, -1); int stallTicks = 0; while (true) { if (joystickGetDigital(1, 8, JOY_DOWN)) { return; } int newLeftTicks = analogRead(potLiftLeft); int newRightTicks = analogRead(potLiftRight); if (newLeftTicks < HANG_HEIGHT && newRightTicks < HANG_HEIGHT) { // we're at the right height, stop break; } if ((stallTicks * HANG_DT) > STALL_DELAY) { // do stall detection int change = abs(leftTicks - newLeftTicks) + abs(rightTicks - newRightTicks); pushArrayValue(changes, changesArrayLength, change); int totalChanges = sumStallChanges(changes, changesArrayLength); // if changes has data (>= 0) and it's stalled, stop motors if (totalChanges >= 0 && totalChanges < STALL_THRESHOLD) { // stalled, wait 3 seconds hangMotors(0); delay(3000); // clear changes fillArray(changes, changesArrayLength, -1); continue; } } leftTicks = newLeftTicks; rightTicks = newRightTicks; hangMotors(127); stallTicks++; taskDelay(HANG_DT); } hangMotors(0); }
VOID ResetFpga_ALTERA() { BSP_AT91F_OR_PIO_ClearOutput(BSP_PIOC,FPGA_RESET); taskDelay(2); BSP_AT91F_OR_PIO_SetOutput(BSP_PIOC,FPGA_RESET); }
/***************************************************************** 功能:从CF卡读取FPGA代码,然后加载到FPGA芯片中 输入:无 输出:无 返回:0 成功 其他失败 *****************************************************************/ SDWORD LoadFpga_ALTERA(CHAR filename[4][80],DWORD fileno) { BYTE *data = NULL; DWORD bytes ; BYTE *bootrom_p1 ; FILE *bootrom_fd ; BYTE *pFpgaFile = NULL; BYTE dataTemp; DWORD i = 0; DWORD j = 0; DWORD count = 0 ; DWORD ulFileLength = 0; DWORD tempfileno = 0; InitFpgaPin_ALTERA(); StartDownLoadFpga_ALTERA(); /*检测FPGA_INIT信号,如果变为1 表示已经对FPGA里的memory清除完毕*/ while(0 == GpioValGet_ALTERA(FPGA_INIT_ALTERA)) { taskDelay(2) ; count++ ; if( 10 == count) { printf("\r\n FPGA INIT Signal error !!") ; return (2); } } for(tempfileno=0;tempfileno<fileno;tempfileno++) { pFpgaFile = (BYTE*)malloc(FPGA_FILE_MAX_LENGTH); if(NULL == pFpgaFile) { printf( "\nERROR: NO memory!\n" ) ; return (1); } bootrom_p1 = pFpgaFile ; if((bootrom_fd = fopen( filename[tempfileno], "rb" )) == NULL ) { printf( "\nERROR: can not open file %s\n",filename ) ; free( pFpgaFile ) ; return (1); } bytes = 0 ; printf( "read FPGA file\n" ) ; while( fscanf( bootrom_fd, "%c", &dataTemp ) != EOF ) { *bootrom_p1++ = dataTemp ; bytes = bytes + 1 ; if( bytes >= FPGA_FILE_MAX_LENGTH ) break ; } fclose( bootrom_fd ) ; if( bytes > FPGA_FILE_MAX_LENGTH ) { printf( "ERROR: bootrom file is too long!\n" ) ; return (1); } printf( "File %08x\n" ,bytes) ; ulFileLength=bytes; /*把文件存入到申请的内存块中*/ /*ulFileLength = ftpDownload("nsp2000.bin", pFpgaFile); */ data = pFpgaFile; /* 启始点*/ /*开始加载*/ for(i = 0; i<ulFileLength; i++, data++) { for( j=0; j<8; j++) { /*置FPGA_CLK为0*/ GpioValSet_ALTERA(FPGA_CLK_ALTERA, 0); /*一个字节的最高bit先打入FPGA, 送到DIN脚上,在CLK上升沿打入*/ /*if(((*data) << j) & 0x80 )*/ if(((*data) >> j) & 0x01 ) { GpioValSet_ALTERA(FPGA_DIN_ALTERA, 1); } else { GpioValSet_ALTERA(FPGA_DIN_ALTERA, 0); } mydelay(); /*置FPGA_CLK为1,因为上升沿把DIN 上的电平值打入FPGA,此时打入*/ GpioValSet_ALTERA(FPGA_CLK_ALTERA, 1); mydelay(); } if(0 == GpioValGet_ALTERA(FPGA_INIT_ALTERA)) { free(pFpgaFile); printf("\r\n FPGA_INIT_ALTERA in middle!\n") ; return(3); } } free(pFpgaFile); for(i = 0; i <100; i++) { GpioValSet_ALTERA(FPGA_CLK_ALTERA , 0 ); mydelay(); GpioValSet_ALTERA(FPGA_CLK_ALTERA , 1 ); mydelay(); } }
milliseconds_t Sleep(milliseconds_t milliDuration) { int ticks = TimeUtils::Milli2Ticks(milliDuration); taskDelay(ticks); return TimeUtils::Ticks2Milli(ticks); }
/*************************************************************************** cs4281_hw_init: bring up the part **************************************************************************/ int cs4281_hw_init( void ) { UINT32 ac97_slotid; UINT32 temp1, temp2; /**************************************************** * set up the Sound System configuration ****************************************************/ printf("\nCS4281 HardWare Initialization ...\n"); /* ease the 'write protect' */ writel(0x4281, CS4281_pBA0 + BA0_CWPR); /* Blast the clock control register to 0, so that PLL starts out Blast the master serial port cntl register to 0, so that serial port starts out */ writel(0, CS4281_pBA0 + BA0_CLKCR1); writel(0, CS4281_pBA0 + BA0_SERMC); /***** <1> Make ESYN go to 0, to turn off the Sync pulse */ writel(0, CS4281_pBA0 + BA0_ACCTL); taskDelay(50); /*udelay(50);*/ /***** <2> Drive ARST# pin low for 1uS, then drive high, so that the external logic are reset */ writel(0, CS4281_pBA0 + BA0_SPMC); taskDelay(100); /* udelay(100);*/ writel(SPMC_RSTN, CS4281_pBA0 + BA0_SPMC); taskDelay(500); /*delayus(50000);*/ /***** <3> Turn on the Sound System clocks */ writel(CLKCR1_PLLP, CS4281_pBA0 + BA0_CLKCR1); taskDelay(500); /*delayus(50000);*/ writel(CLKCR1_PLLP | CLKCR1_SWCE, CS4281_pBA0 + BA0_CLKCR1); /***** <4> Power on everything for now */ writel(0x7e, CS4281_pBA0 + BA0_SSPM); /***** <5> Wait for clock stabilization */ for(temp1=0; temp1<10000; temp1++) { taskDelay(10); /*udelay(1000);*/ if( readl(CS4281_pBA0 + BA0_CLKCR1) & CLKCR1_DLLRDY ) break; } if(!(readl(CS4281_pBA0 + BA0_CLKCR1) & CLKCR1_DLLRDY)) { printf("cs4281: DLLRDY failed! \n"); return -1; } /***** <6> Enable ASYNC generation */ writel(ACCTL_ESYN, CS4281_pBA0 +BA0_ACCTL); /* wait for a while to start generating bit clock */ taskDelay(500); /*dealyus(50000);*/ /* Set the serial port timing configuration */ writel( SERMC_PTC_AC97, CS4281_pBA0 + BA0_SERMC ); /***** <7> Wait for the codec ready signal from the AC97 codec */ for(temp1=0; temp1<1000; temp1++) { taskDelay(1); /*udelay(1000);*/ if(readl(CS4281_pBA0 + BA0_ACSTS) & ACSTS_CRDY ) break; } if(!(readl(CS4281_pBA0 + BA0_ACSTS)& ACSTS_CRDY)) { printf("cs4281: ACTST never came ready!\n"); return -1; } /***** <8> Assert the 'valid frame' signal to begin sending commands to AC97 codec */ writel(ACCTL_VFRM |ACCTL_ESYN, CS4281_pBA0 + BA0_ACCTL); /***** <9> Wait until CODEC calibration is finished.*/ for(temp1=0; temp1<1000; temp1++) { taskDelay(10); /* delayus(10000);*/ if(cs4281_read_ac97(BA0_AC97_POWERDOWN, &temp2) ) return -1; if( (temp2 & 0x0000000F) == 0x0000000F ) break; } if( (temp2 & 0x0000000F) != 0x0000000F ) { printf("cs4281: Codec failed to calibrate\n"); return -1; } /***** <12> Start digital data transfer of audio data to codec */ writel(ACOSV_SLV3 | ACOSV_SLV4, CS4281_pBA0 + BA0_ACOSV ); writel(ACISV_ISV3 | ACISV_ISV4, CS4281_pBA0 + BA0_ACISV ); /************************************************ * Unmute the Master and * Alternate (headphone) volumes, to max. ************************************************/ cs4281_write_ac97(BA0_AC97_MASTER_VOLUME, 0x0000); cs4281_write_ac97(BA0_AC97_HEADPHONE_VOLUME, 0x0000); cs4281_write_ac97(BA0_AC97_MASTER_VOLUME_MONO, 0x0010); cs4281_write_ac97(BA0_AC97_PC_BEEP_VOLUME, 0x0010); cs4281_write_ac97(BA0_AC97_PHONE_VOLUME, 0x0008); cs4281_write_ac97(BA0_AC97_MIC_VOLUME, 0x0008); cs4281_write_ac97(BA0_AC97_CD_VOLUME, 0x0808); cs4281_write_ac97(BA0_AC97_VIDEO_VOLUME, 0x0808); cs4281_write_ac97(BA0_AC97_AUX_VOLUME, 0x0808); cs4281_write_ac97(BA0_AC97_PCM_OUT_VOLUME, 0x0000); cs4281_write_ac97(BA0_AC97_RECORD_GAIN, 0x0b0b); cs4281_write_ac97(BA0_AC97_RECORD_GAIN_MIC, 0x0b0b); /************************************************ * POWER on the DAC ************************************************/ cs4281_read_ac97(BA0_AC97_POWERDOWN, &temp1); cs4281_write_ac97(BA0_AC97_POWERDOWN, temp1 &= 0xfdff); /* Wait until we sample a DAC ready state */ for(temp2=0; temp2<32; temp2++) { taskDelay(1); /* delayus(1000);*/ cs4281_read_ac97(BA0_AC97_POWERDOWN, &temp1); if(temp1 & 0x2) break; } /************************************************ * POWER on the ADC ************************************************/ cs4281_read_ac97(BA0_AC97_POWERDOWN, &temp1); cs4281_write_ac97(BA0_AC97_POWERDOWN, temp1 &= 0xfeff); /* Wait until we sample a DAC ready state */ for(temp2=0; temp2<32; temp2++) { taskDelay(1); /* delayus(1000);*/ cs4281_read_ac97(BA0_AC97_POWERDOWN, &temp1); if(temp1 & 0x1) break; } /* For playback, we map AC97 slot 3 and 4(Left & Right PCM playback) to DMA Channel 0. Set the fifo to be 15 bytes at offset zero. */ ac97_slotid = 0x01000f00; writel(ac97_slotid, CS4281_pBA0 + BA0_FCR0); writel(ac97_slotid | FCRn_FEN, CS4281_pBA0 + BA0_FCR0); /* For record, we map AC97 slots 3 and 4(Left & Right Record) to DMA Channel 1. Set the fifo to be 15 bytes at offset 63. */ ac97_slotid = 0x0b0a0f3f; writel(ac97_slotid, CS4281_pBA0 + BA0_FCR1); writel(ac97_slotid | FCRn_FEN, CS4281_pBA0 + BA0_FCR1); /* Map the Playback SRC to the same AC97 slots(3 & 4-- --Playback left & right)as DMA channel 0. Map the record SRC to the same AC97 slots(10 & 11-- -- Record left & right) as DMA channel 1. */ ac97_slotid = 0x0b0a0100; writel(ac97_slotid, CS4281_pBA0 + BA0_SRCSA); /* Set 'Half Terminal Count Interrupt Enable' and 'Terminal Count Interrupt Enable' in DMA Control Registers 0 & 1. Set 'MSK' flag to 1 to keep the DMA engines paused. */ /* temp1 = (DCRn_TCIE | DCRn_MSK); writel(temp1, CS4281_pBA0 + BA0_DCR0);*/ temp1 = (DCRn_HTCIE | DCRn_TCIE | DCRn_MSK); writel(temp1, CS4281_pBA0 + BA0_DCR0); /* temp1 = (DCRn_HTCIE | DCRn_TCIE | DCRn_MSK); writel(temp1, CS4281_pBA0 + BA0_DCR1); */ temp1 = (DCRn_TCIE | DCRn_MSK); writel(temp1, CS4281_pBA0 + BA0_DCR1); /* Set 'Auto-Initialize Control' to 'enabled'; For playback, set 'Transfer Type Control'(TR[1:0]) to 'read transfer', for record, set Transfer Type Control to 'write transfer'. All other bits set to zero; Some will be changed @ transfer start. */ temp1 = (DMRn_DMA | DMRn_AUTO | DMRn_TR_READ); writel(temp1, CS4281_pBA0 + BA0_DMR0); temp1 = (DMRn_DMA | DMRn_AUTO | DMRn_TR_WRITE); writel(temp1, CS4281_pBA0 + BA0_DMR1); /* Enable DMA interrupts generally, and DMA0 & DMA1 interrupts specifically. */ temp1 = readl(CS4281_pBA0 + BA0_HIMR) & 0xfffbfcff; writel(temp1, CS4281_pBA0+BA0_HIMR); printf("\nCS4281 Hardware Initialization Complete!!\n"); return 0; }
void Suspend() { taskDelay(0); }
/* emulate unix sleep * casey */ void sleep(int seconds) { taskDelay(seconds*TICK); }
void usrAppInit(void) { printf("In usr usrAppInit\n"); // task_leds_start(); int i = 0; uint32_t cnt = 0; uint32_t heart_5[5]; uint32_t heart; uint32_t max; uint32_t min; uint32_t total; adc_init(); OSTaskChangePrio(0, 7); uint32_t ad; uint32_t pre_ad = get_v(); pre_tick = tickGet(); while (1) { ad = get_v(); if ((pre_ad < 570) && (ad >= 570)) { // printf("%d pre_ad:%d ad:%d", i++, pre_ad, ad); heart_tick = tickGet(); pre_tick = heart_tick - pre_tick; heart = (1200000 / pre_tick); if (heart > 35*10 && heart < 180*10) { memmove(&heart_5[0], &heart_5[1], 4*sizeof(uint32_t)); heart_5[4] = heart; if (cnt < 5) { cnt++; } else { total = max = min = heart_5[0]; for(int i = 1; i < 5; i++) { total += heart_5[i]; if (heart_5[i] > max) { max = heart_5[i]; } if (heart_5[i] < min) { min = heart_5[i]; } } heart = (total - max - min) / 3; printf("心率:%d.%d\n", heart / 10, heart % 10);//pre_tick } } pre_tick = heart_tick; sys_gpio_write(IO_LED2, E_LED_ON); } else if ((ad < 570) && (pre_ad >= 570)) { sys_gpio_write(IO_LED2, E_LED_OFF); } pre_ad = ad; taskDelay(2); } }
int nanosleep ( const struct timespec *rqtp, /* time to delay */ struct timespec *rmtp /* premature wakeup (NULL=no result) */ ) { int status; /* int oldErrno; */ ULONG delayTicks; struct timespec then; struct timespec now; int returnStatus; int savtype; if (rqtp == NULL || !TV_VALID(*rqtp)) { errno = EINVAL; return (ERROR); } if (TV_ISZERO(*rqtp)) return (OK); if (_func_pthread_setcanceltype != NULL) { _func_pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &savtype); } (void)clockLibInit (); /* make sure clock "running" */ (void)clock_gettime (CLOCK_REALTIME, &then); TV_CONVERT_TO_TICK (delayTicks, *rqtp); /* return's 1 (RESTART) if interrupted sleep */ status = taskDelay (delayTicks); if (status == 0) returnStatus = 0; else returnStatus = -1; if (rmtp != NULL) { (void)clock_gettime (CLOCK_REALTIME, &now); TV_SUB (now, then); /* make time relative to start */ if (TV_LT(now, *rqtp)) { TV_SET(*rmtp, *rqtp); TV_SUB(*rmtp, now); } else TV_ZERO((*rmtp)); } if (_func_pthread_setcanceltype != NULL) { _func_pthread_setcanceltype(savtype, NULL); } return (returnStatus); }
/****************************************************************************** ** Function: CFE_PSP_Restart() ** ** Purpose: ** Provides a common interface to reset the processor. This implementation ** may need to be customized for missions with custom reset requirements. ** ** Arguments: ** reset_type : Type of reset. ** ** Return: ** (none) */ void CFE_PSP_Restart(uint32 reset_type) { OS_printf("\nWarning: CFE PSP Restart with reset type %u!\n\n", reset_type); /* ** Delay for second or two, allow the print statement to send */ taskDelay(100); #ifndef _WRS_VX_SMP /* Changed for SMP API compatability. */ taskLock(); intLock(); #else /* Note: This only locks the current CPU core. Other cores * are still active and may continue to access system resources * or service the watchdog on an SMP system. */ taskCpuLock(); intCpuLock(); #endif if ( reset_type == CFE_ES_POWERON_RESET ) { CFE_PSP_ReservedMemoryPtr->bsp_reset_type = CFE_ES_POWERON_RESET; /* ** The sysToMonitor function flushes caches for us * * reboot(BOOT_CLEAR); */ /* ** Use the watchdog timer to assert a reset */ CFE_PSP_WatchdogEnable(); for(;;) { /* ** Set the current count value to something small */ CFE_PSP_WatchdogSet(CFE_PSP_WATCHDOG_MIN); /* ** Wait for the watchdog to expire... */ taskDelay(100); } } else { CFE_PSP_ReservedMemoryPtr->bsp_reset_type = CFE_ES_PROCESSOR_RESET; /* ** The sysToMonitor function flushes caches for us * * reboot(0); */ /* ** Use the watchdog timer to assert a reset */ CFE_PSP_WatchdogEnable(); for(;;) { /* ** Set the current count value to something small */ CFE_PSP_WatchdogSet(CFE_PSP_WATCHDOG_MIN); /* ** Wait for the watchdog to expire... */ taskDelay(100); } } }
static int dspIoctl (int devId, int function, int arg) { DSP_FD *pDsp = (DSP_FD *)devId; SND_DEV *pDev = pDsp->dev.pDev; AT91PS_SSC pSSC = (AT91PS_SSC ) pDev->port; union arg_union { int i; long l; int *pInt; long *pLong; snd_info_t *pInfo; } u_arg; u_arg.i = arg; switch (function) { case SNDCTL_DSP_SYNC: while (pDev->taskBusy) taskDelay (1); return OK; case SNDCTL_DSP_GETBLKSIZE: *u_arg.pInt = MAX_DMA_SIZE; return OK; case SNDCTL_DSP_SPEED: { int i = *u_arg.pLong; if (i < RATE_MIN) i = RATE_MIN; if (i > RATE_MAX) i = RATE_MAX; while (pDev->taskBusy) taskDelay (1); pDsp->info.rate = i; AT91F_SSC_SetBaudrate(pSSC, MCK, pDsp->info.rate*(BITS_BY_SLOT*SLOT_BY_FRAME)); return OK; } case SNDCTL_DSP_STEREO: while (pDev->taskBusy) taskDelay (1); pDsp->info.stereo = *u_arg.pInt; return OK; case SNDCTL_DSP_SAMPLESIZE: if (*u_arg.pInt == 8 || *u_arg.pInt == 16) { while (pDev->taskBusy) taskDelay (1); pDsp->info.sampleSize = *u_arg.pInt; return OK; } break; case SNDCTL_DSP_SETFORMAT: pDsp->info.uLaw = *u_arg.pInt; return OK; break; case SNDCTL_GET_INFO: *u_arg.pInfo = pDsp->info; return OK; case SNDCTL_SET_INFO: while (pDev->taskBusy) taskDelay (1); pDsp->info = *u_arg.pInfo; return OK; } errno = S_ioLib_UNKNOWN_REQUEST; return ERROR; }
void autonSelectMenu() { lcdSetBacklight(uart1, true); while (button != LCD_BTN_CENTER) { lcdClear(uart1); lcdSetText(uart1, 1, "Alliance Color?"); lcdSetText(uart1, 2, "< Red Blue >"); button = getLcdButtons(); if (button == LCD_BTN_LEFT) { lcdClear(uart1); lcdSetText(uart1, 1, "Starting Side?"); lcdSetText(uart1, 2, "< Middle Hang >"); while (button != LCD_BTN_LEFT || button != LCD_BTN_RIGHT) { button = getLcdButtons(); // Red Middle if (button == LCD_BTN_LEFT) { int screen = 0; int minScreen = 0; int maxScreen = 4; lcdClear(uart1); while (button != LCD_BTN_CENTER) { switch (screen) { case 0: lcdSetText(uart1, 1, "auton one"); break; case 1: lcdSetText(uart1, 1, "auton two"); break; case 2: lcdSetText(uart1, 1, "auton three"); break; case 3: lcdSetText(uart1, 1, "auton four"); break; } button = getLcdButtons(); if (button == LCD_BTN_RIGHT) { if (++screen > maxScreen) screen = minScreen; } else if (button == LCD_BTN_LEFT) { if (--screen < minScreen) screen = maxScreen; } } autonSelection = screen; lcdSetText(uart1, 1, " AUTONOMOUS"); lcdSetText(uart1, 2, " SET"); taskDelay(400); return; } // Red Hang if (button == LCD_BTN_RIGHT) { int screen = 0; int minScreen = 0; int maxScreen = 4; lcdClear(uart1); while (button != LCD_BTN_CENTER) { switch (screen) { case 0: lcdSetText(uart1, 1, "auton one"); break; case 1: lcdSetText(uart1, 1, "auton two"); break; case 2: lcdSetText(uart1, 1, "auton three"); break; case 3: lcdSetText(uart1, 1, "auton four"); break; } button = getLcdButtons(); if (button == LCD_BTN_RIGHT) { if (++screen > maxScreen) screen = minScreen; } else if (button == LCD_BTN_LEFT) { if (--screen < minScreen) screen = maxScreen; } } autonSelection = screen; lcdSetText(uart1, 1, " AUTONOMOUS"); lcdSetText(uart1, 2, " SET"); taskDelay(400); return; } } } if (button == LCD_BTN_RIGHT) { lcdClear(uart1); lcdSetText(uart1, 1, "Starting Side?"); lcdSetText(uart1, 2, "< Middle Hang >"); while (button != LCD_BTN_LEFT || button != LCD_BTN_RIGHT) { button = getLcdButtons(); // Blue Middle if (button == LCD_BTN_LEFT) { int screen = 0; int minScreen = 0; int maxScreen = 4; lcdClear(uart1); while (button != LCD_BTN_CENTER) { switch (screen) { case 0: lcdSetText(uart1, 1, "auton one"); break; case 1: lcdSetText(uart1, 1, "auton two"); break; case 2: lcdSetText(uart1, 1, "auton three"); break; case 3: lcdSetText(uart1, 1, "auton four"); break; } button = getLcdButtons(); if (button == LCD_BTN_RIGHT) { if (++screen > maxScreen) screen = minScreen; } else if (button == LCD_BTN_LEFT) { if (--screen < minScreen) screen = maxScreen; } } autonSelection = screen; lcdSetText(uart1, 1, " AUTONOMOUS"); lcdSetText(uart1, 2, " SET"); taskDelay(400); return; } // Blue Hang if (button == LCD_BTN_RIGHT) { int screen = 0; int minScreen = 0; int maxScreen = 4; lcdClear(uart1); while (button != LCD_BTN_CENTER) { switch (screen) { case 0: lcdSetText(uart1, 1, "auton one"); break; case 1: lcdSetText(uart1, 1, "auton two"); break; case 2: lcdSetText(uart1, 1, "auton three"); break; case 3: lcdSetText(uart1, 1, "auton four"); break; } button = getLcdButtons(); if (button == LCD_BTN_RIGHT) { if (++screen > maxScreen) screen = minScreen; } else if (button == LCD_BTN_LEFT) { if (--screen < minScreen) screen = maxScreen; } } autonSelection = screen; lcdSetText(uart1, 1, " AUTONOMOUS"); lcdSetText(uart1, 2, " SET"); taskDelay(400); return; } } } } }
/** * To talk to an ET system on another computer, we need * to find the TCP port number the server thread of that * ET system is listening on. There are a number of ways to * do this.<p> * * The remote client (who is running this routine) either * says he doesn't know where the ET is (config->host = * ET_HOST_REMOTE or ET_HOST_ANYWHERE), or he specifies * the host that the ET system is running on.<p> * * If we don't know the host name, we can broadcast and/or * multicast (UDP) to addresses which the ET system is * listening on (each address on a separate thread). In * this broad/multicast, we send the name of the ET system * that we want to find. The system which receives the * packet and has the same name, responds with quite a bit of information * including its TCP port #, uname, canonical host name, and * all of its IP addresses in dotted-decimal form. This * info is sent from each interface or broad/multicast address * that received the packet.<p> * * Notice that if broad/multicasting to an unknown host was successful, * only 1 structure in passed back in the allETinfo arg even though it * is a linked list.<p> * * If we know the hostname, however, we send a udp packet * to ET systems listening on that host and UDP port. The * proper system should be the only one to respond with its * server TCP port.<p> * * When a server responds to a broad/multicast and we've * specified ET_HOST_REMOTE, this routine must see whether * the server is really on a remote host and not the local one. * To make this determination, the server has also sent * his host name obtained by running "uname". That is compared * to the result of running "uname" on this host. * * @param etname ET system file name * @param ethost returns name of ET system's host * @param port returns ET system's TCP port * @param inetaddr returns host address as network-byte-ordered, 32-bit unsigned int * @param allETinfo returns a pointer to structure (linked list) with all of the ET system's * response information; * may be null if all relevant info given in ethost, port, and inetaddr args * @param config configuration passed to et_open * @param trys max # of times to broadcast UDP packet * @param waittime wait time for response to broad/multicast * * @return ET_OK if successful * @return ET_ERROR failure in select statement * @return ET_ERROR_BADARG NULL arg for ethost, port, or inetaddr * @return ET_ERROR_NOMEM cannot allocate memory * @return ET_ERROR_NETWORK error find IP addresses or resolving host name; * error converting dotted-decimal IP addr to binary; * error create socket; failure reading socket * @return ET_ERROR_SOCKET error setting socket option * @return ET_ERROR_TOOMANY if multiple ET systems responded when policy is to return an error when * more than one responds. * @return ET_ERROR_TIMEOUT if no responses received in allotted time */ int et_findserver2(const char *etname, char *ethost, int *port, uint32_t *inetaddr, et_response **allETinfo, et_open_config *config, int trys, struct timeval *waittime) { int i, j, k, l, m, n, err, version, addrCount, castType, gotMatch=0, subnetCount=0, ipAddrCount=0; int length, len_net, lastdelay, maxtrys=6, serverport=0, debug=1, magicInts[3]; const int on=1, timeincr[]={0,1,2,3,4,5}; uint32_t addr; codaIpList *baddr; /* encoding & decoding packets */ char *pbuf, buffer[4096]; /* should be way more than enough */ char outbuf[ET_FILENAME_LENGTH+1+5*sizeof(int)]; char localhost[ET_MAXHOSTNAMELEN]; char localuname[ET_MAXHOSTNAMELEN]; char **ipAddrs; char specifiedhost[ET_MAXHOSTNAMELEN]; char unqualifiedhost[ET_MAXHOSTNAMELEN]; int numresponses=0, remoteresponses=0; et_response *answer, *answer_first=NULL, *answer_prev=NULL, *first_remote=NULL; /* socket & select stuff */ struct in_addr castaddr; struct sockaddr_in servaddr, cliaddr; socklen_t len; int sockfd, numsockets=0, biggestsockfd=-1; struct timespec delaytime; struct timeval tv; fd_set rset; /* structure for holding info to send out a packet on a socket */ struct senddata { int sockfd; struct sockaddr_in servaddr; }; struct senddata *send; /* check args */ if ((ethost == NULL) || (port == NULL) || (inetaddr == NULL)) { fprintf(stderr, "et_findserver: invalid (null) arguments\n"); return ET_ERROR_BADARG; } /* count # of subnet addrs */ baddr = config->bcastaddrs; while (baddr != NULL) { subnetCount++; baddr = baddr->next; } /* allocate space to store all our outgoing stuff */ send = (struct senddata *) calloc(subnetCount + config->mcastaddrs.count, sizeof(struct senddata)); if (send == NULL) { fprintf(stderr, "et_findserver: cannot allocate memory\n"); return ET_ERROR_NOMEM; } /* find local uname */ if (etNetGetUname(localuname, ET_MAXHOSTNAMELEN) != ET_OK) { /* nothing will match this */ strcpy(localuname, "..."); } /* If the host is local or we know its name... */ if ((strcmp(config->host, ET_HOST_REMOTE) != 0) && (strcmp(config->host, ET_HOST_ANYWHERE) != 0)) { /* if it's local, find dot-decimal IP addrs */ if ((strcmp(config->host, ET_HOST_LOCAL) == 0) || (strcmp(config->host, "localhost") == 0)) { if ((etNetGetIpAddrs(&ipAddrs, &ipAddrCount, NULL) != ET_OK) || ipAddrCount < 1) { fprintf(stderr, "et_findserver: cannot find local IP addresses\n"); return ET_ERROR_NETWORK; } } /* else if we know its name, find dot-decimal IP addrs */ else { if ((etNetGetIpAddrs(&ipAddrs, &ipAddrCount, config->host) != ET_OK) || ipAddrCount < 1) { fprintf(stderr, "et_findserver: cannot find IP addresses for %s\n", config->host); return ET_ERROR_NETWORK; } } } /* else if the host name is not specified, and it's either * remote or anywhere out there, broad/multicast to find it */ else { } /* We need 1 socket for each subnet if broadcasting */ if ((config->cast == ET_BROADCAST) || (config->cast == ET_BROADANDMULTICAST)) { printf("et_findserver2 1\n"); /* for each listed broadcast address ... */ baddr = config->bcastaddrs; while (baddr != NULL) { /* put address into net-ordered binary form */ if (inet_aton(baddr->addr, &castaddr) == INET_ATON_ERR) { fprintf(stderr, "et_findserver: inet_aton error net_addr = %s\n", baddr->addr); for (j=0; j<numsockets; j++) { close(send[j].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); return ET_ERROR_NETWORK; } if (debug) { printf("et_findserver: send broadcast packet to %s on port %d\n", baddr->addr, config->udpport); } /* server's location */ bzero((void *)&servaddr, sizeof(servaddr)); servaddr.sin_family = AF_INET; servaddr.sin_addr = castaddr; servaddr.sin_port = htons((unsigned short)config->udpport); /* create socket */ sockfd = socket(AF_INET, SOCK_DGRAM, 0); if (sockfd < 0) { fprintf(stderr, "et_findserver: socket error\n"); for (j=0; j<numsockets; j++) { close(send[j].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); return ET_ERROR_NETWORK; } /* make this a broadcast socket */ err = setsockopt(sockfd, SOL_SOCKET, SO_BROADCAST, (void *) &on, sizeof(on)); if (err < 0) { fprintf(stderr, "et_findserver: setsockopt SO_BROADCAST error\n"); close(sockfd); for (j=0; j<numsockets; j++) { close(send[j].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); return ET_ERROR_SOCKET; } /* for sending packet and for select */ send[numsockets].sockfd = sockfd; send[numsockets].servaddr = servaddr; numsockets++; if (biggestsockfd < sockfd) biggestsockfd = sockfd; FD_SET(sockfd, &rset); /* go to next broadcast address */ baddr = baddr->next; } } /* if also configured for multicast, send that too */ if ((config->cast == ET_MULTICAST) || (config->cast == ET_BROADANDMULTICAST)) { printf("et_findserver2 2\n"); /* for each listed address ... */ for (i=0; i < config->mcastaddrs.count; i++) { /* put address into net-ordered binary form */ if (inet_aton(config->mcastaddrs.addr[i], &castaddr) == INET_ATON_ERR) { fprintf(stderr, "et_findserver: inet_aton error\n"); for (j=0; j<numsockets; j++) { close(send[j].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); return ET_ERROR_NETWORK; } if (debug) { printf("et_findserver: send multicast packet to %s on port %d\n", config->mcastaddrs.addr[i], config->multiport); } /* server's location */ bzero((void *)&servaddr, sizeof(servaddr)); servaddr.sin_family = AF_INET; servaddr.sin_addr = castaddr; servaddr.sin_port = htons((unsigned short)config->multiport); /* create socket */ sockfd = socket(AF_INET, SOCK_DGRAM, 0); if (sockfd < 0) { fprintf(stderr, "et_findserver: socket error\n"); for (j=0; j<numsockets; j++) { close(send[j].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); return ET_ERROR_NETWORK; } /* Set the scope of the multicast, but don't bother * if ttl = 1 since that's the default. */ if (config->ttl != 1) { unsigned char ttl = config->ttl; err = setsockopt(sockfd, IPPROTO_IP, IP_MULTICAST_TTL, (void *) &ttl, sizeof(ttl)); if (err < 0){ fprintf(stderr, "et_findserver: setsockopt IP_MULTICAST_TTL error\n"); close(sockfd); for (j=0; j<numsockets; j++) { close(send[j].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); return ET_ERROR_SOCKET; } } /* for sending packet and for select */ send[numsockets].sockfd = sockfd; send[numsockets].servaddr = servaddr; numsockets++; if (biggestsockfd < sockfd) biggestsockfd = sockfd; FD_SET(sockfd, &rset); } } /* time to wait for responses to broad/multicasting */ if (waittime != NULL) { delaytime.tv_sec = waittime->tv_sec; delaytime.tv_nsec = 1000*waittime->tv_usec; } else { delaytime.tv_sec = 0; delaytime.tv_nsec = 0; } lastdelay = delaytime.tv_sec; /* make select return after 0.01 seconds */ tv.tv_sec = 0; tv.tv_usec = 10000; /* 0.01 sec */ /* max # of broadcasts is maxtrys */ if ((trys > maxtrys) || (trys < 1)) { trys = maxtrys; } printf("et_findserver2 9\n"); /* Prepare output buffer that we send to servers: * (1) ET magic numbers (3 ints), * (2) ET version #, * (3) ET file name length (includes null) * (4) ET file name */ pbuf = outbuf; /* magic numbers */ magicInts[0] = htonl(ET_MAGIC_INT1); magicInts[1] = htonl(ET_MAGIC_INT2); magicInts[2] = htonl(ET_MAGIC_INT3); memcpy(pbuf, magicInts, sizeof(magicInts)); pbuf += sizeof(magicInts); /* ET major version number */ version = htonl(ET_VERSION); memcpy(pbuf, &version, sizeof(version)); pbuf += sizeof(version); /* length of ET system name string */ length = strlen(etname)+1; len_net = htonl(length); memcpy(pbuf, &len_net, sizeof(len_net)); pbuf += sizeof(len_net); /* name of the ET system we want to open */ memcpy(pbuf, etname, length); /* zero socket-set for select */ FD_ZERO(&rset); for (i=0; i < trys; i++) { /* send out packets */ for (j=0; j<numsockets; j++) { sendto(send[j].sockfd, (void *) outbuf, sizeof(outbuf), 0, (SA *) &send[j].servaddr, sizeof(servaddr)); } /* Increase waiting time for response each round. */ delaytime.tv_sec = lastdelay + timeincr[i]; lastdelay = delaytime.tv_sec; /* reset "rset" value each time select is called */ for (j=0; j<numsockets; j++) { FD_SET(send[j].sockfd, &rset); } /* Linux reportedly changes tv in "select" so reset it each round */ tv.tv_sec = 0; tv.tv_usec = 10000; /* 0.01 sec */ /* wait for responses from ET systems */ #ifdef VXWORKS taskDelay(lastdelay*60); #else nanosleep(&delaytime, NULL); #endif /* select */ err = select(biggestsockfd + 1, &rset, NULL, NULL, &tv); /* if select error ... */ if (err == -1) { fprintf(stderr, "et_findserver: select error\n"); for (j=0; j<numsockets; j++) { close(send[j].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); return ET_ERROR; } /* else if select times out, rebroadcast with longer waiting time */ else if (err == 0) { continue; } /* else if we have some response(s) ... */ else { for(j=0; j < numsockets; j++) { int bytes; if (FD_ISSET(send[j].sockfd, &rset) == 0) { /* this socket is not ready for reading */ continue; } anotherpacket: /* get back a packet from a server */ len = sizeof(cliaddr); n = recvfrom(send[j].sockfd, (void *) buffer, sizeof(buffer), 0, (SA *) &cliaddr, &len); /* if error ... */ if (n < 0) { fprintf(stderr, "et_findserver: recvfrom error, %s\n", strerror(errno)); for (k=0; k<numsockets; k++) { close(send[k].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); /* free up all answers */ et_freeAnswers(answer_first); return ET_ERROR_NETWORK; } /* allocate space for single response */ answer = (et_response *) calloc(1, sizeof(et_response)); if (answer == NULL) { fprintf(stderr, "et_findserver: out of memory\n"); for (k=0; k<numsockets; k++) { close(send[k].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); et_freeAnswers(answer_first); return ET_ERROR_NOMEM; } /* string answer structs in linked list */ if (answer_prev != NULL) { answer_prev->next = answer; } else { answer_first = answer; } /* see if there's another packet to be read on this socket */ bytes = 0; #ifdef VXWORKS ioctl(send[j].sockfd, FIONREAD, (int) &bytes); #else ioctl(send[j].sockfd, FIONREAD, (void *) &bytes); #endif /* decode packet from ET system: * * (0) ET magic numbers (3 ints) * (1) ET version # * (2) port of tcp server thread (not udp config->port) * (3) ET_BROADCAST or ET_MULTICAST (int) * (4) length of next string * (5) broadcast address (dotted-dec) if broadcast received or * multicast address (dotted-dec) if multicast received * (see int #3) * (6) length of next string * (7) hostname given by "uname" (used as a general * identifier of this host no matter which interface is used) * (8) length of next string * (9) canonical name of host * (10) number of IP addresses * (11) 32bit, net-byte ordered IPv4 address assoc with following address * (12) length of next string * (13) first dotted-decimal IPv4 address * (14) 32bit, net-byte ordered IPv4 address assoc with following address * (15) length of next string * (16) second dotted-decimal IPv4 address ... * * All known IP addresses are sent here both in numerical & dotted-decimal forms. */ pbuf = buffer; do { /* get ET magic #s */ memcpy(magicInts, pbuf, sizeof(magicInts)); pbuf += sizeof(magicInts); /* if the wrong magic numbers, reject it */ if (ntohl(magicInts[0]) != ET_MAGIC_INT1 || ntohl(magicInts[1]) != ET_MAGIC_INT2 || ntohl(magicInts[2]) != ET_MAGIC_INT3) { free(answer); break; } /* get ET system's major version # */ memcpy(&version, pbuf, sizeof(version)); version = ntohl(version); pbuf += sizeof(version); /* if the wrong version ET system is responding, reject it */ if (version != ET_VERSION) { free(answer); break; } /* get ET system's TCP port */ memcpy(&serverport, pbuf, sizeof(serverport)); serverport = ntohl(serverport); if ((serverport < 1) || (serverport > 65536)) { free(answer); break; } answer->port = serverport; pbuf += sizeof(serverport); /* broad or multi cast? might be both for java. */ memcpy(&castType, pbuf, sizeof(castType)); castType = ntohl(castType); if ((castType != ET_BROADCAST) && (castType != ET_MULTICAST) && (castType != ET_BROADANDMULTICAST)) { free(answer); break; } answer->castType = castType; pbuf += sizeof(castType); if (debug) { if (castType == ET_BROADCAST) { printf("et_findserver: reply to broadcast\n"); } else if (castType != ET_MULTICAST) { printf("et_findserver: reply to multicast\n"); } else if (castType != ET_BROADANDMULTICAST) { printf("et_findserver: reply to broad & multicast\n"); } else { printf("et_findserver: reply -> don't know if broad or multi casting\n"); } } /* get broad/multicast IP original packet sent to */ memcpy(&length, pbuf, sizeof(length)); length = ntohl(length); if ((length < 1) || (length > ET_IPADDRSTRLEN)) { free(answer); break; } pbuf += sizeof(length); memcpy(answer->castIP, pbuf, length); pbuf += length; /* get ET system's uname */ memcpy(&length, pbuf, sizeof(length)); length = ntohl(length); if ((length < 1) || (length > ET_MAXHOSTNAMELEN)) { free(answer); break; } pbuf += sizeof(length); memcpy(answer->uname, pbuf, length); pbuf += length; /* get ET system's canonical name */ memcpy(&length, pbuf, sizeof(length)); length = ntohl(length); if (length > ET_MAXHOSTNAMELEN) { free(answer); break; } pbuf += sizeof(length); if (length > 0) { memcpy(answer->canon, pbuf, length); pbuf += length; } /* get number of IP addrs */ memcpy(&addrCount, pbuf, sizeof(addrCount)); addrCount = ntohl(addrCount); if ((addrCount < 0) || (addrCount > 20)) { free(answer); break; } answer->addrCount = addrCount; pbuf += sizeof(addrCount); /* allocate mem - arrays of ints & char *'s */ answer->addrs = (uint32_t *)calloc(addrCount, sizeof(uint32_t)); answer->ipaddrs = (char **)calloc(addrCount, sizeof(char *)); if (answer->addrs == NULL || answer->ipaddrs == NULL) { for (k=0; k<numsockets; k++) { close(send[k].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); free(answer); et_freeAnswers(answer_first); return ET_ERROR_NOMEM; } /* read in all the addrs */ for (k=0; k < addrCount; k++) { memcpy(&addr, pbuf, sizeof(addr)); /* do not swap the addr as it must be network byte ordered */ answer->addrs[k] = addr; pbuf += sizeof(addr); memcpy(&length, pbuf, sizeof(length)); length = ntohl(length); if ((length < 1) || (length > ET_MAXHOSTNAMELEN)) { for (l=0; l < k; l++) { free(answer->ipaddrs[l]); } free(answer->ipaddrs); free(answer->addrs); free(answer); break; } pbuf += sizeof(length); answer->ipaddrs[k] = strdup(pbuf); /* ending NULL is sent so we're OK */ if (answer->ipaddrs[k] == NULL) { for (l=0; l<numsockets; l++) { close(send[l].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); for (l=0; l < k; l++) { free(answer->ipaddrs[l]); } free(answer->ipaddrs); free(answer->addrs); free(answer); et_freeAnswers(answer_first); return ET_ERROR_NOMEM; } answer->ipaddrs[k][length-1] = '\0'; pbuf += length; } if (debug) { printf("et_findserver: RESPONSE from %s at %d with cast addr = %s and uname = %s\n", answer->ipaddrs[0], answer->port, answer->castIP, answer->uname); } numresponses++; } while(0); /* Now that we have a real answer, make that a part of the list */ answer_prev = answer; /* see if there's another packet to be read on this socket */ if (bytes > 0) { goto anotherpacket; } } /* for each socket (j) */ /* If host is local or we know its name. There may be many responses. * Look only at the response that matches the host's address. */ if ((strcmp(config->host, ET_HOST_REMOTE) != 0) && (strcmp(config->host, ET_HOST_ANYWHERE) != 0)) { /* analyze the answers/responses */ answer = answer_first; while (answer != NULL) { /* The problem here is another ET system of the * same name may respond, but we're interested * only in the one on the specified host. */ for (n=0; n < answer->addrCount; n++) { for (m=0; m<ipAddrCount; m++) { if (debug) printf("et_findserver: comparing %s to incoming %s\n", ipAddrs[m], answer->ipaddrs[n]); if (strcmp(ipAddrs[m], answer->ipaddrs[n]) == 0) { gotMatch = 1; break; } } if (gotMatch) break; } if (gotMatch) { if (debug) printf("et_findserver: got a match to local or specific: %s\n", answer->ipaddrs[n]); for (k=0; k<numsockets; k++) { close(send[k].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); strcpy(ethost, answer->ipaddrs[n]); *port = answer->port; *inetaddr = answer->addrs[n]; et_freeAnswers(answer_first); return ET_OK; } answer = answer->next; } } /* If the host may be anywhere (local or remote) ... */ else if (strcmp(config->host, ET_HOST_ANYWHERE) == 0) { /* if our policy is to return an error for more than 1 response ... */ if ((config->policy == ET_POLICY_ERROR) && (numresponses > 1)) { /* print error message and return */ if (debug) printf("et_findserver: %d responses to broad/multicast -\n", numresponses); j = 1; answer = answer_first; while (answer != NULL) { fprintf(stderr, " RESPONSE %d from %s at %d\n", j++, answer->ipaddrs[0], answer->port); answer = answer->next; } for (k=0; k<numsockets; k++) { close(send[k].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); et_freeAnswers(answer_first); return ET_ERROR_TOOMANY; } /* analyze the answers/responses */ j = 0; answer = answer_first; while (answer != NULL) { /* If our policy is to take the first response do so. If our * policy is ET_POLICY_ERROR, we can also return the first as * we can only be here if there's been just 1 response. */ if ((config->policy == ET_POLICY_FIRST) || (config->policy == ET_POLICY_ERROR)) { if (debug) printf("et_findserver: got a match to .anywhere (%s), first or error policy\n", answer->ipaddrs[0]); for (k=0; k<numsockets; k++) { close(send[k].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); strcpy(ethost, answer->ipaddrs[0]); *port = answer->port; *inetaddr = answer->addrs[0]; if (allETinfo != NULL) { /* remove answer from linked list & return the first of new list */ answer_first = removeResponseFromList(answer_first, answer); *allETinfo = answer; } /* free the rest of the linked list */ et_freeAnswers(answer_first); return ET_OK; } /* else if our policy is to take the first local response ... */ else if (config->policy == ET_POLICY_LOCAL) { if (strcmp(localuname, answer->uname) == 0) { if (debug) printf("et_findserver: got a uname match to .anywhere, local policy\n"); for (k=0; k<numsockets; k++) { close(send[k].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); strcpy(ethost, answer->ipaddrs[0]); *port = answer->port; *inetaddr = answer->addrs[0]; if (allETinfo != NULL) { answer_first = removeResponseFromList(answer_first, answer); *allETinfo = answer; } et_freeAnswers(answer_first); return ET_OK; } /* If we went through all responses without finding * a local one, pick the first one we received. */ if (++j == numresponses-1) { if (debug) printf("et_findserver: got a match to .anywhere, nothing local available\n"); answer = answer_first; for (k=0; k<numsockets; k++) { close(send[k].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); strcpy(ethost, answer->ipaddrs[0]); *port = answer->port; *inetaddr = answer->addrs[0]; if (allETinfo != NULL) { answer_first = removeResponseFromList(answer_first, answer); *allETinfo = answer; } et_freeAnswers(answer_first); return ET_OK; } } answer = answer->next; } } /* if user did not specify host name, and it must be remote ... */ else if (strcmp(config->host, ET_HOST_REMOTE) == 0) { /* analyze the answers/responses */ answer = answer_first; while (answer != NULL) { /* The problem here is if we are broadcasting, a local ET * system of the same name may respond, but we're interested * only in the remote systems. Weed out the local system. */ if (strcmp(localuname, answer->uname) == 0) { answer = answer->next; continue; } remoteresponses++; if (first_remote == NULL) { first_remote = answer; } /* The ET_POLICY_LOCAL doesn't make any sense here so * default to ET_POLICY_FIRST */ if ((config->policy == ET_POLICY_FIRST) || (config->policy == ET_POLICY_LOCAL)) { if (debug) printf("et_findserver: got a match to .remote, first or local policy\n"); for (k=0; k<numsockets; k++) { close(send[k].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); strcpy(ethost, answer->ipaddrs[0]); *port = answer->port; *inetaddr = answer->addrs[0]; if (allETinfo != NULL) { answer_first = removeResponseFromList(answer_first, answer); *allETinfo = answer; } et_freeAnswers(answer_first); return ET_OK; } answer = answer->next; } /* If we're here, we do NOT have a first/local policy with at least * 1 remote response. */ if (config->policy == ET_POLICY_ERROR) { if (remoteresponses == 1) { if (debug) printf("et_findserver: got a match to .remote, error policy\n"); for (k=0; k<numsockets; k++) { close(send[k].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); strcpy(ethost, first_remote->ipaddrs[0]); *port = first_remote->port; *inetaddr = first_remote->addrs[0]; if (allETinfo != NULL) { answer_first = removeResponseFromList(answer_first, first_remote); *allETinfo = first_remote; } et_freeAnswers(answer_first); return ET_OK; } else if (remoteresponses > 1) { /* too many proper responses, return error */ fprintf(stderr, "et_findserver: %d responses to broad/multicast -\n", numresponses); j = 0; answer = answer_first; while (answer != NULL) { if (strcmp(localuname, answer->uname) == 0) { answer = answer->next; continue; } fprintf(stderr, " RESPONSE %d from %s at %d\n", j++, answer->ipaddrs[0], answer->port); answer = answer->next; } for (k=0; k<numsockets; k++) { close(send[k].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); et_freeAnswers(answer_first); return ET_ERROR_TOOMANY; } } } /* else if host is remote */ } /* else if we have some response(s) ... */ } /* try another broadcast */ /* if we're here, we got no response from any matching ET system */ if (debug) printf("et_findserver: no valid response received\n"); for (k=0; k<numsockets; k++) { close(send[k].sockfd); } free(send); freeIpAddrs(ipAddrs, ipAddrCount); et_freeAnswers(answer_first); return ET_ERROR_TIMEOUT; }
void fssrGainScan(int id, char *filename, \ int beg_chip, int end_chip, \ int beg_chan, int end_chan, int start_thr, int chan_mult) { FILE *fd; char fname[256], reg27[20]; int thr, ichip, ich; int i, j; int zero_count, amp, min_hits; int base_thr = start_thr; int temp_start_thr, zero_status, a; uint32_t pulser_rate = vscmGetPulserRate(id); uint32_t scaler[128]; /* Pulser Settings */ const int start_amp = 75; const int end_amp = 125; const int step_amp = 25; int steps = ((end_amp - start_amp) / step_amp) + 1; /* Get current time for file saving */ time_t now; time(&now); threshold_scan *data; #ifdef VXWORKS int ticks = 1; #else struct timespec ts; ts.tv_sec = 0; ts.tv_nsec = 16666667; /* ~16.6 ms (similar to 1 tick on VxWorks) */ #endif if (vscmIsNotInit(&id, __func__)) return; /* if ((end_chip - beg_chip) > 3) { logMsg("ERROR: %s: Chip range must be 4 chips or less\n", __func__); logMsg("ERROR: %s: Output files will be overwritten if >4\n", __func__); return; } */ if ((chan_mult < 1) || ((chan_mult & (chan_mult - 1)) != 0)) { logMsg("ERROR: %s: Channel multiplier (%d) ", __func__, chan_mult); logMsg("must be a power of 2\n"); return; } data = malloc(steps * chan_mult * sizeof(threshold_scan)); /* Temporarily disable both pulser outputs */ vscmPulser(id, 0, 0, 1); #ifdef VXWORKS min_hits = (int)(pulser_rate * (ticks / (float)sysClkRateGet()) * 0.99); #else min_hits = (int)(pulser_rate * (float)(ts.tv_nsec * 1.0e-9) * 0.99); #endif logMsg("INFO: %s: Min Hits = %d\n", __func__, min_hits); for (ichip = beg_chip; ichip <= end_chip; ichip++) { #ifdef DEBUG fssrStatus(id, ichip); #endif if(filename) { logMsg("%s\n", filename); logMsg("cd %d\n", chdir(filename)); logMsg("%s\n", getcwd()); char datetime[] = "YYYYMMDD_HHMM"; strftime(datetime, sizeof(datetime), "%Y%m%d_%H%M", localtime(&now)); struct stat sb; if (stat(datetime, &sb) != 0) mkdir(datetime, S_IRWXU | S_IRWXG | S_IROTH); char host[255]; gethostname(host, 255); sprintf(fname,"%s/%s_s%02d_c%1d_u%1d", datetime, host, id, ((ichip > 3) ? 2 : 1), ((ichip % 4) + 1)); if( (fd = fopen(fname, "w")) <= NULL ) { logMsg("ERROR: %s: Opening file: %s\n", __func__, fname); free(data); return; } else { logMsg("INFO: %s: Output file: %s\n", __func__, fname); } } else { logMsg("Chip %d\n", ichip); } fssrParseControl(id, ichip, reg27); if (filename) { fprintf(fd, "%s\n", reg27); } else printf("%s\n", reg27); /* Set all discriminators - start of loop*/ for (i = 0; i < 8; i++) { fssrSetThreshold(id, ichip, i, base_thr + (i * 30)); } for (i = 0; i < (128 / chan_mult); i++) { start_thr = base_thr; thr = start_thr; temp_start_thr = 1; fssrKillMaskDisableAll(id, ichip); fssrInjectMaskDisableAll(id, ichip); /* printf("[%d] Chan: ",id);*/ for (j = 0; j < chan_mult; j++) { ich = i + (j * (128 / chan_mult)); /* printf("%d ", ich);*/ fssrKillMaskEnableSingle(id, ichip, ich); fssrInjectMaskEnableSingle(id, ichip, ich); } /* printf("\n"); fflush(stdout);*/ for (a = 0; a < steps; a++) { amp = start_amp + (a * step_amp); thr = start_thr; zero_count = 0; /* printf("[%d] Amp: %d Start: %d\n",id, amp, thr); fflush(stdout);*/ vscmPulser(id, ((ichip > 3) ? 2 : 1), amp, (int)(min_hits / 0.99)); while(thr < 256 && zero_count < 2) { zero_status = 0; temp_start_thr = 0; fssrSetThreshold(id, ichip, 0, thr); vscmDisableChipScaler(id, ichip); vscmClearStripScalers(id, ichip); vscmEnableChipScaler(id, ichip); vscmDisableChipScaler(id, ichip); vscmEnableChipScaler(id, ichip); vscmPulserStart(id); /* Gather data by sleeping */ #ifdef VXWORKS taskDelay(ticks); #else nanosleep(&ts, NULL); #endif vscmDisableChipScaler(id, ichip); vscmReadStripScalers(id, ichip, scaler); for (j = 0; j < chan_mult; j++) { ich = i + (j * (128 / chan_mult)); if (thr == start_thr) { data[(a * chan_mult) + j].amp = amp; data[(a * chan_mult) + j].start = start_thr; } data[(a * chan_mult) + j].hits[thr] = scaler[ich]; if (scaler[ich] > min_hits) temp_start_thr++; if (scaler[ich] == 0) zero_status++; } if (zero_status == chan_mult) zero_count++; else zero_count = 0; if (temp_start_thr == chan_mult) start_thr = thr; thr++; } } /* Save the data struct to file */ if (filename) { for (j = 0; j < chan_mult; j++) { ich = i + (j * (128 / chan_mult)); fprintf(fd, "%d\n", ich); for (a = 0; a < steps; a++) { fprintf(fd, "%d, ", data[(a * chan_mult) + j].amp); fprintf(fd, "%d ", data[(a * chan_mult) + j].start); zero_count = 0; thr = data[(a *chan_mult) + j].start; while(thr < 256 && zero_count < 2) { fprintf(fd, ",%d ", data[(a * chan_mult) + j].hits[thr]); if (data[(a * chan_mult) + j].hits[thr] == 0) zero_count++; else zero_count = 0; thr++; } fprintf(fd, "\n"); } fprintf(fd, "\n"); } fflush(fd); } } /* End of Channel loop */ if (filename) fclose(fd); else printf("\n"); /* Set all discriminators - end of loop*/ for (i = 0; i < 8; i++) { fssrSetThreshold(id, ichip, i, base_thr + (i * 30)); } } /* End of Chip loop */ free(data); }
/************************************************************* * fadcFirmwareGLoad * - load up firmware for all initialized modules * * NOTE: Make call to * fadcFirmwareSetFilename(...); * if not using firmware from the default */ int fadcFirmwareGLoad(int chip, int pFlag) { int ifadc=0, id=0, step=0; unsigned int passed[FA_MAX_BOARDS+1], stepfail[FA_MAX_BOARDS+1]; if(chip<0 || chip>2) { printf("%s: ERROR: Invalid chip parameter %d\n", __FUNCTION__,chip); return ERROR; } /* Perform a hardware and software reset */ step=0; FALOCK; for(ifadc=0 ; ifadc<nfadc; ifadc++) { id = fadcID[ifadc]; if((id<=0) || (id>21) || (FAp[id] == NULL)) { printf("%s: ERROR : ADC in slot %d is not initialized \n", __FUNCTION__,id); passed[id] = 0; stepfail[id] = step; } else { passed[id] = 1; vmeWrite32(&FAp[id]->reset, 0xFFFF); } } FAUNLOCK; taskDelay(60); /* Check if FADC is Ready */ step=1; for(ifadc=0 ; ifadc<nfadc; ifadc++) { id = fadcID[ifadc]; if(fadcFirmwareTestReady(id, 60, pFlag) != OK) { printf("%s: ERROR: FADC %2d not ready after reset\n", __FUNCTION__,id); passed[id] = 0; stepfail[id] = step; } } /* Data to SRAM */ step=2; printf("%s: Loading SRAM with data \n",__FUNCTION__); for(ifadc=0 ; ifadc<nfadc; ifadc++) { id = fadcID[ifadc]; printf("%2d: ",id); fflush(stdout); if(passed[id]) /* Skip the ones that have previously failed */ { fadcFirmwareDownloadConfigData(id); if(fadcFirmwareVerifyDownload(id) != OK) { printf("%s: ERROR: FADC %2d Failed data verification at SRAM\n", __FUNCTION__,id); passed[id] = 0; stepfail[id] = step; } else printf(" Done\n"); } } /* SRAM TO PROM */ step=3; taskDelay(1); printf("%s: Loading PROM with SRAM data \n",__FUNCTION__); FALOCK; for(ifadc=0 ; ifadc<nfadc; ifadc++) { id = fadcID[ifadc]; if(passed[id]) /* Skip the ones that have previously failed */ { if(chip==FADC_FIRMWARE_LX110) vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_SRAM_TO_PROM1); else if(chip==FADC_FIRMWARE_FX70T) vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_SRAM_TO_PROM2); } } FAUNLOCK; taskDelay(1); for(ifadc=0 ; ifadc<nfadc; ifadc++) { id = fadcID[ifadc]; if(passed[id]) /* Skip the ones that have previously failed */ { if(fadcFirmwareTestReady(id, 60000, pFlag) != OK) /* Wait til it's done */ { printf("%s: ERROR: FADC %2d ready timeout SRAM -> PROM\n", __FUNCTION__,id); passed[id] = 0; stepfail[id] = step; } } } /* PROM TO SRAM (For verification) */ printf("%s: Loading SRAM with PROM data \n",__FUNCTION__); step=4; for(ifadc=0 ; ifadc<nfadc; ifadc++) { id = fadcID[ifadc]; if(passed[id]) /* Skip the ones that have previously failed */ { fadcFirmwareZeroSRAM(id); FALOCK; if(chip==FADC_FIRMWARE_LX110) vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_PROM1_TO_SRAM); else if(chip==FADC_FIRMWARE_FX70T) vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_PROM2_TO_SRAM); FAUNLOCK; } } taskDelay(1); for(ifadc=0 ; ifadc<nfadc; ifadc++) { id = fadcID[ifadc]; if(passed[id]) /* Skip the ones that have previously failed */ { if(fadcFirmwareTestReady(id, 60000, pFlag) != OK) /* Wait til it's done */ { printf("%s: ERROR: FADC %2d ready timeout PROM -> SRAM\n", __FUNCTION__,id); passed[id] = 0; stepfail[id] = step; } } } /* Compare SRAM to Data Array */ printf("%s: Verifying data \n",__FUNCTION__); step=5; for(ifadc=0 ; ifadc<nfadc; ifadc++) { id = fadcID[ifadc]; printf("%2d: ",id); fflush(stdout); if(passed[id]) /* Skip the ones that have previously failed */ { if(fadcFirmwareVerifyDownload(id) != OK) { printf("%s: ERROR: FADC %d PROM data not verified\n", __FUNCTION__,id); passed[id] = 0; stepfail[id] = step; } else printf(" Done\n"); } } /* PROM to FPGA (Reboot FPGA) */ printf("%s: Rebooting FPGA \n",__FUNCTION__); step=6; for(ifadc=0 ; ifadc<nfadc; ifadc++) { id = fadcID[ifadc]; if(passed[id]) /* Skip the ones that have previously failed */ { FALOCK; if(chip==FADC_FIRMWARE_LX110) vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_REBOOT_FPGA1); else if(chip==FADC_FIRMWARE_FX70T) vmeWrite32(&FAp[id]->prom_reg1,FA_PROMREG1_REBOOT_FPGA2); FAUNLOCK; } } taskDelay(1); for(ifadc=0 ; ifadc<nfadc; ifadc++) { id = fadcID[ifadc]; if(passed[id]) /* Skip the ones that have previously failed */ { if(fadcFirmwareTestReady(id, 60000, pFlag) != OK) /* Wait til it's done */ { printf("%s: ERROR: FADC %2d ready timeout PROM -> FPGA\n", __FUNCTION__,id); passed[id] = 0; stepfail[id] = step; } } } for(ifadc=0 ; ifadc<nfadc; ifadc++) { id = fadcID[ifadc]; if(passed[id]) /* Skip the ones that have previously failed */ { printf("%s: Done programming FADC %2d\n", __FUNCTION__,id); } else { printf("%s: FAILED programming FADC %2d at step %d\n", __FUNCTION__,id,stepfail[id]); } } return OK; }