Bug Fix: PDK-3358

[keystone-rtos/fvid2.git] / src / fvid2_drvMgr.c

diff --git a/src/fvid2_drvMgr.c b/src/fvid2_drvMgr.c
index c709238313b250bc43614af4880a5b9382e7e9d3..17d9fb2245aa0f7537cbdb3c6ed35e1cb5835cca 100755 (executable)
--- a/src/fvid2_drvMgr.c
+++ b/src/fvid2_drvMgr.c
@@ -141,6 +141,8 @@ typedef struct
     /**< FDM Channel objects. */
     SemaphoreP_Handle   lockSem;
     /**< Semaphore to protect function calls and other memory allocation. */
     /**< FDM Channel objects. */
     SemaphoreP_Handle   lockSem;
     /**< Semaphore to protect function calls and other memory allocation. */

+    SemaphoreP_Handle   printSem;
+    /**< Semaphore to protect print buffer. */

 } Fdm_Object;
 
 #if defined (FVID2_CFG_TRACE_ENABLE)
 } Fdm_Object;
 
 #if defined (FVID2_CFG_TRACE_ENABLE)


@@ -508,6 +510,15 @@ int32_t Fvid2_init(const Fvid2_InitPrms *initPrms)
         GT_0trace(Fvid2Trace, GT_ERR, "FVID2 semaphore create failed!!\r\n");
         retVal = FVID2_EALLOC;
     }
         GT_0trace(Fvid2Trace, GT_ERR, "FVID2 semaphore create failed!!\r\n");
         retVal = FVID2_EALLOC;
     }

+    SemaphoreP_Params_init(&params);
+    params.mode = SemaphoreP_Mode_BINARY;
+    initValue       = 1;
+    gFdmObj.printSem = SemaphoreP_create(initValue, &params);
+    if (NULL == gFdmObj.printSem)
+    {
+        GT_0trace(Fvid2Trace, GT_ERR, "FVID2 semaphore create failed!!\r\n");
+        retVal = FVID2_EALLOC;
+    }

 
     /* Free-up memory if error occurs */
     if (FVID2_SOK != retVal)
 
     /* Free-up memory if error occurs */
     if (FVID2_SOK != retVal)


@@ -538,6 +549,11 @@ int32_t Fvid2_deInit(void *args)
         SemaphoreP_delete(gFdmObj.lockSem);
         gFdmObj.lockSem = NULL;
     }
         SemaphoreP_delete(gFdmObj.lockSem);
         gFdmObj.lockSem = NULL;
     }

+    if (NULL != gFdmObj.printSem)
+    {
+        SemaphoreP_delete(gFdmObj.printSem);
+        gFdmObj.printSem = NULL;
+    }

 
     return (retVal);
 }
 
     return (retVal);
 }


@@ -1450,10 +1466,9 @@ void Fvid2_printf(const char *format, ...)
 {
 #if defined (FVID2_CFG_TRACE_ENABLE)
     va_list     vaArgPtr;
 {
 #if defined (FVID2_CFG_TRACE_ENABLE)
     va_list     vaArgPtr;

-    uintptr_t   cookie;

     char       *buf = NULL;
 
     char       *buf = NULL;
 

-    cookie = HwiP_disable();
+    SemaphoreP_pend(gFdmObj.printSem, SemaphoreP_WAIT_FOREVER);

 
     buf = &gRemoteDebug_serverObj.printBuf[0];
     va_start(vaArgPtr, format);
 
     buf = &gRemoteDebug_serverObj.printBuf[0];
     va_start(vaArgPtr, format);


@@ -1461,12 +1476,12 @@ void Fvid2_printf(const char *format, ...)
               (const char *) format, vaArgPtr);
     va_end(vaArgPtr);
 
               (const char *) format, vaArgPtr);
     va_end(vaArgPtr);
 

-    HwiP_restore(cookie);
-

     if(gFdmInitPrms.printFxn != NULL)
     {
         gFdmInitPrms.printFxn(buf);
     }
     if(gFdmInitPrms.printFxn != NULL)
     {
         gFdmInitPrms.printFxn(buf);
     }

+
+    SemaphoreP_post(gFdmObj.printSem);

 #endif
     return;
 }
 #endif
     return;
 }