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 "commands.h"
  35
  36 struct cmd_queue_page {
  37         struct cmd_queue_page *next;
  38         void *address;
  39         size_t used;
  40 };
  41
  42 #define CMD_QUEUE_PAGE_SIZE (1024 * 1024)
  43 static struct cmd_queue_page *cmd_queue_pages;
  44 static struct cmd_queue_page *cmd_queue_pages_tail;
  45
  46 struct jtag_command *jtag_command_queue;
  47 static struct jtag_command **next_command_pointer = &jtag_command_queue;
  48
  49 void jtag_queue_command(struct jtag_command *cmd)
  50 {
  51         /* this command goes on the end, so ensure the queue terminates */
  52         cmd->next = NULL;
  53
  54         struct jtag_command **last_cmd = next_command_pointer;
  55         assert(NULL != last_cmd);
  56         assert(NULL == *last_cmd);
  57         *last_cmd = cmd;
  58
  59         /* store location where the next command pointer will be stored */
  60         next_command_pointer = &cmd->next;
  61 }
  62
  63 void *cmd_queue_alloc(size_t size)
  64 {
  65         struct cmd_queue_page **p_page = &cmd_queue_pages;
  66         int offset;
  67         uint8_t *t;
  68
  69         /*
  70          * WARNING:
  71          *    We align/round the *SIZE* per below
  72          *    so that all pointers returned by
  73          *    this function are reasonably well
  74          *    aligned.
  75          *
  76          * If we did not, then an "odd-length" request would cause the
  77          * *next* allocation to be at an *odd* address, and because
  78          * this function has the same type of api as malloc() - we
  79          * must also return pointers that have the same type of
  80          * alignment.
  81          *
  82          * What I do not/have is a reasonable portable means
  83          * to align by...
  84          *
  85          * The solution here, is based on these suggestions.
  86          * http://gcc.gnu.org/ml/gcc-help/2008-12/msg00041.html
  87          *
  88          */
  89         union worse_case_align {
  90                 int i;
  91                 long l;
  92                 float f;
  93                 void *v;
  94         };
  95 #define ALIGN_SIZE  (sizeof(union worse_case_align))
  96
  97         /* The alignment process. */
  98         size = (size + ALIGN_SIZE - 1) & (~(ALIGN_SIZE - 1));
  99         /* Done... */
 100
 101         if (*p_page) {
 102                 p_page = &cmd_queue_pages_tail;
 103                 if (CMD_QUEUE_PAGE_SIZE - (*p_page)->used < size)
 104                         p_page = &((*p_page)->next);
 105         }
 106
 107         if (!*p_page) {
 108                 *p_page = malloc(sizeof(struct cmd_queue_page));
 109                 (*p_page)->used = 0;
 110                 size_t alloc_size = (size < CMD_QUEUE_PAGE_SIZE) ?
 111                                         CMD_QUEUE_PAGE_SIZE : size;
 112                 (*p_page)->address = malloc(alloc_size);
 113                 (*p_page)->next = NULL;
 114                 cmd_queue_pages_tail = *p_page;
 115         }
 116
 117         offset = (*p_page)->used;
 118         (*p_page)->used += size;
 119
 120         t = (*p_page)->address;
 121         return t + offset;
 122 }
 123
 124 static void cmd_queue_free(void)
 125 {
 126         struct cmd_queue_page *page = cmd_queue_pages;
 127
 128         while (page) {
 129                 struct cmd_queue_page *last = page;
 130                 free(page->address);
 131                 page = page->next;
 132                 free(last);
 133         }
 134
 135         cmd_queue_pages = NULL;
 136         cmd_queue_pages_tail = NULL;
 137 }
 138
 139 void jtag_command_queue_reset(void)
 140 {
 141         cmd_queue_free();
 142
 143         jtag_command_queue = NULL;
 144         next_command_pointer = &jtag_command_queue;
 145 }
 146
 147 /**
 148  * Copy a struct scan_field for insertion into the queue.
 149  *
 150  * This allocates a new copy of out_value using cmd_queue_alloc.
 151  */
 152 void jtag_scan_field_clone(struct scan_field *dst, const struct scan_field *src)
 153 {
 154         dst->num_bits   = src->num_bits;
 155         dst->out_value  = buf_cpy(src->out_value, cmd_queue_alloc(DIV_ROUND_UP(src->num_bits, 8)), src->num_bits);
 156         dst->in_value   = src->in_value;
 157 }
 158
 159 enum scan_type jtag_scan_type(const struct scan_command *cmd)
 160 {
 161         int i;
 162         int type = 0;
 163
 164         for (i = 0; i < cmd->num_fields; i++) {
 165                 if (cmd->fields[i].in_value)
 166                         type |= SCAN_IN;
 167                 if (cmd->fields[i].out_value)
 168                         type |= SCAN_OUT;
 169         }
 170
 171         return type;
 172 }
 173
 174 int jtag_scan_size(const struct scan_command *cmd)
 175 {
 176         int bit_count = 0;
 177         int i;
 178
 179         /* count bits in scan command */
 180         for (i = 0; i < cmd->num_fields; i++)
 181                 bit_count += cmd->fields[i].num_bits;
 182
 183         return bit_count;
 184 }
 185
 186 int jtag_build_buffer(const struct scan_command *cmd, uint8_t **buffer)
 187 {
 188         int bit_count = 0;
 189         int i;
 190
 191         bit_count = jtag_scan_size(cmd);
 192         *buffer = calloc(1, DIV_ROUND_UP(bit_count, 8));
 193
 194         bit_count = 0;
 195
 196         DEBUG_JTAG_IO("%s num_fields: %i",
 197                         cmd->ir_scan ? "IRSCAN" : "DRSCAN",
 198                         cmd->num_fields);
 199
 200         for (i = 0; i < cmd->num_fields; i++) {
 201                 if (cmd->fields[i].out_value) {
 202 #ifdef _DEBUG_JTAG_IO_
 203                         char *char_buf = buf_to_str(cmd->fields[i].out_value,
 204                                 (cmd->fields[i].num_bits > DEBUG_JTAG_IOZ)
 205                                         ? DEBUG_JTAG_IOZ
 206                                         : cmd->fields[i].num_bits, 16);
 207
 208                         LOG_DEBUG("fields[%i].out_value[%i]: 0x%s", i,
 209                                         cmd->fields[i].num_bits, char_buf);
 210                         free(char_buf);
 211 #endif
 212                         buf_set_buf(cmd->fields[i].out_value, 0, *buffer,
 213                                         bit_count, cmd->fields[i].num_bits);
 214                 } else {
 215                         DEBUG_JTAG_IO("fields[%i].out_value[%i]: NULL",
 216                                         i, cmd->fields[i].num_bits);
 217                 }
 218
 219                 bit_count += cmd->fields[i].num_bits;
 220         }
 221
 222         /*DEBUG_JTAG_IO("bit_count totalling: %i",  bit_count); */
 223
 224         return bit_count;
 225 }
 226
 227 int jtag_read_buffer(uint8_t *buffer, const struct scan_command *cmd)
 228 {
 229         int i;
 230         int bit_count = 0;
 231         int retval;
 232
 233         /* we return ERROR_OK, unless a check fails, or a handler reports a problem */
 234         retval = ERROR_OK;
 235
 236         for (i = 0; i < cmd->num_fields; i++) {
 237                 /* if neither in_value nor in_handler
 238                  * are specified we don't have to examine this field
 239                  */
 240                 if (cmd->fields[i].in_value) {
 241                         int num_bits = cmd->fields[i].num_bits;
 242                         uint8_t *captured = buf_set_buf(buffer, bit_count,
 243                                         malloc(DIV_ROUND_UP(num_bits, 8)), 0, num_bits);
 244
 245 #ifdef _DEBUG_JTAG_IO_
 246                         char *char_buf = buf_to_str(captured,
 247                                         (num_bits > DEBUG_JTAG_IOZ)
 248                                                 ? DEBUG_JTAG_IOZ
 249                                                 : num_bits, 16);
 250
 251                         LOG_DEBUG("fields[%i].in_value[%i]: 0x%s",
 252                                         i, num_bits, char_buf);
 253                         free(char_buf);
 254 #endif
 255
 256                         if (cmd->fields[i].in_value)
 257                                 buf_cpy(captured, cmd->fields[i].in_value, num_bits);
 258
 259                         free(captured);
 260                 }
 261                 bit_count += cmd->fields[i].num_bits;
 262         }
 263
 264         return retval;
 265 }