From ba78bd9ba834260d035a9830726afc34fdad2a15 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 18 Oct 2009 23:32:54 +0200 Subject: import firmware from LEGO v1.05 --- AT91SAM7S256/Source/c_cmd.c | 6292 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 6292 insertions(+) create mode 100644 AT91SAM7S256/Source/c_cmd.c (limited to 'AT91SAM7S256/Source/c_cmd.c') diff --git a/AT91SAM7S256/Source/c_cmd.c b/AT91SAM7S256/Source/c_cmd.c new file mode 100644 index 0000000..9b83c14 --- /dev/null +++ b/AT91SAM7S256/Source/c_cmd.c @@ -0,0 +1,6292 @@ +// +// Date init 14.12.2004 +// +// Revision date $Date:: 28-03-07 14:53 $ +// +// Filename $Workfile:: c_cmd.c $ +// +// Version $Revision:: 67 $ +// +// Archive $Archive:: /LMS2006/Sys01/Main/Firmware/Source/c_cmd.c $ +// +// Platform C +// + +// +// File Description: +// This file contains the virtual machine implementation to run bytecode +// programs compatible with LEGO MINDSTORMS NXT Software 1.0. +// +// This module (c_cmd) is also responsible for reading the system timer +// (d_timer) and returning on 1 ms timer boundaries. +// + +#include "stdconst.h" +#include "modules.h" + +#include "c_cmd.iom" +#include "c_output.iom" +#include "c_input.iom" +#include "c_loader.iom" +#include "c_ui.iom" +#include "c_sound.iom" +#include "c_button.iom" +#include "c_display.iom" +#include "c_comm.iom" +#include "c_lowspeed.iom" + +#include "c_cmd.h" +#include "c_cmd_bytecodes.h" +#include "d_timer.h" +#include +#include +#include + + +static IOMAPCMD IOMapCmd; +static VARSCMD VarsCmd; +static HEADER **pHeaders; + +const HEADER cCmd = +{ + 0x00010001L, + "Command", + cCmdInit, + cCmdCtrl, + cCmdExit, + (void *)&IOMapCmd, + (void *)&VarsCmd, + (UWORD)sizeof(IOMapCmd), + (UWORD)sizeof(VarsCmd), + 0x0000 //Code size - not used so far +}; + +#if ENABLE_VM + +// c_cmd_drawing.inc is just another C source file +// (the graphics implementation was split off for practical file management reasons) +#include "c_cmd_drawing.inc" + + +// +//Function pointers to sub-interpreters +//This table is indexed by arity +//Unary operations can have arity of 1 or 2 (some need a destination) +//All instructions taking 4 or more operands are handled as "Other" +// +static pInterp InterpFuncs[INTERP_COUNT] = +{ + cCmdInterpNoArg, + cCmdInterpUnop1, + cCmdInterpUnop2, + cCmdInterpBinop, + cCmdInterpOther +}; + +// +//Function pointers to SysCall implementations +//See interpreter for OP_SYSCALL +// +static pSysCall SysCallFuncs[SYSCALL_COUNT] = +{ + cCmdWrapFileOpenRead, + cCmdWrapFileOpenWrite, + cCmdWrapFileOpenAppend, + cCmdWrapFileRead, + cCmdWrapFileWrite, + cCmdWrapFileClose, + cCmdWrapFileResolveHandle, + cCmdWrapFileRename, + cCmdWrapFileDelete, + cCmdWrapSoundPlayFile, + cCmdWrapSoundPlayTone, + cCmdWrapSoundGetState, + cCmdWrapSoundSetState, + cCmdWrapDrawText, + cCmdWrapDrawPoint, + cCmdWrapDrawLine, + cCmdWrapDrawCircle, + cCmdWrapDrawRect, + cCmdWrapDrawPicture, + cCmdWrapSetScreenMode, + cCmdWrapReadButton, + cCmdWrapCommLSWrite, + cCmdWrapCommLSRead, + cCmdWrapCommLSCheckStatus, + cCmdWrapRandomNumber, + cCmdWrapGetStartTick, + cCmdWrapMessageWrite, + cCmdWrapMessageRead, + cCmdWrapCommBTCheckStatus, + cCmdWrapCommBTWrite, + cCmdWrapCommBTRead, + cCmdWrapKeepAlive, + cCmdWrapIOMapRead, + cCmdWrapIOMapWrite +}; + +// +//Next set of arrays are lookup tables for IOM access bytecodes +// +TYPE_CODE IO_TYPES_IN[IO_IN_FIELD_COUNT] = +{ + //IO_IN0 + TC_UBYTE, //IO_IN_TYPE + TC_UBYTE, //IO_IN_MODE + TC_UWORD, //IO_IN_ADRAW + TC_UWORD, //IO_IN_NORMRAW + TC_SWORD, //IO_IN_SCALED_VAL + TC_UBYTE, //IO_IN_INVALID_DATA + + //IO_IN1 + TC_UBYTE, //IO_IN_TYPE + TC_UBYTE, //IO_IN_MODE + TC_UWORD, //IO_IN_ADRAW + TC_UWORD, //IO_IN_NORMRAW + TC_SWORD, //IO_IN_SCALED_VAL + TC_UBYTE, //IO_IN_INVALID_DATA + + //IO_IN2 + TC_UBYTE, //IO_IN_TYPE + TC_UBYTE, //IO_IN_MODE + TC_UWORD, //IO_IN_ADRAW + TC_UWORD, //IO_IN_NORMRAW + TC_SWORD, //IO_IN_SCALED_VAL + TC_UBYTE, //IO_IN_INVALID_DATA + + //IO_IN3 + TC_UBYTE, //IO_IN_TYPE + TC_UBYTE, //IO_IN_MODE + TC_UWORD, //IO_IN_ADRAW + TC_UWORD, //IO_IN_NORMRAW + TC_SWORD, //IO_IN_SCALED_VAL + TC_UBYTE, //IO_IN_INVALID_DATA +}; + +TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] = +{ + //IO_OUT0 + TC_UBYTE, //IO_OUT_FLAGS + TC_UBYTE, //IO_OUT_MODE + TC_SBYTE, //IO_OUT_SPEED + TC_SBYTE, //IO_OUT_ACTUAL_SPEED + TC_SLONG, //IO_OUT_TACH_COUNT + TC_ULONG, //IO_OUT_TACH_LIMIT + TC_UBYTE, //IO_OUT_RUN_STATE + TC_SBYTE, //IO_OUT_TURN_RATIO + TC_UBYTE, //IO_OUT_REG_MODE + TC_UBYTE, //IO_OUT_OVERLOAD + TC_UBYTE, //IO_OUT_REG_P_VAL + TC_UBYTE, //IO_OUT_REG_I_VAL + TC_UBYTE, //IO_OUT_REG_D_VAL + TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT + TC_SLONG, //IO_OUT_ROTATION_COUNT + + //IO_OUT1 + TC_UBYTE, //IO_OUT_FLAGS + TC_UBYTE, //IO_OUT_MODE + TC_SBYTE, //IO_OUT_SPEED + TC_SBYTE, //IO_OUT_ACTUAL_SPEED + TC_SLONG, //IO_OUT_TACH_COUNT + TC_ULONG, //IO_OUT_TACH_LIMIT + TC_UBYTE, //IO_OUT_RUN_STATE + TC_SBYTE, //IO_OUT_TURN_RATIO + TC_UBYTE, //IO_OUT_REG_MODE + TC_UBYTE, //IO_OUT_OVERLOAD + TC_UBYTE, //IO_OUT_REG_P_VAL + TC_UBYTE, //IO_OUT_REG_I_VAL + TC_UBYTE, //IO_OUT_REG_D_VAL + TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT + TC_SLONG, //IO_OUT_ROTATION_COUNT + + //IO_OUT2 + TC_UBYTE, //IO_OUT_FLAGS + TC_UBYTE, //IO_OUT_MODE + TC_SBYTE, //IO_OUT_SPEED + TC_SBYTE, //IO_OUT_ACTUAL_SPEED + TC_SLONG, //IO_OUT_TACH_COUNT + TC_ULONG, //IO_OUT_TACH_LIMIT + TC_UBYTE, //IO_OUT_RUN_STATE + TC_SBYTE, //IO_OUT_TURN_RATIO + TC_UBYTE, //IO_OUT_REG_MODE + TC_UBYTE, //IO_OUT_OVERLOAD + TC_UBYTE, //IO_OUT_REG_P_VAL + TC_UBYTE, //IO_OUT_REG_I_VAL + TC_UBYTE, //IO_OUT_REG_D_VAL + TC_SLONG, //IO_OUT_BLOCK_TACH_COUNT + TC_SLONG, //IO_OUT_ROTATION_COUNT +}; + + +TYPE_CODE * IO_TYPES[2] = +{ + IO_TYPES_IN, + IO_TYPES_OUT +}; + +//Actual pointers filled in during cCmdInit() +void * IO_PTRS_IN[IO_IN_FIELD_COUNT]; +void * IO_PTRS_OUT[IO_OUT_FIELD_COUNT]; + +void ** IO_PTRS[2] = +{ + IO_PTRS_IN, + IO_PTRS_OUT +}; + + +//cCmdHandleRemoteCommands is the registered handler for "direct" command protocol packets +//It is only intended to be called via c_comm's main protocol handler +UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) +{ + NXT_STATUS RCStatus = NO_ERR; + //Response packet length. Always includes RCStatus byte. + ULONG ResponseLen = 1; + //Boolean flag to send a response. TRUE unless overridden below. + ULONG SendResponse = TRUE; + //Boolean flag if we are handling a reply telegram. FALSE unless overridden. + ULONG IncomingReply = FALSE; + ULONG i, FirstPort, LastPort; + UWORD LStatus; + UWORD Count, QueueID; + UBYTE * pData; + + //Illegal call, give up + if (pInBuf == NULL || pLen == NULL) + { + NXT_BREAK; + return (0xFFFF); + } + + //No output buffer provided, so skip any work related to returning a response + if (pOutBuf == NULL) + SendResponse = FALSE; + + //If first byte identifies this as a reply telegram, we have different work to do. + if (pInBuf[0] == 0x02) + { + IncomingReply = TRUE; + //Reply telegrams never get responses, even if caller provided a buffer. + SendResponse = FALSE; + } + + //Advance pInBuf past command type byte + pInBuf++; + + if (!IncomingReply) + { + switch(pInBuf[0]) + { + case RC_START_PROGRAM: + { + //Check that file exists. If not, return error + //!!! Should return standard loader file error in cases like this?? + //!!! Proper solution would also check file mode to avoid confusing errors + if (LOADER_ERR(LStatus = pMapLoader->pFunc(FINDFIRST, (&pInBuf[1]), NULL, NULL)) != SUCCESS) + { + RCStatus = ERR_RC_ILLEGAL_VAL; + break; + } + + //Close file handle returned by FINDFIRST + pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(LStatus), NULL, NULL); + + //File must exist, so inform UI to attempt execution in the usual way (enables consistent feedback) + pMapUi->Flags |= UI_EXECUTE_LMS_FILE; + strncpy((PSZ)(pMapUi->LMSfilename), (PSZ)(&pInBuf[1]), FILENAME_LENGTH + 1); + } + break; + + case RC_STOP_PROGRAM: + { + if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE) + { + RCStatus = ERR_NO_PROG; + break; + } + + IOMapCmd.DeactivateFlag = TRUE; + } + break; + + case RC_PLAY_SOUND_FILE: + { + if (LOADER_ERR(pMapLoader->pFunc(FINDFIRST, (&pInBuf[2]), NULL, NULL)) != SUCCESS) + { + RCStatus = ERR_RC_ILLEGAL_VAL; + break; + } + + //Close file handle returned by FINDFIRST + pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(LStatus), NULL, NULL); + + if (pInBuf[1] == FALSE) + pMapSound->Mode = SOUND_ONCE; + else //Any non-zero value treated as TRUE + pMapSound->Mode = SOUND_LOOP; + + strncpy((PSZ)pMapSound->SoundFilename, (PSZ)(&pInBuf[2]), FILENAME_LENGTH + 1); + pMapSound->Flags |= SOUND_UPDATE; + } + break; + + case RC_PLAY_TONE: + { + pMapSound->Mode = SOUND_TONE; + //!!! Range check valid values? + memcpy((PSZ)(&(pMapSound->Freq)), (PSZ)(&pInBuf[1]), 2); + memcpy((PSZ)(&(pMapSound->Duration)), (PSZ)(&pInBuf[3]), 2); + + pMapSound->Flags |= SOUND_UPDATE; + } + break; + + case RC_SET_OUT_STATE: + { + //Don't do anything if illegal port specification is made + if (pInBuf[1] >= NO_OF_OUTPUTS && pInBuf[1] != 0xFF) + { + RCStatus = ERR_RC_ILLEGAL_VAL; + break; + } + + //0xFF is protocol defined to mean "all ports". + if (pInBuf[1] == 0xFF) + { + FirstPort = 0; + LastPort = NO_OF_OUTPUTS - 1; + } + else + FirstPort = LastPort = pInBuf[1]; + + for (i = FirstPort; i <= LastPort; i++) + { + pMapOutPut->Outputs[i].Speed = pInBuf[2]; + pMapOutPut->Outputs[i].Mode = pInBuf[3]; + pMapOutPut->Outputs[i].RegMode = pInBuf[4]; + pMapOutPut->Outputs[i].SyncTurnParameter = pInBuf[5]; + pMapOutPut->Outputs[i].RunState = pInBuf[6]; + memcpy((PSZ)(&(pMapOutPut->Outputs[i].TachoLimit)), (PSZ)(&pInBuf[7]), 4); + + pMapOutPut->Outputs[i].Flags |= UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT; + } + } + break; + + case RC_SET_IN_MODE: + { + i = pInBuf[1]; + + //Don't do anything if illegal port specification is made + //!!! Should check against legal Types and Modes? (bitmask for Modes?) + if (i >= NO_OF_INPUTS) + { + RCStatus = ERR_RC_ILLEGAL_VAL; + break; + } + + pMapInput->Inputs[i].SensorType = pInBuf[2]; + pMapInput->Inputs[i].SensorMode = pInBuf[3]; + + //Set InvalidData flag automatically since type may have changed + pMapInput->Inputs[i].InvalidData = TRUE; + } + break; + + case RC_GET_OUT_STATE: + { + if (SendResponse == TRUE) + { + i = pInBuf[1]; + + //Return error and all zeros if illegal port specification is made + if (i >= NO_OF_OUTPUTS) + { + RCStatus = ERR_RC_ILLEGAL_VAL; + memset(&(pOutBuf[ResponseLen]), 0, 22); + ResponseLen += 22; + break; + } + + //Echo port + pOutBuf[ResponseLen] = i; + ResponseLen++; + + //Power + pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].Speed; + ResponseLen++; + + //Mode + pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].Mode; + ResponseLen++; + + //RegMode + pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].RegMode; + ResponseLen++; + + //TurnRatio + pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].SyncTurnParameter; + ResponseLen++; + + //RunState + pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].RunState; + ResponseLen++; + + //TachoLimit ULONG + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].TachoLimit)), 4); + ResponseLen += 4; + + //TachoCount SLONG + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].TachoCnt)), 4); + ResponseLen += 4; + + //BlockTachoCount SLONG + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].BlockTachoCount)), 4); + ResponseLen += 4; + + //RotationCount SLONG + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].RotationCount)), 4); + ResponseLen += 4; + + NXT_ASSERT(ResponseLen == 23); + } + } + break; + + case RC_GET_IN_VALS: + { + if (SendResponse == TRUE) + { + i = pInBuf[1]; + + //Return error and all zeros if illegal port specification is made + if (i >= NO_OF_INPUTS) + { + RCStatus = ERR_RC_ILLEGAL_VAL; + memset(&(pOutBuf[ResponseLen]), 0, 13); + ResponseLen += 13; + break; + } + + //Echo port + pOutBuf[ResponseLen] = i; + ResponseLen++; + + //Set "Valid?" boolean + if (pMapInput->Inputs[i].InvalidData) + pOutBuf[ResponseLen] = FALSE; + else + pOutBuf[ResponseLen] = TRUE; + + ResponseLen++; + + //Set "Calibrated?" boolean + //!!! "Calibrated?" is a placeholder in the protocol. Always FALSE for now. + pOutBuf[ResponseLen] = FALSE; + ResponseLen++; + + pOutBuf[ResponseLen] = pMapInput->Inputs[i].SensorType; + ResponseLen++; + + pOutBuf[ResponseLen] = pMapInput->Inputs[i].SensorMode; + ResponseLen++; + + //Set Raw, Normalized, and Scaled values + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].ADRaw)), 2); + ResponseLen += 2; + + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorRaw)), 2); + ResponseLen += 2; + + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorValue)), 2); + ResponseLen += 2; + + //!!! Return normalized raw value in place of calibrated value for now -- see comment above + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorRaw)), 2); + ResponseLen += 2; + + NXT_ASSERT(ResponseLen == 14); + } + } + break; + + case RC_RESET_IN_VAL: + { + i = pInBuf[1]; + + //Don't do anything if illegal port specification is made + if (i >= NO_OF_INPUTS) + { + RCStatus = ERR_RC_ILLEGAL_VAL; + break; + } + + //Clear SensorValue to zero. Leave Raw and Normalized as-is, since they never accumulate running values. + pMapInput->Inputs[i].SensorValue = 0; + } + break; + + case RC_MESSAGE_WRITE: + { + QueueID = pInBuf[1]; + Count = pInBuf[2]; + pData = &(pInBuf[3]); + + //If Count is illegal or MsgData is not null-terminated, + // we can't accept it as a valid string + if (Count == 0 || Count > MAX_MESSAGE_SIZE || pData[Count - 1] != 0x00) + { + RCStatus = ERR_RC_ILLEGAL_VAL; + break; + } + + RCStatus = cCmdMessageWrite(QueueID, pData, Count); + + //ERR_MEM here means we must compact the dataspace and retry message write + if (RCStatus == ERR_MEM) + { + cCmdDSCompact(); + RCStatus = cCmdMessageWrite(QueueID, pData, Count); + } + } + break; + + case RC_RESET_POSITION: + { + i = pInBuf[1]; + + //Don't do anything if illegal port specification is made + if (i >= NO_OF_OUTPUTS) + { + RCStatus = ERR_RC_ILLEGAL_VAL; + break; + } + + //pInBuf[2] is a selector + //FALSE: Position relative to start of last program + //TRUE: Position relative to start of last motor control block + if (pInBuf[2] == FALSE) + { + pMapOutPut->Outputs[i].Flags |= UPDATE_RESET_ROTATION_COUNT; + } + else + { + pMapOutPut->Outputs[i].Flags |= UPDATE_RESET_BLOCK_COUNT; + } + } + break; + + case RC_GET_BATT_LVL: + { + if (SendResponse == TRUE) + { + //Return BatteryVoltage directly from IOMapUI, in mV + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&(pMapUi->BatteryVoltage), 2); + ResponseLen += 2; + } + } + break; + + case RC_STOP_SOUND: + { + //Tell sound module to stop playback, no questions asked + pMapSound->State = SOUND_STOP; + } + break; + + case RC_KEEP_ALIVE: + { + pMapUi->Flags |= UI_RESET_SLEEP_TIMER; + + if (SendResponse == TRUE) + { + //Convert to milliseconds to match external conventions + i = (pMapUi->SleepTimeout * 60 * 1000); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&i, 4); + ResponseLen += 4; + } + } + break; + + case RC_LS_GET_STATUS: + { + if (SendResponse == TRUE) + { + i = pInBuf[1]; + + //Don't do anything if illegal port specification is made + if (i >= NO_OF_LOWSPEED_COM_CHANNEL) + { + RCStatus = ERR_RC_ILLEGAL_VAL; + break; + } + + RCStatus = cCmdLSCheckStatus(i); + + pOutBuf[ResponseLen] = cCmdLSCalcBytesReady(i); + ResponseLen++; + } + } + break; + + case RC_LS_WRITE: + { + i = pInBuf[1]; + Count = pInBuf[2]; + + //Don't do anything if illegal port specification is made + if (i >= NO_OF_LOWSPEED_COM_CHANNEL) + { + RCStatus = ERR_RC_ILLEGAL_VAL; + break; + } + + RCStatus = cCmdLSWrite(i, Count, &(pInBuf[4]), pInBuf[3]); + } + break; + + case RC_LS_READ: + { + if (SendResponse == TRUE) + { + i = pInBuf[1]; + + //Don't do anything if illegal port specification is made + if (i >= NO_OF_LOWSPEED_COM_CHANNEL) + { + RCStatus = ERR_RC_ILLEGAL_VAL; + break; + } + + //Get channel status and number of bytes available to read + RCStatus = cCmdLSCheckStatus(i); + Count = cCmdLSCalcBytesReady(i); + + pOutBuf[ResponseLen] = (UBYTE)Count; + ResponseLen++; + + //If channel is ready and has data ready for us, put the data into outgoing buffer + if (!IS_ERR(RCStatus) && Count > 0) + { + RCStatus = cCmdLSRead(i, (UBYTE)Count, &(pOutBuf[ResponseLen])); + ResponseLen += Count; + } + + //Pad remaining data bytes with zeroes + Count = 16 - Count; + memset(&(pOutBuf[ResponseLen]), 0, Count); + ResponseLen += Count; + } + } + break; + + case RC_GET_CURR_PROGRAM: + { + if (SendResponse == TRUE) + { + //If there's no active program, return error and empty name buffer + if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE) + { + RCStatus = ERR_NO_PROG; + memset(&(pOutBuf[ResponseLen]), 0, FILENAME_LENGTH + 1); + } + //Else, copy out stashed program name + else + { + strncpy((PSZ)(&(pOutBuf[ResponseLen])), (PSZ)(VarsCmd.ActiveProgName), FILENAME_LENGTH + 1); + } + + //Regardless, we've copied out a filename's worth of bytes... + ResponseLen += FILENAME_LENGTH + 1; + } + } + break; + + case RC_MESSAGE_READ: + { + if (SendResponse == TRUE) + { + QueueID = pInBuf[1]; + + //Fill in response with remote mailbox number so remote device knows where to store this message. + pOutBuf[ResponseLen] = pInBuf[2]; + ResponseLen++; + + RCStatus = cCmdMessageGetSize(QueueID, &Count); + pOutBuf[ResponseLen] = Count; + ResponseLen++; + + if (!IS_ERR(RCStatus) && Count > 0) + { + pData = &(pOutBuf[ResponseLen]); + RCStatus = cCmdMessageRead(QueueID, pData, Count, (pInBuf[3])); + //If cCmdMessageRead encountered an error, there is no real data in the buffer, so clear it out (below) + if (IS_ERR(RCStatus)) + Count = 0; + else + ResponseLen += Count; + } + + //Pad remaining data bytes with zeroes + Count = MAX_MESSAGE_SIZE - Count; + memset(&(pOutBuf[ResponseLen]), 0, Count); + ResponseLen += Count; + } + } + break; + + default: + { + //Unknown remote command -- still inform client to not expect any response bytes + NXT_BREAK; + RCStatus = ERR_RC_UNKNOWN_CMD; + } + break; + }; + } + //Handle reply telegrams + else + { + switch(pInBuf[0]) + { + case RC_MESSAGE_READ: + { + QueueID = pInBuf[2]; + Count = pInBuf[3]; + pData = &(pInBuf[4]); + + //This is a response to our request to read a message from a remote mailbox. + //If telegram looks valid, write the resulting message into our local mailbox. + //(If MsgData is not null-terminated, we can't accept it as a valid string.) + if (!IS_ERR((SBYTE)(pInBuf[1])) + && Count > 0 + && Count <= MAX_MESSAGE_SIZE + && pData[Count - 1] == 0x00) + { + RCStatus = cCmdMessageWrite(QueueID, pData, Count); + + //ERR_MEM here means we must compact the dataspace + if (RCStatus == ERR_MEM) + { + cCmdDSCompact(); + RCStatus = cCmdMessageWrite(QueueID, pData, Count); + } + } + + //If telegram doesn't check out, do nothing. No errors are ever returned for reply telegrams. + } + break; + + default: + { + //Unhandled reply telegram. Do nothing. + //!!! Could/should stash unhandled/all replies somewhere so a syscall could read them + } + break; + }; + } + + if (SendResponse == TRUE) + { + //Return response length (pointer checked above) + *pLen = (UBYTE)ResponseLen; + //Fill in status byte + pOutBuf[0] = (UBYTE)(RCStatus); + } + else + *pLen = 0; + + return (0); +} + + +// +// Standard interface functions +// + +void cCmdInit(void* pHeader) +{ + ULONG i; + + pHeaders = pHeader; + + IOMapCmd.pRCHandler = &cCmdHandleRemoteCommands; + +#if defined(ARM_DEBUG) + //Init run-time assert tracking variables + VarsCmd.AssertFlag = FALSE; + VarsCmd.AssertLine = 0; +#endif + + //Initialize IO_PTRS_OUT + for (i = 0; i < NO_OF_OUTPUTS; i++) + { + IO_PTRS_OUT[IO_OUT_FLAGS + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Flags); + IO_PTRS_OUT[IO_OUT_MODE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Mode); + IO_PTRS_OUT[IO_OUT_SPEED + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Speed); + IO_PTRS_OUT[IO_OUT_ACTUAL_SPEED + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].ActualSpeed); + IO_PTRS_OUT[IO_OUT_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].TachoCnt); + IO_PTRS_OUT[IO_OUT_TACH_LIMIT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].TachoLimit); + IO_PTRS_OUT[IO_OUT_RUN_STATE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RunState); + IO_PTRS_OUT[IO_OUT_TURN_RATIO + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].SyncTurnParameter); + IO_PTRS_OUT[IO_OUT_REG_MODE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegMode); + IO_PTRS_OUT[IO_OUT_OVERLOAD + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Overloaded); + IO_PTRS_OUT[IO_OUT_REG_P_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegPParameter); + IO_PTRS_OUT[IO_OUT_REG_I_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegIParameter); + IO_PTRS_OUT[IO_OUT_REG_D_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegDParameter); + IO_PTRS_OUT[IO_OUT_BLOCK_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].BlockTachoCount); + IO_PTRS_OUT[IO_OUT_ROTATION_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RotationCount); + } + + //Initialize IO_PTRS_IN + for (i = 0; i < NO_OF_INPUTS; i++) + { + IO_PTRS_IN[IO_IN_TYPE + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorType); + IO_PTRS_IN[IO_IN_MODE + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorMode); + IO_PTRS_IN[IO_IN_ADRAW + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].ADRaw); + IO_PTRS_IN[IO_IN_NORMRAW + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorRaw); + IO_PTRS_IN[IO_IN_SCALEDVAL + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorValue); + IO_PTRS_IN[IO_IN_INVALID_DATA + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].InvalidData); + } + + //Clear memory pool and initialize VarsCmd (cCmdDeactivateProgram effectively re-inits VarsCmd) + cCmdInitPool(); + cCmdDeactivateProgram(); + + //Global state variables for BlueTooth communication. + VarsCmd.CommStat = (SWORD)SUCCESS; + VarsCmd.CommStatReset = (SWORD)BTBUSY; + VarsCmd.CommCurrConnection = 1; + + //Global flags for various reset and bookkeeping scenarios + VarsCmd.DirtyComm = FALSE; + VarsCmd.DirtyDisplay = FALSE; + + VarsCmd.VMState = VM_IDLE; + +#if defined (ARM_NXT) + //Make sure Pool is long-aligned + NXT_ASSERT(!((ULONG)(POOL_START) % SIZE_SLONG)); +#endif + + IOMapCmd.ProgStatus = PROG_IDLE; + IOMapCmd.ActivateFlag = FALSE; + IOMapCmd.Awake = TRUE; + + //Default offsets explicitly chosen to cause an error if used with IOMAPREAD/IOMAPWRITE + //Real values will be set when programs run and/or the DS is re-arranged. + IOMapCmd.OffsetDVA = 0xFFFF; + IOMapCmd.OffsetDS = 0xFFFF; + + //Initialize format string and clear out FileName string + strncpy((PSZ)(IOMapCmd.FormatString), VM_FORMAT_STRING, VM_FORMAT_STRING_SIZE); + memset(IOMapCmd.FileName, 0, sizeof(IOMapCmd.FileName)); + + dTimerInit(); + IOMapCmd.Tick = dTimerRead(); + + return; +} + + +void cCmdCtrl(void) +{ + UBYTE Continue = TRUE; + NXT_STATUS Status = NO_ERR; + ULONG i; + CLUMP_ID CurrClumpID; + + switch (VarsCmd.VMState) + { + case VM_IDLE: + { + //If there's a new program to activate... + if (IOMapCmd.ActivateFlag == TRUE) + { + //Clear flag so we only activate once per new file + IOMapCmd.ActivateFlag = FALSE; + + Status = cCmdActivateProgram(IOMapCmd.FileName); + + //If we hit an activation error: + //1. Set PROG_ERROR status + //2. Proceed to VM_RESET1 (some unneeded work, yes, but preserves contract with UI + if (IS_ERR(Status)) + { + IOMapCmd.ProgStatus = PROG_ERROR; + VarsCmd.VMState = VM_RESET1; + } + //Else start running program + else + { + VarsCmd.VMState = VM_RUN_FREE; + IOMapCmd.ProgStatus = PROG_RUNNING; + VarsCmd.StartTick = IOMapCmd.Tick; + +#if VM_BENCHMARK + //Re-init benchmark + VarsCmd.InstrCount = 0; + VarsCmd.Average = 0; + VarsCmd.OverTimeCount = 0; + VarsCmd.MaxOverTimeLength = 0; + VarsCmd.CmdCtrlCount = 0; + VarsCmd.CompactionCount = 0; + VarsCmd.LastCompactionTick = 0; + VarsCmd.MaxCompactionTime = 0; + memset(VarsCmd.OpcodeBenchmarks, 0, sizeof(VarsCmd.OpcodeBenchmarks)); + memset(VarsCmd.SyscallBenchmarks, 0, sizeof(VarsCmd.SyscallBenchmarks)); +#endif + //Reset devices to a known state before we begin running + cCmdResetDevices(); + + pMapUi->Flags |= (UI_DISABLE_LEFT_RIGHT_ENTER | UI_DISABLE_EXIT); + } + } + + break; + } + + //Initialize VM internal state data and devices which must respond immediately to program ending + case VM_RESET1: + { + //If we aborted a program, reset devices (specifically, motors) immediately + //Otherwise, wait for UI to put us into PROG_RESET (gives motors a chance to brake before setting to coast) + //!!! This means cCmdResetDevices will get called twice on abort. Should not be a big deal. + if (IOMapCmd.ProgStatus == PROG_ABORT) + cCmdResetDevices(); + + //Reenable UI access to buttons + pMapUi->Flags &= ~(UI_DISABLE_LEFT_RIGHT_ENTER | UI_DISABLE_EXIT); + +#if VM_BENCHMARK + if (IOMapCmd.Tick != VarsCmd.StartTick) + VarsCmd.Average = VarsCmd.InstrCount / (IOMapCmd.Tick - VarsCmd.StartTick); + else + //It appears that we finished in 0 milliseconds. Very unlikely on ARM, so set a flag value. + VarsCmd.Average = 0xFFFFFFFF; + + cCmdWriteBenchmarkFile(); +#endif + + //Re-initialize program state data (contents of memory pool preserved) + //!!! Skip this step in simulator builds so helper access methods still work +#ifndef SIM_NXT + cCmdDeactivateProgram(); +#endif //SIM_NXT + + //If this program has taken over the display, reset it for the UI + cCmdRestoreDefaultScreen(); + + //Stop any currently playing sound and re-init volume according to UI prefs + pMapSound->State = SOUND_STOP; + pMapSound->Volume = pMapUi->Volume; + + //Artificially set CommStatReset to BTBUSY to force at least one SETCMDMODE call (see VM_RESET2 case) + VarsCmd.CommStatReset = (SWORD)BTBUSY; + + VarsCmd.VMState = VM_RESET2; + } + break; + + case VM_RESET2: + { + //Reset BlueCore into "command mode" (close any open streams) + //Since SETCMDMODE subject to BTBUSY, we may need to make multiple calls + //Any CommStatReset value other than BTBUSY means our request was accepted + //Assumptions: + //Process should never take longer than UI timeout (see below), but if it does, + // we could be left with the stream open to an NXT peer and block out the PC. + //Also assuming that once SETCMDMODE request is accepted, it never fails. + if (VarsCmd.CommStatReset == (SWORD)BTBUSY && VarsCmd.DirtyComm == TRUE) + pMapComm->pFunc(SETCMDMODE, 0, 0, 0, NULL, (UWORD*)&(VarsCmd.CommStatReset)); + + //If UI is done displaying ending program status, move on. + if (IOMapCmd.ProgStatus == PROG_RESET) + { + //Reset devices whenever a program ends for any reason + cCmdResetDevices(); + + VarsCmd.DirtyComm = FALSE; + + //Go to VM_IDLE state + VarsCmd.VMState = VM_IDLE; + IOMapCmd.ProgStatus = PROG_IDLE; + } + break; + } + + case VM_RUN_FREE: + case VM_RUN_SINGLE: + { + +#if VM_BENCHMARK + //IOMapCmd.Tick currently holds the tick from the end of last cCmdCtrl call. + //If we don't come back here before dTimerRead() increments, the m_sched loop has taken *at least* 1 ms. + if (IOMapCmd.Tick != dTimerRead()) + { + VarsCmd.OverTimeCount++; + //Record maximum magnitude of schedule loop overage, in millisecs + if (dTimerRead() - IOMapCmd.Tick > VarsCmd.MaxOverTimeLength) + VarsCmd.MaxOverTimeLength = dTimerRead() - IOMapCmd.Tick; + } + VarsCmd.CmdCtrlCount++; +#endif + //Abort current program if cancel button is pressed + if (IOMapCmd.DeactivateFlag == TRUE || pMapButton->State[BTN1] & PRESSED_EV) + { + IOMapCmd.DeactivateFlag = FALSE; + + //Clear pressed event so it doesn't get double-counted by UI + pMapButton->State[BTN1] &= ~PRESSED_EV; + + //Go to VM_RESET1 state and report abort + VarsCmd.VMState = VM_RESET1; + IOMapCmd.ProgStatus = PROG_ABORT; + break; + } + + //Assert that we have an active program + NXT_ASSERT(VarsCmd.ActiveProgHandle != NOT_A_HANDLE); + + //Execute from at least one clump + do + { + if (cCmdIsClumpIDSane(VarsCmd.RunQ.Head)) + { + //Stash and dequeue RunQ's head clump + CurrClumpID = VarsCmd.RunQ.Head; + + //Execute at least one instruction from current clump + //Execute up to 'Priority' instructions as long as we are in VM_FREE_RUN mode + //Finishing/suspending a clump, BREAKOUT_REQ, or any errors will also end this loop + i = 0; + do + { + //Interpret one instruction per call, advancing PC as needed + Status = cCmdInterpFromClump(CurrClumpID); + +#if VM_BENCHMARK + VarsCmd.InstrCount++; +#endif + + NXT_ASSERT(!IS_ERR(Status)); + if (IS_ERR(Status) || Status == CLUMP_DONE || Status == CLUMP_SUSPEND || Status == BREAKOUT_REQ || Status == STOP_REQ) + { + //We're done with this clump or breaking out prematurely, + //so break the multi-instruction loop + break; + } + else + { + //Count up one more instruction for this pass + i++; + } + } while (VarsCmd.VMState == VM_RUN_FREE && i < VarsCmd.pAllClumps[CurrClumpID].Priority); + + //Only rotate RunQ on a "normal" finish, i.e. no error, clump end, or breakout request + if (!(IS_ERR(Status) || Status == CLUMP_DONE || Status == CLUMP_SUSPEND || Status == BREAKOUT_REQ)) + cCmdRotateQ(&(VarsCmd.RunQ)); + } + + //Re-evaluate conditions for stopping the dataflow scheduler + //Halt program on all errors + if (IS_ERR(Status)) + { + Continue = FALSE; + + VarsCmd.VMState = VM_RESET1; + IOMapCmd.ProgStatus = PROG_ERROR; + } + else if (Status == BREAKOUT_REQ) + { + Continue = FALSE; + } + //If RunQ is empty or user requested early termination, program is done + else if (!cCmdIsClumpIDSane(VarsCmd.RunQ.Head) || Status == STOP_REQ) + { + Continue = FALSE; + + VarsCmd.VMState = VM_RESET1; + IOMapCmd.ProgStatus = PROG_OK; + } + //VM_RUN_FREE means continue executing until a new ms tick rolls over + else if (VarsCmd.VMState == VM_RUN_FREE) + { + Continue = (IOMapCmd.Tick == dTimerRead()); + } + //Otherwise execute only one pass per call + else //VarsCmd.VMState == VM_RUN_SINGLE + { + VarsCmd.VMState = VM_RUN_PAUSE; + Continue = FALSE; + } + + } while (Continue == TRUE); + + break; + } + }//END state machine switch + + //Busy wait to always maintain 1ms period + BUSY_WAIT_NEXT_MS; + + //Set tick to new value for next time 'round + IOMapCmd.Tick = dTimerRead(); + + return; +} + + +void cCmdExit(void) +{ + dTimerExit(); + + return; +} + + +NXT_STATUS cCmdReadFileHeader(UBYTE* pData, ULONG DataSize, + PROG_FILE_OFFSETS* pFileOffsets) +{ + ULONG i; + UBYTE * pCursor; + UWORD CurrOffset = 0; + UBYTE DepCount; + UWORD DopeVectorOffset; + UWORD FileClumpCount; + UBYTE FileMajor, FileMinor, + CompatibleMinor, CompatibleMajor, + CurrentMajor; + + NXT_ASSERT(pData != NULL); + + //Assign pCursor to point to version word inside file header + pCursor = (pData + VM_FORMAT_STRING_SIZE - 2); + + //Decode version numbers into comparable bytes + FileMajor = *pCursor; + FileMinor = *(pCursor + 1); + CompatibleMajor = (UBYTE)(VM_OLDEST_COMPATIBLE_VERSION >> 8); + CompatibleMinor = (UBYTE)(VM_OLDEST_COMPATIBLE_VERSION); + CurrentMajor = (UBYTE)(FIRMWAREVERSION >> 8); + //CurrentMinor = (UBYTE)(FIRMWAREVERSION); + + //Return ERR_VER if file lacks proper format string or version number + //!!! Only checking major version recommended for future development + if (strncmp((PSZ)pData, VM_FORMAT_STRING, VM_FORMAT_STRING_SIZE) + || FileMajor < CompatibleMajor || FileMinor < CompatibleMinor + || FileMajor > CurrentMajor) + { + NXT_BREAK; + return (ERR_VER); + } + + //Advance CurrOffset past header information + CurrOffset += VM_FORMAT_STRING_SIZE; + + // + //Initialize bookkeeping variables + // + VarsCmd.DataspaceCount = *((UWORD*)(pData + CurrOffset)); + CurrOffset += 2; + + VarsCmd.DataspaceSize = *((UWORD*)(pData + CurrOffset)); + CurrOffset += 2; + + VarsCmd.DSStaticSize = *((UWORD*)(pData + CurrOffset)); + CurrOffset += 2; + + pFileOffsets->DSDefaultsSize = *((UWORD*)(pData + CurrOffset)); + CurrOffset += 2; + + pFileOffsets->DynamicDefaults = *((UWORD*)(pData + CurrOffset)); + CurrOffset += 2; + + pFileOffsets->DynamicDefaultsSize = *((UWORD*)(pData + CurrOffset)); + CurrOffset += 2; + + VarsCmd.MemMgr.Head = *((UWORD*)(pData + CurrOffset)); + CurrOffset += 2; + + VarsCmd.MemMgr.Tail = *((UWORD*)(pData + CurrOffset)); + CurrOffset += 2; + + DopeVectorOffset = *((UWORD*)(pData + CurrOffset)); + CurrOffset += 2; + + //!!! Odd code here to deal with type mismatch between file format and CLUMP_ID typedef. + //Neither is trivial to change, so it's best to just check the data for consistency here. + FileClumpCount = *((UWORD*)(pData + CurrOffset)); + CurrOffset += 2; + + //Must have at least one clump and count can't exceed the NOT_A_CLUMP sentinel + if (FileClumpCount == 0 || FileClumpCount >= NOT_A_CLUMP) + return (ERR_FILE); + else + VarsCmd.AllClumpsCount = (CLUMP_ID)FileClumpCount; + + VarsCmd.CodespaceCount = *((UWORD*)(pData + CurrOffset)); + CurrOffset += 2; + + //Can't have a valid program with no code + if (VarsCmd.CodespaceCount == 0) + return (ERR_FILE); + + // + // Now, calculate offsets for each data segment in the file + // + + CurrOffset += CurrOffset % 2; + pFileOffsets->DSTOC = CurrOffset; + CurrOffset += VarsCmd.DataspaceCount * sizeof(DS_TOC_ENTRY); + + CurrOffset += CurrOffset % 2; + pFileOffsets->DSDefaults = CurrOffset; + CurrOffset += pFileOffsets->DSDefaultsSize; + + //ClumpRecs must be aligned on even boundaries + CurrOffset += CurrOffset % 2; + pFileOffsets->Clumps = CurrOffset; + + //Set cursor to start of clump records + pCursor = pData + CurrOffset; + + //Set CurrOffset to start of dependent lists + CurrOffset += VarsCmd.AllClumpsCount * VM_FILE_CLUMP_REC_SIZE; + + //Read dependent count from each clump record, advancing CurrOffset accordingly + for (i = 0; i < VarsCmd.AllClumpsCount; i++) + { + DepCount = *(pCursor + 1); + CurrOffset += DepCount; + pCursor += VM_FILE_CLUMP_REC_SIZE; + } + + //Codespace must be aligned on even boundary + CurrOffset += CurrOffset % 2; + pFileOffsets->Codespace = CurrOffset; + + //No need to read through codespace, but make sure CurrOffset ended up sane + //If not, something went wrong reading the header information + if (CurrOffset != (DataSize - VarsCmd.CodespaceCount * 2)) + { + NXT_BREAK; + return (ERR_FILE); + } + + // + // Finally, update VarsCmd fields + // + + VarsCmd.RunQ.Head = NOT_A_CLUMP; + VarsCmd.RunQ.Tail = NOT_A_CLUMP; + + //Reset codespace pointer + VarsCmd.pCodespace = (CODE_WORD*)(pData + pFileOffsets->Codespace); + + //...placing clump records first... + VarsCmd.pAllClumps = (CLUMP_REC*)(VarsCmd.Pool + VarsCmd.PoolSize); + VarsCmd.PoolSize += VarsCmd.AllClumpsCount * sizeof(CLUMP_REC); + + //...then DSTOC... + VarsCmd.pDataspaceTOC = (DS_TOC_ENTRY*)(pData + pFileOffsets->DSTOC); + + //...then the dataspace itself + ALIGN_TO_MOD(VarsCmd.PoolSize, POOL_ALIGN); + VarsCmd.pDataspace = (VarsCmd.Pool + VarsCmd.PoolSize); + IOMapCmd.OffsetDS = (UWORD)((ULONG)(VarsCmd.pDataspace) - (ULONG)&(IOMapCmd)); + VarsCmd.PoolSize += VarsCmd.DataspaceSize; + + //init rest of MemMgr + VarsCmd.MemMgr.pDopeVectorArray = (DOPE_VECTOR *)(VarsCmd.pDataspace + DopeVectorOffset); + IOMapCmd.OffsetDVA = (UWORD)((ULONG)(VarsCmd.MemMgr.pDopeVectorArray) - (ULONG)&(IOMapCmd)); + VarsCmd.MemMgr.FreeHead = NOT_A_DS_ID; + + + if (VarsCmd.PoolSize > POOL_MAX_SIZE) + { + NXT_BREAK; + return (ERR_FILE); + } + + return (NO_ERR); +} + + +//!!! Recursive function +NXT_STATUS cCmdInflateDSDefaults(UBYTE* pDSDefaults, UWORD *pDefaultsOffset, DS_ELEMENT_ID DSElementID) +{ + NXT_STATUS Status = NO_ERR; + TYPE_CODE TypeCode; + UWORD i, Count; + UBYTE *pVal; + + NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID)); + + TypeCode = cCmdDSType(DSElementID); + + if (TypeCode == TC_CLUSTER) + { + Count = cCmdClusterCount(DSElementID); + //Advance DSElementID to sub-type + DSElementID = INC_ID(DSElementID); + //Loop through sub-types, inflate recursively + for (i = 0; i < Count; i++) + { + Status = cCmdInflateDSDefaults(pDSDefaults, pDefaultsOffset, DSElementID); + if (IS_ERR(Status)) + return Status; + DSElementID = cCmdNextDSElement(DSElementID); + } + } + else + { + if (TypeCode == TC_ARRAY) + { + //Resolve pointer to DVIndex + pVal = VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset; + } + else + { + pVal = cCmdResolveDataArg(DSElementID, 0, NULL); + } + + //Check if the element has the "default default" + if (VarsCmd.pDataspaceTOC[DSElementID].Flags & DS_DEFAULT_DEFAULT) + { + //Fill element with the "default default" of zero + memset(pVal, 0, cCmdSizeOf(TypeCode)); + } + else + { + //Get default from stream + memmove(pVal, pDSDefaults + *pDefaultsOffset, cCmdSizeOf(TypeCode)); + *pDefaultsOffset += cCmdSizeOf(TypeCode); + } + } + + //!!! Currently will always return NO_ERR + return Status; +} + + +NXT_STATUS cCmdActivateProgram(UBYTE * pFileName) +{ + UWORD i, j; + UBYTE * pCursor; + + NXT_STATUS Status = NO_ERR; + PROG_FILE_OFFSETS FileOffsets; + + LOADER_STATUS LStatus; + ULONG DataSize; + UBYTE * pData; + ULONG pDataHolder; + UWORD DefaultsOffset; + + LStatus = pMapLoader->pFunc(OPENREADLINEAR, pFileName, (UBYTE*)(&pDataHolder), &DataSize); + pData = (UBYTE*)(pDataHolder); + + //If Loader returned error or bad file pointer, bail out + if (LOADER_ERR(LStatus) != SUCCESS || pData == NULL || DataSize == 0) + return (ERR_FILE); + + //Deactivate current program and re-initialize memory pool + cCmdDeactivateProgram(); + cCmdInitPool(); + + //Stash this program's handle since we hold it open while running + VarsCmd.ActiveProgHandle = LOADER_HANDLE(LStatus); + + //Stash this program's name for easy reference later + strncpy((PSZ)(VarsCmd.ActiveProgName), (PSZ)(pFileName), FILENAME_LENGTH + 1); + + //Consume activation record data stream. + //See TargettingVIs/NXT.PackAR.vi for data stream packing details + + //Read header portion of the file, calculating offsets and initializing VarsCmd + Status = cCmdReadFileHeader(pData, DataSize, &FileOffsets); + if (IS_ERR(Status)) + return Status; + + //Do some spot checks to make sure bad file contents didn't leave us with obviously insane VarsCmd contents + //!!! Should add alignment checks on these pointers to avoid data abort exceptions later + if (((UBYTE*)(VarsCmd.pCodespace) < pData) + || ((UBYTE*)(VarsCmd.pCodespace) >= (pData + DataSize)) + || ((UBYTE*)(VarsCmd.pAllClumps) < POOL_START) + || ((UBYTE*)(VarsCmd.pAllClumps) >= POOL_SENTINEL) + || ((UBYTE*)(VarsCmd.pDataspace) < POOL_START) + || ((UBYTE*)(VarsCmd.pDataspace) >= POOL_SENTINEL) + || (VarsCmd.DataspaceSize == 0) ) + { + NXT_BREAK; + return ERR_FILE; + } + + //Initialize CLUMP_RECs as contiguous list in RAM + pCursor = (pData + FileOffsets.Clumps); + for (i = 0; i < VarsCmd.AllClumpsCount; i++) + { + VarsCmd.pAllClumps[i].InitFireCount = *(UBYTE*)(pCursor + i * VM_FILE_CLUMP_REC_SIZE); + VarsCmd.pAllClumps[i].DependentCount = *(UBYTE*)(pCursor + (i * VM_FILE_CLUMP_REC_SIZE) + 1); + VarsCmd.pAllClumps[i].CodeStart = *(UWORD*)(pCursor + (i * VM_FILE_CLUMP_REC_SIZE) + 2); + + //Initialize remaining CLUMP_REC fields + VarsCmd.pAllClumps[i].PC = 0; + VarsCmd.pAllClumps[i].Priority = 20; + VarsCmd.pAllClumps[i].Link = NOT_A_CLUMP; + + //Activate any clumps with CurrFireCount of 0 + VarsCmd.pAllClumps[i].CurrFireCount = VarsCmd.pAllClumps[i].InitFireCount; + if (VarsCmd.pAllClumps[i].CurrFireCount == 0) + cCmdEnQClump(&(VarsCmd.RunQ), (CLUMP_ID)i); + } + + //Patch up dependents in separate pass (reuse of pCursor) + pCursor += VarsCmd.AllClumpsCount * VM_FILE_CLUMP_REC_SIZE; + for (i = 0; i < VarsCmd.AllClumpsCount; i++) + { + if (VarsCmd.pAllClumps[i].DependentCount > 0) + { + VarsCmd.pAllClumps[i].pDependents = (CLUMP_ID*)(pCursor); + + pCursor += (VarsCmd.pAllClumps[i].DependentCount * sizeof(CLUMP_ID)); + } + else + VarsCmd.pAllClumps[i].pDependents = NULL; + + //Patch up CodeEnd value based on CodeStart of next clump or last overall codeword + if (i < (VarsCmd.AllClumpsCount - 1)) + VarsCmd.pAllClumps[i].CodeEnd = VarsCmd.pAllClumps[i+1].CodeStart - 1; + else + VarsCmd.pAllClumps[i].CodeEnd = VarsCmd.CodespaceCount - 1; + + //Test for empty/insane clump code definitions + NXT_ASSERT(VarsCmd.pAllClumps[i].CodeStart < VarsCmd.pAllClumps[i].CodeEnd); + } + + //Programs with no active clumps constitutes an activation error + if (VarsCmd.RunQ.Head == NOT_A_CLUMP) + return (ERR_FILE); + + //Initialize dataspace with default values from file + //!!! This would be a good place to enforce check against potentially + // unsafe nested types (deeply nested types mean deep recursive calls) + DefaultsOffset = 0; + for (i = 0; i != NOT_A_DS_ID; i = cCmdNextDSElement(i)) + { + + Status = cCmdInflateDSDefaults(pData + FileOffsets.DSDefaults, &DefaultsOffset, i); + if (IS_ERR(Status)) + return Status; + } + + if ((DefaultsOffset != FileOffsets.DynamicDefaults) + || (DefaultsOffset + FileOffsets.DynamicDefaultsSize != FileOffsets.DSDefaultsSize)) + { + NXT_BREAK; + return (ERR_FILE); + } + + //Copy Dynamic defaults from file + memmove(VarsCmd.pDataspace + VarsCmd.DSStaticSize, pData + FileOffsets.DSDefaults + FileOffsets.DynamicDefaults, FileOffsets.DynamicDefaultsSize); + + //Verify the MemMgr ended up where we said it would + if ((UBYTE *)VarsCmd.MemMgr.pDopeVectorArray != VarsCmd.pDataspace + DV_ARRAY[0].Offset) + { + NXT_BREAK; + return (ERR_FILE); + } + + //Initialize message queues + for (i = 0; i < MESSAGE_QUEUE_COUNT; i++) + { + VarsCmd.MessageQueues[i].ReadIndex = 0; + VarsCmd.MessageQueues[i].WriteIndex = 0; + + for (j = 0; j < MESSAGES_PER_QUEUE; j++) + { + VarsCmd.MessageQueues[i].Messages[j] = NOT_A_DS_ID; + } + } + + if (cCmdVerifyMemMgr() != TRUE) + return (ERR_FILE); + + return (Status); +} + + +void cCmdDeactivateProgram() +{ + UBYTE i, tmp; + + //Wipe away all references into the pool and clear all run-time data + VarsCmd.pCodespace = NULL; + VarsCmd.CodespaceCount = 0; + + VarsCmd.pAllClumps = NULL; + VarsCmd.AllClumpsCount = 0; + + VarsCmd.DataspaceCount = 0; + VarsCmd.pDataspaceTOC = NULL; + VarsCmd.pDataspace = NULL; + VarsCmd.DataspaceSize = 0; + VarsCmd.DSStaticSize = 0; + + VarsCmd.MemMgr.Head = NOT_A_DS_ID; + VarsCmd.MemMgr.Tail = NOT_A_DS_ID; + VarsCmd.MemMgr.FreeHead = NOT_A_DS_ID; + VarsCmd.MemMgr.pDopeVectorArray = NULL; + + VarsCmd.RunQ.Head = NOT_A_CLUMP; + VarsCmd.RunQ.Tail = NOT_A_CLUMP; + VarsCmd.ScratchPC = 0; + VarsCmd.CallerClump = NOT_A_CLUMP; + + if (VarsCmd.ActiveProgHandle != NOT_A_HANDLE) + { + //Close handle that we've kept open for this program + pMapLoader->pFunc(CLOSE, &(VarsCmd.ActiveProgHandle), NULL, NULL); + VarsCmd.ActiveProgHandle = NOT_A_HANDLE; + + //Clear internal stashed name + memset(VarsCmd.ActiveProgName, 0, FILENAME_LENGTH + 1); + } + + //Close any files we had opened programatically + for (i = 0; i < MAX_HANDLES; i++) + { + //Copy i to tmp, because we pass a pointer to it to pFunc + tmp = i; + //Close file + if (*(VarsCmd.FileHandleTable[i]) != 0) + pMapLoader->pFunc(CLOSE, &tmp, NULL, NULL); + } + + //Clear FileHandleTable + memset(VarsCmd.FileHandleTable, 0, sizeof(VarsCmd.FileHandleTable)); + + return; +} + + +void cCmdResetDevices(void) +{ + UBYTE i; + + //Clear NXT button counts so 'bumped' will work on first run + for (i = 0; i < NO_OF_BTNS; i++) + { + pMapButton->BtnCnt[i].RelCnt = 0; + //Need to clear short and long counts too, because RelCnt depends on them. No known side effects. + pMapButton->BtnCnt[i].ShortRelCnt = 0; + pMapButton->BtnCnt[i].LongRelCnt = 0; + } + + for (i = 0; i < NO_OF_INPUTS; i++) + { + //Clear type and mode to defaults + pMapInput->Inputs[i].SensorType = NO_SENSOR; + pMapInput->Inputs[i].SensorMode = RAWMODE; + + //Reset input values to 0 prior to running (clear things like stale rotation counts) + pMapInput->Inputs[i].ADRaw = 0; + pMapInput->Inputs[i].SensorRaw = 0; + pMapInput->Inputs[i].SensorValue = 0; + + //Assert invalid data flag so future code is aware of these changes + pMapInput->Inputs[i].InvalidData = TRUE; + } + + for (i = 0; i < NO_OF_OUTPUTS; i++) + { + //Coast and reset all motor parameters + pMapOutPut->Outputs[i].Mode = 0; + pMapOutPut->Outputs[i].RegMode = REGULATION_MODE_IDLE; + pMapOutPut->Outputs[i].RunState = MOTOR_RUN_STATE_IDLE; + pMapOutPut->Outputs[i].Speed = 0; + pMapOutPut->Outputs[i].TachoLimit = 0; + pMapOutPut->Outputs[i].SyncTurnParameter = 0; + pMapOutPut->Outputs[i].Flags = UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT | UPDATE_RESET_COUNT | UPDATE_RESET_BLOCK_COUNT | UPDATE_RESET_ROTATION_COUNT; + } + + //Lowspeed init, INSERT CODE !!! + for (i = 0; i < NO_OF_LOWSPEED_COM_CHANNEL; i++) + { + pMapLowSpeed->InBuf[i].InPtr = 0; + pMapLowSpeed->InBuf[i].OutPtr = 0; + pMapLowSpeed->InBuf[i].BytesToRx = 0; + pMapLowSpeed->OutBuf[i].InPtr = 0; + pMapLowSpeed->OutBuf[i].OutPtr = 0; + if (pMapLowSpeed->ChannelState[i] != LOWSPEED_IDLE) + { + pMapLowSpeed->ChannelState[i] = LOWSPEED_DONE; + pMapLowSpeed->State |= (0x01<Head == NOT_A_CLUMP) + { + NXT_ASSERT(Queue->Tail == NOT_A_CLUMP); + + Queue->Head = NewClump; + Queue->Tail = NewClump; + } + //Otherwise, tack onto the end + else + { + VarsCmd.pAllClumps[Queue->Tail].Link = NewClump; + Queue->Tail = NewClump; + } + + return; +} + +//Dequeue specified clump +//Normal usage is to dequeue only from the head (i.e. pass Queue.Head as arg) +void cCmdDeQClump(CLUMP_Q * Queue, CLUMP_ID Clump) +{ + CLUMP_ID CurrID, LinkID; + + //Make sure Clump's ID is valid and is already on Queue + NXT_ASSERT(cCmdIsClumpIDSane(Clump)); + NXT_ASSERT(cCmdIsQSane(Queue) == TRUE); + NXT_ASSERT(cCmdIsClumpOnQ(Queue, Clump)); + + CurrID = Queue->Head; + + //If our clump is the head, move up the next and disconnect + if (CurrID == Clump) + { + Queue->Head = VarsCmd.pAllClumps[Clump].Link; + VarsCmd.pAllClumps[Clump].Link = NOT_A_CLUMP; + + //If we just removed the last clump, patch up the queue's tail + if (Queue->Head == NOT_A_CLUMP) + Queue->Tail = NOT_A_CLUMP; + } + //Else, look through rest of list looking for a link to our clump + else + { + do + { + LinkID = VarsCmd.pAllClumps[CurrID].Link; + + //If we find a link to our clump, patch up predecessor's link + if (VarsCmd.pAllClumps[CurrID].Link == Clump) + { + VarsCmd.pAllClumps[CurrID].Link = VarsCmd.pAllClumps[Clump].Link; + VarsCmd.pAllClumps[Clump].Link = NOT_A_CLUMP; + + //If we just removed the tail, patch tail + if (Clump == Queue->Tail) + Queue->Tail = CurrID; + } + + CurrID = LinkID; + } while (CurrID != NOT_A_CLUMP); + } + + return; +} + + +//Rotate head to tail and advance head for given Queue +void cCmdRotateQ(CLUMP_Q * Queue) +{ + CLUMP_ID CurrID; + CLUMP_REC * pClumpRec; + + //Make sure Queue is sane + NXT_ASSERT(cCmdIsQSane(Queue) == TRUE); + + //If queue has at least two clumps + if (Queue->Head != Queue->Tail) + { + CurrID = Queue->Head; + pClumpRec = &(VarsCmd.pAllClumps[CurrID]); + + //Disconnect head + Queue->Head = pClumpRec->Link; + pClumpRec->Link = NOT_A_CLUMP; + + //Reconnect head as tail + pClumpRec = &(VarsCmd.pAllClumps[Queue->Tail]); + pClumpRec->Link = CurrID; + Queue->Tail = CurrID; + + //Make sure we didn't make any really stupid mistakes + NXT_ASSERT(cCmdIsQSane(Queue) == TRUE); + } + + return; +} + + +UBYTE cCmdIsClumpOnQ(CLUMP_Q * Queue, CLUMP_ID Clump) +{ + CLUMP_ID CurrID; + + //Make sure Clump's ID is valid and is already on Queue + NXT_ASSERT(cCmdIsClumpIDSane(Clump)); + NXT_ASSERT(cCmdIsQSane(Queue) == TRUE); + + CurrID = Queue->Head; + + while (CurrID != NOT_A_CLUMP) + { + if (CurrID == Clump) + return TRUE; + + CurrID = VarsCmd.pAllClumps[CurrID].Link; + } + + return FALSE; +} + + +UBYTE cCmdIsQSane(CLUMP_Q * Queue) +{ + CLUMP_ID Head, Tail; + CLUMP_REC * pHead; + + if (Queue == NULL) + { + NXT_BREAK; + return FALSE; + } + + Head = Queue->Head; + Tail = Queue->Tail; + + if (Head == NOT_A_CLUMP && cCmdIsClumpIDSane(Tail)) + return FALSE; + + if (cCmdIsClumpIDSane(Head) && Tail == NOT_A_CLUMP) + return FALSE; + + if (cCmdIsClumpIDSane(Head) && cCmdIsClumpIDSane(Tail)) + { + pHead = &(VarsCmd.pAllClumps[Head]); + + //!!! More comprehensive queue tests could go here + + //Check for mislinked head if there are at least two queue members + if (Head != Tail && pHead->Link == NOT_A_CLUMP) + return FALSE; + } + + return TRUE; +} + +// +// Mutex queuing functions +// + +NXT_STATUS cCmdAcquireMutex(MUTEX_Q * Mutex, CLUMP_ID Clump) +{ + NXT_STATUS Status = NO_ERR; + + NXT_ASSERT(Mutex != NULL && cCmdIsClumpIDSane(Clump)); + + if (Mutex->Owner == NOT_A_CLUMP) + { + //Mutex is open, so just take it + Mutex->Owner = Clump; + + NXT_ASSERT(Mutex->WaitQ.Head == NOT_A_CLUMP && Mutex->WaitQ.Tail == NOT_A_CLUMP); + } + else + { + //Mutex is reserved by someone else, take self off RunQ and add to WaitQ + cCmdDeQClump(&(VarsCmd.RunQ), Clump); + cCmdEnQClump(&(Mutex->WaitQ), Clump); + Status = CLUMP_SUSPEND; + } + + NXT_ASSERT(cCmdIsQSane(&(Mutex->WaitQ))); + + return (Status); +} + + +NXT_STATUS cCmdReleaseMutex(MUTEX_Q * Mutex, CLUMP_ID Clump) +{ + NXT_ASSERT(Mutex != NULL); + //!!! don't actually need to pass in Owner clump, but provides nice error checking for now + // Might want to return an error/warning if we see a Release on an free mutex, though... + NXT_ASSERT(Clump != NOT_A_CLUMP && Mutex->Owner == Clump); + + //Always set new Owner to WaitQ's Head, since NOT_A_CLUMP means mutex is free + Mutex->Owner = Mutex->WaitQ.Head; + + if (Mutex->Owner != NOT_A_CLUMP) + { + cCmdDeQClump(&(Mutex->WaitQ), Mutex->Owner); + cCmdEnQClump(&(VarsCmd.RunQ), Mutex->Owner); + } + + NXT_ASSERT(cCmdIsQSane(&(Mutex->WaitQ))); + NXT_ASSERT(cCmdIsQSane(&(VarsCmd.RunQ))); + + return (NO_ERR); +} + + +NXT_STATUS cCmdSchedDependents(CLUMP_ID Clump, SWORD Begin, SWORD End) +{ + CLUMP_ID CurrDepClumpID; + SWORD i; + + //Begin and End specify range of CLUMP_IDs in dependent list to schedule + //If either equals -1, both should equal -1, and no dependents will be scheduled + //Else schedule specified subset offset from pDependents + + //Check for valid args + NXT_ASSERT(cCmdIsClumpIDSane(Clump)); + NXT_ASSERT((Begin >= 0 && End >= 0 && End < VarsCmd.pAllClumps[Clump].DependentCount) + || (Begin == -1 && End == -1)); + + //If non-empty range + if (Begin != -1 || End != -1) + { + //update dependents, scheduling if their CurrFireCount reaches 0 + for (i = Begin; i <= End; i++) + { + CurrDepClumpID = VarsCmd.pAllClumps[Clump].pDependents[i]; + + NXT_ASSERT(cCmdIsClumpIDSane(CurrDepClumpID)); + + VarsCmd.pAllClumps[CurrDepClumpID].CurrFireCount--; + + if (VarsCmd.pAllClumps[CurrDepClumpID].CurrFireCount == 0) + cCmdEnQClump(&(VarsCmd.RunQ), CurrDepClumpID); + } + } + + return (NO_ERR); +} + + +NXT_STATUS cCmdSchedDependent(CLUMP_ID Clump, CLUMP_ID TargetClump) +{ + //TargetClump specifies the clump number of the target to schedule explicitly. + + //Check for valid args + NXT_ASSERT(cCmdIsClumpIDSane(Clump)); + NXT_ASSERT(cCmdIsClumpIDSane(TargetClump)); + + VarsCmd.pAllClumps[TargetClump].CurrFireCount--; + if (VarsCmd.pAllClumps[TargetClump].CurrFireCount == 0) + cCmdEnQClump(&(VarsCmd.RunQ), TargetClump); + + return (NO_ERR); +} + + +UBYTE cCmdIsClumpIDSane(CLUMP_ID Clump) +{ + if (Clump < VarsCmd.AllClumpsCount) + return TRUE; + else + return FALSE; +} + + +// +// Memory pool management functions +// +void cCmdInitPool(void) +{ + ULONG i; + + //VarsCmd.Pool is a UBYTE pointer to ULONG array + //This was done to enforce portable alignment. + VarsCmd.Pool = (UBYTE*)(IOMapCmd.MemoryPool); + + for (i = 0; i < (POOL_MAX_SIZE / 4); i++) + ((SLONG*)(POOL_START))[i] = 0xDEADBEEF; + + VarsCmd.PoolSize = 0; +} + + +NXT_STATUS cCmdDSArrayAlloc(DS_ELEMENT_ID DSElementID, UWORD Offset, UWORD NewCount) +{ + NXT_STATUS Status = NO_ERR; + UWORD DVIndex; + UWORD OldCount; + UWORD i; + + NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID)); + + //Only arrays are valid here + //!!! Recommended to upgrade NXT_ASSERT to ERR_INSTR return + NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY); + + DVIndex = cCmdGetDVIndex(DSElementID, Offset); + OldCount = DV_ARRAY[DVIndex].Count; + + Status = cCmdDVArrayAlloc(DVIndex, NewCount); + if (Status < NO_ERR) + return Status; + + if (OldCount > NewCount) + { + //Free dope vectors for sub-arrays. + for (i = NewCount; i < OldCount; i++) + { + Status = cCmdFreeSubArrayDopeVectors(INC_ID(DSElementID), ARRAY_ELEM_OFFSET(DVIndex, i)); + if (IS_ERR(Status)) + return Status; + } + } + else if (OldCount < NewCount) + { + //Alloc dope vectors for sub-arrays. Set up DVIndexes + for (i = OldCount; i < NewCount; i++) + { + Status = cCmdAllocSubArrayDopeVectors(INC_ID(DSElementID), ARRAY_ELEM_OFFSET(DVIndex, i)); + if (IS_ERR(Status)) + return Status; + } + } + + NXT_ASSERT(cCmdVerifyMemMgr()); + + return Status; +} + +NXT_STATUS cCmdDVArrayAlloc(DV_INDEX DVIndex, UWORD NewCount) +{ + NXT_STATUS Status = NO_ERR; + UBYTE *pData; + UWORD ArraySize, InplaceSize; + UWORD NextDVIndex; + UWORD OldCount; + + OldCount = DV_ARRAY[DVIndex].Count; + + if (OldCount == NewCount) + { + //Nothing to alloc. Return. + return Status; + } + else if (OldCount > NewCount) + { + //Already have the space. Shrink inplace. + DV_ARRAY[DVIndex].Count = NewCount; + return Status; + } + else // need to grow array + { + //Calculate new array size + ArraySize = NewCount * DV_ARRAY[DVIndex].ElemSize; + + //Try growing inplace + // If the Offset == NOT_AN_OFFSET then the array has never been allocated and can't grow inplace. + if (DV_ARRAY[DVIndex].Offset != NOT_AN_OFFSET) + { + //Get pointer to next dope vector in dataspace + if (DV_ARRAY[DVIndex].Link != NOT_A_DS_ID) + { + NextDVIndex = DV_ARRAY[DVIndex].Link; + InplaceSize = DV_ARRAY[NextDVIndex].Offset - DV_ARRAY[DVIndex].Offset; + } + else + { + //Last element in dataspace. + NXT_ASSERT(DVIndex == VarsCmd.MemMgr.Tail); + InplaceSize = VarsCmd.DataspaceSize - DV_ARRAY[DVIndex].Offset; + } + + if (ArraySize <= InplaceSize) + { + DV_ARRAY[DVIndex].Count = NewCount; + return Status; + } + } + + //Can't grow inplace, have to allocate new space + + //Make sure we properly align for type + //!!! This could also overflow memory (make PoolSize > POOL_MAX_SIZE) if we're within 3 bytes of the end. + // I don't think it matters because if it does happend, we'll trigger the ERR_MEM below and compact. + // During compaction, we'll reclaim these unused bytes. + //!!! Aligning beginning of ALL arrays to 4 byte address + ALIGN_TO_MOD(VarsCmd.PoolSize, SIZE_ULONG); + ALIGN_TO_MOD(VarsCmd.DataspaceSize, SIZE_ULONG); + + if (VarsCmd.PoolSize + ArraySize >= POOL_MAX_SIZE) + { + //Not enough memory available + return ERR_MEM; + } + + //Get data from end of pool + pData = VarsCmd.Pool + VarsCmd.PoolSize; + //Grow pool and dataspace + VarsCmd.PoolSize += ArraySize; + VarsCmd.DataspaceSize += ArraySize; + + //Move Array Data + memmove(pData, VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset, (UWORD)(DV_ARRAY[DVIndex].ElemSize * DV_ARRAY[DVIndex].Count)); + //!!! Clear mem so we make sure we don't reference stale data. Not strictly needed. + memset(VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset, 0xFF, (UWORD)(DV_ARRAY[DVIndex].ElemSize * DV_ARRAY[DVIndex].Count)); + + //Update dope vector + DV_ARRAY[DVIndex].Offset = pData - VarsCmd.pDataspace; + DV_ARRAY[DVIndex].Count = NewCount; + + //Move dope vector to end of MemMgr list + Status = cCmdMemMgrMoveToTail(DVIndex); + if (IS_ERR(Status)) + return Status; + + NXT_ASSERT(cCmdVerifyMemMgr()); + } + + return Status; +} + + +//!!! Recursive function +NXT_STATUS cCmdAllocSubArrayDopeVectors(DS_ELEMENT_ID DSElementID, UWORD Offset) +{ + // Walks a single array element to see if it contains arrays + // For any array it finds, a dope vector is allocated and the DVIndex is placed in the dataspace for the parent array. + // This is a non-recursive function. It only walks the immediate array element. + // DSElementID - ID of array sub-entry + // Offset - offset to array element in dataspace + NXT_STATUS Status = NO_ERR; + TYPE_CODE TypeCode; + DV_INDEX DVIndex; + UWORD i; + UWORD DVIndexOffset; //Offset to DVIndex field that points to the DopeVector from pDataspace + UWORD LoopCount = 1; + UWORD ElemSize; + + for (i = 0; i < LoopCount; i++) + { + TypeCode = cCmdDSType((DS_ELEMENT_ID)(DSElementID + i)); + if (TypeCode == TC_CLUSTER) + { + LoopCount += cCmdClusterCount(DSElementID); + } + else if (TypeCode == TC_ARRAY) + { + //!!! ElemSize is a static value, but we don't have anywhere we put it (another TOC sub-entry?) + // It'd be nice to not have to recalculate it. + ElemSize = cCmdCalcArrayElemSize((DS_ELEMENT_ID)(DSElementID + i)); + DVIndexOffset = VarsCmd.pDataspaceTOC[DSElementID + i].DSOffset + Offset; + Status = cCmdAllocDopeVector(&DVIndex, ElemSize, DVIndexOffset); + if (IS_ERR(Status)) + return Status; + + *((UWORD *)(VarsCmd.pDataspace + DVIndexOffset)) = DVIndex; + } + } + + return Status; +} + + +//!!! Recursive function +NXT_STATUS cCmdFreeSubArrayDopeVectors(DS_ELEMENT_ID DSElementID, UWORD Offset) +{ + // Walks a single array element to see if it contains arrays + // Frees all dope vectors associated with the array element. + // Recursively deletes sub-arrays. + // DSElementID - ID of array sub-entry + // Offset - offset to array element in dataspace + NXT_STATUS Status = NO_ERR; + TYPE_CODE TypeCode; + DV_INDEX DVIndex; + UWORD i, Count; + + TypeCode = cCmdDSType(DSElementID); + + if (TypeCode == TC_ARRAY) + { + DVIndex = cCmdGetDVIndex(DSElementID, Offset); + + NXT_ASSERT(DVIndex < DV_ARRAY[0].Count); + + Count = DV_ARRAY[DVIndex].Count; + //Recur on sub-elements + for (i = 0; i < Count; i++) + { + Status = cCmdFreeSubArrayDopeVectors(INC_ID(DSElementID), ARRAY_ELEM_OFFSET(DVIndex, i)); + if (IS_ERR(Status)) + return Status; + } + + //Free Dope Vector + Status = cCmdFreeDopeVector(DVIndex); + } + else if (TypeCode == TC_CLUSTER) + { + Count = cCmdClusterCount(DSElementID); + DSElementID = INC_ID(DSElementID); + //Recur on sub-elements + for (i = 0; i < Count; i++) + { + Status = cCmdFreeSubArrayDopeVectors((DS_ELEMENT_ID)(DSElementID + i), Offset); + if (IS_ERR(Status)) + return Status; + } + } + + return Status; +} + + +NXT_STATUS cCmdAllocDopeVector(DV_INDEX *pIndex, UWORD ElemSize, UWORD BackPtr) +{ + NXT_STATUS Status = NO_ERR; + + if (VarsCmd.MemMgr.FreeHead == NOT_A_DS_ID) + { + //No free DVs. Need to grow DopeVector array. + Status = cCmdGrowDopeVectorArray(DV_ARRAY_GROWTH_COUNT); + if (IS_ERR(Status)) + return Status; + } + + NXT_ASSERT(VarsCmd.MemMgr.FreeHead != NOT_A_DS_ID); + + //Remove DV from free list + *pIndex = VarsCmd.MemMgr.FreeHead; + VarsCmd.MemMgr.FreeHead = DV_ARRAY[VarsCmd.MemMgr.FreeHead].Link; + //Add DV to tail of MemMgr list + Status = cCmdMemMgrInsertAtTail(*pIndex); + + //Initialize values + DV_ARRAY[*pIndex].Offset = NOT_AN_OFFSET; + DV_ARRAY[*pIndex].ElemSize = ElemSize; + DV_ARRAY[*pIndex].Count = 0; + DV_ARRAY[*pIndex].BackPtr = BackPtr; + + NXT_ASSERT(cCmdVerifyMemMgr()); + + return Status; +} + +// +//cCmdFreeDopeVector() - Open up a spot in the DopeVectorArray for future use +// The DopeVectorArray doesn't shrink when arrays (and their dope vectors) are deleted. +// Instead they're pushed on the free list and the array stays the same size. +// Future allocations check the free list before resorting to cCmdGrowDopeVectorArray() +// +NXT_STATUS cCmdFreeDopeVector(DV_INDEX DVIndex) +{ + NXT_STATUS Status = NO_ERR; + DV_INDEX i; + + //Bounds check + NXT_ASSERT(DVIndex < DV_ARRAY[0].Count); + + //Reset dope vector fields + DV_ARRAY[DVIndex].Count = 0; + DV_ARRAY[DVIndex].ElemSize = 0; + DV_ARRAY[DVIndex].Offset = NOT_AN_OFFSET; + DV_ARRAY[DVIndex].BackPtr = NOT_AN_OFFSET; + + //Remove from MemMgr list + if (DVIndex == VarsCmd.MemMgr.Head) + { + VarsCmd.MemMgr.Head = DV_ARRAY[DVIndex].Link; + } + else + { + //Walk MemMgr list to find previous. + //!!! Could speed this up if MemMgr list were doubly linked + for (i = VarsCmd.MemMgr.Head; i != NOT_A_DS_ID; i = DV_ARRAY[i].Link) + { + if (DV_ARRAY[i].Link == DVIndex) + { + DV_ARRAY[i].Link = DV_ARRAY[DVIndex].Link; + if (DVIndex == VarsCmd.MemMgr.Tail) + VarsCmd.MemMgr.Tail = i; + break; + } + } + //Make sure we found the previous DV, otherwise this DV was not in the the list (already freed?) + NXT_ASSERT(i != NOT_A_DS_ID); + } + + //Push on to free list + DV_ARRAY[DVIndex].Link = VarsCmd.MemMgr.FreeHead; + VarsCmd.MemMgr.FreeHead = DVIndex; + + NXT_ASSERT(cCmdVerifyMemMgr()); + + return Status; +} + +// +//cCmdGrowDopeVectorArray() - expand DopeVectorArray to be able to track more dataspace arrays +// +NXT_STATUS cCmdGrowDopeVectorArray(UWORD NewNodesCount) +{ + NXT_STATUS Status = NO_ERR; + UWORD ArraySize; + UWORD OldCount, NewCount, i; + UBYTE * pData; + + NXT_ASSERT(cCmdVerifyMemMgr()); + + OldCount = DV_ARRAY[0].Count; + NewCount = OldCount + NewNodesCount; + NXT_ASSERT(NewCount > OldCount); + + ArraySize = DV_ARRAY[0].ElemSize * NewCount; + + //!!! Aligning beginning of ALL arrays to 4 byte address + ALIGN_TO_MOD(VarsCmd.PoolSize, SIZE_ULONG); + ALIGN_TO_MOD(VarsCmd.DataspaceSize, SIZE_ULONG); + + if (VarsCmd.PoolSize + ArraySize >= POOL_MAX_SIZE) + { + //Not enough memory available + return ERR_MEM; + } + + //Get data from end of pool + pData = VarsCmd.Pool + VarsCmd.PoolSize; + //Grow pool and dataspace + VarsCmd.PoolSize += ArraySize; + VarsCmd.DataspaceSize += ArraySize; + + //Move DopeVector Array + memmove(pData, (UBYTE *)VarsCmd.MemMgr.pDopeVectorArray, (UWORD)(DV_ARRAY[0].ElemSize * DV_ARRAY[0].Count)); + + //Update MemMgr pointer + VarsCmd.MemMgr.pDopeVectorArray = (DOPE_VECTOR *)pData; + IOMapCmd.OffsetDVA = (UWORD)((ULONG)(VarsCmd.MemMgr.pDopeVectorArray) - (ULONG)&(IOMapCmd)); + + //Update dope vector + DV_ARRAY[0].Offset = pData - VarsCmd.pDataspace; + DV_ARRAY[0].Count = NewCount; + + //Add new DopeVectors to free list + //Push in reverse order so they get popped in order (mostly for ease of debugging) + for (i = NewCount - 1; i >= OldCount; i--) + { + DV_ARRAY[i].Offset = 0xFFFF; + DV_ARRAY[i].ElemSize = 0; + DV_ARRAY[i].Count = 0; + DV_ARRAY[i].BackPtr = 0xFFFF; + DV_ARRAY[i].Link = VarsCmd.MemMgr.FreeHead; + VarsCmd.MemMgr.FreeHead = i; + } + + //Move dope vector to end of MemMgr list + Status = cCmdMemMgrMoveToTail(0); + + NXT_ASSERT(cCmdVerifyMemMgr()); + + return Status; +} + +NXT_STATUS cCmdCompactDopeVectorArray(void) +{ + //!!! Not implemented. Needs BackPtr support. + NXT_BREAK; + return ERR_ARG; +} + + +UWORD cCmdCalcArrayElemSize(DS_ELEMENT_ID DSElementID) +{ + TYPE_CODE TypeCode; + UWORD SizeOfType; + UWORD i; + UWORD LoopCount = 1; + UWORD Size = 0; + UWORD Alignment = 0; + + NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY); + + DSElementID = INC_ID(DSElementID); + for (i = 0; i < LoopCount; i++) + { + TypeCode = cCmdDSType((DS_ELEMENT_ID)(DSElementID + i)); + if (TypeCode == TC_CLUSTER) + { + LoopCount += cCmdClusterCount((DS_ELEMENT_ID)(DSElementID + i)); + } + else + { + SizeOfType = cCmdSizeOf(TypeCode); + ALIGN_TO_MOD(Size, SizeOfType); + Size += SizeOfType; + if (SizeOfType > Alignment) + Alignment = SizeOfType; + } + } + ALIGN_TO_MOD(Size, Alignment); + + return Size; +} + + +NXT_STATUS cCmdMemMgrMoveToTail(DV_INDEX DVIndex) +{ + DV_INDEX i; + + //Bounds check + NXT_ASSERT(DVIndex < DV_ARRAY[0].Count); + + //Short circut if its already at the tail + if (DVIndex == VarsCmd.MemMgr.Tail) + return NO_ERR; + + if (DVIndex == VarsCmd.MemMgr.Head) + VarsCmd.MemMgr.Head = DV_ARRAY[DVIndex].Link; + else + { + //Walk MemMgr list to find previous. + //!!! Could speed this up if MemMgr list were doubly linked + for (i = VarsCmd.MemMgr.Head; i != NOT_A_DS_ID; i = DV_ARRAY[i].Link) + { + if (DV_ARRAY[i].Link == DVIndex) + { + DV_ARRAY[i].Link = DV_ARRAY[DVIndex].Link; + break; + } + } + //Make sure we found the previous DV, otherwise this DV was not in the the list + NXT_ASSERT(i != NOT_A_DS_ID); + } + + DV_ARRAY[DVIndex].Link = NOT_A_DS_ID; + DV_ARRAY[VarsCmd.MemMgr.Tail].Link = DVIndex; + VarsCmd.MemMgr.Tail = DVIndex; + + NXT_ASSERT(cCmdVerifyMemMgr()); + + return NO_ERR; +} + + +NXT_STATUS cCmdMemMgrInsertAtTail(DV_INDEX DVIndex) +{ + //Bounds check + NXT_ASSERT(DVIndex < DV_ARRAY[0].Count); + + DV_ARRAY[VarsCmd.MemMgr.Tail].Link = DVIndex; + VarsCmd.MemMgr.Tail = DVIndex; + DV_ARRAY[DVIndex].Link = NOT_A_DS_ID; + + NXT_ASSERT(cCmdVerifyMemMgr()); + + return NO_ERR; +} + + +UBYTE cCmdVerifyMemMgr() +{ + DV_INDEX i; + UWORD CurrOffset = 0; + UWORD PrevOffset = 0; + UWORD DVCount = 0; + + //Make sure the MemMgr list is properly sorted in ascending offset order + for (i = VarsCmd.MemMgr.Head; i != NOT_A_DS_ID; i = DV_ARRAY[i].Link) + { + CurrOffset = DV_ARRAY[i].Offset; + + if (CurrOffset != 0xFFFF) + { + if (PrevOffset > CurrOffset) + return FALSE; + + PrevOffset = CurrOffset; + } + + if (DV_ARRAY[i].Link == NOT_A_DS_ID && i != VarsCmd.MemMgr.Tail) + return FALSE; + + DVCount++; + } + + for (i = VarsCmd.MemMgr.FreeHead; i != NOT_A_DS_ID; i = DV_ARRAY[i].Link) + { + DVCount++; + } + + //Make sure the # of dope vectors = # used + # free + if (DVCount != DV_ARRAY[0].Count) + return FALSE; + + return TRUE; +} + + +NXT_STATUS cCmdDSCompact(void) +{ + NXT_STATUS Status = NO_ERR; + + DV_INDEX CurrIndex; + UWORD NewOffset; + UWORD CurrOffset; + UWORD Size; + UWORD DeltaDSSize; + UWORD TempOffset, TempSize; + +#if VM_BENCHMARK + ULONG StartTime, TotalTime; + + VarsCmd.CompactionCount++; + VarsCmd.LastCompactionTick = IOMapCmd.Tick - VarsCmd.StartTick; + + StartTime = dTimerRead(); +#endif + + NXT_ASSERT(cCmdVerifyMemMgr()); + + NewOffset = VarsCmd.DSStaticSize; + for (CurrIndex = VarsCmd.MemMgr.Head; CurrIndex != NOT_A_DS_ID; CurrIndex = DV_ARRAY[CurrIndex].Link) + { + //Align NewOffset for array to 4 byte address. + ALIGN_TO_MOD(NewOffset, SIZE_ULONG); + + CurrOffset = DV_ARRAY[CurrIndex].Offset; + if (CurrOffset != NOT_AN_OFFSET) + { + Size = DV_ARRAY[CurrIndex].ElemSize * DV_ARRAY[CurrIndex].Count; + if (CurrOffset != NewOffset) + { + NXT_ASSERT(NewOffset < CurrOffset); + memmove(VarsCmd.pDataspace + NewOffset, VarsCmd.pDataspace + CurrOffset, Size); + + // Clear mem to make stale data references more obvious while debugging. + // Correct for overlapping memory regions (make sure we don't clear what we just moved). + //!!! Clearing step not strictly necessary, so it could be optimized out + if (NewOffset + Size > CurrOffset) + { + TempOffset = NewOffset + Size; + TempSize = Size - (TempOffset - CurrOffset); + } + else + { + TempOffset = CurrOffset; + TempSize = Size; + } + memset(VarsCmd.pDataspace + TempOffset, 0xFF, TempSize); + + //Update pDopeVectorArray if we move the dope vector array + if (CurrIndex == 0) + { + VarsCmd.MemMgr.pDopeVectorArray = (DOPE_VECTOR *)(VarsCmd.pDataspace + NewOffset); + IOMapCmd.OffsetDVA = (UWORD)((ULONG)(VarsCmd.MemMgr.pDopeVectorArray) - (ULONG)&(IOMapCmd)); + } + + //Update offset in DV Array + DV_ARRAY[CurrIndex].Offset = NewOffset; + } + + NewOffset += Size; + } + } + + DeltaDSSize = VarsCmd.DataspaceSize - NewOffset; + + VarsCmd.PoolSize -= DeltaDSSize; + VarsCmd.DataspaceSize -= DeltaDSSize; + + NXT_ASSERT(cCmdVerifyMemMgr()); + +#if VM_BENCHMARK + TotalTime = dTimerRead() - StartTime; + + if (TotalTime > VarsCmd.MaxCompactionTime) + VarsCmd.MaxCompactionTime = TotalTime; +#endif + + return Status; +} + + +// +// Message Queue functions +// + +NXT_STATUS cCmdMessageWrite(UWORD QueueID, UBYTE * pData, UWORD Length) +{ + NXT_STATUS Status = NO_ERR; + + if (pData == NULL) + return ERR_ARG; + + if (QueueID >= MESSAGE_QUEUE_COUNT) + return ERR_INVALID_QUEUE; + + if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE) + return ERR_NO_PROG; + + //Can't accept oversize messages because we treat them as strings (truncation would remove null termination) + if (Length > MAX_MESSAGE_SIZE) + return ERR_INVALID_SIZE; + + if (IS_DV_INDEX_SANE(GET_WRITE_MSG(QueueID))) + { + //A message is already there, the queue is full + NXT_ASSERT(VarsCmd.MessageQueues[QueueID].WriteIndex == VarsCmd.MessageQueues[QueueID].ReadIndex); + + //Bump read index, drop existing message to make room for our new incoming message + VarsCmd.MessageQueues[QueueID].ReadIndex = (VarsCmd.MessageQueues[QueueID].ReadIndex + 1) % MESSAGES_PER_QUEUE; + } + else + { + //Allocate dope vector for message + Status = cCmdAllocDopeVector(&GET_WRITE_MSG(QueueID), 1, NOT_AN_OFFSET); + if (IS_ERR(Status)) + return Status; + } + + //Allocate storage for message + Status = cCmdDVArrayAlloc(GET_WRITE_MSG(QueueID), Length); + if (IS_ERR(Status)) + { + //Clear the dope vector for the message, since we're unable to put a message there. + cCmdFreeDopeVector(GET_WRITE_MSG(QueueID)); + SET_WRITE_MSG(QueueID, NOT_A_DS_ID); + return Status; + } + + //Copy message + memmove(cCmdDVPtr(GET_WRITE_MSG(QueueID)), pData, Length); + + //Advance write index + VarsCmd.MessageQueues[QueueID].WriteIndex = (VarsCmd.MessageQueues[QueueID].WriteIndex + 1) % MESSAGES_PER_QUEUE; + + return Status; +} + + +NXT_STATUS cCmdMessageGetSize(UWORD QueueID, UWORD * Size) +{ + DV_INDEX ReadDVIndex; + + if (Size == NULL) + return (ERR_ARG); + + if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE) + { + *Size = 0; + return (ERR_NO_PROG); + } + + if (QueueID >= MESSAGE_QUEUE_COUNT) + { + *Size = 0; + return (ERR_INVALID_QUEUE); + } + + ReadDVIndex = GET_READ_MSG(QueueID); + + if (IS_DV_INDEX_SANE(ReadDVIndex)) + { + *Size = (DV_ARRAY[ReadDVIndex].Count); + return (NO_ERR); + } + else + { + *Size = 0; + return (STAT_MSG_EMPTY_MAILBOX); + } +} + + +NXT_STATUS cCmdMessageRead(UWORD QueueID, UBYTE * pBuffer, UWORD Length, UBYTE Remove) +{ + NXT_STATUS Status = NO_ERR; + DV_INDEX ReadDVIndex; + + if (pBuffer == NULL) + return (ERR_ARG); + + if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE) + return (ERR_NO_PROG); + + if (QueueID >= MESSAGE_QUEUE_COUNT) + return (ERR_INVALID_QUEUE); + + ReadDVIndex = GET_READ_MSG(QueueID); + + if (IS_DV_INDEX_SANE(ReadDVIndex)) + { + //If Buffer doesn't have room for the entire message, + //don't risk incomplete string floating around + if (Length < DV_ARRAY[ReadDVIndex].Count) + return (ERR_INVALID_SIZE); + + //Copy message + memmove(pBuffer, cCmdDVPtr(ReadDVIndex), DV_ARRAY[ReadDVIndex].Count); + + if (Remove) + { + //Free memory used by message + Status = cCmdFreeDopeVector(ReadDVIndex); + if (IS_ERR(Status)) + return Status; + + SET_READ_MSG(QueueID, NOT_A_DS_ID); + + //Advance read index + VarsCmd.MessageQueues[QueueID].ReadIndex = (VarsCmd.MessageQueues[QueueID].ReadIndex + 1) % MESSAGES_PER_QUEUE; + } + } + else + { + //No message to read, message Queue is empty + NXT_ASSERT(VarsCmd.MessageQueues[QueueID].ReadIndex == VarsCmd.MessageQueues[QueueID].WriteIndex); + + return (STAT_MSG_EMPTY_MAILBOX); + } + + return Status; +} + + +// +// Dataspace Support functions +// + +UBYTE cCmdIsDSElementIDSane(DS_ELEMENT_ID Index) +{ + if (Index < VarsCmd.DataspaceCount) + return TRUE; + else + return FALSE; +} + +TYPE_CODE cCmdDSType(DS_ELEMENT_ID DSElementID) +{ + NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID)); + + return (VarsCmd.pDataspaceTOC[DSElementID].TypeCode); +} + + +void * cCmdResolveDataArg(DATA_ARG DataArg, UWORD Offset, TYPE_CODE * TypeCode) +{ + UBYTE ModuleID; + UWORD FieldID; + void * ret_val = NULL; + + // + //!!! DATA_ARG masking system only for internal c_cmd use! + // All normal bytecode arguments should go through top if() block. + // + + if (DataArg <= (DATA_ARG)(DATA_ARG_ADDR_MASK) ) + { + NXT_ASSERT(cCmdIsDSElementIDSane(DataArg)); + ret_val = cCmdDSPtr(DataArg, Offset); + if (TypeCode) + *TypeCode = VarsCmd.pDataspaceTOC[DataArg].TypeCode; + } + else if (DataArg & ~((DATA_ARG)(DATA_ARG_ADDR_MASK))) + { + //DataArg refers to a field in the IO map + ModuleID = (UBYTE)((DataArg >> 9) & 0x001F); + FieldID = (UWORD)(DataArg & 0x01FF); + + //!!! Preliminary bounds check -- still could allow invalid combos through + if (ModuleID > MOD_OUTPUT || FieldID >= IO_OUT_FIELD_COUNT) + { + NXT_BREAK; + return NULL; + } + + ret_val = IO_PTRS[ModuleID][FieldID]; + if (TypeCode) + *TypeCode = IO_TYPES[ModuleID][FieldID]; + } + + //!!! Caller beware! If DataArg isn't sane, ret_val may be out of range or NULL! + return ret_val; +} + +void cCmdSetVal(void * pVal, TYPE_CODE TypeCode, ULONG NewVal) +{ + + if (pVal) + { + switch (TypeCode) + { + case TC_ULONG: + case TC_SLONG: + { + *(ULONG*)pVal = NewVal; + } + break; + + case TC_UWORD: + case TC_SWORD: + { + *(UWORD*)pVal = (UWORD)NewVal; + } + break; + + case TC_UBYTE: + case TC_SBYTE: + { + *(UBYTE*)pVal = (UBYTE)NewVal; + } + break; + } + } + + return; +} + + +ULONG cCmdGetVal(void * pVal, TYPE_CODE TypeCode) +{ + if (pVal) + { + switch (TypeCode) + { + case TC_ULONG: + { + return (ULONG)(*(ULONG*)pVal); + } + + case TC_SLONG: + { + return (SLONG)(*(SLONG*)pVal); + } + + case TC_UWORD: + { + return (UWORD)(*(UWORD*)pVal); + } + + case TC_SWORD: + { + return (SWORD)(*(SWORD*)pVal); + } + + case TC_UBYTE: + { + return (UBYTE)(*(UBYTE*)pVal); + } + + case TC_SBYTE: + { + return (SBYTE)(*(SBYTE*)pVal); + } + + default: + break; + } + } + + //!!! Default return value places responsibility on caller to use this function wisely + return 0; +} + + +UWORD cCmdSizeOf(TYPE_CODE TypeCode) +{ + //!!! Why not use a lookup table? No particular reason... + switch(TypeCode) + { + case TC_ULONG: + return SIZE_ULONG; + case TC_SLONG: + return SIZE_SLONG; + case TC_UWORD: + return SIZE_UWORD; + case TC_SWORD: + return SIZE_SWORD; + case TC_UBYTE: + return SIZE_UBYTE; + case TC_SBYTE: + return SIZE_SBYTE; + case TC_MUTEX: + return SIZE_MUTEX; + case TC_ARRAY: + //Arrays have a 2-byte structure in the dataspace for the DVIndex + return SIZE_UWORD; + case TC_CLUSTER: + default: + return 0; + } +} + + +void* cCmdDSPtr(DS_ELEMENT_ID DSElementID, UWORD Offset) +{ + void * pDSItem; + DV_INDEX DVIndex; + TYPE_CODE TypeCode; + + NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID)); + + TypeCode = cCmdDSType(DSElementID); + if (TypeCode == TC_ARRAY) + { + //!!! Empty arrays return NULL. + if (cCmdArrayCount(DSElementID, Offset) == 0) + pDSItem = NULL; + else + { + DVIndex = cCmdGetDVIndex(DSElementID, Offset); + pDSItem = (VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset); + } + } + else if (TypeCode == TC_CLUSTER) + { + NXT_ASSERT(cCmdClusterCount(DSElementID) != 0) + + //Returning pointer to the first element in the cluster + pDSItem = cCmdDSPtr(INC_ID(DSElementID), Offset); + } + else + pDSItem = (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset + Offset); + + NXT_ASSERT((UBYTE*)pDSItem < POOL_SENTINEL); + + return pDSItem; +} + +void* cCmdDVPtr(DV_INDEX DVIndex) +{ + NXT_ASSERT(IS_DV_INDEX_SANE(DVIndex)); + return (VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset); +} + + +//!!! Recursive function +DS_ELEMENT_ID cCmdNextDSElement(DS_ELEMENT_ID CurrID) +{ + DS_ELEMENT_ID NextID; + TYPE_CODE CurrType; + UWORD ClusterCount, i; + + NXT_ASSERT(cCmdIsDSElementIDSane(CurrID)); + + NextID = CurrID + 1; + + if (!cCmdIsDSElementIDSane(NextID)) + return NOT_A_DS_ID; + + CurrType = cCmdDSType(CurrID); + + if (CurrType == TC_ARRAY) + { + //Arrays contain two elements. Advance past the second one. + NextID = cCmdNextDSElement(NextID); + } + else if (CurrType == TC_CLUSTER) + { + ClusterCount = cCmdClusterCount(CurrID); + for (i = 0; i < ClusterCount; i++) + { + NextID = cCmdNextDSElement(NextID); + } + } + + return NextID; +} + + +//!!! Recursive function +UBYTE cCmdCompareDSType(DS_ELEMENT_ID DSElementID1, DS_ELEMENT_ID DSElementID2) +{ + TYPE_CODE Type1, Type2; + UWORD i, Count1, Count2; + + Type1 = cCmdDSType(DSElementID1); + Type2 = cCmdDSType(DSElementID2); + + if (Type1 != Type2) + return FALSE; + + if (Type1 == TC_CLUSTER) + { + Count1 = cCmdClusterCount(DSElementID1); + Count2 = cCmdClusterCount(DSElementID2); + + if(Count1 != Count2) + return FALSE; + + DSElementID1 = INC_ID(DSElementID1); + DSElementID2 = INC_ID(DSElementID2); + + for (i = 0; i < Count1; i++) + { + if (!cCmdCompareDSType(DSElementID1, DSElementID2)) + return FALSE; + + DSElementID1 = cCmdNextDSElement(DSElementID1); + DSElementID2 = cCmdNextDSElement(DSElementID2); + } + } + else if (Type1 == TC_ARRAY) + { + if (!cCmdCompareDSType(INC_ID(DSElementID1), INC_ID(DSElementID2))) + return FALSE; + } + + return TRUE; +} + + +//!!! Recursive function +UWORD cCmdCalcFlattenedSize(DS_ELEMENT_ID DSElementID, UWORD Offset) +{ + UWORD Size = 0; + TYPE_CODE TypeCode; + DV_INDEX DVIndex; + UWORD i; + UWORD Count; + + TypeCode = cCmdDSType(DSElementID); + + if (TypeCode == TC_ARRAY) + { + DVIndex = cCmdGetDVIndex(DSElementID, Offset); + + DSElementID = INC_ID(DSElementID); + TypeCode = cCmdDSType(DSElementID); + + if (!IS_AGGREGATE_TYPE(TypeCode)) + { + //Short circuit recursive calculation if our array sub-type is a scalar + Size += DV_ARRAY[DVIndex].ElemSize * DV_ARRAY[DVIndex].Count; + } + else + { + //If the sub type is an aggregate type, then it can contain arrays, so we have to recur + for (i = 0; i < DV_ARRAY[DVIndex].Count; i++) + { + Size += cCmdCalcFlattenedSize(DSElementID, ARRAY_ELEM_OFFSET(DVIndex, i)); + } + } + } + else if (TypeCode == TC_CLUSTER) + { + Count = cCmdClusterCount(DSElementID); + + DSElementID = INC_ID(DSElementID); + for (i = 0; i < Count; i++) + { + Size += cCmdCalcFlattenedSize(DSElementID, Offset); + DSElementID = cCmdNextDSElement(DSElementID); + } + } + else //Scalar + { + Size += cCmdSizeOf(TypeCode); + } + return Size; +} + + +//!!! Recursive function +NXT_STATUS cCmdFlattenToByteArray(UBYTE * pByteArray, UWORD * pByteOffset, DS_ELEMENT_ID DSElementID, UWORD Offset) +{ + NXT_STATUS Status = NO_ERR; + TYPE_CODE TypeCode; + DV_INDEX DVIndex; + UWORD i; + UWORD Count; + UBYTE *pVal; + + TypeCode = cCmdDSType(DSElementID); + + if (TypeCode == TC_ARRAY) + { + DVIndex = cCmdGetDVIndex(DSElementID, Offset); + Count = DV_ARRAY[DVIndex].Count; + + DSElementID = INC_ID(DSElementID); + TypeCode = cCmdDSType(DSElementID); + if (!IS_AGGREGATE_TYPE(TypeCode)) + { + //Short circuit recursive calculation if our array sub-type is a scalar + Count = DV_ARRAY[DVIndex].ElemSize * DV_ARRAY[DVIndex].Count; + memmove((pByteArray + *pByteOffset), (VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset), Count); + *pByteOffset += Count; + } + else + { + //If the sub type is an aggregate type, then it can contain arrays, so we have to recur + for (i = 0; i < Count; i++) + { + cCmdFlattenToByteArray(pByteArray, pByteOffset, DSElementID, ARRAY_ELEM_OFFSET(DVIndex, i)); + } + } + } + else if (TypeCode == TC_CLUSTER) + { + Count = cCmdClusterCount(DSElementID); + + DSElementID = INC_ID(DSElementID); + for (i = 0; i < Count; i++) + { + cCmdFlattenToByteArray(pByteArray, pByteOffset, DSElementID, Offset); + DSElementID = cCmdNextDSElement(DSElementID); + } + } + else //Scalar + { + pVal = cCmdResolveDataArg(DSElementID, Offset, NULL); + Count = cCmdSizeOf(TypeCode); + + memmove((pByteArray + *pByteOffset), pVal, Count); + *pByteOffset += Count; + } + + return Status; +} + +NXT_STATUS cCmdUnflattenFromByteArray(UBYTE * pByteArray, UWORD * pByteOffset, DS_ELEMENT_ID DSElementID, UWORD Offset) +{ + NXT_STATUS Status = NO_ERR; + TYPE_CODE TypeCode; + DV_INDEX DVIndex; + UWORD i; + UWORD Count; + UBYTE *pVal; + + TypeCode = cCmdDSType(DSElementID); + + if (TypeCode == TC_ARRAY) + { + DVIndex = cCmdGetDVIndex(DSElementID, Offset); + Count = DV_ARRAY[DVIndex].Count; + + DSElementID = INC_ID(DSElementID); + TypeCode = cCmdDSType(DSElementID); + if (!IS_AGGREGATE_TYPE(TypeCode)) + { + //Short circuit recursive calculation if our array sub-type is a scalar + Count = DV_ARRAY[DVIndex].ElemSize * DV_ARRAY[DVIndex].Count; + memmove((VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset), (pByteArray + *pByteOffset), Count); + *pByteOffset += Count; + } + else + { + //If the sub type is an aggregate type, then it can contain arrays, so we have to recur + for (i = 0; i < Count; i++) + { + cCmdUnflattenFromByteArray(pByteArray, pByteOffset, DSElementID, ARRAY_ELEM_OFFSET(DVIndex, i)); + } + } + } + else if (TypeCode == TC_CLUSTER) + { + Count = cCmdClusterCount(DSElementID); + + DSElementID = INC_ID(DSElementID); + for (i = 0; i < Count; i++) + { + cCmdUnflattenFromByteArray(pByteArray, pByteOffset, DSElementID, Offset); + DSElementID = cCmdNextDSElement(DSElementID); + } + } + else //Scalar + { + pVal = cCmdResolveDataArg(DSElementID, Offset, NULL); + Count = cCmdSizeOf(TypeCode); + + memmove(pVal, (pByteArray + *pByteOffset), Count); + *pByteOffset += Count; + } + + return Status; +} + + +UWORD cCmdClusterCount(DS_ELEMENT_ID DSElementID) +{ + UWORD ClusterCount; + + NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID)); + NXT_ASSERT(cCmdDSType(DSElementID) == TC_CLUSTER); + + ClusterCount = VarsCmd.pDataspaceTOC[DSElementID].DSOffset; + + return ClusterCount; +} + + +UWORD cCmdGetDVIndex(DS_ELEMENT_ID DSElementID, UWORD Offset) +{ + UWORD DVIndex; + + NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY); + + DVIndex = *(UWORD *)(VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset + Offset); + + //Make sure we're returning a valid DVIndex + NXT_ASSERT(DVIndex != 0 && DVIndex < DV_ARRAY[0].Count); + + return DVIndex; +} + + +UWORD cCmdArrayCount(DS_ELEMENT_ID DSElementID, UWORD Offset) +{ + DV_INDEX DVIndex; + + NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID)); + NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY); + + DVIndex = cCmdGetDVIndex(DSElementID, Offset); + return DV_ARRAY[DVIndex].Count; +} + +TYPE_CODE cCmdArrayType(DS_ELEMENT_ID DSElementID) +{ + TYPE_CODE TypeCode; + + NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID)); + NXT_ASSERT(cCmdIsDSElementIDSane(INC_ID(DSElementID))); + NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY); + + TypeCode = VarsCmd.pDataspaceTOC[DSElementID + 1].TypeCode; + + return TypeCode; +} + + +DS_ELEMENT_ID cCmdGetDataspaceCount(void) +{ + return (VarsCmd.DataspaceCount); +} + + +CODE_INDEX cCmdGetCodespaceCount(CLUMP_ID Clump) +{ + if (Clump == NOT_A_CLUMP) + return (VarsCmd.CodespaceCount); + else + { + NXT_ASSERT(cCmdIsClumpIDSane(Clump)); + return (VarsCmd.pAllClumps[Clump].CodeEnd - VarsCmd.pAllClumps[Clump].CodeStart + 1); + } +} + + +UBYTE cCmdCompare(UBYTE CompCode, ULONG Val1, ULONG Val2, TYPE_CODE TypeCode1, TYPE_CODE TypeCode2) +{ + SLONG SVal1, SVal2; + + if (IS_SIGNED_TYPE(TypeCode1) && IS_SIGNED_TYPE(TypeCode2)) + { + SVal1 = (SLONG)Val1; + SVal2 = (SLONG)Val2; + return ((CompCode == OPCC1_LT && SVal1 < SVal2) + || (CompCode == OPCC1_GT && SVal1 > SVal2) + || (CompCode == OPCC1_LTEQ && SVal1 <= SVal2) + || (CompCode == OPCC1_GTEQ && SVal1 >= SVal2) + || (CompCode == OPCC1_EQ && SVal1 == SVal2) + || (CompCode == OPCC1_NEQ && SVal1 != SVal2)); + } + else + { + return ((CompCode == OPCC1_LT && Val1 < Val2) + || (CompCode == OPCC1_GT && Val1 > Val2) + || (CompCode == OPCC1_LTEQ && Val1 <= Val2) + || (CompCode == OPCC1_GTEQ && Val1 >= Val2) + || (CompCode == OPCC1_EQ && Val1 == Val2) + || (CompCode == OPCC1_NEQ && Val1 != Val2)); + } + +} + + +NXT_STATUS cCmdCompareAggregates(UBYTE CompCode, UBYTE *ReturnBool, DATA_ARG Arg2, UWORD Offset2, DATA_ARG Arg3, UWORD Offset3) +{ + NXT_STATUS Status = NO_ERR; + UBYTE Finished; + + Finished = FALSE; + Status = cCmdRecursiveCompareAggregates(CompCode, ReturnBool, &Finished, Arg2, Offset2, Arg3, Offset3); + if (Finished == FALSE) + { + //If Finished has not been set to TRUE, it means that it was unable to find an inequality, thereby ending the comparison. + //Both elements are equal. Assign the proper value to ReturnBool + *ReturnBool = (CompCode == OPCC1_EQ || CompCode == OPCC1_GTEQ || CompCode == OPCC1_LTEQ); + } + + return Status; +} + + +//!!! Recursive function +NXT_STATUS cCmdRecursiveCompareAggregates(UBYTE CompCode, UBYTE *ReturnBool, UBYTE *Finished, DATA_ARG Arg2, UWORD Offset2, DATA_ARG Arg3, UWORD Offset3) +{ + //The value of Finished must be set to FALSE before calling this function. + //We are able to determine the result of the comparison once we find an inequality. + //Once an inequality is found, Finished is set to TRUE and ReturnBool is set based on the CompCode. + //A call to this function will return with Finished still equal to FALSE if both elements are equal in value and count. + //It is the caller of this function's job to set ReturnBool if this function returns with Finished == FALSE. + + NXT_STATUS Status = NO_ERR; + TYPE_CODE TypeCode2, TypeCode3; + DV_INDEX DVIndex2, DVIndex3; + ULONG ArgVal2, ArgVal3; + UWORD Count2, Count3, MinCount; + UWORD i; + + void *pArg2 = NULL, + *pArg3 = NULL; + + TypeCode2 = cCmdDSType(Arg2); + TypeCode3 = cCmdDSType(Arg3); + + //Make sure the two things we're comparing are the same type + if (IS_AGGREGATE_TYPE(TypeCode2) && (TypeCode2 != TypeCode3)) + { + NXT_BREAK; + return ERR_ARG; + } + + //Simple case, both args are scalars. Solve and return. + if (!IS_AGGREGATE_TYPE(TypeCode2)) + { + pArg2 = cCmdResolveDataArg(Arg2, Offset2, &TypeCode2); + pArg3 = cCmdResolveDataArg(Arg3, Offset3, &TypeCode3); + + ArgVal2 = cCmdGetVal(pArg2, TypeCode2); + ArgVal3 = cCmdGetVal(pArg3, TypeCode3); + + //Once we find an inequality, we can determine the result of the comparison + *Finished = cCmdCompare(OPCC1_NEQ, ArgVal2, ArgVal3, TypeCode2, TypeCode3); + + if (*Finished) + *ReturnBool = cCmdCompare(CompCode, ArgVal2, ArgVal3, TypeCode2, TypeCode3); + + return Status; + } + + // Initialize local variables for each argument + + if (TypeCode2 == TC_ARRAY) + { + Count2 = cCmdArrayCount(Arg2, Offset2); + DVIndex2 = cCmdGetDVIndex(Arg2, Offset2); + Offset2 = DV_ARRAY[DVIndex2].Offset; + + Count3 = cCmdArrayCount(Arg3, Offset3); + DVIndex3 = cCmdGetDVIndex(Arg3, Offset3); + Offset3 = DV_ARRAY[DVIndex3].Offset; + } + else if (TypeCode2 == TC_CLUSTER) + { + Count2 = cCmdClusterCount(Arg2); + Count3 = cCmdClusterCount(Arg3); + } + + //Short circuit evaluation of EQ and NEQ if counts are different + if (Count2 != Count3) + { + if ((CompCode == OPCC1_EQ) || (CompCode == OPCC1_NEQ)) + { + *Finished = TRUE; + *ReturnBool = (CompCode == OPCC1_NEQ); + return Status; + } + } + + MinCount = (Count2 < Count3) ? Count2 : Count3; + + //Advance aggregate args to first sub-element for next call + Arg2 = INC_ID(Arg2); + Arg3 = INC_ID(Arg3); + + // + // Loop through the sub-elements of aggregate arguments. + // Call cCmdRecursiveCompareAggregates recursively with simpler type. + // + + for (i = 0; i < MinCount; i++) + { + Status = cCmdRecursiveCompareAggregates(CompCode, ReturnBool, Finished, Arg2, Offset2, Arg3, Offset3); + if (*Finished || IS_ERR(Status)) + return Status; + + //Advance aggregate args to next sub-element + if (TypeCode2 == TC_ARRAY) + { + Offset2 += DV_ARRAY[DVIndex2].ElemSize; + Offset3 += DV_ARRAY[DVIndex3].ElemSize; + } + else if (TypeCode2 == TC_CLUSTER) + { + Arg2 = cCmdNextDSElement(Arg2); + Arg3 = cCmdNextDSElement(Arg3); + } + } + + //All elements in aggregates type up to MinCount are equal. Count discrepancy determines comparison outcome. + if (Count2 != Count3) + { + *Finished = TRUE; + *ReturnBool = cCmdCompare(CompCode, Count2, Count3, TC_UWORD, TC_UWORD); + } + //Else, no size discrepancy. Elements are equal. Comparison still not resolved, + //so return !Finished and status back up the call chain for further comparison + + return Status; +} + + +// +// Interpreter Functions +// +#define VAR_INSTR_SIZE 0xE + +NXT_STATUS cCmdInterpFromClump(CLUMP_ID Clump) +{ + NXT_STATUS Status = NO_ERR; + CLUMP_REC * pClumpRec; + CODE_WORD * pInstr; + UBYTE InterpFuncIndex, InstrSize; +#if VM_BENCHMARK + ULONG InstrTime = dTimerRead(); +#endif + + if (!cCmdIsClumpIDSane(Clump)) + { + //Caller gave us a bad clump ID -- something is very wrong! Force interpretter to halt. + NXT_BREAK; + return (ERR_ARG); + } + + //Resolve clump record structure and current instruction pointer + pClumpRec = &(VarsCmd.pAllClumps[Clump]); + pInstr = (VarsCmd.pCodespace + pClumpRec->CodeStart + pClumpRec->PC); + + //Get instruction size in bytes. + InstrSize = INSTR_SIZE(pInstr); + + //If instruction is odd-sized or if "real" op code is out of range, give up and return ERR_INSTR + if ((InstrSize & 0x01) || OP_CODE(pInstr) >= OPCODE_COUNT) + return (ERR_INSTR); + + InterpFuncIndex = (InstrSize / 2) - 1; + +#ifdef USE_SHORT_OPS + //If instruction has shortened encoding, add 1 for true interpretter + if (IS_SHORT_OP(pInstr)) + { + InterpFuncIndex++; + } +#endif + + //Peg InterpFuncIndex to 'Other'. Illegal instructions will be caught in cCmdInterpOther(). + if (InterpFuncIndex > 4) + InterpFuncIndex = 4; + + //If instruction is variably-sized; true size is held in the first argument + //!!! This InstrSize wrangling MUST occur after computing InterpFuncIndex + //because variable sized instructions may confuse the code otherwise + if (InstrSize == VAR_INSTR_SIZE) + InstrSize = (UBYTE)(pInstr[1]); + + //Set ScratchPC to clump's current PC so sub-interpreters can apply relative offsets + VarsCmd.ScratchPC = pClumpRec->PC; + + //Set CallerClump to Clump, for use by instructions such as OP_ACQUIRE + VarsCmd.CallerClump = Clump; + + Status = (*InterpFuncs[InterpFuncIndex])(pInstr); + + if (Status == ERR_MEM) + { + //Memory is full. Compact dataspace and try the instruction again. + //!!! Could compact DopeVectorArray here + cCmdDSCompact(); + Status = (*InterpFuncs[InterpFuncIndex])(pInstr); + } + + if (!IS_ERR(Status)) + { + //If clump is finished, reset PC and firecount + if (Status == CLUMP_DONE) + { + pClumpRec->PC = 0; + pClumpRec->CurrFireCount = pClumpRec->InitFireCount; + } + //Else, if instruction has provided override program counter, use it + else if (Status == PC_OVERRIDE) + { + pClumpRec->PC = VarsCmd.ScratchPC; + } + //Else, auto-advance from encoded instruction size (PC is word-based) + else + { + pClumpRec->PC += InstrSize / 2; + } + + //Throw error if we ever advance beyond the clump's codespace + if (pClumpRec->PC > cCmdGetCodespaceCount(Clump)) + { + NXT_BREAK; + Status = ERR_INSTR; + } + } + +#if VM_BENCHMARK + //Increment opcode count + VarsCmd.OpcodeBenchmarks[OP_CODE(pInstr)][0]++; + + InstrTime = dTimerRead() - InstrTime; + if (InstrTime > 1) + { + VarsCmd.OpcodeBenchmarks[OP_CODE(pInstr)][1]++; + if (InstrTime > VarsCmd.OpcodeBenchmarks[OP_CODE(pInstr)][2]) + VarsCmd.OpcodeBenchmarks[OP_CODE(pInstr)][2] = InstrTime; + } +#endif + + return (Status); +} + + +NXT_STATUS cCmdInterpUnop1(CODE_WORD * const pCode) +{ + NXT_STATUS Status = NO_ERR; + UBYTE opCode; + DATA_ARG Arg1; + void *pArg1 = NULL; + TYPE_CODE TypeCode1; + + NXT_ASSERT(pCode != NULL); + +#ifdef USE_SHORT_OPS + if (IS_SHORT_OP(pCode)) + { + //add mapping from quick op to real op + opCode = ShortOpMap[SHORT_OP_CODE(pCode)]; + Arg1 = SHORT_ARG(pCode); + } + else + { + opCode = OP_CODE(pCode); + Arg1 = pCode[1]; + } +#else + opCode = OP_CODE(pCode); + Arg1 = pCode[1]; +#endif //USE_SHORT_OPS + + + switch (opCode) + { + case OP_JMP: + { + VarsCmd.ScratchPC = VarsCmd.ScratchPC + (SWORD)Arg1; + Status = PC_OVERRIDE; + } + break; + + case OP_ACQUIRE: + { + NXT_ASSERT(cCmdIsDSElementIDSane(Arg1)); + NXT_ASSERT(VarsCmd.pDataspaceTOC[Arg1].TypeCode == TC_MUTEX); + + Status = cCmdAcquireMutex((MUTEX_Q *)cCmdDSPtr(Arg1, 0), VarsCmd.CallerClump); + } + break; + + case OP_RELEASE: + { + NXT_ASSERT(cCmdIsDSElementIDSane(Arg1)); + NXT_ASSERT(VarsCmd.pDataspaceTOC[Arg1].TypeCode == TC_MUTEX); + + Status = cCmdReleaseMutex((MUTEX_Q *)cCmdDSPtr(Arg1, 0), VarsCmd.CallerClump); + } + break; + + case OP_SUBRET: + { + NXT_ASSERT(cCmdIsDSElementIDSane(Arg1)); + + //Take Subroutine off RunQ + //Add Subroutine's caller to RunQ + cCmdDeQClump(&(VarsCmd.RunQ), VarsCmd.CallerClump); + cCmdEnQClump(&(VarsCmd.RunQ), *((CLUMP_ID *)cCmdDSPtr(Arg1, 0))); + + Status = CLUMP_DONE; + } + break; + + case OP_FINCLUMPIMMED: + { + cCmdDeQClump(&(VarsCmd.RunQ), VarsCmd.CallerClump); //Dequeue finalized clump + cCmdSchedDependent(VarsCmd.CallerClump, (CLUMP_ID)Arg1); // Use immediate form + + Status = CLUMP_DONE; + } + break; + + case OP_GETTICK: + { + pArg1 = cCmdResolveDataArg(Arg1, 0, &TypeCode1); + + + cCmdSetVal(pArg1, TypeCode1, dTimerRead()); + } + break; + + case OP_STOP: + { + //Unwired Arg1 means always stop + if (Arg1 == NOT_A_DS_ID) + { + Status = STOP_REQ; + } + else + { + pArg1 = cCmdResolveDataArg(Arg1, 0, &TypeCode1); + + if (cCmdGetVal(pArg1, TypeCode1) > 0) + Status = STOP_REQ; + } + } + break; + + default: + { + //Fatal error: Unrecognized instruction + NXT_BREAK; + Status = ERR_INSTR; + } + break; + } + + return (Status); +} + + +NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode) +{ + NXT_STATUS Status = NO_ERR; + UBYTE opCode; + DATA_ARG Arg1; + DATA_ARG Arg2; + void *pArg1 = NULL, *pArg2 = NULL; + TYPE_CODE TypeCode1, TypeCode2; + + ULONG i; + UWORD ArgC; + static UBYTE * ArgV[MAX_CALL_ARGS + 1]; + + UWORD Count; + UWORD Offset; + SLONG TmpSLong; + ULONG TmpULong; + ULONG ArgVal2; + + NXT_ASSERT(pCode != NULL); + +#ifdef USE_SHORT_OPS + if (IS_SHORT_OP(pCode)) + { + //add mapping from quick op to real op + opCode = ShortOpMap[SHORT_OP_CODE(pCode)]; + Arg1 = SHORT_ARG(pCode) + pCode[1]; + Arg2 = pCode[1]; + } + else + { + opCode = OP_CODE(pCode); + Arg1 = pCode[1]; + Arg2 = pCode[2]; + } +#else + opCode = OP_CODE(pCode); + Arg1 = pCode[1]; + Arg2 = pCode[2]; +#endif //USE_SHORT_OPS + + if (opCode == OP_NEG || opCode == OP_NOT || opCode == OP_TST) + { + return cCmdInterpPolyUnop2(*pCode, Arg1, 0, Arg2, 0); + } + + switch (opCode) + { + case OP_MOV: + { + //!!! Optimized move for byte arrays (makes File I/O involving CStrs tolerable). Optimize for other cases? + if ((cCmdDSType(Arg1) == TC_ARRAY) && (cCmdDSType(INC_ID(Arg1)) == TC_UBYTE) && + (cCmdDSType(Arg2) == TC_ARRAY) && (cCmdDSType(INC_ID(Arg2)) == TC_UBYTE)) + { + Count = cCmdArrayCount(Arg2, 0); + Status = cCmdDSArrayAlloc(Arg1, 0, Count); + if (IS_ERR(Status)) + return Status; + + pArg1 = cCmdResolveDataArg(Arg1, 0, NULL); + pArg2 = cCmdResolveDataArg(Arg2, 0, NULL); + + memmove(pArg1, pArg2, Count); + } + else + { + Status = cCmdInterpPolyUnop2(OP_MOV, Arg1, 0, Arg2, 0); + } + } + break; + + case OP_SET: + { + //!!! Should throw error if TypeCode1 is non-scalar + // Accepting non-scalar destinations could have unpredictable results! + pArg1 = cCmdResolveDataArg(Arg1, 0, &TypeCode1); + cCmdSetVal(pArg1, TypeCode1, Arg2); + } + break; + + case OP_BRTST: + { + pArg2 = cCmdResolveDataArg(Arg2, 0, &TypeCode2); + if (cCmdCompare(COMP_CODE(pCode), (SLONG)cCmdGetVal(pArg2, TypeCode2), 0, TC_SLONG, TC_SLONG)) + { + VarsCmd.ScratchPC = VarsCmd.ScratchPC + (SWORD)Arg1; + Status = PC_OVERRIDE; + } + } + break; + + case OP_FINCLUMP: + { + cCmdDeQClump(&(VarsCmd.RunQ), VarsCmd.CallerClump); //Dequeue finalized clump + cCmdSchedDependents(VarsCmd.CallerClump, (SWORD)Arg1, (SWORD)Arg2); + + Status = CLUMP_DONE; + } + break; + + case OP_SUBCALL: + { + NXT_ASSERT(cCmdIsClumpIDSane((CLUMP_ID)Arg1)); + NXT_ASSERT(!cCmdIsClumpOnQ(&(VarsCmd.RunQ), (CLUMP_ID)Arg1)); + + NXT_ASSERT(cCmdIsDSElementIDSane(Arg2)); + + *((CLUMP_ID *)(cCmdDSPtr(Arg2, 0))) = VarsCmd.CallerClump; + + cCmdDeQClump(&(VarsCmd.RunQ), VarsCmd.CallerClump); //Take caller off RunQ + cCmdEnQClump(&(VarsCmd.RunQ), (CLUMP_ID)Arg1); //Add callee to RunQ + + Status = CLUMP_SUSPEND; + } + break; + + case OP_ARRSIZE: + { + pArg1 = cCmdResolveDataArg(Arg1, 0, &TypeCode1); + cCmdSetVal(pArg1, TypeCode1, cCmdArrayCount(Arg2, 0)); + } + break; + + case OP_SYSCALL: + { + if (Arg1 >= SYSCALL_COUNT) + { + NXT_BREAK; + Status = ERR_INSTR; + break; + } + + ArgC = cCmdClusterCount(Arg2); + + if (ArgC > MAX_CALL_ARGS) + { + NXT_BREAK; + Status = ERR_INSTR; + break; + } + + if (ArgC > 0) + { + Arg2 = INC_ID(Arg2); + + for (i = 0; i < ArgC; i++) + { + if (cCmdDSType(Arg2) == TC_ARRAY) + { + //Storing pointer to array's DV_INDEX + //!!! This resolve is different than cCmdDSPtr + // since SysCalls may need the DVIndex to re-alloc arrays + ArgV[i] = VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[Arg2].DSOffset; + } + else + { + ArgV[i] = cCmdDSPtr(Arg2, 0); + } + + //If any argument fails to resolve, return a fatal error. + if (ArgV[i] == NULL) + { + Status = ERR_BAD_PTR; + break; + } + + Arg2 = cCmdNextDSElement(Arg2); + } + } + else + { + i = 0; + } + + //ArgV list is null terminated + ArgV[i] = NULL; + + Status = (*SysCallFuncs[Arg1])(ArgV); + } + break; + + case OP_FLATTEN: + { + //Flatten Arg2 to a NULL terminated string + + //Assert that the destination is a string (array of bytes) + NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY); + NXT_ASSERT(cCmdDSType(INC_ID(Arg1)) == TC_UBYTE); + + Count = cCmdCalcFlattenedSize(Arg2, 0); + //Add room for NULL terminator + Count++; + Status = cCmdDSArrayAlloc(Arg1, 0, Count); + if (IS_ERR(Status)) + return Status; + + pArg1 = cCmdResolveDataArg(Arg1, 0, NULL); + Offset = 0; + + Status = cCmdFlattenToByteArray(pArg1, &Offset, Arg2, 0); + //Append NULL terminator + *((UBYTE *)pArg1 + Offset) = 0; + Offset++; + NXT_ASSERT(Offset == Count); + } + break; + + case OP_NUMTOSTRING: + { + //Assert that the destination is a string (array of bytes) + NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY); + NXT_ASSERT(cCmdDSType(INC_ID(Arg1)) == TC_UBYTE); + + pArg2 = cCmdResolveDataArg(Arg2, 0, &TypeCode2); + //Make sure we're trying to convert a scalar to a string + NXT_ASSERT(!IS_AGGREGATE_TYPE(TypeCode2)); + + ArgVal2 = cCmdGetVal(pArg2, TypeCode2); + + //Calculate size of array + if (ArgVal2 == 0) + Count = 1; + else + Count = 0; + + if (TypeCode2 == TC_SLONG || TypeCode2 == TC_SWORD || TypeCode2 == TC_SBYTE) + { + TmpSLong = (SLONG)ArgVal2; + //Add room for negative sign + if (TmpSLong < 0) + Count++; + + while (TmpSLong) + { + TmpSLong /= 10; + Count++; + } + } + else + { + TmpULong = ArgVal2; + while (TmpULong) + { + TmpULong /= 10; + Count++; + } + } + + //add room for NULL terminator + Count++; + + //Allocate array + Status = cCmdDSArrayAlloc(Arg1, 0, Count); + if (IS_ERR(Status)) + return Status; + + pArg1 = cCmdResolveDataArg(Arg1, 0, &TypeCode1); + + //Populate array + if (TypeCode2 == TC_SLONG || TypeCode2 == TC_SWORD || TypeCode2 == TC_SBYTE) + { + sprintf(pArg1, "%d", (SLONG)ArgVal2); + } + else + { + sprintf(pArg1, "%u", ArgVal2); + } + } + break; + + case OP_STRTOBYTEARR: + { + NXT_ASSERT((cCmdDSType(Arg1) == TC_ARRAY) && (cCmdDSType(INC_ID(Arg1)) == TC_UBYTE)); + NXT_ASSERT((cCmdDSType(Arg2) == TC_ARRAY) && (cCmdDSType(INC_ID(Arg2)) == TC_UBYTE)); + + Count = cCmdArrayCount(Arg2, 0); + + if (Count > 0) + { + Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)(Count - 1)); + if (IS_ERR(Status)) + return Status; + + pArg1 = cCmdResolveDataArg(Arg1, 0, NULL); + pArg2 = cCmdResolveDataArg(Arg2, 0, NULL); + + memmove(pArg1, pArg2, Count - 1); + } + } + break; + + case OP_BYTEARRTOSTR: + { + NXT_ASSERT((cCmdDSType(Arg1) == TC_ARRAY) && (cCmdDSType(INC_ID(Arg1)) == TC_UBYTE)); + NXT_ASSERT((cCmdDSType(Arg2) == TC_ARRAY) && (cCmdDSType(INC_ID(Arg2)) == TC_UBYTE)); + + Count = cCmdArrayCount(Arg2, 0); + + Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)(Count + 1)); + if (IS_ERR(Status)) + return Status; + + pArg1 = cCmdResolveDataArg(Arg1, 0, NULL); + pArg2 = cCmdResolveDataArg(Arg2, 0, NULL); + + memmove(pArg1, pArg2, Count); + *((UBYTE *)pArg1 + Count) = '\0'; + } + break; + + default: + { + //Fatal error: Unrecognized instruction + NXT_BREAK; + Status = ERR_INSTR; + } + break; + } + + return (Status); +} + + +NXT_STATUS cCmdInterpPolyUnop2(CODE_WORD const Code, DATA_ARG Arg1, UWORD Offset1, DATA_ARG Arg2, UWORD Offset2) +{ + NXT_STATUS Status = NO_ERR; + TYPE_CODE TypeCode1, TypeCode2; + DV_INDEX DVIndex1, DVIndex2; + ULONG ArgVal1, ArgVal2; + UWORD Count1, Count2; + UWORD MinArrayCount; + UWORD i; + //!!! AdvCluster is intended to catch the case where sources are Cluster and an Array of Clusters. + // In practice, the logic it uses is broken, leading to some source cluster elements being ignored. + UBYTE AdvCluster; + + void * pArg1 = NULL, + *pArg2 = NULL; + + TypeCode1 = cCmdDSType(Arg1); + TypeCode2 = cCmdDSType(Arg2); + + //Simple case, scalar. Solve and return. + if (!IS_AGGREGATE_TYPE(TypeCode2)) + { + NXT_ASSERT(!IS_AGGREGATE_TYPE(TypeCode1)); + + pArg1 = cCmdResolveDataArg(Arg1, Offset1, &TypeCode1); + pArg2 = cCmdResolveDataArg(Arg2, Offset2, &TypeCode2); + + ArgVal2 = cCmdGetVal(pArg2, TypeCode2); + ArgVal1 = cCmdUnop2(Code, ArgVal2, TypeCode2); + cCmdSetVal(pArg1, TypeCode1, ArgVal1); + return Status; + } + + //At least one of the args is an aggregate type + + // + // Initialize Count and ArrayType local variables for each argument + // + + if (TypeCode2 == TC_ARRAY) + { + Count2 = cCmdArrayCount(Arg2, Offset2); + DVIndex2 = cCmdGetDVIndex(Arg2, Offset2); + Offset2 = DV_ARRAY[DVIndex2].Offset; + } + else if (TypeCode2 == TC_CLUSTER) + { + Count2 = cCmdClusterCount(Arg2); + } + + if (TypeCode1 == TC_ARRAY) + { + if (TypeCode2 != TC_ARRAY) + { + //If output is an array, but source is not an array, that's a fatal error! + NXT_BREAK; + return (ERR_ARG); + } + + MinArrayCount = Count2; + + //Make sure the destination array is the proper size to hold the result + Status = cCmdDSArrayAlloc(Arg1, Offset1, MinArrayCount); + if (IS_ERR(Status)) + return Status; + + Count1 = MinArrayCount; + DVIndex1 = cCmdGetDVIndex(Arg1, Offset1); + Offset1 = DV_ARRAY[DVIndex1].Offset; + AdvCluster = FALSE; + } + else if (TypeCode1 == TC_CLUSTER) + { + Count1 = cCmdClusterCount(Arg1); + AdvCluster = TRUE; + } + + //Advance aggregate args to first sub-element for next call + if (IS_AGGREGATE_TYPE(TypeCode1)) + Arg1 = INC_ID(Arg1); + if (IS_AGGREGATE_TYPE(TypeCode2)) + Arg2 = INC_ID(Arg2); + + // + // Loop through the sub-elements of aggregate arguments. + // Call cCmdInterpPolyUnop2 recursively with simpler type. + // + + for (i = 0; i < Count1; i++) + { + Status = cCmdInterpPolyUnop2(Code, Arg1, Offset1, Arg2, Offset2); + if (IS_ERR(Status)) + return Status; + + //Advance aggregate args to next sub-element + if (TypeCode1 == TC_ARRAY) + Offset1 += DV_ARRAY[DVIndex1].ElemSize; + else if ((TypeCode1 == TC_CLUSTER) && AdvCluster) + Arg1 = cCmdNextDSElement(Arg1); + + if (TypeCode2 == TC_ARRAY) + Offset2 += DV_ARRAY[DVIndex2].ElemSize; + else if ((TypeCode2 == TC_CLUSTER) && AdvCluster) + Arg2 = cCmdNextDSElement(Arg2); + } + + return Status; +} + + +ULONG cCmdUnop2(CODE_WORD const Code, ULONG Operand, TYPE_CODE TypeCode) +{ + UBYTE opCode; + + opCode = OP_CODE((&Code)); + + switch (opCode) + { + case OP_MOV: + { + return Operand; + } + + case OP_NEG: + { + return (-((SLONG)Operand)); + } + + case OP_NOT: + { + //!!! OP_NOT is logical, *not* bit-wise. + //This differs from the other logical ops because we don't distinguish booleans from UBYTEs. + return (!Operand); + } + + case OP_TST: + { + return cCmdCompare(COMP_CODE((&Code)), Operand, 0, TypeCode, TypeCode); + } + + default: + { + //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller) + NXT_BREAK; + return 0; + } + } +} + + +NXT_STATUS cCmdInterpBinop(CODE_WORD * const pCode) +{ + NXT_STATUS Status = NO_ERR; + UBYTE opCode; + DATA_ARG Arg1, Arg2, Arg3; + TYPE_CODE TypeCode1, TypeCode2, TypeCode3; + ULONG ArgVal2, ArgVal3; + UBYTE CmpBool; + DV_INDEX DVIndex1, DVIndex2; + UWORD i; + + void * pArg1 = NULL, + *pArg2 = NULL, + *pArg3 = NULL; + + NXT_ASSERT(pCode != NULL); +#ifdef USE_SHORT_OPS + if (IS_SHORT_OP(pCode)) + { + //add mapping from quick op to real op + opCode = ShortOpMap[SHORT_OP_CODE(pCode)]; + Arg1 = SHORT_ARG(pCode) + pCode[1]; + Arg2 = pCode[1]; + Arg3 = pCode[2]; + } + else + { + opCode = OP_CODE(pCode); + Arg1 = pCode[1]; + Arg2 = pCode[2]; + Arg3 = pCode[3]; + } +#else + opCode = OP_CODE(pCode); + Arg1 = pCode[1]; + Arg2 = pCode[2]; + Arg3 = pCode[3]; +#endif //USE_SHORT_OPS + + if (opCode == OP_ADD || opCode == OP_SUB || opCode == OP_MUL || opCode == OP_DIV || opCode == OP_MOD || + opCode == OP_AND || opCode == OP_OR || opCode == OP_XOR) + { + return cCmdInterpPolyBinop(opCode, Arg1, 0, Arg2, 0, Arg3, 0); + } + + //Resolve data arguments, except for opcodes which the arguments are not DataArgs + if (opCode != OP_BRCMP) + { + pArg1 = cCmdResolveDataArg(Arg1, 0, &TypeCode1); + } + + if (opCode != OP_INDEX) + { + pArg2 = cCmdResolveDataArg(Arg2, 0, &TypeCode2); + ArgVal2 = cCmdGetVal(pArg2, TypeCode2); + } + + if ((opCode != OP_GETOUT) && (opCode != OP_SETIN) && (opCode != OP_GETIN) && (opCode != OP_INDEX) && (opCode != OP_ARRINIT)) + { + pArg3 = cCmdResolveDataArg(Arg3, 0, &TypeCode3); + ArgVal3 = cCmdGetVal(pArg3, TypeCode3); + } + + switch (opCode) + { + + case OP_CMP: + { + if (!IS_AGGREGATE_TYPE(cCmdDSType(Arg1)) && IS_AGGREGATE_TYPE(cCmdDSType(Arg2)) && IS_AGGREGATE_TYPE(cCmdDSType(Arg3))) + { + //Compare Aggregates + Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0); + cCmdSetVal(pArg1, TypeCode1, CmpBool); + } + else + { + //Compare Elements + Status = cCmdInterpPolyBinop(*pCode, Arg1, 0, Arg2, 0, Arg3, 0); + } + } + break; + + case OP_BRCMP: + { + //Compare Aggregates + Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0); + + if (CmpBool) + { + VarsCmd.ScratchPC = VarsCmd.ScratchPC + (SWORD)Arg1; + Status = PC_OVERRIDE; + } + } + break; + + case OP_GETOUT: + { + Arg2 = (UWORD)(0xC200 | (Arg3 + ArgVal2 * IO_OUT_FPP)); + pArg2 = cCmdResolveDataArg(Arg2, 0, &TypeCode2); + + cCmdSetVal(pArg1, TypeCode1, cCmdGetVal(pArg2, TypeCode2)); + } + break; + + //!!! All IO map access commands should screen illegal port values! + // Right now, cCmdResolveDataArg's implementation allows SETIN/GETIN to access arbitrary RAM! + case OP_SETIN: + { + Arg2 = (UWORD)(0xC000 | (Arg3 + ArgVal2 * IO_IN_FPP)); + pArg2 = cCmdResolveDataArg(Arg2, 0, &TypeCode2); + + cCmdSetVal(pArg2, TypeCode2, cCmdGetVal(pArg1, TypeCode1)); + } + break; + + case OP_GETIN: + { + Arg2 = (UWORD)(0xC000 | (Arg3 + ArgVal2 * IO_IN_FPP)); + pArg2 = cCmdResolveDataArg(Arg2, 0, &TypeCode2); + + cCmdSetVal(pArg1, TypeCode1, cCmdGetVal(pArg2, TypeCode2)); + } + break; + + case OP_INDEX: + { + if (Arg3 != NOT_A_DS_ID) + { + pArg3 = cCmdResolveDataArg(Arg3, 0, &TypeCode3); + ArgVal3 = cCmdGetVal(pArg3, TypeCode3); + } + else //Index input unwired + { + ArgVal3 = 0; + } + + DVIndex2 = cCmdGetDVIndex(Arg2, 0); + if (ArgVal3 >= DV_ARRAY[DVIndex2].Count) + return (ERR_ARG); + + Status = cCmdInterpPolyUnop2(OP_MOV, Arg1, 0, INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, ArgVal3)); + } + break; + + case OP_ARRINIT: + { + //Arg1 - Dst, Arg2 - element type/default val, Arg3 - length + + NXT_ASSERT(TypeCode1 == TC_ARRAY); + + if (Arg3 != NOT_A_DS_ID) + { + pArg3 = cCmdResolveDataArg(Arg3, 0, &TypeCode3); + ArgVal3 = cCmdGetVal(pArg3, TypeCode3); + } + else //Length input unwired + { + ArgVal3 = 0; + } + + Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)ArgVal3); + if (IS_ERR(Status)) + return Status; + + DVIndex1 = cCmdGetDVIndex(Arg1, 0); + for (i = 0; i < ArgVal3; i++) + { + //copy Arg2 into each element of Arg1 + Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, i), Arg2, 0); + } + } + break; + + default: + { + //Fatal error: Unrecognized instruction + NXT_BREAK; + Status = ERR_INSTR; + } + break; + } + + return (Status); +} + + +NXT_STATUS cCmdInterpPolyBinop(CODE_WORD const Code, DATA_ARG Arg1, UWORD Offset1, DATA_ARG Arg2, UWORD Offset2, DATA_ARG Arg3, UWORD Offset3) +{ + NXT_STATUS Status = NO_ERR; + TYPE_CODE TypeCode1, TypeCode2, TypeCode3; + DV_INDEX DVIndex1, DVIndex2, DVIndex3; + ULONG ArgVal1, ArgVal2, ArgVal3; + UWORD Count1, Count2, Count3; + UWORD MinArrayCount; + UWORD i; + //!!! AdvCluster is intended to catch the case where sources are Cluster and an Array of Clusters. + // In practice, the logic it uses is broken, leading to some source cluster elements being ignored. + UBYTE AdvCluster; + + void * pArg1 = NULL, + *pArg2 = NULL, + *pArg3 = NULL; + + TypeCode1 = cCmdDSType(Arg1); + TypeCode2 = cCmdDSType(Arg2); + TypeCode3 = cCmdDSType(Arg3); + + //Simple case, both args are scalars. Solve and return. + if ((!IS_AGGREGATE_TYPE(TypeCode2)) && (!IS_AGGREGATE_TYPE(TypeCode3))) + { + NXT_ASSERT(!IS_AGGREGATE_TYPE(TypeCode1)); + + pArg1 = cCmdResolveDataArg(Arg1, Offset1, &TypeCode1); + pArg2 = cCmdResolveDataArg(Arg2, Offset2, &TypeCode2); + pArg3 = cCmdResolveDataArg(Arg3, Offset3, &TypeCode3); + + ArgVal2 = cCmdGetVal(pArg2, TypeCode2); + ArgVal3 = cCmdGetVal(pArg3, TypeCode3); + ArgVal1 = cCmdBinop(Code, ArgVal2, ArgVal3, TypeCode2, TypeCode3); + cCmdSetVal(pArg1, TypeCode1, ArgVal1); + return Status; + } + + //At least one of the args is an aggregate type + + // + // Initialize Count and ArrayType local variables for each argument + // + + if (TypeCode2 == TC_ARRAY) + { + Count2 = cCmdArrayCount(Arg2, Offset2); + DVIndex2 = cCmdGetDVIndex(Arg2, Offset2); + Offset2 = DV_ARRAY[DVIndex2].Offset; + } + else if (TypeCode2 == TC_CLUSTER) + { + Count2 = cCmdClusterCount(Arg2); + } + + if (TypeCode3 == TC_ARRAY) + { + Count3 = cCmdArrayCount(Arg3, Offset3); + DVIndex3 = cCmdGetDVIndex(Arg3, Offset3); + Offset3 = DV_ARRAY[DVIndex3].Offset; + } + else if (TypeCode3 == TC_CLUSTER) + { + Count3 = cCmdClusterCount(Arg3); + } + + + if (TypeCode1 == TC_ARRAY) + { + //If the destination is an array, make sure it has enough memory to hold the result + if ((TypeCode2 == TC_ARRAY) && (TypeCode3 == TC_ARRAY)) + { + if (Count2 < Count3) + MinArrayCount = Count2; + else + MinArrayCount = Count3; + } + else if (TypeCode2 == TC_ARRAY) + MinArrayCount = Count2; + else if (TypeCode3 == TC_ARRAY) + MinArrayCount = Count3; + else + { + //If output is an array, but no sources are arrays, that's a fatal error! + NXT_BREAK; + return (ERR_ARG); + } + + //Make sure the destination array is the proper size to hold the result + Status = cCmdDSArrayAlloc(Arg1, Offset1, MinArrayCount); + if (IS_ERR(Status)) + return Status; + + Count1 = MinArrayCount; + DVIndex1 = cCmdGetDVIndex(Arg1, Offset1); + Offset1 = DV_ARRAY[DVIndex1].Offset; + AdvCluster = FALSE; + } + else if (TypeCode1 == TC_CLUSTER) + { + Count1 = cCmdClusterCount(Arg1); + AdvCluster = TRUE; + } + + //Advance aggregate args to first sub-element for next call + if (IS_AGGREGATE_TYPE(TypeCode1)) + Arg1 = INC_ID(Arg1); + if (IS_AGGREGATE_TYPE(TypeCode2)) + Arg2 = INC_ID(Arg2); + if (IS_AGGREGATE_TYPE(TypeCode3)) + Arg3 = INC_ID(Arg3); + + // + // Loop through the sub-elements of aggregate arguments. + // Call cCmdInterpPolyBinop recursively with simpler type. + // + + for (i = 0; i < Count1; i++) + { + Status = cCmdInterpPolyBinop(Code, Arg1, Offset1, Arg2, Offset2, Arg3, Offset3); + if (IS_ERR(Status)) + return Status; + + //Advance aggregate args to next sub-element + if (TypeCode1 == TC_ARRAY) + Offset1 += DV_ARRAY[DVIndex1].ElemSize; + else if ((TypeCode1 == TC_CLUSTER) && AdvCluster) + Arg1 = cCmdNextDSElement(Arg1); + + if (TypeCode2 == TC_ARRAY) + Offset2 += DV_ARRAY[DVIndex2].ElemSize; + else if ((TypeCode2 == TC_CLUSTER) && AdvCluster) + Arg2 = cCmdNextDSElement(Arg2); + + if (TypeCode3 == TC_ARRAY) + Offset3 += DV_ARRAY[DVIndex3].ElemSize; + else if ((TypeCode3 == TC_CLUSTER) && AdvCluster) + Arg3 = cCmdNextDSElement(Arg3); + } + + return Status; +} + + +ULONG cCmdBinop(CODE_WORD const Code, ULONG LeftOp, ULONG RightOp, TYPE_CODE LeftType, TYPE_CODE RightType) +{ + UBYTE opCode; + + opCode = OP_CODE((&Code)); + + switch (opCode) + { + case OP_ADD: + { + return LeftOp + RightOp; + } + + case OP_SUB: + { + return LeftOp - RightOp; + } + + case OP_MUL: + { + return LeftOp * RightOp; + } + + case OP_DIV: + { + //Catch divide-by-zero for a portable, well-defined result. + //(x / 0) = 0. Thus Spake LOTHAR!! (It's technical.) + if (RightOp == 0) + return 0; + + if (IS_SIGNED_TYPE(LeftType) && IS_SIGNED_TYPE(RightType)) + return ((SLONG)LeftOp) / ((SLONG)RightOp); + else if (IS_SIGNED_TYPE(LeftType)) + return ((SLONG)LeftOp) / RightOp; + else if (IS_SIGNED_TYPE(RightType)) + return LeftOp / ((SLONG)RightOp); + else + return LeftOp / RightOp; + } + + case OP_MOD: + { + //As with OP_DIV, make sure (x % 0) = x is well-defined + if (RightOp == 0) + return (LeftOp); + + if (IS_SIGNED_TYPE(LeftType) && IS_SIGNED_TYPE(RightType)) + return ((SLONG)LeftOp) % ((SLONG)RightOp); + else if (IS_SIGNED_TYPE(LeftType)) + return ((SLONG)LeftOp) % RightOp; + else if (IS_SIGNED_TYPE(RightType)) + return LeftOp % ((SLONG)RightOp); + else + return LeftOp % RightOp; + } + + case OP_AND: + { + return (LeftOp & RightOp); + } + + case OP_OR: + { + return (LeftOp | RightOp); + } + + case OP_XOR: + { + return ((LeftOp | RightOp) & (~(LeftOp & RightOp))); + } + + case OP_CMP: + { + return cCmdCompare(COMP_CODE((&Code)), LeftOp, RightOp, LeftType, RightType); + } + + default: + { + //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller) + NXT_BREAK; + return 0; + } + } +} + +NXT_STATUS cCmdInterpNoArg(CODE_WORD * const pCode) +{ + //Fatal error: Unrecognized instruction (no current opcodes have zero instructions) + NXT_BREAK; + return (ERR_INSTR); +} + + +//OP_SETOUT gets it's own interpreter function because it is relatively complex +// (called from cCmdInterpOther()) +//This also serves as a convenient breakpoint stop for investigating output module behavior +NXT_STATUS cCmdExecuteSetOut(CODE_WORD * const pCode) +{ + TYPE_CODE TypeCodeField, TypeCodeSrc, TypeCodePortArg; + void *pField = NULL, + *pSrc = NULL, + *pPort = NULL; + DS_ELEMENT_ID PortArg; + UWORD PortCount, InstrSize; + ULONG Port, FieldTableIndex, i, j; + DV_INDEX DVIndex; + + //Arg1 = InstrSize + //Arg2 = port number or list of ports + //Arg3 and beyond = FieldID, src DSID tuples + + //Calculate number of tuples + //!!! Might want to throw ERR_INSTR if instrSize and tuples don't check out + InstrSize = (pCode[1] / 2); + + //Second argument may specify a single port or an array list. + //Figure out which and resolve accordingly. + PortArg = pCode[2]; + TypeCodePortArg = cCmdDSType(PortArg); + + if (TypeCodePortArg == TC_ARRAY) + { + DVIndex = cCmdGetDVIndex(PortArg, 0); + PortCount = cCmdArrayCount(PortArg, 0); + } + else + PortCount = 1; + + //For each port, process all the tuples + for (i = 0; i < PortCount; i++) + { + if (TypeCodePortArg == TC_ARRAY) + { + pPort = (UBYTE*)cCmdResolveDataArg(INC_ID(PortArg), ARRAY_ELEM_OFFSET(DVIndex, i), NULL); + Port = cCmdGetVal(pPort, cCmdDSType(INC_ID(PortArg))); + } + else + { + pPort = (UBYTE*)cCmdResolveDataArg(PortArg, 0, NULL); + Port = cCmdGetVal(pPort, TypeCodePortArg); + } + + //If user specified a valid port, process the tuples. Else, this port is a no-op + if (Port < NO_OF_OUTPUTS) + { + for (j = 3; j < InstrSize; j += 2) + { + FieldTableIndex = (Port * IO_OUT_FPP) + pCode[j]; + pSrc = cCmdResolveDataArg(pCode[j + 1], 0, &TypeCodeSrc); + + //If FieldTableIndex is valid, go ahead and set the value + if (FieldTableIndex < IO_OUT_FIELD_COUNT) + { + pField = IO_PTRS[MOD_OUTPUT][FieldTableIndex]; + TypeCodeField = IO_TYPES[MOD_OUTPUT][FieldTableIndex]; + cCmdSetVal(pField, TypeCodeField, cCmdGetVal(pSrc, TypeCodeSrc)); + } + //Else, compiler is nutso! Return fatal error. + else + return (ERR_INSTR); + } + } + } + + return (NO_ERR); +} + + +NXT_STATUS cCmdInterpOther(CODE_WORD * const pCode) +{ + NXT_STATUS Status = NO_ERR; + UBYTE opCode; + DATA_ARG Arg1, Arg2, Arg3, Arg4, Arg5; + TYPE_CODE TypeCode1, TypeCode2, TypeCode3, TypeCode4, TypeCode5; + ULONG ArgVal1, ArgVal2, ArgVal3, ArgVal4, ArgVal5; + UWORD ArrayCount1, ArrayCount2, ArrayCount3, ArrayCount4; + UWORD MinCount; + UWORD i,j; + DV_INDEX DVIndex1, DVIndex2, DVIndex4,TmpDVIndex; + UWORD SrcCount; + DS_ELEMENT_ID TmpDSID; + UWORD DstIndex; + UWORD Size; + UWORD Offset; + + void *pArg1 = NULL; + void *pArg2 = NULL; + void *pArg3 = NULL; + void *pArg4 = NULL; + void *pArg5 = NULL; + + NXT_ASSERT(pCode != NULL); + + opCode = OP_CODE(pCode); + + switch (opCode) + { + + case OP_REPLACE: + { + //Arg1 - Dst + //Arg2 - Src + //Arg3 - Index + //Arg4 - New val / array of vals + + Arg1 = pCode[1]; + Arg2 = pCode[2]; + Arg3 = pCode[3]; + Arg4 = pCode[4]; + + NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY); + NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY); + + //Copy Src to Dst + //!!! Could avoid full data copy if we knew which portion to overwrite + if (Arg1 != Arg2) + { + Status = cCmdInterpPolyUnop2(OP_MOV, Arg1, 0, Arg2, 0); + if (IS_ERR(Status)) + return Status; + } + + DVIndex1 = cCmdGetDVIndex(Arg1, 0); + //Copy new val to Dst + if (Arg3 != NOT_A_DS_ID) + { + pArg3 = cCmdResolveDataArg(Arg3, 0, &TypeCode3); + ArgVal3 = cCmdGetVal(pArg3, TypeCode3); + } + else + { + //Index input unwired + ArgVal3 = 0; + } + + ArrayCount1 = cCmdArrayCount(Arg1, 0); + //Bounds check + //If array index (ArgVal3) is out of range, just pass out the copy of Src (effectively no-op) + if (ArgVal3 >= ArrayCount1) + return (NO_ERR); + + if (cCmdDSType(Arg4) != TC_ARRAY) + { + Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, ArgVal3), Arg4, 0); + if (IS_ERR(Status)) + return Status; + } + else + { + DVIndex4 = cCmdGetDVIndex(Arg4, 0); + + ArrayCount4 = cCmdArrayCount(Arg4, 0); + if (ArrayCount1 - ArgVal3 < ArrayCount4) + MinCount = (UWORD)(ArrayCount1 - ArgVal3); + else + MinCount = ArrayCount4; + + for (i = 0; i < MinCount; i++) + { + Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, ArgVal3 + i), INC_ID(Arg4), ARRAY_ELEM_OFFSET(DVIndex4, i)); + if (IS_ERR(Status)) + return Status; + } + } + } + break; + + case OP_ARRSUBSET: + { + //Arg1 - Dst + //Arg2 - Src + //Arg3 - Index + //Arg4 - Length + + Arg1 = pCode[1]; + Arg2 = pCode[2]; + Arg3 = pCode[3]; + Arg4 = pCode[4]; + + NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY); + NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY); + + ArrayCount2 = cCmdArrayCount(Arg2, 0); + + if (Arg3 != NOT_A_DS_ID) + { + pArg3 = cCmdResolveDataArg(Arg3, 0, &TypeCode3); + ArgVal3 = cCmdGetVal(pArg3, TypeCode3); + } + else //Index input unwired + { + ArgVal3 = 0; + } + + if (Arg4 != NOT_A_DS_ID) + { + pArg4 = cCmdResolveDataArg(Arg4, 0, &TypeCode4); + ArgVal4 = cCmdGetVal(pArg4, TypeCode4); + } + else //Length input unwired, set to "rest" + { + ArgVal4 = (UWORD)(ArrayCount2 - ArgVal3); + } + + //Bounds check + if (ArgVal3 > ArrayCount2) + { + //Illegal range - return empty subset + Status = cCmdDSArrayAlloc(Arg1, 0, 0); + return Status; + } + + //Set MinCount to "rest" + MinCount = (UWORD)(ArrayCount2 - ArgVal3); + + // Copy "Length" if it is less than "rest" + if (ArgVal4 < (ULONG)MinCount) + MinCount = (UWORD)ArgVal4; + + //Allocate Dst array + Status = cCmdDSArrayAlloc(Arg1, 0, MinCount); + if (IS_ERR(Status)) + return Status; + + DVIndex1 = cCmdGetDVIndex(Arg1, 0); + DVIndex2 = cCmdGetDVIndex(Arg2, 0); + + //Move src subset to dst + for (i = 0; i < MinCount; i++) + { + Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, i), INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, ArgVal3 + i)); + if (IS_ERR(Status)) + return Status; + } + } + break; + + case OP_STRSUBSET: + { + //Arg1 - Dst + //Arg2 - Src + //Arg3 - Index + //Arg4 - Length + + Arg1 = pCode[1]; + Arg2 = pCode[2]; + Arg3 = pCode[3]; + Arg4 = pCode[4]; + + NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY); + NXT_ASSERT(cCmdDSType(INC_ID(Arg1)) == TC_UBYTE); + NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY); + NXT_ASSERT(cCmdDSType(INC_ID(Arg2)) == TC_UBYTE); + + ArrayCount2 = cCmdArrayCount(Arg2, 0); + + //Remove NULL from Count + ArrayCount2--; + + if (Arg3 != NOT_A_DS_ID) + { + pArg3 = cCmdResolveDataArg(Arg3, 0, &TypeCode3); + ArgVal3 = cCmdGetVal(pArg3, TypeCode3); + } + else //Index input unwired + { + ArgVal3 = 0; + } + + if (Arg4 != NOT_A_DS_ID) + { + pArg4 = cCmdResolveDataArg(Arg4, 0, &TypeCode4); + ArgVal4 = cCmdGetVal(pArg4, TypeCode4); + } + else //Length input unwired, set to "rest" + { + ArgVal4 = (UWORD)(ArrayCount2 - ArgVal3); + } + + //Bounds check + if (ArgVal3 > ArrayCount2) + { + //Illegal range - return empty string + Status = cCmdDSArrayAlloc(Arg1, 0, 1); + if (!IS_ERR(Status)) + { + pArg1 = cCmdResolveDataArg(Arg1, 0, NULL); + *((UBYTE *)pArg1) = '\0'; + } + return Status; + } + + //Set MinCount to "rest" + MinCount = (UWORD)(ArrayCount2 - ArgVal3); + + // Copy "Length" if it is less than "rest" + if (ArgVal4 < (ArrayCount2 - ArgVal3)) + MinCount = (UWORD)ArgVal4; + + //Allocate Dst array + Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)(MinCount + 1)); + if (IS_ERR(Status)) + return Status; + + pArg1 = cCmdResolveDataArg(Arg1, 0, NULL); + pArg2 = cCmdResolveDataArg(Arg2, 0, NULL); + + //Move src subset to dst + memmove((UBYTE *)pArg1, (UBYTE *)pArg2 + ArgVal3, MinCount); + + //Append NULL terminator to Dst + *((UBYTE *)pArg1 + MinCount) = '\0'; + + } + break; + + case OP_SETOUT: + { + Status = cCmdExecuteSetOut(pCode); + } + break; + + case OP_ARRBUILD: + { + // Arg1 - Instruction Size in bytes + // Arg2 - Dst + // Arg3-N - Srcs + + Arg2 = pCode[2]; + + NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY); + + //Number of Srcs = total code words - 3 (account for opcode word, size, and Dst) + //!!! Argument access like this is potentially unsafe. + //A function/macro which checks proper encoding would be better + SrcCount = (pCode[1] / 2) - 3; + + //Calculate Dst array count + ArrayCount2 = 0; + for (i = 0; i < SrcCount; i++) + { + TmpDSID = pCode[3 + i]; + NXT_ASSERT(cCmdIsDSElementIDSane(TmpDSID)); + + //If the type descriptors are the same, then the input is an array, not a single element + if (cCmdCompareDSType(Arg2, TmpDSID)) + { + NXT_ASSERT(cCmdDSType(TmpDSID) == TC_ARRAY); + ArrayCount2 += cCmdArrayCount(TmpDSID, 0); + } + else + { + //Assert that the output is an array of this input type + NXT_ASSERT(cCmdCompareDSType(INC_ID(Arg2), TmpDSID)); + ArrayCount2++; + } + } + + //Allocate Dst array + Status = cCmdDSArrayAlloc(Arg2, 0, ArrayCount2); + if (IS_ERR(Status)) + return Status; + + DVIndex2 = cCmdGetDVIndex(Arg2, 0); + + //Move Src(s) to Dst + DstIndex = 0; + for (i = 0; i < SrcCount; i++) + { + TmpDSID = pCode[3 + i]; + + //If the type descriptors are the same, then the input is an array, not a single element + if (cCmdCompareDSType(Arg2, TmpDSID)) + { + NXT_ASSERT(cCmdDSType(TmpDSID) == TC_ARRAY); + TmpDVIndex = cCmdGetDVIndex(TmpDSID, 0); + for (j = 0; j < DV_ARRAY[TmpDVIndex].Count; j++) + { + Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, DstIndex), INC_ID(TmpDSID), ARRAY_ELEM_OFFSET(TmpDVIndex, j)); + if (IS_ERR(Status)) + return Status; + DstIndex++; + } + } + else + { + //Assert that the output is an array of this input type + NXT_ASSERT(cCmdCompareDSType(INC_ID(Arg2), TmpDSID)); + Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, DstIndex), TmpDSID, 0); + if (IS_ERR(Status)) + return Status; + DstIndex++; + } + } + + NXT_ASSERT(DstIndex == ArrayCount2); + } + break; + + case OP_STRCAT: + { + // Arg1 - Instruction Size in bytes + // Arg2 - Dst + // Arg3-N - Srcs + + Arg2 = pCode[2]; + + //Make sure Dst arg is a string + NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY); + NXT_ASSERT(cCmdDSType(INC_ID(Arg2)) == TC_UBYTE); + + //Number of Srcs = total code words - 3 (account for opcode word, size, and Dst) + //!!! Argument access like this is potentially unsafe. + //A function/macro which checks proper encoding would be better + SrcCount = (pCode[1] / 2) - 3; + + //Calculate Dst array count + ArrayCount2 = 0; + for (i = 0; i < SrcCount; i++) + { + TmpDSID = pCode[3 + i]; + NXT_ASSERT(cCmdIsDSElementIDSane(TmpDSID)); + + //Make sure Src arg is a string + //!!! Type checks here should be richer to allow array of strings as input (match LabVIEW behavior) + NXT_ASSERT(cCmdDSType(TmpDSID) == TC_ARRAY); + + if (cCmdDSType(INC_ID(TmpDSID)) != TC_UBYTE) + { + NXT_BREAK; + return ERR_ARG; + } + + ArrayCount3 = cCmdArrayCount(TmpDSID, 0); + NXT_ASSERT(ArrayCount3 > 0); + //Subtract NULL terminator from Src array count + ArrayCount3--; + + //Increase Dst array count by Src array count + ArrayCount2 += ArrayCount3; + } + + //Add room for NULL terminator + ArrayCount2++; + + //Allocate Dst array + Status = cCmdDSArrayAlloc(Arg2, 0, ArrayCount2); + if (IS_ERR(Status)) + return Status; + + //Move Src(s) to Dst + DstIndex = 0; + pArg2 = cCmdResolveDataArg(Arg2, 0, NULL); + for (i = 0; i < SrcCount; i++) + { + TmpDSID = pCode[3 + i]; + + pArg3 = cCmdResolveDataArg(TmpDSID, 0, NULL); + + ArrayCount3 = cCmdArrayCount(TmpDSID, 0); + NXT_ASSERT(ArrayCount3 > 0); + //Subtract NULL terminator from Src array count + ArrayCount3--; + + memmove((UBYTE *)pArg2 + DstIndex, pArg3, ArrayCount3); + DstIndex += ArrayCount3; + } + + //Append NULL terminator to Dst + *((UBYTE *)pArg2 + DstIndex) = '\0'; + DstIndex++; + + NXT_ASSERT(DstIndex == ArrayCount2); + } + break; + + case OP_UNFLATTEN: + { + //Arg1 - Dst + //Arg2 - Err (output) + //Arg3 - Src (byte stream) + //Arg4 - Type + + //The Type arg is a preallocated structure of the exact size you + //want to unflatten into. This allows us to support unflattening arbitrary types. + + //!!! Currently, both outputs must have valid destinations. + // It would be trivial to handle NOT_A_DS_ID to avoid dummy + // allocations when outputs are unused. + + Arg1 = pCode[1]; + Arg2 = pCode[2]; + Arg3 = pCode[3]; + Arg4 = pCode[4]; + + //Move Type template to Dst + //This provides a default value for Dst and makes sure Dst is properly sized + Status = cCmdInterpPolyUnop2(OP_MOV, Arg1, 0, Arg4, 0); + if (IS_ERR(Status)) + return Status; + + //Resolve error data pointer + pArg2 = cCmdResolveDataArg(Arg2, 0, &TypeCode2); + + //Make sure Arg3 is a String + NXT_ASSERT(cCmdDSType(Arg3) == TC_ARRAY); + NXT_ASSERT(cCmdDSType(INC_ID(Arg3)) == TC_UBYTE); + + ArrayCount3 = cCmdArrayCount(Arg3, 0); + //Take NULL terminator out of count + ArrayCount3--; + + Size = cCmdCalcFlattenedSize(Arg4, 0); + + //Check that we have a proper type template to unflatten into + if (ArrayCount3 == Size) + { + pArg3 = cCmdResolveDataArg(Arg3, 0, NULL); + Offset = 0; + Status = cCmdUnflattenFromByteArray(pArg3, &Offset, Arg1, 0); + + //!!! Status ignored from cCmdUnflattenFromByteArray + // If future revisions of this function provide better error checking, + // Err arg should be conditionally set based on the result. + //Unflatten succeeded; set Err arg to FALSE + cCmdSetVal(pArg2, TypeCode2, FALSE); + + NXT_ASSERT(Offset == Size); + } + else + { + //Unflatten failed; set Err arg to TRUE + cCmdSetVal(pArg2, TypeCode2, TRUE); + } + } + break; + + case OP_STRINGTONUM: + { + + // Arg1 - Dst number (output) + // Arg2 - Offset past match (output) + // Arg3 - Src string + // Arg4 - Offset + // Arg5 - Default (type/value) + + //!!! Currently, both outputs must have valid destinations. + // It would be trivial to handle NOT_A_DS_ID to avoid dummy + // allocations when outputs are unused. + + Arg1 = pCode[1]; + Arg2 = pCode[2]; + Arg3 = pCode[3]; + Arg4 = pCode[4]; + Arg5 = pCode[5]; + + pArg1 = cCmdResolveDataArg(Arg1, 0, &TypeCode1); + pArg2 = cCmdResolveDataArg(Arg2, 0, &TypeCode2); + pArg3 = cCmdResolveDataArg(Arg3, 0, &TypeCode3); + + if (Arg4 != NOT_A_DS_ID) + { + pArg4 = cCmdResolveDataArg(Arg4, 0, &TypeCode4); + ArgVal4 = cCmdGetVal(pArg4, TypeCode4); + } + else //Offset input unwired + { + ArgVal4 = 0; + } + + if (Arg5 != NOT_A_DS_ID) + { + pArg5 = cCmdResolveDataArg(Arg5, 0, &TypeCode5); + ArgVal5 = cCmdGetVal(pArg5, TypeCode5); + } + else //Default input unwired + { + ArgVal5 = 0; + } + + //Read number from string + if (sscanf(((PSZ)pArg3 + ArgVal4), "%d", &ArgVal1) == 1) + { + i = (UWORD)ArgVal4; + //Scan until we see the number + while ((((UBYTE *)pArg3)[i] < '0') || (((UBYTE *)pArg3)[i] > '9')) + i++; + + //Scan until we get past the number + while ((((UBYTE *)pArg3)[i] >= '0') && (((UBYTE *)pArg3)[i] <= '9')) + i++; + + ArgVal2 = i; + } + else + { + //Number wasn't found in string, use defaults + ArgVal1 = ArgVal5; + ArgVal2 = 0; + } + + //Set outputs + cCmdSetVal(pArg1, TypeCode1, ArgVal1); + cCmdSetVal(pArg2, TypeCode2, ArgVal2); + } + break; + + default: + { + //Fatal error: Unrecognized instruction + NXT_BREAK; + Status = ERR_INSTR; + } + break; + } + + return (Status); +} + + +// +//Support functions for lowspeed (I2C devices, i.e. ultrasonic sensor) communications +// + +//Simple lookup table for pMapLowSpeed->ChannelState[Port] values +//This is used to keep VM status code handling consistent +//...and ChannelState gives us too much information, anyway... +static const NXT_STATUS MapLStoVMStat[6] = +{ + NO_ERR, //LOWSPEED_IDLE, + STAT_COMM_PENDING, //LOWSPEED_INIT, + STAT_COMM_PENDING, //LOWSPEED_LOAD_BUFFER, + STAT_COMM_PENDING, //LOWSPEED_COMMUNICATING, + ERR_COMM_BUS_ERR, //LOWSPEED_ERROR, + STAT_COMM_PENDING, //LOWSPEED_DONE (really means c_lowspeed state machine is resetting) +}; + + +//cCmdLSCheckStatus +//Check lowspeed port status, optionally returning bytes available in the buffer for reading +NXT_STATUS cCmdLSCheckStatus(UBYTE Port) +{ + if (Port >= NO_OF_LOWSPEED_COM_CHANNEL) + { + return (ERR_COMM_CHAN_INVALID); + } + + //If port is not configured properly ahead of time, report that error + //!!! This seems like the right policy, but may restrict otherwise valid read operations... + if (!(pMapInput->Inputs[Port].SensorType == LOWSPEED_9V || pMapInput->Inputs[Port].SensorType == LOWSPEED) + || !(pMapInput->Inputs[Port].InvalidData == FALSE)) + { + return (ERR_COMM_CHAN_NOT_READY); + } + + return (MapLStoVMStat[pMapLowSpeed->ChannelState[Port]]); +} + +//cCmdLSCalcBytesReady +//Calculate true number of bytes available in the inbound LS buffer +UBYTE cCmdLSCalcBytesReady(UBYTE Port) +{ + SLONG Tmp; + + //Expect callers to validate Port, but short circuit here to be safe. + if (Port >= NO_OF_LOWSPEED_COM_CHANNEL) + return 0; + + //Normally, bytes available is a simple difference. + Tmp = pMapLowSpeed->InBuf[Port].InPtr - pMapLowSpeed->InBuf[Port].OutPtr; + + //If InPtr is actually behind OutPtr, circular buffer has wrapped. Account for wrappage... + if (Tmp < 0) + Tmp = (pMapLowSpeed->InBuf[Port].InPtr + (SIZE_OF_LSBUF - pMapLowSpeed->InBuf[Port].OutPtr)); + + return (UBYTE)(Tmp); +} + +//cCmdLSWrite +//Write BufLength bytes into specified port's lowspeed buffer and kick off comm process to device +NXT_STATUS cCmdLSWrite(UBYTE Port, UBYTE BufLength, UBYTE *pBuf, UBYTE ResponseLength) +{ + if (Port >= NO_OF_LOWSPEED_COM_CHANNEL) + { + return (ERR_COMM_CHAN_INVALID); + } + + if (BufLength > SIZE_OF_LSBUF || ResponseLength > SIZE_OF_LSBUF) + { + return (ERR_INVALID_SIZE); + } + + //Only start writing process if port is properly configured and c_lowspeed module is ready + if ((pMapInput->Inputs[Port].SensorType == LOWSPEED_9V || pMapInput->Inputs[Port].SensorType == LOWSPEED) + && (pMapInput->Inputs[Port].InvalidData == FALSE) + && (pMapLowSpeed->ChannelState[Port] == LOWSPEED_IDLE) || (pMapLowSpeed->ChannelState[Port] == LOWSPEED_ERROR)) + { + pMapLowSpeed->OutBuf[Port].InPtr = 0; + pMapLowSpeed->OutBuf[Port].OutPtr = 0; + + memcpy(pMapLowSpeed->OutBuf[Port].Buf, pBuf, BufLength); + pMapLowSpeed->OutBuf[Port].InPtr = (UBYTE)BufLength; + + pMapLowSpeed->InBuf[Port].BytesToRx = ResponseLength; + + pMapLowSpeed->ChannelState[Port] = LOWSPEED_INIT; + pMapLowSpeed->State |= (COM_CHANNEL_ONE_ACTIVE << Port); + + return (NO_ERR); + } + else + { + //!!! Would be more consistent to return STAT_COMM_PENDING if c_lowspeed is busy + return (ERR_COMM_CHAN_NOT_READY); + } +} + + +//cCmdLSRead +//Read BufLength bytes from specified port's lowspeed buffer +NXT_STATUS cCmdLSRead(UBYTE Port, UBYTE BufLength, UBYTE * pBuf) +{ + UBYTE BytesReady, BytesToRead; + + if (Port >= NO_OF_LOWSPEED_COM_CHANNEL) + { + return (ERR_COMM_CHAN_INVALID); + } + + if (BufLength > SIZE_OF_LSBUF) + { + return (ERR_INVALID_SIZE); + } + + BytesReady = cCmdLSCalcBytesReady(Port); + + if (BufLength > BytesReady) + { + return (ERR_COMM_CHAN_NOT_READY); + } + + BytesToRead = BufLength; + + //If the bytes we want to read wrap around the end, we must first read the end, then reset back to the beginning + if (pMapLowSpeed->InBuf[Port].OutPtr + BytesToRead >= SIZE_OF_LSBUF) + { + BytesToRead = SIZE_OF_LSBUF - pMapLowSpeed->InBuf[Port].OutPtr; + memcpy(pBuf, pMapLowSpeed->InBuf[Port].Buf + pMapLowSpeed->InBuf[Port].OutPtr, BytesToRead); + pMapLowSpeed->InBuf[Port].OutPtr = 0; + pBuf += BytesToRead; + BytesToRead = BufLength - BytesToRead; + } + + memcpy(pBuf, pMapLowSpeed->InBuf[Port].Buf + pMapLowSpeed->InBuf[Port].OutPtr, BytesToRead); + pMapLowSpeed->InBuf[Port].OutPtr += BytesToRead; + + return (NO_ERR); +} + + +// +//Wrappers for OP_SYSCALL +// + +// +//cCmdWrapFileOpenRead +//ArgV[0]: (Function return) Loader status, U16 return +//ArgV[1]: File Handle, U8 return +//ArgV[2]: Filename, CStr +//ArgV[3]: Length, U32 return +NXT_STATUS cCmdWrapFileOpenRead(UBYTE * ArgV[]) +{ + LOADER_STATUS LStatus; + DV_INDEX DVIndex; + + //Resolve array argument + DVIndex = *(DV_INDEX *)(ArgV[2]); + ArgV[2] = cCmdDVPtr(DVIndex); + + LStatus = pMapLoader->pFunc(OPENREAD, ArgV[2], NULL, (ULONG *)ArgV[3]); + + //Add entry into FileHandleTable + if (LOADER_ERR(LStatus) == SUCCESS) + { + VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)][0] = 'r'; + strcpy((PSZ)(VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)] + 1), (PSZ)(ArgV[2])); + } + + //Status code in high byte of LStatus + *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus); + //File handle in low byte of LStatus + *(ArgV[1]) = LOADER_HANDLE(LStatus); + + return NO_ERR; +} + +//cCmdWrapFileOpenWrite +//ArgV[0]: (Function return) Loader status, U16 return +//ArgV[1]: File Handle, U8 return +//ArgV[2]: Filename, CStr +//ArgV[3]: Length, U32 return +NXT_STATUS cCmdWrapFileOpenWrite(UBYTE * ArgV[]) +{ + LOADER_STATUS LStatus; + DV_INDEX DVIndex; + + //Resolve array argument + DVIndex = *(DV_INDEX *)(ArgV[2]); + ArgV[2] = cCmdDVPtr(DVIndex); + + LStatus = pMapLoader->pFunc(OPENWRITEDATA, ArgV[2], NULL, (ULONG *)ArgV[3]); + + //Add entry into FileHandleTable + if (LOADER_ERR(LStatus) == SUCCESS) + { + VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)][0] = 'w'; + strcpy((PSZ)(VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)] + 1), (PSZ)(ArgV[2])); + } + + //Status code in high byte of LStatus + *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus); + //File handle in low byte of LStatus + *(ArgV[1]) = LOADER_HANDLE(LStatus); + + return NO_ERR; +} + +//cCmdWrapFileOpenAppend +//ArgV[0]: (Function return) Loader status, U16 return +//ArgV[1]: File Handle, U8 return +//ArgV[2]: Filename, CStr +//ArgV[3]: Length Remaining, U32 return +NXT_STATUS cCmdWrapFileOpenAppend(UBYTE * ArgV[]) +{ + LOADER_STATUS LStatus; + DV_INDEX DVIndex; + + //Resolve array argument + DVIndex = *(DV_INDEX *)(ArgV[2]); + ArgV[2] = cCmdDVPtr(DVIndex); + + LStatus = pMapLoader->pFunc(OPENAPPENDDATA, ArgV[2], NULL, (ULONG *)ArgV[3]); + + //Add entry into FileHandleTable + if (LOADER_ERR(LStatus) == SUCCESS) + { + VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)][0] = 'w'; + strcpy((PSZ)(VarsCmd.FileHandleTable[LOADER_HANDLE(LStatus)] + 1), (PSZ)(ArgV[2])); + } + + //Status code in high byte of LStatus + *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus); + //File handle in low byte of LStatus + *(ArgV[1]) = LOADER_HANDLE(LStatus); + + return NO_ERR; +} + +//cCmdWrapFileRead +//ArgV[0]: (Function return) Loader status, U16 return +//ArgV[1]: File Handle, U8 in/out +//ArgV[2]: Buffer, CStr out +//ArgV[3]: Length, U32 in/out +NXT_STATUS cCmdWrapFileRead(UBYTE * ArgV[]) +{ + NXT_STATUS Status = NO_ERR; + LOADER_STATUS LStatus; + DV_INDEX DVIndex; + + //Resolve array argument + DVIndex = *(DV_INDEX *)(ArgV[2]); + //Size Buffer to Length + //Add room for null terminator to length + Status = cCmdDVArrayAlloc(DVIndex, (UWORD)(*(ULONG *)ArgV[3] + 1)); + if (IS_ERR(Status)) + return Status; + + ArgV[2] = cCmdDVPtr(DVIndex); + LStatus = pMapLoader->pFunc(READ, ArgV[1], ArgV[2], (ULONG *)ArgV[3]); + + //Tack on NULL terminator + //Note that loader code may have adjusted length (*ArgV[3]) if all requested data was not available + //!!! Better solution would be to resize buffer to new length + 1, + // but then you must also be wary of side effects if resize allocation fails! + *(ArgV[2] + *(ULONG *)ArgV[3]) = '\0'; + + //Status code in high byte of LStatus + *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus); + //File handle in low byte of LStatus + *(ArgV[1]) = LOADER_HANDLE(LStatus); + + return Status; +} + +//cCmdWrapFileWrite +//ArgV[0]: (Function return) Loader status, U16 return +//ArgV[1]: File Handle, U8 in/out +//ArgV[2]: Buffer, CStr +//ArgV[3]: Length, U32 return +NXT_STATUS cCmdWrapFileWrite(UBYTE * ArgV[]) +{ + LOADER_STATUS LStatus; + DV_INDEX DVIndex; + + //Resolve array argument + DVIndex = *(DV_INDEX *)(ArgV[2]); + ArgV[2] = cCmdDVPtr(DVIndex); + + LStatus = pMapLoader->pFunc(WRITE, ArgV[1], ArgV[2], (ULONG *)ArgV[3]); + + //Status code in high byte of LStatus + *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus); + //File handle in low byte of LStatus + *(ArgV[1]) = LOADER_HANDLE(LStatus); + + return NO_ERR; +} + +//cCmdWrapFileClose +//ArgV[0]: (Function return) Loader status, U16 return +//ArgV[1]: File Handle, U8 +NXT_STATUS cCmdWrapFileClose(UBYTE * ArgV[]) +{ + LOADER_STATUS LStatus; + + //!!! This bounds check also exists in dLoaderCloseHandle(), but we provide an explicit error code + if (*(ArgV[1]) >= MAX_HANDLES) + { + *((UWORD *)ArgV[0]) = ILLEGALHANDLE; + return NO_ERR; + } + + LStatus = pMapLoader->pFunc(CLOSE, ArgV[1], NULL, NULL); + + //Clear entry in FileHandleTable + memset(VarsCmd.FileHandleTable[*(ArgV[1])], 0, FILENAME_LENGTH + 2); + + //Status code in high byte of LStatus + *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus); + + return NO_ERR; +} + +//cCmdWrapFileResolveHandle +//ArgV[0]: (Function return) Loader status, U16 return +//ArgV[1]: File Handle, U8 return +//ArgV[2]: Write Handle?, Bool return +//ArgV[3]: Filename, CStr +NXT_STATUS cCmdWrapFileResolveHandle (UBYTE * ArgV[]) +{ + UBYTE i; + DV_INDEX DVIndex; + + //Resolve array argument + DVIndex = *(DV_INDEX *)(ArgV[3]); + ArgV[3] = cCmdDVPtr(DVIndex); + + for (i = 0; i < MAX_HANDLES; i++) + { + if (strcmp((PSZ)(ArgV[3]), (PSZ)(VarsCmd.FileHandleTable[i] + 1)) == 0) + { + *(ArgV[2]) = (VarsCmd.FileHandleTable[i][0] == 'w'); + break; + } + } + + if (i == MAX_HANDLES) + { + i = NOT_A_HANDLE; + *((UWORD *)ArgV[0]) = HANDLEALREADYCLOSED; + } + else + { + *((UWORD *)ArgV[0]) = SUCCESS; + } + + *(ArgV[1]) = i; + + return NO_ERR; +} + + +//cCmdWrapFileRename +//ArgV[0]: (Function return) Loader status, U16 return +//ArgV[1]: Old Filename, CStr +//ArgV[2]: New Filename, CStr +NXT_STATUS cCmdWrapFileRename (UBYTE * ArgV[]) +{ + LOADER_STATUS LStatus; + ULONG Tmp; + DV_INDEX DVIndex; + + //Resolve array arguments + DVIndex = *(DV_INDEX *)(ArgV[1]); + ArgV[1] = cCmdDVPtr(DVIndex); + DVIndex = *(DV_INDEX *)(ArgV[2]); + ArgV[2] = cCmdDVPtr(DVIndex); + + //!!! Tmp placeholder passed into loader code to avoid illegal dereferencing. + LStatus = pMapLoader->pFunc(RENAMEFILE, ArgV[1], ArgV[2], &Tmp); + + //Status code in high byte of LStatus + *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus); + + return NO_ERR; +} + + +//cCmdWrapFileDelete +//ArgV[0]: (Function return) Loader status, U16 return +//ArgV[1]: Filename, CStr +NXT_STATUS cCmdWrapFileDelete (UBYTE * ArgV[]) +{ + LOADER_STATUS LStatus; + DV_INDEX DVIndex; + + //Resolve array arguments + DVIndex = *(DV_INDEX *)(ArgV[1]); + ArgV[1] = cCmdDVPtr(DVIndex); + + LStatus = pMapLoader->pFunc(DELETE, ArgV[1], NULL, NULL); + + //Status code in high byte of LStatus + *((UWORD *)ArgV[0]) = LOADER_ERR(LStatus); + + return NO_ERR; +} + +// +//cCmdWrapSoundPlayFile +//ArgV[0]: (Return value) Status code, SBYTE +//ArgV[1]: Filename, CStr +//ArgV[2]: Loop?, UBYTE (bool) +//ArgV[3]: Volume, UBYTE +// +NXT_STATUS cCmdWrapSoundPlayFile(UBYTE * ArgV[]) +{ + DV_INDEX DVIndex; + + //Resolve array arguments + DVIndex = *(DV_INDEX *)(ArgV[1]); + ArgV[1] = cCmdDVPtr(DVIndex); + + //!!! Should check filename and/or existence and return error before proceeding + strncpy((PSZ)(pMapSound->SoundFilename), (PSZ)(ArgV[1]), FILENAME_LENGTH); + + if (*(ArgV[2]) == TRUE) + pMapSound->Mode = SOUND_LOOP; + else + pMapSound->Mode = SOUND_ONCE; + + pMapSound->Volume = *(ArgV[3]); + //SampleRate of '0' means "let file specify SampleRate" + pMapSound->SampleRate = 0; + pMapSound->Flags |= SOUND_UPDATE; + + *((SBYTE*)(ArgV[0])) = (NO_ERR); + + return (NO_ERR); +} + +// +//cCmdWrapSoundPlayTone +//ArgV[0]: (Return value) Status code, SBYTE +//ArgV[1]: Frequency, UWORD +//ArgV[2]: Duration, UWORD +//ArgV[3]: Loop?, UBYTE (Boolean) +//ArgV[4]: Volume, UBYTE +// +NXT_STATUS cCmdWrapSoundPlayTone(UBYTE * ArgV[]) +{ + pMapSound->Freq = *(UWORD*)(ArgV[1]); + pMapSound->Duration = *(UWORD*)(ArgV[2]); + pMapSound->Volume = *(ArgV[4]); + pMapSound->Flags |= SOUND_UPDATE; + + if (*(ArgV[3]) == TRUE) + pMapSound->Mode = SOUND_TONE | SOUND_LOOP; + else + pMapSound->Mode = SOUND_TONE; + + *((SBYTE*)(ArgV[0])) = (NO_ERR); + + return (NO_ERR); +} + +// +//cCmdWrapSoundGetState +//ArgV[0]: (Return value) sound module state, UBYTE +//ArgV[1]: Flags, UBYTE +// +NXT_STATUS cCmdWrapSoundGetState(UBYTE * ArgV[]) +{ + *(ArgV[0]) = pMapSound->State; + *(ArgV[1]) = pMapSound->Flags; + return (NO_ERR); +} + +// +//cCmdWrapSoundSetState +//ArgV[0]: (Return value) sound module state, UBYTE +//ArgV[1]: State, UBYTE +//ArgV[2]: Flags, UBYTE +// +NXT_STATUS cCmdWrapSoundSetState(UBYTE * ArgV[]) +{ + pMapSound->State = *(ArgV[1]); + //Return same state we just set, mostly for interface consistency + *(ArgV[0]) = pMapSound->State; + + //OR in provided flags (usually 0) + pMapSound->Flags |= *(ArgV[2]); + + return (NO_ERR); +} + +// +//cCmdWrapReadButton +//ArgV[0]: (Function return) Status code, SBYTE +//ArgV[1]: Index (U8) +//ArgV[2]: Pressed (bool) +//ArgV[3]: Count (U8) (count of press-then-release cycles) +//ArgV[4]: ResetCount? (bool in) +// +NXT_STATUS cCmdWrapReadButton(UBYTE * ArgV[]) +{ + UBYTE btnIndex; + + btnIndex = *((UBYTE*)(ArgV[1])); + + if (btnIndex < NO_OF_BTNS) + { + //Set pressed boolean output + if (pMapButton->State[btnIndex] & PRESSED_STATE) + *(ArgV[2]) = TRUE; + else + *(ArgV[2]) = FALSE; + + //Set count output + *(ArgV[3]) = (UBYTE)(pMapButton->BtnCnt[btnIndex].RelCnt); + + //Optionally reset internal count + if (*(ArgV[4]) != 0) + { + pMapButton->BtnCnt[btnIndex].RelCnt = 0; + //Need to clear short and long counts too, because RelCnt depends on them. No known side effects. + pMapButton->BtnCnt[btnIndex].ShortRelCnt = 0; + pMapButton->BtnCnt[btnIndex].LongRelCnt = 0; + } + + // Set status code 'OK' + *((SBYTE*)(ArgV[0])) = NO_ERR; + } + else + { + //Bad button index specified, return error and default outputs + *((SBYTE*)(ArgV[0])) = ERR_INVALID_PORT; + *(ArgV[2]) = FALSE; + *(ArgV[3]) = 0; + } + + return (NO_ERR); +} + +// +//cCmdWrapCommLSWrite +//ArgV[0]: (return) Status code, SBYTE +//ArgV[1]: Port specifier, UBYTE +//ArgV[2]: Buffer to send, UBYTE array, only SIZE_OF_LSBUF bytes will be used +//ArgV[3]: ResponseLength, UBYTE, specifies expected bytes back from slave device +// +NXT_STATUS cCmdWrapCommLSWrite(UBYTE * ArgV[]) +{ + SBYTE * pReturnVal = (SBYTE*)(ArgV[0]); + UBYTE Port = *(ArgV[1]); + UBYTE * pBuf; + UWORD BufLength; + UBYTE ResponseLength = *(ArgV[3]); + DV_INDEX DVIndex; + + //Resolve array arguments + DVIndex = *(DV_INDEX *)(ArgV[2]); + pBuf = cCmdDVPtr(DVIndex); + BufLength = DV_ARRAY[DVIndex].Count; + + *pReturnVal = cCmdLSWrite(Port, (UBYTE)BufLength, pBuf, ResponseLength); + + return (NO_ERR); +} + +// +//cCmdWrapCommLSCheckStatus +//ArgV[0]: (return) Status code, SBYTE +//ArgV[1]: Port specifier, UBYTE +//ArgV[2]: BytesReady, UBYTE +// +NXT_STATUS cCmdWrapCommLSCheckStatus(UBYTE * ArgV[]) +{ + UBYTE Port = *(ArgV[1]); + + *((SBYTE*)(ArgV[0])) = cCmdLSCheckStatus(Port); + *((UBYTE*)(ArgV[2])) = cCmdLSCalcBytesReady(Port); + + return (NO_ERR); +} + +// +//cCmdWrapCommLSRead +//ArgV[0]: (return) Status code, SBYTE +//ArgV[1]: Port specifier, UBYTE +//ArgV[2]: Buffer for data, UBYTE array, max SIZE_OF_LSBUF bytes will be written +//ArgV[3]: BufferLength, UBYTE, specifies size of buffer requested +// +NXT_STATUS cCmdWrapCommLSRead(UBYTE * ArgV[]) +{ + SBYTE * pReturnVal = (SBYTE*)(ArgV[0]); + UBYTE Port = *(ArgV[1]); + UBYTE * pBuf; + UBYTE BufLength = *(ArgV[3]); + UBYTE BytesToRead; + DV_INDEX DVIndex = *(DV_INDEX *)(ArgV[2]); + NXT_STATUS AllocStatus; + + *pReturnVal = cCmdLSCheckStatus(Port); + BytesToRead = cCmdLSCalcBytesReady(Port); + + //If channel is OK and has data ready for us, put the data into outgoing buffer + if (!IS_ERR(*pReturnVal) && BytesToRead > 0) + { + //Limit buffer to available data + if (BufLength > BytesToRead) + BufLength = BytesToRead; + + AllocStatus = cCmdDVArrayAlloc(DVIndex, BufLength); + if (IS_ERR(AllocStatus)) + return (AllocStatus); + + pBuf = cCmdDVPtr(DVIndex); + *pReturnVal = cCmdLSRead(Port, BufLength, pBuf); + } + //Else, the channel has an error and/or there's no data to read; clear the output array + else + { + AllocStatus = cCmdDVArrayAlloc(DVIndex, 0); + if (IS_ERR(AllocStatus)) + return (AllocStatus); + } + + return (NO_ERR); +} + +// +//cCmdWrapRandomNumber +//ArgV[0]: (return) Random number, SWORD +// +NXT_STATUS cCmdWrapRandomNumber(UBYTE * ArgV[]) +{ + static UBYTE count = 0; + SWORD random; + + if (count == 0) + srand(dTimerRead()); + + if (count > 20) + count = 0; + else + count++; + + //!!! IAR's implementation of the rand() library function returns signed values, and we want it that way. + //Some stdlib implementations may return only positive numbers, so be wary if this code is ported. + random = rand(); + + *((SWORD *)ArgV[0]) = random; + + return NO_ERR; +} + +// +//cCmdWrapGetStartTick +//ArgV[0]: (return) Start Tick, ULONG +// +NXT_STATUS cCmdWrapGetStartTick(UBYTE * ArgV[]) +{ + *((ULONG *)ArgV[0]) = VarsCmd.StartTick; + return NO_ERR; +} + +// +//cCmdWrapMessageWrite +//ArgV[0]: (return) Error Code, SBYTE (NXT_STATUS) +//ArgV[1]: QueueID, UBYTE +//ArgV[2]: Message, CStr +// +NXT_STATUS cCmdWrapMessageWrite(UBYTE * ArgV[]) +{ + NXT_STATUS Status = NO_ERR; + DV_INDEX DVIndex; + + //Resolve array arguments + DVIndex = *(DV_INDEX *)(ArgV[2]); + ArgV[2] = cCmdDVPtr(DVIndex); + + Status = cCmdMessageWrite(*(UBYTE *)(ArgV[1]), ArgV[2], DV_ARRAY[DVIndex].Count); + + *(SBYTE *)(ArgV[0]) = Status; + + if (IS_FATAL(Status)) + return Status; + else + return (NO_ERR); +} + +#define UNPACK_STATUS(StatusWord) ((SBYTE)(StatusWord)) + +NXT_STATUS cCmdBTCheckStatus(UBYTE Connection) +{ + //If specified connection is invalid, return error code to the user. + if (Connection >= SIZE_OF_BT_CONNECT_TABLE) + { + return (ERR_INVALID_PORT); + } + + //INPROGRESS means a request is currently pending completion by the comm module + if (VarsCmd.CommStat == INPROGRESS) + { + return (STAT_COMM_PENDING); + } + //Translate BTBUSY to ERR_COMM_CHAN_NOT_READY + //And check if specified connection is indeed configured + else if (VarsCmd.CommStat == (SWORD)BTBUSY + || (pMapComm->BtConnectTable[Connection].Name[0]) == '\0') + { + return (ERR_COMM_CHAN_NOT_READY); + } + else + { + return (UNPACK_STATUS(VarsCmd.CommStat)); + } +} + +//Default packet to send for a remote MESSAGE_READ command. +//3rd byte must be replaced with remote mailbox (QueueID) +//4th byte must be replaced with local mailbox +static UBYTE RemoteMsgReadPacket[5] = {0x00, 0x13, 0xFF, 0xFF, 0x01}; + +// +//cCmdWrapMessageRead +//ArgV[0]: (return) Error Code, SBYTE (NXT_STATUS) +//ArgV[1]: QueueID, UBYTE +//ArgV[2]: Remove, UBYTE +//ArgV[3]: (return) Message, CStr +// +NXT_STATUS cCmdWrapMessageRead(UBYTE * ArgV[]) +{ + NXT_STATUS Status = NO_ERR; + NXT_STATUS AllocStatus = NO_ERR; + UBYTE QueueID = *(UBYTE *)(ArgV[1]); + DV_INDEX DestDVIndex = *(DV_INDEX *)(ArgV[3]); + UWORD MessageSize; + UBYTE i; + + NXT_ASSERT(IS_DV_INDEX_SANE(DestDVIndex)); + + //Check Next Message's size + Status = cCmdMessageGetSize(QueueID, &MessageSize); + + //If there is a valid message in local mailbox, read it + if (!IS_ERR(Status) && MessageSize > 0 ) + { + //!!! Also check for EMPTY_MAILBOX status? + //Size destination string + AllocStatus = cCmdDVArrayAlloc(DestDVIndex, MessageSize); + if (IS_ERR(AllocStatus)) + return AllocStatus; + + //Get Message + //!!! Should more aggressively enforce null termination before blindly copying to dataspace + Status = cCmdMessageRead(QueueID, cCmdDVPtr(DestDVIndex), MessageSize, *(ArgV[2])); + } + else + { + //Clear destination string + AllocStatus = cCmdDVArrayAlloc(DestDVIndex, 1); + if (IS_ERR(AllocStatus)) + return AllocStatus; + + //Successful allocation, make sure first byte is null terminator + *(UBYTE*)(cCmdDVPtr(DestDVIndex)) = '\0'; + } + + //If there were no local messages, see if there are any waiting in our slaves' outboxes + if (Status == STAT_MSG_EMPTY_MAILBOX && QueueID < INCOMING_QUEUE_COUNT) + { + //If there's an old error code hanging around, clear it before proceeding. + //!!! Clearing error here means bytecode status checking loops could get false SUCCESS results? + if (VarsCmd.CommStat < 0) + VarsCmd.CommStat = SUCCESS; + + //Search through possible slaves, looking for valid connection + for (i = 0; i < SIZE_OF_BT_CONNECT_TABLE - 1; i++) + { + //Advance CommCurrConnection and limit to 1, 2, or 3 (only slave connection slots are checked) + VarsCmd.CommCurrConnection++; + if (VarsCmd.CommCurrConnection == SIZE_OF_BT_CONNECT_TABLE) + VarsCmd.CommCurrConnection = 1; + + if (cCmdBTCheckStatus(VarsCmd.CommCurrConnection) == NO_ERR) + break; + } + + //If there is at least one configured slave connection, make a remote read request + if (i < SIZE_OF_BT_CONNECT_TABLE - 1) + { + //Outgoing QueueID on slave device is the local QueueID + INCOMING_QUEUE_COUNT + RemoteMsgReadPacket[2] = QueueID + INCOMING_QUEUE_COUNT; + RemoteMsgReadPacket[3] = QueueID; + + //Request comm module to send assembled packet and not go idle until response comes back (or error) + pMapComm->pFunc(SENDDATA, sizeof(RemoteMsgReadPacket), VarsCmd.CommCurrConnection, TRUE, RemoteMsgReadPacket, (UWORD*)&(VarsCmd.CommStat)); + + //Read status back after SENDDATA call so bytecode gets STAT_COMM_PENDING or error + Status = cCmdBTCheckStatus(VarsCmd.CommCurrConnection); + + //If our request was accepted, set the DirtyComm flag so stream will get cleaned up later + if (Status == STAT_COMM_PENDING) + VarsCmd.DirtyComm = TRUE; + } + } + + *(SBYTE *)(ArgV[0]) = Status; + if (IS_FATAL(Status)) + return Status; + else + return (NO_ERR); +} + + +// +//cCmdWrapCommBTCheckStatus +//ArgV[0]: (return) Status byte, SBYTE +//ArgV[1]: Connection index, 0-3 +// +NXT_STATUS cCmdWrapCommBTCheckStatus(UBYTE * ArgV[]) +{ + *((SBYTE*)(ArgV[0])) = cCmdBTCheckStatus(*(ArgV[1])); + + return (NO_ERR); +} + +// +//cCmdWrapCommBTWrite +//ArgV[0]: (return) Status byte, SBYTE +//ArgV[1]: Connection index, 0-3 +//ArgV[2]: Buffer +// +NXT_STATUS cCmdWrapCommBTWrite(UBYTE * ArgV[]) +{ + SBYTE * pReturnVal = (SBYTE*)(ArgV[0]); + UBYTE Connection = *(ArgV[1]); + UBYTE * pBuf; + UWORD BufLength; + DV_INDEX DVIndex; + + //Resolve array arguments + DVIndex = *(DV_INDEX *)(ArgV[2]); + pBuf = cCmdDVPtr(DVIndex); + + BufLength = DV_ARRAY[DVIndex].Count; + + //If there's an old error code hanging around, clear it before proceeding. + if (VarsCmd.CommStat < 0) + VarsCmd.CommStat = SUCCESS; + + //!!! Only first 256 bytes could possibly make it through! Should return error on longer input? + //!!! Not requesting a wait-for-response because only known use doesn't read responses. + pMapComm->pFunc(SENDDATA, (UBYTE)BufLength, Connection, FALSE, pBuf, (UWORD*)&(VarsCmd.CommStat)); + + //!!! Reasonable to wrap below code in cCmdCommBTCheckStatus? + //INPROGRESS means our request was accepted by His Funkiness of pFunc + if (VarsCmd.CommStat == (SWORD)INPROGRESS) + { + *pReturnVal = STAT_COMM_PENDING; + + //Set DirtyComm flag so stream is reset after program ends + VarsCmd.DirtyComm = TRUE; + } + //Translate BTBUSY to ERR_COMM_CHAN_NOT_READY + else if (VarsCmd.CommStat == (SWORD)BTBUSY) + { + *pReturnVal = ERR_COMM_CHAN_NOT_READY; + } + else + { + *pReturnVal = UNPACK_STATUS(VarsCmd.CommStat); + } + + return (NO_ERR); +} + +// +//cCmdWrapCommBTRead +//ArgV[0]: (return) Status byte, SBYTE +//ArgV[1]: Count to read +//ArgV[2]: Buffer +// +NXT_STATUS cCmdWrapCommBTRead(UBYTE * ArgV[]) +{ + //SBYTE * pReturnVal = (SBYTE*)(ArgV[0]); + //UBYTE * pBuf = (ArgV[2]); + //!!! should provide length and/or connection to read? + + //!!! This syscall is not implemented; return fatal error. + return (ERR_INSTR); +} + +// +//cCmdWrapKeepAlive +//ArgV[0]: (return) Current timer limit in ms, ULONG +// +NXT_STATUS cCmdWrapKeepAlive(UBYTE * ArgV[]) +{ + pMapUi->Flags |= UI_RESET_SLEEP_TIMER; + + //Convert UI's minute-based timeout value to millisecs + //Milliseconds are the "natural" time unit in user-land. + *(ULONG*)(ArgV[0]) = (pMapUi->SleepTimeout * 60 * 1000); + + return (NO_ERR); +} + +#define MAX_IOM_BUFFER_SIZE 64 +// +//cCmdWrapIOMapRead +//ArgV[0]: (return) Status byte, SBYTE +//ArgV[1]: Module name, CStr +//ArgV[2]: Offset, UWORD +//ArgV[3]: Count, UWORD +//ArgV[4]: Buffer, UBYTE array +// +NXT_STATUS cCmdWrapIOMapRead(UBYTE * ArgV[]) +{ + UWORD LStatus; + NXT_STATUS Status; + + SBYTE * pReturnVal = (SBYTE*)(ArgV[0]); + UWORD Offset = *(UWORD*)(ArgV[2]); + //Our copy of 'Count' must be a ULONG to match the loader interface + ULONG Count = *(UWORD*)(ArgV[3]); + + DV_INDEX DVIndex; + + //Buffer for return of FINDFIRSTMODULE call, structure defined in protocol doc + //We need it to transfer the ModuleID to the IOMAPREAD call + UBYTE FindBuffer[FILENAME_LENGTH + 10]; + //Buffer to store data and offset in for IOMAPREAD call + //!!! Constant size means only limited reads and writes + UBYTE DataBuffer[MAX_IOM_BUFFER_SIZE + 2]; + + if (Count > MAX_IOM_BUFFER_SIZE) + { + //Request to read too much data at once; clear buffer, return error. + DVIndex = *(DV_INDEX *)(ArgV[4]); + *pReturnVal = cCmdDVArrayAlloc(DVIndex, 0); + if (IS_ERR(*pReturnVal)) + return (*pReturnVal); + + *pReturnVal = ERR_INVALID_SIZE; + return (NO_ERR); + } + + //Resolve module name + DVIndex = *(DV_INDEX *)(ArgV[1]); + ArgV[1] = cCmdDVPtr(DVIndex); + + //Find module by name. Note that wildcards are accepted, but only first match matters. + LStatus = pMapLoader->pFunc(FINDFIRSTMODULE, ArgV[1], FindBuffer, NULL); + + if (LOADER_ERR(LStatus) == SUCCESS) + { + //Module was found, transfer Offset into first two bytes of DataBuffer and attempt to read + *(UWORD*)(DataBuffer) = Offset; + LStatus = pMapLoader->pFunc(IOMAPREAD, &(FindBuffer[FILENAME_LENGTH + 1]), DataBuffer, &Count); + + if (LOADER_ERR(LStatus) == SUCCESS) + { + //No error from IOMAPREAD, so copy the data into VM's dataspace + //Size destination array + DVIndex = *(DV_INDEX *)(ArgV[4]); + Status = cCmdDVArrayAlloc(DVIndex, (UWORD)Count); + if (IS_ERR(Status)) + { + //Alloc failed, so close handle and return + pMapLoader->pFunc(CLOSEMODHANDLE, NULL, NULL, NULL); + return (Status); + } + + //Alloc succeeded, so resolve and copy away + ArgV[4] = cCmdDVPtr(DVIndex); + memcpy(ArgV[4], &(DataBuffer[2]), Count); + } + } + + *pReturnVal = LOADER_ERR_BYTE(LStatus); + + pMapLoader->pFunc(CLOSEMODHANDLE, NULL, NULL, NULL); + return (NO_ERR); +} + +// +//cCmdWrapIOMapWrite +//ArgV[0]: (return) Status byte, SBYTE +//ArgV[1]: Module name, CStr +//ArgV[2]: Offset, UWORD +//ArgV[3]: Buffer, UBYTE array +// +NXT_STATUS cCmdWrapIOMapWrite(UBYTE * ArgV[]) +{ + UWORD LStatus; + + SBYTE * pReturnVal = (SBYTE*)(ArgV[0]); + UWORD Offset = *(UWORD*)(ArgV[2]); + + //Our copy of 'Count' must be a ULONG to match the loader interface + ULONG Count; + DV_INDEX DVIndex; + + //Buffer for return of FINDFIRSTMODULE call, structure defined in protocol doc + //We need it to transfer the ModuleID to the IOMAPREAD call + UBYTE FindBuffer[FILENAME_LENGTH + 10]; + //Buffer to store data and offset in for IOMAPREAD call + //!!! Constant size means only limited reads and writes + UBYTE DataBuffer[MAX_IOM_BUFFER_SIZE + 2]; + + //Resolve module name and buffer + DVIndex = *(DV_INDEX *)(ArgV[1]); + ArgV[1] = cCmdDVPtr(DVIndex); + + DVIndex = *(DV_INDEX *)(ArgV[3]); + ArgV[3] = cCmdDVPtr(DVIndex); + Count = DV_ARRAY[DVIndex].Count; + + if (Count > MAX_IOM_BUFFER_SIZE) + { + //Request to read too much data at once; return error and give up + *pReturnVal = ERR_INVALID_SIZE; + return (NO_ERR); + } + + LStatus = pMapLoader->pFunc(FINDFIRSTMODULE, ArgV[1], FindBuffer, NULL); + + if (LOADER_ERR(LStatus) == SUCCESS) + { + //Module was found, transfer Offset into first two bytes of DataBuffer, copy data into rest of buffer, then write + *(UWORD*)(DataBuffer) = Offset; + memcpy(&(DataBuffer[2]), ArgV[3], Count); + LStatus = pMapLoader->pFunc(IOMAPWRITE, &(FindBuffer[FILENAME_LENGTH + 1]), DataBuffer, &Count); + } + + *pReturnVal = LOADER_ERR_BYTE(LStatus); + + pMapLoader->pFunc(CLOSEMODHANDLE, NULL, NULL, NULL); + return (NO_ERR); +} + +#if VM_BENCHMARK +void cCmdWriteBenchmarkFile() +{ + LOADER_STATUS LStatus; + UBYTE Handle; + ULONG BenchFileSize; + ULONG i, Length; + UBYTE Buffer[256]; + + //Remove old benchmark file, create a new one + strcpy((char *)Buffer, "benchmark.txt"); + pMapLoader->pFunc(DELETE, Buffer, NULL, NULL); + BenchFileSize = 2048; + LStatus = pMapLoader->pFunc(OPENWRITEDATA, Buffer, NULL, &BenchFileSize); + + if (!LOADER_ERR(LStatus)) + { + //Write Benchmark file + Handle = LOADER_HANDLE(LStatus); + + //Header + sprintf((char *)Buffer, "Program Name: %s\r\n", VarsCmd.ActiveProgName); + Length = strlen((char *)Buffer); + LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length); + + sprintf((char *)Buffer, "InstrCount: %d\r\n", VarsCmd.InstrCount); + Length = strlen((char *)Buffer); + LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length); + + sprintf((char *)Buffer, "Time: %d\r\n", IOMapCmd.Tick - VarsCmd.StartTick); + Length = strlen((char *)Buffer); + LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length); + + sprintf((char *)Buffer, "Instr/Tick: %d\r\n", VarsCmd.Average); + Length = strlen((char *)Buffer); + LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length); + + sprintf((char *)Buffer, "CmdCtrl Calls: %d\r\n", VarsCmd.CmdCtrlCount); + Length = strlen((char *)Buffer); + LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length); + + sprintf((char *)Buffer, "OverTime Rounds: %d\r\n", VarsCmd.OverTimeCount); + Length = strlen((char *)Buffer); + LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length); + + sprintf((char *)Buffer, "Max OverTime Length: %d\r\n", VarsCmd.MaxOverTimeLength); + Length = strlen((char *)Buffer); + LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length); + + sprintf((char *)Buffer, "CompactionCount: %d\r\n", VarsCmd.CompactionCount); + Length = strlen((char *)Buffer); + LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length); + + sprintf((char *)Buffer, "LastCompactionTick: %d\r\n", VarsCmd.LastCompactionTick); + Length = strlen((char *)Buffer); + LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length); + + sprintf((char *)Buffer, "MaxCompactionTime: %d\r\n", VarsCmd.MaxCompactionTime); + Length = strlen((char *)Buffer); + LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length); + + //opcode benchmarks + sprintf((char *)Buffer, "Op\tCnt\tOver\tMax\r\n"); + Length = strlen((char *)Buffer); + LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length); + for (i = 0; i < OPCODE_COUNT; i++) + { + sprintf((char *)Buffer, "%x\t%d\t%d\t%d\t%d\r\n", i, VarsCmd.OpcodeBenchmarks[i][0], VarsCmd.OpcodeBenchmarks[i][1], VarsCmd.OpcodeBenchmarks[i][2], VarsCmd.OpcodeBenchmarks[i][3]); + Length = strlen((char *)Buffer); + LStatus = pMapLoader->pFunc(WRITE, &Handle, Buffer, &Length); + } + //close file + LStatus = pMapLoader->pFunc(CLOSE, &Handle, NULL, NULL); + } +} +#endif + +#ifdef SIM_NXT +// Accessors for simulator library code +SWORD cCmdGetCodeWord(CLUMP_ID Clump, CODE_INDEX Index) +{ + if (Clump == NOT_A_CLUMP) + { + NXT_ASSERT(Index < VarsCmd.CodespaceCount); + return (VarsCmd.pCodespace[Index]); + } + else + { + NXT_ASSERT(cCmdIsClumpIDSane(Clump)); + return (((SWORD)VarsCmd.pCodespace[VarsCmd.pAllClumps[Clump].CodeStart + Index])); + } +} + + +UBYTE * cCmdGetDataspace(UWORD *DataspaceSize) +{ + if (DataspaceSize) + *DataspaceSize = VarsCmd.DataspaceSize; + return (VarsCmd.pDataspace); +} + + +DOPE_VECTOR * cCmdGetDopeVectorPtr() +{ + return VarsCmd.MemMgr.pDopeVectorArray; +} + + +MEM_MGR cCmdGetMemMgr(void) +{ + return VarsCmd.MemMgr; +} + + +ULONG cCmdGetPoolSize() +{ + return VarsCmd.PoolSize; +} +#endif + +#else //!ENABLE_VM +// +//Implementations of standard interface if VM is disabled. +//Place low-level test code here if VM is causing issues. +//Test code must implement cCmdInit(), cCmdCtrl(), and cCmdExit() at a minimum. +//Recommend using a pattern like #include "c_cmd_alternate.c" +// + +//!!! !ENABLE_VM implementations really should provide a placeholder function for this pointer +//IOMapCmd.pRCHandler = &cCmdHandleRemoteCommands; +//#include "c_cmd_alternate.c" +//#include "c_cmd_FB_LowSpeed_Test.c" +//#include "c_cmd_FB_LowSpeed_JB_Compass.c" +//#include "c_cmd_FB_LowSpeed_Continius.c" +//#include "c_cmd_FB_LowSpeed_JB_Color.c" +#include "c_cmd_FB_LowSpeed_NorthStar_Demo2.c" +//#include "c_cmd_FB_LowSpeed_LEGO_TEST.c" + +#endif //ENABLE_VM -- cgit v1.2.3