src/jtag/commands.c

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