flash/nor/rp2040: fix setting sp

[openocd.git] / src / flash / nor / rp2040.c

diff --git a/src/flash/nor/rp2040.c b/src/flash/nor/rp2040.c
index 7cc6434ffeab573d279eae1f898c4e397b8b2877..feff9a6f9b5bce50c0c4004fc0843df69adcd0df 100644 (file)
--- a/src/flash/nor/rp2040.c
+++ b/src/flash/nor/rp2040.c
@@ -99,8 +99,7 @@ static int rp2040_call_rom_func(struct target *target, struct rp2040_flash_bank
        }
        target_addr_t stacktop = priv->stack->address + priv->stack->size;
 
-       LOG_DEBUG("Calling ROM func @0x%" PRIx16 " with %d arguments", func_offset, n_args);
-       LOG_DEBUG("Calling on core \"%s\"", target->cmd_name);
+       LOG_TARGET_DEBUG(target, "Calling ROM func @0x%" PRIx16 " with %u arguments", func_offset, n_args);
 
        struct reg_param args[ARRAY_SIZE(regnames) + 2];
        struct armv7m_algorithm alg_info;
@@ -112,11 +111,12 @@ static int rp2040_call_rom_func(struct target *target, struct rp2040_flash_bank
        /* Pass function pointer in r7 */
        init_reg_param(&args[n_args], "r7", 32, PARAM_OUT);
        buf_set_u32(args[n_args].value, 0, 32, func_offset);
+       /* Setup stack */
        init_reg_param(&args[n_args + 1], "sp", 32, PARAM_OUT);
        buf_set_u32(args[n_args + 1].value, 0, 32, stacktop);
+       unsigned int n_reg_params = n_args + 2; /* User arguments + r7 + sp */
 
-
-       for (unsigned int i = 0; i < n_args + 2; ++i)
+       for (unsigned int i = 0; i < n_reg_params; ++i)
                LOG_DEBUG("Set %s = 0x%" PRIx32, args[i].reg_name, buf_get_u32(args[i].value, 0, 32));
 
        /* Actually call the function */
@@ -125,17 +125,19 @@ static int rp2040_call_rom_func(struct target *target, struct rp2040_flash_bank
        int err = target_run_algorithm(
                target,
                0, NULL,          /* No memory arguments */
-               n_args + 1, args, /* User arguments + r7 */
+               n_reg_params, args, /* User arguments + r7 + sp */
                priv->jump_debug_trampoline, priv->jump_debug_trampoline_end,
                timeout_ms,
                &alg_info
        );
-       for (unsigned int i = 0; i < n_args + 2; ++i)
+
+       for (unsigned int i = 0; i < n_reg_params; ++i)
                destroy_reg_param(&args[i]);
+
        if (err != ERROR_OK)
-               LOG_ERROR("Failed to invoke ROM function @0x%" PRIx16 "\n", func_offset);
-       return err;
+               LOG_ERROR("Failed to invoke ROM function @0x%" PRIx16, func_offset);
 
+       return err;
 }
 
 /* Finalize flash write/erase/read ID
@@ -211,6 +213,12 @@ static int rp2040_flash_write(struct flash_bank *bank, const uint8_t *buffer, ui
 
        struct rp2040_flash_bank *priv = bank->driver_priv;
        struct target *target = bank->target;
+
+       if (target->state != TARGET_HALTED) {
+               LOG_ERROR("Target not halted");
+               return ERROR_TARGET_NOT_HALTED;
+       }
+
        struct working_area *bounce = NULL;
 
        int err = rp2040_stack_grab_and_prep(bank);
@@ -266,6 +274,13 @@ cleanup:
 static int rp2040_flash_erase(struct flash_bank *bank, unsigned int first, unsigned int last)
 {
        struct rp2040_flash_bank *priv = bank->driver_priv;
+       struct target *target = bank->target;
+
+       if (target->state != TARGET_HALTED) {
+               LOG_ERROR("Target not halted");
+               return ERROR_TARGET_NOT_HALTED;
+       }
+
        uint32_t start_addr = bank->sectors[first].offset;
        uint32_t length = bank->sectors[last].offset + bank->sectors[last].size - start_addr;
        LOG_DEBUG("RP2040 erase %d bytes starting at 0x%" PRIx32, length, start_addr);
@@ -294,7 +309,7 @@ static int rp2040_flash_erase(struct flash_bank *bank, unsigned int first, unsig
        */
 
        int timeout_ms = 2000 * (last - first) + 1000;
-       err = rp2040_call_rom_func(bank->target, priv, priv->jump_flash_range_erase,
+       err = rp2040_call_rom_func(target, priv, priv->jump_flash_range_erase,
                                                        args, ARRAY_SIZE(args), timeout_ms);
 
 cleanup:
@@ -362,6 +377,11 @@ static int rp2040_flash_probe(struct flash_bank *bank)
        struct rp2040_flash_bank *priv = bank->driver_priv;
        struct target *target = bank->target;
 
+       if (target->state != TARGET_HALTED) {
+               LOG_ERROR("Target not halted");
+               return ERROR_TARGET_NOT_HALTED;
+       }
+
        int err = rp2040_lookup_symbol(target, FUNC_DEBUG_TRAMPOLINE, &priv->jump_debug_trampoline);
        if (err != ERROR_OK) {
                LOG_ERROR("Debug trampoline not found in RP2040 ROM.");