From 55c1371c79713fb3795a04d369e46680be5ae2bf Mon Sep 17 00:00:00 2001 From: Marc Reilly Date: Fri, 16 Mar 2012 12:11:43 +1100 Subject: [PATCH] regmap: Use pad_bits and reg_bits when determining register format. This change combines any padding bits into the register address bits when determining register format handlers to use the next byte-divisible register size. A reg_shift member is introduced to the regmap struct to enable fixup of the reg format. Format handlers now take an extra parameter specifying the number of bits to shift the value by. Signed-off-by: Marc Reilly Signed-off-by: Mark Brown --- drivers/base/regmap/internal.h | 7 +++++-- drivers/base/regmap/regmap.c | 27 +++++++++++++++------------ 2 files changed, 20 insertions(+), 14 deletions(-) diff --git a/drivers/base/regmap/internal.h b/drivers/base/regmap/internal.h index fcafc5b2e651..606b83d75458 100644 --- a/drivers/base/regmap/internal.h +++ b/drivers/base/regmap/internal.h @@ -26,8 +26,8 @@ struct regmap_format { size_t val_bytes; void (*format_write)(struct regmap *map, unsigned int reg, unsigned int val); - void (*format_reg)(void *buf, unsigned int reg); - void (*format_val)(void *buf, unsigned int val); + void (*format_reg)(void *buf, unsigned int reg, unsigned int shift); + void (*format_val)(void *buf, unsigned int val, unsigned int shift); unsigned int (*parse_val)(void *buf); }; @@ -52,6 +52,9 @@ struct regmap { u8 read_flag_mask; u8 write_flag_mask; + /* number of bits to (left) shift the reg value when formatting*/ + int reg_shift; + /* regcache specific members */ const struct regcache_ops *cache_ops; enum regcache_type cache_type; diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c index 8ffce9bdb481..178989a8949e 100644 --- a/drivers/base/regmap/regmap.c +++ b/drivers/base/regmap/regmap.c @@ -112,34 +112,36 @@ static void regmap_format_10_14_write(struct regmap *map, out[0] = reg >> 2; } -static void regmap_format_8(void *buf, unsigned int val) +static void regmap_format_8(void *buf, unsigned int val, unsigned int shift) { u8 *b = buf; - b[0] = val; + b[0] = val << shift; } -static void regmap_format_16(void *buf, unsigned int val) +static void regmap_format_16(void *buf, unsigned int val, unsigned int shift) { __be16 *b = buf; - b[0] = cpu_to_be16(val); + b[0] = cpu_to_be16(val << shift); } -static void regmap_format_24(void *buf, unsigned int val) +static void regmap_format_24(void *buf, unsigned int val, unsigned int shift) { u8 *b = buf; + val <<= shift; + b[0] = val >> 16; b[1] = val >> 8; b[2] = val; } -static void regmap_format_32(void *buf, unsigned int val) +static void regmap_format_32(void *buf, unsigned int val, unsigned int shift) { __be32 *b = buf; - b[0] = cpu_to_be32(val); + b[0] = cpu_to_be32(val << shift); } static unsigned int regmap_parse_8(void *buf) @@ -210,6 +212,7 @@ struct regmap *regmap_init(struct device *dev, map->format.pad_bytes = config->pad_bits / 8; map->format.val_bytes = DIV_ROUND_UP(config->val_bits, 8); map->format.buf_size += map->format.pad_bytes; + map->reg_shift = config->pad_bits % 8; map->dev = dev; map->bus = bus; map->max_register = config->max_register; @@ -226,7 +229,7 @@ struct regmap *regmap_init(struct device *dev, map->read_flag_mask = bus->read_flag_mask; } - switch (config->reg_bits) { + switch (config->reg_bits + map->reg_shift) { case 2: switch (config->val_bits) { case 6: @@ -454,7 +457,7 @@ static int _regmap_raw_write(struct regmap *map, unsigned int reg, } } - map->format.format_reg(map->work_buf, reg); + map->format.format_reg(map->work_buf, reg, map->reg_shift); u8[0] |= map->write_flag_mask; @@ -529,7 +532,7 @@ int _regmap_write(struct regmap *map, unsigned int reg, return ret; } else { map->format.format_val(map->work_buf + map->format.reg_bytes - + map->format.pad_bytes, val); + + map->format.pad_bytes, val, 0); return _regmap_raw_write(map, reg, map->work_buf + map->format.reg_bytes + @@ -649,7 +652,7 @@ static int _regmap_raw_read(struct regmap *map, unsigned int reg, void *val, u8 *u8 = map->work_buf; int ret; - map->format.format_reg(map->work_buf, reg); + map->format.format_reg(map->work_buf, reg, map->reg_shift); /* * Some buses or devices flag reads by setting the high bits in the @@ -757,7 +760,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); + map->format.format_val(val + (i * val_bytes), v, 0); } } -- 2.34.1