drivers/mtk_wcn_combo/common/core/psm_core.c

   1 #include "osal_typedef.h"
   2 #include "osal.h"
   3 #include "psm_core.h"
   4 #include "stp_core.h"
   5 #include <mach/mtk_wcn_cmb_stub.h>
   6
   7 INT32         gPsmDbgLevel = STP_PSM_LOG_INFO;
   8 MTKSTP_PSM_T  stp_psm_i;
   9 MTKSTP_PSM_T  *stp_psm = &stp_psm_i;
  10
  11 #define STP_PSM_LOUD_FUNC(fmt, arg...)   if(gPsmDbgLevel >= STP_PSM_LOG_LOUD){ osal_dbg_print(PFX_PSM "%s: "  fmt, __FUNCTION__ ,##arg);}
  12 #define STP_PSM_DBG_FUNC(fmt, arg...)    if(gPsmDbgLevel >= STP_PSM_LOG_DBG){  osal_dbg_print(PFX_PSM "%s: "  fmt, __FUNCTION__ ,##arg);}
  13 #define STP_PSM_INFO_FUNC(fmt, arg...)   if(gPsmDbgLevel >= STP_PSM_LOG_INFO){ osal_dbg_print(PFX_PSM "[I]%s: "  fmt, __FUNCTION__ ,##arg);}
  14 #define STP_PSM_WARN_FUNC(fmt, arg...)   if(gPsmDbgLevel >= STP_PSM_LOG_WARN){ osal_dbg_print(PFX_PSM "[W]%s: "  fmt, __FUNCTION__ ,##arg);}
  15 #define STP_PSM_ERR_FUNC(fmt, arg...)    if(gPsmDbgLevel >= STP_PSM_LOG_ERR){  osal_dbg_print(PFX_PSM "[E]%s(%d):ERROR! "   fmt, __FUNCTION__ , __LINE__, ##arg);}
  16 #define STP_PSM_TRC_FUNC(f)              if(gPsmDbgLevel >= STP_PSM_LOG_DBG){  osal_dbg_print(PFX_PSM "<%s> <%d>\n", __FUNCTION__, __LINE__);}
  17
  18 static inline INT32 _stp_psm_notify_wmt(MTKSTP_PSM_T *stp_psm, const MTKSTP_PSM_ACTION_T action);
  19 static INT32  _stp_psm_thread_lock_aquire(MTKSTP_PSM_T *stp_psm);
  20 static INT32  _stp_psm_thread_lock_release(MTKSTP_PSM_T *stp_psm);
  21
  22 static const PCHAR g_psm_state[STP_PSM_MAX_STATE] = {
  23     "ACT",
  24     "ACT_INACT",
  25     "INACT",
  26     "INACT_ACT"
  27 };
  28
  29 static const PCHAR g_psm_action[STP_PSM_MAX_ACTION] = {
  30     "SLEEP" ,
  31     "HOST_AWAKE",
  32     "WAKEUP",
  33     "EIRQ",
  34     "ROLL_BACK"
  35 };
  36
  37 static const PCHAR g_psm_op_name[STP_OPID_PSM_NUM] = {
  38     "STP_OPID_PSM_SLEEP",
  39     "STP_OPID_PSM_WAKEUP",
  40     "STP_OPID_PSM_HOST_AWAKE",
  41     "STP_OPID_PSM_EXIT"
  42 };
  43
  44 INT32 _stp_psm_release_data(MTKSTP_PSM_T *stp_psm);
  45
  46 static inline INT32  _stp_psm_get_state(MTKSTP_PSM_T *stp_psm);
  47
  48 static INT32 _stp_psm_is_redundant_active_op(
  49     P_OSAL_OP pOp,
  50     P_OSAL_OP_Q pOpQ
  51 );
  52
  53 static INT32 _stp_psm_clean_up_redundant_active_op(
  54         P_OSAL_OP_Q pOpQ
  55     );
  56 static MTK_WCN_BOOL _stp_psm_is_quick_ps_support (VOID);
  57 extern void mt_combo_plt_enter_deep_idle (
  58     COMBO_IF src
  59     ) ;
  60
  61 extern void mt_combo_plt_exit_deep_idle (
  62     COMBO_IF src
  63     ) ;
  64 MTK_WCN_BOOL mtk_wcn_stp_psm_dbg_level(UINT32 dbglevel)
  65 {
  66    if (0 <= dbglevel && dbglevel <= 4)
  67    {
  68       gPsmDbgLevel = dbglevel;
  69       STP_PSM_INFO_FUNC("gPsmDbgLevel = %d\n", gPsmDbgLevel);
  70       return true;
  71    }
  72    else
  73    {
  74       STP_PSM_INFO_FUNC("invalid psm debug level. gPsmDbgLevel = %d\n", gPsmDbgLevel);
  75    }
  76    return false;
  77 }
  78
  79 /* change from macro to static function to enforce type checking on parameters. */
  80 static INT32 psm_fifo_lock_init (MTKSTP_PSM_T *psm)
  81 {
  82
  83
  84 #if CFG_PSM_CORE_FIFO_SPIN_LOCK
  85         #if defined(CONFIG_PROVE_LOCKING)
  86                 osal_unsleepable_lock_init(&(psm->hold_fifo_lock));
  87                 return 0;
  88         #else
  89         return osal_unsleepable_lock_init(&(psm->hold_fifo_lock));
  90         #endif
  91 #else
  92 #if defined(CONFIG_PROVE_LOCKING)
  93         osal_sleepable_lock_init(&(psm->hold_fifo_lock));
  94         return 0;
  95 #else
  96     return osal_sleepable_lock_init(&(psm->hold_fifo_lock));
  97 #endif
  98 #endif
  99 }
 100
 101 static INT32 psm_fifo_lock_deinit (MTKSTP_PSM_T *psm)
 102 {
 103 #if CFG_PSM_CORE_FIFO_SPIN_LOCK
 104     return osal_unsleepable_lock_deinit(&(psm->hold_fifo_lock));
 105 #else
 106     return osal_sleepable_lock_deinit(&(psm->hold_fifo_lock));
 107 #endif
 108 }
 109
 110 static INT32 psm_fifo_lock (MTKSTP_PSM_T *psm)
 111 {
 112
 113
 114 #if CFG_PSM_CORE_FIFO_SPIN_LOCK
 115     return osal_lock_unsleepable_lock(&(psm->hold_fifo_lock));
 116 #else
 117     return osal_lock_sleepable_lock(&(psm->hold_fifo_lock));
 118 #endif
 119 }
 120
 121 static INT32 psm_fifo_unlock (MTKSTP_PSM_T *psm)
 122 {
 123
 124
 125 #if CFG_PSM_CORE_FIFO_SPIN_LOCK
 126     return osal_unlock_unsleepable_lock(&(psm->hold_fifo_lock));
 127 #else
 128     return osal_unlock_sleepable_lock(&(psm->hold_fifo_lock));
 129 #endif
 130 }
 131
 132 static INT32 _stp_psm_handler(MTKSTP_PSM_T *stp_psm, P_STP_OP pStpOp)
 133 {
 134     INT32 ret = -1;
 135
 136     //if (NULL == pStpOp)
 137     //{
 138     //    return -1;
 139     //}
 140     ret = _stp_psm_thread_lock_aquire(stp_psm);
 141     if (ret) {
 142         STP_PSM_ERR_FUNC("--->lock psm_thread_lock failed ret=%d\n", ret);
 143         return ret;
 144     }
 145
 146     switch(pStpOp->opId)
 147     {
 148         case STP_OPID_PSM_EXIT:
 149             // TODO: clean all up?
 150             ret = 0;
 151             break;
 152
 153         case STP_OPID_PSM_SLEEP:
 154             if (stp_psm_check_sleep_enable(stp_psm) > 0) {
 155             ret =_stp_psm_notify_wmt(stp_psm, SLEEP);
 156             } else {
 157                 STP_PSM_INFO_FUNC("cancel sleep request\n");
 158             }
 159             break;
 160
 161         case STP_OPID_PSM_WAKEUP:
 162            ret = _stp_psm_notify_wmt(stp_psm, WAKEUP);
 163            break;
 164
 165         case STP_OPID_PSM_HOST_AWAKE:
 166            ret = _stp_psm_notify_wmt(stp_psm, HOST_AWAKE);
 167            break;
 168
 169         default:
 170            STP_PSM_ERR_FUNC("invalid operation id (%d)\n", pStpOp->opId);
 171            ret = -1;
 172            break;
 173     }
 174     _stp_psm_thread_lock_release(stp_psm);
 175     return ret;
 176 }
 177
 178 static P_OSAL_OP _stp_psm_get_op (
 179     MTKSTP_PSM_T *stp_psm,
 180     P_OSAL_OP_Q pOpQ
 181     )
 182 {
 183     P_OSAL_OP pOp;
 184
 185     if (!pOpQ)
 186     {
 187         STP_PSM_WARN_FUNC("pOpQ == NULL\n");
 188         return NULL;
 189     }
 190
 191     osal_lock_unsleepable_lock(&(stp_psm->wq_spinlock));
 192     /* acquire lock success */
 193     RB_GET(pOpQ, pOp);
 194
 195     if((pOpQ == &stp_psm->rActiveOpQ) && (NULL != pOp))
 196     {
 197         //stp_psm->current_active_op = pOp;//*(pOp);
 198         stp_psm->last_active_opId = pOp->op.opId;
 199     }
 200     osal_unlock_unsleepable_lock(&(stp_psm->wq_spinlock));
 201
 202     if((pOpQ == &stp_psm->rActiveOpQ) && (NULL != pOp))
 203     {
 204         STP_PSM_DBG_FUNC("last_active_opId(%d)\n", stp_psm->last_active_opId);
 205     }
 206
 207     if (!pOp)
 208     {
 209         STP_PSM_WARN_FUNC("RB_GET fail\n");
 210     }
 211
 212     return pOp;
 213 }
 214
 215 static INT32 _stp_psm_dump_active_q(
 216     P_OSAL_OP_Q pOpQ
 217 )
 218 {
 219     UINT32 read_idx;
 220     UINT32 write_idx;
 221     UINT32 opId;
 222
 223     if(pOpQ == &stp_psm->rActiveOpQ)
 224     {
 225         read_idx = stp_psm->rActiveOpQ.read;
 226         write_idx = stp_psm->rActiveOpQ.write;
 227
 228         STP_PSM_DBG_FUNC("Active op list:++\n");
 229         while ((read_idx & RB_MASK(pOpQ)) != (write_idx & RB_MASK(pOpQ)))
 230         {
 231             opId = pOpQ->queue[read_idx & RB_MASK(pOpQ)]->op.opId;
 232             if(opId < STP_OPID_PSM_NUM)
 233             {
 234                 STP_PSM_DBG_FUNC("%s\n", g_psm_op_name[opId] );
 235             }
 236             else
 237             {
 238                 STP_PSM_WARN_FUNC("Unkown OP Id\n");
 239             }
 240             ++read_idx;
 241         }
 242         STP_PSM_DBG_FUNC("Active op list:--\n");
 243     }
 244     else
 245     {
 246         STP_PSM_DBG_FUNC("%s: not active queue, dont dump\n", __func__);
 247     }
 248
 249     return 0;
 250 }
 251
 252 static INT32 _stp_psm_is_redundant_active_op(
 253     P_OSAL_OP pOp,
 254     P_OSAL_OP_Q pOpQ
 255 )
 256 {
 257     UINT32 opId = 0;
 258     UINT32 prev_opId = 0;
 259
 260     //if((pOpQ == &stp_psm->rActiveOpQ) && (NULL != stp_psm->current_active_op))
 261     if((pOpQ == &stp_psm->rActiveOpQ) && (STP_OPID_PSM_INALID != stp_psm->last_active_opId))
 262
 263     {
 264         opId =  pOp->op.opId;
 265
 266         if(opId == STP_OPID_PSM_SLEEP)
 267         {
 268             if(RB_EMPTY(pOpQ))
 269             {
 270                 //prev_opId = stp_psm->current_active_op->op.opId;
 271                 prev_opId = stp_psm->last_active_opId;
 272             }
 273             else
 274             {
 275                 prev_opId = pOpQ->queue[(pOpQ->write - 1) & RB_MASK(pOpQ)]->op.opId;
 276             }
 277
 278             if(prev_opId == STP_OPID_PSM_SLEEP)
 279             {
 280                 STP_PSM_DBG_FUNC("redundant sleep opId found\n");
 281                 return 1;
 282             }
 283             else
 284             {
 285                 return 0;
 286             }
 287         }
 288         else
 289         {
 290             if(RB_EMPTY(pOpQ))
 291             {
 292                 //prev_opId = stp_psm->current_active_op->op.opId;
 293                 prev_opId = stp_psm->last_active_opId;
 294             }
 295             else
 296             {
 297                 prev_opId = pOpQ->queue[(pOpQ->write - 1) & RB_MASK(pOpQ)]->op.opId;
 298             }
 299
 300             if(((opId== STP_OPID_PSM_WAKEUP) && ( prev_opId == STP_OPID_PSM_WAKEUP)) ||
 301                         ((opId == STP_OPID_PSM_HOST_AWAKE) && ( prev_opId == STP_OPID_PSM_WAKEUP)) ||
 302                             ((opId == STP_OPID_PSM_HOST_AWAKE) && ( prev_opId == STP_OPID_PSM_HOST_AWAKE)) ||
 303                                 ((opId == STP_OPID_PSM_WAKEUP) && ( prev_opId == STP_OPID_PSM_HOST_AWAKE))
 304                     )
 305             {
 306                 STP_PSM_DBG_FUNC("redundant opId found, opId(%d), preOpid(%d)\n", opId, prev_opId);
 307                 return 1;
 308             }
 309             else
 310             {
 311                 return 0;
 312             }
 313         }
 314     }
 315     else
 316     {
 317         return 0;
 318     }
 319
 320 }
 321
 322 static INT32 _stp_psm_clean_up_redundant_active_op(
 323         P_OSAL_OP_Q pOpQ
 324     )
 325 {
 326     UINT32 prev_opId = 0;
 327     UINT32 prev_prev_opId = 0;
 328
 329     P_OSAL_OP pOp;
 330     P_OSAL_OP_Q pFreeOpQ= &stp_psm->rFreeOpQ;
 331
 332     if(pOpQ == &stp_psm->rActiveOpQ)
 333     {
 334         // sleep , wakeup | sleep, --> null | sleep (x)
 335         // wakeup , sleep , wakeup | sleep --> wakeup | sleep (v)
 336         // sleep , wakeup , sleep | wakeup --> sleep | wakeup (v)
 337         // xxx, sleep | sleep --> xxx, sleep  (v)
 338         // xxx, wakeup | wakeup --> xxx, wakeup  (v)
 339         // xxx, awake | awake --> xxx, awake  (v) --> should never happen
 340         while(RB_COUNT(pOpQ) > 2)
 341         {
 342             prev_opId = pOpQ->queue[(pOpQ->write - 1) & RB_MASK(pOpQ)]->op.opId;
 343             prev_prev_opId = pOpQ->queue[(pOpQ->write - 2) & RB_MASK(pOpQ)]->op.opId;
 344
 345             if((prev_opId == STP_OPID_PSM_SLEEP && prev_prev_opId == STP_OPID_PSM_WAKEUP)||
 346                 (prev_opId == STP_OPID_PSM_SLEEP && prev_prev_opId == STP_OPID_PSM_HOST_AWAKE) ||
 347                 (prev_opId == STP_OPID_PSM_WAKEUP&& prev_prev_opId == STP_OPID_PSM_SLEEP) ||
 348                 (prev_opId == STP_OPID_PSM_HOST_AWAKE&& prev_prev_opId == STP_OPID_PSM_SLEEP)
 349             )
 350             {
 351                 RB_GET(pOpQ, pOp);
 352                 RB_PUT(pFreeOpQ, pOp);
 353                 RB_GET(pOpQ, pOp);
 354                 RB_PUT(pFreeOpQ, pOp);
 355             }
 356                         else if (prev_opId == prev_prev_opId)
 357                         {
 358                             RB_GET(pOpQ, pOp);
 359                                 STP_PSM_DBG_FUNC("redundant opId(%d) found, remove it\n", pOp->op.opId);
 360                 RB_PUT(pFreeOpQ, pOp);
 361                         }
 362         }
 363     }
 364
 365     return 0;
 366 }
 367
 368 static INT32 _stp_psm_put_op (
 369     MTKSTP_PSM_T *stp_psm,
 370     P_OSAL_OP_Q pOpQ,
 371     P_OSAL_OP pOp
 372     )
 373 {
 374    INT32 ret;
 375
 376    // if (!pOpQ || !pOp)
 377    // {
 378    //     STP_PSM_WARN_FUNC("pOpQ = 0x%p, pLxOp = 0x%p \n", pOpQ, pOp);
 379    //     return 0;
 380    // }
 381     ret = 0;
 382
 383     osal_lock_unsleepable_lock(&(stp_psm->wq_spinlock));
 384     /* acquire lock success */
 385     if(pOpQ == &stp_psm->rActiveOpQ)
 386     {
 387         if(!_stp_psm_is_redundant_active_op(pOp, pOpQ))
 388         {
 389             /* acquire lock success */
 390             if (!RB_FULL(pOpQ))
 391             {
 392                 RB_PUT(pOpQ, pOp);
 393                 STP_PSM_DBG_FUNC("opId(%d) enqueue\n", pOp->op.opId);
 394             }
 395             else
 396             {
 397                 STP_PSM_INFO_FUNC("************ Active Queue Full ************\n");
 398                 ret = -1;
 399             }
 400
 401             _stp_psm_clean_up_redundant_active_op(pOpQ);
 402         }
 403         else
 404         {
 405             /*redundant opId, mark ret as success*/
 406             P_OSAL_OP_Q pFreeOpQ = &stp_psm->rFreeOpQ;
 407             if (!RB_FULL(pFreeOpQ))
 408             {
 409                 RB_PUT(pFreeOpQ, pOp);
 410             }
 411             else
 412             {
 413                 osal_assert(!RB_FULL(pFreeOpQ));
 414             }
 415             ret = 0;
 416         }
 417     }
 418     else
 419     {
 420         if (!RB_FULL(pOpQ))
 421         {
 422             RB_PUT(pOpQ, pOp);
 423         }
 424         else
 425         {
 426             ret = -1;
 427         }
 428     }
 429
 430     if(pOpQ == &stp_psm->rActiveOpQ)
 431     {
 432         _stp_psm_dump_active_q(&stp_psm->rActiveOpQ);
 433     }
 434
 435     osal_unlock_unsleepable_lock(&(stp_psm->wq_spinlock));
 436
 437     if (ret)
 438     {
 439         STP_PSM_WARN_FUNC("RB_FULL, RB_COUNT=%d , RB_SIZE=%d\n",RB_COUNT(pOpQ), RB_SIZE(pOpQ));
 440         return 0;
 441     }
 442     else
 443     {
 444         return 1;
 445     }
 446 }
 447
 448 P_OSAL_OP _stp_psm_get_free_op (
 449     MTKSTP_PSM_T *stp_psm
 450     )
 451 {
 452     P_OSAL_OP pOp;
 453
 454     if (stp_psm)
 455     {
 456         pOp = _stp_psm_get_op(stp_psm, &stp_psm->rFreeOpQ);
 457         if (pOp)
 458         {
 459             osal_memset(&pOp->op, 0, sizeof(pOp->op));
 460         }
 461         return pOp;
 462     }
 463     else
 464     {
 465         return NULL;
 466     }
 467 }
 468
 469 INT32 _stp_psm_put_act_op (
 470     MTKSTP_PSM_T *stp_psm,
 471     P_OSAL_OP pOp
 472     )
 473 {
 474     INT32 bRet = 0;//MTK_WCN_BOOL_FALSE;
 475     INT32 bCleanup = 0;//MTK_WCN_BOOL_FALSE;
 476     INT32 wait_ret = -1;
 477     P_OSAL_SIGNAL pSignal = NULL;
 478
 479     do
 480     {
 481         if (!stp_psm || !pOp)
 482         {
 483             STP_PSM_ERR_FUNC("stp_psm = %p, pOp = %p\n", stp_psm, pOp);
 484             break;
 485         }
 486
 487         pSignal = &pOp->signal;
 488
 489         if (pSignal->timeoutValue)
 490         {
 491             pOp->result = -9;
 492             osal_signal_init(&pOp->signal);
 493         }
 494
 495         /* put to active Q */
 496         bRet = _stp_psm_put_op(stp_psm, &stp_psm->rActiveOpQ, pOp);
 497
 498         if(0 == bRet)
 499         {
 500             STP_PSM_WARN_FUNC("+++++++++++ Put op Active queue Fail\n");
 501             bCleanup = 1;//MTK_WCN_BOOL_TRUE;
 502             break;
 503         }
 504
 505         /* wake up wmtd */
 506         osal_trigger_event(&stp_psm->STPd_event);
 507
 508         if (pSignal->timeoutValue == 0)
 509         {
 510             bRet = 1;//MTK_WCN_BOOL_TRUE;
 511             /* clean it in wmtd */
 512             break;
 513         }
 514
 515         /* wait result, clean it here */
 516         bCleanup = 1;//MTK_WCN_BOOL_TRUE;
 517
 518         /* check result */
 519         wait_ret = osal_wait_for_signal_timeout(&pOp->signal);
 520         STP_PSM_DBG_FUNC("wait completion:%d\n", wait_ret);
 521         if (!wait_ret)
 522         {
 523             STP_PSM_ERR_FUNC("wait completion timeout \n");
 524             // TODO: how to handle it? retry?
 525         }
 526         else
 527         {
 528             if (pOp->result)
 529             {
 530                 STP_PSM_WARN_FUNC("op(%d) result:%d\n", pOp->op.opId, pOp->result);
 531             }
 532             /* op completes, check result */
 533             bRet = (pOp->result) ? 0 : 1;
 534         }
 535     } while(0);
 536
 537     if (bCleanup) {
 538         /* put Op back to freeQ */
 539         bRet = _stp_psm_put_op(stp_psm, &stp_psm->rFreeOpQ, pOp);
 540         if(bRet == 0)
 541         {
 542             STP_PSM_WARN_FUNC("+++++++++++ Put op active free fail, maybe disable/enable psm\n");
 543         }
 544     }
 545
 546     return bRet;
 547 }
 548
 549 static INT32 _stp_psm_wait_for_msg(void *pvData)
 550 {
 551     MTKSTP_PSM_T *stp_psm = (MTKSTP_PSM_T *)pvData;
 552     STP_PSM_DBG_FUNC("%s: stp_psm->rActiveOpQ = %d\n", __func__, RB_COUNT(&stp_psm->rActiveOpQ));
 553
 554     return ((!RB_EMPTY(&stp_psm->rActiveOpQ)) || osal_thread_should_stop(&stp_psm->PSMd));
 555 }
 556
 557 static INT32 _stp_psm_proc (void *pvData)
 558 {
 559     MTKSTP_PSM_T *stp_psm = (MTKSTP_PSM_T *)pvData;
 560     P_OSAL_OP pOp;
 561     UINT32 id;
 562     INT32 result;
 563
 564     if (!stp_psm) {
 565         STP_PSM_WARN_FUNC("!stp_psm \n");
 566         return -1;
 567     }
 568
 569 //    STP_PSM_INFO_FUNC("wmtd starts running: pWmtDev(0x%p) [pol, rt_pri, n_pri, pri]=[%d, %d, %d, %d] \n",
 570 //        stp_psm, current->policy, current->rt_priority, current->normal_prio, current->prio);
 571
 572     for (;;) {
 573
 574         pOp = NULL;
 575
 576         osal_wait_for_event(&stp_psm->STPd_event,
 577             _stp_psm_wait_for_msg,
 578             (void *)stp_psm);
 579
 580         //we set reset flag when calling stp_reset after cleanup all op.
 581         if(stp_psm->flag & STP_PSM_RESET_EN)
 582         {
 583             stp_psm->flag &= ~STP_PSM_RESET_EN;
 584         }
 585
 586         if (osal_thread_should_stop(&stp_psm->PSMd))
 587         {
 588             STP_PSM_INFO_FUNC("should stop now... \n");
 589             // TODO: clean up active opQ
 590             break;
 591         }
 592
 593         /* get Op from activeQ */
 594         pOp = _stp_psm_get_op(stp_psm, &stp_psm->rActiveOpQ);
 595         if (!pOp)
 596         {
 597             STP_PSM_WARN_FUNC("+++++++++++ Get op from activeQ fail, maybe disable/enable psm\n");
 598             continue;
 599         }
 600
 601         id = osal_op_get_id(pOp);
 602
 603         if (id >= STP_OPID_PSM_NUM)
 604         {
 605             STP_PSM_WARN_FUNC("abnormal opid id: 0x%x \n", id);
 606             result = -1;
 607             goto handler_done;
 608         }
 609
 610         result = _stp_psm_handler(stp_psm, &pOp->op);
 611
 612 handler_done:
 613
 614         if (result)
 615         {
 616             STP_PSM_WARN_FUNC("opid id(0x%x)(%s) error(%d)\n", id, (id >= 4)?("???"):(g_psm_op_name[id]), result);
 617         }
 618
 619         if (osal_op_is_wait_for_signal(pOp))
 620         {
 621             osal_op_raise_signal(pOp, result);
 622         }
 623         else
 624         {
 625            /* put Op back to freeQ */
 626            if(_stp_psm_put_op(stp_psm, &stp_psm->rFreeOpQ, pOp) == 0)
 627            {
 628                 STP_PSM_WARN_FUNC("+++++++++++ Put op to FreeOpQ fail, maybe disable/enable psm\n");
 629            }
 630         }
 631
 632         if (STP_OPID_PSM_EXIT == id)
 633         {
 634             break;
 635         }
 636     }
 637     STP_PSM_INFO_FUNC("exits \n");
 638
 639     return 0;
 640 };
 641
 642 static inline INT32 _stp_psm_get_time(void)
 643 {
 644     if(gPsmDbgLevel >= STP_PSM_LOG_LOUD)
 645     {
 646         osal_printtimeofday("<psm time>>>>");
 647     }
 648
 649     return 0;
 650 }
 651
 652 static inline INT32  _stp_psm_get_state(MTKSTP_PSM_T *stp_psm)
 653 {
 654
 655     if(stp_psm == NULL)
 656     {
 657         return STP_PSM_OPERATION_FAIL;
 658     }
 659     else
 660     {
 661         if(stp_psm->work_state < STP_PSM_MAX_STATE)
 662         {
 663             return stp_psm->work_state;
 664         }
 665         else
 666         {
 667             STP_PSM_ERR_FUNC("work_state = %d, invalid\n", stp_psm->work_state);
 668
 669             return -STP_PSM_OPERATION_FAIL;
 670         }
 671     }
 672 }
 673
 674 static inline INT32 _stp_psm_set_state(MTKSTP_PSM_T *stp_psm,const MTKSTP_PSM_STATE_T state)
 675 {
 676     if(stp_psm == NULL)
 677     {
 678         return STP_PSM_OPERATION_FAIL;
 679     }
 680     else
 681     {
 682          if(stp_psm->work_state < STP_PSM_MAX_STATE)
 683          {
 684             _stp_psm_get_time();
 685             //STP_PSM_INFO_FUNC("work_state = %s --> %s\n", g_psm_state[stp_psm->work_state], g_psm_state[state]);
 686
 687             stp_psm->work_state = state;
 688             if(stp_psm->work_state != ACT)
 689             {
 690 //                osal_lock_unsleepable_lock(&stp_psm->flagSpinlock);
 691                 stp_psm->flag |= STP_PSM_BLOCK_DATA_EN;
 692 //                osal_unlock_unsleepable_lock(&stp_psm->flagSpinlock);
 693             }
 694          }
 695          else
 696          {
 697             STP_PSM_ERR_FUNC("work_state = %d, invalid\n", stp_psm->work_state);
 698          }
 699     }
 700
 701     return STP_PSM_OPERATION_SUCCESS;
 702 }
 703
 704 static inline INT32 _stp_psm_start_monitor(MTKSTP_PSM_T *stp_psm)
 705 {
 706
 707      if(!stp_psm)
 708      {
 709         return STP_PSM_OPERATION_FAIL;
 710      }
 711
 712      if((stp_psm->flag & STP_PSM_WMT_EVENT_DISABLE_MONITOR)!= 0)
 713      {
 714         STP_PSM_DBG_FUNC("STP-PSM DISABLE, DONT restart monitor!\n\r");
 715         return STP_PSM_OPERATION_SUCCESS;
 716      }
 717
 718
 719      STP_PSM_LOUD_FUNC("start monitor\n");
 720      osal_timer_modify(&stp_psm->psm_timer, stp_psm->idle_time_to_sleep);
 721
 722      return STP_PSM_OPERATION_SUCCESS;
 723 }
 724
 725 static inline INT32 _stp_psm_stop_monitor(MTKSTP_PSM_T *stp_psm)
 726 {
 727
 728     if(!stp_psm)
 729     {
 730         return STP_PSM_OPERATION_FAIL;
 731     }
 732     else
 733     {
 734         STP_PSM_DBG_FUNC("stop monitor\n");
 735         osal_timer_stop_sync(&stp_psm->psm_timer);
 736     }
 737
 738     return STP_PSM_OPERATION_SUCCESS;
 739 }
 740
 741 INT32
 742 _stp_psm_hold_data (
 743     MTKSTP_PSM_T *stp_psm,
 744     const UINT8 *buffer,
 745     const UINT32 len,
 746     const UINT8 type
 747     )
 748 {
 749     INT32 available_space = 0;
 750     INT32 needed_space = 0;
 751     UINT8 delimiter [] = {0xbb, 0xbb};
 752
 753     if(!stp_psm)
 754     {
 755         return STP_PSM_OPERATION_FAIL;
 756     }
 757     else
 758     {
 759         psm_fifo_lock(stp_psm);
 760
 761         available_space = STP_PSM_FIFO_SIZE- osal_fifo_len(&stp_psm->hold_fifo);
 762         needed_space = len + sizeof(UINT8) + sizeof(UINT32) + 2;
 763
 764         //STP_PSM_INFO_FUNC("*******FIFO Available(%d), Need(%d)\n", available_space, needed_space);
 765
 766         if( available_space < needed_space )
 767         {
 768             STP_PSM_ERR_FUNC("FIFO Available!! Reset FIFO\n");
 769             osal_fifo_reset(&stp_psm->hold_fifo);
 770         }
 771         //type
 772         osal_fifo_in(&stp_psm->hold_fifo,(UINT8 *) &type , sizeof(UINT8));
 773         //lenght
 774         osal_fifo_in(&stp_psm->hold_fifo,(UINT8 *) &len , sizeof(UINT32));
 775         //buffer
 776         osal_fifo_in(&stp_psm->hold_fifo,(UINT8 *) buffer, len);
 777         //delimiter
 778         osal_fifo_in(&stp_psm->hold_fifo,(UINT8 *) delimiter, 2);
 779
 780         psm_fifo_unlock(stp_psm);
 781
 782         return len;
 783     }
 784 }
 785
 786 INT32 _stp_psm_has_pending_data(MTKSTP_PSM_T *stp_psm)
 787 {
 788     return osal_fifo_len(&stp_psm->hold_fifo);
 789 }
 790
 791 INT32 _stp_psm_release_data(MTKSTP_PSM_T *stp_psm)
 792 {
 793
 794     INT32 i = 20; /*Max buffered packet number*/
 795     INT32 ret = 0;
 796     UINT8 type = 0;
 797     UINT32  len = 0;
 798     UINT8 delimiter[2];
 799
 800     //STP_PSM_ERR_FUNC("++++++++++release data++len=%d\n", osal_fifo_len(&stp_psm->hold_fifo));
 801     while(osal_fifo_len(&stp_psm->hold_fifo) && i > 0)
 802     {
 803         //acquire spinlock
 804         psm_fifo_lock(stp_psm);
 805
 806         ret = osal_fifo_out(&stp_psm->hold_fifo, (UINT8 *)&type, sizeof(UINT8));
 807         ret = osal_fifo_out(&stp_psm->hold_fifo, (UINT8 *)&len, sizeof(UINT32));
 808
 809         if(len > STP_PSM_PACKET_SIZE_MAX)
 810         {
 811             STP_PSM_ERR_FUNC("***psm packet's length too Long!****\n");
 812             STP_PSM_INFO_FUNC("***reset psm's fifo***\n");
 813         }
 814         else
 815         {
 816             osal_memset(stp_psm->out_buf, 0, STP_PSM_TX_SIZE);
 817             ret = osal_fifo_out(&stp_psm->hold_fifo, (UINT8 *)stp_psm->out_buf, len);
 818         }
 819
 820         ret = osal_fifo_out(&stp_psm->hold_fifo, (UINT8 *)delimiter, 2);
 821
 822         if(delimiter[0]==0xbb && delimiter[1]==0xbb)
 823         {
 824              //osal_buffer_dump(stp_psm->out_buf, "psm->out_buf", len, 32);
 825              stp_send_data_no_ps(stp_psm->out_buf, len, type);
 826         }
 827         else
 828         {
 829             STP_PSM_ERR_FUNC("***psm packet fifo parsing fail****\n");
 830             STP_PSM_INFO_FUNC("***reset psm's fifo***\n");
 831
 832             osal_fifo_reset(&stp_psm->hold_fifo);
 833         }
 834         i--;
 835         psm_fifo_unlock(stp_psm);
 836     }
 837     return STP_PSM_OPERATION_SUCCESS;
 838 }
 839
 840 static inline INT32 _stp_psm_notify_wmt_host_awake_wq(MTKSTP_PSM_T *stp_psm)
 841 {
 842
 843     P_OSAL_OP  pOp;
 844     INT32      bRet;
 845     INT32      retval;
 846
 847     if(stp_psm == NULL)
 848     {
 849         return  STP_PSM_OPERATION_FAIL;
 850     }
 851     else
 852     {
 853         pOp = _stp_psm_get_free_op(stp_psm);
 854         if (!pOp)
 855         {
 856             STP_PSM_WARN_FUNC("get_free_lxop fail \n");
 857             return -1;//break;
 858         }
 859
 860         pOp->op.opId = STP_OPID_PSM_HOST_AWAKE;
 861         pOp->signal.timeoutValue = 0;
 862         bRet = _stp_psm_put_act_op(stp_psm, pOp);
 863
 864         STP_PSM_DBG_FUNC("OPID(%d) type(%d) bRet(%d) \n\n",
 865             pOp->op.opId,
 866             pOp->op.au4OpData[0],
 867             bRet);
 868
 869         retval = (0 == bRet) ? (STP_PSM_OPERATION_FAIL) : 0;
 870     }
 871     return retval;
 872 }
 873
 874 static inline INT32 _stp_psm_notify_wmt_wakeup_wq(MTKSTP_PSM_T *stp_psm)
 875 {
 876     P_OSAL_OP  pOp;
 877     INT32      bRet;
 878     INT32      retval;
 879
 880     if(stp_psm == NULL)
 881     {
 882         return (STP_PSM_OPERATION_FAIL);
 883     }
 884     else
 885     {
 886         pOp = _stp_psm_get_free_op(stp_psm);
 887         if (!pOp)
 888         {
 889             STP_PSM_WARN_FUNC("get_free_lxop fail \n");
 890             return -1;//break;
 891         }
 892
 893         pOp->op.opId = STP_OPID_PSM_WAKEUP;
 894         pOp->signal.timeoutValue = 0;
 895         bRet = _stp_psm_put_act_op(stp_psm, pOp);
 896         if (0 == bRet)
 897         {
 898             STP_PSM_WARN_FUNC("OPID(%d) type(%d) bRet(%s)\n\n",
 899                 pOp->op.opId,
 900                 pOp->op.au4OpData[0],
 901                 "fail");
 902         }
 903         retval = (0 == bRet) ? (STP_PSM_OPERATION_FAIL) : (STP_PSM_OPERATION_SUCCESS);
 904     }
 905     return retval;
 906 }
 907
 908 static inline INT32 _stp_psm_notify_wmt_sleep_wq(MTKSTP_PSM_T *stp_psm)
 909 {
 910     P_OSAL_OP       pOp;
 911     INT32           bRet;
 912     INT32 retval;
 913
 914     if(stp_psm == NULL)
 915     {
 916         return STP_PSM_OPERATION_FAIL;
 917     }
 918     else
 919     {
 920         if((stp_psm->flag & STP_PSM_WMT_EVENT_DISABLE_MONITOR_TX_HIGH_DENSITY) != 0)
 921         {
 922             return 0;
 923         }
 924
 925         if((stp_psm->flag & STP_PSM_WMT_EVENT_DISABLE_MONITOR_RX_HIGH_DENSITY) != 0)
 926         {
 927             return 0;
 928         }
 929
 930         if((stp_psm->flag & STP_PSM_WMT_EVENT_DISABLE_MONITOR) != 0)
 931         {
 932             return 0;
 933         }
 934
 935         pOp = _stp_psm_get_free_op(stp_psm);
 936         if (!pOp) {
 937             STP_PSM_WARN_FUNC("get_free_lxop fail \n");
 938             return -1;//break;
 939         }
 940
 941         pOp->op.opId = STP_OPID_PSM_SLEEP;
 942         pOp->signal.timeoutValue = 0;
 943         bRet = _stp_psm_put_act_op(stp_psm, pOp);
 944
 945         STP_PSM_DBG_FUNC("OPID(%d) type(%d) bRet(%d) \n\n",
 946             pOp->op.opId,
 947             pOp->op.au4OpData[0],
 948             bRet);
 949
 950         retval = (0 == bRet) ? (STP_PSM_OPERATION_FAIL) : 0;
 951     }
 952     return retval;
 953 }
 954
 955 /*internal function*/
 956
 957 static inline INT32 _stp_psm_reset(MTKSTP_PSM_T *stp_psm)
 958 {
 959     INT32 i = 0;
 960     P_OSAL_OP_Q pOpQ;
 961     P_OSAL_OP pOp;
 962     INT32 ret = 0;
 963
 964     STP_PSM_DBG_FUNC("PSM MODE RESET=============================>\n\r");
 965
 966     STP_PSM_INFO_FUNC("_stp_psm_reset\n");
 967     STP_PSM_INFO_FUNC("reset-wake_lock(%d)\n", osal_wake_lock_count(&stp_psm->wake_lock));
 968     osal_wake_unlock(&stp_psm->wake_lock);
 969     STP_PSM_INFO_FUNC("reset-wake_lock(%d)\n", osal_wake_lock_count(&stp_psm->wake_lock));
 970
 971     //--> serialized the request from wmt <--//
 972     ret = osal_lock_sleepable_lock(&stp_psm->user_lock);
 973     if (ret) {
 974         STP_PSM_ERR_FUNC("--->lock stp_psm->user_lock failed, ret=%d\n", ret);
 975         return ret;
 976     }
 977
 978     //--> disable psm <--//
 979     stp_psm->flag = STP_PSM_WMT_EVENT_DISABLE_MONITOR;
 980     _stp_psm_stop_monitor(stp_psm);
 981
 982     //--> prepare the op list <--//
 983     osal_lock_unsleepable_lock(&(stp_psm->wq_spinlock));
 984     RB_INIT(&stp_psm->rFreeOpQ, STP_OP_BUF_SIZE);
 985     RB_INIT(&stp_psm->rActiveOpQ, STP_OP_BUF_SIZE);
 986
 987     //stp_psm->current_active_op = NULL;
 988     stp_psm->last_active_opId = STP_OPID_PSM_INALID;
 989
 990     pOpQ = &stp_psm->rFreeOpQ;
 991     for (i = 0; i < STP_OP_BUF_SIZE; i++)
 992     {
 993         if (!RB_FULL(pOpQ))
 994         {
 995             pOp = &stp_psm->arQue[i];
 996             RB_PUT(pOpQ, pOp);
 997         }
 998     }
 999     osal_unlock_unsleepable_lock(&(stp_psm->wq_spinlock));
1000
1001     //--> clean up interal data structure<--//
1002     _stp_psm_set_state(stp_psm, ACT);
1003
1004     psm_fifo_lock(stp_psm);
1005     osal_fifo_reset(&stp_psm->hold_fifo);
1006     psm_fifo_unlock(stp_psm);
1007
1008     //--> stop psm thread wait <--//
1009     stp_psm->flag |= STP_PSM_RESET_EN;
1010     osal_trigger_event(&stp_psm->wait_wmt_q);
1011
1012     osal_unlock_sleepable_lock(&stp_psm->user_lock);
1013
1014     STP_PSM_DBG_FUNC("PSM MODE RESET<============================\n\r");
1015
1016     return STP_PSM_OPERATION_SUCCESS;
1017 }
1018
1019 static INT32 _stp_psm_wait_wmt_event(void *pvData)
1020 {
1021     MTKSTP_PSM_T *stp_psm = (MTKSTP_PSM_T *)pvData;
1022
1023     STP_PSM_DBG_FUNC("%s, stp_psm->flag=0x%08x\n", __func__, stp_psm->flag);
1024
1025     return ((stp_psm->flag & STP_PSM_WMT_EVENT_SLEEP_EN) ||
1026         (stp_psm->flag & STP_PSM_WMT_EVENT_WAKEUP_EN) ||
1027         (stp_psm->flag & STP_PSM_WMT_EVENT_ROLL_BACK_EN) ||
1028         (stp_psm->flag & STP_PSM_RESET_EN));
1029 }
1030
1031
1032 static inline INT32 _stp_psm_wait_wmt_event_wq(MTKSTP_PSM_T *stp_psm){
1033
1034     INT32 retval = 0;
1035
1036     if(stp_psm == NULL)
1037     {
1038         return STP_PSM_OPERATION_FAIL;
1039     }
1040     else
1041     {
1042         osal_wait_for_event_timeout(&stp_psm->wait_wmt_q,
1043              _stp_psm_wait_wmt_event,
1044              (void *)stp_psm);
1045
1046         if(stp_psm->flag & STP_PSM_WMT_EVENT_WAKEUP_EN)
1047         {
1048             stp_psm->flag &= ~STP_PSM_WMT_EVENT_WAKEUP_EN;
1049 //            osal_lock_unsleepable_lock(&stp_psm->flagSpinlock);
1050             //STP send data here: STP enqueue data to psm buffer.
1051             _stp_psm_release_data(stp_psm);
1052             //STP send data here: STP enqueue data to psm buffer. We release packet by the next one.
1053             stp_psm->flag &= ~STP_PSM_BLOCK_DATA_EN;
1054             //STP send data here: STP sends data directly without PSM.
1055             _stp_psm_set_state(stp_psm, ACT);
1056 //            osal_unlock_unsleepable_lock(&stp_psm->flagSpinlock);
1057
1058             if (stp_psm_is_quick_ps_support())
1059                 stp_psm_notify_wmt_sleep(stp_psm);
1060                         else
1061                             _stp_psm_start_monitor(stp_psm);
1062         }
1063         else if ( stp_psm->flag & STP_PSM_WMT_EVENT_SLEEP_EN)
1064         {
1065             stp_psm->flag &= ~STP_PSM_WMT_EVENT_SLEEP_EN;
1066             _stp_psm_set_state(stp_psm, INACT);
1067
1068             STP_PSM_DBG_FUNC("mt_combo_plt_enter_deep_idle++\n");
1069             mt_combo_plt_enter_deep_idle(COMBO_IF_UART);
1070             STP_PSM_DBG_FUNC("mt_combo_plt_enter_deep_idle--\n");
1071
1072             STP_PSM_DBG_FUNC("sleep-wake_lock(%d)\n", osal_wake_lock_count(&stp_psm->wake_lock));
1073             osal_wake_unlock(&stp_psm->wake_lock);
1074             STP_PSM_DBG_FUNC("sleep-wake_lock#(%d)\n", osal_wake_lock_count(&stp_psm->wake_lock));
1075         }
1076         else if ( stp_psm->flag & STP_PSM_WMT_EVENT_ROLL_BACK_EN)
1077         {
1078             stp_psm->flag &= ~STP_PSM_WMT_EVENT_ROLL_BACK_EN;
1079             if(_stp_psm_get_state(stp_psm) == ACT_INACT){
1080 //                osal_lock_unsleepable_lock(&stp_psm->flagSpinlock);
1081                 _stp_psm_release_data(stp_psm);
1082                 stp_psm->flag &= ~STP_PSM_BLOCK_DATA_EN;
1083                 _stp_psm_set_state(stp_psm, ACT);
1084 //                osal_unlock_unsleepable_lock(&stp_psm->flagSpinlock);
1085             } else if(_stp_psm_get_state(stp_psm) == INACT_ACT) {
1086                 _stp_psm_set_state(stp_psm, INACT);
1087                 STP_PSM_INFO_FUNC("[WARNING]PSM state rollback due too wakeup fail\n");
1088             }
1089         }
1090         else if (stp_psm->flag & STP_PSM_RESET_EN)
1091         {
1092             stp_psm->flag &= ~STP_PSM_WMT_EVENT_ROLL_BACK_EN;
1093         }
1094         else
1095         {
1096             STP_PSM_ERR_FUNC("flag = %x<== Abnormal flag be set!!\n\r", stp_psm->flag);
1097             STP_PSM_ERR_FUNC("state = %d, flag = %d\n", stp_psm->work_state, stp_psm->flag);
1098         }
1099         retval = STP_PSM_OPERATION_SUCCESS;
1100     }
1101     return retval;
1102 }
1103
1104 static inline INT32 _stp_psm_notify_stp(MTKSTP_PSM_T *stp_psm, const MTKSTP_PSM_ACTION_T action){
1105
1106     INT32 retval = 0;
1107
1108     if(action == EIRQ)
1109     {
1110         STP_PSM_DBG_FUNC("Call _stp_psm_notify_wmt_host_awake_wq\n\r");
1111
1112         _stp_psm_notify_wmt_host_awake_wq(stp_psm);
1113
1114         return STP_PSM_OPERATION_FAIL;
1115     }
1116
1117     if((_stp_psm_get_state(stp_psm) < STP_PSM_MAX_STATE) && (_stp_psm_get_state(stp_psm) >= 0))
1118     {
1119         STP_PSM_DBG_FUNC("state = %s, action=%s \n\r", g_psm_state[_stp_psm_get_state(stp_psm)], g_psm_action[action]);
1120     }
1121
1122     // If STP trigger WAKEUP and SLEEP, to do the job below
1123     switch(_stp_psm_get_state(stp_psm))
1124     {
1125             //stp trigger
1126         case ACT_INACT:
1127
1128             if(action == SLEEP)
1129             {
1130                 STP_PSM_LOUD_FUNC("Action = %s, ACT_INACT state, ready to INACT\n\r", g_psm_action[action]);
1131                 stp_psm->flag &= ~STP_PSM_WMT_EVENT_WAKEUP_EN;
1132                 stp_psm->flag |= STP_PSM_WMT_EVENT_SLEEP_EN;
1133
1134                 //wake_up(&stp_psm->wait_wmt_q);
1135                 osal_trigger_event(&stp_psm->wait_wmt_q);
1136             }
1137             else if(action == ROLL_BACK)
1138             {
1139                 STP_PSM_LOUD_FUNC("Action = %s, ACT_INACT state, back to ACT\n\r", g_psm_action[action]);
1140                 //stp_psm->flag &= ~STP_PSM_WMT_EVENT_ROLL_BACK_EN;
1141                 stp_psm->flag |=  STP_PSM_WMT_EVENT_ROLL_BACK_EN;
1142                 //wake_up(&stp_psm->wait_wmt_q);
1143                  osal_trigger_event(&stp_psm->wait_wmt_q);
1144             }
1145             else
1146             {
1147                 if(action < STP_PSM_MAX_ACTION)
1148                 {
1149                     STP_PSM_ERR_FUNC("Action = %s, ACT_INACT state, the case should not happens\n\r", g_psm_action[action]);
1150                     STP_PSM_ERR_FUNC("state = %d, flag = %d\n", stp_psm->work_state, stp_psm->flag);
1151                 }
1152                 else
1153                 {
1154                     STP_PSM_ERR_FUNC("Invalid Action!!\n\r");
1155                 }
1156                 retval = STP_PSM_OPERATION_FAIL;
1157             }
1158             break;
1159             //stp trigger
1160
1161         case INACT_ACT:
1162
1163             if(action == WAKEUP)
1164             {
1165                 STP_PSM_LOUD_FUNC("Action = %s, INACT_ACT state, ready to ACT\n\r", g_psm_action[action]);
1166                 stp_psm->flag &= ~STP_PSM_WMT_EVENT_SLEEP_EN;
1167                 stp_psm->flag |= STP_PSM_WMT_EVENT_WAKEUP_EN;
1168                 //wake_up(&stp_psm->wait_wmt_q);
1169                  osal_trigger_event(&stp_psm->wait_wmt_q);
1170             }
1171             else if(action == HOST_AWAKE)
1172             {
1173                 STP_PSM_LOUD_FUNC("Action = %s, INACT_ACT state, ready to ACT\n\r", g_psm_action[action]);
1174                 stp_psm->flag &= ~STP_PSM_WMT_EVENT_SLEEP_EN;
1175                 stp_psm->flag |= STP_PSM_WMT_EVENT_WAKEUP_EN;
1176                 //wake_up(&stp_psm->wait_wmt_q);
1177                  osal_trigger_event(&stp_psm->wait_wmt_q);
1178             }
1179             else if(action == ROLL_BACK)
1180             {
1181                 STP_PSM_LOUD_FUNC("Action = %s, INACT_ACT state, back to INACT\n\r", g_psm_action[action]);
1182                // stp_psm->flag &= ~STP_PSM_WMT_EVENT_ROLL_BACK_EN;
1183                 stp_psm->flag |=  STP_PSM_WMT_EVENT_ROLL_BACK_EN;
1184                 //wake_up(&stp_psm->wait_wmt_q);
1185                  osal_trigger_event(&stp_psm->wait_wmt_q);
1186             }
1187             else
1188             {
1189                 if(action < STP_PSM_MAX_ACTION)
1190                 {
1191                     STP_PSM_ERR_FUNC("Action = %s, INACT_ACT state, the case should not happens\n\r", g_psm_action[action]);
1192                     STP_PSM_ERR_FUNC("state = %d, flag = %d\n", stp_psm->work_state, stp_psm->flag);
1193                 }
1194                 else
1195                 {
1196                     STP_PSM_ERR_FUNC("Invalid Action!!\n\r");
1197                 }
1198                 retval = STP_PSM_OPERATION_FAIL;
1199             }
1200             break;
1201
1202         case INACT:
1203
1204             if(action < STP_PSM_MAX_ACTION)
1205             {
1206                 STP_PSM_ERR_FUNC("Action = %s, INACT state, the case should not happens\n\r", g_psm_action[action]);
1207                 STP_PSM_ERR_FUNC("state = %d, flag = %d\n", stp_psm->work_state, stp_psm->flag);
1208             }
1209             else
1210             {
1211                 STP_PSM_ERR_FUNC("Invalid Action!!\n\r");
1212             }
1213
1214             retval = -1;
1215
1216             break;
1217
1218         case ACT:
1219
1220             if(action < STP_PSM_MAX_ACTION)
1221             {
1222                 STP_PSM_ERR_FUNC("Action = %s, ACT state, the case should not happens\n\r", g_psm_action[action]);
1223                 STP_PSM_ERR_FUNC("state = %d, flag = %d\n", stp_psm->work_state, stp_psm->flag);
1224             }
1225             else
1226             {
1227                 STP_PSM_ERR_FUNC("Invalid Action!!\n\r");
1228             }
1229
1230             retval = STP_PSM_OPERATION_FAIL;
1231
1232             break;
1233
1234         default:
1235
1236             /*invalid*/
1237             if(action < STP_PSM_MAX_ACTION)
1238             {
1239                 STP_PSM_ERR_FUNC("Action = %s, Invalid state, the case should not happens\n\r", g_psm_action[action]);
1240                 STP_PSM_ERR_FUNC("state = %d, flag = %d\n", stp_psm->work_state, stp_psm->flag);
1241             }
1242             else
1243             {
1244                 STP_PSM_ERR_FUNC("Invalid Action!!\n\r");
1245             }
1246
1247             retval = STP_PSM_OPERATION_FAIL;
1248
1249             break;
1250     }
1251
1252     return retval;
1253
1254 }
1255
1256 static inline INT32 _stp_psm_notify_wmt(MTKSTP_PSM_T *stp_psm, const MTKSTP_PSM_ACTION_T action)
1257 {
1258     INT32 ret = 0;
1259
1260     if (stp_psm == NULL)
1261     {
1262         return STP_PSM_OPERATION_FAIL;
1263     }
1264
1265     switch(_stp_psm_get_state(stp_psm))
1266     {
1267         case ACT:
1268
1269             if(action == SLEEP)
1270             {
1271                 if (stp_psm->flag & STP_PSM_WMT_EVENT_DISABLE_MONITOR) {
1272                     STP_PSM_ERR_FUNC("psm monitor disabled, can't do sleep op\n");
1273                     return STP_PSM_OPERATION_FAIL;
1274                 }
1275
1276                 _stp_psm_set_state(stp_psm, ACT_INACT);
1277
1278                 _stp_psm_release_data(stp_psm);
1279
1280                 if(stp_psm->wmt_notify)
1281                 {
1282                     stp_psm->wmt_notify(SLEEP);
1283                     _stp_psm_wait_wmt_event_wq(stp_psm);
1284                 }
1285                 else
1286                 {
1287                     STP_PSM_ERR_FUNC("stp_psm->wmt_notify = NULL\n");
1288                     ret = STP_PSM_OPERATION_FAIL;
1289                 }
1290             }
1291             else if(action == WAKEUP || action == HOST_AWAKE)
1292             {
1293                 STP_PSM_INFO_FUNC("In ACT state, dont do WAKEUP/HOST_AWAKE again\n");
1294                 _stp_psm_release_data(stp_psm);
1295             }
1296             else
1297             {
1298                 STP_PSM_ERR_FUNC("invalid operation, the case should not happen\n");
1299                 STP_PSM_ERR_FUNC("state = %d, flag = %d\n", stp_psm->work_state, stp_psm->flag);
1300
1301                 ret = STP_PSM_OPERATION_FAIL;
1302
1303             }
1304
1305         break;
1306
1307         case INACT:
1308
1309             if(action == WAKEUP)
1310             {
1311                 _stp_psm_set_state(stp_psm, INACT_ACT);
1312
1313                 if(stp_psm->wmt_notify)
1314                 {
1315                     STP_PSM_DBG_FUNC("wakeup +wake_lock(%d)\n", osal_wake_lock_count(&stp_psm->wake_lock));
1316                     osal_wake_lock(&stp_psm->wake_lock);
1317                     STP_PSM_DBG_FUNC("wakeup +wake_lock(%d)#\n", osal_wake_lock_count(&stp_psm->wake_lock));
1318
1319                     STP_PSM_DBG_FUNC("mt_combo_plt_exit_deep_idle++\n");
1320                     mt_combo_plt_exit_deep_idle(COMBO_IF_UART);
1321                     STP_PSM_DBG_FUNC("mt_combo_plt_exit_deep_idle--\n");
1322
1323                     stp_psm->wmt_notify(WAKEUP);
1324                     _stp_psm_wait_wmt_event_wq(stp_psm);
1325                 }
1326                 else
1327                 {
1328                     STP_PSM_ERR_FUNC("stp_psm->wmt_notify = NULL\n");
1329                     ret = STP_PSM_OPERATION_FAIL;
1330                 }
1331             }
1332             else if(action == HOST_AWAKE)
1333             {
1334                 _stp_psm_set_state(stp_psm, INACT_ACT);
1335
1336                 if(stp_psm->wmt_notify)
1337                 {
1338                     STP_PSM_DBG_FUNC("host awake +wake_lock(%d)\n", osal_wake_lock_count(&stp_psm->wake_lock));
1339                     osal_wake_lock(&stp_psm->wake_lock);
1340                     STP_PSM_DBG_FUNC("host awake +wake_lock(%d)#\n", osal_wake_lock_count(&stp_psm->wake_lock));
1341
1342                     STP_PSM_DBG_FUNC("mt_combo_plt_exit_deep_idle++\n");
1343                     mt_combo_plt_exit_deep_idle(COMBO_IF_UART);
1344                     STP_PSM_DBG_FUNC("mt_combo_plt_exit_deep_idle--\n");
1345
1346                     stp_psm->wmt_notify(HOST_AWAKE);
1347                     _stp_psm_wait_wmt_event_wq(stp_psm);
1348                 }
1349                 else
1350                 {
1351                     STP_PSM_ERR_FUNC("stp_psm->wmt_notify = NULL\n");
1352                     ret = STP_PSM_OPERATION_FAIL;
1353                 }
1354             }
1355             else if(action == SLEEP)
1356             {
1357                 STP_PSM_INFO_FUNC("In INACT state, dont do SLEEP again\n");
1358             }
1359             else
1360             {
1361                 STP_PSM_ERR_FUNC("invalid operation, the case should not happen\n");
1362                 STP_PSM_ERR_FUNC("state = %d, flag = %d\n", stp_psm->work_state, stp_psm->flag);
1363                 ret = STP_PSM_OPERATION_FAIL;
1364             }
1365
1366         break;
1367
1368         default:
1369
1370             /*invalid*/
1371             STP_PSM_ERR_FUNC("invalid state, the case should not happen\n");
1372             STP_PSM_ERR_FUNC("state = %d, flag = %d\n", stp_psm->work_state, stp_psm->flag);
1373             ret = STP_PSM_OPERATION_FAIL;
1374
1375             break;
1376     }
1377     return ret;
1378 }
1379
1380 static inline void _stp_psm_stp_is_idle(ULONG data)
1381 {
1382      MTKSTP_PSM_T *stp_psm = (MTKSTP_PSM_T *)data;
1383
1384      stp_psm->flag &= ~STP_PSM_WMT_EVENT_DISABLE_MONITOR_RX_HIGH_DENSITY;
1385      stp_psm->flag &= ~STP_PSM_WMT_EVENT_DISABLE_MONITOR_TX_HIGH_DENSITY;
1386
1387      if((stp_psm->flag & STP_PSM_WMT_EVENT_DISABLE_MONITOR)!= 0)
1388      {
1389         STP_PSM_DBG_FUNC("STP-PSM DISABLE!\n");
1390         return ;
1391      }
1392
1393      STP_PSM_INFO_FUNC("**IDLE is over %d msec, go to sleep!!!**\n", stp_psm->idle_time_to_sleep);
1394      _stp_psm_notify_wmt_sleep_wq(stp_psm);
1395 }
1396
1397 static inline INT32 _stp_psm_init_monitor(MTKSTP_PSM_T *stp_psm)
1398 {
1399     if(!stp_psm)
1400     {
1401         return STP_PSM_OPERATION_FAIL;
1402     }
1403
1404     STP_PSM_INFO_FUNC("init monitor\n");
1405
1406     stp_psm->psm_timer.timeoutHandler = _stp_psm_stp_is_idle;
1407     stp_psm->psm_timer.timeroutHandlerData = (UINT32)stp_psm;
1408     osal_timer_create(&stp_psm->psm_timer);
1409
1410     return STP_PSM_OPERATION_SUCCESS;
1411 }
1412
1413 static inline INT32 _stp_psm_deinit_monitor(MTKSTP_PSM_T *stp_psm)
1414 {
1415
1416      if(!stp_psm)
1417      {
1418         return STP_PSM_OPERATION_FAIL;
1419      }
1420      else
1421      {
1422         STP_PSM_INFO_FUNC("deinit monitor\n");
1423
1424         osal_timer_stop_sync(&stp_psm->psm_timer);
1425      }
1426
1427      return 0;
1428 }
1429
1430 static inline INT32 _stp_psm_is_to_block_traffic(MTKSTP_PSM_T *stp_psm)
1431 {
1432     INT32 iRet = -1;
1433
1434 //    osal_lock_unsleepable_lock(&stp_psm->flagSpinlock);
1435
1436     if(stp_psm->flag & STP_PSM_BLOCK_DATA_EN)
1437     {
1438         iRet = 1;
1439     }
1440     else
1441     {
1442         iRet = 0;
1443     }
1444 //    osal_unlock_unsleepable_lock(&stp_psm->flagSpinlock);
1445     return iRet;
1446 }
1447
1448 static inline INT32 _stp_psm_is_disable(MTKSTP_PSM_T *stp_psm)
1449 {
1450     if(stp_psm->flag & STP_PSM_WMT_EVENT_DISABLE_MONITOR)
1451     {
1452         return 1;
1453     }
1454     else
1455     {
1456         return 0;
1457     }
1458 }
1459
1460 static inline INT32  _stp_psm_do_wait(MTKSTP_PSM_T *stp_psm, MTKSTP_PSM_STATE_T state){
1461
1462      #define POLL_WAIT 20//200
1463      #define POLL_WAIT_TIME 2000
1464
1465      INT32 i = 0;
1466      INT32 limit = POLL_WAIT_TIME/POLL_WAIT;
1467
1468      while(_stp_psm_get_state(stp_psm)!=state && i < limit)
1469      {
1470         osal_msleep(POLL_WAIT);
1471         i++;
1472         STP_PSM_INFO_FUNC("STP is waiting state for %s, i=%d, state = %d\n", g_psm_state[state],i , _stp_psm_get_state(stp_psm));
1473      }
1474
1475      if(i == limit)
1476      {
1477         STP_PSM_WARN_FUNC("-Wait for %s takes %d msec\n", g_psm_state[state], i*POLL_WAIT);
1478         return STP_PSM_OPERATION_FAIL;
1479      }
1480      else
1481      {
1482         STP_PSM_DBG_FUNC("+Total waits for %s takes %d msec\n", g_psm_state[state], i*POLL_WAIT);
1483         return STP_PSM_OPERATION_SUCCESS;
1484      }
1485 }
1486
1487 static inline INT32  _stp_psm_do_wakeup(MTKSTP_PSM_T *stp_psm)
1488 {
1489
1490     INT32 ret = 0;
1491     INT32 retry = 10;
1492     P_OSAL_OP_Q pOpQ;
1493     P_OSAL_OP   pOp;
1494
1495     STP_PSM_LOUD_FUNC("*** Do Force Wakeup!***\n\r");
1496
1497     //<1>If timer is active, we will stop it.
1498     _stp_psm_stop_monitor(stp_psm);
1499
1500     osal_lock_unsleepable_lock(&(stp_psm->wq_spinlock));
1501
1502     pOpQ = &stp_psm->rFreeOpQ;
1503
1504     while (!RB_EMPTY(&stp_psm->rActiveOpQ))
1505     {
1506         RB_GET(&stp_psm->rActiveOpQ, pOp);
1507                 if (NULL != pOp && !RB_FULL(pOpQ))
1508                 {
1509             STP_PSM_DBG_FUNC("opid = %d\n", pOp->op.opId);
1510             RB_PUT(pOpQ, pOp);
1511                 }
1512                 else
1513         {
1514             STP_PSM_ERR_FUNC("clear up active queue fail, freeQ full\n");
1515         }
1516     }
1517     osal_unlock_unsleepable_lock(&(stp_psm->wq_spinlock));
1518     //<5>We issue wakeup request into op queue. and wait for active.
1519     do{
1520         ret = _stp_psm_notify_wmt_wakeup_wq(stp_psm);
1521
1522         if(ret == STP_PSM_OPERATION_SUCCESS)
1523         {
1524             ret = _stp_psm_do_wait(stp_psm, ACT);
1525
1526             //STP_PSM_INFO_FUNC("<< wait ret = %d, num of activeQ = %d\n", ret,  RB_COUNT(&stp_psm->rActiveOpQ));
1527             if(ret == STP_PSM_OPERATION_SUCCESS)
1528             {
1529                 break;
1530             }
1531         }
1532         else
1533         {
1534             STP_PSM_ERR_FUNC("_stp_psm_notify_wmt_wakeup_wq fail!!\n");
1535         }
1536
1537         //STP_PSM_INFO_FUNC("retry = %d\n", retry);
1538         retry--;
1539
1540         if(retry == 0)
1541         {
1542             break;
1543         }
1544     }
1545     while(1);
1546
1547     if(retry == 0)
1548     {
1549         return STP_PSM_OPERATION_FAIL;
1550     }
1551     else
1552     {
1553         return STP_PSM_OPERATION_SUCCESS;
1554     }
1555 }
1556
1557 static inline INT32 _stp_psm_disable(MTKSTP_PSM_T *stp_psm)
1558 {
1559     INT32 ret = STP_PSM_OPERATION_FAIL;
1560
1561     STP_PSM_DBG_FUNC("PSM Disable start\n\r");
1562
1563     ret = osal_lock_sleepable_lock(&stp_psm->user_lock);
1564     if (ret) {
1565         STP_PSM_ERR_FUNC("--->lock stp_psm->user_lock failed, ret=%d\n", ret);
1566         return ret;
1567     }
1568
1569     stp_psm->flag |= STP_PSM_WMT_EVENT_DISABLE_MONITOR;
1570     ret = _stp_psm_do_wakeup(stp_psm);
1571     osal_unlock_sleepable_lock(&stp_psm->user_lock);
1572     if (ret == STP_PSM_OPERATION_SUCCESS)
1573     {
1574         STP_PSM_DBG_FUNC("PSM Disable Success\n");
1575     }
1576     else
1577     {
1578         STP_PSM_ERR_FUNC("***PSM Disable Fail***\n");
1579     }
1580
1581     return ret;
1582 }
1583
1584 static inline INT32 _stp_psm_enable(MTKSTP_PSM_T *stp_psm, INT32 idle_time_to_sleep)
1585 {
1586     INT32 ret = STP_PSM_OPERATION_FAIL;
1587     STP_PSM_LOUD_FUNC("PSM Enable start\n\r");
1588
1589     ret = osal_lock_sleepable_lock(&stp_psm->user_lock);
1590     if (ret) {
1591         STP_PSM_ERR_FUNC("--->lock stp_psm->user_lock failed, ret=%d\n", ret);
1592         return ret;
1593     }
1594
1595     stp_psm->flag |= STP_PSM_WMT_EVENT_DISABLE_MONITOR;
1596
1597     ret = _stp_psm_do_wakeup(stp_psm);
1598     if(ret == STP_PSM_OPERATION_SUCCESS)
1599     {
1600         stp_psm->flag &= ~STP_PSM_WMT_EVENT_DISABLE_MONITOR;
1601         stp_psm->idle_time_to_sleep = idle_time_to_sleep;
1602
1603         if(osal_wake_lock_count(&stp_psm->wake_lock) == 0)
1604         {
1605             STP_PSM_DBG_FUNC("psm_en+wake_lock(%d)\n", osal_wake_lock_count(&stp_psm->wake_lock));
1606             osal_wake_lock(&stp_psm->wake_lock);
1607             STP_PSM_DBG_FUNC("psm_en+wake_lock(%d)#\n", osal_wake_lock_count(&stp_psm->wake_lock));
1608         }
1609
1610         _stp_psm_start_monitor(stp_psm);
1611
1612         STP_PSM_DBG_FUNC("PSM Enable succeed\n\r");
1613     }
1614     else
1615     {
1616         STP_PSM_ERR_FUNC("***PSM Enable Fail***\n");
1617     }
1618     osal_unlock_sleepable_lock(&stp_psm->user_lock);
1619
1620     return ret;
1621 }
1622
1623 INT32  _stp_psm_thread_lock_aquire(MTKSTP_PSM_T *stp_psm)
1624 {
1625     return osal_lock_sleepable_lock(&stp_psm->stp_psm_lock);
1626 }
1627
1628 INT32  _stp_psm_thread_lock_release(MTKSTP_PSM_T *stp_psm)
1629 {
1630     osal_unlock_sleepable_lock(&stp_psm->stp_psm_lock);
1631     return 0;
1632 }
1633
1634 MTK_WCN_BOOL _stp_psm_is_quick_ps_support (VOID)
1635 {
1636     if (stp_psm->is_wmt_quick_ps_support)
1637     {
1638         return (*(stp_psm->is_wmt_quick_ps_support))();
1639     }
1640         STP_PSM_DBG_FUNC("stp_psm->is_wmt_quick_ps_support is NULL, return false\n\r");
1641         return MTK_WCN_BOOL_FALSE;
1642 }
1643
1644
1645 MTK_WCN_BOOL stp_psm_is_quick_ps_support (VOID)
1646 {
1647     return _stp_psm_is_quick_ps_support();
1648 }
1649
1650 INT32 stp_psm_disable_by_tx_rx_density(MTKSTP_PSM_T *stp_psm, INT32 dir){
1651
1652     //easy the variable maintain beween stp tx, rx thread.
1653     //so we create variable for tx, rx respectively.
1654
1655     static INT32 tx_cnt = 0;
1656     static INT32 rx_cnt = 0;
1657     static INT32 is_tx_first = 1;
1658     static INT32 is_rx_first = 1;
1659     static ULONG tx_end_time = 0;
1660     static ULONG rx_end_time = 0;
1661
1662     //
1663     //BT A2DP                  TX CNT = 220, RX CNT = 843
1664     //BT FTP Transferring  TX CNT = 574, RX CNT = 2233 (1228~1588)
1665     //BT FTP Receiving      TX CNT = 204, RX CNT = 3301 (2072~2515)
1666     //BT OPP  Tx               TX_CNT= 330, RX CNT = 1300~1800
1667     //BT OPP  Rx               TX_CNT= (109~157), RX CNT = 1681~2436
1668     if(dir == 0)//tx
1669     {
1670         tx_cnt++;
1671
1672         if(((LONG)jiffies - (LONG)tx_end_time >= 0) || (is_tx_first))
1673         {
1674             tx_end_time = jiffies + (3*HZ);
1675             STP_PSM_INFO_FUNC("tx cnt = %d in the previous 3 sec\n",  tx_cnt);
1676             //if(tx_cnt > 400)//for high traffic , not to do sleep.
1677             if (tx_cnt > 300)
1678             {
1679                 stp_psm->flag |= STP_PSM_WMT_EVENT_DISABLE_MONITOR_TX_HIGH_DENSITY;
1680                 stp_psm_start_monitor(stp_psm);
1681             }
1682             else
1683             {
1684                 stp_psm->flag &= ~STP_PSM_WMT_EVENT_DISABLE_MONITOR_TX_HIGH_DENSITY;
1685             }
1686             tx_cnt = 0;
1687             if(is_tx_first)
1688                 is_tx_first = 0;
1689         }
1690     }
1691     else
1692     {
1693         rx_cnt++;
1694
1695         if(((LONG)jiffies - (LONG)rx_end_time >= 0) || (is_rx_first))
1696         {
1697             rx_end_time = jiffies + (3*HZ);
1698             STP_PSM_INFO_FUNC("rx cnt = %d in the previous 3 sec\n", rx_cnt);
1699
1700             //if(rx_cnt > 2000)//for high traffic , not to do sleep.
1701             if(rx_cnt > 1200)//for high traffic , not to do sleep.
1702             {
1703                 stp_psm->flag |= STP_PSM_WMT_EVENT_DISABLE_MONITOR_RX_HIGH_DENSITY;
1704                 stp_psm_start_monitor(stp_psm);
1705             }
1706             else
1707             {
1708                 stp_psm->flag &= ~STP_PSM_WMT_EVENT_DISABLE_MONITOR_RX_HIGH_DENSITY;
1709             }
1710             rx_cnt = 0;
1711             if(is_rx_first)
1712                 is_rx_first = 0;
1713         }
1714     }
1715
1716     return 0;
1717 }
1718
1719 /*external function for WMT module to do sleep/wakeup*/
1720 INT32 stp_psm_set_state(MTKSTP_PSM_T *stp_psm, MTKSTP_PSM_STATE_T state)
1721 {
1722     return _stp_psm_set_state(stp_psm, state);
1723 }
1724
1725
1726 INT32  stp_psm_thread_lock_aquire(MTKSTP_PSM_T *stp_psm)
1727 {
1728     return _stp_psm_thread_lock_aquire(stp_psm);
1729 }
1730
1731
1732 INT32  stp_psm_thread_lock_release(MTKSTP_PSM_T *stp_psm)
1733 {
1734     return _stp_psm_thread_lock_release(stp_psm);
1735 }
1736
1737
1738
1739 INT32  stp_psm_do_wakeup(MTKSTP_PSM_T *stp_psm)
1740 {
1741     return _stp_psm_do_wakeup(stp_psm);
1742 }
1743
1744 INT32 stp_psm_notify_stp(MTKSTP_PSM_T *stp_psm, const MTKSTP_PSM_ACTION_T action)
1745 {
1746
1747     return  _stp_psm_notify_stp(stp_psm, action);
1748 }
1749
1750 INT32 stp_psm_notify_wmt_wakeup(MTKSTP_PSM_T *stp_psm)
1751 {
1752     return _stp_psm_notify_wmt_wakeup_wq(stp_psm);
1753 }
1754
1755 INT32 stp_psm_notify_wmt_sleep(MTKSTP_PSM_T *stp_psm){
1756
1757     return _stp_psm_notify_wmt_sleep_wq(stp_psm);
1758 }
1759
1760 INT32 stp_psm_start_monitor(MTKSTP_PSM_T *stp_psm)
1761 {
1762     return _stp_psm_start_monitor(stp_psm);
1763 }
1764
1765 INT32 stp_psm_is_to_block_traffic(MTKSTP_PSM_T *stp_psm)
1766 {
1767     return _stp_psm_is_to_block_traffic(stp_psm);
1768 }
1769
1770 INT32 stp_psm_is_disable(MTKSTP_PSM_T *stp_psm)
1771 {
1772     return _stp_psm_is_disable(stp_psm);
1773 }
1774
1775 INT32 stp_psm_has_pending_data(MTKSTP_PSM_T *stp_psm)
1776 {
1777     return _stp_psm_has_pending_data(stp_psm);
1778 }
1779
1780 INT32 stp_psm_release_data(MTKSTP_PSM_T *stp_psm)
1781 {
1782     return _stp_psm_release_data(stp_psm);
1783 }
1784
1785 INT32
1786 stp_psm_hold_data (
1787     MTKSTP_PSM_T *stp_psm,
1788     const UINT8 *buffer,
1789     const UINT32 len,
1790     const UINT8 type
1791     )
1792 {
1793     return _stp_psm_hold_data(stp_psm, buffer, len, type);
1794 }
1795
1796 INT32 stp_psm_disable(MTKSTP_PSM_T *stp_psm)
1797 {
1798     return _stp_psm_disable(stp_psm);
1799 }
1800
1801 INT32 stp_psm_enable(MTKSTP_PSM_T *stp_psm, INT32 idle_time_to_sleep)
1802 {
1803     return _stp_psm_enable(stp_psm, idle_time_to_sleep);
1804 }
1805
1806 INT32 stp_psm_reset(MTKSTP_PSM_T *stp_psm)
1807 {
1808     stp_psm_set_sleep_enable(stp_psm);
1809
1810     return _stp_psm_reset(stp_psm);
1811 }
1812
1813 INT32 stp_psm_sleep_for_thermal(MTKSTP_PSM_T *stp_psm)
1814 {
1815     return _stp_psm_notify_wmt_sleep_wq(stp_psm);
1816 }
1817
1818
1819 INT32 stp_psm_set_sleep_enable(MTKSTP_PSM_T *stp_psm)
1820 {
1821     INT32 ret = 0;
1822
1823     if (stp_psm) {
1824         stp_psm->sleep_en = 1;
1825         STP_PSM_DBG_FUNC("\n");
1826         ret = 0;
1827     } else {
1828         STP_PSM_INFO_FUNC("Null pointer\n");
1829         ret = -1;
1830     }
1831
1832     return ret;
1833 }
1834
1835
1836 INT32 stp_psm_set_sleep_disable(MTKSTP_PSM_T *stp_psm)
1837 {
1838     INT32 ret = 0;
1839
1840     if (stp_psm) {
1841         stp_psm->sleep_en = 0;
1842         STP_PSM_DBG_FUNC("\n");
1843         ret = 0;
1844     } else {
1845         STP_PSM_INFO_FUNC("Null pointer\n");
1846         ret = -1;
1847     }
1848
1849     return ret;
1850 }
1851
1852
1853 /* stp_psm_check_sleep_enable  - to check if sleep cmd is enabled or not
1854  * @ stp_psm - pointer of psm
1855  *
1856  * return 1 if sleep is enabled; else return 0 if disabled; else error code
1857  */
1858 INT32 stp_psm_check_sleep_enable(MTKSTP_PSM_T *stp_psm)
1859 {
1860     INT32 ret = 0;
1861
1862     if (stp_psm) {
1863         ret = stp_psm->sleep_en;
1864         STP_PSM_DBG_FUNC("%s \n", ret? "enabled" : "disabled");
1865     } else {
1866         STP_PSM_INFO_FUNC("Null pointer\n");
1867         ret = -1;
1868     }
1869
1870     return ret;
1871 }
1872
1873
1874 MTKSTP_PSM_T *stp_psm_init(void)
1875 {
1876     INT32 err = 0;
1877     INT32 i = 0;
1878     INT32 ret = -1;
1879
1880     STP_PSM_INFO_FUNC("psm init\n");
1881
1882     stp_psm->work_state = ACT;
1883     stp_psm->wmt_notify = wmt_lib_ps_stp_cb;
1884         stp_psm->is_wmt_quick_ps_support = wmt_lib_is_quick_ps_support;
1885     stp_psm->idle_time_to_sleep = STP_PSM_IDLE_TIME_SLEEP;
1886     stp_psm->flag = 0;
1887     stp_psm->stp_tx_cb = NULL;
1888     stp_psm_set_sleep_enable(stp_psm);
1889
1890     ret = osal_fifo_init(&stp_psm->hold_fifo, NULL, STP_PSM_FIFO_SIZE);
1891     if(ret < 0)
1892     {
1893         STP_PSM_ERR_FUNC("FIFO INIT FAILS\n");
1894         goto ERR_EXIT4;
1895     }
1896
1897     osal_fifo_reset(&stp_psm->hold_fifo);
1898     osal_sleepable_lock_init(&stp_psm->user_lock);
1899     psm_fifo_lock_init(stp_psm);
1900     osal_unsleepable_lock_init(&stp_psm->wq_spinlock);
1901     osal_sleepable_lock_init(&stp_psm->stp_psm_lock);
1902
1903 //    osal_unsleepable_lock_init(&stp_psm->flagSpinlock);
1904
1905     osal_memcpy(stp_psm->wake_lock.name, "MT662x", 6);
1906     osal_wake_lock_init(&stp_psm->wake_lock);
1907
1908     osal_event_init(&stp_psm->STPd_event);
1909     RB_INIT(&stp_psm->rFreeOpQ, STP_OP_BUF_SIZE);
1910     RB_INIT(&stp_psm->rActiveOpQ, STP_OP_BUF_SIZE);
1911     /* Put all to free Q */
1912     for (i = 0; i < STP_OP_BUF_SIZE; i++)
1913     {
1914          osal_signal_init(&(stp_psm->arQue[i].signal));
1915          _stp_psm_put_op(stp_psm, &stp_psm->rFreeOpQ, &(stp_psm->arQue[i]));
1916     }
1917     //stp_psm->current_active_op = NULL;
1918     stp_psm->last_active_opId = STP_OPID_PSM_INALID;
1919     /*Generate BTM thread, to servie STP-CORE and WMT-CORE for sleeping, waking up and host awake*/
1920     stp_psm->PSMd.pThreadData = (VOID *)stp_psm;
1921     stp_psm->PSMd.pThreadFunc = (VOID *)_stp_psm_proc;
1922     osal_memcpy(stp_psm->PSMd.threadName, PSM_THREAD_NAME, osal_strlen(PSM_THREAD_NAME));
1923
1924     ret = osal_thread_create(&stp_psm->PSMd);
1925     if (ret < 0)
1926     {
1927         STP_PSM_ERR_FUNC("osal_thread_create fail...\n");
1928         goto ERR_EXIT5;
1929     }
1930
1931     //init_waitqueue_head(&stp_psm->wait_wmt_q);
1932     stp_psm->wait_wmt_q.timeoutValue = STP_PSM_WAIT_EVENT_TIMEOUT;
1933     osal_event_init(&stp_psm->wait_wmt_q);
1934
1935     err = _stp_psm_init_monitor(stp_psm);
1936     if(err)
1937     {
1938         STP_PSM_ERR_FUNC("__stp_psm_init ERROR\n");
1939         goto ERR_EXIT6;
1940     }
1941
1942     //Start STPd thread
1943     ret = osal_thread_run(&stp_psm->PSMd);
1944     if(ret < 0)
1945     {
1946         STP_PSM_ERR_FUNC("osal_thread_run FAILS\n");
1947         goto ERR_EXIT6;
1948     }
1949
1950     //psm disable in default
1951     _stp_psm_disable(stp_psm);
1952
1953
1954     return stp_psm;
1955
1956 ERR_EXIT6:
1957
1958     ret = osal_thread_destroy(&stp_psm->PSMd);
1959     if(ret < 0)
1960     {
1961         STP_PSM_ERR_FUNC("osal_thread_destroy FAILS\n");
1962         goto ERR_EXIT5;
1963     }
1964 ERR_EXIT5:
1965     osal_fifo_deinit(&stp_psm->hold_fifo);
1966 ERR_EXIT4:
1967
1968     return NULL;
1969 }
1970
1971 INT32 stp_psm_deinit(MTKSTP_PSM_T *stp_psm)
1972 {
1973     INT32 ret = -1;
1974
1975     STP_PSM_INFO_FUNC("psm deinit\n");
1976
1977     if(!stp_psm)
1978     {
1979         return STP_PSM_OPERATION_FAIL;
1980     }
1981
1982     ret = osal_thread_destroy(&stp_psm->PSMd);
1983     if(ret < 0)
1984     {
1985         STP_PSM_ERR_FUNC("osal_thread_destroy FAILS\n");
1986     }
1987
1988     ret = _stp_psm_deinit_monitor(stp_psm);
1989     if(ret < 0)
1990     {
1991         STP_PSM_ERR_FUNC("_stp_psm_deinit_monitor ERROR\n");
1992     }
1993
1994     osal_fifo_deinit(&stp_psm->hold_fifo);
1995     osal_sleepable_lock_deinit(&stp_psm->user_lock);
1996     psm_fifo_lock_deinit(stp_psm);
1997     osal_unsleepable_lock_deinit(&stp_psm->wq_spinlock);
1998         osal_sleepable_lock_deinit(&stp_psm->stp_psm_lock);
1999 //    osal_unsleepable_lock_deinit(&stp_psm->flagSpinlock);
2000
2001     return STP_PSM_OPERATION_SUCCESS;
2002 }
2003
2004 EXPORT_SYMBOL(mtk_wcn_stp_psm_dbg_level);
2005