soc-cache.c 27.0 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13
/*
 * soc-cache.c  --  ASoC register cache helpers
 *
 * Copyright 2009 Wolfson Microelectronics PLC.
 *
 * Author: Mark Brown <broonie@opensource.wolfsonmicro.com>
 *
 *  This program is free software; you can redistribute  it and/or modify it
 *  under  the terms of  the GNU General  Public License as published by the
 *  Free Software Foundation;  either version 2 of the  License, or (at your
 *  option) any later version.
 */

14
#include <linux/i2c.h>
15
#include <linux/spi/spi.h>
16
#include <sound/soc.h>
17 18
#include <linux/lzo.h>
#include <linux/bitmap.h>
19
#include <linux/rbtree.h>
20

21 22
#include <trace/events/asoc.h>

23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49
static bool snd_soc_set_cache_val(void *base, unsigned int idx,
				  unsigned int val, unsigned int word_size)
{
	switch (word_size) {
	case 1: {
		u8 *cache = base;
		if (cache[idx] == val)
			return true;
		cache[idx] = val;
		break;
	}
	case 2: {
		u16 *cache = base;
		if (cache[idx] == val)
			return true;
		cache[idx] = val;
		break;
	}
	default:
		BUG();
	}
	return false;
}

static unsigned int snd_soc_get_cache_val(const void *base, unsigned int idx,
		unsigned int word_size)
{
50 51 52
	if (!base)
		return -1;

53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68
	switch (word_size) {
	case 1: {
		const u8 *cache = base;
		return cache[idx];
	}
	case 2: {
		const u16 *cache = base;
		return cache[idx];
	}
	default:
		BUG();
	}
	/* unreachable */
	return -1;
}

69
struct snd_soc_rbtree_node {
70 71 72 73 74
	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 */
75 76 77 78
} __attribute__ ((packed));

struct snd_soc_rbtree_ctx {
	struct rb_root root;
79
	struct snd_soc_rbtree_node *cached_rbnode;
80 81
};

82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132
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;
	}
}

133 134 135 136 137
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;
138
	unsigned int base_reg, top_reg;
139 140 141 142

	node = root->rb_node;
	while (node) {
		rbnode = container_of(node, struct snd_soc_rbtree_node, node);
143 144
		snd_soc_rbtree_get_base_top_reg(rbnode, &base_reg, &top_reg);
		if (reg >= base_reg && reg <= top_reg)
145
			return rbnode;
146 147 148 149
		else if (reg > top_reg)
			node = node->rb_right;
		else if (reg < base_reg)
			node = node->rb_left;
150 151 152 153 154 155 156 157 158 159
	}

	return NULL;
}

static int snd_soc_rbtree_insert(struct rb_root *root,
				 struct snd_soc_rbtree_node *rbnode)
{
	struct rb_node **new, *parent;
	struct snd_soc_rbtree_node *rbnode_tmp;
160 161
	unsigned int base_reg_tmp, top_reg_tmp;
	unsigned int base_reg;
162 163 164 165 166 167

	parent = NULL;
	new = &root->rb_node;
	while (*new) {
		rbnode_tmp = container_of(*new, struct snd_soc_rbtree_node,
					  node);
168 169 170 171 172
		/* 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;
173
		parent = *new;
174 175 176
		/* if this register has already been inserted, just return */
		if (base_reg >= base_reg_tmp &&
		    base_reg <= top_reg_tmp)
177
			return 0;
178 179 180 181
		else if (base_reg > top_reg_tmp)
			new = &((*new)->rb_right);
		else if (base_reg < base_reg_tmp)
			new = &((*new)->rb_left);
182 183 184 185 186 187 188 189 190 191 192 193 194 195
	}

	/* insert the node into the rbtree */
	rb_link_node(&rbnode->node, parent, new);
	rb_insert_color(&rbnode->node, root);

	return 1;
}

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;
196
	unsigned int regtmp;
197
	unsigned int val, def;
198
	int ret;
199
	int i;
200 201 202 203

	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);
204 205 206 207 208
		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);
209 210 211 212 213
			def = snd_soc_get_cache_val(codec->reg_def_copy, i,
						    rbnode->word_size);
			if (val == def)
				continue;

214 215 216 217 218 219 220 221
			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);
		}
222 223 224 225 226
	}

	return 0;
}

227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252
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;
}

253 254 255 256
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;
257 258 259 260
	struct snd_soc_rbtree_node *rbnode, *rbnode_tmp;
	struct rb_node *node;
	unsigned int val;
	unsigned int reg_tmp;
261
	unsigned int base_reg, top_reg;
262 263 264
	unsigned int pos;
	int i;
	int ret;
265 266

	rbtree_ctx = codec->reg_cache;
267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282
	/* 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.
	 */
283 284
	rbnode = snd_soc_rbtree_lookup(&rbtree_ctx->root, reg);
	if (rbnode) {
285 286 287
		reg_tmp = reg - rbnode->base_reg;
		val = snd_soc_rbtree_get_register(rbnode, reg_tmp);
		if (val == value)
288
			return 0;
289
		snd_soc_rbtree_set_register(rbnode, reg_tmp, value);
290
		rbtree_ctx->cached_rbnode = rbnode;
291 292 293 294
	} else {
		/* bail out early, no need to create the rbnode yet */
		if (!value)
			return 0;
295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311
		/* 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;
312
				rbtree_ctx->cached_rbnode = rbnode_tmp;
313 314 315 316 317 318 319
				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.
320 321 322 323
		 */
		rbnode = kzalloc(sizeof *rbnode, GFP_KERNEL);
		if (!rbnode)
			return -ENOMEM;
324 325 326 327 328 329 330 331 332 333
		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);
334
		snd_soc_rbtree_insert(&rbtree_ctx->root, rbnode);
335
		rbtree_ctx->cached_rbnode = rbnode;
336 337 338 339 340 341 342 343 344 345
	}

	return 0;
}

static int snd_soc_rbtree_cache_read(struct snd_soc_codec *codec,
				     unsigned int reg, unsigned int *value)
{
	struct snd_soc_rbtree_ctx *rbtree_ctx;
	struct snd_soc_rbtree_node *rbnode;
346
	unsigned int base_reg, top_reg;
347
	unsigned int reg_tmp;
348 349

	rbtree_ctx = codec->reg_cache;
350 351 352 353 354 355 356 357 358 359 360 361 362
	/* 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.
	 */
363 364
	rbnode = snd_soc_rbtree_lookup(&rbtree_ctx->root, reg);
	if (rbnode) {
365 366
		reg_tmp = reg - rbnode->base_reg;
		*value = snd_soc_rbtree_get_register(rbnode, reg_tmp);
367
		rbtree_ctx->cached_rbnode = rbnode;
368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392
	} else {
		/* uninitialized registers default to 0 */
		*value = 0;
	}

	return 0;
}

static int snd_soc_rbtree_cache_exit(struct snd_soc_codec *codec)
{
	struct rb_node *next;
	struct snd_soc_rbtree_ctx *rbtree_ctx;
	struct snd_soc_rbtree_node *rbtree_node;

	/* if we've already been called then just return */
	rbtree_ctx = codec->reg_cache;
	if (!rbtree_ctx)
		return 0;

	/* free up the rbtree */
	next = rb_first(&rbtree_ctx->root);
	while (next) {
		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);
393
		kfree(rbtree_node->block);
394 395 396 397 398 399 400 401 402 403 404 405 406
		kfree(rbtree_node);
	}

	/* release the resources */
	kfree(codec->reg_cache);
	codec->reg_cache = NULL;

	return 0;
}

static int snd_soc_rbtree_cache_init(struct snd_soc_codec *codec)
{
	struct snd_soc_rbtree_ctx *rbtree_ctx;
407
	unsigned int word_size;
408
	unsigned int val;
409 410
	int i;
	int ret;
411 412 413 414 415 416 417

	codec->reg_cache = kmalloc(sizeof *rbtree_ctx, GFP_KERNEL);
	if (!codec->reg_cache)
		return -ENOMEM;

	rbtree_ctx = codec->reg_cache;
	rbtree_ctx->root = RB_ROOT;
418
	rbtree_ctx->cached_rbnode = NULL;
419

420
	if (!codec->reg_def_copy)
421 422
		return 0;

423 424
	word_size = codec->driver->reg_word_size;
	for (i = 0; i < codec->driver->reg_cache_size; ++i) {
425 426
		val = snd_soc_get_cache_val(codec->reg_def_copy, i,
					    word_size);
427 428
		if (!val)
			continue;
429 430 431
		ret = snd_soc_rbtree_cache_write(codec, i, val);
		if (ret)
			goto err;
432 433 434
	}

	return 0;
435 436 437 438

err:
	snd_soc_cache_exit(codec);
	return ret;
439 440
}

441
#ifdef CONFIG_SND_SOC_CACHE_LZO
442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531
struct snd_soc_lzo_ctx {
	void *wmem;
	void *dst;
	const void *src;
	size_t src_len;
	size_t dst_len;
	size_t decompressed_size;
	unsigned long *sync_bmp;
	int sync_bmp_nbits;
};

#define LZO_BLOCK_NUM 8
static int snd_soc_lzo_block_count(void)
{
	return LZO_BLOCK_NUM;
}

static int snd_soc_lzo_prepare(struct snd_soc_lzo_ctx *lzo_ctx)
{
	lzo_ctx->wmem = kmalloc(LZO1X_MEM_COMPRESS, GFP_KERNEL);
	if (!lzo_ctx->wmem)
		return -ENOMEM;
	return 0;
}

static int snd_soc_lzo_compress(struct snd_soc_lzo_ctx *lzo_ctx)
{
	size_t compress_size;
	int ret;

	ret = lzo1x_1_compress(lzo_ctx->src, lzo_ctx->src_len,
			       lzo_ctx->dst, &compress_size, lzo_ctx->wmem);
	if (ret != LZO_E_OK || compress_size > lzo_ctx->dst_len)
		return -EINVAL;
	lzo_ctx->dst_len = compress_size;
	return 0;
}

static int snd_soc_lzo_decompress(struct snd_soc_lzo_ctx *lzo_ctx)
{
	size_t dst_len;
	int ret;

	dst_len = lzo_ctx->dst_len;
	ret = lzo1x_decompress_safe(lzo_ctx->src, lzo_ctx->src_len,
				    lzo_ctx->dst, &dst_len);
	if (ret != LZO_E_OK || dst_len != lzo_ctx->dst_len)
		return -EINVAL;
	return 0;
}

static int snd_soc_lzo_compress_cache_block(struct snd_soc_codec *codec,
		struct snd_soc_lzo_ctx *lzo_ctx)
{
	int ret;

	lzo_ctx->dst_len = lzo1x_worst_compress(PAGE_SIZE);
	lzo_ctx->dst = kmalloc(lzo_ctx->dst_len, GFP_KERNEL);
	if (!lzo_ctx->dst) {
		lzo_ctx->dst_len = 0;
		return -ENOMEM;
	}

	ret = snd_soc_lzo_compress(lzo_ctx);
	if (ret < 0)
		return ret;
	return 0;
}

static int snd_soc_lzo_decompress_cache_block(struct snd_soc_codec *codec,
		struct snd_soc_lzo_ctx *lzo_ctx)
{
	int ret;

	lzo_ctx->dst_len = lzo_ctx->decompressed_size;
	lzo_ctx->dst = kmalloc(lzo_ctx->dst_len, GFP_KERNEL);
	if (!lzo_ctx->dst) {
		lzo_ctx->dst_len = 0;
		return -ENOMEM;
	}

	ret = snd_soc_lzo_decompress(lzo_ctx);
	if (ret < 0)
		return ret;
	return 0;
}

static inline int snd_soc_lzo_get_blkindex(struct snd_soc_codec *codec,
		unsigned int reg)
{
532
	const struct snd_soc_codec_driver *codec_drv;
533 534 535

	codec_drv = codec->driver;
	return (reg * codec_drv->reg_word_size) /
536
	       DIV_ROUND_UP(codec->reg_size, snd_soc_lzo_block_count());
537 538 539 540 541
}

static inline int snd_soc_lzo_get_blkpos(struct snd_soc_codec *codec,
		unsigned int reg)
{
542
	const struct snd_soc_codec_driver *codec_drv;
543 544

	codec_drv = codec->driver;
545
	return reg % (DIV_ROUND_UP(codec->reg_size, snd_soc_lzo_block_count()) /
546 547 548 549 550
		      codec_drv->reg_word_size);
}

static inline int snd_soc_lzo_get_blksize(struct snd_soc_codec *codec)
{
551
	const struct snd_soc_codec_driver *codec_drv;
552 553

	codec_drv = codec->driver;
554
	return DIV_ROUND_UP(codec->reg_size, snd_soc_lzo_block_count());
555 556 557 558 559 560 561
}

static int snd_soc_lzo_cache_sync(struct snd_soc_codec *codec)
{
	struct snd_soc_lzo_ctx **lzo_blocks;
	unsigned int val;
	int i;
562
	int ret;
563 564 565

	lzo_blocks = codec->reg_cache;
	for_each_set_bit(i, lzo_blocks[0]->sync_bmp, lzo_blocks[0]->sync_bmp_nbits) {
566 567
		WARN_ON(codec->writable_register &&
			codec->writable_register(codec, i));
568 569 570
		ret = snd_soc_cache_read(codec, i, &val);
		if (ret)
			return ret;
571
		codec->cache_bypass = 1;
572
		ret = snd_soc_write(codec, i, val);
573
		codec->cache_bypass = 0;
574 575
		if (ret)
			return ret;
576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615
		dev_dbg(codec->dev, "Synced register %#x, value = %#x\n",
			i, val);
	}

	return 0;
}

static int snd_soc_lzo_cache_write(struct snd_soc_codec *codec,
				   unsigned int reg, unsigned int value)
{
	struct snd_soc_lzo_ctx *lzo_block, **lzo_blocks;
	int ret, blkindex, blkpos;
	size_t blksize, tmp_dst_len;
	void *tmp_dst;

	/* index of the compressed lzo block */
	blkindex = snd_soc_lzo_get_blkindex(codec, reg);
	/* register index within the decompressed block */
	blkpos = snd_soc_lzo_get_blkpos(codec, reg);
	/* size of the compressed block */
	blksize = snd_soc_lzo_get_blksize(codec);
	lzo_blocks = codec->reg_cache;
	lzo_block = lzo_blocks[blkindex];

	/* save the pointer and length of the compressed block */
	tmp_dst = lzo_block->dst;
	tmp_dst_len = lzo_block->dst_len;

	/* prepare the source to be the compressed block */
	lzo_block->src = lzo_block->dst;
	lzo_block->src_len = lzo_block->dst_len;

	/* decompress the block */
	ret = snd_soc_lzo_decompress_cache_block(codec, lzo_block);
	if (ret < 0) {
		kfree(lzo_block->dst);
		goto out;
	}

	/* write the new value to the cache */
616 617 618 619
	if (snd_soc_set_cache_val(lzo_block->dst, blkpos, value,
				  codec->driver->reg_word_size)) {
		kfree(lzo_block->dst);
		goto out;
620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672
	}

	/* prepare the source to be the decompressed block */
	lzo_block->src = lzo_block->dst;
	lzo_block->src_len = lzo_block->dst_len;

	/* compress the block */
	ret = snd_soc_lzo_compress_cache_block(codec, lzo_block);
	if (ret < 0) {
		kfree(lzo_block->dst);
		kfree(lzo_block->src);
		goto out;
	}

	/* set the bit so we know we have to sync this register */
	set_bit(reg, lzo_block->sync_bmp);
	kfree(tmp_dst);
	kfree(lzo_block->src);
	return 0;
out:
	lzo_block->dst = tmp_dst;
	lzo_block->dst_len = tmp_dst_len;
	return ret;
}

static int snd_soc_lzo_cache_read(struct snd_soc_codec *codec,
				  unsigned int reg, unsigned int *value)
{
	struct snd_soc_lzo_ctx *lzo_block, **lzo_blocks;
	int ret, blkindex, blkpos;
	size_t blksize, tmp_dst_len;
	void *tmp_dst;

	*value = 0;
	/* index of the compressed lzo block */
	blkindex = snd_soc_lzo_get_blkindex(codec, reg);
	/* register index within the decompressed block */
	blkpos = snd_soc_lzo_get_blkpos(codec, reg);
	/* size of the compressed block */
	blksize = snd_soc_lzo_get_blksize(codec);
	lzo_blocks = codec->reg_cache;
	lzo_block = lzo_blocks[blkindex];

	/* save the pointer and length of the compressed block */
	tmp_dst = lzo_block->dst;
	tmp_dst_len = lzo_block->dst_len;

	/* prepare the source to be the compressed block */
	lzo_block->src = lzo_block->dst;
	lzo_block->src_len = lzo_block->dst_len;

	/* decompress the block */
	ret = snd_soc_lzo_decompress_cache_block(codec, lzo_block);
673
	if (ret >= 0)
674
		/* fetch the value from the cache */
675 676
		*value = snd_soc_get_cache_val(lzo_block->dst, blkpos,
					       codec->driver->reg_word_size);
677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717

	kfree(lzo_block->dst);
	/* restore the pointer and length of the compressed block */
	lzo_block->dst = tmp_dst;
	lzo_block->dst_len = tmp_dst_len;
	return 0;
}

static int snd_soc_lzo_cache_exit(struct snd_soc_codec *codec)
{
	struct snd_soc_lzo_ctx **lzo_blocks;
	int i, blkcount;

	lzo_blocks = codec->reg_cache;
	if (!lzo_blocks)
		return 0;

	blkcount = snd_soc_lzo_block_count();
	/*
	 * the pointer to the bitmap used for syncing the cache
	 * is shared amongst all lzo_blocks.  Ensure it is freed
	 * only once.
	 */
	if (lzo_blocks[0])
		kfree(lzo_blocks[0]->sync_bmp);
	for (i = 0; i < blkcount; ++i) {
		if (lzo_blocks[i]) {
			kfree(lzo_blocks[i]->wmem);
			kfree(lzo_blocks[i]->dst);
		}
		/* each lzo_block is a pointer returned by kmalloc or NULL */
		kfree(lzo_blocks[i]);
	}
	kfree(lzo_blocks);
	codec->reg_cache = NULL;
	return 0;
}

static int snd_soc_lzo_cache_init(struct snd_soc_codec *codec)
{
	struct snd_soc_lzo_ctx **lzo_blocks;
718
	size_t bmp_size;
719
	const struct snd_soc_codec_driver *codec_drv;
720 721 722 723 724 725 726 727 728 729 730 731 732
	int ret, tofree, i, blksize, blkcount;
	const char *p, *end;
	unsigned long *sync_bmp;

	ret = 0;
	codec_drv = codec->driver;

	/*
	 * If we have not been given a default register cache
	 * then allocate a dummy zero-ed out region, compress it
	 * and remember to free it afterwards.
	 */
	tofree = 0;
733
	if (!codec->reg_def_copy)
734 735
		tofree = 1;

736
	if (!codec->reg_def_copy) {
737
		codec->reg_def_copy = kzalloc(codec->reg_size, GFP_KERNEL);
738
		if (!codec->reg_def_copy)
739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757
			return -ENOMEM;
	}

	blkcount = snd_soc_lzo_block_count();
	codec->reg_cache = kzalloc(blkcount * sizeof *lzo_blocks,
				   GFP_KERNEL);
	if (!codec->reg_cache) {
		ret = -ENOMEM;
		goto err_tofree;
	}
	lzo_blocks = codec->reg_cache;

	/*
	 * allocate a bitmap to be used when syncing the cache with
	 * the hardware.  Each time a register is modified, the corresponding
	 * bit is set in the bitmap, so we know that we have to sync
	 * that register.
	 */
	bmp_size = codec_drv->reg_cache_size;
758
	sync_bmp = kmalloc(BITS_TO_LONGS(bmp_size) * sizeof(long),
759 760 761 762 763
			   GFP_KERNEL);
	if (!sync_bmp) {
		ret = -ENOMEM;
		goto err;
	}
764
	bitmap_zero(sync_bmp, bmp_size);
765 766 767 768 769 770 771 772 773 774 775

	/* allocate the lzo blocks and initialize them */
	for (i = 0; i < blkcount; ++i) {
		lzo_blocks[i] = kzalloc(sizeof **lzo_blocks,
					GFP_KERNEL);
		if (!lzo_blocks[i]) {
			kfree(sync_bmp);
			ret = -ENOMEM;
			goto err;
		}
		lzo_blocks[i]->sync_bmp = sync_bmp;
776
		lzo_blocks[i]->sync_bmp_nbits = bmp_size;
777 778 779 780 781 782 783
		/* alloc the working space for the compressed block */
		ret = snd_soc_lzo_prepare(lzo_blocks[i]);
		if (ret < 0)
			goto err;
	}

	blksize = snd_soc_lzo_get_blksize(codec);
784
	p = codec->reg_def_copy;
785
	end = codec->reg_def_copy + codec->reg_size;
786 787 788 789 790 791 792 793 794 795 796 797 798 799 800
	/* compress the register map and fill the lzo blocks */
	for (i = 0; i < blkcount; ++i, p += blksize) {
		lzo_blocks[i]->src = p;
		if (p + blksize > end)
			lzo_blocks[i]->src_len = end - p;
		else
			lzo_blocks[i]->src_len = blksize;
		ret = snd_soc_lzo_compress_cache_block(codec,
						       lzo_blocks[i]);
		if (ret < 0)
			goto err;
		lzo_blocks[i]->decompressed_size =
			lzo_blocks[i]->src_len;
	}

801 802 803 804
	if (tofree) {
		kfree(codec->reg_def_copy);
		codec->reg_def_copy = NULL;
	}
805 806 807 808
	return 0;
err:
	snd_soc_cache_exit(codec);
err_tofree:
809 810 811 812
	if (tofree) {
		kfree(codec->reg_def_copy);
		codec->reg_def_copy = NULL;
	}
813 814
	return ret;
}
815
#endif
816

817 818 819
static int snd_soc_flat_cache_sync(struct snd_soc_codec *codec)
{
	int i;
820
	int ret;
821
	const struct snd_soc_codec_driver *codec_drv;
822 823 824 825
	unsigned int val;

	codec_drv = codec->driver;
	for (i = 0; i < codec_drv->reg_cache_size; ++i) {
826 827
		WARN_ON(codec->writable_register &&
			codec->writable_register(codec, i));
828 829 830
		ret = snd_soc_cache_read(codec, i, &val);
		if (ret)
			return ret;
831 832
		if (codec->reg_def_copy)
			if (snd_soc_get_cache_val(codec->reg_def_copy,
833 834
						  i, codec_drv->reg_word_size) == val)
				continue;
835 836 837
		ret = snd_soc_write(codec, i, val);
		if (ret)
			return ret;
838 839 840 841 842 843 844 845 846
		dev_dbg(codec->dev, "Synced register %#x, value = %#x\n",
			i, val);
	}
	return 0;
}

static int snd_soc_flat_cache_write(struct snd_soc_codec *codec,
				    unsigned int reg, unsigned int value)
{
847 848
	snd_soc_set_cache_val(codec->reg_cache, reg, value,
			      codec->driver->reg_word_size);
849 850 851 852 853 854
	return 0;
}

static int snd_soc_flat_cache_read(struct snd_soc_codec *codec,
				   unsigned int reg, unsigned int *value)
{
855 856
	*value = snd_soc_get_cache_val(codec->reg_cache, reg,
				       codec->driver->reg_word_size);
857 858 859 860 861 862 863 864 865 866 867 868 869 870
	return 0;
}

static int snd_soc_flat_cache_exit(struct snd_soc_codec *codec)
{
	if (!codec->reg_cache)
		return 0;
	kfree(codec->reg_cache);
	codec->reg_cache = NULL;
	return 0;
}

static int snd_soc_flat_cache_init(struct snd_soc_codec *codec)
{
871
	const struct snd_soc_codec_driver *codec_drv;
872 873 874

	codec_drv = codec->driver;

875 876
	if (codec->reg_def_copy)
		codec->reg_cache = kmemdup(codec->reg_def_copy,
877
					   codec->reg_size, GFP_KERNEL);
878
	else
879
		codec->reg_cache = kzalloc(codec->reg_size, GFP_KERNEL);
880 881 882 883 884 885 886 887
	if (!codec->reg_cache)
		return -ENOMEM;

	return 0;
}

/* an array of all supported compression types */
static const struct snd_soc_cache_ops cache_types[] = {
888
	/* Flat *must* be the first entry for fallback */
889
	{
890
		.id = SND_SOC_FLAT_COMPRESSION,
891
		.name = "flat",
892 893 894 895 896
		.init = snd_soc_flat_cache_init,
		.exit = snd_soc_flat_cache_exit,
		.read = snd_soc_flat_cache_read,
		.write = snd_soc_flat_cache_write,
		.sync = snd_soc_flat_cache_sync
897
	},
898
#ifdef CONFIG_SND_SOC_CACHE_LZO
899 900
	{
		.id = SND_SOC_LZO_COMPRESSION,
901
		.name = "LZO",
902 903 904 905 906
		.init = snd_soc_lzo_cache_init,
		.exit = snd_soc_lzo_cache_exit,
		.read = snd_soc_lzo_cache_read,
		.write = snd_soc_lzo_cache_write,
		.sync = snd_soc_lzo_cache_sync
907
	},
908
#endif
909 910
	{
		.id = SND_SOC_RBTREE_COMPRESSION,
911
		.name = "rbtree",
912 913 914 915 916
		.init = snd_soc_rbtree_cache_init,
		.exit = snd_soc_rbtree_cache_exit,
		.read = snd_soc_rbtree_cache_read,
		.write = snd_soc_rbtree_cache_write,
		.sync = snd_soc_rbtree_cache_sync
917 918 919 920 921 922 923 924
	}
};

int snd_soc_cache_init(struct snd_soc_codec *codec)
{
	int i;

	for (i = 0; i < ARRAY_SIZE(cache_types); ++i)
925
		if (cache_types[i].id == codec->compress_type)
926
			break;
927 928

	/* Fall back to flat compression */
929
	if (i == ARRAY_SIZE(cache_types)) {
930 931 932
		dev_warn(codec->dev, "Could not match compress type: %d\n",
			 codec->compress_type);
		i = 0;
933 934 935 936 937
	}

	mutex_init(&codec->cache_rw_mutex);
	codec->cache_ops = &cache_types[i];

938 939 940 941
	if (codec->cache_ops->init) {
		if (codec->cache_ops->name)
			dev_dbg(codec->dev, "Initializing %s cache for %s codec\n",
				codec->cache_ops->name, codec->name);
942
		return codec->cache_ops->init(codec);
943
	}
944
	return -ENOSYS;
945 946 947 948 949 950 951 952
}

/*
 * NOTE: keep in mind that this function might be called
 * multiple times.
 */
int snd_soc_cache_exit(struct snd_soc_codec *codec)
{
953 954 955 956
	if (codec->cache_ops && codec->cache_ops->exit) {
		if (codec->cache_ops->name)
			dev_dbg(codec->dev, "Destroying %s cache for %s codec\n",
				codec->cache_ops->name, codec->name);
957
		return codec->cache_ops->exit(codec);
958
	}
959
	return -ENOSYS;
960 961 962 963 964 965 966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982
}

/**
 * snd_soc_cache_read: Fetch the value of a given register from the cache.
 *
 * @codec: CODEC to configure.
 * @reg: The register index.
 * @value: The value to be returned.
 */
int snd_soc_cache_read(struct snd_soc_codec *codec,
		       unsigned int reg, unsigned int *value)
{
	int ret;

	mutex_lock(&codec->cache_rw_mutex);

	if (value && codec->cache_ops && codec->cache_ops->read) {
		ret = codec->cache_ops->read(codec, reg, value);
		mutex_unlock(&codec->cache_rw_mutex);
		return ret;
	}

	mutex_unlock(&codec->cache_rw_mutex);
983
	return -ENOSYS;
984 985 986 987 988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007
}
EXPORT_SYMBOL_GPL(snd_soc_cache_read);

/**
 * snd_soc_cache_write: Set the value of a given register in the cache.
 *
 * @codec: CODEC to configure.
 * @reg: The register index.
 * @value: The new register value.
 */
int snd_soc_cache_write(struct snd_soc_codec *codec,
			unsigned int reg, unsigned int value)
{
	int ret;

	mutex_lock(&codec->cache_rw_mutex);

	if (codec->cache_ops && codec->cache_ops->write) {
		ret = codec->cache_ops->write(codec, reg, value);
		mutex_unlock(&codec->cache_rw_mutex);
		return ret;
	}

	mutex_unlock(&codec->cache_rw_mutex);
1008
	return -ENOSYS;
1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023
}
EXPORT_SYMBOL_GPL(snd_soc_cache_write);

/**
 * snd_soc_cache_sync: Sync the register cache with the hardware.
 *
 * @codec: CODEC to configure.
 *
 * Any registers that should not be synced should be marked as
 * volatile.  In general drivers can choose not to use the provided
 * syncing functionality if they so require.
 */
int snd_soc_cache_sync(struct snd_soc_codec *codec)
{
	int ret;
1024
	const char *name;
1025 1026 1027 1028 1029

	if (!codec->cache_sync) {
		return 0;
	}

1030
	if (!codec->cache_ops || !codec->cache_ops->sync)
1031
		return -ENOSYS;
1032

1033 1034 1035 1036 1037
	if (codec->cache_ops->name)
		name = codec->cache_ops->name;
	else
		name = "unknown";

1038 1039 1040 1041 1042 1043 1044 1045 1046
	if (codec->cache_ops->name)
		dev_dbg(codec->dev, "Syncing %s cache for %s codec\n",
			codec->cache_ops->name, codec->name);
	trace_snd_soc_cache_sync(codec, name, "start");
	ret = codec->cache_ops->sync(codec);
	if (!ret)
		codec->cache_sync = 0;
	trace_snd_soc_cache_sync(codec, name, "end");
	return ret;
1047 1048
}
EXPORT_SYMBOL_GPL(snd_soc_cache_sync);
1049 1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097

static int snd_soc_get_reg_access_index(struct snd_soc_codec *codec,
					unsigned int reg)
{
	const struct snd_soc_codec_driver *codec_drv;
	unsigned int min, max, index;

	codec_drv = codec->driver;
	min = 0;
	max = codec_drv->reg_access_size - 1;
	do {
		index = (min + max) / 2;
		if (codec_drv->reg_access_default[index].reg == reg)
			return index;
		if (codec_drv->reg_access_default[index].reg < reg)
			min = index + 1;
		else
			max = index;
	} while (min <= max);
	return -1;
}

int snd_soc_default_volatile_register(struct snd_soc_codec *codec,
				      unsigned int reg)
{
	int index;

	if (reg >= codec->driver->reg_cache_size)
		return 1;
	index = snd_soc_get_reg_access_index(codec, reg);
	if (index < 0)
		return 0;
	return codec->driver->reg_access_default[index].vol;
}
EXPORT_SYMBOL_GPL(snd_soc_default_volatile_register);

int snd_soc_default_readable_register(struct snd_soc_codec *codec,
				      unsigned int reg)
{
	int index;

	if (reg >= codec->driver->reg_cache_size)
		return 1;
	index = snd_soc_get_reg_access_index(codec, reg);
	if (index < 0)
		return 0;
	return codec->driver->reg_access_default[index].read;
}
EXPORT_SYMBOL_GPL(snd_soc_default_readable_register);
1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111

int snd_soc_default_writable_register(struct snd_soc_codec *codec,
				      unsigned int reg)
{
	int index;

	if (reg >= codec->driver->reg_cache_size)
		return 1;
	index = snd_soc_get_reg_access_index(codec, reg);
	if (index < 0)
		return 0;
	return codec->driver->reg_access_default[index].write;
}
EXPORT_SYMBOL_GPL(snd_soc_default_writable_register);