debugos.c

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

    if (Mode != USR32mode) /* and Mode != SYS32mode -- see above */
        if (ADP_CPUread_SPSR & Mask)
            cpuwrite(Angel_GetBankedReg(rb, Mode, 15), ReadData, &c);    

    return RDIError_NoError;
}

#if defined(SYSTEM_COPROCESSOR_SUPPORTED) && SYSTEM_COPROCESSOR_SUPPORTED > 0
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)
        {
            /* 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;

            Angel_EnterSVC();

            WriteFunction(*Data);

            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)
{
#if defined(SYSTEM_COPROCESSOR_SUPPORTED) && SYSTEM_COPROCESSOR_SUPPORTED > 0
    int regno;
    word status = RDIError_NoError;
    word *dp = Data;

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

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

#if defined(SYSTEM_COPROCESSOR_SUPPORTED) && SYSTEM_COPROCESSOR_SUPPORTED > 0
    if (CPnum != 15)
#endif
        return RDIError_UnknownCoPro;

#if defined(SYSTEM_COPROCESSOR_SUPPORTED) && SYSTEM_COPROCESSOR_SUPPORTED > 0
    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
}

#if defined(SYSTEM_COPROCESSOR_SUPPORTED) && SYSTEM_COPROCESSOR_SUPPORTED > 0
  /* 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)
        {
            /* 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;

            Angel_EnterSVC();

            *Data = ReadFunction();

            Angel_ExitToUSR();

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

int 
angelOS_CPRead(word OSinfo1, word OSinfo2, word CPnum, word Mask,
               word * Data, word * nbytes)
{
#if defined(SYSTEM_COPROCESSOR_SUPPORTED) && SYSTEM_COPROCESSOR_SUPPORTED > 0
    int regno;
    word status = RDIError_NoError;
    word *dp = Data;

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

    IGNORE(OSinfo1);
    IGNORE(OSinfo2);

    *nbytes = 0;

#if defined(SYSTEM_COPROCESSOR_SUPPORTED) && SYSTEM_COPROCESSOR_SUPPORTED > 0
    if (CPnum != 15)
#endif
        return RDIError_UnknownCoPro;

#if defined(SYSTEM_COPROCESSOR_SUPPORTED) && SYSTEM_COPROCESSOR_SUPPORTED > 0
    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;
        }
    }

    ReturnValue->pointHandle = (word) b;

    LogInfo(LOG_DEBUGOS, ("handle = %x head_bp = %x free_bps = %x.\n",
              ReturnValue->pointHandle, (word) head_bp, (word) free_bps));

    return RDIError_NoError;
}


int 
angelOS_ClearBreak(word OSinfo1, word OSinfo2, word Handle)
{
    struct BreakPoint *b, *p, **pp;
    word status, n;

    LogInfo(LOG_DEBUGOS, ( "angelOS_ClearBreak: Handle = %x.\n", Handle));

    b = (struct BreakPoint *)Handle;
    if (b == NULL)
        return RDIError_NoSuchPoint;

    for (pp = &head_bp; (p = *pp) != b; pp = &p->next)
    {
        LogInfo(LOG_DEBUGOS, ( "Current bp address = %x.\n", (word) p));

        if (p == NULL)
            return RDIError_NoSuchPoint;
    }

    if (--b->count != 0)
        return RDIError_NoError;

    *pp = b->next;

    LogInfo(LOG_DEBUGOS, ( "Freeing breakpoint %d.\n", b->bpno));

    free_bps |= (1 << b->bpno);
    status = angelOS_MemWrite(OSinfo1, OSinfo2, b->address,
                              b->size == bs_32bit ? 4 : 2,
                              (byte *) (&(b->inst)), &n);

    if (status != RDIError_NoError)
    {
        LogWarning(LOG_DEBUGOS, ( "Memwrite error in CB.\n"));
        return status;
    }

    LogInfo(LOG_DEBUGOS, ( "Leaving angelOS_ClearBreak function.\n"));
    return RDIError_NoError;
}

int 
angelOS_SetWatch(word OSinfo1, word OSinfo2,
                 struct SetPointValue *SetData,
                 struct PointValueReturn *ReturnValue)
{
    IGNORE(OSinfo1);
    IGNORE(OSinfo2);
    IGNORE(ReturnValue);
    IGNORE(SetData);
    return RDIError_UnimplementedMessage;
}

int 
angelOS_ClearWatch(word OSinfo1, word OSinfo2, word Handle)
{
    IGNORE(OSinfo1);
    IGNORE(OSinfo2);
    IGNORE(Handle);
    return RDIError_UnimplementedMessage;
}

int 
angelOS_BreakPointStatus(word OSinfo1, word OSinfo2, word Handle,
                         word * Hw, word * Type)
{
    IGNORE(OSinfo1);
    IGNORE(OSinfo2);
    IGNORE(Handle);
    IGNORE(Hw);
    IGNORE(Type);
    return RDIError_UnimplementedMessage;
}

int 
angelOS_WatchPointStatus(word OSinfo1, word OSinfo2, word Handle,
                         word * Hw, word * Type)
{
    IGNORE(OSinfo1);
    IGNORE(OSinfo2);
    IGNORE(Handle);
    IGNORE(Hw);
    IGNORE(Type);
    return RDIError_UnimplementedMessage;
}

int 
angelOS_SetupStep(word OSinfo1, word OSinfo2, word StepCount,
                  void **handle)
{
    IGNORE(OSinfo1);
    IGNORE(OSinfo2);
    IGNORE(StepCount);
    IGNORE(handle);
    return RDIError_UnimplementedMessage;
}

int 
angelOS_DoStep(void *handle)
{
    IGNORE(handle);
    return RDIError_UnimplementedMessage;
}

int 
angelOS_Execute(word OSinfo1, word OSinfo2)
{
    angel_RegBlock *rb;
    
    IGNORE(OSinfo1);
    IGNORE(OSinfo2);

    /* Find a task with the Application priority */
    rb = angel_AccessQueuedRegBlock(TP_Application);