1 files changed, 58 insertions, 12 deletions

diff --git a/drivers/base/regmap/regcache-maple.c b/drivers/base/regmap/regcache-maple.c
index c2e3a0f6c218..08316d578be2 100644
--- a/drivers/base/regmap/regcache-maple.c
+++ b/drivers/base/regmap/regcache-maple.c
@@ -74,7 +74,7 @@ static int regcache_maple_write(struct regmap *map, unsigned int reg,
 	rcu_read_unlock();
 
 	entry = kmalloc((last - index + 1) * sizeof(unsigned long),
-			GFP_KERNEL);
+			map->alloc_flags);
 	if (!entry)
 		return -ENOMEM;
 
@@ -92,7 +92,7 @@ static int regcache_maple_write(struct regmap *map, unsigned int reg,
 	mas_lock(&mas);
 
 	mas_set_range(&mas, index, last);
-	ret = mas_store_gfp(&mas, entry, GFP_KERNEL);
+	ret = mas_store_gfp(&mas, entry, map->alloc_flags);
 
 	mas_unlock(&mas);
 
@@ -134,7 +134,7 @@ static int regcache_maple_drop(struct regmap *map, unsigned int min,
 
 			lower = kmemdup(entry, ((min - mas.index) *
 						sizeof(unsigned long)),
-					GFP_KERNEL);
+					map->alloc_flags);
 			if (!lower) {
 				ret = -ENOMEM;
 				goto out_unlocked;
@@ -148,7 +148,7 @@ static int regcache_maple_drop(struct regmap *map, unsigned int min,
 			upper = kmemdup(&entry[max + 1],
 					((mas.last - max) *
 					 sizeof(unsigned long)),
-					GFP_KERNEL);
+					map->alloc_flags);
 			if (!upper) {
 				ret = -ENOMEM;
 				goto out_unlocked;
@@ -162,7 +162,7 @@ static int regcache_maple_drop(struct regmap *map, unsigned int min,
 		/* Insert new nodes with the saved data */
 		if (lower) {
 			mas_set_range(&mas, lower_index, lower_last);
-			ret = mas_store_gfp(&mas, lower, GFP_KERNEL);
+			ret = mas_store_gfp(&mas, lower, map->alloc_flags);
 			if (ret != 0)
 				goto out;
 			lower = NULL;
@@ -170,7 +170,7 @@ static int regcache_maple_drop(struct regmap *map, unsigned int min,
 
 		if (upper) {
 			mas_set_range(&mas, upper_index, upper_last);
-			ret = mas_store_gfp(&mas, upper, GFP_KERNEL);
+			ret = mas_store_gfp(&mas, upper, map->alloc_flags);
 			if (ret != 0)
 				goto out;
 			upper = NULL;
@@ -242,11 +242,41 @@ static int regcache_maple_exit(struct regmap *map)
 	return 0;
 }
 
+static int regcache_maple_insert_block(struct regmap *map, int first,
+					int last)
+{
+	struct maple_tree *mt = map->cache;
+	MA_STATE(mas, mt, first, last);
+	unsigned long *entry;
+	int i, ret;
+
+	entry = kcalloc(last - first + 1, sizeof(unsigned long), map->alloc_flags);
+	if (!entry)
+		return -ENOMEM;
+
+	for (i = 0; i < last - first + 1; i++)
+		entry[i] = map->reg_defaults[first + i].def;
+
+	mas_lock(&mas);
+
+	mas_set_range(&mas, map->reg_defaults[first].reg,
+		      map->reg_defaults[last].reg);
+	ret = mas_store_gfp(&mas, entry, map->alloc_flags);
+
+	mas_unlock(&mas);
+
+	if (ret)
+		kfree(entry);
+
+	return ret;
+}
+
 static int regcache_maple_init(struct regmap *map)
 {
 	struct maple_tree *mt;
 	int i;
 	int ret;
+	int range_start;
 
 	mt = kmalloc(sizeof(*mt), GFP_KERNEL);
 	if (!mt)
@@ -255,14 +285,30 @@ static int regcache_maple_init(struct regmap *map)
 
 	mt_init(mt);
 
-	for (i = 0; i < map->num_reg_defaults; i++) {
-		ret = regcache_maple_write(map,
-					   map->reg_defaults[i].reg,
-					   map->reg_defaults[i].def);
-		if (ret)
-			goto err;
+	if (!map->num_reg_defaults)
+		return 0;
+
+	range_start = 0;
+
+	/* Scan for ranges of contiguous registers */
+	for (i = 1; i < map->num_reg_defaults; i++) {
+		if (map->reg_defaults[i].reg !=
+		    map->reg_defaults[i - 1].reg + 1) {
+			ret = regcache_maple_insert_block(map, range_start,
+							  i - 1);
+			if (ret != 0)
+				goto err;
+
+			range_start = i;
+		}
 	}
 
+	/* Add the last block */
+	ret = regcache_maple_insert_block(map, range_start,
+					  map->num_reg_defaults - 1);
+	if (ret != 0)
+		goto err;
+
 	return 0;
 
 err: