debugos.c

ARM入门的好帮手.包含了从简单到相对较复杂的程序.

     * to do it.
     */
    if (Mode != (rb->cpsr & ModeMask))
        return RDIError_BadCPUStateSetting;

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

    for (i = 0; i < reg_pc; i++)
        if ((1L << i) & Mask)
            rb->r[i] = cpuread(WriteData, &c);

    if (ADP_CPUread_PCmode & Mask)
        rb->r[reg_pc] = cpuread(WriteData, &c);

    if (ADP_CPUread_PCnomode & Mask)
        rb->r[reg_pc] = cpuread(WriteData, &c);

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

    if (!USRmode && (ADP_CPUread_SPSR & Mask))
        rb->spsr = 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, first_banked;
    angel_RegBlock *rb, *banked_rb;
    angel_RegBlock other_mode_rb;

    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;

    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.
     */
    if ((Mode & 0x10) == 0)
    {
        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;
    }

    /* ALWAYS read the unbanked registers and the PC + CPSR from the
     * Application regblock.  If current mode is the same as that in
     * Appl Regblock then read all registers from there, otherwise we
     * will have to read the banked regs from the relevent mode.
     */
    if (Mode == (rb->cpsr & ModeMask))
    {
        first_banked = 15;
        banked_rb = rb;
    }
    else
    {
        if (Mode == FIQ32mode)
            first_banked = 8;
        else
            first_banked = 13;
        angel_ReadBankedRegs(&other_mode_rb, Mode);
        banked_rb = &other_mode_rb;
    }

    /* Do the unbanked registers */
    for (i = 0; i < first_banked; i++)
    {
        if ((1L << i) & Mask)
            cpuwrite(rb->r[i], ReadData, &c);
    }

    /* Now do banked regs r8-r14 / r13-r14 if necessary */
    for (i = first_banked; i < reg_pc; i++)
    {
        if ((1L << i) & Mask)
            cpuwrite(banked_rb->r[i], ReadData, &c);
    }

    if (ADP_CPUread_PCmode & Mask)
        cpuwrite(rb->r[reg_pc], ReadData, &c);

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

    if (ADP_CPUread_CPSR & Mask)
    {
        cpuwrite(rb->cpsr, ReadData, &c);
    }

    if ((Mode != USR32mode) && (ADP_CPUread_SPSR & Mask))
    {
        cpuwrite(banked_rb->spsr, ReadData, &c);
    }

    return RDIError_NoError;
}

#ifdef SUPPORT_SYSTEM_COPROCESSOR
static int
angelOS_CPSysWriteReg(word regnum, word * Data)
{
    /* This function writes a single system coprocessor register. It
     * does so as defined by the system coprocessor register
     * description.  To do this we must build an instruction and then
     * execute it.
     */
    int i;
    word WriteInstrs[2] =
    {0xEE000F10,                /* MCR p15, 0, r0, c0, c0, 0 */
     0xE1A0F00E                 /* MOV pc,lr (return) */
    };
    void (*WriteFunction) (word);

    /* Find the entry */
    for (i = 0; i < sys_coproc_reg_entries; i++)
    {
        if (regnum >= sys_coproc_regdesc[i].rmin &&
            regnum <= sys_coproc_regdesc[i].rmax)
        {
            word mode;

            /* Found the description - patch up the instruction */
            WriteInstrs[0] |= (sys_coproc_regdesc[i].accessinst.cprt.write_b0
                               & 0xff);
            WriteInstrs[0] |= (sys_coproc_regdesc[i].accessinst.cprt.write_b1
                               & 0xff) << 16;
            if ((sys_coproc_regdesc[i].accessinst.cprt.write_b1 & 0x0f) == 0)
            {
                /* Use the coprocessor reg number from the call in the instruction */
                WriteInstrs[0] |= regnum << 16;
            }
#if CACHE_SUPPORTED
            Cache_IBR(&WriteInstrs[0], &WriteInstrs[1]);
#endif
            WriteFunction = (void (*)(word))WriteInstrs;

            /* mode = SwitchToPriv(); */
            Angel_EnterSVC();

            WriteFunction(*Data);

            /* SwitchToMode(mode); */
            Angel_ExitToUSR();

            return RDIError_NoError;
        }
    }
    /* Not found */
    return RDIError_UnknownCoProState;
}
#endif

int
angelOS_CPWrite(word OSinfo1, word OSinfo2, word CPnum, word Mask,
                word * Data, word * nbytes)
{
#ifdef SUPPORT_SYSTEM_COPROCESSOR
    int regno;
    word status = RDIError_NoError;
    word *dp = Data;

#else
    IGNORE(Mask);
    IGNORE(CPnum);
    IGNORE(Data);
#endif

    IGNORE(OSinfo1);
    IGNORE(OSinfo2);
    IGNORE(nbytes);

#ifdef SUPPORT_SYSTEM_COPROCESSOR
    if (CPnum != 15)
#endif
        return RDIError_UnknownCoPro;

#ifdef SUPPORT_SYSTEM_COPROCESSOR
    for (regno = 0; regno < 32; regno++)
    {
        if ((Mask >> regno) & 0x1)
        {
            status = angelOS_CPSysWriteReg(regno, dp);
            if (status != RDIError_NoError)
            {
                return status;
            }
            dp++;
        }
    }
    return status;
#endif
}

#ifdef SYSTEM_COPROCESSOR_SUPPORTED
  /* This function writes a single system coprocessor register. It
   * does so as defined by the system coprocessor register
   * description.  To do this we must build an instruction and then
   * execute it.
   */
static int
angelOS_CPSysReadReg(word regnum, word * Data)
{
    int i;
    word ReadInstrs[2] =
    {0xEE100F10,                /* MRC p15, 0, r0, c0, c0, 0 */
     0xE1A0F00E                 /* MOV pc,lr (return) */
    };

    word(*ReadFunction) (void);
    /* Find the entry */
    for (i = 0; i < sys_coproc_reg_entries; i++)
    {
        if (regnum >= sys_coproc_regdesc[i].rmin &&
            regnum <= sys_coproc_regdesc[i].rmax)
        {
            word mode;

            /* Found the description - patch up the instruction */
            ReadInstrs[0] |= (sys_coproc_regdesc[i].accessinst.cprt.read_b0
                              & 0xff);
            ReadInstrs[0] |= (sys_coproc_regdesc[i].accessinst.cprt.read_b1
                              & 0xff) << 16;
            if ((sys_coproc_regdesc[i].accessinst.cprt.read_b1 & 0x0f) == 0)
            {
                /* Use the coprocessor reg number from the call in the
                 *  instruction
                 */
                ReadInstrs[0] |= regnum << 16;
            }
#if CACHE_SUPPORTED
            Cache_IBR(&ReadInstrs[0], &ReadInstrs[1]);
#endif
            ReadFunction = (word(*)(void))ReadInstrs;
            mode = SwitchToPriv();
            *Data = ReadFunction();
            SwitchToMode(mode);
            return RDIError_NoError;
        }
        /* Not found */
        return RDIError_UnknownCoProState;
    }
}
#endif

int 
angelOS_CPRead(word OSinfo1, word OSinfo2, word CPnum, word Mask,
               word * Data, word * nbytes)
{
#ifdef SYSTEM_COPROCESSOR_SUPPORTED
    int regno;
    word status = RDIError_NoError;
    word *dp = Data;

#else
    IGNORE(CPnum);
    IGNORE(Mask);
    IGNORE(Data);
#endif

    IGNORE(OSinfo1);
    IGNORE(OSinfo2);

    *nbytes = 0;

#ifdef SYSTEM_COPROCESSOR_SUPPORTED
    if (CPnum != 15)
#endif
        return RDIError_UnknownCoPro;

#ifdef SYSTEM_COPROCESSOR_SUPPORTED
    for (regno = 0; regno < 32; regno++)
    {
        if ((Mask >> regno) & 0x1)
        {
            status = angelOS_CPSysReadReg(regno, dp);
            if (status != RDIError_NoError)
                return status;
            *nbytes += 4;
            dp++;
        }
    }
    return status;
#endif
}

static struct BreakPoint *
getfreebp(void)
{
    int c;
    struct BreakPoint *b;

    LogInfo(LOG_DEBUGOS, ( "getfreebp: free_bps = %x.\n", (word) free_bps));

    for (c = 0; c < 32; c++)
    {
        if ((free_bps & (1 << c)) != 0)
        {
            free_bps &= ~(1 << c);

            LogInfo(LOG_DEBUGOS, ( "Allocating breakpoint %d.\n", c));

            b = &(bp[c]);
            b->bpno = c;
            return (b);
        }
    }

    return NULL;
}

int 
angelOS_SetBreak(word OSinfo1, word OSinfo2,
                 struct SetPointValue *SetData,
                 struct PointValueReturn *ReturnValue)
{
    struct BreakPoint *b = NULL, **pp, **p;
    BreakPointSize size;

    LogInfo(LOG_DEBUGOS, ( "angelOS_SetBreak:\n"));

    size = (SetData->pointType & ADP_SetBreak_Thumb) ? bs_16bit : bs_32bit;

    SetData->pointType &= ~ADP_SetBreak_Thumb;
    if (SetData->pointType != ADP_SetBreak_EqualsAddress)
        return RDIError_UnimplementedType;

    for (p = NULL, pp = &head_bp; (b = *pp) != NULL; p = pp, pp = &b->next)
        if (b->address == SetData->pointAddress)
            break;

    if (b != NULL)
    {
        if (size != b->size)
            return RDIError_ConflictingPoint;

        b->count++;
    }
    else
    {
        word n;
        word status;

        /* We have to be careful with these as they can hold 2 or 4 bytes */
        word data;
        word testRead;

        if (free_bps == 0)
            return RDIError_NoMorePoints;

        b = getfreebp();
        *pp = b;
        b->next = NULL;
        b->count = 1;
        b->size = size;
        b->address = SetData->pointAddress;

        status = angelOS_MemRead(OSinfo1, OSinfo2, b->address,
                                 size == bs_32bit ? 4 : 2,
                                 (byte *) (&b->inst), &n);

        if (status != RDIError_NoError)
        {
            free_bps |= (1 << b->bpno);
            if (p != NULL)
                *p = NULL;
            return status;
        }

        if (size == bs_32bit)
        {
            *((word *) & data) = angel_BreakPointInstruction_ARM;
        }
        else
        {
            *((short *)&data) = (short)angel_BreakPointInstruction_THUMB;
        }
        status = angelOS_MemWrite(OSinfo1, OSinfo2, b->address,
                                  size == bs_32bit ? 4 : 2,
                                  (byte *) (&data), &n);

        if (status != RDIError_NoError)
        {
            free_bps |= (1 << b->bpno);
            if (p != NULL)
                *p = NULL;
            return status;
        }

        /* Check that the break point has been written. */
        status = angelOS_MemRead(OSinfo1, OSinfo2, b->address,
                                 size == bs_32bit ? 4 : 2,
                                 (byte *) (&testRead), &n);
        if (status != RDIError_NoError ||
            __rt_memcmp((void *)&testRead, (void *)&data, size == bs_32bit ? 4 : 2))
        {
            LogWarning(LOG_DEBUGOS, ("Error setting breakpoint at 0x%x: orig data 0x%x current 0x%x.\n", b->address, b->inst, testRead));
            free_bps |= (1 << b->bpno);
            if (p != NULL)
                *p = NULL;
            return RDIError_CantSetPoint;