File content as of revision 1:ea8e179320d7:

// Target2.cpp 2013/9/23
#include "Target2.h"
#include "mydebug.h"

#define SYSMEMREMAP  0x40048000

#define CoreDebug_BASE      (0xE000EDF0UL)
#define DHCSR (CoreDebug_BASE+0)
#define DCRSR (CoreDebug_BASE+4) 
#define DCRDR (CoreDebug_BASE+8)
#define DEMCR (CoreDebug_BASE+12)

#define NVIC_AIRCR 0xE000ED0C

// FPB (breakpoint)
#define FP_CTRL (0xE0002000)
#define FP_CTRL_KEY (1 << 1)
#define FP_COMP0 (0xE0002008)

Target2::Target2(PinName swdio, PinName swclk, PinName reset)
{
    _swd = new SWD(swdio, swclk, reset);
    inst();
}

Target2::Target2(SWD* swd) : _swd(swd)
{
    inst();
}

void Target2::inst()
{
    r0.setup(this, 0);
    r1.setup(this, 1);
    r2.setup(this, 2);
    r3.setup(this, 3);
    r4.setup(this, 4);
    r5.setup(this, 5);
    r6.setup(this, 6);
    r7.setup(this, 7);
    r8.setup(this, 8);
    r9.setup(this, 9);
    r10.setup(this, 10);
    r11.setup(this, 11);
    r12.setup(this, 12);
    sp.setup(this, 13);
    lr.setup(this, 14);
    pc.setup(this, 15);
    xpsr.setup(this, 16);
}

bool Target2::setup()
{
    _swd->Setup();
    JTAG2SWD();
    
    uint32_t data;
    uint8_t ack = _swd->Transfer(DP_IDCODE, &data);
    TEST_ASSERT(ack == SWD_OK);
    if (ack != SWD_OK) {
        return false;
    }
    idcode = data;

    Abort();

    data = 0x0;
    ack = _swd->Transfer(DP_SELECT, &data);
    TEST_ASSERT(ack == SWD_OK);
    if (ack != SWD_OK) {
        return false;
    }

    ack = _swd->Transfer(DP_RDBUFF, &data);
    TEST_ASSERT(ack == SWD_OK);
    if (ack != SWD_OK) {
        return false;
    }

    data = CSYSPWRUPREQ | CDBGPWRUPREQ;
    ack = _swd->Transfer(DP_CTRL_STAT, &data);
    TEST_ASSERT(ack == SWD_OK);
    if (ack != SWD_OK) {
        return false;
    }

    ack = _swd->Transfer(DP_RDBUFF, &data);
    TEST_ASSERT(ack == SWD_OK);
    if (ack != SWD_OK) {
        return false;
    }

    ack = _swd->Transfer(DP_CTRL_STAT_R, &data);
    TEST_ASSERT(ack == SWD_OK);
    if (ack != SWD_OK) {
        return false;
    }
    TEST_ASSERT(data == 0xf0000040);

    data = CSYSPWRUPREQ | CDBGPWRUPREQ | 0x04000000;
    ack = _swd->Transfer(DP_CTRL_STAT, &data);
    TEST_ASSERT(ack == SWD_OK);
    if (ack != SWD_OK) {
        return false;
    }

    ack = _swd->Transfer(DP_RDBUFF, &data);
    TEST_ASSERT(ack == SWD_OK);
    if (ack != SWD_OK) {
        return false;
    }

    data = CSYSPWRUPREQ | CDBGPWRUPREQ | MASKLANE;
    ack = _swd->Transfer(DP_CTRL_STAT, &data);
    TEST_ASSERT(ack == SWD_OK);
    if (ack != SWD_OK) {
        return false;
    }

    ack = _swd->Transfer(DP_RDBUFF, &data);
    TEST_ASSERT(ack == SWD_OK);
    if (ack != SWD_OK) {
        return false;
    }
    return true;
}

void Target2::SWJClock(uint32_t clock_hz)
{
    _swd->SWJClock(clock_hz);
}

void Target2::JTAG2SWD()
{
    const uint8_t data1[] = {0xff,0xff,0xff,0xff,0xff,0xff,0xff};
    const uint8_t data2[] = {0x9e,0xe7};
    const uint8_t data3[] = {0x00};
    _swd->SWJSequence(sizeof(data1)*8, data1);
    _swd->SWJSequence(sizeof(data2)*8, data2);
    _swd->SWJSequence(sizeof(data1)*8, data1);
    _swd->SWJSequence(sizeof(data3)*8, data3);
}

void Target2::HardwareReset()
{
    _swd->SWJPins(0x00, 0x80); // nReset off
    _swd->SWJPins(0x80, 0x80); // nReset on
}

void Target2::SoftwareReset()
{
    writeMemory(NVIC_AIRCR, 0x05fa0004);
}

uint32_t Target2::readMemory(uint32_t addr)
{
    _setaddr(addr);

    uint32_t data;
    uint8_t ack = _swd->Transfer(AP_DRW_R, &data); // dummy read
    TEST_ASSERT(ack == SWD_OK);

    ack = _swd->Transfer(DP_RDBUFF, &data);
    TEST_ASSERT(ack == SWD_OK);
    return data;
}

void Target2::readMemory(uint32_t addr, uint32_t* data, int count)
{
    if (count == 0) {
        return;
    }

    _setaddr(addr);
    
    uint8_t ack = _swd->Transfer(AP_DRW_R, NULL); // dummy read
    TEST_ASSERT(ack == SWD_OK);

    for(int i = 0; i < count-1; i++) {
        ack = _swd->Transfer(AP_DRW_R, data++);
        TEST_ASSERT(ack == SWD_OK);
    }
    ack = _swd->Transfer(DP_RDBUFF, data);
    TEST_ASSERT(ack == SWD_OK);
}

void Target2::writeMemory(uint32_t addr, uint32_t data)
{
    writeMemory(addr, &data, 1);
}

void Target2::writeMemory(uint32_t addr, uint32_t* data, int count)
{
    _setaddr(addr);

    while(count-- > 0) {
        uint8_t ack = _swd->Transfer(AP_DRW_W, data);
        TEST_ASSERT(ack == SWD_OK);
        data++;
    }
}

uint8_t Target2::readMemory8(uint32_t addr)
{
    _setaddr8(addr);

    uint32_t data32;
    uint8_t ack = _swd->Transfer(AP_DRW_R, &data32); // dummy read
    TEST_ASSERT(ack == SWD_OK);

    ack = _swd->Transfer(DP_RDBUFF, &data32);
    TEST_ASSERT(ack == SWD_OK);
    return (data32 >> ((addr & 0x03) << 3)) & 0xff;
}

void Target2::writeMemory8(uint32_t addr, uint8_t data)
{
    _setaddr8(addr);

    uint32_t data32 = data;
    data32 <<= ((addr & 0x03) << 3);
    uint8_t ack = _swd->Transfer(AP_DRW_W, &data32);
    TEST_ASSERT(ack == SWD_OK);
}

void Target2::_setaddr(uint32_t addr)
{
    uint32_t ctl = CSW_VALUE|CSW_SIZE32;
    uint8_t ack = _swd->Transfer(AP_CSW, &ctl);
    TEST_ASSERT(ack == SWD_OK);

    ack = _swd->Transfer(DP_RDBUFF, NULL);
    TEST_ASSERT(ack == SWD_OK);

    ack = _swd->Transfer(AP_TAR, &addr);
    TEST_ASSERT(ack == SWD_OK);

    ack = _swd->Transfer(DP_RDBUFF, NULL);
    TEST_ASSERT(ack == SWD_OK);
} 

void Target2::_setaddr8(uint32_t addr)
{
    uint32_t ctl = CSW_VALUE|CSW_SIZE8;
    uint8_t ack = _swd->Transfer(AP_CSW, &ctl);
    TEST_ASSERT(ack == SWD_OK);

    ack = _swd->Transfer(DP_RDBUFF, NULL);
    TEST_ASSERT(ack == SWD_OK);

    ack = _swd->Transfer(AP_TAR, &addr);
    TEST_ASSERT(ack == SWD_OK);

    ack = _swd->Transfer(DP_RDBUFF, NULL);
    TEST_ASSERT(ack == SWD_OK);
} 

void Target2::Abort()
{
    uint32_t data = 0x1e;
    uint8_t ack = _swd->Transfer(DP_ABORT, &data);
    TEST_ASSERT(ack == SWD_OK);
}

int Target2::getStatus()
{
    return readMemory(DHCSR) & 6 ? TARGET_HALTED : TARGET_RUNNING;
}

bool Target2::wait_status(int status, int timeout_ms)
{
    Timer t;
    t.reset();
    t.start();
    while(t.read_ms() < timeout_ms) {
        if (getStatus() == status) {
            return true;
        }
    }
    return false;
}

bool Target2::prog_status()
{
    writeMemory(DEMCR, 1);
    int status = getStatus();
    TEST_ASSERT(status == TARGET_HALTED);
    if (status == TARGET_RUNNING) {
        halt();
    }
    bool st = wait_status(TARGET_HALTED);
    TEST_ASSERT(st == true);
    writeMemory(DEMCR, 0);
    writeMemory(SYSMEMREMAP, 2); // user flash page
    uint32_t reset_handler = readMemory(4);
    if (setBreakpoint0(reset_handler)) {
        writeMemory(NVIC_AIRCR, 0x05fa0004); // SYSRESETREQ software reset
        st = wait_status(TARGET_HALTED);
        TEST_ASSERT(st == true);
        TEST_ASSERT((reset_handler&0xfffffffe) == pc);
        removeBreakpoint0(0);
    }    
    return true;
}

bool Target2::setBreakpoint0(uint32_t addr)
{
    if ((addr&1) == 0 || addr >= 0x20000000) {
        return false;
    }
    uint32_t data = (addr&0x1ffffffc) | 0xc0000001;
    if (addr&0x00000002) {
        data |= 0x80000000;
    } else {
        data |= 0x40000000;
    }
    writeMemory(FP_COMP0, data); // set breakpoint
    writeMemory(FP_CTRL, 3); // enable FPB
    return true;
}

void Target2::removeBreakpoint0(uint32_t addr)
{
    writeMemory(FP_COMP0, 0); // breakpoint clear
    writeMemory(FP_CTRL, 2); // desable FPB
}

void Target2::halt()
{
    writeMemory(DHCSR, 0xa05f0003);     
}

void Target2::resume()
{
    writeMemory(DHCSR, 0xa05f0001);     
}

void Target2::step()
{
    writeMemory(DHCSR, 0xa05f0005);     
}

uint32_t CoreReg::read()
{
    _target->writeMemory(DCRSR, _reg);
    return _target->readMemory(DCRDR);
}

void CoreReg::write(uint32_t value)
{
    _target->writeMemory(DCRDR, value);
    _target->writeMemory(DCRSR, _reg|0x10000);
}

void CoreReg::setup(Target2* target, uint8_t reg)
{
    _target = target;
    _reg = reg;    
}