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 approperiate");
 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 static int wait_state(struct command_context_s *cmd_ctx, char *cmd, enum target_state state, int ms);
1687
1688 int handle_poll_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1689 {
1690         target_t *target = get_current_target(cmd_ctx);
1691
1692         if (argc == 0)
1693         {
1694                 target_poll(target);
1695                 target_arch_state(target);
1696         }
1697         else
1698         {
1699                 if (strcmp(args[0], "on") == 0)
1700                 {
1701                         target_continous_poll = 1;
1702                 }
1703                 else if (strcmp(args[0], "off") == 0)
1704                 {
1705                         target_continous_poll = 0;
1706                 }
1707                 else
1708                 {
1709                         command_print(cmd_ctx, "arg is \"on\" or \"off\"");
1710                 }
1711         }
1712
1713
1714         return ERROR_OK;
1715 }
1716
1717 int handle_wait_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1718 {
1719         int ms = 5000;
1720
1721         if (argc > 0)
1722         {
1723                 char *end;
1724
1725                 ms = strtoul(args[0], &end, 0) * 1000;
1726                 if (*end)
1727                 {
1728                         command_print(cmd_ctx, "usage: %s [seconds]", cmd);
1729                         return ERROR_OK;
1730                 }
1731         }
1732
1733         return wait_state(cmd_ctx, cmd, TARGET_HALTED, ms);
1734 }
1735
1736 static int wait_state(struct command_context_s *cmd_ctx, char *cmd, 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         target_t *target = get_current_target(cmd_ctx);
1745         for (;;)
1746         {
1747                 if ((retval=target_poll(target))!=ERROR_OK)
1748                         return retval;
1749                 target_call_timer_callbacks_now();
1750                 if (target->state == state)
1751                 {
1752                         break;
1753                 }
1754                 if (once)
1755                 {
1756                         once=0;
1757                         command_print(cmd_ctx, "waiting for target %s...", target_state_strings[state]);
1758                 }
1759
1760                 gettimeofday(&now, NULL);
1761                 if ((now.tv_sec > timeout.tv_sec) || ((now.tv_sec == timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec)))
1762                 {
1763                         LOG_ERROR("timed out while waiting for target %s", target_state_strings[state]);
1764                         break;
1765                 }
1766         }
1767
1768         return ERROR_OK;
1769 }
1770
1771 int handle_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1772 {
1773         int retval;
1774         target_t *target = get_current_target(cmd_ctx);
1775
1776         LOG_DEBUG("-");
1777
1778         if ((retval = target_halt(target)) != ERROR_OK)
1779         {
1780                 return retval;
1781         }
1782
1783         return handle_wait_halt_command(cmd_ctx, cmd, args, argc);
1784 }
1785
1786 int handle_soft_reset_halt_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1787 {
1788         target_t *target = get_current_target(cmd_ctx);
1789
1790         LOG_USER("requesting target halt and executing a soft reset");
1791
1792         target->type->soft_reset_halt(target);
1793
1794         return ERROR_OK;
1795 }
1796
1797 int handle_reset_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1798 {
1799         target_t *target = get_current_target(cmd_ctx);
1800         enum target_reset_mode reset_mode = RESET_RUN;
1801
1802         LOG_DEBUG("-");
1803
1804         if (argc >= 1)
1805         {
1806                 if (strcmp("run", args[0]) == 0)
1807                         reset_mode = RESET_RUN;
1808                 else if (strcmp("halt", args[0]) == 0)
1809                         reset_mode = RESET_HALT;
1810                 else if (strcmp("init", args[0]) == 0)
1811                         reset_mode = RESET_INIT;
1812                 else if (strcmp("run_and_halt", args[0]) == 0)
1813                 {
1814                         reset_mode = RESET_RUN_AND_HALT;
1815                         if (argc >= 2)
1816                         {
1817                                 target->run_and_halt_time = strtoul(args[1], NULL, 0);
1818                         }
1819                 }
1820                 else if (strcmp("run_and_init", args[0]) == 0)
1821                 {
1822                         reset_mode = RESET_RUN_AND_INIT;
1823                         if (argc >= 2)
1824                         {
1825                                 target->run_and_halt_time = strtoul(args[1], NULL, 0);
1826                         }
1827                 }
1828                 else
1829                 {
1830                         command_print(cmd_ctx, "usage: reset ['run', 'halt', 'init', 'run_and_halt', 'run_and_init]");
1831                         return ERROR_OK;
1832                 }
1833         }
1834
1835         /* reset *all* targets */
1836         target_process_reset(cmd_ctx, reset_mode);
1837
1838         return ERROR_OK;
1839 }
1840
1841 int handle_resume_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1842 {
1843         int retval;
1844         target_t *target = get_current_target(cmd_ctx);
1845
1846         target_invoke_script(cmd_ctx, target, "pre_resume");
1847
1848         if (argc == 0)
1849                 retval = target_resume(target, 1, 0, 1, 0); /* current pc, addr = 0, handle breakpoints, not debugging */
1850         else if (argc == 1)
1851                 retval = target_resume(target, 0, strtoul(args[0], NULL, 0), 1, 0); /* addr = args[0], handle breakpoints, not debugging */
1852         else
1853         {
1854                 return ERROR_COMMAND_SYNTAX_ERROR;
1855         }
1856
1857         return retval;
1858 }
1859
1860 int handle_step_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1861 {
1862         target_t *target = get_current_target(cmd_ctx);
1863
1864         LOG_DEBUG("-");
1865
1866         if (argc == 0)
1867                 target->type->step(target, 1, 0, 1); /* current pc, addr = 0, handle breakpoints */
1868
1869         if (argc == 1)
1870                 target->type->step(target, 0, strtoul(args[0], NULL, 0), 1); /* addr = args[0], handle breakpoints */
1871
1872         return ERROR_OK;
1873 }
1874
1875 int handle_md_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1876 {
1877         const int line_bytecnt = 32;
1878         int count = 1;
1879         int size = 4;
1880         u32 address = 0;
1881         int line_modulo;
1882         int i;
1883
1884         char output[128];
1885         int output_len;
1886
1887         int retval;
1888
1889         u8 *buffer;
1890         target_t *target = get_current_target(cmd_ctx);
1891
1892         if (argc < 1)
1893                 return ERROR_OK;
1894
1895         if (argc == 2)
1896                 count = strtoul(args[1], NULL, 0);
1897
1898         address = strtoul(args[0], NULL, 0);
1899
1900
1901         switch (cmd[2])
1902         {
1903                 case 'w':
1904                         size = 4; line_modulo = line_bytecnt / 4;
1905                         break;
1906                 case 'h':
1907                         size = 2; line_modulo = line_bytecnt / 2;
1908                         break;
1909                 case 'b':
1910                         size = 1; line_modulo = line_bytecnt / 1;
1911                         break;
1912                 default:
1913                         return ERROR_OK;
1914         }
1915
1916         buffer = calloc(count, size);
1917         retval  = target->type->read_memory(target, address, size, count, buffer);
1918         if (retval == ERROR_OK)
1919         {
1920                 output_len = 0;
1921
1922                 for (i = 0; i < count; i++)
1923                 {
1924                         if (i%line_modulo == 0)
1925                                 output_len += snprintf(output + output_len, 128 - output_len, "0x%8.8x: ", address + (i*size));
1926
1927                         switch (size)
1928                         {
1929                                 case 4:
1930                                         output_len += snprintf(output + output_len, 128 - output_len, "%8.8x ", target_buffer_get_u32(target, &buffer[i*4]));
1931                                         break;
1932                                 case 2:
1933                                         output_len += snprintf(output + output_len, 128 - output_len, "%4.4x ", target_buffer_get_u16(target, &buffer[i*2]));
1934                                         break;
1935                                 case 1:
1936                                         output_len += snprintf(output + output_len, 128 - output_len, "%2.2x ", buffer[i*1]);
1937                                         break;
1938                         }
1939
1940                         if ((i%line_modulo == line_modulo-1) || (i == count - 1))
1941                         {
1942                                 command_print(cmd_ctx, output);
1943                                 output_len = 0;
1944                         }
1945                 }
1946         }
1947
1948         free(buffer);
1949
1950         return retval;
1951 }
1952
1953 int handle_mw_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1954 {
1955         u32 address = 0;
1956         u32 value = 0;
1957         int count = 1;
1958         int i;
1959         int wordsize;
1960         target_t *target = get_current_target(cmd_ctx);
1961         u8 value_buf[4];
1962
1963          if ((argc < 2) || (argc > 3))
1964                 return ERROR_COMMAND_SYNTAX_ERROR;
1965
1966         address = strtoul(args[0], NULL, 0);
1967         value = strtoul(args[1], NULL, 0);
1968         if (argc == 3)
1969                 count = strtoul(args[2], NULL, 0);
1970
1971         switch (cmd[2])
1972         {
1973                 case 'w':
1974                         wordsize = 4;
1975                         target_buffer_set_u32(target, value_buf, value);
1976                         break;
1977                 case 'h':
1978                         wordsize = 2;
1979                         target_buffer_set_u16(target, value_buf, value);
1980                         break;
1981                 case 'b':
1982                         wordsize = 1;
1983                         value_buf[0] = value;
1984                         break;
1985                 default:
1986                         return ERROR_COMMAND_SYNTAX_ERROR;
1987         }
1988         for (i=0; i<count; i++)
1989         {
1990                 int retval;
1991                 switch (wordsize)
1992                 {
1993                         case 4:
1994                                 retval = target->type->write_memory(target, address + i*wordsize, 4, 1, value_buf);
1995                                 break;
1996                         case 2:
1997                                 retval = target->type->write_memory(target, address + i*wordsize, 2, 1, value_buf);
1998                                 break;
1999                         case 1:
2000                                 retval = target->type->write_memory(target, address + i*wordsize, 1, 1, value_buf);
2001                         break;
2002                         default:
2003                         return ERROR_OK;
2004                 }
2005                 if (retval!=ERROR_OK)
2006                 {
2007                         return retval;
2008                 }
2009         }
2010
2011         return ERROR_OK;
2012
2013 }
2014
2015 int handle_load_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2016 {
2017         u8 *buffer;
2018         u32 buf_cnt;
2019         u32 image_size;
2020         int i;
2021         int retval;
2022
2023         image_t image;
2024
2025         duration_t duration;
2026         char *duration_text;
2027
2028         target_t *target = get_current_target(cmd_ctx);
2029
2030         if (argc < 1)
2031         {
2032                 command_print(cmd_ctx, "usage: load_image <filename> [address] [type]");
2033                 return ERROR_OK;
2034         }
2035
2036         /* a base address isn't always necessary, default to 0x0 (i.e. don't relocate) */
2037         if (argc >= 2)
2038         {
2039                 image.base_address_set = 1;
2040                 image.base_address = strtoul(args[1], NULL, 0);
2041         }
2042         else
2043         {
2044                 image.base_address_set = 0;
2045         }
2046
2047         image.start_address_set = 0;
2048
2049         duration_start_measure(&duration);
2050
2051         if (image_open(&image, args[0], (argc >= 3) ? args[2] : NULL) != ERROR_OK)
2052         {
2053                 return ERROR_OK;
2054         }
2055
2056         image_size = 0x0;
2057         retval = ERROR_OK;
2058         for (i = 0; i < image.num_sections; i++)
2059         {
2060                 buffer = malloc(image.sections[i].size);
2061                 if (buffer == NULL)
2062                 {
2063                         command_print(cmd_ctx, "error allocating buffer for section (%d bytes)", image.sections[i].size);
2064                         break;
2065                 }
2066
2067                 if ((retval = image_read_section(&image, i, 0x0, image.sections[i].size, buffer, &buf_cnt)) != ERROR_OK)
2068                 {
2069                         free(buffer);
2070                         break;
2071                 }
2072                 if ((retval = target_write_buffer(target, image.sections[i].base_address, buf_cnt, buffer)) != ERROR_OK)
2073                 {
2074                         free(buffer);
2075                         break;
2076                 }
2077                 image_size += buf_cnt;
2078                 command_print(cmd_ctx, "%u byte written at address 0x%8.8x", buf_cnt, image.sections[i].base_address);
2079
2080                 free(buffer);
2081         }
2082
2083         duration_stop_measure(&duration, &duration_text);
2084         if (retval==ERROR_OK)
2085         {
2086                 command_print(cmd_ctx, "downloaded %u byte in %s", image_size, duration_text);
2087         }
2088         free(duration_text);
2089
2090         image_close(&image);
2091
2092         return retval;
2093
2094 }
2095
2096 int handle_dump_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2097 {
2098         fileio_t fileio;
2099
2100         u32 address;
2101         u32 size;
2102         u8 buffer[560];
2103         int retval=ERROR_OK;
2104
2105         duration_t duration;
2106         char *duration_text;
2107
2108         target_t *target = get_current_target(cmd_ctx);
2109
2110         if (argc != 3)
2111         {
2112                 command_print(cmd_ctx, "usage: dump_image <filename> <address> <size>");
2113                 return ERROR_OK;
2114         }
2115
2116         address = strtoul(args[1], NULL, 0);
2117         size = strtoul(args[2], NULL, 0);
2118
2119         if ((address & 3) || (size & 3))
2120         {
2121                 command_print(cmd_ctx, "only 32-bit aligned address and size are supported");
2122                 return ERROR_OK;
2123         }
2124
2125         if (fileio_open(&fileio, args[0], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK)
2126         {
2127                 return ERROR_OK;
2128         }
2129
2130         duration_start_measure(&duration);
2131
2132         while (size > 0)
2133         {
2134                 u32 size_written;
2135                 u32 this_run_size = (size > 560) ? 560 : size;
2136
2137                 retval = target->type->read_memory(target, address, 4, this_run_size / 4, buffer);
2138                 if (retval != ERROR_OK)
2139                 {
2140                         break;
2141                 }
2142
2143                 retval = fileio_write(&fileio, this_run_size, buffer, &size_written);
2144                 if (retval != ERROR_OK)
2145                 {
2146                         break;
2147                 }
2148
2149                 size -= this_run_size;
2150                 address += this_run_size;
2151         }
2152
2153         fileio_close(&fileio);
2154
2155         duration_stop_measure(&duration, &duration_text);
2156         if (retval==ERROR_OK)
2157         {
2158                 command_print(cmd_ctx, "dumped %"PRIi64" byte in %s", fileio.size, duration_text);
2159         }
2160         free(duration_text);
2161
2162         return ERROR_OK;
2163 }
2164
2165 int handle_verify_image_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2166 {
2167         u8 *buffer;
2168         u32 buf_cnt;
2169         u32 image_size;
2170         int i;
2171         int retval;
2172         u32 checksum = 0;
2173         u32 mem_checksum = 0;
2174
2175         image_t image;
2176
2177         duration_t duration;
2178         char *duration_text;
2179
2180         target_t *target = get_current_target(cmd_ctx);
2181
2182         if (argc < 1)
2183         {
2184                 return ERROR_COMMAND_SYNTAX_ERROR;
2185         }
2186
2187         if (!target)
2188         {
2189                 LOG_ERROR("no target selected");
2190                 return ERROR_FAIL;
2191         }
2192
2193         duration_start_measure(&duration);
2194
2195         if (argc >= 2)
2196         {
2197                 image.base_address_set = 1;
2198                 image.base_address = strtoul(args[1], NULL, 0);
2199         }
2200         else
2201         {
2202                 image.base_address_set = 0;
2203                 image.base_address = 0x0;
2204         }
2205
2206         image.start_address_set = 0;
2207
2208         if ((retval=image_open(&image, args[0], (argc == 3) ? args[2] : NULL)) != ERROR_OK)
2209         {
2210                 return retval;
2211         }
2212
2213         image_size = 0x0;
2214         retval=ERROR_OK;
2215         for (i = 0; i < image.num_sections; i++)
2216         {
2217                 buffer = malloc(image.sections[i].size);
2218                 if (buffer == NULL)
2219                 {
2220                         command_print(cmd_ctx, "error allocating buffer for section (%d bytes)", image.sections[i].size);
2221                         break;
2222                 }
2223                 if ((retval = image_read_section(&image, i, 0x0, image.sections[i].size, buffer, &buf_cnt)) != ERROR_OK)
2224                 {
2225                         free(buffer);
2226                         break;
2227                 }
2228
2229                 /* calculate checksum of image */
2230                 image_calculate_checksum( buffer, buf_cnt, &checksum );
2231
2232                 retval = target_checksum_memory(target, image.sections[i].base_address, buf_cnt, &mem_checksum);
2233                 if( retval != ERROR_OK )
2234                 {
2235                         free(buffer);
2236                         break;
2237                 }
2238
2239                 if( checksum != mem_checksum )
2240                 {
2241                         /* failed crc checksum, fall back to a binary compare */
2242                         u8 *data;
2243
2244                         command_print(cmd_ctx, "checksum mismatch - attempting binary compare");
2245
2246                         data = (u8*)malloc(buf_cnt);
2247
2248                         /* Can we use 32bit word accesses? */
2249                         int size = 1;
2250                         int count = buf_cnt;
2251                         if ((count % 4) == 0)
2252                         {
2253                                 size *= 4;
2254                                 count /= 4;
2255                         }
2256                         retval = target->type->read_memory(target, image.sections[i].base_address, size, count, data);
2257                         if (retval == ERROR_OK)
2258                         {
2259                                 int t;
2260                                 for (t = 0; t < buf_cnt; t++)
2261                                 {
2262                                         if (data[t] != buffer[t])
2263                                         {
2264                                                 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]);
2265                                                 free(data);
2266                                                 free(buffer);
2267                                                 retval=ERROR_FAIL;
2268                                                 goto done;
2269                                         }
2270                                 }
2271                         }
2272
2273                         free(data);
2274                 }
2275
2276                 free(buffer);
2277                 image_size += buf_cnt;
2278         }
2279 done:
2280         duration_stop_measure(&duration, &duration_text);
2281         if (retval==ERROR_OK)
2282         {
2283                 command_print(cmd_ctx, "verified %u bytes in %s", image_size, duration_text);
2284         }
2285         free(duration_text);
2286
2287         image_close(&image);
2288
2289         return retval;
2290 }
2291
2292 int handle_bp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2293 {
2294         int retval;
2295         target_t *target = get_current_target(cmd_ctx);
2296
2297         if (argc == 0)
2298         {
2299                 breakpoint_t *breakpoint = target->breakpoints;
2300
2301                 while (breakpoint)
2302                 {
2303                         if (breakpoint->type == BKPT_SOFT)
2304                         {
2305                                 char* buf = buf_to_str(breakpoint->orig_instr, breakpoint->length, 16);
2306                                 command_print(cmd_ctx, "0x%8.8x, 0x%x, %i, 0x%s", breakpoint->address, breakpoint->length, breakpoint->set, buf);
2307                                 free(buf);
2308                         }
2309                         else
2310                         {
2311                                 command_print(cmd_ctx, "0x%8.8x, 0x%x, %i", breakpoint->address, breakpoint->length, breakpoint->set);
2312                         }
2313                         breakpoint = breakpoint->next;
2314                 }
2315         }
2316         else if (argc >= 2)
2317         {
2318                 int hw = BKPT_SOFT;
2319                 u32 length = 0;
2320
2321                 length = strtoul(args[1], NULL, 0);
2322
2323                 if (argc >= 3)
2324                         if (strcmp(args[2], "hw") == 0)
2325                                 hw = BKPT_HARD;
2326
2327                 if ((retval = breakpoint_add(target, strtoul(args[0], NULL, 0), length, hw)) != ERROR_OK)
2328                 {
2329                         LOG_ERROR("Failure setting breakpoints");
2330                 }
2331                 else
2332                 {
2333                         command_print(cmd_ctx, "breakpoint added at address 0x%8.8x", strtoul(args[0], NULL, 0));
2334                 }
2335         }
2336         else
2337         {
2338                 command_print(cmd_ctx, "usage: bp <address> <length> ['hw']");
2339         }
2340
2341         return ERROR_OK;
2342 }
2343
2344 int handle_rbp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2345 {
2346         target_t *target = get_current_target(cmd_ctx);
2347
2348         if (argc > 0)
2349                 breakpoint_remove(target, strtoul(args[0], NULL, 0));
2350
2351         return ERROR_OK;
2352 }
2353
2354 int handle_wp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2355 {
2356         target_t *target = get_current_target(cmd_ctx);
2357         int retval;
2358
2359         if (argc == 0)
2360         {
2361                 watchpoint_t *watchpoint = target->watchpoints;
2362
2363                 while (watchpoint)
2364                 {
2365                         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);
2366                         watchpoint = watchpoint->next;
2367                 }
2368         }
2369         else if (argc >= 2)
2370         {
2371                 enum watchpoint_rw type = WPT_ACCESS;
2372                 u32 data_value = 0x0;
2373                 u32 data_mask = 0xffffffff;
2374
2375                 if (argc >= 3)
2376                 {
2377                         switch(args[2][0])
2378                         {
2379                                 case 'r':
2380                                         type = WPT_READ;
2381                                         break;
2382                                 case 'w':
2383                                         type = WPT_WRITE;
2384                                         break;
2385                                 case 'a':
2386                                         type = WPT_ACCESS;
2387                                         break;
2388                                 default:
2389                                         command_print(cmd_ctx, "usage: wp <address> <length> [r/w/a] [value] [mask]");
2390                                         return ERROR_OK;
2391                         }
2392                 }
2393                 if (argc >= 4)
2394                 {
2395                         data_value = strtoul(args[3], NULL, 0);
2396                 }
2397                 if (argc >= 5)
2398                 {
2399                         data_mask = strtoul(args[4], NULL, 0);
2400                 }
2401
2402                 if ((retval = watchpoint_add(target, strtoul(args[0], NULL, 0),
2403                                 strtoul(args[1], NULL, 0), type, data_value, data_mask)) != ERROR_OK)
2404                 {
2405                         LOG_ERROR("Failure setting breakpoints");
2406                 }
2407         }
2408         else
2409         {
2410                 command_print(cmd_ctx, "usage: wp <address> <length> [r/w/a] [value] [mask]");
2411         }
2412
2413         return ERROR_OK;
2414 }
2415
2416 int handle_rwp_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2417 {
2418         target_t *target = get_current_target(cmd_ctx);
2419
2420         if (argc > 0)
2421                 watchpoint_remove(target, strtoul(args[0], NULL, 0));
2422
2423         return ERROR_OK;
2424 }
2425
2426 int handle_virt2phys_command(command_context_t *cmd_ctx, char *cmd, char **args, int argc)
2427 {
2428         int retval;
2429         target_t *target = get_current_target(cmd_ctx);
2430         u32 va;
2431         u32 pa;
2432
2433         if (argc != 1)
2434         {
2435                 return ERROR_COMMAND_SYNTAX_ERROR;
2436         }
2437         va = strtoul(args[0], NULL, 0);
2438
2439         retval = target->type->virt2phys(target, va, &pa);
2440         if (retval == ERROR_OK)
2441         {
2442                 command_print(cmd_ctx, "Physical address 0x%08x", pa);
2443         }
2444         else
2445         {
2446                 /* lower levels will have logged a detailed error which is
2447                  * forwarded to telnet/GDB session.
2448                  */
2449         }
2450         return retval;
2451 }
2452 static void writeLong(FILE *f, int l)
2453 {
2454         int i;
2455         for (i=0; i<4; i++)
2456         {
2457                 char c=(l>>(i*8))&0xff;
2458                 fwrite(&c, 1, 1, f);
2459         }
2460
2461 }
2462 static void writeString(FILE *f, char *s)
2463 {
2464         fwrite(s, 1, strlen(s), f);
2465 }
2466
2467
2468
2469 // Dump a gmon.out histogram file.
2470 static void writeGmon(u32 *samples, int sampleNum, char *filename)
2471 {
2472         int i;
2473         FILE *f=fopen(filename, "w");
2474         if (f==NULL)
2475                 return;
2476         fwrite("gmon", 1, 4, f);
2477         writeLong(f, 0x00000001); // Version
2478         writeLong(f, 0); // padding
2479         writeLong(f, 0); // padding
2480         writeLong(f, 0); // padding
2481
2482         fwrite("", 1, 1, f);  // GMON_TAG_TIME_HIST
2483
2484         // figure out bucket size
2485         u32 min=samples[0];
2486         u32 max=samples[0];
2487         for (i=0; i<sampleNum; i++)
2488         {
2489                 if (min>samples[i])
2490                 {
2491                         min=samples[i];
2492                 }
2493                 if (max<samples[i])
2494                 {
2495                         max=samples[i];
2496                 }
2497         }
2498
2499         int addressSpace=(max-min+1);
2500
2501         static int const maxBuckets=256*1024; // maximum buckets.
2502         int length=addressSpace;
2503         if (length > maxBuckets)
2504         {
2505                 length=maxBuckets;
2506         }
2507         int *buckets=malloc(sizeof(int)*length);
2508         if (buckets==NULL)
2509         {
2510                 fclose(f);
2511                 return;
2512         }
2513         memset(buckets, 0, sizeof(int)*length);
2514         for (i=0; i<sampleNum;i++)
2515         {
2516                 u32 address=samples[i];
2517                 long long a=address-min;
2518                 long long b=length-1;
2519                 long long c=addressSpace-1;
2520                 int index=(a*b)/c; // danger!!!! int32 overflows
2521                 buckets[index]++;
2522         }
2523
2524         //                         append binary memory gmon.out &profile_hist_hdr ((char*)&profile_hist_hdr + sizeof(struct gmon_hist_hdr))
2525         writeLong(f, min);                                      // low_pc
2526         writeLong(f, max);              // high_pc
2527         writeLong(f, length);           // # of samples
2528         writeLong(f, 64000000);                         // 64MHz
2529         writeString(f, "seconds");
2530         for (i=0; i<(15-strlen("seconds")); i++)
2531         {
2532                 fwrite("", 1, 1, f);  // padding
2533         }
2534         writeString(f, "s");
2535
2536 //                         append binary memory gmon.out profile_hist_data (profile_hist_data + profile_hist_hdr.hist_size)
2537
2538         char *data=malloc(2*length);
2539         if (data!=NULL)
2540         {
2541                 for (i=0; i<length;i++)
2542                 {
2543                         int val;
2544                         val=buckets[i];
2545                         if (val>65535)
2546                         {
2547                                 val=65535;
2548                         }
2549                         data[i*2]=val&0xff;
2550                         data[i*2+1]=(val>>8)&0xff;
2551                 }
2552                 free(buckets);
2553                 fwrite(data, 1, length*2, f);
2554                 free(data);
2555         } else
2556         {
2557                 free(buckets);
2558         }
2559
2560         fclose(f);
2561 }
2562
2563 /* profiling samples the CPU PC as quickly as OpenOCD is able, which will be used as a random sampling of PC */
2564 int handle_profile_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
2565 {
2566         target_t *target = get_current_target(cmd_ctx);
2567         struct timeval timeout, now;
2568
2569         gettimeofday(&timeout, NULL);
2570         if (argc!=2)
2571         {
2572                 return ERROR_COMMAND_SYNTAX_ERROR;
2573         }
2574         char *end;
2575         timeval_add_time(&timeout, strtoul(args[0], &end, 0), 0);
2576         if (*end)
2577         {
2578                 return ERROR_OK;
2579         }
2580
2581         command_print(cmd_ctx, "Starting profiling. Halting and resuming the target as often as we can...");
2582
2583         static const int maxSample=10000;
2584         u32 *samples=malloc(sizeof(u32)*maxSample);
2585         if (samples==NULL)
2586                 return ERROR_OK;
2587
2588         int numSamples=0;
2589         int retval=ERROR_OK;
2590         // hopefully it is safe to cache! We want to stop/restart as quickly as possible.
2591         reg_t *reg = register_get_by_name(target->reg_cache, "pc", 1);
2592
2593         for (;;)
2594         {
2595                 target_poll(target);
2596                 if (target->state == TARGET_HALTED)
2597                 {
2598                         u32 t=*((u32 *)reg->value);
2599                         samples[numSamples++]=t;
2600                         retval = target_resume(target, 1, 0, 0, 0); /* current pc, addr = 0, do not handle breakpoints, not debugging */
2601                         target_poll(target);
2602                         usleep(10*1000); // sleep 10ms, i.e. <100 samples/second.
2603                 } else if (target->state == TARGET_RUNNING)
2604                 {
2605                         // We want to quickly sample the PC.
2606                         target_halt(target);
2607                 } else
2608                 {
2609                         command_print(cmd_ctx, "Target not halted or running");
2610                         retval=ERROR_OK;
2611                         break;
2612                 }
2613                 if (retval!=ERROR_OK)
2614                 {
2615                         break;
2616                 }
2617
2618                 gettimeofday(&now, NULL);
2619                 if ((numSamples>=maxSample) || ((now.tv_sec >= timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec)))
2620                 {
2621                         command_print(cmd_ctx, "Profiling completed. %d samples.", numSamples);
2622                         target_poll(target);
2623                         if (target->state == TARGET_HALTED)
2624                         {
2625                                 target_resume(target, 1, 0, 0, 0); /* current pc, addr = 0, do not handle breakpoints, not debugging */
2626                         }
2627                         target_poll(target);
2628                         writeGmon(samples, numSamples, args[1]);
2629                         command_print(cmd_ctx, "Wrote %s", args[1]);
2630                         break;
2631                 }
2632         }
2633         free(samples);
2634
2635         return ERROR_OK;
2636 }
2637
2638 static int new_int_array_element(Jim_Interp * interp, const char *varname, int idx, u32 val)
2639 {
2640         char *namebuf;
2641         Jim_Obj *nameObjPtr, *valObjPtr;
2642         int result;
2643
2644         namebuf = alloc_printf("%s(%d)", varname, idx);
2645         if (!namebuf)
2646                 return JIM_ERR;
2647
2648         nameObjPtr = Jim_NewStringObj(interp, namebuf, -1);
2649         valObjPtr = Jim_NewIntObj(interp, val);
2650         if (!nameObjPtr || !valObjPtr)
2651         {
2652                 free(namebuf);
2653                 return JIM_ERR;
2654         }
2655
2656         Jim_IncrRefCount(nameObjPtr);
2657         Jim_IncrRefCount(valObjPtr);
2658         result = Jim_SetVariable(interp, nameObjPtr, valObjPtr);
2659         Jim_DecrRefCount(interp, nameObjPtr);
2660         Jim_DecrRefCount(interp, valObjPtr);
2661         free(namebuf);
2662         /* printf("%s(%d) <= 0%08x\n", varname, idx, val); */
2663         return result;
2664 }
2665
2666 static int jim_mem2array(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
2667 {
2668         target_t *target;
2669         command_context_t *context;
2670         long l;
2671         u32 width;
2672         u32 len;
2673         u32 addr;
2674         u32 count;
2675         u32 v;
2676         const char *varname;
2677         u8 buffer[4096];
2678         int  i, n, e, retval;
2679
2680         /* argv[1] = name of array to receive the data
2681          * argv[2] = desired width
2682          * argv[3] = memory address
2683          * argv[4] = count of times to read
2684          */
2685         if (argc != 5) {
2686                 Jim_WrongNumArgs(interp, 1, argv, "varname width addr nelems");
2687                 return JIM_ERR;
2688         }
2689         varname = Jim_GetString(argv[1], &len);
2690         /* given "foo" get space for worse case "foo(%d)" .. add 20 */
2691
2692         e = Jim_GetLong(interp, argv[2], &l);
2693         width = l;
2694         if (e != JIM_OK) {
2695                 return e;
2696         }
2697
2698         e = Jim_GetLong(interp, argv[3], &l);
2699         addr = l;
2700         if (e != JIM_OK) {
2701                 return e;
2702         }
2703         e = Jim_GetLong(interp, argv[4], &l);
2704         len = l;
2705         if (e != JIM_OK) {
2706                 return e;
2707         }
2708         switch (width) {
2709                 case 8:
2710                         width = 1;
2711                         break;
2712                 case 16:
2713                         width = 2;
2714                         break;
2715                 case 32:
2716                         width = 4;
2717                         break;
2718                 default:
2719                         Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2720                         Jim_AppendStrings( interp, Jim_GetResult(interp), "Invalid width param, must be 8/16/32", NULL );
2721                         return JIM_ERR;
2722         }
2723         if (len == 0) {
2724                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2725                 Jim_AppendStrings(interp, Jim_GetResult(interp), "mem2array: zero width read?", NULL);
2726                 return JIM_ERR;
2727         }
2728         if ((addr + (len * width)) < addr) {
2729                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2730                 Jim_AppendStrings(interp, Jim_GetResult(interp), "mem2array: addr + len - wraps to zero?", NULL);
2731                 return JIM_ERR;
2732         }
2733         /* absurd transfer size? */
2734         if (len > 65536) {
2735                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2736                 Jim_AppendStrings(interp, Jim_GetResult(interp), "mem2array: absurd > 64K item request", NULL);
2737                 return JIM_ERR;
2738         }
2739
2740         if ((width == 1) ||
2741                 ((width == 2) && ((addr & 1) == 0)) ||
2742                 ((width == 4) && ((addr & 3) == 0))) {
2743                 /* all is well */
2744         } else {
2745                 char buf[100];
2746                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2747                 sprintf(buf, "mem2array address: 0x%08x is not aligned for %d byte reads", addr, width);
2748                 Jim_AppendStrings(interp, Jim_GetResult(interp), buf , NULL);
2749                 return JIM_ERR;
2750         }
2751
2752         context = Jim_GetAssocData(interp, "context");
2753         if (context == NULL)
2754         {
2755                 LOG_ERROR("mem2array: no command context");
2756                 return JIM_ERR;
2757         }
2758         target = get_current_target(context);
2759         if (target == NULL)
2760         {
2761                 LOG_ERROR("mem2array: no current target");
2762                 return JIM_ERR;
2763         }
2764
2765         /* Transfer loop */
2766
2767         /* index counter */
2768         n = 0;
2769         /* assume ok */
2770         e = JIM_OK;
2771         while (len) {
2772                 /* Slurp... in buffer size chunks */
2773
2774                 count = len; /* in objects.. */
2775                 if (count > (sizeof(buffer)/width)) {
2776                         count = (sizeof(buffer)/width);
2777                 }
2778
2779                 retval = target->type->read_memory( target, addr, width, count, buffer );
2780                 if (retval != ERROR_OK) {
2781                         /* BOO !*/
2782                         LOG_ERROR("mem2array: Read @ 0x%08x, w=%d, cnt=%d, failed", addr, width, count);
2783                         Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2784                         Jim_AppendStrings(interp, Jim_GetResult(interp), "mem2array: cannot read memory", NULL);
2785                         e = JIM_ERR;
2786                         len = 0;
2787                 } else {
2788                         v = 0; /* shut up gcc */
2789                         for (i = 0 ;i < count ;i++, n++) {
2790                                 switch (width) {
2791                                         case 4:
2792                                                 v = target_buffer_get_u32(target, &buffer[i*width]);
2793                                                 break;
2794                                         case 2:
2795                                                 v = target_buffer_get_u16(target, &buffer[i*width]);
2796                                                 break;
2797                                         case 1:
2798                                                 v = buffer[i] & 0x0ff;
2799                                                 break;
2800                                 }
2801                                 new_int_array_element(interp, varname, n, v);
2802                         }
2803                         len -= count;
2804                 }
2805         }
2806
2807         Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2808
2809         return JIM_OK;
2810 }
2811
2812 static int get_int_array_element(Jim_Interp * interp, const char *varname, int idx, u32 *val)
2813 {
2814         char *namebuf;
2815         Jim_Obj *nameObjPtr, *valObjPtr;
2816         int result;
2817         long l;
2818
2819         namebuf = alloc_printf("%s(%d)", varname, idx);
2820         if (!namebuf)
2821                 return JIM_ERR;
2822
2823         nameObjPtr = Jim_NewStringObj(interp, namebuf, -1);
2824         if (!nameObjPtr)
2825         {
2826                 free(namebuf);
2827                 return JIM_ERR;
2828         }
2829
2830         Jim_IncrRefCount(nameObjPtr);
2831         valObjPtr = Jim_GetVariable(interp, nameObjPtr, JIM_ERRMSG);
2832         Jim_DecrRefCount(interp, nameObjPtr);
2833         free(namebuf);
2834         if (valObjPtr == NULL)
2835                 return JIM_ERR;
2836
2837         result = Jim_GetLong(interp, valObjPtr, &l);
2838         /* printf("%s(%d) => 0%08x\n", varname, idx, val); */
2839         *val = l;
2840         return result;
2841 }
2842
2843 static int jim_array2mem(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
2844 {
2845         target_t *target;
2846         command_context_t *context;
2847         long l;
2848         u32 width;
2849         u32 len;
2850         u32 addr;
2851         u32 count;
2852         u32 v;
2853         const char *varname;
2854         u8 buffer[4096];
2855         int  i, n, e, retval;
2856
2857         /* argv[1] = name of array to get the data
2858          * argv[2] = desired width
2859          * argv[3] = memory address
2860          * argv[4] = count to write
2861          */
2862         if (argc != 5) {
2863                 Jim_WrongNumArgs(interp, 1, argv, "varname width addr nelems");
2864                 return JIM_ERR;
2865         }
2866         varname = Jim_GetString(argv[1], &len);
2867         /* given "foo" get space for worse case "foo(%d)" .. add 20 */
2868
2869         e = Jim_GetLong(interp, argv[2], &l);
2870         width = l;
2871         if (e != JIM_OK) {
2872                 return e;
2873         }
2874
2875         e = Jim_GetLong(interp, argv[3], &l);
2876         addr = l;
2877         if (e != JIM_OK) {
2878                 return e;
2879         }
2880         e = Jim_GetLong(interp, argv[4], &l);
2881         len = l;
2882         if (e != JIM_OK) {
2883                 return e;
2884         }
2885         switch (width) {
2886                 case 8:
2887                         width = 1;
2888                         break;
2889                 case 16:
2890                         width = 2;
2891                         break;
2892                 case 32:
2893                         width = 4;
2894                         break;
2895                 default:
2896                         Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2897                         Jim_AppendStrings( interp, Jim_GetResult(interp), "Invalid width param, must be 8/16/32", NULL );
2898                         return JIM_ERR;
2899         }
2900         if (len == 0) {
2901                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2902                 Jim_AppendStrings(interp, Jim_GetResult(interp), "array2mem: zero width read?", NULL);
2903                 return JIM_ERR;
2904         }
2905         if ((addr + (len * width)) < addr) {
2906                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2907                 Jim_AppendStrings(interp, Jim_GetResult(interp), "array2mem: addr + len - wraps to zero?", NULL);
2908                 return JIM_ERR;
2909         }
2910         /* absurd transfer size? */
2911         if (len > 65536) {
2912                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2913                 Jim_AppendStrings(interp, Jim_GetResult(interp), "array2mem: absurd > 64K item request", NULL);
2914                 return JIM_ERR;
2915         }
2916
2917         if ((width == 1) ||
2918                 ((width == 2) && ((addr & 1) == 0)) ||
2919                 ((width == 4) && ((addr & 3) == 0))) {
2920                 /* all is well */
2921         } else {
2922                 char buf[100];
2923                 Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2924                 sprintf(buf, "array2mem address: 0x%08x is not aligned for %d byte reads", addr, width);
2925                 Jim_AppendStrings(interp, Jim_GetResult(interp), buf , NULL);
2926                 return JIM_ERR;
2927         }
2928
2929         context = Jim_GetAssocData(interp, "context");
2930         if (context == NULL)
2931         {
2932                 LOG_ERROR("array2mem: no command context");
2933                 return JIM_ERR;
2934         }
2935         target = get_current_target(context);
2936         if (target == NULL)
2937         {
2938                 LOG_ERROR("array2mem: no current target");
2939                 return JIM_ERR;
2940         }
2941
2942         /* Transfer loop */
2943
2944         /* index counter */
2945         n = 0;
2946         /* assume ok */
2947         e = JIM_OK;
2948         while (len) {
2949                 /* Slurp... in buffer size chunks */
2950
2951                 count = len; /* in objects.. */
2952                 if (count > (sizeof(buffer)/width)) {
2953                         count = (sizeof(buffer)/width);
2954                 }
2955
2956                 v = 0; /* shut up gcc */
2957                 for (i = 0 ;i < count ;i++, n++) {
2958                         get_int_array_element(interp, varname, n, &v);
2959                         switch (width) {
2960                         case 4:
2961                                 target_buffer_set_u32(target, &buffer[i*width], v);
2962                                 break;
2963                         case 2:
2964                                 target_buffer_set_u16(target, &buffer[i*width], v);
2965                                 break;
2966                         case 1:
2967                                 buffer[i] = v & 0x0ff;
2968                                 break;
2969                         }
2970                 }
2971                 len -= count;
2972
2973                 retval = target->type->write_memory(target, addr, width, count, buffer);
2974                 if (retval != ERROR_OK) {
2975                         /* BOO !*/
2976                         LOG_ERROR("array2mem: Write @ 0x%08x, w=%d, cnt=%d, failed", addr, width, count);
2977                         Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2978                         Jim_AppendStrings(interp, Jim_GetResult(interp), "mem2array: cannot read memory", NULL);
2979                         e = JIM_ERR;
2980                         len = 0;
2981                 }
2982         }
2983
2984         Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
2985
2986         return JIM_OK;
2987 }