From 6c5075bc99ba07f46562f7f10db14e153d7a93ec Mon Sep 17 00:00:00 2001 From: jhansen Date: Mon, 26 Apr 2010 21:49:44 +0000 Subject: git-svn-id: https://mindboards.svn.sourceforge.net/svnroot/mindboards/lms_nbcnxc_128/trunk@6 c9361245-7fe8-9947-84e8-057757c4e366 --- AT91SAM7S256/Source/c_cmd.c | 2722 +++++++++++++++++++++++++++++++++---------- 1 file changed, 2134 insertions(+), 588 deletions(-) (limited to 'AT91SAM7S256/Source/c_cmd.c') diff --git a/AT91SAM7S256/Source/c_cmd.c b/AT91SAM7S256/Source/c_cmd.c index 4ce9829..9253735 100644 --- a/AT91SAM7S256/Source/c_cmd.c +++ b/AT91SAM7S256/Source/c_cmd.c @@ -1,11 +1,11 @@ // // Date init 14.12.2004 // -// Revision date $Date: 24-06-09 8:53 $ +// Revision date $Date: 17-02-09 7:30 $ // // Filename $Workfile:: c_cmd.c $ // -// Version $Revision: 14 $ +// Version $Revision: 13 $ // // Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/c_cmd. $ // @@ -43,13 +43,13 @@ #include #include #include // for sqrt, abs, and trig stuff +#include #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 @@ -192,7 +192,7 @@ static pSysCall SysCallFuncs[SYSCALL_COUNT] = cCmdWrapKeepAlive, cCmdWrapIOMapRead, cCmdWrapIOMapWrite, - cCmdWrapColorSensorRead, + cCmdWrapColorSensorRead, // new in 2.0 cCmdWrapCommBTOnOff, // 35 cCmdWrapCommBTConnection, cCmdWrapCommHSWrite, @@ -205,8 +205,61 @@ static pSysCall SysCallFuncs[SYSCALL_COUNT] = cCmdWrapDatalogWrite, cCmdWrapDatalogGetTimes, //45 cCmdWrapSetSleepTimeout, - cCmdWrapListFiles //47 - + cCmdWrapListFiles, //47 + cCmdWrapUndefinedSysCall, // leave a gap so that I don't have to keep renumbering system calls + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, // 50 + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, // 55 + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, // 60 + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, // 65 + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, // 70 + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, // 75 + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, +// enhanced NBC/NXC + cCmdWrapIOMapReadByID, // 78 + cCmdWrapIOMapWriteByID, + cCmdWrapDisplayExecuteFunction, // 80 + cCmdWrapCommExecuteFunction, + cCmdWrapLoaderExecuteFunction, + cCmdWrapFileFindFirst, + cCmdWrapFileFindNext, + cCmdWrapFileOpenWriteLinear, // 85 + cCmdWrapFileOpenWriteNonLinear, + cCmdWrapFileOpenReadLinear, + cCmdWrapCommHSControl, + cCmdWrapCommLSWriteEx, + cCmdWrapFileSeek, // 90 + cCmdWrapFileResize, + cCmdWrapDrawPictureArray, + cCmdWrapDrawPolygon, + cCmdWrapDrawEllipse, + cCmdWrapDrawFont, // 95 + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall, + cCmdWrapUndefinedSysCall // 99 --> 100 system call slots + // don't forget to update SYSCALL_COUNT in c_cmd.h }; @@ -266,6 +319,7 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] = 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 //IO_OUT1 TC_UBYTE, //IO_OUT_FLAGS @@ -283,6 +337,7 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] = 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 //IO_OUT2 TC_UBYTE, //IO_OUT_FLAGS @@ -300,6 +355,7 @@ TYPE_CODE IO_TYPES_OUT[IO_OUT_FIELD_COUNT] = 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 }; @@ -461,32 +517,64 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) case RC_SET_OUT_STATE: { + UBYTE Port = pInBuf[1]; //Don't do anything if illegal port specification is made - if (pInBuf[1] >= NO_OF_OUTPUTS && pInBuf[1] != 0xFF) + // supported ports are 0, 1, 2 == A, B, C + // 3 == AB, 4 == AC, 5 == BC, 6 == ABC + if (Port > RC_OUT_ABC && Port < RC_PORTS_AB) { RCStatus = ERR_RC_ILLEGAL_VAL; break; } //0xFF is protocol defined to mean "all ports". - if (pInBuf[1] == 0xFF) - { - FirstPort = 0; - LastPort = NO_OF_OUTPUTS - 1; + switch(Port) { + case RC_PORTS_ALL: + case RC_OUT_ABC: + case RC_PORTS_AC: + case RC_OUT_AC: + { + FirstPort = 0; + LastPort = NO_OF_OUTPUTS - 1; + } + break; + case RC_PORTS_BC: + case RC_OUT_BC: + { + // B&C + FirstPort = 1; + LastPort = NO_OF_OUTPUTS - 1; + } + break; + case RC_PORTS_AB: + case RC_OUT_AB: + { + // A&B + FirstPort = 0; + LastPort = 1; + } + break; + default: + { + FirstPort = LastPort = Port; + } + break; } - else - FirstPort = LastPort = pInBuf[1]; for (i = FirstPort; i <= LastPort; i++) { - pMapOutPut->Outputs[i].Speed = pInBuf[2]; - pMapOutPut->Outputs[i].Mode = pInBuf[3]; - pMapOutPut->Outputs[i].RegMode = pInBuf[4]; - pMapOutPut->Outputs[i].SyncTurnParameter = pInBuf[5]; - pMapOutPut->Outputs[i].RunState = pInBuf[6]; - memcpy((PSZ)(&(pMapOutPut->Outputs[i].TachoLimit)), (PSZ)(&pInBuf[7]), 4); - - pMapOutPut->Outputs[i].Flags |= UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT; + if (((Port == RC_PORTS_AC) || (Port == RC_OUT_AC)) && (i > FirstPort) && (i < LastPort)) + continue; + 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; @@ -502,12 +590,12 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) RCStatus = ERR_RC_ILLEGAL_VAL; break; } + INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]); - pMapInput->Inputs[i].SensorType = pInBuf[2]; - pMapInput->Inputs[i].SensorMode = pInBuf[3]; - + pIn->SensorType = pInBuf[2]; + pIn->SensorMode = pInBuf[3]; //Set InvalidData flag automatically since type may have changed - pMapInput->Inputs[i].InvalidData = TRUE; + pIn->InvalidData = TRUE; } break; @@ -525,45 +613,46 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) ResponseLen += 22; break; } + OUTPUT * pOut = &(pMapOutPut->Outputs[i]); //Echo port pOutBuf[ResponseLen] = i; ResponseLen++; //Power - pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].Speed; + pOutBuf[ResponseLen] = pOut->Speed; ResponseLen++; //Mode - pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].Mode; + pOutBuf[ResponseLen] = pOut->Mode; ResponseLen++; //RegMode - pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].RegMode; + pOutBuf[ResponseLen] = pOut->RegMode; ResponseLen++; //TurnRatio - pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].SyncTurnParameter; + pOutBuf[ResponseLen] = pOut->SyncTurnParameter; ResponseLen++; //RunState - pOutBuf[ResponseLen] = pMapOutPut->Outputs[i].RunState; + pOutBuf[ResponseLen] = pOut->RunState; ResponseLen++; //TachoLimit ULONG - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].TachoLimit)), 4); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->TachoLimit)), 4); ResponseLen += 4; //TachoCount SLONG - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].TachoCnt)), 4); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->TachoCnt)), 4); ResponseLen += 4; //BlockTachoCount SLONG - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].BlockTachoCount)), 4); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->BlockTachoCount)), 4); ResponseLen += 4; //RotationCount SLONG - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapOutPut->Outputs[i].RotationCount)), 4); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pOut->RotationCount)), 4); ResponseLen += 4; NXT_ASSERT(ResponseLen == 23); @@ -589,9 +678,11 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) //Echo port pOutBuf[ResponseLen] = i; ResponseLen++; + + INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]); //Set "Valid?" boolean - if (pMapInput->Inputs[i].InvalidData) + if (pIn->InvalidData) pOutBuf[ResponseLen] = FALSE; else pOutBuf[ResponseLen] = TRUE; @@ -603,24 +694,24 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) pOutBuf[ResponseLen] = FALSE; ResponseLen++; - pOutBuf[ResponseLen] = pMapInput->Inputs[i].SensorType; + pOutBuf[ResponseLen] = pIn->SensorType; ResponseLen++; - pOutBuf[ResponseLen] = pMapInput->Inputs[i].SensorMode; + pOutBuf[ResponseLen] = pIn->SensorMode; ResponseLen++; //Set Raw, Normalized, and Scaled values - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].ADRaw)), 2); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->ADRaw)), 2); ResponseLen += 2; - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorRaw)), 2); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->SensorRaw)), 2); ResponseLen += 2; - memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pMapInput->Inputs[i].SensorValue)), 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)(&(pMapInput->Inputs[i].SensorRaw)), 2); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)(&(pIn->SensorRaw)), 2); ResponseLen += 2; NXT_ASSERT(ResponseLen == 14); @@ -683,14 +774,7 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) //pInBuf[2] is a selector //FALSE: Position relative to start of last program //TRUE: Position relative to start of last motor control block - if (pInBuf[2] == FALSE) - { - pMapOutPut->Outputs[i].Flags |= UPDATE_RESET_ROTATION_COUNT; - } - else - { - pMapOutPut->Outputs[i].Flags |= UPDATE_RESET_BLOCK_COUNT; - } + pMapOutPut->Outputs[i].Flags |= (pInBuf[2] ? UPDATE_RESET_BLOCK_COUNT : UPDATE_RESET_ROTATION_COUNT); } break; @@ -759,7 +843,7 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) break; } - RCStatus = cCmdLSWrite(i, Count, &(pInBuf[4]), pInBuf[3]); + RCStatus = cCmdLSWrite(i, Count, &(pInBuf[4]), pInBuf[3], 0); } break; @@ -958,6 +1042,14 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) pMapUi->SleepTimeout= value / 60000; } break; + case RC_PROP_DEBUGGING: { // ulong debug info + ULONG value; + memcpy((PSZ)&value, (PSZ)&(pInBuf[2]), 4); + VarsCmd.Debugging = (UBYTE)((value>>24)&0xFF); + VarsCmd.PauseClump = (UBYTE)((value>>16)&0xFF); + VarsCmd.PausePC = (CODE_INDEX)(value&0xFFFF); + } + break; default: //Unknown property -- still inform client to not expect any response bytes NXT_BREAK; @@ -979,7 +1071,14 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) } break; case RC_PROP_SLEEP_TIMEOUT: { - ULONG value= (pMapUi->SleepTimeout * 60 * 1000); + ULONG value= (pMapUi->SleepTimeout * 60000); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&value, 4); + ResponseLen += 4; + } + break; + case RC_PROP_DEBUGGING: { // ulong debug info + ULONG value; + value = ((VarsCmd.Debugging<<24)|(VarsCmd.PauseClump<<16)|VarsCmd.PausePC); memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&value, 4); ResponseLen += 4; } @@ -992,7 +1091,7 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) } } break; - case RC_UPDATE_RESET_COUNT: + case RC_UPDATE_RESET_COUNT: { i = pInBuf[1]; @@ -1006,6 +1105,87 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) pMapOutPut->Outputs[i].Flags |= UPDATE_RESET_COUNT; } break; + case RC_SET_VM_STATE: + { + // don't change the VM state if the state is currently idle or resetting + if (VarsCmd.VMState > VM_IDLE && VarsCmd.VMState < VM_RESET1) + { + VarsCmd.VMState = (VM_STATE)pInBuf[1]; + // setting the VM state turns on debugging + VarsCmd.Debugging = TRUE; + if (VarsCmd.VMState == VM_RESET1) + IOMapCmd.ProgStatus = PROG_ABORT; + } + // fall through to RC_GET_VM_STATE + } + case RC_GET_VM_STATE: + { + if (SendResponse == TRUE) + { + // output the vm state, current clump and its relative program counter (4 bytes) + pOutBuf[ResponseLen] = VarsCmd.VMState; + ResponseLen++; + pOutBuf[ResponseLen] = VarsCmd.RunQ.Head; + ResponseLen++; + CLUMP_REC* pClumpRec = &(VarsCmd.pAllClumps[VarsCmd.RunQ.Head]); + CODE_INDEX pc = (CODE_INDEX)(pClumpRec->PC-pClumpRec->CodeStart); + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&(pc), 2); + ResponseLen += 2; + } + } + break; + + case RC_SET_BREAKPOINTS: + { + CLUMP_ID Clump = (CLUMP_ID)pInBuf[1]; + //Don't do anything if illegal clump specification is made + if (Clump >= VarsCmd.AllClumpsCount) + { + RCStatus = ERR_RC_ILLEGAL_VAL; + break; + } + // setting breakpoint information turns on debugging mode + VarsCmd.Debugging = TRUE; + CLUMP_BREAK_REC* pBreakpoints = VarsCmd.pAllClumps[Clump].Breakpoints; + // length varies from 6 bytes min to 18 bytes max + // clump byte, bpidx, bplocation (2 bytes), bp enabled, [...] terminal byte 0xFF + UBYTE idx = 2; + UBYTE bDone = FALSE; + while (!bDone) { + UBYTE bpIdx = (UBYTE)pInBuf[idx]; + idx++; + memcpy((PSZ)(&(pBreakpoints[bpIdx].Location)), (PSZ)(&pInBuf[idx]), 2); + idx += 2; + pBreakpoints[bpIdx].Enabled = (UBYTE)pInBuf[idx]; + idx++; + bDone = (((UBYTE)pInBuf[idx] == 0xFF) || (idx >= 18)); + } + // fall through to RC_GET_BREAKPOINTS + } + + case RC_GET_BREAKPOINTS: + { + if (SendResponse == TRUE) + { + // output the list of breakpoints for the specified clump ID + CLUMP_ID Clump = (CLUMP_ID)pInBuf[1]; + //Don't do anything if illegal clump specification is made + if (Clump >= VarsCmd.AllClumpsCount) + { + RCStatus = ERR_RC_ILLEGAL_VAL; + break; + } + CLUMP_BREAK_REC* pBreakpoints = VarsCmd.pAllClumps[Clump].Breakpoints; + for(int j = 0; j < MAX_BREAKPOINTS; j++) + { + memcpy((PSZ)&(pOutBuf[ResponseLen]), (PSZ)&(pBreakpoints[j].Location), 2); + ResponseLen += 2; + pOutBuf[ResponseLen] = pBreakpoints[j].Enabled; + ResponseLen++; + } + } + } + break; default: { //Unknown remote command -- still inform client to not expect any response bytes @@ -1092,32 +1272,35 @@ void cCmdInit(void* pHeader) //Initialize IO_PTRS_OUT for (i = 0; i < NO_OF_OUTPUTS; i++) { - IO_PTRS_OUT[IO_OUT_FLAGS + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Flags); - IO_PTRS_OUT[IO_OUT_MODE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Mode); - IO_PTRS_OUT[IO_OUT_SPEED + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Speed); - IO_PTRS_OUT[IO_OUT_ACTUAL_SPEED + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].ActualSpeed); - IO_PTRS_OUT[IO_OUT_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].TachoCnt); - IO_PTRS_OUT[IO_OUT_TACH_LIMIT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].TachoLimit); - IO_PTRS_OUT[IO_OUT_RUN_STATE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RunState); - IO_PTRS_OUT[IO_OUT_TURN_RATIO + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].SyncTurnParameter); - IO_PTRS_OUT[IO_OUT_REG_MODE + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegMode); - IO_PTRS_OUT[IO_OUT_OVERLOAD + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].Overloaded); - IO_PTRS_OUT[IO_OUT_REG_P_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegPParameter); - IO_PTRS_OUT[IO_OUT_REG_I_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegIParameter); - IO_PTRS_OUT[IO_OUT_REG_D_VAL + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RegDParameter); - IO_PTRS_OUT[IO_OUT_BLOCK_TACH_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].BlockTachoCount); - IO_PTRS_OUT[IO_OUT_ROTATION_COUNT + i * IO_OUT_FPP] = (void*)&(pMapOutPut->Outputs[i].RotationCount); + 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); } //Initialize IO_PTRS_IN for (i = 0; i < NO_OF_INPUTS; i++) { - IO_PTRS_IN[IO_IN_TYPE + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorType); - IO_PTRS_IN[IO_IN_MODE + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorMode); - IO_PTRS_IN[IO_IN_ADRAW + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].ADRaw); - IO_PTRS_IN[IO_IN_NORMRAW + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorRaw); - IO_PTRS_IN[IO_IN_SCALEDVAL + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].SensorValue); - IO_PTRS_IN[IO_IN_INVALID_DATA + i * IO_IN_FPP] = (void*)&(pMapInput->Inputs[i].InvalidData); + 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) @@ -1190,12 +1373,14 @@ void cCmdCtrl(void) VarsCmd.CmdCtrlCount++; #endif //Abort current program if cancel button is pressed - if (IOMapCmd.DeactivateFlag == TRUE || pMapButton->State[BTN1] & PRESSED_EV) + if (IOMapCmd.DeactivateFlag == TRUE || + ((pMapButton->State[BTN1] & pMapUi->AbortFlag) && + ((pMapButton->State[BTN4] & PRESSED_EV) != PRESSED_EV))) // JCH 2010-01-13 Make sure enter button is not also pressed { IOMapCmd.DeactivateFlag = FALSE; //Clear pressed event so it doesn't get double-counted by UI - pMapButton->State[BTN1] &= ~PRESSED_EV; + pMapButton->State[BTN1] &= ~(pMapUi->AbortFlag); //Go to VM_RESET1 state and report abort VarsCmd.VMState = VM_RESET1; @@ -1221,6 +1406,9 @@ void cCmdCtrl(void) #if VMProfilingCode CmdCtrlClumpTime[clump] += dTimerReadHiRes() - ClumpEnterTime; #endif + // automatically switch from RUN_SINGLE to RUN_PAUSE after a single step + if (VarsCmd.VMState == VM_RUN_SINGLE) + VarsCmd.VMState = VM_RUN_PAUSE; //If RunQ and RestQ are empty, program is done, or wacko if (!cCmdIsClumpIDSane(VarsCmd.RunQ.Head)) { @@ -1230,9 +1418,10 @@ void cCmdCtrl(void) IOMapCmd.ProgStatus = PROG_OK; } } - else if (Status == CLUMP_SUSPEND || Status == CLUMP_DONE) + 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; @@ -1245,7 +1434,8 @@ void cCmdCtrl(void) { Continue = FALSE; VarsCmd.VMState = VM_RESET1; - IOMapCmd.ProgStatus = PROG_ERROR; +// IOMapCmd.ProgStatus = PROG_ERROR; + IOMapCmd.ProgStatus = Status; } else if (Status == STOP_REQ) { @@ -1257,7 +1447,7 @@ void cCmdCtrl(void) { Continue = FALSE; } - } while (Continue == TRUE); + } while (Continue == TRUE && VarsCmd.VMState == VM_RUN_FREE); #if VMProfilingCode FinishTime= dTimerReadHiRes(); if(NotFirstCall) @@ -1286,7 +1476,8 @@ void cCmdCtrl(void) //2. Proceed to VM_RESET1 (some unneeded work, yes, but preserves contract with UI if (IS_ERR(Status)) { - IOMapCmd.ProgStatus = PROG_ERROR; +// IOMapCmd.ProgStatus = PROG_ERROR; + IOMapCmd.ProgStatus = Status; VarsCmd.VMState = VM_RESET1; } //Else start running program @@ -1295,10 +1486,6 @@ void cCmdCtrl(void) 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 @@ -1344,7 +1531,7 @@ void cCmdCtrl(void) 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 @@ -1392,7 +1579,13 @@ void cCmdCtrl(void) } while (IOMapCmd.Tick == dTimerRead()); // delay until scheduled time } - break; + break; + + case VM_RUN_PAUSE: + { + while (IOMapCmd.Tick == dTimerRead()); // delay until scheduled time + } + break; }//END state machine switch //Set tick to new value for next time 'round @@ -1425,6 +1618,16 @@ NXT_STATUS cCmdReadFileHeader(UBYTE* pData, ULONG DataSize, 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); @@ -1486,7 +1689,7 @@ NXT_STATUS cCmdReadFileHeader(UBYTE* pData, ULONG DataSize, //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); + return (ERR_CLUMP_COUNT); else VarsCmd.AllClumpsCount = (CLUMP_ID)FileClumpCount; @@ -1495,7 +1698,7 @@ NXT_STATUS cCmdReadFileHeader(UBYTE* pData, ULONG DataSize, //Can't have a valid program with no code if (VarsCmd.CodespaceCount == 0) - return (ERR_FILE); + return (ERR_NO_CODE); // // Now, calculate offsets for each data segment in the file @@ -1536,7 +1739,7 @@ NXT_STATUS cCmdReadFileHeader(UBYTE* pData, ULONG DataSize, if (CurrOffset != (DataSize - VarsCmd.CodespaceCount * 2)) { NXT_BREAK; - return (ERR_FILE); + return (ERR_INSANE_OFFSET); } // @@ -1573,7 +1776,7 @@ NXT_STATUS cCmdReadFileHeader(UBYTE* pData, ULONG DataSize, if (VarsCmd.PoolSize > POOL_MAX_SIZE) { NXT_BREAK; - return (ERR_FILE); + return (ERR_BAD_POOL_SIZE); } return (NO_ERR); @@ -1671,7 +1874,7 @@ NXT_STATUS cCmdActivateProgram(UBYTE * pFileName) //If Loader returned error or bad file pointer, bail out if (LOADER_ERR(LStatus) != SUCCESS || pData == NULL || DataSize == 0) - return (ERR_FILE); + return (ERR_LOADER_ERR); //Deactivate current program and re-initialize memory pool cCmdDeactivateProgram(); @@ -1702,7 +1905,7 @@ NXT_STATUS cCmdActivateProgram(UBYTE * pFileName) || (VarsCmd.DataspaceSize == 0) ) { NXT_BREAK; - return ERR_FILE; + return ERR_SPOTCHECK_FAIL; } //Initialize CLUMP_RECs as contiguous list in RAM @@ -1717,6 +1920,14 @@ NXT_STATUS cCmdActivateProgram(UBYTE * pFileName) //Initialize remaining CLUMP_REC fields clumpPtr->PC = clumpPtr->CodeStart; clumpPtr->Link = NOT_A_CLUMP; + clumpPtr->Priority = INSTR_MAX_COUNT; + + CLUMP_BREAK_REC* pBreakpoints = clumpPtr->Breakpoints; + for (j = 0; j < MAX_BREAKPOINTS; j++) + { + pBreakpoints[j].Location = 0; + pBreakpoints[j].Enabled = FALSE; + } //Activate any clumps with CurrFireCount of 0 clumpPtr->CurrFireCount = clumpPtr->InitFireCount; @@ -1804,7 +2015,7 @@ NXT_STATUS cCmdActivateProgram(UBYTE * pFileName) } //Programs with no active clumps constitutes an activation error if (VarsCmd.RunQ.Head == NOT_A_CLUMP) - return (ERR_FILE); + return (ERR_NO_ACTIVE_CLUMP); else { // now that we know which clumps are scalar and poly, refresh dispatch table to match head @@ -1828,7 +2039,7 @@ NXT_STATUS cCmdActivateProgram(UBYTE * pFileName) || (DefaultsOffset + FileOffsets.DynamicDefaultsSize != FileOffsets.DSDefaultsSize)) { NXT_BREAK; - return (ERR_FILE); + return (ERR_DEFAULT_OFFSETS); } //Copy Dynamic defaults from file @@ -1845,7 +2056,7 @@ NXT_STATUS cCmdActivateProgram(UBYTE * pFileName) if ((UBYTE *)VarsCmd.MemMgr.pDopeVectorArray != VarsCmd.pDataspace + DV_ARRAY[0].Offset) { NXT_BREAK; - return (ERR_FILE); + return (ERR_MEMMGR_FAIL); } //Initialize message queues @@ -1867,7 +2078,7 @@ NXT_STATUS cCmdActivateProgram(UBYTE * pFileName) { 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) @@ -1881,6 +2092,14 @@ NXT_STATUS cCmdActivateProgram(UBYTE * pFileName) gRequestSemData= 0; // preload all calibration coefficients into mem cCmdLoadCalibrationFiles(); + // initialize the graphic globals + gpPassedImgVars = NULL; + memset(gpImgData,0,sizeof(gpImgData)); + gPassedVarsCount = 0; + // configure debugging flags in VarsCmd + VarsCmd.Debugging = FALSE; + VarsCmd.PauseClump = NOT_A_CLUMP; + VarsCmd.PausePC = 0xFFFF; return (Status); } @@ -1889,6 +2108,10 @@ void cCmdDeactivateProgram() { UBYTE i, tmp; + // reset the DS and DVA Offsets + IOMapCmd.OffsetDVA = 0xFFFF; + IOMapCmd.OffsetDS = 0xFFFF; + //Wipe away all references into the pool and clear all run-time data VarsCmd.pCodespace = NULL; VarsCmd.CodespaceCount = 0; @@ -1927,12 +2150,19 @@ void cCmdDeactivateProgram() tmp = i; //Close file if (*(VarsCmd.FileHandleTable[i]) != 0) - pMapLoader->pFunc(CROPDATAFILE, &tmp, NULL, NULL); + pMapLoader->pFunc(CLOSE, &tmp, NULL, NULL); +// pMapLoader->pFunc(CROPDATAFILE, &tmp, NULL, NULL); } //Clear FileHandleTable memset(VarsCmd.FileHandleTable, 0, sizeof(VarsCmd.FileHandleTable)); - + + // reset AbortFlag to default value + pMapUi->AbortFlag = PRESSED_EV; + + // reset Contrast to default value + pMapDisplay->Contrast = DISPLAY_CONTRAST_DEFAULT; + return; } @@ -1952,29 +2182,31 @@ void cCmdResetDevices(void) for (i = 0; i < NO_OF_INPUTS; i++) { + INPUTSTRUCT * pIn = &(pMapInput->Inputs[i]); //Clear type and mode to defaults - pMapInput->Inputs[i].SensorType = NO_SENSOR; - pMapInput->Inputs[i].SensorMode = RAWMODE; + pIn->SensorType = NO_SENSOR; + pIn->SensorMode = RAWMODE; //Reset input values to 0 prior to running (clear things like stale rotation counts) - pMapInput->Inputs[i].ADRaw = 0; - pMapInput->Inputs[i].SensorRaw = 0; - pMapInput->Inputs[i].SensorValue = 0; + pIn->ADRaw = 0; + pIn->SensorRaw = 0; + pIn->SensorValue = 0; //Assert invalid data flag so future code is aware of these changes - pMapInput->Inputs[i].InvalidData = TRUE; + pIn->InvalidData = TRUE; } for (i = 0; i < NO_OF_OUTPUTS; i++) { //Coast and reset all motor parameters - pMapOutPut->Outputs[i].Mode = 0; - pMapOutPut->Outputs[i].RegMode = REGULATION_MODE_IDLE; - pMapOutPut->Outputs[i].RunState = MOTOR_RUN_STATE_IDLE; - pMapOutPut->Outputs[i].Speed = 0; - pMapOutPut->Outputs[i].TachoLimit = 0; - pMapOutPut->Outputs[i].SyncTurnParameter = 0; - pMapOutPut->Outputs[i].Flags = UPDATE_MODE | UPDATE_SPEED | UPDATE_TACHO_LIMIT | UPDATE_RESET_COUNT | UPDATE_RESET_BLOCK_COUNT | UPDATE_RESET_ROTATION_COUNT; + 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 !!! @@ -2201,6 +2433,45 @@ NXT_STATUS cCmdAcquireMutex(MUTEX_Q * Mutex) return (Status); } +UBYTE cCmdIsClumpOnAMutexWaitQ(CLUMP_ID Clump) +{ + //Make sure Clump's ID is valid + NXT_ASSERT(cCmdIsClumpIDSane(Clump)); + DATA_ARG Arg1; + MUTEX_Q * Mutex; + for (Arg1=0; Arg1 < VarsCmd.DataspaceCount; Arg1++) + { + if (VarsCmd.pDataspaceTOC[Arg1].TypeCode == TC_MUTEX) + { + Mutex = cCmdDSPtr(Arg1, 0); + if (cCmdIsClumpOnQ(&(Mutex->WaitQ), Clump)) + return TRUE; + } + } + return FALSE; +} + +NXT_STATUS cCmdReleaseAllMutexes(CLUMP_ID Clump) +{ + //Make sure Clump's ID is valid + NXT_ASSERT(cCmdIsClumpIDSane(Clump)); + DATA_ARG Arg1; + MUTEX_Q * Mutex; + for (Arg1=0; Arg1 < VarsCmd.DataspaceCount; Arg1++) + { + if (VarsCmd.pDataspaceTOC[Arg1].TypeCode == TC_MUTEX) + { + Mutex = cCmdDSPtr(Arg1, 0); + // if this clump owns the Mutex then release it + if (Mutex->Owner == Clump) + cCmdReleaseMutex(Mutex); + // also make sure that this Clump is not waiting in this mutex's wait queue + if (cCmdIsClumpOnQ(&(Mutex->WaitQ), Clump)) + cCmdDeQClump(&(Mutex->WaitQ), Clump); + } + } + return (NO_ERR); +} NXT_STATUS cCmdReleaseMutex(MUTEX_Q * Mutex) { @@ -3226,32 +3497,33 @@ NXT_STATUS cCmdDatalogRead(UBYTE * pBuffer, UWORD Length, UBYTE Remove) return Status; } - // // Color Sensor Functions // NXT_STATUS cCmdColorSensorRead (UBYTE Port, SWORD * SensorValue, UWORD * RawArray, UWORD * NormalizedArray, SWORD * ScaledArray, UBYTE * InvalidData) { - ULONG i; + ULONG i; //Make sure Port is valid for Color Sensor - if (!(pMapInput->Inputs[Port].SensorType == COLORFULL || pMapInput->Inputs[Port].SensorType == COLORRED - || pMapInput->Inputs[Port].SensorType == COLORGREEN || pMapInput->Inputs[Port].SensorType == COLORBLUE - || pMapInput->Inputs[Port].SensorType == COLORNONE)) + 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 = pMapInput->Inputs[Port].SensorValue; + *SensorValue = pIn->SensorValue; //Copy all raw, normalized and scaled data from I/O Map for (i=0; iColors[Port].ADRaw[i]; - NormalizedArray[i] = pMapInput->Colors[Port].SensorRaw[i]; - ScaledArray[i] = pMapInput->Colors[Port].SensorValue[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 = pMapInput->Inputs[Port].InvalidData; + *InvalidData = pIn->InvalidData; return NO_ERR; @@ -3363,13 +3635,14 @@ void cCmdSetByte(void * pVal, ULONG NewVal); void cCmdSetWord(void * pVal, ULONG NewVal); void cCmdSetLong(void * pVal, ULONG NewVal); void cCmdSetError(void * pVal, ULONG NewVal); +void cCmdSetFloat(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 +static pSetOperand SetProcArray[11]= {cCmdSetByte, cCmdSetByte, cCmdSetByte, cCmdSetWord, cCmdSetWord, cCmdSetLong, cCmdSetLong, cCmdSetError, cCmdSetError, cCmdSetError, cCmdSetFloat}; // dup UByte to line up void cCmdSetError(void * pVal, ULONG NewVal) { NXT_BREAK; @@ -3387,6 +3660,10 @@ void cCmdSetByte(void * pVal, ULONG NewVal) { *(UBYTE*)pVal = (UBYTE)NewVal; } +void cCmdSetFloat(void * pVal, ULONG NewVal) { + *(float*)pVal = (float)NewVal; +} + // only works on simple types, equivalent to resolve and get, but faster ULONG cCmdGetScalarValFromDataArg(DATA_ARG DataArg, UWORD Offset) { @@ -3394,6 +3671,11 @@ ULONG cCmdGetScalarValFromDataArg(DATA_ARG DataArg, UWORD Offset) return GetProcArray[dsTOCPtr->TypeCode](VarsCmd.pDataspace + dsTOCPtr->DSOffset + Offset); } +float cCmdGetFloatValFromDataArg(DATA_ARG DataArg, UWORD Offset) +{ + DS_TOC_ENTRY *dsTOCPtr= &VarsCmd.pDataspaceTOC[DataArg]; + return (float)(*(float*)(VarsCmd.pDataspace + dsTOCPtr->DSOffset + Offset)); +} ULONG cCmdGetError(void * pVal) { NXT_BREAK; @@ -3426,11 +3708,11 @@ ULONG cCmdGetSByte(void * pVal) { ULONG cCmdGetFloat(void * pVal) { float tempVal = *(float*)pVal; - if (tempVal >= 0) { - tempVal += 0.5; + if (tempVal >= (float)0.0) { + tempVal += (float)0.5; } else { - tempVal -= 0.5; + tempVal -= (float)0.5; } return (ULONG)tempVal; } @@ -3822,6 +4104,17 @@ UWORD cCmdGetDVIndex(DS_ELEMENT_ID DSElementID, UWORD Offset) return DVIndex; } +UWORD cCmdArrayDimensions(DS_ELEMENT_ID DSElementID) +{ + NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID)); + UWORD result = 0; + while (cCmdDSType(DSElementID) == TC_ARRAY) + { + result++; + DSElementID = INC_ID(DSElementID); + } + return result; +} UWORD cCmdArrayCount(DS_ELEMENT_ID DSElementID, UWORD Offset) { @@ -4137,10 +4430,66 @@ NXT_STATUS cCmdInterpFromClump() pInstr = pClumpRec->PC; // abs lastClumpInstr= pClumpRec->CodeEnd; // abs - i= gInstrsToExecute; +/* + // are we free running and reach a breakpoint? + if (VarsCmd.VMState == VM_RUN_FREE) + { + CLUMP_BREAK_REC* pBreakpoints = pClumpRec->Breakpoints; + for(int j = 0; j < MAX_BREAKPOINTS; j++) + { + if (pBreakpoints[j].Enabled && + (pBreakpoints[j].Location == (CODE_INDEX)(pClumpRec->PC-pClumpRec->CodeStart))) + { + VarsCmd.VMState = VM_RUN_PAUSE; + return BREAKOUT_REQ; + } + } + // auto pause at clump == pauseClump and relative PC = pausePC + if ((Clump == VarsCmd.PauseClump) && + ((CODE_INDEX)(pClumpRec->PC-pClumpRec->CodeStart) == VarsCmd.PausePC)) + { + VarsCmd.VMState = VM_RUN_PAUSE; + // turn off the auto pause flags + VarsCmd.PauseClump = NOT_A_CLUMP; + VarsCmd.PausePC = 0xFFFF; + return BREAKOUT_REQ; + } + } +*/ + + if(VarsCmd.VMState == VM_RUN_FREE) + i = pClumpRec->Priority; + else + i = 1; nextMSTick= dTimerGetNextMSTickCnt(); do { + // are we debugging and are free running and reach a breakpoint/autopause? + if (VarsCmd.Debugging && (VarsCmd.VMState == VM_RUN_FREE)) + { + CLUMP_BREAK_REC* pBreakpoints = pClumpRec->Breakpoints; + for(int j = 0; j < MAX_BREAKPOINTS; j++) + { + if (pBreakpoints[j].Enabled && + (pBreakpoints[j].Location == (CODE_INDEX)(pClumpRec->PC-pClumpRec->CodeStart))) + { + VarsCmd.VMState = VM_RUN_PAUSE; + return BREAKOUT_REQ; + } + } + // auto pause at clump == pauseClump and relative PC = pausePC + if ((Clump == VarsCmd.PauseClump) && + ((CODE_INDEX)(pClumpRec->PC-pClumpRec->CodeStart) == VarsCmd.PausePC)) + { + // pause the VM + VarsCmd.VMState = VM_RUN_PAUSE; + // and turn off the auto pause flags + VarsCmd.PauseClump = NOT_A_CLUMP; + VarsCmd.PausePC = 0xFFFF; + return BREAKOUT_REQ; + } + } + #if VMProfilingCode ULONG instrStartTime; instrStartTime= dTimerReadHiRes(); @@ -4291,6 +4640,22 @@ NXT_STATUS cCmdInterpUnop1(CODE_WORD * const pCode) } break; + case OP_WAITI: + case OP_WAITV: + { + ULONG wait= 0; + if (opCode == OP_WAITV) { + wait = cCmdGetScalarValFromDataArg(Arg1, 0); + } + else + wait = Arg1; + if(wait == 0) + Status= ROTATE_QUEUE; + else + Status = cCmdSleepClump(wait + IOMapCmd.Tick); // put to sleep, to wake up wait ms in future + } + break; + case OP_GETTICK: { cCmdSetScalarValFromDataArg(Arg1, dTimerReadNoPoll()); @@ -4307,6 +4672,45 @@ NXT_STATUS cCmdInterpUnop1(CODE_WORD * const pCode) } break; + case OP_STOPCLUMPIMMED: + { + // Release any mutexes that the clump we are stopping owns + CLUMP_ID Clump = (CLUMP_ID)Arg1; + // release any mutexes owned by this clump + // and remove it from any wait queues that it might be on + cCmdReleaseAllMutexes(Clump); + if (cCmdIsClumpOnQ(&(VarsCmd.RunQ), Clump)) { + // remove the specified clump from the run queue if it is on it + cCmdDeQClump(&(VarsCmd.RunQ), Clump); + } + else if (cCmdIsClumpOnQ(&(VarsCmd.RestQ), Clump)) { + // if the specified clump happened to be sleeping then + // remove it from the rest queue + cCmdDeQClump(&(VarsCmd.RestQ), Clump); + } + // since we have stopped that clump we should reset its clump rec values. + CLUMP_REC* pClumpRec = &(VarsCmd.pAllClumps[Clump]); + pClumpRec->PC = pClumpRec->CodeStart; + pClumpRec->CurrFireCount = pClumpRec->InitFireCount; + pClumpRec->awakenTime = 0; + } + break; + + case OP_STARTCLUMPIMMED: + { + CLUMP_ID Clump = (CLUMP_ID)Arg1; + // only enqueue the clump if it is not already on the run queue + // otherwise this is a no-op + if (!cCmdIsClumpOnQ(&(VarsCmd.RunQ), Clump) && + !cCmdIsClumpOnQ(&(VarsCmd.RestQ), Clump) && + !cCmdIsClumpOnAMutexWaitQ(Clump)) + { + cCmdEnQClump(&(VarsCmd.RunQ), Clump); //Enqueue the specified clump + Status = CLUMP_SUSPEND; + } + } + break; + default: { //Fatal error: Unrecognized instruction @@ -4408,13 +4812,10 @@ NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode) polyUn2Dispatch ++; UWORD Count; UWORD Offset; - SLONG TmpSLong; - ULONG TmpULong; +// SLONG TmpSLong; +// ULONG TmpULong; ULONG ArgVal2; float FltArgVal2; - char Buffer[30]; - char FormatString[5]; - UBYTE CheckTrailingZeros = 0; NXT_ASSERT(pCode != NULL); @@ -4423,7 +4824,10 @@ NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode) Arg1 = pCode[1]; Arg2 = pCode[2]; - if (opCode == OP_NEG || opCode == OP_NOT || opCode == OP_TST || opCode == OP_SQRT || opCode == OP_ABS) + if (opCode == OP_NEG || opCode == OP_NOT || opCode == OP_TST || + opCode == OP_CMNT || opCode == OP_SQRT || opCode == OP_ABS || opCode == OP_SIGN || + (opCode >= OP_ACOS && opCode <= OP_FRAC) || + (opCode >= OP_ACOSD && opCode <= OP_SINHD)) { return cCmdInterpPolyUnop2(*pCode, Arg1, 0, Arg2, 0); } @@ -4440,7 +4844,13 @@ NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode) { //!!! Should throw error if TypeCode1 is non-scalar // Accepting non-scalar destinations could have unpredictable results! - cCmdSetScalarValFromDataArg(Arg1, Arg2); + pArg1 = cCmdResolveDataArg(Arg1, 0, &TypeCode1); + if (TypeCode1 == TC_SLONG) + *(ULONG*)pArg1 = (SWORD)Arg2; + else if (TypeCode1 == TC_ULONG) + *(ULONG*)pArg1 = (UWORD)Arg2; + else if (TypeCode1 < TC_ULONG) + cCmdSetScalarValFromDataArg(Arg1, Arg2); } break; @@ -4482,6 +4892,19 @@ NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode) } break; + case OP_PRIORITY: + { + // set the priority of the specified clump + CLUMP_ID clump; + if (Arg2 != NOT_A_DS_ID) + clump = (CLUMP_ID)Arg1; + else + clump = VarsCmd.RunQ.Head; + CLUMP_REC* pClumpRec = &(VarsCmd.pAllClumps[clump]); + pClumpRec->Priority = (UBYTE)Arg2; + } + break; + case OP_SUBCALL: { NXT_ASSERT(cCmdIsClumpIDSane((CLUMP_ID)Arg1)); @@ -4590,6 +5013,7 @@ NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode) case OP_NUMTOSTRING: { + char Buffer[36]; //Assert that the destination is a string (array of bytes) NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY); NXT_ASSERT(cCmdDSType(INC_ID(Arg1)) == TC_UBYTE); @@ -4600,65 +5024,36 @@ NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode) 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.99)||(FltArgVal2 < -999999999999.99)){ // 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); + FltArgVal2 = cCmdGetFloatValFromDataArg(Arg2, 0); + Count = sprintf(Buffer, "%.4f", 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 - } - } + // remove trailing zeros + while (Buffer[Count-2] == 0x30) { + Buffer[Count-2] = 0x00; + Count--; + } + // if last character is now a period then delete it too + if (Buffer[Count-2] == '.') { + Buffer[Count-2] = 0x00; + Count--; } } 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) + ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, 0); + + // Calculate size of array + if (IS_SIGNED_TYPE(TypeCode2)) { - TmpSLong = (SLONG)ArgVal2; - //Add room for negative sign - if (TmpSLong < 0) { - Count++; - TmpULong= -TmpSLong; - } - else - TmpULong= ArgVal2; + Count = sprintf(Buffer, "%d", (SLONG)ArgVal2); } else - TmpULong= ArgVal2; - - while (Tmp <= TmpULong && digits < 10) { // maxint is ten digits, max - Tmp *= 10; - digits++; + { + Count = sprintf(Buffer, "%u", ArgVal2); } - Count += digits; - } - //add room for NULL terminator - Count++; + + //add room for NULL terminator + Count++; } //Allocate array @@ -4669,18 +5064,7 @@ NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode) 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); - } + memcpy(pArg1, Buffer, Count); } break; @@ -4739,7 +5123,7 @@ NXT_STATUS cCmdInterpUnop2(CODE_WORD * const pCode) } break; - default: + default: { //Fatal error: Unrecognized instruction NXT_BREAK; @@ -4772,6 +5156,8 @@ NXT_STATUS cCmdInterpPolyUnop2(CODE_WORD const Code, DATA_ARG Arg1, UWORD Offset TypeCode1 = cCmdDSType(Arg1); TypeCode2 = cCmdDSType(Arg2); + UBYTE opCode = OP_CODE(&Code); + //Simple case, scalar. Solve and return. if (!IS_AGGREGATE_TYPE(TypeCode2)) { @@ -4789,8 +5175,8 @@ NXT_STATUS cCmdInterpPolyUnop2(CODE_WORD const Code, DATA_ARG Arg1, UWORD Offset else { ArgVal2= cCmdGetScalarValFromDataArg(Arg2, Offset2); - if(OP_CODE(&Code) == OP_MOV) - ArgVal1= ArgVal2; + if (opCode == OP_MOV) + ArgVal1 = ArgVal2; else ArgVal1 = cCmdUnop2(Code, ArgVal2, TypeCode2); cCmdSetVal(pArg1, TypeCode1, ArgVal1); @@ -4799,12 +5185,11 @@ NXT_STATUS cCmdInterpPolyUnop2(CODE_WORD const Code, DATA_ARG Arg1, UWORD Offset } //At least one of the args is an aggregate type - - if(TypeCode1 == TC_ARRAY && TypeCode2 == TC_ARRAY) { + if(TypeCode1 == TC_ARRAY && TypeCode2 == TC_ARRAY && opCode == OP_MOV) { TYPE_CODE tc1, tc2; - tc1= cCmdDSType(INC_ID(Arg1)); - tc2= cCmdDSType(INC_ID(Arg2)); - if(tc1 <= TC_LAST_INT_SCALAR && tc1 == tc2) { + tc1 = cCmdDSType(INC_ID(Arg1)); + tc2 = cCmdDSType(INC_ID(Arg2)); + if((tc1 <= TC_LAST_INT_SCALAR || tc1 == TC_FLOAT) && tc1 == tc2) { void *pArg1, *pArg2; ULONG Count = cCmdArrayCount(Arg2, Offset2); Status = cCmdDSArrayAlloc(Arg1, Offset1, Count); @@ -4915,6 +5300,10 @@ ULONG cCmdUnop2(CODE_WORD const Code, ULONG Operand, TYPE_CODE TypeCode) return cCmdCompare(COMP_CODE((&Code)), Operand, 0, TypeCode, TypeCode); else if(opCode == OP_ABS) return abs(Operand); + else if (opCode == OP_CMNT) + return (~Operand); + else if (opCode == OP_SIGN) + return (((SLONG)Operand) < 0) ? -1 : ((Operand == 0) ? 0 : 1); else { //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller) @@ -4923,9 +5312,13 @@ ULONG cCmdUnop2(CODE_WORD const Code, ULONG Operand, TYPE_CODE TypeCode) } } +#define DEG2RAD 0.017453F +#define RAD2DEG 57.29578F + float cCmdUnop2Flt(CODE_WORD const Code, float Operand, TYPE_CODE TypeCode) { UBYTE opCode; + float ip, fp; opCode = OP_CODE((&Code)); if(opCode == OP_MOV) @@ -4940,22 +5333,66 @@ float cCmdUnop2Flt(CODE_WORD const Code, float Operand, TYPE_CODE TypeCode) return cCmdCompareFlt(COMP_CODE((&Code)), Operand, 0, TypeCode, TypeCode); else if(opCode == OP_ABS) return fabsf(Operand); + else if (opCode == OP_SIGN) + return (Operand < 0) ? -1 : ((Operand == 0) ? 0 : 1); else if(opCode == OP_SQRT) - return sqrt(Operand); -#if 0 + return sqrtf(Operand); else if(opCode == OP_SIN) - return sin(Operand); + return sinf(Operand); else if(opCode == OP_COS) - return cos(Operand); + return cosf(Operand); else if(opCode == OP_TAN) - return tan(Operand); + return tanf(Operand); else if(opCode == OP_ASIN) - return asin(Operand); + return asinf(Operand); else if(opCode == OP_ACOS) - return acos(Operand); + return acosf(Operand); else if(opCode == OP_ATAN) - return atan(Operand); -#endif + return atanf(Operand); + else if(opCode == OP_CEIL) + return ceilf(Operand); + else if(opCode == OP_EXP) + return expf(Operand); + else if(opCode == OP_FLOOR) + return floorf(Operand); + else if(opCode == OP_LOG) + return logf(Operand); + else if(opCode == OP_LOG10) + return log10f(Operand); + else if (opCode == OP_TRUNC) + { + modff(Operand, &ip); + return ip; + } + else if (opCode == OP_FRAC) + { + fp = modff(Operand, &ip); + return fp; + } + else if(opCode == OP_SIND) + return sinf((float)Operand*DEG2RAD); + else if(opCode == OP_COSD) + return cosf((float)Operand*DEG2RAD); + else if(opCode == OP_TAND) + return tanf((float)Operand*DEG2RAD); + else if(opCode == OP_ASIND) + return (float)(asinf(Operand)*RAD2DEG); + else if(opCode == OP_ACOSD) + return (float)(acosf(Operand)*RAD2DEG); + else if(opCode == OP_ATAND) + return (float)(atanf(Operand)*RAD2DEG); + else if(opCode == OP_TANH) + return tanhf(Operand); + else if(opCode == OP_COSH) + return coshf(Operand); + else if(opCode == OP_SINH) + return sinhf(Operand); + else if(opCode == OP_TANHD) + return tanhf((float)Operand*DEG2RAD); + else if(opCode == OP_COSHD) + return coshf((float)Operand*DEG2RAD); + else if(opCode == OP_SINHD) + return sinhf((float)Operand*DEG2RAD); else { //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller) @@ -4999,7 +5436,7 @@ NXT_STATUS cCmdIOGetSet(ULONG opCode, DATA_ARG Arg1, DATA_ARG Arg2, DATA_ARG Arg pArg2 = cCmdResolveIODataArg(Arg2, 0, &TypeCode2); TypeCode1= cCmdDSType(Arg1); pArg1= cCmdDSScalarPtr(Arg1, 0); - if(TypeCode1 <= TC_SBYTE && TypeCode1 <= TC_SBYTE) // seems really common + if(TypeCode1 <= TC_SBYTE && TypeCode2 <= TC_SBYTE) // seems really common *(UBYTE*)pArg1= *(UBYTE*)pArg2; else cCmdSetVal(pArg1, TypeCode1, cCmdGetVal(pArg2, TypeCode2)); @@ -5037,12 +5474,12 @@ NXT_STATUS cCmdInterpScalarBinop(CODE_WORD * const pCode) 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); + 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; - } + scalarCmp++; + Status = NO_ERR; + } else if (TypeCode2 == TC_ARRAY) // two strings { // memcmp(); here or in compareagg, could use memcmp to speed up string compares ??? @@ -5056,31 +5493,31 @@ NXT_STATUS cCmdInterpScalarBinop(CODE_WORD * const pCode) } } else if(opCode == OP_BRCMP) { // t2 and t3 guaranteed scalar - TYPE_CODE TypeCode2, TypeCode3; - ULONG ArgVal2, ArgVal3; + 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); + 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; - } + 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]; + Arg1 = pCode[1]; + Arg2 = pCode[2]; + Arg3 = pCode[3]; Status= cCmdIOGetSet(opCode, Arg1, Arg2, Arg3); gPCDelta= 4; - } + } else { scalarOther ++; Status= cCmdInterpBinop(pCode); @@ -5098,116 +5535,181 @@ NXT_STATUS cCmdInterpBinop(CODE_WORD * const pCode) UBYTE CmpBool; DV_INDEX DVIndex1, DVIndex2; UWORD i; + void *pArg1 = NULL, *pArg2 = NULL; + UWORD Count; polyBinopDispatch ++; gPCDelta= 4; NXT_ASSERT(pCode != NULL); - opCode = OP_CODE(pCode); - Arg1 = pCode[1]; - Arg2 = pCode[2]; - Arg3 = pCode[3]; + 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 + if ((opCode <= OP_XOR) || + (opCode >= OP_LSL && opCode <= OP_ROTR) || + (opCode == OP_ATAN2) || (opCode == OP_POW) || (opCode == OP_ATAN2D)) // && ! 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: + 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 ArgVal1, ArgVal2, ArgVal3; + if(TypeCode2 <= TC_LAST_INT_SCALAR && TypeCode3 <= TC_LAST_INT_SCALAR) { + ULONG ArgVal2, ArgVal3; ArgVal2= cCmdGetScalarValFromDataArg(Arg2, 0); ArgVal3= cCmdGetScalarValFromDataArg(Arg3, 0); - ArgVal1= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3); - cCmdSetScalarValFromDataArg(Arg1, ArgVal1); - PolyScalarCmp++; + CmpBool= cCmdCompare(COMP_CODE(pCode), ArgVal2, ArgVal3, TypeCode2, TypeCode3); } - else if (IS_AGGREGATE_TYPE(TypeCode2) && IS_AGGREGATE_TYPE(TypeCode3) && !IS_AGGREGATE_TYPE(cCmdDSType(Arg1))) - { - //Compare Aggregates + else //Compare Aggregates Status = cCmdCompareAggregates(COMP_CODE(pCode), &CmpBool, Arg2, 0, Arg3, 0); - cCmdSetScalarValFromDataArg(Arg1, CmpBool); - recursiveCmp++; + + if (CmpBool) + gPCDelta = (SWORD)Arg1; } - else + break; + + case OP_INDEX: { - //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); + 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)); } - 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 - + break; + + case OP_ARRINIT: + { + //Arg1 - Dst, Arg2 - element type/default val, Arg3 - length + NXT_ASSERT(cCmdDSType(Arg1) == TC_ARRAY); - + + // determine the type of the array destination arg + TYPE_CODE TypeCode = cCmdDSType(INC_ID(Arg1)); + + // How many elements do we want? ArgVal3 = (Arg3 != NOT_A_DS_ID) ? cCmdGetScalarValFromDataArg(Arg3, 0) : 0; - - Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)ArgVal3); + + Status = cCmdDSArrayAlloc(Arg1, 0, (UWORD)ArgVal3); if (!IS_ERR(Status)) { - DVIndex1 = cCmdGetDVIndex(Arg1, 0); - if(cCmdDSType(Arg2) <= TC_LAST_INT_SCALAR) + DVIndex1 = cCmdGetDVIndex(Arg1, 0); + if(cCmdDSType(Arg2) <= TC_LAST_INT_SCALAR && TypeCode <= 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 + { + //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); + Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, i), Arg2, 0); + } } - } - break; + break; + + case OP_FMTNUM: + { + //Check that the destination is a string (array of bytes) + if (cCmdDSType(Arg1) != TC_ARRAY || cCmdDSType(INC_ID(Arg1)) != TC_UBYTE) { + Status = ERR_INSTR; + return (Status); + } + + //Check that the format is a string (array of bytes) + if (cCmdDSType(Arg2) != TC_ARRAY || cCmdDSType(INC_ID(Arg2)) != TC_UBYTE) { + Status = ERR_INSTR; + return (Status); + } + + pArg2 = cCmdResolveDataArg(Arg2, 0, NULL); + TYPE_CODE TypeCode3 = cCmdDSType(Arg3); - default: - { - //Fatal error: Unrecognized instruction - NXT_BREAK; - Status = ERR_INSTR; + //Make sure we're trying to convert a scalar/float to a string + if (TypeCode3 == TC_VOID || (TypeCode3 > TC_LAST_INT_SCALAR && TypeCode3 != TC_FLOAT)) { + Status = ERR_INSTR; + return (Status); + } + + char fmtBuf[256]; // arbitrary limit!!! + // handle floats separately from scalar types + if (TypeCode3 == TC_FLOAT) { + float FltArgVal3 = cCmdGetFloatValFromDataArg(Arg3, 0); + Count = sprintf(fmtBuf, pArg2, FltArgVal3); + } + else + { + ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, 0); + // Calculate size of array + if (IS_SIGNED_TYPE(TypeCode3)) + { + Count = sprintf(fmtBuf, pArg2, (SLONG)ArgVal3); + } + else + { + Count = sprintf(fmtBuf, pArg2, ArgVal3); + } + } + + //add room for NULL terminator + Count++; + + //Allocate array + Status = cCmdDSArrayAlloc(Arg1, 0, Count); + if (IS_ERR(Status)) + return Status; + + pArg1 = cCmdResolveDataArg(Arg1, 0, NULL); + + //Populate array + memcpy(pArg1, fmtBuf, Count); + } + break; + + default: + { + //Fatal error: Unrecognized instruction + NXT_BREAK; + Status = ERR_INSTR; + } + break; } - break; - } } return (Status); } @@ -5252,10 +5754,10 @@ NXT_STATUS cCmdInterpPolyBinop(CODE_WORD const Code, DATA_ARG Arg1, UWORD Offset } else { - ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, Offset2); - ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, Offset3); - ArgVal1 = cCmdBinop(Code, ArgVal2, ArgVal3, TypeCode2, TypeCode3); - cCmdSetVal(pArg1, TypeCode1, ArgVal1); + ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, Offset2); + ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, Offset3); + ArgVal1 = cCmdBinop(Code, ArgVal2, ArgVal3, TypeCode2, TypeCode3); + cCmdSetVal(pArg1, TypeCode1, ArgVal1); } return Status; } @@ -5434,7 +5936,7 @@ ULONG cCmdBinop(CODE_WORD const Code, ULONG LeftOp, ULONG RightOp, TYPE_CODE Lef case OP_XOR: { - return ((LeftOp | RightOp) & (~(LeftOp & RightOp))); + return ((LeftOp | RightOp) & (~(LeftOp & RightOp))); // LeftOp ^ RightOp } case OP_CMP: @@ -5442,6 +5944,70 @@ ULONG cCmdBinop(CODE_WORD const Code, ULONG LeftOp, ULONG RightOp, TYPE_CODE Lef return cCmdCompare(COMP_CODE((&Code)), LeftOp, RightOp, LeftType, RightType); } + case OP_LSL: + { + if (((SLONG)RightOp) <= 0) + return LeftOp; // negative shifts == shifting by zero + else + return LeftOp << RightOp; + } + + case OP_LSR: + { + if (((SLONG)RightOp) <= 0) + return LeftOp; // negative shifts == shifting by zero + else + return LeftOp >> RightOp; + } + + case OP_ASL: + { + if (((SLONG)RightOp) <= 0) + return LeftOp; // negative shifts == shifting by zero + else if (!IS_SIGNED_TYPE(LeftType)) + return LeftOp << RightOp; + else + return LeftOp * (1 << RightOp); + } + + case OP_ASR: + { + if (((SLONG)RightOp) <= 0) + return LeftOp; // negative shifts == shifting by zero + else if (!IS_SIGNED_TYPE(LeftType)) + return LeftOp >> RightOp; + else + return ((SLONG)LeftOp) / (1 << RightOp); + } + + case OP_ROTL: + { + if (((SLONG)RightOp) <= 0) + return LeftOp; // negative rotates == rotating by zero + else { + if (LeftType == TC_ULONG || LeftType == TC_SLONG) + return (LeftOp << RightOp) | (LeftOp >> (32 - RightOp)); + else if (LeftType == TC_UWORD || LeftType == TC_SWORD) + return (((UWORD)LeftOp) << RightOp) | (((UWORD)LeftOp) >> (16 - RightOp)); + else if (LeftType == TC_UBYTE || LeftType == TC_SBYTE) + return (((UBYTE)LeftOp) << RightOp) | (((UBYTE)LeftOp) >> (8 - RightOp)); + } + } + + case OP_ROTR: + { + if (((SLONG)RightOp) <= 0) + return LeftOp; // negative rotates == rotating by zero + else { + if (LeftType == TC_ULONG || LeftType == TC_SLONG) + return (LeftOp >> RightOp) | (LeftOp << (32 - RightOp)); + else if (LeftType == TC_UWORD || LeftType == TC_SWORD) + return (((UWORD)LeftOp) >> RightOp) | (((UWORD)LeftOp) << (16 - RightOp)); + else if (LeftType == TC_UBYTE || LeftType == TC_SBYTE) + return (((UBYTE)LeftOp) >> RightOp) | (((UBYTE)LeftOp) << (8 - RightOp)); + } + } + default: { //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller) @@ -5491,7 +6057,7 @@ float cCmdBinopFlt(CODE_WORD const Code, float LeftOp, float RightOp, TYPE_CODE if (RightOp == 0) return (LeftOp); - return (SLONG)LeftOp % (SLONG)RightOp; + return fmodf(LeftOp, RightOp); } case OP_AND: @@ -5514,6 +6080,26 @@ float cCmdBinopFlt(CODE_WORD const Code, float LeftOp, float RightOp, TYPE_CODE return cCmdCompareFlt(COMP_CODE((&Code)), LeftOp, RightOp, LeftType, RightType); } + case OP_ATAN2: + { + return atan2f(LeftOp, RightOp); + } + + case OP_POW: + { + float intpart, fracpart; + fracpart = modff(LeftOp, &intpart); + if (LeftOp < 0 && fracpart != 0) + return 0; // make the result zero if you try to raise a negative number to a fractional exponent + else + return powf(LeftOp, RightOp); + } + + case OP_ATAN2D: + { + return (float)(atan2f(LeftOp, RightOp)*RAD2DEG); + } + default: { //Unrecognized instruction, NXT_BREAK for easy debugging (ERR_INSTR handled in caller) @@ -5560,13 +6146,15 @@ NXT_STATUS cCmdInterpShortSubCall(CODE_WORD * const pCode) return Status; } -ULONG moveSameInt= 0, moveDiffInt= 0, moveFloat= 0, moveArrInt= 0, moveOther= 0; +ULONG moveSameInt= 0, moveDiffInt= 0, moveFloat= 0, moveIntFloat= 0, moveFloatInt= 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; + UBYTE ElemSize1 = cCmdSizeOf((TOC1Ptr+1)->TypeCode), + ElemSize2 = cCmdSizeOf((TOC2Ptr+1)->TypeCode); void *pArg1, *pArg2; if(tc1 <= TC_LAST_INT_SCALAR && tc2 <= TC_LAST_INT_SCALAR) @@ -5585,7 +6173,7 @@ NXT_STATUS cCmdMove(DATA_ARG Arg1, DATA_ARG Arg2) *(UWORD*)pArg1= *(UWORD*)pArg2; Status= NO_ERR; } - else + else { moveDiffInt++; ULONG val= cCmdGetScalarValFromDataArg(Arg2, 0); @@ -5593,17 +6181,53 @@ NXT_STATUS cCmdMove(DATA_ARG Arg1, DATA_ARG Arg2) 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 + else if(tc1 == TC_FLOAT && tc2 == TC_FLOAT) { 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)) + else if(tc1 == TC_FLOAT && tc2 <= TC_LAST_INT_SCALAR) { // int to float + moveIntFloat++; + pArg1= VarsCmd.pDataspace + TOC1Ptr->DSOffset; + pArg2= VarsCmd.pDataspace + TOC2Ptr->DSOffset; + if (tc2 == TC_SLONG) + *(float*)pArg1 = *(SLONG*)pArg2; + else if (tc2 == TC_ULONG) + *(float*)pArg1 = *(ULONG*)pArg2; + else if (tc2 == TC_SBYTE) + *(float*)pArg1 = *(SBYTE*)pArg2; + else if (tc2 == TC_UBYTE) + *(float*)pArg1 = *(UBYTE*)pArg2; + else if (tc2 == TC_UWORD) + *(float*)pArg1 = *(UWORD*)pArg2; + else + *(float*)pArg1= *(SWORD*)pArg2; + Status= NO_ERR; + } + else if(tc2 == TC_FLOAT && tc1 <= TC_LAST_INT_SCALAR) { // float to int + moveFloatInt++; + pArg1= VarsCmd.pDataspace + TOC1Ptr->DSOffset; + pArg2= VarsCmd.pDataspace + TOC2Ptr->DSOffset; + if (tc1 == TC_SLONG) + *(SLONG*)pArg1 = *(float*)pArg2; + else if (tc1 == TC_ULONG) + *(ULONG*)pArg1 = *(float*)pArg2; + else if (tc1 == TC_SBYTE) + *(SBYTE*)pArg1 = *(float*)pArg2; + else if (tc1 == TC_UBYTE) + *(UBYTE*)pArg1 = *(float*)pArg2; + else if (tc1 == TC_UWORD) + *(UWORD*)pArg1 = *(float*)pArg2; + else + *(SWORD*)pArg1 = *(float*)pArg2; + Status= NO_ERR; + } + //!!! Optimized move for arrays of ints and floats. + else if ((tc1 == TC_ARRAY) && (tc2 == TC_ARRAY) && + (((TOC1Ptr+1)->TypeCode <= TC_LAST_INT_SCALAR && ElemSize1 == ElemSize2) || + ((TOC1Ptr+1)->TypeCode == TC_FLOAT && (TOC2Ptr+1)->TypeCode == TC_FLOAT))) { ULONG Count; moveArrInt++; @@ -5669,6 +6293,23 @@ NXT_STATUS cCmdInterpShortRelease(CODE_WORD * const pCode) } +ULONG cCmdGetPortFromValue(ULONG val, ULONG i) +{ + ULONG result = NO_OF_OUTPUTS; // invalid NO-OP output + if (val < NO_OF_OUTPUTS) + result = val; + else + { + if (val <= RC_OUT_ABC) + { + result = i; + if ((val == RC_OUT_BC) || (val == RC_OUT_AC && i)) + result++; + } + } + return result; +} + //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 @@ -5680,7 +6321,7 @@ NXT_STATUS cCmdExecuteSetOut(CODE_WORD * const pCode) *pPort = NULL; DS_ELEMENT_ID PortArg; UWORD PortCount, InstrSize; - ULONG Port, FieldTableIndex, i, j; + ULONG Port, FieldTableIndex, i, j, val = 0; DV_INDEX DVIndex; //Arg1 = InstrSize @@ -5702,7 +6343,17 @@ NXT_STATUS cCmdExecuteSetOut(CODE_WORD * const pCode) PortCount = cCmdArrayCount(PortArg, 0); } else - PortCount = 1; + { + // arg may refer to multiple ports + // (0, 1, 2 are single ports; + val = cCmdGetScalarValFromDataArg(PortArg, 0); + if (val < NO_OF_OUTPUTS) + PortCount = 1; + else if (val < RC_OUT_ABC) + PortCount = 2; + else + PortCount = 3; + } //For each port, process all the tuples for (i = 0; i < PortCount; i++) @@ -5714,7 +6365,7 @@ NXT_STATUS cCmdExecuteSetOut(CODE_WORD * const pCode) } else { - Port = cCmdGetScalarValFromDataArg(PortArg, 0); + Port = cCmdGetPortFromValue(val, i); } //If user specified a valid port, process the tuples. Else, this port is a no-op @@ -5743,6 +6394,174 @@ NXT_STATUS cCmdExecuteSetOut(CODE_WORD * const pCode) } +void shell_sort_u1(UBYTE* A, UWORD size) +{ + UWORD i, j, increment; + UBYTE temp; + increment = size / 2; + + while (increment > 0) { + for (i = increment; i < size; i++) { + j = i; + temp = A[i]; + while ((j >= increment) && (A[j-increment] > temp)) { + A[j] = A[j - increment]; + j = j - increment; + } + A[j] = temp; + } + + if (increment == 2) + increment = 1; + else + increment = (UWORD)((float)increment / (float)2.2); + } +} + +void shell_sort_s1(SBYTE* A, UWORD size) +{ + UWORD i, j, increment; + SBYTE temp; + increment = size / 2; + + while (increment > 0) { + for (i = increment; i < size; i++) { + j = i; + temp = A[i]; + while ((j >= increment) && (A[j-increment] > temp)) { + A[j] = A[j - increment]; + j = j - increment; + } + A[j] = temp; + } + + if (increment == 2) + increment = 1; + else + increment = (UWORD)((float)increment / (float)2.2); + } +} + +void shell_sort_u2(UWORD* A, UWORD size) +{ + UWORD i, j, increment; + UWORD temp; + increment = size / 2; + + while (increment > 0) { + for (i = increment; i < size; i++) { + j = i; + temp = A[i]; + while ((j >= increment) && (A[j-increment] > temp)) { + A[j] = A[j - increment]; + j = j - increment; + } + A[j] = temp; + } + + if (increment == 2) + increment = 1; + else + increment = (UWORD)((float)increment / (float)2.2); + } +} + +void shell_sort_s2(SWORD* A, UWORD size) +{ + UWORD i, j, increment; + SWORD temp; + increment = size / 2; + + while (increment > 0) { + for (i = increment; i < size; i++) { + j = i; + temp = A[i]; + while ((j >= increment) && (A[j-increment] > temp)) { + A[j] = A[j - increment]; + j = j - increment; + } + A[j] = temp; + } + + if (increment == 2) + increment = 1; + else + increment = (UWORD)((float)increment / (float)2.2); + } +} + +void shell_sort_u4(ULONG* A, UWORD size) +{ + UWORD i, j, increment; + ULONG temp; + increment = size / 2; + + while (increment > 0) { + for (i = increment; i < size; i++) { + j = i; + temp = A[i]; + while ((j >= increment) && (A[j-increment] > temp)) { + A[j] = A[j - increment]; + j = j - increment; + } + A[j] = temp; + } + + if (increment == 2) + increment = 1; + else + increment = (UWORD)((float)increment / (float)2.2); + } +} + +void shell_sort_s4(SLONG* A, UWORD size) +{ + UWORD i, j, increment; + SLONG temp; + increment = size / 2; + + while (increment > 0) { + for (i = increment; i < size; i++) { + j = i; + temp = A[i]; + while ((j >= increment) && (A[j-increment] > temp)) { + A[j] = A[j - increment]; + j = j - increment; + } + A[j] = temp; + } + + if (increment == 2) + increment = 1; + else + increment = (UWORD)((float)increment / (float)2.2); + } +} + +void shell_sort_flt(float* A, UWORD size) +{ + UWORD i, j, increment; + float temp; + increment = size / 2; + + while (increment > 0) { + for (i = increment; i < size; i++) { + j = i; + temp = A[i]; + while ((j >= increment) && (A[j-increment] > temp)) { + A[j] = A[j - increment]; + j = j - increment; + } + A[j] = temp; + } + + if (increment == 2) + increment = 1; + else + increment = (UWORD)((float)increment / (float)2.2); + } +} + NXT_STATUS cCmdInterpOther(CODE_WORD * const pCode) { NXT_STATUS Status = NO_ERR; @@ -5783,6 +6602,7 @@ NXT_STATUS cCmdInterpOther(CODE_WORD * const pCode) //Arg2 - Src //Arg3 - Index //Arg4 - New val / array of vals + UWORD SrcDims, NewValDims; Arg1 = pCode[1]; Arg2 = pCode[2]; @@ -5796,7 +6616,8 @@ NXT_STATUS cCmdInterpOther(CODE_WORD * const pCode) //!!! Could avoid full data copy if we knew which portion to overwrite if (Arg1 != Arg2) { - Status= cCmdMove(Arg1, Arg2); +// Status= cCmdMove(Arg1, Arg2); + Status = cCmdInterpPolyUnop2(OP_MOV, Arg1, 0, Arg2, 0); if (IS_ERR(Status)) return Status; } @@ -5820,13 +6641,17 @@ NXT_STATUS cCmdInterpOther(CODE_WORD * const pCode) if (ArgVal3 >= ArrayCount1) return (NO_ERR); - if (cCmdDSType(Arg4) != TC_ARRAY) + SrcDims = cCmdArrayDimensions(Arg2); + NewValDims = cCmdArrayDimensions(Arg4); + // if the new value argument has an array dimension that is 1 less than + // the array dimension of the source array then use MOV to copy data + if (NewValDims == (SrcDims-1)) { Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg1), ARRAY_ELEM_OFFSET(DVIndex1, ArgVal3), Arg4, 0); if (IS_ERR(Status)) return Status; } - else + else if (NewValDims == SrcDims) { DVIndex4 = cCmdGetDVIndex(Arg4, 0); @@ -5843,6 +6668,12 @@ NXT_STATUS cCmdInterpOther(CODE_WORD * const pCode) return Status; } } + else + { + // any other situation is unsupported + NXT_BREAK; + return 0; + } } break; @@ -6205,6 +7036,7 @@ NXT_STATUS cCmdInterpOther(CODE_WORD * const pCode) case OP_STRINGTONUM: { + ULONG ArgVal1; float ArgValF; SLONG decimals= 0; UBYTE cont= TRUE; @@ -6214,10 +7046,6 @@ NXT_STATUS cCmdInterpOther(CODE_WORD * const pCode) // 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]; @@ -6243,113 +7071,437 @@ NXT_STATUS cCmdInterpOther(CODE_WORD * const pCode) } //Read number from string - if (sscanf(((PSZ)pArg3 + ArgVal4), "%f", &ArgValF) == 1) + // scan either to integer or float, depending on TypeCode1 + int scanResult; + if (TypeCode1 == TC_FLOAT) + scanResult = sscanf(((PSZ)pArg3 + ArgVal4), "%f", &ArgValF); + else + scanResult = sscanf(((PSZ)pArg3 + ArgVal4), "%d", &ArgVal1); + // check the result + if (scanResult == 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) { + if (Arg2 != NOT_A_DS_ID) + { + i = (UWORD)ArgVal4; + //Scan until we see the number, consumes negative sign too + while ((((UBYTE *)pArg3)[i] < '0') || (((UBYTE *)pArg3)[i] > '9')) i++; - decimals++; + + if (TypeCode1 == TC_FLOAT) + { + //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; } - else - cont= FALSE; + } + else { + //Scan until we get past the number + while ((((UBYTE *)pArg3)[i] >= '0') && (((UBYTE *)pArg3)[i] <= '9')) + i++; + } + ArgVal2 = i; } - ArgVal2 = i; } else { //Number wasn't found in string, use defaults ArgValF = ArgVal5; + ArgVal1 = ArgVal5; ArgVal2 = 0; } //Set outputs - cCmdSetValFlt(pArg1, TypeCode1, ArgValF); - cCmdSetScalarValFromDataArg(Arg2, ArgVal2); + if (TypeCode1 == TC_FLOAT) + cCmdSetValFlt(pArg1, TypeCode1, ArgValF); + else + cCmdSetVal(pArg1, TypeCode1, ArgVal1); + if (Arg2 != NOT_A_DS_ID) + cCmdSetScalarValFromDataArg(Arg2, ArgVal2); } break; - default: + case OP_ARROP: { - //Fatal error: Unrecognized instruction - NXT_BREAK; - Status = ERR_INSTR; - } - break; - } - - return (Status); -} + //Arg1 - Command (immediate constant) + //Arg2 - Dst (scalar|array) + //Arg3 - Src (scalar array) + //Arg4 - Index + //Arg5 - Length + Arg1 = pCode[1]; + Arg2 = pCode[2]; + Arg3 = pCode[3]; + Arg4 = pCode[4]; + Arg5 = pCode[5]; -// -//Support functions for lowspeed (I2C devices, i.e. ultrasonic sensor) communications -// + // array operation + if (Arg1 == OPARR_SORT) { + // source must be an array of non-aggregate type + NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY); + TypeCode2 = cCmdDSType(INC_ID(Arg2)); + NXT_ASSERT(!IS_AGGREGATE_TYPE(TypeCode2)); + } + else { + // destination must be a non-aggregate type + NXT_ASSERT(!IS_AGGREGATE_TYPE(cCmdDSType(Arg2))); + } + // source must be an array of non-aggregate type + NXT_ASSERT(cCmdDSType(Arg3) == TC_ARRAY); + TypeCode3 = cCmdDSType(INC_ID(Arg3)); + NXT_ASSERT(!IS_AGGREGATE_TYPE(TypeCode3)); -//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) -}; + ArrayCount3 = cCmdArrayCount(Arg3, 0); + if (Arg4 != NOT_A_DS_ID) + ArgVal4 = cCmdGetScalarValFromDataArg(Arg4, 0); + else //Index input unwired + ArgVal4 = 0; -//cCmdLSCheckStatus -//Check lowspeed port status, optionally returning bytes available in the buffer for reading -NXT_STATUS cCmdLSCheckStatus(UBYTE Port) -{ - if (Port >= NO_OF_LOWSPEED_COM_CHANNEL) - { - return (ERR_COMM_CHAN_INVALID); - } + if (Arg5 != NOT_A_DS_ID) + ArgVal5 = cCmdGetScalarValFromDataArg(Arg5, 0); + else //Length input unwired, set to "rest" + ArgVal5 = 0xFFFF; - //If port is not configured properly ahead of time, report that error - //!!! This seems like the right policy, but may restrict otherwise valid read operations... - if (!(pMapInput->Inputs[Port].SensorType == LOWSPEED_9V || pMapInput->Inputs[Port].SensorType == LOWSPEED) - || !(pMapInput->Inputs[Port].InvalidData == FALSE)) - { - return (ERR_COMM_CHAN_NOT_READY); - } + //Bounds check + if (ArgVal4 > ArrayCount3) + { + if (Arg1 == OPARR_SORT) { + //Illegal range - return empty subset + Status = cCmdDSArrayAlloc(Arg2, 0, 0); + return Status; + } + else { + //Illegal range - return zero + pArg2 = cCmdResolveDataArg(Arg2, 0, &TypeCode2); + cCmdSetVal(pArg2, TypeCode2, 0); + return NO_ERR; + } + } - return (MapLStoVMStat[pMapLowSpeed->ChannelState[Port]]); -} + //Set MinCount to "rest" + MinCount = (UWORD)(ArrayCount3 - ArgVal4); -//cCmdLSCalcBytesReady -//Calculate true number of bytes available in the inbound LS buffer -UBYTE cCmdLSCalcBytesReady(UBYTE Port) -{ - SLONG Tmp; + // Copy "Length" if it is less than "rest" + if (ArgVal5 < (ULONG)MinCount) + MinCount = (UWORD)ArgVal5; - //Expect callers to validate Port, but short circuit here to be safe. - if (Port >= NO_OF_LOWSPEED_COM_CHANNEL) - return 0; + DV_INDEX DVIndex3 = cCmdGetDVIndex(Arg3, 0); - //Normally, bytes available is a simple difference. - Tmp = pMapLowSpeed->InBuf[Port].InPtr - pMapLowSpeed->InBuf[Port].OutPtr; + SLONG sval, svaltmp; + ULONG uval, uvaltmp; + float numElements = (float)MinCount; + //sum elements from src subset to dst + if ((Arg1 == OPARR_SUM) || (Arg1 == OPARR_MEAN) || + (Arg1 == OPARR_SUMSQR) || (Arg1 == OPARR_STD)) + { + pArg2 = cCmdResolveDataArg(Arg2, 0, &TypeCode2); + if (IS_SIGNED_TYPE(TypeCode3) && (Arg1 != OPARR_SUMSQR)) + { + sval = 0; + for (i = 0; i < MinCount; i++) + { + pArg3 = cCmdResolveDataArg(INC_ID(Arg3), ARRAY_ELEM_OFFSET(DVIndex3, ArgVal4 + i), NULL); + svaltmp = (SLONG)cCmdGetVal(pArg3, TypeCode3); + sval += svaltmp; + } + if (Arg1 == OPARR_MEAN) + cCmdSetVal(pArg2, TypeCode2, (SLONG)(float)sval/numElements); + else if (Arg1 != OPARR_STD) + cCmdSetVal(pArg2, TypeCode2, sval); + } + else + { + uval = 0; + for (i = 0; i < MinCount; i++) + { + pArg3 = cCmdResolveDataArg(INC_ID(Arg3), ARRAY_ELEM_OFFSET(DVIndex3, ArgVal4 + i), NULL); + if (IS_SIGNED_TYPE(TypeCode3)) + { + // this can only be the SUMSQR operation (given the IF statement above) + svaltmp = cCmdGetVal(pArg3, TypeCode3); + uvaltmp = (ULONG)abs(svaltmp) * (ULONG)abs(svaltmp); + uval += uvaltmp; + } + else { + uvaltmp = cCmdGetVal(pArg3, TypeCode3); + if (Arg1 == OPARR_SUMSQR) + uvaltmp *= uvaltmp; + uval += uvaltmp; + } + } + if (Arg1 == OPARR_MEAN) + cCmdSetVal(pArg2, TypeCode2, (ULONG)(float)uval/numElements); + else if (Arg1 != OPARR_STD) + cCmdSetVal(pArg2, TypeCode2, uval); + } + // calculate standard deviation + if (Arg1 == OPARR_STD) { + float avg, delta, sumSqr; + if (IS_SIGNED_TYPE(TypeCode3)) + avg = (float)sval/numElements; + else + avg = (float)uval/numElements; + sumSqr = 0; + for (i = 0; i < MinCount; i++) + { + pArg3 = cCmdResolveDataArg(INC_ID(Arg3), ARRAY_ELEM_OFFSET(DVIndex3, ArgVal4 + i), NULL); + if (IS_SIGNED_TYPE(TypeCode3)) + delta = (float)(SLONG)cCmdGetVal(pArg3, TypeCode3) - avg; + else + delta = (float)cCmdGetVal(pArg3, TypeCode3) - avg; + sumSqr += (delta*delta); + } + delta = sqrtf(sumSqr / (numElements - (float)1.0)); + if (IS_SIGNED_TYPE(TypeCode3)) + cCmdSetVal(pArg2, TypeCode2, (SLONG)delta); + else + cCmdSetVal(pArg2, TypeCode2, (ULONG)delta); + } + } + else if ((Arg1 == OPARR_MIN) || (Arg1 == OPARR_MAX)) + { + pArg2 = cCmdResolveDataArg(Arg2, 0, &TypeCode2); + if (IS_SIGNED_TYPE(TypeCode3)) + { + if (Arg1 == OPARR_MIN) + sval = LONG_MAX; + else + sval = LONG_MIN; + for (i = 0; i < MinCount; i++) + { + pArg3 = cCmdResolveDataArg(INC_ID(Arg3), ARRAY_ELEM_OFFSET(DVIndex3, ArgVal4 + i), NULL); + svaltmp = (SLONG)cCmdGetVal(pArg3, TypeCode3); + if (((Arg1 == OPARR_MIN) && (svaltmp < sval)) || + ((Arg1 == OPARR_MAX) && (svaltmp > sval))) + sval = svaltmp; + } + cCmdSetVal(pArg2, TypeCode2, sval); + } + else + { + if (Arg1 == OPARR_MIN) + uval = ULONG_MAX; + else + uval = 0; + for (i = 0; i < MinCount; i++) + { + pArg3 = cCmdResolveDataArg(INC_ID(Arg3), ARRAY_ELEM_OFFSET(DVIndex3, ArgVal4 + i), NULL); + uvaltmp = cCmdGetVal(pArg3, TypeCode3); + if (((Arg1 == OPARR_MIN) && (uvaltmp < uval)) || + ((Arg1 == OPARR_MAX) && (uvaltmp > uval))) + uval = uvaltmp; + } + cCmdSetVal(pArg2, TypeCode2, uval); + } + } + else if (Arg1 == OPARR_SORT) + { + //Allocate Dst array + Status = cCmdDSArrayAlloc(Arg2, 0, MinCount); + if (IS_ERR(Status)) + return Status; - //If InPtr is actually behind OutPtr, circular buffer has wrapped. Account for wrappage... - if (Tmp < 0) - Tmp = (pMapLowSpeed->InBuf[Port].InPtr + (SIZE_OF_LSBUF - pMapLowSpeed->InBuf[Port].OutPtr)); + DVIndex2 = cCmdGetDVIndex(Arg2, 0); + + //Move src subset to dst + for (i = 0; i < MinCount; i++) + { + Status = cCmdInterpPolyUnop2(OP_MOV, INC_ID(Arg2), ARRAY_ELEM_OFFSET(DVIndex2, i), INC_ID(Arg3), ARRAY_ELEM_OFFSET(DVIndex3, ArgVal4 + i)); + if (IS_ERR(Status)) + return Status; + } + // now dst is ready to be sorted + pArg2 = cCmdResolveDataArg(Arg2, 0, NULL); + Size = cCmdSizeOf(TypeCode2); + if (TypeCode2 == TC_SBYTE) + shell_sort_s1(pArg2, MinCount); + else if (TypeCode2 == TC_SWORD) + shell_sort_s2(pArg2, MinCount); + else if (TypeCode2 == TC_SLONG) + shell_sort_s4(pArg2, MinCount); + else if (TypeCode2 == TC_UBYTE) + shell_sort_u1(pArg2, MinCount); + else if (TypeCode2 == TC_UWORD) + shell_sort_u2(pArg2, MinCount); + else if (TypeCode2 == TC_ULONG) + shell_sort_u4(pArg2, MinCount); + else if (TypeCode2 == TC_FLOAT) + shell_sort_flt(pArg2, MinCount); + } + else + { + //Fatal error: Unrecognized instruction + NXT_BREAK; + Status = ERR_INSTR; + } + } + break; + + case OP_MULDIV: + { + //Arg1 - Dst (scalar) + //Arg2 - SrcA (scalar) + //Arg3 - SrcB (scalar) + //Arg4 - SrcC (scalar) + + Arg1 = pCode[1]; + Arg2 = pCode[2]; + Arg3 = pCode[3]; + Arg4 = pCode[4]; + ArgVal2 = cCmdGetScalarValFromDataArg(Arg2, 0); + ArgVal3 = cCmdGetScalarValFromDataArg(Arg3, 0); + ArgVal4 = cCmdGetScalarValFromDataArg(Arg4, 0); + ArgVal3 = (ULONG)(((long long)ArgVal2*(long long)ArgVal3)/(long long)ArgVal4); + pArg1 = cCmdResolveDataArg(Arg1, 0, &TypeCode1); + cCmdSetVal(pArg1, TypeCode1, ArgVal3); + } + break; + +/* + case OP_PRINTF: + { + // Arg1 - Instruction Size in bytes + // Arg2 - Dst + // Arg3 - Fmtstr + // Arg4-N - Srcs (max args = 8) + void *srcPtrs[8] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL}; + void *pArg2 = NULL, *pArg3 = NULL; + + Arg2 = pCode[2]; + Arg3 = pCode[3]; + + //Make sure Dst arg is a string + NXT_ASSERT(cCmdDSType(Arg2) == TC_ARRAY); + NXT_ASSERT(cCmdDSType(INC_ID(Arg2)) == TC_UBYTE); + + //Make sure Fmtstr arg is a string + NXT_ASSERT(cCmdDSType(Arg3) == TC_ARRAY); + NXT_ASSERT(cCmdDSType(INC_ID(Arg3)) == TC_UBYTE); + + //Number of Srcs = total code words - 4 (account for opcode word, size, Dst, and Fmtstr) + //!!! Argument access like this is potentially unsafe. + //A function/macro which checks proper encoding would be better + SrcCount = (pCode[1] / 2) - 4; + if (SrcCount > 8) { + Status = ERR_INSTR; + return (Status); + } + + // get pointers to Dst and FmtSt + pArg2 = cCmdResolveDataArg(Arg2, 0, &TypeCode2); + pArg3 = cCmdResolveDataArg(Arg3, 0, &TypeCode3); + + // resolve src pointers for all our sources + for (i = 0; i < SrcCount; i++) + { + TmpDSID = pCode[4 + i]; + TYPE_CODE tc = cCmdDSType(TmpDSID); + if ((tc == TC_ARRAY && cCmdDSType(INC_ID(TmpDSID)) != TC_UBYTE) || + (tc == TC_VOID) || (tc > TC_LAST_INT_SCALAR && tc != TC_FLOAT)) + { + // invalid source (only scalars, floats, and strings are supported) + Status = ERR_INSTR; + return (Status); + } + srcPtrs[i] = cCmdResolveDataArg(TmpDSID, 0, &TypeCode1); + } + + //Calculate Dst array count + ArrayCount2 = sprintf(NULL, pArg3, srcPtrs[0], srcPtrs[1], srcPtrs[2], + srcPtrs[3], srcPtrs[4], srcPtrs[5], + srcPtrs[6], srcPtrs[7], srcPtrs[8]); + } + 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)); + else if ((Tmp == 0) && + (pInBuf->BytesToRx == SIZE_OF_LSBUF) && + (pMapLowSpeed->ChannelState[Port] == LOWSPEED_IDLE)) + Tmp = SIZE_OF_LSBUF; 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) +NXT_STATUS cCmdLSWrite(UBYTE Port, UBYTE BufLength, UBYTE *pBuf, UBYTE ResponseLength, UBYTE NoRestartOnRead) { if (Port >= NO_OF_LOWSPEED_COM_CHANNEL) { @@ -6361,21 +7513,29 @@ NXT_STATUS cCmdLSWrite(UBYTE Port, UBYTE BufLength, UBYTE *pBuf, UBYTE ResponseL 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 ((pMapInput->Inputs[Port].SensorType == LOWSPEED_9V || pMapInput->Inputs[Port].SensorType == LOWSPEED) - && (pMapInput->Inputs[Port].InvalidData == FALSE) - && (pMapLowSpeed->ChannelState[Port] == LOWSPEED_IDLE) || (pMapLowSpeed->ChannelState[Port] == LOWSPEED_ERROR)) + if ((pInput->SensorType == LOWSPEED_9V || pInput->SensorType == LOWSPEED) + && (pInput->InvalidData == FALSE) + && (LOWSPEED_IDLE == *pChState) || (LOWSPEED_ERROR == *pChState)) { - pMapLowSpeed->OutBuf[Port].InPtr = 0; - pMapLowSpeed->OutBuf[Port].OutPtr = 0; + pOutBuf->InPtr = 0; + pOutBuf->OutPtr = 0; - memcpy(pMapLowSpeed->OutBuf[Port].Buf, pBuf, BufLength); - pMapLowSpeed->OutBuf[Port].InPtr = (UBYTE)BufLength; + memcpy(pOutBuf->Buf, pBuf, BufLength); + pOutBuf->InPtr = (UBYTE)BufLength; pMapLowSpeed->InBuf[Port].BytesToRx = ResponseLength; - pMapLowSpeed->ChannelState[Port] = LOWSPEED_INIT; + *pChState = LOWSPEED_INIT; pMapLowSpeed->State |= (COM_CHANNEL_ONE_ACTIVE << Port); + if (NoRestartOnRead) + pMapLowSpeed->NoRestartOnRead |= (0x01 << Port); + else + pMapLowSpeed->NoRestartOnRead &= ~(0x01 << Port); return (NO_ERR); } @@ -6412,18 +7572,21 @@ NXT_STATUS cCmdLSRead(UBYTE Port, UBYTE BufLength, UBYTE * pBuf) 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 (pMapLowSpeed->InBuf[Port].OutPtr + BytesToRead >= SIZE_OF_LSBUF) + if (pInBuf->OutPtr + BytesToRead >= SIZE_OF_LSBUF) { - BytesToRead = SIZE_OF_LSBUF - pMapLowSpeed->InBuf[Port].OutPtr; - memcpy(pBuf, pMapLowSpeed->InBuf[Port].Buf + pMapLowSpeed->InBuf[Port].OutPtr, BytesToRead); - pMapLowSpeed->InBuf[Port].OutPtr = 0; + BytesToRead = SIZE_OF_LSBUF - pInBuf->OutPtr; + memcpy(pBuf, pInBuf->Buf + pInBuf->OutPtr, BytesToRead); + pInBuf->OutPtr = 0; pBuf += BytesToRead; BytesToRead = BufLength - BytesToRead; } - - memcpy(pBuf, pMapLowSpeed->InBuf[Port].Buf + pMapLowSpeed->InBuf[Port].OutPtr, BytesToRead); - pMapLowSpeed->InBuf[Port].OutPtr += BytesToRead; + if (BytesToRead > 0) { + memcpy(pBuf, pInBuf->Buf + pInBuf->OutPtr, BytesToRead); + pInBuf->OutPtr += BytesToRead; + } return (NO_ERR); } @@ -6433,13 +7596,7 @@ NXT_STATUS cCmdLSRead(UBYTE Port, UBYTE BufLength, UBYTE * pBuf) //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[]) +NXT_STATUS cCmdWrapFileOpenReadHelper(UBYTE Cmd, UBYTE * ArgV[]) { LOADER_STATUS LStatus; DV_INDEX DVIndex; @@ -6448,7 +7605,7 @@ NXT_STATUS cCmdWrapFileOpenRead(UBYTE * ArgV[]) DVIndex = *(DV_INDEX *)(ArgV[2]); ArgV[2] = cCmdDVPtr(DVIndex); - LStatus = pMapLoader->pFunc(OPENREAD, ArgV[2], NULL, (ULONG *)ArgV[3]); + LStatus = pMapLoader->pFunc(Cmd, ArgV[2], NULL, (ULONG *)ArgV[3]); //Add entry into FileHandleTable if (LOADER_ERR(LStatus) == SUCCESS) @@ -6465,12 +7622,7 @@ NXT_STATUS cCmdWrapFileOpenRead(UBYTE * ArgV[]) 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[]) +NXT_STATUS cCmdWrapFileOpenWriteHelper(UBYTE Cmd, UBYTE * ArgV[]) { LOADER_STATUS LStatus; DV_INDEX DVIndex; @@ -6479,7 +7631,7 @@ NXT_STATUS cCmdWrapFileOpenWrite(UBYTE * ArgV[]) DVIndex = *(DV_INDEX *)(ArgV[2]); ArgV[2] = cCmdDVPtr(DVIndex); - LStatus = pMapLoader->pFunc(OPENWRITEDATA, ArgV[2], NULL, (ULONG *)ArgV[3]); + LStatus = pMapLoader->pFunc(Cmd, ArgV[2], NULL, (ULONG *)ArgV[3]); //Add entry into FileHandleTable if (LOADER_ERR(LStatus) == SUCCESS) @@ -6496,6 +7648,27 @@ NXT_STATUS cCmdWrapFileOpenWrite(UBYTE * ArgV[]) return NO_ERR; } +// +//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[]) +{ + return cCmdWrapFileOpenReadHelper(OPENREAD, ArgV); +} + +//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[]) +{ + return cCmdWrapFileOpenWriteHelper(OPENWRITEDATA, ArgV); +} + //cCmdWrapFileOpenAppend //ArgV[0]: (Function return) Loader status, U16 return //ArgV[1]: File Handle, U8 return @@ -6503,28 +7676,7 @@ NXT_STATUS cCmdWrapFileOpenWrite(UBYTE * ArgV[]) //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; + return cCmdWrapFileOpenWriteHelper(OPENAPPENDDATA, ArgV); } //cCmdWrapFileRead @@ -6860,7 +8012,7 @@ NXT_STATUS cCmdWrapCommLSWrite(UBYTE * ArgV[]) pBuf = cCmdDVPtr(DVIndex); BufLength = DV_ARRAY[DVIndex].Count; - *pReturnVal = cCmdLSWrite(Port, (UBYTE)BufLength, pBuf, ResponseLength); + *pReturnVal = cCmdLSWrite(Port, (UBYTE)BufLength, pBuf, ResponseLength, 0); return (NO_ERR); } @@ -7266,7 +8418,7 @@ NXT_STATUS cCmdWrapKeepAlive(UBYTE * ArgV[]) -#define MAX_IOM_BUFFER_SIZE 64 +#define MAX_IOM_BUFFER_SIZE 800 // //cCmdWrapIOMapRead //ArgV[0]: (return) Status byte, SBYTE @@ -7477,7 +8629,6 @@ void cCmdWriteBenchmarkFile() } #endif - ///////////////////////////////////////////////////////////// // Dymanic syscall implementations //////////////////////////////////////////////////////////// @@ -7541,149 +8692,61 @@ NXT_STATUS cCmdWrapSetSleepTimeout(UBYTE * ArgV[]) 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 +//cCmdWrapCommBTOnOff +//ArgV[0]: (return) Status byte, SBYTE // JCH - this should be UWORD +//ArgV[1]: Power State, 0-1 // -NXT_STATUS cCmdWrapCommHSWrite(UBYTE * ArgV[]) +NXT_STATUS cCmdWrapCommBTOnOff(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; + UWORD retVal; + UWORD status; + UWORD * pReturnVal = (UWORD*)(ArgV[0]); - *pReturnVal = cCmdLSWrite(Port, (UBYTE)BufLength, pBuf, ResponseLength); + 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); } // -//cCmdWrapCommHSCheckStatus -//ArgV[0]: (return) Status code, SBYTE -//ArgV[1]: Port specifier, UBYTE -//ArgV[2]: BytesReady, UBYTE +//cCmdWrapCommBTConnection +//ArgV[0]: (return) Status byte, SBYTE // JCH - this should be UWORD +//ArgV[1]: Action, UBYTE +//ArgV[2]: name, UBYTE array CStr +//ArgV[3]: connection slot, UBYTE // -NXT_STATUS cCmdWrapCommHSCheckStatus(UBYTE * ArgV[]) +NXT_STATUS cCmdWrapCommBTConnection(UBYTE * ArgV[]) { - UBYTE Port = *(ArgV[1]); + UWORD retVal; + UWORD status; + UWORD * pReturnVal = (UWORD*)(ArgV[0]); + UBYTE *nmPtr; - *((SBYTE*)(ArgV[0])) = cCmdLSCheckStatus(Port); - *((UBYTE*)(ArgV[2])) = cCmdLSCalcBytesReady(Port); + 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); } + // -//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 +//cCmdWrapReadSemData +//ArgV[0]: return data, U8 +//ArgV[1]: which (0=used, 1=request), U8 // -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[]) +NXT_STATUS cCmdWrapReadSemData(UBYTE * ArgV[]) { if(!(*((UBYTE *)ArgV[1]))) *((UBYTE *)ArgV[0])= gUsageSemData; @@ -7923,6 +8986,489 @@ NXT_STATUS cCmdWrapListFiles (UBYTE * ArgV[]) return Status; } +// +//cCmdWrapCommExecuteFunction +//ArgV[0]: (return) Result word, UWORD +//ArgV[1]: UBYTE Cmd +//ArgV[2]: UBYTE Param1 +//ArgV[3]: UBYTE Param2 +//ArgV[4]: UBYTE Param3 +//ArgV[5]: Name, UBYTE array +//ArgV[6]: UWORD RetVal +// +NXT_STATUS cCmdWrapCommExecuteFunction(UBYTE * ArgV[]) +{ + // resolve Name + ArgV[5] = cCmdDVPtr(*(DV_INDEX *)(ArgV[5])); + + *(UWORD*)(ArgV[0]) = + pMapComm->pFunc(*(UBYTE*)(ArgV[1]), + *(UBYTE*)(ArgV[2]), + *(UBYTE*)(ArgV[3]), + *(UBYTE*)(ArgV[4]), + (UBYTE*)(ArgV[5]), + (UWORD*)(ArgV[6])); + return (NO_ERR); +} + +// +//cCmdWrapLoaderExecuteFunction +//ArgV[0]: (return) Result word, UWORD +//ArgV[1]: UBYTE Cmd +//ArgV[2]: FileName, UBYTE array +//ArgV[3]: Buffer, UBYTE array +//ArgV[4]: ULONG pLength +// +NXT_STATUS cCmdWrapLoaderExecuteFunction(UBYTE * ArgV[]) +{ + // resolve FileName + ArgV[2] = cCmdDVPtr(*(DV_INDEX *)(ArgV[2])); + // resolve Buffer + ArgV[3] = cCmdDVPtr(*(DV_INDEX *)(ArgV[3])); + + *(UWORD*)(ArgV[0]) = + pMapLoader->pFunc(*(UBYTE*)(ArgV[1]), + (UBYTE*)(ArgV[2]), + (UBYTE*)(ArgV[3]), + (ULONG*)(ArgV[4])); + return (NO_ERR); +} + +// +//cCmdWrapIOMapReadByID +//ArgV[0]: (return) Status byte, SBYTE +//ArgV[1]: ModuleID, ULONG +//ArgV[2]: Offset, UWORD +//ArgV[3]: Count, UWORD +//ArgV[4]: Buffer, UBYTE array +// +NXT_STATUS cCmdWrapIOMapReadByID(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]); + ULONG ModuleID = *(ULONG*)ArgV[1]; + + DV_INDEX DVIndex; + + //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); + } + + //Module was found, transfer Offset into first two bytes of DataBuffer and attempt to read + *(UWORD*)(DataBuffer) = Offset; + LStatus = pMapLoader->pFunc(IOMAPREAD, (UBYTE *)&ModuleID, 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 return + return (Status); + } + + //Alloc succeeded, so resolve and copy away + ArgV[4] = cCmdDVPtr(DVIndex); + memcpy(ArgV[4], &(DataBuffer[2]), Count); + } + + *pReturnVal = LOADER_ERR_BYTE(LStatus); + + return (NO_ERR); +} + +// +//cCmdWrapIOMapWriteByID +//ArgV[0]: (return) Status byte, SBYTE +//ArgV[1]: ModuleID, ULONG +//ArgV[2]: Offset, UWORD +//ArgV[3]: Buffer, UBYTE array +// +NXT_STATUS cCmdWrapIOMapWriteByID(UBYTE * ArgV[]) +{ + UWORD LStatus; + + SBYTE * pReturnVal = (SBYTE*)(ArgV[0]); + UWORD Offset = *(UWORD*)(ArgV[2]); + ULONG ModuleID = *(ULONG*)ArgV[1]; + + //Our copy of 'Count' must be a ULONG to match the loader interface + ULONG Count; + DV_INDEX DVIndex; + + //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 buffer + 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); + } + + //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, (UBYTE *)&ModuleID, DataBuffer, &Count); + + *pReturnVal = LOADER_ERR_BYTE(LStatus); + + return (NO_ERR); +} + +/* +NXT_STATUS cCmdWrapFileFindHelper(UBYTE First, UBYTE * ArgV[]) +{ + LOADER_STATUS LStatus; + NXT_STATUS Status; + DV_INDEX DVIndex; + UBYTE LoaderCmd = FINDNEXT; + + UBYTE FileMask[FILENAME_LENGTH+1]; + + //Resolve array arguments + // input mask/output filename + DVIndex = *(DV_INDEX *)(ArgV[2]); + if (First) { + LoaderCmd = FINDFIRST; + ArgV[1] = FileMask; + memcpy(FileMask, cCmdDVPtr(DVIndex), DV_ARRAY[DVIndex].Count); + } + //Size Buffer to Length + //Add room for null terminator to length + Status = cCmdDVArrayAlloc(DVIndex, (UWORD)(FILENAME_LENGTH + 1)); + if (IS_ERR(Status)) + return Status; + ArgV[2] = cCmdDVPtr(DVIndex); + + LStatus = pMapLoader->pFunc(LoaderCmd, 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); +} +*/ +//cCmdWrapFileFindFirst +//ArgV[0]: (Function return) Loader status, U16 return +//ArgV[1]: File Handle, U8 out +//ArgV[2]: Filename, CStr in/out +//ArgV[3]: Length, U32 out +NXT_STATUS cCmdWrapFileFindFirst(UBYTE * ArgV[]) +{ +// return cCmdWrapFileFindHelper(TRUE, ArgV); + LOADER_STATUS LStatus; + NXT_STATUS Status; + DV_INDEX DVIndex; + + UBYTE FileMask[FILENAME_LENGTH+1]; + + //Resolve array arguments + // input mask/output filename + DVIndex = *(DV_INDEX *)(ArgV[2]); + memcpy(FileMask, cCmdDVPtr(DVIndex), DV_ARRAY[DVIndex].Count); + //Size Buffer to Length + //Add room for null terminator to length + Status = cCmdDVArrayAlloc(DVIndex, (UWORD)(FILENAME_LENGTH + 1)); + if (IS_ERR(Status)) + return Status; + ArgV[2] = cCmdDVPtr(DVIndex); + + LStatus = pMapLoader->pFunc(FINDFIRST, FileMask, 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); +} + +//cCmdWrapFileFindNext +//ArgV[0]: (Function return) Loader status, U16 return +//ArgV[1]: File Handle, U8 in/out +//ArgV[2]: Filename, CStr out +//ArgV[3]: Length, U32 out +NXT_STATUS cCmdWrapFileFindNext(UBYTE * ArgV[]) +{ +// return cCmdWrapFileFindHelper(FALSE, ArgV); + LOADER_STATUS LStatus; + NXT_STATUS Status; + DV_INDEX DVIndex; + + //Resolve array arguments + // output filename + DVIndex = *(DV_INDEX *)(ArgV[2]); + //Size Buffer to Length + //Add room for null terminator to length + Status = cCmdDVArrayAlloc(DVIndex, (UWORD)(FILENAME_LENGTH + 1)); + if (IS_ERR(Status)) + return Status; + ArgV[2] = cCmdDVPtr(DVIndex); + + LStatus = pMapLoader->pFunc(FINDNEXT, 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); +} + +//cCmdWrapFileOpenReadLinear +//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 cCmdWrapFileOpenReadLinear(UBYTE * ArgV[]) +{ + return cCmdWrapFileOpenReadHelper(OPENREADLINEAR, ArgV); +} + +//cCmdWrapFileOpenWriteLinear +//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 cCmdWrapFileOpenWriteLinear(UBYTE * ArgV[]) +{ + return cCmdWrapFileOpenWriteHelper(OPENWRITELINEAR, ArgV); +} + +//cCmdWrapFileOpenWriteNonLinear +//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 cCmdWrapFileOpenWriteNonLinear(UBYTE * ArgV[]) +{ + return cCmdWrapFileOpenWriteHelper(OPENWRITE, ArgV); +} + +// +//cCmdWrapCommHSControl +//ArgV[0]: (return) Status byte, SBYTE +//ArgV[1]: Command, UBYTE (init, uart, or exit) +//ArgV[2]: BaudRate, UBYTE +//ArgV[3]: Mode, UWORD +NXT_STATUS cCmdWrapCommHSControl(UBYTE * ArgV[]) +{ + pMapComm->HsInBuf.InPtr = 0; + pMapComm->HsInBuf.OutPtr = 0; + pMapComm->HsOutBuf.InPtr = 0; + pMapComm->HsOutBuf.OutPtr = 0; + switch (*(ArgV[1])) + { + case HS_CTRL_INIT: + { + // hi-speed init + pMapComm->HsState = HS_ENABLE; + pMapComm->HsFlags = HS_UPDATE; + } + break; + + case HS_CTRL_UART: + { + // hi-speed setup uart + pMapComm->HsSpeed = *(ArgV[2]); + pMapComm->HsMode = *(ArgV[3]); + pMapComm->HsState = HS_INITIALISE; + pMapComm->HsFlags = HS_UPDATE; + } + break; + + case HS_CTRL_EXIT: + { + // hi-speed exit + pMapComm->HsState = HS_DISABLE; + pMapComm->HsFlags = HS_UPDATE; + } + break; + } + + *((UWORD *)ArgV[0]) = pMapComm->HsState; + + return (NO_ERR); +} + +//cCmdWrapCommHSCheckStatus +//ArgV[0]: SendingData, UBYTE out +//ArgV[1]: DataAvailable, UBYTE out +NXT_STATUS cCmdWrapCommHSCheckStatus(UBYTE * ArgV[]) +{ + *(ArgV[0]) = (pMapComm->HsOutBuf.InPtr != pMapComm->HsOutBuf.OutPtr) || (pMapComm->HsState == HS_SEND_DATA); + *(ArgV[1]) = (pMapComm->HsInBuf.InPtr != pMapComm->HsInBuf.OutPtr); + return (NO_ERR); +} + +//cCmdWrapCommHSWrite +//ArgV[0]: (return) Status byte, SBYTE +//ArgV[1]: Buffer +NXT_STATUS cCmdWrapCommHSWrite(UBYTE * ArgV[]) +{ + SBYTE * pReturnVal = (SBYTE*)(ArgV[0]); + UBYTE * pBuf; + UWORD BufLength; + DV_INDEX DVIndex; + + //Resolve array arguments + DVIndex = *(DV_INDEX *)(ArgV[1]); + pBuf = cCmdDVPtr(DVIndex); + BufLength = DV_ARRAY[DVIndex].Count; + + if (BufLength > SIZE_OF_HSBUF) + { + *pReturnVal = ERR_INVALID_SIZE; + return (NO_ERR); + } + + // set inptr & outptr + pMapComm->HsOutBuf.OutPtr = 0; + pMapComm->HsOutBuf.InPtr = BufLength; + memcpy(pMapComm->HsOutBuf.Buf, pBuf, BufLength); + + // send the data + pMapComm->HsState = HS_SEND_DATA; + pMapComm->HsFlags = HS_UPDATE; + + *pReturnVal = pMapComm->HsState; + + return (NO_ERR); +} + +//cCmdWrapCommHSRead +//ArgV[0]: (return) Status byte, SBYTE +//ArgV[1]: Buffer, out +NXT_STATUS cCmdWrapCommHSRead(UBYTE * ArgV[]) +{ + //Normally, bytes available is a simple difference. + SLONG Tmp = pMapComm->HsInBuf.InPtr - pMapComm->HsInBuf.OutPtr; + + //If InPtr is actually behind OutPtr, circular buffer has wrapped. Account for wrappage... + if (Tmp < 0) + Tmp = (pMapComm->HsInBuf.InPtr + (SIZE_OF_HSBUF - pMapComm->HsInBuf.OutPtr)); + + //Resolve array arguments + // output buffer + DV_INDEX DVIndex = *(DV_INDEX *)(ArgV[1]); + //Size Buffer to Length + NXT_STATUS Status = cCmdDVArrayAlloc(DVIndex, (UWORD)Tmp); + if (IS_ERR(Status)) + return Status; + UBYTE* pBuf = cCmdDVPtr(DVIndex); + ArgV[1] = pBuf; + + //If the bytes we want to read wrap around the end, we must first read the end, then reset back to the beginning + UBYTE BytesToRead = (UBYTE)Tmp; + if (pMapComm->HsInBuf.OutPtr + BytesToRead >= SIZE_OF_HSBUF) + { + BytesToRead = SIZE_OF_HSBUF - pMapComm->HsInBuf.OutPtr; + memcpy(pBuf, pMapComm->HsInBuf.Buf + pMapComm->HsInBuf.OutPtr, BytesToRead); + pMapComm->HsInBuf.OutPtr = 0; + pBuf += BytesToRead; + BytesToRead = Tmp - BytesToRead; + } + + memcpy(pBuf, pMapComm->HsInBuf.Buf + pMapComm->HsInBuf.OutPtr, BytesToRead); + pMapComm->HsInBuf.OutPtr += BytesToRead; + + return (NO_ERR); +} + +//cCmdWrapCommLSWriteEx +//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 +//ArgV[4]: NoRestartOnRead, UBYTE, specifies whether or not to restart before the read +// +NXT_STATUS cCmdWrapCommLSWriteEx(UBYTE * ArgV[]) +{ + SBYTE * pReturnVal = (SBYTE*)(ArgV[0]); + UBYTE Port = *(ArgV[1]); + UBYTE * pBuf; + UWORD BufLength; + UBYTE ResponseLength = *(ArgV[3]); + UBYTE NoRestartOnRead = *(ArgV[4]); + 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, NoRestartOnRead); + + return (NO_ERR); +} + +//cCmdWrapFileSeek +//ArgV[0]: (Function return) Loader status, U16 return +//ArgV[1]: File Handle, U8 in/out +//ArgV[2]: Origin, U8 in +//ArgV[3]: Length, S32 in +NXT_STATUS cCmdWrapFileSeek(UBYTE * ArgV[]) +{ + UBYTE Origin = *((UBYTE *)ArgV[2]); + LOADER_STATUS LStatus = pMapLoader->pFunc(Origin+SEEKFROMSTART, ArgV[1], NULL, (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); +} + +//cCmdWrapFileResize +//ArgV[0]: (Function return) Loader status, U16 return +//ArgV[1]: File Handle, U8 in/out +//ArgV[2]: NewSize, U16 in +NXT_STATUS cCmdWrapFileResize(UBYTE * ArgV[]) +{ + LOADER_STATUS LStatus = pMapLoader->pFunc(RESIZEDATAFILE, ArgV[1], NULL, (ULONG *)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); +} + +NXT_STATUS cCmdWrapUndefinedSysCall(UBYTE * ArgV[]) +{ + return (NO_ERR); +} + #ifdef SIM_NXT // Accessors for simulator library code SWORD cCmdGetCodeWord(CLUMP_ID Clump, CODE_INDEX Index) -- cgit v1.2.3