aboutsummaryrefslogtreecommitdiffhomepage
path: root/AT91SAM7S256/Source/c_cmd.c
diff options
context:
space:
mode:
Diffstat (limited to 'AT91SAM7S256/Source/c_cmd.c')
-rw-r--r--AT91SAM7S256/Source/c_cmd.c8020
1 files changed, 0 insertions, 8020 deletions
diff --git a/AT91SAM7S256/Source/c_cmd.c b/AT91SAM7S256/Source/c_cmd.c
deleted file mode 100644
index 87b3e24..0000000
--- a/AT91SAM7S256/Source/c_cmd.c
+++ /dev/null
@@ -1,8020 +0,0 @@
-//
-// Date init 14.12.2004
-//
-// Revision date $Date: 24-06-09 8:53 $
-//
-// Filename $Workfile:: c_cmd.c $
-//
-// Version $Revision: 14 $
-//
-// Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/c_cmd. $
-//
-// Platform C
-//
-
-//
-// File Description:
-// This file contains the virtual machine implementation to run bytecode
-// programs compatible with LEGO MINDSTORMS NXT Software 2.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 "m_sched.h"
-
-#include "c_cmd.h"
-#include "c_cmd_bytecodes.h"
-#include "d_timer.h"
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <math.h> // for sqrt, abs, and trig stuff
-
-#define VMProfilingCode 0
-
-static IOMAPCMD IOMapCmd;
-static VARSCMD VarsCmd;
-static HEADER **pHeaders;
-static ULONG gInstrsToExecute;
-static SLONG gPCDelta;
-#define NUM_INTERP_FUNCS 16
-#define NUM_SHORT_INTERP_FUNCS 8
-#define VAR_INSTR_SIZE 0xE
-// important to cast since most args are assigned from signed value, and locals may be ULONG
-#define GetDataArg(arg) ((UWORD)(arg))
-#if VMProfilingCode
-static ULONG ExecutedInstrs= 0, CmdCtrlTime= 0, OverheadTime= 0, CmdCtrlCalls= 0, LeaveTime= 0, NotFirstCall= 0, LastAvgCount= 0;
-static ULONG CmdCtrlClumpTime[256];
-typedef struct {
- ULONG Time;
- ULONG Count;
- ULONG Avg;
- ULONG Max;
-} VMInstrProfileInfo;
-static VMInstrProfileInfo InstrProfile[OPCODE_COUNT];
-static VMInstrProfileInfo SysCallProfile[SYSCALL_COUNT];
-static VMInstrProfileInfo InterpFuncProfile[NUM_INTERP_FUNCS];
-static VMInstrProfileInfo ShortInstrProfile[NUM_SHORT_OPCODE_COUNT];
-#endif
-
-#define cCmdDSType(Arg) (VarsCmd.pDataspaceTOC[(Arg)].TypeCode)
-#define cCmdDSScalarPtr(DSElementID, Offset) (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset + Offset)
-#define cCmdSizeOf(TC) (TC_Size_Table[(TC)])
-
-#define scalarBinopDispatchMask 0x1
-#define scalarUnop2DispatchMask 0x2
-
-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 instr size
-//Unary operations can have arity of 1 or 2 (some need a destination)
-//All instructions taking 4 or more operands are handled as "Other"
-// Table uses NoArg for illegal instr sizes such as zero and odd sizes
-//
-static pInterp InterpFuncs[NUM_INTERP_FUNCS] =
-{
- cCmdInterpNoArg,
- cCmdInterpNoArg,
- cCmdInterpNoArg, // size 2
- cCmdInterpNoArg,
- cCmdInterpUnop1, // size 4
- cCmdInterpNoArg,
- cCmdInterpUnop2, // size 6 general poly is cCmdInterpUnop2, scalar is cCmdInterpScalarUnop2
- cCmdInterpNoArg,
- cCmdInterpBinop, // size 8, general poly is cCmdInterpBinop, scalar is cCmdInterpScalarBinop
- cCmdInterpNoArg,
- cCmdInterpOther, // size 10
- cCmdInterpNoArg,
- cCmdInterpOther, // size 12
- cCmdInterpNoArg,
- cCmdInterpOther, // size 14
- cCmdInterpNoArg
-};
-
-static pInterpShort ShortInterpFuncs[NUM_SHORT_INTERP_FUNCS] =
-{
- cCmdInterpShortMove,
- cCmdInterpShortAcquire,
- cCmdInterpShortRelease,
- cCmdInterpShortSubCall,
- cCmdInterpShortError,
- cCmdInterpShortError,
- cCmdInterpShortError,
- cCmdInterpShortError
-};
-
-ULONG TC_Size_Table[]= {
- 0, // void
- SIZE_UBYTE,
- SIZE_SBYTE,
- SIZE_UWORD,
- SIZE_SWORD,
- SIZE_ULONG,
- SIZE_SLONG,
- SIZE_UWORD, // array
- 0, // cluster
- SIZE_MUTEX,
- SIZE_FLOAT
-};
-
-
-//
-//Function pointers to SysCall implementations
-//See interpreter for OP_SYSCALL
-//
-static pSysCall SysCallFuncs[SYSCALL_COUNT] =
-{
- cCmdWrapFileOpenRead,
- cCmdWrapFileOpenWrite,
- cCmdWrapFileOpenAppend,
- cCmdWrapFileRead,
- cCmdWrapFileWrite,
- cCmdWrapFileClose, // 5
- cCmdWrapFileResolveHandle,
- cCmdWrapFileRename,
- cCmdWrapFileDelete,
- cCmdWrapSoundPlayFile,
- cCmdWrapSoundPlayTone, // 10
- cCmdWrapSoundGetState,
- cCmdWrapSoundSetState,
- cCmdWrapDrawText,
- cCmdWrapDrawPoint,
- cCmdWrapDrawLine, // 15
- cCmdWrapDrawCircle,
- cCmdWrapDrawRect,
- cCmdWrapDrawPicture,
- cCmdWrapSetScreenMode,
- cCmdWrapReadButton, // 20
- cCmdWrapCommLSWrite,
- cCmdWrapCommLSRead,
- cCmdWrapCommLSCheckStatus,
- cCmdWrapRandomNumber,
- cCmdWrapGetStartTick, // 25
- cCmdWrapMessageWrite,
- cCmdWrapMessageRead,
- cCmdWrapCommBTCheckStatus,
- cCmdWrapCommBTWrite,
- cCmdWrapCommBTRead, // 30
- cCmdWrapKeepAlive,
- cCmdWrapIOMapRead,
- cCmdWrapIOMapWrite,
- cCmdWrapColorSensorRead,
- cCmdWrapCommBTOnOff, // 35
- cCmdWrapCommBTConnection,
- cCmdWrapCommHSWrite,
- cCmdWrapCommHSRead,
- cCmdWrapCommHSCheckStatus,
- cCmdWrapReadSemData, //40
- cCmdWrapWriteSemData,
- cCmdWrapComputeCalibValue,
- cCmdWrapUpdateCalibCacheInfo,
- cCmdWrapDatalogWrite,
- cCmdWrapDatalogGetTimes, //45
- cCmdWrapSetSleepTimeout,
- cCmdWrapListFiles //47
-
- // don't forget to update SYSCALL_COUNT in c_cmd.h
-};
-
-//
-//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
- TC_UBYTE, //IO_OUT_OPTIONS
- TC_SBYTE, //IO_OUT_MAX_SPEED
- TC_SBYTE, //IO_OUT_MAX_ACCELERATION
-
- //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
- TC_UBYTE, //IO_OUT_OPTIONS
- TC_SBYTE, //IO_OUT_MAX_SPEED
- TC_SBYTE, //IO_OUT_MAX_ACCELERATION
-
- //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
- TC_UBYTE, //IO_OUT_OPTIONS
- TC_SBYTE, //IO_OUT_MAX_SPEED
- TC_SBYTE, //IO_OUT_MAX_ACCELERATION
-};
-
-
-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
-};
-
-// Data used to indicate usage of motor ports, or usage requests
-UBYTE gUsageSemData, gRequestSemData;
-
-UBYTE cCmdBTGetDeviceType(UBYTE *pCOD)
-{
- ULONG COD;
- UBYTE Result;
- UBYTE Tmp;
-
- COD = 0;
- for (Tmp = 0;Tmp < SIZE_OF_CLASS_OF_DEVICE;Tmp++)
- {
- COD <<= 8;
- COD |= (ULONG)*pCOD;
- pCOD++;
- }
-
- Result = DEVICETYPE_UNKNOWN;
- if ((COD & 0x00001FFF) == 0x00000804)
- {
- Result = DEVICETYPE_NXT;
- }
- if ((COD & 0x00001F00) == 0x00000200)
- {
- Result = DEVICETYPE_PHONE;
- }
- if ((COD & 0x00001F00) == 0x00000100)
- {
- Result = DEVICETYPE_PC;
- }
-
- return (Result);
-}
-
-//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(LStatus = 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:
- {
- UBYTE Port = pInBuf[1];
- //Don't do anything if illegal port specification is made
- if (Port >= NO_OF_OUTPUTS && Port != 0xFF)
- {
- RCStatus = ERR_RC_ILLEGAL_VAL;
- break;
- }
-
- //0xFF is protocol defined to mean "all ports".
- if (Port == 0xFF)
- {
- FirstPort = 0;
- LastPort = NO_OF_OUTPUTS - 1;
- }
- else
- FirstPort = LastPort = Port;
-
- for (i = FirstPort; i <= LastPort; i++)
- {
- OUTPUT * pOut = &(pMapOutPut->Outputs[i]);
- pOut->Speed = pInBuf[2];
- pOut->Mode = pInBuf[3];
- pOut->RegMode = pInBuf[4];
- pOut->SyncTurnParameter = pInBuf[5];
- pOut->RunState = pInBuf[6];
- pOut->Options = pOut->Mode & REG_METHOD;
- memcpy((PSZ)(&(pOut->TachoLimit)), (PSZ)(&pInBuf[7]), 4);
-
- pOut->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;
- }
- INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]);
-
- pIn->SensorType = pInBuf[2];
- pIn->SensorMode = pInBuf[3];
-
- //Set InvalidData flag automatically since type may have changed
- pIn->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;
- }
- OUTPUT * pOut = &(pMapOutPut->Outputs[i]);
-
- //Echo port
- pOutBuf[ResponseLen] = i;
- ResponseLen++;
-
- //Power
- pOutBuf[ResponseLen] = pOut->Speed;
- ResponseLen++;
-
- //Mode
- pOutBuf[ResponseLen] = pOut->Mode;
- ResponseLen++;
-
- //RegMode
- pOutBuf[ResponseLen] = pOut->RegMode;
- ResponseLen++;
-
- //TurnRatio
- pOutBuf[ResponseLen] = pOut->SyncTurnParameter;
- ResponseLen++;
-
- //RunState
- pOutBuf[ResponseLen] = pOut->RunState;
- ResponseLen++;
-
- //TachoLimit ULONG
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->TachoLimit)), 4);
- ResponseLen += 4;
-
- //TachoCount SLONG
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->TachoCnt)), 4);
- ResponseLen += 4;
-
- //BlockTachoCount SLONG
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->BlockTachoCount)), 4);
- ResponseLen += 4;
-
- //RotationCount SLONG
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->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++;
-
- INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]);
-
- //Set "Valid?" boolean
- if (pIn->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] = pIn->SensorType;
- ResponseLen++;
-
- pOutBuf[ResponseLen] = pIn->SensorMode;
- ResponseLen++;
-
- //Set Raw, Normalized, and Scaled values
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->ADRaw)), 2);
- ResponseLen += 2;
-
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->SensorRaw)), 2);
- ResponseLen += 2;
-
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->SensorValue)), 2);
- ResponseLen += 2;
-
- //!!! Return normalized raw value in place of calibrated value for now -- see comment above
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->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
- pMapOutPut->Outputs[i].Flags |= (pInBuf[2] ? UPDATE_RESET_BLOCK_COUNT : UPDATE_RESET_ROTATION_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;
-
- // remote-only command to read from datalog buffer
- // pInBuf[1] = Remove? (bool)
- case RC_DATALOG_READ:
- {
- if (SendResponse == TRUE)
- {
- RCStatus = cCmdDatalogGetSize(&Count);
- pOutBuf[ResponseLen] = Count;
- ResponseLen++;
-
- if (!IS_ERR(RCStatus) && Count > 0)
- {
- pData = &(pOutBuf[ResponseLen]);
- RCStatus = cCmdDatalogRead(pData, Count, (pInBuf[1]));
- //If cCmdDatalogRead 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_DATALOG_SIZE - Count;
- memset(&(pOutBuf[ResponseLen]), 0, Count);
- ResponseLen += Count;
- }
- }
- break;
- case RC_DATALOG_SET_TIMES:
- {
- //SyncTime SLONG
- memcpy((PSZ)&IOMapCmd.SyncTime, (PSZ)&(pInBuf[1]), 4);
- IOMapCmd.SyncTick= dTimerReadNoPoll();
- }
- break;
-
- case RC_BT_GET_CONN_COUNT:
- if (SendResponse == TRUE) {
- pOutBuf[ResponseLen]= SIZE_OF_BT_CONNECT_TABLE;
- ResponseLen++;
- }
- break;
- case RC_BT_GET_CONN_NAME: // param in is index, param out is name
- if (SendResponse == TRUE) { // get index from inbuf
- i = pInBuf[1];
- if(i < SIZE_OF_BT_CONNECT_TABLE) { // unsigned, so guaranteed >= 0
- pOutBuf[ResponseLen] = cCmdBTGetDeviceType(pMapComm->BtConnectTable[i].ClassOfDevice);
- memcpy((PSZ)(&(pOutBuf[ResponseLen+1])), (PSZ)(pMapComm->BtConnectTable[i].Name), SIZE_OF_BT_NAME + 1);
- ResponseLen += SIZE_OF_BT_NAME + 2;
- }
- else {
- pOutBuf[ResponseLen] = 0;
- ResponseLen += SIZE_OF_BT_NAME + 2;
- }
- }
- break;
- case RC_BT_GET_CONTACT_COUNT:
- if (SendResponse == TRUE) {
- pOutBuf[ResponseLen]= SIZE_OF_BT_DEVICE_TABLE;
- ResponseLen++;
- }
- break;
- case RC_BT_GET_CONTACT_NAME:
- if (SendResponse == TRUE) { // get index from inbuf
- i = pInBuf[1];
- if(i < SIZE_OF_BT_DEVICE_TABLE && (pMapComm->BtDeviceTable[i].DeviceStatus & BT_DEVICE_KNOWN)) { // unsigned, so guaranteed >= 0
- (pOutBuf[ResponseLen])= cCmdBTGetDeviceType(pMapComm->BtDeviceTable[i].ClassOfDevice);
- memcpy((PSZ)(&(pOutBuf[ResponseLen+1])), (PSZ)(pMapComm->BtDeviceTable[i].Name), SIZE_OF_BT_NAME + 1);
- ResponseLen += SIZE_OF_BT_NAME + 2;
- }
- else
- {
- pOutBuf[ResponseLen] = 0;
- memset((PSZ)(&(pOutBuf[ResponseLen+1])), 0, SIZE_OF_BT_NAME + 1);
- ResponseLen += SIZE_OF_BT_NAME + 2;
- }
- }
- break;
- case RC_SET_PROPERTY: // label/value pairs
- i = pInBuf[1];
- switch(i) {
- case RC_PROP_BTONOFF: {
- UWORD retVal, status;
- if(pInBuf[2])
- status= pMapComm->pFunc(BTON, 0, 0, 0, NULL, &retVal);
- else
- status= pMapComm->pFunc(BTOFF, 0, 0, 0, NULL, &retVal);
-
- RCStatus= (status == SUCCESS) ? retVal : status;
- }
- break;
- case RC_PROP_SOUND_LEVEL: {
- UBYTE volume= pInBuf[2];
- if(volume > 4)
- volume= 4;
- pMapSound->Volume= volume; // apparently stored in two places
- pMapUi->Volume= volume;
- }
- break;
- case RC_PROP_SLEEP_TIMEOUT: { // ulong millisecs to sleep
- ULONG value;
- memcpy((PSZ)&value, (PSZ)&(pInBuf[2]), 4);
- pMapUi->SleepTimeout= value / 60000;
- }
- break;
- default:
- //Unknown property -- still inform client to not expect any response bytes
- NXT_BREAK;
- RCStatus = ERR_RC_UNKNOWN_CMD;
- break;
- }
- break;
- case RC_GET_PROPERTY: // label/value pairs
- if (SendResponse == TRUE) { // get index from inbuf
- i = pInBuf[1];
- switch(i) {
- case RC_PROP_BTONOFF:
- pOutBuf[ResponseLen]= pMapUi->BluetoothState != BT_STATE_OFF;
- ResponseLen++;
- break;
- case RC_PROP_SOUND_LEVEL: {
- pOutBuf[ResponseLen]= pMapSound->Volume;
- ResponseLen++;
- }
- break;
- case RC_PROP_SLEEP_TIMEOUT: {
- ULONG value= (pMapUi->SleepTimeout * 60 * 1000);
- memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&value, 4);
- ResponseLen += 4;
- }
- break;
- default:
- //Unknown property -- still inform client to not expect any response bytes
- NXT_BREAK;
- RCStatus = ERR_RC_UNKNOWN_CMD;
- break;
- }
- }
- break;
- case RC_UPDATE_RESET_COUNT:
- {
- i = pInBuf[1];
-
- //Don't do anything if illegal port specification is made
- if (i >= NO_OF_OUTPUTS)
- {
- RCStatus = ERR_RC_ILLEGAL_VAL;
- break;
- }
-
- pMapOutPut->Outputs[i].Flags |= UPDATE_RESET_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++)
- {
- OUTPUT * pOut = &(pMapOutPut->Outputs[i]);
- IO_PTRS_OUT[IO_OUT_FLAGS + i * IO_OUT_FPP] = (void*)&(pOut->Flags);
- IO_PTRS_OUT[IO_OUT_MODE + i * IO_OUT_FPP] = (void*)&(pOut->Mode);
- IO_PTRS_OUT[IO_OUT_SPEED + i * IO_OUT_FPP] = (void*)&(pOut->Speed);
- IO_PTRS_OUT[IO_OUT_ACTUAL_SPEED + i * IO_OUT_FPP] = (void*)&(pOut->ActualSpeed);
- IO_PTRS_OUT[IO_OUT_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pOut->TachoCnt);
- IO_PTRS_OUT[IO_OUT_TACH_LIMIT + i * IO_OUT_FPP] = (void*)&(pOut->TachoLimit);
- IO_PTRS_OUT[IO_OUT_RUN_STATE + i * IO_OUT_FPP] = (void*)&(pOut->RunState);
- IO_PTRS_OUT[IO_OUT_TURN_RATIO + i * IO_OUT_FPP] = (void*)&(pOut->SyncTurnParameter);
- IO_PTRS_OUT[IO_OUT_REG_MODE + i * IO_OUT_FPP] = (void*)&(pOut->RegMode);
- IO_PTRS_OUT[IO_OUT_OVERLOAD + i * IO_OUT_FPP] = (void*)&(pOut->Overloaded);
- IO_PTRS_OUT[IO_OUT_REG_P_VAL + i * IO_OUT_FPP] = (void*)&(pOut->RegPParameter);
- IO_PTRS_OUT[IO_OUT_REG_I_VAL + i * IO_OUT_FPP] = (void*)&(pOut->RegIParameter);
- IO_PTRS_OUT[IO_OUT_REG_D_VAL + i * IO_OUT_FPP] = (void*)&(pOut->RegDParameter);
- IO_PTRS_OUT[IO_OUT_BLOCK_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pOut->BlockTachoCount);
- IO_PTRS_OUT[IO_OUT_ROTATION_COUNT + i * IO_OUT_FPP] = (void*)&(pOut->RotationCount);
- IO_PTRS_OUT[IO_OUT_OPTIONS + i * IO_OUT_FPP] = (void*)&(pOut->Options);
- IO_PTRS_OUT[IO_OUT_MAX_SPEED + i * IO_OUT_FPP] = (void*)&(pOut->MaxSpeed);
- IO_PTRS_OUT[IO_OUT_MAX_ACCELERATION + i * IO_OUT_FPP] = (void*)&(pOut->MaxAcceleration);
- }
-
- //Initialize IO_PTRS_IN
- for (i = 0; i < NO_OF_INPUTS; i++)
- {
- INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]);
- IO_PTRS_IN[IO_IN_TYPE + i * IO_IN_FPP] = (void*)&(pIn->SensorType);
- IO_PTRS_IN[IO_IN_MODE + i * IO_IN_FPP] = (void*)&(pIn->SensorMode);
- IO_PTRS_IN[IO_IN_ADRAW + i * IO_IN_FPP] = (void*)&(pIn->ADRaw);
- IO_PTRS_IN[IO_IN_NORMRAW + i * IO_IN_FPP] = (void*)&(pIn->SensorRaw);
- IO_PTRS_IN[IO_IN_SCALEDVAL + i * IO_IN_FPP] = (void*)&(pIn->SensorValue);
- IO_PTRS_IN[IO_IN_INVALID_DATA + i * IO_IN_FPP] = (void*)&(pIn->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();
- IOMapCmd.SyncTime= 0;
- IOMapCmd.SyncTick= 0;
-
- return;
-}
-
-
-void cCmdCtrl(void)
-{
- NXT_STATUS Status = NO_ERR;
-
- switch (VarsCmd.VMState)
- {
- case VM_RUN_FREE:
- case VM_RUN_SINGLE:
- {
- #if VMProfilingCode
- ULONG EnterTime= dTimerReadHiRes(), FinishTime;
- CmdCtrlCalls ++;
-#endif
- ULONG Continue;
-
-#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);
-
- //Handle any resting clumps that are ready to awaken
- cCmdCheckRestQ(IOMapCmd.Tick); // not using result, yet
- //Execute from at least one clump
- do
- {
- //Execute instructions from a clump up to INSTR_MAX, to end of millisec,
- //Finishing/suspending a clump, BREAKOUT_REQ, or any errors will cause a return
-#if VMProfilingCode
- ULONG ClumpEnterTime= dTimerReadHiRes();
- CLUMP_ID clump= VarsCmd.RunQ.Head;
-#endif
- Status = cCmdInterpFromClump();
-#if VMProfilingCode
- CmdCtrlClumpTime[clump] += dTimerReadHiRes() - ClumpEnterTime;
-#endif
-
- //If RunQ and RestQ are empty, program is done, or wacko
- if (!cCmdIsClumpIDSane(VarsCmd.RunQ.Head)) {
- Continue = FALSE;
- if(!cCmdIsClumpIDSane(VarsCmd.RestQ.Head)) {
- VarsCmd.VMState = VM_RESET1;
- IOMapCmd.ProgStatus = PROG_OK;
- }
- }
- else if (Status == CLUMP_SUSPEND || Status == CLUMP_DONE)
- Continue = TRUE; // queue isn't empty, didn't timeout
- //Only rotate RunQ on a "normal" finish, i.e. no error, clump end, or breakout request
- else if (Status == ROTATE_QUEUE) { // done and suspend do their own
- cCmdRotateQ();
- Continue= TRUE;
- }
- else if (Status == TIMES_UP) {
- cCmdRotateQ();
- Continue = FALSE;
- }
- else if (IS_ERR(Status)) // mem error is handled in InterpFromClump if possible
- {
- Continue = FALSE;
- VarsCmd.VMState = VM_RESET1;
- IOMapCmd.ProgStatus = PROG_ERROR;
- }
- else if (Status == STOP_REQ)
- {
- Continue = FALSE;
- VarsCmd.VMState = VM_RESET1;
- IOMapCmd.ProgStatus = PROG_OK;
- }
- else if (Status == BREAKOUT_REQ)
- {
- Continue = FALSE;
- }
- } while (Continue == TRUE);
-#if VMProfilingCode
- FinishTime= dTimerReadHiRes();
- if(NotFirstCall)
- OverheadTime += EnterTime - LeaveTime;
- else
- NotFirstCall= 1;
- CmdCtrlTime += FinishTime - EnterTime;
- LeaveTime= FinishTime;
-#endif
- // May busy wait to postpone to 1ms schedule
- while (IOMapCmd.Tick == dTimerRead());
- }
- break;
- 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(VarsCmd.VMState == VM_RUN_FREE)
- gInstrsToExecute = 20;
- else
- gInstrsToExecute= 1;
-
-#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);
- }
- }
- while (IOMapCmd.Tick == dTimerRead()); // delay until scheduled time
- }
- 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;
- while (IOMapCmd.Tick == dTimerRead()); // delay until scheduled time
- }
- 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;
- }
- while (IOMapCmd.Tick == dTimerRead()); // delay until scheduled time
- }
- break;
- }//END state machine switch
-
- //Set tick to new value for next time 'round
- IOMapCmd.Tick = dTimerReadNoPoll();
-
- 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);
-
- if (strncmp((PSZ)pData, "NXTBINARY", VM_FORMAT_STRING_SIZE) == 0)
- {
- ULONG NativeOffset;
- pCursor = (pData + 12);
- NativeOffset = (ULONG)(*pCursor);
- void (*native)(ULONG, ULONG) = (void (*)())(pData + NativeOffset);
- (*native)((ULONG)pData, DataSize);
- NXT_BREAK;
- return (ERR_VER);
- }
- //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;
- VarsCmd.RestQ.Head = NOT_A_CLUMP;
- VarsCmd.RestQ.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_LAST_VALID)
- return ERR_INSTR;
- else 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;
-}
-
-void cCmdRefreshActiveClump(CLUMP_ID CurrID)
-{
- CLUMP_REC * clumpRecPtr= &(VarsCmd.pAllClumps[CurrID]);
-
- if(clumpRecPtr->clumpScalarDispatchHints & scalarBinopDispatchMask)
- InterpFuncs[8]= cCmdInterpScalarBinop;
- else
- InterpFuncs[8]= cCmdInterpBinop;
- if(clumpRecPtr->clumpScalarDispatchHints & scalarUnop2DispatchMask)
- InterpFuncs[6]= cCmdInterpScalarUnop2;
- else
- InterpFuncs[6]= cCmdInterpUnop2;
-}
-
-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++)
- {
- CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[i];
- clumpPtr->InitFireCount = *(UBYTE*)(pCursor + i * VM_FILE_CLUMP_REC_SIZE);
- clumpPtr->DependentCount = *(UBYTE*)(pCursor + (i * VM_FILE_CLUMP_REC_SIZE) + 1);
- clumpPtr->CodeStart = *(UWORD*)(pCursor + (i * VM_FILE_CLUMP_REC_SIZE) + 2) + VarsCmd.pCodespace;
-
- //Initialize remaining CLUMP_REC fields
- clumpPtr->PC = clumpPtr->CodeStart;
- clumpPtr->Link = NOT_A_CLUMP;
-
- //Activate any clumps with CurrFireCount of 0
- clumpPtr->CurrFireCount = clumpPtr->InitFireCount;
- if (clumpPtr->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++)
- {
- CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[i];
- if (clumpPtr->DependentCount > 0)
- {
- clumpPtr->pDependents = (CLUMP_ID*)(pCursor);
-
- pCursor += (clumpPtr->DependentCount * sizeof(CLUMP_ID));
- }
- else
- clumpPtr->pDependents = NULL;
-
- //Patch up CodeEnd value based on CodeStart of next clump or last overall codeword
- if (i < (VarsCmd.AllClumpsCount - 1))
- clumpPtr->CodeEnd = (clumpPtr+1)->CodeStart - 1;
- else
- clumpPtr->CodeEnd = VarsCmd.CodespaceCount - 1 + VarsCmd.pCodespace;
-
- //Test for empty/insane clump code definitions
- NXT_ASSERT(clumpPtr->CodeStart < clumpPtr->CodeEnd);
- }
-
- // Check if the instructions within a clump are polymorphic and mark which table to dispatch from
- for (i = 0; i < VarsCmd.AllClumpsCount; i++)
- { // Check type on Boolean, math, ArrInit and ArrIndex, ingore GetSet I/O as these are always scalar
- // do we need to check for DataArg encodings to I/O map??? GM
- // Get Opcode and size of each instr, if ^^, check Arg types for Array or Cluster
- CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[i];
- CODE_WORD *pInstr = clumpPtr->CodeStart, *lastPC = clumpPtr->CodeEnd;
- ULONG InstrSize, opCode, shortOp, isT2Agg, isT3Agg, isScalarBinop= TRUE, isScalarUnop2= TRUE;
- TYPE_CODE t1, t2, t3;
- ULONG instrWord;
- do
- {
- instrWord= *(UWORD*)pInstr;
- opCode= OP_CODE(pInstr);
- shortOp= (instrWord>>8) & 0x0F;
- InstrSize = INSTR_SIZE(instrWord);
- if (InstrSize == VAR_INSTR_SIZE)
- InstrSize = ((UWORD*)pInstr)[1];
- if(shortOp <= 7) // no shorts are binOps
- {
- t2= cCmdDSType(pInstr[2]);
- isT2Agg= IS_AGGREGATE_TYPE(t2);
- if(InstrSize == 8) {
- t3= cCmdDSType(pInstr[3]);
- isT3Agg= IS_AGGREGATE_TYPE(t3);
- if(isT2Agg || isT3Agg) {
- if(opCode == OP_CMP) {
- UBYTE isString2, isString3;
- isString2= (t2 == TC_ARRAY) && cCmdDSType(INC_ID(pInstr[2])) == TC_UBYTE;
- isString3= (t3 == TC_ARRAY) && cCmdDSType(INC_ID(pInstr[3])) == TC_UBYTE;
- t1= cCmdDSType(pInstr[1]);
- if((!isString2 || !isString3) || t1 == TC_ARRAY) // allow strings to go scalar, don't let through element compares of bytes or Bools
- isScalarBinop= FALSE;
- }
- else if(opCode == OP_BRCMP)
- isScalarBinop= FALSE;
- }
- }
- else if(InstrSize == 6 && isT2Agg && (opCode == OP_NOT || opCode == OP_BRTST))
- isScalarUnop2= FALSE;
- }
- pInstr += InstrSize/2;
- } while((isScalarBinop || isScalarUnop2) && pInstr < lastPC);
- if(isScalarBinop)
- clumpPtr->clumpScalarDispatchHints |= scalarBinopDispatchMask;
- else
- clumpPtr->clumpScalarDispatchHints &= ~scalarBinopDispatchMask;
-
- if(isScalarUnop2)
- clumpPtr->clumpScalarDispatchHints |= scalarUnop2DispatchMask;
- else
- clumpPtr->clumpScalarDispatchHints &= ~scalarUnop2DispatchMask;
-
- }
- //Programs with no active clumps constitutes an activation error
- if (VarsCmd.RunQ.Head == NOT_A_CLUMP)
- return (ERR_FILE);
- else
- {
- // now that we know which clumps are scalar and poly, refresh dispatch table to match head
- cCmdRefreshActiveClump(VarsCmd.RunQ.Head);
-
- }
-
- //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);
-
- // fix memmgr links. old files contain unused backPtrs, we now use these to store backLink
- DV_INDEX prev= NOT_A_DS_ID;
- for (i = VarsCmd.MemMgr.Head; i != NOT_A_DS_ID; i = DV_ARRAY[i].Link) {
- DV_ARRAY[i].BackLink= prev;
- prev= i;
- }
-
- //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;
- }
- }
-
- //Initialize datalog queue
- VarsCmd.DatalogBuffer.ReadIndex = 0;
- VarsCmd.DatalogBuffer.WriteIndex = 0;
- for (j = 0; j < DATALOG_QUEUE_DEPTH; j++)
- {
- VarsCmd.DatalogBuffer.Datalogs[j] = NOT_A_DS_ID;
- }
-
- // now that we've loaded program, prime memmgr dopevectors based upon number of handles in ds.
- ULONG numHandles= DV_ARRAY[0].Count/2;
- if(numHandles > 200)
- numHandles= 200;
- Status = cCmdGrowDopeVectorArray(numHandles);
-
- if (cCmdVerifyMemMgr() != TRUE)
- return (ERR_FILE);
-
- gUsageSemData= 0;
- gRequestSemData= 0;
- // preload all calibration coefficients into mem
- cCmdLoadCalibrationFiles();
- 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;
-
- 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(CROPDATAFILE, &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++)
- {
- INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]);
- //Clear type and mode to defaults
- pIn->SensorType = NO_SENSOR;
- pIn->SensorMode = RAWMODE;
-
- //Reset input values to 0 prior to running (clear things like stale rotation counts)
- pIn->ADRaw = 0;
- pIn->SensorRaw = 0;
- pIn->SensorValue = 0;
-
- //Assert invalid data flag so future code is aware of these changes
- pIn->InvalidData = TRUE;
- }
-
- for (i = 0; i < NO_OF_OUTPUTS; i++)
- {
- //Coast and reset all motor parameters
- OUTPUT * pOut = &(pMapOutPut->Outputs[i]);
- pOut->Mode = 0;
- pOut->RegMode = REGULATION_MODE_IDLE;
- pOut->RunState = MOTOR_RUN_STATE_IDLE;
- pOut->Speed = 0;
- pOut->TachoLimit = 0;
- pOut->SyncTurnParameter = 0;
- pOut->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<<i);
- }
- }
-
-}
-
-
-//Add NewClump to end, updating Queue's head/tail as needed
-void cCmdEnQClump(CLUMP_Q * Queue, CLUMP_ID NewClump)
-{
- //Make sure NewClump's ID is valid and not already on Q
- NXT_ASSERT(cCmdIsClumpIDSane(NewClump));
- NXT_ASSERT(cCmdIsQSane(Queue) == TRUE);
- NXT_ASSERT(!cCmdIsClumpOnQ(Queue, NewClump));
-
- VarsCmd.pAllClumps[NewClump].Link = NOT_A_CLUMP;
-
- //If queue is empty, NewClump becomes both head and tail
- if (Queue->Head == NOT_A_CLUMP)
- {
- NXT_ASSERT(Queue->Tail == NOT_A_CLUMP);
-
- Queue->Head = NewClump;
- Queue->Tail = NewClump;
- if(Queue == &(VarsCmd.RunQ))
- cCmdRefreshActiveClump(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 if(Queue == &(VarsCmd.RunQ))
- cCmdRefreshActiveClump(Queue->Head);
- }
- //Else, look through rest of list looking for a link to our clump
- else
- {
- do
- {
- CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[CurrID];
- LinkID = clumpPtr->Link;
-
- //If we find a link to our clump, patch up predecessor's link
- if (clumpPtr->Link == Clump)
- {
- clumpPtr->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_ID CurrID;
- CLUMP_REC * pClumpRec;
- CLUMP_Q * Queue = &VarsCmd.RunQ;
-
- //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;
-
- // reinit clump info
- CurrID= Queue->Head;
- cCmdRefreshActiveClump(Queue->Head);
-
- //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)
-{
- NXT_STATUS Status = NO_ERR;
- CLUMP_ID Clump= VarsCmd.RunQ.Head; // save off before queue changes below
-
- 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)
-{
-#if WIN_DEBUG || defined(ARM_DEBUG)
- CLUMP_ID Clump= VarsCmd.RunQ.Head;
-#endif
- 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);
-}
-
-// No instruction to do this yet, but put current clump to sleep until awakeTime occurs
-NXT_STATUS cCmdSleepClump(ULONG time)
-{
- CLUMP_ID Clump= VarsCmd.RunQ.Head; // save off before queue changes below
- CLUMP_REC * pClump = &(VarsCmd.pAllClumps[Clump]);
- cCmdDeQClump(&(VarsCmd.RunQ), Clump);
- cCmdEnQClump(&(VarsCmd.RestQ), Clump);
- pClump->awakenTime= time;
- return CLUMP_SUSPEND;
-}
-
-UBYTE cCmdCheckRestQ(ULONG currTime)
-{
- UBYTE awakened= FALSE;
- CLUMP_ID curr, next;
- CLUMP_REC * pClump;
- curr= VarsCmd.RestQ.Head;
- while(curr != NOT_A_CLUMP) {
- pClump= &(VarsCmd.pAllClumps[curr]);
- next= pClump->Link;
- if(pClump->awakenTime <= currTime) {
- pClump->awakenTime= 0; // not necessary, but for debugging identification
- cCmdDeQClump(&(VarsCmd.RestQ), curr);
- cCmdEnQClump(&(VarsCmd.RunQ), curr);
- awakened= TRUE;
- }
- curr= next;
- }
- return awakened;
-}
-
-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));
-
- CLUMP_REC *clumpPtr= &VarsCmd.pAllClumps[TargetClump];
- clumpPtr->CurrFireCount--;
- if (clumpPtr->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;
- ULONG *poolPtr;
-
- //VarsCmd.Pool is a UBYTE pointer to ULONG array
- //This was done to enforce portable alignment.
- VarsCmd.Pool = (UBYTE*)(IOMapCmd.MemoryPool);
-
- for (i = (POOL_MAX_SIZE / 4), poolPtr= (ULONG*)&(POOL_START)[0]; i>0; i--, poolPtr++)
- *poolPtr = 0xDEADBEEF;
-
- VarsCmd.PoolSize = 0;
-}
-
-
-#if VMProfilingCode
-ULONG memMgrTime= 0;
-#endif
-NXT_STATUS cCmdDSArrayAlloc(DS_ELEMENT_ID DSElementID, UWORD Offset, UWORD NewCount)
-{
- NXT_STATUS Status = NO_ERR;
- UWORD DVIndex;
- UWORD OldCount;
- UWORD i;
-#if VMProfilingCode
- ULONG enterTime= dTimerReadHiRes();
-#endif
- 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;
-
- if(OldCount == NewCount)
- goto allocExit;
- Status = cCmdDVArrayAlloc(DVIndex, NewCount);
-
- if (Status < NO_ERR)
- goto allocExit;
-
- if(!IS_AGGREGATE_TYPE(cCmdDSType(INC_ID(DSElementID))))
- goto allocExit;
-
- 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))
- goto allocExit;
- }
- }
- 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))
- goto allocExit;
- }
- }
-
- NXT_ASSERT(cCmdVerifyMemMgr());
-allocExit:
-#if VMProfilingCode
- memMgrTime += dTimerReadHiRes() - enterTime;
-#endif
- 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 old Array Data to new allocation
- if(OldCount)
- memmove(pData, VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset, (UWORD)(DV_ARRAY[DVIndex].ElemSize * OldCount));
- //!!! Clear mem so old mem doesn't contain stale data. Not strictly needed.
-#if WIN_DEBUG || defined(ARM_DEBUG)
- memset(VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset, 0xFF, (UWORD)(DV_ARRAY[DVIndex].ElemSize * OldCount));
-#endif
- //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);
- 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)
-{
- 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;
- }
-
- if(VarsCmd.MemMgr.FreeHead == NOT_A_DS_ID)
- return ERR_MEM;
-
- //Remove DV from free list
- *pIndex = VarsCmd.MemMgr.FreeHead;
- VarsCmd.MemMgr.FreeHead = DV_ARRAY[*pIndex].Link;
- if(VarsCmd.MemMgr.FreeHead != NOT_A_DS_ID)
- DV_ARRAY[VarsCmd.MemMgr.FreeHead].BackLink= NOT_A_DS_ID;
- //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;
-
- 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 prev, post;
-
- //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;
-
- //Remove from MemMgr list
- if (DVIndex == VarsCmd.MemMgr.Head)
- {
- VarsCmd.MemMgr.Head = DV_ARRAY[DVIndex].Link;
- if(VarsCmd.MemMgr.Head != NOT_A_DS_ID)
- DV_ARRAY[VarsCmd.MemMgr.Head].BackLink= NOT_A_DS_ID;
- }
- else
- {
- // patchup middle or end of list.
- prev= DV_ARRAY[DVIndex].BackLink;
- post= DV_ARRAY[DVIndex].Link;
- NXT_ASSERT(prev != NOT_A_DS_ID);
-
- DV_ARRAY[prev].Link = post;
- if(post != NOT_A_DS_ID)
- DV_ARRAY[post].BackLink= prev;
- if (DVIndex == VarsCmd.MemMgr.Tail)
- VarsCmd.MemMgr.Tail = prev;
- //Make sure we found the previous DV, otherwise this DV was not in the the list (already freed?)
- }
-
- //Push onto free list
- DV_ARRAY[DVIndex].Link = VarsCmd.MemMgr.FreeHead;
- DV_ARRAY[DVIndex].BackLink = NOT_A_DS_ID;
- DV_ARRAY[VarsCmd.MemMgr.FreeHead].BackLink= DVIndex;
- 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].BackLink = NOT_A_DS_ID;
- if(VarsCmd.MemMgr.FreeHead != NOT_A_DS_ID)
- DV_ARRAY[VarsCmd.MemMgr.FreeHead].BackLink = i;
- 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;
-}
-
-
-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 prev, post;
-
- //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;
- DV_ARRAY[VarsCmd.MemMgr.Head].BackLink= NOT_A_DS_ID;
- }
- else
- {
- // connect to middle or end of list.
- prev= DV_ARRAY[DVIndex].BackLink;
- post= DV_ARRAY[DVIndex].Link;
- NXT_ASSERT(prev != NOT_A_DS_ID);
- DV_ARRAY[prev].Link = post;
- if(post != NOT_A_DS_ID)
- DV_ARRAY[post].BackLink= prev;
- }
-
- DV_ARRAY[DVIndex].Link = NOT_A_DS_ID;
- DV_ARRAY[DVIndex].BackLink = VarsCmd.MemMgr.Tail;
- if(VarsCmd.MemMgr.Tail != 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;
- DV_ARRAY[DVIndex].BackLink= VarsCmd.MemMgr.Tail;
- DV_ARRAY[DVIndex].Link = NOT_A_DS_ID;
- VarsCmd.MemMgr.Tail = DVIndex;
-
- NXT_ASSERT(cCmdVerifyMemMgr());
-
- return NO_ERR;
-}
-
-
-UBYTE cCmdVerifyMemMgr()
-{
- DV_INDEX i, prev, post;
- 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;
- }
-
- prev= DV_ARRAY[i].BackLink;
- post= DV_ARRAY[i].Link;
- if (post == NOT_A_DS_ID && i != VarsCmd.MemMgr.Tail)
- return FALSE;
- else if(prev == NOT_A_DS_ID && i != VarsCmd.MemMgr.Head)
- return FALSE;
- else if(prev != NOT_A_DS_ID && DV_ARRAY[prev].Link != i)
- return FALSE;
- else if(post != NOT_A_DS_ID && DV_ARRAY[post].BackLink != i)
- return FALSE;
-
- DVCount++;
- }
-
- // could check link and backlinks too
- 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);
- 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;
-}
-
-//
-// Datalog Queue function(s)
-//
-
-NXT_STATUS cCmdDatalogWrite(UBYTE * pData, UWORD Length)
-{
- NXT_STATUS Status = NO_ERR;
-
- if (pData == NULL)
- return ERR_ARG;
-
- 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_DATALOG_SIZE)
- return ERR_INVALID_SIZE;
-
- if (IS_DV_INDEX_SANE(GET_WRITE_DTLG()))
- {
- //A message is already there, the queue is full
- NXT_ASSERT(VarsCmd.DatalogBuffer.WriteIndex == VarsCmd.DatalogBuffer.ReadIndex);
- Status = STAT_MSG_BUFFERWRAP;
- //Bump read index, drop existing message to make room for our newly acquired datalog
- VarsCmd.DatalogBuffer.ReadIndex = (VarsCmd.DatalogBuffer.ReadIndex + 1) % DATALOG_QUEUE_DEPTH;
- }
- else
- {
- //Allocate dope vector for message
- Status = cCmdAllocDopeVector(&GET_WRITE_DTLG(), 1);
- if (IS_ERR(Status))
- return Status;
- }
-
- //Allocate storage for message
- Status |= cCmdDVArrayAlloc(GET_WRITE_DTLG(), Length);
- if (IS_ERR(Status))
- {
- //Clear the dope vector for the message, since we're unable to put a message there.
- cCmdFreeDopeVector(GET_WRITE_DTLG());
- SET_WRITE_DTLG(NOT_A_DS_ID);
- return Status;
- }
-
- //Copy message
- memmove(cCmdDVPtr(GET_WRITE_DTLG()), pData, Length);
-
- //Advance write index
- VarsCmd.DatalogBuffer.WriteIndex = (VarsCmd.DatalogBuffer.WriteIndex + 1) % DATALOG_QUEUE_DEPTH;
-
- return Status;
-}
-
-NXT_STATUS cCmdDatalogGetSize(UWORD * Size)
-{
- DV_INDEX ReadDVIndex;
-
- if (Size == NULL)
- return (ERR_ARG);
-
- if (VarsCmd.ActiveProgHandle == NOT_A_HANDLE)
- {
- *Size = 0;
- return (ERR_NO_PROG);
- }
-
- ReadDVIndex = GET_READ_DTLG();
-
- if (IS_DV_INDEX_SANE(ReadDVIndex))
- {
- *Size = (DV_ARRAY[ReadDVIndex].Count);
- return (NO_ERR);
- }
- else
- {
- *Size = 0;
- return (STAT_MSG_EMPTY_MAILBOX);
- }
-}
-
-NXT_STATUS cCmdDatalogRead(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);
-
- ReadDVIndex = GET_READ_DTLG();
-
- 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_DTLG(NOT_A_DS_ID);
-
- //Advance read index
- VarsCmd.DatalogBuffer.ReadIndex = (VarsCmd.DatalogBuffer.ReadIndex + 1) % DATALOG_QUEUE_DEPTH;
- }
- }
- else
- {
- //No message to read, datalog Queue is empty
- NXT_ASSERT(VarsCmd.DatalogBuffer.ReadIndex == VarsCmd.DatalogBuffer.WriteIndex);
-
- return (STAT_MSG_EMPTY_MAILBOX);
- }
-
- return Status;
-}
-
-
-//
-// Color Sensor Functions
-//
-NXT_STATUS cCmdColorSensorRead (UBYTE Port, SWORD * SensorValue, UWORD * RawArray, UWORD * NormalizedArray,
- SWORD * ScaledArray, UBYTE * InvalidData)
-{
- ULONG i;
- //Make sure Port is valid for Color Sensor
- INPUTSTRUCT * pIn = &(pMapInput->Inputs[Port]);
- UBYTE sType = pIn->SensorType;
- if (!(sType == COLORFULL || sType == COLORRED || sType == COLORGREEN ||
- sType == COLORBLUE || sType == COLORNONE))
- {
- return (ERR_COMM_CHAN_NOT_READY); //TODO - is this the right error?
- }
- //Copy Detected Color
- *SensorValue = pIn->SensorValue;
-
- //Copy all raw, normalized and scaled data from I/O Map
- for (i=0; i<NO_OF_COLORS; i++){
- COLORSTRUCT * pColor = &(pMapInput->Colors[Port]);
- RawArray[i] = pColor->ADRaw[i];
- NormalizedArray[i] = pColor->SensorRaw[i];
- ScaledArray[i] = pColor->SensorValue[i];
- }
- //Copy the Invalid Data Flag
- *InvalidData = pIn->InvalidData;
-
- return NO_ERR;
-
-}
-
-
-//
-// Dataspace Support functions
-//
-
-UBYTE cCmdIsDSElementIDSane(DS_ELEMENT_ID Index)
-{
- if (Index < VarsCmd.DataspaceCount)
- return TRUE;
- else
- return FALSE;
-}
-
-void * cCmdResolveDataArg(DATA_ARG DataArg, UWORD Offset, TYPE_CODE * TypeCode)
-{
- void * ret_val = NULL;
-
- //!!! DATA_ARG masking system only for internal c_cmd use!
- // All normal bytecode arguments should go through top if() block.
-
- NXT_ASSERT(cCmdIsDSElementIDSane(DataArg));
- ret_val = cCmdDSPtr(DataArg, Offset);
- if (TypeCode)
- *TypeCode = VarsCmd.pDataspaceTOC[DataArg].TypeCode;
-
- //!!! Caller beware! If DataArg isn't sane, ret_val may be out of range or NULL!
- return ret_val;
-}
-
-// normal Resolve handles both, but this is specific to I/O args
-void * cCmdResolveIODataArg(DATA_ARG DataArg, ULONG Offset, TYPE_CODE * TypeCode)
- {
- void * ret_val = NULL;
-
- ULONG ModuleID;
- ULONG FieldID;
- //DataArg refers to a field in the IO map
- // ModuleID = ((DataArg >> 9) & 0x1F);
- ModuleID = ((DataArg & 0x3FFF) >> 9);
- FieldID = (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];
- return ret_val;
-}
-
-void cCmdSetValFlt(void * pVal, TYPE_CODE TypeCode, float 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;
-
- case TC_FLOAT:
- {
- *(float*)pVal = (float)NewVal;
- }
- break;
- }
- }
-
- return;
-}
-
-ULONG cCmdGetUByte(void * pVal);
-ULONG cCmdGetSByte(void * pVal);
-ULONG cCmdGetUWord(void * pVal);
-ULONG cCmdGetSWord(void * pVal);
-ULONG cCmdGetULong(void * pVal);
-ULONG cCmdGetSLong(void * pVal);
-ULONG cCmdGetError(void * pVal);
-ULONG cCmdGetFloat(void * pVal);
-
-void cCmdSetByte(void * pVal, ULONG NewVal);
-void cCmdSetWord(void * pVal, ULONG NewVal);
-void cCmdSetLong(void * pVal, ULONG NewVal);
-void cCmdSetError(void * pVal, ULONG NewVal);
-
-
-typedef ULONG (*pGetOperand)(void *);
-static pGetOperand GetProcArray[11]= {cCmdGetUByte, cCmdGetUByte, cCmdGetSByte, cCmdGetUWord, cCmdGetSWord, cCmdGetULong, cCmdGetSLong, cCmdGetError, cCmdGetError, cCmdGetError, cCmdGetFloat}; // dup UByte to line up
-
-typedef void (*pSetOperand)(void *, ULONG);
-static pSetOperand SetProcArray[9]= {cCmdSetByte, cCmdSetByte, cCmdSetByte, cCmdSetWord, cCmdSetWord, cCmdSetLong, cCmdSetLong, cCmdSetError, cCmdSetError}; // dup UByte to line up
-
-void cCmdSetError(void * pVal, ULONG NewVal) {
- NXT_BREAK;
-}
-
-void cCmdSetLong(void * pVal, ULONG NewVal) {
- *(ULONG*)pVal = NewVal;
-}
-
-void cCmdSetWord(void * pVal, ULONG NewVal) {
- *(UWORD*)pVal = (UWORD)NewVal;
-}
-
-void cCmdSetByte(void * pVal, ULONG NewVal) {
- *(UBYTE*)pVal = (UBYTE)NewVal;
-}
-
-// only works on simple types, equivalent to resolve and get, but faster
-ULONG cCmdGetScalarValFromDataArg(DATA_ARG DataArg, UWORD Offset)
-{
- DS_TOC_ENTRY *dsTOCPtr= &VarsCmd.pDataspaceTOC[DataArg];
- return GetProcArray[dsTOCPtr->TypeCode](VarsCmd.pDataspace + dsTOCPtr->DSOffset + Offset);
-}
-
-
-ULONG cCmdGetError(void * pVal) {
- NXT_BREAK;
- return 0;
-}
-
-ULONG cCmdGetULong(void * pVal) {
- return (ULONG)(*(ULONG*)pVal);
-}
-
-ULONG cCmdGetSLong(void * pVal) {
- return (SLONG)(*(SLONG*)pVal);
-}
-
-ULONG cCmdGetUWord(void * pVal) {
- return (UWORD)(*(UWORD*)pVal);
-}
-
-ULONG cCmdGetSWord(void * pVal) {
- return (SWORD)(*(SWORD*)pVal);
-}
-
-ULONG cCmdGetUByte(void * pVal) {
- return (UBYTE)(*(UBYTE*)pVal);
-}
-
-ULONG cCmdGetSByte(void * pVal) {
- return (SBYTE)(*(SBYTE*)pVal);
-}
-
-ULONG cCmdGetFloat(void * pVal) {
- float tempVal = *(float*)pVal;
- if (tempVal >= 0.0f) {
- tempVal += 0.5f;
- }
- else {
- tempVal -= 0.5f;
- }
- return (ULONG)tempVal;
-}
-
-ULONG cCmdGetVal(void * pVal, TYPE_CODE TypeCode)
-{
- if (pVal)
- return GetProcArray[TypeCode](pVal);
- else
- //!!! Default return value places responsibility on caller to use this function wisely
- return 0;
-}
-
-
-float cCmdGetValFlt(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);
- }
-
- case TC_FLOAT:
- {
- return (float)(*(float*)pVal);
- }
-
- default:
- break;
- }
- }
-
- //!!! Default return value places responsibility on caller to use this function wisely
- return 0;
-}
-
-
-
-// Only for scalar types and no offset
-void cCmdSetScalarValFromDataArg(DATA_ARG DataArg, ULONG NewVal)
-{
- DS_TOC_ENTRY *dsTOCPtr= &VarsCmd.pDataspaceTOC[DataArg];
- SetProcArray[dsTOCPtr->TypeCode](VarsCmd.pDataspace + dsTOCPtr->DSOffset, NewVal);
-}
-
-void cCmdSetVal(void * pVal, TYPE_CODE TypeCode, ULONG NewVal)
-{
- if (pVal)
- SetProcArray[TypeCode](pVal, NewVal);
-}
-
-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);
-}
-
-
-UBYTE cCmdCompare(UBYTE CompCode, ULONG Val1, ULONG Val2, TYPE_CODE TypeCode1, TYPE_CODE TypeCode2)
-{
- SLONG SVal1, SVal2;
- if (QUICK_UNSIGNED_TEST(TypeCode1) || QUICK_UNSIGNED_TEST(TypeCode2))
- {
- 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));
- }
- else
- {
- 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));
- }
-}
-
-UBYTE cCmdCompareFlt(UBYTE CompCode, float Val1, float Val2, TYPE_CODE TypeCode1, TYPE_CODE TypeCode2)
-{
- //!!! add threshold to equality comparisons
- 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;
-
- 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))
- {
- ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, Offset2);
- ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, Offset3);
-
- //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;
-}
-
-ULONG gClearProfileInfo= 0, bigExecTime= 0;
-#if VMProfilingCode
-void UpdateProfileInfo(ULONG shortOp, CODE_WORD *pInstr, ULONG execTime, ULONG InstrSize)
-{
- ULONG j;
- ULONG opCode;
-
- if(execTime > 500 && shortOp == 8)
- bigExecTime= shortOp;
- if(gClearProfileInfo) {
- ExecutedInstrs= 0;
- CmdCtrlTime= 0;
- OverheadTime= 0;
- CmdCtrlCalls= 0;
- LastAvgCount= 0;
- for(j= 0; j < 255; j++)
- CmdCtrlClumpTime[j]= 0;
- for(j= 0; j < OPCODE_COUNT; j++) {
- InstrProfile[j].Avg= 0;
- InstrProfile[j].Time= 0;
- InstrProfile[j].Count= 0;
- InstrProfile[j].Max= 0;
- }
- for(j= 0; j < SYSCALL_COUNT; j++) {
- SysCallProfile[j].Avg= 0;
- SysCallProfile[j].Time= 0;
- SysCallProfile[j].Count= 0;
- SysCallProfile[j].Max= 0;
- }
- for(j= 0; j < NUM_SHORT_OPCODE_COUNT; j++) {
- ShortInstrProfile[j].Avg= 0;
- ShortInstrProfile[j].Time= 0;
- ShortInstrProfile[j].Count= 0;
- ShortInstrProfile[j].Max= 0;
- }
- for(j= 0; j < NUM_INTERP_FUNCS; j++) {
- InterpFuncProfile[j].Avg= 0;
- InterpFuncProfile[j].Time= 0;
- InterpFuncProfile[j].Count= 0;
- InterpFuncProfile[j].Max= 0;
- }
- gClearProfileInfo= FALSE;
- }
- ExecutedInstrs ++;
- if(shortOp > 7) // shortop bit set
- {
- ShortInstrProfile[shortOp-8].Time += execTime;
- ShortInstrProfile[shortOp-8].Count++;
- if(execTime > ShortInstrProfile[shortOp-8].Max)
- ShortInstrProfile[shortOp-8].Max= execTime;
- }
- else
- {
- opCode = OP_CODE(pInstr);
- InstrProfile[opCode].Time += execTime;
- InstrProfile[opCode].Count++;
- if(execTime > InstrProfile[opCode].Max)
- InstrProfile[opCode].Max= execTime;
- if(opCode == OP_SYSCALL)
- {
- SysCallProfile[GetDataArg(pInstr[1])].Time += execTime;
- SysCallProfile[GetDataArg(pInstr[1])].Count++;
- if(execTime > SysCallProfile[GetDataArg(pInstr[1])].Max)
- SysCallProfile[GetDataArg(pInstr[1])].Max= execTime;
- }
-
- InterpFuncProfile[InstrSize].Time += execTime;
- InterpFuncProfile[InstrSize].Count++;
- if(execTime > InterpFuncProfile[InstrSize].Max)
- InterpFuncProfile[InstrSize].Max= execTime;
- }
- if(ExecutedInstrs - LastAvgCount > 999) // every N instrs, update avgs
- {
- for(j= 0; j < OPCODE_COUNT; j++)
- if(InstrProfile[j].Count)
- InstrProfile[j].Avg= InstrProfile[j].Time/InstrProfile[j].Count;
- for(j= 0; j < SYSCALL_COUNT; j++)
- if(SysCallProfile[j].Count)
- SysCallProfile[j].Avg= SysCallProfile[j].Time/SysCallProfile[j].Count;
- for(j= 0; j < NUM_SHORT_OPCODE_COUNT; j++)
- if(ShortInstrProfile[j].Count)
- ShortInstrProfile[j].Avg= ShortInstrProfile[j].Time/ShortInstrProfile[j].Count;
- for(j= 0; j < NUM_INTERP_FUNCS; j++)
- if(InterpFuncProfile[j].Count)
- InterpFuncProfile[j].Avg= InterpFuncProfile[j].Time/InterpFuncProfile[j].Count;
- LastAvgCount= ExecutedInstrs;
- }
-}
-#endif
-
-
-//
-// Interpreter Functions
-//
-
-NXT_STATUS cCmdInterpFromClump()
-{
- CLUMP_ID Clump= VarsCmd.RunQ.Head;
- NXT_STATUS Status = NO_ERR;
- CLUMP_REC * pClumpRec;
- CODE_WORD * pInstr, *lastClumpInstr;
- UBYTE InstrSize;
- ULONG shortOp, nextMSTick;
- SLONG i;
-#if VM_BENCHMARK
- ULONG InstrTime = dTimerRead();
-#endif
-
- if (!cCmdIsClumpIDSane(Clump)) // this means all clumps are asleep
- return TIMES_UP;
-
- //Resolve clump record structure and current instruction pointer
- pClumpRec = &(VarsCmd.pAllClumps[Clump]);
- pInstr = pClumpRec->PC; // abs
- lastClumpInstr= pClumpRec->CodeEnd; // abs
-
- i= gInstrsToExecute;
- nextMSTick= dTimerGetNextMSTickCnt();
- do
- {
-#if VMProfilingCode
- ULONG instrStartTime;
- instrStartTime= dTimerReadHiRes();
-#endif
-
- ULONG instrWord= *(UWORD*)pInstr;
- shortOp= (instrWord>>8) & 0x0F;
- if(shortOp > 7) // shortop bit set
- Status= ShortInterpFuncs[shortOp - 8](pInstr);
- else
- { // we know this is a long instr, dispatch on num params, which correlates to size
- InstrSize = INSTR_SIZE(instrWord); // keep in a local for profiling
- Status = (*InterpFuncs[InstrSize])(pInstr);
- }
-
-#if VMProfilingCode
- UpdateProfileInfo(shortOp, pInstr, dTimerReadHiRes() - instrStartTime, InstrSize);
-#endif
-
-afterCompaction:
- if (Status == NO_ERR)
- pInstr += gPCDelta;
- else if (Status == CLUMP_DONE) // already requeued
- {
- pClumpRec->PC = pClumpRec->CodeStart;
- pClumpRec->CurrFireCount = pClumpRec->InitFireCount;
- return Status;
- }
- else if (Status == CLUMP_SUSPEND || Status == BREAKOUT_REQ || Status == ROTATE_QUEUE) // already requeued
- {
- pClumpRec->PC = pInstr + gPCDelta;
- //Throw error if we ever advance beyond the clump's codespace
- if (pInstr > lastClumpInstr)
- {
- NXT_BREAK;
- Status = ERR_INSTR;
- }
- return Status;
- }
- else if (Status == ERR_MEM)
- {
- //Memory is full. Compact dataspace and try the instruction again.
- //!!! Could compact DopeVectorArray here
- cCmdDSCompact();
- if(shortOp > 7) // shortop bit set
- Status= ShortInterpFuncs[shortOp - 8](pInstr);
- else
- Status = (*InterpFuncs[InstrSize])(pInstr);
- if(Status == ERR_MEM)
- return Status;
- else
- goto afterCompaction;
- }
- else // other errors, breakout, stop
- return Status;
-
- //Throw error if we ever advance beyond the clump's codespace
- if (pInstr > lastClumpInstr)
- {
- 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;
- }
- VarsCmd.InstrCount++;
-#endif
-
- //Count one more instruction for this pass
- if ((SLONG)(nextMSTick - dTimerReadTicks()) <= 0) // HWTimer has passed ms tick limit
- Status= TIMES_UP;
- else if(--i <= 0)
- Status= ROTATE_QUEUE;
- } while (!Status);
- pClumpRec->PC= pInstr;
- return (Status);
-}
-
-
-NXT_STATUS cCmdInterpUnop1(CODE_WORD * const pCode)
-{
- NXT_STATUS Status = NO_ERR;
- UBYTE opCode;
- DATA_ARG Arg1;
-
- NXT_ASSERT(pCode != NULL);
-
- gPCDelta= 2;
- opCode = OP_CODE(pCode);
- Arg1 = pCode[1];
-
- switch (opCode)
- {
- case OP_JMP:
- {
- gPCDelta= (SWORD)Arg1;
- Status = NO_ERR;
- }
- break;
-
- case OP_ACQUIRE:
- {
- NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));
- NXT_ASSERT(VarsCmd.pDataspaceTOC[Arg1].TypeCode == TC_MUTEX);
-
- Status = cCmdAcquireMutex((MUTEX_Q *)cCmdDSScalarPtr(Arg1, 0));
- }
- break;
-
- case OP_RELEASE:
- {
- NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));
- NXT_ASSERT(VarsCmd.pDataspaceTOC[Arg1].TypeCode == TC_MUTEX);
-
- Status = cCmdReleaseMutex((MUTEX_Q *)cCmdDSScalarPtr(Arg1, 0));
- }
- break;
-
- case OP_SUBRET:
- {
- NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));
-
- //Take Subroutine off RunQ
- //Add Subroutine's caller to RunQ
- cCmdDeQClump(&(VarsCmd.RunQ), VarsCmd.RunQ.Head);
- cCmdEnQClump(&(VarsCmd.RunQ), *((CLUMP_ID *)cCmdDSScalarPtr(Arg1, 0)));
-
- Status = CLUMP_DONE;
- }
- break;
-
- case OP_FINCLUMPIMMED:
- {
- CLUMP_ID Clump= VarsCmd.RunQ.Head; // DeQ changes Head, use local val
- cCmdDeQClump(&(VarsCmd.RunQ), Clump); //Dequeue finalized clump
- cCmdSchedDependent(Clump, (CLUMP_ID)Arg1); // Use immediate form
-
- Status = CLUMP_DONE;
- }
- break;
-
- case OP_GETTICK:
- {
- cCmdSetScalarValFromDataArg(Arg1, dTimerReadNoPoll());
- }
- break;
-
- case OP_STOP:
- {
- //Unwired Arg1 means always stop
- if (Arg1 == NOT_A_DS_ID)
- Status = STOP_REQ;
- else if (cCmdGetScalarValFromDataArg(Arg1, 0) > 0)
- Status = STOP_REQ;
- }
- break;
-
- default:
- {
- //Fatal error: Unrecognized instruction
- NXT_BREAK;
- Status = ERR_INSTR;
- }
- break;
- }
-
- return (Status);
-}
-
-ULONG scalarNots= 0, scalarBrtst= 0, scalarUn2Other= 0, scalarUn2Dispatch= 0, polyUn2Dispatch= 0;
-NXT_STATUS cCmdInterpScalarUnop2(CODE_WORD * const pCode)
-{
- NXT_STATUS Status;
- UBYTE opCode;
-
- NXT_ASSERT(pCode != NULL);
- opCode = OP_CODE(pCode);
- DATA_ARG Arg1, Arg2;
-
- scalarUn2Dispatch ++;
- if(opCode == OP_NOT) // t2 && t3 guaranteed scalar
- {
- gPCDelta= 3;
- Arg1 = pCode[1];
- Arg2 = pCode[2];
- ULONG ArgVal1, ArgVal2;
-
- ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0);
- //!!! OP_NOT is logical, *not* bit-wise.
- //This differs from the other logical ops because we don't distinguish booleans from UBYTEs.
- ArgVal1= (!ArgVal2);
- cCmdSetScalarValFromDataArg(Arg1, ArgVal1);
- Status = NO_ERR;
- scalarNots ++;
- }
- else if(opCode == OP_BRTST)
- {
- ULONG Branch, compare= COMP_CODE(pCode);
- ULONG TypeCode;
-
- Arg1 = pCode[1];
- Arg2 = pCode[2];
- TypeCode = cCmdDSType(Arg2);
-
- if(Arg2 == NOT_A_DS_ID)
- {
- Branch= ((compare == OPCC1_EQ)
- || (compare == OPCC1_LTEQ)
- || (compare == OPCC1_GTEQ));
- }
- else
- {
- if(compare == OPCC1_EQ && TypeCode == TC_UBYTE) // very common for loops
- {
- UBYTE *pBRVal = (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[Arg2].DSOffset);
- Branch= *pBRVal == 0;
- }
- else
- {
- SLONG SVal1 = (SLONG)cCmdGetScalarValFromDataArg(Arg2, 0);
- Branch= ((compare == OPCC1_EQ && SVal1 == 0)
- || (compare == OPCC1_NEQ && SVal1 != 0)
- || (compare == OPCC1_GT && SVal1 > 0)
- || (compare == OPCC1_LT && SVal1 < 0)
- || (compare == OPCC1_LTEQ && SVal1 <= 0)
- || (compare == OPCC1_GTEQ && SVal1 >= 0));
- }
- }
- if (Branch)
- gPCDelta = (SWORD)Arg1;
- else
- gPCDelta= 3;
- Status = NO_ERR;
- scalarBrtst ++;
- }
- else {
- Status= cCmdInterpUnop2(pCode);
- scalarUn2Other ++;
- }
- 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];
-
- polyUn2Dispatch ++;
- UWORD Count;
- UWORD Offset;
- SLONG TmpSLong;
- ULONG TmpULong;
- ULONG ArgVal2;
- float FltArgVal2;
- char Buffer[30];
- char FormatString[5];
- UBYTE CheckTrailingZeros = 0;
-
- NXT_ASSERT(pCode != NULL);
-
- gPCDelta= 3;
- opCode = OP_CODE(pCode);
- Arg1 = pCode[1];
- Arg2 = pCode[2];
-
- if (opCode == OP_NEG || opCode == OP_NOT || opCode == OP_TST || opCode == OP_SQRT || opCode == OP_ABS)
- {
- return cCmdInterpPolyUnop2(*pCode, Arg1, 0, Arg2, 0);
- }
-
- switch (opCode)
- {
- case OP_MOV:
- {
- Status= cCmdMove(Arg1, Arg2);
- }
- break;
-
- case OP_SET:
- {
- //!!! Should throw error if TypeCode1 is non-scalar
- // Accepting non-scalar destinations could have unpredictable results!
- cCmdSetScalarValFromDataArg(Arg1, Arg2);
- }
- break;
-
- case OP_BRTST:
- {
- //!!!BDUGGAN BRTST w/ Float?
- ULONG Branch, compare= COMP_CODE(pCode);
- ULONG TypeCode = cCmdDSType(Arg2);
- if(compare == OPCC1_EQ && TypeCode == TC_UBYTE) // very common for loops
- {
- UBYTE *pBRVal = (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[Arg2].DSOffset);
- Branch= *pBRVal == 0;
- }
- else
- {
- SLONG SVal1 = (SLONG)cCmdGetScalarValFromDataArg(Arg2, 0);
- Branch= ((compare == OPCC1_EQ && SVal1 == 0)
- || (compare == OPCC1_NEQ && SVal1 != 0)
- || (compare == OPCC1_GT && SVal1 > 0)
- || (compare == OPCC1_LT && SVal1 < 0)
- || (compare == OPCC1_LTEQ && SVal1 <= 0)
- || (compare == OPCC1_GTEQ && SVal1 >= 0));
- }
- if (Branch)
-
- {
- gPCDelta = (SWORD)Arg1;
- Status = NO_ERR;
- }
- }
- break;
-
- case OP_FINCLUMP:
- {
- CLUMP_ID Clump= VarsCmd.RunQ.Head; // DeQ changes Head, use local val
- cCmdDeQClump(&(VarsCmd.RunQ), Clump); //Dequeue finalized clump
- cCmdSchedDependents(Clump, (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 *)(cCmdDSScalarPtr(Arg2, 0))) = VarsCmd.RunQ.Head;
-
- cCmdDeQClump(&(VarsCmd.RunQ), VarsCmd.RunQ.Head); //Take caller off RunQ
- cCmdEnQClump(&(VarsCmd.RunQ), (CLUMP_ID)Arg1); //Add callee to RunQ
-
- Status = CLUMP_SUSPEND;
- }
- break;
-
- case OP_ARRSIZE:
- {
- cCmdSetScalarValFromDataArg(Arg1, 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);
-
- //Make sure we're trying to convert a scalar to a string
- TypeCode2= cCmdDSType(Arg2);
- NXT_ASSERT(!IS_AGGREGATE_TYPE(TypeCode2));
-
- if (TypeCode2 == TC_FLOAT)
- {
- pArg2 = cCmdResolveDataArg(Arg2, 0, NULL);
- FltArgVal2 = cCmdGetValFlt(pArg2, TypeCode2);
- // is number too big for display? then format differently and don't bother with trailing zeros
- if ((FltArgVal2 > 9999999999999.99f)||(FltArgVal2 < -999999999999.99f)){ // these are the widest %.2f numbers that will fit on display
- strcpy (FormatString, "%.6g");
- }
- else{
- strcpy (FormatString, "%.2f");
- CheckTrailingZeros = 1;
- }
- Count = sprintf(Buffer, FormatString, FltArgVal2);
- Count++; //add room for null terminator
-
- if (CheckTrailingZeros){
- // Determine if the trailing digits are zeros. If so, drop them
- if (Buffer[Count-2] == 0x30) { // NOTE: 0x30 is ASCII 0
- if (Buffer[Count-3] == 0x30){
- strcpy (FormatString, "%.0f"); // the last two digits = 0, copy as integer
- Count = Count - 3; // don't need memory for decimal and 2 ascii characters
- }
- else {
- strcpy (FormatString, "%.1f"); // only the 2nd digit = 0 so drop it, but keep the tenths place
- Count = Count - 1; // don't need memory for 2nd ascii character
- }
- }
- }
- }
- else
- {
- ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, 0);
- //Calculate size of array
- if (ArgVal2 == 0)
- Count = 1;
- else {
- Count = 0;
- SLONG digits= 0;
- ULONG Tmp= 1;
- if (TypeCode2 == TC_SLONG || TypeCode2 == TC_SWORD || TypeCode2 == TC_SBYTE)
- {
- TmpSLong = (SLONG)ArgVal2;
- //Add room for negative sign
- if (TmpSLong < 0) {
- Count++;
- TmpULong= -TmpSLong;
- }
- else
- TmpULong= ArgVal2;
- }
- else
- TmpULong= ArgVal2;
-
- while (Tmp <= TmpULong && digits < 10) { // maxint is ten digits, max
- Tmp *= 10;
- digits++;
- }
- Count += digits;
- }
- //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_FLOAT)
- {
- sprintf(pArg1, FormatString, FltArgVal2);
- }
- else 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;
-
- case OP_WAIT:
- {
- ULONG wait= 0;
- //Unwired Arg2 defaults to wait 0, which rotates queue
- if (Arg2 != NOT_A_DS_ID)
- wait= cCmdGetScalarValFromDataArg(Arg2, 0);
- if(wait == 0)
- Status= ROTATE_QUEUE;
- else
- Status = cCmdSleepClump(wait + IOMapCmd.Tick); // put to sleep, to wake up wait ms in future
- if(Arg1 != NOT_A_DS_ID)
- cCmdSetScalarValFromDataArg(Arg1, dTimerReadNoPoll());
- }
- 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 Offset1Param, DATA_ARG Arg2, UWORD Offset2Param)
-{
- NXT_STATUS Status = NO_ERR;
- TYPE_CODE TypeCode1, TypeCode2;
- DV_INDEX DVIndex1, DVIndex2;
- ULONG ArgVal1, ArgVal2;
- float FltArgVal1, FltArgVal2;
- UWORD Count1, Count2, Offset1= Offset1Param, Offset2= Offset2Param;
- 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);
-
- if (TypeCode1 == TC_FLOAT || TypeCode2 == TC_FLOAT)
- {
- pArg2 = cCmdResolveDataArg(Arg2, Offset2, &TypeCode2);
- FltArgVal2 = cCmdGetValFlt(pArg2, TypeCode2);
- FltArgVal1 = cCmdUnop2Flt(Code, FltArgVal2, TypeCode2);
- cCmdSetValFlt(pArg1, TypeCode1, FltArgVal1);
- }
- else
- {
- ArgVal2= cCmdGetScalarValFromDataArg(Arg2, Offset2);
- if(OP_CODE(&Code) == OP_MOV)
- ArgVal1= ArgVal2;
- else
- ArgVal1 = cCmdUnop2(Code, ArgVal2, TypeCode2);
- cCmdSetVal(pArg1, TypeCode1, ArgVal1);
- }
- return Status;
- }
-
- //At least one of the args is an aggregate type
-
- if(TypeCode1 == TC_ARRAY && TypeCode2 == TC_ARRAY) {
- TYPE_CODE tc1, tc2;
- tc1= cCmdDSType(INC_ID(Arg1));
- tc2= cCmdDSType(INC_ID(Arg2));
- if(tc1 <= TC_LAST_INT_SCALAR && tc1 == tc2) {
- void *pArg1, *pArg2;
- ULONG Count = cCmdArrayCount(Arg2, Offset2);
- Status = cCmdDSArrayAlloc(Arg1, Offset1, Count);
- if (IS_ERR(Status))
- return Status;
- pArg1 = cCmdResolveDataArg(Arg1, Offset1, NULL);
- pArg2 = cCmdResolveDataArg(Arg2, Offset2, NULL);
- memmove(pArg1, pArg2, Count * cCmdSizeOf(tc1));
- return Status;
- }
- }
-
-
- //
- // 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);
- }
- if(Count2 == 0) { // both arrays, input is empty, is output already empty?
- Count1= cCmdArrayCount(Arg1, Offset1);
- if(Count1 == 0)
- return NO_ERR;
- }
-
- 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;
-
- if(MinArrayCount == 0) // if we emptied array, nothing else to do.
- return NO_ERR;
- 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));
- if(opCode == OP_MOV)
- return Operand;
- else if(opCode == OP_NEG)
- return (-((SLONG)Operand));
- else if(opCode == 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);
- else if(opCode == OP_TST)
- return cCmdCompare(COMP_CODE((&Code)), Operand, 0, TypeCode, TypeCode);
- else if(opCode == OP_ABS)
- return abs(Operand);
- else
- {
- //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller)
- NXT_BREAK;
- return 0;
- }
-}
-
-float cCmdUnop2Flt(CODE_WORD const Code, float Operand, TYPE_CODE TypeCode)
-{
- UBYTE opCode;
-
- opCode = OP_CODE((&Code));
- if(opCode == OP_MOV)
- return Operand;
- else if(opCode == OP_NEG)
- return (-(Operand));
- else if(opCode == 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);
- else if(opCode == OP_TST)
- return cCmdCompareFlt(COMP_CODE((&Code)), Operand, 0, TypeCode, TypeCode);
- else if(opCode == OP_ABS)
- return fabsf(Operand);
- else if(opCode == OP_SQRT)
- return sqrtf(Operand);
-#if 0
- else if(opCode == OP_SIN)
- return sinf(Operand);
- else if(opCode == OP_COS)
- return cosf(Operand);
- else if(opCode == OP_TAN)
- return tanf(Operand);
- else if(opCode == OP_ASIN)
- return asinf(Operand);
- else if(opCode == OP_ACOS)
- return acosf(Operand);
- else if(opCode == OP_ATAN)
- return atanf(Operand);
-#endif
- else
- {
- //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller)
- NXT_BREAK;
- return 0;
- }
-}
-
-NXT_STATUS cCmdIOGetSet(ULONG opCode, DATA_ARG Arg1, DATA_ARG Arg2, DATA_ARG Arg3)
-{
- ULONG ArgVal1, ArgVal2;
- TYPE_CODE TypeCode2;
- void *pArg2 = NULL;
- switch(opCode)
- {
- case OP_GETOUT:
- {
- ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, 0);
- Arg2 = (UWORD)(0x200 | (Arg3 + ArgVal2 * IO_OUT_FPP));
- pArg2 = cCmdResolveIODataArg(Arg2, 0, &TypeCode2);
- cCmdSetScalarValFromDataArg(Arg1, cCmdGetVal(pArg2, TypeCode2));
- }
- break;
- //!!! All IO map access commands should screen illegal port values!
- // Right now, cCmdResolveIODataArg's implementation allows SETIN/GETIN to access arbitrary RAM!
- case OP_SETIN:
- {
- ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, 0);
- Arg2 = (UWORD)(Arg3 + ArgVal2 * IO_IN_FPP);
- pArg2 = cCmdResolveIODataArg(Arg2, 0, &TypeCode2);
- ArgVal1 = cCmdGetScalarValFromDataArg(Arg1, 0);
- cCmdSetVal(pArg2, TypeCode2, ArgVal1);
- }
- break;
- case OP_GETIN:
- {
- TYPE_CODE TypeCode1;
- void * pArg1;
- ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, 0);
- Arg2 = (UWORD)(Arg3 + ArgVal2 * IO_IN_FPP);
- pArg2 = cCmdResolveIODataArg(Arg2, 0, &TypeCode2);
- TypeCode1= cCmdDSType(Arg1);
- pArg1= cCmdDSScalarPtr(Arg1, 0);
- if(TypeCode1 <= TC_SBYTE && TypeCode1 <= TC_SBYTE) // seems really common
- *(UBYTE*)pArg1= *(UBYTE*)pArg2;
- else
- cCmdSetVal(pArg1, TypeCode1, cCmdGetVal(pArg2, TypeCode2));
- }
- break;
- }
- return NO_ERR;
-}
-
-ULONG scalarCmp= 0, scalarFloatCmp= 0, recursiveCmp= 0, PolyScalarCmp= 0, polyPolyCmp= 0, scalarOther= 0, scalarBinopDispatch= 0, polyBinopDispatch= 0;
-NXT_STATUS cCmdInterpScalarBinop(CODE_WORD * const pCode)
-{
- NXT_STATUS Status;
- UBYTE opCode;
- UBYTE CmpBool;
-
- NXT_ASSERT(pCode != NULL);
- opCode = OP_CODE(pCode);
- DATA_ARG Arg1, Arg2, Arg3;
-
- scalarBinopDispatch ++;
- if(opCode == OP_CMP) // t2 && t3 guaranteed scalar or string
- {
- gPCDelta= 4;
- Arg1 = pCode[1];
- Arg2 = pCode[2];
- Arg3 = pCode[3];
- ULONG ArgVal1, ArgVal2, ArgVal3;
- TYPE_CODE TypeCode2, TypeCode3;
- DS_TOC_ENTRY *dsTOC2Ptr= &VarsCmd.pDataspaceTOC[Arg2];
- DS_TOC_ENTRY *dsTOC3Ptr= &VarsCmd.pDataspaceTOC[Arg3];
-
- TypeCode2 = dsTOC2Ptr->TypeCode;
- TypeCode3 = dsTOC3Ptr->TypeCode;
- if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) {
- ArgVal2= GetProcArray[TypeCode2](VarsCmd.pDataspace + dsTOC2Ptr->DSOffset);
- ArgVal3= GetProcArray[TypeCode3](VarsCmd.pDataspace + dsTOC3Ptr->DSOffset);
- ArgVal1= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3);
- DS_TOC_ENTRY *dsTOC1Ptr= &VarsCmd.pDataspaceTOC[Arg1];
- SetProcArray[dsTOC1Ptr->TypeCode](VarsCmd.pDataspace + dsTOC1Ptr->DSOffset, ArgVal1);
- scalarCmp++;
- Status = NO_ERR;
- }
- else if (TypeCode2 == TC_ARRAY) // two strings
- {
- // memcmp(); here or in compareagg, could use memcmp to speed up string compares ???
- Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0);
- cCmdSetScalarValFromDataArg(Arg1, CmpBool);
- recursiveCmp++;
- }
- else { // floats
- Status = cCmdInterpPolyBinop(*pCode, Arg1, 0, Arg2, 0, Arg3, 0);
- scalarFloatCmp++;
- }
- }
- else if(opCode == OP_BRCMP) { // t2 and t3 guaranteed scalar
- TYPE_CODE TypeCode2, TypeCode3;
- ULONG ArgVal2, ArgVal3;
-
- Arg1 = pCode[1];
- Arg2 = pCode[2];
- Arg3 = pCode[3];
- TypeCode2= cCmdDSType(Arg2);
- TypeCode3= cCmdDSType(Arg3);
- ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0);
- ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0);
- CmpBool= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3);
-
- if (CmpBool)
- gPCDelta = (SWORD)Arg1;
- else
- gPCDelta= 4;
- Status= NO_ERR;
- }
- else if(opCode >= OP_SETIN && opCode <= OP_GETOUT) {
- Arg1 = pCode[1];
- Arg2 = pCode[2];
- Arg3 = pCode[3];
- Status= cCmdIOGetSet(opCode, Arg1, Arg2, Arg3);
- gPCDelta= 4;
- }
- else {
- scalarOther ++;
- Status= cCmdInterpBinop(pCode);
- }
- return Status;
-}
-
-
-NXT_STATUS cCmdInterpBinop(CODE_WORD * const pCode)
-{
- NXT_STATUS Status = NO_ERR;
- UBYTE opCode;
- DATA_ARG Arg1, Arg2, Arg3;
- ULONG ArgVal3;
- UBYTE CmpBool;
- DV_INDEX DVIndex1, DVIndex2;
- UWORD i;
-
- polyBinopDispatch ++;
- gPCDelta= 4;
-
- NXT_ASSERT(pCode != NULL);
- opCode = OP_CODE(pCode);
- Arg1 = pCode[1];
- Arg2 = pCode[2];
- Arg3 = pCode[3];
-
- if (opCode <= OP_XOR) // && ! OP_NEG, can't happen since it is unop
- Status= cCmdInterpPolyBinop(opCode, Arg1, 0, Arg2, 0, Arg3, 0);
- else if(opCode >= OP_SETIN && opCode <= OP_GETOUT)
- Status= cCmdIOGetSet(opCode, Arg1, Arg2, Arg3);
- else {
- switch (opCode)
- {
- case OP_CMP:
- {
- TYPE_CODE TypeCode2= cCmdDSType(Arg2), TypeCode3= cCmdDSType(Arg3);
- if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) {
- ULONG ArgVal1, ArgVal2, ArgVal3;
- ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0);
- ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0);
- ArgVal1= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3);
- cCmdSetScalarValFromDataArg(Arg1, ArgVal1);
- PolyScalarCmp++;
- }
- else if (IS_AGGREGATE_TYPE(TypeCode2) && IS_AGGREGATE_TYPE(TypeCode3) && !IS_AGGREGATE_TYPE(cCmdDSType(Arg1)))
- {
- //Compare Aggregates
- Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0);
- cCmdSetScalarValFromDataArg(Arg1, CmpBool);
- recursiveCmp++;
- }
- else
- {
- //Compare Elements
- Status = cCmdInterpPolyBinop(*pCode, Arg1, 0, Arg2, 0, Arg3, 0);
- polyPolyCmp++;
- }
- }
- break;
-
- case OP_BRCMP:
- {
- TYPE_CODE TypeCode2= cCmdDSType(Arg2), TypeCode3= cCmdDSType(Arg3);
- if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) {
- ULONG ArgVal2, ArgVal3;
- ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0);
- ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0);
- CmpBool= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3);
- }
- else //Compare Aggregates
- Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0);
-
- if (CmpBool)
- gPCDelta = (SWORD)Arg1;
- }
- break;
-
- case OP_INDEX:
- {
- ArgVal3 = (Arg3 != NOT_A_DS_ID) ? cCmdGetScalarValFromDataArg(Arg3, 0) : 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(cCmdDSType(Arg1) == TC_ARRAY);
-
- ArgVal3 = (Arg3 != NOT_A_DS_ID) ? cCmdGetScalarValFromDataArg(Arg3, 0) : 0;
-
- Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)ArgVal3);
- if (!IS_ERR(Status))
- {
- DVIndex1 = cCmdGetDVIndex(Arg1, 0);
- if(cCmdDSType(Arg2) <= TC_LAST_INT_SCALAR)
- {
- ULONG val= cCmdGetScalarValFromDataArg(Arg2, 0);
- TYPE_CODE TypeCode= cCmdDSType(INC_ID(Arg1));
- for (i = 0; i < ArgVal3; i++) // could init ptr and incr by offset GM???
- {
- //copy Arg2 into each element of Arg1
- cCmdSetVal(VarsCmd.pDataspace + ARRAY_ELEM_OFFSET(DVIndex1, i), TypeCode, val);
- }
- }
- else
- 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;
- float FltArgVal1, FltArgVal2, FltArgVal3;
- 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, NULL);
-
- if (TypeCode1 == TC_FLOAT || TypeCode2 == TC_FLOAT || TypeCode3 == TC_FLOAT){
- pArg2 = cCmdResolveDataArg(Arg2, Offset2, NULL);
- pArg3 = cCmdResolveDataArg(Arg3, Offset3, NULL);
- FltArgVal2 = cCmdGetValFlt(pArg2, TypeCode2);
- FltArgVal3 = cCmdGetValFlt(pArg3, TypeCode3);
- FltArgVal1 = cCmdBinopFlt(Code, FltArgVal2, FltArgVal3, TypeCode2, TypeCode3);
- cCmdSetValFlt(pArg1, TypeCode1, FltArgVal1);
- }
- else
- {
- ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, Offset2);
- ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, Offset3);
- 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;
- }
- }
-}
-
-
-float cCmdBinopFlt(CODE_WORD const Code, float LeftOp, float 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;
-
- return LeftOp / RightOp;
- }
-
- case OP_MOD:
- {
- //As with OP_DIV, make sure (x % 0) = x is well-defined
- if (RightOp == 0)
- return (LeftOp);
-
- return (SLONG)LeftOp % (SLONG)RightOp;
- }
-
- case OP_AND:
- {
- return ((SLONG)LeftOp & (SLONG)RightOp);
- }
-
- case OP_OR:
- {
- return ((SLONG)LeftOp | (SLONG)RightOp);
- }
-
- case OP_XOR:
- {
- return (((SLONG)LeftOp | (SLONG)RightOp) & (~((SLONG)LeftOp & (SLONG)RightOp)));
- }
-
- case OP_CMP:
- {
- return cCmdCompareFlt(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);
-}
-
-NXT_STATUS cCmdInterpShortError(CODE_WORD * const pCode)
-{
- //Fatal error: Unrecognized instruction (no current opcodes have zero instructions)
- NXT_BREAK;
- return (ERR_INSTR);
-}
-
-NXT_STATUS cCmdInterpShortSubCall(CODE_WORD * const pCode)
-{
- NXT_STATUS Status;
- DATA_ARG Arg1, Arg2;
-
- gPCDelta= 2;
- Arg1 = GetDataArg(SHORT_ARG(pCode) + pCode[1]);
- Arg2 = GetDataArg(pCode[1]);
- NXT_ASSERT(cCmdIsClumpIDSane((CLUMP_ID)Arg1));
- NXT_ASSERT(!cCmdIsClumpOnQ(&(VarsCmd.RunQ), (CLUMP_ID)Arg1));
-
- NXT_ASSERT(cCmdIsDSElementIDSane(Arg2));
-
- *((CLUMP_ID *)(cCmdDSScalarPtr(Arg2, 0))) = VarsCmd.RunQ.Head;
-
- cCmdDeQClump(&(VarsCmd.RunQ), VarsCmd.RunQ.Head); //Take caller off RunQ
- cCmdEnQClump(&(VarsCmd.RunQ), (CLUMP_ID)Arg1); //Add callee to RunQ
-
- Status = CLUMP_SUSPEND;
-
- return Status;
-}
-
-ULONG moveSameInt= 0, moveDiffInt= 0, moveFloat= 0, moveArrInt= 0, moveOther= 0;
-NXT_STATUS cCmdMove(DATA_ARG Arg1, DATA_ARG Arg2)
-{
- NXT_STATUS Status;
- DS_TOC_ENTRY *TOC1Ptr= &VarsCmd.pDataspaceTOC[Arg1],
- *TOC2Ptr= &VarsCmd.pDataspaceTOC[Arg2];
- TYPE_CODE tc1= TOC1Ptr->TypeCode, tc2= TOC2Ptr->TypeCode;
- void *pArg1, *pArg2;
-
- if(tc1 <= TC_LAST_INT_SCALAR && tc2 <= TC_LAST_INT_SCALAR)
- {
- // if tc1 == tc2, do long, byte, then word assignment
- if(tc1 == tc2)
- {
- moveSameInt++;
- pArg1= VarsCmd.pDataspace + TOC1Ptr->DSOffset;
- pArg2= VarsCmd.pDataspace + TOC2Ptr->DSOffset;
- if(tc1 >= TC_ULONG)
- *(ULONG*)pArg1= *(ULONG*)pArg2;
- else if(tc1 <= TC_SBYTE)
- *(UBYTE*)pArg1= *(UBYTE*)pArg2;
- else
- *(UWORD*)pArg1= *(UWORD*)pArg2;
- Status= NO_ERR;
- }
- else
- {
- moveDiffInt++;
- ULONG val= cCmdGetScalarValFromDataArg(Arg2, 0);
- cCmdSetScalarValFromDataArg(Arg1, val);
- Status= NO_ERR;
- }
- }
- else if(tc1 == TC_FLOAT && tc2 == TC_FLOAT) { // may also need to speed up float to int and int to float conversions
- moveFloat++;
- pArg1= VarsCmd.pDataspace + TOC1Ptr->DSOffset;
- pArg2= VarsCmd.pDataspace + TOC2Ptr->DSOffset;
- *(float*)pArg1= *(float*)pArg2;
- Status= NO_ERR;
- }
- //!!! Optimized move for arrays of ints.
- else if ((tc1 == TC_ARRAY) && (tc2 == TC_ARRAY)
- && ((TOC1Ptr+1)->TypeCode <= TC_LAST_INT_SCALAR)
- && ((TOC1Ptr+1)->TypeCode == (TOC2Ptr+1)->TypeCode))
- {
- ULONG Count;
- moveArrInt++;
- 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 * cCmdSizeOf((TOC1Ptr+1)->TypeCode));
- }
- else { // if ((tc1 == TC_CLUSTER) && (tc2 == TC_CLUSTER))
- moveOther++;
- Status = cCmdInterpPolyUnop2(OP_MOV, Arg1, 0, Arg2, 0);
- }
- return Status;
-}
-
-
-NXT_STATUS cCmdInterpShortMove(CODE_WORD * const pCode)
-{
- NXT_STATUS Status;
- DATA_ARG Arg1, Arg2;
-
- Arg1 = GetDataArg(SHORT_ARG(pCode) + pCode[1]);
- Arg2 = GetDataArg(pCode[1]);
- Status= cCmdMove(Arg1, Arg2);
-
- gPCDelta= 2;
- return Status;
-}
-
-NXT_STATUS cCmdInterpShortAcquire(CODE_WORD * const pCode)
-{
- NXT_STATUS Status;
- DATA_ARG Arg1;
-
- gPCDelta= 1;
- Arg1 = GetDataArg(SHORT_ARG(pCode));
- NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));
- NXT_ASSERT(cCmdDSType(Arg1) == TC_MUTEX);
-
- Status = cCmdAcquireMutex((MUTEX_Q *)cCmdDSScalarPtr(Arg1, 0));
-
- return Status;
-}
-
-NXT_STATUS cCmdInterpShortRelease(CODE_WORD * const pCode)
-{
- NXT_STATUS Status;
- DATA_ARG Arg1;
-
- gPCDelta= 1;
- Arg1 = GetDataArg(SHORT_ARG(pCode));
- NXT_ASSERT(cCmdIsDSElementIDSane(Arg1));
- NXT_ASSERT(cCmdDSType(Arg1) == TC_MUTEX);
-
- Status = cCmdReleaseMutex((MUTEX_Q *)cCmdDSScalarPtr(Arg1, 0));
-
- return Status;
-}
-
-
-//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
- {
- Port = cCmdGetScalarValFromDataArg(PortArg, 0);
- }
-
- //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, TypeCode5;
- ULONG 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 *pArg5 = NULL;
-
- NXT_ASSERT(pCode != NULL);
-
- ULONG sz= INSTR_SIZE(*(UWORD*)pCode);
- if (sz == VAR_INSTR_SIZE)
- sz = ((UWORD*)pCode)[1];
- gPCDelta= sz/2; // advance words, sz is in bytes
-
- 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= cCmdMove(Arg1, Arg2);
- 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)
- ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, 0);
- else //Index input unwired
- ArgVal3 = 0;
-
- if (Arg4 != NOT_A_DS_ID)
- ArgVal4 = cCmdGetScalarValFromDataArg(Arg4, 0);
- 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)
- ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, 0);
- else //Index input unwired
- ArgVal3 = 0;
-
- if (Arg4 != NOT_A_DS_ID)
- ArgVal4 = cCmdGetScalarValFromDataArg(Arg4, 0);
- 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);
- // if flat, use memmove, otherwise this stuff
- if(cCmdDSType(INC_ID(TmpDSID)) <= TC_LAST_INT_SCALAR)
- {
- memmove(VarsCmd.pDataspace + ARRAY_ELEM_OFFSET(DVIndex2, DstIndex), VarsCmd.pDataspace + DV_ARRAY[TmpDVIndex].Offset, (UWORD)(DV_ARRAY[TmpDVIndex].ElemSize * DV_ARRAY[TmpDVIndex].Count));
- DstIndex += DV_ARRAY[TmpDVIndex].Count;
- }
- else
- 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= cCmdMove(Arg1, Arg4);
- 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:
- {
- float ArgValF;
- SLONG decimals= 0;
- UBYTE cont= TRUE;
- // 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);
- pArg3 = cCmdResolveDataArg(Arg3, 0, &TypeCode3);
-
- if (Arg4 != NOT_A_DS_ID)
- ArgVal4 = cCmdGetScalarValFromDataArg(Arg4, 0);
- 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), "%f", &ArgValF) == 1)
- {
- i = (UWORD)ArgVal4;
- //Scan until we see the number, consumes negative sign too
- while ((((UBYTE *)pArg3)[i] < '0') || (((UBYTE *)pArg3)[i] > '9'))
- i++;
-
- //Scan until we get past the number and no more than one decimal
- while (cont) {
- if ((((UBYTE *)pArg3)[i] >= '0') && (((UBYTE *)pArg3)[i] <= '9'))
- i++;
- else if(((UBYTE *)pArg3)[i] == '.' && !decimals) {
- i++;
- decimals++;
- }
- else
- cont= FALSE;
- }
- ArgVal2 = i;
- }
- else
- {
- //Number wasn't found in string, use defaults
- ArgValF = ArgVal5;
- ArgVal2 = 0;
- }
-
- //Set outputs
- cCmdSetValFlt(pArg1, TypeCode1, ArgValF);
- cCmdSetScalarValFromDataArg(Arg2, 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);
- }
-
- INPUTSTRUCT * pInput = &(pMapInput->Inputs[Port]);
-
- //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 (!(pInput->SensorType == LOWSPEED_9V || pInput->SensorType == LOWSPEED)
- || !(pInput->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;
-
- LSBUF * pInBuf = &(pMapLowSpeed->InBuf[Port]);
-
- //Normally, bytes available is a simple difference.
- Tmp = pInBuf->InPtr - pInBuf->OutPtr;
-
- //If InPtr is actually behind OutPtr, circular buffer has wrapped. Account for wrappage...
- if (Tmp < 0)
- Tmp = (pInBuf->InPtr + (SIZE_OF_LSBUF - pInBuf->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);
- }
-
- INPUTSTRUCT * pInput = &(pMapInput->Inputs[Port]);
- UBYTE * pChState = &(pMapLowSpeed->ChannelState[Port]);
- LSBUF * pOutBuf = &(pMapLowSpeed->OutBuf[Port]);
-
- //Only start writing process if port is properly configured and c_lowspeed module is ready
- if ((pInput->SensorType == LOWSPEED_9V || pInput->SensorType == LOWSPEED)
- && (pInput->InvalidData == FALSE)
- && (*pChState == LOWSPEED_IDLE) || (*pChState == LOWSPEED_ERROR))
- {
- pOutBuf->InPtr = 0;
- pOutBuf->OutPtr = 0;
-
- memcpy(pOutBuf->Buf, pBuf, BufLength);
- pOutBuf->InPtr = (UBYTE)BufLength;
-
- pMapLowSpeed->InBuf[Port].BytesToRx = ResponseLength;
-
- *pChState = 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;
-
- LSBUF * pInBuf = &(pMapLowSpeed->InBuf[Port]);
-
- //If the bytes we want to read wrap around the end, we must first read the end, then reset back to the beginning
- if (pInBuf->OutPtr + BytesToRead >= SIZE_OF_LSBUF)
- {
- BytesToRead = SIZE_OF_LSBUF - pInBuf->OutPtr;
- memcpy(pBuf, pInBuf->Buf + pInBuf->OutPtr, BytesToRead);
- pInBuf->OutPtr = 0;
- pBuf += BytesToRead;
- BytesToRead = BufLength - BytesToRead;
- }
-
- memcpy(pBuf, pInBuf->Buf + pInBuf->OutPtr, BytesToRead);
- pInBuf->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]);
- UBYTE sndVol= *(ArgV[3]);
- 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;
-
- if(sndVol > 4)
- sndVol= 4;
- pMapSound->Volume = sndVol;
- //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[])
-{
- UBYTE sndVol= *(ArgV[4]);
- pMapSound->Freq = *(UWORD*)(ArgV[1]);
- pMapSound->Duration = *(UWORD*)(ArgV[2]);
- if(sndVol > 4)
- sndVol= 4;
- pMapSound->Volume = sndVol;
- 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);
-}
-
-
-
-//
-//cCmdWrapColorSensorRead
-//ArgV[0]: (return) Error code, SBYTE
-//ArgV[1]: Port, UBYTE
-//ArgV[2]: SensorValue, SWORD
-//ArgV[3]: RawArray, UWORD[NO_OF_COLORS]
-//ArgV[4]: NormalizedArray, UWORD[NO_OF_COLORS]
-//ArgV[5]: ScaledArray, SWORD[NO_OF_COLORS]
-//ArgV[6]: InvalidData, UBYTE
-//
-NXT_STATUS cCmdWrapColorSensorRead (UBYTE * ArgV[])
-{
- DV_INDEX DVIndex;
- NXT_STATUS Status = NO_ERR;
- //Resolve return val arguments
- SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
- //Resolve Port argument
- UBYTE Port = *(UBYTE*)(ArgV[1]);
- //Resolve SensorValue
- SWORD SensorValue = *(SWORD*)(ArgV[2]);
- //Resolve RawArray as array
- DVIndex = *(DV_INDEX*)(ArgV[3]);
- NXT_ASSERT(IS_DV_INDEX_SANE(DestDVIndex));
- Status= cCmdDVArrayAlloc(DVIndex, NO_OF_COLORS);
- if (IS_ERR(Status))
- return (Status);
- ArgV[3] = cCmdDVPtr (DVIndex);
- //Resolve NormalizedArray as array
- DVIndex = *(DV_INDEX*)(ArgV[4]);
- NXT_ASSERT(IS_DV_INDEX_SANE(DestDVIndex));
- Status= cCmdDVArrayAlloc(DVIndex, NO_OF_COLORS);
- if (IS_ERR(Status))
- return (Status);
- ArgV[4] = cCmdDVPtr (DVIndex);
- //Resolve ScaledArray as array
- DVIndex = *(DV_INDEX*)(ArgV[5]);
- NXT_ASSERT(IS_DV_INDEX_SANE(DestDVIndex));
- Status= cCmdDVArrayAlloc(DVIndex, NO_OF_COLORS);
- if (IS_ERR(Status))
- return (Status);
- ArgV[5] = cCmdDVPtr (DVIndex);
- //Resolve InvalidData
- UBYTE InvalidData = *(UBYTE*)(ArgV[6]);
-
- //call implementation with unwrapped parameters
- *pReturnVal = cCmdColorSensorRead (Port, &SensorValue, (UWORD*)ArgV[3], (UWORD*)ArgV[4], (SWORD*)ArgV[5], &InvalidData);
-
- *(ArgV[2]) = SensorValue;
- *(ArgV[6]) = InvalidData;
-
- if (IS_ERR(*pReturnVal)){
- return (*pReturnVal);
- }
- 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
-
-
-/////////////////////////////////////////////////////////////
-// Dymanic syscall implementations
-////////////////////////////////////////////////////////////
-
-//
-//cCmdWrapDatalogWrite
-//ArgV[0]: (return) Error Code, SBYTE (NXT_STATUS)
-//ArgV[1]: Message, CStr
-//
-NXT_STATUS cCmdWrapDatalogWrite(UBYTE * ArgV[])
-{
- NXT_STATUS Status = NO_ERR;
- DV_INDEX DVIndex;
-
- //Resolve array arguments
- DVIndex = *(DV_INDEX *)(ArgV[1]);
- ArgV[1] = cCmdDVPtr(DVIndex);
-
- Status = cCmdDatalogWrite(ArgV[1], DV_ARRAY[DVIndex].Count);
-
- *(SBYTE *)(ArgV[0]) = Status;
-
- if (IS_FATAL(Status))
- return Status;
- else
- return (NO_ERR);
-}
-
-//
-//cCmdWrapDatalogGetTimes
-//ArgV[0]: SyncTime, U32
-//ArgV[1]: SyncTick, U32
-//
-NXT_STATUS cCmdWrapDatalogGetTimes(UBYTE * ArgV[])
-{
- *((ULONG *)ArgV[1]) = IOMapCmd.SyncTime;
- *((ULONG *)ArgV[2]) = IOMapCmd.SyncTick;
- return (NO_ERR);
-}
-
-//
-//cCmdWrapSetSleepTimeout
-//ArgV[0]: (return) Status byte, SBYTE
-//ArgV[1]: desired timer limit in ms, ULONG
-//
-NXT_STATUS cCmdWrapSetSleepTimeout(UBYTE * ArgV[])
-{
- ULONG value = *(ULONG*)(ArgV[1]);
- if(value==0)
- {
- pMapUi->SleepTimeout=0;
- }
- else if(value < 60000)
- {
- pMapUi->SleepTimeout=1; //integer math would've made this zero
- }
- else
- {
- pMapUi->SleepTimeout= value / 60000;
- }
- return (NO_ERR);
-}
-
-// currently copied from LS, not finished.
-//
-//cCmdWrapCommHSWrite
-//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 cCmdWrapCommHSWrite(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);
-}
-
-//
-//cCmdWrapCommHSCheckStatus
-//ArgV[0]: (return) Status code, SBYTE
-//ArgV[1]: Port specifier, UBYTE
-//ArgV[2]: BytesReady, UBYTE
-//
-NXT_STATUS cCmdWrapCommHSCheckStatus(UBYTE * ArgV[])
-{
- UBYTE Port = *(ArgV[1]);
-
- *((SBYTE*)(ArgV[0])) = cCmdLSCheckStatus(Port);
- *((UBYTE*)(ArgV[2])) = cCmdLSCalcBytesReady(Port);
-
- return (NO_ERR);
-}
-
-//
-//cCmdWrapCommHSRead
-//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 cCmdWrapCommHSRead(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);
-}
-
-//
-//cCmdWrapCommBTOnOff
-//ArgV[0]: (return) Status byte, SBYTE
-//ArgV[1]: Power State, 0-1
-//
-NXT_STATUS cCmdWrapCommBTOnOff(UBYTE * ArgV[])
-{
- UWORD retVal;
- NXT_STATUS status;
- SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
-
- UBYTE powerState = *(ArgV[1]);
- if(powerState)
- status= pMapComm->pFunc(BTON, 0, 0, 0, NULL, &retVal);
- else
- status= pMapComm->pFunc(BTOFF, 0, 0, 0, NULL, &retVal);
-
- *pReturnVal= (status == SUCCESS) ? retVal : status;
- return (NO_ERR);
-}
-
-//
-//cCmdWrapCommBTConnection
-//ArgV[0]: (return) Status byte, SBYTE
-//ArgV[1]: Action, UBYTE
-//ArgV[2]: name, UBYTE array CStr
-//ArgV[3]: connection slot, UBYTE
-//
-NXT_STATUS cCmdWrapCommBTConnection(UBYTE * ArgV[])
-{
- UWORD retVal;
- NXT_STATUS status;
- SBYTE * pReturnVal = (SBYTE*)(ArgV[0]);
- UBYTE *nmPtr;
-
- UBYTE action = *(ArgV[1]);
- UBYTE connection = *(ArgV[3]);
- nmPtr = cCmdDVPtr(*(DV_INDEX *)(ArgV[2]));
-
- if(action) // Init
- status= pMapComm->pFunc(CONNECTBYNAME, 0, connection, 0, nmPtr, &retVal);
- else // Close
- status= pMapComm->pFunc(DISCONNECT, connection, 0, 0, NULL, &retVal);
-
- *pReturnVal= (status == SUCCESS) ? retVal : status;
- return (NO_ERR);
-}
-
-
-//
-//cCmdWrapReadSemData
-//ArgV[0]: return data, U8
-//ArgV[1]: which (0=used, 1=request), U8
-//
-NXT_STATUS cCmdWrapReadSemData(UBYTE * ArgV[])
-{
- if(!(*((UBYTE *)ArgV[1])))
- *((UBYTE *)ArgV[0])= gUsageSemData;
- else
- *((UBYTE *)ArgV[0])= gRequestSemData;
- return (NO_ERR);
-}
-
-//
-//cCmdWrapWriteSemData
-//ArgV[0]: return data, U8
-//ArgV[1]: which (0=used, 1=request), U8
-//ArgV[2]: newValue, U8
-//ArgV[3]: action (0= OR, 1= AND), U8
-//
-NXT_STATUS cCmdWrapWriteSemData(UBYTE * ArgV[])
-{
- UBYTE curVal, newVal, which= (*((UBYTE *)ArgV[1]));
- if(!which)
- curVal= gUsageSemData;
- else
- curVal= gRequestSemData;
-
- newVal= *((UBYTE *)ArgV[2]);
-
- if(*((UBYTE *)ArgV[3]))
- curVal &= ~newVal;
- else
- curVal |= newVal;
-
- if(!which)
- gUsageSemData= curVal;
- else
- gRequestSemData= curVal;
- *((UBYTE *)ArgV[0])= curVal;
- return (NO_ERR);
-}
-
-
-//
-//cCmdWrapUpdateCalibCacheInfo
-//ArgV[0]: return data, U8
-//ArgV[1]: nm, UBYTE array CStr
-//ArgV[2]: min, U16
-//ArgV[3]: max , U16
-//
-NXT_STATUS cCmdWrapUpdateCalibCacheInfo(UBYTE * ArgV[])
-{
- UBYTE *nm= cCmdDVPtr(*(DV_INDEX *)(ArgV[1]));
- SWORD min= (*((SWORD *)ArgV[2]));
- SWORD max= (*((SWORD *)ArgV[3]));
-
- cCmdUpdateCalibrationCache(nm, min, max);
- *((UBYTE *)ArgV[0])= SUCCESS;
- return (NO_ERR);
-}
-
-//
-//cCmdWrapComputeCalibValue
-//ArgV[0]: return data, U8
-//ArgV[1]: nm, UBYTE array CStr
-//ArgV[2]: raw, U16 ref in out
-NXT_STATUS cCmdWrapComputeCalibValue (UBYTE * ArgV[])
-{
- UBYTE *nm= cCmdDVPtr(*(DV_INDEX *)(ArgV[1]));
- SWORD raw= (*((SWORD *)ArgV[2]));
-
- *((UBYTE *)ArgV[0])= cCmdComputeCalibratedValue(nm, &raw);
- (*((SWORD *)ArgV[2]))= raw;
- return (NO_ERR);
-}
-
-typedef struct {
- SWORD min, max;
- UBYTE nm[FILENAME_LENGTH + 1];
-} CalibCacheType;
-
-SBYTE gCalibCacheCnt= 0;
-DV_INDEX gCalibCacheArrayDVIdx= NOT_A_DS_ID;
-CalibCacheType *gCalibCacheArray= NULL;
-
-SWORD cCmdGetCalibrationIndex(UBYTE *nm) {
- SBYTE i;
- for(i= 0; i < gCalibCacheCnt; i++)
- if(!strcmp((PSZ)nm, (PSZ)gCalibCacheArray[i].nm))
- break;
- return i;
-}
-
-NXT_STATUS cCmdComputeCalibratedValue(UBYTE *nm, SWORD *pRaw) {
- SBYTE i= cCmdGetCalibrationIndex(nm);
- NXT_STATUS status= ERR_RC_ILLEGAL_VAL;
- SLONG raw= *pRaw, range;
- if(i < gCalibCacheCnt) {
- status= SUCCESS;
- raw -= gCalibCacheArray[i].min;
- range= (gCalibCacheArray[i].max - gCalibCacheArray[i].min);
- }
- else
- range= 1023;
- raw *= 100;
- raw /= range;
- if(raw < 0) raw= 0;
- else if(raw > 100) raw= 100;
- *pRaw= raw;
- return status;
-}
-
-
-NXT_STATUS ResizeCalibCache(ULONG elements) { // alloc dv if needed, grow if needed. dv never freed. on boot, set to NOT_A_DS_ID. use cnt for valid elements.
- NXT_STATUS Status = NO_ERR;
-
- if(gCalibCacheArrayDVIdx == NOT_A_DS_ID)
- Status = cCmdAllocDopeVector(&gCalibCacheArrayDVIdx, sizeof(CalibCacheType));
- if(!IS_ERR(Status) && DV_ARRAY[gCalibCacheArrayDVIdx].Count < elements) //Allocate storage for cache element
- Status = cCmdDVArrayAlloc(gCalibCacheArrayDVIdx, elements);
- if(!IS_ERR(Status))
- gCalibCacheArray= cCmdDVPtr(gCalibCacheArrayDVIdx);
- // on error, does old DVIdx still point to array, or should we null out array???
- return Status;
-}
-
-// called to update min/max on existing cache element, and to add new named element
-void cCmdUpdateCalibrationCache(UBYTE *nm, SWORD min, SWORD max) {
- SWORD i= cCmdGetCalibrationIndex(nm);
- NXT_STATUS Status = NO_ERR;
-
- if(i == gCalibCacheCnt) { // sensor wasn't found, insert into cache
- Status= ResizeCalibCache(gCalibCacheCnt+1);
- if(!IS_ERR(Status)) {
- gCalibCacheCnt++;
- strcpy((PSZ)gCalibCacheArray[i].nm, (PSZ)nm);
- }
- }
- if(!IS_ERR(Status)) {
- gCalibCacheArray[i].min= min;
- gCalibCacheArray[i].max= max;
- }
-}
-
-void cCmdLoadCalibrationFiles(void) {
- ULONG cnt, DataSize;
- UBYTE nm[FILENAME_LENGTH + 1], nmLen;
- SWORD Handle, HandleSearch;
- gCalibCacheCnt= 0;
- gCalibCacheArrayDVIdx= NOT_A_DS_ID;
- // file I/O to load all .cal files into cached globals used by scaling syscall
- HandleSearch = pMapLoader->pFunc(FINDFIRST, "*.cal", nm, &cnt); // returns total files and nm of first one
- while (LOADER_ERR(HandleSearch) == SUCCESS) { // if we have a file, process it by closing and opening
- SWORD min= 0, max= 0, tmp;
- ULONG length;
- pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(HandleSearch), NULL, NULL);
- Handle = pMapLoader->pFunc(OPENREAD, nm, NULL, &DataSize);
- if (LOADER_ERR(Handle) == SUCCESS && DataSize == 4) {
- // access data, two bytes for min and two for max
- length= 2;
- pMapLoader->pFunc(READ,LOADER_HANDLE_P(Handle),(UBYTE*)&tmp,&length);
- if (length == 2)
- min= tmp;
- length= 2;
- pMapLoader->pFunc(READ,LOADER_HANDLE_P(Handle),(UBYTE*)&tmp,&length);
- if (length == 2)
- max= tmp;
- }
- pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(Handle), NULL, NULL);
- // update calibration cache with nm, min, and max
- nmLen= strlen((PSZ)nm) - 4; // chop off .cal extension
- nm[nmLen]= 0;
- cCmdUpdateCalibrationCache(nm, min, max);
-
- HandleSearch = pMapLoader->pFunc(FINDNEXT, LOADER_HANDLE_P(HandleSearch), nm, &cnt);
- }
- pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(HandleSearch), NULL, NULL);
-}
-
-//
-//cCmdWrapListFiles
-//ArgV[0]: return data, SBYTE
-//ArgV[1]: pattern, UBYTE array CStr
-//ArgV[2]: list, UBYTE array CStr array ref in out
-NXT_STATUS cCmdWrapListFiles (UBYTE * ArgV[])
-{
- ULONG fileSize, matchCount=0, i=0, oldCount;
- SWORD HandleSearch;
- NXT_STATUS Status = NO_ERR;
- DV_INDEX listIdx, *list;
- UBYTE *strTemp, *pattern;
- UBYTE name[FILENAME_LENGTH + 1];
-
- //Resolve array arguments
- pattern = cCmdDVPtr(*(DV_INDEX *)(ArgV[1]));
- listIdx = *(DV_INDEX *)(ArgV[2]);
-
- HandleSearch = pMapLoader->pFunc(FINDFIRST, pattern, name, &fileSize); // returns first file matching pattern
-
- //Count how many files we're going to have
- while (LOADER_ERR(HandleSearch) == SUCCESS)
- {
- matchCount++;
- pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(HandleSearch), NULL, NULL);
- HandleSearch = pMapLoader->pFunc(FINDNEXT, LOADER_HANDLE_P(HandleSearch), name, &fileSize);
- }
-
- HandleSearch = pMapLoader->pFunc(FINDFIRST, pattern, name, &fileSize); // returns first file matching pattern
-
- oldCount = DV_ARRAY[listIdx].Count; // Check to see how many dope vectors are already in the array (if they passed us a non-blank array of strings)
-
- Status = cCmdDVArrayAlloc(listIdx, matchCount); // Size the top-level array
- if(IS_ERR(Status))
- return Status;
-
- list = (DV_INDEX*)(VarsCmd.pDataspace + DV_ARRAY[listIdx].Offset); // Get a pointer into the dataspace for the array of DV_INDEXes
-
- while (LOADER_ERR(HandleSearch) == SUCCESS && !IS_ERR(Status))
- {
- pMapLoader->pFunc(CLOSE, LOADER_HANDLE_P(HandleSearch), NULL, NULL); // Close the handle that we automatically opened above
- // Allocate a new dope vector if one doesn't already exist
- if(i >= oldCount)
- Status = cCmdAllocDopeVector(&(list[i]), sizeof(char));
-
- // Allocate the string buffer for output array[i]
- if(!IS_ERR(Status))
- Status = cCmdDVArrayAlloc(list[i], strlen((PSZ)name) + 1);
-
- if(!IS_ERR(Status))
- {
- strTemp = VarsCmd.pDataspace + DV_ARRAY[list[i]].Offset; // Get a pointer into the dataspace for this string
- strcpy((PSZ)strTemp, (PSZ)name);
- }
- i++;
-
- HandleSearch = pMapLoader->pFunc(FINDNEXT, LOADER_HANDLE_P(HandleSearch), name, &fileSize);
- }
-
- *(SBYTE *)(ArgV[0]) = Status;
-
- return Status;
-}
-
-#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));
-#error // CodeStart is now absolute, but not sure how to fix
- 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"
-
-#endif //ENABLE_VM