c_cmd.c

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

  }

  return FALSE;
}


UBYTE cCmdIsQSane(CLUMP_Q * Queue)
{
  CLUMP_ID Head, Tail;
  CLUMP_REC * pHead;

  if (Queue == NULL)
  {
    NXT_BREAK;
    return FALSE;
  }

  Head = Queue->Head;
  Tail = Queue->Tail;

  if (Head == NOT_A_CLUMP && cCmdIsClumpIDSane(Tail))
    return FALSE;

  if (cCmdIsClumpIDSane(Head) && Tail == NOT_A_CLUMP)
    return FALSE;

  if (cCmdIsClumpIDSane(Head) && cCmdIsClumpIDSane(Tail))
  {
    pHead = &(VarsCmd.pAllClumps[Head]);

    //!!! More comprehensive queue tests could go here

    //Check for mislinked head if there are at least two queue members
    if (Head != Tail && pHead->Link == NOT_A_CLUMP)
      return FALSE;
  }

    return TRUE;
}

//
// Mutex queuing functions
//

NXT_STATUS cCmdAcquireMutex(MUTEX_Q * Mutex, CLUMP_ID Clump)
{
  NXT_STATUS Status = NO_ERR;

  NXT_ASSERT(Mutex != NULL && cCmdIsClumpIDSane(Clump));

  if (Mutex->Owner == NOT_A_CLUMP)
  {
    //Mutex is open, so just take it
    Mutex->Owner = Clump;

    NXT_ASSERT(Mutex->WaitQ.Head == NOT_A_CLUMP && Mutex->WaitQ.Tail == NOT_A_CLUMP);
  }
  else
  {
    //Mutex is reserved by someone else, take self off RunQ and add to WaitQ
    cCmdDeQClump(&(VarsCmd.RunQ), Clump);
    cCmdEnQClump(&(Mutex->WaitQ), Clump);
    Status = CLUMP_SUSPEND;
  }

  NXT_ASSERT(cCmdIsQSane(&(Mutex->WaitQ)));

  return (Status);
}


NXT_STATUS cCmdReleaseMutex(MUTEX_Q * Mutex, CLUMP_ID Clump)
{
  NXT_ASSERT(Mutex != NULL);
  //!!! don't actually need to pass in Owner clump, but provides nice error checking for now
  // Might want to return an error/warning if we see a Release on an free mutex, though...
  NXT_ASSERT(Clump != NOT_A_CLUMP && Mutex->Owner == Clump);

  //Always set new Owner to WaitQ's Head, since NOT_A_CLUMP means mutex is free
  Mutex->Owner = Mutex->WaitQ.Head;

  if (Mutex->Owner != NOT_A_CLUMP)
  {
    cCmdDeQClump(&(Mutex->WaitQ), Mutex->Owner);
    cCmdEnQClump(&(VarsCmd.RunQ), Mutex->Owner);
  }

  NXT_ASSERT(cCmdIsQSane(&(Mutex->WaitQ)));
  NXT_ASSERT(cCmdIsQSane(&(VarsCmd.RunQ)));

  return (NO_ERR);
}


NXT_STATUS cCmdSchedDependents(CLUMP_ID Clump, SWORD Begin, SWORD End)
{
  CLUMP_ID CurrDepClumpID;
  SWORD i;

  //Begin and End specify range of CLUMP_IDs in dependent list to schedule
  //If either equals -1, both should equal -1, and no dependents will be scheduled
  //Else schedule specified subset offset from pDependents

  //Check for valid args
  NXT_ASSERT(cCmdIsClumpIDSane(Clump));
  NXT_ASSERT((Begin >= 0 && End >= 0 && End < VarsCmd.pAllClumps[Clump].DependentCount)
          || (Begin == -1 && End == -1));

  //If non-empty range
  if (Begin != -1 || End != -1)
  {
    //update dependents, scheduling if their CurrFireCount reaches 0
    for (i = Begin; i <= End; i++)
    {
      CurrDepClumpID = VarsCmd.pAllClumps[Clump].pDependents[i];

      NXT_ASSERT(cCmdIsClumpIDSane(CurrDepClumpID));

      VarsCmd.pAllClumps[CurrDepClumpID].CurrFireCount--;

      if (VarsCmd.pAllClumps[CurrDepClumpID].CurrFireCount == 0)
        cCmdEnQClump(&(VarsCmd.RunQ), CurrDepClumpID);
    }
  }

  return (NO_ERR);
}


NXT_STATUS cCmdSchedDependent(CLUMP_ID Clump, CLUMP_ID TargetClump)
{
  //TargetClump specifies the clump number of the target to schedule explicitly.

  //Check for valid args
  NXT_ASSERT(cCmdIsClumpIDSane(Clump));
  NXT_ASSERT(cCmdIsClumpIDSane(TargetClump));

  VarsCmd.pAllClumps[TargetClump].CurrFireCount--;
  if (VarsCmd.pAllClumps[TargetClump].CurrFireCount == 0)
    cCmdEnQClump(&(VarsCmd.RunQ), TargetClump);

  return (NO_ERR);
}


UBYTE cCmdIsClumpIDSane(CLUMP_ID Clump)
{
  if (Clump < VarsCmd.AllClumpsCount)
    return TRUE;
  else
    return FALSE;
}


//
// Memory pool management functions
//
void cCmdInitPool(void)
{
  ULONG i;

  //VarsCmd.Pool is a UBYTE pointer to ULONG array
  //This was done to enforce portable alignment.
  VarsCmd.Pool = (UBYTE*)(IOMapCmd.MemoryPool);

  for (i = 0; i < (POOL_MAX_SIZE / 4); i++)
    ((SLONG*)(POOL_START))[i] = 0xDEADBEEF;

  VarsCmd.PoolSize = 0;
}


NXT_STATUS cCmdDSArrayAlloc(DS_ELEMENT_ID DSElementID, UWORD Offset, UWORD NewCount)
{
  NXT_STATUS Status = NO_ERR;
  UWORD DVIndex;
  UWORD OldCount;
  UWORD i;

  NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID));

  //Only arrays are valid here
  //!!! Recommended to upgrade NXT_ASSERT to ERR_INSTR return
  NXT_ASSERT(cCmdDSType(DSElementID) == TC_ARRAY);

  DVIndex = cCmdGetDVIndex(DSElementID, Offset);
  OldCount = DV_ARRAY[DVIndex].Count;

  Status = cCmdDVArrayAlloc(DVIndex, NewCount);
  if (Status < NO_ERR)
    return Status;

  if (OldCount > NewCount)
  {
    //Free dope vectors for sub-arrays.
    for (i = NewCount; i < OldCount; i++)
    {
      Status = cCmdFreeSubArrayDopeVectors(INC_ID(DSElementID), ARRAY_ELEM_OFFSET(DVIndex, i));
      if (IS_ERR(Status))
        return Status;
    }
  }
  else if (OldCount < NewCount)
  {
    //Alloc dope vectors for sub-arrays. Set up DVIndexes
    for (i = OldCount; i < NewCount; i++)
    {
      Status = cCmdAllocSubArrayDopeVectors(INC_ID(DSElementID), ARRAY_ELEM_OFFSET(DVIndex, i));
      if (IS_ERR(Status))
        return Status;
    }
  }

  NXT_ASSERT(cCmdVerifyMemMgr());

  return Status;
}

NXT_STATUS cCmdDVArrayAlloc(DV_INDEX DVIndex, UWORD NewCount)
{
  NXT_STATUS Status = NO_ERR;
  UBYTE *pData;
  UWORD ArraySize, InplaceSize;
  UWORD NextDVIndex;
  UWORD OldCount;

  OldCount = DV_ARRAY[DVIndex].Count;

  if (OldCount == NewCount)
  {
    //Nothing to alloc. Return.
    return Status;
  }
  else if (OldCount > NewCount)
  {
    //Already have the space. Shrink inplace.
    DV_ARRAY[DVIndex].Count = NewCount;
        return Status;
  }
  else // need to grow array
  {
    //Calculate new array size
    ArraySize = NewCount * DV_ARRAY[DVIndex].ElemSize;

    //Try growing inplace
    // If the Offset == NOT_AN_OFFSET then the array has never been allocated and can't grow inplace.
    if (DV_ARRAY[DVIndex].Offset != NOT_AN_OFFSET)
    {
      //Get pointer to next dope vector in dataspace
      if (DV_ARRAY[DVIndex].Link != NOT_A_DS_ID)
      {
        NextDVIndex = DV_ARRAY[DVIndex].Link;
        InplaceSize = DV_ARRAY[NextDVIndex].Offset - DV_ARRAY[DVIndex].Offset;
      }
      else
      {
        //Last element in dataspace.
        NXT_ASSERT(DVIndex == VarsCmd.MemMgr.Tail);
        InplaceSize = VarsCmd.DataspaceSize - DV_ARRAY[DVIndex].Offset;
      }

      if (ArraySize <= InplaceSize)
      {
        DV_ARRAY[DVIndex].Count = NewCount;
            return Status;
      }
    }

    //Can't grow inplace, have to allocate new space

    //Make sure we properly align for type
    //!!! This could also overflow memory (make PoolSize > POOL_MAX_SIZE) if we're within 3 bytes of the end.
    //     I don't think it matters because if it does happend, we'll trigger the ERR_MEM below and compact.
    //     During compaction, we'll reclaim these unused bytes.
    //!!! Aligning beginning of ALL arrays to 4 byte address
    ALIGN_TO_MOD(VarsCmd.PoolSize, SIZE_ULONG);
    ALIGN_TO_MOD(VarsCmd.DataspaceSize, SIZE_ULONG);

    if (VarsCmd.PoolSize + ArraySize >= POOL_MAX_SIZE)
    {
      //Not enough memory available
      return ERR_MEM;
    }

    //Get data from end of pool
    pData = VarsCmd.Pool + VarsCmd.PoolSize;
    //Grow pool and dataspace
    VarsCmd.PoolSize += ArraySize;
    VarsCmd.DataspaceSize += ArraySize;

    //Move Array Data
    memmove(pData, VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset, (UWORD)(DV_ARRAY[DVIndex].ElemSize * DV_ARRAY[DVIndex].Count));
    //!!! Clear mem so we make sure we don't reference stale data. Not strictly needed.
    memset(VarsCmd.pDataspace + DV_ARRAY[DVIndex].Offset, 0xFF, (UWORD)(DV_ARRAY[DVIndex].ElemSize * DV_ARRAY[DVIndex].Count));

    //Update dope vector
    DV_ARRAY[DVIndex].Offset = pData - VarsCmd.pDataspace;
    DV_ARRAY[DVIndex].Count = NewCount;

    //Move dope vector to end of MemMgr list
    Status = cCmdMemMgrMoveToTail(DVIndex);
    if (IS_ERR(Status))
      return Status;

    NXT_ASSERT(cCmdVerifyMemMgr());
  }

  return Status;
}


//!!! Recursive function
NXT_STATUS cCmdAllocSubArrayDopeVectors(DS_ELEMENT_ID DSElementID, UWORD Offset)
{
  // Walks a single array element to see if it contains arrays
  // For any array it finds, a dope vector is allocated and the DVIndex is placed in the dataspace for the parent array.
  // This is a non-recursive function. It only walks the immediate array element.
  // DSElementID - ID of array sub-entry
  // Offset - offset to array element in dataspace
  NXT_STATUS Status = NO_ERR;
  TYPE_CODE TypeCode;
  DV_INDEX DVIndex;
  UWORD i;
  UWORD DVIndexOffset; //Offset to DVIndex field that points to the DopeVector from pDataspace
  UWORD LoopCount = 1;
  UWORD ElemSize;

  for (i = 0; i < LoopCount; i++)
  {
    TypeCode = cCmdDSType((DS_ELEMENT_ID)(DSElementID + i));
    if (TypeCode == TC_CLUSTER)
    {
      LoopCount += cCmdClusterCount(DSElementID);
    }
    else if (TypeCode == TC_ARRAY)
    {
      //!!! ElemSize is a static value, but we don't have anywhere we put it (another TOC sub-entry?)
      //     It'd be nice to not have to recalculate it.
      ElemSize = cCmdCalcArrayElemSize((DS_ELEMENT_ID)(DSElementID + i));
      DVIndexOffset = VarsCmd.pDataspaceTOC[DSElementID + i].DSOffset + Offset;
      Status = cCmdAllocDopeVector(&DVIndex, ElemSize, DVIndexOffset);
      if (IS_ERR(Status))
        return Status;

      *((UWORD *)(VarsCmd.pDataspace + DVIndexOffset)) = DVIndex;
    }
  }

  return Status;
}


//!!! Recursive function
NXT_STATUS cCmdFreeSubArrayDopeVectors(DS_ELEMENT_ID DSElementID, UWORD Offset)
{
  // Walks a single array element to see if it contains arrays
  // Frees all dope vectors associated with the array element.
  // Recursively deletes sub-arrays.
  // DSElementID - ID of array sub-entry
  // Offset - offset to array element in dataspace
  NXT_STATUS Status = NO_ERR;
  TYPE_CODE TypeCode;
  DV_INDEX DVIndex;
  UWORD i, Count;

  TypeCode = cCmdDSType(DSElementID);

  if (TypeCode == TC_ARRAY)
  {
    DVIndex = cCmdGetDVIndex(DSElementID, Offset);

    NXT_ASSERT(DVIndex < DV_ARRAY[0].Count);

    Count = DV_ARRAY[DVIndex].Count;
    //Recur on sub-elements
    for (i = 0; i < Count; i++)
    {
      Status = cCmdFreeSubArrayDopeVectors(INC_ID(DSElementID), ARRAY_ELEM_OFFSET(DVIndex, i));
      if (IS_ERR(Status))
        return Status;
    }

    //Free Dope Vector
    Status = cCmdFreeDopeVector(DVIndex);
  }
  else if (TypeCode == TC_CLUSTER)
  {
    Count = cCmdClusterCount(DSElementID);
    DSElementID = INC_ID(DSElementID);
    //Recur on sub-elements
    for (i = 0; i < Count; i++)
    {
      Status = cCmdFreeSubArrayDopeVectors((DS_ELEMENT_ID)(DSElementID + i), Offset);
      if (IS_ERR(Status))
        return Status;
    }
  }

  return Status;
}


NXT_STATUS cCmdAllocDopeVector(DV_INDEX *pIndex, UWORD ElemSize, UWORD BackPtr)
{
  NXT_STATUS Status = NO_ERR;

  if (VarsCmd.MemMgr.FreeHead == NOT_A_DS_ID)
  {
    //No free DVs. Need to grow DopeVector array.
    Status = cCmdGrowDopeVectorArray(DV_ARRAY_GROWTH_COUNT);
    if (IS_ERR(Status))
      return Status;
  }

  NXT_ASSERT(VarsCmd.MemMgr.FreeHead != NOT_A_DS_ID);

  //Remove DV from free list
  *pIndex = VarsCmd.MemMgr.FreeHead;
  VarsCmd.MemMgr.FreeHead = DV_ARRAY[VarsCmd.MemMgr.FreeHead].Link;
  //Add DV to tail of MemMgr list
  Status = cCmdMemMgrInsertAtTail(*pIndex);

  //Initialize values
  DV_ARRAY[*pIndex].Offset = NOT_AN_OFFSET;
  DV_ARRAY[*pIndex].ElemSize = ElemSize;
  DV_ARRAY[*pIndex].Count = 0;
  DV_ARRAY[*pIndex].BackPtr = BackPtr;

  NXT_ASSERT(cCmdVerifyMemMgr());

  return Status;
}

//
//cCmdFreeDopeVector() - Open up a spot in the DopeVectorArray for future use
//