memblk.c

linux环境支持psos的操作系统。非常适合进行移植的朋友。

/*****************************************************************************
 * memblk.c - defines the wrapper functions and data structures needed
 *            to implement a Wind River pSOS+ (R) partition API 
 *            in a POSIX Threads environment.
 ****************************************************************************/

#include <errno.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <signal.h>
#include <sys/time.h>
#include "p2pthread.h"

#undef DIAG_PRINTFS

#define PT_DEL       0x04

#define ERR_TIMEOUT  0x01
#define ERR_NODENO   0x04
#define ERR_OBJDEL   0x05
#define ERR_OBJTFULL 0x08
#define ERR_OBJNF    0x09

#define ERR_BUFSIZE  0x29
#define ERR_BUFINUSE 0x2B
#define ERR_NOBUF    0x2C
#define ERR_BUFADDR  0x2D
#define ERR_BUFFREE  0x2F

/*****************************************************************************
**  p2pthread partition extent type - this is the header for a dynamically 
**                                    allocated array of contiguous data blocks
*****************************************************************************/
typedef struct prtn_extent
{
    char *
        baddr;           /* Pointer to data block for current extent */
   ULONG 
        bcount;
} prtn_extent_t;

/*****************************************************************************
**  Control block for p2pthread memory partition
**
*****************************************************************************/
typedef struct p2pt_partition
{
        /*
        ** ID for partition
        */
    ULONG
        prtn_id;

        /*
        ** Partition Name
        */
    char
        ptname[4];

        /*
        ** Option Flags for partition
        */
    ULONG
        flags;

        /*
        ** Mutex for partition get/release block
        */
    pthread_mutex_t
        prtn_lock;

        /*
        **  Pointer to first data block in free_list for partition
        */
    char **
        first_free;

        /*
        **  Pointer to last data block in free_list for partition
        */
    char **
        last_free;

        /*
        ** Total number of free data blocks in partition
        */
    ULONG 
        free_blk_count;

        /*
        ** Total number of allocated data blocks in partition
        */
    ULONG
        used_blk_count;

        /*
        ** Number of bytes per allocatable data block
        */
    ULONG 
        blk_size;

        /*
        **  Pointer to data extent allocated for partition
        */
    prtn_extent_t *
        data_extent;

        /*
        **  Pointer to next partition control block in partition list.
        */
    struct p2pt_partition *
        nxt_prtn;

} p2pt_prtn_t;

/*****************************************************************************
**  External function and data references
*****************************************************************************/
extern void *
    ts_malloc( size_t blksize );
extern void 
    ts_free( void *blkaddr );
extern void
   sched_lock( void );
extern void
   sched_unlock( void );
extern p2pthread_cb_t *
   my_tcb( void );

/*****************************************************************************
**  p2pthread Global Data Structures
*****************************************************************************/

/*
**  prtn_list is a linked list of partition control blocks.  It is used to
**            locate partitions by their ID numbers.
*/
static p2pt_prtn_t *
    prtn_list;

/*
**  prtn_list_lock is a mutex used to serialize access to the partition list
*/
static pthread_mutex_t
    prtn_list_lock = PTHREAD_MUTEX_INITIALIZER;


/*****************************************************************************
** pcb_for - returns the address of the partition control block for the
**           partition idenified by prtn_id
*****************************************************************************/
static p2pt_prtn_t *
   pcb_for( int prtn_id )
{
    p2pt_prtn_t *current_pcb;
    int found_prtn_id;

        if ( prtn_list != (p2pt_prtn_t *)NULL )
        {
            /*
            **  One or more partitions already exist in the partition list...
            **  Scan the existing partitions for a matching ID.
            */
            found_prtn_id = FALSE;
            for ( current_pcb = prtn_list; 
                  current_pcb != (p2pt_prtn_t *)NULL;
                  current_pcb = current_pcb->nxt_prtn )
            {
                if ( current_pcb->prtn_id == prtn_id )
                {
                    found_prtn_id = TRUE;
                    break;
                }
            }
            if ( found_prtn_id == FALSE )
                /*
                **  No matching ID found
                */
                current_pcb = (p2pt_prtn_t *)NULL;
        }
        else
            current_pcb = (p2pt_prtn_t *)NULL;
 
    return( current_pcb );
}

/*****************************************************************************
** new_prtn_id - automatically returns a valid, unused partition ID
*****************************************************************************/
static ULONG
   new_prtn_id( void )
{
    p2pt_prtn_t *current_pcb;
    ULONG new_prtn_id;

    /*
    **  Protect the queue list while we examine it.
    */

    pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                          (void *)&prtn_list_lock );
    pthread_mutex_lock( &prtn_list_lock );

    /*
    **  Get the highest previously assigned queue id and add one.
    */
    if ( prtn_list != (p2pt_prtn_t *)NULL )
    {
        /*
        **  One or more queues already exist in the queue list...
        **  Find the highest queue ID number in the existing list.
        */
        new_prtn_id = prtn_list->prtn_id;
        for ( current_pcb = prtn_list; 
              current_pcb->nxt_prtn != (p2pt_prtn_t *)NULL;
              current_pcb = current_pcb->nxt_prtn )
        {
            if ( (current_pcb->nxt_prtn)->prtn_id > new_prtn_id )
            {
                new_prtn_id = (current_pcb->nxt_prtn)->prtn_id;
            }
        }

        /*
        **  Add one to the highest existing queue ID
        */
        new_prtn_id++;
    }
    else
    {
        /*
        **  this is the first queue being added to the queue list.
        */
        new_prtn_id = 1;
    }
 
    /*
    **  Re-enable access to the queue list by other threads.
    */
    pthread_mutex_unlock( &prtn_list_lock );
    pthread_cleanup_pop( 0 );

    return( new_prtn_id );
}

/*****************************************************************************
** link_pcb - appends a new partition control block pointer to the prtn_list
*****************************************************************************/
static void
   link_pcb( p2pt_prtn_t *new_prtn )
{
    p2pt_prtn_t *current_pcb;

    /*
    **  Protect the partition list while we examine and modify it.
    */
    pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                          (void *)&prtn_list_lock );
    pthread_mutex_lock( &prtn_list_lock );

    new_prtn->nxt_prtn = (p2pt_prtn_t *)NULL;
    if ( prtn_list != (p2pt_prtn_t *)NULL )
    {
        /*
        **  One or more partitions already exist in the partition list...
        **  Insert the new entry in ascending numerical sequence by prtn_id.
        */
        for ( current_pcb = prtn_list; 
              current_pcb->nxt_prtn != (p2pt_prtn_t *)NULL;
              current_pcb = current_pcb->nxt_prtn )
        {
            if ( (current_pcb->nxt_prtn)->prtn_id > new_prtn->prtn_id )
            {
                new_prtn->nxt_prtn = current_pcb->nxt_prtn;
                break;
            }
        }
        current_pcb->nxt_prtn = new_prtn;
#ifdef DIAG_PRINTFS 
        printf("\r\nadd partition cb @ %p to list @ %p", new_prtn, current_pcb);
#endif
    }
    else
    {
        /*
        **  this is the first partition being added to the partition list.
        */
        prtn_list = new_prtn;
#ifdef DIAG_PRINTFS 
        printf("\r\nadd partition cb @ %p to list @ %p", new_prtn, &prtn_list);
#endif
    }
 
    /*
    **  Re-enable access to the partition list by other threads.
    */
    pthread_mutex_unlock( &prtn_list_lock );
    pthread_cleanup_pop( 0 );
}

/*****************************************************************************
** unlink_pcb - removes a partition control block pointer from the prtn_list
*****************************************************************************/
static p2pt_prtn_t *
   unlink_pcb( int prtn_id )
{
    p2pt_prtn_t *current_pcb;
    p2pt_prtn_t *selected_pcb;

    selected_pcb =  (p2pt_prtn_t *)NULL;

    if ( prtn_list != (p2pt_prtn_t *)NULL )
    {
        /*
        **  One or more partitions exist in the partition list...
        **  Protect the partition list while we examine and modify it.
        */
        pthread_cleanup_push( (void(*)(void *))pthread_mutex_unlock,
                              (void *)&prtn_list_lock );
        pthread_mutex_lock( &prtn_list_lock );

        /*
        **  Scan the partition list for a pcb with a matching partition ID
        */
        if ( prtn_list->prtn_id == prtn_id )
        {
            /*
            **  The first partition in the list matches the partition ID
            */
            selected_pcb = prtn_list; 
            prtn_list = selected_pcb->nxt_prtn;
#ifdef DIAG_PRINTFS 
            printf( "\r\ndel partition cb @ %p from list @ %p", selected_pcb,
                    &prtn_list );
#endif
        }
        else
        {
            /*
            **  Scan the next pcb for a matching prtn_id while retaining a
            **  pointer to the current pcb.  If the next pcb matches,
            **  select it and then unlink it from the partition list.
            */
            for ( current_pcb = prtn_list; 
                  current_pcb->nxt_prtn != (p2pt_prtn_t *)NULL;
                  current_pcb = current_pcb->nxt_prtn )
            {
                if ( (current_pcb->nxt_prtn)->prtn_id == prtn_id )
                {
                    /*
                    **  Queue ID of next pcb matches...
                    **  Select the pcb and then unlink it by linking
                    **  the selected pcb's next pcb into the current pcb.
                    */
                    selected_pcb = current_pcb->nxt_prtn;
                    current_pcb->nxt_prtn = selected_pcb->nxt_prtn;
#ifdef DIAG_PRINTFS 
                    printf( "\r\ndel partition cb @ %p from list @ %p",
                            selected_pcb, current_pcb );
#endif
                    break;
                }
            }
        }

        /*
        **  Re-enable access to the partition list by other threads.
        */
        pthread_mutex_unlock( &prtn_list_lock );
        pthread_cleanup_pop( 0 );
    }

    return( selected_pcb );
}

/*****************************************************************************
** new_extent_for - allocates space for an extent control block, whioh maps
**                  partition data areas.  Initializes the free block list in
**                  the data block specified and updates the partition control
**                  block to reflect the addition of the new data extent to
**                  the free block and extent lists.
*****************************************************************************/
static prtn_extent_t *
    new_extent_for( p2pt_prtn_t *prtn, char *datablk, unsigned long numblks )
{
    prtn_extent_t *new_extent;
    size_t extent_data_size;
    char *block_ptr;

    /*
    **  The prtn_extent_t contains a pointer to the data block and the
    **  number of bytes in the data block.
    **  Now allocate a block of memory to contain the extent control block.
    */
    if ( (new_extent = (prtn_extent_t *)ts_malloc( sizeof( prtn_extent_t ) )) !=
                                                           (void *)NULL )
    {
        if ( datablk != (char *)NULL )
        {
            /*
            **  Fill in the extent control block itself.
            */
            new_extent->baddr = datablk;
            new_extent->bcount = numblks;

            /*
            **  Clear the data block memory.
            */
            extent_data_size = (size_t)(prtn->blk_size * numblks);
            if ( extent_data_size )
                memset( datablk, 0, extent_data_size );

            /*
            **  Initialize the free block list for the extent.  The free block
            **  list is a forward-linked list of pointers to each of the
            **  unused data blocks in the partition.  The linked list pointers
            **  are kept in the start of the data blocks themselves, and
            **  are overwritten when the blocks are allocated for use.  The
            **  pointer in the last data block is left NULL to terminate
            **  the list.
            */
            for (block_ptr = datablk;
                (block_ptr + prtn->blk_size) < (datablk + extent_data_size);
                 block_ptr += prtn->blk_size )
            {
                /*
                **  Write a pointer to the next data block into the
                **  first few bytes of each data block in the extent.
                */
                *((char **)block_ptr) = (block_ptr + prtn->blk_size);
#ifdef DIAG_PRINTFS 

                printf( "\r\n   add prtn_data_block @ %p nxt_blk @ %p",
                        block_ptr, *((char **)block_ptr) );
#endif
            }
#ifdef DIAG_PRINTFS 
                printf( "\r\n   add prtn_data_block @ %p nxt_blk @ %p",
                        block_ptr, *((char **)block_ptr) );
#endif

            /*
            **  Link the new extent into the partition control block
            */
            prtn->data_extent = new_extent;

            /*
            **  First data block in this extent is first free block
            */
            prtn->first_free = (char **)datablk;

            /*
            **  Last block in new extent is new last_free block
            */
            prtn->last_free = (char **)block_ptr;

            /*
            ** Initialize total number of free data blocks in partition
            */
            prtn->free_blk_count = new_extent->bcount;
        }
        else
        {
            ts_free( (void *)new_extent );
            new_extent = (prtn_extent_t *)NULL;
        }
    }
    return( new_extent );
}

/*****************************************************************************
** pt_create - creates a new memory management area from which fixed-size
**             data blocks may be allocated for applications use.