drivers/media/video/tegra/avp/avp_svc.c

   1 /*
   2  * Copyright (C) 2010 Google, Inc.
   3  * Author: Dima Zavin <dima@android.com>
   4  *
   5  * This software is licensed under the terms of the GNU General Public
   6  * License version 2, as published by the Free Software Foundation, and
   7  * may be copied, distributed, and modified under those terms.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  */
  15
  16 #include <linux/clk.h>
  17 #include <linux/delay.h>
  18 #include <linux/dma-mapping.h>
  19 #include <linux/err.h>
  20 #include <linux/io.h>
  21 #include <linux/kthread.h>
  22 #include <linux/list.h>
  23 #include <linux/mutex.h>
  24 #include <linux/slab.h>
  25 #include <linux/tegra_rpc.h>
  26 #include <linux/types.h>
  27
  28 #include <mach/clk.h>
  29 #include <mach/nvmap.h>
  30
  31 #include "../../../../video/tegra/nvmap/nvmap.h"
  32
  33 #include "avp_msg.h"
  34 #include "trpc.h"
  35 #include "avp.h"
  36
  37 enum {
  38         AVP_DBG_TRACE_SVC               = 1U << 0,
  39 };
  40
  41 static u32 debug_mask = 0;
  42 module_param_named(debug_mask, debug_mask, uint, S_IWUSR | S_IRUGO);
  43
  44 #define DBG(flag, args...) \
  45         do { if (unlikely(debug_mask & (flag))) pr_info(args); } while (0)
  46
  47 enum {
  48         CLK_REQUEST_VCP         = 0,
  49         CLK_REQUEST_BSEA        = 1,
  50         CLK_REQUEST_VDE         = 2,
  51         NUM_CLK_REQUESTS,
  52 };
  53
  54 struct avp_module {
  55         const char              *name;
  56         u32                     clk_req;
  57 };
  58
  59 static struct avp_module avp_modules[] = {
  60         [AVP_MODULE_ID_VCP] = {
  61                 .name           = "vcp",
  62                 .clk_req        = CLK_REQUEST_VCP,
  63         },
  64         [AVP_MODULE_ID_BSEA]    = {
  65                 .name           = "bsea",
  66                 .clk_req        = CLK_REQUEST_BSEA,
  67         },
  68         [AVP_MODULE_ID_VDE]     = {
  69                 .name           = "vde",
  70                 .clk_req        = CLK_REQUEST_VDE,
  71         },
  72 };
  73 #define NUM_AVP_MODULES         ARRAY_SIZE(avp_modules)
  74
  75 struct avp_clk {
  76         struct clk              *clk;
  77         int                     refcnt;
  78         struct avp_module       *mod;
  79 };
  80
  81 struct avp_svc_info {
  82         struct avp_clk                  clks[NUM_CLK_REQUESTS];
  83         /* used for dvfs */
  84         struct clk                      *sclk;
  85         struct clk                      *emcclk;
  86
  87         struct mutex                    clk_lock;
  88
  89         struct trpc_endpoint            *cpu_ep;
  90         struct task_struct              *svc_thread;
  91
  92         /* client for remote allocations, for easy tear down */
  93         struct nvmap_client             *nvmap_remote;
  94         struct trpc_node                *rpc_node;
  95 };
  96
  97 static void do_svc_nvmap_create(struct avp_svc_info *avp_svc,
  98                                 struct svc_msg *_msg,
  99                                 size_t len)
 100 {
 101         struct svc_nvmap_create *msg = (struct svc_nvmap_create *)_msg;
 102         struct svc_nvmap_create_resp resp;
 103         struct nvmap_handle_ref *handle;
 104         u32 handle_id = 0;
 105         u32 err = 0;
 106
 107         handle = nvmap_create_handle(avp_svc->nvmap_remote, msg->size);
 108         if (unlikely(IS_ERR(handle))) {
 109                 pr_err("avp_svc: error creating handle (%d bytes) for remote\n",
 110                        msg->size);
 111                 err = AVP_ERR_ENOMEM;
 112         } else
 113                 handle_id = (u32)nvmap_ref_to_id(handle);
 114
 115         resp.svc_id = SVC_NVMAP_CREATE_RESPONSE;
 116         resp.err = err;
 117         resp.handle_id = handle_id;
 118         trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
 119                       sizeof(resp), GFP_KERNEL);
 120         /* TODO: do we need to put the handle if send_msg failed? */
 121 }
 122
 123 static void do_svc_nvmap_alloc(struct avp_svc_info *avp_svc,
 124                                struct svc_msg *_msg,
 125                                size_t len)
 126 {
 127         struct svc_nvmap_alloc *msg = (struct svc_nvmap_alloc *)_msg;
 128         struct svc_common_resp resp;
 129         struct nvmap_handle *handle;
 130         u32 err = 0;
 131         u32 heap_mask = 0;
 132         int i;
 133         size_t align;
 134
 135         handle = nvmap_get_handle_id(avp_svc->nvmap_remote, msg->handle_id);
 136         if (IS_ERR(handle)) {
 137                 pr_err("avp_svc: unknown remote handle 0x%x\n", msg->handle_id);
 138                 err = AVP_ERR_EACCES;
 139                 goto out;
 140         }
 141
 142         if (msg->num_heaps > 4) {
 143                 pr_err("avp_svc: invalid remote alloc request (%d heaps?!)\n",
 144                        msg->num_heaps);
 145                 /* TODO: should we error out instead ? */
 146                 msg->num_heaps = 0;
 147         }
 148         if (msg->num_heaps == 0)
 149                 heap_mask = NVMAP_HEAP_CARVEOUT_GENERIC | NVMAP_HEAP_SYSMEM;
 150
 151         for (i = 0; i < msg->num_heaps; i++) {
 152                 switch (msg->heaps[i]) {
 153                 case AVP_NVMAP_HEAP_EXTERNAL:
 154                         heap_mask |= NVMAP_HEAP_SYSMEM;
 155                         break;
 156                 case AVP_NVMAP_HEAP_GART:
 157                         heap_mask |= NVMAP_HEAP_IOVMM;
 158                         break;
 159                 case AVP_NVMAP_HEAP_EXTERNAL_CARVEOUT:
 160                         heap_mask |= NVMAP_HEAP_CARVEOUT_GENERIC;
 161                         break;
 162                 case AVP_NVMAP_HEAP_IRAM:
 163                         heap_mask |= NVMAP_HEAP_CARVEOUT_IRAM;
 164                         break;
 165                 default:
 166                         break;
 167                 }
 168         }
 169
 170         align = max_t(size_t, L1_CACHE_BYTES, msg->align);
 171         err = nvmap_alloc_handle_id(avp_svc->nvmap_remote, msg->handle_id,
 172                                     heap_mask, align, 0);
 173         nvmap_handle_put(handle);
 174         if (err) {
 175                 pr_err("avp_svc: can't allocate for handle 0x%x (%d)\n",
 176                        msg->handle_id, err);
 177                 err = AVP_ERR_ENOMEM;
 178         }
 179
 180 out:
 181         resp.svc_id = SVC_NVMAP_ALLOC_RESPONSE;
 182         resp.err = err;
 183         trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
 184                       sizeof(resp), GFP_KERNEL);
 185 }
 186
 187 static void do_svc_nvmap_free(struct avp_svc_info *avp_svc,
 188                               struct svc_msg *_msg,
 189                               size_t len)
 190 {
 191         struct svc_nvmap_free *msg = (struct svc_nvmap_free *)_msg;
 192
 193         nvmap_free_handle_id(avp_svc->nvmap_remote, msg->handle_id);
 194 }
 195
 196 static void do_svc_nvmap_pin(struct avp_svc_info *avp_svc,
 197                              struct svc_msg *_msg,
 198                              size_t len)
 199 {
 200         struct svc_nvmap_pin *msg = (struct svc_nvmap_pin *)_msg;
 201         struct svc_nvmap_pin_resp resp;
 202         struct nvmap_handle_ref *handle;
 203         unsigned long addr = ~0UL;
 204         unsigned long id = msg->handle_id;
 205         int err;
 206
 207         handle = nvmap_duplicate_handle_id(avp_svc->nvmap_remote, id);
 208         if (IS_ERR(handle)) {
 209                 pr_err("avp_svc: can't dup handle %lx\n", id);
 210                 goto out;
 211         }
 212         err = nvmap_pin_ids(avp_svc->nvmap_remote, 1, &id);
 213         if (err) {
 214                 pr_err("avp_svc: can't pin for handle %lx (%d)\n", id, err);
 215                 goto out;
 216         }
 217         addr = nvmap_handle_address(avp_svc->nvmap_remote, id);
 218
 219 out:
 220         resp.svc_id = SVC_NVMAP_PIN_RESPONSE;
 221         resp.addr = addr;
 222         trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
 223                       sizeof(resp), GFP_KERNEL);
 224 }
 225
 226 static void do_svc_nvmap_unpin(struct avp_svc_info *avp_svc,
 227                                struct svc_msg *_msg,
 228                                size_t len)
 229 {
 230         struct svc_nvmap_unpin *msg = (struct svc_nvmap_unpin *)_msg;
 231         struct svc_common_resp resp;
 232         unsigned long id = msg->handle_id;
 233
 234         nvmap_unpin_ids(avp_svc->nvmap_remote, 1, &id);
 235         nvmap_free_handle_id(avp_svc->nvmap_remote, id);
 236
 237         resp.svc_id = SVC_NVMAP_UNPIN_RESPONSE;
 238         resp.err = 0;
 239         trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
 240                       sizeof(resp), GFP_KERNEL);
 241 }
 242
 243 static void do_svc_nvmap_from_id(struct avp_svc_info *avp_svc,
 244                                  struct svc_msg *_msg,
 245                                  size_t len)
 246 {
 247         struct svc_nvmap_from_id *msg = (struct svc_nvmap_from_id *)_msg;
 248         struct svc_common_resp resp;
 249         struct nvmap_handle_ref *handle;
 250         int err = 0;
 251
 252         handle = nvmap_duplicate_handle_id(avp_svc->nvmap_remote,
 253                                            msg->handle_id);
 254         if (IS_ERR(handle)) {
 255                 pr_err("avp_svc: can't duplicate handle for id 0x%x (%d)\n",
 256                        msg->handle_id, (int)PTR_ERR(handle));
 257                 err = AVP_ERR_ENOMEM;
 258         }
 259
 260         resp.svc_id = SVC_NVMAP_FROM_ID_RESPONSE;
 261         resp.err = err;
 262         trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
 263                       sizeof(resp), GFP_KERNEL);
 264 }
 265
 266 static void do_svc_nvmap_get_addr(struct avp_svc_info *avp_svc,
 267                                   struct svc_msg *_msg,
 268                                   size_t len)
 269 {
 270         struct svc_nvmap_get_addr *msg = (struct svc_nvmap_get_addr *)_msg;
 271         struct svc_nvmap_get_addr_resp resp;
 272
 273         resp.svc_id = SVC_NVMAP_GET_ADDRESS_RESPONSE;
 274         resp.addr = nvmap_handle_address(avp_svc->nvmap_remote, msg->handle_id);
 275         resp.addr += msg->offs;
 276         trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
 277                       sizeof(resp), GFP_KERNEL);
 278 }
 279
 280 static void do_svc_pwr_register(struct avp_svc_info *avp_svc,
 281                                 struct svc_msg *_msg,
 282                                 size_t len)
 283 {
 284         struct svc_pwr_register *msg = (struct svc_pwr_register *)_msg;
 285         struct svc_pwr_register_resp resp;
 286
 287         resp.svc_id = SVC_POWER_RESPONSE;
 288         resp.err = 0;
 289         resp.client_id = msg->client_id;
 290
 291         trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
 292                       sizeof(resp), GFP_KERNEL);
 293 }
 294
 295 static struct avp_module *find_avp_module(struct avp_svc_info *avp_svc, u32 id)
 296 {
 297         if (id < NUM_AVP_MODULES && avp_modules[id].name)
 298                 return &avp_modules[id];
 299         return NULL;
 300 }
 301
 302 static void do_svc_module_reset(struct avp_svc_info *avp_svc,
 303                                 struct svc_msg *_msg,
 304                                 size_t len)
 305 {
 306         struct svc_module_ctrl *msg = (struct svc_module_ctrl *)_msg;
 307         struct svc_common_resp resp;
 308         struct avp_module *mod;
 309         struct avp_clk *aclk;
 310
 311         mod = find_avp_module(avp_svc, msg->module_id);
 312         if (!mod) {
 313                 if (msg->module_id == AVP_MODULE_ID_AVP)
 314                         pr_err("avp_svc: AVP suicidal?!?!\n");
 315                 else
 316                         pr_err("avp_svc: Unknown module reset requested: %d\n",
 317                                msg->module_id);
 318                 /* other side doesn't handle errors for reset */
 319                 resp.err = 0;
 320                 goto send_response;
 321         }
 322
 323         aclk = &avp_svc->clks[mod->clk_req];
 324         tegra_periph_reset_assert(aclk->clk);
 325         udelay(10);
 326         tegra_periph_reset_deassert(aclk->clk);
 327         resp.err = 0;
 328
 329 send_response:
 330         resp.svc_id = SVC_MODULE_RESET_RESPONSE;
 331         trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
 332                       sizeof(resp), GFP_KERNEL);
 333 }
 334
 335 static void do_svc_module_clock(struct avp_svc_info *avp_svc,
 336                                 struct svc_msg *_msg,
 337                                 size_t len)
 338 {
 339         struct svc_module_ctrl *msg = (struct svc_module_ctrl *)_msg;
 340         struct svc_common_resp resp;
 341         struct avp_module *mod;
 342         struct avp_clk *aclk;
 343
 344         mod = find_avp_module(avp_svc, msg->module_id);
 345         if (!mod) {
 346                 pr_err("avp_svc: unknown module clock requested: %d\n",
 347                        msg->module_id);
 348                 resp.err = AVP_ERR_EINVAL;
 349                 goto send_response;
 350         }
 351
 352         mutex_lock(&avp_svc->clk_lock);
 353         aclk = &avp_svc->clks[mod->clk_req];
 354         if (msg->enable) {
 355                 if (aclk->refcnt++ == 0) {
 356                         clk_enable(avp_svc->emcclk);
 357                         clk_enable(avp_svc->sclk);
 358                         clk_enable(aclk->clk);
 359                 }
 360         } else {
 361                 if (unlikely(aclk->refcnt == 0)) {
 362                         pr_err("avp_svc: unbalanced clock disable for '%s'\n",
 363                                aclk->mod->name);
 364                 } else if (--aclk->refcnt == 0) {
 365                         clk_disable(aclk->clk);
 366                         clk_disable(avp_svc->sclk);
 367                         clk_disable(avp_svc->emcclk);
 368                 }
 369         }
 370         mutex_unlock(&avp_svc->clk_lock);
 371         resp.err = 0;
 372
 373 send_response:
 374         resp.svc_id = SVC_MODULE_CLOCK_RESPONSE;
 375         trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
 376                       sizeof(resp), GFP_KERNEL);
 377 }
 378
 379 static void do_svc_null_response(struct avp_svc_info *avp_svc,
 380                                  struct svc_msg *_msg,
 381                                  size_t len, u32 resp_svc_id)
 382 {
 383         struct svc_common_resp resp;
 384         resp.svc_id = resp_svc_id;
 385         resp.err = 0;
 386         trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
 387                       sizeof(resp), GFP_KERNEL);
 388 }
 389
 390 static void do_svc_dfs_get_state(struct avp_svc_info *avp_svc,
 391                                  struct svc_msg *_msg,
 392                                  size_t len)
 393 {
 394         struct svc_dfs_get_state_resp resp;
 395         resp.svc_id = SVC_DFS_GETSTATE_RESPONSE;
 396         resp.state = AVP_DFS_STATE_STOPPED;
 397         trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
 398                       sizeof(resp), GFP_KERNEL);
 399 }
 400
 401 static void do_svc_dfs_get_clk_util(struct avp_svc_info *avp_svc,
 402                                     struct svc_msg *_msg,
 403                                     size_t len)
 404 {
 405         struct svc_dfs_get_clk_util_resp resp;
 406
 407         resp.svc_id = SVC_DFS_GET_CLK_UTIL_RESPONSE;
 408         resp.err = 0;
 409         memset(&resp.usage, 0, sizeof(struct avp_clk_usage));
 410         trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
 411                       sizeof(resp), GFP_KERNEL);
 412 }
 413
 414 static void do_svc_pwr_max_freq(struct avp_svc_info *avp_svc,
 415                                 struct svc_msg *_msg,
 416                                 size_t len)
 417 {
 418         struct svc_pwr_max_freq_resp resp;
 419
 420         resp.svc_id = SVC_POWER_MAXFREQ;
 421         resp.freq = 0;
 422         trpc_send_msg(avp_svc->rpc_node, avp_svc->cpu_ep, &resp,
 423                       sizeof(resp), GFP_KERNEL);
 424 }
 425
 426 static void do_svc_printf(struct avp_svc_info *avp_svc, struct svc_msg *_msg,
 427                           size_t len)
 428 {
 429         struct svc_printf *msg = (struct svc_printf *)_msg;
 430         char tmp_str[SVC_MAX_STRING_LEN];
 431
 432         /* ensure we null terminate the source */
 433         strlcpy(tmp_str, msg->str, SVC_MAX_STRING_LEN);
 434         pr_info("[AVP]: %s", tmp_str);
 435 }
 436
 437 static int dispatch_svc_message(struct avp_svc_info *avp_svc,
 438                                 struct svc_msg *msg,
 439                                 size_t len)
 440 {
 441         int ret = 0;
 442
 443         switch (msg->svc_id) {
 444         case SVC_NVMAP_CREATE:
 445                 DBG(AVP_DBG_TRACE_SVC, "%s: got nvmap_create\n", __func__);
 446                 do_svc_nvmap_create(avp_svc, msg, len);
 447                 break;
 448         case SVC_NVMAP_ALLOC:
 449                 DBG(AVP_DBG_TRACE_SVC, "%s: got nvmap_alloc\n", __func__);
 450                 do_svc_nvmap_alloc(avp_svc, msg, len);
 451                 break;
 452         case SVC_NVMAP_FREE:
 453                 DBG(AVP_DBG_TRACE_SVC, "%s: got nvmap_free\n", __func__);
 454                 do_svc_nvmap_free(avp_svc, msg, len);
 455                 break;
 456         case SVC_NVMAP_PIN:
 457                 DBG(AVP_DBG_TRACE_SVC, "%s: got nvmap_pin\n", __func__);
 458                 do_svc_nvmap_pin(avp_svc, msg, len);
 459                 break;
 460         case SVC_NVMAP_UNPIN:
 461                 DBG(AVP_DBG_TRACE_SVC, "%s: got nvmap_unpin\n", __func__);
 462                 do_svc_nvmap_unpin(avp_svc, msg, len);
 463                 break;
 464         case SVC_NVMAP_FROM_ID:
 465                 DBG(AVP_DBG_TRACE_SVC, "%s: got nvmap_from_id\n", __func__);
 466                 do_svc_nvmap_from_id(avp_svc, msg, len);
 467                 break;
 468         case SVC_NVMAP_GET_ADDRESS:
 469                 DBG(AVP_DBG_TRACE_SVC, "%s: got nvmap_get_addr\n", __func__);
 470                 do_svc_nvmap_get_addr(avp_svc, msg, len);
 471                 break;
 472         case SVC_POWER_REGISTER:
 473                 DBG(AVP_DBG_TRACE_SVC, "%s: got power_register\n", __func__);
 474                 do_svc_pwr_register(avp_svc, msg, len);
 475                 break;
 476         case SVC_POWER_UNREGISTER:
 477                 DBG(AVP_DBG_TRACE_SVC, "%s: got power_unregister\n", __func__);
 478                 /* nothing to do */
 479                 break;
 480         case SVC_POWER_BUSY_HINT_MULTI:
 481                 DBG(AVP_DBG_TRACE_SVC, "%s: got power_busy_hint_multi\n",
 482                     __func__);
 483                 /* nothing to do */
 484                 break;
 485         case SVC_POWER_BUSY_HINT:
 486         case SVC_POWER_STARVATION:
 487                 DBG(AVP_DBG_TRACE_SVC, "%s: got power busy/starve hint\n",
 488                     __func__);
 489                 do_svc_null_response(avp_svc, msg, len, SVC_POWER_RESPONSE);
 490                 break;
 491         case SVC_POWER_MAXFREQ:
 492                 DBG(AVP_DBG_TRACE_SVC, "%s: got power get_max_freq\n",
 493                     __func__);
 494                 do_svc_pwr_max_freq(avp_svc, msg, len);
 495                 break;
 496         case SVC_DFS_GETSTATE:
 497                 DBG(AVP_DBG_TRACE_SVC, "%s: got dfs_get_state\n", __func__);
 498                 do_svc_dfs_get_state(avp_svc, msg, len);
 499                 break;
 500         case SVC_MODULE_RESET:
 501                 DBG(AVP_DBG_TRACE_SVC, "%s: got module_reset\n", __func__);
 502                 do_svc_module_reset(avp_svc, msg, len);
 503                 break;
 504         case SVC_MODULE_CLOCK:
 505                 DBG(AVP_DBG_TRACE_SVC, "%s: got module_clock\n", __func__);
 506                 do_svc_module_clock(avp_svc, msg, len);
 507                 break;
 508         case SVC_DFS_GET_CLK_UTIL:
 509                 DBG(AVP_DBG_TRACE_SVC, "%s: got get_clk_util\n", __func__);
 510                 do_svc_dfs_get_clk_util(avp_svc, msg, len);
 511                 break;
 512         case SVC_PRINTF:
 513                 DBG(AVP_DBG_TRACE_SVC, "%s: got remote printf\n", __func__);
 514                 do_svc_printf(avp_svc, msg, len);
 515                 break;
 516         case SVC_AVP_WDT_RESET:
 517                 pr_err("avp_svc: AVP has been reset by watchdog\n");
 518                 break;
 519         default:
 520                 pr_err("avp_svc: invalid SVC call 0x%x\n", msg->svc_id);
 521                 ret = -ENOMSG;
 522                 break;
 523         }
 524
 525         return ret;
 526 }
 527
 528 static int avp_svc_thread(void *data)
 529 {
 530         struct avp_svc_info *avp_svc = data;
 531         u8 buf[TEGRA_RPC_MAX_MSG_LEN];
 532         struct svc_msg *msg = (struct svc_msg *)buf;
 533         int ret;
 534
 535         BUG_ON(!avp_svc->cpu_ep);
 536
 537         ret = trpc_wait_peer(avp_svc->cpu_ep, -1);
 538         if (ret) {
 539                 /* XXX: teardown?! */
 540                 pr_err("%s: no connection from AVP (%d)\n", __func__, ret);
 541                 goto err;
 542         }
 543
 544         pr_info("%s: got remote peer\n", __func__);
 545
 546         while (!kthread_should_stop()) {
 547                 DBG(AVP_DBG_TRACE_SVC, "%s: waiting for message\n", __func__);
 548                 ret = trpc_recv_msg(avp_svc->rpc_node, avp_svc->cpu_ep, buf,
 549                                     TEGRA_RPC_MAX_MSG_LEN, -1);
 550                 DBG(AVP_DBG_TRACE_SVC, "%s: got message\n", __func__);
 551                 if (ret < 0) {
 552                         pr_err("%s: couldn't receive msg\n", __func__);
 553                         /* XXX: port got closed? we should exit? */
 554                         goto err;
 555                 } else if (!ret) {
 556                         pr_err("%s: received msg of len 0?!\n", __func__);
 557                         continue;
 558                 }
 559                 dispatch_svc_message(avp_svc, msg, ret);
 560         }
 561
 562 err:
 563         trpc_put(avp_svc->cpu_ep);
 564         pr_info("%s: done\n", __func__);
 565         return ret;
 566 }
 567
 568 int avp_svc_start(struct avp_svc_info *avp_svc)
 569 {
 570         struct trpc_endpoint *ep;
 571         int ret;
 572
 573         avp_svc->nvmap_remote = nvmap_create_client(nvmap_dev, "avp_remote");
 574         if (IS_ERR(avp_svc->nvmap_remote)) {
 575                 pr_err("%s: cannot create remote nvmap client\n", __func__);
 576                 ret = PTR_ERR(avp_svc->nvmap_remote);
 577                 goto err_nvmap_create_remote_client;
 578         }
 579
 580         ep = trpc_create(avp_svc->rpc_node, "RPC_CPU_PORT", NULL, NULL);
 581         if (IS_ERR(ep)) {
 582                 pr_err("%s: can't create RPC_CPU_PORT\n", __func__);
 583                 ret = PTR_ERR(ep);
 584                 goto err_cpu_port_create;
 585         }
 586
 587         /* TODO: protect this */
 588         avp_svc->cpu_ep = ep;
 589
 590         /* the service thread should get an extra reference for the port */
 591         trpc_get(avp_svc->cpu_ep);
 592         avp_svc->svc_thread = kthread_run(avp_svc_thread, avp_svc,
 593                                           "avp_svc_thread");
 594         if (IS_ERR_OR_NULL(avp_svc->svc_thread)) {
 595                 avp_svc->svc_thread = NULL;
 596                 pr_err("%s: can't create svc thread\n", __func__);
 597                 ret = -ENOMEM;
 598                 goto err_kthread;
 599         }
 600         return 0;
 601
 602 err_kthread:
 603         trpc_close(avp_svc->cpu_ep);
 604         trpc_put(avp_svc->cpu_ep);
 605         avp_svc->cpu_ep = NULL;
 606 err_cpu_port_create:
 607         nvmap_client_put(avp_svc->nvmap_remote);
 608 err_nvmap_create_remote_client:
 609         avp_svc->nvmap_remote = NULL;
 610         return ret;
 611 }
 612
 613 void avp_svc_stop(struct avp_svc_info *avp_svc)
 614 {
 615         int ret;
 616         int i;
 617
 618         trpc_close(avp_svc->cpu_ep);
 619         ret = kthread_stop(avp_svc->svc_thread);
 620         if (ret == -EINTR) {
 621                 /* the thread never started, drop it's extra reference */
 622                 trpc_put(avp_svc->cpu_ep);
 623         }
 624         avp_svc->cpu_ep = NULL;
 625
 626         nvmap_client_put(avp_svc->nvmap_remote);
 627         avp_svc->nvmap_remote = NULL;
 628
 629         mutex_lock(&avp_svc->clk_lock);
 630         for (i = 0; i < NUM_CLK_REQUESTS; i++) {
 631                 struct avp_clk *aclk = &avp_svc->clks[i];
 632                 BUG_ON(aclk->refcnt < 0);
 633                 if (aclk->refcnt > 0) {
 634                         pr_info("%s: remote left clock '%s' on\n", __func__,
 635                                 aclk->mod->name);
 636                         clk_disable(aclk->clk);
 637                         /* sclk/emcclk was enabled once for every clock */
 638                         clk_disable(avp_svc->sclk);
 639                         clk_disable(avp_svc->emcclk);
 640                 }
 641                 aclk->refcnt = 0;
 642         }
 643         mutex_unlock(&avp_svc->clk_lock);
 644 }
 645
 646 struct avp_svc_info *avp_svc_init(struct platform_device *pdev,
 647                                   struct trpc_node *rpc_node)
 648 {
 649         struct avp_svc_info *avp_svc;
 650         int ret;
 651         int i;
 652         int cnt = 0;
 653
 654         BUG_ON(!rpc_node);
 655
 656         avp_svc = kzalloc(sizeof(struct avp_svc_info), GFP_KERNEL);
 657         if (!avp_svc) {
 658                 ret = -ENOMEM;
 659                 goto err_alloc;
 660         }
 661
 662         BUILD_BUG_ON(NUM_CLK_REQUESTS > BITS_PER_LONG);
 663
 664         for (i = 0; i < NUM_AVP_MODULES; i++) {
 665                 struct avp_module *mod = &avp_modules[i];
 666                 struct clk *clk;
 667                 if (!mod->name)
 668                         continue;
 669                 BUG_ON(mod->clk_req >= NUM_CLK_REQUESTS ||
 670                        cnt++ >= NUM_CLK_REQUESTS);
 671
 672                 clk = clk_get(&pdev->dev, mod->name);
 673                 if (IS_ERR(clk)) {
 674                         ret = PTR_ERR(clk);
 675                         pr_err("avp_svc: Couldn't get required clocks\n");
 676                         goto err_get_clks;
 677                 }
 678                 avp_svc->clks[mod->clk_req].clk = clk;
 679                 avp_svc->clks[mod->clk_req].mod = mod;
 680                 avp_svc->clks[mod->clk_req].refcnt = 0;
 681         }
 682
 683         avp_svc->sclk = clk_get(&pdev->dev, "sclk");
 684         if (IS_ERR(avp_svc->sclk)) {
 685                 pr_err("avp_svc: Couldn't get sclk for dvfs\n");
 686                 ret = -ENOENT;
 687                 goto err_get_clks;
 688         }
 689
 690         avp_svc->emcclk = clk_get(&pdev->dev, "emc");
 691         if (IS_ERR(avp_svc->emcclk)) {
 692                 pr_err("avp_svc: Couldn't get emcclk for dvfs\n");
 693                 ret = -ENOENT;
 694                 goto err_get_clks;
 695         }
 696
 697         /*
 698          * The emc is a shared clock, it will be set to the highest
 699          * requested rate from any user.  Set the rate to ULONG_MAX to
 700          * always request the max rate whenever this request is enabled
 701          */
 702         clk_set_rate(avp_svc->emcclk, ULONG_MAX);
 703
 704         avp_svc->rpc_node = rpc_node;
 705
 706         mutex_init(&avp_svc->clk_lock);
 707
 708         return avp_svc;
 709
 710 err_get_clks:
 711         for (i = 0; i < NUM_CLK_REQUESTS; i++)
 712                 if (avp_svc->clks[i].clk)
 713                         clk_put(avp_svc->clks[i].clk);
 714         if (!IS_ERR_OR_NULL(avp_svc->sclk))
 715                 clk_put(avp_svc->sclk);
 716         if (!IS_ERR_OR_NULL(avp_svc->emcclk))
 717                 clk_put(avp_svc->emcclk);
 718 err_alloc:
 719         return ERR_PTR(ret);
 720 }
 721
 722 void avp_svc_destroy(struct avp_svc_info *avp_svc)
 723 {
 724         int i;
 725
 726         for (i = 0; i < NUM_CLK_REQUESTS; i++)
 727                 clk_put(avp_svc->clks[i].clk);
 728         clk_put(avp_svc->sclk);
 729         clk_put(avp_svc->emcclk);
 730
 731         kfree(avp_svc);
 732 }