+ if (target->state != TARGET_HALTED)
+ {
+ LOG_WARNING("target not halted");
+ return ERROR_TARGET_NOT_HALTED;
+ }
+
+ // FIXME
+// if (armv4_5_mode_to_number(arm11->core_mode)==-1)
+// return ERROR_FAIL;
+
+ // Save regs
+ for (i = 0; i < 16; i++)
+ {
+ context[i] = buf_get_u32((u8*)(&arm11->reg_values[i]),0,32);
+ LOG_DEBUG("Save %i: 0x%x",i,context[i]);
+ }
+
+ cpsr = buf_get_u32((u8*)(arm11->reg_values+ARM11_RC_CPSR),0,32);
+ LOG_DEBUG("Save CPSR: 0x%x", cpsr);
+
+ for (i = 0; i < num_mem_params; i++)
+ {
+ target_write_buffer(target, mem_params[i].address, mem_params[i].size, mem_params[i].value);
+ }
+
+ // Set register parameters
+ for (i = 0; i < num_reg_params; i++)
+ {
+ reg_t *reg = register_get_by_name(arm11->core_cache, reg_params[i].reg_name, 0);
+ if (!reg)
+ {
+ LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
+ exit(-1);
+ }
+
+ if (reg->size != reg_params[i].size)
+ {
+ LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size", reg_params[i].reg_name);
+ exit(-1);
+ }
+ arm11_set_reg(reg,reg_params[i].value);
+// printf("%i: Set %s =%08x\n", i, reg_params[i].reg_name,val);
+ }
+
+ exit_breakpoint_size = 4;
+
+/* arm11->core_state = arm11_algorithm_info->core_state;
+ if (arm11->core_state == ARMV4_5_STATE_ARM)
+ exit_breakpoint_size = 4;
+ else if (arm11->core_state == ARMV4_5_STATE_THUMB)
+ exit_breakpoint_size = 2;
+ else
+ {
+ LOG_ERROR("BUG: can't execute algorithms when not in ARM or Thumb state");
+ exit(-1);
+ }
+*/
+ if (arm11_algorithm_info->core_mode != ARMV4_5_MODE_ANY)
+ {
+ LOG_DEBUG("setting core_mode: 0x%2.2x", arm11_algorithm_info->core_mode);
+ buf_set_u32(arm11->reg_list[ARM11_RC_CPSR].value, 0, 5, arm11_algorithm_info->core_mode);
+ arm11->reg_list[ARM11_RC_CPSR].dirty = 1;
+ arm11->reg_list[ARM11_RC_CPSR].valid = 1;
+ }
+
+ if ((retval = breakpoint_add(target, exit_point, exit_breakpoint_size, BKPT_HARD)) != ERROR_OK)
+ {
+ LOG_ERROR("can't add breakpoint to finish algorithm execution");
+ retval = ERROR_TARGET_FAILURE;
+ goto restore;
+ }
+
+ target_resume(target, 0, entry_point, 1, 0); // no debug, otherwise breakpoint is not set
+
+ target_wait_state(target, TARGET_HALTED, timeout_ms);
+ if (target->state != TARGET_HALTED)
+ {
+ if ((retval=target_halt(target))!=ERROR_OK)
+ return retval;
+ if ((retval=target_wait_state(target, TARGET_HALTED, 500))!=ERROR_OK)
+ {
+ return retval;
+ }
+ retval = ERROR_TARGET_TIMEOUT;
+ goto del_breakpoint;
+ }
+
+ if (buf_get_u32(arm11->reg_list[15].value, 0, 32) != exit_point)
+ {
+ LOG_WARNING("target reentered debug state, but not at the desired exit point: 0x%4.4x",
+ buf_get_u32(arm11->reg_list[15].value, 0, 32));
+ retval = ERROR_TARGET_TIMEOUT;
+ goto del_breakpoint;
+ }
+
+ for (i = 0; i < num_mem_params; i++)
+ {
+ if (mem_params[i].direction != PARAM_OUT)
+ target_read_buffer(target, mem_params[i].address, mem_params[i].size, mem_params[i].value);
+ }
+
+ for (i = 0; i < num_reg_params; i++)
+ {
+ if (reg_params[i].direction != PARAM_OUT)
+ {
+ reg_t *reg = register_get_by_name(arm11->core_cache, reg_params[i].reg_name, 0);
+ if (!reg)
+ {
+ LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
+ exit(-1);
+ }
+
+ if (reg->size != reg_params[i].size)
+ {
+ LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size", reg_params[i].reg_name);
+ exit(-1);
+ }
+
+ buf_set_u32(reg_params[i].value, 0, 32, buf_get_u32(reg->value, 0, 32));
+ }
+ }
+
+del_breakpoint:
+ breakpoint_remove(target, exit_point);
+
+restore:
+ // Restore context
+ for (i = 0; i < 16; i++)
+ {
+ LOG_DEBUG("restoring register %s with value 0x%8.8x",
+ arm11->reg_list[i].name, context[i]);
+ arm11_set_reg(&arm11->reg_list[i], (u8*)&context[i]);
+ }
+ LOG_DEBUG("restoring CPSR with value 0x%8.8x", cpsr);
+ arm11_set_reg(&arm11->reg_list[ARM11_RC_CPSR], (u8*)&cpsr);
+
+// arm11->core_state = core_state;
+// arm11->core_mode = core_mode;
+
+ return retval;