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