1 files changed, 225 insertions, 69 deletions

diff --git a/sound/soc/soc-cache.c b/sound/soc/soc-cache.c
index c005ceb70c9d..cad70c9640dc 100644
--- a/sound/soc/soc-cache.c
+++ b/sound/soc/soc-cache.c
@@ -107,12 +107,11 @@ static unsigned int snd_soc_7_9_read(struct snd_soc_codec *codec,
 static int snd_soc_7_9_write(struct snd_soc_codec *codec, unsigned int reg,
 			     unsigned int value)
 {
-	u8 data[2];
+	u16 data;
 
-	data[0] = (reg << 1) | ((value >> 8) & 0x0001);
-	data[1] = value & 0x00ff;
+	data = cpu_to_be16((reg << 9) | (value & 0x1ff));
 
-	return do_hw_write(codec, reg, value, data, 2);
+	return do_hw_write(codec, reg, value, &data, 2);
 }
 
 static int snd_soc_8_8_write(struct snd_soc_codec *codec, unsigned int reg,
@@ -137,10 +136,10 @@ static int snd_soc_8_16_write(struct snd_soc_codec *codec, unsigned int reg,
 			      unsigned int value)
 {
 	u8 data[3];
+	u16 val = cpu_to_be16(value);
 
 	data[0] = reg;
-	data[1] = (value >> 8) & 0xff;
-	data[2] = value & 0xff;
+	memcpy(&data[1], &val, sizeof(val));
 
 	return do_hw_write(codec, reg, value, data, 3);
 }
@@ -243,9 +242,9 @@ static int snd_soc_16_8_write(struct snd_soc_codec *codec, unsigned int reg,
 			      unsigned int value)
 {
 	u8 data[3];
+	u16 rval = cpu_to_be16(reg);
 
-	data[0] = (reg >> 8) & 0xff;
-	data[1] = reg & 0xff;
+	memcpy(data, &rval, sizeof(rval));
 	data[2] = value;
 
 	return do_hw_write(codec, reg, value, data, 3);
@@ -277,14 +276,12 @@ static unsigned int snd_soc_16_16_read(struct snd_soc_codec *codec,
 static int snd_soc_16_16_write(struct snd_soc_codec *codec, unsigned int reg,
 			       unsigned int value)
 {
-	u8 data[4];
+	u16 data[2];
 
-	data[0] = (reg >> 8) & 0xff;
-	data[1] = reg & 0xff;
-	data[2] = (value >> 8) & 0xff;
-	data[3] = value & 0xff;
+	data[0] = cpu_to_be16(reg);
+	data[1] = cpu_to_be16(value);
 
-	return do_hw_write(codec, reg, value, data, 4);
+	return do_hw_write(codec, reg, value, data, sizeof(data));
 }
 
 /* Primitive bulk write support for soc-cache.  The data pointed to by
@@ -486,31 +483,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;
@@ -521,19 +573,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 */
@@ -548,58 +609,141 @@ 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 regtmp;
 	unsigned int val;
 	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);
+			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;
@@ -610,11 +754,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;
@@ -640,6 +801,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);
 	}
 
@@ -652,10 +814,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;
 
@@ -665,32 +826,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