ARM NAND I/O refactor code copying.

[openocd.git] / src / flash / arm_nandio.c

diff --git a/src/flash/arm_nandio.c b/src/flash/arm_nandio.c
index 20a76f43cc27458ec25b6741006b41ecc3761b04..990d3a0fc4296c5c86a19f5c2d6a81d485f8c689 100644 (file)
--- a/src/flash/arm_nandio.c
+++ b/src/flash/arm_nandio.c
@@ -28,6 +28,45 @@
 #include "armv4_5.h"
 #include "algorithm.h"
 
+/**
+ * Copies code to a working area.  This will allocate room for the code plus the
+ * additional amount requested if the working area pointer is null.
+ *
+ * @param target Pointer to the target to copy code to
+ * @param code Pointer to the code area to be copied
+ * @param code_size Size of the code being copied
+ * @param additional Size of the additional area to be allocated in addition to
+ *                   code
+ * @param area Pointer to a pointer to a working area to copy code to
+ * @return Success or failure of the operation
+ */
+int arm_code_to_working_area(struct target *target, const uint32_t *code, unsigned code_size,
+               unsigned additional, struct working_area **area)
+{
+       uint8_t code_buf[code_size];
+       unsigned i;
+       int retval;
+       unsigned size = code_size + additional;
+
+       /* make sure we have a working area */
+       if (NULL == *area) {
+               retval = target_alloc_working_area(target, size, area);
+               if (retval != ERROR_OK) {
+                       LOG_DEBUG("%s: no %d byte buffer", __FUNCTION__, (int) size);
+                       return ERROR_NAND_NO_BUFFER;
+               }
+       }
+
+       /* buffer code in target endianness */
+       for (i = 0; i < code_size / 4; i++)
+               target_buffer_set_u32(target, code_buf + i * 4, code[i]);
+
+       /* copy code to work area */
+       retval = target_write_memory(target, (*area)->address,
+                       4, code_size / 4, code_buf);
+
+       return retval;
+}
 
 /*
  * ARM-specific bulk write from buffer to address of 8-bit wide NAND.
@@ -66,29 +105,11 @@ int arm_nandwrite(struct arm_nand_data *nand, uint8_t *data, int size)
        };
 
        if (!nand->copy_area) {
-               uint8_t         code_buf[sizeof(code)];
-               unsigned        i;
-
-               /* make sure we have a working area */
-               if (target_alloc_working_area(target,
-                               sizeof(code) + nand->chunk_size,
-                               &nand->copy_area) != ERROR_OK) {
-                       LOG_DEBUG("%s: no %d byte buffer",
-                                       __FUNCTION__,
-                                       (int) sizeof(code) + nand->chunk_size);
-                       return ERROR_NAND_NO_BUFFER;
-               }
-
-               /* buffer code in target endianness */
-               for (i = 0; i < sizeof(code) / 4; i++)
-                       target_buffer_set_u32(target, code_buf + i * 4, code[i]);
-
-               /* copy code to work area */
-                retval = target_write_memory(target,
-                                       nand->copy_area->address,
-                                       4, sizeof(code) / 4, code_buf);
-               if (retval != ERROR_OK)
+               retval = arm_code_to_working_area(target, code, sizeof(code),
+                               nand->chunk_size, &nand->copy_area);
+               if (retval != ERROR_OK) {
                        return retval;
+               }
        }
 
        /* copy data to work area */
@@ -139,7 +160,8 @@ int arm_nandwrite(struct arm_nand_data *nand, uint8_t *data, int size)
  * @param data Pointer to the data buffer to store the read data
  * @param size Amount of data to be stored to the buffer.
  */
-int arm_nandread(struct arm_nand_data *nand, uint8_t *data, uint32_t size) {
+int arm_nandread(struct arm_nand_data *nand, uint8_t *data, uint32_t size)
+{
        struct target *target = nand->target;
        struct armv4_5_algorithm algo;
        struct arm *armv4_5 = target->arch_info;
@@ -165,23 +187,8 @@ int arm_nandread(struct arm_nand_data *nand, uint8_t *data, uint32_t size) {
 
        /* create the copy area if not yet available */
        if (!nand->copy_area) {
-               uint8_t code_buf[sizeof(code)];
-               unsigned i;
-
-               /* make sure we have a working area */
-               retval = target_alloc_working_area(target, sizeof(code) + nand->chunk_size, &nand->copy_area);
-               if (retval != ERROR_OK) {
-                       LOG_DEBUG("%s: no %d byte buffer", __FUNCTION__, (int) sizeof(code) + nand->chunk_size);
-                       return ERROR_NAND_NO_BUFFER;
-               }
-
-               /* buffer code in target endianness */
-               for (i = 0; i < sizeof(code) / 4; i++) {
-                       target_buffer_set_u32(target, code_buf + i * 4, code[i]);
-               }
-
-               /* copy code to work area */
-        retval = target_write_memory(target, nand->copy_area->address, 4, sizeof(code) / 4, code_buf);
+               retval = arm_code_to_working_area(target, code, sizeof(code),
+                               nand->chunk_size, &nand->copy_area);
                if (retval != ERROR_OK) {
                        return retval;
                }