git.gag.com Git - fw/openocd/blob - src/jtag/commands.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2
   3 /***************************************************************************
   4  *   Copyright (C) 2005 by Dominic Rath                                    *
   5  *   Dominic.Rath@gmx.de                                                   *
   6  *                                                                         *
   7  *   Copyright (C) 2007,2008 Øyvind Harboe                                 *
   8  *   oyvind.harboe@zylin.com                                               *
   9  *                                                                         *
  10  *   Copyright (C) 2009 SoftPLC Corporation                                *
  11  *       http://softplc.com                                                *
  12  *   dick@softplc.com                                                      *
  13  *                                                                         *
  14  *   Copyright (C) 2009 Zachary T Welch                                    *
  15  *   zw@superlucidity.net                                                  *
  16  ***************************************************************************/
  17
  18 #ifdef HAVE_CONFIG_H
  19 #include "config.h"
  20 #endif
  21
  22 #include <jtag/jtag.h>
  23 #include <transport/transport.h>
  24 #include "commands.h"
  25
  26 struct cmd_queue_page {
  27         struct cmd_queue_page *next;
  28         void *address;
  29         size_t used;
  30 };
  31
  32 #define CMD_QUEUE_PAGE_SIZE (1024 * 1024)
  33 static struct cmd_queue_page *cmd_queue_pages;
  34 static struct cmd_queue_page *cmd_queue_pages_tail;
  35
  36 struct jtag_command *jtag_command_queue;
  37 static struct jtag_command **next_command_pointer = &jtag_command_queue;
  38
  39 void jtag_queue_command(struct jtag_command *cmd)
  40 {
  41         if (!transport_is_jtag()) {
  42                 /*
  43                  * FIXME: This should not happen!
  44                  * There could be old code that queues jtag commands with non jtag interfaces so, for
  45                  * the moment simply highlight it by log an error.
  46                  * We should fix it quitting with assert(0) because it is an internal error, or returning
  47                  * an error after call to jtag_command_queue_reset() to free the jtag queue and avoid
  48                  * memory leaks.
  49                  * The fix can be applied immediately after next release (v0.11.0 ?)
  50                  */
  51                 LOG_ERROR("JTAG API jtag_queue_command() called on non JTAG interface");
  52         }
  53
  54         /* this command goes on the end, so ensure the queue terminates */
  55         cmd->next = NULL;
  56
  57         struct jtag_command **last_cmd = next_command_pointer;
  58         assert(last_cmd);
  59         assert(!*last_cmd);
  60         *last_cmd = cmd;
  61
  62         /* store location where the next command pointer will be stored */
  63         next_command_pointer = &cmd->next;
  64 }
  65
  66 void *cmd_queue_alloc(size_t size)
  67 {
  68         struct cmd_queue_page **p_page = &cmd_queue_pages;
  69         int offset;
  70         uint8_t *t;
  71
  72         /*
  73          * WARNING:
  74          *    We align/round the *SIZE* per below
  75          *    so that all pointers returned by
  76          *    this function are reasonably well
  77          *    aligned.
  78          *
  79          * If we did not, then an "odd-length" request would cause the
  80          * *next* allocation to be at an *odd* address, and because
  81          * this function has the same type of api as malloc() - we
  82          * must also return pointers that have the same type of
  83          * alignment.
  84          *
  85          * What I do not/have is a reasonable portable means
  86          * to align by...
  87          *
  88          * The solution here, is based on these suggestions.
  89          * http://gcc.gnu.org/ml/gcc-help/2008-12/msg00041.html
  90          *
  91          */
  92         union worse_case_align {
  93                 int i;
  94                 long l;
  95                 float f;
  96                 void *v;
  97         };
  98 #define ALIGN_SIZE  (sizeof(union worse_case_align))
  99
 100         /* The alignment process. */
 101         size = (size + ALIGN_SIZE - 1) & (~(ALIGN_SIZE - 1));
 102         /* Done... */
 103
 104         if (*p_page) {
 105                 p_page = &cmd_queue_pages_tail;
 106                 if (CMD_QUEUE_PAGE_SIZE - (*p_page)->used < size)
 107                         p_page = &((*p_page)->next);
 108         }
 109
 110         if (!*p_page) {
 111                 *p_page = malloc(sizeof(struct cmd_queue_page));
 112                 (*p_page)->used = 0;
 113                 size_t alloc_size = (size < CMD_QUEUE_PAGE_SIZE) ?
 114                                         CMD_QUEUE_PAGE_SIZE : size;
 115                 (*p_page)->address = malloc(alloc_size);
 116                 (*p_page)->next = NULL;
 117                 cmd_queue_pages_tail = *p_page;
 118         }
 119
 120         offset = (*p_page)->used;
 121         (*p_page)->used += size;
 122
 123         t = (*p_page)->address;
 124         return t + offset;
 125 }
 126
 127 static void cmd_queue_free(void)
 128 {
 129         struct cmd_queue_page *page = cmd_queue_pages;
 130
 131         while (page) {
 132                 struct cmd_queue_page *last = page;
 133                 free(page->address);
 134                 page = page->next;
 135                 free(last);
 136         }
 137
 138         cmd_queue_pages = NULL;
 139         cmd_queue_pages_tail = NULL;
 140 }
 141
 142 void jtag_command_queue_reset(void)
 143 {
 144         cmd_queue_free();
 145
 146         jtag_command_queue = NULL;
 147         next_command_pointer = &jtag_command_queue;
 148 }
 149
 150 /**
 151  * Copy a struct scan_field for insertion into the queue.
 152  *
 153  * This allocates a new copy of out_value using cmd_queue_alloc.
 154  */
 155 void jtag_scan_field_clone(struct scan_field *dst, const struct scan_field *src)
 156 {
 157         dst->num_bits   = src->num_bits;
 158         dst->out_value  = buf_cpy(src->out_value, cmd_queue_alloc(DIV_ROUND_UP(src->num_bits, 8)), src->num_bits);
 159         dst->in_value   = src->in_value;
 160 }
 161
 162 enum scan_type jtag_scan_type(const struct scan_command *cmd)
 163 {
 164         int i;
 165         int type = 0;
 166
 167         for (i = 0; i < cmd->num_fields; i++) {
 168                 if (cmd->fields[i].in_value)
 169                         type |= SCAN_IN;
 170                 if (cmd->fields[i].out_value)
 171                         type |= SCAN_OUT;
 172         }
 173
 174         return type;
 175 }
 176
 177 int jtag_scan_size(const struct scan_command *cmd)
 178 {
 179         int bit_count = 0;
 180         int i;
 181
 182         /* count bits in scan command */
 183         for (i = 0; i < cmd->num_fields; i++)
 184                 bit_count += cmd->fields[i].num_bits;
 185
 186         return bit_count;
 187 }
 188
 189 int jtag_build_buffer(const struct scan_command *cmd, uint8_t **buffer)
 190 {
 191         int bit_count = 0;
 192         int i;
 193
 194         bit_count = jtag_scan_size(cmd);
 195         *buffer = calloc(1, DIV_ROUND_UP(bit_count, 8));
 196
 197         bit_count = 0;
 198
 199         LOG_DEBUG_IO("%s num_fields: %i",
 200                         cmd->ir_scan ? "IRSCAN" : "DRSCAN",
 201                         cmd->num_fields);
 202
 203         for (i = 0; i < cmd->num_fields; i++) {
 204                 if (cmd->fields[i].out_value) {
 205                         if (LOG_LEVEL_IS(LOG_LVL_DEBUG_IO)) {
 206                                 char *char_buf = buf_to_hex_str(cmd->fields[i].out_value,
 207                                                 (cmd->fields[i].num_bits > DEBUG_JTAG_IOZ)
 208                                                 ? DEBUG_JTAG_IOZ
 209                                                                 : cmd->fields[i].num_bits);
 210
 211                                 LOG_DEBUG("fields[%i].out_value[%i]: 0x%s", i,
 212                                                 cmd->fields[i].num_bits, char_buf);
 213                                 free(char_buf);
 214                         }
 215                         buf_set_buf(cmd->fields[i].out_value, 0, *buffer,
 216                                         bit_count, cmd->fields[i].num_bits);
 217                 } else {
 218                         LOG_DEBUG_IO("fields[%i].out_value[%i]: NULL",
 219                                         i, cmd->fields[i].num_bits);
 220                 }
 221
 222                 bit_count += cmd->fields[i].num_bits;
 223         }
 224
 225         /*LOG_DEBUG_IO("bit_count totalling: %i",  bit_count); */
 226
 227         return bit_count;
 228 }
 229
 230 int jtag_read_buffer(uint8_t *buffer, const struct scan_command *cmd)
 231 {
 232         int i;
 233         int bit_count = 0;
 234         int retval;
 235
 236         /* we return ERROR_OK, unless a check fails, or a handler reports a problem */
 237         retval = ERROR_OK;
 238
 239         for (i = 0; i < cmd->num_fields; i++) {
 240                 /* if neither in_value nor in_handler
 241                  * are specified we don't have to examine this field
 242                  */
 243                 if (cmd->fields[i].in_value) {
 244                         int num_bits = cmd->fields[i].num_bits;
 245                         uint8_t *captured = buf_set_buf(buffer, bit_count,
 246                                         malloc(DIV_ROUND_UP(num_bits, 8)), 0, num_bits);
 247
 248                         if (LOG_LEVEL_IS(LOG_LVL_DEBUG_IO)) {
 249                                 char *char_buf = buf_to_hex_str(captured,
 250                                                 (num_bits > DEBUG_JTAG_IOZ)
 251                                                 ? DEBUG_JTAG_IOZ
 252                                                                 : num_bits);
 253
 254                                 LOG_DEBUG("fields[%i].in_value[%i]: 0x%s",
 255                                                 i, num_bits, char_buf);
 256                                 free(char_buf);
 257                         }
 258
 259                         if (cmd->fields[i].in_value)
 260                                 buf_cpy(captured, cmd->fields[i].in_value, num_bits);
 261
 262                         free(captured);
 263                 }
 264                 bit_count += cmd->fields[i].num_bits;
 265         }
 266
 267         return retval;
 268 }