From 4cf9e1ac0f0f6133baf4e8136e600ea51aaab65d Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Wed, 16 Mar 2022 11:11:42 -0700
Subject: Return if add is successful

---
 include/sys/schedule.h |  2 +-
 src/sys/schedule.c     | 16 ++++++++++++----
 2 files changed, 13 insertions(+), 5 deletions(-)

diff --git a/include/sys/schedule.h b/include/sys/schedule.h
index eb9d251..3015e98 100644
--- a/include/sys/schedule.h
+++ b/include/sys/schedule.h
@@ -53,7 +53,7 @@ struct Scheduler {
 };
 
 void init_scheduler(void);
-void add_thread(void* pc, void* arg, unsigned char priority);
+unsigned char add_thread(void* pc, void* arg, unsigned char priority);
 void uart_scheduler(void);
 struct Thread* next_thread(void);
 extern void schedule(void);
diff --git a/src/sys/schedule.c b/src/sys/schedule.c
index 287091c..fe919e6 100644
--- a/src/sys/schedule.c
+++ b/src/sys/schedule.c
@@ -80,7 +80,7 @@ struct Thread* get_unused_thread(void)
 	return 0;
 }
 
-void add_thread(void* pc, void* arg, unsigned char priority)
+unsigned char add_thread(void* pc, void* arg, unsigned char priority)
 {
 	struct Thread* thread = get_unused_thread();
 	thread_table[thread->offset] = 1;
@@ -98,28 +98,36 @@ void add_thread(void* pc, void* arg, unsigned char priority)
 	if (nextpid < FIRST_AVAIL_PID) {
 		nextpid = FIRST_AVAIL_PID;
 	}
+	// Cap Priority Level
 	if (priority >= PRIORITIES)
 		thread->priority = PRIORITIES - 1;
 	else
 		thread->priority = priority;
+	// This thread is new
 	thread->old_priority = -1;
+	// Reserved for non-preemptible tasking
 	thread->preempt = 0;
 	// Get the Ready Queue
 	struct ThreadQueue* ready_queue = &scheduler.ready[thread->priority];
 	// Get the write pointer
 	struct ThreadEntryIterator* ready_write = &ready_queue->write;
-	// TODO: Check if it is possible to add
-	//if (ready_write->queue->thread == 0) {
+	if (ready_write->entry->thread == 0) {
 		// Add the thread to the write pointer
 		ready_write->entry->thread = thread;
 		// Move the write pointer to the next entry
 		ready_write->entry = ready_write->entry->next;
-	//}
+	} else {
+		// Cannot add, mark the stack space as available
+		//  so that it can be used on a subsequent add thread
+		thread_table[thread->offset] = 0;
+		return 1;
+	}
 	// Schedule if this was called in usermode
 	unsigned long mode = getmode() & 0x1F;
 	if (mode == 0x10) {
 		sys0(SYS_YIELD_HIGH);
 	}
+	return 0;
 }
 
 void uart_scheduler(void)
-- 
cgit v1.2.1