AVR Freaks

Solution/Error was the way the context switch was built.  See below for the working solution - thankyou freaks.

struct task_ctrl_obj * api_program(void(*task)(void), char const * name, void * variable_pass, unsigned int mpu_mem, unsigned char priority, unsigned char ceiling ){

	api_system_gateway();

	if(priority > 63){
		return(0);
	}

	struct task_ctrl_obj * tcb;

	tcb = safe_malloc(sizeof(struct task_ctrl_obj));			// Allocate Memory for the Task Ctrl Block, used by the Kernel to manage the Task

	memset(tcb, 0x00, sizeof(struct task_ctrl_obj));

	if(tcb == NULL){
		return(NULL);
	}

	unsigned int stack_offset;

	// Allocate Memory for the Stack
	tcb->stack_allocation = (unsigned char *)safe_malloc(mpu_mem * sizeof(int));


	// find the number of bytes allocated
	stack_offset = mpu_mem * sizeof(int);


	if(tcb->stack_allocation == NULL){

		safe_free(tcb);
		return(NULL);
	}

	// PointStack to the end of the memory structure, Stack grows downwards
	unsigned int * cpu_context = (unsigned int *)(stack_offset + tcb->stack_allocation);

	/* Align stack pointer.                                 */
	cpu_context = (unsigned int *)((unsigned int)(cpu_context) & 0xFFFFFFF8u);

	tcb->task_status = DORMANT;	// Initial State of all Tasks is DORMANT


	/*
	 *	Register task name with operating system
	 */
	tcb->name[0] = '\0';

	//if task name has been specified then load the name into the system
	if(strlen(name)){

		struct task_id * log = safe_malloc(sizeof(struct task_id ));
		log->id = tcb;

		int i = 0;	while(* name != '\0'){

				// copy the name
				tcb->name[i] = * name; log->name[i++] = * name; name++;
		}

		// tidy up strings by nulling the end
		tcb->name[i] = '\0'; log->name[i] = '\0';
		add_system_name(log);
	}


	/*
	 * Signal Setup
	 */
	// Setup Signal Receive Buffer, also associated with TCB
	tcb->signals = safe_malloc(sizeof(struct ipc_signal_sent));
	if(tcb->signals == NULL){

		safe_free(tcb);
		safe_free(tcb->stack_allocation);
		safe_free((struct ipc_signal_obj *)tcb->signal_management);
		return(NULL);
	}

	tcb->signal_management->nr_signal_registered = 0;
	tcb->signals->nr_signal_received = 0;



	/*
	 * Setup tcb priority
	 */
	tcb->delta_priority   = priority;		// Set Task Priority
	tcb->ceiling_priority = ceiling;
	tcb->default_priority = priority;		// Set Base Priority, used by the Priority inversion protocol


	unsigned int program = (unsigned int)task & ~1u; /* Mask off lower bit in case task is thumb mode        */               					   /* Mask off lower bit in case task is thumb mode        */
	/*
	 * Setup general purpose registers
	 */
     * --cpu_context = (unsigned int) program;										    /* PC                                                    */
     * --cpu_context = (unsigned int) internal_task_destructor;		                    /* LR: Supervisor mode                                       */
     * --cpu_context = (unsigned int) 0x00000011;                                       /* R1`                                                        */
     * --cpu_context = (unsigned int) 0x00000010;                                       /* R10                                                        */
     * --cpu_context = (unsigned int) 0x00000009;                                       /* R09				                                           */
     * --cpu_context = (unsigned int) 0x00000008;		                                /* R8							    		                   */
     * --cpu_context = (unsigned int) 0x00000007;                                       /* R7                                                        */
     * --cpu_context = (unsigned int) 0x00000006;									    /* R6				                                           */
     * --cpu_context = (unsigned int) 0x00000005;                                       /* R5                                                        */
     * --cpu_context = (unsigned int) 0x00000004;                                       /* R4                                                        */
     * --cpu_context = (unsigned int) 0x00000003;                                       /* R3                                                        */
     * --cpu_context = (unsigned int) 0x00000002;                                       /* R2                                                        */
     * --cpu_context = (unsigned int) 0x00000001;                                       /* R1                                                        */
     * --cpu_context = (unsigned int) 0x00000000;                                       /* R0  ARGUMENT PASS                                                         */
     * --cpu_context = (unsigned int) variable_pass;                                    /* status regsiter                             */

     	/****************************************************************/
        // used for the status register
		#define  ARM_MODE_ARM           0x00000200u
		#define  ARM_MODE_THUMB         0x00000220u

		#define  ARM_SVC_MODE_THUMB    (0x0000001f + ARM_MODE_THUMB)
		#define  ARM_SVC_MODE_ARM      (0x0000001f + ARM_MODE_ARM)
       /*****************************************************************/

     // Setup Status register
     if (((unsigned int)task & 0x01) == 0x01)                   /* See if task arch_sama5d4_context_switch_starts in Thumb or ARM mode                */
        * --cpu_context = ARM_SVC_MODE_THUMB;                   /* CPSR  (Enable IRQ and FIQ interrupts, THUMB-mode)    */
     else
        * --cpu_context = ARM_SVC_MODE_ARM;                     /* CPSR  (Enable IRQ and FIQ interrupts, ARM-mode)      */

     /*
      *	Setup Floating Point registers
      */

     // floating point status register
     * --cpu_context = (unsigned int)0x00000000;	 	/* Initialise Floating point status & control register  */

     // floating point registers
     for (int i = 0; i < 32; i++)
    	 * --cpu_context = (unsigned int)0x00000000;	/* Initialise general-purpose Floating point registers  */

     // floating point execution register
     * --cpu_context = (unsigned int)0x40000000;  	 	/* Initialise Floating-Point Exception Register (Enable)*/


    // Insert Newly created task on to the Kernel
	internal_kernel_insert_task(tcb);					// insert task ctrl block unto Kernel Schedular


	tcb->stack_ptr = (unsigned char *)cpu_context;		// Store Stack pointer unto the task ctrl block
	return(tcb);										// Return Handle to the newly Created Task
}