<|start_filename|>dap_target.c<|end_filename|>
/*
 * Copyright (c) 2014-2016, <NAME> <<EMAIL>>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/*- Includes ----------------------------------------------------------------*/
#include <unistd.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include "dap.h"

/*- Definitions -------------------------------------------------------------*/
#define DHCSR                  0xe000edf0
#define DEMCR                  0xe000edfc
#define AIRCR                  0xe000ed0c

#define DSU_CTRL_STATUS        0x41002100
#define DSU_DID                0x41002118

#define NVMCTRL_CTRLA          0x41004000
#define NVMCTRL_CTRLB          0x41004004
#define NVMCTRL_PARAM          0x41004008
#define NVMCTRL_INTFLAG        0x41004014
#define NVMCTRL_STATUS         0x41004018
#define NVMCTRL_ADDR           0x4100401c

#define NVMCTRL_CMD_ER         0xa502
#define NVMCTRL_CMD_WP         0xa504
#define NVMCTRL_CMD_EAR        0xa505
#define NVMCTRL_CMD_WAP        0xa506
#define NVMCTRL_CMD_WL         0xa50f
#define NVMCTRL_CMD_UR         0xa541
#define NVMCTRL_CMD_PBC        0xa544
#define NVMCTRL_CMD_SSB        0xa545

/*- Implementations ---------------------------------------------------------*/

//-----------------------------------------------------------------------------
void dap_target_select(void)
{
  // Stop the core
  dap_write_word(DHCSR, 0xa05f0003);
  dap_write_word(DEMCR, 0x00000001);
  dap_write_word(AIRCR, 0x05fa0004);

  // Enable automatic writes
  dap_write_word(NVMCTRL_CTRLB, 0);
}

//-----------------------------------------------------------------------------
void dap_target_deselect(void)
{
  dap_write_word(DEMCR, 0x00000000);
  dap_write_word(AIRCR, 0x05fa0004);
}

//-----------------------------------------------------------------------------
void dap_target_erase(void)
{
  dap_write_word(DSU_CTRL_STATUS, 0x00001f00); // Clear flags
  dap_write_word(DSU_CTRL_STATUS, 0x00000010); // Chip erase
  while (0 == (dap_read_word(DSU_CTRL_STATUS) & 0x00000100));
}

//-----------------------------------------------------------------------------
void dap_target_lock(void)
{
  dap_write_word(NVMCTRL_CTRLA, NVMCTRL_CMD_SSB); // Set Security Bit
}

//-----------------------------------------------------------------------------
void dap_target_erase_row(uint32_t addr)
{
  dap_write_word(NVMCTRL_ADDR, addr >> 1);

  dap_write_word(NVMCTRL_CTRLA, NVMCTRL_CMD_UR); // Unlock Region
  while (0 == (dap_read_word(NVMCTRL_INTFLAG) & 1));

  dap_write_word(NVMCTRL_CTRLA, NVMCTRL_CMD_ER); // Erase Row
  while (0 == (dap_read_word(NVMCTRL_INTFLAG) & 1));
}

//-----------------------------------------------------------------------------
void dap_target_write_page(uint32_t addr, uint8_t *data)
{
  dap_write_word(NVMCTRL_ADDR, addr >> 1);

  dap_write_word(NVMCTRL_CTRLA, NVMCTRL_CMD_UR); // Unlock Region
  while (0 == (dap_read_word(NVMCTRL_INTFLAG) & 1));

  for (int offs = 0; offs < TARGET_PAGE_SIZE; offs += sizeof(uint32_t))
  {
    uint32_t value =
        ((uint32_t)data[offs + 3] << 24) | ((uint32_t)data[offs + 2] << 16) |
        ((uint32_t)data[offs + 1] << 8) | (uint32_t)data[offs];

    dap_write_word(addr + offs, value);
  }
}

//-----------------------------------------------------------------------------
void dap_target_read_page(uint32_t addr, uint8_t *data)
{
  for (int offs = 0; offs < TARGET_PAGE_SIZE; offs += sizeof(uint32_t))
  {
    uint32_t value = dap_read_word(addr + offs);

    data[offs + 0] = (value >> 0) & 0xff;
    data[offs + 1] = (value >> 8) & 0xff;
    data[offs + 2] = (value >> 16) & 0xff;
    data[offs + 3] = (value >> 24) & 0xff;
  }
}



<|start_filename|>main.c<|end_filename|>
/*
 * Copyright (c) 2015, <NAME> <<EMAIL>>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

//-----------------------------------------------------------------------------
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include "samd21.h"
#include "hal_gpio.h"
#include "dap.h"

//-----------------------------------------------------------------------------
#define PERIOD_FAST     100
#define PERIOD_SLOW     500

HAL_GPIO_PIN(LED,      B, 30)
HAL_GPIO_PIN(BUTTON,   A, 15)
HAL_GPIO_PIN(UART_TX,  A, 22)
HAL_GPIO_PIN(UART_RX,  A, 23)

//-----------------------------------------------------------------------------
static void uart_init(uint32_t baud)
{
  uint64_t br = (uint64_t)65536 * (F_CPU - 16 * baud) / F_CPU;

  HAL_GPIO_UART_TX_out();
  HAL_GPIO_UART_TX_pmuxen(PORT_PMUX_PMUXE_C_Val);
  HAL_GPIO_UART_RX_in();
  HAL_GPIO_UART_RX_pmuxen(PORT_PMUX_PMUXE_C_Val);

  PM->APBCMASK.reg |= PM_APBCMASK_SERCOM3;

  GCLK->CLKCTRL.reg = GCLK_CLKCTRL_ID(SERCOM3_GCLK_ID_CORE) |
      GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN(0);

  SERCOM3->USART.CTRLA.reg =
      SERCOM_USART_CTRLA_DORD | SERCOM_USART_CTRLA_MODE_USART_INT_CLK |
      SERCOM_USART_CTRLA_RXPO(1/*PAD1*/) | SERCOM_USART_CTRLA_TXPO(0/*PAD0*/);

  SERCOM3->USART.CTRLB.reg = SERCOM_USART_CTRLB_RXEN | SERCOM_USART_CTRLB_TXEN |
      SERCOM_USART_CTRLB_CHSIZE(0/*8 bits*/);

  SERCOM3->USART.BAUD.reg = (uint16_t)br;

  SERCOM3->USART.CTRLA.reg |= SERCOM_USART_CTRLA_ENABLE;
}

//-----------------------------------------------------------------------------
void uart_putc(char c)
{
  while (!(SERCOM3->USART.INTFLAG.reg & SERCOM_USART_INTFLAG_DRE));
  SERCOM3->USART.DATA.reg = c;
}

//-----------------------------------------------------------------------------
void uart_puts(char *s)
{
  while (*s)
    uart_putc(*s++);
}

//-----------------------------------------------------------------------------
void uart_puthex(uint32_t v, int size)
{
  char hex[] = "0123456789abcdef";

  for (int i = 0; i < size; i++)
  {
    int offs = ((size - 1) - i) * 4;
    uart_putc(hex[(v >> offs) & 0xf]);
  }
}


//-----------------------------------------------------------------------------
static void sys_init(void)
{
  // Switch to 8MHz clock (disable prescaler)
  SYSCTRL->OSC8M.bit.PRESC = 0;
}

//-----------------------------------------------------------------------------
static void test_swd(void)
{
  dap_init();
  dap_connect();
  dap_reset_link();
  dap_target_prepare();
  dap_target_select();

  uint32_t id = dap_read_idcode();
  uart_puts("\r\nIDCODE = ");
  uart_puthex(id, 8);

  uart_puts("\r\nDSU_DID = ");
  uint32_t dsu_did = dap_read_word(0x41002118/*DSU_DID*/);
  uart_puthex(dsu_did, 8);
  uart_puts("\r\n");

  uart_puts("Erasing entire device... ");
  dap_target_erase();
  uart_puts("done\r\n");


  uint32_t addr = 0x8000;
  uint8_t page_buf[TARGET_PAGE_SIZE];

  uart_puts("Erasing a row device... ");
  dap_target_erase_row(addr);
  uart_puts("done\r\n");

  for (int i = 0; i < TARGET_PAGE_SIZE; i++)
    page_buf[i] = i;

  uart_puts("Writing a page... ");
  dap_target_write_page(addr, page_buf);
  uart_puts("done\r\n");

  for (int i = 0; i < TARGET_PAGE_SIZE; i++) // Clear the buffer for readback test
    page_buf[i] = 0;

  uart_puts("Reading a page... ");
  dap_target_read_page(addr, page_buf);
  uart_puts("done\r\n");

  for (int i = 0; i < TARGET_PAGE_SIZE; i++)
  {
    if (page_buf[i] != i)
    {
      uart_puts("\r\nERROR @ ");
      uart_puthex(i, 2);
      uart_puts("\r\n");
    }
  }

  dap_target_deselect();

  dap_disconnect();
}

//-----------------------------------------------------------------------------
int main(void)
{
  int cnt = 0;

  sys_init();
  uart_init(115200);

  uart_puts("\r\n--- start ---\r\n");

  HAL_GPIO_LED_out();
  HAL_GPIO_LED_clr();

  HAL_GPIO_BUTTON_in();
  HAL_GPIO_BUTTON_pullup();

  while (1)
  {
    if (HAL_GPIO_BUTTON_read())
      cnt = 0;
    else if (cnt < 5001)
      cnt++;

    if (5000 == cnt)
    {
      uart_puts("Test SWD: ");
      test_swd();
      uart_puts("\r\ndone.\r\n");
    }
  }

  return 0;
}


<|start_filename|>dap.c<|end_filename|>
/*
 * Copyright (c) 2014-2016, <NAME> <<EMAIL>>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/*- Includes ----------------------------------------------------------------*/
#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#include "dap_config.h"
#include "dap.h"

/*- Definitions -------------------------------------------------------------*/
#define DAP_IDLE_CYCLES     0
#define DAP_RETRY_COUNT     128
#define DAP_SWD_TURNAROUND  1
#define DAP_SWD_DATA_PHASE  false

enum
{
  DAP_TRANSFER_APnDP        = 1 << 0,
  DAP_TRANSFER_RnW          = 1 << 1,
  DAP_TRANSFER_A2           = 1 << 2,
  DAP_TRANSFER_A3           = 1 << 3,
  DAP_TRANSFER_MATCH_VALUE  = 1 << 4,
  DAP_TRANSFER_MATCH_MASK   = 1 << 5,
};

enum
{
  DAP_TRANSFER_INVALID      = 0,
  DAP_TRANSFER_OK           = 1 << 0,
  DAP_TRANSFER_WAIT         = 1 << 1,
  DAP_TRANSFER_FAULT        = 1 << 2,
  DAP_TRANSFER_ERROR        = 1 << 3,
  DAP_TRANSFER_MISMATCH     = 1 << 4,
};

enum
{
  SWD_DP_R_IDCODE           = 0x00,
  SWD_DP_W_ABORT            = 0x00,
  SWD_DP_R_CTRL_STAT        = 0x04,
  SWD_DP_W_CTRL_STAT        = 0x04, // When CTRLSEL == 0
  SWD_DP_W_WCR              = 0x04, // When CTRLSEL == 1
  SWD_DP_R_RESEND           = 0x08,
  SWD_DP_W_SELECT           = 0x08,
  SWD_DP_R_RDBUFF           = 0x0c,
};

enum
{
  SWD_AP_CSW  = 0x00 | DAP_TRANSFER_APnDP,
  SWD_AP_TAR  = 0x04 | DAP_TRANSFER_APnDP,
  SWD_AP_DRW  = 0x0c | DAP_TRANSFER_APnDP,

  SWD_AP_DB0  = 0x00 | DAP_TRANSFER_APnDP, // 0x10
  SWD_AP_DB1  = 0x04 | DAP_TRANSFER_APnDP, // 0x14
  SWD_AP_DB2  = 0x08 | DAP_TRANSFER_APnDP, // 0x18
  SWD_AP_DB3  = 0x0c | DAP_TRANSFER_APnDP, // 0x1c

  SWD_AP_CFG  = 0x04 | DAP_TRANSFER_APnDP, // 0xf4
  SWD_AP_BASE = 0x08 | DAP_TRANSFER_APnDP, // 0xf8
  SWD_AP_IDR  = 0x0c | DAP_TRANSFER_APnDP, // 0xfc
};

/*- Implementations ---------------------------------------------------------*/

//-----------------------------------------------------------------------------
static inline void dap_delay_loop(void)
{
#ifdef DAP_CONFIG_CLOCK_DELAY
  int delay = DAP_CONFIG_CLOCK_DELAY;

  while (--delay)
    asm("nop");
#endif
}

//-----------------------------------------------------------------------------
static void dap_swd_clock(int cycles)
{
  while (cycles--)
  {
    DAP_CONFIG_SWCLK_clr();
    dap_delay_loop();
    DAP_CONFIG_SWCLK_set();
    dap_delay_loop();
  }
}

//-----------------------------------------------------------------------------
static void dap_swd_write(uint32_t value, int size)
{
  for (int i = 0; i < size; i++)
  {
    DAP_CONFIG_SWDIO_write(value & 1);
    DAP_CONFIG_SWCLK_clr();
    dap_delay_loop();
    DAP_CONFIG_SWCLK_set();
    dap_delay_loop();
    value >>= 1;
  }
}

//-----------------------------------------------------------------------------
static uint32_t dap_swd_read(int size)
{
  uint32_t value = 0;

  for (int i = 0; i < size; i++)
  {
    DAP_CONFIG_SWCLK_clr();
    dap_delay_loop();
    value |= ((uint32_t)DAP_CONFIG_SWDIO_read() << i);
    DAP_CONFIG_SWCLK_set();
    dap_delay_loop();
  }

  return value;
}

//-----------------------------------------------------------------------------
static inline uint32_t dap_parity(uint32_t value)
{
  value ^= value >> 16;
  value ^= value >> 8;
  value ^= value >> 4;
  value &= 0x0f;

  return (0x6996 >> value) & 1;
}

//-----------------------------------------------------------------------------
static int dap_swd_operation(int req, uint32_t *data)
{
  uint32_t value;
  int ack = 0;

  dap_swd_write(0x81 | (dap_parity(req) << 5) | (req << 1), 8);

  DAP_CONFIG_SWDIO_in();

  dap_swd_clock(DAP_SWD_TURNAROUND);

  ack = dap_swd_read(3);

  if (DAP_TRANSFER_OK == ack)
  {
    if (req & DAP_TRANSFER_RnW)
    {
      value = dap_swd_read(32);

      if (dap_parity(value) != dap_swd_read(1))
        ack = DAP_TRANSFER_ERROR;

      if (data)
        *data = value;

      dap_swd_clock(DAP_SWD_TURNAROUND);

      DAP_CONFIG_SWDIO_out();
    }
    else
    {
      dap_swd_clock(DAP_SWD_TURNAROUND);

      DAP_CONFIG_SWDIO_out();

      dap_swd_write(*data, 32);
      dap_swd_write(dap_parity(*data), 1);
    }

    DAP_CONFIG_SWDIO_write(0);
    dap_swd_clock(DAP_IDLE_CYCLES);
  }

  else if (DAP_TRANSFER_WAIT == ack || DAP_TRANSFER_FAULT == ack)
  {
    if (DAP_SWD_DATA_PHASE && (req & DAP_TRANSFER_RnW))
      dap_swd_clock(32 + 1);

    dap_swd_clock(DAP_SWD_TURNAROUND);

    DAP_CONFIG_SWDIO_out();

    if (DAP_SWD_DATA_PHASE && (0 == (req & DAP_TRANSFER_RnW)))
    {
      DAP_CONFIG_SWDIO_write(0);
      dap_swd_clock(32 + 1);
    }
  }

  else
  {
    dap_swd_clock(DAP_SWD_TURNAROUND + 32 + 1);
  }

  DAP_CONFIG_SWDIO_write(1);

  return ack;
}

//-----------------------------------------------------------------------------
static int dap_swd_transfer_word(int req, uint32_t *data)
{
  int ack;

  req &= (DAP_TRANSFER_APnDP | DAP_TRANSFER_RnW | DAP_TRANSFER_A2 | DAP_TRANSFER_A3);

  for (int i = 0; i < DAP_RETRY_COUNT; i++)
  {
    ack = dap_swd_operation(req, data);

    if (DAP_TRANSFER_WAIT != ack)
      break;
  }

  return ack;
}

//-----------------------------------------------------------------------------
static int dap_swd_transfer(int request, uint32_t *data)
{
  int ack = DAP_TRANSFER_INVALID;
  bool posted_read = false;
  bool verify_write = false;

  if (request & DAP_TRANSFER_RnW)
  {
    if (request & DAP_TRANSFER_APnDP)
    {
      ack = dap_swd_transfer_word(request, NULL);

      if (ack != DAP_TRANSFER_OK)
        return ack;

      posted_read = true;
    }
    else
    {
      ack = dap_swd_transfer_word(request, data);

      if (DAP_TRANSFER_OK != ack)
        return ack;
    }
  }
  else
  {
    ack = dap_swd_transfer_word(request, data);

    if (ack != DAP_TRANSFER_OK)
      return ack;

    verify_write = true;
  }

  if (DAP_TRANSFER_OK == ack)
  {
    if (posted_read)
      ack = dap_swd_transfer_word(SWD_DP_R_RDBUFF | DAP_TRANSFER_RnW, data);

    else if (verify_write)
      ack = dap_swd_transfer_word(SWD_DP_R_RDBUFF | DAP_TRANSFER_RnW, NULL);
  }

  return ack;
}

//-----------------------------------------------------------------------------
static void dap_swj_sequence(int size, uint8_t *data)
{
  int offset = 0;

  while (size)
  {
    int sz = (size > 8) ? 8 : size;

    dap_swd_write(data[offset], sz);

    size -= sz;
    offset++;
  }
}

//-----------------------------------------------------------------------------
static bool dap_read_reg(uint8_t reg, uint32_t *value)
{
  return (DAP_TRANSFER_OK == dap_swd_transfer(reg | DAP_TRANSFER_RnW, value));
}

//-----------------------------------------------------------------------------
static bool dap_write_reg(uint8_t reg, uint32_t value)
{
  return (DAP_TRANSFER_OK == dap_swd_transfer(reg, &value));
}

//-----------------------------------------------------------------------------
void dap_init(void)
{
  DAP_CONFIG_SETUP();
}

//-----------------------------------------------------------------------------
void dap_connect(void)
{
  DAP_CONFIG_CONNECT_SWD();
}

//-----------------------------------------------------------------------------
void dap_disconnect(void)
{
  DAP_CONFIG_DISCONNECT();
}

//-----------------------------------------------------------------------------
uint32_t dap_read_word(uint32_t addr)
{
  uint32_t data;

  if (!dap_write_reg(SWD_AP_TAR, addr))
    return 0;

  if (!dap_read_reg(SWD_AP_DRW, &data))
    return 0;

  return data;
}

//-----------------------------------------------------------------------------
void dap_write_word(uint32_t addr, uint32_t data)
{
  dap_write_reg(SWD_AP_TAR, addr);
  dap_write_reg(SWD_AP_DRW, data);
}

//-----------------------------------------------------------------------------
void dap_reset_link(void)
{
  uint8_t buf[20];
  uint32_t idcode;

  buf[0] = 0xff;
  buf[1] = 0xff;
  buf[2] = 0xff;
  buf[3] = 0xff;
  buf[4] = 0xff;
  buf[5] = 0xff;
  buf[6] = 0xff;
  buf[7] = 0x9e;
  buf[8] = 0xe7;
  buf[9] = 0xff;
  buf[10] = 0xff;
  buf[11] = 0xff;
  buf[12] = 0xff;
  buf[13] = 0xff;
  buf[14] = 0xff;
  buf[15] = 0xff;
  buf[16] = 0x00;

  dap_swj_sequence((7 + 2 + 7 + 1) * 8, buf);

  dap_read_reg(SWD_DP_R_IDCODE, &idcode);
}

//-----------------------------------------------------------------------------
uint32_t dap_read_idcode(void)
{
  uint32_t idcode;

  if (dap_read_reg(SWD_DP_R_IDCODE, &idcode))
    return idcode;

  return 0xffffffff;
}

//-----------------------------------------------------------------------------
void dap_target_prepare(void)
{
  dap_write_reg(SWD_DP_W_ABORT, 0x00000016);
  dap_write_reg(SWD_DP_W_SELECT, 0x00000000);
  dap_write_reg(SWD_DP_W_CTRL_STAT, 0x50000f00);
  dap_write_reg(SWD_AP_CSW, 0x23000052);
}

