////////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2002 Xilinx, Inc.  All rights reserved.
//
// Xilinx, Inc.
// XILINX IS PROVIDING THIS DESIGN, CODE, OR INFORMATION "AS IS" AS A 
// COURTESY TO YOU.  BY PROVIDING THIS DESIGN, CODE, OR INFORMATION AS
// ONE POSSIBLE   IMPLEMENTATION OF THIS FEATURE, APPLICATION OR 
// STANDARD, XILINX IS MAKING NO REPRESENTATION THAT THIS IMPLEMENTATION
// IS FREE FROM ANY CLAIMS OF INFRINGEMENT, AND YOU ARE RESPONSIBLE 
// FOR OBTAINING ANY RIGHTS YOU MAY REQUIRE FOR YOUR IMPLEMENTATION.  
// XILINX EXPRESSLY DISCLAIMS ANY WARRANTY WHATSOEVER WITH RESPECT TO 
// THE ADEQUACY OF THE IMPLEMENTATION, INCLUDING BUT NOT LIMITED TO 
// ANY WARRANTIES OR REPRESENTATIONS THAT THIS IMPLEMENTATION IS FREE 
// FROM CLAIMS OF INFRINGEMENT, IMPLIED WARRANTIES OF MERCHANTABILITY 
// AND FITNESS FOR A PARTICULAR PURPOSE.
// 
// $Id: sched.c,v 1.1.2.4 2003/04/01 00:54:51 mohan Exp $
//
//////////////////////////////////////////////////////////////////////////////

/***************************************************************************/
/**
 * @file sched.c 
 *
 * This contains routines for process scheduling
 *
 ***************************************************************************/
#include <stdio.h>
#ifdef MB_XILKERNEL
#include <sys/mblaze_asm.h>
#endif
#include <sys/sched.h>
#ifdef PPC_XILKERNEL
#include <xpseudo_asm.h>
#include <xtime_l.h>
#endif
signed char run_q = -1 ;		/**< Run Queue */
/**
 * Ready Queue - This is an array of N_PRIO process Queue's. Each process
 * queue array is of different size wich is configured by the user.
 */
struct _queue ready_q[N_PRIO] ;		

extern process_struct ptable[] ;	/* Process Table */
extern signed char current_pid ;	/* Current Process ID */
extern signed char flags ;
extern process_context *scontext ;
extern int sys_process_remove(unsigned int pid);



/**************************************************************************/
/**
 * Main process Scheduler. Schedules the next process based on the scheduling
 * scheme.
 * - Call the appr. scheduler.
 * - If the current_pid = -1, then something's terribly wrong !!
 * - If the current process is the next process, continue and return.
 * - Else, Save the process context to the current process table location.
 * - Restore the context of the process to be scheduled next.
 *
 * @return
 * 	None
 * 	
 * @note
 * 	None
 * 
 ***************************************************************************/
void process_scheduler()
{
  int pid, state ;

  pid = current_pid ;
#if SCHED_TYPE == SCHED_RR
  process_rr_scheduler() ;
#endif
#if SCHED_TYPE == SCHED_PRIO
  process_prio_scheduler() ;
#endif

  if( current_pid == -1 ) {	
    print("Stuck here...\r\n") ;
#ifdef MB_XILKERNEL
    microblaze_enable_interrupts() ;
#endif
    while(1) ;
  } else if( pid != current_pid ) {
#ifdef MB_XILKERNEL
    if( pid != -1 ) {
      /*
       * Save the Context of the current process.
       */
      state = ptable[pid].state ;
      scontext = &ptable[pid].pcontext ;
      /*
       * State of the process is stored in Reg r11 
       */
      SAVE_CONTEXT( scontext, state ) ;
    }
		
    /*
     * Restore the context of the next schedulable process.
     */
    scontext = &ptable[current_pid].pcontext ;
    RESTORE_CONTEXT( scontext )  	;
#endif
#ifdef PPC_XILKERNEL
    // print("nextpid "); putnum(current_pid);
    scontext = &ptable[current_pid].pcontext ;
    if (ptable[current_pid].state == PROC_NEW) {   
      ptable[current_pid].state = PROC_RUNNING; 
      // print("loaded\n\r");     
      // push the address of scontext to USPRG0 
      asm volatile ("lwz 0, 0(%0)" : : "b" (scontext)) ;
      asm volatile ("mtspr 0x100, 0") ;	//USPRG0
      asm volatile ("mr 9,0") ;
    }
    else {
      ptable[current_pid].state = PROC_RUNNING;
      // print("switch\r\n");     
      // push the address of scontext to USPRG0
      asm volatile ("lwz 0, 0(%0)" : : "b" (scontext)) ;
      asm volatile ("mtspr 0x100, 0") ;	//USPRG0
      // check if the stack pointer is non-zero => restore 
      asm volatile ("mr 9,0") ;
      asm volatile ("lwz 0, 4(9)") ;		//SP
      asm volatile ("cmpwi 0,0x0") ;
      asm volatile ("bne ret_restore") ;
    }
    // load the initial PC and MSR into SRR0 and SRR1 before returning from intr
			
    asm volatile ("lwz 0, 8(9)") ;		//PC
    asm volatile ("mtspr 0x1A, 0") ;	//SRR0		
    asm volatile ("lwz 0, 12(9)") ;		//MSR
    asm volatile ("mtspr 0x1B, 0") ;	//SRR1


    // clear timer status register and enable interrupts

    // mtspr(XREG_SPR_TSR,0x08000000 ) ;	
    // XTime_TSRClearStatusBits(XREG_TSR_PIT_INTERRUPT_STATUS);
    // lis r3, 2048
    // mttsr r3
    asm volatile ("lis 0, 2048");
    asm volatile ("mttsr 0");
      
    // XTime_PITEnableInterrupt();	
    // mftcr r0
    //oris r0, r0, 1024
    //mttcr r0	
    asm volatile ("mftcr 0");
    asm volatile ("oris 0, 0, 1024");
    asm volatile("mttcr 0");
    // return from interrupt
    asm volatile ("rfi") ;

    asm volatile ("ret_restore:") ;
    // print("restored\r\n");
    
#endif
  }
}


/**************************************************************************/
/**
 * Round Robin Scheduler.
 *
 * @return
 * 	None
 * 	
 * @note
 * 	None
 * 
 ***************************************************************************/
#if SCHED_TYPE == SCHED_RR
void process_rr_scheduler()
{
  unsigned int ready ;
	
  if( ready_q[0].item_count == 0 ){	/* Ready Q Empty */
    flags |= CS_TIMER ;
    return ;
  }
  if ( run_q != -1 ) {
    if (ptable[run_q].state == PROC_DEAD) {
      sys_process_remove(run_q);
    }
    else {
      if (ptable[run_q].state != PROC_WAITING) {
        ptable[run_q].state = PROC_READY ;
        penq( &ready_q[0], run_q, 0 ) ;
      }
      else { // the process has been put on the appropriate wait queue by somebody else, such as sema_wait
      }
    }
  }
  pdeq( &ready_q[0], &ready, 0 ) ;
#ifdef MB_XILKERNEL
   ptable[ready].state = PROC_RUNNING ;
#endif
  if(ready != 255) {
    run_q = ready ;
    current_pid = ready ;
  } else {
    run_q = -1;
    current_pid = -1;
  }
  flags &= ~CS_TIMER ;
}
#endif


/**************************************************************************/
/**
 * Priority pre-emptive scheduler
 *
 * @return
 * 	None
 * 	
 * @note
 * 	None
 * 	
 ***************************************************************************/
#if SCHED_TYPE == SCHED_PRIO
void process_prio_scheduler()
{
  unsigned int high_prio, i = 0 ;
  unsigned char ready ;
	
  /*
   * Get the highest priority, used to determine the next process.
   * - If no process is currently running or if PROCESS_YIELD or 
   *   PROCESS_BLOCK flag is set, assign highest priority to the 
   *   least PRIORITY.
   * - Else get the priority of the current process.
   */
  if((run_q == -1)||(flags & PROCESS_YIELD)||
     (flags & PROCESS_BLOCK)|| 
     ((run_q != -1) && 
      (ptable[run_q].state == PROC_DEAD || ptable[run_q].state == PROC_WAITING)
      )
     ){
    high_prio = MAX_PRIORITY ;
    flags &= ~PROCESS_YIELD ;
  } else 
    high_prio = ptable[run_q].priority ;
  /* Get the first ready queue (from high prio) that has process */
  while( (i < N_PRIO) && !ready_q[i].item_count )
    i++ ;
  /*
   * If process is blocking on some resource, then this is obviously 
   * not the next process. So get the next process.
   * Else, check the priority of the ready queue process.
   */
  if( !(flags & PROCESS_BLOCK) ) {
    /*
     * If there are no process or if the priority of the ready
     * queue process is less than high_prio, context switch not
     * needed. Set the kernel flag to indicate this.
     */
    if( (i == N_PRIO) || (i > high_prio)) {
      flags |= CS_TIMER ;
      return ;
    }
    /*
     * Enqueue the current process in the Ready Queue.
     */
    if ( run_q != -1 ) { 
      if(ptable[run_q].state == PROC_DEAD) {
        sys_process_remove(run_q);
      }
      else {
        if (ptable[run_q].state != PROC_WAITING) {
          ptable[run_q].state = PROC_READY ;
          penq( &ready_q[ptable[run_q].priority], run_q, 0 ) ;
        }
        else { // the process has been put on the appropriate wait queue by somebody else, such as sema_wait
        }
      }
    }
  }
  /*
   * Dequeue the next process from the Ready Queue. Assign run_q and
   * current_pid with next PID.
   */
  pdeq( &ready_q[i], &ready, 0 ) ;
#ifdef MB_XILKERNEL
   ptable[ready].state = PROC_RUNNING ;
#endif
  if(ready != 0xff) {
    run_q = ready ;
    current_pid = ready ;
  } else {
    run_q = -1;
    current_pid = -1;
  }
  flags &= ~(PROCESS_BLOCK | CS_TIMER ) ;
}
#endif

/**************************************************************************/
/**
 * Initialise the Ready Queue
 *
 * @return
 * 	None
 * 	
 * @note
 * 	None
 * 
 ***************************************************************************/
void readyq_init()
{
	unsigned int i = 0 ;
	run_q = -1 ;
	for( ; i < N_PRIO; i++ ) {
		alloc_q( &ready_q[i], MAX_READYQ, READY_Q, 
				sizeof(char), i ) ;
	}
}

