From 83e0293f7ba3604b4ad98b3a8101388e3e52f9fe Mon Sep 17 00:00:00 2001 From: Richard Pasek Date: Wed, 11 Dec 2024 00:43:57 -0500 Subject: [PATCH 01/13] Add Linux SPI device SWD adapter support To alleviate the need to bitbang SWD, I've written a SWD SPI implementation. This code is inspired by the work of luppy@appkaki.com as shown at github.com/lupyuen/openocd-spi but with the desire to be more generic. This implementation makes use of the more common 4 wire SPI port using full duplex transfers to be able to capture the SWD ACK bits when a SWD TX operation is in progress. TEST: Connects successfully with the following combinations: Hosts: Raspberry Pi 4B Unnamed Qualcomm SoC with QUPv3 based SPI port Targets: Raspberry Pi 2040 Nordic nRF52840 NXP RT500 Change-Id: Ic2f38a1806085d527e6f999a3d15aea6f32d1019 Signed-off-by: Richard Pasek Reviewed-on: https://review.openocd.org/c/openocd/+/8645 Reviewed-by: Tomas Vanek Reviewed-by: Antonio Borneo Reviewed-by: zapb Tested-by: jenkins --- configure.ac | 6 + doc/openocd.texi | 67 ++++ src/jtag/drivers/Makefile.am | 3 + src/jtag/drivers/linuxspidev.c | 622 +++++++++++++++++++++++++++++++ src/jtag/interface.h | 1 + src/jtag/interfaces.c | 3 + tcl/interface/spidev_example.cfg | 9 + 7 files changed, 711 insertions(+) create mode 100644 src/jtag/drivers/linuxspidev.c create mode 100644 tcl/interface/spidev_example.cfg diff --git a/configure.ac b/configure.ac index db47dcba6..49054288f 100644 --- a/configure.ac +++ b/configure.ac @@ -163,6 +163,9 @@ m4_define([PCIE_ADAPTERS], m4_define([SERIAL_PORT_ADAPTERS], [[[buspirate], [Bus Pirate], [BUS_PIRATE]]]) +m4_define([LINUXSPIDEV_ADAPTER], + [[[linuxspidev], [Linux spidev driver], [LINUXSPIDEV]]]) + # The word 'Adapter' in "Dummy Adapter" below must begin with a capital letter # because there is an M4 macro called 'adapter'. m4_define([DUMMY_ADAPTER], @@ -289,6 +292,7 @@ AC_ARG_ADAPTERS([ LIBFTDI_ADAPTERS, LIBFTDI_USB1_ADAPTERS, LIBGPIOD_ADAPTERS, + LINUXSPIDEV_ADAPTER, SERIAL_PORT_ADAPTERS, DUMMY_ADAPTER, PCIE_ADAPTERS, @@ -726,6 +730,7 @@ PROCESS_ADAPTERS([LIBJAYLINK_ADAPTERS], ["x$use_internal_libjaylink" = "xyes" -o PROCESS_ADAPTERS([PCIE_ADAPTERS], ["x$is_linux" = "xyes"], [Linux build]) PROCESS_ADAPTERS([SERIAL_PORT_ADAPTERS], ["x$can_build_buspirate" = "xyes"], [internal error: validation should happen beforehand]) +PROCESS_ADAPTERS([LINUXSPIDEV_ADAPTER], ["x$is_linux" = "xyes"], [Linux spidev]) PROCESS_ADAPTERS([DUMMY_ADAPTER], [true], [unused]) AS_IF([test "x$enable_linuxgpiod" != "xno"], [ @@ -875,6 +880,7 @@ m4_foreach([adapter], [USB1_ADAPTERS, LIBFTDI_USB1_ADAPTERS, LIBGPIOD_ADAPTERS, LIBJAYLINK_ADAPTERS, PCIE_ADAPTERS, SERIAL_PORT_ADAPTERS, + LINUXSPIDEV_ADAPTER, DUMMY_ADAPTER, OPTIONAL_LIBRARIES, COVERAGE], diff --git a/doc/openocd.texi b/doc/openocd.texi index 7c5f84c55..47a6f69eb 100644 --- a/doc/openocd.texi +++ b/doc/openocd.texi @@ -614,6 +614,9 @@ emulation model of target hardware. @item @b{xlnx_pcie_xvc} @* A JTAG driver exposing Xilinx Virtual Cable over PCI Express to OpenOCD as JTAG/SWD interface. +@item @b{linuxspidev} +@* A SPI based SWD driver using Linux SPI devices. + @item @b{linuxgpiod} @* A bitbang JTAG driver using Linux GPIO through library libgpiod. @@ -3401,6 +3404,70 @@ See @file{interface/beaglebone-swd-native.cfg} for a sample configuration file. @end deffn +@deffn {Interface Driver} {linuxspidev} +Linux provides userspace access to SPI through spidev. Full duplex SPI +transactions are used to simultaneously read and write to/from the target to +emulate the SWD transport. + +@deffn {Config Command} {spidev path} path +Specifies the path to the spidev device. +@end deffn + +@deffn {Config Command} {spidev mode} value +Set the mode of the spi port with optional bit flags (default=3). +See /usr/include/linux/spi/spidev.h for all of the SPI mode options. +@end deffn + +@deffn {Config Command} {spidev queue_entries} value +Set the maximum number of queued transactions per spi exchange (default=64). +More queued transactions may offer greater performance when the target doesn't +need to wait. On the contrary higher numbers will reduce performance when the +target requests a wait as all queued transactions will need to be exchanged +before spidev can see the wait request. +@end deffn + +See @file{tcl/interface/spidev_example.cfg} for a sample configuration file. + +Electrical connections: +@example ++--------------+ +--------------+ +| | 1K | | +| MOSI|---/\/\/\---+ | | +| Host | | | Target | +| MISO|------------+---|SWDIO | +| | | | +| SCK|----------------|SWDCLK | +| | | | ++--------------+ +--------------+ +@end example + +The 1K resistor works well with most MCUs up to 3 MHz. A lower resistance +could be used to achieve higher speeds granted that the target SWDIO pin has +enough drive strength to pull the signal high while being pulled low by this +resistor. + +If you are having trouble here are some tips: + +@itemize @bullet + +@item @b{Make sure MISO and MOSI are tied together with a 1K resistor.} +MISO should be attached to the target. + +@item @b{Make sure that your host and target are using the same I/O voltage} +(for example both are using 3.3 volts). + +@item @b{Your host's SPI port may not idle low.} +This will lead to an additional clock edge being sent to the target, causing +the host and target being 1 clock off from each other. Try setting +SPI_MOSI_IDLE_LOW in spi_mode. Try using a different spi_mode (0 - 3). + +@item @b{Your target may pull SWDIO and/or SWDCLK high.} +This will create an extra edge when the host releases control of the SPI port +at the end of a transaction. You'll need to confirm this with a scope or meter. +Try installing 10K resistors on SWDIO and SWDCLK to ground to stop this. + +@end itemize +@end deffn @deffn {Interface Driver} {linuxgpiod} Linux provides userspace access to GPIO through libgpiod since Linux kernel diff --git a/src/jtag/drivers/Makefile.am b/src/jtag/drivers/Makefile.am index da0663fd4..ec233b247 100644 --- a/src/jtag/drivers/Makefile.am +++ b/src/jtag/drivers/Makefile.am @@ -176,6 +176,9 @@ endif if SYSFSGPIO DRIVERFILES += %D%/sysfsgpio.c endif +if LINUXSPIDEV +DRIVERFILES += %D%/linuxspidev.c +endif if XLNX_PCIE_XVC DRIVERFILES += %D%/xlnx-pcie-xvc.c endif diff --git a/src/jtag/drivers/linuxspidev.c b/src/jtag/drivers/linuxspidev.c new file mode 100644 index 000000000..737d2bef8 --- /dev/null +++ b/src/jtag/drivers/linuxspidev.c @@ -0,0 +1,622 @@ +// SPDX-License-Identifier: GPL-2.0-or-later + +/* Copyright (C) 2020 by Lup Yuen Lee + * Copyright (C) 2024 by Richard Pasek + */ + +/** + * @file + * Implementation of SWD protocol with a Linux SPI device. + */ + +/* Uncomment to log SPI exchanges (very verbose, slows things down a lot) + * + * A quick note on interpreting SPI exchange messages: + * + * This implementation works by performing SPI exchanges with MOSI and MISO + * tied together with a 1K resistor. This combined signal becomes SWDIO. + * + * Since we are performing SPI exchanges, (reading and writing at the same + * time) this means when the target isn't driving SWDIO, what is written by + * the host should also be read by the host. + * + * On SWD writes: + * The TX and RX data should match except for the ACK bits from the target + * swd write reg exchange: len=6 + * tx_buf=C5 02 40 00 02 2C + * rx_buf=C5 42 40 00 02 2C + * ^ + * | + * ACK from target + * + * On SWD reads: + * Only the command byte should match + * swd read reg exchange: len=6 + * tx_buf=B1 00 00 00 00 00 + * rx_buf=B1 40 20 00 00 F8 + * ^^ + * || + * Command packet from host + * + */ +// #define LOG_SPI_EXCHANGE + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Number of bits per SPI exchange +#define SPI_BITS 8 + +// Time in uS after the last bit of transfer before deselecting the device +#define SPI_DESELECT_DELAY 0 + +// Maximum number of SWD transactions to queue together in a SPI exchange +#define MAX_QUEUE_ENTRIES 64 + +#define CMD_BITS 8 +#define TURN_BITS 1 +#define ACK_BITS 3 +#define DATA_BITS 32 +#define PARITY_BITS 1 + +#define SWD_WR_BITS (CMD_BITS + TURN_BITS + ACK_BITS + TURN_BITS + DATA_BITS + PARITY_BITS) +#define SWD_RD_BITS (CMD_BITS + TURN_BITS + ACK_BITS + DATA_BITS + PARITY_BITS + TURN_BITS) +#define SWD_OP_BITS (MAX(SWD_WR_BITS, SWD_RD_BITS)) +#define SWD_OP_BYTES (DIV_ROUND_UP(SWD_OP_BITS, SPI_BITS)) + +#define AP_DELAY_CLOCKS 8 +#define AP_DELAY_BYTES (DIV_ROUND_UP(AP_DELAY_CLOCKS, SPI_BITS)) + +#define END_IDLE_CLOCKS 8 +#define END_IDLE_BYTES (DIV_ROUND_UP(END_IDLE_CLOCKS, SPI_BITS)) + +// File descriptor for SPI device +static int spi_fd = -1; + +// SPI Configuration +static char *spi_path; +static uint32_t spi_mode = SPI_MODE_3; // Note: SPI in LSB mode is not often supported. We'll flip LSB to MSB ourselves. +static uint32_t spi_speed; + +struct queue_info { + unsigned int buf_idx; + uint32_t *rx_ptr; +}; + +static int queue_retval; +static unsigned int max_queue_entries; +static unsigned int queue_fill; +static unsigned int queue_buf_fill; +static unsigned int queue_buf_size; +static struct queue_info *queue_infos; +static uint8_t *queue_tx_buf; +static uint8_t *queue_rx_buf; +static uint8_t *tx_flip_buf; + +static int spidev_swd_switch_seq(enum swd_special_seq seq); +static void spidev_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay_clk); + +static void spi_exchange(const uint8_t *tx_data, uint8_t *rx_data, unsigned int len) +{ +#ifdef LOG_SPI_EXCHANGE + LOG_OUTPUT("exchange: len=%u\n", len); +#endif // LOG_SPI_EXCHANGE + + if (len == 0) { + LOG_DEBUG("exchange with no length"); + return; + } + + if (!tx_data && !rx_data) { + LOG_DEBUG("exchange with no valid tx or rx pointers"); + return; + } + + if (len > queue_buf_size) { + LOG_ERROR("exchange too large len=%u ", len); + return; + } + + if (tx_data) { + // Reverse LSB to MSB + for (unsigned int i = 0; i < len; i++) + tx_flip_buf[i] = flip_u32(tx_data[i], 8); + +#ifdef LOG_SPI_EXCHANGE + if (len != 0) { + LOG_OUTPUT(" tx_buf="); + for (unsigned int i = 0; i < len; i++) + LOG_OUTPUT("%.2" PRIx8 " ", tx_flip_buf[i]); + } + LOG_OUTPUT("\n"); +#endif // LOG_SPI_EXCHANGE + } + // Transmit the MSB buffer to SPI device. + struct spi_ioc_transfer tr = { + /* The following must be cast to unsigned long to compile correctly on + * 32 and 64 bit machines (as is done in the spidev examples in Linux + * kernel code in tools/spi/). + */ + .tx_buf = (unsigned long)(tx_data ? tx_flip_buf : NULL), + .rx_buf = (unsigned long)rx_data, + .len = len, + .delay_usecs = SPI_DESELECT_DELAY, + .speed_hz = spi_speed, + .bits_per_word = SPI_BITS, + }; + int ret = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &tr); + + if (ret < 1) { + LOG_ERROR("exchange failed"); + return; + } + + if (rx_data) { +#ifdef LOG_SPI_EXCHANGE + if (len != 0) { + LOG_OUTPUT(" rx_buf="); + for (unsigned int i = 0; i < len; i++) + LOG_OUTPUT("%.2" PRIx8 " ", rx_data[i]); + } + LOG_OUTPUT("\n"); +#endif // LOG_SPI_EXCHANGE + + // Reverse MSB to LSB + for (unsigned int i = 0; i < len; i++) + rx_data[i] = flip_u32(rx_data[i], 8); + } +} + +static int spidev_speed(int speed) +{ + uint32_t tmp_speed = speed; + + int ret = ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &tmp_speed); + if (ret == -1) { + LOG_ERROR("Failed to set SPI %d speed", speed); + return ERROR_FAIL; + } + + spi_speed = speed; + + return ERROR_OK; +} + +static int spidev_speed_div(int speed, int *khz) +{ + *khz = speed / 1000; + return ERROR_OK; +} + +static int spidev_khz(int khz, int *jtag_speed) +{ + if (khz == 0) { + LOG_DEBUG("RCLK not supported"); + return ERROR_FAIL; + } + + *jtag_speed = khz * 1000; + return ERROR_OK; +} + +static void spidev_free_queue(void) +{ + max_queue_entries = 0; + queue_buf_size = 0; + + free(queue_infos); + queue_infos = NULL; + + free(queue_tx_buf); + queue_tx_buf = NULL; + + free(queue_rx_buf); + queue_rx_buf = NULL; + + free(tx_flip_buf); + tx_flip_buf = NULL; +} + +static int spidev_alloc_queue(unsigned int new_queue_entries) +{ + if (queue_fill || queue_buf_fill) { + LOG_ERROR("Can't realloc allocate queue when queue is in use"); + return ERROR_FAIL; + } + + unsigned int new_queue_buf_size = + (new_queue_entries * (SWD_OP_BYTES + AP_DELAY_BYTES)) + END_IDLE_BYTES; + + queue_infos = realloc(queue_infos, sizeof(struct queue_info) * new_queue_entries); + if (!queue_infos) + goto realloc_fail; + + queue_tx_buf = realloc(queue_tx_buf, new_queue_buf_size); + if (!queue_tx_buf) + goto realloc_fail; + + queue_rx_buf = realloc(queue_rx_buf, new_queue_buf_size); + if (!queue_rx_buf) + goto realloc_fail; + + tx_flip_buf = realloc(tx_flip_buf, new_queue_buf_size); + if (!tx_flip_buf) + goto realloc_fail; + + max_queue_entries = new_queue_entries; + queue_buf_size = new_queue_buf_size; + + LOG_DEBUG("Set queue entries to %u (buffers %u bytes)", max_queue_entries, queue_buf_size); + + return ERROR_OK; + +realloc_fail: + spidev_free_queue(); + + LOG_ERROR("Couldn't allocate queue. Out of memory."); + return ERROR_FAIL; +} + +static int spidev_init(void) +{ + LOG_INFO("SPI SWD driver"); + + if (spi_fd >= 0) + return ERROR_OK; + + if (!spi_path) { + LOG_ERROR("Path to spidev not set"); + return ERROR_JTAG_INIT_FAILED; + } + + // Open SPI device. + spi_fd = open(spi_path, O_RDWR); + if (spi_fd < 0) { + LOG_ERROR("Failed to open SPI port at %s", spi_path); + return ERROR_JTAG_INIT_FAILED; + } + + // Set SPI mode. + int ret = ioctl(spi_fd, SPI_IOC_WR_MODE32, &spi_mode); + if (ret == -1) { + LOG_ERROR("Failed to set SPI mode 0x%" PRIx32, spi_mode); + return ERROR_JTAG_INIT_FAILED; + } + + // Set SPI bits per word. + uint32_t spi_bits = SPI_BITS; + ret = ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits); + if (ret == -1) { + LOG_ERROR("Failed to set SPI %" PRIu8 " bits per transfer", spi_bits); + return ERROR_JTAG_INIT_FAILED; + } + + LOG_INFO("Opened SPI device at %s in mode 0x%" PRIx32 " with %" PRIu8 " bits ", + spi_path, spi_mode, spi_bits); + + // Set SPI read and write max speed. + int speed; + ret = adapter_get_speed(&speed); + if (ret != ERROR_OK) { + LOG_ERROR("Failed to get adapter speed"); + return ERROR_JTAG_INIT_FAILED; + } + + ret = spidev_speed(speed); + if (ret != ERROR_OK) + return ERROR_JTAG_INIT_FAILED; + + if (max_queue_entries == 0) { + ret = spidev_alloc_queue(MAX_QUEUE_ENTRIES); + if (ret != ERROR_OK) + return ERROR_JTAG_INIT_FAILED; + } + + return ERROR_OK; +} + +static int spidev_quit(void) +{ + spidev_free_queue(); + + if (spi_fd < 0) + return ERROR_OK; + + close(spi_fd); + spi_fd = -1; + return ERROR_OK; +} + +static int spidev_swd_init(void) +{ + LOG_DEBUG("spidev_swd_init"); + return ERROR_OK; +} + +static int spidev_swd_execute_queue(unsigned int end_idle_bytes) +{ + LOG_DEBUG_IO("Executing %u queued transactions", queue_fill); + + if (queue_retval != ERROR_OK) { + LOG_DEBUG_IO("Skipping due to previous errors: %d", queue_retval); + goto skip; + } + + /* A transaction must be followed by another transaction or at least 8 idle + * cycles to ensure that data is clocked through the AP. Since the tx + * buffer is zeroed after each queue run, every byte added to the buffer + * fill will add on an additional 8 idle cycles. + */ + queue_buf_fill += end_idle_bytes; + + spi_exchange(queue_tx_buf, queue_rx_buf, queue_buf_fill); + + for (unsigned int queue_idx = 0; queue_idx < queue_fill; queue_idx++) { + unsigned int buf_idx = queue_infos[queue_idx].buf_idx; + uint8_t *tx_ptr = &queue_tx_buf[buf_idx]; + uint8_t *rx_ptr = &queue_rx_buf[buf_idx]; + uint8_t cmd = buf_get_u32(tx_ptr, 0, CMD_BITS); + bool read = cmd & SWD_CMD_RNW ? true : false; + int ack = buf_get_u32(rx_ptr, CMD_BITS + TURN_BITS, ACK_BITS); + uint32_t data = read ? + buf_get_u32(rx_ptr, CMD_BITS + TURN_BITS + ACK_BITS, DATA_BITS) : + buf_get_u32(tx_ptr, CMD_BITS + TURN_BITS + ACK_BITS + TURN_BITS, DATA_BITS); + + // Devices do not reply to DP_TARGETSEL write cmd, ignore received ack + bool check_ack = swd_cmd_returns_ack(cmd); + + LOG_CUSTOM_LEVEL((check_ack && ack != SWD_ACK_OK) ? LOG_LVL_DEBUG : LOG_LVL_DEBUG_IO, + "%s%s %s %s reg %X = %08" PRIx32, + check_ack ? "" : "ack ignored ", + ack == SWD_ACK_OK ? "OK" : + ack == SWD_ACK_WAIT ? "WAIT" : + ack == SWD_ACK_FAULT ? "FAULT" : "JUNK", + cmd & SWD_CMD_APNDP ? "AP" : "DP", + read ? "read" : "write", + (cmd & SWD_CMD_A32) >> 1, + data); + + if (ack != SWD_ACK_OK && check_ack) { + queue_retval = swd_ack_to_error_code(ack); + goto skip; + + } else if (read) { + int parity = buf_get_u32(rx_ptr, CMD_BITS + TURN_BITS + ACK_BITS + DATA_BITS, PARITY_BITS); + + if (parity != parity_u32(data)) { + LOG_ERROR("SWD Read data parity mismatch"); + queue_retval = ERROR_FAIL; + goto skip; + } + + if (queue_infos[queue_idx].rx_ptr) + *queue_infos[queue_idx].rx_ptr = data; + } + } + +skip: + // Clear everything in the queue + queue_fill = 0; + queue_buf_fill = 0; + memset(queue_infos, 0, sizeof(queue_infos[0]) * max_queue_entries); + memset(queue_tx_buf, 0, queue_buf_size); + memset(queue_rx_buf, 0, queue_buf_size); + + int retval = queue_retval; + queue_retval = ERROR_OK; + + return retval; +} + +static int spidev_swd_run_queue(void) +{ + /* Since we are unsure if another SWD transaction will follow the + * transactions we are just about to execute, we need to add on 8 idle + * cycles. + */ + return spidev_swd_execute_queue(END_IDLE_BYTES); +} + +static void spidev_swd_queue_cmd(uint8_t cmd, uint32_t *dst, uint32_t data, uint32_t ap_delay_clk) +{ + unsigned int swd_op_bytes = DIV_ROUND_UP(SWD_OP_BITS + ap_delay_clk, SPI_BITS); + + if (queue_fill >= max_queue_entries || + queue_buf_fill + swd_op_bytes + END_IDLE_BYTES > queue_buf_size) { + /* Not enough room in the queue. Run the queue. No idle bytes are + * needed because we are going to execute transactions right after + * the queue is emptied. + */ + queue_retval = spidev_swd_execute_queue(0); + } + + if (queue_retval != ERROR_OK) + return; + + uint8_t *tx_ptr = &queue_tx_buf[queue_buf_fill]; + + cmd |= SWD_CMD_START | SWD_CMD_PARK; + + buf_set_u32(tx_ptr, 0, CMD_BITS, cmd); + + if (cmd & SWD_CMD_RNW) { + // Queue a read transaction + queue_infos[queue_fill].rx_ptr = dst; + } else { + // Queue a write transaction + buf_set_u32(tx_ptr, + CMD_BITS + TURN_BITS + ACK_BITS + TURN_BITS, DATA_BITS, data); + buf_set_u32(tx_ptr, + CMD_BITS + TURN_BITS + ACK_BITS + TURN_BITS + DATA_BITS, PARITY_BITS, parity_u32(data)); + } + + queue_infos[queue_fill].buf_idx = queue_buf_fill; + + /* Add idle cycles after AP accesses to avoid WAIT. Buffer is already + * zeroed so we just need to advance the pointer to add idle cycles. + */ + queue_buf_fill += swd_op_bytes; + + queue_fill++; +} + +static void spidev_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t ap_delay_clk) +{ + assert(cmd & SWD_CMD_RNW); + spidev_swd_queue_cmd(cmd, value, 0, ap_delay_clk); +} + +static void spidev_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay_clk) +{ + assert(!(cmd & SWD_CMD_RNW)); + spidev_swd_queue_cmd(cmd, NULL, value, ap_delay_clk); +} + +static int spidev_swd_switch_seq(enum swd_special_seq seq) +{ + switch (seq) { + case LINE_RESET: + LOG_DEBUG_IO("SWD line reset"); + spi_exchange(swd_seq_line_reset, NULL, swd_seq_line_reset_len / SPI_BITS); + break; + case JTAG_TO_SWD: + LOG_DEBUG("JTAG-to-SWD"); + spi_exchange(swd_seq_jtag_to_swd, NULL, swd_seq_jtag_to_swd_len / SPI_BITS); + break; + case JTAG_TO_DORMANT: + LOG_DEBUG("JTAG-to-DORMANT"); + spi_exchange(swd_seq_jtag_to_dormant, NULL, swd_seq_jtag_to_dormant_len / SPI_BITS); + break; + case SWD_TO_JTAG: + LOG_DEBUG("SWD-to-JTAG"); + spi_exchange(swd_seq_swd_to_jtag, NULL, swd_seq_swd_to_jtag_len / SPI_BITS); + break; + case SWD_TO_DORMANT: + LOG_DEBUG("SWD-to-DORMANT"); + spi_exchange(swd_seq_swd_to_dormant, NULL, swd_seq_swd_to_dormant_len / SPI_BITS); + break; + case DORMANT_TO_SWD: + LOG_DEBUG("DORMANT-to-SWD"); + spi_exchange(swd_seq_dormant_to_swd, NULL, swd_seq_dormant_to_swd_len / SPI_BITS); + break; + case DORMANT_TO_JTAG: + LOG_DEBUG("DORMANT-to-JTAG"); + spi_exchange(swd_seq_dormant_to_jtag, NULL, swd_seq_dormant_to_jtag_len / SPI_BITS); + break; + default: + LOG_ERROR("Sequence %d not supported", seq); + return ERROR_FAIL; + } + + return ERROR_OK; +} + +COMMAND_HANDLER(spidev_handle_path_command) +{ + if (CMD_ARGC != 1) + return ERROR_COMMAND_SYNTAX_ERROR; + + free(spi_path); + spi_path = strdup(CMD_ARGV[0]); + if (!spi_path) { + LOG_ERROR("Out of memory"); + return ERROR_FAIL; + } + + return ERROR_OK; +} + +COMMAND_HANDLER(spidev_handle_mode_command) +{ + if (CMD_ARGC != 1) + return ERROR_COMMAND_SYNTAX_ERROR; + + COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], spi_mode); + return ERROR_OK; +} + +COMMAND_HANDLER(spidev_handle_queue_entries_command) +{ + uint32_t new_queue_entries; + + if (CMD_ARGC != 1) + return ERROR_COMMAND_SYNTAX_ERROR; + + COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], new_queue_entries); + + return spidev_alloc_queue(new_queue_entries); +} + +const struct swd_driver spidev_swd = { + .init = spidev_swd_init, + .switch_seq = spidev_swd_switch_seq, + .read_reg = spidev_swd_read_reg, + .write_reg = spidev_swd_write_reg, + .run = spidev_swd_run_queue, +}; + +static const struct command_registration spidev_subcommand_handlers[] = { + { + .name = "path", + .handler = &spidev_handle_path_command, + .mode = COMMAND_CONFIG, + .help = "set the path to the spidev device", + .usage = "path_to_spidev", + }, + { + .name = "mode", + .handler = &spidev_handle_mode_command, + .mode = COMMAND_CONFIG, + .help = "set the mode of the spi port with optional bit flags (default=3)", + .usage = "mode", + }, + { + .name = "queue_entries", + .handler = &spidev_handle_queue_entries_command, + .mode = COMMAND_CONFIG, + .help = "set the queue entry size (default=64)", + .usage = "queue_entries", + }, + COMMAND_REGISTRATION_DONE +}; + +static const struct command_registration spidev_command_handlers[] = { + { + .name = "spidev", + .mode = COMMAND_ANY, + .help = "perform spidev management", + .chain = spidev_subcommand_handlers, + .usage = "", + }, + COMMAND_REGISTRATION_DONE +}; + +// Only SWD transport supported +static const char *const spidev_transports[] = { "swd", NULL }; + +struct adapter_driver linuxspidev_adapter_driver = { + .name = "linuxspidev", + .transports = spidev_transports, + .commands = spidev_command_handlers, + + .init = spidev_init, + .quit = spidev_quit, + .speed = spidev_speed, + .khz = spidev_khz, + .speed_div = spidev_speed_div, + + .swd_ops = &spidev_swd, +}; diff --git a/src/jtag/interface.h b/src/jtag/interface.h index b448851dc..f69326492 100644 --- a/src/jtag/interface.h +++ b/src/jtag/interface.h @@ -386,6 +386,7 @@ extern struct adapter_driver jtag_dpi_adapter_driver; extern struct adapter_driver jtag_vpi_adapter_driver; extern struct adapter_driver kitprog_adapter_driver; extern struct adapter_driver linuxgpiod_adapter_driver; +extern struct adapter_driver linuxspidev_adapter_driver; extern struct adapter_driver opendous_adapter_driver; extern struct adapter_driver openjtag_adapter_driver; extern struct adapter_driver osbdm_adapter_driver; diff --git a/src/jtag/interfaces.c b/src/jtag/interfaces.c index 67f0838e3..e49bd9e0f 100644 --- a/src/jtag/interfaces.c +++ b/src/jtag/interfaces.c @@ -123,6 +123,9 @@ struct adapter_driver *adapter_drivers[] = { #if BUILD_LINUXGPIOD == 1 &linuxgpiod_adapter_driver, #endif +#if BUILD_LINUXSPIDEV == 1 + &linuxspidev_adapter_driver, +#endif #if BUILD_XLNX_PCIE_XVC == 1 &xlnx_pcie_xvc_adapter_driver, #endif diff --git a/tcl/interface/spidev_example.cfg b/tcl/interface/spidev_example.cfg new file mode 100644 index 000000000..2354c781b --- /dev/null +++ b/tcl/interface/spidev_example.cfg @@ -0,0 +1,9 @@ +# SPDX-License-Identifier: GPL-2.0-or-later + +# Example config for using Linux spidev as a SWD adapter. + +adapter driver linuxspidev +adapter speed 3000 +spidev path "/dev/spidev0.0" +spidev mode 3 +spidev queue_entries 64 From 41f7d18161592cf3e2e6f51f0ef36d345aeed2e0 Mon Sep 17 00:00:00 2001 From: Antonio Borneo Date: Mon, 30 Dec 2024 12:10:28 +0100 Subject: [PATCH 02/13] target: armv7m: add support of per register data_type Extend the struct armv7m_regs to include the optional pointer to a struct reg_data_type. Update armv7m_build_reg_cache() to check for the new optional field and to use it. Change-Id: I57c7f9abefd614308be8aa8419d687477b44679d Signed-off-by: Antonio Borneo Reviewed-on: https://review.openocd.org/c/openocd/+/8680 Tested-by: jenkins --- src/target/armv7m.c | 129 +++++++++++++++++++++++--------------------- 1 file changed, 67 insertions(+), 62 deletions(-) diff --git a/src/target/armv7m.c b/src/target/armv7m.c index 49a8a4bba..b4473c30a 100644 --- a/src/target/armv7m.c +++ b/src/target/armv7m.c @@ -80,30 +80,31 @@ static const struct { enum reg_type type; const char *group; const char *feature; + struct reg_data_type *data_type; } armv7m_regs[] = { - { ARMV7M_R0, "r0", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_R1, "r1", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_R2, "r2", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_R3, "r3", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_R4, "r4", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_R5, "r5", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_R6, "r6", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_R7, "r7", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_R8, "r8", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_R9, "r9", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_R10, "r10", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_R11, "r11", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_R12, "r12", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_R13, "sp", 32, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_R14, "lr", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_PC, "pc", 32, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.arm.m-profile" }, - { ARMV7M_XPSR, "xpsr", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile" }, + { ARMV7M_R0, "r0", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_R1, "r1", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_R2, "r2", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_R3, "r3", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_R4, "r4", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_R5, "r5", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_R6, "r6", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_R7, "r7", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_R8, "r8", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_R9, "r9", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_R10, "r10", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_R11, "r11", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_R12, "r12", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_R13, "sp", 32, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_R14, "lr", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_PC, "pc", 32, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.arm.m-profile", NULL, }, + { ARMV7M_XPSR, "xpsr", 32, REG_TYPE_INT, "general", "org.gnu.gdb.arm.m-profile", NULL, }, - { ARMV7M_MSP, "msp", 32, REG_TYPE_DATA_PTR, "system", "org.gnu.gdb.arm.m-system" }, - { ARMV7M_PSP, "psp", 32, REG_TYPE_DATA_PTR, "system", "org.gnu.gdb.arm.m-system" }, + { ARMV7M_MSP, "msp", 32, REG_TYPE_DATA_PTR, "system", "org.gnu.gdb.arm.m-system", NULL, }, + { ARMV7M_PSP, "psp", 32, REG_TYPE_DATA_PTR, "system", "org.gnu.gdb.arm.m-system", NULL, }, /* A working register for packing/unpacking special regs, hidden from gdb */ - { ARMV7M_PMSK_BPRI_FLTMSK_CTRL, "pmsk_bpri_fltmsk_ctrl", 32, REG_TYPE_INT, NULL, NULL }, + { ARMV7M_PMSK_BPRI_FLTMSK_CTRL, "pmsk_bpri_fltmsk_ctrl", 32, REG_TYPE_INT, NULL, NULL, NULL }, /* WARNING: If you use armv7m_write_core_reg() on one of 4 following * special registers, the new data go to ARMV7M_PMSK_BPRI_FLTMSK_CTRL @@ -111,52 +112,52 @@ static const struct { * To trigger write to CPU HW register, add * armv7m_write_core_reg(,,ARMV7M_PMSK_BPRI_FLTMSK_CTRL,); */ - { ARMV7M_PRIMASK, "primask", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" }, - { ARMV7M_BASEPRI, "basepri", 8, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" }, - { ARMV7M_FAULTMASK, "faultmask", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" }, - { ARMV7M_CONTROL, "control", 3, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system" }, + { ARMV7M_PRIMASK, "primask", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system", NULL, }, + { ARMV7M_BASEPRI, "basepri", 8, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system", NULL, }, + { ARMV7M_FAULTMASK, "faultmask", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system", NULL, }, + { ARMV7M_CONTROL, "control", 3, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.m-system", NULL, }, /* ARMv8-M security extension (TrustZone) specific registers */ - { ARMV8M_MSP_NS, "msp_ns", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext" }, - { ARMV8M_PSP_NS, "psp_ns", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext" }, - { ARMV8M_MSP_S, "msp_s", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext" }, - { ARMV8M_PSP_S, "psp_s", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext" }, - { ARMV8M_MSPLIM_S, "msplim_s", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext" }, - { ARMV8M_PSPLIM_S, "psplim_s", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext" }, - { ARMV8M_MSPLIM_NS, "msplim_ns", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext" }, - { ARMV8M_PSPLIM_NS, "psplim_ns", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext" }, + { ARMV8M_MSP_NS, "msp_ns", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext", NULL, }, + { ARMV8M_PSP_NS, "psp_ns", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext", NULL, }, + { ARMV8M_MSP_S, "msp_s", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext", NULL, }, + { ARMV8M_PSP_S, "psp_s", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext", NULL, }, + { ARMV8M_MSPLIM_S, "msplim_s", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext", NULL, }, + { ARMV8M_PSPLIM_S, "psplim_s", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext", NULL, }, + { ARMV8M_MSPLIM_NS, "msplim_ns", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext", NULL, }, + { ARMV8M_PSPLIM_NS, "psplim_ns", 32, REG_TYPE_DATA_PTR, "stack", "org.gnu.gdb.arm.secext", NULL, }, - { ARMV8M_PMSK_BPRI_FLTMSK_CTRL_S, "pmsk_bpri_fltmsk_ctrl_s", 32, REG_TYPE_INT, NULL, NULL }, - { ARMV8M_PRIMASK_S, "primask_s", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext" }, - { ARMV8M_BASEPRI_S, "basepri_s", 8, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext" }, - { ARMV8M_FAULTMASK_S, "faultmask_s", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext" }, - { ARMV8M_CONTROL_S, "control_s", 3, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext" }, + { ARMV8M_PMSK_BPRI_FLTMSK_CTRL_S, "pmsk_bpri_fltmsk_ctrl_s", 32, REG_TYPE_INT, NULL, NULL, NULL, }, + { ARMV8M_PRIMASK_S, "primask_s", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext", NULL, }, + { ARMV8M_BASEPRI_S, "basepri_s", 8, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext", NULL, }, + { ARMV8M_FAULTMASK_S, "faultmask_s", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext", NULL, }, + { ARMV8M_CONTROL_S, "control_s", 3, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext", NULL, }, - { ARMV8M_PMSK_BPRI_FLTMSK_CTRL_NS, "pmsk_bpri_fltmsk_ctrl_ns", 32, REG_TYPE_INT, NULL, NULL }, - { ARMV8M_PRIMASK_NS, "primask_ns", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext" }, - { ARMV8M_BASEPRI_NS, "basepri_ns", 8, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext" }, - { ARMV8M_FAULTMASK_NS, "faultmask_ns", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext" }, - { ARMV8M_CONTROL_NS, "control_ns", 3, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext" }, + { ARMV8M_PMSK_BPRI_FLTMSK_CTRL_NS, "pmsk_bpri_fltmsk_ctrl_ns", 32, REG_TYPE_INT, NULL, NULL, NULL, }, + { ARMV8M_PRIMASK_NS, "primask_ns", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext", NULL, }, + { ARMV8M_BASEPRI_NS, "basepri_ns", 8, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext", NULL, }, + { ARMV8M_FAULTMASK_NS, "faultmask_ns", 1, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext", NULL, }, + { ARMV8M_CONTROL_NS, "control_ns", 3, REG_TYPE_INT8, "system", "org.gnu.gdb.arm.secext", NULL, }, /* FPU registers */ - { ARMV7M_D0, "d0", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D1, "d1", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D2, "d2", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D3, "d3", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D4, "d4", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D5, "d5", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D6, "d6", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D7, "d7", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D8, "d8", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D9, "d9", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D10, "d10", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D11, "d11", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D12, "d12", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D13, "d13", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D14, "d14", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, - { ARMV7M_D15, "d15", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_D0, "d0", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D1, "d1", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D2, "d2", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D3, "d3", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D4, "d4", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D5, "d5", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D6, "d6", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D7, "d7", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D8, "d8", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D9, "d9", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D10, "d10", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D11, "d11", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D12, "d12", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D13, "d13", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D14, "d14", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, + { ARMV7M_D15, "d15", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, - { ARMV7M_FPSCR, "fpscr", 32, REG_TYPE_INT, "float", "org.gnu.gdb.arm.vfp" }, + { ARMV7M_FPSCR, "fpscr", 32, REG_TYPE_INT, "float", "org.gnu.gdb.arm.vfp", NULL, }, }; #define ARMV7M_NUM_REGS ARRAY_SIZE(armv7m_regs) @@ -811,10 +812,14 @@ struct reg_cache *armv7m_build_reg_cache(struct target *target) LOG_TARGET_ERROR(target, "unable to allocate feature list"); reg_list[i].reg_data_type = calloc(1, sizeof(struct reg_data_type)); - if (reg_list[i].reg_data_type) - reg_list[i].reg_data_type->type = armv7m_regs[i].type; - else + if (reg_list[i].reg_data_type) { + if (armv7m_regs[i].data_type) + *reg_list[i].reg_data_type = *armv7m_regs[i].data_type; + else + reg_list[i].reg_data_type->type = armv7m_regs[i].type; + } else { LOG_TARGET_ERROR(target, "unable to allocate reg type list"); + } } arm->cpsr = reg_list + ARMV7M_XPSR; From 8e89a8af63f242290c800d6f640b432d3af9ec89 Mon Sep 17 00:00:00 2001 From: Antonio Borneo Date: Mon, 30 Dec 2024 12:14:24 +0100 Subject: [PATCH 03/13] target: cortex_m: add support of ARMv8.1-M register 'vpr' The register 'vpr' is present when MVFR1.MVE is not zero. For the moment, reuse the existing flag 'fp_feature'. To be reviewed for the case of MVE supported without floating point. The documentation of GDB [1] reports that the register 'vpr' should be represented as 3 fields. Tested on Cortex-M55 based STM32N6570. Change-Id: I8737a24d01a13eeb09a0f2075b96be400f9f91c6 Signed-off-by: Antonio Borneo Link: [1] https://sourceware.org/gdb/download/onlinedocs/gdb.html/ARM-Features.html#M_002dprofile-Vector-Extension-_0028MVE_0029 Reviewed-on: https://review.openocd.org/c/openocd/+/8681 Tested-by: jenkins --- src/target/armv7m.c | 27 +++++++++++++++++++++++++++ src/target/armv7m.h | 6 +++++- src/target/cortex_m.c | 4 ++++ 3 files changed, 36 insertions(+), 1 deletion(-) diff --git a/src/target/armv7m.c b/src/target/armv7m.c index b4473c30a..440ca49d1 100644 --- a/src/target/armv7m.c +++ b/src/target/armv7m.c @@ -65,6 +65,28 @@ const int armv7m_msp_reg_map[ARMV7M_NUM_CORE_REGS] = { ARMV7M_XPSR, }; +static struct reg_data_type_bitfield armv8m_vpr_bits[] = { + { 0, 15, REG_TYPE_UINT }, + { 16, 19, REG_TYPE_UINT }, + { 20, 23, REG_TYPE_UINT }, +}; + +static struct reg_data_type_flags_field armv8m_vpr_fields[] = { + { "P0", armv8m_vpr_bits + 0, armv8m_vpr_fields + 1, }, + { "MASK01", armv8m_vpr_bits + 1, armv8m_vpr_fields + 2, }, + { "MASK23", armv8m_vpr_bits + 2, NULL }, +}; + +static struct reg_data_type_flags armv8m_vpr_flags[] = { + { 4, armv8m_vpr_fields }, +}; + +static struct reg_data_type armv8m_flags_vpr[] = { + { REG_TYPE_ARCH_DEFINED, "vpr_reg", REG_TYPE_CLASS_FLAGS, + { .reg_type_flags = armv8m_vpr_flags }, + }, +}; + /* * These registers are not memory-mapped. The ARMv7-M profile includes * memory mapped registers too, such as for the NVIC (interrupt controller) @@ -158,6 +180,8 @@ static const struct { { ARMV7M_D15, "d15", 64, REG_TYPE_IEEE_DOUBLE, "float", "org.gnu.gdb.arm.vfp", NULL, }, { ARMV7M_FPSCR, "fpscr", 32, REG_TYPE_INT, "float", "org.gnu.gdb.arm.vfp", NULL, }, + + { ARMV8M_VPR, "vpr", 32, REG_TYPE_INT, "float", "org.gnu.gdb.arm.m-profile-mve", armv8m_flags_vpr, }, }; #define ARMV7M_NUM_REGS ARRAY_SIZE(armv7m_regs) @@ -273,6 +297,9 @@ uint32_t armv7m_map_id_to_regsel(unsigned int arm_reg_id) case ARMV7M_FPSCR: return ARMV7M_REGSEL_FPSCR; + case ARMV8M_VPR: + return ARMV8M_REGSEL_VPR; + case ARMV7M_D0 ... ARMV7M_D15: return ARMV7M_REGSEL_S0 + 2 * (arm_reg_id - ARMV7M_D0); diff --git a/src/target/armv7m.h b/src/target/armv7m.h index 2878dd1c7..86c45f7f2 100644 --- a/src/target/armv7m.h +++ b/src/target/armv7m.h @@ -62,6 +62,7 @@ enum { ARMV7M_REGSEL_PMSK_BPRI_FLTMSK_CTRL = 0x14, ARMV8M_REGSEL_PMSK_BPRI_FLTMSK_CTRL_S = 0x22, ARMV8M_REGSEL_PMSK_BPRI_FLTMSK_CTRL_NS = 0x23, + ARMV8M_REGSEL_VPR = 0x24, ARMV7M_REGSEL_FPSCR = 0x21, /* 32bit Floating-point registers */ @@ -196,12 +197,15 @@ enum { /* Floating-point status register */ ARMV7M_FPSCR, + /* Vector Predication Status and Control Register */ + ARMV8M_VPR, + /* for convenience add registers' block delimiters */ ARMV7M_LAST_REG, ARMV7M_CORE_FIRST_REG = ARMV7M_R0, ARMV7M_CORE_LAST_REG = ARMV7M_XPSR, ARMV7M_FPU_FIRST_REG = ARMV7M_D0, - ARMV7M_FPU_LAST_REG = ARMV7M_FPSCR, + ARMV7M_FPU_LAST_REG = ARMV8M_VPR, ARMV8M_FIRST_REG = ARMV8M_MSP_NS, ARMV8M_LAST_REG = ARMV8M_CONTROL_NS, }; diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c index fa95fcbc7..2cea203a2 100644 --- a/src/target/cortex_m.c +++ b/src/target/cortex_m.c @@ -2708,6 +2708,10 @@ int cortex_m_examine(struct target *target) for (size_t idx = ARMV7M_FPU_FIRST_REG; idx <= ARMV7M_FPU_LAST_REG; idx++) armv7m->arm.core_cache->reg_list[idx].exist = false; + /* TODO: MVE can be present without floating points. Revisit this test */ + if (armv7m->fp_feature != FPV5_MVE_F && armv7m->fp_feature != FPV5_MVE_I) + armv7m->arm.core_cache->reg_list[ARMV8M_VPR].exist = false; + if (!cortex_m_has_tz(target)) for (size_t idx = ARMV8M_FIRST_REG; idx <= ARMV8M_LAST_REG; idx++) armv7m->arm.core_cache->reg_list[idx].exist = false; From 3099547069896ccff054d64bac6041fe1e20add9 Mon Sep 17 00:00:00 2001 From: Antonio Borneo Date: Mon, 16 Sep 2024 13:45:19 +0200 Subject: [PATCH 04/13] OpenOCD: fix code indentation Fix checkpatch errors ERROR:SUSPECT_CODE_INDENT: suspect code indent for conditional statements Change-Id: I94d4fa5720c25dd2fb0334a824cd9026babcce4e Signed-off-by: Antonio Borneo Reviewed-on: https://review.openocd.org/c/openocd/+/8497 Tested-by: jenkins --- src/flash/nor/fespi.c | 6 +++--- src/flash/nor/kinetis_ke.c | 2 +- src/flash/nor/niietcm4.c | 6 +++--- src/flash/nor/psoc4.c | 10 +++++----- src/helper/log.c | 8 ++++---- src/rtos/hwthread.c | 8 +++----- src/rtos/linux.c | 5 +++-- src/target/aarch64.c | 6 +++--- src/target/arc.c | 18 ++++++++---------- src/target/armv4_5.c | 22 +++++++++++----------- src/target/armv7a_mmu.c | 9 +++------ src/target/armv8.c | 4 ++-- src/target/cortex_a.c | 29 +++++++++++++---------------- src/target/mips32.c | 2 +- src/target/mips_m4k.c | 2 +- src/target/stm8.c | 10 ++++------ src/xsvf/xsvf.c | 8 ++++---- 17 files changed, 72 insertions(+), 83 deletions(-) diff --git a/src/flash/nor/fespi.c b/src/flash/nor/fespi.c index 6c4e8a928..395722cb5 100644 --- a/src/flash/nor/fespi.c +++ b/src/flash/nor/fespi.c @@ -751,9 +751,9 @@ static int fespi_probe(struct flash_bank *bank) target_device->name, bank->base); } else { - LOG_DEBUG("Assuming FESPI as specified at address " TARGET_ADDR_FMT - " with ctrl at " TARGET_ADDR_FMT, fespi_info->ctrl_base, - bank->base); + LOG_DEBUG("Assuming FESPI as specified at address " TARGET_ADDR_FMT + " with ctrl at " TARGET_ADDR_FMT, fespi_info->ctrl_base, + bank->base); } /* read and decode flash ID; returns in SW mode */ diff --git a/src/flash/nor/kinetis_ke.c b/src/flash/nor/kinetis_ke.c index e4dffa6d5..8207504b5 100644 --- a/src/flash/nor/kinetis_ke.c +++ b/src/flash/nor/kinetis_ke.c @@ -1005,7 +1005,7 @@ static int kinetis_ke_write(struct flash_bank *bank, const uint8_t *buffer, result = kinetis_ke_stop_watchdog(bank->target); if (result != ERROR_OK) - return result; + return result; result = kinetis_ke_prepare_flash(bank); if (result != ERROR_OK) diff --git a/src/flash/nor/niietcm4.c b/src/flash/nor/niietcm4.c index 0c36e2c96..aaf072655 100644 --- a/src/flash/nor/niietcm4.c +++ b/src/flash/nor/niietcm4.c @@ -311,7 +311,7 @@ static int niietcm4_uflash_page_erase(struct flash_bank *bank, int page_num, int /* status check */ retval = niietcm4_uopstatus_check(bank); if (retval != ERROR_OK) - return retval; + return retval; return retval; } @@ -394,7 +394,7 @@ COMMAND_HANDLER(niietcm4_handle_uflash_read_byte_command) uint32_t uflash_data; if (strcmp("info", CMD_ARGV[0]) == 0) - uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB; + uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB; else if (strcmp("main", CMD_ARGV[0]) == 0) uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ; else @@ -539,7 +539,7 @@ COMMAND_HANDLER(niietcm4_handle_uflash_erase_command) int mem_type; if (strcmp("info", CMD_ARGV[0]) == 0) - mem_type = 1; + mem_type = 1; else if (strcmp("main", CMD_ARGV[0]) == 0) mem_type = 0; else diff --git a/src/flash/nor/psoc4.c b/src/flash/nor/psoc4.c index 1064fa93d..72cf0ee05 100644 --- a/src/flash/nor/psoc4.c +++ b/src/flash/nor/psoc4.c @@ -384,15 +384,15 @@ static int psoc4_get_silicon_id(struct flash_bank *bank, uint32_t *silicon_id, u * bit 7..0 family ID (lowest 8 bits) */ if (silicon_id) - *silicon_id = ((part0 & 0x0000ffff) << 16) - | ((part0 & 0x00ff0000) >> 8) - | (part1 & 0x000000ff); + *silicon_id = ((part0 & 0x0000ffff) << 16) + | ((part0 & 0x00ff0000) >> 8) + | (part1 & 0x000000ff); if (family_id) - *family_id = part1 & 0x0fff; + *family_id = part1 & 0x0fff; if (protection) - *protection = (part1 >> 12) & 0x0f; + *protection = (part1 >> 12) & 0x0f; return ERROR_OK; } diff --git a/src/helper/log.c b/src/helper/log.c index 9ad00ce62..e02556b6d 100644 --- a/src/helper/log.c +++ b/src/helper/log.c @@ -272,10 +272,10 @@ void log_init(void) if (debug_env) { int value; int retval = parse_int(debug_env, &value); - if (retval == ERROR_OK && - debug_level >= LOG_LVL_SILENT && - debug_level <= LOG_LVL_DEBUG_IO) - debug_level = value; + if (retval == ERROR_OK + && debug_level >= LOG_LVL_SILENT + && debug_level <= LOG_LVL_DEBUG_IO) + debug_level = value; } if (!log_output) diff --git a/src/rtos/hwthread.c b/src/rtos/hwthread.c index 1890a3d87..c9f1a1792 100644 --- a/src/rtos/hwthread.c +++ b/src/rtos/hwthread.c @@ -154,9 +154,8 @@ static int hwthread_update_threads(struct rtos *rtos) if (curr->debug_reason == DBG_REASON_SINGLESTEP) { current_reason = curr->debug_reason; current_thread = tid; - } else - /* multiple breakpoints, prefer gdbs' threadid */ - if (curr->debug_reason == DBG_REASON_BREAKPOINT) { + } else if (curr->debug_reason == DBG_REASON_BREAKPOINT) { + /* multiple breakpoints, prefer gdbs' threadid */ if (tid == rtos->current_threadid) current_thread = tid; } @@ -176,8 +175,7 @@ static int hwthread_update_threads(struct rtos *rtos) curr->debug_reason == DBG_REASON_BREAKPOINT) { current_reason = curr->debug_reason; current_thread = tid; - } else - if (curr->debug_reason == DBG_REASON_DBGRQ) { + } else if (curr->debug_reason == DBG_REASON_DBGRQ) { if (tid == rtos->current_threadid) current_thread = tid; } diff --git a/src/rtos/linux.c b/src/rtos/linux.c index 7517ec7a9..5467988f3 100644 --- a/src/rtos/linux.c +++ b/src/rtos/linux.c @@ -624,7 +624,7 @@ static struct threads *liste_add_task(struct threads *task_list, struct threads { t->next = NULL; - if (!*last) + if (!*last) { if (!task_list) { task_list = t; return task_list; @@ -637,7 +637,8 @@ static struct threads *liste_add_task(struct threads *task_list, struct threads temp->next = t; *last = t; return task_list; - } else { + } + } else { (*last)->next = t; *last = t; return task_list; diff --git a/src/target/aarch64.c b/src/target/aarch64.c index 9f122070a..3cc813005 100644 --- a/src/target/aarch64.c +++ b/src/target/aarch64.c @@ -2908,9 +2908,9 @@ static int aarch64_jim_configure(struct target *target, struct jim_getopt_info * pc = (struct aarch64_private_config *)target->private_config; if (!pc) { - pc = calloc(1, sizeof(struct aarch64_private_config)); - pc->adiv5_config.ap_num = DP_APSEL_INVALID; - target->private_config = pc; + pc = calloc(1, sizeof(struct aarch64_private_config)); + pc->adiv5_config.ap_num = DP_APSEL_INVALID; + target->private_config = pc; } /* diff --git a/src/target/arc.c b/src/target/arc.c index 5c08c5664..0c111d553 100644 --- a/src/target/arc.c +++ b/src/target/arc.c @@ -388,7 +388,7 @@ static int arc_build_reg_cache(struct target *target) } list_for_each_entry(reg_desc, &arc->aux_reg_descriptions, list) { - CHECK_RETVAL(arc_init_reg(target, ®_list[i], reg_desc, i)); + CHECK_RETVAL(arc_init_reg(target, ®_list[i], reg_desc, i)); LOG_TARGET_DEBUG(target, "reg n=%3li name=%3s group=%s feature=%s", i, reg_list[i].name, reg_list[i].group, @@ -464,7 +464,7 @@ static int arc_build_bcr_reg_cache(struct target *target) } list_for_each_entry(reg_desc, &arc->bcr_reg_descriptions, list) { - CHECK_RETVAL(arc_init_reg(target, ®_list[i], reg_desc, gdb_regnum)); + CHECK_RETVAL(arc_init_reg(target, ®_list[i], reg_desc, gdb_regnum)); /* BCRs always semantically, they are just read-as-zero, if there is * not real register. */ reg_list[i].exist = true; @@ -719,14 +719,14 @@ static int arc_configure(struct target *target) LOG_TARGET_DEBUG(target, "Configuring ARC ICCM and DCCM"); /* Configuring DCCM if DCCM_BUILD and AUX_DCCM are known registers. */ - if (arc_reg_get_by_name(target->reg_cache, "dccm_build", true) && - arc_reg_get_by_name(target->reg_cache, "aux_dccm", true)) - CHECK_RETVAL(arc_configure_dccm(target)); + if (arc_reg_get_by_name(target->reg_cache, "dccm_build", true) + && arc_reg_get_by_name(target->reg_cache, "aux_dccm", true)) + CHECK_RETVAL(arc_configure_dccm(target)); /* Configuring ICCM if ICCM_BUILD and AUX_ICCM are known registers. */ - if (arc_reg_get_by_name(target->reg_cache, "iccm_build", true) && - arc_reg_get_by_name(target->reg_cache, "aux_iccm", true)) - CHECK_RETVAL(arc_configure_iccm(target)); + if (arc_reg_get_by_name(target->reg_cache, "iccm_build", true) + && arc_reg_get_by_name(target->reg_cache, "aux_iccm", true)) + CHECK_RETVAL(arc_configure_iccm(target)); return ERROR_OK; } @@ -1067,9 +1067,7 @@ static int arc_poll(struct target *target) LOG_TARGET_DEBUG(target, "Discrepancy of STATUS32[0] HALT bit and ARC_JTAG_STAT_RU, " "target is still running"); } - } else if (target->state == TARGET_DEBUG_RUNNING) { - target->state = TARGET_HALTED; LOG_TARGET_DEBUG(target, "ARC core is in debug running mode"); diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c index ceec3619b..a258c7fd4 100644 --- a/src/target/armv4_5.c +++ b/src/target/armv4_5.c @@ -1301,11 +1301,11 @@ int arm_get_gdb_reg_list(struct target *target, *reg_list = malloc(sizeof(struct reg *) * (*reg_list_size)); for (i = 0; i < 16; i++) - (*reg_list)[i] = arm_reg_current(arm, i); + (*reg_list)[i] = arm_reg_current(arm, i); /* For GDB compatibility, take FPA registers size into account and zero-fill it*/ for (i = 16; i < 24; i++) - (*reg_list)[i] = &arm_gdb_dummy_fp_reg; + (*reg_list)[i] = &arm_gdb_dummy_fp_reg; (*reg_list)[24] = &arm_gdb_dummy_fps_reg; (*reg_list)[25] = arm->cpsr; @@ -1330,25 +1330,25 @@ int arm_get_gdb_reg_list(struct target *target, *reg_list = malloc(sizeof(struct reg *) * (*reg_list_size)); for (i = 0; i < 16; i++) - (*reg_list)[i] = arm_reg_current(arm, i); + (*reg_list)[i] = arm_reg_current(arm, i); for (i = 13; i < ARRAY_SIZE(arm_core_regs); i++) { - int reg_index = arm->core_cache->reg_list[i].number; + int reg_index = arm->core_cache->reg_list[i].number; - if (arm_core_regs[i].mode == ARM_MODE_MON + if (arm_core_regs[i].mode == ARM_MODE_MON && arm->core_type != ARM_CORE_TYPE_SEC_EXT && arm->core_type != ARM_CORE_TYPE_VIRT_EXT) - continue; - if (arm_core_regs[i].mode == ARM_MODE_HYP + continue; + if (arm_core_regs[i].mode == ARM_MODE_HYP && arm->core_type != ARM_CORE_TYPE_VIRT_EXT) - continue; - (*reg_list)[reg_index] = &(arm->core_cache->reg_list[i]); + continue; + (*reg_list)[reg_index] = &arm->core_cache->reg_list[i]; } /* When we supply the target description, there is no need for fake FPA */ for (i = 16; i < 24; i++) { - (*reg_list)[i] = &arm_gdb_dummy_fp_reg; - (*reg_list)[i]->size = 0; + (*reg_list)[i] = &arm_gdb_dummy_fp_reg; + (*reg_list)[i]->size = 0; } (*reg_list)[24] = &arm_gdb_dummy_fps_reg; (*reg_list)[24]->size = 0; diff --git a/src/target/armv7a_mmu.c b/src/target/armv7a_mmu.c index c4d294eae..43b5dae8e 100644 --- a/src/target/armv7a_mmu.c +++ b/src/target/armv7a_mmu.c @@ -260,8 +260,7 @@ COMMAND_HANDLER(armv7a_mmu_dump_table) /* skip empty entries in the first level table */ if ((first_lvl_descriptor & 3) == 0) { pt_idx++; - } else - if ((first_lvl_descriptor & 0x40002) == 2) { + } else if ((first_lvl_descriptor & 0x40002) == 2) { /* section descriptor */ uint32_t va_range = 1024*1024-1; /* 1MB range */ uint32_t va_start = pt_idx << 20; @@ -273,8 +272,7 @@ COMMAND_HANDLER(armv7a_mmu_dump_table) LOG_USER("SECT: VA[%8.8"PRIx32" -- %8.8"PRIx32"]: PA[%8.8"PRIx32" -- %8.8"PRIx32"] %s", va_start, va_end, pa_start, pa_end, l1_desc_bits_to_string(first_lvl_descriptor, afe)); pt_idx++; - } else - if ((first_lvl_descriptor & 0x40002) == 0x40002) { + } else if ((first_lvl_descriptor & 0x40002) == 0x40002) { /* supersection descriptor */ uint32_t va_range = 16*1024*1024-1; /* 16MB range */ uint32_t va_start = pt_idx << 20; @@ -310,8 +308,7 @@ COMMAND_HANDLER(armv7a_mmu_dump_table) if ((second_lvl_descriptor & 3) == 0) { /* skip entry */ pt2_idx++; - } else - if ((second_lvl_descriptor & 3) == 1) { + } else if ((second_lvl_descriptor & 3) == 1) { /* large page */ uint32_t va_range = 64*1024-1; /* 64KB range */ uint32_t va_start = (pt_idx << 20) + (pt2_idx << 12); diff --git a/src/target/armv8.c b/src/target/armv8.c index 88534d962..50a9f4688 100644 --- a/src/target/armv8.c +++ b/src/target/armv8.c @@ -1966,7 +1966,7 @@ int armv8_get_gdb_reg_list(struct target *target, *reg_list = malloc(sizeof(struct reg *) * (*reg_list_size)); for (i = 0; i < *reg_list_size; i++) - (*reg_list)[i] = armv8_reg_current(arm, i); + (*reg_list)[i] = armv8_reg_current(arm, i); return ERROR_OK; case REG_CLASS_ALL: @@ -1974,7 +1974,7 @@ int armv8_get_gdb_reg_list(struct target *target, *reg_list = malloc(sizeof(struct reg *) * (*reg_list_size)); for (i = 0; i < *reg_list_size; i++) - (*reg_list)[i] = armv8_reg_current(arm, i); + (*reg_list)[i] = armv8_reg_current(arm, i); return ERROR_OK; diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c index 086aafe19..bfe698051 100644 --- a/src/target/cortex_a.c +++ b/src/target/cortex_a.c @@ -1324,21 +1324,21 @@ static int cortex_a_set_breakpoint(struct target *target, brp_list[brp_i].value); } else if (breakpoint->type == BKPT_SOFT) { uint8_t code[4]; - /* length == 2: Thumb breakpoint */ - if (breakpoint->length == 2) + if (breakpoint->length == 2) { + /* length == 2: Thumb breakpoint */ buf_set_u32(code, 0, 32, ARMV5_T_BKPT(0x11)); - else - /* length == 3: Thumb-2 breakpoint, actual encoding is - * a regular Thumb BKPT instruction but we replace a - * 32bit Thumb-2 instruction, so fix-up the breakpoint - * length - */ - if (breakpoint->length == 3) { + } else if (breakpoint->length == 3) { + /* length == 3: Thumb-2 breakpoint, actual encoding is + * a regular Thumb BKPT instruction but we replace a + * 32bit Thumb-2 instruction, so fix-up the breakpoint + * length + */ buf_set_u32(code, 0, 32, ARMV5_T_BKPT(0x11)); breakpoint->length = 4; - } else + } else { /* length == 4, normal ARM breakpoint */ buf_set_u32(code, 0, 32, ARMV5_BKPT(0x11)); + } retval = target_read_memory(target, breakpoint->address & 0xFFFFFFFE, @@ -1348,8 +1348,7 @@ static int cortex_a_set_breakpoint(struct target *target, return retval; /* make sure data cache is cleaned & invalidated down to PoC */ - armv7a_cache_flush_virt(target, breakpoint->address, - breakpoint->length); + armv7a_cache_flush_virt(target, breakpoint->address, breakpoint->length); retval = target_write_memory(target, breakpoint->address & 0xFFFFFFFE, @@ -1358,10 +1357,8 @@ static int cortex_a_set_breakpoint(struct target *target, return retval; /* update i-cache at breakpoint location */ - armv7a_l1_d_cache_inval_virt(target, breakpoint->address, - breakpoint->length); - armv7a_l1_i_cache_inval_virt(target, breakpoint->address, - breakpoint->length); + armv7a_l1_d_cache_inval_virt(target, breakpoint->address, breakpoint->length); + armv7a_l1_i_cache_inval_virt(target, breakpoint->address, breakpoint->length); breakpoint->is_set = true; } diff --git a/src/target/mips32.c b/src/target/mips32.c index dd40558a1..fcb7042cb 100644 --- a/src/target/mips32.c +++ b/src/target/mips32.c @@ -2357,7 +2357,7 @@ COMMAND_HANDLER(mips32_handle_scan_delay_command) if (CMD_ARGC == 1) COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], ejtag_info->scan_delay); else if (CMD_ARGC > 1) - return ERROR_COMMAND_SYNTAX_ERROR; + return ERROR_COMMAND_SYNTAX_ERROR; command_print(CMD, "scan delay: %d nsec", ejtag_info->scan_delay); if (ejtag_info->scan_delay >= MIPS32_SCAN_DELAY_LEGACY_MODE) { diff --git a/src/target/mips_m4k.c b/src/target/mips_m4k.c index ad9808961..1543de355 100644 --- a/src/target/mips_m4k.c +++ b/src/target/mips_m4k.c @@ -1397,7 +1397,7 @@ COMMAND_HANDLER(mips_m4k_handle_scan_delay_command) if (CMD_ARGC == 1) COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], ejtag_info->scan_delay); else if (CMD_ARGC > 1) - return ERROR_COMMAND_SYNTAX_ERROR; + return ERROR_COMMAND_SYNTAX_ERROR; command_print(CMD, "scan delay: %d nsec", ejtag_info->scan_delay); if (ejtag_info->scan_delay >= MIPS32_SCAN_DELAY_LEGACY_MODE) { diff --git a/src/target/stm8.c b/src/target/stm8.c index 2b3466dac..fb5c81f06 100644 --- a/src/target/stm8.c +++ b/src/target/stm8.c @@ -689,15 +689,13 @@ static int stm8_write_flash(struct target *target, enum mem_type type, if (stm8->flash_ncr2) stm8_write_u8(target, stm8->flash_ncr2, ~(PRG + opt)); blocksize = blocksize_param; - } else - if ((bytecnt >= 4) && ((address & 0x3) == 0)) { + } else if ((bytecnt >= 4) && ((address & 0x3) == 0)) { if (stm8->flash_cr2) stm8_write_u8(target, stm8->flash_cr2, WPRG + opt); if (stm8->flash_ncr2) stm8_write_u8(target, stm8->flash_ncr2, ~(WPRG + opt)); blocksize = 4; - } else - if (blocksize != 1) { + } else if (blocksize != 1) { if (stm8->flash_cr2) stm8_write_u8(target, stm8->flash_cr2, opt); if (stm8->flash_ncr2) @@ -1552,8 +1550,8 @@ static int stm8_set_watchpoint(struct target *target, } if (watchpoint->length != 1) { - LOG_ERROR("Only watchpoints of length 1 are supported"); - return ERROR_TARGET_UNALIGNED_ACCESS; + LOG_ERROR("Only watchpoints of length 1 are supported"); + return ERROR_TARGET_UNALIGNED_ACCESS; } enum hw_break_type enable = 0; diff --git a/src/xsvf/xsvf.c b/src/xsvf/xsvf.c index 74a4dcfde..88275043e 100644 --- a/src/xsvf/xsvf.c +++ b/src/xsvf/xsvf.c @@ -749,10 +749,10 @@ COMMAND_HANDLER(handle_xsvf_command) int delay; if (read(xsvf_fd, &wait_local, 1) < 0 - || read(xsvf_fd, &end, 1) < 0 - || read(xsvf_fd, delay_buf, 4) < 0) { - do_abort = 1; - break; + || read(xsvf_fd, &end, 1) < 0 + || read(xsvf_fd, delay_buf, 4) < 0) { + do_abort = 1; + break; } wait_state = xsvf_to_tap(wait_local); From 77f9da76264d4970faf22a40a31fc66fa7543b57 Mon Sep 17 00:00:00 2001 From: Tomas Vanek Date: Fri, 17 Jan 2025 18:02:50 +0100 Subject: [PATCH 05/13] flash/nor/kinetis: fix assertion during flash write If the device has at lest one FlexNVM bank and it is set as EE backup only, the bank has no protection blocks. kinetis_fill_fcf() collects protection data from all banks before flash write of the sector containing FCF block. In case it encountered a FlexNVM bank with no protection blocks assert failed. Failed flash write of previously erased FCF block could cause engaging debugging lock (if the device was run or reset). Skip banks with zero protection blocks. Replace assert() by LOG_ERROR() as we have to finish FCF write. Change-Id: Ibe7e7ec6d0db4453b8a53c8256987621b809c99d Signed-off-by: Tomas Vanek Suggested-by: Jasper v. Blanckenburg Fixes: https://sourceforge.net/p/openocd/tickets/448/ Reviewed-on: https://review.openocd.org/c/openocd/+/8719 Tested-by: jenkins Reviewed-by: Jasper v. Blanckenburg Reviewed-by: Antonio Borneo --- src/flash/nor/kinetis.c | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/src/flash/nor/kinetis.c b/src/flash/nor/kinetis.c index 2f88b9c1f..85c306bd4 100644 --- a/src/flash/nor/kinetis.c +++ b/src/flash/nor/kinetis.c @@ -1489,7 +1489,22 @@ static int kinetis_fill_fcf(struct flash_bank *bank, uint8_t *fcf) kinetis_auto_probe(bank_iter); - assert(bank_iter->prot_blocks); + if (bank_iter->num_prot_blocks == 0) { + if (k_bank->flash_class == FC_PFLASH) { + LOG_ERROR("BUG: PFLASH bank %u has no protection blocks", + bank_idx); + } else { + LOG_DEBUG("skipping FLEX_NVM bank %u with no prot blocks (EE bkp only)", + bank_idx); + } + continue; + } + + if (!bank_iter->prot_blocks) { + LOG_ERROR("BUG: bank %u has NULL protection blocks array", + bank_idx); + continue; + } if (k_bank->flash_class == FC_PFLASH) { for (unsigned int i = 0; i < bank_iter->num_prot_blocks; i++) { From 0b97973bfbb980f987ec8e0e8d8ad32c85e9e5b0 Mon Sep 17 00:00:00 2001 From: Marek Vrbka Date: Mon, 13 Jan 2025 10:28:15 +0100 Subject: [PATCH 06/13] vdebug: Fix socket comparison warning on Windows On GCC version 13.2, the previous code emitted the following warning on Windows: openocd/src/jtag/drivers/vdebug.c:254:19: warning: comparison of integer expressions of different signedness: 'int' and 'long long unsigned int' [-Wsign-compare] 254 | if (hsock == INVALID_SOCKET) This patch fixes it and brings it in line with other socket handling code. Change-Id: I7e05f83c6905cfaf66b68e8988c783e80cee4a48 Signed-off-by: Marek Vrbka Reviewed-on: https://review.openocd.org/c/openocd/+/8717 Tested-by: jenkins Reviewed-by: Jan Matyas Reviewed-by: Jacek Wuwer Reviewed-by: Antonio Borneo Reviewed-by: R. Diez --- src/jtag/drivers/vdebug.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/jtag/drivers/vdebug.c b/src/jtag/drivers/vdebug.c index 691e576e5..20819f70b 100644 --- a/src/jtag/drivers/vdebug.c +++ b/src/jtag/drivers/vdebug.c @@ -252,7 +252,7 @@ static int vdebug_socket_open(char *server_addr, uint32_t port) #ifdef _WIN32 hsock = socket(AF_INET, SOCK_STREAM, IPPROTO_IP); - if (hsock == INVALID_SOCKET) + if (hsock < 0) rc = vdebug_socket_error(); #elif defined __CYGWIN__ /* SO_RCVLOWAT unsupported on CYGWIN */ From 778d2dc4bb982ffa00f5fbd343346555fcbf9113 Mon Sep 17 00:00:00 2001 From: Evgeniy Naydanov Date: Fri, 17 Jan 2025 18:03:40 +0300 Subject: [PATCH 07/13] helper/options: drop redundant argument checks In case the option is passed with a single `:` in `optstring` argument, the call to `getopt_long()` should return `?`. Therefore the check on `optarg` is redundand in case of `l` and `c`. Change-Id: I1ac176fdae449a34db0a0496b69a9ea65ccd6aec Signed-off-by: Evgeniy Naydanov Reported-by: Antonio Borneo Reviewed-on: https://review.openocd.org/c/openocd/+/8718 Tested-by: jenkins Reviewed-by: Antonio Borneo --- src/helper/options.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/helper/options.c b/src/helper/options.c index 61a101469..9e332cc8c 100644 --- a/src/helper/options.c +++ b/src/helper/options.c @@ -303,12 +303,10 @@ int parse_cmdline_args(struct command_context *cmd_ctx, int argc, char *argv[]) break; } case 'l': /* --log_output | -l */ - if (optarg) - command_run_linef(cmd_ctx, "log_output %s", optarg); + command_run_linef(cmd_ctx, "log_output %s", optarg); break; case 'c': /* --command | -c */ - if (optarg) - add_config_command(optarg); + add_config_command(optarg); break; default: /* '?' */ /* getopt will emit an error message, all we have to do is bail. */ From 345473f3ceaf6a1241f62354e31eeababfc56588 Mon Sep 17 00:00:00 2001 From: Evgeniy Naydanov Date: Tue, 17 Dec 2024 18:13:00 +0300 Subject: [PATCH 08/13] helper/options: handle errors in `-l` Before the patch an error in opening the log file (e.g. can't write a file) was ignored when specified via `-l`. E.g.: ``` > touch log > chmod -w log > openocd -l log -c shutdown ... failed to open output log "log" shutdown command invoked > echo $? 0 ``` After the patch: ``` ... > openocd -l log -c shutdown ... failed to open output log "log" > echo $? 1 ``` Change-Id: Ibab45f580dc46a499bf967c4afad071f9c2972a2 Signed-off-by: Evgeniy Naydanov Reviewed-on: https://review.openocd.org/c/openocd/+/8666 Tested-by: jenkins Reviewed-by: Antonio Borneo --- src/helper/options.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/helper/options.c b/src/helper/options.c index 9e332cc8c..50977a610 100644 --- a/src/helper/options.c +++ b/src/helper/options.c @@ -303,8 +303,12 @@ int parse_cmdline_args(struct command_context *cmd_ctx, int argc, char *argv[]) break; } case 'l': /* --log_output | -l */ - command_run_linef(cmd_ctx, "log_output %s", optarg); + { + int retval = command_run_linef(cmd_ctx, "log_output %s", optarg); + if (retval != ERROR_OK) + return retval; break; + } case 'c': /* --command | -c */ add_config_command(optarg); break; From ac18b8cd6a3f21b97f942efdbb0ec2f362bb132b Mon Sep 17 00:00:00 2001 From: Antonio Borneo Date: Sun, 17 Nov 2024 21:35:45 +0100 Subject: [PATCH 09/13] configure: make more robust the check for elf 64 The check if 'elf.h' defines the type 'Elf64_Ehdr' is currently done through 'grep' on the file. While there is no false positive, so far, such test could incorrectly find the text inside a comment or in a block guarded by #if/#endif. Use the autoconf macro AC_CHECK_TYPE() to detect if the type is properly declared. Change-Id: Ibb74db3d90ac6d1589b9dc1e5a7ae59e47945e78 Signed-off-by: Antonio Borneo Reviewed-on: https://review.openocd.org/c/openocd/+/8591 Tested-by: jenkins --- configure.ac | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/configure.ac b/configure.ac index 49054288f..fc1e20ef8 100644 --- a/configure.ac +++ b/configure.ac @@ -52,9 +52,11 @@ AC_SEARCH_LIBS([openpty], [util]) AC_CHECK_HEADERS([sys/socket.h]) AC_CHECK_HEADERS([elf.h]) -AC_EGREP_HEADER(Elf64_Ehdr, [elf.h], [ - AC_DEFINE([HAVE_ELF64], [1], [Define to 1 if the system has the type `Elf64_Ehdr'.]) -]) + +AC_CHECK_TYPE([Elf64_Ehdr], + AC_DEFINE([HAVE_ELF64], [1], [Define to 1 if the system has the type 'Elf64_Ehdr'.]), + [], [[#include ]]) + AC_CHECK_HEADERS([fcntl.h]) AC_CHECK_HEADERS([malloc.h]) AC_CHECK_HEADERS([netdb.h]) From 8038e2f7548792a090bdd255ec6bfbf4128fead7 Mon Sep 17 00:00:00 2001 From: Antonio Borneo Date: Sun, 17 Nov 2024 22:46:15 +0100 Subject: [PATCH 10/13] configure: allow --enable-malloc-logging only with glibc The feature for 'malloc-logging' uses functionalities that are available only in GNU libc. Detect in 'configure' if OpenOCD is being compiled with glibc. Set the macro '_DEBUG_FREE_SPACE_' only in case of glibc. Change-Id: I43e9b87c7ad47171cfe3e7c1e5f96f11e19f98d0 Signed-off-by: Antonio Borneo Reviewed-on: https://review.openocd.org/c/openocd/+/8592 Tested-by: jenkins --- configure.ac | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/configure.ac b/configure.ac index fc1e20ef8..a558650b3 100644 --- a/configure.ac +++ b/configure.ac @@ -57,6 +57,11 @@ AC_CHECK_TYPE([Elf64_Ehdr], AC_DEFINE([HAVE_ELF64], [1], [Define to 1 if the system has the type 'Elf64_Ehdr'.]), [], [[#include ]]) +AC_MSG_CHECKING([for glibc]) +AC_COMPILE_IFELSE([AC_LANG_PROGRAM([[#include ]], [[int v = __GLIBC__;return 0;]])], + [have_glibc=yes], [have_glibc=no]) +AC_MSG_RESULT($have_glibc) + AC_CHECK_HEADERS([fcntl.h]) AC_CHECK_HEADERS([malloc.h]) AC_CHECK_HEADERS([netdb.h]) @@ -262,7 +267,7 @@ AC_ARG_ENABLE([malloc_logging], AC_MSG_CHECKING([whether to enable malloc free space logging]); AC_MSG_RESULT([$debug_malloc]) -AS_IF([test "x$debug_malloc" = "xyes"], [ +AS_IF([test "x$debug_malloc" = "xyes" -a "x$have_glibc" = "xyes"], [ AC_DEFINE([_DEBUG_FREE_SPACE_],[1], [Include malloc free space in logging]) ]) From fceccde0b394cff9127dca13708ef689094c9975 Mon Sep 17 00:00:00 2001 From: Antonio Borneo Date: Mon, 19 Feb 2024 20:11:07 -0600 Subject: [PATCH 11/13] helper/log: Fix build using _DEBUG_FREE_SPACE_ The glibc API 'mallinfo' is deprecated and the new 'mallinfo2' should be used from glibc 2.33 (2021-02-01). Throw an error when '--enable-malloc-logging' is used on systems that compile without glibc. Detect the glibc version and, for backward compatibility, define 'mallinfo2' as the old 'mallinfo'. Define a macro for the format of 'fordblks'. Change-Id: I68bff7b1b58f0ec2669db0b911f19c1c5a26ed30 Reported-by: Steven J. Hill Signed-off-by: Antonio Borneo Reviewed-on: https://review.openocd.org/c/openocd/+/8589 Tested-by: jenkins --- src/helper/log.c | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/helper/log.c b/src/helper/log.c index e02556b6d..8f7ab0039 100644 --- a/src/helper/log.c +++ b/src/helper/log.c @@ -30,6 +30,18 @@ #else #error "malloc.h is required to use --enable-malloc-logging" #endif + +#ifdef __GLIBC__ +#if __GLIBC_PREREQ(2, 33) +#define FORDBLKS_FORMAT " %zu" +#else +/* glibc older than 2.33 (2021-02-01) use mallinfo(). Overwrite it */ +#define mallinfo2 mallinfo +#define FORDBLKS_FORMAT " %d" +#endif +#else +#error "GNU glibc is required to use --enable-malloc-logging" +#endif #endif int debug_level = LOG_LVL_INFO; @@ -105,12 +117,11 @@ static void log_puts(enum log_levels level, /* print with count and time information */ int64_t t = timeval_ms() - start; #ifdef _DEBUG_FREE_SPACE_ - struct mallinfo info; - info = mallinfo(); + struct mallinfo2 info = mallinfo2(); #endif fprintf(log_output, "%s%d %" PRId64 " %s:%d %s()" #ifdef _DEBUG_FREE_SPACE_ - " %d" + FORDBLKS_FORMAT #endif ": %s", log_strings[level + 1], count, t, file, line, function, #ifdef _DEBUG_FREE_SPACE_ From 77c904fd13c08077cafd0845107506db408b5bb1 Mon Sep 17 00:00:00 2001 From: Marc Schink Date: Tue, 2 Jul 2024 17:12:46 +0200 Subject: [PATCH 12/13] Deprecate jimtcl Git submodule jimtcl was integrated as Git submodule for convenience and probably also because packages were not widely available at the time. Today, jimtcl is available in many popular package repositories [1] and the integration as Git submodule adds unnecessary complexity to the OpenOCD build process. For details, see the discussion on the mailing list in [2]. Disable the jimtcl Git submodule by default and announce it as deprecated feature that will be removed in the next release. This gives package maintainers time to adapt to the change and, if necessary, build a package for jimtcl. [1] https://repology.org/project/jimtcl/versions [2] https://sourceforge.net/p/openocd/mailman/message/58786630/ Change-Id: I07930ac07f7d7a6317c08b21dc118f4f128b331c Signed-off-by: Marc Schink Reviewed-on: https://review.openocd.org/c/openocd/+/8380 Tested-by: jenkins Reviewed-by: Paul Fertser Reviewed-by: Antonio Borneo --- configure.ac | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/configure.ac b/configure.ac index a558650b3..4b10aabd5 100644 --- a/configure.ac +++ b/configure.ac @@ -399,8 +399,8 @@ AS_CASE([$host_os], ]) AC_ARG_ENABLE([internal-jimtcl], - AS_HELP_STRING([--disable-internal-jimtcl], [Disable building internal jimtcl]), - [use_internal_jimtcl=$enableval], [use_internal_jimtcl=yes]) + AS_HELP_STRING([--enable-internal-jimtcl], [Enable building internal jimtcl (deprecated)]), + [use_internal_jimtcl=$enableval], [use_internal_jimtcl=no]) AC_ARG_ENABLE([jimtcl-maintainer], AS_HELP_STRING([--enable-jimtcl-maintainer], [Enable maintainer mode when building internal jimtcl]), @@ -878,6 +878,10 @@ AS_IF([test "x$enable_jlink" != "xno"], [ ]]) ) +AS_IF([test "x$use_internal_jimtcl" = "xyes"], [ + AC_MSG_WARN([Using the internal jimtcl is deprecated and will not be possible in the future.]) +]) + echo echo echo OpenOCD configuration summary From a510d51a78f14fbb8416037a587ce1bfc6016d24 Mon Sep 17 00:00:00 2001 From: Marc Schink Date: Wed, 26 Jun 2024 15:50:45 +0200 Subject: [PATCH 13/13] bootstrap: Do not set up Git submodules by default Building OpenOCD with jimtcl and libjaylink Git submodules is deprecated and will be removed in the upcoming releases. The remaining 'git2cl' submodule is only required during the OpenOCD release process. Only set up Git submodules when the 'with-submodules' argument is used, for example during the OpenOCD release process or for the transition period until all submodules are replaced by external dependencies. We keep the existing 'nosubmodule' argument in order to not break automatic testing with Jenkins. Change-Id: Ia4fd765e3a2d6b2c40b084a1ffdf919d5f4f35bb Signed-off-by: Marc Schink Reviewed-on: https://review.openocd.org/c/openocd/+/8381 Tested-by: jenkins Reviewed-by: Antonio Borneo Reviewed-by: R. Diez --- bootstrap | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/bootstrap b/bootstrap index 9dfdc41ac..0ee26d4ed 100755 --- a/bootstrap +++ b/bootstrap @@ -15,19 +15,21 @@ else exit 1 fi -SKIP_SUBMODULE=0 +WITH_SUBMODULES=0 case "$#" in 0) ;; - 1) if [ "$1" = "nosubmodule" ]; then - SKIP_SUBMODULE=1 - else + 1) if [ "$1" = "with-submodules" ]; then + WITH_SUBMODULES=1 + elif [ "$1" = "nosubmodule" ]; then + WITH_SUBMODULES=0 + elif [ -n "$1" ]; then echo "$0: Illegal argument $1" >&2 - echo "USAGE: $0 [nosubmodule]" >&2 + echo "USAGE: $0 [with-submodules]" >&2 exit 1 fi;; *) echo "$0: Wrong number of command-line arguments." >&2 - echo "USAGE: $0 [nosubmodule]" >&2 + echo "USAGE: $0 [with-submodules]" >&2 exit 1;; esac @@ -42,12 +44,12 @@ autoheader --warnings=all automake --warnings=all --gnu --add-missing --copy ) -if [ "$SKIP_SUBMODULE" -ne 0 ]; then - echo "Skipping submodule setup" -else +if [ "$WITH_SUBMODULES" -ne 0 ]; then echo "Setting up submodules" git submodule sync git submodule update --init +else + echo "Skipping submodule setup" fi if [ -x src/jtag/drivers/libjaylink/autogen.sh ]; then