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 #ifdef CONFIG_SPI_MASTER
  24 static int do_spi_write(void *control, const char *data, int len)
  25 {
  26         struct spi_device *spi = control;
  27         int ret;
  28
  29         ret = spi_write(spi, data, len);
  30         if (ret < 0)
  31                 return ret;
  32
  33         return len;
  34 }
  35 #endif
  36
  37 static int do_hw_write(struct snd_soc_codec *codec, unsigned int reg,
  38                        unsigned int value, const void *data, int len)
  39 {
  40         int ret;
  41
  42         if (!snd_soc_codec_volatile_register(codec, reg) &&
  43             reg < codec->driver->reg_cache_size &&
  44             !codec->cache_bypass) {
  45                 ret = snd_soc_cache_write(codec, reg, value);
  46                 if (ret < 0)
  47                         return -1;
  48         }
  49
  50         if (codec->cache_only) {
  51                 codec->cache_sync = 1;
  52                 return 0;
  53         }
  54
  55         ret = codec->hw_write(codec->control_data, data, len);
  56         if (ret == len)
  57                 return 0;
  58         if (ret < 0)
  59                 return ret;
  60         else
  61                 return -EIO;
  62 }
  63
  64 static unsigned int do_hw_read(struct snd_soc_codec *codec, unsigned int reg)
  65 {
  66         int ret;
  67         unsigned int val;
  68
  69         if (reg >= codec->driver->reg_cache_size ||
  70             snd_soc_codec_volatile_register(codec, reg) ||
  71             codec->cache_bypass) {
  72                 if (codec->cache_only)
  73                         return -1;
  74
  75                 BUG_ON(!codec->hw_read);
  76                 return codec->hw_read(codec, reg);
  77         }
  78
  79         ret = snd_soc_cache_read(codec, reg, &val);
  80         if (ret < 0)
  81                 return -1;
  82         return val;
  83 }
  84
  85 static unsigned int snd_soc_4_12_read(struct snd_soc_codec *codec,
  86                                       unsigned int reg)
  87 {
  88         return do_hw_read(codec, reg);
  89 }
  90
  91 static int snd_soc_4_12_write(struct snd_soc_codec *codec, unsigned int reg,
  92                               unsigned int value)
  93 {
  94         u16 data;
  95
  96         data = cpu_to_be16((reg << 12) | (value & 0xffffff));
  97
  98         return do_hw_write(codec, reg, value, &data, 2);
  99 }
 100
 101 static unsigned int snd_soc_7_9_read(struct snd_soc_codec *codec,
 102                                      unsigned int reg)
 103 {
 104         return do_hw_read(codec, reg);
 105 }
 106
 107 static int snd_soc_7_9_write(struct snd_soc_codec *codec, unsigned int reg,
 108                              unsigned int value)
 109 {
 110         u8 data[2];
 111
 112         data[0] = (reg << 1) | ((value >> 8) & 0x0001);
 113         data[1] = value & 0x00ff;
 114
 115         return do_hw_write(codec, reg, value, data, 2);
 116 }
 117
 118 static int snd_soc_8_8_write(struct snd_soc_codec *codec, unsigned int reg,
 119                              unsigned int value)
 120 {
 121         u8 data[2];
 122
 123         reg &= 0xff;
 124         data[0] = reg;
 125         data[1] = value & 0xff;
 126
 127         return do_hw_write(codec, reg, value, data, 2);
 128 }
 129
 130 static unsigned int snd_soc_8_8_read(struct snd_soc_codec *codec,
 131                                      unsigned int reg)
 132 {
 133         return do_hw_read(codec, reg);
 134 }
 135
 136 static int snd_soc_8_16_write(struct snd_soc_codec *codec, unsigned int reg,
 137                               unsigned int value)
 138 {
 139         u8 data[3];
 140
 141         data[0] = reg;
 142         data[1] = (value >> 8) & 0xff;
 143         data[2] = value & 0xff;
 144
 145         return do_hw_write(codec, reg, value, data, 3);
 146 }
 147
 148 static unsigned int snd_soc_8_16_read(struct snd_soc_codec *codec,
 149                                       unsigned int reg)
 150 {
 151         return do_hw_read(codec, reg);
 152 }
 153
 154 #if defined(CONFIG_I2C) || (defined(CONFIG_I2C_MODULE) && defined(MODULE))
 155 static unsigned int do_i2c_read(struct snd_soc_codec *codec,
 156                                 void *reg, int reglen,
 157                                 void *data, int datalen)
 158 {
 159         struct i2c_msg xfer[2];
 160         int ret;
 161         struct i2c_client *client = codec->control_data;
 162
 163         /* Write register */
 164         xfer[0].addr = client->addr;
 165         xfer[0].flags = 0;
 166         xfer[0].len = reglen;
 167         xfer[0].buf = reg;
 168
 169         /* Read data */
 170         xfer[1].addr = client->addr;
 171         xfer[1].flags = I2C_M_RD;
 172         xfer[1].len = datalen;
 173         xfer[1].buf = data;
 174
 175         ret = i2c_transfer(client->adapter, xfer, 2);
 176         if (ret == 2)
 177                 return 0;
 178         else if (ret < 0)
 179                 return ret;
 180         else
 181                 return -EIO;
 182 }
 183 #endif
 184
 185 #if defined(CONFIG_I2C) || (defined(CONFIG_I2C_MODULE) && defined(MODULE))
 186 static unsigned int snd_soc_8_8_read_i2c(struct snd_soc_codec *codec,
 187                                          unsigned int r)
 188 {
 189         u8 reg = r;
 190         u8 data;
 191         int ret;
 192
 193         ret = do_i2c_read(codec, &reg, 1, &data, 1);
 194         if (ret < 0)
 195                 return 0;
 196         return data;
 197 }
 198 #else
 199 #define snd_soc_8_8_read_i2c NULL
 200 #endif
 201
 202 #if defined(CONFIG_I2C) || (defined(CONFIG_I2C_MODULE) && defined(MODULE))
 203 static unsigned int snd_soc_8_16_read_i2c(struct snd_soc_codec *codec,
 204                                           unsigned int r)
 205 {
 206         u8 reg = r;
 207         u16 data;
 208         int ret;
 209
 210         ret = do_i2c_read(codec, &reg, 1, &data, 2);
 211         if (ret < 0)
 212                 return 0;
 213         return (data >> 8) | ((data & 0xff) << 8);
 214 }
 215 #else
 216 #define snd_soc_8_16_read_i2c NULL
 217 #endif
 218
 219 #if defined(CONFIG_I2C) || (defined(CONFIG_I2C_MODULE) && defined(MODULE))
 220 static unsigned int snd_soc_16_8_read_i2c(struct snd_soc_codec *codec,
 221                                           unsigned int r)
 222 {
 223         u16 reg = r;
 224         u8 data;
 225         int ret;
 226
 227         ret = do_i2c_read(codec, &reg, 2, &data, 1);
 228         if (ret < 0)
 229                 return 0;
 230         return data;
 231 }
 232 #else
 233 #define snd_soc_16_8_read_i2c NULL
 234 #endif
 235
 236 static unsigned int snd_soc_16_8_read(struct snd_soc_codec *codec,
 237                                       unsigned int reg)
 238 {
 239         return do_hw_read(codec, reg);
 240 }
 241
 242 static int snd_soc_16_8_write(struct snd_soc_codec *codec, unsigned int reg,
 243                               unsigned int value)
 244 {
 245         u8 data[3];
 246
 247         data[0] = (reg >> 8) & 0xff;
 248         data[1] = reg & 0xff;
 249         data[2] = value;
 250
 251         return do_hw_write(codec, reg, value, data, 3);
 252 }
 253
 254 #if defined(CONFIG_I2C) || (defined(CONFIG_I2C_MODULE) && defined(MODULE))
 255 static unsigned int snd_soc_16_16_read_i2c(struct snd_soc_codec *codec,
 256                                            unsigned int r)
 257 {
 258         u16 reg = cpu_to_be16(r);
 259         u16 data;
 260         int ret;
 261
 262         ret = do_i2c_read(codec, &reg, 2, &data, 2);
 263         if (ret < 0)
 264                 return 0;
 265         return be16_to_cpu(data);
 266 }
 267 #else
 268 #define snd_soc_16_16_read_i2c NULL
 269 #endif
 270
 271 static unsigned int snd_soc_16_16_read(struct snd_soc_codec *codec,
 272                                        unsigned int reg)
 273 {
 274         return do_hw_read(codec, reg);
 275 }
 276
 277 static int snd_soc_16_16_write(struct snd_soc_codec *codec, unsigned int reg,
 278                                unsigned int value)
 279 {
 280         u8 data[4];
 281
 282         data[0] = (reg >> 8) & 0xff;
 283         data[1] = reg & 0xff;
 284         data[2] = (value >> 8) & 0xff;
 285         data[3] = value & 0xff;
 286
 287         return do_hw_write(codec, reg, value, data, 4);
 288 }
 289
 290 /* Primitive bulk write support for soc-cache.  The data pointed to by
 291  * `data' needs to already be in the form the hardware expects
 292  * including any leading register specific data.  Any data written
 293  * through this function will not go through the cache as it only
 294  * handles writing to volatile or out of bounds registers.
 295  */
 296 static int snd_soc_hw_bulk_write_raw(struct snd_soc_codec *codec, unsigned int reg,
 297                                      const void *data, size_t len)
 298 {
 299         int ret;
 300
 301         /* To ensure that we don't get out of sync with the cache, check
 302          * whether the base register is volatile or if we've directly asked
 303          * to bypass the cache.  Out of bounds registers are considered
 304          * volatile.
 305          */
 306         if (!codec->cache_bypass
 307             && !snd_soc_codec_volatile_register(codec, reg)
 308             && reg < codec->driver->reg_cache_size)
 309                 return -EINVAL;
 310
 311         switch (codec->control_type) {
 312 #if defined(CONFIG_I2C) || (defined(CONFIG_I2C_MODULE) && defined(MODULE))
 313         case SND_SOC_I2C:
 314                 ret = i2c_master_send(codec->control_data, data, len);
 315                 break;
 316 #endif
 317 #if defined(CONFIG_SPI_MASTER)
 318         case SND_SOC_SPI:
 319                 ret = spi_write(codec->control_data, data, len);
 320                 break;
 321 #endif
 322         default:
 323                 BUG();
 324         }
 325
 326         if (ret == len)
 327                 return 0;
 328         if (ret < 0)
 329                 return ret;
 330         else
 331                 return -EIO;
 332 }
 333
 334 static struct {
 335         int addr_bits;
 336         int data_bits;
 337         int (*write)(struct snd_soc_codec *codec, unsigned int, unsigned int);
 338         unsigned int (*read)(struct snd_soc_codec *, unsigned int);
 339         unsigned int (*i2c_read)(struct snd_soc_codec *, unsigned int);
 340 } io_types[] = {
 341         {
 342                 .addr_bits = 4, .data_bits = 12,
 343                 .write = snd_soc_4_12_write, .read = snd_soc_4_12_read,
 344         },
 345         {
 346                 .addr_bits = 7, .data_bits = 9,
 347                 .write = snd_soc_7_9_write, .read = snd_soc_7_9_read,
 348         },
 349         {
 350                 .addr_bits = 8, .data_bits = 8,
 351                 .write = snd_soc_8_8_write, .read = snd_soc_8_8_read,
 352                 .i2c_read = snd_soc_8_8_read_i2c,
 353         },
 354         {
 355                 .addr_bits = 8, .data_bits = 16,
 356                 .write = snd_soc_8_16_write, .read = snd_soc_8_16_read,
 357                 .i2c_read = snd_soc_8_16_read_i2c,
 358         },
 359         {
 360                 .addr_bits = 16, .data_bits = 8,
 361                 .write = snd_soc_16_8_write, .read = snd_soc_16_8_read,
 362                 .i2c_read = snd_soc_16_8_read_i2c,
 363         },
 364         {
 365                 .addr_bits = 16, .data_bits = 16,
 366                 .write = snd_soc_16_16_write, .read = snd_soc_16_16_read,
 367                 .i2c_read = snd_soc_16_16_read_i2c,
 368         },
 369 };
 370
 371 /**
 372  * snd_soc_codec_set_cache_io: Set up standard I/O functions.
 373  *
 374  * @codec: CODEC to configure.
 375  * @addr_bits: Number of bits of register address data.
 376  * @data_bits: Number of bits of data per register.
 377  * @control: Control bus used.
 378  *
 379  * Register formats are frequently shared between many I2C and SPI
 380  * devices.  In order to promote code reuse the ASoC core provides
 381  * some standard implementations of CODEC read and write operations
 382  * which can be set up using this function.
 383  *
 384  * The caller is responsible for allocating and initialising the
 385  * actual cache.
 386  *
 387  * Note that at present this code cannot be used by CODECs with
 388  * volatile registers.
 389  */
 390 int snd_soc_codec_set_cache_io(struct snd_soc_codec *codec,
 391                                int addr_bits, int data_bits,
 392                                enum snd_soc_control_type control)
 393 {
 394         int i;
 395
 396         for (i = 0; i < ARRAY_SIZE(io_types); i++)
 397                 if (io_types[i].addr_bits == addr_bits &&
 398                     io_types[i].data_bits == data_bits)
 399                         break;
 400         if (i == ARRAY_SIZE(io_types)) {
 401                 printk(KERN_ERR
 402                        "No I/O functions for %d bit address %d bit data\n",
 403                        addr_bits, data_bits);
 404                 return -EINVAL;
 405         }
 406
 407         codec->write = io_types[i].write;
 408         codec->read = io_types[i].read;
 409         codec->bulk_write_raw = snd_soc_hw_bulk_write_raw;
 410
 411         switch (control) {
 412         case SND_SOC_I2C:
 413 #if defined(CONFIG_I2C) || (defined(CONFIG_I2C_MODULE) && defined(MODULE))
 414                 codec->hw_write = (hw_write_t)i2c_master_send;
 415 #endif
 416                 if (io_types[i].i2c_read)
 417                         codec->hw_read = io_types[i].i2c_read;
 418
 419                 codec->control_data = container_of(codec->dev,
 420                                                    struct i2c_client,
 421                                                    dev);
 422                 break;
 423
 424         case SND_SOC_SPI:
 425 #ifdef CONFIG_SPI_MASTER
 426                 codec->hw_write = do_spi_write;
 427 #endif
 428
 429                 codec->control_data = container_of(codec->dev,
 430                                                    struct spi_device,
 431                                                    dev);
 432                 break;
 433         }
 434
 435         return 0;
 436 }
 437 EXPORT_SYMBOL_GPL(snd_soc_codec_set_cache_io);
 438
 439 static bool snd_soc_set_cache_val(void *base, unsigned int idx,
 440                                   unsigned int val, unsigned int word_size)
 441 {
 442         switch (word_size) {
 443         case 1: {
 444                 u8 *cache = base;
 445                 if (cache[idx] == val)
 446                         return true;
 447                 cache[idx] = val;
 448                 break;
 449         }
 450         case 2: {
 451                 u16 *cache = base;
 452                 if (cache[idx] == val)
 453                         return true;
 454                 cache[idx] = val;
 455                 break;
 456         }
 457         default:
 458                 BUG();
 459         }
 460         return false;
 461 }
 462
 463 static unsigned int snd_soc_get_cache_val(const void *base, unsigned int idx,
 464                 unsigned int word_size)
 465 {
 466         if (!base)
 467                 return -1;
 468
 469         switch (word_size) {
 470         case 1: {
 471                 const u8 *cache = base;
 472                 return cache[idx];
 473         }
 474         case 2: {
 475                 const u16 *cache = base;
 476                 return cache[idx];
 477         }
 478         default:
 479                 BUG();
 480         }
 481         /* unreachable */
 482         return -1;
 483 }
 484
 485 struct snd_soc_rbtree_node {
 486         struct rb_node node;
 487         unsigned int reg;
 488         unsigned int value;
 489         unsigned int defval;
 490 } __attribute__ ((packed));
 491
 492 struct snd_soc_rbtree_ctx {
 493         struct rb_root root;
 494 };
 495
 496 static struct snd_soc_rbtree_node *snd_soc_rbtree_lookup(
 497         struct rb_root *root, unsigned int reg)
 498 {
 499         struct rb_node *node;
 500         struct snd_soc_rbtree_node *rbnode;
 501
 502         node = root->rb_node;
 503         while (node) {
 504                 rbnode = container_of(node, struct snd_soc_rbtree_node, node);
 505                 if (rbnode->reg < reg)
 506                         node = node->rb_left;
 507                 else if (rbnode->reg > reg)
 508                         node = node->rb_right;
 509                 else
 510                         return rbnode;
 511         }
 512
 513         return NULL;
 514 }
 515
 516 static int snd_soc_rbtree_insert(struct rb_root *root,
 517                                  struct snd_soc_rbtree_node *rbnode)
 518 {
 519         struct rb_node **new, *parent;
 520         struct snd_soc_rbtree_node *rbnode_tmp;
 521
 522         parent = NULL;
 523         new = &root->rb_node;
 524         while (*new) {
 525                 rbnode_tmp = container_of(*new, struct snd_soc_rbtree_node,
 526                                           node);
 527                 parent = *new;
 528                 if (rbnode_tmp->reg < rbnode->reg)
 529                         new = &((*new)->rb_left);
 530                 else if (rbnode_tmp->reg > rbnode->reg)
 531                         new = &((*new)->rb_right);
 532                 else
 533                         return 0;
 534         }
 535
 536         /* insert the node into the rbtree */
 537         rb_link_node(&rbnode->node, parent, new);
 538         rb_insert_color(&rbnode->node, root);
 539
 540         return 1;
 541 }
 542
 543 static int snd_soc_rbtree_cache_sync(struct snd_soc_codec *codec)
 544 {
 545         struct snd_soc_rbtree_ctx *rbtree_ctx;
 546         struct rb_node *node;
 547         struct snd_soc_rbtree_node *rbnode;
 548         unsigned int val;
 549         int ret;
 550
 551         rbtree_ctx = codec->reg_cache;
 552         for (node = rb_first(&rbtree_ctx->root); node; node = rb_next(node)) {
 553                 rbnode = rb_entry(node, struct snd_soc_rbtree_node, node);
 554                 if (rbnode->value == rbnode->defval)
 555                         continue;
 556                 WARN_ON(codec->writable_register &&
 557                         codec->writable_register(codec, rbnode->reg));
 558                 ret = snd_soc_cache_read(codec, rbnode->reg, &val);
 559                 if (ret)
 560                         return ret;
 561                 codec->cache_bypass = 1;
 562                 ret = snd_soc_write(codec, rbnode->reg, val);
 563                 codec->cache_bypass = 0;
 564                 if (ret)
 565                         return ret;
 566                 dev_dbg(codec->dev, "Synced register %#x, value = %#x\n",
 567                         rbnode->reg, val);
 568         }
 569
 570         return 0;
 571 }
 572
 573 static int snd_soc_rbtree_cache_write(struct snd_soc_codec *codec,
 574                                       unsigned int reg, unsigned int value)
 575 {
 576         struct snd_soc_rbtree_ctx *rbtree_ctx;
 577         struct snd_soc_rbtree_node *rbnode;
 578
 579         rbtree_ctx = codec->reg_cache;
 580         rbnode = snd_soc_rbtree_lookup(&rbtree_ctx->root, reg);
 581         if (rbnode) {
 582                 if (rbnode->value == value)
 583                         return 0;
 584                 rbnode->value = value;
 585         } else {
 586                 /* bail out early, no need to create the rbnode yet */
 587                 if (!value)
 588                         return 0;
 589                 /*
 590                  * for uninitialized registers whose value is changed
 591                  * from the default zero, create an rbnode and insert
 592                  * it into the tree.
 593                  */
 594                 rbnode = kzalloc(sizeof *rbnode, GFP_KERNEL);
 595                 if (!rbnode)
 596                         return -ENOMEM;
 597                 rbnode->reg = reg;
 598                 rbnode->value = value;
 599                 snd_soc_rbtree_insert(&rbtree_ctx->root, rbnode);
 600         }
 601
 602         return 0;
 603 }
 604
 605 static int snd_soc_rbtree_cache_read(struct snd_soc_codec *codec,
 606                                      unsigned int reg, unsigned int *value)
 607 {
 608         struct snd_soc_rbtree_ctx *rbtree_ctx;
 609         struct snd_soc_rbtree_node *rbnode;
 610
 611         rbtree_ctx = codec->reg_cache;
 612         rbnode = snd_soc_rbtree_lookup(&rbtree_ctx->root, reg);
 613         if (rbnode) {
 614                 *value = rbnode->value;
 615         } else {
 616                 /* uninitialized registers default to 0 */
 617                 *value = 0;
 618         }
 619
 620         return 0;
 621 }
 622
 623 static int snd_soc_rbtree_cache_exit(struct snd_soc_codec *codec)
 624 {
 625         struct rb_node *next;
 626         struct snd_soc_rbtree_ctx *rbtree_ctx;
 627         struct snd_soc_rbtree_node *rbtree_node;
 628
 629         /* if we've already been called then just return */
 630         rbtree_ctx = codec->reg_cache;
 631         if (!rbtree_ctx)
 632                 return 0;
 633
 634         /* free up the rbtree */
 635         next = rb_first(&rbtree_ctx->root);
 636         while (next) {
 637                 rbtree_node = rb_entry(next, struct snd_soc_rbtree_node, node);
 638                 next = rb_next(&rbtree_node->node);
 639                 rb_erase(&rbtree_node->node, &rbtree_ctx->root);
 640                 kfree(rbtree_node);
 641         }
 642
 643         /* release the resources */
 644         kfree(codec->reg_cache);
 645         codec->reg_cache = NULL;
 646
 647         return 0;
 648 }
 649
 650 static int snd_soc_rbtree_cache_init(struct snd_soc_codec *codec)
 651 {
 652         struct snd_soc_rbtree_node *rbtree_node;
 653         struct snd_soc_rbtree_ctx *rbtree_ctx;
 654         unsigned int val;
 655         unsigned int word_size;
 656         int i;
 657         int ret;
 658
 659         codec->reg_cache = kmalloc(sizeof *rbtree_ctx, GFP_KERNEL);
 660         if (!codec->reg_cache)
 661                 return -ENOMEM;
 662
 663         rbtree_ctx = codec->reg_cache;
 664         rbtree_ctx->root = RB_ROOT;
 665
 666         if (!codec->reg_def_copy)
 667                 return 0;
 668
 669         /*
 670          * populate the rbtree with the initialized registers.  All other
 671          * registers will be inserted when they are first modified.
 672          */
 673         word_size = codec->driver->reg_word_size;
 674         for (i = 0; i < codec->driver->reg_cache_size; ++i) {
 675                 val = snd_soc_get_cache_val(codec->reg_def_copy, i, word_size);
 676                 if (!val)
 677                         continue;
 678                 rbtree_node = kzalloc(sizeof *rbtree_node, GFP_KERNEL);
 679                 if (!rbtree_node) {
 680                         ret = -ENOMEM;
 681                         snd_soc_cache_exit(codec);
 682                         break;
 683                 }
 684                 rbtree_node->reg = i;
 685                 rbtree_node->value = val;
 686                 rbtree_node->defval = val;
 687                 snd_soc_rbtree_insert(&rbtree_ctx->root, rbtree_node);
 688         }
 689
 690         return 0;
 691 }
 692
 693 #ifdef CONFIG_SND_SOC_CACHE_LZO
 694 struct snd_soc_lzo_ctx {
 695         void *wmem;
 696         void *dst;
 697         const void *src;
 698         size_t src_len;
 699         size_t dst_len;
 700         size_t decompressed_size;
 701         unsigned long *sync_bmp;
 702         int sync_bmp_nbits;
 703 };
 704
 705 #define LZO_BLOCK_NUM 8
 706 static int snd_soc_lzo_block_count(void)
 707 {
 708         return LZO_BLOCK_NUM;
 709 }
 710
 711 static int snd_soc_lzo_prepare(struct snd_soc_lzo_ctx *lzo_ctx)
 712 {
 713         lzo_ctx->wmem = kmalloc(LZO1X_MEM_COMPRESS, GFP_KERNEL);
 714         if (!lzo_ctx->wmem)
 715                 return -ENOMEM;
 716         return 0;
 717 }
 718
 719 static int snd_soc_lzo_compress(struct snd_soc_lzo_ctx *lzo_ctx)
 720 {
 721         size_t compress_size;
 722         int ret;
 723
 724         ret = lzo1x_1_compress(lzo_ctx->src, lzo_ctx->src_len,
 725                                lzo_ctx->dst, &compress_size, lzo_ctx->wmem);
 726         if (ret != LZO_E_OK || compress_size > lzo_ctx->dst_len)
 727                 return -EINVAL;
 728         lzo_ctx->dst_len = compress_size;
 729         return 0;
 730 }
 731
 732 static int snd_soc_lzo_decompress(struct snd_soc_lzo_ctx *lzo_ctx)
 733 {
 734         size_t dst_len;
 735         int ret;
 736
 737         dst_len = lzo_ctx->dst_len;
 738         ret = lzo1x_decompress_safe(lzo_ctx->src, lzo_ctx->src_len,
 739                                     lzo_ctx->dst, &dst_len);
 740         if (ret != LZO_E_OK || dst_len != lzo_ctx->dst_len)
 741                 return -EINVAL;
 742         return 0;
 743 }
 744
 745 static int snd_soc_lzo_compress_cache_block(struct snd_soc_codec *codec,
 746                 struct snd_soc_lzo_ctx *lzo_ctx)
 747 {
 748         int ret;
 749
 750         lzo_ctx->dst_len = lzo1x_worst_compress(PAGE_SIZE);
 751         lzo_ctx->dst = kmalloc(lzo_ctx->dst_len, GFP_KERNEL);
 752         if (!lzo_ctx->dst) {
 753                 lzo_ctx->dst_len = 0;
 754                 return -ENOMEM;
 755         }
 756
 757         ret = snd_soc_lzo_compress(lzo_ctx);
 758         if (ret < 0)
 759                 return ret;
 760         return 0;
 761 }
 762
 763 static int snd_soc_lzo_decompress_cache_block(struct snd_soc_codec *codec,
 764                 struct snd_soc_lzo_ctx *lzo_ctx)
 765 {
 766         int ret;
 767
 768         lzo_ctx->dst_len = lzo_ctx->decompressed_size;
 769         lzo_ctx->dst = kmalloc(lzo_ctx->dst_len, GFP_KERNEL);
 770         if (!lzo_ctx->dst) {
 771                 lzo_ctx->dst_len = 0;
 772                 return -ENOMEM;
 773         }
 774
 775         ret = snd_soc_lzo_decompress(lzo_ctx);
 776         if (ret < 0)
 777                 return ret;
 778         return 0;
 779 }
 780
 781 static inline int snd_soc_lzo_get_blkindex(struct snd_soc_codec *codec,
 782                 unsigned int reg)
 783 {
 784         const struct snd_soc_codec_driver *codec_drv;
 785
 786         codec_drv = codec->driver;
 787         return (reg * codec_drv->reg_word_size) /
 788                DIV_ROUND_UP(codec->reg_size, snd_soc_lzo_block_count());
 789 }
 790
 791 static inline int snd_soc_lzo_get_blkpos(struct snd_soc_codec *codec,
 792                 unsigned int reg)
 793 {
 794         const struct snd_soc_codec_driver *codec_drv;
 795
 796         codec_drv = codec->driver;
 797         return reg % (DIV_ROUND_UP(codec->reg_size, snd_soc_lzo_block_count()) /
 798                       codec_drv->reg_word_size);
 799 }
 800
 801 static inline int snd_soc_lzo_get_blksize(struct snd_soc_codec *codec)
 802 {
 803         const struct snd_soc_codec_driver *codec_drv;
 804
 805         codec_drv = codec->driver;
 806         return DIV_ROUND_UP(codec->reg_size, snd_soc_lzo_block_count());
 807 }
 808
 809 static int snd_soc_lzo_cache_sync(struct snd_soc_codec *codec)
 810 {
 811         struct snd_soc_lzo_ctx **lzo_blocks;
 812         unsigned int val;
 813         int i;
 814         int ret;
 815
 816         lzo_blocks = codec->reg_cache;
 817         for_each_set_bit(i, lzo_blocks[0]->sync_bmp, lzo_blocks[0]->sync_bmp_nbits) {
 818                 WARN_ON(codec->writable_register &&
 819                         codec->writable_register(codec, i));
 820                 ret = snd_soc_cache_read(codec, i, &val);
 821                 if (ret)
 822                         return ret;
 823                 codec->cache_bypass = 1;
 824                 ret = snd_soc_write(codec, i, val);
 825                 codec->cache_bypass = 0;
 826                 if (ret)
 827                         return ret;
 828                 dev_dbg(codec->dev, "Synced register %#x, value = %#x\n",
 829                         i, val);
 830         }
 831
 832         return 0;
 833 }
 834
 835 static int snd_soc_lzo_cache_write(struct snd_soc_codec *codec,
 836                                    unsigned int reg, unsigned int value)
 837 {
 838         struct snd_soc_lzo_ctx *lzo_block, **lzo_blocks;
 839         int ret, blkindex, blkpos;
 840         size_t blksize, tmp_dst_len;
 841         void *tmp_dst;
 842
 843         /* index of the compressed lzo block */
 844         blkindex = snd_soc_lzo_get_blkindex(codec, reg);
 845         /* register index within the decompressed block */
 846         blkpos = snd_soc_lzo_get_blkpos(codec, reg);
 847         /* size of the compressed block */
 848         blksize = snd_soc_lzo_get_blksize(codec);
 849         lzo_blocks = codec->reg_cache;
 850         lzo_block = lzo_blocks[blkindex];
 851
 852         /* save the pointer and length of the compressed block */
 853         tmp_dst = lzo_block->dst;
 854         tmp_dst_len = lzo_block->dst_len;
 855
 856         /* prepare the source to be the compressed block */
 857         lzo_block->src = lzo_block->dst;
 858         lzo_block->src_len = lzo_block->dst_len;
 859
 860         /* decompress the block */
 861         ret = snd_soc_lzo_decompress_cache_block(codec, lzo_block);
 862         if (ret < 0) {
 863                 kfree(lzo_block->dst);
 864                 goto out;
 865         }
 866
 867         /* write the new value to the cache */
 868         if (snd_soc_set_cache_val(lzo_block->dst, blkpos, value,
 869                                   codec->driver->reg_word_size)) {
 870                 kfree(lzo_block->dst);
 871                 goto out;
 872         }
 873
 874         /* prepare the source to be the decompressed block */
 875         lzo_block->src = lzo_block->dst;
 876         lzo_block->src_len = lzo_block->dst_len;
 877
 878         /* compress the block */
 879         ret = snd_soc_lzo_compress_cache_block(codec, lzo_block);
 880         if (ret < 0) {
 881                 kfree(lzo_block->dst);
 882                 kfree(lzo_block->src);
 883                 goto out;
 884         }
 885
 886         /* set the bit so we know we have to sync this register */
 887         set_bit(reg, lzo_block->sync_bmp);
 888         kfree(tmp_dst);
 889         kfree(lzo_block->src);
 890         return 0;
 891 out:
 892         lzo_block->dst = tmp_dst;
 893         lzo_block->dst_len = tmp_dst_len;
 894         return ret;
 895 }
 896
 897 static int snd_soc_lzo_cache_read(struct snd_soc_codec *codec,
 898                                   unsigned int reg, unsigned int *value)
 899 {
 900         struct snd_soc_lzo_ctx *lzo_block, **lzo_blocks;
 901         int ret, blkindex, blkpos;
 902         size_t blksize, tmp_dst_len;
 903         void *tmp_dst;
 904
 905         *value = 0;
 906         /* index of the compressed lzo block */
 907         blkindex = snd_soc_lzo_get_blkindex(codec, reg);
 908         /* register index within the decompressed block */
 909         blkpos = snd_soc_lzo_get_blkpos(codec, reg);
 910         /* size of the compressed block */
 911         blksize = snd_soc_lzo_get_blksize(codec);
 912         lzo_blocks = codec->reg_cache;
 913         lzo_block = lzo_blocks[blkindex];
 914
 915         /* save the pointer and length of the compressed block */
 916         tmp_dst = lzo_block->dst;
 917         tmp_dst_len = lzo_block->dst_len;
 918
 919         /* prepare the source to be the compressed block */
 920         lzo_block->src = lzo_block->dst;
 921         lzo_block->src_len = lzo_block->dst_len;
 922
 923         /* decompress the block */
 924         ret = snd_soc_lzo_decompress_cache_block(codec, lzo_block);
 925         if (ret >= 0)
 926                 /* fetch the value from the cache */
 927                 *value = snd_soc_get_cache_val(lzo_block->dst, blkpos,
 928                                                codec->driver->reg_word_size);
 929
 930         kfree(lzo_block->dst);
 931         /* restore the pointer and length of the compressed block */
 932         lzo_block->dst = tmp_dst;
 933         lzo_block->dst_len = tmp_dst_len;
 934         return 0;
 935 }
 936
 937 static int snd_soc_lzo_cache_exit(struct snd_soc_codec *codec)
 938 {
 939         struct snd_soc_lzo_ctx **lzo_blocks;
 940         int i, blkcount;
 941
 942         lzo_blocks = codec->reg_cache;
 943         if (!lzo_blocks)
 944                 return 0;
 945
 946         blkcount = snd_soc_lzo_block_count();
 947         /*
 948          * the pointer to the bitmap used for syncing the cache
 949          * is shared amongst all lzo_blocks.  Ensure it is freed
 950          * only once.
 951          */
 952         if (lzo_blocks[0])
 953                 kfree(lzo_blocks[0]->sync_bmp);
 954         for (i = 0; i < blkcount; ++i) {
 955                 if (lzo_blocks[i]) {
 956                         kfree(lzo_blocks[i]->wmem);
 957                         kfree(lzo_blocks[i]->dst);
 958                 }
 959                 /* each lzo_block is a pointer returned by kmalloc or NULL */
 960                 kfree(lzo_blocks[i]);
 961         }
 962         kfree(lzo_blocks);
 963         codec->reg_cache = NULL;
 964         return 0;
 965 }
 966
 967 static int snd_soc_lzo_cache_init(struct snd_soc_codec *codec)
 968 {
 969         struct snd_soc_lzo_ctx **lzo_blocks;
 970         size_t bmp_size;
 971         const struct snd_soc_codec_driver *codec_drv;
 972         int ret, tofree, i, blksize, blkcount;
 973         const char *p, *end;
 974         unsigned long *sync_bmp;
 975
 976         ret = 0;
 977         codec_drv = codec->driver;
 978
 979         /*
 980          * If we have not been given a default register cache
 981          * then allocate a dummy zero-ed out region, compress it
 982          * and remember to free it afterwards.
 983          */
 984         tofree = 0;
 985         if (!codec->reg_def_copy)
 986                 tofree = 1;
 987
 988         if (!codec->reg_def_copy) {
 989                 codec->reg_def_copy = kzalloc(codec->reg_size, GFP_KERNEL);
 990                 if (!codec->reg_def_copy)
 991                         return -ENOMEM;
 992         }
 993
 994         blkcount = snd_soc_lzo_block_count();
 995         codec->reg_cache = kzalloc(blkcount * sizeof *lzo_blocks,
 996                                    GFP_KERNEL);
 997         if (!codec->reg_cache) {
 998                 ret = -ENOMEM;
 999                 goto err_tofree;
1000         }
1001         lzo_blocks = codec->reg_cache;
1002
1003         /*
1004          * allocate a bitmap to be used when syncing the cache with
1005          * the hardware.  Each time a register is modified, the corresponding
1006          * bit is set in the bitmap, so we know that we have to sync
1007          * that register.
1008          */
1009         bmp_size = codec_drv->reg_cache_size;
1010         sync_bmp = kmalloc(BITS_TO_LONGS(bmp_size) * sizeof(long),
1011                            GFP_KERNEL);
1012         if (!sync_bmp) {
1013                 ret = -ENOMEM;
1014                 goto err;
1015         }
1016         bitmap_zero(sync_bmp, bmp_size);
1017
1018         /* allocate the lzo blocks and initialize them */
1019         for (i = 0; i < blkcount; ++i) {
1020                 lzo_blocks[i] = kzalloc(sizeof **lzo_blocks,
1021                                         GFP_KERNEL);
1022                 if (!lzo_blocks[i]) {
1023                         kfree(sync_bmp);
1024                         ret = -ENOMEM;
1025                         goto err;
1026                 }
1027                 lzo_blocks[i]->sync_bmp = sync_bmp;
1028                 lzo_blocks[i]->sync_bmp_nbits = bmp_size;
1029                 /* alloc the working space for the compressed block */
1030                 ret = snd_soc_lzo_prepare(lzo_blocks[i]);
1031                 if (ret < 0)
1032                         goto err;
1033         }
1034
1035         blksize = snd_soc_lzo_get_blksize(codec);
1036         p = codec->reg_def_copy;
1037         end = codec->reg_def_copy + codec->reg_size;
1038         /* compress the register map and fill the lzo blocks */
1039         for (i = 0; i < blkcount; ++i, p += blksize) {
1040                 lzo_blocks[i]->src = p;
1041                 if (p + blksize > end)
1042                         lzo_blocks[i]->src_len = end - p;
1043                 else
1044                         lzo_blocks[i]->src_len = blksize;
1045                 ret = snd_soc_lzo_compress_cache_block(codec,
1046                                                        lzo_blocks[i]);
1047                 if (ret < 0)
1048                         goto err;
1049                 lzo_blocks[i]->decompressed_size =
1050                         lzo_blocks[i]->src_len;
1051         }
1052
1053         if (tofree) {
1054                 kfree(codec->reg_def_copy);
1055                 codec->reg_def_copy = NULL;
1056         }
1057         return 0;
1058 err:
1059         snd_soc_cache_exit(codec);
1060 err_tofree:
1061         if (tofree) {
1062                 kfree(codec->reg_def_copy);
1063                 codec->reg_def_copy = NULL;
1064         }
1065         return ret;
1066 }
1067 #endif
1068
1069 static int snd_soc_flat_cache_sync(struct snd_soc_codec *codec)
1070 {
1071         int i;
1072         int ret;
1073         const struct snd_soc_codec_driver *codec_drv;
1074         unsigned int val;
1075
1076         codec_drv = codec->driver;
1077         for (i = 0; i < codec_drv->reg_cache_size; ++i) {
1078                 WARN_ON(codec->writable_register &&
1079                         codec->writable_register(codec, i));
1080                 ret = snd_soc_cache_read(codec, i, &val);
1081                 if (ret)
1082                         return ret;
1083                 if (codec->reg_def_copy)
1084                         if (snd_soc_get_cache_val(codec->reg_def_copy,
1085                                                   i, codec_drv->reg_word_size) == val)
1086                                 continue;
1087                 ret = snd_soc_write(codec, i, val);
1088                 if (ret)
1089                         return ret;
1090                 dev_dbg(codec->dev, "Synced register %#x, value = %#x\n",
1091                         i, val);
1092         }
1093         return 0;
1094 }
1095
1096 static int snd_soc_flat_cache_write(struct snd_soc_codec *codec,
1097                                     unsigned int reg, unsigned int value)
1098 {
1099         snd_soc_set_cache_val(codec->reg_cache, reg, value,
1100                               codec->driver->reg_word_size);
1101         return 0;
1102 }
1103
1104 static int snd_soc_flat_cache_read(struct snd_soc_codec *codec,
1105                                    unsigned int reg, unsigned int *value)
1106 {
1107         *value = snd_soc_get_cache_val(codec->reg_cache, reg,
1108                                        codec->driver->reg_word_size);
1109         return 0;
1110 }
1111
1112 static int snd_soc_flat_cache_exit(struct snd_soc_codec *codec)
1113 {
1114         if (!codec->reg_cache)
1115                 return 0;
1116         kfree(codec->reg_cache);
1117         codec->reg_cache = NULL;
1118         return 0;
1119 }
1120
1121 static int snd_soc_flat_cache_init(struct snd_soc_codec *codec)
1122 {
1123         const struct snd_soc_codec_driver *codec_drv;
1124
1125         codec_drv = codec->driver;
1126
1127         if (codec->reg_def_copy)
1128                 codec->reg_cache = kmemdup(codec->reg_def_copy,
1129                                            codec->reg_size, GFP_KERNEL);
1130         else
1131                 codec->reg_cache = kzalloc(codec->reg_size, GFP_KERNEL);
1132         if (!codec->reg_cache)
1133                 return -ENOMEM;
1134
1135         return 0;
1136 }
1137
1138 /* an array of all supported compression types */
1139 static const struct snd_soc_cache_ops cache_types[] = {
1140         /* Flat *must* be the first entry for fallback */
1141         {
1142                 .id = SND_SOC_FLAT_COMPRESSION,
1143                 .name = "flat",
1144                 .init = snd_soc_flat_cache_init,
1145                 .exit = snd_soc_flat_cache_exit,
1146                 .read = snd_soc_flat_cache_read,
1147                 .write = snd_soc_flat_cache_write,
1148                 .sync = snd_soc_flat_cache_sync
1149         },
1150 #ifdef CONFIG_SND_SOC_CACHE_LZO
1151         {
1152                 .id = SND_SOC_LZO_COMPRESSION,
1153                 .name = "LZO",
1154                 .init = snd_soc_lzo_cache_init,
1155                 .exit = snd_soc_lzo_cache_exit,
1156                 .read = snd_soc_lzo_cache_read,
1157                 .write = snd_soc_lzo_cache_write,
1158                 .sync = snd_soc_lzo_cache_sync
1159         },
1160 #endif
1161         {
1162                 .id = SND_SOC_RBTREE_COMPRESSION,
1163                 .name = "rbtree",
1164                 .init = snd_soc_rbtree_cache_init,
1165                 .exit = snd_soc_rbtree_cache_exit,
1166                 .read = snd_soc_rbtree_cache_read,
1167                 .write = snd_soc_rbtree_cache_write,
1168                 .sync = snd_soc_rbtree_cache_sync
1169         }
1170 };
1171
1172 int snd_soc_cache_init(struct snd_soc_codec *codec)
1173 {
1174         int i;
1175
1176         for (i = 0; i < ARRAY_SIZE(cache_types); ++i)
1177                 if (cache_types[i].id == codec->compress_type)
1178                         break;
1179
1180         /* Fall back to flat compression */
1181         if (i == ARRAY_SIZE(cache_types)) {
1182                 dev_warn(codec->dev, "Could not match compress type: %d\n",
1183                          codec->compress_type);
1184                 i = 0;
1185         }
1186
1187         mutex_init(&codec->cache_rw_mutex);
1188         codec->cache_ops = &cache_types[i];
1189
1190         if (codec->cache_ops->init) {
1191                 if (codec->cache_ops->name)
1192                         dev_dbg(codec->dev, "Initializing %s cache for %s codec\n",
1193                                 codec->cache_ops->name, codec->name);
1194                 return codec->cache_ops->init(codec);
1195         }
1196         return -ENOSYS;
1197 }
1198
1199 /*
1200  * NOTE: keep in mind that this function might be called
1201  * multiple times.
1202  */
1203 int snd_soc_cache_exit(struct snd_soc_codec *codec)
1204 {
1205         if (codec->cache_ops && codec->cache_ops->exit) {
1206                 if (codec->cache_ops->name)
1207                         dev_dbg(codec->dev, "Destroying %s cache for %s codec\n",
1208                                 codec->cache_ops->name, codec->name);
1209                 return codec->cache_ops->exit(codec);
1210         }
1211         return -ENOSYS;
1212 }
1213
1214 /**
1215  * snd_soc_cache_read: Fetch the value of a given register from the cache.
1216  *
1217  * @codec: CODEC to configure.
1218  * @reg: The register index.
1219  * @value: The value to be returned.
1220  */
1221 int snd_soc_cache_read(struct snd_soc_codec *codec,
1222                        unsigned int reg, unsigned int *value)
1223 {
1224         int ret;
1225
1226         mutex_lock(&codec->cache_rw_mutex);
1227
1228         if (value && codec->cache_ops && codec->cache_ops->read) {
1229                 ret = codec->cache_ops->read(codec, reg, value);
1230                 mutex_unlock(&codec->cache_rw_mutex);
1231                 return ret;
1232         }
1233
1234         mutex_unlock(&codec->cache_rw_mutex);
1235         return -ENOSYS;
1236 }
1237 EXPORT_SYMBOL_GPL(snd_soc_cache_read);
1238
1239 /**
1240  * snd_soc_cache_write: Set the value of a given register in the cache.
1241  *
1242  * @codec: CODEC to configure.
1243  * @reg: The register index.
1244  * @value: The new register value.
1245  */
1246 int snd_soc_cache_write(struct snd_soc_codec *codec,
1247                         unsigned int reg, unsigned int value)
1248 {
1249         int ret;
1250
1251         mutex_lock(&codec->cache_rw_mutex);
1252
1253         if (codec->cache_ops && codec->cache_ops->write) {
1254                 ret = codec->cache_ops->write(codec, reg, value);
1255                 mutex_unlock(&codec->cache_rw_mutex);
1256                 return ret;
1257         }
1258
1259         mutex_unlock(&codec->cache_rw_mutex);
1260         return -ENOSYS;
1261 }
1262 EXPORT_SYMBOL_GPL(snd_soc_cache_write);
1263
1264 /**
1265  * snd_soc_cache_sync: Sync the register cache with the hardware.
1266  *
1267  * @codec: CODEC to configure.
1268  *
1269  * Any registers that should not be synced should be marked as
1270  * volatile.  In general drivers can choose not to use the provided
1271  * syncing functionality if they so require.
1272  */
1273 int snd_soc_cache_sync(struct snd_soc_codec *codec)
1274 {
1275         int ret;
1276         const char *name;
1277
1278         if (!codec->cache_sync) {
1279                 return 0;
1280         }
1281
1282         if (!codec->cache_ops || !codec->cache_ops->sync)
1283                 return -ENOSYS;
1284
1285         if (codec->cache_ops->name)
1286                 name = codec->cache_ops->name;
1287         else
1288                 name = "unknown";
1289
1290         if (codec->cache_ops->name)
1291                 dev_dbg(codec->dev, "Syncing %s cache for %s codec\n",
1292                         codec->cache_ops->name, codec->name);
1293         trace_snd_soc_cache_sync(codec, name, "start");
1294         ret = codec->cache_ops->sync(codec);
1295         if (!ret)
1296                 codec->cache_sync = 0;
1297         trace_snd_soc_cache_sync(codec, name, "end");
1298         return ret;
1299 }
1300 EXPORT_SYMBOL_GPL(snd_soc_cache_sync);
1301
1302 static int snd_soc_get_reg_access_index(struct snd_soc_codec *codec,
1303                                         unsigned int reg)
1304 {
1305         const struct snd_soc_codec_driver *codec_drv;
1306         unsigned int min, max, index;
1307
1308         codec_drv = codec->driver;
1309         min = 0;
1310         max = codec_drv->reg_access_size - 1;
1311         do {
1312                 index = (min + max) / 2;
1313                 if (codec_drv->reg_access_default[index].reg == reg)
1314                         return index;
1315                 if (codec_drv->reg_access_default[index].reg < reg)
1316                         min = index + 1;
1317                 else
1318                         max = index;
1319         } while (min <= max);
1320         return -1;
1321 }
1322
1323 int snd_soc_default_volatile_register(struct snd_soc_codec *codec,
1324                                       unsigned int reg)
1325 {
1326         int index;
1327
1328         if (reg >= codec->driver->reg_cache_size)
1329                 return 1;
1330         index = snd_soc_get_reg_access_index(codec, reg);
1331         if (index < 0)
1332                 return 0;
1333         return codec->driver->reg_access_default[index].vol;
1334 }
1335 EXPORT_SYMBOL_GPL(snd_soc_default_volatile_register);
1336
1337 int snd_soc_default_readable_register(struct snd_soc_codec *codec,
1338                                       unsigned int reg)
1339 {
1340         int index;
1341
1342         if (reg >= codec->driver->reg_cache_size)
1343                 return 1;
1344         index = snd_soc_get_reg_access_index(codec, reg);
1345         if (index < 0)
1346                 return 0;
1347         return codec->driver->reg_access_default[index].read;
1348 }
1349 EXPORT_SYMBOL_GPL(snd_soc_default_readable_register);
1350
1351 int snd_soc_default_writable_register(struct snd_soc_codec *codec,
1352                                       unsigned int reg)
1353 {
1354         int index;
1355
1356         if (reg >= codec->driver->reg_cache_size)
1357                 return 1;
1358         index = snd_soc_get_reg_access_index(codec, reg);
1359         if (index < 0)
1360                 return 0;
1361         return codec->driver->reg_access_default[index].write;
1362 }
1363 EXPORT_SYMBOL_GPL(snd_soc_default_writable_register);