fdi_que.c

Flash file system

/* ### Global Declarations
 * ################################# */
/*E.5.0.603.START*/ 
/* define these semaphores for static creation by GSM_MS initialization */

#if(SEM_CREATE_DESTROY == FALSE)
   SEM_ID SEM_cntrl_sync;              /* true when an item is in queue */
   SEM_MTX_ID SEM_cntrl_mutex;         /* mutex queue protection */
#endif
/*E.5.0.603.END*/

/* ### Global Functions
 * ################################# */

#if (PERF_TEST == TRUE)
extern SEM_ID PERF_SEM_Bkgd;           
#endif

/*############################################################################
 *### Q_Create
 *###
 *### DESCRIPTION:
 *###    This function is used to create a new message queue.  The capacity
 *###    of queue is given by the "max_queue_size" input parameter.  The
 *###    maximum queue size is 65532 bytes, and the minumum size is 32 bytes.
 *###    The size includes both the space required for storing messages in
 *###    the queue, as well as space for the internal control structures
 *###    needed to manage the queue.  The size should be an even multiple of
 *###    of 4 bytes -- if it is not, it will be rounded down to the nearest
 *###    multiple.
 *###
 *###    If the application wants to control where the queue gets its memory
 *###    from, it can supply a pointer to memory for the queue to use in the
 *###    "queue_memory" input parameter.  If the "queue_memory" parameter
 *###    is NULL, the queue code will attempt to allocate its own
 *###    memory by using the MALLOC macro.  Allowing the queue to manage
 *###    its own memory is the preferred method, since it better protects the
 *###    queue's data from other processes, though this may not be possible
 *###    (or feasible) on every system.
 *###
 *###    An error code will be returned indicating the success or failure
 *###    of the queue creation in the parameter "status_ptr".
 *###
 *###    The queue's identifier is returned from the function as a Q_ID type.
 *###
 *### PARAMETERS:
 *###    IN:
 *###       max_queue_size    -  The maximum total size of the queue (bytes)
 *###       queue_memory_ptr  -  Address of memory to use for queue
 *###    OUT:
 *###    Returns the following errors codes in the status_ptr
 *###       Q_ERR_NONE
 *###       Q_ERR_INVALID_SIZE
 *###       Q_ERR_NO_MEMORY
 *###       Q_ERR_SEMAPHORE
 *###
 *### RETURNS:
 *###       the queue's identifier
 *###*/
Q_ID
Q_Create(WORD max_queue_size, VOID_PTR queue_memory_ptr, Q_ERROR * status_ptr)
{
   register BYTE_PTR queue_ptr = (BYTE_PTR) queue_memory_ptr;
#if (Q_ALIGN_CHECKING == TRUE)
   WORD alignment_size;
#endif

   /* Initialize queue allocation controls */
   WORD i;

   QitemFree[0] = 1;
   QitemPtr [0] = Queue;
   QitemSize[0] = FDI_QUEUE_SIZE;
   for (i=1 ; i<MAX_QUEUE_ITEMS ; i++)
   {
     QitemFree[i] = 0;
     QitemPtr [i] = NULL;
     QitemSize[i] = 0;
   }

   *status_ptr = Q_ERR_NONE;

#if (Q_ERROR_CHECKING == TRUE)
   /* Make sure the requested queue size is not too small */
   if (max_queue_size < Q_MIN_SIZE)
   {
      *status_ptr = Q_ERR_INVALID_SIZE;
   }
#endif

   if (*status_ptr == Q_ERR_NONE)
   {
      /*
       * If a memory region was supplied by the caller, store its address in
       * the memory referenced by the "queue_id_ptr" input parameter.
       */
      if (queue_memory_ptr != NULL)
      {
         queue_ptr = (BYTE_PTR) queue_memory_ptr;
      }
      /*
       * Otherwise, try to allocate the memory needed for a  queue of the
       * requested size, and store its address.
       */
      else
      {
#if (Q_ALIGN_CHECKING == FALSE)
         /* Allocate descriptor */

         queue_ptr = (BYTE_PTR) FDI_MALLOC(sizeof(Q_DESCRIPTOR));
#else
         /*
          * Add 3 to the MALLOC size to ensure that we will be able to align
          * our data regardless of where the memory is allocated from.  If the
          * memory is not available, return an error.
          */

         queue_ptr = (BYTE_PTR) FDI_MALLOC(sizeof(Q_DESCRIPTOR) + 3);
#endif
         /* If the memory is not available, return an error */
         if (queue_ptr == NULL)
         {
            *status_ptr = Q_ERR_NO_MEMORY;
            return (Q_ID) queue_ptr;
         }
      }                                /* ENDELSE queue_memory == NULL */

#if (Q_ALIGN_CHECKING == TRUE)
      /*
       * Make sure that the queue memory is DWORD aligned.  The number of
       * bytes that the memory pointer is advanced will be stored in
       * alignment_size.
       */
      alignment_size = (WORD) ((4 - (DWORD) queue_ptr) & 3);
      queue_ptr += alignment_size;
      /*
       * If the memory was provided externally, subract the shift count  from
       * the total known size of the provided memory region.
       */
      if (queue_memory_ptr != NULL)
      {
         max_queue_size -= alignment_size;
      }
#endif

      max_queue_size &= Q_DWORD_MASK;
      /*
       * keep track of the free space in the descriptor table with the field
       * free_count
       */
/* Queue align checking was not originally used to adjust free count here */
#if (Q_ALIGN_CHECKING == FALSE)
      ((Q_DESC_PTR) queue_ptr)->free_count = max_queue_size -
       sizeof(Q_DESCRIPTOR);
#else
      ((Q_DESC_PTR) queue_ptr)->free_count = max_queue_size -
       (sizeof(Q_DESCRIPTOR) +3);
#endif
      /*
       * Indicate whether the memory was allocated or provided by the caller,
       * and how far the pointer was shifted for alignment.
       */
      if (queue_memory_ptr != NULL)
      {
         ((Q_DESC_PTR) queue_ptr)->alignment_shift = 0;
      }
#if (Q_ALIGN_CHECKING == TRUE)
      else
      {
         ((Q_DESC_PTR) queue_ptr)->alignment_shift = (BYTE) alignment_size;
      }
#endif

#if (Q_VERIFICATION == TRUE)
      /* Set up the validation entries in the queue descriptor */
      ((Q_DESC_PTR) queue_ptr)->queue_id = WORD_LOW((DWORD) queue_ptr);
      ((Q_DESC_PTR) queue_ptr)->queue_signature = WORD_LOW(~(DWORD) queue_ptr);
#endif

#if(SEM_CREATE_DESTROY == TRUE)
      /* create a binary semaphore for the sem_cntrl_sync field of queue_ptr */
      ((Q_DESC_PTR) queue_ptr)->sem_cntrl_sync = SEM_BIN_CREATE();
#else /* SEM_CREATE_DESTROY */
      ((Q_DESC_PTR) queue_ptr)->sem_cntrl_sync = SEM_cntrl_sync;
#endif /* SEM_CREATE_DESTROY */

      if (((Q_DESC_PTR) queue_ptr)->sem_cntrl_sync == NULL)
      {
         *status_ptr = Q_ERR_SEMAPHORE;

#if (Q_ALIGN_CHECKING == TRUE)
         alignment_size = ((Q_DESC_PTR) queue_ptr)->alignment_shift;
         queue_ptr -= alignment_size;
#endif
         FDI_FREE(queue_ptr);

         /* Set the queue pointer to NULL and return */
         queue_ptr = NULL;

         /* Return NULL as status */
         return (Q_ID) queue_ptr;
      }

#if(SEM_CREATE_DESTROY == TRUE)
      /* create a mutex semaphore for the sem_cntrl_mutex field of queue_ptr */
      ((Q_DESC_PTR) queue_ptr)->sem_cntrl_mutex = SEM_MTX_CREATE();
#else /* SEM_CREATE_DESTROY */
      /* set the sem_cntrl_mutex field equal to previously created mutex */
      ((Q_DESC_PTR) queue_ptr)->sem_cntrl_mutex = SEM_cntrl_mutex;
#endif /* SEM_CREATE_DESTROY */

      if (((Q_DESC_PTR) queue_ptr)->sem_cntrl_mutex == NULL)
      {
         *status_ptr = Q_ERR_SEMAPHORE;
#if(SEM_CREATE_DESTROY == TRUE)
         /* destroy the binary semaphore just created */
         SEM_DESTROY(((Q_DESC_PTR) queue_ptr)->sem_cntrl_sync);
#endif /* SEM_CREATE_DESTROY */

         /* free the queue space allocated */

#if (Q_ALIGN_CHECKING == TRUE)
         alignment_size = ((Q_DESC_PTR) queue_ptr)->alignment_shift;
         queue_ptr -= alignment_size;
#endif
         FDI_FREE(queue_ptr);

         /* Set the queue pointer to NULL and return */
         queue_ptr = NULL;
      }

      /* assign the first_header_ptr field of queue_ptr to NULL */
      /* DM Lint */
      if (queue_ptr != NULL)
      {
         ((Q_DESC_PTR) queue_ptr)->first_header_ptr = NULL;
         ((Q_DESC_PTR) queue_ptr)->number_items = 0;  /* no items in queue */
      }
      /* DM Lint */

   }                                   /* ENDIF *status_ptr == Q_ERR_NONE */
   return (Q_ID) queue_ptr;

}                                      /* END Q_Create */


/*############################################################################
 *### Q_Add
 *###
 *### DESCRIPTION:
 *### Q_Add inserts two data objects into a data queue.  A check is made to
 *### see if the "queue_id" input is valid and if the objects fit in the
 *### predefined sized data queue.  Q_Add allocates memory for the item
 *### information structure and each data object.  The item information
 *### structure (Q_ITEM_INFO) is a queue structure that controls the objects
 *### in the queue.  The item information structure, "data1_ptr" info and
 *### "data2_ptr" info are copied into the new item structure buffer.
 *### The queue is locked with the sem_cntrl_mutex semaphore and the object
 *### count in the queue is incremented.  The free count in the queue is
 *### decremented by the size of the new item structure created.  Q_Add calls
 *### FindItem to locate:
 *###    a) the last item of the same priority
 *### or b) the priority header before the item's priority header
 *### or c) the last priority header in the queue
 *### or d) the descriptor structure because there are no priority headers
 *### If the priority does not exist the priority header is allocated and
 *### the data object is attached as the first item pointer.  The new
 *### priority header either points to the next header or NULL and is
 *### connected to the previous header or descriptor structure.
 *### If the object is a item then the new item object is attached to the
 *### returned object's next item pointer.
 *### The sem_cntrl_sync semaphore is asserted to acknowledge to the
 *### pending back ground task that an item is in the queue.  The
 *### sem_cntrl_mutex semaphore is unlocked to allow others to access
 *### the queue.
 *###
 *### PARAMETERS:
 *###    IN:
 *###       queue_id    -  add item to the queue with this handle
 *###       data1_ptr   -  address of the first data item to add into queue
 *###       data2_ptr   -  address of the second data item to add into queue
 *###       data1_size  -  size of first data item to add into queue
 *###       data2_size  -  size of second data item to add into queue
 *###       priority    -  priority level to add item into queue
 *###       append_to_replace - flag; if true indicates the append is within
 *###                           an existing granularity, similar to replace
 *###    OUT:
 *###
 *### RETURNS:
 *###    Returns the following errors codes:
 *###       Q_ERR_NONE
 *###       Q_ERR_NO_SPACE
 *###       Q_ERR_INVALID_HANDLE
 *###       Q_ERR_NO_MEMORY
 *###*/
Q_ERROR
Q_Add(Q_ID queue_id,
      const VOID * data1_ptr,
      const VOID * data2_ptr,
      WORD data1_size,
      WORD data2_size,
      BYTE priority,
      BYTE append_to_replace)
{
   Q_HDR_PTR header_ptr;               /* temporary storage for priority */
   Q_ITEM_PTR item_ptr;                /* temporary storage for item */
   VOID_PTR data_ptr = NULL;           /* header or item ptr */
   register Q_DESC_PTR queue_ptr = (Q_DESC_PTR) queue_id;   /* ptr to queue */
   union uwords
   {
      WORD of_data_item;               /* keeps track of data item size */
      WORD of_item_info_stuff;         /* the item info package */
   }  usize;
   Q_ERROR status = Q_ERR_NONE;        /* returned status */

   WORD free_count;

#if (Q_VERIFICATION == TRUE)

   if (VALID_QUEUE_PTR(queue_ptr) == FALSE)  /* IF call ValidQueuePtr fails */
   {
      return Q_ERR_INVALID_HANDLE;
   }

#endif

   /*
    * IF the sub_command is WRITE_RESERVE do not allocate the
    * data2_size space in the queue memory
    */
#if (PACKET_DATA == TRUE)
   if (((COMMAND_PTR)data1_ptr)->sub_command == WRITE_RESERVED ||
        ((COMMAND_PTR)data1_ptr)->sub_command == WRITE_RSRVPCKT )
#else /* PACKET_DATA */
   if (((COMMAND_PTR)data1_ptr)->sub_command == WRITE_RESERVED)
#endif /* PACKET_DATA */
   {
      usize.of_item_info_stuff = data1_size + sizeof(Q_ITEM_INFO);
   }
   else