ASoC: wm0010: Split out the stage2 load from the boot function

Signed-off-by: Scott Ling <sl@opensource.wolfsonmicro.com>
Signed-off-by: Mark Brown <broonie@opensource.wolfsonmicro.com>

1 files changed, 79 insertions, 50 deletions

diff --git a/sound/soc/codecs/wm0010.c b/sound/soc/codecs/wm0010.c
index 7576330044ba..4b68ea86d7cb 100644
--- a/sound/soc/codecs/wm0010.c
+++ b/sound/soc/codecs/wm0010.c
@@ -477,6 +477,82 @@ abort:
         return ret;
 }
 
+static int wm0010_stage2_load(struct snd_soc_codec *codec)
+{
+        struct spi_device *spi = to_spi_device(codec->dev);
+        struct wm0010_priv *wm0010 = snd_soc_codec_get_drvdata(codec);
+        const struct firmware *fw;
+        struct spi_message m;
+        struct spi_transfer t;
+        u32 *img;
+        u8 *out;
+        int i;
+        int ret = 0;
+
+        ret = request_firmware(&fw, "wm0010_stage2.bin", codec->dev);
+        if (ret != 0) {
+                dev_err(codec->dev, "Failed to request stage2 loader: %d\n",
+                        ret);
+                return ret;
+        }
+
+        dev_dbg(codec->dev, "Downloading %zu byte stage 2 loader\n", fw->size);
+
+        /* Copy to local buffer first as vmalloc causes problems for dma */
+        img = kzalloc(fw->size, GFP_KERNEL);
+        if (!img) {
+                dev_err(codec->dev, "Failed to allocate image buffer\n");
+                ret = -ENOMEM;
+                goto abort2;
+        }
+
+        out = kzalloc(fw->size, GFP_KERNEL);
+        if (!out) {
+                dev_err(codec->dev, "Failed to allocate output buffer\n");
+                ret = -ENOMEM;
+                goto abort1;
+        }
+
+        memcpy(img, &fw->data[0], fw->size);
+
+        spi_message_init(&m);
+        memset(&t, 0, sizeof(t));
+        t.rx_buf = out;
+        t.tx_buf = img;
+        t.len = fw->size;
+        t.bits_per_word = 8;
+        t.speed_hz = wm0010->sysclk / 10;
+        spi_message_add_tail(&t, &m);
+
+        dev_dbg(codec->dev, "Starting initial download at %dHz\n",
+                t.speed_hz);
+
+        ret = spi_sync(spi, &m);
+        if (ret != 0) {
+                dev_err(codec->dev, "Initial download failed: %d\n", ret);
+                goto abort;
+        }
+
+        /* Look for errors from the boot ROM */
+        for (i = 0; i < fw->size; i++) {
+                if (out[i] != 0x55) {
+                        dev_err(codec->dev, "Boot ROM error: %x in %d\n",
+                                out[i], i);
+                        wm0010_mark_boot_failure(wm0010);
+                        ret = -EBUSY;
+                        goto abort;
+                }
+        }
+abort:
+        kfree(out);
+abort1:
+        kfree(img);
+abort2:
+        release_firmware(fw);
+
+        return ret;
+}
+
 static int wm0010_boot(struct snd_soc_codec *codec)
 {
         struct spi_device *spi = to_spi_device(codec->dev);
@@ -487,10 +563,9 @@ static int wm0010_boot(struct snd_soc_codec *codec)
         struct spi_message m;
         struct spi_transfer t;
         struct dfw_pllrec pll_rec;
-        u32 *img, *p;
+        u32 *p, len;
         u64 *img_swap;
         u8 *out;
-        u32 len;
         int i;
 
         spin_lock_irqsave(&wm0010->irq_lock, flags);
@@ -546,55 +621,9 @@ static int wm0010_boot(struct snd_soc_codec *codec)
         wm0010->state = WM0010_BOOTROM;
         spin_unlock_irqrestore(&wm0010->irq_lock, flags);
 
-        dev_dbg(codec->dev, "Downloading %zu byte stage 2 loader\n", fw->size);
-
-        /* Copy to local buffer first as vmalloc causes problems for dma */
-        img = kzalloc(fw->size, GFP_KERNEL);
-        if (!img) {
-                dev_err(codec->dev, "Failed to allocate image buffer\n");
-                goto abort;
-        }
-
-        out = kzalloc(fw->size, GFP_KERNEL);
-        if (!out) {
-                dev_err(codec->dev, "Failed to allocate output buffer\n");
-                goto abort;
-        }
-
-        memcpy(img, &fw->data[0], fw->size);
-
-        spi_message_init(&m);
-        memset(&t, 0, sizeof(t));
-        t.rx_buf = out;
-        t.tx_buf = img;
-        t.len = fw->size;
-        t.bits_per_word = 8;
-        t.speed_hz = wm0010->sysclk / 10;
-        spi_message_add_tail(&t, &m);
-
-        dev_dbg(codec->dev, "Starting initial download at %dHz\n",
-                t.speed_hz);
-
-        ret = spi_sync(spi, &m);
-        if (ret != 0) {
-                dev_err(codec->dev, "Initial download failed: %d\n", ret);
+        ret = wm0010_stage2_load(codec);
+        if (ret)
                 goto abort;
-        }
-
-        /* Look for errors from the boot ROM */
-        for (i = 0; i < fw->size; i++) {
-                if (out[i] != 0x55) {
-                        ret = -EBUSY;
-                        dev_err(codec->dev, "Boot ROM error: %x in %d\n",
-                                out[i], i);
-                        wm0010_mark_boot_failure(wm0010);
-                        goto abort;
-                }
-        }
-
-        release_firmware(fw);
-        kfree(img);
-        kfree(out);
 
         if (!wait_for_completion_timeout(&wm0010->boot_completion,
                                          msecs_to_jiffies(10)))