/*
 *	Delay functions
 *	See delay.h for details
 *
 *	Make sure this code is compiled with full optimization!!!
 */

#include	"delay.h"
#include <cp0defs.h>
#include "xc.h"
#define CORE_TIMER_FREQUENCY        ((unsigned long int)SYS_CLK_FREQ  / 2)
#define CORE_TIMER_MILLISECONDS     ((unsigned long int)CORE_TIMER_FREQUENCY / 1000)
#define CORE_TIMER_MICROSECONDS     ((unsigned long int)CORE_TIMER_FREQUENCY / 1000000)
#define READ_CORE_TIMER()    _CP0_GET_COUNT()                // Read the MIPS Core Timer


/***********************************************************
 *   Millisecond Delay function using the Count register 
 *   in coprocessor 0 in the MIPS core.
 */
void DelayMs(uint32_t milliseconds)
{
uint32_t time;

time = READ_CORE_TIMER(); // Read Core Timer

time += (SYS_CLK_FREQ / 2 / 1000) * milliseconds; // calc the Stop Time

while ((int32_t)(time - READ_CORE_TIMER()) > 0){};
}

/***********************************************************
 *   Microsecond Delay function using the Count register 
 *   in coprocessor 0 in the MIPS core.
 */
void DelayUs(uint32_t microseconds)
{
uint32_t time;

time = READ_CORE_TIMER(); // Read Core Timer

time += (SYS_CLK_FREQ / 2 / 1000000) * microseconds; // calc the Stop Time

while ((int32_t)(time - READ_CORE_TIMER()) > 0){};

}