debugos.c

AT91所有开发板的资料 AT91所有开发板的资料

 *         These rules are fairly simple to understand and include the
 *         above requirements as a subset.
 *         
 *         Finally, the code must protect Angel from overwriting itself,
 *         and may thus refuse the request with a privilegde error if
 *         the requested address is in a protected region.
 *         
 *         This routine uses the volatile variables 
 *            memory_is_being_accessed   - flag to data abort handler
 *            memory_access_aborted      - count of data aborts caught
 *         
 *         The data abort handler is expected, when memory_is_being_accessed
 *         is non-zero, to increment the count of aborts found in
 *         memory_access_aborted whenever an abort occurs, returning to the
 *         *next* instruction. That is, this code requires that if data aborts
 *         are retried then when the handler returns the attempt *will*
 *         succeed; in this case, the aborted count should *not* be
 *         incremented.
 *         
 *    Params: 
 *       Input:
 *              OsInfo[12]  - unused; would be used for thread context etc
 *              Address     - the address in the target to write the data
 *              Nbytes      - the number of bytes to write
 *              DataArea    - where to get the data from
 *              
 *      Output: 
 *              BytesWritten- return of how many bytes were transferred
 *
 *   Returns: Error code: OK if memort transferred, DataAbort or Priviledge
 *                        error otherwise.
 */
int
angelOS_MemWrite(word OSinfo1, word OSinfo2, word Address, word Nbytes,
                 byte * DataArea, word * BytesWritten)
{
    int all_permitted;

    IGNORE(OSinfo1);
    IGNORE(OSinfo2);

    /*
     * If we detect that either the start or end of the area is not
     * a permitted write area then give up immediately.
     * Otherwise transfer the whole lot a byte (or word) at a time.
     * This is preferable to transferring words due to a number of
     * problesm: endianess and non alignment of data in target memory
     * and/or the datablock.
     */

    all_permitted = (WRITE_PERMITTED(Address) &&
                     WRITE_PERMITTED(Address + Nbytes - 1));

    if (!all_permitted)
    {
        *BytesWritten = 0;
        return RDIError_InsufficientPrivilege;
    }

    memory_is_being_accessed = 1;
    memory_access_aborted = 0;

    /*
     * use halfword copy if addresses are halfword aligned but not
     * word aligned and number of bytes is not odd
     *
     * use word copy if addresses are word aligned and copying integral number
     * of words
     *      *(((char *)Address) + c) = DataArea[c];
     *
     * otherwise, copy byte-by-byte.
     *
     */
    if ((((int)DataArea | (int)Address | Nbytes) & 0x3) == 0)
    {
        int *swAddress = (int*)DataArea;
        int *dwAddress = (int*)Address;
        word Nwords = Nbytes >> 2;
        
        while (Nwords >= 4)
        {
            *dwAddress++ = *swAddress++;
            *dwAddress++ = *swAddress++;
            *dwAddress++ = *swAddress++;
            *dwAddress++ = *swAddress++;
            Nwords -= 4;
            /* check to see access was ok */
            if (memory_access_aborted)
                break;
        }
        
        switch(Nwords)
        {
            case 3:
                *dwAddress++ = *swAddress++;
                /*fallthrough*/
            case 2:
                *dwAddress++ = *swAddress++;
                /*fallthrough*/
            case 1:
                *dwAddress++ = *swAddress++;
                break;
                
            default:
                /* could have 0 or > 3 words here... either way, do nothing */
                break;
        }
        dwAddress -= memory_access_aborted;
        *BytesWritten = (int)dwAddress - (int)Address;
    }
    else if ((((int)DataArea | (int)Address | Nbytes) & 0x1) == 0)
    {
        short *ssAddress = (short *)DataArea;
        short *dsAddress = (short *)Address;
        word Nshorts = Nbytes >> 1;

        while (Nshorts >= 4)
        {
            *dsAddress++ = *ssAddress++;
            *dsAddress++ = *ssAddress++;
            *dsAddress++ = *ssAddress++;
            *dsAddress++ = *ssAddress++;
            Nshorts -= 4;
            /* check to see access was ok */
            if (memory_access_aborted)
                break;
        }
        switch(Nshorts)
        {
            case 3:
                *dsAddress++ = *ssAddress++;
                /*fallthrough*/
            case 2:
                *dsAddress++ = *ssAddress++;
                /*fallthrough*/
            case 1:
                *dsAddress++ = *ssAddress++;
                break;
                
            default:
                /* could have 0 or > 3 halfwords here... either way, do nothing */
                break;
        }
        dsAddress -= memory_access_aborted;
        *BytesWritten = (int)dsAddress - (int)Address;
    }
    else
    {
        char *sbAddress = (char *)DataArea;
        char *dbAddress = (char *)Address;

        while (Nbytes >= 4)
        {
            *dbAddress++ = *sbAddress++;
            *dbAddress++ = *sbAddress++;
            *dbAddress++ = *sbAddress++;
            *dbAddress++ = *sbAddress++;
            Nbytes -= 4;
            /* check to see access was ok */
            if (memory_access_aborted)
                break;
        }
        switch(Nbytes)
        {
            case 3:
                *dbAddress++ = *sbAddress++;
                /*fallthrough*/
            case 2:
                *dbAddress++ = *sbAddress++;
                /*fallthrough*/
            case 1:
                *dbAddress++ = *sbAddress++;
                break;
                
            default:
                /* could have 0 or > 3 bytes here... either way, do nothing */
                break;
        }
        dbAddress -= memory_access_aborted;
        *BytesWritten = (int)dbAddress - (int)Address;
    }

    memory_is_being_accessed = 0;


#if CACHE_SUPPORTED
    Cache_IBR((char *)Address, (char *)(Address + Nbytes - 1));
#endif

    return memory_access_aborted ? RDIError_DataAbort : RDIError_NoError;
}


/*
 *  Function: cpuread
 *   Purpose: Convenient function to read the next word from a byte buffer and
 *            then increment a counter appropriately
 *
 *    Params: 
 *       Input: 
 *
 *   Returns: 
 */

static word
cpuread(byte * wd, int *c)
{
    word w;

    w = GET32LE(wd + *c);
    *c += 4;
    return (w);
}


/*
 *  Function: angelOS_MemWrite
 *   Purpose: 
 *
 *    Params: 
 *       Input: 
 *
 *   Returns: 
 */
int
angelOS_CPUWrite(word OSinfo1, word OSinfo2, word Mode, word Mask,
                 byte * WriteData)
{
    int i, c = 0;
    angel_RegBlock *rb;
    unsigned *r, cpsr;

    IGNORE(OSinfo1);
    IGNORE(OSinfo2);

    LogInfo(LOG_DEBUGOS, ( "angelOS_CPUWrite: Mode = 0x%x, Mask = 0x%x.\n", Mode, Mask));

    rb = Angel_AccessApplicationRegBlock();

    if (rb == NULL)
        return RDIError_Error;

    cpsr = rb->cpsr;
    
    if (Mode == ADP_CPUmode_Current)
        Mode = (cpsr & ModeMask);

    if (Mode == SYS32mode)
        Mode = USR32mode;

    if ((Mode & ModeMask) != Mode)
    {
        LogWarning(LOG_DEBUGOS, ( "ERROR: Unrecognised mode in angelOS_CPUWrite.\n"));
        return RDIError_BadCPUStateSetting;
    }

    /*
     * requested mode must match application mode  - if not then refuse
     * to do it.
     */
    if (Mode != (cpsr & ModeMask))
        return RDIError_BadCPUStateSetting;

    if ((Mask & 0x7FFFF) != Mask)
    {
        LogWarning(LOG_DEBUGOS, ( "ERROR: Mask invalid in angelOS_CPUWrite.\n"));
        return RDIError_Error;
    }

    /* deal first with r0 */
    if (Mask & 1L)
        rb->r0 = cpuread(WriteData, &c);

    /* now r1 -- r7 */
    r = &rb->r1;
    for (i = 1; i < 8; i++, r++)
        if ((1L << i) & Mask)
            *r = cpuread(WriteData, &c);

    /* now r8 - r12, banked FIQ / NotFIQ */
    if ((cpsr & ModeMask) == FIQmode)
        r = &rb->r8fiq;
    else
        r = &rb->r8usr;
    
    for (; i < 13; i++, r++)
        if ((1L << i) & Mask)
            *r = cpuread(WriteData, &c);

    /* finally, r13, r14 */
    if (Mask & (1L << 13))
        *Angel_AdrBankedReg(rb, Mode, 13) = cpuread(WriteData, &c);
    
    if (Mask & (1L << 14))
        *Angel_AdrBankedReg(rb, Mode, 14) = cpuread(WriteData, &c);

    /* now r15 -- pc */
    if (ADP_CPUread_PCmode & Mask)
        rb->pc = cpuread(WriteData, &c);

    if (ADP_CPUread_PCnomode & Mask)
        rb->pc = cpuread(WriteData, &c);

    if (ADP_CPUread_CPSR & Mask)
        rb->cpsr = cpuread(WriteData, &c);

    /* banked register 15 is the SPSR for the mode */
    if (Mode != USR32mode) /* and Mode != SYS32mode -- see above */
        if (ADP_CPUread_SPSR & Mask)
            *Angel_AdrBankedReg(rb, Mode, 15) = cpuread(WriteData, &c);

    return RDIError_NoError;
}

/*
 *  Function: cpuwrite
 *   Purpose: 
 *            Convenient function to write the next word to a byte buffer and
 *            then increment a counter appropriately
 *
 *    Params: 
 *       Input: 
 *
 *   Returns: 
 */
static void
cpuwrite(word w, byte * wd, int *c)
{
    PUT32LE(wd + *c, w);
    *c += 4;
}

/*
 *  Function: angelOS_CPURead
 *   Purpose: 
 *
 *    Params: 
 *       Input: 
 *
 *   Returns: 
 */
int
angelOS_CPURead(word OSinfo1, word OSinfo2, word Mode, word Mask,
                byte * ReadData)
{
    int i, c = 0;
    angel_RegBlock *rb;
    unsigned *r, cpsr;

    IGNORE(OSinfo1);
    IGNORE(OSinfo2);

    LogInfo(LOG_DEBUGOS, ( "angelOS_CPURead: Mode = 0x%x, Mask = 0x%x.\n", Mode, Mask));

    rb = Angel_AccessApplicationRegBlock();

    if (rb == NULL)
        return RDIError_Error;

    cpsr = rb->cpsr;
    
    if (Mode == ADP_CPUmode_Current)
        Mode = (rb->cpsr & ModeMask);

    if (Mode == SYS32mode)
        Mode = USR32mode;

    /* If we have made a request for a 26 bit mode, change it to the
     * 32 bit equivalent mode - this allows such requests to work
     * properly on the StrongARM.
     */
    Mode |= 0x10;

    if ((Mode & ModeMask) != Mode)
    {
        LogWarning(LOG_DEBUGOS, ( "ERROR: Unrecognised mode in angelOS_CPURead.\n"));
        return RDIError_BadCPUStateSetting;
    }

    if ((Mask & 0x7FFFF) != Mask)
    {
        LogWarning(LOG_DEBUGOS, ( "ERROR: Mask invalid in angelOS_CPURead.\n"));
        return RDIError_Error;
    }


    /* deal first with r0 */
    if (Mask & 1L)
        cpuwrite(rb->r0, ReadData, &c);

    /* now r1 -- r7 */
    r = &rb->r1;
    for (i = 1; i < 8; i++, r++)
        if ((1L << i) & Mask)
            cpuwrite(*r, ReadData, &c);

    /* now r8 - r12, banked FIQ / NotFIQ */
    if ((cpsr & ModeMask) == FIQmode)
        r = &rb->r8fiq;
    else
        r = &rb->r8usr;
    
    for (; i < 13; i++, r++)
        if ((1L << i) & Mask)
            cpuwrite(*r, ReadData, &c);

    /* finally, r13, r14 */
    if (Mask & (1L << 13))
        cpuwrite(Angel_GetBankedReg(rb, Mode, 13), ReadData, &c);
    
    if (Mask & (1L << 14))
        cpuwrite(Angel_GetBankedReg(rb, Mode, 14), ReadData, &c);

    /* now r15 -- pc */
    if (ADP_CPUread_PCmode & Mask)
        cpuwrite(rb->pc, ReadData, &c);

    if (ADP_CPUread_PCnomode & Mask)
    {
        if (!(cpsr) & (1 << 4))
        {
            /* if in 26 bit mode */
            cpuwrite(rb->pc & 0x0FFFFFFb, ReadData, &c);
        }
        else
        {
            /* 32 bit, return full PC */
            cpuwrite(rb->pc, ReadData, &c);
        }
    }
    
    if (ADP_CPUread_CPSR & Mask)
        cpuwrite(rb->cpsr, ReadData, &c);

    /* banked register 15 is the SPSR for the mode */