]> git.sur5r.net Git - openocd/commitdiff
From Michael Bruck
authoroharboe <oharboe@b42882b7-edfa-0310-969c-e2dbd0fdcd60>
Mon, 25 Feb 2008 08:22:18 +0000 (08:22 +0000)
committeroharboe <oharboe@b42882b7-edfa-0310-969c-e2dbd0fdcd60>
Mon, 25 Feb 2008 08:22:18 +0000 (08:22 +0000)
- bugfix in server.c
- removed unused parameter from jtag_add_ir_scan et al. This
wasn't necessary in hindsight but anyway.
- arm11 source committed but not not in Makefile.am/target.c for now.

git-svn-id: svn://svn.berlios.de/openocd/trunk@341 b42882b7-edfa-0310-969c-e2dbd0fdcd60

21 files changed:
src/flash/str9xpec.c
src/jtag/jtag.c
src/jtag/jtag.h
src/pld/virtex2.c
src/target/arm11.c [new file with mode: 0644]
src/target/arm11.h [new file with mode: 0644]
src/target/arm11_dbgtap.c [new file with mode: 0644]
src/target/arm720t.c
src/target/arm7tdmi.c
src/target/arm920t.c
src/target/arm926ejs.c
src/target/arm966e.c
src/target/arm9tdmi.c
src/target/arm_jtag.c
src/target/cortex_swjdp.c
src/target/embeddedice.c
src/target/etb.c
src/target/etm.c
src/target/target.c
src/target/xscale.c
src/xsvf/xsvf.c

index 4959df9ff8d99ab39aaba99556bff6d98f5550d6..9df57dd5b1dbedc001acef9fafabceee71b60a0e 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2005 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "replacements.h"
-
-#include "str9xpec.h"
-#include "flash.h"
-#include "target.h"
-#include "log.h"
-#include "armv4_5.h"
-#include "arm7_9_common.h"
-#include "jtag.h"
-#include "binarybuffer.h"
-
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <getopt.h>
-
-str9xpec_mem_layout_t mem_layout_str9pec[] = {
-       {0x00000000, 0x10000, 0},
-       {0x00010000, 0x10000, 1},
-       {0x00020000, 0x10000, 2},
-       {0x00030000, 0x10000, 3},
-       {0x00040000, 0x10000, 4},
-       {0x00050000, 0x10000, 5},
-       {0x00060000, 0x10000, 6},
-       {0x00070000, 0x10000, 7},
-       {0x00080000, 0x02000, 32},
-       {0x00082000, 0x02000, 33},
-       {0x00084000, 0x02000, 34},
-       {0x00086000, 0x02000, 35}
-};
-
-int str9xpec_register_commands(struct command_context_s *cmd_ctx);
-int str9xpec_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
-int str9xpec_erase(struct flash_bank_s *bank, int first, int last);
-int str9xpec_protect(struct flash_bank_s *bank, int set, int first, int last);
-int str9xpec_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
-int str9xpec_probe(struct flash_bank_s *bank);
-int str9xpec_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_protect_check(struct flash_bank_s *bank);
-int str9xpec_erase_check(struct flash_bank_s *bank);
-int str9xpec_info(struct flash_bank_s *bank, char *buf, int buf_size);
-
-int str9xpec_erase_area(struct flash_bank_s *bank, int first, int last);
-int str9xpec_set_address(struct flash_bank_s *bank, u8 sector);
-int str9xpec_write_options(struct flash_bank_s *bank);
-
-int str9xpec_handle_flash_options_cmap_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_options_lvdthd_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_options_lvdsel_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_options_lvdwarn_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_enable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-flash_driver_t str9xpec_flash =
-{
-       .name = "str9xpec",
-       .register_commands = str9xpec_register_commands,
-       .flash_bank_command = str9xpec_flash_bank_command,
-       .erase = str9xpec_erase,
-       .protect = str9xpec_protect,
-       .write = str9xpec_write,
-       .probe = str9xpec_probe,
-       .auto_probe = str9xpec_probe,
-       .erase_check = str9xpec_erase_check,
-       .protect_check = str9xpec_protect_check,
-       .info = str9xpec_info
-};
-
-int str9xpec_register_commands(struct command_context_s *cmd_ctx)
-{
-       command_t *str9xpec_cmd = register_command(cmd_ctx, NULL, "str9xpec", NULL, COMMAND_ANY, "str9xpec flash specific commands");
-       
-       register_command(cmd_ctx, str9xpec_cmd, "enable_turbo", str9xpec_handle_flash_enable_turbo_command, COMMAND_EXEC,
-                                        "enable str9xpec turbo mode");
-       register_command(cmd_ctx, str9xpec_cmd, "disable_turbo", str9xpec_handle_flash_disable_turbo_command, COMMAND_EXEC,
-                                        "disable str9xpec turbo mode");
-       register_command(cmd_ctx, str9xpec_cmd, "options_cmap", str9xpec_handle_flash_options_cmap_command, COMMAND_EXEC,
-                                        "configure str9xpec boot sector");
-       register_command(cmd_ctx, str9xpec_cmd, "options_lvdthd", str9xpec_handle_flash_options_lvdthd_command, COMMAND_EXEC,
-                                        "configure str9xpec lvd threshold");
-       register_command(cmd_ctx, str9xpec_cmd, "options_lvdsel", str9xpec_handle_flash_options_lvdsel_command, COMMAND_EXEC,
-                                        "configure str9xpec lvd selection");
-       register_command(cmd_ctx, str9xpec_cmd, "options_lvdwarn", str9xpec_handle_flash_options_lvdwarn_command, COMMAND_EXEC,
-                                        "configure str9xpec lvd warning");
-       register_command(cmd_ctx, str9xpec_cmd, "options_read", str9xpec_handle_flash_options_read_command, COMMAND_EXEC,
-                                        "read str9xpec options");
-       register_command(cmd_ctx, str9xpec_cmd, "options_write", str9xpec_handle_flash_options_write_command, COMMAND_EXEC,
-                                        "write str9xpec options");
-       register_command(cmd_ctx, str9xpec_cmd, "lock", str9xpec_handle_flash_lock_command, COMMAND_EXEC,
-                                        "lock str9xpec device");
-       register_command(cmd_ctx, str9xpec_cmd, "unlock", str9xpec_handle_flash_unlock_command, COMMAND_EXEC,
-                                        "unlock str9xpec device");
-       register_command(cmd_ctx, str9xpec_cmd, "part_id", str9xpec_handle_part_id_command, COMMAND_EXEC,
-                                        "print part id of str9xpec flash bank <num>");
-       
-       return ERROR_OK;
-}
-
-int str9xpec_set_instr(int chain_pos, u32 new_instr, enum tap_state end_state)
-{
-       jtag_device_t *device = jtag_get_device(chain_pos);
-       
-       if (device == NULL)
-       {
-               DEBUG("Invalid Target");
-               return ERROR_TARGET_INVALID;
-       }
-               
-       if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)
-       {
-               scan_field_t field;
-                               
-               field.device = chain_pos;
-               field.num_bits = device->ir_length;
-               field.out_value = calloc(CEIL(field.num_bits, 8), 1);
-               buf_set_u32(field.out_value, 0, field.num_bits, new_instr);
-               field.out_mask = NULL;
-               field.in_value = NULL;
-               field.in_check_value = NULL;
-               field.in_check_mask = NULL;
-               field.in_handler = NULL;
-               field.in_handler_priv = NULL;
-               
-               jtag_add_ir_scan(1, &field, end_state, NULL);
-               
-               free(field.out_value);
-       }
-       
-       return ERROR_OK;
-}
-
-u8 str9xpec_isc_status(int chain_pos)
-{
-       scan_field_t field;
-       u8 status;
-       
-       if (str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI) != ERROR_OK)
-               return ISC_STATUS_ERROR;
-       
-       field.device = chain_pos;
-       field.num_bits = 8;
-       field.out_value = NULL;
-       field.out_mask = NULL;
-       field.in_value = &status;
-       field.in_check_value = NULL;
-       field.in_check_mask = NULL;
-       field.in_handler = NULL;
-       field.in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-       jtag_execute_queue();
-       
-       DEBUG("status: 0x%2.2x", status);
-       
-       if (status & ISC_STATUS_SECURITY)
-               INFO("Device Security Bit Set");
-       
-       return status;
-}
-
-int str9xpec_isc_enable(struct flash_bank_s *bank)
-{
-       u8 status;
-       u32 chain_pos;
-       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-       
-       chain_pos = str9xpec_info->chain_pos;
-       
-       if (str9xpec_info->isc_enable)
-               return ERROR_OK;
-       
-       /* enter isc mode */
-       if (str9xpec_set_instr(chain_pos, ISC_ENABLE, TAP_RTI) != ERROR_OK)
-               return ERROR_TARGET_INVALID;
-       
-       /* check ISC status */
-       status = str9xpec_isc_status(chain_pos);
-       if (status & ISC_STATUS_MODE)
-       {
-               /* we have entered isc mode */
-               str9xpec_info->isc_enable = 1;
-               DEBUG("ISC_MODE Enabled");
-       }
-       
-       return ERROR_OK;
-}
-
-int str9xpec_isc_disable(struct flash_bank_s *bank)
-{
-       u8 status;
-       u32 chain_pos;
-       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-       
-       chain_pos = str9xpec_info->chain_pos;
-       
-       if (!str9xpec_info->isc_enable)
-               return ERROR_OK;
-       
-       if (str9xpec_set_instr(chain_pos, ISC_DISABLE, TAP_RTI) != ERROR_OK)
-               return ERROR_TARGET_INVALID;
-       
-       /* delay to handle aborts */
-       jtag_add_sleep(50);
-       
-       /* check ISC status */
-       status = str9xpec_isc_status(chain_pos);
-       if (!(status & ISC_STATUS_MODE))
-       {
-               /* we have left isc mode */
-               str9xpec_info->isc_enable = 0;
-               DEBUG("ISC_MODE Disabled");
-       }
-       
-       return ERROR_OK;
-}
-
-int str9xpec_read_config(struct flash_bank_s *bank)
-{
-       scan_field_t field;
-       u8 status;
-       u32 chain_pos;
-               
-       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-       
-       chain_pos = str9xpec_info->chain_pos;
-       
-       DEBUG("ISC_CONFIGURATION");
-       
-       /* execute ISC_CONFIGURATION command */
-       str9xpec_set_instr(chain_pos, ISC_CONFIGURATION, TAP_PI);
-       
-       field.device = chain_pos;
-       field.num_bits = 64;
-       field.out_value = NULL;
-       field.out_mask = NULL;
-       field.in_value = str9xpec_info->options;
-       field.in_check_value = NULL;
-       field.in_check_mask = NULL;
-       field.in_handler = NULL;
-       field.in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-       jtag_execute_queue();
-       
-       status = str9xpec_isc_status(chain_pos);
-       
-       return status;
-}
-
-int str9xpec_build_block_list(struct flash_bank_s *bank)
-{
-       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-       
-       int i;
-       int num_sectors = 0, b0_sectors = 0;
-               
-       switch (bank->size)
-       {
-               case (256 * 1024):
-                       b0_sectors = 4;
-                       break;
-               case (512 * 1024):
-                       b0_sectors = 8;
-                       break;
-               default:
-                       ERROR("BUG: unknown bank->size encountered");
-                       exit(-1);
-       }
-       
-       /* include bank 1 sectors */
-       num_sectors = b0_sectors + 4;
-       bank->size += (32 * 1024);
-       
-       bank->num_sectors = num_sectors;
-       bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);
-       str9xpec_info->sector_bits = malloc(sizeof(u32) * num_sectors);
-       
-       num_sectors = 0;
-       
-       for (i = 0; i < b0_sectors; i++)
-       {
-               bank->sectors[num_sectors].offset = mem_layout_str9pec[i].sector_start;
-               bank->sectors[num_sectors].size = mem_layout_str9pec[i].sector_size;
-               bank->sectors[num_sectors].is_erased = -1;
-               bank->sectors[num_sectors].is_protected = 1;
-               str9xpec_info->sector_bits[num_sectors++] = mem_layout_str9pec[i].sector_bit;
-       }
-       
-       for (i = 8; i < 12; i++)
-       {
-               bank->sectors[num_sectors].offset = mem_layout_str9pec[i].sector_start;
-               bank->sectors[num_sectors].size = mem_layout_str9pec[i].sector_size;
-               bank->sectors[num_sectors].is_erased = -1;
-               bank->sectors[num_sectors].is_protected = 1;
-               str9xpec_info->sector_bits[num_sectors++] = mem_layout_str9pec[i].sector_bit;
-       }
-       
-       return ERROR_OK;
-}
-
-/* flash bank str9x <base> <size> 0 0 <target#>
- */
-int str9xpec_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
-{
-       str9xpec_flash_controller_t *str9xpec_info;
-       armv4_5_common_t *armv4_5 = NULL;
-       arm7_9_common_t *arm7_9 = NULL;
-       arm_jtag_t *jtag_info = NULL;
-       
-       if (argc < 6)
-       {
-               WARNING("incomplete flash_bank str9x configuration");
-               return ERROR_FLASH_BANK_INVALID;
-       }
-       
-       str9xpec_info = malloc(sizeof(str9xpec_flash_controller_t));
-       bank->driver_priv = str9xpec_info;
-       
-       if (bank->base != 0x00000000)
-       {
-               WARNING("overriding flash base address for STR91x device with 0x00000000");
-               bank->base = 0x00000000;
-       }
-
-       /* find out jtag position of flash controller
-        * it is always after the arm966 core */
-       
-       armv4_5 = bank->target->arch_info;
-       arm7_9 = armv4_5->arch_info;
-       jtag_info = &arm7_9->jtag_info;
-       
-       str9xpec_info->chain_pos = (jtag_info->chain_pos - 1);
-       str9xpec_info->isc_enable = 0;
-       str9xpec_info->devarm = NULL;
-       
-       str9xpec_build_block_list(bank);
-       
-       /* clear option byte register */
-       buf_set_u32(str9xpec_info->options, 0, 64, 0);
-       
-       return ERROR_OK;
-}
-
-int str9xpec_blank_check(struct flash_bank_s *bank, int first, int last)
-{
-       scan_field_t field;
-       u8 status;
-       u32 chain_pos;
-       int i;
-       u8 *buffer = NULL;
-               
-       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-       
-       chain_pos = str9xpec_info->chain_pos;
-       
-       if (!str9xpec_info->isc_enable) {
-               str9xpec_isc_enable( bank );
-       }
-       
-       if (!str9xpec_info->isc_enable) {
-               return ERROR_FLASH_OPERATION_FAILED;
-       }
-       
-       buffer = calloc(CEIL(64, 8), 1);
-
-       DEBUG("blank check: first_bank: %i, last_bank: %i", first, last);
-       
-       for (i = first; i <= last; i++) {
-               buf_set_u32(buffer, str9xpec_info->sector_bits[i], 1, 1);
-       }
-       
-       /* execute ISC_BLANK_CHECK command */
-       str9xpec_set_instr(chain_pos, ISC_BLANK_CHECK, TAP_PI);
-       
-       field.device = chain_pos;
-       field.num_bits = 64;
-       field.out_value = buffer;
-       field.out_mask = NULL;
-       field.in_value = NULL;
-       field.in_check_value = NULL;
-       field.in_check_mask = NULL;
-       field.in_handler = NULL;
-       field.in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-       jtag_add_sleep(40000);
-       
-       /* read blank check result */
-       field.device = chain_pos;
-       field.num_bits = 64;
-       field.out_value = NULL;
-       field.out_mask = NULL;
-       field.in_value = buffer;
-       field.in_check_value = NULL;
-       field.in_check_mask = NULL;
-       field.in_handler = NULL;
-       field.in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(1, &field, TAP_PI, NULL);
-       jtag_execute_queue();
-       
-       status = str9xpec_isc_status(chain_pos);
-       
-       for (i = first; i <= last; i++)
-       {
-               if (buf_get_u32(buffer, str9xpec_info->sector_bits[i], 1))
-                       bank->sectors[i].is_erased = 0;
-               else
-                       bank->sectors[i].is_erased = 1;
-       }
-       
-       free(buffer);
-       
-       str9xpec_isc_disable(bank);
-       
-       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-               return ERROR_FLASH_OPERATION_FAILED; 
-       return ERROR_OK;
-}
-
-int str9xpec_protect_check(struct flash_bank_s *bank)
-{
-       u8 status;
-       int i;
-               
-       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-       
-       status = str9xpec_read_config(bank);
-       
-       for (i = 0; i < bank->num_sectors; i++)
-       {
-               if (buf_get_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1))
-                       bank->sectors[i].is_protected = 1;
-               else
-                       bank->sectors[i].is_protected = 0;
-       }
-       
-       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-               return ERROR_FLASH_OPERATION_FAILED;
-       return ERROR_OK;
-}
-
-int str9xpec_erase_area(struct flash_bank_s *bank, int first, int last)
-{
-       scan_field_t field;
-       u8 status;
-       u32 chain_pos;
-       int i;
-       u8 *buffer = NULL;
-       
-       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-       
-       chain_pos = str9xpec_info->chain_pos;
-       
-       if (!str9xpec_info->isc_enable) {
-               str9xpec_isc_enable( bank );
-       }
-       
-       if (!str9xpec_info->isc_enable) {
-               return ISC_STATUS_ERROR;
-       }
-       
-       buffer = calloc(CEIL(64, 8), 1);
-       
-       DEBUG("erase: first_bank: %i, last_bank: %i", first, last);
-       
-       /* last bank: 0xFF signals a full erase (unlock complete device) */
-       /* last bank: 0xFE signals a option byte erase */
-       if (last == 0xFF)
-       {
-               for (i = 0; i < 64; i++) {
-                       buf_set_u32(buffer, i, 1, 1);
-               }       
-       }
-       else if (last == 0xFE)
-       {
-               buf_set_u32(buffer, 49, 1, 1);
-       }
-       else
-       {       
-               for (i = first; i <= last; i++) {
-                       buf_set_u32(buffer, str9xpec_info->sector_bits[i], 1, 1);
-               }
-       }
-       
-       DEBUG("ISC_ERASE");
-       
-       /* execute ISC_ERASE command */
-       str9xpec_set_instr(chain_pos, ISC_ERASE, TAP_PI);
-       
-       field.device = chain_pos;
-       field.num_bits = 64;
-       field.out_value = buffer;
-       field.out_mask = NULL;
-       field.in_value = NULL;
-       field.in_check_value = NULL;
-       field.in_check_mask = NULL;
-       field.in_handler = NULL;
-       field.in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-       jtag_execute_queue();
-       
-       jtag_add_sleep(10);
-       
-       /* wait for erase completion */
-       while (!((status = str9xpec_isc_status(chain_pos)) & ISC_STATUS_BUSY)) {
-               usleep(1000);
-       }
-       
-       free(buffer);
-       
-       str9xpec_isc_disable(bank);
-       
-       return status;
-}
-
-int str9xpec_erase(struct flash_bank_s *bank, int first, int last)
-{
-       int status;
-       
-       status = str9xpec_erase_area(bank, first, last);
-       
-       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-               return ERROR_FLASH_OPERATION_FAILED;
-       
-       return ERROR_OK;
-}
-
-int str9xpec_lock_device(struct flash_bank_s *bank)
-{
-       scan_field_t field;
-       u8 status;
-       u32 chain_pos;
-       str9xpec_flash_controller_t *str9xpec_info = NULL;
-       
-       str9xpec_info = bank->driver_priv;
-       chain_pos = str9xpec_info->chain_pos;
-       
-       if (!str9xpec_info->isc_enable) {
-               str9xpec_isc_enable( bank );
-       }
-       
-       if (!str9xpec_info->isc_enable) {
-               return ISC_STATUS_ERROR;
-       }
-       
-       /* set security address */
-       str9xpec_set_address(bank, 0x80);
-       
-       /* execute ISC_PROGRAM command */
-       str9xpec_set_instr(chain_pos, ISC_PROGRAM_SECURITY, TAP_RTI);
-       
-       str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);
-       
-       do {
-               field.device = chain_pos;
-               field.num_bits = 8;
-               field.out_value = NULL;
-               field.out_mask = NULL;
-               field.in_value = &status;
-               field.in_check_value = NULL;
-               field.in_check_mask = NULL;
-               field.in_handler = NULL;
-               field.in_handler_priv = NULL;
-               
-               jtag_add_dr_scan(1, &field, -1, NULL);
-               jtag_execute_queue();
-               
-       } while(!(status & ISC_STATUS_BUSY));
-       
-       str9xpec_isc_disable(bank);
-       
-       return status;
-}
-
-int str9xpec_unlock_device(struct flash_bank_s *bank)
-{
-       u8 status;
-       
-       status = str9xpec_erase_area(bank, 0, 255);
-       
-       return status;
-}
-
-int str9xpec_protect(struct flash_bank_s *bank, int set, int first, int last)
-{
-       u8 status;
-       int i;
-       
-       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-       
-       status = str9xpec_read_config(bank);
-       
-       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-               return ERROR_FLASH_OPERATION_FAILED;
-
-       DEBUG("protect: first_bank: %i, last_bank: %i", first, last);
-       
-       /* last bank: 0xFF signals a full device protect */
-       if (last == 0xFF)
-       {
-               if( set )
-               {
-                       status = str9xpec_lock_device(bank);
-               }
-               else
-               {
-                       /* perform full erase to unlock device */
-                       status = str9xpec_unlock_device(bank);
-               }
-       }
-       else
-       {       
-               for (i = first; i <= last; i++)
-               {
-                       if( set )
-                               buf_set_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1, 1);
-                       else
-                               buf_set_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1, 0);
-               }
-               
-               status = str9xpec_write_options(bank);
-       }
-       
-       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-               return ERROR_FLASH_OPERATION_FAILED;
-       
-       return ERROR_OK;
-}
-
-int str9xpec_set_address(struct flash_bank_s *bank, u8 sector)
-{
-       u32 chain_pos;
-       scan_field_t field;
-       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-       
-       chain_pos = str9xpec_info->chain_pos;
-       
-       /* set flash controller address */
-       str9xpec_set_instr(chain_pos, ISC_ADDRESS_SHIFT, TAP_PI);
-       
-       field.device = chain_pos;
-       field.num_bits = 8;
-       field.out_value = &sector;
-       field.out_mask = NULL;
-       field.in_value = NULL;
-       field.in_check_value = NULL;
-       field.in_check_mask = NULL;
-       field.in_handler = NULL;
-       field.in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(1, &field, -1, NULL);
-               
-       return ERROR_OK;
-}
-
-int str9xpec_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
-{
-       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
-       u32 dwords_remaining = (count / 8);
-       u32 bytes_remaining = (count & 0x00000007);
-       u32 bytes_written = 0;
-       u8 status;
-       u32 check_address = offset;
-       u32 chain_pos;
-       scan_field_t field;
-       u8 *scanbuf;
-       int i;
-       u32 first_sector = 0;
-       u32 last_sector = 0;
-       
-       chain_pos = str9xpec_info->chain_pos;
-       
-       if (!str9xpec_info->isc_enable) {
-               str9xpec_isc_enable(bank);
-       }
-       
-       if (!str9xpec_info->isc_enable) {
-               return ERROR_FLASH_OPERATION_FAILED;
-       }
-       
-       if (offset & 0x7)
-       {
-               WARNING("offset 0x%x breaks required 8-byte alignment", offset);
-               return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
-       }
-       
-       for (i = 0; i < bank->num_sectors; i++)
-       {
-               u32 sec_start = bank->sectors[i].offset;
-               u32 sec_end = sec_start + bank->sectors[i].size;
-               
-               /* check if destination falls within the current sector */
-               if ((check_address >= sec_start) && (check_address < sec_end))
-               {
-                       /* check if destination ends in the current sector */
-                       if (offset + count < sec_end)
-                               check_address = offset + count;
-                       else
-                               check_address = sec_end;
-               }
-               
-               if ((offset >= sec_start) && (offset < sec_end)){
-                       first_sector = i;
-               }
-               
-               if ((offset + count >= sec_start) && (offset + count < sec_end)){
-                       last_sector = i;
-               }
-       }
-       
-       if (check_address != offset + count)
-               return ERROR_FLASH_DST_OUT_OF_BANK;
-
-       DEBUG("first_sector: %i, last_sector: %i", first_sector, last_sector);
-       
-       scanbuf = calloc(CEIL(64, 8), 1);
-       
-       DEBUG("ISC_PROGRAM");
-       
-       for (i = first_sector; i <= last_sector; i++)
-       {
-               str9xpec_set_address(bank, str9xpec_info->sector_bits[i]);
-               
-               dwords_remaining = dwords_remaining < (bank->sectors[i].size/8) ? dwords_remaining : (bank->sectors[i].size/8);
-
-               while (dwords_remaining > 0)
-               {       
-                       str9xpec_set_instr(chain_pos, ISC_PROGRAM, TAP_PI);
-                       
-                       field.device = chain_pos;
-                       field.num_bits = 64;
-                       field.out_value = (buffer + bytes_written);
-                       field.out_mask = NULL;
-                       field.in_value = NULL;
-                       field.in_check_value = NULL;
-                       field.in_check_mask = NULL;
-                       field.in_handler = NULL;
-                       field.in_handler_priv = NULL;
-                       
-                       jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-                       
-                       /* small delay before polling */
-                       jtag_add_sleep(50);
-                       
-                       str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);
-                       
-                       do {
-                               field.device = chain_pos;
-                               field.num_bits = 8;
-                               field.out_value = NULL;
-                               field.out_mask = NULL;
-                               field.in_value = scanbuf;
-                               field.in_check_value = NULL;
-                               field.in_check_mask = NULL;
-                               field.in_handler = NULL;
-                               field.in_handler_priv = NULL;
-                               
-                               jtag_add_dr_scan(1, &field, -1, NULL);
-                               jtag_execute_queue();
-                               
-                               status = buf_get_u32(scanbuf, 0, 8);
-                               
-                       } while(!(status & ISC_STATUS_BUSY));
-                       
-                       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-                               return ERROR_FLASH_OPERATION_FAILED;
-                       
-                       //if ((status & ISC_STATUS_INT_ERROR) != STR9XPEC_ISC_INTFAIL)
-                       //      return ERROR_FLASH_OPERATION_FAILED;
-               
-                       dwords_remaining--;
-                       bytes_written += 8;
-               }
-       }
-       
-       if (bytes_remaining)
-       {
-               u8 last_dword[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-               int i = 0;
-                               
-               while(bytes_remaining > 0)
-               {
-                       last_dword[i++] = *(buffer + bytes_written); 
-                       bytes_remaining--;
-                       bytes_written++;
-               }
-               
-               str9xpec_set_instr(chain_pos, ISC_PROGRAM, TAP_PI);
-               
-               field.device = chain_pos;
-               field.num_bits = 64;
-               field.out_value = last_dword;
-               field.out_mask = NULL;
-               field.in_value = NULL;
-               field.in_check_value = NULL;
-               field.in_check_mask = NULL;
-               field.in_handler = NULL;
-               field.in_handler_priv = NULL;
-               
-               jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-               
-               /* small delay before polling */
-               jtag_add_sleep(50);
-               
-               str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);
-               
-               do {
-                       field.device = chain_pos;
-                       field.num_bits = 8;
-                       field.out_value = NULL;
-                       field.out_mask = NULL;
-                       field.in_value = scanbuf;
-                       field.in_check_value = NULL;
-                       field.in_check_mask = NULL;
-                       field.in_handler = NULL;
-                       field.in_handler_priv = NULL;
-                       
-                       jtag_add_dr_scan(1, &field, -1, NULL);
-                       jtag_execute_queue();
-                       
-                       status = buf_get_u32(scanbuf, 0, 8);
-                       
-               } while(!(status & ISC_STATUS_BUSY));
-               
-               if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-                       return ERROR_FLASH_OPERATION_FAILED;
-               
-               //if ((status & ISC_STATUS_INT_ERROR) != STR9XPEC_ISC_INTFAIL)
-               //      return ERROR_FLASH_OPERATION_FAILED;
-       }
-
-       free(scanbuf);
-
-       str9xpec_isc_disable(bank);
-                               
-       return ERROR_OK;
-}
-
-int str9xpec_probe(struct flash_bank_s *bank)
-{
-       return ERROR_OK;
-}
-
-int str9xpec_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       flash_bank_t *bank;
-       scan_field_t field;
-       u8 *buffer = NULL;
-       u32 chain_pos;
-       u32 idcode;
-       str9xpec_flash_controller_t *str9xpec_info = NULL;
-
-       if (argc < 1)
-       {
-               return ERROR_COMMAND_SYNTAX_ERROR;
-       }
-       
-       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-       if (!bank)
-       {
-               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-               return ERROR_OK;
-       }
-       
-       str9xpec_info = bank->driver_priv;
-       chain_pos = str9xpec_info->chain_pos;
-       
-       buffer = calloc(CEIL(32, 8), 1);
-       
-       str9xpec_set_instr(chain_pos, ISC_IDCODE, TAP_PI);
-       
-       field.device = chain_pos;
-       field.num_bits = 32;
-       field.out_value = NULL;
-       field.out_mask = NULL;
-       field.in_value = buffer;
-       field.in_check_value = NULL;
-       field.in_check_mask = NULL;
-       field.in_handler = NULL;
-       field.in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-       jtag_execute_queue();
-       
-       idcode = buf_get_u32(buffer, 0, 32);
-       
-       command_print(cmd_ctx, "str9xpec part id: 0x%8.8x", idcode);
-       
-       free(buffer);
-       
-       return ERROR_OK;
-}
-
-int str9xpec_erase_check(struct flash_bank_s *bank)
-{
-       return str9xpec_blank_check(bank, 0, bank->num_sectors - 1);
-}
-
-int str9xpec_info(struct flash_bank_s *bank, char *buf, int buf_size)
-{
-       snprintf(buf, buf_size, "str9xpec flash driver info" );
-       return ERROR_OK;
-}
-
-int str9xpec_handle_flash_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       flash_bank_t *bank;
-       u8 status;
-       str9xpec_flash_controller_t *str9xpec_info = NULL;
-       
-       if (argc < 1)
-       {
-               command_print(cmd_ctx, "str9xpec options_read <bank>");
-               return ERROR_OK;        
-       }
-       
-       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-       if (!bank)
-       {
-               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-               return ERROR_OK;
-       }
-       
-       str9xpec_info = bank->driver_priv;
-       
-       status = str9xpec_read_config(bank);
-       
-       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-               return ERROR_FLASH_OPERATION_FAILED;
-       
-       /* boot bank */
-       if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1))
-               command_print(cmd_ctx, "CS Map: bank1");
-       else
-               command_print(cmd_ctx, "CS Map: bank0");
-       
-       /* OTP lock */
-       if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_OTPBIT, 1))
-               command_print(cmd_ctx, "OTP Lock: OTP Locked");
-       else
-               command_print(cmd_ctx, "OTP Lock: OTP Unlocked");
-       
-       /* LVD Threshold */
-       if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1))
-               command_print(cmd_ctx, "LVD Threshold: 2.7v");
-       else
-               command_print(cmd_ctx, "LVD Threshold: 2.4v");
-       
-       /* LVD reset warning */
-       if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1))
-               command_print(cmd_ctx, "LVD Reset Warning: VDD or VDDQ Inputs");
-       else
-               command_print(cmd_ctx, "LVD Reset Warning: VDD Input Only");
-       
-       /* LVD reset select */
-       if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1))
-               command_print(cmd_ctx, "LVD Reset Selection: VDD or VDDQ Inputs");
-       else
-               command_print(cmd_ctx, "LVD Reset Selection: VDD Input Only");
-       
-       return ERROR_OK;
-}
-
-int str9xpec_write_options(struct flash_bank_s *bank)
-{
-       scan_field_t field;
-       u8 status;
-       u32 chain_pos;
-       str9xpec_flash_controller_t *str9xpec_info = NULL;
-       
-       str9xpec_info = bank->driver_priv;
-       chain_pos = str9xpec_info->chain_pos;
-       
-       /* erase config options first */
-       status = str9xpec_erase_area( bank, 0xFE, 0xFE );
-       
-       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-               return status; 
-       
-       if (!str9xpec_info->isc_enable) {
-               str9xpec_isc_enable( bank );
-       }
-       
-       if (!str9xpec_info->isc_enable) {
-               return ISC_STATUS_ERROR;
-       }
-       
-       /* according to data 64th bit has to be set */
-       buf_set_u32(str9xpec_info->options, 63, 1, 1);
-       
-       /* set option byte address */
-       str9xpec_set_address(bank, 0x50);
-       
-       /* execute ISC_PROGRAM command */
-       str9xpec_set_instr(chain_pos, ISC_PROGRAM, TAP_PI);
-               
-       field.device = chain_pos;
-       field.num_bits = 64;
-       field.out_value = str9xpec_info->options;
-       field.out_mask = NULL;
-       field.in_value = NULL;
-       field.in_check_value = NULL;
-       field.in_check_mask = NULL;
-       field.in_handler = NULL;
-       field.in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
-       
-       /* small delay before polling */
-       jtag_add_sleep(50);
-       
-       str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);
-       
-       do {
-               field.device = chain_pos;
-               field.num_bits = 8;
-               field.out_value = NULL;
-               field.out_mask = NULL;
-               field.in_value = &status;
-               field.in_check_value = NULL;
-               field.in_check_mask = NULL;
-               field.in_handler = NULL;
-               field.in_handler_priv = NULL;
-               
-               jtag_add_dr_scan(1, &field, -1, NULL);
-               jtag_execute_queue();
-               
-       } while(!(status & ISC_STATUS_BUSY));
-       
-       str9xpec_isc_disable(bank);
-       
-       return status;
-}
-
-int str9xpec_handle_flash_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       flash_bank_t *bank;
-       u8 status;
-       
-       if (argc < 1)
-       {
-               command_print(cmd_ctx, "str9xpec options_write <bank>");
-               return ERROR_OK;        
-       }
-       
-       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-       if (!bank)
-       {
-               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-               return ERROR_OK;
-       }
-       
-       status = str9xpec_write_options(bank);
-       
-       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-               return ERROR_FLASH_OPERATION_FAILED;
-       
-       return ERROR_OK;
-}
-
-int str9xpec_handle_flash_options_cmap_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       flash_bank_t *bank;
-       str9xpec_flash_controller_t *str9xpec_info = NULL;
-       
-       if (argc < 2)
-       {
-               command_print(cmd_ctx, "str9xpec options_cmap <bank> <bank0|bank1>");
-               return ERROR_OK;        
-       }
-       
-       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-       if (!bank)
-       {
-               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-               return ERROR_OK;
-       }
-       
-       str9xpec_info = bank->driver_priv;
-       
-       if (strcmp(args[1], "bank1") == 0)
-       {
-               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 1);
-       }
-       else
-       {
-               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 0);
-       }
-       
-       return ERROR_OK;
-}
-
-int str9xpec_handle_flash_options_lvdthd_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       flash_bank_t *bank;
-       str9xpec_flash_controller_t *str9xpec_info = NULL;
-       
-       if (argc < 2)
-       {
-               command_print(cmd_ctx, "str9xpec options_lvdthd <bank> <2.4v|2.7v>");
-               return ERROR_OK;        
-       }
-       
-       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-       if (!bank)
-       {
-               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-               return ERROR_OK;
-       }
-       
-       str9xpec_info = bank->driver_priv;
-       
-       if (strcmp(args[1], "2.7v") == 0)
-       {
-               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 1);
-       }
-       else
-       {
-               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 0);
-       }
-       
-       return ERROR_OK;
-}
-
-int str9xpec_handle_flash_options_lvdsel_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       flash_bank_t *bank;
-       str9xpec_flash_controller_t *str9xpec_info = NULL;
-       
-       if (argc < 2)
-       {
-               command_print(cmd_ctx, "str9xpec options_lvdsel <bank> <vdd|vdd_vddq>");
-               return ERROR_OK;        
-       }
-       
-       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-       if (!bank)
-       {
-               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-               return ERROR_OK;
-       }
-       
-       str9xpec_info = bank->driver_priv;
-       
-       if (strcmp(args[1], "vdd_vddq") == 0)
-       {
-               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 1);
-       }
-       else
-       {
-               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 0);
-       }
-       
-       return ERROR_OK;
-}
-
-int str9xpec_handle_flash_options_lvdwarn_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       flash_bank_t *bank;
-       str9xpec_flash_controller_t *str9xpec_info = NULL;
-       
-       if (argc < 2)
-       {
-               command_print(cmd_ctx, "str9xpec options_lvdwarn <bank> <vdd|vdd_vddq>");
-               return ERROR_OK;        
-       }
-       
-       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-       if (!bank)
-       {
-               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-               return ERROR_OK;
-       }
-       
-       str9xpec_info = bank->driver_priv;
-       
-       if (strcmp(args[1], "vdd_vddq") == 0)
-       {
-               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 1);
-       }
-       else
-       {
-               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 0);
-       }
-       
-       return ERROR_OK;
-}
-
-int str9xpec_handle_flash_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       u8 status;
-       flash_bank_t *bank;
-       
-       if (argc < 1)
-       {
-               command_print(cmd_ctx, "str9xpec lock <bank>");
-               return ERROR_OK;        
-       }
-       
-       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-       if (!bank)
-       {
-               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-               return ERROR_OK;
-       }
-       
-       status = str9xpec_lock_device(bank);
-       
-       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-               return ERROR_FLASH_OPERATION_FAILED;
-       
-       return ERROR_OK;
-}
-
-int str9xpec_handle_flash_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       u8 status;
-       flash_bank_t *bank;
-       
-       if (argc < 1)
-       {
-               command_print(cmd_ctx, "str9xpec unlock <bank>");
-               return ERROR_OK;        
-       }
-       
-       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-       if (!bank)
-       {
-               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-               return ERROR_OK;
-       }
-       
-       status = str9xpec_unlock_device(bank);
-       
-       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
-               return ERROR_FLASH_OPERATION_FAILED;
-       
-       return ERROR_OK;
-}
-
-int str9xpec_handle_flash_enable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       flash_bank_t *bank;
-       u32 chain_pos;
-       jtag_device_t* dev0;
-       jtag_device_t* dev2;
-       str9xpec_flash_controller_t *str9xpec_info = NULL;
-       
-       if (argc < 1)
-       {
-               command_print(cmd_ctx, "str9xpec enable_turbo <bank>");
-               return ERROR_OK;        
-       }
-       
-       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-       if (!bank)
-       {
-               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-               return ERROR_OK;
-       }
-       
-       str9xpec_info = bank->driver_priv;
-       
-       chain_pos = str9xpec_info->chain_pos;
-       
-       /* remove arm core from chain - enter turbo mode */
-       
-       str9xpec_set_instr(chain_pos+2, 0xD, TAP_RTI);
-       jtag_execute_queue();
-       
-       /* modify scan chain - str9 core has been removed */
-       dev0 = jtag_get_device(chain_pos);
-       str9xpec_info->devarm = jtag_get_device(chain_pos+1);
-       dev2 = jtag_get_device(chain_pos+2);
-       dev0->next = dev2;
-       jtag_num_devices--;
-       
-       return ERROR_OK;
-}
-
-int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       flash_bank_t *bank;
-       u32 chain_pos;
-       jtag_device_t* dev0;
-       str9xpec_flash_controller_t *str9xpec_info = NULL;
-       
-       if (argc < 1)
-       {
-               command_print(cmd_ctx, "str9xpec disable_turbo <bank>");
-               return ERROR_OK;        
-       }
-       
-       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
-       if (!bank)
-       {
-               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
-               return ERROR_OK;
-       }
-       
-       str9xpec_info = bank->driver_priv;
-       
-       chain_pos = str9xpec_info->chain_pos;
-       
-       dev0 = jtag_get_device(chain_pos);
-       
-       /* exit turbo mode via TLR */
-       str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_TLR);
-       jtag_execute_queue();
-       
-       /* restore previous scan chain */
-       if( str9xpec_info->devarm ) {
-               dev0->next = str9xpec_info->devarm;
-               jtag_num_devices++;
-               str9xpec_info->devarm = NULL;
-       }
-       
-       return ERROR_OK;
-}
+/***************************************************************************\r
+ *   Copyright (C) 2005 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "replacements.h"\r
+\r
+#include "str9xpec.h"\r
+#include "flash.h"\r
+#include "target.h"\r
+#include "log.h"\r
+#include "armv4_5.h"\r
+#include "arm7_9_common.h"\r
+#include "jtag.h"\r
+#include "binarybuffer.h"\r
+\r
+#include <stdlib.h>\r
+#include <string.h>\r
+#include <unistd.h>\r
+#include <getopt.h>\r
+\r
+str9xpec_mem_layout_t mem_layout_str9pec[] = {\r
+       {0x00000000, 0x10000, 0},\r
+       {0x00010000, 0x10000, 1},\r
+       {0x00020000, 0x10000, 2},\r
+       {0x00030000, 0x10000, 3},\r
+       {0x00040000, 0x10000, 4},\r
+       {0x00050000, 0x10000, 5},\r
+       {0x00060000, 0x10000, 6},\r
+       {0x00070000, 0x10000, 7},\r
+       {0x00080000, 0x02000, 32},\r
+       {0x00082000, 0x02000, 33},\r
+       {0x00084000, 0x02000, 34},\r
+       {0x00086000, 0x02000, 35}\r
+};\r
+\r
+int str9xpec_register_commands(struct command_context_s *cmd_ctx);\r
+int str9xpec_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);\r
+int str9xpec_erase(struct flash_bank_s *bank, int first, int last);\r
+int str9xpec_protect(struct flash_bank_s *bank, int set, int first, int last);\r
+int str9xpec_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);\r
+int str9xpec_probe(struct flash_bank_s *bank);\r
+int str9xpec_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int str9xpec_protect_check(struct flash_bank_s *bank);\r
+int str9xpec_erase_check(struct flash_bank_s *bank);\r
+int str9xpec_info(struct flash_bank_s *bank, char *buf, int buf_size);\r
+\r
+int str9xpec_erase_area(struct flash_bank_s *bank, int first, int last);\r
+int str9xpec_set_address(struct flash_bank_s *bank, u8 sector);\r
+int str9xpec_write_options(struct flash_bank_s *bank);\r
+\r
+int str9xpec_handle_flash_options_cmap_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int str9xpec_handle_flash_options_lvdthd_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int str9xpec_handle_flash_options_lvdsel_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int str9xpec_handle_flash_options_lvdwarn_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int str9xpec_handle_flash_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int str9xpec_handle_flash_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int str9xpec_handle_flash_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int str9xpec_handle_flash_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int str9xpec_handle_flash_enable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+flash_driver_t str9xpec_flash =\r
+{\r
+       .name = "str9xpec",\r
+       .register_commands = str9xpec_register_commands,\r
+       .flash_bank_command = str9xpec_flash_bank_command,\r
+       .erase = str9xpec_erase,\r
+       .protect = str9xpec_protect,\r
+       .write = str9xpec_write,\r
+       .probe = str9xpec_probe,\r
+       .auto_probe = str9xpec_probe,\r
+       .erase_check = str9xpec_erase_check,\r
+       .protect_check = str9xpec_protect_check,\r
+       .info = str9xpec_info\r
+};\r
+\r
+int str9xpec_register_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       command_t *str9xpec_cmd = register_command(cmd_ctx, NULL, "str9xpec", NULL, COMMAND_ANY, "str9xpec flash specific commands");\r
+       \r
+       register_command(cmd_ctx, str9xpec_cmd, "enable_turbo", str9xpec_handle_flash_enable_turbo_command, COMMAND_EXEC,\r
+                                        "enable str9xpec turbo mode");\r
+       register_command(cmd_ctx, str9xpec_cmd, "disable_turbo", str9xpec_handle_flash_disable_turbo_command, COMMAND_EXEC,\r
+                                        "disable str9xpec turbo mode");\r
+       register_command(cmd_ctx, str9xpec_cmd, "options_cmap", str9xpec_handle_flash_options_cmap_command, COMMAND_EXEC,\r
+                                        "configure str9xpec boot sector");\r
+       register_command(cmd_ctx, str9xpec_cmd, "options_lvdthd", str9xpec_handle_flash_options_lvdthd_command, COMMAND_EXEC,\r
+                                        "configure str9xpec lvd threshold");\r
+       register_command(cmd_ctx, str9xpec_cmd, "options_lvdsel", str9xpec_handle_flash_options_lvdsel_command, COMMAND_EXEC,\r
+                                        "configure str9xpec lvd selection");\r
+       register_command(cmd_ctx, str9xpec_cmd, "options_lvdwarn", str9xpec_handle_flash_options_lvdwarn_command, COMMAND_EXEC,\r
+                                        "configure str9xpec lvd warning");\r
+       register_command(cmd_ctx, str9xpec_cmd, "options_read", str9xpec_handle_flash_options_read_command, COMMAND_EXEC,\r
+                                        "read str9xpec options");\r
+       register_command(cmd_ctx, str9xpec_cmd, "options_write", str9xpec_handle_flash_options_write_command, COMMAND_EXEC,\r
+                                        "write str9xpec options");\r
+       register_command(cmd_ctx, str9xpec_cmd, "lock", str9xpec_handle_flash_lock_command, COMMAND_EXEC,\r
+                                        "lock str9xpec device");\r
+       register_command(cmd_ctx, str9xpec_cmd, "unlock", str9xpec_handle_flash_unlock_command, COMMAND_EXEC,\r
+                                        "unlock str9xpec device");\r
+       register_command(cmd_ctx, str9xpec_cmd, "part_id", str9xpec_handle_part_id_command, COMMAND_EXEC,\r
+                                        "print part id of str9xpec flash bank <num>");\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_set_instr(int chain_pos, u32 new_instr, enum tap_state end_state)\r
+{\r
+       jtag_device_t *device = jtag_get_device(chain_pos);\r
+       \r
+       if (device == NULL)\r
+       {\r
+               DEBUG("Invalid Target");\r
+               return ERROR_TARGET_INVALID;\r
+       }\r
+               \r
+       if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)\r
+       {\r
+               scan_field_t field;\r
+                               \r
+               field.device = chain_pos;\r
+               field.num_bits = device->ir_length;\r
+               field.out_value = calloc(CEIL(field.num_bits, 8), 1);\r
+               buf_set_u32(field.out_value, 0, field.num_bits, new_instr);\r
+               field.out_mask = NULL;\r
+               field.in_value = NULL;\r
+               field.in_check_value = NULL;\r
+               field.in_check_mask = NULL;\r
+               field.in_handler = NULL;\r
+               field.in_handler_priv = NULL;\r
+               \r
+               jtag_add_ir_scan(1, &field, end_state);\r
+               \r
+               free(field.out_value);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+u8 str9xpec_isc_status(int chain_pos)\r
+{\r
+       scan_field_t field;\r
+       u8 status;\r
+       \r
+       if (str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI) != ERROR_OK)\r
+               return ISC_STATUS_ERROR;\r
+       \r
+       field.device = chain_pos;\r
+       field.num_bits = 8;\r
+       field.out_value = NULL;\r
+       field.out_mask = NULL;\r
+       field.in_value = &status;\r
+       field.in_check_value = NULL;\r
+       field.in_check_mask = NULL;\r
+       field.in_handler = NULL;\r
+       field.in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(1, &field, TAP_RTI);\r
+       jtag_execute_queue();\r
+       \r
+       DEBUG("status: 0x%2.2x", status);\r
+       \r
+       if (status & ISC_STATUS_SECURITY)\r
+               INFO("Device Security Bit Set");\r
+       \r
+       return status;\r
+}\r
+\r
+int str9xpec_isc_enable(struct flash_bank_s *bank)\r
+{\r
+       u8 status;\r
+       u32 chain_pos;\r
+       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;\r
+       \r
+       chain_pos = str9xpec_info->chain_pos;\r
+       \r
+       if (str9xpec_info->isc_enable)\r
+               return ERROR_OK;\r
+       \r
+       /* enter isc mode */\r
+       if (str9xpec_set_instr(chain_pos, ISC_ENABLE, TAP_RTI) != ERROR_OK)\r
+               return ERROR_TARGET_INVALID;\r
+       \r
+       /* check ISC status */\r
+       status = str9xpec_isc_status(chain_pos);\r
+       if (status & ISC_STATUS_MODE)\r
+       {\r
+               /* we have entered isc mode */\r
+               str9xpec_info->isc_enable = 1;\r
+               DEBUG("ISC_MODE Enabled");\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_isc_disable(struct flash_bank_s *bank)\r
+{\r
+       u8 status;\r
+       u32 chain_pos;\r
+       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;\r
+       \r
+       chain_pos = str9xpec_info->chain_pos;\r
+       \r
+       if (!str9xpec_info->isc_enable)\r
+               return ERROR_OK;\r
+       \r
+       if (str9xpec_set_instr(chain_pos, ISC_DISABLE, TAP_RTI) != ERROR_OK)\r
+               return ERROR_TARGET_INVALID;\r
+       \r
+       /* delay to handle aborts */\r
+       jtag_add_sleep(50);\r
+       \r
+       /* check ISC status */\r
+       status = str9xpec_isc_status(chain_pos);\r
+       if (!(status & ISC_STATUS_MODE))\r
+       {\r
+               /* we have left isc mode */\r
+               str9xpec_info->isc_enable = 0;\r
+               DEBUG("ISC_MODE Disabled");\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_read_config(struct flash_bank_s *bank)\r
+{\r
+       scan_field_t field;\r
+       u8 status;\r
+       u32 chain_pos;\r
+               \r
+       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;\r
+       \r
+       chain_pos = str9xpec_info->chain_pos;\r
+       \r
+       DEBUG("ISC_CONFIGURATION");\r
+       \r
+       /* execute ISC_CONFIGURATION command */\r
+       str9xpec_set_instr(chain_pos, ISC_CONFIGURATION, TAP_PI);\r
+       \r
+       field.device = chain_pos;\r
+       field.num_bits = 64;\r
+       field.out_value = NULL;\r
+       field.out_mask = NULL;\r
+       field.in_value = str9xpec_info->options;\r
+       field.in_check_value = NULL;\r
+       field.in_check_mask = NULL;\r
+       field.in_handler = NULL;\r
+       field.in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(1, &field, TAP_RTI);\r
+       jtag_execute_queue();\r
+       \r
+       status = str9xpec_isc_status(chain_pos);\r
+       \r
+       return status;\r
+}\r
+\r
+int str9xpec_build_block_list(struct flash_bank_s *bank)\r
+{\r
+       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;\r
+       \r
+       int i;\r
+       int num_sectors = 0, b0_sectors = 0;\r
+               \r
+       switch (bank->size)\r
+       {\r
+               case (256 * 1024):\r
+                       b0_sectors = 4;\r
+                       break;\r
+               case (512 * 1024):\r
+                       b0_sectors = 8;\r
+                       break;\r
+               default:\r
+                       ERROR("BUG: unknown bank->size encountered");\r
+                       exit(-1);\r
+       }\r
+       \r
+       /* include bank 1 sectors */\r
+       num_sectors = b0_sectors + 4;\r
+       bank->size += (32 * 1024);\r
+       \r
+       bank->num_sectors = num_sectors;\r
+       bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);\r
+       str9xpec_info->sector_bits = malloc(sizeof(u32) * num_sectors);\r
+       \r
+       num_sectors = 0;\r
+       \r
+       for (i = 0; i < b0_sectors; i++)\r
+       {\r
+               bank->sectors[num_sectors].offset = mem_layout_str9pec[i].sector_start;\r
+               bank->sectors[num_sectors].size = mem_layout_str9pec[i].sector_size;\r
+               bank->sectors[num_sectors].is_erased = -1;\r
+               bank->sectors[num_sectors].is_protected = 1;\r
+               str9xpec_info->sector_bits[num_sectors++] = mem_layout_str9pec[i].sector_bit;\r
+       }\r
+       \r
+       for (i = 8; i < 12; i++)\r
+       {\r
+               bank->sectors[num_sectors].offset = mem_layout_str9pec[i].sector_start;\r
+               bank->sectors[num_sectors].size = mem_layout_str9pec[i].sector_size;\r
+               bank->sectors[num_sectors].is_erased = -1;\r
+               bank->sectors[num_sectors].is_protected = 1;\r
+               str9xpec_info->sector_bits[num_sectors++] = mem_layout_str9pec[i].sector_bit;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+/* flash bank str9x <base> <size> 0 0 <target#>\r
+ */\r
+int str9xpec_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)\r
+{\r
+       str9xpec_flash_controller_t *str9xpec_info;\r
+       armv4_5_common_t *armv4_5 = NULL;\r
+       arm7_9_common_t *arm7_9 = NULL;\r
+       arm_jtag_t *jtag_info = NULL;\r
+       \r
+       if (argc < 6)\r
+       {\r
+               WARNING("incomplete flash_bank str9x configuration");\r
+               return ERROR_FLASH_BANK_INVALID;\r
+       }\r
+       \r
+       str9xpec_info = malloc(sizeof(str9xpec_flash_controller_t));\r
+       bank->driver_priv = str9xpec_info;\r
+       \r
+       if (bank->base != 0x00000000)\r
+       {\r
+               WARNING("overriding flash base address for STR91x device with 0x00000000");\r
+               bank->base = 0x00000000;\r
+       }\r
+\r
+       /* find out jtag position of flash controller\r
+        * it is always after the arm966 core */\r
+       \r
+       armv4_5 = bank->target->arch_info;\r
+       arm7_9 = armv4_5->arch_info;\r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       str9xpec_info->chain_pos = (jtag_info->chain_pos - 1);\r
+       str9xpec_info->isc_enable = 0;\r
+       str9xpec_info->devarm = NULL;\r
+       \r
+       str9xpec_build_block_list(bank);\r
+       \r
+       /* clear option byte register */\r
+       buf_set_u32(str9xpec_info->options, 0, 64, 0);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_blank_check(struct flash_bank_s *bank, int first, int last)\r
+{\r
+       scan_field_t field;\r
+       u8 status;\r
+       u32 chain_pos;\r
+       int i;\r
+       u8 *buffer = NULL;\r
+               \r
+       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;\r
+       \r
+       chain_pos = str9xpec_info->chain_pos;\r
+       \r
+       if (!str9xpec_info->isc_enable) {\r
+               str9xpec_isc_enable( bank );\r
+       }\r
+       \r
+       if (!str9xpec_info->isc_enable) {\r
+               return ERROR_FLASH_OPERATION_FAILED;\r
+       }\r
+       \r
+       buffer = calloc(CEIL(64, 8), 1);\r
+\r
+       DEBUG("blank check: first_bank: %i, last_bank: %i", first, last);\r
+       \r
+       for (i = first; i <= last; i++) {\r
+               buf_set_u32(buffer, str9xpec_info->sector_bits[i], 1, 1);\r
+       }\r
+       \r
+       /* execute ISC_BLANK_CHECK command */\r
+       str9xpec_set_instr(chain_pos, ISC_BLANK_CHECK, TAP_PI);\r
+       \r
+       field.device = chain_pos;\r
+       field.num_bits = 64;\r
+       field.out_value = buffer;\r
+       field.out_mask = NULL;\r
+       field.in_value = NULL;\r
+       field.in_check_value = NULL;\r
+       field.in_check_mask = NULL;\r
+       field.in_handler = NULL;\r
+       field.in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(1, &field, TAP_RTI);\r
+       jtag_add_sleep(40000);\r
+       \r
+       /* read blank check result */\r
+       field.device = chain_pos;\r
+       field.num_bits = 64;\r
+       field.out_value = NULL;\r
+       field.out_mask = NULL;\r
+       field.in_value = buffer;\r
+       field.in_check_value = NULL;\r
+       field.in_check_mask = NULL;\r
+       field.in_handler = NULL;\r
+       field.in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(1, &field, TAP_PI);\r
+       jtag_execute_queue();\r
+       \r
+       status = str9xpec_isc_status(chain_pos);\r
+       \r
+       for (i = first; i <= last; i++)\r
+       {\r
+               if (buf_get_u32(buffer, str9xpec_info->sector_bits[i], 1))\r
+                       bank->sectors[i].is_erased = 0;\r
+               else\r
+                       bank->sectors[i].is_erased = 1;\r
+       }\r
+       \r
+       free(buffer);\r
+       \r
+       str9xpec_isc_disable(bank);\r
+       \r
+       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)\r
+               return ERROR_FLASH_OPERATION_FAILED; \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_protect_check(struct flash_bank_s *bank)\r
+{\r
+       u8 status;\r
+       int i;\r
+               \r
+       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;\r
+       \r
+       status = str9xpec_read_config(bank);\r
+       \r
+       for (i = 0; i < bank->num_sectors; i++)\r
+       {\r
+               if (buf_get_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1))\r
+                       bank->sectors[i].is_protected = 1;\r
+               else\r
+                       bank->sectors[i].is_protected = 0;\r
+       }\r
+       \r
+       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)\r
+               return ERROR_FLASH_OPERATION_FAILED;\r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_erase_area(struct flash_bank_s *bank, int first, int last)\r
+{\r
+       scan_field_t field;\r
+       u8 status;\r
+       u32 chain_pos;\r
+       int i;\r
+       u8 *buffer = NULL;\r
+       \r
+       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;\r
+       \r
+       chain_pos = str9xpec_info->chain_pos;\r
+       \r
+       if (!str9xpec_info->isc_enable) {\r
+               str9xpec_isc_enable( bank );\r
+       }\r
+       \r
+       if (!str9xpec_info->isc_enable) {\r
+               return ISC_STATUS_ERROR;\r
+       }\r
+       \r
+       buffer = calloc(CEIL(64, 8), 1);\r
+       \r
+       DEBUG("erase: first_bank: %i, last_bank: %i", first, last);\r
+       \r
+       /* last bank: 0xFF signals a full erase (unlock complete device) */\r
+       /* last bank: 0xFE signals a option byte erase */\r
+       if (last == 0xFF)\r
+       {\r
+               for (i = 0; i < 64; i++) {\r
+                       buf_set_u32(buffer, i, 1, 1);\r
+               }       \r
+       }\r
+       else if (last == 0xFE)\r
+       {\r
+               buf_set_u32(buffer, 49, 1, 1);\r
+       }\r
+       else\r
+       {       \r
+               for (i = first; i <= last; i++) {\r
+                       buf_set_u32(buffer, str9xpec_info->sector_bits[i], 1, 1);\r
+               }\r
+       }\r
+       \r
+       DEBUG("ISC_ERASE");\r
+       \r
+       /* execute ISC_ERASE command */\r
+       str9xpec_set_instr(chain_pos, ISC_ERASE, TAP_PI);\r
+       \r
+       field.device = chain_pos;\r
+       field.num_bits = 64;\r
+       field.out_value = buffer;\r
+       field.out_mask = NULL;\r
+       field.in_value = NULL;\r
+       field.in_check_value = NULL;\r
+       field.in_check_mask = NULL;\r
+       field.in_handler = NULL;\r
+       field.in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(1, &field, TAP_RTI);\r
+       jtag_execute_queue();\r
+       \r
+       jtag_add_sleep(10);\r
+       \r
+       /* wait for erase completion */\r
+       while (!((status = str9xpec_isc_status(chain_pos)) & ISC_STATUS_BUSY)) {\r
+               usleep(1000);\r
+       }\r
+       \r
+       free(buffer);\r
+       \r
+       str9xpec_isc_disable(bank);\r
+       \r
+       return status;\r
+}\r
+\r
+int str9xpec_erase(struct flash_bank_s *bank, int first, int last)\r
+{\r
+       int status;\r
+       \r
+       status = str9xpec_erase_area(bank, first, last);\r
+       \r
+       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)\r
+               return ERROR_FLASH_OPERATION_FAILED;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_lock_device(struct flash_bank_s *bank)\r
+{\r
+       scan_field_t field;\r
+       u8 status;\r
+       u32 chain_pos;\r
+       str9xpec_flash_controller_t *str9xpec_info = NULL;\r
+       \r
+       str9xpec_info = bank->driver_priv;\r
+       chain_pos = str9xpec_info->chain_pos;\r
+       \r
+       if (!str9xpec_info->isc_enable) {\r
+               str9xpec_isc_enable( bank );\r
+       }\r
+       \r
+       if (!str9xpec_info->isc_enable) {\r
+               return ISC_STATUS_ERROR;\r
+       }\r
+       \r
+       /* set security address */\r
+       str9xpec_set_address(bank, 0x80);\r
+       \r
+       /* execute ISC_PROGRAM command */\r
+       str9xpec_set_instr(chain_pos, ISC_PROGRAM_SECURITY, TAP_RTI);\r
+       \r
+       str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);\r
+       \r
+       do {\r
+               field.device = chain_pos;\r
+               field.num_bits = 8;\r
+               field.out_value = NULL;\r
+               field.out_mask = NULL;\r
+               field.in_value = &status;\r
+               field.in_check_value = NULL;\r
+               field.in_check_mask = NULL;\r
+               field.in_handler = NULL;\r
+               field.in_handler_priv = NULL;\r
+               \r
+               jtag_add_dr_scan(1, &field, -1);\r
+               jtag_execute_queue();\r
+               \r
+       } while(!(status & ISC_STATUS_BUSY));\r
+       \r
+       str9xpec_isc_disable(bank);\r
+       \r
+       return status;\r
+}\r
+\r
+int str9xpec_unlock_device(struct flash_bank_s *bank)\r
+{\r
+       u8 status;\r
+       \r
+       status = str9xpec_erase_area(bank, 0, 255);\r
+       \r
+       return status;\r
+}\r
+\r
+int str9xpec_protect(struct flash_bank_s *bank, int set, int first, int last)\r
+{\r
+       u8 status;\r
+       int i;\r
+       \r
+       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;\r
+       \r
+       status = str9xpec_read_config(bank);\r
+       \r
+       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)\r
+               return ERROR_FLASH_OPERATION_FAILED;\r
+\r
+       DEBUG("protect: first_bank: %i, last_bank: %i", first, last);\r
+       \r
+       /* last bank: 0xFF signals a full device protect */\r
+       if (last == 0xFF)\r
+       {\r
+               if( set )\r
+               {\r
+                       status = str9xpec_lock_device(bank);\r
+               }\r
+               else\r
+               {\r
+                       /* perform full erase to unlock device */\r
+                       status = str9xpec_unlock_device(bank);\r
+               }\r
+       }\r
+       else\r
+       {       \r
+               for (i = first; i <= last; i++)\r
+               {\r
+                       if( set )\r
+                               buf_set_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1, 1);\r
+                       else\r
+                               buf_set_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1, 0);\r
+               }\r
+               \r
+               status = str9xpec_write_options(bank);\r
+       }\r
+       \r
+       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)\r
+               return ERROR_FLASH_OPERATION_FAILED;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_set_address(struct flash_bank_s *bank, u8 sector)\r
+{\r
+       u32 chain_pos;\r
+       scan_field_t field;\r
+       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;\r
+       \r
+       chain_pos = str9xpec_info->chain_pos;\r
+       \r
+       /* set flash controller address */\r
+       str9xpec_set_instr(chain_pos, ISC_ADDRESS_SHIFT, TAP_PI);\r
+       \r
+       field.device = chain_pos;\r
+       field.num_bits = 8;\r
+       field.out_value = &sector;\r
+       field.out_mask = NULL;\r
+       field.in_value = NULL;\r
+       field.in_check_value = NULL;\r
+       field.in_check_mask = NULL;\r
+       field.in_handler = NULL;\r
+       field.in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(1, &field, -1);\r
+               \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)\r
+{\r
+       str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;\r
+       u32 dwords_remaining = (count / 8);\r
+       u32 bytes_remaining = (count & 0x00000007);\r
+       u32 bytes_written = 0;\r
+       u8 status;\r
+       u32 check_address = offset;\r
+       u32 chain_pos;\r
+       scan_field_t field;\r
+       u8 *scanbuf;\r
+       int i;\r
+       u32 first_sector = 0;\r
+       u32 last_sector = 0;\r
+       \r
+       chain_pos = str9xpec_info->chain_pos;\r
+       \r
+       if (!str9xpec_info->isc_enable) {\r
+               str9xpec_isc_enable(bank);\r
+       }\r
+       \r
+       if (!str9xpec_info->isc_enable) {\r
+               return ERROR_FLASH_OPERATION_FAILED;\r
+       }\r
+       \r
+       if (offset & 0x7)\r
+       {\r
+               WARNING("offset 0x%x breaks required 8-byte alignment", offset);\r
+               return ERROR_FLASH_DST_BREAKS_ALIGNMENT;\r
+       }\r
+       \r
+       for (i = 0; i < bank->num_sectors; i++)\r
+       {\r
+               u32 sec_start = bank->sectors[i].offset;\r
+               u32 sec_end = sec_start + bank->sectors[i].size;\r
+               \r
+               /* check if destination falls within the current sector */\r
+               if ((check_address >= sec_start) && (check_address < sec_end))\r
+               {\r
+                       /* check if destination ends in the current sector */\r
+                       if (offset + count < sec_end)\r
+                               check_address = offset + count;\r
+                       else\r
+                               check_address = sec_end;\r
+               }\r
+               \r
+               if ((offset >= sec_start) && (offset < sec_end)){\r
+                       first_sector = i;\r
+               }\r
+               \r
+               if ((offset + count >= sec_start) && (offset + count < sec_end)){\r
+                       last_sector = i;\r
+               }\r
+       }\r
+       \r
+       if (check_address != offset + count)\r
+               return ERROR_FLASH_DST_OUT_OF_BANK;\r
+\r
+       DEBUG("first_sector: %i, last_sector: %i", first_sector, last_sector);\r
+       \r
+       scanbuf = calloc(CEIL(64, 8), 1);\r
+       \r
+       DEBUG("ISC_PROGRAM");\r
+       \r
+       for (i = first_sector; i <= last_sector; i++)\r
+       {\r
+               str9xpec_set_address(bank, str9xpec_info->sector_bits[i]);\r
+               \r
+               dwords_remaining = dwords_remaining < (bank->sectors[i].size/8) ? dwords_remaining : (bank->sectors[i].size/8);\r
+\r
+               while (dwords_remaining > 0)\r
+               {       \r
+                       str9xpec_set_instr(chain_pos, ISC_PROGRAM, TAP_PI);\r
+                       \r
+                       field.device = chain_pos;\r
+                       field.num_bits = 64;\r
+                       field.out_value = (buffer + bytes_written);\r
+                       field.out_mask = NULL;\r
+                       field.in_value = NULL;\r
+                       field.in_check_value = NULL;\r
+                       field.in_check_mask = NULL;\r
+                       field.in_handler = NULL;\r
+                       field.in_handler_priv = NULL;\r
+                       \r
+                       jtag_add_dr_scan(1, &field, TAP_RTI);\r
+                       \r
+                       /* small delay before polling */\r
+                       jtag_add_sleep(50);\r
+                       \r
+                       str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);\r
+                       \r
+                       do {\r
+                               field.device = chain_pos;\r
+                               field.num_bits = 8;\r
+                               field.out_value = NULL;\r
+                               field.out_mask = NULL;\r
+                               field.in_value = scanbuf;\r
+                               field.in_check_value = NULL;\r
+                               field.in_check_mask = NULL;\r
+                               field.in_handler = NULL;\r
+                               field.in_handler_priv = NULL;\r
+                               \r
+                               jtag_add_dr_scan(1, &field, -1);\r
+                               jtag_execute_queue();\r
+                               \r
+                               status = buf_get_u32(scanbuf, 0, 8);\r
+                               \r
+                       } while(!(status & ISC_STATUS_BUSY));\r
+                       \r
+                       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)\r
+                               return ERROR_FLASH_OPERATION_FAILED;\r
+                       \r
+                       //if ((status & ISC_STATUS_INT_ERROR) != STR9XPEC_ISC_INTFAIL)\r
+                       //      return ERROR_FLASH_OPERATION_FAILED;\r
+               \r
+                       dwords_remaining--;\r
+                       bytes_written += 8;\r
+               }\r
+       }\r
+       \r
+       if (bytes_remaining)\r
+       {\r
+               u8 last_dword[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};\r
+               int i = 0;\r
+                               \r
+               while(bytes_remaining > 0)\r
+               {\r
+                       last_dword[i++] = *(buffer + bytes_written); \r
+                       bytes_remaining--;\r
+                       bytes_written++;\r
+               }\r
+               \r
+               str9xpec_set_instr(chain_pos, ISC_PROGRAM, TAP_PI);\r
+               \r
+               field.device = chain_pos;\r
+               field.num_bits = 64;\r
+               field.out_value = last_dword;\r
+               field.out_mask = NULL;\r
+               field.in_value = NULL;\r
+               field.in_check_value = NULL;\r
+               field.in_check_mask = NULL;\r
+               field.in_handler = NULL;\r
+               field.in_handler_priv = NULL;\r
+               \r
+               jtag_add_dr_scan(1, &field, TAP_RTI);\r
+               \r
+               /* small delay before polling */\r
+               jtag_add_sleep(50);\r
+               \r
+               str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);\r
+               \r
+               do {\r
+                       field.device = chain_pos;\r
+                       field.num_bits = 8;\r
+                       field.out_value = NULL;\r
+                       field.out_mask = NULL;\r
+                       field.in_value = scanbuf;\r
+                       field.in_check_value = NULL;\r
+                       field.in_check_mask = NULL;\r
+                       field.in_handler = NULL;\r
+                       field.in_handler_priv = NULL;\r
+                       \r
+                       jtag_add_dr_scan(1, &field, -1);\r
+                       jtag_execute_queue();\r
+                       \r
+                       status = buf_get_u32(scanbuf, 0, 8);\r
+                       \r
+               } while(!(status & ISC_STATUS_BUSY));\r
+               \r
+               if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)\r
+                       return ERROR_FLASH_OPERATION_FAILED;\r
+               \r
+               //if ((status & ISC_STATUS_INT_ERROR) != STR9XPEC_ISC_INTFAIL)\r
+               //      return ERROR_FLASH_OPERATION_FAILED;\r
+       }\r
+\r
+       free(scanbuf);\r
+\r
+       str9xpec_isc_disable(bank);\r
+                               \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_probe(struct flash_bank_s *bank)\r
+{\r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       flash_bank_t *bank;\r
+       scan_field_t field;\r
+       u8 *buffer = NULL;\r
+       u32 chain_pos;\r
+       u32 idcode;\r
+       str9xpec_flash_controller_t *str9xpec_info = NULL;\r
+\r
+       if (argc < 1)\r
+       {\r
+               return ERROR_COMMAND_SYNTAX_ERROR;\r
+       }\r
+       \r
+       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));\r
+       if (!bank)\r
+       {\r
+               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       str9xpec_info = bank->driver_priv;\r
+       chain_pos = str9xpec_info->chain_pos;\r
+       \r
+       buffer = calloc(CEIL(32, 8), 1);\r
+       \r
+       str9xpec_set_instr(chain_pos, ISC_IDCODE, TAP_PI);\r
+       \r
+       field.device = chain_pos;\r
+       field.num_bits = 32;\r
+       field.out_value = NULL;\r
+       field.out_mask = NULL;\r
+       field.in_value = buffer;\r
+       field.in_check_value = NULL;\r
+       field.in_check_mask = NULL;\r
+       field.in_handler = NULL;\r
+       field.in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(1, &field, TAP_RTI);\r
+       jtag_execute_queue();\r
+       \r
+       idcode = buf_get_u32(buffer, 0, 32);\r
+       \r
+       command_print(cmd_ctx, "str9xpec part id: 0x%8.8x", idcode);\r
+       \r
+       free(buffer);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_erase_check(struct flash_bank_s *bank)\r
+{\r
+       return str9xpec_blank_check(bank, 0, bank->num_sectors - 1);\r
+}\r
+\r
+int str9xpec_info(struct flash_bank_s *bank, char *buf, int buf_size)\r
+{\r
+       snprintf(buf, buf_size, "str9xpec flash driver info" );\r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_handle_flash_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       flash_bank_t *bank;\r
+       u8 status;\r
+       str9xpec_flash_controller_t *str9xpec_info = NULL;\r
+       \r
+       if (argc < 1)\r
+       {\r
+               command_print(cmd_ctx, "str9xpec options_read <bank>");\r
+               return ERROR_OK;        \r
+       }\r
+       \r
+       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));\r
+       if (!bank)\r
+       {\r
+               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       str9xpec_info = bank->driver_priv;\r
+       \r
+       status = str9xpec_read_config(bank);\r
+       \r
+       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)\r
+               return ERROR_FLASH_OPERATION_FAILED;\r
+       \r
+       /* boot bank */\r
+       if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1))\r
+               command_print(cmd_ctx, "CS Map: bank1");\r
+       else\r
+               command_print(cmd_ctx, "CS Map: bank0");\r
+       \r
+       /* OTP lock */\r
+       if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_OTPBIT, 1))\r
+               command_print(cmd_ctx, "OTP Lock: OTP Locked");\r
+       else\r
+               command_print(cmd_ctx, "OTP Lock: OTP Unlocked");\r
+       \r
+       /* LVD Threshold */\r
+       if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1))\r
+               command_print(cmd_ctx, "LVD Threshold: 2.7v");\r
+       else\r
+               command_print(cmd_ctx, "LVD Threshold: 2.4v");\r
+       \r
+       /* LVD reset warning */\r
+       if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1))\r
+               command_print(cmd_ctx, "LVD Reset Warning: VDD or VDDQ Inputs");\r
+       else\r
+               command_print(cmd_ctx, "LVD Reset Warning: VDD Input Only");\r
+       \r
+       /* LVD reset select */\r
+       if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1))\r
+               command_print(cmd_ctx, "LVD Reset Selection: VDD or VDDQ Inputs");\r
+       else\r
+               command_print(cmd_ctx, "LVD Reset Selection: VDD Input Only");\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_write_options(struct flash_bank_s *bank)\r
+{\r
+       scan_field_t field;\r
+       u8 status;\r
+       u32 chain_pos;\r
+       str9xpec_flash_controller_t *str9xpec_info = NULL;\r
+       \r
+       str9xpec_info = bank->driver_priv;\r
+       chain_pos = str9xpec_info->chain_pos;\r
+       \r
+       /* erase config options first */\r
+       status = str9xpec_erase_area( bank, 0xFE, 0xFE );\r
+       \r
+       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)\r
+               return status; \r
+       \r
+       if (!str9xpec_info->isc_enable) {\r
+               str9xpec_isc_enable( bank );\r
+       }\r
+       \r
+       if (!str9xpec_info->isc_enable) {\r
+               return ISC_STATUS_ERROR;\r
+       }\r
+       \r
+       /* according to data 64th bit has to be set */\r
+       buf_set_u32(str9xpec_info->options, 63, 1, 1);\r
+       \r
+       /* set option byte address */\r
+       str9xpec_set_address(bank, 0x50);\r
+       \r
+       /* execute ISC_PROGRAM command */\r
+       str9xpec_set_instr(chain_pos, ISC_PROGRAM, TAP_PI);\r
+               \r
+       field.device = chain_pos;\r
+       field.num_bits = 64;\r
+       field.out_value = str9xpec_info->options;\r
+       field.out_mask = NULL;\r
+       field.in_value = NULL;\r
+       field.in_check_value = NULL;\r
+       field.in_check_mask = NULL;\r
+       field.in_handler = NULL;\r
+       field.in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(1, &field, TAP_RTI);\r
+       \r
+       /* small delay before polling */\r
+       jtag_add_sleep(50);\r
+       \r
+       str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);\r
+       \r
+       do {\r
+               field.device = chain_pos;\r
+               field.num_bits = 8;\r
+               field.out_value = NULL;\r
+               field.out_mask = NULL;\r
+               field.in_value = &status;\r
+               field.in_check_value = NULL;\r
+               field.in_check_mask = NULL;\r
+               field.in_handler = NULL;\r
+               field.in_handler_priv = NULL;\r
+               \r
+               jtag_add_dr_scan(1, &field, -1);\r
+               jtag_execute_queue();\r
+               \r
+       } while(!(status & ISC_STATUS_BUSY));\r
+       \r
+       str9xpec_isc_disable(bank);\r
+       \r
+       return status;\r
+}\r
+\r
+int str9xpec_handle_flash_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       flash_bank_t *bank;\r
+       u8 status;\r
+       \r
+       if (argc < 1)\r
+       {\r
+               command_print(cmd_ctx, "str9xpec options_write <bank>");\r
+               return ERROR_OK;        \r
+       }\r
+       \r
+       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));\r
+       if (!bank)\r
+       {\r
+               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       status = str9xpec_write_options(bank);\r
+       \r
+       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)\r
+               return ERROR_FLASH_OPERATION_FAILED;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_handle_flash_options_cmap_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       flash_bank_t *bank;\r
+       str9xpec_flash_controller_t *str9xpec_info = NULL;\r
+       \r
+       if (argc < 2)\r
+       {\r
+               command_print(cmd_ctx, "str9xpec options_cmap <bank> <bank0|bank1>");\r
+               return ERROR_OK;        \r
+       }\r
+       \r
+       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));\r
+       if (!bank)\r
+       {\r
+               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       str9xpec_info = bank->driver_priv;\r
+       \r
+       if (strcmp(args[1], "bank1") == 0)\r
+       {\r
+               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 1);\r
+       }\r
+       else\r
+       {\r
+               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 0);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_handle_flash_options_lvdthd_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       flash_bank_t *bank;\r
+       str9xpec_flash_controller_t *str9xpec_info = NULL;\r
+       \r
+       if (argc < 2)\r
+       {\r
+               command_print(cmd_ctx, "str9xpec options_lvdthd <bank> <2.4v|2.7v>");\r
+               return ERROR_OK;        \r
+       }\r
+       \r
+       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));\r
+       if (!bank)\r
+       {\r
+               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       str9xpec_info = bank->driver_priv;\r
+       \r
+       if (strcmp(args[1], "2.7v") == 0)\r
+       {\r
+               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 1);\r
+       }\r
+       else\r
+       {\r
+               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 0);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_handle_flash_options_lvdsel_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       flash_bank_t *bank;\r
+       str9xpec_flash_controller_t *str9xpec_info = NULL;\r
+       \r
+       if (argc < 2)\r
+       {\r
+               command_print(cmd_ctx, "str9xpec options_lvdsel <bank> <vdd|vdd_vddq>");\r
+               return ERROR_OK;        \r
+       }\r
+       \r
+       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));\r
+       if (!bank)\r
+       {\r
+               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       str9xpec_info = bank->driver_priv;\r
+       \r
+       if (strcmp(args[1], "vdd_vddq") == 0)\r
+       {\r
+               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 1);\r
+       }\r
+       else\r
+       {\r
+               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 0);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_handle_flash_options_lvdwarn_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       flash_bank_t *bank;\r
+       str9xpec_flash_controller_t *str9xpec_info = NULL;\r
+       \r
+       if (argc < 2)\r
+       {\r
+               command_print(cmd_ctx, "str9xpec options_lvdwarn <bank> <vdd|vdd_vddq>");\r
+               return ERROR_OK;        \r
+       }\r
+       \r
+       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));\r
+       if (!bank)\r
+       {\r
+               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       str9xpec_info = bank->driver_priv;\r
+       \r
+       if (strcmp(args[1], "vdd_vddq") == 0)\r
+       {\r
+               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 1);\r
+       }\r
+       else\r
+       {\r
+               buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 0);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_handle_flash_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       u8 status;\r
+       flash_bank_t *bank;\r
+       \r
+       if (argc < 1)\r
+       {\r
+               command_print(cmd_ctx, "str9xpec lock <bank>");\r
+               return ERROR_OK;        \r
+       }\r
+       \r
+       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));\r
+       if (!bank)\r
+       {\r
+               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       status = str9xpec_lock_device(bank);\r
+       \r
+       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)\r
+               return ERROR_FLASH_OPERATION_FAILED;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_handle_flash_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       u8 status;\r
+       flash_bank_t *bank;\r
+       \r
+       if (argc < 1)\r
+       {\r
+               command_print(cmd_ctx, "str9xpec unlock <bank>");\r
+               return ERROR_OK;        \r
+       }\r
+       \r
+       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));\r
+       if (!bank)\r
+       {\r
+               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       status = str9xpec_unlock_device(bank);\r
+       \r
+       if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)\r
+               return ERROR_FLASH_OPERATION_FAILED;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_handle_flash_enable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       flash_bank_t *bank;\r
+       u32 chain_pos;\r
+       jtag_device_t* dev0;\r
+       jtag_device_t* dev2;\r
+       str9xpec_flash_controller_t *str9xpec_info = NULL;\r
+       \r
+       if (argc < 1)\r
+       {\r
+               command_print(cmd_ctx, "str9xpec enable_turbo <bank>");\r
+               return ERROR_OK;        \r
+       }\r
+       \r
+       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));\r
+       if (!bank)\r
+       {\r
+               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       str9xpec_info = bank->driver_priv;\r
+       \r
+       chain_pos = str9xpec_info->chain_pos;\r
+       \r
+       /* remove arm core from chain - enter turbo mode */\r
+       \r
+       str9xpec_set_instr(chain_pos+2, 0xD, TAP_RTI);\r
+       jtag_execute_queue();\r
+       \r
+       /* modify scan chain - str9 core has been removed */\r
+       dev0 = jtag_get_device(chain_pos);\r
+       str9xpec_info->devarm = jtag_get_device(chain_pos+1);\r
+       dev2 = jtag_get_device(chain_pos+2);\r
+       dev0->next = dev2;\r
+       jtag_num_devices--;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       flash_bank_t *bank;\r
+       u32 chain_pos;\r
+       jtag_device_t* dev0;\r
+       str9xpec_flash_controller_t *str9xpec_info = NULL;\r
+       \r
+       if (argc < 1)\r
+       {\r
+               command_print(cmd_ctx, "str9xpec disable_turbo <bank>");\r
+               return ERROR_OK;        \r
+       }\r
+       \r
+       bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));\r
+       if (!bank)\r
+       {\r
+               command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       str9xpec_info = bank->driver_priv;\r
+       \r
+       chain_pos = str9xpec_info->chain_pos;\r
+       \r
+       dev0 = jtag_get_device(chain_pos);\r
+       \r
+       /* exit turbo mode via TLR */\r
+       str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_TLR);\r
+       jtag_execute_queue();\r
+       \r
+       /* restore previous scan chain */\r
+       if( str9xpec_info->devarm ) {\r
+               dev0->next = str9xpec_info->devarm;\r
+               jtag_num_devices++;\r
+               str9xpec_info->devarm = NULL;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
index c4015118ca96f2543d578b790a56993518eee24e..605c34cf56c90905f3dfe48f7ff2c57550e27415 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2005 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "replacements.h"
-
-#include "jtag.h"
-
-#include "command.h"
-#include "log.h"
-#include "interpreter.h"
-
-#include "stdlib.h"
-#include "string.h"
-#include <unistd.h>
-
-char* tap_state_strings[16] =
-{
-       "tlr", 
-       "sds", "cd", "sd", "e1d", "pd", "e2d", "ud",
-       "rti",
-       "sis", "ci", "si", "e1i", "pi", "e2i", "ui"
-};
-
-typedef struct cmd_queue_page_s
-{
-       void *address;
-       size_t used;
-       struct cmd_queue_page_s *next;
-} cmd_queue_page_t;
-
-#define CMD_QUEUE_PAGE_SIZE (1024 * 1024)
-static cmd_queue_page_t *cmd_queue_pages = NULL;
-
-/* tap_move[i][j]: tap movement command to go from state i to state j
- * 0: Test-Logic-Reset
- * 1: Run-Test/Idle
- * 2: Shift-DR
- * 3: Pause-DR
- * 4: Shift-IR
- * 5: Pause-IR
- * 
- * SD->SD and SI->SI have to be caught in interface specific code
- */
-u8 tap_move[6][6] =
-{
-/*       TLR   RTI   SD    PD    SI    PI             */
-       {0x7f, 0x00, 0x17, 0x0a, 0x1b, 0x16},   /* TLR */
-       {0x7f, 0x00, 0x25, 0x05, 0x2b, 0x0b},   /* RTI */
-       {0x7f, 0x31, 0x00, 0x01, 0x0f, 0x2f},   /* SD  */
-       {0x7f, 0x30, 0x20, 0x17, 0x1e, 0x2f},   /* PD  */
-       {0x7f, 0x31, 0x07, 0x17, 0x00, 0x01},   /* SI  */
-       {0x7f, 0x30, 0x1c, 0x17, 0x20, 0x2f}    /* PI  */
-};
-
-int tap_move_map[16] = {
-       0, -1, -1,  2, -1,  3, -1, -1,
-       1, -1, -1,  4, -1,  5, -1, -1
-};
-
-tap_transition_t tap_transitions[16] =
-{
-       {TAP_TLR, TAP_RTI},             /* TLR */
-       {TAP_SIS, TAP_CD},              /* SDS */
-       {TAP_E1D, TAP_SD},              /* CD  */
-       {TAP_E1D, TAP_SD},              /* SD  */
-       {TAP_UD,  TAP_PD},              /* E1D */
-       {TAP_E2D, TAP_PD},              /* PD  */
-       {TAP_UD,  TAP_SD},              /* E2D */
-       {TAP_SDS, TAP_RTI},             /* UD  */
-       {TAP_SDS, TAP_RTI},             /* RTI */
-       {TAP_TLR, TAP_CI},              /* SIS */
-       {TAP_E1I, TAP_SI},              /* CI  */
-       {TAP_E1I, TAP_SI},              /* SI  */
-       {TAP_UI,  TAP_PI},              /* E1I */
-       {TAP_E2I, TAP_PI},              /* PI  */
-       {TAP_UI,  TAP_SI},              /* E2I */
-       {TAP_SDS, TAP_RTI}              /* UI  */
-};
-
-char* jtag_event_strings[] =
-{
-       "SRST asserted",
-       "TRST asserted",
-       "SRST released",
-       "TRST released"
-};
-
-enum tap_state end_state = TAP_TLR;
-enum tap_state cur_state = TAP_TLR;
-int jtag_trst = 0;
-int jtag_srst = 0;
-
-jtag_command_t *jtag_command_queue = NULL;
-jtag_command_t **last_comand_pointer = &jtag_command_queue;
-jtag_device_t *jtag_devices = NULL;
-int jtag_num_devices = 0;
-int jtag_ir_scan_size = 0;
-enum reset_types jtag_reset_config = RESET_NONE;
-enum tap_state cmd_queue_end_state = TAP_TLR;
-enum tap_state cmd_queue_cur_state = TAP_TLR;
-
-int jtag_verify_capture_ir = 1;
-
-/* how long the OpenOCD should wait before attempting JTAG communication after reset lines deasserted (in ms) */
-int jtag_nsrst_delay = 0; /* default to no nSRST delay */
-int jtag_ntrst_delay = 0; /* default to no nTRST delay */ 
-
-/* maximum number of JTAG devices expected in the chain
- */
-#define JTAG_MAX_CHAIN_SIZE 20 
-
-/* callbacks to inform high-level handlers about JTAG state changes */
-jtag_event_callback_t *jtag_event_callbacks;
-
-/* jtag interfaces (parport, FTDI-USB, TI-USB, ...)
- */
-#if BUILD_PARPORT == 1
-       extern jtag_interface_t parport_interface;
-#endif
-
-#if BUILD_FT2232_FTD2XX == 1
-       extern jtag_interface_t ft2232_interface;
-#endif
-
-#if BUILD_FT2232_LIBFTDI == 1
-       extern jtag_interface_t ft2232_interface;
-#endif
-
-#if BUILD_AMTJTAGACCEL == 1
-       extern jtag_interface_t amt_jtagaccel_interface;
-#endif
-
-#if BUILD_EP93XX == 1
-       extern jtag_interface_t ep93xx_interface;
-#endif
-
-#if BUILD_AT91RM9200 == 1
-       extern jtag_interface_t at91rm9200_interface;
-#endif
-
-#if BUILD_GW16012 == 1
-       extern jtag_interface_t gw16012_interface;
-#endif
-
-#if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1
-       extern jtag_interface_t presto_interface;
-#endif
-
-#if BUILD_USBPROG == 1
-       extern jtag_interface_t usbprog_interface;
-#endif
-
-jtag_interface_t *jtag_interfaces[] = {
-#if BUILD_PARPORT == 1
-       &parport_interface,
-#endif
-#if BUILD_FT2232_FTD2XX == 1
-       &ft2232_interface,
-#endif
-#if BUILD_FT2232_LIBFTDI == 1
-       &ft2232_interface,
-#endif
-#if BUILD_AMTJTAGACCEL == 1
-       &amt_jtagaccel_interface,
-#endif
-#if BUILD_EP93XX == 1
-       &ep93xx_interface,
-#endif
-#if BUILD_AT91RM9200 == 1
-       &at91rm9200_interface,
-#endif
-#if BUILD_GW16012 == 1
-       &gw16012_interface,
-#endif
-#if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1
-       &presto_interface,
-#endif
-#if BUILD_USBPROG == 1
-       &usbprog_interface,
-#endif
-       NULL,
-};
-
-jtag_interface_t *jtag = NULL;
-
-/* configuration */
-char* jtag_interface = NULL;
-int jtag_speed = -1;
-
-
-/* forward declarations */
-int jtag_add_statemove(enum tap_state endstate);
-int jtag_add_pathmove(int num_states, enum tap_state *path);
-int jtag_add_runtest(int num_cycles, enum tap_state endstate);
-int jtag_add_reset(int trst, int srst);
-int jtag_add_end_state(enum tap_state endstate);
-int jtag_add_sleep(u32 us);
-int jtag_execute_queue(void);
-int jtag_cancel_queue(void);
-
-/* jtag commands */
-int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_statemove_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_drscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-int jtag_register_event_callback(int (*callback)(enum jtag_event event, void *priv), void *priv)
-{
-       jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;
-       
-       if (callback == NULL)
-       {
-               return ERROR_INVALID_ARGUMENTS;
-       }
-       
-       if (*callbacks_p)
-       {
-               while ((*callbacks_p)->next)
-                       callbacks_p = &((*callbacks_p)->next);
-               callbacks_p = &((*callbacks_p)->next);
-       }
-       
-       (*callbacks_p) = malloc(sizeof(jtag_event_callback_t));
-       (*callbacks_p)->callback = callback;
-       (*callbacks_p)->priv = priv;
-       (*callbacks_p)->next = NULL;
-       
-       return ERROR_OK;
-}
-
-int jtag_unregister_event_callback(int (*callback)(enum jtag_event event, void *priv))
-{
-       jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;
-       
-       if (callback == NULL)
-       {
-               return ERROR_INVALID_ARGUMENTS;
-       }
-               
-       while (*callbacks_p)
-       {
-               jtag_event_callback_t **next = &((*callbacks_p)->next);
-               if ((*callbacks_p)->callback == callback)
-               {
-                       free(*callbacks_p);
-                       *callbacks_p = *next;
-               }
-               callbacks_p = next;
-       }
-       
-       return ERROR_OK;
-}
-
-int jtag_call_event_callbacks(enum jtag_event event)
-{
-       jtag_event_callback_t *callback = jtag_event_callbacks;
-       
-       DEBUG("jtag event: %s", jtag_event_strings[event]);
-       
-       while (callback)
-       {
-               callback->callback(event, callback->priv);
-               callback = callback->next;
-       }
-       
-       return ERROR_OK;
-}
-
-/* returns a pointer to the pointer of the last command in queue
- * this may be a pointer to the root pointer (jtag_command_queue)
- * or to the next member of the last but one command
- */
-jtag_command_t** jtag_get_last_command_p(void)
-{
-/*     jtag_command_t *cmd = jtag_command_queue;
-       
-       if (cmd)
-               while (cmd->next)
-                       cmd = cmd->next;
-       else
-               return &jtag_command_queue;
-       
-       return &cmd->next;*/
-       
-       return last_comand_pointer;
-}
-
-/* returns a pointer to the n-th device in the scan chain */
-jtag_device_t* jtag_get_device(int num)
-{
-       jtag_device_t *device = jtag_devices;
-       int i = 0;
-
-       while (device)
-       {
-               if (num == i)
-                       return device;
-               device = device->next;
-               i++;
-       }
-       
-       ERROR("jtag device number %d not defined", num);
-       exit(-1);
-}
-
-void* cmd_queue_alloc(size_t size)
-{
-       cmd_queue_page_t **p_page = &cmd_queue_pages;
-       int offset;
-
-       if (*p_page)
-       {
-               while ((*p_page)->next)
-                       p_page = &((*p_page)->next);
-               if (CMD_QUEUE_PAGE_SIZE - (*p_page)->used < size)
-                       p_page = &((*p_page)->next);
-       }
-
-       if (!*p_page)
-       {
-               *p_page = malloc(sizeof(cmd_queue_page_t));
-               (*p_page)->used = 0;
-               (*p_page)->address = malloc(CMD_QUEUE_PAGE_SIZE);
-               (*p_page)->next = NULL;
-       }
-
-       offset = (*p_page)->used;
-       (*p_page)->used += size;
-       
-       u8 *t=(u8 *)((*p_page)->address);
-       return t + offset;
-}
-
-void cmd_queue_free()
-{
-       cmd_queue_page_t *page = cmd_queue_pages;
-
-       while (page)
-       {
-               cmd_queue_page_t *last = page;
-               free(page->address);
-               page = page->next;
-               free(last);
-       }
-
-       cmd_queue_pages = NULL;
-}
-
-int jtag_add_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state, void *dummy_anachronism)
-{
-       jtag_command_t **last_cmd;
-       jtag_device_t *device;
-       int i, j;
-       int scan_size = 0;
-
-       if (jtag_trst == 1)
-       {
-               WARNING("JTAG command queued, while TRST is low (TAP in reset)");
-               return ERROR_JTAG_TRST_ASSERTED;
-       }
-
-       last_cmd = jtag_get_last_command_p();
-       
-       /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       (*last_cmd)->next = NULL;
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->type = JTAG_SCAN;
-
-       /* allocate memory for ir scan command */
-       (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
-       (*last_cmd)->cmd.scan->ir_scan = 1;
-       (*last_cmd)->cmd.scan->num_fields = jtag_num_devices;   /* one field per device */
-       (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(jtag_num_devices * sizeof(scan_field_t));
-       (*last_cmd)->cmd.scan->end_state = state;
-               
-       if (state != -1)
-               cmd_queue_end_state = state;
-
-       if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
-               jtag_call_event_callbacks(JTAG_TRST_RELEASED);
-       
-       if (cmd_queue_end_state == TAP_TLR)
-               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
-       
-       cmd_queue_cur_state = cmd_queue_end_state;
-               
-       for (i = 0; i < jtag_num_devices; i++)
-       {
-               int found = 0;
-               device = jtag_get_device(i);
-               scan_size = device->ir_length;
-               (*last_cmd)->cmd.scan->fields[i].device = i;
-               (*last_cmd)->cmd.scan->fields[i].num_bits = scan_size;
-               (*last_cmd)->cmd.scan->fields[i].in_value = NULL;
-               (*last_cmd)->cmd.scan->fields[i].in_handler = NULL;     /* disable verification by default */
-
-               /* search the list */
-               for (j = 0; j < num_fields; j++)
-               {
-                       if (i == fields[j].device)
-                       {
-                               found = 1;
-                               (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
-                               (*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
-                       
-                               if (jtag_verify_capture_ir)
-                               {
-                                       if (fields[j].in_handler==NULL)
-                                       {
-                                               jtag_set_check_value((*last_cmd)->cmd.scan->fields+i, device->expected, device->expected_mask, NULL);
-                                       } else
-                                       {
-                                               (*last_cmd)->cmd.scan->fields[i].in_handler = fields[j].in_handler;
-                                               (*last_cmd)->cmd.scan->fields[i].in_handler_priv = fields[j].in_handler_priv;
-                                               (*last_cmd)->cmd.scan->fields[i].in_check_value = device->expected; 
-                                               (*last_cmd)->cmd.scan->fields[i].in_check_mask = device->expected_mask;
-                                       }
-                               }
-                               
-                               device->bypass = 0;
-                               break;
-                       }
-               }
-       
-               if (!found)
-               {
-                       /* if a device isn't listed, set it to BYPASS */
-                       (*last_cmd)->cmd.scan->fields[i].out_value = buf_set_ones(cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
-                       (*last_cmd)->cmd.scan->fields[i].out_mask = NULL;
-                       device->bypass = 1;
-               
-               }
-               
-               /* update device information */
-               buf_cpy((*last_cmd)->cmd.scan->fields[i].out_value, jtag_get_device(i)->cur_instr, scan_size);
-       }
-       
-       return ERROR_OK;
-}
-
-int jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state, void *dummy_anachronism)
-{
-       jtag_command_t **last_cmd;
-       int i;
-
-       if (jtag_trst == 1)
-       {
-               WARNING("JTAG command queued, while TRST is low (TAP in reset)");
-               return ERROR_JTAG_TRST_ASSERTED;
-       }
-
-       last_cmd = jtag_get_last_command_p();
-       
-       /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       (*last_cmd)->next = NULL;
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->type = JTAG_SCAN;
-
-       /* allocate memory for ir scan command */
-       (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
-       (*last_cmd)->cmd.scan->ir_scan = 1;
-       (*last_cmd)->cmd.scan->num_fields = num_fields;
-       (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
-       (*last_cmd)->cmd.scan->end_state = state;
-
-       if (state != -1)
-               cmd_queue_end_state = state;
-
-       if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
-               jtag_call_event_callbacks(JTAG_TRST_RELEASED);
-       
-       if (cmd_queue_end_state == TAP_TLR)
-               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
-               
-       cmd_queue_cur_state = cmd_queue_end_state;
-       
-       for (i = 0; i < num_fields; i++)
-       {
-               int num_bits = fields[i].num_bits;
-               int num_bytes = CEIL(fields[i].num_bits, 8);
-               (*last_cmd)->cmd.scan->fields[i].device = fields[i].device;
-               (*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
-               (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
-               (*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);
-               (*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
-               (*last_cmd)->cmd.scan->fields[i].in_check_value = fields[i].in_check_value;
-               (*last_cmd)->cmd.scan->fields[i].in_check_mask = fields[i].in_check_mask;
-               (*last_cmd)->cmd.scan->fields[i].in_handler = NULL;
-               (*last_cmd)->cmd.scan->fields[i].in_handler_priv = NULL;
-       }
-       return ERROR_OK;
-}
-
-int jtag_add_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state, void *dummy_anachronism)
-{
-       int i, j;
-       int bypass_devices = 0;
-       int field_count = 0;
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
-       jtag_device_t *device = jtag_devices;
-       int scan_size;
-
-       if (jtag_trst == 1)
-       {
-               WARNING("JTAG command queued, while TRST is low (TAP in reset)");
-               return ERROR_JTAG_TRST_ASSERTED;
-       }
-
-       /* count devices in bypass */
-       while (device)
-       {
-               if (device->bypass)
-                       bypass_devices++;
-               device = device->next;
-       }
-       
-       /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->next = NULL;
-       (*last_cmd)->type = JTAG_SCAN;
-
-       /* allocate memory for dr scan command */
-       (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
-       (*last_cmd)->cmd.scan->ir_scan = 0;
-       (*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;
-       (*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));
-       (*last_cmd)->cmd.scan->end_state = state;
-       
-       if (state != -1)
-               cmd_queue_end_state = state;
-
-       if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
-               jtag_call_event_callbacks(JTAG_TRST_RELEASED);
-       
-       if (cmd_queue_end_state == TAP_TLR)
-               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
-                       
-       cmd_queue_cur_state = cmd_queue_end_state;
-       
-       for (i = 0; i < jtag_num_devices; i++)
-       {
-               int found = 0;
-               (*last_cmd)->cmd.scan->fields[field_count].device = i;
-       
-               for (j = 0; j < num_fields; j++)
-               {
-                       if (i == fields[j].device)
-                       {
-                               found = 1;
-                               scan_size = fields[j].num_bits;
-                               (*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;
-                               (*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
-                               (*last_cmd)->cmd.scan->fields[field_count].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);
-                               (*last_cmd)->cmd.scan->fields[field_count].in_value = fields[j].in_value;
-                               (*last_cmd)->cmd.scan->fields[field_count].in_check_value = fields[j].in_check_value;
-                               (*last_cmd)->cmd.scan->fields[field_count].in_check_mask = fields[j].in_check_mask;
-                               (*last_cmd)->cmd.scan->fields[field_count].in_handler = fields[j].in_handler;
-                               (*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = fields[j].in_handler_priv;
-                       }
-               }
-               if (!found)
-               {
-                       /* if a device isn't listed, the BYPASS register should be selected */
-                       if (!jtag_get_device(i)->bypass)
-                       {
-                               ERROR("BUG: no scan data for a device not in BYPASS");
-                               exit(-1);
-                       }
-       
-                       /* program the scan field to 1 bit length, and ignore it's value */
-                       (*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;
-                       (*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;
-                       (*last_cmd)->cmd.scan->fields[field_count].out_mask = NULL;
-                       (*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;
-                       (*last_cmd)->cmd.scan->fields[field_count].in_check_value = NULL;
-                       (*last_cmd)->cmd.scan->fields[field_count].in_check_mask = NULL;
-                       (*last_cmd)->cmd.scan->fields[field_count].in_handler = NULL;
-                       (*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = NULL;
-               }
-               else
-               {
-                       /* if a device is listed, the BYPASS register must not be selected */
-                       if (jtag_get_device(i)->bypass)
-                       {
-                               WARNING("scan data for a device in BYPASS");
-                       }
-               }
-       }
-       return ERROR_OK;
-}
-
-int jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state, void *dummy_anachronism)
-{
-       int i;
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
-       
-       if (jtag_trst == 1)
-       {
-               WARNING("JTAG command queued, while TRST is low (TAP in reset)");
-               return ERROR_JTAG_TRST_ASSERTED;
-       }
-
-       /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->next = NULL;
-       (*last_cmd)->type = JTAG_SCAN;
-
-       /* allocate memory for scan command */
-       (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));
-       (*last_cmd)->cmd.scan->ir_scan = 0;
-       (*last_cmd)->cmd.scan->num_fields = num_fields;
-       (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));
-       (*last_cmd)->cmd.scan->end_state = state;
-               
-       if (state != -1)
-               cmd_queue_end_state = state;
-
-       if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
-               jtag_call_event_callbacks(JTAG_TRST_RELEASED);
-       
-       if (cmd_queue_end_state == TAP_TLR)
-               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
-                       
-       cmd_queue_cur_state = cmd_queue_end_state;
-       
-       for (i = 0; i < num_fields; i++)
-       {
-               int num_bits = fields[i].num_bits;
-               int num_bytes = CEIL(fields[i].num_bits, 8);
-               (*last_cmd)->cmd.scan->fields[i].device = fields[i].device;
-               (*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;
-               (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);
-               (*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);
-               (*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;
-               (*last_cmd)->cmd.scan->fields[i].in_check_value = fields[i].in_check_value;
-               (*last_cmd)->cmd.scan->fields[i].in_check_mask = fields[i].in_check_mask;
-               (*last_cmd)->cmd.scan->fields[i].in_handler = fields[i].in_handler;
-               (*last_cmd)->cmd.scan->fields[i].in_handler_priv = fields[i].in_handler_priv;
-       }
-
-       return ERROR_OK;
-}
-int jtag_add_statemove(enum tap_state state)
-{
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
-       
-       if (jtag_trst == 1)
-       {
-               WARNING("JTAG command queued, while TRST is low (TAP in reset)");
-               return ERROR_JTAG_TRST_ASSERTED;
-       }
-
-       /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->next = NULL;
-       (*last_cmd)->type = JTAG_STATEMOVE;
-
-       (*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));
-       (*last_cmd)->cmd.statemove->end_state = state;
-       
-       if (state != -1)
-               cmd_queue_end_state = state;
-
-       if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
-               jtag_call_event_callbacks(JTAG_TRST_RELEASED);
-       
-       if (cmd_queue_end_state == TAP_TLR)
-               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
-                       
-       cmd_queue_cur_state = cmd_queue_end_state;
-       
-       return ERROR_OK;
-}
-
-int jtag_add_pathmove(int num_states, enum tap_state *path)
-{
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
-       int i;
-       
-       if (jtag_trst == 1)
-       {
-               WARNING("JTAG command queued, while TRST is low (TAP in reset)");
-               return ERROR_JTAG_TRST_ASSERTED;
-       }
-       
-       /* the last state has to be a stable state */
-       if (tap_move_map[path[num_states - 1]] == -1)
-       {
-               ERROR("TAP path doesn't finish in a stable state");
-               return ERROR_JTAG_NOT_IMPLEMENTED;
-       }
-       
-       /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->next = NULL;
-       (*last_cmd)->type = JTAG_PATHMOVE;
-
-       (*last_cmd)->cmd.pathmove = cmd_queue_alloc(sizeof(pathmove_command_t));
-       (*last_cmd)->cmd.pathmove->num_states = num_states;
-       (*last_cmd)->cmd.pathmove->path = cmd_queue_alloc(sizeof(enum tap_state) * num_states);
-       
-       for (i = 0; i < num_states; i++)
-               (*last_cmd)->cmd.pathmove->path[i] = path[i];
-
-       if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
-               jtag_call_event_callbacks(JTAG_TRST_RELEASED);
-       
-       if (cmd_queue_end_state == TAP_TLR)
-               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
-       
-       cmd_queue_cur_state = path[num_states - 1];
-       
-       return ERROR_OK;
-}
-
-int jtag_add_runtest(int num_cycles, enum tap_state state)
-{
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
-       
-       if (jtag_trst == 1)
-       {
-               WARNING("JTAG command queued, while TRST is low (TAP in reset)");
-               return ERROR_JTAG_TRST_ASSERTED;
-       }
-
-       /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       (*last_cmd)->next = NULL;
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->type = JTAG_RUNTEST;
-
-       (*last_cmd)->cmd.runtest = cmd_queue_alloc(sizeof(runtest_command_t));
-       (*last_cmd)->cmd.runtest->num_cycles = num_cycles;
-       (*last_cmd)->cmd.runtest->end_state = state;
-       
-       if (state != -1)
-               cmd_queue_end_state = state;
-
-       if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)
-               jtag_call_event_callbacks(JTAG_TRST_RELEASED);
-       
-       if (cmd_queue_end_state == TAP_TLR)
-               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
-                       
-       cmd_queue_cur_state = cmd_queue_end_state;
-       
-       return ERROR_OK;
-}
-
-int jtag_add_reset(int req_trst, int req_srst)
-{
-       int trst_with_tms = 0;
-       
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
-       
-       if (req_trst == -1)
-               req_trst = jtag_trst;
-       
-       if (req_srst == -1)
-               req_srst = jtag_srst;
-
-       /* Make sure that jtag_reset_config allows the requested reset */
-       /* if SRST pulls TRST, we can't fulfill srst == 1 with trst == 0 */
-       if (((jtag_reset_config & RESET_SRST_PULLS_TRST) && (req_srst == 1)) && (req_trst == 0))
-               return ERROR_JTAG_RESET_WOULD_ASSERT_TRST;
-               
-       /* if TRST pulls SRST, we reset with TAP T-L-R */
-       if (((jtag_reset_config & RESET_TRST_PULLS_SRST) && (req_trst == 1)) && (req_srst == 0))
-       {
-               req_trst = 0;
-               trst_with_tms = 1;
-       }
-       
-       if (req_srst && !(jtag_reset_config & RESET_HAS_SRST))
-       {
-               ERROR("requested nSRST assertion, but the current configuration doesn't support this");
-               return ERROR_JTAG_RESET_CANT_SRST;
-       }
-       
-       if (req_trst && !(jtag_reset_config & RESET_HAS_TRST))
-       {
-               req_trst = 0;
-               trst_with_tms = 1;
-       }
-       
-       /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       (*last_cmd)->next = NULL;
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->type = JTAG_RESET;
-
-       (*last_cmd)->cmd.reset = cmd_queue_alloc(sizeof(reset_command_t));
-       (*last_cmd)->cmd.reset->trst = req_trst;
-       (*last_cmd)->cmd.reset->srst = req_srst;
-
-       jtag_trst = req_trst;
-       jtag_srst = req_srst;
-
-       if (jtag_srst)
-       {
-               jtag_call_event_callbacks(JTAG_SRST_ASSERTED);
-       }
-       else
-       {
-               jtag_call_event_callbacks(JTAG_SRST_RELEASED);
-               if (jtag_nsrst_delay)
-                       jtag_add_sleep(jtag_nsrst_delay * 1000);
-       }
-       
-       if (trst_with_tms)
-       {
-               last_cmd = &((*last_cmd)->next);
-               
-               /* allocate memory for a new list member */
-               *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-               (*last_cmd)->next = NULL;
-               last_comand_pointer = &((*last_cmd)->next);
-               (*last_cmd)->type = JTAG_STATEMOVE;
-
-               (*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));
-               (*last_cmd)->cmd.statemove->end_state = TAP_TLR;
-               
-               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
-               cmd_queue_cur_state = TAP_TLR;
-               cmd_queue_end_state = TAP_TLR;
-               
-               return ERROR_OK;
-       }
-       else
-       {
-               if (jtag_trst)
-               {
-                       /* we just asserted nTRST, so we're now in Test-Logic-Reset,
-                        * and inform possible listeners about this
-                        */
-                       cmd_queue_cur_state = TAP_TLR;
-                       jtag_call_event_callbacks(JTAG_TRST_ASSERTED);
-               }
-               else
-               {
-                       /* the nTRST line got deasserted, so we're still in Test-Logic-Reset,
-                        * but we might want to add a delay to give the TAP time to settle
-                        */
-                       if (jtag_ntrst_delay)
-                               jtag_add_sleep(jtag_ntrst_delay * 1000);
-               }
-       }
-
-       return ERROR_OK;
-}
-
-int jtag_add_end_state(enum tap_state state)
-{
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
-       
-       /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       (*last_cmd)->next = NULL;
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->type = JTAG_END_STATE;
-
-       (*last_cmd)->cmd.end_state = cmd_queue_alloc(sizeof(end_state_command_t));
-       (*last_cmd)->cmd.end_state->end_state = state;
-
-       if (state != -1)
-               cmd_queue_end_state = state;
-       
-       return ERROR_OK;
-}
-
-int jtag_add_sleep(u32 us)
-{
-       jtag_command_t **last_cmd = jtag_get_last_command_p();
-       
-       /* allocate memory for a new list member */
-       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));
-       (*last_cmd)->next = NULL;
-       last_comand_pointer = &((*last_cmd)->next);
-       (*last_cmd)->type = JTAG_SLEEP;
-
-       (*last_cmd)->cmd.sleep = cmd_queue_alloc(sizeof(sleep_command_t));
-       (*last_cmd)->cmd.sleep->us = us;
-       
-       return ERROR_OK;
-}
-
-int jtag_scan_size(scan_command_t *cmd)
-{
-       int bit_count = 0;
-       int i;
-
-       /* count bits in scan command */
-       for (i = 0; i < cmd->num_fields; i++)
-       {
-               bit_count += cmd->fields[i].num_bits;
-       }
-
-       return bit_count;
-}
-
-int jtag_build_buffer(scan_command_t *cmd, u8 **buffer)
-{
-       int bit_count = 0;
-       int i;
-       
-       bit_count = jtag_scan_size(cmd);
-       *buffer = malloc(CEIL(bit_count, 8));
-       
-       bit_count = 0;
-
-       for (i = 0; i < cmd->num_fields; i++)
-       {
-               if (cmd->fields[i].out_value)
-               {
-#ifdef _DEBUG_JTAG_IO_
-                       char* char_buf = buf_to_str(cmd->fields[i].out_value, (cmd->fields[i].num_bits > 64) ? 64 : cmd->fields[i].num_bits, 16);
-#endif
-                       buf_set_buf(cmd->fields[i].out_value, 0, *buffer, bit_count, cmd->fields[i].num_bits);
-#ifdef _DEBUG_JTAG_IO_
-                       DEBUG("fields[%i].out_value: 0x%s", i, char_buf);
-                       free(char_buf);
-#endif
-               }
-               
-               bit_count += cmd->fields[i].num_bits;
-       }
-
-       return bit_count;
-
-}
-
-int jtag_read_buffer(u8 *buffer, scan_command_t *cmd)
-{
-       int i;
-       int bit_count = 0;
-       int retval;
-       
-       /* we return ERROR_OK, unless a check fails, or a handler reports a problem */
-       retval = ERROR_OK;
-       
-       for (i = 0; i < cmd->num_fields; i++)
-       {
-               /* if neither in_value nor in_handler
-                * are specified we don't have to examine this field
-                */
-               if (cmd->fields[i].in_value || cmd->fields[i].in_handler)
-               {
-                       int num_bits = cmd->fields[i].num_bits;
-                       u8 *captured = buf_set_buf(buffer, bit_count, malloc(CEIL(num_bits, 8)), 0, num_bits);
-                       
-#ifdef _DEBUG_JTAG_IO_
-                       char *char_buf;
-
-                       char_buf = buf_to_str(captured, (num_bits > 64) ? 64 : num_bits, 16);
-                       DEBUG("fields[%i].in_value: 0x%s", i, char_buf);
-                       free(char_buf);
-#endif
-                       
-                       if (cmd->fields[i].in_value)
-                       {
-                               buf_cpy(captured, cmd->fields[i].in_value, num_bits);
-                               
-                               if (cmd->fields[i].in_handler)
-                               {
-                                       if (cmd->fields[i].in_handler(cmd->fields[i].in_value, cmd->fields[i].in_handler_priv, cmd->fields+i) != ERROR_OK)
-                                       {
-                                               WARNING("in_handler reported a failed check");
-                                               retval = ERROR_JTAG_QUEUE_FAILED;
-                                       }
-                               }
-                       }
-                       
-                       /* no in_value specified, but a handler takes care of the scanned data */
-                       if (cmd->fields[i].in_handler && (!cmd->fields[i].in_value))
-                       {
-                               if (cmd->fields[i].in_handler(captured, cmd->fields[i].in_handler_priv, cmd->fields+i) != ERROR_OK)
-                               {
-                                       /* We're going to call the error:handler later, but if the in_handler
-                                        * reported an error we report this failure upstream
-                                        */
-                                       WARNING("in_handler reported a failed check");
-                                       retval = ERROR_JTAG_QUEUE_FAILED;
-                               }
-                       }
-
-                       free(captured);
-               }
-               bit_count += cmd->fields[i].num_bits;
-       }
-
-       return retval;
-}
-
-int jtag_check_value(u8 *captured, void *priv, scan_field_t *field)
-{
-       int retval = ERROR_OK;
-       int num_bits = field->num_bits;
-       
-       int compare_failed = 0;
-       
-       if (field->in_check_mask)
-               compare_failed = buf_cmp_mask(captured, field->in_check_value, field->in_check_mask, num_bits);
-       else
-               compare_failed = buf_cmp(captured, field->in_check_value, num_bits);
-       
-       if (compare_failed)
-               {
-               /* An error handler could have caught the failing check
-                * only report a problem when there wasn't a handler, or if the handler
-                * acknowledged the error
-                */ 
-               if (compare_failed)
-               {
-                       char *captured_char = buf_to_str(captured, (num_bits > 64) ? 64 : num_bits, 16);
-                       char *in_check_value_char = buf_to_str(field->in_check_value, (num_bits > 64) ? 64 : num_bits, 16);
-
-                       if (field->in_check_mask)
-                       {
-                               char *in_check_mask_char;
-                               in_check_mask_char = buf_to_str(field->in_check_mask, (num_bits > 64) ? 64 : num_bits, 16);
-                               WARNING("value captured during scan didn't pass the requested check: captured: 0x%s check_value: 0x%s check_mask: 0x%s", captured_char, in_check_value_char, in_check_mask_char);
-                               free(in_check_mask_char);
-                       }
-                       else
-                       {
-                               WARNING("value captured during scan didn't pass the requested check: captured: 0x%s check_value: 0x%s", captured_char, in_check_value_char);
-                       }
-
-                       free(captured_char);
-                       free(in_check_value_char);
-                       
-                       retval = ERROR_JTAG_QUEUE_FAILED;
-               }
-               
-       }
-       return retval;
-}
-
-/* 
-  set up checking of this field using the in_handler. The values passed in must be valid until
-  after jtag_execute() has completed.
- */
-void jtag_set_check_value(scan_field_t *field, u8 *value, u8 *mask, error_handler_t *in_error_handler)
-{
-       if (value)
-               field->in_handler = jtag_check_value;
-       else
-               field->in_handler = NULL;       /* No check, e.g. embeddedice uses value==NULL to indicate no check */
-       field->in_handler_priv = NULL;  /* this will be filled in at the invocation site to point to the field duplicate */ 
-       field->in_check_value = value;
-       field->in_check_mask = mask;
-}
-
-enum scan_type jtag_scan_type(scan_command_t *cmd)
-{
-       int i;
-       int type = 0;
-       
-       for (i = 0; i < cmd->num_fields; i++)
-       {
-               if (cmd->fields[i].in_value || cmd->fields[i].in_handler)
-                       type |= SCAN_IN;
-               if (cmd->fields[i].out_value)
-                       type |= SCAN_OUT;
-       }
-
-       return type;
-}
-
-int jtag_execute_queue(void)
-{
-       int retval;
-
-       retval = jtag->execute_queue();
-       
-       cmd_queue_free();
-
-       jtag_command_queue = NULL;
-       last_comand_pointer = &jtag_command_queue;
-
-       return retval;
-}
-
-int jtag_reset_callback(enum jtag_event event, void *priv)
-{
-       jtag_device_t *device = priv;
-
-       DEBUG("-");
-       
-       if (event == JTAG_TRST_ASSERTED)
-       {
-               buf_set_ones(device->cur_instr, device->ir_length);
-               device->bypass = 1;
-       }
-       
-       return ERROR_OK;
-}
-
-void jtag_sleep(u32 us)
-{
-       usleep(us);
-}
-
-/* Try to examine chain layout according to IEEE 1149.1 Â§12
- */
-int jtag_examine_chain()
-{
-       jtag_device_t *device = jtag_devices;
-       scan_field_t field;
-       u8 idcode_buffer[JTAG_MAX_CHAIN_SIZE * 4];
-       int i;
-       int bit_count;
-       int device_count = 0;
-       u8 zero_check = 0x0;
-       u8 one_check = 0xff;
-       
-       field.device = 0;
-       field.num_bits = sizeof(idcode_buffer) * 8;
-       field.out_value = idcode_buffer;
-       field.out_mask = NULL;
-       field.in_value = idcode_buffer;
-       field.in_check_value = NULL;
-       field.in_check_mask = NULL;
-       field.in_handler = NULL;
-       field.in_handler_priv = NULL;
-       
-       for (i = 0; i < JTAG_MAX_CHAIN_SIZE; i++)
-       {
-               buf_set_u32(idcode_buffer, i * 32, 32, 0x000000FF);
-       }
-       
-       jtag_add_plain_dr_scan(1, &field, TAP_TLR, NULL);
-       jtag_execute_queue();
-       
-       for (i = 0; i < JTAG_MAX_CHAIN_SIZE * 4; i++)
-       {
-               zero_check |= idcode_buffer[i];
-               one_check &= idcode_buffer[i];
-       }
-       
-       /* if there wasn't a single non-zero bit or if all bits were one, the scan isn't valid */
-       if ((zero_check == 0x00) || (one_check == 0xff))
-       {
-               ERROR("JTAG communication failure, check connection, JTAG interface, target power etc.");
-               return ERROR_JTAG_INIT_FAILED;
-       }
-       
-       for (bit_count = 0; bit_count < (JTAG_MAX_CHAIN_SIZE * 32) - 31;)
-       {
-               u32 idcode = buf_get_u32(idcode_buffer, bit_count, 32);
-               if ((idcode & 1) == 0)
-               {
-                       /* LSB must not be 0, this indicates a device in bypass */
-                       device_count++;
-                       
-                       bit_count += 1;
-               }
-               else
-               {
-                       u32 manufacturer;
-                       u32 part;
-                       u32 version;
-                       
-                       if (idcode == 0x000000FF)
-                       {
-                               /* End of chain (invalid manufacturer ID) */
-                               break;
-                       }
-                       
-                       if (device)
-                       {
-                               device->idcode = idcode;
-                               device = device->next;
-                       }
-                       device_count++;
-                       
-                       manufacturer = (idcode & 0xffe) >> 1;
-                       part = (idcode & 0xffff000) >> 12;
-                       version = (idcode & 0xf0000000) >> 28;
-
-                       INFO("JTAG device found: 0x%8.8x (Manufacturer: 0x%3.3x, Part: 0x%4.4x, Version: 0x%1.1x)", 
-                               idcode, manufacturer, part, version);
-                       
-                       bit_count += 32;
-               }
-       }
-       
-       /* see if number of discovered devices matches configuration */
-       if (device_count != jtag_num_devices)
-       {
-               ERROR("number of discovered devices in JTAG chain (%i) doesn't match configuration (%i)", 
-                       device_count, jtag_num_devices);
-               ERROR("check the config file and ensure proper JTAG communication (connections, speed, ...)");
-               return ERROR_JTAG_INIT_FAILED;
-       }
-       
-       return ERROR_OK;
-}
-
-int jtag_validate_chain()
-{
-       jtag_device_t *device = jtag_devices;
-       int total_ir_length = 0;
-       u8 *ir_test = NULL;
-       scan_field_t field;
-       int chain_pos = 0;
-       
-       while (device)
-       {
-               total_ir_length += device->ir_length;
-               device = device->next;
-       }
-       
-       total_ir_length += 2;
-       ir_test = malloc(CEIL(total_ir_length, 8));
-       buf_set_ones(ir_test, total_ir_length);
-       
-       field.device = 0;
-       field.num_bits = total_ir_length;
-       field.out_value = ir_test;
-       field.out_mask = NULL;
-       field.in_value = ir_test;
-       field.in_check_value = NULL;
-       field.in_check_mask = NULL;
-       field.in_handler = NULL;
-       field.in_handler_priv = NULL;
-       
-       jtag_add_plain_ir_scan(1, &field, TAP_TLR, NULL);
-       jtag_execute_queue();
-       
-       device = jtag_devices;
-       while (device)
-       {
-               if (buf_get_u32(ir_test, chain_pos, 2) != 0x1)
-               {
-                       char *cbuf = buf_to_str(ir_test, total_ir_length, 16);
-                       ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);
-                       free(cbuf);
-                       free(ir_test);
-                       return ERROR_JTAG_INIT_FAILED;
-               }
-               chain_pos += device->ir_length;
-               device = device->next;
-       }
-       
-       if (buf_get_u32(ir_test, chain_pos, 2) != 0x3)
-       {
-               char *cbuf = buf_to_str(ir_test, total_ir_length, 16);
-               ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);
-               free(cbuf);
-               free(ir_test);
-               return ERROR_JTAG_INIT_FAILED;
-       }
-       
-       free(ir_test);
-       
-       return ERROR_OK;
-}
-
-int jtag_register_commands(struct command_context_s *cmd_ctx)
-{
-       register_command(cmd_ctx, NULL, "interface", handle_interface_command,
-               COMMAND_CONFIG, NULL);
-       register_command(cmd_ctx, NULL, "jtag_speed", handle_jtag_speed_command,
-               COMMAND_ANY, "set jtag speed (if supported) <speed>");
-       register_command(cmd_ctx, NULL, "jtag_device", handle_jtag_device_command,
-               COMMAND_CONFIG, "jtag_device <ir_length> <ir_expected> <ir_mask>");
-       register_command(cmd_ctx, NULL, "reset_config", handle_reset_config_command,
-               COMMAND_CONFIG, NULL);
-       register_command(cmd_ctx, NULL, "jtag_nsrst_delay", handle_jtag_nsrst_delay_command,
-               COMMAND_CONFIG, NULL);
-       register_command(cmd_ctx, NULL, "jtag_ntrst_delay", handle_jtag_ntrst_delay_command,
-               COMMAND_CONFIG, NULL);
-               
-       register_command(cmd_ctx, NULL, "scan_chain", handle_scan_chain_command,
-               COMMAND_EXEC, "print current scan chain configuration");
-
-       register_command(cmd_ctx, NULL, "endstate", handle_endstate_command,
-               COMMAND_EXEC, "finish JTAG operations in <tap_state>");
-       register_command(cmd_ctx, NULL, "jtag_reset", handle_jtag_reset_command,
-               COMMAND_EXEC, "toggle reset lines <trst> <srst>");
-       register_command(cmd_ctx, NULL, "runtest", handle_runtest_command,
-               COMMAND_EXEC, "move to Run-Test/Idle, and execute <num_cycles>");
-       register_command(cmd_ctx, NULL, "statemove", handle_statemove_command,
-               COMMAND_EXEC, "move to current endstate or [tap_state]");
-       register_command(cmd_ctx, NULL, "irscan", handle_irscan_command,
-               COMMAND_EXEC, "execute IR scan <device> <instr> [dev2] [instr2] ...");
-       register_command(cmd_ctx, NULL, "drscan", handle_drscan_command,
-               COMMAND_EXEC, "execute DR scan <device> <var> [dev2] [var2] ...");
-
-       register_command(cmd_ctx, NULL, "verify_ircapture", handle_verify_ircapture_command,
-               COMMAND_ANY, "verify value captured during Capture-IR <enable|disable>");
-       return ERROR_OK;
-}
-
-int jtag_init(struct command_context_s *cmd_ctx)
-{
-       int i, validate_tries = 0;
-       
-       DEBUG("-");
-
-       if (jtag_speed == -1)
-               jtag_speed = 0;
-       
-       if (jtag_interface && (jtag_interface[0] != 0))
-               /* configuration var 'jtag_interface' is set, and not empty */
-               for (i = 0; jtag_interfaces[i]; i++)
-               {
-                       if (strcmp(jtag_interface, jtag_interfaces[i]->name) == 0)
-                       {
-                               jtag_device_t *device;
-                               device = jtag_devices;
-       
-                               if (jtag_interfaces[i]->init() != ERROR_OK)
-                                       return ERROR_JTAG_INIT_FAILED;
-                               jtag = jtag_interfaces[i];
-
-                               jtag_ir_scan_size = 0;
-                               jtag_num_devices = 0;
-                               while (device != NULL)
-                               {
-                                       jtag_ir_scan_size += device->ir_length;
-                                       jtag_num_devices++;
-                                       device = device->next;
-                               }
-                               
-                               jtag_add_statemove(TAP_TLR);
-                               jtag_execute_queue();
-
-                               /* examine chain first, as this could discover the real chain layout */
-                               if (jtag_examine_chain() != ERROR_OK)
-                               {
-                                       ERROR("trying to validate configured JTAG chain anyway...");
-                               }
-                               
-                               while (jtag_validate_chain() != ERROR_OK)
-                               {
-                                       validate_tries++;
-                                       if (validate_tries > 5)
-                                       {
-                                               ERROR("Could not validate JTAG chain, exit");
-                                               jtag = NULL;
-                                               return ERROR_JTAG_INVALID_INTERFACE;
-                                       }
-                                       usleep(10000);
-                               }
-
-                               return ERROR_OK;
-                       }
-               }
-       
-       /* no valid interface was found (i.e. the configuration option,
-        * didn't match one of the compiled-in interfaces
-        */
-       ERROR("No valid jtag interface found (%s)", jtag_interface);
-       ERROR("compiled-in jtag interfaces:");
-       for (i = 0; jtag_interfaces[i]; i++)
-       {
-               ERROR("%i: %s", i, jtag_interfaces[i]->name);
-       }
-       
-       jtag = NULL;
-       return ERROR_JTAG_INVALID_INTERFACE;
-}
-
-int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       int i;
-       
-       /* only if the configuration var isn't overwritten from cmdline */
-       if (!jtag_interface)
-       {
-               if (args[0] && (args[0][0] != 0))
-               {
-                       for (i=0; jtag_interfaces[i]; i++)
-                       {
-                               if (strcmp(args[0], jtag_interfaces[i]->name) == 0)
-                               {
-                                       if (jtag_interfaces[i]->register_commands(cmd_ctx) != ERROR_OK)
-                                               exit(-1);
-                               
-                                       jtag_interface = jtag_interfaces[i]->name;
-               
-                                       return ERROR_OK;
-                               }
-                       }
-               }
-               
-               /* remember the requested interface name, so we can complain about it later */
-               jtag_interface = strdup(args[0]);
-               DEBUG("'interface' command didn't specify a valid interface");
-       }
-       
-       return ERROR_OK;
-}
-
-int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       jtag_device_t **last_device_p = &jtag_devices;
-
-       if (*last_device_p)
-       {
-               while ((*last_device_p)->next)
-                       last_device_p = &((*last_device_p)->next);
-               last_device_p = &((*last_device_p)->next);
-       }
-
-       if (argc < 3)
-               return ERROR_OK;
-
-       *last_device_p = malloc(sizeof(jtag_device_t));
-       (*last_device_p)->ir_length = strtoul(args[0], NULL, 0);
-
-       (*last_device_p)->expected = malloc((*last_device_p)->ir_length);
-       buf_set_u32((*last_device_p)->expected, 0, (*last_device_p)->ir_length, strtoul(args[1], NULL, 0));
-       (*last_device_p)->expected_mask = malloc((*last_device_p)->ir_length);
-       buf_set_u32((*last_device_p)->expected_mask, 0, (*last_device_p)->ir_length, strtoul(args[2], NULL, 0));
-
-       (*last_device_p)->cur_instr = malloc((*last_device_p)->ir_length);
-       (*last_device_p)->bypass = 1;
-       buf_set_ones((*last_device_p)->cur_instr, (*last_device_p)->ir_length);
-       
-       (*last_device_p)->next = NULL;
-       
-       jtag_register_event_callback(jtag_reset_callback, (*last_device_p));
-       
-       jtag_num_devices++;
-
-       return ERROR_OK;
-}
-
-int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       jtag_device_t *device = jtag_devices;
-       int device_count = 0;
-       
-       while (device)
-       {
-               u32 expected, expected_mask, cur_instr;
-               expected = buf_get_u32(device->expected, 0, device->ir_length);
-               expected_mask = buf_get_u32(device->expected_mask, 0, device->ir_length);
-               cur_instr = buf_get_u32(device->cur_instr, 0, device->ir_length);
-               command_print(cmd_ctx, "%i: idcode: 0x%8.8x ir length %i, ir capture 0x%x, ir mask 0x%x, current instruction 0x%x", device_count, device->idcode, device->ir_length, expected, expected_mask, cur_instr);
-               device = device->next;
-               device_count++;
-       }
-
-       return ERROR_OK;
-}
-
-int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       if (argc >= 1)
-       {
-               if (strcmp(args[0], "none") == 0)
-                       jtag_reset_config = RESET_NONE;
-               else if (strcmp(args[0], "trst_only") == 0)
-                       jtag_reset_config = RESET_HAS_TRST;
-               else if (strcmp(args[0], "srst_only") == 0)
-                       jtag_reset_config = RESET_HAS_SRST;
-               else if (strcmp(args[0], "trst_and_srst") == 0)
-                       jtag_reset_config = RESET_TRST_AND_SRST;
-               else
-               {
-                       ERROR("invalid reset_config argument, defaulting to none");
-                       jtag_reset_config = RESET_NONE;
-                       return ERROR_INVALID_ARGUMENTS;
-               }
-       }
-       
-       if (argc >= 2)
-       {
-               if (strcmp(args[1], "srst_pulls_trst") == 0)
-                       jtag_reset_config |= RESET_SRST_PULLS_TRST;
-               else if (strcmp(args[1], "trst_pulls_srst") == 0)
-                       jtag_reset_config |= RESET_TRST_PULLS_SRST;
-               else if (strcmp(args[1], "combined") == 0)
-                       jtag_reset_config |= RESET_SRST_PULLS_TRST | RESET_TRST_PULLS_SRST;
-               else if (strcmp(args[1], "separate") == 0)
-                       jtag_reset_config &= ~(RESET_SRST_PULLS_TRST | RESET_TRST_PULLS_SRST);
-               else
-               {
-                       ERROR("invalid reset_config argument, defaulting to none");
-                       jtag_reset_config = RESET_NONE;
-                       return ERROR_INVALID_ARGUMENTS;
-               }
-       }
-       
-       if (argc >= 3)
-       {
-               if (strcmp(args[2], "trst_open_drain") == 0)
-                       jtag_reset_config |= RESET_TRST_OPEN_DRAIN;
-               else if (strcmp(args[2], "trst_push_pull") == 0)
-                       jtag_reset_config &= ~RESET_TRST_OPEN_DRAIN;
-               else
-               {
-                       ERROR("invalid reset_config argument, defaulting to none");
-                       jtag_reset_config = RESET_NONE;
-                       return ERROR_INVALID_ARGUMENTS;
-               }
-       }
-
-       if (argc >= 4)
-       {
-               if (strcmp(args[3], "srst_push_pull") == 0)
-                       jtag_reset_config |= RESET_SRST_PUSH_PULL;
-               else if (strcmp(args[3], "srst_open_drain") == 0)
-                       jtag_reset_config &= ~RESET_SRST_PUSH_PULL;
-               else
-               {
-                       ERROR("invalid reset_config argument, defaulting to none");
-                       jtag_reset_config = RESET_NONE;
-                       return ERROR_INVALID_ARGUMENTS;
-               }
-       }
-       
-       return ERROR_OK;
-}
-
-int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       if (argc < 1)
-       {
-               ERROR("jtag_nsrst_delay <ms> command takes one required argument");
-               exit(-1);
-       }
-       else
-       {
-               jtag_nsrst_delay = strtoul(args[0], NULL, 0);
-       }
-       
-       return ERROR_OK;
-}
-
-int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       if (argc < 1)
-       {
-               ERROR("jtag_ntrst_delay <ms> command takes one required argument");
-               exit(-1);
-       }
-       else
-       {
-               jtag_ntrst_delay = strtoul(args[0], NULL, 0);
-       }
-       
-       return ERROR_OK;
-}
-
-int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       if (argc == 0)
-               command_print(cmd_ctx, "jtag_speed: %i", jtag_speed);
-
-       if (argc > 0)
-       {
-               /* this command can be called during CONFIG, 
-                * in which case jtag isn't initialized */
-               if (jtag)
-                       jtag->speed(strtoul(args[0], NULL, 0));
-               else
-                       jtag_speed = strtoul(args[0], NULL, 0);
-       }
-
-       return ERROR_OK;
-}
-
-int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       enum tap_state state;
-
-       if (argc < 1)
-       {
+/***************************************************************************\r
+ *   Copyright (C) 2005 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "replacements.h"\r
+\r
+#include "jtag.h"\r
+\r
+#include "command.h"\r
+#include "log.h"\r
+#include "interpreter.h"\r
+\r
+#include "stdlib.h"\r
+#include "string.h"\r
+#include <unistd.h>\r
+\r
+char* tap_state_strings[16] =\r
+{\r
+       "tlr", \r
+       "sds", "cd", "sd", "e1d", "pd", "e2d", "ud",\r
+       "rti",\r
+       "sis", "ci", "si", "e1i", "pi", "e2i", "ui"\r
+};\r
+\r
+typedef struct cmd_queue_page_s\r
+{\r
+       void *address;\r
+       size_t used;\r
+       struct cmd_queue_page_s *next;\r
+} cmd_queue_page_t;\r
+\r
+#define CMD_QUEUE_PAGE_SIZE (1024 * 1024)\r
+static cmd_queue_page_t *cmd_queue_pages = NULL;\r
+\r
+/* tap_move[i][j]: tap movement command to go from state i to state j\r
+ * 0: Test-Logic-Reset\r
+ * 1: Run-Test/Idle\r
+ * 2: Shift-DR\r
+ * 3: Pause-DR\r
+ * 4: Shift-IR\r
+ * 5: Pause-IR\r
+ * \r
+ * SD->SD and SI->SI have to be caught in interface specific code\r
+ */\r
+u8 tap_move[6][6] =\r
+{\r
+/*       TLR   RTI   SD    PD    SI    PI             */\r
+       {0x7f, 0x00, 0x17, 0x0a, 0x1b, 0x16},   /* TLR */\r
+       {0x7f, 0x00, 0x25, 0x05, 0x2b, 0x0b},   /* RTI */\r
+       {0x7f, 0x31, 0x00, 0x01, 0x0f, 0x2f},   /* SD  */\r
+       {0x7f, 0x30, 0x20, 0x17, 0x1e, 0x2f},   /* PD  */\r
+       {0x7f, 0x31, 0x07, 0x17, 0x00, 0x01},   /* SI  */\r
+       {0x7f, 0x30, 0x1c, 0x17, 0x20, 0x2f}    /* PI  */\r
+};\r
+\r
+int tap_move_map[16] = {\r
+       0, -1, -1,  2, -1,  3, -1, -1,\r
+       1, -1, -1,  4, -1,  5, -1, -1\r
+};\r
+\r
+tap_transition_t tap_transitions[16] =\r
+{\r
+       {TAP_TLR, TAP_RTI},             /* TLR */\r
+       {TAP_SIS, TAP_CD},              /* SDS */\r
+       {TAP_E1D, TAP_SD},              /* CD  */\r
+       {TAP_E1D, TAP_SD},              /* SD  */\r
+       {TAP_UD,  TAP_PD},              /* E1D */\r
+       {TAP_E2D, TAP_PD},              /* PD  */\r
+       {TAP_UD,  TAP_SD},              /* E2D */\r
+       {TAP_SDS, TAP_RTI},             /* UD  */\r
+       {TAP_SDS, TAP_RTI},             /* RTI */\r
+       {TAP_TLR, TAP_CI},              /* SIS */\r
+       {TAP_E1I, TAP_SI},              /* CI  */\r
+       {TAP_E1I, TAP_SI},              /* SI  */\r
+       {TAP_UI,  TAP_PI},              /* E1I */\r
+       {TAP_E2I, TAP_PI},              /* PI  */\r
+       {TAP_UI,  TAP_SI},              /* E2I */\r
+       {TAP_SDS, TAP_RTI}              /* UI  */\r
+};\r
+\r
+char* jtag_event_strings[] =\r
+{\r
+       "SRST asserted",\r
+       "TRST asserted",\r
+       "SRST released",\r
+       "TRST released"\r
+};\r
+\r
+enum tap_state end_state = TAP_TLR;\r
+enum tap_state cur_state = TAP_TLR;\r
+int jtag_trst = 0;\r
+int jtag_srst = 0;\r
+\r
+jtag_command_t *jtag_command_queue = NULL;\r
+jtag_command_t **last_comand_pointer = &jtag_command_queue;\r
+jtag_device_t *jtag_devices = NULL;\r
+int jtag_num_devices = 0;\r
+int jtag_ir_scan_size = 0;\r
+enum reset_types jtag_reset_config = RESET_NONE;\r
+enum tap_state cmd_queue_end_state = TAP_TLR;\r
+enum tap_state cmd_queue_cur_state = TAP_TLR;\r
+\r
+int jtag_verify_capture_ir = 1;\r
+\r
+/* how long the OpenOCD should wait before attempting JTAG communication after reset lines deasserted (in ms) */\r
+int jtag_nsrst_delay = 0; /* default to no nSRST delay */\r
+int jtag_ntrst_delay = 0; /* default to no nTRST delay */ \r
+\r
+/* maximum number of JTAG devices expected in the chain\r
+ */\r
+#define JTAG_MAX_CHAIN_SIZE 20 \r
+\r
+/* callbacks to inform high-level handlers about JTAG state changes */\r
+jtag_event_callback_t *jtag_event_callbacks;\r
+\r
+/* jtag interfaces (parport, FTDI-USB, TI-USB, ...)\r
+ */\r
+#if BUILD_PARPORT == 1\r
+       extern jtag_interface_t parport_interface;\r
+#endif\r
+\r
+#if BUILD_FT2232_FTD2XX == 1\r
+       extern jtag_interface_t ft2232_interface;\r
+#endif\r
+\r
+#if BUILD_FT2232_LIBFTDI == 1\r
+       extern jtag_interface_t ft2232_interface;\r
+#endif\r
+\r
+#if BUILD_AMTJTAGACCEL == 1\r
+       extern jtag_interface_t amt_jtagaccel_interface;\r
+#endif\r
+\r
+#if BUILD_EP93XX == 1\r
+       extern jtag_interface_t ep93xx_interface;\r
+#endif\r
+\r
+#if BUILD_AT91RM9200 == 1\r
+       extern jtag_interface_t at91rm9200_interface;\r
+#endif\r
+\r
+#if BUILD_GW16012 == 1\r
+       extern jtag_interface_t gw16012_interface;\r
+#endif\r
+\r
+#if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1\r
+       extern jtag_interface_t presto_interface;\r
+#endif\r
+\r
+#if BUILD_USBPROG == 1\r
+       extern jtag_interface_t usbprog_interface;\r
+#endif\r
+\r
+jtag_interface_t *jtag_interfaces[] = {\r
+#if BUILD_PARPORT == 1\r
+       &parport_interface,\r
+#endif\r
+#if BUILD_FT2232_FTD2XX == 1\r
+       &ft2232_interface,\r
+#endif\r
+#if BUILD_FT2232_LIBFTDI == 1\r
+       &ft2232_interface,\r
+#endif\r
+#if BUILD_AMTJTAGACCEL == 1\r
+       &amt_jtagaccel_interface,\r
+#endif\r
+#if BUILD_EP93XX == 1\r
+       &ep93xx_interface,\r
+#endif\r
+#if BUILD_AT91RM9200 == 1\r
+       &at91rm9200_interface,\r
+#endif\r
+#if BUILD_GW16012 == 1\r
+       &gw16012_interface,\r
+#endif\r
+#if BUILD_PRESTO_LIBFTDI == 1 || BUILD_PRESTO_FTD2XX == 1\r
+       &presto_interface,\r
+#endif\r
+#if BUILD_USBPROG == 1\r
+       &usbprog_interface,\r
+#endif\r
+       NULL,\r
+};\r
+\r
+jtag_interface_t *jtag = NULL;\r
+\r
+/* configuration */\r
+char* jtag_interface = NULL;\r
+int jtag_speed = -1;\r
+\r
+\r
+/* forward declarations */\r
+int jtag_add_statemove(enum tap_state endstate);\r
+int jtag_add_pathmove(int num_states, enum tap_state *path);\r
+int jtag_add_runtest(int num_cycles, enum tap_state endstate);\r
+int jtag_add_reset(int trst, int srst);\r
+int jtag_add_end_state(enum tap_state endstate);\r
+int jtag_add_sleep(u32 us);\r
+int jtag_execute_queue(void);\r
+int jtag_cancel_queue(void);\r
+\r
+/* jtag commands */\r
+int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_statemove_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_drscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+int jtag_register_event_callback(int (*callback)(enum jtag_event event, void *priv), void *priv)\r
+{\r
+       jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;\r
+       \r
+       if (callback == NULL)\r
+       {\r
+               return ERROR_INVALID_ARGUMENTS;\r
+       }\r
+       \r
+       if (*callbacks_p)\r
+       {\r
+               while ((*callbacks_p)->next)\r
+                       callbacks_p = &((*callbacks_p)->next);\r
+               callbacks_p = &((*callbacks_p)->next);\r
+       }\r
+       \r
+       (*callbacks_p) = malloc(sizeof(jtag_event_callback_t));\r
+       (*callbacks_p)->callback = callback;\r
+       (*callbacks_p)->priv = priv;\r
+       (*callbacks_p)->next = NULL;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int jtag_unregister_event_callback(int (*callback)(enum jtag_event event, void *priv))\r
+{\r
+       jtag_event_callback_t **callbacks_p = &jtag_event_callbacks;\r
+       \r
+       if (callback == NULL)\r
+       {\r
+               return ERROR_INVALID_ARGUMENTS;\r
+       }\r
+               \r
+       while (*callbacks_p)\r
+       {\r
+               jtag_event_callback_t **next = &((*callbacks_p)->next);\r
+               if ((*callbacks_p)->callback == callback)\r
+               {\r
+                       free(*callbacks_p);\r
+                       *callbacks_p = *next;\r
+               }\r
+               callbacks_p = next;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int jtag_call_event_callbacks(enum jtag_event event)\r
+{\r
+       jtag_event_callback_t *callback = jtag_event_callbacks;\r
+       \r
+       DEBUG("jtag event: %s", jtag_event_strings[event]);\r
+       \r
+       while (callback)\r
+       {\r
+               callback->callback(event, callback->priv);\r
+               callback = callback->next;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+/* returns a pointer to the pointer of the last command in queue\r
+ * this may be a pointer to the root pointer (jtag_command_queue)\r
+ * or to the next member of the last but one command\r
+ */\r
+jtag_command_t** jtag_get_last_command_p(void)\r
+{\r
+/*     jtag_command_t *cmd = jtag_command_queue;\r
+       \r
+       if (cmd)\r
+               while (cmd->next)\r
+                       cmd = cmd->next;\r
+       else\r
+               return &jtag_command_queue;\r
+       \r
+       return &cmd->next;*/\r
+       \r
+       return last_comand_pointer;\r
+}\r
+\r
+/* returns a pointer to the n-th device in the scan chain */\r
+jtag_device_t* jtag_get_device(int num)\r
+{\r
+       jtag_device_t *device = jtag_devices;\r
+       int i = 0;\r
+\r
+       while (device)\r
+       {\r
+               if (num == i)\r
+                       return device;\r
+               device = device->next;\r
+               i++;\r
+       }\r
+       \r
+       ERROR("jtag device number %d not defined", num);\r
+       exit(-1);\r
+}\r
+\r
+void* cmd_queue_alloc(size_t size)\r
+{\r
+       cmd_queue_page_t **p_page = &cmd_queue_pages;\r
+       int offset;\r
+\r
+       if (*p_page)\r
+       {\r
+               while ((*p_page)->next)\r
+                       p_page = &((*p_page)->next);\r
+               if (CMD_QUEUE_PAGE_SIZE - (*p_page)->used < size)\r
+                       p_page = &((*p_page)->next);\r
+       }\r
+\r
+       if (!*p_page)\r
+       {\r
+               *p_page = malloc(sizeof(cmd_queue_page_t));\r
+               (*p_page)->used = 0;\r
+               (*p_page)->address = malloc(CMD_QUEUE_PAGE_SIZE);\r
+               (*p_page)->next = NULL;\r
+       }\r
+\r
+       offset = (*p_page)->used;\r
+       (*p_page)->used += size;\r
+       \r
+       u8 *t=(u8 *)((*p_page)->address);\r
+       return t + offset;\r
+}\r
+\r
+void cmd_queue_free()\r
+{\r
+       cmd_queue_page_t *page = cmd_queue_pages;\r
+\r
+       while (page)\r
+       {\r
+               cmd_queue_page_t *last = page;\r
+               free(page->address);\r
+               page = page->next;\r
+               free(last);\r
+       }\r
+\r
+       cmd_queue_pages = NULL;\r
+}\r
+\r
+int jtag_add_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+{\r
+       jtag_command_t **last_cmd;\r
+       jtag_device_t *device;\r
+       int i, j;\r
+       int scan_size = 0;\r
+\r
+       if (jtag_trst == 1)\r
+       {\r
+               WARNING("JTAG command queued, while TRST is low (TAP in reset)");\r
+               return ERROR_JTAG_TRST_ASSERTED;\r
+       }\r
+\r
+       last_cmd = jtag_get_last_command_p();\r
+       \r
+       /* allocate memory for a new list member */\r
+       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));\r
+       (*last_cmd)->next = NULL;\r
+       last_comand_pointer = &((*last_cmd)->next);\r
+       (*last_cmd)->type = JTAG_SCAN;\r
+\r
+       /* allocate memory for ir scan command */\r
+       (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));\r
+       (*last_cmd)->cmd.scan->ir_scan = 1;\r
+       (*last_cmd)->cmd.scan->num_fields = jtag_num_devices;   /* one field per device */\r
+       (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(jtag_num_devices * sizeof(scan_field_t));\r
+       (*last_cmd)->cmd.scan->end_state = state;\r
+               \r
+       if (state != -1)\r
+               cmd_queue_end_state = state;\r
+\r
+       if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)\r
+               jtag_call_event_callbacks(JTAG_TRST_RELEASED);\r
+       \r
+       if (cmd_queue_end_state == TAP_TLR)\r
+               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);\r
+       \r
+       cmd_queue_cur_state = cmd_queue_end_state;\r
+               \r
+       for (i = 0; i < jtag_num_devices; i++)\r
+       {\r
+               int found = 0;\r
+               device = jtag_get_device(i);\r
+               scan_size = device->ir_length;\r
+               (*last_cmd)->cmd.scan->fields[i].device = i;\r
+               (*last_cmd)->cmd.scan->fields[i].num_bits = scan_size;\r
+               (*last_cmd)->cmd.scan->fields[i].in_value = NULL;\r
+               (*last_cmd)->cmd.scan->fields[i].in_handler = NULL;     /* disable verification by default */\r
+\r
+               /* search the list */\r
+               for (j = 0; j < num_fields; j++)\r
+               {\r
+                       if (i == fields[j].device)\r
+                       {\r
+                               found = 1;\r
+                               (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);\r
+                               (*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);\r
+                       \r
+                               if (jtag_verify_capture_ir)\r
+                               {\r
+                                       if (fields[j].in_handler==NULL)\r
+                                       {\r
+                                               jtag_set_check_value((*last_cmd)->cmd.scan->fields+i, device->expected, device->expected_mask, NULL);\r
+                                       } else\r
+                                       {\r
+                                               (*last_cmd)->cmd.scan->fields[i].in_handler = fields[j].in_handler;\r
+                                               (*last_cmd)->cmd.scan->fields[i].in_handler_priv = fields[j].in_handler_priv;\r
+                                               (*last_cmd)->cmd.scan->fields[i].in_check_value = device->expected; \r
+                                               (*last_cmd)->cmd.scan->fields[i].in_check_mask = device->expected_mask;\r
+                                       }\r
+                               }\r
+                               \r
+                               device->bypass = 0;\r
+                               break;\r
+                       }\r
+               }\r
+       \r
+               if (!found)\r
+               {\r
+                       /* if a device isn't listed, set it to BYPASS */\r
+                       (*last_cmd)->cmd.scan->fields[i].out_value = buf_set_ones(cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);\r
+                       (*last_cmd)->cmd.scan->fields[i].out_mask = NULL;\r
+                       device->bypass = 1;\r
+               \r
+               }\r
+               \r
+               /* update device information */\r
+               buf_cpy((*last_cmd)->cmd.scan->fields[i].out_value, jtag_get_device(i)->cur_instr, scan_size);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+{\r
+       jtag_command_t **last_cmd;\r
+       int i;\r
+\r
+       if (jtag_trst == 1)\r
+       {\r
+               WARNING("JTAG command queued, while TRST is low (TAP in reset)");\r
+               return ERROR_JTAG_TRST_ASSERTED;\r
+       }\r
+\r
+       last_cmd = jtag_get_last_command_p();\r
+       \r
+       /* allocate memory for a new list member */\r
+       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));\r
+       (*last_cmd)->next = NULL;\r
+       last_comand_pointer = &((*last_cmd)->next);\r
+       (*last_cmd)->type = JTAG_SCAN;\r
+\r
+       /* allocate memory for ir scan command */\r
+       (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));\r
+       (*last_cmd)->cmd.scan->ir_scan = 1;\r
+       (*last_cmd)->cmd.scan->num_fields = num_fields;\r
+       (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));\r
+       (*last_cmd)->cmd.scan->end_state = state;\r
+\r
+       if (state != -1)\r
+               cmd_queue_end_state = state;\r
+\r
+       if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)\r
+               jtag_call_event_callbacks(JTAG_TRST_RELEASED);\r
+       \r
+       if (cmd_queue_end_state == TAP_TLR)\r
+               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);\r
+               \r
+       cmd_queue_cur_state = cmd_queue_end_state;\r
+       \r
+       for (i = 0; i < num_fields; i++)\r
+       {\r
+               int num_bits = fields[i].num_bits;\r
+               int num_bytes = CEIL(fields[i].num_bits, 8);\r
+               (*last_cmd)->cmd.scan->fields[i].device = fields[i].device;\r
+               (*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;\r
+               (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);\r
+               (*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);\r
+               (*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;\r
+               (*last_cmd)->cmd.scan->fields[i].in_check_value = fields[i].in_check_value;\r
+               (*last_cmd)->cmd.scan->fields[i].in_check_mask = fields[i].in_check_mask;\r
+               (*last_cmd)->cmd.scan->fields[i].in_handler = NULL;\r
+               (*last_cmd)->cmd.scan->fields[i].in_handler_priv = NULL;\r
+       }\r
+       return ERROR_OK;\r
+}\r
+\r
+int jtag_add_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+{\r
+       int i, j;\r
+       int bypass_devices = 0;\r
+       int field_count = 0;\r
+       jtag_command_t **last_cmd = jtag_get_last_command_p();\r
+       jtag_device_t *device = jtag_devices;\r
+       int scan_size;\r
+\r
+       if (jtag_trst == 1)\r
+       {\r
+               WARNING("JTAG command queued, while TRST is low (TAP in reset)");\r
+               return ERROR_JTAG_TRST_ASSERTED;\r
+       }\r
+\r
+       /* count devices in bypass */\r
+       while (device)\r
+       {\r
+               if (device->bypass)\r
+                       bypass_devices++;\r
+               device = device->next;\r
+       }\r
+       \r
+       /* allocate memory for a new list member */\r
+       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));\r
+       last_comand_pointer = &((*last_cmd)->next);\r
+       (*last_cmd)->next = NULL;\r
+       (*last_cmd)->type = JTAG_SCAN;\r
+\r
+       /* allocate memory for dr scan command */\r
+       (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));\r
+       (*last_cmd)->cmd.scan->ir_scan = 0;\r
+       (*last_cmd)->cmd.scan->num_fields = num_fields + bypass_devices;\r
+       (*last_cmd)->cmd.scan->fields = cmd_queue_alloc((num_fields + bypass_devices) * sizeof(scan_field_t));\r
+       (*last_cmd)->cmd.scan->end_state = state;\r
+       \r
+       if (state != -1)\r
+               cmd_queue_end_state = state;\r
+\r
+       if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)\r
+               jtag_call_event_callbacks(JTAG_TRST_RELEASED);\r
+       \r
+       if (cmd_queue_end_state == TAP_TLR)\r
+               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);\r
+                       \r
+       cmd_queue_cur_state = cmd_queue_end_state;\r
+       \r
+       for (i = 0; i < jtag_num_devices; i++)\r
+       {\r
+               int found = 0;\r
+               (*last_cmd)->cmd.scan->fields[field_count].device = i;\r
+       \r
+               for (j = 0; j < num_fields; j++)\r
+               {\r
+                       if (i == fields[j].device)\r
+                       {\r
+                               found = 1;\r
+                               scan_size = fields[j].num_bits;\r
+                               (*last_cmd)->cmd.scan->fields[field_count].num_bits = scan_size;\r
+                               (*last_cmd)->cmd.scan->fields[field_count].out_value = buf_cpy(fields[j].out_value, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);\r
+                               (*last_cmd)->cmd.scan->fields[field_count].out_mask = buf_cpy(fields[j].out_mask, cmd_queue_alloc(CEIL(scan_size, 8)), scan_size);\r
+                               (*last_cmd)->cmd.scan->fields[field_count].in_value = fields[j].in_value;\r
+                               (*last_cmd)->cmd.scan->fields[field_count].in_check_value = fields[j].in_check_value;\r
+                               (*last_cmd)->cmd.scan->fields[field_count].in_check_mask = fields[j].in_check_mask;\r
+                               (*last_cmd)->cmd.scan->fields[field_count].in_handler = fields[j].in_handler;\r
+                               (*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = fields[j].in_handler_priv;\r
+                       }\r
+               }\r
+               if (!found)\r
+               {\r
+                       /* if a device isn't listed, the BYPASS register should be selected */\r
+                       if (!jtag_get_device(i)->bypass)\r
+                       {\r
+                               ERROR("BUG: no scan data for a device not in BYPASS");\r
+                               exit(-1);\r
+                       }\r
+       \r
+                       /* program the scan field to 1 bit length, and ignore it's value */\r
+                       (*last_cmd)->cmd.scan->fields[field_count].num_bits = 1;\r
+                       (*last_cmd)->cmd.scan->fields[field_count].out_value = NULL;\r
+                       (*last_cmd)->cmd.scan->fields[field_count].out_mask = NULL;\r
+                       (*last_cmd)->cmd.scan->fields[field_count].in_value = NULL;\r
+                       (*last_cmd)->cmd.scan->fields[field_count].in_check_value = NULL;\r
+                       (*last_cmd)->cmd.scan->fields[field_count].in_check_mask = NULL;\r
+                       (*last_cmd)->cmd.scan->fields[field_count].in_handler = NULL;\r
+                       (*last_cmd)->cmd.scan->fields[field_count++].in_handler_priv = NULL;\r
+               }\r
+               else\r
+               {\r
+                       /* if a device is listed, the BYPASS register must not be selected */\r
+                       if (jtag_get_device(i)->bypass)\r
+                       {\r
+                               WARNING("scan data for a device in BYPASS");\r
+                       }\r
+               }\r
+       }\r
+       return ERROR_OK;\r
+}\r
+\r
+\r
+int jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, enum tap_state state)
+{\r
+       int i;\r
+       jtag_command_t **last_cmd = jtag_get_last_command_p();\r
+       \r
+       if (jtag_trst == 1)\r
+       {\r
+               WARNING("JTAG command queued, while TRST is low (TAP in reset)");\r
+               return ERROR_JTAG_TRST_ASSERTED;\r
+       }\r
+\r
+       /* allocate memory for a new list member */\r
+       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));\r
+       last_comand_pointer = &((*last_cmd)->next);\r
+       (*last_cmd)->next = NULL;\r
+       (*last_cmd)->type = JTAG_SCAN;\r
+\r
+       /* allocate memory for scan command */\r
+       (*last_cmd)->cmd.scan = cmd_queue_alloc(sizeof(scan_command_t));\r
+       (*last_cmd)->cmd.scan->ir_scan = 0;\r
+       (*last_cmd)->cmd.scan->num_fields = num_fields;\r
+       (*last_cmd)->cmd.scan->fields = cmd_queue_alloc(num_fields * sizeof(scan_field_t));\r
+       (*last_cmd)->cmd.scan->end_state = state;\r
+               \r
+       if (state != -1)\r
+               cmd_queue_end_state = state;\r
+\r
+       if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)\r
+               jtag_call_event_callbacks(JTAG_TRST_RELEASED);\r
+       \r
+       if (cmd_queue_end_state == TAP_TLR)\r
+               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);\r
+                       \r
+       cmd_queue_cur_state = cmd_queue_end_state;\r
+       \r
+       for (i = 0; i < num_fields; i++)\r
+       {\r
+               int num_bits = fields[i].num_bits;\r
+               int num_bytes = CEIL(fields[i].num_bits, 8);\r
+               (*last_cmd)->cmd.scan->fields[i].device = fields[i].device;\r
+               (*last_cmd)->cmd.scan->fields[i].num_bits = num_bits;\r
+               (*last_cmd)->cmd.scan->fields[i].out_value = buf_cpy(fields[i].out_value, cmd_queue_alloc(num_bytes), num_bits);\r
+               (*last_cmd)->cmd.scan->fields[i].out_mask = buf_cpy(fields[i].out_mask, cmd_queue_alloc(num_bytes), num_bits);\r
+               (*last_cmd)->cmd.scan->fields[i].in_value = fields[i].in_value;\r
+               (*last_cmd)->cmd.scan->fields[i].in_check_value = fields[i].in_check_value;\r
+               (*last_cmd)->cmd.scan->fields[i].in_check_mask = fields[i].in_check_mask;\r
+               (*last_cmd)->cmd.scan->fields[i].in_handler = fields[i].in_handler;\r
+               (*last_cmd)->cmd.scan->fields[i].in_handler_priv = fields[i].in_handler_priv;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+int jtag_add_statemove(enum tap_state state)\r
+{\r
+       jtag_command_t **last_cmd = jtag_get_last_command_p();\r
+       \r
+       if (jtag_trst == 1)\r
+       {\r
+               WARNING("JTAG command queued, while TRST is low (TAP in reset)");\r
+               return ERROR_JTAG_TRST_ASSERTED;\r
+       }\r
+\r
+       /* allocate memory for a new list member */\r
+       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));\r
+       last_comand_pointer = &((*last_cmd)->next);\r
+       (*last_cmd)->next = NULL;\r
+       (*last_cmd)->type = JTAG_STATEMOVE;\r
+\r
+       (*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));\r
+       (*last_cmd)->cmd.statemove->end_state = state;\r
+       \r
+       if (state != -1)\r
+               cmd_queue_end_state = state;\r
+\r
+       if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)\r
+               jtag_call_event_callbacks(JTAG_TRST_RELEASED);\r
+       \r
+       if (cmd_queue_end_state == TAP_TLR)\r
+               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);\r
+                       \r
+       cmd_queue_cur_state = cmd_queue_end_state;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int jtag_add_pathmove(int num_states, enum tap_state *path)\r
+{\r
+       jtag_command_t **last_cmd = jtag_get_last_command_p();\r
+       int i;\r
+       \r
+       if (jtag_trst == 1)\r
+       {\r
+               WARNING("JTAG command queued, while TRST is low (TAP in reset)");\r
+               return ERROR_JTAG_TRST_ASSERTED;\r
+       }\r
+       \r
+       /* the last state has to be a stable state */\r
+       if (tap_move_map[path[num_states - 1]] == -1)\r
+       {\r
+               ERROR("TAP path doesn't finish in a stable state");\r
+               return ERROR_JTAG_NOT_IMPLEMENTED;\r
+       }\r
+       \r
+       /* allocate memory for a new list member */\r
+       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));\r
+       last_comand_pointer = &((*last_cmd)->next);\r
+       (*last_cmd)->next = NULL;\r
+       (*last_cmd)->type = JTAG_PATHMOVE;\r
+\r
+       (*last_cmd)->cmd.pathmove = cmd_queue_alloc(sizeof(pathmove_command_t));\r
+       (*last_cmd)->cmd.pathmove->num_states = num_states;\r
+       (*last_cmd)->cmd.pathmove->path = cmd_queue_alloc(sizeof(enum tap_state) * num_states);\r
+       \r
+       for (i = 0; i < num_states; i++)\r
+               (*last_cmd)->cmd.pathmove->path[i] = path[i];\r
+\r
+       if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)\r
+               jtag_call_event_callbacks(JTAG_TRST_RELEASED);\r
+       \r
+       if (cmd_queue_end_state == TAP_TLR)\r
+               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);\r
+       \r
+       cmd_queue_cur_state = path[num_states - 1];\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int jtag_add_runtest(int num_cycles, enum tap_state state)\r
+{\r
+       jtag_command_t **last_cmd = jtag_get_last_command_p();\r
+       \r
+       if (jtag_trst == 1)\r
+       {\r
+               WARNING("JTAG command queued, while TRST is low (TAP in reset)");\r
+               return ERROR_JTAG_TRST_ASSERTED;\r
+       }\r
+\r
+       /* allocate memory for a new list member */\r
+       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));\r
+       (*last_cmd)->next = NULL;\r
+       last_comand_pointer = &((*last_cmd)->next);\r
+       (*last_cmd)->type = JTAG_RUNTEST;\r
+\r
+       (*last_cmd)->cmd.runtest = cmd_queue_alloc(sizeof(runtest_command_t));\r
+       (*last_cmd)->cmd.runtest->num_cycles = num_cycles;\r
+       (*last_cmd)->cmd.runtest->end_state = state;\r
+       \r
+       if (state != -1)\r
+               cmd_queue_end_state = state;\r
+\r
+       if (cmd_queue_cur_state == TAP_TLR && cmd_queue_end_state != TAP_TLR)\r
+               jtag_call_event_callbacks(JTAG_TRST_RELEASED);\r
+       \r
+       if (cmd_queue_end_state == TAP_TLR)\r
+               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);\r
+                       \r
+       cmd_queue_cur_state = cmd_queue_end_state;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int jtag_add_reset(int req_trst, int req_srst)\r
+{\r
+       int trst_with_tms = 0;\r
+       \r
+       jtag_command_t **last_cmd = jtag_get_last_command_p();\r
+       \r
+       if (req_trst == -1)\r
+               req_trst = jtag_trst;\r
+       \r
+       if (req_srst == -1)\r
+               req_srst = jtag_srst;\r
+\r
+       /* Make sure that jtag_reset_config allows the requested reset */\r
+       /* if SRST pulls TRST, we can't fulfill srst == 1 with trst == 0 */\r
+       if (((jtag_reset_config & RESET_SRST_PULLS_TRST) && (req_srst == 1)) && (req_trst == 0))\r
+               return ERROR_JTAG_RESET_WOULD_ASSERT_TRST;\r
+               \r
+       /* if TRST pulls SRST, we reset with TAP T-L-R */\r
+       if (((jtag_reset_config & RESET_TRST_PULLS_SRST) && (req_trst == 1)) && (req_srst == 0))\r
+       {\r
+               req_trst = 0;\r
+               trst_with_tms = 1;\r
+       }\r
+       \r
+       if (req_srst && !(jtag_reset_config & RESET_HAS_SRST))\r
+       {\r
+               ERROR("requested nSRST assertion, but the current configuration doesn't support this");\r
+               return ERROR_JTAG_RESET_CANT_SRST;\r
+       }\r
+       \r
+       if (req_trst && !(jtag_reset_config & RESET_HAS_TRST))\r
+       {\r
+               req_trst = 0;\r
+               trst_with_tms = 1;\r
+       }\r
+       \r
+       /* allocate memory for a new list member */\r
+       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));\r
+       (*last_cmd)->next = NULL;\r
+       last_comand_pointer = &((*last_cmd)->next);\r
+       (*last_cmd)->type = JTAG_RESET;\r
+\r
+       (*last_cmd)->cmd.reset = cmd_queue_alloc(sizeof(reset_command_t));\r
+       (*last_cmd)->cmd.reset->trst = req_trst;\r
+       (*last_cmd)->cmd.reset->srst = req_srst;\r
+\r
+       jtag_trst = req_trst;\r
+       jtag_srst = req_srst;\r
+\r
+       if (jtag_srst)\r
+       {\r
+               jtag_call_event_callbacks(JTAG_SRST_ASSERTED);\r
+       }\r
+       else\r
+       {\r
+               jtag_call_event_callbacks(JTAG_SRST_RELEASED);\r
+               if (jtag_nsrst_delay)\r
+                       jtag_add_sleep(jtag_nsrst_delay * 1000);\r
+       }\r
+       \r
+       if (trst_with_tms)\r
+       {\r
+               last_cmd = &((*last_cmd)->next);\r
+               \r
+               /* allocate memory for a new list member */\r
+               *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));\r
+               (*last_cmd)->next = NULL;\r
+               last_comand_pointer = &((*last_cmd)->next);\r
+               (*last_cmd)->type = JTAG_STATEMOVE;\r
+\r
+               (*last_cmd)->cmd.statemove = cmd_queue_alloc(sizeof(statemove_command_t));\r
+               (*last_cmd)->cmd.statemove->end_state = TAP_TLR;\r
+               \r
+               jtag_call_event_callbacks(JTAG_TRST_ASSERTED);\r
+               cmd_queue_cur_state = TAP_TLR;\r
+               cmd_queue_end_state = TAP_TLR;\r
+               \r
+               return ERROR_OK;\r
+       }\r
+       else\r
+       {\r
+               if (jtag_trst)\r
+               {\r
+                       /* we just asserted nTRST, so we're now in Test-Logic-Reset,\r
+                        * and inform possible listeners about this\r
+                        */\r
+                       cmd_queue_cur_state = TAP_TLR;\r
+                       jtag_call_event_callbacks(JTAG_TRST_ASSERTED);\r
+               }\r
+               else\r
+               {\r
+                       /* the nTRST line got deasserted, so we're still in Test-Logic-Reset,\r
+                        * but we might want to add a delay to give the TAP time to settle\r
+                        */\r
+                       if (jtag_ntrst_delay)\r
+                               jtag_add_sleep(jtag_ntrst_delay * 1000);\r
+               }\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int jtag_add_end_state(enum tap_state state)\r
+{\r
+       jtag_command_t **last_cmd = jtag_get_last_command_p();\r
+       \r
+       /* allocate memory for a new list member */\r
+       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));\r
+       (*last_cmd)->next = NULL;\r
+       last_comand_pointer = &((*last_cmd)->next);\r
+       (*last_cmd)->type = JTAG_END_STATE;\r
+\r
+       (*last_cmd)->cmd.end_state = cmd_queue_alloc(sizeof(end_state_command_t));\r
+       (*last_cmd)->cmd.end_state->end_state = state;\r
+\r
+       if (state != -1)\r
+               cmd_queue_end_state = state;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int jtag_add_sleep(u32 us)\r
+{\r
+       jtag_command_t **last_cmd = jtag_get_last_command_p();\r
+       \r
+       /* allocate memory for a new list member */\r
+       *last_cmd = cmd_queue_alloc(sizeof(jtag_command_t));\r
+       (*last_cmd)->next = NULL;\r
+       last_comand_pointer = &((*last_cmd)->next);\r
+       (*last_cmd)->type = JTAG_SLEEP;\r
+\r
+       (*last_cmd)->cmd.sleep = cmd_queue_alloc(sizeof(sleep_command_t));\r
+       (*last_cmd)->cmd.sleep->us = us;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int jtag_scan_size(scan_command_t *cmd)\r
+{\r
+       int bit_count = 0;\r
+       int i;\r
+\r
+       /* count bits in scan command */\r
+       for (i = 0; i < cmd->num_fields; i++)\r
+       {\r
+               bit_count += cmd->fields[i].num_bits;\r
+       }\r
+\r
+       return bit_count;\r
+}\r
+\r
+int jtag_build_buffer(scan_command_t *cmd, u8 **buffer)\r
+{\r
+       int bit_count = 0;\r
+       int i;\r
+       \r
+       bit_count = jtag_scan_size(cmd);\r
+       *buffer = malloc(CEIL(bit_count, 8));\r
+       \r
+       bit_count = 0;\r
+\r
+       for (i = 0; i < cmd->num_fields; i++)\r
+       {\r
+               if (cmd->fields[i].out_value)\r
+               {\r
+#ifdef _DEBUG_JTAG_IO_\r
+                       char* char_buf = buf_to_str(cmd->fields[i].out_value, (cmd->fields[i].num_bits > 64) ? 64 : cmd->fields[i].num_bits, 16);\r
+#endif\r
+                       buf_set_buf(cmd->fields[i].out_value, 0, *buffer, bit_count, cmd->fields[i].num_bits);\r
+#ifdef _DEBUG_JTAG_IO_\r
+                       DEBUG("fields[%i].out_value: 0x%s", i, char_buf);\r
+                       free(char_buf);\r
+#endif\r
+               }\r
+               \r
+               bit_count += cmd->fields[i].num_bits;\r
+       }\r
+\r
+       return bit_count;\r
+\r
+}\r
+\r
+int jtag_read_buffer(u8 *buffer, scan_command_t *cmd)\r
+{\r
+       int i;\r
+       int bit_count = 0;\r
+       int retval;\r
+       \r
+       /* we return ERROR_OK, unless a check fails, or a handler reports a problem */\r
+       retval = ERROR_OK;\r
+       \r
+       for (i = 0; i < cmd->num_fields; i++)\r
+       {\r
+               /* if neither in_value nor in_handler\r
+                * are specified we don't have to examine this field\r
+                */\r
+               if (cmd->fields[i].in_value || cmd->fields[i].in_handler)\r
+               {\r
+                       int num_bits = cmd->fields[i].num_bits;\r
+                       u8 *captured = buf_set_buf(buffer, bit_count, malloc(CEIL(num_bits, 8)), 0, num_bits);\r
+                       \r
+#ifdef _DEBUG_JTAG_IO_\r
+                       char *char_buf;\r
+\r
+                       char_buf = buf_to_str(captured, (num_bits > 64) ? 64 : num_bits, 16);\r
+                       DEBUG("fields[%i].in_value: 0x%s", i, char_buf);\r
+                       free(char_buf);\r
+#endif\r
+                       \r
+                       if (cmd->fields[i].in_value)\r
+                       {\r
+                               buf_cpy(captured, cmd->fields[i].in_value, num_bits);\r
+                               \r
+                               if (cmd->fields[i].in_handler)\r
+                               {\r
+                                       if (cmd->fields[i].in_handler(cmd->fields[i].in_value, cmd->fields[i].in_handler_priv, cmd->fields+i) != ERROR_OK)\r
+                                       {\r
+                                               WARNING("in_handler reported a failed check");\r
+                                               retval = ERROR_JTAG_QUEUE_FAILED;\r
+                                       }\r
+                               }\r
+                       }\r
+                       \r
+                       /* no in_value specified, but a handler takes care of the scanned data */\r
+                       if (cmd->fields[i].in_handler && (!cmd->fields[i].in_value))\r
+                       {\r
+                               if (cmd->fields[i].in_handler(captured, cmd->fields[i].in_handler_priv, cmd->fields+i) != ERROR_OK)\r
+                               {\r
+                                       /* We're going to call the error:handler later, but if the in_handler\r
+                                        * reported an error we report this failure upstream\r
+                                        */\r
+                                       WARNING("in_handler reported a failed check");\r
+                                       retval = ERROR_JTAG_QUEUE_FAILED;\r
+                               }\r
+                       }\r
+\r
+                       free(captured);\r
+               }\r
+               bit_count += cmd->fields[i].num_bits;\r
+       }\r
+\r
+       return retval;\r
+}\r
+\r
+int jtag_check_value(u8 *captured, void *priv, scan_field_t *field)\r
+{\r
+       int retval = ERROR_OK;\r
+       int num_bits = field->num_bits;\r
+       \r
+       int compare_failed = 0;\r
+       \r
+       if (field->in_check_mask)\r
+               compare_failed = buf_cmp_mask(captured, field->in_check_value, field->in_check_mask, num_bits);\r
+       else\r
+               compare_failed = buf_cmp(captured, field->in_check_value, num_bits);\r
+       \r
+       if (compare_failed)\r
+               {\r
+               /* An error handler could have caught the failing check\r
+                * only report a problem when there wasn't a handler, or if the handler\r
+                * acknowledged the error\r
+                */ \r
+               if (compare_failed)\r
+               {\r
+                       char *captured_char = buf_to_str(captured, (num_bits > 64) ? 64 : num_bits, 16);\r
+                       char *in_check_value_char = buf_to_str(field->in_check_value, (num_bits > 64) ? 64 : num_bits, 16);\r
+\r
+                       if (field->in_check_mask)\r
+                       {\r
+                               char *in_check_mask_char;\r
+                               in_check_mask_char = buf_to_str(field->in_check_mask, (num_bits > 64) ? 64 : num_bits, 16);\r
+                               WARNING("value captured during scan didn't pass the requested check: captured: 0x%s check_value: 0x%s check_mask: 0x%s", captured_char, in_check_value_char, in_check_mask_char);\r
+                               free(in_check_mask_char);\r
+                       }\r
+                       else\r
+                       {\r
+                               WARNING("value captured during scan didn't pass the requested check: captured: 0x%s check_value: 0x%s", captured_char, in_check_value_char);\r
+                       }\r
+\r
+                       free(captured_char);\r
+                       free(in_check_value_char);\r
+                       \r
+                       retval = ERROR_JTAG_QUEUE_FAILED;\r
+               }\r
+               \r
+       }\r
+       return retval;\r
+}\r
+\r
+/* \r
+  set up checking of this field using the in_handler. The values passed in must be valid until\r
+  after jtag_execute() has completed.\r
+ */\r
+void jtag_set_check_value(scan_field_t *field, u8 *value, u8 *mask, error_handler_t *in_error_handler)\r
+{\r
+       if (value)\r
+               field->in_handler = jtag_check_value;\r
+       else\r
+               field->in_handler = NULL;       /* No check, e.g. embeddedice uses value==NULL to indicate no check */\r
+       field->in_handler_priv = NULL;  /* this will be filled in at the invocation site to point to the field duplicate */ \r
+       field->in_check_value = value;\r
+       field->in_check_mask = mask;\r
+}\r
+\r
+enum scan_type jtag_scan_type(scan_command_t *cmd)\r
+{\r
+       int i;\r
+       int type = 0;\r
+       \r
+       for (i = 0; i < cmd->num_fields; i++)\r
+       {\r
+               if (cmd->fields[i].in_value || cmd->fields[i].in_handler)\r
+                       type |= SCAN_IN;\r
+               if (cmd->fields[i].out_value)\r
+                       type |= SCAN_OUT;\r
+       }\r
+\r
+       return type;\r
+}\r
+\r
+int jtag_execute_queue(void)\r
+{\r
+       int retval;\r
+\r
+       retval = jtag->execute_queue();\r
+       \r
+       cmd_queue_free();\r
+\r
+       jtag_command_queue = NULL;\r
+       last_comand_pointer = &jtag_command_queue;\r
+\r
+       return retval;\r
+}\r
+\r
+int jtag_reset_callback(enum jtag_event event, void *priv)\r
+{\r
+       jtag_device_t *device = priv;\r
+\r
+       DEBUG("-");\r
+       \r
+       if (event == JTAG_TRST_ASSERTED)\r
+       {\r
+               buf_set_ones(device->cur_instr, device->ir_length);\r
+               device->bypass = 1;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+void jtag_sleep(u32 us)\r
+{\r
+       usleep(us);\r
+}\r
+\r
+/* Try to examine chain layout according to IEEE 1149.1 Â§12\r
+ */\r
+int jtag_examine_chain()\r
+{\r
+       jtag_device_t *device = jtag_devices;\r
+       scan_field_t field;\r
+       u8 idcode_buffer[JTAG_MAX_CHAIN_SIZE * 4];\r
+       int i;\r
+       int bit_count;\r
+       int device_count = 0;\r
+       u8 zero_check = 0x0;\r
+       u8 one_check = 0xff;\r
+       \r
+       field.device = 0;\r
+       field.num_bits = sizeof(idcode_buffer) * 8;\r
+       field.out_value = idcode_buffer;\r
+       field.out_mask = NULL;\r
+       field.in_value = idcode_buffer;\r
+       field.in_check_value = NULL;\r
+       field.in_check_mask = NULL;\r
+       field.in_handler = NULL;\r
+       field.in_handler_priv = NULL;\r
+       \r
+       for (i = 0; i < JTAG_MAX_CHAIN_SIZE; i++)\r
+       {\r
+               buf_set_u32(idcode_buffer, i * 32, 32, 0x000000FF);\r
+       }\r
+       \r
+       jtag_add_plain_dr_scan(1, &field, TAP_TLR);\r
+       jtag_execute_queue();\r
+       \r
+       for (i = 0; i < JTAG_MAX_CHAIN_SIZE * 4; i++)\r
+       {\r
+               zero_check |= idcode_buffer[i];\r
+               one_check &= idcode_buffer[i];\r
+       }\r
+       \r
+       /* if there wasn't a single non-zero bit or if all bits were one, the scan isn't valid */\r
+       if ((zero_check == 0x00) || (one_check == 0xff))\r
+       {\r
+               ERROR("JTAG communication failure, check connection, JTAG interface, target power etc.");\r
+               return ERROR_JTAG_INIT_FAILED;\r
+       }\r
+       \r
+       for (bit_count = 0; bit_count < (JTAG_MAX_CHAIN_SIZE * 32) - 31;)\r
+       {\r
+               u32 idcode = buf_get_u32(idcode_buffer, bit_count, 32);\r
+               if ((idcode & 1) == 0)\r
+               {\r
+                       /* LSB must not be 0, this indicates a device in bypass */\r
+                       device_count++;\r
+                       \r
+                       bit_count += 1;\r
+               }\r
+               else\r
+               {\r
+                       u32 manufacturer;\r
+                       u32 part;\r
+                       u32 version;\r
+                       \r
+                       if (idcode == 0x000000FF)\r
+                       {\r
+                               /* End of chain (invalid manufacturer ID) */\r
+                               break;\r
+                       }\r
+                       \r
+                       if (device)\r
+                       {\r
+                               device->idcode = idcode;\r
+                               device = device->next;\r
+                       }\r
+                       device_count++;\r
+                       \r
+                       manufacturer = (idcode & 0xffe) >> 1;\r
+                       part = (idcode & 0xffff000) >> 12;\r
+                       version = (idcode & 0xf0000000) >> 28;\r
+\r
+                       INFO("JTAG device found: 0x%8.8x (Manufacturer: 0x%3.3x, Part: 0x%4.4x, Version: 0x%1.1x)", \r
+                               idcode, manufacturer, part, version);\r
+                       \r
+                       bit_count += 32;\r
+               }\r
+       }\r
+       \r
+       /* see if number of discovered devices matches configuration */\r
+       if (device_count != jtag_num_devices)\r
+       {\r
+               ERROR("number of discovered devices in JTAG chain (%i) doesn't match configuration (%i)", \r
+                       device_count, jtag_num_devices);\r
+               ERROR("check the config file and ensure proper JTAG communication (connections, speed, ...)");\r
+               return ERROR_JTAG_INIT_FAILED;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int jtag_validate_chain()\r
+{\r
+       jtag_device_t *device = jtag_devices;\r
+       int total_ir_length = 0;\r
+       u8 *ir_test = NULL;\r
+       scan_field_t field;\r
+       int chain_pos = 0;\r
+       \r
+       while (device)\r
+       {\r
+               total_ir_length += device->ir_length;\r
+               device = device->next;\r
+       }\r
+       \r
+       total_ir_length += 2;\r
+       ir_test = malloc(CEIL(total_ir_length, 8));\r
+       buf_set_ones(ir_test, total_ir_length);\r
+       \r
+       field.device = 0;\r
+       field.num_bits = total_ir_length;\r
+       field.out_value = ir_test;\r
+       field.out_mask = NULL;\r
+       field.in_value = ir_test;\r
+       field.in_check_value = NULL;\r
+       field.in_check_mask = NULL;\r
+       field.in_handler = NULL;\r
+       field.in_handler_priv = NULL;\r
+       \r
+       jtag_add_plain_ir_scan(1, &field, TAP_TLR);\r
+       jtag_execute_queue();\r
+       \r
+       device = jtag_devices;\r
+       while (device)\r
+       {\r
+               if (buf_get_u32(ir_test, chain_pos, 2) != 0x1)\r
+               {\r
+                       char *cbuf = buf_to_str(ir_test, total_ir_length, 16);\r
+                       ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);\r
+                       free(cbuf);\r
+                       free(ir_test);\r
+                       return ERROR_JTAG_INIT_FAILED;\r
+               }\r
+               chain_pos += device->ir_length;\r
+               device = device->next;\r
+       }\r
+       \r
+       if (buf_get_u32(ir_test, chain_pos, 2) != 0x3)\r
+       {\r
+               char *cbuf = buf_to_str(ir_test, total_ir_length, 16);\r
+               ERROR("Error validating JTAG scan chain, IR mismatch, scan returned 0x%s", cbuf);\r
+               free(cbuf);\r
+               free(ir_test);\r
+               return ERROR_JTAG_INIT_FAILED;\r
+       }\r
+       \r
+       free(ir_test);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int jtag_register_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       register_command(cmd_ctx, NULL, "interface", handle_interface_command,\r
+               COMMAND_CONFIG, NULL);\r
+       register_command(cmd_ctx, NULL, "jtag_speed", handle_jtag_speed_command,\r
+               COMMAND_ANY, "set jtag speed (if supported) <speed>");\r
+       register_command(cmd_ctx, NULL, "jtag_device", handle_jtag_device_command,\r
+               COMMAND_CONFIG, "jtag_device <ir_length> <ir_expected> <ir_mask>");\r
+       register_command(cmd_ctx, NULL, "reset_config", handle_reset_config_command,\r
+               COMMAND_CONFIG, NULL);\r
+       register_command(cmd_ctx, NULL, "jtag_nsrst_delay", handle_jtag_nsrst_delay_command,\r
+               COMMAND_CONFIG, NULL);\r
+       register_command(cmd_ctx, NULL, "jtag_ntrst_delay", handle_jtag_ntrst_delay_command,\r
+               COMMAND_CONFIG, NULL);\r
+               \r
+       register_command(cmd_ctx, NULL, "scan_chain", handle_scan_chain_command,\r
+               COMMAND_EXEC, "print current scan chain configuration");\r
+\r
+       register_command(cmd_ctx, NULL, "endstate", handle_endstate_command,\r
+               COMMAND_EXEC, "finish JTAG operations in <tap_state>");\r
+       register_command(cmd_ctx, NULL, "jtag_reset", handle_jtag_reset_command,\r
+               COMMAND_EXEC, "toggle reset lines <trst> <srst>");\r
+       register_command(cmd_ctx, NULL, "runtest", handle_runtest_command,\r
+               COMMAND_EXEC, "move to Run-Test/Idle, and execute <num_cycles>");\r
+       register_command(cmd_ctx, NULL, "statemove", handle_statemove_command,\r
+               COMMAND_EXEC, "move to current endstate or [tap_state]");\r
+       register_command(cmd_ctx, NULL, "irscan", handle_irscan_command,\r
+               COMMAND_EXEC, "execute IR scan <device> <instr> [dev2] [instr2] ...");\r
+       register_command(cmd_ctx, NULL, "drscan", handle_drscan_command,\r
+               COMMAND_EXEC, "execute DR scan <device> <var> [dev2] [var2] ...");\r
+\r
+       register_command(cmd_ctx, NULL, "verify_ircapture", handle_verify_ircapture_command,\r
+               COMMAND_ANY, "verify value captured during Capture-IR <enable|disable>");\r
+       return ERROR_OK;\r
+}\r
+\r
+int jtag_init(struct command_context_s *cmd_ctx)\r
+{\r
+       int i, validate_tries = 0;\r
+       \r
+       DEBUG("-");\r
+\r
+       if (jtag_speed == -1)\r
+               jtag_speed = 0;\r
+       \r
+       if (jtag_interface && (jtag_interface[0] != 0))\r
+               /* configuration var 'jtag_interface' is set, and not empty */\r
+               for (i = 0; jtag_interfaces[i]; i++)\r
+               {\r
+                       if (strcmp(jtag_interface, jtag_interfaces[i]->name) == 0)\r
+                       {\r
+                               jtag_device_t *device;\r
+                               device = jtag_devices;\r
+       \r
+                               if (jtag_interfaces[i]->init() != ERROR_OK)\r
+                                       return ERROR_JTAG_INIT_FAILED;\r
+                               jtag = jtag_interfaces[i];\r
+\r
+                               jtag_ir_scan_size = 0;\r
+                               jtag_num_devices = 0;\r
+                               while (device != NULL)\r
+                               {\r
+                                       jtag_ir_scan_size += device->ir_length;\r
+                                       jtag_num_devices++;\r
+                                       device = device->next;\r
+                               }\r
+                               \r
+                               jtag_add_statemove(TAP_TLR);\r
+                               jtag_execute_queue();\r
+\r
+                               /* examine chain first, as this could discover the real chain layout */\r
+                               if (jtag_examine_chain() != ERROR_OK)\r
+                               {\r
+                                       ERROR("trying to validate configured JTAG chain anyway...");\r
+                               }\r
+                               \r
+                               while (jtag_validate_chain() != ERROR_OK)\r
+                               {\r
+                                       validate_tries++;\r
+                                       if (validate_tries > 5)\r
+                                       {\r
+                                               ERROR("Could not validate JTAG chain, exit");\r
+                                               jtag = NULL;\r
+                                               return ERROR_JTAG_INVALID_INTERFACE;\r
+                                       }\r
+                                       usleep(10000);\r
+                               }\r
+\r
+                               return ERROR_OK;\r
+                       }\r
+               }\r
+       \r
+       /* no valid interface was found (i.e. the configuration option,\r
+        * didn't match one of the compiled-in interfaces\r
+        */\r
+       ERROR("No valid jtag interface found (%s)", jtag_interface);\r
+       ERROR("compiled-in jtag interfaces:");\r
+       for (i = 0; jtag_interfaces[i]; i++)\r
+       {\r
+               ERROR("%i: %s", i, jtag_interfaces[i]->name);\r
+       }\r
+       \r
+       jtag = NULL;\r
+       return ERROR_JTAG_INVALID_INTERFACE;\r
+}\r
+\r
+int handle_interface_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       int i;\r
+       \r
+       /* only if the configuration var isn't overwritten from cmdline */\r
+       if (!jtag_interface)\r
+       {\r
+               if (args[0] && (args[0][0] != 0))\r
+               {\r
+                       for (i=0; jtag_interfaces[i]; i++)\r
+                       {\r
+                               if (strcmp(args[0], jtag_interfaces[i]->name) == 0)\r
+                               {\r
+                                       if (jtag_interfaces[i]->register_commands(cmd_ctx) != ERROR_OK)\r
+                                               exit(-1);\r
+                               \r
+                                       jtag_interface = jtag_interfaces[i]->name;\r
+               \r
+                                       return ERROR_OK;\r
+                               }\r
+                       }\r
+               }\r
+               \r
+               /* remember the requested interface name, so we can complain about it later */\r
+               jtag_interface = strdup(args[0]);\r
+               DEBUG("'interface' command didn't specify a valid interface");\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_jtag_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       jtag_device_t **last_device_p = &jtag_devices;\r
+\r
+       if (*last_device_p)\r
+       {\r
+               while ((*last_device_p)->next)\r
+                       last_device_p = &((*last_device_p)->next);\r
+               last_device_p = &((*last_device_p)->next);\r
+       }\r
+\r
+       if (argc < 3)\r
+               return ERROR_OK;\r
+\r
+       *last_device_p = malloc(sizeof(jtag_device_t));\r
+       (*last_device_p)->ir_length = strtoul(args[0], NULL, 0);\r
+\r
+       (*last_device_p)->expected = malloc((*last_device_p)->ir_length);\r
+       buf_set_u32((*last_device_p)->expected, 0, (*last_device_p)->ir_length, strtoul(args[1], NULL, 0));\r
+       (*last_device_p)->expected_mask = malloc((*last_device_p)->ir_length);\r
+       buf_set_u32((*last_device_p)->expected_mask, 0, (*last_device_p)->ir_length, strtoul(args[2], NULL, 0));\r
+\r
+       (*last_device_p)->cur_instr = malloc((*last_device_p)->ir_length);\r
+       (*last_device_p)->bypass = 1;\r
+       buf_set_ones((*last_device_p)->cur_instr, (*last_device_p)->ir_length);\r
+       \r
+       (*last_device_p)->next = NULL;\r
+       \r
+       jtag_register_event_callback(jtag_reset_callback, (*last_device_p));\r
+       \r
+       jtag_num_devices++;\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_scan_chain_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       jtag_device_t *device = jtag_devices;\r
+       int device_count = 0;\r
+       \r
+       while (device)\r
+       {\r
+               u32 expected, expected_mask, cur_instr;\r
+               expected = buf_get_u32(device->expected, 0, device->ir_length);\r
+               expected_mask = buf_get_u32(device->expected_mask, 0, device->ir_length);\r
+               cur_instr = buf_get_u32(device->cur_instr, 0, device->ir_length);\r
+               command_print(cmd_ctx, "%i: idcode: 0x%8.8x ir length %i, ir capture 0x%x, ir mask 0x%x, current instruction 0x%x", device_count, device->idcode, device->ir_length, expected, expected_mask, cur_instr);\r
+               device = device->next;\r
+               device_count++;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_reset_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       if (argc >= 1)\r
+       {\r
+               if (strcmp(args[0], "none") == 0)\r
+                       jtag_reset_config = RESET_NONE;\r
+               else if (strcmp(args[0], "trst_only") == 0)\r
+                       jtag_reset_config = RESET_HAS_TRST;\r
+               else if (strcmp(args[0], "srst_only") == 0)\r
+                       jtag_reset_config = RESET_HAS_SRST;\r
+               else if (strcmp(args[0], "trst_and_srst") == 0)\r
+                       jtag_reset_config = RESET_TRST_AND_SRST;\r
+               else\r
+               {\r
+                       ERROR("invalid reset_config argument, defaulting to none");\r
+                       jtag_reset_config = RESET_NONE;\r
+                       return ERROR_INVALID_ARGUMENTS;\r
+               }\r
+       }\r
+       \r
+       if (argc >= 2)\r
+       {\r
+               if (strcmp(args[1], "srst_pulls_trst") == 0)\r
+                       jtag_reset_config |= RESET_SRST_PULLS_TRST;\r
+               else if (strcmp(args[1], "trst_pulls_srst") == 0)\r
+                       jtag_reset_config |= RESET_TRST_PULLS_SRST;\r
+               else if (strcmp(args[1], "combined") == 0)\r
+                       jtag_reset_config |= RESET_SRST_PULLS_TRST | RESET_TRST_PULLS_SRST;\r
+               else if (strcmp(args[1], "separate") == 0)\r
+                       jtag_reset_config &= ~(RESET_SRST_PULLS_TRST | RESET_TRST_PULLS_SRST);\r
+               else\r
+               {\r
+                       ERROR("invalid reset_config argument, defaulting to none");\r
+                       jtag_reset_config = RESET_NONE;\r
+                       return ERROR_INVALID_ARGUMENTS;\r
+               }\r
+       }\r
+       \r
+       if (argc >= 3)\r
+       {\r
+               if (strcmp(args[2], "trst_open_drain") == 0)\r
+                       jtag_reset_config |= RESET_TRST_OPEN_DRAIN;\r
+               else if (strcmp(args[2], "trst_push_pull") == 0)\r
+                       jtag_reset_config &= ~RESET_TRST_OPEN_DRAIN;\r
+               else\r
+               {\r
+                       ERROR("invalid reset_config argument, defaulting to none");\r
+                       jtag_reset_config = RESET_NONE;\r
+                       return ERROR_INVALID_ARGUMENTS;\r
+               }\r
+       }\r
+\r
+       if (argc >= 4)\r
+       {\r
+               if (strcmp(args[3], "srst_push_pull") == 0)\r
+                       jtag_reset_config |= RESET_SRST_PUSH_PULL;\r
+               else if (strcmp(args[3], "srst_open_drain") == 0)\r
+                       jtag_reset_config &= ~RESET_SRST_PUSH_PULL;\r
+               else\r
+               {\r
+                       ERROR("invalid reset_config argument, defaulting to none");\r
+                       jtag_reset_config = RESET_NONE;\r
+                       return ERROR_INVALID_ARGUMENTS;\r
+               }\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_jtag_nsrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       if (argc < 1)\r
+       {\r
+               ERROR("jtag_nsrst_delay <ms> command takes one required argument");\r
+               exit(-1);\r
+       }\r
+       else\r
+       {\r
+               jtag_nsrst_delay = strtoul(args[0], NULL, 0);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_jtag_ntrst_delay_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       if (argc < 1)\r
+       {\r
+               ERROR("jtag_ntrst_delay <ms> command takes one required argument");\r
+               exit(-1);\r
+       }\r
+       else\r
+       {\r
+               jtag_ntrst_delay = strtoul(args[0], NULL, 0);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_jtag_speed_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       if (argc == 0)\r
+               command_print(cmd_ctx, "jtag_speed: %i", jtag_speed);\r
+\r
+       if (argc > 0)\r
+       {\r
+               /* this command can be called during CONFIG, \r
+                * in which case jtag isn't initialized */\r
+               if (jtag)\r
+                       jtag->speed(strtoul(args[0], NULL, 0));\r
+               else\r
+                       jtag_speed = strtoul(args[0], NULL, 0);\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_endstate_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       enum tap_state state;\r
+\r
+       if (argc < 1)\r
+       {\r
                return ERROR_COMMAND_SYNTAX_ERROR;\r
-       }
-       else
-       {
-               for (state = 0; state < 16; state++)
-               {
-                       if (strcmp(args[0], tap_state_strings[state]) == 0)
-                       {
-                               jtag_add_end_state(state);
-                               jtag_execute_queue();
-                       }
-               }
-       }
-       command_print(cmd_ctx, "current endstate: %s", tap_state_strings[end_state]);
-       
-       return ERROR_OK;
-}
-
-int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       int trst = -1;
-       int srst = -1;
-       int retval;
-       
-       if (argc < 1)
-       {
+       }\r
+       else\r
+       {\r
+               for (state = 0; state < 16; state++)\r
+               {\r
+                       if (strcmp(args[0], tap_state_strings[state]) == 0)\r
+                       {\r
+                               jtag_add_end_state(state);\r
+                               jtag_execute_queue();\r
+                       }\r
+               }\r
+       }\r
+       command_print(cmd_ctx, "current endstate: %s", tap_state_strings[end_state]);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_jtag_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       int trst = -1;\r
+       int srst = -1;\r
+       int retval;\r
+       \r
+       if (argc < 1)\r
+       {\r
                return ERROR_COMMAND_SYNTAX_ERROR;\r
 \r
-       }
-
-       if (args[0][0] == '1')
-               trst = 1;
-       else if (args[0][0] == '0')
-               trst = 0;
-       else
-       {
+       }\r
+\r
+       if (args[0][0] == '1')\r
+               trst = 1;\r
+       else if (args[0][0] == '0')\r
+               trst = 0;\r
+       else\r
+       {\r
                return ERROR_COMMAND_SYNTAX_ERROR;\r
-       }
-
-       if (args[1][0] == '1')
-               srst = 1;
-       else if (args[1][0] == '0')
-               srst = 0;
-       else
-       {
+       }\r
+\r
+       if (args[1][0] == '1')\r
+               srst = 1;\r
+       else if (args[1][0] == '0')\r
+               srst = 0;\r
+       else\r
+       {\r
                return ERROR_COMMAND_SYNTAX_ERROR;\r
-       }
-
-       if ((retval = jtag_add_reset(trst, srst)) != ERROR_OK)
-       {
-               switch (retval)
-               {
-                       case ERROR_JTAG_RESET_WOULD_ASSERT_TRST:
-                               command_print(cmd_ctx, "requested reset would assert trst\nif this is acceptable, use jtag_reset 1 %c", args[1][0]);
-                               break;
-                       case ERROR_JTAG_RESET_CANT_SRST:
-                               command_print(cmd_ctx, "can't assert srst because the current reset_config doesn't support it");
-                               break;
-                       default:
-                               command_print(cmd_ctx, "unknown error");
-               }
-       }
-       jtag_execute_queue();
-
-       return ERROR_OK;
-}
-
-int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       if (argc < 1)
-       {
+       }\r
+\r
+       if ((retval = jtag_add_reset(trst, srst)) != ERROR_OK)\r
+       {\r
+               switch (retval)\r
+               {\r
+                       case ERROR_JTAG_RESET_WOULD_ASSERT_TRST:\r
+                               command_print(cmd_ctx, "requested reset would assert trst\nif this is acceptable, use jtag_reset 1 %c", args[1][0]);\r
+                               break;\r
+                       case ERROR_JTAG_RESET_CANT_SRST:\r
+                               command_print(cmd_ctx, "can't assert srst because the current reset_config doesn't support it");\r
+                               break;\r
+                       default:\r
+                               command_print(cmd_ctx, "unknown error");\r
+               }\r
+       }\r
+       jtag_execute_queue();\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_runtest_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       if (argc < 1)\r
+       {\r
                return ERROR_COMMAND_SYNTAX_ERROR;\r
-       }
-
-       jtag_add_runtest(strtol(args[0], NULL, 0), -1);
-       jtag_execute_queue();
-
-       return ERROR_OK;
-
-}
-
-int handle_statemove_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       enum tap_state state;
-
-       state = -1;
-       if (argc == 1)
-       {
-               for (state = 0; state < 16; state++)
-               {
-                       if (strcmp(args[0], tap_state_strings[state]) == 0)
-                       {
-                               break;
-                       }
-               }
-       }
-
-       jtag_add_statemove(state);
-       jtag_execute_queue();
-
-       return ERROR_OK;
-
-}
-
-int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       int i;
-       scan_field_t *fields;
-       
-       if ((argc < 2) || (argc % 2))
-       {
+       }\r
+\r
+       jtag_add_runtest(strtol(args[0], NULL, 0), -1);\r
+       jtag_execute_queue();\r
+\r
+       return ERROR_OK;\r
+\r
+}\r
+\r
+int handle_statemove_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       enum tap_state state;\r
+\r
+       state = -1;\r
+       if (argc == 1)\r
+       {\r
+               for (state = 0; state < 16; state++)\r
+               {\r
+                       if (strcmp(args[0], tap_state_strings[state]) == 0)\r
+                       {\r
+                               break;\r
+                       }\r
+               }\r
+       }\r
+\r
+       jtag_add_statemove(state);\r
+       jtag_execute_queue();\r
+\r
+       return ERROR_OK;\r
+\r
+}\r
+\r
+int handle_irscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       int i;\r
+       scan_field_t *fields;\r
+       \r
+       if ((argc < 2) || (argc % 2))\r
+       {\r
                return ERROR_COMMAND_SYNTAX_ERROR;\r
-       }
-
-       fields = malloc(sizeof(scan_field_t) * argc / 2);
-       
-       for (i = 0; i < argc / 2; i++)
-       {
-               int device = strtoul(args[i*2], NULL, 0);
-               int field_size = jtag_get_device(device)->ir_length;
-               fields[i].device = device;
-               fields[i].out_value = malloc(CEIL(field_size, 8));
-               buf_set_u32(fields[i].out_value, 0, field_size, strtoul(args[i*2+1], NULL, 0));
-               fields[i].out_mask = NULL;
-               fields[i].in_value = NULL;
-               fields[i].in_check_mask = NULL;
-               fields[i].in_handler = NULL;
-               fields[i].in_handler_priv = NULL;
-       }
-
-       jtag_add_ir_scan(argc / 2, fields, -1, NULL);
-       jtag_execute_queue();
-
-       for (i = 0; i < argc / 2; i++)
-               free(fields[i].out_value);
-
-       free (fields);
-
-       return ERROR_OK;
-}
-
-int handle_drscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       scan_field_t *fields;
-       int num_fields = 0;
-       int field_count = 0;
-       var_t *var;
-       int i, j;
-       
-       if ((argc < 2) || (argc % 2))
-       {
+       }\r
+\r
+       fields = malloc(sizeof(scan_field_t) * argc / 2);\r
+       \r
+       for (i = 0; i < argc / 2; i++)\r
+       {\r
+               int device = strtoul(args[i*2], NULL, 0);\r
+               int field_size = jtag_get_device(device)->ir_length;\r
+               fields[i].device = device;\r
+               fields[i].out_value = malloc(CEIL(field_size, 8));\r
+               buf_set_u32(fields[i].out_value, 0, field_size, strtoul(args[i*2+1], NULL, 0));\r
+               fields[i].out_mask = NULL;\r
+               fields[i].in_value = NULL;\r
+               fields[i].in_check_mask = NULL;\r
+               fields[i].in_handler = NULL;\r
+               fields[i].in_handler_priv = NULL;\r
+       }\r
+\r
+       jtag_add_ir_scan(argc / 2, fields, -1);\r
+       jtag_execute_queue();\r
+\r
+       for (i = 0; i < argc / 2; i++)\r
+               free(fields[i].out_value);\r
+\r
+       free (fields);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_drscan_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       scan_field_t *fields;\r
+       int num_fields = 0;\r
+       int field_count = 0;\r
+       var_t *var;\r
+       int i, j;\r
+       \r
+       if ((argc < 2) || (argc % 2))\r
+       {\r
                return ERROR_COMMAND_SYNTAX_ERROR;\r
-       }
-
-       for (i = 0; i < argc; i+=2)
-       {
-               var = get_var_by_namenum(args[i+1]);
-               if (var)
-               {
-                       num_fields += var->num_fields;
-               }
-               else
-               {
-                       command_print(cmd_ctx, "variable %s doesn't exist", args[i+1]);
-                       return ERROR_OK;
-               }
-       }
-
-       fields = malloc(sizeof(scan_field_t) * num_fields);
-
-       for (i = 0; i < argc; i+=2)
-       {
-               var = get_var_by_namenum(args[i+1]);
-       
-               for (j = 0; j < var->num_fields; j++)
-               {
-                       fields[field_count].device = strtol(args[i], NULL, 0);
-                       fields[field_count].num_bits = var->fields[j].num_bits;
-                       fields[field_count].out_value = malloc(CEIL(var->fields[j].num_bits, 8));
-                       buf_set_u32(fields[field_count].out_value, 0, var->fields[j].num_bits, var->fields[j].value);
-                       fields[field_count].out_mask = NULL;
-                       fields[field_count].in_value = fields[field_count].out_value;
-                       fields[field_count].in_check_mask = NULL;
-                       fields[field_count].in_check_value = NULL;
-                       fields[field_count].in_handler = field_le_to_host;
-                       fields[field_count++].in_handler_priv = &(var->fields[j]);
-               }
-       }
-
-       jtag_add_dr_scan(num_fields, fields, -1, NULL);
-       jtag_execute_queue();
-       
-       for (i = 0; i < argc / 2; i++)
-               free(fields[i].out_value);
-
-       free(fields);
-
-       return ERROR_OK;
-}
-
-int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
+       }\r
+\r
+       for (i = 0; i < argc; i+=2)\r
+       {\r
+               var = get_var_by_namenum(args[i+1]);\r
+               if (var)\r
+               {\r
+                       num_fields += var->num_fields;\r
+               }\r
+               else\r
+               {\r
+                       command_print(cmd_ctx, "variable %s doesn't exist", args[i+1]);\r
+                       return ERROR_OK;\r
+               }\r
+       }\r
+\r
+       fields = malloc(sizeof(scan_field_t) * num_fields);\r
+\r
+       for (i = 0; i < argc; i+=2)\r
+       {\r
+               var = get_var_by_namenum(args[i+1]);\r
+       \r
+               for (j = 0; j < var->num_fields; j++)\r
+               {\r
+                       fields[field_count].device = strtol(args[i], NULL, 0);\r
+                       fields[field_count].num_bits = var->fields[j].num_bits;\r
+                       fields[field_count].out_value = malloc(CEIL(var->fields[j].num_bits, 8));\r
+                       buf_set_u32(fields[field_count].out_value, 0, var->fields[j].num_bits, var->fields[j].value);\r
+                       fields[field_count].out_mask = NULL;\r
+                       fields[field_count].in_value = fields[field_count].out_value;\r
+                       fields[field_count].in_check_mask = NULL;\r
+                       fields[field_count].in_check_value = NULL;\r
+                       fields[field_count].in_handler = field_le_to_host;\r
+                       fields[field_count++].in_handler_priv = &(var->fields[j]);\r
+               }\r
+       }\r
+\r
+       jtag_add_dr_scan(num_fields, fields, -1);\r
+       jtag_execute_queue();\r
+       \r
+       for (i = 0; i < argc / 2; i++)\r
+               free(fields[i].out_value);\r
+\r
+       free(fields);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_verify_ircapture_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
        if (argc == 1)\r
-       {
-               if (strcmp(args[0], "enable") == 0)
-               {
-                       jtag_verify_capture_ir = 1;
-               }
-               else if (strcmp(args[0], "disable") == 0)
-               {
-                       jtag_verify_capture_ir = 0;
+       {\r
+               if (strcmp(args[0], "enable") == 0)\r
+               {\r
+                       jtag_verify_capture_ir = 1;\r
+               }\r
+               else if (strcmp(args[0], "disable") == 0)\r
+               {\r
+                       jtag_verify_capture_ir = 0;\r
                } else\r
                {\r
                        return ERROR_COMMAND_SYNTAX_ERROR;\r
-               }
+               }\r
        } else if (argc != 0)\r
        {\r
                return ERROR_COMMAND_SYNTAX_ERROR;\r
        }\r
        \r
        command_print(cmd_ctx, "verify Capture-IR is %s", (jtag_verify_capture_ir) ? "enabled": "disabled");\r
-       
-       return ERROR_OK;
-}
+       \r
+       return ERROR_OK;\r
+}\r
index e5df6619bf67808152c1be17bff5b72f3d0be6e8..0e48a020273781da4b6a0a3d894b2998a69f6b3d 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2005 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifndef JTAG_H
-#define JTAG_H
-
-#include "types.h"
-#include "binarybuffer.h"
-
-#include "command.h"
-
-#if 0
-#define _DEBUG_JTAG_IO_
-#endif
-
-/* Tap States
- * TLR - Test-Logic-Reset, RTI - Run-Test/Idle, 
- * SDS - Select-DR-Scan, CD - Capture-DR, SD - Shift-DR, E1D - Exit1-DR,
- * PD - Pause-DR, E2D - Exit2-DR, UD - Update-DR,
- * SIS - Select-IR-Scan, CI - Capture-IR, SI - Shift-IR, E1I - Exit1-IR,
- * PI - Pause-IR, E2I - Exit2-IR, UI - Update-IR 
- */
-enum tap_state
-{
-       TAP_TLR = 0x0, TAP_RTI = 0x8, 
-       TAP_SDS = 0x1, TAP_CD = 0x2, TAP_SD = 0x3, TAP_E1D = 0x4, 
-       TAP_PD = 0x5, TAP_E2D = 0x6, TAP_UD = 0x7,
-       TAP_SIS = 0x9, TAP_CI = 0xa, TAP_SI = 0xb, TAP_E1I = 0xc,
-       TAP_PI = 0xd, TAP_E2I = 0xe, TAP_UI = 0xf
-};
-
-typedef struct tap_transition_s
-{
-       enum tap_state high;
-       enum tap_state low;
-} tap_transition_t;
-
-extern char* tap_state_strings[16];
-extern int tap_move_map[16];   /* map 16 TAP states to 6 stable states */
-extern u8 tap_move[6][6];              /* value scanned to TMS to move from one of six stable states to another */
-extern tap_transition_t tap_transitions[16];   /* describe the TAP state diagram */
-
-extern enum tap_state end_state;               /* finish DR scans in dr_end_state */
-extern enum tap_state cur_state;               /* current TAP state */
-
-extern enum tap_state cmd_queue_end_state;             /* finish DR scans in dr_end_state */
-extern enum tap_state cmd_queue_cur_state;             /* current TAP state */
-
-#define TAP_MOVE(from, to) tap_move[tap_move_map[from]][tap_move_map[to]]
-
-typedef void * error_handler_t; /* Later on we can delete error_handler_t, but keep it for now to make patches more readable */
-
-struct scan_field_s;
-typedef int (*in_handler_t)(u8 *in_value, void *priv, struct scan_field_s *field);
-
-typedef struct scan_field_s
-{
-       int device;                     /* ordinal device number this instruction refers to */
-       int num_bits;           /* number of bits this field specifies (up to 32) */
-       u8 *out_value;          /* value to be scanned into the device */
-       u8 *out_mask;           /* only masked bits care */
-       u8 *in_value;           /* pointer to a 32-bit memory location to take data scanned out */
-       /* in_check_value/mask, in_handler_error_handler, in_handler_priv can be used by the in handler, otherwise they contain garbage  */
-       u8 *in_check_value;             /* used to validate scan results */ 
-       u8 *in_check_mask;              /* check specified bits against check_value */
-       in_handler_t in_handler;            /* process received buffer using this handler */
-       void *in_handler_priv;  /* additional information for the in_handler */
-} scan_field_t;
-
-enum scan_type
-{
-       /* IN: from device to host, OUT: from host to device */
-       SCAN_IN = 1, SCAN_OUT = 2, SCAN_IO = 3
-};
-
-typedef struct scan_command_s
-{
-       int ir_scan;    /* instruction/not data scan */
-       int num_fields;         /* number of fields in *fields array */
-       scan_field_t *fields;   /* pointer to an array of data scan fields */
-       enum tap_state end_state;       /* TAP state in which JTAG commands should finish */
-} scan_command_t;
-
-typedef struct statemove_command_s
-{
-       enum tap_state end_state;       /* TAP state in which JTAG commands should finish */
-} statemove_command_t;
-
-typedef struct pathmove_command_s
-{
-       int num_states;                         /* number of states in *path */
-       enum tap_state *path;           /* states that have to be passed */
-} pathmove_command_t;
-
-typedef struct runtest_command_s
-{
-       int num_cycles;         /* number of cycles that should be spent in Run-Test/Idle */
-       enum tap_state end_state;       /* TAP state in which JTAG commands should finish */
-} runtest_command_t;
-
-typedef struct reset_command_s
-{
-       int trst;                       /* trst/srst 0: deassert, 1: assert, -1: don't change */
-       int srst;
-} reset_command_t;
-
-typedef struct end_state_command_s
-{
-       enum tap_state end_state;       /* TAP state in which JTAG commands should finish */
-} end_state_command_t;
-
-typedef struct sleep_command_s
-{
-       u32 us;         /* number of microseconds to sleep */
-} sleep_command_t;
-
-typedef union jtag_command_container_u
-{
-       scan_command_t *scan;
-       statemove_command_t *statemove;
-       pathmove_command_t *pathmove;
-       runtest_command_t *runtest;
-       reset_command_t *reset;
-       end_state_command_t *end_state;
-       sleep_command_t *sleep;
-} jtag_command_container_t;
-
-enum jtag_command_type
-{
-       JTAG_SCAN = 1,
-       JTAG_STATEMOVE = 2, JTAG_RUNTEST = 3,
-       JTAG_RESET = 4, JTAG_END_STATE = 5,
-       JTAG_PATHMOVE = 6, JTAG_SLEEP = 7
-};
-
-typedef struct jtag_command_s
-{
-       jtag_command_container_t cmd;
-       enum jtag_command_type type;
-       struct jtag_command_s *next;
-} jtag_command_t;
-
-extern jtag_command_t *jtag_command_queue;
-
-typedef struct jtag_device_s
-{
-       int ir_length;          /* size of instruction register */
-       u8 *expected;           /* Capture-IR expected value */
-       u8 *expected_mask;      /* Capture-IR expected mask */
-       u32 idcode;                     /* device identification code */
-       u8 *cur_instr;          /* current instruction */
-       int bypass;                     /* bypass register selected */
-       struct jtag_device_s *next;
-} jtag_device_t;
-
-extern jtag_device_t *jtag_devices;
-extern int jtag_num_devices;
-extern int jtag_ir_scan_size;
-
-enum reset_line_mode
-{
-       LINE_OPEN_DRAIN = 0x0,
-       LINE_PUSH_PULL = 0x1,
-};
-
-typedef struct jtag_interface_s
-{
-       char* name;
-       
-       /* queued command execution
-        */
-       int (*execute_queue)(void);
-       
-       /* interface initalization
-        */
-       int (*speed)(int speed);
-       int (*register_commands)(struct command_context_s *cmd_ctx);
-       int (*init)(void);
-       int (*quit)(void);
-       
-} jtag_interface_t;
-
-enum jtag_event
-{
-       JTAG_SRST_ASSERTED,
-       JTAG_TRST_ASSERTED,
-       JTAG_SRST_RELEASED,
-       JTAG_TRST_RELEASED,
-};
-
-extern char* jtag_event_strings[];
-
-extern int jtag_trst;
-extern int jtag_srst;
-
-typedef struct jtag_event_callback_s
-{
-       int (*callback)(enum jtag_event event, void *priv);
-       void *priv;
-       struct jtag_event_callback_s *next;
-} jtag_event_callback_t;
-
-extern jtag_event_callback_t *jtag_event_callbacks;
-
-extern jtag_interface_t *jtag; /* global pointer to configured JTAG interface */
-extern enum tap_state end_state;
-extern enum tap_state cur_state;
-
-extern char* jtag_interface;
-extern int jtag_speed;
-
-enum reset_types
-{
-       RESET_NONE = 0x0, 
-       RESET_HAS_TRST = 0x1, 
-       RESET_HAS_SRST = 0x2, 
-       RESET_TRST_AND_SRST = 0x3, 
-       RESET_SRST_PULLS_TRST = 0x4,
-       RESET_TRST_PULLS_SRST = 0x8,
-       RESET_TRST_OPEN_DRAIN = 0x10,
-       RESET_SRST_PUSH_PULL = 0x20,
-};
-
-extern enum reset_types jtag_reset_config;
-
-/* JTAG subsystem */
-extern int jtag_init(struct command_context_s *cmd_ctx);
-extern int jtag_register_commands(struct command_context_s *cmd_ctx);
-
-/* JTAG interface, can be implemented with a software or hardware fifo */
-extern int jtag_add_ir_scan(int num_fields, scan_field_t *fields, enum tap_state endstate, void *dummy_anachronism);
-extern int jtag_add_dr_scan(int num_fields, scan_field_t *fields, enum tap_state endstate, void *dummy_anachronism);
-extern int jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, enum tap_state endstate, void *dummy_anachronism);
-extern int jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, enum tap_state endstate, void *dummy_anachronism);
-/* execute a state transition within the JTAG standard, but the exact path
- * path that is taken is undefined. Many implementations use precisely
- * 7 clocks to perform a transition, but it could be more or less
- * than that.
- *
- * The following assertions are made about certain common state moves:
- *
- * - A state move from Pause-[ID]R to Pause-[ID]R should always go through 
- *   Update-[ID]R and Capture-[ID]R before returning to Pause-[ID]R, otherwise 
- *   there's no way force a register update, if you can't go to Run-Test/Idle for 
- *   some reason.
- *
- *   - A state move from Pause-[ID]R to Shift-[ID]R must not go through 
- *   Update-[ID]R.
- *
- *   - Run-Test/Idle must not be entered unless requested, because R-T/I may have 
- *   side effects.
- */
-extern int jtag_add_statemove(enum tap_state endstate);
-/* A list of unambigious single clock state transitions, not
- * all drivers can support this, but it is required for e.g.
- * XScale and Xilinx support
- */
-extern int jtag_add_pathmove(int num_states, enum tap_state *path);
-/* cycle precisely num_cycles in the TAP_RTI state */
-extern int jtag_add_runtest(int num_cycles, enum tap_state endstate);
-extern int jtag_add_reset(int trst, int srst);
-extern int jtag_add_end_state(enum tap_state endstate);
-extern int jtag_add_sleep(u32 us);
-/*
- * For software FIFO implementations, the queued commands can be executed 
- * during this call or earlier. A sw queue might decide to push out
- * some of the jtag_add_xxx() operations once the queue is "big enough".
- * 
- * This fn will return an error code if any of the prior jtag_add_xxx() 
- * calls caused a failure, e.g. check failure. Note that it does not
- * matter if the operation was executed *before* jtag_execute_queue(),
- * jtag_execute_queue() will still return an error code. 
- * 
- * All jtag_add_xxx() calls that have in_handler!=NULL will have been
- * executed when this fn returns, but if what has been queued only 
- * clocks data out, without reading anything back, then JTAG could 
- * be running *after* jtag_execute_queue() returns. The API does 
- * not define a way to flush a hw FIFO that runs *after* 
- * jtag_execute_queue() returns. 
- * 
- * jtag_add_xxx() commands can either be executed immediately or 
- * at some time between the jtag_add_xxx() fn call and jtag_execute_queue().  
- */
-extern int jtag_execute_queue(void);
-
-/* JTAG support functions */
-extern void jtag_set_check_value(scan_field_t *field, u8 *value,  u8 *mask, error_handler_t *in_error_handler);
-extern enum scan_type jtag_scan_type(scan_command_t *cmd);
-extern int jtag_scan_size(scan_command_t *cmd);
-extern int jtag_read_buffer(u8 *buffer, scan_command_t *cmd);
-extern int jtag_build_buffer(scan_command_t *cmd, u8 **buffer);
-extern jtag_device_t* jtag_get_device(int num);
-extern void jtag_sleep(u32 us);
-extern int jtag_call_event_callbacks(enum jtag_event event);
-extern int jtag_register_event_callback(int (*callback)(enum jtag_event event, void *priv), void *priv);
-
-extern int jtag_verify_capture_ir;
-
-
-/* error codes
- * JTAG subsystem uses codes between -100 and -199 */
-
-#define ERROR_JTAG_INIT_FAILED                 (-100)
-#define ERROR_JTAG_INVALID_INTERFACE   (-101)
-#define ERROR_JTAG_NOT_IMPLEMENTED             (-102)
-#define ERROR_JTAG_TRST_ASSERTED               (-103)
-#define ERROR_JTAG_QUEUE_FAILED                        (-104)
-#define ERROR_JTAG_RESET_WOULD_ASSERT_TRST             (-105)
-#define ERROR_JTAG_RESET_CANT_SRST                             (-106)
-#define ERROR_JTAG_DEVICE_ERROR                        (-107)
-#endif /* JTAG_H */
+/***************************************************************************\r
+ *   Copyright (C) 2005 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifndef JTAG_H\r
+#define JTAG_H\r
+\r
+#include "types.h"\r
+#include "binarybuffer.h"\r
+\r
+#include "command.h"\r
+\r
+#if 0\r
+#define _DEBUG_JTAG_IO_\r
+#endif\r
+\r
+/* Tap States\r
+ * TLR - Test-Logic-Reset, RTI - Run-Test/Idle, \r
+ * SDS - Select-DR-Scan, CD - Capture-DR, SD - Shift-DR, E1D - Exit1-DR,\r
+ * PD - Pause-DR, E2D - Exit2-DR, UD - Update-DR,\r
+ * SIS - Select-IR-Scan, CI - Capture-IR, SI - Shift-IR, E1I - Exit1-IR,\r
+ * PI - Pause-IR, E2I - Exit2-IR, UI - Update-IR \r
+ */\r
+enum tap_state\r
+{\r
+       TAP_TLR = 0x0, TAP_RTI = 0x8, \r
+       TAP_SDS = 0x1, TAP_CD = 0x2, TAP_SD = 0x3, TAP_E1D = 0x4, \r
+       TAP_PD = 0x5, TAP_E2D = 0x6, TAP_UD = 0x7,\r
+       TAP_SIS = 0x9, TAP_CI = 0xa, TAP_SI = 0xb, TAP_E1I = 0xc,\r
+       TAP_PI = 0xd, TAP_E2I = 0xe, TAP_UI = 0xf\r
+};\r
+\r
+typedef struct tap_transition_s\r
+{\r
+       enum tap_state high;\r
+       enum tap_state low;\r
+} tap_transition_t;\r
+\r
+extern char* tap_state_strings[16];\r
+extern int tap_move_map[16];   /* map 16 TAP states to 6 stable states */\r
+extern u8 tap_move[6][6];              /* value scanned to TMS to move from one of six stable states to another */\r
+extern tap_transition_t tap_transitions[16];   /* describe the TAP state diagram */\r
+\r
+extern enum tap_state end_state;               /* finish DR scans in dr_end_state */\r
+extern enum tap_state cur_state;               /* current TAP state */\r
+\r
+extern enum tap_state cmd_queue_end_state;             /* finish DR scans in dr_end_state */\r
+extern enum tap_state cmd_queue_cur_state;             /* current TAP state */\r
+\r
+#define TAP_MOVE(from, to) tap_move[tap_move_map[from]][tap_move_map[to]]\r
+\r
+typedef void * error_handler_t; /* Later on we can delete error_handler_t, but keep it for now to make patches more readable */\r
+\r
+struct scan_field_s;\r
+typedef int (*in_handler_t)(u8 *in_value, void *priv, struct scan_field_s *field);\r
+\r
+typedef struct scan_field_s\r
+{\r
+       int device;                     /* ordinal device number this instruction refers to */\r
+       int num_bits;           /* number of bits this field specifies (up to 32) */\r
+       u8 *out_value;          /* value to be scanned into the device */\r
+       u8 *out_mask;           /* only masked bits care */\r
+       u8 *in_value;           /* pointer to a 32-bit memory location to take data scanned out */\r
+       /* in_check_value/mask, in_handler_error_handler, in_handler_priv can be used by the in handler, otherwise they contain garbage  */\r
+       u8 *in_check_value;             /* used to validate scan results */ \r
+       u8 *in_check_mask;              /* check specified bits against check_value */\r
+       in_handler_t in_handler;            /* process received buffer using this handler */\r
+       void *in_handler_priv;  /* additional information for the in_handler */\r
+} scan_field_t;\r
+\r
+enum scan_type\r
+{\r
+       /* IN: from device to host, OUT: from host to device */\r
+       SCAN_IN = 1, SCAN_OUT = 2, SCAN_IO = 3\r
+};\r
+\r
+typedef struct scan_command_s\r
+{\r
+       int ir_scan;    /* instruction/not data scan */\r
+       int num_fields;         /* number of fields in *fields array */\r
+       scan_field_t *fields;   /* pointer to an array of data scan fields */\r
+       enum tap_state end_state;       /* TAP state in which JTAG commands should finish */\r
+} scan_command_t;\r
+\r
+typedef struct statemove_command_s\r
+{\r
+       enum tap_state end_state;       /* TAP state in which JTAG commands should finish */\r
+} statemove_command_t;\r
+\r
+typedef struct pathmove_command_s\r
+{\r
+       int num_states;                         /* number of states in *path */\r
+       enum tap_state *path;           /* states that have to be passed */\r
+} pathmove_command_t;\r
+\r
+typedef struct runtest_command_s\r
+{\r
+       int num_cycles;         /* number of cycles that should be spent in Run-Test/Idle */\r
+       enum tap_state end_state;       /* TAP state in which JTAG commands should finish */\r
+} runtest_command_t;\r
+\r
+typedef struct reset_command_s\r
+{\r
+       int trst;                       /* trst/srst 0: deassert, 1: assert, -1: don't change */\r
+       int srst;\r
+} reset_command_t;\r
+\r
+typedef struct end_state_command_s\r
+{\r
+       enum tap_state end_state;       /* TAP state in which JTAG commands should finish */\r
+} end_state_command_t;\r
+\r
+typedef struct sleep_command_s\r
+{\r
+       u32 us;         /* number of microseconds to sleep */\r
+} sleep_command_t;\r
+\r
+typedef union jtag_command_container_u\r
+{\r
+       scan_command_t *scan;\r
+       statemove_command_t *statemove;\r
+       pathmove_command_t *pathmove;\r
+       runtest_command_t *runtest;\r
+       reset_command_t *reset;\r
+       end_state_command_t *end_state;\r
+       sleep_command_t *sleep;\r
+} jtag_command_container_t;\r
+\r
+enum jtag_command_type\r
+{\r
+       JTAG_SCAN = 1,\r
+       JTAG_STATEMOVE = 2, JTAG_RUNTEST = 3,\r
+       JTAG_RESET = 4, JTAG_END_STATE = 5,\r
+       JTAG_PATHMOVE = 6, JTAG_SLEEP = 7\r
+};\r
+\r
+typedef struct jtag_command_s\r
+{\r
+       jtag_command_container_t cmd;\r
+       enum jtag_command_type type;\r
+       struct jtag_command_s *next;\r
+} jtag_command_t;\r
+\r
+extern jtag_command_t *jtag_command_queue;\r
+\r
+typedef struct jtag_device_s\r
+{\r
+       int ir_length;          /* size of instruction register */\r
+       u8 *expected;           /* Capture-IR expected value */\r
+       u8 *expected_mask;      /* Capture-IR expected mask */\r
+       u32 idcode;                     /* device identification code */\r
+       u8 *cur_instr;          /* current instruction */\r
+       int bypass;                     /* bypass register selected */\r
+       struct jtag_device_s *next;\r
+} jtag_device_t;\r
+\r
+extern jtag_device_t *jtag_devices;\r
+extern int jtag_num_devices;\r
+extern int jtag_ir_scan_size;\r
+\r
+enum reset_line_mode\r
+{\r
+       LINE_OPEN_DRAIN = 0x0,\r
+       LINE_PUSH_PULL = 0x1,\r
+};\r
+\r
+typedef struct jtag_interface_s\r
+{\r
+       char* name;\r
+       \r
+       /* queued command execution\r
+        */\r
+       int (*execute_queue)(void);\r
+       \r
+       /* interface initalization\r
+        */\r
+       int (*speed)(int speed);\r
+       int (*register_commands)(struct command_context_s *cmd_ctx);\r
+       int (*init)(void);\r
+       int (*quit)(void);\r
+       \r
+} jtag_interface_t;\r
+\r
+enum jtag_event\r
+{\r
+       JTAG_SRST_ASSERTED,\r
+       JTAG_TRST_ASSERTED,\r
+       JTAG_SRST_RELEASED,\r
+       JTAG_TRST_RELEASED,\r
+};\r
+\r
+extern char* jtag_event_strings[];\r
+\r
+extern int jtag_trst;\r
+extern int jtag_srst;\r
+\r
+typedef struct jtag_event_callback_s\r
+{\r
+       int (*callback)(enum jtag_event event, void *priv);\r
+       void *priv;\r
+       struct jtag_event_callback_s *next;\r
+} jtag_event_callback_t;\r
+\r
+extern jtag_event_callback_t *jtag_event_callbacks;\r
+\r
+extern jtag_interface_t *jtag; /* global pointer to configured JTAG interface */\r
+extern enum tap_state end_state;\r
+extern enum tap_state cur_state;\r
+\r
+extern char* jtag_interface;\r
+extern int jtag_speed;\r
+\r
+enum reset_types\r
+{\r
+       RESET_NONE = 0x0, \r
+       RESET_HAS_TRST = 0x1, \r
+       RESET_HAS_SRST = 0x2, \r
+       RESET_TRST_AND_SRST = 0x3, \r
+       RESET_SRST_PULLS_TRST = 0x4,\r
+       RESET_TRST_PULLS_SRST = 0x8,\r
+       RESET_TRST_OPEN_DRAIN = 0x10,\r
+       RESET_SRST_PUSH_PULL = 0x20,\r
+};\r
+\r
+extern enum reset_types jtag_reset_config;\r
+\r
+/* JTAG subsystem */\r
+extern int jtag_init(struct command_context_s *cmd_ctx);\r
+extern int jtag_register_commands(struct command_context_s *cmd_ctx);\r
+\r
+/* JTAG interface, can be implemented with a software or hardware fifo */\r
+extern int jtag_add_ir_scan(int num_fields, scan_field_t *fields, enum tap_state endstate);
+extern int jtag_add_dr_scan(int num_fields, scan_field_t *fields, enum tap_state endstate);
+extern int jtag_add_plain_ir_scan(int num_fields, scan_field_t *fields, enum tap_state endstate);
+extern int jtag_add_plain_dr_scan(int num_fields, scan_field_t *fields, enum tap_state endstate);
+/* execute a state transition within the JTAG standard, but the exact path\r
+ * path that is taken is undefined. Many implementations use precisely\r
+ * 7 clocks to perform a transition, but it could be more or less\r
+ * than that.\r
+ *\r
+ * The following assertions are made about certain common state moves:\r
+ *\r
+ * - A state move from Pause-[ID]R to Pause-[ID]R should always go through \r
+ *   Update-[ID]R and Capture-[ID]R before returning to Pause-[ID]R, otherwise \r
+ *   there's no way force a register update, if you can't go to Run-Test/Idle for \r
+ *   some reason.\r
+ *\r
+ *   - A state move from Pause-[ID]R to Shift-[ID]R must not go through \r
+ *   Update-[ID]R.\r
+ *\r
+ *   - Run-Test/Idle must not be entered unless requested, because R-T/I may have \r
+ *   side effects.\r
+ */\r
+extern int jtag_add_statemove(enum tap_state endstate);\r
+/* A list of unambigious single clock state transitions, not\r
+ * all drivers can support this, but it is required for e.g.\r
+ * XScale and Xilinx support\r
+ */\r
+extern int jtag_add_pathmove(int num_states, enum tap_state *path);\r
+/* cycle precisely num_cycles in the TAP_RTI state */\r
+extern int jtag_add_runtest(int num_cycles, enum tap_state endstate);\r
+extern int jtag_add_reset(int trst, int srst);\r
+extern int jtag_add_end_state(enum tap_state endstate);\r
+extern int jtag_add_sleep(u32 us);\r
+/*\r
+ * For software FIFO implementations, the queued commands can be executed \r
+ * during this call or earlier. A sw queue might decide to push out\r
+ * some of the jtag_add_xxx() operations once the queue is "big enough".\r
+ * \r
+ * This fn will return an error code if any of the prior jtag_add_xxx() \r
+ * calls caused a failure, e.g. check failure. Note that it does not\r
+ * matter if the operation was executed *before* jtag_execute_queue(),\r
+ * jtag_execute_queue() will still return an error code. \r
+ * \r
+ * All jtag_add_xxx() calls that have in_handler!=NULL will have been\r
+ * executed when this fn returns, but if what has been queued only \r
+ * clocks data out, without reading anything back, then JTAG could \r
+ * be running *after* jtag_execute_queue() returns. The API does \r
+ * not define a way to flush a hw FIFO that runs *after* \r
+ * jtag_execute_queue() returns. \r
+ * \r
+ * jtag_add_xxx() commands can either be executed immediately or \r
+ * at some time between the jtag_add_xxx() fn call and jtag_execute_queue().  \r
+ */\r
+extern int jtag_execute_queue(void);\r
+\r
+/* JTAG support functions */\r
+extern void jtag_set_check_value(scan_field_t *field, u8 *value,  u8 *mask, error_handler_t *in_error_handler);\r
+extern enum scan_type jtag_scan_type(scan_command_t *cmd);\r
+extern int jtag_scan_size(scan_command_t *cmd);\r
+extern int jtag_read_buffer(u8 *buffer, scan_command_t *cmd);\r
+extern int jtag_build_buffer(scan_command_t *cmd, u8 **buffer);\r
+extern jtag_device_t* jtag_get_device(int num);\r
+extern void jtag_sleep(u32 us);\r
+extern int jtag_call_event_callbacks(enum jtag_event event);\r
+extern int jtag_register_event_callback(int (*callback)(enum jtag_event event, void *priv), void *priv);\r
+\r
+extern int jtag_verify_capture_ir;\r
+\r
+\r
+/* error codes\r
+ * JTAG subsystem uses codes between -100 and -199 */\r
+\r
+#define ERROR_JTAG_INIT_FAILED                 (-100)\r
+#define ERROR_JTAG_INVALID_INTERFACE   (-101)\r
+#define ERROR_JTAG_NOT_IMPLEMENTED             (-102)\r
+#define ERROR_JTAG_TRST_ASSERTED               (-103)\r
+#define ERROR_JTAG_QUEUE_FAILED                        (-104)\r
+#define ERROR_JTAG_RESET_WOULD_ASSERT_TRST             (-105)\r
+#define ERROR_JTAG_RESET_CANT_SRST                             (-106)\r
+#define ERROR_JTAG_DEVICE_ERROR                        (-107)\r
+#endif /* JTAG_H */\r
index a31c37d94f1be30545c7c3ba9929f9f29c51fcfe..e0cd1a11aeb7081d5e523f17a190d05554fe0fa7 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2006 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "virtex2.h"
-
-#include "pld.h"
-#include "xilinx_bit.h"
-#include "command.h"
-#include "log.h"
-#include "jtag.h"
-
-#include <stdlib.h>
-
-int virtex2_register_commands(struct command_context_s *cmd_ctx);
-int virtex2_pld_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct pld_device_s *pld_device);
-int virtex2_load(struct pld_device_s *pld_device, char *filename);
-
-pld_driver_t virtex2_pld =
-{
-       .name = "virtex2",
-       .register_commands = virtex2_register_commands,
-       .pld_device_command = virtex2_pld_device_command,
-       .load = virtex2_load,
-};
-
-int virtex2_set_instr(int chain_pos, u32 new_instr)
-{
-       jtag_device_t *device = jtag_get_device(chain_pos);
-       
-       if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)
-       {
-               scan_field_t field;
-       
-               field.device = chain_pos;
-               field.num_bits = device->ir_length;
-               field.out_value = calloc(CEIL(field.num_bits, 8), 1);
-               buf_set_u32(field.out_value, 0, field.num_bits, new_instr);
-               field.out_mask = NULL;
-               field.in_value = NULL;
-               field.in_check_value = NULL;
-               field.in_check_mask = NULL;
-               field.in_handler = NULL;
-               field.in_handler_priv = NULL;
-               
-               jtag_add_ir_scan(1, &field, TAP_RTI, NULL);
-               
-               free(field.out_value);
-       }
-       
-       return ERROR_OK;
-}
-
-int virtex2_send_32(struct pld_device_s *pld_device, int num_words, u32 *words)
-{
-       virtex2_pld_device_t *virtex2_info = pld_device->driver_priv;
-       scan_field_t scan_field;
-       u8 *values;
-       int i;
-       
-       values = malloc(num_words * 4);
-
-       scan_field.device = virtex2_info->chain_pos;
-       scan_field.num_bits = num_words * 32;
-       scan_field.out_value = values;
-       scan_field.out_mask = NULL;
-       scan_field.in_value = NULL;
-       scan_field.in_check_value = NULL;
-       scan_field.in_check_mask = NULL;
-       scan_field.in_handler = NULL;
-       scan_field.in_handler_priv = NULL;
-       
-       for (i = 0; i < num_words; i++)
-               buf_set_u32(values + 4 * i, 0, 32, flip_u32(*words++, 32));
-       
-       virtex2_set_instr(virtex2_info->chain_pos, 0x5); /* CFG_IN */
-       
-       jtag_add_dr_scan(1, &scan_field, TAP_PD, NULL);
-       
-       free(values);
-       
-       return ERROR_OK;
-}
-
-int virtex2_jtag_buf_to_u32(u8 *in_buf, void *priv, struct scan_field_s *field)
-{
-       u32 *dest = priv;
-       *dest = flip_u32(le_to_h_u32(in_buf), 32);
-       return ERROR_OK;
-}
-
-int virtex2_receive_32(struct pld_device_s *pld_device, int num_words, u32 *words)
-{
-       virtex2_pld_device_t *virtex2_info = pld_device->driver_priv;
-       scan_field_t scan_field;
-       
-       scan_field.device = virtex2_info->chain_pos;
-       scan_field.num_bits = 32;
-       scan_field.out_value = NULL;
-       scan_field.out_mask = NULL;
-       scan_field.in_value = NULL;
-       scan_field.in_check_value = NULL;
-       scan_field.in_check_mask = NULL;
-       scan_field.in_handler = virtex2_jtag_buf_to_u32;
-       
-       virtex2_set_instr(virtex2_info->chain_pos, 0x4); /* CFG_OUT */
-       
-       while (num_words--)
-       {
-               scan_field.in_handler_priv = words++;
-               jtag_add_dr_scan(1, &scan_field, TAP_PD, NULL);
-       }
-       
-       return ERROR_OK;
-}
-
-int virtex2_read_stat(struct pld_device_s *pld_device, u32 *status)
-{
-       u32 data[5];
-       
-       jtag_add_statemove(TAP_TLR);
-       
-       data[0] = 0xaa995566; /* synch word */
-       data[1] = 0x2800E001; /* Type 1, read, address 7, 1 word */
-       data[2] = 0x20000000; /* NOOP (Type 1, read, address 0, 0 words */
-       data[3] = 0x20000000; /* NOOP */
-       data[4] = 0x20000000; /* NOOP */
-       virtex2_send_32(pld_device, 5, data);
-       
-       virtex2_receive_32(pld_device, 1, status);
-       
-       jtag_execute_queue();
-       
-       DEBUG("status: 0x%8.8x", *status);
-       
-       return ERROR_OK;
-}
-
-int virtex2_load(struct pld_device_s *pld_device, char *filename)
-{
-       virtex2_pld_device_t *virtex2_info = pld_device->driver_priv;
-       xilinx_bit_file_t bit_file;
-       int retval;
-       int i;
-
-       scan_field_t field;
-
-       field.device = virtex2_info->chain_pos;
-       field.out_mask = NULL;
-       field.in_value = NULL;
-       field.in_check_value = NULL;
-       field.in_check_mask = NULL;
-       field.in_handler = NULL;
-       field.in_handler_priv = NULL;
-       
-       if ((retval = xilinx_read_bit_file(&bit_file, filename)) != ERROR_OK)
-               return retval;
-       
-       jtag_add_end_state(TAP_RTI);
-       virtex2_set_instr(virtex2_info->chain_pos, 0xb); /* JPROG_B */
-       jtag_execute_queue();
-       jtag_add_sleep(1000);
-       
-       virtex2_set_instr(virtex2_info->chain_pos, 0x5); /* CFG_IN */
-       jtag_execute_queue();
-       
-       for (i = 0; i < bit_file.length; i++)
-               bit_file.data[i] = flip_u32(bit_file.data[i], 8);
-       
-       field.num_bits = bit_file.length * 8;
-       field.out_value = bit_file.data;
-
-       jtag_add_dr_scan(1, &field, TAP_PD, NULL);
-       jtag_execute_queue();
-       
-       jtag_add_statemove(TAP_TLR);
-       
-       jtag_add_end_state(TAP_RTI);
-       virtex2_set_instr(virtex2_info->chain_pos, 0xc); /* JSTART */
-       jtag_add_runtest(13, TAP_RTI);
-       virtex2_set_instr(virtex2_info->chain_pos, 0x3f); /* BYPASS */
-       virtex2_set_instr(virtex2_info->chain_pos, 0x3f); /* BYPASS */
-       virtex2_set_instr(virtex2_info->chain_pos, 0xc); /* JSTART */
-       jtag_add_runtest(13, TAP_RTI);
-       virtex2_set_instr(virtex2_info->chain_pos, 0x3f); /* BYPASS */
-       jtag_execute_queue();
-
-       return ERROR_OK;
-}
-
-int virtex2_handle_read_stat_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       pld_device_t *device;
-       virtex2_pld_device_t *virtex2_info;
-       u32 status;
-       
-       if (argc < 1)
-       {
-               command_print(cmd_ctx, "usage: virtex2 read_stat <num>");
-               return ERROR_OK;
-       }
-       
-       device = get_pld_device_by_num(strtoul(args[0], NULL, 0));
-       if (!device)
-       {
-               command_print(cmd_ctx, "pld device '#%s' is out of bounds", args[0]);
-               return ERROR_OK;
-       }
-
-       virtex2_info = device->driver_priv;
-       
-       virtex2_read_stat(device, &status);
-       
-       command_print(cmd_ctx, "virtex2 status register: 0x%8.8x", status);
-       
-       return ERROR_OK;
-}
-
-int virtex2_register_commands(struct command_context_s *cmd_ctx)
-{
-       command_t *virtex2_cmd = register_command(cmd_ctx, NULL, "virtex2", NULL, COMMAND_ANY, "virtex2 specific commands");
-
-       register_command(cmd_ctx, virtex2_cmd, "read_stat", virtex2_handle_read_stat_command, COMMAND_EXEC,
-                                        "read Virtex-II status register");
-
-       return ERROR_OK;
-}
-
-int virtex2_pld_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct pld_device_s *pld_device)
-{
-       virtex2_pld_device_t *virtex2_info;
-       
-       if (argc < 2)
-       {
-               WARNING("incomplete pld device 'virtex2' configuration");
-               return ERROR_PLD_DEVICE_INVALID;
-       }
-       
-       virtex2_info = malloc(sizeof(virtex2_pld_device_t));
-       pld_device->driver_priv = virtex2_info;
-       
-       virtex2_info->chain_pos = strtoul(args[1], NULL, 0);
-       
-       return ERROR_OK;
-}
+/***************************************************************************\r
+ *   Copyright (C) 2006 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "virtex2.h"\r
+\r
+#include "pld.h"\r
+#include "xilinx_bit.h"\r
+#include "command.h"\r
+#include "log.h"\r
+#include "jtag.h"\r
+\r
+#include <stdlib.h>\r
+\r
+int virtex2_register_commands(struct command_context_s *cmd_ctx);\r
+int virtex2_pld_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct pld_device_s *pld_device);\r
+int virtex2_load(struct pld_device_s *pld_device, char *filename);\r
+\r
+pld_driver_t virtex2_pld =\r
+{\r
+       .name = "virtex2",\r
+       .register_commands = virtex2_register_commands,\r
+       .pld_device_command = virtex2_pld_device_command,\r
+       .load = virtex2_load,\r
+};\r
+\r
+int virtex2_set_instr(int chain_pos, u32 new_instr)\r
+{\r
+       jtag_device_t *device = jtag_get_device(chain_pos);\r
+       \r
+       if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)\r
+       {\r
+               scan_field_t field;\r
+       \r
+               field.device = chain_pos;\r
+               field.num_bits = device->ir_length;\r
+               field.out_value = calloc(CEIL(field.num_bits, 8), 1);\r
+               buf_set_u32(field.out_value, 0, field.num_bits, new_instr);\r
+               field.out_mask = NULL;\r
+               field.in_value = NULL;\r
+               field.in_check_value = NULL;\r
+               field.in_check_mask = NULL;\r
+               field.in_handler = NULL;\r
+               field.in_handler_priv = NULL;\r
+               \r
+               jtag_add_ir_scan(1, &field, TAP_RTI);\r
+               \r
+               free(field.out_value);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int virtex2_send_32(struct pld_device_s *pld_device, int num_words, u32 *words)\r
+{\r
+       virtex2_pld_device_t *virtex2_info = pld_device->driver_priv;\r
+       scan_field_t scan_field;\r
+       u8 *values;\r
+       int i;\r
+       \r
+       values = malloc(num_words * 4);\r
+\r
+       scan_field.device = virtex2_info->chain_pos;\r
+       scan_field.num_bits = num_words * 32;\r
+       scan_field.out_value = values;\r
+       scan_field.out_mask = NULL;\r
+       scan_field.in_value = NULL;\r
+       scan_field.in_check_value = NULL;\r
+       scan_field.in_check_mask = NULL;\r
+       scan_field.in_handler = NULL;\r
+       scan_field.in_handler_priv = NULL;\r
+       \r
+       for (i = 0; i < num_words; i++)\r
+               buf_set_u32(values + 4 * i, 0, 32, flip_u32(*words++, 32));\r
+       \r
+       virtex2_set_instr(virtex2_info->chain_pos, 0x5); /* CFG_IN */\r
+       \r
+       jtag_add_dr_scan(1, &scan_field, TAP_PD);\r
+       \r
+       free(values);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int virtex2_jtag_buf_to_u32(u8 *in_buf, void *priv, struct scan_field_s *field)\r
+{\r
+       u32 *dest = priv;\r
+       *dest = flip_u32(le_to_h_u32(in_buf), 32);\r
+       return ERROR_OK;\r
+}\r
+\r
+int virtex2_receive_32(struct pld_device_s *pld_device, int num_words, u32 *words)\r
+{\r
+       virtex2_pld_device_t *virtex2_info = pld_device->driver_priv;\r
+       scan_field_t scan_field;\r
+       \r
+       scan_field.device = virtex2_info->chain_pos;\r
+       scan_field.num_bits = 32;\r
+       scan_field.out_value = NULL;\r
+       scan_field.out_mask = NULL;\r
+       scan_field.in_value = NULL;\r
+       scan_field.in_check_value = NULL;\r
+       scan_field.in_check_mask = NULL;\r
+       scan_field.in_handler = virtex2_jtag_buf_to_u32;\r
+       \r
+       virtex2_set_instr(virtex2_info->chain_pos, 0x4); /* CFG_OUT */\r
+       \r
+       while (num_words--)\r
+       {\r
+               scan_field.in_handler_priv = words++;\r
+               jtag_add_dr_scan(1, &scan_field, TAP_PD);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int virtex2_read_stat(struct pld_device_s *pld_device, u32 *status)\r
+{\r
+       u32 data[5];\r
+       \r
+       jtag_add_statemove(TAP_TLR);\r
+       \r
+       data[0] = 0xaa995566; /* synch word */\r
+       data[1] = 0x2800E001; /* Type 1, read, address 7, 1 word */\r
+       data[2] = 0x20000000; /* NOOP (Type 1, read, address 0, 0 words */\r
+       data[3] = 0x20000000; /* NOOP */\r
+       data[4] = 0x20000000; /* NOOP */\r
+       virtex2_send_32(pld_device, 5, data);\r
+       \r
+       virtex2_receive_32(pld_device, 1, status);\r
+       \r
+       jtag_execute_queue();\r
+       \r
+       DEBUG("status: 0x%8.8x", *status);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int virtex2_load(struct pld_device_s *pld_device, char *filename)\r
+{\r
+       virtex2_pld_device_t *virtex2_info = pld_device->driver_priv;\r
+       xilinx_bit_file_t bit_file;\r
+       int retval;\r
+       int i;\r
+\r
+       scan_field_t field;\r
+\r
+       field.device = virtex2_info->chain_pos;\r
+       field.out_mask = NULL;\r
+       field.in_value = NULL;\r
+       field.in_check_value = NULL;\r
+       field.in_check_mask = NULL;\r
+       field.in_handler = NULL;\r
+       field.in_handler_priv = NULL;\r
+       \r
+       if ((retval = xilinx_read_bit_file(&bit_file, filename)) != ERROR_OK)\r
+               return retval;\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       virtex2_set_instr(virtex2_info->chain_pos, 0xb); /* JPROG_B */\r
+       jtag_execute_queue();\r
+       jtag_add_sleep(1000);\r
+       \r
+       virtex2_set_instr(virtex2_info->chain_pos, 0x5); /* CFG_IN */\r
+       jtag_execute_queue();\r
+       \r
+       for (i = 0; i < bit_file.length; i++)\r
+               bit_file.data[i] = flip_u32(bit_file.data[i], 8);\r
+       \r
+       field.num_bits = bit_file.length * 8;\r
+       field.out_value = bit_file.data;\r
+\r
+       jtag_add_dr_scan(1, &field, TAP_PD);\r
+       jtag_execute_queue();\r
+       \r
+       jtag_add_statemove(TAP_TLR);\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       virtex2_set_instr(virtex2_info->chain_pos, 0xc); /* JSTART */\r
+       jtag_add_runtest(13, TAP_RTI);\r
+       virtex2_set_instr(virtex2_info->chain_pos, 0x3f); /* BYPASS */\r
+       virtex2_set_instr(virtex2_info->chain_pos, 0x3f); /* BYPASS */\r
+       virtex2_set_instr(virtex2_info->chain_pos, 0xc); /* JSTART */\r
+       jtag_add_runtest(13, TAP_RTI);\r
+       virtex2_set_instr(virtex2_info->chain_pos, 0x3f); /* BYPASS */\r
+       jtag_execute_queue();\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int virtex2_handle_read_stat_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       pld_device_t *device;\r
+       virtex2_pld_device_t *virtex2_info;\r
+       u32 status;\r
+       \r
+       if (argc < 1)\r
+       {\r
+               command_print(cmd_ctx, "usage: virtex2 read_stat <num>");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       device = get_pld_device_by_num(strtoul(args[0], NULL, 0));\r
+       if (!device)\r
+       {\r
+               command_print(cmd_ctx, "pld device '#%s' is out of bounds", args[0]);\r
+               return ERROR_OK;\r
+       }\r
+\r
+       virtex2_info = device->driver_priv;\r
+       \r
+       virtex2_read_stat(device, &status);\r
+       \r
+       command_print(cmd_ctx, "virtex2 status register: 0x%8.8x", status);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int virtex2_register_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       command_t *virtex2_cmd = register_command(cmd_ctx, NULL, "virtex2", NULL, COMMAND_ANY, "virtex2 specific commands");\r
+\r
+       register_command(cmd_ctx, virtex2_cmd, "read_stat", virtex2_handle_read_stat_command, COMMAND_EXEC,\r
+                                        "read Virtex-II status register");\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int virtex2_pld_device_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct pld_device_s *pld_device)\r
+{\r
+       virtex2_pld_device_t *virtex2_info;\r
+       \r
+       if (argc < 2)\r
+       {\r
+               WARNING("incomplete pld device 'virtex2' configuration");\r
+               return ERROR_PLD_DEVICE_INVALID;\r
+       }\r
+       \r
+       virtex2_info = malloc(sizeof(virtex2_pld_device_t));\r
+       pld_device->driver_priv = virtex2_info;\r
+       \r
+       virtex2_info->chain_pos = strtoul(args[1], NULL, 0);\r
+       \r
+       return ERROR_OK;\r
+}\r
diff --git a/src/target/arm11.c b/src/target/arm11.c
new file mode 100644 (file)
index 0000000..11e376a
--- /dev/null
@@ -0,0 +1,1358 @@
+/***************************************************************************\r
+ *   Copyright (C) 2008 digenius technology GmbH.                          *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "arm11.h"\r
+#include "jtag.h"\r
+#include "log.h"\r
+\r
+#include <stdlib.h>\r
+#include <string.h>\r
+\r
+#if 0\r
+#define _DEBUG_INSTRUCTION_EXECUTION_\r
+#endif\r
+\r
+\r
+#if 0\r
+#define FNC_INFO    DEBUG("-")\r
+#else\r
+#define FNC_INFO\r
+#endif\r
+\r
+#if 1\r
+#define FNC_INFO_NOTIMPLEMENTED    do { DEBUG("NOT IMPLEMENTED"); /*exit(-1);*/ } while (0)\r
+#else\r
+#define FNC_INFO_NOTIMPLEMENTED\r
+#endif\r
+\r
+static void arm11_on_enter_debug_state(arm11_common_t * arm11);\r
+\r
+\r
+#define ARM11_HANDLER(x)       \\r
+    .x                         = arm11_##x\r
+\r
+target_type_t arm11_target =\r
+{\r
+    .name                      = "arm11",\r
+\r
+    ARM11_HANDLER(poll),\r
+    ARM11_HANDLER(arch_state),\r
+\r
+    ARM11_HANDLER(target_request_data),\r
+\r
+    ARM11_HANDLER(halt),\r
+    ARM11_HANDLER(resume),\r
+    ARM11_HANDLER(step),\r
+\r
+    ARM11_HANDLER(assert_reset),\r
+    ARM11_HANDLER(deassert_reset),\r
+    ARM11_HANDLER(soft_reset_halt),\r
+    ARM11_HANDLER(prepare_reset_halt),\r
+       \r
+    ARM11_HANDLER(get_gdb_reg_list),\r
+       \r
+    ARM11_HANDLER(read_memory),\r
+    ARM11_HANDLER(write_memory),\r
+       \r
+    ARM11_HANDLER(bulk_write_memory),\r
+       \r
+    ARM11_HANDLER(checksum_memory),\r
+\r
+    ARM11_HANDLER(add_breakpoint),\r
+    ARM11_HANDLER(remove_breakpoint),\r
+    ARM11_HANDLER(add_watchpoint),\r
+    ARM11_HANDLER(remove_watchpoint),\r
+\r
+    ARM11_HANDLER(run_algorithm),\r
+       \r
+    ARM11_HANDLER(register_commands),\r
+    ARM11_HANDLER(target_command),\r
+    ARM11_HANDLER(init_target),\r
+    ARM11_HANDLER(quit),\r
+};\r
+\r
+int arm11_regs_arch_type = -1;\r
+\r
+\r
+enum arm11_regtype\r
+{\r
+    ARM11_REGISTER_CORE,\r
+    ARM11_REGISTER_CPSR,\r
+\r
+    ARM11_REGISTER_FX,\r
+    ARM11_REGISTER_FPS,\r
+\r
+    ARM11_REGISTER_FIQ,\r
+    ARM11_REGISTER_SVC,\r
+    ARM11_REGISTER_ABT,\r
+    ARM11_REGISTER_IRQ,\r
+    ARM11_REGISTER_UND,\r
+    ARM11_REGISTER_MON,\r
+\r
+    ARM11_REGISTER_SPSR_FIQ,\r
+    ARM11_REGISTER_SPSR_SVC,\r
+    ARM11_REGISTER_SPSR_ABT,\r
+    ARM11_REGISTER_SPSR_IRQ,\r
+    ARM11_REGISTER_SPSR_UND,\r
+    ARM11_REGISTER_SPSR_MON,\r
+\r
+    /* debug regs */\r
+    ARM11_REGISTER_DSCR,\r
+    ARM11_REGISTER_WDTR,\r
+    ARM11_REGISTER_RDTR,\r
+};\r
+\r
+\r
+typedef struct arm11_reg_defs_s\r
+{\r
+    char *                     name;\r
+    u32                                num;\r
+    int                                gdb_num;\r
+    enum arm11_regtype         type;\r
+} arm11_reg_defs_t;\r
+\r
+/* update arm11_regcache_ids when changing this */\r
+static const arm11_reg_defs_t arm11_reg_defs[] =\r
+{\r
+    {"r0",     0,      0,      ARM11_REGISTER_CORE},\r
+    {"r1",     1,      1,      ARM11_REGISTER_CORE},\r
+    {"r2",     2,      2,      ARM11_REGISTER_CORE},\r
+    {"r3",     3,      3,      ARM11_REGISTER_CORE},\r
+    {"r4",     4,      4,      ARM11_REGISTER_CORE},\r
+    {"r5",     5,      5,      ARM11_REGISTER_CORE},\r
+    {"r6",     6,      6,      ARM11_REGISTER_CORE},\r
+    {"r7",     7,      7,      ARM11_REGISTER_CORE},\r
+    {"r8",     8,      8,      ARM11_REGISTER_CORE},\r
+    {"r9",     9,      9,      ARM11_REGISTER_CORE},\r
+    {"r10",    10,     10,     ARM11_REGISTER_CORE},\r
+    {"r11",    11,     11,     ARM11_REGISTER_CORE},\r
+    {"r12",    12,     12,     ARM11_REGISTER_CORE},\r
+    {"sp",     13,     13,     ARM11_REGISTER_CORE},\r
+    {"lr",     14,     14,     ARM11_REGISTER_CORE},\r
+    {"pc",     15,     15,     ARM11_REGISTER_CORE},\r
+\r
+#if ARM11_REGCACHE_FREGS\r
+    {"f0",     0,      16,     ARM11_REGISTER_FX},\r
+    {"f1",     1,      17,     ARM11_REGISTER_FX},\r
+    {"f2",     2,      18,     ARM11_REGISTER_FX},\r
+    {"f3",     3,      19,     ARM11_REGISTER_FX},\r
+    {"f4",     4,      20,     ARM11_REGISTER_FX},\r
+    {"f5",     5,      21,     ARM11_REGISTER_FX},\r
+    {"f6",     6,      22,     ARM11_REGISTER_FX},\r
+    {"f7",     7,      23,     ARM11_REGISTER_FX},\r
+    {"fps",    0,      24,     ARM11_REGISTER_FPS},\r
+#endif\r
+\r
+    {"cpsr",   0,      25,     ARM11_REGISTER_CPSR},\r
+\r
+#if ARM11_REGCACHE_MODEREGS\r
+    {"r8_fiq", 8,      -1,     ARM11_REGISTER_FIQ},\r
+    {"r9_fiq", 9,      -1,     ARM11_REGISTER_FIQ},\r
+    {"r10_fiq",        10,     -1,     ARM11_REGISTER_FIQ},\r
+    {"r11_fiq",        11,     -1,     ARM11_REGISTER_FIQ},\r
+    {"r12_fiq",        12,     -1,     ARM11_REGISTER_FIQ},\r
+    {"r13_fiq",        13,     -1,     ARM11_REGISTER_FIQ},\r
+    {"r14_fiq",        14,     -1,     ARM11_REGISTER_FIQ},\r
+    {"spsr_fiq", 0,    -1,     ARM11_REGISTER_SPSR_FIQ},\r
+\r
+    {"r13_svc",        13,     -1,     ARM11_REGISTER_SVC},\r
+    {"r14_svc",        14,     -1,     ARM11_REGISTER_SVC},\r
+    {"spsr_svc", 0,    -1,     ARM11_REGISTER_SPSR_SVC},\r
+\r
+    {"r13_abt",        13,     -1,     ARM11_REGISTER_ABT},\r
+    {"r14_abt",        14,     -1,     ARM11_REGISTER_ABT},\r
+    {"spsr_abt", 0,    -1,     ARM11_REGISTER_SPSR_ABT},\r
+\r
+    {"r13_irq",        13,     -1,     ARM11_REGISTER_IRQ},\r
+    {"r14_irq",        14,     -1,     ARM11_REGISTER_IRQ},\r
+    {"spsr_irq", 0,    -1,     ARM11_REGISTER_SPSR_IRQ},\r
+\r
+    {"r13_und",        13,     -1,     ARM11_REGISTER_UND},\r
+    {"r14_und",        14,     -1,     ARM11_REGISTER_UND},\r
+    {"spsr_und", 0,    -1,     ARM11_REGISTER_SPSR_UND},\r
+\r
+    /* ARM1176 only */\r
+    {"r13_mon",        13,     -1,     ARM11_REGISTER_MON},\r
+    {"r14_mon",        14,     -1,     ARM11_REGISTER_MON},\r
+    {"spsr_mon", 0,    -1,     ARM11_REGISTER_SPSR_MON},\r
+#endif\r
+\r
+    /* Debug Registers */\r
+    {"dscr",   0,      -1,     ARM11_REGISTER_DSCR},\r
+    {"wdtr",   0,      -1,     ARM11_REGISTER_WDTR},\r
+    {"rdtr",   0,      -1,     ARM11_REGISTER_RDTR},\r
+};\r
+\r
+enum arm11_regcache_ids\r
+{\r
+    ARM11_RC_R0,\r
+    ARM11_RC_RX                        = ARM11_RC_R0,\r
+\r
+    ARM11_RC_R1,\r
+    ARM11_RC_R2,\r
+    ARM11_RC_R3,\r
+    ARM11_RC_R4,\r
+    ARM11_RC_R5,\r
+    ARM11_RC_R6,\r
+    ARM11_RC_R7,\r
+    ARM11_RC_R8,\r
+    ARM11_RC_R9,\r
+    ARM11_RC_R10,\r
+    ARM11_RC_R11,\r
+    ARM11_RC_R12,\r
+    ARM11_RC_R13,\r
+    ARM11_RC_SP                        = ARM11_RC_R13,\r
+    ARM11_RC_R14,\r
+    ARM11_RC_LR                        = ARM11_RC_R14,\r
+    ARM11_RC_R15,\r
+    ARM11_RC_PC                        = ARM11_RC_R15,\r
+\r
+#if ARM11_REGCACHE_FREGS\r
+    ARM11_RC_F0,\r
+    ARM11_RC_FX                        = ARM11_RC_F0,\r
+    ARM11_RC_F1,\r
+    ARM11_RC_F2,\r
+    ARM11_RC_F3,\r
+    ARM11_RC_F4,\r
+    ARM11_RC_F5,\r
+    ARM11_RC_F6,\r
+    ARM11_RC_F7,\r
+    ARM11_RC_FPS,\r
+#endif\r
+\r
+    ARM11_RC_CPSR,\r
+\r
+#if ARM11_REGCACHE_MODEREGS\r
+    ARM11_RC_R8_FIQ,\r
+    ARM11_RC_R9_FIQ,\r
+    ARM11_RC_R10_FIQ,\r
+    ARM11_RC_R11_FIQ,\r
+    ARM11_RC_R12_FIQ,\r
+    ARM11_RC_R13_FIQ,\r
+    ARM11_RC_R14_FIQ,\r
+    ARM11_RC_SPSR_FIQ,\r
+\r
+    ARM11_RC_R13_SVC,\r
+    ARM11_RC_R14_SVC,\r
+    ARM11_RC_SPSR_SVC,\r
+\r
+    ARM11_RC_R13_ABT,\r
+    ARM11_RC_R14_ABT,\r
+    ARM11_RC_SPSR_ABT,\r
+\r
+    ARM11_RC_R13_IRQ,\r
+    ARM11_RC_R14_IRQ,\r
+    ARM11_RC_SPSR_IRQ,\r
+\r
+    ARM11_RC_R13_UND,\r
+    ARM11_RC_R14_UND,\r
+    ARM11_RC_SPSR_UND,\r
+\r
+    ARM11_RC_R13_MON,\r
+    ARM11_RC_R14_MON,\r
+    ARM11_RC_SPSR_MON,\r
+#endif\r
+\r
+    ARM11_RC_DSCR,\r
+    ARM11_RC_WDTR,\r
+    ARM11_RC_RDTR,\r
+\r
+\r
+    ARM11_RC_MAX,\r
+};\r
+\r
+#define ARM11_GDB_REGISTER_COUNT       26\r
+\r
+u8 arm11_gdb_dummy_fp_value[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};\r
+\r
+reg_t arm11_gdb_dummy_fp_reg =\r
+{\r
+    "GDB dummy floating-point register", arm11_gdb_dummy_fp_value, 0, 1, 96, NULL, 0, NULL, 0\r
+};\r
+\r
+u8 arm11_gdb_dummy_fps_value[] = {0, 0, 0, 0};\r
+\r
+reg_t arm11_gdb_dummy_fps_reg =\r
+{\r
+    "GDB dummy floating-point status register", arm11_gdb_dummy_fps_value, 0, 1, 32, NULL, 0, NULL, 0\r
+};\r
+\r
+\r
+\r
+/** Check and if necessary take control of the system\r
+ *\r
+ * \param arm11                Target state variable.\r
+ * \param dscr         If the current DSCR content is\r
+ *                     available a pointer to a word holding the\r
+ *                     DSCR can be passed. Otherwise use NULL.\r
+ */\r
+void arm11_check_init(arm11_common_t * arm11, u32 * dscr)\r
+{\r
+    FNC_INFO;\r
+\r
+    u32                        dscr_local_tmp_copy;\r
+\r
+    if (!dscr)\r
+    {\r
+       dscr = &dscr_local_tmp_copy;\r
+       *dscr = arm11_read_DSCR(arm11);\r
+    }\r
+\r
+    if (!(*dscr & ARM11_DSCR_MODE_SELECT))\r
+    {\r
+       DEBUG("Bringing target into debug mode");\r
+\r
+       *dscr |= ARM11_DSCR_MODE_SELECT;                /* Halt debug-mode */\r
+       arm11_write_DSCR(arm11, *dscr);\r
+\r
+       /* add further reset initialization here */\r
+\r
+       if (*dscr & ARM11_DSCR_CORE_HALTED)\r
+       {\r
+           arm11->target->state        = TARGET_HALTED;\r
+           arm11->target->debug_reason = arm11_get_DSCR_debug_reason(*dscr);\r
+       }\r
+       else\r
+       {\r
+           arm11->target->state        = TARGET_RUNNING;\r
+           arm11->target->debug_reason = DBG_REASON_NOTHALTED;\r
+       }\r
+\r
+       arm11_sc7_clear_bw(arm11);\r
+    }\r
+}\r
+\r
+\r
+\r
+#define R(x) \\r
+    (arm11->reg_values[ARM11_RC_##x])\r
+\r
+/** Save processor state.\r
+  *\r
+  * This is called when the HALT instruction has succeeded\r
+  * or on other occasions that stop the processor.\r
+  *\r
+  */\r
+static void arm11_on_enter_debug_state(arm11_common_t * arm11)\r
+{\r
+    FNC_INFO;\r
+\r
+    {size_t i;\r
+    for(i = 0; i < asizeof(arm11->reg_values); i++)\r
+    {\r
+       arm11->reg_list[i].valid        = 1;\r
+       arm11->reg_list[i].dirty        = 0;\r
+    }}\r
+\r
+    /* Save DSCR */\r
+\r
+    R(DSCR) = arm11_read_DSCR(arm11);\r
+\r
+    /* Save wDTR */\r
+\r
+    if (R(DSCR) & ARM11_DSCR_WDTR_FULL)\r
+    {\r
+       arm11_add_debug_SCAN_N(arm11, 0x05, -1);\r
+\r
+       arm11_add_IR(arm11, ARM11_INTEST, -1);\r
+\r
+       scan_field_t    chain5_fields[3];\r
+\r
+       arm11_setup_field(arm11, 32, NULL, &R(WDTR),    chain5_fields + 0);\r
+       arm11_setup_field(arm11,  1, NULL, NULL,        chain5_fields + 1);\r
+       arm11_setup_field(arm11,  1, NULL, NULL,        chain5_fields + 2);\r
+\r
+       jtag_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, TAP_PD);\r
+    }\r
+    else\r
+    {\r
+       arm11->reg_list[ARM11_RC_WDTR].valid    = 0;\r
+    }\r
+\r
+\r
+    /* DSCR: set ARM11_DSCR_EXECUTE_ARM_INSTRUCTION_ENABLE */\r
+    /* ARM1176 spec says this is needed only for wDTR/rDTR's "ITR mode", but not to issue ITRs\r
+       ARM1136 seems to require this to issue ITR's as well */\r
+\r
+    u32 new_dscr = R(DSCR) | ARM11_DSCR_EXECUTE_ARM_INSTRUCTION_ENABLE;\r
+\r
+    /* this executes JTAG queue: */ \r
+\r
+    arm11_write_DSCR(arm11, new_dscr);\r
+\r
+//    jtag_execute_queue();\r
+\r
+\r
+\r
+//    DEBUG("SAVE DSCR %08x", R(DSCR));\r
+\r
+//    if (R(DSCR) & ARM11_DSCR_WDTR_FULL)\r
+//     DEBUG("SAVE wDTR %08x", R(WDTR));\r
+\r
+\r
+    /* From the spec:\r
+       Before executing any instruction in debug state you have to drain the write buffer.\r
+        This ensures that no imprecise Data Aborts can return at a later point:*/\r
+\r
+    /** \todo TODO: Test drain write buffer. */\r
+\r
+#if 0\r
+    while (1)\r
+    {\r
+       /* MRC p14,0,R0,c5,c10,0 */\r
+//     arm11_run_instr_no_data1(arm11, /*0xee150e1a*/0xe320f000);\r
+\r
+       /* mcr     15, 0, r0, cr7, cr10, {4} */\r
+       arm11_run_instr_no_data1(arm11, 0xee070f9a);\r
+               \r
+       u32 dscr = arm11_read_DSCR(arm11);\r
+\r
+       DEBUG("DRAIN, DSCR %08x", dscr);\r
+\r
+       if (dscr & ARM11_DSCR_STICKY_IMPRECISE_DATA_ABORT)\r
+       {\r
+           arm11_run_instr_no_data1(arm11, 0xe320f000);\r
+\r
+           dscr = arm11_read_DSCR(arm11);\r
+\r
+           DEBUG("DRAIN, DSCR %08x (DONE)", dscr);\r
+\r
+           break;\r
+       }\r
+    }\r
+#endif\r
+\r
+\r
+    arm11_run_instr_data_prepare(arm11);\r
+\r
+    /* save r0 - r14 */\r
+\r
+\r
+    /** \todo TODO: handle other mode registers */\r
+\r
+    {size_t i;\r
+    for (i = 0; i < 15; i++)\r
+    {\r
+       /* MCR p14,0,R?,c0,c5,0 */\r
+       arm11_run_instr_data_from_core(arm11, 0xEE000E15 | (i << 12), &R(RX + i), 1);\r
+    }}\r
+\r
+\r
+    /* save rDTR */\r
+\r
+    /* check rDTRfull in DSCR */\r
+\r
+    if (R(DSCR) & ARM11_DSCR_RDTR_FULL)\r
+    {\r
+       /* MRC p14,0,R0,c0,c5,0 (move rDTR -> r0 (-> wDTR -> local var)) */\r
+       arm11_run_instr_data_from_core_via_r0(arm11, 0xEE100E15, &R(RDTR));\r
+    }\r
+    else\r
+    {\r
+       arm11->reg_list[ARM11_RC_RDTR].valid    = 0;\r
+    }\r
+\r
+    /* save CPSR */\r
+\r
+    /* MRS r0,CPSR (move CPSR -> r0 (-> wDTR -> local var)) */\r
+    arm11_run_instr_data_from_core_via_r0(arm11, 0xE10F0000, &R(CPSR));\r
+\r
+    /* save PC */\r
+\r
+    /* MOV R0,PC (move PC -> r0 (-> wDTR -> local var)) */\r
+    arm11_run_instr_data_from_core_via_r0(arm11, 0xE1A0000F, &R(PC));\r
+\r
+    /* adjust PC depending on ARM state */\r
+\r
+    if (R(CPSR) & ARM11_CPSR_J)        /* Java state */\r
+    {\r
+       arm11->reg_values[ARM11_RC_PC] -= 0;\r
+    }\r
+    else if (R(CPSR) & ARM11_CPSR_T)   /* Thumb state */\r
+    {\r
+       arm11->reg_values[ARM11_RC_PC] -= 4;\r
+    }\r
+    else                                       /* ARM state */\r
+    {\r
+       arm11->reg_values[ARM11_RC_PC] -= 8;\r
+    }\r
+\r
+//    DEBUG("SAVE PC   %08x", R(PC));\r
+\r
+    arm11_run_instr_data_finish(arm11);\r
+\r
+\r
+    {size_t i;\r
+    for(i = 0; i < ARM11_REGCACHE_COUNT; i++)\r
+    {\r
+       if (!arm11->reg_list[i].valid)\r
+       {\r
+           if (arm11->reg_history[i].valid)\r
+               INFO("%8s INVALID    (%08x)", arm11_reg_defs[i].name, arm11->reg_history[i].value);\r
+       }\r
+       else\r
+       {\r
+           if (arm11->reg_history[i].valid)\r
+           {\r
+               if (arm11->reg_history[i].value != arm11->reg_values[i])\r
+                   INFO("%8s %08x (%08x)", arm11_reg_defs[i].name, arm11->reg_values[i], arm11->reg_history[i].value);\r
+           }\r
+           else\r
+           {\r
+               INFO("%8s %08x (INVALID)", arm11_reg_defs[i].name, arm11->reg_values[i]);\r
+           }\r
+       }\r
+    }}\r
+}\r
+\r
+\r
+/** Restore processor state\r
+  *\r
+  * This is called in preparation for the RESTART function.\r
+  *\r
+  */\r
+void arm11_leave_debug_state(arm11_common_t * arm11)\r
+{\r
+    FNC_INFO;\r
+\r
+    arm11_run_instr_data_prepare(arm11);\r
+\r
+    /** \todo TODO: handle other mode registers */\r
+\r
+    /* restore R1 - R14 */\r
+    {size_t i;\r
+    for (i = 1; i < 15; i++)\r
+    {\r
+       if (!arm11->reg_list[ARM11_RC_RX + i].dirty)\r
+           continue;\r
+\r
+       /* MRC p14,0,r?,c0,c5,0 */\r
+       arm11_run_instr_data_to_core1(arm11, 0xee100e15 | (i << 12), R(RX + i));\r
+\r
+//     DEBUG("RESTORE R%d %08x", i, R(RX + i));\r
+    }}\r
+\r
+    arm11_run_instr_data_finish(arm11);\r
+\r
+\r
+    /* spec says clear wDTR and rDTR; we assume they are clear as\r
+       otherwide out programming would be sloppy */\r
+\r
+    {\r
+       u32 DSCR = arm11_read_DSCR(arm11);\r
+\r
+       if (DSCR & (ARM11_DSCR_RDTR_FULL | ARM11_DSCR_WDTR_FULL))\r
+       {\r
+           ERROR("wDTR/rDTR inconsistent (DSCR %08x)", DSCR);\r
+       }\r
+    }\r
+\r
+    arm11_run_instr_data_prepare(arm11);\r
+\r
+    /* restore original wDTR */\r
+\r
+    if ((R(DSCR) & ARM11_DSCR_WDTR_FULL) || arm11->reg_list[ARM11_RC_WDTR].dirty)\r
+    {\r
+       /* MCR p14,0,R0,c0,c5,0 */\r
+       arm11_run_instr_data_to_core_via_r0(arm11, 0xee000e15, R(WDTR));\r
+    }\r
+\r
+    /* restore CPSR */\r
+\r
+    /* MSR CPSR,R0*/\r
+    arm11_run_instr_data_to_core_via_r0(arm11, 0xe129f000, R(CPSR));\r
+\r
+\r
+    /* restore PC */\r
+\r
+    /* MOV PC,R0 */\r
+    arm11_run_instr_data_to_core_via_r0(arm11, 0xe1a0f000, R(PC));\r
+\r
+\r
+    /* restore R0 */\r
+\r
+    /* MRC p14,0,r0,c0,c5,0 */\r
+    arm11_run_instr_data_to_core1(arm11, 0xee100e15, R(R0));\r
+\r
+    arm11_run_instr_data_finish(arm11);\r
+\r
+\r
+    /* restore DSCR */\r
+\r
+    arm11_write_DSCR(arm11, R(DSCR));\r
+\r
+\r
+    /* restore rDTR */\r
+    \r
+    if (R(DSCR) & ARM11_DSCR_RDTR_FULL || arm11->reg_list[ARM11_RC_RDTR].dirty)\r
+    {\r
+       arm11_add_debug_SCAN_N(arm11, 0x05, -1);\r
+\r
+       arm11_add_IR(arm11, ARM11_EXTEST, -1);\r
+\r
+       scan_field_t    chain5_fields[3];\r
+\r
+       u8                      Ready       = 0;        /* ignored */\r
+       u8                      Valid       = 0;        /* ignored */\r
+\r
+       arm11_setup_field(arm11, 32, &R(RDTR),  NULL, chain5_fields + 0);\r
+       arm11_setup_field(arm11,  1, &Ready,    NULL, chain5_fields + 1);\r
+       arm11_setup_field(arm11,  1, &Valid,    NULL, chain5_fields + 2);\r
+\r
+       jtag_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, TAP_PD);\r
+    }\r
+\r
+\r
+    {size_t i;\r
+    for(i = 0; i < ARM11_REGCACHE_COUNT; i++)\r
+    {\r
+       arm11->reg_history[i].value     = arm11->reg_values[i];\r
+       arm11->reg_history[i].valid     = arm11->reg_list[i].valid;\r
+\r
+       arm11->reg_list[i].valid        = 0;\r
+       arm11->reg_list[i].dirty        = 0;\r
+    }}\r
+}\r
+\r
+\r
+/* poll current target status */\r
+int arm11_poll(struct target_s *target)\r
+{\r
+    FNC_INFO;\r
+\r
+    arm11_common_t * arm11 = target->arch_info;\r
+\r
+    if (arm11->trst_active)\r
+       return ERROR_OK;\r
+\r
+    u32        dscr = arm11_read_DSCR(arm11);\r
+\r
+    DEBUG("DSCR %08x", dscr);\r
+\r
+    arm11_check_init(arm11, &dscr);\r
+\r
+    if (dscr & ARM11_DSCR_CORE_HALTED)\r
+    {\r
+//     DEBUG("CH %d", target->state);\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+           DEBUG("enter TARGET_HALTED");\r
+           target->state               = TARGET_HALTED;\r
+           target->debug_reason        = arm11_get_DSCR_debug_reason(dscr);\r
+           arm11_on_enter_debug_state(arm11);\r
+       }\r
+    }\r
+    else\r
+    {\r
+//     DEBUG("CR %d", target->state);\r
+\r
+       if (target->state != TARGET_RUNNING)\r
+       {\r
+           DEBUG("enter TARGET_RUNNING");\r
+           target->state               = TARGET_RUNNING;\r
+           target->debug_reason        = DBG_REASON_NOTHALTED;\r
+       }\r
+    }\r
+\r
+    return ERROR_OK;\r
+}\r
+/* architecture specific status reply */\r
+int arm11_arch_state(struct target_s *target)\r
+{\r
+    FNC_INFO_NOTIMPLEMENTED;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+\r
+/* target request support */\r
+int arm11_target_request_data(struct target_s *target, u32 size, u8 *buffer)\r
+{\r
+    FNC_INFO_NOTIMPLEMENTED;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+\r
+\r
+/* target execution control */\r
+int arm11_halt(struct target_s *target)\r
+{\r
+    FNC_INFO;\r
+\r
+    arm11_common_t * arm11 = target->arch_info;\r
+\r
+    DEBUG("target->state: %s", target_state_strings[target->state]);\r
+\r
+    if (target->state == TARGET_HALTED)\r
+    {\r
+       WARNING("target was already halted");\r
+       return ERROR_TARGET_ALREADY_HALTED;\r
+    }\r
+\r
+    if (arm11->trst_active)\r
+    {\r
+       arm11->halt_requested = true;\r
+       return ERROR_OK;\r
+    }\r
+\r
+    arm11_add_IR(arm11, ARM11_HALT, TAP_RTI);\r
+\r
+    jtag_execute_queue();\r
+\r
+    u32 dscr;\r
+\r
+    while (1)\r
+    {\r
+       dscr = arm11_read_DSCR(arm11);\r
+\r
+       if (dscr & ARM11_DSCR_CORE_HALTED)\r
+           break;\r
+    }\r
+\r
+    arm11_on_enter_debug_state(arm11);\r
+\r
+    target->state              = TARGET_HALTED;\r
+    target->debug_reason       = arm11_get_DSCR_debug_reason(dscr);\r
+    \r
+    return ERROR_OK;\r
+}\r
+\r
+\r
+int arm11_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution)\r
+{\r
+    FNC_INFO;\r
+\r
+    arm11_common_t * arm11 = target->arch_info;\r
+\r
+    DEBUG("target->state: %s", target_state_strings[target->state]);\r
+\r
+    if (target->state != TARGET_HALTED)\r
+    {\r
+       WARNING("target was not halted");\r
+       return ERROR_TARGET_NOT_HALTED;\r
+    }\r
+\r
+    if (!current)\r
+       R(PC) = address;\r
+\r
+    target->state              = TARGET_RUNNING;\r
+    target->debug_reason       = DBG_REASON_NOTHALTED;\r
+\r
+    arm11_leave_debug_state(arm11);\r
+\r
+    arm11_add_IR(arm11, ARM11_RESTART, TAP_RTI);\r
+\r
+    jtag_execute_queue();\r
+\r
+    while (1)\r
+    {\r
+       u32 dscr = arm11_read_DSCR(arm11);\r
+\r
+       DEBUG("DSCR %08x", dscr);\r
+\r
+       if (dscr & ARM11_DSCR_CORE_RESTARTED)\r
+           break;\r
+    }\r
+\r
+    DEBUG("RES %d", target->state);\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+int arm11_step(struct target_s *target, int current, u32 address, int handle_breakpoints)\r
+{\r
+    FNC_INFO;\r
+\r
+    DEBUG("target->state: %s", target_state_strings[target->state]);\r
+\r
+    if (target->state != TARGET_HALTED)\r
+    {\r
+       WARNING("target was not halted");\r
+       return ERROR_TARGET_NOT_HALTED;\r
+    }\r
+\r
+    arm11_common_t * arm11 = target->arch_info;\r
+\r
+    /** \todo TODO: check if break-/watchpoints make any sense at all in combination\r
+      * with this. */\r
+\r
+    /** \todo TODO: check if disabling IRQs might be a good idea here. Alternatively\r
+        the VCR might be something worth looking into. */\r
+\r
+    /* Set up breakpoint for stepping */\r
+\r
+    arm11_sc7_action_t brp[2];\r
+\r
+    brp[0].write       = 1;\r
+    brp[0].address     = ARM11_SC7_BVR0;\r
+    brp[0].value       = R(PC);\r
+    brp[1].write       = 1;\r
+    brp[1].address     = ARM11_SC7_BCR0;\r
+    brp[1].value       = 0x1 | (3 << 1) | (0x0F << 5) | (0 << 14) | (0 << 16) | (0 << 20) | (2 << 21);\r
+\r
+    arm11_sc7_run(arm11, brp, asizeof(brp));\r
+\r
+    /* resume */\r
+\r
+    arm11_leave_debug_state(arm11);\r
+\r
+    arm11_add_IR(arm11, ARM11_RESTART, TAP_RTI);\r
+\r
+    jtag_execute_queue();\r
+\r
+    /** \todo TODO: add a timeout */\r
+\r
+    /* wait for halt */\r
+\r
+    while (1)\r
+    {\r
+       u32 dscr = arm11_read_DSCR(arm11);\r
+\r
+       DEBUG("DSCR %08x", dscr);\r
+\r
+        if ((dscr & (ARM11_DSCR_CORE_RESTARTED | ARM11_DSCR_CORE_HALTED)) ==\r
+           (ARM11_DSCR_CORE_RESTARTED | ARM11_DSCR_CORE_HALTED))\r
+           break;\r
+    }\r
+\r
+\r
+    /* clear breakpoint */\r
+\r
+    arm11_sc7_clear_bw(arm11);\r
+\r
+\r
+    /* save state */\r
+\r
+    arm11_on_enter_debug_state(arm11);\r
+\r
+//    target->state            = TARGET_HALTED;\r
+    target->debug_reason       = DBG_REASON_SINGLESTEP;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+\r
+/* target reset control */\r
+int arm11_assert_reset(struct target_s *target)\r
+{\r
+    FNC_INFO;\r
+\r
+#if 0\r
+    /* assert reset lines */\r
+    /* resets only the DBGTAP, not the ARM */\r
+\r
+    jtag_add_reset(1, 0);\r
+    jtag_add_sleep(5000);\r
+\r
+    arm11_common_t * arm11 = target->arch_info;\r
+    arm11->trst_active = true;\r
+#endif\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+int arm11_deassert_reset(struct target_s *target)\r
+{\r
+    FNC_INFO;\r
+\r
+#if 0\r
+    DEBUG("target->state: %s", target_state_strings[target->state]);\r
+\r
+    /* deassert reset lines */\r
+    jtag_add_reset(0, 0);\r
+\r
+    arm11_common_t * arm11 = target->arch_info;\r
+    arm11->trst_active = false;\r
+\r
+    if (arm11->halt_requested)\r
+       return arm11_halt(target);\r
+#endif\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+int arm11_soft_reset_halt(struct target_s *target)\r
+{\r
+    FNC_INFO_NOTIMPLEMENTED;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+int arm11_prepare_reset_halt(struct target_s *target)\r
+{\r
+    FNC_INFO_NOTIMPLEMENTED;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+\r
+/* target register access for gdb */\r
+int arm11_get_gdb_reg_list(struct target_s *target, struct reg_s **reg_list[], int *reg_list_size)\r
+{\r
+    FNC_INFO;\r
+\r
+    arm11_common_t * arm11 = target->arch_info;\r
+\r
+    if (target->state != TARGET_HALTED)\r
+    {\r
+       return ERROR_TARGET_NOT_HALTED;\r
+    }\r
+       \r
+    *reg_list_size  = ARM11_GDB_REGISTER_COUNT;\r
+    *reg_list      = malloc(sizeof(reg_t*) * ARM11_GDB_REGISTER_COUNT);\r
+\r
+    {size_t i;\r
+    for (i = 16; i < 24; i++)\r
+    {\r
+       (*reg_list)[i] = &arm11_gdb_dummy_fp_reg;\r
+    }}\r
+\r
+    (*reg_list)[24] = &arm11_gdb_dummy_fps_reg;\r
+\r
+\r
+    {size_t i;\r
+    for (i = 0; i < ARM11_REGCACHE_COUNT; i++)\r
+    {\r
+       if (arm11_reg_defs[i].gdb_num == -1)\r
+           continue;\r
+\r
+       (*reg_list)[arm11_reg_defs[i].gdb_num] = arm11->reg_list + i;\r
+    }}\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+\r
+/* target memory access \r
+* size: 1 = byte (8bit), 2 = half-word (16bit), 4 = word (32bit)\r
+* count: number of items of <size>\r
+*/\r
+int arm11_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)\r
+{\r
+    /** \todo TODO: check if buffer cast to u32* and u16* might cause alignment problems */\r
+\r
+    FNC_INFO;\r
+\r
+    DEBUG("ADDR %08x  SIZE %08x  COUNT %08x", address, size, count);\r
+\r
+    arm11_common_t * arm11 = target->arch_info;\r
+\r
+    arm11_run_instr_data_prepare(arm11);\r
+\r
+    /* MRC p14,0,r0,c0,c5,0 */\r
+    arm11_run_instr_data_to_core1(arm11, 0xee100e15, address);\r
+\r
+    switch (size)\r
+    {\r
+    case 1:\r
+       /** \todo TODO: check if dirty is the right choice to force a rewrite on arm11_resume() */\r
+       arm11->reg_list[ARM11_RC_R1].dirty = 1;\r
+\r
+       while (count--)\r
+       {\r
+           /* ldrb    r1, [r0], #1 */\r
+           arm11_run_instr_no_data1(arm11, 0xe4d01001);\r
+\r
+           u32 res;\r
+           /* MCR p14,0,R1,c0,c5,0 */\r
+           arm11_run_instr_data_from_core(arm11, 0xEE001E15, &res, 1);\r
+\r
+           *buffer++ = res;\r
+       }\r
+       break;\r
+\r
+    case 2:\r
+    {\r
+       arm11->reg_list[ARM11_RC_R1].dirty = 1;\r
+\r
+       u16 * buf16 = (u16*)buffer;\r
+\r
+       while (count--)\r
+       {\r
+           /* ldrh    r1, [r0], #2 */\r
+           arm11_run_instr_no_data1(arm11, 0xe0d010b2);\r
+\r
+           u32 res;\r
+\r
+           /* MCR p14,0,R1,c0,c5,0 */\r
+           arm11_run_instr_data_from_core(arm11, 0xEE001E15, &res, 1);\r
+\r
+           *buf16++ = res;\r
+       }\r
+       break;\r
+    }\r
+\r
+    case 4:\r
+\r
+       /* LDC p14,c5,[R0],#4 */\r
+       arm11_run_instr_data_from_core(arm11, 0xecb05e01, (u32 *)buffer, count);\r
+       break;\r
+    }\r
+\r
+    arm11_run_instr_data_finish(arm11);\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+int arm11_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)\r
+{\r
+    FNC_INFO;\r
+\r
+    DEBUG("ADDR %08x  SIZE %08x  COUNT %08x", address, size, count);\r
+\r
+    arm11_common_t * arm11 = target->arch_info;\r
+\r
+    arm11_run_instr_data_prepare(arm11);\r
+\r
+    /* MRC p14,0,r0,c0,c5,0 */\r
+    arm11_run_instr_data_to_core1(arm11, 0xee100e15, address);\r
+\r
+    switch (size)\r
+    {\r
+    case 1:\r
+       arm11->reg_list[ARM11_RC_R1].dirty = 1;\r
+\r
+       while (count--)\r
+       {\r
+           /* MRC p14,0,r1,c0,c5,0 */\r
+           arm11_run_instr_data_to_core1(arm11, 0xee101e15, *buffer++);\r
+\r
+           /* strb    r1, [r0], #1 */\r
+           arm11_run_instr_no_data1(arm11, 0xe4c01001);\r
+       }\r
+       break;\r
+\r
+    case 2:\r
+    {\r
+       arm11->reg_list[ARM11_RC_R1].dirty = 1;\r
+\r
+       u16 * buf16 = (u16*)buffer;\r
+\r
+       while (count--)\r
+       {\r
+           /* MRC p14,0,r1,c0,c5,0 */\r
+           arm11_run_instr_data_to_core1(arm11, 0xee101e15, *buf16++);\r
+\r
+           /* strh    r1, [r0], #2 */\r
+           arm11_run_instr_no_data1(arm11, 0xe0c010b2);\r
+       }\r
+       break;\r
+    }\r
+\r
+    case 4:\r
+       /** \todo TODO: check if buffer cast to u32* might cause alignment problems */\r
+\r
+       /* STC p14,c5,[R0],#4 */\r
+       arm11_run_instr_data_to_core(arm11, 0xeca05e01, (u32 *)buffer, count);\r
+       break;\r
+    }\r
+\r
+    arm11_run_instr_data_finish(arm11);\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+\r
+/* write target memory in multiples of 4 byte, optimized for writing large quantities of data */\r
+int arm11_bulk_write_memory(struct target_s *target, u32 address, u32 count, u8 *buffer)\r
+{\r
+    FNC_INFO;\r
+\r
+    return arm11_write_memory(target, address, 4, count, buffer);\r
+}\r
+\r
+\r
+int arm11_checksum_memory(struct target_s *target, u32 address, u32 count, u32* checksum)\r
+{\r
+    FNC_INFO_NOTIMPLEMENTED;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+\r
+/* target break-/watchpoint control \r
+* rw: 0 = write, 1 = read, 2 = access\r
+*/\r
+int arm11_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint)\r
+{\r
+    FNC_INFO_NOTIMPLEMENTED;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+int arm11_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint)\r
+{\r
+    FNC_INFO_NOTIMPLEMENTED;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+int arm11_add_watchpoint(struct target_s *target, watchpoint_t *watchpoint)\r
+{\r
+    FNC_INFO_NOTIMPLEMENTED;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+int arm11_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint)\r
+{\r
+    FNC_INFO_NOTIMPLEMENTED;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+\r
+/* target algorithm support */\r
+int arm11_run_algorithm(struct target_s *target, int num_mem_params, mem_param_t *mem_params, int num_reg_params, reg_param_t *reg_param, u32 entry_point, u32 exit_point, int timeout_ms, void *arch_info)\r
+{\r
+    FNC_INFO_NOTIMPLEMENTED;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+\r
+int arm11_register_commands(struct command_context_s *cmd_ctx)\r
+{\r
+    FNC_INFO;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+int arm11_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)\r
+{\r
+    FNC_INFO;\r
+\r
+    if (argc < 4)\r
+    {\r
+       ERROR("'target arm11' 4th argument <jtag chain pos>");\r
+       exit(-1);\r
+    }\r
+\r
+    int chain_pos = strtoul(args[3], NULL, 0);\r
+\r
+    NEW(arm11_common_t, arm11, 1);\r
+\r
+    arm11->target = target;\r
+\r
+    /* prepare JTAG information for the new target */\r
+    arm11->jtag_info.chain_pos = chain_pos;\r
+    arm11->jtag_info.scann_size        = 5;\r
+\r
+    arm_jtag_setup_connection(&arm11->jtag_info);\r
+\r
+    jtag_device_t *device = jtag_get_device(chain_pos);\r
+\r
+    if (device->ir_length != 5)\r
+    {\r
+       ERROR("'target arm11' expects 'jtag_device 5 0x01 0x1F 0x1E'");\r
+       exit(-1);\r
+    }\r
+\r
+    target->arch_info = arm11;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+int arm11_init_target(struct command_context_s *cmd_ctx, struct target_s *target)\r
+{\r
+    FNC_INFO;\r
+\r
+    arm11_common_t * arm11 = target->arch_info;\r
+\r
+    /* check IDCODE */\r
+\r
+    arm11_add_IR(arm11, ARM11_IDCODE, -1);\r
+\r
+    scan_field_t               idcode_field;\r
+\r
+    arm11_setup_field(arm11, 32, NULL, &arm11->device_id, &idcode_field);\r
+\r
+    jtag_add_dr_scan_vc(1, &idcode_field, TAP_PD);\r
+\r
+    /* check DIDR */\r
+\r
+    arm11_add_debug_SCAN_N(arm11, 0x00, -1);\r
+\r
+    arm11_add_IR(arm11, ARM11_INTEST, -1);\r
+\r
+    scan_field_t               chain0_fields[2];\r
+\r
+    arm11_setup_field(arm11, 32, NULL, &arm11->didr,           chain0_fields + 0);\r
+    arm11_setup_field(arm11,  8, NULL, &arm11->implementor,    chain0_fields + 1);\r
+\r
+    jtag_add_dr_scan_vc(asizeof(chain0_fields), chain0_fields, TAP_RTI);\r
+\r
+    jtag_execute_queue();\r
+\r
+\r
+    switch (arm11->device_id & 0x0FFFF000)\r
+    {\r
+    case 0x07B36000:   INFO("found ARM1136"); break;\r
+    case 0x07B56000:   INFO("found ARM1156"); break;\r
+    case 0x07B76000:   INFO("found ARM1176"); break;\r
+    default:\r
+    {\r
+       ERROR("'target arm11' expects IDCODE 0x*7B*7****");\r
+       exit(-1);\r
+    }\r
+    }\r
+\r
+    arm11->brp = ((arm11->didr >> 24) & 0x0F) + 1;\r
+    arm11->wrp = ((arm11->didr >> 28) & 0x0F) + 1;\r
+\r
+\r
+    DEBUG("IDCODE %08x IMPLEMENTOR %02x DIDR %08x",\r
+       arm11->device_id,\r
+       arm11->implementor,\r
+       arm11->didr);\r
+\r
+    arm11_build_reg_cache(target);\r
+\r
+\r
+    /* as a side-effect this reads DSCR and thus\r
+     * clears the ARM11_DSCR_STICKY_PRECISE_DATA_ABORT / Sticky Precise Data Abort Flag\r
+     * as suggested by the spec.\r
+     */\r
+\r
+    arm11_check_init(arm11, NULL);\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+int arm11_quit(void)\r
+{\r
+    FNC_INFO_NOTIMPLEMENTED;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+/** Load a register that is marked !valid in the register cache */\r
+int arm11_get_reg(reg_t *reg)\r
+{\r
+    FNC_INFO;\r
+\r
+    target_t * target = ((arm11_reg_state_t *)reg->arch_info)->target;\r
+\r
+    if (target->state != TARGET_HALTED)\r
+    {\r
+       return ERROR_TARGET_NOT_HALTED;\r
+    }\r
+\r
+    /** \todo TODO: Check this. We assume that all registers are fetched debug entry. */\r
+\r
+#if 0\r
+    arm11_common_t *arm11 = target->arch_info;\r
+    const arm11_reg_defs_t * arm11_reg_info = arm11_reg_defs + ((arm11_reg_state_t *)reg->arch_info)->def_index;\r
+#endif\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+/** Change a value in the register cache */\r
+int arm11_set_reg(reg_t *reg, u8 *buf)\r
+{\r
+    FNC_INFO;\r
+\r
+    target_t * target = ((arm11_reg_state_t *)reg->arch_info)->target;\r
+    arm11_common_t *arm11 = target->arch_info;\r
+//    const arm11_reg_defs_t * arm11_reg_info = arm11_reg_defs + ((arm11_reg_state_t *)reg->arch_info)->def_index;\r
+\r
+    arm11->reg_values[((arm11_reg_state_t *)reg->arch_info)->def_index] = buf_get_u32(buf, 0, 32);\r
+    reg->valid = 1;\r
+    reg->dirty = 1;\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+\r
+void arm11_build_reg_cache(target_t *target)\r
+{\r
+    arm11_common_t *arm11 = target->arch_info;\r
+\r
+    NEW(reg_cache_t,           cache,                  1);\r
+    NEW(reg_t,                 reg_list,               ARM11_REGCACHE_COUNT);\r
+    NEW(arm11_reg_state_t,     arm11_reg_states,       ARM11_REGCACHE_COUNT);\r
+\r
+    if (arm11_regs_arch_type == -1)\r
+       arm11_regs_arch_type = register_reg_arch_type(arm11_get_reg, arm11_set_reg);\r
+\r
+    arm11->reg_list    = reg_list;\r
+\r
+    /* Build the process context cache */ \r
+    cache->name                = "arm11 registers";\r
+    cache->next                = NULL;\r
+    cache->reg_list    = reg_list;\r
+    cache->num_regs    = ARM11_REGCACHE_COUNT;\r
+\r
+    reg_cache_t **cache_p = register_get_last_cache_p(&target->reg_cache);\r
+    (*cache_p) = cache;\r
+\r
+//    armv7m->core_cache = cache;\r
+//    armv7m->process_context = cache;\r
+\r
+    size_t i;\r
+\r
+    /* Not very elegant assertion */\r
+    if (ARM11_REGCACHE_COUNT != asizeof(arm11->reg_values) ||\r
+       ARM11_REGCACHE_COUNT != asizeof(arm11_reg_defs) ||\r
+       ARM11_REGCACHE_COUNT != ARM11_RC_MAX)\r
+    {\r
+       ERROR("arm11->reg_values inconsistent (%d %d %d %d)", ARM11_REGCACHE_COUNT, asizeof(arm11->reg_values), asizeof(arm11_reg_defs), ARM11_RC_MAX);\r
+       exit(-1);\r
+    }\r
+\r
+    for (i = 0; i < ARM11_REGCACHE_COUNT; i++)\r
+    {\r
+       reg_t *                         r       = reg_list              + i;\r
+       const arm11_reg_defs_t *        rd      = arm11_reg_defs        + i;\r
+       arm11_reg_state_t *             rs      = arm11_reg_states      + i;\r
+\r
+       r->name                 = rd->name;\r
+       r->size                 = 32;\r
+       r->value                = (u8 *)(arm11->reg_values + i);\r
+       r->dirty                = 0;\r
+       r->valid                = 0;\r
+       r->bitfield_desc        = NULL;\r
+       r->num_bitfields        = 0;\r
+       r->arch_type            = arm11_regs_arch_type;\r
+       r->arch_info            = rs;\r
+\r
+       rs->def_index           = i;\r
+       rs->target              = target;\r
+    }\r
+}\r
+\r
+#if 0\r
+    arm11_run_instr_data_prepare(arm11);\r
+\r
+    /* MRC p14,0,r0,c0,c5,0 */\r
+    arm11_run_instr_data_to_core(arm11, 0xee100e15, 0xCA00003C);\r
+    /* MRC p14,0,r1,c0,c5,0 */\r
+    arm11_run_instr_data_to_core(arm11, 0xee101e15, 0xFFFFFFFF);\r
+\r
+    arm11_run_instr_data_finish(arm11);\r
+#endif\r
+\r
+\r
diff --git a/src/target/arm11.h b/src/target/arm11.h
new file mode 100644 (file)
index 0000000..4e061ae
--- /dev/null
@@ -0,0 +1,250 @@
+/***************************************************************************\r
+ *   Copyright (C) 2008 digenius technology GmbH.                          *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+\r
+#ifndef ARM11_H\r
+#define ARM11_H\r
+\r
+#include "target.h"\r
+#include "register.h"\r
+#include "embeddedice.h"\r
+#include "arm_jtag.h"\r
+\r
+\r
+#define bool   int\r
+#define true   1\r
+#define false  0\r
+\r
+#define asizeof(x)     (sizeof(x) / sizeof((x)[0]))\r
+\r
+#define NEW(type, variable, items) \\r
+    type * variable = malloc(sizeof(type) * items)\r
+\r
+\r
+#define ARM11_REGCACHE_MODEREGS                0\r
+#define ARM11_REGCACHE_FREGS           0\r
+\r
+#define ARM11_REGCACHE_COUNT           (20 +                                   \\r
+                                        23 * ARM11_REGCACHE_MODEREGS +         \\r
+                                         9 * ARM11_REGCACHE_FREGS)\r
+\r
+\r
+typedef struct arm11_register_history_s\r
+{\r
+    u32            value;\r
+    u8     valid;\r
+}arm11_register_history_t;\r
+\r
+\r
+\r
+typedef struct arm11_common_s\r
+{\r
+    target_t * target;\r
+\r
+    arm_jtag_t jtag_info;\r
+\r
+    /** \name Processor type detection */\r
+    /*@{*/\r
+\r
+    u32                device_id;          /**< IDCODE readout                         */\r
+    u32                didr;               /**< DIDR readout (debug capabilities)      */\r
+    u8         implementor;        /**< DIDR Implementor readout               */\r
+\r
+    size_t     brp;                /**< Number of Breakpoint Register Pairs    */\r
+    size_t     wrp;                /**< Number of Watchpoint Register Pairs    */\r
+\r
+    /*@}*/\r
+\r
+\r
+    u32                last_dscr;          /**< Last retrieved DSCR value;\r
+                                    *   Can be used to detect changes          */\r
+\r
+    u8         trst_active;\r
+    u8         halt_requested;\r
+\r
+    /** \name Shadow registers to save processor state */\r
+    /*@{*/\r
+\r
+    reg_t *    reg_list;                               /**< target register list */\r
+    u32                reg_values[ARM11_REGCACHE_COUNT];       /**< data for registers */\r
+\r
+    /*@}*/\r
+\r
+    arm11_register_history_t\r
+               reg_history[ARM11_REGCACHE_COUNT];      /**< register state before last resume */\r
+\r
+\r
+} arm11_common_t;\r
+\r
+\r
+/**\r
+ * ARM11 DBGTAP instructions \r
+ * \r
+ * http://infocenter.arm.com/help/topic/com.arm.doc.ddi0301f/I1006229.html\r
+ */\r
+enum arm11_instructions\r
+{\r
+    ARM11_EXTEST    = 0x00,\r
+    ARM11_SCAN_N    = 0x02,\r
+    ARM11_RESTART   = 0x04,\r
+    ARM11_HALT     = 0x08,\r
+    ARM11_INTEST    = 0x0C,\r
+    ARM11_ITRSEL    = 0x1D,\r
+    ARM11_IDCODE    = 0x1E,\r
+    ARM11_BYPASS    = 0x1F,\r
+};\r
+\r
+enum arm11_dscr\r
+{\r
+    ARM11_DSCR_CORE_HALTED                             = 1 << 0,\r
+    ARM11_DSCR_CORE_RESTARTED                          = 1 << 1,\r
+\r
+    ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_MASK              = 0x0F << 2,\r
+    ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_HALT              = 0x00 << 2,\r
+    ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_BREAKPOINT                = 0x01 << 2,\r
+    ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_WATCHPOINT                = 0x02 << 2,\r
+    ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_BKPT_INSTRUCTION  = 0x03 << 2,\r
+    ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_EDBGRQ            = 0x04 << 2,\r
+    ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_VECTOR_CATCH      = 0x05 << 2,\r
+\r
+    ARM11_DSCR_STICKY_PRECISE_DATA_ABORT               = 1 << 6,\r
+    ARM11_DSCR_STICKY_IMPRECISE_DATA_ABORT             = 1 << 7,\r
+    ARM11_DSCR_EXECUTE_ARM_INSTRUCTION_ENABLE          = 1 << 13,\r
+    ARM11_DSCR_MODE_SELECT                             = 1 << 14,\r
+    ARM11_DSCR_WDTR_FULL                               = 1 << 29,\r
+    ARM11_DSCR_RDTR_FULL                               = 1 << 30,\r
+};\r
+\r
+enum arm11_cpsr\r
+{\r
+    ARM11_CPSR_T                               = 1 << 5,\r
+    ARM11_CPSR_J                               = 1 << 24,\r
+};\r
+\r
+enum arm11_sc7\r
+{\r
+    ARM11_SC7_NULL                             = 0,\r
+    ARM11_SC7_VCR                              = 7,\r
+    ARM11_SC7_PC                               = 8,\r
+    ARM11_SC7_BVR0                             = 64,\r
+    ARM11_SC7_BCR0                             = 80,\r
+    ARM11_SC7_WVR0                             = 96,\r
+    ARM11_SC7_WCR0                             = 112,\r
+};\r
+\r
+\r
+\r
+typedef struct arm11_reg_state_s\r
+{\r
+    u32                                def_index;\r
+    target_t *                 target;\r
+} arm11_reg_state_t;\r
+\r
+\r
+\r
+\r
+/* poll current target status */\r
+int arm11_poll(struct target_s *target);\r
+/* architecture specific status reply */\r
+int arm11_arch_state(struct target_s *target);\r
+\r
+/* target request support */\r
+int arm11_target_request_data(struct target_s *target, u32 size, u8 *buffer);\r
+\r
+/* target execution control */\r
+int arm11_halt(struct target_s *target);\r
+int arm11_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution);\r
+int arm11_step(struct target_s *target, int current, u32 address, int handle_breakpoints);\r
+\r
+/* target reset control */\r
+int arm11_assert_reset(struct target_s *target);\r
+int arm11_deassert_reset(struct target_s *target);\r
+int arm11_soft_reset_halt(struct target_s *target);\r
+int arm11_prepare_reset_halt(struct target_s *target);\r
+\r
+/* target register access for gdb */\r
+int arm11_get_gdb_reg_list(struct target_s *target, struct reg_s **reg_list[], int *reg_list_size);\r
+\r
+/* target memory access \r
+* size: 1 = byte (8bit), 2 = half-word (16bit), 4 = word (32bit)\r
+* count: number of items of <size>\r
+*/\r
+int arm11_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);\r
+int arm11_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);\r
+\r
+/* write target memory in multiples of 4 byte, optimized for writing large quantities of data */\r
+int arm11_bulk_write_memory(struct target_s *target, u32 address, u32 count, u8 *buffer);\r
+\r
+int arm11_checksum_memory(struct target_s *target, u32 address, u32 count, u32* checksum);\r
+\r
+/* target break-/watchpoint control \r
+* rw: 0 = write, 1 = read, 2 = access\r
+*/\r
+int arm11_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint);\r
+int arm11_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint);\r
+int arm11_add_watchpoint(struct target_s *target, watchpoint_t *watchpoint);\r
+int arm11_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint);\r
+\r
+/* target algorithm support */\r
+int arm11_run_algorithm(struct target_s *target, int num_mem_params, mem_param_t *mem_params, int num_reg_params, reg_param_t *reg_param, u32 entry_point, u32 exit_point, int timeout_ms, void *arch_info);\r
+\r
+int arm11_register_commands(struct command_context_s *cmd_ctx);\r
+int arm11_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);\r
+int arm11_init_target(struct command_context_s *cmd_ctx, struct target_s *target);\r
+int arm11_quit(void);\r
+\r
+\r
+/* helpers */\r
+void arm11_build_reg_cache(target_t *target);\r
+\r
+\r
+/* internals */\r
+\r
+void arm11_setup_field         (arm11_common_t * arm11, int num_bits, void * in_data, void * out_data, scan_field_t * field);\r
+void arm11_add_IR              (arm11_common_t * arm11, u8 instr, enum tap_state state);\r
+void arm11_add_debug_SCAN_N    (arm11_common_t * arm11, u8 chain, enum tap_state state);\r
+void arm11_add_debug_INST      (arm11_common_t * arm11, u32 inst, u8 * flag, enum tap_state state);\r
+u32  arm11_read_DSCR           (arm11_common_t * arm11);\r
+void arm11_write_DSCR          (arm11_common_t * arm11, u32 dscr);\r
+\r
+enum target_debug_reason arm11_get_DSCR_debug_reason(u32 dscr);\r
+\r
+void arm11_run_instr_data_prepare              (arm11_common_t * arm11);\r
+void arm11_run_instr_data_finish               (arm11_common_t * arm11);\r
+void arm11_run_instr_no_data                   (arm11_common_t * arm11, u32 * opcode, size_t count);\r
+void arm11_run_instr_no_data1                  (arm11_common_t * arm11, u32 opcode);\r
+void arm11_run_instr_data_to_core              (arm11_common_t * arm11, u32 opcode, u32 * data, size_t count);\r
+void arm11_run_instr_data_to_core1             (arm11_common_t * arm11, u32 opcode, u32 data);\r
+void arm11_run_instr_data_from_core            (arm11_common_t * arm11, u32 opcode, u32 * data, size_t count);\r
+void arm11_run_instr_data_from_core_via_r0     (arm11_common_t * arm11, u32 opcode, u32 * data);\r
+void arm11_run_instr_data_to_core_via_r0       (arm11_common_t * arm11, u32 opcode, u32 data);\r
+\r
+\r
+typedef struct arm11_sc7_action_s\r
+{\r
+    bool    write;\r
+    u8     address;\r
+    u32            value;\r
+} arm11_sc7_action_t;\r
+\r
+void arm11_sc7_run(arm11_common_t * arm11, arm11_sc7_action_t * actions, size_t count);\r
+void arm11_sc7_clear_bw(arm11_common_t * arm11);\r
+\r
+\r
+\r
+#endif /* ARM11_H */\r
diff --git a/src/target/arm11_dbgtap.c b/src/target/arm11_dbgtap.c
new file mode 100644 (file)
index 0000000..13b412c
--- /dev/null
@@ -0,0 +1,611 @@
+/***************************************************************************\r
+ *   Copyright (C) 2008 digenius technology GmbH.                          *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "arm11.h"\r
+#include "jtag.h"\r
+#include "log.h"\r
+\r
+#include <stdlib.h>\r
+#include <string.h>\r
+\r
+#if 0\r
+#define JTAG_DEBUG(expr ...) \\r
+       do { \\r
+           log_printf (LOG_DEBUG, __FILE__, __LINE__, __FUNCTION__, expr); \\r
+       } while(0)\r
+#else\r
+#define JTAG_DEBUG(expr ...) \\r
+       do {} while(0)\r
+#endif\r
+\r
+/** Code de-clutter: Construct scan_field_t to write out a value\r
+ *\r
+ * \param arm11                Target state variable.\r
+ * \param num_bits     Length of the data field\r
+ * \param out_data     pointer to the data that will be sent out\r
+ *                     <em>(data is read when it is added to the JTAG queue)</em>\r
+ * \param in_data      pointer to the memory that will receive data that was clocked in\r
+ *                     <em>(data is written when the JTAG queue is executed)</em>\r
+ * \param field target data structure that will be initialized\r
+ */\r
+void arm11_setup_field(arm11_common_t * arm11, int num_bits, void * out_data, void * in_data, scan_field_t * field)\r
+{\r
+    field->device              = arm11->jtag_info.chain_pos;\r
+    field->num_bits            = num_bits;\r
+    field->out_mask            = NULL;\r
+    field->in_check_mask       = NULL;\r
+    field->in_check_value      = NULL;\r
+    field->in_handler          = NULL;\r
+    field->in_handler_priv     = NULL;\r
+\r
+    field->out_value           = out_data;\r
+    field->in_value            = in_data;\r
+}\r
+\r
+\r
+/** Write JTAG instruction register\r
+ *\r
+ * \param arm11 Target state variable.\r
+ * \param instr An ARM11 DBGTAP instruction. Use enum #arm11_instructions.\r
+ * \param state Pass the final TAP state or -1 for the default value (Pause-IR).\r
+ *\r
+ * \remarks This adds to the JTAG command queue but does \em not execute it.\r
+ */\r
+void arm11_add_IR(arm11_common_t * arm11, u8 instr, enum tap_state state)\r
+{\r
+    jtag_device_t *device = jtag_get_device(arm11->jtag_info.chain_pos);\r
+\r
+    if (buf_get_u32(device->cur_instr, 0, 5) == instr)\r
+    {\r
+       JTAG_DEBUG("IR <= 0x%02x SKIPPED", instr);\r
+       return;\r
+    }\r
+\r
+    JTAG_DEBUG("IR <= 0x%02x", instr);\r
+\r
+    scan_field_t field;\r
+\r
+    arm11_setup_field(arm11, 5, &instr, NULL, &field);\r
+\r
+    jtag_add_ir_scan_vc(1, &field, state == -1 ? TAP_PI : state);\r
+}\r
+\r
+/** Verify shifted out data from Scan Chain Register (SCREG)\r
+ *  Used as parameter to scan_field_t::in_handler in\r
+ *  arm11_add_debug_SCAN_N().\r
+ *\r
+ */\r
+static int arm11_in_handler_SCAN_N(u8 *in_value, void *priv, struct scan_field_s *field)\r
+{\r
+    /** \todo TODO: clarify why this isnt properly masked in jtag.c jtag_read_buffer() */\r
+    u8 v = *in_value & 0x1F;\r
+\r
+    if (v != 0x10)\r
+    {\r
+        ERROR("'arm11 target' JTAG communication error SCREG SCAN OUT 0x%02x (expected 0x10)", v);\r
+        exit(-1);\r
+    }\r
+\r
+    JTAG_DEBUG("SCREG SCAN OUT 0x%02x", v);\r
+    return ERROR_OK;\r
+}\r
+\r
+/** Select and write to Scan Chain Register (SCREG)\r
+ * \r
+ * This function sets the instruction register to SCAN_N and writes\r
+ * the data register with the selected chain number.\r
+ *\r
+ * http://infocenter.arm.com/help/topic/com.arm.doc.ddi0301f/Cacbjhfg.html\r
+ *\r
+ * \param arm11            Target state variable.\r
+ * \param chain            Scan chain that will be selected.\r
+ * \param state            Pass the final TAP state or -1 for the default\r
+ *                 value (Pause-DR).\r
+ *\r
+ * The chain takes effect when Update-DR is passed (usually when subsequently\r
+ * the INTEXT/EXTEST instructions are written).\r
+ *\r
+ * \warning (Obsolete) Using this twice in a row will \em fail. The first call will end\r
+ *          in Pause-DR. The second call, due to the IR caching, will not\r
+ *         go through Capture-DR when shifting in the new scan chain number.\r
+ *         As a result the verification in arm11_in_handler_SCAN_N() must\r
+ *         fail.\r
+ *\r
+ * \remarks This adds to the JTAG command queue but does \em not execute it.\r
+ */\r
+\r
+void arm11_add_debug_SCAN_N(arm11_common_t * arm11, u8 chain, enum tap_state state)\r
+{\r
+    JTAG_DEBUG("SCREG <= 0x%02x", chain);\r
+\r
+    arm11_add_IR(arm11, ARM11_SCAN_N, -1);\r
+\r
+    scan_field_t               field;\r
+\r
+    arm11_setup_field(arm11, 5, &chain, NULL, &field);\r
+\r
+    field.in_handler = arm11_in_handler_SCAN_N;\r
+\r
+    jtag_add_dr_scan_vc(1, &field, state == -1 ? TAP_PD : state);\r
+}\r
+\r
+/** Write an instruction into the ITR register\r
+ * \r
+ * \param arm11        Target state variable.\r
+ * \param inst An ARM11 processor instruction/opcode.\r
+ * \param flag Optional parameter to retrieve the InstCompl flag\r
+ *             (this will be written when the JTAG chain is executed). \r
+ * \param state        Pass the final TAP state or -1 for the default\r
+ *             value (Run-Test/Idle).\r
+ *\r
+ * \remarks By default this ends with Run-Test/Idle state\r
+ * and causes the instruction to be executed. If\r
+ * a subsequent write to DTR is needed before\r
+ * executing the instruction then TAP_PD should be\r
+ * passed to \p state.\r
+ *\r
+ * \remarks This adds to the JTAG command queue but does \em not execute it.\r
+ */\r
+void arm11_add_debug_INST(arm11_common_t * arm11, u32 inst, u8 * flag, enum tap_state state)\r
+{\r
+    JTAG_DEBUG("INST <= 0x%08x", inst);\r
+\r
+    scan_field_t               itr[2];\r
+\r
+    arm11_setup_field(arm11, 32,    &inst,     NULL, itr + 0);\r
+    arm11_setup_field(arm11, 1,            NULL,       flag, itr + 1);\r
+\r
+    jtag_add_dr_scan_vc(asizeof(itr), itr, state == -1 ? TAP_RTI : state);\r
+}\r
+\r
+/** Read the Debug Status and Control Register (DSCR)\r
+ *\r
+ * same as CP14 c1\r
+ *\r
+ * \param arm11 Target state variable.\r
+ * \return DSCR content\r
+ * \r
+ * \remarks This is a stand-alone function that executes the JTAG command queue.\r
+ */\r
+u32 arm11_read_DSCR(arm11_common_t * arm11)\r
+{\r
+    arm11_add_debug_SCAN_N(arm11, 0x01, -1);\r
+\r
+    arm11_add_IR(arm11, ARM11_INTEST, -1);\r
+\r
+    u32                        dscr;\r
+    scan_field_t       chain1_field;\r
+\r
+    arm11_setup_field(arm11, 32, NULL, &dscr, &chain1_field);\r
+\r
+    jtag_add_dr_scan_vc(1, &chain1_field, TAP_PD);\r
+\r
+    jtag_execute_queue();\r
+\r
+    if (arm11->last_dscr != dscr)\r
+        JTAG_DEBUG("DSCR  = %08x (OLD %08x)", dscr, arm11->last_dscr);\r
+\r
+    arm11->last_dscr = dscr;\r
+\r
+    return dscr;\r
+}\r
+\r
+/** Write the Debug Status and Control Register (DSCR)\r
+ *\r
+ * same as CP14 c1\r
+ *\r
+ * \param arm11 Target state variable.\r
+ * \param dscr DSCR content\r
+ * \r
+ * \remarks This is a stand-alone function that executes the JTAG command queue.\r
+ */\r
+void arm11_write_DSCR(arm11_common_t * arm11, u32 dscr)\r
+{\r
+    arm11_add_debug_SCAN_N(arm11, 0x01, -1);\r
+\r
+    arm11_add_IR(arm11, ARM11_EXTEST, -1);\r
+\r
+    scan_field_t                   chain1_field;\r
+\r
+    arm11_setup_field(arm11, 32, &dscr, NULL, &chain1_field);\r
+\r
+    jtag_add_dr_scan_vc(1, &chain1_field, TAP_PD);\r
+\r
+    jtag_execute_queue();\r
+\r
+    JTAG_DEBUG("DSCR <= %08x (OLD %08x)", dscr, arm11->last_dscr);\r
+\r
+    arm11->last_dscr = dscr;\r
+}\r
+\r
+\r
+\r
+/** Get the debug reason from Debug Status and Control Register (DSCR)\r
+ *\r
+ * \param dscr DSCR value to analyze\r
+ * \return Debug reason\r
+ * \r
+ */\r
+enum target_debug_reason arm11_get_DSCR_debug_reason(u32 dscr)\r
+{\r
+    switch (dscr & ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_MASK)\r
+    {\r
+    case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_HALT:                        return DBG_REASON_DBGRQ;\r
+    case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_BREAKPOINT:          return DBG_REASON_BREAKPOINT;\r
+    case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_WATCHPOINT:          return DBG_REASON_WATCHPOINT;\r
+    case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_BKPT_INSTRUCTION:    return DBG_REASON_BREAKPOINT;\r
+    case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_EDBGRQ:              return DBG_REASON_DBGRQ;\r
+    case ARM11_DSCR_METHOD_OF_DEBUG_ENTRY_VECTOR_CATCH:                return DBG_REASON_BREAKPOINT;\r
+\r
+    default:\r
+       return DBG_REASON_DBGRQ;\r
+    }\r
+};\r
+\r
+\r
+\r
+/** Prepare the stage for ITR/DTR operations\r
+ * from the arm11_run_instr... group of functions.\r
+ *\r
+ * Put arm11_run_instr_data_prepare() and arm11_run_instr_data_finish()\r
+ * around a block of arm11_run_instr_... calls.\r
+ *\r
+ * Select scan chain 5 to allow quick access to DTR. When scan\r
+ * chain 4 is needed to put in a register the ITRSel instruction\r
+ * shortcut is used instead of actually changing the Scan_N\r
+ * register.\r
+ *\r
+ * \param arm11 Target state variable.\r
+ *\r
+ */\r
+void arm11_run_instr_data_prepare(arm11_common_t * arm11)\r
+{\r
+    arm11_add_debug_SCAN_N(arm11, 0x05, -1);\r
+}\r
+\r
+/** Cleanup after ITR/DTR operations\r
+ * from the arm11_run_instr... group of functions\r
+ *\r
+ * Put arm11_run_instr_data_prepare() and arm11_run_instr_data_finish()\r
+ * around a block of arm11_run_instr_... calls.\r
+ *\r
+ * Any RTI can lead to an instruction execution when\r
+ * scan chains 4 or 5 are selected and the IR holds\r
+ * INTEST or EXTEST. So we must disable that before\r
+ * any following activities lead to an RTI.\r
+ *\r
+ * \param arm11 Target state variable.\r
+ *\r
+ */\r
+void arm11_run_instr_data_finish(arm11_common_t * arm11)\r
+{\r
+    arm11_add_debug_SCAN_N(arm11, 0x00, -1);\r
+}\r
+\r
+\r
+/** Execute one or multiple instructions via ITR\r
+ *\r
+ * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block\r
+ *\r
+ * \param arm11                Target state variable.\r
+ * \param opcode       Pointer to sequence of ARM opcodes\r
+ * \param count                Number of opcodes to execute\r
+ *\r
+ */\r
+void arm11_run_instr_no_data(arm11_common_t * arm11, u32 * opcode, size_t count)\r
+{\r
+    arm11_add_IR(arm11, ARM11_ITRSEL, -1);\r
+\r
+    while (count--)\r
+    {\r
+       arm11_add_debug_INST(arm11, *opcode++, NULL, TAP_RTI);\r
+\r
+       while (1)\r
+       {\r
+           u8 flag;\r
+\r
+           arm11_add_debug_INST(arm11, 0, &flag, count ? TAP_RTI : TAP_PD);\r
+\r
+           jtag_execute_queue();\r
+\r
+           if (flag)\r
+               break;\r
+       }\r
+    }\r
+}\r
+\r
+/** Execute one instruction via ITR\r
+ *\r
+ * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block\r
+ *\r
+ * \param arm11                Target state variable.\r
+ * \param opcode       ARM opcode\r
+ *\r
+ */\r
+void arm11_run_instr_no_data1(arm11_common_t * arm11, u32 opcode)\r
+{\r
+    arm11_run_instr_no_data(arm11, &opcode, 1);\r
+}\r
+\r
+\r
+/** Execute one instruction via ITR repeatedly while\r
+ *  passing data to the core via DTR on each execution.\r
+ *\r
+ *  The executed instruction \em must read data from DTR.\r
+ *\r
+ * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block\r
+ *\r
+ * \param arm11                Target state variable.\r
+ * \param opcode       ARM opcode\r
+ * \param data         Pointer to the data words to be passed to the core\r
+ * \param count                Number of data words and instruction repetitions\r
+ *\r
+ */\r
+void arm11_run_instr_data_to_core(arm11_common_t * arm11, u32 opcode, u32 * data, size_t count)\r
+{\r
+    arm11_add_IR(arm11, ARM11_ITRSEL, -1);\r
+\r
+    arm11_add_debug_INST(arm11, opcode, NULL, TAP_PD);\r
+\r
+    arm11_add_IR(arm11, ARM11_EXTEST, -1);\r
+\r
+    scan_field_t       chain5_fields[3];\r
+\r
+    u32                        Data;\r
+    u8                 Ready;\r
+    u8                 nRetry;\r
+\r
+    arm11_setup_field(arm11, 32,    &Data,  NULL,      chain5_fields + 0);\r
+    arm11_setup_field(arm11,  1,    NULL,   &Ready,    chain5_fields + 1);\r
+    arm11_setup_field(arm11,  1,    NULL,   &nRetry,   chain5_fields + 2);\r
+\r
+    while (count--)\r
+    {\r
+       do\r
+       {\r
+           Data            = *data;\r
+\r
+           jtag_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, TAP_RTI);\r
+           jtag_execute_queue();\r
+\r
+           JTAG_DEBUG("DTR  Ready %d  nRetry %d", Ready, nRetry);\r
+       }\r
+       while (!Ready);\r
+\r
+       data++;\r
+    }\r
+\r
+    arm11_add_IR(arm11, ARM11_INTEST, -1);\r
+\r
+    do\r
+    {\r
+       Data        = 0;\r
+\r
+       jtag_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, TAP_PD);\r
+       jtag_execute_queue();\r
+\r
+       JTAG_DEBUG("DTR  Data %08x  Ready %d  nRetry %d", Data, Ready, nRetry);\r
+    }\r
+    while (!Ready);\r
+\r
+\r
+}\r
+\r
+/** Execute an instruction via ITR while handing data into the core via DTR.\r
+ *\r
+ *  The executed instruction \em must read data from DTR.\r
+ *\r
+ * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block\r
+ *\r
+ * \param arm11                Target state variable.\r
+ * \param opcode       ARM opcode\r
+ * \param data         Data word to be passed to the core via DTR\r
+ *\r
+ */\r
+void arm11_run_instr_data_to_core1(arm11_common_t * arm11, u32 opcode, u32 data)\r
+{\r
+    arm11_run_instr_data_to_core(arm11, opcode, &data, 1);\r
+}\r
+\r
+\r
+/** Execute one instruction via ITR repeatedly while\r
+ *  reading data from the core via DTR on each execution.\r
+ *\r
+ *  The executed instruction \em must write data to DTR.\r
+ *\r
+ * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block\r
+ *\r
+ * \param arm11                Target state variable.\r
+ * \param opcode       ARM opcode\r
+ * \param data         Pointer to an array that receives the data words from the core\r
+ * \param count                Number of data words and instruction repetitions\r
+ *\r
+ */\r
+void arm11_run_instr_data_from_core(arm11_common_t * arm11, u32 opcode, u32 * data, size_t count)\r
+{\r
+    arm11_add_IR(arm11, ARM11_ITRSEL, -1);\r
+\r
+    arm11_add_debug_INST(arm11, opcode, NULL, TAP_RTI);\r
+\r
+    arm11_add_IR(arm11, ARM11_INTEST, -1);\r
+\r
+    scan_field_t       chain5_fields[3];\r
+\r
+    u32                        Data;\r
+    u8                 Ready;\r
+    u8                 nRetry;\r
+\r
+    arm11_setup_field(arm11, 32,    NULL,      &Data,      chain5_fields + 0);\r
+    arm11_setup_field(arm11,  1,    NULL,      &Ready,     chain5_fields + 1);\r
+    arm11_setup_field(arm11,  1,    NULL,      &nRetry,    chain5_fields + 2);\r
+\r
+    while (count--)\r
+    {\r
+       do\r
+       {\r
+           jtag_add_dr_scan_vc(asizeof(chain5_fields), chain5_fields, count ? TAP_RTI : TAP_PD);\r
+           jtag_execute_queue();\r
+\r
+           JTAG_DEBUG("DTR  Data %08x  Ready %d  nRetry %d", Data, Ready, nRetry);\r
+       }\r
+       while (!Ready);\r
+\r
+       *data++ = Data;\r
+    }\r
+}\r
+\r
+/** Execute one instruction via ITR\r
+ *  then load r0 into DTR and read DTR from core.\r
+ *\r
+ *  The first executed instruction (\p opcode) should write data to r0.\r
+ *\r
+ * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block\r
+ *\r
+ * \param arm11                Target state variable.\r
+ * \param opcode       ARM opcode to write r0 with the value of interest\r
+ * \param data         Pointer to a data word that receives the value from r0 after \p opcode was executed.\r
+ *\r
+ */\r
+void arm11_run_instr_data_from_core_via_r0(arm11_common_t * arm11, u32 opcode, u32 * data)\r
+{\r
+    arm11_run_instr_no_data1(arm11, opcode);\r
+\r
+    /* MCR p14,0,R0,c0,c5,0 (move r0 -> wDTR -> local var) */\r
+    arm11_run_instr_data_from_core(arm11, 0xEE000E15, data, 1);\r
+}\r
+\r
+/** Load data into core via DTR then move it to r0 then\r
+ *  execute one instruction via ITR\r
+ *\r
+ *  The final executed instruction (\p opcode) should read data from r0.\r
+ *\r
+ * \pre arm11_run_instr_data_prepare() /  arm11_run_instr_data_finish() block\r
+ *\r
+ * \param arm11                Target state variable.\r
+ * \param opcode       ARM opcode to read r0 act upon it\r
+ * \param data         Data word that will be written to r0 before \p opcode is executed\r
+ *\r
+ */\r
+void arm11_run_instr_data_to_core_via_r0(arm11_common_t * arm11, u32 opcode, u32 data)\r
+{\r
+    /* MRC p14,0,r0,c0,c5,0 */\r
+    arm11_run_instr_data_to_core1(arm11, 0xEE100E15, data);\r
+\r
+    arm11_run_instr_no_data1(arm11, opcode);\r
+}\r
+\r
+\r
+void arm11_sc7_run(arm11_common_t * arm11, arm11_sc7_action_t * actions, size_t count)\r
+{\r
+    arm11_add_debug_SCAN_N(arm11, 0x07, -1);\r
+\r
+    arm11_add_IR(arm11, ARM11_EXTEST, -1);\r
+\r
+    scan_field_t       chain7_fields[3];\r
+\r
+    u8         nRW;\r
+    u32                DataOut;\r
+    u8         AddressOut;\r
+    u8         Ready;\r
+    u32                DataIn;\r
+    u8         AddressIn;\r
+\r
+    arm11_setup_field(arm11,  1, &nRW,         &Ready,         chain7_fields + 0);\r
+    arm11_setup_field(arm11, 32, &DataOut,     &DataIn,        chain7_fields + 1);\r
+    arm11_setup_field(arm11,  7, &AddressOut,  &AddressIn,     chain7_fields + 2);\r
+\r
+    {size_t i;\r
+    for (i = 0; i < count + 1; i++)\r
+    {\r
+       if (i < count)\r
+       {\r
+           nRW         = actions[i].write ? 1 : 0;\r
+           DataOut     = actions[i].value;\r
+           AddressOut  = actions[i].address;\r
+       }\r
+       else\r
+       {\r
+           nRW         = 0;\r
+           DataOut     = 0;\r
+           AddressOut  = 0;\r
+       }\r
+\r
+        do\r
+       {\r
+           JTAG_DEBUG("SC7 <= Address %02x  Data %08x    nRW %d", AddressOut, DataOut, nRW);\r
+\r
+           jtag_add_dr_scan_vc(asizeof(chain7_fields), chain7_fields, TAP_PD);\r
+           jtag_execute_queue();\r
+\r
+           JTAG_DEBUG("SC7 => Address %02x  Data %08x  Ready %d", AddressIn, DataIn, Ready);\r
+       }\r
+       while (!Ready); /* 'nRW' is 'Ready' on read out */\r
+\r
+       if (i > 0)\r
+       {\r
+           if (actions[i - 1].address != AddressIn)\r
+           {\r
+               WARNING("Scan chain 7 shifted out unexpected address");\r
+           }\r
+\r
+           if (!actions[i - 1].write)\r
+           {\r
+               actions[i - 1].value = DataIn;\r
+           }\r
+           else\r
+           {\r
+               if (actions[i - 1].value != DataIn)\r
+               {\r
+                   WARNING("Scan chain 7 shifted out unexpected data");\r
+               }\r
+           }\r
+       }\r
+    }}\r
+\r
+    {size_t i;\r
+    for (i = 0; i < count; i++)\r
+    {\r
+       JTAG_DEBUG("SC7 %02d: %02x %s %08x", i, actions[i].address, actions[i].write ? "<=" : "=>", actions[i].value);\r
+    }}\r
+}\r
+\r
+void arm11_sc7_clear_bw(arm11_common_t * arm11)\r
+{\r
+    size_t actions = arm11->brp + arm11->wrp;\r
+\r
+    arm11_sc7_action_t         clear_bw[actions];\r
+\r
+    {size_t i;\r
+    for (i = 0; i < actions; i++)\r
+    {\r
+       clear_bw[i].write       = true;\r
+       clear_bw[i].value       = 0;\r
+       clear_bw[i].address     =\r
+           i < arm11->brp ?\r
+               ARM11_SC7_BCR0 + i :\r
+               ARM11_SC7_WCR0 + i - arm11->brp;\r
+    }}\r
+\r
+    arm11_sc7_run(arm11, clear_bw, actions);\r
+}\r
+\r
index 62b70e3b6a657aa5c734d7ec20495d0aad8d439d..87a1afb463074c674d5949ad6f1e12d00d29e984 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2005 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "arm720t.h"
-#include "jtag.h"
-#include "log.h"
-
-#include <stdlib.h>
-#include <string.h>
-
-#if 0
-#define _DEBUG_INSTRUCTION_EXECUTION_
-#endif
-
-/* cli handling */
-int arm720t_register_commands(struct command_context_s *cmd_ctx);
-
-int arm720t_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm720t_handle_virt2phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm720t_handle_md_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm720t_handle_mw_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-/* forward declarations */
-int arm720t_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);
-int arm720t_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
-int arm720t_quit();
-int arm720t_arch_state(struct target_s *target);
-int arm720t_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
-int arm720t_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
-int arm720t_soft_reset_halt(struct target_s *target);
-
-target_type_t arm720t_target =
-{
-       .name = "arm720t",
-
-       .poll = arm7_9_poll,
-       .arch_state = arm720t_arch_state,
-
-       .halt = arm7_9_halt,
-       .resume = arm7_9_resume,
-       .step = arm7_9_step,
-
-       .assert_reset = arm7_9_assert_reset,
-       .deassert_reset = arm7_9_deassert_reset,
-       .soft_reset_halt = arm720t_soft_reset_halt,
-       .prepare_reset_halt = arm7_9_prepare_reset_halt,
-       
-       .get_gdb_reg_list = armv4_5_get_gdb_reg_list,
-
-       .read_memory = arm720t_read_memory,
-       .write_memory = arm720t_write_memory,
-       .bulk_write_memory = arm7_9_bulk_write_memory,
-       .checksum_memory = arm7_9_checksum_memory,
-       
-       .run_algorithm = armv4_5_run_algorithm,
-
-       .add_breakpoint = arm7_9_add_breakpoint,
-       .remove_breakpoint = arm7_9_remove_breakpoint,
-       .add_watchpoint = arm7_9_add_watchpoint,
-       .remove_watchpoint = arm7_9_remove_watchpoint,
-
-       .register_commands = arm720t_register_commands,
-       .target_command = arm720t_target_command,
-       .init_target = arm720t_init_target,
-       .quit = arm720t_quit
-};
-
-int arm720t_scan_cp15(target_t *target, u32 out, u32 *in, int instruction, int clock)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       scan_field_t fields[2];
-       u8 out_buf[4];
-       u8 instruction_buf = instruction;
-       
-       buf_set_u32(out_buf, 0, 32, flip_u32(out, 32));
-       
-       jtag_add_end_state(TAP_PD);
-       arm_jtag_scann(jtag_info, 0xf);
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-               
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 1;
-       fields[0].out_value = &instruction_buf;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 32;
-       fields[1].out_value = out_buf;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       if (in)
-       {
-               fields[1].in_handler = arm_jtag_buf_to_u32_flip;
-               fields[1].in_handler_priv = in;
-       } else
-       {
-               fields[1].in_handler = NULL;
-               fields[1].in_handler_priv = NULL;
-       }
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       
-       jtag_add_dr_scan(2, fields, -1, NULL);
-
-       if (clock)
-               jtag_add_runtest(0, -1);
-
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-       jtag_execute_queue();
-
-       if (in)
-               DEBUG("out: %8.8x, in: %8.8x, instruction: %i, clock: %i", out, *in, instruction, clock);
-       else
-               DEBUG("out: %8.8x, instruction: %i, clock: %i", out, instruction, clock);
-#else
-               DEBUG("out: %8.8x, instruction: %i, clock: %i", out, instruction, clock);
-#endif
-
-       return ERROR_OK;
-}
-
-int arm720t_read_cp15(target_t *target, u32 opcode, u32 *value)
-{
-       /* fetch CP15 opcode */
-       arm720t_scan_cp15(target, opcode, NULL, 1, 1);
-       /* "DECODE" stage */
-       arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 1);
-       /* "EXECUTE" stage (1) */
-       arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 0);
-       arm720t_scan_cp15(target, 0x0, NULL, 0, 1);
-       /* "EXECUTE" stage (2) */
-       arm720t_scan_cp15(target, 0x0, NULL, 0, 1);
-       /* "EXECUTE" stage (3), CDATA is read */
-       arm720t_scan_cp15(target, ARMV4_5_NOP, value, 1, 1);
-       
-       return ERROR_OK;
-}
-
-int arm720t_write_cp15(target_t *target, u32 opcode, u32 value)
-{
-       /* fetch CP15 opcode */
-       arm720t_scan_cp15(target, opcode, NULL, 1, 1);
-       /* "DECODE" stage */
-       arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 1);
-       /* "EXECUTE" stage (1) */
-       arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 0);
-       arm720t_scan_cp15(target, 0x0, NULL, 0, 1);
-       /* "EXECUTE" stage (2) */
-       arm720t_scan_cp15(target, value, NULL, 0, 1);
-       arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 1);
-
-       return ERROR_OK;
-}
-
-u32 arm720t_get_ttb(target_t *target)
-{
-       u32 ttb = 0x0;
-
-       arm720t_read_cp15(target, 0xee120f10, &ttb);
-       jtag_execute_queue();
-       
-       ttb &= 0xffffc000;
-       
-       return ttb;
-}
-
-void arm720t_disable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)
-{
-       u32 cp15_control;
-
-       /* read cp15 control register */
-       arm720t_read_cp15(target, 0xee110f10, &cp15_control);
-       jtag_execute_queue();
-               
-       if (mmu)
-               cp15_control &= ~0x1U;
-       
-       if (d_u_cache || i_cache)
-               cp15_control &= ~0x4U;
-
-       arm720t_write_cp15(target, 0xee010f10, cp15_control);
-}
-
-void arm720t_enable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)
-{
-       u32 cp15_control;
-
-       /* read cp15 control register */
-       arm720t_read_cp15(target, 0xee110f10, &cp15_control);
-       jtag_execute_queue();
-               
-       if (mmu)
-               cp15_control |= 0x1U;
-       
-       if (d_u_cache || i_cache)
-               cp15_control |= 0x4U;
-       
-       arm720t_write_cp15(target, 0xee010f10, cp15_control);
-}
-
-void arm720t_post_debug_entry(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;
-       arm720t_common_t *arm720t = arm7tdmi->arch_info;
-       
-       /* examine cp15 control reg */
-       arm720t_read_cp15(target, 0xee110f10, &arm720t->cp15_control_reg);
-       jtag_execute_queue();
-       DEBUG("cp15_control_reg: %8.8x", arm720t->cp15_control_reg);
-
-       arm720t->armv4_5_mmu.mmu_enabled = (arm720t->cp15_control_reg & 0x1U) ? 1 : 0;
-       arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = (arm720t->cp15_control_reg & 0x4U) ? 1 : 0;
-       arm720t->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;
-
-       /* save i/d fault status and address register */
-       arm720t_read_cp15(target, 0xee150f10, &arm720t->fsr_reg);
-       arm720t_read_cp15(target, 0xee160f10, &arm720t->far_reg);
-       jtag_execute_queue();
-}
-
-void arm720t_pre_restore_context(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;
-       arm720t_common_t *arm720t = arm7tdmi->arch_info;
-       
-       /* restore i/d fault status and address register */
-       arm720t_write_cp15(target, 0xee050f10, arm720t->fsr_reg);
-       arm720t_write_cp15(target, 0xee060f10, arm720t->far_reg);
-}
-
-int arm720t_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p, arm7tdmi_common_t **arm7tdmi_p, arm720t_common_t **arm720t_p)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9;
-       arm7tdmi_common_t *arm7tdmi;
-       arm720t_common_t *arm720t;
-       
-       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       arm7_9 = armv4_5->arch_info;
-       if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       arm7tdmi = arm7_9->arch_info;
-       if (arm7tdmi->common_magic != ARM7TDMI_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       arm720t = arm7tdmi->arch_info;
-       if (arm720t->common_magic != ARM720T_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       *armv4_5_p = armv4_5;
-       *arm7_9_p = arm7_9;
-       *arm7tdmi_p = arm7tdmi;
-       *arm720t_p = arm720t;
-       
-       return ERROR_OK;
-}
-
-int arm720t_arch_state(struct target_s *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;
-       arm720t_common_t *arm720t = arm7tdmi->arch_info;
-       
-       char *state[] = 
-       {
-               "disabled", "enabled"
-       };
-       
-       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
-       {
-               ERROR("BUG: called for a non-ARMv4/5 target");
-               exit(-1);
-       }
-       
-       USER("target halted in %s state due to %s, current mode: %s\n"
-                       "cpsr: 0x%8.8x pc: 0x%8.8x\n"
-                       "MMU: %s, Cache: %s",
-                        armv4_5_state_strings[armv4_5->core_state],
-                        target_debug_reason_strings[target->debug_reason],
-                        armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)],
-                        buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),
-                        buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32),
-                        state[arm720t->armv4_5_mmu.mmu_enabled],
-                        state[arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled]);
-       
-       return ERROR_OK;
-}
-
-int arm720t_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
-{
-       int retval;
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;
-       arm720t_common_t *arm720t = arm7tdmi->arch_info;
-       
-       /* disable cache, but leave MMU enabled */
-       if (arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled)
-               arm720t_disable_mmu_caches(target, 0, 1, 0);
-       
-       retval = arm7_9_read_memory(target, address, size, count, buffer);
-       
-       if (arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled)
-               arm720t_enable_mmu_caches(target, 0, 1, 0);
-       
-       return retval;
-}
-
-int arm720t_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
-{
-       int retval;
-       
-       if ((retval = arm7_9_write_memory(target, address, size, count, buffer)) != ERROR_OK)
-               return retval;
-
-       return retval;
-}
-
-int arm720t_soft_reset_halt(struct target_s *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;
-       arm720t_common_t *arm720t = arm7tdmi->arch_info;
-       reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
-       
-       if (target->state == TARGET_RUNNING)
-       {
-               target->type->halt(target);
-       }
-       
-       while (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) == 0)
-       {
-               embeddedice_read_reg(dbg_stat);
-               jtag_execute_queue();
-       }
-       
-       target->state = TARGET_HALTED;
-       
-       /* SVC, ARM state, IRQ and FIQ disabled */
-       buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8, 0xd3);
-       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
-       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
-       
-       /* start fetching from 0x0 */
-       buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, 0x0);
-       armv4_5->core_cache->reg_list[15].dirty = 1;
-       armv4_5->core_cache->reg_list[15].valid = 1;
-       
-       armv4_5->core_mode = ARMV4_5_MODE_SVC;
-       armv4_5->core_state = ARMV4_5_STATE_ARM;
-       
-       arm720t_disable_mmu_caches(target, 1, 1, 1);
-       arm720t->armv4_5_mmu.mmu_enabled = 0;
-       arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 0;
-       arm720t->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;
-
-       target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-       
-       return ERROR_OK;
-}
-
-int arm720t_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
-{
-       arm7tdmi_init_target(cmd_ctx, target);
-               
-       return ERROR_OK;
-       
-}
-
-int arm720t_quit()
-{
-       
-       return ERROR_OK;
-}
-
-int arm720t_init_arch_info(target_t *target, arm720t_common_t *arm720t, int chain_pos, char *variant)
-{
-       arm7tdmi_common_t *arm7tdmi = &arm720t->arm7tdmi_common;
-       arm7_9_common_t *arm7_9 = &arm7tdmi->arm7_9_common;
-       
-       arm7tdmi_init_arch_info(target, arm7tdmi, chain_pos, variant);
-
-       arm7tdmi->arch_info = arm720t;
-       arm720t->common_magic = ARM720T_COMMON_MAGIC;
-       
-       arm7_9->post_debug_entry = arm720t_post_debug_entry;
-       arm7_9->pre_restore_context = arm720t_pre_restore_context;
-       
-       arm720t->armv4_5_mmu.armv4_5_cache.ctype = -1;
-       arm720t->armv4_5_mmu.get_ttb = arm720t_get_ttb;
-       arm720t->armv4_5_mmu.read_memory = arm7_9_read_memory;
-       arm720t->armv4_5_mmu.write_memory = arm7_9_write_memory;
-       arm720t->armv4_5_mmu.disable_mmu_caches = arm720t_disable_mmu_caches;
-       arm720t->armv4_5_mmu.enable_mmu_caches = arm720t_enable_mmu_caches;
-       arm720t->armv4_5_mmu.has_tiny_pages = 0;
-       arm720t->armv4_5_mmu.mmu_enabled = 0;
-       
-       return ERROR_OK;
-}
-
-int arm720t_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)
-{
-       int chain_pos;
-       char *variant = NULL;
-       arm720t_common_t *arm720t = malloc(sizeof(arm720t_common_t));
-       
-       if (argc < 4)
-       {
-               ERROR("'target arm720t' requires at least one additional argument");
-               exit(-1);
-       }
-       
-       chain_pos = strtoul(args[3], NULL, 0);
-       
-       if (argc >= 5)
-               variant = args[4];
-       
-       DEBUG("chain_pos: %i, variant: %s", chain_pos, variant);
-       
-       arm720t_init_arch_info(target, arm720t, chain_pos, variant);
-
-       return ERROR_OK;
-}
-
-int arm720t_register_commands(struct command_context_s *cmd_ctx)
-{
-       int retval;
-       command_t *arm720t_cmd;
-       
-               
-       retval = arm7tdmi_register_commands(cmd_ctx);
-       
-       arm720t_cmd = register_command(cmd_ctx, NULL, "arm720t", NULL, COMMAND_ANY, "arm720t specific commands");
-
-       register_command(cmd_ctx, arm720t_cmd, "cp15", arm720t_handle_cp15_command, COMMAND_EXEC, "display/modify cp15 register <opcode> [value]");
-       register_command(cmd_ctx, arm720t_cmd, "virt2phys", arm720t_handle_virt2phys_command, COMMAND_EXEC, "translate va to pa <va>");
-
-       register_command(cmd_ctx, arm720t_cmd, "mdw_phys", arm720t_handle_md_phys_command, COMMAND_EXEC, "display memory words <physical addr> [count]");
-       register_command(cmd_ctx, arm720t_cmd, "mdh_phys", arm720t_handle_md_phys_command, COMMAND_EXEC, "display memory half-words <physical addr> [count]");
-       register_command(cmd_ctx, arm720t_cmd, "mdb_phys", arm720t_handle_md_phys_command, COMMAND_EXEC, "display memory bytes <physical addr> [count]");
-
-       register_command(cmd_ctx, arm720t_cmd, "mww_phys", arm720t_handle_mw_phys_command, COMMAND_EXEC, "write memory word <physical addr> <value>");
-       register_command(cmd_ctx, arm720t_cmd, "mwh_phys", arm720t_handle_mw_phys_command, COMMAND_EXEC, "write memory half-word <physical addr> <value>");
-       register_command(cmd_ctx, arm720t_cmd, "mwb_phys", arm720t_handle_mw_phys_command, COMMAND_EXEC, "write memory byte <physical addr> <value>");
-       
-       return ERROR_OK;
-}
-
-int arm720t_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       int retval;
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm7tdmi_common_t *arm7tdmi;
-       arm720t_common_t *arm720t;
-       arm_jtag_t *jtag_info;
-
-       if (arm720t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm7tdmi, &arm720t) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM720t target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-
-       /* one or more argument, access a single register (write if second argument is given */
-       if (argc >= 1)
-       {
-               u32 opcode = strtoul(args[0], NULL, 0);
-
-               if (argc == 1)
-               {
-                       u32 value;
-                       if ((retval = arm720t_read_cp15(target, opcode, &value)) != ERROR_OK)
-                       {
-                               command_print(cmd_ctx, "couldn't access cp15 with opcode 0x%8.8x", opcode);
-                               return ERROR_OK;
-                       }
-                       jtag_execute_queue();
-                       
-                       command_print(cmd_ctx, "0x%8.8x: 0x%8.8x", opcode, value);
-               }
-               else if (argc == 2)
-               {
-                       u32 value = strtoul(args[1], NULL, 0);
-                       if ((retval = arm720t_write_cp15(target, opcode, value)) != ERROR_OK)
-                       {
-                               command_print(cmd_ctx, "couldn't access cp15 with opcode 0x%8.8x", opcode);
-                               return ERROR_OK;
-                       }
-                       command_print(cmd_ctx, "0x%8.8x: 0x%8.8x", opcode, value);
-               }
-       }
-
-       return ERROR_OK;
-}
-
-int arm720t_handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
-{      
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm7tdmi_common_t *arm7tdmi;
-       arm720t_common_t *arm720t;
-       arm_jtag_t *jtag_info;
-
-       if (arm720t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm7tdmi, &arm720t) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM720t target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-               
-       return armv4_5_mmu_handle_virt2phys_command(cmd_ctx, cmd, args, argc, target, &arm720t->armv4_5_mmu);
-}
-
-int arm720t_handle_md_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
-{      
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm7tdmi_common_t *arm7tdmi;
-       arm720t_common_t *arm720t;
-       arm_jtag_t *jtag_info;
-
-       if (arm720t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm7tdmi, &arm720t) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM720t target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-       
-       return armv4_5_mmu_handle_md_phys_command(cmd_ctx, cmd, args, argc, target, &arm720t->armv4_5_mmu);
-}
-
-int arm720t_handle_mw_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
-{      
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm7tdmi_common_t *arm7tdmi;
-       arm720t_common_t *arm720t;
-       arm_jtag_t *jtag_info;
-
-       if (arm720t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm7tdmi, &arm720t) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM720t target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-       
-       return armv4_5_mmu_handle_mw_phys_command(cmd_ctx, cmd, args, argc, target, &arm720t->armv4_5_mmu);
-}
-
+/***************************************************************************\r
+ *   Copyright (C) 2005 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "arm720t.h"\r
+#include "jtag.h"\r
+#include "log.h"\r
+\r
+#include <stdlib.h>\r
+#include <string.h>\r
+\r
+#if 0\r
+#define _DEBUG_INSTRUCTION_EXECUTION_\r
+#endif\r
+\r
+/* cli handling */\r
+int arm720t_register_commands(struct command_context_s *cmd_ctx);\r
+\r
+int arm720t_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm720t_handle_virt2phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm720t_handle_md_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm720t_handle_mw_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+/* forward declarations */\r
+int arm720t_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);\r
+int arm720t_init_target(struct command_context_s *cmd_ctx, struct target_s *target);\r
+int arm720t_quit();\r
+int arm720t_arch_state(struct target_s *target);\r
+int arm720t_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);\r
+int arm720t_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);\r
+int arm720t_soft_reset_halt(struct target_s *target);\r
+\r
+target_type_t arm720t_target =\r
+{\r
+       .name = "arm720t",\r
+\r
+       .poll = arm7_9_poll,\r
+       .arch_state = arm720t_arch_state,\r
+\r
+       .halt = arm7_9_halt,\r
+       .resume = arm7_9_resume,\r
+       .step = arm7_9_step,\r
+\r
+       .assert_reset = arm7_9_assert_reset,\r
+       .deassert_reset = arm7_9_deassert_reset,\r
+       .soft_reset_halt = arm720t_soft_reset_halt,\r
+       .prepare_reset_halt = arm7_9_prepare_reset_halt,\r
+       \r
+       .get_gdb_reg_list = armv4_5_get_gdb_reg_list,\r
+\r
+       .read_memory = arm720t_read_memory,\r
+       .write_memory = arm720t_write_memory,\r
+       .bulk_write_memory = arm7_9_bulk_write_memory,\r
+       .checksum_memory = arm7_9_checksum_memory,\r
+       \r
+       .run_algorithm = armv4_5_run_algorithm,\r
+\r
+       .add_breakpoint = arm7_9_add_breakpoint,\r
+       .remove_breakpoint = arm7_9_remove_breakpoint,\r
+       .add_watchpoint = arm7_9_add_watchpoint,\r
+       .remove_watchpoint = arm7_9_remove_watchpoint,\r
+\r
+       .register_commands = arm720t_register_commands,\r
+       .target_command = arm720t_target_command,\r
+       .init_target = arm720t_init_target,\r
+       .quit = arm720t_quit\r
+};\r
+\r
+int arm720t_scan_cp15(target_t *target, u32 out, u32 *in, int instruction, int clock)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       scan_field_t fields[2];\r
+       u8 out_buf[4];\r
+       u8 instruction_buf = instruction;\r
+       \r
+       buf_set_u32(out_buf, 0, 32, flip_u32(out, 32));\r
+       \r
+       jtag_add_end_state(TAP_PD);\r
+       arm_jtag_scann(jtag_info, 0xf);\r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+               \r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 1;\r
+       fields[0].out_value = &instruction_buf;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+\r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 32;\r
+       fields[1].out_value = out_buf;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       if (in)\r
+       {\r
+               fields[1].in_handler = arm_jtag_buf_to_u32_flip;\r
+               fields[1].in_handler_priv = in;\r
+       } else\r
+       {\r
+               fields[1].in_handler = NULL;\r
+               fields[1].in_handler_priv = NULL;\r
+       }\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       \r
+       jtag_add_dr_scan(2, fields, -1);\r
+\r
+       if (clock)\r
+               jtag_add_runtest(0, -1);\r
+\r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+       jtag_execute_queue();\r
+\r
+       if (in)\r
+               DEBUG("out: %8.8x, in: %8.8x, instruction: %i, clock: %i", out, *in, instruction, clock);\r
+       else\r
+               DEBUG("out: %8.8x, instruction: %i, clock: %i", out, instruction, clock);\r
+#else\r
+               DEBUG("out: %8.8x, instruction: %i, clock: %i", out, instruction, clock);\r
+#endif\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm720t_read_cp15(target_t *target, u32 opcode, u32 *value)\r
+{\r
+       /* fetch CP15 opcode */\r
+       arm720t_scan_cp15(target, opcode, NULL, 1, 1);\r
+       /* "DECODE" stage */\r
+       arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 1);\r
+       /* "EXECUTE" stage (1) */\r
+       arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 0);\r
+       arm720t_scan_cp15(target, 0x0, NULL, 0, 1);\r
+       /* "EXECUTE" stage (2) */\r
+       arm720t_scan_cp15(target, 0x0, NULL, 0, 1);\r
+       /* "EXECUTE" stage (3), CDATA is read */\r
+       arm720t_scan_cp15(target, ARMV4_5_NOP, value, 1, 1);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm720t_write_cp15(target_t *target, u32 opcode, u32 value)\r
+{\r
+       /* fetch CP15 opcode */\r
+       arm720t_scan_cp15(target, opcode, NULL, 1, 1);\r
+       /* "DECODE" stage */\r
+       arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 1);\r
+       /* "EXECUTE" stage (1) */\r
+       arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 0);\r
+       arm720t_scan_cp15(target, 0x0, NULL, 0, 1);\r
+       /* "EXECUTE" stage (2) */\r
+       arm720t_scan_cp15(target, value, NULL, 0, 1);\r
+       arm720t_scan_cp15(target, ARMV4_5_NOP, NULL, 1, 1);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+u32 arm720t_get_ttb(target_t *target)\r
+{\r
+       u32 ttb = 0x0;\r
+\r
+       arm720t_read_cp15(target, 0xee120f10, &ttb);\r
+       jtag_execute_queue();\r
+       \r
+       ttb &= 0xffffc000;\r
+       \r
+       return ttb;\r
+}\r
+\r
+void arm720t_disable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)\r
+{\r
+       u32 cp15_control;\r
+\r
+       /* read cp15 control register */\r
+       arm720t_read_cp15(target, 0xee110f10, &cp15_control);\r
+       jtag_execute_queue();\r
+               \r
+       if (mmu)\r
+               cp15_control &= ~0x1U;\r
+       \r
+       if (d_u_cache || i_cache)\r
+               cp15_control &= ~0x4U;\r
+\r
+       arm720t_write_cp15(target, 0xee010f10, cp15_control);\r
+}\r
+\r
+void arm720t_enable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)\r
+{\r
+       u32 cp15_control;\r
+\r
+       /* read cp15 control register */\r
+       arm720t_read_cp15(target, 0xee110f10, &cp15_control);\r
+       jtag_execute_queue();\r
+               \r
+       if (mmu)\r
+               cp15_control |= 0x1U;\r
+       \r
+       if (d_u_cache || i_cache)\r
+               cp15_control |= 0x4U;\r
+       \r
+       arm720t_write_cp15(target, 0xee010f10, cp15_control);\r
+}\r
+\r
+void arm720t_post_debug_entry(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;\r
+       arm720t_common_t *arm720t = arm7tdmi->arch_info;\r
+       \r
+       /* examine cp15 control reg */\r
+       arm720t_read_cp15(target, 0xee110f10, &arm720t->cp15_control_reg);\r
+       jtag_execute_queue();\r
+       DEBUG("cp15_control_reg: %8.8x", arm720t->cp15_control_reg);\r
+\r
+       arm720t->armv4_5_mmu.mmu_enabled = (arm720t->cp15_control_reg & 0x1U) ? 1 : 0;\r
+       arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = (arm720t->cp15_control_reg & 0x4U) ? 1 : 0;\r
+       arm720t->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;\r
+\r
+       /* save i/d fault status and address register */\r
+       arm720t_read_cp15(target, 0xee150f10, &arm720t->fsr_reg);\r
+       arm720t_read_cp15(target, 0xee160f10, &arm720t->far_reg);\r
+       jtag_execute_queue();\r
+}\r
+\r
+void arm720t_pre_restore_context(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;\r
+       arm720t_common_t *arm720t = arm7tdmi->arch_info;\r
+       \r
+       /* restore i/d fault status and address register */\r
+       arm720t_write_cp15(target, 0xee050f10, arm720t->fsr_reg);\r
+       arm720t_write_cp15(target, 0xee060f10, arm720t->far_reg);\r
+}\r
+\r
+int arm720t_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p, arm7tdmi_common_t **arm7tdmi_p, arm720t_common_t **arm720t_p)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9;\r
+       arm7tdmi_common_t *arm7tdmi;\r
+       arm720t_common_t *arm720t;\r
+       \r
+       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       arm7_9 = armv4_5->arch_info;\r
+       if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       arm7tdmi = arm7_9->arch_info;\r
+       if (arm7tdmi->common_magic != ARM7TDMI_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       arm720t = arm7tdmi->arch_info;\r
+       if (arm720t->common_magic != ARM720T_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       *armv4_5_p = armv4_5;\r
+       *arm7_9_p = arm7_9;\r
+       *arm7tdmi_p = arm7tdmi;\r
+       *arm720t_p = arm720t;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm720t_arch_state(struct target_s *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;\r
+       arm720t_common_t *arm720t = arm7tdmi->arch_info;\r
+       \r
+       char *state[] = \r
+       {\r
+               "disabled", "enabled"\r
+       };\r
+       \r
+       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)\r
+       {\r
+               ERROR("BUG: called for a non-ARMv4/5 target");\r
+               exit(-1);\r
+       }\r
+       \r
+       USER("target halted in %s state due to %s, current mode: %s\n"\r
+                       "cpsr: 0x%8.8x pc: 0x%8.8x\n"\r
+                       "MMU: %s, Cache: %s",\r
+                        armv4_5_state_strings[armv4_5->core_state],\r
+                        target_debug_reason_strings[target->debug_reason],\r
+                        armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)],\r
+                        buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),\r
+                        buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32),\r
+                        state[arm720t->armv4_5_mmu.mmu_enabled],\r
+                        state[arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled]);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm720t_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)\r
+{\r
+       int retval;\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;\r
+       arm720t_common_t *arm720t = arm7tdmi->arch_info;\r
+       \r
+       /* disable cache, but leave MMU enabled */\r
+       if (arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled)\r
+               arm720t_disable_mmu_caches(target, 0, 1, 0);\r
+       \r
+       retval = arm7_9_read_memory(target, address, size, count, buffer);\r
+       \r
+       if (arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled)\r
+               arm720t_enable_mmu_caches(target, 0, 1, 0);\r
+       \r
+       return retval;\r
+}\r
+\r
+int arm720t_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)\r
+{\r
+       int retval;\r
+       \r
+       if ((retval = arm7_9_write_memory(target, address, size, count, buffer)) != ERROR_OK)\r
+               return retval;\r
+\r
+       return retval;\r
+}\r
+\r
+int arm720t_soft_reset_halt(struct target_s *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;\r
+       arm720t_common_t *arm720t = arm7tdmi->arch_info;\r
+       reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];\r
+       \r
+       if (target->state == TARGET_RUNNING)\r
+       {\r
+               target->type->halt(target);\r
+       }\r
+       \r
+       while (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) == 0)\r
+       {\r
+               embeddedice_read_reg(dbg_stat);\r
+               jtag_execute_queue();\r
+       }\r
+       \r
+       target->state = TARGET_HALTED;\r
+       \r
+       /* SVC, ARM state, IRQ and FIQ disabled */\r
+       buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8, 0xd3);\r
+       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;\r
+       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;\r
+       \r
+       /* start fetching from 0x0 */\r
+       buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, 0x0);\r
+       armv4_5->core_cache->reg_list[15].dirty = 1;\r
+       armv4_5->core_cache->reg_list[15].valid = 1;\r
+       \r
+       armv4_5->core_mode = ARMV4_5_MODE_SVC;\r
+       armv4_5->core_state = ARMV4_5_STATE_ARM;\r
+       \r
+       arm720t_disable_mmu_caches(target, 1, 1, 1);\r
+       arm720t->armv4_5_mmu.mmu_enabled = 0;\r
+       arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 0;\r
+       arm720t->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;\r
+\r
+       target_call_event_callbacks(target, TARGET_EVENT_HALTED);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm720t_init_target(struct command_context_s *cmd_ctx, struct target_s *target)\r
+{\r
+       arm7tdmi_init_target(cmd_ctx, target);\r
+               \r
+       return ERROR_OK;\r
+       \r
+}\r
+\r
+int arm720t_quit()\r
+{\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm720t_init_arch_info(target_t *target, arm720t_common_t *arm720t, int chain_pos, char *variant)\r
+{\r
+       arm7tdmi_common_t *arm7tdmi = &arm720t->arm7tdmi_common;\r
+       arm7_9_common_t *arm7_9 = &arm7tdmi->arm7_9_common;\r
+       \r
+       arm7tdmi_init_arch_info(target, arm7tdmi, chain_pos, variant);\r
+\r
+       arm7tdmi->arch_info = arm720t;\r
+       arm720t->common_magic = ARM720T_COMMON_MAGIC;\r
+       \r
+       arm7_9->post_debug_entry = arm720t_post_debug_entry;\r
+       arm7_9->pre_restore_context = arm720t_pre_restore_context;\r
+       \r
+       arm720t->armv4_5_mmu.armv4_5_cache.ctype = -1;\r
+       arm720t->armv4_5_mmu.get_ttb = arm720t_get_ttb;\r
+       arm720t->armv4_5_mmu.read_memory = arm7_9_read_memory;\r
+       arm720t->armv4_5_mmu.write_memory = arm7_9_write_memory;\r
+       arm720t->armv4_5_mmu.disable_mmu_caches = arm720t_disable_mmu_caches;\r
+       arm720t->armv4_5_mmu.enable_mmu_caches = arm720t_enable_mmu_caches;\r
+       arm720t->armv4_5_mmu.has_tiny_pages = 0;\r
+       arm720t->armv4_5_mmu.mmu_enabled = 0;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm720t_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)\r
+{\r
+       int chain_pos;\r
+       char *variant = NULL;\r
+       arm720t_common_t *arm720t = malloc(sizeof(arm720t_common_t));\r
+       \r
+       if (argc < 4)\r
+       {\r
+               ERROR("'target arm720t' requires at least one additional argument");\r
+               exit(-1);\r
+       }\r
+       \r
+       chain_pos = strtoul(args[3], NULL, 0);\r
+       \r
+       if (argc >= 5)\r
+               variant = args[4];\r
+       \r
+       DEBUG("chain_pos: %i, variant: %s", chain_pos, variant);\r
+       \r
+       arm720t_init_arch_info(target, arm720t, chain_pos, variant);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm720t_register_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       int retval;\r
+       command_t *arm720t_cmd;\r
+       \r
+               \r
+       retval = arm7tdmi_register_commands(cmd_ctx);\r
+       \r
+       arm720t_cmd = register_command(cmd_ctx, NULL, "arm720t", NULL, COMMAND_ANY, "arm720t specific commands");\r
+\r
+       register_command(cmd_ctx, arm720t_cmd, "cp15", arm720t_handle_cp15_command, COMMAND_EXEC, "display/modify cp15 register <opcode> [value]");\r
+       register_command(cmd_ctx, arm720t_cmd, "virt2phys", arm720t_handle_virt2phys_command, COMMAND_EXEC, "translate va to pa <va>");\r
+\r
+       register_command(cmd_ctx, arm720t_cmd, "mdw_phys", arm720t_handle_md_phys_command, COMMAND_EXEC, "display memory words <physical addr> [count]");\r
+       register_command(cmd_ctx, arm720t_cmd, "mdh_phys", arm720t_handle_md_phys_command, COMMAND_EXEC, "display memory half-words <physical addr> [count]");\r
+       register_command(cmd_ctx, arm720t_cmd, "mdb_phys", arm720t_handle_md_phys_command, COMMAND_EXEC, "display memory bytes <physical addr> [count]");\r
+\r
+       register_command(cmd_ctx, arm720t_cmd, "mww_phys", arm720t_handle_mw_phys_command, COMMAND_EXEC, "write memory word <physical addr> <value>");\r
+       register_command(cmd_ctx, arm720t_cmd, "mwh_phys", arm720t_handle_mw_phys_command, COMMAND_EXEC, "write memory half-word <physical addr> <value>");\r
+       register_command(cmd_ctx, arm720t_cmd, "mwb_phys", arm720t_handle_mw_phys_command, COMMAND_EXEC, "write memory byte <physical addr> <value>");\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm720t_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       int retval;\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm7tdmi_common_t *arm7tdmi;\r
+       arm720t_common_t *arm720t;\r
+       arm_jtag_t *jtag_info;\r
+\r
+       if (arm720t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm7tdmi, &arm720t) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM720t target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+\r
+       /* one or more argument, access a single register (write if second argument is given */\r
+       if (argc >= 1)\r
+       {\r
+               u32 opcode = strtoul(args[0], NULL, 0);\r
+\r
+               if (argc == 1)\r
+               {\r
+                       u32 value;\r
+                       if ((retval = arm720t_read_cp15(target, opcode, &value)) != ERROR_OK)\r
+                       {\r
+                               command_print(cmd_ctx, "couldn't access cp15 with opcode 0x%8.8x", opcode);\r
+                               return ERROR_OK;\r
+                       }\r
+                       jtag_execute_queue();\r
+                       \r
+                       command_print(cmd_ctx, "0x%8.8x: 0x%8.8x", opcode, value);\r
+               }\r
+               else if (argc == 2)\r
+               {\r
+                       u32 value = strtoul(args[1], NULL, 0);\r
+                       if ((retval = arm720t_write_cp15(target, opcode, value)) != ERROR_OK)\r
+                       {\r
+                               command_print(cmd_ctx, "couldn't access cp15 with opcode 0x%8.8x", opcode);\r
+                               return ERROR_OK;\r
+                       }\r
+                       command_print(cmd_ctx, "0x%8.8x: 0x%8.8x", opcode, value);\r
+               }\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm720t_handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)\r
+{      \r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm7tdmi_common_t *arm7tdmi;\r
+       arm720t_common_t *arm720t;\r
+       arm_jtag_t *jtag_info;\r
+\r
+       if (arm720t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm7tdmi, &arm720t) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM720t target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+               \r
+       return armv4_5_mmu_handle_virt2phys_command(cmd_ctx, cmd, args, argc, target, &arm720t->armv4_5_mmu);\r
+}\r
+\r
+int arm720t_handle_md_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)\r
+{      \r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm7tdmi_common_t *arm7tdmi;\r
+       arm720t_common_t *arm720t;\r
+       arm_jtag_t *jtag_info;\r
+\r
+       if (arm720t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm7tdmi, &arm720t) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM720t target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       return armv4_5_mmu_handle_md_phys_command(cmd_ctx, cmd, args, argc, target, &arm720t->armv4_5_mmu);\r
+}\r
+\r
+int arm720t_handle_mw_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)\r
+{      \r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm7tdmi_common_t *arm7tdmi;\r
+       arm720t_common_t *arm720t;\r
+       arm_jtag_t *jtag_info;\r
+\r
+       if (arm720t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm7tdmi, &arm720t) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM720t target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       return armv4_5_mmu_handle_mw_phys_command(cmd_ctx, cmd, args, argc, target, &arm720t->armv4_5_mmu);\r
+}\r
+\r
index c308380a6a160960581896e84c5e7a0f99ad2b77..5679bbc6e6d4898c05bb251187b08f5e16618fee 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2005 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "arm7tdmi.h"
-
-#include "arm7_9_common.h"
-#include "register.h"
-#include "target.h"
-#include "armv4_5.h"
-#include "embeddedice.h"
-#include "etm.h"
-#include "log.h"
-#include "jtag.h"
-#include "arm_jtag.h"
-
-#include <stdlib.h>
-#include <string.h>
-
-#if 0
-#define _DEBUG_INSTRUCTION_EXECUTION_
-#endif
-
-/* cli handling */
-int arm7tdmi_register_commands(struct command_context_s *cmd_ctx);
-
-/* forward declarations */
-int arm7tdmi_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);
-int arm7tdmi_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
-int arm7tdmi_quit();
-
-/* target function declarations */
-int arm7tdmi_poll(struct target_s *target);
-int arm7tdmi_halt(target_t *target);
-               
-target_type_t arm7tdmi_target =
-{
-       .name = "arm7tdmi",
-
-       .poll = arm7_9_poll,
-       .arch_state = armv4_5_arch_state,
-
-       .target_request_data = arm7_9_target_request_data,
-
-       .halt = arm7_9_halt,
-       .resume = arm7_9_resume,
-       .step = arm7_9_step,
-
-       .assert_reset = arm7_9_assert_reset,
-       .deassert_reset = arm7_9_deassert_reset,
-       .soft_reset_halt = arm7_9_soft_reset_halt,
-       .prepare_reset_halt = arm7_9_prepare_reset_halt,
-
-       .get_gdb_reg_list = armv4_5_get_gdb_reg_list,
-       
-       .read_memory = arm7_9_read_memory,
-       .write_memory = arm7_9_write_memory,
-       .bulk_write_memory = arm7_9_bulk_write_memory,
-       .checksum_memory = arm7_9_checksum_memory,
-       
-       .run_algorithm = armv4_5_run_algorithm,
-       
-       .add_breakpoint = arm7_9_add_breakpoint,
-       .remove_breakpoint = arm7_9_remove_breakpoint,
-       .add_watchpoint = arm7_9_add_watchpoint,
-       .remove_watchpoint = arm7_9_remove_watchpoint,
-
-       .register_commands = arm7tdmi_register_commands,
-       .target_command = arm7tdmi_target_command,
-       .init_target = arm7tdmi_init_target,
-       .quit = arm7tdmi_quit
-};
-
-int arm7tdmi_examine_debug_reason(target_t *target)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       
-       /* only check the debug reason if we don't know it already */
-       if ((target->debug_reason != DBG_REASON_DBGRQ)
-                       && (target->debug_reason != DBG_REASON_SINGLESTEP))
-       {
-               scan_field_t fields[2];
-               u8 databus[4];
-               u8 breakpoint;
-               
-               jtag_add_end_state(TAP_PD);
-
-               fields[0].device = arm7_9->jtag_info.chain_pos;
-               fields[0].num_bits = 1;
-               fields[0].out_value = NULL;
-               fields[0].out_mask = NULL;
-               fields[0].in_value = &breakpoint;
-               fields[0].in_check_value = NULL;
-               fields[0].in_check_mask = NULL;
-               fields[0].in_handler = NULL;
-               fields[0].in_handler_priv = NULL;
-               
-               fields[1].device = arm7_9->jtag_info.chain_pos;
-               fields[1].num_bits = 32;
-               fields[1].out_value = NULL;
-               fields[1].out_mask = NULL;
-               fields[1].in_value = databus;
-               fields[1].in_check_value = NULL;
-               fields[1].in_check_mask = NULL;
-               fields[1].in_handler = NULL;
-               fields[1].in_handler_priv = NULL;
-               
-               arm_jtag_scann(&arm7_9->jtag_info, 0x1);
-               arm_jtag_set_instr(&arm7_9->jtag_info, arm7_9->jtag_info.intest_instr, NULL);
-
-               jtag_add_dr_scan(2, fields, TAP_PD, NULL);
-               jtag_execute_queue();
-               
-               fields[0].in_value = NULL;
-               fields[0].out_value = &breakpoint;
-               fields[1].in_value = NULL;
-               fields[1].out_value = databus;
-               
-               jtag_add_dr_scan(2, fields, TAP_PD, NULL);
-
-               if (breakpoint & 1)
-                       target->debug_reason = DBG_REASON_WATCHPOINT; 
-               else
-                       target->debug_reason = DBG_REASON_BREAKPOINT; 
-       }
-
-       return ERROR_OK;
-}
-
-/* put an instruction in the ARM7TDMI pipeline or write the data bus, and optionally read data */
-int arm7tdmi_clock_out(arm_jtag_t *jtag_info, u32 out, u32 *in, int breakpoint)
-{
-       scan_field_t fields[2];
-       u8 out_buf[4];
-       u8 breakpoint_buf;
-       
-       buf_set_u32(out_buf, 0, 32, flip_u32(out, 32));
-       buf_set_u32(&breakpoint_buf, 0, 1, breakpoint);
-
-       jtag_add_end_state(TAP_PD);
-       arm_jtag_scann(jtag_info, 0x1);
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-       
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 1;
-       fields[0].out_value = &breakpoint_buf;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-               
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 32;
-       fields[1].out_value = out_buf;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       if (in)
-       {
-               fields[1].in_handler = arm_jtag_buf_to_u32_flip;
-               fields[1].in_handler_priv = in;
-       }
-       else
-       {
-               fields[1].in_handler = NULL;
-               fields[1].in_handler_priv = NULL;
-       }
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-
-       jtag_add_dr_scan(2, fields, -1, NULL);
-
-       jtag_add_runtest(0, -1);
-       
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-{
-               jtag_execute_queue();
-               
-               if (in)
-               {
-                       DEBUG("out: 0x%8.8x, in: 0x%8.8x", out, *in);
-               }
-               else
-                       DEBUG("out: 0x%8.8x", out);
-}
-#endif
-
-       return ERROR_OK;
-}
-
-/* clock the target, reading the databus */
-int arm7tdmi_clock_data_in(arm_jtag_t *jtag_info, u32 *in)
-{
-       scan_field_t fields[2];
-
-       jtag_add_end_state(TAP_PD);
-       arm_jtag_scann(jtag_info, 0x1);
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-       
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 1;
-       fields[0].out_value = NULL;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-               
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 32;
-       fields[1].out_value = NULL;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_handler = arm_jtag_buf_to_u32_flip;
-       fields[1].in_handler_priv = in;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-
-       jtag_add_dr_scan(2, fields, -1, NULL);
-
-       jtag_add_runtest(0, -1);
-       
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-{
-               jtag_execute_queue();
-                       
-               if (in)
-               {
-                       DEBUG("in: 0x%8.8x", *in);
-               }
-               else
-               {
-                       ERROR("BUG: called with in == NULL");
-               }
-}
-#endif
-
-       return ERROR_OK;
-}
-
-/* clock the target, and read the databus
- * the *in pointer points to a buffer where elements of 'size' bytes
- * are stored in big (be==1) or little (be==0) endianness
- */ 
-int arm7tdmi_clock_data_in_endianness(arm_jtag_t *jtag_info, void *in, int size, int be)
-{
-       scan_field_t fields[2];
-
-       jtag_add_end_state(TAP_PD);
-       arm_jtag_scann(jtag_info, 0x1);
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-       
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 1;
-       fields[0].out_value = NULL;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-               
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 32;
-       fields[1].out_value = NULL;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       switch (size)
-       {
-               case 4:
-                       fields[1].in_handler = (be) ? arm_jtag_buf_to_be32_flip : arm_jtag_buf_to_le32_flip;
-                       break;
-               case 2:
-                       fields[1].in_handler = (be) ? arm_jtag_buf_to_be16_flip : arm_jtag_buf_to_le16_flip;
-                       break;
-               case 1:
-                       fields[1].in_handler = arm_jtag_buf_to_8_flip;
-                       break;
-       }
-       fields[1].in_handler_priv = in;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-
-       jtag_add_dr_scan(2, fields, -1, NULL);
-
-       jtag_add_runtest(0, -1);
-       
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-{
-               jtag_execute_queue();
-                       
-               if (in)
-               {
-                       DEBUG("in: 0x%8.8x", *in);
-               }
-               else
-               {
-                       ERROR("BUG: called with in == NULL");
-               }
-}
-#endif
-
-       return ERROR_OK;
-}
-
-void arm7tdmi_change_to_arm(target_t *target, u32 *r0, u32 *pc)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       
-       /* save r0 before using it and put system in ARM state 
-        * to allow common handling of ARM and THUMB debugging */
-       
-       /* fetch STR r0, [r0] */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), NULL, 0);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-       /* nothing fetched, STR r0, [r0] in Execute (2) */
-       arm7tdmi_clock_data_in(jtag_info, r0);
-
-       /* MOV r0, r15 fetched, STR in Decode */        
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_MOV(0, 15), NULL, 0);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), NULL, 0);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-       /* nothing fetched, STR r0, [r0] in Execute (2) */
-       arm7tdmi_clock_data_in(jtag_info, pc);
-
-       /* use pc-relative LDR to clear r0[1:0] (for switch to ARM mode) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), NULL, 0);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-       /* nothing fetched, data for LDR r0, [PC, #0] */
-       arm7tdmi_clock_out(jtag_info, 0x0, NULL, 0);
-       /* nothing fetched, data from previous cycle is written to register */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-       
-       /* fetch BX */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_BX(0), NULL, 0);
-       /* NOP fetched, BX in Decode, MOV in Execute */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-       /* NOP fetched, BX in Execute (1) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-       
-       jtag_execute_queue();
-       
-       /* fix program counter:
-        * MOV r0, r15 was the 4th instruction (+6)
-        * reading PC in Thumb state gives address of instruction + 4
-        */
-       *pc -= 0xa;
-       
-}
-
-void arm7tdmi_read_core_regs(target_t *target, u32 mask, u32* core_regs[16])
-{
-       int i;
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-               
-       /* STMIA r0-15, [r0] at debug speed
-        * register values will start to appear on 4th DCLK
-        */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), NULL, 0);
-
-       /* fetch NOP, STM in DECODE stage */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* fetch NOP, STM in EXECUTE stage (1st cycle) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-
-       for (i = 0; i <= 15; i++)
-       {
-               if (mask & (1 << i))
-                       /* nothing fetched, STM still in EXECUTE (1+i cycle) */
-                       arm7tdmi_clock_data_in(jtag_info, core_regs[i]);
-       }
-
-}
-
-void arm7tdmi_read_core_regs_target_buffer(target_t *target, u32 mask, void* buffer, int size)
-{
-       int i;
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       int be = (target->endianness == TARGET_BIG_ENDIAN) ? 1 : 0;
-       u32 *buf_u32 = buffer;
-       u16 *buf_u16 = buffer;
-       u8 *buf_u8 = buffer;
-               
-       /* STMIA r0-15, [r0] at debug speed
-        * register values will start to appear on 4th DCLK
-        */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), NULL, 0);
-
-       /* fetch NOP, STM in DECODE stage */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* fetch NOP, STM in EXECUTE stage (1st cycle) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-
-       for (i = 0; i <= 15; i++)
-       {
-               /* nothing fetched, STM still in EXECUTE (1+i cycle), read databus */
-               if (mask & (1 << i))
-               {
-                       switch (size)
-                       {
-                               case 4:
-                                       arm7tdmi_clock_data_in_endianness(jtag_info, buf_u32++, 4, be);
-                                       break;
-                               case 2:
-                                       arm7tdmi_clock_data_in_endianness(jtag_info, buf_u16++, 2, be);
-                                       break;
-                               case 1:
-                                       arm7tdmi_clock_data_in_endianness(jtag_info, buf_u8++, 1, be);
-                                       break;
-                       }
-               }
-       }
-       
-}
-
-void arm7tdmi_read_xpsr(target_t *target, u32 *xpsr, int spsr)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-               
-       /* MRS r0, cpsr */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_MRS(0, spsr & 1), NULL, 0);
-       
-       /* STR r0, [r15] */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_STR(0, 15), NULL, 0);
-       /* fetch NOP, STR in DECODE stage */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* fetch NOP, STR in EXECUTE stage (1st cycle) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* nothing fetched, STR still in EXECUTE (2nd cycle) */
-       arm7tdmi_clock_data_in(jtag_info, xpsr);
-
-}
-
-void arm7tdmi_write_xpsr(target_t *target, u32 xpsr, int spsr)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-               
-       DEBUG("xpsr: %8.8x, spsr: %i", xpsr, spsr);
-
-       /* MSR1 fetched */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr & 0xff, 0, 1, spsr), NULL, 0);
-       /* MSR2 fetched, MSR1 in DECODE */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff00) >> 8, 0xc, 2, spsr), NULL, 0);
-       /* MSR3 fetched, MSR1 in EXECUTE (1), MSR2 in DECODE */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff0000) >> 16, 0x8, 4, spsr), NULL, 0);
-       /* nothing fetched, MSR1 in EXECUTE (2) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* MSR4 fetched, MSR2 in EXECUTE (1), MSR3 in DECODE */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff000000) >> 24, 0x4, 8, spsr), NULL, 0);
-       /* nothing fetched, MSR2 in EXECUTE (2) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* NOP fetched, MSR3 in EXECUTE (1), MSR4 in DECODE */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* nothing fetched, MSR3 in EXECUTE (2) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* NOP fetched, MSR4 in EXECUTE (1) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* nothing fetched, MSR4 in EXECUTE (2) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-}
-
-void arm7tdmi_write_xpsr_im8(target_t *target, u8 xpsr_im, int rot, int spsr)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-               
-       DEBUG("xpsr_im: %2.2x, rot: %i, spsr: %i", xpsr_im, rot, spsr);
-       
-       /* MSR fetched */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr_im, rot, 1, spsr), NULL, 0);
-       /* NOP fetched, MSR in DECODE */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* NOP fetched, MSR in EXECUTE (1) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* nothing fetched, MSR in EXECUTE (2) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       
-}
-
-void arm7tdmi_write_core_regs(target_t *target, u32 mask, u32 core_regs[16])
-{
-       int i;
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-               
-       /* LDMIA r0-15, [r0] at debug speed
-       * register values will start to appear on 4th DCLK
-       */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 0), NULL, 0);
-
-       /* fetch NOP, LDM in DECODE stage */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* fetch NOP, LDM in EXECUTE stage (1st cycle) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-
-       for (i = 0; i <= 15; i++)
-       {
-               if (mask & (1 << i))
-                       /* nothing fetched, LDM still in EXECUTE (1+i cycle) */
-                       arm7tdmi_clock_out(jtag_info, core_regs[i], NULL, 0);
-       }
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       
-}
-
-void arm7tdmi_load_word_regs(target_t *target, u32 mask)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-
-       /* put system-speed load-multiple into the pipeline */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 1), NULL, 0);
-
-}
-
-void arm7tdmi_load_hword_reg(target_t *target, int num)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       
-       /* put system-speed load half-word into the pipeline */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_LDRH_IP(num, 0), NULL, 0);
-
-}
-
-void arm7tdmi_load_byte_reg(target_t *target, int num)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-
-       /* put system-speed load byte into the pipeline */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_LDRB_IP(num, 0), NULL, 0);
-
-}
-
-void arm7tdmi_store_word_regs(target_t *target, u32 mask)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-
-       /* put system-speed store-multiple into the pipeline */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask, 0, 1), NULL, 0);
-       
-}
-
-void arm7tdmi_store_hword_reg(target_t *target, int num)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-
-       /* put system-speed store half-word into the pipeline */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_STRH_IP(num, 0), NULL, 0);
-
-}
-
-void arm7tdmi_store_byte_reg(target_t *target, int num)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-
-       /* put system-speed store byte into the pipeline */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_STRB_IP(num, 0), NULL, 0);
-
-}
-
-void arm7tdmi_write_pc(target_t *target, u32 pc)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       
-       /* LDMIA r0-15, [r0] at debug speed
-        * register values will start to appear on 4th DCLK
-        */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x8000, 0, 0), NULL, 0);
-       /* fetch NOP, LDM in DECODE stage */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* fetch NOP, LDM in EXECUTE stage (1st cycle) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* nothing fetched, LDM in EXECUTE stage (1st cycle) load register */
-       arm7tdmi_clock_out(jtag_info, pc, NULL, 0);
-       /* nothing fetched, LDM in EXECUTE stage (2nd cycle) load register */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* nothing fetched, LDM in EXECUTE stage (3rd cycle) load register */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* fetch NOP, LDM in EXECUTE stage (4th cycle) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* fetch NOP, LDM in EXECUTE stage (5th cycle) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-}
-
-void arm7tdmi_branch_resume(target_t *target)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_B(0xfffffa, 0), NULL, 0);
-
-}
-
-void arm7tdmi_branch_resume_thumb(target_t *target)
-{
-       DEBUG("-");
-       
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
-
-       /* LDMIA r0, [r0] at debug speed
-        * register values will start to appear on 4th DCLK
-        */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x1, 0, 0), NULL, 0);
-
-       /* fetch NOP, LDM in DECODE stage */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* fetch NOP, LDM in EXECUTE stage (1st cycle) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       /* nothing fetched, LDM in EXECUTE stage (2nd cycle) */
-       arm7tdmi_clock_out(jtag_info, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32) | 1, NULL, 0);
-       /* nothing fetched, LDM in EXECUTE stage (3rd cycle) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-
-       /* Branch and eXchange */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_BX(0), NULL, 0);
-       
-       embeddedice_read_reg(dbg_stat);
-       
-       /* fetch NOP, BX in DECODE stage */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-       
-       /* target is now in Thumb state */
-       embeddedice_read_reg(dbg_stat);
-       
-       /* fetch NOP, BX in EXECUTE stage (1st cycle) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);
-
-       /* target is now in Thumb state */
-       embeddedice_read_reg(dbg_stat);
-
-       /* load r0 value */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), NULL, 0);
-       /* fetch NOP, LDR in Decode */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-       /* fetch NOP, LDR in Execute */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-       /* nothing fetched, LDR in EXECUTE stage (2nd cycle) */
-       arm7tdmi_clock_out(jtag_info, buf_get_u32(armv4_5->core_cache->reg_list[0].value, 0, 32), NULL, 0);
-       /* nothing fetched, LDR in EXECUTE stage (3rd cycle) */
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-       
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);
-
-       embeddedice_read_reg(dbg_stat);
-       
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 1);
-       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_B(0x7f8), NULL, 0);
-
-}
-               
-void arm7tdmi_build_reg_cache(target_t *target)
-{
-       reg_cache_t **cache_p = register_get_last_cache_p(&target->reg_cache);
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-
-       (*cache_p) = armv4_5_build_reg_cache(target, armv4_5);
-       armv4_5->core_cache = (*cache_p);
-       
-       (*cache_p)->next = embeddedice_build_reg_cache(target, arm7_9);
-       arm7_9->eice_cache = (*cache_p)->next;
-       
-       if (arm7_9->etm_ctx)
-       {
-               (*cache_p)->next->next = etm_build_reg_cache(target, jtag_info, arm7_9->etm_ctx);
-               arm7_9->etm_ctx->reg_cache = (*cache_p)->next->next;
-       }
-}
-
-int arm7tdmi_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
-{
-       
-       arm7tdmi_build_reg_cache(target);
-       
-       return ERROR_OK;
-       
-}
-
-int arm7tdmi_quit()
-{
-       
-       return ERROR_OK;
-}
-
-int arm7tdmi_init_arch_info(target_t *target, arm7tdmi_common_t *arm7tdmi, int chain_pos, char *variant)
-{
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       
-       arm7_9 = &arm7tdmi->arm7_9_common;
-       armv4_5 = &arm7_9->armv4_5_common;
-       
-       /* prepare JTAG information for the new target */
-       arm7_9->jtag_info.chain_pos = chain_pos;
-       arm7_9->jtag_info.scann_size = 4;
-       
-       /* register arch-specific functions */
-       arm7_9->examine_debug_reason = arm7tdmi_examine_debug_reason;
-       arm7_9->change_to_arm = arm7tdmi_change_to_arm;
-       arm7_9->read_core_regs = arm7tdmi_read_core_regs;
-       arm7_9->read_core_regs_target_buffer = arm7tdmi_read_core_regs_target_buffer;
-       arm7_9->read_xpsr = arm7tdmi_read_xpsr;
-       
-       arm7_9->write_xpsr = arm7tdmi_write_xpsr;
-       arm7_9->write_xpsr_im8 = arm7tdmi_write_xpsr_im8;
-       arm7_9->write_core_regs = arm7tdmi_write_core_regs;
-       
-       arm7_9->load_word_regs = arm7tdmi_load_word_regs;
-       arm7_9->load_hword_reg = arm7tdmi_load_hword_reg;
-       arm7_9->load_byte_reg = arm7tdmi_load_byte_reg;
-       
-       arm7_9->store_word_regs = arm7tdmi_store_word_regs;
-       arm7_9->store_hword_reg = arm7tdmi_store_hword_reg;
-       arm7_9->store_byte_reg = arm7tdmi_store_byte_reg;
-       
-       arm7_9->write_pc = arm7tdmi_write_pc;
-       arm7_9->branch_resume = arm7tdmi_branch_resume;
-       arm7_9->branch_resume_thumb = arm7tdmi_branch_resume_thumb;
-       
-       arm7_9->enable_single_step = arm7_9_enable_eice_step;
-       arm7_9->disable_single_step = arm7_9_disable_eice_step;
-               
-       arm7_9->pre_debug_entry = NULL;
-       arm7_9->post_debug_entry = NULL;
-       
-       arm7_9->pre_restore_context = NULL;
-       arm7_9->post_restore_context = NULL;
-       
-       /* initialize arch-specific breakpoint handling */
-       arm7_9->arm_bkpt = 0xdeeedeee;
-       arm7_9->thumb_bkpt = 0xdeee;
-       
-       arm7_9->sw_bkpts_use_wp = 1;
-       arm7_9->sw_bkpts_enabled = 0;
-       arm7_9->dbgreq_adjust_pc = 2;
-       arm7_9->arch_info = arm7tdmi;
-
-       arm7tdmi->arch_info = NULL;
-       arm7tdmi->common_magic = ARM7TDMI_COMMON_MAGIC;
-       
-       if (variant)
-       {
-               arm7tdmi->variant = strdup(variant);
-       }
-       else
-       {
-               arm7tdmi->variant = strdup("");
-       }
-       
-       arm7_9_init_arch_info(target, arm7_9);
-
-       return ERROR_OK;
-}
-
-/* target arm7tdmi <endianess> <startup_mode> <chain_pos> <variant> */
-int arm7tdmi_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)
-{
-       int chain_pos;
-       char *variant = NULL;
-       arm7tdmi_common_t *arm7tdmi = malloc(sizeof(arm7tdmi_common_t));
-
-       if (argc < 4)
-       {
-               ERROR("'target arm7tdmi' requires at least one additional argument");
-               exit(-1);
-       }
-       
-       chain_pos = strtoul(args[3], NULL, 0);
-       
-       if (argc >= 5)
-               variant = args[4];
-       
-       arm7tdmi_init_arch_info(target, arm7tdmi, chain_pos, variant);
-       
-       return ERROR_OK;
-}
-
-int arm7tdmi_register_commands(struct command_context_s *cmd_ctx)
-{
-       int retval;
-       
-       retval = arm7_9_register_commands(cmd_ctx);
-       
-       return ERROR_OK;
-
-}
-
+/***************************************************************************\r
+ *   Copyright (C) 2005 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "arm7tdmi.h"\r
+\r
+#include "arm7_9_common.h"\r
+#include "register.h"\r
+#include "target.h"\r
+#include "armv4_5.h"\r
+#include "embeddedice.h"\r
+#include "etm.h"\r
+#include "log.h"\r
+#include "jtag.h"\r
+#include "arm_jtag.h"\r
+\r
+#include <stdlib.h>\r
+#include <string.h>\r
+\r
+#if 0\r
+#define _DEBUG_INSTRUCTION_EXECUTION_\r
+#endif\r
+\r
+/* cli handling */\r
+int arm7tdmi_register_commands(struct command_context_s *cmd_ctx);\r
+\r
+/* forward declarations */\r
+int arm7tdmi_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);\r
+int arm7tdmi_init_target(struct command_context_s *cmd_ctx, struct target_s *target);\r
+int arm7tdmi_quit();\r
+\r
+/* target function declarations */\r
+int arm7tdmi_poll(struct target_s *target);\r
+int arm7tdmi_halt(target_t *target);\r
+               \r
+target_type_t arm7tdmi_target =\r
+{\r
+       .name = "arm7tdmi",\r
+\r
+       .poll = arm7_9_poll,\r
+       .arch_state = armv4_5_arch_state,\r
+\r
+       .target_request_data = arm7_9_target_request_data,\r
+\r
+       .halt = arm7_9_halt,\r
+       .resume = arm7_9_resume,\r
+       .step = arm7_9_step,\r
+\r
+       .assert_reset = arm7_9_assert_reset,\r
+       .deassert_reset = arm7_9_deassert_reset,\r
+       .soft_reset_halt = arm7_9_soft_reset_halt,\r
+       .prepare_reset_halt = arm7_9_prepare_reset_halt,\r
+\r
+       .get_gdb_reg_list = armv4_5_get_gdb_reg_list,\r
+       \r
+       .read_memory = arm7_9_read_memory,\r
+       .write_memory = arm7_9_write_memory,\r
+       .bulk_write_memory = arm7_9_bulk_write_memory,\r
+       .checksum_memory = arm7_9_checksum_memory,\r
+       \r
+       .run_algorithm = armv4_5_run_algorithm,\r
+       \r
+       .add_breakpoint = arm7_9_add_breakpoint,\r
+       .remove_breakpoint = arm7_9_remove_breakpoint,\r
+       .add_watchpoint = arm7_9_add_watchpoint,\r
+       .remove_watchpoint = arm7_9_remove_watchpoint,\r
+\r
+       .register_commands = arm7tdmi_register_commands,\r
+       .target_command = arm7tdmi_target_command,\r
+       .init_target = arm7tdmi_init_target,\r
+       .quit = arm7tdmi_quit\r
+};\r
+\r
+int arm7tdmi_examine_debug_reason(target_t *target)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       \r
+       /* only check the debug reason if we don't know it already */\r
+       if ((target->debug_reason != DBG_REASON_DBGRQ)\r
+                       && (target->debug_reason != DBG_REASON_SINGLESTEP))\r
+       {\r
+               scan_field_t fields[2];\r
+               u8 databus[4];\r
+               u8 breakpoint;\r
+               \r
+               jtag_add_end_state(TAP_PD);\r
+\r
+               fields[0].device = arm7_9->jtag_info.chain_pos;\r
+               fields[0].num_bits = 1;\r
+               fields[0].out_value = NULL;\r
+               fields[0].out_mask = NULL;\r
+               fields[0].in_value = &breakpoint;\r
+               fields[0].in_check_value = NULL;\r
+               fields[0].in_check_mask = NULL;\r
+               fields[0].in_handler = NULL;\r
+               fields[0].in_handler_priv = NULL;\r
+               \r
+               fields[1].device = arm7_9->jtag_info.chain_pos;\r
+               fields[1].num_bits = 32;\r
+               fields[1].out_value = NULL;\r
+               fields[1].out_mask = NULL;\r
+               fields[1].in_value = databus;\r
+               fields[1].in_check_value = NULL;\r
+               fields[1].in_check_mask = NULL;\r
+               fields[1].in_handler = NULL;\r
+               fields[1].in_handler_priv = NULL;\r
+               \r
+               arm_jtag_scann(&arm7_9->jtag_info, 0x1);\r
+               arm_jtag_set_instr(&arm7_9->jtag_info, arm7_9->jtag_info.intest_instr, NULL);\r
+\r
+               jtag_add_dr_scan(2, fields, TAP_PD);\r
+               jtag_execute_queue();\r
+               \r
+               fields[0].in_value = NULL;\r
+               fields[0].out_value = &breakpoint;\r
+               fields[1].in_value = NULL;\r
+               fields[1].out_value = databus;\r
+               \r
+               jtag_add_dr_scan(2, fields, TAP_PD);\r
+\r
+               if (breakpoint & 1)\r
+                       target->debug_reason = DBG_REASON_WATCHPOINT; \r
+               else\r
+                       target->debug_reason = DBG_REASON_BREAKPOINT; \r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* put an instruction in the ARM7TDMI pipeline or write the data bus, and optionally read data */\r
+int arm7tdmi_clock_out(arm_jtag_t *jtag_info, u32 out, u32 *in, int breakpoint)\r
+{\r
+       scan_field_t fields[2];\r
+       u8 out_buf[4];\r
+       u8 breakpoint_buf;\r
+       \r
+       buf_set_u32(out_buf, 0, 32, flip_u32(out, 32));\r
+       buf_set_u32(&breakpoint_buf, 0, 1, breakpoint);\r
+\r
+       jtag_add_end_state(TAP_PD);\r
+       arm_jtag_scann(jtag_info, 0x1);\r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+       \r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 1;\r
+       fields[0].out_value = &breakpoint_buf;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+               \r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 32;\r
+       fields[1].out_value = out_buf;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       if (in)\r
+       {\r
+               fields[1].in_handler = arm_jtag_buf_to_u32_flip;\r
+               fields[1].in_handler_priv = in;\r
+       }\r
+       else\r
+       {\r
+               fields[1].in_handler = NULL;\r
+               fields[1].in_handler_priv = NULL;\r
+       }\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+\r
+       jtag_add_dr_scan(2, fields, -1);\r
+\r
+       jtag_add_runtest(0, -1);\r
+       \r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+{\r
+               jtag_execute_queue();\r
+               \r
+               if (in)\r
+               {\r
+                       DEBUG("out: 0x%8.8x, in: 0x%8.8x", out, *in);\r
+               }\r
+               else\r
+                       DEBUG("out: 0x%8.8x", out);\r
+}\r
+#endif\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* clock the target, reading the databus */\r
+int arm7tdmi_clock_data_in(arm_jtag_t *jtag_info, u32 *in)\r
+{\r
+       scan_field_t fields[2];\r
+\r
+       jtag_add_end_state(TAP_PD);\r
+       arm_jtag_scann(jtag_info, 0x1);\r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+       \r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 1;\r
+       fields[0].out_value = NULL;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+               \r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 32;\r
+       fields[1].out_value = NULL;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_handler = arm_jtag_buf_to_u32_flip;\r
+       fields[1].in_handler_priv = in;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+\r
+       jtag_add_dr_scan(2, fields, -1);\r
+\r
+       jtag_add_runtest(0, -1);\r
+       \r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+{\r
+               jtag_execute_queue();\r
+                       \r
+               if (in)\r
+               {\r
+                       DEBUG("in: 0x%8.8x", *in);\r
+               }\r
+               else\r
+               {\r
+                       ERROR("BUG: called with in == NULL");\r
+               }\r
+}\r
+#endif\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* clock the target, and read the databus\r
+ * the *in pointer points to a buffer where elements of 'size' bytes\r
+ * are stored in big (be==1) or little (be==0) endianness\r
+ */ \r
+int arm7tdmi_clock_data_in_endianness(arm_jtag_t *jtag_info, void *in, int size, int be)\r
+{\r
+       scan_field_t fields[2];\r
+\r
+       jtag_add_end_state(TAP_PD);\r
+       arm_jtag_scann(jtag_info, 0x1);\r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+       \r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 1;\r
+       fields[0].out_value = NULL;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+               \r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 32;\r
+       fields[1].out_value = NULL;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       switch (size)\r
+       {\r
+               case 4:\r
+                       fields[1].in_handler = (be) ? arm_jtag_buf_to_be32_flip : arm_jtag_buf_to_le32_flip;\r
+                       break;\r
+               case 2:\r
+                       fields[1].in_handler = (be) ? arm_jtag_buf_to_be16_flip : arm_jtag_buf_to_le16_flip;\r
+                       break;\r
+               case 1:\r
+                       fields[1].in_handler = arm_jtag_buf_to_8_flip;\r
+                       break;\r
+       }\r
+       fields[1].in_handler_priv = in;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+\r
+       jtag_add_dr_scan(2, fields, -1);\r
+\r
+       jtag_add_runtest(0, -1);\r
+       \r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+{\r
+               jtag_execute_queue();\r
+                       \r
+               if (in)\r
+               {\r
+                       DEBUG("in: 0x%8.8x", *in);\r
+               }\r
+               else\r
+               {\r
+                       ERROR("BUG: called with in == NULL");\r
+               }\r
+}\r
+#endif\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+void arm7tdmi_change_to_arm(target_t *target, u32 *r0, u32 *pc)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       \r
+       /* save r0 before using it and put system in ARM state \r
+        * to allow common handling of ARM and THUMB debugging */\r
+       \r
+       /* fetch STR r0, [r0] */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), NULL, 0);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);\r
+       /* nothing fetched, STR r0, [r0] in Execute (2) */\r
+       arm7tdmi_clock_data_in(jtag_info, r0);\r
+\r
+       /* MOV r0, r15 fetched, STR in Decode */        \r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_MOV(0, 15), NULL, 0);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), NULL, 0);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);\r
+       /* nothing fetched, STR r0, [r0] in Execute (2) */\r
+       arm7tdmi_clock_data_in(jtag_info, pc);\r
+\r
+       /* use pc-relative LDR to clear r0[1:0] (for switch to ARM mode) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), NULL, 0);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);\r
+       /* nothing fetched, data for LDR r0, [PC, #0] */\r
+       arm7tdmi_clock_out(jtag_info, 0x0, NULL, 0);\r
+       /* nothing fetched, data from previous cycle is written to register */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);\r
+       \r
+       /* fetch BX */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_BX(0), NULL, 0);\r
+       /* NOP fetched, BX in Decode, MOV in Execute */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);\r
+       /* NOP fetched, BX in Execute (1) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);\r
+       \r
+       jtag_execute_queue();\r
+       \r
+       /* fix program counter:\r
+        * MOV r0, r15 was the 4th instruction (+6)\r
+        * reading PC in Thumb state gives address of instruction + 4\r
+        */\r
+       *pc -= 0xa;\r
+       \r
+}\r
+\r
+void arm7tdmi_read_core_regs(target_t *target, u32 mask, u32* core_regs[16])\r
+{\r
+       int i;\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+               \r
+       /* STMIA r0-15, [r0] at debug speed\r
+        * register values will start to appear on 4th DCLK\r
+        */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), NULL, 0);\r
+\r
+       /* fetch NOP, STM in DECODE stage */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* fetch NOP, STM in EXECUTE stage (1st cycle) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+\r
+       for (i = 0; i <= 15; i++)\r
+       {\r
+               if (mask & (1 << i))\r
+                       /* nothing fetched, STM still in EXECUTE (1+i cycle) */\r
+                       arm7tdmi_clock_data_in(jtag_info, core_regs[i]);\r
+       }\r
+\r
+}\r
+\r
+void arm7tdmi_read_core_regs_target_buffer(target_t *target, u32 mask, void* buffer, int size)\r
+{\r
+       int i;\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       int be = (target->endianness == TARGET_BIG_ENDIAN) ? 1 : 0;\r
+       u32 *buf_u32 = buffer;\r
+       u16 *buf_u16 = buffer;\r
+       u8 *buf_u8 = buffer;\r
+               \r
+       /* STMIA r0-15, [r0] at debug speed\r
+        * register values will start to appear on 4th DCLK\r
+        */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), NULL, 0);\r
+\r
+       /* fetch NOP, STM in DECODE stage */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* fetch NOP, STM in EXECUTE stage (1st cycle) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+\r
+       for (i = 0; i <= 15; i++)\r
+       {\r
+               /* nothing fetched, STM still in EXECUTE (1+i cycle), read databus */\r
+               if (mask & (1 << i))\r
+               {\r
+                       switch (size)\r
+                       {\r
+                               case 4:\r
+                                       arm7tdmi_clock_data_in_endianness(jtag_info, buf_u32++, 4, be);\r
+                                       break;\r
+                               case 2:\r
+                                       arm7tdmi_clock_data_in_endianness(jtag_info, buf_u16++, 2, be);\r
+                                       break;\r
+                               case 1:\r
+                                       arm7tdmi_clock_data_in_endianness(jtag_info, buf_u8++, 1, be);\r
+                                       break;\r
+                       }\r
+               }\r
+       }\r
+       \r
+}\r
+\r
+void arm7tdmi_read_xpsr(target_t *target, u32 *xpsr, int spsr)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+               \r
+       /* MRS r0, cpsr */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_MRS(0, spsr & 1), NULL, 0);\r
+       \r
+       /* STR r0, [r15] */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_STR(0, 15), NULL, 0);\r
+       /* fetch NOP, STR in DECODE stage */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* fetch NOP, STR in EXECUTE stage (1st cycle) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* nothing fetched, STR still in EXECUTE (2nd cycle) */\r
+       arm7tdmi_clock_data_in(jtag_info, xpsr);\r
+\r
+}\r
+\r
+void arm7tdmi_write_xpsr(target_t *target, u32 xpsr, int spsr)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+               \r
+       DEBUG("xpsr: %8.8x, spsr: %i", xpsr, spsr);\r
+\r
+       /* MSR1 fetched */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr & 0xff, 0, 1, spsr), NULL, 0);\r
+       /* MSR2 fetched, MSR1 in DECODE */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff00) >> 8, 0xc, 2, spsr), NULL, 0);\r
+       /* MSR3 fetched, MSR1 in EXECUTE (1), MSR2 in DECODE */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff0000) >> 16, 0x8, 4, spsr), NULL, 0);\r
+       /* nothing fetched, MSR1 in EXECUTE (2) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* MSR4 fetched, MSR2 in EXECUTE (1), MSR3 in DECODE */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff000000) >> 24, 0x4, 8, spsr), NULL, 0);\r
+       /* nothing fetched, MSR2 in EXECUTE (2) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* NOP fetched, MSR3 in EXECUTE (1), MSR4 in DECODE */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* nothing fetched, MSR3 in EXECUTE (2) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* NOP fetched, MSR4 in EXECUTE (1) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* nothing fetched, MSR4 in EXECUTE (2) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+}\r
+\r
+void arm7tdmi_write_xpsr_im8(target_t *target, u8 xpsr_im, int rot, int spsr)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+               \r
+       DEBUG("xpsr_im: %2.2x, rot: %i, spsr: %i", xpsr_im, rot, spsr);\r
+       \r
+       /* MSR fetched */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr_im, rot, 1, spsr), NULL, 0);\r
+       /* NOP fetched, MSR in DECODE */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* NOP fetched, MSR in EXECUTE (1) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* nothing fetched, MSR in EXECUTE (2) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       \r
+}\r
+\r
+void arm7tdmi_write_core_regs(target_t *target, u32 mask, u32 core_regs[16])\r
+{\r
+       int i;\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+               \r
+       /* LDMIA r0-15, [r0] at debug speed\r
+       * register values will start to appear on 4th DCLK\r
+       */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 0), NULL, 0);\r
+\r
+       /* fetch NOP, LDM in DECODE stage */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* fetch NOP, LDM in EXECUTE stage (1st cycle) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+\r
+       for (i = 0; i <= 15; i++)\r
+       {\r
+               if (mask & (1 << i))\r
+                       /* nothing fetched, LDM still in EXECUTE (1+i cycle) */\r
+                       arm7tdmi_clock_out(jtag_info, core_regs[i], NULL, 0);\r
+       }\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       \r
+}\r
+\r
+void arm7tdmi_load_word_regs(target_t *target, u32 mask)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+\r
+       /* put system-speed load-multiple into the pipeline */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 1), NULL, 0);\r
+\r
+}\r
+\r
+void arm7tdmi_load_hword_reg(target_t *target, int num)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       \r
+       /* put system-speed load half-word into the pipeline */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_LDRH_IP(num, 0), NULL, 0);\r
+\r
+}\r
+\r
+void arm7tdmi_load_byte_reg(target_t *target, int num)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+\r
+       /* put system-speed load byte into the pipeline */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_LDRB_IP(num, 0), NULL, 0);\r
+\r
+}\r
+\r
+void arm7tdmi_store_word_regs(target_t *target, u32 mask)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+\r
+       /* put system-speed store-multiple into the pipeline */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask, 0, 1), NULL, 0);\r
+       \r
+}\r
+\r
+void arm7tdmi_store_hword_reg(target_t *target, int num)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+\r
+       /* put system-speed store half-word into the pipeline */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_STRH_IP(num, 0), NULL, 0);\r
+\r
+}\r
+\r
+void arm7tdmi_store_byte_reg(target_t *target, int num)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+\r
+       /* put system-speed store byte into the pipeline */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_STRB_IP(num, 0), NULL, 0);\r
+\r
+}\r
+\r
+void arm7tdmi_write_pc(target_t *target, u32 pc)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       \r
+       /* LDMIA r0-15, [r0] at debug speed\r
+        * register values will start to appear on 4th DCLK\r
+        */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x8000, 0, 0), NULL, 0);\r
+       /* fetch NOP, LDM in DECODE stage */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* fetch NOP, LDM in EXECUTE stage (1st cycle) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* nothing fetched, LDM in EXECUTE stage (1st cycle) load register */\r
+       arm7tdmi_clock_out(jtag_info, pc, NULL, 0);\r
+       /* nothing fetched, LDM in EXECUTE stage (2nd cycle) load register */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* nothing fetched, LDM in EXECUTE stage (3rd cycle) load register */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* fetch NOP, LDM in EXECUTE stage (4th cycle) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* fetch NOP, LDM in EXECUTE stage (5th cycle) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+}\r
+\r
+void arm7tdmi_branch_resume(target_t *target)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       \r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 1);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_B(0xfffffa, 0), NULL, 0);\r
+\r
+}\r
+\r
+void arm7tdmi_branch_resume_thumb(target_t *target)\r
+{\r
+       DEBUG("-");\r
+       \r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];\r
+\r
+       /* LDMIA r0, [r0] at debug speed\r
+        * register values will start to appear on 4th DCLK\r
+        */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x1, 0, 0), NULL, 0);\r
+\r
+       /* fetch NOP, LDM in DECODE stage */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* fetch NOP, LDM in EXECUTE stage (1st cycle) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       /* nothing fetched, LDM in EXECUTE stage (2nd cycle) */\r
+       arm7tdmi_clock_out(jtag_info, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32) | 1, NULL, 0);\r
+       /* nothing fetched, LDM in EXECUTE stage (3rd cycle) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+\r
+       /* Branch and eXchange */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_BX(0), NULL, 0);\r
+       \r
+       embeddedice_read_reg(dbg_stat);\r
+       \r
+       /* fetch NOP, BX in DECODE stage */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+       \r
+       /* target is now in Thumb state */\r
+       embeddedice_read_reg(dbg_stat);\r
+       \r
+       /* fetch NOP, BX in EXECUTE stage (1st cycle) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_NOP, NULL, 0);\r
+\r
+       /* target is now in Thumb state */\r
+       embeddedice_read_reg(dbg_stat);\r
+\r
+       /* load r0 value */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), NULL, 0);\r
+       /* fetch NOP, LDR in Decode */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);\r
+       /* fetch NOP, LDR in Execute */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);\r
+       /* nothing fetched, LDR in EXECUTE stage (2nd cycle) */\r
+       arm7tdmi_clock_out(jtag_info, buf_get_u32(armv4_5->core_cache->reg_list[0].value, 0, 32), NULL, 0);\r
+       /* nothing fetched, LDR in EXECUTE stage (3rd cycle) */\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);\r
+       \r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 0);\r
+\r
+       embeddedice_read_reg(dbg_stat);\r
+       \r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, NULL, 1);\r
+       arm7tdmi_clock_out(jtag_info, ARMV4_5_T_B(0x7f8), NULL, 0);\r
+\r
+}\r
+               \r
+void arm7tdmi_build_reg_cache(target_t *target)\r
+{\r
+       reg_cache_t **cache_p = register_get_last_cache_p(&target->reg_cache);\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+\r
+       (*cache_p) = armv4_5_build_reg_cache(target, armv4_5);\r
+       armv4_5->core_cache = (*cache_p);\r
+       \r
+       (*cache_p)->next = embeddedice_build_reg_cache(target, arm7_9);\r
+       arm7_9->eice_cache = (*cache_p)->next;\r
+       \r
+       if (arm7_9->etm_ctx)\r
+       {\r
+               (*cache_p)->next->next = etm_build_reg_cache(target, jtag_info, arm7_9->etm_ctx);\r
+               arm7_9->etm_ctx->reg_cache = (*cache_p)->next->next;\r
+       }\r
+}\r
+\r
+int arm7tdmi_init_target(struct command_context_s *cmd_ctx, struct target_s *target)\r
+{\r
+       \r
+       arm7tdmi_build_reg_cache(target);\r
+       \r
+       return ERROR_OK;\r
+       \r
+}\r
+\r
+int arm7tdmi_quit()\r
+{\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm7tdmi_init_arch_info(target_t *target, arm7tdmi_common_t *arm7tdmi, int chain_pos, char *variant)\r
+{\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       \r
+       arm7_9 = &arm7tdmi->arm7_9_common;\r
+       armv4_5 = &arm7_9->armv4_5_common;\r
+       \r
+       /* prepare JTAG information for the new target */\r
+       arm7_9->jtag_info.chain_pos = chain_pos;\r
+       arm7_9->jtag_info.scann_size = 4;\r
+       \r
+       /* register arch-specific functions */\r
+       arm7_9->examine_debug_reason = arm7tdmi_examine_debug_reason;\r
+       arm7_9->change_to_arm = arm7tdmi_change_to_arm;\r
+       arm7_9->read_core_regs = arm7tdmi_read_core_regs;\r
+       arm7_9->read_core_regs_target_buffer = arm7tdmi_read_core_regs_target_buffer;\r
+       arm7_9->read_xpsr = arm7tdmi_read_xpsr;\r
+       \r
+       arm7_9->write_xpsr = arm7tdmi_write_xpsr;\r
+       arm7_9->write_xpsr_im8 = arm7tdmi_write_xpsr_im8;\r
+       arm7_9->write_core_regs = arm7tdmi_write_core_regs;\r
+       \r
+       arm7_9->load_word_regs = arm7tdmi_load_word_regs;\r
+       arm7_9->load_hword_reg = arm7tdmi_load_hword_reg;\r
+       arm7_9->load_byte_reg = arm7tdmi_load_byte_reg;\r
+       \r
+       arm7_9->store_word_regs = arm7tdmi_store_word_regs;\r
+       arm7_9->store_hword_reg = arm7tdmi_store_hword_reg;\r
+       arm7_9->store_byte_reg = arm7tdmi_store_byte_reg;\r
+       \r
+       arm7_9->write_pc = arm7tdmi_write_pc;\r
+       arm7_9->branch_resume = arm7tdmi_branch_resume;\r
+       arm7_9->branch_resume_thumb = arm7tdmi_branch_resume_thumb;\r
+       \r
+       arm7_9->enable_single_step = arm7_9_enable_eice_step;\r
+       arm7_9->disable_single_step = arm7_9_disable_eice_step;\r
+               \r
+       arm7_9->pre_debug_entry = NULL;\r
+       arm7_9->post_debug_entry = NULL;\r
+       \r
+       arm7_9->pre_restore_context = NULL;\r
+       arm7_9->post_restore_context = NULL;\r
+       \r
+       /* initialize arch-specific breakpoint handling */\r
+       arm7_9->arm_bkpt = 0xdeeedeee;\r
+       arm7_9->thumb_bkpt = 0xdeee;\r
+       \r
+       arm7_9->sw_bkpts_use_wp = 1;\r
+       arm7_9->sw_bkpts_enabled = 0;\r
+       arm7_9->dbgreq_adjust_pc = 2;\r
+       arm7_9->arch_info = arm7tdmi;\r
+\r
+       arm7tdmi->arch_info = NULL;\r
+       arm7tdmi->common_magic = ARM7TDMI_COMMON_MAGIC;\r
+       \r
+       if (variant)\r
+       {\r
+               arm7tdmi->variant = strdup(variant);\r
+       }\r
+       else\r
+       {\r
+               arm7tdmi->variant = strdup("");\r
+       }\r
+       \r
+       arm7_9_init_arch_info(target, arm7_9);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* target arm7tdmi <endianess> <startup_mode> <chain_pos> <variant> */\r
+int arm7tdmi_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)\r
+{\r
+       int chain_pos;\r
+       char *variant = NULL;\r
+       arm7tdmi_common_t *arm7tdmi = malloc(sizeof(arm7tdmi_common_t));\r
+\r
+       if (argc < 4)\r
+       {\r
+               ERROR("'target arm7tdmi' requires at least one additional argument");\r
+               exit(-1);\r
+       }\r
+       \r
+       chain_pos = strtoul(args[3], NULL, 0);\r
+       \r
+       if (argc >= 5)\r
+               variant = args[4];\r
+       \r
+       arm7tdmi_init_arch_info(target, arm7tdmi, chain_pos, variant);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm7tdmi_register_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       int retval;\r
+       \r
+       retval = arm7_9_register_commands(cmd_ctx);\r
+       \r
+       return ERROR_OK;\r
+\r
+}\r
+\r
index bf159dbeb388606d488fee6f4a98b32ab41df9a6..17c539736d2ae77040e74385c7b86c0ebea48c48 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2005 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "arm920t.h"
-#include "jtag.h"
-#include "log.h"
-
-#include <stdlib.h>
-#include <string.h>
-
-#if 0
-#define _DEBUG_INSTRUCTION_EXECUTION_
-#endif
-
-/* cli handling */
-int arm920t_register_commands(struct command_context_s *cmd_ctx);
-
-int arm920t_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm920t_handle_cp15i_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm920t_handle_virt2phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm920t_handle_cache_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm920t_handle_md_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm920t_handle_mw_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-int arm920t_handle_read_cache_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm920t_handle_read_mmu_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-/* forward declarations */
-int arm920t_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);
-int arm920t_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
-int arm920t_quit();
-int arm920t_arch_state(struct target_s *target);
-int arm920t_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
-int arm920t_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
-int arm920t_soft_reset_halt(struct target_s *target);
-
-#define ARM920T_CP15_PHYS_ADDR(x, y, z) ((x << 5) | (y << 1) << (z))
-
-target_type_t arm920t_target =
-{
-       .name = "arm920t",
-
-       .poll = arm7_9_poll,
-       .arch_state = arm920t_arch_state,
-
-       .target_request_data = arm7_9_target_request_data,
-
-       .halt = arm7_9_halt,
-       .resume = arm7_9_resume,
-       .step = arm7_9_step,
-
-       .assert_reset = arm7_9_assert_reset,
-       .deassert_reset = arm7_9_deassert_reset,
-       .soft_reset_halt = arm920t_soft_reset_halt,
-       .prepare_reset_halt = arm7_9_prepare_reset_halt,
-       
-       .get_gdb_reg_list = armv4_5_get_gdb_reg_list,
-
-       .read_memory = arm920t_read_memory,
-       .write_memory = arm920t_write_memory,
-       .bulk_write_memory = arm7_9_bulk_write_memory,
-       .checksum_memory = arm7_9_checksum_memory,
-       
-       .run_algorithm = armv4_5_run_algorithm,
-
-       .add_breakpoint = arm7_9_add_breakpoint,
-       .remove_breakpoint = arm7_9_remove_breakpoint,
-       .add_watchpoint = arm7_9_add_watchpoint,
-       .remove_watchpoint = arm7_9_remove_watchpoint,
-
-       .register_commands = arm920t_register_commands,
-       .target_command = arm920t_target_command,
-       .init_target = arm920t_init_target,
-       .quit = arm920t_quit
-};
-
-int arm920t_read_cp15_physical(target_t *target, int reg_addr, u32 *value)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       scan_field_t fields[4];
-       u8 access_type_buf = 1;
-       u8 reg_addr_buf = reg_addr & 0x3f;
-       u8 nr_w_buf = 0;
-       
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_scann(jtag_info, 0xf);
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 1;
-       fields[0].out_value = &access_type_buf;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 32;
-       fields[1].out_value = NULL;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = jtag_info->chain_pos;
-       fields[2].num_bits = 6;
-       fields[2].out_value = &reg_addr_buf;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-
-       fields[3].device = jtag_info->chain_pos;
-       fields[3].num_bits = 1;
-       fields[3].out_value = &nr_w_buf;
-       fields[3].out_mask = NULL;
-       fields[3].in_value = NULL;
-       fields[3].in_check_value = NULL;
-       fields[3].in_check_mask = NULL;
-       fields[3].in_handler = NULL;
-       fields[3].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(4, fields, -1, NULL);
-
-       fields[1].in_handler_priv = value;
-       fields[1].in_handler = arm_jtag_buf_to_u32;
-
-       jtag_add_dr_scan(4, fields, -1, NULL);
-
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-       jtag_execute_queue();
-       DEBUG("addr: 0x%x value: %8.8x", reg_addr, *value);
-#endif
-
-       return ERROR_OK;
-}
-
-int arm920t_write_cp15_physical(target_t *target, int reg_addr, u32 value)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       scan_field_t fields[4];
-       u8 access_type_buf = 1;
-       u8 reg_addr_buf = reg_addr & 0x3f;
-       u8 nr_w_buf = 1;
-       u8 value_buf[4];
-       
-       buf_set_u32(value_buf, 0, 32, value);
-       
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_scann(jtag_info, 0xf);
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 1;
-       fields[0].out_value = &access_type_buf;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 32;
-       fields[1].out_value = value_buf;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = jtag_info->chain_pos;
-       fields[2].num_bits = 6;
-       fields[2].out_value = &reg_addr_buf;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-
-       fields[3].device = jtag_info->chain_pos;
-       fields[3].num_bits = 1;
-       fields[3].out_value = &nr_w_buf;
-       fields[3].out_mask = NULL;
-       fields[3].in_value = NULL;
-       fields[3].in_check_value = NULL;
-       fields[3].in_check_mask = NULL;
-       fields[3].in_handler = NULL;
-       fields[3].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(4, fields, -1, NULL);
-
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-       DEBUG("addr: 0x%x value: %8.8x", reg_addr, value);
-#endif
-
-       return ERROR_OK;
-}
-
-int arm920t_execute_cp15(target_t *target, u32 cp15_opcode, u32 arm_opcode)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       scan_field_t fields[4];
-       u8 access_type_buf = 0;         /* interpreted access */
-       u8 reg_addr_buf = 0x0;
-       u8 nr_w_buf = 0;
-       u8 cp15_opcode_buf[4];
-       
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_scann(jtag_info, 0xf);
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-       
-       buf_set_u32(cp15_opcode_buf, 0, 32, cp15_opcode);
-
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 1;
-       fields[0].out_value = &access_type_buf;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 32;
-       fields[1].out_value = cp15_opcode_buf;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = jtag_info->chain_pos;
-       fields[2].num_bits = 6;
-       fields[2].out_value = &reg_addr_buf;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-
-       fields[3].device = jtag_info->chain_pos;
-       fields[3].num_bits = 1;
-       fields[3].out_value = &nr_w_buf;
-       fields[3].out_mask = NULL;
-       fields[3].in_value = NULL;
-       fields[3].in_check_value = NULL;
-       fields[3].in_check_mask = NULL;
-       fields[3].in_handler = NULL;
-       fields[3].in_handler_priv = NULL;
-
-       jtag_add_dr_scan(4, fields, -1, NULL);
-
-       arm9tdmi_clock_out(jtag_info, arm_opcode, 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
-       arm7_9_execute_sys_speed(target);
-       
-       if (jtag_execute_queue() != ERROR_OK)
-       {
-               ERROR("failed executing JTAG queue, exiting");
-               exit(-1);
-       }
-       
-       return ERROR_OK;
-}
-
-int arm920t_read_cp15_interpreted(target_t *target, u32 cp15_opcode, u32 address, u32 *value)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       u32* regs_p[1];
-       u32 regs[2];
-       u32 cp15c15 = 0x0;
-
-       /* load address into R1 */
-       regs[1] = address;
-       arm9tdmi_write_core_regs(target, 0x2, regs); 
-       
-       /* read-modify-write CP15 test state register 
-       * to enable interpreted access mode */
-       arm920t_read_cp15_physical(target, 0x1e, &cp15c15);     
-       jtag_execute_queue();
-       cp15c15 |= 1;   /* set interpret mode */
-       arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-
-       /* execute CP15 instruction and ARM load (reading from coprocessor) */
-       arm920t_execute_cp15(target, cp15_opcode, ARMV4_5_LDR(0, 1));
-       
-       /* disable interpreted access mode */
-       cp15c15 &= ~1U; /* clear interpret mode */
-       arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-
-       /* retrieve value from R0 */
-       regs_p[0] = value;
-       arm9tdmi_read_core_regs(target, 0x1, regs_p);
-       jtag_execute_queue();
-       
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-       DEBUG("cp15_opcode: %8.8x, address: %8.8x, value: %8.8x", cp15_opcode, address, *value);
-#endif
-
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = 1;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 1).dirty = 1;
-
-       return ERROR_OK;
-}
-
-int arm920t_write_cp15_interpreted(target_t *target, u32 cp15_opcode, u32 value, u32 address)
-{
-       u32 cp15c15 = 0x0;
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       u32 regs[2];
-
-       /* load value, address into R0, R1 */
-       regs[0] = value;
-       regs[1] = address;
-       arm9tdmi_write_core_regs(target, 0x3, regs);
-
-       /* read-modify-write CP15 test state register 
-       * to enable interpreted access mode */
-       arm920t_read_cp15_physical(target, 0x1e, &cp15c15);
-       jtag_execute_queue();
-       cp15c15 |= 1;   /* set interpret mode */
-       arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-
-       /* execute CP15 instruction and ARM store (writing to coprocessor) */
-       arm920t_execute_cp15(target, cp15_opcode, ARMV4_5_STR(0, 1));
-
-       /* disable interpreted access mode */
-       cp15c15 &= ~1U; /* set interpret mode */
-       arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-       DEBUG("cp15_opcode: %8.8x, value: %8.8x, address: %8.8x", cp15_opcode, value, address);
-#endif
-
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = 1;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 1).dirty = 1;
-       
-       return ERROR_OK;
-}
-
-u32 arm920t_get_ttb(target_t *target)
-{
-       int retval;
-       u32 ttb = 0x0;
-
-       if ((retval = arm920t_read_cp15_interpreted(target, 0xeebf0f51, 0x0, &ttb)) != ERROR_OK)
-               return retval;
-
-       return ttb;
-}
-
-void arm920t_disable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)
-{
-       u32 cp15_control;
-
-       /* read cp15 control register */
-       arm920t_read_cp15_physical(target, 0x2, &cp15_control);
-       jtag_execute_queue();
-               
-       if (mmu)
-               cp15_control &= ~0x1U;
-       
-       if (d_u_cache)
-               cp15_control &= ~0x4U;
-       
-       if (i_cache)
-               cp15_control &= ~0x1000U;
-
-       arm920t_write_cp15_physical(target, 0x2, cp15_control);
-}
-
-void arm920t_enable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)
-{
-       u32 cp15_control;
-
-       /* read cp15 control register */
-       arm920t_read_cp15_physical(target, 0x2, &cp15_control);
-       jtag_execute_queue();
-               
-       if (mmu)
-               cp15_control |= 0x1U;
-       
-       if (d_u_cache)
-               cp15_control |= 0x4U;
-       
-       if (i_cache)
-               cp15_control |= 0x1000U;
-       
-       arm920t_write_cp15_physical(target, 0x2, cp15_control);
-}
-
-void arm920t_post_debug_entry(target_t *target)
-{
-       u32 cp15c15;
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
-       arm920t_common_t *arm920t = arm9tdmi->arch_info;
-       
-       /* examine cp15 control reg */
-       arm920t_read_cp15_physical(target, 0x2, &arm920t->cp15_control_reg);
-       jtag_execute_queue();
-       DEBUG("cp15_control_reg: %8.8x", arm920t->cp15_control_reg);
-
-       if (arm920t->armv4_5_mmu.armv4_5_cache.ctype == -1)
-       {
-               u32 cache_type_reg;
-               /* identify caches */
-               arm920t_read_cp15_physical(target, 0x1, &cache_type_reg);
-               jtag_execute_queue();
-               armv4_5_identify_cache(cache_type_reg, &arm920t->armv4_5_mmu.armv4_5_cache);
-       }
-
-       arm920t->armv4_5_mmu.mmu_enabled = (arm920t->cp15_control_reg & 0x1U) ? 1 : 0;
-       arm920t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = (arm920t->cp15_control_reg & 0x4U) ? 1 : 0;
-       arm920t->armv4_5_mmu.armv4_5_cache.i_cache_enabled = (arm920t->cp15_control_reg & 0x1000U) ? 1 : 0;
-
-       /* save i/d fault status and address register */
-       arm920t_read_cp15_interpreted(target, 0xee150f10, 0x0, &arm920t->d_fsr);
-       arm920t_read_cp15_interpreted(target, 0xee150f30, 0x0, &arm920t->i_fsr);
-       arm920t_read_cp15_interpreted(target, 0xee160f10, 0x0, &arm920t->d_far);
-       arm920t_read_cp15_interpreted(target, 0xee160f30, 0x0, &arm920t->i_far);
-       
-       DEBUG("D FSR: 0x%8.8x, D FAR: 0x%8.8x, I FSR: 0x%8.8x, I FAR: 0x%8.8x",
-               arm920t->d_fsr, arm920t->d_far, arm920t->i_fsr, arm920t->i_far);  
-
-       if (arm920t->preserve_cache)
-       {
-               /* read-modify-write CP15 test state register 
-                * to disable I/D-cache linefills */
-               arm920t_read_cp15_physical(target, 0x1e, &cp15c15);
-               jtag_execute_queue();
-               cp15c15 |= 0x600;
-               arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-       }
-}
-
-void arm920t_pre_restore_context(target_t *target)
-{
-       u32 cp15c15;
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
-       arm920t_common_t *arm920t = arm9tdmi->arch_info;
-       
-       /* restore i/d fault status and address register */
-       arm920t_write_cp15_interpreted(target, 0xee050f10, arm920t->d_fsr, 0x0);
-       arm920t_write_cp15_interpreted(target, 0xee050f30, arm920t->i_fsr, 0x0);
-       arm920t_write_cp15_interpreted(target, 0xee060f10, arm920t->d_far, 0x0);
-       arm920t_write_cp15_interpreted(target, 0xee060f30, arm920t->i_far, 0x0);
-       
-       /* read-modify-write CP15 test state register 
-       * to reenable I/D-cache linefills */
-       if (arm920t->preserve_cache)
-       {
-               arm920t_read_cp15_physical(target, 0x1e, &cp15c15);
-               jtag_execute_queue();
-               cp15c15 &= ~0x600U;
-               arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-       }
-}
-
-int arm920t_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p, arm9tdmi_common_t **arm9tdmi_p, arm920t_common_t **arm920t_p)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm920t_common_t *arm920t;
-       
-       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       arm7_9 = armv4_5->arch_info;
-       if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       arm9tdmi = arm7_9->arch_info;
-       if (arm9tdmi->common_magic != ARM9TDMI_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       arm920t = arm9tdmi->arch_info;
-       if (arm920t->common_magic != ARM920T_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       *armv4_5_p = armv4_5;
-       *arm7_9_p = arm7_9;
-       *arm9tdmi_p = arm9tdmi;
-       *arm920t_p = arm920t;
-       
-       return ERROR_OK;
-}
-
-int arm920t_arch_state(struct target_s *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
-       arm920t_common_t *arm920t = arm9tdmi->arch_info;
-       
-       char *state[] = 
-       {
-               "disabled", "enabled"
-       };
-       
-       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
-       {
-               ERROR("BUG: called for a non-ARMv4/5 target");
-               exit(-1);
-       }
-       
-       USER(   "target halted in %s state due to %s, current mode: %s\n"
-                       "cpsr: 0x%8.8x pc: 0x%8.8x\n"
-                       "MMU: %s, D-Cache: %s, I-Cache: %s",
-                        armv4_5_state_strings[armv4_5->core_state],
-                        target_debug_reason_strings[target->debug_reason],
-                        armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)],
-                        buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),
-                        buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32),
-                        state[arm920t->armv4_5_mmu.mmu_enabled],
-                        state[arm920t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled], 
-                        state[arm920t->armv4_5_mmu.armv4_5_cache.i_cache_enabled]);
-       
-       return ERROR_OK;
-}
-
-int arm920t_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
-{
-       int retval;
-       
-       retval = arm7_9_read_memory(target, address, size, count, buffer);
-       
-       return retval;
-}
-
-int arm920t_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
-{
-       int retval;
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
-       arm920t_common_t *arm920t = arm9tdmi->arch_info;
-       
-       if ((retval = arm7_9_write_memory(target, address, size, count, buffer)) != ERROR_OK)
-               return retval;
-
-       if (((size == 4) || (size == 2)) && (count == 1))
-       {
-               if (arm920t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled)
-               {
-                       DEBUG("D-Cache enabled, writing through to main memory");
-                       u32 pa, cb, ap;
-                       int type, domain;
-
-                       pa = armv4_5_mmu_translate_va(target, &arm920t->armv4_5_mmu, address, &type, &cb, &domain, &ap);
-                       if (type == -1)
-                               return ERROR_OK;
-                       /* cacheable & bufferable means write-back region */
-                       if (cb == 3)
-                               armv4_5_mmu_write_physical(target, &arm920t->armv4_5_mmu, pa, size, count, buffer);
-               }
-               
-               if (arm920t->armv4_5_mmu.armv4_5_cache.i_cache_enabled)
-               {
-                       DEBUG("I-Cache enabled, invalidating affected I-Cache line");
-                       arm920t_write_cp15_interpreted(target, 0xee070f35, 0x0, address);
-               }
-       }
-
-       return retval;
-}
-
-int arm920t_soft_reset_halt(struct target_s *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
-       arm920t_common_t *arm920t = arm9tdmi->arch_info;
-       reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
-       
-       if (target->state == TARGET_RUNNING)
-       {
-               target->type->halt(target);
-       }
-       
-       while (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) == 0)
-       {
-               embeddedice_read_reg(dbg_stat);
-               jtag_execute_queue();
-       }
-       
-       target->state = TARGET_HALTED;
-       
-       /* SVC, ARM state, IRQ and FIQ disabled */
-       buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8, 0xd3);
-       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
-       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
-       
-       /* start fetching from 0x0 */
-       buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, 0x0);
-       armv4_5->core_cache->reg_list[15].dirty = 1;
-       armv4_5->core_cache->reg_list[15].valid = 1;
-       
-       armv4_5->core_mode = ARMV4_5_MODE_SVC;
-       armv4_5->core_state = ARMV4_5_STATE_ARM;
-       
-       arm920t_disable_mmu_caches(target, 1, 1, 1);
-       arm920t->armv4_5_mmu.mmu_enabled = 0;
-       arm920t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 0;
-       arm920t->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;
-
-       target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-       
-       return ERROR_OK;
-}
-
-int arm920t_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
-{
-       arm9tdmi_init_target(cmd_ctx, target);
-               
-       return ERROR_OK;
-       
-}
-
-int arm920t_quit()
-{
-       
-       return ERROR_OK;
-}
-
-int arm920t_init_arch_info(target_t *target, arm920t_common_t *arm920t, int chain_pos, char *variant)
-{
-       arm9tdmi_common_t *arm9tdmi = &arm920t->arm9tdmi_common;
-       arm7_9_common_t *arm7_9 = &arm9tdmi->arm7_9_common;
-       
-       /* initialize arm9tdmi specific info (including arm7_9 and armv4_5)
-        */
-       arm9tdmi_init_arch_info(target, arm9tdmi, chain_pos, variant);
-
-       arm9tdmi->arch_info = arm920t;
-       arm920t->common_magic = ARM920T_COMMON_MAGIC;
-       
-       arm7_9->post_debug_entry = arm920t_post_debug_entry;
-       arm7_9->pre_restore_context = arm920t_pre_restore_context;
-       
-       arm920t->armv4_5_mmu.armv4_5_cache.ctype = -1;
-       arm920t->armv4_5_mmu.get_ttb = arm920t_get_ttb;
-       arm920t->armv4_5_mmu.read_memory = arm7_9_read_memory;
-       arm920t->armv4_5_mmu.write_memory = arm7_9_write_memory;
-       arm920t->armv4_5_mmu.disable_mmu_caches = arm920t_disable_mmu_caches;
-       arm920t->armv4_5_mmu.enable_mmu_caches = arm920t_enable_mmu_caches;
-       arm920t->armv4_5_mmu.has_tiny_pages = 1;
-       arm920t->armv4_5_mmu.mmu_enabled = 0;
-       
-       /* disabling linefills leads to lockups, so keep them enabled for now
-        * this doesn't affect correctness, but might affect timing issues, if
-        * important data is evicted from the cache during the debug session
-        * */
-       arm920t->preserve_cache = 0;
-       
-       /* override hw single-step capability from ARM9TDMI */
-       arm7_9->has_single_step = 1;
-       
-       return ERROR_OK;
-}
-
-int arm920t_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)
-{
-       int chain_pos;
-       char *variant = NULL;
-       arm920t_common_t *arm920t = malloc(sizeof(arm920t_common_t));
-       
-       if (argc < 4)
-       {
-               ERROR("'target arm920t' requires at least one additional argument");
-               exit(-1);
-       }
-       
-       chain_pos = strtoul(args[3], NULL, 0);
-       
-       if (argc >= 5)
-               variant = args[4];
-       
-       DEBUG("chain_pos: %i, variant: %s", chain_pos, variant);
-       
-       arm920t_init_arch_info(target, arm920t, chain_pos, variant);
-
-       return ERROR_OK;
-}
-
-int arm920t_register_commands(struct command_context_s *cmd_ctx)
-{
-       int retval;
-       command_t *arm920t_cmd;
-       
-               
-       retval = arm9tdmi_register_commands(cmd_ctx);
-       
-       arm920t_cmd = register_command(cmd_ctx, NULL, "arm920t", NULL, COMMAND_ANY, "arm920t specific commands");
-
-       register_command(cmd_ctx, arm920t_cmd, "cp15", arm920t_handle_cp15_command, COMMAND_EXEC, "display/modify cp15 register <num> [value]");
-       register_command(cmd_ctx, arm920t_cmd, "cp15i", arm920t_handle_cp15i_command, COMMAND_EXEC, "display/modify cp15 (interpreted access) <opcode> [value] [address]");
-       register_command(cmd_ctx, arm920t_cmd, "cache_info", arm920t_handle_cache_info_command, COMMAND_EXEC, "display information about target caches");
-       register_command(cmd_ctx, arm920t_cmd, "virt2phys", arm920t_handle_virt2phys_command, COMMAND_EXEC, "translate va to pa <va>");
-
-       register_command(cmd_ctx, arm920t_cmd, "mdw_phys", arm920t_handle_md_phys_command, COMMAND_EXEC, "display memory words <physical addr> [count]");
-       register_command(cmd_ctx, arm920t_cmd, "mdh_phys", arm920t_handle_md_phys_command, COMMAND_EXEC, "display memory half-words <physical addr> [count]");
-       register_command(cmd_ctx, arm920t_cmd, "mdb_phys", arm920t_handle_md_phys_command, COMMAND_EXEC, "display memory bytes <physical addr> [count]");
-
-       register_command(cmd_ctx, arm920t_cmd, "mww_phys", arm920t_handle_mw_phys_command, COMMAND_EXEC, "write memory word <physical addr> <value>");
-       register_command(cmd_ctx, arm920t_cmd, "mwh_phys", arm920t_handle_mw_phys_command, COMMAND_EXEC, "write memory half-word <physical addr> <value>");
-       register_command(cmd_ctx, arm920t_cmd, "mwb_phys", arm920t_handle_mw_phys_command, COMMAND_EXEC, "write memory byte <physical addr> <value>");
-
-       register_command(cmd_ctx, arm920t_cmd, "read_cache", arm920t_handle_read_cache_command, COMMAND_EXEC, "display I/D cache content");
-       register_command(cmd_ctx, arm920t_cmd, "read_mmu", arm920t_handle_read_mmu_command, COMMAND_EXEC, "display I/D mmu content");
-
-       return ERROR_OK;
-}
-
-int arm920t_handle_read_cache_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm920t_common_t *arm920t;
-       arm_jtag_t *jtag_info;
-       u32 cp15c15;
-       u32 cp15_ctrl, cp15_ctrl_saved;
-       u32 regs[16];
-       u32 *regs_p[16];
-       u32 C15_C_D_Ind, C15_C_I_Ind;
-       int i;
-       FILE *output;
-       arm920t_cache_line_t d_cache[8][64], i_cache[8][64];
-       int segment, index;
-       
-       if (argc != 1)
-       {
-               command_print(cmd_ctx, "usage: arm920t read_cache <filename>");
-               return ERROR_OK;
-       }
-       
-       if ((output = fopen(args[0], "w")) == NULL)
-       {
-               DEBUG("error opening cache content file");
-               return ERROR_OK;
-       }
-       
-       for (i = 0; i < 16; i++)
-               regs_p[i] = &regs[i];
-               
-       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM920t target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       /* disable MMU and Caches */
-       arm920t_read_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), &cp15_ctrl);
-       jtag_execute_queue();
-       cp15_ctrl_saved = cp15_ctrl;
-       cp15_ctrl &= ~(ARMV4_5_MMU_ENABLED | ARMV4_5_D_U_CACHE_ENABLED | ARMV4_5_I_CACHE_ENABLED);
-       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), cp15_ctrl);
-
-       /* read CP15 test state register */ 
-       arm920t_read_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), &cp15c15);
-       jtag_execute_queue();
-       
-       /* read DCache content */
-       fprintf(output, "DCache:\n");
-       
-       /* go through segments 0 to nsets (8 on ARM920T, 4 on ARM922T) */ 
-       for (segment = 0; segment < arm920t->armv4_5_mmu.armv4_5_cache.d_u_size.nsets; segment++)
-       {
-               fprintf(output, "\nsegment: %i\n----------", segment);
-               
-               /* Ra: r0 = SBZ(31:8):segment(7:5):SBZ(4:0) */
-               regs[0] = 0x0 | (segment << 5);
-               arm9tdmi_write_core_regs(target, 0x1, regs);
-               
-               /* set interpret mode */
-               cp15c15 |= 0x1;
-               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
-               
-               /* D CAM Read, loads current victim into C15.C.D.Ind */
-               arm920t_execute_cp15(target, ARMV4_5_MCR(15,2,0,15,6,2), ARMV4_5_LDR(1, 0));
-       
-               /* read current victim */
-               arm920t_read_cp15_physical(target, 0x3d, &C15_C_D_Ind);
-
-               /* clear interpret mode */
-               cp15c15 &= ~0x1;
-               arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-
-               for (index = 0; index < 64; index++)
-               {
-                       /* Ra: r0 = index(31:26):SBZ(25:8):segment(7:5):SBZ(4:0) */
-                       regs[0] = 0x0 | (segment << 5) | (index << 26);
-                       arm9tdmi_write_core_regs(target, 0x1, regs);
-
-                       /* set interpret mode */
-                       cp15c15 |= 0x1;
-                       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
-       
-                       /* Write DCache victim */
-                       arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,9,1,0), ARMV4_5_LDR(1, 0));
-       
-                       /* Read D RAM */
-                       arm920t_execute_cp15(target, ARMV4_5_MCR(15,2,0,15,10,2), ARMV4_5_LDMIA(0, 0x1fe, 0, 0));
-                       
-                       /* Read D CAM */
-                       arm920t_execute_cp15(target, ARMV4_5_MCR(15,2,0,15,6,2), ARMV4_5_LDR(9, 0));
-                       
-                       /* clear interpret mode */
-                       cp15c15 &= ~0x1;
-                       arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-
-                       /* read D RAM and CAM content */
-                       arm9tdmi_read_core_regs(target, 0x3fe, regs_p);
-                       jtag_execute_queue();
-
-                       d_cache[segment][index].cam = regs[9];
-                       
-                       /* mask LFSR[6] */
-                       regs[9] &= 0xfffffffe;
-                       fprintf(output, "\nsegment: %i, index: %i, CAM: 0x%8.8x, content (%s):\n", segment, index, regs[9], (regs[9] & 0x10) ? "valid" : "invalid");
-                       
-                       for (i = 1; i < 9; i++)
-                       {
-                                d_cache[segment][index].data[i] = regs[i];
-                                fprintf(output, "%i: 0x%8.8x\n", i-1, regs[i]);
-                       }
-       
-               }
-               
-               /* Ra: r0 = index(31:26):SBZ(25:8):segment(7:5):SBZ(4:0) */
-               regs[0] = 0x0 | (segment << 5) | (C15_C_D_Ind << 26);
-               arm9tdmi_write_core_regs(target, 0x1, regs);
-
-               /* set interpret mode */
-               cp15c15 |= 0x1;
-               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
-       
-               /* Write DCache victim */
-               arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,9,1,0), ARMV4_5_LDR(1, 0));
-       
-               /* clear interpret mode */
-               cp15c15 &= ~0x1;
-               arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-       }
-
-       /* read ICache content */
-       fprintf(output, "ICache:\n");
-       
-       /* go through segments 0 to nsets (8 on ARM920T, 4 on ARM922T) */ 
-       for (segment = 0; segment < arm920t->armv4_5_mmu.armv4_5_cache.d_u_size.nsets; segment++)
-       {
-               fprintf(output, "segment: %i\n----------", segment);
-               
-               /* Ra: r0 = SBZ(31:8):segment(7:5):SBZ(4:0) */
-               regs[0] = 0x0 | (segment << 5);
-               arm9tdmi_write_core_regs(target, 0x1, regs);
-               
-               /* set interpret mode */
-               cp15c15 |= 0x1;
-               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
-               
-               /* I CAM Read, loads current victim into C15.C.I.Ind */
-               arm920t_execute_cp15(target, ARMV4_5_MCR(15,2,0,15,5,2), ARMV4_5_LDR(1, 0));
-       
-               /* read current victim */
-               arm920t_read_cp15_physical(target, 0x3b, &C15_C_I_Ind);
-
-               /* clear interpret mode */
-               cp15c15 &= ~0x1;
-               arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-
-               for (index = 0; index < 64; index++)
-               {
-                       /* Ra: r0 = index(31:26):SBZ(25:8):segment(7:5):SBZ(4:0) */
-                       regs[0] = 0x0 | (segment << 5) | (index << 26);
-                       arm9tdmi_write_core_regs(target, 0x1, regs);
-
-                       /* set interpret mode */
-                       cp15c15 |= 0x1;
-                       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
-       
-                       /* Write ICache victim */
-                       arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,9,1,1), ARMV4_5_LDR(1, 0));
-       
-                       /* Read I RAM */
-                       arm920t_execute_cp15(target, ARMV4_5_MCR(15,2,0,15,9,2), ARMV4_5_LDMIA(0, 0x1fe, 0, 0));
-                       
-                       /* Read I CAM */
-                       arm920t_execute_cp15(target, ARMV4_5_MCR(15,2,0,15,5,2), ARMV4_5_LDR(9, 0));
-                       
-                       /* clear interpret mode */
-                       cp15c15 &= ~0x1;
-                       arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-
-                       /* read I RAM and CAM content */
-                       arm9tdmi_read_core_regs(target, 0x3fe, regs_p);
-                       jtag_execute_queue();
-
-                       i_cache[segment][index].cam = regs[9];
-                       
-                       /* mask LFSR[6] */
-                       regs[9] &= 0xfffffffe;
-                       fprintf(output, "\nsegment: %i, index: %i, CAM: 0x%8.8x, content (%s):\n", segment, index, regs[9], (regs[9] & 0x10) ? "valid" : "invalid");
-                       
-                       for (i = 1; i < 9; i++)
-                       {
-                                i_cache[segment][index].data[i] = regs[i];
-                                fprintf(output, "%i: 0x%8.8x\n", i-1, regs[i]);
-                       }
-       
-               }
-               
-       
-               /* Ra: r0 = index(31:26):SBZ(25:8):segment(7:5):SBZ(4:0) */
-               regs[0] = 0x0 | (segment << 5) | (C15_C_D_Ind << 26);
-               arm9tdmi_write_core_regs(target, 0x1, regs);
-
-               /* set interpret mode */
-               cp15c15 |= 0x1;
-               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
-       
-               /* Write ICache victim */
-               arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,9,1,1), ARMV4_5_LDR(1, 0));
-       
-               /* clear interpret mode */
-               cp15c15 &= ~0x1;
-               arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-       }
-       
-       /* restore CP15 MMU and Cache settings */
-       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), cp15_ctrl_saved);
-       
-       command_print(cmd_ctx, "cache content successfully output to %s", args[0]);
-       
-       fclose(output);
-       
-       /* mark registers dirty. */
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 1).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 1).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 2).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 2).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 3).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 3).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 4).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 4).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 5).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 5).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 6).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 6).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 7).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 7).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 8).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 8).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 9).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 9).valid;
-       
-       return ERROR_OK;
-}
-
-int arm920t_handle_read_mmu_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm920t_common_t *arm920t;
-       arm_jtag_t *jtag_info;
-       u32 cp15c15;
-       u32 cp15_ctrl, cp15_ctrl_saved;
-       u32 regs[16];
-       u32 *regs_p[16];
-       int i;
-       FILE *output;
-       u32 Dlockdown, Ilockdown;
-       arm920t_tlb_entry_t d_tlb[64], i_tlb[64];
-       int victim;
-       
-       if (argc != 1)
-       {
-               command_print(cmd_ctx, "usage: arm920t read_mmu <filename>");
-               return ERROR_OK;
-       }
-       
-       if ((output = fopen(args[0], "w")) == NULL)
-       {
-               DEBUG("error opening mmu content file");
-               return ERROR_OK;
-       }
-       
-       for (i = 0; i < 16; i++)
-               regs_p[i] = &regs[i];
-               
-       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM920t target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       /* disable MMU and Caches */
-       arm920t_read_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), &cp15_ctrl);
-       jtag_execute_queue();
-       cp15_ctrl_saved = cp15_ctrl;
-       cp15_ctrl &= ~(ARMV4_5_MMU_ENABLED | ARMV4_5_D_U_CACHE_ENABLED | ARMV4_5_I_CACHE_ENABLED);
-       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), cp15_ctrl);
-
-       /* read CP15 test state register */ 
-       arm920t_read_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), &cp15c15);
-       jtag_execute_queue();
-
-       /* prepare reading D TLB content 
-        * */
-       
-       /* set interpret mode */
-       cp15c15 |= 0x1;
-       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
-       
-       /* Read D TLB lockdown */
-       arm920t_execute_cp15(target, ARMV4_5_MRC(15,0,0,10,0,0), ARMV4_5_LDR(1, 0));
-       
-       /* clear interpret mode */
-       cp15c15 &= ~0x1;
-       arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-       
-       /* read D TLB lockdown stored to r1 */
-       arm9tdmi_read_core_regs(target, 0x2, regs_p);
-       jtag_execute_queue();
-       Dlockdown = regs[1];
-       
-       for (victim = 0; victim < 64; victim += 8)
-       {
-               /* new lockdown value: base[31:26]:victim[25:20]:SBZ[19:1]:p[0] 
-                * base remains unchanged, victim goes through entries 0 to 63 */
-               regs[1] = (Dlockdown & 0xfc000000) | (victim << 20);
-               arm9tdmi_write_core_regs(target, 0x2, regs);
-               
-               /* set interpret mode */
-               cp15c15 |= 0x1;
-               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
-               
-               /* Write D TLB lockdown */
-               arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,0), ARMV4_5_STR(1, 0));
-       
-               /* Read D TLB CAM */
-               arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,6,4), ARMV4_5_LDMIA(0, 0x3fc, 0, 0));
-               
-               /* clear interpret mode */
-               cp15c15 &= ~0x1;
-               arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-               
-               /* read D TLB CAM content stored to r2-r9 */
-               arm9tdmi_read_core_regs(target, 0x3fc, regs_p);
-               jtag_execute_queue();
-               
-               for (i = 0; i < 8; i++)
-                       d_tlb[victim + i].cam = regs[i + 2]; 
-       }
-
-       for (victim = 0; victim < 64; victim++)
-       {
-               /* new lockdown value: base[31:26]:victim[25:20]:SBZ[19:1]:p[0] 
-                * base remains unchanged, victim goes through entries 0 to 63 */
-               regs[1] = (Dlockdown & 0xfc000000) | (victim << 20);
-               arm9tdmi_write_core_regs(target, 0x2, regs);
-               
-               /* set interpret mode */
-               cp15c15 |= 0x1;
-               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
-               
-               /* Write D TLB lockdown */
-               arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,0), ARMV4_5_STR(1, 0));
-       
-               /* Read D TLB RAM1 */
-               arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,10,4), ARMV4_5_LDR(2,0));
-
-               /* Read D TLB RAM2 */
-               arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,2,5), ARMV4_5_LDR(3,0));
-               
-               /* clear interpret mode */
-               cp15c15 &= ~0x1;
-               arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-               
-               /* read D TLB RAM content stored to r2 and r3 */
-               arm9tdmi_read_core_regs(target, 0xc, regs_p);
-               jtag_execute_queue();
-
-               d_tlb[victim].ram1 = regs[2]; 
-               d_tlb[victim].ram2 = regs[3]; 
-       }
-               
-       /* restore D TLB lockdown */
-       regs[1] = Dlockdown;
-       arm9tdmi_write_core_regs(target, 0x2, regs);
-       
-       /* Write D TLB lockdown */
-       arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,0), ARMV4_5_STR(1, 0));
-
-       /* prepare reading I TLB content 
-        * */
-       
-       /* set interpret mode */
-       cp15c15 |= 0x1;
-       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
-       
-       /* Read I TLB lockdown */
-       arm920t_execute_cp15(target, ARMV4_5_MRC(15,0,0,10,0,1), ARMV4_5_LDR(1, 0));
-       
-       /* clear interpret mode */
-       cp15c15 &= ~0x1;
-       arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-       
-       /* read I TLB lockdown stored to r1 */
-       arm9tdmi_read_core_regs(target, 0x2, regs_p);
-       jtag_execute_queue();
-       Ilockdown = regs[1];
-       
-       for (victim = 0; victim < 64; victim += 8)
-       {
-               /* new lockdown value: base[31:26]:victim[25:20]:SBZ[19:1]:p[0] 
-                * base remains unchanged, victim goes through entries 0 to 63 */
-               regs[1] = (Ilockdown & 0xfc000000) | (victim << 20);
-               arm9tdmi_write_core_regs(target, 0x2, regs);
-               
-               /* set interpret mode */
-               cp15c15 |= 0x1;
-               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
-               
-               /* Write I TLB lockdown */
-               arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,1), ARMV4_5_STR(1, 0));
-       
-               /* Read I TLB CAM */
-               arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,5,4), ARMV4_5_LDMIA(0, 0x3fc, 0, 0));
-               
-               /* clear interpret mode */
-               cp15c15 &= ~0x1;
-               arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-               
-               /* read I TLB CAM content stored to r2-r9 */
-               arm9tdmi_read_core_regs(target, 0x3fc, regs_p);
-               jtag_execute_queue();
-               
-               for (i = 0; i < 8; i++)
-                       i_tlb[i + victim].cam = regs[i + 2]; 
-       }
-
-       for (victim = 0; victim < 64; victim++)
-       {
-               /* new lockdown value: base[31:26]:victim[25:20]:SBZ[19:1]:p[0] 
-                * base remains unchanged, victim goes through entries 0 to 63 */
-               regs[1] = (Dlockdown & 0xfc000000) | (victim << 20);
-               arm9tdmi_write_core_regs(target, 0x2, regs);
-               
-               /* set interpret mode */
-               cp15c15 |= 0x1;
-               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
-               
-               /* Write I TLB lockdown */
-               arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,1), ARMV4_5_STR(1, 0));
-       
-               /* Read I TLB RAM1 */
-               arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,9,4), ARMV4_5_LDR(2,0));
-
-               /* Read I TLB RAM2 */
-               arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,1,5), ARMV4_5_LDR(3,0));
-               
-               /* clear interpret mode */
-               cp15c15 &= ~0x1;
-               arm920t_write_cp15_physical(target, 0x1e, cp15c15);
-               
-               /* read I TLB RAM content stored to r2 and r3 */
-               arm9tdmi_read_core_regs(target, 0xc, regs_p);
-               jtag_execute_queue();
-
-               i_tlb[victim].ram1 = regs[2]; 
-               i_tlb[victim].ram2 = regs[3]; 
-       }
-               
-       /* restore I TLB lockdown */
-       regs[1] = Ilockdown;
-       arm9tdmi_write_core_regs(target, 0x2, regs);
-       
-       /* Write I TLB lockdown */
-       arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,1), ARMV4_5_STR(1, 0));
-       
-       /* restore CP15 MMU and Cache settings */
-       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), cp15_ctrl_saved);
-
-       /* output data to file */       
-       fprintf(output, "D TLB content:\n");
-       for (i = 0; i < 64; i++)
-       {
-               fprintf(output, "%i: 0x%8.8x 0x%8.8x 0x%8.8x %s\n", i, d_tlb[i].cam, d_tlb[i].ram1, d_tlb[i].ram2, (d_tlb[i].cam & 0x20) ? "(valid)" : "(invalid)");
-       }
-
-       fprintf(output, "\n\nI TLB content:\n");
-       for (i = 0; i < 64; i++)
-       {
-               fprintf(output, "%i: 0x%8.8x 0x%8.8x 0x%8.8x %s\n", i, i_tlb[i].cam, i_tlb[i].ram1, i_tlb[i].ram2, (i_tlb[i].cam & 0x20) ? "(valid)" : "(invalid)");
-       }
-       
-       command_print(cmd_ctx, "mmu content successfully output to %s", args[0]);
-       
-       fclose(output);
-       
-       /* mark registers dirty */
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 1).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 1).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 2).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 2).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 3).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 3).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 4).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 4).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 5).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 5).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 6).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 6).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 7).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 7).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 8).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 8).valid;
-       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 9).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 9).valid;
-       
-       return ERROR_OK;
-}
-int arm920t_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       int retval;
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm920t_common_t *arm920t;
-       arm_jtag_t *jtag_info;
-
-       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM920t target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-
-       /* one or more argument, access a single register (write if second argument is given */
-       if (argc >= 1)
-       {
-               int address = strtoul(args[0], NULL, 0);
-
-               if (argc == 1)
-               {
-                       u32 value;
-                       if ((retval = arm920t_read_cp15_physical(target, address, &value)) != ERROR_OK)
-                       {
-                               command_print(cmd_ctx, "couldn't access reg %i", address);
-                               return ERROR_OK;
-                       }
-                       jtag_execute_queue();
-                       
-                       command_print(cmd_ctx, "%i: %8.8x", address, value);
-               }
-               else if (argc == 2)
-               {
-                       u32 value = strtoul(args[1], NULL, 0);
-                       if ((retval = arm920t_write_cp15_physical(target, address, value)) != ERROR_OK)
-                       {
-                               command_print(cmd_ctx, "couldn't access reg %i", address);
-                               return ERROR_OK;
-                       }
-                       command_print(cmd_ctx, "%i: %8.8x", address, value);
-               }
-       }
-
-       return ERROR_OK;
-}
-
-int arm920t_handle_cp15i_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       int retval;
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm920t_common_t *arm920t;
-       arm_jtag_t *jtag_info;
-
-       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM920t target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-
-       /* one or more argument, access a single register (write if second argument is given */
-       if (argc >= 1)
-       {
-               u32 opcode = strtoul(args[0], NULL, 0);
-
-               if (argc == 1)
-               {
-                       u32 value;
-                       if ((retval = arm920t_read_cp15_interpreted(target, opcode, 0x0, &value)) != ERROR_OK)
-                       {
-                               command_print(cmd_ctx, "couldn't execute %8.8x", opcode);
-                               return ERROR_OK;
-                       }
-                       
-                       command_print(cmd_ctx, "%8.8x: %8.8x", opcode, value);
-               }
-               else if (argc == 2)
-               {
-                       u32 value = strtoul(args[1], NULL, 0);
-                       if ((retval = arm920t_write_cp15_interpreted(target, opcode, value, 0)) != ERROR_OK)
-                       {
-                               command_print(cmd_ctx, "couldn't execute %8.8x", opcode);
-                               return ERROR_OK;
-                       }
-                       command_print(cmd_ctx, "%8.8x: %8.8x", opcode, value);
-               }
-               else if (argc == 3)
-               {
-                       u32 value = strtoul(args[1], NULL, 0);
-                       u32 address = strtoul(args[2], NULL, 0);
-                       if ((retval = arm920t_write_cp15_interpreted(target, opcode, value, address)) != ERROR_OK)
-                       {
-                               command_print(cmd_ctx, "couldn't execute %8.8x", opcode);
-                               return ERROR_OK;
-                       }
-                       command_print(cmd_ctx, "%8.8x: %8.8x %8.8x", opcode, value, address);
-               }
-       }
-       else
-       {
-               command_print(cmd_ctx, "usage: arm920t cp15i <opcode> [value] [address]");
-       }
-
-       return ERROR_OK;
-}
-
-int arm920t_handle_cache_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm920t_common_t *arm920t;
-       
-       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM920t target");
-               return ERROR_OK;
-       }
-       
-       return armv4_5_handle_cache_info_command(cmd_ctx, &arm920t->armv4_5_mmu.armv4_5_cache);
-}
-
-int arm920t_handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
-{      
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm920t_common_t *arm920t;
-       arm_jtag_t *jtag_info;
-
-       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM920t target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-               
-       return armv4_5_mmu_handle_virt2phys_command(cmd_ctx, cmd, args, argc, target, &arm920t->armv4_5_mmu);
-}
-
-int arm920t_handle_md_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
-{      
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm920t_common_t *arm920t;
-       arm_jtag_t *jtag_info;
-
-       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM920t target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-       
-       return armv4_5_mmu_handle_md_phys_command(cmd_ctx, cmd, args, argc, target, &arm920t->armv4_5_mmu);
-}
-
-int arm920t_handle_mw_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
-{      
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm920t_common_t *arm920t;
-       arm_jtag_t *jtag_info;
-
-       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM920t target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-       
-       return armv4_5_mmu_handle_mw_phys_command(cmd_ctx, cmd, args, argc, target, &arm920t->armv4_5_mmu);
-}
+/***************************************************************************\r
+ *   Copyright (C) 2005 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "arm920t.h"\r
+#include "jtag.h"\r
+#include "log.h"\r
+\r
+#include <stdlib.h>\r
+#include <string.h>\r
+\r
+#if 0\r
+#define _DEBUG_INSTRUCTION_EXECUTION_\r
+#endif\r
+\r
+/* cli handling */\r
+int arm920t_register_commands(struct command_context_s *cmd_ctx);\r
+\r
+int arm920t_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm920t_handle_cp15i_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm920t_handle_virt2phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm920t_handle_cache_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm920t_handle_md_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm920t_handle_mw_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+int arm920t_handle_read_cache_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm920t_handle_read_mmu_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+/* forward declarations */\r
+int arm920t_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);\r
+int arm920t_init_target(struct command_context_s *cmd_ctx, struct target_s *target);\r
+int arm920t_quit();\r
+int arm920t_arch_state(struct target_s *target);\r
+int arm920t_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);\r
+int arm920t_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);\r
+int arm920t_soft_reset_halt(struct target_s *target);\r
+\r
+#define ARM920T_CP15_PHYS_ADDR(x, y, z) ((x << 5) | (y << 1) << (z))\r
+\r
+target_type_t arm920t_target =\r
+{\r
+       .name = "arm920t",\r
+\r
+       .poll = arm7_9_poll,\r
+       .arch_state = arm920t_arch_state,\r
+\r
+       .target_request_data = arm7_9_target_request_data,\r
+\r
+       .halt = arm7_9_halt,\r
+       .resume = arm7_9_resume,\r
+       .step = arm7_9_step,\r
+\r
+       .assert_reset = arm7_9_assert_reset,\r
+       .deassert_reset = arm7_9_deassert_reset,\r
+       .soft_reset_halt = arm920t_soft_reset_halt,\r
+       .prepare_reset_halt = arm7_9_prepare_reset_halt,\r
+       \r
+       .get_gdb_reg_list = armv4_5_get_gdb_reg_list,\r
+\r
+       .read_memory = arm920t_read_memory,\r
+       .write_memory = arm920t_write_memory,\r
+       .bulk_write_memory = arm7_9_bulk_write_memory,\r
+       .checksum_memory = arm7_9_checksum_memory,\r
+       \r
+       .run_algorithm = armv4_5_run_algorithm,\r
+\r
+       .add_breakpoint = arm7_9_add_breakpoint,\r
+       .remove_breakpoint = arm7_9_remove_breakpoint,\r
+       .add_watchpoint = arm7_9_add_watchpoint,\r
+       .remove_watchpoint = arm7_9_remove_watchpoint,\r
+\r
+       .register_commands = arm920t_register_commands,\r
+       .target_command = arm920t_target_command,\r
+       .init_target = arm920t_init_target,\r
+       .quit = arm920t_quit\r
+};\r
+\r
+int arm920t_read_cp15_physical(target_t *target, int reg_addr, u32 *value)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       scan_field_t fields[4];\r
+       u8 access_type_buf = 1;\r
+       u8 reg_addr_buf = reg_addr & 0x3f;\r
+       u8 nr_w_buf = 0;\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_scann(jtag_info, 0xf);\r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+\r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 1;\r
+       fields[0].out_value = &access_type_buf;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+\r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 32;\r
+       fields[1].out_value = NULL;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = jtag_info->chain_pos;\r
+       fields[2].num_bits = 6;\r
+       fields[2].out_value = &reg_addr_buf;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+\r
+       fields[3].device = jtag_info->chain_pos;\r
+       fields[3].num_bits = 1;\r
+       fields[3].out_value = &nr_w_buf;\r
+       fields[3].out_mask = NULL;\r
+       fields[3].in_value = NULL;\r
+       fields[3].in_check_value = NULL;\r
+       fields[3].in_check_mask = NULL;\r
+       fields[3].in_handler = NULL;\r
+       fields[3].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(4, fields, -1);\r
+\r
+       fields[1].in_handler_priv = value;\r
+       fields[1].in_handler = arm_jtag_buf_to_u32;\r
+\r
+       jtag_add_dr_scan(4, fields, -1);\r
+\r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+       jtag_execute_queue();\r
+       DEBUG("addr: 0x%x value: %8.8x", reg_addr, *value);\r
+#endif\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm920t_write_cp15_physical(target_t *target, int reg_addr, u32 value)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       scan_field_t fields[4];\r
+       u8 access_type_buf = 1;\r
+       u8 reg_addr_buf = reg_addr & 0x3f;\r
+       u8 nr_w_buf = 1;\r
+       u8 value_buf[4];\r
+       \r
+       buf_set_u32(value_buf, 0, 32, value);\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_scann(jtag_info, 0xf);\r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+\r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 1;\r
+       fields[0].out_value = &access_type_buf;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+\r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 32;\r
+       fields[1].out_value = value_buf;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = jtag_info->chain_pos;\r
+       fields[2].num_bits = 6;\r
+       fields[2].out_value = &reg_addr_buf;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+\r
+       fields[3].device = jtag_info->chain_pos;\r
+       fields[3].num_bits = 1;\r
+       fields[3].out_value = &nr_w_buf;\r
+       fields[3].out_mask = NULL;\r
+       fields[3].in_value = NULL;\r
+       fields[3].in_check_value = NULL;\r
+       fields[3].in_check_mask = NULL;\r
+       fields[3].in_handler = NULL;\r
+       fields[3].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(4, fields, -1);\r
+\r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+       DEBUG("addr: 0x%x value: %8.8x", reg_addr, value);\r
+#endif\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm920t_execute_cp15(target_t *target, u32 cp15_opcode, u32 arm_opcode)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       scan_field_t fields[4];\r
+       u8 access_type_buf = 0;         /* interpreted access */\r
+       u8 reg_addr_buf = 0x0;\r
+       u8 nr_w_buf = 0;\r
+       u8 cp15_opcode_buf[4];\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_scann(jtag_info, 0xf);\r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+       \r
+       buf_set_u32(cp15_opcode_buf, 0, 32, cp15_opcode);\r
+\r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 1;\r
+       fields[0].out_value = &access_type_buf;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+\r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 32;\r
+       fields[1].out_value = cp15_opcode_buf;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = jtag_info->chain_pos;\r
+       fields[2].num_bits = 6;\r
+       fields[2].out_value = &reg_addr_buf;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+\r
+       fields[3].device = jtag_info->chain_pos;\r
+       fields[3].num_bits = 1;\r
+       fields[3].out_value = &nr_w_buf;\r
+       fields[3].out_mask = NULL;\r
+       fields[3].in_value = NULL;\r
+       fields[3].in_check_value = NULL;\r
+       fields[3].in_check_mask = NULL;\r
+       fields[3].in_handler = NULL;\r
+       fields[3].in_handler_priv = NULL;\r
+\r
+       jtag_add_dr_scan(4, fields, -1);\r
+\r
+       arm9tdmi_clock_out(jtag_info, arm_opcode, 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);\r
+       arm7_9_execute_sys_speed(target);\r
+       \r
+       if (jtag_execute_queue() != ERROR_OK)\r
+       {\r
+               ERROR("failed executing JTAG queue, exiting");\r
+               exit(-1);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm920t_read_cp15_interpreted(target_t *target, u32 cp15_opcode, u32 address, u32 *value)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       u32* regs_p[1];\r
+       u32 regs[2];\r
+       u32 cp15c15 = 0x0;\r
+\r
+       /* load address into R1 */\r
+       regs[1] = address;\r
+       arm9tdmi_write_core_regs(target, 0x2, regs); \r
+       \r
+       /* read-modify-write CP15 test state register \r
+       * to enable interpreted access mode */\r
+       arm920t_read_cp15_physical(target, 0x1e, &cp15c15);     \r
+       jtag_execute_queue();\r
+       cp15c15 |= 1;   /* set interpret mode */\r
+       arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+\r
+       /* execute CP15 instruction and ARM load (reading from coprocessor) */\r
+       arm920t_execute_cp15(target, cp15_opcode, ARMV4_5_LDR(0, 1));\r
+       \r
+       /* disable interpreted access mode */\r
+       cp15c15 &= ~1U; /* clear interpret mode */\r
+       arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+\r
+       /* retrieve value from R0 */\r
+       regs_p[0] = value;\r
+       arm9tdmi_read_core_regs(target, 0x1, regs_p);\r
+       jtag_execute_queue();\r
+       \r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+       DEBUG("cp15_opcode: %8.8x, address: %8.8x, value: %8.8x", cp15_opcode, address, *value);\r
+#endif\r
+\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = 1;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 1).dirty = 1;\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm920t_write_cp15_interpreted(target_t *target, u32 cp15_opcode, u32 value, u32 address)\r
+{\r
+       u32 cp15c15 = 0x0;\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       u32 regs[2];\r
+\r
+       /* load value, address into R0, R1 */\r
+       regs[0] = value;\r
+       regs[1] = address;\r
+       arm9tdmi_write_core_regs(target, 0x3, regs);\r
+\r
+       /* read-modify-write CP15 test state register \r
+       * to enable interpreted access mode */\r
+       arm920t_read_cp15_physical(target, 0x1e, &cp15c15);\r
+       jtag_execute_queue();\r
+       cp15c15 |= 1;   /* set interpret mode */\r
+       arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+\r
+       /* execute CP15 instruction and ARM store (writing to coprocessor) */\r
+       arm920t_execute_cp15(target, cp15_opcode, ARMV4_5_STR(0, 1));\r
+\r
+       /* disable interpreted access mode */\r
+       cp15c15 &= ~1U; /* set interpret mode */\r
+       arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+\r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+       DEBUG("cp15_opcode: %8.8x, value: %8.8x, address: %8.8x", cp15_opcode, value, address);\r
+#endif\r
+\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = 1;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 1).dirty = 1;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+u32 arm920t_get_ttb(target_t *target)\r
+{\r
+       int retval;\r
+       u32 ttb = 0x0;\r
+\r
+       if ((retval = arm920t_read_cp15_interpreted(target, 0xeebf0f51, 0x0, &ttb)) != ERROR_OK)\r
+               return retval;\r
+\r
+       return ttb;\r
+}\r
+\r
+void arm920t_disable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)\r
+{\r
+       u32 cp15_control;\r
+\r
+       /* read cp15 control register */\r
+       arm920t_read_cp15_physical(target, 0x2, &cp15_control);\r
+       jtag_execute_queue();\r
+               \r
+       if (mmu)\r
+               cp15_control &= ~0x1U;\r
+       \r
+       if (d_u_cache)\r
+               cp15_control &= ~0x4U;\r
+       \r
+       if (i_cache)\r
+               cp15_control &= ~0x1000U;\r
+\r
+       arm920t_write_cp15_physical(target, 0x2, cp15_control);\r
+}\r
+\r
+void arm920t_enable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)\r
+{\r
+       u32 cp15_control;\r
+\r
+       /* read cp15 control register */\r
+       arm920t_read_cp15_physical(target, 0x2, &cp15_control);\r
+       jtag_execute_queue();\r
+               \r
+       if (mmu)\r
+               cp15_control |= 0x1U;\r
+       \r
+       if (d_u_cache)\r
+               cp15_control |= 0x4U;\r
+       \r
+       if (i_cache)\r
+               cp15_control |= 0x1000U;\r
+       \r
+       arm920t_write_cp15_physical(target, 0x2, cp15_control);\r
+}\r
+\r
+void arm920t_post_debug_entry(target_t *target)\r
+{\r
+       u32 cp15c15;\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;\r
+       arm920t_common_t *arm920t = arm9tdmi->arch_info;\r
+       \r
+       /* examine cp15 control reg */\r
+       arm920t_read_cp15_physical(target, 0x2, &arm920t->cp15_control_reg);\r
+       jtag_execute_queue();\r
+       DEBUG("cp15_control_reg: %8.8x", arm920t->cp15_control_reg);\r
+\r
+       if (arm920t->armv4_5_mmu.armv4_5_cache.ctype == -1)\r
+       {\r
+               u32 cache_type_reg;\r
+               /* identify caches */\r
+               arm920t_read_cp15_physical(target, 0x1, &cache_type_reg);\r
+               jtag_execute_queue();\r
+               armv4_5_identify_cache(cache_type_reg, &arm920t->armv4_5_mmu.armv4_5_cache);\r
+       }\r
+\r
+       arm920t->armv4_5_mmu.mmu_enabled = (arm920t->cp15_control_reg & 0x1U) ? 1 : 0;\r
+       arm920t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = (arm920t->cp15_control_reg & 0x4U) ? 1 : 0;\r
+       arm920t->armv4_5_mmu.armv4_5_cache.i_cache_enabled = (arm920t->cp15_control_reg & 0x1000U) ? 1 : 0;\r
+\r
+       /* save i/d fault status and address register */\r
+       arm920t_read_cp15_interpreted(target, 0xee150f10, 0x0, &arm920t->d_fsr);\r
+       arm920t_read_cp15_interpreted(target, 0xee150f30, 0x0, &arm920t->i_fsr);\r
+       arm920t_read_cp15_interpreted(target, 0xee160f10, 0x0, &arm920t->d_far);\r
+       arm920t_read_cp15_interpreted(target, 0xee160f30, 0x0, &arm920t->i_far);\r
+       \r
+       DEBUG("D FSR: 0x%8.8x, D FAR: 0x%8.8x, I FSR: 0x%8.8x, I FAR: 0x%8.8x",\r
+               arm920t->d_fsr, arm920t->d_far, arm920t->i_fsr, arm920t->i_far);  \r
+\r
+       if (arm920t->preserve_cache)\r
+       {\r
+               /* read-modify-write CP15 test state register \r
+                * to disable I/D-cache linefills */\r
+               arm920t_read_cp15_physical(target, 0x1e, &cp15c15);\r
+               jtag_execute_queue();\r
+               cp15c15 |= 0x600;\r
+               arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+       }\r
+}\r
+\r
+void arm920t_pre_restore_context(target_t *target)\r
+{\r
+       u32 cp15c15;\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;\r
+       arm920t_common_t *arm920t = arm9tdmi->arch_info;\r
+       \r
+       /* restore i/d fault status and address register */\r
+       arm920t_write_cp15_interpreted(target, 0xee050f10, arm920t->d_fsr, 0x0);\r
+       arm920t_write_cp15_interpreted(target, 0xee050f30, arm920t->i_fsr, 0x0);\r
+       arm920t_write_cp15_interpreted(target, 0xee060f10, arm920t->d_far, 0x0);\r
+       arm920t_write_cp15_interpreted(target, 0xee060f30, arm920t->i_far, 0x0);\r
+       \r
+       /* read-modify-write CP15 test state register \r
+       * to reenable I/D-cache linefills */\r
+       if (arm920t->preserve_cache)\r
+       {\r
+               arm920t_read_cp15_physical(target, 0x1e, &cp15c15);\r
+               jtag_execute_queue();\r
+               cp15c15 &= ~0x600U;\r
+               arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+       }\r
+}\r
+\r
+int arm920t_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p, arm9tdmi_common_t **arm9tdmi_p, arm920t_common_t **arm920t_p)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm920t_common_t *arm920t;\r
+       \r
+       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       arm7_9 = armv4_5->arch_info;\r
+       if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       arm9tdmi = arm7_9->arch_info;\r
+       if (arm9tdmi->common_magic != ARM9TDMI_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       arm920t = arm9tdmi->arch_info;\r
+       if (arm920t->common_magic != ARM920T_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       *armv4_5_p = armv4_5;\r
+       *arm7_9_p = arm7_9;\r
+       *arm9tdmi_p = arm9tdmi;\r
+       *arm920t_p = arm920t;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm920t_arch_state(struct target_s *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;\r
+       arm920t_common_t *arm920t = arm9tdmi->arch_info;\r
+       \r
+       char *state[] = \r
+       {\r
+               "disabled", "enabled"\r
+       };\r
+       \r
+       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)\r
+       {\r
+               ERROR("BUG: called for a non-ARMv4/5 target");\r
+               exit(-1);\r
+       }\r
+       \r
+       USER(   "target halted in %s state due to %s, current mode: %s\n"\r
+                       "cpsr: 0x%8.8x pc: 0x%8.8x\n"\r
+                       "MMU: %s, D-Cache: %s, I-Cache: %s",\r
+                        armv4_5_state_strings[armv4_5->core_state],\r
+                        target_debug_reason_strings[target->debug_reason],\r
+                        armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)],\r
+                        buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),\r
+                        buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32),\r
+                        state[arm920t->armv4_5_mmu.mmu_enabled],\r
+                        state[arm920t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled], \r
+                        state[arm920t->armv4_5_mmu.armv4_5_cache.i_cache_enabled]);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm920t_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)\r
+{\r
+       int retval;\r
+       \r
+       retval = arm7_9_read_memory(target, address, size, count, buffer);\r
+       \r
+       return retval;\r
+}\r
+\r
+int arm920t_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)\r
+{\r
+       int retval;\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;\r
+       arm920t_common_t *arm920t = arm9tdmi->arch_info;\r
+       \r
+       if ((retval = arm7_9_write_memory(target, address, size, count, buffer)) != ERROR_OK)\r
+               return retval;\r
+\r
+       if (((size == 4) || (size == 2)) && (count == 1))\r
+       {\r
+               if (arm920t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled)\r
+               {\r
+                       DEBUG("D-Cache enabled, writing through to main memory");\r
+                       u32 pa, cb, ap;\r
+                       int type, domain;\r
+\r
+                       pa = armv4_5_mmu_translate_va(target, &arm920t->armv4_5_mmu, address, &type, &cb, &domain, &ap);\r
+                       if (type == -1)\r
+                               return ERROR_OK;\r
+                       /* cacheable & bufferable means write-back region */\r
+                       if (cb == 3)\r
+                               armv4_5_mmu_write_physical(target, &arm920t->armv4_5_mmu, pa, size, count, buffer);\r
+               }\r
+               \r
+               if (arm920t->armv4_5_mmu.armv4_5_cache.i_cache_enabled)\r
+               {\r
+                       DEBUG("I-Cache enabled, invalidating affected I-Cache line");\r
+                       arm920t_write_cp15_interpreted(target, 0xee070f35, 0x0, address);\r
+               }\r
+       }\r
+\r
+       return retval;\r
+}\r
+\r
+int arm920t_soft_reset_halt(struct target_s *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;\r
+       arm920t_common_t *arm920t = arm9tdmi->arch_info;\r
+       reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];\r
+       \r
+       if (target->state == TARGET_RUNNING)\r
+       {\r
+               target->type->halt(target);\r
+       }\r
+       \r
+       while (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) == 0)\r
+       {\r
+               embeddedice_read_reg(dbg_stat);\r
+               jtag_execute_queue();\r
+       }\r
+       \r
+       target->state = TARGET_HALTED;\r
+       \r
+       /* SVC, ARM state, IRQ and FIQ disabled */\r
+       buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8, 0xd3);\r
+       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;\r
+       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;\r
+       \r
+       /* start fetching from 0x0 */\r
+       buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, 0x0);\r
+       armv4_5->core_cache->reg_list[15].dirty = 1;\r
+       armv4_5->core_cache->reg_list[15].valid = 1;\r
+       \r
+       armv4_5->core_mode = ARMV4_5_MODE_SVC;\r
+       armv4_5->core_state = ARMV4_5_STATE_ARM;\r
+       \r
+       arm920t_disable_mmu_caches(target, 1, 1, 1);\r
+       arm920t->armv4_5_mmu.mmu_enabled = 0;\r
+       arm920t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 0;\r
+       arm920t->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;\r
+\r
+       target_call_event_callbacks(target, TARGET_EVENT_HALTED);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm920t_init_target(struct command_context_s *cmd_ctx, struct target_s *target)\r
+{\r
+       arm9tdmi_init_target(cmd_ctx, target);\r
+               \r
+       return ERROR_OK;\r
+       \r
+}\r
+\r
+int arm920t_quit()\r
+{\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm920t_init_arch_info(target_t *target, arm920t_common_t *arm920t, int chain_pos, char *variant)\r
+{\r
+       arm9tdmi_common_t *arm9tdmi = &arm920t->arm9tdmi_common;\r
+       arm7_9_common_t *arm7_9 = &arm9tdmi->arm7_9_common;\r
+       \r
+       /* initialize arm9tdmi specific info (including arm7_9 and armv4_5)\r
+        */\r
+       arm9tdmi_init_arch_info(target, arm9tdmi, chain_pos, variant);\r
+\r
+       arm9tdmi->arch_info = arm920t;\r
+       arm920t->common_magic = ARM920T_COMMON_MAGIC;\r
+       \r
+       arm7_9->post_debug_entry = arm920t_post_debug_entry;\r
+       arm7_9->pre_restore_context = arm920t_pre_restore_context;\r
+       \r
+       arm920t->armv4_5_mmu.armv4_5_cache.ctype = -1;\r
+       arm920t->armv4_5_mmu.get_ttb = arm920t_get_ttb;\r
+       arm920t->armv4_5_mmu.read_memory = arm7_9_read_memory;\r
+       arm920t->armv4_5_mmu.write_memory = arm7_9_write_memory;\r
+       arm920t->armv4_5_mmu.disable_mmu_caches = arm920t_disable_mmu_caches;\r
+       arm920t->armv4_5_mmu.enable_mmu_caches = arm920t_enable_mmu_caches;\r
+       arm920t->armv4_5_mmu.has_tiny_pages = 1;\r
+       arm920t->armv4_5_mmu.mmu_enabled = 0;\r
+       \r
+       /* disabling linefills leads to lockups, so keep them enabled for now\r
+        * this doesn't affect correctness, but might affect timing issues, if\r
+        * important data is evicted from the cache during the debug session\r
+        * */\r
+       arm920t->preserve_cache = 0;\r
+       \r
+       /* override hw single-step capability from ARM9TDMI */\r
+       arm7_9->has_single_step = 1;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm920t_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)\r
+{\r
+       int chain_pos;\r
+       char *variant = NULL;\r
+       arm920t_common_t *arm920t = malloc(sizeof(arm920t_common_t));\r
+       \r
+       if (argc < 4)\r
+       {\r
+               ERROR("'target arm920t' requires at least one additional argument");\r
+               exit(-1);\r
+       }\r
+       \r
+       chain_pos = strtoul(args[3], NULL, 0);\r
+       \r
+       if (argc >= 5)\r
+               variant = args[4];\r
+       \r
+       DEBUG("chain_pos: %i, variant: %s", chain_pos, variant);\r
+       \r
+       arm920t_init_arch_info(target, arm920t, chain_pos, variant);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm920t_register_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       int retval;\r
+       command_t *arm920t_cmd;\r
+       \r
+               \r
+       retval = arm9tdmi_register_commands(cmd_ctx);\r
+       \r
+       arm920t_cmd = register_command(cmd_ctx, NULL, "arm920t", NULL, COMMAND_ANY, "arm920t specific commands");\r
+\r
+       register_command(cmd_ctx, arm920t_cmd, "cp15", arm920t_handle_cp15_command, COMMAND_EXEC, "display/modify cp15 register <num> [value]");\r
+       register_command(cmd_ctx, arm920t_cmd, "cp15i", arm920t_handle_cp15i_command, COMMAND_EXEC, "display/modify cp15 (interpreted access) <opcode> [value] [address]");\r
+       register_command(cmd_ctx, arm920t_cmd, "cache_info", arm920t_handle_cache_info_command, COMMAND_EXEC, "display information about target caches");\r
+       register_command(cmd_ctx, arm920t_cmd, "virt2phys", arm920t_handle_virt2phys_command, COMMAND_EXEC, "translate va to pa <va>");\r
+\r
+       register_command(cmd_ctx, arm920t_cmd, "mdw_phys", arm920t_handle_md_phys_command, COMMAND_EXEC, "display memory words <physical addr> [count]");\r
+       register_command(cmd_ctx, arm920t_cmd, "mdh_phys", arm920t_handle_md_phys_command, COMMAND_EXEC, "display memory half-words <physical addr> [count]");\r
+       register_command(cmd_ctx, arm920t_cmd, "mdb_phys", arm920t_handle_md_phys_command, COMMAND_EXEC, "display memory bytes <physical addr> [count]");\r
+\r
+       register_command(cmd_ctx, arm920t_cmd, "mww_phys", arm920t_handle_mw_phys_command, COMMAND_EXEC, "write memory word <physical addr> <value>");\r
+       register_command(cmd_ctx, arm920t_cmd, "mwh_phys", arm920t_handle_mw_phys_command, COMMAND_EXEC, "write memory half-word <physical addr> <value>");\r
+       register_command(cmd_ctx, arm920t_cmd, "mwb_phys", arm920t_handle_mw_phys_command, COMMAND_EXEC, "write memory byte <physical addr> <value>");\r
+\r
+       register_command(cmd_ctx, arm920t_cmd, "read_cache", arm920t_handle_read_cache_command, COMMAND_EXEC, "display I/D cache content");\r
+       register_command(cmd_ctx, arm920t_cmd, "read_mmu", arm920t_handle_read_mmu_command, COMMAND_EXEC, "display I/D mmu content");\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm920t_handle_read_cache_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm920t_common_t *arm920t;\r
+       arm_jtag_t *jtag_info;\r
+       u32 cp15c15;\r
+       u32 cp15_ctrl, cp15_ctrl_saved;\r
+       u32 regs[16];\r
+       u32 *regs_p[16];\r
+       u32 C15_C_D_Ind, C15_C_I_Ind;\r
+       int i;\r
+       FILE *output;\r
+       arm920t_cache_line_t d_cache[8][64], i_cache[8][64];\r
+       int segment, index;\r
+       \r
+       if (argc != 1)\r
+       {\r
+               command_print(cmd_ctx, "usage: arm920t read_cache <filename>");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if ((output = fopen(args[0], "w")) == NULL)\r
+       {\r
+               DEBUG("error opening cache content file");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       for (i = 0; i < 16; i++)\r
+               regs_p[i] = &regs[i];\r
+               \r
+       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM920t target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       /* disable MMU and Caches */\r
+       arm920t_read_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), &cp15_ctrl);\r
+       jtag_execute_queue();\r
+       cp15_ctrl_saved = cp15_ctrl;\r
+       cp15_ctrl &= ~(ARMV4_5_MMU_ENABLED | ARMV4_5_D_U_CACHE_ENABLED | ARMV4_5_I_CACHE_ENABLED);\r
+       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), cp15_ctrl);\r
+\r
+       /* read CP15 test state register */ \r
+       arm920t_read_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), &cp15c15);\r
+       jtag_execute_queue();\r
+       \r
+       /* read DCache content */\r
+       fprintf(output, "DCache:\n");\r
+       \r
+       /* go through segments 0 to nsets (8 on ARM920T, 4 on ARM922T) */ \r
+       for (segment = 0; segment < arm920t->armv4_5_mmu.armv4_5_cache.d_u_size.nsets; segment++)\r
+       {\r
+               fprintf(output, "\nsegment: %i\n----------", segment);\r
+               \r
+               /* Ra: r0 = SBZ(31:8):segment(7:5):SBZ(4:0) */\r
+               regs[0] = 0x0 | (segment << 5);\r
+               arm9tdmi_write_core_regs(target, 0x1, regs);\r
+               \r
+               /* set interpret mode */\r
+               cp15c15 |= 0x1;\r
+               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);\r
+               \r
+               /* D CAM Read, loads current victim into C15.C.D.Ind */\r
+               arm920t_execute_cp15(target, ARMV4_5_MCR(15,2,0,15,6,2), ARMV4_5_LDR(1, 0));\r
+       \r
+               /* read current victim */\r
+               arm920t_read_cp15_physical(target, 0x3d, &C15_C_D_Ind);\r
+\r
+               /* clear interpret mode */\r
+               cp15c15 &= ~0x1;\r
+               arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+\r
+               for (index = 0; index < 64; index++)\r
+               {\r
+                       /* Ra: r0 = index(31:26):SBZ(25:8):segment(7:5):SBZ(4:0) */\r
+                       regs[0] = 0x0 | (segment << 5) | (index << 26);\r
+                       arm9tdmi_write_core_regs(target, 0x1, regs);\r
+\r
+                       /* set interpret mode */\r
+                       cp15c15 |= 0x1;\r
+                       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);\r
+       \r
+                       /* Write DCache victim */\r
+                       arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,9,1,0), ARMV4_5_LDR(1, 0));\r
+       \r
+                       /* Read D RAM */\r
+                       arm920t_execute_cp15(target, ARMV4_5_MCR(15,2,0,15,10,2), ARMV4_5_LDMIA(0, 0x1fe, 0, 0));\r
+                       \r
+                       /* Read D CAM */\r
+                       arm920t_execute_cp15(target, ARMV4_5_MCR(15,2,0,15,6,2), ARMV4_5_LDR(9, 0));\r
+                       \r
+                       /* clear interpret mode */\r
+                       cp15c15 &= ~0x1;\r
+                       arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+\r
+                       /* read D RAM and CAM content */\r
+                       arm9tdmi_read_core_regs(target, 0x3fe, regs_p);\r
+                       jtag_execute_queue();\r
+\r
+                       d_cache[segment][index].cam = regs[9];\r
+                       \r
+                       /* mask LFSR[6] */\r
+                       regs[9] &= 0xfffffffe;\r
+                       fprintf(output, "\nsegment: %i, index: %i, CAM: 0x%8.8x, content (%s):\n", segment, index, regs[9], (regs[9] & 0x10) ? "valid" : "invalid");\r
+                       \r
+                       for (i = 1; i < 9; i++)\r
+                       {\r
+                                d_cache[segment][index].data[i] = regs[i];\r
+                                fprintf(output, "%i: 0x%8.8x\n", i-1, regs[i]);\r
+                       }\r
+       \r
+               }\r
+               \r
+               /* Ra: r0 = index(31:26):SBZ(25:8):segment(7:5):SBZ(4:0) */\r
+               regs[0] = 0x0 | (segment << 5) | (C15_C_D_Ind << 26);\r
+               arm9tdmi_write_core_regs(target, 0x1, regs);\r
+\r
+               /* set interpret mode */\r
+               cp15c15 |= 0x1;\r
+               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);\r
+       \r
+               /* Write DCache victim */\r
+               arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,9,1,0), ARMV4_5_LDR(1, 0));\r
+       \r
+               /* clear interpret mode */\r
+               cp15c15 &= ~0x1;\r
+               arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+       }\r
+\r
+       /* read ICache content */\r
+       fprintf(output, "ICache:\n");\r
+       \r
+       /* go through segments 0 to nsets (8 on ARM920T, 4 on ARM922T) */ \r
+       for (segment = 0; segment < arm920t->armv4_5_mmu.armv4_5_cache.d_u_size.nsets; segment++)\r
+       {\r
+               fprintf(output, "segment: %i\n----------", segment);\r
+               \r
+               /* Ra: r0 = SBZ(31:8):segment(7:5):SBZ(4:0) */\r
+               regs[0] = 0x0 | (segment << 5);\r
+               arm9tdmi_write_core_regs(target, 0x1, regs);\r
+               \r
+               /* set interpret mode */\r
+               cp15c15 |= 0x1;\r
+               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);\r
+               \r
+               /* I CAM Read, loads current victim into C15.C.I.Ind */\r
+               arm920t_execute_cp15(target, ARMV4_5_MCR(15,2,0,15,5,2), ARMV4_5_LDR(1, 0));\r
+       \r
+               /* read current victim */\r
+               arm920t_read_cp15_physical(target, 0x3b, &C15_C_I_Ind);\r
+\r
+               /* clear interpret mode */\r
+               cp15c15 &= ~0x1;\r
+               arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+\r
+               for (index = 0; index < 64; index++)\r
+               {\r
+                       /* Ra: r0 = index(31:26):SBZ(25:8):segment(7:5):SBZ(4:0) */\r
+                       regs[0] = 0x0 | (segment << 5) | (index << 26);\r
+                       arm9tdmi_write_core_regs(target, 0x1, regs);\r
+\r
+                       /* set interpret mode */\r
+                       cp15c15 |= 0x1;\r
+                       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);\r
+       \r
+                       /* Write ICache victim */\r
+                       arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,9,1,1), ARMV4_5_LDR(1, 0));\r
+       \r
+                       /* Read I RAM */\r
+                       arm920t_execute_cp15(target, ARMV4_5_MCR(15,2,0,15,9,2), ARMV4_5_LDMIA(0, 0x1fe, 0, 0));\r
+                       \r
+                       /* Read I CAM */\r
+                       arm920t_execute_cp15(target, ARMV4_5_MCR(15,2,0,15,5,2), ARMV4_5_LDR(9, 0));\r
+                       \r
+                       /* clear interpret mode */\r
+                       cp15c15 &= ~0x1;\r
+                       arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+\r
+                       /* read I RAM and CAM content */\r
+                       arm9tdmi_read_core_regs(target, 0x3fe, regs_p);\r
+                       jtag_execute_queue();\r
+\r
+                       i_cache[segment][index].cam = regs[9];\r
+                       \r
+                       /* mask LFSR[6] */\r
+                       regs[9] &= 0xfffffffe;\r
+                       fprintf(output, "\nsegment: %i, index: %i, CAM: 0x%8.8x, content (%s):\n", segment, index, regs[9], (regs[9] & 0x10) ? "valid" : "invalid");\r
+                       \r
+                       for (i = 1; i < 9; i++)\r
+                       {\r
+                                i_cache[segment][index].data[i] = regs[i];\r
+                                fprintf(output, "%i: 0x%8.8x\n", i-1, regs[i]);\r
+                       }\r
+       \r
+               }\r
+               \r
+       \r
+               /* Ra: r0 = index(31:26):SBZ(25:8):segment(7:5):SBZ(4:0) */\r
+               regs[0] = 0x0 | (segment << 5) | (C15_C_D_Ind << 26);\r
+               arm9tdmi_write_core_regs(target, 0x1, regs);\r
+\r
+               /* set interpret mode */\r
+               cp15c15 |= 0x1;\r
+               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);\r
+       \r
+               /* Write ICache victim */\r
+               arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,9,1,1), ARMV4_5_LDR(1, 0));\r
+       \r
+               /* clear interpret mode */\r
+               cp15c15 &= ~0x1;\r
+               arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+       }\r
+       \r
+       /* restore CP15 MMU and Cache settings */\r
+       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), cp15_ctrl_saved);\r
+       \r
+       command_print(cmd_ctx, "cache content successfully output to %s", args[0]);\r
+       \r
+       fclose(output);\r
+       \r
+       /* mark registers dirty. */\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 1).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 1).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 2).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 2).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 3).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 3).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 4).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 4).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 5).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 5).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 6).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 6).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 7).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 7).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 8).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 8).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 9).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 9).valid;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm920t_handle_read_mmu_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm920t_common_t *arm920t;\r
+       arm_jtag_t *jtag_info;\r
+       u32 cp15c15;\r
+       u32 cp15_ctrl, cp15_ctrl_saved;\r
+       u32 regs[16];\r
+       u32 *regs_p[16];\r
+       int i;\r
+       FILE *output;\r
+       u32 Dlockdown, Ilockdown;\r
+       arm920t_tlb_entry_t d_tlb[64], i_tlb[64];\r
+       int victim;\r
+       \r
+       if (argc != 1)\r
+       {\r
+               command_print(cmd_ctx, "usage: arm920t read_mmu <filename>");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if ((output = fopen(args[0], "w")) == NULL)\r
+       {\r
+               DEBUG("error opening mmu content file");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       for (i = 0; i < 16; i++)\r
+               regs_p[i] = &regs[i];\r
+               \r
+       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM920t target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       /* disable MMU and Caches */\r
+       arm920t_read_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), &cp15_ctrl);\r
+       jtag_execute_queue();\r
+       cp15_ctrl_saved = cp15_ctrl;\r
+       cp15_ctrl &= ~(ARMV4_5_MMU_ENABLED | ARMV4_5_D_U_CACHE_ENABLED | ARMV4_5_I_CACHE_ENABLED);\r
+       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), cp15_ctrl);\r
+\r
+       /* read CP15 test state register */ \r
+       arm920t_read_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), &cp15c15);\r
+       jtag_execute_queue();\r
+\r
+       /* prepare reading D TLB content \r
+        * */\r
+       \r
+       /* set interpret mode */\r
+       cp15c15 |= 0x1;\r
+       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);\r
+       \r
+       /* Read D TLB lockdown */\r
+       arm920t_execute_cp15(target, ARMV4_5_MRC(15,0,0,10,0,0), ARMV4_5_LDR(1, 0));\r
+       \r
+       /* clear interpret mode */\r
+       cp15c15 &= ~0x1;\r
+       arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+       \r
+       /* read D TLB lockdown stored to r1 */\r
+       arm9tdmi_read_core_regs(target, 0x2, regs_p);\r
+       jtag_execute_queue();\r
+       Dlockdown = regs[1];\r
+       \r
+       for (victim = 0; victim < 64; victim += 8)\r
+       {\r
+               /* new lockdown value: base[31:26]:victim[25:20]:SBZ[19:1]:p[0] \r
+                * base remains unchanged, victim goes through entries 0 to 63 */\r
+               regs[1] = (Dlockdown & 0xfc000000) | (victim << 20);\r
+               arm9tdmi_write_core_regs(target, 0x2, regs);\r
+               \r
+               /* set interpret mode */\r
+               cp15c15 |= 0x1;\r
+               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);\r
+               \r
+               /* Write D TLB lockdown */\r
+               arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,0), ARMV4_5_STR(1, 0));\r
+       \r
+               /* Read D TLB CAM */\r
+               arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,6,4), ARMV4_5_LDMIA(0, 0x3fc, 0, 0));\r
+               \r
+               /* clear interpret mode */\r
+               cp15c15 &= ~0x1;\r
+               arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+               \r
+               /* read D TLB CAM content stored to r2-r9 */\r
+               arm9tdmi_read_core_regs(target, 0x3fc, regs_p);\r
+               jtag_execute_queue();\r
+               \r
+               for (i = 0; i < 8; i++)\r
+                       d_tlb[victim + i].cam = regs[i + 2]; \r
+       }\r
+\r
+       for (victim = 0; victim < 64; victim++)\r
+       {\r
+               /* new lockdown value: base[31:26]:victim[25:20]:SBZ[19:1]:p[0] \r
+                * base remains unchanged, victim goes through entries 0 to 63 */\r
+               regs[1] = (Dlockdown & 0xfc000000) | (victim << 20);\r
+               arm9tdmi_write_core_regs(target, 0x2, regs);\r
+               \r
+               /* set interpret mode */\r
+               cp15c15 |= 0x1;\r
+               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);\r
+               \r
+               /* Write D TLB lockdown */\r
+               arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,0), ARMV4_5_STR(1, 0));\r
+       \r
+               /* Read D TLB RAM1 */\r
+               arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,10,4), ARMV4_5_LDR(2,0));\r
+\r
+               /* Read D TLB RAM2 */\r
+               arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,2,5), ARMV4_5_LDR(3,0));\r
+               \r
+               /* clear interpret mode */\r
+               cp15c15 &= ~0x1;\r
+               arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+               \r
+               /* read D TLB RAM content stored to r2 and r3 */\r
+               arm9tdmi_read_core_regs(target, 0xc, regs_p);\r
+               jtag_execute_queue();\r
+\r
+               d_tlb[victim].ram1 = regs[2]; \r
+               d_tlb[victim].ram2 = regs[3]; \r
+       }\r
+               \r
+       /* restore D TLB lockdown */\r
+       regs[1] = Dlockdown;\r
+       arm9tdmi_write_core_regs(target, 0x2, regs);\r
+       \r
+       /* Write D TLB lockdown */\r
+       arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,0), ARMV4_5_STR(1, 0));\r
+\r
+       /* prepare reading I TLB content \r
+        * */\r
+       \r
+       /* set interpret mode */\r
+       cp15c15 |= 0x1;\r
+       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);\r
+       \r
+       /* Read I TLB lockdown */\r
+       arm920t_execute_cp15(target, ARMV4_5_MRC(15,0,0,10,0,1), ARMV4_5_LDR(1, 0));\r
+       \r
+       /* clear interpret mode */\r
+       cp15c15 &= ~0x1;\r
+       arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+       \r
+       /* read I TLB lockdown stored to r1 */\r
+       arm9tdmi_read_core_regs(target, 0x2, regs_p);\r
+       jtag_execute_queue();\r
+       Ilockdown = regs[1];\r
+       \r
+       for (victim = 0; victim < 64; victim += 8)\r
+       {\r
+               /* new lockdown value: base[31:26]:victim[25:20]:SBZ[19:1]:p[0] \r
+                * base remains unchanged, victim goes through entries 0 to 63 */\r
+               regs[1] = (Ilockdown & 0xfc000000) | (victim << 20);\r
+               arm9tdmi_write_core_regs(target, 0x2, regs);\r
+               \r
+               /* set interpret mode */\r
+               cp15c15 |= 0x1;\r
+               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);\r
+               \r
+               /* Write I TLB lockdown */\r
+               arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,1), ARMV4_5_STR(1, 0));\r
+       \r
+               /* Read I TLB CAM */\r
+               arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,5,4), ARMV4_5_LDMIA(0, 0x3fc, 0, 0));\r
+               \r
+               /* clear interpret mode */\r
+               cp15c15 &= ~0x1;\r
+               arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+               \r
+               /* read I TLB CAM content stored to r2-r9 */\r
+               arm9tdmi_read_core_regs(target, 0x3fc, regs_p);\r
+               jtag_execute_queue();\r
+               \r
+               for (i = 0; i < 8; i++)\r
+                       i_tlb[i + victim].cam = regs[i + 2]; \r
+       }\r
+\r
+       for (victim = 0; victim < 64; victim++)\r
+       {\r
+               /* new lockdown value: base[31:26]:victim[25:20]:SBZ[19:1]:p[0] \r
+                * base remains unchanged, victim goes through entries 0 to 63 */\r
+               regs[1] = (Dlockdown & 0xfc000000) | (victim << 20);\r
+               arm9tdmi_write_core_regs(target, 0x2, regs);\r
+               \r
+               /* set interpret mode */\r
+               cp15c15 |= 0x1;\r
+               arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);\r
+               \r
+               /* Write I TLB lockdown */\r
+               arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,1), ARMV4_5_STR(1, 0));\r
+       \r
+               /* Read I TLB RAM1 */\r
+               arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,9,4), ARMV4_5_LDR(2,0));\r
+\r
+               /* Read I TLB RAM2 */\r
+               arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,1,5), ARMV4_5_LDR(3,0));\r
+               \r
+               /* clear interpret mode */\r
+               cp15c15 &= ~0x1;\r
+               arm920t_write_cp15_physical(target, 0x1e, cp15c15);\r
+               \r
+               /* read I TLB RAM content stored to r2 and r3 */\r
+               arm9tdmi_read_core_regs(target, 0xc, regs_p);\r
+               jtag_execute_queue();\r
+\r
+               i_tlb[victim].ram1 = regs[2]; \r
+               i_tlb[victim].ram2 = regs[3]; \r
+       }\r
+               \r
+       /* restore I TLB lockdown */\r
+       regs[1] = Ilockdown;\r
+       arm9tdmi_write_core_regs(target, 0x2, regs);\r
+       \r
+       /* Write I TLB lockdown */\r
+       arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,1), ARMV4_5_STR(1, 0));\r
+       \r
+       /* restore CP15 MMU and Cache settings */\r
+       arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), cp15_ctrl_saved);\r
+\r
+       /* output data to file */       \r
+       fprintf(output, "D TLB content:\n");\r
+       for (i = 0; i < 64; i++)\r
+       {\r
+               fprintf(output, "%i: 0x%8.8x 0x%8.8x 0x%8.8x %s\n", i, d_tlb[i].cam, d_tlb[i].ram1, d_tlb[i].ram2, (d_tlb[i].cam & 0x20) ? "(valid)" : "(invalid)");\r
+       }\r
+\r
+       fprintf(output, "\n\nI TLB content:\n");\r
+       for (i = 0; i < 64; i++)\r
+       {\r
+               fprintf(output, "%i: 0x%8.8x 0x%8.8x 0x%8.8x %s\n", i, i_tlb[i].cam, i_tlb[i].ram1, i_tlb[i].ram2, (i_tlb[i].cam & 0x20) ? "(valid)" : "(invalid)");\r
+       }\r
+       \r
+       command_print(cmd_ctx, "mmu content successfully output to %s", args[0]);\r
+       \r
+       fclose(output);\r
+       \r
+       /* mark registers dirty */\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 1).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 1).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 2).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 2).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 3).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 3).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 4).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 4).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 5).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 5).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 6).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 6).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 7).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 7).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 8).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 8).valid;\r
+       ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 9).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 9).valid;\r
+       \r
+       return ERROR_OK;\r
+}\r
+int arm920t_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       int retval;\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm920t_common_t *arm920t;\r
+       arm_jtag_t *jtag_info;\r
+\r
+       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM920t target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+\r
+       /* one or more argument, access a single register (write if second argument is given */\r
+       if (argc >= 1)\r
+       {\r
+               int address = strtoul(args[0], NULL, 0);\r
+\r
+               if (argc == 1)\r
+               {\r
+                       u32 value;\r
+                       if ((retval = arm920t_read_cp15_physical(target, address, &value)) != ERROR_OK)\r
+                       {\r
+                               command_print(cmd_ctx, "couldn't access reg %i", address);\r
+                               return ERROR_OK;\r
+                       }\r
+                       jtag_execute_queue();\r
+                       \r
+                       command_print(cmd_ctx, "%i: %8.8x", address, value);\r
+               }\r
+               else if (argc == 2)\r
+               {\r
+                       u32 value = strtoul(args[1], NULL, 0);\r
+                       if ((retval = arm920t_write_cp15_physical(target, address, value)) != ERROR_OK)\r
+                       {\r
+                               command_print(cmd_ctx, "couldn't access reg %i", address);\r
+                               return ERROR_OK;\r
+                       }\r
+                       command_print(cmd_ctx, "%i: %8.8x", address, value);\r
+               }\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm920t_handle_cp15i_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       int retval;\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm920t_common_t *arm920t;\r
+       arm_jtag_t *jtag_info;\r
+\r
+       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM920t target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+\r
+       /* one or more argument, access a single register (write if second argument is given */\r
+       if (argc >= 1)\r
+       {\r
+               u32 opcode = strtoul(args[0], NULL, 0);\r
+\r
+               if (argc == 1)\r
+               {\r
+                       u32 value;\r
+                       if ((retval = arm920t_read_cp15_interpreted(target, opcode, 0x0, &value)) != ERROR_OK)\r
+                       {\r
+                               command_print(cmd_ctx, "couldn't execute %8.8x", opcode);\r
+                               return ERROR_OK;\r
+                       }\r
+                       \r
+                       command_print(cmd_ctx, "%8.8x: %8.8x", opcode, value);\r
+               }\r
+               else if (argc == 2)\r
+               {\r
+                       u32 value = strtoul(args[1], NULL, 0);\r
+                       if ((retval = arm920t_write_cp15_interpreted(target, opcode, value, 0)) != ERROR_OK)\r
+                       {\r
+                               command_print(cmd_ctx, "couldn't execute %8.8x", opcode);\r
+                               return ERROR_OK;\r
+                       }\r
+                       command_print(cmd_ctx, "%8.8x: %8.8x", opcode, value);\r
+               }\r
+               else if (argc == 3)\r
+               {\r
+                       u32 value = strtoul(args[1], NULL, 0);\r
+                       u32 address = strtoul(args[2], NULL, 0);\r
+                       if ((retval = arm920t_write_cp15_interpreted(target, opcode, value, address)) != ERROR_OK)\r
+                       {\r
+                               command_print(cmd_ctx, "couldn't execute %8.8x", opcode);\r
+                               return ERROR_OK;\r
+                       }\r
+                       command_print(cmd_ctx, "%8.8x: %8.8x %8.8x", opcode, value, address);\r
+               }\r
+       }\r
+       else\r
+       {\r
+               command_print(cmd_ctx, "usage: arm920t cp15i <opcode> [value] [address]");\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm920t_handle_cache_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm920t_common_t *arm920t;\r
+       \r
+       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM920t target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       return armv4_5_handle_cache_info_command(cmd_ctx, &arm920t->armv4_5_mmu.armv4_5_cache);\r
+}\r
+\r
+int arm920t_handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)\r
+{      \r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm920t_common_t *arm920t;\r
+       arm_jtag_t *jtag_info;\r
+\r
+       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM920t target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+               \r
+       return armv4_5_mmu_handle_virt2phys_command(cmd_ctx, cmd, args, argc, target, &arm920t->armv4_5_mmu);\r
+}\r
+\r
+int arm920t_handle_md_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)\r
+{      \r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm920t_common_t *arm920t;\r
+       arm_jtag_t *jtag_info;\r
+\r
+       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM920t target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       return armv4_5_mmu_handle_md_phys_command(cmd_ctx, cmd, args, argc, target, &arm920t->armv4_5_mmu);\r
+}\r
+\r
+int arm920t_handle_mw_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)\r
+{      \r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm920t_common_t *arm920t;\r
+       arm_jtag_t *jtag_info;\r
+\r
+       if (arm920t_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm920t) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM920t target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       return armv4_5_mmu_handle_mw_phys_command(cmd_ctx, cmd, args, argc, target, &arm920t->armv4_5_mmu);\r
+}\r
index 75d43fd8a798991df45308ee2ff5e955a98f5a8c..a02c27ae79bd3289aac7c4b797c4928ec18685d7 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2007 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "arm926ejs.h"
-#include "jtag.h"
-#include "log.h"
-
-#include <stdlib.h>
-#include <string.h>
-
-#if 1
-#define _DEBUG_INSTRUCTION_EXECUTION_
-#endif
-
-/* cli handling */
-int arm926ejs_register_commands(struct command_context_s *cmd_ctx);
-
-int arm926ejs_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm926ejs_handle_cp15i_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm926ejs_handle_virt2phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm926ejs_handle_cache_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm926ejs_handle_md_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm926ejs_handle_mw_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-int arm926ejs_handle_read_cache_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int arm926ejs_handle_read_mmu_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-/* forward declarations */
-int arm926ejs_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);
-int arm926ejs_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
-int arm926ejs_quit();
-int arm926ejs_arch_state(struct target_s *target);
-int arm926ejs_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
-int arm926ejs_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
-int arm926ejs_soft_reset_halt(struct target_s *target);
-static int arm926ejs_virt2phys(struct target_s *target, u32 virtual, u32 *physical);
-static int arm926ejs_mmu(struct target_s *target, int *enabled);
-
-target_type_t arm926ejs_target =
-{
-       .name = "arm926ejs",
-
-       .poll = arm7_9_poll,
-       .arch_state = arm926ejs_arch_state,
-
-       .target_request_data = arm7_9_target_request_data,
-
-       .halt = arm7_9_halt,
-       .resume = arm7_9_resume,
-       .step = arm7_9_step,
-
-       .assert_reset = arm7_9_assert_reset,
-       .deassert_reset = arm7_9_deassert_reset,
-       .soft_reset_halt = arm926ejs_soft_reset_halt,
-       .prepare_reset_halt = arm7_9_prepare_reset_halt,
-       
-       .get_gdb_reg_list = armv4_5_get_gdb_reg_list,
-
-       .read_memory = arm7_9_read_memory,
-       .write_memory = arm926ejs_write_memory,
-       .bulk_write_memory = arm7_9_bulk_write_memory,
-       .checksum_memory = arm7_9_checksum_memory,
-       
-       .run_algorithm = armv4_5_run_algorithm,
-
-       .add_breakpoint = arm7_9_add_breakpoint,
-       .remove_breakpoint = arm7_9_remove_breakpoint,
-       .add_watchpoint = arm7_9_add_watchpoint,
-       .remove_watchpoint = arm7_9_remove_watchpoint,
-
-       .register_commands = arm926ejs_register_commands,
-       .target_command = arm926ejs_target_command,
-       .init_target = arm926ejs_init_target,
-       .quit = arm926ejs_quit,
-       .virt2phys = arm926ejs_virt2phys,
-       .mmu = arm926ejs_mmu
-};
-
-
-int arm926ejs_catch_broken_irscan(u8 *captured, void *priv, scan_field_t *field)
-{
-       /* The ARM926EJ-S' instruction register is 4 bits wide */
-       u8 t = *captured & 0xf;
-       u8 t2 = *field->in_check_value & 0xf;
-       if (t == t2)
-       {
-               return ERROR_OK;
-       }
-       else if ((t == 0x0f) || (t == 0x00))
-       {
-               DEBUG("caught ARM926EJ-S invalid Capture-IR result after CP15 access");
-               return ERROR_OK;
-       }
-       return ERROR_JTAG_QUEUE_FAILED;;
-}
-
-#define ARM926EJS_CP15_ADDR(opcode_1, opcode_2, CRn, CRm) ((opcode_1 << 11) | (opcode_2 << 8) | (CRn << 4) | (CRm << 0))
-
-int arm926ejs_cp15_read(target_t *target, u32 op1, u32 op2, u32 CRn, u32 CRm, u32 *value)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       u32 address = ARM926EJS_CP15_ADDR(op1, op2, CRn, CRm);
-       scan_field_t fields[4];
-       u8 address_buf[2];
-       u8 nr_w_buf = 0;
-       u8 access = 1;
-       
-       buf_set_u32(address_buf, 0, 14, address);
-       
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_scann(jtag_info, 0xf);
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = NULL;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 1;
-       fields[1].out_value = &access;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = &access;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = jtag_info->chain_pos;
-       fields[2].num_bits = 14;
-       fields[2].out_value = address_buf;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-
-       fields[3].device = jtag_info->chain_pos;
-       fields[3].num_bits = 1;
-       fields[3].out_value = &nr_w_buf;
-       fields[3].out_mask = NULL;
-       fields[3].in_value = NULL;
-       fields[3].in_check_value = NULL;
-       fields[3].in_check_mask = NULL;
-       fields[3].in_handler = NULL;
-       fields[3].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(4, fields, -1, NULL);
-
-       fields[0].in_handler_priv = value;
-       fields[0].in_handler = arm_jtag_buf_to_u32;
-       
-       do
-       {
-               /* rescan with NOP, to wait for the access to complete */
-               access = 0;
-               nr_w_buf = 0;
-               jtag_add_dr_scan(4, fields, -1, NULL);
-               jtag_execute_queue();
-       } while (buf_get_u32(&access, 0, 1) != 1);
-
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-       DEBUG("addr: 0x%x value: %8.8x", address, *value);
-#endif
-       
-       arm_jtag_set_instr(jtag_info, 0xc, &arm926ejs_catch_broken_irscan);
-
-       return ERROR_OK;
-}
-
-int arm926ejs_cp15_write(target_t *target, u32 op1, u32 op2, u32 CRn, u32 CRm, u32 value)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       u32 address = ARM926EJS_CP15_ADDR(op1, op2, CRn, CRm);
-       scan_field_t fields[4];
-       u8 value_buf[4];
-       u8 address_buf[2];
-       u8 nr_w_buf = 1;
-       u8 access = 1;
-       
-       buf_set_u32(address_buf, 0, 14, address);
-       buf_set_u32(value_buf, 0, 32, value);
-       
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_scann(jtag_info, 0xf);
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = value_buf;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 1;
-       fields[1].out_value = &access;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = &access;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = jtag_info->chain_pos;
-       fields[2].num_bits = 14;
-       fields[2].out_value = address_buf;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-
-       fields[3].device = jtag_info->chain_pos;
-       fields[3].num_bits = 1;
-       fields[3].out_value = &nr_w_buf;
-       fields[3].out_mask = NULL;
-       fields[3].in_value = NULL;
-       fields[3].in_check_value = NULL;
-       fields[3].in_check_mask = NULL;
-       fields[3].in_handler = NULL;
-       fields[3].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(4, fields, -1, NULL);
-
-       do
-       {
-               /* rescan with NOP, to wait for the access to complete */
-               access = 0;
-               nr_w_buf = 0;
-               jtag_add_dr_scan(4, fields, -1, NULL);
-               jtag_execute_queue();
-       } while (buf_get_u32(&access, 0, 1) != 1);
-
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-       DEBUG("addr: 0x%x value: %8.8x", address, value);
-#endif
-
-       arm_jtag_set_instr(jtag_info, 0xf, &arm926ejs_catch_broken_irscan);
-
-       return ERROR_OK;
-}
-
-int arm926ejs_examine_debug_reason(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
-       int debug_reason;
-       int retval;
-
-       embeddedice_read_reg(dbg_stat);
-       if ((retval = jtag_execute_queue()) != ERROR_OK)
-               return retval;
-       
-       debug_reason = buf_get_u32(dbg_stat->value, 6, 4);
-       
-       switch (debug_reason)
-       {
-               case 1:
-                       DEBUG("breakpoint from EICE unit 0");
-                       target->debug_reason = DBG_REASON_BREAKPOINT;
-                       break;
-               case 2:
-                       DEBUG("breakpoint from EICE unit 1");
-                       target->debug_reason = DBG_REASON_BREAKPOINT;
-                       break;
-               case 3:
-                       DEBUG("soft breakpoint (BKPT instruction)");
-                       target->debug_reason = DBG_REASON_BREAKPOINT;
-                       break;
-               case 4:
-                       DEBUG("vector catch breakpoint");
-                       target->debug_reason = DBG_REASON_BREAKPOINT;
-                       break;
-               case 5:
-                       DEBUG("external breakpoint");
-                       target->debug_reason = DBG_REASON_BREAKPOINT;
-                       break;
-               case 6:
-                       DEBUG("watchpoint from EICE unit 0");
-                       target->debug_reason = DBG_REASON_WATCHPOINT;
-                       break;
-               case 7:
-                       DEBUG("watchpoint from EICE unit 1");
-                       target->debug_reason = DBG_REASON_WATCHPOINT;
-                       break;
-               case 8:
-                       DEBUG("external watchpoint");
-                       target->debug_reason = DBG_REASON_WATCHPOINT;
-                       break;
-               case 9:
-                       DEBUG("internal debug request");
-                       target->debug_reason = DBG_REASON_DBGRQ;
-                       break;
-               case 10:
-                       DEBUG("external debug request");
-                       target->debug_reason = DBG_REASON_DBGRQ;
-                       break;
-               case 11:
-                       ERROR("BUG: debug re-entry from system speed access shouldn't be handled here");
-                       break;
-               default:
-                       ERROR("BUG: unknown debug reason: 0x%x", debug_reason);
-                       target->debug_reason = DBG_REASON_DBGRQ;
-       }
-       
-       return ERROR_OK;
-}
-
-u32 arm926ejs_get_ttb(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
-       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;
-       int retval;
-       u32 ttb = 0x0;
-
-       if ((retval = arm926ejs->read_cp15(target, 0, 0, 2, 0, &ttb)) != ERROR_OK)
-               return retval;
-
-       return ttb;
-}
-
-void arm926ejs_disable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
-       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;
-       u32 cp15_control;
-
-       /* read cp15 control register */
-       arm926ejs->read_cp15(target, 0, 0, 1, 0, &cp15_control);
-       jtag_execute_queue();
-       
-       if (mmu)
-       {
-               /* invalidate TLB */
-               arm926ejs->write_cp15(target, 0, 0, 8, 7, 0x0);
-               
-               cp15_control &= ~0x1U;
-       }
-       
-       if (d_u_cache)
-       {
-               u32 debug_override;
-               /* read-modify-write CP15 debug override register 
-                * to enable "test and clean all" */
-               arm926ejs->read_cp15(target, 0, 0, 15, 0, &debug_override);
-               debug_override |= 0x80000;
-               arm926ejs->write_cp15(target, 0, 0, 15, 0, debug_override);
-               
-               /* clean and invalidate DCache */
-               arm926ejs->write_cp15(target, 0, 0, 7, 5, 0x0);
-
-               /* write CP15 debug override register 
-                * to disable "test and clean all" */
-               debug_override &= ~0x80000;
-               arm926ejs->write_cp15(target, 0, 0, 15, 0, debug_override);
-               
-               cp15_control &= ~0x4U;
-       }
-       
-       if (i_cache)
-       {
-               /* invalidate ICache */
-               arm926ejs->write_cp15(target, 0, 0, 7, 5, 0x0);
-               
-               cp15_control &= ~0x1000U;
-       }
-       
-       arm926ejs->write_cp15(target, 0, 0, 1, 0, cp15_control);
-}
-
-void arm926ejs_enable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
-       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;
-       u32 cp15_control;
-
-       /* read cp15 control register */
-       arm926ejs->read_cp15(target, 0, 0, 1, 0, &cp15_control);
-       jtag_execute_queue();
-               
-       if (mmu)
-               cp15_control |= 0x1U;
-       
-       if (d_u_cache)
-               cp15_control |= 0x4U;
-       
-       if (i_cache)
-               cp15_control |= 0x1000U;
-       
-       arm926ejs->write_cp15(target, 0, 0, 1, 0, cp15_control);
-}
-
-void arm926ejs_post_debug_entry(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
-       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;
-
-       /* examine cp15 control reg */
-       arm926ejs->read_cp15(target, 0, 0, 1, 0, &arm926ejs->cp15_control_reg);
-       jtag_execute_queue();
-       DEBUG("cp15_control_reg: %8.8x", arm926ejs->cp15_control_reg);
-
-       if (arm926ejs->armv4_5_mmu.armv4_5_cache.ctype == -1)
-       {
-               u32 cache_type_reg;
-               /* identify caches */
-               arm926ejs->read_cp15(target, 0, 1, 0, 0, &cache_type_reg);
-               jtag_execute_queue();
-               armv4_5_identify_cache(cache_type_reg, &arm926ejs->armv4_5_mmu.armv4_5_cache);
-       }
-
-       arm926ejs->armv4_5_mmu.mmu_enabled = (arm926ejs->cp15_control_reg & 0x1U) ? 1 : 0;
-       arm926ejs->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = (arm926ejs->cp15_control_reg & 0x4U) ? 1 : 0;
-       arm926ejs->armv4_5_mmu.armv4_5_cache.i_cache_enabled = (arm926ejs->cp15_control_reg & 0x1000U) ? 1 : 0;
-
-       /* save i/d fault status and address register */
-       arm926ejs->read_cp15(target, 0, 0, 5, 0, &arm926ejs->d_fsr);
-       arm926ejs->read_cp15(target, 0, 1, 5, 0, &arm926ejs->i_fsr);
-       arm926ejs->read_cp15(target, 0, 0, 6, 0, &arm926ejs->d_far);
-       
-       DEBUG("D FSR: 0x%8.8x, D FAR: 0x%8.8x, I FSR: 0x%8.8x",
-               arm926ejs->d_fsr, arm926ejs->d_far, arm926ejs->i_fsr);  
-
-
-       u32 cache_dbg_ctrl;
-       
-       /* read-modify-write CP15 cache debug control register 
-        * to disable I/D-cache linefills and force WT */
-       arm926ejs->read_cp15(target, 7, 0, 15, 0, &cache_dbg_ctrl);
-       cache_dbg_ctrl |= 0x7;
-       arm926ejs->write_cp15(target, 7, 0, 15, 0, cache_dbg_ctrl);
-}
-
-void arm926ejs_pre_restore_context(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
-       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;
-
-       /* restore i/d fault status and address register */
-       arm926ejs->write_cp15(target, 0, 0, 5, 0, arm926ejs->d_fsr);
-       arm926ejs->write_cp15(target, 0, 1, 5, 0, arm926ejs->i_fsr);
-       arm926ejs->write_cp15(target, 0, 0, 6, 0, arm926ejs->d_far);
-       
-       u32 cache_dbg_ctrl;
-       
-       /* read-modify-write CP15 cache debug control register 
-        * to reenable I/D-cache linefills and disable WT */
-       arm926ejs->read_cp15(target, 7, 0, 15, 0, &cache_dbg_ctrl);
-       cache_dbg_ctrl &= ~0x7;
-       arm926ejs->write_cp15(target, 7, 0, 15, 0, cache_dbg_ctrl);
-}
-
-int arm926ejs_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p, arm9tdmi_common_t **arm9tdmi_p, arm926ejs_common_t **arm926ejs_p)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm926ejs_common_t *arm926ejs;
-       
-       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       arm7_9 = armv4_5->arch_info;
-       if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       arm9tdmi = arm7_9->arch_info;
-       if (arm9tdmi->common_magic != ARM9TDMI_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       arm926ejs = arm9tdmi->arch_info;
-       if (arm926ejs->common_magic != ARM926EJS_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       *armv4_5_p = armv4_5;
-       *arm7_9_p = arm7_9;
-       *arm9tdmi_p = arm9tdmi;
-       *arm926ejs_p = arm926ejs;
-       
-       return ERROR_OK;
-}
-
-int arm926ejs_arch_state(struct target_s *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
-       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;
-       
-       char *state[] = 
-       {
-               "disabled", "enabled"
-       };
-       
-       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
-       {
-               ERROR("BUG: called for a non-ARMv4/5 target");
-               exit(-1);
-       }
-       
-       USER(
-                       "target halted in %s state due to %s, current mode: %s\n"
-                       "cpsr: 0x%8.8x pc: 0x%8.8x\n"
-                       "MMU: %s, D-Cache: %s, I-Cache: %s",
-                        armv4_5_state_strings[armv4_5->core_state],
-                        target_debug_reason_strings[target->debug_reason],
-                        armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)],
-                        buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),
-                        buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32),
-                        state[arm926ejs->armv4_5_mmu.mmu_enabled],
-                        state[arm926ejs->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled], 
-                        state[arm926ejs->armv4_5_mmu.armv4_5_cache.i_cache_enabled]);
-       
-       return ERROR_OK;
-}
-
-int arm926ejs_soft_reset_halt(struct target_s *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
-       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;
-       reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
-       
-       if (target->state == TARGET_RUNNING)
-       {
-               target->type->halt(target);
-       }
-       
-       while (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) == 0)
-       {
-               embeddedice_read_reg(dbg_stat);
-               jtag_execute_queue();
-       }
-       
-       target->state = TARGET_HALTED;
-       
-       /* SVC, ARM state, IRQ and FIQ disabled */
-       buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8, 0xd3);
-       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
-       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
-       
-       /* start fetching from 0x0 */
-       buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, 0x0);
-       armv4_5->core_cache->reg_list[15].dirty = 1;
-       armv4_5->core_cache->reg_list[15].valid = 1;
-       
-       armv4_5->core_mode = ARMV4_5_MODE_SVC;
-       armv4_5->core_state = ARMV4_5_STATE_ARM;
-       
-       arm926ejs_disable_mmu_caches(target, 1, 1, 1);
-       arm926ejs->armv4_5_mmu.mmu_enabled = 0;
-       arm926ejs->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 0;
-       arm926ejs->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;
-
-       target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-       
-       return ERROR_OK;
-}
-
-int arm926ejs_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
-{
-       int retval;
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
-       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;
-       
-       if ((retval = arm7_9_write_memory(target, address, size, count, buffer)) != ERROR_OK)
-               return retval;
-
-       /* If ICache is enabled, we have to invalidate affected ICache lines
-        * the DCache is forced to write-through, so we don't have to clean it here
-        */
-       if (arm926ejs->armv4_5_mmu.armv4_5_cache.i_cache_enabled)
-       {
-               if (count <= 1)
-               {
-                       /* invalidate ICache single entry with MVA */
-                       arm926ejs->write_cp15(target, 0, 1, 7, 5, address);
-               }
-               else
-               {
-                       /* invalidate ICache */
-                       arm926ejs->write_cp15(target, 0, 0, 7, 5, address);
-               }
-       }
-
-       return retval;
-}
-
-int arm926ejs_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
-{
-       arm9tdmi_init_target(cmd_ctx, target);
-               
-       return ERROR_OK;
-       
-}
-
-int arm926ejs_quit()
-{
-       
-       return ERROR_OK;
-}
-
-int arm926ejs_init_arch_info(target_t *target, arm926ejs_common_t *arm926ejs, int chain_pos, char *variant)
-{
-       arm9tdmi_common_t *arm9tdmi = &arm926ejs->arm9tdmi_common;
-       arm7_9_common_t *arm7_9 = &arm9tdmi->arm7_9_common;
-       
-       /* initialize arm9tdmi specific info (including arm7_9 and armv4_5)
-        */
-       arm9tdmi_init_arch_info(target, arm9tdmi, chain_pos, variant);
-
-       arm9tdmi->arch_info = arm926ejs;
-       arm926ejs->common_magic = ARM926EJS_COMMON_MAGIC;
-       
-       arm7_9->post_debug_entry = arm926ejs_post_debug_entry;
-       arm7_9->pre_restore_context = arm926ejs_pre_restore_context;
-       
-       arm926ejs->read_cp15 = arm926ejs_cp15_read;
-       arm926ejs->write_cp15 = arm926ejs_cp15_write;
-       arm926ejs->armv4_5_mmu.armv4_5_cache.ctype = -1;
-       arm926ejs->armv4_5_mmu.get_ttb = arm926ejs_get_ttb;
-       arm926ejs->armv4_5_mmu.read_memory = arm7_9_read_memory;
-       arm926ejs->armv4_5_mmu.write_memory = arm7_9_write_memory;
-       arm926ejs->armv4_5_mmu.disable_mmu_caches = arm926ejs_disable_mmu_caches;
-       arm926ejs->armv4_5_mmu.enable_mmu_caches = arm926ejs_enable_mmu_caches;
-       arm926ejs->armv4_5_mmu.has_tiny_pages = 1;
-       arm926ejs->armv4_5_mmu.mmu_enabled = 0;
-       
-       arm7_9->examine_debug_reason = arm926ejs_examine_debug_reason;
-       
-       /* The ARM926EJ-S implements the ARMv5TE architecture which
-        * has the BKPT instruction, so we don't have to use a watchpoint comparator
-        */
-       arm7_9->arm_bkpt = ARMV5_BKPT(0x0);
-       arm7_9->thumb_bkpt = ARMV5_T_BKPT(0x0) & 0xffff;
-       
-       arm7_9->sw_bkpts_use_wp = 0;
-       arm7_9->sw_bkpts_enabled = 1;
-       
-       return ERROR_OK;
-}
-
-int arm926ejs_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)
-{
-       int chain_pos;
-       char *variant = NULL;
-       arm926ejs_common_t *arm926ejs = malloc(sizeof(arm926ejs_common_t));
-       
-       if (argc < 4)
-       {
-               ERROR("'target arm926ejs' requires at least one additional argument");
-               exit(-1);
-       }
-       
-       chain_pos = strtoul(args[3], NULL, 0);
-       
-       if (argc >= 5)
-               variant = args[4];
-       
-       DEBUG("chain_pos: %i, variant: %s", chain_pos, variant);
-       
-       arm926ejs_init_arch_info(target, arm926ejs, chain_pos, variant);
-
-       return ERROR_OK;
-}
-
-int arm926ejs_register_commands(struct command_context_s *cmd_ctx)
-{
-       int retval;
-       command_t *arm926ejs_cmd;
-       
-               
-       retval = arm9tdmi_register_commands(cmd_ctx);
-       
-       arm926ejs_cmd = register_command(cmd_ctx, NULL, "arm926ejs", NULL, COMMAND_ANY, "arm926ejs specific commands");
-
-       register_command(cmd_ctx, arm926ejs_cmd, "cp15", arm926ejs_handle_cp15_command, COMMAND_EXEC, "display/modify cp15 register <opcode_1> <opcode_2> <CRn> <CRm> [value]");
-       
-       register_command(cmd_ctx, arm926ejs_cmd, "cache_info", arm926ejs_handle_cache_info_command, COMMAND_EXEC, "display information about target caches");
-       register_command(cmd_ctx, arm926ejs_cmd, "virt2phys", arm926ejs_handle_virt2phys_command, COMMAND_EXEC, "translate va to pa <va>");
-
-       register_command(cmd_ctx, arm926ejs_cmd, "mdw_phys", arm926ejs_handle_md_phys_command, COMMAND_EXEC, "display memory words <physical addr> [count]");
-       register_command(cmd_ctx, arm926ejs_cmd, "mdh_phys", arm926ejs_handle_md_phys_command, COMMAND_EXEC, "display memory half-words <physical addr> [count]");
-       register_command(cmd_ctx, arm926ejs_cmd, "mdb_phys", arm926ejs_handle_md_phys_command, COMMAND_EXEC, "display memory bytes <physical addr> [count]");
-
-       register_command(cmd_ctx, arm926ejs_cmd, "mww_phys", arm926ejs_handle_mw_phys_command, COMMAND_EXEC, "write memory word <physical addr> <value>");
-       register_command(cmd_ctx, arm926ejs_cmd, "mwh_phys", arm926ejs_handle_mw_phys_command, COMMAND_EXEC, "write memory half-word <physical addr> <value>");
-       register_command(cmd_ctx, arm926ejs_cmd, "mwb_phys", arm926ejs_handle_mw_phys_command, COMMAND_EXEC, "write memory byte <physical addr> <value>");
-
-       return ERROR_OK;
-}
-
-int arm926ejs_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       int retval;
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm926ejs_common_t *arm926ejs;
-       int opcode_1;
-       int opcode_2;
-       int CRn;
-       int CRm;
-
-       if ((argc < 4) || (argc > 5))
-       {
-               command_print(cmd_ctx, "usage: arm926ejs cp15 <opcode_1> <opcode_2> <CRn> <CRm> [value]");
-               return ERROR_OK;
-       }
-       
-       opcode_1 = strtoul(args[0], NULL, 0);
-       opcode_2 = strtoul(args[1], NULL, 0);
-       CRn = strtoul(args[2], NULL, 0);
-       CRm = strtoul(args[3], NULL, 0);
-
-       if (arm926ejs_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm926ejs) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM926EJ-S target");
-               return ERROR_OK;
-       }
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-       
-       if (argc == 4)
-       {
-               u32 value;
-               if ((retval = arm926ejs->read_cp15(target, opcode_1, opcode_2, CRn, CRm, &value)) != ERROR_OK)
-               {
-                       command_print(cmd_ctx, "couldn't access register");
-                       return ERROR_OK;
-               }
-               jtag_execute_queue();
-               
-               command_print(cmd_ctx, "%i %i %i %i: %8.8x", opcode_1, opcode_2, CRn, CRm, value);
-       }
-       else
-       {
-               u32 value = strtoul(args[4], NULL, 0);
-               if ((retval = arm926ejs->write_cp15(target, opcode_1, opcode_2, CRn, CRm, value)) != ERROR_OK)
-               {
-                       command_print(cmd_ctx, "couldn't access register");
-                       return ERROR_OK;
-               }
-               command_print(cmd_ctx, "%i %i %i %i: %8.8x", opcode_1, opcode_2, CRn, CRm, value);
-       }
-
-       return ERROR_OK;
-}
-
-int arm926ejs_handle_cache_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm926ejs_common_t *arm926ejs;
-       
-       if (arm926ejs_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm926ejs) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM926EJ-S target");
-               return ERROR_OK;
-       }
-       
-       return armv4_5_handle_cache_info_command(cmd_ctx, &arm926ejs->armv4_5_mmu.armv4_5_cache);
-}
-
-int arm926ejs_handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
-{      
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm926ejs_common_t *arm926ejs;
-       arm_jtag_t *jtag_info;
-
-       if (arm926ejs_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm926ejs) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM926EJ-S target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-               
-       return armv4_5_mmu_handle_virt2phys_command(cmd_ctx, cmd, args, argc, target, &arm926ejs->armv4_5_mmu);
-}
-
-int arm926ejs_handle_md_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
-{      
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm926ejs_common_t *arm926ejs;
-       arm_jtag_t *jtag_info;
-
-       if (arm926ejs_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm926ejs) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM926EJ-S target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-       
-       return armv4_5_mmu_handle_md_phys_command(cmd_ctx, cmd, args, argc, target, &arm926ejs->armv4_5_mmu);
-}
-
-int arm926ejs_handle_mw_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
-{      
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm926ejs_common_t *arm926ejs;
-       arm_jtag_t *jtag_info;
-
-       if (arm926ejs_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm926ejs) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM926EJ-S target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-       
-       return armv4_5_mmu_handle_mw_phys_command(cmd_ctx, cmd, args, argc, target, &arm926ejs->armv4_5_mmu);
-}
-static int arm926ejs_virt2phys(struct target_s *target, u32 virtual, u32 *physical)
-{
-       int retval;
-       int type;
-       u32 cb;
-       int domain;
-       u32 ap;
-       
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm926ejs_common_t *arm926ejs;
-       retval= arm926ejs_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm926ejs);
-       if (retval != ERROR_OK)
-       {
-               return retval;
-       }
-       u32 ret = armv4_5_mmu_translate_va(target, &arm926ejs->armv4_5_mmu, virtual, &type, &cb, &domain, &ap);
-       if (type == -1)
-       {
-               return ret;
-       }
-       *physical = ret;
-       return ERROR_OK;
-}
-
-static int arm926ejs_mmu(struct target_s *target, int *enabled)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm926ejs_common_t *arm926ejs = armv4_5->arch_info;
-       
-       if (target->state != TARGET_HALTED)
-       {
-               ERROR("Target not halted");
-               return ERROR_TARGET_INVALID;
-       }
-       *enabled = arm926ejs->armv4_5_mmu.mmu_enabled;
-       return ERROR_OK;
-}
+/***************************************************************************\r
+ *   Copyright (C) 2007 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "arm926ejs.h"\r
+#include "jtag.h"\r
+#include "log.h"\r
+\r
+#include <stdlib.h>\r
+#include <string.h>\r
+\r
+#if 1\r
+#define _DEBUG_INSTRUCTION_EXECUTION_\r
+#endif\r
+\r
+/* cli handling */\r
+int arm926ejs_register_commands(struct command_context_s *cmd_ctx);\r
+\r
+int arm926ejs_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm926ejs_handle_cp15i_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm926ejs_handle_virt2phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm926ejs_handle_cache_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm926ejs_handle_md_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm926ejs_handle_mw_phys_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+int arm926ejs_handle_read_cache_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int arm926ejs_handle_read_mmu_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+/* forward declarations */\r
+int arm926ejs_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);\r
+int arm926ejs_init_target(struct command_context_s *cmd_ctx, struct target_s *target);\r
+int arm926ejs_quit();\r
+int arm926ejs_arch_state(struct target_s *target);\r
+int arm926ejs_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);\r
+int arm926ejs_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);\r
+int arm926ejs_soft_reset_halt(struct target_s *target);\r
+static int arm926ejs_virt2phys(struct target_s *target, u32 virtual, u32 *physical);\r
+static int arm926ejs_mmu(struct target_s *target, int *enabled);\r
+\r
+target_type_t arm926ejs_target =\r
+{\r
+       .name = "arm926ejs",\r
+\r
+       .poll = arm7_9_poll,\r
+       .arch_state = arm926ejs_arch_state,\r
+\r
+       .target_request_data = arm7_9_target_request_data,\r
+\r
+       .halt = arm7_9_halt,\r
+       .resume = arm7_9_resume,\r
+       .step = arm7_9_step,\r
+\r
+       .assert_reset = arm7_9_assert_reset,\r
+       .deassert_reset = arm7_9_deassert_reset,\r
+       .soft_reset_halt = arm926ejs_soft_reset_halt,\r
+       .prepare_reset_halt = arm7_9_prepare_reset_halt,\r
+       \r
+       .get_gdb_reg_list = armv4_5_get_gdb_reg_list,\r
+\r
+       .read_memory = arm7_9_read_memory,\r
+       .write_memory = arm926ejs_write_memory,\r
+       .bulk_write_memory = arm7_9_bulk_write_memory,\r
+       .checksum_memory = arm7_9_checksum_memory,\r
+       \r
+       .run_algorithm = armv4_5_run_algorithm,\r
+\r
+       .add_breakpoint = arm7_9_add_breakpoint,\r
+       .remove_breakpoint = arm7_9_remove_breakpoint,\r
+       .add_watchpoint = arm7_9_add_watchpoint,\r
+       .remove_watchpoint = arm7_9_remove_watchpoint,\r
+\r
+       .register_commands = arm926ejs_register_commands,\r
+       .target_command = arm926ejs_target_command,\r
+       .init_target = arm926ejs_init_target,\r
+       .quit = arm926ejs_quit,\r
+       .virt2phys = arm926ejs_virt2phys,\r
+       .mmu = arm926ejs_mmu\r
+};\r
+\r
+\r
+int arm926ejs_catch_broken_irscan(u8 *captured, void *priv, scan_field_t *field)\r
+{\r
+       /* The ARM926EJ-S' instruction register is 4 bits wide */\r
+       u8 t = *captured & 0xf;\r
+       u8 t2 = *field->in_check_value & 0xf;\r
+       if (t == t2)\r
+       {\r
+               return ERROR_OK;\r
+       }\r
+       else if ((t == 0x0f) || (t == 0x00))\r
+       {\r
+               DEBUG("caught ARM926EJ-S invalid Capture-IR result after CP15 access");\r
+               return ERROR_OK;\r
+       }\r
+       return ERROR_JTAG_QUEUE_FAILED;;\r
+}\r
+\r
+#define ARM926EJS_CP15_ADDR(opcode_1, opcode_2, CRn, CRm) ((opcode_1 << 11) | (opcode_2 << 8) | (CRn << 4) | (CRm << 0))\r
+\r
+int arm926ejs_cp15_read(target_t *target, u32 op1, u32 op2, u32 CRn, u32 CRm, u32 *value)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       u32 address = ARM926EJS_CP15_ADDR(op1, op2, CRn, CRm);\r
+       scan_field_t fields[4];\r
+       u8 address_buf[2];\r
+       u8 nr_w_buf = 0;\r
+       u8 access = 1;\r
+       \r
+       buf_set_u32(address_buf, 0, 14, address);\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_scann(jtag_info, 0xf);\r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+\r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = NULL;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+\r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 1;\r
+       fields[1].out_value = &access;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = &access;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = jtag_info->chain_pos;\r
+       fields[2].num_bits = 14;\r
+       fields[2].out_value = address_buf;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+\r
+       fields[3].device = jtag_info->chain_pos;\r
+       fields[3].num_bits = 1;\r
+       fields[3].out_value = &nr_w_buf;\r
+       fields[3].out_mask = NULL;\r
+       fields[3].in_value = NULL;\r
+       fields[3].in_check_value = NULL;\r
+       fields[3].in_check_mask = NULL;\r
+       fields[3].in_handler = NULL;\r
+       fields[3].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(4, fields, -1);\r
+\r
+       fields[0].in_handler_priv = value;\r
+       fields[0].in_handler = arm_jtag_buf_to_u32;\r
+       \r
+       do\r
+       {\r
+               /* rescan with NOP, to wait for the access to complete */\r
+               access = 0;\r
+               nr_w_buf = 0;\r
+               jtag_add_dr_scan(4, fields, -1);\r
+               jtag_execute_queue();\r
+       } while (buf_get_u32(&access, 0, 1) != 1);\r
+\r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+       DEBUG("addr: 0x%x value: %8.8x", address, *value);\r
+#endif\r
+       \r
+       arm_jtag_set_instr(jtag_info, 0xc, &arm926ejs_catch_broken_irscan);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm926ejs_cp15_write(target_t *target, u32 op1, u32 op2, u32 CRn, u32 CRm, u32 value)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       u32 address = ARM926EJS_CP15_ADDR(op1, op2, CRn, CRm);\r
+       scan_field_t fields[4];\r
+       u8 value_buf[4];\r
+       u8 address_buf[2];\r
+       u8 nr_w_buf = 1;\r
+       u8 access = 1;\r
+       \r
+       buf_set_u32(address_buf, 0, 14, address);\r
+       buf_set_u32(value_buf, 0, 32, value);\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_scann(jtag_info, 0xf);\r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+\r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = value_buf;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+\r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 1;\r
+       fields[1].out_value = &access;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = &access;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = jtag_info->chain_pos;\r
+       fields[2].num_bits = 14;\r
+       fields[2].out_value = address_buf;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+\r
+       fields[3].device = jtag_info->chain_pos;\r
+       fields[3].num_bits = 1;\r
+       fields[3].out_value = &nr_w_buf;\r
+       fields[3].out_mask = NULL;\r
+       fields[3].in_value = NULL;\r
+       fields[3].in_check_value = NULL;\r
+       fields[3].in_check_mask = NULL;\r
+       fields[3].in_handler = NULL;\r
+       fields[3].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(4, fields, -1);\r
+\r
+       do\r
+       {\r
+               /* rescan with NOP, to wait for the access to complete */\r
+               access = 0;\r
+               nr_w_buf = 0;\r
+               jtag_add_dr_scan(4, fields, -1);\r
+               jtag_execute_queue();\r
+       } while (buf_get_u32(&access, 0, 1) != 1);\r
+\r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+       DEBUG("addr: 0x%x value: %8.8x", address, value);\r
+#endif\r
+\r
+       arm_jtag_set_instr(jtag_info, 0xf, &arm926ejs_catch_broken_irscan);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm926ejs_examine_debug_reason(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];\r
+       int debug_reason;\r
+       int retval;\r
+\r
+       embeddedice_read_reg(dbg_stat);\r
+       if ((retval = jtag_execute_queue()) != ERROR_OK)\r
+               return retval;\r
+       \r
+       debug_reason = buf_get_u32(dbg_stat->value, 6, 4);\r
+       \r
+       switch (debug_reason)\r
+       {\r
+               case 1:\r
+                       DEBUG("breakpoint from EICE unit 0");\r
+                       target->debug_reason = DBG_REASON_BREAKPOINT;\r
+                       break;\r
+               case 2:\r
+                       DEBUG("breakpoint from EICE unit 1");\r
+                       target->debug_reason = DBG_REASON_BREAKPOINT;\r
+                       break;\r
+               case 3:\r
+                       DEBUG("soft breakpoint (BKPT instruction)");\r
+                       target->debug_reason = DBG_REASON_BREAKPOINT;\r
+                       break;\r
+               case 4:\r
+                       DEBUG("vector catch breakpoint");\r
+                       target->debug_reason = DBG_REASON_BREAKPOINT;\r
+                       break;\r
+               case 5:\r
+                       DEBUG("external breakpoint");\r
+                       target->debug_reason = DBG_REASON_BREAKPOINT;\r
+                       break;\r
+               case 6:\r
+                       DEBUG("watchpoint from EICE unit 0");\r
+                       target->debug_reason = DBG_REASON_WATCHPOINT;\r
+                       break;\r
+               case 7:\r
+                       DEBUG("watchpoint from EICE unit 1");\r
+                       target->debug_reason = DBG_REASON_WATCHPOINT;\r
+                       break;\r
+               case 8:\r
+                       DEBUG("external watchpoint");\r
+                       target->debug_reason = DBG_REASON_WATCHPOINT;\r
+                       break;\r
+               case 9:\r
+                       DEBUG("internal debug request");\r
+                       target->debug_reason = DBG_REASON_DBGRQ;\r
+                       break;\r
+               case 10:\r
+                       DEBUG("external debug request");\r
+                       target->debug_reason = DBG_REASON_DBGRQ;\r
+                       break;\r
+               case 11:\r
+                       ERROR("BUG: debug re-entry from system speed access shouldn't be handled here");\r
+                       break;\r
+               default:\r
+                       ERROR("BUG: unknown debug reason: 0x%x", debug_reason);\r
+                       target->debug_reason = DBG_REASON_DBGRQ;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+u32 arm926ejs_get_ttb(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;\r
+       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;\r
+       int retval;\r
+       u32 ttb = 0x0;\r
+\r
+       if ((retval = arm926ejs->read_cp15(target, 0, 0, 2, 0, &ttb)) != ERROR_OK)\r
+               return retval;\r
+\r
+       return ttb;\r
+}\r
+\r
+void arm926ejs_disable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;\r
+       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;\r
+       u32 cp15_control;\r
+\r
+       /* read cp15 control register */\r
+       arm926ejs->read_cp15(target, 0, 0, 1, 0, &cp15_control);\r
+       jtag_execute_queue();\r
+       \r
+       if (mmu)\r
+       {\r
+               /* invalidate TLB */\r
+               arm926ejs->write_cp15(target, 0, 0, 8, 7, 0x0);\r
+               \r
+               cp15_control &= ~0x1U;\r
+       }\r
+       \r
+       if (d_u_cache)\r
+       {\r
+               u32 debug_override;\r
+               /* read-modify-write CP15 debug override register \r
+                * to enable "test and clean all" */\r
+               arm926ejs->read_cp15(target, 0, 0, 15, 0, &debug_override);\r
+               debug_override |= 0x80000;\r
+               arm926ejs->write_cp15(target, 0, 0, 15, 0, debug_override);\r
+               \r
+               /* clean and invalidate DCache */\r
+               arm926ejs->write_cp15(target, 0, 0, 7, 5, 0x0);\r
+\r
+               /* write CP15 debug override register \r
+                * to disable "test and clean all" */\r
+               debug_override &= ~0x80000;\r
+               arm926ejs->write_cp15(target, 0, 0, 15, 0, debug_override);\r
+               \r
+               cp15_control &= ~0x4U;\r
+       }\r
+       \r
+       if (i_cache)\r
+       {\r
+               /* invalidate ICache */\r
+               arm926ejs->write_cp15(target, 0, 0, 7, 5, 0x0);\r
+               \r
+               cp15_control &= ~0x1000U;\r
+       }\r
+       \r
+       arm926ejs->write_cp15(target, 0, 0, 1, 0, cp15_control);\r
+}\r
+\r
+void arm926ejs_enable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;\r
+       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;\r
+       u32 cp15_control;\r
+\r
+       /* read cp15 control register */\r
+       arm926ejs->read_cp15(target, 0, 0, 1, 0, &cp15_control);\r
+       jtag_execute_queue();\r
+               \r
+       if (mmu)\r
+               cp15_control |= 0x1U;\r
+       \r
+       if (d_u_cache)\r
+               cp15_control |= 0x4U;\r
+       \r
+       if (i_cache)\r
+               cp15_control |= 0x1000U;\r
+       \r
+       arm926ejs->write_cp15(target, 0, 0, 1, 0, cp15_control);\r
+}\r
+\r
+void arm926ejs_post_debug_entry(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;\r
+       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;\r
+\r
+       /* examine cp15 control reg */\r
+       arm926ejs->read_cp15(target, 0, 0, 1, 0, &arm926ejs->cp15_control_reg);\r
+       jtag_execute_queue();\r
+       DEBUG("cp15_control_reg: %8.8x", arm926ejs->cp15_control_reg);\r
+\r
+       if (arm926ejs->armv4_5_mmu.armv4_5_cache.ctype == -1)\r
+       {\r
+               u32 cache_type_reg;\r
+               /* identify caches */\r
+               arm926ejs->read_cp15(target, 0, 1, 0, 0, &cache_type_reg);\r
+               jtag_execute_queue();\r
+               armv4_5_identify_cache(cache_type_reg, &arm926ejs->armv4_5_mmu.armv4_5_cache);\r
+       }\r
+\r
+       arm926ejs->armv4_5_mmu.mmu_enabled = (arm926ejs->cp15_control_reg & 0x1U) ? 1 : 0;\r
+       arm926ejs->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = (arm926ejs->cp15_control_reg & 0x4U) ? 1 : 0;\r
+       arm926ejs->armv4_5_mmu.armv4_5_cache.i_cache_enabled = (arm926ejs->cp15_control_reg & 0x1000U) ? 1 : 0;\r
+\r
+       /* save i/d fault status and address register */\r
+       arm926ejs->read_cp15(target, 0, 0, 5, 0, &arm926ejs->d_fsr);\r
+       arm926ejs->read_cp15(target, 0, 1, 5, 0, &arm926ejs->i_fsr);\r
+       arm926ejs->read_cp15(target, 0, 0, 6, 0, &arm926ejs->d_far);\r
+       \r
+       DEBUG("D FSR: 0x%8.8x, D FAR: 0x%8.8x, I FSR: 0x%8.8x",\r
+               arm926ejs->d_fsr, arm926ejs->d_far, arm926ejs->i_fsr);  \r
+\r
+\r
+       u32 cache_dbg_ctrl;\r
+       \r
+       /* read-modify-write CP15 cache debug control register \r
+        * to disable I/D-cache linefills and force WT */\r
+       arm926ejs->read_cp15(target, 7, 0, 15, 0, &cache_dbg_ctrl);\r
+       cache_dbg_ctrl |= 0x7;\r
+       arm926ejs->write_cp15(target, 7, 0, 15, 0, cache_dbg_ctrl);\r
+}\r
+\r
+void arm926ejs_pre_restore_context(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;\r
+       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;\r
+\r
+       /* restore i/d fault status and address register */\r
+       arm926ejs->write_cp15(target, 0, 0, 5, 0, arm926ejs->d_fsr);\r
+       arm926ejs->write_cp15(target, 0, 1, 5, 0, arm926ejs->i_fsr);\r
+       arm926ejs->write_cp15(target, 0, 0, 6, 0, arm926ejs->d_far);\r
+       \r
+       u32 cache_dbg_ctrl;\r
+       \r
+       /* read-modify-write CP15 cache debug control register \r
+        * to reenable I/D-cache linefills and disable WT */\r
+       arm926ejs->read_cp15(target, 7, 0, 15, 0, &cache_dbg_ctrl);\r
+       cache_dbg_ctrl &= ~0x7;\r
+       arm926ejs->write_cp15(target, 7, 0, 15, 0, cache_dbg_ctrl);\r
+}\r
+\r
+int arm926ejs_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p, arm9tdmi_common_t **arm9tdmi_p, arm926ejs_common_t **arm926ejs_p)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm926ejs_common_t *arm926ejs;\r
+       \r
+       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       arm7_9 = armv4_5->arch_info;\r
+       if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       arm9tdmi = arm7_9->arch_info;\r
+       if (arm9tdmi->common_magic != ARM9TDMI_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       arm926ejs = arm9tdmi->arch_info;\r
+       if (arm926ejs->common_magic != ARM926EJS_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       *armv4_5_p = armv4_5;\r
+       *arm7_9_p = arm7_9;\r
+       *arm9tdmi_p = arm9tdmi;\r
+       *arm926ejs_p = arm926ejs;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm926ejs_arch_state(struct target_s *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;\r
+       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;\r
+       \r
+       char *state[] = \r
+       {\r
+               "disabled", "enabled"\r
+       };\r
+       \r
+       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)\r
+       {\r
+               ERROR("BUG: called for a non-ARMv4/5 target");\r
+               exit(-1);\r
+       }\r
+       \r
+       USER(\r
+                       "target halted in %s state due to %s, current mode: %s\n"\r
+                       "cpsr: 0x%8.8x pc: 0x%8.8x\n"\r
+                       "MMU: %s, D-Cache: %s, I-Cache: %s",\r
+                        armv4_5_state_strings[armv4_5->core_state],\r
+                        target_debug_reason_strings[target->debug_reason],\r
+                        armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)],\r
+                        buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),\r
+                        buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32),\r
+                        state[arm926ejs->armv4_5_mmu.mmu_enabled],\r
+                        state[arm926ejs->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled], \r
+                        state[arm926ejs->armv4_5_mmu.armv4_5_cache.i_cache_enabled]);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm926ejs_soft_reset_halt(struct target_s *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;\r
+       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;\r
+       reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];\r
+       \r
+       if (target->state == TARGET_RUNNING)\r
+       {\r
+               target->type->halt(target);\r
+       }\r
+       \r
+       while (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) == 0)\r
+       {\r
+               embeddedice_read_reg(dbg_stat);\r
+               jtag_execute_queue();\r
+       }\r
+       \r
+       target->state = TARGET_HALTED;\r
+       \r
+       /* SVC, ARM state, IRQ and FIQ disabled */\r
+       buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 8, 0xd3);\r
+       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;\r
+       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;\r
+       \r
+       /* start fetching from 0x0 */\r
+       buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, 0x0);\r
+       armv4_5->core_cache->reg_list[15].dirty = 1;\r
+       armv4_5->core_cache->reg_list[15].valid = 1;\r
+       \r
+       armv4_5->core_mode = ARMV4_5_MODE_SVC;\r
+       armv4_5->core_state = ARMV4_5_STATE_ARM;\r
+       \r
+       arm926ejs_disable_mmu_caches(target, 1, 1, 1);\r
+       arm926ejs->armv4_5_mmu.mmu_enabled = 0;\r
+       arm926ejs->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 0;\r
+       arm926ejs->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;\r
+\r
+       target_call_event_callbacks(target, TARGET_EVENT_HALTED);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm926ejs_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)\r
+{\r
+       int retval;\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;\r
+       arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;\r
+       \r
+       if ((retval = arm7_9_write_memory(target, address, size, count, buffer)) != ERROR_OK)\r
+               return retval;\r
+\r
+       /* If ICache is enabled, we have to invalidate affected ICache lines\r
+        * the DCache is forced to write-through, so we don't have to clean it here\r
+        */\r
+       if (arm926ejs->armv4_5_mmu.armv4_5_cache.i_cache_enabled)\r
+       {\r
+               if (count <= 1)\r
+               {\r
+                       /* invalidate ICache single entry with MVA */\r
+                       arm926ejs->write_cp15(target, 0, 1, 7, 5, address);\r
+               }\r
+               else\r
+               {\r
+                       /* invalidate ICache */\r
+                       arm926ejs->write_cp15(target, 0, 0, 7, 5, address);\r
+               }\r
+       }\r
+\r
+       return retval;\r
+}\r
+\r
+int arm926ejs_init_target(struct command_context_s *cmd_ctx, struct target_s *target)\r
+{\r
+       arm9tdmi_init_target(cmd_ctx, target);\r
+               \r
+       return ERROR_OK;\r
+       \r
+}\r
+\r
+int arm926ejs_quit()\r
+{\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm926ejs_init_arch_info(target_t *target, arm926ejs_common_t *arm926ejs, int chain_pos, char *variant)\r
+{\r
+       arm9tdmi_common_t *arm9tdmi = &arm926ejs->arm9tdmi_common;\r
+       arm7_9_common_t *arm7_9 = &arm9tdmi->arm7_9_common;\r
+       \r
+       /* initialize arm9tdmi specific info (including arm7_9 and armv4_5)\r
+        */\r
+       arm9tdmi_init_arch_info(target, arm9tdmi, chain_pos, variant);\r
+\r
+       arm9tdmi->arch_info = arm926ejs;\r
+       arm926ejs->common_magic = ARM926EJS_COMMON_MAGIC;\r
+       \r
+       arm7_9->post_debug_entry = arm926ejs_post_debug_entry;\r
+       arm7_9->pre_restore_context = arm926ejs_pre_restore_context;\r
+       \r
+       arm926ejs->read_cp15 = arm926ejs_cp15_read;\r
+       arm926ejs->write_cp15 = arm926ejs_cp15_write;\r
+       arm926ejs->armv4_5_mmu.armv4_5_cache.ctype = -1;\r
+       arm926ejs->armv4_5_mmu.get_ttb = arm926ejs_get_ttb;\r
+       arm926ejs->armv4_5_mmu.read_memory = arm7_9_read_memory;\r
+       arm926ejs->armv4_5_mmu.write_memory = arm7_9_write_memory;\r
+       arm926ejs->armv4_5_mmu.disable_mmu_caches = arm926ejs_disable_mmu_caches;\r
+       arm926ejs->armv4_5_mmu.enable_mmu_caches = arm926ejs_enable_mmu_caches;\r
+       arm926ejs->armv4_5_mmu.has_tiny_pages = 1;\r
+       arm926ejs->armv4_5_mmu.mmu_enabled = 0;\r
+       \r
+       arm7_9->examine_debug_reason = arm926ejs_examine_debug_reason;\r
+       \r
+       /* The ARM926EJ-S implements the ARMv5TE architecture which\r
+        * has the BKPT instruction, so we don't have to use a watchpoint comparator\r
+        */\r
+       arm7_9->arm_bkpt = ARMV5_BKPT(0x0);\r
+       arm7_9->thumb_bkpt = ARMV5_T_BKPT(0x0) & 0xffff;\r
+       \r
+       arm7_9->sw_bkpts_use_wp = 0;\r
+       arm7_9->sw_bkpts_enabled = 1;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm926ejs_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)\r
+{\r
+       int chain_pos;\r
+       char *variant = NULL;\r
+       arm926ejs_common_t *arm926ejs = malloc(sizeof(arm926ejs_common_t));\r
+       \r
+       if (argc < 4)\r
+       {\r
+               ERROR("'target arm926ejs' requires at least one additional argument");\r
+               exit(-1);\r
+       }\r
+       \r
+       chain_pos = strtoul(args[3], NULL, 0);\r
+       \r
+       if (argc >= 5)\r
+               variant = args[4];\r
+       \r
+       DEBUG("chain_pos: %i, variant: %s", chain_pos, variant);\r
+       \r
+       arm926ejs_init_arch_info(target, arm926ejs, chain_pos, variant);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm926ejs_register_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       int retval;\r
+       command_t *arm926ejs_cmd;\r
+       \r
+               \r
+       retval = arm9tdmi_register_commands(cmd_ctx);\r
+       \r
+       arm926ejs_cmd = register_command(cmd_ctx, NULL, "arm926ejs", NULL, COMMAND_ANY, "arm926ejs specific commands");\r
+\r
+       register_command(cmd_ctx, arm926ejs_cmd, "cp15", arm926ejs_handle_cp15_command, COMMAND_EXEC, "display/modify cp15 register <opcode_1> <opcode_2> <CRn> <CRm> [value]");\r
+       \r
+       register_command(cmd_ctx, arm926ejs_cmd, "cache_info", arm926ejs_handle_cache_info_command, COMMAND_EXEC, "display information about target caches");\r
+       register_command(cmd_ctx, arm926ejs_cmd, "virt2phys", arm926ejs_handle_virt2phys_command, COMMAND_EXEC, "translate va to pa <va>");\r
+\r
+       register_command(cmd_ctx, arm926ejs_cmd, "mdw_phys", arm926ejs_handle_md_phys_command, COMMAND_EXEC, "display memory words <physical addr> [count]");\r
+       register_command(cmd_ctx, arm926ejs_cmd, "mdh_phys", arm926ejs_handle_md_phys_command, COMMAND_EXEC, "display memory half-words <physical addr> [count]");\r
+       register_command(cmd_ctx, arm926ejs_cmd, "mdb_phys", arm926ejs_handle_md_phys_command, COMMAND_EXEC, "display memory bytes <physical addr> [count]");\r
+\r
+       register_command(cmd_ctx, arm926ejs_cmd, "mww_phys", arm926ejs_handle_mw_phys_command, COMMAND_EXEC, "write memory word <physical addr> <value>");\r
+       register_command(cmd_ctx, arm926ejs_cmd, "mwh_phys", arm926ejs_handle_mw_phys_command, COMMAND_EXEC, "write memory half-word <physical addr> <value>");\r
+       register_command(cmd_ctx, arm926ejs_cmd, "mwb_phys", arm926ejs_handle_mw_phys_command, COMMAND_EXEC, "write memory byte <physical addr> <value>");\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm926ejs_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       int retval;\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm926ejs_common_t *arm926ejs;\r
+       int opcode_1;\r
+       int opcode_2;\r
+       int CRn;\r
+       int CRm;\r
+\r
+       if ((argc < 4) || (argc > 5))\r
+       {\r
+               command_print(cmd_ctx, "usage: arm926ejs cp15 <opcode_1> <opcode_2> <CRn> <CRm> [value]");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       opcode_1 = strtoul(args[0], NULL, 0);\r
+       opcode_2 = strtoul(args[1], NULL, 0);\r
+       CRn = strtoul(args[2], NULL, 0);\r
+       CRm = strtoul(args[3], NULL, 0);\r
+\r
+       if (arm926ejs_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm926ejs) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM926EJ-S target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (argc == 4)\r
+       {\r
+               u32 value;\r
+               if ((retval = arm926ejs->read_cp15(target, opcode_1, opcode_2, CRn, CRm, &value)) != ERROR_OK)\r
+               {\r
+                       command_print(cmd_ctx, "couldn't access register");\r
+                       return ERROR_OK;\r
+               }\r
+               jtag_execute_queue();\r
+               \r
+               command_print(cmd_ctx, "%i %i %i %i: %8.8x", opcode_1, opcode_2, CRn, CRm, value);\r
+       }\r
+       else\r
+       {\r
+               u32 value = strtoul(args[4], NULL, 0);\r
+               if ((retval = arm926ejs->write_cp15(target, opcode_1, opcode_2, CRn, CRm, value)) != ERROR_OK)\r
+               {\r
+                       command_print(cmd_ctx, "couldn't access register");\r
+                       return ERROR_OK;\r
+               }\r
+               command_print(cmd_ctx, "%i %i %i %i: %8.8x", opcode_1, opcode_2, CRn, CRm, value);\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm926ejs_handle_cache_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm926ejs_common_t *arm926ejs;\r
+       \r
+       if (arm926ejs_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm926ejs) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM926EJ-S target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       return armv4_5_handle_cache_info_command(cmd_ctx, &arm926ejs->armv4_5_mmu.armv4_5_cache);\r
+}\r
+\r
+int arm926ejs_handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)\r
+{      \r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm926ejs_common_t *arm926ejs;\r
+       arm_jtag_t *jtag_info;\r
+\r
+       if (arm926ejs_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm926ejs) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM926EJ-S target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+               \r
+       return armv4_5_mmu_handle_virt2phys_command(cmd_ctx, cmd, args, argc, target, &arm926ejs->armv4_5_mmu);\r
+}\r
+\r
+int arm926ejs_handle_md_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)\r
+{      \r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm926ejs_common_t *arm926ejs;\r
+       arm_jtag_t *jtag_info;\r
+\r
+       if (arm926ejs_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm926ejs) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM926EJ-S target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       return armv4_5_mmu_handle_md_phys_command(cmd_ctx, cmd, args, argc, target, &arm926ejs->armv4_5_mmu);\r
+}\r
+\r
+int arm926ejs_handle_mw_phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)\r
+{      \r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm926ejs_common_t *arm926ejs;\r
+       arm_jtag_t *jtag_info;\r
+\r
+       if (arm926ejs_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm926ejs) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM926EJ-S target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       return armv4_5_mmu_handle_mw_phys_command(cmd_ctx, cmd, args, argc, target, &arm926ejs->armv4_5_mmu);\r
+}\r
+static int arm926ejs_virt2phys(struct target_s *target, u32 virtual, u32 *physical)\r
+{\r
+       int retval;\r
+       int type;\r
+       u32 cb;\r
+       int domain;\r
+       u32 ap;\r
+       \r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm926ejs_common_t *arm926ejs;\r
+       retval= arm926ejs_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm926ejs);\r
+       if (retval != ERROR_OK)\r
+       {\r
+               return retval;\r
+       }\r
+       u32 ret = armv4_5_mmu_translate_va(target, &arm926ejs->armv4_5_mmu, virtual, &type, &cb, &domain, &ap);\r
+       if (type == -1)\r
+       {\r
+               return ret;\r
+       }\r
+       *physical = ret;\r
+       return ERROR_OK;\r
+}\r
+\r
+static int arm926ejs_mmu(struct target_s *target, int *enabled)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm926ejs_common_t *arm926ejs = armv4_5->arch_info;\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               ERROR("Target not halted");\r
+               return ERROR_TARGET_INVALID;\r
+       }\r
+       *enabled = arm926ejs->armv4_5_mmu.mmu_enabled;\r
+       return ERROR_OK;\r
+}\r
index 2885e3dfb5d549b9b3303f3ffa2936dd3ba093fc..1ea2ce77cc97ecca94bc6a2939ac228856f3391c 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2005 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "arm966e.h"
-
-#include "arm7_9_common.h"
-#include "register.h"
-#include "target.h"
-#include "armv4_5.h"
-#include "embeddedice.h"
-#include "log.h"
-#include "jtag.h"
-#include "arm_jtag.h"
-
-#include <stdlib.h>
-#include <string.h>
-
-#if 0
-#define _DEBUG_INSTRUCTION_EXECUTION_
-#endif
-
-/* cli handling */
-int arm966e_register_commands(struct command_context_s *cmd_ctx);
-
-/* forward declarations */
-int arm966e_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);
-int arm966e_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
-int arm966e_quit(void);
-
-target_type_t arm966e_target =
-{
-       .name = "arm966e",
-
-       .poll = arm7_9_poll,
-       .arch_state = armv4_5_arch_state,
-
-       .target_request_data = arm7_9_target_request_data,
-
-       .halt = arm7_9_halt,
-       .resume = arm7_9_resume,
-       .step = arm7_9_step,
-
-       .assert_reset = arm7_9_assert_reset,
-       .deassert_reset = arm7_9_deassert_reset,
-       .soft_reset_halt = arm7_9_soft_reset_halt,
-       .prepare_reset_halt = arm7_9_prepare_reset_halt,
-
-       .get_gdb_reg_list = armv4_5_get_gdb_reg_list,
-
-       .read_memory = arm7_9_read_memory,
-       .write_memory = arm7_9_write_memory,
-       .bulk_write_memory = arm7_9_bulk_write_memory,
-       .checksum_memory = arm7_9_checksum_memory,
-       
-       .run_algorithm = armv4_5_run_algorithm,
-       
-       .add_breakpoint = arm7_9_add_breakpoint,
-       .remove_breakpoint = arm7_9_remove_breakpoint,
-       .add_watchpoint = arm7_9_add_watchpoint,
-       .remove_watchpoint = arm7_9_remove_watchpoint,
-
-       .register_commands = arm966e_register_commands,
-       .target_command = arm966e_target_command,
-       .init_target = arm966e_init_target,
-       .quit = arm966e_quit,
-};
-
-int arm966e_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
-{
-       arm9tdmi_init_target(cmd_ctx, target);
-               
-       return ERROR_OK;
-}
-
-int arm966e_quit(void)
-{
-       
-       return ERROR_OK;
-}
-
-int arm966e_init_arch_info(target_t *target, arm966e_common_t *arm966e, int chain_pos, char *variant)
-{
-       arm9tdmi_common_t *arm9tdmi = &arm966e->arm9tdmi_common;
-       arm7_9_common_t *arm7_9 = &arm9tdmi->arm7_9_common;
-       
-       arm9tdmi_init_arch_info(target, arm9tdmi, chain_pos, variant);
-
-       arm9tdmi->arch_info = arm966e;
-       arm966e->common_magic = ARM966E_COMMON_MAGIC;
-       
-       /* The ARM966E-S implements the ARMv5TE architecture which
-        * has the BKPT instruction, so we don't have to use a watchpoint comparator
-        */
-       arm7_9->arm_bkpt = ARMV5_BKPT(0x0);
-       arm7_9->thumb_bkpt = ARMV5_T_BKPT(0x0) & 0xffff;
-       
-       arm7_9->sw_bkpts_use_wp = 0;
-       arm7_9->sw_bkpts_enabled = 1;
-       
-       return ERROR_OK;
-}
-
-int arm966e_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)
-{
-       int chain_pos;
-       char *variant = NULL;
-       arm966e_common_t *arm966e = malloc(sizeof(arm966e_common_t));
-       
-       if (argc < 4)
-       {
-               ERROR("'target arm966e' requires at least one additional argument");
-               exit(-1);
-       }
-       
-       chain_pos = strtoul(args[3], NULL, 0);
-       
-       if (argc >= 5)
-               variant = args[4];
-       
-       DEBUG("chain_pos: %i, variant: %s", chain_pos, variant);
-       
-       arm966e_init_arch_info(target, arm966e, chain_pos, variant);
-
-       return ERROR_OK;
-}
-
-int arm966e_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p, arm9tdmi_common_t **arm9tdmi_p, arm966e_common_t **arm966e_p)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm966e_common_t *arm966e;
-       
-       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       arm7_9 = armv4_5->arch_info;
-       if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       arm9tdmi = arm7_9->arch_info;
-       if (arm9tdmi->common_magic != ARM9TDMI_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       arm966e = arm9tdmi->arch_info;
-       if (arm966e->common_magic != ARM966E_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       *armv4_5_p = armv4_5;
-       *arm7_9_p = arm7_9;
-       *arm9tdmi_p = arm9tdmi;
-       *arm966e_p = arm966e;
-       
-       return ERROR_OK;
-}
-
-int arm966e_read_cp15(target_t *target, int reg_addr, u32 *value)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       scan_field_t fields[3];
-       u8 reg_addr_buf = reg_addr & 0x3f;
-       u8 nr_w_buf = 0;
-       
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_scann(jtag_info, 0xf);
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = NULL;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 6;
-       fields[1].out_value = &reg_addr_buf;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = jtag_info->chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = &nr_w_buf;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(3, fields, -1, NULL);
-
-       fields[0].in_handler_priv = value;
-       fields[0].in_handler = arm_jtag_buf_to_u32;
-
-       jtag_add_dr_scan(3, fields, -1, NULL);
-
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-       jtag_execute_queue();
-       DEBUG("addr: 0x%x value: %8.8x", reg_addr, *value);
-#endif
-
-       return ERROR_OK;
-}
-
-int arm966e_write_cp15(target_t *target, int reg_addr, u32 value)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       scan_field_t fields[3];
-       u8 reg_addr_buf = reg_addr & 0x3f;
-       u8 nr_w_buf = 1;
-       u8 value_buf[4];
-       
-       buf_set_u32(value_buf, 0, 32, value);
-       
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_scann(jtag_info, 0xf);
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = value_buf;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 6;
-       fields[1].out_value = &reg_addr_buf;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = jtag_info->chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = &nr_w_buf;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(3, fields, -1, NULL);
-
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-       DEBUG("addr: 0x%x value: %8.8x", reg_addr, value);
-#endif
-
-       return ERROR_OK;
-}
-
-int arm966e_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       int retval;
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       arm966e_common_t *arm966e;
-       arm_jtag_t *jtag_info;
-
-       if (arm966e_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm966e) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM966e target");
-               return ERROR_OK;
-       }
-       
-       jtag_info = &arm7_9->jtag_info;
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-
-       /* one or more argument, access a single register (write if second argument is given */
-       if (argc >= 1)
-       {
-               int address = strtoul(args[0], NULL, 0);
-
-               if (argc == 1)
-               {
-                       u32 value;
-                       if ((retval = arm966e_read_cp15(target, address, &value)) != ERROR_OK)
-                       {
-                               command_print(cmd_ctx, "couldn't access reg %i", address);
-                               return ERROR_OK;
-                       }
-                       jtag_execute_queue();
-                       
-                       command_print(cmd_ctx, "%i: %8.8x", address, value);
-               }
-               else if (argc == 2)
-               {
-                       u32 value = strtoul(args[1], NULL, 0);
-                       if ((retval = arm966e_write_cp15(target, address, value)) != ERROR_OK)
-                       {
-                               command_print(cmd_ctx, "couldn't access reg %i", address);
-                               return ERROR_OK;
-                       }
-                       command_print(cmd_ctx, "%i: %8.8x", address, value);
-               }
-       }
-
-       return ERROR_OK;
-}
-
-int arm966e_register_commands(struct command_context_s *cmd_ctx)
-{
-       int retval;
-       command_t *arm966e_cmd;
-       
-       retval = arm9tdmi_register_commands(cmd_ctx);
-       arm966e_cmd = register_command(cmd_ctx, NULL, "arm966e", NULL, COMMAND_ANY, "arm966e specific commands");
-       register_command(cmd_ctx, arm966e_cmd, "cp15", arm966e_handle_cp15_command, COMMAND_EXEC, "display/modify cp15 register <num> [value]");
-       
-       return ERROR_OK;
-}
+/***************************************************************************\r
+ *   Copyright (C) 2005 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "arm966e.h"\r
+\r
+#include "arm7_9_common.h"\r
+#include "register.h"\r
+#include "target.h"\r
+#include "armv4_5.h"\r
+#include "embeddedice.h"\r
+#include "log.h"\r
+#include "jtag.h"\r
+#include "arm_jtag.h"\r
+\r
+#include <stdlib.h>\r
+#include <string.h>\r
+\r
+#if 0\r
+#define _DEBUG_INSTRUCTION_EXECUTION_\r
+#endif\r
+\r
+/* cli handling */\r
+int arm966e_register_commands(struct command_context_s *cmd_ctx);\r
+\r
+/* forward declarations */\r
+int arm966e_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);\r
+int arm966e_init_target(struct command_context_s *cmd_ctx, struct target_s *target);\r
+int arm966e_quit(void);\r
+\r
+target_type_t arm966e_target =\r
+{\r
+       .name = "arm966e",\r
+\r
+       .poll = arm7_9_poll,\r
+       .arch_state = armv4_5_arch_state,\r
+\r
+       .target_request_data = arm7_9_target_request_data,\r
+\r
+       .halt = arm7_9_halt,\r
+       .resume = arm7_9_resume,\r
+       .step = arm7_9_step,\r
+\r
+       .assert_reset = arm7_9_assert_reset,\r
+       .deassert_reset = arm7_9_deassert_reset,\r
+       .soft_reset_halt = arm7_9_soft_reset_halt,\r
+       .prepare_reset_halt = arm7_9_prepare_reset_halt,\r
+\r
+       .get_gdb_reg_list = armv4_5_get_gdb_reg_list,\r
+\r
+       .read_memory = arm7_9_read_memory,\r
+       .write_memory = arm7_9_write_memory,\r
+       .bulk_write_memory = arm7_9_bulk_write_memory,\r
+       .checksum_memory = arm7_9_checksum_memory,\r
+       \r
+       .run_algorithm = armv4_5_run_algorithm,\r
+       \r
+       .add_breakpoint = arm7_9_add_breakpoint,\r
+       .remove_breakpoint = arm7_9_remove_breakpoint,\r
+       .add_watchpoint = arm7_9_add_watchpoint,\r
+       .remove_watchpoint = arm7_9_remove_watchpoint,\r
+\r
+       .register_commands = arm966e_register_commands,\r
+       .target_command = arm966e_target_command,\r
+       .init_target = arm966e_init_target,\r
+       .quit = arm966e_quit,\r
+};\r
+\r
+int arm966e_init_target(struct command_context_s *cmd_ctx, struct target_s *target)\r
+{\r
+       arm9tdmi_init_target(cmd_ctx, target);\r
+               \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm966e_quit(void)\r
+{\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm966e_init_arch_info(target_t *target, arm966e_common_t *arm966e, int chain_pos, char *variant)\r
+{\r
+       arm9tdmi_common_t *arm9tdmi = &arm966e->arm9tdmi_common;\r
+       arm7_9_common_t *arm7_9 = &arm9tdmi->arm7_9_common;\r
+       \r
+       arm9tdmi_init_arch_info(target, arm9tdmi, chain_pos, variant);\r
+\r
+       arm9tdmi->arch_info = arm966e;\r
+       arm966e->common_magic = ARM966E_COMMON_MAGIC;\r
+       \r
+       /* The ARM966E-S implements the ARMv5TE architecture which\r
+        * has the BKPT instruction, so we don't have to use a watchpoint comparator\r
+        */\r
+       arm7_9->arm_bkpt = ARMV5_BKPT(0x0);\r
+       arm7_9->thumb_bkpt = ARMV5_T_BKPT(0x0) & 0xffff;\r
+       \r
+       arm7_9->sw_bkpts_use_wp = 0;\r
+       arm7_9->sw_bkpts_enabled = 1;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm966e_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)\r
+{\r
+       int chain_pos;\r
+       char *variant = NULL;\r
+       arm966e_common_t *arm966e = malloc(sizeof(arm966e_common_t));\r
+       \r
+       if (argc < 4)\r
+       {\r
+               ERROR("'target arm966e' requires at least one additional argument");\r
+               exit(-1);\r
+       }\r
+       \r
+       chain_pos = strtoul(args[3], NULL, 0);\r
+       \r
+       if (argc >= 5)\r
+               variant = args[4];\r
+       \r
+       DEBUG("chain_pos: %i, variant: %s", chain_pos, variant);\r
+       \r
+       arm966e_init_arch_info(target, arm966e, chain_pos, variant);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm966e_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p, arm9tdmi_common_t **arm9tdmi_p, arm966e_common_t **arm966e_p)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm966e_common_t *arm966e;\r
+       \r
+       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       arm7_9 = armv4_5->arch_info;\r
+       if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       arm9tdmi = arm7_9->arch_info;\r
+       if (arm9tdmi->common_magic != ARM9TDMI_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       arm966e = arm9tdmi->arch_info;\r
+       if (arm966e->common_magic != ARM966E_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       *armv4_5_p = armv4_5;\r
+       *arm7_9_p = arm7_9;\r
+       *arm9tdmi_p = arm9tdmi;\r
+       *arm966e_p = arm966e;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm966e_read_cp15(target_t *target, int reg_addr, u32 *value)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       scan_field_t fields[3];\r
+       u8 reg_addr_buf = reg_addr & 0x3f;\r
+       u8 nr_w_buf = 0;\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_scann(jtag_info, 0xf);\r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+\r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = NULL;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+\r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 6;\r
+       fields[1].out_value = &reg_addr_buf;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = jtag_info->chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = &nr_w_buf;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(3, fields, -1);\r
+\r
+       fields[0].in_handler_priv = value;\r
+       fields[0].in_handler = arm_jtag_buf_to_u32;\r
+\r
+       jtag_add_dr_scan(3, fields, -1);\r
+\r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+       jtag_execute_queue();\r
+       DEBUG("addr: 0x%x value: %8.8x", reg_addr, *value);\r
+#endif\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm966e_write_cp15(target_t *target, int reg_addr, u32 value)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       scan_field_t fields[3];\r
+       u8 reg_addr_buf = reg_addr & 0x3f;\r
+       u8 nr_w_buf = 1;\r
+       u8 value_buf[4];\r
+       \r
+       buf_set_u32(value_buf, 0, 32, value);\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_scann(jtag_info, 0xf);\r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+\r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = value_buf;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+\r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 6;\r
+       fields[1].out_value = &reg_addr_buf;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = jtag_info->chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = &nr_w_buf;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(3, fields, -1);\r
+\r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+       DEBUG("addr: 0x%x value: %8.8x", reg_addr, value);\r
+#endif\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm966e_handle_cp15_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       int retval;\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       arm966e_common_t *arm966e;\r
+       arm_jtag_t *jtag_info;\r
+\r
+       if (arm966e_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi, &arm966e) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM966e target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_info = &arm7_9->jtag_info;\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+\r
+       /* one or more argument, access a single register (write if second argument is given */\r
+       if (argc >= 1)\r
+       {\r
+               int address = strtoul(args[0], NULL, 0);\r
+\r
+               if (argc == 1)\r
+               {\r
+                       u32 value;\r
+                       if ((retval = arm966e_read_cp15(target, address, &value)) != ERROR_OK)\r
+                       {\r
+                               command_print(cmd_ctx, "couldn't access reg %i", address);\r
+                               return ERROR_OK;\r
+                       }\r
+                       jtag_execute_queue();\r
+                       \r
+                       command_print(cmd_ctx, "%i: %8.8x", address, value);\r
+               }\r
+               else if (argc == 2)\r
+               {\r
+                       u32 value = strtoul(args[1], NULL, 0);\r
+                       if ((retval = arm966e_write_cp15(target, address, value)) != ERROR_OK)\r
+                       {\r
+                               command_print(cmd_ctx, "couldn't access reg %i", address);\r
+                               return ERROR_OK;\r
+                       }\r
+                       command_print(cmd_ctx, "%i: %8.8x", address, value);\r
+               }\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int arm966e_register_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       int retval;\r
+       command_t *arm966e_cmd;\r
+       \r
+       retval = arm9tdmi_register_commands(cmd_ctx);\r
+       arm966e_cmd = register_command(cmd_ctx, NULL, "arm966e", NULL, COMMAND_ANY, "arm966e specific commands");\r
+       register_command(cmd_ctx, arm966e_cmd, "cp15", arm966e_handle_cp15_command, COMMAND_EXEC, "display/modify cp15 register <num> [value]");\r
+       \r
+       return ERROR_OK;\r
+}\r
index 5f808cdbb3f5a4e09121e04f0b3c8789c37fb744..7170693dc4b99eafc24fbb81cd1ebc971abc4e61 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2005 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "arm9tdmi.h"
-
-#include "arm7_9_common.h"
-#include "register.h"
-#include "target.h"
-#include "armv4_5.h"
-#include "embeddedice.h"
-#include "etm.h"
-#include "etb.h"
-#include "log.h"
-#include "jtag.h"
-#include "arm_jtag.h"
-
-#include <stdlib.h>
-#include <string.h>
-
-#if 0
-#define _DEBUG_INSTRUCTION_EXECUTION_
-#endif
-
-/* cli handling */
-int arm9tdmi_register_commands(struct command_context_s *cmd_ctx);
-int handle_arm9tdmi_catch_vectors_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-/* forward declarations */
-int arm9tdmi_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);
-int arm9tdmi_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
-int arm9tdmi_quit();
-               
-target_type_t arm9tdmi_target =
-{
-       .name = "arm9tdmi",
-
-       .poll = arm7_9_poll,
-       .arch_state = armv4_5_arch_state,
-
-       .target_request_data = arm7_9_target_request_data,
-
-       .halt = arm7_9_halt,
-       .resume = arm7_9_resume,
-       .step = arm7_9_step,
-
-       .assert_reset = arm7_9_assert_reset,
-       .deassert_reset = arm7_9_deassert_reset,
-       .soft_reset_halt = arm7_9_soft_reset_halt,
-       .prepare_reset_halt = arm7_9_prepare_reset_halt,
-
-       .get_gdb_reg_list = armv4_5_get_gdb_reg_list,
-
-       .read_memory = arm7_9_read_memory,
-       .write_memory = arm7_9_write_memory,
-       .bulk_write_memory = arm7_9_bulk_write_memory,
-       .checksum_memory = arm7_9_checksum_memory,
-       
-       .run_algorithm = armv4_5_run_algorithm,
-       
-       .add_breakpoint = arm7_9_add_breakpoint,
-       .remove_breakpoint = arm7_9_remove_breakpoint,
-       .add_watchpoint = arm7_9_add_watchpoint,
-       .remove_watchpoint = arm7_9_remove_watchpoint,
-
-       .register_commands = arm9tdmi_register_commands,
-       .target_command = arm9tdmi_target_command,
-       .init_target = arm9tdmi_init_target,
-       .quit = arm9tdmi_quit
-};
-
-arm9tdmi_vector_t arm9tdmi_vectors[] =
-{
-       {"reset", ARM9TDMI_RESET_VECTOR},
-       {"undef", ARM9TDMI_UNDEF_VECTOR},
-       {"swi", ARM9TDMI_SWI_VECTOR},
-       {"pabt", ARM9TDMI_PABT_VECTOR},
-       {"dabt", ARM9TDMI_DABT_VECTOR},
-       {"reserved", ARM9TDMI_RESERVED_VECTOR},
-       {"irq", ARM9TDMI_IRQ_VECTOR},
-       {"fiq", ARM9TDMI_FIQ_VECTOR},
-       {0, 0},
-};
-
-int arm9tdmi_examine_debug_reason(target_t *target)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       
-       /* only check the debug reason if we don't know it already */
-       if ((target->debug_reason != DBG_REASON_DBGRQ)
-                       && (target->debug_reason != DBG_REASON_SINGLESTEP))
-       {
-               scan_field_t fields[3];
-               u8 databus[4];
-               u8 instructionbus[4];
-               u8 debug_reason;
-
-               jtag_add_end_state(TAP_PD);
-
-               fields[0].device = arm7_9->jtag_info.chain_pos;
-               fields[0].num_bits = 32;
-               fields[0].out_value = NULL;
-               fields[0].out_mask = NULL;
-               fields[0].in_value = databus;
-               fields[0].in_check_value = NULL;
-               fields[0].in_check_mask = NULL;
-               fields[0].in_handler = NULL;
-               fields[0].in_handler_priv = NULL;
-               
-               fields[1].device = arm7_9->jtag_info.chain_pos;
-               fields[1].num_bits = 3;
-               fields[1].out_value = NULL;
-               fields[1].out_mask = NULL;
-               fields[1].in_value = &debug_reason;
-               fields[1].in_check_value = NULL;
-               fields[1].in_check_mask = NULL;
-               fields[1].in_handler = NULL;
-               fields[1].in_handler_priv = NULL;
-               
-               fields[2].device = arm7_9->jtag_info.chain_pos;
-               fields[2].num_bits = 32;
-               fields[2].out_value = NULL;
-               fields[2].out_mask = NULL;
-               fields[2].in_value = instructionbus;
-               fields[2].in_check_value = NULL;
-               fields[2].in_check_mask = NULL;
-               fields[2].in_handler = NULL;
-               fields[2].in_handler_priv = NULL;
-               
-               arm_jtag_scann(&arm7_9->jtag_info, 0x1);
-               arm_jtag_set_instr(&arm7_9->jtag_info, arm7_9->jtag_info.intest_instr, NULL);
-
-               jtag_add_dr_scan(3, fields, TAP_PD, NULL);
-               jtag_execute_queue();
-               
-               fields[0].in_value = NULL;
-               fields[0].out_value = databus;
-               fields[1].in_value = NULL;
-               fields[1].out_value = &debug_reason;
-               fields[2].in_value = NULL;
-               fields[2].out_value = instructionbus;
-               
-               jtag_add_dr_scan(3, fields, TAP_PD, NULL);
-
-               if (debug_reason & 0x4)
-                       if (debug_reason & 0x2)
-                               target->debug_reason = DBG_REASON_WPTANDBKPT;
-               else
-                       target->debug_reason = DBG_REASON_WATCHPOINT;
-               else
-                       target->debug_reason = DBG_REASON_BREAKPOINT;
-       }
-
-       return ERROR_OK;
-}
-
-/* put an instruction in the ARM9TDMI pipeline or write the data bus, and optionally read data */
-int arm9tdmi_clock_out(arm_jtag_t *jtag_info, u32 instr, u32 out, u32 *in, int sysspeed)
-{
-       scan_field_t fields[3];
-       u8 out_buf[4];
-       u8 instr_buf[4];
-       u8 sysspeed_buf = 0x0;
-       
-       /* prepare buffer */
-       buf_set_u32(out_buf, 0, 32, out);
-       
-       buf_set_u32(instr_buf, 0, 32, flip_u32(instr, 32));
-       
-       if (sysspeed)
-               buf_set_u32(&sysspeed_buf, 2, 1, 1);
-       
-       jtag_add_end_state(TAP_PD);
-       arm_jtag_scann(jtag_info, 0x1);
-       
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-               
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = out_buf;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       if (in)
-       {
-               fields[0].in_handler = arm_jtag_buf_to_u32;
-               fields[0].in_handler_priv = in;
-       }
-       else
-       {
-               fields[0].in_handler = NULL;
-               fields[0].in_handler_priv = NULL;
-       }
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 3;
-       fields[1].out_value = &sysspeed_buf;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-               
-       fields[2].device = jtag_info->chain_pos;
-       fields[2].num_bits = 32;
-       fields[2].out_value = instr_buf;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-
-       jtag_add_dr_scan(3, fields, -1, NULL);
-
-       jtag_add_runtest(0, -1);
-       
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-       {
-               jtag_execute_queue();
-               
-               if (in)
-               {
-                       DEBUG("instr: 0x%8.8x, out: 0x%8.8x, in: 0x%8.8x", instr, out, *in);
-               }
-               else
-                       DEBUG("instr: 0x%8.8x, out: 0x%8.8x", instr, out);
-       }
-#endif
-
-       return ERROR_OK;
-}
-
-/* just read data (instruction and data-out = don't care) */
-int arm9tdmi_clock_data_in(arm_jtag_t *jtag_info, u32 *in)
-{
-       scan_field_t fields[3];
-
-       jtag_add_end_state(TAP_PD);
-       arm_jtag_scann(jtag_info, 0x1);
-       
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-               
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = NULL;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_handler = arm_jtag_buf_to_u32;
-       fields[0].in_handler_priv = in;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 3;
-       fields[1].out_value = NULL;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-
-       fields[2].device = jtag_info->chain_pos;
-       fields[2].num_bits = 32;
-       fields[2].out_value = NULL;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(3, fields, -1, NULL);
-
-       jtag_add_runtest(0, -1);
-       
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-       {
-               jtag_execute_queue();
-                       
-               if (in)
-               {
-                       DEBUG("in: 0x%8.8x", *in);
-               }
-               else
-               {
-                       ERROR("BUG: called with in == NULL");
-               }
-       }
-#endif
-
-       return ERROR_OK;
-}
-
-/* clock the target, and read the databus
- * the *in pointer points to a buffer where elements of 'size' bytes
- * are stored in big (be==1) or little (be==0) endianness
- */
-int arm9tdmi_clock_data_in_endianness(arm_jtag_t *jtag_info, void *in, int size, int be)
-{
-       scan_field_t fields[3];
-       
-       jtag_add_end_state(TAP_PD);
-       arm_jtag_scann(jtag_info, 0x1);
-       
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-               
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = NULL;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       switch (size)
-       {
-               case 4:
-                       fields[0].in_handler = (be) ? arm_jtag_buf_to_be32 : arm_jtag_buf_to_le32;
-                       break;
-               case 2:
-                       fields[0].in_handler = (be) ? arm_jtag_buf_to_be16 : arm_jtag_buf_to_le16;
-                       break;
-               case 1:
-                       fields[0].in_handler = arm_jtag_buf_to_8;
-                       break;
-       }
-       fields[0].in_handler_priv = in;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 3;
-       fields[1].out_value = NULL;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-
-       fields[2].device = jtag_info->chain_pos;
-       fields[2].num_bits = 32;
-       fields[2].out_value = NULL;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(3, fields, -1, NULL);
-
-       jtag_add_runtest(0, -1);
-       
-#ifdef _DEBUG_INSTRUCTION_EXECUTION_
-       {
-               jtag_execute_queue();
-                       
-               if (in)
-               {
-                       DEBUG("in: 0x%8.8x", *in);
-               }
-               else
-               {
-                       ERROR("BUG: called with in == NULL");
-               }
-       }
-#endif
-
-       return ERROR_OK;
-}
-
-void arm9tdmi_change_to_arm(target_t *target, u32 *r0, u32 *pc)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       
-       /* save r0 before using it and put system in ARM state 
-        * to allow common handling of ARM and THUMB debugging */
-       
-       /* fetch STR r0, [r0] */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-       /* STR r0, [r0] in Memory */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, r0, 0);
-
-       /* MOV r0, r15 fetched, STR in Decode */        
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_MOV(0, 15), 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-       /* nothing fetched, STR r0, [r0] in Memory */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, pc, 0);
-
-       /* use pc-relative LDR to clear r0[1:0] (for switch to ARM mode) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), 0, NULL, 0);
-       /* LDR in Decode */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-       /* LDR in Execute */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-       /* LDR in Memory (to account for interlock) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-
-       /* fetch BX */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_BX(0), 0, NULL, 0);
-       /* NOP fetched, BX in Decode, MOV in Execute */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-       /* NOP fetched, BX in Execute (1) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-       
-       jtag_execute_queue();
-       
-       /* fix program counter:
-        * MOV r0, r15 was the 5th instruction (+8)
-        * reading PC in Thumb state gives address of instruction + 4
-        */
-       *pc -= 0xc;
-}
-
-void arm9tdmi_read_core_regs(target_t *target, u32 mask, u32* core_regs[16])
-{
-       int i;
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-               
-       /* STMIA r0-15, [r0] at debug speed
-        * register values will start to appear on 4th DCLK
-        */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), 0, NULL, 0);
-
-       /* fetch NOP, STM in DECODE stage */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* fetch NOP, STM in EXECUTE stage (1st cycle) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-
-       for (i = 0; i <= 15; i++)
-       {
-               if (mask & (1 << i))
-                       /* nothing fetched, STM in MEMORY (i'th cycle) */
-                       arm9tdmi_clock_data_in(jtag_info, core_regs[i]);
-       }
-
-}
-
-void arm9tdmi_read_core_regs_target_buffer(target_t *target, u32 mask, void* buffer, int size)
-{
-       int i;
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       int be = (target->endianness == TARGET_BIG_ENDIAN) ? 1 : 0;
-       u32 *buf_u32 = buffer;
-       u16 *buf_u16 = buffer;
-       u8 *buf_u8 = buffer;
-       
-       /* STMIA r0-15, [r0] at debug speed
-        * register values will start to appear on 4th DCLK
-        */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), 0, NULL, 0);
-
-       /* fetch NOP, STM in DECODE stage */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* fetch NOP, STM in EXECUTE stage (1st cycle) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-
-       for (i = 0; i <= 15; i++)
-       {
-               if (mask & (1 << i))
-                       /* nothing fetched, STM in MEMORY (i'th cycle) */
-                       switch (size)
-                       {
-                               case 4:
-                                       arm9tdmi_clock_data_in_endianness(jtag_info, buf_u32++, 4, be);
-                                       break;
-                               case 2:
-                                       arm9tdmi_clock_data_in_endianness(jtag_info, buf_u16++, 2, be);
-                                       break;
-                               case 1:
-                                       arm9tdmi_clock_data_in_endianness(jtag_info, buf_u8++, 1, be);
-                                       break;
-                       }
-       }
-
-}
-
-void arm9tdmi_read_xpsr(target_t *target, u32 *xpsr, int spsr)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-               
-       /* MRS r0, cpsr */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_MRS(0, spsr & 1), 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-
-       /* STR r0, [r15] */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_STR(0, 15), 0, NULL, 0);
-       /* fetch NOP, STR in DECODE stage */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* fetch NOP, STR in EXECUTE stage (1st cycle) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* nothing fetched, STR in MEMORY */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, xpsr, 0);
-
-}
-
-void arm9tdmi_write_xpsr(target_t *target, u32 xpsr, int spsr)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-               
-       DEBUG("xpsr: %8.8x, spsr: %i", xpsr, spsr);
-
-       /* MSR1 fetched */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr & 0xff, 0, 1, spsr), 0, NULL, 0);
-       /* MSR2 fetched, MSR1 in DECODE */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff00) >> 8, 0xc, 2, spsr), 0, NULL, 0);
-       /* MSR3 fetched, MSR1 in EXECUTE (1), MSR2 in DECODE */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff0000) >> 16, 0x8, 4, spsr), 0, NULL, 0);
-       /* nothing fetched, MSR1 in EXECUTE (2) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* nothing fetched, MSR1 in EXECUTE (3) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* MSR4 fetched, MSR2 in EXECUTE (1), MSR3 in DECODE */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff000000) >> 24, 0x4, 8, spsr), 0, NULL, 0);
-       /* nothing fetched, MSR2 in EXECUTE (2) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* nothing fetched, MSR2 in EXECUTE (3) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* NOP fetched, MSR3 in EXECUTE (1), MSR4 in DECODE */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* nothing fetched, MSR3 in EXECUTE (2) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* nothing fetched, MSR3 in EXECUTE (3) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* NOP fetched, MSR4 in EXECUTE (1) */
-       /* last MSR writes flags, which takes only one cycle */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-}
-
-void arm9tdmi_write_xpsr_im8(target_t *target, u8 xpsr_im, int rot, int spsr)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-               
-       DEBUG("xpsr_im: %2.2x, rot: %i, spsr: %i", xpsr_im, rot, spsr);
-       
-       /* MSR fetched */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr_im, rot, 1, spsr), 0, NULL, 0);
-       /* NOP fetched, MSR in DECODE */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* NOP fetched, MSR in EXECUTE (1) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       
-       /* rot == 4 writes flags, which takes only one cycle */
-       if (rot != 4)
-       {
-               /* nothing fetched, MSR in EXECUTE (2) */
-               arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-               /* nothing fetched, MSR in EXECUTE (3) */
-               arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       }
-}
-
-void arm9tdmi_write_core_regs(target_t *target, u32 mask, u32 core_regs[16])
-{
-       int i;
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-               
-       /* LDMIA r0-15, [r0] at debug speed
-       * register values will start to appear on 4th DCLK
-       */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 0), 0, NULL, 0);
-
-       /* fetch NOP, LDM in DECODE stage */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* fetch NOP, LDM in EXECUTE stage (1st cycle) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-
-       for (i = 0; i <= 15; i++)
-       {
-               if (mask & (1 << i))
-                       /* nothing fetched, LDM still in EXECUTE (1+i cycle) */
-                       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, core_regs[i], NULL, 0);
-       }
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       
-}
-
-void arm9tdmi_load_word_regs(target_t *target, u32 mask)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-
-       /* put system-speed load-multiple into the pipeline */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 1), 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
-
-}
-
-void arm9tdmi_load_hword_reg(target_t *target, int num)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       
-       /* put system-speed load half-word into the pipeline */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_LDRH_IP(num, 0), 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
-}
-
-void arm9tdmi_load_byte_reg(target_t *target, int num)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-
-       /* put system-speed load byte into the pipeline */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_LDRB_IP(num, 0), 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
-
-}
-
-void arm9tdmi_store_word_regs(target_t *target, u32 mask)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-
-       /* put system-speed store-multiple into the pipeline */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask, 0, 1), 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
-
-}
-
-void arm9tdmi_store_hword_reg(target_t *target, int num)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-
-       /* put system-speed store half-word into the pipeline */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_STRH_IP(num, 0), 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
-
-}
-
-void arm9tdmi_store_byte_reg(target_t *target, int num)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-
-       /* put system-speed store byte into the pipeline */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_STRB_IP(num, 0), 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
-
-}
-
-void arm9tdmi_write_pc(target_t *target, u32 pc)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       
-       /* LDMIA r0-15, [r0] at debug speed
-        * register values will start to appear on 4th DCLK
-        */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x8000, 0, 0), 0, NULL, 0);
-
-       /* fetch NOP, LDM in DECODE stage */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* fetch NOP, LDM in EXECUTE stage (1st cycle) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* nothing fetched, LDM in EXECUTE stage (2nd cycle) (output data) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, pc, NULL, 0);
-       /* nothing fetched, LDM in EXECUTE stage (3rd cycle) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* fetch NOP, LDM in EXECUTE stage (4th cycle) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* fetch NOP, LDM in EXECUTE stage (5th cycle) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-
-}
-
-void arm9tdmi_branch_resume(target_t *target)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_B(0xfffffc, 0), 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);
-
-}
-
-void arm9tdmi_branch_resume_thumb(target_t *target)
-{
-       DEBUG("-");
-       
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
-
-       /* LDMIA r0-15, [r0] at debug speed
-       * register values will start to appear on 4th DCLK
-       */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x1, 0, 0), 0, NULL, 0);
-
-       /* fetch NOP, LDM in DECODE stage */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* fetch NOP, LDM in EXECUTE stage (1st cycle) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       /* nothing fetched, LDM in EXECUTE stage (2nd cycle) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32) | 1, NULL, 0);
-       /* nothing fetched, LDM in EXECUTE stage (3rd cycle) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-
-       /* Branch and eXchange */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_BX(0), 0, NULL, 0);
-       
-       embeddedice_read_reg(dbg_stat);
-       
-       /* fetch NOP, BX in DECODE stage */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-       
-       embeddedice_read_reg(dbg_stat);
-       
-       /* fetch NOP, BX in EXECUTE stage (1st cycle) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);
-
-       /* target is now in Thumb state */
-       embeddedice_read_reg(dbg_stat);
-
-       /* load r0 value, MOV_IM in Decode*/
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), 0, NULL, 0);
-       /* fetch NOP, LDR in Decode, MOV_IM in Execute */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-       /* fetch NOP, LDR in Execute */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-       /* nothing fetched, LDR in EXECUTE stage (2nd cycle) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, buf_get_u32(armv4_5->core_cache->reg_list[0].value, 0, 32), NULL, 0);
-       /* nothing fetched, LDR in EXECUTE stage (3rd cycle) */
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-       
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-
-       embeddedice_read_reg(dbg_stat);
-       
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_B(0x7f7), 0, NULL, 1);
-       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);
-
-}
-
-void arm9tdmi_enable_single_step(target_t *target)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       
-       if (arm7_9->has_single_step)
-       {
-               buf_set_u32(arm7_9->eice_cache->reg_list[EICE_DBG_CTRL].value, 3, 1, 1);
-               embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_DBG_CTRL]);
-       }
-       else
-       {
-               arm7_9_enable_eice_step(target);
-       }
-}
-
-void arm9tdmi_disable_single_step(target_t *target)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       
-       if (arm7_9->has_single_step)
-       {
-               buf_set_u32(arm7_9->eice_cache->reg_list[EICE_DBG_CTRL].value, 3, 1, 0);
-               embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_DBG_CTRL]);
-       }
-       else
-       {
-               arm7_9_disable_eice_step(target);
-       }
-}
-
-void arm9tdmi_build_reg_cache(target_t *target)
-{
-       reg_cache_t **cache_p = register_get_last_cache_p(&target->reg_cache);
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9 = armv4_5->arch_info;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-
-       (*cache_p) = armv4_5_build_reg_cache(target, armv4_5);
-       armv4_5->core_cache = (*cache_p);
-       
-       /* one extra register (vector catch) */
-       (*cache_p)->next = embeddedice_build_reg_cache(target, arm7_9);
-       arm7_9->eice_cache = (*cache_p)->next;
-
-       if (arm7_9->etm_ctx)
-       {
-               (*cache_p)->next->next = etm_build_reg_cache(target, jtag_info, arm7_9->etm_ctx);
-               arm7_9->etm_ctx->reg_cache = (*cache_p)->next->next;
-       }
-}
-
-int arm9tdmi_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
-{
-       
-       arm9tdmi_build_reg_cache(target);
-       
-       return ERROR_OK;
-       
-}
-
-int arm9tdmi_quit()
-{
-       
-       return ERROR_OK;
-}
-
-int arm9tdmi_init_arch_info(target_t *target, arm9tdmi_common_t *arm9tdmi, int chain_pos, char *variant)
-{
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       
-       arm7_9 = &arm9tdmi->arm7_9_common;
-       armv4_5 = &arm7_9->armv4_5_common;
-       
-       /* prepare JTAG information for the new target */
-       arm7_9->jtag_info.chain_pos = chain_pos;
-       arm7_9->jtag_info.scann_size = 5;
-       
-       /* register arch-specific functions */
-       arm7_9->examine_debug_reason = arm9tdmi_examine_debug_reason;
-       arm7_9->change_to_arm = arm9tdmi_change_to_arm;
-       arm7_9->read_core_regs = arm9tdmi_read_core_regs;
-       arm7_9->read_core_regs_target_buffer = arm9tdmi_read_core_regs_target_buffer;
-       arm7_9->read_xpsr = arm9tdmi_read_xpsr;
-       
-       arm7_9->write_xpsr = arm9tdmi_write_xpsr;
-       arm7_9->write_xpsr_im8 = arm9tdmi_write_xpsr_im8;
-       arm7_9->write_core_regs = arm9tdmi_write_core_regs;
-       
-       arm7_9->load_word_regs = arm9tdmi_load_word_regs;
-       arm7_9->load_hword_reg = arm9tdmi_load_hword_reg;
-       arm7_9->load_byte_reg = arm9tdmi_load_byte_reg;
-       
-       arm7_9->store_word_regs = arm9tdmi_store_word_regs;
-       arm7_9->store_hword_reg = arm9tdmi_store_hword_reg;
-       arm7_9->store_byte_reg = arm9tdmi_store_byte_reg;
-       
-       arm7_9->write_pc = arm9tdmi_write_pc;
-       arm7_9->branch_resume = arm9tdmi_branch_resume;
-       arm7_9->branch_resume_thumb = arm9tdmi_branch_resume_thumb;
-
-       arm7_9->enable_single_step = arm9tdmi_enable_single_step;
-       arm7_9->disable_single_step = arm9tdmi_disable_single_step;
-       
-       arm7_9->pre_debug_entry = NULL;
-       arm7_9->post_debug_entry = NULL;
-       
-       arm7_9->pre_restore_context = NULL;
-       arm7_9->post_restore_context = NULL;
-
-       /* initialize arch-specific breakpoint handling */
-       arm7_9->arm_bkpt = 0xdeeedeee;
-       arm7_9->thumb_bkpt = 0xdeee;
-       
-       arm7_9->sw_bkpts_use_wp = 1;
-       arm7_9->sw_bkpts_enabled = 0;
-       arm7_9->dbgreq_adjust_pc = 3;
-       arm7_9->arch_info = arm9tdmi;
-       
-       arm9tdmi->common_magic = ARM9TDMI_COMMON_MAGIC;
-       arm9tdmi->arch_info = NULL;
-
-       if (variant)
-       {
-               arm9tdmi->variant = strdup(variant);
-       }
-       else
-       {
-               arm9tdmi->variant = strdup("");
-       }
-       
-       arm7_9_init_arch_info(target, arm7_9);
-
-       /* override use of DBGRQ, this is safe on ARM9TDMI */
-       arm7_9->use_dbgrq = 1;
-
-       /* all ARM9s have the vector catch register */
-       arm7_9->has_vector_catch = 1;
-       
-       return ERROR_OK;
-}
-
-int arm9tdmi_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p, arm9tdmi_common_t **arm9tdmi_p)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       
-       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       arm7_9 = armv4_5->arch_info;
-       if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       arm9tdmi = arm7_9->arch_info;
-       if (arm9tdmi->common_magic != ARM9TDMI_COMMON_MAGIC)
-       {
-               return -1;
-       }
-       
-       *armv4_5_p = armv4_5;
-       *arm7_9_p = arm7_9;
-       *arm9tdmi_p = arm9tdmi;
-       
-       return ERROR_OK;
-}
-
-
-/* target arm9tdmi <endianess> <startup_mode> <chain_pos> <variant>*/
-int arm9tdmi_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)
-{
-       int chain_pos;
-       char *variant = NULL;
-       arm9tdmi_common_t *arm9tdmi = malloc(sizeof(arm9tdmi_common_t));
-
-       if (argc < 4)
-       {
-               ERROR("'target arm9tdmi' requires at least one additional argument");
-               exit(-1);
-       }
-       
-       chain_pos = strtoul(args[3], NULL, 0);
-       
-       if (argc >= 5)
-               variant = args[4];
-       
-       arm9tdmi_init_arch_info(target, arm9tdmi, chain_pos, variant);
-       
-       return ERROR_OK;
-}
-
-int arm9tdmi_register_commands(struct command_context_s *cmd_ctx)
-{
-       int retval;
-       
-       command_t *arm9tdmi_cmd;
-       
-               
-       retval = arm7_9_register_commands(cmd_ctx);
-       
-       arm9tdmi_cmd = register_command(cmd_ctx, NULL, "arm9tdmi", NULL, COMMAND_ANY, "arm9tdmi specific commands");
-
-       register_command(cmd_ctx, arm9tdmi_cmd, "vector_catch", handle_arm9tdmi_catch_vectors_command, COMMAND_EXEC, "catch arm920t vectors ['all'|'none'|'<vec1 vec2 ...>']");
-       
-       
-       return ERROR_OK;
-
-}
-
-int handle_arm9tdmi_catch_vectors_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       arm9tdmi_common_t *arm9tdmi;
-       reg_t *vector_catch;
-       u32 vector_catch_value;
-       int i, j;
-       
-       if (arm9tdmi_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM9TDMI based target");
-               return ERROR_OK;
-       }
-       
-       vector_catch = &arm7_9->eice_cache->reg_list[EICE_VEC_CATCH];
-       
-       /* read the vector catch register if necessary */
-       if (!vector_catch->valid)
-               embeddedice_read_reg(vector_catch);
-       
-       /* get the current setting */
-       vector_catch_value = buf_get_u32(vector_catch->value, 0, 32);
-       
-       if (argc > 0)
-       {
-               vector_catch_value = 0x0;
-               if (strcmp(args[0], "all") == 0)
-               {
-                       vector_catch_value = 0xdf;
-               }
-               else if (strcmp(args[0], "none") == 0)
-               {
-                       /* do nothing */
-               }
-               else
-               {
-                       for (i = 0; i < argc; i++)
-                       {
-                               /* go through list of vectors */
-                               for(j = 0; arm9tdmi_vectors[j].name; j++)
-                               {
-                                       if (strcmp(args[i], arm9tdmi_vectors[j].name) == 0)
-                                       {
-                                               vector_catch_value |= arm9tdmi_vectors[j].value;
-                                               break;
-                                       }
-                               }
-                               
-                               /* complain if vector wasn't found */
-                               if (!arm9tdmi_vectors[j].name)
-                               {
-                                       command_print(cmd_ctx, "vector '%s' not found, leaving current setting unchanged", args[i]);
-                                       
-                                       /* reread current setting */
-                                       vector_catch_value = buf_get_u32(vector_catch->value, 0, 32);
-                                       
-                                       break;
-                               }
-                       }
-               }
-               
-               /* store new settings */
-               buf_set_u32(vector_catch->value, 0, 32, vector_catch_value);
-               embeddedice_store_reg(vector_catch);
-       }
-               
-       /* output current settings (skip RESERVED vector) */
-       for (i = 0; i < 8; i++)
-       {
-               if (i != 5)
-               {
-                       command_print(cmd_ctx, "%s: %s", arm9tdmi_vectors[i].name,
-                               (vector_catch_value & (1 << i)) ? "catch" : "don't catch");
-               }  
-       }
-
-       return ERROR_OK;
-}
+/***************************************************************************\r
+ *   Copyright (C) 2005 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "arm9tdmi.h"\r
+\r
+#include "arm7_9_common.h"\r
+#include "register.h"\r
+#include "target.h"\r
+#include "armv4_5.h"\r
+#include "embeddedice.h"\r
+#include "etm.h"\r
+#include "etb.h"\r
+#include "log.h"\r
+#include "jtag.h"\r
+#include "arm_jtag.h"\r
+\r
+#include <stdlib.h>\r
+#include <string.h>\r
+\r
+#if 0\r
+#define _DEBUG_INSTRUCTION_EXECUTION_\r
+#endif\r
+\r
+/* cli handling */\r
+int arm9tdmi_register_commands(struct command_context_s *cmd_ctx);\r
+int handle_arm9tdmi_catch_vectors_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+/* forward declarations */\r
+int arm9tdmi_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);\r
+int arm9tdmi_init_target(struct command_context_s *cmd_ctx, struct target_s *target);\r
+int arm9tdmi_quit();\r
+               \r
+target_type_t arm9tdmi_target =\r
+{\r
+       .name = "arm9tdmi",\r
+\r
+       .poll = arm7_9_poll,\r
+       .arch_state = armv4_5_arch_state,\r
+\r
+       .target_request_data = arm7_9_target_request_data,\r
+\r
+       .halt = arm7_9_halt,\r
+       .resume = arm7_9_resume,\r
+       .step = arm7_9_step,\r
+\r
+       .assert_reset = arm7_9_assert_reset,\r
+       .deassert_reset = arm7_9_deassert_reset,\r
+       .soft_reset_halt = arm7_9_soft_reset_halt,\r
+       .prepare_reset_halt = arm7_9_prepare_reset_halt,\r
+\r
+       .get_gdb_reg_list = armv4_5_get_gdb_reg_list,\r
+\r
+       .read_memory = arm7_9_read_memory,\r
+       .write_memory = arm7_9_write_memory,\r
+       .bulk_write_memory = arm7_9_bulk_write_memory,\r
+       .checksum_memory = arm7_9_checksum_memory,\r
+       \r
+       .run_algorithm = armv4_5_run_algorithm,\r
+       \r
+       .add_breakpoint = arm7_9_add_breakpoint,\r
+       .remove_breakpoint = arm7_9_remove_breakpoint,\r
+       .add_watchpoint = arm7_9_add_watchpoint,\r
+       .remove_watchpoint = arm7_9_remove_watchpoint,\r
+\r
+       .register_commands = arm9tdmi_register_commands,\r
+       .target_command = arm9tdmi_target_command,\r
+       .init_target = arm9tdmi_init_target,\r
+       .quit = arm9tdmi_quit\r
+};\r
+\r
+arm9tdmi_vector_t arm9tdmi_vectors[] =\r
+{\r
+       {"reset", ARM9TDMI_RESET_VECTOR},\r
+       {"undef", ARM9TDMI_UNDEF_VECTOR},\r
+       {"swi", ARM9TDMI_SWI_VECTOR},\r
+       {"pabt", ARM9TDMI_PABT_VECTOR},\r
+       {"dabt", ARM9TDMI_DABT_VECTOR},\r
+       {"reserved", ARM9TDMI_RESERVED_VECTOR},\r
+       {"irq", ARM9TDMI_IRQ_VECTOR},\r
+       {"fiq", ARM9TDMI_FIQ_VECTOR},\r
+       {0, 0},\r
+};\r
+\r
+int arm9tdmi_examine_debug_reason(target_t *target)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       \r
+       /* only check the debug reason if we don't know it already */\r
+       if ((target->debug_reason != DBG_REASON_DBGRQ)\r
+                       && (target->debug_reason != DBG_REASON_SINGLESTEP))\r
+       {\r
+               scan_field_t fields[3];\r
+               u8 databus[4];\r
+               u8 instructionbus[4];\r
+               u8 debug_reason;\r
+\r
+               jtag_add_end_state(TAP_PD);\r
+\r
+               fields[0].device = arm7_9->jtag_info.chain_pos;\r
+               fields[0].num_bits = 32;\r
+               fields[0].out_value = NULL;\r
+               fields[0].out_mask = NULL;\r
+               fields[0].in_value = databus;\r
+               fields[0].in_check_value = NULL;\r
+               fields[0].in_check_mask = NULL;\r
+               fields[0].in_handler = NULL;\r
+               fields[0].in_handler_priv = NULL;\r
+               \r
+               fields[1].device = arm7_9->jtag_info.chain_pos;\r
+               fields[1].num_bits = 3;\r
+               fields[1].out_value = NULL;\r
+               fields[1].out_mask = NULL;\r
+               fields[1].in_value = &debug_reason;\r
+               fields[1].in_check_value = NULL;\r
+               fields[1].in_check_mask = NULL;\r
+               fields[1].in_handler = NULL;\r
+               fields[1].in_handler_priv = NULL;\r
+               \r
+               fields[2].device = arm7_9->jtag_info.chain_pos;\r
+               fields[2].num_bits = 32;\r
+               fields[2].out_value = NULL;\r
+               fields[2].out_mask = NULL;\r
+               fields[2].in_value = instructionbus;\r
+               fields[2].in_check_value = NULL;\r
+               fields[2].in_check_mask = NULL;\r
+               fields[2].in_handler = NULL;\r
+               fields[2].in_handler_priv = NULL;\r
+               \r
+               arm_jtag_scann(&arm7_9->jtag_info, 0x1);\r
+               arm_jtag_set_instr(&arm7_9->jtag_info, arm7_9->jtag_info.intest_instr, NULL);\r
+\r
+               jtag_add_dr_scan(3, fields, TAP_PD);\r
+               jtag_execute_queue();\r
+               \r
+               fields[0].in_value = NULL;\r
+               fields[0].out_value = databus;\r
+               fields[1].in_value = NULL;\r
+               fields[1].out_value = &debug_reason;\r
+               fields[2].in_value = NULL;\r
+               fields[2].out_value = instructionbus;\r
+               \r
+               jtag_add_dr_scan(3, fields, TAP_PD);\r
+\r
+               if (debug_reason & 0x4)\r
+                       if (debug_reason & 0x2)\r
+                               target->debug_reason = DBG_REASON_WPTANDBKPT;\r
+               else\r
+                       target->debug_reason = DBG_REASON_WATCHPOINT;\r
+               else\r
+                       target->debug_reason = DBG_REASON_BREAKPOINT;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* put an instruction in the ARM9TDMI pipeline or write the data bus, and optionally read data */\r
+int arm9tdmi_clock_out(arm_jtag_t *jtag_info, u32 instr, u32 out, u32 *in, int sysspeed)\r
+{\r
+       scan_field_t fields[3];\r
+       u8 out_buf[4];\r
+       u8 instr_buf[4];\r
+       u8 sysspeed_buf = 0x0;\r
+       \r
+       /* prepare buffer */\r
+       buf_set_u32(out_buf, 0, 32, out);\r
+       \r
+       buf_set_u32(instr_buf, 0, 32, flip_u32(instr, 32));\r
+       \r
+       if (sysspeed)\r
+               buf_set_u32(&sysspeed_buf, 2, 1, 1);\r
+       \r
+       jtag_add_end_state(TAP_PD);\r
+       arm_jtag_scann(jtag_info, 0x1);\r
+       \r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+               \r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = out_buf;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       if (in)\r
+       {\r
+               fields[0].in_handler = arm_jtag_buf_to_u32;\r
+               fields[0].in_handler_priv = in;\r
+       }\r
+       else\r
+       {\r
+               fields[0].in_handler = NULL;\r
+               fields[0].in_handler_priv = NULL;\r
+       }\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       \r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 3;\r
+       fields[1].out_value = &sysspeed_buf;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+               \r
+       fields[2].device = jtag_info->chain_pos;\r
+       fields[2].num_bits = 32;\r
+       fields[2].out_value = instr_buf;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+\r
+       jtag_add_dr_scan(3, fields, -1);\r
+\r
+       jtag_add_runtest(0, -1);\r
+       \r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+       {\r
+               jtag_execute_queue();\r
+               \r
+               if (in)\r
+               {\r
+                       DEBUG("instr: 0x%8.8x, out: 0x%8.8x, in: 0x%8.8x", instr, out, *in);\r
+               }\r
+               else\r
+                       DEBUG("instr: 0x%8.8x, out: 0x%8.8x", instr, out);\r
+       }\r
+#endif\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* just read data (instruction and data-out = don't care) */\r
+int arm9tdmi_clock_data_in(arm_jtag_t *jtag_info, u32 *in)\r
+{\r
+       scan_field_t fields[3];\r
+\r
+       jtag_add_end_state(TAP_PD);\r
+       arm_jtag_scann(jtag_info, 0x1);\r
+       \r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+               \r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = NULL;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_handler = arm_jtag_buf_to_u32;\r
+       fields[0].in_handler_priv = in;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       \r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 3;\r
+       fields[1].out_value = NULL;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+\r
+       fields[2].device = jtag_info->chain_pos;\r
+       fields[2].num_bits = 32;\r
+       fields[2].out_value = NULL;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(3, fields, -1);\r
+\r
+       jtag_add_runtest(0, -1);\r
+       \r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+       {\r
+               jtag_execute_queue();\r
+                       \r
+               if (in)\r
+               {\r
+                       DEBUG("in: 0x%8.8x", *in);\r
+               }\r
+               else\r
+               {\r
+                       ERROR("BUG: called with in == NULL");\r
+               }\r
+       }\r
+#endif\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* clock the target, and read the databus\r
+ * the *in pointer points to a buffer where elements of 'size' bytes\r
+ * are stored in big (be==1) or little (be==0) endianness\r
+ */\r
+int arm9tdmi_clock_data_in_endianness(arm_jtag_t *jtag_info, void *in, int size, int be)\r
+{\r
+       scan_field_t fields[3];\r
+       \r
+       jtag_add_end_state(TAP_PD);\r
+       arm_jtag_scann(jtag_info, 0x1);\r
+       \r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+               \r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = NULL;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       switch (size)\r
+       {\r
+               case 4:\r
+                       fields[0].in_handler = (be) ? arm_jtag_buf_to_be32 : arm_jtag_buf_to_le32;\r
+                       break;\r
+               case 2:\r
+                       fields[0].in_handler = (be) ? arm_jtag_buf_to_be16 : arm_jtag_buf_to_le16;\r
+                       break;\r
+               case 1:\r
+                       fields[0].in_handler = arm_jtag_buf_to_8;\r
+                       break;\r
+       }\r
+       fields[0].in_handler_priv = in;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       \r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 3;\r
+       fields[1].out_value = NULL;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+\r
+       fields[2].device = jtag_info->chain_pos;\r
+       fields[2].num_bits = 32;\r
+       fields[2].out_value = NULL;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(3, fields, -1);\r
+\r
+       jtag_add_runtest(0, -1);\r
+       \r
+#ifdef _DEBUG_INSTRUCTION_EXECUTION_\r
+       {\r
+               jtag_execute_queue();\r
+                       \r
+               if (in)\r
+               {\r
+                       DEBUG("in: 0x%8.8x", *in);\r
+               }\r
+               else\r
+               {\r
+                       ERROR("BUG: called with in == NULL");\r
+               }\r
+       }\r
+#endif\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+void arm9tdmi_change_to_arm(target_t *target, u32 *r0, u32 *pc)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       \r
+       /* save r0 before using it and put system in ARM state \r
+        * to allow common handling of ARM and THUMB debugging */\r
+       \r
+       /* fetch STR r0, [r0] */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+       /* STR r0, [r0] in Memory */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, r0, 0);\r
+\r
+       /* MOV r0, r15 fetched, STR in Decode */        \r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_MOV(0, 15), 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_STR(0, 0), 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+       /* nothing fetched, STR r0, [r0] in Memory */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, pc, 0);\r
+\r
+       /* use pc-relative LDR to clear r0[1:0] (for switch to ARM mode) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), 0, NULL, 0);\r
+       /* LDR in Decode */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+       /* LDR in Execute */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+       /* LDR in Memory (to account for interlock) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+\r
+       /* fetch BX */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_BX(0), 0, NULL, 0);\r
+       /* NOP fetched, BX in Decode, MOV in Execute */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+       /* NOP fetched, BX in Execute (1) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+       \r
+       jtag_execute_queue();\r
+       \r
+       /* fix program counter:\r
+        * MOV r0, r15 was the 5th instruction (+8)\r
+        * reading PC in Thumb state gives address of instruction + 4\r
+        */\r
+       *pc -= 0xc;\r
+}\r
+\r
+void arm9tdmi_read_core_regs(target_t *target, u32 mask, u32* core_regs[16])\r
+{\r
+       int i;\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+               \r
+       /* STMIA r0-15, [r0] at debug speed\r
+        * register values will start to appear on 4th DCLK\r
+        */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), 0, NULL, 0);\r
+\r
+       /* fetch NOP, STM in DECODE stage */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* fetch NOP, STM in EXECUTE stage (1st cycle) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+\r
+       for (i = 0; i <= 15; i++)\r
+       {\r
+               if (mask & (1 << i))\r
+                       /* nothing fetched, STM in MEMORY (i'th cycle) */\r
+                       arm9tdmi_clock_data_in(jtag_info, core_regs[i]);\r
+       }\r
+\r
+}\r
+\r
+void arm9tdmi_read_core_regs_target_buffer(target_t *target, u32 mask, void* buffer, int size)\r
+{\r
+       int i;\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       int be = (target->endianness == TARGET_BIG_ENDIAN) ? 1 : 0;\r
+       u32 *buf_u32 = buffer;\r
+       u16 *buf_u16 = buffer;\r
+       u8 *buf_u8 = buffer;\r
+       \r
+       /* STMIA r0-15, [r0] at debug speed\r
+        * register values will start to appear on 4th DCLK\r
+        */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask & 0xffff, 0, 0), 0, NULL, 0);\r
+\r
+       /* fetch NOP, STM in DECODE stage */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* fetch NOP, STM in EXECUTE stage (1st cycle) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+\r
+       for (i = 0; i <= 15; i++)\r
+       {\r
+               if (mask & (1 << i))\r
+                       /* nothing fetched, STM in MEMORY (i'th cycle) */\r
+                       switch (size)\r
+                       {\r
+                               case 4:\r
+                                       arm9tdmi_clock_data_in_endianness(jtag_info, buf_u32++, 4, be);\r
+                                       break;\r
+                               case 2:\r
+                                       arm9tdmi_clock_data_in_endianness(jtag_info, buf_u16++, 2, be);\r
+                                       break;\r
+                               case 1:\r
+                                       arm9tdmi_clock_data_in_endianness(jtag_info, buf_u8++, 1, be);\r
+                                       break;\r
+                       }\r
+       }\r
+\r
+}\r
+\r
+void arm9tdmi_read_xpsr(target_t *target, u32 *xpsr, int spsr)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+               \r
+       /* MRS r0, cpsr */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_MRS(0, spsr & 1), 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+\r
+       /* STR r0, [r15] */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_STR(0, 15), 0, NULL, 0);\r
+       /* fetch NOP, STR in DECODE stage */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* fetch NOP, STR in EXECUTE stage (1st cycle) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* nothing fetched, STR in MEMORY */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, xpsr, 0);\r
+\r
+}\r
+\r
+void arm9tdmi_write_xpsr(target_t *target, u32 xpsr, int spsr)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+               \r
+       DEBUG("xpsr: %8.8x, spsr: %i", xpsr, spsr);\r
+\r
+       /* MSR1 fetched */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr & 0xff, 0, 1, spsr), 0, NULL, 0);\r
+       /* MSR2 fetched, MSR1 in DECODE */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff00) >> 8, 0xc, 2, spsr), 0, NULL, 0);\r
+       /* MSR3 fetched, MSR1 in EXECUTE (1), MSR2 in DECODE */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff0000) >> 16, 0x8, 4, spsr), 0, NULL, 0);\r
+       /* nothing fetched, MSR1 in EXECUTE (2) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* nothing fetched, MSR1 in EXECUTE (3) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* MSR4 fetched, MSR2 in EXECUTE (1), MSR3 in DECODE */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM((xpsr & 0xff000000) >> 24, 0x4, 8, spsr), 0, NULL, 0);\r
+       /* nothing fetched, MSR2 in EXECUTE (2) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* nothing fetched, MSR2 in EXECUTE (3) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* NOP fetched, MSR3 in EXECUTE (1), MSR4 in DECODE */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* nothing fetched, MSR3 in EXECUTE (2) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* nothing fetched, MSR3 in EXECUTE (3) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* NOP fetched, MSR4 in EXECUTE (1) */\r
+       /* last MSR writes flags, which takes only one cycle */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+}\r
+\r
+void arm9tdmi_write_xpsr_im8(target_t *target, u8 xpsr_im, int rot, int spsr)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+               \r
+       DEBUG("xpsr_im: %2.2x, rot: %i, spsr: %i", xpsr_im, rot, spsr);\r
+       \r
+       /* MSR fetched */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_MSR_IM(xpsr_im, rot, 1, spsr), 0, NULL, 0);\r
+       /* NOP fetched, MSR in DECODE */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* NOP fetched, MSR in EXECUTE (1) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       \r
+       /* rot == 4 writes flags, which takes only one cycle */\r
+       if (rot != 4)\r
+       {\r
+               /* nothing fetched, MSR in EXECUTE (2) */\r
+               arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+               /* nothing fetched, MSR in EXECUTE (3) */\r
+               arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       }\r
+}\r
+\r
+void arm9tdmi_write_core_regs(target_t *target, u32 mask, u32 core_regs[16])\r
+{\r
+       int i;\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+               \r
+       /* LDMIA r0-15, [r0] at debug speed\r
+       * register values will start to appear on 4th DCLK\r
+       */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 0), 0, NULL, 0);\r
+\r
+       /* fetch NOP, LDM in DECODE stage */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* fetch NOP, LDM in EXECUTE stage (1st cycle) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+\r
+       for (i = 0; i <= 15; i++)\r
+       {\r
+               if (mask & (1 << i))\r
+                       /* nothing fetched, LDM still in EXECUTE (1+i cycle) */\r
+                       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, core_regs[i], NULL, 0);\r
+       }\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       \r
+}\r
+\r
+void arm9tdmi_load_word_regs(target_t *target, u32 mask)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+\r
+       /* put system-speed load-multiple into the pipeline */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, mask & 0xffff, 0, 1), 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);\r
+\r
+}\r
+\r
+void arm9tdmi_load_hword_reg(target_t *target, int num)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       \r
+       /* put system-speed load half-word into the pipeline */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_LDRH_IP(num, 0), 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);\r
+}\r
+\r
+void arm9tdmi_load_byte_reg(target_t *target, int num)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+\r
+       /* put system-speed load byte into the pipeline */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_LDRB_IP(num, 0), 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);\r
+\r
+}\r
+\r
+void arm9tdmi_store_word_regs(target_t *target, u32 mask)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+\r
+       /* put system-speed store-multiple into the pipeline */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_STMIA(0, mask, 0, 1), 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);\r
+\r
+}\r
+\r
+void arm9tdmi_store_hword_reg(target_t *target, int num)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+\r
+       /* put system-speed store half-word into the pipeline */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_STRH_IP(num, 0), 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);\r
+\r
+}\r
+\r
+void arm9tdmi_store_byte_reg(target_t *target, int num)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+\r
+       /* put system-speed store byte into the pipeline */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_STRB_IP(num, 0), 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);\r
+\r
+}\r
+\r
+void arm9tdmi_write_pc(target_t *target, u32 pc)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       \r
+       /* LDMIA r0-15, [r0] at debug speed\r
+        * register values will start to appear on 4th DCLK\r
+        */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x8000, 0, 0), 0, NULL, 0);\r
+\r
+       /* fetch NOP, LDM in DECODE stage */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* fetch NOP, LDM in EXECUTE stage (1st cycle) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* nothing fetched, LDM in EXECUTE stage (2nd cycle) (output data) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, pc, NULL, 0);\r
+       /* nothing fetched, LDM in EXECUTE stage (3rd cycle) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* fetch NOP, LDM in EXECUTE stage (4th cycle) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* fetch NOP, LDM in EXECUTE stage (5th cycle) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+\r
+}\r
+\r
+void arm9tdmi_branch_resume(target_t *target)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       \r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_B(0xfffffc, 0), 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 1);\r
+\r
+}\r
+\r
+void arm9tdmi_branch_resume_thumb(target_t *target)\r
+{\r
+       DEBUG("-");\r
+       \r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];\r
+\r
+       /* LDMIA r0-15, [r0] at debug speed\r
+       * register values will start to appear on 4th DCLK\r
+       */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_LDMIA(0, 0x1, 0, 0), 0, NULL, 0);\r
+\r
+       /* fetch NOP, LDM in DECODE stage */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* fetch NOP, LDM in EXECUTE stage (1st cycle) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       /* nothing fetched, LDM in EXECUTE stage (2nd cycle) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32) | 1, NULL, 0);\r
+       /* nothing fetched, LDM in EXECUTE stage (3rd cycle) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+\r
+       /* Branch and eXchange */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_BX(0), 0, NULL, 0);\r
+       \r
+       embeddedice_read_reg(dbg_stat);\r
+       \r
+       /* fetch NOP, BX in DECODE stage */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+       \r
+       embeddedice_read_reg(dbg_stat);\r
+       \r
+       /* fetch NOP, BX in EXECUTE stage (1st cycle) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_NOP, 0, NULL, 0);\r
+\r
+       /* target is now in Thumb state */\r
+       embeddedice_read_reg(dbg_stat);\r
+\r
+       /* load r0 value, MOV_IM in Decode*/\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_LDR_PCREL(0), 0, NULL, 0);\r
+       /* fetch NOP, LDR in Decode, MOV_IM in Execute */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+       /* fetch NOP, LDR in Execute */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+       /* nothing fetched, LDR in EXECUTE stage (2nd cycle) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, buf_get_u32(armv4_5->core_cache->reg_list[0].value, 0, 32), NULL, 0);\r
+       /* nothing fetched, LDR in EXECUTE stage (3rd cycle) */\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+       \r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+\r
+       embeddedice_read_reg(dbg_stat);\r
+       \r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_B(0x7f7), 0, NULL, 1);\r
+       arm9tdmi_clock_out(jtag_info, ARMV4_5_T_NOP, 0, NULL, 0);\r
+\r
+}\r
+\r
+void arm9tdmi_enable_single_step(target_t *target)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       \r
+       if (arm7_9->has_single_step)\r
+       {\r
+               buf_set_u32(arm7_9->eice_cache->reg_list[EICE_DBG_CTRL].value, 3, 1, 1);\r
+               embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_DBG_CTRL]);\r
+       }\r
+       else\r
+       {\r
+               arm7_9_enable_eice_step(target);\r
+       }\r
+}\r
+\r
+void arm9tdmi_disable_single_step(target_t *target)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       \r
+       if (arm7_9->has_single_step)\r
+       {\r
+               buf_set_u32(arm7_9->eice_cache->reg_list[EICE_DBG_CTRL].value, 3, 1, 0);\r
+               embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_DBG_CTRL]);\r
+       }\r
+       else\r
+       {\r
+               arm7_9_disable_eice_step(target);\r
+       }\r
+}\r
+\r
+void arm9tdmi_build_reg_cache(target_t *target)\r
+{\r
+       reg_cache_t **cache_p = register_get_last_cache_p(&target->reg_cache);\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9 = armv4_5->arch_info;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+\r
+       (*cache_p) = armv4_5_build_reg_cache(target, armv4_5);\r
+       armv4_5->core_cache = (*cache_p);\r
+       \r
+       /* one extra register (vector catch) */\r
+       (*cache_p)->next = embeddedice_build_reg_cache(target, arm7_9);\r
+       arm7_9->eice_cache = (*cache_p)->next;\r
+\r
+       if (arm7_9->etm_ctx)\r
+       {\r
+               (*cache_p)->next->next = etm_build_reg_cache(target, jtag_info, arm7_9->etm_ctx);\r
+               arm7_9->etm_ctx->reg_cache = (*cache_p)->next->next;\r
+       }\r
+}\r
+\r
+int arm9tdmi_init_target(struct command_context_s *cmd_ctx, struct target_s *target)\r
+{\r
+       \r
+       arm9tdmi_build_reg_cache(target);\r
+       \r
+       return ERROR_OK;\r
+       \r
+}\r
+\r
+int arm9tdmi_quit()\r
+{\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm9tdmi_init_arch_info(target_t *target, arm9tdmi_common_t *arm9tdmi, int chain_pos, char *variant)\r
+{\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       \r
+       arm7_9 = &arm9tdmi->arm7_9_common;\r
+       armv4_5 = &arm7_9->armv4_5_common;\r
+       \r
+       /* prepare JTAG information for the new target */\r
+       arm7_9->jtag_info.chain_pos = chain_pos;\r
+       arm7_9->jtag_info.scann_size = 5;\r
+       \r
+       /* register arch-specific functions */\r
+       arm7_9->examine_debug_reason = arm9tdmi_examine_debug_reason;\r
+       arm7_9->change_to_arm = arm9tdmi_change_to_arm;\r
+       arm7_9->read_core_regs = arm9tdmi_read_core_regs;\r
+       arm7_9->read_core_regs_target_buffer = arm9tdmi_read_core_regs_target_buffer;\r
+       arm7_9->read_xpsr = arm9tdmi_read_xpsr;\r
+       \r
+       arm7_9->write_xpsr = arm9tdmi_write_xpsr;\r
+       arm7_9->write_xpsr_im8 = arm9tdmi_write_xpsr_im8;\r
+       arm7_9->write_core_regs = arm9tdmi_write_core_regs;\r
+       \r
+       arm7_9->load_word_regs = arm9tdmi_load_word_regs;\r
+       arm7_9->load_hword_reg = arm9tdmi_load_hword_reg;\r
+       arm7_9->load_byte_reg = arm9tdmi_load_byte_reg;\r
+       \r
+       arm7_9->store_word_regs = arm9tdmi_store_word_regs;\r
+       arm7_9->store_hword_reg = arm9tdmi_store_hword_reg;\r
+       arm7_9->store_byte_reg = arm9tdmi_store_byte_reg;\r
+       \r
+       arm7_9->write_pc = arm9tdmi_write_pc;\r
+       arm7_9->branch_resume = arm9tdmi_branch_resume;\r
+       arm7_9->branch_resume_thumb = arm9tdmi_branch_resume_thumb;\r
+\r
+       arm7_9->enable_single_step = arm9tdmi_enable_single_step;\r
+       arm7_9->disable_single_step = arm9tdmi_disable_single_step;\r
+       \r
+       arm7_9->pre_debug_entry = NULL;\r
+       arm7_9->post_debug_entry = NULL;\r
+       \r
+       arm7_9->pre_restore_context = NULL;\r
+       arm7_9->post_restore_context = NULL;\r
+\r
+       /* initialize arch-specific breakpoint handling */\r
+       arm7_9->arm_bkpt = 0xdeeedeee;\r
+       arm7_9->thumb_bkpt = 0xdeee;\r
+       \r
+       arm7_9->sw_bkpts_use_wp = 1;\r
+       arm7_9->sw_bkpts_enabled = 0;\r
+       arm7_9->dbgreq_adjust_pc = 3;\r
+       arm7_9->arch_info = arm9tdmi;\r
+       \r
+       arm9tdmi->common_magic = ARM9TDMI_COMMON_MAGIC;\r
+       arm9tdmi->arch_info = NULL;\r
+\r
+       if (variant)\r
+       {\r
+               arm9tdmi->variant = strdup(variant);\r
+       }\r
+       else\r
+       {\r
+               arm9tdmi->variant = strdup("");\r
+       }\r
+       \r
+       arm7_9_init_arch_info(target, arm7_9);\r
+\r
+       /* override use of DBGRQ, this is safe on ARM9TDMI */\r
+       arm7_9->use_dbgrq = 1;\r
+\r
+       /* all ARM9s have the vector catch register */\r
+       arm7_9->has_vector_catch = 1;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm9tdmi_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, arm7_9_common_t **arm7_9_p, arm9tdmi_common_t **arm9tdmi_p)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       \r
+       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       arm7_9 = armv4_5->arch_info;\r
+       if (arm7_9->common_magic != ARM7_9_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       arm9tdmi = arm7_9->arch_info;\r
+       if (arm9tdmi->common_magic != ARM9TDMI_COMMON_MAGIC)\r
+       {\r
+               return -1;\r
+       }\r
+       \r
+       *armv4_5_p = armv4_5;\r
+       *arm7_9_p = arm7_9;\r
+       *arm9tdmi_p = arm9tdmi;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+\r
+/* target arm9tdmi <endianess> <startup_mode> <chain_pos> <variant>*/\r
+int arm9tdmi_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)\r
+{\r
+       int chain_pos;\r
+       char *variant = NULL;\r
+       arm9tdmi_common_t *arm9tdmi = malloc(sizeof(arm9tdmi_common_t));\r
+\r
+       if (argc < 4)\r
+       {\r
+               ERROR("'target arm9tdmi' requires at least one additional argument");\r
+               exit(-1);\r
+       }\r
+       \r
+       chain_pos = strtoul(args[3], NULL, 0);\r
+       \r
+       if (argc >= 5)\r
+               variant = args[4];\r
+       \r
+       arm9tdmi_init_arch_info(target, arm9tdmi, chain_pos, variant);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm9tdmi_register_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       int retval;\r
+       \r
+       command_t *arm9tdmi_cmd;\r
+       \r
+               \r
+       retval = arm7_9_register_commands(cmd_ctx);\r
+       \r
+       arm9tdmi_cmd = register_command(cmd_ctx, NULL, "arm9tdmi", NULL, COMMAND_ANY, "arm9tdmi specific commands");\r
+\r
+       register_command(cmd_ctx, arm9tdmi_cmd, "vector_catch", handle_arm9tdmi_catch_vectors_command, COMMAND_EXEC, "catch arm920t vectors ['all'|'none'|'<vec1 vec2 ...>']");\r
+       \r
+       \r
+       return ERROR_OK;\r
+\r
+}\r
+\r
+int handle_arm9tdmi_catch_vectors_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       arm9tdmi_common_t *arm9tdmi;\r
+       reg_t *vector_catch;\r
+       u32 vector_catch_value;\r
+       int i, j;\r
+       \r
+       if (arm9tdmi_get_arch_pointers(target, &armv4_5, &arm7_9, &arm9tdmi) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM9TDMI based target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       vector_catch = &arm7_9->eice_cache->reg_list[EICE_VEC_CATCH];\r
+       \r
+       /* read the vector catch register if necessary */\r
+       if (!vector_catch->valid)\r
+               embeddedice_read_reg(vector_catch);\r
+       \r
+       /* get the current setting */\r
+       vector_catch_value = buf_get_u32(vector_catch->value, 0, 32);\r
+       \r
+       if (argc > 0)\r
+       {\r
+               vector_catch_value = 0x0;\r
+               if (strcmp(args[0], "all") == 0)\r
+               {\r
+                       vector_catch_value = 0xdf;\r
+               }\r
+               else if (strcmp(args[0], "none") == 0)\r
+               {\r
+                       /* do nothing */\r
+               }\r
+               else\r
+               {\r
+                       for (i = 0; i < argc; i++)\r
+                       {\r
+                               /* go through list of vectors */\r
+                               for(j = 0; arm9tdmi_vectors[j].name; j++)\r
+                               {\r
+                                       if (strcmp(args[i], arm9tdmi_vectors[j].name) == 0)\r
+                                       {\r
+                                               vector_catch_value |= arm9tdmi_vectors[j].value;\r
+                                               break;\r
+                                       }\r
+                               }\r
+                               \r
+                               /* complain if vector wasn't found */\r
+                               if (!arm9tdmi_vectors[j].name)\r
+                               {\r
+                                       command_print(cmd_ctx, "vector '%s' not found, leaving current setting unchanged", args[i]);\r
+                                       \r
+                                       /* reread current setting */\r
+                                       vector_catch_value = buf_get_u32(vector_catch->value, 0, 32);\r
+                                       \r
+                                       break;\r
+                               }\r
+                       }\r
+               }\r
+               \r
+               /* store new settings */\r
+               buf_set_u32(vector_catch->value, 0, 32, vector_catch_value);\r
+               embeddedice_store_reg(vector_catch);\r
+       }\r
+               \r
+       /* output current settings (skip RESERVED vector) */\r
+       for (i = 0; i < 8; i++)\r
+       {\r
+               if (i != 5)\r
+               {\r
+                       command_print(cmd_ctx, "%s: %s", arm9tdmi_vectors[i].name,\r
+                               (vector_catch_value & (1 << i)) ? "catch" : "don't catch");\r
+               }  \r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
index 070af2137b4b783a3e4d1398e5d383e1e99b5290..08cf3462dcdaab060db23864ff79de3a4357fdd0 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2005 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "arm_jtag.h"
-
-#include "binarybuffer.h"
-#include "log.h"
-#include "jtag.h"
-
-#include <stdlib.h>
-
-#if 0
-#define _ARM_JTAG_SCAN_N_CHECK_
-#endif
-
-int arm_jtag_set_instr(arm_jtag_t *jtag_info, u32 new_instr,  in_handler_t handler)
-{
-       jtag_device_t *device = jtag_get_device(jtag_info->chain_pos);
-       
-       if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)
-       {
-               scan_field_t field;
-       
-               field.device = jtag_info->chain_pos;
-               field.num_bits = device->ir_length;
-               field.out_value = calloc(CEIL(field.num_bits, 8), 1);
-               buf_set_u32(field.out_value, 0, field.num_bits, new_instr);
-               field.out_mask = NULL;
-               field.in_value = NULL;
-               field.in_check_value = NULL;
-               field.in_check_mask = NULL;
-               field.in_handler = handler;
-               field.in_handler_priv = NULL;
-               jtag_add_ir_scan(1, &field, -1, NULL);
-               
-               
-               free(field.out_value);
-       }
-       
-       return ERROR_OK;
-}
-
-int arm_jtag_scann(arm_jtag_t *jtag_info, u32 new_scan_chain)
-{
-       if(jtag_info->cur_scan_chain != new_scan_chain)
-       {
-#ifdef _ARM_JTAG_SCAN_N_CHECK_
-               u8 scan_n_check_value = 1 << (jtag_info->scann_size - 1);
-#endif
-               scan_field_t field;
-               
-               field.device = jtag_info->chain_pos;
-               field.num_bits = jtag_info->scann_size;
-               field.out_value = calloc(CEIL(field.num_bits, 8), 1);
-               buf_set_u32(field.out_value, 0, field.num_bits, new_scan_chain);
-               field.out_mask = NULL;
-               field.in_value = NULL;
-#ifdef _ARM_JTAG_SCAN_N_CHECK_
-               jtag_set_check_value(&field, &scan_n_check_value, NULL, NULL, NULL);
-#else
-               field.in_handler = NULL;
-               field.in_handler_priv = NULL;
-#endif 
-               
-               
-               arm_jtag_set_instr(jtag_info, jtag_info->scann_instr, NULL);
-               jtag_add_dr_scan(1, &field, -1, NULL);
-               
-               jtag_info->cur_scan_chain = new_scan_chain;
-               
-               free(field.out_value);
-       }
-       
-       return ERROR_OK;
-}
-
-int arm_jtag_reset_callback(enum jtag_event event, void *priv)
-{
-       arm_jtag_t *jtag_info = priv;
-       
-       if (event == JTAG_TRST_ASSERTED)
-       {
-               jtag_info->cur_scan_chain = 0;
-       }
-       
-       return ERROR_OK;
-}
-
-int arm_jtag_setup_connection(arm_jtag_t *jtag_info)
-{
-       jtag_info->scann_instr = 0x2;
-       jtag_info->cur_scan_chain = 0;
-       jtag_info->intest_instr = 0xc;
-
-       jtag_register_event_callback(arm_jtag_reset_callback, jtag_info);
-       
-       return ERROR_OK;
-}
-
-/* read JTAG buffer into host-endian u32, flipping bit-order */
-int arm_jtag_buf_to_u32_flip(u8 *in_buf, void *priv, struct scan_field_s *field)
-{
-       u32 *dest = priv;
-       *dest = flip_u32(le_to_h_u32(in_buf), 32);
-       return ERROR_OK;
-}
-
-/* read JTAG buffer into little-endian u32, flipping bit-order */
-int arm_jtag_buf_to_le32_flip(u8 *in_buf, void *priv, struct scan_field_s *field)
-{
-       h_u32_to_le(((u8*)priv), flip_u32(le_to_h_u32(in_buf), 32));
-       return ERROR_OK;
-}
-
-/* read JTAG buffer into little-endian u16, flipping bit-order */
-int arm_jtag_buf_to_le16_flip(u8 *in_buf, void *priv, struct scan_field_s *field)
-{
-       h_u16_to_le(((u8*)priv), flip_u32(le_to_h_u32(in_buf), 32) & 0xffff);
-       return ERROR_OK;
-}
-
-/* read JTAG buffer into big-endian u32, flipping bit-order */
-int arm_jtag_buf_to_be32_flip(u8 *in_buf, void *priv, struct scan_field_s *field)
-{
-       h_u32_to_be(((u8*)priv), flip_u32(le_to_h_u32(in_buf), 32));
-       return ERROR_OK;
-}
-
-/* read JTAG buffer into big-endian u16, flipping bit-order */
-int arm_jtag_buf_to_be16_flip(u8 *in_buf, void *priv, struct scan_field_s *field)
-{
-       h_u16_to_be(((u8*)priv), flip_u32(le_to_h_u32(in_buf), 32) & 0xffff);
-       return ERROR_OK;
-}
-
-/* read JTAG buffer into u8, flipping bit-order */
-int arm_jtag_buf_to_8_flip(u8 *in_buf, void *priv, struct scan_field_s *field)
-{
-       u8 *dest = priv;
-       *dest = flip_u32(le_to_h_u32(in_buf), 32) & 0xff;
-       return ERROR_OK;
-}
-
-/* not-flipping variants */
-/* read JTAG buffer into host-endian u32 */
-int arm_jtag_buf_to_u32(u8 *in_buf, void *priv, struct scan_field_s *field)
-{
-       u32 *dest = priv;
-       *dest = le_to_h_u32(in_buf);
-       return ERROR_OK;
-}
-
-/* read JTAG buffer into little-endian u32 */
-int arm_jtag_buf_to_le32(u8 *in_buf, void *priv, struct scan_field_s *field)
-{
-       h_u32_to_le(((u8*)priv), le_to_h_u32(in_buf));
-       return ERROR_OK;
-}
-
-/* read JTAG buffer into little-endian u16 */
-int arm_jtag_buf_to_le16(u8 *in_buf, void *priv, struct scan_field_s *field)
-{
-       h_u16_to_le(((u8*)priv), le_to_h_u32(in_buf) & 0xffff);
-       return ERROR_OK;
-}
-
-/* read JTAG buffer into big-endian u32 */
-int arm_jtag_buf_to_be32(u8 *in_buf, void *priv, struct scan_field_s *field)
-{
-       h_u32_to_be(((u8*)priv), le_to_h_u32(in_buf));
-       return ERROR_OK;
-}
-
-/* read JTAG buffer into big-endian u16 */
-int arm_jtag_buf_to_be16(u8 *in_buf, void *priv, struct scan_field_s *field)
-{
-       h_u16_to_be(((u8*)priv), le_to_h_u32(in_buf) & 0xffff);
-       return ERROR_OK;
-}
-
-/* read JTAG buffer into u8 */
-int arm_jtag_buf_to_8(u8 *in_buf, void *priv, struct scan_field_s *field)
-{
-       u8 *dest = priv;
-       *dest = le_to_h_u32(in_buf) & 0xff;
-       return ERROR_OK;
-}
+/***************************************************************************\r
+ *   Copyright (C) 2005 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "arm_jtag.h"\r
+\r
+#include "binarybuffer.h"\r
+#include "log.h"\r
+#include "jtag.h"\r
+\r
+#include <stdlib.h>\r
+\r
+#if 0\r
+#define _ARM_JTAG_SCAN_N_CHECK_\r
+#endif\r
+\r
+int arm_jtag_set_instr(arm_jtag_t *jtag_info, u32 new_instr,  in_handler_t handler)\r
+{\r
+       jtag_device_t *device = jtag_get_device(jtag_info->chain_pos);\r
+       \r
+       if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)\r
+       {\r
+               scan_field_t field;\r
+       \r
+               field.device = jtag_info->chain_pos;\r
+               field.num_bits = device->ir_length;\r
+               field.out_value = calloc(CEIL(field.num_bits, 8), 1);\r
+               buf_set_u32(field.out_value, 0, field.num_bits, new_instr);\r
+               field.out_mask = NULL;\r
+               field.in_value = NULL;\r
+               field.in_check_value = NULL;\r
+               field.in_check_mask = NULL;\r
+               field.in_handler = handler;\r
+               field.in_handler_priv = NULL;\r
+               jtag_add_ir_scan(1, &field, -1);\r
+               \r
+               \r
+               free(field.out_value);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm_jtag_scann(arm_jtag_t *jtag_info, u32 new_scan_chain)\r
+{\r
+       if(jtag_info->cur_scan_chain != new_scan_chain)\r
+       {\r
+#ifdef _ARM_JTAG_SCAN_N_CHECK_\r
+               u8 scan_n_check_value = 1 << (jtag_info->scann_size - 1);\r
+#endif\r
+               scan_field_t field;\r
+               \r
+               field.device = jtag_info->chain_pos;\r
+               field.num_bits = jtag_info->scann_size;\r
+               field.out_value = calloc(CEIL(field.num_bits, 8), 1);\r
+               buf_set_u32(field.out_value, 0, field.num_bits, new_scan_chain);\r
+               field.out_mask = NULL;\r
+               field.in_value = NULL;\r
+#ifdef _ARM_JTAG_SCAN_N_CHECK_\r
+               jtag_set_check_value(&field, &scan_n_check_value, NULL, NULL, NULL);\r
+#else\r
+               field.in_handler = NULL;\r
+               field.in_handler_priv = NULL;\r
+#endif \r
+               \r
+               \r
+               arm_jtag_set_instr(jtag_info, jtag_info->scann_instr, NULL);\r
+               jtag_add_dr_scan(1, &field, -1);\r
+               \r
+               jtag_info->cur_scan_chain = new_scan_chain;\r
+               \r
+               free(field.out_value);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm_jtag_reset_callback(enum jtag_event event, void *priv)\r
+{\r
+       arm_jtag_t *jtag_info = priv;\r
+       \r
+       if (event == JTAG_TRST_ASSERTED)\r
+       {\r
+               jtag_info->cur_scan_chain = 0;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int arm_jtag_setup_connection(arm_jtag_t *jtag_info)\r
+{\r
+       jtag_info->scann_instr = 0x2;\r
+       jtag_info->cur_scan_chain = 0;\r
+       jtag_info->intest_instr = 0xc;\r
+\r
+       jtag_register_event_callback(arm_jtag_reset_callback, jtag_info);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+/* read JTAG buffer into host-endian u32, flipping bit-order */\r
+int arm_jtag_buf_to_u32_flip(u8 *in_buf, void *priv, struct scan_field_s *field)\r
+{\r
+       u32 *dest = priv;\r
+       *dest = flip_u32(le_to_h_u32(in_buf), 32);\r
+       return ERROR_OK;\r
+}\r
+\r
+/* read JTAG buffer into little-endian u32, flipping bit-order */\r
+int arm_jtag_buf_to_le32_flip(u8 *in_buf, void *priv, struct scan_field_s *field)\r
+{\r
+       h_u32_to_le(((u8*)priv), flip_u32(le_to_h_u32(in_buf), 32));\r
+       return ERROR_OK;\r
+}\r
+\r
+/* read JTAG buffer into little-endian u16, flipping bit-order */\r
+int arm_jtag_buf_to_le16_flip(u8 *in_buf, void *priv, struct scan_field_s *field)\r
+{\r
+       h_u16_to_le(((u8*)priv), flip_u32(le_to_h_u32(in_buf), 32) & 0xffff);\r
+       return ERROR_OK;\r
+}\r
+\r
+/* read JTAG buffer into big-endian u32, flipping bit-order */\r
+int arm_jtag_buf_to_be32_flip(u8 *in_buf, void *priv, struct scan_field_s *field)\r
+{\r
+       h_u32_to_be(((u8*)priv), flip_u32(le_to_h_u32(in_buf), 32));\r
+       return ERROR_OK;\r
+}\r
+\r
+/* read JTAG buffer into big-endian u16, flipping bit-order */\r
+int arm_jtag_buf_to_be16_flip(u8 *in_buf, void *priv, struct scan_field_s *field)\r
+{\r
+       h_u16_to_be(((u8*)priv), flip_u32(le_to_h_u32(in_buf), 32) & 0xffff);\r
+       return ERROR_OK;\r
+}\r
+\r
+/* read JTAG buffer into u8, flipping bit-order */\r
+int arm_jtag_buf_to_8_flip(u8 *in_buf, void *priv, struct scan_field_s *field)\r
+{\r
+       u8 *dest = priv;\r
+       *dest = flip_u32(le_to_h_u32(in_buf), 32) & 0xff;\r
+       return ERROR_OK;\r
+}\r
+\r
+/* not-flipping variants */\r
+/* read JTAG buffer into host-endian u32 */\r
+int arm_jtag_buf_to_u32(u8 *in_buf, void *priv, struct scan_field_s *field)\r
+{\r
+       u32 *dest = priv;\r
+       *dest = le_to_h_u32(in_buf);\r
+       return ERROR_OK;\r
+}\r
+\r
+/* read JTAG buffer into little-endian u32 */\r
+int arm_jtag_buf_to_le32(u8 *in_buf, void *priv, struct scan_field_s *field)\r
+{\r
+       h_u32_to_le(((u8*)priv), le_to_h_u32(in_buf));\r
+       return ERROR_OK;\r
+}\r
+\r
+/* read JTAG buffer into little-endian u16 */\r
+int arm_jtag_buf_to_le16(u8 *in_buf, void *priv, struct scan_field_s *field)\r
+{\r
+       h_u16_to_le(((u8*)priv), le_to_h_u32(in_buf) & 0xffff);\r
+       return ERROR_OK;\r
+}\r
+\r
+/* read JTAG buffer into big-endian u32 */\r
+int arm_jtag_buf_to_be32(u8 *in_buf, void *priv, struct scan_field_s *field)\r
+{\r
+       h_u32_to_be(((u8*)priv), le_to_h_u32(in_buf));\r
+       return ERROR_OK;\r
+}\r
+\r
+/* read JTAG buffer into big-endian u16 */\r
+int arm_jtag_buf_to_be16(u8 *in_buf, void *priv, struct scan_field_s *field)\r
+{\r
+       h_u16_to_be(((u8*)priv), le_to_h_u32(in_buf) & 0xffff);\r
+       return ERROR_OK;\r
+}\r
+\r
+/* read JTAG buffer into u8 */\r
+int arm_jtag_buf_to_8(u8 *in_buf, void *priv, struct scan_field_s *field)\r
+{\r
+       u8 *dest = priv;\r
+       *dest = le_to_h_u32(in_buf) & 0xff;\r
+       return ERROR_OK;\r
+}\r
index 8cefcbfd3930b8725bcb0814cae66d1bdade17cf..88b641e3e35c4219cd1ef71f68750fdbe8f935e0 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2006 by Magnus Lundin                                   *
- *   lundin@mlu.mine.nu                                                    *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-/***************************************************************************
- *                                                                         *
- * CoreSight (Light?) SerialWireJtagDebugPort                              *
- *                                                                         *
- * CoreSightâ„¢ DAP-Lite TRM, ARM DDI 0316A                                  *
- * Cortex-M3â„¢ TRM, ARM DDI 0337C                                            *
- *                                                                         *
-***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "replacements.h"
-
-#include "cortex_m3.h"
-#include "cortex_swjdp.h"
-#include "jtag.h"
-#include "log.h"
-#include <stdlib.h>
-
-/*
-
-Transaction Mode:
-swjdp->trans_mode = TRANS_MODE_COMPOSITE;
-Uses Overrun checking mode and does not do actual JTAG send/receive or transaction 
-result checking until swjdp_end_transaction()
-This must be done before using or deallocating any return variables.
-
-swjdp->trans_mode == TRANS_MODE_ATOMIC
-All reads and writes to the AHB bus are checked for valid completion, and return values
-are immediatley available.
-
-*/
-
-/***************************************************************************
- *                                                                         *
- * DPACC and APACC scanchain access through JTAG-DR                        *
- *                                                                         *
-***************************************************************************/
-
-/* Scan out and in from target ordered u8 buffers */
-int swjdp_scan(arm_jtag_t *jtag_info, u8 instr, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue, u8 *ack)
-{
-       scan_field_t fields[2];
-       u8 out_addr_buf;
-       
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_set_instr(jtag_info, instr, NULL);
-
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 3;
-       buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));
-       fields[0].out_value = &out_addr_buf;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = ack;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 32;
-       fields[1].out_value = outvalue;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = invalue;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-
-       jtag_add_dr_scan(2, fields, -1, NULL);
-
-       return ERROR_OK;
-}
-
-/* Scan out and in from host ordered u32 variables */
-int swjdp_scan_u32(arm_jtag_t *jtag_info, u8 instr, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue, u8 *ack)
-{
-       scan_field_t fields[2];
-       u8 out_value_buf[4];
-       u8 out_addr_buf;
-       
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_set_instr(jtag_info, instr, NULL);
-
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 3;
-       buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));
-       fields[0].out_value = &out_addr_buf;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = ack;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 32;
-       buf_set_u32(out_value_buf, 0, 32, outvalue);
-       fields[1].out_value = out_value_buf;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       if (invalue)
-       {
-               fields[1].in_handler = arm_jtag_buf_to_u32;
-               fields[1].in_handler_priv = invalue;
-       }
-       else
-       {
-               fields[1].in_handler = NULL;
-               fields[1].in_handler_priv = NULL;
-       }
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-
-       jtag_add_dr_scan(2, fields, -1, NULL);
-
-       return ERROR_OK;
-}
-
-/* scan_inout_check adds one extra inscan for DPAP_READ commands to read variables */ 
-int scan_inout_check(swjdp_common_t *swjdp, u8 instr, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue)
-{
-       swjdp_scan(swjdp->jtag_info, instr, reg_addr, RnW, outvalue, NULL, NULL);
-       if ((RnW == DPAP_READ) && (invalue != NULL))
-       {
-               swjdp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, 0xC, DPAP_READ, 0, invalue, &swjdp->ack);
-       }
-       
-       /* In TRANS_MODE_ATOMIC all SWJDP_IR_APACC transactions wait for ack=OK/FAULT and the check CTRL_STAT */
-       if ((instr == SWJDP_IR_APACC) && (swjdp->trans_mode == TRANS_MODE_ATOMIC))
-       {
-               return swjdp_transaction_endcheck(swjdp);
-       }
-
-       return ERROR_OK;
-}
-
-int scan_inout_check_u32(swjdp_common_t *swjdp, u8 instr, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue)
-{
-
-       swjdp_scan_u32(swjdp->jtag_info, instr, reg_addr, RnW, outvalue, NULL, NULL);
-       if ((RnW==DPAP_READ) && (invalue != NULL))
-       {
-               swjdp_scan_u32(swjdp->jtag_info, SWJDP_IR_DPACC, 0xC, DPAP_READ, 0, invalue, &swjdp->ack);
-       }
-       
-       /* In TRANS_MODE_ATOMIC all SWJDP_IR_APACC transactions wait for ack=OK/FAULT and then check CTRL_STAT */
-       if ((instr == SWJDP_IR_APACC) && (swjdp->trans_mode == TRANS_MODE_ATOMIC))
-       {
-               return swjdp_transaction_endcheck(swjdp);
-       }
-
-       return ERROR_OK;
-}
-
-int swjdp_transaction_endcheck(swjdp_common_t *swjdp)
-{
-       int waitcount = 0;
-       u32 ctrlstat;
-
-       scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
-       scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
-       jtag_execute_queue();
-       
-       swjdp->ack = swjdp->ack & 0x7;
-       
-       while (swjdp->ack != 2)
-       {
-               if (swjdp->ack == 1)
-               {
-                       waitcount++;
-                       if (waitcount > 100)
-                       {
-                               WARNING("Timeout waiting for ACK = OK/FAULT in SWJDP transaction");
-
-                               return ERROR_JTAG_DEVICE_ERROR;
-                       }
-               }
-               else
-               {
-                       WARNING("Invalid ACK in SWJDP transaction");
-                       return ERROR_JTAG_DEVICE_ERROR;
-               }
-
-               scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
-               jtag_execute_queue();
-               swjdp->ack = swjdp->ack & 0x7;
-       }
-
-       /* Check for STICKYERR and STICKYORUN */
-       if (ctrlstat & (SSTICKYORUN | SSTICKYERR))
-       {
-               DEBUG("swjdp: CTRL/STAT error 0x%x", ctrlstat);
-               /* Check power to debug regions */
-               if ((ctrlstat & 0xf0000000) != 0xf0000000)
-               {
-                        ahbap_debugport_init(swjdp);
-               }
-               else
-               {
-                       u32 dcb_dhcsr,nvic_shcsr, nvic_bfar, nvic_cfsr;
-                       
-                       if (ctrlstat & SSTICKYORUN)
-                               ERROR("SWJ-DP OVERRUN - check clock or reduce jtag speed");
-                       
-                       if (ctrlstat & SSTICKYERR)
-                               ERROR("SWJ-DP STICKY ERROR");
-                       
-                       /* Clear Sticky Error Bits */
-                       scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_WRITE, swjdp->dp_ctrl_stat | SSTICKYORUN | SSTICKYERR, NULL);
-                       scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
-                       jtag_execute_queue();
-
-                       DEBUG("swjdp: status 0x%x", ctrlstat);
-                       
-                       /* Can we find out the reason for the error ?? */                       
-                       ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &dcb_dhcsr);
-                       ahbap_read_system_atomic_u32(swjdp, NVIC_SHCSR, &nvic_shcsr);
-                       ahbap_read_system_atomic_u32(swjdp, NVIC_CFSR, &nvic_cfsr);
-                       ahbap_read_system_atomic_u32(swjdp, NVIC_BFAR, &nvic_bfar);
-                       ERROR("dcb_dhcsr 0x%x, nvic_shcsr 0x%x, nvic_cfsr 0x%x, nvic_bfar 0x%x", dcb_dhcsr, nvic_shcsr, nvic_cfsr, nvic_bfar);
-               }
-               jtag_execute_queue();
-               return ERROR_JTAG_DEVICE_ERROR;
-       }
-
-       return ERROR_OK;
-}
-
-/***************************************************************************
- *                                                                         *
- * DP and AHB-AP  register access  through APACC and DPACC                 *
- *                                                                         *
-***************************************************************************/
-
-int swjdp_write_dpacc(swjdp_common_t *swjdp, u32 value, u8 reg_addr)
-{
-       u8 out_value_buf[4];
-       
-       buf_set_u32(out_value_buf, 0, 32, value);
-       return scan_inout_check(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
-}
-
-int swjdp_read_dpacc(swjdp_common_t *swjdp, u32 *value, u8 reg_addr)
-{
-       scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_READ, 0, value);
-
-    return ERROR_OK;
-}
-
-int swjdp_bankselect_apacc(swjdp_common_t *swjdp,u32 reg_addr)
-{
-       u32 select;
-       select = (reg_addr & 0xFF0000F0);
-
-       if (select != swjdp->dp_select_value)
-       {
-               swjdp_write_dpacc(swjdp, select, DP_SELECT);
-               swjdp->dp_select_value = select;
-       }
-
-       return ERROR_OK;
-}
-
-int ahbap_write_reg(swjdp_common_t *swjdp, u32 reg_addr, u8* out_value_buf)
-{
-       swjdp_bankselect_apacc(swjdp, reg_addr);
-       scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
-
-       return ERROR_OK;
-}
-
-int ahbap_read_reg(swjdp_common_t *swjdp, u32 reg_addr, u8 *in_value_buf)
-{
-       swjdp_bankselect_apacc(swjdp, reg_addr);
-       scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, in_value_buf);
-
-       return ERROR_OK;
-}
-int ahbap_write_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 value)
-{
-       u8 out_value_buf[4];
-       
-       buf_set_u32(out_value_buf, 0, 32, value);
-       swjdp_bankselect_apacc(swjdp, reg_addr);
-       scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
-
-       return ERROR_OK;
-}
-
-int ahbap_read_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 *value)
-{
-       swjdp_bankselect_apacc(swjdp, reg_addr);
-       scan_inout_check_u32(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, value);
-
-       return ERROR_OK;
-}
-
-/***************************************************************************
- *                                                                         *
- * AHB-AP access to memory and system registers on AHB bus                 *
- *                                                                         *
-***************************************************************************/
-
-int ahbap_setup_accessport(swjdp_common_t *swjdp, u32 csw, u32 tar)
-{
-       csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT;
-       if (csw != swjdp->ap_csw_value)
-       {
-               //DEBUG("swjdp : Set CSW %x",csw);
-               ahbap_write_reg_u32(swjdp, AHBAP_CSW, csw ); 
-               swjdp->ap_csw_value = csw;
-       }
-       if (tar != swjdp->ap_tar_value)
-       {
-               //DEBUG("swjdp : Set TAR %x",tar);
-               ahbap_write_reg_u32(swjdp, AHBAP_TAR, tar );
-               swjdp->ap_tar_value = tar;
-       }
-       if (csw & CSW_ADDRINC_MASK)
-       {       
-               /* Do not cache TAR value when autoincrementing */      
-               swjdp->ap_tar_value = -1;
-       }
-       return ERROR_OK;
-}
-
-/*****************************************************************************
-*                                                                            *
-* ahbap_read_system_u32(swjdp_common_t *swjdp, u32 address, u32 *value)      *
-*                                                                            *
-* Read a u32 value from memory or system register                            *
-* Functionally equivalent to target_read_u32(target, address, u32 *value),   *
-* but with less overhead                                                     *
-*****************************************************************************/
-int ahbap_read_system_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
-{
-       swjdp->trans_mode = TRANS_MODE_COMPOSITE;
-
-       ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
-       ahbap_read_reg_u32(swjdp, AHBAP_BD0 | (address & 0xC), value );
-       
-       return ERROR_OK;
-}
-
-int ahbap_read_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
-{
-       ahbap_read_system_u32(swjdp, address, value);
-       
-       return swjdp_transaction_endcheck(swjdp);
-}
-
-/*****************************************************************************
-*                                                                            *
-* ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)      *
-*                                                                            *
-* Write a u32 value to memory or system register                             *
-*                                                                            *
-*****************************************************************************/
-int ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)
-{
-       swjdp->trans_mode = TRANS_MODE_COMPOSITE;
-
-       ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
-       ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (address & 0xC), value );
-
-       return ERROR_OK;
-}
-
-int ahbap_write_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 value)
-{
-       ahbap_write_system_u32(swjdp, address, value);
-       
-       return swjdp_transaction_endcheck(swjdp);
-}
-
-/*****************************************************************************
-*                                                                            *
-* ahbap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address) *
-*                                                                            *
-* Write a buffer in target order (little endian)                             *
-*                                                                            *
-*****************************************************************************/
-int ahbap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
-{
-       u32 outvalue;
-       int wcount, blocksize, writecount, errorcount = 0, retval = ERROR_OK;
-
-       swjdp->trans_mode = TRANS_MODE_COMPOSITE;
-
-       while ((address & 0x3) && (count > 0))
-       {
-               ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
-               outvalue = (*buffer++) << 8 * (address & 0x3);
-               ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
-               swjdp_transaction_endcheck(swjdp);
-               count--;
-               address++;
-       }
-       wcount = count >> 2;
-       count = count - 4 * wcount;
-       while (wcount > 0)
-       {
-               /* Adjust to write blocks within 4K aligned boundaries */
-               blocksize = (0x1000 - (0xFFF & address)) >> 2;
-               if (wcount < blocksize)
-                       blocksize = wcount;
-               ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
-               for (writecount=0; writecount<blocksize; writecount++)
-               {
-                       ahbap_write_reg(swjdp, AHBAP_DRW, buffer + 4 * writecount );
-               }
-               if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
-               {
-                       wcount = wcount - blocksize;
-                       address = address + 4 * blocksize;
-                       buffer = buffer + 4 * blocksize;
-               }
-               else
-               {
-                       errorcount++;
-               }
-               if (errorcount > 1)
-               {
-                       WARNING("Block write error address 0x%x, wcount 0x%x", address, wcount);
-                       return ERROR_JTAG_DEVICE_ERROR;
-               }
-       }
-       
-       while (count > 0)
-       {
-               ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
-               outvalue = (*buffer++) << 8 * (address & 0x3);
-               ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
-               retval = swjdp_transaction_endcheck(swjdp);
-               count--;
-               address++;
-       }
-
-       return retval;
-}
-
-int ahbap_write_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
-{
-       u32 outvalue;
-       int retval = ERROR_OK;
-       
-       swjdp->trans_mode = TRANS_MODE_COMPOSITE;
-       
-       while (count > 0)
-       {
-               ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
-               outvalue = *((u16*)buffer) << 8 * (address & 0x3);
-               ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );
-               retval = swjdp_transaction_endcheck(swjdp);
-               count -= 2;
-               address += 2;
-               buffer += 2;
-       }
-
-       return retval;
-}
-
-/*****************************************************************************
-*                                                                            *
-* ahbap_read_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)  *
-*                                                                            *
-* Read block fast in target order (little endian) into a buffer       *
-*                                                                            *
-*****************************************************************************/
-int ahbap_read_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
-{
-       u32 invalue;
-       int wcount, blocksize, readcount, errorcount = 0, retval = ERROR_OK;
-
-       swjdp->trans_mode = TRANS_MODE_COMPOSITE;
-
-       while ((address & 0x3) && (count > 0))
-       {
-               ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
-               ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue);
-               swjdp_transaction_endcheck(swjdp);
-               *buffer++ = (invalue >> 8 * (address & 0x3)) & 0xFF;
-               count--;
-               address++;
-       }
-       wcount = count >> 2;
-       count = count - 4 * wcount;
-       while (wcount > 0)
-       {
-               /* Adjust to read within 4K block boundaries */
-               blocksize = (0x1000 - (0xFFF & address)) >> 2;
-               if (wcount < blocksize)
-                       blocksize = wcount;
-               ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
-               /* Scan out first read */
-               swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, NULL, NULL);
-               for (readcount = 0; readcount < blocksize - 1; readcount++)
-               {
-                       /* Scan out read instruction and scan in previous value */
-                       swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
-               }
-               /* Scan in last value */
-               swjdp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, 0xC, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
-               if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
-               {
-                       wcount = wcount - blocksize;
-                       address += 4 * blocksize;
-                       buffer += 4 * blocksize; 
-               }
-               else
-               {
-                       errorcount++;
-               }
-               if (errorcount > 1)
-               {
-                       WARNING("Block read error address 0x%x, count 0x%x", address, count);
-                       return ERROR_JTAG_DEVICE_ERROR;
-               }
-       }
-
-       while (count > 0)
-       {
-               ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
-               ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
-               retval = swjdp_transaction_endcheck(swjdp);
-               *buffer++ = (invalue >> 8 * (address & 0x3)) & 0xFF;
-               count--;
-               address++;
-       }
-
-       return retval;
-}
-
-int ahbap_read_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
-{
-       u32 invalue;
-       int retval = ERROR_OK;
-       
-       swjdp->trans_mode = TRANS_MODE_COMPOSITE;
-       
-       while (count > 0)
-       {
-               ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
-               ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );
-               retval = swjdp_transaction_endcheck(swjdp);
-               *((u16*)buffer) = (invalue >> 8 * (address & 0x3));
-               count -= 2;
-               address += 2;
-               buffer += 2;
-       }
-
-       return retval;
-}
-
-int ahbap_block_read_u32(swjdp_common_t *swjdp, u32 *buffer, int count, u32 address)
-{
-       int readcount, errorcount = 0;
-       u32 blocksize;
-       
-       swjdp->trans_mode = TRANS_MODE_COMPOSITE;
-       
-       while (count > 0)
-       {
-               /* Adjust to read within 4K block boundaries */
-               blocksize = (0x1000 - (0xFFF & address)) >> 2;
-               if (count < blocksize)
-                       blocksize = count;
-               ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
-               for (readcount = 0; readcount < blocksize; readcount++)
-               {
-                       ahbap_read_reg_u32(swjdp, AHBAP_DRW, buffer + readcount );
-               }
-               if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
-               {
-                       count = count - blocksize;
-                       address = address + 4 * blocksize;
-                       buffer = buffer + blocksize;
-               }
-               else
-               {
-                       errorcount++;
-               }
-               if (errorcount > 1)
-               {
-                       WARNING("Block read error address 0x%x, count 0x%x", address, count);
-                       return ERROR_JTAG_DEVICE_ERROR;
-               }
-       }
-
-       return ERROR_OK;
-}
-
-int ahbap_read_coreregister_u32(swjdp_common_t *swjdp, u32 *value, int regnum)
-{
-       int retval;
-       u32 dcrdr;
-       
-       ahbap_read_system_atomic_u32(swjdp, DCB_DCRDR, &dcrdr);
-       
-       swjdp->trans_mode = TRANS_MODE_COMPOSITE;
-
-       /* ahbap_write_system_u32(swjdp, DCB_DCRSR, regnum); */
-       ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
-       ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRSR & 0xC), regnum );
-
-       /* ahbap_read_system_u32(swjdp, DCB_DCRDR, value); */
-       ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
-       ahbap_read_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRDR & 0xC), value );
-       
-       retval = swjdp_transaction_endcheck(swjdp);
-       ahbap_write_system_atomic_u32(swjdp, DCB_DCRDR, dcrdr);
-       return retval;
-}
-
-int ahbap_write_coreregister_u32(swjdp_common_t *swjdp, u32 value, int regnum)
-{
-       int retval;
-       u32 dcrdr;
-       
-       ahbap_read_system_atomic_u32(swjdp, DCB_DCRDR, &dcrdr);
-       
-       swjdp->trans_mode = TRANS_MODE_COMPOSITE;
-       
-       /* ahbap_write_system_u32(swjdp, DCB_DCRDR, core_regs[i]); */
-       ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
-       ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRDR & 0xC), value );
-
-       /* ahbap_write_system_u32(swjdp, DCB_DCRSR, i | DCRSR_WnR       ); */
-       ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
-       ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRSR & 0xC), regnum | DCRSR_WnR );
-       
-       retval = swjdp_transaction_endcheck(swjdp);
-       ahbap_write_system_atomic_u32(swjdp, DCB_DCRDR, dcrdr);
-       return retval;
-}
-
-int ahbap_debugport_init(swjdp_common_t *swjdp)
-{
-       u32 idreg, romaddr, dummy;
-       u32 ctrlstat;
-       int cnt = 0;
-       
-       DEBUG(" ");
-       
-       swjdp->ap_csw_value = -1;
-       swjdp->ap_tar_value = -1;
-       swjdp->trans_mode = TRANS_MODE_ATOMIC;
-       swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
-       swjdp_write_dpacc(swjdp, SSTICKYERR, DP_CTRL_STAT);
-       swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
-       
-       swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
-
-       swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
-       swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
-       jtag_execute_queue();
-
-       /* Check that we have debug power domains activated */
-       while (!(ctrlstat & CDBGPWRUPACK) && (cnt++ < 10))
-       {
-               DEBUG("swjdp: wait CDBGPWRUPACK");
-               swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
-               jtag_execute_queue();
-               usleep(10000);
-       }
-
-       while (!(ctrlstat & CSYSPWRUPACK) && (cnt++ < 10))
-       {
-               DEBUG("swjdp: wait CSYSPWRUPACK");
-               swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);
-               jtag_execute_queue();
-               usleep(10000);
-       }
-
-       swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
-       /* With debug power on we can activate OVERRUN checking */
-       swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
-       swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat , DP_CTRL_STAT);
-       swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);
-       
-       ahbap_read_reg_u32(swjdp, 0xFC, &idreg);
-       ahbap_read_reg_u32(swjdp, 0xF8, &romaddr);
-       
-       DEBUG("AHB-AP ID Register 0x%x, Debug ROM Address 0x%x", idreg, romaddr);       
-       
-       return ERROR_OK;
-}
+/***************************************************************************\r
+ *   Copyright (C) 2006 by Magnus Lundin                                   *\r
+ *   lundin@mlu.mine.nu                                                    *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+/***************************************************************************\r
+ *                                                                         *\r
+ * CoreSight (Light?) SerialWireJtagDebugPort                              *\r
+ *                                                                         *\r
+ * CoreSightâ„¢ DAP-Lite TRM, ARM DDI 0316A                                  *\r
+ * Cortex-M3â„¢ TRM, ARM DDI 0337C                                            *\r
+ *                                                                         *\r
+***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "replacements.h"\r
+\r
+#include "cortex_m3.h"\r
+#include "cortex_swjdp.h"\r
+#include "jtag.h"\r
+#include "log.h"\r
+#include <stdlib.h>\r
+\r
+/*\r
+\r
+Transaction Mode:\r
+swjdp->trans_mode = TRANS_MODE_COMPOSITE;\r
+Uses Overrun checking mode and does not do actual JTAG send/receive or transaction \r
+result checking until swjdp_end_transaction()\r
+This must be done before using or deallocating any return variables.\r
+\r
+swjdp->trans_mode == TRANS_MODE_ATOMIC\r
+All reads and writes to the AHB bus are checked for valid completion, and return values\r
+are immediatley available.\r
+\r
+*/\r
+\r
+/***************************************************************************\r
+ *                                                                         *\r
+ * DPACC and APACC scanchain access through JTAG-DR                        *\r
+ *                                                                         *\r
+***************************************************************************/\r
+\r
+/* Scan out and in from target ordered u8 buffers */\r
+int swjdp_scan(arm_jtag_t *jtag_info, u8 instr, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue, u8 *ack)\r
+{\r
+       scan_field_t fields[2];\r
+       u8 out_addr_buf;\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_set_instr(jtag_info, instr, NULL);\r
+\r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 3;\r
+       buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));\r
+       fields[0].out_value = &out_addr_buf;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = ack;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+\r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 32;\r
+       fields[1].out_value = outvalue;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = invalue;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+\r
+       jtag_add_dr_scan(2, fields, -1);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* Scan out and in from host ordered u32 variables */\r
+int swjdp_scan_u32(arm_jtag_t *jtag_info, u8 instr, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue, u8 *ack)\r
+{\r
+       scan_field_t fields[2];\r
+       u8 out_value_buf[4];\r
+       u8 out_addr_buf;\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_set_instr(jtag_info, instr, NULL);\r
+\r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 3;\r
+       buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));\r
+       fields[0].out_value = &out_addr_buf;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = ack;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+\r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 32;\r
+       buf_set_u32(out_value_buf, 0, 32, outvalue);\r
+       fields[1].out_value = out_value_buf;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       if (invalue)\r
+       {\r
+               fields[1].in_handler = arm_jtag_buf_to_u32;\r
+               fields[1].in_handler_priv = invalue;\r
+       }\r
+       else\r
+       {\r
+               fields[1].in_handler = NULL;\r
+               fields[1].in_handler_priv = NULL;\r
+       }\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+\r
+       jtag_add_dr_scan(2, fields, -1);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* scan_inout_check adds one extra inscan for DPAP_READ commands to read variables */ \r
+int scan_inout_check(swjdp_common_t *swjdp, u8 instr, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue)\r
+{\r
+       swjdp_scan(swjdp->jtag_info, instr, reg_addr, RnW, outvalue, NULL, NULL);\r
+       if ((RnW == DPAP_READ) && (invalue != NULL))\r
+       {\r
+               swjdp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, 0xC, DPAP_READ, 0, invalue, &swjdp->ack);\r
+       }\r
+       \r
+       /* In TRANS_MODE_ATOMIC all SWJDP_IR_APACC transactions wait for ack=OK/FAULT and the check CTRL_STAT */\r
+       if ((instr == SWJDP_IR_APACC) && (swjdp->trans_mode == TRANS_MODE_ATOMIC))\r
+       {\r
+               return swjdp_transaction_endcheck(swjdp);\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int scan_inout_check_u32(swjdp_common_t *swjdp, u8 instr, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue)\r
+{\r
+\r
+       swjdp_scan_u32(swjdp->jtag_info, instr, reg_addr, RnW, outvalue, NULL, NULL);\r
+       if ((RnW==DPAP_READ) && (invalue != NULL))\r
+       {\r
+               swjdp_scan_u32(swjdp->jtag_info, SWJDP_IR_DPACC, 0xC, DPAP_READ, 0, invalue, &swjdp->ack);\r
+       }\r
+       \r
+       /* In TRANS_MODE_ATOMIC all SWJDP_IR_APACC transactions wait for ack=OK/FAULT and then check CTRL_STAT */\r
+       if ((instr == SWJDP_IR_APACC) && (swjdp->trans_mode == TRANS_MODE_ATOMIC))\r
+       {\r
+               return swjdp_transaction_endcheck(swjdp);\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int swjdp_transaction_endcheck(swjdp_common_t *swjdp)\r
+{\r
+       int waitcount = 0;\r
+       u32 ctrlstat;\r
+\r
+       scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);\r
+       scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);\r
+       jtag_execute_queue();\r
+       \r
+       swjdp->ack = swjdp->ack & 0x7;\r
+       \r
+       while (swjdp->ack != 2)\r
+       {\r
+               if (swjdp->ack == 1)\r
+               {\r
+                       waitcount++;\r
+                       if (waitcount > 100)\r
+                       {\r
+                               WARNING("Timeout waiting for ACK = OK/FAULT in SWJDP transaction");\r
+\r
+                               return ERROR_JTAG_DEVICE_ERROR;\r
+                       }\r
+               }\r
+               else\r
+               {\r
+                       WARNING("Invalid ACK in SWJDP transaction");\r
+                       return ERROR_JTAG_DEVICE_ERROR;\r
+               }\r
+\r
+               scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);\r
+               jtag_execute_queue();\r
+               swjdp->ack = swjdp->ack & 0x7;\r
+       }\r
+\r
+       /* Check for STICKYERR and STICKYORUN */\r
+       if (ctrlstat & (SSTICKYORUN | SSTICKYERR))\r
+       {\r
+               DEBUG("swjdp: CTRL/STAT error 0x%x", ctrlstat);\r
+               /* Check power to debug regions */\r
+               if ((ctrlstat & 0xf0000000) != 0xf0000000)\r
+               {\r
+                        ahbap_debugport_init(swjdp);\r
+               }\r
+               else\r
+               {\r
+                       u32 dcb_dhcsr,nvic_shcsr, nvic_bfar, nvic_cfsr;\r
+                       \r
+                       if (ctrlstat & SSTICKYORUN)\r
+                               ERROR("SWJ-DP OVERRUN - check clock or reduce jtag speed");\r
+                       \r
+                       if (ctrlstat & SSTICKYERR)\r
+                               ERROR("SWJ-DP STICKY ERROR");\r
+                       \r
+                       /* Clear Sticky Error Bits */\r
+                       scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_WRITE, swjdp->dp_ctrl_stat | SSTICKYORUN | SSTICKYERR, NULL);\r
+                       scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);\r
+                       jtag_execute_queue();\r
+\r
+                       DEBUG("swjdp: status 0x%x", ctrlstat);\r
+                       \r
+                       /* Can we find out the reason for the error ?? */                       \r
+                       ahbap_read_system_atomic_u32(swjdp, DCB_DHCSR, &dcb_dhcsr);\r
+                       ahbap_read_system_atomic_u32(swjdp, NVIC_SHCSR, &nvic_shcsr);\r
+                       ahbap_read_system_atomic_u32(swjdp, NVIC_CFSR, &nvic_cfsr);\r
+                       ahbap_read_system_atomic_u32(swjdp, NVIC_BFAR, &nvic_bfar);\r
+                       ERROR("dcb_dhcsr 0x%x, nvic_shcsr 0x%x, nvic_cfsr 0x%x, nvic_bfar 0x%x", dcb_dhcsr, nvic_shcsr, nvic_cfsr, nvic_bfar);\r
+               }\r
+               jtag_execute_queue();\r
+               return ERROR_JTAG_DEVICE_ERROR;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/***************************************************************************\r
+ *                                                                         *\r
+ * DP and AHB-AP  register access  through APACC and DPACC                 *\r
+ *                                                                         *\r
+***************************************************************************/\r
+\r
+int swjdp_write_dpacc(swjdp_common_t *swjdp, u32 value, u8 reg_addr)\r
+{\r
+       u8 out_value_buf[4];\r
+       \r
+       buf_set_u32(out_value_buf, 0, 32, value);\r
+       return scan_inout_check(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);\r
+}\r
+\r
+int swjdp_read_dpacc(swjdp_common_t *swjdp, u32 *value, u8 reg_addr)\r
+{\r
+       scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_READ, 0, value);\r
+\r
+    return ERROR_OK;\r
+}\r
+\r
+int swjdp_bankselect_apacc(swjdp_common_t *swjdp,u32 reg_addr)\r
+{\r
+       u32 select;\r
+       select = (reg_addr & 0xFF0000F0);\r
+\r
+       if (select != swjdp->dp_select_value)\r
+       {\r
+               swjdp_write_dpacc(swjdp, select, DP_SELECT);\r
+               swjdp->dp_select_value = select;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int ahbap_write_reg(swjdp_common_t *swjdp, u32 reg_addr, u8* out_value_buf)\r
+{\r
+       swjdp_bankselect_apacc(swjdp, reg_addr);\r
+       scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int ahbap_read_reg(swjdp_common_t *swjdp, u32 reg_addr, u8 *in_value_buf)\r
+{\r
+       swjdp_bankselect_apacc(swjdp, reg_addr);\r
+       scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, in_value_buf);\r
+\r
+       return ERROR_OK;\r
+}\r
+int ahbap_write_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 value)\r
+{\r
+       u8 out_value_buf[4];\r
+       \r
+       buf_set_u32(out_value_buf, 0, 32, value);\r
+       swjdp_bankselect_apacc(swjdp, reg_addr);\r
+       scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int ahbap_read_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 *value)\r
+{\r
+       swjdp_bankselect_apacc(swjdp, reg_addr);\r
+       scan_inout_check_u32(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, value);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/***************************************************************************\r
+ *                                                                         *\r
+ * AHB-AP access to memory and system registers on AHB bus                 *\r
+ *                                                                         *\r
+***************************************************************************/\r
+\r
+int ahbap_setup_accessport(swjdp_common_t *swjdp, u32 csw, u32 tar)\r
+{\r
+       csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT;\r
+       if (csw != swjdp->ap_csw_value)\r
+       {\r
+               //DEBUG("swjdp : Set CSW %x",csw);\r
+               ahbap_write_reg_u32(swjdp, AHBAP_CSW, csw ); \r
+               swjdp->ap_csw_value = csw;\r
+       }\r
+       if (tar != swjdp->ap_tar_value)\r
+       {\r
+               //DEBUG("swjdp : Set TAR %x",tar);\r
+               ahbap_write_reg_u32(swjdp, AHBAP_TAR, tar );\r
+               swjdp->ap_tar_value = tar;\r
+       }\r
+       if (csw & CSW_ADDRINC_MASK)\r
+       {       \r
+               /* Do not cache TAR value when autoincrementing */      \r
+               swjdp->ap_tar_value = -1;\r
+       }\r
+       return ERROR_OK;\r
+}\r
+\r
+/*****************************************************************************\r
+*                                                                            *\r
+* ahbap_read_system_u32(swjdp_common_t *swjdp, u32 address, u32 *value)      *\r
+*                                                                            *\r
+* Read a u32 value from memory or system register                            *\r
+* Functionally equivalent to target_read_u32(target, address, u32 *value),   *\r
+* but with less overhead                                                     *\r
+*****************************************************************************/\r
+int ahbap_read_system_u32(swjdp_common_t *swjdp, u32 address, u32 *value)\r
+{\r
+       swjdp->trans_mode = TRANS_MODE_COMPOSITE;\r
+\r
+       ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);\r
+       ahbap_read_reg_u32(swjdp, AHBAP_BD0 | (address & 0xC), value );\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int ahbap_read_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 *value)\r
+{\r
+       ahbap_read_system_u32(swjdp, address, value);\r
+       \r
+       return swjdp_transaction_endcheck(swjdp);\r
+}\r
+\r
+/*****************************************************************************\r
+*                                                                            *\r
+* ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)      *\r
+*                                                                            *\r
+* Write a u32 value to memory or system register                             *\r
+*                                                                            *\r
+*****************************************************************************/\r
+int ahbap_write_system_u32(swjdp_common_t *swjdp, u32 address, u32 value)\r
+{\r
+       swjdp->trans_mode = TRANS_MODE_COMPOSITE;\r
+\r
+       ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);\r
+       ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (address & 0xC), value );\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int ahbap_write_system_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 value)\r
+{\r
+       ahbap_write_system_u32(swjdp, address, value);\r
+       \r
+       return swjdp_transaction_endcheck(swjdp);\r
+}\r
+\r
+/*****************************************************************************\r
+*                                                                            *\r
+* ahbap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address) *\r
+*                                                                            *\r
+* Write a buffer in target order (little endian)                             *\r
+*                                                                            *\r
+*****************************************************************************/\r
+int ahbap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)\r
+{\r
+       u32 outvalue;\r
+       int wcount, blocksize, writecount, errorcount = 0, retval = ERROR_OK;\r
+\r
+       swjdp->trans_mode = TRANS_MODE_COMPOSITE;\r
+\r
+       while ((address & 0x3) && (count > 0))\r
+       {\r
+               ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);\r
+               outvalue = (*buffer++) << 8 * (address & 0x3);\r
+               ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );\r
+               swjdp_transaction_endcheck(swjdp);\r
+               count--;\r
+               address++;\r
+       }\r
+       wcount = count >> 2;\r
+       count = count - 4 * wcount;\r
+       while (wcount > 0)\r
+       {\r
+               /* Adjust to write blocks within 4K aligned boundaries */\r
+               blocksize = (0x1000 - (0xFFF & address)) >> 2;\r
+               if (wcount < blocksize)\r
+                       blocksize = wcount;\r
+               ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);\r
+               for (writecount=0; writecount<blocksize; writecount++)\r
+               {\r
+                       ahbap_write_reg(swjdp, AHBAP_DRW, buffer + 4 * writecount );\r
+               }\r
+               if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)\r
+               {\r
+                       wcount = wcount - blocksize;\r
+                       address = address + 4 * blocksize;\r
+                       buffer = buffer + 4 * blocksize;\r
+               }\r
+               else\r
+               {\r
+                       errorcount++;\r
+               }\r
+               if (errorcount > 1)\r
+               {\r
+                       WARNING("Block write error address 0x%x, wcount 0x%x", address, wcount);\r
+                       return ERROR_JTAG_DEVICE_ERROR;\r
+               }\r
+       }\r
+       \r
+       while (count > 0)\r
+       {\r
+               ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);\r
+               outvalue = (*buffer++) << 8 * (address & 0x3);\r
+               ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );\r
+               retval = swjdp_transaction_endcheck(swjdp);\r
+               count--;\r
+               address++;\r
+       }\r
+\r
+       return retval;\r
+}\r
+\r
+int ahbap_write_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)\r
+{\r
+       u32 outvalue;\r
+       int retval = ERROR_OK;\r
+       \r
+       swjdp->trans_mode = TRANS_MODE_COMPOSITE;\r
+       \r
+       while (count > 0)\r
+       {\r
+               ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);\r
+               outvalue = *((u16*)buffer) << 8 * (address & 0x3);\r
+               ahbap_write_reg_u32(swjdp, AHBAP_DRW, outvalue );\r
+               retval = swjdp_transaction_endcheck(swjdp);\r
+               count -= 2;\r
+               address += 2;\r
+               buffer += 2;\r
+       }\r
+\r
+       return retval;\r
+}\r
+\r
+/*****************************************************************************\r
+*                                                                            *\r
+* ahbap_read_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)  *\r
+*                                                                            *\r
+* Read block fast in target order (little endian) into a buffer       *\r
+*                                                                            *\r
+*****************************************************************************/\r
+int ahbap_read_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)\r
+{\r
+       u32 invalue;\r
+       int wcount, blocksize, readcount, errorcount = 0, retval = ERROR_OK;\r
+\r
+       swjdp->trans_mode = TRANS_MODE_COMPOSITE;\r
+\r
+       while ((address & 0x3) && (count > 0))\r
+       {\r
+               ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);\r
+               ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue);\r
+               swjdp_transaction_endcheck(swjdp);\r
+               *buffer++ = (invalue >> 8 * (address & 0x3)) & 0xFF;\r
+               count--;\r
+               address++;\r
+       }\r
+       wcount = count >> 2;\r
+       count = count - 4 * wcount;\r
+       while (wcount > 0)\r
+       {\r
+               /* Adjust to read within 4K block boundaries */\r
+               blocksize = (0x1000 - (0xFFF & address)) >> 2;\r
+               if (wcount < blocksize)\r
+                       blocksize = wcount;\r
+               ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);\r
+               /* Scan out first read */\r
+               swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, NULL, NULL);\r
+               for (readcount = 0; readcount < blocksize - 1; readcount++)\r
+               {\r
+                       /* Scan out read instruction and scan in previous value */\r
+                       swjdp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AHBAP_DRW, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);\r
+               }\r
+               /* Scan in last value */\r
+               swjdp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, 0xC, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);\r
+               if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)\r
+               {\r
+                       wcount = wcount - blocksize;\r
+                       address += 4 * blocksize;\r
+                       buffer += 4 * blocksize; \r
+               }\r
+               else\r
+               {\r
+                       errorcount++;\r
+               }\r
+               if (errorcount > 1)\r
+               {\r
+                       WARNING("Block read error address 0x%x, count 0x%x", address, count);\r
+                       return ERROR_JTAG_DEVICE_ERROR;\r
+               }\r
+       }\r
+\r
+       while (count > 0)\r
+       {\r
+               ahbap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);\r
+               ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );\r
+               retval = swjdp_transaction_endcheck(swjdp);\r
+               *buffer++ = (invalue >> 8 * (address & 0x3)) & 0xFF;\r
+               count--;\r
+               address++;\r
+       }\r
+\r
+       return retval;\r
+}\r
+\r
+int ahbap_read_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)\r
+{\r
+       u32 invalue;\r
+       int retval = ERROR_OK;\r
+       \r
+       swjdp->trans_mode = TRANS_MODE_COMPOSITE;\r
+       \r
+       while (count > 0)\r
+       {\r
+               ahbap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);\r
+               ahbap_read_reg_u32(swjdp, AHBAP_DRW, &invalue );\r
+               retval = swjdp_transaction_endcheck(swjdp);\r
+               *((u16*)buffer) = (invalue >> 8 * (address & 0x3));\r
+               count -= 2;\r
+               address += 2;\r
+               buffer += 2;\r
+       }\r
+\r
+       return retval;\r
+}\r
+\r
+int ahbap_block_read_u32(swjdp_common_t *swjdp, u32 *buffer, int count, u32 address)\r
+{\r
+       int readcount, errorcount = 0;\r
+       u32 blocksize;\r
+       \r
+       swjdp->trans_mode = TRANS_MODE_COMPOSITE;\r
+       \r
+       while (count > 0)\r
+       {\r
+               /* Adjust to read within 4K block boundaries */\r
+               blocksize = (0x1000 - (0xFFF & address)) >> 2;\r
+               if (count < blocksize)\r
+                       blocksize = count;\r
+               ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);\r
+               for (readcount = 0; readcount < blocksize; readcount++)\r
+               {\r
+                       ahbap_read_reg_u32(swjdp, AHBAP_DRW, buffer + readcount );\r
+               }\r
+               if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)\r
+               {\r
+                       count = count - blocksize;\r
+                       address = address + 4 * blocksize;\r
+                       buffer = buffer + blocksize;\r
+               }\r
+               else\r
+               {\r
+                       errorcount++;\r
+               }\r
+               if (errorcount > 1)\r
+               {\r
+                       WARNING("Block read error address 0x%x, count 0x%x", address, count);\r
+                       return ERROR_JTAG_DEVICE_ERROR;\r
+               }\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int ahbap_read_coreregister_u32(swjdp_common_t *swjdp, u32 *value, int regnum)\r
+{\r
+       int retval;\r
+       u32 dcrdr;\r
+       \r
+       ahbap_read_system_atomic_u32(swjdp, DCB_DCRDR, &dcrdr);\r
+       \r
+       swjdp->trans_mode = TRANS_MODE_COMPOSITE;\r
+\r
+       /* ahbap_write_system_u32(swjdp, DCB_DCRSR, regnum); */\r
+       ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);\r
+       ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRSR & 0xC), regnum );\r
+\r
+       /* ahbap_read_system_u32(swjdp, DCB_DCRDR, value); */\r
+       ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);\r
+       ahbap_read_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRDR & 0xC), value );\r
+       \r
+       retval = swjdp_transaction_endcheck(swjdp);\r
+       ahbap_write_system_atomic_u32(swjdp, DCB_DCRDR, dcrdr);\r
+       return retval;\r
+}\r
+\r
+int ahbap_write_coreregister_u32(swjdp_common_t *swjdp, u32 value, int regnum)\r
+{\r
+       int retval;\r
+       u32 dcrdr;\r
+       \r
+       ahbap_read_system_atomic_u32(swjdp, DCB_DCRDR, &dcrdr);\r
+       \r
+       swjdp->trans_mode = TRANS_MODE_COMPOSITE;\r
+       \r
+       /* ahbap_write_system_u32(swjdp, DCB_DCRDR, core_regs[i]); */\r
+       ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);\r
+       ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRDR & 0xC), value );\r
+\r
+       /* ahbap_write_system_u32(swjdp, DCB_DCRSR, i | DCRSR_WnR       ); */\r
+       ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);\r
+       ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRSR & 0xC), regnum | DCRSR_WnR );\r
+       \r
+       retval = swjdp_transaction_endcheck(swjdp);\r
+       ahbap_write_system_atomic_u32(swjdp, DCB_DCRDR, dcrdr);\r
+       return retval;\r
+}\r
+\r
+int ahbap_debugport_init(swjdp_common_t *swjdp)\r
+{\r
+       u32 idreg, romaddr, dummy;\r
+       u32 ctrlstat;\r
+       int cnt = 0;\r
+       \r
+       DEBUG(" ");\r
+       \r
+       swjdp->ap_csw_value = -1;\r
+       swjdp->ap_tar_value = -1;\r
+       swjdp->trans_mode = TRANS_MODE_ATOMIC;\r
+       swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);\r
+       swjdp_write_dpacc(swjdp, SSTICKYERR, DP_CTRL_STAT);\r
+       swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);\r
+       \r
+       swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;\r
+\r
+       swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);\r
+       swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);\r
+       jtag_execute_queue();\r
+\r
+       /* Check that we have debug power domains activated */\r
+       while (!(ctrlstat & CDBGPWRUPACK) && (cnt++ < 10))\r
+       {\r
+               DEBUG("swjdp: wait CDBGPWRUPACK");\r
+               swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);\r
+               jtag_execute_queue();\r
+               usleep(10000);\r
+       }\r
+\r
+       while (!(ctrlstat & CSYSPWRUPACK) && (cnt++ < 10))\r
+       {\r
+               DEBUG("swjdp: wait CSYSPWRUPACK");\r
+               swjdp_read_dpacc(swjdp, &ctrlstat, DP_CTRL_STAT);\r
+               jtag_execute_queue();\r
+               usleep(10000);\r
+       }\r
+\r
+       swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);\r
+       /* With debug power on we can activate OVERRUN checking */\r
+       swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;\r
+       swjdp_write_dpacc(swjdp, swjdp->dp_ctrl_stat , DP_CTRL_STAT);\r
+       swjdp_read_dpacc(swjdp, &dummy, DP_CTRL_STAT);\r
+       \r
+       ahbap_read_reg_u32(swjdp, 0xFC, &idreg);\r
+       ahbap_read_reg_u32(swjdp, 0xF8, &romaddr);\r
+       \r
+       DEBUG("AHB-AP ID Register 0x%x, Debug ROM Address 0x%x", idreg, romaddr);       \r
+       \r
+       return ERROR_OK;\r
+}\r
index 3713fe95261b79815af5dfcde9ec4e2cb7af1209..07b965b07ecfa2d3317955e863030cf00d30207d 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2005 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "embeddedice.h"
-
-#include "armv4_5.h"
-#include "arm7_9_common.h"
-
-#include "log.h"
-#include "arm_jtag.h"
-#include "types.h"
-#include "binarybuffer.h"
-#include "target.h"
-#include "register.h"
-#include "jtag.h"
-
-#include <stdlib.h>
-
-bitfield_desc_t embeddedice_comms_ctrl_bitfield_desc[] = 
-{
-       {"R", 1},
-       {"W", 1},
-       {"reserved", 26},
-       {"version", 4}
-};
-
-int embeddedice_reg_arch_info[] =
-{
-       0x0, 0x1, 0x4, 0x5,
-       0x8, 0x9, 0xa, 0xb, 0xc, 0xd,
-       0x10, 0x11, 0x12, 0x13, 0x14, 0x15,
-       0x2
-};
-
-char* embeddedice_reg_list[] =
-{
-       "debug_ctrl",
-       "debug_status",
-       
-       "comms_ctrl",
-       "comms_data",
-       
-       "watch 0 addr value",
-       "watch 0 addr mask",
-       "watch 0 data value",
-       "watch 0 data mask",
-       "watch 0 control value",
-       "watch 0 control mask",
-       
-       "watch 1 addr value",
-       "watch 1 addr mask",
-       "watch 1 data value",
-       "watch 1 data mask",
-       "watch 1 control value",
-       "watch 1 control mask",
-       
-       "vector catch"
-};
-
-int embeddedice_reg_arch_type = -1;
-
-int embeddedice_get_reg(reg_t *reg);
-int embeddedice_set_reg(reg_t *reg, u32 value);
-int embeddedice_set_reg_w_exec(reg_t *reg, u8 *buf);
-
-int embeddedice_write_reg(reg_t *reg, u32 value);
-int embeddedice_read_reg(reg_t *reg);
-
-reg_cache_t* embeddedice_build_reg_cache(target_t *target, arm7_9_common_t *arm7_9)
-{
-       reg_cache_t *reg_cache = malloc(sizeof(reg_cache_t));
-       reg_t *reg_list = NULL;
-       embeddedice_reg_t *arch_info = NULL;
-       arm_jtag_t *jtag_info = &arm7_9->jtag_info;
-       int num_regs;
-       int i;
-       int eice_version = 0;
-       
-       /* register a register arch-type for EmbeddedICE registers only once */
-       if (embeddedice_reg_arch_type == -1)
-               embeddedice_reg_arch_type = register_reg_arch_type(embeddedice_get_reg, embeddedice_set_reg_w_exec);
-       
-       if (arm7_9->has_vector_catch)
-               num_regs = 17;
-       else
-               num_regs = 16;
-               
-       /* the actual registers are kept in two arrays */
-       reg_list = calloc(num_regs, sizeof(reg_t));
-       arch_info = calloc(num_regs, sizeof(embeddedice_reg_t));
-       
-       /* fill in values for the reg cache */
-       reg_cache->name = "EmbeddedICE registers";
-       reg_cache->next = NULL;
-       reg_cache->reg_list = reg_list;
-       reg_cache->num_regs = num_regs;
-       
-       /* set up registers */
-       for (i = 0; i < num_regs; i++)
-       {
-               reg_list[i].name = embeddedice_reg_list[i];
-               reg_list[i].size = 32;
-               reg_list[i].dirty = 0;
-               reg_list[i].valid = 0;
-               reg_list[i].bitfield_desc = NULL;
-               reg_list[i].num_bitfields = 0;
-               reg_list[i].value = calloc(1, 4);
-               reg_list[i].arch_info = &arch_info[i];
-               reg_list[i].arch_type = embeddedice_reg_arch_type;
-               arch_info[i].addr = embeddedice_reg_arch_info[i];
-               arch_info[i].jtag_info = jtag_info;
-       }
-       
-       /* identify EmbeddedICE version by reading DCC control register */
-       embeddedice_read_reg(&reg_list[EICE_COMMS_CTRL]);
-       jtag_execute_queue();
-       
-       eice_version = buf_get_u32(reg_list[EICE_COMMS_CTRL].value, 28, 4);
-       
-       switch (eice_version)
-       {
-               case 1:
-                       reg_list[EICE_DBG_CTRL].size = 3;
-                       reg_list[EICE_DBG_STAT].size = 5;
-                       break;
-               case 2:
-                       reg_list[EICE_DBG_CTRL].size = 4;
-                       reg_list[EICE_DBG_STAT].size = 5;
-                       arm7_9->has_single_step = 1;
-                       break;
-               case 3:
-                       ERROR("EmbeddedICE version 3 detected, EmbeddedICE handling might be broken"); 
-                       reg_list[EICE_DBG_CTRL].size = 6;
-                       reg_list[EICE_DBG_STAT].size = 5;
-                       arm7_9->has_single_step = 1;
-                       arm7_9->has_monitor_mode = 1;
-                       break;
-               case 4:
-                       reg_list[EICE_DBG_CTRL].size = 6;
-                       reg_list[EICE_DBG_STAT].size = 5;
-                       arm7_9->has_monitor_mode = 1;
-                       break;
-               case 5:
-                       reg_list[EICE_DBG_CTRL].size = 6;
-                       reg_list[EICE_DBG_STAT].size = 5;
-                       arm7_9->has_single_step = 1;
-                       arm7_9->has_monitor_mode = 1;
-                       break;
-               case 6:
-                       reg_list[EICE_DBG_CTRL].size = 6;
-                       reg_list[EICE_DBG_STAT].size = 10;
-                       arm7_9->has_monitor_mode = 1;
-                       break;
-               case 7:
-                       WARNING("EmbeddedICE version 7 detected, EmbeddedICE handling might be broken");
-                       reg_list[EICE_DBG_CTRL].size = 6;
-                       reg_list[EICE_DBG_STAT].size = 5;
-                       arm7_9->has_monitor_mode = 1;
-                       break;
-               default:
-                       ERROR("unknown EmbeddedICE version (comms ctrl: 0x%8.8x)", buf_get_u32(reg_list[EICE_COMMS_CTRL].value, 0, 32));
-       }
-       
-       /* explicitly disable monitor mode */
-       if (arm7_9->has_monitor_mode)
-       {
-               embeddedice_read_reg(&reg_list[EICE_DBG_CTRL]);
-               jtag_execute_queue();
-               buf_set_u32(reg_list[EICE_DBG_CTRL].value, 4, 1, 0);
-               embeddedice_set_reg_w_exec(&reg_list[EICE_DBG_CTRL], reg_list[EICE_DBG_CTRL].value);
-       }
-       
-       return reg_cache;
-}
-
-int embeddedice_get_reg(reg_t *reg)
-{
-       if (embeddedice_read_reg(reg) != ERROR_OK)
-       {
-               ERROR("BUG: error scheduling EmbeddedICE register read");
-               exit(-1);
-       }
-       
-       if (jtag_execute_queue() != ERROR_OK)
-       {
-               ERROR("register read failed");
-       }
-       
-       return ERROR_OK;
-}
-
-int embeddedice_read_reg_w_check(reg_t *reg, u8* check_value, u8* check_mask)
-{
-       embeddedice_reg_t *ice_reg = reg->arch_info;
-       u8 reg_addr = ice_reg->addr & 0x1f;
-       scan_field_t fields[3];
-       u8 field1_out[1];
-       u8 field2_out[1];
-
-       DEBUG("%i", ice_reg->addr);
-
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_scann(ice_reg->jtag_info, 0x2);
-       
-       arm_jtag_set_instr(ice_reg->jtag_info, ice_reg->jtag_info->intest_instr, NULL);
-       
-       fields[0].device = ice_reg->jtag_info->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = reg->value;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-       
-       fields[1].device = ice_reg->jtag_info->chain_pos;
-       fields[1].num_bits = 5;
-       fields[1].out_value = field1_out;
-       buf_set_u32(fields[1].out_value, 0, 5, reg_addr);
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = ice_reg->jtag_info->chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = field2_out;
-       buf_set_u32(fields[2].out_value, 0, 1, 0);
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(3, fields, -1, NULL);
-       
-       fields[0].in_value = reg->value;
-       jtag_set_check_value(fields+0, check_value, check_mask, NULL);
-       
-       /* when reading the DCC data register, leaving the address field set to
-        * EICE_COMMS_DATA would read the register twice
-        * reading the control register is safe
-        */
-       buf_set_u32(fields[1].out_value, 0, 5, embeddedice_reg_arch_info[EICE_COMMS_CTRL]);
-       
-       jtag_add_dr_scan(3, fields, -1, NULL);
-
-       return ERROR_OK;
-}
-
-/* receive <size> words of 32 bit from the DCC
- * we pretend the target is always going to be fast enough
- * (relative to the JTAG clock), so we don't need to handshake
- */
-int embeddedice_receive(arm_jtag_t *jtag_info, u32 *data, u32 size)
-{
-       scan_field_t fields[3];
-       u8 field1_out[1];
-       u8 field2_out[1];
-
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_scann(jtag_info, 0x2);
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-       
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = NULL;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-       
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 5;
-       fields[1].out_value = field1_out;
-       buf_set_u32(fields[1].out_value, 0, 5, embeddedice_reg_arch_info[EICE_COMMS_DATA]);
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = jtag_info->chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = field2_out;
-       buf_set_u32(fields[2].out_value, 0, 1, 0);
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(3, fields, -1, NULL);
-       
-       while (size > 0)
-       {
-               /* when reading the last item, set the register address to the DCC control reg,
-                * to avoid reading additional data from the DCC data reg
-                */
-               if (size == 1)
-                       buf_set_u32(fields[1].out_value, 0, 5, embeddedice_reg_arch_info[EICE_COMMS_CTRL]);
-               
-               fields[0].in_handler = arm_jtag_buf_to_u32;
-               fields[0].in_handler_priv = data;
-               jtag_add_dr_scan(3, fields, -1, NULL);
-               
-               data++;
-               size--;
-       }
-       
-       return jtag_execute_queue();
-}
-
-int embeddedice_read_reg(reg_t *reg)
-{
-       return embeddedice_read_reg_w_check(reg, NULL, NULL);   
-}
-
-int embeddedice_set_reg(reg_t *reg, u32 value)
-{
-       if (embeddedice_write_reg(reg, value) != ERROR_OK)
-       {
-               ERROR("BUG: error scheduling EmbeddedICE register write");
-               exit(-1);
-       }
-       
-       buf_set_u32(reg->value, 0, reg->size, value);
-       reg->valid = 1;
-       reg->dirty = 0;
-       
-       return ERROR_OK;
-}
-
-int embeddedice_set_reg_w_exec(reg_t *reg, u8 *buf)
-{
-       embeddedice_set_reg(reg, buf_get_u32(buf, 0, reg->size));
-       
-       if (jtag_execute_queue() != ERROR_OK)
-       {
-               ERROR("register write failed");
-               exit(-1);
-       }
-       return ERROR_OK;
-}
-
-int embeddedice_write_reg(reg_t *reg, u32 value)
-{
-       embeddedice_reg_t *ice_reg = reg->arch_info;
-       u8 reg_addr = ice_reg->addr & 0x1f;
-       scan_field_t fields[3];
-       u8 field0_out[4];
-       u8 field1_out[1];
-       u8 field2_out[1];
-
-       DEBUG("%i: 0x%8.8x", ice_reg->addr, value);
-       
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_scann(ice_reg->jtag_info, 0x2);
-       
-       arm_jtag_set_instr(ice_reg->jtag_info, ice_reg->jtag_info->intest_instr, NULL);
-       
-       fields[0].device = ice_reg->jtag_info->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = field0_out;
-       buf_set_u32(fields[0].out_value, 0, 32, value);
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-       
-       fields[1].device = ice_reg->jtag_info->chain_pos;
-       fields[1].num_bits = 5;
-       fields[1].out_value = field1_out;
-       buf_set_u32(fields[1].out_value, 0, 5, reg_addr);
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = ice_reg->jtag_info->chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = field2_out;
-       buf_set_u32(fields[2].out_value, 0, 1, 1);
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(3, fields, -1, NULL);
-       
-       return ERROR_OK;
-}
-
-int embeddedice_store_reg(reg_t *reg)
-{
-       return embeddedice_write_reg(reg, buf_get_u32(reg->value, 0, reg->size));
-}
-
-/* send <size> words of 32 bit to the DCC
- * we pretend the target is always going to be fast enough
- * (relative to the JTAG clock), so we don't need to handshake
- */
-int embeddedice_send(arm_jtag_t *jtag_info, u32 *data, u32 size)
-{
-       scan_field_t fields[3];
-       u8 field0_out[4];
-       u8 field1_out[1];
-       u8 field2_out[1];
-
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_scann(jtag_info, 0x2);
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = field0_out;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 5;
-       fields[1].out_value = field1_out;
-       buf_set_u32(fields[1].out_value, 0, 5, embeddedice_reg_arch_info[EICE_COMMS_DATA]);
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = jtag_info->chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = field2_out;
-       buf_set_u32(fields[2].out_value, 0, 1, 1);
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-
-       while (size > 0)
-       {
-               buf_set_u32(fields[0].out_value, 0, 32, *data);
-               jtag_add_dr_scan(3, fields, -1, NULL);
-
-               data++;
-               size--;
-       }
-
-       /* call to jtag_execute_queue() intentionally omitted */
-       return ERROR_OK;
-}
-
-/* wait for DCC control register R/W handshake bit to become active
- */
-int embeddedice_handshake(arm_jtag_t *jtag_info, int hsbit, u32 timeout)
-{
-       scan_field_t fields[3];
-       u8 field0_in[4];
-       u8 field1_out[1];
-       u8 field2_out[1];
-       int retval;
-       int hsact;
-       struct timeval lap;
-       struct timeval now;
-
-       if (hsbit == EICE_COMM_CTRL_WBIT)
-               hsact = 1;
-       else if (hsbit == EICE_COMM_CTRL_RBIT)
-               hsact = 0;
-       else
-               return ERROR_INVALID_ARGUMENTS;
-
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_scann(jtag_info, 0x2);
-       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);
-
-       fields[0].device = jtag_info->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = NULL;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = field0_in;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-
-       fields[1].device = jtag_info->chain_pos;
-       fields[1].num_bits = 5;
-       fields[1].out_value = field1_out;
-       buf_set_u32(fields[1].out_value, 0, 5, embeddedice_reg_arch_info[EICE_COMMS_CTRL]);
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = jtag_info->chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = field2_out;
-       buf_set_u32(fields[2].out_value, 0, 1, 0);
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-
-       jtag_add_dr_scan(3, fields, -1, NULL);
-       gettimeofday(&lap, NULL);
-       do
-       {
-               jtag_add_dr_scan(3, fields, -1, NULL);
-               if ((retval = jtag_execute_queue()) != ERROR_OK)
-                       return retval;
-
-               if (buf_get_u32(field0_in, hsbit, 1) == hsact)
-                       return ERROR_OK;
-
-               gettimeofday(&now, NULL);
-       }
-       while ((now.tv_sec-lap.tv_sec)*1000 + (now.tv_usec-lap.tv_usec)/1000 <= timeout);
-
-       return ERROR_TARGET_TIMEOUT;
-}
+/***************************************************************************\r
+ *   Copyright (C) 2005 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "embeddedice.h"\r
+\r
+#include "armv4_5.h"\r
+#include "arm7_9_common.h"\r
+\r
+#include "log.h"\r
+#include "arm_jtag.h"\r
+#include "types.h"\r
+#include "binarybuffer.h"\r
+#include "target.h"\r
+#include "register.h"\r
+#include "jtag.h"\r
+\r
+#include <stdlib.h>\r
+\r
+bitfield_desc_t embeddedice_comms_ctrl_bitfield_desc[] = \r
+{\r
+       {"R", 1},\r
+       {"W", 1},\r
+       {"reserved", 26},\r
+       {"version", 4}\r
+};\r
+\r
+int embeddedice_reg_arch_info[] =\r
+{\r
+       0x0, 0x1, 0x4, 0x5,\r
+       0x8, 0x9, 0xa, 0xb, 0xc, 0xd,\r
+       0x10, 0x11, 0x12, 0x13, 0x14, 0x15,\r
+       0x2\r
+};\r
+\r
+char* embeddedice_reg_list[] =\r
+{\r
+       "debug_ctrl",\r
+       "debug_status",\r
+       \r
+       "comms_ctrl",\r
+       "comms_data",\r
+       \r
+       "watch 0 addr value",\r
+       "watch 0 addr mask",\r
+       "watch 0 data value",\r
+       "watch 0 data mask",\r
+       "watch 0 control value",\r
+       "watch 0 control mask",\r
+       \r
+       "watch 1 addr value",\r
+       "watch 1 addr mask",\r
+       "watch 1 data value",\r
+       "watch 1 data mask",\r
+       "watch 1 control value",\r
+       "watch 1 control mask",\r
+       \r
+       "vector catch"\r
+};\r
+\r
+int embeddedice_reg_arch_type = -1;\r
+\r
+int embeddedice_get_reg(reg_t *reg);\r
+int embeddedice_set_reg(reg_t *reg, u32 value);\r
+int embeddedice_set_reg_w_exec(reg_t *reg, u8 *buf);\r
+\r
+int embeddedice_write_reg(reg_t *reg, u32 value);\r
+int embeddedice_read_reg(reg_t *reg);\r
+\r
+reg_cache_t* embeddedice_build_reg_cache(target_t *target, arm7_9_common_t *arm7_9)\r
+{\r
+       reg_cache_t *reg_cache = malloc(sizeof(reg_cache_t));\r
+       reg_t *reg_list = NULL;\r
+       embeddedice_reg_t *arch_info = NULL;\r
+       arm_jtag_t *jtag_info = &arm7_9->jtag_info;\r
+       int num_regs;\r
+       int i;\r
+       int eice_version = 0;\r
+       \r
+       /* register a register arch-type for EmbeddedICE registers only once */\r
+       if (embeddedice_reg_arch_type == -1)\r
+               embeddedice_reg_arch_type = register_reg_arch_type(embeddedice_get_reg, embeddedice_set_reg_w_exec);\r
+       \r
+       if (arm7_9->has_vector_catch)\r
+               num_regs = 17;\r
+       else\r
+               num_regs = 16;\r
+               \r
+       /* the actual registers are kept in two arrays */\r
+       reg_list = calloc(num_regs, sizeof(reg_t));\r
+       arch_info = calloc(num_regs, sizeof(embeddedice_reg_t));\r
+       \r
+       /* fill in values for the reg cache */\r
+       reg_cache->name = "EmbeddedICE registers";\r
+       reg_cache->next = NULL;\r
+       reg_cache->reg_list = reg_list;\r
+       reg_cache->num_regs = num_regs;\r
+       \r
+       /* set up registers */\r
+       for (i = 0; i < num_regs; i++)\r
+       {\r
+               reg_list[i].name = embeddedice_reg_list[i];\r
+               reg_list[i].size = 32;\r
+               reg_list[i].dirty = 0;\r
+               reg_list[i].valid = 0;\r
+               reg_list[i].bitfield_desc = NULL;\r
+               reg_list[i].num_bitfields = 0;\r
+               reg_list[i].value = calloc(1, 4);\r
+               reg_list[i].arch_info = &arch_info[i];\r
+               reg_list[i].arch_type = embeddedice_reg_arch_type;\r
+               arch_info[i].addr = embeddedice_reg_arch_info[i];\r
+               arch_info[i].jtag_info = jtag_info;\r
+       }\r
+       \r
+       /* identify EmbeddedICE version by reading DCC control register */\r
+       embeddedice_read_reg(&reg_list[EICE_COMMS_CTRL]);\r
+       jtag_execute_queue();\r
+       \r
+       eice_version = buf_get_u32(reg_list[EICE_COMMS_CTRL].value, 28, 4);\r
+       \r
+       switch (eice_version)\r
+       {\r
+               case 1:\r
+                       reg_list[EICE_DBG_CTRL].size = 3;\r
+                       reg_list[EICE_DBG_STAT].size = 5;\r
+                       break;\r
+               case 2:\r
+                       reg_list[EICE_DBG_CTRL].size = 4;\r
+                       reg_list[EICE_DBG_STAT].size = 5;\r
+                       arm7_9->has_single_step = 1;\r
+                       break;\r
+               case 3:\r
+                       ERROR("EmbeddedICE version 3 detected, EmbeddedICE handling might be broken"); \r
+                       reg_list[EICE_DBG_CTRL].size = 6;\r
+                       reg_list[EICE_DBG_STAT].size = 5;\r
+                       arm7_9->has_single_step = 1;\r
+                       arm7_9->has_monitor_mode = 1;\r
+                       break;\r
+               case 4:\r
+                       reg_list[EICE_DBG_CTRL].size = 6;\r
+                       reg_list[EICE_DBG_STAT].size = 5;\r
+                       arm7_9->has_monitor_mode = 1;\r
+                       break;\r
+               case 5:\r
+                       reg_list[EICE_DBG_CTRL].size = 6;\r
+                       reg_list[EICE_DBG_STAT].size = 5;\r
+                       arm7_9->has_single_step = 1;\r
+                       arm7_9->has_monitor_mode = 1;\r
+                       break;\r
+               case 6:\r
+                       reg_list[EICE_DBG_CTRL].size = 6;\r
+                       reg_list[EICE_DBG_STAT].size = 10;\r
+                       arm7_9->has_monitor_mode = 1;\r
+                       break;\r
+               case 7:\r
+                       WARNING("EmbeddedICE version 7 detected, EmbeddedICE handling might be broken");\r
+                       reg_list[EICE_DBG_CTRL].size = 6;\r
+                       reg_list[EICE_DBG_STAT].size = 5;\r
+                       arm7_9->has_monitor_mode = 1;\r
+                       break;\r
+               default:\r
+                       ERROR("unknown EmbeddedICE version (comms ctrl: 0x%8.8x)", buf_get_u32(reg_list[EICE_COMMS_CTRL].value, 0, 32));\r
+       }\r
+       \r
+       /* explicitly disable monitor mode */\r
+       if (arm7_9->has_monitor_mode)\r
+       {\r
+               embeddedice_read_reg(&reg_list[EICE_DBG_CTRL]);\r
+               jtag_execute_queue();\r
+               buf_set_u32(reg_list[EICE_DBG_CTRL].value, 4, 1, 0);\r
+               embeddedice_set_reg_w_exec(&reg_list[EICE_DBG_CTRL], reg_list[EICE_DBG_CTRL].value);\r
+       }\r
+       \r
+       return reg_cache;\r
+}\r
+\r
+int embeddedice_get_reg(reg_t *reg)\r
+{\r
+       if (embeddedice_read_reg(reg) != ERROR_OK)\r
+       {\r
+               ERROR("BUG: error scheduling EmbeddedICE register read");\r
+               exit(-1);\r
+       }\r
+       \r
+       if (jtag_execute_queue() != ERROR_OK)\r
+       {\r
+               ERROR("register read failed");\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int embeddedice_read_reg_w_check(reg_t *reg, u8* check_value, u8* check_mask)\r
+{\r
+       embeddedice_reg_t *ice_reg = reg->arch_info;\r
+       u8 reg_addr = ice_reg->addr & 0x1f;\r
+       scan_field_t fields[3];\r
+       u8 field1_out[1];\r
+       u8 field2_out[1];\r
+\r
+       DEBUG("%i", ice_reg->addr);\r
+\r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_scann(ice_reg->jtag_info, 0x2);\r
+       \r
+       arm_jtag_set_instr(ice_reg->jtag_info, ice_reg->jtag_info->intest_instr, NULL);\r
+       \r
+       fields[0].device = ice_reg->jtag_info->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = reg->value;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+       \r
+       fields[1].device = ice_reg->jtag_info->chain_pos;\r
+       fields[1].num_bits = 5;\r
+       fields[1].out_value = field1_out;\r
+       buf_set_u32(fields[1].out_value, 0, 5, reg_addr);\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = ice_reg->jtag_info->chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = field2_out;\r
+       buf_set_u32(fields[2].out_value, 0, 1, 0);\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(3, fields, -1);\r
+       \r
+       fields[0].in_value = reg->value;\r
+       jtag_set_check_value(fields+0, check_value, check_mask, NULL);\r
+       \r
+       /* when reading the DCC data register, leaving the address field set to\r
+        * EICE_COMMS_DATA would read the register twice\r
+        * reading the control register is safe\r
+        */\r
+       buf_set_u32(fields[1].out_value, 0, 5, embeddedice_reg_arch_info[EICE_COMMS_CTRL]);\r
+       \r
+       jtag_add_dr_scan(3, fields, -1);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* receive <size> words of 32 bit from the DCC\r
+ * we pretend the target is always going to be fast enough\r
+ * (relative to the JTAG clock), so we don't need to handshake\r
+ */\r
+int embeddedice_receive(arm_jtag_t *jtag_info, u32 *data, u32 size)\r
+{\r
+       scan_field_t fields[3];\r
+       u8 field1_out[1];\r
+       u8 field2_out[1];\r
+\r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_scann(jtag_info, 0x2);\r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+       \r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = NULL;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+       \r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 5;\r
+       fields[1].out_value = field1_out;\r
+       buf_set_u32(fields[1].out_value, 0, 5, embeddedice_reg_arch_info[EICE_COMMS_DATA]);\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = jtag_info->chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = field2_out;\r
+       buf_set_u32(fields[2].out_value, 0, 1, 0);\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(3, fields, -1);\r
+       \r
+       while (size > 0)\r
+       {\r
+               /* when reading the last item, set the register address to the DCC control reg,\r
+                * to avoid reading additional data from the DCC data reg\r
+                */\r
+               if (size == 1)\r
+                       buf_set_u32(fields[1].out_value, 0, 5, embeddedice_reg_arch_info[EICE_COMMS_CTRL]);\r
+               \r
+               fields[0].in_handler = arm_jtag_buf_to_u32;\r
+               fields[0].in_handler_priv = data;\r
+               jtag_add_dr_scan(3, fields, -1);\r
+               \r
+               data++;\r
+               size--;\r
+       }\r
+       \r
+       return jtag_execute_queue();\r
+}\r
+\r
+int embeddedice_read_reg(reg_t *reg)\r
+{\r
+       return embeddedice_read_reg_w_check(reg, NULL, NULL);   \r
+}\r
+\r
+int embeddedice_set_reg(reg_t *reg, u32 value)\r
+{\r
+       if (embeddedice_write_reg(reg, value) != ERROR_OK)\r
+       {\r
+               ERROR("BUG: error scheduling EmbeddedICE register write");\r
+               exit(-1);\r
+       }\r
+       \r
+       buf_set_u32(reg->value, 0, reg->size, value);\r
+       reg->valid = 1;\r
+       reg->dirty = 0;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int embeddedice_set_reg_w_exec(reg_t *reg, u8 *buf)\r
+{\r
+       embeddedice_set_reg(reg, buf_get_u32(buf, 0, reg->size));\r
+       \r
+       if (jtag_execute_queue() != ERROR_OK)\r
+       {\r
+               ERROR("register write failed");\r
+               exit(-1);\r
+       }\r
+       return ERROR_OK;\r
+}\r
+\r
+int embeddedice_write_reg(reg_t *reg, u32 value)\r
+{\r
+       embeddedice_reg_t *ice_reg = reg->arch_info;\r
+       u8 reg_addr = ice_reg->addr & 0x1f;\r
+       scan_field_t fields[3];\r
+       u8 field0_out[4];\r
+       u8 field1_out[1];\r
+       u8 field2_out[1];\r
+\r
+       DEBUG("%i: 0x%8.8x", ice_reg->addr, value);\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_scann(ice_reg->jtag_info, 0x2);\r
+       \r
+       arm_jtag_set_instr(ice_reg->jtag_info, ice_reg->jtag_info->intest_instr, NULL);\r
+       \r
+       fields[0].device = ice_reg->jtag_info->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = field0_out;\r
+       buf_set_u32(fields[0].out_value, 0, 32, value);\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+       \r
+       fields[1].device = ice_reg->jtag_info->chain_pos;\r
+       fields[1].num_bits = 5;\r
+       fields[1].out_value = field1_out;\r
+       buf_set_u32(fields[1].out_value, 0, 5, reg_addr);\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = ice_reg->jtag_info->chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = field2_out;\r
+       buf_set_u32(fields[2].out_value, 0, 1, 1);\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(3, fields, -1);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int embeddedice_store_reg(reg_t *reg)\r
+{\r
+       return embeddedice_write_reg(reg, buf_get_u32(reg->value, 0, reg->size));\r
+}\r
+\r
+/* send <size> words of 32 bit to the DCC\r
+ * we pretend the target is always going to be fast enough\r
+ * (relative to the JTAG clock), so we don't need to handshake\r
+ */\r
+int embeddedice_send(arm_jtag_t *jtag_info, u32 *data, u32 size)\r
+{\r
+       scan_field_t fields[3];\r
+       u8 field0_out[4];\r
+       u8 field1_out[1];\r
+       u8 field2_out[1];\r
+\r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_scann(jtag_info, 0x2);\r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+\r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = field0_out;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+\r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 5;\r
+       fields[1].out_value = field1_out;\r
+       buf_set_u32(fields[1].out_value, 0, 5, embeddedice_reg_arch_info[EICE_COMMS_DATA]);\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = jtag_info->chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = field2_out;\r
+       buf_set_u32(fields[2].out_value, 0, 1, 1);\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+\r
+       while (size > 0)\r
+       {\r
+               buf_set_u32(fields[0].out_value, 0, 32, *data);\r
+               jtag_add_dr_scan(3, fields, -1);\r
+\r
+               data++;\r
+               size--;\r
+       }\r
+\r
+       /* call to jtag_execute_queue() intentionally omitted */\r
+       return ERROR_OK;\r
+}\r
+\r
+/* wait for DCC control register R/W handshake bit to become active\r
+ */\r
+int embeddedice_handshake(arm_jtag_t *jtag_info, int hsbit, u32 timeout)\r
+{\r
+       scan_field_t fields[3];\r
+       u8 field0_in[4];\r
+       u8 field1_out[1];\r
+       u8 field2_out[1];\r
+       int retval;\r
+       int hsact;\r
+       struct timeval lap;\r
+       struct timeval now;\r
+\r
+       if (hsbit == EICE_COMM_CTRL_WBIT)\r
+               hsact = 1;\r
+       else if (hsbit == EICE_COMM_CTRL_RBIT)\r
+               hsact = 0;\r
+       else\r
+               return ERROR_INVALID_ARGUMENTS;\r
+\r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_scann(jtag_info, 0x2);\r
+       arm_jtag_set_instr(jtag_info, jtag_info->intest_instr, NULL);\r
+\r
+       fields[0].device = jtag_info->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = NULL;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = field0_in;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+\r
+       fields[1].device = jtag_info->chain_pos;\r
+       fields[1].num_bits = 5;\r
+       fields[1].out_value = field1_out;\r
+       buf_set_u32(fields[1].out_value, 0, 5, embeddedice_reg_arch_info[EICE_COMMS_CTRL]);\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = jtag_info->chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = field2_out;\r
+       buf_set_u32(fields[2].out_value, 0, 1, 0);\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+\r
+       jtag_add_dr_scan(3, fields, -1);\r
+       gettimeofday(&lap, NULL);\r
+       do\r
+       {\r
+               jtag_add_dr_scan(3, fields, -1);\r
+               if ((retval = jtag_execute_queue()) != ERROR_OK)\r
+                       return retval;\r
+\r
+               if (buf_get_u32(field0_in, hsbit, 1) == hsact)\r
+                       return ERROR_OK;\r
+\r
+               gettimeofday(&now, NULL);\r
+       }\r
+       while ((now.tv_sec-lap.tv_sec)*1000 + (now.tv_usec-lap.tv_usec)/1000 <= timeout);\r
+\r
+       return ERROR_TARGET_TIMEOUT;\r
+}\r
index 0c409fb0ca15cae497de12ee7db6767ffc6cc9b1..6c9d3a0c139f5f5dbb842634ce5b2ec898200cfa 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2007 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <string.h>
-
-#include "arm7_9_common.h"
-#include "etb.h"
-#include "etm.h"
-
-#include "log.h"
-#include "types.h"
-#include "binarybuffer.h"
-#include "target.h"
-#include "register.h"
-#include "jtag.h"
-
-#include <stdlib.h>
-
-char* etb_reg_list[] =
-{
-       "ETB_identification",
-       "ETB_ram_depth",
-       "ETB_ram_width",
-       "ETB_status",
-       "ETB_ram_data",
-       "ETB_ram_read_pointer",
-       "ETB_ram_write_pointer",
-       "ETB_trigger_counter",
-       "ETB_control",
-};
-
-int etb_reg_arch_type = -1;
-
-int etb_get_reg(reg_t *reg);
-int etb_set_reg(reg_t *reg, u32 value);
-int etb_set_reg_w_exec(reg_t *reg, u8 *buf);
-
-int etb_write_reg(reg_t *reg, u32 value);
-int etb_read_reg(reg_t *reg);
-
-int handle_etb_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-int etb_set_instr(etb_t *etb, u32 new_instr)
-{
-       jtag_device_t *device = jtag_get_device(etb->chain_pos);
-       
-       if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)
-       {
-               scan_field_t field;
-       
-               field.device = etb->chain_pos;
-               field.num_bits = device->ir_length;
-               field.out_value = calloc(CEIL(field.num_bits, 8), 1);
-               buf_set_u32(field.out_value, 0, field.num_bits, new_instr);
-               field.out_mask = NULL;
-               field.in_value = NULL;
-               field.in_check_value = NULL;
-               field.in_check_mask = NULL;
-               field.in_handler = NULL;
-               field.in_handler_priv = NULL;
-                               
-               jtag_add_ir_scan(1, &field, -1, NULL);
-               
-               free(field.out_value);
-       }
-       
-       return ERROR_OK;
-}
-
-int etb_scann(etb_t *etb, u32 new_scan_chain)
-{
-       if(etb->cur_scan_chain != new_scan_chain)
-       {
-               scan_field_t field;
-               
-               field.device = etb->chain_pos;
-               field.num_bits = 5;
-               field.out_value = calloc(CEIL(field.num_bits, 8), 1);
-               buf_set_u32(field.out_value, 0, field.num_bits, new_scan_chain);
-               field.out_mask = NULL;
-               field.in_value = NULL;
-               field.in_check_value = NULL;
-               field.in_check_mask = NULL;
-               field.in_handler = NULL;
-               field.in_handler_priv = NULL;
-               
-               /* select INTEST instruction */
-               etb_set_instr(etb, 0x2);
-               jtag_add_dr_scan(1, &field, -1, NULL);
-               
-               etb->cur_scan_chain = new_scan_chain;
-               
-               free(field.out_value);
-       }
-
-       return ERROR_OK;
-}
-
-reg_cache_t* etb_build_reg_cache(etb_t *etb)
-{
-       reg_cache_t *reg_cache = malloc(sizeof(reg_cache_t));
-       reg_t *reg_list = NULL;
-       etb_reg_t *arch_info = NULL;
-       int num_regs = 9;
-       int i;
-       
-       /* register a register arch-type for etm registers only once */
-       if (etb_reg_arch_type == -1)
-               etb_reg_arch_type = register_reg_arch_type(etb_get_reg, etb_set_reg_w_exec);
-       
-       /* the actual registers are kept in two arrays */
-       reg_list = calloc(num_regs, sizeof(reg_t));
-       arch_info = calloc(num_regs, sizeof(etb_reg_t));
-       
-       /* fill in values for the reg cache */
-       reg_cache->name = "etb registers";
-       reg_cache->next = NULL;
-       reg_cache->reg_list = reg_list;
-       reg_cache->num_regs = num_regs;
-       
-       /* set up registers */
-       for (i = 0; i < num_regs; i++)
-       {
-               reg_list[i].name = etb_reg_list[i];
-               reg_list[i].size = 32;
-               reg_list[i].dirty = 0;
-               reg_list[i].valid = 0;
-               reg_list[i].bitfield_desc = NULL;
-               reg_list[i].num_bitfields = 0;
-               reg_list[i].value = calloc(1, 4);
-               reg_list[i].arch_info = &arch_info[i];
-               reg_list[i].arch_type = etb_reg_arch_type;
-               reg_list[i].size = 32;
-               arch_info[i].addr = i;
-               arch_info[i].etb = etb;
-       }
-       
-       return reg_cache;
-}
-
-int etb_get_reg(reg_t *reg)
-{
-       if (etb_read_reg(reg) != ERROR_OK)
-       {
-               ERROR("BUG: error scheduling etm register read");
-               exit(-1);
-       }
-       
-       if (jtag_execute_queue() != ERROR_OK)
-       {
-               ERROR("register read failed");
-       }
-       
-       return ERROR_OK;
-}
-
-int etb_read_ram(etb_t *etb, u32 *data, int num_frames)
-{
-       scan_field_t fields[3];
-       int i;
-       
-       jtag_add_end_state(TAP_RTI);
-       etb_scann(etb, 0x0);
-       etb_set_instr(etb, 0xc);
-       
-       fields[0].device = etb->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = NULL;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-       
-       fields[1].device = etb->chain_pos;
-       fields[1].num_bits = 7;
-       fields[1].out_value = malloc(1);
-       buf_set_u32(fields[1].out_value, 0, 7, 4);
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = etb->chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = malloc(1);
-       buf_set_u32(fields[2].out_value, 0, 1, 0);
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(3, fields, -1, NULL);
-
-       fields[0].in_handler = buf_to_u32_handler;
-       
-       for (i = 0; i < num_frames; i++)
-       {
-               /* ensure nR/W reamins set to read */
-               buf_set_u32(fields[2].out_value, 0, 1, 0);
-               
-               /* address remains set to 0x4 (RAM data) until we read the last frame */
-               if (i < num_frames - 1)
-                       buf_set_u32(fields[1].out_value, 0, 7, 4);
-               else
-                       buf_set_u32(fields[1].out_value, 0, 7, 0);
-               
-               fields[0].in_handler_priv = &data[i];
-               jtag_add_dr_scan(3, fields, -1, NULL);
-       }
-       
-       jtag_execute_queue();
-       
-       free(fields[1].out_value);
-       free(fields[2].out_value);
-       
-       return ERROR_OK;
-}
-
-int etb_read_reg_w_check(reg_t *reg, u8* check_value, u8* check_mask)
-{
-       etb_reg_t *etb_reg = reg->arch_info;
-       u8 reg_addr = etb_reg->addr & 0x7f;
-       scan_field_t fields[3];
-       
-       DEBUG("%i", etb_reg->addr);
-
-       jtag_add_end_state(TAP_RTI);
-       etb_scann(etb_reg->etb, 0x0);
-       etb_set_instr(etb_reg->etb, 0xc);
-       
-       fields[0].device = etb_reg->etb->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = reg->value;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-       
-       fields[1].device = etb_reg->etb->chain_pos;
-       fields[1].num_bits = 7;
-       fields[1].out_value = malloc(1);
-       buf_set_u32(fields[1].out_value, 0, 7, reg_addr);
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = etb_reg->etb->chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = malloc(1);
-       buf_set_u32(fields[2].out_value, 0, 1, 0);
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(3, fields, -1, NULL);
-       
-       /* read the identification register in the second run, to make sure we
-        * don't read the ETB data register twice, skipping every second entry
-        */
-       buf_set_u32(fields[1].out_value, 0, 7, 0x0);
-       fields[0].in_value = reg->value;
-       
-       jtag_set_check_value(fields+0, check_value, check_mask, NULL);
-       
-       jtag_add_dr_scan(3, fields, -1, NULL);
-
-       free(fields[1].out_value);
-       free(fields[2].out_value);
-       
-       return ERROR_OK;
-}
-
-int etb_read_reg(reg_t *reg)
-{
-       return etb_read_reg_w_check(reg, NULL, NULL);   
-}
-
-int etb_set_reg(reg_t *reg, u32 value)
-{
-       if (etb_write_reg(reg, value) != ERROR_OK)
-       {
-               ERROR("BUG: error scheduling etm register write");
-               exit(-1);
-       }
-       
-       buf_set_u32(reg->value, 0, reg->size, value);
-       reg->valid = 1;
-       reg->dirty = 0;
-       
-       return ERROR_OK;
-}
-
-int etb_set_reg_w_exec(reg_t *reg, u8 *buf)
-{
-       etb_set_reg(reg, buf_get_u32(buf, 0, reg->size));
-       
-       if (jtag_execute_queue() != ERROR_OK)
-       {
-               ERROR("register write failed");
-               exit(-1);
-       }
-       return ERROR_OK;
-}
-
-int etb_write_reg(reg_t *reg, u32 value)
-{
-       etb_reg_t *etb_reg = reg->arch_info;
-       u8 reg_addr = etb_reg->addr & 0x7f;
-       scan_field_t fields[3];
-       
-       DEBUG("%i: 0x%8.8x", etb_reg->addr, value);
-       
-       jtag_add_end_state(TAP_RTI);
-       etb_scann(etb_reg->etb, 0x0);
-       etb_set_instr(etb_reg->etb, 0xc);
-       
-       fields[0].device = etb_reg->etb->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = malloc(4);
-       buf_set_u32(fields[0].out_value, 0, 32, value);
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-       
-       fields[1].device = etb_reg->etb->chain_pos;
-       fields[1].num_bits = 7;
-       fields[1].out_value = malloc(1);
-       buf_set_u32(fields[1].out_value, 0, 7, reg_addr);
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = etb_reg->etb->chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = malloc(1);
-       buf_set_u32(fields[2].out_value, 0, 1, 1);
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(3, fields, -1, NULL);
-       
-       free(fields[0].out_value);
-       free(fields[1].out_value);
-       free(fields[2].out_value);
-       
-       return ERROR_OK;
-}
-
-int etb_store_reg(reg_t *reg)
-{
-       return etb_write_reg(reg, buf_get_u32(reg->value, 0, reg->size));
-}
-
-int etb_register_commands(struct command_context_s *cmd_ctx)
-{
-       command_t *etb_cmd;
-       
-       etb_cmd = register_command(cmd_ctx, NULL, "etb", NULL, COMMAND_ANY, "Embedded Trace Buffer");
-       
-       register_command(cmd_ctx, etb_cmd, "config", handle_etb_config_command, COMMAND_CONFIG, NULL);
-
-       return ERROR_OK;
-}
-
-int handle_etb_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target;
-       jtag_device_t *jtag_device;
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       
-       if (argc != 2)
-       {
-               ERROR("incomplete 'etb config <target> <chain_pos>' command");
-               exit(-1);
-       }
-       
-       target = get_target_by_num(strtoul(args[0], NULL, 0));
-       
-       if (!target)
-       {
-               ERROR("target number '%s' not defined", args[0]);
-               exit(-1);
-       }
-       
-       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-               return ERROR_OK;
-       }
-       
-       jtag_device = jtag_get_device(strtoul(args[1], NULL, 0));
-       
-       if (!jtag_device)
-       {
-               ERROR("jtag device number '%s' not defined", args[1]);
-               exit(-1);
-       }
-       
-       if (arm7_9->etm_ctx)
-       {
-               etb_t *etb = malloc(sizeof(etb_t));
-               
-               arm7_9->etm_ctx->capture_driver_priv = etb;
-               
-               etb->chain_pos = strtoul(args[1], NULL, 0);
-               etb->cur_scan_chain = -1;
-               etb->reg_cache = NULL;
-               etb->ram_width = 0;
-               etb->ram_depth = 0;
-       }
-       else
-       {
-               ERROR("target has no ETM defined, ETB left unconfigured");
-       }
-
-       return ERROR_OK;
-}
-
-int etb_init(etm_context_t *etm_ctx)
-{
-       etb_t *etb = etm_ctx->capture_driver_priv;
-       
-       etb->etm_ctx = etm_ctx;
-       
-       /* identify ETB RAM depth and width */
-       etb_read_reg(&etb->reg_cache->reg_list[ETB_RAM_DEPTH]);
-       etb_read_reg(&etb->reg_cache->reg_list[ETB_RAM_WIDTH]);
-       jtag_execute_queue();
-
-       etb->ram_depth = buf_get_u32(etb->reg_cache->reg_list[ETB_RAM_DEPTH].value, 0, 32);
-       etb->ram_width = buf_get_u32(etb->reg_cache->reg_list[ETB_RAM_WIDTH].value, 0, 32);
-       
-       return ERROR_OK;
-}
-
-trace_status_t etb_status(etm_context_t *etm_ctx)
-{
-       etb_t *etb = etm_ctx->capture_driver_priv;
-       
-       etb->etm_ctx = etm_ctx;
-       
-       /* if tracing is currently idle, return this information */
-       if (etm_ctx->capture_status == TRACE_IDLE)
-       {
-               return etm_ctx->capture_status;
-       }
-       else if (etm_ctx->capture_status & TRACE_RUNNING)
-       {
-               reg_t *etb_status_reg = &etb->reg_cache->reg_list[ETB_STATUS];
-               int etb_timeout = 100;
-               
-               /* trace is running, check the ETB status flags */
-               etb_get_reg(etb_status_reg);
-       
-               /* check Full bit to identify an overflow */
-               if (buf_get_u32(etb_status_reg->value, 0, 1) == 1)
-                       etm_ctx->capture_status |= TRACE_OVERFLOWED;
-
-               /* check Triggered bit to identify trigger condition */
-               if (buf_get_u32(etb_status_reg->value, 1, 1) == 1)
-                       etm_ctx->capture_status |= TRACE_TRIGGERED;
-
-               /* check AcqComp to identify trace completion */
-               if (buf_get_u32(etb_status_reg->value, 2, 1) == 1)
-               {
-                       while (etb_timeout-- && (buf_get_u32(etb_status_reg->value, 3, 1) == 0))
-                       {
-                               /* wait for data formatter idle */
-                               etb_get_reg(etb_status_reg);
-                       }
-                       
-                       if (etb_timeout == 0)
-                       {
-                               ERROR("AcqComp set but DFEmpty won't go high, ETB status: 0x%x",
-                                       buf_get_u32(etb_status_reg->value, 0, etb_status_reg->size));
-                       }
-                       
-                       if (!(etm_ctx->capture_status && TRACE_TRIGGERED))
-                       {
-                               ERROR("trace completed, but no trigger condition detected");
-                       }
-                       
-                       etm_ctx->capture_status &= ~TRACE_RUNNING;
-                       etm_ctx->capture_status |= TRACE_COMPLETED;
-               }
-       }
-       
-       return etm_ctx->capture_status;
-}
-
-int etb_read_trace(etm_context_t *etm_ctx)
-{
-       etb_t *etb = etm_ctx->capture_driver_priv;
-       int first_frame = 0;
-       int num_frames = etb->ram_depth;
-       u32 *trace_data = NULL;
-       int i, j;
-       
-       etb_read_reg(&etb->reg_cache->reg_list[ETB_STATUS]);
-       etb_read_reg(&etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER]);
-       jtag_execute_queue();
-       
-       /* check if we overflowed, and adjust first frame of the trace accordingly
-        * if we didn't overflow, read only up to the frame that would be written next,
-        * i.e. don't read invalid entries
-        */
-       if (buf_get_u32(etb->reg_cache->reg_list[ETB_STATUS].value, 0, 1))
-       {
-               first_frame = buf_get_u32(etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER].value, 0, 32);
-       }
-       else
-       {
-               num_frames = buf_get_u32(etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER].value, 0, 32);
-       }
-       
-       etb_write_reg(&etb->reg_cache->reg_list[ETB_RAM_READ_POINTER], first_frame);
-
-       /* read data into temporary array for unpacking */      
-       trace_data = malloc(sizeof(u32) * num_frames);
-       etb_read_ram(etb, trace_data, num_frames);
-
-       if (etm_ctx->trace_depth > 0)
-       {
-               free(etm_ctx->trace_data);
-       }
-       
-       if ((etm_ctx->portmode & ETM_PORT_WIDTH_MASK) == ETM_PORT_4BIT)
-               etm_ctx->trace_depth = num_frames * 3;
-       else if ((etm_ctx->portmode & ETM_PORT_WIDTH_MASK) == ETM_PORT_8BIT)
-               etm_ctx->trace_depth = num_frames * 2;
-       else
-               etm_ctx->trace_depth = num_frames;
-
-       etm_ctx->trace_data = malloc(sizeof(etmv1_trace_data_t) * etm_ctx->trace_depth);
-       
-       for (i = 0, j = 0; i < num_frames; i++)
-       {
-               if ((etm_ctx->portmode & ETM_PORT_WIDTH_MASK) == ETM_PORT_4BIT)
-               {
-                       /* trace word j */
-                       etm_ctx->trace_data[j].pipestat = trace_data[i] & 0x7;
-                       etm_ctx->trace_data[j].packet = (trace_data[i] & 0x78) >> 3;
-                       etm_ctx->trace_data[j].flags = 0;
-                       if ((trace_data[i] & 0x80) >> 7)
-                       {
-                               etm_ctx->trace_data[j].flags |= ETMV1_TRACESYNC_CYCLE;
-                       }
-                       if (etm_ctx->trace_data[j].pipestat == STAT_TR)
-                       {
-                               etm_ctx->trace_data[j].pipestat = etm_ctx->trace_data[j].packet & 0x7;
-                               etm_ctx->trace_data[j].flags |= ETMV1_TRIGGER_CYCLE;
-                       }
-                       
-                       /* trace word j+1 */
-                       etm_ctx->trace_data[j+1].pipestat = (trace_data[i] & 0x100) >> 8;
-                       etm_ctx->trace_data[j+1].packet = (trace_data[i] & 0x7800) >> 11;
-                       etm_ctx->trace_data[j+1].flags = 0;
-                       if ((trace_data[i] & 0x8000) >> 15)
-                       {
-                               etm_ctx->trace_data[j+1].flags |= ETMV1_TRACESYNC_CYCLE;
-                       }
-                       if (etm_ctx->trace_data[j+1].pipestat == STAT_TR)
-                       {
-                               etm_ctx->trace_data[j+1].pipestat = etm_ctx->trace_data[j+1].packet & 0x7;
-                               etm_ctx->trace_data[j+1].flags |= ETMV1_TRIGGER_CYCLE;
-                       }
-                       
-                       /* trace word j+2 */
-                       etm_ctx->trace_data[j+2].pipestat = (trace_data[i] & 0x10000) >> 16;
-                       etm_ctx->trace_data[j+2].packet = (trace_data[i] & 0x780000) >> 19;
-                       etm_ctx->trace_data[j+2].flags = 0;
-                       if ((trace_data[i] & 0x800000) >> 23)
-                       {
-                               etm_ctx->trace_data[j+2].flags |= ETMV1_TRACESYNC_CYCLE;
-                       }
-                       if (etm_ctx->trace_data[j+2].pipestat == STAT_TR)
-                       {
-                               etm_ctx->trace_data[j+2].pipestat = etm_ctx->trace_data[j+2].packet & 0x7;
-                               etm_ctx->trace_data[j+2].flags |= ETMV1_TRIGGER_CYCLE;
-                       }
-                       
-                       j += 3;
-               }
-               else if ((etm_ctx->portmode & ETM_PORT_WIDTH_MASK) == ETM_PORT_8BIT)
-               {
-                       /* trace word j */
-                       etm_ctx->trace_data[j].pipestat = trace_data[i] & 0x7;
-                       etm_ctx->trace_data[j].packet = (trace_data[i] & 0x7f8) >> 3;
-                       etm_ctx->trace_data[j].flags = 0;
-                       if ((trace_data[i] & 0x800) >> 11)
-                       {
-                               etm_ctx->trace_data[j].flags |= ETMV1_TRACESYNC_CYCLE;
-                       }
-                       if (etm_ctx->trace_data[j].pipestat == STAT_TR)
-                       {
-                               etm_ctx->trace_data[j].pipestat = etm_ctx->trace_data[j].packet & 0x7;
-                               etm_ctx->trace_data[j].flags |= ETMV1_TRIGGER_CYCLE;
-                       }
-
-                       /* trace word j+1 */
-                       etm_ctx->trace_data[j+1].pipestat = (trace_data[i] & 0x7000) >> 12;
-                       etm_ctx->trace_data[j+1].packet = (trace_data[i] & 0x7f8000) >> 15;
-                       etm_ctx->trace_data[j+1].flags = 0;
-                       if ((trace_data[i] & 0x800000) >> 23)
-                       {
-                               etm_ctx->trace_data[j+1].flags |= ETMV1_TRACESYNC_CYCLE;
-                       }
-                       if (etm_ctx->trace_data[j+1].pipestat == STAT_TR)
-                       {
-                               etm_ctx->trace_data[j+1].pipestat = etm_ctx->trace_data[j+1].packet & 0x7;
-                               etm_ctx->trace_data[j+1].flags |= ETMV1_TRIGGER_CYCLE;
-                       }
-                       
-                       j += 2;
-               }
-               else
-               {
-                       /* trace word j */
-                       etm_ctx->trace_data[j].pipestat = trace_data[i] & 0x7;
-                       etm_ctx->trace_data[j].packet = (trace_data[i] & 0x7fff8) >> 3;
-                       etm_ctx->trace_data[j].flags = 0;
-                       if ((trace_data[i] & 0x80000) >> 19)
-                       {
-                               etm_ctx->trace_data[j].flags |= ETMV1_TRACESYNC_CYCLE;
-                       }
-                       if (etm_ctx->trace_data[j].pipestat == STAT_TR)
-                       {
-                               etm_ctx->trace_data[j].pipestat = etm_ctx->trace_data[j].packet & 0x7;
-                               etm_ctx->trace_data[j].flags |= ETMV1_TRIGGER_CYCLE;
-                       }
-                       
-                       j += 1;
-               }
-       }
-       
-       free(trace_data);
-       
-       return ERROR_OK;
-}
-
-int etb_start_capture(etm_context_t *etm_ctx)
-{
-       etb_t *etb = etm_ctx->capture_driver_priv;
-       u32 etb_ctrl_value = 0x1;
-       u32 trigger_count;
-
-       if ((etm_ctx->portmode & ETM_PORT_MODE_MASK) == ETM_PORT_DEMUXED)
-       {
-               if ((etm_ctx->portmode & ETM_PORT_WIDTH_MASK) != ETM_PORT_8BIT)
-               {
-                       ERROR("ETB can't run in demultiplexed mode with a 4 or 16 bit port");
-                       return ERROR_ETM_PORTMODE_NOT_SUPPORTED;
-               }
-               etb_ctrl_value |= 0x2;
-       }
-       
-       if ((etm_ctx->portmode & ETM_PORT_MODE_MASK) == ETM_PORT_MUXED)
-               return ERROR_ETM_PORTMODE_NOT_SUPPORTED;
-       
-       trigger_count = (etb->ram_depth * etm_ctx->trigger_percent) / 100;
-       
-       etb_write_reg(&etb->reg_cache->reg_list[ETB_TRIGGER_COUNTER], trigger_count);
-       etb_write_reg(&etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER], 0x0);
-       etb_write_reg(&etb->reg_cache->reg_list[ETB_CTRL], etb_ctrl_value);
-       jtag_execute_queue();
-       
-       /* we're starting a new trace, initialize capture status */
-       etm_ctx->capture_status = TRACE_RUNNING;
-       
-       return ERROR_OK; 
-}
-
-int etb_stop_capture(etm_context_t *etm_ctx)
-{
-       etb_t *etb = etm_ctx->capture_driver_priv;
-       reg_t *etb_ctrl_reg = &etb->reg_cache->reg_list[ETB_CTRL];
-
-       etb_write_reg(etb_ctrl_reg, 0x0);
-       jtag_execute_queue();
-       
-       /* trace stopped, just clear running flag, but preserve others */ 
-       etm_ctx->capture_status &= ~TRACE_RUNNING;
-       
-       return ERROR_OK;
-}
-
-etm_capture_driver_t etb_capture_driver =
-{
-       .name = "etb",
-       .register_commands = etb_register_commands,
-       .init = etb_init,
-       .status = etb_status,
-       .start_capture = etb_start_capture,
-       .stop_capture = etb_stop_capture,
-       .read_trace = etb_read_trace,
-};
+/***************************************************************************\r
+ *   Copyright (C) 2007 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include <string.h>\r
+\r
+#include "arm7_9_common.h"\r
+#include "etb.h"\r
+#include "etm.h"\r
+\r
+#include "log.h"\r
+#include "types.h"\r
+#include "binarybuffer.h"\r
+#include "target.h"\r
+#include "register.h"\r
+#include "jtag.h"\r
+\r
+#include <stdlib.h>\r
+\r
+char* etb_reg_list[] =\r
+{\r
+       "ETB_identification",\r
+       "ETB_ram_depth",\r
+       "ETB_ram_width",\r
+       "ETB_status",\r
+       "ETB_ram_data",\r
+       "ETB_ram_read_pointer",\r
+       "ETB_ram_write_pointer",\r
+       "ETB_trigger_counter",\r
+       "ETB_control",\r
+};\r
+\r
+int etb_reg_arch_type = -1;\r
+\r
+int etb_get_reg(reg_t *reg);\r
+int etb_set_reg(reg_t *reg, u32 value);\r
+int etb_set_reg_w_exec(reg_t *reg, u8 *buf);\r
+\r
+int etb_write_reg(reg_t *reg, u32 value);\r
+int etb_read_reg(reg_t *reg);\r
+\r
+int handle_etb_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+int etb_set_instr(etb_t *etb, u32 new_instr)\r
+{\r
+       jtag_device_t *device = jtag_get_device(etb->chain_pos);\r
+       \r
+       if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)\r
+       {\r
+               scan_field_t field;\r
+       \r
+               field.device = etb->chain_pos;\r
+               field.num_bits = device->ir_length;\r
+               field.out_value = calloc(CEIL(field.num_bits, 8), 1);\r
+               buf_set_u32(field.out_value, 0, field.num_bits, new_instr);\r
+               field.out_mask = NULL;\r
+               field.in_value = NULL;\r
+               field.in_check_value = NULL;\r
+               field.in_check_mask = NULL;\r
+               field.in_handler = NULL;\r
+               field.in_handler_priv = NULL;\r
+                               \r
+               jtag_add_ir_scan(1, &field, -1);\r
+               \r
+               free(field.out_value);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int etb_scann(etb_t *etb, u32 new_scan_chain)\r
+{\r
+       if(etb->cur_scan_chain != new_scan_chain)\r
+       {\r
+               scan_field_t field;\r
+               \r
+               field.device = etb->chain_pos;\r
+               field.num_bits = 5;\r
+               field.out_value = calloc(CEIL(field.num_bits, 8), 1);\r
+               buf_set_u32(field.out_value, 0, field.num_bits, new_scan_chain);\r
+               field.out_mask = NULL;\r
+               field.in_value = NULL;\r
+               field.in_check_value = NULL;\r
+               field.in_check_mask = NULL;\r
+               field.in_handler = NULL;\r
+               field.in_handler_priv = NULL;\r
+               \r
+               /* select INTEST instruction */\r
+               etb_set_instr(etb, 0x2);\r
+               jtag_add_dr_scan(1, &field, -1);\r
+               \r
+               etb->cur_scan_chain = new_scan_chain;\r
+               \r
+               free(field.out_value);\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+reg_cache_t* etb_build_reg_cache(etb_t *etb)\r
+{\r
+       reg_cache_t *reg_cache = malloc(sizeof(reg_cache_t));\r
+       reg_t *reg_list = NULL;\r
+       etb_reg_t *arch_info = NULL;\r
+       int num_regs = 9;\r
+       int i;\r
+       \r
+       /* register a register arch-type for etm registers only once */\r
+       if (etb_reg_arch_type == -1)\r
+               etb_reg_arch_type = register_reg_arch_type(etb_get_reg, etb_set_reg_w_exec);\r
+       \r
+       /* the actual registers are kept in two arrays */\r
+       reg_list = calloc(num_regs, sizeof(reg_t));\r
+       arch_info = calloc(num_regs, sizeof(etb_reg_t));\r
+       \r
+       /* fill in values for the reg cache */\r
+       reg_cache->name = "etb registers";\r
+       reg_cache->next = NULL;\r
+       reg_cache->reg_list = reg_list;\r
+       reg_cache->num_regs = num_regs;\r
+       \r
+       /* set up registers */\r
+       for (i = 0; i < num_regs; i++)\r
+       {\r
+               reg_list[i].name = etb_reg_list[i];\r
+               reg_list[i].size = 32;\r
+               reg_list[i].dirty = 0;\r
+               reg_list[i].valid = 0;\r
+               reg_list[i].bitfield_desc = NULL;\r
+               reg_list[i].num_bitfields = 0;\r
+               reg_list[i].value = calloc(1, 4);\r
+               reg_list[i].arch_info = &arch_info[i];\r
+               reg_list[i].arch_type = etb_reg_arch_type;\r
+               reg_list[i].size = 32;\r
+               arch_info[i].addr = i;\r
+               arch_info[i].etb = etb;\r
+       }\r
+       \r
+       return reg_cache;\r
+}\r
+\r
+int etb_get_reg(reg_t *reg)\r
+{\r
+       if (etb_read_reg(reg) != ERROR_OK)\r
+       {\r
+               ERROR("BUG: error scheduling etm register read");\r
+               exit(-1);\r
+       }\r
+       \r
+       if (jtag_execute_queue() != ERROR_OK)\r
+       {\r
+               ERROR("register read failed");\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int etb_read_ram(etb_t *etb, u32 *data, int num_frames)\r
+{\r
+       scan_field_t fields[3];\r
+       int i;\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       etb_scann(etb, 0x0);\r
+       etb_set_instr(etb, 0xc);\r
+       \r
+       fields[0].device = etb->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = NULL;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+       \r
+       fields[1].device = etb->chain_pos;\r
+       fields[1].num_bits = 7;\r
+       fields[1].out_value = malloc(1);\r
+       buf_set_u32(fields[1].out_value, 0, 7, 4);\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = etb->chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = malloc(1);\r
+       buf_set_u32(fields[2].out_value, 0, 1, 0);\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(3, fields, -1);\r
+\r
+       fields[0].in_handler = buf_to_u32_handler;\r
+       \r
+       for (i = 0; i < num_frames; i++)\r
+       {\r
+               /* ensure nR/W reamins set to read */\r
+               buf_set_u32(fields[2].out_value, 0, 1, 0);\r
+               \r
+               /* address remains set to 0x4 (RAM data) until we read the last frame */\r
+               if (i < num_frames - 1)\r
+                       buf_set_u32(fields[1].out_value, 0, 7, 4);\r
+               else\r
+                       buf_set_u32(fields[1].out_value, 0, 7, 0);\r
+               \r
+               fields[0].in_handler_priv = &data[i];\r
+               jtag_add_dr_scan(3, fields, -1);\r
+       }\r
+       \r
+       jtag_execute_queue();\r
+       \r
+       free(fields[1].out_value);\r
+       free(fields[2].out_value);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int etb_read_reg_w_check(reg_t *reg, u8* check_value, u8* check_mask)\r
+{\r
+       etb_reg_t *etb_reg = reg->arch_info;\r
+       u8 reg_addr = etb_reg->addr & 0x7f;\r
+       scan_field_t fields[3];\r
+       \r
+       DEBUG("%i", etb_reg->addr);\r
+\r
+       jtag_add_end_state(TAP_RTI);\r
+       etb_scann(etb_reg->etb, 0x0);\r
+       etb_set_instr(etb_reg->etb, 0xc);\r
+       \r
+       fields[0].device = etb_reg->etb->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = reg->value;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+       \r
+       fields[1].device = etb_reg->etb->chain_pos;\r
+       fields[1].num_bits = 7;\r
+       fields[1].out_value = malloc(1);\r
+       buf_set_u32(fields[1].out_value, 0, 7, reg_addr);\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = etb_reg->etb->chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = malloc(1);\r
+       buf_set_u32(fields[2].out_value, 0, 1, 0);\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(3, fields, -1);\r
+       \r
+       /* read the identification register in the second run, to make sure we\r
+        * don't read the ETB data register twice, skipping every second entry\r
+        */\r
+       buf_set_u32(fields[1].out_value, 0, 7, 0x0);\r
+       fields[0].in_value = reg->value;\r
+       \r
+       jtag_set_check_value(fields+0, check_value, check_mask, NULL);\r
+       \r
+       jtag_add_dr_scan(3, fields, -1);\r
+\r
+       free(fields[1].out_value);\r
+       free(fields[2].out_value);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int etb_read_reg(reg_t *reg)\r
+{\r
+       return etb_read_reg_w_check(reg, NULL, NULL);   \r
+}\r
+\r
+int etb_set_reg(reg_t *reg, u32 value)\r
+{\r
+       if (etb_write_reg(reg, value) != ERROR_OK)\r
+       {\r
+               ERROR("BUG: error scheduling etm register write");\r
+               exit(-1);\r
+       }\r
+       \r
+       buf_set_u32(reg->value, 0, reg->size, value);\r
+       reg->valid = 1;\r
+       reg->dirty = 0;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int etb_set_reg_w_exec(reg_t *reg, u8 *buf)\r
+{\r
+       etb_set_reg(reg, buf_get_u32(buf, 0, reg->size));\r
+       \r
+       if (jtag_execute_queue() != ERROR_OK)\r
+       {\r
+               ERROR("register write failed");\r
+               exit(-1);\r
+       }\r
+       return ERROR_OK;\r
+}\r
+\r
+int etb_write_reg(reg_t *reg, u32 value)\r
+{\r
+       etb_reg_t *etb_reg = reg->arch_info;\r
+       u8 reg_addr = etb_reg->addr & 0x7f;\r
+       scan_field_t fields[3];\r
+       \r
+       DEBUG("%i: 0x%8.8x", etb_reg->addr, value);\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       etb_scann(etb_reg->etb, 0x0);\r
+       etb_set_instr(etb_reg->etb, 0xc);\r
+       \r
+       fields[0].device = etb_reg->etb->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = malloc(4);\r
+       buf_set_u32(fields[0].out_value, 0, 32, value);\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+       \r
+       fields[1].device = etb_reg->etb->chain_pos;\r
+       fields[1].num_bits = 7;\r
+       fields[1].out_value = malloc(1);\r
+       buf_set_u32(fields[1].out_value, 0, 7, reg_addr);\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = etb_reg->etb->chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = malloc(1);\r
+       buf_set_u32(fields[2].out_value, 0, 1, 1);\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(3, fields, -1);\r
+       \r
+       free(fields[0].out_value);\r
+       free(fields[1].out_value);\r
+       free(fields[2].out_value);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int etb_store_reg(reg_t *reg)\r
+{\r
+       return etb_write_reg(reg, buf_get_u32(reg->value, 0, reg->size));\r
+}\r
+\r
+int etb_register_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       command_t *etb_cmd;\r
+       \r
+       etb_cmd = register_command(cmd_ctx, NULL, "etb", NULL, COMMAND_ANY, "Embedded Trace Buffer");\r
+       \r
+       register_command(cmd_ctx, etb_cmd, "config", handle_etb_config_command, COMMAND_CONFIG, NULL);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_etb_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target;\r
+       jtag_device_t *jtag_device;\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       \r
+       if (argc != 2)\r
+       {\r
+               ERROR("incomplete 'etb config <target> <chain_pos>' command");\r
+               exit(-1);\r
+       }\r
+       \r
+       target = get_target_by_num(strtoul(args[0], NULL, 0));\r
+       \r
+       if (!target)\r
+       {\r
+               ERROR("target number '%s' not defined", args[0]);\r
+               exit(-1);\r
+       }\r
+       \r
+       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       jtag_device = jtag_get_device(strtoul(args[1], NULL, 0));\r
+       \r
+       if (!jtag_device)\r
+       {\r
+               ERROR("jtag device number '%s' not defined", args[1]);\r
+               exit(-1);\r
+       }\r
+       \r
+       if (arm7_9->etm_ctx)\r
+       {\r
+               etb_t *etb = malloc(sizeof(etb_t));\r
+               \r
+               arm7_9->etm_ctx->capture_driver_priv = etb;\r
+               \r
+               etb->chain_pos = strtoul(args[1], NULL, 0);\r
+               etb->cur_scan_chain = -1;\r
+               etb->reg_cache = NULL;\r
+               etb->ram_width = 0;\r
+               etb->ram_depth = 0;\r
+       }\r
+       else\r
+       {\r
+               ERROR("target has no ETM defined, ETB left unconfigured");\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int etb_init(etm_context_t *etm_ctx)\r
+{\r
+       etb_t *etb = etm_ctx->capture_driver_priv;\r
+       \r
+       etb->etm_ctx = etm_ctx;\r
+       \r
+       /* identify ETB RAM depth and width */\r
+       etb_read_reg(&etb->reg_cache->reg_list[ETB_RAM_DEPTH]);\r
+       etb_read_reg(&etb->reg_cache->reg_list[ETB_RAM_WIDTH]);\r
+       jtag_execute_queue();\r
+\r
+       etb->ram_depth = buf_get_u32(etb->reg_cache->reg_list[ETB_RAM_DEPTH].value, 0, 32);\r
+       etb->ram_width = buf_get_u32(etb->reg_cache->reg_list[ETB_RAM_WIDTH].value, 0, 32);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+trace_status_t etb_status(etm_context_t *etm_ctx)\r
+{\r
+       etb_t *etb = etm_ctx->capture_driver_priv;\r
+       \r
+       etb->etm_ctx = etm_ctx;\r
+       \r
+       /* if tracing is currently idle, return this information */\r
+       if (etm_ctx->capture_status == TRACE_IDLE)\r
+       {\r
+               return etm_ctx->capture_status;\r
+       }\r
+       else if (etm_ctx->capture_status & TRACE_RUNNING)\r
+       {\r
+               reg_t *etb_status_reg = &etb->reg_cache->reg_list[ETB_STATUS];\r
+               int etb_timeout = 100;\r
+               \r
+               /* trace is running, check the ETB status flags */\r
+               etb_get_reg(etb_status_reg);\r
+       \r
+               /* check Full bit to identify an overflow */\r
+               if (buf_get_u32(etb_status_reg->value, 0, 1) == 1)\r
+                       etm_ctx->capture_status |= TRACE_OVERFLOWED;\r
+\r
+               /* check Triggered bit to identify trigger condition */\r
+               if (buf_get_u32(etb_status_reg->value, 1, 1) == 1)\r
+                       etm_ctx->capture_status |= TRACE_TRIGGERED;\r
+\r
+               /* check AcqComp to identify trace completion */\r
+               if (buf_get_u32(etb_status_reg->value, 2, 1) == 1)\r
+               {\r
+                       while (etb_timeout-- && (buf_get_u32(etb_status_reg->value, 3, 1) == 0))\r
+                       {\r
+                               /* wait for data formatter idle */\r
+                               etb_get_reg(etb_status_reg);\r
+                       }\r
+                       \r
+                       if (etb_timeout == 0)\r
+                       {\r
+                               ERROR("AcqComp set but DFEmpty won't go high, ETB status: 0x%x",\r
+                                       buf_get_u32(etb_status_reg->value, 0, etb_status_reg->size));\r
+                       }\r
+                       \r
+                       if (!(etm_ctx->capture_status && TRACE_TRIGGERED))\r
+                       {\r
+                               ERROR("trace completed, but no trigger condition detected");\r
+                       }\r
+                       \r
+                       etm_ctx->capture_status &= ~TRACE_RUNNING;\r
+                       etm_ctx->capture_status |= TRACE_COMPLETED;\r
+               }\r
+       }\r
+       \r
+       return etm_ctx->capture_status;\r
+}\r
+\r
+int etb_read_trace(etm_context_t *etm_ctx)\r
+{\r
+       etb_t *etb = etm_ctx->capture_driver_priv;\r
+       int first_frame = 0;\r
+       int num_frames = etb->ram_depth;\r
+       u32 *trace_data = NULL;\r
+       int i, j;\r
+       \r
+       etb_read_reg(&etb->reg_cache->reg_list[ETB_STATUS]);\r
+       etb_read_reg(&etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER]);\r
+       jtag_execute_queue();\r
+       \r
+       /* check if we overflowed, and adjust first frame of the trace accordingly\r
+        * if we didn't overflow, read only up to the frame that would be written next,\r
+        * i.e. don't read invalid entries\r
+        */\r
+       if (buf_get_u32(etb->reg_cache->reg_list[ETB_STATUS].value, 0, 1))\r
+       {\r
+               first_frame = buf_get_u32(etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER].value, 0, 32);\r
+       }\r
+       else\r
+       {\r
+               num_frames = buf_get_u32(etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER].value, 0, 32);\r
+       }\r
+       \r
+       etb_write_reg(&etb->reg_cache->reg_list[ETB_RAM_READ_POINTER], first_frame);\r
+\r
+       /* read data into temporary array for unpacking */      \r
+       trace_data = malloc(sizeof(u32) * num_frames);\r
+       etb_read_ram(etb, trace_data, num_frames);\r
+\r
+       if (etm_ctx->trace_depth > 0)\r
+       {\r
+               free(etm_ctx->trace_data);\r
+       }\r
+       \r
+       if ((etm_ctx->portmode & ETM_PORT_WIDTH_MASK) == ETM_PORT_4BIT)\r
+               etm_ctx->trace_depth = num_frames * 3;\r
+       else if ((etm_ctx->portmode & ETM_PORT_WIDTH_MASK) == ETM_PORT_8BIT)\r
+               etm_ctx->trace_depth = num_frames * 2;\r
+       else\r
+               etm_ctx->trace_depth = num_frames;\r
+\r
+       etm_ctx->trace_data = malloc(sizeof(etmv1_trace_data_t) * etm_ctx->trace_depth);\r
+       \r
+       for (i = 0, j = 0; i < num_frames; i++)\r
+       {\r
+               if ((etm_ctx->portmode & ETM_PORT_WIDTH_MASK) == ETM_PORT_4BIT)\r
+               {\r
+                       /* trace word j */\r
+                       etm_ctx->trace_data[j].pipestat = trace_data[i] & 0x7;\r
+                       etm_ctx->trace_data[j].packet = (trace_data[i] & 0x78) >> 3;\r
+                       etm_ctx->trace_data[j].flags = 0;\r
+                       if ((trace_data[i] & 0x80) >> 7)\r
+                       {\r
+                               etm_ctx->trace_data[j].flags |= ETMV1_TRACESYNC_CYCLE;\r
+                       }\r
+                       if (etm_ctx->trace_data[j].pipestat == STAT_TR)\r
+                       {\r
+                               etm_ctx->trace_data[j].pipestat = etm_ctx->trace_data[j].packet & 0x7;\r
+                               etm_ctx->trace_data[j].flags |= ETMV1_TRIGGER_CYCLE;\r
+                       }\r
+                       \r
+                       /* trace word j+1 */\r
+                       etm_ctx->trace_data[j+1].pipestat = (trace_data[i] & 0x100) >> 8;\r
+                       etm_ctx->trace_data[j+1].packet = (trace_data[i] & 0x7800) >> 11;\r
+                       etm_ctx->trace_data[j+1].flags = 0;\r
+                       if ((trace_data[i] & 0x8000) >> 15)\r
+                       {\r
+                               etm_ctx->trace_data[j+1].flags |= ETMV1_TRACESYNC_CYCLE;\r
+                       }\r
+                       if (etm_ctx->trace_data[j+1].pipestat == STAT_TR)\r
+                       {\r
+                               etm_ctx->trace_data[j+1].pipestat = etm_ctx->trace_data[j+1].packet & 0x7;\r
+                               etm_ctx->trace_data[j+1].flags |= ETMV1_TRIGGER_CYCLE;\r
+                       }\r
+                       \r
+                       /* trace word j+2 */\r
+                       etm_ctx->trace_data[j+2].pipestat = (trace_data[i] & 0x10000) >> 16;\r
+                       etm_ctx->trace_data[j+2].packet = (trace_data[i] & 0x780000) >> 19;\r
+                       etm_ctx->trace_data[j+2].flags = 0;\r
+                       if ((trace_data[i] & 0x800000) >> 23)\r
+                       {\r
+                               etm_ctx->trace_data[j+2].flags |= ETMV1_TRACESYNC_CYCLE;\r
+                       }\r
+                       if (etm_ctx->trace_data[j+2].pipestat == STAT_TR)\r
+                       {\r
+                               etm_ctx->trace_data[j+2].pipestat = etm_ctx->trace_data[j+2].packet & 0x7;\r
+                               etm_ctx->trace_data[j+2].flags |= ETMV1_TRIGGER_CYCLE;\r
+                       }\r
+                       \r
+                       j += 3;\r
+               }\r
+               else if ((etm_ctx->portmode & ETM_PORT_WIDTH_MASK) == ETM_PORT_8BIT)\r
+               {\r
+                       /* trace word j */\r
+                       etm_ctx->trace_data[j].pipestat = trace_data[i] & 0x7;\r
+                       etm_ctx->trace_data[j].packet = (trace_data[i] & 0x7f8) >> 3;\r
+                       etm_ctx->trace_data[j].flags = 0;\r
+                       if ((trace_data[i] & 0x800) >> 11)\r
+                       {\r
+                               etm_ctx->trace_data[j].flags |= ETMV1_TRACESYNC_CYCLE;\r
+                       }\r
+                       if (etm_ctx->trace_data[j].pipestat == STAT_TR)\r
+                       {\r
+                               etm_ctx->trace_data[j].pipestat = etm_ctx->trace_data[j].packet & 0x7;\r
+                               etm_ctx->trace_data[j].flags |= ETMV1_TRIGGER_CYCLE;\r
+                       }\r
+\r
+                       /* trace word j+1 */\r
+                       etm_ctx->trace_data[j+1].pipestat = (trace_data[i] & 0x7000) >> 12;\r
+                       etm_ctx->trace_data[j+1].packet = (trace_data[i] & 0x7f8000) >> 15;\r
+                       etm_ctx->trace_data[j+1].flags = 0;\r
+                       if ((trace_data[i] & 0x800000) >> 23)\r
+                       {\r
+                               etm_ctx->trace_data[j+1].flags |= ETMV1_TRACESYNC_CYCLE;\r
+                       }\r
+                       if (etm_ctx->trace_data[j+1].pipestat == STAT_TR)\r
+                       {\r
+                               etm_ctx->trace_data[j+1].pipestat = etm_ctx->trace_data[j+1].packet & 0x7;\r
+                               etm_ctx->trace_data[j+1].flags |= ETMV1_TRIGGER_CYCLE;\r
+                       }\r
+                       \r
+                       j += 2;\r
+               }\r
+               else\r
+               {\r
+                       /* trace word j */\r
+                       etm_ctx->trace_data[j].pipestat = trace_data[i] & 0x7;\r
+                       etm_ctx->trace_data[j].packet = (trace_data[i] & 0x7fff8) >> 3;\r
+                       etm_ctx->trace_data[j].flags = 0;\r
+                       if ((trace_data[i] & 0x80000) >> 19)\r
+                       {\r
+                               etm_ctx->trace_data[j].flags |= ETMV1_TRACESYNC_CYCLE;\r
+                       }\r
+                       if (etm_ctx->trace_data[j].pipestat == STAT_TR)\r
+                       {\r
+                               etm_ctx->trace_data[j].pipestat = etm_ctx->trace_data[j].packet & 0x7;\r
+                               etm_ctx->trace_data[j].flags |= ETMV1_TRIGGER_CYCLE;\r
+                       }\r
+                       \r
+                       j += 1;\r
+               }\r
+       }\r
+       \r
+       free(trace_data);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int etb_start_capture(etm_context_t *etm_ctx)\r
+{\r
+       etb_t *etb = etm_ctx->capture_driver_priv;\r
+       u32 etb_ctrl_value = 0x1;\r
+       u32 trigger_count;\r
+\r
+       if ((etm_ctx->portmode & ETM_PORT_MODE_MASK) == ETM_PORT_DEMUXED)\r
+       {\r
+               if ((etm_ctx->portmode & ETM_PORT_WIDTH_MASK) != ETM_PORT_8BIT)\r
+               {\r
+                       ERROR("ETB can't run in demultiplexed mode with a 4 or 16 bit port");\r
+                       return ERROR_ETM_PORTMODE_NOT_SUPPORTED;\r
+               }\r
+               etb_ctrl_value |= 0x2;\r
+       }\r
+       \r
+       if ((etm_ctx->portmode & ETM_PORT_MODE_MASK) == ETM_PORT_MUXED)\r
+               return ERROR_ETM_PORTMODE_NOT_SUPPORTED;\r
+       \r
+       trigger_count = (etb->ram_depth * etm_ctx->trigger_percent) / 100;\r
+       \r
+       etb_write_reg(&etb->reg_cache->reg_list[ETB_TRIGGER_COUNTER], trigger_count);\r
+       etb_write_reg(&etb->reg_cache->reg_list[ETB_RAM_WRITE_POINTER], 0x0);\r
+       etb_write_reg(&etb->reg_cache->reg_list[ETB_CTRL], etb_ctrl_value);\r
+       jtag_execute_queue();\r
+       \r
+       /* we're starting a new trace, initialize capture status */\r
+       etm_ctx->capture_status = TRACE_RUNNING;\r
+       \r
+       return ERROR_OK; \r
+}\r
+\r
+int etb_stop_capture(etm_context_t *etm_ctx)\r
+{\r
+       etb_t *etb = etm_ctx->capture_driver_priv;\r
+       reg_t *etb_ctrl_reg = &etb->reg_cache->reg_list[ETB_CTRL];\r
+\r
+       etb_write_reg(etb_ctrl_reg, 0x0);\r
+       jtag_execute_queue();\r
+       \r
+       /* trace stopped, just clear running flag, but preserve others */ \r
+       etm_ctx->capture_status &= ~TRACE_RUNNING;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+etm_capture_driver_t etb_capture_driver =\r
+{\r
+       .name = "etb",\r
+       .register_commands = etb_register_commands,\r
+       .init = etb_init,\r
+       .status = etb_status,\r
+       .start_capture = etb_start_capture,\r
+       .stop_capture = etb_stop_capture,\r
+       .read_trace = etb_read_trace,\r
+};\r
index 0d20b0d5866060f37bddeef1d9c4b00277960d6d..fe4ac77f6dd7386456b69edc828e17981a14a57d 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2005 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include <string.h>
-
-#include "etm.h"
-#include "etb.h"
-
-#include "armv4_5.h"
-#include "arm7_9_common.h"
-#include "arm_disassembler.h"
-#include "arm_simulator.h"
-
-#include "log.h"
-#include "arm_jtag.h"
-#include "types.h"
-#include "binarybuffer.h"
-#include "target.h"
-#include "register.h"
-#include "jtag.h"
-#include "fileio.h"
-
-#include <stdlib.h>
-
-/* ETM register access functionality 
- * 
- */
-
-bitfield_desc_t etm_comms_ctrl_bitfield_desc[] = 
-{
-       {"R", 1},
-       {"W", 1},
-       {"reserved", 26},
-       {"version", 4}
-};
-
-int etm_reg_arch_info[] =
-{
-       0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-       0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
-       0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 
-       0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 
-       0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27,
-       0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 
-       0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37,
-       0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f,
-       0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 
-       0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f, 
-       0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57,
-       0x58, 0x59, 0x5a, 0x5b, 0x5c, 0x5d, 0x5e, 0x5f,
-       0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 0x67, 
-       0x68, 0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f, 
-};
-
-int etm_reg_arch_size_info[] =
-{
-       32, 32, 17, 8, 3, 9, 32, 16,
-       17, 26, 25, 8, 17, 32, 32, 17,
-       32, 32, 32, 32, 32, 32, 32, 32, 
-       32, 32, 32, 32, 32, 32, 32, 32, 
-       7, 7, 7, 7, 7, 7, 7, 7, 
-       7, 7, 7, 7, 7, 7, 7, 7, 
-       32, 32, 32, 32, 32, 32, 32, 32, 
-       32, 32, 32, 32, 32, 32, 32, 32, 
-       32, 32, 32, 32, 32, 32, 32, 32, 
-       32, 32, 32, 32, 32, 32, 32, 32, 
-       16, 16, 16, 16, 18, 18, 18, 18,
-       17, 17, 17, 17, 16, 16, 16, 16,
-       17, 17, 17, 17, 17, 17, 2, 
-       17, 17, 17, 17, 32, 32, 32, 32 
-};
-
-char* etm_reg_list[] =
-{
-       "ETM_CTRL",
-       "ETM_CONFIG",
-       "ETM_TRIG_EVENT",
-       "ETM_MMD_CTRL",
-       "ETM_STATUS",
-       "ETM_SYS_CONFIG",
-       "ETM_TRACE_RESOURCE_CTRL",
-       "ETM_TRACE_EN_CTRL2",
-       "ETM_TRACE_EN_EVENT",
-       "ETM_TRACE_EN_CTRL1",
-       "ETM_FIFOFULL_REGION",
-       "ETM_FIFOFULL_LEVEL",
-       "ETM_VIEWDATA_EVENT",
-       "ETM_VIEWDATA_CTRL1",
-       "ETM_VIEWDATA_CTRL2",
-       "ETM_VIEWDATA_CTRL3",
-       "ETM_ADDR_COMPARATOR_VALUE1",
-       "ETM_ADDR_COMPARATOR_VALUE2",
-       "ETM_ADDR_COMPARATOR_VALUE3",
-       "ETM_ADDR_COMPARATOR_VALUE4",
-       "ETM_ADDR_COMPARATOR_VALUE5",
-       "ETM_ADDR_COMPARATOR_VALUE6",
-       "ETM_ADDR_COMPARATOR_VALUE7",
-       "ETM_ADDR_COMPARATOR_VALUE8",
-       "ETM_ADDR_COMPARATOR_VALUE9",
-       "ETM_ADDR_COMPARATOR_VALUE10",
-       "ETM_ADDR_COMPARATOR_VALUE11",
-       "ETM_ADDR_COMPARATOR_VALUE12",
-       "ETM_ADDR_COMPARATOR_VALUE13",
-       "ETM_ADDR_COMPARATOR_VALUE14",
-       "ETM_ADDR_COMPARATOR_VALUE15",
-       "ETM_ADDR_COMPARATOR_VALUE16",
-       "ETM_ADDR_ACCESS_TYPE1",
-       "ETM_ADDR_ACCESS_TYPE2",
-       "ETM_ADDR_ACCESS_TYPE3",
-       "ETM_ADDR_ACCESS_TYPE4",
-       "ETM_ADDR_ACCESS_TYPE5",
-       "ETM_ADDR_ACCESS_TYPE6",
-       "ETM_ADDR_ACCESS_TYPE7",
-       "ETM_ADDR_ACCESS_TYPE8",
-       "ETM_ADDR_ACCESS_TYPE9",
-       "ETM_ADDR_ACCESS_TYPE10",
-       "ETM_ADDR_ACCESS_TYPE11",
-       "ETM_ADDR_ACCESS_TYPE12",
-       "ETM_ADDR_ACCESS_TYPE13",
-       "ETM_ADDR_ACCESS_TYPE14",
-       "ETM_ADDR_ACCESS_TYPE15",
-       "ETM_ADDR_ACCESS_TYPE16",
-       "ETM_DATA_COMPARATOR_VALUE1",
-       "ETM_DATA_COMPARATOR_VALUE2",
-       "ETM_DATA_COMPARATOR_VALUE3",
-       "ETM_DATA_COMPARATOR_VALUE4",
-       "ETM_DATA_COMPARATOR_VALUE5",
-       "ETM_DATA_COMPARATOR_VALUE6",
-       "ETM_DATA_COMPARATOR_VALUE7",
-       "ETM_DATA_COMPARATOR_VALUE8",
-       "ETM_DATA_COMPARATOR_VALUE9",
-       "ETM_DATA_COMPARATOR_VALUE10",
-       "ETM_DATA_COMPARATOR_VALUE11",
-       "ETM_DATA_COMPARATOR_VALUE12",
-       "ETM_DATA_COMPARATOR_VALUE13",
-       "ETM_DATA_COMPARATOR_VALUE14",
-       "ETM_DATA_COMPARATOR_VALUE15",
-       "ETM_DATA_COMPARATOR_VALUE16",
-       "ETM_DATA_COMPARATOR_MASK1",
-       "ETM_DATA_COMPARATOR_MASK2",
-       "ETM_DATA_COMPARATOR_MASK3",
-       "ETM_DATA_COMPARATOR_MASK4",
-       "ETM_DATA_COMPARATOR_MASK5",
-       "ETM_DATA_COMPARATOR_MASK6",
-       "ETM_DATA_COMPARATOR_MASK7",
-       "ETM_DATA_COMPARATOR_MASK8",
-       "ETM_DATA_COMPARATOR_MASK9",
-       "ETM_DATA_COMPARATOR_MASK10",
-       "ETM_DATA_COMPARATOR_MASK11",
-       "ETM_DATA_COMPARATOR_MASK12",
-       "ETM_DATA_COMPARATOR_MASK13",
-       "ETM_DATA_COMPARATOR_MASK14",
-       "ETM_DATA_COMPARATOR_MASK15",
-       "ETM_DATA_COMPARATOR_MASK16",
-       "ETM_COUNTER_INITAL_VALUE1",
-       "ETM_COUNTER_INITAL_VALUE2",
-       "ETM_COUNTER_INITAL_VALUE3",
-       "ETM_COUNTER_INITAL_VALUE4",
-       "ETM_COUNTER_ENABLE1",
-       "ETM_COUNTER_ENABLE2",
-       "ETM_COUNTER_ENABLE3",
-       "ETM_COUNTER_ENABLE4",
-       "ETM_COUNTER_RELOAD_VALUE1",
-       "ETM_COUNTER_RELOAD_VALUE2",
-       "ETM_COUNTER_RELOAD_VALUE3",
-       "ETM_COUNTER_RELOAD_VALUE4",
-       "ETM_COUNTER_VALUE1",
-       "ETM_COUNTER_VALUE2",
-       "ETM_COUNTER_VALUE3",
-       "ETM_COUNTER_VALUE4",
-       "ETM_SEQUENCER_CTRL1",
-       "ETM_SEQUENCER_CTRL2",
-       "ETM_SEQUENCER_CTRL3",
-       "ETM_SEQUENCER_CTRL4",
-       "ETM_SEQUENCER_CTRL5",
-       "ETM_SEQUENCER_CTRL6",
-       "ETM_SEQUENCER_STATE",
-       "ETM_EXTERNAL_OUTPUT1",
-       "ETM_EXTERNAL_OUTPUT2",
-       "ETM_EXTERNAL_OUTPUT3",
-       "ETM_EXTERNAL_OUTPUT4",
-       "ETM_CONTEXTID_COMPARATOR_VALUE1",
-       "ETM_CONTEXTID_COMPARATOR_VALUE2",
-       "ETM_CONTEXTID_COMPARATOR_VALUE3",
-       "ETM_CONTEXTID_COMPARATOR_MASK"
-};  
-
-int etm_reg_arch_type = -1;
-
-int etm_get_reg(reg_t *reg);
-int etm_set_reg(reg_t *reg, u32 value);
-int etm_set_reg_w_exec(reg_t *reg, u8 *buf);
-
-int etm_write_reg(reg_t *reg, u32 value);
-int etm_read_reg(reg_t *reg);
-
-command_t *etm_cmd = NULL;
-
-reg_cache_t* etm_build_reg_cache(target_t *target, arm_jtag_t *jtag_info, etm_context_t *etm_ctx)
-{
-       reg_cache_t *reg_cache = malloc(sizeof(reg_cache_t));
-       reg_t *reg_list = NULL;
-       etm_reg_t *arch_info = NULL;
-       int num_regs = sizeof(etm_reg_arch_info)/sizeof(int);
-       int i;
-       u32 etm_ctrl_value;
-       
-       /* register a register arch-type for etm registers only once */
-       if (etm_reg_arch_type == -1)
-               etm_reg_arch_type = register_reg_arch_type(etm_get_reg, etm_set_reg_w_exec);
-       
-       /* the actual registers are kept in two arrays */
-       reg_list = calloc(num_regs, sizeof(reg_t));
-       arch_info = calloc(num_regs, sizeof(etm_reg_t));
-       
-       /* fill in values for the reg cache */
-       reg_cache->name = "etm registers";
-       reg_cache->next = NULL;
-       reg_cache->reg_list = reg_list;
-       reg_cache->num_regs = num_regs;
-       
-       /* set up registers */
-       for (i = 0; i < num_regs; i++)
-       {
-               reg_list[i].name = etm_reg_list[i];
-               reg_list[i].size = 32;
-               reg_list[i].dirty = 0;
-               reg_list[i].valid = 0;
-               reg_list[i].bitfield_desc = NULL;
-               reg_list[i].num_bitfields = 0;
-               reg_list[i].value = calloc(1, 4);
-               reg_list[i].arch_info = &arch_info[i];
-               reg_list[i].arch_type = etm_reg_arch_type;
-               reg_list[i].size = etm_reg_arch_size_info[i];
-               arch_info[i].addr = etm_reg_arch_info[i];
-               arch_info[i].jtag_info = jtag_info;
-       }
-
-       /* initialize some ETM control register settings */     
-       etm_get_reg(&reg_list[ETM_CTRL]);
-       etm_ctrl_value = buf_get_u32(reg_list[ETM_CTRL].value, 0, reg_list[ETM_CTRL].size);
-       
-       /* clear the ETM powerdown bit (0) */
-       etm_ctrl_value &= ~0x1;
-               
-       /* configure port width (6:4), mode (17:16) and clocking (13) */
-       etm_ctrl_value = (etm_ctrl_value & 
-               ~ETM_PORT_WIDTH_MASK & ~ETM_PORT_MODE_MASK & ~ETM_PORT_CLOCK_MASK)
-               | etm_ctx->portmode;
-       
-       buf_set_u32(reg_list[ETM_CTRL].value, 0, reg_list[ETM_CTRL].size, etm_ctrl_value);
-       etm_store_reg(&reg_list[ETM_CTRL]);
-       
-       /* the ETM might have an ETB connected */
-       if (strcmp(etm_ctx->capture_driver->name, "etb") == 0)
-       {
-               etb_t *etb = etm_ctx->capture_driver_priv;
-               
-               if (!etb)
-               {
-                       ERROR("etb selected as etm capture driver, but no ETB configured");
-                       return ERROR_OK;
-               }
-               
-               reg_cache->next = etb_build_reg_cache(etb);
-               
-               etb->reg_cache = reg_cache->next;
-       }
-       
-       if (etm_ctx->capture_driver->init(etm_ctx) != ERROR_OK)
-       {
-               ERROR("ETM capture driver initialization failed");
-               exit(-1);
-       }
-       
-       return reg_cache;
-}
-
-int etm_get_reg(reg_t *reg)
-{
-       if (etm_read_reg(reg) != ERROR_OK)
-       {
-               ERROR("BUG: error scheduling etm register read");
-               exit(-1);
-       }
-       
-       if (jtag_execute_queue() != ERROR_OK)
-       {
-               ERROR("register read failed");
-       }
-       
-       return ERROR_OK;
-}
-
-int etm_read_reg_w_check(reg_t *reg, u8* check_value, u8* check_mask)
-{
-       etm_reg_t *etm_reg = reg->arch_info;
-       u8 reg_addr = etm_reg->addr & 0x7f;
-       scan_field_t fields[3];
-       
-       DEBUG("%i", etm_reg->addr);
-
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_scann(etm_reg->jtag_info, 0x6);
-       arm_jtag_set_instr(etm_reg->jtag_info, etm_reg->jtag_info->intest_instr, NULL);
-       
-       fields[0].device = etm_reg->jtag_info->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = reg->value;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-       
-       fields[1].device = etm_reg->jtag_info->chain_pos;
-       fields[1].num_bits = 7;
-       fields[1].out_value = malloc(1);
-       buf_set_u32(fields[1].out_value, 0, 7, reg_addr);
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = etm_reg->jtag_info->chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = malloc(1);
-       buf_set_u32(fields[2].out_value, 0, 1, 0);
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(3, fields, -1, NULL);
-       
-       fields[0].in_value = reg->value;
-       jtag_set_check_value(fields+0, check_value, check_mask, NULL);  
-               
-       jtag_add_dr_scan(3, fields, -1, NULL);
-
-       free(fields[1].out_value);
-       free(fields[2].out_value);
-       
-       return ERROR_OK;
-}
-
-int etm_read_reg(reg_t *reg)
-{
-       return etm_read_reg_w_check(reg, NULL, NULL);   
-}
-
-int etm_set_reg(reg_t *reg, u32 value)
-{
-       if (etm_write_reg(reg, value) != ERROR_OK)
-       {
-               ERROR("BUG: error scheduling etm register write");
-               exit(-1);
-       }
-       
-       buf_set_u32(reg->value, 0, reg->size, value);
-       reg->valid = 1;
-       reg->dirty = 0;
-       
-       return ERROR_OK;
-}
-
-int etm_set_reg_w_exec(reg_t *reg, u8 *buf)
-{
-       etm_set_reg(reg, buf_get_u32(buf, 0, reg->size));
-       
-       if (jtag_execute_queue() != ERROR_OK)
-       {
-               ERROR("register write failed");
-               exit(-1);
-       }
-       return ERROR_OK;
-}
-
-int etm_write_reg(reg_t *reg, u32 value)
-{
-       etm_reg_t *etm_reg = reg->arch_info;
-       u8 reg_addr = etm_reg->addr & 0x7f;
-       scan_field_t fields[3];
-       
-       DEBUG("%i: 0x%8.8x", etm_reg->addr, value);
-       
-       jtag_add_end_state(TAP_RTI);
-       arm_jtag_scann(etm_reg->jtag_info, 0x6);
-       arm_jtag_set_instr(etm_reg->jtag_info, etm_reg->jtag_info->intest_instr, NULL);
-       
-       fields[0].device = etm_reg->jtag_info->chain_pos;
-       fields[0].num_bits = 32;
-       fields[0].out_value = malloc(4);
-       buf_set_u32(fields[0].out_value, 0, 32, value);
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-       
-       fields[1].device = etm_reg->jtag_info->chain_pos;
-       fields[1].num_bits = 7;
-       fields[1].out_value = malloc(1);
-       buf_set_u32(fields[1].out_value, 0, 7, reg_addr);
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       fields[2].device = etm_reg->jtag_info->chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = malloc(1);
-       buf_set_u32(fields[2].out_value, 0, 1, 1);
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_check_value = NULL;
-       fields[2].in_check_mask = NULL;
-       fields[2].in_handler = NULL;
-       fields[2].in_handler_priv = NULL;
-       
-       jtag_add_dr_scan(3, fields, -1, NULL);
-       
-       free(fields[0].out_value);
-       free(fields[1].out_value);
-       free(fields[2].out_value);
-       
-       return ERROR_OK;
-}
-
-int etm_store_reg(reg_t *reg)
-{
-       return etm_write_reg(reg, buf_get_u32(reg->value, 0, reg->size));
-}
-
-/* ETM trace analysis functionality
- * 
- */
-extern etm_capture_driver_t etb_capture_driver;
-extern etm_capture_driver_t etm_dummy_capture_driver;
-#if BUILD_OOCD_TRACE == 1
-extern etm_capture_driver_t oocd_trace_capture_driver;
-#endif
-
-etm_capture_driver_t *etm_capture_drivers[] = 
-{
-       &etb_capture_driver,
-       &etm_dummy_capture_driver,
-#if BUILD_OOCD_TRACE == 1
-       &oocd_trace_capture_driver,
-#endif
-       NULL
-};
-
-char *etmv1v1_branch_reason_strings[] =
-{
-       "normal PC change",
-       "tracing enabled",
-       "trace restarted after overflow",
-       "exit from debug",
-       "periodic synchronization",
-       "reserved",
-       "reserved",
-       "reserved",
-};
-
-int etm_read_instruction(etm_context_t *ctx, arm_instruction_t *instruction)
-{
-       int i;
-       int section = -1;
-       u32 size_read;
-       u32 opcode;
-       int retval;
-       
-       if (!ctx->image)
-               return ERROR_TRACE_IMAGE_UNAVAILABLE;
-       
-       /* search for the section the current instruction belongs to */ 
-       for (i = 0; i < ctx->image->num_sections; i++)
-       {
-               if ((ctx->image->sections[i].base_address <= ctx->current_pc) &&
-                       (ctx->image->sections[i].base_address + ctx->image->sections[i].size > ctx->current_pc))
-               {
-                       section = i;
-                       break;
-               }
-       }
-       
-       if (section == -1)
-       {
-               /* current instruction couldn't be found in the image */
-               return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;
-       }
-       
-       if (ctx->core_state == ARMV4_5_STATE_ARM)
-       {
-               u8 buf[4];
-               if ((retval = image_read_section(ctx->image, section, 
-                       ctx->current_pc - ctx->image->sections[section].base_address,
-                       4, buf, &size_read)) != ERROR_OK)
-               {
-                       ERROR("error while reading instruction: %i", retval);
-                       return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;
-               }
-               opcode = target_buffer_get_u32(ctx->target, buf);
-               arm_evaluate_opcode(opcode, ctx->current_pc, instruction);
-       }
-       else if (ctx->core_state == ARMV4_5_STATE_THUMB)
-       {
-               u8 buf[2];
-               if ((retval = image_read_section(ctx->image, section, 
-                       ctx->current_pc - ctx->image->sections[section].base_address,
-                       2, buf, &size_read)) != ERROR_OK)
-               {
-                       ERROR("error while reading instruction: %i", retval);
-                       return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;
-               }
-               opcode = target_buffer_get_u16(ctx->target, buf);
-               thumb_evaluate_opcode(opcode, ctx->current_pc, instruction);
-       }
-       else if (ctx->core_state == ARMV4_5_STATE_JAZELLE)
-       {
-               ERROR("BUG: tracing of jazelle code not supported");
-               exit(-1);
-       }
-       else
-       {
-               ERROR("BUG: unknown core state encountered");
-               exit(-1);
-       }
-       
-       return ERROR_OK;
-}
-
-int etmv1_next_packet(etm_context_t *ctx, u8 *packet, int apo)
-{
-       while (ctx->data_index < ctx->trace_depth)
-       {
-               /* if the caller specified an address packet offset, skip until the
-                * we reach the n-th cycle marked with tracesync */
-               if (apo > 0)
-               {
-                       if (ctx->trace_data[ctx->data_index].flags & ETMV1_TRACESYNC_CYCLE)
-                               apo--;
-                       
-                       if (apo > 0)
-                       {
-                               ctx->data_index++;
-                               ctx->data_half = 0;
-                       }
-                       continue;
-               }
-               
-               /* no tracedata output during a TD cycle
-                * or in a trigger cycle */
-               if ((ctx->trace_data[ctx->data_index].pipestat == STAT_TD)
-                       || (ctx->trace_data[ctx->data_index].flags & ETMV1_TRIGGER_CYCLE))
-               {
-                       ctx->data_index++;
-                       ctx->data_half = 0;
-                       continue;
-               }
-               
-               if ((ctx->portmode & ETM_PORT_WIDTH_MASK) == ETM_PORT_16BIT)
-               {
-                       if (ctx->data_half == 0)
-                       {
-                               *packet = ctx->trace_data[ctx->data_index].packet & 0xff;
-                               ctx->data_half = 1;
-                       }
-                       else
-                       {
-                               *packet = (ctx->trace_data[ctx->data_index].packet & 0xff00) >> 8;
-                               ctx->data_half = 0;
-                               ctx->data_index++;
-                       }
-               }
-               else if ((ctx->portmode & ETM_PORT_WIDTH_MASK) == ETM_PORT_8BIT)
-               {
-                       *packet = ctx->trace_data[ctx->data_index].packet & 0xff;
-                       ctx->data_index++;
-               }
-               else
-               {
-                       /* on a 4-bit port, a packet will be output during two consecutive cycles */
-                       if (ctx->data_index > (ctx->trace_depth - 2))
-                               return -1;
-                       
-                       *packet = ctx->trace_data[ctx->data_index].packet & 0xf;
-                       *packet |= (ctx->trace_data[ctx->data_index + 1].packet & 0xf) << 4;
-                       ctx->data_index += 2;
-               }
-                                       
-               return 0;
-       }
-       
-       return -1;
-}
-
-int etmv1_branch_address(etm_context_t *ctx)
-{
-       int retval;
-       u8 packet;
-       int shift = 0;
-       int apo;
-       int i;
-       
-       /* quit analysis if less than two cycles are left in the trace
-        * because we can't extract the APO */
-       if (ctx->data_index > (ctx->trace_depth - 2))
-               return -1;
-               
-       /* a BE could be output during an APO cycle, skip the current
-        * and continue with the new one */
-       if (ctx->trace_data[ctx->pipe_index + 1].pipestat & 0x4)
-               return 1;
-       if (ctx->trace_data[ctx->pipe_index + 2].pipestat & 0x4)
-               return 2;
-               
-       /* address packet offset encoded in the next two cycles' pipestat bits */
-       apo = ctx->trace_data[ctx->pipe_index + 1].pipestat & 0x3;
-       apo |= (ctx->trace_data[ctx->pipe_index + 2].pipestat & 0x3) << 2;
-       
-       /* count number of tracesync cycles between current pipe_index and data_index
-        * i.e. the number of tracesyncs that data_index already passed by
-        * to subtract them from the APO */
-       for (i = ctx->pipe_index; i < ctx->data_index; i++)
-       {
-               if (ctx->trace_data[ctx->pipe_index + 1].pipestat & ETMV1_TRACESYNC_CYCLE)
-                       apo--;
-       }
-       
-       /* extract up to four 7-bit packets */
-       do {
-               if ((retval = etmv1_next_packet(ctx, &packet, (shift == 0) ? apo + 1 : 0)) != 0)
-                       return -1;
-               ctx->last_branch &= ~(0x7f << shift);
-               ctx->last_branch |= (packet & 0x7f) << shift;
-               shift += 7;
-       } while ((packet & 0x80) && (shift < 28));
-       
-       /* one last packet holding 4 bits of the address, plus the branch reason code */
-       if ((shift == 28) && (packet & 0x80))
-       {
-               if ((retval = etmv1_next_packet(ctx, &packet, 0)) != 0)
-                       return -1;
-               ctx->last_branch &= 0x0fffffff;
-               ctx->last_branch |= (packet & 0x0f) << 28;
-               ctx->last_branch_reason = (packet & 0x70) >> 4;
-               shift += 4;
-       }
-       else
-       {
-               ctx->last_branch_reason = 0;
-       }
-       
-       if (shift == 32)
-       {
-               ctx->pc_ok = 1;
-       }
-       
-       /* if a full address was output, we might have branched into Jazelle state */
-       if ((shift == 32) && (packet & 0x80))
-       {
-               ctx->core_state = ARMV4_5_STATE_JAZELLE;
-       }
-       else
-       {
-               /* if we didn't branch into Jazelle state, the current processor state is
-                * encoded in bit 0 of the branch target address */
-               if (ctx->last_branch & 0x1)
-               {
-                       ctx->core_state = ARMV4_5_STATE_THUMB;
-                       ctx->last_branch &= ~0x1;
-               }
-               else
-               {
-                       ctx->core_state = ARMV4_5_STATE_ARM;
-                       ctx->last_branch &= ~0x3;
-               }
-       }
-       
-       return 0;
-}
-
-int etmv1_data(etm_context_t *ctx, int size, u32 *data)
-{
-       int j;
-       u8 buf[4];
-       int retval;
-       
-       for (j = 0; j < size; j++)
-       {
-               if ((retval = etmv1_next_packet(ctx, &buf[j], 0)) != 0)
-                       return -1;
-       }
-       
-       if (size == 8)
-               ERROR("TODO: add support for 64-bit values");
-       else if (size == 4)
-               *data = target_buffer_get_u32(ctx->target, buf);
-       else if (size == 2)
-               *data = target_buffer_get_u16(ctx->target, buf);
-       else if (size == 1)
-               *data = buf[0];
-               
-       return 0;
-}
-
-int etmv1_analyze_trace(etm_context_t *ctx, struct command_context_s *cmd_ctx)
-{
-       int retval;
-       arm_instruction_t instruction;
-       
-       /* read the trace data if it wasn't read already */
-       if (ctx->trace_depth == 0)
-               ctx->capture_driver->read_trace(ctx);
-       
-       /* start at the beginning of the captured trace */
-       ctx->pipe_index = 0;
-       ctx->data_index = 0;
-       ctx->data_half = 0;
-
-       /* neither the PC nor the data pointer are valid */     
-       ctx->pc_ok = 0;
-       ctx->ptr_ok = 0;
-       
-       while (ctx->pipe_index < ctx->trace_depth)
-       {
-               u8 pipestat = ctx->trace_data[ctx->pipe_index].pipestat;
-               u32 next_pc = ctx->current_pc;
-               u32 old_data_index = ctx->data_index;
-               u32 old_data_half = ctx->data_half;
-               u32 old_index = ctx->pipe_index;
-               u32 last_instruction = ctx->last_instruction;
-               u32 cycles = 0;
-               int current_pc_ok = ctx->pc_ok;
-               
-               if (ctx->trace_data[ctx->pipe_index].flags & ETMV1_TRIGGER_CYCLE)
-               {
-                       command_print(cmd_ctx, "--- trigger ---");
-               }
-
-               /* instructions execute in IE/D or BE/D cycles */
-               if ((pipestat == STAT_IE) || (pipestat == STAT_ID))
-                       ctx->last_instruction = ctx->pipe_index;
-               
-               /* if we don't have a valid pc skip until we reach an indirect branch */
-               if ((!ctx->pc_ok) && (pipestat != STAT_BE))
-               {
-                       ctx->pipe_index++;
-                       continue;
-               }
-               
-               /* any indirect branch could have interrupted instruction flow
-                * - the branch reason code could indicate a trace discontinuity
-                * - a branch to the exception vectors indicates an exception
-                */
-               if ((pipestat == STAT_BE) || (pipestat == STAT_BD))
-               {
-                       /* backup current data index, to be able to consume the branch address
-                        * before examining data address and values
-                        */
-                       old_data_index = ctx->data_index;
-                       old_data_half = ctx->data_half;
-
-                       ctx->last_instruction = ctx->pipe_index;
-                       
-                       if ((retval = etmv1_branch_address(ctx)) != 0)
-                       {
-                               /* negative return value from etmv1_branch_address means we ran out of packets,
-                                * quit analysing the trace */
-                               if (retval < 0)
-                                       break;
-                               
-                               /* a positive return values means the current branch was abandoned,
-                                * and a new branch was encountered in cycle ctx->pipe_index + retval;
-                                */
-                               WARNING("abandoned branch encountered, correctnes of analysis uncertain");
-                               ctx->pipe_index += retval;
-                               continue;
-                       }
-                       
-                       /* skip over APO cycles */
-                       ctx->pipe_index += 2;
-                       
-                       switch (ctx->last_branch_reason)
-                       {
-                               case 0x0:       /* normal PC change */
-                                       next_pc = ctx->last_branch;
-                                       break;
-                               case 0x1:       /* tracing enabled */
-                                       command_print(cmd_ctx, "--- tracing enabled at 0x%8.8x ---", ctx->last_branch);
-                                       ctx->current_pc = ctx->last_branch;
-                                       ctx->pipe_index++;
-                                       continue;
-                                       break;
-                               case 0x2:       /* trace restarted after FIFO overflow */
-                                       command_print(cmd_ctx, "--- trace restarted after FIFO overflow at 0x%8.8x ---", ctx->last_branch);
-                                       ctx->current_pc = ctx->last_branch;
-                                       ctx->pipe_index++;
-                                       continue;
-                                       break;
-                               case 0x3:       /* exit from debug state */
-                                       command_print(cmd_ctx, "--- exit from debug state at 0x%8.8x ---", ctx->last_branch);
-                                       ctx->current_pc = ctx->last_branch;
-                                       ctx->pipe_index++;
-                                       continue;
-                                       break;
-                               case 0x4:       /* periodic synchronization point */
-                                       next_pc = ctx->last_branch;
-                                       /* if we had no valid PC prior to this synchronization point,
-                                        * we have to move on with the next trace cycle
-                                        */
-                                       if (!current_pc_ok)
-                                       {
-                                               command_print(cmd_ctx, "--- periodic synchronization point at 0x%8.8x ---", next_pc);
-                                               ctx->current_pc = next_pc;
-                                               ctx->pipe_index++;
-                                               continue;
-                                       }
-                                       break;
-                               default:        /* reserved */
-                                       ERROR("BUG: branch reason code 0x%x is reserved", ctx->last_branch_reason);             
-                                       exit(-1);
-                                       break;
-                       }
-                       
-                       /* if we got here the branch was a normal PC change
-                        * (or a periodic synchronization point, which means the same for that matter)
-                        * if we didn't accquire a complete PC continue with the next cycle
-                        */
-                       if (!ctx->pc_ok)
-                               continue;
-                       
-                       /* indirect branch to the exception vector means an exception occured */
-                       if (((ctx->last_branch >= 0x0) && (ctx->last_branch <= 0x20))
-                               || ((ctx->last_branch >= 0xffff0000) && (ctx->last_branch <= 0xffff0020)))
-                       {
-                               if ((ctx->last_branch & 0xff) == 0x10)
-                               {
-                                       command_print(cmd_ctx, "data abort");
-                               }
-                               else
-                               {
-                                       command_print(cmd_ctx, "exception vector 0x%2.2x", ctx->last_branch);
-                                       ctx->current_pc = ctx->last_branch;
-                                       ctx->pipe_index++;
-                                       continue;
-                               }
-                       }
-               }
-               
-               /* an instruction was executed (or not, depending on the condition flags)
-                * retrieve it from the image for displaying */
-               if (ctx->pc_ok && (pipestat != STAT_WT) && (pipestat != STAT_TD) &&
-                       !(((pipestat == STAT_BE) || (pipestat == STAT_BD)) &&
-                               ((ctx->last_branch_reason != 0x0) && (ctx->last_branch_reason != 0x4))))  
-               {
-                       if ((retval = etm_read_instruction(ctx, &instruction)) != ERROR_OK)
-                       {
-                               /* can't continue tracing with no image available */
-                               if (retval == ERROR_TRACE_IMAGE_UNAVAILABLE)
-                               {
-                                       return retval;
-                               }
-                               else if (retval == ERROR_TRACE_INSTRUCTION_UNAVAILABLE)
-                               {
-                                       /* TODO: handle incomplete images 
-                                        * for now we just quit the analsysis*/
-                                       return retval;
-                               }
-                       }
-                       
-                       cycles = old_index - last_instruction;
-               }
-               
-               if ((pipestat == STAT_ID) || (pipestat == STAT_BD))
-               {
-                       u32 new_data_index = ctx->data_index;
-                       u32 new_data_half = ctx->data_half;
-                       
-                       /* in case of a branch with data, the branch target address was consumed before
-                        * we temporarily go back to the saved data index */
-                       if (pipestat == STAT_BD)
-                       {
-                               ctx->data_index = old_data_index;
-                               ctx->data_half = old_data_half;
-                       }
-                       
-                       if (ctx->tracemode & ETMV1_TRACE_ADDR)
-                       {                       
-                               u8 packet;
-                               int shift = 0;
-                               
-                               do {
-                                       if ((retval = etmv1_next_packet(ctx, &packet, 0)) != 0)
-                                               return ERROR_ETM_ANALYSIS_FAILED;
-                                       ctx->last_ptr &= ~(0x7f << shift);
-                                       ctx->last_ptr |= (packet & 0x7f) << shift;
-                                       shift += 7;
-                               } while ((packet & 0x80) && (shift < 32));
-                               
-                               if (shift >= 32)
-                                       ctx->ptr_ok = 1;
-                               
-                               if (ctx->ptr_ok)
-                               {
-                                       command_print(cmd_ctx, "address: 0x%8.8x", ctx->last_ptr);
-                               }
-                       }
-                       
-                       if (ctx->tracemode & ETMV1_TRACE_DATA)
-                       {
-                               if ((instruction.type == ARM_LDM) || (instruction.type == ARM_STM))
-                               {
-                                       int i;
-                                       for (i = 0; i < 16; i++)
-                                       {
-                                               if (instruction.info.load_store_multiple.register_list & (1 << i))
-                                               {
-                                                       u32 data;
-                                                       if (etmv1_data(ctx, 4, &data) != 0)
-                                                               return ERROR_ETM_ANALYSIS_FAILED;
-                                                       command_print(cmd_ctx, "data: 0x%8.8x", data);
-                                               }
-                                       }
-                               }
-                               else if ((instruction.type >= ARM_LDR) && (instruction.type <= ARM_STRH))
-                               {
-                                       u32 data;
-                                       if (etmv1_data(ctx, arm_access_size(&instruction), &data) != 0)
-                                               return ERROR_ETM_ANALYSIS_FAILED;
-                                       command_print(cmd_ctx, "data: 0x%8.8x", data);
-                               }
-                       }
-                       
-                       /* restore data index after consuming BD address and data */
-                       if (pipestat == STAT_BD)
-                       {
-                               ctx->data_index = new_data_index;
-                               ctx->data_half = new_data_half;
-                       }
-               }
-               
-               /* adjust PC */
-               if ((pipestat == STAT_IE) || (pipestat == STAT_ID))
-               {
-                       if (((instruction.type == ARM_B) ||
-                               (instruction.type == ARM_BL) ||
-                               (instruction.type == ARM_BLX)) &&
-                               (instruction.info.b_bl_bx_blx.target_address != -1))
-                       {
-                               next_pc = instruction.info.b_bl_bx_blx.target_address;
-                       }
-                       else
-                       {
-                               next_pc += (ctx->core_state == ARMV4_5_STATE_ARM) ? 4 : 2;
-                       }
-               }
-               else if (pipestat == STAT_IN)
-               {
-                       next_pc += (ctx->core_state == ARMV4_5_STATE_ARM) ? 4 : 2;
-               }
-
-               if ((pipestat != STAT_TD) && (pipestat != STAT_WT))
-               {
-                       char cycles_text[32] = "";
-                       
-                       /* if the trace was captured with cycle accurate tracing enabled,
-                        * output the number of cycles since the last executed instruction
-                        */
-                       if (ctx->tracemode & ETMV1_CYCLE_ACCURATE)
-                       {
-                               snprintf(cycles_text, 32, " (%i %s)",
-                                       cycles,
-                                       (cycles == 1) ? "cycle" : "cycles");
-                       }
-                       
-                       command_print(cmd_ctx, "%s%s%s",
-                               instruction.text,
-                               (pipestat == STAT_IN) ? " (not executed)" : "",
-                               cycles_text);
-
-                       ctx->current_pc = next_pc;
-                       
-                       /* packets for an instruction don't start on or before the preceding
-                        * functional pipestat (i.e. other than WT or TD)
-                        */
-                       if (ctx->data_index <= ctx->pipe_index)
-                       {
-                               ctx->data_index = ctx->pipe_index + 1;
-                               ctx->data_half = 0;
-                       }
-               }
-               
-               ctx->pipe_index += 1;
-       }
-       
-       return ERROR_OK;
-}
-
-int handle_etm_tracemode_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target;
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       etmv1_tracemode_t tracemode;
-       
-       target = get_current_target(cmd_ctx);
-       
-       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-               return ERROR_OK;
-       }
-       
-       if (!arm7_9->etm_ctx)
-       {
-               command_print(cmd_ctx, "current target doesn't have an ETM configured");
-               return ERROR_OK;
-       }
-       
-       tracemode = arm7_9->etm_ctx->tracemode;
-
-       if (argc == 4)
-       {
-               if (strcmp(args[0], "none") == 0)
-               {
-                       tracemode = ETMV1_TRACE_NONE;
-               }
-               else if (strcmp(args[0], "data") == 0)
-               {
-                       tracemode = ETMV1_TRACE_DATA;
-               }
-               else if (strcmp(args[0], "address") == 0)
-               {
-                       tracemode = ETMV1_TRACE_ADDR;
-               }
-               else if (strcmp(args[0], "all") == 0)
-               {
-                       tracemode = ETMV1_TRACE_DATA | ETMV1_TRACE_ADDR;
-               }
-               else
-               {
-                       command_print(cmd_ctx, "invalid option '%s'", args[0]);
-                       return ERROR_OK;
-               }
-               
-               switch (strtol(args[1], NULL, 0))
-               {
-                       case 0:
-                               tracemode |= ETMV1_CONTEXTID_NONE;
-                               break;
-                       case 8:
-                               tracemode |= ETMV1_CONTEXTID_8;
-                               break;
-                       case 16:
-                               tracemode |= ETMV1_CONTEXTID_16;
-                               break;
-                       case 32:
-                               tracemode |= ETMV1_CONTEXTID_32;
-                               break;
-                       default:
-                               command_print(cmd_ctx, "invalid option '%s'", args[1]);
-                               return ERROR_OK;
-               }
-               
-               if (strcmp(args[2], "enable") == 0)
-               {
-                       tracemode |= ETMV1_CYCLE_ACCURATE;
-               }
-               else if (strcmp(args[2], "disable") == 0)
-               {
-                       tracemode |= 0;
-               }
-               else
-               {
-                       command_print(cmd_ctx, "invalid option '%s'", args[2]);
-                       return ERROR_OK;
-               }
-               
-               if (strcmp(args[3], "enable") == 0)
-               {
-                       tracemode |= ETMV1_BRANCH_OUTPUT;
-               }
-               else if (strcmp(args[3], "disable") == 0)
-               {
-                       tracemode |= 0;
-               }
-               else
-               {
-                       command_print(cmd_ctx, "invalid option '%s'", args[2]);
-                       return ERROR_OK;
-               }
-       }
-       else if (argc != 0)
-       {
-               command_print(cmd_ctx, "usage: configure trace mode <none|data|address|all> <context id bits> <cycle accurate> <branch output>");
-               return ERROR_OK;
-       }
-       
-       command_print(cmd_ctx, "current tracemode configuration:");
-       
-       switch (tracemode & ETMV1_TRACE_MASK)
-       {
-               case ETMV1_TRACE_NONE:
-                       command_print(cmd_ctx, "data tracing: none");
-                       break;
-               case ETMV1_TRACE_DATA:
-                       command_print(cmd_ctx, "data tracing: data only");
-                       break;
-               case ETMV1_TRACE_ADDR:
-                       command_print(cmd_ctx, "data tracing: address only");
-                       break;
-               case ETMV1_TRACE_DATA | ETMV1_TRACE_ADDR:
-                       command_print(cmd_ctx, "data tracing: address and data");
-                       break;
-       }
-       
-       switch (tracemode & ETMV1_CONTEXTID_MASK)
-       {
-               case ETMV1_CONTEXTID_NONE:
-                       command_print(cmd_ctx, "contextid tracing: none");
-                       break;
-               case ETMV1_CONTEXTID_8:
-                       command_print(cmd_ctx, "contextid tracing: 8 bit");
-                       break;
-               case ETMV1_CONTEXTID_16:
-                       command_print(cmd_ctx, "contextid tracing: 16 bit");
-                       break;
-               case ETMV1_CONTEXTID_32:
-                       command_print(cmd_ctx, "contextid tracing: 32 bit");
-                       break;
-       }
-       
-       if (tracemode & ETMV1_CYCLE_ACCURATE)
-       {
-               command_print(cmd_ctx, "cycle-accurate tracing enabled");
-       }
-       else
-       {
-               command_print(cmd_ctx, "cycle-accurate tracing disabled");
-       }
-
-       if (tracemode & ETMV1_BRANCH_OUTPUT)
-       {
-               command_print(cmd_ctx, "full branch address output enabled");
-       }
-       else
-       {
-               command_print(cmd_ctx, "full branch address output disabled");
-       }
-       
-       /* only update ETM_CTRL register if tracemode changed */
-       if (arm7_9->etm_ctx->tracemode != tracemode)
-       {
-               reg_t *etm_ctrl_reg = &arm7_9->etm_ctx->reg_cache->reg_list[ETM_CTRL];
-               
-               etm_get_reg(etm_ctrl_reg);
-               
-               buf_set_u32(etm_ctrl_reg->value, 2, 2, tracemode & ETMV1_TRACE_MASK);
-               buf_set_u32(etm_ctrl_reg->value, 14, 2, (tracemode & ETMV1_CONTEXTID_MASK) >> 4);
-               buf_set_u32(etm_ctrl_reg->value, 12, 1, (tracemode & ETMV1_CYCLE_ACCURATE) >> 8);
-               buf_set_u32(etm_ctrl_reg->value, 8, 1, (tracemode & ETMV1_BRANCH_OUTPUT) >> 9);
-               etm_store_reg(etm_ctrl_reg);
-               
-               arm7_9->etm_ctx->tracemode = tracemode;
-               
-               /* invalidate old trace data */
-               arm7_9->etm_ctx->capture_status = TRACE_IDLE;
-               if (arm7_9->etm_ctx->trace_depth > 0)
-               {
-                       free(arm7_9->etm_ctx->trace_data);
-                       arm7_9->etm_ctx->trace_data = NULL;
-               }
-               arm7_9->etm_ctx->trace_depth = 0;
-       }
-       
-       return ERROR_OK;
-}
-
-int handle_etm_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target;
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       etm_portmode_t portmode = 0x0;
-       etm_context_t *etm_ctx = malloc(sizeof(etm_context_t));
-       int i;
-       
-       if (argc != 5)
-       {
-               ERROR("incomplete 'etm config <target> <port_width> <port_mode> <clocking> <capture_driver>' command");
-               exit(-1);
-       }
-       
-       target = get_target_by_num(strtoul(args[0], NULL, 0));
-       
-       if (!target)
-       {
-               ERROR("target number '%s' not defined", args[0]);
-               exit(-1);
-       }
-       
-       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-               return ERROR_OK;
-       }
-       
-       switch (strtoul(args[1], NULL, 0))
-       {
-               case 4:
-                       portmode |= ETM_PORT_4BIT;
-                       break;
-               case 8:
-                       portmode |= ETM_PORT_8BIT;
-                       break;
-               case 16:
-                       portmode |= ETM_PORT_16BIT;
-                       break;
-               default:
-                       command_print(cmd_ctx, "unsupported ETM port width '%s', must be 4, 8 or 16", args[1]);
-                       return ERROR_OK;
-       }
-       
-       if (strcmp("normal", args[2]) == 0)
-       {
-               portmode |= ETM_PORT_NORMAL;
-       }
-       else if (strcmp("multiplexed", args[2]) == 0)
-       {
-               portmode |= ETM_PORT_MUXED;
-       }
-       else if (strcmp("demultiplexed", args[2]) == 0)
-       {
-               portmode |= ETM_PORT_DEMUXED;
-       }
-       else
-       {
-               command_print(cmd_ctx, "unsupported ETM port mode '%s', must be 'normal', 'multiplexed' or 'demultiplexed'", args[2]);
-               return ERROR_OK;
-       }
-       
-       if (strcmp("half", args[3]) == 0)
-       {
-               portmode |= ETM_PORT_HALF_CLOCK;
-       }
-       else if (strcmp("full", args[3]) == 0)
-       {
-               portmode |= ETM_PORT_FULL_CLOCK;
-       }
-       else
-       {
-               command_print(cmd_ctx, "unsupported ETM port clocking '%s', must be 'full' or 'half'", args[3]);
-               return ERROR_OK;
-       }
-       
-       for (i=0; etm_capture_drivers[i]; i++)
-       {
-               if (strcmp(args[4], etm_capture_drivers[i]->name) == 0)
-               {
-                       if (etm_capture_drivers[i]->register_commands(cmd_ctx) != ERROR_OK)
-                       {
-                               free(etm_ctx);
-                               exit(-1);
-                       }
-               
-                       etm_ctx->capture_driver = etm_capture_drivers[i];
-
-                       break;
-               }
-       }
-       
-       if (!etm_capture_drivers[i])
-       {
-               /* no supported capture driver found, don't register an ETM */
-               free(etm_ctx);
-               ERROR("trace capture driver '%s' not found", args[4]);
-               return ERROR_OK;
-       }
-       
-       etm_ctx->target = target;
-       etm_ctx->trigger_percent = 50;
-       etm_ctx->trace_data = NULL;
-       etm_ctx->trace_depth = 0;
-       etm_ctx->portmode = portmode;
-       etm_ctx->tracemode = 0x0;
-       etm_ctx->core_state = ARMV4_5_STATE_ARM;
-       etm_ctx->image = NULL;
-       etm_ctx->pipe_index = 0;
-       etm_ctx->data_index = 0;
-       etm_ctx->current_pc = 0x0;
-       etm_ctx->pc_ok = 0;
-       etm_ctx->last_branch = 0x0;
-       etm_ctx->last_branch_reason = 0x0;
-       etm_ctx->last_ptr = 0x0;
-       etm_ctx->ptr_ok = 0x0;
-       etm_ctx->context_id = 0x0;
-       etm_ctx->last_instruction = 0;
-       
-       arm7_9->etm_ctx = etm_ctx;
-       
-       etm_register_user_commands(cmd_ctx);
-       
-       return ERROR_OK;
-}
-
-int handle_etm_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target;
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       reg_t *etm_config_reg;
-       reg_t *etm_sys_config_reg;
-       
-       int max_port_size;
-               
-       target = get_current_target(cmd_ctx);
-       
-       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-               return ERROR_OK;
-       }
-       
-       if (!arm7_9->etm_ctx)
-       {
-               command_print(cmd_ctx, "current target doesn't have an ETM configured");
-               return ERROR_OK;
-       }
-       
-       etm_config_reg = &arm7_9->etm_ctx->reg_cache->reg_list[ETM_CONFIG];
-       etm_sys_config_reg = &arm7_9->etm_ctx->reg_cache->reg_list[ETM_SYS_CONFIG];
-       
-       etm_get_reg(etm_config_reg);
-       command_print(cmd_ctx, "pairs of address comparators: %i", buf_get_u32(etm_config_reg->value, 0, 4));
-       command_print(cmd_ctx, "pairs of data comparators: %i", buf_get_u32(etm_config_reg->value, 4, 4));
-       command_print(cmd_ctx, "memory map decoders: %i", buf_get_u32(etm_config_reg->value, 8, 5));
-       command_print(cmd_ctx, "number of counters: %i", buf_get_u32(etm_config_reg->value, 13, 3));
-       command_print(cmd_ctx, "sequencer %spresent",
-                       (buf_get_u32(etm_config_reg->value, 16, 1) == 1) ? "" : "not ");
-       command_print(cmd_ctx, "number of ext. inputs: %i", buf_get_u32(etm_config_reg->value, 17, 3));
-       command_print(cmd_ctx, "number of ext. outputs: %i", buf_get_u32(etm_config_reg->value, 20, 3));
-       command_print(cmd_ctx, "FIFO full %spresent",
-                       (buf_get_u32(etm_config_reg->value, 23, 1) == 1) ? "" : "not ");
-       command_print(cmd_ctx, "protocol version: %i", buf_get_u32(etm_config_reg->value, 28, 3));
-       
-       etm_get_reg(etm_sys_config_reg);
-
-       switch (buf_get_u32(etm_sys_config_reg->value, 0, 3))
-       {
-               case 0:
-                       max_port_size = 4;
-                       break;
-               case 1:
-                       max_port_size = 8;
-                       break;
-               case 2:
-                       max_port_size = 16;
-                       break;
-       }
-       command_print(cmd_ctx, "max. port size: %i", max_port_size);
-       
-       command_print(cmd_ctx, "half-rate clocking %ssupported",
-                       (buf_get_u32(etm_sys_config_reg->value, 3, 1) == 1) ? "" : "not ");
-       command_print(cmd_ctx, "full-rate clocking %ssupported",
-                       (buf_get_u32(etm_sys_config_reg->value, 4, 1) == 1) ? "" : "not ");
-       command_print(cmd_ctx, "normal trace format %ssupported",
-                       (buf_get_u32(etm_sys_config_reg->value, 5, 1) == 1) ? "" : "not ");
-       command_print(cmd_ctx, "multiplex trace format %ssupported",
-                       (buf_get_u32(etm_sys_config_reg->value, 6, 1) == 1) ? "" : "not ");
-       command_print(cmd_ctx, "demultiplex trace format %ssupported",
-                       (buf_get_u32(etm_sys_config_reg->value, 7, 1) == 1) ? "" : "not ");
-       command_print(cmd_ctx, "FIFO full %ssupported",
-                       (buf_get_u32(etm_sys_config_reg->value, 8, 1) == 1) ? "" : "not ");
-       
-       return ERROR_OK;
-}
-
-int handle_etm_status_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target;
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       trace_status_t trace_status;
-       
-       target = get_current_target(cmd_ctx);
-       
-       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-               return ERROR_OK;
-       }
-       
-       if (!arm7_9->etm_ctx)
-       {
-               command_print(cmd_ctx, "current target doesn't have an ETM configured");
-               return ERROR_OK;
-       }
-       
-       trace_status = arm7_9->etm_ctx->capture_driver->status(arm7_9->etm_ctx);
-       
-       if (trace_status == TRACE_IDLE)
-       {
-               command_print(cmd_ctx, "tracing is idle");
-       }
-       else
-       {
-               static char *completed = " completed";
-               static char *running = " is running";
-               static char *overflowed = ", trace overflowed";
-               static char *triggered = ", trace triggered";
-               
-               command_print(cmd_ctx, "trace collection%s%s%s", 
-                       (trace_status & TRACE_RUNNING) ? running : completed,
-                       (trace_status & TRACE_OVERFLOWED) ? overflowed : "",
-                       (trace_status & TRACE_TRIGGERED) ? triggered : "");
-               
-               if (arm7_9->etm_ctx->trace_depth > 0)
-               {
-                       command_print(cmd_ctx, "%i frames of trace data read", arm7_9->etm_ctx->trace_depth);
-               }
-       }
-       
-       return ERROR_OK;
-}
-
-int handle_etm_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target;
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       etm_context_t *etm_ctx;
-
-       if (argc < 1)
-       {
-               command_print(cmd_ctx, "usage: etm image <file> [base address] [type]");
-               return ERROR_OK;
-       }
-       
-       target = get_current_target(cmd_ctx);
-       
-       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-               return ERROR_OK;
-       }
-       
-       if (!(etm_ctx = arm7_9->etm_ctx))
-       {
-               command_print(cmd_ctx, "current target doesn't have an ETM configured");
-               return ERROR_OK;
-       }
-       
-       if (etm_ctx->image)
-       {
-               image_close(etm_ctx->image);
-               free(etm_ctx->image);
-               command_print(cmd_ctx, "previously loaded image found and closed");
-       }
-       
-       etm_ctx->image = malloc(sizeof(image_t));
-       etm_ctx->image->base_address_set = 0;
-       etm_ctx->image->start_address_set = 0;
-       
-       /* a base address isn't always necessary, default to 0x0 (i.e. don't relocate) */
-       if (argc >= 2)
-       {
-               etm_ctx->image->base_address_set = 1;
-               etm_ctx->image->base_address = strtoul(args[1], NULL, 0);
-       }
-       else
-       {
-               etm_ctx->image->base_address_set = 0;
-       }
-               
-       if (image_open(etm_ctx->image, args[0], (argc >= 3) ? args[2] : NULL) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "image opening error: %s", etm_ctx->image->error_str);
-               free(etm_ctx->image);
-               etm_ctx->image = NULL;
-               return ERROR_OK;
-       }
-       
-       return ERROR_OK;
-}
-
-int handle_etm_dump_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       fileio_t file;
-       target_t *target;
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       etm_context_t *etm_ctx;
-       int i;
-       
-       if (argc != 1)
-       {
-               command_print(cmd_ctx, "usage: etm dump <file>");
-               return ERROR_OK;
-       }
-       
-       target = get_current_target(cmd_ctx);
-       
-       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-               return ERROR_OK;
-       }
-       
-       if (!(etm_ctx = arm7_9->etm_ctx))
-       {
-               command_print(cmd_ctx, "current target doesn't have an ETM configured");
-               return ERROR_OK;
-       }
-       
-       if (etm_ctx->capture_driver->status == TRACE_IDLE)
-       {
-               command_print(cmd_ctx, "trace capture wasn't enabled, no trace data captured");
-               return ERROR_OK;
-       }
-
-       if (etm_ctx->capture_driver->status(etm_ctx) & TRACE_RUNNING)
-       {
-               /* TODO: if on-the-fly capture is to be supported, this needs to be changed */
-               command_print(cmd_ctx, "trace capture not completed");
-               return ERROR_OK;
-       }
-       
-       /* read the trace data if it wasn't read already */
-       if (etm_ctx->trace_depth == 0)
-               etm_ctx->capture_driver->read_trace(etm_ctx);
-       
-       if (fileio_open(&file, args[0], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "file open error: %s", file.error_str);
-               return ERROR_OK;
-       }
-       
-       fileio_write_u32(&file, etm_ctx->capture_status);
-       fileio_write_u32(&file, etm_ctx->portmode);
-       fileio_write_u32(&file, etm_ctx->tracemode);
-       fileio_write_u32(&file, etm_ctx->trace_depth);
-       
-       for (i = 0; i < etm_ctx->trace_depth; i++)
-       {
-               fileio_write_u32(&file, etm_ctx->trace_data[i].pipestat);
-               fileio_write_u32(&file, etm_ctx->trace_data[i].packet);
-               fileio_write_u32(&file, etm_ctx->trace_data[i].flags);
-       }
-       
-       fileio_close(&file);
-       
-       return ERROR_OK;        
-}
-
-int handle_etm_load_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       fileio_t file;
-       target_t *target;
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       etm_context_t *etm_ctx;
-       int i;
-       
-       if (argc != 1)
-       {
-               command_print(cmd_ctx, "usage: etm load <file>");
-               return ERROR_OK;
-       }
-       
-       target = get_current_target(cmd_ctx);
-       
-       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-               return ERROR_OK;
-       }
-       
-       if (!(etm_ctx = arm7_9->etm_ctx))
-       {
-               command_print(cmd_ctx, "current target doesn't have an ETM configured");
-               return ERROR_OK;
-       }
-       
-       if (etm_ctx->capture_driver->status(etm_ctx) & TRACE_RUNNING)
-       {
-               command_print(cmd_ctx, "trace capture running, stop first");
-               return ERROR_OK;
-       }
-       
-       if (fileio_open(&file, args[0], FILEIO_READ, FILEIO_BINARY) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "file open error: %s", file.error_str);
-               return ERROR_OK;
-       }
-       
-       if (file.size % 4)
-       {
-               command_print(cmd_ctx, "size isn't a multiple of 4, no valid trace data");
-               return ERROR_OK;
-       }
-       
-       if (etm_ctx->trace_depth > 0)
-       {
-               free(etm_ctx->trace_data);
-       }
-       
-       fileio_read_u32(&file, &etm_ctx->capture_status);
-       fileio_read_u32(&file, &etm_ctx->portmode);
-       fileio_read_u32(&file, &etm_ctx->tracemode);
-       fileio_read_u32(&file, &etm_ctx->trace_depth);
-       
-       etm_ctx->trace_data = malloc(sizeof(etmv1_trace_data_t) * etm_ctx->trace_depth);
-       
-       for (i = 0; i < etm_ctx->trace_depth; i++)
-       {
-               u32 pipestat, packet, flags;
-               fileio_read_u32(&file, &pipestat);
-               fileio_read_u32(&file, &packet);
-               fileio_read_u32(&file, &flags);
-               etm_ctx->trace_data[i].pipestat = pipestat & 0xff;
-               etm_ctx->trace_data[i].packet = packet & 0xffff;
-               etm_ctx->trace_data[i].flags = flags;
-       }
-       
-       fileio_close(&file);
-       
-       return ERROR_OK;        
-}
-
-int handle_etm_trigger_percent_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target;
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       etm_context_t *etm_ctx;
-       
-       target = get_current_target(cmd_ctx);
-       
-       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-               return ERROR_OK;
-       }
-       
-       if (!(etm_ctx = arm7_9->etm_ctx))
-       {
-               command_print(cmd_ctx, "current target doesn't have an ETM configured");
-               return ERROR_OK;
-       }
-       
-       if (argc > 0)
-       {
-               u32 new_value = strtoul(args[0], NULL, 0);
-               
-               if ((new_value < 2) || (new_value > 100))
-               {
-                       command_print(cmd_ctx, "valid settings are 2% to 100%");
-               }
-               else
-               {
-                       etm_ctx->trigger_percent = new_value;
-               }
-       }
-       
-       command_print(cmd_ctx, "%i percent of the tracebuffer reserved for after the trigger", etm_ctx->trigger_percent);
-
-       return ERROR_OK;
-}
-
-int handle_etm_start_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target;
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       etm_context_t *etm_ctx;
-       reg_t *etm_ctrl_reg;
-
-       target = get_current_target(cmd_ctx);
-       
-       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-               return ERROR_OK;
-       }
-       
-       if (!(etm_ctx = arm7_9->etm_ctx))
-       {
-               command_print(cmd_ctx, "current target doesn't have an ETM configured");
-               return ERROR_OK;
-       }
-       
-       /* invalidate old tracing data */
-       arm7_9->etm_ctx->capture_status = TRACE_IDLE;
-       if (arm7_9->etm_ctx->trace_depth > 0)
-       {
-               free(arm7_9->etm_ctx->trace_data);
-               arm7_9->etm_ctx->trace_data = NULL;
-       }
-       arm7_9->etm_ctx->trace_depth = 0;
-               
-       etm_ctrl_reg = &arm7_9->etm_ctx->reg_cache->reg_list[ETM_CTRL];
-       etm_get_reg(etm_ctrl_reg);
-               
-       /* Clear programming bit (10), set port selection bit (11) */
-       buf_set_u32(etm_ctrl_reg->value, 10, 2, 0x2);
-
-       etm_store_reg(etm_ctrl_reg);
-       jtag_execute_queue();
-
-       etm_ctx->capture_driver->start_capture(etm_ctx);
-
-       return ERROR_OK;
-}
-
-int handle_etm_stop_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target;
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       etm_context_t *etm_ctx;
-       reg_t *etm_ctrl_reg;
-
-       target = get_current_target(cmd_ctx);
-       
-       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-               return ERROR_OK;
-       }
-       
-       if (!(etm_ctx = arm7_9->etm_ctx))
-       {
-               command_print(cmd_ctx, "current target doesn't have an ETM configured");
-               return ERROR_OK;
-       }
-       
-       etm_ctrl_reg = &arm7_9->etm_ctx->reg_cache->reg_list[ETM_CTRL];
-       etm_get_reg(etm_ctrl_reg);
-               
-       /* Set programming bit (10), clear port selection bit (11) */
-       buf_set_u32(etm_ctrl_reg->value, 10, 2, 0x1);
-
-       etm_store_reg(etm_ctrl_reg);    
-       jtag_execute_queue();
-       
-       etm_ctx->capture_driver->stop_capture(etm_ctx);
-       
-       return ERROR_OK;
-}
-
-int handle_etm_analyze_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target;
-       armv4_5_common_t *armv4_5;
-       arm7_9_common_t *arm7_9;
-       etm_context_t *etm_ctx;
-       int retval;
-
-       target = get_current_target(cmd_ctx);
-       
-       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");
-               return ERROR_OK;
-       }
-       
-       if (!(etm_ctx = arm7_9->etm_ctx))
-       {
-               command_print(cmd_ctx, "current target doesn't have an ETM configured");
-               return ERROR_OK;
-       }
-       
-       if ((retval = etmv1_analyze_trace(etm_ctx, cmd_ctx)) != ERROR_OK)
-       {
-               switch(retval)
-               {
-                       case ERROR_ETM_ANALYSIS_FAILED:
-                               command_print(cmd_ctx, "further analysis failed (corrupted trace data or just end of data");
-                               break;
-                       case ERROR_TRACE_INSTRUCTION_UNAVAILABLE:
-                               command_print(cmd_ctx, "no instruction for current address available, analysis aborted");
-                               break;
-                       case ERROR_TRACE_IMAGE_UNAVAILABLE:
-                               command_print(cmd_ctx, "no image available for trace analysis");
-                               break;
-                       default:
-                               command_print(cmd_ctx, "unknown error: %i", retval);
-               }
-       }
-       
-       return ERROR_OK;
-}
-
-int etm_register_commands(struct command_context_s *cmd_ctx)
-{
-       etm_cmd = register_command(cmd_ctx, NULL, "etm", NULL, COMMAND_ANY, "Embedded Trace Macrocell");
-
-       register_command(cmd_ctx, etm_cmd, "config", handle_etm_config_command, COMMAND_CONFIG, NULL);
-
-       return ERROR_OK;
-}
-
-int etm_register_user_commands(struct command_context_s *cmd_ctx)
-{
-       register_command(cmd_ctx, etm_cmd, "tracemode", handle_etm_tracemode_command,
-               COMMAND_EXEC, "configure trace mode <none|data|address|all> <context id bits> <cycle accurate> <branch output");
-
-       register_command(cmd_ctx, etm_cmd, "info", handle_etm_info_command,
-               COMMAND_EXEC, "display info about the current target's ETM");
-
-       register_command(cmd_ctx, etm_cmd, "trigger_percent <percent>", handle_etm_trigger_percent_command,
-               COMMAND_EXEC, "amount (<percent>) of trace buffer to be filled after the trigger occured");
-       register_command(cmd_ctx, etm_cmd, "status", handle_etm_status_command,
-               COMMAND_EXEC, "display current target's ETM status");
-       register_command(cmd_ctx, etm_cmd, "start", handle_etm_start_command,
-               COMMAND_EXEC, "start ETM trace collection");
-       register_command(cmd_ctx, etm_cmd, "stop", handle_etm_stop_command,
-               COMMAND_EXEC, "stop ETM trace collection");
-
-       register_command(cmd_ctx, etm_cmd, "analyze", handle_etm_analyze_command,
-               COMMAND_EXEC, "anaylze collected ETM trace");
-
-       register_command(cmd_ctx, etm_cmd, "image", handle_etm_image_command,
-               COMMAND_EXEC, "load image from <file> [base address]");
-
-       register_command(cmd_ctx, etm_cmd, "dump", handle_etm_dump_command,
-               COMMAND_EXEC, "dump captured trace data <file>");
-       register_command(cmd_ctx, etm_cmd, "load", handle_etm_load_command,
-               COMMAND_EXEC, "load trace data for analysis <file>");
-
-       return ERROR_OK;
-}
+/***************************************************************************\r
+ *   Copyright (C) 2005 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include <string.h>\r
+\r
+#include "etm.h"\r
+#include "etb.h"\r
+\r
+#include "armv4_5.h"\r
+#include "arm7_9_common.h"\r
+#include "arm_disassembler.h"\r
+#include "arm_simulator.h"\r
+\r
+#include "log.h"\r
+#include "arm_jtag.h"\r
+#include "types.h"\r
+#include "binarybuffer.h"\r
+#include "target.h"\r
+#include "register.h"\r
+#include "jtag.h"\r
+#include "fileio.h"\r
+\r
+#include <stdlib.h>\r
+\r
+/* ETM register access functionality \r
+ * \r
+ */\r
+\r
+bitfield_desc_t etm_comms_ctrl_bitfield_desc[] = \r
+{\r
+       {"R", 1},\r
+       {"W", 1},\r
+       {"reserved", 26},\r
+       {"version", 4}\r
+};\r
+\r
+int etm_reg_arch_info[] =\r
+{\r
+       0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,\r
+       0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,\r
+       0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, \r
+       0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f, \r
+       0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27,\r
+       0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, \r
+       0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37,\r
+       0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f,\r
+       0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, \r
+       0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f, \r
+       0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57,\r
+       0x58, 0x59, 0x5a, 0x5b, 0x5c, 0x5d, 0x5e, 0x5f,\r
+       0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 0x67, \r
+       0x68, 0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f, \r
+};\r
+\r
+int etm_reg_arch_size_info[] =\r
+{\r
+       32, 32, 17, 8, 3, 9, 32, 16,\r
+       17, 26, 25, 8, 17, 32, 32, 17,\r
+       32, 32, 32, 32, 32, 32, 32, 32, \r
+       32, 32, 32, 32, 32, 32, 32, 32, \r
+       7, 7, 7, 7, 7, 7, 7, 7, \r
+       7, 7, 7, 7, 7, 7, 7, 7, \r
+       32, 32, 32, 32, 32, 32, 32, 32, \r
+       32, 32, 32, 32, 32, 32, 32, 32, \r
+       32, 32, 32, 32, 32, 32, 32, 32, \r
+       32, 32, 32, 32, 32, 32, 32, 32, \r
+       16, 16, 16, 16, 18, 18, 18, 18,\r
+       17, 17, 17, 17, 16, 16, 16, 16,\r
+       17, 17, 17, 17, 17, 17, 2, \r
+       17, 17, 17, 17, 32, 32, 32, 32 \r
+};\r
+\r
+char* etm_reg_list[] =\r
+{\r
+       "ETM_CTRL",\r
+       "ETM_CONFIG",\r
+       "ETM_TRIG_EVENT",\r
+       "ETM_MMD_CTRL",\r
+       "ETM_STATUS",\r
+       "ETM_SYS_CONFIG",\r
+       "ETM_TRACE_RESOURCE_CTRL",\r
+       "ETM_TRACE_EN_CTRL2",\r
+       "ETM_TRACE_EN_EVENT",\r
+       "ETM_TRACE_EN_CTRL1",\r
+       "ETM_FIFOFULL_REGION",\r
+       "ETM_FIFOFULL_LEVEL",\r
+       "ETM_VIEWDATA_EVENT",\r
+       "ETM_VIEWDATA_CTRL1",\r
+       "ETM_VIEWDATA_CTRL2",\r
+       "ETM_VIEWDATA_CTRL3",\r
+       "ETM_ADDR_COMPARATOR_VALUE1",\r
+       "ETM_ADDR_COMPARATOR_VALUE2",\r
+       "ETM_ADDR_COMPARATOR_VALUE3",\r
+       "ETM_ADDR_COMPARATOR_VALUE4",\r
+       "ETM_ADDR_COMPARATOR_VALUE5",\r
+       "ETM_ADDR_COMPARATOR_VALUE6",\r
+       "ETM_ADDR_COMPARATOR_VALUE7",\r
+       "ETM_ADDR_COMPARATOR_VALUE8",\r
+       "ETM_ADDR_COMPARATOR_VALUE9",\r
+       "ETM_ADDR_COMPARATOR_VALUE10",\r
+       "ETM_ADDR_COMPARATOR_VALUE11",\r
+       "ETM_ADDR_COMPARATOR_VALUE12",\r
+       "ETM_ADDR_COMPARATOR_VALUE13",\r
+       "ETM_ADDR_COMPARATOR_VALUE14",\r
+       "ETM_ADDR_COMPARATOR_VALUE15",\r
+       "ETM_ADDR_COMPARATOR_VALUE16",\r
+       "ETM_ADDR_ACCESS_TYPE1",\r
+       "ETM_ADDR_ACCESS_TYPE2",\r
+       "ETM_ADDR_ACCESS_TYPE3",\r
+       "ETM_ADDR_ACCESS_TYPE4",\r
+       "ETM_ADDR_ACCESS_TYPE5",\r
+       "ETM_ADDR_ACCESS_TYPE6",\r
+       "ETM_ADDR_ACCESS_TYPE7",\r
+       "ETM_ADDR_ACCESS_TYPE8",\r
+       "ETM_ADDR_ACCESS_TYPE9",\r
+       "ETM_ADDR_ACCESS_TYPE10",\r
+       "ETM_ADDR_ACCESS_TYPE11",\r
+       "ETM_ADDR_ACCESS_TYPE12",\r
+       "ETM_ADDR_ACCESS_TYPE13",\r
+       "ETM_ADDR_ACCESS_TYPE14",\r
+       "ETM_ADDR_ACCESS_TYPE15",\r
+       "ETM_ADDR_ACCESS_TYPE16",\r
+       "ETM_DATA_COMPARATOR_VALUE1",\r
+       "ETM_DATA_COMPARATOR_VALUE2",\r
+       "ETM_DATA_COMPARATOR_VALUE3",\r
+       "ETM_DATA_COMPARATOR_VALUE4",\r
+       "ETM_DATA_COMPARATOR_VALUE5",\r
+       "ETM_DATA_COMPARATOR_VALUE6",\r
+       "ETM_DATA_COMPARATOR_VALUE7",\r
+       "ETM_DATA_COMPARATOR_VALUE8",\r
+       "ETM_DATA_COMPARATOR_VALUE9",\r
+       "ETM_DATA_COMPARATOR_VALUE10",\r
+       "ETM_DATA_COMPARATOR_VALUE11",\r
+       "ETM_DATA_COMPARATOR_VALUE12",\r
+       "ETM_DATA_COMPARATOR_VALUE13",\r
+       "ETM_DATA_COMPARATOR_VALUE14",\r
+       "ETM_DATA_COMPARATOR_VALUE15",\r
+       "ETM_DATA_COMPARATOR_VALUE16",\r
+       "ETM_DATA_COMPARATOR_MASK1",\r
+       "ETM_DATA_COMPARATOR_MASK2",\r
+       "ETM_DATA_COMPARATOR_MASK3",\r
+       "ETM_DATA_COMPARATOR_MASK4",\r
+       "ETM_DATA_COMPARATOR_MASK5",\r
+       "ETM_DATA_COMPARATOR_MASK6",\r
+       "ETM_DATA_COMPARATOR_MASK7",\r
+       "ETM_DATA_COMPARATOR_MASK8",\r
+       "ETM_DATA_COMPARATOR_MASK9",\r
+       "ETM_DATA_COMPARATOR_MASK10",\r
+       "ETM_DATA_COMPARATOR_MASK11",\r
+       "ETM_DATA_COMPARATOR_MASK12",\r
+       "ETM_DATA_COMPARATOR_MASK13",\r
+       "ETM_DATA_COMPARATOR_MASK14",\r
+       "ETM_DATA_COMPARATOR_MASK15",\r
+       "ETM_DATA_COMPARATOR_MASK16",\r
+       "ETM_COUNTER_INITAL_VALUE1",\r
+       "ETM_COUNTER_INITAL_VALUE2",\r
+       "ETM_COUNTER_INITAL_VALUE3",\r
+       "ETM_COUNTER_INITAL_VALUE4",\r
+       "ETM_COUNTER_ENABLE1",\r
+       "ETM_COUNTER_ENABLE2",\r
+       "ETM_COUNTER_ENABLE3",\r
+       "ETM_COUNTER_ENABLE4",\r
+       "ETM_COUNTER_RELOAD_VALUE1",\r
+       "ETM_COUNTER_RELOAD_VALUE2",\r
+       "ETM_COUNTER_RELOAD_VALUE3",\r
+       "ETM_COUNTER_RELOAD_VALUE4",\r
+       "ETM_COUNTER_VALUE1",\r
+       "ETM_COUNTER_VALUE2",\r
+       "ETM_COUNTER_VALUE3",\r
+       "ETM_COUNTER_VALUE4",\r
+       "ETM_SEQUENCER_CTRL1",\r
+       "ETM_SEQUENCER_CTRL2",\r
+       "ETM_SEQUENCER_CTRL3",\r
+       "ETM_SEQUENCER_CTRL4",\r
+       "ETM_SEQUENCER_CTRL5",\r
+       "ETM_SEQUENCER_CTRL6",\r
+       "ETM_SEQUENCER_STATE",\r
+       "ETM_EXTERNAL_OUTPUT1",\r
+       "ETM_EXTERNAL_OUTPUT2",\r
+       "ETM_EXTERNAL_OUTPUT3",\r
+       "ETM_EXTERNAL_OUTPUT4",\r
+       "ETM_CONTEXTID_COMPARATOR_VALUE1",\r
+       "ETM_CONTEXTID_COMPARATOR_VALUE2",\r
+       "ETM_CONTEXTID_COMPARATOR_VALUE3",\r
+       "ETM_CONTEXTID_COMPARATOR_MASK"\r
+};  \r
+\r
+int etm_reg_arch_type = -1;\r
+\r
+int etm_get_reg(reg_t *reg);\r
+int etm_set_reg(reg_t *reg, u32 value);\r
+int etm_set_reg_w_exec(reg_t *reg, u8 *buf);\r
+\r
+int etm_write_reg(reg_t *reg, u32 value);\r
+int etm_read_reg(reg_t *reg);\r
+\r
+command_t *etm_cmd = NULL;\r
+\r
+reg_cache_t* etm_build_reg_cache(target_t *target, arm_jtag_t *jtag_info, etm_context_t *etm_ctx)\r
+{\r
+       reg_cache_t *reg_cache = malloc(sizeof(reg_cache_t));\r
+       reg_t *reg_list = NULL;\r
+       etm_reg_t *arch_info = NULL;\r
+       int num_regs = sizeof(etm_reg_arch_info)/sizeof(int);\r
+       int i;\r
+       u32 etm_ctrl_value;\r
+       \r
+       /* register a register arch-type for etm registers only once */\r
+       if (etm_reg_arch_type == -1)\r
+               etm_reg_arch_type = register_reg_arch_type(etm_get_reg, etm_set_reg_w_exec);\r
+       \r
+       /* the actual registers are kept in two arrays */\r
+       reg_list = calloc(num_regs, sizeof(reg_t));\r
+       arch_info = calloc(num_regs, sizeof(etm_reg_t));\r
+       \r
+       /* fill in values for the reg cache */\r
+       reg_cache->name = "etm registers";\r
+       reg_cache->next = NULL;\r
+       reg_cache->reg_list = reg_list;\r
+       reg_cache->num_regs = num_regs;\r
+       \r
+       /* set up registers */\r
+       for (i = 0; i < num_regs; i++)\r
+       {\r
+               reg_list[i].name = etm_reg_list[i];\r
+               reg_list[i].size = 32;\r
+               reg_list[i].dirty = 0;\r
+               reg_list[i].valid = 0;\r
+               reg_list[i].bitfield_desc = NULL;\r
+               reg_list[i].num_bitfields = 0;\r
+               reg_list[i].value = calloc(1, 4);\r
+               reg_list[i].arch_info = &arch_info[i];\r
+               reg_list[i].arch_type = etm_reg_arch_type;\r
+               reg_list[i].size = etm_reg_arch_size_info[i];\r
+               arch_info[i].addr = etm_reg_arch_info[i];\r
+               arch_info[i].jtag_info = jtag_info;\r
+       }\r
+\r
+       /* initialize some ETM control register settings */     \r
+       etm_get_reg(&reg_list[ETM_CTRL]);\r
+       etm_ctrl_value = buf_get_u32(reg_list[ETM_CTRL].value, 0, reg_list[ETM_CTRL].size);\r
+       \r
+       /* clear the ETM powerdown bit (0) */\r
+       etm_ctrl_value &= ~0x1;\r
+               \r
+       /* configure port width (6:4), mode (17:16) and clocking (13) */\r
+       etm_ctrl_value = (etm_ctrl_value & \r
+               ~ETM_PORT_WIDTH_MASK & ~ETM_PORT_MODE_MASK & ~ETM_PORT_CLOCK_MASK)\r
+               | etm_ctx->portmode;\r
+       \r
+       buf_set_u32(reg_list[ETM_CTRL].value, 0, reg_list[ETM_CTRL].size, etm_ctrl_value);\r
+       etm_store_reg(&reg_list[ETM_CTRL]);\r
+       \r
+       /* the ETM might have an ETB connected */\r
+       if (strcmp(etm_ctx->capture_driver->name, "etb") == 0)\r
+       {\r
+               etb_t *etb = etm_ctx->capture_driver_priv;\r
+               \r
+               if (!etb)\r
+               {\r
+                       ERROR("etb selected as etm capture driver, but no ETB configured");\r
+                       return ERROR_OK;\r
+               }\r
+               \r
+               reg_cache->next = etb_build_reg_cache(etb);\r
+               \r
+               etb->reg_cache = reg_cache->next;\r
+       }\r
+       \r
+       if (etm_ctx->capture_driver->init(etm_ctx) != ERROR_OK)\r
+       {\r
+               ERROR("ETM capture driver initialization failed");\r
+               exit(-1);\r
+       }\r
+       \r
+       return reg_cache;\r
+}\r
+\r
+int etm_get_reg(reg_t *reg)\r
+{\r
+       if (etm_read_reg(reg) != ERROR_OK)\r
+       {\r
+               ERROR("BUG: error scheduling etm register read");\r
+               exit(-1);\r
+       }\r
+       \r
+       if (jtag_execute_queue() != ERROR_OK)\r
+       {\r
+               ERROR("register read failed");\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int etm_read_reg_w_check(reg_t *reg, u8* check_value, u8* check_mask)\r
+{\r
+       etm_reg_t *etm_reg = reg->arch_info;\r
+       u8 reg_addr = etm_reg->addr & 0x7f;\r
+       scan_field_t fields[3];\r
+       \r
+       DEBUG("%i", etm_reg->addr);\r
+\r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_scann(etm_reg->jtag_info, 0x6);\r
+       arm_jtag_set_instr(etm_reg->jtag_info, etm_reg->jtag_info->intest_instr, NULL);\r
+       \r
+       fields[0].device = etm_reg->jtag_info->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = reg->value;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+       \r
+       fields[1].device = etm_reg->jtag_info->chain_pos;\r
+       fields[1].num_bits = 7;\r
+       fields[1].out_value = malloc(1);\r
+       buf_set_u32(fields[1].out_value, 0, 7, reg_addr);\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = etm_reg->jtag_info->chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = malloc(1);\r
+       buf_set_u32(fields[2].out_value, 0, 1, 0);\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(3, fields, -1);\r
+       \r
+       fields[0].in_value = reg->value;\r
+       jtag_set_check_value(fields+0, check_value, check_mask, NULL);  \r
+               \r
+       jtag_add_dr_scan(3, fields, -1);\r
+\r
+       free(fields[1].out_value);\r
+       free(fields[2].out_value);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int etm_read_reg(reg_t *reg)\r
+{\r
+       return etm_read_reg_w_check(reg, NULL, NULL);   \r
+}\r
+\r
+int etm_set_reg(reg_t *reg, u32 value)\r
+{\r
+       if (etm_write_reg(reg, value) != ERROR_OK)\r
+       {\r
+               ERROR("BUG: error scheduling etm register write");\r
+               exit(-1);\r
+       }\r
+       \r
+       buf_set_u32(reg->value, 0, reg->size, value);\r
+       reg->valid = 1;\r
+       reg->dirty = 0;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int etm_set_reg_w_exec(reg_t *reg, u8 *buf)\r
+{\r
+       etm_set_reg(reg, buf_get_u32(buf, 0, reg->size));\r
+       \r
+       if (jtag_execute_queue() != ERROR_OK)\r
+       {\r
+               ERROR("register write failed");\r
+               exit(-1);\r
+       }\r
+       return ERROR_OK;\r
+}\r
+\r
+int etm_write_reg(reg_t *reg, u32 value)\r
+{\r
+       etm_reg_t *etm_reg = reg->arch_info;\r
+       u8 reg_addr = etm_reg->addr & 0x7f;\r
+       scan_field_t fields[3];\r
+       \r
+       DEBUG("%i: 0x%8.8x", etm_reg->addr, value);\r
+       \r
+       jtag_add_end_state(TAP_RTI);\r
+       arm_jtag_scann(etm_reg->jtag_info, 0x6);\r
+       arm_jtag_set_instr(etm_reg->jtag_info, etm_reg->jtag_info->intest_instr, NULL);\r
+       \r
+       fields[0].device = etm_reg->jtag_info->chain_pos;\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = malloc(4);\r
+       buf_set_u32(fields[0].out_value, 0, 32, value);\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+       \r
+       fields[1].device = etm_reg->jtag_info->chain_pos;\r
+       fields[1].num_bits = 7;\r
+       fields[1].out_value = malloc(1);\r
+       buf_set_u32(fields[1].out_value, 0, 7, reg_addr);\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       fields[2].device = etm_reg->jtag_info->chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = malloc(1);\r
+       buf_set_u32(fields[2].out_value, 0, 1, 1);\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_check_value = NULL;\r
+       fields[2].in_check_mask = NULL;\r
+       fields[2].in_handler = NULL;\r
+       fields[2].in_handler_priv = NULL;\r
+       \r
+       jtag_add_dr_scan(3, fields, -1);\r
+       \r
+       free(fields[0].out_value);\r
+       free(fields[1].out_value);\r
+       free(fields[2].out_value);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int etm_store_reg(reg_t *reg)\r
+{\r
+       return etm_write_reg(reg, buf_get_u32(reg->value, 0, reg->size));\r
+}\r
+\r
+/* ETM trace analysis functionality\r
+ * \r
+ */\r
+extern etm_capture_driver_t etb_capture_driver;\r
+extern etm_capture_driver_t etm_dummy_capture_driver;\r
+#if BUILD_OOCD_TRACE == 1\r
+extern etm_capture_driver_t oocd_trace_capture_driver;\r
+#endif\r
+\r
+etm_capture_driver_t *etm_capture_drivers[] = \r
+{\r
+       &etb_capture_driver,\r
+       &etm_dummy_capture_driver,\r
+#if BUILD_OOCD_TRACE == 1\r
+       &oocd_trace_capture_driver,\r
+#endif\r
+       NULL\r
+};\r
+\r
+char *etmv1v1_branch_reason_strings[] =\r
+{\r
+       "normal PC change",\r
+       "tracing enabled",\r
+       "trace restarted after overflow",\r
+       "exit from debug",\r
+       "periodic synchronization",\r
+       "reserved",\r
+       "reserved",\r
+       "reserved",\r
+};\r
+\r
+int etm_read_instruction(etm_context_t *ctx, arm_instruction_t *instruction)\r
+{\r
+       int i;\r
+       int section = -1;\r
+       u32 size_read;\r
+       u32 opcode;\r
+       int retval;\r
+       \r
+       if (!ctx->image)\r
+               return ERROR_TRACE_IMAGE_UNAVAILABLE;\r
+       \r
+       /* search for the section the current instruction belongs to */ \r
+       for (i = 0; i < ctx->image->num_sections; i++)\r
+       {\r
+               if ((ctx->image->sections[i].base_address <= ctx->current_pc) &&\r
+                       (ctx->image->sections[i].base_address + ctx->image->sections[i].size > ctx->current_pc))\r
+               {\r
+                       section = i;\r
+                       break;\r
+               }\r
+       }\r
+       \r
+       if (section == -1)\r
+       {\r
+               /* current instruction couldn't be found in the image */\r
+               return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;\r
+       }\r
+       \r
+       if (ctx->core_state == ARMV4_5_STATE_ARM)\r
+       {\r
+               u8 buf[4];\r
+               if ((retval = image_read_section(ctx->image, section, \r
+                       ctx->current_pc - ctx->image->sections[section].base_address,\r
+                       4, buf, &size_read)) != ERROR_OK)\r
+               {\r
+                       ERROR("error while reading instruction: %i", retval);\r
+                       return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;\r
+               }\r
+               opcode = target_buffer_get_u32(ctx->target, buf);\r
+               arm_evaluate_opcode(opcode, ctx->current_pc, instruction);\r
+       }\r
+       else if (ctx->core_state == ARMV4_5_STATE_THUMB)\r
+       {\r
+               u8 buf[2];\r
+               if ((retval = image_read_section(ctx->image, section, \r
+                       ctx->current_pc - ctx->image->sections[section].base_address,\r
+                       2, buf, &size_read)) != ERROR_OK)\r
+               {\r
+                       ERROR("error while reading instruction: %i", retval);\r
+                       return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;\r
+               }\r
+               opcode = target_buffer_get_u16(ctx->target, buf);\r
+               thumb_evaluate_opcode(opcode, ctx->current_pc, instruction);\r
+       }\r
+       else if (ctx->core_state == ARMV4_5_STATE_JAZELLE)\r
+       {\r
+               ERROR("BUG: tracing of jazelle code not supported");\r
+               exit(-1);\r
+       }\r
+       else\r
+       {\r
+               ERROR("BUG: unknown core state encountered");\r
+               exit(-1);\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int etmv1_next_packet(etm_context_t *ctx, u8 *packet, int apo)\r
+{\r
+       while (ctx->data_index < ctx->trace_depth)\r
+       {\r
+               /* if the caller specified an address packet offset, skip until the\r
+                * we reach the n-th cycle marked with tracesync */\r
+               if (apo > 0)\r
+               {\r
+                       if (ctx->trace_data[ctx->data_index].flags & ETMV1_TRACESYNC_CYCLE)\r
+                               apo--;\r
+                       \r
+                       if (apo > 0)\r
+                       {\r
+                               ctx->data_index++;\r
+                               ctx->data_half = 0;\r
+                       }\r
+                       continue;\r
+               }\r
+               \r
+               /* no tracedata output during a TD cycle\r
+                * or in a trigger cycle */\r
+               if ((ctx->trace_data[ctx->data_index].pipestat == STAT_TD)\r
+                       || (ctx->trace_data[ctx->data_index].flags & ETMV1_TRIGGER_CYCLE))\r
+               {\r
+                       ctx->data_index++;\r
+                       ctx->data_half = 0;\r
+                       continue;\r
+               }\r
+               \r
+               if ((ctx->portmode & ETM_PORT_WIDTH_MASK) == ETM_PORT_16BIT)\r
+               {\r
+                       if (ctx->data_half == 0)\r
+                       {\r
+                               *packet = ctx->trace_data[ctx->data_index].packet & 0xff;\r
+                               ctx->data_half = 1;\r
+                       }\r
+                       else\r
+                       {\r
+                               *packet = (ctx->trace_data[ctx->data_index].packet & 0xff00) >> 8;\r
+                               ctx->data_half = 0;\r
+                               ctx->data_index++;\r
+                       }\r
+               }\r
+               else if ((ctx->portmode & ETM_PORT_WIDTH_MASK) == ETM_PORT_8BIT)\r
+               {\r
+                       *packet = ctx->trace_data[ctx->data_index].packet & 0xff;\r
+                       ctx->data_index++;\r
+               }\r
+               else\r
+               {\r
+                       /* on a 4-bit port, a packet will be output during two consecutive cycles */\r
+                       if (ctx->data_index > (ctx->trace_depth - 2))\r
+                               return -1;\r
+                       \r
+                       *packet = ctx->trace_data[ctx->data_index].packet & 0xf;\r
+                       *packet |= (ctx->trace_data[ctx->data_index + 1].packet & 0xf) << 4;\r
+                       ctx->data_index += 2;\r
+               }\r
+                                       \r
+               return 0;\r
+       }\r
+       \r
+       return -1;\r
+}\r
+\r
+int etmv1_branch_address(etm_context_t *ctx)\r
+{\r
+       int retval;\r
+       u8 packet;\r
+       int shift = 0;\r
+       int apo;\r
+       int i;\r
+       \r
+       /* quit analysis if less than two cycles are left in the trace\r
+        * because we can't extract the APO */\r
+       if (ctx->data_index > (ctx->trace_depth - 2))\r
+               return -1;\r
+               \r
+       /* a BE could be output during an APO cycle, skip the current\r
+        * and continue with the new one */\r
+       if (ctx->trace_data[ctx->pipe_index + 1].pipestat & 0x4)\r
+               return 1;\r
+       if (ctx->trace_data[ctx->pipe_index + 2].pipestat & 0x4)\r
+               return 2;\r
+               \r
+       /* address packet offset encoded in the next two cycles' pipestat bits */\r
+       apo = ctx->trace_data[ctx->pipe_index + 1].pipestat & 0x3;\r
+       apo |= (ctx->trace_data[ctx->pipe_index + 2].pipestat & 0x3) << 2;\r
+       \r
+       /* count number of tracesync cycles between current pipe_index and data_index\r
+        * i.e. the number of tracesyncs that data_index already passed by\r
+        * to subtract them from the APO */\r
+       for (i = ctx->pipe_index; i < ctx->data_index; i++)\r
+       {\r
+               if (ctx->trace_data[ctx->pipe_index + 1].pipestat & ETMV1_TRACESYNC_CYCLE)\r
+                       apo--;\r
+       }\r
+       \r
+       /* extract up to four 7-bit packets */\r
+       do {\r
+               if ((retval = etmv1_next_packet(ctx, &packet, (shift == 0) ? apo + 1 : 0)) != 0)\r
+                       return -1;\r
+               ctx->last_branch &= ~(0x7f << shift);\r
+               ctx->last_branch |= (packet & 0x7f) << shift;\r
+               shift += 7;\r
+       } while ((packet & 0x80) && (shift < 28));\r
+       \r
+       /* one last packet holding 4 bits of the address, plus the branch reason code */\r
+       if ((shift == 28) && (packet & 0x80))\r
+       {\r
+               if ((retval = etmv1_next_packet(ctx, &packet, 0)) != 0)\r
+                       return -1;\r
+               ctx->last_branch &= 0x0fffffff;\r
+               ctx->last_branch |= (packet & 0x0f) << 28;\r
+               ctx->last_branch_reason = (packet & 0x70) >> 4;\r
+               shift += 4;\r
+       }\r
+       else\r
+       {\r
+               ctx->last_branch_reason = 0;\r
+       }\r
+       \r
+       if (shift == 32)\r
+       {\r
+               ctx->pc_ok = 1;\r
+       }\r
+       \r
+       /* if a full address was output, we might have branched into Jazelle state */\r
+       if ((shift == 32) && (packet & 0x80))\r
+       {\r
+               ctx->core_state = ARMV4_5_STATE_JAZELLE;\r
+       }\r
+       else\r
+       {\r
+               /* if we didn't branch into Jazelle state, the current processor state is\r
+                * encoded in bit 0 of the branch target address */\r
+               if (ctx->last_branch & 0x1)\r
+               {\r
+                       ctx->core_state = ARMV4_5_STATE_THUMB;\r
+                       ctx->last_branch &= ~0x1;\r
+               }\r
+               else\r
+               {\r
+                       ctx->core_state = ARMV4_5_STATE_ARM;\r
+                       ctx->last_branch &= ~0x3;\r
+               }\r
+       }\r
+       \r
+       return 0;\r
+}\r
+\r
+int etmv1_data(etm_context_t *ctx, int size, u32 *data)\r
+{\r
+       int j;\r
+       u8 buf[4];\r
+       int retval;\r
+       \r
+       for (j = 0; j < size; j++)\r
+       {\r
+               if ((retval = etmv1_next_packet(ctx, &buf[j], 0)) != 0)\r
+                       return -1;\r
+       }\r
+       \r
+       if (size == 8)\r
+               ERROR("TODO: add support for 64-bit values");\r
+       else if (size == 4)\r
+               *data = target_buffer_get_u32(ctx->target, buf);\r
+       else if (size == 2)\r
+               *data = target_buffer_get_u16(ctx->target, buf);\r
+       else if (size == 1)\r
+               *data = buf[0];\r
+               \r
+       return 0;\r
+}\r
+\r
+int etmv1_analyze_trace(etm_context_t *ctx, struct command_context_s *cmd_ctx)\r
+{\r
+       int retval;\r
+       arm_instruction_t instruction;\r
+       \r
+       /* read the trace data if it wasn't read already */\r
+       if (ctx->trace_depth == 0)\r
+               ctx->capture_driver->read_trace(ctx);\r
+       \r
+       /* start at the beginning of the captured trace */\r
+       ctx->pipe_index = 0;\r
+       ctx->data_index = 0;\r
+       ctx->data_half = 0;\r
+\r
+       /* neither the PC nor the data pointer are valid */     \r
+       ctx->pc_ok = 0;\r
+       ctx->ptr_ok = 0;\r
+       \r
+       while (ctx->pipe_index < ctx->trace_depth)\r
+       {\r
+               u8 pipestat = ctx->trace_data[ctx->pipe_index].pipestat;\r
+               u32 next_pc = ctx->current_pc;\r
+               u32 old_data_index = ctx->data_index;\r
+               u32 old_data_half = ctx->data_half;\r
+               u32 old_index = ctx->pipe_index;\r
+               u32 last_instruction = ctx->last_instruction;\r
+               u32 cycles = 0;\r
+               int current_pc_ok = ctx->pc_ok;\r
+               \r
+               if (ctx->trace_data[ctx->pipe_index].flags & ETMV1_TRIGGER_CYCLE)\r
+               {\r
+                       command_print(cmd_ctx, "--- trigger ---");\r
+               }\r
+\r
+               /* instructions execute in IE/D or BE/D cycles */\r
+               if ((pipestat == STAT_IE) || (pipestat == STAT_ID))\r
+                       ctx->last_instruction = ctx->pipe_index;\r
+               \r
+               /* if we don't have a valid pc skip until we reach an indirect branch */\r
+               if ((!ctx->pc_ok) && (pipestat != STAT_BE))\r
+               {\r
+                       ctx->pipe_index++;\r
+                       continue;\r
+               }\r
+               \r
+               /* any indirect branch could have interrupted instruction flow\r
+                * - the branch reason code could indicate a trace discontinuity\r
+                * - a branch to the exception vectors indicates an exception\r
+                */\r
+               if ((pipestat == STAT_BE) || (pipestat == STAT_BD))\r
+               {\r
+                       /* backup current data index, to be able to consume the branch address\r
+                        * before examining data address and values\r
+                        */\r
+                       old_data_index = ctx->data_index;\r
+                       old_data_half = ctx->data_half;\r
+\r
+                       ctx->last_instruction = ctx->pipe_index;\r
+                       \r
+                       if ((retval = etmv1_branch_address(ctx)) != 0)\r
+                       {\r
+                               /* negative return value from etmv1_branch_address means we ran out of packets,\r
+                                * quit analysing the trace */\r
+                               if (retval < 0)\r
+                                       break;\r
+                               \r
+                               /* a positive return values means the current branch was abandoned,\r
+                                * and a new branch was encountered in cycle ctx->pipe_index + retval;\r
+                                */\r
+                               WARNING("abandoned branch encountered, correctnes of analysis uncertain");\r
+                               ctx->pipe_index += retval;\r
+                               continue;\r
+                       }\r
+                       \r
+                       /* skip over APO cycles */\r
+                       ctx->pipe_index += 2;\r
+                       \r
+                       switch (ctx->last_branch_reason)\r
+                       {\r
+                               case 0x0:       /* normal PC change */\r
+                                       next_pc = ctx->last_branch;\r
+                                       break;\r
+                               case 0x1:       /* tracing enabled */\r
+                                       command_print(cmd_ctx, "--- tracing enabled at 0x%8.8x ---", ctx->last_branch);\r
+                                       ctx->current_pc = ctx->last_branch;\r
+                                       ctx->pipe_index++;\r
+                                       continue;\r
+                                       break;\r
+                               case 0x2:       /* trace restarted after FIFO overflow */\r
+                                       command_print(cmd_ctx, "--- trace restarted after FIFO overflow at 0x%8.8x ---", ctx->last_branch);\r
+                                       ctx->current_pc = ctx->last_branch;\r
+                                       ctx->pipe_index++;\r
+                                       continue;\r
+                                       break;\r
+                               case 0x3:       /* exit from debug state */\r
+                                       command_print(cmd_ctx, "--- exit from debug state at 0x%8.8x ---", ctx->last_branch);\r
+                                       ctx->current_pc = ctx->last_branch;\r
+                                       ctx->pipe_index++;\r
+                                       continue;\r
+                                       break;\r
+                               case 0x4:       /* periodic synchronization point */\r
+                                       next_pc = ctx->last_branch;\r
+                                       /* if we had no valid PC prior to this synchronization point,\r
+                                        * we have to move on with the next trace cycle\r
+                                        */\r
+                                       if (!current_pc_ok)\r
+                                       {\r
+                                               command_print(cmd_ctx, "--- periodic synchronization point at 0x%8.8x ---", next_pc);\r
+                                               ctx->current_pc = next_pc;\r
+                                               ctx->pipe_index++;\r
+                                               continue;\r
+                                       }\r
+                                       break;\r
+                               default:        /* reserved */\r
+                                       ERROR("BUG: branch reason code 0x%x is reserved", ctx->last_branch_reason);             \r
+                                       exit(-1);\r
+                                       break;\r
+                       }\r
+                       \r
+                       /* if we got here the branch was a normal PC change\r
+                        * (or a periodic synchronization point, which means the same for that matter)\r
+                        * if we didn't accquire a complete PC continue with the next cycle\r
+                        */\r
+                       if (!ctx->pc_ok)\r
+                               continue;\r
+                       \r
+                       /* indirect branch to the exception vector means an exception occured */\r
+                       if (((ctx->last_branch >= 0x0) && (ctx->last_branch <= 0x20))\r
+                               || ((ctx->last_branch >= 0xffff0000) && (ctx->last_branch <= 0xffff0020)))\r
+                       {\r
+                               if ((ctx->last_branch & 0xff) == 0x10)\r
+                               {\r
+                                       command_print(cmd_ctx, "data abort");\r
+                               }\r
+                               else\r
+                               {\r
+                                       command_print(cmd_ctx, "exception vector 0x%2.2x", ctx->last_branch);\r
+                                       ctx->current_pc = ctx->last_branch;\r
+                                       ctx->pipe_index++;\r
+                                       continue;\r
+                               }\r
+                       }\r
+               }\r
+               \r
+               /* an instruction was executed (or not, depending on the condition flags)\r
+                * retrieve it from the image for displaying */\r
+               if (ctx->pc_ok && (pipestat != STAT_WT) && (pipestat != STAT_TD) &&\r
+                       !(((pipestat == STAT_BE) || (pipestat == STAT_BD)) &&\r
+                               ((ctx->last_branch_reason != 0x0) && (ctx->last_branch_reason != 0x4))))  \r
+               {\r
+                       if ((retval = etm_read_instruction(ctx, &instruction)) != ERROR_OK)\r
+                       {\r
+                               /* can't continue tracing with no image available */\r
+                               if (retval == ERROR_TRACE_IMAGE_UNAVAILABLE)\r
+                               {\r
+                                       return retval;\r
+                               }\r
+                               else if (retval == ERROR_TRACE_INSTRUCTION_UNAVAILABLE)\r
+                               {\r
+                                       /* TODO: handle incomplete images \r
+                                        * for now we just quit the analsysis*/\r
+                                       return retval;\r
+                               }\r
+                       }\r
+                       \r
+                       cycles = old_index - last_instruction;\r
+               }\r
+               \r
+               if ((pipestat == STAT_ID) || (pipestat == STAT_BD))\r
+               {\r
+                       u32 new_data_index = ctx->data_index;\r
+                       u32 new_data_half = ctx->data_half;\r
+                       \r
+                       /* in case of a branch with data, the branch target address was consumed before\r
+                        * we temporarily go back to the saved data index */\r
+                       if (pipestat == STAT_BD)\r
+                       {\r
+                               ctx->data_index = old_data_index;\r
+                               ctx->data_half = old_data_half;\r
+                       }\r
+                       \r
+                       if (ctx->tracemode & ETMV1_TRACE_ADDR)\r
+                       {                       \r
+                               u8 packet;\r
+                               int shift = 0;\r
+                               \r
+                               do {\r
+                                       if ((retval = etmv1_next_packet(ctx, &packet, 0)) != 0)\r
+                                               return ERROR_ETM_ANALYSIS_FAILED;\r
+                                       ctx->last_ptr &= ~(0x7f << shift);\r
+                                       ctx->last_ptr |= (packet & 0x7f) << shift;\r
+                                       shift += 7;\r
+                               } while ((packet & 0x80) && (shift < 32));\r
+                               \r
+                               if (shift >= 32)\r
+                                       ctx->ptr_ok = 1;\r
+                               \r
+                               if (ctx->ptr_ok)\r
+                               {\r
+                                       command_print(cmd_ctx, "address: 0x%8.8x", ctx->last_ptr);\r
+                               }\r
+                       }\r
+                       \r
+                       if (ctx->tracemode & ETMV1_TRACE_DATA)\r
+                       {\r
+                               if ((instruction.type == ARM_LDM) || (instruction.type == ARM_STM))\r
+                               {\r
+                                       int i;\r
+                                       for (i = 0; i < 16; i++)\r
+                                       {\r
+                                               if (instruction.info.load_store_multiple.register_list & (1 << i))\r
+                                               {\r
+                                                       u32 data;\r
+                                                       if (etmv1_data(ctx, 4, &data) != 0)\r
+                                                               return ERROR_ETM_ANALYSIS_FAILED;\r
+                                                       command_print(cmd_ctx, "data: 0x%8.8x", data);\r
+                                               }\r
+                                       }\r
+                               }\r
+                               else if ((instruction.type >= ARM_LDR) && (instruction.type <= ARM_STRH))\r
+                               {\r
+                                       u32 data;\r
+                                       if (etmv1_data(ctx, arm_access_size(&instruction), &data) != 0)\r
+                                               return ERROR_ETM_ANALYSIS_FAILED;\r
+                                       command_print(cmd_ctx, "data: 0x%8.8x", data);\r
+                               }\r
+                       }\r
+                       \r
+                       /* restore data index after consuming BD address and data */\r
+                       if (pipestat == STAT_BD)\r
+                       {\r
+                               ctx->data_index = new_data_index;\r
+                               ctx->data_half = new_data_half;\r
+                       }\r
+               }\r
+               \r
+               /* adjust PC */\r
+               if ((pipestat == STAT_IE) || (pipestat == STAT_ID))\r
+               {\r
+                       if (((instruction.type == ARM_B) ||\r
+                               (instruction.type == ARM_BL) ||\r
+                               (instruction.type == ARM_BLX)) &&\r
+                               (instruction.info.b_bl_bx_blx.target_address != -1))\r
+                       {\r
+                               next_pc = instruction.info.b_bl_bx_blx.target_address;\r
+                       }\r
+                       else\r
+                       {\r
+                               next_pc += (ctx->core_state == ARMV4_5_STATE_ARM) ? 4 : 2;\r
+                       }\r
+               }\r
+               else if (pipestat == STAT_IN)\r
+               {\r
+                       next_pc += (ctx->core_state == ARMV4_5_STATE_ARM) ? 4 : 2;\r
+               }\r
+\r
+               if ((pipestat != STAT_TD) && (pipestat != STAT_WT))\r
+               {\r
+                       char cycles_text[32] = "";\r
+                       \r
+                       /* if the trace was captured with cycle accurate tracing enabled,\r
+                        * output the number of cycles since the last executed instruction\r
+                        */\r
+                       if (ctx->tracemode & ETMV1_CYCLE_ACCURATE)\r
+                       {\r
+                               snprintf(cycles_text, 32, " (%i %s)",\r
+                                       cycles,\r
+                                       (cycles == 1) ? "cycle" : "cycles");\r
+                       }\r
+                       \r
+                       command_print(cmd_ctx, "%s%s%s",\r
+                               instruction.text,\r
+                               (pipestat == STAT_IN) ? " (not executed)" : "",\r
+                               cycles_text);\r
+\r
+                       ctx->current_pc = next_pc;\r
+                       \r
+                       /* packets for an instruction don't start on or before the preceding\r
+                        * functional pipestat (i.e. other than WT or TD)\r
+                        */\r
+                       if (ctx->data_index <= ctx->pipe_index)\r
+                       {\r
+                               ctx->data_index = ctx->pipe_index + 1;\r
+                               ctx->data_half = 0;\r
+                       }\r
+               }\r
+               \r
+               ctx->pipe_index += 1;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_etm_tracemode_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target;\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       etmv1_tracemode_t tracemode;\r
+       \r
+       target = get_current_target(cmd_ctx);\r
+       \r
+       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (!arm7_9->etm_ctx)\r
+       {\r
+               command_print(cmd_ctx, "current target doesn't have an ETM configured");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       tracemode = arm7_9->etm_ctx->tracemode;\r
+\r
+       if (argc == 4)\r
+       {\r
+               if (strcmp(args[0], "none") == 0)\r
+               {\r
+                       tracemode = ETMV1_TRACE_NONE;\r
+               }\r
+               else if (strcmp(args[0], "data") == 0)\r
+               {\r
+                       tracemode = ETMV1_TRACE_DATA;\r
+               }\r
+               else if (strcmp(args[0], "address") == 0)\r
+               {\r
+                       tracemode = ETMV1_TRACE_ADDR;\r
+               }\r
+               else if (strcmp(args[0], "all") == 0)\r
+               {\r
+                       tracemode = ETMV1_TRACE_DATA | ETMV1_TRACE_ADDR;\r
+               }\r
+               else\r
+               {\r
+                       command_print(cmd_ctx, "invalid option '%s'", args[0]);\r
+                       return ERROR_OK;\r
+               }\r
+               \r
+               switch (strtol(args[1], NULL, 0))\r
+               {\r
+                       case 0:\r
+                               tracemode |= ETMV1_CONTEXTID_NONE;\r
+                               break;\r
+                       case 8:\r
+                               tracemode |= ETMV1_CONTEXTID_8;\r
+                               break;\r
+                       case 16:\r
+                               tracemode |= ETMV1_CONTEXTID_16;\r
+                               break;\r
+                       case 32:\r
+                               tracemode |= ETMV1_CONTEXTID_32;\r
+                               break;\r
+                       default:\r
+                               command_print(cmd_ctx, "invalid option '%s'", args[1]);\r
+                               return ERROR_OK;\r
+               }\r
+               \r
+               if (strcmp(args[2], "enable") == 0)\r
+               {\r
+                       tracemode |= ETMV1_CYCLE_ACCURATE;\r
+               }\r
+               else if (strcmp(args[2], "disable") == 0)\r
+               {\r
+                       tracemode |= 0;\r
+               }\r
+               else\r
+               {\r
+                       command_print(cmd_ctx, "invalid option '%s'", args[2]);\r
+                       return ERROR_OK;\r
+               }\r
+               \r
+               if (strcmp(args[3], "enable") == 0)\r
+               {\r
+                       tracemode |= ETMV1_BRANCH_OUTPUT;\r
+               }\r
+               else if (strcmp(args[3], "disable") == 0)\r
+               {\r
+                       tracemode |= 0;\r
+               }\r
+               else\r
+               {\r
+                       command_print(cmd_ctx, "invalid option '%s'", args[2]);\r
+                       return ERROR_OK;\r
+               }\r
+       }\r
+       else if (argc != 0)\r
+       {\r
+               command_print(cmd_ctx, "usage: configure trace mode <none|data|address|all> <context id bits> <cycle accurate> <branch output>");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       command_print(cmd_ctx, "current tracemode configuration:");\r
+       \r
+       switch (tracemode & ETMV1_TRACE_MASK)\r
+       {\r
+               case ETMV1_TRACE_NONE:\r
+                       command_print(cmd_ctx, "data tracing: none");\r
+                       break;\r
+               case ETMV1_TRACE_DATA:\r
+                       command_print(cmd_ctx, "data tracing: data only");\r
+                       break;\r
+               case ETMV1_TRACE_ADDR:\r
+                       command_print(cmd_ctx, "data tracing: address only");\r
+                       break;\r
+               case ETMV1_TRACE_DATA | ETMV1_TRACE_ADDR:\r
+                       command_print(cmd_ctx, "data tracing: address and data");\r
+                       break;\r
+       }\r
+       \r
+       switch (tracemode & ETMV1_CONTEXTID_MASK)\r
+       {\r
+               case ETMV1_CONTEXTID_NONE:\r
+                       command_print(cmd_ctx, "contextid tracing: none");\r
+                       break;\r
+               case ETMV1_CONTEXTID_8:\r
+                       command_print(cmd_ctx, "contextid tracing: 8 bit");\r
+                       break;\r
+               case ETMV1_CONTEXTID_16:\r
+                       command_print(cmd_ctx, "contextid tracing: 16 bit");\r
+                       break;\r
+               case ETMV1_CONTEXTID_32:\r
+                       command_print(cmd_ctx, "contextid tracing: 32 bit");\r
+                       break;\r
+       }\r
+       \r
+       if (tracemode & ETMV1_CYCLE_ACCURATE)\r
+       {\r
+               command_print(cmd_ctx, "cycle-accurate tracing enabled");\r
+       }\r
+       else\r
+       {\r
+               command_print(cmd_ctx, "cycle-accurate tracing disabled");\r
+       }\r
+\r
+       if (tracemode & ETMV1_BRANCH_OUTPUT)\r
+       {\r
+               command_print(cmd_ctx, "full branch address output enabled");\r
+       }\r
+       else\r
+       {\r
+               command_print(cmd_ctx, "full branch address output disabled");\r
+       }\r
+       \r
+       /* only update ETM_CTRL register if tracemode changed */\r
+       if (arm7_9->etm_ctx->tracemode != tracemode)\r
+       {\r
+               reg_t *etm_ctrl_reg = &arm7_9->etm_ctx->reg_cache->reg_list[ETM_CTRL];\r
+               \r
+               etm_get_reg(etm_ctrl_reg);\r
+               \r
+               buf_set_u32(etm_ctrl_reg->value, 2, 2, tracemode & ETMV1_TRACE_MASK);\r
+               buf_set_u32(etm_ctrl_reg->value, 14, 2, (tracemode & ETMV1_CONTEXTID_MASK) >> 4);\r
+               buf_set_u32(etm_ctrl_reg->value, 12, 1, (tracemode & ETMV1_CYCLE_ACCURATE) >> 8);\r
+               buf_set_u32(etm_ctrl_reg->value, 8, 1, (tracemode & ETMV1_BRANCH_OUTPUT) >> 9);\r
+               etm_store_reg(etm_ctrl_reg);\r
+               \r
+               arm7_9->etm_ctx->tracemode = tracemode;\r
+               \r
+               /* invalidate old trace data */\r
+               arm7_9->etm_ctx->capture_status = TRACE_IDLE;\r
+               if (arm7_9->etm_ctx->trace_depth > 0)\r
+               {\r
+                       free(arm7_9->etm_ctx->trace_data);\r
+                       arm7_9->etm_ctx->trace_data = NULL;\r
+               }\r
+               arm7_9->etm_ctx->trace_depth = 0;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_etm_config_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target;\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       etm_portmode_t portmode = 0x0;\r
+       etm_context_t *etm_ctx = malloc(sizeof(etm_context_t));\r
+       int i;\r
+       \r
+       if (argc != 5)\r
+       {\r
+               ERROR("incomplete 'etm config <target> <port_width> <port_mode> <clocking> <capture_driver>' command");\r
+               exit(-1);\r
+       }\r
+       \r
+       target = get_target_by_num(strtoul(args[0], NULL, 0));\r
+       \r
+       if (!target)\r
+       {\r
+               ERROR("target number '%s' not defined", args[0]);\r
+               exit(-1);\r
+       }\r
+       \r
+       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       switch (strtoul(args[1], NULL, 0))\r
+       {\r
+               case 4:\r
+                       portmode |= ETM_PORT_4BIT;\r
+                       break;\r
+               case 8:\r
+                       portmode |= ETM_PORT_8BIT;\r
+                       break;\r
+               case 16:\r
+                       portmode |= ETM_PORT_16BIT;\r
+                       break;\r
+               default:\r
+                       command_print(cmd_ctx, "unsupported ETM port width '%s', must be 4, 8 or 16", args[1]);\r
+                       return ERROR_OK;\r
+       }\r
+       \r
+       if (strcmp("normal", args[2]) == 0)\r
+       {\r
+               portmode |= ETM_PORT_NORMAL;\r
+       }\r
+       else if (strcmp("multiplexed", args[2]) == 0)\r
+       {\r
+               portmode |= ETM_PORT_MUXED;\r
+       }\r
+       else if (strcmp("demultiplexed", args[2]) == 0)\r
+       {\r
+               portmode |= ETM_PORT_DEMUXED;\r
+       }\r
+       else\r
+       {\r
+               command_print(cmd_ctx, "unsupported ETM port mode '%s', must be 'normal', 'multiplexed' or 'demultiplexed'", args[2]);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (strcmp("half", args[3]) == 0)\r
+       {\r
+               portmode |= ETM_PORT_HALF_CLOCK;\r
+       }\r
+       else if (strcmp("full", args[3]) == 0)\r
+       {\r
+               portmode |= ETM_PORT_FULL_CLOCK;\r
+       }\r
+       else\r
+       {\r
+               command_print(cmd_ctx, "unsupported ETM port clocking '%s', must be 'full' or 'half'", args[3]);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       for (i=0; etm_capture_drivers[i]; i++)\r
+       {\r
+               if (strcmp(args[4], etm_capture_drivers[i]->name) == 0)\r
+               {\r
+                       if (etm_capture_drivers[i]->register_commands(cmd_ctx) != ERROR_OK)\r
+                       {\r
+                               free(etm_ctx);\r
+                               exit(-1);\r
+                       }\r
+               \r
+                       etm_ctx->capture_driver = etm_capture_drivers[i];\r
+\r
+                       break;\r
+               }\r
+       }\r
+       \r
+       if (!etm_capture_drivers[i])\r
+       {\r
+               /* no supported capture driver found, don't register an ETM */\r
+               free(etm_ctx);\r
+               ERROR("trace capture driver '%s' not found", args[4]);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       etm_ctx->target = target;\r
+       etm_ctx->trigger_percent = 50;\r
+       etm_ctx->trace_data = NULL;\r
+       etm_ctx->trace_depth = 0;\r
+       etm_ctx->portmode = portmode;\r
+       etm_ctx->tracemode = 0x0;\r
+       etm_ctx->core_state = ARMV4_5_STATE_ARM;\r
+       etm_ctx->image = NULL;\r
+       etm_ctx->pipe_index = 0;\r
+       etm_ctx->data_index = 0;\r
+       etm_ctx->current_pc = 0x0;\r
+       etm_ctx->pc_ok = 0;\r
+       etm_ctx->last_branch = 0x0;\r
+       etm_ctx->last_branch_reason = 0x0;\r
+       etm_ctx->last_ptr = 0x0;\r
+       etm_ctx->ptr_ok = 0x0;\r
+       etm_ctx->context_id = 0x0;\r
+       etm_ctx->last_instruction = 0;\r
+       \r
+       arm7_9->etm_ctx = etm_ctx;\r
+       \r
+       etm_register_user_commands(cmd_ctx);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_etm_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target;\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       reg_t *etm_config_reg;\r
+       reg_t *etm_sys_config_reg;\r
+       \r
+       int max_port_size;\r
+               \r
+       target = get_current_target(cmd_ctx);\r
+       \r
+       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (!arm7_9->etm_ctx)\r
+       {\r
+               command_print(cmd_ctx, "current target doesn't have an ETM configured");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       etm_config_reg = &arm7_9->etm_ctx->reg_cache->reg_list[ETM_CONFIG];\r
+       etm_sys_config_reg = &arm7_9->etm_ctx->reg_cache->reg_list[ETM_SYS_CONFIG];\r
+       \r
+       etm_get_reg(etm_config_reg);\r
+       command_print(cmd_ctx, "pairs of address comparators: %i", buf_get_u32(etm_config_reg->value, 0, 4));\r
+       command_print(cmd_ctx, "pairs of data comparators: %i", buf_get_u32(etm_config_reg->value, 4, 4));\r
+       command_print(cmd_ctx, "memory map decoders: %i", buf_get_u32(etm_config_reg->value, 8, 5));\r
+       command_print(cmd_ctx, "number of counters: %i", buf_get_u32(etm_config_reg->value, 13, 3));\r
+       command_print(cmd_ctx, "sequencer %spresent",\r
+                       (buf_get_u32(etm_config_reg->value, 16, 1) == 1) ? "" : "not ");\r
+       command_print(cmd_ctx, "number of ext. inputs: %i", buf_get_u32(etm_config_reg->value, 17, 3));\r
+       command_print(cmd_ctx, "number of ext. outputs: %i", buf_get_u32(etm_config_reg->value, 20, 3));\r
+       command_print(cmd_ctx, "FIFO full %spresent",\r
+                       (buf_get_u32(etm_config_reg->value, 23, 1) == 1) ? "" : "not ");\r
+       command_print(cmd_ctx, "protocol version: %i", buf_get_u32(etm_config_reg->value, 28, 3));\r
+       \r
+       etm_get_reg(etm_sys_config_reg);\r
+\r
+       switch (buf_get_u32(etm_sys_config_reg->value, 0, 3))\r
+       {\r
+               case 0:\r
+                       max_port_size = 4;\r
+                       break;\r
+               case 1:\r
+                       max_port_size = 8;\r
+                       break;\r
+               case 2:\r
+                       max_port_size = 16;\r
+                       break;\r
+       }\r
+       command_print(cmd_ctx, "max. port size: %i", max_port_size);\r
+       \r
+       command_print(cmd_ctx, "half-rate clocking %ssupported",\r
+                       (buf_get_u32(etm_sys_config_reg->value, 3, 1) == 1) ? "" : "not ");\r
+       command_print(cmd_ctx, "full-rate clocking %ssupported",\r
+                       (buf_get_u32(etm_sys_config_reg->value, 4, 1) == 1) ? "" : "not ");\r
+       command_print(cmd_ctx, "normal trace format %ssupported",\r
+                       (buf_get_u32(etm_sys_config_reg->value, 5, 1) == 1) ? "" : "not ");\r
+       command_print(cmd_ctx, "multiplex trace format %ssupported",\r
+                       (buf_get_u32(etm_sys_config_reg->value, 6, 1) == 1) ? "" : "not ");\r
+       command_print(cmd_ctx, "demultiplex trace format %ssupported",\r
+                       (buf_get_u32(etm_sys_config_reg->value, 7, 1) == 1) ? "" : "not ");\r
+       command_print(cmd_ctx, "FIFO full %ssupported",\r
+                       (buf_get_u32(etm_sys_config_reg->value, 8, 1) == 1) ? "" : "not ");\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_etm_status_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target;\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       trace_status_t trace_status;\r
+       \r
+       target = get_current_target(cmd_ctx);\r
+       \r
+       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (!arm7_9->etm_ctx)\r
+       {\r
+               command_print(cmd_ctx, "current target doesn't have an ETM configured");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       trace_status = arm7_9->etm_ctx->capture_driver->status(arm7_9->etm_ctx);\r
+       \r
+       if (trace_status == TRACE_IDLE)\r
+       {\r
+               command_print(cmd_ctx, "tracing is idle");\r
+       }\r
+       else\r
+       {\r
+               static char *completed = " completed";\r
+               static char *running = " is running";\r
+               static char *overflowed = ", trace overflowed";\r
+               static char *triggered = ", trace triggered";\r
+               \r
+               command_print(cmd_ctx, "trace collection%s%s%s", \r
+                       (trace_status & TRACE_RUNNING) ? running : completed,\r
+                       (trace_status & TRACE_OVERFLOWED) ? overflowed : "",\r
+                       (trace_status & TRACE_TRIGGERED) ? triggered : "");\r
+               \r
+               if (arm7_9->etm_ctx->trace_depth > 0)\r
+               {\r
+                       command_print(cmd_ctx, "%i frames of trace data read", arm7_9->etm_ctx->trace_depth);\r
+               }\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_etm_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target;\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       etm_context_t *etm_ctx;\r
+\r
+       if (argc < 1)\r
+       {\r
+               command_print(cmd_ctx, "usage: etm image <file> [base address] [type]");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       target = get_current_target(cmd_ctx);\r
+       \r
+       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (!(etm_ctx = arm7_9->etm_ctx))\r
+       {\r
+               command_print(cmd_ctx, "current target doesn't have an ETM configured");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (etm_ctx->image)\r
+       {\r
+               image_close(etm_ctx->image);\r
+               free(etm_ctx->image);\r
+               command_print(cmd_ctx, "previously loaded image found and closed");\r
+       }\r
+       \r
+       etm_ctx->image = malloc(sizeof(image_t));\r
+       etm_ctx->image->base_address_set = 0;\r
+       etm_ctx->image->start_address_set = 0;\r
+       \r
+       /* a base address isn't always necessary, default to 0x0 (i.e. don't relocate) */\r
+       if (argc >= 2)\r
+       {\r
+               etm_ctx->image->base_address_set = 1;\r
+               etm_ctx->image->base_address = strtoul(args[1], NULL, 0);\r
+       }\r
+       else\r
+       {\r
+               etm_ctx->image->base_address_set = 0;\r
+       }\r
+               \r
+       if (image_open(etm_ctx->image, args[0], (argc >= 3) ? args[2] : NULL) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "image opening error: %s", etm_ctx->image->error_str);\r
+               free(etm_ctx->image);\r
+               etm_ctx->image = NULL;\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_etm_dump_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       fileio_t file;\r
+       target_t *target;\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       etm_context_t *etm_ctx;\r
+       int i;\r
+       \r
+       if (argc != 1)\r
+       {\r
+               command_print(cmd_ctx, "usage: etm dump <file>");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       target = get_current_target(cmd_ctx);\r
+       \r
+       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (!(etm_ctx = arm7_9->etm_ctx))\r
+       {\r
+               command_print(cmd_ctx, "current target doesn't have an ETM configured");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (etm_ctx->capture_driver->status == TRACE_IDLE)\r
+       {\r
+               command_print(cmd_ctx, "trace capture wasn't enabled, no trace data captured");\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (etm_ctx->capture_driver->status(etm_ctx) & TRACE_RUNNING)\r
+       {\r
+               /* TODO: if on-the-fly capture is to be supported, this needs to be changed */\r
+               command_print(cmd_ctx, "trace capture not completed");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       /* read the trace data if it wasn't read already */\r
+       if (etm_ctx->trace_depth == 0)\r
+               etm_ctx->capture_driver->read_trace(etm_ctx);\r
+       \r
+       if (fileio_open(&file, args[0], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "file open error: %s", file.error_str);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       fileio_write_u32(&file, etm_ctx->capture_status);\r
+       fileio_write_u32(&file, etm_ctx->portmode);\r
+       fileio_write_u32(&file, etm_ctx->tracemode);\r
+       fileio_write_u32(&file, etm_ctx->trace_depth);\r
+       \r
+       for (i = 0; i < etm_ctx->trace_depth; i++)\r
+       {\r
+               fileio_write_u32(&file, etm_ctx->trace_data[i].pipestat);\r
+               fileio_write_u32(&file, etm_ctx->trace_data[i].packet);\r
+               fileio_write_u32(&file, etm_ctx->trace_data[i].flags);\r
+       }\r
+       \r
+       fileio_close(&file);\r
+       \r
+       return ERROR_OK;        \r
+}\r
+\r
+int handle_etm_load_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       fileio_t file;\r
+       target_t *target;\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       etm_context_t *etm_ctx;\r
+       int i;\r
+       \r
+       if (argc != 1)\r
+       {\r
+               command_print(cmd_ctx, "usage: etm load <file>");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       target = get_current_target(cmd_ctx);\r
+       \r
+       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (!(etm_ctx = arm7_9->etm_ctx))\r
+       {\r
+               command_print(cmd_ctx, "current target doesn't have an ETM configured");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (etm_ctx->capture_driver->status(etm_ctx) & TRACE_RUNNING)\r
+       {\r
+               command_print(cmd_ctx, "trace capture running, stop first");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (fileio_open(&file, args[0], FILEIO_READ, FILEIO_BINARY) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "file open error: %s", file.error_str);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (file.size % 4)\r
+       {\r
+               command_print(cmd_ctx, "size isn't a multiple of 4, no valid trace data");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (etm_ctx->trace_depth > 0)\r
+       {\r
+               free(etm_ctx->trace_data);\r
+       }\r
+       \r
+       fileio_read_u32(&file, &etm_ctx->capture_status);\r
+       fileio_read_u32(&file, &etm_ctx->portmode);\r
+       fileio_read_u32(&file, &etm_ctx->tracemode);\r
+       fileio_read_u32(&file, &etm_ctx->trace_depth);\r
+       \r
+       etm_ctx->trace_data = malloc(sizeof(etmv1_trace_data_t) * etm_ctx->trace_depth);\r
+       \r
+       for (i = 0; i < etm_ctx->trace_depth; i++)\r
+       {\r
+               u32 pipestat, packet, flags;\r
+               fileio_read_u32(&file, &pipestat);\r
+               fileio_read_u32(&file, &packet);\r
+               fileio_read_u32(&file, &flags);\r
+               etm_ctx->trace_data[i].pipestat = pipestat & 0xff;\r
+               etm_ctx->trace_data[i].packet = packet & 0xffff;\r
+               etm_ctx->trace_data[i].flags = flags;\r
+       }\r
+       \r
+       fileio_close(&file);\r
+       \r
+       return ERROR_OK;        \r
+}\r
+\r
+int handle_etm_trigger_percent_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target;\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       etm_context_t *etm_ctx;\r
+       \r
+       target = get_current_target(cmd_ctx);\r
+       \r
+       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (!(etm_ctx = arm7_9->etm_ctx))\r
+       {\r
+               command_print(cmd_ctx, "current target doesn't have an ETM configured");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (argc > 0)\r
+       {\r
+               u32 new_value = strtoul(args[0], NULL, 0);\r
+               \r
+               if ((new_value < 2) || (new_value > 100))\r
+               {\r
+                       command_print(cmd_ctx, "valid settings are 2% to 100%");\r
+               }\r
+               else\r
+               {\r
+                       etm_ctx->trigger_percent = new_value;\r
+               }\r
+       }\r
+       \r
+       command_print(cmd_ctx, "%i percent of the tracebuffer reserved for after the trigger", etm_ctx->trigger_percent);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_etm_start_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target;\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       etm_context_t *etm_ctx;\r
+       reg_t *etm_ctrl_reg;\r
+\r
+       target = get_current_target(cmd_ctx);\r
+       \r
+       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (!(etm_ctx = arm7_9->etm_ctx))\r
+       {\r
+               command_print(cmd_ctx, "current target doesn't have an ETM configured");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       /* invalidate old tracing data */\r
+       arm7_9->etm_ctx->capture_status = TRACE_IDLE;\r
+       if (arm7_9->etm_ctx->trace_depth > 0)\r
+       {\r
+               free(arm7_9->etm_ctx->trace_data);\r
+               arm7_9->etm_ctx->trace_data = NULL;\r
+       }\r
+       arm7_9->etm_ctx->trace_depth = 0;\r
+               \r
+       etm_ctrl_reg = &arm7_9->etm_ctx->reg_cache->reg_list[ETM_CTRL];\r
+       etm_get_reg(etm_ctrl_reg);\r
+               \r
+       /* Clear programming bit (10), set port selection bit (11) */\r
+       buf_set_u32(etm_ctrl_reg->value, 10, 2, 0x2);\r
+\r
+       etm_store_reg(etm_ctrl_reg);\r
+       jtag_execute_queue();\r
+\r
+       etm_ctx->capture_driver->start_capture(etm_ctx);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_etm_stop_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target;\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       etm_context_t *etm_ctx;\r
+       reg_t *etm_ctrl_reg;\r
+\r
+       target = get_current_target(cmd_ctx);\r
+       \r
+       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (!(etm_ctx = arm7_9->etm_ctx))\r
+       {\r
+               command_print(cmd_ctx, "current target doesn't have an ETM configured");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       etm_ctrl_reg = &arm7_9->etm_ctx->reg_cache->reg_list[ETM_CTRL];\r
+       etm_get_reg(etm_ctrl_reg);\r
+               \r
+       /* Set programming bit (10), clear port selection bit (11) */\r
+       buf_set_u32(etm_ctrl_reg->value, 10, 2, 0x1);\r
+\r
+       etm_store_reg(etm_ctrl_reg);    \r
+       jtag_execute_queue();\r
+       \r
+       etm_ctx->capture_driver->stop_capture(etm_ctx);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_etm_analyze_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target;\r
+       armv4_5_common_t *armv4_5;\r
+       arm7_9_common_t *arm7_9;\r
+       etm_context_t *etm_ctx;\r
+       int retval;\r
+\r
+       target = get_current_target(cmd_ctx);\r
+       \r
+       if (arm7_9_get_arch_pointers(target, &armv4_5, &arm7_9) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "current target isn't an ARM7/ARM9 target");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (!(etm_ctx = arm7_9->etm_ctx))\r
+       {\r
+               command_print(cmd_ctx, "current target doesn't have an ETM configured");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if ((retval = etmv1_analyze_trace(etm_ctx, cmd_ctx)) != ERROR_OK)\r
+       {\r
+               switch(retval)\r
+               {\r
+                       case ERROR_ETM_ANALYSIS_FAILED:\r
+                               command_print(cmd_ctx, "further analysis failed (corrupted trace data or just end of data");\r
+                               break;\r
+                       case ERROR_TRACE_INSTRUCTION_UNAVAILABLE:\r
+                               command_print(cmd_ctx, "no instruction for current address available, analysis aborted");\r
+                               break;\r
+                       case ERROR_TRACE_IMAGE_UNAVAILABLE:\r
+                               command_print(cmd_ctx, "no image available for trace analysis");\r
+                               break;\r
+                       default:\r
+                               command_print(cmd_ctx, "unknown error: %i", retval);\r
+               }\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int etm_register_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       etm_cmd = register_command(cmd_ctx, NULL, "etm", NULL, COMMAND_ANY, "Embedded Trace Macrocell");\r
+\r
+       register_command(cmd_ctx, etm_cmd, "config", handle_etm_config_command, COMMAND_CONFIG, NULL);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int etm_register_user_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       register_command(cmd_ctx, etm_cmd, "tracemode", handle_etm_tracemode_command,\r
+               COMMAND_EXEC, "configure trace mode <none|data|address|all> <context id bits> <cycle accurate> <branch output");\r
+\r
+       register_command(cmd_ctx, etm_cmd, "info", handle_etm_info_command,\r
+               COMMAND_EXEC, "display info about the current target's ETM");\r
+\r
+       register_command(cmd_ctx, etm_cmd, "trigger_percent <percent>", handle_etm_trigger_percent_command,\r
+               COMMAND_EXEC, "amount (<percent>) of trace buffer to be filled after the trigger occured");\r
+       register_command(cmd_ctx, etm_cmd, "status", handle_etm_status_command,\r
+               COMMAND_EXEC, "display current target's ETM status");\r
+       register_command(cmd_ctx, etm_cmd, "start", handle_etm_start_command,\r
+               COMMAND_EXEC, "start ETM trace collection");\r
+       register_command(cmd_ctx, etm_cmd, "stop", handle_etm_stop_command,\r
+               COMMAND_EXEC, "stop ETM trace collection");\r
+\r
+       register_command(cmd_ctx, etm_cmd, "analyze", handle_etm_analyze_command,\r
+               COMMAND_EXEC, "anaylze collected ETM trace");\r
+\r
+       register_command(cmd_ctx, etm_cmd, "image", handle_etm_image_command,\r
+               COMMAND_EXEC, "load image from <file> [base address]");\r
+\r
+       register_command(cmd_ctx, etm_cmd, "dump", handle_etm_dump_command,\r
+               COMMAND_EXEC, "dump captured trace data <file>");\r
+       register_command(cmd_ctx, etm_cmd, "load", handle_etm_load_command,\r
+               COMMAND_EXEC, "load trace data for analysis <file>");\r
+\r
+       return ERROR_OK;\r
+}\r
index 27df12fc33f42d34466c6f5ea6ad300ce90f822a..a9f7c1034b893b2e9f557b120e0fcb2ee3eed643 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2005 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "replacements.h"
-#include "target.h"
-#include "target_request.h"
-
-#include "log.h"
-#include "configuration.h"
-#include "binarybuffer.h"
-#include "jtag.h"
-
-#include <string.h>
-#include <stdlib.h>
-#include <inttypes.h>
-
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <unistd.h>
-#include <errno.h>
-
-#include <sys/time.h>
-#include <time.h>
-
-#include <time_support.h>
-
-#include <fileio.h>
-#include <image.h>
-
-int cli_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv);
-
-
-int handle_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_daemon_startup_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_targets_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-int handle_target_script_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_run_and_halt_time_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_working_area_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-
-int handle_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_poll_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_wait_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_soft_reset_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_resume_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_step_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_md_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_mw_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_load_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_dump_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_verify_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_bp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_rbp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_wp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_rwp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
-int handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc);
-
-/* targets
- */
-extern target_type_t arm7tdmi_target;
-extern target_type_t arm720t_target;
-extern target_type_t arm9tdmi_target;
-extern target_type_t arm920t_target;
-extern target_type_t arm966e_target;
-extern target_type_t arm926ejs_target;
-extern target_type_t feroceon_target;
-extern target_type_t xscale_target;
-extern target_type_t cortexm3_target;
-
-target_type_t *target_types[] =
-{
-       &arm7tdmi_target,
-       &arm9tdmi_target,
-       &arm920t_target,
-       &arm720t_target,
-       &arm966e_target,
-       &arm926ejs_target,
-       &feroceon_target,
-       &xscale_target,
-       &cortexm3_target,
-       NULL,
-};
-
-target_t *targets = NULL;
-target_event_callback_t *target_event_callbacks = NULL;
-target_timer_callback_t *target_timer_callbacks = NULL;
-
-char *target_state_strings[] =
-{
-       "unknown",
-       "running",
-       "halted",
-       "reset",
-       "debug_running",
-};
-
-char *target_debug_reason_strings[] =
-{
-       "debug request", "breakpoint", "watchpoint",
-       "watchpoint and breakpoint", "single step",
-       "target not halted"
-};
-
-char *target_endianess_strings[] =
-{
-       "big endian",
-       "little endian",
-};
-
-enum daemon_startup_mode startup_mode = DAEMON_ATTACH;
-
-static int target_continous_poll = 1;
-
-/* read a u32 from a buffer in target memory endianness */
-u32 target_buffer_get_u32(target_t *target, u8 *buffer)
-{
-       if (target->endianness == TARGET_LITTLE_ENDIAN)
-               return le_to_h_u32(buffer);
-       else
-               return be_to_h_u32(buffer);
-}
-
-/* read a u16 from a buffer in target memory endianness */
-u16 target_buffer_get_u16(target_t *target, u8 *buffer)
-{
-       if (target->endianness == TARGET_LITTLE_ENDIAN)
-               return le_to_h_u16(buffer);
-       else
-               return be_to_h_u16(buffer);
-}
-
-/* write a u32 to a buffer in target memory endianness */
-void target_buffer_set_u32(target_t *target, u8 *buffer, u32 value)
-{
-       if (target->endianness == TARGET_LITTLE_ENDIAN)
-               h_u32_to_le(buffer, value);
-       else
-               h_u32_to_be(buffer, value);
-}
-
-/* write a u16 to a buffer in target memory endianness */
-void target_buffer_set_u16(target_t *target, u8 *buffer, u16 value)
-{
-       if (target->endianness == TARGET_LITTLE_ENDIAN)
-               h_u16_to_le(buffer, value);
-       else
-               h_u16_to_be(buffer, value);
-}
-
-/* returns a pointer to the n-th configured target */
-target_t* get_target_by_num(int num)
-{
-       target_t *target = targets;
-       int i = 0;
-
-       while (target)
-       {
-               if (num == i)
-                       return target;
-               target = target->next;
-               i++;
-       }
-
-       return NULL;
-}
-
-int get_num_by_target(target_t *query_target)
-{
-       target_t *target = targets;
-       int i = 0;      
-       
-       while (target)
-       {
-               if (target == query_target)
-                       return i;
-               target = target->next;
-               i++;
-       }
-       
-       return -1;
-}
-
-target_t* get_current_target(command_context_t *cmd_ctx)
-{
-       target_t *target = get_target_by_num(cmd_ctx->current_target);
-       
-       if (target == NULL)
-       {
-               ERROR("BUG: current_target out of bounds");
-               exit(-1);
-       }
-       
-       return target;
-}
-
-/* Process target initialization, when target entered debug out of reset
- * the handler is unregistered at the end of this function, so it's only called once
- */
-int target_init_handler(struct target_s *target, enum target_event event, void *priv)
-{
-       FILE *script;
-       struct command_context_s *cmd_ctx = priv;
-       
-       if ((event == TARGET_EVENT_HALTED) && (target->reset_script))
-       {
-               target_unregister_event_callback(target_init_handler, priv);
-
-               script = open_file_from_path(cmd_ctx, target->reset_script, "r");
-               if (!script)
-               {
-                       ERROR("couldn't open script file %s", target->reset_script);
-                               return ERROR_OK;
-               }
-
-               INFO("executing reset script '%s'", target->reset_script);
-               command_run_file(cmd_ctx, script, COMMAND_EXEC);
-               fclose(script);
-
-               jtag_execute_queue();
-       }
-       
-       return ERROR_OK;
-}
-
-int target_run_and_halt_handler(void *priv)
-{
-       target_t *target = priv;
-       
-       target->type->halt(target);
-       
-       return ERROR_OK;
-}
-
-int target_process_reset(struct command_context_s *cmd_ctx)
-{
-       int retval = ERROR_OK;
-       target_t *target;
-       struct timeval timeout, now;
-       
-       /* prepare reset_halt where necessary */
-       target = targets;
-       while (target)
-       {
-               if (jtag_reset_config & RESET_SRST_PULLS_TRST)
-               {
-                       switch (target->reset_mode)
-                       {
-                               case RESET_HALT:
-                                       command_print(cmd_ctx, "nSRST pulls nTRST, falling back to RESET_RUN_AND_HALT");
-                                       target->reset_mode = RESET_RUN_AND_HALT;
-                                       break;
-                               case RESET_INIT:
-                                       command_print(cmd_ctx, "nSRST pulls nTRST, falling back to RESET_RUN_AND_INIT");
-                                       target->reset_mode = RESET_RUN_AND_INIT;
-                                       break;
-                               default:
-                                       break;
-                       } 
-               }
-               switch (target->reset_mode)
-               {
-                       case RESET_HALT:
-                       case RESET_INIT:
-                               target->type->prepare_reset_halt(target);
-                               break;
-                       default:
-                               break;
-               }
-               target = target->next;
-       }
-       
-       target = targets;
-       while (target)
-       {
-               target->type->assert_reset(target);
-               target = target->next;
-       }
-       jtag_execute_queue();
-       
-       /* request target halt if necessary, and schedule further action */
-       target = targets;
-       while (target)
-       {
-               switch (target->reset_mode)
-               {
-                       case RESET_RUN:
-                               /* nothing to do if target just wants to be run */
-                               break;
-                       case RESET_RUN_AND_HALT:
-                               /* schedule halt */
-                               target_register_timer_callback(target_run_and_halt_handler, target->run_and_halt_time, 0, target);
-                               break;
-                       case RESET_RUN_AND_INIT:
-                               /* schedule halt */
-                               target_register_timer_callback(target_run_and_halt_handler, target->run_and_halt_time, 0, target);
-                               target_register_event_callback(target_init_handler, cmd_ctx);
-                               break;
-                       case RESET_HALT:
-                               target->type->halt(target);
-                               break;
-                       case RESET_INIT:
-                               target->type->halt(target);
-                               target_register_event_callback(target_init_handler, cmd_ctx);
-                               break;
-                       default:
-                               ERROR("BUG: unknown target->reset_mode");
-               }
-               target = target->next;
-       }
-       
-       target = targets;
-       while (target)
-       {
-               target->type->deassert_reset(target);
-               target = target->next;
-       }
-       jtag_execute_queue();
-
-       /* Wait for reset to complete, maximum 5 seconds. */    
-       gettimeofday(&timeout, NULL);
-       timeval_add_time(&timeout, 5, 0);
-       for(;;)
-       {
-               gettimeofday(&now, NULL);
-               
-               target_call_timer_callbacks();
-               
-               target = targets;
-               while (target)
-               {
-                       target->type->poll(target);
-                       if ((target->reset_mode == RESET_RUN_AND_INIT) || (target->reset_mode == RESET_RUN_AND_HALT))
-                       {
-                               if (target->state != TARGET_HALTED)
-                               {
-                                       if ((now.tv_sec > timeout.tv_sec) || ((now.tv_sec == timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec)))
-                                       {
-                                               command_print(cmd_ctx, "Timed out waiting for reset");
-                                               goto done;
-                                       }
-                                       usleep(100*1000); /* Do not eat all cpu */
-                                       goto again;
-                               }
-                       }
-                       target = target->next;
-               }
-               /* All targets we're waiting for are halted */
-               break;
-               
-               again:;
-       }
-       done:
-       
-       
-       /* We want any events to be processed before the prompt */
-       target_call_timer_callbacks();
-       
-       return retval;
-}
-
-static int default_virt2phys(struct target_s *target, u32 virtual, u32 *physical)
-{
-       *physical = virtual;
-       return ERROR_OK;
-}
-
-static int default_mmu(struct target_s *target, int *enabled)
-{
-       *enabled = 0;
-       return ERROR_OK;
-}
-
-int target_init(struct command_context_s *cmd_ctx)
-{
-       target_t *target = targets;
-       
-       while (target)
-       {
-               if (target->type->init_target(cmd_ctx, target) != ERROR_OK)
-               {
-                       ERROR("target '%s' init failed", target->type->name);
-                       exit(-1);
-               }
-               
-               /* Set up default functions if none are provided by target */
-               if (target->type->virt2phys == NULL)
-               {
-                       target->type->virt2phys = default_virt2phys;
-               }
-               if (target->type->mmu == NULL)
-               {
-                       target->type->mmu = default_mmu;
-               }
-               target = target->next;
-       }
-       
-       if (targets)
-       {
-               target_register_user_commands(cmd_ctx);
-               target_register_timer_callback(handle_target, 100, 1, NULL);
-       }
-               
-       return ERROR_OK;
-}
-
-int target_init_reset(struct command_context_s *cmd_ctx)
-{
-       if (startup_mode == DAEMON_RESET)
-               target_process_reset(cmd_ctx);
-       
-       return ERROR_OK;
-}
-
-int target_register_event_callback(int (*callback)(struct target_s *target, enum target_event event, void *priv), void *priv)
-{
-       target_event_callback_t **callbacks_p = &target_event_callbacks;
-       
-       if (callback == NULL)
-       {
-               return ERROR_INVALID_ARGUMENTS;
-       }
-       
-       if (*callbacks_p)
-       {
-               while ((*callbacks_p)->next)
-                       callbacks_p = &((*callbacks_p)->next);
-               callbacks_p = &((*callbacks_p)->next);
-       }
-       
-       (*callbacks_p) = malloc(sizeof(target_event_callback_t));
-       (*callbacks_p)->callback = callback;
-       (*callbacks_p)->priv = priv;
-       (*callbacks_p)->next = NULL;
-       
-       return ERROR_OK;
-}
-
-int target_register_timer_callback(int (*callback)(void *priv), int time_ms, int periodic, void *priv)
-{
-       target_timer_callback_t **callbacks_p = &target_timer_callbacks;
-       struct timeval now;
-       
-       if (callback == NULL)
-       {
-               return ERROR_INVALID_ARGUMENTS;
-       }
-       
-       if (*callbacks_p)
-       {
-               while ((*callbacks_p)->next)
-                       callbacks_p = &((*callbacks_p)->next);
-               callbacks_p = &((*callbacks_p)->next);
-       }
-       
-       (*callbacks_p) = malloc(sizeof(target_timer_callback_t));
-       (*callbacks_p)->callback = callback;
-       (*callbacks_p)->periodic = periodic;
-       (*callbacks_p)->time_ms = time_ms;
-       
-       gettimeofday(&now, NULL);
-       (*callbacks_p)->when.tv_usec = now.tv_usec + (time_ms % 1000) * 1000;
-       time_ms -= (time_ms % 1000);
-       (*callbacks_p)->when.tv_sec = now.tv_sec + (time_ms / 1000);
-       if ((*callbacks_p)->when.tv_usec > 1000000)
-       {
-               (*callbacks_p)->when.tv_usec = (*callbacks_p)->when.tv_usec - 1000000;
-               (*callbacks_p)->when.tv_sec += 1;
-       }
-       
-       (*callbacks_p)->priv = priv;
-       (*callbacks_p)->next = NULL;
-       
-       return ERROR_OK;
-}
-
-int target_unregister_event_callback(int (*callback)(struct target_s *target, enum target_event event, void *priv), void *priv)
-{
-       target_event_callback_t **p = &target_event_callbacks;
-       target_event_callback_t *c = target_event_callbacks;
-       
-       if (callback == NULL)
-       {
-               return ERROR_INVALID_ARGUMENTS;
-       }
-               
-       while (c)
-       {
-               target_event_callback_t *next = c->next;
-               if ((c->callback == callback) && (c->priv == priv))
-               {
-                       *p = next;
-                       free(c);
-                       return ERROR_OK;
-               }
-               else
-                       p = &(c->next);
-               c = next;
-       }
-       
-       return ERROR_OK;
-}
-
-int target_unregister_timer_callback(int (*callback)(void *priv), void *priv)
-{
-       target_timer_callback_t **p = &target_timer_callbacks;
-       target_timer_callback_t *c = target_timer_callbacks;
-       
-       if (callback == NULL)
-       {
-               return ERROR_INVALID_ARGUMENTS;
-       }
-               
-       while (c)
-       {
-               target_timer_callback_t *next = c->next;
-               if ((c->callback == callback) && (c->priv == priv))
-               {
-                       *p = next;
-                       free(c);
-                       return ERROR_OK;
-               }
-               else
-                       p = &(c->next);
-               c = next;
-       }
-       
-       return ERROR_OK;
-}
-
-int target_call_event_callbacks(target_t *target, enum target_event event)
-{
-       target_event_callback_t *callback = target_event_callbacks;
-       target_event_callback_t *next_callback;
-       
-       DEBUG("target event %i", event);
-       
-       while (callback)
-       {
-               next_callback = callback->next;
-               callback->callback(target, event, callback->priv);
-               callback = next_callback;
-       }
-       
-       return ERROR_OK;
-}
-
-int target_call_timer_callbacks()
-{
-       target_timer_callback_t *callback = target_timer_callbacks;
-       target_timer_callback_t *next_callback;
-       struct timeval now;
-
-       gettimeofday(&now, NULL);
-       
-       while (callback)
-       {
-               next_callback = callback->next;
-               
-               if (((now.tv_sec >= callback->when.tv_sec) && (now.tv_usec >= callback->when.tv_usec))
-                       || (now.tv_sec > callback->when.tv_sec))
-               {
-                       callback->callback(callback->priv);
-                       if (callback->periodic)
-                       {
-                               int time_ms = callback->time_ms;
-                               callback->when.tv_usec = now.tv_usec + (time_ms % 1000) * 1000;
-                               time_ms -= (time_ms % 1000);
-                               callback->when.tv_sec = now.tv_sec + time_ms / 1000;
-                               if (callback->when.tv_usec > 1000000)
-                               {
-                                       callback->when.tv_usec = callback->when.tv_usec - 1000000;
-                                       callback->when.tv_sec += 1;
-                               }
-                       }
-                       else
-                               target_unregister_timer_callback(callback->callback, callback->priv);
-               }
-                       
-               callback = next_callback;
-       }
-       
-       return ERROR_OK;
-}
-
-int target_alloc_working_area(struct target_s *target, u32 size, working_area_t **area)
-{
-       working_area_t *c = target->working_areas;
-       working_area_t *new_wa = NULL;
-       
-       /* Reevaluate working area address based on MMU state*/
-       if (target->working_areas == NULL)
-       {
-               int retval;
-               int enabled;
-               retval = target->type->mmu(target, &enabled);
-               if (retval != ERROR_OK)
-               {
-                       return retval;
-               }
-               if (enabled)
-               {
-                       target->working_area = target->working_area_virt;
-               }
-               else
-               {
-                       target->working_area = target->working_area_phys;
-               }
-       }
-       
-       /* only allocate multiples of 4 byte */
-       if (size % 4)
-       {
-               ERROR("BUG: code tried to allocate unaligned number of bytes, padding");
-               size = CEIL(size, 4);
-       }
-       
-       /* see if there's already a matching working area */
-       while (c)
-       {
-               if ((c->free) && (c->size == size))
-               {
-                       new_wa = c;
-                       break;
-               }
-               c = c->next;
-       }
-       
-       /* if not, allocate a new one */
-       if (!new_wa)
-       {
-               working_area_t **p = &target->working_areas;
-               u32 first_free = target->working_area;
-               u32 free_size = target->working_area_size;
-               
-               DEBUG("allocating new working area");
-               
-               c = target->working_areas;
-               while (c)
-               {
-                       first_free += c->size;
-                       free_size -= c->size;
-                       p = &c->next;
-                       c = c->next;
-               }
-               
-               if (free_size < size)
-               {
-                       WARNING("not enough working area available(requested %d, free %d)", size, free_size);
-                       return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-               }
-               
-               new_wa = malloc(sizeof(working_area_t));
-               new_wa->next = NULL;
-               new_wa->size = size;
-               new_wa->address = first_free;
-               
-               if (target->backup_working_area)
-               {
-                       new_wa->backup = malloc(new_wa->size);
-                       target->type->read_memory(target, new_wa->address, 4, new_wa->size / 4, new_wa->backup);
-               }
-               else
-               {
-                       new_wa->backup = NULL;
-               }
-               
-               /* put new entry in list */
-               *p = new_wa;
-       }
-       
-       /* mark as used, and return the new (reused) area */
-       new_wa->free = 0;
-       *area = new_wa;
-       
-       /* user pointer */
-       new_wa->user = area;
-       
-       return ERROR_OK;
-}
-
-int target_free_working_area(struct target_s *target, working_area_t *area)
-{
-       if (area->free)
-               return ERROR_OK;
-       
-       if (target->backup_working_area)
-               target->type->write_memory(target, area->address, 4, area->size / 4, area->backup);
-       
-       area->free = 1;
-       
-       /* mark user pointer invalid */
-       *area->user = NULL;
-       area->user = NULL;
-       
-       return ERROR_OK;
-}
-
-int target_free_all_working_areas(struct target_s *target)
-{
-       working_area_t *c = target->working_areas;
-
-       while (c)
-       {
-               working_area_t *next = c->next;
-               target_free_working_area(target, c);
-               
-               if (c->backup)
-                       free(c->backup);
-               
-               free(c);
-               
-               c = next;
-       }
-       
-       target->working_areas = NULL;
-       
-       return ERROR_OK;
-}
-
-int target_register_commands(struct command_context_s *cmd_ctx)
-{
-       register_command(cmd_ctx, NULL, "target", handle_target_command, COMMAND_CONFIG, NULL);
-       register_command(cmd_ctx, NULL, "targets", handle_targets_command, COMMAND_EXEC, NULL);
-       register_command(cmd_ctx, NULL, "daemon_startup", handle_daemon_startup_command, COMMAND_CONFIG, NULL);
-       register_command(cmd_ctx, NULL, "target_script", handle_target_script_command, COMMAND_CONFIG, NULL);
-       register_command(cmd_ctx, NULL, "run_and_halt_time", handle_run_and_halt_time_command, COMMAND_CONFIG, NULL);
-       register_command(cmd_ctx, NULL, "working_area", handle_working_area_command, COMMAND_ANY, "working_area <target#> <address> <size> <'backup'|'nobackup'> [virtual address]");
-       register_command(cmd_ctx, NULL, "virt2phys", handle_virt2phys_command, COMMAND_ANY, "virt2phys <virtual address>");
-
-       return ERROR_OK;
-}
-
-int target_arch_state(struct target_s *target)
-{
-       int retval;
-       if (target==NULL)
-       {
-               USER("No target has been configured");
-               return ERROR_OK;
-       }
-       
-       USER("target state: %s", target_state_strings[target->state]);
-       
-       if (target->state!=TARGET_HALTED)
-               return ERROR_OK;
-       
-       retval=target->type->arch_state(target);
-       return retval;
-}
-
-/* Single aligned words are guaranteed to use 16 or 32 bit access 
- * mode respectively, otherwise data is handled as quickly as 
- * possible
- */
-int target_write_buffer(struct target_s *target, u32 address, u32 size, u8 *buffer)
-{
-       int retval;
-       
-       DEBUG("writing buffer of %i byte at 0x%8.8x", size, address);
-       
-       if (((address % 2) == 0) && (size == 2))
-       {
-               return target->type->write_memory(target, address, 2, 1, buffer);
-       }
-       
-       /* handle unaligned head bytes */
-       if (address % 4)
-       {
-               int unaligned = 4 - (address % 4);
-               
-               if (unaligned > size)
-                       unaligned = size;
-
-               if ((retval = target->type->write_memory(target, address, 1, unaligned, buffer)) != ERROR_OK)
-                       return retval;
-               
-               buffer += unaligned;
-               address += unaligned;
-               size -= unaligned;
-       }
-               
-       /* handle aligned words */
-       if (size >= 4)
-       {
-               int aligned = size - (size % 4);
-       
-               /* use bulk writes above a certain limit. This may have to be changed */
-               if (aligned > 128)
-               {
-                       if ((retval = target->type->bulk_write_memory(target, address, aligned / 4, buffer)) != ERROR_OK)
-                               return retval;
-               }
-               else
-               {
-                       if ((retval = target->type->write_memory(target, address, 4, aligned / 4, buffer)) != ERROR_OK)
-                               return retval;
-               }
-               
-               buffer += aligned;
-               address += aligned;
-               size -= aligned;
-       }
-       
-       /* handle tail writes of less than 4 bytes */
-       if (size > 0)
-       {
-               if ((retval = target->type->write_memory(target, address, 1, size, buffer)) != ERROR_OK)
-                       return retval;
-       }
-       
-       return ERROR_OK;
-}
-
-
-/* Single aligned words are guaranteed to use 16 or 32 bit access 
- * mode respectively, otherwise data is handled as quickly as 
- * possible
- */
-int target_read_buffer(struct target_s *target, u32 address, u32 size, u8 *buffer)
-{
-       int retval;
-       
-       DEBUG("reading buffer of %i byte at 0x%8.8x", size, address);
-       
-       if (((address % 2) == 0) && (size == 2))
-       {
-               return target->type->read_memory(target, address, 2, 1, buffer);
-       }
-       
-       /* handle unaligned head bytes */
-       if (address % 4)
-       {
-               int unaligned = 4 - (address % 4);
-               
-               if (unaligned > size)
-                       unaligned = size;
-
-               if ((retval = target->type->read_memory(target, address, 1, unaligned, buffer)) != ERROR_OK)
-                       return retval;
-               
-               buffer += unaligned;
-               address += unaligned;
-               size -= unaligned;
-       }
-               
-       /* handle aligned words */
-       if (size >= 4)
-       {
-               int aligned = size - (size % 4);
-       
-               if ((retval = target->type->read_memory(target, address, 4, aligned / 4, buffer)) != ERROR_OK)
-                       return retval;
-               
-               buffer += aligned;
-               address += aligned;
-               size -= aligned;
-       }
-       
-       /* handle tail writes of less than 4 bytes */
-       if (size > 0)
-       {
-               if ((retval = target->type->read_memory(target, address, 1, size, buffer)) != ERROR_OK)
-                       return retval;
-       }
-       
-       return ERROR_OK;
-}
-
-int target_checksum_memory(struct target_s *target, u32 address, u32 size, u32* crc)
-{
-       u8 *buffer;
-       int retval;
-       int i;
-       u32 checksum = 0;
-       
-       if ((retval = target->type->checksum_memory(target, address,
-               size, &checksum)) == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
-       {
-               buffer = malloc(size);
-               if (buffer == NULL)
-               {
-                       ERROR("error allocating buffer for section (%d bytes)", size);
-                       return ERROR_INVALID_ARGUMENTS;
-               }
-               retval = target_read_buffer(target, address, size, buffer);
-               if (retval != ERROR_OK)
-               {
-                       free(buffer);
-                       return retval;
-               }
-
-               /* convert to target endianess */
-               for (i = 0; i < (size/sizeof(u32)); i++)
-               {
-                       u32 target_data;
-                       target_data = target_buffer_get_u32(target, &buffer[i*sizeof(u32)]);
-                       target_buffer_set_u32(target, &buffer[i*sizeof(u32)], target_data);
-               }
-
-               retval = image_calculate_checksum( buffer, size, &checksum );
-               free(buffer);
-       }
-       
-       *crc = checksum;
-       
-       return retval;
-}
-
-int target_read_u32(struct target_s *target, u32 address, u32 *value)
-{
-       u8 value_buf[4];
-
-       int retval = target->type->read_memory(target, address, 4, 1, value_buf);
-       
-       if (retval == ERROR_OK)
-       {
-               *value = target_buffer_get_u32(target, value_buf);
-               DEBUG("address: 0x%8.8x, value: 0x%8.8x", address, *value);
-       }
-       else
-       {
-               *value = 0x0;
-               DEBUG("address: 0x%8.8x failed", address);
-       }
-       
-       return retval;
-}
-
-int target_read_u16(struct target_s *target, u32 address, u16 *value)
-{
-       u8 value_buf[2];
-       
-       int retval = target->type->read_memory(target, address, 2, 1, value_buf);
-       
-       if (retval == ERROR_OK)
-       {
-               *value = target_buffer_get_u16(target, value_buf);
-               DEBUG("address: 0x%8.8x, value: 0x%4.4x", address, *value);
-       }
-       else
-       {
-               *value = 0x0;
-               DEBUG("address: 0x%8.8x failed", address);
-       }
-       
-       return retval;
-}
-
-int target_read_u8(struct target_s *target, u32 address, u8 *value)
-{
-       int retval = target->type->read_memory(target, address, 1, 1, value);
-
-       if (retval == ERROR_OK)
-       {
-               DEBUG("address: 0x%8.8x, value: 0x%2.2x", address, *value);
-       }
-       else
-       {
-               *value = 0x0;
-               DEBUG("address: 0x%8.8x failed", address);
-       }
-       
-       return retval;
-}
-
-int target_write_u32(struct target_s *target, u32 address, u32 value)
-{
-       int retval;
-       u8 value_buf[4];
-
-       DEBUG("address: 0x%8.8x, value: 0x%8.8x", address, value);
-
-       target_buffer_set_u32(target, value_buf, value);        
-       if ((retval = target->type->write_memory(target, address, 4, 1, value_buf)) != ERROR_OK)
-       {
-               DEBUG("failed: %i", retval);
-       }
-       
-       return retval;
-}
-
-int target_write_u16(struct target_s *target, u32 address, u16 value)
-{
-       int retval;
-       u8 value_buf[2];
-       
-       DEBUG("address: 0x%8.8x, value: 0x%8.8x", address, value);
-
-       target_buffer_set_u16(target, value_buf, value);        
-       if ((retval = target->type->write_memory(target, address, 2, 1, value_buf)) != ERROR_OK)
-       {
-               DEBUG("failed: %i", retval);
-       }
-       
-       return retval;
-}
-
-int target_write_u8(struct target_s *target, u32 address, u8 value)
-{
-       int retval;
-       
-       DEBUG("address: 0x%8.8x, value: 0x%2.2x", address, value);
-
-       if ((retval = target->type->read_memory(target, address, 1, 1, &value)) != ERROR_OK)
-       {
-               DEBUG("failed: %i", retval);
-       }
-       
-       return retval;
-}
-
-int target_register_user_commands(struct command_context_s *cmd_ctx)
-{
-       register_command(cmd_ctx,  NULL, "reg", handle_reg_command, COMMAND_EXEC, NULL);
-       register_command(cmd_ctx,  NULL, "poll", handle_poll_command, COMMAND_EXEC, "poll target state");
-       register_command(cmd_ctx,  NULL, "wait_halt", handle_wait_halt_command, COMMAND_EXEC, "wait for target halt [time (s)]");
-       register_command(cmd_ctx,  NULL, "halt", handle_halt_command, COMMAND_EXEC, "halt target");
-       register_command(cmd_ctx,  NULL, "resume", handle_resume_command, COMMAND_EXEC, "resume target [addr]");
-       register_command(cmd_ctx,  NULL, "step", handle_step_command, COMMAND_EXEC, "step one instruction from current PC or [addr]");
-       register_command(cmd_ctx,  NULL, "reset", handle_reset_command, COMMAND_EXEC, "reset target [run|halt|init|run_and_halt|run_and_init]");
-       register_command(cmd_ctx,  NULL, "soft_reset_halt", handle_soft_reset_halt_command, COMMAND_EXEC, "halt the target and do a soft reset");
-
-       register_command(cmd_ctx,  NULL, "mdw", handle_md_command, COMMAND_EXEC, "display memory words <addr> [count]");
-       register_command(cmd_ctx,  NULL, "mdh", handle_md_command, COMMAND_EXEC, "display memory half-words <addr> [count]");
-       register_command(cmd_ctx,  NULL, "mdb", handle_md_command, COMMAND_EXEC, "display memory bytes <addr> [count]");
-       
-       register_command(cmd_ctx,  NULL, "mww", handle_mw_command, COMMAND_EXEC, "write memory word <addr> <value>");
-       register_command(cmd_ctx,  NULL, "mwh", handle_mw_command, COMMAND_EXEC, "write memory half-word <addr> <value>");
-       register_command(cmd_ctx,  NULL, "mwb", handle_mw_command, COMMAND_EXEC, "write memory byte <addr> <value>");
-       
-       register_command(cmd_ctx,  NULL, "bp", handle_bp_command, COMMAND_EXEC, "set breakpoint <address> <length> [hw]");      
-       register_command(cmd_ctx,  NULL, "rbp", handle_rbp_command, COMMAND_EXEC, "remove breakpoint <adress>");
-       register_command(cmd_ctx,  NULL, "wp", handle_wp_command, COMMAND_EXEC, "set watchpoint <address> <length> <r/w/a> [value] [mask]");    
-       register_command(cmd_ctx,  NULL, "rwp", handle_rwp_command, COMMAND_EXEC, "remove watchpoint <adress>");
-       
-       register_command(cmd_ctx,  NULL, "load_image", handle_load_image_command, COMMAND_EXEC, "load_image <file> <address> ['bin'|'ihex'|'elf'|'s19']");
-       register_command(cmd_ctx,  NULL, "dump_image", handle_dump_image_command, COMMAND_EXEC, "dump_image <file> <address> <size>");
-       register_command(cmd_ctx,  NULL, "verify_image", handle_verify_image_command, COMMAND_EXEC, "verify_image <file> [offset] [type]");
-       register_command(cmd_ctx,  NULL, "load_binary", handle_load_image_command, COMMAND_EXEC, "[DEPRECATED] load_binary <file> <address>");
-       register_command(cmd_ctx,  NULL, "dump_binary", handle_dump_image_command, COMMAND_EXEC, "[DEPRECATED] dump_binary <file> <address> <size>");
-       
-       target_request_register_commands(cmd_ctx);
-       trace_register_commands(cmd_ctx);
-       
-       return ERROR_OK;
-}
-
-int handle_targets_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = targets;
-       int count = 0;
-       
-       if (argc == 1)
-       {
-               int num = strtoul(args[0], NULL, 0);
-               
-               while (target)
-               {
-                       count++;
-                       target = target->next;
-               }
-               
-               if (num < count)
-                       cmd_ctx->current_target = num;
-               else
-                       command_print(cmd_ctx, "%i is out of bounds, only %i targets are configured", num, count);
-                       
-               return ERROR_OK;
-       }
-               
-       while (target)
-       {
-               command_print(cmd_ctx, "%i: %s (%s), state: %s", count++, target->type->name, target_endianess_strings[target->endianness], target_state_strings[target->state]);
-               target = target->next;
-       }
-       
-       return ERROR_OK;
-}
-
-int handle_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       int i;
-       int found = 0;
-       
-       if (argc < 3)
-       {
-               ERROR("target command requires at least three arguments: <type> <endianess> <reset_mode>");
-               exit(-1);
-       }
-       
-       /* search for the specified target */
-       if (args[0] && (args[0][0] != 0))
-       {
-               for (i = 0; target_types[i]; i++)
-               {
-                       if (strcmp(args[0], target_types[i]->name) == 0)
-                       {
-                               target_t **last_target_p = &targets;
-                               
-                               /* register target specific commands */
-                               if (target_types[i]->register_commands(cmd_ctx) != ERROR_OK)
-                               {
-                                       ERROR("couldn't register '%s' commands", args[0]);
-                                       exit(-1);
-                               }
-
-                               if (*last_target_p)
-                               {
-                                       while ((*last_target_p)->next)
-                                               last_target_p = &((*last_target_p)->next);
-                                       last_target_p = &((*last_target_p)->next);
-                               }
-
-                               *last_target_p = malloc(sizeof(target_t));
-                               
-                               (*last_target_p)->type = target_types[i];
-                               
-                               if (strcmp(args[1], "big") == 0)
-                                       (*last_target_p)->endianness = TARGET_BIG_ENDIAN;
-                               else if (strcmp(args[1], "little") == 0)
-                                       (*last_target_p)->endianness = TARGET_LITTLE_ENDIAN;
-                               else
-                               {
-                                       ERROR("endianness must be either 'little' or 'big', not '%s'", args[1]);
-                                       exit(-1);
-                               }
-                               
-                               /* what to do on a target reset */
-                               if (strcmp(args[2], "reset_halt") == 0)
-                                       (*last_target_p)->reset_mode = RESET_HALT;
-                               else if (strcmp(args[2], "reset_run") == 0)
-                                       (*last_target_p)->reset_mode = RESET_RUN;
-                               else if (strcmp(args[2], "reset_init") == 0)
-                                       (*last_target_p)->reset_mode = RESET_INIT;
-                               else if (strcmp(args[2], "run_and_halt") == 0)
-                                       (*last_target_p)->reset_mode = RESET_RUN_AND_HALT;
-                               else if (strcmp(args[2], "run_and_init") == 0)
-                                       (*last_target_p)->reset_mode = RESET_RUN_AND_INIT;
-                               else
-                               {
-                                       ERROR("unknown target startup mode %s", args[2]);
-                                       exit(-1);
-                               }
-                               (*last_target_p)->run_and_halt_time = 1000; /* default 1s */
-                               
-                               (*last_target_p)->reset_script = NULL;
-                               (*last_target_p)->post_halt_script = NULL;
-                               (*last_target_p)->pre_resume_script = NULL;
-                               (*last_target_p)->gdb_program_script = NULL;
-                               
-                               (*last_target_p)->working_area = 0x0;
-                               (*last_target_p)->working_area_size = 0x0;
-                               (*last_target_p)->working_areas = NULL;
-                               (*last_target_p)->backup_working_area = 0;
-                               
-                               (*last_target_p)->state = TARGET_UNKNOWN;
-                               (*last_target_p)->reg_cache = NULL;
-                               (*last_target_p)->breakpoints = NULL;
-                               (*last_target_p)->watchpoints = NULL;
-                               (*last_target_p)->next = NULL;
-                               (*last_target_p)->arch_info = NULL;
-                               
-                               /* initialize trace information */
-                               (*last_target_p)->trace_info = malloc(sizeof(trace_t));
-                               (*last_target_p)->trace_info->num_trace_points = 0;
-                               (*last_target_p)->trace_info->trace_points_size = 0;
-                               (*last_target_p)->trace_info->trace_points = NULL;
-                               (*last_target_p)->trace_info->trace_history_size = 0;
-                               (*last_target_p)->trace_info->trace_history = NULL;
-                               (*last_target_p)->trace_info->trace_history_pos = 0;
-                               (*last_target_p)->trace_info->trace_history_overflowed = 0;
-                               
-                               (*last_target_p)->dbgmsg = NULL;
-                               (*last_target_p)->dbg_msg_enabled = 0;
-                                                               
-                               (*last_target_p)->type->target_command(cmd_ctx, cmd, args, argc, *last_target_p);
-                               
-                               found = 1;
-                               break;
-                       }
-               }
-       }
-       
-       /* no matching target found */
-       if (!found)
-       {
-               ERROR("target '%s' not found", args[0]);
-               exit(-1);
-       }
-
-       return ERROR_OK;
-}
-
-/* usage: target_script <target#> <event> <script_file> */
-int handle_target_script_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = NULL;
-       
-       if (argc < 3)
-       {
-               ERROR("incomplete target_script command");
-               exit(-1);
-       }
-       
-       target = get_target_by_num(strtoul(args[0], NULL, 0));
-       
-       if (!target)
-       {
-               ERROR("target number '%s' not defined", args[0]);
-               exit(-1);
-       }
-       
-       if (strcmp(args[1], "reset") == 0)
-       {
-               if (target->reset_script)
-                       free(target->reset_script);
-               target->reset_script = strdup(args[2]);
-       }
-       else if (strcmp(args[1], "post_halt") == 0)
-       {
-               if (target->post_halt_script)
-                       free(target->post_halt_script);
-               target->post_halt_script = strdup(args[2]);
-       }
-       else if (strcmp(args[1], "pre_resume") == 0)
-       {
-               if (target->pre_resume_script)
-                       free(target->pre_resume_script);
-               target->pre_resume_script = strdup(args[2]);
-       }
-       else if (strcmp(args[1], "gdb_program_config") == 0)
-       {
-               if (target->gdb_program_script)
-                       free(target->gdb_program_script);
-               target->gdb_program_script = strdup(args[2]);
-       }
-       else
-       {
-               ERROR("unknown event type: '%s", args[1]);
-               exit(-1);       
-       }
-       
-       return ERROR_OK;
-}
-
-int handle_run_and_halt_time_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = NULL;
-       
-       if (argc < 2)
-       {
-               ERROR("incomplete run_and_halt_time command");
-               exit(-1);
-       }
-       
-       target = get_target_by_num(strtoul(args[0], NULL, 0));
-       
-       if (!target)
-       {
-               ERROR("target number '%s' not defined", args[0]);
-               exit(-1);
-       }
-       
-       target->run_and_halt_time = strtoul(args[1], NULL, 0);
-       
-       return ERROR_OK;
-}
-
-int handle_working_area_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = NULL;
-       
-       if ((argc < 4) || (argc > 5))
-       {
-               return ERROR_COMMAND_SYNTAX_ERROR;
-       }
-       
-       target = get_target_by_num(strtoul(args[0], NULL, 0));
-       
-       if (!target)
-       {
-               ERROR("target number '%s' not defined", args[0]);
-               exit(-1);
-       }
-       target_free_all_working_areas(target);
-       
-       target->working_area_phys = target->working_area_virt = strtoul(args[1], NULL, 0);
-       if (argc == 5)
-       {
-               target->working_area_virt = strtoul(args[4], NULL, 0);
-       }
-       target->working_area_size = strtoul(args[2], NULL, 0);
-       
-       if (strcmp(args[3], "backup") == 0)
-       {
-               target->backup_working_area = 1;
-       }
-       else if (strcmp(args[3], "nobackup") == 0)
-       {
-               target->backup_working_area = 0;
-       }
-       else
-       {
-               ERROR("unrecognized <backup|nobackup> argument (%s)", args[3]);
-               return ERROR_COMMAND_SYNTAX_ERROR;
-       }
-       
-       return ERROR_OK;
-}
-
-
-/* process target state changes */
-int handle_target(void *priv)
-{
-       int retval;
-       target_t *target = targets;
-       
-       while (target)
-       {
-               /* only poll if target isn't already halted */
-               if (target->state != TARGET_HALTED)
-               {
-                       if (target_continous_poll)
-                               if ((retval = target->type->poll(target)) != ERROR_OK)
-                               {
-                                       ERROR("couldn't poll target(%d). It's due for a reset.", retval);
-                               }
-               }
-       
-               target = target->next;
-       }
-       
-       return ERROR_OK;
-}
-
-int handle_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target;
-       reg_t *reg = NULL;
-       int count = 0;
-       char *value;
-       
-       DEBUG("-");
-       
-       target = get_current_target(cmd_ctx);
-       
-       /* list all available registers for the current target */
-       if (argc == 0)
-       {
-               reg_cache_t *cache = target->reg_cache;
-               
-               count = 0;
-               while(cache)
-               {
-                       int i;
-                       for (i = 0; i < cache->num_regs; i++)
-                       {
-                               value = buf_to_str(cache->reg_list[i].value, cache->reg_list[i].size, 16);
-                               command_print(cmd_ctx, "(%i) %s (/%i): 0x%s (dirty: %i, valid: %i)", count++, cache->reg_list[i].name, cache->reg_list[i].size, value, cache->reg_list[i].dirty, cache->reg_list[i].valid);
-                               free(value);
-                       }
-                       cache = cache->next;
-               }
-               
-               return ERROR_OK;
-       }
-       
-       /* access a single register by its ordinal number */
-       if ((args[0][0] >= '0') && (args[0][0] <= '9'))
-       {
-               int num = strtoul(args[0], NULL, 0);
-               reg_cache_t *cache = target->reg_cache;
-               
-               count = 0;
-               while(cache)
-               {
-                       int i;
-                       for (i = 0; i < cache->num_regs; i++)
-                       {
-                               if (count++ == num)
-                               {
-                                       reg = &cache->reg_list[i];
-                                       break;
-                               }
-                       }
-                       if (reg)
-                               break;
-                       cache = cache->next;
-               }
-               
-               if (!reg)
-               {
-                       command_print(cmd_ctx, "%i is out of bounds, the current target has only %i registers (0 - %i)", num, count, count - 1);
-                       return ERROR_OK;
-               }
-       } else /* access a single register by its name */
-       {
-               reg = register_get_by_name(target->reg_cache, args[0], 1);
-               
-               if (!reg)
-               {
-                       command_print(cmd_ctx, "register %s not found in current target", args[0]);
-                       return ERROR_OK;
-               }
-       }
-
-       /* display a register */
-       if ((argc == 1) || ((argc == 2) && !((args[1][0] >= '0') && (args[1][0] <= '9'))))
-       {
-               if ((argc == 2) && (strcmp(args[1], "force") == 0))
-                       reg->valid = 0;
-               
-               if (reg->valid == 0)
-               {
-                       reg_arch_type_t *arch_type = register_get_arch_type(reg->arch_type);
-                       if (arch_type == NULL)
-                       {
-                               ERROR("BUG: encountered unregistered arch type");
-                               return ERROR_OK;
-                       }
-                       arch_type->get(reg);
-               }
-               value = buf_to_str(reg->value, reg->size, 16);
-               command_print(cmd_ctx, "%s (/%i): 0x%s", reg->name, reg->size, value);
-               free(value);
-               return ERROR_OK;
-       }
-       
-       /* set register value */
-       if (argc == 2)
-       {
-               u8 *buf = malloc(CEIL(reg->size, 8));
-               str_to_buf(args[1], strlen(args[1]), buf, reg->size, 0);
-
-               reg_arch_type_t *arch_type = register_get_arch_type(reg->arch_type);
-               if (arch_type == NULL)
-               {
-                       ERROR("BUG: encountered unregistered arch type");
-                       return ERROR_OK;
-               }
-               
-               arch_type->set(reg, buf);
-               
-               value = buf_to_str(reg->value, reg->size, 16);
-               command_print(cmd_ctx, "%s (/%i): 0x%s", reg->name, reg->size, value);
-               free(value);
-               
-               free(buf);
-               
-               return ERROR_OK;
-       }
-       
-       command_print(cmd_ctx, "usage: reg <#|name> [value]");
-       
-       return ERROR_OK;
-}
-
-static int wait_state(struct command_context_s *cmd_ctx, char *cmd, enum target_state state, int ms);
-
-int handle_poll_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-
-       if (argc == 0)
-       {
-               target->type->poll(target);
-                       target_arch_state(target);
-       }
-       else
-       {
-               if (strcmp(args[0], "on") == 0)
-               {
-                       target_continous_poll = 1;
-               }
-               else if (strcmp(args[0], "off") == 0)
-               {
-                       target_continous_poll = 0;
-               }
-               else
-               {
-                       command_print(cmd_ctx, "arg is \"on\" or \"off\"");
-               }
-       }
-       
-       
-       return ERROR_OK;
-}
-
-int handle_wait_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       int ms = 5000;
-       
-       if (argc > 0)
-       {
-               char *end;
-
-               ms = strtoul(args[0], &end, 0) * 1000;
-               if (*end)
-               {
-                       command_print(cmd_ctx, "usage: %s [seconds]", cmd);
-                       return ERROR_OK;
-               }
-       }
-
-       return wait_state(cmd_ctx, cmd, TARGET_HALTED, ms); 
-}
-
-static void target_process_events(struct command_context_s *cmd_ctx)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       target->type->poll(target);
-       target_call_timer_callbacks();
-}
-
-static int wait_state(struct command_context_s *cmd_ctx, char *cmd, enum target_state state, int ms)
-{
-       int retval;
-       struct timeval timeout, now;
-       
-       gettimeofday(&timeout, NULL);
-       timeval_add_time(&timeout, 0, ms * 1000);
-       
-       target_t *target = get_current_target(cmd_ctx);
-       for (;;)
-       {
-               if ((retval=target->type->poll(target))!=ERROR_OK)
-                       return retval;
-               target_call_timer_callbacks();
-               if (target->state == state)
-               {
-                       break;
-               }
-               command_print(cmd_ctx, "waiting for target %s...", target_state_strings[state]);
-               
-               gettimeofday(&now, NULL);
-               if ((now.tv_sec > timeout.tv_sec) || ((now.tv_sec == timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec)))
-               {
-                       command_print(cmd_ctx, "timed out while waiting for target %s", target_state_strings[state]);
-                       ERROR("timed out while waiting for target %s", target_state_strings[state]);
-                       break;
-               }
-       }
-       
-       return ERROR_OK;
-}
-
-int handle_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       int retval;
-       target_t *target = get_current_target(cmd_ctx);
-
-       DEBUG("-");
-       
-       command_print(cmd_ctx, "requesting target halt...");
-
-       if ((retval = target->type->halt(target)) != ERROR_OK)
-       {       
-               switch (retval)
-               {
-                       case ERROR_TARGET_ALREADY_HALTED:
-                               command_print(cmd_ctx, "target already halted");
-                               break;
-                       case ERROR_TARGET_TIMEOUT:
-                               command_print(cmd_ctx, "target timed out... shutting down");
-                               return retval;
-                       default:
-                               command_print(cmd_ctx, "unknown error... shutting down");
-                               return retval;
-               }
-       }
-       
-       return handle_wait_halt_command(cmd_ctx, cmd, args, argc);
-}
-
-/* what to do on daemon startup */
-int handle_daemon_startup_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       if (argc == 1)
-       {
-               if (strcmp(args[0], "attach") == 0)
-               {
-                       startup_mode = DAEMON_ATTACH;
-                       return ERROR_OK;
-               }
-               else if (strcmp(args[0], "reset") == 0)
-               {
-                       startup_mode = DAEMON_RESET;
-                       return ERROR_OK;
-               }
-       }
-       
-       WARNING("invalid daemon_startup configuration directive: %s", args[0]);
-       return ERROR_OK;
-
-}
-               
-int handle_soft_reset_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       int retval;
-       
-       command_print(cmd_ctx, "requesting target halt and executing a soft reset");
-       
-       if ((retval = target->type->soft_reset_halt(target)) != ERROR_OK)
-       {       
-               switch (retval)
-               {
-                       case ERROR_TARGET_TIMEOUT:
-                               command_print(cmd_ctx, "target timed out... shutting down");
-                               exit(-1);
-                       default:
-                               command_print(cmd_ctx, "unknown error... shutting down");
-                               exit(-1);
-               }
-       }
-       
-       return ERROR_OK;
-}
-
-int handle_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       enum target_reset_mode reset_mode = target->reset_mode;
-       enum target_reset_mode save = target->reset_mode;
-       
-       DEBUG("-");
-       
-       if (argc >= 1)
-       {
-               if (strcmp("run", args[0]) == 0)
-                       reset_mode = RESET_RUN;
-               else if (strcmp("halt", args[0]) == 0)
-                       reset_mode = RESET_HALT;
-               else if (strcmp("init", args[0]) == 0)
-                       reset_mode = RESET_INIT;
-               else if (strcmp("run_and_halt", args[0]) == 0)
-               {
-                       reset_mode = RESET_RUN_AND_HALT;
-                       if (argc >= 2)
-                       {
-                               target->run_and_halt_time = strtoul(args[1], NULL, 0);
-                       }
-               }
-               else if (strcmp("run_and_init", args[0]) == 0)
-               {
-                       reset_mode = RESET_RUN_AND_INIT;
-                       if (argc >= 2)
-                       {
-                               target->run_and_halt_time = strtoul(args[1], NULL, 0);
-                       }
-               }
-               else
-               {
-                       command_print(cmd_ctx, "usage: reset ['run', 'halt', 'init', 'run_and_halt', 'run_and_init]");
-                       return ERROR_OK;
-               }
-       }
-       
-       /* temporarily modify mode of current reset target */
-       target->reset_mode = reset_mode;
-
-       /* reset *all* targets */
-       target_process_reset(cmd_ctx);
-       
-       /* Restore default reset mode for this target */
-    target->reset_mode = save;
-       
-       return ERROR_OK;
-}
-
-int handle_resume_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       int retval;
-       target_t *target = get_current_target(cmd_ctx);
-       
-       if (argc == 0)
-               retval = target->type->resume(target, 1, 0, 1, 0); /* current pc, addr = 0, handle breakpoints, not debugging */
-       else if (argc == 1)
-               retval = target->type->resume(target, 0, strtoul(args[0], NULL, 0), 1, 0); /* addr = args[0], handle breakpoints, not debugging */
-       else
-       {
-               return ERROR_COMMAND_SYNTAX_ERROR;
-       }
-
-       target_process_events(cmd_ctx);
-       
-       target_arch_state(target);
-       
-       return retval;
-}
-
-int handle_step_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       
-       DEBUG("-");
-       
-       if (argc == 0)
-               target->type->step(target, 1, 0, 1); /* current pc, addr = 0, handle breakpoints */
-
-       if (argc == 1)
-               target->type->step(target, 0, strtoul(args[0], NULL, 0), 1); /* addr = args[0], handle breakpoints */
-       
-       return ERROR_OK;
-}
-
-int handle_md_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       const int line_bytecnt = 32;
-       int count = 1;
-       int size = 4;
-       u32 address = 0;
-       int line_modulo;
-       int i;
-
-       char output[128];
-       int output_len;
-
-       int retval;
-
-       u8 *buffer;
-       target_t *target = get_current_target(cmd_ctx);
-
-       if (argc < 1)
-               return ERROR_OK;
-
-       if (argc == 2)
-               count = strtoul(args[1], NULL, 0);
-
-       address = strtoul(args[0], NULL, 0);
-       
-
-       switch (cmd[2])
-       {
-               case 'w':
-                       size = 4; line_modulo = line_bytecnt / 4;
-                       break;
-               case 'h':
-                       size = 2; line_modulo = line_bytecnt / 2;
-                       break;
-               case 'b':
-                       size = 1; line_modulo = line_bytecnt / 1;
-                       break;
-               default:
-                       return ERROR_OK;
-       }
-
-       buffer = calloc(count, size);
-       retval  = target->type->read_memory(target, address, size, count, buffer);
-       if (retval != ERROR_OK)
-       {
-               switch (retval)
-               {
-                       case ERROR_TARGET_UNALIGNED_ACCESS:
-                               command_print(cmd_ctx, "error: address not aligned");
-                               break;
-                       case ERROR_TARGET_NOT_HALTED:
-                               command_print(cmd_ctx, "error: target must be halted for memory accesses");
-                               break;                  
-                       case ERROR_TARGET_DATA_ABORT:
-                               command_print(cmd_ctx, "error: access caused data abort, system possibly corrupted");
-                               break;
-                       default:
-                               command_print(cmd_ctx, "error: unknown error");
-                               break;
-               }
-               return ERROR_OK;
-       }
-
-       output_len = 0;
-
-       for (i = 0; i < count; i++)
-       {
-               if (i%line_modulo == 0)
-                       output_len += snprintf(output + output_len, 128 - output_len, "0x%8.8x: ", address + (i*size));
-               
-               switch (size)
-               {
-                       case 4:
-                               output_len += snprintf(output + output_len, 128 - output_len, "%8.8x ", target_buffer_get_u32(target, &buffer[i*4]));
-                               break;
-                       case 2:
-                               output_len += snprintf(output + output_len, 128 - output_len, "%4.4x ", target_buffer_get_u16(target, &buffer[i*2]));
-                               break;
-                       case 1:
-                               output_len += snprintf(output + output_len, 128 - output_len, "%2.2x ", buffer[i*1]);
-                               break;
-               }
-
-               if ((i%line_modulo == line_modulo-1) || (i == count - 1))
-               {
-                       command_print(cmd_ctx, output);
-                       output_len = 0;
-               }
-       }
-
-       free(buffer);
-       
-       return ERROR_OK;
-}
-
-int handle_mw_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       u32 address = 0;
-       u32 value = 0;
-       int retval;
-       target_t *target = get_current_target(cmd_ctx);
-       u8 value_buf[4];
-
-       if (argc < 2)
-               return ERROR_OK;
-
-       address = strtoul(args[0], NULL, 0);
-       value = strtoul(args[1], NULL, 0);
-
-       switch (cmd[2])
-       {
-               case 'w':
-                       target_buffer_set_u32(target, value_buf, value);
-                       retval = target->type->write_memory(target, address, 4, 1, value_buf);
-                       break;
-               case 'h':
-                       target_buffer_set_u16(target, value_buf, value);
-                       retval = target->type->write_memory(target, address, 2, 1, value_buf);
-                       break;
-               case 'b':
-                       value_buf[0] = value;
-                       retval = target->type->write_memory(target, address, 1, 1, value_buf);
-                       break;
-               default:
-                       return ERROR_OK;
-       }
-
-       switch (retval)
-       {
-               case ERROR_TARGET_UNALIGNED_ACCESS:
-                       command_print(cmd_ctx, "error: address not aligned");
-                       break;
-               case ERROR_TARGET_DATA_ABORT:
-                       command_print(cmd_ctx, "error: access caused data abort, system possibly corrupted");
-                       break;
-               case ERROR_TARGET_NOT_HALTED:
-                       command_print(cmd_ctx, "error: target must be halted for memory accesses");
-                       break;
-               case ERROR_OK:
-                       break;
-               default:
-                       command_print(cmd_ctx, "error: unknown error");
-                       break;
-       }
-
-       return ERROR_OK;
-
-}
-
-int handle_load_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       u8 *buffer;
-       u32 buf_cnt;
-       u32 image_size;
-       int i;
-       int retval;
-
-       image_t image;  
-       
-       duration_t duration;
-       char *duration_text;
-       
-       target_t *target = get_current_target(cmd_ctx);
-
-       if (argc < 1)
-       {
-               command_print(cmd_ctx, "usage: load_image <filename> [address] [type]");
-               return ERROR_OK;
-       }
-       
-       /* a base address isn't always necessary, default to 0x0 (i.e. don't relocate) */
-       if (argc >= 2)
-       {
-               image.base_address_set = 1;
-               image.base_address = strtoul(args[1], NULL, 0);
-       }
-       else
-       {
-               image.base_address_set = 0;
-       }
-       
-       image.start_address_set = 0;
-
-       duration_start_measure(&duration);
-       
-       if (image_open(&image, args[0], (argc >= 3) ? args[2] : NULL) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "load_image error: %s", image.error_str);
-               return ERROR_OK;
-       }
-       
-       image_size = 0x0;
-       for (i = 0; i < image.num_sections; i++)
-       {
-               buffer = malloc(image.sections[i].size);
-               if (buffer == NULL)
-               {
-                       command_print(cmd_ctx, "error allocating buffer for section (%d bytes)", image.sections[i].size);
-                       break;
-               }
-               
-               if ((retval = image_read_section(&image, i, 0x0, image.sections[i].size, buffer, &buf_cnt)) != ERROR_OK)
-               {
-                       ERROR("image_read_section failed with error code: %i", retval);
-                       command_print(cmd_ctx, "image reading failed, download aborted");
-                       free(buffer);
-                       image_close(&image);
-                       return ERROR_OK;
-               }
-               target_write_buffer(target, image.sections[i].base_address, buf_cnt, buffer);
-               image_size += buf_cnt;
-               command_print(cmd_ctx, "%u byte written at address 0x%8.8x", buf_cnt, image.sections[i].base_address);
-               
-               free(buffer);
-       }
-
-       duration_stop_measure(&duration, &duration_text);
-       command_print(cmd_ctx, "downloaded %u byte in %s", image_size, duration_text);
-       free(duration_text);
-       
-       image_close(&image);
-
-       return ERROR_OK;
-
-}
-
-int handle_dump_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       fileio_t fileio;
-       
-       u32 address;
-       u32 size;
-       u8 buffer[560];
-       int retval;
-       
-       duration_t duration;
-       char *duration_text;
-       
-       target_t *target = get_current_target(cmd_ctx);
-
-       if (argc != 3)
-       {
-               command_print(cmd_ctx, "usage: dump_image <filename> <address> <size>");
-               return ERROR_OK;
-       }
-
-       address = strtoul(args[1], NULL, 0);
-       size = strtoul(args[2], NULL, 0);
-
-       if ((address & 3) || (size & 3))
-       {
-               command_print(cmd_ctx, "only 32-bit aligned address and size are supported");
-               return ERROR_OK;
-       }
-       
-       if (fileio_open(&fileio, args[0], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "dump_image error: %s", fileio.error_str);
-               return ERROR_OK;
-       }
-       
-       duration_start_measure(&duration);
-       
-       while (size > 0)
-       {
-               u32 size_written;
-               u32 this_run_size = (size > 560) ? 560 : size;
-               
-               retval = target->type->read_memory(target, address, 4, this_run_size / 4, buffer);
-               if (retval != ERROR_OK)
-               {
-                       command_print(cmd_ctx, "Reading memory failed %d", retval);
-                       break;
-               }
-               
-               fileio_write(&fileio, this_run_size, buffer, &size_written);
-               
-               size -= this_run_size;
-               address += this_run_size;
-       }
-
-       fileio_close(&fileio);
-
-       duration_stop_measure(&duration, &duration_text);
-       command_print(cmd_ctx, "dumped %"PRIi64" byte in %s", fileio.size, duration_text);
-       free(duration_text);
-       
-       return ERROR_OK;
-}
-
-int handle_verify_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       u8 *buffer;
-       u32 buf_cnt;
-       u32 image_size;
-       int i;
-       int retval;
-       u32 checksum = 0;
-       u32 mem_checksum = 0;
-
-       image_t image;  
-       
-       duration_t duration;
-       char *duration_text;
-       
-       target_t *target = get_current_target(cmd_ctx);
-       
-       if (argc < 1)
-       {
-               command_print(cmd_ctx, "usage: verify_image <file> [offset] [type]");
-               return ERROR_OK;
-       }
-       
-       if (!target)
-       {
-               ERROR("no target selected");
-               return ERROR_OK;
-       }
-       
-       duration_start_measure(&duration);
-       
-       if (argc >= 2)
-       {
-               image.base_address_set = 1;
-               image.base_address = strtoul(args[1], NULL, 0);
-       }
-       else
-       {
-               image.base_address_set = 0;
-               image.base_address = 0x0;
-       }
-
-       image.start_address_set = 0;
-
-       if (image_open(&image, args[0], (argc == 3) ? args[2] : NULL) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "verify_image error: %s", image.error_str);
-               return ERROR_OK;
-       }
-       
-       image_size = 0x0;
-       for (i = 0; i < image.num_sections; i++)
-       {
-               buffer = malloc(image.sections[i].size);
-               if (buffer == NULL)
-               {
-                       command_print(cmd_ctx, "error allocating buffer for section (%d bytes)", image.sections[i].size);
-                       break;
-               }
-               if ((retval = image_read_section(&image, i, 0x0, image.sections[i].size, buffer, &buf_cnt)) != ERROR_OK)
-               {
-                       ERROR("image_read_section failed with error code: %i", retval);
-                       command_print(cmd_ctx, "image reading failed, verify aborted");
-                       free(buffer);
-                       image_close(&image);
-                       return ERROR_OK;
-               }
-               
-               /* calculate checksum of image */
-               image_calculate_checksum( buffer, buf_cnt, &checksum );
-               
-               retval = target_checksum_memory(target, image.sections[i].base_address, buf_cnt, &mem_checksum);
-               
-               if( retval != ERROR_OK )
-               {
-                       command_print(cmd_ctx, "could not calculate checksum, verify aborted");
-                       free(buffer);
-                       image_close(&image);
-                       return ERROR_OK;
-               }
-               
-               if( checksum != mem_checksum )
-               {
-                       /* failed crc checksum, fall back to a binary compare */
-                       u8 *data;
-                       
-                       command_print(cmd_ctx, "checksum mismatch - attempting binary compare");
-                       
-                       data = (u8*)malloc(buf_cnt);
-                       
-                       /* Can we use 32bit word accesses? */
-                       int size = 1;
-                       int count = buf_cnt;
-                       if ((count % 4) == 0)
-                       {
-                               size *= 4;
-                               count /= 4;
-                       }
-                       retval = target->type->read_memory(target, image.sections[i].base_address, size, count, data);
-       
-                       if (retval == ERROR_OK)
-                       {
-                               int t;
-                               for (t = 0; t < buf_cnt; t++)
-                               {
-                                       if (data[t] != buffer[t])
-                                       {
-                                               command_print(cmd_ctx, "Verify operation failed address 0x%08x. Was 0x%02x instead of 0x%02x\n", t + image.sections[i].base_address, data[t], buffer[t]);
-                                               free(data);
-                                               free(buffer);
-                                               image_close(&image);
-                                               return ERROR_OK;
-                                       }
-                               }
-                       }
-                       
-                       free(data);
-               }
-               
-               free(buffer);
-               image_size += buf_cnt;
-       }
-       
-       duration_stop_measure(&duration, &duration_text);
-       command_print(cmd_ctx, "verified %u bytes in %s", image_size, duration_text);
-       free(duration_text);
-       
-       image_close(&image);
-       
-       return ERROR_OK;
-}
-
-int handle_bp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       int retval;
-       target_t *target = get_current_target(cmd_ctx);
-
-       if (argc == 0)
-       {
-               breakpoint_t *breakpoint = target->breakpoints;
-
-               while (breakpoint)
-               {
-                       if (breakpoint->type == BKPT_SOFT)
-                       {
-                               char* buf = buf_to_str(breakpoint->orig_instr, breakpoint->length, 16);
-                               command_print(cmd_ctx, "0x%8.8x, 0x%x, %i, 0x%s", breakpoint->address, breakpoint->length, breakpoint->set, buf);
-                               free(buf);
-                       }
-                       else
-                       {
-                               command_print(cmd_ctx, "0x%8.8x, 0x%x, %i", breakpoint->address, breakpoint->length, breakpoint->set);
-                       }
-                       breakpoint = breakpoint->next;
-               }
-       }
-       else if (argc >= 2)
-       {
-               int hw = BKPT_SOFT;
-               u32 length = 0;
-
-               length = strtoul(args[1], NULL, 0);
-               
-               if (argc >= 3)
-                       if (strcmp(args[2], "hw") == 0)
-                               hw = BKPT_HARD;
-
-               if ((retval = breakpoint_add(target, strtoul(args[0], NULL, 0), length, hw)) != ERROR_OK)
-               {
-                       switch (retval)
-                       {
-                               case ERROR_TARGET_NOT_HALTED:
-                                       command_print(cmd_ctx, "target must be halted to set breakpoints");
-                                       break;
-                               case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:
-                                       command_print(cmd_ctx, "no more breakpoints available");
-                                       break;
-                               default:
-                                       command_print(cmd_ctx, "unknown error, breakpoint not set");
-                                       break;
-                       }
-               }
-               else
-               {
-                       command_print(cmd_ctx, "breakpoint added at address 0x%8.8x", strtoul(args[0], NULL, 0));
-               }
-       }
-       else
-       {
-               command_print(cmd_ctx, "usage: bp <address> <length> ['hw']");
-       }
-
-       return ERROR_OK;
-}
-
-int handle_rbp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-
-       if (argc > 0)
-               breakpoint_remove(target, strtoul(args[0], NULL, 0));
-
-       return ERROR_OK;
-}
-
-int handle_wp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       int retval;
-
-       if (argc == 0)
-       {
-               watchpoint_t *watchpoint = target->watchpoints;
-
-               while (watchpoint)
-               {
-                       command_print(cmd_ctx, "address: 0x%8.8x, mask: 0x%8.8x, r/w/a: %i, value: 0x%8.8x, mask: 0x%8.8x", watchpoint->address, watchpoint->length, watchpoint->rw, watchpoint->value, watchpoint->mask);
-                       watchpoint = watchpoint->next;
-               }
-       } 
-       else if (argc >= 2)
-       {
-               enum watchpoint_rw type = WPT_ACCESS;
-               u32 data_value = 0x0;
-               u32 data_mask = 0xffffffff;
-               
-               if (argc >= 3)
-               {
-                       switch(args[2][0])
-                       {
-                               case 'r':
-                                       type = WPT_READ;
-                                       break;
-                               case 'w':
-                                       type = WPT_WRITE;
-                                       break;
-                               case 'a':
-                                       type = WPT_ACCESS;
-                                       break;
-                               default:
-                                       command_print(cmd_ctx, "usage: wp <address> <length> [r/w/a] [value] [mask]");
-                                       return ERROR_OK;
-                       }
-               }
-               if (argc >= 4)
-               {
-                       data_value = strtoul(args[3], NULL, 0);
-               }
-               if (argc >= 5)
-               {
-                       data_mask = strtoul(args[4], NULL, 0);
-               }
-               
-               if ((retval = watchpoint_add(target, strtoul(args[0], NULL, 0),
-                               strtoul(args[1], NULL, 0), type, data_value, data_mask)) != ERROR_OK)
-               {
-                       switch (retval)
-                       {
-                               case ERROR_TARGET_NOT_HALTED:
-                                       command_print(cmd_ctx, "target must be halted to set watchpoints");
-                                       break;
-                               case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:
-                                       command_print(cmd_ctx, "no more watchpoints available");
-                                       break;
-                               default:
-                                       command_print(cmd_ctx, "unknown error, watchpoint not set");
-                                       break;
-                       }       
-               }
-       }
-       else
-       {
-               command_print(cmd_ctx, "usage: wp <address> <length> [r/w/a] [value] [mask]");
-       }
-               
-       return ERROR_OK;
-}
-
-int handle_rwp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-
-       if (argc > 0)
-               watchpoint_remove(target, strtoul(args[0], NULL, 0));
-       
-       return ERROR_OK;
-}
-
-int handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
-{
-       int retval;
-       target_t *target = get_current_target(cmd_ctx);
-       u32 va;
-       u32 pa;
-
-       if (argc != 1)
-       {
-               return ERROR_COMMAND_SYNTAX_ERROR;
-       }
-       va = strtoul(args[0], NULL, 0);
-
-       retval = target->type->virt2phys(target, va, &pa);
-       if (retval == ERROR_OK)
-       {
-               command_print(cmd_ctx, "Physical address 0x%08x", pa);
-       }
-       else
-       {
-               /* lower levels will have logged a detailed error which is 
-                * forwarded to telnet/GDB session.  
-                */
-       }
-       return retval;
-}
+/***************************************************************************\r
+ *   Copyright (C) 2005 by Dominic Rath                                    *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "replacements.h"\r
+#include "target.h"\r
+#include "target_request.h"\r
+\r
+#include "log.h"\r
+#include "configuration.h"\r
+#include "binarybuffer.h"\r
+#include "jtag.h"\r
+\r
+#include <string.h>\r
+#include <stdlib.h>\r
+#include <inttypes.h>\r
+\r
+#include <sys/types.h>\r
+#include <sys/stat.h>\r
+#include <unistd.h>\r
+#include <errno.h>\r
+\r
+#include <sys/time.h>\r
+#include <time.h>\r
+\r
+#include <time_support.h>\r
+\r
+#include <fileio.h>\r
+#include <image.h>\r
+\r
+int cli_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv);\r
+\r
+\r
+int handle_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_daemon_startup_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_targets_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+int handle_target_script_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_run_and_halt_time_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_working_area_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+int handle_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_poll_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_wait_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_soft_reset_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_resume_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_step_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_md_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_mw_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_load_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_dump_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_verify_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_bp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_rbp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_wp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_rwp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);\r
+int handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc);\r
+\r
+/* targets\r
+ */\r
+extern target_type_t arm7tdmi_target;\r
+extern target_type_t arm720t_target;\r
+extern target_type_t arm9tdmi_target;\r
+extern target_type_t arm920t_target;\r
+extern target_type_t arm966e_target;\r
+extern target_type_t arm926ejs_target;\r
+extern target_type_t feroceon_target;\r
+extern target_type_t xscale_target;\r
+extern target_type_t cortexm3_target;\r
+\r
+target_type_t *target_types[] =\r
+{\r
+       &arm7tdmi_target,\r
+       &arm9tdmi_target,\r
+       &arm920t_target,\r
+       &arm720t_target,\r
+       &arm966e_target,\r
+       &arm926ejs_target,\r
+       &feroceon_target,\r
+       &xscale_target,\r
+       &cortexm3_target,\r
+       NULL,\r
+};\r
+\r
+target_t *targets = NULL;\r
+target_event_callback_t *target_event_callbacks = NULL;\r
+target_timer_callback_t *target_timer_callbacks = NULL;\r
+\r
+char *target_state_strings[] =\r
+{\r
+       "unknown",\r
+       "running",\r
+       "halted",\r
+       "reset",\r
+       "debug_running",\r
+};\r
+\r
+char *target_debug_reason_strings[] =\r
+{\r
+       "debug request", "breakpoint", "watchpoint",\r
+       "watchpoint and breakpoint", "single step",\r
+       "target not halted"\r
+};\r
+\r
+char *target_endianess_strings[] =\r
+{\r
+       "big endian",\r
+       "little endian",\r
+};\r
+\r
+enum daemon_startup_mode startup_mode = DAEMON_ATTACH;\r
+\r
+static int target_continous_poll = 1;\r
+\r
+/* read a u32 from a buffer in target memory endianness */\r
+u32 target_buffer_get_u32(target_t *target, u8 *buffer)\r
+{\r
+       if (target->endianness == TARGET_LITTLE_ENDIAN)\r
+               return le_to_h_u32(buffer);\r
+       else\r
+               return be_to_h_u32(buffer);\r
+}\r
+\r
+/* read a u16 from a buffer in target memory endianness */\r
+u16 target_buffer_get_u16(target_t *target, u8 *buffer)\r
+{\r
+       if (target->endianness == TARGET_LITTLE_ENDIAN)\r
+               return le_to_h_u16(buffer);\r
+       else\r
+               return be_to_h_u16(buffer);\r
+}\r
+\r
+/* write a u32 to a buffer in target memory endianness */\r
+void target_buffer_set_u32(target_t *target, u8 *buffer, u32 value)\r
+{\r
+       if (target->endianness == TARGET_LITTLE_ENDIAN)\r
+               h_u32_to_le(buffer, value);\r
+       else\r
+               h_u32_to_be(buffer, value);\r
+}\r
+\r
+/* write a u16 to a buffer in target memory endianness */\r
+void target_buffer_set_u16(target_t *target, u8 *buffer, u16 value)\r
+{\r
+       if (target->endianness == TARGET_LITTLE_ENDIAN)\r
+               h_u16_to_le(buffer, value);\r
+       else\r
+               h_u16_to_be(buffer, value);\r
+}\r
+\r
+/* returns a pointer to the n-th configured target */\r
+target_t* get_target_by_num(int num)\r
+{\r
+       target_t *target = targets;\r
+       int i = 0;\r
+\r
+       while (target)\r
+       {\r
+               if (num == i)\r
+                       return target;\r
+               target = target->next;\r
+               i++;\r
+       }\r
+\r
+       return NULL;\r
+}\r
+\r
+int get_num_by_target(target_t *query_target)\r
+{\r
+       target_t *target = targets;\r
+       int i = 0;      \r
+       \r
+       while (target)\r
+       {\r
+               if (target == query_target)\r
+                       return i;\r
+               target = target->next;\r
+               i++;\r
+       }\r
+       \r
+       return -1;\r
+}\r
+\r
+target_t* get_current_target(command_context_t *cmd_ctx)\r
+{\r
+       target_t *target = get_target_by_num(cmd_ctx->current_target);\r
+       \r
+       if (target == NULL)\r
+       {\r
+               ERROR("BUG: current_target out of bounds");\r
+               exit(-1);\r
+       }\r
+       \r
+       return target;\r
+}\r
+\r
+/* Process target initialization, when target entered debug out of reset\r
+ * the handler is unregistered at the end of this function, so it's only called once\r
+ */\r
+int target_init_handler(struct target_s *target, enum target_event event, void *priv)\r
+{\r
+       FILE *script;\r
+       struct command_context_s *cmd_ctx = priv;\r
+       \r
+       if ((event == TARGET_EVENT_HALTED) && (target->reset_script))\r
+       {\r
+               target_unregister_event_callback(target_init_handler, priv);\r
+\r
+               script = open_file_from_path(cmd_ctx, target->reset_script, "r");\r
+               if (!script)\r
+               {\r
+                       ERROR("couldn't open script file %s", target->reset_script);\r
+                               return ERROR_OK;\r
+               }\r
+\r
+               INFO("executing reset script '%s'", target->reset_script);\r
+               command_run_file(cmd_ctx, script, COMMAND_EXEC);\r
+               fclose(script);\r
+\r
+               jtag_execute_queue();\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int target_run_and_halt_handler(void *priv)\r
+{\r
+       target_t *target = priv;\r
+       \r
+       target->type->halt(target);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int target_process_reset(struct command_context_s *cmd_ctx)\r
+{\r
+       int retval = ERROR_OK;\r
+       target_t *target;\r
+       struct timeval timeout, now;\r
+       \r
+       /* prepare reset_halt where necessary */\r
+       target = targets;\r
+       while (target)\r
+       {\r
+               if (jtag_reset_config & RESET_SRST_PULLS_TRST)\r
+               {\r
+                       switch (target->reset_mode)\r
+                       {\r
+                               case RESET_HALT:\r
+                                       command_print(cmd_ctx, "nSRST pulls nTRST, falling back to RESET_RUN_AND_HALT");\r
+                                       target->reset_mode = RESET_RUN_AND_HALT;\r
+                                       break;\r
+                               case RESET_INIT:\r
+                                       command_print(cmd_ctx, "nSRST pulls nTRST, falling back to RESET_RUN_AND_INIT");\r
+                                       target->reset_mode = RESET_RUN_AND_INIT;\r
+                                       break;\r
+                               default:\r
+                                       break;\r
+                       } \r
+               }\r
+               switch (target->reset_mode)\r
+               {\r
+                       case RESET_HALT:\r
+                       case RESET_INIT:\r
+                               target->type->prepare_reset_halt(target);\r
+                               break;\r
+                       default:\r
+                               break;\r
+               }\r
+               target = target->next;\r
+       }\r
+       \r
+       target = targets;\r
+       while (target)\r
+       {\r
+               target->type->assert_reset(target);\r
+               target = target->next;\r
+       }\r
+       jtag_execute_queue();\r
+       \r
+       /* request target halt if necessary, and schedule further action */\r
+       target = targets;\r
+       while (target)\r
+       {\r
+               switch (target->reset_mode)\r
+               {\r
+                       case RESET_RUN:\r
+                               /* nothing to do if target just wants to be run */\r
+                               break;\r
+                       case RESET_RUN_AND_HALT:\r
+                               /* schedule halt */\r
+                               target_register_timer_callback(target_run_and_halt_handler, target->run_and_halt_time, 0, target);\r
+                               break;\r
+                       case RESET_RUN_AND_INIT:\r
+                               /* schedule halt */\r
+                               target_register_timer_callback(target_run_and_halt_handler, target->run_and_halt_time, 0, target);\r
+                               target_register_event_callback(target_init_handler, cmd_ctx);\r
+                               break;\r
+                       case RESET_HALT:\r
+                               target->type->halt(target);\r
+                               break;\r
+                       case RESET_INIT:\r
+                               target->type->halt(target);\r
+                               target_register_event_callback(target_init_handler, cmd_ctx);\r
+                               break;\r
+                       default:\r
+                               ERROR("BUG: unknown target->reset_mode");\r
+               }\r
+               target = target->next;\r
+       }\r
+       \r
+       target = targets;\r
+       while (target)\r
+       {\r
+               target->type->deassert_reset(target);\r
+               target = target->next;\r
+       }\r
+       jtag_execute_queue();\r
+\r
+       /* Wait for reset to complete, maximum 5 seconds. */    \r
+       gettimeofday(&timeout, NULL);\r
+       timeval_add_time(&timeout, 5, 0);\r
+       for(;;)\r
+       {\r
+               gettimeofday(&now, NULL);\r
+               \r
+               target_call_timer_callbacks();\r
+               \r
+               target = targets;\r
+               while (target)\r
+               {\r
+                       target->type->poll(target);\r
+                       if ((target->reset_mode == RESET_RUN_AND_INIT) || (target->reset_mode == RESET_RUN_AND_HALT))\r
+                       {\r
+                               if (target->state != TARGET_HALTED)\r
+                               {\r
+                                       if ((now.tv_sec > timeout.tv_sec) || ((now.tv_sec == timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec)))\r
+                                       {\r
+                                               command_print(cmd_ctx, "Timed out waiting for reset");\r
+                                               goto done;\r
+                                       }\r
+                                       usleep(100*1000); /* Do not eat all cpu */\r
+                                       goto again;\r
+                               }\r
+                       }\r
+                       target = target->next;\r
+               }\r
+               /* All targets we're waiting for are halted */\r
+               break;\r
+               \r
+               again:;\r
+       }\r
+       done:\r
+       \r
+       \r
+       /* We want any events to be processed before the prompt */\r
+       target_call_timer_callbacks();\r
+       \r
+       return retval;\r
+}\r
+\r
+static int default_virt2phys(struct target_s *target, u32 virtual, u32 *physical)\r
+{\r
+       *physical = virtual;\r
+       return ERROR_OK;\r
+}\r
+\r
+static int default_mmu(struct target_s *target, int *enabled)\r
+{\r
+       *enabled = 0;\r
+       return ERROR_OK;\r
+}\r
+\r
+int target_init(struct command_context_s *cmd_ctx)\r
+{\r
+       target_t *target = targets;\r
+       \r
+       while (target)\r
+       {\r
+               if (target->type->init_target(cmd_ctx, target) != ERROR_OK)\r
+               {\r
+                       ERROR("target '%s' init failed", target->type->name);\r
+                       exit(-1);\r
+               }\r
+               \r
+               /* Set up default functions if none are provided by target */\r
+               if (target->type->virt2phys == NULL)\r
+               {\r
+                       target->type->virt2phys = default_virt2phys;\r
+               }\r
+               if (target->type->mmu == NULL)\r
+               {\r
+                       target->type->mmu = default_mmu;\r
+               }\r
+               target = target->next;\r
+       }\r
+       \r
+       if (targets)\r
+       {\r
+               target_register_user_commands(cmd_ctx);\r
+               target_register_timer_callback(handle_target, 100, 1, NULL);\r
+       }\r
+               \r
+       return ERROR_OK;\r
+}\r
+\r
+int target_init_reset(struct command_context_s *cmd_ctx)\r
+{\r
+       if (startup_mode == DAEMON_RESET)\r
+               target_process_reset(cmd_ctx);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int target_register_event_callback(int (*callback)(struct target_s *target, enum target_event event, void *priv), void *priv)\r
+{\r
+       target_event_callback_t **callbacks_p = &target_event_callbacks;\r
+       \r
+       if (callback == NULL)\r
+       {\r
+               return ERROR_INVALID_ARGUMENTS;\r
+       }\r
+       \r
+       if (*callbacks_p)\r
+       {\r
+               while ((*callbacks_p)->next)\r
+                       callbacks_p = &((*callbacks_p)->next);\r
+               callbacks_p = &((*callbacks_p)->next);\r
+       }\r
+       \r
+       (*callbacks_p) = malloc(sizeof(target_event_callback_t));\r
+       (*callbacks_p)->callback = callback;\r
+       (*callbacks_p)->priv = priv;\r
+       (*callbacks_p)->next = NULL;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int target_register_timer_callback(int (*callback)(void *priv), int time_ms, int periodic, void *priv)\r
+{\r
+       target_timer_callback_t **callbacks_p = &target_timer_callbacks;\r
+       struct timeval now;\r
+       \r
+       if (callback == NULL)\r
+       {\r
+               return ERROR_INVALID_ARGUMENTS;\r
+       }\r
+       \r
+       if (*callbacks_p)\r
+       {\r
+               while ((*callbacks_p)->next)\r
+                       callbacks_p = &((*callbacks_p)->next);\r
+               callbacks_p = &((*callbacks_p)->next);\r
+       }\r
+       \r
+       (*callbacks_p) = malloc(sizeof(target_timer_callback_t));\r
+       (*callbacks_p)->callback = callback;\r
+       (*callbacks_p)->periodic = periodic;\r
+       (*callbacks_p)->time_ms = time_ms;\r
+       \r
+       gettimeofday(&now, NULL);\r
+       (*callbacks_p)->when.tv_usec = now.tv_usec + (time_ms % 1000) * 1000;\r
+       time_ms -= (time_ms % 1000);\r
+       (*callbacks_p)->when.tv_sec = now.tv_sec + (time_ms / 1000);\r
+       if ((*callbacks_p)->when.tv_usec > 1000000)\r
+       {\r
+               (*callbacks_p)->when.tv_usec = (*callbacks_p)->when.tv_usec - 1000000;\r
+               (*callbacks_p)->when.tv_sec += 1;\r
+       }\r
+       \r
+       (*callbacks_p)->priv = priv;\r
+       (*callbacks_p)->next = NULL;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int target_unregister_event_callback(int (*callback)(struct target_s *target, enum target_event event, void *priv), void *priv)\r
+{\r
+       target_event_callback_t **p = &target_event_callbacks;\r
+       target_event_callback_t *c = target_event_callbacks;\r
+       \r
+       if (callback == NULL)\r
+       {\r
+               return ERROR_INVALID_ARGUMENTS;\r
+       }\r
+               \r
+       while (c)\r
+       {\r
+               target_event_callback_t *next = c->next;\r
+               if ((c->callback == callback) && (c->priv == priv))\r
+               {\r
+                       *p = next;\r
+                       free(c);\r
+                       return ERROR_OK;\r
+               }\r
+               else\r
+                       p = &(c->next);\r
+               c = next;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int target_unregister_timer_callback(int (*callback)(void *priv), void *priv)\r
+{\r
+       target_timer_callback_t **p = &target_timer_callbacks;\r
+       target_timer_callback_t *c = target_timer_callbacks;\r
+       \r
+       if (callback == NULL)\r
+       {\r
+               return ERROR_INVALID_ARGUMENTS;\r
+       }\r
+               \r
+       while (c)\r
+       {\r
+               target_timer_callback_t *next = c->next;\r
+               if ((c->callback == callback) && (c->priv == priv))\r
+               {\r
+                       *p = next;\r
+                       free(c);\r
+                       return ERROR_OK;\r
+               }\r
+               else\r
+                       p = &(c->next);\r
+               c = next;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int target_call_event_callbacks(target_t *target, enum target_event event)\r
+{\r
+       target_event_callback_t *callback = target_event_callbacks;\r
+       target_event_callback_t *next_callback;\r
+       \r
+       DEBUG("target event %i", event);\r
+       \r
+       while (callback)\r
+       {\r
+               next_callback = callback->next;\r
+               callback->callback(target, event, callback->priv);\r
+               callback = next_callback;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int target_call_timer_callbacks()\r
+{\r
+       target_timer_callback_t *callback = target_timer_callbacks;\r
+       target_timer_callback_t *next_callback;\r
+       struct timeval now;\r
+\r
+       gettimeofday(&now, NULL);\r
+       \r
+       while (callback)\r
+       {\r
+               next_callback = callback->next;\r
+               \r
+               if (((now.tv_sec >= callback->when.tv_sec) && (now.tv_usec >= callback->when.tv_usec))\r
+                       || (now.tv_sec > callback->when.tv_sec))\r
+               {\r
+                       callback->callback(callback->priv);\r
+                       if (callback->periodic)\r
+                       {\r
+                               int time_ms = callback->time_ms;\r
+                               callback->when.tv_usec = now.tv_usec + (time_ms % 1000) * 1000;\r
+                               time_ms -= (time_ms % 1000);\r
+                               callback->when.tv_sec = now.tv_sec + time_ms / 1000;\r
+                               if (callback->when.tv_usec > 1000000)\r
+                               {\r
+                                       callback->when.tv_usec = callback->when.tv_usec - 1000000;\r
+                                       callback->when.tv_sec += 1;\r
+                               }\r
+                       }\r
+                       else\r
+                               target_unregister_timer_callback(callback->callback, callback->priv);\r
+               }\r
+                       \r
+               callback = next_callback;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int target_alloc_working_area(struct target_s *target, u32 size, working_area_t **area)\r
+{\r
+       working_area_t *c = target->working_areas;\r
+       working_area_t *new_wa = NULL;\r
+       \r
+       /* Reevaluate working area address based on MMU state*/\r
+       if (target->working_areas == NULL)\r
+       {\r
+               int retval;\r
+               int enabled;\r
+               retval = target->type->mmu(target, &enabled);\r
+               if (retval != ERROR_OK)\r
+               {\r
+                       return retval;\r
+               }\r
+               if (enabled)\r
+               {\r
+                       target->working_area = target->working_area_virt;\r
+               }\r
+               else\r
+               {\r
+                       target->working_area = target->working_area_phys;\r
+               }\r
+       }\r
+       \r
+       /* only allocate multiples of 4 byte */\r
+       if (size % 4)\r
+       {\r
+               ERROR("BUG: code tried to allocate unaligned number of bytes, padding");\r
+               size = CEIL(size, 4);\r
+       }\r
+       \r
+       /* see if there's already a matching working area */\r
+       while (c)\r
+       {\r
+               if ((c->free) && (c->size == size))\r
+               {\r
+                       new_wa = c;\r
+                       break;\r
+               }\r
+               c = c->next;\r
+       }\r
+       \r
+       /* if not, allocate a new one */\r
+       if (!new_wa)\r
+       {\r
+               working_area_t **p = &target->working_areas;\r
+               u32 first_free = target->working_area;\r
+               u32 free_size = target->working_area_size;\r
+               \r
+               DEBUG("allocating new working area");\r
+               \r
+               c = target->working_areas;\r
+               while (c)\r
+               {\r
+                       first_free += c->size;\r
+                       free_size -= c->size;\r
+                       p = &c->next;\r
+                       c = c->next;\r
+               }\r
+               \r
+               if (free_size < size)\r
+               {\r
+                       WARNING("not enough working area available(requested %d, free %d)", size, free_size);\r
+                       return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;\r
+               }\r
+               \r
+               new_wa = malloc(sizeof(working_area_t));\r
+               new_wa->next = NULL;\r
+               new_wa->size = size;\r
+               new_wa->address = first_free;\r
+               \r
+               if (target->backup_working_area)\r
+               {\r
+                       new_wa->backup = malloc(new_wa->size);\r
+                       target->type->read_memory(target, new_wa->address, 4, new_wa->size / 4, new_wa->backup);\r
+               }\r
+               else\r
+               {\r
+                       new_wa->backup = NULL;\r
+               }\r
+               \r
+               /* put new entry in list */\r
+               *p = new_wa;\r
+       }\r
+       \r
+       /* mark as used, and return the new (reused) area */\r
+       new_wa->free = 0;\r
+       *area = new_wa;\r
+       \r
+       /* user pointer */\r
+       new_wa->user = area;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int target_free_working_area(struct target_s *target, working_area_t *area)\r
+{\r
+       if (area->free)\r
+               return ERROR_OK;\r
+       \r
+       if (target->backup_working_area)\r
+               target->type->write_memory(target, area->address, 4, area->size / 4, area->backup);\r
+       \r
+       area->free = 1;\r
+       \r
+       /* mark user pointer invalid */\r
+       *area->user = NULL;\r
+       area->user = NULL;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int target_free_all_working_areas(struct target_s *target)\r
+{\r
+       working_area_t *c = target->working_areas;\r
+\r
+       while (c)\r
+       {\r
+               working_area_t *next = c->next;\r
+               target_free_working_area(target, c);\r
+               \r
+               if (c->backup)\r
+                       free(c->backup);\r
+               \r
+               free(c);\r
+               \r
+               c = next;\r
+       }\r
+       \r
+       target->working_areas = NULL;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int target_register_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       register_command(cmd_ctx, NULL, "target", handle_target_command, COMMAND_CONFIG, NULL);\r
+       register_command(cmd_ctx, NULL, "targets", handle_targets_command, COMMAND_EXEC, NULL);\r
+       register_command(cmd_ctx, NULL, "daemon_startup", handle_daemon_startup_command, COMMAND_CONFIG, NULL);\r
+       register_command(cmd_ctx, NULL, "target_script", handle_target_script_command, COMMAND_CONFIG, NULL);\r
+       register_command(cmd_ctx, NULL, "run_and_halt_time", handle_run_and_halt_time_command, COMMAND_CONFIG, NULL);\r
+       register_command(cmd_ctx, NULL, "working_area", handle_working_area_command, COMMAND_ANY, "working_area <target#> <address> <size> <'backup'|'nobackup'> [virtual address]");\r
+       register_command(cmd_ctx, NULL, "virt2phys", handle_virt2phys_command, COMMAND_ANY, "virt2phys <virtual address>");\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int target_arch_state(struct target_s *target)\r
+{\r
+       int retval;\r
+       if (target==NULL)\r
+       {\r
+               USER("No target has been configured");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       USER("target state: %s", target_state_strings[target->state]);\r
+       \r
+       if (target->state!=TARGET_HALTED)\r
+               return ERROR_OK;\r
+       \r
+       retval=target->type->arch_state(target);\r
+       return retval;\r
+}\r
+\r
+/* Single aligned words are guaranteed to use 16 or 32 bit access \r
+ * mode respectively, otherwise data is handled as quickly as \r
+ * possible\r
+ */\r
+int target_write_buffer(struct target_s *target, u32 address, u32 size, u8 *buffer)\r
+{\r
+       int retval;\r
+       \r
+       DEBUG("writing buffer of %i byte at 0x%8.8x", size, address);\r
+       \r
+       if (((address % 2) == 0) && (size == 2))\r
+       {\r
+               return target->type->write_memory(target, address, 2, 1, buffer);\r
+       }\r
+       \r
+       /* handle unaligned head bytes */\r
+       if (address % 4)\r
+       {\r
+               int unaligned = 4 - (address % 4);\r
+               \r
+               if (unaligned > size)\r
+                       unaligned = size;\r
+\r
+               if ((retval = target->type->write_memory(target, address, 1, unaligned, buffer)) != ERROR_OK)\r
+                       return retval;\r
+               \r
+               buffer += unaligned;\r
+               address += unaligned;\r
+               size -= unaligned;\r
+       }\r
+               \r
+       /* handle aligned words */\r
+       if (size >= 4)\r
+       {\r
+               int aligned = size - (size % 4);\r
+       \r
+               /* use bulk writes above a certain limit. This may have to be changed */\r
+               if (aligned > 128)\r
+               {\r
+                       if ((retval = target->type->bulk_write_memory(target, address, aligned / 4, buffer)) != ERROR_OK)\r
+                               return retval;\r
+               }\r
+               else\r
+               {\r
+                       if ((retval = target->type->write_memory(target, address, 4, aligned / 4, buffer)) != ERROR_OK)\r
+                               return retval;\r
+               }\r
+               \r
+               buffer += aligned;\r
+               address += aligned;\r
+               size -= aligned;\r
+       }\r
+       \r
+       /* handle tail writes of less than 4 bytes */\r
+       if (size > 0)\r
+       {\r
+               if ((retval = target->type->write_memory(target, address, 1, size, buffer)) != ERROR_OK)\r
+                       return retval;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+\r
+/* Single aligned words are guaranteed to use 16 or 32 bit access \r
+ * mode respectively, otherwise data is handled as quickly as \r
+ * possible\r
+ */\r
+int target_read_buffer(struct target_s *target, u32 address, u32 size, u8 *buffer)\r
+{\r
+       int retval;\r
+       \r
+       DEBUG("reading buffer of %i byte at 0x%8.8x", size, address);\r
+       \r
+       if (((address % 2) == 0) && (size == 2))\r
+       {\r
+               return target->type->read_memory(target, address, 2, 1, buffer);\r
+       }\r
+       \r
+       /* handle unaligned head bytes */\r
+       if (address % 4)\r
+       {\r
+               int unaligned = 4 - (address % 4);\r
+               \r
+               if (unaligned > size)\r
+                       unaligned = size;\r
+\r
+               if ((retval = target->type->read_memory(target, address, 1, unaligned, buffer)) != ERROR_OK)\r
+                       return retval;\r
+               \r
+               buffer += unaligned;\r
+               address += unaligned;\r
+               size -= unaligned;\r
+       }\r
+               \r
+       /* handle aligned words */\r
+       if (size >= 4)\r
+       {\r
+               int aligned = size - (size % 4);\r
+       \r
+               if ((retval = target->type->read_memory(target, address, 4, aligned / 4, buffer)) != ERROR_OK)\r
+                       return retval;\r
+               \r
+               buffer += aligned;\r
+               address += aligned;\r
+               size -= aligned;\r
+       }\r
+       \r
+       /* handle tail writes of less than 4 bytes */\r
+       if (size > 0)\r
+       {\r
+               if ((retval = target->type->read_memory(target, address, 1, size, buffer)) != ERROR_OK)\r
+                       return retval;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int target_checksum_memory(struct target_s *target, u32 address, u32 size, u32* crc)\r
+{\r
+       u8 *buffer;\r
+       int retval;\r
+       int i;\r
+       u32 checksum = 0;\r
+       \r
+       if ((retval = target->type->checksum_memory(target, address,\r
+               size, &checksum)) == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)\r
+       {\r
+               buffer = malloc(size);\r
+               if (buffer == NULL)\r
+               {\r
+                       ERROR("error allocating buffer for section (%d bytes)", size);\r
+                       return ERROR_INVALID_ARGUMENTS;\r
+               }\r
+               retval = target_read_buffer(target, address, size, buffer);\r
+               if (retval != ERROR_OK)\r
+               {\r
+                       free(buffer);\r
+                       return retval;\r
+               }\r
+\r
+               /* convert to target endianess */\r
+               for (i = 0; i < (size/sizeof(u32)); i++)\r
+               {\r
+                       u32 target_data;\r
+                       target_data = target_buffer_get_u32(target, &buffer[i*sizeof(u32)]);\r
+                       target_buffer_set_u32(target, &buffer[i*sizeof(u32)], target_data);\r
+               }\r
+\r
+               retval = image_calculate_checksum( buffer, size, &checksum );\r
+               free(buffer);\r
+       }\r
+       \r
+       *crc = checksum;\r
+       \r
+       return retval;\r
+}\r
+\r
+int target_read_u32(struct target_s *target, u32 address, u32 *value)\r
+{\r
+       u8 value_buf[4];\r
+\r
+       int retval = target->type->read_memory(target, address, 4, 1, value_buf);\r
+       \r
+       if (retval == ERROR_OK)\r
+       {\r
+               *value = target_buffer_get_u32(target, value_buf);\r
+               DEBUG("address: 0x%8.8x, value: 0x%8.8x", address, *value);\r
+       }\r
+       else\r
+       {\r
+               *value = 0x0;\r
+               DEBUG("address: 0x%8.8x failed", address);\r
+       }\r
+       \r
+       return retval;\r
+}\r
+\r
+int target_read_u16(struct target_s *target, u32 address, u16 *value)\r
+{\r
+       u8 value_buf[2];\r
+       \r
+       int retval = target->type->read_memory(target, address, 2, 1, value_buf);\r
+       \r
+       if (retval == ERROR_OK)\r
+       {\r
+               *value = target_buffer_get_u16(target, value_buf);\r
+               DEBUG("address: 0x%8.8x, value: 0x%4.4x", address, *value);\r
+       }\r
+       else\r
+       {\r
+               *value = 0x0;\r
+               DEBUG("address: 0x%8.8x failed", address);\r
+       }\r
+       \r
+       return retval;\r
+}\r
+\r
+int target_read_u8(struct target_s *target, u32 address, u8 *value)\r
+{\r
+       int retval = target->type->read_memory(target, address, 1, 1, value);\r
+\r
+       if (retval == ERROR_OK)\r
+       {\r
+               DEBUG("address: 0x%8.8x, value: 0x%2.2x", address, *value);\r
+       }\r
+       else\r
+       {\r
+               *value = 0x0;\r
+               DEBUG("address: 0x%8.8x failed", address);\r
+       }\r
+       \r
+       return retval;\r
+}\r
+\r
+int target_write_u32(struct target_s *target, u32 address, u32 value)\r
+{\r
+       int retval;\r
+       u8 value_buf[4];\r
+\r
+       DEBUG("address: 0x%8.8x, value: 0x%8.8x", address, value);\r
+\r
+       target_buffer_set_u32(target, value_buf, value);        \r
+       if ((retval = target->type->write_memory(target, address, 4, 1, value_buf)) != ERROR_OK)\r
+       {\r
+               DEBUG("failed: %i", retval);\r
+       }\r
+       \r
+       return retval;\r
+}\r
+\r
+int target_write_u16(struct target_s *target, u32 address, u16 value)\r
+{\r
+       int retval;\r
+       u8 value_buf[2];\r
+       \r
+       DEBUG("address: 0x%8.8x, value: 0x%8.8x", address, value);\r
+\r
+       target_buffer_set_u16(target, value_buf, value);        \r
+       if ((retval = target->type->write_memory(target, address, 2, 1, value_buf)) != ERROR_OK)\r
+       {\r
+               DEBUG("failed: %i", retval);\r
+       }\r
+       \r
+       return retval;\r
+}\r
+\r
+int target_write_u8(struct target_s *target, u32 address, u8 value)\r
+{\r
+       int retval;\r
+       \r
+       DEBUG("address: 0x%8.8x, value: 0x%2.2x", address, value);\r
+\r
+       if ((retval = target->type->read_memory(target, address, 1, 1, &value)) != ERROR_OK)\r
+       {\r
+               DEBUG("failed: %i", retval);\r
+       }\r
+       \r
+       return retval;\r
+}\r
+\r
+int target_register_user_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       register_command(cmd_ctx,  NULL, "reg", handle_reg_command, COMMAND_EXEC, NULL);\r
+       register_command(cmd_ctx,  NULL, "poll", handle_poll_command, COMMAND_EXEC, "poll target state");\r
+       register_command(cmd_ctx,  NULL, "wait_halt", handle_wait_halt_command, COMMAND_EXEC, "wait for target halt [time (s)]");\r
+       register_command(cmd_ctx,  NULL, "halt", handle_halt_command, COMMAND_EXEC, "halt target");\r
+       register_command(cmd_ctx,  NULL, "resume", handle_resume_command, COMMAND_EXEC, "resume target [addr]");\r
+       register_command(cmd_ctx,  NULL, "step", handle_step_command, COMMAND_EXEC, "step one instruction from current PC or [addr]");\r
+       register_command(cmd_ctx,  NULL, "reset", handle_reset_command, COMMAND_EXEC, "reset target [run|halt|init|run_and_halt|run_and_init]");\r
+       register_command(cmd_ctx,  NULL, "soft_reset_halt", handle_soft_reset_halt_command, COMMAND_EXEC, "halt the target and do a soft reset");\r
+\r
+       register_command(cmd_ctx,  NULL, "mdw", handle_md_command, COMMAND_EXEC, "display memory words <addr> [count]");\r
+       register_command(cmd_ctx,  NULL, "mdh", handle_md_command, COMMAND_EXEC, "display memory half-words <addr> [count]");\r
+       register_command(cmd_ctx,  NULL, "mdb", handle_md_command, COMMAND_EXEC, "display memory bytes <addr> [count]");\r
+       \r
+       register_command(cmd_ctx,  NULL, "mww", handle_mw_command, COMMAND_EXEC, "write memory word <addr> <value>");\r
+       register_command(cmd_ctx,  NULL, "mwh", handle_mw_command, COMMAND_EXEC, "write memory half-word <addr> <value>");\r
+       register_command(cmd_ctx,  NULL, "mwb", handle_mw_command, COMMAND_EXEC, "write memory byte <addr> <value>");\r
+       \r
+       register_command(cmd_ctx,  NULL, "bp", handle_bp_command, COMMAND_EXEC, "set breakpoint <address> <length> [hw]");      \r
+       register_command(cmd_ctx,  NULL, "rbp", handle_rbp_command, COMMAND_EXEC, "remove breakpoint <adress>");\r
+       register_command(cmd_ctx,  NULL, "wp", handle_wp_command, COMMAND_EXEC, "set watchpoint <address> <length> <r/w/a> [value] [mask]");    \r
+       register_command(cmd_ctx,  NULL, "rwp", handle_rwp_command, COMMAND_EXEC, "remove watchpoint <adress>");\r
+       \r
+       register_command(cmd_ctx,  NULL, "load_image", handle_load_image_command, COMMAND_EXEC, "load_image <file> <address> ['bin'|'ihex'|'elf'|'s19']");\r
+       register_command(cmd_ctx,  NULL, "dump_image", handle_dump_image_command, COMMAND_EXEC, "dump_image <file> <address> <size>");\r
+       register_command(cmd_ctx,  NULL, "verify_image", handle_verify_image_command, COMMAND_EXEC, "verify_image <file> [offset] [type]");\r
+       register_command(cmd_ctx,  NULL, "load_binary", handle_load_image_command, COMMAND_EXEC, "[DEPRECATED] load_binary <file> <address>");\r
+       register_command(cmd_ctx,  NULL, "dump_binary", handle_dump_image_command, COMMAND_EXEC, "[DEPRECATED] dump_binary <file> <address> <size>");\r
+       \r
+       target_request_register_commands(cmd_ctx);\r
+       trace_register_commands(cmd_ctx);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_targets_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = targets;\r
+       int count = 0;\r
+       \r
+       if (argc == 1)\r
+       {\r
+               int num = strtoul(args[0], NULL, 0);\r
+               \r
+               while (target)\r
+               {\r
+                       count++;\r
+                       target = target->next;\r
+               }\r
+               \r
+               if (num < count)\r
+                       cmd_ctx->current_target = num;\r
+               else\r
+                       command_print(cmd_ctx, "%i is out of bounds, only %i targets are configured", num, count);\r
+                       \r
+               return ERROR_OK;\r
+       }\r
+               \r
+       while (target)\r
+       {\r
+               command_print(cmd_ctx, "%i: %s (%s), state: %s", count++, target->type->name, target_endianess_strings[target->endianness], target_state_strings[target->state]);\r
+               target = target->next;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       int i;\r
+       int found = 0;\r
+       \r
+       if (argc < 3)\r
+       {\r
+               ERROR("target command requires at least three arguments: <type> <endianess> <reset_mode>");\r
+               exit(-1);\r
+       }\r
+       \r
+       /* search for the specified target */\r
+       if (args[0] && (args[0][0] != 0))\r
+       {\r
+               for (i = 0; target_types[i]; i++)\r
+               {\r
+                       if (strcmp(args[0], target_types[i]->name) == 0)\r
+                       {\r
+                               target_t **last_target_p = &targets;\r
+                               \r
+                               /* register target specific commands */\r
+                               if (target_types[i]->register_commands(cmd_ctx) != ERROR_OK)\r
+                               {\r
+                                       ERROR("couldn't register '%s' commands", args[0]);\r
+                                       exit(-1);\r
+                               }\r
+\r
+                               if (*last_target_p)\r
+                               {\r
+                                       while ((*last_target_p)->next)\r
+                                               last_target_p = &((*last_target_p)->next);\r
+                                       last_target_p = &((*last_target_p)->next);\r
+                               }\r
+\r
+                               *last_target_p = malloc(sizeof(target_t));\r
+                               \r
+                               (*last_target_p)->type = target_types[i];\r
+                               \r
+                               if (strcmp(args[1], "big") == 0)\r
+                                       (*last_target_p)->endianness = TARGET_BIG_ENDIAN;\r
+                               else if (strcmp(args[1], "little") == 0)\r
+                                       (*last_target_p)->endianness = TARGET_LITTLE_ENDIAN;\r
+                               else\r
+                               {\r
+                                       ERROR("endianness must be either 'little' or 'big', not '%s'", args[1]);\r
+                                       exit(-1);\r
+                               }\r
+                               \r
+                               /* what to do on a target reset */\r
+                               if (strcmp(args[2], "reset_halt") == 0)\r
+                                       (*last_target_p)->reset_mode = RESET_HALT;\r
+                               else if (strcmp(args[2], "reset_run") == 0)\r
+                                       (*last_target_p)->reset_mode = RESET_RUN;\r
+                               else if (strcmp(args[2], "reset_init") == 0)\r
+                                       (*last_target_p)->reset_mode = RESET_INIT;\r
+                               else if (strcmp(args[2], "run_and_halt") == 0)\r
+                                       (*last_target_p)->reset_mode = RESET_RUN_AND_HALT;\r
+                               else if (strcmp(args[2], "run_and_init") == 0)\r
+                                       (*last_target_p)->reset_mode = RESET_RUN_AND_INIT;\r
+                               else\r
+                               {\r
+                                       ERROR("unknown target startup mode %s", args[2]);\r
+                                       exit(-1);\r
+                               }\r
+                               (*last_target_p)->run_and_halt_time = 1000; /* default 1s */\r
+                               \r
+                               (*last_target_p)->reset_script = NULL;\r
+                               (*last_target_p)->post_halt_script = NULL;\r
+                               (*last_target_p)->pre_resume_script = NULL;\r
+                               (*last_target_p)->gdb_program_script = NULL;\r
+                               \r
+                               (*last_target_p)->working_area = 0x0;\r
+                               (*last_target_p)->working_area_size = 0x0;\r
+                               (*last_target_p)->working_areas = NULL;\r
+                               (*last_target_p)->backup_working_area = 0;\r
+                               \r
+                               (*last_target_p)->state = TARGET_UNKNOWN;\r
+                               (*last_target_p)->reg_cache = NULL;\r
+                               (*last_target_p)->breakpoints = NULL;\r
+                               (*last_target_p)->watchpoints = NULL;\r
+                               (*last_target_p)->next = NULL;\r
+                               (*last_target_p)->arch_info = NULL;\r
+                               \r
+                               /* initialize trace information */\r
+                               (*last_target_p)->trace_info = malloc(sizeof(trace_t));\r
+                               (*last_target_p)->trace_info->num_trace_points = 0;\r
+                               (*last_target_p)->trace_info->trace_points_size = 0;\r
+                               (*last_target_p)->trace_info->trace_points = NULL;\r
+                               (*last_target_p)->trace_info->trace_history_size = 0;\r
+                               (*last_target_p)->trace_info->trace_history = NULL;\r
+                               (*last_target_p)->trace_info->trace_history_pos = 0;\r
+                               (*last_target_p)->trace_info->trace_history_overflowed = 0;\r
+                               \r
+                               (*last_target_p)->dbgmsg = NULL;\r
+                               (*last_target_p)->dbg_msg_enabled = 0;\r
+                                                               \r
+                               (*last_target_p)->type->target_command(cmd_ctx, cmd, args, argc, *last_target_p);\r
+                               \r
+                               found = 1;\r
+                               break;\r
+                       }\r
+               }\r
+       }\r
+       \r
+       /* no matching target found */\r
+       if (!found)\r
+       {\r
+               ERROR("target '%s' not found", args[0]);\r
+               exit(-1);\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* usage: target_script <target#> <event> <script_file> */\r
+int handle_target_script_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = NULL;\r
+       \r
+       if (argc < 3)\r
+       {\r
+               ERROR("incomplete target_script command");\r
+               exit(-1);\r
+       }\r
+       \r
+       target = get_target_by_num(strtoul(args[0], NULL, 0));\r
+       \r
+       if (!target)\r
+       {\r
+               ERROR("target number '%s' not defined", args[0]);\r
+               exit(-1);\r
+       }\r
+       \r
+       if (strcmp(args[1], "reset") == 0)\r
+       {\r
+               if (target->reset_script)\r
+                       free(target->reset_script);\r
+               target->reset_script = strdup(args[2]);\r
+       }\r
+       else if (strcmp(args[1], "post_halt") == 0)\r
+       {\r
+               if (target->post_halt_script)\r
+                       free(target->post_halt_script);\r
+               target->post_halt_script = strdup(args[2]);\r
+       }\r
+       else if (strcmp(args[1], "pre_resume") == 0)\r
+       {\r
+               if (target->pre_resume_script)\r
+                       free(target->pre_resume_script);\r
+               target->pre_resume_script = strdup(args[2]);\r
+       }\r
+       else if (strcmp(args[1], "gdb_program_config") == 0)\r
+       {\r
+               if (target->gdb_program_script)\r
+                       free(target->gdb_program_script);\r
+               target->gdb_program_script = strdup(args[2]);\r
+       }\r
+       else\r
+       {\r
+               ERROR("unknown event type: '%s", args[1]);\r
+               exit(-1);       \r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_run_and_halt_time_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = NULL;\r
+       \r
+       if (argc < 2)\r
+       {\r
+               ERROR("incomplete run_and_halt_time command");\r
+               exit(-1);\r
+       }\r
+       \r
+       target = get_target_by_num(strtoul(args[0], NULL, 0));\r
+       \r
+       if (!target)\r
+       {\r
+               ERROR("target number '%s' not defined", args[0]);\r
+               exit(-1);\r
+       }\r
+       \r
+       target->run_and_halt_time = strtoul(args[1], NULL, 0);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_working_area_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = NULL;\r
+       \r
+       if ((argc < 4) || (argc > 5))\r
+       {\r
+               return ERROR_COMMAND_SYNTAX_ERROR;\r
+       }\r
+       \r
+       target = get_target_by_num(strtoul(args[0], NULL, 0));\r
+       \r
+       if (!target)\r
+       {\r
+               ERROR("target number '%s' not defined", args[0]);\r
+               exit(-1);\r
+       }\r
+       target_free_all_working_areas(target);\r
+       \r
+       target->working_area_phys = target->working_area_virt = strtoul(args[1], NULL, 0);\r
+       if (argc == 5)\r
+       {\r
+               target->working_area_virt = strtoul(args[4], NULL, 0);\r
+       }\r
+       target->working_area_size = strtoul(args[2], NULL, 0);\r
+       \r
+       if (strcmp(args[3], "backup") == 0)\r
+       {\r
+               target->backup_working_area = 1;\r
+       }\r
+       else if (strcmp(args[3], "nobackup") == 0)\r
+       {\r
+               target->backup_working_area = 0;\r
+       }\r
+       else\r
+       {\r
+               ERROR("unrecognized <backup|nobackup> argument (%s)", args[3]);\r
+               return ERROR_COMMAND_SYNTAX_ERROR;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+\r
+/* process target state changes */\r
+int handle_target(void *priv)\r
+{\r
+       int retval;\r
+       target_t *target = targets;\r
+       \r
+       while (target)\r
+       {\r
+               /* only poll if target isn't already halted */\r
+               if (target->state != TARGET_HALTED)\r
+               {\r
+                       if (target_continous_poll)\r
+                               if ((retval = target->type->poll(target)) != ERROR_OK)\r
+                               {\r
+                                       ERROR("couldn't poll target(%d). It's due for a reset.", retval);\r
+                               }\r
+               }\r
+       \r
+               target = target->next;\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target;\r
+       reg_t *reg = NULL;\r
+       int count = 0;\r
+       char *value;\r
+       \r
+       DEBUG("-");\r
+       \r
+       target = get_current_target(cmd_ctx);\r
+       \r
+       /* list all available registers for the current target */\r
+       if (argc == 0)\r
+       {\r
+               reg_cache_t *cache = target->reg_cache;\r
+               \r
+               count = 0;\r
+               while(cache)\r
+               {\r
+                       int i;\r
+                       for (i = 0; i < cache->num_regs; i++)\r
+                       {\r
+                               value = buf_to_str(cache->reg_list[i].value, cache->reg_list[i].size, 16);\r
+                               command_print(cmd_ctx, "(%i) %s (/%i): 0x%s (dirty: %i, valid: %i)", count++, cache->reg_list[i].name, cache->reg_list[i].size, value, cache->reg_list[i].dirty, cache->reg_list[i].valid);\r
+                               free(value);\r
+                       }\r
+                       cache = cache->next;\r
+               }\r
+               \r
+               return ERROR_OK;\r
+       }\r
+       \r
+       /* access a single register by its ordinal number */\r
+       if ((args[0][0] >= '0') && (args[0][0] <= '9'))\r
+       {\r
+               int num = strtoul(args[0], NULL, 0);\r
+               reg_cache_t *cache = target->reg_cache;\r
+               \r
+               count = 0;\r
+               while(cache)\r
+               {\r
+                       int i;\r
+                       for (i = 0; i < cache->num_regs; i++)\r
+                       {\r
+                               if (count++ == num)\r
+                               {\r
+                                       reg = &cache->reg_list[i];\r
+                                       break;\r
+                               }\r
+                       }\r
+                       if (reg)\r
+                               break;\r
+                       cache = cache->next;\r
+               }\r
+               \r
+               if (!reg)\r
+               {\r
+                       command_print(cmd_ctx, "%i is out of bounds, the current target has only %i registers (0 - %i)", num, count, count - 1);\r
+                       return ERROR_OK;\r
+               }\r
+       } else /* access a single register by its name */\r
+       {\r
+               reg = register_get_by_name(target->reg_cache, args[0], 1);\r
+               \r
+               if (!reg)\r
+               {\r
+                       command_print(cmd_ctx, "register %s not found in current target", args[0]);\r
+                       return ERROR_OK;\r
+               }\r
+       }\r
+\r
+       /* display a register */\r
+       if ((argc == 1) || ((argc == 2) && !((args[1][0] >= '0') && (args[1][0] <= '9'))))\r
+       {\r
+               if ((argc == 2) && (strcmp(args[1], "force") == 0))\r
+                       reg->valid = 0;\r
+               \r
+               if (reg->valid == 0)\r
+               {\r
+                       reg_arch_type_t *arch_type = register_get_arch_type(reg->arch_type);\r
+                       if (arch_type == NULL)\r
+                       {\r
+                               ERROR("BUG: encountered unregistered arch type");\r
+                               return ERROR_OK;\r
+                       }\r
+                       arch_type->get(reg);\r
+               }\r
+               value = buf_to_str(reg->value, reg->size, 16);\r
+               command_print(cmd_ctx, "%s (/%i): 0x%s", reg->name, reg->size, value);\r
+               free(value);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       /* set register value */\r
+       if (argc == 2)\r
+       {\r
+               u8 *buf = malloc(CEIL(reg->size, 8));\r
+               str_to_buf(args[1], strlen(args[1]), buf, reg->size, 0);\r
+\r
+               reg_arch_type_t *arch_type = register_get_arch_type(reg->arch_type);\r
+               if (arch_type == NULL)\r
+               {\r
+                       ERROR("BUG: encountered unregistered arch type");\r
+                       return ERROR_OK;\r
+               }\r
+               \r
+               arch_type->set(reg, buf);\r
+               \r
+               value = buf_to_str(reg->value, reg->size, 16);\r
+               command_print(cmd_ctx, "%s (/%i): 0x%s", reg->name, reg->size, value);\r
+               free(value);\r
+               \r
+               free(buf);\r
+               \r
+               return ERROR_OK;\r
+       }\r
+       \r
+       command_print(cmd_ctx, "usage: reg <#|name> [value]");\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+static int wait_state(struct command_context_s *cmd_ctx, char *cmd, enum target_state state, int ms);\r
+\r
+int handle_poll_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+\r
+       if (argc == 0)\r
+       {\r
+               target->type->poll(target);\r
+                       target_arch_state(target);\r
+       }\r
+       else\r
+       {\r
+               if (strcmp(args[0], "on") == 0)\r
+               {\r
+                       target_continous_poll = 1;\r
+               }\r
+               else if (strcmp(args[0], "off") == 0)\r
+               {\r
+                       target_continous_poll = 0;\r
+               }\r
+               else\r
+               {\r
+                       command_print(cmd_ctx, "arg is \"on\" or \"off\"");\r
+               }\r
+       }\r
+       \r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_wait_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       int ms = 5000;\r
+       \r
+       if (argc > 0)\r
+       {\r
+               char *end;\r
+\r
+               ms = strtoul(args[0], &end, 0) * 1000;\r
+               if (*end)\r
+               {\r
+                       command_print(cmd_ctx, "usage: %s [seconds]", cmd);\r
+                       return ERROR_OK;\r
+               }\r
+       }\r
+\r
+       return wait_state(cmd_ctx, cmd, TARGET_HALTED, ms); \r
+}\r
+\r
+static void target_process_events(struct command_context_s *cmd_ctx)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       target->type->poll(target);\r
+       target_call_timer_callbacks();\r
+}\r
+\r
+static int wait_state(struct command_context_s *cmd_ctx, char *cmd, enum target_state state, int ms)\r
+{\r
+       int retval;\r
+       struct timeval timeout, now;\r
+       \r
+       gettimeofday(&timeout, NULL);\r
+       timeval_add_time(&timeout, 0, ms * 1000);\r
+       \r
+       target_t *target = get_current_target(cmd_ctx);\r
+       for (;;)\r
+       {\r
+               if ((retval=target->type->poll(target))!=ERROR_OK)\r
+                       return retval;\r
+               target_call_timer_callbacks();\r
+               if (target->state == state)\r
+               {\r
+                       break;\r
+               }\r
+               command_print(cmd_ctx, "waiting for target %s...", target_state_strings[state]);\r
+               \r
+               gettimeofday(&now, NULL);\r
+               if ((now.tv_sec > timeout.tv_sec) || ((now.tv_sec == timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec)))\r
+               {\r
+                       command_print(cmd_ctx, "timed out while waiting for target %s", target_state_strings[state]);\r
+                       ERROR("timed out while waiting for target %s", target_state_strings[state]);\r
+                       break;\r
+               }\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       int retval;\r
+       target_t *target = get_current_target(cmd_ctx);\r
+\r
+       DEBUG("-");\r
+       \r
+       command_print(cmd_ctx, "requesting target halt...");\r
+\r
+       if ((retval = target->type->halt(target)) != ERROR_OK)\r
+       {       \r
+               switch (retval)\r
+               {\r
+                       case ERROR_TARGET_ALREADY_HALTED:\r
+                               command_print(cmd_ctx, "target already halted");\r
+                               break;\r
+                       case ERROR_TARGET_TIMEOUT:\r
+                               command_print(cmd_ctx, "target timed out... shutting down");\r
+                               return retval;\r
+                       default:\r
+                               command_print(cmd_ctx, "unknown error... shutting down");\r
+                               return retval;\r
+               }\r
+       }\r
+       \r
+       return handle_wait_halt_command(cmd_ctx, cmd, args, argc);\r
+}\r
+\r
+/* what to do on daemon startup */\r
+int handle_daemon_startup_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       if (argc == 1)\r
+       {\r
+               if (strcmp(args[0], "attach") == 0)\r
+               {\r
+                       startup_mode = DAEMON_ATTACH;\r
+                       return ERROR_OK;\r
+               }\r
+               else if (strcmp(args[0], "reset") == 0)\r
+               {\r
+                       startup_mode = DAEMON_RESET;\r
+                       return ERROR_OK;\r
+               }\r
+       }\r
+       \r
+       WARNING("invalid daemon_startup configuration directive: %s", args[0]);\r
+       return ERROR_OK;\r
+\r
+}\r
+               \r
+int handle_soft_reset_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       int retval;\r
+       \r
+       command_print(cmd_ctx, "requesting target halt and executing a soft reset");\r
+       \r
+       if ((retval = target->type->soft_reset_halt(target)) != ERROR_OK)\r
+       {       \r
+               switch (retval)\r
+               {\r
+                       case ERROR_TARGET_TIMEOUT:\r
+                               command_print(cmd_ctx, "target timed out... shutting down");\r
+                               exit(-1);\r
+                       default:\r
+                               command_print(cmd_ctx, "unknown error... shutting down");\r
+                               exit(-1);\r
+               }\r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       enum target_reset_mode reset_mode = target->reset_mode;\r
+       enum target_reset_mode save = target->reset_mode;\r
+       \r
+       DEBUG("-");\r
+       \r
+       if (argc >= 1)\r
+       {\r
+               if (strcmp("run", args[0]) == 0)\r
+                       reset_mode = RESET_RUN;\r
+               else if (strcmp("halt", args[0]) == 0)\r
+                       reset_mode = RESET_HALT;\r
+               else if (strcmp("init", args[0]) == 0)\r
+                       reset_mode = RESET_INIT;\r
+               else if (strcmp("run_and_halt", args[0]) == 0)\r
+               {\r
+                       reset_mode = RESET_RUN_AND_HALT;\r
+                       if (argc >= 2)\r
+                       {\r
+                               target->run_and_halt_time = strtoul(args[1], NULL, 0);\r
+                       }\r
+               }\r
+               else if (strcmp("run_and_init", args[0]) == 0)\r
+               {\r
+                       reset_mode = RESET_RUN_AND_INIT;\r
+                       if (argc >= 2)\r
+                       {\r
+                               target->run_and_halt_time = strtoul(args[1], NULL, 0);\r
+                       }\r
+               }\r
+               else\r
+               {\r
+                       command_print(cmd_ctx, "usage: reset ['run', 'halt', 'init', 'run_and_halt', 'run_and_init]");\r
+                       return ERROR_OK;\r
+               }\r
+       }\r
+       \r
+       /* temporarily modify mode of current reset target */\r
+       target->reset_mode = reset_mode;\r
+\r
+       /* reset *all* targets */\r
+       target_process_reset(cmd_ctx);\r
+       \r
+       /* Restore default reset mode for this target */\r
+    target->reset_mode = save;\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_resume_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       int retval;\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       \r
+       if (argc == 0)\r
+               retval = target->type->resume(target, 1, 0, 1, 0); /* current pc, addr = 0, handle breakpoints, not debugging */\r
+       else if (argc == 1)\r
+               retval = target->type->resume(target, 0, strtoul(args[0], NULL, 0), 1, 0); /* addr = args[0], handle breakpoints, not debugging */\r
+       else\r
+       {\r
+               return ERROR_COMMAND_SYNTAX_ERROR;\r
+       }\r
+\r
+       target_process_events(cmd_ctx);\r
+       \r
+       target_arch_state(target);\r
+       \r
+       return retval;\r
+}\r
+\r
+int handle_step_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       \r
+       DEBUG("-");\r
+       \r
+       if (argc == 0)\r
+               target->type->step(target, 1, 0, 1); /* current pc, addr = 0, handle breakpoints */\r
+\r
+       if (argc == 1)\r
+               target->type->step(target, 0, strtoul(args[0], NULL, 0), 1); /* addr = args[0], handle breakpoints */\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_md_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       const int line_bytecnt = 32;\r
+       int count = 1;\r
+       int size = 4;\r
+       u32 address = 0;\r
+       int line_modulo;\r
+       int i;\r
+\r
+       char output[128];\r
+       int output_len;\r
+\r
+       int retval;\r
+\r
+       u8 *buffer;\r
+       target_t *target = get_current_target(cmd_ctx);\r
+\r
+       if (argc < 1)\r
+               return ERROR_OK;\r
+\r
+       if (argc == 2)\r
+               count = strtoul(args[1], NULL, 0);\r
+\r
+       address = strtoul(args[0], NULL, 0);\r
+       \r
+\r
+       switch (cmd[2])\r
+       {\r
+               case 'w':\r
+                       size = 4; line_modulo = line_bytecnt / 4;\r
+                       break;\r
+               case 'h':\r
+                       size = 2; line_modulo = line_bytecnt / 2;\r
+                       break;\r
+               case 'b':\r
+                       size = 1; line_modulo = line_bytecnt / 1;\r
+                       break;\r
+               default:\r
+                       return ERROR_OK;\r
+       }\r
+\r
+       buffer = calloc(count, size);\r
+       retval  = target->type->read_memory(target, address, size, count, buffer);\r
+       if (retval != ERROR_OK)\r
+       {\r
+               switch (retval)\r
+               {\r
+                       case ERROR_TARGET_UNALIGNED_ACCESS:\r
+                               command_print(cmd_ctx, "error: address not aligned");\r
+                               break;\r
+                       case ERROR_TARGET_NOT_HALTED:\r
+                               command_print(cmd_ctx, "error: target must be halted for memory accesses");\r
+                               break;                  \r
+                       case ERROR_TARGET_DATA_ABORT:\r
+                               command_print(cmd_ctx, "error: access caused data abort, system possibly corrupted");\r
+                               break;\r
+                       default:\r
+                               command_print(cmd_ctx, "error: unknown error");\r
+                               break;\r
+               }\r
+               return ERROR_OK;\r
+       }\r
+\r
+       output_len = 0;\r
+\r
+       for (i = 0; i < count; i++)\r
+       {\r
+               if (i%line_modulo == 0)\r
+                       output_len += snprintf(output + output_len, 128 - output_len, "0x%8.8x: ", address + (i*size));\r
+               \r
+               switch (size)\r
+               {\r
+                       case 4:\r
+                               output_len += snprintf(output + output_len, 128 - output_len, "%8.8x ", target_buffer_get_u32(target, &buffer[i*4]));\r
+                               break;\r
+                       case 2:\r
+                               output_len += snprintf(output + output_len, 128 - output_len, "%4.4x ", target_buffer_get_u16(target, &buffer[i*2]));\r
+                               break;\r
+                       case 1:\r
+                               output_len += snprintf(output + output_len, 128 - output_len, "%2.2x ", buffer[i*1]);\r
+                               break;\r
+               }\r
+\r
+               if ((i%line_modulo == line_modulo-1) || (i == count - 1))\r
+               {\r
+                       command_print(cmd_ctx, output);\r
+                       output_len = 0;\r
+               }\r
+       }\r
+\r
+       free(buffer);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_mw_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       u32 address = 0;\r
+       u32 value = 0;\r
+       int retval;\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       u8 value_buf[4];\r
+\r
+       if (argc < 2)\r
+               return ERROR_OK;\r
+\r
+       address = strtoul(args[0], NULL, 0);\r
+       value = strtoul(args[1], NULL, 0);\r
+\r
+       switch (cmd[2])\r
+       {\r
+               case 'w':\r
+                       target_buffer_set_u32(target, value_buf, value);\r
+                       retval = target->type->write_memory(target, address, 4, 1, value_buf);\r
+                       break;\r
+               case 'h':\r
+                       target_buffer_set_u16(target, value_buf, value);\r
+                       retval = target->type->write_memory(target, address, 2, 1, value_buf);\r
+                       break;\r
+               case 'b':\r
+                       value_buf[0] = value;\r
+                       retval = target->type->write_memory(target, address, 1, 1, value_buf);\r
+                       break;\r
+               default:\r
+                       return ERROR_OK;\r
+       }\r
+\r
+       switch (retval)\r
+       {\r
+               case ERROR_TARGET_UNALIGNED_ACCESS:\r
+                       command_print(cmd_ctx, "error: address not aligned");\r
+                       break;\r
+               case ERROR_TARGET_DATA_ABORT:\r
+                       command_print(cmd_ctx, "error: access caused data abort, system possibly corrupted");\r
+                       break;\r
+               case ERROR_TARGET_NOT_HALTED:\r
+                       command_print(cmd_ctx, "error: target must be halted for memory accesses");\r
+                       break;\r
+               case ERROR_OK:\r
+                       break;\r
+               default:\r
+                       command_print(cmd_ctx, "error: unknown error");\r
+                       break;\r
+       }\r
+\r
+       return ERROR_OK;\r
+\r
+}\r
+\r
+int handle_load_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       u8 *buffer;\r
+       u32 buf_cnt;\r
+       u32 image_size;\r
+       int i;\r
+       int retval;\r
+\r
+       image_t image;  \r
+       \r
+       duration_t duration;\r
+       char *duration_text;\r
+       \r
+       target_t *target = get_current_target(cmd_ctx);\r
+\r
+       if (argc < 1)\r
+       {\r
+               command_print(cmd_ctx, "usage: load_image <filename> [address] [type]");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       /* a base address isn't always necessary, default to 0x0 (i.e. don't relocate) */\r
+       if (argc >= 2)\r
+       {\r
+               image.base_address_set = 1;\r
+               image.base_address = strtoul(args[1], NULL, 0);\r
+       }\r
+       else\r
+       {\r
+               image.base_address_set = 0;\r
+       }\r
+       \r
+       image.start_address_set = 0;\r
+\r
+       duration_start_measure(&duration);\r
+       \r
+       if (image_open(&image, args[0], (argc >= 3) ? args[2] : NULL) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "load_image error: %s", image.error_str);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       image_size = 0x0;\r
+       for (i = 0; i < image.num_sections; i++)\r
+       {\r
+               buffer = malloc(image.sections[i].size);\r
+               if (buffer == NULL)\r
+               {\r
+                       command_print(cmd_ctx, "error allocating buffer for section (%d bytes)", image.sections[i].size);\r
+                       break;\r
+               }\r
+               \r
+               if ((retval = image_read_section(&image, i, 0x0, image.sections[i].size, buffer, &buf_cnt)) != ERROR_OK)\r
+               {\r
+                       ERROR("image_read_section failed with error code: %i", retval);\r
+                       command_print(cmd_ctx, "image reading failed, download aborted");\r
+                       free(buffer);\r
+                       image_close(&image);\r
+                       return ERROR_OK;\r
+               }\r
+               target_write_buffer(target, image.sections[i].base_address, buf_cnt, buffer);\r
+               image_size += buf_cnt;\r
+               command_print(cmd_ctx, "%u byte written at address 0x%8.8x", buf_cnt, image.sections[i].base_address);\r
+               \r
+               free(buffer);\r
+       }\r
+\r
+       duration_stop_measure(&duration, &duration_text);\r
+       command_print(cmd_ctx, "downloaded %u byte in %s", image_size, duration_text);\r
+       free(duration_text);\r
+       \r
+       image_close(&image);\r
+\r
+       return ERROR_OK;\r
+\r
+}\r
+\r
+int handle_dump_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       fileio_t fileio;\r
+       \r
+       u32 address;\r
+       u32 size;\r
+       u8 buffer[560];\r
+       int retval;\r
+       \r
+       duration_t duration;\r
+       char *duration_text;\r
+       \r
+       target_t *target = get_current_target(cmd_ctx);\r
+\r
+       if (argc != 3)\r
+       {\r
+               command_print(cmd_ctx, "usage: dump_image <filename> <address> <size>");\r
+               return ERROR_OK;\r
+       }\r
+\r
+       address = strtoul(args[1], NULL, 0);\r
+       size = strtoul(args[2], NULL, 0);\r
+\r
+       if ((address & 3) || (size & 3))\r
+       {\r
+               command_print(cmd_ctx, "only 32-bit aligned address and size are supported");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (fileio_open(&fileio, args[0], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "dump_image error: %s", fileio.error_str);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       duration_start_measure(&duration);\r
+       \r
+       while (size > 0)\r
+       {\r
+               u32 size_written;\r
+               u32 this_run_size = (size > 560) ? 560 : size;\r
+               \r
+               retval = target->type->read_memory(target, address, 4, this_run_size / 4, buffer);\r
+               if (retval != ERROR_OK)\r
+               {\r
+                       command_print(cmd_ctx, "Reading memory failed %d", retval);\r
+                       break;\r
+               }\r
+               \r
+               fileio_write(&fileio, this_run_size, buffer, &size_written);\r
+               \r
+               size -= this_run_size;\r
+               address += this_run_size;\r
+       }\r
+\r
+       fileio_close(&fileio);\r
+\r
+       duration_stop_measure(&duration, &duration_text);\r
+       command_print(cmd_ctx, "dumped %"PRIi64" byte in %s", fileio.size, duration_text);\r
+       free(duration_text);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_verify_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       u8 *buffer;\r
+       u32 buf_cnt;\r
+       u32 image_size;\r
+       int i;\r
+       int retval;\r
+       u32 checksum = 0;\r
+       u32 mem_checksum = 0;\r
+\r
+       image_t image;  \r
+       \r
+       duration_t duration;\r
+       char *duration_text;\r
+       \r
+       target_t *target = get_current_target(cmd_ctx);\r
+       \r
+       if (argc < 1)\r
+       {\r
+               command_print(cmd_ctx, "usage: verify_image <file> [offset] [type]");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (!target)\r
+       {\r
+               ERROR("no target selected");\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       duration_start_measure(&duration);\r
+       \r
+       if (argc >= 2)\r
+       {\r
+               image.base_address_set = 1;\r
+               image.base_address = strtoul(args[1], NULL, 0);\r
+       }\r
+       else\r
+       {\r
+               image.base_address_set = 0;\r
+               image.base_address = 0x0;\r
+       }\r
+\r
+       image.start_address_set = 0;\r
+\r
+       if (image_open(&image, args[0], (argc == 3) ? args[2] : NULL) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "verify_image error: %s", image.error_str);\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       image_size = 0x0;\r
+       for (i = 0; i < image.num_sections; i++)\r
+       {\r
+               buffer = malloc(image.sections[i].size);\r
+               if (buffer == NULL)\r
+               {\r
+                       command_print(cmd_ctx, "error allocating buffer for section (%d bytes)", image.sections[i].size);\r
+                       break;\r
+               }\r
+               if ((retval = image_read_section(&image, i, 0x0, image.sections[i].size, buffer, &buf_cnt)) != ERROR_OK)\r
+               {\r
+                       ERROR("image_read_section failed with error code: %i", retval);\r
+                       command_print(cmd_ctx, "image reading failed, verify aborted");\r
+                       free(buffer);\r
+                       image_close(&image);\r
+                       return ERROR_OK;\r
+               }\r
+               \r
+               /* calculate checksum of image */\r
+               image_calculate_checksum( buffer, buf_cnt, &checksum );\r
+               \r
+               retval = target_checksum_memory(target, image.sections[i].base_address, buf_cnt, &mem_checksum);\r
+               \r
+               if( retval != ERROR_OK )\r
+               {\r
+                       command_print(cmd_ctx, "could not calculate checksum, verify aborted");\r
+                       free(buffer);\r
+                       image_close(&image);\r
+                       return ERROR_OK;\r
+               }\r
+               \r
+               if( checksum != mem_checksum )\r
+               {\r
+                       /* failed crc checksum, fall back to a binary compare */\r
+                       u8 *data;\r
+                       \r
+                       command_print(cmd_ctx, "checksum mismatch - attempting binary compare");\r
+                       \r
+                       data = (u8*)malloc(buf_cnt);\r
+                       \r
+                       /* Can we use 32bit word accesses? */\r
+                       int size = 1;\r
+                       int count = buf_cnt;\r
+                       if ((count % 4) == 0)\r
+                       {\r
+                               size *= 4;\r
+                               count /= 4;\r
+                       }\r
+                       retval = target->type->read_memory(target, image.sections[i].base_address, size, count, data);\r
+       \r
+                       if (retval == ERROR_OK)\r
+                       {\r
+                               int t;\r
+                               for (t = 0; t < buf_cnt; t++)\r
+                               {\r
+                                       if (data[t] != buffer[t])\r
+                                       {\r
+                                               command_print(cmd_ctx, "Verify operation failed address 0x%08x. Was 0x%02x instead of 0x%02x\n", t + image.sections[i].base_address, data[t], buffer[t]);\r
+                                               free(data);\r
+                                               free(buffer);\r
+                                               image_close(&image);\r
+                                               return ERROR_OK;\r
+                                       }\r
+                               }\r
+                       }\r
+                       \r
+                       free(data);\r
+               }\r
+               \r
+               free(buffer);\r
+               image_size += buf_cnt;\r
+       }\r
+       \r
+       duration_stop_measure(&duration, &duration_text);\r
+       command_print(cmd_ctx, "verified %u bytes in %s", image_size, duration_text);\r
+       free(duration_text);\r
+       \r
+       image_close(&image);\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_bp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       int retval;\r
+       target_t *target = get_current_target(cmd_ctx);\r
+\r
+       if (argc == 0)\r
+       {\r
+               breakpoint_t *breakpoint = target->breakpoints;\r
+\r
+               while (breakpoint)\r
+               {\r
+                       if (breakpoint->type == BKPT_SOFT)\r
+                       {\r
+                               char* buf = buf_to_str(breakpoint->orig_instr, breakpoint->length, 16);\r
+                               command_print(cmd_ctx, "0x%8.8x, 0x%x, %i, 0x%s", breakpoint->address, breakpoint->length, breakpoint->set, buf);\r
+                               free(buf);\r
+                       }\r
+                       else\r
+                       {\r
+                               command_print(cmd_ctx, "0x%8.8x, 0x%x, %i", breakpoint->address, breakpoint->length, breakpoint->set);\r
+                       }\r
+                       breakpoint = breakpoint->next;\r
+               }\r
+       }\r
+       else if (argc >= 2)\r
+       {\r
+               int hw = BKPT_SOFT;\r
+               u32 length = 0;\r
+\r
+               length = strtoul(args[1], NULL, 0);\r
+               \r
+               if (argc >= 3)\r
+                       if (strcmp(args[2], "hw") == 0)\r
+                               hw = BKPT_HARD;\r
+\r
+               if ((retval = breakpoint_add(target, strtoul(args[0], NULL, 0), length, hw)) != ERROR_OK)\r
+               {\r
+                       switch (retval)\r
+                       {\r
+                               case ERROR_TARGET_NOT_HALTED:\r
+                                       command_print(cmd_ctx, "target must be halted to set breakpoints");\r
+                                       break;\r
+                               case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:\r
+                                       command_print(cmd_ctx, "no more breakpoints available");\r
+                                       break;\r
+                               default:\r
+                                       command_print(cmd_ctx, "unknown error, breakpoint not set");\r
+                                       break;\r
+                       }\r
+               }\r
+               else\r
+               {\r
+                       command_print(cmd_ctx, "breakpoint added at address 0x%8.8x", strtoul(args[0], NULL, 0));\r
+               }\r
+       }\r
+       else\r
+       {\r
+               command_print(cmd_ctx, "usage: bp <address> <length> ['hw']");\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_rbp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+\r
+       if (argc > 0)\r
+               breakpoint_remove(target, strtoul(args[0], NULL, 0));\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_wp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       int retval;\r
+\r
+       if (argc == 0)\r
+       {\r
+               watchpoint_t *watchpoint = target->watchpoints;\r
+\r
+               while (watchpoint)\r
+               {\r
+                       command_print(cmd_ctx, "address: 0x%8.8x, mask: 0x%8.8x, r/w/a: %i, value: 0x%8.8x, mask: 0x%8.8x", watchpoint->address, watchpoint->length, watchpoint->rw, watchpoint->value, watchpoint->mask);\r
+                       watchpoint = watchpoint->next;\r
+               }\r
+       } \r
+       else if (argc >= 2)\r
+       {\r
+               enum watchpoint_rw type = WPT_ACCESS;\r
+               u32 data_value = 0x0;\r
+               u32 data_mask = 0xffffffff;\r
+               \r
+               if (argc >= 3)\r
+               {\r
+                       switch(args[2][0])\r
+                       {\r
+                               case 'r':\r
+                                       type = WPT_READ;\r
+                                       break;\r
+                               case 'w':\r
+                                       type = WPT_WRITE;\r
+                                       break;\r
+                               case 'a':\r
+                                       type = WPT_ACCESS;\r
+                                       break;\r
+                               default:\r
+                                       command_print(cmd_ctx, "usage: wp <address> <length> [r/w/a] [value] [mask]");\r
+                                       return ERROR_OK;\r
+                       }\r
+               }\r
+               if (argc >= 4)\r
+               {\r
+                       data_value = strtoul(args[3], NULL, 0);\r
+               }\r
+               if (argc >= 5)\r
+               {\r
+                       data_mask = strtoul(args[4], NULL, 0);\r
+               }\r
+               \r
+               if ((retval = watchpoint_add(target, strtoul(args[0], NULL, 0),\r
+                               strtoul(args[1], NULL, 0), type, data_value, data_mask)) != ERROR_OK)\r
+               {\r
+                       switch (retval)\r
+                       {\r
+                               case ERROR_TARGET_NOT_HALTED:\r
+                                       command_print(cmd_ctx, "target must be halted to set watchpoints");\r
+                                       break;\r
+                               case ERROR_TARGET_RESOURCE_NOT_AVAILABLE:\r
+                                       command_print(cmd_ctx, "no more watchpoints available");\r
+                                       break;\r
+                               default:\r
+                                       command_print(cmd_ctx, "unknown error, watchpoint not set");\r
+                                       break;\r
+                       }       \r
+               }\r
+       }\r
+       else\r
+       {\r
+               command_print(cmd_ctx, "usage: wp <address> <length> [r/w/a] [value] [mask]");\r
+       }\r
+               \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_rwp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+\r
+       if (argc > 0)\r
+               watchpoint_remove(target, strtoul(args[0], NULL, 0));\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       int retval;\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       u32 va;\r
+       u32 pa;\r
+\r
+       if (argc != 1)\r
+       {\r
+               return ERROR_COMMAND_SYNTAX_ERROR;\r
+       }\r
+       va = strtoul(args[0], NULL, 0);\r
+\r
+       retval = target->type->virt2phys(target, va, &pa);\r
+       if (retval == ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "Physical address 0x%08x", pa);\r
+       }\r
+       else\r
+       {\r
+               /* lower levels will have logged a detailed error which is \r
+                * forwarded to telnet/GDB session.  \r
+                */\r
+       }\r
+       return retval;\r
+}\r
index bfa0f9f9f7a5a2652d03895d9c5e9df8448e93d5..11ae5eedf4149679ee104c3d54b0a84b115263fa 100644 (file)
-/***************************************************************************
- *   Copyright (C) 2006, 2007 by Dominic Rath                              *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
- ***************************************************************************/
-#ifdef HAVE_CONFIG_H
-#include "config.h"
-#endif
-
-#include "replacements.h"
-
-#include "xscale.h"
-
-#include "register.h"
-#include "target.h"
-#include "armv4_5.h"
-#include "arm_simulator.h"
-#include "arm_disassembler.h"
-#include "log.h"
-#include "jtag.h"
-#include "binarybuffer.h"
-#include "time_support.h"
-#include "breakpoints.h"
-#include "fileio.h"
-
-#include <stdlib.h>
-#include <string.h>
-
-#include <sys/types.h>
-#include <unistd.h>
-#include <errno.h>
-
-
-/* cli handling */
-int xscale_register_commands(struct command_context_s *cmd_ctx);
-
-/* forward declarations */
-int xscale_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);
-int xscale_init_target(struct command_context_s *cmd_ctx, struct target_s *target);
-int xscale_quit();
-
-int xscale_arch_state(struct target_s *target);
-int xscale_poll(target_t *target);
-int xscale_halt(target_t *target);
-int xscale_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution);
-int xscale_step(struct target_s *target, int current, u32 address, int handle_breakpoints);
-int xscale_debug_entry(target_t *target);
-int xscale_restore_context(target_t *target);
-
-int xscale_assert_reset(target_t *target);
-int xscale_deassert_reset(target_t *target);
-int xscale_soft_reset_halt(struct target_s *target);
-int xscale_prepare_reset_halt(struct target_s *target);
-
-int xscale_set_reg_u32(reg_t *reg, u32 value);
-
-int xscale_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode);
-int xscale_write_core_reg(struct target_s *target, int num, enum armv4_5_mode mode, u32 value);
-
-int xscale_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
-int xscale_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);
-int xscale_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffer);
-int xscale_checksum_memory(struct target_s *target, u32 address, u32 count, u32* checksum);
-
-int xscale_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint);
-int xscale_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint);
-int xscale_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint);
-int xscale_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint);
-int xscale_add_watchpoint(struct target_s *target, watchpoint_t *watchpoint);
-int xscale_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint);
-void xscale_enable_watchpoints(struct target_s *target);
-void xscale_enable_breakpoints(struct target_s *target);
-static int xscale_virt2phys(struct target_s *target, u32 virtual, u32 *physical);
-static int xscale_mmu(struct target_s *target, int *enabled);
-
-int xscale_read_trace(target_t *target);
-
-target_type_t xscale_target =
-{
-       .name = "xscale",
-
-       .poll = xscale_poll,
-       .arch_state = xscale_arch_state,
-
-       .target_request_data = NULL,
-
-       .halt = xscale_halt,
-       .resume = xscale_resume,
-       .step = xscale_step,
-
-       .assert_reset = xscale_assert_reset,
-       .deassert_reset = xscale_deassert_reset,
-       .soft_reset_halt = xscale_soft_reset_halt,
-       .prepare_reset_halt = xscale_prepare_reset_halt,
-
-       .get_gdb_reg_list = armv4_5_get_gdb_reg_list,
-
-       .read_memory = xscale_read_memory,
-       .write_memory = xscale_write_memory,
-       .bulk_write_memory = xscale_bulk_write_memory,
-       .checksum_memory = xscale_checksum_memory,
-
-       .run_algorithm = armv4_5_run_algorithm,
-
-       .add_breakpoint = xscale_add_breakpoint,
-       .remove_breakpoint = xscale_remove_breakpoint,
-       .add_watchpoint = xscale_add_watchpoint,
-       .remove_watchpoint = xscale_remove_watchpoint,
-
-       .register_commands = xscale_register_commands,
-       .target_command = xscale_target_command,
-       .init_target = xscale_init_target,
-       .quit = xscale_quit,
-       
-       .virt2phys = xscale_virt2phys,
-       .mmu = xscale_mmu
-};
-
-char* xscale_reg_list[] =
-{
-       "XSCALE_MAINID",                /* 0 */
-       "XSCALE_CACHETYPE",
-       "XSCALE_CTRL",
-       "XSCALE_AUXCTRL",
-       "XSCALE_TTB",
-       "XSCALE_DAC",
-       "XSCALE_FSR",
-       "XSCALE_FAR",
-       "XSCALE_PID",
-       "XSCALE_CPACCESS",
-       "XSCALE_IBCR0",                 /* 10 */
-       "XSCALE_IBCR1",
-       "XSCALE_DBR0",
-       "XSCALE_DBR1",
-       "XSCALE_DBCON",
-       "XSCALE_TBREG",
-       "XSCALE_CHKPT0",
-       "XSCALE_CHKPT1",
-       "XSCALE_DCSR",
-       "XSCALE_TX",
-       "XSCALE_RX",                    /* 20 */
-       "XSCALE_TXRXCTRL",
-};
-
-xscale_reg_t xscale_reg_arch_info[] =
-{
-       {XSCALE_MAINID, NULL},
-       {XSCALE_CACHETYPE, NULL},
-       {XSCALE_CTRL, NULL},
-       {XSCALE_AUXCTRL, NULL},
-       {XSCALE_TTB, NULL},
-       {XSCALE_DAC, NULL},
-       {XSCALE_FSR, NULL},
-       {XSCALE_FAR, NULL},
-       {XSCALE_PID, NULL},
-       {XSCALE_CPACCESS, NULL},
-       {XSCALE_IBCR0, NULL},
-       {XSCALE_IBCR1, NULL},
-       {XSCALE_DBR0, NULL},
-       {XSCALE_DBR1, NULL},
-       {XSCALE_DBCON, NULL},
-       {XSCALE_TBREG, NULL},
-       {XSCALE_CHKPT0, NULL},
-       {XSCALE_CHKPT1, NULL},
-       {XSCALE_DCSR, NULL}, /* DCSR accessed via JTAG or SW */
-       {-1, NULL}, /* TX accessed via JTAG */
-       {-1, NULL}, /* RX accessed via JTAG */
-       {-1, NULL}, /* TXRXCTRL implicit access via JTAG */
-};
-
-int xscale_reg_arch_type = -1;
-
-int xscale_get_reg(reg_t *reg);
-int xscale_set_reg(reg_t *reg, u8 *buf);
-
-int xscale_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, xscale_common_t **xscale_p)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
-       {
-               ERROR("target isn't an XScale target");
-               return -1;
-       }
-
-       if (xscale->common_magic != XSCALE_COMMON_MAGIC)
-       {
-               ERROR("target isn't an XScale target");
-               return -1;
-       }
-
-       *armv4_5_p = armv4_5;
-       *xscale_p = xscale;
-
-       return ERROR_OK;
-}
-
-int xscale_jtag_set_instr(int chain_pos, u32 new_instr)
-{
-       jtag_device_t *device = jtag_get_device(chain_pos);
-
-       if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)
-       {
-               scan_field_t field;
-
-               field.device = chain_pos;
-               field.num_bits = device->ir_length;
-               field.out_value = calloc(CEIL(field.num_bits, 8), 1);
-               buf_set_u32(field.out_value, 0, field.num_bits, new_instr);
-               field.out_mask = NULL;
-               field.in_value = NULL;
-               jtag_set_check_value(&field, device->expected, device->expected_mask, NULL);
-
-               jtag_add_ir_scan(1, &field, -1, NULL);
-
-               free(field.out_value);
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_jtag_callback(enum jtag_event event, void *priv)
-{
-       switch (event)
-       {
-               case JTAG_TRST_ASSERTED:
-                       break;
-               case JTAG_TRST_RELEASED:
-                       break;
-               case JTAG_SRST_ASSERTED:
-                       break;
-               case JTAG_SRST_RELEASED:
-                       break;
-               default:
-                       WARNING("unhandled JTAG event");
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_read_dcsr(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       int retval;
-
-       scan_field_t fields[3];
-       u8 field0 = 0x0;
-       u8 field0_check_value = 0x2;
-       u8 field0_check_mask = 0x7;
-       u8 field2 = 0x0;
-       u8 field2_check_value = 0x0;
-       u8 field2_check_mask = 0x1;
-
-       jtag_add_end_state(TAP_PD);
-       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.dcsr);
-
-       buf_set_u32(&field0, 1, 1, xscale->hold_rst);
-       buf_set_u32(&field0, 2, 1, xscale->external_debug_break);
-
-       fields[0].device = xscale->jtag_info.chain_pos;
-       fields[0].num_bits = 3;
-       fields[0].out_value = &field0;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       jtag_set_check_value(fields+0, &field0_check_value, &field0_check_mask, NULL);
-
-       fields[1].device = xscale->jtag_info.chain_pos;
-       fields[1].num_bits = 32;
-       fields[1].out_value = NULL;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = xscale->reg_cache->reg_list[XSCALE_DCSR].value;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-
-
-
-       fields[2].device = xscale->jtag_info.chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = &field2;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       jtag_set_check_value(fields+2, &field2_check_value, &field2_check_mask, NULL);
-
-       jtag_add_dr_scan(3, fields, -1, NULL);
-
-       if ((retval = jtag_execute_queue()) != ERROR_OK)
-       {
-               ERROR("JTAG error while reading DCSR");
-               return retval;
-       }
-
-       xscale->reg_cache->reg_list[XSCALE_DCSR].dirty = 0;
-       xscale->reg_cache->reg_list[XSCALE_DCSR].valid = 1;
-
-       /* write the register with the value we just read
-        * on this second pass, only the first bit of field0 is guaranteed to be 0)
-        */
-       field0_check_mask = 0x1;
-       fields[1].out_value = xscale->reg_cache->reg_list[XSCALE_DCSR].value;
-       fields[1].in_value = NULL;
-
-       jtag_add_end_state(TAP_RTI);
-
-       jtag_add_dr_scan(3, fields, -1, NULL);
-
-       return ERROR_OK;
-}
-
-int xscale_receive(target_t *target, u32 *buffer, int num_words)
-{
-       int retval = ERROR_OK;
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       enum tap_state path[3];
-       scan_field_t fields[3];
-
-       u8 *field0 = malloc(num_words * 1);
-       u8 field0_check_value = 0x2;
-       u8 field0_check_mask = 0x6;
-       u32 *field1 = malloc(num_words * 4);
-       u8 field2_check_value = 0x0;
-       u8 field2_check_mask = 0x1;
-       int words_done = 0;
-       int words_scheduled = 0;
-
-       int i;
-
-       path[0] = TAP_SDS;
-       path[1] = TAP_CD;
-       path[2] = TAP_SD;
-
-       fields[0].device = xscale->jtag_info.chain_pos;
-       fields[0].num_bits = 3;
-       fields[0].out_value = NULL;
-       fields[0].out_mask = NULL;
-       /* fields[0].in_value = field0; */
-       jtag_set_check_value(fields+0, &field0_check_value, &field0_check_mask, NULL);
-
-       fields[1].device = xscale->jtag_info.chain_pos;
-       fields[1].num_bits = 32;
-       fields[1].out_value = NULL;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-
-
-
-       fields[2].device = xscale->jtag_info.chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = NULL;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       jtag_set_check_value(fields+2, &field2_check_value, &field2_check_mask, NULL);
-
-       jtag_add_end_state(TAP_RTI);
-       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.dbgtx);
-       jtag_add_runtest(1, -1);
-
-       /* repeat until all words have been collected */
-       int attempts = 0;
-       while (words_done < num_words)
-       {
-               /* schedule reads */
-               words_scheduled = 0;
-               for (i = words_done; i < num_words; i++)
-               {
-                       fields[0].in_value = &field0[i];
-                       fields[1].in_handler = buf_to_u32_handler;
-                       fields[1].in_handler_priv = (u8*)&field1[i];
-
-                       jtag_add_pathmove(3, path);
-                       jtag_add_dr_scan(3, fields, TAP_RTI, NULL);
-                       words_scheduled++;
-               }
-
-               if ((retval = jtag_execute_queue()) != ERROR_OK)
-               {
-                       ERROR("JTAG error while receiving data from debug handler");
-                       break;
-               }
-
-               /* examine results */
-               for (i = words_done; i < num_words; i++)
-               {
-                       if (!(field0[0] & 1))
-                       {
-                               /* move backwards if necessary */
-                               int j;
-                               for (j = i; j < num_words - 1; j++)
-                               {
-                                       field0[j] = field0[j+1];
-                                       field1[j] = field1[j+1];
-                               }
-                               words_scheduled--;
-                       }
-               }
-               if (words_scheduled == 0)
-               {
-                       if (attempts++ == 1000)
-                       {
-                               ERROR("Failed to receiving data from debug handler after 1000 attempts");
-                               retval = ERROR_JTAG_QUEUE_FAILED;
-                               break;
-                       }
-               }
-               
-               words_done += words_scheduled;
-       }
-
-       for (i = 0; i < num_words; i++)
-               *(buffer++) = buf_get_u32((u8*)&field1[i], 0, 32);
-
-       free(field1);
-
-       return retval;
-}
-
-int xscale_read_tx(target_t *target, int consume)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       enum tap_state path[3];
-       enum tap_state noconsume_path[9];
-
-       int retval;
-       struct timeval timeout, now;
-
-       scan_field_t fields[3];
-       u8 field0_in = 0x0;
-       u8 field0_check_value = 0x2;
-       u8 field0_check_mask = 0x6;
-       u8 field2_check_value = 0x0;
-       u8 field2_check_mask = 0x1;
-
-       jtag_add_end_state(TAP_RTI);
-
-       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.dbgtx);
-
-       path[0] = TAP_SDS;
-       path[1] = TAP_CD;
-       path[2] = TAP_SD;
-
-       noconsume_path[0] = TAP_SDS;
-       noconsume_path[1] = TAP_CD;
-       noconsume_path[2] = TAP_E1D;
-       noconsume_path[3] = TAP_PD;
-       noconsume_path[4] = TAP_E2D;
-       noconsume_path[5] = TAP_UD;
-       noconsume_path[6] = TAP_SDS;
-       noconsume_path[7] = TAP_CD;
-       noconsume_path[8] = TAP_SD;
-
-       fields[0].device = xscale->jtag_info.chain_pos;
-       fields[0].num_bits = 3;
-       fields[0].out_value = NULL;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = &field0_in;
-       jtag_set_check_value(fields+0, &field0_check_value, &field0_check_mask, NULL);
-
-       fields[1].device = xscale->jtag_info.chain_pos;
-       fields[1].num_bits = 32;
-       fields[1].out_value = NULL;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = xscale->reg_cache->reg_list[XSCALE_TX].value;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-
-
-
-       fields[2].device = xscale->jtag_info.chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = NULL;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       jtag_set_check_value(fields+2, &field2_check_value, &field2_check_mask, NULL);
-
-       gettimeofday(&timeout, NULL);
-       timeval_add_time(&timeout, 5, 0);
-
-       do
-       {
-               /* if we want to consume the register content (i.e. clear TX_READY),
-                * we have to go straight from Capture-DR to Shift-DR
-                * otherwise, we go from Capture-DR to Exit1-DR to Pause-DR
-               */
-               if (consume)
-                       jtag_add_pathmove(3, path);
-               else
-                       jtag_add_pathmove(sizeof(noconsume_path)/sizeof(*noconsume_path), noconsume_path);
-
-               jtag_add_dr_scan(3, fields, TAP_RTI, NULL);
-
-               if ((retval = jtag_execute_queue()) != ERROR_OK)
-               {
-                       ERROR("JTAG error while reading TX");
-                       return ERROR_TARGET_TIMEOUT;
-               }
-
-               gettimeofday(&now, NULL);
-               if ((now.tv_sec > timeout.tv_sec) || ((now.tv_sec == timeout.tv_sec)&& (now.tv_usec > timeout.tv_usec)))
-               {
-                       ERROR("time out reading TX register");
-                       return ERROR_TARGET_TIMEOUT;
-               }
-       } while ((!(field0_in & 1)) && consume);
-
-       if (!(field0_in & 1))
-               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-
-       return ERROR_OK;
-}
-
-int xscale_write_rx(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       int retval;
-       struct timeval timeout, now;
-
-       scan_field_t fields[3];
-       u8 field0_out = 0x0;
-       u8 field0_in = 0x0;
-       u8 field0_check_value = 0x2;
-       u8 field0_check_mask = 0x6;
-       u8 field2 = 0x0;
-       u8 field2_check_value = 0x0;
-       u8 field2_check_mask = 0x1;
-
-       jtag_add_end_state(TAP_RTI);
-
-       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.dbgrx);
-
-       fields[0].device = xscale->jtag_info.chain_pos;
-       fields[0].num_bits = 3;
-       fields[0].out_value = &field0_out;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = &field0_in;
-       jtag_set_check_value(fields+0, &field0_check_value, &field0_check_mask, NULL);
-
-       fields[1].device = xscale->jtag_info.chain_pos;
-       fields[1].num_bits = 32;
-       fields[1].out_value = xscale->reg_cache->reg_list[XSCALE_RX].value;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-
-
-
-       fields[2].device = xscale->jtag_info.chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = &field2;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       jtag_set_check_value(fields+2, &field2_check_value, &field2_check_mask, NULL);
-
-       gettimeofday(&timeout, NULL);
-       timeval_add_time(&timeout, 5, 0);
-
-       /* poll until rx_read is low */
-       DEBUG("polling RX");
-       do
-       {
-               jtag_add_dr_scan(3, fields, TAP_RTI, NULL);
-
-               if ((retval = jtag_execute_queue()) != ERROR_OK)
-               {
-                       ERROR("JTAG error while writing RX");
-                       return retval;
-               }
-
-               gettimeofday(&now, NULL);
-               if ((now.tv_sec > timeout.tv_sec) || ((now.tv_sec == timeout.tv_sec)&& (now.tv_usec > timeout.tv_usec)))
-               {
-                       ERROR("time out writing RX register");
-                       return ERROR_TARGET_TIMEOUT;
-               }
-       } while (field0_in & 1);
-
-       /* set rx_valid */
-       field2 = 0x1;
-       jtag_add_dr_scan(3, fields, TAP_RTI, NULL);
-
-       if ((retval = jtag_execute_queue()) != ERROR_OK)
-       {
-               ERROR("JTAG error while writing RX");
-               return retval;
-       }
-
-       return ERROR_OK;
-}
-
-/* send count elements of size byte to the debug handler */
-int xscale_send(target_t *target, u8 *buffer, int count, int size)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       int retval;
-
-       int done_count = 0;
-       u8 output[4] = {0, 0, 0, 0};
-
-       scan_field_t fields[3];
-       u8 field0_out = 0x0;
-       u8 field0_check_value = 0x2;
-       u8 field0_check_mask = 0x6;
-       u8 field2 = 0x1;
-       u8 field2_check_value = 0x0;
-       u8 field2_check_mask = 0x1;
-
-       jtag_add_end_state(TAP_RTI);
-
-       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.dbgrx);
-
-       fields[0].device = xscale->jtag_info.chain_pos;
-       fields[0].num_bits = 3;
-       fields[0].out_value = &field0_out;
-       fields[0].out_mask = NULL;
-       fields[0].in_handler = NULL;
-       if (!xscale->fast_memory_access)
-       {
-               jtag_set_check_value(fields+0, &field0_check_value, &field0_check_mask, NULL);
-       }
-
-       fields[1].device = xscale->jtag_info.chain_pos;
-       fields[1].num_bits = 32;
-       fields[1].out_value = output;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-
-
-
-       fields[2].device = xscale->jtag_info.chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = &field2;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       fields[2].in_handler = NULL;
-       if (!xscale->fast_memory_access)
-       {
-               jtag_set_check_value(fields+2, &field2_check_value, &field2_check_mask, NULL);
-       }
-
-       if (size==4)
-       {
-               int endianness = target->endianness;
-               while (done_count++ < count)
-               {
-                       if (endianness == TARGET_LITTLE_ENDIAN)
-                       {
-                               output[0]=buffer[0];
-                               output[1]=buffer[1];
-                               output[2]=buffer[2];
-                               output[3]=buffer[3];
-                       } else
-                       {
-                               output[0]=buffer[3];
-                               output[1]=buffer[2];
-                               output[2]=buffer[1];
-                               output[3]=buffer[0];
-                       }
-                       jtag_add_dr_scan(3, fields, TAP_RTI, NULL);
-                       buffer += size;
-               }
-               
-       } else
-       {
-               while (done_count++ < count)
-               {
-               /* extract sized element from target-endian buffer, and put it
-                * into little-endian output buffer
-                */
-               switch (size)
-               {
-                       case 2:
-                               buf_set_u32(output, 0, 32, target_buffer_get_u16(target, buffer));
-                               break;
-                       case 1:
-                               output[0] = *buffer;
-                               break;
-                       default:
-                               ERROR("BUG: size neither 4, 2 nor 1");
-                               exit(-1);
-               }
-
-               jtag_add_dr_scan(3, fields, TAP_RTI, NULL);
-               buffer += size;
-       }
-
-       }
-
-       if ((retval = jtag_execute_queue()) != ERROR_OK)
-       {
-               ERROR("JTAG error while sending data to debug handler");
-               return retval;
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_send_u32(target_t *target, u32 value)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       buf_set_u32(xscale->reg_cache->reg_list[XSCALE_RX].value, 0, 32, value);
-       return xscale_write_rx(target);
-}
-
-int xscale_write_dcsr(target_t *target, int hold_rst, int ext_dbg_brk)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       int retval;
-
-       scan_field_t fields[3];
-       u8 field0 = 0x0;
-       u8 field0_check_value = 0x2;
-       u8 field0_check_mask = 0x7;
-       u8 field2 = 0x0;
-       u8 field2_check_value = 0x0;
-       u8 field2_check_mask = 0x1;
-
-       if (hold_rst != -1)
-               xscale->hold_rst = hold_rst;
-
-       if (ext_dbg_brk != -1)
-               xscale->external_debug_break = ext_dbg_brk;
-
-       jtag_add_end_state(TAP_RTI);
-       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.dcsr);
-
-       buf_set_u32(&field0, 1, 1, xscale->hold_rst);
-       buf_set_u32(&field0, 2, 1, xscale->external_debug_break);
-
-       fields[0].device = xscale->jtag_info.chain_pos;
-       fields[0].num_bits = 3;
-       fields[0].out_value = &field0;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       jtag_set_check_value(fields+0, &field0_check_value, &field0_check_mask, NULL);
-
-       fields[1].device = xscale->jtag_info.chain_pos;
-       fields[1].num_bits = 32;
-       fields[1].out_value = xscale->reg_cache->reg_list[XSCALE_DCSR].value;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-
-
-
-       fields[2].device = xscale->jtag_info.chain_pos;
-       fields[2].num_bits = 1;
-       fields[2].out_value = &field2;
-       fields[2].out_mask = NULL;
-       fields[2].in_value = NULL;
-       jtag_set_check_value(fields+2, &field2_check_value, &field2_check_mask, NULL);
-
-       jtag_add_dr_scan(3, fields, -1, NULL);
-
-       if ((retval = jtag_execute_queue()) != ERROR_OK)
-       {
-               ERROR("JTAG error while writing DCSR");
-               return retval;
-       }
-
-       xscale->reg_cache->reg_list[XSCALE_DCSR].dirty = 0;
-       xscale->reg_cache->reg_list[XSCALE_DCSR].valid = 1;
-
-       return ERROR_OK;
-}
-
-/* parity of the number of bits 0 if even; 1 if odd. for 32 bit words */
-unsigned int parity (unsigned int v)
-{
-       unsigned int ov = v;
-       v ^= v >> 16;
-       v ^= v >> 8;
-       v ^= v >> 4;
-       v &= 0xf;
-       DEBUG("parity of 0x%x is %i", ov, (0x6996 >> v) & 1);
-       return (0x6996 >> v) & 1;
-}
-
-int xscale_load_ic(target_t *target, int mini, u32 va, u32 buffer[8])
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       u8 packet[4];
-       u8 cmd;
-       int word;
-
-       scan_field_t fields[2];
-
-       DEBUG("loading miniIC at 0x%8.8x", va);
-
-       jtag_add_end_state(TAP_RTI);
-       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.ldic); /* LDIC */
-
-       /* CMD is b010 for Main IC and b011 for Mini IC */
-       if (mini)
-               buf_set_u32(&cmd, 0, 3, 0x3);
-       else
-               buf_set_u32(&cmd, 0, 3, 0x2);
-
-       buf_set_u32(&cmd, 3, 3, 0x0);
-
-       /* virtual address of desired cache line */
-       buf_set_u32(packet, 0, 27, va >> 5);
-
-       fields[0].device = xscale->jtag_info.chain_pos;
-       fields[0].num_bits = 6;
-       fields[0].out_value = &cmd;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-
-       fields[1].device = xscale->jtag_info.chain_pos;
-       fields[1].num_bits = 27;
-       fields[1].out_value = packet;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       jtag_add_dr_scan(2, fields, -1, NULL);
-
-       fields[0].num_bits = 32;
-       fields[0].out_value = packet;
-
-       fields[1].num_bits = 1;
-       fields[1].out_value = &cmd;
-
-       for (word = 0; word < 8; word++)
-       {
-               buf_set_u32(packet, 0, 32, buffer[word]);
-               cmd = parity(*((u32*)packet));
-               jtag_add_dr_scan(2, fields, -1, NULL);
-       }
-
-       jtag_execute_queue();
-
-       return ERROR_OK;
-}
-
-int xscale_invalidate_ic_line(target_t *target, u32 va)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       u8 packet[4];
-       u8 cmd;
-
-       scan_field_t fields[2];
-
-       jtag_add_end_state(TAP_RTI);
-       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.ldic); /* LDIC */
-
-       /* CMD for invalidate IC line b000, bits [6:4] b000 */
-       buf_set_u32(&cmd, 0, 6, 0x0);
-
-       /* virtual address of desired cache line */
-       buf_set_u32(packet, 0, 27, va >> 5);
-
-       fields[0].device = xscale->jtag_info.chain_pos;
-       fields[0].num_bits = 6;
-       fields[0].out_value = &cmd;
-       fields[0].out_mask = NULL;
-       fields[0].in_value = NULL;
-       fields[0].in_check_value = NULL;
-       fields[0].in_check_mask = NULL;
-       fields[0].in_handler = NULL;
-       fields[0].in_handler_priv = NULL;
-
-       fields[1].device = xscale->jtag_info.chain_pos;
-       fields[1].num_bits = 27;
-       fields[1].out_value = packet;
-       fields[1].out_mask = NULL;
-       fields[1].in_value = NULL;
-       fields[1].in_check_value = NULL;
-       fields[1].in_check_mask = NULL;
-       fields[1].in_handler = NULL;
-       fields[1].in_handler_priv = NULL;
-
-       jtag_add_dr_scan(2, fields, -1, NULL);
-
-       return ERROR_OK;
-}
-
-int xscale_update_vectors(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       int i;
-
-       u32 low_reset_branch, high_reset_branch;
-
-       for (i = 1; i < 8; i++)
-       {
-               /* if there's a static vector specified for this exception, override */
-               if (xscale->static_high_vectors_set & (1 << i))
-               {
-                       xscale->high_vectors[i] = xscale->static_high_vectors[i];
-               }
-               else
-               {
-                       if (target_read_u32(target, 0xffff0000 + 4*i, &xscale->high_vectors[i]) != ERROR_OK)
-                       {
-                               xscale->high_vectors[i] = ARMV4_5_B(0xfffffe, 0);
-                       }
-               }
-       }
-
-       for (i = 1; i < 8; i++)
-       {
-               if (xscale->static_low_vectors_set & (1 << i))
-               {
-                       xscale->low_vectors[i] = xscale->static_low_vectors[i];
-               }
-               else
-               {
-                       if (target_read_u32(target, 0x0 + 4*i, &xscale->low_vectors[i]) != ERROR_OK)
-                       {
-                               xscale->low_vectors[i] = ARMV4_5_B(0xfffffe, 0);
-                       }
-               }
-       }
-
-       /* calculate branches to debug handler */
-       low_reset_branch = (xscale->handler_address + 0x20 - 0x0 - 0x8) >> 2;
-       high_reset_branch = (xscale->handler_address + 0x20 - 0xffff0000 - 0x8) >> 2;
-
-       xscale->low_vectors[0] = ARMV4_5_B((low_reset_branch & 0xffffff), 0);
-       xscale->high_vectors[0] = ARMV4_5_B((high_reset_branch & 0xffffff), 0);
-
-       /* invalidate and load exception vectors in mini i-cache */
-       xscale_invalidate_ic_line(target, 0x0);
-       xscale_invalidate_ic_line(target, 0xffff0000);
-
-       xscale_load_ic(target, 1, 0x0, xscale->low_vectors);
-       xscale_load_ic(target, 1, 0xffff0000, xscale->high_vectors);
-
-       return ERROR_OK;
-}
-
-int xscale_arch_state(struct target_s *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       char *state[] =
-       {
-               "disabled", "enabled"
-       };
-
-       char *arch_dbg_reason[] =
-       {
-               "", "\n(processor reset)", "\n(trace buffer full)"
-       };
-
-       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)
-       {
-               ERROR("BUG: called for a non-ARMv4/5 target");
-               exit(-1);
-       }
-
-       USER("target halted in %s state due to %s, current mode: %s\n"
-                       "cpsr: 0x%8.8x pc: 0x%8.8x\n"
-                       "MMU: %s, D-Cache: %s, I-Cache: %s"
-                       "%s",
-                        armv4_5_state_strings[armv4_5->core_state],
-                        target_debug_reason_strings[target->debug_reason],
-                        armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)],
-                        buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),
-                        buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32),
-                        state[xscale->armv4_5_mmu.mmu_enabled],
-                        state[xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled],
-                        state[xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled],
-                        arch_dbg_reason[xscale->arch_debug_reason]);
-
-       return ERROR_OK;
-}
-
-int xscale_poll(target_t *target)
-{
-       int retval=ERROR_OK;
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       if ((target->state == TARGET_RUNNING) || (target->state == TARGET_DEBUG_RUNNING))
-       {
-               enum target_state previous_state = target->state;
-               if ((retval = xscale_read_tx(target, 0)) == ERROR_OK)
-               {
-
-                       /* there's data to read from the tx register, we entered debug state */
-                       xscale->handler_running = 1;
-
-                       target->state = TARGET_HALTED;
-
-                       /* process debug entry, fetching current mode regs */
-                       retval = xscale_debug_entry(target);
-               }
-               else if (retval != ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
-               {
-                       USER("error while polling TX register, reset CPU");
-                       /* here we "lie" so GDB won't get stuck and a reset can be perfomed */
-                       target->state = TARGET_HALTED;
-               }
-
-                       /* debug_entry could have overwritten target state (i.e. immediate resume)
-                        * don't signal event handlers in that case
-                        */
-               if (target->state != TARGET_HALTED)
-                       return ERROR_OK;
-
-               /* if target was running, signal that we halted
-                * otherwise we reentered from debug execution */
-               if (previous_state == TARGET_RUNNING)
-                       target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-               else
-                       target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
-       }
-       return retval;
-}
-
-int xscale_debug_entry(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       u32 pc;
-       u32 buffer[10];
-       int i;
-
-       u32 moe;
-
-       /* clear external dbg break (will be written on next DCSR read) */
-       xscale->external_debug_break = 0;
-       xscale_read_dcsr(target);
-
-       /* get r0, pc, r1 to r7 and cpsr */
-       xscale_receive(target, buffer, 10);
-
-       /* move r0 from buffer to register cache */
-       buf_set_u32(armv4_5->core_cache->reg_list[0].value, 0, 32, buffer[0]);
-       armv4_5->core_cache->reg_list[15].dirty = 1;
-       armv4_5->core_cache->reg_list[15].valid = 1;
-       DEBUG("r0: 0x%8.8x", buffer[0]);
-
-       /* move pc from buffer to register cache */
-       buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, buffer[1]);
-       armv4_5->core_cache->reg_list[15].dirty = 1;
-       armv4_5->core_cache->reg_list[15].valid = 1;
-       DEBUG("pc: 0x%8.8x", buffer[1]);
-
-       /* move data from buffer to register cache */
-       for (i = 1; i <= 7; i++)
-       {
-               buf_set_u32(armv4_5->core_cache->reg_list[i].value, 0, 32, buffer[1 + i]);
-               armv4_5->core_cache->reg_list[i].dirty = 1;
-               armv4_5->core_cache->reg_list[i].valid = 1;
-               DEBUG("r%i: 0x%8.8x", i, buffer[i + 1]);
-       }
-
-       buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32, buffer[9]);
-       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;
-       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;
-       DEBUG("cpsr: 0x%8.8x", buffer[9]);
-
-       armv4_5->core_mode = buffer[9] & 0x1f;
-       if (armv4_5_mode_to_number(armv4_5->core_mode) == -1)
-       {
-               target->state = TARGET_UNKNOWN;
-               ERROR("cpsr contains invalid mode value - communication failure");
-               return ERROR_TARGET_FAILURE;
-       }
-       DEBUG("target entered debug state in %s mode", armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)]);
-
-       if (buffer[9] & 0x20)
-               armv4_5->core_state = ARMV4_5_STATE_THUMB;
-       else
-               armv4_5->core_state = ARMV4_5_STATE_ARM;
-
-       /* get banked registers, r8 to r14, and spsr if not in USR/SYS mode */
-       if ((armv4_5->core_mode != ARMV4_5_MODE_USR) && (armv4_5->core_mode != ARMV4_5_MODE_SYS))
-       {
-               xscale_receive(target, buffer, 8);
-               buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32, buffer[7]);
-               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).dirty = 0;
-               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).valid = 1;
-       }
-       else
-       {
-               /* r8 to r14, but no spsr */
-               xscale_receive(target, buffer, 7);
-       }
-
-       /* move data from buffer to register cache */
-       for (i = 8; i <= 14; i++)
-       {
-               buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, buffer[i - 8]);
-               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 0;
-               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;
-       }
-
-       /* examine debug reason */
-       xscale_read_dcsr(target);
-       moe = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 2, 3);
-
-       /* stored PC (for calculating fixup) */
-       pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
-
-       switch (moe)
-       {
-               case 0x0: /* Processor reset */
-                       target->debug_reason = DBG_REASON_DBGRQ;
-                       xscale->arch_debug_reason = XSCALE_DBG_REASON_RESET;
-                       pc -= 4;
-                       break;
-               case 0x1: /* Instruction breakpoint hit */
-                       target->debug_reason = DBG_REASON_BREAKPOINT;
-                       xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;
-                       pc -= 4;
-                       break;
-               case 0x2: /* Data breakpoint hit */
-                       target->debug_reason = DBG_REASON_WATCHPOINT;
-                       xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;
-                       pc -= 4;
-                       break;
-               case 0x3: /* BKPT instruction executed */
-                       target->debug_reason = DBG_REASON_BREAKPOINT;
-                       xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;
-                       pc -= 4;
-                       break;
-               case 0x4: /* Ext. debug event */
-                       target->debug_reason = DBG_REASON_DBGRQ;
-                       xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;
-                       pc -= 4;
-                       break;
-               case 0x5: /* Vector trap occured */
-                       target->debug_reason = DBG_REASON_BREAKPOINT;
-                       xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;
-                       pc -= 4;
-                       break;
-               case 0x6: /* Trace buffer full break */
-                       target->debug_reason = DBG_REASON_DBGRQ;
-                       xscale->arch_debug_reason = XSCALE_DBG_REASON_TB_FULL;
-                       pc -= 4;
-                       break;
-               case 0x7: /* Reserved */
-               default:
-                       ERROR("Method of Entry is 'Reserved'");
-                       exit(-1);
-                       break;
-       }
-
-       /* apply PC fixup */
-       buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, pc);
-
-       /* on the first debug entry, identify cache type */
-       if (xscale->armv4_5_mmu.armv4_5_cache.ctype == -1)
-       {
-               u32 cache_type_reg;
-
-               /* read cp15 cache type register */
-               xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CACHETYPE]);
-               cache_type_reg = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_CACHETYPE].value, 0, 32);
-
-               armv4_5_identify_cache(cache_type_reg, &xscale->armv4_5_mmu.armv4_5_cache);
-       }
-
-       /* examine MMU and Cache settings */
-       /* read cp15 control register */
-       xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CTRL]);
-       xscale->cp15_control_reg = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_CTRL].value, 0, 32);
-       xscale->armv4_5_mmu.mmu_enabled = (xscale->cp15_control_reg & 0x1U) ? 1 : 0;
-       xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = (xscale->cp15_control_reg & 0x4U) ? 1 : 0;
-       xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled = (xscale->cp15_control_reg & 0x1000U) ? 1 : 0;
-
-       /* tracing enabled, read collected trace data */
-       if (xscale->trace.buffer_enabled)
-       {
-               xscale_read_trace(target);
-               xscale->trace.buffer_fill--;
-
-               /* resume if we're still collecting trace data */
-               if ((xscale->arch_debug_reason == XSCALE_DBG_REASON_TB_FULL)
-                       && (xscale->trace.buffer_fill > 0))
-               {
-                       xscale_resume(target, 1, 0x0, 1, 0);
-               }
-               else
-               {
-                       xscale->trace.buffer_enabled = 0;
-               }
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_halt(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       DEBUG("target->state: %s", target_state_strings[target->state]);
-
-       if (target->state == TARGET_HALTED)
-       {
-               WARNING("target was already halted");
-               return ERROR_TARGET_ALREADY_HALTED;
-       }
-       else if (target->state == TARGET_UNKNOWN)
-       {
-               /* this must not happen for a xscale target */
-               ERROR("target was in unknown state when halt was requested");
-               return ERROR_TARGET_INVALID;
-       }
-       else if (target->state == TARGET_RESET)
-       {
-               DEBUG("target->state == TARGET_RESET");
-       }
-       else
-       {
-               /* assert external dbg break */
-               xscale->external_debug_break = 1;
-               xscale_read_dcsr(target);
-
-               target->debug_reason = DBG_REASON_DBGRQ;
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_enable_single_step(struct target_s *target, u32 next_pc)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale= armv4_5->arch_info;
-       reg_t *ibcr0 = &xscale->reg_cache->reg_list[XSCALE_IBCR0];
-
-       if (xscale->ibcr0_used)
-       {
-               breakpoint_t *ibcr0_bp = breakpoint_find(target, buf_get_u32(ibcr0->value, 0, 32) & 0xfffffffe);
-
-               if (ibcr0_bp)
-               {
-                       xscale_unset_breakpoint(target, ibcr0_bp);
-               }
-               else
-               {
-                       ERROR("BUG: xscale->ibcr0_used is set, but no breakpoint with that address found");
-                       exit(-1);
-               }
-       }
-
-       xscale_set_reg_u32(ibcr0, next_pc | 0x1);
-
-       return ERROR_OK;
-}
-
-int xscale_disable_single_step(struct target_s *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale= armv4_5->arch_info;
-       reg_t *ibcr0 = &xscale->reg_cache->reg_list[XSCALE_IBCR0];
-
-       xscale_set_reg_u32(ibcr0, 0x0);
-
-       return ERROR_OK;
-}
-
-int xscale_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale= armv4_5->arch_info;
-       breakpoint_t *breakpoint = target->breakpoints;
-
-       u32 current_pc;
-
-       int retval;
-       int i;
-
-       DEBUG("-");
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       if (!debug_execution)
-       {
-               target_free_all_working_areas(target);
-       }
-
-       /* update vector tables */
-       xscale_update_vectors(target);
-
-       /* current = 1: continue on current pc, otherwise continue at <address> */
-       if (!current)
-               buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
-
-       current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
-
-       /* if we're at the reset vector, we have to simulate the branch */
-       if (current_pc == 0x0)
-       {
-               arm_simulate_step(target, NULL);
-               current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
-       }
-
-       /* the front-end may request us not to handle breakpoints */
-       if (handle_breakpoints)
-       {
-               if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
-               {
-                       u32 next_pc;
-
-                       /* there's a breakpoint at the current PC, we have to step over it */
-                       DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
-                       xscale_unset_breakpoint(target, breakpoint);
-
-                       /* calculate PC of next instruction */
-                       if ((retval = arm_simulate_step(target, &next_pc)) != ERROR_OK)
-                       {
-                               u32 current_opcode;
-                               target_read_u32(target, current_pc, &current_opcode);
-                               ERROR("BUG: couldn't calculate PC of next instruction, current opcode was 0x%8.8x", current_opcode);
-                       }
-
-                       DEBUG("enable single-step");
-                       xscale_enable_single_step(target, next_pc);
-
-                       /* restore banked registers */
-                       xscale_restore_context(target);
-
-                       /* send resume request (command 0x30 or 0x31)
-                        * clean the trace buffer if it is to be enabled (0x62) */
-                       if (xscale->trace.buffer_enabled)
-                       {
-                               xscale_send_u32(target, 0x62);
-                               xscale_send_u32(target, 0x31);
-                       }
-                       else
-                               xscale_send_u32(target, 0x30);
-
-                       /* send CPSR */
-                       xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
-                       DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
-
-                       for (i = 7; i >= 0; i--)
-                       {
-                               /* send register */
-                               xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));
-                               DEBUG("writing r%i with value 0x%8.8x", i, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));
-                       }
-
-                       /* send PC */
-                       xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
-                       DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
-
-                       /* wait for and process debug entry */
-                       xscale_debug_entry(target);
-
-                       DEBUG("disable single-step");
-                       xscale_disable_single_step(target);
-
-                       DEBUG("set breakpoint at 0x%8.8x", breakpoint->address);
-                       xscale_set_breakpoint(target, breakpoint);
-               }
-       }
-
-       /* enable any pending breakpoints and watchpoints */
-       xscale_enable_breakpoints(target);
-       xscale_enable_watchpoints(target);
-
-       /* restore banked registers */
-       xscale_restore_context(target);
-
-       /* send resume request (command 0x30 or 0x31)
-        * clean the trace buffer if it is to be enabled (0x62) */
-       if (xscale->trace.buffer_enabled)
-       {
-               xscale_send_u32(target, 0x62);
-               xscale_send_u32(target, 0x31);
-       }
-       else
-               xscale_send_u32(target, 0x30);
-
-       /* send CPSR */
-       xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
-       DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
-
-       for (i = 7; i >= 0; i--)
-       {
-               /* send register */
-               xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));
-               DEBUG("writing r%i with value 0x%8.8x", i, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));
-       }
-
-       /* send PC */
-       xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
-       DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
-
-       target->debug_reason = DBG_REASON_NOTHALTED;
-
-       if (!debug_execution)
-       {
-               /* registers are now invalid */
-               armv4_5_invalidate_core_regs(target);
-               target->state = TARGET_RUNNING;
-               target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
-       }
-       else
-       {
-               target->state = TARGET_DEBUG_RUNNING;
-               target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
-       }
-
-       DEBUG("target resumed");
-
-       xscale->handler_running = 1;
-
-       return ERROR_OK;
-}
-
-int xscale_step(struct target_s *target, int current, u32 address, int handle_breakpoints)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       breakpoint_t *breakpoint = target->breakpoints;
-
-       u32 current_pc, next_pc;
-       int i;
-       int retval;
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       /* current = 1: continue on current pc, otherwise continue at <address> */
-       if (!current)
-               buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);
-
-       current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
-
-       /* if we're at the reset vector, we have to simulate the step */
-       if (current_pc == 0x0)
-       {
-               arm_simulate_step(target, NULL);
-               current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
-
-               target->debug_reason = DBG_REASON_SINGLESTEP;
-               target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-
-               return ERROR_OK;
-       }
-
-       /* the front-end may request us not to handle breakpoints */
-       if (handle_breakpoints)
-               if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))
-               {
-                       xscale_unset_breakpoint(target, breakpoint);
-               }
-
-       target->debug_reason = DBG_REASON_SINGLESTEP;
-
-       /* calculate PC of next instruction */
-       if ((retval = arm_simulate_step(target, &next_pc)) != ERROR_OK)
-       {
-               u32 current_opcode;
-               target_read_u32(target, current_pc, &current_opcode);
-               ERROR("BUG: couldn't calculate PC of next instruction, current opcode was 0x%8.8x", current_opcode);
-       }
-
-       DEBUG("enable single-step");
-       xscale_enable_single_step(target, next_pc);
-
-       /* restore banked registers */
-       xscale_restore_context(target);
-
-       /* send resume request (command 0x30 or 0x31)
-        * clean the trace buffer if it is to be enabled (0x62) */
-       if (xscale->trace.buffer_enabled)
-       {
-               xscale_send_u32(target, 0x62);
-               xscale_send_u32(target, 0x31);
-       }
-       else
-               xscale_send_u32(target, 0x30);
-
-       /* send CPSR */
-       xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
-       DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));
-
-       for (i = 7; i >= 0; i--)
-       {
-               /* send register */
-               xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));
-               DEBUG("writing r%i with value 0x%8.8x", i, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));
-       }
-
-       /* send PC */
-       xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
-       DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));
-
-       target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
-
-       /* registers are now invalid */
-       armv4_5_invalidate_core_regs(target);
-
-       /* wait for and process debug entry */
-       xscale_debug_entry(target);
-
-       DEBUG("disable single-step");
-       xscale_disable_single_step(target);
-
-       target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-
-       if (breakpoint)
-       {
-               xscale_set_breakpoint(target, breakpoint);
-       }
-
-       DEBUG("target stepped");
-
-       return ERROR_OK;
-
-}
-
-int xscale_assert_reset(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       DEBUG("target->state: %s", target_state_strings[target->state]);
-
-       /* select DCSR instruction (set endstate to R-T-I to ensure we don't
-        * end up in T-L-R, which would reset JTAG
-        */
-       jtag_add_end_state(TAP_RTI);
-       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.dcsr);
-
-       /* set Hold reset, Halt mode and Trap Reset */
-       buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 30, 1, 0x1);
-       buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 16, 1, 0x1);
-       xscale_write_dcsr(target, 1, 0);
-
-       /* select BYPASS, because having DCSR selected caused problems on the PXA27x */
-       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, 0x7f);
-       jtag_execute_queue();
-
-       /* assert reset */
-       jtag_add_reset(0, 1);
-
-       /* sleep 1ms, to be sure we fulfill any requirements */
-       jtag_add_sleep(1000);
-       jtag_execute_queue();
-
-       target->state = TARGET_RESET;
-
-       return ERROR_OK;
-}
-
-int xscale_deassert_reset(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       fileio_t debug_handler;
-       u32 address;
-       u32 binary_size;
-
-       u32 buf_cnt;
-       int i;
-       int retval;
-
-       breakpoint_t *breakpoint = target->breakpoints;
-
-       DEBUG("-");
-
-       xscale->ibcr_available = 2;
-       xscale->ibcr0_used = 0;
-       xscale->ibcr1_used = 0;
-
-       xscale->dbr_available = 2;
-       xscale->dbr0_used = 0;
-       xscale->dbr1_used = 0;
-
-       /* mark all hardware breakpoints as unset */
-       while (breakpoint)
-       {
-               if (breakpoint->type == BKPT_HARD)
-               {
-                       breakpoint->set = 0;
-               }
-               breakpoint = breakpoint->next;
-       }
-
-       if (!xscale->handler_installed)
-       {
-               /* release SRST */
-               jtag_add_reset(0, 0);
-
-               /* wait 300ms; 150 and 100ms were not enough */
-               jtag_add_sleep(300*1000);
-
-               jtag_add_runtest(2030, TAP_RTI);
-               jtag_execute_queue();
-
-               /* set Hold reset, Halt mode and Trap Reset */
-               buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 30, 1, 0x1);
-               buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 16, 1, 0x1);
-               xscale_write_dcsr(target, 1, 0);
-
-               /* Load debug handler */
-               if (fileio_open(&debug_handler, PKGLIBDIR "/xscale/debug_handler.bin", FILEIO_READ, FILEIO_BINARY) != ERROR_OK)
-               {
-                       ERROR("file open error: %s", debug_handler.error_str);
-                       return ERROR_OK;
-               }
-
-               if ((binary_size = debug_handler.size) % 4)
-               {
-                       ERROR("debug_handler.bin: size not a multiple of 4");
-                       exit(-1);
-               }
-
-               if (binary_size > 0x800)
-               {
-                       ERROR("debug_handler.bin: larger than 2kb");
-                       exit(-1);
-               }
-
-               binary_size = CEIL(binary_size, 32) * 32;
-
-               address = xscale->handler_address;
-               while (binary_size > 0)
-               {
-                       u32 cache_line[8];
-                       u8 buffer[32];
-
-                       if ((retval = fileio_read(&debug_handler, 32, buffer, &buf_cnt)) != ERROR_OK)
-                       {
-                               ERROR("reading debug handler failed: %s", debug_handler.error_str);
-                       }
-
-                       for (i = 0; i < buf_cnt; i += 4)
-                       {
-                               /* convert LE buffer to host-endian u32 */
-                               cache_line[i / 4] = le_to_h_u32(&buffer[i]);
-                       }
-
-                       for (; buf_cnt < 32; buf_cnt += 4)
-                       {
-                                       cache_line[buf_cnt / 4] = 0xe1a08008;
-                       }
-
-                       /* only load addresses other than the reset vectors */
-                       if ((address % 0x400) != 0x0)
-                       {
-                               xscale_load_ic(target, 1, address, cache_line);
-                       }
-
-                       address += buf_cnt;
-                       binary_size -= buf_cnt;
-               };
-
-               xscale_load_ic(target, 1, 0x0, xscale->low_vectors);
-               xscale_load_ic(target, 1, 0xffff0000, xscale->high_vectors);
-
-               jtag_add_runtest(30, TAP_RTI);
-
-               jtag_add_sleep(100000);
-
-               /* set Hold reset, Halt mode and Trap Reset */
-               buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 30, 1, 0x1);
-               buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 16, 1, 0x1);
-               xscale_write_dcsr(target, 1, 0);
-
-               /* clear Hold reset to let the target run (should enter debug handler) */
-               xscale_write_dcsr(target, 0, 1);
-               target->state = TARGET_RUNNING;
-
-               if ((target->reset_mode != RESET_HALT) && (target->reset_mode != RESET_INIT))
-               {
-                       jtag_add_sleep(10000);
-
-                       /* we should have entered debug now */
-                       xscale_debug_entry(target);
-                       target->state = TARGET_HALTED;
-
-                       /* resume the target */
-                       xscale_resume(target, 1, 0x0, 1, 0);
-               }
-
-               fileio_close(&debug_handler);
-       }
-       else
-       {
-               jtag_add_reset(0, 0);
-       }
-
-
-       return ERROR_OK;
-}
-
-int xscale_soft_reset_halt(struct target_s *target)
-{
-
-       return ERROR_OK;
-}
-
-int xscale_prepare_reset_halt(struct target_s *target)
-{
-       /* nothing to be done for reset_halt on XScale targets
-        * we always halt after a reset to upload the debug handler
-        */
-       return ERROR_OK;
-}
-
-int xscale_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode)
-{
-
-       return ERROR_OK;
-}
-
-int xscale_write_core_reg(struct target_s *target, int num, enum armv4_5_mode mode, u32 value)
-{
-
-       return ERROR_OK;
-}
-
-int xscale_full_context(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-
-       u32 *buffer;
-
-       int i, j;
-
-       DEBUG("-");
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       buffer = malloc(4 * 8);
-
-       /* iterate through processor modes (FIQ, IRQ, SVC, ABT, UND and SYS)
-        * we can't enter User mode on an XScale (unpredictable),
-        * but User shares registers with SYS
-        */
-       for(i = 1; i < 7; i++)
-       {
-               int valid = 1;
-
-               /* check if there are invalid registers in the current mode
-                */
-               for (j = 0; j <= 16; j++)
-               {
-                       if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)
-                               valid = 0;
-               }
-
-               if (!valid)
-               {
-                       u32 tmp_cpsr;
-
-                       /* request banked registers */
-                       xscale_send_u32(target, 0x0);
-
-                       tmp_cpsr = 0x0;
-                       tmp_cpsr |= armv4_5_number_to_mode(i);
-                       tmp_cpsr |= 0xc0; /* I/F bits */
-
-                       /* send CPSR for desired mode */
-                       xscale_send_u32(target, tmp_cpsr);
-
-                       /* get banked registers, r8 to r14, and spsr if not in USR/SYS mode */
-                       if ((armv4_5_number_to_mode(i) != ARMV4_5_MODE_USR) && (armv4_5_number_to_mode(i) != ARMV4_5_MODE_SYS))
-                       {
-                               xscale_receive(target, buffer, 8);
-                               buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32, buffer[7]);
-                               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty = 0;
-                               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid = 1;
-                       }
-                       else
-                       {
-                               xscale_receive(target, buffer, 7);
-                       }
-
-                       /* move data from buffer to register cache */
-                       for (j = 8; j <= 14; j++)
-                       {
-                               buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).value, 0, 32, buffer[j - 8]);
-                               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty = 0;
-                               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid = 1;
-                       }
-               }
-       }
-
-       free(buffer);
-
-       return ERROR_OK;
-}
-
-int xscale_restore_context(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-
-       int i, j;
-
-       DEBUG("-");
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       /* iterate through processor modes (FIQ, IRQ, SVC, ABT, UND and SYS)
-       * we can't enter User mode on an XScale (unpredictable),
-       * but User shares registers with SYS
-       */
-       for(i = 1; i < 7; i++)
-       {
-               int dirty = 0;
-
-               /* check if there are invalid registers in the current mode
-               */
-               for (j = 8; j <= 14; j++)
-               {
-                       if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty == 1)
-                               dirty = 1;
-               }
-
-               /* if not USR/SYS, check if the SPSR needs to be written */
-               if ((armv4_5_number_to_mode(i) != ARMV4_5_MODE_USR) && (armv4_5_number_to_mode(i) != ARMV4_5_MODE_SYS))
-               {
-                       if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty == 1)
-                               dirty = 1;
-               }
-
-               if (dirty)
-               {
-                       u32 tmp_cpsr;
-
-                       /* send banked registers */
-                       xscale_send_u32(target, 0x1);
-
-                       tmp_cpsr = 0x0;
-                       tmp_cpsr |= armv4_5_number_to_mode(i);
-                       tmp_cpsr |= 0xc0; /* I/F bits */
-
-                       /* send CPSR for desired mode */
-                       xscale_send_u32(target, tmp_cpsr);
-
-                       /* send banked registers, r8 to r14, and spsr if not in USR/SYS mode */
-                       for (j = 8; j <= 14; j++)
-                       {
-                               xscale_send_u32(target, buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, j).value, 0, 32));
-                               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty = 0;
-                       }
-
-                       if ((armv4_5_number_to_mode(i) != ARMV4_5_MODE_USR) && (armv4_5_number_to_mode(i) != ARMV4_5_MODE_SYS))
-                       {
-                               xscale_send_u32(target, buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32));
-                               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty = 0;
-                       }
-               }
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       u32 *buf32;
-       int i;
-
-       DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       /* sanitize arguments */
-       if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
-               return ERROR_INVALID_ARGUMENTS;
-
-       if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
-               return ERROR_TARGET_UNALIGNED_ACCESS;
-
-       /* send memory read request (command 0x1n, n: access size) */
-       xscale_send_u32(target, 0x10 | size);
-
-       /* send base address for read request */
-       xscale_send_u32(target, address);
-
-       /* send number of requested data words */
-       xscale_send_u32(target, count);
-
-       /* receive data from target (count times 32-bit words in host endianness) */
-       buf32 = malloc(4 * count);
-       xscale_receive(target, buf32, count);
-
-       /* extract data from host-endian buffer into byte stream */
-       for (i = 0; i < count; i++)
-       {
-               switch (size)
-               {
-                       case 4:
-                               target_buffer_set_u32(target, buffer, buf32[i]);
-                               buffer += 4;
-                               break;
-                       case 2:
-                               target_buffer_set_u16(target, buffer, buf32[i] & 0xffff);
-                               buffer += 2;
-                               break;
-                       case 1:
-                               *buffer++ = buf32[i] & 0xff;
-                               break;
-                       default:
-                               ERROR("should never get here");
-                               exit(-1);
-               }
-       }
-
-       free(buf32);
-
-       /* examine DCSR, to see if Sticky Abort (SA) got set */
-       xscale_read_dcsr(target);
-       if (buf_get_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 5, 1) == 1)
-       {
-               /* clear SA bit */
-               xscale_send_u32(target, 0x60);
-
-               return ERROR_TARGET_DATA_ABORT;
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       /* sanitize arguments */
-       if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))
-               return ERROR_INVALID_ARGUMENTS;
-
-       if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))
-               return ERROR_TARGET_UNALIGNED_ACCESS;
-
-       /* send memory write request (command 0x2n, n: access size) */
-       xscale_send_u32(target, 0x20 | size);
-
-       /* send base address for read request */
-       xscale_send_u32(target, address);
-
-       /* send number of requested data words to be written*/
-       xscale_send_u32(target, count);
-
-       /* extract data from host-endian buffer into byte stream */
-#if 0
-       for (i = 0; i < count; i++)
-       {
-               switch (size)
-               {
-                       case 4:
-                               value = target_buffer_get_u32(target, buffer);
-                               xscale_send_u32(target, value);
-                               buffer += 4;
-                               break;
-                       case 2:
-                               value = target_buffer_get_u16(target, buffer);
-                               xscale_send_u32(target, value);
-                               buffer += 2;
-                               break;
-                       case 1:
-                               value = *buffer;
-                               xscale_send_u32(target, value);
-                               buffer += 1;
-                               break;
-                       default:
-                               ERROR("should never get here");
-                               exit(-1);
-               }
-       }
-#endif
-       xscale_send(target, buffer, count, size);
-
-       /* examine DCSR, to see if Sticky Abort (SA) got set */
-       xscale_read_dcsr(target);
-       if (buf_get_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 5, 1) == 1)
-       {
-               /* clear SA bit */
-               xscale_send_u32(target, 0x60);
-
-               return ERROR_TARGET_DATA_ABORT;
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffer)
-{
-       return xscale_write_memory(target, address, 4, count, buffer);
-}
-
-int xscale_checksum_memory(struct target_s *target, u32 address, u32 count, u32* checksum)
-{
-       return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-}
-
-u32 xscale_get_ttb(target_t *target)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       u32 ttb;
-
-       xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_TTB]);
-       ttb = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_TTB].value, 0, 32);
-
-       return ttb;
-}
-
-void xscale_disable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       u32 cp15_control;
-
-       /* read cp15 control register */
-       xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CTRL]);
-       cp15_control = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_CTRL].value, 0, 32);
-
-       if (mmu)
-               cp15_control &= ~0x1U;
-
-       if (d_u_cache)
-       {
-               /* clean DCache */
-               xscale_send_u32(target, 0x50);
-               xscale_send_u32(target, xscale->cache_clean_address);
-
-               /* invalidate DCache */
-               xscale_send_u32(target, 0x51);
-
-               cp15_control &= ~0x4U;
-       }
-
-       if (i_cache)
-       {
-               /* invalidate ICache */
-               xscale_send_u32(target, 0x52);
-               cp15_control &= ~0x1000U;
-       }
-
-       /* write new cp15 control register */
-       xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_CTRL], cp15_control);
-
-       /* execute cpwait to ensure outstanding operations complete */
-       xscale_send_u32(target, 0x53);
-}
-
-void xscale_enable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       u32 cp15_control;
-
-       /* read cp15 control register */
-       xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CTRL]);
-       cp15_control = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_CTRL].value, 0, 32);
-
-       if (mmu)
-               cp15_control |= 0x1U;
-
-       if (d_u_cache)
-               cp15_control |= 0x4U;
-
-       if (i_cache)
-               cp15_control |= 0x1000U;
-
-       /* write new cp15 control register */
-       xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_CTRL], cp15_control);
-
-       /* execute cpwait to ensure outstanding operations complete */
-       xscale_send_u32(target, 0x53);
-}
-
-int xscale_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       if (xscale->force_hw_bkpts)
-               breakpoint->type = BKPT_HARD;
-
-       if (breakpoint->set)
-       {
-               WARNING("breakpoint already set");
-               return ERROR_OK;
-       }
-
-       if (breakpoint->type == BKPT_HARD)
-       {
-               u32 value = breakpoint->address | 1;
-               if (!xscale->ibcr0_used)
-               {
-                       xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_IBCR0], value);
-                       xscale->ibcr0_used = 1;
-                       breakpoint->set = 1;    /* breakpoint set on first breakpoint register */
-               }
-               else if (!xscale->ibcr1_used)
-               {
-                       xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_IBCR1], value);
-                       xscale->ibcr1_used = 1;
-                       breakpoint->set = 2;    /* breakpoint set on second breakpoint register */
-               }
-               else
-               {
-                       ERROR("BUG: no hardware comparator available");
-                       return ERROR_OK;
-               }
-       }
-       else if (breakpoint->type == BKPT_SOFT)
-       {
-               if (breakpoint->length == 4)
-               {
-                       /* keep the original instruction in target endianness */
-                       target->type->read_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr);
-                       /* write the original instruction in target endianness (arm7_9->arm_bkpt is host endian) */
-                       target_write_u32(target, breakpoint->address, xscale->arm_bkpt);
-               }
-               else
-               {
-                       /* keep the original instruction in target endianness */
-                       target->type->read_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr);
-                       /* write the original instruction in target endianness (arm7_9->arm_bkpt is host endian) */
-                       target_write_u32(target, breakpoint->address, xscale->thumb_bkpt);
-               }
-               breakpoint->set = 1;
-       }
-
-       return ERROR_OK;
-
-}
-
-int xscale_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       if (xscale->force_hw_bkpts)
-       {
-               DEBUG("forcing use of hardware breakpoint at address 0x%8.8x", breakpoint->address);
-               breakpoint->type = BKPT_HARD;
-       }
-
-       if ((breakpoint->type == BKPT_HARD) && (xscale->ibcr_available < 1))
-       {
-               INFO("no breakpoint unit available for hardware breakpoint");
-               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-       }
-       else
-       {
-               xscale->ibcr_available--;
-       }
-
-       if ((breakpoint->length != 2) && (breakpoint->length != 4))
-       {
-               INFO("only breakpoints of two (Thumb) or four (ARM) bytes length supported");
-               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       if (!breakpoint->set)
-       {
-               WARNING("breakpoint not set");
-               return ERROR_OK;
-       }
-
-       if (breakpoint->type == BKPT_HARD)
-       {
-               if (breakpoint->set == 1)
-               {
-                       xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_IBCR0], 0x0);
-                       xscale->ibcr0_used = 0;
-               }
-               else if (breakpoint->set == 2)
-               {
-                       xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_IBCR1], 0x0);
-                       xscale->ibcr1_used = 0;
-               }
-               breakpoint->set = 0;
-       }
-       else
-       {
-               /* restore original instruction (kept in target endianness) */
-               if (breakpoint->length == 4)
-               {
-                       target->type->write_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr);
-               }
-               else
-               {
-                       target->type->write_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr);
-               }
-               breakpoint->set = 0;
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       if (breakpoint->set)
-       {
-               xscale_unset_breakpoint(target, breakpoint);
-       }
-
-       if (breakpoint->type == BKPT_HARD)
-               xscale->ibcr_available++;
-
-       return ERROR_OK;
-}
-
-int xscale_set_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       u8 enable = 0;
-       reg_t *dbcon = &xscale->reg_cache->reg_list[XSCALE_DBCON];
-       u32 dbcon_value = buf_get_u32(dbcon->value, 0, 32);
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       xscale_get_reg(dbcon);
-
-       switch (watchpoint->rw)
-       {
-               case WPT_READ:
-                       enable = 0x3;
-                       break;
-               case WPT_ACCESS:
-                       enable = 0x2;
-                       break;
-               case WPT_WRITE:
-                       enable = 0x1;
-                       break;
-               default:
-                       ERROR("BUG: watchpoint->rw neither read, write nor access");
-       }
-
-       if (!xscale->dbr0_used)
-       {
-               xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_DBR0], watchpoint->address);
-               dbcon_value |= enable;
-               xscale_set_reg_u32(dbcon, dbcon_value);
-               watchpoint->set = 1;
-               xscale->dbr0_used = 1;
-       }
-       else if (!xscale->dbr1_used)
-       {
-               xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_DBR1], watchpoint->address);
-               dbcon_value |= enable << 2;
-               xscale_set_reg_u32(dbcon, dbcon_value);
-               watchpoint->set = 2;
-               xscale->dbr1_used = 1;
-       }
-       else
-       {
-               ERROR("BUG: no hardware comparator available");
-               return ERROR_OK;
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_add_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       if (xscale->dbr_available < 1)
-       {
-               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-       }
-
-       if ((watchpoint->length != 1) && (watchpoint->length != 2) && (watchpoint->length != 4))
-       {
-               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-       }
-
-       xscale->dbr_available--;
-
-       return ERROR_OK;
-}
-
-int xscale_unset_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       reg_t *dbcon = &xscale->reg_cache->reg_list[XSCALE_DBCON];
-       u32 dbcon_value = buf_get_u32(dbcon->value, 0, 32);
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       if (!watchpoint->set)
-       {
-               WARNING("breakpoint not set");
-               return ERROR_OK;
-       }
-
-       if (watchpoint->set == 1)
-       {
-               dbcon_value &= ~0x3;
-               xscale_set_reg_u32(dbcon, dbcon_value);
-               xscale->dbr0_used = 0;
-       }
-       else if (watchpoint->set == 2)
-       {
-               dbcon_value &= ~0xc;
-               xscale_set_reg_u32(dbcon, dbcon_value);
-               xscale->dbr1_used = 0;
-       }
-       watchpoint->set = 0;
-
-       return ERROR_OK;
-}
-
-int xscale_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target not halted");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       if (watchpoint->set)
-       {
-               xscale_unset_watchpoint(target, watchpoint);
-       }
-
-       xscale->dbr_available++;
-
-       return ERROR_OK;
-}
-
-void xscale_enable_watchpoints(struct target_s *target)
-{
-       watchpoint_t *watchpoint = target->watchpoints;
-
-       while (watchpoint)
-       {
-               if (watchpoint->set == 0)
-                       xscale_set_watchpoint(target, watchpoint);
-               watchpoint = watchpoint->next;
-       }
-}
-
-void xscale_enable_breakpoints(struct target_s *target)
-{
-       breakpoint_t *breakpoint = target->breakpoints;
-
-       /* set any pending breakpoints */
-       while (breakpoint)
-       {
-               if (breakpoint->set == 0)
-                       xscale_set_breakpoint(target, breakpoint);
-               breakpoint = breakpoint->next;
-       }
-}
-
-int xscale_get_reg(reg_t *reg)
-{
-       xscale_reg_t *arch_info = reg->arch_info;
-       target_t *target = arch_info->target;
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       /* DCSR, TX and RX are accessible via JTAG */
-       if (strcmp(reg->name, "XSCALE_DCSR") == 0)
-       {
-               return xscale_read_dcsr(arch_info->target);
-       }
-       else if (strcmp(reg->name, "XSCALE_TX") == 0)
-       {
-               /* 1 = consume register content */
-               return xscale_read_tx(arch_info->target, 1);
-       }
-       else if (strcmp(reg->name, "XSCALE_RX") == 0)
-       {
-               /* can't read from RX register (host -> debug handler) */
-               return ERROR_OK;
-       }
-       else if (strcmp(reg->name, "XSCALE_TXRXCTRL") == 0)
-       {
-               /* can't (explicitly) read from TXRXCTRL register */
-               return ERROR_OK;
-       }
-       else /* Other DBG registers have to be transfered by the debug handler */
-       {
-               /* send CP read request (command 0x40) */
-               xscale_send_u32(target, 0x40);
-
-               /* send CP register number */
-               xscale_send_u32(target, arch_info->dbg_handler_number);
-
-               /* read register value */
-               xscale_read_tx(target, 1);
-               buf_cpy(xscale->reg_cache->reg_list[XSCALE_TX].value, reg->value, 32);
-
-               reg->dirty = 0;
-               reg->valid = 1;
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_set_reg(reg_t *reg, u8* buf)
-{
-       xscale_reg_t *arch_info = reg->arch_info;
-       target_t *target = arch_info->target;
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       u32 value = buf_get_u32(buf, 0, 32);
-
-       /* DCSR, TX and RX are accessible via JTAG */
-       if (strcmp(reg->name, "XSCALE_DCSR") == 0)
-       {
-               buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 0, 32, value);
-               return xscale_write_dcsr(arch_info->target, -1, -1);
-       }
-       else if (strcmp(reg->name, "XSCALE_RX") == 0)
-       {
-               buf_set_u32(xscale->reg_cache->reg_list[XSCALE_RX].value, 0, 32, value);
-               return xscale_write_rx(arch_info->target);
-       }
-       else if (strcmp(reg->name, "XSCALE_TX") == 0)
-       {
-               /* can't write to TX register (debug-handler -> host) */
-               return ERROR_OK;
-       }
-       else if (strcmp(reg->name, "XSCALE_TXRXCTRL") == 0)
-       {
-               /* can't (explicitly) write to TXRXCTRL register */
-               return ERROR_OK;
-       }
-       else /* Other DBG registers have to be transfered by the debug handler */
-       {
-               /* send CP write request (command 0x41) */
-               xscale_send_u32(target, 0x41);
-
-               /* send CP register number */
-               xscale_send_u32(target, arch_info->dbg_handler_number);
-
-               /* send CP register value */
-               xscale_send_u32(target, value);
-               buf_set_u32(reg->value, 0, 32, value);
-       }
-
-       return ERROR_OK;
-}
-
-/* convenience wrapper to access XScale specific registers */
-int xscale_set_reg_u32(reg_t *reg, u32 value)
-{
-       u8 buf[4];
-
-       buf_set_u32(buf, 0, 32, value);
-
-       return xscale_set_reg(reg, buf);
-}
-
-int xscale_write_dcsr_sw(target_t *target, u32 value)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       reg_t *dcsr = &xscale->reg_cache->reg_list[XSCALE_DCSR];
-       xscale_reg_t *dcsr_arch_info = dcsr->arch_info;
-
-       /* send CP write request (command 0x41) */
-       xscale_send_u32(target, 0x41);
-
-       /* send CP register number */
-       xscale_send_u32(target, dcsr_arch_info->dbg_handler_number);
-
-       /* send CP register value */
-       xscale_send_u32(target, value);
-       buf_set_u32(dcsr->value, 0, 32, value);
-
-       return ERROR_OK;
-}
-
-int xscale_read_trace(target_t *target)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       xscale_trace_data_t **trace_data_p;
-
-       /* 258 words from debug handler
-        * 256 trace buffer entries
-        * 2 checkpoint addresses
-        */
-       u32 trace_buffer[258];
-       int is_address[256];
-       int i, j;
-
-       if (target->state != TARGET_HALTED)
-       {
-               WARNING("target must be stopped to read trace data");
-               return ERROR_TARGET_NOT_HALTED;
-       }
-
-       /* send read trace buffer command (command 0x61) */
-       xscale_send_u32(target, 0x61);
-
-       /* receive trace buffer content */
-       xscale_receive(target, trace_buffer, 258);
-
-       /* parse buffer backwards to identify address entries */
-       for (i = 255; i >= 0; i--)
-       {
-               is_address[i] = 0;
-               if (((trace_buffer[i] & 0xf0) == 0x90) ||
-                       ((trace_buffer[i] & 0xf0) == 0xd0))
-               {
-                       if (i >= 3)
-                               is_address[--i] = 1;
-                       if (i >= 2)
-                               is_address[--i] = 1;
-                       if (i >= 1)
-                               is_address[--i] = 1;
-                       if (i >= 0)
-                               is_address[--i] = 1;
-               }
-       }
-
-
-       /* search first non-zero entry */
-       for (j = 0; (j < 256) && (trace_buffer[j] == 0) && (!is_address[j]); j++)
-               ;
-
-       if (j == 256)
-       {
-               DEBUG("no trace data collected");
-               return ERROR_XSCALE_NO_TRACE_DATA;
-       }
-
-       for (trace_data_p = &xscale->trace.data; *trace_data_p; trace_data_p = &(*trace_data_p)->next)
-               ;
-
-       *trace_data_p = malloc(sizeof(xscale_trace_data_t));
-       (*trace_data_p)->next = NULL;
-       (*trace_data_p)->chkpt0 = trace_buffer[256];
-       (*trace_data_p)->chkpt1 = trace_buffer[257];
-       (*trace_data_p)->last_instruction = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
-       (*trace_data_p)->entries = malloc(sizeof(xscale_trace_entry_t) * (256 - j));
-       (*trace_data_p)->depth = 256 - j;
-
-       for (i = j; i < 256; i++)
-       {
-               (*trace_data_p)->entries[i - j].data = trace_buffer[i];
-               if (is_address[i])
-                       (*trace_data_p)->entries[i - j].type = XSCALE_TRACE_ADDRESS;
-               else
-                       (*trace_data_p)->entries[i - j].type = XSCALE_TRACE_MESSAGE;
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_read_instruction(target_t *target, arm_instruction_t *instruction)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       int i;
-       int section = -1;
-       u32 size_read;
-       u32 opcode;
-       int retval;
-
-       if (!xscale->trace.image)
-               return ERROR_TRACE_IMAGE_UNAVAILABLE;
-
-       /* search for the section the current instruction belongs to */
-       for (i = 0; i < xscale->trace.image->num_sections; i++)
-       {
-               if ((xscale->trace.image->sections[i].base_address <= xscale->trace.current_pc) &&
-                       (xscale->trace.image->sections[i].base_address + xscale->trace.image->sections[i].size > xscale->trace.current_pc))
-               {
-                       section = i;
-                       break;
-               }
-       }
-
-       if (section == -1)
-       {
-               /* current instruction couldn't be found in the image */
-               return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;
-       }
-
-       if (xscale->trace.core_state == ARMV4_5_STATE_ARM)
-       {
-               u8 buf[4];
-               if ((retval = image_read_section(xscale->trace.image, section,
-                       xscale->trace.current_pc - xscale->trace.image->sections[section].base_address,
-                       4, buf, &size_read)) != ERROR_OK)
-               {
-                       ERROR("error while reading instruction: %i", retval);
-                       return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;
-               }
-               opcode = target_buffer_get_u32(target, buf);
-               arm_evaluate_opcode(opcode, xscale->trace.current_pc, instruction);
-       }
-       else if (xscale->trace.core_state == ARMV4_5_STATE_THUMB)
-       {
-               u8 buf[2];
-               if ((retval = image_read_section(xscale->trace.image, section,
-                       xscale->trace.current_pc - xscale->trace.image->sections[section].base_address,
-                       2, buf, &size_read)) != ERROR_OK)
-               {
-                       ERROR("error while reading instruction: %i", retval);
-                       return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;
-               }
-               opcode = target_buffer_get_u16(target, buf);
-               thumb_evaluate_opcode(opcode, xscale->trace.current_pc, instruction);
-       }
-       else
-       {
-               ERROR("BUG: unknown core state encountered");
-               exit(-1);
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_branch_address(xscale_trace_data_t *trace_data, int i, u32 *target)
-{
-       /* if there are less than four entries prior to the indirect branch message
-        * we can't extract the address */
-       if (i < 4)
-       {
-               return -1;
-       }
-
-       *target = (trace_data->entries[i-1].data) | (trace_data->entries[i-2].data << 8) |
-                               (trace_data->entries[i-3].data << 16) | (trace_data->entries[i-4].data << 24);
-
-       return 0;
-}
-
-int xscale_analyze_trace(target_t *target, command_context_t *cmd_ctx)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-       int next_pc_ok = 0;
-       u32 next_pc = 0x0;
-       xscale_trace_data_t *trace_data = xscale->trace.data;
-       int retval;
-
-       while (trace_data)
-       {
-               int i, chkpt;
-               int rollover;
-               int branch;
-               int exception;
-               xscale->trace.core_state = ARMV4_5_STATE_ARM;
-
-               chkpt = 0;
-               rollover = 0;
-
-               for (i = 0; i < trace_data->depth; i++)
-               {
-                       next_pc_ok = 0;
-                       branch = 0;
-                       exception = 0;
-
-                       if (trace_data->entries[i].type == XSCALE_TRACE_ADDRESS)
-                               continue;
-
-                       switch ((trace_data->entries[i].data & 0xf0) >> 4)
-                       {
-                               case 0:         /* Exceptions */
-                               case 1:
-                               case 2:
-                               case 3:
-                               case 4:
-                               case 5:
-                               case 6:
-                               case 7:
-                                       exception = (trace_data->entries[i].data & 0x70) >> 4;
-                                       next_pc_ok = 1;
-                                       next_pc = (trace_data->entries[i].data & 0xf0) >> 2;
-                                       command_print(cmd_ctx, "--- exception %i ---", (trace_data->entries[i].data & 0xf0) >> 4);
-                                       break;
-                               case 8:         /* Direct Branch */
-                                       branch = 1;
-                                       break;
-                               case 9:         /* Indirect Branch */
-                                       branch = 1;
-                                       if (xscale_branch_address(trace_data, i, &next_pc) == 0)
-                                       {
-                                               next_pc_ok = 1;
-                                       }
-                                       break;
-                               case 13:        /* Checkpointed Indirect Branch */
-                                       if (xscale_branch_address(trace_data, i, &next_pc) == 0)
-                                       {
-                                               next_pc_ok = 1;
-                                               if (((chkpt == 0) && (next_pc != trace_data->chkpt0))
-                                                       || ((chkpt == 1) && (next_pc != trace_data->chkpt1)))
-                                                       WARNING("checkpointed indirect branch target address doesn't match checkpoint");
-                                       }
-                                       /* explicit fall-through */
-                               case 12:        /* Checkpointed Direct Branch */
-                                       branch = 1;
-                                       if (chkpt == 0)
-                                       {
-                                               next_pc_ok = 1;
-                                               next_pc = trace_data->chkpt0;
-                                               chkpt++;
-                                       }
-                                       else if (chkpt == 1)
-                                       {
-                                               next_pc_ok = 1;
-                                               next_pc = trace_data->chkpt0;
-                                               chkpt++;
-                                       }
-                                       else
-                                       {
-                                               WARNING("more than two checkpointed branches encountered");
-                                       }
-                                       break;
-                               case 15:        /* Roll-over */
-                                       rollover++;
-                                       continue;
-                               default:        /* Reserved */
-                                       command_print(cmd_ctx, "--- reserved trace message ---");
-                                       ERROR("BUG: trace message %i is reserved", (trace_data->entries[i].data & 0xf0) >> 4);
-                                       return ERROR_OK;
-                       }
-
-                       if (xscale->trace.pc_ok)
-                       {
-                               int executed = (trace_data->entries[i].data & 0xf) + rollover * 16;
-                               arm_instruction_t instruction;
-
-                               if ((exception == 6) || (exception == 7))
-                               {
-                                       /* IRQ or FIQ exception, no instruction executed */
-                                       executed -= 1;
-                               }
-
-                               while (executed-- >= 0)
-                               {
-                                       if ((retval = xscale_read_instruction(target, &instruction)) != ERROR_OK)
-                                       {
-                                               /* can't continue tracing with no image available */
-                                               if (retval == ERROR_TRACE_IMAGE_UNAVAILABLE)
-                                               {
-                                                       return retval;
-                                               }
-                                               else if (retval == ERROR_TRACE_INSTRUCTION_UNAVAILABLE)
-                                               {
-                                                       /* TODO: handle incomplete images */
-                                               }
-                                       }
-
-                                       /* a precise abort on a load to the PC is included in the incremental
-                                        * word count, other instructions causing data aborts are not included
-                                        */
-                                       if ((executed == 0) && (exception == 4)
-                                               && ((instruction.type >= ARM_LDR) && (instruction.type <= ARM_LDM)))
-                                       {
-                                               if ((instruction.type == ARM_LDM)
-                                                       && ((instruction.info.load_store_multiple.register_list & 0x8000) == 0))
-                                               {
-                                                       executed--;
-                                               }
-                                               else if (((instruction.type >= ARM_LDR) && (instruction.type <= ARM_LDRSH))
-                                                       && (instruction.info.load_store.Rd != 15))
-                                               {
-                                                       executed--;
-                                               }
-                                       }
-
-                                       /* only the last instruction executed
-                                        * (the one that caused the control flow change)
-                                        * could be a taken branch
-                                        */
-                                       if (((executed == -1) && (branch == 1)) &&
-                                               (((instruction.type == ARM_B) ||
-                                                       (instruction.type == ARM_BL) ||
-                                                       (instruction.type == ARM_BLX)) &&
-                                                       (instruction.info.b_bl_bx_blx.target_address != -1)))
-                                       {
-                                               xscale->trace.current_pc = instruction.info.b_bl_bx_blx.target_address;
-                                       }
-                                       else
-                                       {
-                                               xscale->trace.current_pc += (xscale->trace.core_state == ARMV4_5_STATE_ARM) ? 4 : 2;
-                                       }
-                                       command_print(cmd_ctx, "%s", instruction.text);
-                               }
-
-                               rollover = 0;
-                       }
-
-                       if (next_pc_ok)
-                       {
-                               xscale->trace.current_pc = next_pc;
-                               xscale->trace.pc_ok = 1;
-                       }
-               }
-
-               for (; xscale->trace.current_pc < trace_data->last_instruction; xscale->trace.current_pc += (xscale->trace.core_state == ARMV4_5_STATE_ARM) ? 4 : 2)
-               {
-                       arm_instruction_t instruction;
-                       if ((retval = xscale_read_instruction(target, &instruction)) != ERROR_OK)
-                       {
-                               /* can't continue tracing with no image available */
-                               if (retval == ERROR_TRACE_IMAGE_UNAVAILABLE)
-                               {
-                                       return retval;
-                               }
-                               else if (retval == ERROR_TRACE_INSTRUCTION_UNAVAILABLE)
-                               {
-                                       /* TODO: handle incomplete images */
-                               }
-                       }
-                       command_print(cmd_ctx, "%s", instruction.text);
-               }
-
-               trace_data = trace_data->next;
-       }
-
-       return ERROR_OK;
-}
-
-void xscale_build_reg_cache(target_t *target)
-{
-       /* get pointers to arch-specific information */
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       reg_cache_t **cache_p = register_get_last_cache_p(&target->reg_cache);
-       xscale_reg_t *arch_info = malloc(sizeof(xscale_reg_arch_info));
-       int i;
-       int num_regs = sizeof(xscale_reg_arch_info) / sizeof(xscale_reg_t);
-
-       (*cache_p) = armv4_5_build_reg_cache(target, armv4_5);
-       armv4_5->core_cache = (*cache_p);
-
-       /* register a register arch-type for XScale dbg registers only once */
-       if (xscale_reg_arch_type == -1)
-               xscale_reg_arch_type = register_reg_arch_type(xscale_get_reg, xscale_set_reg);
-
-       (*cache_p)->next = malloc(sizeof(reg_cache_t));
-       cache_p = &(*cache_p)->next;
-
-       /* fill in values for the xscale reg cache */
-       (*cache_p)->name = "XScale registers";
-       (*cache_p)->next = NULL;
-       (*cache_p)->reg_list = malloc(num_regs * sizeof(reg_t));
-       (*cache_p)->num_regs = num_regs;
-
-       for (i = 0; i < num_regs; i++)
-       {
-               (*cache_p)->reg_list[i].name = xscale_reg_list[i];
-               (*cache_p)->reg_list[i].value = calloc(4, 1);
-               (*cache_p)->reg_list[i].dirty = 0;
-               (*cache_p)->reg_list[i].valid = 0;
-               (*cache_p)->reg_list[i].size = 32;
-               (*cache_p)->reg_list[i].bitfield_desc = NULL;
-               (*cache_p)->reg_list[i].num_bitfields = 0;
-               (*cache_p)->reg_list[i].arch_info = &arch_info[i];
-               (*cache_p)->reg_list[i].arch_type = xscale_reg_arch_type;
-               arch_info[i] = xscale_reg_arch_info[i];
-               arch_info[i].target = target;
-       }
-
-       xscale->reg_cache = (*cache_p);
-}
-
-int xscale_init_target(struct command_context_s *cmd_ctx, struct target_s *target)
-{
-       if (startup_mode != DAEMON_RESET)
-       {
-               ERROR("XScale target requires a reset");
-               ERROR("Reset target to enable debug");
-       }
-
-       /* assert TRST once during startup */
-       jtag_add_reset(1, 0);
-       jtag_add_sleep(5000);
-       jtag_add_reset(0, 0);
-       jtag_execute_queue();
-
-       return ERROR_OK;
-}
-
-int xscale_quit()
-{
-
-       return ERROR_OK;
-}
-
-int xscale_init_arch_info(target_t *target, xscale_common_t *xscale, int chain_pos, char *variant)
-{
-       armv4_5_common_t *armv4_5;
-       u32 high_reset_branch, low_reset_branch;
-       int i;
-
-       armv4_5 = &xscale->armv4_5_common;
-
-       /* store architecture specfic data (none so far) */
-       xscale->arch_info = NULL;
-       xscale->common_magic = XSCALE_COMMON_MAGIC;
-
-       /* remember the variant (PXA25x, PXA27x, IXP42x, ...) */
-       xscale->variant = strdup(variant);
-
-       /* prepare JTAG information for the new target */
-       xscale->jtag_info.chain_pos = chain_pos;
-       jtag_register_event_callback(xscale_jtag_callback, target);
-
-       xscale->jtag_info.dbgrx = 0x02;
-       xscale->jtag_info.dbgtx = 0x10;
-       xscale->jtag_info.dcsr = 0x09;
-       xscale->jtag_info.ldic = 0x07;
-
-       if ((strcmp(xscale->variant, "pxa250") == 0) ||
-               (strcmp(xscale->variant, "pxa255") == 0) ||
-               (strcmp(xscale->variant, "pxa26x") == 0))
-       {
-               xscale->jtag_info.ir_length = 5;
-       }
-       else if ((strcmp(xscale->variant, "pxa27x") == 0) ||
-               (strcmp(xscale->variant, "ixp42x") == 0) ||
-               (strcmp(xscale->variant, "ixp45x") == 0) ||
-               (strcmp(xscale->variant, "ixp46x") == 0))
-       {
-               xscale->jtag_info.ir_length = 7;
-       }
-
-       /* the debug handler isn't installed (and thus not running) at this time */
-       xscale->handler_installed = 0;
-       xscale->handler_running = 0;
-       xscale->handler_address = 0xfe000800;
-
-       /* clear the vectors we keep locally for reference */
-       memset(xscale->low_vectors, 0, sizeof(xscale->low_vectors));
-       memset(xscale->high_vectors, 0, sizeof(xscale->high_vectors));
-
-       /* no user-specified vectors have been configured yet */
-       xscale->static_low_vectors_set = 0x0;
-       xscale->static_high_vectors_set = 0x0;
-
-       /* calculate branches to debug handler */
-       low_reset_branch = (xscale->handler_address + 0x20 - 0x0 - 0x8) >> 2;
-       high_reset_branch = (xscale->handler_address + 0x20 - 0xffff0000 - 0x8) >> 2;
-
-       xscale->low_vectors[0] = ARMV4_5_B((low_reset_branch & 0xffffff), 0);
-       xscale->high_vectors[0] = ARMV4_5_B((high_reset_branch & 0xffffff), 0);
-
-       for (i = 1; i <= 7; i++)
-       {
-               xscale->low_vectors[i] = ARMV4_5_B(0xfffffe, 0);
-               xscale->high_vectors[i] = ARMV4_5_B(0xfffffe, 0);
-       }
-
-       /* 64kB aligned region used for DCache cleaning */
-       xscale->cache_clean_address = 0xfffe0000;
-
-       xscale->hold_rst = 0;
-       xscale->external_debug_break = 0;
-
-       xscale->force_hw_bkpts = 1;
-
-       xscale->ibcr_available = 2;
-       xscale->ibcr0_used = 0;
-       xscale->ibcr1_used = 0;
-
-       xscale->dbr_available = 2;
-       xscale->dbr0_used = 0;
-       xscale->dbr1_used = 0;
-
-       xscale->arm_bkpt = ARMV5_BKPT(0x0);
-       xscale->thumb_bkpt = ARMV5_T_BKPT(0x0) & 0xffff;
-
-       xscale->vector_catch = 0x1;
-
-       xscale->trace.capture_status = TRACE_IDLE;
-       xscale->trace.data = NULL;
-       xscale->trace.image = NULL;
-       xscale->trace.buffer_enabled = 0;
-       xscale->trace.buffer_fill = 0;
-
-       /* prepare ARMv4/5 specific information */
-       armv4_5->arch_info = xscale;
-       armv4_5->read_core_reg = xscale_read_core_reg;
-       armv4_5->write_core_reg = xscale_write_core_reg;
-       armv4_5->full_context = xscale_full_context;
-
-       armv4_5_init_arch_info(target, armv4_5);
-
-       xscale->armv4_5_mmu.armv4_5_cache.ctype = -1;
-       xscale->armv4_5_mmu.get_ttb = xscale_get_ttb;
-       xscale->armv4_5_mmu.read_memory = xscale_read_memory;
-       xscale->armv4_5_mmu.write_memory = xscale_write_memory;
-       xscale->armv4_5_mmu.disable_mmu_caches = xscale_disable_mmu_caches;
-       xscale->armv4_5_mmu.enable_mmu_caches = xscale_enable_mmu_caches;
-       xscale->armv4_5_mmu.has_tiny_pages = 1;
-       xscale->armv4_5_mmu.mmu_enabled = 0;
-
-       xscale->fast_memory_access = 0;
-
-       return ERROR_OK;
-}
-
-/* target xscale <endianess> <startup_mode> <chain_pos> <variant> */
-int xscale_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)
-{
-       int chain_pos;
-       char *variant = NULL;
-       xscale_common_t *xscale = malloc(sizeof(xscale_common_t));
-
-       if (argc < 5)
-       {
-               ERROR("'target xscale' requires four arguments: <endianess> <startup_mode> <chain_pos> <variant>");
-               return ERROR_OK;
-       }
-
-       chain_pos = strtoul(args[3], NULL, 0);
-
-       variant = args[4];
-
-       xscale_init_arch_info(target, xscale, chain_pos, variant);
-       xscale_build_reg_cache(target);
-
-       return ERROR_OK;
-}
-
-int xscale_handle_debug_handler_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = NULL;
-       armv4_5_common_t *armv4_5;
-       xscale_common_t *xscale;
-
-       u32 handler_address;
-
-       if (argc < 2)
-       {
-               ERROR("'xscale debug_handler <target#> <address>' command takes two required operands");
-               return ERROR_OK;
-       }
-
-       if ((target = get_target_by_num(strtoul(args[0], NULL, 0))) == NULL)
-       {
-               ERROR("no target '%s' configured", args[0]);
-               return ERROR_OK;
-       }
-
-       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
-       {
-               return ERROR_OK;
-       }
-
-       handler_address = strtoul(args[1], NULL, 0);
-
-       if (((handler_address >= 0x800) && (handler_address <= 0x1fef800)) ||
-               ((handler_address >= 0xfe000800) && (handler_address <= 0xfffff800)))
-       {
-               xscale->handler_address = handler_address;
-       }
-       else
-       {
-               ERROR("xscale debug_handler <address> must be between 0x800 and 0x1fef800 or between 0xfe000800 and 0xfffff800");
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_handle_cache_clean_address_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = NULL;
-       armv4_5_common_t *armv4_5;
-       xscale_common_t *xscale;
-
-       u32 cache_clean_address;
-
-       if (argc < 2)
-       {
-               ERROR("'xscale cache_clean_address <target#> <address>' command takes two required operands");
-               return ERROR_OK;
-       }
-
-       if ((target = get_target_by_num(strtoul(args[0], NULL, 0))) == NULL)
-       {
-               ERROR("no target '%s' configured", args[0]);
-               return ERROR_OK;
-       }
-
-       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
-       {
-               return ERROR_OK;
-       }
-
-       cache_clean_address = strtoul(args[1], NULL, 0);
-
-       if (cache_clean_address & 0xffff)
-       {
-               ERROR("xscale cache_clean_address <address> must be 64kb aligned");
-       }
-       else
-       {
-               xscale->cache_clean_address = cache_clean_address;
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_handle_cache_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       xscale_common_t *xscale;
-
-       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
-       {
-               return ERROR_OK;
-       }
-
-       return armv4_5_handle_cache_info_command(cmd_ctx, &xscale->armv4_5_mmu.armv4_5_cache);
-}
-
-static int xscale_virt2phys(struct target_s *target, u32 virtual, u32 *physical)
-{
-       armv4_5_common_t *armv4_5;
-       xscale_common_t *xscale;
-       int retval;
-       int type;
-       u32 cb;
-       int domain;
-       u32 ap;
-       
-       if ((retval = xscale_get_arch_pointers(target, &armv4_5, &xscale)) != ERROR_OK)
-       {
-               return retval;
-       }
-       u32 ret = armv4_5_mmu_translate_va(target, &xscale->armv4_5_mmu, virtual, &type, &cb, &domain, &ap);
-       if (type == -1)
-       {
-               return ret;
-       }
-       
-       *physical = ret;
-       return ERROR_OK;
-}
-
-static int xscale_mmu(struct target_s *target, int *enabled)
-{
-       armv4_5_common_t *armv4_5 = target->arch_info;
-       xscale_common_t *xscale = armv4_5->arch_info;
-
-       if (target->state != TARGET_HALTED)
-       {
-               ERROR("Target not halted");
-               return ERROR_TARGET_INVALID;
-       }
-       
-       *enabled = xscale->armv4_5_mmu.mmu_enabled;
-       return ERROR_OK;
-}
-
-int xscale_handle_mmu_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       xscale_common_t *xscale;
-
-       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
-       {
-               return ERROR_OK;
-       }
-
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-
-       if (argc >= 1)
-       {
-               if (strcmp("enable", args[0]) == 0)
-               {
-                       xscale_enable_mmu_caches(target, 1, 0, 0);
-                       xscale->armv4_5_mmu.mmu_enabled = 1;
-               }
-               else if (strcmp("disable", args[0]) == 0)
-               {
-                       xscale_disable_mmu_caches(target, 1, 0, 0);
-                       xscale->armv4_5_mmu.mmu_enabled = 0;
-               }
-       }
-
-       command_print(cmd_ctx, "mmu %s", (xscale->armv4_5_mmu.mmu_enabled) ? "enabled" : "disabled");
-
-       return ERROR_OK;
-}
-
-int xscale_handle_idcache_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       xscale_common_t *xscale;
-       int icache = 0, dcache = 0;
-
-       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
-       {
-               return ERROR_OK;
-       }
-
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-
-       if (strcmp(cmd, "icache") == 0)
-               icache = 1;
-       else if (strcmp(cmd, "dcache") == 0)
-               dcache = 1;
-
-       if (argc >= 1)
-       {
-               if (strcmp("enable", args[0]) == 0)
-               {
-                       xscale_enable_mmu_caches(target, 0, dcache, icache);
-
-                       if (icache)
-                               xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 1;
-                       else if (dcache)
-                               xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 1;
-               }
-               else if (strcmp("disable", args[0]) == 0)
-               {
-                       xscale_disable_mmu_caches(target, 0, dcache, icache);
-
-                       if (icache)
-                               xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;
-                       else if (dcache)
-                               xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 0;
-               }
-       }
-
-       if (icache)
-               command_print(cmd_ctx, "icache %s", (xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled) ? "enabled" : "disabled");
-
-       if (dcache)
-               command_print(cmd_ctx, "dcache %s", (xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled) ? "enabled" : "disabled");
-
-       return ERROR_OK;
-}
-
-int xscale_handle_vector_catch_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       xscale_common_t *xscale;
-
-       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
-       {
-               return ERROR_OK;
-       }
-
-       if (argc < 1)
-       {
-               command_print(cmd_ctx, "usage: xscale vector_catch [mask]");
-       }
-       else
-       {
-               xscale->vector_catch = strtoul(args[0], NULL, 0);
-               buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 16, 8, xscale->vector_catch);
-               xscale_write_dcsr(target, -1, -1);
-       }
-
-       command_print(cmd_ctx, "vector catch mask: 0x%2.2x", xscale->vector_catch);
-
-       return ERROR_OK;
-}
-
-int xscale_handle_force_hw_bkpts_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       xscale_common_t *xscale;
-
-       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
-       {
-               return ERROR_OK;
-       }
-
-       if ((argc >= 1) && (strcmp("enable", args[0]) == 0))
-       {
-               xscale->force_hw_bkpts = 1;
-       }
-       else if ((argc >= 1) && (strcmp("disable", args[0]) == 0))
-       {
-               xscale->force_hw_bkpts = 0;
-       }
-       else
-       {
-               command_print(cmd_ctx, "usage: xscale force_hw_bkpts <enable|disable>");
-       }
-
-       command_print(cmd_ctx, "force hardware breakpoints %s", (xscale->force_hw_bkpts) ? "enabled" : "disabled");
-
-       return ERROR_OK;
-}
-
-int xscale_handle_trace_buffer_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       xscale_common_t *xscale;
-       u32 dcsr_value;
-
-       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
-       {
-               return ERROR_OK;
-       }
-
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-
-       if ((argc >= 1) && (strcmp("enable", args[0]) == 0))
-       {
-               xscale_trace_data_t *td, *next_td;
-               xscale->trace.buffer_enabled = 1;
-
-               /* free old trace data */
-               td = xscale->trace.data;
-               while (td)
-               {
-                       next_td = td->next;
-
-                       if (td->entries)
-                               free(td->entries);
-                       free(td);
-                       td = next_td;
-               }
-               xscale->trace.data = NULL;
-       }
-       else if ((argc >= 1) && (strcmp("disable", args[0]) == 0))
-       {
-               xscale->trace.buffer_enabled = 0;
-       }
-
-       if ((argc >= 2) && (strcmp("fill", args[1]) == 0))
-       {
-               if (argc >= 3)
-                       xscale->trace.buffer_fill = strtoul(args[2], NULL, 0);
-               else
-                       xscale->trace.buffer_fill = 1;
-       }
-       else if ((argc >= 2) && (strcmp("wrap", args[1]) == 0))
-       {
-               xscale->trace.buffer_fill = -1;
-       }
-
-       if (xscale->trace.buffer_enabled)
-       {
-               /* if we enable the trace buffer in fill-once
-                * mode we know the address of the first instruction */
-               xscale->trace.pc_ok = 1;
-               xscale->trace.current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);
-       }
-       else
-       {
-               /* otherwise the address is unknown, and we have no known good PC */
-               xscale->trace.pc_ok = 0;
-       }
-
-       command_print(cmd_ctx, "trace buffer %s (%s)",
-               (xscale->trace.buffer_enabled) ? "enabled" : "disabled",
-               (xscale->trace.buffer_fill > 0) ? "fill" : "wrap");
-
-       dcsr_value = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 0, 32);
-       if (xscale->trace.buffer_fill >= 0)
-               xscale_write_dcsr_sw(target, (dcsr_value & 0xfffffffc) | 2);
-       else
-               xscale_write_dcsr_sw(target, dcsr_value & 0xfffffffc);
-
-       return ERROR_OK;
-}
-
-int xscale_handle_trace_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target;
-       armv4_5_common_t *armv4_5;
-       xscale_common_t *xscale;
-
-       if (argc < 1)
-       {
-               command_print(cmd_ctx, "usage: xscale trace_image <file> [base address] [type]");
-               return ERROR_OK;
-       }
-
-       target = get_current_target(cmd_ctx);
-
-       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
-       {
-               return ERROR_OK;
-       }
-
-       if (xscale->trace.image)
-       {
-               image_close(xscale->trace.image);
-               free(xscale->trace.image);
-               command_print(cmd_ctx, "previously loaded image found and closed");
-       }
-
-       xscale->trace.image = malloc(sizeof(image_t));
-       xscale->trace.image->base_address_set = 0;
-       xscale->trace.image->start_address_set = 0;
-
-       /* a base address isn't always necessary, default to 0x0 (i.e. don't relocate) */
-       if (argc >= 2)
-       {
-               xscale->trace.image->base_address_set = 1;
-               xscale->trace.image->base_address = strtoul(args[1], NULL, 0);
-       }
-       else
-       {
-               xscale->trace.image->base_address_set = 0;
-       }
-
-       if (image_open(xscale->trace.image, args[0], (argc >= 3) ? args[2] : NULL) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "image opening error: %s", xscale->trace.image->error_str);
-               free(xscale->trace.image);
-               xscale->trace.image = NULL;
-               return ERROR_OK;
-       }
-
-       return ERROR_OK;
-}
-
-int xscale_handle_dump_trace_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       xscale_common_t *xscale;
-       xscale_trace_data_t *trace_data;
-       fileio_t file;
-
-       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
-       {
-               return ERROR_OK;
-       }
-
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-
-       if (argc < 1)
-       {
-               command_print(cmd_ctx, "usage: xscale dump_trace <file>");
-               return ERROR_OK;
-       }
-
-       trace_data = xscale->trace.data;
-
-       if (!trace_data)
-       {
-               command_print(cmd_ctx, "no trace data collected");
-               return ERROR_OK;
-       }
-
-       if (fileio_open(&file, args[0], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK)
-       {
-               command_print(cmd_ctx, "file open error: %s", file.error_str);
-               return ERROR_OK;
-       }
-
-       while (trace_data)
-       {
-               int i;
-
-               fileio_write_u32(&file, trace_data->chkpt0);
-               fileio_write_u32(&file, trace_data->chkpt1);
-               fileio_write_u32(&file, trace_data->last_instruction);
-               fileio_write_u32(&file, trace_data->depth);
-
-               for (i = 0; i < trace_data->depth; i++)
-                       fileio_write_u32(&file, trace_data->entries[i].data | ((trace_data->entries[i].type & 0xffff) << 16));
-
-               trace_data = trace_data->next;
-       }
-
-       fileio_close(&file);
-
-       return ERROR_OK;
-}
-
-int xscale_handle_analyze_trace_buffer_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       xscale_common_t *xscale;
-
-       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
-       {
-               return ERROR_OK;
-       }
-
-       xscale_analyze_trace(target, cmd_ctx);
-
-       return ERROR_OK;
-}
-
-int xscale_handle_cp15(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       xscale_common_t *xscale;
-       
-       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
-       {
-               return ERROR_OK;
-       }
-       
-       if (target->state != TARGET_HALTED)
-       {
-               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);
-               return ERROR_OK;
-       }
-       u32 reg_no = 0;
-       reg_t *reg = NULL;
-       if(argc > 0)
-       {
-               reg_no = strtoul(args[0], NULL, 0);
-               /*translate from xscale cp15 register no to openocd register*/
-               switch(reg_no)
-               {
-               case 0:
-                       reg_no = XSCALE_MAINID;
-                       break;
-               case 1:
-                       reg_no = XSCALE_CTRL;
-                       break;
-               case 2:
-                       reg_no = XSCALE_TTB;
-                       break; 
-               case 3:
-                       reg_no = XSCALE_DAC;
-                       break;
-               case 5:
-                       reg_no = XSCALE_FSR;
-                       break;
-               case 6:
-                       reg_no = XSCALE_FAR;
-                       break;
-               case 13:
-                       reg_no = XSCALE_PID;
-                       break;
-               case 15:
-                       reg_no = XSCALE_CPACCESS;
-                       break;
-               default:
-                       command_print(cmd_ctx, "invalid register number");
-                       return ERROR_INVALID_ARGUMENTS;
-               }
-               reg = &xscale->reg_cache->reg_list[reg_no];
-               
-       }
-       if(argc == 1)
-       {
-               u32 value;
-               
-               /* read cp15 control register */
-               xscale_get_reg(reg);
-               value = buf_get_u32(reg->value, 0, 32);
-               command_print(cmd_ctx, "%s (/%i): 0x%x", reg->name, reg->size, value);
-       }
-       else if(argc == 2)
-       {   
-
-               u32 value = strtoul(args[1], NULL, 0);
-               
-               /* send CP write request (command 0x41) */
-               xscale_send_u32(target, 0x41);
-               
-               /* send CP register number */
-               xscale_send_u32(target, reg_no);
-               
-               /* send CP register value */
-               xscale_send_u32(target, value);
-               
-               /* execute cpwait to ensure outstanding operations complete */
-               xscale_send_u32(target, 0x53);
-       }
-       else
-       {
-               command_print(cmd_ctx, "usage: cp15 [register]<, [value]>");    
-       }
-       
-       return ERROR_OK;
-}
-
-int handle_xscale_fast_memory_access_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
-{
-       target_t *target = get_current_target(cmd_ctx);
-       armv4_5_common_t *armv4_5;
-       xscale_common_t *xscale;
-       
-       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)
-       {
-               return ERROR_OK;
-       }
-       
-       if (argc == 1)
-       {
-               if (strcmp("enable", args[0]) == 0)
-               {
-                       xscale->fast_memory_access = 1;
-               }
-               else if (strcmp("disable", args[0]) == 0)
-               {
-                       xscale->fast_memory_access = 0;
-               }
-               else
-               {
-                       return ERROR_COMMAND_SYNTAX_ERROR;
-               }
-       } else if (argc!=0)
-       {
-               return ERROR_COMMAND_SYNTAX_ERROR;
-       }
-               
-       command_print(cmd_ctx, "fast memory access is %s", (xscale->fast_memory_access) ? "enabled" : "disabled");
-
-       return ERROR_OK;
-}
-
-int xscale_register_commands(struct command_context_s *cmd_ctx)
-{
-       command_t *xscale_cmd;
-
-       xscale_cmd = register_command(cmd_ctx, NULL, "xscale", NULL, COMMAND_ANY, "xscale specific commands");
-
-       register_command(cmd_ctx, xscale_cmd, "debug_handler", xscale_handle_debug_handler_command, COMMAND_ANY, "'xscale debug_handler <target#> <address>' command takes two required operands");
-       register_command(cmd_ctx, xscale_cmd, "cache_clean_address", xscale_handle_cache_clean_address_command, COMMAND_ANY, NULL);
-
-       register_command(cmd_ctx, xscale_cmd, "cache_info", xscale_handle_cache_info_command, COMMAND_EXEC, NULL);
-       register_command(cmd_ctx, xscale_cmd, "mmu", xscale_handle_mmu_command, COMMAND_EXEC, "['enable'|'disable'] the MMU");
-       register_command(cmd_ctx, xscale_cmd, "icache", xscale_handle_idcache_command, COMMAND_EXEC, "['enable'|'disable'] the ICache");
-       register_command(cmd_ctx, xscale_cmd, "dcache", xscale_handle_idcache_command, COMMAND_EXEC, "['enable'|'disable'] the DCache");
-
-       register_command(cmd_ctx, xscale_cmd, "vector_catch", xscale_handle_idcache_command, COMMAND_EXEC, "<mask> of vectors that should be catched");
-
-       register_command(cmd_ctx, xscale_cmd, "trace_buffer", xscale_handle_trace_buffer_command, COMMAND_EXEC, "<enable|disable> ['fill' [n]|'wrap']");
-
-       register_command(cmd_ctx, xscale_cmd, "dump_trace", xscale_handle_dump_trace_command, COMMAND_EXEC, "dump content of trace buffer to <file>");
-       register_command(cmd_ctx, xscale_cmd, "analyze_trace", xscale_handle_analyze_trace_buffer_command, COMMAND_EXEC, "analyze content of trace buffer");
-       register_command(cmd_ctx, xscale_cmd, "trace_image", xscale_handle_trace_image_command,
-               COMMAND_EXEC, "load image from <file> [base address]");
-
-       register_command(cmd_ctx, xscale_cmd, "cp15", xscale_handle_cp15, COMMAND_EXEC, "access coproc 15 <register> [value]");
-       register_command(cmd_ctx, xscale_cmd, "fast_memory_access", handle_xscale_fast_memory_access_command,
-                COMMAND_ANY, "use fast memory accesses instead of slower but potentially unsafe slow accesses <enable|disable>");
-       
-       armv4_5_register_commands(cmd_ctx);
-
-       return ERROR_OK;
-}
+/***************************************************************************\r
+ *   Copyright (C) 2006, 2007 by Dominic Rath                              *\r
+ *   Dominic.Rath@gmx.de                                                   *\r
+ *                                                                         *\r
+ *   This program is free software; you can redistribute it and/or modify  *\r
+ *   it under the terms of the GNU General Public License as published by  *\r
+ *   the Free Software Foundation; either version 2 of the License, or     *\r
+ *   (at your option) any later version.                                   *\r
+ *                                                                         *\r
+ *   This program is distributed in the hope that it will be useful,       *\r
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *\r
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *\r
+ *   GNU General Public License for more details.                          *\r
+ *                                                                         *\r
+ *   You should have received a copy of the GNU General Public License     *\r
+ *   along with this program; if not, write to the                         *\r
+ *   Free Software Foundation, Inc.,                                       *\r
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *\r
+ ***************************************************************************/\r
+#ifdef HAVE_CONFIG_H\r
+#include "config.h"\r
+#endif\r
+\r
+#include "replacements.h"\r
+\r
+#include "xscale.h"\r
+\r
+#include "register.h"\r
+#include "target.h"\r
+#include "armv4_5.h"\r
+#include "arm_simulator.h"\r
+#include "arm_disassembler.h"\r
+#include "log.h"\r
+#include "jtag.h"\r
+#include "binarybuffer.h"\r
+#include "time_support.h"\r
+#include "breakpoints.h"\r
+#include "fileio.h"\r
+\r
+#include <stdlib.h>\r
+#include <string.h>\r
+\r
+#include <sys/types.h>\r
+#include <unistd.h>\r
+#include <errno.h>\r
+\r
+\r
+/* cli handling */\r
+int xscale_register_commands(struct command_context_s *cmd_ctx);\r
+\r
+/* forward declarations */\r
+int xscale_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target);\r
+int xscale_init_target(struct command_context_s *cmd_ctx, struct target_s *target);\r
+int xscale_quit();\r
+\r
+int xscale_arch_state(struct target_s *target);\r
+int xscale_poll(target_t *target);\r
+int xscale_halt(target_t *target);\r
+int xscale_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution);\r
+int xscale_step(struct target_s *target, int current, u32 address, int handle_breakpoints);\r
+int xscale_debug_entry(target_t *target);\r
+int xscale_restore_context(target_t *target);\r
+\r
+int xscale_assert_reset(target_t *target);\r
+int xscale_deassert_reset(target_t *target);\r
+int xscale_soft_reset_halt(struct target_s *target);\r
+int xscale_prepare_reset_halt(struct target_s *target);\r
+\r
+int xscale_set_reg_u32(reg_t *reg, u32 value);\r
+\r
+int xscale_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode);\r
+int xscale_write_core_reg(struct target_s *target, int num, enum armv4_5_mode mode, u32 value);\r
+\r
+int xscale_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);\r
+int xscale_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer);\r
+int xscale_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffer);\r
+int xscale_checksum_memory(struct target_s *target, u32 address, u32 count, u32* checksum);\r
+\r
+int xscale_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint);\r
+int xscale_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint);\r
+int xscale_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint);\r
+int xscale_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint);\r
+int xscale_add_watchpoint(struct target_s *target, watchpoint_t *watchpoint);\r
+int xscale_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint);\r
+void xscale_enable_watchpoints(struct target_s *target);\r
+void xscale_enable_breakpoints(struct target_s *target);\r
+static int xscale_virt2phys(struct target_s *target, u32 virtual, u32 *physical);\r
+static int xscale_mmu(struct target_s *target, int *enabled);\r
+\r
+int xscale_read_trace(target_t *target);\r
+\r
+target_type_t xscale_target =\r
+{\r
+       .name = "xscale",\r
+\r
+       .poll = xscale_poll,\r
+       .arch_state = xscale_arch_state,\r
+\r
+       .target_request_data = NULL,\r
+\r
+       .halt = xscale_halt,\r
+       .resume = xscale_resume,\r
+       .step = xscale_step,\r
+\r
+       .assert_reset = xscale_assert_reset,\r
+       .deassert_reset = xscale_deassert_reset,\r
+       .soft_reset_halt = xscale_soft_reset_halt,\r
+       .prepare_reset_halt = xscale_prepare_reset_halt,\r
+\r
+       .get_gdb_reg_list = armv4_5_get_gdb_reg_list,\r
+\r
+       .read_memory = xscale_read_memory,\r
+       .write_memory = xscale_write_memory,\r
+       .bulk_write_memory = xscale_bulk_write_memory,\r
+       .checksum_memory = xscale_checksum_memory,\r
+\r
+       .run_algorithm = armv4_5_run_algorithm,\r
+\r
+       .add_breakpoint = xscale_add_breakpoint,\r
+       .remove_breakpoint = xscale_remove_breakpoint,\r
+       .add_watchpoint = xscale_add_watchpoint,\r
+       .remove_watchpoint = xscale_remove_watchpoint,\r
+\r
+       .register_commands = xscale_register_commands,\r
+       .target_command = xscale_target_command,\r
+       .init_target = xscale_init_target,\r
+       .quit = xscale_quit,\r
+       \r
+       .virt2phys = xscale_virt2phys,\r
+       .mmu = xscale_mmu\r
+};\r
+\r
+char* xscale_reg_list[] =\r
+{\r
+       "XSCALE_MAINID",                /* 0 */\r
+       "XSCALE_CACHETYPE",\r
+       "XSCALE_CTRL",\r
+       "XSCALE_AUXCTRL",\r
+       "XSCALE_TTB",\r
+       "XSCALE_DAC",\r
+       "XSCALE_FSR",\r
+       "XSCALE_FAR",\r
+       "XSCALE_PID",\r
+       "XSCALE_CPACCESS",\r
+       "XSCALE_IBCR0",                 /* 10 */\r
+       "XSCALE_IBCR1",\r
+       "XSCALE_DBR0",\r
+       "XSCALE_DBR1",\r
+       "XSCALE_DBCON",\r
+       "XSCALE_TBREG",\r
+       "XSCALE_CHKPT0",\r
+       "XSCALE_CHKPT1",\r
+       "XSCALE_DCSR",\r
+       "XSCALE_TX",\r
+       "XSCALE_RX",                    /* 20 */\r
+       "XSCALE_TXRXCTRL",\r
+};\r
+\r
+xscale_reg_t xscale_reg_arch_info[] =\r
+{\r
+       {XSCALE_MAINID, NULL},\r
+       {XSCALE_CACHETYPE, NULL},\r
+       {XSCALE_CTRL, NULL},\r
+       {XSCALE_AUXCTRL, NULL},\r
+       {XSCALE_TTB, NULL},\r
+       {XSCALE_DAC, NULL},\r
+       {XSCALE_FSR, NULL},\r
+       {XSCALE_FAR, NULL},\r
+       {XSCALE_PID, NULL},\r
+       {XSCALE_CPACCESS, NULL},\r
+       {XSCALE_IBCR0, NULL},\r
+       {XSCALE_IBCR1, NULL},\r
+       {XSCALE_DBR0, NULL},\r
+       {XSCALE_DBR1, NULL},\r
+       {XSCALE_DBCON, NULL},\r
+       {XSCALE_TBREG, NULL},\r
+       {XSCALE_CHKPT0, NULL},\r
+       {XSCALE_CHKPT1, NULL},\r
+       {XSCALE_DCSR, NULL}, /* DCSR accessed via JTAG or SW */\r
+       {-1, NULL}, /* TX accessed via JTAG */\r
+       {-1, NULL}, /* RX accessed via JTAG */\r
+       {-1, NULL}, /* TXRXCTRL implicit access via JTAG */\r
+};\r
+\r
+int xscale_reg_arch_type = -1;\r
+\r
+int xscale_get_reg(reg_t *reg);\r
+int xscale_set_reg(reg_t *reg, u8 *buf);\r
+\r
+int xscale_get_arch_pointers(target_t *target, armv4_5_common_t **armv4_5_p, xscale_common_t **xscale_p)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)\r
+       {\r
+               ERROR("target isn't an XScale target");\r
+               return -1;\r
+       }\r
+\r
+       if (xscale->common_magic != XSCALE_COMMON_MAGIC)\r
+       {\r
+               ERROR("target isn't an XScale target");\r
+               return -1;\r
+       }\r
+\r
+       *armv4_5_p = armv4_5;\r
+       *xscale_p = xscale;\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_jtag_set_instr(int chain_pos, u32 new_instr)\r
+{\r
+       jtag_device_t *device = jtag_get_device(chain_pos);\r
+\r
+       if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)\r
+       {\r
+               scan_field_t field;\r
+\r
+               field.device = chain_pos;\r
+               field.num_bits = device->ir_length;\r
+               field.out_value = calloc(CEIL(field.num_bits, 8), 1);\r
+               buf_set_u32(field.out_value, 0, field.num_bits, new_instr);\r
+               field.out_mask = NULL;\r
+               field.in_value = NULL;\r
+               jtag_set_check_value(&field, device->expected, device->expected_mask, NULL);\r
+\r
+               jtag_add_ir_scan(1, &field, -1);\r
+\r
+               free(field.out_value);\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_jtag_callback(enum jtag_event event, void *priv)\r
+{\r
+       switch (event)\r
+       {\r
+               case JTAG_TRST_ASSERTED:\r
+                       break;\r
+               case JTAG_TRST_RELEASED:\r
+                       break;\r
+               case JTAG_SRST_ASSERTED:\r
+                       break;\r
+               case JTAG_SRST_RELEASED:\r
+                       break;\r
+               default:\r
+                       WARNING("unhandled JTAG event");\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_read_dcsr(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       int retval;\r
+\r
+       scan_field_t fields[3];\r
+       u8 field0 = 0x0;\r
+       u8 field0_check_value = 0x2;\r
+       u8 field0_check_mask = 0x7;\r
+       u8 field2 = 0x0;\r
+       u8 field2_check_value = 0x0;\r
+       u8 field2_check_mask = 0x1;\r
+\r
+       jtag_add_end_state(TAP_PD);\r
+       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.dcsr);\r
+\r
+       buf_set_u32(&field0, 1, 1, xscale->hold_rst);\r
+       buf_set_u32(&field0, 2, 1, xscale->external_debug_break);\r
+\r
+       fields[0].device = xscale->jtag_info.chain_pos;\r
+       fields[0].num_bits = 3;\r
+       fields[0].out_value = &field0;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       jtag_set_check_value(fields+0, &field0_check_value, &field0_check_mask, NULL);\r
+\r
+       fields[1].device = xscale->jtag_info.chain_pos;\r
+       fields[1].num_bits = 32;\r
+       fields[1].out_value = NULL;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = xscale->reg_cache->reg_list[XSCALE_DCSR].value;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+\r
+\r
+\r
+       fields[2].device = xscale->jtag_info.chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = &field2;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       jtag_set_check_value(fields+2, &field2_check_value, &field2_check_mask, NULL);\r
+\r
+       jtag_add_dr_scan(3, fields, -1);\r
+\r
+       if ((retval = jtag_execute_queue()) != ERROR_OK)\r
+       {\r
+               ERROR("JTAG error while reading DCSR");\r
+               return retval;\r
+       }\r
+\r
+       xscale->reg_cache->reg_list[XSCALE_DCSR].dirty = 0;\r
+       xscale->reg_cache->reg_list[XSCALE_DCSR].valid = 1;\r
+\r
+       /* write the register with the value we just read\r
+        * on this second pass, only the first bit of field0 is guaranteed to be 0)\r
+        */\r
+       field0_check_mask = 0x1;\r
+       fields[1].out_value = xscale->reg_cache->reg_list[XSCALE_DCSR].value;\r
+       fields[1].in_value = NULL;\r
+\r
+       jtag_add_end_state(TAP_RTI);\r
+\r
+       jtag_add_dr_scan(3, fields, -1);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_receive(target_t *target, u32 *buffer, int num_words)\r
+{\r
+       int retval = ERROR_OK;\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       enum tap_state path[3];\r
+       scan_field_t fields[3];\r
+\r
+       u8 *field0 = malloc(num_words * 1);\r
+       u8 field0_check_value = 0x2;\r
+       u8 field0_check_mask = 0x6;\r
+       u32 *field1 = malloc(num_words * 4);\r
+       u8 field2_check_value = 0x0;\r
+       u8 field2_check_mask = 0x1;\r
+       int words_done = 0;\r
+       int words_scheduled = 0;\r
+\r
+       int i;\r
+\r
+       path[0] = TAP_SDS;\r
+       path[1] = TAP_CD;\r
+       path[2] = TAP_SD;\r
+\r
+       fields[0].device = xscale->jtag_info.chain_pos;\r
+       fields[0].num_bits = 3;\r
+       fields[0].out_value = NULL;\r
+       fields[0].out_mask = NULL;\r
+       /* fields[0].in_value = field0; */\r
+       jtag_set_check_value(fields+0, &field0_check_value, &field0_check_mask, NULL);\r
+\r
+       fields[1].device = xscale->jtag_info.chain_pos;\r
+       fields[1].num_bits = 32;\r
+       fields[1].out_value = NULL;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+\r
+\r
+\r
+       fields[2].device = xscale->jtag_info.chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = NULL;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       jtag_set_check_value(fields+2, &field2_check_value, &field2_check_mask, NULL);\r
+\r
+       jtag_add_end_state(TAP_RTI);\r
+       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.dbgtx);\r
+       jtag_add_runtest(1, -1);\r
+\r
+       /* repeat until all words have been collected */\r
+       int attempts = 0;\r
+       while (words_done < num_words)\r
+       {\r
+               /* schedule reads */\r
+               words_scheduled = 0;\r
+               for (i = words_done; i < num_words; i++)\r
+               {\r
+                       fields[0].in_value = &field0[i];\r
+                       fields[1].in_handler = buf_to_u32_handler;\r
+                       fields[1].in_handler_priv = (u8*)&field1[i];\r
+\r
+                       jtag_add_pathmove(3, path);\r
+                       jtag_add_dr_scan(3, fields, TAP_RTI);\r
+                       words_scheduled++;\r
+               }\r
+\r
+               if ((retval = jtag_execute_queue()) != ERROR_OK)\r
+               {\r
+                       ERROR("JTAG error while receiving data from debug handler");\r
+                       break;\r
+               }\r
+\r
+               /* examine results */\r
+               for (i = words_done; i < num_words; i++)\r
+               {\r
+                       if (!(field0[0] & 1))\r
+                       {\r
+                               /* move backwards if necessary */\r
+                               int j;\r
+                               for (j = i; j < num_words - 1; j++)\r
+                               {\r
+                                       field0[j] = field0[j+1];\r
+                                       field1[j] = field1[j+1];\r
+                               }\r
+                               words_scheduled--;\r
+                       }\r
+               }\r
+               if (words_scheduled == 0)\r
+               {\r
+                       if (attempts++ == 1000)\r
+                       {\r
+                               ERROR("Failed to receiving data from debug handler after 1000 attempts");\r
+                               retval = ERROR_JTAG_QUEUE_FAILED;\r
+                               break;\r
+                       }\r
+               }\r
+               \r
+               words_done += words_scheduled;\r
+       }\r
+\r
+       for (i = 0; i < num_words; i++)\r
+               *(buffer++) = buf_get_u32((u8*)&field1[i], 0, 32);\r
+\r
+       free(field1);\r
+\r
+       return retval;\r
+}\r
+\r
+int xscale_read_tx(target_t *target, int consume)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       enum tap_state path[3];\r
+       enum tap_state noconsume_path[9];\r
+\r
+       int retval;\r
+       struct timeval timeout, now;\r
+\r
+       scan_field_t fields[3];\r
+       u8 field0_in = 0x0;\r
+       u8 field0_check_value = 0x2;\r
+       u8 field0_check_mask = 0x6;\r
+       u8 field2_check_value = 0x0;\r
+       u8 field2_check_mask = 0x1;\r
+\r
+       jtag_add_end_state(TAP_RTI);\r
+\r
+       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.dbgtx);\r
+\r
+       path[0] = TAP_SDS;\r
+       path[1] = TAP_CD;\r
+       path[2] = TAP_SD;\r
+\r
+       noconsume_path[0] = TAP_SDS;\r
+       noconsume_path[1] = TAP_CD;\r
+       noconsume_path[2] = TAP_E1D;\r
+       noconsume_path[3] = TAP_PD;\r
+       noconsume_path[4] = TAP_E2D;\r
+       noconsume_path[5] = TAP_UD;\r
+       noconsume_path[6] = TAP_SDS;\r
+       noconsume_path[7] = TAP_CD;\r
+       noconsume_path[8] = TAP_SD;\r
+\r
+       fields[0].device = xscale->jtag_info.chain_pos;\r
+       fields[0].num_bits = 3;\r
+       fields[0].out_value = NULL;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = &field0_in;\r
+       jtag_set_check_value(fields+0, &field0_check_value, &field0_check_mask, NULL);\r
+\r
+       fields[1].device = xscale->jtag_info.chain_pos;\r
+       fields[1].num_bits = 32;\r
+       fields[1].out_value = NULL;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = xscale->reg_cache->reg_list[XSCALE_TX].value;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+\r
+\r
+\r
+       fields[2].device = xscale->jtag_info.chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = NULL;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       jtag_set_check_value(fields+2, &field2_check_value, &field2_check_mask, NULL);\r
+\r
+       gettimeofday(&timeout, NULL);\r
+       timeval_add_time(&timeout, 5, 0);\r
+\r
+       do\r
+       {\r
+               /* if we want to consume the register content (i.e. clear TX_READY),\r
+                * we have to go straight from Capture-DR to Shift-DR\r
+                * otherwise, we go from Capture-DR to Exit1-DR to Pause-DR\r
+               */\r
+               if (consume)\r
+                       jtag_add_pathmove(3, path);\r
+               else\r
+                       jtag_add_pathmove(sizeof(noconsume_path)/sizeof(*noconsume_path), noconsume_path);\r
+\r
+               jtag_add_dr_scan(3, fields, TAP_RTI);\r
+\r
+               if ((retval = jtag_execute_queue()) != ERROR_OK)\r
+               {\r
+                       ERROR("JTAG error while reading TX");\r
+                       return ERROR_TARGET_TIMEOUT;\r
+               }\r
+\r
+               gettimeofday(&now, NULL);\r
+               if ((now.tv_sec > timeout.tv_sec) || ((now.tv_sec == timeout.tv_sec)&& (now.tv_usec > timeout.tv_usec)))\r
+               {\r
+                       ERROR("time out reading TX register");\r
+                       return ERROR_TARGET_TIMEOUT;\r
+               }\r
+       } while ((!(field0_in & 1)) && consume);\r
+\r
+       if (!(field0_in & 1))\r
+               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_write_rx(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       int retval;\r
+       struct timeval timeout, now;\r
+\r
+       scan_field_t fields[3];\r
+       u8 field0_out = 0x0;\r
+       u8 field0_in = 0x0;\r
+       u8 field0_check_value = 0x2;\r
+       u8 field0_check_mask = 0x6;\r
+       u8 field2 = 0x0;\r
+       u8 field2_check_value = 0x0;\r
+       u8 field2_check_mask = 0x1;\r
+\r
+       jtag_add_end_state(TAP_RTI);\r
+\r
+       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.dbgrx);\r
+\r
+       fields[0].device = xscale->jtag_info.chain_pos;\r
+       fields[0].num_bits = 3;\r
+       fields[0].out_value = &field0_out;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = &field0_in;\r
+       jtag_set_check_value(fields+0, &field0_check_value, &field0_check_mask, NULL);\r
+\r
+       fields[1].device = xscale->jtag_info.chain_pos;\r
+       fields[1].num_bits = 32;\r
+       fields[1].out_value = xscale->reg_cache->reg_list[XSCALE_RX].value;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+\r
+\r
+\r
+       fields[2].device = xscale->jtag_info.chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = &field2;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       jtag_set_check_value(fields+2, &field2_check_value, &field2_check_mask, NULL);\r
+\r
+       gettimeofday(&timeout, NULL);\r
+       timeval_add_time(&timeout, 5, 0);\r
+\r
+       /* poll until rx_read is low */\r
+       DEBUG("polling RX");\r
+       do\r
+       {\r
+               jtag_add_dr_scan(3, fields, TAP_RTI);\r
+\r
+               if ((retval = jtag_execute_queue()) != ERROR_OK)\r
+               {\r
+                       ERROR("JTAG error while writing RX");\r
+                       return retval;\r
+               }\r
+\r
+               gettimeofday(&now, NULL);\r
+               if ((now.tv_sec > timeout.tv_sec) || ((now.tv_sec == timeout.tv_sec)&& (now.tv_usec > timeout.tv_usec)))\r
+               {\r
+                       ERROR("time out writing RX register");\r
+                       return ERROR_TARGET_TIMEOUT;\r
+               }\r
+       } while (field0_in & 1);\r
+\r
+       /* set rx_valid */\r
+       field2 = 0x1;\r
+       jtag_add_dr_scan(3, fields, TAP_RTI);\r
+\r
+       if ((retval = jtag_execute_queue()) != ERROR_OK)\r
+       {\r
+               ERROR("JTAG error while writing RX");\r
+               return retval;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* send count elements of size byte to the debug handler */\r
+int xscale_send(target_t *target, u8 *buffer, int count, int size)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       int retval;\r
+\r
+       int done_count = 0;\r
+       u8 output[4] = {0, 0, 0, 0};\r
+\r
+       scan_field_t fields[3];\r
+       u8 field0_out = 0x0;\r
+       u8 field0_check_value = 0x2;\r
+       u8 field0_check_mask = 0x6;\r
+       u8 field2 = 0x1;\r
+       u8 field2_check_value = 0x0;\r
+       u8 field2_check_mask = 0x1;\r
+\r
+       jtag_add_end_state(TAP_RTI);\r
+\r
+       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.dbgrx);\r
+\r
+       fields[0].device = xscale->jtag_info.chain_pos;\r
+       fields[0].num_bits = 3;\r
+       fields[0].out_value = &field0_out;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       if (!xscale->fast_memory_access)\r
+       {\r
+               jtag_set_check_value(fields+0, &field0_check_value, &field0_check_mask, NULL);\r
+       }\r
+\r
+       fields[1].device = xscale->jtag_info.chain_pos;\r
+       fields[1].num_bits = 32;\r
+       fields[1].out_value = output;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+\r
+\r
+\r
+       fields[2].device = xscale->jtag_info.chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = &field2;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       fields[2].in_handler = NULL;\r
+       if (!xscale->fast_memory_access)\r
+       {\r
+               jtag_set_check_value(fields+2, &field2_check_value, &field2_check_mask, NULL);\r
+       }\r
+\r
+       if (size==4)\r
+       {\r
+               int endianness = target->endianness;\r
+               while (done_count++ < count)\r
+               {\r
+                       if (endianness == TARGET_LITTLE_ENDIAN)\r
+                       {\r
+                               output[0]=buffer[0];\r
+                               output[1]=buffer[1];\r
+                               output[2]=buffer[2];\r
+                               output[3]=buffer[3];\r
+                       } else\r
+                       {\r
+                               output[0]=buffer[3];\r
+                               output[1]=buffer[2];\r
+                               output[2]=buffer[1];\r
+                               output[3]=buffer[0];\r
+                       }\r
+                       jtag_add_dr_scan(3, fields, TAP_RTI);\r
+                       buffer += size;\r
+               }\r
+               \r
+       } else\r
+       {\r
+               while (done_count++ < count)\r
+               {\r
+               /* extract sized element from target-endian buffer, and put it\r
+                * into little-endian output buffer\r
+                */\r
+               switch (size)\r
+               {\r
+                       case 2:\r
+                               buf_set_u32(output, 0, 32, target_buffer_get_u16(target, buffer));\r
+                               break;\r
+                       case 1:\r
+                               output[0] = *buffer;\r
+                               break;\r
+                       default:\r
+                               ERROR("BUG: size neither 4, 2 nor 1");\r
+                               exit(-1);\r
+               }\r
+\r
+               jtag_add_dr_scan(3, fields, TAP_RTI);\r
+               buffer += size;\r
+       }\r
+\r
+       }\r
+\r
+       if ((retval = jtag_execute_queue()) != ERROR_OK)\r
+       {\r
+               ERROR("JTAG error while sending data to debug handler");\r
+               return retval;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_send_u32(target_t *target, u32 value)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       buf_set_u32(xscale->reg_cache->reg_list[XSCALE_RX].value, 0, 32, value);\r
+       return xscale_write_rx(target);\r
+}\r
+\r
+int xscale_write_dcsr(target_t *target, int hold_rst, int ext_dbg_brk)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       int retval;\r
+\r
+       scan_field_t fields[3];\r
+       u8 field0 = 0x0;\r
+       u8 field0_check_value = 0x2;\r
+       u8 field0_check_mask = 0x7;\r
+       u8 field2 = 0x0;\r
+       u8 field2_check_value = 0x0;\r
+       u8 field2_check_mask = 0x1;\r
+\r
+       if (hold_rst != -1)\r
+               xscale->hold_rst = hold_rst;\r
+\r
+       if (ext_dbg_brk != -1)\r
+               xscale->external_debug_break = ext_dbg_brk;\r
+\r
+       jtag_add_end_state(TAP_RTI);\r
+       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.dcsr);\r
+\r
+       buf_set_u32(&field0, 1, 1, xscale->hold_rst);\r
+       buf_set_u32(&field0, 2, 1, xscale->external_debug_break);\r
+\r
+       fields[0].device = xscale->jtag_info.chain_pos;\r
+       fields[0].num_bits = 3;\r
+       fields[0].out_value = &field0;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       jtag_set_check_value(fields+0, &field0_check_value, &field0_check_mask, NULL);\r
+\r
+       fields[1].device = xscale->jtag_info.chain_pos;\r
+       fields[1].num_bits = 32;\r
+       fields[1].out_value = xscale->reg_cache->reg_list[XSCALE_DCSR].value;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+\r
+\r
+\r
+       fields[2].device = xscale->jtag_info.chain_pos;\r
+       fields[2].num_bits = 1;\r
+       fields[2].out_value = &field2;\r
+       fields[2].out_mask = NULL;\r
+       fields[2].in_value = NULL;\r
+       jtag_set_check_value(fields+2, &field2_check_value, &field2_check_mask, NULL);\r
+\r
+       jtag_add_dr_scan(3, fields, -1);\r
+\r
+       if ((retval = jtag_execute_queue()) != ERROR_OK)\r
+       {\r
+               ERROR("JTAG error while writing DCSR");\r
+               return retval;\r
+       }\r
+\r
+       xscale->reg_cache->reg_list[XSCALE_DCSR].dirty = 0;\r
+       xscale->reg_cache->reg_list[XSCALE_DCSR].valid = 1;\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* parity of the number of bits 0 if even; 1 if odd. for 32 bit words */\r
+unsigned int parity (unsigned int v)\r
+{\r
+       unsigned int ov = v;\r
+       v ^= v >> 16;\r
+       v ^= v >> 8;\r
+       v ^= v >> 4;\r
+       v &= 0xf;\r
+       DEBUG("parity of 0x%x is %i", ov, (0x6996 >> v) & 1);\r
+       return (0x6996 >> v) & 1;\r
+}\r
+\r
+int xscale_load_ic(target_t *target, int mini, u32 va, u32 buffer[8])\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       u8 packet[4];\r
+       u8 cmd;\r
+       int word;\r
+\r
+       scan_field_t fields[2];\r
+\r
+       DEBUG("loading miniIC at 0x%8.8x", va);\r
+\r
+       jtag_add_end_state(TAP_RTI);\r
+       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.ldic); /* LDIC */\r
+\r
+       /* CMD is b010 for Main IC and b011 for Mini IC */\r
+       if (mini)\r
+               buf_set_u32(&cmd, 0, 3, 0x3);\r
+       else\r
+               buf_set_u32(&cmd, 0, 3, 0x2);\r
+\r
+       buf_set_u32(&cmd, 3, 3, 0x0);\r
+\r
+       /* virtual address of desired cache line */\r
+       buf_set_u32(packet, 0, 27, va >> 5);\r
+\r
+       fields[0].device = xscale->jtag_info.chain_pos;\r
+       fields[0].num_bits = 6;\r
+       fields[0].out_value = &cmd;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+\r
+       fields[1].device = xscale->jtag_info.chain_pos;\r
+       fields[1].num_bits = 27;\r
+       fields[1].out_value = packet;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       jtag_add_dr_scan(2, fields, -1);\r
+\r
+       fields[0].num_bits = 32;\r
+       fields[0].out_value = packet;\r
+\r
+       fields[1].num_bits = 1;\r
+       fields[1].out_value = &cmd;\r
+\r
+       for (word = 0; word < 8; word++)\r
+       {\r
+               buf_set_u32(packet, 0, 32, buffer[word]);\r
+               cmd = parity(*((u32*)packet));\r
+               jtag_add_dr_scan(2, fields, -1);\r
+       }\r
+\r
+       jtag_execute_queue();\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_invalidate_ic_line(target_t *target, u32 va)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       u8 packet[4];\r
+       u8 cmd;\r
+\r
+       scan_field_t fields[2];\r
+\r
+       jtag_add_end_state(TAP_RTI);\r
+       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.ldic); /* LDIC */\r
+\r
+       /* CMD for invalidate IC line b000, bits [6:4] b000 */\r
+       buf_set_u32(&cmd, 0, 6, 0x0);\r
+\r
+       /* virtual address of desired cache line */\r
+       buf_set_u32(packet, 0, 27, va >> 5);\r
+\r
+       fields[0].device = xscale->jtag_info.chain_pos;\r
+       fields[0].num_bits = 6;\r
+       fields[0].out_value = &cmd;\r
+       fields[0].out_mask = NULL;\r
+       fields[0].in_value = NULL;\r
+       fields[0].in_check_value = NULL;\r
+       fields[0].in_check_mask = NULL;\r
+       fields[0].in_handler = NULL;\r
+       fields[0].in_handler_priv = NULL;\r
+\r
+       fields[1].device = xscale->jtag_info.chain_pos;\r
+       fields[1].num_bits = 27;\r
+       fields[1].out_value = packet;\r
+       fields[1].out_mask = NULL;\r
+       fields[1].in_value = NULL;\r
+       fields[1].in_check_value = NULL;\r
+       fields[1].in_check_mask = NULL;\r
+       fields[1].in_handler = NULL;\r
+       fields[1].in_handler_priv = NULL;\r
+\r
+       jtag_add_dr_scan(2, fields, -1);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_update_vectors(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       int i;\r
+\r
+       u32 low_reset_branch, high_reset_branch;\r
+\r
+       for (i = 1; i < 8; i++)\r
+       {\r
+               /* if there's a static vector specified for this exception, override */\r
+               if (xscale->static_high_vectors_set & (1 << i))\r
+               {\r
+                       xscale->high_vectors[i] = xscale->static_high_vectors[i];\r
+               }\r
+               else\r
+               {\r
+                       if (target_read_u32(target, 0xffff0000 + 4*i, &xscale->high_vectors[i]) != ERROR_OK)\r
+                       {\r
+                               xscale->high_vectors[i] = ARMV4_5_B(0xfffffe, 0);\r
+                       }\r
+               }\r
+       }\r
+\r
+       for (i = 1; i < 8; i++)\r
+       {\r
+               if (xscale->static_low_vectors_set & (1 << i))\r
+               {\r
+                       xscale->low_vectors[i] = xscale->static_low_vectors[i];\r
+               }\r
+               else\r
+               {\r
+                       if (target_read_u32(target, 0x0 + 4*i, &xscale->low_vectors[i]) != ERROR_OK)\r
+                       {\r
+                               xscale->low_vectors[i] = ARMV4_5_B(0xfffffe, 0);\r
+                       }\r
+               }\r
+       }\r
+\r
+       /* calculate branches to debug handler */\r
+       low_reset_branch = (xscale->handler_address + 0x20 - 0x0 - 0x8) >> 2;\r
+       high_reset_branch = (xscale->handler_address + 0x20 - 0xffff0000 - 0x8) >> 2;\r
+\r
+       xscale->low_vectors[0] = ARMV4_5_B((low_reset_branch & 0xffffff), 0);\r
+       xscale->high_vectors[0] = ARMV4_5_B((high_reset_branch & 0xffffff), 0);\r
+\r
+       /* invalidate and load exception vectors in mini i-cache */\r
+       xscale_invalidate_ic_line(target, 0x0);\r
+       xscale_invalidate_ic_line(target, 0xffff0000);\r
+\r
+       xscale_load_ic(target, 1, 0x0, xscale->low_vectors);\r
+       xscale_load_ic(target, 1, 0xffff0000, xscale->high_vectors);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_arch_state(struct target_s *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       char *state[] =\r
+       {\r
+               "disabled", "enabled"\r
+       };\r
+\r
+       char *arch_dbg_reason[] =\r
+       {\r
+               "", "\n(processor reset)", "\n(trace buffer full)"\r
+       };\r
+\r
+       if (armv4_5->common_magic != ARMV4_5_COMMON_MAGIC)\r
+       {\r
+               ERROR("BUG: called for a non-ARMv4/5 target");\r
+               exit(-1);\r
+       }\r
+\r
+       USER("target halted in %s state due to %s, current mode: %s\n"\r
+                       "cpsr: 0x%8.8x pc: 0x%8.8x\n"\r
+                       "MMU: %s, D-Cache: %s, I-Cache: %s"\r
+                       "%s",\r
+                        armv4_5_state_strings[armv4_5->core_state],\r
+                        target_debug_reason_strings[target->debug_reason],\r
+                        armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)],\r
+                        buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32),\r
+                        buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32),\r
+                        state[xscale->armv4_5_mmu.mmu_enabled],\r
+                        state[xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled],\r
+                        state[xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled],\r
+                        arch_dbg_reason[xscale->arch_debug_reason]);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_poll(target_t *target)\r
+{\r
+       int retval=ERROR_OK;\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       if ((target->state == TARGET_RUNNING) || (target->state == TARGET_DEBUG_RUNNING))\r
+       {\r
+               enum target_state previous_state = target->state;\r
+               if ((retval = xscale_read_tx(target, 0)) == ERROR_OK)\r
+               {\r
+\r
+                       /* there's data to read from the tx register, we entered debug state */\r
+                       xscale->handler_running = 1;\r
+\r
+                       target->state = TARGET_HALTED;\r
+\r
+                       /* process debug entry, fetching current mode regs */\r
+                       retval = xscale_debug_entry(target);\r
+               }\r
+               else if (retval != ERROR_TARGET_RESOURCE_NOT_AVAILABLE)\r
+               {\r
+                       USER("error while polling TX register, reset CPU");\r
+                       /* here we "lie" so GDB won't get stuck and a reset can be perfomed */\r
+                       target->state = TARGET_HALTED;\r
+               }\r
+\r
+                       /* debug_entry could have overwritten target state (i.e. immediate resume)\r
+                        * don't signal event handlers in that case\r
+                        */\r
+               if (target->state != TARGET_HALTED)\r
+                       return ERROR_OK;\r
+\r
+               /* if target was running, signal that we halted\r
+                * otherwise we reentered from debug execution */\r
+               if (previous_state == TARGET_RUNNING)\r
+                       target_call_event_callbacks(target, TARGET_EVENT_HALTED);\r
+               else\r
+                       target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);\r
+       }\r
+       return retval;\r
+}\r
+\r
+int xscale_debug_entry(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       u32 pc;\r
+       u32 buffer[10];\r
+       int i;\r
+\r
+       u32 moe;\r
+\r
+       /* clear external dbg break (will be written on next DCSR read) */\r
+       xscale->external_debug_break = 0;\r
+       xscale_read_dcsr(target);\r
+\r
+       /* get r0, pc, r1 to r7 and cpsr */\r
+       xscale_receive(target, buffer, 10);\r
+\r
+       /* move r0 from buffer to register cache */\r
+       buf_set_u32(armv4_5->core_cache->reg_list[0].value, 0, 32, buffer[0]);\r
+       armv4_5->core_cache->reg_list[15].dirty = 1;\r
+       armv4_5->core_cache->reg_list[15].valid = 1;\r
+       DEBUG("r0: 0x%8.8x", buffer[0]);\r
+\r
+       /* move pc from buffer to register cache */\r
+       buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, buffer[1]);\r
+       armv4_5->core_cache->reg_list[15].dirty = 1;\r
+       armv4_5->core_cache->reg_list[15].valid = 1;\r
+       DEBUG("pc: 0x%8.8x", buffer[1]);\r
+\r
+       /* move data from buffer to register cache */\r
+       for (i = 1; i <= 7; i++)\r
+       {\r
+               buf_set_u32(armv4_5->core_cache->reg_list[i].value, 0, 32, buffer[1 + i]);\r
+               armv4_5->core_cache->reg_list[i].dirty = 1;\r
+               armv4_5->core_cache->reg_list[i].valid = 1;\r
+               DEBUG("r%i: 0x%8.8x", i, buffer[i + 1]);\r
+       }\r
+\r
+       buf_set_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32, buffer[9]);\r
+       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].dirty = 1;\r
+       armv4_5->core_cache->reg_list[ARMV4_5_CPSR].valid = 1;\r
+       DEBUG("cpsr: 0x%8.8x", buffer[9]);\r
+\r
+       armv4_5->core_mode = buffer[9] & 0x1f;\r
+       if (armv4_5_mode_to_number(armv4_5->core_mode) == -1)\r
+       {\r
+               target->state = TARGET_UNKNOWN;\r
+               ERROR("cpsr contains invalid mode value - communication failure");\r
+               return ERROR_TARGET_FAILURE;\r
+       }\r
+       DEBUG("target entered debug state in %s mode", armv4_5_mode_strings[armv4_5_mode_to_number(armv4_5->core_mode)]);\r
+\r
+       if (buffer[9] & 0x20)\r
+               armv4_5->core_state = ARMV4_5_STATE_THUMB;\r
+       else\r
+               armv4_5->core_state = ARMV4_5_STATE_ARM;\r
+\r
+       /* get banked registers, r8 to r14, and spsr if not in USR/SYS mode */\r
+       if ((armv4_5->core_mode != ARMV4_5_MODE_USR) && (armv4_5->core_mode != ARMV4_5_MODE_SYS))\r
+       {\r
+               xscale_receive(target, buffer, 8);\r
+               buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32, buffer[7]);\r
+               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).dirty = 0;\r
+               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).valid = 1;\r
+       }\r
+       else\r
+       {\r
+               /* r8 to r14, but no spsr */\r
+               xscale_receive(target, buffer, 7);\r
+       }\r
+\r
+       /* move data from buffer to register cache */\r
+       for (i = 8; i <= 14; i++)\r
+       {\r
+               buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).value, 0, 32, buffer[i - 8]);\r
+               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).dirty = 0;\r
+               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, i).valid = 1;\r
+       }\r
+\r
+       /* examine debug reason */\r
+       xscale_read_dcsr(target);\r
+       moe = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 2, 3);\r
+\r
+       /* stored PC (for calculating fixup) */\r
+       pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);\r
+\r
+       switch (moe)\r
+       {\r
+               case 0x0: /* Processor reset */\r
+                       target->debug_reason = DBG_REASON_DBGRQ;\r
+                       xscale->arch_debug_reason = XSCALE_DBG_REASON_RESET;\r
+                       pc -= 4;\r
+                       break;\r
+               case 0x1: /* Instruction breakpoint hit */\r
+                       target->debug_reason = DBG_REASON_BREAKPOINT;\r
+                       xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;\r
+                       pc -= 4;\r
+                       break;\r
+               case 0x2: /* Data breakpoint hit */\r
+                       target->debug_reason = DBG_REASON_WATCHPOINT;\r
+                       xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;\r
+                       pc -= 4;\r
+                       break;\r
+               case 0x3: /* BKPT instruction executed */\r
+                       target->debug_reason = DBG_REASON_BREAKPOINT;\r
+                       xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;\r
+                       pc -= 4;\r
+                       break;\r
+               case 0x4: /* Ext. debug event */\r
+                       target->debug_reason = DBG_REASON_DBGRQ;\r
+                       xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;\r
+                       pc -= 4;\r
+                       break;\r
+               case 0x5: /* Vector trap occured */\r
+                       target->debug_reason = DBG_REASON_BREAKPOINT;\r
+                       xscale->arch_debug_reason = XSCALE_DBG_REASON_GENERIC;\r
+                       pc -= 4;\r
+                       break;\r
+               case 0x6: /* Trace buffer full break */\r
+                       target->debug_reason = DBG_REASON_DBGRQ;\r
+                       xscale->arch_debug_reason = XSCALE_DBG_REASON_TB_FULL;\r
+                       pc -= 4;\r
+                       break;\r
+               case 0x7: /* Reserved */\r
+               default:\r
+                       ERROR("Method of Entry is 'Reserved'");\r
+                       exit(-1);\r
+                       break;\r
+       }\r
+\r
+       /* apply PC fixup */\r
+       buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, pc);\r
+\r
+       /* on the first debug entry, identify cache type */\r
+       if (xscale->armv4_5_mmu.armv4_5_cache.ctype == -1)\r
+       {\r
+               u32 cache_type_reg;\r
+\r
+               /* read cp15 cache type register */\r
+               xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CACHETYPE]);\r
+               cache_type_reg = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_CACHETYPE].value, 0, 32);\r
+\r
+               armv4_5_identify_cache(cache_type_reg, &xscale->armv4_5_mmu.armv4_5_cache);\r
+       }\r
+\r
+       /* examine MMU and Cache settings */\r
+       /* read cp15 control register */\r
+       xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CTRL]);\r
+       xscale->cp15_control_reg = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_CTRL].value, 0, 32);\r
+       xscale->armv4_5_mmu.mmu_enabled = (xscale->cp15_control_reg & 0x1U) ? 1 : 0;\r
+       xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = (xscale->cp15_control_reg & 0x4U) ? 1 : 0;\r
+       xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled = (xscale->cp15_control_reg & 0x1000U) ? 1 : 0;\r
+\r
+       /* tracing enabled, read collected trace data */\r
+       if (xscale->trace.buffer_enabled)\r
+       {\r
+               xscale_read_trace(target);\r
+               xscale->trace.buffer_fill--;\r
+\r
+               /* resume if we're still collecting trace data */\r
+               if ((xscale->arch_debug_reason == XSCALE_DBG_REASON_TB_FULL)\r
+                       && (xscale->trace.buffer_fill > 0))\r
+               {\r
+                       xscale_resume(target, 1, 0x0, 1, 0);\r
+               }\r
+               else\r
+               {\r
+                       xscale->trace.buffer_enabled = 0;\r
+               }\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_halt(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       DEBUG("target->state: %s", target_state_strings[target->state]);\r
+\r
+       if (target->state == TARGET_HALTED)\r
+       {\r
+               WARNING("target was already halted");\r
+               return ERROR_TARGET_ALREADY_HALTED;\r
+       }\r
+       else if (target->state == TARGET_UNKNOWN)\r
+       {\r
+               /* this must not happen for a xscale target */\r
+               ERROR("target was in unknown state when halt was requested");\r
+               return ERROR_TARGET_INVALID;\r
+       }\r
+       else if (target->state == TARGET_RESET)\r
+       {\r
+               DEBUG("target->state == TARGET_RESET");\r
+       }\r
+       else\r
+       {\r
+               /* assert external dbg break */\r
+               xscale->external_debug_break = 1;\r
+               xscale_read_dcsr(target);\r
+\r
+               target->debug_reason = DBG_REASON_DBGRQ;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_enable_single_step(struct target_s *target, u32 next_pc)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale= armv4_5->arch_info;\r
+       reg_t *ibcr0 = &xscale->reg_cache->reg_list[XSCALE_IBCR0];\r
+\r
+       if (xscale->ibcr0_used)\r
+       {\r
+               breakpoint_t *ibcr0_bp = breakpoint_find(target, buf_get_u32(ibcr0->value, 0, 32) & 0xfffffffe);\r
+\r
+               if (ibcr0_bp)\r
+               {\r
+                       xscale_unset_breakpoint(target, ibcr0_bp);\r
+               }\r
+               else\r
+               {\r
+                       ERROR("BUG: xscale->ibcr0_used is set, but no breakpoint with that address found");\r
+                       exit(-1);\r
+               }\r
+       }\r
+\r
+       xscale_set_reg_u32(ibcr0, next_pc | 0x1);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_disable_single_step(struct target_s *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale= armv4_5->arch_info;\r
+       reg_t *ibcr0 = &xscale->reg_cache->reg_list[XSCALE_IBCR0];\r
+\r
+       xscale_set_reg_u32(ibcr0, 0x0);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale= armv4_5->arch_info;\r
+       breakpoint_t *breakpoint = target->breakpoints;\r
+\r
+       u32 current_pc;\r
+\r
+       int retval;\r
+       int i;\r
+\r
+       DEBUG("-");\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target not halted");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       if (!debug_execution)\r
+       {\r
+               target_free_all_working_areas(target);\r
+       }\r
+\r
+       /* update vector tables */\r
+       xscale_update_vectors(target);\r
+\r
+       /* current = 1: continue on current pc, otherwise continue at <address> */\r
+       if (!current)\r
+               buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);\r
+\r
+       current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);\r
+\r
+       /* if we're at the reset vector, we have to simulate the branch */\r
+       if (current_pc == 0x0)\r
+       {\r
+               arm_simulate_step(target, NULL);\r
+               current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);\r
+       }\r
+\r
+       /* the front-end may request us not to handle breakpoints */\r
+       if (handle_breakpoints)\r
+       {\r
+               if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))\r
+               {\r
+                       u32 next_pc;\r
+\r
+                       /* there's a breakpoint at the current PC, we have to step over it */\r
+                       DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);\r
+                       xscale_unset_breakpoint(target, breakpoint);\r
+\r
+                       /* calculate PC of next instruction */\r
+                       if ((retval = arm_simulate_step(target, &next_pc)) != ERROR_OK)\r
+                       {\r
+                               u32 current_opcode;\r
+                               target_read_u32(target, current_pc, &current_opcode);\r
+                               ERROR("BUG: couldn't calculate PC of next instruction, current opcode was 0x%8.8x", current_opcode);\r
+                       }\r
+\r
+                       DEBUG("enable single-step");\r
+                       xscale_enable_single_step(target, next_pc);\r
+\r
+                       /* restore banked registers */\r
+                       xscale_restore_context(target);\r
+\r
+                       /* send resume request (command 0x30 or 0x31)\r
+                        * clean the trace buffer if it is to be enabled (0x62) */\r
+                       if (xscale->trace.buffer_enabled)\r
+                       {\r
+                               xscale_send_u32(target, 0x62);\r
+                               xscale_send_u32(target, 0x31);\r
+                       }\r
+                       else\r
+                               xscale_send_u32(target, 0x30);\r
+\r
+                       /* send CPSR */\r
+                       xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));\r
+                       DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));\r
+\r
+                       for (i = 7; i >= 0; i--)\r
+                       {\r
+                               /* send register */\r
+                               xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));\r
+                               DEBUG("writing r%i with value 0x%8.8x", i, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));\r
+                       }\r
+\r
+                       /* send PC */\r
+                       xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));\r
+                       DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));\r
+\r
+                       /* wait for and process debug entry */\r
+                       xscale_debug_entry(target);\r
+\r
+                       DEBUG("disable single-step");\r
+                       xscale_disable_single_step(target);\r
+\r
+                       DEBUG("set breakpoint at 0x%8.8x", breakpoint->address);\r
+                       xscale_set_breakpoint(target, breakpoint);\r
+               }\r
+       }\r
+\r
+       /* enable any pending breakpoints and watchpoints */\r
+       xscale_enable_breakpoints(target);\r
+       xscale_enable_watchpoints(target);\r
+\r
+       /* restore banked registers */\r
+       xscale_restore_context(target);\r
+\r
+       /* send resume request (command 0x30 or 0x31)\r
+        * clean the trace buffer if it is to be enabled (0x62) */\r
+       if (xscale->trace.buffer_enabled)\r
+       {\r
+               xscale_send_u32(target, 0x62);\r
+               xscale_send_u32(target, 0x31);\r
+       }\r
+       else\r
+               xscale_send_u32(target, 0x30);\r
+\r
+       /* send CPSR */\r
+       xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));\r
+       DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));\r
+\r
+       for (i = 7; i >= 0; i--)\r
+       {\r
+               /* send register */\r
+               xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));\r
+               DEBUG("writing r%i with value 0x%8.8x", i, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));\r
+       }\r
+\r
+       /* send PC */\r
+       xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));\r
+       DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));\r
+\r
+       target->debug_reason = DBG_REASON_NOTHALTED;\r
+\r
+       if (!debug_execution)\r
+       {\r
+               /* registers are now invalid */\r
+               armv4_5_invalidate_core_regs(target);\r
+               target->state = TARGET_RUNNING;\r
+               target_call_event_callbacks(target, TARGET_EVENT_RESUMED);\r
+       }\r
+       else\r
+       {\r
+               target->state = TARGET_DEBUG_RUNNING;\r
+               target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);\r
+       }\r
+\r
+       DEBUG("target resumed");\r
+\r
+       xscale->handler_running = 1;\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_step(struct target_s *target, int current, u32 address, int handle_breakpoints)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       breakpoint_t *breakpoint = target->breakpoints;\r
+\r
+       u32 current_pc, next_pc;\r
+       int i;\r
+       int retval;\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target not halted");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       /* current = 1: continue on current pc, otherwise continue at <address> */\r
+       if (!current)\r
+               buf_set_u32(armv4_5->core_cache->reg_list[15].value, 0, 32, address);\r
+\r
+       current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);\r
+\r
+       /* if we're at the reset vector, we have to simulate the step */\r
+       if (current_pc == 0x0)\r
+       {\r
+               arm_simulate_step(target, NULL);\r
+               current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);\r
+\r
+               target->debug_reason = DBG_REASON_SINGLESTEP;\r
+               target_call_event_callbacks(target, TARGET_EVENT_HALTED);\r
+\r
+               return ERROR_OK;\r
+       }\r
+\r
+       /* the front-end may request us not to handle breakpoints */\r
+       if (handle_breakpoints)\r
+               if ((breakpoint = breakpoint_find(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32))))\r
+               {\r
+                       xscale_unset_breakpoint(target, breakpoint);\r
+               }\r
+\r
+       target->debug_reason = DBG_REASON_SINGLESTEP;\r
+\r
+       /* calculate PC of next instruction */\r
+       if ((retval = arm_simulate_step(target, &next_pc)) != ERROR_OK)\r
+       {\r
+               u32 current_opcode;\r
+               target_read_u32(target, current_pc, &current_opcode);\r
+               ERROR("BUG: couldn't calculate PC of next instruction, current opcode was 0x%8.8x", current_opcode);\r
+       }\r
+\r
+       DEBUG("enable single-step");\r
+       xscale_enable_single_step(target, next_pc);\r
+\r
+       /* restore banked registers */\r
+       xscale_restore_context(target);\r
+\r
+       /* send resume request (command 0x30 or 0x31)\r
+        * clean the trace buffer if it is to be enabled (0x62) */\r
+       if (xscale->trace.buffer_enabled)\r
+       {\r
+               xscale_send_u32(target, 0x62);\r
+               xscale_send_u32(target, 0x31);\r
+       }\r
+       else\r
+               xscale_send_u32(target, 0x30);\r
+\r
+       /* send CPSR */\r
+       xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));\r
+       DEBUG("writing cpsr with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[ARMV4_5_CPSR].value, 0, 32));\r
+\r
+       for (i = 7; i >= 0; i--)\r
+       {\r
+               /* send register */\r
+               xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));\r
+               DEBUG("writing r%i with value 0x%8.8x", i, buf_get_u32(armv4_5->core_cache->reg_list[i].value, 0, 32));\r
+       }\r
+\r
+       /* send PC */\r
+       xscale_send_u32(target, buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));\r
+       DEBUG("writing PC with value 0x%8.8x", buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32));\r
+\r
+       target_call_event_callbacks(target, TARGET_EVENT_RESUMED);\r
+\r
+       /* registers are now invalid */\r
+       armv4_5_invalidate_core_regs(target);\r
+\r
+       /* wait for and process debug entry */\r
+       xscale_debug_entry(target);\r
+\r
+       DEBUG("disable single-step");\r
+       xscale_disable_single_step(target);\r
+\r
+       target_call_event_callbacks(target, TARGET_EVENT_HALTED);\r
+\r
+       if (breakpoint)\r
+       {\r
+               xscale_set_breakpoint(target, breakpoint);\r
+       }\r
+\r
+       DEBUG("target stepped");\r
+\r
+       return ERROR_OK;\r
+\r
+}\r
+\r
+int xscale_assert_reset(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       DEBUG("target->state: %s", target_state_strings[target->state]);\r
+\r
+       /* select DCSR instruction (set endstate to R-T-I to ensure we don't\r
+        * end up in T-L-R, which would reset JTAG\r
+        */\r
+       jtag_add_end_state(TAP_RTI);\r
+       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, xscale->jtag_info.dcsr);\r
+\r
+       /* set Hold reset, Halt mode and Trap Reset */\r
+       buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 30, 1, 0x1);\r
+       buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 16, 1, 0x1);\r
+       xscale_write_dcsr(target, 1, 0);\r
+\r
+       /* select BYPASS, because having DCSR selected caused problems on the PXA27x */\r
+       xscale_jtag_set_instr(xscale->jtag_info.chain_pos, 0x7f);\r
+       jtag_execute_queue();\r
+\r
+       /* assert reset */\r
+       jtag_add_reset(0, 1);\r
+\r
+       /* sleep 1ms, to be sure we fulfill any requirements */\r
+       jtag_add_sleep(1000);\r
+       jtag_execute_queue();\r
+\r
+       target->state = TARGET_RESET;\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_deassert_reset(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       fileio_t debug_handler;\r
+       u32 address;\r
+       u32 binary_size;\r
+\r
+       u32 buf_cnt;\r
+       int i;\r
+       int retval;\r
+\r
+       breakpoint_t *breakpoint = target->breakpoints;\r
+\r
+       DEBUG("-");\r
+\r
+       xscale->ibcr_available = 2;\r
+       xscale->ibcr0_used = 0;\r
+       xscale->ibcr1_used = 0;\r
+\r
+       xscale->dbr_available = 2;\r
+       xscale->dbr0_used = 0;\r
+       xscale->dbr1_used = 0;\r
+\r
+       /* mark all hardware breakpoints as unset */\r
+       while (breakpoint)\r
+       {\r
+               if (breakpoint->type == BKPT_HARD)\r
+               {\r
+                       breakpoint->set = 0;\r
+               }\r
+               breakpoint = breakpoint->next;\r
+       }\r
+\r
+       if (!xscale->handler_installed)\r
+       {\r
+               /* release SRST */\r
+               jtag_add_reset(0, 0);\r
+\r
+               /* wait 300ms; 150 and 100ms were not enough */\r
+               jtag_add_sleep(300*1000);\r
+\r
+               jtag_add_runtest(2030, TAP_RTI);\r
+               jtag_execute_queue();\r
+\r
+               /* set Hold reset, Halt mode and Trap Reset */\r
+               buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 30, 1, 0x1);\r
+               buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 16, 1, 0x1);\r
+               xscale_write_dcsr(target, 1, 0);\r
+\r
+               /* Load debug handler */\r
+               if (fileio_open(&debug_handler, PKGLIBDIR "/xscale/debug_handler.bin", FILEIO_READ, FILEIO_BINARY) != ERROR_OK)\r
+               {\r
+                       ERROR("file open error: %s", debug_handler.error_str);\r
+                       return ERROR_OK;\r
+               }\r
+\r
+               if ((binary_size = debug_handler.size) % 4)\r
+               {\r
+                       ERROR("debug_handler.bin: size not a multiple of 4");\r
+                       exit(-1);\r
+               }\r
+\r
+               if (binary_size > 0x800)\r
+               {\r
+                       ERROR("debug_handler.bin: larger than 2kb");\r
+                       exit(-1);\r
+               }\r
+\r
+               binary_size = CEIL(binary_size, 32) * 32;\r
+\r
+               address = xscale->handler_address;\r
+               while (binary_size > 0)\r
+               {\r
+                       u32 cache_line[8];\r
+                       u8 buffer[32];\r
+\r
+                       if ((retval = fileio_read(&debug_handler, 32, buffer, &buf_cnt)) != ERROR_OK)\r
+                       {\r
+                               ERROR("reading debug handler failed: %s", debug_handler.error_str);\r
+                       }\r
+\r
+                       for (i = 0; i < buf_cnt; i += 4)\r
+                       {\r
+                               /* convert LE buffer to host-endian u32 */\r
+                               cache_line[i / 4] = le_to_h_u32(&buffer[i]);\r
+                       }\r
+\r
+                       for (; buf_cnt < 32; buf_cnt += 4)\r
+                       {\r
+                                       cache_line[buf_cnt / 4] = 0xe1a08008;\r
+                       }\r
+\r
+                       /* only load addresses other than the reset vectors */\r
+                       if ((address % 0x400) != 0x0)\r
+                       {\r
+                               xscale_load_ic(target, 1, address, cache_line);\r
+                       }\r
+\r
+                       address += buf_cnt;\r
+                       binary_size -= buf_cnt;\r
+               };\r
+\r
+               xscale_load_ic(target, 1, 0x0, xscale->low_vectors);\r
+               xscale_load_ic(target, 1, 0xffff0000, xscale->high_vectors);\r
+\r
+               jtag_add_runtest(30, TAP_RTI);\r
+\r
+               jtag_add_sleep(100000);\r
+\r
+               /* set Hold reset, Halt mode and Trap Reset */\r
+               buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 30, 1, 0x1);\r
+               buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 16, 1, 0x1);\r
+               xscale_write_dcsr(target, 1, 0);\r
+\r
+               /* clear Hold reset to let the target run (should enter debug handler) */\r
+               xscale_write_dcsr(target, 0, 1);\r
+               target->state = TARGET_RUNNING;\r
+\r
+               if ((target->reset_mode != RESET_HALT) && (target->reset_mode != RESET_INIT))\r
+               {\r
+                       jtag_add_sleep(10000);\r
+\r
+                       /* we should have entered debug now */\r
+                       xscale_debug_entry(target);\r
+                       target->state = TARGET_HALTED;\r
+\r
+                       /* resume the target */\r
+                       xscale_resume(target, 1, 0x0, 1, 0);\r
+               }\r
+\r
+               fileio_close(&debug_handler);\r
+       }\r
+       else\r
+       {\r
+               jtag_add_reset(0, 0);\r
+       }\r
+\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_soft_reset_halt(struct target_s *target)\r
+{\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_prepare_reset_halt(struct target_s *target)\r
+{\r
+       /* nothing to be done for reset_halt on XScale targets\r
+        * we always halt after a reset to upload the debug handler\r
+        */\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_read_core_reg(struct target_s *target, int num, enum armv4_5_mode mode)\r
+{\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_write_core_reg(struct target_s *target, int num, enum armv4_5_mode mode, u32 value)\r
+{\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_full_context(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+\r
+       u32 *buffer;\r
+\r
+       int i, j;\r
+\r
+       DEBUG("-");\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target not halted");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       buffer = malloc(4 * 8);\r
+\r
+       /* iterate through processor modes (FIQ, IRQ, SVC, ABT, UND and SYS)\r
+        * we can't enter User mode on an XScale (unpredictable),\r
+        * but User shares registers with SYS\r
+        */\r
+       for(i = 1; i < 7; i++)\r
+       {\r
+               int valid = 1;\r
+\r
+               /* check if there are invalid registers in the current mode\r
+                */\r
+               for (j = 0; j <= 16; j++)\r
+               {\r
+                       if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid == 0)\r
+                               valid = 0;\r
+               }\r
+\r
+               if (!valid)\r
+               {\r
+                       u32 tmp_cpsr;\r
+\r
+                       /* request banked registers */\r
+                       xscale_send_u32(target, 0x0);\r
+\r
+                       tmp_cpsr = 0x0;\r
+                       tmp_cpsr |= armv4_5_number_to_mode(i);\r
+                       tmp_cpsr |= 0xc0; /* I/F bits */\r
+\r
+                       /* send CPSR for desired mode */\r
+                       xscale_send_u32(target, tmp_cpsr);\r
+\r
+                       /* get banked registers, r8 to r14, and spsr if not in USR/SYS mode */\r
+                       if ((armv4_5_number_to_mode(i) != ARMV4_5_MODE_USR) && (armv4_5_number_to_mode(i) != ARMV4_5_MODE_SYS))\r
+                       {\r
+                               xscale_receive(target, buffer, 8);\r
+                               buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32, buffer[7]);\r
+                               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty = 0;\r
+                               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).valid = 1;\r
+                       }\r
+                       else\r
+                       {\r
+                               xscale_receive(target, buffer, 7);\r
+                       }\r
+\r
+                       /* move data from buffer to register cache */\r
+                       for (j = 8; j <= 14; j++)\r
+                       {\r
+                               buf_set_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).value, 0, 32, buffer[j - 8]);\r
+                               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty = 0;\r
+                               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).valid = 1;\r
+                       }\r
+               }\r
+       }\r
+\r
+       free(buffer);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_restore_context(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+\r
+       int i, j;\r
+\r
+       DEBUG("-");\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target not halted");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       /* iterate through processor modes (FIQ, IRQ, SVC, ABT, UND and SYS)\r
+       * we can't enter User mode on an XScale (unpredictable),\r
+       * but User shares registers with SYS\r
+       */\r
+       for(i = 1; i < 7; i++)\r
+       {\r
+               int dirty = 0;\r
+\r
+               /* check if there are invalid registers in the current mode\r
+               */\r
+               for (j = 8; j <= 14; j++)\r
+               {\r
+                       if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty == 1)\r
+                               dirty = 1;\r
+               }\r
+\r
+               /* if not USR/SYS, check if the SPSR needs to be written */\r
+               if ((armv4_5_number_to_mode(i) != ARMV4_5_MODE_USR) && (armv4_5_number_to_mode(i) != ARMV4_5_MODE_SYS))\r
+               {\r
+                       if (ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty == 1)\r
+                               dirty = 1;\r
+               }\r
+\r
+               if (dirty)\r
+               {\r
+                       u32 tmp_cpsr;\r
+\r
+                       /* send banked registers */\r
+                       xscale_send_u32(target, 0x1);\r
+\r
+                       tmp_cpsr = 0x0;\r
+                       tmp_cpsr |= armv4_5_number_to_mode(i);\r
+                       tmp_cpsr |= 0xc0; /* I/F bits */\r
+\r
+                       /* send CPSR for desired mode */\r
+                       xscale_send_u32(target, tmp_cpsr);\r
+\r
+                       /* send banked registers, r8 to r14, and spsr if not in USR/SYS mode */\r
+                       for (j = 8; j <= 14; j++)\r
+                       {\r
+                               xscale_send_u32(target, buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, j).value, 0, 32));\r
+                               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), j).dirty = 0;\r
+                       }\r
+\r
+                       if ((armv4_5_number_to_mode(i) != ARMV4_5_MODE_USR) && (armv4_5_number_to_mode(i) != ARMV4_5_MODE_SYS))\r
+                       {\r
+                               xscale_send_u32(target, buf_get_u32(ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 16).value, 0, 32));\r
+                               ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5_number_to_mode(i), 16).dirty = 0;\r
+                       }\r
+               }\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_read_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       u32 *buf32;\r
+       int i;\r
+\r
+       DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target not halted");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       /* sanitize arguments */\r
+       if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))\r
+               return ERROR_INVALID_ARGUMENTS;\r
+\r
+       if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))\r
+               return ERROR_TARGET_UNALIGNED_ACCESS;\r
+\r
+       /* send memory read request (command 0x1n, n: access size) */\r
+       xscale_send_u32(target, 0x10 | size);\r
+\r
+       /* send base address for read request */\r
+       xscale_send_u32(target, address);\r
+\r
+       /* send number of requested data words */\r
+       xscale_send_u32(target, count);\r
+\r
+       /* receive data from target (count times 32-bit words in host endianness) */\r
+       buf32 = malloc(4 * count);\r
+       xscale_receive(target, buf32, count);\r
+\r
+       /* extract data from host-endian buffer into byte stream */\r
+       for (i = 0; i < count; i++)\r
+       {\r
+               switch (size)\r
+               {\r
+                       case 4:\r
+                               target_buffer_set_u32(target, buffer, buf32[i]);\r
+                               buffer += 4;\r
+                               break;\r
+                       case 2:\r
+                               target_buffer_set_u16(target, buffer, buf32[i] & 0xffff);\r
+                               buffer += 2;\r
+                               break;\r
+                       case 1:\r
+                               *buffer++ = buf32[i] & 0xff;\r
+                               break;\r
+                       default:\r
+                               ERROR("should never get here");\r
+                               exit(-1);\r
+               }\r
+       }\r
+\r
+       free(buf32);\r
+\r
+       /* examine DCSR, to see if Sticky Abort (SA) got set */\r
+       xscale_read_dcsr(target);\r
+       if (buf_get_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 5, 1) == 1)\r
+       {\r
+               /* clear SA bit */\r
+               xscale_send_u32(target, 0x60);\r
+\r
+               return ERROR_TARGET_DATA_ABORT;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_write_memory(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       DEBUG("address: 0x%8.8x, size: 0x%8.8x, count: 0x%8.8x", address, size, count);\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target not halted");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       /* sanitize arguments */\r
+       if (((size != 4) && (size != 2) && (size != 1)) || (count == 0) || !(buffer))\r
+               return ERROR_INVALID_ARGUMENTS;\r
+\r
+       if (((size == 4) && (address & 0x3u)) || ((size == 2) && (address & 0x1u)))\r
+               return ERROR_TARGET_UNALIGNED_ACCESS;\r
+\r
+       /* send memory write request (command 0x2n, n: access size) */\r
+       xscale_send_u32(target, 0x20 | size);\r
+\r
+       /* send base address for read request */\r
+       xscale_send_u32(target, address);\r
+\r
+       /* send number of requested data words to be written*/\r
+       xscale_send_u32(target, count);\r
+\r
+       /* extract data from host-endian buffer into byte stream */\r
+#if 0\r
+       for (i = 0; i < count; i++)\r
+       {\r
+               switch (size)\r
+               {\r
+                       case 4:\r
+                               value = target_buffer_get_u32(target, buffer);\r
+                               xscale_send_u32(target, value);\r
+                               buffer += 4;\r
+                               break;\r
+                       case 2:\r
+                               value = target_buffer_get_u16(target, buffer);\r
+                               xscale_send_u32(target, value);\r
+                               buffer += 2;\r
+                               break;\r
+                       case 1:\r
+                               value = *buffer;\r
+                               xscale_send_u32(target, value);\r
+                               buffer += 1;\r
+                               break;\r
+                       default:\r
+                               ERROR("should never get here");\r
+                               exit(-1);\r
+               }\r
+       }\r
+#endif\r
+       xscale_send(target, buffer, count, size);\r
+\r
+       /* examine DCSR, to see if Sticky Abort (SA) got set */\r
+       xscale_read_dcsr(target);\r
+       if (buf_get_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 5, 1) == 1)\r
+       {\r
+               /* clear SA bit */\r
+               xscale_send_u32(target, 0x60);\r
+\r
+               return ERROR_TARGET_DATA_ABORT;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_bulk_write_memory(target_t *target, u32 address, u32 count, u8 *buffer)\r
+{\r
+       return xscale_write_memory(target, address, 4, count, buffer);\r
+}\r
+\r
+int xscale_checksum_memory(struct target_s *target, u32 address, u32 count, u32* checksum)\r
+{\r
+       return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;\r
+}\r
+\r
+u32 xscale_get_ttb(target_t *target)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       u32 ttb;\r
+\r
+       xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_TTB]);\r
+       ttb = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_TTB].value, 0, 32);\r
+\r
+       return ttb;\r
+}\r
+\r
+void xscale_disable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       u32 cp15_control;\r
+\r
+       /* read cp15 control register */\r
+       xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CTRL]);\r
+       cp15_control = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_CTRL].value, 0, 32);\r
+\r
+       if (mmu)\r
+               cp15_control &= ~0x1U;\r
+\r
+       if (d_u_cache)\r
+       {\r
+               /* clean DCache */\r
+               xscale_send_u32(target, 0x50);\r
+               xscale_send_u32(target, xscale->cache_clean_address);\r
+\r
+               /* invalidate DCache */\r
+               xscale_send_u32(target, 0x51);\r
+\r
+               cp15_control &= ~0x4U;\r
+       }\r
+\r
+       if (i_cache)\r
+       {\r
+               /* invalidate ICache */\r
+               xscale_send_u32(target, 0x52);\r
+               cp15_control &= ~0x1000U;\r
+       }\r
+\r
+       /* write new cp15 control register */\r
+       xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_CTRL], cp15_control);\r
+\r
+       /* execute cpwait to ensure outstanding operations complete */\r
+       xscale_send_u32(target, 0x53);\r
+}\r
+\r
+void xscale_enable_mmu_caches(target_t *target, int mmu, int d_u_cache, int i_cache)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       u32 cp15_control;\r
+\r
+       /* read cp15 control register */\r
+       xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CTRL]);\r
+       cp15_control = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_CTRL].value, 0, 32);\r
+\r
+       if (mmu)\r
+               cp15_control |= 0x1U;\r
+\r
+       if (d_u_cache)\r
+               cp15_control |= 0x4U;\r
+\r
+       if (i_cache)\r
+               cp15_control |= 0x1000U;\r
+\r
+       /* write new cp15 control register */\r
+       xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_CTRL], cp15_control);\r
+\r
+       /* execute cpwait to ensure outstanding operations complete */\r
+       xscale_send_u32(target, 0x53);\r
+}\r
+\r
+int xscale_set_breakpoint(struct target_s *target, breakpoint_t *breakpoint)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target not halted");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       if (xscale->force_hw_bkpts)\r
+               breakpoint->type = BKPT_HARD;\r
+\r
+       if (breakpoint->set)\r
+       {\r
+               WARNING("breakpoint already set");\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (breakpoint->type == BKPT_HARD)\r
+       {\r
+               u32 value = breakpoint->address | 1;\r
+               if (!xscale->ibcr0_used)\r
+               {\r
+                       xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_IBCR0], value);\r
+                       xscale->ibcr0_used = 1;\r
+                       breakpoint->set = 1;    /* breakpoint set on first breakpoint register */\r
+               }\r
+               else if (!xscale->ibcr1_used)\r
+               {\r
+                       xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_IBCR1], value);\r
+                       xscale->ibcr1_used = 1;\r
+                       breakpoint->set = 2;    /* breakpoint set on second breakpoint register */\r
+               }\r
+               else\r
+               {\r
+                       ERROR("BUG: no hardware comparator available");\r
+                       return ERROR_OK;\r
+               }\r
+       }\r
+       else if (breakpoint->type == BKPT_SOFT)\r
+       {\r
+               if (breakpoint->length == 4)\r
+               {\r
+                       /* keep the original instruction in target endianness */\r
+                       target->type->read_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr);\r
+                       /* write the original instruction in target endianness (arm7_9->arm_bkpt is host endian) */\r
+                       target_write_u32(target, breakpoint->address, xscale->arm_bkpt);\r
+               }\r
+               else\r
+               {\r
+                       /* keep the original instruction in target endianness */\r
+                       target->type->read_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr);\r
+                       /* write the original instruction in target endianness (arm7_9->arm_bkpt is host endian) */\r
+                       target_write_u32(target, breakpoint->address, xscale->thumb_bkpt);\r
+               }\r
+               breakpoint->set = 1;\r
+       }\r
+\r
+       return ERROR_OK;\r
+\r
+}\r
+\r
+int xscale_add_breakpoint(struct target_s *target, breakpoint_t *breakpoint)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target not halted");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       if (xscale->force_hw_bkpts)\r
+       {\r
+               DEBUG("forcing use of hardware breakpoint at address 0x%8.8x", breakpoint->address);\r
+               breakpoint->type = BKPT_HARD;\r
+       }\r
+\r
+       if ((breakpoint->type == BKPT_HARD) && (xscale->ibcr_available < 1))\r
+       {\r
+               INFO("no breakpoint unit available for hardware breakpoint");\r
+               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;\r
+       }\r
+       else\r
+       {\r
+               xscale->ibcr_available--;\r
+       }\r
+\r
+       if ((breakpoint->length != 2) && (breakpoint->length != 4))\r
+       {\r
+               INFO("only breakpoints of two (Thumb) or four (ARM) bytes length supported");\r
+               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_unset_breakpoint(struct target_s *target, breakpoint_t *breakpoint)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target not halted");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       if (!breakpoint->set)\r
+       {\r
+               WARNING("breakpoint not set");\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (breakpoint->type == BKPT_HARD)\r
+       {\r
+               if (breakpoint->set == 1)\r
+               {\r
+                       xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_IBCR0], 0x0);\r
+                       xscale->ibcr0_used = 0;\r
+               }\r
+               else if (breakpoint->set == 2)\r
+               {\r
+                       xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_IBCR1], 0x0);\r
+                       xscale->ibcr1_used = 0;\r
+               }\r
+               breakpoint->set = 0;\r
+       }\r
+       else\r
+       {\r
+               /* restore original instruction (kept in target endianness) */\r
+               if (breakpoint->length == 4)\r
+               {\r
+                       target->type->write_memory(target, breakpoint->address, 4, 1, breakpoint->orig_instr);\r
+               }\r
+               else\r
+               {\r
+                       target->type->write_memory(target, breakpoint->address, 2, 1, breakpoint->orig_instr);\r
+               }\r
+               breakpoint->set = 0;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_remove_breakpoint(struct target_s *target, breakpoint_t *breakpoint)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target not halted");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       if (breakpoint->set)\r
+       {\r
+               xscale_unset_breakpoint(target, breakpoint);\r
+       }\r
+\r
+       if (breakpoint->type == BKPT_HARD)\r
+               xscale->ibcr_available++;\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_set_watchpoint(struct target_s *target, watchpoint_t *watchpoint)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       u8 enable = 0;\r
+       reg_t *dbcon = &xscale->reg_cache->reg_list[XSCALE_DBCON];\r
+       u32 dbcon_value = buf_get_u32(dbcon->value, 0, 32);\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target not halted");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       xscale_get_reg(dbcon);\r
+\r
+       switch (watchpoint->rw)\r
+       {\r
+               case WPT_READ:\r
+                       enable = 0x3;\r
+                       break;\r
+               case WPT_ACCESS:\r
+                       enable = 0x2;\r
+                       break;\r
+               case WPT_WRITE:\r
+                       enable = 0x1;\r
+                       break;\r
+               default:\r
+                       ERROR("BUG: watchpoint->rw neither read, write nor access");\r
+       }\r
+\r
+       if (!xscale->dbr0_used)\r
+       {\r
+               xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_DBR0], watchpoint->address);\r
+               dbcon_value |= enable;\r
+               xscale_set_reg_u32(dbcon, dbcon_value);\r
+               watchpoint->set = 1;\r
+               xscale->dbr0_used = 1;\r
+       }\r
+       else if (!xscale->dbr1_used)\r
+       {\r
+               xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_DBR1], watchpoint->address);\r
+               dbcon_value |= enable << 2;\r
+               xscale_set_reg_u32(dbcon, dbcon_value);\r
+               watchpoint->set = 2;\r
+               xscale->dbr1_used = 1;\r
+       }\r
+       else\r
+       {\r
+               ERROR("BUG: no hardware comparator available");\r
+               return ERROR_OK;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_add_watchpoint(struct target_s *target, watchpoint_t *watchpoint)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target not halted");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       if (xscale->dbr_available < 1)\r
+       {\r
+               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;\r
+       }\r
+\r
+       if ((watchpoint->length != 1) && (watchpoint->length != 2) && (watchpoint->length != 4))\r
+       {\r
+               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;\r
+       }\r
+\r
+       xscale->dbr_available--;\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_unset_watchpoint(struct target_s *target, watchpoint_t *watchpoint)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       reg_t *dbcon = &xscale->reg_cache->reg_list[XSCALE_DBCON];\r
+       u32 dbcon_value = buf_get_u32(dbcon->value, 0, 32);\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target not halted");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       if (!watchpoint->set)\r
+       {\r
+               WARNING("breakpoint not set");\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (watchpoint->set == 1)\r
+       {\r
+               dbcon_value &= ~0x3;\r
+               xscale_set_reg_u32(dbcon, dbcon_value);\r
+               xscale->dbr0_used = 0;\r
+       }\r
+       else if (watchpoint->set == 2)\r
+       {\r
+               dbcon_value &= ~0xc;\r
+               xscale_set_reg_u32(dbcon, dbcon_value);\r
+               xscale->dbr1_used = 0;\r
+       }\r
+       watchpoint->set = 0;\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_remove_watchpoint(struct target_s *target, watchpoint_t *watchpoint)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target not halted");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       if (watchpoint->set)\r
+       {\r
+               xscale_unset_watchpoint(target, watchpoint);\r
+       }\r
+\r
+       xscale->dbr_available++;\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+void xscale_enable_watchpoints(struct target_s *target)\r
+{\r
+       watchpoint_t *watchpoint = target->watchpoints;\r
+\r
+       while (watchpoint)\r
+       {\r
+               if (watchpoint->set == 0)\r
+                       xscale_set_watchpoint(target, watchpoint);\r
+               watchpoint = watchpoint->next;\r
+       }\r
+}\r
+\r
+void xscale_enable_breakpoints(struct target_s *target)\r
+{\r
+       breakpoint_t *breakpoint = target->breakpoints;\r
+\r
+       /* set any pending breakpoints */\r
+       while (breakpoint)\r
+       {\r
+               if (breakpoint->set == 0)\r
+                       xscale_set_breakpoint(target, breakpoint);\r
+               breakpoint = breakpoint->next;\r
+       }\r
+}\r
+\r
+int xscale_get_reg(reg_t *reg)\r
+{\r
+       xscale_reg_t *arch_info = reg->arch_info;\r
+       target_t *target = arch_info->target;\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       /* DCSR, TX and RX are accessible via JTAG */\r
+       if (strcmp(reg->name, "XSCALE_DCSR") == 0)\r
+       {\r
+               return xscale_read_dcsr(arch_info->target);\r
+       }\r
+       else if (strcmp(reg->name, "XSCALE_TX") == 0)\r
+       {\r
+               /* 1 = consume register content */\r
+               return xscale_read_tx(arch_info->target, 1);\r
+       }\r
+       else if (strcmp(reg->name, "XSCALE_RX") == 0)\r
+       {\r
+               /* can't read from RX register (host -> debug handler) */\r
+               return ERROR_OK;\r
+       }\r
+       else if (strcmp(reg->name, "XSCALE_TXRXCTRL") == 0)\r
+       {\r
+               /* can't (explicitly) read from TXRXCTRL register */\r
+               return ERROR_OK;\r
+       }\r
+       else /* Other DBG registers have to be transfered by the debug handler */\r
+       {\r
+               /* send CP read request (command 0x40) */\r
+               xscale_send_u32(target, 0x40);\r
+\r
+               /* send CP register number */\r
+               xscale_send_u32(target, arch_info->dbg_handler_number);\r
+\r
+               /* read register value */\r
+               xscale_read_tx(target, 1);\r
+               buf_cpy(xscale->reg_cache->reg_list[XSCALE_TX].value, reg->value, 32);\r
+\r
+               reg->dirty = 0;\r
+               reg->valid = 1;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_set_reg(reg_t *reg, u8* buf)\r
+{\r
+       xscale_reg_t *arch_info = reg->arch_info;\r
+       target_t *target = arch_info->target;\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       u32 value = buf_get_u32(buf, 0, 32);\r
+\r
+       /* DCSR, TX and RX are accessible via JTAG */\r
+       if (strcmp(reg->name, "XSCALE_DCSR") == 0)\r
+       {\r
+               buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 0, 32, value);\r
+               return xscale_write_dcsr(arch_info->target, -1, -1);\r
+       }\r
+       else if (strcmp(reg->name, "XSCALE_RX") == 0)\r
+       {\r
+               buf_set_u32(xscale->reg_cache->reg_list[XSCALE_RX].value, 0, 32, value);\r
+               return xscale_write_rx(arch_info->target);\r
+       }\r
+       else if (strcmp(reg->name, "XSCALE_TX") == 0)\r
+       {\r
+               /* can't write to TX register (debug-handler -> host) */\r
+               return ERROR_OK;\r
+       }\r
+       else if (strcmp(reg->name, "XSCALE_TXRXCTRL") == 0)\r
+       {\r
+               /* can't (explicitly) write to TXRXCTRL register */\r
+               return ERROR_OK;\r
+       }\r
+       else /* Other DBG registers have to be transfered by the debug handler */\r
+       {\r
+               /* send CP write request (command 0x41) */\r
+               xscale_send_u32(target, 0x41);\r
+\r
+               /* send CP register number */\r
+               xscale_send_u32(target, arch_info->dbg_handler_number);\r
+\r
+               /* send CP register value */\r
+               xscale_send_u32(target, value);\r
+               buf_set_u32(reg->value, 0, 32, value);\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* convenience wrapper to access XScale specific registers */\r
+int xscale_set_reg_u32(reg_t *reg, u32 value)\r
+{\r
+       u8 buf[4];\r
+\r
+       buf_set_u32(buf, 0, 32, value);\r
+\r
+       return xscale_set_reg(reg, buf);\r
+}\r
+\r
+int xscale_write_dcsr_sw(target_t *target, u32 value)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       reg_t *dcsr = &xscale->reg_cache->reg_list[XSCALE_DCSR];\r
+       xscale_reg_t *dcsr_arch_info = dcsr->arch_info;\r
+\r
+       /* send CP write request (command 0x41) */\r
+       xscale_send_u32(target, 0x41);\r
+\r
+       /* send CP register number */\r
+       xscale_send_u32(target, dcsr_arch_info->dbg_handler_number);\r
+\r
+       /* send CP register value */\r
+       xscale_send_u32(target, value);\r
+       buf_set_u32(dcsr->value, 0, 32, value);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_read_trace(target_t *target)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       xscale_trace_data_t **trace_data_p;\r
+\r
+       /* 258 words from debug handler\r
+        * 256 trace buffer entries\r
+        * 2 checkpoint addresses\r
+        */\r
+       u32 trace_buffer[258];\r
+       int is_address[256];\r
+       int i, j;\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               WARNING("target must be stopped to read trace data");\r
+               return ERROR_TARGET_NOT_HALTED;\r
+       }\r
+\r
+       /* send read trace buffer command (command 0x61) */\r
+       xscale_send_u32(target, 0x61);\r
+\r
+       /* receive trace buffer content */\r
+       xscale_receive(target, trace_buffer, 258);\r
+\r
+       /* parse buffer backwards to identify address entries */\r
+       for (i = 255; i >= 0; i--)\r
+       {\r
+               is_address[i] = 0;\r
+               if (((trace_buffer[i] & 0xf0) == 0x90) ||\r
+                       ((trace_buffer[i] & 0xf0) == 0xd0))\r
+               {\r
+                       if (i >= 3)\r
+                               is_address[--i] = 1;\r
+                       if (i >= 2)\r
+                               is_address[--i] = 1;\r
+                       if (i >= 1)\r
+                               is_address[--i] = 1;\r
+                       if (i >= 0)\r
+                               is_address[--i] = 1;\r
+               }\r
+       }\r
+\r
+\r
+       /* search first non-zero entry */\r
+       for (j = 0; (j < 256) && (trace_buffer[j] == 0) && (!is_address[j]); j++)\r
+               ;\r
+\r
+       if (j == 256)\r
+       {\r
+               DEBUG("no trace data collected");\r
+               return ERROR_XSCALE_NO_TRACE_DATA;\r
+       }\r
+\r
+       for (trace_data_p = &xscale->trace.data; *trace_data_p; trace_data_p = &(*trace_data_p)->next)\r
+               ;\r
+\r
+       *trace_data_p = malloc(sizeof(xscale_trace_data_t));\r
+       (*trace_data_p)->next = NULL;\r
+       (*trace_data_p)->chkpt0 = trace_buffer[256];\r
+       (*trace_data_p)->chkpt1 = trace_buffer[257];\r
+       (*trace_data_p)->last_instruction = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);\r
+       (*trace_data_p)->entries = malloc(sizeof(xscale_trace_entry_t) * (256 - j));\r
+       (*trace_data_p)->depth = 256 - j;\r
+\r
+       for (i = j; i < 256; i++)\r
+       {\r
+               (*trace_data_p)->entries[i - j].data = trace_buffer[i];\r
+               if (is_address[i])\r
+                       (*trace_data_p)->entries[i - j].type = XSCALE_TRACE_ADDRESS;\r
+               else\r
+                       (*trace_data_p)->entries[i - j].type = XSCALE_TRACE_MESSAGE;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_read_instruction(target_t *target, arm_instruction_t *instruction)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       int i;\r
+       int section = -1;\r
+       u32 size_read;\r
+       u32 opcode;\r
+       int retval;\r
+\r
+       if (!xscale->trace.image)\r
+               return ERROR_TRACE_IMAGE_UNAVAILABLE;\r
+\r
+       /* search for the section the current instruction belongs to */\r
+       for (i = 0; i < xscale->trace.image->num_sections; i++)\r
+       {\r
+               if ((xscale->trace.image->sections[i].base_address <= xscale->trace.current_pc) &&\r
+                       (xscale->trace.image->sections[i].base_address + xscale->trace.image->sections[i].size > xscale->trace.current_pc))\r
+               {\r
+                       section = i;\r
+                       break;\r
+               }\r
+       }\r
+\r
+       if (section == -1)\r
+       {\r
+               /* current instruction couldn't be found in the image */\r
+               return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;\r
+       }\r
+\r
+       if (xscale->trace.core_state == ARMV4_5_STATE_ARM)\r
+       {\r
+               u8 buf[4];\r
+               if ((retval = image_read_section(xscale->trace.image, section,\r
+                       xscale->trace.current_pc - xscale->trace.image->sections[section].base_address,\r
+                       4, buf, &size_read)) != ERROR_OK)\r
+               {\r
+                       ERROR("error while reading instruction: %i", retval);\r
+                       return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;\r
+               }\r
+               opcode = target_buffer_get_u32(target, buf);\r
+               arm_evaluate_opcode(opcode, xscale->trace.current_pc, instruction);\r
+       }\r
+       else if (xscale->trace.core_state == ARMV4_5_STATE_THUMB)\r
+       {\r
+               u8 buf[2];\r
+               if ((retval = image_read_section(xscale->trace.image, section,\r
+                       xscale->trace.current_pc - xscale->trace.image->sections[section].base_address,\r
+                       2, buf, &size_read)) != ERROR_OK)\r
+               {\r
+                       ERROR("error while reading instruction: %i", retval);\r
+                       return ERROR_TRACE_INSTRUCTION_UNAVAILABLE;\r
+               }\r
+               opcode = target_buffer_get_u16(target, buf);\r
+               thumb_evaluate_opcode(opcode, xscale->trace.current_pc, instruction);\r
+       }\r
+       else\r
+       {\r
+               ERROR("BUG: unknown core state encountered");\r
+               exit(-1);\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_branch_address(xscale_trace_data_t *trace_data, int i, u32 *target)\r
+{\r
+       /* if there are less than four entries prior to the indirect branch message\r
+        * we can't extract the address */\r
+       if (i < 4)\r
+       {\r
+               return -1;\r
+       }\r
+\r
+       *target = (trace_data->entries[i-1].data) | (trace_data->entries[i-2].data << 8) |\r
+                               (trace_data->entries[i-3].data << 16) | (trace_data->entries[i-4].data << 24);\r
+\r
+       return 0;\r
+}\r
+\r
+int xscale_analyze_trace(target_t *target, command_context_t *cmd_ctx)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+       int next_pc_ok = 0;\r
+       u32 next_pc = 0x0;\r
+       xscale_trace_data_t *trace_data = xscale->trace.data;\r
+       int retval;\r
+\r
+       while (trace_data)\r
+       {\r
+               int i, chkpt;\r
+               int rollover;\r
+               int branch;\r
+               int exception;\r
+               xscale->trace.core_state = ARMV4_5_STATE_ARM;\r
+\r
+               chkpt = 0;\r
+               rollover = 0;\r
+\r
+               for (i = 0; i < trace_data->depth; i++)\r
+               {\r
+                       next_pc_ok = 0;\r
+                       branch = 0;\r
+                       exception = 0;\r
+\r
+                       if (trace_data->entries[i].type == XSCALE_TRACE_ADDRESS)\r
+                               continue;\r
+\r
+                       switch ((trace_data->entries[i].data & 0xf0) >> 4)\r
+                       {\r
+                               case 0:         /* Exceptions */\r
+                               case 1:\r
+                               case 2:\r
+                               case 3:\r
+                               case 4:\r
+                               case 5:\r
+                               case 6:\r
+                               case 7:\r
+                                       exception = (trace_data->entries[i].data & 0x70) >> 4;\r
+                                       next_pc_ok = 1;\r
+                                       next_pc = (trace_data->entries[i].data & 0xf0) >> 2;\r
+                                       command_print(cmd_ctx, "--- exception %i ---", (trace_data->entries[i].data & 0xf0) >> 4);\r
+                                       break;\r
+                               case 8:         /* Direct Branch */\r
+                                       branch = 1;\r
+                                       break;\r
+                               case 9:         /* Indirect Branch */\r
+                                       branch = 1;\r
+                                       if (xscale_branch_address(trace_data, i, &next_pc) == 0)\r
+                                       {\r
+                                               next_pc_ok = 1;\r
+                                       }\r
+                                       break;\r
+                               case 13:        /* Checkpointed Indirect Branch */\r
+                                       if (xscale_branch_address(trace_data, i, &next_pc) == 0)\r
+                                       {\r
+                                               next_pc_ok = 1;\r
+                                               if (((chkpt == 0) && (next_pc != trace_data->chkpt0))\r
+                                                       || ((chkpt == 1) && (next_pc != trace_data->chkpt1)))\r
+                                                       WARNING("checkpointed indirect branch target address doesn't match checkpoint");\r
+                                       }\r
+                                       /* explicit fall-through */\r
+                               case 12:        /* Checkpointed Direct Branch */\r
+                                       branch = 1;\r
+                                       if (chkpt == 0)\r
+                                       {\r
+                                               next_pc_ok = 1;\r
+                                               next_pc = trace_data->chkpt0;\r
+                                               chkpt++;\r
+                                       }\r
+                                       else if (chkpt == 1)\r
+                                       {\r
+                                               next_pc_ok = 1;\r
+                                               next_pc = trace_data->chkpt0;\r
+                                               chkpt++;\r
+                                       }\r
+                                       else\r
+                                       {\r
+                                               WARNING("more than two checkpointed branches encountered");\r
+                                       }\r
+                                       break;\r
+                               case 15:        /* Roll-over */\r
+                                       rollover++;\r
+                                       continue;\r
+                               default:        /* Reserved */\r
+                                       command_print(cmd_ctx, "--- reserved trace message ---");\r
+                                       ERROR("BUG: trace message %i is reserved", (trace_data->entries[i].data & 0xf0) >> 4);\r
+                                       return ERROR_OK;\r
+                       }\r
+\r
+                       if (xscale->trace.pc_ok)\r
+                       {\r
+                               int executed = (trace_data->entries[i].data & 0xf) + rollover * 16;\r
+                               arm_instruction_t instruction;\r
+\r
+                               if ((exception == 6) || (exception == 7))\r
+                               {\r
+                                       /* IRQ or FIQ exception, no instruction executed */\r
+                                       executed -= 1;\r
+                               }\r
+\r
+                               while (executed-- >= 0)\r
+                               {\r
+                                       if ((retval = xscale_read_instruction(target, &instruction)) != ERROR_OK)\r
+                                       {\r
+                                               /* can't continue tracing with no image available */\r
+                                               if (retval == ERROR_TRACE_IMAGE_UNAVAILABLE)\r
+                                               {\r
+                                                       return retval;\r
+                                               }\r
+                                               else if (retval == ERROR_TRACE_INSTRUCTION_UNAVAILABLE)\r
+                                               {\r
+                                                       /* TODO: handle incomplete images */\r
+                                               }\r
+                                       }\r
+\r
+                                       /* a precise abort on a load to the PC is included in the incremental\r
+                                        * word count, other instructions causing data aborts are not included\r
+                                        */\r
+                                       if ((executed == 0) && (exception == 4)\r
+                                               && ((instruction.type >= ARM_LDR) && (instruction.type <= ARM_LDM)))\r
+                                       {\r
+                                               if ((instruction.type == ARM_LDM)\r
+                                                       && ((instruction.info.load_store_multiple.register_list & 0x8000) == 0))\r
+                                               {\r
+                                                       executed--;\r
+                                               }\r
+                                               else if (((instruction.type >= ARM_LDR) && (instruction.type <= ARM_LDRSH))\r
+                                                       && (instruction.info.load_store.Rd != 15))\r
+                                               {\r
+                                                       executed--;\r
+                                               }\r
+                                       }\r
+\r
+                                       /* only the last instruction executed\r
+                                        * (the one that caused the control flow change)\r
+                                        * could be a taken branch\r
+                                        */\r
+                                       if (((executed == -1) && (branch == 1)) &&\r
+                                               (((instruction.type == ARM_B) ||\r
+                                                       (instruction.type == ARM_BL) ||\r
+                                                       (instruction.type == ARM_BLX)) &&\r
+                                                       (instruction.info.b_bl_bx_blx.target_address != -1)))\r
+                                       {\r
+                                               xscale->trace.current_pc = instruction.info.b_bl_bx_blx.target_address;\r
+                                       }\r
+                                       else\r
+                                       {\r
+                                               xscale->trace.current_pc += (xscale->trace.core_state == ARMV4_5_STATE_ARM) ? 4 : 2;\r
+                                       }\r
+                                       command_print(cmd_ctx, "%s", instruction.text);\r
+                               }\r
+\r
+                               rollover = 0;\r
+                       }\r
+\r
+                       if (next_pc_ok)\r
+                       {\r
+                               xscale->trace.current_pc = next_pc;\r
+                               xscale->trace.pc_ok = 1;\r
+                       }\r
+               }\r
+\r
+               for (; xscale->trace.current_pc < trace_data->last_instruction; xscale->trace.current_pc += (xscale->trace.core_state == ARMV4_5_STATE_ARM) ? 4 : 2)\r
+               {\r
+                       arm_instruction_t instruction;\r
+                       if ((retval = xscale_read_instruction(target, &instruction)) != ERROR_OK)\r
+                       {\r
+                               /* can't continue tracing with no image available */\r
+                               if (retval == ERROR_TRACE_IMAGE_UNAVAILABLE)\r
+                               {\r
+                                       return retval;\r
+                               }\r
+                               else if (retval == ERROR_TRACE_INSTRUCTION_UNAVAILABLE)\r
+                               {\r
+                                       /* TODO: handle incomplete images */\r
+                               }\r
+                       }\r
+                       command_print(cmd_ctx, "%s", instruction.text);\r
+               }\r
+\r
+               trace_data = trace_data->next;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+void xscale_build_reg_cache(target_t *target)\r
+{\r
+       /* get pointers to arch-specific information */\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       reg_cache_t **cache_p = register_get_last_cache_p(&target->reg_cache);\r
+       xscale_reg_t *arch_info = malloc(sizeof(xscale_reg_arch_info));\r
+       int i;\r
+       int num_regs = sizeof(xscale_reg_arch_info) / sizeof(xscale_reg_t);\r
+\r
+       (*cache_p) = armv4_5_build_reg_cache(target, armv4_5);\r
+       armv4_5->core_cache = (*cache_p);\r
+\r
+       /* register a register arch-type for XScale dbg registers only once */\r
+       if (xscale_reg_arch_type == -1)\r
+               xscale_reg_arch_type = register_reg_arch_type(xscale_get_reg, xscale_set_reg);\r
+\r
+       (*cache_p)->next = malloc(sizeof(reg_cache_t));\r
+       cache_p = &(*cache_p)->next;\r
+\r
+       /* fill in values for the xscale reg cache */\r
+       (*cache_p)->name = "XScale registers";\r
+       (*cache_p)->next = NULL;\r
+       (*cache_p)->reg_list = malloc(num_regs * sizeof(reg_t));\r
+       (*cache_p)->num_regs = num_regs;\r
+\r
+       for (i = 0; i < num_regs; i++)\r
+       {\r
+               (*cache_p)->reg_list[i].name = xscale_reg_list[i];\r
+               (*cache_p)->reg_list[i].value = calloc(4, 1);\r
+               (*cache_p)->reg_list[i].dirty = 0;\r
+               (*cache_p)->reg_list[i].valid = 0;\r
+               (*cache_p)->reg_list[i].size = 32;\r
+               (*cache_p)->reg_list[i].bitfield_desc = NULL;\r
+               (*cache_p)->reg_list[i].num_bitfields = 0;\r
+               (*cache_p)->reg_list[i].arch_info = &arch_info[i];\r
+               (*cache_p)->reg_list[i].arch_type = xscale_reg_arch_type;\r
+               arch_info[i] = xscale_reg_arch_info[i];\r
+               arch_info[i].target = target;\r
+       }\r
+\r
+       xscale->reg_cache = (*cache_p);\r
+}\r
+\r
+int xscale_init_target(struct command_context_s *cmd_ctx, struct target_s *target)\r
+{\r
+       if (startup_mode != DAEMON_RESET)\r
+       {\r
+               ERROR("XScale target requires a reset");\r
+               ERROR("Reset target to enable debug");\r
+       }\r
+\r
+       /* assert TRST once during startup */\r
+       jtag_add_reset(1, 0);\r
+       jtag_add_sleep(5000);\r
+       jtag_add_reset(0, 0);\r
+       jtag_execute_queue();\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_quit()\r
+{\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_init_arch_info(target_t *target, xscale_common_t *xscale, int chain_pos, char *variant)\r
+{\r
+       armv4_5_common_t *armv4_5;\r
+       u32 high_reset_branch, low_reset_branch;\r
+       int i;\r
+\r
+       armv4_5 = &xscale->armv4_5_common;\r
+\r
+       /* store architecture specfic data (none so far) */\r
+       xscale->arch_info = NULL;\r
+       xscale->common_magic = XSCALE_COMMON_MAGIC;\r
+\r
+       /* remember the variant (PXA25x, PXA27x, IXP42x, ...) */\r
+       xscale->variant = strdup(variant);\r
+\r
+       /* prepare JTAG information for the new target */\r
+       xscale->jtag_info.chain_pos = chain_pos;\r
+       jtag_register_event_callback(xscale_jtag_callback, target);\r
+\r
+       xscale->jtag_info.dbgrx = 0x02;\r
+       xscale->jtag_info.dbgtx = 0x10;\r
+       xscale->jtag_info.dcsr = 0x09;\r
+       xscale->jtag_info.ldic = 0x07;\r
+\r
+       if ((strcmp(xscale->variant, "pxa250") == 0) ||\r
+               (strcmp(xscale->variant, "pxa255") == 0) ||\r
+               (strcmp(xscale->variant, "pxa26x") == 0))\r
+       {\r
+               xscale->jtag_info.ir_length = 5;\r
+       }\r
+       else if ((strcmp(xscale->variant, "pxa27x") == 0) ||\r
+               (strcmp(xscale->variant, "ixp42x") == 0) ||\r
+               (strcmp(xscale->variant, "ixp45x") == 0) ||\r
+               (strcmp(xscale->variant, "ixp46x") == 0))\r
+       {\r
+               xscale->jtag_info.ir_length = 7;\r
+       }\r
+\r
+       /* the debug handler isn't installed (and thus not running) at this time */\r
+       xscale->handler_installed = 0;\r
+       xscale->handler_running = 0;\r
+       xscale->handler_address = 0xfe000800;\r
+\r
+       /* clear the vectors we keep locally for reference */\r
+       memset(xscale->low_vectors, 0, sizeof(xscale->low_vectors));\r
+       memset(xscale->high_vectors, 0, sizeof(xscale->high_vectors));\r
+\r
+       /* no user-specified vectors have been configured yet */\r
+       xscale->static_low_vectors_set = 0x0;\r
+       xscale->static_high_vectors_set = 0x0;\r
+\r
+       /* calculate branches to debug handler */\r
+       low_reset_branch = (xscale->handler_address + 0x20 - 0x0 - 0x8) >> 2;\r
+       high_reset_branch = (xscale->handler_address + 0x20 - 0xffff0000 - 0x8) >> 2;\r
+\r
+       xscale->low_vectors[0] = ARMV4_5_B((low_reset_branch & 0xffffff), 0);\r
+       xscale->high_vectors[0] = ARMV4_5_B((high_reset_branch & 0xffffff), 0);\r
+\r
+       for (i = 1; i <= 7; i++)\r
+       {\r
+               xscale->low_vectors[i] = ARMV4_5_B(0xfffffe, 0);\r
+               xscale->high_vectors[i] = ARMV4_5_B(0xfffffe, 0);\r
+       }\r
+\r
+       /* 64kB aligned region used for DCache cleaning */\r
+       xscale->cache_clean_address = 0xfffe0000;\r
+\r
+       xscale->hold_rst = 0;\r
+       xscale->external_debug_break = 0;\r
+\r
+       xscale->force_hw_bkpts = 1;\r
+\r
+       xscale->ibcr_available = 2;\r
+       xscale->ibcr0_used = 0;\r
+       xscale->ibcr1_used = 0;\r
+\r
+       xscale->dbr_available = 2;\r
+       xscale->dbr0_used = 0;\r
+       xscale->dbr1_used = 0;\r
+\r
+       xscale->arm_bkpt = ARMV5_BKPT(0x0);\r
+       xscale->thumb_bkpt = ARMV5_T_BKPT(0x0) & 0xffff;\r
+\r
+       xscale->vector_catch = 0x1;\r
+\r
+       xscale->trace.capture_status = TRACE_IDLE;\r
+       xscale->trace.data = NULL;\r
+       xscale->trace.image = NULL;\r
+       xscale->trace.buffer_enabled = 0;\r
+       xscale->trace.buffer_fill = 0;\r
+\r
+       /* prepare ARMv4/5 specific information */\r
+       armv4_5->arch_info = xscale;\r
+       armv4_5->read_core_reg = xscale_read_core_reg;\r
+       armv4_5->write_core_reg = xscale_write_core_reg;\r
+       armv4_5->full_context = xscale_full_context;\r
+\r
+       armv4_5_init_arch_info(target, armv4_5);\r
+\r
+       xscale->armv4_5_mmu.armv4_5_cache.ctype = -1;\r
+       xscale->armv4_5_mmu.get_ttb = xscale_get_ttb;\r
+       xscale->armv4_5_mmu.read_memory = xscale_read_memory;\r
+       xscale->armv4_5_mmu.write_memory = xscale_write_memory;\r
+       xscale->armv4_5_mmu.disable_mmu_caches = xscale_disable_mmu_caches;\r
+       xscale->armv4_5_mmu.enable_mmu_caches = xscale_enable_mmu_caches;\r
+       xscale->armv4_5_mmu.has_tiny_pages = 1;\r
+       xscale->armv4_5_mmu.mmu_enabled = 0;\r
+\r
+       xscale->fast_memory_access = 0;\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+/* target xscale <endianess> <startup_mode> <chain_pos> <variant> */\r
+int xscale_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct target_s *target)\r
+{\r
+       int chain_pos;\r
+       char *variant = NULL;\r
+       xscale_common_t *xscale = malloc(sizeof(xscale_common_t));\r
+\r
+       if (argc < 5)\r
+       {\r
+               ERROR("'target xscale' requires four arguments: <endianess> <startup_mode> <chain_pos> <variant>");\r
+               return ERROR_OK;\r
+       }\r
+\r
+       chain_pos = strtoul(args[3], NULL, 0);\r
+\r
+       variant = args[4];\r
+\r
+       xscale_init_arch_info(target, xscale, chain_pos, variant);\r
+       xscale_build_reg_cache(target);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_handle_debug_handler_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = NULL;\r
+       armv4_5_common_t *armv4_5;\r
+       xscale_common_t *xscale;\r
+\r
+       u32 handler_address;\r
+\r
+       if (argc < 2)\r
+       {\r
+               ERROR("'xscale debug_handler <target#> <address>' command takes two required operands");\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if ((target = get_target_by_num(strtoul(args[0], NULL, 0))) == NULL)\r
+       {\r
+               ERROR("no target '%s' configured", args[0]);\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)\r
+       {\r
+               return ERROR_OK;\r
+       }\r
+\r
+       handler_address = strtoul(args[1], NULL, 0);\r
+\r
+       if (((handler_address >= 0x800) && (handler_address <= 0x1fef800)) ||\r
+               ((handler_address >= 0xfe000800) && (handler_address <= 0xfffff800)))\r
+       {\r
+               xscale->handler_address = handler_address;\r
+       }\r
+       else\r
+       {\r
+               ERROR("xscale debug_handler <address> must be between 0x800 and 0x1fef800 or between 0xfe000800 and 0xfffff800");\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_handle_cache_clean_address_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = NULL;\r
+       armv4_5_common_t *armv4_5;\r
+       xscale_common_t *xscale;\r
+\r
+       u32 cache_clean_address;\r
+\r
+       if (argc < 2)\r
+       {\r
+               ERROR("'xscale cache_clean_address <target#> <address>' command takes two required operands");\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if ((target = get_target_by_num(strtoul(args[0], NULL, 0))) == NULL)\r
+       {\r
+               ERROR("no target '%s' configured", args[0]);\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)\r
+       {\r
+               return ERROR_OK;\r
+       }\r
+\r
+       cache_clean_address = strtoul(args[1], NULL, 0);\r
+\r
+       if (cache_clean_address & 0xffff)\r
+       {\r
+               ERROR("xscale cache_clean_address <address> must be 64kb aligned");\r
+       }\r
+       else\r
+       {\r
+               xscale->cache_clean_address = cache_clean_address;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_handle_cache_info_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       xscale_common_t *xscale;\r
+\r
+       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)\r
+       {\r
+               return ERROR_OK;\r
+       }\r
+\r
+       return armv4_5_handle_cache_info_command(cmd_ctx, &xscale->armv4_5_mmu.armv4_5_cache);\r
+}\r
+\r
+static int xscale_virt2phys(struct target_s *target, u32 virtual, u32 *physical)\r
+{\r
+       armv4_5_common_t *armv4_5;\r
+       xscale_common_t *xscale;\r
+       int retval;\r
+       int type;\r
+       u32 cb;\r
+       int domain;\r
+       u32 ap;\r
+       \r
+       if ((retval = xscale_get_arch_pointers(target, &armv4_5, &xscale)) != ERROR_OK)\r
+       {\r
+               return retval;\r
+       }\r
+       u32 ret = armv4_5_mmu_translate_va(target, &xscale->armv4_5_mmu, virtual, &type, &cb, &domain, &ap);\r
+       if (type == -1)\r
+       {\r
+               return ret;\r
+       }\r
+       \r
+       *physical = ret;\r
+       return ERROR_OK;\r
+}\r
+\r
+static int xscale_mmu(struct target_s *target, int *enabled)\r
+{\r
+       armv4_5_common_t *armv4_5 = target->arch_info;\r
+       xscale_common_t *xscale = armv4_5->arch_info;\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               ERROR("Target not halted");\r
+               return ERROR_TARGET_INVALID;\r
+       }\r
+       \r
+       *enabled = xscale->armv4_5_mmu.mmu_enabled;\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_handle_mmu_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       xscale_common_t *xscale;\r
+\r
+       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)\r
+       {\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (argc >= 1)\r
+       {\r
+               if (strcmp("enable", args[0]) == 0)\r
+               {\r
+                       xscale_enable_mmu_caches(target, 1, 0, 0);\r
+                       xscale->armv4_5_mmu.mmu_enabled = 1;\r
+               }\r
+               else if (strcmp("disable", args[0]) == 0)\r
+               {\r
+                       xscale_disable_mmu_caches(target, 1, 0, 0);\r
+                       xscale->armv4_5_mmu.mmu_enabled = 0;\r
+               }\r
+       }\r
+\r
+       command_print(cmd_ctx, "mmu %s", (xscale->armv4_5_mmu.mmu_enabled) ? "enabled" : "disabled");\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_handle_idcache_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       xscale_common_t *xscale;\r
+       int icache = 0, dcache = 0;\r
+\r
+       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)\r
+       {\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (strcmp(cmd, "icache") == 0)\r
+               icache = 1;\r
+       else if (strcmp(cmd, "dcache") == 0)\r
+               dcache = 1;\r
+\r
+       if (argc >= 1)\r
+       {\r
+               if (strcmp("enable", args[0]) == 0)\r
+               {\r
+                       xscale_enable_mmu_caches(target, 0, dcache, icache);\r
+\r
+                       if (icache)\r
+                               xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 1;\r
+                       else if (dcache)\r
+                               xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 1;\r
+               }\r
+               else if (strcmp("disable", args[0]) == 0)\r
+               {\r
+                       xscale_disable_mmu_caches(target, 0, dcache, icache);\r
+\r
+                       if (icache)\r
+                               xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;\r
+                       else if (dcache)\r
+                               xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 0;\r
+               }\r
+       }\r
+\r
+       if (icache)\r
+               command_print(cmd_ctx, "icache %s", (xscale->armv4_5_mmu.armv4_5_cache.i_cache_enabled) ? "enabled" : "disabled");\r
+\r
+       if (dcache)\r
+               command_print(cmd_ctx, "dcache %s", (xscale->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled) ? "enabled" : "disabled");\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_handle_vector_catch_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       xscale_common_t *xscale;\r
+\r
+       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)\r
+       {\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (argc < 1)\r
+       {\r
+               command_print(cmd_ctx, "usage: xscale vector_catch [mask]");\r
+       }\r
+       else\r
+       {\r
+               xscale->vector_catch = strtoul(args[0], NULL, 0);\r
+               buf_set_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 16, 8, xscale->vector_catch);\r
+               xscale_write_dcsr(target, -1, -1);\r
+       }\r
+\r
+       command_print(cmd_ctx, "vector catch mask: 0x%2.2x", xscale->vector_catch);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_handle_force_hw_bkpts_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       xscale_common_t *xscale;\r
+\r
+       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)\r
+       {\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if ((argc >= 1) && (strcmp("enable", args[0]) == 0))\r
+       {\r
+               xscale->force_hw_bkpts = 1;\r
+       }\r
+       else if ((argc >= 1) && (strcmp("disable", args[0]) == 0))\r
+       {\r
+               xscale->force_hw_bkpts = 0;\r
+       }\r
+       else\r
+       {\r
+               command_print(cmd_ctx, "usage: xscale force_hw_bkpts <enable|disable>");\r
+       }\r
+\r
+       command_print(cmd_ctx, "force hardware breakpoints %s", (xscale->force_hw_bkpts) ? "enabled" : "disabled");\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_handle_trace_buffer_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       xscale_common_t *xscale;\r
+       u32 dcsr_value;\r
+\r
+       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)\r
+       {\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if ((argc >= 1) && (strcmp("enable", args[0]) == 0))\r
+       {\r
+               xscale_trace_data_t *td, *next_td;\r
+               xscale->trace.buffer_enabled = 1;\r
+\r
+               /* free old trace data */\r
+               td = xscale->trace.data;\r
+               while (td)\r
+               {\r
+                       next_td = td->next;\r
+\r
+                       if (td->entries)\r
+                               free(td->entries);\r
+                       free(td);\r
+                       td = next_td;\r
+               }\r
+               xscale->trace.data = NULL;\r
+       }\r
+       else if ((argc >= 1) && (strcmp("disable", args[0]) == 0))\r
+       {\r
+               xscale->trace.buffer_enabled = 0;\r
+       }\r
+\r
+       if ((argc >= 2) && (strcmp("fill", args[1]) == 0))\r
+       {\r
+               if (argc >= 3)\r
+                       xscale->trace.buffer_fill = strtoul(args[2], NULL, 0);\r
+               else\r
+                       xscale->trace.buffer_fill = 1;\r
+       }\r
+       else if ((argc >= 2) && (strcmp("wrap", args[1]) == 0))\r
+       {\r
+               xscale->trace.buffer_fill = -1;\r
+       }\r
+\r
+       if (xscale->trace.buffer_enabled)\r
+       {\r
+               /* if we enable the trace buffer in fill-once\r
+                * mode we know the address of the first instruction */\r
+               xscale->trace.pc_ok = 1;\r
+               xscale->trace.current_pc = buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32);\r
+       }\r
+       else\r
+       {\r
+               /* otherwise the address is unknown, and we have no known good PC */\r
+               xscale->trace.pc_ok = 0;\r
+       }\r
+\r
+       command_print(cmd_ctx, "trace buffer %s (%s)",\r
+               (xscale->trace.buffer_enabled) ? "enabled" : "disabled",\r
+               (xscale->trace.buffer_fill > 0) ? "fill" : "wrap");\r
+\r
+       dcsr_value = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_DCSR].value, 0, 32);\r
+       if (xscale->trace.buffer_fill >= 0)\r
+               xscale_write_dcsr_sw(target, (dcsr_value & 0xfffffffc) | 2);\r
+       else\r
+               xscale_write_dcsr_sw(target, dcsr_value & 0xfffffffc);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_handle_trace_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target;\r
+       armv4_5_common_t *armv4_5;\r
+       xscale_common_t *xscale;\r
+\r
+       if (argc < 1)\r
+       {\r
+               command_print(cmd_ctx, "usage: xscale trace_image <file> [base address] [type]");\r
+               return ERROR_OK;\r
+       }\r
+\r
+       target = get_current_target(cmd_ctx);\r
+\r
+       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)\r
+       {\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (xscale->trace.image)\r
+       {\r
+               image_close(xscale->trace.image);\r
+               free(xscale->trace.image);\r
+               command_print(cmd_ctx, "previously loaded image found and closed");\r
+       }\r
+\r
+       xscale->trace.image = malloc(sizeof(image_t));\r
+       xscale->trace.image->base_address_set = 0;\r
+       xscale->trace.image->start_address_set = 0;\r
+\r
+       /* a base address isn't always necessary, default to 0x0 (i.e. don't relocate) */\r
+       if (argc >= 2)\r
+       {\r
+               xscale->trace.image->base_address_set = 1;\r
+               xscale->trace.image->base_address = strtoul(args[1], NULL, 0);\r
+       }\r
+       else\r
+       {\r
+               xscale->trace.image->base_address_set = 0;\r
+       }\r
+\r
+       if (image_open(xscale->trace.image, args[0], (argc >= 3) ? args[2] : NULL) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "image opening error: %s", xscale->trace.image->error_str);\r
+               free(xscale->trace.image);\r
+               xscale->trace.image = NULL;\r
+               return ERROR_OK;\r
+       }\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_handle_dump_trace_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       xscale_common_t *xscale;\r
+       xscale_trace_data_t *trace_data;\r
+       fileio_t file;\r
+\r
+       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)\r
+       {\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (argc < 1)\r
+       {\r
+               command_print(cmd_ctx, "usage: xscale dump_trace <file>");\r
+               return ERROR_OK;\r
+       }\r
+\r
+       trace_data = xscale->trace.data;\r
+\r
+       if (!trace_data)\r
+       {\r
+               command_print(cmd_ctx, "no trace data collected");\r
+               return ERROR_OK;\r
+       }\r
+\r
+       if (fileio_open(&file, args[0], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK)\r
+       {\r
+               command_print(cmd_ctx, "file open error: %s", file.error_str);\r
+               return ERROR_OK;\r
+       }\r
+\r
+       while (trace_data)\r
+       {\r
+               int i;\r
+\r
+               fileio_write_u32(&file, trace_data->chkpt0);\r
+               fileio_write_u32(&file, trace_data->chkpt1);\r
+               fileio_write_u32(&file, trace_data->last_instruction);\r
+               fileio_write_u32(&file, trace_data->depth);\r
+\r
+               for (i = 0; i < trace_data->depth; i++)\r
+                       fileio_write_u32(&file, trace_data->entries[i].data | ((trace_data->entries[i].type & 0xffff) << 16));\r
+\r
+               trace_data = trace_data->next;\r
+       }\r
+\r
+       fileio_close(&file);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_handle_analyze_trace_buffer_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       xscale_common_t *xscale;\r
+\r
+       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)\r
+       {\r
+               return ERROR_OK;\r
+       }\r
+\r
+       xscale_analyze_trace(target, cmd_ctx);\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_handle_cp15(command_context_t *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       xscale_common_t *xscale;\r
+       \r
+       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)\r
+       {\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (target->state != TARGET_HALTED)\r
+       {\r
+               command_print(cmd_ctx, "target must be stopped for \"%s\" command", cmd);\r
+               return ERROR_OK;\r
+       }\r
+       u32 reg_no = 0;\r
+       reg_t *reg = NULL;\r
+       if(argc > 0)\r
+       {\r
+               reg_no = strtoul(args[0], NULL, 0);\r
+               /*translate from xscale cp15 register no to openocd register*/\r
+               switch(reg_no)\r
+               {\r
+               case 0:\r
+                       reg_no = XSCALE_MAINID;\r
+                       break;\r
+               case 1:\r
+                       reg_no = XSCALE_CTRL;\r
+                       break;\r
+               case 2:\r
+                       reg_no = XSCALE_TTB;\r
+                       break; \r
+               case 3:\r
+                       reg_no = XSCALE_DAC;\r
+                       break;\r
+               case 5:\r
+                       reg_no = XSCALE_FSR;\r
+                       break;\r
+               case 6:\r
+                       reg_no = XSCALE_FAR;\r
+                       break;\r
+               case 13:\r
+                       reg_no = XSCALE_PID;\r
+                       break;\r
+               case 15:\r
+                       reg_no = XSCALE_CPACCESS;\r
+                       break;\r
+               default:\r
+                       command_print(cmd_ctx, "invalid register number");\r
+                       return ERROR_INVALID_ARGUMENTS;\r
+               }\r
+               reg = &xscale->reg_cache->reg_list[reg_no];\r
+               \r
+       }\r
+       if(argc == 1)\r
+       {\r
+               u32 value;\r
+               \r
+               /* read cp15 control register */\r
+               xscale_get_reg(reg);\r
+               value = buf_get_u32(reg->value, 0, 32);\r
+               command_print(cmd_ctx, "%s (/%i): 0x%x", reg->name, reg->size, value);\r
+       }\r
+       else if(argc == 2)\r
+       {   \r
+\r
+               u32 value = strtoul(args[1], NULL, 0);\r
+               \r
+               /* send CP write request (command 0x41) */\r
+               xscale_send_u32(target, 0x41);\r
+               \r
+               /* send CP register number */\r
+               xscale_send_u32(target, reg_no);\r
+               \r
+               /* send CP register value */\r
+               xscale_send_u32(target, value);\r
+               \r
+               /* execute cpwait to ensure outstanding operations complete */\r
+               xscale_send_u32(target, 0x53);\r
+       }\r
+       else\r
+       {\r
+               command_print(cmd_ctx, "usage: cp15 [register]<, [value]>");    \r
+       }\r
+       \r
+       return ERROR_OK;\r
+}\r
+\r
+int handle_xscale_fast_memory_access_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)\r
+{\r
+       target_t *target = get_current_target(cmd_ctx);\r
+       armv4_5_common_t *armv4_5;\r
+       xscale_common_t *xscale;\r
+       \r
+       if (xscale_get_arch_pointers(target, &armv4_5, &xscale) != ERROR_OK)\r
+       {\r
+               return ERROR_OK;\r
+       }\r
+       \r
+       if (argc == 1)\r
+       {\r
+               if (strcmp("enable", args[0]) == 0)\r
+               {\r
+                       xscale->fast_memory_access = 1;\r
+               }\r
+               else if (strcmp("disable", args[0]) == 0)\r
+               {\r
+                       xscale->fast_memory_access = 0;\r
+               }\r
+               else\r
+               {\r
+                       return ERROR_COMMAND_SYNTAX_ERROR;\r
+               }\r
+       } else if (argc!=0)\r
+       {\r
+               return ERROR_COMMAND_SYNTAX_ERROR;\r
+       }\r
+               \r
+       command_print(cmd_ctx, "fast memory access is %s", (xscale->fast_memory_access) ? "enabled" : "disabled");\r
+\r
+       return ERROR_OK;\r
+}\r
+\r
+int xscale_register_commands(struct command_context_s *cmd_ctx)\r
+{\r
+       command_t *xscale_cmd;\r
+\r
+       xscale_cmd = register_command(cmd_ctx, NULL, "xscale", NULL, COMMAND_ANY, "xscale specific commands");\r
+\r
+       register_command(cmd_ctx, xscale_cmd, "debug_handler", xscale_handle_debug_handler_command, COMMAND_ANY, "'xscale debug_handler <target#> <address>' command takes two required operands");\r
+       register_command(cmd_ctx, xscale_cmd, "cache_clean_address", xscale_handle_cache_clean_address_command, COMMAND_ANY, NULL);\r
+\r
+       register_command(cmd_ctx, xscale_cmd, "cache_info", xscale_handle_cache_info_command, COMMAND_EXEC, NULL);\r
+       register_command(cmd_ctx, xscale_cmd, "mmu", xscale_handle_mmu_command, COMMAND_EXEC, "['enable'|'disable'] the MMU");\r
+       register_command(cmd_ctx, xscale_cmd, "icache", xscale_handle_idcache_command, COMMAND_EXEC, "['enable'|'disable'] the ICache");\r
+       register_command(cmd_ctx, xscale_cmd, "dcache", xscale_handle_idcache_command, COMMAND_EXEC, "['enable'|'disable'] the DCache");\r
+\r
+       register_command(cmd_ctx, xscale_cmd, "vector_catch", xscale_handle_idcache_command, COMMAND_EXEC, "<mask> of vectors that should be catched");\r
+\r
+       register_command(cmd_ctx, xscale_cmd, "trace_buffer", xscale_handle_trace_buffer_command, COMMAND_EXEC, "<enable|disable> ['fill' [n]|'wrap']");\r
+\r
+       register_command(cmd_ctx, xscale_cmd, "dump_trace", xscale_handle_dump_trace_command, COMMAND_EXEC, "dump content of trace buffer to <file>");\r
+       register_command(cmd_ctx, xscale_cmd, "analyze_trace", xscale_handle_analyze_trace_buffer_command, COMMAND_EXEC, "analyze content of trace buffer");\r
+       register_command(cmd_ctx, xscale_cmd, "trace_image", xscale_handle_trace_image_command,\r
+               COMMAND_EXEC, "load image from <file> [base address]");\r
+\r
+       register_command(cmd_ctx, xscale_cmd, "cp15", xscale_handle_cp15, COMMAND_EXEC, "access coproc 15 <register> [value]");\r
+       register_command(cmd_ctx, xscale_cmd, "fast_memory_access", handle_xscale_fast_memory_access_command,\r
+                COMMAND_ANY, "use fast memory accesses instead of slower but potentially unsafe slow accesses <enable|disable>");\r
+       \r
+       armv4_5_register_commands(cmd_ctx);\r
+\r
+       return ERROR_OK;\r
+}\r
index 48ca0645484ae6937595ecc3be677403458886eb..298377d9d52c25053388a28f2b91c0053324ad70 100644 (file)
@@ -188,9 +188,9 @@ int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **arg
                                                field.in_handler = NULL;\r
                                                field.in_handler_priv = NULL;\r
                                                if (device == -1)\r
-                                                       jtag_add_plain_ir_scan(1, &field, TAP_PI, NULL);\r
+                                                       jtag_add_plain_ir_scan(1, &field, TAP_PI);\r
                                                else\r
-                                                       jtag_add_ir_scan(1, &field, TAP_PI, NULL);\r
+                                                       jtag_add_ir_scan(1, &field, TAP_PI);\r
                                                if (jtag_execute_queue() != ERROR_OK)\r
                                                {\r
                                                        tdo_mismatch = 1;\r
@@ -228,9 +228,9 @@ int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **arg
                                        field.in_value = NULL;\r
                                        jtag_set_check_value(&field, dr_in_buf, dr_in_mask, NULL);\r
                                        if (device == -1)\r
-                                               jtag_add_plain_dr_scan(1, &field, TAP_PD, NULL);\r
+                                               jtag_add_plain_dr_scan(1, &field, TAP_PD);\r
                                        else\r
-                                               jtag_add_dr_scan(1, &field, TAP_PD, NULL);\r
+                                               jtag_add_dr_scan(1, &field, TAP_PD);\r
                                        if (jtag_execute_queue() != ERROR_OK)\r
                                        {\r
                                                tdo_mismatch = 1;\r
@@ -302,9 +302,9 @@ int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **arg
                                                field.in_value = NULL;\r
                                                jtag_set_check_value(&field, dr_in_buf, dr_in_mask, NULL);\r
                                                if (device == -1)\r
-                                                       jtag_add_plain_dr_scan(1, &field, TAP_PD, NULL);\r
+                                                       jtag_add_plain_dr_scan(1, &field, TAP_PD);\r
                                                else\r
-                                                       jtag_add_dr_scan(1, &field, TAP_PD, NULL);\r
+                                                       jtag_add_dr_scan(1, &field, TAP_PD);\r
                                                if (jtag_execute_queue() != ERROR_OK)\r
                                                {\r
                                                        tdo_mismatch = 1;\r
@@ -428,9 +428,9 @@ int handle_xsvf_command(struct command_context_s *cmd_ctx, char *cmd, char **arg
                                                field.in_handler = NULL;\r
                                                field.in_handler_priv = NULL;\r
                                                if (device == -1)\r
-                                                       jtag_add_plain_ir_scan(1, &field, xsvf_to_tap[xendir], NULL);\r
+                                                       jtag_add_plain_ir_scan(1, &field, xsvf_to_tap[xendir]);\r
                                                else\r
-                                                       jtag_add_ir_scan(1, &field, xsvf_to_tap[xendir], NULL);\r
+                                                       jtag_add_ir_scan(1, &field, xsvf_to_tap[xendir]);\r
                                        }\r
                                        free(ir_buf);\r
                                }\r