273 lines
6.3 KiB
C
273 lines
6.3 KiB
C
/*
|
|
* Copyright (c) 2011 Aeroflex Gaisler
|
|
*
|
|
* BSD license:
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in
|
|
* all copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
|
|
* THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
|
* THE SOFTWARE.
|
|
*/
|
|
|
|
|
|
#include <sys/types.h>
|
|
#include <sys/time.h>
|
|
#include <sys/errno.h>
|
|
#include <asm-leon/elfmacro.h>
|
|
#include <asm-leon/leon.h>
|
|
#include <asm-leon/irq.h>
|
|
#include <asm-leon/jiffies.h>
|
|
#include <asm-leon/param.h>
|
|
#include <asm-leon/leoncompat.h>
|
|
|
|
static __inline__ unsigned long do_gettimeoffset ();
|
|
|
|
|
|
extern int *rtc;
|
|
|
|
unsigned long wall_jiffies = INITIAL_JIFFIES;
|
|
unsigned long tick_nsec = TICK_NSEC;
|
|
unsigned long tick_usec = TICK_NSEC / 1000;
|
|
unsigned long seperateirq = 1;
|
|
unsigned long noalarm = 1;
|
|
unsigned long force_noalarm = 0;
|
|
unsigned long nodotimer = 0;
|
|
int leonbare_hz = HZ;
|
|
|
|
|
|
void settimer ();
|
|
inline void
|
|
do_timer (struct leonbare_pt_regs *regs)
|
|
{
|
|
unsigned long ticks;
|
|
jiffies_64++;
|
|
ticks = jiffies - wall_jiffies;
|
|
if (ticks)
|
|
{
|
|
wall_jiffies += ticks;
|
|
do
|
|
{
|
|
ticks--;
|
|
xtime.tv_nsec += tick_nsec;
|
|
if (xtime.tv_nsec >= 1000000000)
|
|
{
|
|
xtime.tv_nsec -= 1000000000;
|
|
xtime.tv_sec++;
|
|
}
|
|
}
|
|
while (ticks);
|
|
}
|
|
settimer ();
|
|
}
|
|
|
|
int
|
|
leonbare_alarm (int irq, void *arg, struct leonbare_pt_regs *regs)
|
|
{
|
|
settimer ();
|
|
}
|
|
|
|
extern clock_t (*clock_custom) (void);
|
|
clock_t
|
|
leonbare_clock_custom ()
|
|
{
|
|
int hz = leonbare_hz ? leonbare_hz : HZ;
|
|
return (clock_t) ((jiffies * (CLOCK_TICK_RATE / hz)) + do_gettimeoffset ());
|
|
}
|
|
|
|
|
|
tickerhandler ticker_callback = 0;
|
|
int
|
|
leonbare_tick (int irq, void *arg, struct leonbare_pt_regs *regs)
|
|
{
|
|
unsigned int ctrl;
|
|
if (!seperateirq)
|
|
{
|
|
/* only leon3 comes here */
|
|
if (!noalarm)
|
|
{
|
|
ctrl = LEON3_GpTimer_Regs->e[1].ctrl;
|
|
if (ctrl & LEON3_GPTIMER_IP)
|
|
{
|
|
leonbare_alarm (irq, arg, regs);
|
|
LEON3_GpTimer_Regs->e[1].ctrl = ctrl & ~LEON3_GPTIMER_IP;
|
|
}
|
|
}
|
|
ctrl = LEON3_GpTimer_Regs->e[0].ctrl;
|
|
if (!(ctrl & LEON3_GPTIMER_IP))
|
|
{
|
|
return 0;
|
|
}
|
|
LEON3_GpTimer_Regs->e[0].ctrl = ctrl & ~LEON3_GPTIMER_IP;
|
|
}
|
|
|
|
if (!nodotimer)
|
|
{
|
|
do_timer (regs);
|
|
}
|
|
if (ticker_callback)
|
|
{
|
|
ticker_callback (regs);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static struct irqaction irqact1 = { (irqhandler) leonbare_tick, 0, 0, 0 };
|
|
static struct irqaction irqact2 = { (irqhandler) leonbare_alarm, 0, 0, 0 };
|
|
|
|
void
|
|
leonbare_init_ticks ()
|
|
{
|
|
int i, irq1 = 0, irq2 = 0;
|
|
int hz = leonbare_hz ? leonbare_hz : HZ;
|
|
amba_apb_device dev[1];
|
|
|
|
//---------------------
|
|
switch (LEONCOMPAT_VERSION)
|
|
{
|
|
case 3:
|
|
default:
|
|
amba_init ();
|
|
if (LEON3_GpTimer_Regs && LEON3_IrqCtrl_Regs)
|
|
{
|
|
if ((LEON3_GpTimer_Regs->config & LEON3_GPTIMER_CONFIG_TIMERMASK) >=
|
|
2 && force_noalarm == 0)
|
|
noalarm = 0;
|
|
if (!(LEON3_GpTimer_Regs->config & LEON3_GPTIMER_CONFIG_SEPERATE))
|
|
seperateirq = 0;
|
|
LEON3_GpTimer_Regs->e[0].val = 0;
|
|
LEON3_GpTimer_Regs->e[0].rld = (((CLOCK_TICK_RATE / hz) - 1));
|
|
LEON3_GpTimer_Regs->e[0].ctrl = 0;
|
|
LEON3_GpTimer_Regs->e[0].ctrl =
|
|
LEON3_GPTIMER_EN |
|
|
LEON3_GPTIMER_RL | LEON3_GPTIMER_LD | LEON3_GPTIMER_IRQEN;
|
|
irq1 = LEON3_GpTimer_Irq;
|
|
irq2 = LEON3_GpTimer_Irq + 1;
|
|
}
|
|
break;
|
|
}
|
|
//---------------------
|
|
|
|
if (irq1)
|
|
{
|
|
clock_custom = leonbare_clock_custom;
|
|
chained_catch_interrupt (irq1, &irqact1);
|
|
leonbare_enable_irq (irq1);
|
|
}
|
|
if (irq2 && (!noalarm) && seperateirq)
|
|
{
|
|
chained_catch_interrupt (irq2, &irqact2);
|
|
leonbare_enable_irq (irq2);
|
|
}
|
|
}
|
|
|
|
|
|
//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
|
|
|
|
static __inline__ unsigned long
|
|
do_gettimeoffset ()
|
|
{
|
|
unsigned long usec = 0;
|
|
//---------------------
|
|
switch (LEONCOMPAT_VERSION)
|
|
{
|
|
case 3:
|
|
default:
|
|
usec = ((LEON3_GpTimer_Regs->e[0].rld & 0x7fffff) -
|
|
(LEON3_GpTimer_Regs->e[0].val & 0x7fffff));
|
|
break;
|
|
}
|
|
//---------------------
|
|
return usec;
|
|
}
|
|
|
|
/* get usec (timeval) resolution,
|
|
* could use nsec (timespec) because pthread use it (todo) */
|
|
void
|
|
do_gettimeofday (struct timeval *tv)
|
|
{
|
|
|
|
unsigned long flags;
|
|
unsigned long seq;
|
|
unsigned long usec, sec;
|
|
|
|
do
|
|
{
|
|
unsigned long lost;
|
|
seq = jiffies;
|
|
|
|
usec = do_gettimeoffset ();
|
|
lost = jiffies - wall_jiffies;
|
|
|
|
if (unlikely (lost))
|
|
{
|
|
usec += lost * tick_usec;
|
|
}
|
|
|
|
sec = xtime.tv_sec;
|
|
usec += (xtime.tv_nsec / 1000);
|
|
}
|
|
while (seq != jiffies);
|
|
|
|
while (usec >= 1000000)
|
|
{
|
|
usec -= 1000000;
|
|
sec++;
|
|
}
|
|
|
|
tv->tv_sec = sec;
|
|
tv->tv_usec = usec;
|
|
}
|
|
|
|
int
|
|
gettimeofday (struct timeval *__p, void *__tz)
|
|
{
|
|
do_gettimeofday (__p);
|
|
return 0;
|
|
}
|
|
|
|
//'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
|
|
|
|
static int
|
|
do_settimeofday (struct timespec *tv)
|
|
{
|
|
time_t sec = tv->tv_sec;
|
|
long nsec = tv->tv_nsec;
|
|
|
|
if ((unsigned long) nsec >= NSEC_PER_SEC)
|
|
return EINVAL;
|
|
|
|
/*
|
|
* This is revolting. We need to set "xtime" correctly. However, the
|
|
* value in this location is the value at the most recent update of
|
|
* wall time. Discover what correction gettimeofday() would have
|
|
* made, and then undo it!
|
|
*/
|
|
nsec -= 1000 * (do_gettimeoffset () +
|
|
(jiffies - wall_jiffies) * (USEC_PER_SEC / HZ));
|
|
|
|
set_normalized_timespec (&xtime, sec, nsec);
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
settimeofday (const struct timeval *tv, const struct timezone *tz)
|
|
{
|
|
struct timespec ts;
|
|
ts.tv_sec = tv->tv_sec;
|
|
ts.tv_nsec = tv->tv_usec * NSEC_PER_USEC;
|
|
return do_settimeofday (&ts);
|
|
}
|