arm: add error propagation for enable/disable mmu caches

Signed-off-by: Øyvind Harboe <oyvind.harboe@zylin.com>

diff --git a/src/target/arm720t.c b/src/target/arm720t.c
index b269f9488eda5acd43ed8bb652df6eea3f6f1024..6bf38bbc76bdabce06729a8028db49328bc547cd 100644 (file)
--- a/src/target/arm720t.c
+++ b/src/target/arm720t.c
@@ -154,14 +154,19 @@ static int arm720t_get_ttb(struct target *target, uint32_t *result)
        return ERROR_OK;
 }
 
-static void arm720t_disable_mmu_caches(struct target *target,
+static int arm720t_disable_mmu_caches(struct target *target,
                int mmu, int d_u_cache, int i_cache)
 {
        uint32_t cp15_control;
+       int retval;
 
        /* read cp15 control register */
-       arm720t_read_cp15(target, 0xee110f10, &cp15_control);
-       jtag_execute_queue();
+       retval = arm720t_read_cp15(target, 0xee110f10, &cp15_control);
+       if (retval != ERROR_OK)
+               return retval;
+       retval = jtag_execute_queue();
+       if (retval != ERROR_OK)
+               return retval;
 
        if (mmu)
                cp15_control &= ~0x1U;
@@ -169,17 +174,23 @@ static void arm720t_disable_mmu_caches(struct target *target,
        if (d_u_cache || i_cache)
                cp15_control &= ~0x4U;
 
-       arm720t_write_cp15(target, 0xee010f10, cp15_control);
+       retval = arm720t_write_cp15(target, 0xee010f10, cp15_control);
+       return retval;
 }
 
-static void arm720t_enable_mmu_caches(struct target *target,
+static int arm720t_enable_mmu_caches(struct target *target,
                int mmu, int d_u_cache, int i_cache)
 {
        uint32_t cp15_control;
+       int retval;
 
        /* read cp15 control register */
-       arm720t_read_cp15(target, 0xee110f10, &cp15_control);
-       jtag_execute_queue();
+       retval = arm720t_read_cp15(target, 0xee110f10, &cp15_control);
+       if (retval != ERROR_OK)
+               return retval;
+       retval = jtag_execute_queue();
+       if (retval != ERROR_OK)
+               return retval;
 
        if (mmu)
                cp15_control |= 0x1U;
@@ -187,7 +198,8 @@ static void arm720t_enable_mmu_caches(struct target *target,
        if (d_u_cache || i_cache)
                cp15_control |= 0x4U;
 
-       arm720t_write_cp15(target, 0xee010f10, cp15_control);
+       retval = arm720t_write_cp15(target, 0xee010f10, cp15_control);
+       return retval;
 }
 
 static void arm720t_post_debug_entry(struct target *target)
@@ -282,12 +294,19 @@ static int arm720t_read_memory(struct target *target,
 
        /* disable cache, but leave MMU enabled */
        if (arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled)
-               arm720t_disable_mmu_caches(target, 0, 1, 0);
-
+       {
+               retval = arm720t_disable_mmu_caches(target, 0, 1, 0);
+               if (retval != ERROR_OK)
+                       return retval;
+       }
        retval = arm7_9_read_memory(target, address, size, count, buffer);
 
        if (arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled)
-               arm720t_enable_mmu_caches(target, 0, 1, 0);
+       {
+               retval = arm720t_enable_mmu_caches(target, 0, 1, 0);
+               if (retval != ERROR_OK)
+                       return retval;
+       }
 
        return retval;
 }
@@ -367,7 +386,9 @@ static int arm720t_soft_reset_halt(struct target *target)
        armv4_5->pc->dirty = 1;
        armv4_5->pc->valid = 1;
 
-       arm720t_disable_mmu_caches(target, 1, 1, 1);
+       retval = arm720t_disable_mmu_caches(target, 1, 1, 1);
+       if (retval != ERROR_OK)
+               return retval;
        arm720t->armv4_5_mmu.mmu_enabled = 0;
        arm720t->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 0;
        arm720t->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;

diff --git a/src/target/arm920t.c b/src/target/arm920t.c
index 9c11d124f45970930febaa599caaf295142f5ff4..a80a37810dba79e08b89f9f1e677887a1ba1a070 100644 (file)
--- a/src/target/arm920t.c
+++ b/src/target/arm920t.c
@@ -333,14 +333,19 @@ int arm920t_get_ttb(struct target *target, uint32_t *result)
 }
 
 // EXPORTED to FA256
-void arm920t_disable_mmu_caches(struct target *target, int mmu,
+int arm920t_disable_mmu_caches(struct target *target, int mmu,
                int d_u_cache, int i_cache)
 {
        uint32_t cp15_control;
+       int retval;
 
        /* read cp15 control register */
-       arm920t_read_cp15_physical(target, CP15PHYS_CTRL, &cp15_control);
-       jtag_execute_queue();
+       retval = arm920t_read_cp15_physical(target, CP15PHYS_CTRL, &cp15_control);
+       if (retval != ERROR_OK)
+               return retval;
+       retval = jtag_execute_queue();
+       if (retval != ERROR_OK)
+               return retval;
 
        if (mmu)
                cp15_control &= ~0x1U;
@@ -351,18 +356,24 @@ void arm920t_disable_mmu_caches(struct target *target, int mmu,
        if (i_cache)
                cp15_control &= ~0x1000U;
 
-       arm920t_write_cp15_physical(target, CP15PHYS_CTRL, cp15_control);
+       retval = arm920t_write_cp15_physical(target, CP15PHYS_CTRL, cp15_control);
+       return retval;
 }
 
 // EXPORTED to FA256
-void arm920t_enable_mmu_caches(struct target *target, int mmu,
+int arm920t_enable_mmu_caches(struct target *target, int mmu,
                int d_u_cache, int i_cache)
 {
        uint32_t cp15_control;
+       int retval;
 
        /* read cp15 control register */
-       arm920t_read_cp15_physical(target, CP15PHYS_CTRL, &cp15_control);
-       jtag_execute_queue();
+       retval = arm920t_read_cp15_physical(target, CP15PHYS_CTRL, &cp15_control);
+       if (retval != ERROR_OK)
+               return retval;
+       retval = jtag_execute_queue();
+       if (retval != ERROR_OK)
+               return retval;
 
        if (mmu)
                cp15_control |= 0x1U;
@@ -373,7 +384,8 @@ void arm920t_enable_mmu_caches(struct target *target, int mmu,
        if (i_cache)
                cp15_control |= 0x1000U;
 
-       arm920t_write_cp15_physical(target, CP15PHYS_CTRL, cp15_control);
+       retval = arm920t_write_cp15_physical(target, CP15PHYS_CTRL, cp15_control);
+       return retval;
 }
 
 // EXPORTED to FA256

diff --git a/src/target/arm920t.h b/src/target/arm920t.h
index 9d5afab8fd819202aa2d829cb42cdb8906caf4d4..20f614d7dad324129b31bdfbb082fa1784b957a3 100644 (file)
--- a/src/target/arm920t.h
+++ b/src/target/arm920t.h
@@ -67,9 +67,9 @@ int arm920t_write_memory(struct target *target,
 void arm920t_post_debug_entry(struct target *target);
 void arm920t_pre_restore_context(struct target *target);
 int arm920t_get_ttb(struct target *target, uint32_t *result);
-void arm920t_disable_mmu_caches(struct target *target,
+int arm920t_disable_mmu_caches(struct target *target,
        int mmu, int d_u_cache, int i_cache);
-void arm920t_enable_mmu_caches(struct target *target,
+int arm920t_enable_mmu_caches(struct target *target,
        int mmu, int d_u_cache, int i_cache);
 
 extern const struct command_registration arm920t_command_handlers[];

diff --git a/src/target/arm926ejs.c b/src/target/arm926ejs.c
index 0cf7173564b6bf6203cb8877a94af7b0e573963e..f8a4f6294cfab20e91d5d38ad418f29ccf9eae31 100644 (file)
--- a/src/target/arm926ejs.c
+++ b/src/target/arm926ejs.c
@@ -337,20 +337,27 @@ static int arm926ejs_get_ttb(struct target *target, uint32_t *result)
        return ERROR_OK;
 }
 
-static void arm926ejs_disable_mmu_caches(struct target *target, int mmu,
+static int arm926ejs_disable_mmu_caches(struct target *target, int mmu,
                int d_u_cache, int i_cache)
 {
        struct arm926ejs_common *arm926ejs = target_to_arm926(target);
        uint32_t cp15_control;
+       int retval;
 
        /* read cp15 control register */
-       arm926ejs->read_cp15(target, 0, 0, 1, 0, &cp15_control);
-       jtag_execute_queue();
+       retval = arm926ejs->read_cp15(target, 0, 0, 1, 0, &cp15_control);
+       if (retval != ERROR_OK)
+               return retval;
+       retval = jtag_execute_queue();
+       if (retval != ERROR_OK)
+               return retval;
 
        if (mmu)
        {
                /* invalidate TLB */
-               arm926ejs->write_cp15(target, 0, 0, 8, 7, 0x0);
+               retval = arm926ejs->write_cp15(target, 0, 0, 8, 7, 0x0);
+               if (retval != ERROR_OK)
+                       return retval;
 
                cp15_control &= ~0x1U;
        }
@@ -360,17 +367,25 @@ static void arm926ejs_disable_mmu_caches(struct target *target, int mmu,
                uint32_t debug_override;
                /* read-modify-write CP15 debug override register
                 * to enable "test and clean all" */
-               arm926ejs->read_cp15(target, 0, 0, 15, 0, &debug_override);
+               retval = arm926ejs->read_cp15(target, 0, 0, 15, 0, &debug_override);
+               if (retval != ERROR_OK)
+                       return retval;
                debug_override |= 0x80000;
-               arm926ejs->write_cp15(target, 0, 0, 15, 0, debug_override);
+               retval = arm926ejs->write_cp15(target, 0, 0, 15, 0, debug_override);
+               if (retval != ERROR_OK)
+                       return retval;
 
                /* clean and invalidate DCache */
-               arm926ejs->write_cp15(target, 0, 0, 7, 5, 0x0);
+               retval = arm926ejs->write_cp15(target, 0, 0, 7, 5, 0x0);
+               if (retval != ERROR_OK)
+                       return retval;
 
                /* write CP15 debug override register
                 * to disable "test and clean all" */
                debug_override &= ~0x80000;
-               arm926ejs->write_cp15(target, 0, 0, 15, 0, debug_override);
+               retval = arm926ejs->write_cp15(target, 0, 0, 15, 0, debug_override);
+               if (retval != ERROR_OK)
+                       return retval;
 
                cp15_control &= ~0x4U;
        }
@@ -378,23 +393,31 @@ static void arm926ejs_disable_mmu_caches(struct target *target, int mmu,
        if (i_cache)
        {
                /* invalidate ICache */
-               arm926ejs->write_cp15(target, 0, 0, 7, 5, 0x0);
+               retval = arm926ejs->write_cp15(target, 0, 0, 7, 5, 0x0);
+               if (retval != ERROR_OK)
+                       return retval;
 
                cp15_control &= ~0x1000U;
        }
 
-       arm926ejs->write_cp15(target, 0, 0, 1, 0, cp15_control);
+       retval = arm926ejs->write_cp15(target, 0, 0, 1, 0, cp15_control);
+       return retval;
 }
 
-static void arm926ejs_enable_mmu_caches(struct target *target, int mmu,
+static int arm926ejs_enable_mmu_caches(struct target *target, int mmu,
                int d_u_cache, int i_cache)
 {
        struct arm926ejs_common *arm926ejs = target_to_arm926(target);
        uint32_t cp15_control;
+       int retval;
 
        /* read cp15 control register */
-       arm926ejs->read_cp15(target, 0, 0, 1, 0, &cp15_control);
-       jtag_execute_queue();
+       retval = arm926ejs->read_cp15(target, 0, 0, 1, 0, &cp15_control);
+       if (retval != ERROR_OK)
+               return retval;
+       retval = jtag_execute_queue();
+       if (retval != ERROR_OK)
+               return retval;
 
        if (mmu)
                cp15_control |= 0x1U;
@@ -405,7 +428,8 @@ static void arm926ejs_enable_mmu_caches(struct target *target, int mmu,
        if (i_cache)
                cp15_control |= 0x1000U;
 
-       arm926ejs->write_cp15(target, 0, 0, 1, 0, cp15_control);
+       retval = arm926ejs->write_cp15(target, 0, 0, 1, 0, cp15_control);
+       return retval;
 }
 
 static void arm926ejs_post_debug_entry(struct target *target)
@@ -564,7 +588,9 @@ int arm926ejs_soft_reset_halt(struct target *target)
        armv4_5->pc->dirty = 1;
        armv4_5->pc->valid = 1;
 
-       arm926ejs_disable_mmu_caches(target, 1, 1, 1);
+       retval = arm926ejs_disable_mmu_caches(target, 1, 1, 1);
+       if (retval != ERROR_OK)
+               return retval;
        arm926ejs->armv4_5_mmu.mmu_enabled = 0;
        arm926ejs->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled = 0;
        arm926ejs->armv4_5_mmu.armv4_5_cache.i_cache_enabled = 0;

diff --git a/src/target/armv4_5_mmu.c b/src/target/armv4_5_mmu.c
index 3d450ae132b468a504bc53dba6f9b62c89b2e994..8978f354dafa9c5b01ed7ab31e4b7fb4021850c4 100644 (file)
--- a/src/target/armv4_5_mmu.c
+++ b/src/target/armv4_5_mmu.c
@@ -131,14 +131,20 @@ int armv4_5_mmu_read_physical(struct target *target, struct armv4_5_mmu_common *
                return ERROR_TARGET_NOT_HALTED;
 
        /* disable MMU and data (or unified) cache */
-       armv4_5_mmu->disable_mmu_caches(target, 1, 1, 0);
+       retval = armv4_5_mmu->disable_mmu_caches(target, 1, 1, 0);
+       if (retval !=ERROR_OK)
+               return retval;
 
        retval = armv4_5_mmu->read_memory(target, address, size, count, buffer);
+       if (retval !=ERROR_OK)
+               return retval;
 
        /* reenable MMU / cache */
-       armv4_5_mmu->enable_mmu_caches(target, armv4_5_mmu->mmu_enabled,
+       retval = armv4_5_mmu->enable_mmu_caches(target, armv4_5_mmu->mmu_enabled,
                armv4_5_mmu->armv4_5_cache.d_u_cache_enabled,
                armv4_5_mmu->armv4_5_cache.i_cache_enabled);
+       if (retval !=ERROR_OK)
+               return retval;
 
        return retval;
 }
@@ -151,14 +157,20 @@ int armv4_5_mmu_write_physical(struct target *target, struct armv4_5_mmu_common
                return ERROR_TARGET_NOT_HALTED;
 
        /* disable MMU and data (or unified) cache */
-       armv4_5_mmu->disable_mmu_caches(target, 1, 1, 0);
+       retval = armv4_5_mmu->disable_mmu_caches(target, 1, 1, 0);
+       if (retval !=ERROR_OK)
+               return retval;
 
        retval = armv4_5_mmu->write_memory(target, address, size, count, buffer);
+       if (retval !=ERROR_OK)
+               return retval;
 
        /* reenable MMU / cache */
-       armv4_5_mmu->enable_mmu_caches(target, armv4_5_mmu->mmu_enabled,
+       retval = armv4_5_mmu->enable_mmu_caches(target, armv4_5_mmu->mmu_enabled,
                armv4_5_mmu->armv4_5_cache.d_u_cache_enabled,
                armv4_5_mmu->armv4_5_cache.i_cache_enabled);
+       if (retval !=ERROR_OK)
+               return retval;
 
        return retval;
 }

diff --git a/src/target/armv4_5_mmu.h b/src/target/armv4_5_mmu.h
index d2716fb0fdc683ec5f1ea8da2c47e8d85335f2d9..f39834e27b74a0b12b819bb368d0eecc72073259 100644 (file)
--- a/src/target/armv4_5_mmu.h
+++ b/src/target/armv4_5_mmu.h
@@ -29,8 +29,8 @@ struct armv4_5_mmu_common
        int (*get_ttb)(struct target *target, uint32_t *result);
        int (*read_memory)(struct target *target, uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer);
        int (*write_memory)(struct target *target, uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer);
-       void (*disable_mmu_caches)(struct target *target, int mmu, int d_u_cache, int i_cache);
-       void (*enable_mmu_caches)(struct target *target, int mmu, int d_u_cache, int i_cache);
+       int (*disable_mmu_caches)(struct target *target, int mmu, int d_u_cache, int i_cache);
+       int (*enable_mmu_caches)(struct target *target, int mmu, int d_u_cache, int i_cache);
        struct armv4_5_cache_common armv4_5_cache;
        int has_tiny_pages;
        int mmu_enabled;

diff --git a/src/target/cortex_a8.c b/src/target/cortex_a8.c
index e1acbf74c705aa6eae17b4004c8ec3416bc7096b..9a9018026d004eb4065a664f71934f87730bfd47 100644 (file)
--- a/src/target/cortex_a8.c
+++ b/src/target/cortex_a8.c
@@ -58,9 +58,9 @@ static int cortex_a8_dap_write_coreregister_u32(struct target *target,
 static int cortex_a8_mmu(struct target *target, int *enabled);
 static int cortex_a8_virt2phys(struct target *target,
                 uint32_t virt, uint32_t *phys);
-static void cortex_a8_disable_mmu_caches(struct target *target, int mmu,
+static int cortex_a8_disable_mmu_caches(struct target *target, int mmu,
                 int d_u_cache, int i_cache);
-static void cortex_a8_enable_mmu_caches(struct target *target, int mmu,
+static int cortex_a8_enable_mmu_caches(struct target *target, int mmu,
                 int d_u_cache, int i_cache);
 static int cortex_a8_get_ttb(struct target *target, uint32_t *result);
 
@@ -1916,19 +1916,21 @@ static int cortex_a8_get_ttb(struct target *target, uint32_t *result)
     return ERROR_OK;
 }
 
-/* FIX! error propagation missing from this fn */
-static void cortex_a8_disable_mmu_caches(struct target *target, int mmu,
+static int cortex_a8_disable_mmu_caches(struct target *target, int mmu,
                 int d_u_cache, int i_cache)
 {
     struct cortex_a8_common *cortex_a8 = target_to_cortex_a8(target);
     struct armv7a_common *armv7a = &cortex_a8->armv7a_common;
     uint32_t cp15_control;
+    int retval;
 
     /* read cp15 control register */
-    armv7a->armv4_5_common.mrc(target, 15,
+    retval = armv7a->armv4_5_common.mrc(target, 15,
                     0, 0,   /* op1, op2 */
                     1, 0,   /* CRn, CRm */
                     &cp15_control);
+    if (retval != ERROR_OK)
+       return retval;
 
 
     if (mmu)
@@ -1940,25 +1942,28 @@ static void cortex_a8_disable_mmu_caches(struct target *target, int mmu,
     if (i_cache)
             cp15_control &= ~0x1000U;
 
-    armv7a->armv4_5_common.mcr(target, 15,
+    retval = armv7a->armv4_5_common.mcr(target, 15,
                     0, 0,   /* op1, op2 */
                     1, 0,   /* CRn, CRm */
                     cp15_control);
+       return retval;
 }
 
-/* FIX! error propagation missing from this fn */
-static void cortex_a8_enable_mmu_caches(struct target *target, int mmu,
+static int cortex_a8_enable_mmu_caches(struct target *target, int mmu,
                 int d_u_cache, int i_cache)
 {
     struct cortex_a8_common *cortex_a8 = target_to_cortex_a8(target);
     struct armv7a_common *armv7a = &cortex_a8->armv7a_common;
     uint32_t cp15_control;
+    int retval;
 
     /* read cp15 control register */
-    armv7a->armv4_5_common.mrc(target, 15,
+    retval = armv7a->armv4_5_common.mrc(target, 15,
                     0, 0,   /* op1, op2 */
                     1, 0,   /* CRn, CRm */
                     &cp15_control);
+    if (retval != ERROR_OK)
+       return retval;
 
     if (mmu)
             cp15_control |= 0x1U;
@@ -1969,10 +1974,11 @@ static void cortex_a8_enable_mmu_caches(struct target *target, int mmu,
     if (i_cache)
             cp15_control |= 0x1000U;
 
-    armv7a->armv4_5_common.mcr(target, 15,
+    retval = armv7a->armv4_5_common.mcr(target, 15,
                     0, 0,   /* op1, op2 */
                     1, 0,   /* CRn, CRm */
                     cp15_control);
+       return retval;
 }
 
 

diff --git a/src/target/xscale.c b/src/target/xscale.c
index 35efa8579c17932c4d7f6898d45deab996e577d5..c0080b2b388c7c1b3d80e6ca0e7942e6c3e4dae5 100644 (file)
--- a/src/target/xscale.c
+++ b/src/target/xscale.c
@@ -2018,14 +2018,17 @@ static int xscale_get_ttb(struct target *target, uint32_t *result)
        return ERROR_OK;
 }
 
-static void xscale_disable_mmu_caches(struct target *target, int mmu,
+static int xscale_disable_mmu_caches(struct target *target, int mmu,
                int d_u_cache, int i_cache)
 {
        struct xscale_common *xscale = target_to_xscale(target);
        uint32_t cp15_control;
+       int retval;
 
        /* read cp15 control register */
-       xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CTRL]);
+       retval = xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CTRL]);
+       if (retval !=ERROR_OK)
+               return retval;
        cp15_control = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_CTRL].value, 0, 32);
 
        if (mmu)
@@ -2034,11 +2037,17 @@ static void xscale_disable_mmu_caches(struct target *target, int mmu,
        if (d_u_cache)
        {
                /* clean DCache */
-               xscale_send_u32(target, 0x50);
-               xscale_send_u32(target, xscale->cache_clean_address);
+               retval = xscale_send_u32(target, 0x50);
+               if (retval !=ERROR_OK)
+                       return retval;
+               retval = xscale_send_u32(target, xscale->cache_clean_address);
+               if (retval !=ERROR_OK)
+                       return retval;
 
                /* invalidate DCache */
-               xscale_send_u32(target, 0x51);
+               retval = xscale_send_u32(target, 0x51);
+               if (retval !=ERROR_OK)
+                       return retval;
 
                cp15_control &= ~0x4U;
        }
@@ -2046,25 +2055,33 @@ static void xscale_disable_mmu_caches(struct target *target, int mmu,
        if (i_cache)
        {
                /* invalidate ICache */
-               xscale_send_u32(target, 0x52);
+               retval = xscale_send_u32(target, 0x52);
+               if (retval !=ERROR_OK)
+                       return retval;
                cp15_control &= ~0x1000U;
        }
 
        /* write new cp15 control register */
-       xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_CTRL], cp15_control);
+       retval = xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_CTRL], cp15_control);
+       if (retval !=ERROR_OK)
+               return retval;
 
        /* execute cpwait to ensure outstanding operations complete */
-       xscale_send_u32(target, 0x53);
+       retval = xscale_send_u32(target, 0x53);
+       return retval;
 }
 
-static void xscale_enable_mmu_caches(struct target *target, int mmu,
+static int xscale_enable_mmu_caches(struct target *target, int mmu,
                int d_u_cache, int i_cache)
 {
        struct xscale_common *xscale = target_to_xscale(target);
        uint32_t cp15_control;
+       int retval;
 
        /* read cp15 control register */
-       xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CTRL]);
+       retval = xscale_get_reg(&xscale->reg_cache->reg_list[XSCALE_CTRL]);
+       if (retval !=ERROR_OK)
+               return retval;
        cp15_control = buf_get_u32(xscale->reg_cache->reg_list[XSCALE_CTRL].value, 0, 32);
 
        if (mmu)
@@ -2077,10 +2094,13 @@ static void xscale_enable_mmu_caches(struct target *target, int mmu,
                cp15_control |= 0x1000U;
 
        /* write new cp15 control register */
-       xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_CTRL], cp15_control);
+       retval = xscale_set_reg_u32(&xscale->reg_cache->reg_list[XSCALE_CTRL], cp15_control);
+       if (retval !=ERROR_OK)
+               return retval;
 
        /* execute cpwait to ensure outstanding operations complete */
-       xscale_send_u32(target, 0x53);
+       retval = xscale_send_u32(target, 0x53);
+       return retval;
 }
 
 static int xscale_set_breakpoint(struct target *target,