(VITA) Ugly blocking audio driver fix

audio/drivers/psp_audio.c

@@ -17,6 +17,7 @@
 
 #include <stdint.h>
 #include <malloc.h>
+#include <stdio.h>
 
 #ifdef VITA
 #include <psp2/kernel/processmgr.h>
@@ -34,14 +35,22 @@
 typedef struct psp_audio
 {
    bool nonblocking;
+
    uint32_t* buffer;
    uint32_t* zeroBuffer;
+
    SceUID thread;
    int rate;
 
    volatile bool running;
    volatile uint16_t readPos;
    volatile uint16_t writePos;
+   
+#ifdef VITA 
+   char lock[32] __attribute__ ((aligned (8)));
+   char cond_lock[32] __attribute__ ((aligned (8)));
+   char cond[32] __attribute__ ((aligned (8)));
+#endif
 } psp_audio_t;
 
 #define AUDIO_OUT_COUNT 512u
@@ -55,6 +64,9 @@ typedef struct psp_audio
 #define sceKernelGetThreadInfo sceKernelReferThreadRunStatus
 #endif
 
+int sceKernelLockLwMutex(void*, int, int);
+int sceKernelWaitLwCond(void*, int);
+
 static int audioMainLoop(SceSize args, void* argp)
 {
    psp_audio_t* psp = *((psp_audio_t**)argp);
@@ -68,18 +80,37 @@ static int audioMainLoop(SceSize args, void* argp)
 
    while (psp->running)
    {
+#ifdef VITA
+      
+      //sys_event_queue_receive(id, &event, SYS_NO_TIMEOUT);
+
+      sceKernelLockLwMutex(&psp->lock, 1, 0);
+      uint16_t readPos = psp->readPos;
+      uint16_t readPos2 = psp->readPos;
+      bool cond        = ((uint16_t)(psp->writePos - readPos) & AUDIO_BUFFER_SIZE_MASK)
+            < (AUDIO_OUT_COUNT * 2);
+      if (!cond)
+      {
+         readPos += AUDIO_OUT_COUNT;
+         readPos &= AUDIO_BUFFER_SIZE_MASK;
+         psp->readPos = readPos;
+      }
+      sceKernelUnlockLwMutex(&psp->lock, 1);
+      sceKernelSignalLwCond(&psp->cond);
+      
+      sceAudioOutOutput(port,
+        cond ? (psp->zeroBuffer)
+              : (psp->buffer + readPos2));
+              
+      
+#else
       /* Get a non-volatile copy. */
       uint16_t readPos = psp->readPos;
       bool cond        = ((uint16_t)(psp->writePos - readPos) & AUDIO_BUFFER_SIZE_MASK)
             < (AUDIO_OUT_COUNT * 2);
 
-#ifdef VITA
-      sceAudioOutOutput(port,
-            cond ? psp->zeroBuffer : (psp->buffer + readPos));
-#else
       sceAudioSRCOutputBlocking(PSP_AUDIO_VOLUME_MAX, cond ? (psp->zeroBuffer)
             : (psp->buffer + readPos));
-#endif
 
       if (!cond)
       {
@@ -87,6 +118,9 @@ static int audioMainLoop(SceSize args, void* argp)
          readPos &= AUDIO_BUFFER_SIZE_MASK;
          psp->readPos = readPos;
       }
+
+#endif
+
    }
 
 #ifdef VITA
@@ -109,11 +143,11 @@ static void *psp_audio_init(const char *device,
    (void)device;
    (void)latency;
 
+
    /* Cache aligned, not necessary but helpful. */
    psp->buffer      = (uint32_t*)
       memalign(64, AUDIO_BUFFER_SIZE * sizeof(uint32_t));
    memset(psp->buffer, 0, AUDIO_BUFFER_SIZE * sizeof(uint32_t));
-
    psp->zeroBuffer  = (uint32_t*)
       memalign(64, AUDIO_OUT_COUNT   * sizeof(uint32_t));
    memset(psp->zeroBuffer, 0, AUDIO_OUT_COUNT * sizeof(uint32_t));
@@ -122,14 +156,18 @@ static void *psp_audio_init(const char *device,
    psp->writePos    = 0;
    psp->rate        = rate;
 #if defined(VITA)
+
+   sceKernelCreateLwMutex(&psp->lock, "audio_get_lock", 0, 0, 0);
+   sceKernelCreateLwMutex(&psp->cond_lock, "audio_get_cond_lock", 0, 0, 0);
+   sceKernelCreateLwCond(&psp->cond, "audio_get_cond", 0, &psp->cond_lock, 0);
    psp->thread      = sceKernelCreateThread
       ("audioMainLoop", audioMainLoop, 0x10000100, 0x10000, 0, 0, NULL);
 #else
    psp->thread      = sceKernelCreateThread
       ("audioMainLoop", audioMainLoop, 0x08, 0x10000, 0, NULL);
 #endif
-   psp->nonblocking = false;
 
+   psp->nonblocking = false;
    psp->running     = true;
    sceKernelStartThread(psp->thread, sizeof(psp_audio_t*), &psp);
 
@@ -146,23 +184,61 @@ static void psp_audio_free(void *data)
    psp->running    = false;
 #if defined(VITA)
    sceKernelWaitThreadEnd(psp->thread, NULL, &timeout);
+   sceKernelDeleteLwMutex(&psp->lock);
+   sceKernelDeleteLwMutex(&psp->cond_lock);
+   sceKernelDeleteLwCond(&psp->cond);
 #else
    sceKernelWaitThreadEnd(psp->thread, &timeout);
 #endif
-   sceKernelDeleteThread(psp->thread);
-
    free(psp->buffer);
+   sceKernelDeleteThread(psp->thread);
    free(psp->zeroBuffer);
    free(psp);
 }
 
 static ssize_t psp_audio_write(void *data, const void *buf, size_t size)
 {
-   uint16_t sampleCount;
    psp_audio_t* psp = (psp_audio_t*)data;
+   
+   uint16_t sampleCount;
    uint16_t writePos = psp->writePos;
 
    sampleCount= size / sizeof(uint32_t);
+       
+#ifdef VITA
+   if (psp->nonblocking)
+   {
+      if (AUDIO_BUFFER_SIZE - ((uint16_t)
+            (psp->writePos - psp->readPos) & AUDIO_BUFFER_SIZE_MASK) < size)
+         return 0;
+   }
+
+   while (AUDIO_BUFFER_SIZE - ((uint16_t)
+         (psp->writePos - psp->readPos) & AUDIO_BUFFER_SIZE_MASK) < size){
+      sceKernelWaitLwCond(&psp->cond, 0);
+   }
+
+   sceKernelLockLwMutex(&psp->lock, 1, 0);
+   if((writePos + sampleCount) > AUDIO_BUFFER_SIZE)
+   {
+      memcpy(psp->buffer + writePos, buf,
+            (AUDIO_BUFFER_SIZE - writePos) * sizeof(uint32_t));
+      memcpy(psp->buffer, (uint32_t*) buf +
+            (AUDIO_BUFFER_SIZE - writePos),
+            (writePos + sampleCount - AUDIO_BUFFER_SIZE) * sizeof(uint32_t));
+   }
+   else
+      memcpy(psp->buffer + writePos, buf, size);
+
+   writePos  += sampleCount;
+   writePos  &= AUDIO_BUFFER_SIZE_MASK;
+   psp->writePos = writePos;
+   sceKernelUnlockLwMutex(&psp->lock, 1);
+   
+  return size;
+#else
+
+  
 
 #if 0
    if (psp->nonblocking)
@@ -187,6 +263,7 @@ static ssize_t psp_audio_write(void *data, const void *buf, size_t size)
    psp->writePos = writePos;
 
    return sampleCount;
+#endif
 }
 
 static bool psp_audio_alive(void *data)
@@ -258,16 +335,27 @@ static bool psp_audio_use_float(void *data)
 
 static size_t psp_write_avail(void *data)
 {
+#ifdef VITA
+   psp_audio_t* psp = (psp_audio_t*)data;
+   size_t val;
+
+   sceKernelLockLwMutex(&psp->lock, 1, 0);
+   val = AUDIO_BUFFER_SIZE - ((uint16_t)
+         (psp->writePos - psp->readPos) & AUDIO_BUFFER_SIZE_MASK);
+   sceKernelUnlockLwMutex(&psp->lock, 1);
+   return val;
+#else
    /* TODO */
    psp_audio_t* psp = (psp_audio_t*)data;
    return AUDIO_BUFFER_SIZE - ((uint16_t)
          (psp->writePos - psp->readPos) & AUDIO_BUFFER_SIZE_MASK);
+#endif
 }
 
 static size_t psp_buffer_size(void *data)
 {
    /* TODO */
-   return AUDIO_BUFFER_SIZE;
+   return AUDIO_BUFFER_SIZE /** sizeof(uint32_t)*/;
 }
 
 
@@ -288,5 +376,5 @@ audio_driver_t audio_psp = {
    NULL,
    NULL,
    psp_write_avail,
-   psp_buffer_size,
+   psp_buffer_size
 };