c_cmd.c

乐高机器人的源码,开发平台是IAR_for_AVR.

  IOMapCmd.OffsetDS = 0xFFFF;

  //Initialize format string and clear out FileName string
  strncpy((PSZ)(IOMapCmd.FormatString), VM_FORMAT_STRING, VM_FORMAT_STRING_SIZE);
  memset(IOMapCmd.FileName, 0, sizeof(IOMapCmd.FileName));

  dTimerInit();
  IOMapCmd.Tick = dTimerRead();

  return;
}


void cCmdCtrl(void)
{
  UBYTE Continue = TRUE;
  NXT_STATUS Status = NO_ERR;
  ULONG i;
  CLUMP_ID CurrClumpID;

  switch (VarsCmd.VMState)
  {
    case VM_IDLE:
    {
      //If there's a new program to activate...
      if (IOMapCmd.ActivateFlag == TRUE)
      {
        //Clear flag so we only activate once per new file
        IOMapCmd.ActivateFlag = FALSE;

        Status = cCmdActivateProgram(IOMapCmd.FileName);

        //If we hit an activation error:
        //1. Set PROG_ERROR status
        //2. Proceed to VM_RESET1 (some unneeded work, yes, but preserves contract with UI
        if (IS_ERR(Status))
        {
          IOMapCmd.ProgStatus = PROG_ERROR;
          VarsCmd.VMState = VM_RESET1;
        }
        //Else start running program
        else
        {
          VarsCmd.VMState = VM_RUN_FREE;
          IOMapCmd.ProgStatus = PROG_RUNNING;
          VarsCmd.StartTick = IOMapCmd.Tick;

#if VM_BENCHMARK
          //Re-init benchmark
          VarsCmd.InstrCount         = 0;
          VarsCmd.Average            = 0;
          VarsCmd.OverTimeCount      = 0;
          VarsCmd.MaxOverTimeLength  = 0;
          VarsCmd.CmdCtrlCount       = 0;
          VarsCmd.CompactionCount    = 0;
          VarsCmd.LastCompactionTick = 0;
          VarsCmd.MaxCompactionTime  = 0;
          memset(VarsCmd.OpcodeBenchmarks, 0, sizeof(VarsCmd.OpcodeBenchmarks));
          memset(VarsCmd.SyscallBenchmarks, 0, sizeof(VarsCmd.SyscallBenchmarks));
#endif
          //Reset devices to a known state before we begin running
          cCmdResetDevices();

          pMapUi->Flags |= (UI_DISABLE_LEFT_RIGHT_ENTER | UI_DISABLE_EXIT);
        }
      }

      break;
    }

    //Initialize VM internal state data and devices which must respond immediately to program ending
    case VM_RESET1:
    {
      //If we aborted a program, reset devices (specifically, motors) immediately
      //Otherwise, wait for UI to put us into PROG_RESET (gives motors a chance to brake before setting to coast)
      //!!! This means cCmdResetDevices will get called twice on abort.  Should not be a big deal.
      if (IOMapCmd.ProgStatus == PROG_ABORT)
        cCmdResetDevices();

      //Reenable UI access to buttons
      pMapUi->Flags &= ~(UI_DISABLE_LEFT_RIGHT_ENTER | UI_DISABLE_EXIT);

#if VM_BENCHMARK
      if (IOMapCmd.Tick != VarsCmd.StartTick)
        VarsCmd.Average = VarsCmd.InstrCount / (IOMapCmd.Tick - VarsCmd.StartTick);
      else
        //It appears that we finished in 0 milliseconds.  Very unlikely on ARM, so set a flag value.
        VarsCmd.Average = 0xFFFFFFFF;

      cCmdWriteBenchmarkFile();
#endif

      //Re-initialize program state data (contents of memory pool preserved)
      //!!! Skip this step in simulator builds so helper access methods still work
#ifndef SIM_NXT
      cCmdDeactivateProgram();
#endif //SIM_NXT

      //If this program has taken over the display, reset it for the UI
      cCmdRestoreDefaultScreen();

      //Stop any currently playing sound and re-init volume according to UI prefs
      pMapSound->State = SOUND_STOP;
      pMapSound->Volume = pMapUi->Volume;

      //Artificially set CommStatReset to BTBUSY to force at least one SETCMDMODE call (see VM_RESET2 case)
      VarsCmd.CommStatReset = (SWORD)BTBUSY;

      VarsCmd.VMState = VM_RESET2;
    }
    break;

    case VM_RESET2:
    {
      //Reset BlueCore into "command mode" (close any open streams)
      //Since SETCMDMODE subject to BTBUSY, we may need to make multiple calls
      //Any CommStatReset value other than BTBUSY means our request was accepted
      //Assumptions:
      //Process should never take longer than UI timeout (see below), but if it does,
      // we could be left with the stream open to an NXT peer and block out the PC.
      //Also assuming that once SETCMDMODE request is accepted, it never fails.
      if (VarsCmd.CommStatReset == (SWORD)BTBUSY && VarsCmd.DirtyComm == TRUE)
        pMapComm->pFunc(SETCMDMODE, 0, 0, 0, NULL, (UWORD*)&(VarsCmd.CommStatReset));

      //If UI is done displaying ending program status, move on.
      if (IOMapCmd.ProgStatus == PROG_RESET)
      {
        //Reset devices whenever a program ends for any reason
        cCmdResetDevices();

        VarsCmd.DirtyComm = FALSE;

        //Go to VM_IDLE state
        VarsCmd.VMState = VM_IDLE;
        IOMapCmd.ProgStatus = PROG_IDLE;
      }
      break;
    }

    case VM_RUN_FREE:
    case VM_RUN_SINGLE:
    {

#if VM_BENCHMARK
      //IOMapCmd.Tick currently holds the tick from the end of last cCmdCtrl call.
      //If we don't come back here before dTimerRead() increments, the m_sched loop has taken *at least* 1 ms.
      if (IOMapCmd.Tick != dTimerRead())
      {
        VarsCmd.OverTimeCount++;
        //Record maximum magnitude of schedule loop overage, in millisecs
        if (dTimerRead() - IOMapCmd.Tick > VarsCmd.MaxOverTimeLength)
          VarsCmd.MaxOverTimeLength = dTimerRead() - IOMapCmd.Tick;
      }
      VarsCmd.CmdCtrlCount++;
#endif
      //Abort current program if cancel button is pressed
      if (IOMapCmd.DeactivateFlag == TRUE || pMapButton->State[BTN1] & PRESSED_EV)
      {
        IOMapCmd.DeactivateFlag = FALSE;

        //Clear pressed event so it doesn't get double-counted by UI
        pMapButton->State[BTN1] &= ~PRESSED_EV;

        //Go to VM_RESET1 state and report abort
        VarsCmd.VMState = VM_RESET1;
        IOMapCmd.ProgStatus = PROG_ABORT;
        break;
      }

      //Assert that we have an active program
      NXT_ASSERT(VarsCmd.ActiveProgHandle != NOT_A_HANDLE);

      //Execute from at least one clump
      do
      {
        if (cCmdIsClumpIDSane(VarsCmd.RunQ.Head))
        {
          //Stash and dequeue RunQ's head clump
          CurrClumpID = VarsCmd.RunQ.Head;

          //Execute at least one instruction from current clump
          //Execute up to 'Priority' instructions as long as we are in VM_FREE_RUN mode
          //Finishing/suspending a clump, BREAKOUT_REQ, or any errors will also end this loop
          i = 0;
          do
          {
            //Interpret one instruction per call, advancing PC as needed
            Status = cCmdInterpFromClump(CurrClumpID);

#if VM_BENCHMARK
            VarsCmd.InstrCount++;
#endif

            NXT_ASSERT(!IS_ERR(Status));
            if (IS_ERR(Status) || Status == CLUMP_DONE || Status == CLUMP_SUSPEND || Status == BREAKOUT_REQ || Status == STOP_REQ)
            {
              //We're done with this clump or breaking out prematurely,
              //so break the multi-instruction loop
              break;
            }
            else
            {
              //Count up one more instruction for this pass
              i++;
            }
          } while (VarsCmd.VMState == VM_RUN_FREE && i < VarsCmd.pAllClumps[CurrClumpID].Priority);

          //Only rotate RunQ on a "normal" finish, i.e. no error, clump end, or breakout request
          if (!(IS_ERR(Status) || Status == CLUMP_DONE || Status == CLUMP_SUSPEND || Status == BREAKOUT_REQ))
            cCmdRotateQ(&(VarsCmd.RunQ));
        }

        //Re-evaluate conditions for stopping the dataflow scheduler
        //Halt program on all errors
        if (IS_ERR(Status))
        {
          Continue = FALSE;

          VarsCmd.VMState = VM_RESET1;
          IOMapCmd.ProgStatus = PROG_ERROR;
        }
        else if (Status == BREAKOUT_REQ)
        {
          Continue = FALSE;
        }
        //If RunQ is empty or user requested early termination, program is done
        else if (!cCmdIsClumpIDSane(VarsCmd.RunQ.Head) || Status == STOP_REQ)
        {
          Continue = FALSE;

          VarsCmd.VMState = VM_RESET1;
          IOMapCmd.ProgStatus = PROG_OK;
        }
        //VM_RUN_FREE means continue executing until a new ms tick rolls over
        else if (VarsCmd.VMState == VM_RUN_FREE)
        {
          Continue = (IOMapCmd.Tick == dTimerRead());
        }
        //Otherwise execute only one pass per call
        else //VarsCmd.VMState == VM_RUN_SINGLE
        {
          VarsCmd.VMState = VM_RUN_PAUSE;
          Continue = FALSE;
        }

      } while (Continue == TRUE);

    break;
    }
  }//END state machine switch

  //Busy wait to always maintain 1ms period
  BUSY_WAIT_NEXT_MS;

  //Set tick to new value for next time 'round
  IOMapCmd.Tick = dTimerRead();

  return;
}


void cCmdExit(void)
{
  dTimerExit();

  return;
}


NXT_STATUS cCmdReadFileHeader(UBYTE* pData, ULONG DataSize,
            PROG_FILE_OFFSETS* pFileOffsets)
{
  ULONG i;
  UBYTE * pCursor;
  UWORD CurrOffset = 0;
  UBYTE DepCount;
  UWORD DopeVectorOffset;
  UWORD FileClumpCount;
  UBYTE FileMajor, FileMinor,
        CompatibleMinor, CompatibleMajor,
        CurrentMajor;

  NXT_ASSERT(pData != NULL);

  //Assign pCursor to point to version word inside file header
  pCursor = (pData + VM_FORMAT_STRING_SIZE - 2);

  //Decode version numbers into comparable bytes
  FileMajor = *pCursor;
  FileMinor = *(pCursor + 1);
  CompatibleMajor = (UBYTE)(VM_OLDEST_COMPATIBLE_VERSION >> 8);
  CompatibleMinor = (UBYTE)(VM_OLDEST_COMPATIBLE_VERSION);
  CurrentMajor = (UBYTE)(FIRMWAREVERSION >> 8);
  //CurrentMinor = (UBYTE)(FIRMWAREVERSION);

  //Return ERR_VER if file lacks proper format string or version number
  //!!! Only checking major version recommended for future development
  if (strncmp((PSZ)pData, VM_FORMAT_STRING, VM_FORMAT_STRING_SIZE)
   || FileMajor < CompatibleMajor || FileMinor < CompatibleMinor
   || FileMajor > CurrentMajor)
  {
    NXT_BREAK;
    return (ERR_VER);
  }

  //Advance CurrOffset past header information
  CurrOffset += VM_FORMAT_STRING_SIZE;

  //
  //Initialize bookkeeping variables
  //
  VarsCmd.DataspaceCount = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  VarsCmd.DataspaceSize = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  VarsCmd.DSStaticSize = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  pFileOffsets->DSDefaultsSize = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  pFileOffsets->DynamicDefaults = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  pFileOffsets->DynamicDefaultsSize = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  VarsCmd.MemMgr.Head = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  VarsCmd.MemMgr.Tail = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  DopeVectorOffset = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  //!!! Odd code here to deal with type mismatch between file format and CLUMP_ID typedef.
  //Neither is trivial to change, so it's best to just check the data for consistency here.
  FileClumpCount = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  //Must have at least one clump and count can't exceed the NOT_A_CLUMP sentinel
  if (FileClumpCount == 0 || FileClumpCount >= NOT_A_CLUMP)
    return (ERR_FILE);
  else
    VarsCmd.AllClumpsCount = (CLUMP_ID)FileClumpCount;

  VarsCmd.CodespaceCount = *((UWORD*)(pData + CurrOffset));
  CurrOffset += 2;

  //Can't have a valid program with no code
  if (VarsCmd.CodespaceCount == 0)
    return (ERR_FILE);

  //
  // Now, calculate offsets for each data segment in the file
  //

  CurrOffset += CurrOffset % 2;
  pFileOffsets->DSTOC = CurrOffset;
  CurrOffset += VarsCmd.DataspaceCount * sizeof(DS_TOC_ENTRY);

  CurrOffset += CurrOffset % 2;
  pFileOffsets->DSDefaults = CurrOffset;
  CurrOffset += pFileOffsets->DSDefaultsSize;

  //ClumpRecs must be aligned on even boundaries
  CurrOffset += CurrOffset % 2;
  pFileOffsets->Clumps = CurrOffset;

  //Set cursor to start of clump records
  pCursor = pData + CurrOffset;

  //Set CurrOffset to start of dependent lists
  CurrOffset += VarsCmd.AllClumpsCount * VM_FILE_CLUMP_REC_SIZE;

  //Read dependent count from each clump record, advancing CurrOffset accordingly
  for (i = 0; i < VarsCmd.AllClumpsCount; i++)
  {
    DepCount = *(pCursor + 1);
    CurrOffset += DepCount;
    pCursor += VM_FILE_CLUMP_REC_SIZE;
  }

  //Codespace must be aligned on even boundary
  CurrOffset += CurrOffset % 2;
  pFileOffsets->Codespace = CurrOffset;

  //No need to read through codespace, but make sure CurrOffset ended up sane
  //If not, something went wrong reading the header information
  if (CurrOffset != (DataSize - VarsCmd.CodespaceCount * 2))
  {
    NXT_BREAK;
    return (ERR_FILE);
  }

  //
  // Finally, update VarsCmd fields
  //

  VarsCmd.RunQ.Head = NOT_A_CLUMP;
  VarsCmd.RunQ.Tail = NOT_A_CLUMP;

  //Reset codespace pointer
  VarsCmd.pCodespace = (CODE_WORD*)(pData + pFileOffsets->Codespace);

  //...placing clump records first...
  VarsCmd.pAllClumps = (CLUMP_REC*)(VarsCmd.Pool + VarsCmd.PoolSize);
  VarsCmd.PoolSize += VarsCmd.AllClumpsCount * sizeof(CLUMP_REC);

  //...then DSTOC...
  VarsCmd.pDataspaceTOC = (DS_TOC_ENTRY*)(pData + pFileOffsets->DSTOC);

  //...then the dataspace itself
  ALIGN_TO_MOD(VarsCmd.PoolSize, POOL_ALIGN);
  VarsCmd.pDataspace = (VarsCmd.Pool + VarsCmd.PoolSize);
  IOMapCmd.OffsetDS = (UWORD)((ULONG)(VarsCmd.pDataspace) - (ULONG)&(IOMapCmd));
  VarsCmd.PoolSize += VarsCmd.DataspaceSize;

  //init rest of MemMgr
  VarsCmd.MemMgr.pDopeVectorArray = (DOPE_VECTOR *)(VarsCmd.pDataspace + DopeVectorOffset);
  IOMapCmd.OffsetDVA = (UWORD)((ULONG)(VarsCmd.MemMgr.pDopeVectorArray) - (ULONG)&(IOMapCmd));
  VarsCmd.MemMgr.FreeHead = NOT_A_DS_ID;


  if (VarsCmd.PoolSize > POOL_MAX_SIZE)
  {
    NXT_BREAK;
    return (ERR_FILE);
  }

  return (NO_ERR);
}


//!!! Recursive function
NXT_STATUS cCmdInflateDSDefaults(UBYTE* pDSDefaults, UWORD *pDefaultsOffset, DS_ELEMENT_ID DSElementID)