1 files changed, 43 insertions, 26 deletions

diff --git a/drivers/fpga/altera-cvp.c b/drivers/fpga/altera-cvp.c
index 9df2073331cb..3b8386fd32e7 100644
--- a/drivers/fpga/altera-cvp.c
+++ b/drivers/fpga/altera-cvp.c
@@ -143,6 +143,42 @@ static int altera_cvp_wait_status(struct altera_cvp_conf *conf, u32 status_mask,
         return -ETIMEDOUT;
 }
 
+static int altera_cvp_chk_error(struct fpga_manager *mgr, size_t bytes)
+{
+        struct altera_cvp_conf *conf = mgr->priv;
+        u32 val;
+        int ret;
+
+        /* STEP 10 (optional) - check CVP_CONFIG_ERROR flag */
+        ret = altera_read_config_dword(conf, VSE_CVP_STATUS, &val);
+        if (ret || (val & VSE_CVP_STATUS_CFG_ERR)) {
+                dev_err(&mgr->dev, "CVP_CONFIG_ERROR after %zu bytes!\n",
+                        bytes);
+                return -EPROTO;
+        }
+        return 0;
+}
+
+static int altera_cvp_send_block(struct altera_cvp_conf *conf,
+                                 const u32 *data, size_t len)
+{
+        u32 mask, words = len / sizeof(u32);
+        int i, remainder;
+
+        for (i = 0; i < words; i++)
+                conf->write_data(conf, *data++);
+
+        /* write up to 3 trailing bytes, if any */
+        remainder = len % sizeof(u32);
+        if (remainder) {
+                mask = BIT(remainder * 8) - 1;
+                if (mask)
+                        conf->write_data(conf, *data & mask);
+        }
+
+        return 0;
+}
+
 static int altera_cvp_teardown(struct fpga_manager *mgr,
                                struct fpga_image_info *info)
 {
@@ -265,39 +301,25 @@ static int altera_cvp_write_init(struct fpga_manager *mgr,
         return 0;
 }
 
-static inline int altera_cvp_chk_error(struct fpga_manager *mgr, size_t bytes)
-{
-        struct altera_cvp_conf *conf = mgr->priv;
-        u32 val;
-
-        /* STEP 10 (optional) - check CVP_CONFIG_ERROR flag */
-        altera_read_config_dword(conf, VSE_CVP_STATUS, &val);
-        if (val & VSE_CVP_STATUS_CFG_ERR) {
-                dev_err(&mgr->dev, "CVP_CONFIG_ERROR after %zu bytes!\n",
-                        bytes);
-                return -EPROTO;
-        }
-        return 0;
-}
-
 static int altera_cvp_write(struct fpga_manager *mgr, const char *buf,
                             size_t count)
 {
         struct altera_cvp_conf *conf = mgr->priv;
+        size_t done, remaining, len;
         const u32 *data;
-        size_t done, remaining;
         int status = 0;
-        u32 mask;
 
         /* STEP 9 - write 32-bit data from RBF file to CVP data register */
         data = (u32 *)buf;
         remaining = count;
         done = 0;
 
-        while (remaining >= 4) {
-                conf->write_data(conf, *data++);
-                done += 4;
-                remaining -= 4;
+        while (remaining) {
+                len = min(sizeof(u32), remaining);
+                altera_cvp_send_block(conf, data, len);
+                data++;
+                done += len;
+                remaining -= len;
 
                 /*
                  * STEP 10 (optional) and STEP 11
@@ -315,11 +337,6 @@ static int altera_cvp_write(struct fpga_manager *mgr, const char *buf,
                 }
         }
 
-        /* write up to 3 trailing bytes, if any */
-        mask = BIT(remaining * 8) - 1;
-        if (mask)
-                conf->write_data(conf, *data & mask);
-
         if (altera_cvp_chkcfg)
                 status = altera_cvp_chk_error(mgr, count);