529 lines
11 KiB
C
529 lines
11 KiB
C
/* SPDX-License-Identifier: GPL-2.0-or-later */
|
|
|
|
/*
|
|
* Copyright (c) 2020, Mellanox Technologies Ltd. - All Rights Reserved
|
|
* Liming Sun <lsun@mellanox.com>
|
|
*/
|
|
|
|
#ifdef HAVE_CONFIG_H
|
|
#include "config.h"
|
|
#endif
|
|
|
|
#include <helper/types.h>
|
|
#include <helper/system.h>
|
|
#include <helper/time_support.h>
|
|
#include <helper/list.h>
|
|
#include <jtag/interface.h>
|
|
#ifdef HAVE_SYS_IOCTL_H
|
|
#include <sys/ioctl.h>
|
|
#endif
|
|
#include <target/arm_adi_v5.h>
|
|
#include <transport/transport.h>
|
|
|
|
/* Rshim channel where the CoreSight register resides. */
|
|
#define RSH_MMIO_CHANNEL_RSHIM 0x1
|
|
|
|
/* APB and tile address translation. */
|
|
#define RSH_CS_ROM_BASE 0x80000000
|
|
#define RSH_CS_TILE_BASE 0x44000000
|
|
#define RSH_CS_TILE_SIZE 0x04000000
|
|
|
|
/*
|
|
* APB-AP Identification Register
|
|
* The default value is defined in "CoreSight on-chip trace and debug
|
|
* (Revision: r1p0)", Section 3.16.5 APB-AP register summary.
|
|
*/
|
|
#define APB_AP_IDR 0x44770002
|
|
|
|
/* CoreSight register definition. */
|
|
#define RSH_CORESIGHT_CTL 0x0e00
|
|
#define RSH_CORESIGHT_CTL_GO_SHIFT 0
|
|
#define RSH_CORESIGHT_CTL_GO_MASK 0x1ULL
|
|
#define RSH_CORESIGHT_CTL_ACTION_SHIFT 1
|
|
#define RSH_CORESIGHT_CTL_ACTION_MASK 0x2ULL
|
|
#define RSH_CORESIGHT_CTL_ADDR_SHIFT 2
|
|
#define RSH_CORESIGHT_CTL_ADDR_MASK 0x7ffffffcULL
|
|
#define RSH_CORESIGHT_CTL_ERR_SHIFT 31
|
|
#define RSH_CORESIGHT_CTL_ERR_MASK 0x80000000ULL
|
|
#define RSH_CORESIGHT_CTL_DATA_SHIFT 32
|
|
#define RSH_CORESIGHT_CTL_DATA_MASK 0xffffffff00000000ULL
|
|
|
|
/* Util macros to access the CoreSight register. */
|
|
#define RSH_CS_GET_FIELD(reg, field) \
|
|
(((uint64_t)(reg) & RSH_CORESIGHT_CTL_##field##_MASK) >> \
|
|
RSH_CORESIGHT_CTL_##field##_SHIFT)
|
|
|
|
#define RSH_CS_SET_FIELD(reg, field, value) \
|
|
(reg) = (((reg) & ~RSH_CORESIGHT_CTL_##field##_MASK) | \
|
|
(((uint64_t)(value) << RSH_CORESIGHT_CTL_##field##_SHIFT) & \
|
|
RSH_CORESIGHT_CTL_##field##_MASK))
|
|
|
|
#ifdef HAVE_SYS_IOCTL_H
|
|
/* Message used to program rshim via ioctl(). */
|
|
typedef struct {
|
|
uint32_t addr;
|
|
uint64_t data;
|
|
} __attribute__((packed)) rshim_ioctl_msg;
|
|
|
|
enum {
|
|
RSH_IOC_READ = _IOWR('R', 0, rshim_ioctl_msg),
|
|
RSH_IOC_WRITE = _IOWR('R', 1, rshim_ioctl_msg),
|
|
};
|
|
#endif
|
|
|
|
/* Use local variable stub for DP/AP registers. */
|
|
static uint32_t dp_ctrl_stat;
|
|
static uint32_t dp_id_code;
|
|
static uint32_t ap_sel, ap_bank;
|
|
static uint32_t ap_csw;
|
|
static uint32_t ap_drw;
|
|
static uint32_t ap_tar, ap_tar_inc;
|
|
|
|
/* Static functions to read/write via rshim/coresight. */
|
|
static int (*rshim_read)(int chan, int addr, uint64_t *value);
|
|
static int (*rshim_write)(int chan, int addr, uint64_t value);
|
|
static int coresight_write(uint32_t tile, uint32_t addr, uint32_t wdata);
|
|
static int coresight_read(uint32_t tile, uint32_t addr, uint32_t *value);
|
|
|
|
/* RShim file handler. */
|
|
static int rshim_fd = -1;
|
|
|
|
/* DAP error code. */
|
|
static int rshim_dap_retval = ERROR_OK;
|
|
|
|
/* Default rshim device. */
|
|
#define RSHIM_DEV_PATH_DEFAULT "/dev/rshim0/rshim"
|
|
static char *rshim_dev_path;
|
|
|
|
static int rshim_dev_read(int chan, int addr, uint64_t *value)
|
|
{
|
|
int rc;
|
|
|
|
addr = (addr & 0xFFFF) | (1 << 16);
|
|
rc = pread(rshim_fd, value, sizeof(*value), addr);
|
|
|
|
#ifdef HAVE_SYS_IOCTL_H
|
|
if (rc < 0 && errno == ENOSYS) {
|
|
rshim_ioctl_msg msg;
|
|
|
|
msg.addr = addr;
|
|
msg.data = 0;
|
|
rc = ioctl(rshim_fd, RSH_IOC_READ, &msg);
|
|
if (!rc)
|
|
*value = msg.data;
|
|
}
|
|
#endif
|
|
|
|
return rc;
|
|
}
|
|
|
|
static int rshim_dev_write(int chan, int addr, uint64_t value)
|
|
{
|
|
int rc;
|
|
|
|
addr = (addr & 0xFFFF) | (1 << 16);
|
|
rc = pwrite(rshim_fd, &value, sizeof(value), addr);
|
|
|
|
#ifdef HAVE_SYS_IOCTL_H
|
|
if (rc < 0 && errno == ENOSYS) {
|
|
rshim_ioctl_msg msg;
|
|
|
|
msg.addr = addr;
|
|
msg.data = value;
|
|
rc = ioctl(rshim_fd, RSH_IOC_WRITE, &msg);
|
|
}
|
|
#endif
|
|
|
|
return rc;
|
|
}
|
|
|
|
/* Convert AP address to tile local address. */
|
|
static void ap_addr_2_tile(int *tile, uint32_t *addr)
|
|
{
|
|
*addr -= RSH_CS_ROM_BASE;
|
|
|
|
if (*addr < RSH_CS_TILE_BASE) {
|
|
*tile = 0;
|
|
} else {
|
|
*addr -= RSH_CS_TILE_BASE;
|
|
*tile = *addr / RSH_CS_TILE_SIZE + 1;
|
|
*addr = *addr % RSH_CS_TILE_SIZE;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Write 4 bytes on the APB bus.
|
|
* tile = 0: access the root CS_ROM table
|
|
* > 0: access the ROM table of cluster (tile - 1)
|
|
*/
|
|
static int coresight_write(uint32_t tile, uint32_t addr, uint32_t wdata)
|
|
{
|
|
uint64_t ctl = 0;
|
|
int rc;
|
|
|
|
if (!rshim_read || !rshim_write)
|
|
return ERROR_FAIL;
|
|
|
|
/*
|
|
* ADDR[28] - must be set to 1 due to coresight ip.
|
|
* ADDR[27:24] - linear tile id
|
|
*/
|
|
addr = (addr >> 2) | (tile << 24);
|
|
if (tile)
|
|
addr |= (1 << 28);
|
|
RSH_CS_SET_FIELD(ctl, ADDR, addr);
|
|
RSH_CS_SET_FIELD(ctl, ACTION, 0); /* write */
|
|
RSH_CS_SET_FIELD(ctl, DATA, wdata);
|
|
RSH_CS_SET_FIELD(ctl, GO, 1); /* start */
|
|
|
|
rshim_write(RSH_MMIO_CHANNEL_RSHIM, RSH_CORESIGHT_CTL, ctl);
|
|
|
|
do {
|
|
rc = rshim_read(RSH_MMIO_CHANNEL_RSHIM,
|
|
RSH_CORESIGHT_CTL, &ctl);
|
|
if (rc < 0) {
|
|
LOG_ERROR("Failed to read rshim.\n");
|
|
return rc;
|
|
}
|
|
} while (RSH_CS_GET_FIELD(ctl, GO));
|
|
|
|
return ERROR_OK;
|
|
}
|
|
|
|
static int coresight_read(uint32_t tile, uint32_t addr, uint32_t *value)
|
|
{
|
|
uint64_t ctl = 0;
|
|
int rc;
|
|
|
|
if (!rshim_read || !rshim_write)
|
|
return ERROR_FAIL;
|
|
|
|
/*
|
|
* ADDR[28] - must be set to 1 due to coresight ip.
|
|
* ADDR[27:24] - linear tile id
|
|
*/
|
|
addr = (addr >> 2) | (tile << 24);
|
|
if (tile)
|
|
addr |= (1 << 28);
|
|
RSH_CS_SET_FIELD(ctl, ADDR, addr);
|
|
RSH_CS_SET_FIELD(ctl, ACTION, 1); /* read */
|
|
RSH_CS_SET_FIELD(ctl, GO, 1); /* start */
|
|
|
|
rshim_write(RSH_MMIO_CHANNEL_RSHIM, RSH_CORESIGHT_CTL, ctl);
|
|
|
|
do {
|
|
rc = rshim_read(RSH_MMIO_CHANNEL_RSHIM,
|
|
RSH_CORESIGHT_CTL, &ctl);
|
|
if (rc < 0) {
|
|
LOG_ERROR("Failed to write rshim.\n");
|
|
return rc;
|
|
}
|
|
} while (RSH_CS_GET_FIELD(ctl, GO));
|
|
|
|
*value = RSH_CS_GET_FIELD(ctl, DATA);
|
|
return ERROR_OK;
|
|
}
|
|
|
|
static int rshim_dp_q_read(struct adiv5_dap *dap, unsigned int reg,
|
|
uint32_t *data)
|
|
{
|
|
if (!data)
|
|
return ERROR_OK;
|
|
|
|
switch (reg) {
|
|
case DP_DPIDR:
|
|
*data = dp_id_code;
|
|
break;
|
|
|
|
case DP_CTRL_STAT:
|
|
*data = CDBGPWRUPACK | CSYSPWRUPACK;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return ERROR_OK;
|
|
}
|
|
|
|
static int rshim_dp_q_write(struct adiv5_dap *dap, unsigned int reg,
|
|
uint32_t data)
|
|
{
|
|
switch (reg) {
|
|
case DP_CTRL_STAT:
|
|
dp_ctrl_stat = data;
|
|
break;
|
|
case DP_SELECT:
|
|
ap_sel = (data & DP_SELECT_APSEL) >> 24;
|
|
ap_bank = (data & DP_SELECT_APBANK) >> 4;
|
|
break;
|
|
default:
|
|
LOG_INFO("Unknown command");
|
|
break;
|
|
}
|
|
|
|
return ERROR_OK;
|
|
}
|
|
|
|
static int rshim_ap_q_read(struct adiv5_ap *ap, unsigned int reg,
|
|
uint32_t *data)
|
|
{
|
|
uint32_t addr;
|
|
int rc = ERROR_OK, tile;
|
|
|
|
if (is_adiv6(ap->dap)) {
|
|
static bool error_flagged;
|
|
if (!error_flagged)
|
|
LOG_ERROR("ADIv6 dap not supported by rshim dap-direct mode");
|
|
error_flagged = true;
|
|
return ERROR_FAIL;
|
|
}
|
|
|
|
switch (reg) {
|
|
case ADIV5_MEM_AP_REG_CSW:
|
|
*data = ap_csw;
|
|
break;
|
|
|
|
case ADIV5_MEM_AP_REG_CFG:
|
|
*data = 0;
|
|
break;
|
|
|
|
case ADIV5_MEM_AP_REG_BASE:
|
|
*data = RSH_CS_ROM_BASE;
|
|
break;
|
|
|
|
case ADIV5_AP_REG_IDR:
|
|
if (ap->ap_num == 0)
|
|
*data = APB_AP_IDR;
|
|
else
|
|
*data = 0;
|
|
break;
|
|
|
|
case ADIV5_MEM_AP_REG_BD0:
|
|
case ADIV5_MEM_AP_REG_BD1:
|
|
case ADIV5_MEM_AP_REG_BD2:
|
|
case ADIV5_MEM_AP_REG_BD3:
|
|
addr = (ap_tar & ~0xf) + (reg & 0x0C);
|
|
ap_addr_2_tile(&tile, &addr);
|
|
rc = coresight_read(tile, addr, data);
|
|
break;
|
|
|
|
case ADIV5_MEM_AP_REG_DRW:
|
|
addr = (ap_tar & ~0x3) + ap_tar_inc;
|
|
ap_addr_2_tile(&tile, &addr);
|
|
rc = coresight_read(tile, addr, data);
|
|
if (!rc && (ap_csw & CSW_ADDRINC_MASK))
|
|
ap_tar_inc += (ap_csw & 0x03) * 2;
|
|
break;
|
|
|
|
default:
|
|
LOG_INFO("Unknown command");
|
|
rc = ERROR_FAIL;
|
|
break;
|
|
}
|
|
|
|
/* Track the last error code. */
|
|
if (rc != ERROR_OK)
|
|
rshim_dap_retval = rc;
|
|
|
|
return rc;
|
|
}
|
|
|
|
static int rshim_ap_q_write(struct adiv5_ap *ap, unsigned int reg,
|
|
uint32_t data)
|
|
{
|
|
int rc = ERROR_OK, tile;
|
|
uint32_t addr;
|
|
|
|
if (is_adiv6(ap->dap)) {
|
|
static bool error_flagged;
|
|
if (!error_flagged)
|
|
LOG_ERROR("ADIv6 dap not supported by rshim dap-direct mode");
|
|
error_flagged = true;
|
|
return ERROR_FAIL;
|
|
}
|
|
|
|
if (ap_bank != 0) {
|
|
rshim_dap_retval = ERROR_FAIL;
|
|
return ERROR_FAIL;
|
|
}
|
|
|
|
switch (reg) {
|
|
case ADIV5_MEM_AP_REG_CSW:
|
|
ap_csw = data;
|
|
break;
|
|
|
|
case ADIV5_MEM_AP_REG_TAR:
|
|
ap_tar = data;
|
|
ap_tar_inc = 0;
|
|
break;
|
|
|
|
case ADIV5_MEM_AP_REG_BD0:
|
|
case ADIV5_MEM_AP_REG_BD1:
|
|
case ADIV5_MEM_AP_REG_BD2:
|
|
case ADIV5_MEM_AP_REG_BD3:
|
|
addr = (ap_tar & ~0xf) + (reg & 0x0C);
|
|
ap_addr_2_tile(&tile, &addr);
|
|
rc = coresight_write(tile, addr, data);
|
|
break;
|
|
|
|
case ADIV5_MEM_AP_REG_DRW:
|
|
ap_drw = data;
|
|
addr = (ap_tar & ~0x3) + ap_tar_inc;
|
|
ap_addr_2_tile(&tile, &addr);
|
|
rc = coresight_write(tile, addr, data);
|
|
if (!rc && (ap_csw & CSW_ADDRINC_MASK))
|
|
ap_tar_inc += (ap_csw & 0x03) * 2;
|
|
break;
|
|
|
|
default:
|
|
rc = EINVAL;
|
|
break;
|
|
}
|
|
|
|
/* Track the last error code. */
|
|
if (rc != ERROR_OK)
|
|
rshim_dap_retval = rc;
|
|
|
|
return rc;
|
|
}
|
|
|
|
static int rshim_ap_q_abort(struct adiv5_dap *dap, uint8_t *ack)
|
|
{
|
|
return ERROR_OK;
|
|
}
|
|
|
|
static int rshim_dp_run(struct adiv5_dap *dap)
|
|
{
|
|
int retval = rshim_dap_retval;
|
|
|
|
/* Clear the error code. */
|
|
rshim_dap_retval = ERROR_OK;
|
|
|
|
return retval;
|
|
}
|
|
|
|
static int rshim_connect(struct adiv5_dap *dap)
|
|
{
|
|
char *path = rshim_dev_path ? rshim_dev_path : RSHIM_DEV_PATH_DEFAULT;
|
|
|
|
rshim_fd = open(path, O_RDWR | O_SYNC);
|
|
if (rshim_fd == -1) {
|
|
LOG_ERROR("Unable to open %s\n", path);
|
|
return ERROR_FAIL;
|
|
}
|
|
|
|
/*
|
|
* Set read/write operation via the device file. Function pointers
|
|
* are used here so more ways like remote accessing via socket could
|
|
* be added later.
|
|
*/
|
|
rshim_read = rshim_dev_read;
|
|
rshim_write = rshim_dev_write;
|
|
|
|
return ERROR_OK;
|
|
}
|
|
|
|
static void rshim_disconnect(struct adiv5_dap *dap)
|
|
{
|
|
if (rshim_fd != -1) {
|
|
close(rshim_fd);
|
|
rshim_fd = -1;
|
|
}
|
|
}
|
|
|
|
COMMAND_HANDLER(rshim_dap_device_command)
|
|
{
|
|
if (CMD_ARGC != 1) {
|
|
command_print(CMD, "Too many arguments");
|
|
return ERROR_COMMAND_SYNTAX_ERROR;
|
|
}
|
|
|
|
free(rshim_dev_path);
|
|
rshim_dev_path = strdup(CMD_ARGV[0]);
|
|
return ERROR_OK;
|
|
}
|
|
|
|
static const struct command_registration rshim_dap_subcommand_handlers[] = {
|
|
{
|
|
.name = "device",
|
|
.handler = rshim_dap_device_command,
|
|
.mode = COMMAND_CONFIG,
|
|
.help = "set the rshim device",
|
|
.usage = "</dev/rshim<N>/rshim>",
|
|
},
|
|
COMMAND_REGISTRATION_DONE
|
|
};
|
|
|
|
static const struct command_registration rshim_dap_command_handlers[] = {
|
|
{
|
|
.name = "rshim",
|
|
.mode = COMMAND_ANY,
|
|
.help = "perform rshim management",
|
|
.chain = rshim_dap_subcommand_handlers,
|
|
.usage = "",
|
|
},
|
|
COMMAND_REGISTRATION_DONE
|
|
};
|
|
|
|
static int rshim_dap_init(void)
|
|
{
|
|
return ERROR_OK;
|
|
}
|
|
|
|
static int rshim_dap_quit(void)
|
|
{
|
|
return ERROR_OK;
|
|
}
|
|
|
|
static int rshim_dap_reset(int req_trst, int req_srst)
|
|
{
|
|
return ERROR_OK;
|
|
}
|
|
|
|
static int rshim_dap_speed(int speed)
|
|
{
|
|
return ERROR_OK;
|
|
}
|
|
|
|
static int rshim_dap_khz(int khz, int *jtag_speed)
|
|
{
|
|
*jtag_speed = khz;
|
|
return ERROR_OK;
|
|
}
|
|
|
|
static int rshim_dap_speed_div(int speed, int *khz)
|
|
{
|
|
*khz = speed;
|
|
return ERROR_OK;
|
|
}
|
|
|
|
/* DAP operations. */
|
|
static const struct dap_ops rshim_dap_ops = {
|
|
.connect = rshim_connect,
|
|
.queue_dp_read = rshim_dp_q_read,
|
|
.queue_dp_write = rshim_dp_q_write,
|
|
.queue_ap_read = rshim_ap_q_read,
|
|
.queue_ap_write = rshim_ap_q_write,
|
|
.queue_ap_abort = rshim_ap_q_abort,
|
|
.run = rshim_dp_run,
|
|
.quit = rshim_disconnect,
|
|
};
|
|
|
|
static const char *const rshim_dap_transport[] = { "dapdirect_swd", NULL };
|
|
|
|
struct adapter_driver rshim_dap_adapter_driver = {
|
|
.name = "rshim",
|
|
.transports = rshim_dap_transport,
|
|
.commands = rshim_dap_command_handlers,
|
|
|
|
.init = rshim_dap_init,
|
|
.quit = rshim_dap_quit,
|
|
.reset = rshim_dap_reset,
|
|
.speed = rshim_dap_speed,
|
|
.khz = rshim_dap_khz,
|
|
.speed_div = rshim_dap_speed_div,
|
|
|
|
.dap_swd_ops = &rshim_dap_ops,
|
|
};
|