From 5469f52601c8714cf20b2f4dd6e78481f9d17bf1 Mon Sep 17 00:00:00 2001
From: Max Schwarz <max.schwarz@online.de>
Date: Fri, 18 Apr 2014 00:38:47 +0200
Subject: [PATCH] regmap: support for registers with write mask in upper half

Rockchip RK3xxx SoCs have some registers which use the upper half as the
write enable mask for the lower half. We support this by always writing
ones to the upper half if requested by config->write_mask.

This patch adds wrappers around the format_val and format_write hooks,
which apply the write mask before calling the real formatting hooks.

Signed-off-by: Max Schwarz <max.schwarz@online.de>
---
 drivers/base/regmap/internal.h |  7 +++++++
 drivers/base/regmap/regmap.c   | 41 ++++++++++++++++++++++++++++++++++++-----
 include/linux/regmap.h         |  5 +++++
 3 files changed, 48 insertions(+), 5 deletions(-)

diff --git a/drivers/base/regmap/internal.h b/drivers/base/regmap/internal.h
index 7d13269..8043519 100644
--- a/drivers/base/regmap/internal.h
+++ b/drivers/base/regmap/internal.h
@@ -34,9 +34,13 @@ struct regmap_format {
 	size_t reg_bytes;
 	size_t pad_bytes;
 	size_t val_bytes;
+
+	/* don't use directly, use regmap_format_write */
 	void (*format_write)(struct regmap *map,
 			     unsigned int reg, unsigned int val);
 	void (*format_reg)(void *buf, unsigned int reg, unsigned int shift);
+
+	/* don't use directly, use regmap_format_val */
 	void (*format_val)(void *buf, unsigned int val, unsigned int shift);
 	unsigned int (*parse_val)(const void *buf);
 	void (*parse_inplace)(void *buf);
@@ -139,6 +143,9 @@ struct regmap {
 
 	struct rb_root range_tree;
 	void *selector_work_buf;	/* Scratch buffer used for selector */
+
+	/* applied during write */
+	unsigned int write_mask;
 };
 
 struct regcache_ops {
diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c
index 63e30ef..95e5816 100644
--- a/drivers/base/regmap/regmap.c
+++ b/drivers/base/regmap/regmap.c
@@ -42,6 +42,28 @@ static int _regmap_bus_formatted_write(void *context, unsigned int reg,
 static int _regmap_bus_raw_write(void *context, unsigned int reg,
 				 unsigned int val);
 
+/*
+ * Some targets use the upper half of the register for write masking of the
+ * lower bits. To use regmap on those registers, we always have to set the
+ * upper bits to ones during write.
+ */
+
+static void regmap_format_val(struct regmap *map, void *buf, unsigned int val,
+			      unsigned int shift)
+{
+	val |= map->write_mask;
+
+	return map->format.format_val(buf, val, shift);
+}
+
+static void regmap_format_write(struct regmap *map, unsigned int reg,
+				unsigned int val)
+{
+	val |= map->write_mask;
+
+	return map->format.format_write(map, reg, val);
+}
+
 bool regmap_reg_in_ranges(unsigned int reg,
 			  const struct regmap_range *ranges,
 			  unsigned int nranges)
@@ -489,6 +511,15 @@ struct regmap *regmap_init(struct device *dev,
 		map->read_flag_mask = bus->read_flag_mask;
 	}
 
+	if (config->write_mask) {
+		/* that's only possible with an even number of bits */
+		if (config->val_bits % 2)
+			goto err;
+
+		/* generate a mask with the upper half of val_bits set to 1 */
+		map->write_mask = -(1LL << (config->val_bits/2));
+	}
+
 	if (!bus) {
 		map->reg_read  = config->reg_read;
 		map->reg_write = config->reg_write;
@@ -1272,7 +1303,7 @@ static int _regmap_bus_formatted_write(void *context, unsigned int reg,
 			return ret;
 	}
 
-	map->format.format_write(map, reg, val);
+	regmap_format_write(map, reg, val);
 
 	trace_regmap_hw_write_start(map->dev, reg, 1);
 
@@ -1291,8 +1322,8 @@ static int _regmap_bus_raw_write(void *context, unsigned int reg,
 
 	WARN_ON(!map->bus || !map->format.format_val);
 
-	map->format.format_val(map->work_buf + map->format.reg_bytes
-			       + map->format.pad_bytes, val, 0);
+	regmap_format_val(map, map->work_buf + map->format.reg_bytes
+			  + map->format.pad_bytes, val, 0);
 	return _regmap_raw_write(map, reg,
 				 map->work_buf +
 				 map->format.reg_bytes +
@@ -1629,7 +1660,7 @@ static int _regmap_raw_multi_reg_write(struct regmap *map,
 		trace_regmap_hw_write_start(map->dev, reg, 1);
 		map->format.format_reg(u8, reg, map->reg_shift);
 		u8 += reg_bytes + pad_bytes;
-		map->format.format_val(u8, val, 0);
+		regmap_format_val(map, u8, val, 0);
 		u8 += val_bytes;
 	}
 	u8 = buf;
@@ -2047,7 +2078,7 @@ int regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 			if (ret != 0)
 				goto out;
 
-			map->format.format_val(val + (i * val_bytes), v, 0);
+			regmap_format_val(map, val + (i * val_bytes), v, 0);
 		}
 	}
 
diff --git a/include/linux/regmap.h b/include/linux/regmap.h
index 85691b9..3eda5cf 100644
--- a/include/linux/regmap.h
+++ b/include/linux/regmap.h
@@ -146,6 +146,9 @@ typedef void (*regmap_unlock)(void *);
  *		  This field is a duplicate of a similar file in
  *		  'struct regmap_bus' and serves exact same purpose.
  *		   Use it only for "no-bus" cases.
+ * @write_mask:   The hardware uses the upper half of the register for
+ *                write masking. If this is enabled, regmap will force all
+ *                upper bits to ones during writes.
  * @max_register: Optional, specifies the maximum valid register index.
  * @wr_table:     Optional, points to a struct regmap_access_table specifying
  *                valid ranges for write access.
@@ -203,6 +206,8 @@ struct regmap_config {
 
 	bool fast_io;
 
+	bool write_mask;
+
 	unsigned int max_register;
 	const struct regmap_access_table *wr_table;
 	const struct regmap_access_table *rd_table;
-- 
1.8.3.2

