pvr/sysutils.c

   1 /**********************************************************************
   2  *
   3  * Copyright(c) 2008 Imagination Technologies Ltd. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms and conditions of the GNU General Public License,
   7  * version 2, as published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope it will be useful but, except
  10  * as otherwise stated in writing, without any warranty; without even the
  11  * implied warranty of merchantability or fitness for a particular purpose.
  12  * See the GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along with
  15  * this program; if not, write to the Free Software Foundation, Inc.,
  16  * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
  17  *
  18  * The full GNU General Public License is included in this distribution in
  19  * the file called "COPYING".
  20  *
  21  * Contact Information:
  22  * Imagination Technologies Ltd. <gpl-support@imgtec.com>
  23  * Home Park Estate, Kings Langley, Herts, WD4 8LZ, UK
  24  *
  25  ******************************************************************************/
  26
  27 #include <linux/version.h>
  28 #include <linux/clk.h>
  29 #include <linux/err.h>
  30 #include <linux/hardirq.h>
  31 #include <plat/omap-pm.h>
  32 #include <linux/bug.h>
  33 #include <plat/clock.h>
  34 #include <plat/cpu.h>
  35 #include "sgxdefs.h"
  36 #include "services_headers.h"
  37 #include "sysinfo.h"
  38 #include "sgxapi_km.h"
  39 #include "sysconfig.h"
  40 #include "sgxinfokm.h"
  41 #include "syslocal.h"
  42 #include "env_data.h"
  43 #include "ocpdefs.h"
  44
  45 #define HZ_TO_MHZ(m) ((m) / 1000000)
  46
  47 static inline unsigned long scale_by_rate(unsigned long val,
  48                                           unsigned long rate1,
  49                                           unsigned long rate2)
  50 {
  51         if (rate1 >= rate2)
  52                 return val * (rate1 / rate2);
  53
  54         return val / (rate2 / rate1);
  55 }
  56
  57 static inline unsigned long scale_prop_to_SGX_clock(unsigned long val,
  58                                                     unsigned long rate)
  59 {
  60         return scale_by_rate(val, rate, sgx_get_max_freq());
  61 }
  62
  63 void SysGetSGXTimingInformation(struct SGX_TIMING_INFORMATION *psTimingInfo)
  64 {
  65         unsigned long rate;
  66
  67 #if defined(NO_HARDWARE)
  68         rate = SYS_SGX_MAX_FREQ_NO_HW;
  69 #else
  70         PVR_ASSERT(atomic_read(&gpsSysSpecificData->sSGXClocksEnabled) != 0);
  71
  72         rate = clk_get_rate(gpsSysSpecificData->psSGX_FCK);
  73         PVR_ASSERT(rate != 0);
  74 #endif
  75         psTimingInfo->ui32CoreClockSpeed = rate;
  76         psTimingInfo->ui32HWRecoveryFreq =
  77             scale_prop_to_SGX_clock(SYS_SGX_HWRECOVERY_TIMEOUT_FREQ, rate);
  78         psTimingInfo->ui32uKernelFreq =
  79             scale_prop_to_SGX_clock(SYS_SGX_PDS_TIMER_FREQ, rate);
  80         psTimingInfo->ui32ActivePowManLatencyms =
  81             SYS_SGX_ACTIVE_POWER_LATENCY_MS;
  82 }
  83
  84 static int vdd2_post_func(struct notifier_block *n, unsigned long event,
  85                           void *ptr)
  86 {
  87         PVR_UNREFERENCED_PARAMETER(n);
  88         PVR_UNREFERENCED_PARAMETER(event);
  89         PVR_UNREFERENCED_PARAMETER(ptr);
  90
  91         if (atomic_read(&gpsSysSpecificData->sSGXClocksEnabled) != 0 &&
  92             gpsSysSpecificData->bSGXInitComplete) {
  93 #if defined(CONFIG_PVR_DEBUG_EXTRA)
  94                 unsigned long rate;
  95
  96                 rate = clk_get_rate(gpsSysSpecificData->psSGX_FCK);
  97
  98                 PVR_ASSERT(rate != 0);
  99
 100                 PVR_TRACE("%s: SGX clock rate: %dMHz", __func__,
 101                            HZ_TO_MHZ(rate));
 102 #endif
 103                 PVRSRVDevicePostClockSpeedChange(gpsSysSpecificData->
 104                                                  psSGXDevNode->sDevId.
 105                                                  ui32DeviceIndex, IMG_TRUE,
 106                                                  NULL);
 107         }
 108         return 0;
 109 }
 110
 111 static int vdd2_pre_func(struct notifier_block *n, unsigned long event,
 112                          void *ptr)
 113 {
 114         PVR_UNREFERENCED_PARAMETER(n);
 115         PVR_UNREFERENCED_PARAMETER(event);
 116         PVR_UNREFERENCED_PARAMETER(ptr);
 117
 118         if (atomic_read(&gpsSysSpecificData->sSGXClocksEnabled) != 0 &&
 119             gpsSysSpecificData->bSGXInitComplete) {
 120                 BUG_ON(gpsSysData->eCurrentPowerState > PVRSRV_POWER_STATE_D1);
 121                 PVRSRVDevicePreClockSpeedChange(gpsSysSpecificData->
 122                                                 psSGXDevNode->sDevId.
 123                                                 ui32DeviceIndex, IMG_TRUE,
 124                                                 NULL);
 125         }
 126
 127         return 0;
 128 }
 129
 130 static int vdd2_pre_post_func(struct notifier_block *n, unsigned long event,
 131                               void *ptr)
 132 {
 133         struct clk_notifier_data *cnd;
 134
 135         PVR_UNREFERENCED_PARAMETER(n);
 136
 137         cnd = (struct clk_notifier_data *)ptr;
 138
 139         PVR_TRACE("vdd2_pre_post_func: %s clock rate = %lu",
 140                   (CLK_PRE_RATE_CHANGE == event) ? "old" :
 141                   (CLK_POST_RATE_CHANGE == event) ? "new" :
 142                   "???",
 143                   cnd->rate);
 144
 145         if (CLK_PRE_RATE_CHANGE == event) {
 146                 PVRSRVDvfsLock();
 147                 PVR_TRACE("vdd2_pre_post_func: CLK_PRE_RATE_CHANGE event");
 148                 vdd2_pre_func(n, event, ptr);
 149         } else if (CLK_POST_RATE_CHANGE == event) {
 150                 PVR_TRACE("vdd2_pre_post_func: CLK_POST_RATE_CHANGE event");
 151                 vdd2_post_func(n, event, ptr);
 152                 PVRSRVDvfsUnlock();
 153         } else if (CLK_ABORT_RATE_CHANGE == event) {
 154                 PVR_TRACE("vdd2_pre_post_func: CLK_ABORT_RATE_CHANGE event");
 155                 PVRSRVDvfsUnlock();
 156         } else {
 157                 printk(KERN_ERR "vdd2_pre_post_func: unexpected event (%lu)\n",
 158                         event);
 159                 PVR_DPF(PVR_DBG_ERROR,
 160                          "vdd2_pre_post_func: unexpected event (%lu)", event);
 161         }
 162         PVR_TRACE("vdd2_pre_post_func end.");
 163         return 0;
 164 }
 165
 166 static struct notifier_block vdd2_pre_post = {
 167         vdd2_pre_post_func,
 168         NULL
 169 };
 170
 171 static void RegisterConstraintNotifications(struct SYS_SPECIFIC_DATA
 172                                             *psSysSpecData)
 173 {
 174         PVR_TRACE("Registering constraint notifications");
 175
 176         clk_notifier_register(psSysSpecData->psSGX_FCK, &vdd2_pre_post);
 177         PVR_TRACE("VDD2 constraint notifications registered");
 178 }
 179
 180 static void UnRegisterConstraintNotifications(struct SYS_SPECIFIC_DATA
 181                                               *psSysSpecData)
 182 {
 183         PVR_TRACE("Unregistering constraint notifications");
 184
 185         clk_notifier_unregister(psSysSpecData->psSGX_FCK, &vdd2_pre_post);
 186 }
 187
 188 static struct device sgx_dev;
 189 static int sgx_clock_enabled;
 190
 191 /* return value: current sgx load
 192  * 0 - not busy
 193  * 100 - busy
 194  */
 195 static unsigned int sgx_current_load(void)
 196 {
 197         enum PVRSRV_ERROR eError;
 198         struct SYS_DATA *psSysData;
 199         struct SYS_SPECIFIC_DATA *psSysSpecData;
 200         struct PVRSRV_DEVICE_NODE *psDeviceNode;
 201         static unsigned int kicks_prev;
 202         static long time_prev;
 203
 204         eError = SysAcquireData(&psSysData);
 205         if (eError != PVRSRV_OK)
 206                 return 0;
 207         psSysSpecData =
 208             (struct SYS_SPECIFIC_DATA *)psSysData->pvSysSpecificData;
 209         if (!psSysSpecData ||
 210             atomic_read(&psSysSpecData->sSGXClocksEnabled) == 0)
 211                 return 0;
 212         psDeviceNode = psSysData->psDeviceNodeList;
 213         while (psDeviceNode) {
 214                 if ((psDeviceNode->sDevId.eDeviceType ==
 215                             PVRSRV_DEVICE_TYPE_SGX) &&
 216                     psDeviceNode->pvDevice) {
 217                         struct PVRSRV_SGXDEV_INFO *psDevInfo =
 218                             (struct PVRSRV_SGXDEV_INFO *)psDeviceNode->pvDevice;
 219                         unsigned int kicks = psDevInfo->ui32KickTACounter;
 220                         unsigned int load;
 221                         long time_elapsed;
 222
 223                         time_elapsed = jiffies - time_prev;
 224                         if (likely(time_elapsed))
 225                                 load =
 226                                     1000 * (kicks - kicks_prev) / time_elapsed;
 227                         else
 228                                 load = 0;
 229                         kicks_prev = kicks;
 230                         time_prev += time_elapsed;
 231                         /*
 232                          * if the period between calls to this function was
 233                          * too long, then load stats are invalid
 234                          */
 235                         if (time_elapsed > 5 * HZ)
 236                                 return 0;
 237                         /*pr_err("SGX load %u\n", load); */
 238
 239                         /*
 240                          * 'load' shows how many times sgx was kicked
 241                          * per 1000 jiffies
 242                          * 150 is arbitrarily chosen threshold.
 243                          * If the number of kicks is below threshold
 244                          * then sgx is doing
 245                          * some small jobs and we can keep the clock freq low.
 246                          */
 247                         if (load < 150)
 248                                 return 0;
 249                         else
 250                                 return 100;
 251                 }
 252                 psDeviceNode = psDeviceNode->psNext;
 253         }
 254         return 0;
 255 }
 256
 257 static void sgx_lock_perf(struct work_struct *work)
 258 {
 259         int vdd1, vdd2;
 260         static int bHigh;
 261         int high;
 262         unsigned int load;
 263         struct delayed_work *d_work =
 264             container_of(work, struct delayed_work, work);
 265         struct ENV_DATA *psEnvData =
 266             container_of(d_work, struct ENV_DATA, sPerfWork);
 267
 268         load = sgx_current_load();
 269         if (load) {
 270                 vdd1 = 500000000;
 271                 vdd2 = 400000;
 272                 high = 1;
 273         } else {
 274                 vdd1 = 0;
 275                 vdd2 = 0;
 276                 high = 0;
 277         }
 278         if (high != bHigh) {
 279                 omap_pm_set_min_bus_tput(&sgx_dev, OCP_INITIATOR_AGENT, vdd2);
 280                 bHigh = high;
 281         }
 282
 283         if (sgx_clock_enabled || load)
 284                 queue_delayed_work(psEnvData->psPerfWorkqueue,
 285                                    &psEnvData->sPerfWork, HZ / 5);
 286 }
 287
 288 static void sgx_need_perf(struct SYS_DATA *psSysData, int ena)
 289 {
 290         struct ENV_DATA *psEnvData =
 291             (struct ENV_DATA *)psSysData->pvEnvSpecificData;
 292
 293         sgx_clock_enabled = ena;
 294         cancel_delayed_work(&psEnvData->sPerfWork);
 295         queue_delayed_work(psEnvData->psPerfWorkqueue, &psEnvData->sPerfWork,
 296                            0);
 297 }
 298
 299 enum PVRSRV_ERROR OSInitPerf(void *pvSysData)
 300 {
 301         struct SYS_DATA *psSysData = (struct SYS_DATA *)pvSysData;
 302         struct ENV_DATA *psEnvData =
 303             (struct ENV_DATA *)psSysData->pvEnvSpecificData;
 304
 305         if (psEnvData->psPerfWorkqueue) {
 306                 PVR_DPF(PVR_DBG_ERROR, "OSInitPerf: already inited");
 307                 return PVRSRV_ERROR_GENERIC;
 308         }
 309
 310         PVR_TRACE("Initing DVFS %x", pvSysData);
 311
 312         psEnvData->psPerfWorkqueue = create_singlethread_workqueue("sgx_perf");
 313         INIT_DELAYED_WORK(&psEnvData->sPerfWork, sgx_lock_perf);
 314
 315         return PVRSRV_OK;
 316 }
 317
 318 enum PVRSRV_ERROR OSCleanupPerf(void *pvSysData)
 319 {
 320         struct SYS_DATA *psSysData = (struct SYS_DATA *)pvSysData;
 321         struct ENV_DATA *psEnvData =
 322             (struct ENV_DATA *)psSysData->pvEnvSpecificData;
 323
 324         if (!psEnvData->psPerfWorkqueue) {
 325                 PVR_DPF(PVR_DBG_ERROR, "OSCleanupPerf: not inited");
 326                 return PVRSRV_ERROR_GENERIC;
 327         }
 328
 329         PVR_TRACE("Cleaning up DVFS");
 330
 331         sgx_clock_enabled = 0;
 332         flush_workqueue(psEnvData->psPerfWorkqueue);
 333         destroy_workqueue(psEnvData->psPerfWorkqueue);
 334
 335         return PVRSRV_OK;
 336 }
 337
 338 static inline void setup_int_bypass(void)
 339 {
 340         if (cpu_is_omap3630())
 341                 sgx_ocp_write_reg(EUR_CR_OCP_DEBUG_CONFIG,
 342                               EUR_CR_OCP_DEBUG_CONFIG_THALIA_INT_BYPASS_MASK);
 343 }
 344
 345 #ifndef NO_HARDWARE
 346
 347 static enum PVRSRV_ERROR sgx_force_enable_clocks(struct SYS_DATA *psSysData)
 348 {
 349         struct SYS_SPECIFIC_DATA *psSysSpecData =
 350             (struct SYS_SPECIFIC_DATA *)psSysData->pvSysSpecificData;
 351         int res;
 352
 353         res = clk_enable(psSysSpecData->psSGX_FCK);
 354         if (res < 0) {
 355                 PVR_DPF(PVR_DBG_ERROR, "%s: "
 356                                 "Couldn't enable SGX functional clock (%d)",
 357                          __func__, res);
 358                 return PVRSRV_ERROR_GENERIC;
 359         }
 360
 361         res = clk_enable(psSysSpecData->psSGX_ICK);
 362         if (res < 0) {
 363                 PVR_DPF(PVR_DBG_ERROR, "%s: "
 364                                 "Couldn't enable SGX interface clock (%d)",
 365                          __func__, res);
 366
 367                 clk_disable(psSysSpecData->psSGX_FCK);
 368                 return PVRSRV_ERROR_GENERIC;
 369         }
 370
 371         setup_int_bypass();
 372
 373         return PVRSRV_OK;
 374 }
 375
 376 static void sgx_force_disable_clocks(struct SYS_DATA *psSysData)
 377 {
 378         struct SYS_SPECIFIC_DATA *psSysSpecData =
 379             (struct SYS_SPECIFIC_DATA *)psSysData->pvSysSpecificData;
 380
 381         if (psSysSpecData->psSGX_ICK)
 382                 clk_disable(psSysSpecData->psSGX_ICK);
 383
 384         if (psSysSpecData->psSGX_FCK)
 385                 clk_disable(psSysSpecData->psSGX_FCK);
 386 }
 387
 388 #else           /* NO_HARDWARE */
 389
 390 static enum PVRSRV_ERROR sgx_force_enable_clocks(struct SYS_DATA *psSYsData)
 391 {
 392         return PVRSRV_OK;
 393 }
 394
 395 static void sgx_force_disable_clocks(struct SYS_DATA *psSYsData)
 396 {
 397 }
 398
 399 #endif          /* NO_HARDWARE */
 400
 401 static bool force_clocks_on(void)
 402 {
 403 #ifdef CONFIG_PVR_FORCE_CLOCKS_ON
 404         return true;
 405 #else
 406         return false;
 407 #endif
 408 }
 409
 410 enum PVRSRV_ERROR EnableSGXClocks(struct SYS_DATA *psSysData)
 411 {
 412         struct SYS_SPECIFIC_DATA *psSysSpecData =
 413             (struct SYS_SPECIFIC_DATA *)psSysData->pvSysSpecificData;
 414         enum PVRSRV_ERROR res = PVRSRV_OK;
 415
 416         if (atomic_xchg(&psSysSpecData->sSGXClocksEnabled, 1))
 417                 return PVRSRV_OK;
 418
 419         /*
 420          * In case of force clocks on we have already enabled the clocks
 421          * at init time.
 422          */
 423         if (!force_clocks_on())
 424                 res = sgx_force_enable_clocks(psSysData);
 425
 426         if (res == PVRSRV_OK) {
 427                 BUG_ON(!atomic_read(&psSysSpecData->sSGXClocksEnabled));
 428                 sgx_need_perf(psSysData, 1);
 429         } else {
 430                 atomic_set(&psSysSpecData->sSGXClocksEnabled, 0);
 431         }
 432
 433         return res;
 434 }
 435
 436 void DisableSGXClocks(struct SYS_DATA *psSysData)
 437 {
 438         struct SYS_SPECIFIC_DATA *psSysSpecData =
 439             (struct SYS_SPECIFIC_DATA *)psSysData->pvSysSpecificData;
 440
 441         if (!atomic_xchg(&psSysSpecData->sSGXClocksEnabled, 0))
 442                 return;
 443
 444         if (!force_clocks_on())
 445                 sgx_force_disable_clocks(psSysData);
 446
 447         BUG_ON(atomic_read(&psSysSpecData->sSGXClocksEnabled));
 448
 449         sgx_need_perf(psSysData, 0);
 450 }
 451
 452 static enum PVRSRV_ERROR InitSgxClocks(struct SYS_DATA *psSysData)
 453 {
 454         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 455         struct clk *psCLK;
 456         struct clk *core_ck = NULL;
 457         int r;
 458
 459         psCLK = clk_get(NULL, "sgx_fck");
 460         if (IS_ERR(psCLK))
 461                 goto err0;
 462         psSysSpecData->psSGX_FCK = psCLK;
 463
 464         psCLK = clk_get(NULL, "sgx_ick");
 465         if (IS_ERR(psCLK))
 466                 goto err1;
 467         psSysSpecData->psSGX_ICK = psCLK;
 468
 469         core_ck = clk_get(NULL, "core_ck");
 470         if (IS_ERR(core_ck))
 471                 goto err2;
 472         if (clk_set_parent(psSysSpecData->psSGX_FCK, core_ck) < 0) {
 473                 clk_put(core_ck);
 474                 goto err2;
 475         }
 476         clk_put(core_ck);
 477
 478         r = clk_set_rate(psSysSpecData->psSGX_FCK,
 479                          sgx_get_max_freq());
 480         if (r < 0) {
 481                 unsigned long rate;
 482
 483                 rate = clk_get_rate(psSysSpecData->psSGX_FCK);
 484                 rate /= 1000000;
 485                 pr_warning("error %d when setting SGX fclk to %luMHz, "
 486                            "falling back to %luMHz\n",
 487                            r, sgx_get_max_freq() / 1000000, rate);
 488         }
 489
 490         RegisterConstraintNotifications(psSysSpecData);
 491         return PVRSRV_OK;
 492
 493 err2:
 494         clk_put(psSysSpecData->psSGX_ICK);
 495 err1:
 496         clk_put(psSysSpecData->psSGX_FCK);
 497 err0:
 498         PVR_DPF(PVR_DBG_ERROR,
 499                  "%s: couldn't init clocks fck %p ick %p core %p", __func__,
 500                  psSysSpecData->psSGX_FCK, psSysSpecData->psSGX_ICK, core_ck);
 501         psSysSpecData->psSGX_FCK = NULL;
 502         psSysSpecData->psSGX_ICK = NULL;
 503
 504         return PVRSRV_ERROR_GENERIC;
 505 }
 506
 507 static void CleanupSgxClocks(struct SYS_DATA *psSysData)
 508 {
 509         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 510         UnRegisterConstraintNotifications(psSysSpecData);
 511
 512         if (psSysSpecData->psSGX_ICK) {
 513                 clk_put(psSysSpecData->psSGX_ICK);
 514                 psSysSpecData->psSGX_ICK = NULL;
 515         }
 516
 517         if (psSysSpecData->psSGX_FCK) {
 518                 clk_put(psSysSpecData->psSGX_FCK);
 519                 psSysSpecData->psSGX_FCK = NULL;
 520         }
 521 }
 522
 523 #if defined(CONFIG_PVR_DEBUG_EXTRA) || defined(TIMING)
 524 static inline u32 gpt_read_reg(struct SYS_DATA *psSysData, u32 reg)
 525 {
 526         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 527
 528         return __raw_readl(psSysSpecData->gpt_base + reg);
 529 }
 530
 531 static inline void gpt_write_reg(struct SYS_DATA *psSysData, u32 reg, u32 val)
 532 {
 533         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 534
 535         __raw_writel(val, psSysSpecData->gpt_base + reg);
 536 }
 537
 538 static enum PVRSRV_ERROR InitDebugClocks(struct SYS_DATA *psSysData)
 539 {
 540         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 541         struct clk *psCLK;
 542         struct clk *sys_ck = NULL;
 543         u32 rate;
 544
 545         psCLK = clk_get(NULL, "mpu_ck");
 546         if (IS_ERR(psCLK))
 547                 goto err0;
 548         psSysSpecData->psMPU_CK = psCLK;
 549
 550         psCLK = clk_get(NULL, "gpt11_fck");
 551         if (IS_ERR(psCLK))
 552                 goto err1;
 553         psSysSpecData->psGPT11_FCK = psCLK;
 554
 555         psCLK = clk_get(NULL, "gpt11_ick");
 556         if (IS_ERR(psCLK))
 557                 goto err2;
 558         psSysSpecData->psGPT11_ICK = psCLK;
 559
 560         sys_ck = clk_get(NULL, "sys_ck");
 561         if (IS_ERR(sys_ck))
 562                 goto err3;
 563         if (clk_get_parent(psSysSpecData->psGPT11_FCK) != sys_ck)
 564                 if (clk_set_parent(psSysSpecData->psGPT11_FCK, sys_ck) < 0) {
 565                         clk_put(sys_ck);
 566                         goto err3;
 567                 }
 568         clk_put(sys_ck);
 569
 570         PVR_TRACE("GPTIMER11 clock is %dMHz",
 571                    HZ_TO_MHZ(clk_get_rate(psSysSpecData->psGPT11_FCK)));
 572
 573         psSysSpecData->gpt_base = ioremap(SYS_OMAP3430_GP11TIMER_PHYS_BASE,
 574                                           SYS_OMAP3430_GPTIMER_SIZE);
 575         if (!psSysSpecData->gpt_base)
 576                 goto err3;
 577
 578         clk_enable(psSysSpecData->psGPT11_ICK);
 579         clk_enable(psSysSpecData->psGPT11_FCK);
 580
 581         rate = gpt_read_reg(psSysData, SYS_OMAP3430_GPTIMER_TSICR);
 582         if (!(rate & 4)) {
 583                 PVR_TRACE("Setting GPTIMER11 mode to posted "
 584                           "(currently is non-posted)");
 585                 gpt_write_reg(psSysData, SYS_OMAP3430_GPTIMER_TSICR, rate | 4);
 586         }
 587
 588         clk_disable(psSysSpecData->psGPT11_FCK);
 589         clk_disable(psSysSpecData->psGPT11_ICK);
 590
 591         return PVRSRV_OK;
 592
 593 err3:
 594         clk_put(psSysSpecData->psGPT11_ICK);
 595 err2:
 596         clk_put(psSysSpecData->psGPT11_FCK);
 597 err1:
 598         clk_put(psSysSpecData->psMPU_CK);
 599 err0:
 600         PVR_DPF(PVR_DBG_ERROR,
 601                  "%s: couldn't init clocks: mpu %p sys %p fck %p ick %p",
 602                  __func__, psSysSpecData->psMPU_CK, sys_ck,
 603                  psSysSpecData->psGPT11_FCK, psSysSpecData->psGPT11_ICK);
 604
 605         psSysSpecData->psMPU_CK = NULL;
 606         psSysSpecData->psGPT11_FCK = NULL;
 607         psSysSpecData->psGPT11_ICK = NULL;
 608
 609         return PVRSRV_ERROR_GENERIC;
 610 }
 611
 612 static void CleanupDebugClocks(struct SYS_DATA *psSysData)
 613 {
 614         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 615
 616         if (psSysSpecData->psMPU_CK) {
 617                 clk_put(psSysSpecData->psMPU_CK);
 618                 psSysSpecData->psMPU_CK = NULL;
 619         }
 620         if (psSysSpecData->psGPT11_FCK) {
 621                 clk_put(psSysSpecData->psGPT11_FCK);
 622                 psSysSpecData->psGPT11_FCK = NULL;
 623         }
 624         if (psSysSpecData->psGPT11_ICK) {
 625                 clk_put(psSysSpecData->psGPT11_ICK);
 626                 psSysSpecData->psGPT11_ICK = NULL;
 627         }
 628 }
 629
 630 static enum PVRSRV_ERROR EnableDebugClocks(struct SYS_DATA *psSysData)
 631 {
 632         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 633
 634         if (clk_enable(psSysSpecData->psGPT11_FCK) < 0)
 635                 goto err0;
 636
 637         if (clk_enable(psSysSpecData->psGPT11_ICK) < 0)
 638                 goto err1;
 639
 640         gpt_write_reg(psSysData, SYS_OMAP3430_GPTIMER_ENABLE, 3);
 641
 642         return PVRSRV_OK;
 643
 644 err1:
 645         clk_disable(psSysSpecData->psGPT11_FCK);
 646 err0:
 647         PVR_DPF(PVR_DBG_ERROR, "%s: can't enable clocks", __func__);
 648
 649         return PVRSRV_ERROR_GENERIC;
 650 }
 651
 652 static inline void DisableDebugClocks(struct SYS_DATA *psSysData)
 653 {
 654         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 655
 656         gpt_write_reg(psSysData, SYS_OMAP3430_GPTIMER_ENABLE, 0);
 657
 658         clk_disable(psSysSpecData->psGPT11_ICK);
 659         clk_disable(psSysSpecData->psGPT11_FCK);
 660 }
 661
 662 #else
 663
 664 inline enum PVRSRV_ERROR InitDebugClocks(struct SYS_DATA *psSysData)
 665 {
 666         return PVRSRV_OK;
 667 }
 668
 669 static inline void CleanupDebugClocks(struct SYS_DATA *psSysData)
 670 {
 671 }
 672
 673 static inline enum PVRSRV_ERROR EnableDebugClocks(struct SYS_DATA *psSysData)
 674 {
 675         return PVRSRV_OK;
 676 }
 677
 678 static inline void DisableDebugClocks(struct SYS_DATA *psSysData)
 679 {
 680 }
 681 #endif
 682
 683 enum PVRSRV_ERROR InitSystemClocks(struct SYS_DATA *psSysData)
 684 {
 685         if (InitSgxClocks(psSysData) != PVRSRV_OK)
 686                 goto err0;
 687
 688         if (InitDebugClocks(psSysData) != PVRSRV_OK)
 689                 goto err1;
 690
 691         return PVRSRV_OK;
 692
 693 err1:
 694         CleanupSgxClocks(psSysData);
 695 err0:
 696         return PVRSRV_ERROR_GENERIC;
 697 }
 698
 699 void CleanupSystemClocks(struct SYS_DATA *psSysData)
 700 {
 701         CleanupDebugClocks(psSysData);
 702         CleanupSgxClocks(psSysData);
 703 }
 704
 705 enum PVRSRV_ERROR EnableSystemClocks(struct SYS_DATA *psSysData)
 706 {
 707         PVR_TRACE("EnableSystemClocks: Enabling System Clocks");
 708
 709         /*
 710          * We force clocks on by increasing their refcount here during
 711          * module init time and decreasing it at cleanup time.
 712          */
 713         if (force_clocks_on())
 714                 sgx_force_enable_clocks(gpsSysData);
 715         if (EnableDebugClocks(psSysData) != PVRSRV_OK)
 716                 goto err1;
 717
 718         return PVRSRV_OK;
 719
 720 err1:
 721         return PVRSRV_ERROR_GENERIC;
 722 }
 723
 724 void DisableSystemClocks(struct SYS_DATA *psSysData)
 725 {
 726         PVR_TRACE("DisableSystemClocks: Disabling System Clocks");
 727
 728         DisableDebugClocks(psSysData);
 729         /* Decrease the clocks' refcount that was increased at init time. */
 730         if (force_clocks_on())
 731                 sgx_force_disable_clocks(gpsSysData);
 732 }