xscale_stub.c

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

			    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
		offset = get_register(opcode & 15);

	    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
	}
    } else if ((opcode & 0x0e000000) == 0x08000000) {
	// LDM/STM      xxxx 100P USWL _Rn_ rrrr rrrr rrrr rrrr
	for (i = size = 0; i < 16; i++)
	    if (opcode & (1 << i))
		size += 4;

	base = get_register(Rn);
	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 < 16; 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 {
	    if (opcode & U_bit){
		if (opcode & W_bit)
		    addr = base - size;
		else
		    addr = base;
		if (opcode & P_bit)
		    addr += 4;
	    } else {
		if (opcode & W_bit)
		    addr = base;
		else
		    addr = base - size;
		if ((opcode & P_bit) == 0)
		    addr += 4;
	    }
	}
    } else if ((opcode & 0x0e000000) == 0x0c000000) {
	// LDC/STC      xxxx 110P UNWL _Rn_ CRd_ CP#_ iiii iiii
	size = 4;
	offset = (opcode & 0xff) * 4;
	if ((opcode & U_bit) == 0)
	    offset = -offset;
	if ((opcode & P_bit) && (opcode & W_bit))
	    addr = get_register(Rn);
	else
	    addr = get_register(Rn) + offset;
    }

    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 (waddr_match(wa0, addr, size))
	    return wa0;
	if (waddr_match(wa1, addr, size))
	    return wa1;
    }
    return wa0;  // should never happen, but return valid address
}
#endif

// Return indication of whether or not we stopped because of a
// watchpoint or hardware breakpoint. If stopped by a watchpoint,
// also set '*data_addr_p' to the data address which triggered the
// watchpoint.
int cyg_hal_plf_is_stopped_by_hardware(void **data_addr_p)
{
    unsigned fsr, dcsr, dbcon, kind = 0;

    // Check for debug event
    asm volatile ("mrc p15,0,%0,c5,c0,0" : "=r"(fsr) : );
    if ((fsr & 0x200) == 0)
	return HAL_STUB_HW_STOP_NONE;

    // There was a debug event. Check the MOE for details
    dcsr = get_dcsr();
    switch ((dcsr >> 2) & 7) {
      case 1:  // HW breakpoint
      case 3:  // BKPT breakpoint
	return HAL_STUB_HW_STOP_BREAK;
      case 2:  // Watchpoint
	dbcon = get_dbcon();
#if HAL_STUB_HW_WATCHPOINT_LIST_SIZE == 2
	if (dbcon & 0x100) {
#endif
	    if ((kind = (dbcon & 3)) != WATCH_MODE_NONE)
		*data_addr_p = (void *)get_dbr0();
#if HAL_STUB_HW_WATCHPOINT_LIST_SIZE == 2
	} else {
	    // This can get tricky because the debug unit offers no way to
	    // tell which watchpoint triggered.
	    if ((kind = (dbcon & 3)) == WATCH_MODE_NONE) {
		if ((kind = ((dbcon >> 2) & 3)) != WATCH_MODE_NONE)
		    *data_addr_p = (void *)get_dbr1();
	    } else if ((kind = ((dbcon >> 2) & 3)) == WATCH_MODE_NONE) {
		if ((kind = (dbcon & 3)) != WATCH_MODE_NONE)
		    *data_addr_p = (void *)get_dbr0();
	    } else {
		// This is the tricky part. We have to look at the trapping
		// opcode (which has already issued) to try to see if we can
		// tell which watchpoint triggered. Turn off watchpoints while
		// we figure this out.
		set_dcsr(dcsr & ~0x80000000);
		*data_addr_p = (void *)find_watch_address(get_dbr0(), dbcon & 3,
							  get_dbr1(), (dbcon >> 2) & 3);
		set_dcsr(dcsr);

		if (*data_addr_p == (void *)get_dbr0())
		    kind = dbcon & 3;
		else if (*data_addr_p == (void *)get_dbr1())
		    kind = (dbcon >> 2) & 3;
		else
		    kind = WATCH_MODE_NONE;
	    }
	}
#endif
	if (kind == WATCH_MODE_WRITE)
	    return HAL_STUB_HW_STOP_WATCH;
	if (kind == WATCH_MODE_ACCESS)
	    return HAL_STUB_HW_STOP_AWATCH;
	if (kind == WATCH_MODE_READ)
	    return HAL_STUB_HW_STOP_RWATCH;
	// should never get here
	break;
    }
    return HAL_STUB_HW_STOP_NONE;
}
#endif // CYGDBG_HAL_DEBUG_GDB_INCLUDE_STUBS

/*------------------------------------------------------------------------*/
// EOF xscale_stub.c