Another attempt to get CMSIS semaphores correct.

2 files changed, 17 insertions, 9 deletions

diff --git a/src/gos/gos_cmsis.c b/src/gos/gos_cmsis.c
index b2f25dd0..bfbafd62 100644
--- a/src/gos/gos_cmsis.c
+++ b/src/gos/gos_cmsis.c
@@ -43,9 +43,9 @@ void gfxSemInit(gfxSem* psem, semcount_t val, semcount_t limit)
 	osSemaphoreDef_t def;
 	def.semaphore = psem->semaphore;
 	
-	psem->id = osSemaphoreCreate(&def, limit);
-	while(val--)
-		osSemaphoreRelease(psem->id);
+	if (val > limit) val = limit;
+	psem->available = limit - val;
+	psem->id = osSemaphoreCreate(&def, val);
 }
 
 void gfxSemDestroy(gfxSem* psem)
@@ -55,22 +55,29 @@ void gfxSemDestroy(gfxSem* psem)
 
 bool_t gfxSemWait(gfxSem* psem, delaytime_t ms)
 {
-	return osSemaphoreWait(psem->id, ms) > 0;
+	if (osSemaphoreWait(psem->id, ms) == osOK) {
+		psem->available++;
+		return TRUE;
+	}
+	return FALSE;
 }
 
 bool_t gfxSemWaitI(gfxSem* psem)
 {
-	return osSemaphoreWait(psem->id, 0) > 0;
+	return gfxSemWait(psem, 0);
 }
 
 void gfxSemSignal(gfxSem* psem)
 {
-	osSemaphoreRelease(psem->id);
+	gfxSemSignalI(psem);
 }
 
 void gfxSemSignalI(gfxSem* psem)
 {
-	osSemaphoreRelease(psem->id);
+	if (psem->available) {
+		psem->available--;
+		osSemaphoreRelease(psem->id);
+	}
 }
 
 gfxThreadHandle gfxThreadCreate(void* stackarea, size_t stacksz, threadpriority_t prio, DECLARE_THREAD_FUNCTION((*fn),p), void* param)
diff --git a/src/gos/gos_cmsis.h b/src/gos/gos_cmsis.h
index d263feb6..85aa1362 100644
--- a/src/gos/gos_cmsis.h
+++ b/src/gos/gos_cmsis.h
@@ -42,8 +42,9 @@ typedef osPriority			threadpriority_t;
 #define HIGH_PRIORITY		osPriorityHigh
 
 typedef struct gfxSem {
-	uint32_t semaphore[2];
-	osSemaphoreId id;
+	uint32_t		semaphore[2];
+	osSemaphoreId	id;
+	semcount_t		available;
 } gfxSem;
 
 typedef struct gfxMutex {