You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 

266 lines
7.1 KiB

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