git.gag.com Git - fw/openocd/blob - src/target/target.c

   1 /***************************************************************************
   2  *   Copyright (C) 2005 by Dominic Rath                                    *
   3  *   Dominic.Rath@gmx.de                                                   *
   4  *                                                                         *
   5  *   This program is free software; you can redistribute it and/or modify  *
   6  *   it under the terms of the GNU General Public License as published by  *
   7  *   the Free Software Foundation; either version 2 of the License, or     *
   8  *   (at your option) any later version.                                   *
   9  *                                                                         *
  10  *   This program is distributed in the hope that it will be useful,       *
  11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  13  *   GNU General Public License for more details.                          *
  14  *                                                                         *
  15  *   You should have received a copy of the GNU General Public License     *
  16  *   along with this program; if not, write to the                         *
  17  *   Free Software Foundation, Inc.,                                       *
  18  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  19  ***************************************************************************/
  20 #ifdef HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24 #include "replacements.h"
  25 #include "target.h"
  26 #include "target_request.h"
  27
  28 #include "log.h"
  29 #include "configuration.h"
  30 #include "binarybuffer.h"
  31 #include "jtag.h"
  32
  33 #include <string.h>
  34 #include <stdlib.h>
  35 #include <inttypes.h>
  36
  37 #include <sys/types.h>
  38 #include <sys/stat.h>
  39 #include <unistd.h>
  40 #include <errno.h>
  41
  42 #include <sys/time.h>
  43 #include <time.h>
  44
  45 #include <time_support.h>
  46
  47 #include <fileio.h>
  48 #include <image.h>
  49
  50 int cli_target_callback_event_handler(struct target_s *target, enum target_event event, void *priv);
  51
  52 int handle_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  53 int handle_targets_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  54
  55 int handle_run_and_halt_time_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  56 int handle_working_area_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  57
  58 int handle_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  59 int handle_poll_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  60 int handle_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  61 int handle_wait_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  62 int handle_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  63 int handle_soft_reset_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  64 int handle_resume_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  65 int handle_step_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  66 int handle_md_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  67 int handle_mw_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  68 int handle_load_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  69 int handle_dump_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  70 int handle_verify_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  71 int handle_bp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  72 int handle_rbp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  73 int handle_wp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  74 int handle_rwp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  75 int handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc);
  76 int handle_profile_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  77 static int jim_array2mem(Jim_Interp *interp, int argc, Jim_Obj *const *argv);
  78 static int jim_mem2array(Jim_Interp *interp, int argc, Jim_Obj *const *argv);
  79
  80
  81 /* targets */
  82 extern target_type_t arm7tdmi_target;
  83 extern target_type_t arm720t_target;
  84 extern target_type_t arm9tdmi_target;
  85 extern target_type_t arm920t_target;
  86 extern target_type_t arm966e_target;
  87 extern target_type_t arm926ejs_target;
  88 extern target_type_t feroceon_target;
  89 extern target_type_t xscale_target;
  90 extern target_type_t cortexm3_target;
  91 extern target_type_t arm11_target;
  92
  93 target_type_t *target_types[] =
  94 {
  95         &arm7tdmi_target,
  96         &arm9tdmi_target,
  97         &arm920t_target,
  98         &arm720t_target,
  99         &arm966e_target,
 100         &arm926ejs_target,
 101         &feroceon_target,
 102         &xscale_target,
 103         &cortexm3_target,
 104         &arm11_target,
 105         NULL,
 106 };
 107
 108 target_t *targets = NULL;
 109 target_event_callback_t *target_event_callbacks = NULL;
 110 target_timer_callback_t *target_timer_callbacks = NULL;
 111
 112 char *target_state_strings[] =
 113 {
 114         "unknown",
 115         "running",
 116         "halted",
 117         "reset",
 118         "debug_running",
 119 };
 120
 121 char *target_debug_reason_strings[] =
 122 {
 123         "debug request", "breakpoint", "watchpoint",
 124         "watchpoint and breakpoint", "single step",
 125         "target not halted", "undefined"
 126 };
 127
 128 char *target_endianess_strings[] =
 129 {
 130         "big endian",
 131         "little endian",
 132 };
 133
 134 static int target_continous_poll = 1;
 135
 136 /* read a u32 from a buffer in target memory endianness */
 137 u32 target_buffer_get_u32(target_t *target, u8 *buffer)
 138 {
 139         if (target->endianness == TARGET_LITTLE_ENDIAN)
 140                 return le_to_h_u32(buffer);
 141         else
 142                 return be_to_h_u32(buffer);
 143 }
 144
 145 /* read a u16 from a buffer in target memory endianness */
 146 u16 target_buffer_get_u16(target_t *target, u8 *buffer)
 147 {
 148         if (target->endianness == TARGET_LITTLE_ENDIAN)
 149                 return le_to_h_u16(buffer);
 150         else
 151                 return be_to_h_u16(buffer);
 152 }
 153
 154 /* write a u32 to a buffer in target memory endianness */
 155 void target_buffer_set_u32(target_t *target, u8 *buffer, u32 value)
 156 {
 157         if (target->endianness == TARGET_LITTLE_ENDIAN)
 158                 h_u32_to_le(buffer, value);
 159         else
 160                 h_u32_to_be(buffer, value);
 161 }
 162
 163 /* write a u16 to a buffer in target memory endianness */
 164 void target_buffer_set_u16(target_t *target, u8 *buffer, u16 value)
 165 {
 166         if (target->endianness == TARGET_LITTLE_ENDIAN)
 167                 h_u16_to_le(buffer, value);
 168         else
 169                 h_u16_to_be(buffer, value);
 170 }
 171
 172 /* returns a pointer to the n-th configured target */
 173 target_t* get_target_by_num(int num)
 174 {
 175         target_t *target = targets;
 176         int i = 0;
 177
 178         while (target)
 179         {
 180                 if (num == i)
 181                         return target;
 182                 target = target->next;
 183                 i++;
 184         }
 185
 186         return NULL;
 187 }
 188
 189 int get_num_by_target(target_t *query_target)
 190 {
 191         target_t *target = targets;
 192         int i = 0;
 193
 194         while (target)
 195         {
 196                 if (target == query_target)
 197                         return i;
 198                 target = target->next;
 199                 i++;
 200         }
 201
 202         return -1;
 203 }
 204
 205 target_t* get_current_target(command_context_t *cmd_ctx)
 206 {
 207         target_t *target = get_target_by_num(cmd_ctx->current_target);
 208
 209         if (target == NULL)
 210         {
 211                 LOG_ERROR("BUG: current_target out of bounds");
 212                 exit(-1);
 213         }
 214
 215         return target;
 216 }
 217
 218 /* Process target initialization, when target entered debug out of reset
 219  * the handler is unregistered at the end of this function, so it's only called once
 220  */
 221 int target_init_handler(struct target_s *target, enum target_event event, void *priv)
 222 {
 223         struct command_context_s *cmd_ctx = priv;
 224
 225         if (event == TARGET_EVENT_HALTED)
 226         {
 227                 target_unregister_event_callback(target_init_handler, priv);
 228                 target_invoke_script(cmd_ctx, target, "post_reset");
 229                 jtag_execute_queue();
 230         }
 231
 232         return ERROR_OK;
 233 }
 234
 235 int target_run_and_halt_handler(void *priv)
 236 {
 237         target_t *target = priv;
 238
 239         target_halt(target);
 240
 241         return ERROR_OK;
 242 }
 243
 244 int target_poll(struct target_s *target)
 245 {
 246         /* We can't poll until after examine */
 247         if (!target->type->examined)
 248         {
 249                 /* Fail silently lest we pollute the log */
 250                 return ERROR_FAIL;
 251         }
 252         return target->type->poll(target);
 253 }
 254
 255 int target_halt(struct target_s *target)
 256 {
 257         /* We can't poll until after examine */
 258         if (!target->type->examined)
 259         {
 260                 LOG_ERROR("Target not examined yet");
 261                 return ERROR_FAIL;
 262         }
 263         return target->type->halt(target);
 264 }
 265
 266 int target_resume(struct target_s *target, int current, u32 address, int handle_breakpoints, int debug_execution)
 267 {
 268         int retval;
 269
 270         /* We can't poll until after examine */
 271         if (!target->type->examined)
 272         {
 273                 LOG_ERROR("Target not examined yet");
 274                 return ERROR_FAIL;
 275         }
 276
 277         /* note that resume *must* be asynchronous. The CPU can halt before we poll. The CPU can
 278          * even halt at the current PC as a result of a software breakpoint being inserted by (a bug?)
 279          * the application.
 280          */
 281         if ((retval = target->type->resume(target, current, address, handle_breakpoints, debug_execution)) != ERROR_OK)
 282                 return retval;
 283
 284         return retval;
 285 }
 286
 287 int target_process_reset(struct command_context_s *cmd_ctx, enum target_reset_mode reset_mode)
 288 {
 289         int retval = ERROR_OK;
 290         target_t *target;
 291         struct timeval timeout, now;
 292
 293         target = targets;
 294         while (target)
 295         {
 296                 target_invoke_script(cmd_ctx, target, "pre_reset");
 297                 target = target->next;
 298         }
 299
 300         if ((retval = jtag_init_reset(cmd_ctx)) != ERROR_OK)
 301                 return retval;
 302
 303         keep_alive(); /* we might be running on a very slow JTAG clk */
 304
 305         /* First time this is executed after launching OpenOCD, it will read out
 306          * the type of CPU, etc. and init Embedded ICE registers in host
 307          * memory.
 308          *
 309          * It will also set up ICE registers in the target.
 310          *
 311          * However, if we assert TRST later, we need to set up the registers again.
 312          *
 313          * For the "reset halt/init" case we must only set up the registers here.
 314          */
 315         if ((retval = target_examine(cmd_ctx)) != ERROR_OK)
 316                 return retval;
 317
 318         keep_alive(); /* we might be running on a very slow JTAG clk */
 319
 320         target = targets;
 321         while (target)
 322         {
 323                 /* we have no idea what state the target is in, so we
 324                  * have to drop working areas
 325                  */
 326                 target_free_all_working_areas_restore(target, 0);
 327                 target->reset_halt=((reset_mode==RESET_HALT)||(reset_mode==RESET_INIT));
 328                 target->type->assert_reset(target);
 329                 target = target->next;
 330         }
 331         if ((retval = jtag_execute_queue()) != ERROR_OK)
 332         {
 333                 LOG_WARNING("JTAG communication failed asserting reset.");
 334                 retval = ERROR_OK;
 335         }
 336
 337         /* request target halt if necessary, and schedule further action */
 338         target = targets;
 339         while (target)
 340         {
 341                 switch (reset_mode)
 342                 {
 343                         case RESET_RUN:
 344                                 /* nothing to do if target just wants to be run */
 345                                 break;
 346                         case RESET_RUN_AND_HALT:
 347                                 /* schedule halt */
 348                                 target_register_timer_callback(target_run_and_halt_handler, target->run_and_halt_time, 0, target);
 349                                 break;
 350                         case RESET_RUN_AND_INIT:
 351                                 /* schedule halt */
 352                                 target_register_timer_callback(target_run_and_halt_handler, target->run_and_halt_time, 0, target);
 353                                 target_register_event_callback(target_init_handler, cmd_ctx);
 354                                 break;
 355                         case RESET_HALT:
 356                                 if ((jtag_reset_config & RESET_SRST_PULLS_TRST)==0)
 357                                         target_halt(target);
 358                                 break;
 359                         case RESET_INIT:
 360                                 if ((jtag_reset_config & RESET_SRST_PULLS_TRST)==0)
 361                                         target_halt(target);
 362                                 target_register_event_callback(target_init_handler, cmd_ctx);
 363                                 break;
 364                         default:
 365                                 LOG_ERROR("BUG: unknown target->reset_mode");
 366                 }
 367                 target = target->next;
 368         }
 369
 370         if ((retval = jtag_execute_queue()) != ERROR_OK)
 371         {
 372                 LOG_WARNING("JTAG communication failed while reset was asserted. Consider using srst_only for reset_config.");
 373                 retval = ERROR_OK;
 374         }
 375
 376         target = targets;
 377         while (target)
 378         {
 379                 target->type->deassert_reset(target);
 380                 /* We can fail to bring the target into the halted state  */
 381                 target_poll(target);
 382                 if (target->reset_halt&&((target->state != TARGET_HALTED)))
 383                 {
 384                         LOG_WARNING("Failed to reset target into halted mode - issuing halt");
 385                         target->type->halt(target);
 386                 }
 387
 388                 target = target->next;
 389         }
 390
 391         if ((retval = jtag_execute_queue()) != ERROR_OK)
 392         {
 393                 LOG_WARNING("JTAG communication failed while deasserting reset.");
 394                 retval = ERROR_OK;
 395         }
 396
 397         if (jtag_reset_config & RESET_SRST_PULLS_TRST)
 398         {
 399                 /* If TRST was asserted we need to set up registers again */
 400                 if ((retval = target_examine(cmd_ctx)) != ERROR_OK)
 401                         return retval;
 402         }
 403
 404         LOG_DEBUG("Waiting for halted stated as appropriate");
 405
 406         /* Wait for reset to complete, maximum 5 seconds. */
 407         gettimeofday(&timeout, NULL);
 408         timeval_add_time(&timeout, 5, 0);
 409         for(;;)
 410         {
 411                 gettimeofday(&now, NULL);
 412
 413                 target_call_timer_callbacks_now();
 414
 415                 target = targets;
 416                 while (target)
 417                 {
 418                         LOG_DEBUG("Polling target");
 419                         target_poll(target);
 420                         if ((reset_mode == RESET_RUN_AND_INIT) ||
 421                                         (reset_mode == RESET_RUN_AND_HALT) ||
 422                                         (reset_mode == RESET_HALT) ||
 423                                         (reset_mode == RESET_INIT))
 424                         {
 425                                 if (target->state != TARGET_HALTED)
 426                                 {
 427                                         if ((now.tv_sec > timeout.tv_sec) || ((now.tv_sec == timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec)))
 428                                         {
 429                                                 LOG_USER("Timed out waiting for halt after reset");
 430                                                 goto done;
 431                                         }
 432                                         /* this will send alive messages on e.g. GDB remote protocol. */
 433                                         usleep(500*1000);
 434                                         LOG_USER_N("%s", ""); /* avoid warning about zero length formatting message*/
 435                                         goto again;
 436                                 }
 437                         }
 438                         target = target->next;
 439                 }
 440                 /* All targets we're waiting for are halted */
 441                 break;
 442
 443                 again:;
 444         }
 445         done:
 446
 447
 448         /* We want any events to be processed before the prompt */
 449         target_call_timer_callbacks_now();
 450
 451         /* if we timed out we need to unregister these handlers */
 452         target = targets;
 453         while (target)
 454         {
 455                 target_unregister_timer_callback(target_run_and_halt_handler, target);
 456                 target = target->next;
 457         }
 458         target_unregister_event_callback(target_init_handler, cmd_ctx);
 459
 460         return retval;
 461 }
 462
 463 static int default_virt2phys(struct target_s *target, u32 virtual, u32 *physical)
 464 {
 465         *physical = virtual;
 466         return ERROR_OK;
 467 }
 468
 469 static int default_mmu(struct target_s *target, int *enabled)
 470 {
 471         *enabled = 0;
 472         return ERROR_OK;
 473 }
 474
 475 static int default_examine(struct command_context_s *cmd_ctx, struct target_s *target)
 476 {
 477         target->type->examined = 1;
 478         return ERROR_OK;
 479 }
 480
 481
 482 /* Targets that correctly implement init+examine, i.e.
 483  * no communication with target during init:
 484  *
 485  * XScale
 486  */
 487 int target_examine(struct command_context_s *cmd_ctx)
 488 {
 489         int retval = ERROR_OK;
 490         target_t *target = targets;
 491         while (target)
 492         {
 493                 if ((retval = target->type->examine(cmd_ctx, target))!=ERROR_OK)
 494                         return retval;
 495                 target = target->next;
 496         }
 497         return retval;
 498 }
 499
 500 static int target_write_memory_imp(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
 501 {
 502         if (!target->type->examined)
 503         {
 504                 LOG_ERROR("Target not examined yet");
 505                 return ERROR_FAIL;
 506         }
 507         return target->type->write_memory_imp(target, address, size, count, buffer);
 508 }
 509
 510 static int target_read_memory_imp(struct target_s *target, u32 address, u32 size, u32 count, u8 *buffer)
 511 {
 512         if (!target->type->examined)
 513         {
 514                 LOG_ERROR("Target not examined yet");
 515                 return ERROR_FAIL;
 516         }
 517         return target->type->read_memory_imp(target, address, size, count, buffer);
 518 }
 519
 520 static int target_soft_reset_halt_imp(struct target_s *target)
 521 {
 522         if (!target->type->examined)
 523         {
 524                 LOG_ERROR("Target not examined yet");
 525                 return ERROR_FAIL;
 526         }
 527         return target->type->soft_reset_halt_imp(target);
 528 }
 529
 530 static int target_run_algorithm_imp(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)
 531 {
 532         if (!target->type->examined)
 533         {
 534                 LOG_ERROR("Target not examined yet");
 535                 return ERROR_FAIL;
 536         }
 537         return target->type->run_algorithm_imp(target, num_mem_params, mem_params, num_reg_params, reg_param, entry_point, exit_point, timeout_ms, arch_info);
 538 }
 539
 540 int target_init(struct command_context_s *cmd_ctx)
 541 {
 542         target_t *target = targets;
 543
 544         while (target)
 545         {
 546                 target->type->examined = 0;
 547                 if (target->type->examine == NULL)
 548                 {
 549                         target->type->examine = default_examine;
 550                 }
 551
 552                 if (target->type->init_target(cmd_ctx, target) != ERROR_OK)
 553                 {
 554                         LOG_ERROR("target '%s' init failed", target->type->name);
 555                         exit(-1);
 556                 }
 557
 558                 /* Set up default functions if none are provided by target */
 559                 if (target->type->virt2phys == NULL)
 560                 {
 561                         target->type->virt2phys = default_virt2phys;
 562                 }
 563                 target->type->virt2phys = default_virt2phys;
 564                 /* a non-invasive way(in terms of patches) to add some code that
 565                  * runs before the type->write/read_memory implementation
 566                  */
 567                 target->type->write_memory_imp = target->type->write_memory;
 568                 target->type->write_memory = target_write_memory_imp;
 569                 target->type->read_memory_imp = target->type->read_memory;
 570                 target->type->read_memory = target_read_memory_imp;
 571                 target->type->soft_reset_halt_imp = target->type->soft_reset_halt;
 572                 target->type->soft_reset_halt = target_soft_reset_halt_imp;
 573                 target->type->run_algorithm_imp = target->type->run_algorithm;
 574                 target->type->run_algorithm = target_run_algorithm_imp;
 575
 576
 577                 if (target->type->mmu == NULL)
 578                 {
 579                         target->type->mmu = default_mmu;
 580                 }
 581                 target = target->next;
 582         }
 583
 584         if (targets)
 585         {
 586                 target_register_user_commands(cmd_ctx);
 587                 target_register_timer_callback(handle_target, 100, 1, NULL);
 588         }
 589
 590         return ERROR_OK;
 591 }
 592
 593 int target_register_event_callback(int (*callback)(struct target_s *target, enum target_event event, void *priv), void *priv)
 594 {
 595         target_event_callback_t **callbacks_p = &target_event_callbacks;
 596
 597         if (callback == NULL)
 598         {
 599                 return ERROR_INVALID_ARGUMENTS;
 600         }
 601
 602         if (*callbacks_p)
 603         {
 604                 while ((*callbacks_p)->next)
 605                         callbacks_p = &((*callbacks_p)->next);
 606                 callbacks_p = &((*callbacks_p)->next);
 607         }
 608
 609         (*callbacks_p) = malloc(sizeof(target_event_callback_t));
 610         (*callbacks_p)->callback = callback;
 611         (*callbacks_p)->priv = priv;
 612         (*callbacks_p)->next = NULL;
 613
 614         return ERROR_OK;
 615 }
 616
 617 int target_register_timer_callback(int (*callback)(void *priv), int time_ms, int periodic, void *priv)
 618 {
 619         target_timer_callback_t **callbacks_p = &target_timer_callbacks;
 620         struct timeval now;
 621
 622         if (callback == NULL)
 623         {
 624                 return ERROR_INVALID_ARGUMENTS;
 625         }
 626
 627         if (*callbacks_p)
 628         {
 629                 while ((*callbacks_p)->next)
 630                         callbacks_p = &((*callbacks_p)->next);
 631                 callbacks_p = &((*callbacks_p)->next);
 632         }
 633
 634         (*callbacks_p) = malloc(sizeof(target_timer_callback_t));
 635         (*callbacks_p)->callback = callback;
 636         (*callbacks_p)->periodic = periodic;
 637         (*callbacks_p)->time_ms = time_ms;
 638
 639         gettimeofday(&now, NULL);
 640         (*callbacks_p)->when.tv_usec = now.tv_usec + (time_ms % 1000) * 1000;
 641         time_ms -= (time_ms % 1000);
 642         (*callbacks_p)->when.tv_sec = now.tv_sec + (time_ms / 1000);
 643         if ((*callbacks_p)->when.tv_usec > 1000000)
 644         {
 645                 (*callbacks_p)->when.tv_usec = (*callbacks_p)->when.tv_usec - 1000000;
 646                 (*callbacks_p)->when.tv_sec += 1;
 647         }
 648
 649         (*callbacks_p)->priv = priv;
 650         (*callbacks_p)->next = NULL;
 651
 652         return ERROR_OK;
 653 }
 654
 655 int target_unregister_event_callback(int (*callback)(struct target_s *target, enum target_event event, void *priv), void *priv)
 656 {
 657         target_event_callback_t **p = &target_event_callbacks;
 658         target_event_callback_t *c = target_event_callbacks;
 659
 660         if (callback == NULL)
 661         {
 662                 return ERROR_INVALID_ARGUMENTS;
 663         }
 664
 665         while (c)
 666         {
 667                 target_event_callback_t *next = c->next;
 668                 if ((c->callback == callback) && (c->priv == priv))
 669                 {
 670                         *p = next;
 671                         free(c);
 672                         return ERROR_OK;
 673                 }
 674                 else
 675                         p = &(c->next);
 676                 c = next;
 677         }
 678
 679         return ERROR_OK;
 680 }
 681
 682 int target_unregister_timer_callback(int (*callback)(void *priv), void *priv)
 683 {
 684         target_timer_callback_t **p = &target_timer_callbacks;
 685         target_timer_callback_t *c = target_timer_callbacks;
 686
 687         if (callback == NULL)
 688         {
 689                 return ERROR_INVALID_ARGUMENTS;
 690         }
 691
 692         while (c)
 693         {
 694                 target_timer_callback_t *next = c->next;
 695                 if ((c->callback == callback) && (c->priv == priv))
 696                 {
 697                         *p = next;
 698                         free(c);
 699                         return ERROR_OK;
 700                 }
 701                 else
 702                         p = &(c->next);
 703                 c = next;
 704         }
 705
 706         return ERROR_OK;
 707 }
 708
 709 int target_call_event_callbacks(target_t *target, enum target_event event)
 710 {
 711         target_event_callback_t *callback = target_event_callbacks;
 712         target_event_callback_t *next_callback;
 713
 714         LOG_DEBUG("target event %i", event);
 715
 716         while (callback)
 717         {
 718                 next_callback = callback->next;
 719                 callback->callback(target, event, callback->priv);
 720                 callback = next_callback;
 721         }
 722
 723         return ERROR_OK;
 724 }
 725
 726 static int target_call_timer_callbacks_check_time(int checktime)
 727 {
 728         target_timer_callback_t *callback = target_timer_callbacks;
 729         target_timer_callback_t *next_callback;
 730         struct timeval now;
 731
 732         keep_alive();
 733
 734         gettimeofday(&now, NULL);
 735
 736         while (callback)
 737         {
 738                 next_callback = callback->next;
 739
 740                 if ((!checktime&&callback->periodic)||
 741                                 (((now.tv_sec >= callback->when.tv_sec) && (now.tv_usec >= callback->when.tv_usec))
 742                                                 || (now.tv_sec > callback->when.tv_sec)))
 743                 {
 744                         if(callback->callback != NULL)
 745                         {
 746                                 callback->callback(callback->priv);
 747                                 if (callback->periodic)
 748                                 {
 749                                         int time_ms = callback->time_ms;
 750                                         callback->when.tv_usec = now.tv_usec + (time_ms % 1000) * 1000;
 751                                         time_ms -= (time_ms % 1000);
 752                                         callback->when.tv_sec = now.tv_sec + time_ms / 1000;
 753                                         if (callback->when.tv_usec > 1000000)
 754                                         {
 755                                                 callback->when.tv_usec = callback->when.tv_usec - 1000000;
 756                                                 callback->when.tv_sec += 1;
 757                                         }
 758                                 }
 759                                 else
 760                                         target_unregister_timer_callback(callback->callback, callback->priv);
 761                         }
 762                 }
 763
 764                 callback = next_callback;
 765         }
 766
 767         return ERROR_OK;
 768 }
 769
 770 int target_call_timer_callbacks()
 771 {
 772         return target_call_timer_callbacks_check_time(1);
 773 }
 774
 775 /* invoke periodic callbacks immediately */
 776 int target_call_timer_callbacks_now()
 777 {
 778         return target_call_timer_callbacks(0);
 779 }
 780
 781 int target_alloc_working_area(struct target_s *target, u32 size, working_area_t **area)
 782 {
 783         working_area_t *c = target->working_areas;
 784         working_area_t *new_wa = NULL;
 785
 786         /* Reevaluate working area address based on MMU state*/
 787         if (target->working_areas == NULL)
 788         {
 789                 int retval;
 790                 int enabled;
 791                 retval = target->type->mmu(target, &enabled);
 792                 if (retval != ERROR_OK)
 793                 {
 794                         return retval;
 795                 }
 796                 if (enabled)
 797                 {
 798                         target->working_area = target->working_area_virt;
 799                 }
 800                 else
 801                 {
 802                         target->working_area = target->working_area_phys;
 803                 }
 804         }
 805
 806         /* only allocate multiples of 4 byte */
 807         if (size % 4)
 808         {
 809                 LOG_ERROR("BUG: code tried to allocate unaligned number of bytes, padding");
 810                 size = CEIL(size, 4);
 811         }
 812
 813         /* see if there's already a matching working area */
 814         while (c)
 815         {
 816                 if ((c->free) && (c->size == size))
 817                 {
 818                         new_wa = c;
 819                         break;
 820                 }
 821                 c = c->next;
 822         }
 823
 824         /* if not, allocate a new one */
 825         if (!new_wa)
 826         {
 827                 working_area_t **p = &target->working_areas;
 828                 u32 first_free = target->working_area;
 829                 u32 free_size = target->working_area_size;
 830
 831                 LOG_DEBUG("allocating new working area");
 832
 833                 c = target->working_areas;
 834                 while (c)
 835                 {
 836                         first_free += c->size;
 837                         free_size -= c->size;
 838                         p = &c->next;
 839                         c = c->next;
 840                 }
 841
 842                 if (free_size < size)
 843                 {
 844                         LOG_WARNING("not enough working area available(requested %d, free %d)", size, free_size);
 845                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 846                 }
 847
 848                 new_wa = malloc(sizeof(working_area_t));
 849                 new_wa->next = NULL;
 850                 new_wa->size = size;
 851                 new_wa->address = first_free;
 852
 853                 if (target->backup_working_area)
 854                 {
 855                         new_wa->backup = malloc(new_wa->size);
 856                         target->type->read_memory(target, new_wa->address, 4, new_wa->size / 4, new_wa->backup);
 857                 }
 858                 else
 859                 {
 860                         new_wa->backup = NULL;
 861                 }
 862
 863                 /* put new entry in list */
 864                 *p = new_wa;
 865         }
 866
 867         /* mark as used, and return the new (reused) area */
 868         new_wa->free = 0;
 869         *area = new_wa;
 870
 871         /* user pointer */
 872         new_wa->user = area;
 873
 874         return ERROR_OK;
 875 }
 876
 877 int target_free_working_area_restore(struct target_s *target, working_area_t *area, int restore)
 878 {
 879         if (area->free)
 880                 return ERROR_OK;
 881
 882         if (restore&&target->backup_working_area)
 883                 target->type->write_memory(target, area->address, 4, area->size / 4, area->backup);
 884
 885         area->free = 1;
 886
 887         /* mark user pointer invalid */
 888         *area->user = NULL;
 889         area->user = NULL;
 890
 891         return ERROR_OK;
 892 }
 893
 894 int target_free_working_area(struct target_s *target, working_area_t *area)
 895 {
 896         return target_free_working_area_restore(target, area, 1);
 897 }
 898
 899 int target_free_all_working_areas_restore(struct target_s *target, int restore)
 900 {
 901         working_area_t *c = target->working_areas;
 902
 903         while (c)
 904         {
 905                 working_area_t *next = c->next;
 906                 target_free_working_area_restore(target, c, restore);
 907
 908                 if (c->backup)
 909                         free(c->backup);
 910
 911                 free(c);
 912
 913                 c = next;
 914         }
 915
 916         target->working_areas = NULL;
 917
 918         return ERROR_OK;
 919 }
 920
 921 int target_free_all_working_areas(struct target_s *target)
 922 {
 923         return target_free_all_working_areas_restore(target, 1);
 924 }
 925
 926 int target_register_commands(struct command_context_s *cmd_ctx)
 927 {
 928         register_command(cmd_ctx, NULL, "target", handle_target_command, COMMAND_CONFIG, "target <cpu> [reset_init default - DEPRECATED] <chainpos> <endianness> <variant> [cpu type specifc args]");
 929         register_command(cmd_ctx, NULL, "targets", handle_targets_command, COMMAND_EXEC, NULL);
 930         register_command(cmd_ctx, NULL, "run_and_halt_time", handle_run_and_halt_time_command, COMMAND_CONFIG, "<target> <run time ms>");
 931         register_command(cmd_ctx, NULL, "working_area", handle_working_area_command, COMMAND_ANY, "working_area <target#> <address> <size> <'backup'|'nobackup'> [virtual address]");
 932         register_command(cmd_ctx, NULL, "virt2phys", handle_virt2phys_command, COMMAND_ANY, "virt2phys <virtual address>");
 933         register_command(cmd_ctx, NULL, "profile", handle_profile_command, COMMAND_EXEC, "PRELIMINARY! - profile <seconds> <gmon.out>");
 934
 935
 936         /* script procedures */
 937         register_jim(cmd_ctx, "ocd_mem2array", jim_mem2array, "read memory and return as a TCL array for script processing");
 938         register_jim(cmd_ctx, "ocd_array2mem", jim_array2mem, "convert a TCL array to memory locations and write the values");
 939         return ERROR_OK;
 940 }
 941
 942 int target_arch_state(struct target_s *target)
 943 {
 944         int retval;
 945         if (target==NULL)
 946         {
 947                 LOG_USER("No target has been configured");
 948                 return ERROR_OK;
 949         }
 950
 951         LOG_USER("target state: %s", target_state_strings[target->state]);
 952
 953         if (target->state!=TARGET_HALTED)
 954                 return ERROR_OK;
 955
 956         retval=target->type->arch_state(target);
 957         return retval;
 958 }
 959
 960 /* Single aligned words are guaranteed to use 16 or 32 bit access
 961  * mode respectively, otherwise data is handled as quickly as
 962  * possible
 963  */
 964 int target_write_buffer(struct target_s *target, u32 address, u32 size, u8 *buffer)
 965 {
 966         int retval;
 967         if (!target->type->examined)
 968         {
 969                 LOG_ERROR("Target not examined yet");
 970                 return ERROR_FAIL;
 971         }
 972
 973         LOG_DEBUG("writing buffer of %i byte at 0x%8.8x", size, address);
 974
 975         if (((address % 2) == 0) && (size == 2))
 976         {
 977                 return target->type->write_memory(target, address, 2, 1, buffer);
 978         }
 979
 980         /* handle unaligned head bytes */
 981         if (address % 4)
 982         {
 983                 int unaligned = 4 - (address % 4);
 984
 985                 if (unaligned > size)
 986                         unaligned = size;
 987
 988                 if ((retval = target->type->write_memory(target, address, 1, unaligned, buffer)) != ERROR_OK)
 989                         return retval;
 990
 991                 buffer += unaligned;
 992                 address += unaligned;
 993                 size -= unaligned;
 994         }
 995
 996         /* handle aligned words */
 997         if (size >= 4)
 998         {
 999                 int aligned = size - (size % 4);
1000
1001                 /* use bulk writes above a certain limit. This may have to be changed */
1002                 if (aligned > 128)
1003                 {
1004                         if ((retval = target->type->bulk_write_memory(target, address, aligned / 4, buffer)) != ERROR_OK)
1005                                 return retval;
1006                 }
1007                 else
1008                 {
1009                         if ((retval = target->type->write_memory(target, address, 4, aligned / 4, buffer)) != ERROR_OK)
1010                                 return retval;
1011                 }
1012
1013                 buffer += aligned;
1014                 address += aligned;
1015                 size -= aligned;
1016         }
1017
1018         /* handle tail writes of less than 4 bytes */
1019         if (size > 0)
1020         {
1021                 if ((retval = target->type->write_memory(target, address, 1, size, buffer)) != ERROR_OK)
1022                         return retval;
1023         }
1024
1025         return ERROR_OK;
1026 }
1027
1028
1029 /* Single aligned words are guaranteed to use 16 or 32 bit access
1030  * mode respectively, otherwise data is handled as quickly as
1031  * possible
1032  */
1033 int target_read_buffer(struct target_s *target, u32 address, u32 size, u8 *buffer)
1034 {
1035         int retval;
1036         if (!target->type->examined)
1037         {
1038                 LOG_ERROR("Target not examined yet");
1039                 return ERROR_FAIL;
1040         }
1041
1042         LOG_DEBUG("reading buffer of %i byte at 0x%8.8x", size, address);
1043
1044         if (((address % 2) == 0) && (size == 2))
1045         {
1046                 return target->type->read_memory(target, address, 2, 1, buffer);
1047         }
1048
1049         /* handle unaligned head bytes */
1050         if (address % 4)
1051         {
1052                 int unaligned = 4 - (address % 4);
1053
1054                 if (unaligned > size)
1055                         unaligned = size;
1056
1057                 if ((retval = target->type->read_memory(target, address, 1, unaligned, buffer)) != ERROR_OK)
1058                         return retval;
1059
1060                 buffer += unaligned;
1061                 address += unaligned;
1062                 size -= unaligned;
1063         }
1064
1065         /* handle aligned words */
1066         if (size >= 4)
1067         {
1068                 int aligned = size - (size % 4);
1069
1070                 if ((retval = target->type->read_memory(target, address, 4, aligned / 4, buffer)) != ERROR_OK)
1071                         return retval;
1072
1073                 buffer += aligned;
1074                 address += aligned;
1075                 size -= aligned;
1076         }
1077
1078         /* handle tail writes of less than 4 bytes */
1079         if (size > 0)
1080         {
1081                 if ((retval = target->type->read_memory(target, address, 1, size, buffer)) != ERROR_OK)
1082                         return retval;
1083         }
1084
1085         return ERROR_OK;
1086 }
1087
1088 int target_checksum_memory(struct target_s *target, u32 address, u32 size, u32* crc)
1089 {
1090         u8 *buffer;
1091         int retval;
1092         int i;
1093         u32 checksum = 0;
1094         if (!target->type->examined)
1095         {
1096                 LOG_ERROR("Target not examined yet");
1097                 return ERROR_FAIL;
1098         }
1099
1100         if ((retval = target->type->checksum_memory(target, address,
1101                 size, &checksum)) == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
1102         {
1103                 buffer = malloc(size);
1104                 if (buffer == NULL)
1105                 {
1106                         LOG_ERROR("error allocating buffer for section (%d bytes)", size);
1107                         return ERROR_INVALID_ARGUMENTS;
1108                 }
1109                 retval = target_read_buffer(target, address, size, buffer);
1110                 if (retval != ERROR_OK)
1111                 {
1112                         free(buffer);
1113                         return retval;
1114                 }
1115
1116                 /* convert to target endianess */
1117                 for (i = 0; i < (size/sizeof(u32)); i++)
1118                 {
1119                         u32 target_data;
1120                         target_data = target_buffer_get_u32(target, &buffer[i*sizeof(u32)]);
1121                         target_buffer_set_u32(target, &buffer[i*sizeof(u32)], target_data);
1122                 }
1123
1124                 retval = image_calculate_checksum( buffer, size, &checksum );
1125                 free(buffer);
1126         }
1127
1128         *crc = checksum;
1129
1130         return retval;
1131 }
1132
1133 int target_blank_check_memory(struct target_s *target, u32 address, u32 size, u32* blank)
1134 {
1135         int retval;
1136         if (!target->type->examined)
1137         {
1138                 LOG_ERROR("Target not examined yet");
1139                 return ERROR_FAIL;
1140         }
1141
1142         if (target->type->blank_check_memory == 0)
1143                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1144
1145         retval = target->type->blank_check_memory(target, address, size, blank);
1146
1147         return retval;
1148 }
1149
1150 int target_read_u32(struct target_s *target, u32 address, u32 *value)
1151 {
1152         u8 value_buf[4];
1153         if (!target->type->examined)
1154         {
1155                 LOG_ERROR("Target not examined yet");
1156                 return ERROR_FAIL;
1157         }
1158
1159         int retval = target->type->read_memory(target, address, 4, 1, value_buf);
1160
1161         if (retval == ERROR_OK)
1162         {
1163                 *value = target_buffer_get_u32(target, value_buf);
1164                 LOG_DEBUG("address: 0x%8.8x, value: 0x%8.8x", address, *value);
1165         }
1166         else
1167         {
1168                 *value = 0x0;
1169                 LOG_DEBUG("address: 0x%8.8x failed", address);
1170         }
1171
1172         return retval;
1173 }
1174
1175 int target_read_u16(struct target_s *target, u32 address, u16 *value)
1176 {
1177         u8 value_buf[2];
1178         if (!target->type->examined)
1179         {
1180                 LOG_ERROR("Target not examined yet");
1181                 return ERROR_FAIL;
1182         }
1183
1184         int retval = target->type->read_memory(target, address, 2, 1, value_buf);
1185
1186         if (retval == ERROR_OK)
1187         {
1188                 *value = target_buffer_get_u16(target, value_buf);
1189                 LOG_DEBUG("address: 0x%8.8x, value: 0x%4.4x", address, *value);
1190         }
1191         else
1192         {
1193                 *value = 0x0;
1194                 LOG_DEBUG("address: 0x%8.8x failed", address);
1195         }
1196
1197         return retval;
1198 }
1199
1200 int target_read_u8(struct target_s *target, u32 address, u8 *value)
1201 {
1202         int retval = target->type->read_memory(target, address, 1, 1, value);
1203         if (!target->type->examined)
1204         {
1205                 LOG_ERROR("Target not examined yet");
1206                 return ERROR_FAIL;
1207         }
1208
1209         if (retval == ERROR_OK)
1210         {
1211                 LOG_DEBUG("address: 0x%8.8x, value: 0x%2.2x", address, *value);
1212         }
1213         else
1214         {
1215                 *value = 0x0;
1216                 LOG_DEBUG("address: 0x%8.8x failed", address);
1217         }
1218
1219         return retval;
1220 }
1221
1222 int target_write_u32(struct target_s *target, u32 address, u32 value)
1223 {
1224         int retval;
1225         u8 value_buf[4];
1226         if (!target->type->examined)
1227         {
1228                 LOG_ERROR("Target not examined yet");
1229                 return ERROR_FAIL;
1230         }
1231
1232         LOG_DEBUG("address: 0x%8.8x, value: 0x%8.8x", address, value);
1233
1234         target_buffer_set_u32(target, value_buf, value);
1235         if ((retval = target->type->write_memory(target, address, 4, 1, value_buf)) != ERROR_OK)
1236         {
1237                 LOG_DEBUG("failed: %i", retval);
1238         }
1239
1240         return retval;
1241 }
1242
1243 int target_write_u16(struct target_s *target, u32 address, u16 value)
1244 {
1245         int retval;
1246         u8 value_buf[2];
1247         if (!target->type->examined)
1248         {
1249                 LOG_ERROR("Target not examined yet");
1250                 return ERROR_FAIL;
1251         }
1252
1253         LOG_DEBUG("address: 0x%8.8x, value: 0x%8.8x", address, value);
1254
1255         target_buffer_set_u16(target, value_buf, value);
1256         if ((retval = target->type->write_memory(target, address, 2, 1, value_buf)) != ERROR_OK)
1257         {
1258                 LOG_DEBUG("failed: %i", retval);
1259         }
1260
1261         return retval;
1262 }
1263
1264 int target_write_u8(struct target_s *target, u32 address, u8 value)
1265 {
1266         int retval;
1267         if (!target->type->examined)
1268         {
1269                 LOG_ERROR("Target not examined yet");
1270                 return ERROR_FAIL;
1271         }
1272
1273         LOG_DEBUG("address: 0x%8.8x, value: 0x%2.2x", address, value);
1274
1275         if ((retval = target->type->read_memory(target, address, 1, 1, &value)) != ERROR_OK)
1276         {
1277                 LOG_DEBUG("failed: %i", retval);
1278         }
1279
1280         return retval;
1281 }
1282
1283 int target_register_user_commands(struct command_context_s *cmd_ctx)
1284 {
1285         register_command(cmd_ctx,  NULL, "reg", handle_reg_command, COMMAND_EXEC, NULL);
1286         register_command(cmd_ctx,  NULL, "poll", handle_poll_command, COMMAND_EXEC, "poll target state");
1287         register_command(cmd_ctx,  NULL, "wait_halt", handle_wait_halt_command, COMMAND_EXEC, "wait for target halt [time (s)]");
1288         register_command(cmd_ctx,  NULL, "halt", handle_halt_command, COMMAND_EXEC, "halt target");
1289         register_command(cmd_ctx,  NULL, "resume", handle_resume_command, COMMAND_EXEC, "resume target [addr]");
1290         register_command(cmd_ctx,  NULL, "step", handle_step_command, COMMAND_EXEC, "step one instruction from current PC or [addr]");
1291         register_command(cmd_ctx,  NULL, "reset", handle_reset_command, COMMAND_EXEC, "reset target [run|halt|init|run_and_halt|run_and_init]");
1292         register_command(cmd_ctx,  NULL, "soft_reset_halt", handle_soft_reset_halt_command, COMMAND_EXEC, "halt the target and do a soft reset");
1293
1294         register_command(cmd_ctx,  NULL, "mdw", handle_md_command, COMMAND_EXEC, "display memory words <addr> [count]");
1295         register_command(cmd_ctx,  NULL, "mdh", handle_md_command, COMMAND_EXEC, "display memory half-words <addr> [count]");
1296         register_command(cmd_ctx,  NULL, "mdb", handle_md_command, COMMAND_EXEC, "display memory bytes <addr> [count]");
1297
1298         register_command(cmd_ctx,  NULL, "mww", handle_mw_command, COMMAND_EXEC, "write memory word <addr> <value> [count]");
1299         register_command(cmd_ctx,  NULL, "mwh", handle_mw_command, COMMAND_EXEC, "write memory half-word <addr> <value> [count]");
1300         register_command(cmd_ctx,  NULL, "mwb", handle_mw_command, COMMAND_EXEC, "write memory byte <addr> <value> [count]");
1301
1302         register_command(cmd_ctx,  NULL, "bp", handle_bp_command, COMMAND_EXEC, "set breakpoint <address> <length> [hw]");
1303         register_command(cmd_ctx,  NULL, "rbp", handle_rbp_command, COMMAND_EXEC, "remove breakpoint <adress>");
1304         register_command(cmd_ctx,  NULL, "wp", handle_wp_command, COMMAND_EXEC, "set watchpoint <address> <length> <r/w/a> [value] [mask]");
1305         register_command(cmd_ctx,  NULL, "rwp", handle_rwp_command, COMMAND_EXEC, "remove watchpoint <adress>");
1306
1307         register_command(cmd_ctx,  NULL, "load_image", handle_load_image_command, COMMAND_EXEC, "load_image <file> <address> ['bin'|'ihex'|'elf'|'s19']");
1308         register_command(cmd_ctx,  NULL, "dump_image", handle_dump_image_command, COMMAND_EXEC, "dump_image <file> <address> <size>");
1309         register_command(cmd_ctx,  NULL, "verify_image", handle_verify_image_command, COMMAND_EXEC, "verify_image <file> [offset] [type]");
1310         register_command(cmd_ctx,  NULL, "load_binary", handle_load_image_command, COMMAND_EXEC, "[DEPRECATED] load_binary <file> <address>");
1311         register_command(cmd_ctx,  NULL, "dump_binary", handle_dump_image_command, COMMAND_EXEC, "[DEPRECATED] dump_binary <file> <address> <size>");
1312
1313         target_request_register_commands(cmd_ctx);
1314         trace_register_commands(cmd_ctx);
1315
1316         return ERROR_OK;
1317 }
1318
1319 int handle_targets_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1320 {
1321         target_t *target = targets;
1322         int count = 0;
1323
1324         if (argc == 1)
1325         {
1326                 int num = strtoul(args[0], NULL, 0);
1327
1328                 while (target)
1329                 {
1330                         count++;
1331                         target = target->next;
1332                 }
1333
1334                 if (num < count)
1335                         cmd_ctx->current_target = num;
1336                 else
1337                         command_print(cmd_ctx, "%i is out of bounds, only %i targets are configured", num, count);
1338
1339                 return ERROR_OK;
1340         }
1341
1342         while (target)
1343         {
1344                 command_print(cmd_ctx, "%i: %s (%s), state: %s", count++, target->type->name, target_endianess_strings[target->endianness], target_state_strings[target->state]);
1345                 target = target->next;
1346         }
1347
1348         return ERROR_OK;
1349 }
1350
1351 int handle_target_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1352 {
1353         int i;
1354         int found = 0;
1355
1356         if (argc < 3)
1357         {
1358                 return ERROR_COMMAND_SYNTAX_ERROR;
1359         }
1360
1361         /* search for the specified target */
1362         if (args[0] && (args[0][0] != 0))
1363         {
1364                 for (i = 0; target_types[i]; i++)
1365                 {
1366                         if (strcmp(args[0], target_types[i]->name) == 0)
1367                         {
1368                                 target_t **last_target_p = &targets;
1369
1370                                 /* register target specific commands */
1371                                 if (target_types[i]->register_commands(cmd_ctx) != ERROR_OK)
1372                                 {
1373                                         LOG_ERROR("couldn't register '%s' commands", args[0]);
1374                                         exit(-1);
1375                                 }
1376
1377                                 if (*last_target_p)
1378                                 {
1379                                         while ((*last_target_p)->next)
1380                                                 last_target_p = &((*last_target_p)->next);
1381                                         last_target_p = &((*last_target_p)->next);
1382                                 }
1383
1384                                 *last_target_p = malloc(sizeof(target_t));
1385
1386                                 /* allocate memory for each unique target type */
1387                                 (*last_target_p)->type = (target_type_t*)malloc(sizeof(target_type_t));
1388                                 *((*last_target_p)->type) = *target_types[i];
1389
1390                                 if (strcmp(args[1], "big") == 0)
1391                                         (*last_target_p)->endianness = TARGET_BIG_ENDIAN;
1392                                 else if (strcmp(args[1], "little") == 0)
1393                                         (*last_target_p)->endianness = TARGET_LITTLE_ENDIAN;
1394                                 else
1395                                 {
1396                                         LOG_ERROR("endianness must be either 'little' or 'big', not '%s'", args[1]);
1397                                         return ERROR_COMMAND_SYNTAX_ERROR;
1398                                 }
1399
1400                                 if (strcmp(args[2], "reset_halt") == 0)
1401                                 {
1402                                         LOG_WARNING("reset_mode argument is obsolete.");
1403                                         return ERROR_COMMAND_SYNTAX_ERROR;
1404                                 }
1405                                 else if (strcmp(args[2], "reset_run") == 0)
1406                                 {
1407                                         LOG_WARNING("reset_mode argument is obsolete.");
1408                                         return ERROR_COMMAND_SYNTAX_ERROR;
1409                                 }
1410                                 else if (strcmp(args[2], "reset_init") == 0)
1411                                 {
1412                                         LOG_WARNING("reset_mode argument is obsolete.");
1413                                         return ERROR_COMMAND_SYNTAX_ERROR;
1414                                 }
1415                                 else if (strcmp(args[2], "run_and_halt") == 0)
1416                                 {
1417                                         LOG_WARNING("reset_mode argument is obsolete.");
1418                                         return ERROR_COMMAND_SYNTAX_ERROR;
1419                                 }
1420                                 else if (strcmp(args[2], "run_and_init") == 0)
1421                                 {
1422                                         LOG_WARNING("reset_mode argument is obsolete.");
1423                                         return ERROR_COMMAND_SYNTAX_ERROR;
1424                                 }
1425                                 else
1426                                 {
1427                                         /* Kludge! we want to make this reset arg optional while remaining compatible! */
1428                                         args--;
1429                                         argc++;
1430                                 }
1431                                 (*last_target_p)->run_and_halt_time = 1000; /* default 1s */
1432
1433                                 (*last_target_p)->working_area = 0x0;
1434                                 (*last_target_p)->working_area_size = 0x0;
1435                                 (*last_target_p)->working_areas = NULL;
1436                                 (*last_target_p)->backup_working_area = 0;
1437
1438                                 (*last_target_p)->state = TARGET_UNKNOWN;
1439                                 (*last_target_p)->debug_reason = DBG_REASON_UNDEFINED;
1440                                 (*last_target_p)->reg_cache = NULL;
1441                                 (*last_target_p)->breakpoints = NULL;
1442                                 (*last_target_p)->watchpoints = NULL;
1443                                 (*last_target_p)->next = NULL;
1444                                 (*last_target_p)->arch_info = NULL;
1445
1446                                 /* initialize trace information */
1447                                 (*last_target_p)->trace_info = malloc(sizeof(trace_t));
1448                                 (*last_target_p)->trace_info->num_trace_points = 0;
1449                                 (*last_target_p)->trace_info->trace_points_size = 0;
1450                                 (*last_target_p)->trace_info->trace_points = NULL;
1451                                 (*last_target_p)->trace_info->trace_history_size = 0;
1452                                 (*last_target_p)->trace_info->trace_history = NULL;
1453                                 (*last_target_p)->trace_info->trace_history_pos = 0;
1454                                 (*last_target_p)->trace_info->trace_history_overflowed = 0;
1455
1456                                 (*last_target_p)->dbgmsg = NULL;
1457                                 (*last_target_p)->dbg_msg_enabled = 0;
1458
1459                                 (*last_target_p)->type->target_command(cmd_ctx, cmd, args, argc, *last_target_p);
1460
1461                                 found = 1;
1462                                 break;
1463                         }
1464                 }
1465         }
1466
1467         /* no matching target found */
1468         if (!found)
1469         {
1470                 LOG_ERROR("target '%s' not found", args[0]);
1471                 return ERROR_COMMAND_SYNTAX_ERROR;
1472         }
1473
1474         return ERROR_OK;
1475 }
1476
1477 int target_invoke_script(struct command_context_s *cmd_ctx, target_t *target, char *name)
1478 {
1479         return command_run_linef(cmd_ctx, " if {[catch {info body target_%d_%s} t]==0} {target_%d_%s}",
1480                         get_num_by_target(target), name,
1481                         get_num_by_target(target), name);
1482 }
1483
1484 int handle_run_and_halt_time_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1485 {
1486         target_t *target = NULL;
1487
1488         if (argc < 2)
1489         {
1490                 return ERROR_COMMAND_SYNTAX_ERROR;
1491         }
1492
1493         target = get_target_by_num(strtoul(args[0], NULL, 0));
1494         if (!target)
1495         {
1496                 return ERROR_COMMAND_SYNTAX_ERROR;
1497         }
1498
1499         target->run_and_halt_time = strtoul(args[1], NULL, 0);
1500
1501         return ERROR_OK;
1502 }
1503
1504 int handle_working_area_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1505 {
1506         target_t *target = NULL;
1507
1508         if ((argc < 4) || (argc > 5))
1509         {
1510                 return ERROR_COMMAND_SYNTAX_ERROR;
1511         }
1512
1513         target = get_target_by_num(strtoul(args[0], NULL, 0));
1514         if (!target)
1515         {
1516                 return ERROR_COMMAND_SYNTAX_ERROR;
1517         }
1518         target_free_all_working_areas(target);
1519
1520         target->working_area_phys = target->working_area_virt = strtoul(args[1], NULL, 0);
1521         if (argc == 5)
1522         {
1523                 target->working_area_virt = strtoul(args[4], NULL, 0);
1524         }
1525         target->working_area_size = strtoul(args[2], NULL, 0);
1526
1527         if (strcmp(args[3], "backup") == 0)
1528         {
1529                 target->backup_working_area = 1;
1530         }
1531         else if (strcmp(args[3], "nobackup") == 0)
1532         {
1533                 target->backup_working_area = 0;
1534         }
1535         else
1536         {
1537                 LOG_ERROR("unrecognized <backup|nobackup> argument (%s)", args[3]);
1538                 return ERROR_COMMAND_SYNTAX_ERROR;
1539         }
1540
1541         return ERROR_OK;
1542 }
1543
1544
1545 /* process target state changes */
1546 int handle_target(void *priv)
1547 {
1548         target_t *target = targets;
1549
1550         while (target)
1551         {
1552                 if (target_continous_poll)
1553                 {
1554                         /* polling may fail silently until the target has been examined */
1555                         target_poll(target);
1556                 }
1557
1558                 target = target->next;
1559         }
1560
1561         return ERROR_OK;
1562 }
1563
1564 int handle_reg_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1565 {
1566         target_t *target;
1567         reg_t *reg = NULL;
1568         int count = 0;
1569         char *value;
1570
1571         LOG_DEBUG("-");
1572
1573         target = get_current_target(cmd_ctx);
1574
1575         /* list all available registers for the current target */
1576         if (argc == 0)
1577         {
1578                 reg_cache_t *cache = target->reg_cache;
1579
1580                 count = 0;
1581                 while(cache)
1582                 {
1583                         int i;
1584                         for (i = 0; i < cache->num_regs; i++)
1585                         {
1586                                 value = buf_to_str(cache->reg_list[i].value, cache->reg_list[i].size, 16);
1587                                 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);
1588                                 free(value);
1589                         }
1590                         cache = cache->next;
1591                 }
1592
1593                 return ERROR_OK;
1594         }
1595
1596         /* access a single register by its ordinal number */
1597         if ((args[0][0] >= '0') && (args[0][0] <= '9'))
1598         {
1599                 int num = strtoul(args[0], NULL, 0);
1600                 reg_cache_t *cache = target->reg_cache;
1601
1602                 count = 0;
1603                 while(cache)
1604                 {
1605                         int i;
1606                         for (i = 0; i < cache->num_regs; i++)
1607                         {
1608                                 if (count++ == num)
1609                                 {
1610                                         reg = &cache->reg_list[i];
1611                                         break;
1612                                 }
1613                         }
1614                         if (reg)
1615                                 break;
1616                         cache = cache->next;
1617                 }
1618
1619                 if (!reg)
1620                 {
1621                         command_print(cmd_ctx, "%i is out of bounds, the current target has only %i registers (0 - %i)", num, count, count - 1);
1622                         return ERROR_OK;
1623                 }
1624         } else /* access a single register by its name */
1625         {
1626                 reg = register_get_by_name(target->reg_cache, args[0], 1);
1627
1628                 if (!reg)
1629                 {
1630                         command_print(cmd_ctx, "register %s not found in current target", args[0]);
1631                         return ERROR_OK;
1632                 }
1633         }
1634
1635         /* display a register */
1636         if ((argc == 1) || ((argc == 2) && !((args[1][0] >= '0') && (args[1][0] <= '9'))))
1637         {
1638                 if ((argc == 2) && (strcmp(args[1], "force") == 0))
1639                         reg->valid = 0;
1640
1641                 if (reg->valid == 0)
1642                 {
1643                         reg_arch_type_t *arch_type = register_get_arch_type(reg->arch_type);
1644                         if (arch_type == NULL)
1645                         {
1646                                 LOG_ERROR("BUG: encountered unregistered arch type");
1647                                 return ERROR_OK;
1648                         }
1649                         arch_type->get(reg);
1650                 }
1651                 value = buf_to_str(reg->value, reg->size, 16);
1652                 command_print(cmd_ctx, "%s (/%i): 0x%s", reg->name, reg->size, value);
1653                 free(value);
1654                 return ERROR_OK;
1655         }
1656
1657         /* set register value */
1658         if (argc == 2)
1659         {
1660                 u8 *buf = malloc(CEIL(reg->size, 8));
1661                 str_to_buf(args[1], strlen(args[1]), buf, reg->size, 0);
1662
1663                 reg_arch_type_t *arch_type = register_get_arch_type(reg->arch_type);
1664                 if (arch_type == NULL)
1665                 {
1666                         LOG_ERROR("BUG: encountered unregistered arch type");
1667                         return ERROR_OK;
1668                 }
1669
1670                 arch_type->set(reg, buf);
1671
1672                 value = buf_to_str(reg->value, reg->size, 16);
1673                 command_print(cmd_ctx, "%s (/%i): 0x%s", reg->name, reg->size, value);
1674                 free(value);
1675
1676                 free(buf);
1677
1678                 return ERROR_OK;
1679         }
1680
1681         command_print(cmd_ctx, "usage: reg <#|name> [value]");
1682
1683         return ERROR_OK;
1684 }
1685
1686
1687 int handle_poll_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1688 {
1689         target_t *target = get_current_target(cmd_ctx);
1690
1691         if (argc == 0)
1692         {
1693                 target_poll(target);
1694                 target_arch_state(target);
1695         }
1696         else
1697         {
1698                 if (strcmp(args[0], "on") == 0)
1699                 {
1700                         target_continous_poll = 1;
1701                 }
1702                 else if (strcmp(args[0], "off") == 0)
1703                 {
1704                         target_continous_poll = 0;
1705                 }
1706                 else
1707                 {
1708                         command_print(cmd_ctx, "arg is \"on\" or \"off\"");
1709                 }
1710         }
1711
1712
1713         return ERROR_OK;
1714 }
1715
1716 int handle_wait_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1717 {
1718         int ms = 5000;
1719
1720         if (argc > 0)
1721         {
1722                 char *end;
1723
1724                 ms = strtoul(args[0], &end, 0) * 1000;
1725                 if (*end)
1726                 {
1727                         command_print(cmd_ctx, "usage: %s [seconds]", cmd);
1728                         return ERROR_OK;
1729                 }
1730         }
1731         target_t *target = get_current_target(cmd_ctx);
1732
1733         return target_wait_state(target, TARGET_HALTED, ms);
1734 }
1735
1736 int target_wait_state(target_t *target, enum target_state state, int ms)
1737 {
1738         int retval;
1739         struct timeval timeout, now;
1740         int once=1;
1741         gettimeofday(&timeout, NULL);
1742         timeval_add_time(&timeout, 0, ms * 1000);
1743
1744         for (;;)
1745         {
1746                 if ((retval=target_poll(target))!=ERROR_OK)
1747                         return retval;
1748                 target_call_timer_callbacks_now();
1749                 if (target->state == state)
1750                 {
1751                         break;
1752                 }
1753                 if (once)
1754                 {
1755                         once=0;
1756                         LOG_USER("waiting for target %s...", target_state_strings[state]);
1757                 }
1758
1759                 gettimeofday(&now, NULL);
1760                 if ((now.tv_sec > timeout.tv_sec) || ((now.tv_sec == timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec)))
1761                 {
1762                         LOG_ERROR("timed out while waiting for target %s", target_state_strings[state]);
1763                         break;
1764                 }
1765         }
1766
1767         return ERROR_OK;
1768 }
1769
1770 int handle_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1771 {
1772         int retval;
1773         target_t *target = get_current_target(cmd_ctx);
1774
1775         LOG_DEBUG("-");
1776
1777         if ((retval = target_halt(target)) != ERROR_OK)
1778         {
1779                 return retval;
1780         }
1781
1782         return handle_wait_halt_command(cmd_ctx, cmd, args, argc);
1783 }
1784
1785 int handle_soft_reset_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1786 {
1787         target_t *target = get_current_target(cmd_ctx);
1788
1789         LOG_USER("requesting target halt and executing a soft reset");
1790
1791         target->type->soft_reset_halt(target);
1792
1793         return ERROR_OK;
1794 }
1795
1796 int handle_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1797 {
1798         target_t *target = get_current_target(cmd_ctx);
1799         enum target_reset_mode reset_mode = RESET_RUN;
1800
1801         LOG_DEBUG("-");
1802
1803         if (argc >= 1)
1804         {
1805                 if (strcmp("run", args[0]) == 0)
1806                         reset_mode = RESET_RUN;
1807                 else if (strcmp("halt", args[0]) == 0)
1808                         reset_mode = RESET_HALT;
1809                 else if (strcmp("init", args[0]) == 0)
1810                         reset_mode = RESET_INIT;
1811                 else if (strcmp("run_and_halt", args[0]) == 0)
1812                 {
1813                         reset_mode = RESET_RUN_AND_HALT;
1814                         if (argc >= 2)
1815                         {
1816                                 target->run_and_halt_time = strtoul(args[1], NULL, 0);
1817                         }
1818                 }
1819                 else if (strcmp("run_and_init", args[0]) == 0)
1820                 {
1821                         reset_mode = RESET_RUN_AND_INIT;
1822                         if (argc >= 2)
1823                         {
1824                                 target->run_and_halt_time = strtoul(args[1], NULL, 0);
1825                         }
1826                 }
1827                 else
1828                 {
1829                         command_print(cmd_ctx, "usage: reset ['run', 'halt', 'init', 'run_and_halt', 'run_and_init]");
1830                         return ERROR_OK;
1831                 }
1832         }
1833
1834         /* reset *all* targets */
1835         target_process_reset(cmd_ctx, reset_mode);
1836
1837         return ERROR_OK;
1838 }
1839
1840 int handle_resume_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1841 {
1842         int retval;
1843         target_t *target = get_current_target(cmd_ctx);
1844
1845         target_invoke_script(cmd_ctx, target, "pre_resume");
1846
1847         if (argc == 0)
1848                 retval = target_resume(target, 1, 0, 1, 0); /* current pc, addr = 0, handle breakpoints, not debugging */
1849         else if (argc == 1)
1850                 retval = target_resume(target, 0, strtoul(args[0], NULL, 0), 1, 0); /* addr = args[0], handle breakpoints, not debugging */
1851         else
1852         {
1853                 return ERROR_COMMAND_SYNTAX_ERROR;
1854         }
1855
1856         return retval;
1857 }
1858
1859 int handle_step_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1860 {
1861         target_t *target = get_current_target(cmd_ctx);
1862
1863         LOG_DEBUG("-");
1864
1865         if (argc == 0)
1866                 target->type->step(target, 1, 0, 1); /* current pc, addr = 0, handle breakpoints */
1867
1868         if (argc == 1)
1869                 target->type->step(target, 0, strtoul(args[0], NULL, 0), 1); /* addr = args[0], handle breakpoints */
1870
1871         return ERROR_OK;
1872 }
1873
1874 int handle_md_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1875 {
1876         const int line_bytecnt = 32;
1877         int count = 1;
1878         int size = 4;
1879         u32 address = 0;
1880         int line_modulo;
1881         int i;
1882
1883         char output[128];
1884         int output_len;
1885
1886         int retval;
1887
1888         u8 *buffer;
1889         target_t *target = get_current_target(cmd_ctx);
1890
1891         if (argc < 1)
1892                 return ERROR_OK;
1893
1894         if (argc == 2)
1895                 count = strtoul(args[1], NULL, 0);
1896
1897         address = strtoul(args[0], NULL, 0);
1898
1899
1900         switch (cmd[2])
1901         {
1902                 case 'w':
1903                         size = 4; line_modulo = line_bytecnt / 4;
1904                         break;
1905                 case 'h':
1906                         size = 2; line_modulo = line_bytecnt / 2;
1907                         break;
1908                 case 'b':
1909                         size = 1; line_modulo = line_bytecnt / 1;
1910                         break;
1911                 default:
1912                         return ERROR_OK;
1913         }
1914
1915         buffer = calloc(count, size);
1916         retval  = target->type->read_memory(target, address, size, count, buffer);
1917         if (retval == ERROR_OK)
1918         {
1919                 output_len = 0;
1920
1921                 for (i = 0; i < count; i++)
1922                 {
1923                         if (i%line_modulo == 0)
1924                                 output_len += snprintf(output + output_len, 128 - output_len, "0x%8.8x: ", address + (i*size));
1925
1926                         switch (size)
1927                         {
1928                                 case 4:
1929                                         output_len += snprintf(output + output_len, 128 - output_len, "%8.8x ", target_buffer_get_u32(target, &buffer[i*4]));
1930                                         break;
1931                                 case 2:
1932                                         output_len += snprintf(output + output_len, 128 - output_len, "%4.4x ", target_buffer_get_u16(target, &buffer[i*2]));
1933                                         break;
1934                                 case 1:
1935                                         output_len += snprintf(output + output_len, 128 - output_len, "%2.2x ", buffer[i*1]);
1936                                         break;
1937                         }
1938
1939                         if ((i%line_modulo == line_modulo-1) || (i == count - 1))
1940                         {
1941                                 command_print(cmd_ctx, output);
1942                                 output_len = 0;
1943                         }
1944                 }
1945         }
1946
1947         free(buffer);
1948
1949         return retval;
1950 }
1951
1952 int handle_mw_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1953 {
1954         u32 address = 0;
1955         u32 value = 0;
1956         int count = 1;
1957         int i;
1958         int wordsize;
1959         target_t *target = get_current_target(cmd_ctx);
1960         u8 value_buf[4];
1961
1962          if ((argc < 2) || (argc > 3))
1963                 return ERROR_COMMAND_SYNTAX_ERROR;
1964
1965         address = strtoul(args[0], NULL, 0);
1966         value = strtoul(args[1], NULL, 0);
1967         if (argc == 3)
1968                 count = strtoul(args[2], NULL, 0);
1969
1970         switch (cmd[2])
1971         {
1972                 case 'w':
1973                         wordsize = 4;
1974                         target_buffer_set_u32(target, value_buf, value);
1975                         break;
1976                 case 'h':
1977                         wordsize = 2;
1978                         target_buffer_set_u16(target, value_buf, value);
1979                         break;
1980                 case 'b':
1981                         wordsize = 1;
1982                         value_buf[0] = value;
1983                         break;
1984                 default:
1985                         return ERROR_COMMAND_SYNTAX_ERROR;
1986         }
1987         for (i=0; i<count; i++)
1988         {
1989                 int retval;
1990                 switch (wordsize)
1991                 {
1992                         case 4:
1993                                 retval = target->type->write_memory(target, address + i*wordsize, 4, 1, value_buf);
1994                                 break;
1995                         case 2:
1996                                 retval = target->type->write_memory(target, address + i*wordsize, 2, 1, value_buf);
1997                                 break;
1998                         case 1:
1999                                 retval = target->type->write_memory(target, address + i*wordsize, 1, 1, value_buf);
2000                         break;
2001                         default:
2002                         return ERROR_OK;
2003                 }
2004                 if (retval!=ERROR_OK)
2005                 {
2006                         return retval;
2007                 }
2008         }
2009
2010         return ERROR_OK;
2011
2012 }
2013
2014 int handle_load_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2015 {
2016         u8 *buffer;
2017         u32 buf_cnt;
2018         u32 image_size;
2019         int i;
2020         int retval;
2021
2022         image_t image;
2023
2024         duration_t duration;
2025         char *duration_text;
2026
2027         target_t *target = get_current_target(cmd_ctx);
2028
2029         if (argc < 1)
2030         {
2031                 command_print(cmd_ctx, "usage: load_image <filename> [address] [type]");
2032                 return ERROR_OK;
2033         }
2034
2035         /* a base address isn't always necessary, default to 0x0 (i.e. don't relocate) */
2036         if (argc >= 2)
2037         {
2038                 image.base_address_set = 1;
2039                 image.base_address = strtoul(args[1], NULL, 0);
2040         }
2041         else
2042         {
2043                 image.base_address_set = 0;
2044         }
2045
2046         image.start_address_set = 0;
2047
2048         duration_start_measure(&duration);
2049
2050         if (image_open(&image, args[0], (argc >= 3) ? args[2] : NULL) != ERROR_OK)
2051         {
2052                 return ERROR_OK;
2053         }
2054
2055         image_size = 0x0;
2056         retval = ERROR_OK;
2057         for (i = 0; i < image.num_sections; i++)
2058         {
2059                 buffer = malloc(image.sections[i].size);
2060                 if (buffer == NULL)
2061                 {
2062                         command_print(cmd_ctx, "error allocating buffer for section (%d bytes)", image.sections[i].size);
2063                         break;
2064                 }
2065
2066                 if ((retval = image_read_section(&image, i, 0x0, image.sections[i].size, buffer, &buf_cnt)) != ERROR_OK)
2067                 {
2068                         free(buffer);
2069                         break;
2070                 }
2071                 if ((retval = target_write_buffer(target, image.sections[i].base_address, buf_cnt, buffer)) != ERROR_OK)
2072                 {
2073                         free(buffer);
2074                         break;
2075                 }
2076                 image_size += buf_cnt;
2077                 command_print(cmd_ctx, "%u byte written at address 0x%8.8x", buf_cnt, image.sections[i].base_address);
2078
2079                 free(buffer);
2080         }
2081
2082         duration_stop_measure(&duration, &duration_text);
2083         if (retval==ERROR_OK)
2084         {
2085                 command_print(cmd_ctx, "downloaded %u byte in %s", image_size, duration_text);
2086         }
2087         free(duration_text);
2088
2089         image_close(&image);
2090
2091         return retval;
2092
2093 }
2094
2095 int handle_dump_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2096 {
2097         fileio_t fileio;
2098
2099         u32 address;
2100         u32 size;
2101         u8 buffer[560];
2102         int retval=ERROR_OK;
2103
2104         duration_t duration;
2105         char *duration_text;
2106
2107         target_t *target = get_current_target(cmd_ctx);
2108
2109         if (argc != 3)
2110         {
2111                 command_print(cmd_ctx, "usage: dump_image <filename> <address> <size>");
2112                 return ERROR_OK;
2113         }
2114
2115         address = strtoul(args[1], NULL, 0);
2116         size = strtoul(args[2], NULL, 0);
2117
2118         if ((address & 3) || (size & 3))
2119         {
2120                 command_print(cmd_ctx, "only 32-bit aligned address and size are supported");
2121                 return ERROR_OK;
2122         }
2123
2124         if (fileio_open(&fileio, args[0], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK)
2125         {
2126                 return ERROR_OK;
2127         }
2128
2129         duration_start_measure(&duration);
2130
2131         while (size > 0)
2132         {
2133                 u32 size_written;
2134                 u32 this_run_size = (size > 560) ? 560 : size;
2135
2136                 retval = target->type->read_memory(target, address, 4, this_run_size / 4, buffer);
2137                 if (retval != ERROR_OK)
2138                 {
2139                         break;
2140                 }
2141
2142                 retval = fileio_write(&fileio, this_run_size, buffer, &size_written);
2143                 if (retval != ERROR_OK)
2144                 {
2145                         break;
2146                 }
2147
2148                 size -= this_run_size;
2149                 address += this_run_size;
2150         }
2151
2152         fileio_close(&fileio);
2153
2154         duration_stop_measure(&duration, &duration_text);
2155         if (retval==ERROR_OK)
2156         {
2157                 command_print(cmd_ctx, "dumped %"PRIi64" byte in %s", fileio.size, duration_text);
2158         }
2159         free(duration_text);
2160
2161         return ERROR_OK;
2162 }
2163
2164 int handle_verify_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2165 {
2166         u8 *buffer;
2167         u32 buf_cnt;
2168         u32 image_size;
2169         int i;
2170         int retval;
2171         u32 checksum = 0;
2172         u32 mem_checksum = 0;
2173
2174         image_t image;
2175
2176         duration_t duration;
2177         char *duration_text;
2178
2179         target_t *target = get_current_target(cmd_ctx);
2180
2181         if (argc < 1)
2182         {
2183                 return ERROR_COMMAND_SYNTAX_ERROR;
2184         }
2185
2186         if (!target)
2187         {
2188                 LOG_ERROR("no target selected");
2189                 return ERROR_FAIL;
2190         }
2191
2192         duration_start_measure(&duration);
2193
2194         if (argc >= 2)
2195         {
2196                 image.base_address_set = 1;
2197                 image.base_address = strtoul(args[1], NULL, 0);
2198         }
2199         else
2200         {
2201                 image.base_address_set = 0;
2202                 image.base_address = 0x0;
2203         }
2204
2205         image.start_address_set = 0;
2206
2207         if ((retval=image_open(&image, args[0], (argc == 3) ? args[2] : NULL)) != ERROR_OK)
2208         {
2209                 return retval;
2210         }
2211
2212         image_size = 0x0;
2213         retval=ERROR_OK;
2214         for (i = 0; i < image.num_sections; i++)
2215         {
2216                 buffer = malloc(image.sections[i].size);
2217                 if (buffer == NULL)
2218                 {
2219                         command_print(cmd_ctx, "error allocating buffer for section (%d bytes)", image.sections[i].size);
2220                         break;
2221                 }
2222                 if ((retval = image_read_section(&image, i, 0x0, image.sections[i].size, buffer, &buf_cnt)) != ERROR_OK)
2223                 {
2224                         free(buffer);
2225                         break;
2226                 }
2227
2228                 /* calculate checksum of image */
2229                 image_calculate_checksum( buffer, buf_cnt, &checksum );
2230
2231                 retval = target_checksum_memory(target, image.sections[i].base_address, buf_cnt, &mem_checksum);
2232                 if( retval != ERROR_OK )
2233                 {
2234                         free(buffer);
2235                         break;
2236                 }
2237
2238                 if( checksum != mem_checksum )
2239                 {
2240                         /* failed crc checksum, fall back to a binary compare */
2241                         u8 *data;
2242
2243                         command_print(cmd_ctx, "checksum mismatch - attempting binary compare");
2244
2245                         data = (u8*)malloc(buf_cnt);
2246
2247                         /* Can we use 32bit word accesses? */
2248                         int size = 1;
2249                         int count = buf_cnt;
2250                         if ((count % 4) == 0)
2251                         {
2252                                 size *= 4;
2253                                 count /= 4;
2254                         }
2255                         retval = target->type->read_memory(target, image.sections[i].base_address, size, count, data);
2256                         if (retval == ERROR_OK)
2257                         {
2258                                 int t;
2259                                 for (t = 0; t < buf_cnt; t++)
2260                                 {
2261                                         if (data[t] != buffer[t])
2262                                         {
2263                                                 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]);
2264                                                 free(data);
2265                                                 free(buffer);
2266                                                 retval=ERROR_FAIL;
2267                                                 goto done;
2268                                         }
2269                                 }
2270                         }
2271
2272                         free(data);
2273                 }
2274
2275                 free(buffer);
2276                 image_size += buf_cnt;
2277         }
2278 done:
2279         duration_stop_measure(&duration, &duration_text);
2280         if (retval==ERROR_OK)
2281         {
2282                 command_print(cmd_ctx, "verified %u bytes in %s", image_size, duration_text);
2283         }
2284         free(duration_text);
2285
2286         image_close(&image);
2287
2288         return retval;
2289 }
2290
2291 int handle_bp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2292 {
2293         int retval;
2294         target_t *target = get_current_target(cmd_ctx);
2295
2296         if (argc == 0)
2297         {
2298                 breakpoint_t *breakpoint = target->breakpoints;
2299
2300                 while (breakpoint)
2301                 {
2302                         if (breakpoint->type == BKPT_SOFT)
2303                         {
2304                                 char* buf = buf_to_str(breakpoint->orig_instr, breakpoint->length, 16);
2305                                 command_print(cmd_ctx, "0x%8.8x, 0x%x, %i, 0x%s", breakpoint->address, breakpoint->length, breakpoint->set, buf);
2306                                 free(buf);
2307                         }
2308                         else
2309                         {
2310                                 command_print(cmd_ctx, "0x%8.8x, 0x%x, %i", breakpoint->address, breakpoint->length, breakpoint->set);
2311                         }
2312                         breakpoint = breakpoint->next;
2313                 }
2314         }
2315         else if (argc >= 2)
2316         {
2317                 int hw = BKPT_SOFT;
2318                 u32 length = 0;
2319
2320                 length = strtoul(args[1], NULL, 0);
2321
2322                 if (argc >= 3)
2323                         if (strcmp(args[2], "hw") == 0)
2324                                 hw = BKPT_HARD;
2325
2326                 if ((retval = breakpoint_add(target, strtoul(args[0], NULL, 0), length, hw)) != ERROR_OK)
2327                 {
2328                         LOG_ERROR("Failure setting breakpoints");
2329                 }
2330                 else
2331                 {
2332                         command_print(cmd_ctx, "breakpoint added at address 0x%8.8x", strtoul(args[0], NULL, 0));
2333                 }
2334         }
2335         else
2336         {
2337                 command_print(cmd_ctx, "usage: bp <address> <length> ['hw']");
2338         }
2339
2340         return ERROR_OK;
2341 }
2342
2343 int handle_rbp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2344 {
2345         target_t *target = get_current_target(cmd_ctx);
2346
2347         if (argc > 0)
2348                 breakpoint_remove(target, strtoul(args[0], NULL, 0));
2349
2350         return ERROR_OK;
2351 }
2352
2353 int handle_wp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2354 {
2355         target_t *target = get_current_target(cmd_ctx);
2356         int retval;
2357
2358         if (argc == 0)
2359         {
2360                 watchpoint_t *watchpoint = target->watchpoints;
2361
2362                 while (watchpoint)
2363                 {
2364                         command_print(cmd_ctx, "address: 0x%8.8x, len: 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);
2365                         watchpoint = watchpoint->next;
2366                 }
2367         }
2368         else if (argc >= 2)
2369         {
2370                 enum watchpoint_rw type = WPT_ACCESS;
2371                 u32 data_value = 0x0;
2372                 u32 data_mask = 0xffffffff;
2373
2374                 if (argc >= 3)
2375                 {
2376                         switch(args[2][0])
2377                         {
2378                                 case 'r':
2379                                         type = WPT_READ;
2380                                         break;
2381                                 case 'w':
2382                                         type = WPT_WRITE;
2383                                         break;
2384                                 case 'a':
2385                                         type = WPT_ACCESS;
2386                                         break;
2387                                 default:
2388                                         command_print(cmd_ctx, "usage: wp <address> <length> [r/w/a] [value] [mask]");
2389                                         return ERROR_OK;
2390                         }
2391                 }
2392                 if (argc >= 4)
2393                 {
2394                         data_value = strtoul(args[3], NULL, 0);
2395                 }
2396                 if (argc >= 5)
2397                 {
2398                         data_mask = strtoul(args[4], NULL, 0);
2399                 }
2400
2401                 if ((retval = watchpoint_add(target, strtoul(args[0], NULL, 0),
2402                                 strtoul(args[1], NULL, 0), type, data_value, data_mask)) != ERROR_OK)
2403                 {
2404                         LOG_ERROR("Failure setting breakpoints");
2405                 }
2406         }
2407         else
2408         {
2409                 command_print(cmd_ctx, "usage: wp <address> <length> [r/w/a] [value] [mask]");
2410         }
2411
2412         return ERROR_OK;
2413 }
2414
2415 int handle_rwp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2416 {
2417         target_t *target = get_current_target(cmd_ctx);
2418
2419         if (argc > 0)
2420                 watchpoint_remove(target, strtoul(args[0], NULL, 0));
2421
2422         return ERROR_OK;
2423 }
2424
2425 int handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
2426 {
2427         int retval;
2428         target_t *target = get_current_target(cmd_ctx);
2429         u32 va;
2430         u32 pa;
2431
2432         if (argc != 1)
2433         {
2434                 return ERROR_COMMAND_SYNTAX_ERROR;
2435         }
2436         va = strtoul(args[0], NULL, 0);
2437
2438         retval = target->type->virt2phys(target, va, &pa);
2439         if (retval == ERROR_OK)
2440         {
2441                 command_print(cmd_ctx, "Physical address 0x%08x", pa);
2442         }
2443         else
2444         {
2445                 /* lower levels will have logged a detailed error which is
2446                  * forwarded to telnet/GDB session.
2447                  */
2448         }
2449         return retval;
2450 }
2451 static void writeLong(FILE *f, int l)
2452 {
2453         int i;
2454         for (i=0; i<4; i++)
2455         {
2456                 char c=(l>>(i*8))&0xff;
2457                 fwrite(&c, 1, 1, f);
2458         }
2459
2460 }
2461 static void writeString(FILE *f, char *s)
2462 {
2463         fwrite(s, 1, strlen(s), f);
2464 }
2465
2466
2467
2468 // Dump a gmon.out histogram file.
2469 static void writeGmon(u32 *samples, int sampleNum, char *filename)
2470 {
2471         int i;
2472         FILE *f=fopen(filename, "w");
2473         if (f==NULL)
2474                 return;
2475         fwrite("gmon", 1, 4, f);
2476         writeLong(f, 0x00000001); // Version
2477         writeLong(f, 0); // padding
2478         writeLong(f, 0); // padding
2479         writeLong(f, 0); // padding
2480
2481         fwrite("", 1, 1, f);  // GMON_TAG_TIME_HIST
2482
2483         // figure out bucket size
2484         u32 min=samples[0];
2485         u32 max=samples[0];
2486         for (i=0; i<sampleNum; i++)
2487         {
2488                 if (min>samples[i])
2489                 {
2490                         min=samples[i];
2491                 }
2492                 if (max<samples[i])
2493                 {
2494                         max=samples[i];
2495                 }
2496         }
2497
2498         int addressSpace=(max-min+1);
2499
2500         static int const maxBuckets=256*1024; // maximum buckets.
2501         int length=addressSpace;
2502         if (length > maxBuckets)
2503         {
2504                 length=maxBuckets;
2505         }
2506         int *buckets=malloc(sizeof(int)*length);
2507         if (buckets==NULL)
2508         {
2509                 fclose(f);
2510                 return;
2511         }
2512         memset(buckets, 0, sizeof(int)*length);
2513         for (i=0; i<sampleNum;i++)
2514         {
2515                 u32 address=samples[i];
2516                 long long a=address-min;
2517                 long long b=length-1;
2518                 long long c=addressSpace-1;
2519                 int index=(a*b)/c; // danger!!!! int32 overflows
2520                 buckets[index]++;
2521         }
2522
2523         //                         append binary memory gmon.out &profile_hist_hdr ((char*)&profile_hist_hdr + sizeof(struct gmon_hist_hdr))
2524         writeLong(f, min);                                      // low_pc
2525         writeLong(f, max);              // high_pc
2526         writeLong(f, length);           // # of samples
2527         writeLong(f, 64000000);                         // 64MHz
2528         writeString(f, "seconds");
2529         for (i=0; i<(15-strlen("seconds")); i++)
2530         {
2531                 fwrite("", 1, 1, f);  // padding
2532         }
2533         writeString(f, "s");
2534
2535 //                         append binary memory gmon.out profile_hist_data (profile_hist_data + profile_hist_hdr.hist_size)
2536
2537         char *data=malloc(2*length);
2538         if (data!=NULL)
2539         {
2540                 for (i=0; i<length;i++)
2541                 {
2542                         int val;
2543                         val=buckets[i];
2544                         if (val>65535)
2545                         {
2546                                 val=65535;
2547                         }
2548                         data[i*2]=val&0xff;
2549                         data[i*2+1]=(val>>8)&0xff;
2550                 }
2551                 free(buckets);
2552                 fwrite(data, 1, length*2, f);
2553                 free(data);
2554         } else
2555         {
2556                 free(buckets);
2557         }
2558
2559         fclose(f);
2560 }
2561
2562 /* profiling samples the CPU PC as quickly as OpenOCD is able, which will be used as a random sampling of PC */
2563 int handle_profile_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2564 {
2565         target_t *target = get_current_target(cmd_ctx);
2566         struct timeval timeout, now;
2567
2568         gettimeofday(&timeout, NULL);
2569         if (argc!=2)
2570         {
2571                 return ERROR_COMMAND_SYNTAX_ERROR;
2572         }
2573         char *end;
2574         timeval_add_time(&timeout, strtoul(args[0], &end, 0), 0);
2575         if (*end)
2576         {
2577                 return ERROR_OK;
2578         }
2579
2580         command_print(cmd_ctx, "Starting profiling. Halting and resuming the target as often as we can...");
2581
2582         static const int maxSample=10000;
2583         u32 *samples=malloc(sizeof(u32)*maxSample);
2584         if (samples==NULL)
2585                 return ERROR_OK;
2586
2587         int numSamples=0;
2588         int retval=ERROR_OK;
2589         // hopefully it is safe to cache! We want to stop/restart as quickly as possible.
2590         reg_t *reg = register_get_by_name(target->reg_cache, "pc", 1);
2591
2592         for (;;)
2593         {
2594                 target_poll(target);
2595                 if (target->state == TARGET_HALTED)
2596                 {
2597                         u32 t=*((u32 *)reg->value);
2598                         samples[numSamples++]=t;
2599                         retval = target_resume(target, 1, 0, 0, 0); /* current pc, addr = 0, do not handle breakpoints, not debugging */
2600                         target_poll(target);
2601                         usleep(10*1000); // sleep 10ms, i.e. <100 samples/second.
2602                 } else if (target->state == TARGET_RUNNING)
2603                 {
2604                         // We want to quickly sample the PC.
2605                         target_halt(target);
2606                 } else
2607                 {
2608                         command_print(cmd_ctx, "Target not halted or running");
2609                         retval=ERROR_OK;
2610                         break;
2611                 }
2612                 if (retval!=ERROR_OK)
2613                 {
2614                         break;
2615                 }
2616
2617                 gettimeofday(&now, NULL);
2618                 if ((numSamples>=maxSample) || ((now.tv_sec >= timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec)))
2619                 {
2620                         command_print(cmd_ctx, "Profiling completed. %d samples.", numSamples);
2621                         target_poll(target);
2622                         if (target->state == TARGET_HALTED)
2623                         {
2624                                 target_resume(target, 1, 0, 0, 0); /* current pc, addr = 0, do not handle breakpoints, not debugging */
2625                         }
2626                         target_poll(target);
2627                         writeGmon(samples, numSamples, args[1]);
2628                         command_print(cmd_ctx, "Wrote %s", args[1]);
2629                         break;
2630                 }
2631         }
2632         free(samples);
2633
2634         return ERROR_OK;
2635 }
2636
2637 static int new_int_array_element(Jim_Interp * interp, const char *varname, int idx, u32 val)
2638 {
2639         char *namebuf;
2640         Jim_Obj *nameObjPtr, *valObjPtr;
2641         int result;
2642
2643         namebuf = alloc_printf("%s(%d)", varname, idx);
2644         if (!namebuf)
2645                 return JIM_ERR;
2646
2647         nameObjPtr = Jim_NewStringObj(interp, namebuf, -1);
2648         valObjPtr = Jim_NewIntObj(interp, val);
2649         if (!nameObjPtr || !valObjPtr)
2650         {
2651                 free(namebuf);
2652                 return JIM_ERR;
2653         }
2654
2655         Jim_IncrRefCount(nameObjPtr);
2656         Jim_IncrRefCount(valObjPtr);
2657         result = Jim_SetVariable(interp, nameObjPtr, valObjPtr);
2658         Jim_DecrRefCount(interp, nameObjPtr);
2659         Jim_DecrRefCount(interp, valObjPtr);
2660         free(namebuf);
2661         /* printf("%s(%d) <= 0%08x\n", varname, idx, val); */
2662         return result;
2663 }
2664
2665 static int jim_mem2array(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
2666 {
2667         target_t *target;
2668         command_context_t *context;
2669         long l;
2670         u32 width;
2671         u32 len;
2672         u32 addr;
2673         u32 count;
2674         u32 v;
2675         const char *varname;
2676         u8 buffer[4096];
2677         int  i, n, e, retval;
2678
2679         /* argv[1] = name of array to receive the data
2680          * argv[2] = desired width
2681          * argv[3] = memory address
2682          * argv[4] = count of times to read
2683          */
2684         if (argc != 5) {
2685                 Jim_WrongNumArgs(interp, 1, argv, "varname width addr nelems");
2686                 return JIM_ERR;
2687         }
2688         varname = Jim_GetString(argv[1], &len);
2689         /* given "foo" get space for worse case "foo(%d)" .. add 20 */
2690
2691         e = Jim_GetLong(interp, argv[2], &l);
2692         width = l;
2693         if (e != JIM_OK) {
2694                 return e;
2695         }
2696
2697         e = Jim_GetLong(interp, argv[3], &l);
2698         addr = l;
2699         if (e != JIM_OK) {
2700                 return e;
2701         }
2702         e = Jim_GetLong(interp, argv[4], &l);
2703         len = l;
2704         if (e != JIM_OK) {
2705                 return e;
2706         }
2707         switch (width) {
2708                 case 8:
2709                         width = 1;
2710                         break;
2711                 case 16:
2712                         width = 2;
2713                         break;
2714                 case 32:
2715                         width = 4;
2716                         break;
2717                 default:
2718                         Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2719                         Jim_AppendStrings( interp, Jim_GetResult(interp), "Invalid width param, must be 8/16/32", NULL );
2720                         return JIM_ERR;
2721         }
2722         if (len == 0) {
2723                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2724                 Jim_AppendStrings(interp, Jim_GetResult(interp), "mem2array: zero width read?", NULL);
2725                 return JIM_ERR;
2726         }
2727         if ((addr + (len * width)) < addr) {
2728                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2729                 Jim_AppendStrings(interp, Jim_GetResult(interp), "mem2array: addr + len - wraps to zero?", NULL);
2730                 return JIM_ERR;
2731         }
2732         /* absurd transfer size? */
2733         if (len > 65536) {
2734                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2735                 Jim_AppendStrings(interp, Jim_GetResult(interp), "mem2array: absurd > 64K item request", NULL);
2736                 return JIM_ERR;
2737         }
2738
2739         if ((width == 1) ||
2740                 ((width == 2) && ((addr & 1) == 0)) ||
2741                 ((width == 4) && ((addr & 3) == 0))) {
2742                 /* all is well */
2743         } else {
2744                 char buf[100];
2745                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2746                 sprintf(buf, "mem2array address: 0x%08x is not aligned for %d byte reads", addr, width);
2747                 Jim_AppendStrings(interp, Jim_GetResult(interp), buf , NULL);
2748                 return JIM_ERR;
2749         }
2750
2751         context = Jim_GetAssocData(interp, "context");
2752         if (context == NULL)
2753         {
2754                 LOG_ERROR("mem2array: no command context");
2755                 return JIM_ERR;
2756         }
2757         target = get_current_target(context);
2758         if (target == NULL)
2759         {
2760                 LOG_ERROR("mem2array: no current target");
2761                 return JIM_ERR;
2762         }
2763
2764         /* Transfer loop */
2765
2766         /* index counter */
2767         n = 0;
2768         /* assume ok */
2769         e = JIM_OK;
2770         while (len) {
2771                 /* Slurp... in buffer size chunks */
2772
2773                 count = len; /* in objects.. */
2774                 if (count > (sizeof(buffer)/width)) {
2775                         count = (sizeof(buffer)/width);
2776                 }
2777
2778                 retval = target->type->read_memory( target, addr, width, count, buffer );
2779                 if (retval != ERROR_OK) {
2780                         /* BOO !*/
2781                         LOG_ERROR("mem2array: Read @ 0x%08x, w=%d, cnt=%d, failed", addr, width, count);
2782                         Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2783                         Jim_AppendStrings(interp, Jim_GetResult(interp), "mem2array: cannot read memory", NULL);
2784                         e = JIM_ERR;
2785                         len = 0;
2786                 } else {
2787                         v = 0; /* shut up gcc */
2788                         for (i = 0 ;i < count ;i++, n++) {
2789                                 switch (width) {
2790                                         case 4:
2791                                                 v = target_buffer_get_u32(target, &buffer[i*width]);
2792                                                 break;
2793                                         case 2:
2794                                                 v = target_buffer_get_u16(target, &buffer[i*width]);
2795                                                 break;
2796                                         case 1:
2797                                                 v = buffer[i] & 0x0ff;
2798                                                 break;
2799                                 }
2800                                 new_int_array_element(interp, varname, n, v);
2801                         }
2802                         len -= count;
2803                 }
2804         }
2805
2806         Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2807
2808         return JIM_OK;
2809 }
2810
2811 static int get_int_array_element(Jim_Interp * interp, const char *varname, int idx, u32 *val)
2812 {
2813         char *namebuf;
2814         Jim_Obj *nameObjPtr, *valObjPtr;
2815         int result;
2816         long l;
2817
2818         namebuf = alloc_printf("%s(%d)", varname, idx);
2819         if (!namebuf)
2820                 return JIM_ERR;
2821
2822         nameObjPtr = Jim_NewStringObj(interp, namebuf, -1);
2823         if (!nameObjPtr)
2824         {
2825                 free(namebuf);
2826                 return JIM_ERR;
2827         }
2828
2829         Jim_IncrRefCount(nameObjPtr);
2830         valObjPtr = Jim_GetVariable(interp, nameObjPtr, JIM_ERRMSG);
2831         Jim_DecrRefCount(interp, nameObjPtr);
2832         free(namebuf);
2833         if (valObjPtr == NULL)
2834                 return JIM_ERR;
2835
2836         result = Jim_GetLong(interp, valObjPtr, &l);
2837         /* printf("%s(%d) => 0%08x\n", varname, idx, val); */
2838         *val = l;
2839         return result;
2840 }
2841
2842 static int jim_array2mem(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
2843 {
2844         target_t *target;
2845         command_context_t *context;
2846         long l;
2847         u32 width;
2848         u32 len;
2849         u32 addr;
2850         u32 count;
2851         u32 v;
2852         const char *varname;
2853         u8 buffer[4096];
2854         int  i, n, e, retval;
2855
2856         /* argv[1] = name of array to get the data
2857          * argv[2] = desired width
2858          * argv[3] = memory address
2859          * argv[4] = count to write
2860          */
2861         if (argc != 5) {
2862                 Jim_WrongNumArgs(interp, 1, argv, "varname width addr nelems");
2863                 return JIM_ERR;
2864         }
2865         varname = Jim_GetString(argv[1], &len);
2866         /* given "foo" get space for worse case "foo(%d)" .. add 20 */
2867
2868         e = Jim_GetLong(interp, argv[2], &l);
2869         width = l;
2870         if (e != JIM_OK) {
2871                 return e;
2872         }
2873
2874         e = Jim_GetLong(interp, argv[3], &l);
2875         addr = l;
2876         if (e != JIM_OK) {
2877                 return e;
2878         }
2879         e = Jim_GetLong(interp, argv[4], &l);
2880         len = l;
2881         if (e != JIM_OK) {
2882                 return e;
2883         }
2884         switch (width) {
2885                 case 8:
2886                         width = 1;
2887                         break;
2888                 case 16:
2889                         width = 2;
2890                         break;
2891                 case 32:
2892                         width = 4;
2893                         break;
2894                 default:
2895                         Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2896                         Jim_AppendStrings( interp, Jim_GetResult(interp), "Invalid width param, must be 8/16/32", NULL );
2897                         return JIM_ERR;
2898         }
2899         if (len == 0) {
2900                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2901                 Jim_AppendStrings(interp, Jim_GetResult(interp), "array2mem: zero width read?", NULL);
2902                 return JIM_ERR;
2903         }
2904         if ((addr + (len * width)) < addr) {
2905                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2906                 Jim_AppendStrings(interp, Jim_GetResult(interp), "array2mem: addr + len - wraps to zero?", NULL);
2907                 return JIM_ERR;
2908         }
2909         /* absurd transfer size? */
2910         if (len > 65536) {
2911                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2912                 Jim_AppendStrings(interp, Jim_GetResult(interp), "array2mem: absurd > 64K item request", NULL);
2913                 return JIM_ERR;
2914         }
2915
2916         if ((width == 1) ||
2917                 ((width == 2) && ((addr & 1) == 0)) ||
2918                 ((width == 4) && ((addr & 3) == 0))) {
2919                 /* all is well */
2920         } else {
2921                 char buf[100];
2922                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2923                 sprintf(buf, "array2mem address: 0x%08x is not aligned for %d byte reads", addr, width);
2924                 Jim_AppendStrings(interp, Jim_GetResult(interp), buf , NULL);
2925                 return JIM_ERR;
2926         }
2927
2928         context = Jim_GetAssocData(interp, "context");
2929         if (context == NULL)
2930         {
2931                 LOG_ERROR("array2mem: no command context");
2932                 return JIM_ERR;
2933         }
2934         target = get_current_target(context);
2935         if (target == NULL)
2936         {
2937                 LOG_ERROR("array2mem: no current target");
2938                 return JIM_ERR;
2939         }
2940
2941         /* Transfer loop */
2942
2943         /* index counter */
2944         n = 0;
2945         /* assume ok */
2946         e = JIM_OK;
2947         while (len) {
2948                 /* Slurp... in buffer size chunks */
2949
2950                 count = len; /* in objects.. */
2951                 if (count > (sizeof(buffer)/width)) {
2952                         count = (sizeof(buffer)/width);
2953                 }
2954
2955                 v = 0; /* shut up gcc */
2956                 for (i = 0 ;i < count ;i++, n++) {
2957                         get_int_array_element(interp, varname, n, &v);
2958                         switch (width) {
2959                         case 4:
2960                                 target_buffer_set_u32(target, &buffer[i*width], v);
2961                                 break;
2962                         case 2:
2963                                 target_buffer_set_u16(target, &buffer[i*width], v);
2964                                 break;
2965                         case 1:
2966                                 buffer[i] = v & 0x0ff;
2967                                 break;
2968                         }
2969                 }
2970                 len -= count;
2971
2972                 retval = target->type->write_memory(target, addr, width, count, buffer);
2973                 if (retval != ERROR_OK) {
2974                         /* BOO !*/
2975                         LOG_ERROR("array2mem: Write @ 0x%08x, w=%d, cnt=%d, failed", addr, width, count);
2976                         Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2977                         Jim_AppendStrings(interp, Jim_GetResult(interp), "mem2array: cannot read memory", NULL);
2978                         e = JIM_ERR;
2979                         len = 0;
2980                 }
2981         }
2982
2983         Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2984
2985         return JIM_OK;
2986 }