/*	RLPOS: RLPotter Operating System

		   Version 0.7 for the 68HC12D60A microcontroller
		   by Ryan Potter
		   ryan@rlpotter.com
	
	
	v0.7 April 25, 2003: 11174 bytes:
		   - combined kernel.c & semlib.c with rlpos.c to make one big kernel.c
		   - changed context switch mechanism to give each task a dedicated
		   	 stack, making the kernel context switches >400% faster.
		   - added a basic timer task that will eventually keep system time/date.
		   
	v0.6 April 24, 2003: 11042 bytes
		   - made the task block and the task control block dynamic, so that
		   	 now tasks can create tasks, and can also start/stop them
		   - implemented the sysInit() task for kicking off a user system
		   - implemented a kernel debug command for tracing code execution
		   - implemented small msg box system in kernel for tasks
		   - implemented COP watchdog reset timer
		   - added system resources to semlib
		   - added skeleton ISR_handler() code for the other interrupts
		   - implemented _HC12Setup.c to initialize/harden the system
		   
	v0.5 April 16, 2003:
		   - added priority preemption
		   - finished kernel task state switcher 
		   - it is now officially a legitimate
		   	 	rate monotonic
				priority preemptive
				multitasking
				Real Time Operating System  :)
		   - made kernel.c and semlib.c consistent with the 
		   	 rest of the kernel
		   - included the early framework for a msg box system
			 
	v0.4 April 13, 2003: 8096 bytes
		   - gerneralized the shell command-line input parser: 
		   	 cmd <arg1, arg2, ... argn> (up to 32 chars)
		   - added kernel and shell functions
		   
	v0.3 April 12, 2003: 6155 bytes
		   - added a beginning shell user interface.
		   - added a resource control block and basic semaphore functions.
		   - added basic kernel functions
		   
	v0.2 April 9, 2003:
	  	   - able to round-robin with RTI interrupt.
		   - bonified/certified multitasking with 3 tasks. :)
		   
	v0.1 April 3, 2003:
	  	   - able to round-robin without interrupts.
		   - not multitasking, really.
		   
	v0.0 started April 1, 2003; 0 bytes
		   - no idea where to start.
		   - don't want to look at anyone else's work. ;0)



	
	Architecture/C assumptions:
		1) 'D' register is the accumulator
		2) 'X' register points to the top of the current stack
		3) 'Y' register is for general use in indexed operations
		4) the heap grows upward and mem segments allocated by malloc,
		   realloc, and calloc are linear and contiguous.
		5) the stack grows downwards, and there is no boundary checking.
		   
	Potential problem areas:
		1) 'running' section of the kernel get's compiled using extra
		   push and pop instructions
		2) run out of ram (global + stack + heap)
		
*/
	
	
	
#include <912d60.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "kernel.h"



// FUNCTION PROTOTYPES
void RTI_handler(void);
void sysInit(void);
void shell(void);
void sysTime(void);
void (*task_ptr[MAXTASKS])(void);




// GLOBAL VARIABLES
unsigned char *main_frame_ptr = NULL;	   	// bottom of main() frame
unsigned char *main_frame_x_ptr = NULL;		// top of main() frame (x-reg ptr)
unsigned char *temp_task_frame_ptr = NULL;	// temp CCR pointer for RTI


unsigned int current;		   	 		    // current task id number
unsigned long int system_tick;
unsigned int cop_cycle;

unsigned long int time_tick;
unsigned long int delta_t;
unsigned long int last_time_mark;


// task control block
typedef struct task_block {
	 	void (*address)();			   		// Address of the task
	 	unsigned char id; 			  		// ID of task
		char name[9];	  					// Name
		enum task_state state;				// State
		unsigned char priority;				// Priority
		unsigned long int period_tick;		// for determining if deadline is up
		unsigned int interrupt_msg_box;		// flags for pending interrupts
		enum message_box message;			// misc flags
		unsigned char message_data[2];		// data for misc_msg_box flags
		unsigned int stack_size;			// heap allocated for task stack
	 	unsigned char *top_of_stack;			// top of task stack in the heap
		unsigned char *frame_ptr;			// CCR pointer in idle stack
		};


// resource control block
typedef struct resource_block {
		unsigned char id;	   				// ID of resource
		char name[5];	  					// Name of resource
		enum resource_state state;			// State (busy, free...)
		unsigned char owner;				// Current resource owner
		signed char queue[3];				// Tasks waiting on resource
		unsigned char queue_ptr;			// Next free spot in queue
		};
		
		
struct task_block *task[MAXTASKS];
struct resource_block resource[NUMRESOURCES];


int kdb_trace = 1;
int kdb_trace_cycle = 0;


// global interrupt flags
//unsigned int interrupt_flags_ADC;
//unsigned int interrupt_flags_TC;




// error massages
const char error_msg[][25] = {"unimplimented function",	// error 0 
	 				  	"syntax error",				// error 1 
						"illegal task ID",			// error 2 
						"illegal task state",		// error 3 
						"illegal task priority",	// error 4 
						"out of memory",			// error 5 
						"illegal resource ID",		// error 6
						"illegal resource state",	// error 7
						"cannot create task"		// error 8
						};
const char error_src[][18] = {"kernel error:",			// source 0
	 				  	"kernel.c error:",			// source 1
						"kernel RTI error:",		// source 2
						"shell error:",				// source 3
						"semlib.c error:"			// source 4
						};
						
						



main() {

	 // LOCAL VARIABLES
	 int result, id, i;
	 unsigned char priority_check;
	 unsigned int deadline;
	 extern int _bss_end, _textmode;		   	 		
	  
	  
	 _textmode = 1;	 	  // maps '\n' to "CR/LF" for Windows terminals
	 current = 0;  	 	  // start the shell first
	 cop_cycle = 0;		  // fresh watchdog
	 
	 
	 /* redundant... done by _startup()
	 // initialize the task pointer array and the tcb array.
	 for (i=0; i<MAXTASKS; i++) {
	 	 task_ptr[i] = NULL;
		 task[i] = NULL;
		 }
	 */
		 
		 
		 
	 // initialize the rcb pointer block
	 for (i=0; i<NUMRESOURCES; i++) {
	 	 resource[i].id = i;
		 resource[i].state = NOTBUSY;
		 resource[i].owner = 255;
		 resource[i].queue_ptr = 0;
		 }

	 
	 
 	 // save SP value for use in the RTI
	 asm("TFR s,d");
	 asm("STD _main_frame_ptr");
	 // save X reg value for use in the RTI
	 asm("TFR x,d");
	 asm("STD _main_frame_x_ptr");
	 
	 
	 
	 
	 // START/INITIALIZE the os
		// KDB_TRACE Section 1
	 
	 // create and initialize the heap (for dynamic (runtime) var allocation)
	 _NewHeap(&_bss_end, (char *)(&_bss_end) + (INITIAL_HEAP_SIZE));
	 
	 
	 // set up the serial port
	 setbaud(BAUD38K);	 	  // actually running at 19K baud due to xtal speed
	 #ifdef KDB_TRACE_LEVEL_1
		if (kdb_trace) puts("kDB1-0");	 
	 #endif
	 

	 // create the sysInit task
	 if (create_task("sysInit", &sysInit, 0, PENDING, 0) < 0)
	 	puts("sysInit failure");
	 #ifdef KDB_TRACE_LEVEL_1
		if (kdb_trace) puts("kDB1-1");	 
	 #endif 


		
	 // create the shell
	 if (create_task("shell", &shell, 0, PENDING, DEFAULT_SHELL_STACK_SIZE) < 0)
	 	puts("shell failure");
	 #ifdef KDB_TRACE_LEVEL_1
		if (kdb_trace) puts("kDB1-2");	 
	 #endif

		
		
	 /*// create sysTime
	 create_task("sysTime", &sysTime, 5, PENDING);
	 #ifdef KDB_TRACE_LEVEL_1
		if (kdb_trace) puts("kDB1-3");	 
	 #endif*/
	 

	 
	 // service the COP and system tick
	 COP_PET(0x55);
	 COP_PET(0xAA);
	 system_tick = 0;
	 
	 
	 // print the opening comments
	 puts("rlpOS v0.7\n\n");

	 

	 
	 // MULTITASKING KERNEL: Priority Preemptive, Real Time, multitasking 
		// KDB_TRACE Section 2
	 while(1) {	 
	 	
		//-------------------------------
		/* REENTRY POINT after either 
		   	   1) task finishes, or 
		   	   2) RTI
		*/
		//-------------------------------
		
		
		if (kdb_trace_cycle >= KDB_CYCLES)
		   kdb_trace = 0;
		
		#ifdef KDB_TRACE_LEVEL_1
			if (kdb_trace) puts("kDB2.1");	 
		#endif
		   
		   		
		/* No interrupts allowed inside of main().  Only allowed
		   inside of non-critical sections of tasks */ 
	 	INTR_OFF();	
		
		
		// PET THE DOG: cop watchdog reset timer (pet freq = 2x COP freq)
		if (cop_cycle == 4) {
		   COP_PET(0x55);
		   COP_PET(0xAA);
		   cop_cycle = 0;
		   }
		#ifdef KDB_TRACE_LEVEL_1
			if (kdb_trace) puts("kDB2.2-1");	 
		#endif
		
		
		
		// CHANGE STATES ACCORDING TO MESSAGES
		// 'waiting' needs to have highest precedence here
		for (i=0; i<MAXTASKS; i++) {
			if (task[i] == NULL)
			   continue;
			if (task[i]->message && STATE_FLAG) {  
			   // set task state to what the message says
		   	   task[i]->state = task[i]->message_data[STATE_BOX];
			   // clear the STATE_FLAG
			   task[i]->message &= ~(STATE_FLAG);
			   #ifdef KDB_TRACE_LEVEL_2
				if (kdb_trace) puts("kDB2.3-2a");	 
			   #endif
			   }
			#ifdef KDB_TRACE_LEVEL_2
				if (kdb_trace) puts("kDB2.3-2b");	 
			#endif
			}
		#ifdef KDB_TRACE_LEVEL_1
			if (kdb_trace) puts("kDB2.3-1");	 
		#endif
		
			

		// RT PRIORITY BLOCK:
		// set the current task id based on priority and deadline
		
		// determine if the deadline is up for idle tasks
		/* deadline, is equal to period plus an initial time (t0) reference
		   (t0 = system_tick).  Period = priority + 1.  
		   Changing the state from idle to PENDING occurs here.  */
		for (i=0; i<MAXTASKS; i++) {
			if (task[i]->state == IDLE) {
			   deadline = task[i]->period_tick + (task[i]->priority + 1);
			   if (system_tick >= deadline) {
			   	  task[i]->state = PENDING; 	  // change state at deadline
				  #ifdef KDB_TRACE_LEVEL_2
					if (kdb_trace) puts("kDB2.4-2");	 
				  #endif
			   	  } 	
				}
			 // if (task[i]->message) {} ???		
			 }
		#ifdef KDB_TRACE_LEVEL_1
			if (kdb_trace) puts("kDB2.4-1");	 
		#endif			
		
		
		// set current = to highest priority pending/running task.
		priority_check = 255;   	  	  // lowest possible
		
		for (i=0; i<MAXTASKS; i++) {
			if (!(task[i] == NULL)) {
			   if ((task[i]->state == PENDING) || (task[i]->state == RUNNING)) {
			   	  if (task[i]->priority <= priority_check) {
				  	 current = i;
				 	 priority_check = task[i]->priority;
				 	 #ifdef KDB_TRACE_LEVEL_2
						if (kdb_trace) puts("kDB2.5-2");	 
				  	 #endif 
			   	 	 }
			   	   }
				}
			 }
		#ifdef KDB_TRACE_LEVEL_1
			if (kdb_trace) puts("kDB2.5-1");	 
		#endif
		
		
		
		// DISPATCH, or otherwise deal with the current task
			// KDB_TRACE Section 3

		#ifdef KDB_TRACE_LEVEL_2
		if (kdb_trace) {
			printf("task[%d]->state = %d\n", current, task[current]->state);
			printf("task[%d]->prior = %d\n", current, task[current]->priority);
			}
		#endif
		
		switch (task[current]->state) {
			   case IDLE:			   // skip task
					#ifdef KDB_TRACE_LEVEL_1
						if (kdb_trace) puts("kDB3.1-1");
					#endif
					break;
			   case PENDING:		   // ready and waiting to run
					#ifdef KDB_TRACE_LEVEL_1
						if (kdb_trace) puts("kDB3.2-1");
					#endif
					#ifdef KDB_TRACE_LEVEL_2
						if (kdb_trace) {
							//puts("kDB3.2-2");
							putchar('S');
							putchar(current+48);
							putchar('\n');
						    }
					#endif  
					task[current]->state = RUNNING;
					task[current]->period_tick = system_tick;
					temp_task_frame_ptr = task[current]->top_of_stack;
					asm("LDS _temp_task_frame_ptr");		// set the SP
			   		(*task_ptr[current])();	 	  			// start the task
					asm("LDS _main_frame_ptr");				// reset the SP
					task[current]->state = IDLE;			// task finished
					#ifdef KDB_TRACE_LEVEL_1
						if (kdb_trace) puts("kDB3.3-1");
					#endif
					#ifdef KDB_TRACE_LEVEL_2
						if (kdb_trace) {
							//puts("kDB3.3-2");
							putchar('F');
							putchar(current+48);
							putchar('\n');
						    }
					#endif  				
			   		break;
			   case RUNNING:			// interrupted. continue running.
					// restore context and run
					temp_task_frame_ptr = task[current]->frame_ptr;
					asm("LDS _temp_task_frame_ptr");
					asm("RTI");
			   		break;
			   case WAITING:   		   // waiting on a resource
					#ifdef KDB_TRACE_LEVEL_1
						if (kdb_trace) puts("kDB3.5-1");
					#endif
			   		break;
			   case STOPPED:		   // done running until later
					#ifdef KDB_TRACE_LEVEL_1
						if (kdb_trace) puts("kDB3.6-1");
					#endif
			   		break;
			   default:	  			   // shouldn't happen, but, error if so.
			   		puts("KERNEL: task state error\n");
			   		exit(1);
				}	// end switch
		
	 		}		// end while(1)
	 
	 return 0;
	 
}			 		// end main()



#pragma interrupt_handler RTI_handler()

void RTI_handler(void) {

	 size_t frame_size;
	 unsigned int i;
	 unsigned char *local_thp;
	 
	 
	 //ACKNOWLEDGE THE INTERRUPT
	 RTIFLG = 0x0080;  	// acknowledge/clear the interrupt
	 
	 
	 #ifdef KDB_TRACE_LEVEL_1
	  	if (kdb_trace) putchar('.');
	 #endif
	 
	 
	 // SET THE FRAME POINTER for the interupted task.
	 // should point to the CCR entry on the stack.
	 asm("TFR x,d");  		  	  	  	 // start of RTI stack
	 asm("ADDD #2");				  	 // adjust to CCR stack entry
	 asm("STD _temp_task_frame_ptr");	 // put into task_frame_ptr
	 task[current]->frame_ptr = temp_task_frame_ptr;
	 

	 
	 // update system time base and cop reset counter
	 system_tick++;
	 cop_cycle++;
	 
	 delta_t = (system_tick - last_time_mark) * TICK_SIZE;
	 
			  	 last_mark = system_tick;
			  	 sl++;
	 
	 
	 // RETURN: simulate a RTS instruction
	 // set stack pointer for main()
	 asm("LDS _main_frame_ptr");
	 asm("LDX _main_frame_x_ptr");
	 /* return to main() at the reentry point
	    and must be adjusted after each kernel mod/compilation!!  */
	 asm("JMP $125B");
	 

	 // return (0)  -- NOT used 
	 /* this ISR should never use the 'return x' command.
	    It doesn't make sense since it always interrupts a Task, but
	    never returns to it... but to main() instead.  */
	 	 
}





// KERNEL FUNCTIONS

int get_task_address(char id) {

	return (int)task[id]->address;

}




char get_task_id() { 
	 
	 return task[current]->id;
}



char *get_task_name(unsigned char id) {
	 if (task[id] == NULL)
	 	return NULL;
	 else
	 	return task[id]->name; 
}




int get_task_state(unsigned char id) { 

	if (task[id] == NULL)
	   return -1;
	else if (id < MAXTASKS)
	 	return task[id]->state;
	else {
	 	  #ifdef KERNEL_ERROR_MSG
	 	  puts(error_src[1]); puts(error_msg[3]);
		  #endif
		  return -1;
		  }
		  
}



int get_task_messages(unsigned char id) {

	if (task[id] == NULL)
	   return -1;
	else if (id < MAXTASKS)
	   return task[id]->message;
	else {
		 #ifdef KERNEL_ERROR_MSG
	 	 puts(error_src[1]); puts(error_msg[3]);
		 #endif
		 return -1;
		 }

}



int get_task_priority(unsigned char id) { 

		 INTR_OFF();
		 
		 if (id < MAXTASKS) {
		 	INTR_ON();
		 	return task[id]->priority;
			}
		 else {
	 	 	#ifdef KERNEL_ERROR_MSG
	 	 	puts(error_src[1]); puts(error_msg[4]);
			#endif
			INTR_ON();
			return -1;
		 	}

}



int set_task_state(unsigned char id, unsigned char newstate) {
	
	
	INTR_OFF();
	
	
	// check id validity
	if (id < MAXTASKS) {

	   if (task[id] == NULL) {
	   	  #ifdef KERNEL_ERROR_MSG
	 	  puts(error_src[1]); puts(error_msg[2]);
		  #endif
		  INTR_ON();
	 	  return -1;
	   	  }
		  
		  
	   // check newstate validity
	   if ((newstate == RUNNING) || 
	   	  (newstate == WAITING)  ||
	   	  (newstate > STOPPED)) { 
	      		 #ifdef KERNEL_ERROR_MSG
	 	 	  	 puts(error_src[1]); puts(error_msg[3]);
			  	 #endif
				 INTR_ON();
	 	  		 return -1;
	   			 }
	 
	 		// referenced to the task's current state:
	   		switch (task[id]->state) {
		   	  	   case IDLE:
				   		if (newstate == IDLE) {	   // no change
						   INTR_ON();
				 		   return 0;
						   }
				 		else if (newstate == PENDING) {
				 		   // pass msg to task[id]->msgbox;
						   task[id]->message |= STATE_FLAG;
		  				   task[id]->message_data[STATE_BOX] = PENDING;
						   INTR_ON();
					  	   return 0;
				 		   }
						else if (newstate == STOPPED) {
							// pass msg to task[id]->msgbox;
						    task[id]->message |= STATE_FLAG;
		  					task[id]->message_data[STATE_BOX] = STOPPED;
							INTR_ON();
					  		return 0; 
						    }
				 		else {	  // nothing else is legal;
						   	 #ifdef KERNEL_ERROR_MSG
	 	 	  			   	 puts(error_src[1]); puts(error_msg[3]);
			  			   	 #endif
							 INTR_ON();
					  		 return -1;
				 			 }
				 		break;
		   			case PENDING:
						 if (newstate == PENDING) {	// no change
						 	INTR_ON();
				 		 	return 0;
							}
						 else if (newstate == IDLE) {
				   		 	  // pass msg to task[id]->msgbox;
						   	  #ifdef KERNEL_ERROR_MSG
	 	 	  			   	  puts(error_src[1]); puts(error_msg[0]);
			  			   	  #endif
							  INTR_ON();
					  		  return -1;
							  }
						 else if (newstate == STOPPED) {
				   		 	  	 // pass msg to task[id]->msgbox;
							  	 task[id]->message |= STATE_FLAG;
		  					  	 task[id]->message_data[STATE_BOX] = STOPPED;
							  	 INTR_ON();
					  		  	 return 0;
						 	  	 }	
						 else {
				   		 	  #ifdef KERNEL_ERROR_MSG
							  puts(error_src[1]); puts(error_msg[3]);
							  #endif
							  INTR_ON();
							  return -1;
						 	  }
						 break;
					case RUNNING:
						 if (newstate == STOPPED) {
						 	// pass msg to task[id]->msgbox;
							task[id]->message |= STATE_FLAG;
		  					task[id]->message_data[STATE_BOX] = STOPPED;
							
							INTR_ON();
					  		return 0;
						 	}
						 else {
				   		 	  #ifdef KERNEL_ERROR_MSG
							  puts(error_src[1]); puts(error_msg[3]);
							  #endif
							  INTR_ON();
							  return -1;
						 	  }
						 break;
		   			case STOPPED:
		   				 if (newstate == STOPPED)	{ 	// no change
				   		 	INTR_ON();
							return 0;
							}
						 else if (newstate == PENDING) {
				   		 	  // pass msg to task[id]->msgbox;
							  task[id]->message |= STATE_FLAG;
		  					  task[id]->message_data[STATE_BOX] = PENDING;
							  INTR_ON();
					  		  return 0;
							  }
						 else {
					 	 	  #ifdef KERNEL_ERROR_MSG
							  puts(error_src[1]); puts(error_msg[3]);
							  #endif
							  INTR_ON();
							  return -1;
							  }
						 break;
		   			default:
		   		   			#ifdef KERNEL_ERROR_MSG
							puts(error_src[1]); puts(error_msg[3]);
							#endif
							INTR_ON();
							return -1;
				   			break;
							
	 				}	// end switch (task[id]->state)
					
	 		}		// if (id < MAXTASKS)
			
		   else {
		   		#ifdef KERNEL_ERROR_MSG
				puts(error_src[1]); puts(error_msg[2]);
				#endif
				INTR_ON();
				return -1;
				}

	 INTR_ON();
	 return 0;

}



int set_task_priority(unsigned char id, unsigned char priority) {

	 if (priority == 0) 			 // priority 0 is reserved for the shell
	 	priority = 1;
		
	 if (id >= (MAXTASKS)) {
	 	#ifdef KERNEL_ERROR_MSG
		puts(error_src[1]); puts(error_msg[4]);
		#endif
		return -1;
		}
	 else if ((priority > 255) || (priority < 0)) {
	 	#ifdef KERNEL_ERROR_MSG
		puts(error_src[1]); puts(error_msg[4]);
		#endif
		return -1;
		}
	 else {
	 	 task[id]->priority = priority;
		 return 0;
		 }
}




int create_task(char *name, 
				void (*addr)(), 
				unsigned char priority, 
				int state,
				int stack_size) {

	// LOCAL VARIABLES
	unsigned char id;
	
	
	INTR_OFF();	  // critical section	  
	
	
	//if (get_free_memory() > 64) {
	
	// determine lowest free id available to assign to this task
	for (id=0; id<MAXTASKS; id++) {
		if (task_ptr[id] != NULL)
		   continue;
		else
		   break;
		 }

	
	// create task block
	if ((task[id] = malloc(sizeof(struct task_block))) == NULL) {
	   #ifdef KERNEL_ERROR_MSG
	   puts(error_src[1]); puts(error_msg[5]);
	   #endif
	   return -1;
	   }
	else { 
	 	 // put addr entry into task_ptr[]
		 task_ptr[id] = addr;
		 
		 
	 	 // initialize the task structures
	 	 task[id]->address = addr;
		 task[id]->id = id;
		 strcpy(task[id]->name, name);
 		 task[id]->state = state;
		 task[id]->priority = priority;
		 task[id]->period_tick = 0;
		 task[id]->interrupt_msg_box = NULL;
		 task[id]->message = NULL;
		 task[id]->message_data[0] = NULL;
		 task[id]->message_data[1] = NULL;
		 if (stack_size == 0) stack_size = DEFAULT_STACK_SIZE;
		 task[id]->stack_size = stack_size;
		 task[id]->top_of_stack = malloc(task[id]->stack_size); 
		 task[id]->top_of_stack += task[id]->stack_size;
		 task[id]->frame_ptr = NULL;
	
	}
	//}
	INTR_ON();
	 
	return id;
	 
}




int get_resource_state(unsigned char rid) {

	if (rid < NUMRESOURCES) {
	 	return resource[rid].state;
		}
	else {
		 #ifdef KERNEL_ERROR_MSG
	 	 puts(error_src[1]); puts(error_msg[6]);
		 #endif
		 return -1;
		 }

}





int get_resource_owner(unsigned char rid) {

	if (rid < NUMRESOURCES) {
	 	return resource[rid].owner;
		}
	else {
		 #ifdef KERNEL_ERROR_MSG
	 	 puts(error_src[1]); puts(error_msg[6]);
		 #endif
		 return -1;
		 }

}




int get_resource_queuelen(unsigned char rid) {

	if (rid < NUMRESOURCES) {
	 	return resource[rid].queue_ptr;
		}
	else {
		 #ifdef KERNEL_ERROR_MSG
	 	 puts(error_src[1]); puts(error_msg[6]);
		 #endif
		 return -1;
		 }

}




int get_free_memory(void) {

	// LOCAL VARIABLES
	char *memory, *last;
	int i;
	
	
	INTR_OFF();
	// check for largest free memory block
	for (i=0; i<INITIAL_HEAP_SIZE; i+=32) {
		if ((memory = malloc(i)) != NULL)
		   last = memory;
		else
		   break;
		free(last);
		}
		
	INTR_ON();
	//printf("%d bytes\n", i);
	
	return (i-32);

}




int sem_get(char rid) {

	 /* Gives a resource to a requesting task.
	 	returns the resource id number (0,1,2,...) if free.
	 	otherwise returns -1.
		
		At this point, semaphores are a procedural control
		that the tasks have to follow to avoid resource contention.
		There is no kernel control over resources yet.  */
	 
	 
	 // critical section
	 INTR_OFF();
	 
	 
	 
	 // GET SEMAPHORE
	 // give semaphore to task
	 if (resource[rid].state == NOTBUSY) {
	 	resource[rid].state = BUSY;
		resource[rid].owner = current;
		
		INTR_ON();
		return rid;
	 	}
	 
	 // resource is taken/busy so make task wait
	 else {
		  task[current]->message |= STATE_FLAG;
		  task[current]->message_data[STATE_BOX] = WAITING;
		
		  // put waiting task into the resources queue if there's space
		  if (resource[rid].queue_ptr < 3) {
		   	 resource[rid].queue[resource[rid].queue_ptr] = current;
		   	 resource[rid].queue_ptr++;
		   	 }
		
		  INTR_ON();
		  return -1;
	 	  } 	 
		
}




int sem_give(char rid) {

	/* Takes a resource back from a task.
	   will (eventually) pass a message to a waiting task.  
	   
	   	At this point, semaphores are a procedural control
		that the tasks have to follow to avoid resource contention.
		There is no kernel control over resources yet.  */
	
	
	// critical section
	INTR_OFF();
	
	
	// return the resource
	resource[rid].state = NOTBUSY;
	//resource[rid].owner = NULL;
	
	INTR_ON();
	
	return 0;
	  
}