[RFC PATCH v2 2/5] Altered regmap_format_X_X_write functions to account for padding bits

[linuxkernel@fbautosys.co.uk]

From: Christopher Tyerman <c.tyerman@firebladeautomationsystems.co.uk>

Altered regmap_format_X_X_write functions to account for padding bits by
adjusting reg position shifts by value of map->reg_shift

as map->format.format_write is selected based on
(config->reg_bits + map->reg_shift) each regmap_format_X_X_write needs
to account for map->reg_shift or will be misaligned if padding bits
greater than zero

	modified:   drivers/base/regmap/regmap.c

Signed-off-by: Christopher Tyerman <c.tyerman@firebladeautomationsystems.co.uk>
---
 drivers/base/regmap/regmap.c | 31 +++++++++++++++++++++++--------
 1 file changed, 23 insertions(+), 8 deletions(-)

diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c
index 05c104659381..05c65312a9b3 100644
--- a/drivers/base/regmap/regmap.c
+++ b/drivers/base/regmap/regmap.c
@@ -220,9 +220,14 @@ static void regmap_format_12_20_write(struct regmap *map,
 				     unsigned int reg, unsigned int val)
 {
 	u8 *out = map->work_buf;
+	u8 shift = map->reg_shift;
 
-	out[0] = reg >> 4;
-	out[1] = (reg << 4) | (val >> 16);
+	if (shift <= 4)
+		out[0] = reg >> (4 - shift);
+	else
+		out[0] = reg << (shift - 4);
+
+	out[1] = (reg << 4 + shift) | (val >> 16);
 	out[2] = val >> 8;
 	out[3] = val;
 }
@@ -232,42 +237,52 @@ static void regmap_format_2_6_write(struct regmap *map,
 				     unsigned int reg, unsigned int val)
 {
 	u8 *out = map->work_buf;
+	u8 shift = map->reg_shift;
 
-	*out = (reg << 6) | val;
+	*out = (reg << (6 + shift) | val);
 }
 
 static void regmap_format_4_12_write(struct regmap *map,
 				     unsigned int reg, unsigned int val)
 {
+	u8 shift = map->reg_shift;
 	__be16 *out = map->work_buf;
-	*out = cpu_to_be16((reg << 12) | val);
+
+	*out = cpu_to_be16((reg << (12 + shift)) | val);
 }
 
 static void regmap_format_7_9_write(struct regmap *map,
 				    unsigned int reg, unsigned int val)
 {
+	u8 shift = map->reg_shift;
 	__be16 *out = map->work_buf;
-	*out = cpu_to_be16((reg << 9) | val);
+
+	*out = cpu_to_be16((reg << (9 + shift)) | val);
 }
 
 static void regmap_format_7_17_write(struct regmap *map,
 				    unsigned int reg, unsigned int val)
 {
+	u8 shift = map->reg_shift;
 	u8 *out = map->work_buf;
 
 	out[2] = val;
 	out[1] = val >> 8;
-	out[0] = (val >> 16) | (reg << 1);
+	out[0] = (val >> 16) | (reg << (1 + shift));
 }
 
 static void regmap_format_10_14_write(struct regmap *map,
 				    unsigned int reg, unsigned int val)
 {
+	u8 shift = map->reg_shift;
 	u8 *out = map->work_buf;
 
 	out[2] = val;
-	out[1] = (val >> 8) | (reg << 6);
-	out[0] = reg >> 2;
+	out[1] = (val >> 8) | (reg << (6 + shift));
+	if (shift <= 2)
+		out[0] = reg >> (2 - shift);
+	else
+		out[0] = reg << (shift - 2);
 }
 
 static void regmap_format_8(void *buf, unsigned int val, unsigned int shift)
-- 
2.25.1