IBDAP-CMSIS-DAP

IBDAP-CMSIS-DAP

 

Armstart's CMSIS-DAP firmware implementation in gcc and makefile.

http://www.armstart.com

IBDAP is a fully CMSIS-DAP compatible debug adapter.

It provides vendor independent debug interface between your PC over USB and target ARM device over JTAG/SWD pins.

You can do debugging functions like stepping, breakpoints, watch points and firmware download etc.

It's fully supported by Keil, OpenOCD, GNU GDB, IAR and other commonly used debugging tools.

Why IBDAP?

Debug adapters are expensive, some could cost thousands of dollars, some may not be compatible among different vendors.

Luckily, ARM standardized the debugging interface which is called CMSIS-DAP and released the firmware implementation on some processors,

however, you still need a Keil MDK Professional edition software in order to build the firmware,

even the open source one provided by mbed and the price for Keil Professional is intimidating.

All these barriers has become the first issue that every inventor is facing,

and we need a solution! IBDAP's objective is to become an affordable open source

& open hardware CMSIS-DAP JTAG/SWD debug probe implementation using gcc & makefile.

Anyone can modify and embed a debug probe on its own device easily with everything under its control.

Besides being functioning as a JTAG/SWD debug probe, IBDAP could also be used as a general development board.

It has an ARM Cortex M0 processor running at 48Mhz and has peripherals like UART, I2C, SPI, USB.

It can be used in many applications like USB audio devices, USB mouse/keyboards, USB mass storage devices,

USB-TTL adapter device and many many more.

Moreover, the 10-bit high precision ADC peripheral also makes IBDAP an ideal device for any sensor projects.

IBDAP-CMSIS-DAP_第1张图片

OpenOCD Tutorial

install deps

sudo apt-get install build-essential

sudo apt-get install autotools-dev autoconf automake libtool

sudo apt-get install texinfo libudev-dev libusb-1.0-0-dev libfox-1.6-dev

Download FTDI driver

http://www.intra2net.com/en/developer/libftdi/download.php
tar xf libftdi1-1.2.tar.bz2

HIDAPI

git clone git://github.com/signal11/hidapi.git
./bootstrap
./configure
make
sudo make install

Build and Install OpenOCD

git clone git://git.code.sf.net/p/openocd/code
cd code
./bootstrap
cd ../
mkdir ./openocd-build
cd openocd-build
../code/configure --with-ftd2xx-linux-tardir=../libftdi1-1.2 --prefix=/opt/openocd --enable-jlink --enable-ftdi --enable-stlink --enable-ti-icdi --enable-ulink --enable-usb-blaster-2 --enable-cmsis-dap --enable-bcm2835gpio
make
make install

Usage

Prepare extra OpenOCD cfg commands

create file openocd.cfg with following contents:

$_TARGETNAME configure -event gdb-attach {
   echo "Halting target"
   halt
}

Start OpenOCD server to Debug LPC11U35

sudo openocd -f interface/jlink.cfg -c "transport select swd" -f target/lpc11xx.cfg -f openocd.cfg 

Flash and Debug

  • First, flash debug firmware on board
telnet localhost 4444
reset halt
flash probe 0
flash write_image erase /home/yliu/projects/ib51822/IBDAP-FW/Firmware/IBDAP/Debug/IBDAP.bin 0x0
reset
exit
  • Second, debug using the commands:
target remote localhost:3333
break IBDAP.c:103
monitor reset halt
monitor reset init
continue

http://wiki.paparazziuav.org/wiki/DevGuide/GDB_OpenOCD_Debug

Breakpoint limits

set remote hardware-breakpoint-limit 4
set remote hardware-watchpoint-limit 2

gdbtui GUI debugging

http://fun-tech.se/stm32/OpenOCD/gdb.php

OpenOCD nRF51822 using IBDAP

Launch OpenOCD

$ sudo openocd -f interface/cmsis-dap.cfg -c "transport select swd" -f target/nrf51.cfg

nrf OpenOCD log

yliu@yliu-xbtu:~$ 
yliu@yliu-xbtu:~$ sudo openocd -f interface/cmsis-dap.cfg -c "transport select swd" -f target/nrf51.cfg
[sudo] password for yliu: 
Open On-Chip Debugger 0.10.0-dev-00001-g70a14db (2015-06-23-16:35)
Licensed under GNU GPL v2
For bug reports, read
    http://openocd.org/doc/doxygen/bugs.html
Info : only one transport option; autoselect 'swd'
Warn : Transport "swd" was already selected
swd
cortex_m reset_config sysresetreq
adapter speed: 1000 kHz
Info : CMSIS-DAP: SWD  Supported
Info : CMSIS-DAP: JTAG Supported
Info : CMSIS-DAP: Interface Initialised (SWD)
Info : CMSIS-DAP: FW Version = 1.0
Info : SWCLK/TCK = 1 SWDIO/TMS = 1 TDI = 1 TDO = 1 nTRST = 0 nRESET = 1
Info : CMSIS-DAP: Interface ready
Info : clock speed 1000 kHz
Info : SWD IDCODE 0x0bb11477
Info : nrf51.cpu: hardware has 4 breakpoints, 2 watchpoints
Info : accepting 'telnet' connection on tcp/4444
yliu@yliu-xbtu:~$ telnet localhost 4444
Trying 127.0.0.1...
Connected to localhost.
Escape character is '^]'.
Open On-Chip Debugger
> reset halt
target state: halted
target halted due to debug-request, current mode: Thread 
xPSR: 0xc1000000 pc: 0x000006d0 msp: 0x000007c0
> flash probe 0
Unknown device (HWID 0x00000057)
flash 'nrf51' found at 0x00000000


/**************************************************************************//**
 * @file     DAP_config.h
 * @brief    IBDAP DAP config for LPC11U35
 * @version  V0.1
 * @date     08. May 2015
 *
 * @note
 * Copyright (C) 2015 Armstart. All rights reserved.
 *
 * @par
 *
 * @par
 *
 ******************************************************************************/

#ifndef __DAP_CONFIG_H__
#define __DAP_CONFIG_H__


//**************************************************************************************************
/**
\defgroup DAP_Config_Debug_gr CMSIS-DAP Debug Unit Information
\ingroup DAP_ConfigIO_gr
@{
Provides definitions about:
 - Definition of Cortex-M processor parameters used in CMSIS-DAP Debug Unit.
 - Debug Unit communication packet size.
 - Debug Access Port communication mode (JTAG or SWD).
 - Optional information about a connected Target Device (for Evaluation Boards).
*/

#ifdef __USE_CMSIS
#include "LPC11Uxx.h"
#endif

#include "compiler.h"

/// Processor Clock of the Cortex-M MCU used in the Debug Unit.
/// This value is used to calculate the SWD/JTAG clock speed.
#define CPU_CLOCK               48000000       ///< Specifies the CPU Clock in Hz

/// Number of processor cycles for I/O Port write operations.
/// This value is used to calculate the SWD/JTAG clock speed that is generated with I/O
/// Port write operations in the Debug Unit by a Cortex-M MCU. Most Cortex-M processors
/// requrie 2 processor cycles for a I/O Port Write operation.  If the Debug Unit uses
/// a Cortex-M0+ processor with high-speed peripheral I/O only 1 processor cycle might be
/// requrired.
#define IO_PORT_WRITE_CYCLES    2               ///< I/O Cycles: 2=default, 1=Cortex-M0+ fast I/0

/// Indicate that Serial Wire Debug (SWD) communication mode is available at the Debug Access Port.
/// This information is returned by the command \ref DAP_Info as part of Capabilities.
#define DAP_SWD                 1               ///< SWD Mode:  1 = available, 0 = not available

/// Indicate that JTAG communication mode is available at the Debug Port.
/// This information is returned by the command \ref DAP_Info as part of Capabilities.
#define DAP_JTAG                1               ///< JTAG Mode: 1 = available, 0 = not available.

/// Configure maximum number of JTAG devices on the scan chain connected to the Debug Access Port.
/// This setting impacts the RAM requirements of the Debug Unit. Valid range is 1 .. 255.
#define DAP_JTAG_DEV_CNT        8               ///< Maximum number of JTAG devices on scan chain

/// Default communication mode on the Debug Access Port.
/// Used for the command \ref DAP_Connect when Port Default mode is selected.
#define DAP_DEFAULT_PORT        1               ///< Default JTAG/SWJ Port Mode: 1 = SWD, 2 = JTAG.

/// Default communication speed on the Debug Access Port for SWD and JTAG mode.
/// Used to initialize the default SWD/JTAG clock frequency.
/// The command \ref DAP_SWJ_Clock can be used to overwrite this default setting.
#define DAP_DEFAULT_SWJ_CLOCK   5000000         ///< Default SWD/JTAG clock frequency in Hz.

/// Maximum Package Size for Command and Response data.
/// This configuration settings is used to optimized the communication performance with the
/// debugger and depends on the USB peripheral. Change setting to 1024 for High-Speed USB.
#define DAP_PACKET_SIZE         64            ///< USB: 64 = Full-Speed, 1024 = High-Speed.

/// Maximum Package Buffers for Command and Response data.
/// This configuration settings is used to optimized the communication performance with the
/// debugger and depends on the USB peripheral. For devices with limited RAM or USB buffer the
/// setting can be reduced (valid range is 1 .. 255). Change setting to 4 for High-Speed USB.
#define DAP_PACKET_COUNT        8               ///< Buffers: 64 = Full-Speed, 4 = High-Speed.


/// Debug Unit is connected to fixed Target Device.
/// The Debug Unit may be part of an evaluation board and always connected to a fixed
/// known device.  In this case a Device Vendor and Device Name string is stored which
/// may be used by the debugger or IDE to configure device parameters.
#define TARGET_DEVICE_FIXED     0               ///< Target Device: 1 = known, 0 = unknown;

#if TARGET_DEVICE_FIXED
#define TARGET_DEVICE_VENDOR    "ARMSTART"              ///< String indicating the Silicon Vendor
#define TARGET_DEVICE_NAME      "IBDAP-LPC11U35 CMSIS-DAP"              ///< String indicating the Target Device
#endif

///@}


// Peripheral register bit masks (used for pin inits)
#define FUNC_0                    0
#define FUNC_1                    1
#define PULL_DOWN_ENABLED        (1 << 3)
#define PULL_UP_ENABLED            (2 << 3)
#define OPENDRAIN                (1 << 10)

// Debug Port I/O Pins
// For LPC11Uxx DAPs all SWD and JTAG pins are on GPIO port 0
// Default is mbed HDK reference design with LPC11U35/501
// SWCLK/TCK Pin                PIO0_7
#define PIN_SWCLK_IN_BIT        7
#define PIN_SWCLK               (1 << PIN_SWCLK_IN_BIT)
#define PIN_SWCLK_TCK_IOCON     LPC_IOCON->PIO0_7

// SWDIO/TMS In/Out Pin         PIO0_8
#define PIN_SWDIO_IN_BIT        8
#define PIN_SWDIO               (1 << PIN_SWDIO_IN_BIT)
#define PIN_SWDIO_TMS_IOCON     LPC_IOCON->PIO0_8

// nRESET Pin                   PIO0_2
#define PIN_nRESET_IN_BIT       2
#define PIN_nRESET              (1 << PIN_nRESET_IN_BIT)
#define PIN_nRESET_IOCON        LPC_IOCON->PIO0_2

#if (DAP_JTAG != 0)

// TDI Pin                      PIO0_17
#define PIN_TDI_IN_BIT          17
#define PIN_TDI                 (1 << PIN_TDI_IN_BIT)
#define PIN_TDI_IOCON           LPC_IOCON->PIO0_17

// SWO/TDO Pin                  PIO0_9
#define PIN_TDO_IN_BIT          9
#define PIN_TDO                 (1 << PIN_TDO_IN_BIT)
#define PIN_TDO_IOCON           LPC_IOCON->PIO0_9
#endif // (DAP_JTAG != 0)

//**************************************************************************************************
/**
\defgroup DAP_Config_PortIO_gr CMSIS-DAP Hardware I/O Pin Access
\ingroup DAP_ConfigIO_gr
@{

Standard I/O Pins of the CMSIS-DAP Hardware Debug Port support standard JTAG mode
and Serial Wire Debug (SWD) mode. In SWD mode only 2 pins are required to implement the debug
interface of a device. The following I/O Pins are provided:

JTAG I/O Pin                 | SWD I/O Pin          | CMSIS-DAP Hardware pin mode
---------------------------- | -------------------- | ---------------------------------------------
TCK: Test Clock              | SWCLK: Clock         | Output Push/Pull
TMS: Test Mode Select        | SWDIO: Data I/O      | Output Push/Pull; Input (for receiving data)
TDI: Test Data Input         |                      | Output Push/Pull
TDO: Test Data Output        |                      | Input
nTRST: Test Reset (optional) |                      | Output Open Drain with pull-up resistor
nRESET: Device Reset         | nRESET: Device Reset | Output Open Drain with pull-up resistor


DAP Hardware I/O Pin Access Functions
-------------------------------------
The various I/O Pins are accessed by functions that implement the Read, Write, Set, or Clear to
these I/O Pins.

For the SWDIO I/O Pin there are additional functions that are called in SWD I/O mode only.
This functions are provided to achieve faster I/O that is possible with some advanced GPIO
peripherals that can independently write/read a single I/O pin without affecting any other pins
of the same I/O port. The following SWDIO I/O Pin functions are provided:
 - \ref PIN_SWDIO_OUT_ENABLE to enable the output mode from the DAP hardware.
 - \ref PIN_SWDIO_OUT_DISABLE to enable the input mode to the DAP hardware.
 - \ref PIN_SWDIO_IN to read from the SWDIO I/O pin with utmost possible speed.
 - \ref PIN_SWDIO_OUT to write to the SWDIO I/O pin with utmost possible speed.
*/


// Configure DAP I/O pins ------------------------------

/** Setup JTAG I/O pins: TCK, TMS, TDI, TDO, nTRST, and nRESET.
Configures the DAP Hardware I/O pins for JTAG mode:
 - TCK, TMS, TDI, nTRST, nRESET to output mode and set to high level.
 - TDO to input mode.
*/
static __inline void PORT_JTAG_SETUP (void) {
#if (DAP_JTAG != 0)
    LPC_GPIO->SET[0]  =  PIN_TDI;
    LPC_GPIO->DIR[0] |=  PIN_TDI;
    LPC_GPIO->DIR[0] &= ~PIN_TDO;
#endif
}
 
/** Setup SWD I/O pins: SWCLK, SWDIO, and nRESET.
Configures the DAP Hardware I/O pins for Serial Wire Debug (SWD) mode:
 - SWCLK, SWDIO, nRESET to output mode and set to default high level.
 - TDI, TMS, nTRST to HighZ mode (pins are unused in SWD mode).
*/
static __inline void PORT_SWD_SETUP (void) {
    LPC_GPIO->SET[0] = PIN_SWCLK;
    LPC_GPIO->SET[0] = PIN_SWDIO;
#if defined(CONF_OPENDRAIN)
    // open drain logic
    LPC_GPIO->DIR[0] &= ~PIN_nRESET;
    LPC_GPIO->CLR[0]  =  PIN_nRESET; 
    LPC_GPIO->DIR[0] |= (PIN_SWCLK | PIN_SWDIO);
#else
    LPC_GPIO->SET[0] = PIN_nRESET;
    LPC_GPIO->DIR[0]  |= (PIN_SWCLK | PIN_SWDIO | PIN_nRESET);
#endif
}

/** Disable JTAG/SWD I/O Pins.
Disables the DAP Hardware I/O pins which configures:
 - TCK/SWCLK, TMS/SWDIO, TDI, TDO, nTRST, nRESET to High-Z mode.
*/
static __inline void PORT_OFF (void) {
    LPC_GPIO->CLR[0] = PIN_SWCLK;
    LPC_GPIO->CLR[0] = PIN_SWDIO;
#if defined(CONF_OPENDRAIN)
    // open drain logic
    LPC_GPIO->DIR[0] &= ~PIN_nRESET; // reset not an output
    LPC_GPIO->CLR[0]  =  PIN_nRESET;
    LPC_GPIO->DIR[0] |= (PIN_SWCLK | PIN_SWDIO); 
#else
    LPC_GPIO->SET[0] = PIN_nRESET;
    LPC_GPIO->DIR[0] |= (PIN_SWCLK | PIN_SWDIO | PIN_nRESET);
#endif
}


// SWCLK/TCK I/O pin -------------------------------------

/** SWCLK/TCK I/O pin: Get Input.
\return Current status of the SWCLK/TCK DAP hardware I/O pin.
*/
static __forceinline uint32_t PIN_SWCLK_TCK_IN  (void) {
    return LPC_GPIO->B[PIN_SWCLK_IN_BIT] & 0x1;
}

/** SWCLK/TCK I/O pin: Set Output to High.
Set the SWCLK/TCK DAP hardware I/O pin to high level.
*/
static __forceinline void     PIN_SWCLK_TCK_SET (void) {
    LPC_GPIO->SET[0] = (PIN_SWCLK);
}

/** SWCLK/TCK I/O pin: Set Output to Low.
Set the SWCLK/TCK DAP hardware I/O pin to low level.
*/
static __forceinline void     PIN_SWCLK_TCK_CLR (void) {
    LPC_GPIO->CLR[0] = (PIN_SWCLK);
}


// SWDIO/TMS Pin I/O --------------------------------------

/** SWDIO/TMS I/O pin: Get Input.
\return Current status of the SWDIO/TMS DAP hardware I/O pin.
*/
static __forceinline uint32_t PIN_SWDIO_TMS_IN  (void) {
    return LPC_GPIO->B[PIN_SWDIO_IN_BIT] & 0x1;
}

/** SWDIO/TMS I/O pin: Set Output to High.
Set the SWDIO/TMS DAP hardware I/O pin to high level.
*/
static __forceinline void     PIN_SWDIO_TMS_SET (void) {
    LPC_GPIO->SET[0] = (PIN_SWDIO);
}

/** SWDIO/TMS I/O pin: Set Output to Low.
Set the SWDIO/TMS DAP hardware I/O pin to low level.
*/
static __forceinline void     PIN_SWDIO_TMS_CLR (void) {
    LPC_GPIO->CLR[0] = (PIN_SWDIO);
}

/** SWDIO I/O pin: Get Input (used in SWD mode only).
\return Current status of the SWDIO DAP hardware I/O pin.
*/
static __forceinline uint32_t PIN_SWDIO_IN      (void) {
    return LPC_GPIO->B[PIN_SWDIO_IN_BIT] & 0x1;
}

/** SWDIO I/O pin: Set Output (used in SWD mode only).
\param bit Output value for the SWDIO DAP hardware I/O pin.
*/
static __forceinline void     PIN_SWDIO_OUT     (uint32_t bit){
    if (bit & 0x1)
        LPC_GPIO->SET[0] = (PIN_SWDIO);
    else
        LPC_GPIO->CLR[0] = (PIN_SWDIO);
}

/** SWDIO I/O pin: Switch to Output mode (used in SWD mode only).
Configure the SWDIO DAP hardware I/O pin to output mode. This function is
called prior \ref PIN_SWDIO_OUT function calls.
*/
static __forceinline void     PIN_SWDIO_OUT_ENABLE  (void) {
    LPC_GPIO->DIR[0]  |= (PIN_SWDIO);
}

/** SWDIO I/O pin: Switch to Input mode (used in SWD mode only).
Configure the SWDIO DAP hardware I/O pin to input mode. This function is
called prior \ref PIN_SWDIO_IN function calls.
*/
static __forceinline void     PIN_SWDIO_OUT_DISABLE (void) {
    LPC_GPIO->DIR[0]  &= ~(PIN_SWDIO);
}


// TDI Pin I/O ---------------------------------------------

/** TDI I/O pin: Get Input.
\return Current status of the TDI DAP hardware I/O pin.
*/
static __forceinline uint32_t PIN_TDI_IN  (void) {
#if (DAP_JTAG != 0)
    return LPC_GPIO->B[PIN_TDI_IN_BIT] & 0x1;
#else
  return (0);   // Not available
#endif
}

/** TDI I/O pin: Set Output.
\param bit Output value for the TDI DAP hardware I/O pin.
*/
static __forceinline void     PIN_TDI_OUT (uint32_t bit) {
#if (DAP_JTAG != 0)
    if (bit & 0x1)
        LPC_GPIO->SET[0] = (PIN_TDI);
    else
        LPC_GPIO->CLR[0] = (PIN_TDI);
#else
  ;             // Not available
#endif
}


// TDO Pin I/O ---------------------------------------------

/** TDO I/O pin: Get Input.
\return Current status of the TDO DAP hardware I/O pin.
*/
static __forceinline uint32_t PIN_TDO_IN  (void) {
#if (DAP_JTAG != 0)
    return LPC_GPIO->B[PIN_TDO_IN_BIT] & 0x1;
#else
  return (0);   // Not available
#endif
}


// nTRST Pin I/O -------------------------------------------

/** nTRST I/O pin: Get Input.
\return Current status of the nTRST DAP hardware I/O pin.
*/
static __forceinline uint32_t PIN_nTRST_IN   (void) {
  return (0);   // Not available
}

/** nTRST I/O pin: Set Output.
\param bit JTAG TRST Test Reset pin status:
           - 0: issue a JTAG TRST Test Reset.
           - 1: release JTAG TRST Test Reset.
*/
static __forceinline void     PIN_nTRST_OUT  (uint32_t bit) {
  ;             // Not available
}

// nRESET Pin I/O------------------------------------------

/** nRESET I/O pin: Get Input.
\return Current status of the nRESET DAP hardware I/O pin.
*/
static __forceinline uint32_t PIN_nRESET_IN  (void) {
    return LPC_GPIO->B[PIN_nRESET_IN_BIT] & 0x1;
}

/** nRESET I/O pin: Set Output.
\param bit target device hardware reset pin status:
           - 0: issue a device hardware reset.
           - 1: release device hardware reset.
*/
static __forceinline void     PIN_nRESET_OUT (uint32_t bit) {
#if defined(CONF_OPENDRAIN)
    // open drain logic
    if (bit) LPC_GPIO->DIR[0] &= ~PIN_nRESET; // input (pulled high external)
    else     LPC_GPIO->DIR[0] |=  PIN_nRESET; // output (low)
#else
    if (bit)
        LPC_GPIO->SET[0] = (PIN_nRESET);
    else
        LPC_GPIO->CLR[0] = (PIN_nRESET);
#endif
}

///@}


//**************************************************************************************************
/**
\defgroup DAP_Config_LEDs_gr CMSIS-DAP Hardware Status LEDs
\ingroup DAP_ConfigIO_gr
@{

CMSIS-DAP Hardware may provide LEDs that indicate the status of the CMSIS-DAP Debug Unit.

It is recommended to provide the following LEDs for status indication:
 - Connect LED: is active when the DAP hardware is connected to a debugger.
 - Running LED: is active when the debugger has put the target device into running state.
*/

/** Debug Unit: Set status of Connected LED.
\param bit status of the Connect LED.
           - 1: Connect LED ON: debugger is connected to CMSIS-DAP Debug Unit.
           - 0: Connect LED OFF: debugger is not connected to CMSIS-DAP Debug Unit.
*/
static __inline void LED_CONNECTED_OUT (uint32_t bit) {
}

/** Debug Unit: Set status Target Running LED.
\param bit status of the Target Running LED.
           - 1: Target Running LED ON: program execution in target started.
           - 0: Target Running LED OFF: program execution in target stopped.
*/
static __inline void LED_RUNNING_OUT (uint32_t bit) {
  ;             // Not available
}

///@}


//**************************************************************************************************
/**
\defgroup DAP_Config_Initialization_gr CMSIS-DAP Initialization
\ingroup DAP_ConfigIO_gr
@{

CMSIS-DAP Hardware I/O and LED Pins are initialized with the function \ref DAP_SETUP.
*/

/** Setup of the Debug Unit I/O pins and LEDs (called when Debug Unit is initialized).
This function performs the initialization of the CMSIS-DAP Hardware I/O Pins and the
Status LEDs. In detail the operation of Hardware I/O and LED pins are enabled and set:
 - I/O clock system enabled.
 - all I/O pins: input buffer enabled, output pins are set to HighZ mode.
 - for nTRST, nRESET a weak pull-up (if available) is enabled.
 - LED output pins are enabled and LEDs are turned off.
*/
static __inline void DAP_SETUP (void) {
    // Configure I/O pins
    PIN_SWCLK_TCK_IOCON = FUNC_0 | PULL_UP_ENABLED;  // SWCLK/TCK
    PIN_SWDIO_TMS_IOCON = FUNC_0 | PULL_UP_ENABLED;  // SWDIO/TMS
#if !defined(CONF_OPENDRAIN)
    PIN_nRESET_IOCON    = FUNC_0 | PULL_UP_ENABLED;  // nRESET
#else
    PIN_nRESET_IOCON    = FUNC_0 | OPENDRAIN;        // nRESET
#endif
#if (DAP_JTAG != 0)
    PIN_TDI_IOCON       = FUNC_0 | PULL_UP_ENABLED;  // TDI
    PIN_TDO_IOCON       = FUNC_0 | PULL_UP_ENABLED;  // TDO
#endif
}

/** Reset Target Device with custom specific I/O pin or command sequence.
This function allows the optional implementation of a device specific reset sequence.
It is called when the command \ref DAP_ResetTarget and is for example required
when a device needs a time-critical unlock sequence that enables the debug port.
\return 0 = no device specific reset sequence is implemented.\n
        1 = a device specific reset sequence is implemented.
*/
static __inline uint32_t RESET_TARGET (void) {
  return (0);              // change to '1' when a device reset sequence is implemented
}

///@}


#endif /* __DAP_CONFIG_H__ */

 

/******************************************************************************
 * @file     DAP.h
 * @brief    CMSIS-DAP Definitions
 * @version  V1.00
 * @date     31. May 2012
 *
 * @note
 * Copyright (C) 2012 ARM Limited. All rights reserved.
 *
 * @par
 * ARM Limited (ARM) is supplying this software for use with Cortex-M
 * processor based microcontrollers.
 *
 * @par
 * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
 * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
 * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
 * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
 *
 ******************************************************************************/

#ifndef __DAP_H__
#define __DAP_H__


// DAP Command IDs
#define ID_DAP_Info                     0x00
#define ID_DAP_LED                      0x01
#define ID_DAP_Connect                  0x02
#define ID_DAP_Disconnect               0x03
#define ID_DAP_TransferConfigure        0x04
#define ID_DAP_Transfer                 0x05
#define ID_DAP_TransferBlock            0x06
#define ID_DAP_TransferAbort            0x07
#define ID_DAP_WriteABORT               0x08
#define ID_DAP_Delay                    0x09
#define ID_DAP_ResetTarget              0x0A
#define ID_DAP_SWJ_Pins                 0x10
#define ID_DAP_SWJ_Clock                0x11
#define ID_DAP_SWJ_Sequence             0x12
#define ID_DAP_SWD_Configure            0x13
#define ID_DAP_JTAG_Sequence            0x14
#define ID_DAP_JTAG_Configure           0x15
#define ID_DAP_JTAG_IDCODE              0x16

// DAP Vendor Command IDs
#define ID_DAP_Vendor0                  0x80
#define ID_DAP_Vendor1                  0x81
#define ID_DAP_Vendor2                  0x82
#define ID_DAP_Vendor3                  0x83
#define ID_DAP_Vendor4                  0x84
#define ID_DAP_Vendor5                  0x85
#define ID_DAP_Vendor6                  0x86
#define ID_DAP_Vendor7                  0x87
#define ID_DAP_Vendor8                  0x88
#define ID_DAP_Vendor9                  0x89
#define ID_DAP_Vendor10                 0x8A
#define ID_DAP_Vendor11                 0x8B
#define ID_DAP_Vendor12                 0x8C
#define ID_DAP_Vendor13                 0x8D
#define ID_DAP_Vendor14                 0x8E
#define ID_DAP_Vendor15                 0x8F
#define ID_DAP_Vendor16                 0x90
#define ID_DAP_Vendor17                 0x91
#define ID_DAP_Vendor18                 0x92
#define ID_DAP_Vendor19                 0x93
#define ID_DAP_Vendor20                 0x94
#define ID_DAP_Vendor21                 0x95
#define ID_DAP_Vendor22                 0x96
#define ID_DAP_Vendor23                 0x97
#define ID_DAP_Vendor24                 0x98
#define ID_DAP_Vendor25                 0x99
#define ID_DAP_Vendor26                 0x9A
#define ID_DAP_Vendor27                 0x9B
#define ID_DAP_Vendor28                 0x9C
#define ID_DAP_Vendor29                 0x9D
#define ID_DAP_Vendor30                 0x9E
#define ID_DAP_Vendor31                 0x9F

#define ID_DAP_Invalid                  0xFF

// DAP Status Code
#define DAP_OK                          0
#define DAP_ERROR                       0xFF

// DAP ID
#define DAP_ID_VENDOR                   1
#define DAP_ID_PRODUCT                  2
#define DAP_ID_SER_NUM                  3
#define DAP_ID_FW_VER                   4
#define DAP_ID_DEVICE_VENDOR            5
#define DAP_ID_DEVICE_NAME              6
#define DAP_ID_CAPABILITIES             0xF0
#define DAP_ID_PACKET_COUNT             0xFE
#define DAP_ID_PACKET_SIZE              0xFF

// DAP LEDs
#define DAP_LED_DEBUGGER_CONNECTED      0
#define DAP_LED_TARGET_RUNNING          1

// DAP Port
#define DAP_PORT_AUTODETECT             0       // Autodetect Port
#define DAP_PORT_DISABLED               0       // Port Disabled (I/O pins in High-Z)
#define DAP_PORT_SWD                    1       // SWD Port (SWCLK, SWDIO) + nRESET
#define DAP_PORT_JTAG                   2       // JTAG Port (TCK, TMS, TDI, TDO, nTRST) + nRESET

// DAP SWJ Pins
#define DAP_SWJ_SWCLK_TCK               0       // SWCLK/TCK
#define DAP_SWJ_SWDIO_TMS               1       // SWDIO/TMS
#define DAP_SWJ_TDI                     2       // TDI
#define DAP_SWJ_TDO                     3       // TDO
#define DAP_SWJ_nTRST                   5       // nTRST
#define DAP_SWJ_nRESET                  7       // nRESET

// DAP Transfer Request
#define DAP_TRANSFER_APnDP              (1<<0)
#define DAP_TRANSFER_RnW                (1<<1)
#define DAP_TRANSFER_A2                 (1<<2)
#define DAP_TRANSFER_A3                 (1<<3)
#define DAP_TRANSFER_MATCH_VALUE        (1<<4)
#define DAP_TRANSFER_MATCH_MASK         (1<<5)

// DAP Transfer Response
#define DAP_TRANSFER_OK                 (1<<0)
#define DAP_TRANSFER_WAIT               (1<<1)
#define DAP_TRANSFER_FAULT              (1<<2)
#define DAP_TRANSFER_ERROR              (1<<3)
#define DAP_TRANSFER_MISMATCH           (1<<4)


// Debug Port Register Addresses
#define DP_IDCODE                       0x00    // IDCODE Register (SW Read only)
#define DP_ABORT                        0x00    // Abort Register (SW Write only)
#define DP_CTRL_STAT                    0x04    // Control & Status
#define DP_WCR                          0x04    // Wire Control Register (SW Only)
#define DP_SELECT                       0x08    // Select Register (JTAG R/W & SW W)
#define DP_RESEND                       0x08    // Resend (SW Read Only)
#define DP_RDBUFF                       0x0C    // Read Buffer (Read Only)

// JTAG IR Codes
#define JTAG_ABORT                      0x08
#define JTAG_DPACC                      0x0A
#define JTAG_APACC                      0x0B
#define JTAG_IDCODE                     0x0E
#define JTAG_BYPASS                     0x0F

// JTAG Sequence Info
#define JTAG_SEQUENCE_TCK               0x3F    // TCK count
#define JTAG_SEQUENCE_TMS               0x40    // TMS value
#define JTAG_SEQUENCE_TDO               0x80    // TDO capture


#include 
#include 

// DAP Data structure
typedef struct {
  uint8_t     debug_port;                       // Debug Port
  uint8_t     fast_clock;                       // Fast Clock Flag
  uint32_t   clock_delay;                       // Clock Delay
  struct {                                      // Transfer Configuration
    uint8_t   idle_cycles;                      // Idle cycles after transfer
    uint16_t  retry_count;                      // Number of retries after WAIT response
    uint16_t  match_retry;                      // Number of retries if read value does not match
    uint32_t  match_mask;                       // Match Mask
  } transfer;
#if (DAP_SWD != 0)
  struct {                                      // SWD Configuration
    uint8_t    turnaround;                      // Turnaround period
    uint8_t    data_phase;                      // Always generate Data Phase
  } swd_conf;
#endif
#if (DAP_JTAG != 0)
  struct {                                      // JTAG Device Chain
    uint8_t   count;                            // Number of devices
    uint8_t   index;                            // Device index (device at TDO has index 0)
#if (DAP_JTAG_DEV_CNT != 0)
    uint8_t   ir_length[DAP_JTAG_DEV_CNT];      // IR Length in bits
    uint16_t  ir_before[DAP_JTAG_DEV_CNT];      // Bits before IR
    uint16_t  ir_after [DAP_JTAG_DEV_CNT];      // Bits after IR
#endif
  } jtag_dev;
#endif
} DAP_Data_t;

extern          DAP_Data_t DAP_Data;            // DAP Data
extern volatile uint8_t    DAP_TransferAbort;   // Transfer Abort Flag


// Functions
extern void     SWJ_Sequence    (uint32_t count, uint8_t *data);
extern void     JTAG_Sequence   (uint32_t info,  uint8_t *tdi, uint8_t *tdo);
extern void     JTAG_IR         (uint32_t ir);
extern uint32_t JTAG_ReadIDCode (void);
extern void     JTAG_WriteAbort (uint32_t data);
extern uint8_t  JTAG_Transfer   (uint32_t request, uint32_t *data);
extern uint8_t  SWD_Transfer    (uint32_t request, uint32_t *data);

extern void     Delayms         (uint32_t delay);

extern uint32_t DAP_ProcessVendorCommand (uint8_t *request, uint8_t *response);

extern uint32_t DAP_ProcessCommand (uint8_t *request, uint8_t *response);
extern void     DAP_Setup (void);

// Configurable delay for clock generation
#ifndef DELAY_SLOW_CYCLES
#define DELAY_SLOW_CYCLES       3       // Number of cycles for one iteration
#endif
static __forceinline void PIN_DELAY_SLOW (uint32_t delay) {
  int32_t count;

  count = delay;
  while (--count);
}

// Fixed delay for fast clock generation
#ifndef DELAY_FAST_CYCLES
#define DELAY_FAST_CYCLES       0       // Number of cycles: 0..3
#endif
static __forceinline void PIN_DELAY_FAST (void) {
#if (DELAY_FAST_CYCLES >= 1)
  __nop();
#endif
#if (DELAY_FAST_CYCLES >= 2)
  __nop();
#endif
#if (DELAY_FAST_CYCLES >= 3)
  __nop();
#endif
}


#endif  /* __DAP_H__ */
/******************************************************************************
 * @file     SW_DP.c
 * @brief    CMSIS-DAP SW DP I/O
 * @version  V1.00
 * @date     31. May 2012
 *
 * @note
 * Copyright (C) 2012 ARM Limited. All rights reserved.
 *
 * @par
 * ARM Limited (ARM) is supplying this software for use with Cortex-M
 * processor based microcontrollers.
 *
 * @par
 * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
 * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
 * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
 * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
 *
 ******************************************************************************/

#include "DAP_config.h"
#include "DAP.h"


// SW Macros

#define PIN_SWCLK_SET PIN_SWCLK_TCK_SET
#define PIN_SWCLK_CLR PIN_SWCLK_TCK_CLR

#define SW_CLOCK_CYCLE()                \
  PIN_SWCLK_CLR();                      \
  PIN_DELAY();                          \
  PIN_SWCLK_SET();                      \
  PIN_DELAY()

#define SW_WRITE_BIT(bit)               \
  PIN_SWDIO_OUT(bit);                   \
  PIN_SWCLK_CLR();                      \
  PIN_DELAY();                          \
  PIN_SWCLK_SET();                      \
  PIN_DELAY()

#define SW_READ_BIT(bit)                \
  PIN_SWCLK_CLR();                      \
  PIN_DELAY();                          \
  bit = PIN_SWDIO_IN();                 \
  PIN_SWCLK_SET();                      \
  PIN_DELAY()

#define PIN_DELAY() PIN_DELAY_SLOW(DAP_Data.clock_delay)


// Generate SWJ Sequence
//   count:  sequence bit count
//   data:   pointer to sequence bit data
//   return: none
#if ((DAP_SWD != 0) || (DAP_JTAG != 0))
void SWJ_Sequence (uint32_t count, uint8_t *data) {
  uint32_t val;
  uint32_t n;

  val = 0;
  n = 0;
  while (count--) {
    if (n == 0) {
      val = *data++;
      n = 8;
    }
    if (val & 1) {
      PIN_SWDIO_TMS_SET();
    } else {
      PIN_SWDIO_TMS_CLR();
    }
    SW_CLOCK_CYCLE();
    val >>= 1;
    n--;
  }
}
#endif


#if (DAP_SWD != 0)


// SWD Transfer I/O
//   request: A[3:2] RnW APnDP
//   data:    DATA[31:0]
//   return:  ACK[2:0]
#define SWD_TransferFunction(speed)     /**/                                    \
uint8_t SWD_Transfer##speed (uint32_t request, uint32_t *data) {                \
  uint32_t ack;                                                                 \
  uint32_t bit;                                                                 \
  uint32_t val;                                                                 \
  uint32_t parity;                                                              \
                                                                                \
  uint32_t n;                                                                   \
                                                                                \
  /* Packet Request */                                                          \
  parity = 0;                                                                   \
  SW_WRITE_BIT(1);                      /* Start Bit */                         \
  bit = request >> 0;                                                           \
  SW_WRITE_BIT(bit);                    /* APnDP Bit */                         \
  parity += bit;                                                                \
  bit = request >> 1;                                                           \
  SW_WRITE_BIT(bit);                    /* RnW Bit */                           \
  parity += bit;                                                                \
  bit = request >> 2;                                                           \
  SW_WRITE_BIT(bit);                    /* A2 Bit */                            \
  parity += bit;                                                                \
  bit = request >> 3;                                                           \
  SW_WRITE_BIT(bit);                    /* A3 Bit */                            \
  parity += bit;                                                                \
  SW_WRITE_BIT(parity);                 /* Parity Bit */                        \
  SW_WRITE_BIT(0);                      /* Stop Bit */                          \
  SW_WRITE_BIT(1);                      /* Park Bit */                          \
                                                                                \
  /* Turnaround */                                                              \
  PIN_SWDIO_OUT_DISABLE();                                                      \
  for (n = DAP_Data.swd_conf.turnaround; n; n--) {                              \
    SW_CLOCK_CYCLE();                                                           \
  }                                                                             \
                                                                                \
  /* Acknowledge response */                                                    \
  SW_READ_BIT(bit);                                                             \
  ack  = bit << 0;                                                              \
  SW_READ_BIT(bit);                                                             \
  ack |= bit << 1;                                                              \
  SW_READ_BIT(bit);                                                             \
  ack |= bit << 2;                                                              \
                                                                                \
  if (ack == DAP_TRANSFER_OK) {         /* OK response */                       \
    /* Data transfer */                                                         \
    if (request & DAP_TRANSFER_RnW) {                                           \
      /* Read data */                                                           \
      val = 0;                                                                  \
      parity = 0;                                                               \
      for (n = 32; n; n--) {                                                    \
        SW_READ_BIT(bit);               /* Read RDATA[0:31] */                  \
        parity += bit;                                                          \
        val >>= 1;                                                              \
        val  |= bit << 31;                                                      \
      }                                                                         \
      SW_READ_BIT(bit);                 /* Read Parity */                       \
      if ((parity ^ bit) & 1) {                                                 \
        ack = DAP_TRANSFER_ERROR;                                               \
      }                                                                         \
      if (data) *data = val;                                                    \
      /* Turnaround */                                                          \
      for (n = DAP_Data.swd_conf.turnaround; n; n--) {                          \
        SW_CLOCK_CYCLE();                                                       \
      }                                                                         \
      PIN_SWDIO_OUT_ENABLE();                                                   \
    } else {                                                                    \
      /* Turnaround */                                                          \
      for (n = DAP_Data.swd_conf.turnaround; n; n--) {                          \
        SW_CLOCK_CYCLE();                                                       \
      }                                                                         \
      PIN_SWDIO_OUT_ENABLE();                                                   \
      /* Write data */                                                          \
      val = *data;                                                              \
      parity = 0;                                                               \
      for (n = 32; n; n--) {                                                    \
        SW_WRITE_BIT(val);              /* Write WDATA[0:31] */                 \
        parity += val;                                                          \
        val >>= 1;                                                              \
      }                                                                         \
      SW_WRITE_BIT(parity);             /* Write Parity Bit */                  \
    }                                                                           \
    /* Idle cycles */                                                           \
    n = DAP_Data.transfer.idle_cycles;                                          \
    if (n) {                                                                    \
      PIN_SWDIO_OUT(0);                                                         \
      for (; n; n--) {                                                          \
        SW_CLOCK_CYCLE();                                                       \
      }                                                                         \
    }                                                                           \
    PIN_SWDIO_OUT(1);                                                           \
    return (ack);                                                               \
  }                                                                             \
                                                                                \
  if ((ack == DAP_TRANSFER_WAIT) || (ack == DAP_TRANSFER_FAULT)) {              \
    /* WAIT or FAULT response */                                                \
    if (DAP_Data.swd_conf.data_phase && ((request & DAP_TRANSFER_RnW) != 0)) {  \
      for (n = 32+1; n; n--) {                                                  \
        SW_CLOCK_CYCLE();               /* Dummy Read RDATA[0:31] + Parity */   \
      }                                                                         \
    }                                                                           \
    /* Turnaround */                                                            \
    for (n = DAP_Data.swd_conf.turnaround; n; n--) {                            \
      SW_CLOCK_CYCLE();                                                         \
    }                                                                           \
    PIN_SWDIO_OUT_ENABLE();                                                     \
    if (DAP_Data.swd_conf.data_phase && ((request & DAP_TRANSFER_RnW) == 0)) {  \
      PIN_SWDIO_OUT(0);                                                         \
      for (n = 32+1; n; n--) {                                                  \
        SW_CLOCK_CYCLE();               /* Dummy Write WDATA[0:31] + Parity */  \
      }                                                                         \
    }                                                                           \
    PIN_SWDIO_OUT(1);                                                           \
    return (ack);                                                               \
  }                                                                             \
                                                                                \
  /* Protocol error */                                                          \
  for (n = DAP_Data.swd_conf.turnaround + 32 + 1; n; n--) {                     \
    SW_CLOCK_CYCLE();                   /* Back off data phase */               \
  }                                                                             \
  PIN_SWDIO_OUT(1);                                                             \
  return (ack);                                                                 \
}


#undef  PIN_DELAY
#define PIN_DELAY() PIN_DELAY_FAST()
SWD_TransferFunction(Fast);

#undef  PIN_DELAY
#define PIN_DELAY() PIN_DELAY_SLOW(DAP_Data.clock_delay)
SWD_TransferFunction(Slow);


// SWD Transfer I/O
//   request: A[3:2] RnW APnDP
//   data:    DATA[31:0]
//   return:  ACK[2:0]
uint8_t  SWD_Transfer(uint32_t request, uint32_t *data) {
  if (DAP_Data.fast_clock) {
    return SWD_TransferFast(request, data);
  } else {
    return SWD_TransferSlow(request, data);
  }
}


#endif  /* (DAP_SWD != 0) */
/******************************************************************************
 * @file     DAP.c
 * @brief    CMSIS-DAP Commands
 * @version  V1.00
 * @date     31. May 2012
 *
 * @note
 * Copyright (C) 2012 ARM Limited. All rights reserved.
 *
 * @par
 * ARM Limited (ARM) is supplying this software for use with Cortex-M
 * processor based microcontrollers.
 *
 * @par
 * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
 * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
 * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
 * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
 *
 ******************************************************************************/

#include <string.h>
#include "DAP_config.h"
#include "DAP.h"


#define DAP_FW_VER      "1.0"   // Firmware Version


#if (DAP_PACKET_SIZE < 64)
#error "Minimum Packet Size is 64"
#endif
#if (DAP_PACKET_SIZE > 32768)
#error "Maximum Packet Size is 32768"
#endif
#if (DAP_PACKET_COUNT < 1)
#error "Minimum Packet Count is 1"
#endif
#if (DAP_PACKET_COUNT > 255)
#error "Maximum Packet Count is 255"
#endif


// Clock Macros

#define MAX_SWJ_CLOCK(delay_cycles) \
  (CPU_CLOCK/2 / (IO_PORT_WRITE_CYCLES + delay_cycles))

#define CLOCK_DELAY(swj_clock) \
 ((CPU_CLOCK/2 / swj_clock) - IO_PORT_WRITE_CYCLES)


         DAP_Data_t DAP_Data;           // DAP Data
volatile uint8_t    DAP_TransferAbort;  // Trasfer Abort Flag


#ifdef DAP_VENDOR
const char DAP_Vendor [] = DAP_VENDOR;
#endif
#ifdef DAP_PRODUCT
const char DAP_Product[] = DAP_PRODUCT;
#endif
#ifdef DAP_SER_NUM
const char DAP_SerNum [] = DAP_SER_NUM;
#endif
const char DAP_FW_Ver [] = DAP_FW_VER;

#if TARGET_DEVICE_FIXED
const char TargetDeviceVendor [] = TARGET_DEVICE_VENDOR;
const char TargetDeviceName   [] = TARGET_DEVICE_NAME;
#endif


// Get DAP Information
//   id:      info identifier
//   info:    pointer to info data
//   return:  number of bytes in info data
static uint8_t DAP_Info(uint8_t id, uint8_t *info) {
  uint8_t length = 0;

  switch (id) {
    case DAP_ID_VENDOR:
#ifdef DAP_VENDOR
      memcpy(info, DAP_Vendor, sizeof(DAP_Vendor));
      length = sizeof(DAP_Vendor);
#endif
      break;
    case DAP_ID_PRODUCT:
#ifdef DAP_PRODUCT
      memcpy(info, DAP_Product, sizeof(DAP_Product));
      length = sizeof(DAP_Product);
#endif
      break;
    case DAP_ID_SER_NUM:
#ifdef DAP_SER_NUM
      memcpy(info, DAP_SerNum, sizeof(DAP_SerNum));
      length = sizeof(DAP_SerNum);
#endif
      break;
    case DAP_ID_FW_VER:
      memcpy(info, DAP_FW_Ver, sizeof(DAP_FW_Ver));
      length = sizeof(DAP_FW_Ver);
      break;
    case DAP_ID_DEVICE_VENDOR:
#if TARGET_DEVICE_FIXED
      memcpy(info, TargetDeviceVendor, sizeof(TargetDeviceVendor));
      length = sizeof(TargetDeviceVendor);
#endif
      break;
    case DAP_ID_DEVICE_NAME:
#if TARGET_DEVICE_FIXED
      memcpy(info, TargetDeviceName, sizeof(TargetDeviceName));
      length = sizeof(TargetDeviceName);
#endif
      break;
    case DAP_ID_CAPABILITIES:
      info[0] = ((DAP_SWD  != 0) ? (1 << 0) : 0) |
                ((DAP_JTAG != 0) ? (1 << 1) : 0);
      length = 1;
      break;
    case DAP_ID_PACKET_SIZE:
      info[0] = (uint8_t)(DAP_PACKET_SIZE >> 0);
      info[1] = (uint8_t)(DAP_PACKET_SIZE >> 8);
      length = 2;
      break;
    case DAP_ID_PACKET_COUNT:
      info[0] = DAP_PACKET_COUNT;
      length = 1;
      break;
  }

  return (length);
}


// Timer Functions

#if ((DAP_SWD != 0) || (DAP_JTAG != 0))

// Start Timer
static __inline void TIMER_START (uint32_t usec) {
  SysTick->VAL  = 0;
  SysTick->LOAD = usec * CPU_CLOCK/1000000;
  SysTick->CTRL = (1 << SysTick_CTRL_ENABLE_Pos) |
                  (1 << SysTick_CTRL_CLKSOURCE_Pos);
}

// Stop Timer
static __inline void TIMER_STOP (void) {
  SysTick->CTRL = 0;
}

// Check if Timer expired
static __inline uint32_t TIMER_EXPIRED (void) {
  return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) ? 1 : 0);
}

#endif


// Delay for specified time
//    delay:  delay time in ms
void Delayms(uint32_t delay) {
  delay *= (CPU_CLOCK/1000 + (DELAY_SLOW_CYCLES-1)) / DELAY_SLOW_CYCLES;
  PIN_DELAY_SLOW(delay);
}


// Process Delay command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
static uint32_t DAP_Delay(uint8_t *request, uint8_t *response) {
  uint32_t delay;

  delay  = *(request+0) | (*(request+1) << 8);
  delay *= (CPU_CLOCK/1000000 + (DELAY_SLOW_CYCLES-1)) / DELAY_SLOW_CYCLES;

  PIN_DELAY_SLOW(delay);

  *response = DAP_OK;
  return (1);
}


// Process LED command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
static uint32_t DAP_LED(uint8_t *request, uint8_t *response) {

  switch (*request) {
    case DAP_LED_DEBUGGER_CONNECTED:
      LED_CONNECTED_OUT((*(request+1) & 1));
      break;
    case DAP_LED_TARGET_RUNNING:
      LED_RUNNING_OUT((*(request+1) & 1));
      break;
    default:
      *response = DAP_ERROR;
      return (1);
  }

  *response = DAP_OK;
  return (1);
}


// Process Connect command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
static uint32_t DAP_Connect(uint8_t *request, uint8_t *response) {
  uint32_t port;

  if (*request == DAP_PORT_AUTODETECT) {
    port = DAP_DEFAULT_PORT;
  } else {
    port = *request;
  }
  
  switch (port) {
#if (DAP_SWD != 0)
    case DAP_PORT_SWD:
      DAP_Data.debug_port = DAP_PORT_SWD;
      PORT_SWD_SETUP();
      break;
#endif
#if (DAP_JTAG != 0)
    case DAP_PORT_JTAG:
      DAP_Data.debug_port = DAP_PORT_JTAG;
      PORT_JTAG_SETUP();
      break;
#endif
    default:
      *response = DAP_PORT_DISABLED;
      return (1);
  }

  *response = port;
  return (1);
}


// Process Disconnect command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
static uint32_t DAP_Disconnect(uint8_t *response) {

  DAP_Data.debug_port = DAP_PORT_DISABLED;
  PORT_OFF();

  *response = DAP_OK;
  return (1);
}


// Process Reset Target command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
static uint32_t DAP_ResetTarget(uint8_t *response) {

  *(response+1) = RESET_TARGET();
  *(response+0) = DAP_OK;
  return (2);
}


// Process SWJ Pins command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if ((DAP_SWD != 0) || (DAP_JTAG != 0))
static uint32_t DAP_SWJ_Pins(uint8_t *request, uint8_t *response) {
  uint32_t value;
  uint32_t select;
  uint32_t wait;
  
  value  =  *(request+0);
  select =  *(request+1); 
  wait   = (*(request+2) <<  0) |
           (*(request+3) <<  8) |
           (*(request+4) << 16) |
           (*(request+5) << 24);

  if (select & (1 << DAP_SWJ_SWCLK_TCK)) {
    if (value & (1 << DAP_SWJ_SWCLK_TCK)) {
      PIN_SWCLK_TCK_SET();
    } else {
      PIN_SWCLK_TCK_CLR();
    }
  }
  if (select & (1 << DAP_SWJ_SWDIO_TMS)) {
    if (value & (1 << DAP_SWJ_SWDIO_TMS)) {
      PIN_SWDIO_TMS_SET();
    } else {
      PIN_SWDIO_TMS_CLR();
    }
  }
  if (select & (1 << DAP_SWJ_TDI)) {
    PIN_TDI_OUT(value >> DAP_SWJ_TDI);
  }
  if (select & (1 << DAP_SWJ_nTRST)) {
    PIN_nTRST_OUT(value >> DAP_SWJ_nTRST);
  }
  if (select & (1 << DAP_SWJ_nRESET)) {
    PIN_nRESET_OUT(value >> DAP_SWJ_nRESET);
  }

  if (wait) {
    if (wait > 3000000) wait = 3000000;
    TIMER_START(wait);
    do {
      if (select & (1 << DAP_SWJ_SWCLK_TCK)) {
        if ((value >> DAP_SWJ_SWCLK_TCK) ^ PIN_SWCLK_TCK_IN()) continue;
      }
      if (select & (1 << DAP_SWJ_SWDIO_TMS)) {
        if ((value >> DAP_SWJ_SWDIO_TMS) ^ PIN_SWDIO_TMS_IN()) continue;
      }
      if (select & (1 << DAP_SWJ_TDI)) {
        if ((value >> DAP_SWJ_TDI) ^ PIN_TDI_IN()) continue;
      }
      if (select & (1 << DAP_SWJ_nTRST)) {
        if ((value >> DAP_SWJ_nTRST) ^ PIN_nTRST_IN()) continue;
      }
      if (select & (1 << DAP_SWJ_nRESET)) {
        if ((value >> DAP_SWJ_nRESET) ^ PIN_nRESET_IN()) continue;
      }
      break;
    } while (!TIMER_EXPIRED());
    TIMER_STOP();
  }

  value = (PIN_SWCLK_TCK_IN() << DAP_SWJ_SWCLK_TCK) |
          (PIN_SWDIO_TMS_IN() << DAP_SWJ_SWDIO_TMS) |
          (PIN_TDI_IN()       << DAP_SWJ_TDI)       |
          (PIN_TDO_IN()       << DAP_SWJ_TDO)       |
          (PIN_nTRST_IN()     << DAP_SWJ_nTRST)     |
          (PIN_nRESET_IN()    << DAP_SWJ_nRESET);

  *response = (uint8_t)value;
  return (1);
}
#endif


// Process SWJ Clock command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if ((DAP_SWD != 0) || (DAP_JTAG != 0))
static uint32_t DAP_SWJ_Clock(uint8_t *request, uint8_t *response) {
  uint32_t clock;
  uint32_t delay;

  clock = (*(request+0) <<  0) |
          (*(request+1) <<  8) |
          (*(request+2) << 16) |
          (*(request+3) << 24);

  if (clock == 0) {
    *response = DAP_ERROR;
    return (1);
  }

  if (clock >= MAX_SWJ_CLOCK(DELAY_FAST_CYCLES)) {
    DAP_Data.fast_clock  = 1;
    DAP_Data.clock_delay = 1;
  } else {
    DAP_Data.fast_clock  = 0;

    delay = (CPU_CLOCK/2 + (clock - 1)) / clock;
    if (delay > IO_PORT_WRITE_CYCLES) {
      delay -= IO_PORT_WRITE_CYCLES;
      delay  = (delay + (DELAY_SLOW_CYCLES - 1)) / DELAY_SLOW_CYCLES;
    } else {
      delay  = 1;
    }

    DAP_Data.clock_delay = delay;
  }

  *response = DAP_OK;
  return (1);
}
#endif


// Process SWJ Sequence command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if ((DAP_SWD != 0) || (DAP_JTAG != 0))
static uint32_t DAP_SWJ_Sequence(uint8_t *request, uint8_t *response) {
  uint32_t count;

  count = *request++;
  if (count == 0) count = 256;

  SWJ_Sequence(count, request);

  *response = DAP_OK;
  return (1);
}
#endif


// Process SWD Configure command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if (DAP_SWD != 0)
static uint32_t DAP_SWD_Configure(uint8_t *request, uint8_t *response) {
  uint8_t value;

  value = *request;
  DAP_Data.swd_conf.turnaround  = (value & 0x03) + 1;
  DAP_Data.swd_conf.data_phase  = (value & 0x04) ? 1 : 0;
  
  *response = DAP_OK;

  return (1);
}
#endif


// Process SWD Abort command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if (DAP_SWD != 0)
static uint32_t DAP_SWD_Abort(uint8_t *request, uint8_t *response) {
  uint32_t data;

  if (DAP_Data.debug_port != DAP_PORT_SWD) {
    *response = DAP_ERROR;
    return (1);
  }

  // Load data (Ignore DAP index)
  data = (*(request+1) <<  0) |
         (*(request+2) <<  8) |
         (*(request+3) << 16) |
         (*(request+4) << 24);

  // Write Abort register
  SWD_Transfer(DP_ABORT, &data);
  *response = DAP_OK;

  return (1);
}
#endif


// Process JTAG Sequence command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if (DAP_JTAG != 0)
static uint32_t DAP_JTAG_Sequence(uint8_t *request, uint8_t *response) {
  uint32_t sequence_info;
  uint32_t sequence_count;
  uint32_t response_count;
  uint32_t count;

  *response++ = DAP_OK;
  response_count = 1;

  sequence_count = *request++;
  while (sequence_count--) {
    sequence_info = *request++;
    JTAG_Sequence(sequence_info, request, response);
    count = sequence_info & JTAG_SEQUENCE_TCK;
    if (count == 0) count = 64;
    count = (count + 7) / 8;
    request += count;
    if (sequence_info & JTAG_SEQUENCE_TDO) {
      response += count;
      response_count += count;
    }
  }

  return (response_count);
}
#endif


// Process JTAG Configure command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if (DAP_JTAG != 0)
static uint32_t DAP_JTAG_Configure(uint8_t *request, uint8_t *response) {
  uint32_t count;
  uint32_t length;
  uint32_t bits;
  uint32_t n;

  count = *request++;
  DAP_Data.jtag_dev.count = count;

  bits = 0;
  for (n = 0; n < count; n++) {
    length = *request++;
    DAP_Data.jtag_dev.ir_length[n] = length;
    DAP_Data.jtag_dev.ir_before[n] = bits;
    bits += length;
  }
  for (n = 0; n < count; n++) {
    bits -= DAP_Data.jtag_dev.ir_length[n];
    DAP_Data.jtag_dev.ir_after[n] = bits;
  }

  *response = DAP_OK;
  return (1);
}
#endif


// Process JTAG IDCODE command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if (DAP_JTAG != 0)
static uint32_t DAP_JTAG_IDCode(uint8_t *request, uint8_t *response) {
  uint32_t data;

  if (DAP_Data.debug_port != DAP_PORT_JTAG) {
err:*response = DAP_ERROR;
    return (1);
  }

  // Device index (JTAP TAP)
  DAP_Data.jtag_dev.index = *request;
  if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) goto err;

  // Select JTAG chain
  JTAG_IR(JTAG_IDCODE);

  // Read IDCODE register
  data = JTAG_ReadIDCode();

  // Store Data
  *(response+0) =  DAP_OK;
  *(response+1) = (uint8_t)(data >>  0);
  *(response+2) = (uint8_t)(data >>  8);
  *(response+3) = (uint8_t)(data >> 16);
  *(response+4) = (uint8_t)(data >> 24);

  return (1+4);
}
#endif


// Process JTAG Abort command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if (DAP_JTAG != 0)
static uint32_t DAP_JTAG_Abort(uint8_t *request, uint8_t *response) {
  uint32_t data;

  if (DAP_Data.debug_port != DAP_PORT_JTAG) {
err:*response = DAP_ERROR;
    return (1);
  }

  // Device index (JTAP TAP)
  DAP_Data.jtag_dev.index = *request;
  if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) goto err;

  // Select JTAG chain
  JTAG_IR(JTAG_ABORT);

  // Load data
  data = (*(request+1) <<  0) |
         (*(request+2) <<  8) |
         (*(request+3) << 16) |
         (*(request+4) << 24);

  // Write Abort register
  JTAG_WriteAbort(data);
  *response = DAP_OK;

  return (1);
}
#endif


// Process Transfer Configure command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
static uint32_t DAP_TransferConfigure(uint8_t *request, uint8_t *response) {

  DAP_Data.transfer.idle_cycles = *(request+0);
  DAP_Data.transfer.retry_count = *(request+1) | (*(request+2) << 8);
  DAP_Data.transfer.match_retry = *(request+3) | (*(request+4) << 8);
  
  *response = DAP_OK;

  return (1);
}


// Process SWD Transfer command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if (DAP_SWD != 0)
static uint32_t DAP_SWD_Transfer(uint8_t *request, uint8_t *response) {
  uint32_t  request_count;
  uint32_t  request_value;
  uint32_t  response_count;
  uint32_t  response_value;
  uint8_t  *response_head;
  uint32_t  post_read;
  uint32_t  check_write;
  uint32_t  match_value;
  uint32_t  match_retry;
  uint32_t  retry;
  uint32_t  data;

  response_count = 0;
  response_value = 0;
  response_head  = response;
  response      += 2;

  DAP_TransferAbort = 0;

  post_read   = 0;
  check_write = 0;

  request++;            // Ignore DAP index

  request_count = *request++;
  while (request_count--) {
    request_value = *request++;
    if (request_value & DAP_TRANSFER_RnW) {
      // Read register
      if (post_read) {
        // Read was posted before
        retry = DAP_Data.transfer.retry_count;
        if ((request_value & (DAP_TRANSFER_APnDP | DAP_TRANSFER_MATCH_VALUE)) == DAP_TRANSFER_APnDP) {
          // Read previous AP data and post next AP read
          do {
            response_value = SWD_Transfer(request_value, &data);
          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
        } else {
          // Read previous AP data
          do {
            response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
          post_read = 0;
        }
        if (response_value != DAP_TRANSFER_OK) break;
        // Store previous AP data
        *response++ = (uint8_t) data;
        *response++ = (uint8_t)(data >>  8);
        *response++ = (uint8_t)(data >> 16);
        *response++ = (uint8_t)(data >> 24);
      }
      if (request_value & DAP_TRANSFER_MATCH_VALUE) {
        // Read with value match
        match_value = (*(request+0) <<  0) |
                      (*(request+1) <<  8) |
                      (*(request+2) << 16) |
                      (*(request+3) << 24);
        request += 4;
        match_retry = DAP_Data.transfer.match_retry;
        if (request_value & DAP_TRANSFER_APnDP) {
          // Post AP read
          retry = DAP_Data.transfer.retry_count;
          do {
            response_value = SWD_Transfer(request_value, NULL);
          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
          if (response_value != DAP_TRANSFER_OK) break;
        }
        do {
          // Read register until its value matches or retry counter expires
          retry = DAP_Data.transfer.retry_count;
          do {
            response_value = SWD_Transfer(request_value, &data);
          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
          if (response_value != DAP_TRANSFER_OK) break;
        } while (((data & DAP_Data.transfer.match_mask) != match_value) && match_retry-- && !DAP_TransferAbort);
        if ((data & DAP_Data.transfer.match_mask) != match_value) {
          response_value |= DAP_TRANSFER_MISMATCH;
        }
        if (response_value != DAP_TRANSFER_OK) break;
      } else {
        // Normal read
        retry = DAP_Data.transfer.retry_count;
        if (request_value & DAP_TRANSFER_APnDP) {
          // Read AP register
          if (post_read == 0) {
            // Post AP read
            do {
              response_value = SWD_Transfer(request_value, NULL);
            } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
            if (response_value != DAP_TRANSFER_OK) break;
            post_read = 1;
          }
        } else {
          // Read DP register
          do {
            response_value = SWD_Transfer(request_value, &data);
          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
          if (response_value != DAP_TRANSFER_OK) break;
          // Store data
          *response++ = (uint8_t) data;
          *response++ = (uint8_t)(data >>  8);
          *response++ = (uint8_t)(data >> 16);
          *response++ = (uint8_t)(data >> 24);
        }
      }
      check_write = 0;
    } else {
      // Write register
      if (post_read) {
        // Read previous data
        retry = DAP_Data.transfer.retry_count;
        do {
          response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
        } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
        if (response_value != DAP_TRANSFER_OK) break;
        // Store previous data
        *response++ = (uint8_t) data;
        *response++ = (uint8_t)(data >>  8);
        *response++ = (uint8_t)(data >> 16);
        *response++ = (uint8_t)(data >> 24);
        post_read = 0;
      }
      // Load data
      data = (*(request+0) <<  0) |
             (*(request+1) <<  8) |
             (*(request+2) << 16) |
             (*(request+3) << 24);
      request += 4;
      if (request_value & DAP_TRANSFER_MATCH_MASK) {
        // Write match mask
        DAP_Data.transfer.match_mask = data;
        response_value = DAP_TRANSFER_OK;
      } else {
        // Write DP/AP register
        retry = DAP_Data.transfer.retry_count;
        do {
          response_value = SWD_Transfer(request_value, &data);
        } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
        if (response_value != DAP_TRANSFER_OK) break;
        check_write = 1;
      }
    }
    response_count++;
    if (DAP_TransferAbort) break;
  }

  if (response_value == DAP_TRANSFER_OK) {
    if (post_read) {
      // Read previous data
      retry = DAP_Data.transfer.retry_count;
      do {
        response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
      if (response_value != DAP_TRANSFER_OK) goto end;
      // Store previous data
      *response++ = (uint8_t) data;
      *response++ = (uint8_t)(data >>  8);
      *response++ = (uint8_t)(data >> 16);
      *response++ = (uint8_t)(data >> 24);
    } else if (check_write) {
      // Check last write
      retry = DAP_Data.transfer.retry_count;
      do {
        response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
    }
  }

end:
  *(response_head+0) = (uint8_t)response_count;
  *(response_head+1) = (uint8_t)response_value;

  return (response - response_head);
}
#endif


// Process JTAG Transfer command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if (DAP_JTAG != 0)
static uint32_t DAP_JTAG_Transfer(uint8_t *request, uint8_t *response) {
  uint32_t  request_count;
  uint32_t  request_value;
  uint32_t  request_ir;
  uint32_t  response_count;
  uint32_t  response_value;
  uint8_t  *response_head;
  uint32_t  post_read;
  uint32_t  match_value;
  uint32_t  match_retry;
  uint32_t  retry;
  uint32_t  data;
  uint32_t  ir;

  response_count = 0;
  response_value = 0;
  response_head  = response;
  response      += 2;

  DAP_TransferAbort = 0;

  ir        = 0;
  post_read = 0;

  // Device index (JTAP TAP)
  DAP_Data.jtag_dev.index = *request++;
  if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) goto end;

  request_count = *request++;
  while (request_count--) {
    request_value = *request++;
    request_ir = (request_value & DAP_TRANSFER_APnDP) ? JTAG_APACC : JTAG_DPACC;
    if (request_value & DAP_TRANSFER_RnW) {
      // Read register
      if (post_read) {
        // Read was posted before
        retry = DAP_Data.transfer.retry_count;
        if ((ir == request_ir) && ((request_value & DAP_TRANSFER_MATCH_VALUE) == 0)) {
          // Read previous data and post next read
          do {
            response_value = JTAG_Transfer(request_value, &data);
          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
        } else {
          // Select JTAG chain
          if (ir != JTAG_DPACC) {
            ir = JTAG_DPACC;
            JTAG_IR(ir);
          }
          // Read previous data
          do {
            response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
          post_read = 0;
        }
        if (response_value != DAP_TRANSFER_OK) break;
        // Store previous data
        *response++ = (uint8_t) data;
        *response++ = (uint8_t)(data >>  8);
        *response++ = (uint8_t)(data >> 16);
        *response++ = (uint8_t)(data >> 24);
      }
      if (request_value & DAP_TRANSFER_MATCH_VALUE) {
        // Read with value match
        match_value = (*(request+0) <<  0) |
                      (*(request+1) <<  8) |
                      (*(request+2) << 16) |
                      (*(request+3) << 24);
        request += 4;
        match_retry  = DAP_Data.transfer.match_retry;
        // Select JTAG chain
        if (ir != request_ir) {
          ir = request_ir;
          JTAG_IR(ir);
        }
        // Post DP/AP read
        retry = DAP_Data.transfer.retry_count;
        do {
          response_value = JTAG_Transfer(request_value, NULL);
        } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
        if (response_value != DAP_TRANSFER_OK) break;
        do {
          // Read register until its value matches or retry counter expires
          retry = DAP_Data.transfer.retry_count;
          do {
            response_value = JTAG_Transfer(request_value, &data);
          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
          if (response_value != DAP_TRANSFER_OK) break;
        } while (((data & DAP_Data.transfer.match_mask) != match_value) && match_retry-- && !DAP_TransferAbort);
        if ((data & DAP_Data.transfer.match_mask) != match_value) {
          response_value |= DAP_TRANSFER_MISMATCH;
        }
        if (response_value != DAP_TRANSFER_OK) break;
      } else {
        // Normal read
        if (post_read == 0) {
          // Select JTAG chain
          if (ir != request_ir) {
            ir = request_ir;
            JTAG_IR(ir);
          }
          // Post DP/AP read
          retry = DAP_Data.transfer.retry_count;
          do {
            response_value = JTAG_Transfer(request_value, NULL);
          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
          if (response_value != DAP_TRANSFER_OK) break;
          post_read = 1;
        }
      }
    } else {
      // Write register
      if (post_read) {
        // Select JTAG chain
        if (ir != JTAG_DPACC) {
          ir = JTAG_DPACC;
          JTAG_IR(ir);
        }
        // Read previous data
        retry = DAP_Data.transfer.retry_count;
        do {
          response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
        } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
        if (response_value != DAP_TRANSFER_OK) break;
        // Store previous data
        *response++ = (uint8_t) data;
        *response++ = (uint8_t)(data >>  8);
        *response++ = (uint8_t)(data >> 16);
        *response++ = (uint8_t)(data >> 24);
        post_read = 0;
      }
      // Load data
      data = (*(request+0) <<  0) |
             (*(request+1) <<  8) |
             (*(request+2) << 16) |
             (*(request+3) << 24);
      request += 4;
      if (request_value & DAP_TRANSFER_MATCH_MASK) {
        // Write match mask
        DAP_Data.transfer.match_mask = data;
        response_value = DAP_TRANSFER_OK;
      } else {
        // Select JTAG chain
        if (ir != request_ir) {
          ir = request_ir;
          JTAG_IR(ir);
        }
        // Write DP/AP register
        retry = DAP_Data.transfer.retry_count;
        do {
          response_value = JTAG_Transfer(request_value, &data);
        } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
        if (response_value != DAP_TRANSFER_OK) break;
      }
    }
    response_count++;
    if (DAP_TransferAbort) break;
  }

  if (response_value == DAP_TRANSFER_OK) {
    // Select JTAG chain
    if (ir != JTAG_DPACC) {
      ir = JTAG_DPACC;
      JTAG_IR(ir);
    }
    if (post_read) {
      // Read previous data
      retry = DAP_Data.transfer.retry_count;
      do {
        response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
      if (response_value != DAP_TRANSFER_OK) goto end;
      // Store previous data
      *response++ = (uint8_t) data;
      *response++ = (uint8_t)(data >>  8);
      *response++ = (uint8_t)(data >> 16);
      *response++ = (uint8_t)(data >> 24);
    } else {
      // Check last write
      retry = DAP_Data.transfer.retry_count;
      do {
        response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
    }
  }

end:
  *(response_head+0) = (uint8_t)response_count;
  *(response_head+1) = (uint8_t)response_value;

  return (response - response_head);
}
#endif


// Process SWD Transfer Block command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if (DAP_SWD != 0)
static uint32_t DAP_SWD_TransferBlock(uint8_t *request, uint8_t *response) {
  uint32_t  request_count;
  uint32_t  request_value;
  uint32_t  response_count;
  uint32_t  response_value;
  uint8_t  *response_head;
  uint32_t  retry;
  uint32_t  data;

  response_count = 0;
  response_value = 0;
  response_head  = response;
  response      += 3;

  DAP_TransferAbort = 0;

  request++;            // Ignore DAP index

  request_count = *request | (*(request+1) << 8);
  request += 2;
  if (request_count == 0) goto end;

  request_value = *request++;
  if (request_value & DAP_TRANSFER_RnW) {
    // Read register block
    if (request_value & DAP_TRANSFER_APnDP) {
      // Post AP read
      retry = DAP_Data.transfer.retry_count;
      do {
        response_value = SWD_Transfer(request_value, NULL);
      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
      if (response_value != DAP_TRANSFER_OK) goto end;
    }
    while (request_count--) {
      // Read DP/AP register
      if ((request_count == 0) && (request_value & DAP_TRANSFER_APnDP)) {
        // Last AP read
        request_value = DP_RDBUFF | DAP_TRANSFER_RnW;
      }
      retry = DAP_Data.transfer.retry_count;
      do {
        response_value = SWD_Transfer(request_value, &data);
      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
      if (response_value != DAP_TRANSFER_OK) goto end;
      // Store data
      *response++ = (uint8_t) data;
      *response++ = (uint8_t)(data >>  8);
      *response++ = (uint8_t)(data >> 16);
      *response++ = (uint8_t)(data >> 24);
      response_count++;
    }
  } else {
    // Write register block
    while (request_count--) {
      // Load data
      data = (*(request+0) <<  0) |
             (*(request+1) <<  8) |
             (*(request+2) << 16) |
             (*(request+3) << 24);
      request += 4;
      // Write DP/AP register
      retry = DAP_Data.transfer.retry_count;
      do {
        response_value = SWD_Transfer(request_value, &data);
      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
      if (response_value != DAP_TRANSFER_OK) goto end;
      response_count++;
    }
    // Check last write
    retry = DAP_Data.transfer.retry_count;
    do {
      response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
  }

end:
  *(response_head+0) = (uint8_t)(response_count >> 0);
  *(response_head+1) = (uint8_t)(response_count >> 8);
  *(response_head+2) = (uint8_t) response_value;

  return (response - response_head);
}
#endif


// Process JTAG Transfer Block command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
#if (DAP_JTAG != 0)
static uint32_t DAP_JTAG_TransferBlock(uint8_t *request, uint8_t *response) {
  uint32_t  request_count;
  uint32_t  request_value;
  uint32_t  response_count;
  uint32_t  response_value;
  uint8_t  *response_head;
  uint32_t  retry;
  uint32_t  data;
  uint32_t  ir;

  response_count = 0;
  response_value = 0;
  response_head  = response;
  response      += 3;

  DAP_TransferAbort = 0;

  // Device index (JTAP TAP)
  DAP_Data.jtag_dev.index = *request++;
  if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) goto end;

  request_count = *request | (*(request+1) << 8);
  request += 2;
  if (request_count == 0) goto end;

  request_value = *request++;

  // Select JTAG chain
  ir = (request_value & DAP_TRANSFER_APnDP) ? JTAG_APACC : JTAG_DPACC;
  JTAG_IR(ir);

  if (request_value & DAP_TRANSFER_RnW) {
    // Post read
    retry = DAP_Data.transfer.retry_count;
    do {
      response_value = JTAG_Transfer(request_value, NULL);
    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
    if (response_value != DAP_TRANSFER_OK) goto end;
    // Read register block
    while (request_count--) {
      // Read DP/AP register
      if (request_count == 0) {
        // Last read
        if (ir != JTAG_DPACC) {
          JTAG_IR(JTAG_DPACC);
        }
        request_value = DP_RDBUFF | DAP_TRANSFER_RnW;
      }
      retry = DAP_Data.transfer.retry_count;
      do {
        response_value = JTAG_Transfer(request_value, &data);
      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
      if (response_value != DAP_TRANSFER_OK) goto end;
      // Store data
      *response++ = (uint8_t) data;
      *response++ = (uint8_t)(data >>  8);
      *response++ = (uint8_t)(data >> 16);
      *response++ = (uint8_t)(data >> 24);
      response_count++;
    }
  } else {
    // Write register block
    while (request_count--) {
      // Load data
      data = (*(request+0) <<  0) |
             (*(request+1) <<  8) |
             (*(request+2) << 16) |
             (*(request+3) << 24);
      request += 4;
      // Write DP/AP register
      retry = DAP_Data.transfer.retry_count;
      do {
        response_value = JTAG_Transfer(request_value, &data);
      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
      if (response_value != DAP_TRANSFER_OK) goto end;
      response_count++;
    }
    // Check last write
    if (ir != JTAG_DPACC) {
      JTAG_IR(JTAG_DPACC);
    }
    retry = DAP_Data.transfer.retry_count;
    do {
      response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
  }

end:
  *(response_head+0) = (uint8_t)(response_count >> 0);
  *(response_head+1) = (uint8_t)(response_count >> 8);
  *(response_head+2) = (uint8_t) response_value;

  return (response - response_head);
}
#endif


// Process DAP Vendor command and prepare response
// Default function (can be overridden)
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
__weak uint32_t DAP_ProcessVendorCommand(uint8_t *request, uint8_t *response) {
  *response = ID_DAP_Invalid;
  return (1);
}


// Process DAP command and prepare response
//   request:  pointer to request data
//   response: pointer to response data
//   return:   number of bytes in response
uint32_t DAP_ProcessCommand(uint8_t *request, uint8_t *response) {
  uint32_t num;

  if ((*request >= ID_DAP_Vendor0) && (*request <= ID_DAP_Vendor31)) {
    return DAP_ProcessVendorCommand(request, response);
  }

  *response++ = *request;

  switch (*request++) {
    case ID_DAP_Info:
      num = DAP_Info(*request, response+1);
      *response = num;
      return (2 + num);
    case ID_DAP_LED:
      num = DAP_LED(request, response);
      break;
    case ID_DAP_Connect:
      num = DAP_Connect(request, response);
      break;
    case ID_DAP_Disconnect:
      num = DAP_Disconnect(response);
      break;
    case ID_DAP_Delay:
      num = DAP_Delay(request, response);
      break;
    case ID_DAP_ResetTarget:
      num = DAP_ResetTarget(response);
      break;

#if ((DAP_SWD != 0) || (DAP_JTAG != 0))
    case ID_DAP_SWJ_Pins:
      num = DAP_SWJ_Pins(request, response);
      break;
    case ID_DAP_SWJ_Clock:
      num = DAP_SWJ_Clock(request, response);
      break;
    case ID_DAP_SWJ_Sequence:
      num = DAP_SWJ_Sequence(request, response);
      break;
#else
    case ID_DAP_SWJ_Pins:
    case ID_DAP_SWJ_Clock:
    case ID_DAP_SWJ_Sequence:
      *response = DAP_ERROR;
      return (2);
#endif

#if (DAP_SWD != 0)
    case ID_DAP_SWD_Configure:
      num = DAP_SWD_Configure(request, response);
      break;
#else
    case ID_DAP_SWD_Configure:
      *response = DAP_ERROR;
      return (2);
#endif

#if (DAP_JTAG != 0)
    case ID_DAP_JTAG_Sequence:
      num = DAP_JTAG_Sequence(request, response);
      break;
    case ID_DAP_JTAG_Configure:
      num = DAP_JTAG_Configure(request, response);
      break;
    case ID_DAP_JTAG_IDCODE:
      num = DAP_JTAG_IDCode(request, response);
      break;
#else
    case ID_DAP_JTAG_Sequence:
    case ID_DAP_JTAG_Configure:
    case ID_DAP_JTAG_IDCODE:
      *response = DAP_ERROR;
      return (2);
#endif

    case ID_DAP_TransferConfigure:
      num = DAP_TransferConfigure(request, response);
      break;

    case ID_DAP_Transfer:
      switch (DAP_Data.debug_port) {
#if (DAP_SWD != 0)
        case DAP_PORT_SWD:
          num = DAP_SWD_Transfer (request, response);
          break;
#endif
#if (DAP_JTAG != 0)
        case DAP_PORT_JTAG:
          num = DAP_JTAG_Transfer(request, response);
          break;
#endif
        default:
          *(response+0) = 0;    // Response count
          *(response+1) = 0;    // Response value
          num = 2;
      }
      break;

    case ID_DAP_TransferBlock:
      switch (DAP_Data.debug_port) {
#if (DAP_SWD != 0)
        case DAP_PORT_SWD:
          num = DAP_SWD_TransferBlock (request, response);
          break;
#endif
#if (DAP_JTAG != 0)
        case DAP_PORT_JTAG:
          num = DAP_JTAG_TransferBlock(request, response);
          break;
#endif
        default:
          *(response+0) = 0;    // Response count [7:0]
          *(response+1) = 0;    // Response count[15:8]
          *(response+2) = 0;    // Response value
          num = 3;
      }
      break;

    case ID_DAP_WriteABORT:
      switch (DAP_Data.debug_port) {
#if (DAP_SWD != 0)
        case DAP_PORT_SWD:
          num = DAP_SWD_Abort (request, response);
          break;
#endif
#if (DAP_JTAG != 0)
        case DAP_PORT_JTAG:
          num = DAP_JTAG_Abort(request, response);
          break;
#endif
        default:
          *response = DAP_ERROR;
          return (2);
      }
      break;

    default:
      *(response-1) = ID_DAP_Invalid;
      return (1);
  }

  return (1 + num);
}


// Setup DAP
void DAP_Setup(void) {

  // Default settings (only non-zero values)
//DAP_Data.debug_port  = 0;
//DAP_Data.fast_clock  = 0;
  DAP_Data.clock_delay = CLOCK_DELAY(DAP_DEFAULT_SWJ_CLOCK);
//DAP_Data.transfer.idle_cycles = 0;
  DAP_Data.transfer.retry_count = 100;
//DAP_Data.transfer.match_retry = 0;
//DAP_Data.transfer.match_mask  = 0x000000;
#if (DAP_SWD != 0)
  DAP_Data.swd_conf.turnaround  = 1;
//DAP_Data.swd_conf.data_phase  = 0;
#endif
#if (DAP_JTAG != 0)
//DAP_Data.jtag_dev.count = 0;
#endif

  DAP_SETUP();  // Device specific setup
}

 

 

 

dfa

你可能感兴趣的:(IBDAP-CMSIS-DAP)