xscale_stub.c

开放源码实时操作系统源码.

//==========================================================================
//
//      xscale_stub.c
//
//      HAL stub support code for Intel XScale cores.
//
//==========================================================================
//####ECOSGPLCOPYRIGHTBEGIN####
// -------------------------------------------
// This file is part of eCos, the Embedded Configurable Operating System.
// Copyright (C) 1998, 1999, 2000, 2001, 2002 Red Hat, Inc.
//
// eCos is free software; you can redistribute it and/or modify it under
// the terms of the GNU General Public License as published by the Free
// Software Foundation; either version 2 or (at your option) any later version.
//
// eCos is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or
// FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
// for more details.
//
// You should have received a copy of the GNU General Public License along
// with eCos; if not, write to the Free Software Foundation, Inc.,
// 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
//
// As a special exception, if other files instantiate templates or use macros
// or inline functions from this file, or you compile this file and link it
// with other works to produce a work based on this file, this file does not
// by itself cause the resulting work to be covered by the GNU General Public
// License. However the source code for this file must still be made available
// in accordance with section (3) of the GNU General Public License.
//
// This exception does not invalidate any other reasons why a work based on
// this file might be covered by the GNU General Public License.
//
// Alternative licenses for eCos may be arranged by contacting Red Hat, Inc.
// at http://sources.redhat.com/ecos/ecos-license/
// -------------------------------------------
//####ECOSGPLCOPYRIGHTEND####
//==========================================================================
//#####DESCRIPTIONBEGIN####
//
// Author(s):    msalter
// Contributors: msalter
// Date:         2002-10-18
// Purpose:      XScale core stub support
// Description:  Implementations of HW debugging support.
//
//####DESCRIPTIONEND####
//
//========================================================================*/

#include <pkgconf/hal.h>
#include <pkgconf/system.h>
#include <cyg/infra/diag.h>
#include <cyg/hal/hal_stub.h>           // Stub macros

#ifdef CYGDBG_HAL_DEBUG_GDB_INCLUDE_STUBS
/*------------------------------------------------------------------------*/
//  HW Debug support

// Define this to support two watchpoints. If not defined, one watchpoint with
// a power of two range is supported.
#define USE_TWO_WATCHPOINTS 1

static inline void set_ibcr0(unsigned x)
{
    asm volatile ("mcr p15,0,%0,c14,c8,0" : : "r"(x) );
}

static inline unsigned get_ibcr0(void)
{
    unsigned x;
    asm volatile ("mrc p15,0,%0,c14,c8,0" : "=r"(x) : );
    return x;
}

static inline void set_ibcr1(unsigned x)
{
    asm volatile ("mcr p15,0,%0,c14,c9,0" : : "r"(x) );
}

static inline unsigned get_ibcr1(void)
{
    unsigned x;
    asm volatile ("mrc p15,0,%0,c14,c9,0" : "=r"(x) : );
    return x;
}

static inline void set_dbr0(unsigned x)
{
    asm volatile ("mcr p15,0,%0,c14,c0,0" : : "r"(x) );
}

static inline unsigned get_dbr0(void)
{
    unsigned x;
    asm volatile ("mrc p15,0,%0,c14,c0,0" : "=r"(x) : );
    return x;
}

static inline void set_dbr1(unsigned x)
{
    asm volatile ("mcr p15,0,%0,c14,c3,0" : : "r"(x) );
}

static inline unsigned get_dbr1(void)
{
    unsigned x;
    asm volatile ("mrc p15,0,%0,c14,c3,0" : "=r"(x) : );
    return x;
}

static inline void set_dbcon(unsigned x)
{
    asm volatile ("mcr p15,0,%0,c14,c4,0" : : "r"(x) );
}

static inline unsigned get_dbcon(void)
{
    unsigned x;
    asm volatile ("mrc p15,0,%0,c14,c4,0" : "=r"(x) : );
    return x;
}

static inline void set_dcsr(unsigned x)
{
    asm volatile ("mcr p14,0,%0,c10,c0,0" : : "r"(x) );
}

static inline unsigned get_dcsr(void)
{
    unsigned x;
    asm volatile ("mrc p14,0,%0,c10,c0,0" : "=r"(x) : );
    return x;
}

int cyg_hal_plf_hw_breakpoint(int setflag, void *vaddr, int len)
{
    unsigned int addr = (unsigned)vaddr;

    if (setflag) {
	if (!(get_ibcr0() & 1))
	    set_ibcr0(addr | 1);
	else if (!(get_ibcr1() & 1))
	    set_ibcr1(addr | 1);
	else
	    return -1;
    } else {
	unsigned x = (addr | 1);
	if (get_ibcr0() == x)
	    set_ibcr0(0);
	else if (get_ibcr1() == x)
	    set_ibcr1(0);
	else
	    return -1;
    }

    return 0;
}

#define WATCH_MODE_NONE   0
#define WATCH_MODE_WRITE  1
#define WATCH_MODE_ACCESS 2
#define WATCH_MODE_READ   3

#ifndef HAL_STUB_HW_WATCHPOINT_LIST_SIZE
#error
#endif

int cyg_hal_plf_hw_watchpoint(int setflag, void *vaddr, int len, int type)
{
    unsigned int mode, addr = (unsigned)vaddr;
    unsigned dbcon = get_dbcon();
#if HAL_STUB_HW_WATCHPOINT_LIST_SIZE == 1
    unsigned int mask, bit_nr;

    mask = 0x80000000;
    bit_nr = 31;
    while (bit_nr) {
	if (len & mask)
	    break;
	bit_nr--;
	mask >>= 1;
    }
    mask = ~(0xffffffff << bit_nr);
#endif

    if (setflag) {
	/* set a watchpoint */
	if (type == 2)
	    mode = WATCH_MODE_WRITE;  // break on write
	else if (type == 3)
	    mode = WATCH_MODE_READ;   // break on read
	else if (type == 4)
	    mode = WATCH_MODE_ACCESS; // break on any access
	else
	    return 1;

#if HAL_STUB_HW_WATCHPOINT_LIST_SIZE == 1
	mode |= 0x100;
#endif

	if (!(dbcon & 3)) {
	    set_dbr0(addr);
#if HAL_STUB_HW_WATCHPOINT_LIST_SIZE == 1
	    set_dbr1(mask);
#endif
	    set_dbcon(dbcon | mode);
#if HAL_STUB_HW_WATCHPOINT_LIST_SIZE == 2
	} else if (!(dbcon & (3 << 2))) {
	    set_dbr1(addr);
	    set_dbcon(dbcon | (mode << 2));
#endif
	} else
	    return 1;

    } else {
	/* clear a watchpoint */
	if ((dbcon & 3) && get_dbr0() == addr)
	    set_dbcon(dbcon & ~3);
#if HAL_STUB_HW_WATCHPOINT_LIST_SIZE == 2
	else if ((dbcon & (3 << 2)) && get_dbr1() == addr)
	    set_dbcon(dbcon & ~(3 << 2));
#endif
	else
	    return 1;
    }
    return 0;
}

#if HAL_STUB_HW_WATCHPOINT_LIST_SIZE == 2

// The XScale hardware does not provide a way of positively identinfying
// which of the two watchpoints triggered and exception. The following
// code makes a best effort at determining this by decoding the opcode
// of the instruction which caused the watchpoint trigger. It is *not*
// 100% reliable.

// Some bits common to most ld/st instructions.
#define I_bit (1 << 25)
#define P_bit (1 << 24)
#define U_bit (1 << 23)
#define B_bit (1 << 22)
#define W_bit (1 << 21)
#define L_bit (1 << 20)

// Return non-zero if opcode at given PC is a store instruction for
// purposes of triggering watchpoints.
static int
is_store_insn(unsigned pc)
{
    unsigned opcode = *(unsigned *)pc;

    if ((opcode & 0x0fb00ff0) == 0x01000090) {
	// SWP          xxxx 0001 0B00 _Rn_ _Rd_ 0000 1001 _Rm_
	return 1;
    }

    if ((opcode & 0x0c000000) == 0x04000000) {
	// LDR/STR      xxxx 010P UBWL _Rn_ _Rd_ iiii iiii iiii
	// LDR/STR      xxxx 011P UBWL _Rn_ _Rd_ ssss sSh0 _Rm_
	// Addressing mode 2,  Load/Store word or unsigned byte
	return (opcode & L_bit) == 0;
    }

    if ((opcode & 0x0e000090) == 0x00000090 &&
	(opcode & 0x00000060) &&
	((opcode & (1 << 22)) || (opcode & 0x00000f00) == 0) &&
	((opcode & (P_bit | W_bit)) != W_bit)) {
	// LDR/STR    xxxx 000P U1WL _Rn_ _Rd_ iiii 1SH1 iiii
	// LDR/STR    xxxx 000P U0WL _Rn_ _Rd_ 0000 1SH1 _Rm_
	// Addressing Mode 3, Load/Store halfword, load signed byte
	return (opcode & L_bit) == 0;
    }

    if ((opcode & 0x0e000000) == 0x08000000) {
	// LDM/STM      xxxx 100P USWL _Rn_ rrrr rrrr rrrr rrrr
	return (opcode & L_bit) == 0;
    }

    if ((opcode & 0x0e000000) == 0x0c000000) {
	// LDC/STC      xxxx 110P UNWL _Rn_ CRd_ CP#_ iiii iiii
	return (opcode & L_bit) == 0;
    }

    return 0;
}

static int
is_thumb_store_insn(unsigned pc)
{
    unsigned short opcode = *(unsigned short *)pc;

    opcode &= 0xfe00;

    if (opcode == 0xb400)
	return 1;  // PUSH
    if (opcode == 0x5000)
	return 1;  // STR Rd, [Rn, Rm]
    if (opcode == 0x5400)
	return 1;  // STRB Rd, [Rn, Rm]
    if (opcode == 0x5200)
	return 1;  // STRH Rd, [Rn, Rm]

    opcode &= 0xf800;
    if (opcode == 0xc000)
	return 1;  // STM
    if (opcode == 0x6000)
	return 1;  // STR Rd, [Rn, #5bit_offset]
    if (opcode == 0x9000)
	return 1;  // STR Rd, [SP, #8bit_offset]
    if (opcode == 0x7000)
	return 1;  // STRB Rd, [Rn, #5bit_offset]
    if (opcode == 0x8000)
	return 1;  // STRH Rd, [Rn, #5bit_offset]

    return 0;
}

// Return non-zero if given waddr matches an access at addr.
static int
waddr_match(unsigned waddr, unsigned addr, int size)
{
    if (addr <= waddr && waddr < (addr + size))
	return 1;
    return 0;
}

// Return non-zero if given value matches value at watchpoint address.
static int
wval_match(unsigned waddr, unsigned val, int size)
{
    unsigned wval = *(unsigned *)(waddr & ~3);
    int i;

    if (size == 4)
	return (wval == val);
    if (size == 2) {
	val &= 0xffff;
	return ((wval & 0xffff) == val || ((wval >> 16) == val));
    }
    if (size == 1) {
	val &= 0xff;
	for (i = 0; i < 4; i++) {
	    if ((wval & 0xff) == val)
		return 1;
	    wval >>= 8;
	}
    }
    return 0;
}

static char _sztab[8] = { 4, 2, 1, 1, 4, 2, 1, 2 };

// Given the watch addresses and watch modes for each of the enabled
// watchpoints, figure out which one triggered the current exception.
static unsigned
find_thumb_watch_address(unsigned wa0, int mode0, unsigned wa1, int mode1)
{
    unsigned pc = get_register(PC) - 4;
    unsigned short opcode = *(unsigned short *)pc;
    unsigned short opcode_f8, opcode_fe;
    unsigned val, wd0, wd1, addr = 0;
    int  is_store, use_val, i, offset, size, Rn, Rd, Rm;

    opcode_f8 = opcode & 0xf800;
    opcode_fe = opcode & 0xfe00;

    size = 0;
    is_store = 0;
    val = use_val = 0;

    switch (opcode_f8) {
    case 0xc000: // STMIA Rn!, <list>
	is_store = 1;
    case 0xc800: // LDMIA Rn!, <list>
	Rn = (opcode >> 8) & 7;
	is_store = (opcode & 0x800) == 0;
	for (i = 0; i < 8; i++)
	    if (opcode & (1 << i))
		size += 4;
	if (!is_store && (opcode & (1 << Rn))) {
	    // We can't reconstruct address from opcode because base
	    // was destroyed. Best we can do is try to match data at
            // watchpoint addresses with data in one of the registers.
	    wd0 = *(unsigned *)(wa0 & ~3);
	    wd1 = *(unsigned *)(wa1 & ~3);
	    if (wd0 != wd1) {
		for (i = size = 0; i < 8; i++) {
		    if (opcode & (1 << i)) {
			val = get_register(i);
			if (val == wd0)