sound/soc/soc-cache.c

   1 /*
   2  * soc-cache.c  --  ASoC register cache helpers
   3  *
   4  * Copyright 2009 Wolfson Microelectronics PLC.
   5  *
   6  * Author: Mark Brown <broonie@opensource.wolfsonmicro.com>
   7  *
   8  *  This program is free software; you can redistribute  it and/or modify it
   9  *  under  the terms of  the GNU General  Public License as published by the
  10  *  Free Software Foundation;  either version 2 of the  License, or (at your
  11  *  option) any later version.
  12  */
  13
  14 #include <linux/i2c.h>
  15 #include <linux/spi/spi.h>
  16 #include <sound/soc.h>
  17 #include <linux/lzo.h>
  18 #include <linux/bitmap.h>
  19 #include <linux/rbtree.h>
  20
  21 #include <trace/events/asoc.h>
  22
  23 static bool snd_soc_set_cache_val(void *base, unsigned int idx,
  24                                   unsigned int val, unsigned int word_size)
  25 {
  26         switch (word_size) {
  27         case 1: {
  28                 u8 *cache = base;
  29                 if (cache[idx] == val)
  30                         return true;
  31                 cache[idx] = val;
  32                 break;
  33         }
  34         case 2: {
  35                 u16 *cache = base;
  36                 if (cache[idx] == val)
  37                         return true;
  38                 cache[idx] = val;
  39                 break;
  40         }
  41         default:
  42                 BUG();
  43         }
  44         return false;
  45 }
  46
  47 static unsigned int snd_soc_get_cache_val(const void *base, unsigned int idx,
  48                 unsigned int word_size)
  49 {
  50         if (!base)
  51                 return -1;
  52
  53         switch (word_size) {
  54         case 1: {
  55                 const u8 *cache = base;
  56                 return cache[idx];
  57         }
  58         case 2: {
  59                 const u16 *cache = base;
  60                 return cache[idx];
  61         }
  62         default:
  63                 BUG();
  64         }
  65         /* unreachable */
  66         return -1;
  67 }
  68
  69 struct snd_soc_rbtree_node {
  70         struct rb_node node; /* the actual rbtree node holding this block */
  71         unsigned int base_reg; /* base register handled by this block */
  72         unsigned int word_size; /* number of bytes needed to represent the register index */
  73         void *block; /* block of adjacent registers */
  74         unsigned int blklen; /* number of registers available in the block */
  75 } __attribute__ ((packed));
  76
  77 struct snd_soc_rbtree_ctx {
  78         struct rb_root root;
  79         struct snd_soc_rbtree_node *cached_rbnode;
  80 };
  81
  82 static inline void snd_soc_rbtree_get_base_top_reg(
  83         struct snd_soc_rbtree_node *rbnode,
  84         unsigned int *base, unsigned int *top)
  85 {
  86         *base = rbnode->base_reg;
  87         *top = rbnode->base_reg + rbnode->blklen - 1;
  88 }
  89
  90 static unsigned int snd_soc_rbtree_get_register(
  91         struct snd_soc_rbtree_node *rbnode, unsigned int idx)
  92 {
  93         unsigned int val;
  94
  95         switch (rbnode->word_size) {
  96         case 1: {
  97                 u8 *p = rbnode->block;
  98                 val = p[idx];
  99                 return val;
 100         }
 101         case 2: {
 102                 u16 *p = rbnode->block;
 103                 val = p[idx];
 104                 return val;
 105         }
 106         default:
 107                 BUG();
 108                 break;
 109         }
 110         return -1;
 111 }
 112
 113 static void snd_soc_rbtree_set_register(struct snd_soc_rbtree_node *rbnode,
 114                                         unsigned int idx, unsigned int val)
 115 {
 116         switch (rbnode->word_size) {
 117         case 1: {
 118                 u8 *p = rbnode->block;
 119                 p[idx] = val;
 120                 break;
 121         }
 122         case 2: {
 123                 u16 *p = rbnode->block;
 124                 p[idx] = val;
 125                 break;
 126         }
 127         default:
 128                 BUG();
 129                 break;
 130         }
 131 }
 132
 133 static struct snd_soc_rbtree_node *snd_soc_rbtree_lookup(
 134         struct rb_root *root, unsigned int reg)
 135 {
 136         struct rb_node *node;
 137         struct snd_soc_rbtree_node *rbnode;
 138         unsigned int base_reg, top_reg;
 139
 140         node = root->rb_node;
 141         while (node) {
 142                 rbnode = container_of(node, struct snd_soc_rbtree_node, node);
 143                 snd_soc_rbtree_get_base_top_reg(rbnode, &base_reg, &top_reg);
 144                 if (reg >= base_reg && reg <= top_reg)
 145                         return rbnode;
 146                 else if (reg > top_reg)
 147                         node = node->rb_right;
 148                 else if (reg < base_reg)
 149                         node = node->rb_left;
 150         }
 151
 152         return NULL;
 153 }
 154
 155 static int snd_soc_rbtree_insert(struct rb_root *root,
 156                                  struct snd_soc_rbtree_node *rbnode)
 157 {
 158         struct rb_node **new, *parent;
 159         struct snd_soc_rbtree_node *rbnode_tmp;
 160         unsigned int base_reg_tmp, top_reg_tmp;
 161         unsigned int base_reg;
 162
 163         parent = NULL;
 164         new = &root->rb_node;
 165         while (*new) {
 166                 rbnode_tmp = container_of(*new, struct snd_soc_rbtree_node,
 167                                           node);
 168                 /* base and top registers of the current rbnode */
 169                 snd_soc_rbtree_get_base_top_reg(rbnode_tmp, &base_reg_tmp,
 170                                                 &top_reg_tmp);
 171                 /* base register of the rbnode to be added */
 172                 base_reg = rbnode->base_reg;
 173                 parent = *new;
 174                 /* if this register has already been inserted, just return */
 175                 if (base_reg >= base_reg_tmp &&
 176                     base_reg <= top_reg_tmp)
 177                         return 0;
 178                 else if (base_reg > top_reg_tmp)
 179                         new = &((*new)->rb_right);
 180                 else if (base_reg < base_reg_tmp)
 181                         new = &((*new)->rb_left);
 182         }
 183
 184         /* insert the node into the rbtree */
 185         rb_link_node(&rbnode->node, parent, new);
 186         rb_insert_color(&rbnode->node, root);
 187
 188         return 1;
 189 }
 190
 191 static int snd_soc_rbtree_cache_sync(struct snd_soc_codec *codec)
 192 {
 193         struct snd_soc_rbtree_ctx *rbtree_ctx;
 194         struct rb_node *node;
 195         struct snd_soc_rbtree_node *rbnode;
 196         unsigned int regtmp;
 197         unsigned int val, def;
 198         int ret;
 199         int i;
 200
 201         rbtree_ctx = codec->reg_cache;
 202         for (node = rb_first(&rbtree_ctx->root); node; node = rb_next(node)) {
 203                 rbnode = rb_entry(node, struct snd_soc_rbtree_node, node);
 204                 for (i = 0; i < rbnode->blklen; ++i) {
 205                         regtmp = rbnode->base_reg + i;
 206                         val = snd_soc_rbtree_get_register(rbnode, i);
 207                         def = snd_soc_get_cache_val(codec->reg_def_copy, i,
 208                                                     rbnode->word_size);
 209                         if (val == def)
 210                                 continue;
 211
 212                         WARN_ON(!snd_soc_codec_writable_register(codec, regtmp));
 213
 214                         codec->cache_bypass = 1;
 215                         ret = snd_soc_write(codec, regtmp, val);
 216                         codec->cache_bypass = 0;
 217                         if (ret)
 218                                 return ret;
 219                         dev_dbg(codec->dev, "Synced register %#x, value = %#x\n",
 220                                 regtmp, val);
 221                 }
 222         }
 223
 224         return 0;
 225 }
 226
 227 static int snd_soc_rbtree_insert_to_block(struct snd_soc_rbtree_node *rbnode,
 228                                           unsigned int pos, unsigned int reg,
 229                                           unsigned int value)
 230 {
 231         u8 *blk;
 232
 233         blk = krealloc(rbnode->block,
 234                        (rbnode->blklen + 1) * rbnode->word_size, GFP_KERNEL);
 235         if (!blk)
 236                 return -ENOMEM;
 237
 238         /* insert the register value in the correct place in the rbnode block */
 239         memmove(blk + (pos + 1) * rbnode->word_size,
 240                 blk + pos * rbnode->word_size,
 241                 (rbnode->blklen - pos) * rbnode->word_size);
 242
 243         /* update the rbnode block, its size and the base register */
 244         rbnode->block = blk;
 245         rbnode->blklen++;
 246         if (!pos)
 247                 rbnode->base_reg = reg;
 248
 249         snd_soc_rbtree_set_register(rbnode, pos, value);
 250         return 0;
 251 }
 252
 253 static int snd_soc_rbtree_cache_write(struct snd_soc_codec *codec,
 254                                       unsigned int reg, unsigned int value)
 255 {
 256         struct snd_soc_rbtree_ctx *rbtree_ctx;
 257         struct snd_soc_rbtree_node *rbnode, *rbnode_tmp;
 258         struct rb_node *node;
 259         unsigned int val;
 260         unsigned int reg_tmp;
 261         unsigned int base_reg, top_reg;
 262         unsigned int pos;
 263         int i;
 264         int ret;
 265
 266         rbtree_ctx = codec->reg_cache;
 267         /* look up the required register in the cached rbnode */
 268         rbnode = rbtree_ctx->cached_rbnode;
 269         if (rbnode) {
 270                 snd_soc_rbtree_get_base_top_reg(rbnode, &base_reg, &top_reg);
 271                 if (reg >= base_reg && reg <= top_reg) {
 272                         reg_tmp = reg - base_reg;
 273                         val = snd_soc_rbtree_get_register(rbnode, reg_tmp);
 274                         if (val == value)
 275                                 return 0;
 276                         snd_soc_rbtree_set_register(rbnode, reg_tmp, value);
 277                         return 0;
 278                 }
 279         }
 280         /* if we can't locate it in the cached rbnode we'll have
 281          * to traverse the rbtree looking for it.
 282          */
 283         rbnode = snd_soc_rbtree_lookup(&rbtree_ctx->root, reg);
 284         if (rbnode) {
 285                 reg_tmp = reg - rbnode->base_reg;
 286                 val = snd_soc_rbtree_get_register(rbnode, reg_tmp);
 287                 if (val == value)
 288                         return 0;
 289                 snd_soc_rbtree_set_register(rbnode, reg_tmp, value);
 290                 rbtree_ctx->cached_rbnode = rbnode;
 291         } else {
 292                 /* bail out early, no need to create the rbnode yet */
 293                 if (!value)
 294                         return 0;
 295                 /* look for an adjacent register to the one we are about to add */
 296                 for (node = rb_first(&rbtree_ctx->root); node;
 297                      node = rb_next(node)) {
 298                         rbnode_tmp = rb_entry(node, struct snd_soc_rbtree_node, node);
 299                         for (i = 0; i < rbnode_tmp->blklen; ++i) {
 300                                 reg_tmp = rbnode_tmp->base_reg + i;
 301                                 if (abs(reg_tmp - reg) != 1)
 302                                         continue;
 303                                 /* decide where in the block to place our register */
 304                                 if (reg_tmp + 1 == reg)
 305                                         pos = i + 1;
 306                                 else
 307                                         pos = i;
 308                                 ret = snd_soc_rbtree_insert_to_block(rbnode_tmp, pos,
 309                                                                      reg, value);
 310                                 if (ret)
 311                                         return ret;
 312                                 rbtree_ctx->cached_rbnode = rbnode_tmp;
 313                                 return 0;
 314                         }
 315                 }
 316                 /* we did not manage to find a place to insert it in an existing
 317                  * block so create a new rbnode with a single register in its block.
 318                  * This block will get populated further if any other adjacent
 319                  * registers get modified in the future.
 320                  */
 321                 rbnode = kzalloc(sizeof *rbnode, GFP_KERNEL);
 322                 if (!rbnode)
 323                         return -ENOMEM;
 324                 rbnode->blklen = 1;
 325                 rbnode->base_reg = reg;
 326                 rbnode->word_size = codec->driver->reg_word_size;
 327                 rbnode->block = kmalloc(rbnode->blklen * rbnode->word_size,
 328                                         GFP_KERNEL);
 329                 if (!rbnode->block) {
 330                         kfree(rbnode);
 331                         return -ENOMEM;
 332                 }
 333                 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         return 0;
 339 }
 340
 341 static int snd_soc_rbtree_cache_read(struct snd_soc_codec *codec,
 342                                      unsigned int reg, unsigned int *value)
 343 {
 344         struct snd_soc_rbtree_ctx *rbtree_ctx;
 345         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         /* look up the required register in the cached rbnode */
 351         rbnode = rbtree_ctx->cached_rbnode;
 352         if (rbnode) {
 353                 snd_soc_rbtree_get_base_top_reg(rbnode, &base_reg, &top_reg);
 354                 if (reg >= base_reg && reg <= top_reg) {
 355                         reg_tmp = reg - base_reg;
 356                         *value = snd_soc_rbtree_get_register(rbnode, reg_tmp);
 357                         return 0;
 358                 }
 359         }
 360         /* if we can't locate it in the cached rbnode we'll have
 361          * to traverse the rbtree looking for it.
 362          */
 363         rbnode = snd_soc_rbtree_lookup(&rbtree_ctx->root, reg);
 364         if (rbnode) {
 365                 reg_tmp = reg - rbnode->base_reg;
 366                 *value = snd_soc_rbtree_get_register(rbnode, reg_tmp);
 367                 rbtree_ctx->cached_rbnode = rbnode;
 368         } else {
 369                 /* uninitialized registers default to 0 */
 370                 *value = 0;
 371         }
 372
 373         return 0;
 374 }
 375
 376 static int snd_soc_rbtree_cache_exit(struct snd_soc_codec *codec)
 377 {
 378         struct rb_node *next;
 379         struct snd_soc_rbtree_ctx *rbtree_ctx;
 380         struct snd_soc_rbtree_node *rbtree_node;
 381
 382         /* if we've already been called then just return */
 383         rbtree_ctx = codec->reg_cache;
 384         if (!rbtree_ctx)
 385                 return 0;
 386
 387         /* free up the rbtree */
 388         next = rb_first(&rbtree_ctx->root);
 389         while (next) {
 390                 rbtree_node = rb_entry(next, struct snd_soc_rbtree_node, node);
 391                 next = rb_next(&rbtree_node->node);
 392                 rb_erase(&rbtree_node->node, &rbtree_ctx->root);
 393                 kfree(rbtree_node->block);
 394                 kfree(rbtree_node);
 395         }
 396
 397         /* release the resources */
 398         kfree(codec->reg_cache);
 399         codec->reg_cache = NULL;
 400
 401         return 0;
 402 }
 403
 404 static int snd_soc_rbtree_cache_init(struct snd_soc_codec *codec)
 405 {
 406         struct snd_soc_rbtree_ctx *rbtree_ctx;
 407         unsigned int word_size;
 408         unsigned int val;
 409         int i;
 410         int ret;
 411
 412         codec->reg_cache = kmalloc(sizeof *rbtree_ctx, GFP_KERNEL);
 413         if (!codec->reg_cache)
 414                 return -ENOMEM;
 415
 416         rbtree_ctx = codec->reg_cache;
 417         rbtree_ctx->root = RB_ROOT;
 418         rbtree_ctx->cached_rbnode = NULL;
 419
 420         if (!codec->reg_def_copy)
 421                 return 0;
 422
 423         word_size = codec->driver->reg_word_size;
 424         for (i = 0; i < codec->driver->reg_cache_size; ++i) {
 425                 val = snd_soc_get_cache_val(codec->reg_def_copy, i,
 426                                             word_size);
 427                 if (!val)
 428                         continue;
 429                 ret = snd_soc_rbtree_cache_write(codec, i, val);
 430                 if (ret)
 431                         goto err;
 432         }
 433
 434         return 0;
 435
 436 err:
 437         snd_soc_cache_exit(codec);
 438         return ret;
 439 }
 440
 441 #ifdef CONFIG_SND_SOC_CACHE_LZO
 442 struct snd_soc_lzo_ctx {
 443         void *wmem;
 444         void *dst;
 445         const void *src;
 446         size_t src_len;
 447         size_t dst_len;
 448         size_t decompressed_size;
 449         unsigned long *sync_bmp;
 450         int sync_bmp_nbits;
 451 };
 452
 453 #define LZO_BLOCK_NUM 8
 454 static int snd_soc_lzo_block_count(void)
 455 {
 456         return LZO_BLOCK_NUM;
 457 }
 458
 459 static int snd_soc_lzo_prepare(struct snd_soc_lzo_ctx *lzo_ctx)
 460 {
 461         lzo_ctx->wmem = kmalloc(LZO1X_MEM_COMPRESS, GFP_KERNEL);
 462         if (!lzo_ctx->wmem)
 463                 return -ENOMEM;
 464         return 0;
 465 }
 466
 467 static int snd_soc_lzo_compress(struct snd_soc_lzo_ctx *lzo_ctx)
 468 {
 469         size_t compress_size;
 470         int ret;
 471
 472         ret = lzo1x_1_compress(lzo_ctx->src, lzo_ctx->src_len,
 473                                lzo_ctx->dst, &compress_size, lzo_ctx->wmem);
 474         if (ret != LZO_E_OK || compress_size > lzo_ctx->dst_len)
 475                 return -EINVAL;
 476         lzo_ctx->dst_len = compress_size;
 477         return 0;
 478 }
 479
 480 static int snd_soc_lzo_decompress(struct snd_soc_lzo_ctx *lzo_ctx)
 481 {
 482         size_t dst_len;
 483         int ret;
 484
 485         dst_len = lzo_ctx->dst_len;
 486         ret = lzo1x_decompress_safe(lzo_ctx->src, lzo_ctx->src_len,
 487                                     lzo_ctx->dst, &dst_len);
 488         if (ret != LZO_E_OK || dst_len != lzo_ctx->dst_len)
 489                 return -EINVAL;
 490         return 0;
 491 }
 492
 493 static int snd_soc_lzo_compress_cache_block(struct snd_soc_codec *codec,
 494                 struct snd_soc_lzo_ctx *lzo_ctx)
 495 {
 496         int ret;
 497
 498         lzo_ctx->dst_len = lzo1x_worst_compress(PAGE_SIZE);
 499         lzo_ctx->dst = kmalloc(lzo_ctx->dst_len, GFP_KERNEL);
 500         if (!lzo_ctx->dst) {
 501                 lzo_ctx->dst_len = 0;
 502                 return -ENOMEM;
 503         }
 504
 505         ret = snd_soc_lzo_compress(lzo_ctx);
 506         if (ret < 0)
 507                 return ret;
 508         return 0;
 509 }
 510
 511 static int snd_soc_lzo_decompress_cache_block(struct snd_soc_codec *codec,
 512                 struct snd_soc_lzo_ctx *lzo_ctx)
 513 {
 514         int ret;
 515
 516         lzo_ctx->dst_len = lzo_ctx->decompressed_size;
 517         lzo_ctx->dst = kmalloc(lzo_ctx->dst_len, GFP_KERNEL);
 518         if (!lzo_ctx->dst) {
 519                 lzo_ctx->dst_len = 0;
 520                 return -ENOMEM;
 521         }
 522
 523         ret = snd_soc_lzo_decompress(lzo_ctx);
 524         if (ret < 0)
 525                 return ret;
 526         return 0;
 527 }
 528
 529 static inline int snd_soc_lzo_get_blkindex(struct snd_soc_codec *codec,
 530                 unsigned int reg)
 531 {
 532         const struct snd_soc_codec_driver *codec_drv;
 533
 534         codec_drv = codec->driver;
 535         return (reg * codec_drv->reg_word_size) /
 536                DIV_ROUND_UP(codec->reg_size, snd_soc_lzo_block_count());
 537 }
 538
 539 static inline int snd_soc_lzo_get_blkpos(struct snd_soc_codec *codec,
 540                 unsigned int reg)
 541 {
 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                       codec_drv->reg_word_size);
 547 }
 548
 549 static inline int snd_soc_lzo_get_blksize(struct snd_soc_codec *codec)
 550 {
 551         return DIV_ROUND_UP(codec->reg_size, snd_soc_lzo_block_count());
 552 }
 553
 554 static int snd_soc_lzo_cache_sync(struct snd_soc_codec *codec)
 555 {
 556         struct snd_soc_lzo_ctx **lzo_blocks;
 557         unsigned int val;
 558         int i;
 559         int ret;
 560
 561         lzo_blocks = codec->reg_cache;
 562         for_each_set_bit(i, lzo_blocks[0]->sync_bmp, lzo_blocks[0]->sync_bmp_nbits) {
 563                 WARN_ON(!snd_soc_codec_writable_register(codec, i));
 564                 ret = snd_soc_cache_read(codec, i, &val);
 565                 if (ret)
 566                         return ret;
 567                 codec->cache_bypass = 1;
 568                 ret = snd_soc_write(codec, i, val);
 569                 codec->cache_bypass = 0;
 570                 if (ret)
 571                         return ret;
 572                 dev_dbg(codec->dev, "Synced register %#x, value = %#x\n",
 573                         i, val);
 574         }
 575
 576         return 0;
 577 }
 578
 579 static int snd_soc_lzo_cache_write(struct snd_soc_codec *codec,
 580                                    unsigned int reg, unsigned int value)
 581 {
 582         struct snd_soc_lzo_ctx *lzo_block, **lzo_blocks;
 583         int ret, blkindex, blkpos;
 584         size_t blksize, tmp_dst_len;
 585         void *tmp_dst;
 586
 587         /* index of the compressed lzo block */
 588         blkindex = snd_soc_lzo_get_blkindex(codec, reg);
 589         /* register index within the decompressed block */
 590         blkpos = snd_soc_lzo_get_blkpos(codec, reg);
 591         /* size of the compressed block */
 592         blksize = snd_soc_lzo_get_blksize(codec);
 593         lzo_blocks = codec->reg_cache;
 594         lzo_block = lzo_blocks[blkindex];
 595
 596         /* save the pointer and length of the compressed block */
 597         tmp_dst = lzo_block->dst;
 598         tmp_dst_len = lzo_block->dst_len;
 599
 600         /* prepare the source to be the compressed block */
 601         lzo_block->src = lzo_block->dst;
 602         lzo_block->src_len = lzo_block->dst_len;
 603
 604         /* decompress the block */
 605         ret = snd_soc_lzo_decompress_cache_block(codec, lzo_block);
 606         if (ret < 0) {
 607                 kfree(lzo_block->dst);
 608                 goto out;
 609         }
 610
 611         /* write the new value to the cache */
 612         if (snd_soc_set_cache_val(lzo_block->dst, blkpos, value,
 613                                   codec->driver->reg_word_size)) {
 614                 kfree(lzo_block->dst);
 615                 goto out;
 616         }
 617
 618         /* prepare the source to be the decompressed block */
 619         lzo_block->src = lzo_block->dst;
 620         lzo_block->src_len = lzo_block->dst_len;
 621
 622         /* compress the block */
 623         ret = snd_soc_lzo_compress_cache_block(codec, lzo_block);
 624         if (ret < 0) {
 625                 kfree(lzo_block->dst);
 626                 kfree(lzo_block->src);
 627                 goto out;
 628         }
 629
 630         /* set the bit so we know we have to sync this register */
 631         set_bit(reg, lzo_block->sync_bmp);
 632         kfree(tmp_dst);
 633         kfree(lzo_block->src);
 634         return 0;
 635 out:
 636         lzo_block->dst = tmp_dst;
 637         lzo_block->dst_len = tmp_dst_len;
 638         return ret;
 639 }
 640
 641 static int snd_soc_lzo_cache_read(struct snd_soc_codec *codec,
 642                                   unsigned int reg, unsigned int *value)
 643 {
 644         struct snd_soc_lzo_ctx *lzo_block, **lzo_blocks;
 645         int ret, blkindex, blkpos;
 646         size_t blksize, tmp_dst_len;
 647         void *tmp_dst;
 648
 649         *value = 0;
 650         /* index of the compressed lzo block */
 651         blkindex = snd_soc_lzo_get_blkindex(codec, reg);
 652         /* register index within the decompressed block */
 653         blkpos = snd_soc_lzo_get_blkpos(codec, reg);
 654         /* size of the compressed block */
 655         blksize = snd_soc_lzo_get_blksize(codec);
 656         lzo_blocks = codec->reg_cache;
 657         lzo_block = lzo_blocks[blkindex];
 658
 659         /* save the pointer and length of the compressed block */
 660         tmp_dst = lzo_block->dst;
 661         tmp_dst_len = lzo_block->dst_len;
 662
 663         /* prepare the source to be the compressed block */
 664         lzo_block->src = lzo_block->dst;
 665         lzo_block->src_len = lzo_block->dst_len;
 666
 667         /* decompress the block */
 668         ret = snd_soc_lzo_decompress_cache_block(codec, lzo_block);
 669         if (ret >= 0)
 670                 /* fetch the value from the cache */
 671                 *value = snd_soc_get_cache_val(lzo_block->dst, blkpos,
 672                                                codec->driver->reg_word_size);
 673
 674         kfree(lzo_block->dst);
 675         /* restore the pointer and length of the compressed block */
 676         lzo_block->dst = tmp_dst;
 677         lzo_block->dst_len = tmp_dst_len;
 678         return 0;
 679 }
 680
 681 static int snd_soc_lzo_cache_exit(struct snd_soc_codec *codec)
 682 {
 683         struct snd_soc_lzo_ctx **lzo_blocks;
 684         int i, blkcount;
 685
 686         lzo_blocks = codec->reg_cache;
 687         if (!lzo_blocks)
 688                 return 0;
 689
 690         blkcount = snd_soc_lzo_block_count();
 691         /*
 692          * the pointer to the bitmap used for syncing the cache
 693          * is shared amongst all lzo_blocks.  Ensure it is freed
 694          * only once.
 695          */
 696         if (lzo_blocks[0])
 697                 kfree(lzo_blocks[0]->sync_bmp);
 698         for (i = 0; i < blkcount; ++i) {
 699                 if (lzo_blocks[i]) {
 700                         kfree(lzo_blocks[i]->wmem);
 701                         kfree(lzo_blocks[i]->dst);
 702                 }
 703                 /* each lzo_block is a pointer returned by kmalloc or NULL */
 704                 kfree(lzo_blocks[i]);
 705         }
 706         kfree(lzo_blocks);
 707         codec->reg_cache = NULL;
 708         return 0;
 709 }
 710
 711 static int snd_soc_lzo_cache_init(struct snd_soc_codec *codec)
 712 {
 713         struct snd_soc_lzo_ctx **lzo_blocks;
 714         size_t bmp_size;
 715         const struct snd_soc_codec_driver *codec_drv;
 716         int ret, tofree, i, blksize, blkcount;
 717         const char *p, *end;
 718         unsigned long *sync_bmp;
 719
 720         ret = 0;
 721         codec_drv = codec->driver;
 722
 723         /*
 724          * If we have not been given a default register cache
 725          * then allocate a dummy zero-ed out region, compress it
 726          * and remember to free it afterwards.
 727          */
 728         tofree = 0;
 729         if (!codec->reg_def_copy)
 730                 tofree = 1;
 731
 732         if (!codec->reg_def_copy) {
 733                 codec->reg_def_copy = kzalloc(codec->reg_size, GFP_KERNEL);
 734                 if (!codec->reg_def_copy)
 735                         return -ENOMEM;
 736         }
 737
 738         blkcount = snd_soc_lzo_block_count();
 739         codec->reg_cache = kzalloc(blkcount * sizeof *lzo_blocks,
 740                                    GFP_KERNEL);
 741         if (!codec->reg_cache) {
 742                 ret = -ENOMEM;
 743                 goto err_tofree;
 744         }
 745         lzo_blocks = codec->reg_cache;
 746
 747         /*
 748          * allocate a bitmap to be used when syncing the cache with
 749          * the hardware.  Each time a register is modified, the corresponding
 750          * bit is set in the bitmap, so we know that we have to sync
 751          * that register.
 752          */
 753         bmp_size = codec_drv->reg_cache_size;
 754         sync_bmp = kmalloc(BITS_TO_LONGS(bmp_size) * sizeof(long),
 755                            GFP_KERNEL);
 756         if (!sync_bmp) {
 757                 ret = -ENOMEM;
 758                 goto err;
 759         }
 760         bitmap_zero(sync_bmp, bmp_size);
 761
 762         /* allocate the lzo blocks and initialize them */
 763         for (i = 0; i < blkcount; ++i) {
 764                 lzo_blocks[i] = kzalloc(sizeof **lzo_blocks,
 765                                         GFP_KERNEL);
 766                 if (!lzo_blocks[i]) {
 767                         kfree(sync_bmp);
 768                         ret = -ENOMEM;
 769                         goto err;
 770                 }
 771                 lzo_blocks[i]->sync_bmp = sync_bmp;
 772                 lzo_blocks[i]->sync_bmp_nbits = bmp_size;
 773                 /* alloc the working space for the compressed block */
 774                 ret = snd_soc_lzo_prepare(lzo_blocks[i]);
 775                 if (ret < 0)
 776                         goto err;
 777         }
 778
 779         blksize = snd_soc_lzo_get_blksize(codec);
 780         p = codec->reg_def_copy;
 781         end = codec->reg_def_copy + codec->reg_size;
 782         /* compress the register map and fill the lzo blocks */
 783         for (i = 0; i < blkcount; ++i, p += blksize) {
 784                 lzo_blocks[i]->src = p;
 785                 if (p + blksize > end)
 786                         lzo_blocks[i]->src_len = end - p;
 787                 else
 788                         lzo_blocks[i]->src_len = blksize;
 789                 ret = snd_soc_lzo_compress_cache_block(codec,
 790                                                        lzo_blocks[i]);
 791                 if (ret < 0)
 792                         goto err;
 793                 lzo_blocks[i]->decompressed_size =
 794                         lzo_blocks[i]->src_len;
 795         }
 796
 797         if (tofree) {
 798                 kfree(codec->reg_def_copy);
 799                 codec->reg_def_copy = NULL;
 800         }
 801         return 0;
 802 err:
 803         snd_soc_cache_exit(codec);
 804 err_tofree:
 805         if (tofree) {
 806                 kfree(codec->reg_def_copy);
 807                 codec->reg_def_copy = NULL;
 808         }
 809         return ret;
 810 }
 811 #endif
 812
 813 static int snd_soc_flat_cache_sync(struct snd_soc_codec *codec)
 814 {
 815         int i;
 816         int ret;
 817         const struct snd_soc_codec_driver *codec_drv;
 818         unsigned int val;
 819
 820         codec_drv = codec->driver;
 821         for (i = 0; i < codec_drv->reg_cache_size; ++i) {
 822                 ret = snd_soc_cache_read(codec, i, &val);
 823                 if (ret)
 824                         return ret;
 825                 if (codec->reg_def_copy)
 826                         if (snd_soc_get_cache_val(codec->reg_def_copy,
 827                                                   i, codec_drv->reg_word_size) == val)
 828                                 continue;
 829
 830                 WARN_ON(!snd_soc_codec_writable_register(codec, i));
 831
 832                 ret = snd_soc_write(codec, i, val);
 833                 if (ret)
 834                         return ret;
 835                 dev_dbg(codec->dev, "Synced register %#x, value = %#x\n",
 836                         i, val);
 837         }
 838         return 0;
 839 }
 840
 841 static int snd_soc_flat_cache_write(struct snd_soc_codec *codec,
 842                                     unsigned int reg, unsigned int value)
 843 {
 844         snd_soc_set_cache_val(codec->reg_cache, reg, value,
 845                               codec->driver->reg_word_size);
 846         return 0;
 847 }
 848
 849 static int snd_soc_flat_cache_read(struct snd_soc_codec *codec,
 850                                    unsigned int reg, unsigned int *value)
 851 {
 852         *value = snd_soc_get_cache_val(codec->reg_cache, reg,
 853                                        codec->driver->reg_word_size);
 854         return 0;
 855 }
 856
 857 static int snd_soc_flat_cache_exit(struct snd_soc_codec *codec)
 858 {
 859         if (!codec->reg_cache)
 860                 return 0;
 861         kfree(codec->reg_cache);
 862         codec->reg_cache = NULL;
 863         return 0;
 864 }
 865
 866 static int snd_soc_flat_cache_init(struct snd_soc_codec *codec)
 867 {
 868         if (codec->reg_def_copy)
 869                 codec->reg_cache = kmemdup(codec->reg_def_copy,
 870                                            codec->reg_size, GFP_KERNEL);
 871         else
 872                 codec->reg_cache = kzalloc(codec->reg_size, GFP_KERNEL);
 873         if (!codec->reg_cache)
 874                 return -ENOMEM;
 875
 876         return 0;
 877 }
 878
 879 /* an array of all supported compression types */
 880 static const struct snd_soc_cache_ops cache_types[] = {
 881         /* Flat *must* be the first entry for fallback */
 882         {
 883                 .id = SND_SOC_FLAT_COMPRESSION,
 884                 .name = "flat",
 885                 .init = snd_soc_flat_cache_init,
 886                 .exit = snd_soc_flat_cache_exit,
 887                 .read = snd_soc_flat_cache_read,
 888                 .write = snd_soc_flat_cache_write,
 889                 .sync = snd_soc_flat_cache_sync
 890         },
 891 #ifdef CONFIG_SND_SOC_CACHE_LZO
 892         {
 893                 .id = SND_SOC_LZO_COMPRESSION,
 894                 .name = "LZO",
 895                 .init = snd_soc_lzo_cache_init,
 896                 .exit = snd_soc_lzo_cache_exit,
 897                 .read = snd_soc_lzo_cache_read,
 898                 .write = snd_soc_lzo_cache_write,
 899                 .sync = snd_soc_lzo_cache_sync
 900         },
 901 #endif
 902         {
 903                 .id = SND_SOC_RBTREE_COMPRESSION,
 904                 .name = "rbtree",
 905                 .init = snd_soc_rbtree_cache_init,
 906                 .exit = snd_soc_rbtree_cache_exit,
 907                 .read = snd_soc_rbtree_cache_read,
 908                 .write = snd_soc_rbtree_cache_write,
 909                 .sync = snd_soc_rbtree_cache_sync
 910         }
 911 };
 912
 913 int snd_soc_cache_init(struct snd_soc_codec *codec)
 914 {
 915         int i;
 916
 917         for (i = 0; i < ARRAY_SIZE(cache_types); ++i)
 918                 if (cache_types[i].id == codec->compress_type)
 919                         break;
 920
 921         /* Fall back to flat compression */
 922         if (i == ARRAY_SIZE(cache_types)) {
 923                 dev_warn(codec->dev, "Could not match compress type: %d\n",
 924                          codec->compress_type);
 925                 i = 0;
 926         }
 927
 928         mutex_init(&codec->cache_rw_mutex);
 929         codec->cache_ops = &cache_types[i];
 930
 931         if (codec->cache_ops->init) {
 932                 if (codec->cache_ops->name)
 933                         dev_dbg(codec->dev, "Initializing %s cache for %s codec\n",
 934                                 codec->cache_ops->name, codec->name);
 935                 return codec->cache_ops->init(codec);
 936         }
 937         return -ENOSYS;
 938 }
 939
 940 /*
 941  * NOTE: keep in mind that this function might be called
 942  * multiple times.
 943  */
 944 int snd_soc_cache_exit(struct snd_soc_codec *codec)
 945 {
 946         if (codec->cache_ops && codec->cache_ops->exit) {
 947                 if (codec->cache_ops->name)
 948                         dev_dbg(codec->dev, "Destroying %s cache for %s codec\n",
 949                                 codec->cache_ops->name, codec->name);
 950                 return codec->cache_ops->exit(codec);
 951         }
 952         return -ENOSYS;
 953 }
 954
 955 /**
 956  * snd_soc_cache_read: Fetch the value of a given register from the cache.
 957  *
 958  * @codec: CODEC to configure.
 959  * @reg: The register index.
 960  * @value: The value to be returned.
 961  */
 962 int snd_soc_cache_read(struct snd_soc_codec *codec,
 963                        unsigned int reg, unsigned int *value)
 964 {
 965         int ret;
 966
 967         mutex_lock(&codec->cache_rw_mutex);
 968
 969         if (value && codec->cache_ops && codec->cache_ops->read) {
 970                 ret = codec->cache_ops->read(codec, reg, value);
 971                 mutex_unlock(&codec->cache_rw_mutex);
 972                 return ret;
 973         }
 974
 975         mutex_unlock(&codec->cache_rw_mutex);
 976         return -ENOSYS;
 977 }
 978 EXPORT_SYMBOL_GPL(snd_soc_cache_read);
 979
 980 /**
 981  * snd_soc_cache_write: Set the value of a given register in the cache.
 982  *
 983  * @codec: CODEC to configure.
 984  * @reg: The register index.
 985  * @value: The new register value.
 986  */
 987 int snd_soc_cache_write(struct snd_soc_codec *codec,
 988                         unsigned int reg, unsigned int value)
 989 {
 990         int ret;
 991
 992         mutex_lock(&codec->cache_rw_mutex);
 993
 994         if (codec->cache_ops && codec->cache_ops->write) {
 995                 ret = codec->cache_ops->write(codec, reg, value);
 996                 mutex_unlock(&codec->cache_rw_mutex);
 997                 return ret;
 998         }
 999
1000         mutex_unlock(&codec->cache_rw_mutex);
1001         return -ENOSYS;
1002 }
1003 EXPORT_SYMBOL_GPL(snd_soc_cache_write);
1004
1005 /**
1006  * snd_soc_cache_sync: Sync the register cache with the hardware.
1007  *
1008  * @codec: CODEC to configure.
1009  *
1010  * Any registers that should not be synced should be marked as
1011  * volatile.  In general drivers can choose not to use the provided
1012  * syncing functionality if they so require.
1013  */
1014 int snd_soc_cache_sync(struct snd_soc_codec *codec)
1015 {
1016         int ret;
1017         const char *name;
1018
1019         if (!codec->cache_sync) {
1020                 return 0;
1021         }
1022
1023         if (!codec->cache_ops || !codec->cache_ops->sync)
1024                 return -ENOSYS;
1025
1026         if (codec->cache_ops->name)
1027                 name = codec->cache_ops->name;
1028         else
1029                 name = "unknown";
1030
1031         if (codec->cache_ops->name)
1032                 dev_dbg(codec->dev, "Syncing %s cache for %s codec\n",
1033                         codec->cache_ops->name, codec->name);
1034         trace_snd_soc_cache_sync(codec, name, "start");
1035         ret = codec->cache_ops->sync(codec);
1036         if (!ret)
1037                 codec->cache_sync = 0;
1038         trace_snd_soc_cache_sync(codec, name, "end");
1039         return ret;
1040 }
1041 EXPORT_SYMBOL_GPL(snd_soc_cache_sync);
1042
1043 static int snd_soc_get_reg_access_index(struct snd_soc_codec *codec,
1044                                         unsigned int reg)
1045 {
1046         const struct snd_soc_codec_driver *codec_drv;
1047         unsigned int min, max, index;
1048
1049         codec_drv = codec->driver;
1050         min = 0;
1051         max = codec_drv->reg_access_size - 1;
1052         do {
1053                 index = (min + max) / 2;
1054                 if (codec_drv->reg_access_default[index].reg == reg)
1055                         return index;
1056                 if (codec_drv->reg_access_default[index].reg < reg)
1057                         min = index + 1;
1058                 else
1059                         max = index;
1060         } while (min <= max);
1061         return -1;
1062 }
1063
1064 int snd_soc_default_volatile_register(struct snd_soc_codec *codec,
1065                                       unsigned int reg)
1066 {
1067         int index;
1068
1069         if (reg >= codec->driver->reg_cache_size)
1070                 return 1;
1071         index = snd_soc_get_reg_access_index(codec, reg);
1072         if (index < 0)
1073                 return 0;
1074         return codec->driver->reg_access_default[index].vol;
1075 }
1076 EXPORT_SYMBOL_GPL(snd_soc_default_volatile_register);
1077
1078 int snd_soc_default_readable_register(struct snd_soc_codec *codec,
1079                                       unsigned int reg)
1080 {
1081         int index;
1082
1083         if (reg >= codec->driver->reg_cache_size)
1084                 return 1;
1085         index = snd_soc_get_reg_access_index(codec, reg);
1086         if (index < 0)
1087                 return 0;
1088         return codec->driver->reg_access_default[index].read;
1089 }
1090 EXPORT_SYMBOL_GPL(snd_soc_default_readable_register);
1091
1092 int snd_soc_default_writable_register(struct snd_soc_codec *codec,
1093                                       unsigned int reg)
1094 {
1095         int index;
1096
1097         if (reg >= codec->driver->reg_cache_size)
1098                 return 1;
1099         index = snd_soc_get_reg_access_index(codec, reg);
1100         if (index < 0)
1101                 return 0;
1102         return codec->driver->reg_access_default[index].write;
1103 }
1104 EXPORT_SYMBOL_GPL(snd_soc_default_writable_register);