#include "arm720t.h"
 #include "jtag.h"
 #include "log.h"
+#include "time_support.h"
 
 #include <stdlib.h>
 #include <string.h>
        arm7_9_common_t *arm7_9 = armv4_5->arch_info;
        arm7tdmi_common_t *arm7tdmi = arm7_9->arch_info;
        arm720t_common_t *arm720t = arm7tdmi->arch_info;
-       int i;
        reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
        
        target_halt(target);
        
-       for (i=0; i<10; i++)
+       long long then=timeval_ms();
+       int timeout;
+       while (!(timeout=((timeval_ms()-then)>1000)))
        {
                if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) == 0)
                {
                {
                        break;
                }
-               /* do not eat all CPU, time out after 1 se*/
-               alive_sleep(100);
+               if (debug_level>=3)
+               {
+                       alive_sleep(100);
+               } else
+               {
+                       keep_alive();
+               }
        }
-       if (i==10)
+       if (timeout)
        {
                LOG_ERROR("Failed to halt CPU after 1 sec");
                return ERROR_TARGET_TIMEOUT;
 
 #include "log.h"
 #include "arm7_9_common.h"
 #include "breakpoints.h"
+#include "time_support.h"
 
 #include <stdlib.h>
 #include <string.h>
 
 int arm7_9_execute_sys_speed(struct target_s *target)
 {
-       int timeout;
        int retval;
 
        armv4_5_common_t *armv4_5 = target->arch_info;
        }
        arm_jtag_set_instr(jtag_info, 0x4, NULL);
 
-       for (timeout=0; timeout<50; timeout++)
+       long long then=timeval_ms();
+       int timeout;
+       while (!(timeout=((timeval_ms()-then)>1000)))
        {
                /* read debug status register */
                embeddedice_read_reg(dbg_stat);
                if ((buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1))
                                   && (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_SYSCOMP, 1)))
                        break;
-               alive_sleep(100);
+               if (debug_level>=3)
+               {
+                       alive_sleep(100);
+               } else
+               {
+                       keep_alive();
+               }
        }
-       if (timeout == 50)
+       if (timeout)
        {
                LOG_ERROR("timeout waiting for SYSCOMP & DBGACK, last DBG_STATUS: %x", buf_get_u32(dbg_stat->value, 0, dbg_stat->size));
                return ERROR_TARGET_TIMEOUT;
        if ((retval=target_halt(target))!=ERROR_OK)
                return retval;
 
-       for (i=0; i<10; i++)
+       long long then=timeval_ms();
+       int timeout;
+       while (!(timeout=((timeval_ms()-then)>1000)))
        {
                if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) != 0)
                        break;
                embeddedice_read_reg(dbg_stat);
                if ((retval=jtag_execute_queue())!=ERROR_OK)
                        return retval;
-               /* do not eat all CPU, time out after 1 se*/
-               usleep(100*1000);
-
+               if (debug_level>=3)
+               {
+                       alive_sleep(100);
+               } else
+               {
+                       keep_alive();
+               }
        }
-       if (i==10)
+       if (timeout)
        {
                LOG_ERROR("Failed to halt CPU after 1 sec");
                return ERROR_TARGET_TIMEOUT;
 
        target_halt(target);
 
-       for (i=0; i<100; i++)
+       long long then=timeval_ms();
+       int timeout;
+       while (!(timeout=((timeval_ms()-then)>100)))
        {
                target_poll(target);
                if (target->state == TARGET_HALTED)
                        break;
-               usleep(1000); /* sleep 1ms */
+               if (debug_level>=3)
+               {
+                       alive_sleep(100);
+               } else
+               {
+                       keep_alive();
+               }
        }
-       if (i == 100)
+       if (timeout)
        {
                LOG_ERROR("bulk write timed out, target not halted");
                return ERROR_TARGET_TIMEOUT;
 
 #include "arm920t.h"
 #include "jtag.h"
 #include "log.h"
+#include "time_support.h"
 
 #include <stdlib.h>
 #include <string.h>
        arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
        arm920t_common_t *arm920t = arm9tdmi->arch_info;
        reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
-       int i;
        
        target_halt(target);
        
-       for (i=0; i<10; i++)
+       long long then=timeval_ms();
+       int timeout;
+       while (!(timeout=((timeval_ms()-then)>1000)))
        {
                if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) == 0)
                {
                {
                        break;
                }
-               /* do not eat all CPU, time out after 1 se*/
-               alive_sleep(100);
+               if (debug_level>=3)
+               {
+                       /* do not eat all CPU, time out after 1 se*/
+                       alive_sleep(100);
+               } else
+               {
+                       keep_alive();
+               }
        }
-       if (i==10)
+       if (timeout)
        {
                LOG_ERROR("Failed to halt CPU after 1 sec");
                return ERROR_TARGET_TIMEOUT;
 
 #include "arm926ejs.h"
 #include "jtag.h"
 #include "log.h"
+#include "time_support.h"
 
 #include <stdlib.h>
 #include <string.h>
        arm9tdmi_common_t *arm9tdmi = arm7_9->arch_info;
        arm926ejs_common_t *arm926ejs = arm9tdmi->arch_info;
        reg_t *dbg_stat = &arm7_9->eice_cache->reg_list[EICE_DBG_STAT];
-       int i;
        
        target_halt(target);
        
-       for (i=0; i<10; i++)
+       long long then=timeval_ms();
+       int timeout;
+       while (!(timeout=((timeval_ms()-then)>1000)))
        {
                if (buf_get_u32(dbg_stat->value, EICE_DBG_STATUS_DBGACK, 1) == 0)
                {
                {
                        break;
                }
-               /* do not eat all CPU, time out after 1 se*/
-               alive_sleep(100);
+               if (debug_level>=1)
+               {
+                       /* do not eat all CPU, time out after 1 se*/
+                       alive_sleep(100);
+               } else
+               {
+                       keep_alive();
+               }
        }
-       if (i==10)
+       if (timeout)
        {
                LOG_ERROR("Failed to halt CPU after 1 sec");
                return ERROR_TARGET_TIMEOUT;
 
        }
        
        target_resume(target, 0, entry_point, 1, 1);
-       target_poll(target);
        
-       while (target->state != TARGET_HALTED)
+       target_wait_state(target, TARGET_HALTED, timeout_ms);
+       if (target->state != TARGET_HALTED)
        {
-               alive_sleep(10);
-               target_poll(target);
-               if ((timeout_ms -= 10) <= 0)
+               if ((retval=target_halt(target))!=ERROR_OK)
+                       return retval;
+               if ((retval=target_wait_state(target, TARGET_HALTED, 500))!=ERROR_OK)
                {
-                       LOG_ERROR("timeout waiting for algorithm to complete, trying to halt target");
-                       target_halt(target);
-                       timeout_ms = 1000;
-                       while (target->state != TARGET_HALTED)
-                       {
-                               alive_sleep(10);
-                               target_poll(target);
-                               if ((timeout_ms -= 10) <= 0)
-                               {
-                                       LOG_ERROR("target didn't reenter debug state, exiting");
-                                       exit(-1);
-                               }
-                       }
-                       retval = ERROR_TARGET_TIMEOUT;
+                       return retval;
                }
+               return ERROR_TARGET_TIMEOUT;
        }
        
-       if ((retval != ERROR_TARGET_TIMEOUT) && 
-               (buf_get_u32(armv4_5->core_cache->reg_list[15].value, 0, 32) != exit_point))
+       if (buf_get_u32(armv4_5->core_cache->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(armv4_5->core_cache->reg_list[15].value, 0, 32)); 
+               return ERROR_TARGET_TIMEOUT;
        }
        
        breakpoint_remove(target, exit_point);
 
        target_resume(target, 0, entry_point, 1, 1);
        target_poll(target);
        
-       while (target->state != TARGET_HALTED)
+       target_wait_state(target, TARGET_HALTED, timeout_ms);
+       if (target->state != TARGET_HALTED)
        {
-               usleep(5000);
-               target_poll(target);
-               if ((timeout_ms -= 5) <= 0)
+               if ((retval=target_halt(target))!=ERROR_OK)
+                       return retval;
+               if ((retval=target_wait_state(target, TARGET_HALTED, 500))!=ERROR_OK)
                {
-                       LOG_ERROR("timeout waiting for algorithm to complete, trying to halt target");
-                       target_halt(target);
-                       timeout_ms = 1000;
-                       while (target->state != TARGET_HALTED)
-                       {
-                               usleep(10000);
-                               target_poll(target);
-                               if ((timeout_ms -= 10) <= 0)
-                               {
-                                       LOG_ERROR("target didn't reenter debug state, exiting");
-                                       exit(-1);
-                               }
-                       }
-                       armv7m->load_core_reg_u32(target, ARMV7M_REGISTER_CORE_GP, 15, &pc);
-                       LOG_DEBUG("failed algoritm halted at 0x%x ", pc); 
-                       retval = ERROR_TARGET_TIMEOUT;
+                       return retval;
                }
+               return ERROR_TARGET_TIMEOUT;
+       }
+       
+       
+       armv7m->load_core_reg_u32(target, ARMV7M_REGISTER_CORE_GP, 15, &pc);
+       if (pc != exit_point)
+       {
+               LOG_DEBUG("failed algoritm halted at 0x%x ", pc); 
+               return ERROR_TARGET_TIMEOUT;
        }
        
        breakpoint_remove(target, exit_point);
 
        {
                if ((retval=target_poll(target))!=ERROR_OK)
                        return retval;
-               target_call_timer_callbacks_now();
+               keep_alive();
                if (target->state == state)
                {
                        break;