]> git.sur5r.net Git - openocd/blobdiff - src/target/target.c
target: remove handle_profile_command memory leak
[openocd] / src / target / target.c
index bfa0db79e1718e7727c868a959ea202095d04ea9..b868383d1bed017654b2dfcceb232060293c0fe4 100644 (file)
@@ -36,7 +36,7 @@
  *   You should have received a copy of the GNU General Public License     *
  *   along with this program; if not, write to the                         *
  *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
  ***************************************************************************/
 
 #ifdef HAVE_CONFIG_H
 #include "image.h"
 #include "rtos/rtos.h"
 
+/* default halt wait timeout (ms) */
+#define DEFAULT_HALT_TIMEOUT 5000
+
 static int target_read_buffer_default(struct target *target, uint32_t address,
-               uint32_t size, uint8_t *buffer);
+               uint32_t count, uint8_t *buffer);
 static int target_write_buffer_default(struct target *target, uint32_t address,
-               uint32_t size, const uint8_t *buffer);
+               uint32_t count, const uint8_t *buffer);
 static int target_array2mem(Jim_Interp *interp, struct target *target,
                int argc, Jim_Obj * const *argv);
 static int target_mem2array(Jim_Interp *interp, struct target *target,
                int argc, Jim_Obj * const *argv);
 static int target_register_user_commands(struct command_context *cmd_ctx);
+static int target_get_gdb_fileio_info_default(struct target *target,
+               struct gdb_fileio_info *fileio_info);
+static int target_gdb_fileio_end_default(struct target *target, int retcode,
+               int fileio_errno, bool ctrl_c);
+static int target_profiling_default(struct target *target, uint32_t *samples,
+               uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds);
 
 /* targets */
 extern struct target_type arm7tdmi_target;
@@ -78,7 +87,7 @@ extern struct target_type fa526_target;
 extern struct target_type feroceon_target;
 extern struct target_type dragonite_target;
 extern struct target_type xscale_target;
-extern struct target_type cortexm3_target;
+extern struct target_type cortexm_target;
 extern struct target_type cortexa8_target;
 extern struct target_type cortexr4_target;
 extern struct target_type arm11_target;
@@ -92,6 +101,8 @@ extern struct target_type hla_target;
 extern struct target_type nds32_v2_target;
 extern struct target_type nds32_v3_target;
 extern struct target_type nds32_v3m_target;
+extern struct target_type or1k_target;
+extern struct target_type quark_x10xx_target;
 
 static struct target_type *target_types[] = {
        &arm7tdmi_target,
@@ -105,7 +116,7 @@ static struct target_type *target_types[] = {
        &feroceon_target,
        &dragonite_target,
        &xscale_target,
-       &cortexm3_target,
+       &cortexm_target,
        &cortexa8_target,
        &cortexr4_target,
        &arm11_target,
@@ -119,6 +130,8 @@ static struct target_type *target_types[] = {
        &nds32_v2_target,
        &nds32_v3_target,
        &nds32_v3m_target,
+       &or1k_target,
+       &quark_x10xx_target,
        NULL,
 };
 
@@ -221,6 +234,7 @@ static const Jim_Nvp nvp_target_debug_reason[] = {
        { .name = "watchpoint-and-breakpoint", .value = DBG_REASON_WPTANDBKPT },
        { .name = "single-step"              , .value = DBG_REASON_SINGLESTEP },
        { .name = "target-not-halted"        , .value = DBG_REASON_NOTHALTED  },
+       { .name = "program-exit"             , .value = DBG_REASON_EXIT },
        { .name = "undefined"                , .value = DBG_REASON_UNDEFINED },
        { .name = NULL, .value = -1 },
 };
@@ -282,6 +296,15 @@ static int new_target_number(void)
        return x + 1;
 }
 
+/* read a uint64_t from a buffer in target memory endianness */
+uint64_t target_buffer_get_u64(struct target *target, const uint8_t *buffer)
+{
+       if (target->endianness == TARGET_LITTLE_ENDIAN)
+               return le_to_h_u64(buffer);
+       else
+               return be_to_h_u64(buffer);
+}
+
 /* read a uint32_t from a buffer in target memory endianness */
 uint32_t target_buffer_get_u32(struct target *target, const uint8_t *buffer)
 {
@@ -315,6 +338,15 @@ static uint8_t target_buffer_get_u8(struct target *target, const uint8_t *buffer
        return *buffer & 0x0ff;
 }
 
+/* write a uint64_t to a buffer in target memory endianness */
+void target_buffer_set_u64(struct target *target, uint8_t *buffer, uint64_t value)
+{
+       if (target->endianness == TARGET_LITTLE_ENDIAN)
+               h_u64_to_le(buffer, value);
+       else
+               h_u64_to_be(buffer, value);
+}
+
 /* write a uint32_t to a buffer in target memory endianness */
 void target_buffer_set_u32(struct target *target, uint8_t *buffer, uint32_t value)
 {
@@ -348,6 +380,14 @@ static void target_buffer_set_u8(struct target *target, uint8_t *buffer, uint8_t
        *buffer = value;
 }
 
+/* write a uint64_t array to a buffer in target memory endianness */
+void target_buffer_get_u64_array(struct target *target, const uint8_t *buffer, uint32_t count, uint64_t *dstbuf)
+{
+       uint32_t i;
+       for (i = 0; i < count; i++)
+               dstbuf[i] = target_buffer_get_u64(target, &buffer[i * 8]);
+}
+
 /* write a uint32_t array to a buffer in target memory endianness */
 void target_buffer_get_u32_array(struct target *target, const uint8_t *buffer, uint32_t count, uint32_t *dstbuf)
 {
@@ -364,8 +404,16 @@ void target_buffer_get_u16_array(struct target *target, const uint8_t *buffer, u
                dstbuf[i] = target_buffer_get_u16(target, &buffer[i * 2]);
 }
 
+/* write a uint64_t array to a buffer in target memory endianness */
+void target_buffer_set_u64_array(struct target *target, uint8_t *buffer, uint32_t count, const uint64_t *srcbuf)
+{
+       uint32_t i;
+       for (i = 0; i < count; i++)
+               target_buffer_set_u64(target, &buffer[i * 8], srcbuf[i]);
+}
+
 /* write a uint32_t array to a buffer in target memory endianness */
-void target_buffer_set_u32_array(struct target *target, uint8_t *buffer, uint32_t count, uint32_t *srcbuf)
+void target_buffer_set_u32_array(struct target *target, uint8_t *buffer, uint32_t count, const uint32_t *srcbuf)
 {
        uint32_t i;
        for (i = 0; i < count; i++)
@@ -373,7 +421,7 @@ void target_buffer_set_u32_array(struct target *target, uint8_t *buffer, uint32_
 }
 
 /* write a uint16_t array to a buffer in target memory endianness */
-void target_buffer_set_u16_array(struct target *target, uint8_t *buffer, uint32_t count, uint16_t *srcbuf)
+void target_buffer_set_u16_array(struct target *target, uint8_t *buffer, uint32_t count, const uint16_t *srcbuf)
 {
        uint32_t i;
        for (i = 0; i < count; i++)
@@ -456,7 +504,7 @@ int target_poll(struct target *target)
                        target->halt_issued = false;
                else {
                        long long t = timeval_ms() - target->halt_issued_time;
-                       if (t > 1000) {
+                       if (t > DEFAULT_HALT_TIMEOUT) {
                                target->halt_issued = false;
                                LOG_INFO("Halt timed out, wake up GDB.");
                                target_call_event_callbacks(target, TARGET_EVENT_GDB_HALT);
@@ -575,8 +623,10 @@ static int target_process_reset(struct command_context *cmd_ctx, enum target_res
        retval = target_call_timer_callbacks_now();
 
        struct target *target;
-       for (target = all_targets; target; target = target->next)
+       for (target = all_targets; target; target = target->next) {
                target->type->check_reset(target);
+               target->running_alg = false;
+       }
 
        return retval;
 }
@@ -803,7 +853,7 @@ done:
  */
 
 int target_run_flash_async_algorithm(struct target *target,
-               uint8_t *buffer, uint32_t count, int block_size,
+               const uint8_t *buffer, uint32_t count, int block_size,
                int num_mem_params, struct mem_param *mem_params,
                int num_reg_params, struct reg_param *reg_params,
                uint32_t buffer_start, uint32_t buffer_size,
@@ -976,12 +1026,6 @@ int target_write_phys_memory(struct target *target,
        return target->type->write_phys_memory(target, address, size, count, buffer);
 }
 
-static int target_bulk_write_memory_default(struct target *target,
-               uint32_t address, uint32_t count, const uint8_t *buffer)
-{
-       return target_write_memory(target, address, 4, count, buffer);
-}
-
 int target_add_breakpoint(struct target *target,
                struct breakpoint *breakpoint)
 {
@@ -1032,11 +1076,29 @@ int target_remove_watchpoint(struct target *target,
 {
        return target->type->remove_watchpoint(target, watchpoint);
 }
+int target_hit_watchpoint(struct target *target,
+               struct watchpoint **hit_watchpoint)
+{
+       if (target->state != TARGET_HALTED) {
+               LOG_WARNING("target %s is not halted", target->cmd_name);
+               return ERROR_TARGET_NOT_HALTED;
+       }
+
+       if (target->type->hit_watchpoint == NULL) {
+               /* For backward compatible, if hit_watchpoint is not implemented,
+                * return ERROR_FAIL such that gdb_server will not take the nonsense
+                * information. */
+               return ERROR_FAIL;
+       }
+
+       return target->type->hit_watchpoint(target, hit_watchpoint);
+}
 
 int target_get_gdb_reg_list(struct target *target,
-               struct reg **reg_list[], int *reg_list_size)
+               struct reg **reg_list[], int *reg_list_size,
+               enum target_register_class reg_class)
 {
-       return target->type->get_gdb_reg_list(target, reg_list, reg_list_size);
+       return target->type->get_gdb_reg_list(target, reg_list, reg_list_size, reg_class);
 }
 int target_step(struct target *target,
                int current, uint32_t address, int handle_breakpoints)
@@ -1044,6 +1106,35 @@ int target_step(struct target *target,
        return target->type->step(target, current, address, handle_breakpoints);
 }
 
+int target_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fileio_info)
+{
+       if (target->state != TARGET_HALTED) {
+               LOG_WARNING("target %s is not halted", target->cmd_name);
+               return ERROR_TARGET_NOT_HALTED;
+       }
+       return target->type->get_gdb_fileio_info(target, fileio_info);
+}
+
+int target_gdb_fileio_end(struct target *target, int retcode, int fileio_errno, bool ctrl_c)
+{
+       if (target->state != TARGET_HALTED) {
+               LOG_WARNING("target %s is not halted", target->cmd_name);
+               return ERROR_TARGET_NOT_HALTED;
+       }
+       return target->type->gdb_fileio_end(target, retcode, fileio_errno, ctrl_c);
+}
+
+int target_profiling(struct target *target, uint32_t *samples,
+                       uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds)
+{
+       if (target->state != TARGET_HALTED) {
+               LOG_WARNING("target %s is not halted", target->cmd_name);
+               return ERROR_TARGET_NOT_HALTED;
+       }
+       return target->type->profiling(target, samples, max_num_samples,
+                       num_samples, seconds);
+}
+
 /**
  * Reset the @c examined flag for the given target.
  * Pure paranoia -- targets are zeroed on allocation.
@@ -1127,8 +1218,14 @@ static int target_init_one(struct command_context *cmd_ctx,
        if (target->type->write_buffer == NULL)
                target->type->write_buffer = target_write_buffer_default;
 
-       if (target->type->bulk_write_memory == NULL)
-               target->type->bulk_write_memory = target_bulk_write_memory_default;
+       if (target->type->get_gdb_fileio_info == NULL)
+               target->type->get_gdb_fileio_info = target_get_gdb_fileio_info_default;
+
+       if (target->type->gdb_fileio_end == NULL)
+               target->type->gdb_fileio_end = target_gdb_fileio_end_default;
+
+       if (target->type->profiling == NULL)
+               target->type->profiling = target_profiling_default;
 
        return ERROR_OK;
 }
@@ -1264,7 +1361,7 @@ int target_unregister_event_callback(int (*callback)(struct target *target,
        return ERROR_OK;
 }
 
-static int target_unregister_timer_callback(int (*callback)(void *priv), void *priv)
+int target_unregister_timer_callback(int (*callback)(void *priv), void *priv)
 {
        struct target_timer_callback **p = &target_timer_callbacks;
        struct target_timer_callback *c = target_timer_callbacks;
@@ -1679,6 +1776,71 @@ int target_arch_state(struct target *target)
        return retval;
 }
 
+static int target_get_gdb_fileio_info_default(struct target *target,
+               struct gdb_fileio_info *fileio_info)
+{
+       /* If target does not support semi-hosting function, target
+          has no need to provide .get_gdb_fileio_info callback.
+          It just return ERROR_FAIL and gdb_server will return "Txx"
+          as target halted every time.  */
+       return ERROR_FAIL;
+}
+
+static int target_gdb_fileio_end_default(struct target *target,
+               int retcode, int fileio_errno, bool ctrl_c)
+{
+       return ERROR_OK;
+}
+
+static int target_profiling_default(struct target *target, uint32_t *samples,
+               uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds)
+{
+       struct timeval timeout, now;
+
+       gettimeofday(&timeout, NULL);
+       timeval_add_time(&timeout, seconds, 0);
+
+       LOG_INFO("Starting profiling. Halting and resuming the"
+                       " target as often as we can...");
+
+       uint32_t sample_count = 0;
+       /* hopefully it is safe to cache! We want to stop/restart as quickly as possible. */
+       struct reg *reg = register_get_by_name(target->reg_cache, "pc", 1);
+
+       int retval = ERROR_OK;
+       for (;;) {
+               target_poll(target);
+               if (target->state == TARGET_HALTED) {
+                       uint32_t t = *((uint32_t *)reg->value);
+                       samples[sample_count++] = t;
+                       /* current pc, addr = 0, do not handle breakpoints, not debugging */
+                       retval = target_resume(target, 1, 0, 0, 0);
+                       target_poll(target);
+                       alive_sleep(10); /* sleep 10ms, i.e. <100 samples/second. */
+               } else if (target->state == TARGET_RUNNING) {
+                       /* We want to quickly sample the PC. */
+                       retval = target_halt(target);
+               } else {
+                       LOG_INFO("Target not halted or running");
+                       retval = ERROR_OK;
+                       break;
+               }
+
+               if (retval != ERROR_OK)
+                       break;
+
+               gettimeofday(&now, NULL);
+               if ((sample_count >= max_num_samples) ||
+                       ((now.tv_sec >= timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec))) {
+                       LOG_INFO("Profiling completed. %" PRIu32 " samples.", sample_count);
+                       break;
+               }
+       }
+
+       *num_samples = sample_count;
+       return retval;
+}
+
 /* Single aligned words are guaranteed to use 16 or 32 bit access
  * mode respectively, otherwise data is handled as quickly as
  * possible
@@ -1707,57 +1869,37 @@ int target_write_buffer(struct target *target, uint32_t address, uint32_t size,
        return target->type->write_buffer(target, address, size, buffer);
 }
 
-static int target_write_buffer_default(struct target *target, uint32_t address, uint32_t size, const uint8_t *buffer)
+static int target_write_buffer_default(struct target *target, uint32_t address, uint32_t count, const uint8_t *buffer)
 {
-       int retval = ERROR_OK;
-
-       if (((address % 2) == 0) && (size == 2))
-               return target_write_memory(target, address, 2, 1, buffer);
-
-       /* handle unaligned head bytes */
-       if (address % 4) {
-               uint32_t unaligned = 4 - (address % 4);
-
-               if (unaligned > size)
-                       unaligned = size;
+       uint32_t size;
 
-               retval = target_write_memory(target, address, 1, unaligned, buffer);
-               if (retval != ERROR_OK)
-                       return retval;
-
-               buffer += unaligned;
-               address += unaligned;
-               size -= unaligned;
-       }
-
-       /* handle aligned words */
-       if (size >= 4) {
-               int aligned = size - (size % 4);
-
-               /* use bulk writes above a certain limit. This may have to be changed */
-               if (aligned > 128) {
-                       retval = target->type->bulk_write_memory(target, address, aligned / 4, buffer);
-                       if (retval != ERROR_OK)
-                               return retval;
-               } else {
-                       retval = target_write_memory(target, address, 4, aligned / 4, buffer);
+       /* Align up to maximum 4 bytes. The loop condition makes sure the next pass
+        * will have something to do with the size we leave to it. */
+       for (size = 1; size < 4 && count >= size * 2 + (address & size); size *= 2) {
+               if (address & size) {
+                       int retval = target_write_memory(target, address, size, 1, buffer);
                        if (retval != ERROR_OK)
                                return retval;
+                       address += size;
+                       count -= size;
+                       buffer += size;
                }
-
-               buffer += aligned;
-               address += aligned;
-               size -= aligned;
        }
 
-       /* handle tail writes of less than 4 bytes */
-       if (size > 0) {
-               retval = target_write_memory(target, address, 1, size, buffer);
-               if (retval != ERROR_OK)
-                       return retval;
+       /* Write the data with as large access size as possible. */
+       for (; size > 0; size /= 2) {
+               uint32_t aligned = count - count % size;
+               if (aligned > 0) {
+                       int retval = target_write_memory(target, address, size, aligned / size, buffer);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       address += aligned;
+                       count -= aligned;
+                       buffer += aligned;
+               }
        }
 
-       return retval;
+       return ERROR_OK;
 }
 
 /* Single aligned words are guaranteed to use 16 or 32 bit access
@@ -1788,58 +1930,34 @@ int target_read_buffer(struct target *target, uint32_t address, uint32_t size, u
        return target->type->read_buffer(target, address, size, buffer);
 }
 
-static int target_read_buffer_default(struct target *target, uint32_t address, uint32_t size, uint8_t *buffer)
+static int target_read_buffer_default(struct target *target, uint32_t address, uint32_t count, uint8_t *buffer)
 {
-       int retval = ERROR_OK;
-
-       if (((address % 2) == 0) && (size == 2))
-               return target_read_memory(target, address, 2, 1, buffer);
-
-       /* handle unaligned head bytes */
-       if (address % 4) {
-               uint32_t unaligned = 4 - (address % 4);
-
-               if (unaligned > size)
-                       unaligned = size;
+       uint32_t size;
 
-               retval = target_read_memory(target, address, 1, unaligned, buffer);
-               if (retval != ERROR_OK)
-                       return retval;
-
-               buffer += unaligned;
-               address += unaligned;
-               size -= unaligned;
-       }
-
-       /* handle aligned words */
-       if (size >= 4) {
-               int aligned = size - (size % 4);
-
-               retval = target_read_memory(target, address, 4, aligned / 4, buffer);
-               if (retval != ERROR_OK)
-                       return retval;
-
-               buffer += aligned;
-               address += aligned;
-               size -= aligned;
+       /* Align up to maximum 4 bytes. The loop condition makes sure the next pass
+        * will have something to do with the size we leave to it. */
+       for (size = 1; size < 4 && count >= size * 2 + (address & size); size *= 2) {
+               if (address & size) {
+                       int retval = target_read_memory(target, address, size, 1, buffer);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       address += size;
+                       count -= size;
+                       buffer += size;
+               }
        }
 
-       /*prevent byte access when possible (avoid AHB access limitations in some cases)*/
-       if (size        >= 2) {
-               int aligned = size - (size % 2);
-               retval = target_read_memory(target, address, 2, aligned / 2, buffer);
-               if (retval != ERROR_OK)
-                       return retval;
-
-               buffer += aligned;
-               address += aligned;
-               size -= aligned;
-       }
-       /* handle tail writes of less than 4 bytes */
-       if (size > 0) {
-               retval = target_read_memory(target, address, 1, size, buffer);
-               if (retval != ERROR_OK)
-                       return retval;
+       /* Read the data with as large access size as possible. */
+       for (; size > 0; size /= 2) {
+               uint32_t aligned = count - count % size;
+               if (aligned > 0) {
+                       int retval = target_read_memory(target, address, size, aligned / size, buffer);
+                       if (retval != ERROR_OK)
+                               return retval;
+                       address += aligned;
+                       count -= aligned;
+                       buffer += aligned;
+               }
        }
 
        return ERROR_OK;
@@ -1901,6 +2019,30 @@ int target_blank_check_memory(struct target *target, uint32_t address, uint32_t
        return retval;
 }
 
+int target_read_u64(struct target *target, uint64_t address, uint64_t *value)
+{
+       uint8_t value_buf[8];
+       if (!target_was_examined(target)) {
+               LOG_ERROR("Target not examined yet");
+               return ERROR_FAIL;
+       }
+
+       int retval = target_read_memory(target, address, 8, 1, value_buf);
+
+       if (retval == ERROR_OK) {
+               *value = target_buffer_get_u64(target, value_buf);
+               LOG_DEBUG("address: 0x%" PRIx64 ", value: 0x%16.16" PRIx64 "",
+                                 address,
+                                 *value);
+       } else {
+               *value = 0x0;
+               LOG_DEBUG("address: 0x%" PRIx64 " failed",
+                                 address);
+       }
+
+       return retval;
+}
+
 int target_read_u32(struct target *target, uint32_t address, uint32_t *value)
 {
        uint8_t value_buf[4];
@@ -1951,12 +2093,13 @@ int target_read_u16(struct target *target, uint32_t address, uint16_t *value)
 
 int target_read_u8(struct target *target, uint32_t address, uint8_t *value)
 {
-       int retval = target_read_memory(target, address, 1, 1, value);
        if (!target_was_examined(target)) {
                LOG_ERROR("Target not examined yet");
                return ERROR_FAIL;
        }
 
+       int retval = target_read_memory(target, address, 1, 1, value);
+
        if (retval == ERROR_OK) {
                LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%2.2x",
                                  address,
@@ -1970,6 +2113,27 @@ int target_read_u8(struct target *target, uint32_t address, uint8_t *value)
        return retval;
 }
 
+int target_write_u64(struct target *target, uint64_t address, uint64_t value)
+{
+       int retval;
+       uint8_t value_buf[8];
+       if (!target_was_examined(target)) {
+               LOG_ERROR("Target not examined yet");
+               return ERROR_FAIL;
+       }
+
+       LOG_DEBUG("address: 0x%" PRIx64 ", value: 0x%16.16" PRIx64 "",
+                         address,
+                         value);
+
+       target_buffer_set_u64(target, value_buf, value);
+       retval = target_write_memory(target, address, 8, 1, value_buf);
+       if (retval != ERROR_OK)
+               LOG_DEBUG("failed: %i", retval);
+
+       return retval;
+}
+
 int target_write_u32(struct target *target, uint32_t address, uint32_t value)
 {
        int retval;
@@ -2215,6 +2379,10 @@ static int handle_target(void *priv)
        for (struct target *target = all_targets;
                        is_jtag_poll_safe() && target;
                        target = target->next) {
+
+               if (!target_was_examined(target))
+                       continue;
+
                if (!target->tap->enabled)
                        continue;
 
@@ -2409,13 +2577,11 @@ COMMAND_HANDLER(handle_wait_halt_command)
        if (CMD_ARGC > 1)
                return ERROR_COMMAND_SYNTAX_ERROR;
 
-       unsigned ms = 5000;
+       unsigned ms = DEFAULT_HALT_TIMEOUT;
        if (1 == CMD_ARGC) {
                int retval = parse_uint(CMD_ARGV[0], &ms);
                if (ERROR_OK != retval)
                        return ERROR_COMMAND_SYNTAX_ERROR;
-               /* convert seconds (given) to milliseconds (needed) */
-               ms *= 1000;
        }
 
        struct target *target = get_current_target(CMD_CTX);
@@ -2666,12 +2832,6 @@ COMMAND_HANDLER(handle_md_command)
 typedef int (*target_write_fn)(struct target *target,
                uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
 
-static int target_write_memory_fast(struct target *target,
-               uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer)
-{
-       return target_write_buffer(target, address, size * count, buffer);
-}
-
 static int target_fill_mem(struct target *target,
                uint32_t address,
                target_write_fn fn,
@@ -2736,7 +2896,7 @@ COMMAND_HANDLER(handle_mw_command)
                CMD_ARGV++;
                fn = target_write_phys_memory;
        } else
-               fn = target_write_memory_fast;
+               fn = target_write_memory;
        if ((CMD_ARGC < 2) || (CMD_ARGC > 3))
                return ERROR_COMMAND_SYNTAX_ERROR;
 
@@ -3348,8 +3508,11 @@ static void writeString(FILE *f, char *s)
        writeData(f, s, strlen(s));
 }
 
+typedef unsigned char UNIT[2];  /* unit of profiling */
+
 /* Dump a gmon.out histogram file. */
-static void writeGmon(uint32_t *samples, uint32_t sampleNum, const char *filename)
+static void write_gmon(uint32_t *samples, uint32_t sampleNum, const char *filename,
+               bool with_range, uint32_t start_address, uint32_t end_address)
 {
        uint32_t i;
        FILE *f = fopen(filename, "w");
@@ -3365,33 +3528,50 @@ static void writeGmon(uint32_t *samples, uint32_t sampleNum, const char *filenam
        writeData(f, &zero, 1);
 
        /* figure out bucket size */
-       uint32_t min = samples[0];
-       uint32_t max = samples[0];
-       for (i = 0; i < sampleNum; i++) {
-               if (min > samples[i])
-                       min = samples[i];
-               if (max < samples[i])
-                       max = samples[i];
+       uint32_t min;
+       uint32_t max;
+       if (with_range) {
+               min = start_address;
+               max = end_address;
+       } else {
+               min = samples[0];
+               max = samples[0];
+               for (i = 0; i < sampleNum; i++) {
+                       if (min > samples[i])
+                               min = samples[i];
+                       if (max < samples[i])
+                               max = samples[i];
+               }
+
+               /* max should be (largest sample + 1)
+                * Refer to binutils/gprof/hist.c (find_histogram_for_pc) */
+               max++;
        }
 
-       int addressSpace = (max - min + 1);
+       int addressSpace = max - min;
        assert(addressSpace >= 2);
 
-       static const uint32_t maxBuckets = 16 * 1024; /* maximum buckets. */
-       uint32_t length = addressSpace;
-       if (length > maxBuckets)
-               length = maxBuckets;
-       int *buckets = malloc(sizeof(int)*length);
+       /* FIXME: What is the reasonable number of buckets?
+        * The profiling result will be more accurate if there are enough buckets. */
+       static const uint32_t maxBuckets = 128 * 1024; /* maximum buckets. */
+       uint32_t numBuckets = addressSpace / sizeof(UNIT);
+       if (numBuckets > maxBuckets)
+               numBuckets = maxBuckets;
+       int *buckets = malloc(sizeof(int) * numBuckets);
        if (buckets == NULL) {
                fclose(f);
                return;
        }
-       memset(buckets, 0, sizeof(int) * length);
+       memset(buckets, 0, sizeof(int) * numBuckets);
        for (i = 0; i < sampleNum; i++) {
                uint32_t address = samples[i];
+
+               if ((address < min) || (max <= address))
+                       continue;
+
                long long a = address - min;
-               long long b = length - 1;
-               long long c = addressSpace - 1;
+               long long b = numBuckets;
+               long long c = addressSpace;
                int index_t = (a * b) / c; /* danger!!!! int32 overflows */
                buckets[index_t]++;
        }
@@ -3399,7 +3579,7 @@ static void writeGmon(uint32_t *samples, uint32_t sampleNum, const char *filenam
        /* append binary memory gmon.out &profile_hist_hdr ((char*)&profile_hist_hdr + sizeof(struct gmon_hist_hdr)) */
        writeLong(f, min);                      /* low_pc */
        writeLong(f, max);                      /* high_pc */
-       writeLong(f, length);           /* # of samples */
+       writeLong(f, numBuckets);       /* # of buckets */
        writeLong(f, 100);                      /* KLUDGE! We lie, ca. 100Hz best case. */
        writeString(f, "seconds");
        for (i = 0; i < (15-strlen("seconds")); i++)
@@ -3408,9 +3588,9 @@ static void writeGmon(uint32_t *samples, uint32_t sampleNum, const char *filenam
 
        /*append binary memory gmon.out profile_hist_data (profile_hist_data + profile_hist_hdr.hist_size) */
 
-       char *data = malloc(2 * length);
+       char *data = malloc(2 * numBuckets);
        if (data != NULL) {
-               for (i = 0; i < length; i++) {
+               for (i = 0; i < numBuckets; i++) {
                        int val;
                        val = buckets[i];
                        if (val > 65535)
@@ -3419,7 +3599,7 @@ static void writeGmon(uint32_t *samples, uint32_t sampleNum, const char *filenam
                        data[i * 2 + 1] = (val >> 8) & 0xff;
                }
                free(buckets);
-               writeData(f, data, length * 2);
+               writeData(f, data, numBuckets * 2);
                free(data);
        } else
                free(buckets);
@@ -3432,84 +3612,70 @@ static void writeGmon(uint32_t *samples, uint32_t sampleNum, const char *filenam
 COMMAND_HANDLER(handle_profile_command)
 {
        struct target *target = get_current_target(CMD_CTX);
-       struct timeval timeout, now;
 
-       gettimeofday(&timeout, NULL);
-       if (CMD_ARGC != 2)
+       if ((CMD_ARGC != 2) && (CMD_ARGC != 4))
                return ERROR_COMMAND_SYNTAX_ERROR;
-       unsigned offset;
-       COMMAND_PARSE_NUMBER(uint, CMD_ARGV[0], offset);
 
-       timeval_add_time(&timeout, offset, 0);
+       const uint32_t MAX_PROFILE_SAMPLE_NUM = 10000;
+       uint32_t offset;
+       uint32_t num_of_sampels;
+       int retval = ERROR_OK;
+
+       COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], offset);
+
+       uint32_t *samples = malloc(sizeof(uint32_t) * MAX_PROFILE_SAMPLE_NUM);
+       if (samples == NULL) {
+               LOG_ERROR("No memory to store samples.");
+               return ERROR_FAIL;
+       }
 
        /**
-        * @todo: Some cores let us sample the PC without the
+        * Some cores let us sample the PC without the
         * annoying halt/resume step; for example, ARMv7 PCSR.
         * Provide a way to use that more efficient mechanism.
         */
+       retval = target_profiling(target, samples, MAX_PROFILE_SAMPLE_NUM,
+                               &num_of_sampels, offset);
+       if (retval != ERROR_OK) {
+               free(samples);
+               return retval;
+       }
 
-       command_print(CMD_CTX, "Starting profiling. Halting and resuming the target as often as we can...");
-
-       static const int maxSample = 10000;
-       uint32_t *samples = malloc(sizeof(uint32_t)*maxSample);
-       if (samples == NULL)
-               return ERROR_OK;
-
-       int numSamples = 0;
-       /* hopefully it is safe to cache! We want to stop/restart as quickly as possible. */
-       struct reg *reg = register_get_by_name(target->reg_cache, "pc", 1);
+       assert(num_of_sampels <= MAX_PROFILE_SAMPLE_NUM);
 
-       int retval = ERROR_OK;
-       for (;;) {
-               target_poll(target);
-               if (target->state == TARGET_HALTED) {
-                       uint32_t t = *((uint32_t *)reg->value);
-                       samples[numSamples++] = t;
-                       /* current pc, addr = 0, do not handle breakpoints, not debugging */
-                       retval = target_resume(target, 1, 0, 0, 0);
-                       target_poll(target);
-                       alive_sleep(10); /* sleep 10ms, i.e. <100 samples/second. */
-               } else if (target->state == TARGET_RUNNING) {
-                       /* We want to quickly sample the PC. */
-                       retval = target_halt(target);
-                       if (retval != ERROR_OK) {
-                               free(samples);
-                               return retval;
-                       }
-               } else {
-                       command_print(CMD_CTX, "Target not halted or running");
-                       retval = ERROR_OK;
-                       break;
+       retval = target_poll(target);
+       if (retval != ERROR_OK) {
+               free(samples);
+               return retval;
+       }
+       if (target->state == TARGET_RUNNING) {
+               retval = target_halt(target);
+               if (retval != ERROR_OK) {
+                       free(samples);
+                       return retval;
                }
-               if (retval != ERROR_OK)
-                       break;
+       }
 
-               gettimeofday(&now, NULL);
-               if ((numSamples >= maxSample) || ((now.tv_sec >= timeout.tv_sec)
-                               && (now.tv_usec >= timeout.tv_usec))) {
-                       command_print(CMD_CTX, "Profiling completed. %d samples.", numSamples);
-                       retval = target_poll(target);
-                       if (retval != ERROR_OK) {
-                               free(samples);
-                               return retval;
-                       }
-                       if (target->state == TARGET_HALTED) {
-                               /* current pc, addr = 0, do not handle
-                                * breakpoints, not debugging */
-                               target_resume(target, 1, 0, 0, 0);
-                       }
-                       retval = target_poll(target);
-                       if (retval != ERROR_OK) {
-                               free(samples);
-                               return retval;
-                       }
-                       writeGmon(samples, numSamples, CMD_ARGV[1]);
-                       command_print(CMD_CTX, "Wrote %s", CMD_ARGV[1]);
-                       break;
-               }
+       retval = target_poll(target);
+       if (retval != ERROR_OK) {
+               free(samples);
+               return retval;
+       }
+
+       uint32_t start_address = 0;
+       uint32_t end_address = 0;
+       bool with_range = false;
+       if (CMD_ARGC == 4) {
+               with_range = true;
+               COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], start_address);
+               COMMAND_PARSE_NUMBER(u32, CMD_ARGV[3], end_address);
        }
-       free(samples);
 
+       write_gmon(samples, num_of_sampels, CMD_ARGV[1],
+                       with_range, start_address, end_address);
+       command_print(CMD_CTX, "Wrote %s", CMD_ARGV[1]);
+
+       free(samples);
        return retval;
 }
 
@@ -3687,6 +3853,7 @@ static int target_mem2array(Jim_Interp *interp, struct target *target, int argc,
                                new_int_array_element(interp, varname, n, v);
                        }
                        len -= count;
+                       addr += count * width;
                }
        }
 
@@ -3880,6 +4047,7 @@ static int target_array2mem(Jim_Interp *interp, struct target *target,
                        e = JIM_ERR;
                        break;
                }
+               addr += count * width;
        }
 
        free(buffer);
@@ -4180,11 +4348,10 @@ no_params:
                                                                                   n->name);
                                        return JIM_ERR;
                                }
-                               if (target->variant)
-                                       free((void *)(target->variant));
                                e = Jim_GetOpt_String(goi, &cp, NULL);
                                if (e != JIM_OK)
                                        return e;
+                               free(target->variant);
                                target->variant = strdup(cp);
                        } else {
                                if (goi->argc != 0)
@@ -4292,7 +4459,7 @@ static int jim_target_mw(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
        }
 
        target_write_fn fn;
-       fn = target_write_memory_fast;
+       fn = target_write_memory;
 
        int e;
        if (strcmp(Jim_GetString(argv[1], NULL), "phys") == 0) {
@@ -5022,7 +5189,7 @@ static int target_create(Jim_GetOptInfo *goi)
        }
 
        /* now - create the new target name command */
-       const const struct command_registration target_subcommands[] = {
+       const struct command_registration target_subcommands[] = {
                {
                        .chain = target_instance_command_handlers,
                },
@@ -5031,7 +5198,7 @@ static int target_create(Jim_GetOptInfo *goi)
                },
                COMMAND_REGISTRATION_DONE
        };
-       const const struct command_registration target_commands[] = {
+       const struct command_registration target_commands[] = {
                {
                        .name = cp,
                        .mode = COMMAND_ANY,
@@ -5359,7 +5526,7 @@ COMMAND_HANDLER(handle_fast_load_image_command)
                        fastload[i].data = malloc(length);
                        if (fastload[i].data == NULL) {
                                free(buffer);
-                               command_print(CMD_CTX, "error allocating buffer for section (%d bytes)",
+                               command_print(CMD_CTX, "error allocating buffer for section (%" PRIu32 " bytes)",
                                                          length);
                                retval = ERROR_FAIL;
                                break;
@@ -5482,6 +5649,198 @@ COMMAND_HANDLER(handle_ps_command)
        }
 }
 
+static void binprint(struct command_context *cmd_ctx, const char *text, const uint8_t *buf, int size)
+{
+       if (text != NULL)
+               command_print_sameline(cmd_ctx, "%s", text);
+       for (int i = 0; i < size; i++)
+               command_print_sameline(cmd_ctx, " %02x", buf[i]);
+       command_print(cmd_ctx, " ");
+}
+
+COMMAND_HANDLER(handle_test_mem_access_command)
+{
+       struct target *target = get_current_target(CMD_CTX);
+       uint32_t test_size;
+       int retval = ERROR_OK;
+
+       if (target->state != TARGET_HALTED) {
+               LOG_INFO("target not halted !!");
+               return ERROR_FAIL;
+       }
+
+       if (CMD_ARGC != 1)
+               return ERROR_COMMAND_SYNTAX_ERROR;
+
+       COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], test_size);
+
+       /* Test reads */
+       size_t num_bytes = test_size + 4;
+
+       struct working_area *wa = NULL;
+       retval = target_alloc_working_area(target, num_bytes, &wa);
+       if (retval != ERROR_OK) {
+               LOG_ERROR("Not enough working area");
+               return ERROR_FAIL;
+       }
+
+       uint8_t *test_pattern = malloc(num_bytes);
+
+       for (size_t i = 0; i < num_bytes; i++)
+               test_pattern[i] = rand();
+
+       retval = target_write_memory(target, wa->address, 1, num_bytes, test_pattern);
+       if (retval != ERROR_OK) {
+               LOG_ERROR("Test pattern write failed");
+               goto out;
+       }
+
+       for (int host_offset = 0; host_offset <= 1; host_offset++) {
+               for (int size = 1; size <= 4; size *= 2) {
+                       for (int offset = 0; offset < 4; offset++) {
+                               uint32_t count = test_size / size;
+                               size_t host_bufsiz = (count + 2) * size + host_offset;
+                               uint8_t *read_ref = malloc(host_bufsiz);
+                               uint8_t *read_buf = malloc(host_bufsiz);
+
+                               for (size_t i = 0; i < host_bufsiz; i++) {
+                                       read_ref[i] = rand();
+                                       read_buf[i] = read_ref[i];
+                               }
+                               command_print_sameline(CMD_CTX,
+                                               "Test read %d x %d @ %d to %saligned buffer: ", count,
+                                               size, offset, host_offset ? "un" : "");
+
+                               struct duration bench;
+                               duration_start(&bench);
+
+                               retval = target_read_memory(target, wa->address + offset, size, count,
+                                               read_buf + size + host_offset);
+
+                               duration_measure(&bench);
+
+                               if (retval == ERROR_TARGET_UNALIGNED_ACCESS) {
+                                       command_print(CMD_CTX, "Unsupported alignment");
+                                       goto next;
+                               } else if (retval != ERROR_OK) {
+                                       command_print(CMD_CTX, "Memory read failed");
+                                       goto next;
+                               }
+
+                               /* replay on host */
+                               memcpy(read_ref + size + host_offset, test_pattern + offset, count * size);
+
+                               /* check result */
+                               int result = memcmp(read_ref, read_buf, host_bufsiz);
+                               if (result == 0) {
+                                       command_print(CMD_CTX, "Pass in %fs (%0.3f KiB/s)",
+                                                       duration_elapsed(&bench),
+                                                       duration_kbps(&bench, count * size));
+                               } else {
+                                       command_print(CMD_CTX, "Compare failed");
+                                       binprint(CMD_CTX, "ref:", read_ref, host_bufsiz);
+                                       binprint(CMD_CTX, "buf:", read_buf, host_bufsiz);
+                               }
+next:
+                               free(read_ref);
+                               free(read_buf);
+                       }
+               }
+       }
+
+out:
+       free(test_pattern);
+
+       if (wa != NULL)
+               target_free_working_area(target, wa);
+
+       /* Test writes */
+       num_bytes = test_size + 4 + 4 + 4;
+
+       retval = target_alloc_working_area(target, num_bytes, &wa);
+       if (retval != ERROR_OK) {
+               LOG_ERROR("Not enough working area");
+               return ERROR_FAIL;
+       }
+
+       test_pattern = malloc(num_bytes);
+
+       for (size_t i = 0; i < num_bytes; i++)
+               test_pattern[i] = rand();
+
+       for (int host_offset = 0; host_offset <= 1; host_offset++) {
+               for (int size = 1; size <= 4; size *= 2) {
+                       for (int offset = 0; offset < 4; offset++) {
+                               uint32_t count = test_size / size;
+                               size_t host_bufsiz = count * size + host_offset;
+                               uint8_t *read_ref = malloc(num_bytes);
+                               uint8_t *read_buf = malloc(num_bytes);
+                               uint8_t *write_buf = malloc(host_bufsiz);
+
+                               for (size_t i = 0; i < host_bufsiz; i++)
+                                       write_buf[i] = rand();
+                               command_print_sameline(CMD_CTX,
+                                               "Test write %d x %d @ %d from %saligned buffer: ", count,
+                                               size, offset, host_offset ? "un" : "");
+
+                               retval = target_write_memory(target, wa->address, 1, num_bytes, test_pattern);
+                               if (retval != ERROR_OK) {
+                                       command_print(CMD_CTX, "Test pattern write failed");
+                                       goto nextw;
+                               }
+
+                               /* replay on host */
+                               memcpy(read_ref, test_pattern, num_bytes);
+                               memcpy(read_ref + size + offset, write_buf + host_offset, count * size);
+
+                               struct duration bench;
+                               duration_start(&bench);
+
+                               retval = target_write_memory(target, wa->address + size + offset, size, count,
+                                               write_buf + host_offset);
+
+                               duration_measure(&bench);
+
+                               if (retval == ERROR_TARGET_UNALIGNED_ACCESS) {
+                                       command_print(CMD_CTX, "Unsupported alignment");
+                                       goto nextw;
+                               } else if (retval != ERROR_OK) {
+                                       command_print(CMD_CTX, "Memory write failed");
+                                       goto nextw;
+                               }
+
+                               /* read back */
+                               retval = target_read_memory(target, wa->address, 1, num_bytes, read_buf);
+                               if (retval != ERROR_OK) {
+                                       command_print(CMD_CTX, "Test pattern write failed");
+                                       goto nextw;
+                               }
+
+                               /* check result */
+                               int result = memcmp(read_ref, read_buf, num_bytes);
+                               if (result == 0) {
+                                       command_print(CMD_CTX, "Pass in %fs (%0.3f KiB/s)",
+                                                       duration_elapsed(&bench),
+                                                       duration_kbps(&bench, count * size));
+                               } else {
+                                       command_print(CMD_CTX, "Compare failed");
+                                       binprint(CMD_CTX, "ref:", read_ref, num_bytes);
+                                       binprint(CMD_CTX, "buf:", read_buf, num_bytes);
+                               }
+nextw:
+                               free(read_ref);
+                               free(read_buf);
+                       }
+               }
+       }
+
+       free(test_pattern);
+
+       if (wa != NULL)
+               target_free_working_area(target, wa);
+       return retval;
+}
+
 static const struct command_registration target_exec_command_handlers[] = {
        {
                .name = "fast_load_image",
@@ -5504,7 +5863,7 @@ static const struct command_registration target_exec_command_handlers[] = {
                .name = "profile",
                .handler = handle_profile_command,
                .mode = COMMAND_EXEC,
-               .usage = "seconds filename",
+               .usage = "seconds filename [start end]",
                .help = "profiling samples the CPU PC",
        },
        /** @todo don't register virt2phys() unless target supports it */
@@ -5519,9 +5878,9 @@ static const struct command_registration target_exec_command_handlers[] = {
                .name = "reg",
                .handler = handle_reg_command,
                .mode = COMMAND_EXEC,
-               .help = "display or set a register; with no arguments, "
-                       "displays all registers and their values",
-               .usage = "[(register_name|register_number) [value]]",
+               .help = "display (reread from target with \"force\") or set a register; "
+                       "with no arguments, displays all registers and their values",
+               .usage = "[(register_number|register_name) [(value|'force')]]",
        },
        {
                .name = "poll",
@@ -5535,7 +5894,7 @@ static const struct command_registration target_exec_command_handlers[] = {
                .handler = handle_wait_halt_command,
                .mode = COMMAND_EXEC,
                .help = "wait up to the specified number of milliseconds "
-                       "(default 5) for a previously requested halt",
+                       "(default 5000) for a previously requested halt",
                .usage = "[milliseconds]",
        },
        {
@@ -5543,7 +5902,7 @@ static const struct command_registration target_exec_command_handlers[] = {
                .handler = handle_halt_command,
                .mode = COMMAND_EXEC,
                .help = "request target to halt, then wait up to the specified"
-                       "number of milliseconds (default 5) for it to complete",
+                       "number of milliseconds (default 5000) for it to complete",
                .usage = "[milliseconds]",
        },
        {
@@ -5701,6 +6060,13 @@ static const struct command_registration target_exec_command_handlers[] = {
                .help = "list all tasks ",
                .usage = " ",
        },
+       {
+               .name = "test_mem_access",
+               .handler = handle_test_mem_access_command,
+               .mode = COMMAND_EXEC,
+               .help = "Test the target's memory access functions",
+               .usage = "size",
+       },
 
        COMMAND_REGISTRATION_DONE
 };