ext_convert.c

simulink real-time workshop for dragon12 development board from

/*
 * Copyright 1994-2001 The MathWorks, Inc.
 *
 * File: ext_conv.c     $Revision: 1.13 $
 *
 * Abstract:
 * Utility functions for ext_comm.c.
 */

#include <string.h>

#include "tmwtypes.h"
#include "mex.h"
#include "extsim.h"
#include "extutil.h"


#define DEBUG_MSG_LVL 0



/* Function: slCopyTwoBytes ====================================================
 * Abstract:
 *  Copy two byte elements from src to dst, reversing byte ordering if
 *  requested (e.g., 01 becomes 10).  Handles both aligned and unaligned
 *  sources and destinations.  If src and dst are the same, the function
 *  performs "in place" byte swapping.
 *
 *  ??? This function seems a bit over complicated.
 */
static void slCopyTwoBytes(
    void       *y,        /* Pointer to start of dst   */
    const void *x,	  /* Pointer to start of src   */
    const int  n,         /* Number elements in vector */
    boolean_T  swapBytes)
{
#define MAX_ELS (256)
    if (!swapBytes) {
        if (x != y) {
            (void)memcpy(y, x, n*2);
        } else {
            return;
        }
    } else {
        if (IS_ALIGNED(x,2) && IS_ALIGNED(y,2)) {
            char       *yChar        = (char *)y;
            const char *xChar        = (char *)x;
            boolean_T  xCharAligned4 = IS_ALIGNED(xChar,4);
            boolean_T  yCharAligned4 = IS_ALIGNED(yChar,4);

            /*
             * See if we can optimize by doing two swaps at a time
             * within a 4 byte container.  To do this, the src and dst
             * (x and y) arrays must have the same aligment wrt a 4 byte
             * boundary.
             */
            if ((xCharAligned4 == yCharAligned4) && (n > 1)) {
                int tmpN = n;

                /*
                 * Swap the first 2 bytes if address is not aligned on 4
                 * byte boundary.
                 */
                if (!xCharAligned4) {
                    uint16_T *p16Src = (uint16_T *)xChar;
                    uint16_T *p16Dst = (uint16_T *)yChar;

                    *p16Dst = (uint16_T)
                        ((((*p16Src) >> 8) & 0x00FF) | 
                         (((*p16Src) << 8) & 0xFF00));

                    xChar += 2;
                    yChar += 2;
                    tmpN--;
                }

                assert(IS_ALIGNED(xChar,4));
                assert(IS_ALIGNED(yChar,4));

                /* Swap 4-bytes at a time */
                if ((tmpN-2) >= 0) {
	            uint32_T *p32Src = (uint32_T *)xChar;
	            uint32_T *p32Dst = (uint32_T *)yChar;
	            uint32_T mask1   = 0x00FF00FF;
	            uint32_T mask2   = 0xFF00FF00;

                    while ((tmpN-2) >= 0) {
                        *p32Dst = (((*p32Src) >> 8) & mask1) |
                                  (((*p32Src) << 8) & mask2);
                        p32Src++;
                        p32Dst++;
                        tmpN -= 2;
                    }

                    xChar = (char *)p32Src;
                    yChar = (char *)p32Dst;
                }

                assert(tmpN >= 0);

                /*
                 * Take care of last 2 bytes if needed.
                 */
                if (tmpN > 0) {
                    uint16_T *p16Src = (uint16_T *)xChar;
                    uint16_T *p16Dst = (uint16_T *)yChar;
                    
                    assert(tmpN == 1);

                    *p16Dst = (uint16_T)
                        ((((*p16Src) >> 8) & 0x00FF) | 
                         (((*p16Src) << 8) & 0xFF00));

                    tmpN--;
                }

                assert(tmpN == 0);
            } else {
                /*
                 * Not aligned for 4 bytes operations, so do a series of
                 * 2 byte operations.
                 */
                int      i;
                uint16_T *p16Src = (uint16_T *)xChar;
                uint16_T *p16Dst = (uint16_T *)yChar;

                for (i=0; i<n; i++) {
                    *p16Dst = (uint16_T)
                        ((((*p16Src) >> 8) & 0x00FF) | 
                         (((*p16Src) << 8) & 0xFF00));

                    p16Src++;
                    p16Dst++;
                }

            }
        } else {
            /* account for mis-alignment */
            int count = 0;
            char *src = (char *)x;
            char *dst = (char *)y;
        
            while(count < n) {
                UINT16_T tmp[MAX_ELS];
                int      nLeft   = n - count;
                int      nEls    = (MAX_ELS >= nLeft) ? nLeft : MAX_ELS;
                int      nBytes  = nEls * 2;

                /* get the elements properly aligned */
                (void)memcpy(tmp, src, nBytes);
            
                /* swap them (in place) */
                slCopyTwoBytes(tmp, tmp, nEls, swapBytes);
                
                /* copy to destination */
                (void)memcpy(dst, tmp, nBytes);

                src   += nBytes;
                dst   += nBytes;
                count += nEls;
            }
        }
    }
#undef MAX_ELS
} /* end slCopyTwoBytes */


/* Function: slCopyFourBytes ===================================================
 * Abstract:
 *  Copy four byte elements from src to dst, reversing byte ordering if
 *  requested (e.g., 0123 becomes 3210).  Handles both aligned and unaligned
 *  sources and destinations.  If src and dst are the same, the function
 *  performs "in place" byte swapping.
 */
static void slCopyFourBytes(
    void       *y,        /* Pointer to start of dst   */
    const void *x,	  /* Pointer to start of src   */
    const int  n,         /* Number elements in vector */
    boolean_T  swapBytes)
{
#define MAX_ELS (256)

    if (!swapBytes) {
        if (x != y) {
            (void)memcpy(y, x, n*4);
        } else {
            return;
        }
    } else {
        int i;
    
        if (IS_ALIGNED(x,4) && IS_ALIGNED(y,4)) {
            UINT32_T *src = (UINT32_T *)x;
            UINT32_T *dst = (UINT32_T *)y;
            for (i=0; i<n; i++, src++, dst++) {
                *dst =  ((*src >> 24) & 0xff)    |
                        ((*src >> 8) & 0xff00)   |
		        ((*src << 8) & 0xff0000) |
                        (*src << 24);
            }
        } else {
            /* account for mis-alignment */
            int count = 0;
            char *src = (char *)x;
            char *dst = (char *)y;
        
            while(count < n) {
                UINT32_T tmp[MAX_ELS];
                UINT32_T *tmpPtr = tmp;
                int      nLeft   = n - count;
                int      nEls    = (MAX_ELS >= nLeft) ? nLeft : MAX_ELS;
                int      nBytes  = nEls * 4;

                /* get the elements properly aligned */
                (void)memcpy(tmp, src, nBytes);
            
                /* swap them (in place) */
                for (i=0; i<nEls; i++, tmpPtr++) {
                    *tmpPtr =  ((*tmpPtr >> 24) & 0xff)    |
                               ((*tmpPtr >> 8) & 0xff00)   |
		               ((*tmpPtr << 8) & 0xff0000) |
                               (*tmpPtr << 24);
                }

                /* copy to destination */
                (void)memcpy(dst, tmp, nBytes);

                src   += nBytes;
                dst   += nBytes;
                count += nEls;
            }
        }
    }
#undef MAX_ELS
} /* end slCopyFourBytes */


/* Function: slCopyEightBytes ==================================================
 * Abstract:
 *  Copy eight byte elements from src to dst, reversing byte ordering if
 *  requested (e.g., 01234567 becomes 76543210).  Handles both aligned
 *  and unaligned sources and destinations.  If src and dst are the same, the
 *  function performs "in place" byte swapping.
 */
static void slCopyEightBytes(
    void       *y,       /* Pointer to start of dst   */
    const void *x,	 /* Pointer to start of src   */
    const int  n,        /* Number elements in vector */
    bool       swapBytes)
{
#define MAX_ELS (256)

    if (!swapBytes) {
        if (x != y) {
            (void)memcpy(y, x, n*8);
        } else {
            return;
        }
    } else {
        int i;

        if (IS_ALIGNED(x,8) && IS_ALIGNED(y,8)) {
            UINT32_T *src = (UINT32_T *)x;
            UINT32_T *dst = (UINT32_T *)y;
            for (i=0; i<n; i++, src += 2) {
                UINT32_T r1 = *src;
                UINT32_T r2 = *(src+1);

                *dst =  ((r2 >> 24) & 0xff)    |
                        ((r2 >> 8) & 0xff00)   |
		        ((r2 << 8) & 0xff0000) |
                        (r2 << 24);
                dst++;

                *dst =  ((r1 >> 24) & 0xff)     |
                        ((r1 >> 8)  & 0xff00)   |
		        ((r1 << 8)  & 0xff0000) |
                        (r1 << 24);
                dst++;
            }
        } else {
            /* account for mis-alignment */
            int count = 0;
            char *src = (char *)x;
            char *dst = (char *)y;
        
            while(count < n) {
                UINT32_T tmp[MAX_ELS*2];
                UINT32_T *tmpPtr = tmp;
                int      nLeft   = n - count;
                int      nEls    = (MAX_ELS >= nLeft) ? nLeft : MAX_ELS;
                int      nBytes  = nEls * 8;

                /* get the elements properly aligned */
                (void)memcpy(tmp, src, nBytes);
            
                /* swap them (in place) */
                for (i=0; i<nEls; i++) {
                    UINT32_T r1 = *tmpPtr;
                    UINT32_T r2 = *(tmpPtr+1);

                    *tmpPtr = ((r2 >> 24) & 0xff)     |
                              ((r2 >> 8)  & 0xff00)   |
		              ((r2 << 8)  & 0xff0000) |
                              (r2 << 24);
                    tmpPtr++;

                    *tmpPtr = ((r1 >> 24) & 0xff)     |
                              ((r1 >> 8)  & 0xff00)   |
		              ((r1 << 8)  & 0xff0000) |
                              (r1 << 24);
                    tmpPtr++;
                }

                /* copy to destination */
                (void)memcpy(dst, tmp, nBytes);

                src   += nBytes;
                dst   += nBytes;
                count += nEls;
            }
        }
    }
#undef MAX_ELS
} /* end slCopyEightBytes */


/* Function: slCopyNBytes ======================================================
 * Abstract:
 *  Well, not really copy N bytes, although having an optimized N byte 
 *  copier/swapper would be a really good idea.  Currently, this function
 *  is just a dispatcher for pre-existing byte swapper/copier functions
 *  (2, 4, 8 byte swaps).
 */
/* changed from 'static' to 'extern' (needied in ext_transport_232.c)  --  fw-02-05 */
//static void slCopyNBytes(
void slCopyNBytes(
    void       *y,        /* Pointer to start of dst   */
    const void *x,	  /* Pointer to start of src   */
    const int  n,         /* Number elements in vector */
    boolean_T  swapBytes, /* true to swap              */
    const int  elSize)    /* size of elements in array */
{
    switch(elSize) {

    case 1:
        (void)memcpy(y, x, n);
        break;

    case 2:
        slCopyTwoBytes(y, x, n, swapBytes);
        break;

    case 4:
        slCopyFourBytes(y, x, n, swapBytes);
        break;

    case 8:
        slCopyEightBytes(y, x, n, swapBytes);
        break;
    
    default:
        /*
         * This implementation of ext_convert only supplies byte swapping
         * routines for arrays of 2, 4 and 8 byte elements.
         */
        assert(FALSE);
        break;
    } /* end switch */
} /* end slCopyNBytes */


/* Function: Double_HostToTarget ===============================================
 * Abstract:
 *  Convert Simulink (hosts) double value (8 bytes) to target real_T value.
 *  No assumptions may be made about the alignment of the dst ptr.
 *  The src pointer is aligned for type double.  As implemented, this function
 *  supports only 32 and 64 bit target real values (ieee).
 */
static void Double_HostToTarget(
    ExternalSim   *ES,
    char          *dst,
    const void    *voidSrc,
    const int     n,
    const int     dType) /* internal Simulink data type id */
{
    boolean_T swapBytes = esGetSwapBytes(ES);
    double    *src      = (double *)voidSrc;

    int sizeofTargetDouble = esGetSizeOfTargetDataTypeFcn(ES)(ES, dType) *
                                esGetHostBytesPerTargetByte(ES);

    switch(sizeofTargetDouble) {
    case 4:
    {
        int  i;
        char *dstPtr = dst;

        for (i=0; i<n; i++) {
	    float tmpFloat = (float)src[i];
            (void)memcpy(dstPtr, &tmpFloat, 4);
            dstPtr += 4;
        }