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 #include "pvr_bridge_km.h"
  45
  46 #define HZ_TO_MHZ(m) ((m) / 1000000)
  47
  48 static inline unsigned long scale_by_rate(unsigned long val,
  49                                           unsigned long rate1,
  50                                           unsigned long rate2)
  51 {
  52         if (rate1 >= rate2)
  53                 return val * (rate1 / rate2);
  54
  55         return val / (rate2 / rate1);
  56 }
  57
  58 static inline unsigned long scale_prop_to_SGX_clock(unsigned long val,
  59                                                     unsigned long rate)
  60 {
  61         return scale_by_rate(val, rate, sgx_get_max_freq());
  62 }
  63
  64 void SysGetSGXTimingInformation(struct SGX_TIMING_INFORMATION *psTimingInfo)
  65 {
  66         unsigned long rate;
  67
  68 #if defined(NO_HARDWARE)
  69         rate = SYS_SGX_MAX_FREQ_NO_HW;
  70 #else
  71         rate = clk_get_rate(gpsSysSpecificData->psSGX_FCK);
  72         PVR_ASSERT(rate != 0);
  73 #endif
  74         psTimingInfo->ui32CoreClockSpeed = rate;
  75         psTimingInfo->ui32HWRecoveryFreq =
  76             scale_prop_to_SGX_clock(SYS_SGX_HWRECOVERY_TIMEOUT_FREQ, rate);
  77         psTimingInfo->ui32uKernelFreq =
  78             scale_prop_to_SGX_clock(SYS_SGX_PDS_TIMER_FREQ, rate);
  79         psTimingInfo->ui32ActivePowManLatencyms =
  80             SYS_SGX_ACTIVE_POWER_LATENCY_MS;
  81 }
  82
  83 #if 0
  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                 pvr_dev_lock();
 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                 pvr_dev_unlock();
 153         } else if (CLK_ABORT_RATE_CHANGE == event) {
 154                 PVR_TRACE("vdd2_pre_post_func: CLK_ABORT_RATE_CHANGE event");
 155                 pvr_dev_unlock();
 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 #else
 188 #define RegisterConstraintNotifications(x)
 189 #define UnRegisterConstraintNotifications(x)
 190 #endif
 191
 192 static struct device sgx_dev;
 193 static int sgx_clock_enabled;
 194
 195 /* return value: current sgx load
 196  * 0 - not busy
 197  * 100 - busy
 198  */
 199 static unsigned int sgx_current_load(void)
 200 {
 201         enum PVRSRV_ERROR eError;
 202         struct SYS_DATA *psSysData;
 203         struct SYS_SPECIFIC_DATA *psSysSpecData;
 204         struct PVRSRV_DEVICE_NODE *psDeviceNode;
 205         static unsigned int kicks_prev;
 206         static long time_prev;
 207
 208         eError = SysAcquireData(&psSysData);
 209         if (eError != PVRSRV_OK)
 210                 return 0;
 211         psSysSpecData =
 212             (struct SYS_SPECIFIC_DATA *)psSysData->pvSysSpecificData;
 213         if (!psSysSpecData ||
 214             atomic_read(&psSysSpecData->sSGXClocksEnabled) == 0)
 215                 return 0;
 216         psDeviceNode = psSysData->psDeviceNodeList;
 217         while (psDeviceNode) {
 218                 if ((psDeviceNode->sDevId.eDeviceType ==
 219                             PVRSRV_DEVICE_TYPE_SGX) &&
 220                     psDeviceNode->pvDevice) {
 221                         struct PVRSRV_SGXDEV_INFO *psDevInfo =
 222                             (struct PVRSRV_SGXDEV_INFO *)psDeviceNode->pvDevice;
 223                         unsigned int kicks = psDevInfo->ui32KickTACounter;
 224                         unsigned int load;
 225                         long time_elapsed;
 226
 227                         time_elapsed = jiffies - time_prev;
 228                         if (likely(time_elapsed))
 229                                 load =
 230                                     1000 * (kicks - kicks_prev) / time_elapsed;
 231                         else
 232                                 load = 0;
 233                         kicks_prev = kicks;
 234                         time_prev += time_elapsed;
 235                         /*
 236                          * if the period between calls to this function was
 237                          * too long, then load stats are invalid
 238                          */
 239                         if (time_elapsed > 5 * HZ)
 240                                 return 0;
 241                         /*pr_err("SGX load %u\n", load); */
 242
 243                         /*
 244                          * 'load' shows how many times sgx was kicked
 245                          * per 1000 jiffies
 246                          * 150 is arbitrarily chosen threshold.
 247                          * If the number of kicks is below threshold
 248                          * then sgx is doing
 249                          * some small jobs and we can keep the clock freq low.
 250                          */
 251                         if (load < 150)
 252                                 return 0;
 253                         else
 254                                 return 100;
 255                 }
 256                 psDeviceNode = psDeviceNode->psNext;
 257         }
 258         return 0;
 259 }
 260
 261 static void sgx_lock_perf(struct work_struct *work)
 262 {
 263         int vdd1, vdd2;
 264         static int bHigh;
 265         int high;
 266         unsigned int load;
 267         struct delayed_work *d_work =
 268             container_of(work, struct delayed_work, work);
 269         struct ENV_DATA *psEnvData =
 270             container_of(d_work, struct ENV_DATA, sPerfWork);
 271
 272         pvr_lock();
 273
 274         if (pvr_is_disabled()) {
 275                 pvr_unlock();
 276                 return;
 277         }
 278
 279         load = sgx_current_load();
 280
 281         pvr_unlock();
 282
 283         if (load) {
 284                 vdd1 = 500000000;
 285                 vdd2 = 400000;
 286                 high = 1;
 287         } else {
 288                 vdd1 = 0;
 289                 vdd2 = 0;
 290                 high = 0;
 291         }
 292         if (high != bHigh) {
 293 #if 0
 294                 omap_pm_set_min_bus_tput(&sgx_dev, OCP_INITIATOR_AGENT, vdd2);
 295 #else
 296                 (void)sgx_dev;
 297 #endif
 298                 bHigh = high;
 299         }
 300
 301         if (sgx_clock_enabled || load)
 302                 queue_delayed_work(psEnvData->psPerfWorkqueue,
 303                                    &psEnvData->sPerfWork, HZ / 5);
 304 }
 305
 306 static void sgx_need_perf(struct SYS_DATA *psSysData, int ena)
 307 {
 308         struct ENV_DATA *psEnvData =
 309             (struct ENV_DATA *)psSysData->pvEnvSpecificData;
 310
 311         sgx_clock_enabled = ena;
 312         cancel_delayed_work(&psEnvData->sPerfWork);
 313         queue_delayed_work(psEnvData->psPerfWorkqueue, &psEnvData->sPerfWork,
 314                            0);
 315 }
 316
 317 enum PVRSRV_ERROR OSInitPerf(void *pvSysData)
 318 {
 319         struct SYS_DATA *psSysData = (struct SYS_DATA *)pvSysData;
 320         struct ENV_DATA *psEnvData =
 321             (struct ENV_DATA *)psSysData->pvEnvSpecificData;
 322
 323         if (psEnvData->psPerfWorkqueue) {
 324                 PVR_DPF(PVR_DBG_ERROR, "OSInitPerf: already inited");
 325                 return PVRSRV_ERROR_GENERIC;
 326         }
 327
 328         PVR_TRACE("Initing DVFS %x", pvSysData);
 329
 330         psEnvData->psPerfWorkqueue = create_singlethread_workqueue("sgx_perf");
 331         INIT_DELAYED_WORK(&psEnvData->sPerfWork, sgx_lock_perf);
 332
 333         return PVRSRV_OK;
 334 }
 335
 336 enum PVRSRV_ERROR OSCleanupPerf(void *pvSysData)
 337 {
 338         struct SYS_DATA *psSysData = (struct SYS_DATA *)pvSysData;
 339         struct ENV_DATA *psEnvData =
 340             (struct ENV_DATA *)psSysData->pvEnvSpecificData;
 341
 342         if (!psEnvData->psPerfWorkqueue) {
 343                 PVR_DPF(PVR_DBG_ERROR, "OSCleanupPerf: not inited");
 344                 return PVRSRV_ERROR_GENERIC;
 345         }
 346
 347         PVR_TRACE("Cleaning up DVFS");
 348
 349         sgx_clock_enabled = 0;
 350         flush_workqueue(psEnvData->psPerfWorkqueue);
 351         destroy_workqueue(psEnvData->psPerfWorkqueue);
 352
 353         return PVRSRV_OK;
 354 }
 355
 356 static inline void setup_int_bypass(void)
 357 {
 358         if (cpu_is_omap3630())
 359                 sgx_ocp_write_reg(EUR_CR_OCP_DEBUG_CONFIG,
 360                               EUR_CR_OCP_DEBUG_CONFIG_THALIA_INT_BYPASS_MASK);
 361 }
 362
 363 #ifndef NO_HARDWARE
 364
 365 static enum PVRSRV_ERROR sgx_force_enable_clocks(struct SYS_DATA *psSysData)
 366 {
 367         struct SYS_SPECIFIC_DATA *psSysSpecData =
 368             (struct SYS_SPECIFIC_DATA *)psSysData->pvSysSpecificData;
 369         int res;
 370
 371         res = clk_enable(psSysSpecData->psSGX_FCK);
 372         if (res < 0) {
 373                 PVR_DPF(PVR_DBG_ERROR, "%s: "
 374                                 "Couldn't enable SGX functional clock (%d)",
 375                          __func__, res);
 376                 return PVRSRV_ERROR_GENERIC;
 377         }
 378
 379         res = clk_enable(psSysSpecData->psSGX_ICK);
 380         if (res < 0) {
 381                 PVR_DPF(PVR_DBG_ERROR, "%s: "
 382                                 "Couldn't enable SGX interface clock (%d)",
 383                          __func__, res);
 384
 385                 clk_disable(psSysSpecData->psSGX_FCK);
 386                 return PVRSRV_ERROR_GENERIC;
 387         }
 388
 389         setup_int_bypass();
 390
 391         return PVRSRV_OK;
 392 }
 393
 394 static void sgx_force_disable_clocks(struct SYS_DATA *psSysData)
 395 {
 396         struct SYS_SPECIFIC_DATA *psSysSpecData =
 397             (struct SYS_SPECIFIC_DATA *)psSysData->pvSysSpecificData;
 398
 399         if (psSysSpecData->psSGX_ICK)
 400                 clk_disable(psSysSpecData->psSGX_ICK);
 401
 402         if (psSysSpecData->psSGX_FCK)
 403                 clk_disable(psSysSpecData->psSGX_FCK);
 404 }
 405
 406 #else           /* NO_HARDWARE */
 407
 408 static enum PVRSRV_ERROR sgx_force_enable_clocks(struct SYS_DATA *psSYsData)
 409 {
 410         return PVRSRV_OK;
 411 }
 412
 413 static void sgx_force_disable_clocks(struct SYS_DATA *psSYsData)
 414 {
 415 }
 416
 417 #endif          /* NO_HARDWARE */
 418
 419 static bool force_clocks_on(void)
 420 {
 421 #ifdef CONFIG_PVR_FORCE_CLOCKS_ON
 422         return true;
 423 #else
 424         return false;
 425 #endif
 426 }
 427
 428 enum PVRSRV_ERROR EnableSGXClocks(struct SYS_DATA *psSysData)
 429 {
 430         struct SYS_SPECIFIC_DATA *psSysSpecData =
 431             (struct SYS_SPECIFIC_DATA *)psSysData->pvSysSpecificData;
 432         enum PVRSRV_ERROR res = PVRSRV_OK;
 433
 434         if (atomic_xchg(&psSysSpecData->sSGXClocksEnabled, 1))
 435                 return PVRSRV_OK;
 436
 437         /*
 438          * In case of force clocks on we have already enabled the clocks
 439          * at init time.
 440          */
 441         if (!force_clocks_on())
 442                 res = sgx_force_enable_clocks(psSysData);
 443
 444         if (res == PVRSRV_OK) {
 445                 BUG_ON(!atomic_read(&psSysSpecData->sSGXClocksEnabled));
 446                 sgx_need_perf(psSysData, 1);
 447         } else {
 448                 atomic_set(&psSysSpecData->sSGXClocksEnabled, 0);
 449         }
 450
 451         return res;
 452 }
 453
 454 void DisableSGXClocks(struct SYS_DATA *psSysData)
 455 {
 456         struct SYS_SPECIFIC_DATA *psSysSpecData =
 457             (struct SYS_SPECIFIC_DATA *)psSysData->pvSysSpecificData;
 458
 459         if (!atomic_xchg(&psSysSpecData->sSGXClocksEnabled, 0))
 460                 return;
 461
 462         if (!force_clocks_on())
 463                 sgx_force_disable_clocks(psSysData);
 464
 465         BUG_ON(atomic_read(&psSysSpecData->sSGXClocksEnabled));
 466
 467         sgx_need_perf(psSysData, 0);
 468 }
 469
 470 static enum PVRSRV_ERROR InitSgxClocks(struct SYS_DATA *psSysData)
 471 {
 472         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 473         struct clk *psCLK;
 474         struct clk *core_ck = NULL;
 475         unsigned long rate;
 476         int r;
 477
 478         psCLK = clk_get(NULL, "sgx_fck");
 479         if (IS_ERR(psCLK))
 480                 goto err0;
 481         psSysSpecData->psSGX_FCK = psCLK;
 482
 483         psCLK = clk_get(NULL, "sgx_ick");
 484         if (IS_ERR(psCLK))
 485                 goto err1;
 486         psSysSpecData->psSGX_ICK = psCLK;
 487
 488         core_ck = clk_get(NULL, "core_ck");
 489         if (IS_ERR(core_ck))
 490                 goto err2;
 491         if (clk_set_parent(psSysSpecData->psSGX_FCK, core_ck) < 0) {
 492                 clk_put(core_ck);
 493                 goto err2;
 494         }
 495         clk_put(core_ck);
 496
 497         /* +1 to account for rounding errors */
 498         rate = clk_round_rate(psSysSpecData->psSGX_FCK, sgx_get_max_freq() + 1);
 499         r = clk_set_rate(psSysSpecData->psSGX_FCK, rate);
 500         if (r < 0) {
 501                 unsigned long current_rate;
 502
 503                 current_rate = clk_get_rate(psSysSpecData->psSGX_FCK);
 504                 pr_warning("error %d when setting SGX fclk to %lu Hz, "
 505                            "falling back to %lu Hz\n", r, rate, current_rate);
 506         } else {
 507                 pr_info("SGX clock rate %lu MHz\n", rate / 1000000);
 508         };
 509
 510         RegisterConstraintNotifications(psSysSpecData);
 511         return PVRSRV_OK;
 512
 513 err2:
 514         clk_put(psSysSpecData->psSGX_ICK);
 515 err1:
 516         clk_put(psSysSpecData->psSGX_FCK);
 517 err0:
 518         PVR_DPF(PVR_DBG_ERROR,
 519                  "%s: couldn't init clocks fck %p ick %p core %p", __func__,
 520                  psSysSpecData->psSGX_FCK, psSysSpecData->psSGX_ICK, core_ck);
 521         psSysSpecData->psSGX_FCK = NULL;
 522         psSysSpecData->psSGX_ICK = NULL;
 523
 524         return PVRSRV_ERROR_GENERIC;
 525 }
 526
 527 static void CleanupSgxClocks(struct SYS_DATA *psSysData)
 528 {
 529         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 530         UnRegisterConstraintNotifications(psSysSpecData);
 531
 532         if (psSysSpecData->psSGX_ICK) {
 533                 clk_put(psSysSpecData->psSGX_ICK);
 534                 psSysSpecData->psSGX_ICK = NULL;
 535         }
 536
 537         if (psSysSpecData->psSGX_FCK) {
 538                 clk_put(psSysSpecData->psSGX_FCK);
 539                 psSysSpecData->psSGX_FCK = NULL;
 540         }
 541 }
 542
 543 #if defined(CONFIG_PVR_DEBUG_EXTRA) || defined(TIMING)
 544 static inline u32 gpt_read_reg(struct SYS_DATA *psSysData, u32 reg)
 545 {
 546         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 547
 548         return __raw_readl(psSysSpecData->gpt_base + reg);
 549 }
 550
 551 static inline void gpt_write_reg(struct SYS_DATA *psSysData, u32 reg, u32 val)
 552 {
 553         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 554
 555         __raw_writel(val, psSysSpecData->gpt_base + reg);
 556 }
 557
 558 static enum PVRSRV_ERROR InitDebugClocks(struct SYS_DATA *psSysData)
 559 {
 560         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 561         struct clk *psCLK;
 562         struct clk *sys_ck = NULL;
 563         u32 rate;
 564
 565         psCLK = clk_get(NULL, "mpu_ck");
 566         if (IS_ERR(psCLK))
 567                 goto err0;
 568         psSysSpecData->psMPU_CK = psCLK;
 569
 570         psCLK = clk_get(NULL, "gpt11_fck");
 571         if (IS_ERR(psCLK))
 572                 goto err1;
 573         psSysSpecData->psGPT11_FCK = psCLK;
 574
 575         psCLK = clk_get(NULL, "gpt11_ick");
 576         if (IS_ERR(psCLK))
 577                 goto err2;
 578         psSysSpecData->psGPT11_ICK = psCLK;
 579
 580         sys_ck = clk_get(NULL, "sys_ck");
 581         if (IS_ERR(sys_ck))
 582                 goto err3;
 583         if (clk_get_parent(psSysSpecData->psGPT11_FCK) != sys_ck)
 584                 if (clk_set_parent(psSysSpecData->psGPT11_FCK, sys_ck) < 0) {
 585                         clk_put(sys_ck);
 586                         goto err3;
 587                 }
 588         clk_put(sys_ck);
 589
 590         PVR_TRACE("GPTIMER11 clock is %dMHz",
 591                    HZ_TO_MHZ(clk_get_rate(psSysSpecData->psGPT11_FCK)));
 592
 593         psSysSpecData->gpt_base = ioremap(SYS_OMAP3430_GP11TIMER_PHYS_BASE,
 594                                           SYS_OMAP3430_GPTIMER_SIZE);
 595         if (!psSysSpecData->gpt_base)
 596                 goto err3;
 597
 598         clk_enable(psSysSpecData->psGPT11_ICK);
 599         clk_enable(psSysSpecData->psGPT11_FCK);
 600
 601         rate = gpt_read_reg(psSysData, SYS_OMAP3430_GPTIMER_TSICR);
 602         if (!(rate & 4)) {
 603                 PVR_TRACE("Setting GPTIMER11 mode to posted "
 604                           "(currently is non-posted)");
 605                 gpt_write_reg(psSysData, SYS_OMAP3430_GPTIMER_TSICR, rate | 4);
 606         }
 607
 608         clk_disable(psSysSpecData->psGPT11_FCK);
 609         clk_disable(psSysSpecData->psGPT11_ICK);
 610
 611         return PVRSRV_OK;
 612
 613 err3:
 614         clk_put(psSysSpecData->psGPT11_ICK);
 615 err2:
 616         clk_put(psSysSpecData->psGPT11_FCK);
 617 err1:
 618         clk_put(psSysSpecData->psMPU_CK);
 619 err0:
 620         PVR_DPF(PVR_DBG_ERROR,
 621                  "%s: couldn't init clocks: mpu %p sys %p fck %p ick %p",
 622                  __func__, psSysSpecData->psMPU_CK, sys_ck,
 623                  psSysSpecData->psGPT11_FCK, psSysSpecData->psGPT11_ICK);
 624
 625         psSysSpecData->psMPU_CK = NULL;
 626         psSysSpecData->psGPT11_FCK = NULL;
 627         psSysSpecData->psGPT11_ICK = NULL;
 628
 629         return PVRSRV_ERROR_GENERIC;
 630 }
 631
 632 static void CleanupDebugClocks(struct SYS_DATA *psSysData)
 633 {
 634         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 635
 636         if (psSysSpecData->psMPU_CK) {
 637                 clk_put(psSysSpecData->psMPU_CK);
 638                 psSysSpecData->psMPU_CK = NULL;
 639         }
 640         if (psSysSpecData->psGPT11_FCK) {
 641                 clk_put(psSysSpecData->psGPT11_FCK);
 642                 psSysSpecData->psGPT11_FCK = NULL;
 643         }
 644         if (psSysSpecData->psGPT11_ICK) {
 645                 clk_put(psSysSpecData->psGPT11_ICK);
 646                 psSysSpecData->psGPT11_ICK = NULL;
 647         }
 648 }
 649
 650 static enum PVRSRV_ERROR EnableDebugClocks(struct SYS_DATA *psSysData)
 651 {
 652         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 653
 654         if (clk_enable(psSysSpecData->psGPT11_FCK) < 0)
 655                 goto err0;
 656
 657         if (clk_enable(psSysSpecData->psGPT11_ICK) < 0)
 658                 goto err1;
 659
 660         gpt_write_reg(psSysData, SYS_OMAP3430_GPTIMER_ENABLE, 3);
 661
 662         return PVRSRV_OK;
 663
 664 err1:
 665         clk_disable(psSysSpecData->psGPT11_FCK);
 666 err0:
 667         PVR_DPF(PVR_DBG_ERROR, "%s: can't enable clocks", __func__);
 668
 669         return PVRSRV_ERROR_GENERIC;
 670 }
 671
 672 static inline void DisableDebugClocks(struct SYS_DATA *psSysData)
 673 {
 674         struct SYS_SPECIFIC_DATA *psSysSpecData = psSysData->pvSysSpecificData;
 675
 676         gpt_write_reg(psSysData, SYS_OMAP3430_GPTIMER_ENABLE, 0);
 677
 678         clk_disable(psSysSpecData->psGPT11_ICK);
 679         clk_disable(psSysSpecData->psGPT11_FCK);
 680 }
 681
 682 #else
 683
 684 inline enum PVRSRV_ERROR InitDebugClocks(struct SYS_DATA *psSysData)
 685 {
 686         return PVRSRV_OK;
 687 }
 688
 689 static inline void CleanupDebugClocks(struct SYS_DATA *psSysData)
 690 {
 691 }
 692
 693 static inline enum PVRSRV_ERROR EnableDebugClocks(struct SYS_DATA *psSysData)
 694 {
 695         return PVRSRV_OK;
 696 }
 697
 698 static inline void DisableDebugClocks(struct SYS_DATA *psSysData)
 699 {
 700 }
 701 #endif
 702
 703 enum PVRSRV_ERROR InitSystemClocks(struct SYS_DATA *psSysData)
 704 {
 705         if (InitSgxClocks(psSysData) != PVRSRV_OK)
 706                 goto err0;
 707
 708         if (InitDebugClocks(psSysData) != PVRSRV_OK)
 709                 goto err1;
 710
 711         return PVRSRV_OK;
 712
 713 err1:
 714         CleanupSgxClocks(psSysData);
 715 err0:
 716         return PVRSRV_ERROR_GENERIC;
 717 }
 718
 719 void CleanupSystemClocks(struct SYS_DATA *psSysData)
 720 {
 721         CleanupDebugClocks(psSysData);
 722         CleanupSgxClocks(psSysData);
 723 }
 724
 725 enum PVRSRV_ERROR EnableSystemClocks(struct SYS_DATA *psSysData)
 726 {
 727         PVR_TRACE("EnableSystemClocks: Enabling System Clocks");
 728
 729         /*
 730          * We force clocks on by increasing their refcount here during
 731          * module init time and decreasing it at cleanup time.
 732          */
 733         if (force_clocks_on())
 734                 sgx_force_enable_clocks(gpsSysData);
 735         if (EnableDebugClocks(psSysData) != PVRSRV_OK)
 736                 goto err1;
 737
 738         return PVRSRV_OK;
 739
 740 err1:
 741         return PVRSRV_ERROR_GENERIC;
 742 }
 743
 744 void DisableSystemClocks(struct SYS_DATA *psSysData)
 745 {
 746         PVR_TRACE("DisableSystemClocks: Disabling System Clocks");
 747
 748         DisableDebugClocks(psSysData);
 749         /* Decrease the clocks' refcount that was increased at init time. */
 750         if (force_clocks_on())
 751                 sgx_force_disable_clocks(gpsSysData);
 752 }