jackcartersmith/blackmagic
1
0
Fork 0
Code Issues Pull Requests Releases Activity
blackmagic/src/platforms/hosted/stlinkv2.c
Uwe Bonnes fdc654cfb3 stlink: Implementation of frequency setting.
2020-12-12 18:33:09 +01:00

1165 lines
34 KiB
C
Raw Blame History

/*
* This file is part of the Black Magic Debug project.
*
* Copyright (C) 2019-2020 Uwe Bonnes <bon@elektron.ikp.physik.tu-darmstadt.de>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/* Much code and ideas shamelessly taken form
* https://github.com/texane/stlink.git
* git://git.code.sf.net/p/openocd/code
* https://github.com/pavelrevak/pystlink
* https://github.com/pavelrevak/pystlink
*
* with some contribution.
*/
#include "general.h"
#include "gdb_if.h"
#include "adiv5.h"
#include "bmp_hosted.h"
#include "stlinkv2.h"
#include "exception.h"
#include "jtag_devs.h"
#include "target.h"
#include "cortexm.h"
#include <assert.h>
#include <unistd.h>
#include <signal.h>
#include <ctype.h>
#include <sys/time.h>
#include "cl_utils.h"
#define VENDOR_ID_STLINK 0x483
#define PRODUCT_ID_STLINK_MASK 0xffe0
#define PRODUCT_ID_STLINK_GROUP 0x3740
#define PRODUCT_ID_STLINKV1 0x3744
#define PRODUCT_ID_STLINKV2 0x3748
#define PRODUCT_ID_STLINKV21 0x374b
#define PRODUCT_ID_STLINKV21_MSD 0x3752
#define PRODUCT_ID_STLINKV3 0x374f
#define PRODUCT_ID_STLINKV3E 0x374e
#define STLINK_SWIM_ERR_OK 0x00
#define STLINK_SWIM_BUSY 0x01
#define STLINK_DEBUG_ERR_OK 0x80
#define STLINK_DEBUG_ERR_FAULT 0x81
#define STLINK_JTAG_UNKNOWN_JTAG_CHAIN 0x04
#define STLINK_NO_DEVICE_CONNECTED 0x05
#define STLINK_JTAG_COMMAND_ERROR 0x08
#define STLINK_JTAG_COMMAND_ERROR 0x08
#define STLINK_JTAG_GET_IDCODE_ERROR 0x09
#define STLINK_JTAG_DBG_POWER_ERROR 0x0b
#define STLINK_SWD_AP_WAIT 0x10
#define STLINK_SWD_AP_FAULT 0x11
#define STLINK_SWD_AP_ERROR 0x12
#define STLINK_SWD_AP_PARITY_ERROR 0x13
#define STLINK_JTAG_WRITE_ERROR 0x0c
#define STLINK_JTAG_WRITE_VERIF_ERROR 0x0d
#define STLINK_SWD_DP_WAIT 0x14
#define STLINK_SWD_DP_FAULT 0x15
#define STLINK_SWD_DP_ERROR 0x16
#define STLINK_SWD_DP_PARITY_ERROR 0x17
#define STLINK_SWD_AP_WDATA_ERROR 0x18
#define STLINK_SWD_AP_STICKY_ERROR 0x19
#define STLINK_SWD_AP_STICKYORUN_ERROR 0x1a
#define STLINK_BAD_AP_ERROR 0x1d
#define STLINK_TOO_MANY_AP_ERROR 0x29
#define STLINK_JTAG_UNKNOWN_CMD 0x42
#define STLINK_CORE_RUNNING 0x80
#define STLINK_CORE_HALTED 0x81
#define STLINK_CORE_STAT_UNKNOWN -1
#define STLINK_GET_VERSION 0xF1
#define STLINK_DEBUG_COMMAND 0xF2
#define STLINK_DFU_COMMAND 0xF3
#define STLINK_SWIM_COMMAND 0xF4
#define STLINK_GET_CURRENT_MODE 0xF5
#define STLINK_GET_TARGET_VOLTAGE 0xF7
#define STLINK_DEV_DFU_MODE 0x00
#define STLINK_DEV_MASS_MODE 0x01
#define STLINK_DEV_DEBUG_MODE 0x02
#define STLINK_DEV_SWIM_MODE 0x03
#define STLINK_DEV_BOOTLOADER_MODE 0x04
#define STLINK_DEV_UNKNOWN_MODE -1
#define STLINK_DFU_EXIT 0x07
#define STLINK_SWIM_ENTER 0x00
#define STLINK_SWIM_EXIT 0x01
#define STLINK_SWIM_READ_CAP 0x02
#define STLINK_SWIM_SPEED 0x03
#define STLINK_SWIM_ENTER_SEQ 0x04
#define STLINK_SWIM_GEN_RST 0x05
#define STLINK_SWIM_RESET 0x06
#define STLINK_SWIM_ASSERT_RESET 0x07
#define STLINK_SWIM_DEASSERT_RESET 0x08
#define STLINK_SWIM_READSTATUS 0x09
#define STLINK_SWIM_WRITEMEM 0x0a
#define STLINK_SWIM_READMEM 0x0b
#define STLINK_SWIM_READBUF 0x0c
#define STLINK_DEBUG_GETSTATUS 0x01
#define STLINK_DEBUG_FORCEDEBUG 0x02
#define STLINK_DEBUG_APIV1_RESETSYS 0x03
#define STLINK_DEBUG_APIV1_READALLREGS 0x04
#define STLINK_DEBUG_APIV1_READREG 0x05
#define STLINK_DEBUG_APIV1_WRITEREG 0x06
#define STLINK_DEBUG_READMEM_32BIT 0x07
#define STLINK_DEBUG_WRITEMEM_32BIT 0x08
#define STLINK_DEBUG_RUNCORE 0x09
#define STLINK_DEBUG_STEPCORE 0x0a
#define STLINK_DEBUG_APIV1_SETFP 0x0b
#define STLINK_DEBUG_READMEM_8BIT 0x0c
#define STLINK_DEBUG_WRITEMEM_8BIT 0x0d
#define STLINK_DEBUG_APIV1_CLEARFP 0x0e
#define STLINK_DEBUG_APIV1_WRITEDEBUGREG 0x0f
#define STLINK_DEBUG_APIV1_SETWATCHPOINT 0x10
#define STLINK_DEBUG_ENTER_JTAG_RESET 0x00
#define STLINK_DEBUG_ENTER_SWD_NO_RESET 0xa3
#define STLINK_DEBUG_ENTER_JTAG_NO_RESET 0xa4
#define STLINK_DEBUG_APIV1_ENTER 0x20
#define STLINK_DEBUG_EXIT 0x21
#define STLINK_DEBUG_READCOREID 0x22
#define STLINK_DEBUG_APIV2_ENTER 0x30
#define STLINK_DEBUG_APIV2_READ_IDCODES 0x31
#define STLINK_DEBUG_APIV2_RESETSYS 0x32
#define STLINK_DEBUG_APIV2_READREG 0x33
#define STLINK_DEBUG_APIV2_WRITEREG 0x34
#define STLINK_DEBUG_APIV2_WRITEDEBUGREG 0x35
#define STLINK_DEBUG_APIV2_READDEBUGREG 0x36
#define STLINK_DEBUG_APIV2_READALLREGS 0x3A
#define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B
#define STLINK_DEBUG_APIV2_DRIVE_NRST 0x3C
#define STLINK_DEBUG_APIV2_GETLASTRWSTATUS2 0x3E
#define STLINK_DEBUG_APIV2_START_TRACE_RX 0x40
#define STLINK_DEBUG_APIV2_STOP_TRACE_RX 0x41
#define STLINK_DEBUG_APIV2_GET_TRACE_NB 0x42
#define STLINK_DEBUG_APIV2_SWD_SET_FREQ 0x43
#define STLINK_DEBUG_APIV2_JTAG_SET_FREQ 0x44
#define STLINK_DEBUG_APIV2_READ_DAP_REG 0x45
#define STLINK_DEBUG_APIV2_WRITE_DAP_REG 0x46
#define STLINK_DEBUG_APIV2_READMEM_16BIT 0x47
#define STLINK_DEBUG_APIV2_WRITEMEM_16BIT 0x48
#define STLINK_DEBUG_APIV2_INIT_AP 0x4B
#define STLINK_DEBUG_APIV2_CLOSE_AP_DBG 0x4C
#define STLINK_APIV3_SET_COM_FREQ 0x61
#define STLINK_APIV3_GET_COM_FREQ 0x62
#define STLINK_APIV3_GET_VERSION_EX 0xFB
#define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW 0x00
#define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH 0x01
#define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02
#define STLINK_TRACE_SIZE 4096
#define STLINK_TRACE_MAX_HZ 2000000
#define STLINK_V3_MAX_FREQ_NB 10
/** */
enum stlink_mode {
STLINK_MODE_UNKNOWN = 0,
STLINK_MODE_DFU,
STLINK_MODE_MASS,
STLINK_MODE_DEBUG_JTAG,
STLINK_MODE_DEBUG_SWD,
STLINK_MODE_DEBUG_SWIM
};
enum transport_mode_t{
STLINK_MODE_SWD = 0,
STLINK_MODE_JTAG
};
typedef struct {
libusb_context* libusb_ctx;
uint16_t vid;
uint16_t pid;
bool srst;
uint8_t dap_select;
uint8_t ep_tx;
uint8_t ver_hw; /* 20, 21 or 31 deciphered from USB PID.*/
uint8_t ver_stlink; /* 2 or 3 from API.*/
uint8_t ver_api;
uint8_t ver_jtag;
uint8_t ver_mass;
uint8_t ver_swim;
uint8_t ver_bridge;
uint16_t block_size;
bool ap_error;
} stlink;
stlink Stlink;
static int stlink_usb_get_rw_status(bool verbose);
int debug_level = 0;
#define STLINK_ERROR_DP_FAULT -2
/**
Converts an STLINK status code held in the first byte of a response to
readable error
*/
static int stlink_usb_error_check(uint8_t *data, bool verbose)
{
switch (data[0]) {
case STLINK_DEBUG_ERR_OK:
return STLINK_ERROR_OK;
case STLINK_DEBUG_ERR_FAULT:
if (verbose)
DEBUG_WARN("SWD fault response (0x%x)\n",
STLINK_DEBUG_ERR_FAULT);
return STLINK_ERROR_FAIL;
case STLINK_JTAG_UNKNOWN_JTAG_CHAIN:
if (verbose)
DEBUG_WARN("Unknown JTAG chain\n");
return STLINK_ERROR_FAIL;
case STLINK_NO_DEVICE_CONNECTED:
if (verbose)
DEBUG_WARN("No device connected\n");
return STLINK_ERROR_FAIL;
case STLINK_JTAG_COMMAND_ERROR:
if (verbose)
DEBUG_WARN("Command error\n");
return STLINK_ERROR_FAIL;
case STLINK_JTAG_GET_IDCODE_ERROR:
if (verbose)
DEBUG_WARN("Failure reading IDCODE\n");
return STLINK_ERROR_FAIL;
case STLINK_JTAG_DBG_POWER_ERROR:
if (verbose)
DEBUG_WARN("Failure powering DBG\n");
return STLINK_ERROR_WAIT;
case STLINK_SWD_AP_WAIT:
if (verbose)
DEBUG_WARN("wait status SWD_AP_WAIT (0x%x)\n",
STLINK_SWD_AP_WAIT);
return STLINK_ERROR_WAIT;
case STLINK_SWD_DP_WAIT:
if (verbose)
DEBUG_WARN("wait status SWD_DP_WAIT (0x%x)\n",
STLINK_SWD_DP_WAIT);
return STLINK_ERROR_WAIT;
case STLINK_JTAG_WRITE_ERROR:
if (verbose)
DEBUG_WARN("Write error\n");
return STLINK_ERROR_FAIL;
case STLINK_JTAG_WRITE_VERIF_ERROR:
if (verbose)
DEBUG_WARN("Write verify error, ignoring\n");
return STLINK_ERROR_OK;
case STLINK_SWD_AP_FAULT:
/* git://git.ac6.fr/openocd commit 657e3e885b9ee10
* returns STLINK_ERROR_OK with the comment:
* Change in error status when reading outside RAM.
* This fix allows CDT plugin to visualize memory.
*/
Stlink.ap_error = true;
if (verbose)
DEBUG_WARN("STLINK_SWD_AP_FAULT\n");
return STLINK_ERROR_DP_FAULT;
case STLINK_SWD_AP_ERROR:
if (verbose)
DEBUG_WARN("STLINK_SWD_AP_ERROR\n");
return STLINK_ERROR_FAIL;
case STLINK_SWD_AP_PARITY_ERROR:
if (verbose)
DEBUG_WARN("STLINK_SWD_AP_PARITY_ERROR\n");
return STLINK_ERROR_FAIL;
case STLINK_SWD_DP_FAULT:
if (verbose)
DEBUG_WARN("STLINK_SWD_DP_FAULT\n");
return STLINK_ERROR_FAIL;
case STLINK_SWD_DP_ERROR:
if (verbose)
DEBUG_WARN("STLINK_SWD_DP_ERROR\n");
raise_exception(EXCEPTION_ERROR, "STLINK_SWD_DP_ERROR");
return STLINK_ERROR_FAIL;
case STLINK_SWD_DP_PARITY_ERROR:
if (verbose)
DEBUG_WARN("STLINK_SWD_DP_PARITY_ERROR\n");
return STLINK_ERROR_FAIL;
case STLINK_SWD_AP_WDATA_ERROR:
if (verbose)
DEBUG_WARN("STLINK_SWD_AP_WDATA_ERROR\n");
return STLINK_ERROR_FAIL;
case STLINK_SWD_AP_STICKY_ERROR:
if (verbose)
DEBUG_WARN("STLINK_SWD_AP_STICKY_ERROR\n");
Stlink.ap_error = true;
return STLINK_ERROR_FAIL;
case STLINK_SWD_AP_STICKYORUN_ERROR:
if (verbose)
DEBUG_WARN("STLINK_SWD_AP_STICKYORUN_ERROR\n");
return STLINK_ERROR_FAIL;
case STLINK_BAD_AP_ERROR:
/* ADIV5 probe 256 APs, most of them are non exisitant.*/
return STLINK_ERROR_FAIL;
case STLINK_TOO_MANY_AP_ERROR:
/* TI TM4C duplicates AP. Error happens at AP9.*/
if (verbose)
DEBUG_WARN("STLINK_TOO_MANY_AP_ERROR\n");
return STLINK_ERROR_FAIL;
case STLINK_JTAG_UNKNOWN_CMD :
if (verbose)
DEBUG_WARN("STLINK_JTAG_UNKNOWN_CMD\n");
return STLINK_ERROR_FAIL;
default:
if (verbose)
DEBUG_WARN("unknown/unexpected STLINK status code 0x%x\n", data[0]);
return STLINK_ERROR_FAIL;
}
}
static int stlink_send_recv_retry(uint8_t *txbuf, size_t txsize,
uint8_t *rxbuf, size_t rxsize)
{
uint32_t start = platform_time_ms();
int res;
usb_link_t *link = info.usb_link;
while(1) {
send_recv(link, txbuf, txsize, rxbuf, rxsize);
res = stlink_usb_error_check(rxbuf, false);
if (res == STLINK_ERROR_OK)
return res;
uint32_t now = platform_time_ms();
if (((now - start) > cortexm_wait_timeout) ||
(res != STLINK_ERROR_WAIT)) {
DEBUG_WARN("write_retry failed. ");
return res;
}
}
return res;
}
static int read_retry(uint8_t *txbuf, size_t txsize,
uint8_t *rxbuf, size_t rxsize)
{
uint32_t start = platform_time_ms();
int res;
while(1) {
send_recv(info.usb_link, txbuf, txsize, rxbuf, rxsize);
res = stlink_usb_get_rw_status(false);
if (res == STLINK_ERROR_OK)
return res;
uint32_t now = platform_time_ms();
if (((now -start) > 1000) || (res != STLINK_ERROR_WAIT)) {
DEBUG_WARN("read_retry failed. ");
stlink_usb_get_rw_status(true);
return res;
}
}
return res;
}
static int write_retry(uint8_t *cmdbuf, size_t cmdsize,
uint8_t *txbuf, size_t txsize)
{
uint32_t start = platform_time_ms();
int res;
usb_link_t *link = info.usb_link;
while(1) {
send_recv(link, cmdbuf, cmdsize, NULL, 0);
send_recv(link, txbuf, txsize, NULL, 0);
res = stlink_usb_get_rw_status(false);
if (res == STLINK_ERROR_OK)
return res;
uint32_t now = platform_time_ms();
if (((now - start) > 1000) || (res != STLINK_ERROR_WAIT)) {
stlink_usb_get_rw_status(true);
return res;
}
}
return res;
}
static void stlink_version(bmp_info_t *info)
{
if (Stlink.ver_hw == 30) {
uint8_t cmd[16] = {STLINK_APIV3_GET_VERSION_EX};
uint8_t data[12];
int size = send_recv(info->usb_link, cmd, 16, data, 12);
if (size == -1) {
DEBUG_WARN("[!] stlink_send_recv STLINK_APIV3_GET_VERSION_EX\n");
}
Stlink.ver_stlink = data[0];
Stlink.ver_swim = data[1];
Stlink.ver_jtag = data[2];
Stlink.ver_mass = data[3];
Stlink.ver_bridge = data[4];
Stlink.block_size = 512;
Stlink.vid = data[3] << 9 | data[8];
Stlink.pid = data[5] << 11 | data[10];
} else {
uint8_t cmd[16] = {STLINK_GET_VERSION};
uint8_t data[6];
int size = send_recv(info->usb_link, cmd, 16, data, 6);
if (size == -1) {
DEBUG_WARN("[!] stlink_send_recv STLINK_GET_VERSION_EX\n");
}
Stlink.vid = data[3] << 8 | data[2];
Stlink.pid = data[5] << 8 | data[4];
int version = data[0] << 8 | data[1]; /* Big endian here!*/
Stlink.block_size = 64;
Stlink.ver_stlink = (version >> 12) & 0x0f;
Stlink.ver_jtag = (version >> 6) & 0x3f;
if ((Stlink.pid == PRODUCT_ID_STLINKV21_MSD) ||
(Stlink.pid == PRODUCT_ID_STLINKV21)) {
Stlink.ver_mass = (version >> 0) & 0x3f;
} else {
Stlink.ver_swim = (version >> 0) & 0x3f;
}
}
DEBUG_INFO("STLink firmware version: V%dJ%d",Stlink.ver_stlink,
Stlink.ver_jtag);
if (Stlink.ver_hw == 30) {
DEBUG_INFO("M%dB%dS%d", Stlink.ver_mass, Stlink.ver_bridge, Stlink.ver_swim);
} else if (Stlink.ver_hw == 20) {
DEBUG_INFO("S%d", Stlink.ver_swim);
} else if (Stlink.ver_hw == 21) {
DEBUG_INFO("M%d", Stlink.ver_mass);
}
DEBUG_INFO("\n");
}
static bool stlink_leave_state(bmp_info_t *info)
{
uint8_t cmd[16] = {STLINK_GET_CURRENT_MODE};
uint8_t data[2];
send_recv(info->usb_link,cmd, 16, data, 2);
if (data[0] == STLINK_DEV_DFU_MODE) {
uint8_t dfu_cmd[16] = {STLINK_DFU_COMMAND, STLINK_DFU_EXIT};
DEBUG_INFO("Leaving DFU Mode\n");
send_recv(info->usb_link, dfu_cmd, 16, NULL, 0);
return true;
} else if (data[0] == STLINK_DEV_SWIM_MODE) {
uint8_t swim_cmd[16] = {STLINK_SWIM_COMMAND, STLINK_SWIM_EXIT};
DEBUG_INFO("Leaving SWIM Mode\n");
send_recv(info->usb_link, swim_cmd, 16, NULL, 0);
} else if (data[0] == STLINK_DEV_DEBUG_MODE) {
uint8_t dbg_cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_EXIT};
DEBUG_INFO("Leaving DEBUG Mode\n");
send_recv(info->usb_link, dbg_cmd, 16, NULL, 0);
} else if (data[0] == STLINK_DEV_BOOTLOADER_MODE) {
DEBUG_INFO("Leaving BOOTLOADER Mode\n");
} else if (data[0] == STLINK_DEV_MASS_MODE) {
DEBUG_INFO("Leaving MASS Mode\n");
} else {
DEBUG_INFO("Unknown Mode %02x\n", data[0]);
}
return false;
}
const char *stlink_target_voltage(bmp_info_t *info)
{
uint8_t cmd[16] = {STLINK_GET_TARGET_VOLTAGE};
uint8_t data[8];
send_recv(info->usb_link, cmd, 16, data, 8);
uint16_t adc[2];
adc[0] = data[0] | data[1] << 8; /* Calibration value? */
adc[1] = data[4] | data[5] << 8; /* Measured value?*/
float result = 0.0;
if (adc[0])
result = 2.0 * adc[1] * 1.2 / adc[0];
static char res[6];
sprintf(res, "%4.2fV", result);
return res;
}
static void stlink_resetsys(bmp_info_t *info)
{
uint8_t cmd[16] = {STLINK_DEBUG_COMMAND ,STLINK_DEBUG_APIV2_RESETSYS};
uint8_t data[2];
send_recv(info->usb_link, cmd, 16, data, 2);
}
int stlink_init(bmp_info_t *info)
{
usb_link_t *sl = calloc(1, sizeof(usb_link_t));
if (!sl)
return -1;
info->usb_link = sl;
sl->ul_libusb_ctx = info->libusb_ctx;
libusb_device **devs, *dev;
ssize_t cnt;
cnt = libusb_get_device_list(info->libusb_ctx, &devs);
if (cnt < 0) {
DEBUG_WARN("FATAL: Stlink libusb_get_device_list failed\n");
return -1;
}
int i = 0;
bool found = false;
while ((dev = devs[i++]) != NULL) {
struct libusb_device_descriptor desc;
int r = libusb_get_device_descriptor(dev, &desc);
if (r < 0) {
DEBUG_WARN("libusb_get_device_descriptor failed %s",
libusb_strerror(r));
return -1;
}
if ((desc.idVendor != info->vid) ||
(desc.idProduct != info->pid) ||
(libusb_open(dev, &sl->ul_libusb_device_handle)
!= LIBUSB_SUCCESS)) {
continue;
}
char serial[64];
r = libusb_get_string_descriptor_ascii(
sl->ul_libusb_device_handle, desc.iSerialNumber,
(uint8_t*)serial,sizeof(serial));
if (r <= 0 || !strstr(serial, info->serial)) {
libusb_close(sl->ul_libusb_device_handle);
continue;
}
found = true;
break;
}
libusb_free_device_list(devs, 1);
if (!found)
return 0;
if (info->pid == PRODUCT_ID_STLINKV2) {
Stlink.ver_hw = 20;
info->usb_link->ep_tx = 2;
Stlink.ep_tx = 2;
} else if ((info->pid == PRODUCT_ID_STLINKV21)||
(info->pid == PRODUCT_ID_STLINKV21_MSD)) {
Stlink.ver_hw = 21;
info->usb_link->ep_tx = 1;
Stlink.ep_tx = 1;
} else if ((info->pid == PRODUCT_ID_STLINKV3) ||
(info->pid == PRODUCT_ID_STLINKV3E)) {
Stlink.ver_hw = 30;
info->usb_link->ep_tx = 1;
Stlink.ep_tx = 1;
}
info->usb_link->ep_rx = 1;
int config;
int r = libusb_get_configuration(sl->ul_libusb_device_handle, &config);
if (r) {
DEBUG_WARN("FATAL: Stlink libusb_get_configuration failed %d: %s",
r, libusb_strerror(r));
return -1;
}
if (config != 1) {
r = libusb_set_configuration(sl->ul_libusb_device_handle, 0);
if (r) {
DEBUG_WARN("FATAL: Stlinklibusb_set_configuration "
"failed %d: %s", r, libusb_strerror(r));
return -1;
}
}
r = libusb_claim_interface(sl->ul_libusb_device_handle, 0);
if (r) {
DEBUG_WARN("FATAL: Stlink libusb_claim_interface failed %s\n",
libusb_strerror(r));
return -1;
}
sl->req_trans = libusb_alloc_transfer(0);
sl->rep_trans = libusb_alloc_transfer(0);
stlink_version(info);
if ((Stlink.ver_stlink < 3 && Stlink.ver_jtag < 32) ||
(Stlink.ver_stlink == 3 && Stlink.ver_jtag < 3)) {
/* Maybe the adapter is in some strange state. Try to reset */
int result = libusb_reset_device(sl->ul_libusb_device_handle);
DEBUG_WARN("INFO: Trying Stlink reset\n");
if (result == LIBUSB_ERROR_BUSY) { /* Try again */
platform_delay(50);
result = libusb_reset_device(sl->ul_libusb_device_handle);
}
if (result != LIBUSB_SUCCESS) {
DEBUG_WARN("FATAL: Stlink libusb_reset_device failed\n");
return -1;
}
stlink_version(info);
}
if ((Stlink.ver_stlink < 3 && Stlink.ver_jtag < 32) ||
(Stlink.ver_stlink == 3 && Stlink.ver_jtag < 3)) {
DEBUG_WARN("Please update Firmware\n");
return -1;
}
if (stlink_leave_state(info)) {
DEBUG_WARN("Stlink board was in DFU mode. Restart\n");
return -1;
}
stlink_resetsys(info);
return 0;
}
void stlink_srst_set_val(bmp_info_t *info, bool assert)
{
uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
STLINK_DEBUG_APIV2_DRIVE_NRST,
(assert)? STLINK_DEBUG_APIV2_DRIVE_NRST_LOW
: STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH};
uint8_t data[2];
Stlink.srst = assert;
send_recv(info->usb_link, cmd, 16, data, 2);
stlink_usb_error_check(data, true);
}
bool stlink_srst_get_val(void)
{
return Stlink.srst;
}
int stlink_hwversion(void)
{
return Stlink.ver_stlink;
}
static int stlink_enter_debug_jtag(bmp_info_t *info)
{
stlink_leave_state(info);
uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
STLINK_DEBUG_APIV2_ENTER,
STLINK_DEBUG_ENTER_JTAG_NO_RESET};
uint8_t data[2];
send_recv(info->usb_link, cmd, 16, data, 2);
return stlink_usb_error_check(data, true);
}
static uint32_t stlink_read_coreid(void)
{
uint8_t cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_READ_IDCODES};
uint8_t data[12];
send_recv(info.usb_link, cmd, 16, data, 12);
uint32_t id = data[4] | data[5] << 8 | data[6] << 16 | data[7] << 24;
DEBUG_INFO("Read Core ID: 0x%08" PRIx32 "\n", id);
return id;
}
static int stlink_read_idcodes(bmp_info_t *info, uint32_t *idcodes)
{
uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
STLINK_DEBUG_APIV2_READ_IDCODES};
uint8_t data[12];
send_recv(info->usb_link, cmd, 16, data, 12);
if (stlink_usb_error_check(data, true))
return 0;
uint8_t *p = data + 4;
idcodes[0] = p[0] | p[1] << 8 | p[2] << 16 | p[3] << 24;
p += 4;
idcodes[1] = p[0] | p[1] << 8 | p[2] << 16 | p[3] << 24;
return 2;
}
uint32_t stlink_dp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
uint16_t addr, uint32_t value);
static uint32_t stlink_dp_read(ADIv5_DP_t *dp, uint16_t addr)
{
if (addr & ADIV5_APnDP) {
stlink_dp_low_access(dp, ADIV5_LOW_READ, addr, 0);
return stlink_dp_low_access(dp, ADIV5_LOW_READ,
ADIV5_DP_RDBUFF, 0);
} else {
return stlink_dp_low_access(dp, ADIV5_LOW_READ, addr, 0);
}
}
uint32_t stlink_dp_error(ADIv5_DP_t *dp)
{
uint32_t err, clr = 0;
err = stlink_dp_read(dp, ADIV5_DP_CTRLSTAT) &
(ADIV5_DP_CTRLSTAT_STICKYORUN | ADIV5_DP_CTRLSTAT_STICKYCMP |
ADIV5_DP_CTRLSTAT_STICKYERR | ADIV5_DP_CTRLSTAT_WDATAERR);
if(err & ADIV5_DP_CTRLSTAT_STICKYORUN)
clr |= ADIV5_DP_ABORT_ORUNERRCLR;
if(err & ADIV5_DP_CTRLSTAT_STICKYCMP)
clr |= ADIV5_DP_ABORT_STKCMPCLR;
if(err & ADIV5_DP_CTRLSTAT_STICKYERR)
clr |= ADIV5_DP_ABORT_STKERRCLR;
if(err & ADIV5_DP_CTRLSTAT_WDATAERR)
clr |= ADIV5_DP_ABORT_WDERRCLR;
adiv5_dp_write(dp, ADIV5_DP_ABORT, clr);
dp->fault = 0;
if (err)
DEBUG_WARN("stlink_dp_error %d\n", err);
err |= Stlink.ap_error;
Stlink.ap_error = false;
return err;
}
void stlink_dp_abort(ADIv5_DP_t *dp, uint32_t abort)
{
adiv5_dp_write(dp, ADIV5_DP_ABORT, abort);
}
static int stlink_read_dp_register(uint16_t port, uint16_t addr, uint32_t *reg)
{
uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
STLINK_DEBUG_APIV2_READ_DAP_REG,
port & 0xff, port >> 8,
addr & 0xff, addr >> 8};
if (port == STLINK_DEBUG_PORT_ACCESS && Stlink.dap_select)
cmd[4] = ((Stlink.dap_select & 0xf) << 4) | (addr & 0xf);
else
cmd[4] = addr & 0xff;
uint8_t data[8];
int res = stlink_send_recv_retry(cmd, 16, data, 8);
if (res == STLINK_ERROR_OK) {
uint32_t ret = data[4] | data[5] << 8 | data[6] << 16 | data[7] << 24;
*reg = ret;
} else {
DEBUG_WARN("%s error %d\n", __func__, res);
}
return res;
}
static int stlink_write_dp_register(uint16_t port, uint16_t addr, uint32_t val)
{
if (port == STLINK_DEBUG_PORT_ACCESS && addr == 8) {
Stlink.dap_select = val;
DEBUG_PROBE("Caching SELECT 0x%02" PRIx32 "\n", val);
return STLINK_ERROR_OK;
} else {
uint8_t cmd[16] = {
STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_WRITE_DAP_REG,
port & 0xff, port >> 8,
addr & 0xff, addr >> 8,
val & 0xff, (val >> 8) & 0xff, (val >> 16) & 0xff,
(val >> 24) & 0xff};
uint8_t data[2];
stlink_send_recv_retry(cmd, 16, data, 2);
return stlink_usb_error_check(data, true);
}
}
uint32_t stlink_dp_low_access(ADIv5_DP_t *dp, uint8_t RnW,
uint16_t addr, uint32_t value)
{
uint32_t response = 0;
int res;
if (RnW) {
res = stlink_read_dp_register(
STLINK_DEBUG_PORT_ACCESS, addr, &response);
} else {
res = stlink_write_dp_register(STLINK_DEBUG_PORT_ACCESS, addr, value);
}
if (res == STLINK_ERROR_WAIT)
raise_exception(EXCEPTION_TIMEOUT, "DP ACK timeout");
if(res == STLINK_ERROR_DP_FAULT) {
dp->fault = 1;
return 0;
}
if(res == STLINK_ERROR_FAIL)
raise_exception(EXCEPTION_ERROR, "SWDP invalid ACK");
return response;
}
static bool stlink_ap_setup(int ap)
{
if (ap > 7)
return false;
uint8_t cmd[16] = {
STLINK_DEBUG_COMMAND,
STLINK_DEBUG_APIV2_INIT_AP,
ap,
};
uint8_t data[2];
DEBUG_PROBE("Open AP %d\n", ap);
stlink_send_recv_retry(cmd, 16, data, 2);
int res = stlink_usb_error_check(data, true);
if (res) {
if (Stlink.ver_hw == 30) {
DEBUG_WARN("STLINKV3 only connects to STM8/32!\n");
}
return false;
}
return true;
}
static void stlink_ap_cleanup(int ap)
{
uint8_t cmd[16] = {
STLINK_DEBUG_COMMAND,
STLINK_DEBUG_APIV2_CLOSE_AP_DBG,
ap,
};
uint8_t data[2];
send_recv(info.usb_link, cmd, 16, data, 2);
DEBUG_PROBE("Close AP %d\n", ap);
stlink_usb_error_check(data, true);
}
static int stlink_usb_get_rw_status(bool verbose)
{
uint8_t cmd[16] = {
STLINK_DEBUG_COMMAND,
STLINK_DEBUG_APIV2_GETLASTRWSTATUS2
};
uint8_t data[12];
send_recv(info.usb_link, cmd, 16, data, 12);
return stlink_usb_error_check(data, verbose);
}
static void stlink_readmem(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
{
if (len == 0)
return;
size_t read_len = len;
uint8_t type;
if (src & 1 || len & 1) {
type = STLINK_DEBUG_READMEM_8BIT;
if (len > Stlink.block_size) {
DEBUG_WARN(" Too large!\n");
return;
}
if (len == 1)
read_len ++; /* Fix read length as in openocd*/
} else if (src & 3 || len & 3) {
type = STLINK_DEBUG_APIV2_READMEM_16BIT;
} else {
type = STLINK_DEBUG_READMEM_32BIT;
}
uint8_t cmd[16] = {
STLINK_DEBUG_COMMAND,
type,
src & 0xff, (src >> 8) & 0xff, (src >> 16) & 0xff,
(src >> 24) & 0xff,
len & 0xff, len >> 8, ap->apsel};
int res = read_retry(cmd, 16, dest, read_len);
if (res != STLINK_ERROR_OK) {
/* FIXME: What is the right measure when failing?
*
* E.g. TM4C129 gets here when NRF probe reads 0x10000010
* Approach taken:
* Fill the memory with some fixed pattern so hopefully
* the caller notices the error*/
DEBUG_WARN("stlink_readmem from %" PRIx32 " to %" PRIx32 ", len %"
PRIx32 "failed\n", src, dest, (uint32_t) len);
memset(dest, 0xff, len);
}
DEBUG_PROBE("stlink_readmem from %" PRIx32 " to %" PRIx32 ", len %" PRIx32
"\n", src, dest, (uint32_t) len);
}
static void stlink_writemem8(usb_link_t *link, ADIv5_AP_t *ap, uint32_t addr,
size_t len, uint8_t *buffer)
{
(void)link;
while (len) {
size_t length;
/* OpenOCD has some note about writemem8*/
if (len > Stlink.block_size)
length = Stlink.block_size;
else
length = len;
uint8_t cmd[16] = {
STLINK_DEBUG_COMMAND,
STLINK_DEBUG_WRITEMEM_8BIT,
addr & 0xff, (addr >> 8) & 0xff, (addr >> 16) & 0xff,
(addr >> 24) & 0xff,
length & 0xff, length >> 8, ap->apsel};
send_recv(link, cmd, 16, NULL, 0);
send_recv(link, (void*)buffer, length, NULL, 0);
stlink_usb_get_rw_status(true);
len -= length;
addr += length;
}
}
static void stlink_writemem16(usb_link_t *link, ADIv5_AP_t *ap, uint32_t addr,
size_t len, uint16_t *buffer)
{
(void)link;
uint8_t cmd[16] = {
STLINK_DEBUG_COMMAND,
STLINK_DEBUG_APIV2_WRITEMEM_16BIT,
addr & 0xff, (addr >> 8) & 0xff, (addr >> 16) & 0xff,
(addr >> 24) & 0xff,
len & 0xff, len >> 8, ap->apsel};
send_recv(link, cmd, 16, NULL, 0);
send_recv(link, (void*)buffer, len, NULL, 0);
stlink_usb_get_rw_status(true);
}
static void stlink_writemem32(usb_link_t * link, ADIv5_AP_t *ap, uint32_t addr,
size_t len, uint32_t *buffer)
{
(void)link;
uint8_t cmd[16] = {
STLINK_DEBUG_COMMAND,
STLINK_DEBUG_WRITEMEM_32BIT,
addr & 0xff, (addr >> 8) & 0xff, (addr >> 16) & 0xff,
(addr >> 24) & 0xff,
len & 0xff, len >> 8, ap->apsel};
write_retry(cmd, 16, (void*)buffer, len);
}
static void stlink_regs_read(ADIv5_AP_t *ap, void *data)
{
uint8_t cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_READALLREGS,
ap->apsel};
uint8_t res[88];
DEBUG_PROBE("AP %d: Read all core registers\n", ap->apsel);
send_recv(info.usb_link, cmd, 16, res, 88);
stlink_usb_error_check(res, true);
memcpy(data, res + 4, 84);
}
static uint32_t stlink_reg_read(ADIv5_AP_t *ap, int num)
{
uint8_t cmd[16] = {STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_READREG, num,
ap->apsel};
uint8_t res[8];
send_recv(info.usb_link, cmd, 16, res, 8);
stlink_usb_error_check(res, true);
uint32_t ret = res[0] | res[1] << 8 | res[2] << 16 | res[3] << 24;
DEBUG_PROBE("AP %d: Read reg %02" PRId32 " val 0x%08" PRIx32 "\n",
ap->apsel, num, ret);
return ret;
}
static void stlink_reg_write(ADIv5_AP_t *ap, int num, uint32_t val)
{
uint8_t cmd[16] = {
STLINK_DEBUG_COMMAND, STLINK_DEBUG_APIV2_WRITEREG, num,
val & 0xff, (val >> 8) & 0xff, (val >> 16) & 0xff,
(val >> 24) & 0xff, ap->apsel};
uint8_t res[2];
send_recv(info.usb_link, cmd, 16, res, 2);
DEBUG_PROBE("AP %d: Write reg %02" PRId32 " val 0x%08" PRIx32 "\n",
ap->apsel, num, val);
stlink_usb_error_check(res, true);
}
static void stlink_mem_write_sized( ADIv5_AP_t *ap, uint32_t dest,
const void *src, size_t len,
enum align align)
{
if (len == 0)
return;
usb_link_t *link = info.usb_link;
switch(align) {
case ALIGN_BYTE:
stlink_writemem8(link, ap, dest, len, (uint8_t *) src);
break;
case ALIGN_HALFWORD:
stlink_writemem16(link,ap, dest, len, (uint16_t *) src);
break;
case ALIGN_WORD:
case ALIGN_DWORD:
stlink_writemem32(link, ap, dest, len, (uint32_t *) src);
break;
}
}
static void stlink_ap_write(ADIv5_AP_t *ap, uint16_t addr, uint32_t value)
{
stlink_write_dp_register(ap->apsel, addr, value);
}
static uint32_t stlink_ap_read(ADIv5_AP_t *ap, uint16_t addr)
{
uint32_t ret;
stlink_read_dp_register(ap->apsel, addr, &ret);
return ret;
}
int jtag_scan_stlinkv2(bmp_info_t *info, const uint8_t *irlens)
{
uint32_t idcodes[JTAG_MAX_DEVS+1];
(void) *irlens;
target_list_free();
jtag_dev_count = 0;
memset(&jtag_devs, 0, sizeof(jtag_devs));
if (stlink_enter_debug_jtag(info))
return 0;
jtag_dev_count = stlink_read_idcodes(info, idcodes);
/* Check for known devices and handle accordingly */
for(int i = 0; i < jtag_dev_count; i++)
jtag_devs[i].jd_idcode = idcodes[i];
for(int i = 0; i < jtag_dev_count; i++)
for(int j = 0; dev_descr[j].idcode; j++)
if((jtag_devs[i].jd_idcode & dev_descr[j].idmask) ==
dev_descr[j].idcode) {
if(dev_descr[j].handler)
dev_descr[j].handler(i, dev_descr[j].idcode);
break;
}
return jtag_dev_count;
}
int stlink_jtag_dp_init(ADIv5_DP_t *dp)
{
dp->dp_read = stlink_dp_read;
dp->error = stlink_dp_error;
dp->low_access = stlink_dp_low_access;
dp->abort = stlink_dp_abort;
return true;
}
void stlink_adiv5_dp_defaults(ADIv5_DP_t *dp)
{
dp->ap_regs_read = stlink_regs_read;
dp->ap_reg_read = stlink_reg_read;
dp->ap_reg_write = stlink_reg_write;
dp->ap_setup = stlink_ap_setup;
dp->ap_cleanup = stlink_ap_cleanup;
dp->ap_write = stlink_ap_write;
dp->ap_read = stlink_ap_read;
dp->mem_read = stlink_readmem;
dp->mem_write_sized = stlink_mem_write_sized;
}
int stlink_enter_debug_swd(bmp_info_t *info, ADIv5_DP_t *dp)
{
stlink_leave_state(info);
uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
STLINK_DEBUG_APIV2_ENTER,
STLINK_DEBUG_ENTER_SWD_NO_RESET};
uint8_t data[2];
stlink_send_recv_retry(cmd, 16, data, 2);
if (stlink_usb_error_check(data, true))
return -1;
dp->idcode = stlink_read_coreid();
dp->dp_read = stlink_dp_read;
dp->error = stlink_dp_error;
dp->low_access = stlink_dp_low_access;
dp->abort = stlink_dp_abort;
stlink_dp_error(dp);
return 0;
}
#define V2_USED_SWD_CYCLES 20
#define V2_CYCLES_PER_CNT 20
#define V2_CLOCK_RATE (72*1000*1000)
/* Above values reproduce the known values for V2
#include <stdio.h>
int main(void)
{
int divs[] = {0, 1,2,3,7,15,31,40,79,158,265,798};
for (int i = 0; i < (sizeof(divs) /sizeof(divs[0])); i++) {
float ret = 72.0 * 1000 * 1000 / (20 + 20 * divs[i]);
printf("%3d: %6.4f MHz\n", divs[i], ret/ 1000000);
}
return 0;
}
*/
static int divisor;
static unsigned int v3_freq[2];
void stlink_max_frequency_set(bmp_info_t *info, uint32_t freq)
{
if (Stlink.ver_hw == 30) {
uint8_t cmd[16] = {STLINK_DEBUG_COMMAND,
STLINK_APIV3_GET_COM_FREQ,
info->is_jtag ? STLINK_MODE_JTAG : STLINK_MODE_SWD};
uint8_t data[52];
send_recv(info->usb_link, cmd, 16, data, 52);
stlink_usb_error_check(data, true);
volatile uint8_t *p = data + 12;
int i;
unsigned int last_freq = 0;
DEBUG_INFO("Available speed settings: ");
for (i = 0; i < STLINK_V3_MAX_FREQ_NB; i++) {
unsigned int new_freq = *p++;
new_freq |= *p++ << 8;
new_freq |= *p++ << 16;
new_freq |= *p++ << 24;
if (!new_freq)
break;
else
last_freq = new_freq;
DEBUG_INFO("%s%d", (i)? "/": "", last_freq);
if ((freq / 1000) >= last_freq)
break;
}
DEBUG_INFO(" kHz for %s\n", (info->is_jtag) ? "JTAG" : "SWD");
cmd[1] = STLINK_APIV3_SET_COM_FREQ;
cmd[3] = 0;
cmd[4] = (last_freq >> 0) & 0xff;
cmd[5] = (last_freq >> 8) & 0xff;
cmd[6] = (last_freq >> 16) & 0xff;
cmd[7] = (last_freq >> 24) & 0xff;
send_recv(info->usb_link, cmd, 16, data, 8);
stlink_usb_error_check(data, true);
v3_freq[(info->is_jtag) ? 1 : 0] = last_freq * 1000;
} else {
uint8_t cmd[16];
cmd[0] = STLINK_DEBUG_COMMAND;
if (info->is_jtag) {
cmd[1] = STLINK_DEBUG_APIV2_JTAG_SET_FREQ;
/* V2_CLOCK_RATE / (4, 8, 16, ... 256)*/
int div = (V2_CLOCK_RATE + (2 * freq) - 1) / (2 * freq);
if (div & (div -1)) {/* Round up */
int clz = __builtin_clz(div);
divisor = 1 << (32 - clz);
} else
divisor = div;
if (divisor < 4)
divisor = 4;
else if (divisor > 256)
divisor = 256;
} else {
cmd[1] = STLINK_DEBUG_APIV2_SWD_SET_FREQ;
divisor = V2_CLOCK_RATE + freq - 1;
divisor /= freq;
divisor -= V2_USED_SWD_CYCLES;
if (divisor < 0)
divisor = 0;
divisor /= V2_CYCLES_PER_CNT;
}
DEBUG_WARN("Divisor for %6.4f MHz is %" PRIu32 "\n",
freq/1000000.0, divisor);
cmd[2] = divisor & 0xff;
cmd[3] = (divisor >> 8) & 0xff;
uint8_t data[2];
send_recv(info->usb_link, cmd, 16, data, 2);
if (stlink_usb_error_check(data, false))
DEBUG_WARN("Set frequency failed!\n");
}
}
uint32_t stlink_max_frequency_get(bmp_info_t *info)
{
uint32_t ret = 0;
if (Stlink.ver_hw == 30) {
ret = v3_freq[(info->is_jtag) ? STLINK_MODE_JTAG : STLINK_MODE_SWD];
} else {
ret = V2_CLOCK_RATE;
if (info->is_jtag)
ret /= (2 * divisor);
else
ret /= (V2_USED_SWD_CYCLES + (V2_CYCLES_PER_CNT * divisor));
}
return ret;
}
Reference in New Issue View Git Blame Copy Permalink