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 static 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 struct jtag_command *jtag_command_queue_get(void)
 151 {
 152         return jtag_command_queue;
 153 }
 154
 155 /**
 156  * Copy a struct scan_field for insertion into the queue.
 157  *
 158  * This allocates a new copy of out_value using cmd_queue_alloc.
 159  */
 160 void jtag_scan_field_clone(struct scan_field *dst, const struct scan_field *src)
 161 {
 162         dst->num_bits   = src->num_bits;
 163         dst->out_value  = buf_cpy(src->out_value, cmd_queue_alloc(DIV_ROUND_UP(src->num_bits, 8)), src->num_bits);
 164         dst->in_value   = src->in_value;
 165 }
 166
 167 enum scan_type jtag_scan_type(const struct scan_command *cmd)
 168 {
 169         int i;
 170         int type = 0;
 171
 172         for (i = 0; i < cmd->num_fields; i++) {
 173                 if (cmd->fields[i].in_value)
 174                         type |= SCAN_IN;
 175                 if (cmd->fields[i].out_value)
 176                         type |= SCAN_OUT;
 177         }
 178
 179         return type;
 180 }
 181
 182 int jtag_scan_size(const struct scan_command *cmd)
 183 {
 184         int bit_count = 0;
 185         int i;
 186
 187         /* count bits in scan command */
 188         for (i = 0; i < cmd->num_fields; i++)
 189                 bit_count += cmd->fields[i].num_bits;
 190
 191         return bit_count;
 192 }
 193
 194 int jtag_build_buffer(const struct scan_command *cmd, uint8_t **buffer)
 195 {
 196         int bit_count = 0;
 197         int i;
 198
 199         bit_count = jtag_scan_size(cmd);
 200         *buffer = calloc(1, DIV_ROUND_UP(bit_count, 8));
 201
 202         bit_count = 0;
 203
 204         LOG_DEBUG_IO("%s num_fields: %i",
 205                         cmd->ir_scan ? "IRSCAN" : "DRSCAN",
 206                         cmd->num_fields);
 207
 208         for (i = 0; i < cmd->num_fields; i++) {
 209                 if (cmd->fields[i].out_value) {
 210                         if (LOG_LEVEL_IS(LOG_LVL_DEBUG_IO)) {
 211                                 char *char_buf = buf_to_hex_str(cmd->fields[i].out_value,
 212                                                 (cmd->fields[i].num_bits > DEBUG_JTAG_IOZ)
 213                                                 ? DEBUG_JTAG_IOZ
 214                                                                 : cmd->fields[i].num_bits);
 215
 216                                 LOG_DEBUG("fields[%i].out_value[%i]: 0x%s", i,
 217                                                 cmd->fields[i].num_bits, char_buf);
 218                                 free(char_buf);
 219                         }
 220                         buf_set_buf(cmd->fields[i].out_value, 0, *buffer,
 221                                         bit_count, cmd->fields[i].num_bits);
 222                 } else {
 223                         LOG_DEBUG_IO("fields[%i].out_value[%i]: NULL",
 224                                         i, cmd->fields[i].num_bits);
 225                 }
 226
 227                 bit_count += cmd->fields[i].num_bits;
 228         }
 229
 230         /*LOG_DEBUG_IO("bit_count totalling: %i",  bit_count); */
 231
 232         return bit_count;
 233 }
 234
 235 int jtag_read_buffer(uint8_t *buffer, const struct scan_command *cmd)
 236 {
 237         int i;
 238         int bit_count = 0;
 239         int retval;
 240
 241         /* we return ERROR_OK, unless a check fails, or a handler reports a problem */
 242         retval = ERROR_OK;
 243
 244         for (i = 0; i < cmd->num_fields; i++) {
 245                 /* if neither in_value nor in_handler
 246                  * are specified we don't have to examine this field
 247                  */
 248                 if (cmd->fields[i].in_value) {
 249                         int num_bits = cmd->fields[i].num_bits;
 250                         uint8_t *captured = buf_set_buf(buffer, bit_count,
 251                                         malloc(DIV_ROUND_UP(num_bits, 8)), 0, num_bits);
 252
 253                         if (LOG_LEVEL_IS(LOG_LVL_DEBUG_IO)) {
 254                                 char *char_buf = buf_to_hex_str(captured,
 255                                                 (num_bits > DEBUG_JTAG_IOZ)
 256                                                 ? DEBUG_JTAG_IOZ
 257                                                                 : num_bits);
 258
 259                                 LOG_DEBUG("fields[%i].in_value[%i]: 0x%s",
 260                                                 i, num_bits, char_buf);
 261                                 free(char_buf);
 262                         }
 263
 264                         if (cmd->fields[i].in_value)
 265                                 buf_cpy(captured, cmd->fields[i].in_value, num_bits);
 266
 267                         free(captured);
 268                 }
 269                 bit_count += cmd->fields[i].num_bits;
 270         }
 271
 272         return retval;
 273 }