Subversion Repositories openrisc

//==========================================================================

//

//      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)

                            return wa0;

                        else if (val == wd1)

                            return wa1;

                    }

                }

            }

            return wa0;  // 50% chance of being right

        } else

            addr = get_register(Rn) - size;

        break;

    case 0x6000: // STR  Rd, [Rn, #5bit]

    case 0x7000: // STRB Rd, [Rn, #5bit]

    case 0x8000: // STRH Rd, [Rn, #5bit]

        is_store = 1;

    case 0x6800: // LDR  Rd, [Rn, #5bit]

    case 0x7800: // LDRB Rd, [Rn, #5bit]

    case 0x8800: // LDRH Rd, [Rn, #5bit]

        Rd = opcode & 7;

        Rn = (opcode >> 3) & 7;

        if ((opcode & 0xf000) == 0x6000)

            size = 4;

        else if ((opcode & 0xf000) == 0x8000)

            size = 2;

        else

            size = 1;

        if (!is_store && Rd == Rn) {

            // We can't reconstruct address from opcode because base

            // or offset register was destroyed. Best we can do is try

            // to match data at watchpoint addresses with data in Rd.

            use_val = 1;

            val = get_register(Rd);

        } else {

            offset = ((opcode >> 6) & 0x1f) * size;

            addr = get_register(Rn) + offset;

        }

        break;

    case 0x4800: // LDR Rd, [PC, #8bit]

        size = 4;

        addr = pc + 4 + ((opcode & 0xff) * 4);

        break;

    case 0x9000: // STR Rd, [SP, #8bit]

        is_store = 1;

    case 0x9800: // LDR Rd, [SP, #8bit]

        size = 4;

        addr = get_register(SP) + ((opcode & 0xff) * 4);

        break;

    default:

        switch (opcode_fe) {

        case 0x5000:  // STR   Rd, [Rn, Rm]

        case 0x5400:  // STRB  Rd, [Rn, Rm]

        case 0x5200:  // STRH  Rd, [Rn, Rm]

            is_store = 1;

        case 0x5600:  // LDRSB Rd, [Rn, Rm]

        case 0x5800:  // LDR   Rd, [Rn, Rm]

        case 0x5c00:  // LDRB  Rd, [Rn, Rm]

        case 0x5a00:  // LDRH  Rd, [Rn, Rm]

        case 0x5e00:  // LDRSH Rd, [Rn, Rm]

            Rd = opcode & 7;

            Rn = (opcode >> 3) & 7;

            Rm = (opcode >> 6) & 7;

            size = _sztab[(opcode >> 9) & 7];

            if (!is_store && (Rd == Rn || Rd == Rm)) {

                // We can't reconstruct address from opcode because base

                // or offset register was destroyed. Best we can do is try

                // to match data at watchpoint addresses with data in Rd.

                use_val = 1;

                val = get_register(Rd);

            } else

                addr = Rn + Rm;

            break;

        case 0xb400:  // PUSH

            is_store = 1;

        case 0xbc00:  // POP

            for (i = 0; i < 9; i++)

                if (opcode & (1 << i))

                    size += 4;

            addr = get_register(SP);

            if (!is_store)

                addr -= size;

            break;

        }

        break;

    }

    if (use_val) {

        // We can read from watchpoint addresses and compare against

        // whatever is in the Rd from a load. This is not perfect,

        // but its the best we can do.

        if (wval_match(wa0, val, size))

            return wa0;

        if (wval_match(wa1, val, size))

            return wa1;

    } else if (size) {

        if (waddr_match(wa0, addr, size))

            return wa0;

        if (waddr_match(wa1, addr, size))

            return wa1;

    }

    return wa0;  // should never happen, but return valid address

}

// Given the watch addresses and watch modes for each of the enabled

// watchpoints, figure out which one triggered the current exception.

static unsigned

find_watch_address(unsigned wa0, int mode0, unsigned wa1, int mode1)

{

    unsigned pc = get_register(PC) - 4;

    unsigned cpsr = get_register(PS);

    unsigned opcode, Rn, Rd, Rm, base, addr, val = 0, wd0, wd1;

    int  is_store, use_val, i, offset, shift, size;

    if (cpsr & CPSR_THUMB_ENABLE)

        is_store = is_thumb_store_insn(pc);

    else

        is_store = is_store_insn(pc);

    // First try the easy cases where all we need to know is whether or

    // not the instruction is a load or store.

    if ((mode0 == WATCH_MODE_READ && mode1 == WATCH_MODE_WRITE) ||

        (mode1 == WATCH_MODE_READ && mode0 == WATCH_MODE_WRITE)) {

        if (is_store)

            return (mode0 == WATCH_MODE_WRITE) ? wa0 : wa1;

        else

            return (mode0 == WATCH_MODE_READ) ? wa0 : wa1;

    }

    if ((mode0 == WATCH_MODE_READ && is_store) ||

        (mode0 == WATCH_MODE_WRITE && !is_store))

        return wa1;

    if ((mode1 == WATCH_MODE_READ && is_store) ||

        (mode1 == WATCH_MODE_WRITE && !is_store))

        return wa0;

    // Okay. Now try to figure out address by decoding the opcode.

    if (cpsr & CPSR_THUMB_ENABLE)

        return find_thumb_watch_address(wa0, mode0, wa1, mode1);

    opcode = *(unsigned *)pc;

    Rn = (opcode >> 16) & 15;

    Rd = (opcode >> 12) & 15;

    Rm = opcode & 15;

    size = use_val = 0;

    addr = 0;

    if ((opcode & 0x0fb00ff0) == 0x01000090) {

        // SWP          xxxx 0001 0B00 _Rn_ _Rd_ 0000 1001 _Rm_

        addr = get_register(Rn);

        size = (opcode & B_bit) ? 1 : 4;

    } else 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

        size = (opcode & B_bit) ? 1 : 4;

        if ((opcode & (P_bit | W_bit)) == (P_bit | W_bit)) {

            // This is easy because address is written back to Rn

            addr = get_register(Rn);

        } else if (!is_store &&

                   (Rd == Rn || ((opcode & I_bit) && Rd == Rm))) {

            // We can't reconstruct address from opcode because base

            // or offset register was destroyed. Best we can do is try

            // to match data at watchpoint addresses with data in Rd.

            use_val = 1;

            val = get_register(Rd);

        } else {

            if (opcode & I_bit) {

                shift = (opcode >> 7) & 0x1f;

                offset = get_register(Rm);

                switch ((opcode >> 5) & 3) {

                case 0:

                    offset <<= shift;

                    break;

                case 1:

                    offset >>= shift;

                    offset &= (0xffffffffU >> shift);

                    break;

                case 2:

                    offset >>= shift;

                    break;

                case 3:

                    if (shift) {

                        for (i = 0; i < shift; i++)

                            offset = ((offset >> 1) & 0x7fffffff) | ((offset & 1) << 31);

                    } else {

                        offset >>= 1;

                        offset &= 0x80000000;

                        offset |= ((cpsr & 0x20000000) << 2);

                    }

                    break;

                }

            } else

                offset = opcode & 0xfff;

            if ((opcode & U_bit) == 0)

                offset = -offset;

            if (Rn == 15)

                base = pc + 8;

            else

                base = get_register(Rn);

            if (opcode & P_bit)

                addr = base + offset; // pre-index

            else

                addr = base - offset; // post-index writeback

            size = (opcode & B_bit) ? 1 : 4;

        }

    } else 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

        size = (opcode & (1 << 5)) ? 2 : 1;

        if ((opcode & (P_bit | W_bit)) == (P_bit | W_bit)) {

            // This is easy because address is written back to Rn

            addr = get_register(Rn);

        } if (!is_store &&

              (Rd == Rn || ((opcode & (1 << 22)) && Rd == Rm))) {

            // We can't reconstruct address from opcode because base

            // or offset register was destroyed. Best we can do is try

            // to match data at watchpoint addresses with data in Rd.

            use_val = 1;

            val = get_register(Rd);

        } else {

            if (opcode & (1 << 22))

                offset = ((opcode >> 4) & 0xf0) | (opcode & 0x0f);

            else