1 files changed, 220 insertions, 472 deletions

diff --git a/sound/soc/soc-cache.c b/sound/soc/soc-cache.c
index 039b953..d9f8ade 100644
--- a/sound/soc/soc-cache.c
+++ b/sound/soc/soc-cache.c
@@ -20,422 +20,6 @@
 
 #include <trace/events/asoc.h>
 
-#ifdef CONFIG_SPI_MASTER
-static int do_spi_write(void *control, const char *data, int len)
-{
-	struct spi_device *spi = control;
-	int ret;
-
-	ret = spi_write(spi, data, len);
-	if (ret < 0)
-		return ret;
-
-	return len;
-}
-#endif
-
-static int do_hw_write(struct snd_soc_codec *codec, unsigned int reg,
-		       unsigned int value, const void *data, int len)
-{
-	int ret;
-
-	if (!snd_soc_codec_volatile_register(codec, reg) &&
-	    reg < codec->driver->reg_cache_size &&
-	    !codec->cache_bypass) {
-		ret = snd_soc_cache_write(codec, reg, value);
-		if (ret < 0)
-			return -1;
-	}
-
-	if (codec->cache_only) {
-		codec->cache_sync = 1;
-		return 0;
-	}
-
-	ret = codec->hw_write(codec->control_data, data, len);
-	if (ret == len)
-		return 0;
-	if (ret < 0)
-		return ret;
-	else
-		return -EIO;
-}
-
-static unsigned int do_hw_read(struct snd_soc_codec *codec, unsigned int reg)
-{
-	int ret;
-	unsigned int val;
-
-	if (reg >= codec->driver->reg_cache_size ||
-	    snd_soc_codec_volatile_register(codec, reg) ||
-	    codec->cache_bypass) {
-		if (codec->cache_only)
-			return -1;
-
-		BUG_ON(!codec->hw_read);
-		return codec->hw_read(codec, reg);
-	}
-
-	ret = snd_soc_cache_read(codec, reg, &val);
-	if (ret < 0)
-		return -1;
-	return val;
-}
-
-static unsigned int snd_soc_4_12_read(struct snd_soc_codec *codec,
-				      unsigned int reg)
-{
-	return do_hw_read(codec, reg);
-}
-
-static int snd_soc_4_12_write(struct snd_soc_codec *codec, unsigned int reg,
-			      unsigned int value)
-{
-	u16 data;
-
-	data = cpu_to_be16((reg << 12) | (value & 0xffffff));
-
-	return do_hw_write(codec, reg, value, &data, 2);
-}
-
-static unsigned int snd_soc_7_9_read(struct snd_soc_codec *codec,
-				     unsigned int reg)
-{
-	return do_hw_read(codec, reg);
-}
-
-static int snd_soc_7_9_write(struct snd_soc_codec *codec, unsigned int reg,
-			     unsigned int value)
-{
-	u8 data[2];
-
-	data[0] = (reg << 1) | ((value >> 8) & 0x0001);
-	data[1] = value & 0x00ff;
-
-	return do_hw_write(codec, reg, value, data, 2);
-}
-
-static int snd_soc_8_8_write(struct snd_soc_codec *codec, unsigned int reg,
-			     unsigned int value)
-{
-	u8 data[2];
-
-	reg &= 0xff;
-	data[0] = reg;
-	data[1] = value & 0xff;
-
-	return do_hw_write(codec, reg, value, data, 2);
-}
-
-static unsigned int snd_soc_8_8_read(struct snd_soc_codec *codec,
-				     unsigned int reg)
-{
-	return do_hw_read(codec, reg);
-}
-
-static int snd_soc_8_16_write(struct snd_soc_codec *codec, unsigned int reg,
-			      unsigned int value)
-{
-	u8 data[3];
-
-	data[0] = reg;
-	data[1] = (value >> 8) & 0xff;
-	data[2] = value & 0xff;
-
-	return do_hw_write(codec, reg, value, data, 3);
-}
-
-static unsigned int snd_soc_8_16_read(struct snd_soc_codec *codec,
-				      unsigned int reg)
-{
-	return do_hw_read(codec, reg);
-}
-
-#if defined(CONFIG_I2C) || (defined(CONFIG_I2C_MODULE) && defined(MODULE))
-static unsigned int do_i2c_read(struct snd_soc_codec *codec,
-				void *reg, int reglen,
-				void *data, int datalen)
-{
-	struct i2c_msg xfer[2];
-	int ret;
-	struct i2c_client *client = codec->control_data;
-
-	/* Write register */
-	xfer[0].addr = client->addr;
-	xfer[0].flags = 0;
-	xfer[0].len = reglen;
-	xfer[0].buf = reg;
-
-	/* Read data */
-	xfer[1].addr = client->addr;
-	xfer[1].flags = I2C_M_RD;
-	xfer[1].len = datalen;
-	xfer[1].buf = data;
-
-	ret = i2c_transfer(client->adapter, xfer, 2);
-	if (ret == 2)
-		return 0;
-	else if (ret < 0)
-		return ret;
-	else
-		return -EIO;
-}
-#endif
-
-#if defined(CONFIG_I2C) || (defined(CONFIG_I2C_MODULE) && defined(MODULE))
-static unsigned int snd_soc_8_8_read_i2c(struct snd_soc_codec *codec,
-					 unsigned int r)
-{
-	u8 reg = r;
-	u8 data;
-	int ret;
-
-	ret = do_i2c_read(codec, &reg, 1, &data, 1);
-	if (ret < 0)
-		return 0;
-	return data;
-}
-#else
-#define snd_soc_8_8_read_i2c NULL
-#endif
-
-#if defined(CONFIG_I2C) || (defined(CONFIG_I2C_MODULE) && defined(MODULE))
-static unsigned int snd_soc_8_16_read_i2c(struct snd_soc_codec *codec,
-					  unsigned int r)
-{
-	u8 reg = r;
-	u16 data;
-	int ret;
-
-	ret = do_i2c_read(codec, &reg, 1, &data, 2);
-	if (ret < 0)
-		return 0;
-	return (data >> 8) | ((data & 0xff) << 8);
-}
-#else
-#define snd_soc_8_16_read_i2c NULL
-#endif
-
-#if defined(CONFIG_I2C) || (defined(CONFIG_I2C_MODULE) && defined(MODULE))
-static unsigned int snd_soc_16_8_read_i2c(struct snd_soc_codec *codec,
-					  unsigned int r)
-{
-	u16 reg = r;
-	u8 data;
-	int ret;
-
-	ret = do_i2c_read(codec, &reg, 2, &data, 1);
-	if (ret < 0)
-		return 0;
-	return data;
-}
-#else
-#define snd_soc_16_8_read_i2c NULL
-#endif
-
-static unsigned int snd_soc_16_8_read(struct snd_soc_codec *codec,
-				      unsigned int reg)
-{
-	return do_hw_read(codec, reg);
-}
-
-static int snd_soc_16_8_write(struct snd_soc_codec *codec, unsigned int reg,
-			      unsigned int value)
-{
-	u8 data[3];
-
-	data[0] = (reg >> 8) & 0xff;
-	data[1] = reg & 0xff;
-	data[2] = value;
-
-	return do_hw_write(codec, reg, value, data, 3);
-}
-
-#if defined(CONFIG_I2C) || (defined(CONFIG_I2C_MODULE) && defined(MODULE))
-static unsigned int snd_soc_16_16_read_i2c(struct snd_soc_codec *codec,
-					   unsigned int r)
-{
-	u16 reg = cpu_to_be16(r);
-	u16 data;
-	int ret;
-
-	ret = do_i2c_read(codec, &reg, 2, &data, 2);
-	if (ret < 0)
-		return 0;
-	return be16_to_cpu(data);
-}
-#else
-#define snd_soc_16_16_read_i2c NULL
-#endif
-
-static unsigned int snd_soc_16_16_read(struct snd_soc_codec *codec,
-				       unsigned int reg)
-{
-	return do_hw_read(codec, reg);
-}
-
-static int snd_soc_16_16_write(struct snd_soc_codec *codec, unsigned int reg,
-			       unsigned int value)
-{
-	u8 data[4];
-
-	data[0] = (reg >> 8) & 0xff;
-	data[1] = reg & 0xff;
-	data[2] = (value >> 8) & 0xff;
-	data[3] = value & 0xff;
-
-	return do_hw_write(codec, reg, value, data, 4);
-}
-
-/* Primitive bulk write support for soc-cache.  The data pointed to by
- * `data' needs to already be in the form the hardware expects
- * including any leading register specific data.  Any data written
- * through this function will not go through the cache as it only
- * handles writing to volatile or out of bounds registers.
- */
-static int snd_soc_hw_bulk_write_raw(struct snd_soc_codec *codec, unsigned int reg,
-				     const void *data, size_t len)
-{
-	int ret;
-
-	/* To ensure that we don't get out of sync with the cache, check
-	 * whether the base register is volatile or if we've directly asked
-	 * to bypass the cache.  Out of bounds registers are considered
-	 * volatile.
-	 */
-	if (!codec->cache_bypass
-	    && !snd_soc_codec_volatile_register(codec, reg)
-	    && reg < codec->driver->reg_cache_size)
-		return -EINVAL;
-
-	switch (codec->control_type) {
-#if defined(CONFIG_I2C) || (defined(CONFIG_I2C_MODULE) && defined(MODULE))
-	case SND_SOC_I2C:
-		ret = i2c_master_send(codec->control_data, data, len);
-		break;
-#endif
-#if defined(CONFIG_SPI_MASTER)
-	case SND_SOC_SPI:
-		ret = spi_write(codec->control_data, data, len);
-		break;
-#endif
-	default:
-		BUG();
-	}
-
-	if (ret == len)
-		return 0;
-	if (ret < 0)
-		return ret;
-	else
-		return -EIO;
-}
-
-static struct {
-	int addr_bits;
-	int data_bits;
-	int (*write)(struct snd_soc_codec *codec, unsigned int, unsigned int);
-	unsigned int (*read)(struct snd_soc_codec *, unsigned int);
-	unsigned int (*i2c_read)(struct snd_soc_codec *, unsigned int);
-} io_types[] = {
-	{
-		.addr_bits = 4, .data_bits = 12,
-		.write = snd_soc_4_12_write, .read = snd_soc_4_12_read,
-	},
-	{
-		.addr_bits = 7, .data_bits = 9,
-		.write = snd_soc_7_9_write, .read = snd_soc_7_9_read,
-	},
-	{
-		.addr_bits = 8, .data_bits = 8,
-		.write = snd_soc_8_8_write, .read = snd_soc_8_8_read,
-		.i2c_read = snd_soc_8_8_read_i2c,
-	},
-	{
-		.addr_bits = 8, .data_bits = 16,
-		.write = snd_soc_8_16_write, .read = snd_soc_8_16_read,
-		.i2c_read = snd_soc_8_16_read_i2c,
-	},
-	{
-		.addr_bits = 16, .data_bits = 8,
-		.write = snd_soc_16_8_write, .read = snd_soc_16_8_read,
-		.i2c_read = snd_soc_16_8_read_i2c,
-	},
-	{
-		.addr_bits = 16, .data_bits = 16,
-		.write = snd_soc_16_16_write, .read = snd_soc_16_16_read,
-		.i2c_read = snd_soc_16_16_read_i2c,
-	},
-};
-
-/**
- * snd_soc_codec_set_cache_io: Set up standard I/O functions.
- *
- * @codec: CODEC to configure.
- * @addr_bits: Number of bits of register address data.
- * @data_bits: Number of bits of data per register.
- * @control: Control bus used.
- *
- * Register formats are frequently shared between many I2C and SPI
- * devices.  In order to promote code reuse the ASoC core provides
- * some standard implementations of CODEC read and write operations
- * which can be set up using this function.
- *
- * The caller is responsible for allocating and initialising the
- * actual cache.
- *
- * Note that at present this code cannot be used by CODECs with
- * volatile registers.
- */
-int snd_soc_codec_set_cache_io(struct snd_soc_codec *codec,
-			       int addr_bits, int data_bits,
-			       enum snd_soc_control_type control)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(io_types); i++)
-		if (io_types[i].addr_bits == addr_bits &&
-		    io_types[i].data_bits == data_bits)
-			break;
-	if (i == ARRAY_SIZE(io_types)) {
-		printk(KERN_ERR
-		       "No I/O functions for %d bit address %d bit data\n",
-		       addr_bits, data_bits);
-		return -EINVAL;
-	}
-
-	codec->write = io_types[i].write;
-	codec->read = io_types[i].read;
-	codec->bulk_write_raw = snd_soc_hw_bulk_write_raw;
-
-	switch (control) {
-	case SND_SOC_I2C:
-#if defined(CONFIG_I2C) || (defined(CONFIG_I2C_MODULE) && defined(MODULE))
-		codec->hw_write = (hw_write_t)i2c_master_send;
-#endif
-		if (io_types[i].i2c_read)
-			codec->hw_read = io_types[i].i2c_read;
-
-		codec->control_data = container_of(codec->dev,
-						   struct i2c_client,
-						   dev);
-		break;
-
-	case SND_SOC_SPI:
-#ifdef CONFIG_SPI_MASTER
-		codec->hw_write = do_spi_write;
-#endif
-
-		codec->control_data = container_of(codec->dev,
-						   struct spi_device,
-						   dev);
-		break;
-	}
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(snd_soc_codec_set_cache_io);
-
 static bool snd_soc_set_cache_val(void *base, unsigned int idx,
 				  unsigned int val, unsigned int word_size)
 {
@@ -483,31 +67,86 @@ static unsigned int snd_soc_get_cache_val(const void *base, unsigned int idx,
 }
 
 struct snd_soc_rbtree_node {
-	struct rb_node node;
-	unsigned int reg;
-	unsigned int value;
-	unsigned int defval;
+	struct rb_node node; /* the actual rbtree node holding this block */
+	unsigned int base_reg; /* base register handled by this block */
+	unsigned int word_size; /* number of bytes needed to represent the register index */
+	void *block; /* block of adjacent registers */
+	unsigned int blklen; /* number of registers available in the block */
 } __attribute__ ((packed));
 
 struct snd_soc_rbtree_ctx {
 	struct rb_root root;
+	struct snd_soc_rbtree_node *cached_rbnode;
 };
 
+static inline void snd_soc_rbtree_get_base_top_reg(
+	struct snd_soc_rbtree_node *rbnode,
+	unsigned int *base, unsigned int *top)
+{
+	*base = rbnode->base_reg;
+	*top = rbnode->base_reg + rbnode->blklen - 1;
+}
+
+static unsigned int snd_soc_rbtree_get_register(
+	struct snd_soc_rbtree_node *rbnode, unsigned int idx)
+{
+	unsigned int val;
+
+	switch (rbnode->word_size) {
+	case 1: {
+		u8 *p = rbnode->block;
+		val = p[idx];
+		return val;
+	}
+	case 2: {
+		u16 *p = rbnode->block;
+		val = p[idx];
+		return val;
+	}
+	default:
+		BUG();
+		break;
+	}
+	return -1;
+}
+
+static void snd_soc_rbtree_set_register(struct snd_soc_rbtree_node *rbnode,
+					unsigned int idx, unsigned int val)
+{
+	switch (rbnode->word_size) {
+	case 1: {
+		u8 *p = rbnode->block;
+		p[idx] = val;
+		break;
+	}
+	case 2: {
+		u16 *p = rbnode->block;
+		p[idx] = val;
+		break;
+	}
+	default:
+		BUG();
+		break;
+	}
+}
+
 static struct snd_soc_rbtree_node *snd_soc_rbtree_lookup(
 	struct rb_root *root, unsigned int reg)
 {
 	struct rb_node *node;
 	struct snd_soc_rbtree_node *rbnode;
+	unsigned int base_reg, top_reg;
 
 	node = root->rb_node;
 	while (node) {
 		rbnode = container_of(node, struct snd_soc_rbtree_node, node);
-		if (rbnode->reg < reg)
-			node = node->rb_left;
-		else if (rbnode->reg > reg)
-			node = node->rb_right;
-		else
+		snd_soc_rbtree_get_base_top_reg(rbnode, &base_reg, &top_reg);
+		if (reg >= base_reg && reg <= top_reg)
 			return rbnode;
+		else if (reg > top_reg)
+			node = node->rb_right;
+		else if (reg < base_reg)
+			node = node->rb_left;
 	}
 
 	return NULL;
@@ -518,19 +157,28 @@ static int snd_soc_rbtree_insert(struct rb_root *root,
 {
 	struct rb_node **new, *parent;
 	struct snd_soc_rbtree_node *rbnode_tmp;
+	unsigned int base_reg_tmp, top_reg_tmp;
+	unsigned int base_reg;
 
 	parent = NULL;
 	new = &root->rb_node;
 	while (*new) {
 		rbnode_tmp = container_of(*new, struct snd_soc_rbtree_node,
 					  node);
+		/* base and top registers of the current rbnode */
+		snd_soc_rbtree_get_base_top_reg(rbnode_tmp, &base_reg_tmp,
+						&top_reg_tmp);
+		/* base register of the rbnode to be added */
+		base_reg = rbnode->base_reg;
 		parent = *new;
-		if (rbnode_tmp->reg < rbnode->reg)
-			new = &((*new)->rb_left);
-		else if (rbnode_tmp->reg > rbnode->reg)
-			new = &((*new)->rb_right);
-		else
+		/* if this register has already been inserted, just return */
+		if (base_reg >= base_reg_tmp &&
+		    base_reg <= top_reg_tmp)
 			return 0;
+		else if (base_reg > top_reg_tmp)
+			new = &((*new)->rb_right);
+		else if (base_reg < base_reg_tmp)
+			new = &((*new)->rb_left);
 	}
 
 	/* insert the node into the rbtree */
@@ -545,58 +193,146 @@ static int snd_soc_rbtree_cache_sync(struct snd_soc_codec *codec)
 	struct snd_soc_rbtree_ctx *rbtree_ctx;
 	struct rb_node *node;
 	struct snd_soc_rbtree_node *rbnode;
-	unsigned int val;
+	unsigned int regtmp;
+	unsigned int val, def;
 	int ret;
+	int i;
 
 	rbtree_ctx = codec->reg_cache;
 	for (node = rb_first(&rbtree_ctx->root); node; node = rb_next(node)) {
 		rbnode = rb_entry(node, struct snd_soc_rbtree_node, node);
-		if (rbnode->value == rbnode->defval)
-			continue;
-		WARN_ON(codec->writable_register &&
-			codec->writable_register(codec, rbnode->reg));
-		ret = snd_soc_cache_read(codec, rbnode->reg, &val);
-		if (ret)
-			return ret;
-		codec->cache_bypass = 1;
-		ret = snd_soc_write(codec, rbnode->reg, val);
-		codec->cache_bypass = 0;
-		if (ret)
-			return ret;
-		dev_dbg(codec->dev, "Synced register %#x, value = %#x\n",
-			rbnode->reg, val);
+		for (i = 0; i < rbnode->blklen; ++i) {
+			regtmp = rbnode->base_reg + i;
+			WARN_ON(codec->writable_register &&
+				codec->writable_register(codec, regtmp));
+			val = snd_soc_rbtree_get_register(rbnode, i);
+			def = snd_soc_get_cache_val(codec->reg_def_copy, i,
+						    rbnode->word_size);
+			if (val == def)
+				continue;
+
+			codec->cache_bypass = 1;
+			ret = snd_soc_write(codec, regtmp, val);
+			codec->cache_bypass = 0;
+			if (ret)
+				return ret;
+			dev_dbg(codec->dev, "Synced register %#x, value = %#x\n",
+				regtmp, val);
+		}
 	}
 
 	return 0;
 }
 
+static int snd_soc_rbtree_insert_to_block(struct snd_soc_rbtree_node *rbnode,
+					  unsigned int pos, unsigned int reg,
+					  unsigned int value)
+{
+	u8 *blk;
+
+	blk = krealloc(rbnode->block,
+		       (rbnode->blklen + 1) * rbnode->word_size, GFP_KERNEL);
+	if (!blk)
+		return -ENOMEM;
+
+	/* insert the register value in the correct place in the rbnode block */
+	memmove(blk + (pos + 1) * rbnode->word_size,
+		blk + pos * rbnode->word_size,
+		(rbnode->blklen - pos) * rbnode->word_size);
+
+	/* update the rbnode block, its size and the base register */
+	rbnode->block = blk;
+	rbnode->blklen++;
+	if (!pos)
+		rbnode->base_reg = reg;
+
+	snd_soc_rbtree_set_register(rbnode, pos, value);
+	return 0;
+}
+
 static int snd_soc_rbtree_cache_write(struct snd_soc_codec *codec,
 				      unsigned int reg, unsigned int value)
 {
 	struct snd_soc_rbtree_ctx *rbtree_ctx;
-	struct snd_soc_rbtree_node *rbnode;
+	struct snd_soc_rbtree_node *rbnode, *rbnode_tmp;
+	struct rb_node *node;
+	unsigned int val;
+	unsigned int reg_tmp;
+	unsigned int base_reg, top_reg;
+	unsigned int pos;
+	int i;
+	int ret;
 
 	rbtree_ctx = codec->reg_cache;
+	/* look up the required register in the cached rbnode */
+	rbnode = rbtree_ctx->cached_rbnode;
+	if (rbnode) {
+		snd_soc_rbtree_get_base_top_reg(rbnode, &base_reg, &top_reg);
+		if (reg >= base_reg && reg <= top_reg) {
+			reg_tmp = reg - base_reg;
+			val = snd_soc_rbtree_get_register(rbnode, reg_tmp);
+			if (val == value)
+				return 0;
+			snd_soc_rbtree_set_register(rbnode, reg_tmp, value);
+			return 0;
+		}
+	}
+	/* if we can't locate it in the cached rbnode we'll have
+	 * to traverse the rbtree looking for it.
+	 */
 	rbnode = snd_soc_rbtree_lookup(&rbtree_ctx->root, reg);
 	if (rbnode) {
-		if (rbnode->value == value)
+		reg_tmp = reg - rbnode->base_reg;
+		val = snd_soc_rbtree_get_register(rbnode, reg_tmp);
+		if (val == value)
 			return 0;
-		rbnode->value = value;
+		snd_soc_rbtree_set_register(rbnode, reg_tmp, value);
+		rbtree_ctx->cached_rbnode = rbnode;
 	} else {
 		/* bail out early, no need to create the rbnode yet */
 		if (!value)
 			return 0;
-		/*
-		 * for uninitialized registers whose value is changed
-		 * from the default zero, create an rbnode and insert
-		 * it into the tree.
+		/* look for an adjacent register to the one we are about to add */
+		for (node = rb_first(&rbtree_ctx->root); node;
+		     node = rb_next(node)) {
+			rbnode_tmp = rb_entry(node, struct snd_soc_rbtree_node, node);
+			for (i = 0; i < rbnode_tmp->blklen; ++i) {
+				reg_tmp = rbnode_tmp->base_reg + i;
+				if (abs(reg_tmp - reg) != 1)
+					continue;
+				/* decide where in the block to place our register */
+				if (reg_tmp + 1 == reg)
+					pos = i + 1;
+				else
+					pos = i;
+				ret = snd_soc_rbtree_insert_to_block(rbnode_tmp, pos,
+								     reg, value);
+				if (ret)
+					return ret;
+				rbtree_ctx->cached_rbnode = rbnode_tmp;
+				return 0;
+			}
+		}
+		/* we did not manage to find a place to insert it in an existing
+		 * block so create a new rbnode with a single register in its block.
+		 * This block will get populated further if any other adjacent
+		 * registers get modified in the future.
 		 */
 		rbnode = kzalloc(sizeof *rbnode, GFP_KERNEL);
 		if (!rbnode)
 			return -ENOMEM;
-		rbnode->reg = reg;
-		rbnode->value = value;
+		rbnode->blklen = 1;
+		rbnode->base_reg = reg;
+		rbnode->word_size = codec->driver->reg_word_size;
+		rbnode->block = kmalloc(rbnode->blklen * rbnode->word_size,
+					GFP_KERNEL);
+		if (!rbnode->block) {
+			kfree(rbnode);
+			return -ENOMEM;
+		}
+		snd_soc_rbtree_set_register(rbnode, 0, value);
 		snd_soc_rbtree_insert(&rbtree_ctx->root, rbnode);
+		rbtree_ctx->cached_rbnode = rbnode;
 	}
 
 	return 0;
@@ -607,11 +343,28 @@ static int snd_soc_rbtree_cache_read(struct snd_soc_codec *codec,
 {
 	struct snd_soc_rbtree_ctx *rbtree_ctx;
 	struct snd_soc_rbtree_node *rbnode;
+	unsigned int base_reg, top_reg;
+	unsigned int reg_tmp;
 
 	rbtree_ctx = codec->reg_cache;
+	/* look up the required register in the cached rbnode */
+	rbnode = rbtree_ctx->cached_rbnode;
+	if (rbnode) {
+		snd_soc_rbtree_get_base_top_reg(rbnode, &base_reg, &top_reg);
+		if (reg >= base_reg && reg <= top_reg) {
+			reg_tmp = reg - base_reg;
+			*value = snd_soc_rbtree_get_register(rbnode, reg_tmp);
+			return 0;
+		}
+	}
+	/* if we can't locate it in the cached rbnode we'll have
+	 * to traverse the rbtree looking for it.
+	 */
 	rbnode = snd_soc_rbtree_lookup(&rbtree_ctx->root, reg);
 	if (rbnode) {
-		*value = rbnode->value;
+		reg_tmp = reg - rbnode->base_reg;
+		*value = snd_soc_rbtree_get_register(rbnode, reg_tmp);
+		rbtree_ctx->cached_rbnode = rbnode;
 	} else {
 		/* uninitialized registers default to 0 */
 		*value = 0;
@@ -637,6 +390,7 @@ static int snd_soc_rbtree_cache_exit(struct snd_soc_codec *codec)
 		rbtree_node = rb_entry(next, struct snd_soc_rbtree_node, node);
 		next = rb_next(&rbtree_node->node);
 		rb_erase(&rbtree_node->node, &rbtree_ctx->root);
+		kfree(rbtree_node->block);
 		kfree(rbtree_node);
 	}
 
@@ -649,10 +403,9 @@ static int snd_soc_rbtree_cache_exit(struct snd_soc_codec *codec)
 
 static int snd_soc_rbtree_cache_init(struct snd_soc_codec *codec)
 {
-	struct snd_soc_rbtree_node *rbtree_node;
 	struct snd_soc_rbtree_ctx *rbtree_ctx;
-	unsigned int val;
 	unsigned int word_size;
+	unsigned int val;
 	int i;
 	int ret;
 
@@ -662,32 +415,27 @@ static int snd_soc_rbtree_cache_init(struct snd_soc_codec *codec)
 
 	rbtree_ctx = codec->reg_cache;
 	rbtree_ctx->root = RB_ROOT;
+	rbtree_ctx->cached_rbnode = NULL;
 
 	if (!codec->reg_def_copy)
 		return 0;
 
-	/*
-	 * populate the rbtree with the initialized registers.  All other
-	 * registers will be inserted when they are first modified.
-	 */
 	word_size = codec->driver->reg_word_size;
 	for (i = 0; i < codec->driver->reg_cache_size; ++i) {
-		val = snd_soc_get_cache_val(codec->reg_def_copy, i, word_size);
+		val = snd_soc_get_cache_val(codec->reg_def_copy, i,
+					    word_size);
 		if (!val)
 			continue;
-		rbtree_node = kzalloc(sizeof *rbtree_node, GFP_KERNEL);
-		if (!rbtree_node) {
-			ret = -ENOMEM;
-			snd_soc_cache_exit(codec);
-			break;
-		}
-		rbtree_node->reg = i;
-		rbtree_node->value = val;
-		rbtree_node->defval = val;
-		snd_soc_rbtree_insert(&rbtree_ctx->root, rbtree_node);
+		ret = snd_soc_rbtree_cache_write(codec, i, val);
+		if (ret)
+			goto err;
 	}
 
 	return 0;
+
+err:
+	snd_soc_cache_exit(codec);
+	return ret;
 }
 
 #ifdef CONFIG_SND_SOC_CACHE_LZO