gettimeofday.c

这是leon3处理器的交叉编译链

/*

    LEON2/3 LIBIO low-level routines 
    Written by Jiri Gaisler.
    Copyright (C) 2004  Gaisler Research AB

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

*/

#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/irq.h>
#include <asm-leon/jiffies.h>
#include <asm-leon/param.h>
#include <asm-leon/leoncompat.h>
	       
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;

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();
}

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;
  }
  
  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;
  amba_apb_device dev[1];
   
  //---------------------
  switch (LEONCOMPAT_VERSION) {
  case 2:
    LEON_REG.Timer_Counter_1 = 0;
    LEON_REG.Timer_Reload_1 = (((CLOCK_TICK_RATE / HZ) - 1));
    LEON_REG.Timer_Control_1 = 0;
    LEON_REG.Timer_Control_1 = 
	LEON_REG_TIMER_COUNTER_ENABLE_COUNTING |
	LEON_REG_TIMER_COUNTER_RELOAD_AT_ZERO |
	LEON_REG_TIMER_COUNTER_LOAD_COUNTER | LEON_REG_TIMER_COUNTER_IRQEN;
    irq1 = LEON2_INTERRUPT_TIMER1;
    irq2 = LEON2_INTERRUPT_TIMER2;
    noalarm = 0;
    break;
  case 3:
  default:
    amba_init();
    if (LEON3_GpTimer_Regs && LEON3_IrqCtrl_Regs) {
      if ((LEON3_GpTimer_Regs->config & LEON3_GPTIMER_CONFIG_TIMERMASK) >= 2) 
	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) {
    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 2:
    usec =  ((LEON_REG.Timer_Reload_1&0x7fffff) - 
	     (LEON_REG.Timer_Counter_1&0x7fffff));
    break;
  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 *tv, struct timezone *tz) {
  do_gettimeofday(tv);
  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);
}