drivers/staging/dream/qdsp5/adsp.c

   1 /* arch/arm/mach-msm/qdsp5/adsp.c
   2  *
   3  * Register/Interrupt access for userspace aDSP library.
   4  *
   5  * Copyright (c) 2008 QUALCOMM Incorporated
   6  * Copyright (C) 2008 Google, Inc.
   7  * Author: Iliyan Malchev <ibm@android.com>
   8  *
   9  * This software is licensed under the terms of the GNU General Public
  10  * License version 2, as published by the Free Software Foundation, and
  11  * may be copied, distributed, and modified under those terms.
  12  *
  13  * This program is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  * GNU General Public License for more details.
  17  *
  18  */
  19
  20 /* TODO:
  21  * - move shareable rpc code outside of adsp.c
  22  * - general solution for virt->phys patchup
  23  * - queue IDs should be relative to modules
  24  * - disallow access to non-associated queues
  25  */
  26
  27 #include <linux/clk.h>
  28 #include <linux/delay.h>
  29 #include <linux/interrupt.h>
  30 #include <linux/kernel.h>
  31 #include <linux/kthread.h>
  32 #include <linux/module.h>
  33 #include <linux/uaccess.h>
  34 #include <linux/wait.h>
  35
  36 static inline void prevent_suspend(void)
  37 {
  38 }
  39 static inline void allow_suspend(void)
  40 {
  41 }
  42
  43 #include <linux/io.h>
  44 #include <mach/msm_iomap.h>
  45 #include "adsp.h"
  46
  47 #define INT_ADSP INT_ADSP_A9_A11
  48
  49 static struct adsp_info adsp_info;
  50 static struct msm_rpc_endpoint *rpc_cb_server_client;
  51 static struct msm_adsp_module *adsp_modules;
  52 static int adsp_open_count;
  53 static DEFINE_MUTEX(adsp_open_lock);
  54
  55 /* protect interactions with the ADSP command/message queue */
  56 static spinlock_t adsp_cmd_lock;
  57
  58 static uint32_t current_image = -1;
  59
  60 void adsp_set_image(struct adsp_info *info, uint32_t image)
  61 {
  62         current_image = image;
  63 }
  64
  65 /*
  66  * Checks whether the module_id is available in the
  67  * module_entries table.If module_id is available returns `0`.
  68  * If module_id is not available returns `-ENXIO`.
  69  */
  70 #if CONFIG_MSM_AMSS_VERSION >= 6350
  71 static int32_t adsp_validate_module(uint32_t module_id)
  72 {
  73         uint32_t        *ptr;
  74         uint32_t        module_index;
  75         uint32_t        num_mod_entries;
  76
  77         ptr = adsp_info.init_info_ptr->module_entries;
  78         num_mod_entries = adsp_info.init_info_ptr->module_table_size;
  79
  80         for (module_index = 0; module_index < num_mod_entries; module_index++)
  81                 if (module_id == ptr[module_index])
  82                         return 0;
  83
  84         return -ENXIO;
  85 }
  86 #else
  87 static inline int32_t adsp_validate_module(uint32_t module_id) { return 0; }
  88 #endif
  89
  90 uint32_t adsp_get_module(struct adsp_info *info, uint32_t task)
  91 {
  92         BUG_ON(current_image == -1UL);
  93         return info->task_to_module[current_image][task];
  94 }
  95
  96 uint32_t adsp_get_queue_offset(struct adsp_info *info, uint32_t queue_id)
  97 {
  98         BUG_ON(current_image == -1UL);
  99         return info->queue_offset[current_image][queue_id];
 100 }
 101
 102 static int rpc_adsp_rtos_app_to_modem(uint32_t cmd, uint32_t module,
 103                                       struct msm_adsp_module *adsp_module)
 104 {
 105         int rc;
 106         struct rpc_adsp_rtos_app_to_modem_args_t rpc_req;
 107         struct rpc_reply_hdr *rpc_rsp;
 108
 109         msm_rpc_setup_req(&rpc_req.hdr,
 110                           RPC_ADSP_RTOS_ATOM_PROG,
 111                           msm_rpc_get_vers(adsp_module->rpc_client),
 112                           RPC_ADSP_RTOS_APP_TO_MODEM_PROC);
 113
 114         rpc_req.gotit = cpu_to_be32(1);
 115         rpc_req.cmd = cpu_to_be32(cmd);
 116         rpc_req.proc_id = cpu_to_be32(RPC_ADSP_RTOS_PROC_APPS);
 117         rpc_req.module = cpu_to_be32(module);
 118         rc = msm_rpc_write(adsp_module->rpc_client, &rpc_req, sizeof(rpc_req));
 119         if (rc < 0) {
 120                 pr_err("adsp: could not send RPC request: %d\n", rc);
 121                 return rc;
 122         }
 123
 124         rc = msm_rpc_read(adsp_module->rpc_client,
 125                           (void **)&rpc_rsp, -1, (5*HZ));
 126         if (rc < 0) {
 127                 pr_err("adsp: error receiving RPC reply: %d (%d)\n",
 128                        rc, -ERESTARTSYS);
 129                 return rc;
 130         }
 131
 132         if (be32_to_cpu(rpc_rsp->reply_stat) != RPCMSG_REPLYSTAT_ACCEPTED) {
 133                 pr_err("adsp: RPC call was denied!\n");
 134                 kfree(rpc_rsp);
 135                 return -EPERM;
 136         }
 137
 138         if (be32_to_cpu(rpc_rsp->data.acc_hdr.accept_stat) !=
 139             RPC_ACCEPTSTAT_SUCCESS) {
 140                 pr_err("adsp error: RPC call was not successful (%d)\n",
 141                        be32_to_cpu(rpc_rsp->data.acc_hdr.accept_stat));
 142                 kfree(rpc_rsp);
 143                 return -EINVAL;
 144         }
 145
 146         kfree(rpc_rsp);
 147         return 0;
 148 }
 149
 150 #if CONFIG_MSM_AMSS_VERSION >= 6350
 151 static int get_module_index(uint32_t id)
 152 {
 153         int mod_idx;
 154         for (mod_idx = 0; mod_idx < adsp_info.module_count; mod_idx++)
 155                 if (adsp_info.module[mod_idx].id == id)
 156                         return mod_idx;
 157
 158         return -ENXIO;
 159 }
 160 #endif
 161
 162 static struct msm_adsp_module *find_adsp_module_by_id(
 163         struct adsp_info *info, uint32_t id)
 164 {
 165         if (id > info->max_module_id) {
 166                 return NULL;
 167         } else {
 168 #if CONFIG_MSM_AMSS_VERSION >= 6350
 169                 id = get_module_index(id);
 170                 if (id < 0)
 171                         return NULL;
 172 #endif
 173                 return info->id_to_module[id];
 174         }
 175 }
 176
 177 static struct msm_adsp_module *find_adsp_module_by_name(
 178         struct adsp_info *info, const char *name)
 179 {
 180         unsigned n;
 181         for (n = 0; n < info->module_count; n++)
 182                 if (!strcmp(name, adsp_modules[n].name))
 183                         return adsp_modules + n;
 184         return NULL;
 185 }
 186
 187 static int adsp_rpc_init(struct msm_adsp_module *adsp_module)
 188 {
 189         /* remove the original connect once compatible support is complete */
 190         adsp_module->rpc_client = msm_rpc_connect(
 191                         RPC_ADSP_RTOS_ATOM_PROG,
 192                         RPC_ADSP_RTOS_ATOM_VERS,
 193                         MSM_RPC_UNINTERRUPTIBLE);
 194
 195         if (IS_ERR(adsp_module->rpc_client)) {
 196                 int rc = PTR_ERR(adsp_module->rpc_client);
 197                 adsp_module->rpc_client = 0;
 198                 pr_err("adsp: could not open rpc client: %d\n", rc);
 199                 return rc;
 200         }
 201
 202         return 0;
 203 }
 204
 205 #if CONFIG_MSM_AMSS_VERSION >= 6350
 206 /*
 207  * Send RPC_ADSP_RTOS_CMD_GET_INIT_INFO cmd to ARM9 and get
 208  * queue offsets and module entries (init info) as part of the event.
 209  */
 210 static void  msm_get_init_info(void)
 211 {
 212         int rc;
 213         struct rpc_adsp_rtos_app_to_modem_args_t rpc_req;
 214
 215         adsp_info.init_info_rpc_client = msm_rpc_connect(
 216                         RPC_ADSP_RTOS_ATOM_PROG,
 217                         RPC_ADSP_RTOS_ATOM_VERS,
 218                         MSM_RPC_UNINTERRUPTIBLE);
 219         if (IS_ERR(adsp_info.init_info_rpc_client)) {
 220                 rc = PTR_ERR(adsp_info.init_info_rpc_client);
 221                 adsp_info.init_info_rpc_client = 0;
 222                 pr_err("adsp: could not open rpc client: %d\n", rc);
 223                 return;
 224         }
 225
 226         msm_rpc_setup_req(&rpc_req.hdr,
 227                         RPC_ADSP_RTOS_ATOM_PROG,
 228                         msm_rpc_get_vers(adsp_info.init_info_rpc_client),
 229                         RPC_ADSP_RTOS_APP_TO_MODEM_PROC);
 230
 231         rpc_req.gotit = cpu_to_be32(1);
 232         rpc_req.cmd = cpu_to_be32(RPC_ADSP_RTOS_CMD_GET_INIT_INFO);
 233         rpc_req.proc_id = cpu_to_be32(RPC_ADSP_RTOS_PROC_APPS);
 234         rpc_req.module = 0;
 235
 236         rc = msm_rpc_write(adsp_info.init_info_rpc_client,
 237                                 &rpc_req, sizeof(rpc_req));
 238         if (rc < 0)
 239                 pr_err("adsp: could not send RPC request: %d\n", rc);
 240 }
 241 #endif
 242
 243 int msm_adsp_get(const char *name, struct msm_adsp_module **out,
 244                  struct msm_adsp_ops *ops, void *driver_data)
 245 {
 246         struct msm_adsp_module *module;
 247         int rc = 0;
 248
 249 #if CONFIG_MSM_AMSS_VERSION >= 6350
 250         static uint32_t init_info_cmd_sent;
 251         if (!init_info_cmd_sent) {
 252                 msm_get_init_info();
 253                 init_waitqueue_head(&adsp_info.init_info_wait);
 254                 rc = wait_event_timeout(adsp_info.init_info_wait,
 255                         adsp_info.init_info_state == ADSP_STATE_INIT_INFO,
 256                         5 * HZ);
 257                 if (!rc) {
 258                         pr_info("adsp: INIT_INFO failed\n");
 259                         return -ETIMEDOUT;
 260                 }
 261                 init_info_cmd_sent++;
 262         }
 263 #endif
 264
 265         module = find_adsp_module_by_name(&adsp_info, name);
 266         if (!module)
 267                 return -ENODEV;
 268
 269         mutex_lock(&module->lock);
 270         pr_info("adsp: opening module %s\n", module->name);
 271         if (module->open_count++ == 0 && module->clk)
 272                 clk_enable(module->clk);
 273
 274         mutex_lock(&adsp_open_lock);
 275         if (adsp_open_count++ == 0) {
 276                 enable_irq(INT_ADSP);
 277                 prevent_suspend();
 278         }
 279         mutex_unlock(&adsp_open_lock);
 280
 281         if (module->ops) {
 282                 rc = -EBUSY;
 283                 goto done;
 284         }
 285
 286         rc = adsp_rpc_init(module);
 287         if (rc)
 288                 goto done;
 289
 290         module->ops = ops;
 291         module->driver_data = driver_data;
 292         *out = module;
 293         rc = rpc_adsp_rtos_app_to_modem(RPC_ADSP_RTOS_CMD_REGISTER_APP,
 294                                         module->id, module);
 295         if (rc) {
 296                 module->ops = NULL;
 297                 module->driver_data = NULL;
 298                 *out = NULL;
 299                 pr_err("adsp: REGISTER_APP failed\n");
 300                 goto done;
 301         }
 302
 303         pr_info("adsp: module %s has been registered\n", module->name);
 304
 305 done:
 306         mutex_lock(&adsp_open_lock);
 307         if (rc && --adsp_open_count == 0) {
 308                 disable_irq(INT_ADSP);
 309                 allow_suspend();
 310         }
 311         if (rc && --module->open_count == 0 && module->clk)
 312                 clk_disable(module->clk);
 313         mutex_unlock(&adsp_open_lock);
 314         mutex_unlock(&module->lock);
 315         return rc;
 316 }
 317 EXPORT_SYMBOL(msm_adsp_get);
 318
 319 static int msm_adsp_disable_locked(struct msm_adsp_module *module);
 320
 321 void msm_adsp_put(struct msm_adsp_module *module)
 322 {
 323         unsigned long flags;
 324
 325         mutex_lock(&module->lock);
 326         if (--module->open_count == 0 && module->clk)
 327                 clk_disable(module->clk);
 328         if (module->ops) {
 329                 pr_info("adsp: closing module %s\n", module->name);
 330
 331                 /* lock to ensure a dsp event cannot be delivered
 332                  * during or after removal of the ops and driver_data
 333                  */
 334                 spin_lock_irqsave(&adsp_cmd_lock, flags);
 335                 module->ops = NULL;
 336                 module->driver_data = NULL;
 337                 spin_unlock_irqrestore(&adsp_cmd_lock, flags);
 338
 339                 if (module->state != ADSP_STATE_DISABLED) {
 340                         pr_info("adsp: disabling module %s\n", module->name);
 341                         msm_adsp_disable_locked(module);
 342                 }
 343
 344                 msm_rpc_close(module->rpc_client);
 345                 module->rpc_client = 0;
 346                 if (--adsp_open_count == 0) {
 347                         disable_irq(INT_ADSP);
 348                         allow_suspend();
 349                         pr_info("adsp: disable interrupt\n");
 350                 }
 351         } else {
 352                 pr_info("adsp: module %s is already closed\n", module->name);
 353         }
 354         mutex_unlock(&module->lock);
 355 }
 356 EXPORT_SYMBOL(msm_adsp_put);
 357
 358 /* this should be common code with rpc_servers.c */
 359 static int rpc_send_accepted_void_reply(struct msm_rpc_endpoint *client,
 360                                         uint32_t xid, uint32_t accept_status)
 361 {
 362         int rc = 0;
 363         uint8_t reply_buf[sizeof(struct rpc_reply_hdr)];
 364         struct rpc_reply_hdr *reply = (struct rpc_reply_hdr *)reply_buf;
 365
 366         reply->xid = cpu_to_be32(xid);
 367         reply->type = cpu_to_be32(1); /* reply */
 368         reply->reply_stat = cpu_to_be32(RPCMSG_REPLYSTAT_ACCEPTED);
 369
 370         reply->data.acc_hdr.accept_stat = cpu_to_be32(accept_status);
 371         reply->data.acc_hdr.verf_flavor = 0;
 372         reply->data.acc_hdr.verf_length = 0;
 373
 374         rc = msm_rpc_write(rpc_cb_server_client, reply_buf, sizeof(reply_buf));
 375         if (rc < 0)
 376                 pr_err("adsp: could not write RPC response: %d\n", rc);
 377         return rc;
 378 }
 379
 380 int __msm_adsp_write(struct msm_adsp_module *module, unsigned dsp_queue_addr,
 381                    void *cmd_buf, size_t cmd_size)
 382 {
 383         uint32_t ctrl_word;
 384         uint32_t dsp_q_addr;
 385         uint32_t dsp_addr;
 386         uint32_t cmd_id = 0;
 387         int cnt = 0;
 388         int ret_status = 0;
 389         unsigned long flags;
 390         struct adsp_info *info = module->info;
 391
 392         spin_lock_irqsave(&adsp_cmd_lock, flags);
 393
 394         if (module->state != ADSP_STATE_ENABLED) {
 395                 spin_unlock_irqrestore(&adsp_cmd_lock, flags);
 396                 pr_err("adsp: module %s not enabled before write\n",
 397                        module->name);
 398                 return -ENODEV;
 399         }
 400         if (adsp_validate_module(module->id)) {
 401                 spin_unlock_irqrestore(&adsp_cmd_lock, flags);
 402                 pr_info("adsp: module id validation failed %s  %d\n",
 403                         module->name, module->id);
 404                 return -ENXIO;
 405         }
 406         dsp_q_addr = adsp_get_queue_offset(info, dsp_queue_addr);
 407         dsp_q_addr &= ADSP_RTOS_WRITE_CTRL_WORD_DSP_ADDR_M;
 408
 409         /* Poll until the ADSP is ready to accept a command.
 410          * Wait for 100us, return error if it's not responding.
 411          * If this returns an error, we need to disable ALL modules and
 412          * then retry.
 413          */
 414         while (((ctrl_word = readl(info->write_ctrl)) &
 415                 ADSP_RTOS_WRITE_CTRL_WORD_READY_M) !=
 416                 ADSP_RTOS_WRITE_CTRL_WORD_READY_V) {
 417                 if (cnt > 100) {
 418                         pr_err("adsp: timeout waiting for DSP write ready\n");
 419                         ret_status = -EIO;
 420                         goto fail;
 421                 }
 422                 pr_warning("adsp: waiting for DSP write ready\n");
 423                 udelay(1);
 424                 cnt++;
 425         }
 426
 427         /* Set the mutex bits */
 428         ctrl_word &= ~(ADSP_RTOS_WRITE_CTRL_WORD_MUTEX_M);
 429         ctrl_word |=  ADSP_RTOS_WRITE_CTRL_WORD_MUTEX_NAVAIL_V;
 430
 431         /* Clear the command bits */
 432         ctrl_word &= ~(ADSP_RTOS_WRITE_CTRL_WORD_CMD_M);
 433
 434         /* Set the queue address bits */
 435         ctrl_word &= ~(ADSP_RTOS_WRITE_CTRL_WORD_DSP_ADDR_M);
 436         ctrl_word |= dsp_q_addr;
 437
 438         writel(ctrl_word, info->write_ctrl);
 439
 440         /* Generate an interrupt to the DSP.  This notifies the DSP that
 441          * we are about to send a command on this particular queue.  The
 442          * DSP will in response change its state.
 443          */
 444         writel(1, info->send_irq);
 445
 446         /* Poll until the adsp responds to the interrupt; this does not
 447          * generate an interrupt from the adsp.  This should happen within
 448          * 5ms.
 449          */
 450         cnt = 0;
 451         while ((readl(info->write_ctrl) &
 452                 ADSP_RTOS_WRITE_CTRL_WORD_MUTEX_M) ==
 453                 ADSP_RTOS_WRITE_CTRL_WORD_MUTEX_NAVAIL_V) {
 454                 if (cnt > 5000) {
 455                         pr_err("adsp: timeout waiting for adsp ack\n");
 456                         ret_status = -EIO;
 457                         goto fail;
 458                 }
 459                 udelay(1);
 460                 cnt++;
 461         }
 462
 463         /* Read the ctrl word */
 464         ctrl_word = readl(info->write_ctrl);
 465
 466         if ((ctrl_word & ADSP_RTOS_WRITE_CTRL_WORD_STATUS_M) !=
 467                          ADSP_RTOS_WRITE_CTRL_WORD_NO_ERR_V) {
 468                 ret_status = -EAGAIN;
 469                 goto fail;
 470         }
 471
 472         /* Ctrl word status bits were 00, no error in the ctrl word */
 473
 474         /* Get the DSP buffer address */
 475         dsp_addr = (ctrl_word & ADSP_RTOS_WRITE_CTRL_WORD_DSP_ADDR_M) +
 476                    (uint32_t)MSM_AD5_BASE;
 477
 478         if (dsp_addr < (uint32_t)(MSM_AD5_BASE + QDSP_RAMC_OFFSET)) {
 479                 uint16_t *buf_ptr = (uint16_t *) cmd_buf;
 480                 uint16_t *dsp_addr16 = (uint16_t *)dsp_addr;
 481                 cmd_size /= sizeof(uint16_t);
 482
 483                 /* Save the command ID */
 484                 cmd_id = (uint32_t) buf_ptr[0];
 485
 486                 /* Copy the command to DSP memory */
 487                 cmd_size++;
 488                 while (--cmd_size)
 489                         *dsp_addr16++ = *buf_ptr++;
 490         } else {
 491                 uint32_t *buf_ptr = (uint32_t *) cmd_buf;
 492                 uint32_t *dsp_addr32 = (uint32_t *)dsp_addr;
 493                 cmd_size /= sizeof(uint32_t);
 494
 495                 /* Save the command ID */
 496                 cmd_id = buf_ptr[0];
 497
 498                 cmd_size++;
 499                 while (--cmd_size)
 500                         *dsp_addr32++ = *buf_ptr++;
 501         }
 502
 503         /* Set the mutex bits */
 504         ctrl_word &= ~(ADSP_RTOS_WRITE_CTRL_WORD_MUTEX_M);
 505         ctrl_word |=  ADSP_RTOS_WRITE_CTRL_WORD_MUTEX_NAVAIL_V;
 506
 507         /* Set the command bits to write done */
 508         ctrl_word &= ~(ADSP_RTOS_WRITE_CTRL_WORD_CMD_M);
 509         ctrl_word |= ADSP_RTOS_WRITE_CTRL_WORD_CMD_WRITE_DONE_V;
 510
 511         /* Set the queue address bits */
 512         ctrl_word &= ~(ADSP_RTOS_WRITE_CTRL_WORD_DSP_ADDR_M);
 513         ctrl_word |= dsp_q_addr;
 514
 515         writel(ctrl_word, info->write_ctrl);
 516
 517         /* Generate an interrupt to the DSP.  It does not respond with
 518          * an interrupt, and we do not need to wait for it to
 519          * acknowledge, because it will hold the mutex lock until it's
 520          * ready to receive more commands again.
 521          */
 522         writel(1, info->send_irq);
 523
 524         module->num_commands++;
 525
 526 fail:
 527         spin_unlock_irqrestore(&adsp_cmd_lock, flags);
 528         return ret_status;
 529 }
 530 EXPORT_SYMBOL(msm_adsp_write);
 531
 532 int msm_adsp_write(struct msm_adsp_module *module, unsigned dsp_queue_addr,
 533                    void *cmd_buf, size_t cmd_size)
 534 {
 535         int rc, retries = 0;
 536         do {
 537                 rc = __msm_adsp_write(module, dsp_queue_addr, cmd_buf, cmd_size);
 538                 if (rc == -EAGAIN)
 539                         udelay(10);
 540         } while(rc == -EAGAIN && retries++ < 100);
 541         if (retries > 50)
 542                 pr_warning("adsp: %s command took %d attempts: rc %d\n",
 543                                 module->name, retries, rc);
 544         return rc;
 545 }
 546
 547 #ifdef CONFIG_MSM_ADSP_REPORT_EVENTS
 548 static void *modem_event_addr;
 549 #if CONFIG_MSM_AMSS_VERSION >= 6350
 550 static void read_modem_event(void *buf, size_t len)
 551 {
 552         uint32_t *dptr = buf;
 553         struct rpc_adsp_rtos_modem_to_app_args_t *sptr;
 554         struct adsp_rtos_mp_mtoa_type *pkt_ptr;
 555
 556         sptr = modem_event_addr;
 557         pkt_ptr = &sptr->mtoa_pkt.adsp_rtos_mp_mtoa_data.mp_mtoa_packet;
 558
 559         dptr[0] = be32_to_cpu(sptr->mtoa_pkt.mp_mtoa_header.event);
 560         dptr[1] = be32_to_cpu(pkt_ptr->module);
 561         dptr[2] = be32_to_cpu(pkt_ptr->image);
 562 }
 563 #else
 564 static void read_modem_event(void *buf, size_t len)
 565 {
 566         uint32_t *dptr = buf;
 567         struct rpc_adsp_rtos_modem_to_app_args_t *sptr =
 568                 modem_event_addr;
 569         dptr[0] = be32_to_cpu(sptr->event);
 570         dptr[1] = be32_to_cpu(sptr->module);
 571         dptr[2] = be32_to_cpu(sptr->image);
 572 }
 573 #endif /* CONFIG_MSM_AMSS_VERSION >= 6350 */
 574 #endif /* CONFIG_MSM_ADSP_REPORT_EVENTS */
 575
 576 static void handle_adsp_rtos_mtoa_app(struct rpc_request_hdr *req)
 577 {
 578         struct rpc_adsp_rtos_modem_to_app_args_t *args =
 579                 (struct rpc_adsp_rtos_modem_to_app_args_t *)req;
 580         uint32_t event;
 581         uint32_t proc_id;
 582         uint32_t module_id;
 583         uint32_t image;
 584         struct msm_adsp_module *module;
 585 #if CONFIG_MSM_AMSS_VERSION >= 6350
 586         struct adsp_rtos_mp_mtoa_type *pkt_ptr =
 587                 &args->mtoa_pkt.adsp_rtos_mp_mtoa_data.mp_mtoa_packet;
 588
 589         event = be32_to_cpu(args->mtoa_pkt.mp_mtoa_header.event);
 590         proc_id = be32_to_cpu(args->mtoa_pkt.mp_mtoa_header.proc_id);
 591         module_id = be32_to_cpu(pkt_ptr->module);
 592         image = be32_to_cpu(pkt_ptr->image);
 593
 594         if (be32_to_cpu(args->mtoa_pkt.desc_field) == RPC_ADSP_RTOS_INIT_INFO) {
 595                 struct queue_to_offset_type *qptr;
 596                 struct queue_to_offset_type *qtbl;
 597                 uint32_t *mptr;
 598                 uint32_t *mtbl;
 599                 uint32_t q_idx;
 600                 uint32_t num_entries;
 601                 uint32_t entries_per_image;
 602                 struct adsp_rtos_mp_mtoa_init_info_type *iptr;
 603                 struct adsp_rtos_mp_mtoa_init_info_type *sptr;
 604                 int32_t i_no, e_idx;
 605
 606                 pr_info("adsp:INIT_INFO Event\n");
 607                 sptr = &args->mtoa_pkt.adsp_rtos_mp_mtoa_data.
 608                                 mp_mtoa_init_packet;
 609
 610                 iptr = adsp_info.init_info_ptr;
 611                 iptr->image_count = be32_to_cpu(sptr->image_count);
 612                 iptr->num_queue_offsets = be32_to_cpu(sptr->num_queue_offsets);
 613                 num_entries = iptr->num_queue_offsets;
 614                 qptr = &sptr->queue_offsets_tbl[0][0];
 615                 for (i_no = 0; i_no < iptr->image_count; i_no++) {
 616                         qtbl = &iptr->queue_offsets_tbl[i_no][0];
 617                         for (e_idx = 0; e_idx < num_entries; e_idx++) {
 618                                 qtbl[e_idx].offset = be32_to_cpu(qptr->offset);
 619                                 qtbl[e_idx].queue = be32_to_cpu(qptr->queue);
 620                                 q_idx = be32_to_cpu(qptr->queue);
 621                                 iptr->queue_offsets[i_no][q_idx] =
 622                                                 qtbl[e_idx].offset;
 623                                 qptr++;
 624                         }
 625                 }
 626
 627                 num_entries = be32_to_cpu(sptr->num_task_module_entries);
 628                 iptr->num_task_module_entries = num_entries;
 629                 entries_per_image = num_entries / iptr->image_count;
 630                 mptr = &sptr->task_to_module_tbl[0][0];
 631                 for (i_no = 0; i_no < iptr->image_count; i_no++) {
 632                         mtbl = &iptr->task_to_module_tbl[i_no][0];
 633                         for (e_idx = 0; e_idx < entries_per_image; e_idx++) {
 634                                 mtbl[e_idx] = be32_to_cpu(*mptr);
 635                                 mptr++;
 636                         }
 637                 }
 638
 639                 iptr->module_table_size = be32_to_cpu(sptr->module_table_size);
 640                 mptr = &sptr->module_entries[0];
 641                 for (i_no = 0; i_no < iptr->module_table_size; i_no++)
 642                         iptr->module_entries[i_no] = be32_to_cpu(mptr[i_no]);
 643                 adsp_info.init_info_state = ADSP_STATE_INIT_INFO;
 644                 rpc_send_accepted_void_reply(rpc_cb_server_client, req->xid,
 645                                                 RPC_ACCEPTSTAT_SUCCESS);
 646                 wake_up(&adsp_info.init_info_wait);
 647
 648                 return;
 649         }
 650 #else
 651         event = be32_to_cpu(args->event);
 652         proc_id = be32_to_cpu(args->proc_id);
 653         module_id = be32_to_cpu(args->module);
 654         image = be32_to_cpu(args->image);
 655 #endif
 656
 657         pr_info("adsp: rpc event=%d, proc_id=%d, module=%d, image=%d\n",
 658                 event, proc_id, module_id, image);
 659
 660         module = find_adsp_module_by_id(&adsp_info, module_id);
 661         if (!module) {
 662                 pr_err("adsp: module %d is not supported!\n", module_id);
 663                 rpc_send_accepted_void_reply(rpc_cb_server_client, req->xid,
 664                                 RPC_ACCEPTSTAT_GARBAGE_ARGS);
 665                 return;
 666         }
 667
 668         mutex_lock(&module->lock);
 669         switch (event) {
 670         case RPC_ADSP_RTOS_MOD_READY:
 671                 pr_info("adsp: module %s: READY\n", module->name);
 672                 module->state = ADSP_STATE_ENABLED;
 673                 wake_up(&module->state_wait);
 674                 adsp_set_image(module->info, image);
 675                 break;
 676         case RPC_ADSP_RTOS_MOD_DISABLE:
 677                 pr_info("adsp: module %s: DISABLED\n", module->name);
 678                 module->state = ADSP_STATE_DISABLED;
 679                 wake_up(&module->state_wait);
 680                 break;
 681         case RPC_ADSP_RTOS_SERVICE_RESET:
 682                 pr_info("adsp: module %s: SERVICE_RESET\n", module->name);
 683                 module->state = ADSP_STATE_DISABLED;
 684                 wake_up(&module->state_wait);
 685                 break;
 686         case RPC_ADSP_RTOS_CMD_SUCCESS:
 687                 pr_info("adsp: module %s: CMD_SUCCESS\n", module->name);
 688                 break;
 689         case RPC_ADSP_RTOS_CMD_FAIL:
 690                 pr_info("adsp: module %s: CMD_FAIL\n", module->name);
 691                 break;
 692 #if CONFIG_MSM_AMSS_VERSION >= 6350
 693         case RPC_ADSP_RTOS_DISABLE_FAIL:
 694                 pr_info("adsp: module %s: DISABLE_FAIL\n", module->name);
 695                 break;
 696 #endif
 697         default:
 698                 pr_info("adsp: unknown event %d\n", event);
 699                 rpc_send_accepted_void_reply(rpc_cb_server_client, req->xid,
 700                                              RPC_ACCEPTSTAT_GARBAGE_ARGS);
 701                 mutex_unlock(&module->lock);
 702                 return;
 703         }
 704         rpc_send_accepted_void_reply(rpc_cb_server_client, req->xid,
 705                                      RPC_ACCEPTSTAT_SUCCESS);
 706         mutex_unlock(&module->lock);
 707 #ifdef CONFIG_MSM_ADSP_REPORT_EVENTS
 708         modem_event_addr = (uint32_t *)req;
 709         module->ops->event(module->driver_data, EVENT_MSG_ID,
 710                                 EVENT_LEN, read_modem_event);
 711 #endif
 712 }
 713
 714 static int handle_adsp_rtos_mtoa(struct rpc_request_hdr *req)
 715 {
 716         switch (req->procedure) {
 717         case RPC_ADSP_RTOS_MTOA_NULL_PROC:
 718                 rpc_send_accepted_void_reply(rpc_cb_server_client,
 719                                              req->xid,
 720                                              RPC_ACCEPTSTAT_SUCCESS);
 721                 break;
 722         case RPC_ADSP_RTOS_MODEM_TO_APP_PROC:
 723                 handle_adsp_rtos_mtoa_app(req);
 724                 break;
 725         default:
 726                 pr_err("adsp: unknowned proc %d\n", req->procedure);
 727                 rpc_send_accepted_void_reply(
 728                         rpc_cb_server_client, req->xid,
 729                         RPC_ACCEPTSTAT_PROC_UNAVAIL);
 730                 break;
 731         }
 732         return 0;
 733 }
 734
 735 /* this should be common code with rpc_servers.c */
 736 static int adsp_rpc_thread(void *data)
 737 {
 738         void *buffer;
 739         struct rpc_request_hdr *req;
 740         int rc;
 741
 742         do {
 743                 rc = msm_rpc_read(rpc_cb_server_client, &buffer, -1, -1);
 744                 if (rc < 0) {
 745                         pr_err("adsp: could not read rpc: %d\n", rc);
 746                         break;
 747                 }
 748                 req = (struct rpc_request_hdr *)buffer;
 749
 750                 req->type = be32_to_cpu(req->type);
 751                 req->xid = be32_to_cpu(req->xid);
 752                 req->rpc_vers = be32_to_cpu(req->rpc_vers);
 753                 req->prog = be32_to_cpu(req->prog);
 754                 req->vers = be32_to_cpu(req->vers);
 755                 req->procedure = be32_to_cpu(req->procedure);
 756
 757                 if (req->type != 0)
 758                         goto bad_rpc;
 759                 if (req->rpc_vers != 2)
 760                         goto bad_rpc;
 761                 if (req->prog != RPC_ADSP_RTOS_MTOA_PROG)
 762                         goto bad_rpc;
 763                 if (req->vers != RPC_ADSP_RTOS_MTOA_VERS)
 764                         goto bad_rpc;
 765
 766                 handle_adsp_rtos_mtoa(req);
 767                 kfree(buffer);
 768                 continue;
 769
 770 bad_rpc:
 771                 pr_err("adsp: bogus rpc from modem\n");
 772                 kfree(buffer);
 773         } while (1);
 774
 775         do_exit(0);
 776 }
 777
 778 static size_t read_event_size;
 779 static void *read_event_addr;
 780
 781 static void read_event_16(void *buf, size_t len)
 782 {
 783         uint16_t *dst = buf;
 784         uint16_t *src = read_event_addr;
 785         len /= 2;
 786         if (len > read_event_size)
 787                 len = read_event_size;
 788         while (len--)
 789                 *dst++ = *src++;
 790 }
 791
 792 static void read_event_32(void *buf, size_t len)
 793 {
 794         uint32_t *dst = buf;
 795         uint32_t *src = read_event_addr;
 796         len /= 2;
 797         if (len > read_event_size)
 798                 len = read_event_size;
 799         while (len--)
 800                 *dst++ = *src++;
 801 }
 802
 803 static int adsp_rtos_read_ctrl_word_cmd_tast_to_h_v(
 804         struct adsp_info *info, void *dsp_addr)
 805 {
 806         struct msm_adsp_module *module;
 807         unsigned rtos_task_id;
 808         unsigned msg_id;
 809         unsigned msg_length;
 810         void (*func)(void *, size_t);
 811
 812         if (dsp_addr >= (void *)(MSM_AD5_BASE + QDSP_RAMC_OFFSET)) {
 813                 uint32_t *dsp_addr32 = dsp_addr;
 814                 uint32_t tmp = *dsp_addr32++;
 815                 rtos_task_id = (tmp & ADSP_RTOS_READ_CTRL_WORD_TASK_ID_M) >> 8;
 816                 msg_id = (tmp & ADSP_RTOS_READ_CTRL_WORD_MSG_ID_M);
 817                 read_event_size = tmp >> 16;
 818                 read_event_addr = dsp_addr32;
 819                 msg_length = read_event_size * sizeof(uint32_t);
 820                 func = read_event_32;
 821         } else {
 822                 uint16_t *dsp_addr16 = dsp_addr;
 823                 uint16_t tmp = *dsp_addr16++;
 824                 rtos_task_id = (tmp & ADSP_RTOS_READ_CTRL_WORD_TASK_ID_M) >> 8;
 825                 msg_id = tmp & ADSP_RTOS_READ_CTRL_WORD_MSG_ID_M;
 826                 read_event_size = *dsp_addr16++;
 827                 read_event_addr = dsp_addr16;
 828                 msg_length = read_event_size * sizeof(uint16_t);
 829                 func = read_event_16;
 830         }
 831
 832         if (rtos_task_id > info->max_task_id) {
 833                 pr_err("adsp: bogus task id %d\n", rtos_task_id);
 834                 return 0;
 835         }
 836         module = find_adsp_module_by_id(info,
 837                                         adsp_get_module(info, rtos_task_id));
 838
 839         if (!module) {
 840                 pr_err("adsp: no module for task id %d\n", rtos_task_id);
 841                 return 0;
 842         }
 843
 844         module->num_events++;
 845
 846         if (!module->ops) {
 847                 pr_err("adsp: module %s is not open\n", module->name);
 848                 return 0;
 849         }
 850
 851         module->ops->event(module->driver_data, msg_id, msg_length, func);
 852         return 0;
 853 }
 854
 855 static int adsp_get_event(struct adsp_info *info)
 856 {
 857         uint32_t ctrl_word;
 858         uint32_t ready;
 859         void *dsp_addr;
 860         uint32_t cmd_type;
 861         int cnt;
 862         unsigned long flags;
 863         int rc = 0;
 864
 865         spin_lock_irqsave(&adsp_cmd_lock, flags);
 866
 867         /* Whenever the DSP has a message, it updates this control word
 868          * and generates an interrupt.  When we receive the interrupt, we
 869          * read this register to find out what ADSP task the command is
 870          * comming from.
 871          *
 872          * The ADSP should *always* be ready on the first call, but the
 873          * irq handler calls us in a loop (to handle back-to-back command
 874          * processing), so we give the DSP some time to return to the
 875          * ready state.  The DSP will not issue another IRQ for events
 876          * pending between the first IRQ and the event queue being drained,
 877          * unfortunately.
 878          */
 879
 880         for (cnt = 0; cnt < 10; cnt++) {
 881                 ctrl_word = readl(info->read_ctrl);
 882
 883                 if ((ctrl_word & ADSP_RTOS_READ_CTRL_WORD_FLAG_M) ==
 884                     ADSP_RTOS_READ_CTRL_WORD_FLAG_UP_CONT_V)
 885                         goto ready;
 886
 887                 udelay(10);
 888         }
 889         pr_warning("adsp: not ready after 100uS\n");
 890         rc = -EBUSY;
 891         goto done;
 892
 893 ready:
 894         /* Here we check to see if there are pending messages. If there are
 895          * none, we siply return -EAGAIN to indicate that there are no more
 896          * messages pending.
 897          */
 898         ready = ctrl_word & ADSP_RTOS_READ_CTRL_WORD_READY_M;
 899         if ((ready != ADSP_RTOS_READ_CTRL_WORD_READY_V) &&
 900             (ready != ADSP_RTOS_READ_CTRL_WORD_CONT_V)) {
 901                 rc = -EAGAIN;
 902                 goto done;
 903         }
 904
 905         /* DSP says that there are messages waiting for the host to read */
 906
 907         /* Get the Command Type */
 908         cmd_type = ctrl_word & ADSP_RTOS_READ_CTRL_WORD_CMD_TYPE_M;
 909
 910         /* Get the DSP buffer address */
 911         dsp_addr = (void *)((ctrl_word &
 912                              ADSP_RTOS_READ_CTRL_WORD_DSP_ADDR_M) +
 913                             (uint32_t)MSM_AD5_BASE);
 914
 915         /* We can only handle Task-to-Host messages */
 916         if (cmd_type != ADSP_RTOS_READ_CTRL_WORD_CMD_TASK_TO_H_V) {
 917                 pr_err("adsp: unknown dsp cmd_type %d\n", cmd_type);
 918                 rc = -EIO;
 919                 goto done;
 920         }
 921
 922         adsp_rtos_read_ctrl_word_cmd_tast_to_h_v(info, dsp_addr);
 923
 924         ctrl_word = readl(info->read_ctrl);
 925         ctrl_word &= ~ADSP_RTOS_READ_CTRL_WORD_READY_M;
 926
 927         /* Write ctrl word to the DSP */
 928         writel(ctrl_word, info->read_ctrl);
 929
 930         /* Generate an interrupt to the DSP */
 931         writel(1, info->send_irq);
 932
 933 done:
 934         spin_unlock_irqrestore(&adsp_cmd_lock, flags);
 935         return rc;
 936 }
 937
 938 static irqreturn_t adsp_irq_handler(int irq, void *data)
 939 {
 940         struct adsp_info *info = &adsp_info;
 941         int cnt = 0;
 942         for (cnt = 0; cnt < 10; cnt++)
 943                 if (adsp_get_event(info) < 0)
 944                         break;
 945         if (cnt > info->event_backlog_max)
 946                 info->event_backlog_max = cnt;
 947         info->events_received += cnt;
 948         if (cnt == 10)
 949                 pr_err("adsp: too many (%d) events for single irq!\n", cnt);
 950         return IRQ_HANDLED;
 951 }
 952
 953 int adsp_set_clkrate(struct msm_adsp_module *module, unsigned long clk_rate)
 954 {
 955         if (module->clk && clk_rate)
 956                 return clk_set_rate(module->clk, clk_rate);
 957
 958         return -EINVAL;
 959 }
 960
 961 int msm_adsp_enable(struct msm_adsp_module *module)
 962 {
 963         int rc = 0;
 964
 965         pr_info("msm_adsp_enable() '%s'state[%d] id[%d]\n",
 966                 module->name, module->state, module->id);
 967
 968         mutex_lock(&module->lock);
 969         switch (module->state) {
 970         case ADSP_STATE_DISABLED:
 971                 rc = rpc_adsp_rtos_app_to_modem(RPC_ADSP_RTOS_CMD_ENABLE,
 972                                                 module->id, module);
 973                 if (rc)
 974                         break;
 975                 module->state = ADSP_STATE_ENABLING;
 976                 mutex_unlock(&module->lock);
 977                 rc = wait_event_timeout(module->state_wait,
 978                                         module->state != ADSP_STATE_ENABLING,
 979                                         1 * HZ);
 980                 mutex_lock(&module->lock);
 981                 if (module->state == ADSP_STATE_ENABLED) {
 982                         rc = 0;
 983                 } else {
 984                         pr_err("adsp: module '%s' enable timed out\n",
 985                                module->name);
 986                         rc = -ETIMEDOUT;
 987                 }
 988                 break;
 989         case ADSP_STATE_ENABLING:
 990                 pr_warning("adsp: module '%s' enable in progress\n",
 991                            module->name);
 992                 break;
 993         case ADSP_STATE_ENABLED:
 994                 pr_warning("adsp: module '%s' already enabled\n",
 995                            module->name);
 996                 break;
 997         case ADSP_STATE_DISABLING:
 998                 pr_err("adsp: module '%s' disable in progress\n",
 999                        module->name);
1000                 rc = -EBUSY;
1001                 break;
1002         }
1003         mutex_unlock(&module->lock);
1004         return rc;
1005 }
1006 EXPORT_SYMBOL(msm_adsp_enable);
1007
1008 static int msm_adsp_disable_locked(struct msm_adsp_module *module)
1009 {
1010         int rc = 0;
1011
1012         switch (module->state) {
1013         case ADSP_STATE_DISABLED:
1014                 pr_warning("adsp: module '%s' already disabled\n",
1015                            module->name);
1016                 break;
1017         case ADSP_STATE_ENABLING:
1018         case ADSP_STATE_ENABLED:
1019                 rc = rpc_adsp_rtos_app_to_modem(RPC_ADSP_RTOS_CMD_DISABLE,
1020                                                 module->id, module);
1021                 module->state = ADSP_STATE_DISABLED;
1022         }
1023         return rc;
1024 }
1025
1026 int msm_adsp_disable(struct msm_adsp_module *module)
1027 {
1028         int rc;
1029         pr_info("msm_adsp_disable() '%s'\n", module->name);
1030         mutex_lock(&module->lock);
1031         rc = msm_adsp_disable_locked(module);
1032         mutex_unlock(&module->lock);
1033         return rc;
1034 }
1035 EXPORT_SYMBOL(msm_adsp_disable);
1036
1037 static int msm_adsp_probe(struct platform_device *pdev)
1038 {
1039         unsigned count;
1040         int rc, i;
1041         int max_module_id;
1042
1043         pr_info("adsp: probe\n");
1044
1045 #if CONFIG_MSM_AMSS_VERSION >= 6350
1046         adsp_info.init_info_ptr = kzalloc(
1047                 (sizeof(struct adsp_rtos_mp_mtoa_init_info_type)), GFP_KERNEL);
1048         if (!adsp_info.init_info_ptr)
1049                 return -ENOMEM;
1050 #endif
1051
1052         rc = adsp_init_info(&adsp_info);
1053         if (rc)
1054                 return rc;
1055         adsp_info.send_irq += (uint32_t) MSM_AD5_BASE;
1056         adsp_info.read_ctrl += (uint32_t) MSM_AD5_BASE;
1057         adsp_info.write_ctrl += (uint32_t) MSM_AD5_BASE;
1058         count = adsp_info.module_count;
1059
1060 #if CONFIG_MSM_AMSS_VERSION >= 6350
1061         max_module_id = count;
1062 #else
1063         max_module_id = adsp_info.max_module_id + 1;
1064 #endif
1065
1066         adsp_modules = kzalloc(
1067                 sizeof(struct msm_adsp_module) * count +
1068                 sizeof(void *) * max_module_id, GFP_KERNEL);
1069         if (!adsp_modules)
1070                 return -ENOMEM;
1071
1072         adsp_info.id_to_module = (void *) (adsp_modules + count);
1073
1074         spin_lock_init(&adsp_cmd_lock);
1075
1076         rc = request_irq(INT_ADSP, adsp_irq_handler, IRQF_TRIGGER_RISING,
1077                          "adsp", 0);
1078         if (rc < 0)
1079                 goto fail_request_irq;
1080         disable_irq(INT_ADSP);
1081
1082         rpc_cb_server_client = msm_rpc_open();
1083         if (IS_ERR(rpc_cb_server_client)) {
1084                 rpc_cb_server_client = NULL;
1085                 rc = PTR_ERR(rpc_cb_server_client);
1086                 pr_err("adsp: could not create rpc server (%d)\n", rc);
1087                 goto fail_rpc_open;
1088         }
1089
1090         rc = msm_rpc_register_server(rpc_cb_server_client,
1091                                      RPC_ADSP_RTOS_MTOA_PROG,
1092                                      RPC_ADSP_RTOS_MTOA_VERS);
1093         if (rc) {
1094                 pr_err("adsp: could not register callback server (%d)\n", rc);
1095                 goto fail_rpc_register;
1096         }
1097
1098         /* start the kernel thread to process the callbacks */
1099         kthread_run(adsp_rpc_thread, NULL, "kadspd");
1100
1101         for (i = 0; i < count; i++) {
1102                 struct msm_adsp_module *mod = adsp_modules + i;
1103                 mutex_init(&mod->lock);
1104                 init_waitqueue_head(&mod->state_wait);
1105                 mod->info = &adsp_info;
1106                 mod->name = adsp_info.module[i].name;
1107                 mod->id = adsp_info.module[i].id;
1108                 if (adsp_info.module[i].clk_name)
1109                         mod->clk = clk_get(NULL, adsp_info.module[i].clk_name);
1110                 else
1111                         mod->clk = NULL;
1112                 if (mod->clk && adsp_info.module[i].clk_rate)
1113                         clk_set_rate(mod->clk, adsp_info.module[i].clk_rate);
1114                 mod->verify_cmd = adsp_info.module[i].verify_cmd;
1115                 mod->patch_event = adsp_info.module[i].patch_event;
1116                 INIT_HLIST_HEAD(&mod->pmem_regions);
1117                 mod->pdev.name = adsp_info.module[i].pdev_name;
1118                 mod->pdev.id = -1;
1119 #if CONFIG_MSM_AMSS_VERSION >= 6350
1120                 adsp_info.id_to_module[i] = mod;
1121 #else
1122                 adsp_info.id_to_module[mod->id] = mod;
1123 #endif
1124                 platform_device_register(&mod->pdev);
1125         }
1126
1127         msm_adsp_publish_cdevs(adsp_modules, count);
1128
1129         return 0;
1130
1131 fail_rpc_register:
1132         msm_rpc_close(rpc_cb_server_client);
1133         rpc_cb_server_client = NULL;
1134 fail_rpc_open:
1135         enable_irq(INT_ADSP);
1136         free_irq(INT_ADSP, 0);
1137 fail_request_irq:
1138         kfree(adsp_modules);
1139 #if CONFIG_MSM_AMSS_VERSION >= 6350
1140         kfree(adsp_info.init_info_ptr);
1141 #endif
1142         return rc;
1143 }
1144
1145 static struct platform_driver msm_adsp_driver = {
1146         .probe = msm_adsp_probe,
1147         .driver = {
1148                 .name = MSM_ADSP_DRIVER_NAME,
1149                 .owner = THIS_MODULE,
1150         },
1151 };
1152
1153 static int __init adsp_init(void)
1154 {
1155         return platform_driver_register(&msm_adsp_driver);
1156 }
1157
1158 device_initcall(adsp_init);