jtag/drivers: add debugging support for Mellanox BlueField SoC 57/5457/7
authorLiming Sun <lsun@mellanox.com>
Fri, 10 May 2019 15:02:31 +0000 (11:02 -0400)
committerAntonio Borneo <borneo.antonio@gmail.com>
Tue, 21 Apr 2020 10:18:11 +0000 (11:18 +0100)
This commits adds debugging support for the Mellanox BlueField
SoC via rshim, which is an interface accessible from external USB
or PCIe (for SmartNIC case) via the rshim driver. It implements
the arm dap interfaces based on the existing dapdirect framework.

Change-Id: I18eb1c54293ec2c581f853e0e55b3f96d7978b56
Signed-off-by: Liming Sun <lsun@mellanox.com>
Reviewed-on: http://openocd.zylin.com/5457
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>
configure.ac
src/jtag/drivers/Makefile.am
src/jtag/drivers/rshim.c [new file with mode: 0644]
src/jtag/interfaces.c
tcl/board/bluefield.cfg [new file with mode: 0644]
tcl/interface/rshim.cfg [new file with mode: 0644]
tcl/target/bluefield.cfg [new file with mode: 0644]

index 497b15f..9c2f1d7 100644 (file)
@@ -222,6 +222,12 @@ AC_ARG_ENABLE([dummy],
   AS_HELP_STRING([--enable-dummy], [Enable building the dummy port driver]),
   [build_dummy=$enableval], [build_dummy=no])
 
+AS_CASE([$host_os], [linux*], [host_os_linux=yes], [host_os_linux=no])
+
+AC_ARG_ENABLE([rshim],
+  AS_HELP_STRING([--enable-rshim], [Enable building the rshim driver]),
+  [build_rshim=$enableval], [build_rshim=$host_os_linux])
+
 m4_define([AC_ARG_ADAPTERS], [
   m4_foreach([adapter], [$1],
        [AC_ARG_ENABLE(ADAPTER_OPT([adapter]),
@@ -334,6 +340,13 @@ AS_CASE([$host_os],
     AS_IF([test "x$build_xlnx_pcie_xvc" = "xyes"], [
       AC_MSG_ERROR([xlnx_pcie_xvc is only available on linux])
     ])
+
+    AS_CASE([$host_os], [freebsd*], [],
+    [
+      AS_IF([test "x$build_rshim" = "xyes"], [
+        AC_MSG_ERROR([build_rshim is only available on linux or freebsd])
+      ])
+    ])
 ])
 
 AC_ARG_ENABLE([minidriver_dummy],
@@ -482,6 +495,12 @@ AS_IF([test "x$build_parport" = "xyes"], [
   AC_DEFINE([BUILD_PARPORT], [0], [0 if you don't want parport.])
 ])
 
+AS_IF([test "x$build_rshim" = "xyes"], [
+  AC_DEFINE([BUILD_RSHIM], [1], [1 if you want to debug BlueField SoC via rshim.])
+], [
+  AC_DEFINE([BUILD_RSHIM], [0], [0 if you don't want to debug BlueField SoC via rshim.])
+])
+
 AS_IF([test "x$build_dummy" = "xyes"], [
   build_bitbang=yes
   AC_DEFINE([BUILD_DUMMY], [1], [1 if you want dummy driver.])
@@ -730,6 +749,7 @@ AM_CONDITIONAL([USE_LIBFTDI], [test "x$use_libftdi" = "xyes"])
 AM_CONDITIONAL([USE_HIDAPI], [test "x$use_hidapi" = "xyes"])
 AM_CONDITIONAL([USE_LIBJAYLINK], [test "x$use_libjaylink" = "xyes"])
 AM_CONDITIONAL([TARGET64], [test "x$build_target64" = "xyes"])
+AM_CONDITIONAL([RSHIM], [test "x$build_rshim" = "xyes"])
 
 AM_CONDITIONAL([MINIDRIVER], [test "x$build_minidriver" = "xyes"])
 AM_CONDITIONAL([MINIDRIVER_DUMMY], [test "x$build_minidriver_dummy" = "xyes"])
index aea09b3..ba758e7 100644 (file)
@@ -133,6 +133,9 @@ if HLADAPTER
 DRIVERFILES += %D%/stlink_usb.c
 DRIVERFILES += %D%/ti_icdi_usb.c
 endif
+if RSHIM
+DRIVERFILES += %D%/rshim.c
+endif
 if OSBDM
 DRIVERFILES += %D%/osbdm.c
 endif
diff --git a/src/jtag/drivers/rshim.c b/src/jtag/drivers/rshim.c
new file mode 100644 (file)
index 0000000..c718af5
--- /dev/null
@@ -0,0 +1,523 @@
+/*
+ * Copyright (c) 2020, Mellanox Technologies Ltd. - All Rights Reserved
+ * Liming Sun <lsun@mellanox.com>
+ *
+ * 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 2 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/>.
+ */
+
+#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;
+
+       switch (reg) {
+       case MEM_AP_REG_CSW:
+               *data = ap_csw;
+               break;
+
+       case MEM_AP_REG_CFG:
+               *data = 0;
+               break;
+
+       case MEM_AP_REG_BASE:
+               *data = RSH_CS_ROM_BASE;
+               break;
+
+       case AP_REG_IDR:
+               if (ap->ap_num == 0)
+                       *data = APB_AP_IDR;
+               else
+                       *data = 0;
+               break;
+
+       case MEM_AP_REG_BD0:
+       case MEM_AP_REG_BD1:
+       case MEM_AP_REG_BD2:
+       case MEM_AP_REG_BD3:
+               addr = (ap_tar & ~0xf) + (reg & 0x0C);
+               ap_addr_2_tile(&tile, &addr);
+               rc = coresight_read(tile, addr, data);
+               break;
+
+       case 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 (ap_bank != 0) {
+               rshim_dap_retval = ERROR_FAIL;
+               return ERROR_FAIL;
+       }
+
+       switch (reg) {
+       case MEM_AP_REG_CSW:
+               ap_csw = data;
+               break;
+
+       case MEM_AP_REG_TAR:
+               ap_tar = data;
+               ap_tar_inc = 0;
+               break;
+
+       case MEM_AP_REG_BD0:
+       case MEM_AP_REG_BD1:
+       case MEM_AP_REG_BD2:
+       case MEM_AP_REG_BD3:
+               addr = (ap_tar & ~0xf) + (reg & 0x0C);
+               ap_addr_2_tile(&tile, &addr);
+               rc = coresight_write(tile, addr, data);
+               break;
+
+       case 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. Funtion 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,
+};
index 00b3bb5..25858ea 100644 (file)
@@ -141,6 +141,9 @@ extern struct adapter_driver xds110_adapter_driver;
 #if BUILD_HLADAPTER == 1
 extern struct adapter_driver stlink_dap_adapter_driver;
 #endif
+#if BUILD_RSHIM == 1
+extern struct adapter_driver rshim_dap_adapter_driver;
+#endif
 #endif /* standard drivers */
 
 /**
@@ -252,6 +255,9 @@ struct adapter_driver *adapter_drivers[] = {
 #if BUILD_HLADAPTER == 1
                &stlink_dap_adapter_driver,
 #endif
+#if BUILD_RSHIM == 1
+               &rshim_dap_adapter_driver,
+#endif
 #endif /* standard drivers */
                NULL,
        };
diff --git a/tcl/board/bluefield.cfg b/tcl/board/bluefield.cfg
new file mode 100644 (file)
index 0000000..3058d48
--- /dev/null
@@ -0,0 +1,6 @@
+#
+# Board configuration for BlueField SoC.
+#
+
+source [find interface/rshim.cfg]
+source [find target/bluefield.cfg]
diff --git a/tcl/interface/rshim.cfg b/tcl/interface/rshim.cfg
new file mode 100644 (file)
index 0000000..accabf5
--- /dev/null
@@ -0,0 +1,6 @@
+#
+# BlueField SoC in-circuit debugger/programmer
+#
+
+adapter driver rshim
+transport select dapdirect_swd
diff --git a/tcl/target/bluefield.cfg b/tcl/target/bluefield.cfg
new file mode 100644 (file)
index 0000000..b31dfe8
--- /dev/null
@@ -0,0 +1,78 @@
+# BlueField SoC Target
+
+set _CHIPNAME bluefield
+
+# Specify the target device
+#rshim device /dev/rshim0/rshim
+
+# Main DAP
+if { [info exists DAP_TAPID] } {
+   set _DAP_TAPID $DAP_TAPID
+} else {
+   set _DAP_TAPID 0x4ba00477
+}
+
+adapter speed 1500
+
+swd newdap $_CHIPNAME cpu -expected-id $_DAP_TAPID
+dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu
+
+# Initialize the target name and command variable.
+set _TARGETNAME $_CHIPNAME.cpu
+set _smp_command ""
+
+# CTI relative address
+set $_TARGETNAME.cti(0) 0xC4020000
+set $_TARGETNAME.cti(1) 0xC4120000
+set $_TARGETNAME.cti(2) 0xC8020000
+set $_TARGETNAME.cti(3) 0xC8120000
+set $_TARGETNAME.cti(4) 0xCC020000
+set $_TARGETNAME.cti(5) 0xCC120000
+set $_TARGETNAME.cti(6) 0xD0020000
+set $_TARGETNAME.cti(7) 0xD0120000
+set $_TARGETNAME.cti(8) 0xD4020000
+set $_TARGETNAME.cti(9) 0xD4120000
+set $_TARGETNAME.cti(10) 0xD8020000
+set $_TARGETNAME.cti(11) 0xD8120000
+set $_TARGETNAME.cti(12) 0xDC020000
+set $_TARGETNAME.cti(13) 0xDC120000
+set $_TARGETNAME.cti(14) 0xE0020000
+set $_TARGETNAME.cti(15) 0xE0120000
+
+# Create debug targets for a number of cores starting from core '_core_start'.
+# Adjust the numbers according to board configuration.
+set _core_start 0
+set _cores 16
+
+# Create each core
+for { set _core $_core_start } { $_core < $_core_start + $_cores } { incr _core 1 } {
+    cti create cti$_core -dap $_CHIPNAME.dap -ctibase [set $_TARGETNAME.cti($_core)] -ap-num 0
+
+    set _command "target create ${_TARGETNAME}$_core aarch64 \
+                         -dap $_CHIPNAME.dap -coreid $_core -cti cti$_core"
+
+    if { $_core != $_core_start } {
+        set _smp_command "$_smp_command ${_TARGETNAME}$_core"
+    } else {
+        set _smp_command "target smp ${_TARGETNAME}$_core"
+    }
+
+    eval $_command
+}
+
+# Configure SMP
+if { $_cores > 1 } {
+    eval $_smp_command
+}
+
+# Make sure the default target is the boot core
+targets ${_TARGETNAME}0
+
+proc core_up { args } {
+       global _TARGETNAME
+
+       # Examine remaining cores
+       foreach _core [set args] {
+               ${_TARGETNAME}$_core arp_examine
+       }
+}