drivers/mtk_wcn_combo/common/core/btm_core.c

   1
   2
   3 #include "osal_typedef.h"
   4 #include "osal.h"
   5 #include "stp_dbg.h"
   6 #include "stp_core.h"
   7 #include "btm_core.h"
   8
   9 #define PFX_BTM                         "[STP-BTM] "
  10 #define STP_BTM_LOG_LOUD                 4
  11 #define STP_BTM_LOG_DBG                  3
  12 #define STP_BTM_LOG_INFO                 2
  13 #define STP_BTM_LOG_WARN                 1
  14 #define STP_BTM_LOG_ERR                  0
  15
  16 INT32 gBtmDbgLevel = STP_BTM_LOG_INFO;
  17
  18 #define STP_BTM_LOUD_FUNC(fmt, arg...)   if(gBtmDbgLevel >= STP_BTM_LOG_LOUD){ osal_dbg_print(PFX_BTM "%s: "  fmt, __FUNCTION__ ,##arg);}
  19 #define STP_BTM_DBG_FUNC(fmt, arg...)    if(gBtmDbgLevel >= STP_BTM_LOG_DBG){ osal_dbg_print(PFX_BTM "%s: "  fmt, __FUNCTION__ ,##arg);}
  20 #define STP_BTM_INFO_FUNC(fmt, arg...)   if(gBtmDbgLevel >= STP_BTM_LOG_INFO){ osal_dbg_print(PFX_BTM "[I]%s: "  fmt, __FUNCTION__ ,##arg);}
  21 #define STP_BTM_WARN_FUNC(fmt, arg...)   if(gBtmDbgLevel >= STP_BTM_LOG_WARN){ osal_dbg_print(PFX_BTM "[W]%s: "  fmt, __FUNCTION__ ,##arg);}
  22 #define STP_BTM_ERR_FUNC(fmt, arg...)    if(gBtmDbgLevel >= STP_BTM_LOG_ERR){  osal_dbg_print(PFX_BTM "[E]%s(%d):ERROR! "   fmt, __FUNCTION__ , __LINE__, ##arg);}
  23 #define STP_BTM_TRC_FUNC(f)              if(gBtmDbgLevel >= STP_BTM_LOG_DBG){ osal_dbg_print(PFX_BTM "<%s> <%d>\n", __FUNCTION__, __LINE__);}
  24
  25 INT32 gDumplogflag = 0;
  26 #if WMT_PLAT_ALPS
  27 extern void dump_uart_history(void);
  28 #endif
  29
  30
  31 #define ASSERT(expr)
  32
  33 MTKSTP_BTM_T stp_btm_i;
  34 MTKSTP_BTM_T *stp_btm = &stp_btm_i;
  35
  36 const PCHAR g_btm_op_name[]={
  37         "STP_OPID_BTM_RETRY",
  38         "STP_OPID_BTM_RST",
  39         "STP_OPID_BTM_DBG_DUMP",
  40         "STP_OPID_BTM_EXIT"
  41     };
  42
  43 #if 0
  44 static PCHAR _stp_pkt_type(INT32 type){
  45
  46     static CHAR s[10];
  47
  48     switch(type){
  49         case WMT_TASK_INDX:
  50             osal_memcpy(s, "WMT", strlen("WMT")+1);
  51             break;
  52         case BT_TASK_INDX:
  53             osal_memcpy(s, "BT", strlen("BT")+1);
  54             break;
  55         case GPS_TASK_INDX:
  56             osal_memcpy(s, "GPS", strlen("GPS")+1);
  57             break;
  58         case FM_TASK_INDX:
  59             osal_memcpy(s, "FM", strlen("FM")+1);
  60             break;
  61         default:
  62             osal_memcpy(s, "UNKOWN", strlen("UNKOWN")+1);
  63             break;
  64     }
  65
  66     return s;
  67 }
  68 #endif
  69
  70 static INT32 _stp_btm_put_dump_to_nl(void)
  71 {
  72     #define NUM_FETCH_ENTRY 8
  73
  74     static UINT8  buf[2048];
  75     static UINT8  tmp[2048];
  76
  77     UINT32  buf_len;
  78     STP_PACKET_T  *pkt;
  79     STP_DBG_HDR_T *hdr;
  80     INT32  remain=0, index =0;
  81     INT32 retry = 0, rc = 0, nl_retry = 0;
  82     STP_BTM_INFO_FUNC("Enter..\n");
  83
  84     index = 0;
  85     tmp[index++]='[';
  86     tmp[index++]='M';
  87     tmp[index++]=']';
  88
  89     do
  90     {
  91         index = 3;
  92         remain = stp_dbg_dmp_out_ex(&buf[0], &buf_len);
  93         if (buf_len > 0)
  94         {
  95             pkt = (STP_PACKET_T  *)buf;
  96             hdr = &pkt->hdr;
  97             if (hdr->dbg_type == STP_DBG_FW_DMP){
  98                 osal_memcpy(&tmp[index], pkt->raw, pkt->hdr.len);
  99
 100                 if(pkt->hdr.len <= 1500)
 101                 {
 102                     tmp[index + pkt->hdr.len] = '\n';
 103                     tmp[index + pkt->hdr.len + 1] = '\0';
 104
 105                     //printk("\n%s\n+++\n", tmp);
 106                     rc = stp_dbg_nl_send((PCHAR)&tmp, 2);
 107
 108                     while(rc){
 109                        nl_retry++;
 110                        if(nl_retry > 1000){
 111                             break;
 112                        }
 113                        STP_BTM_WARN_FUNC("**dump send fails, and retry again.**\n");
 114                        osal_msleep(3);
 115                        rc = stp_dbg_nl_send((PCHAR)&tmp, 2);
 116                        if(!rc){
 117                           STP_BTM_WARN_FUNC("****retry again ok!**\n");
 118                        }
 119                     }
 120                     //schedule();
 121                 } else {
 122                     STP_BTM_INFO_FUNC("dump entry length is over long\n");
 123                     osal_bug_on(0);
 124                 }
 125                 retry = 0;
 126             }
 127         }else
 128         {
 129             retry ++;
 130             osal_msleep(100);
 131         }
 132     }while((remain > 0) || (retry < 2));
 133
 134     STP_BTM_INFO_FUNC("Exit..\n");
 135     return 0;
 136 }
 137
 138
 139 static INT32 _stp_btm_put_dump_to_aee(void)
 140 {
 141     static UINT8  buf[2048];
 142     static UINT8  tmp[2048];
 143
 144     UINT32  buf_len;
 145     STP_PACKET_T  *pkt;
 146     STP_DBG_HDR_T *hdr;
 147     INT32 remain = 0;
 148     INT32 retry = 0;
 149     INT32 ret = 0;
 150     STP_BTM_INFO_FUNC("Enter..\n");
 151
 152     do {
 153         remain = stp_dbg_dmp_out_ex(&buf[0], &buf_len);
 154         if (buf_len > 0) {
 155             pkt = (STP_PACKET_T*)buf;
 156             hdr = &pkt->hdr;
 157             if (hdr->dbg_type == STP_DBG_FW_DMP) {
 158                 osal_memcpy(&tmp[0], pkt->raw, pkt->hdr.len);
 159
 160                 if (pkt->hdr.len <= 1500) {
 161                     tmp[pkt->hdr.len] = '\n';
 162                     tmp[pkt->hdr.len + 1] = '\0';
 163
 164                     ret = stp_dbg_aee_send(tmp, pkt->hdr.len, 0);
 165                 } else {
 166                     STP_BTM_INFO_FUNC("dump entry length is over long\n");
 167                     osal_bug_on(0);
 168                 }
 169                 retry = 0;
 170             }
 171         } else {
 172             retry ++;
 173             osal_msleep(100);
 174         }
 175     }while ((remain > 0) || (retry < 2));
 176
 177     STP_BTM_INFO_FUNC("Exit..\n");
 178     return ret;
 179 }
 180
 181 #define COMBO_DUMP2AEE
 182
 183 static INT32 _stp_btm_handler(MTKSTP_BTM_T *stp_btm, P_STP_BTM_OP pStpOp)
 184 {
 185     INT32 ret = -1;
 186     INT32 dump_sink = 1; //core dump target, 0: aee; 1: netlink
 187
 188     if (NULL == pStpOp)
 189     {
 190         return -1;
 191     }
 192
 193     switch(pStpOp->opId)
 194     {
 195         case STP_OPID_BTM_EXIT:
 196             // TODO: clean all up?
 197             ret = 0;
 198         break;
 199
 200         /*tx timeout retry*/
 201         case STP_OPID_BTM_RETRY:
 202             stp_do_tx_timeout();
 203             ret = 0;
 204
 205         break;
 206
 207         /*whole chip reset*/
 208         case STP_OPID_BTM_RST:
 209             STP_BTM_INFO_FUNC("whole chip reset start!\n");
 210             STP_BTM_INFO_FUNC("....+\n");
 211             if(stp_btm->wmt_notify)
 212             {
 213                 stp_btm->wmt_notify(BTM_RST_OP);
 214                 ret = 0;
 215             }
 216             else
 217             {
 218                 STP_BTM_ERR_FUNC("stp_btm->wmt_notify is NULL.");
 219                 ret = -1;
 220             }
 221
 222             STP_BTM_INFO_FUNC("whole chip reset end!\n");
 223
 224         break;
 225
 226         case STP_OPID_BTM_DBG_DUMP:
 227             /*Notify the wmt to get dump data*/
 228             STP_BTM_DBG_FUNC("wmt dmp notification\n");
 229             dump_sink = ((stp_btm->wmt_notify(BTM_GET_AEE_SUPPORT_FLAG) == MTK_WCN_BOOL_TRUE) ? 0 : 1);
 230
 231             if (dump_sink == 0) {
 232                 _stp_btm_put_dump_to_aee();
 233             } else if (dump_sink == 1) {
 234                 _stp_btm_put_dump_to_nl();
 235             } else {
 236                 STP_BTM_ERR_FUNC("unknown sink %d\n", dump_sink);
 237             }
 238
 239         break;
 240
 241
 242         case STP_OPID_BTM_DUMP_TIMEOUT:
 243             // Flush dump data, and reset compressor
 244             STP_BTM_INFO_FUNC("Flush dump data\n");
 245             wcn_core_dump_flush(0);
 246         break;
 247
 248         default:
 249             ret = -1;
 250         break;
 251     }
 252
 253     return ret;
 254 }
 255
 256 static P_OSAL_OP _stp_btm_get_op (
 257     MTKSTP_BTM_T *stp_btm,
 258     P_OSAL_OP_Q pOpQ
 259     )
 260 {
 261     P_OSAL_OP pOp;
 262     //INT32 ret = 0;
 263
 264     if (!pOpQ)
 265     {
 266         STP_BTM_WARN_FUNC("!pOpQ \n");
 267         return NULL;
 268     }
 269
 270     osal_lock_unsleepable_lock(&(stp_btm->wq_spinlock));
 271     /* acquire lock success */
 272     RB_GET(pOpQ, pOp);
 273     osal_unlock_unsleepable_lock(&(stp_btm->wq_spinlock));
 274
 275     if (!pOp)
 276     {
 277         //STP_BTM_WARN_FUNC("RB_GET fail\n");
 278     }
 279
 280     return pOp;
 281 }
 282
 283 static INT32 _stp_btm_put_op (
 284     MTKSTP_BTM_T *stp_btm,
 285     P_OSAL_OP_Q pOpQ,
 286     P_OSAL_OP pOp
 287     )
 288 {
 289     INT32 ret;
 290
 291     if (!pOpQ || !pOp)
 292     {
 293         STP_BTM_WARN_FUNC("invalid input param: 0x%p, 0x%p \n", pOpQ, pOp);
 294         return 0;//;MTK_WCN_BOOL_FALSE;
 295     }
 296
 297     ret = 0;
 298
 299     osal_lock_unsleepable_lock(&(stp_btm->wq_spinlock));
 300     /* acquire lock success */
 301     if (!RB_FULL(pOpQ))
 302     {
 303         RB_PUT(pOpQ, pOp);
 304     }
 305     else
 306     {
 307         ret = -1;
 308     }
 309     osal_unlock_unsleepable_lock(&(stp_btm->wq_spinlock));
 310
 311     if (ret)
 312     {
 313         STP_BTM_WARN_FUNC("RB_FULL(0x%p) %d ,rFreeOpQ = %p, rActiveOpQ = %p\n", pOpQ, RB_COUNT(pOpQ),
 314             &stp_btm->rFreeOpQ,  &stp_btm->rActiveOpQ);
 315         return 0;
 316     }
 317     else
 318     {
 319         //STP_BTM_WARN_FUNC("RB_COUNT = %d\n",RB_COUNT(pOpQ));
 320         return 1;
 321     }
 322 }
 323
 324 P_OSAL_OP _stp_btm_get_free_op (
 325     MTKSTP_BTM_T *stp_btm
 326     )
 327 {
 328     P_OSAL_OP pOp;
 329
 330     if (stp_btm)
 331     {
 332         pOp = _stp_btm_get_op(stp_btm, &stp_btm->rFreeOpQ);
 333         if (pOp)
 334         {
 335             osal_memset(&pOp->op, 0, sizeof(pOp->op));
 336         }
 337         return pOp;
 338     }
 339     else {
 340         return NULL;
 341     }
 342 }
 343
 344 INT32 _stp_btm_put_act_op (
 345     MTKSTP_BTM_T *stp_btm,
 346     P_OSAL_OP pOp
 347     )
 348 {
 349     INT32 bRet = 0;
 350     INT32 bCleanup = 0;
 351     LONG wait_ret = -1;
 352
 353     P_OSAL_SIGNAL pSignal = NULL;
 354
 355     do {
 356         if (!stp_btm || !pOp)
 357         {
 358             break;
 359         }
 360
 361         pSignal = &pOp->signal;
 362
 363         if (pSignal->timeoutValue)
 364         {
 365             pOp->result = -9;
 366             osal_signal_init(&pOp->signal);
 367         }
 368
 369         /* put to active Q */
 370         bRet = _stp_btm_put_op(stp_btm, &stp_btm->rActiveOpQ, pOp);
 371         if(0 == bRet)
 372         {
 373             STP_BTM_WARN_FUNC("put active queue fail\n");
 374             bCleanup = 1;//MTK_WCN_BOOL_TRUE;
 375             break;
 376         }
 377
 378         /* wake up wmtd */
 379         osal_trigger_event(&stp_btm->STPd_event);
 380
 381         if (pSignal->timeoutValue == 0)
 382         {
 383             bRet = 1;//MTK_WCN_BOOL_TRUE;
 384             /* clean it in wmtd */
 385             break;
 386         }
 387
 388         /* wait result, clean it here */
 389         bCleanup = 1;//MTK_WCN_BOOL_TRUE;
 390
 391         /* check result */
 392         wait_ret = osal_wait_for_signal_timeout(&pOp->signal);
 393
 394         STP_BTM_DBG_FUNC("wait completion:%ld\n", wait_ret);
 395         if (!wait_ret)
 396         {
 397             STP_BTM_ERR_FUNC("wait completion timeout \n");
 398             // TODO: how to handle it? retry?
 399         }
 400         else
 401         {
 402             if (pOp->result)
 403             {
 404                 STP_BTM_WARN_FUNC("op(%d) result:%d\n", pOp->op.opId, pOp->result);
 405             }
 406
 407             bRet = (pOp->result) ? 0 : 1;
 408         }
 409     } while(0);
 410
 411     if (bCleanup) {
 412         /* put Op back to freeQ */
 413         _stp_btm_put_op(stp_btm, &stp_btm->rFreeOpQ, pOp);
 414     }
 415
 416     return bRet;
 417 }
 418
 419 static INT32 _stp_btm_wait_for_msg(void *pvData)
 420 {
 421     MTKSTP_BTM_T *stp_btm = (MTKSTP_BTM_T *)pvData;
 422     return ((!RB_EMPTY(&stp_btm->rActiveOpQ)) || osal_thread_should_stop(&stp_btm->BTMd));
 423 }
 424
 425 static INT32 _stp_btm_proc (void *pvData)
 426 {
 427     MTKSTP_BTM_T *stp_btm = (MTKSTP_BTM_T *)pvData;
 428     P_OSAL_OP pOp;
 429     INT32 id;
 430     INT32 result;
 431
 432     if (!stp_btm)
 433     {
 434         STP_BTM_WARN_FUNC("!stp_btm \n");
 435         return -1;
 436     }
 437
 438     for (;;)
 439     {
 440         pOp = NULL;
 441
 442         osal_wait_for_event(&stp_btm->STPd_event,
 443             _stp_btm_wait_for_msg,
 444             (void *)stp_btm
 445             );
 446
 447         if (osal_thread_should_stop(&stp_btm->BTMd))
 448         {
 449             STP_BTM_INFO_FUNC("should stop now... \n");
 450             // TODO: clean up active opQ
 451             break;
 452         }
 453
 454 #if 1
 455         if(gDumplogflag)
 456         {
 457             //printk("enter place1\n");
 458             #if WMT_PLAT_ALPS
 459             dump_uart_history();
 460                         #endif
 461             gDumplogflag = 0;
 462             continue;
 463         }
 464 #endif
 465
 466         /* get Op from activeQ */
 467         pOp = _stp_btm_get_op(stp_btm, &stp_btm->rActiveOpQ);
 468
 469         if (!pOp)
 470         {
 471             STP_BTM_WARN_FUNC("get_lxop activeQ fail\n");
 472             continue;
 473         }
 474
 475         id = osal_op_get_id(pOp);
 476
 477         STP_BTM_DBG_FUNC("======> lxop_get_opid = %d, %s, remaining count = *%d*\n",
 478             id, (id >= 4)?("???"):(g_btm_op_name[id]), RB_COUNT(&stp_btm->rActiveOpQ));
 479
 480         if (id >= STP_OPID_BTM_NUM)
 481         {
 482             STP_BTM_WARN_FUNC("abnormal opid id: 0x%x \n", id);
 483             result = -1;
 484             goto handler_done;
 485         }
 486
 487         result = _stp_btm_handler(stp_btm, &pOp->op);
 488
 489 handler_done:
 490
 491         if (result)
 492         {
 493             STP_BTM_WARN_FUNC("opid id(0x%x)(%s) error(%d)\n", id, (id >= 4)?("???"):(g_btm_op_name[id]), result);
 494         }
 495
 496         if (osal_op_is_wait_for_signal(pOp))
 497         {
 498             osal_op_raise_signal(pOp, result);
 499         }
 500         else
 501         {
 502             /* put Op back to freeQ */
 503             _stp_btm_put_op(stp_btm, &stp_btm->rFreeOpQ, pOp);
 504         }
 505
 506         if (STP_OPID_BTM_EXIT == id)
 507         {
 508             break;
 509         } else if (STP_OPID_BTM_RST == id) {
 510             /* prevent multi reset case */
 511             stp_btm_reset_btm_wq(stp_btm);
 512         }
 513     }
 514
 515     STP_BTM_INFO_FUNC("exits \n");
 516
 517     return 0;
 518 };
 519
 520 static inline INT32 _stp_btm_notify_wmt_rst_wq(MTKSTP_BTM_T *stp_btm)
 521 {
 522
 523     P_OSAL_OP       pOp;
 524     INT32           bRet;
 525     INT32 retval;
 526
 527     if(stp_btm == NULL)
 528     {
 529         return STP_BTM_OPERATION_FAIL;
 530     }
 531     else
 532     {
 533         pOp = _stp_btm_get_free_op(stp_btm);
 534         if (!pOp)
 535         {
 536             STP_BTM_WARN_FUNC("get_free_lxop fail \n");
 537             return -1;//break;
 538         }
 539         pOp->op.opId = STP_OPID_BTM_RST;
 540         pOp->signal.timeoutValue= 0;
 541         bRet = _stp_btm_put_act_op(stp_btm, pOp);
 542         STP_BTM_DBG_FUNC("OPID(%d) type(%d) bRet(%d) \n\n",
 543             pOp->op.opId,
 544             pOp->op.au4OpData[0],
 545             bRet);
 546         retval = (0 == bRet) ? STP_BTM_OPERATION_FAIL : STP_BTM_OPERATION_SUCCESS;
 547     }
 548     return retval;
 549 }
 550
 551 static inline INT32 _stp_btm_notify_stp_retry_wq(MTKSTP_BTM_T *stp_btm){
 552
 553     P_OSAL_OP       pOp;
 554     INT32           bRet;
 555     INT32 retval;
 556
 557     if(stp_btm == NULL)
 558     {
 559         return STP_BTM_OPERATION_FAIL;
 560     }
 561     else
 562     {
 563         pOp = _stp_btm_get_free_op(stp_btm);
 564         if (!pOp)
 565         {
 566             STP_BTM_WARN_FUNC("get_free_lxop fail \n");
 567             return -1;//break;
 568         }
 569         pOp->op.opId = STP_OPID_BTM_RETRY;
 570         pOp->signal.timeoutValue= 0;
 571         bRet = _stp_btm_put_act_op(stp_btm, pOp);
 572         STP_BTM_DBG_FUNC("OPID(%d) type(%d) bRet(%d) \n\n",
 573             pOp->op.opId,
 574             pOp->op.au4OpData[0],
 575             bRet);
 576         retval = (0 == bRet) ? STP_BTM_OPERATION_FAIL : STP_BTM_OPERATION_SUCCESS;
 577     }
 578     return retval;
 579 }
 580
 581
 582 static inline INT32 _stp_btm_notify_coredump_timeout_wq(MTKSTP_BTM_T *stp_btm){
 583
 584     P_OSAL_OP       pOp;
 585     INT32           bRet;
 586     INT32 retval;
 587
 588     if (!stp_btm) {
 589         return STP_BTM_OPERATION_FAIL;
 590     } else {
 591         pOp = _stp_btm_get_free_op(stp_btm);
 592         if (!pOp) {
 593             STP_BTM_WARN_FUNC("get_free_lxop fail \n");
 594             return -1;//break;
 595         }
 596         pOp->op.opId = STP_OPID_BTM_DUMP_TIMEOUT;
 597         pOp->signal.timeoutValue= 0;
 598         bRet = _stp_btm_put_act_op(stp_btm, pOp);
 599         STP_BTM_DBG_FUNC("OPID(%d) type(%d) bRet(%d) \n\n",
 600             pOp->op.opId,
 601             pOp->op.au4OpData[0],
 602             bRet);
 603         retval = (0 == bRet) ? STP_BTM_OPERATION_FAIL : STP_BTM_OPERATION_SUCCESS;
 604     }
 605     return retval;
 606 }
 607
 608
 609 static inline INT32 _stp_btm_notify_wmt_dmp_wq(MTKSTP_BTM_T *stp_btm){
 610
 611     P_OSAL_OP     pOp;
 612     INT32         bRet;
 613     INT32 retval;
 614
 615     if(stp_btm == NULL)
 616     {
 617         return STP_BTM_OPERATION_FAIL;
 618     }
 619     else
 620     {
 621         pOp = _stp_btm_get_free_op(stp_btm);
 622         if (!pOp)
 623         {
 624             //STP_BTM_WARN_FUNC("get_free_lxop fail \n");
 625             return -1;//break;
 626         }
 627         pOp->op.opId = STP_OPID_BTM_DBG_DUMP;
 628         pOp->signal.timeoutValue= 0;
 629         bRet = _stp_btm_put_act_op(stp_btm, pOp);
 630         STP_BTM_DBG_FUNC("OPID(%d) type(%d) bRet(%d) \n\n",
 631             pOp->op.opId,
 632             pOp->op.au4OpData[0],
 633             bRet);
 634         retval = (0 == bRet) ? STP_BTM_OPERATION_FAIL : STP_BTM_OPERATION_SUCCESS;
 635     }
 636     return retval;
 637 }
 638
 639 INT32 stp_btm_notify_wmt_rst_wq(MTKSTP_BTM_T *stp_btm)
 640 {
 641     return _stp_btm_notify_wmt_rst_wq(stp_btm);
 642 }
 643
 644 INT32 stp_btm_notify_stp_retry_wq(MTKSTP_BTM_T *stp_btm)
 645 {
 646     return _stp_btm_notify_stp_retry_wq(stp_btm);
 647 }
 648
 649 INT32 stp_btm_notify_coredump_timeout_wq(MTKSTP_BTM_T *stp_btm)
 650 {
 651     return _stp_btm_notify_coredump_timeout_wq(stp_btm);
 652 }
 653
 654 INT32 stp_btm_notify_wmt_dmp_wq(MTKSTP_BTM_T *stp_btm)
 655 {
 656     return _stp_btm_notify_wmt_dmp_wq(stp_btm);
 657 }
 658
 659 MTKSTP_BTM_T *stp_btm_init(void)
 660 {
 661     INT32 i = 0x0;
 662     INT32 ret =-1;
 663
 664     osal_unsleepable_lock_init(&stp_btm->wq_spinlock);
 665     osal_event_init(&stp_btm->STPd_event);
 666     stp_btm->wmt_notify = wmt_lib_btm_cb;
 667
 668     RB_INIT(&stp_btm->rFreeOpQ, STP_BTM_OP_BUF_SIZE);
 669     RB_INIT(&stp_btm->rActiveOpQ, STP_BTM_OP_BUF_SIZE);
 670
 671     /* Put all to free Q */
 672     for (i = 0; i < STP_BTM_OP_BUF_SIZE; i++)
 673     {
 674          osal_signal_init(&(stp_btm->arQue[i].signal));
 675          _stp_btm_put_op(stp_btm, &stp_btm->rFreeOpQ, &(stp_btm->arQue[i]));
 676     }
 677
 678     /*Generate PSM thread, to servie STP-CORE for packet retrying and core dump receiving*/
 679     stp_btm->BTMd.pThreadData = (VOID *)stp_btm;
 680     stp_btm->BTMd.pThreadFunc = (VOID *)_stp_btm_proc;
 681     osal_memcpy(stp_btm->BTMd.threadName, BTM_THREAD_NAME , osal_strlen(BTM_THREAD_NAME));
 682
 683     ret = osal_thread_create(&stp_btm->BTMd);
 684     if (ret < 0)
 685     {
 686         STP_BTM_ERR_FUNC("osal_thread_create fail...\n");
 687         goto ERR_EXIT1;
 688     }
 689
 690     /* Start STPd thread*/
 691     ret = osal_thread_run(&stp_btm->BTMd);
 692     if(ret < 0)
 693     {
 694         STP_BTM_ERR_FUNC("osal_thread_run FAILS\n");
 695         goto ERR_EXIT1;
 696     }
 697
 698     return stp_btm;
 699
 700 ERR_EXIT1:
 701
 702     return NULL;
 703
 704 }
 705
 706 INT32 stp_btm_deinit(MTKSTP_BTM_T *stp_btm){
 707
 708     UINT32 ret = -1;
 709
 710     STP_BTM_INFO_FUNC("btm deinit\n");
 711
 712     if(!stp_btm)
 713     {
 714         return STP_BTM_OPERATION_FAIL;
 715     }
 716
 717     ret = osal_thread_destroy(&stp_btm->BTMd);
 718     if(ret < 0)
 719     {
 720         STP_BTM_ERR_FUNC("osal_thread_destroy FAILS\n");
 721         return STP_BTM_OPERATION_FAIL;
 722     }
 723
 724     return STP_BTM_OPERATION_SUCCESS;
 725 }
 726
 727
 728 INT32 stp_btm_reset_btm_wq(MTKSTP_BTM_T *stp_btm)
 729 {
 730     UINT32 i = 0;
 731     osal_lock_unsleepable_lock(&(stp_btm->wq_spinlock));
 732     RB_INIT(&stp_btm->rFreeOpQ, STP_BTM_OP_BUF_SIZE);
 733     RB_INIT(&stp_btm->rActiveOpQ, STP_BTM_OP_BUF_SIZE);
 734     osal_unlock_unsleepable_lock(&(stp_btm->wq_spinlock));
 735     /* Put all to free Q */
 736     for (i = 0; i < STP_BTM_OP_BUF_SIZE; i++)
 737     {
 738          osal_signal_init(&(stp_btm->arQue[i].signal));
 739          _stp_btm_put_op(stp_btm, &stp_btm->rFreeOpQ, &(stp_btm->arQue[i]));
 740     }
 741
 742     return 0;
 743 }
 744
 745
 746 INT32 stp_notify_btm_dump(MTKSTP_BTM_T *stp_btm)
 747 {
 748     //printk("%s:enter++\n",__func__);
 749         if(NULL == stp_btm)
 750         {
 751             osal_dbg_print("%s: NULL POINTER\n",__func__);
 752             return -1;
 753         }
 754         else
 755         {
 756             gDumplogflag = 1;
 757             osal_trigger_event(&stp_btm->STPd_event);
 758             return 0;
 759   }
 760 }
 761