Merge tag 'armsoc-dt' of git://git.kernel.org/pub/scm/linux/kernel/git/arm/arm-soc

[karo-tx-linux.git] / drivers / base / regmap / regcache.c

diff --git a/drivers/base/regmap/regcache.c b/drivers/base/regmap/regcache.c
index 348be3a354108eb2533587dabed9495e9c324b5a..4170b7d952767074140ce26c4c1ac621823607ad 100644 (file)
--- a/drivers/base/regmap/regcache.c
+++ b/drivers/base/regmap/regcache.c
@@ -30,7 +30,7 @@ static int regcache_hw_init(struct regmap *map)
        int i, j;
        int ret;
        int count;
-       unsigned int val;
+       unsigned int reg, val;
        void *tmp_buf;
 
        if (!map->num_reg_defaults_raw)
@@ -57,7 +57,7 @@ static int regcache_hw_init(struct regmap *map)
                bool cache_bypass = map->cache_bypass;
                dev_warn(map->dev, "No cache defaults, reading back from HW\n");
 
-               /* Bypass the cache access till data read from HW*/
+               /* Bypass the cache access till data read from HW */
                map->cache_bypass = true;
                tmp_buf = kmalloc(map->cache_size_raw, GFP_KERNEL);
                if (!tmp_buf) {
@@ -65,29 +65,48 @@ static int regcache_hw_init(struct regmap *map)
                        goto err_free;
                }
                ret = regmap_raw_read(map, 0, tmp_buf,
-                                     map->num_reg_defaults_raw);
+                                     map->cache_size_raw);
                map->cache_bypass = cache_bypass;
-               if (ret < 0)
-                       goto err_cache_free;
-
-               map->reg_defaults_raw = tmp_buf;
-               map->cache_free = 1;
+               if (ret == 0) {
+                       map->reg_defaults_raw = tmp_buf;
+                       map->cache_free = 1;
+               } else {
+                       kfree(tmp_buf);
+               }
        }
 
        /* fill the reg_defaults */
        for (i = 0, j = 0; i < map->num_reg_defaults_raw; i++) {
-               if (regmap_volatile(map, i * map->reg_stride))
+               reg = i * map->reg_stride;
+
+               if (!regmap_readable(map, reg))
                        continue;
-               val = regcache_get_val(map, map->reg_defaults_raw, i);
-               map->reg_defaults[j].reg = i * map->reg_stride;
+
+               if (regmap_volatile(map, reg))
+                       continue;
+
+               if (map->reg_defaults_raw) {
+                       val = regcache_get_val(map, map->reg_defaults_raw, i);
+               } else {
+                       bool cache_bypass = map->cache_bypass;
+
+                       map->cache_bypass = true;
+                       ret = regmap_read(map, reg, &val);
+                       map->cache_bypass = cache_bypass;
+                       if (ret != 0) {
+                               dev_err(map->dev, "Failed to read %d: %d\n",
+                                       reg, ret);
+                               goto err_free;
+                       }
+               }
+
+               map->reg_defaults[j].reg = reg;
                map->reg_defaults[j].def = val;
                j++;
        }
 
        return 0;
 
-err_cache_free:
-       kfree(tmp_buf);
 err_free:
        kfree(map->reg_defaults);