2 files changed, 45 insertions, 16 deletions

diff --git a/flash.c b/flash.c
index a46a511..c5d5244 100644
--- a/flash.c
+++ b/flash.c
@@ -29,15 +29,24 @@
 #define PERSIST_ERR_COULDNTWRITE 9
 #define PERSIST_ERR_COUNDNTOPENFILE 10
 
+// flash writing queue bits
+#define MAXQUEUEDJOBS 1 // This makes the queue a little bit pointless
+						// but you might want to queue a few more
+
 typedef struct {
 	FlashStruct *mem;
 	int addr;
 	int numbytes;
 } userflashjob;
 
+
 static bool alive = true;
+static GMutex writermutex;
+static GCond writerwakeup;
+static GCond writerqueueopen;
 static GThread* userflashwritethread;
 static GAsyncQueue* userflashwritequeue;
+//
 
 // crc32 routine
 
@@ -340,14 +349,20 @@ void writeUserBlockNow(FlashStruct *mem, int addr, int numbytes)
 
 void writeUserBlock(FlashStruct *mem, int addr, int numbytes) {
 	if (alive) {
+		g_mutex_lock(&writermutex);
+		if (g_async_queue_length(userflashwritequeue) >= MAXQUEUEDJOBS) {
+			//printf("Queue closed, waiting\n");
+			g_cond_wait(&writerqueueopen, &writermutex);
+		}
+		g_mutex_unlock(&writermutex);
+
 		userflashjob* job = g_malloc(sizeof(userflashjob));
-		job->mem = mem;
+		job->mem = g_malloc(sizeof(FlashStruct));
+		memcpy(job->mem, mem, sizeof(FlashStruct));
 		job->addr = addr;
 		job->numbytes = numbytes;
 		g_async_queue_push(userflashwritequeue, job);
-		while (g_async_queue_length(userflashwritequeue) > 1) {
-			// spin
-		}
+		g_cond_broadcast(&writerwakeup);
 	}
 }
 
@@ -800,19 +815,33 @@ static void initFlashValues(FlashStruct *mem)
 
 static gpointer userflashwritethreadfunc(gpointer data) {
 
-	printf("userflash write thread start\n");
+	//printf("userflash write thread start\n");
 	while (alive || g_async_queue_length(userflashwritequeue) > 0) { // make sure the last job in the queue gets written
-		GTimeVal whentotimeout;
-		g_get_current_time(&whentotimeout);
-		g_time_val_add(&whentotimeout, 250 * 1000);
-		userflashjob* job = (userflashjob*) g_async_queue_timed_pop(userflashwritequeue, &whentotimeout);
-		if(job != NULL){
-			// process job
-			writeUserBlockNow(job->mem, job->addr, job->numbytes);
-			g_free(job);
+		if (g_async_queue_length(userflashwritequeue) == 0){ // go into a sleep
+			g_mutex_lock(&writermutex);
+			g_cond_wait(&writerwakeup, &writermutex);
+			g_mutex_unlock(&writermutex);
 		}
+		else {
+			userflashjob* job = (userflashjob*) g_async_queue_pop(userflashwritequeue);
+			if (job != NULL ) {
+				// process job
+				//printf("processing write\n");
+				writeUserBlockNow(job->mem, job->addr, job->numbytes);
+				g_free(job->mem);
+				g_free(job);
+			}
+			// If someone is waiting for the queue to start accepting
+			// jobs again and we are tell them about it..
+			if (g_async_queue_length(userflashwritequeue) < MAXQUEUEDJOBS) {
+				g_mutex_lock(&writermutex);
+				g_cond_broadcast(&writerqueueopen);
+				g_mutex_unlock(&writermutex);
+			}
+		}
+
 	}
-	printf("userflash write thread stop\n");
+	//printf("userflash write thread stop\n");
 
 	return NULL ;
 }
@@ -826,6 +855,7 @@ void startFlashWriterThread(){
 void initFlash(FlashStruct *mem, gboolean reset_to_defaults, int starting_location)
 {
 	startFlashWriterThread();
+	atexit(stopFlashWriterThread);
 
 	int read_size = readUserBlock(mem);
 
@@ -858,6 +888,7 @@ void initFlash(FlashStruct *mem, gboolean reset_to_defaults, int starting_locati
 
 void stopFlashWriterThread(){
 	alive = false;
+	g_cond_broadcast(&writerwakeup);
 	g_thread_join(userflashwritethread); // block until the write thread is totally finished.
 }
 
diff --git a/instr-daemon.c b/instr-daemon.c
index 2e67c2c..e110863 100644
--- a/instr-daemon.c
+++ b/instr-daemon.c
@@ -302,8 +302,6 @@ int main(int argc, char **argv)
 
 	g_main_loop_run (loop);
 
-	stopFlashWriterThread();
-
 	bus_shutdown();
 	free(stdinQueue);
 	free(peers);