63 lines
1.4 KiB
C
63 lines
1.4 KiB
C
#include <stdlib.h>
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
#include <stdio.h>
|
|
#include <sys/time.h>
|
|
#include "device.h"
|
|
|
|
struct timeval boot_time, last_jiffy;
|
|
|
|
uint16_t clockdiv;
|
|
uint16_t intmsg;
|
|
uint64_t jiffies;
|
|
uint16_t clock_ticks;
|
|
|
|
void clock_init() {
|
|
gettimeofday(&boot_time, NULL);
|
|
clockdiv = intmsg = clock_ticks = 0;
|
|
last_jiffy.tv_sec = last_jiffy.tv_usec = 0;
|
|
jiffies = 0;
|
|
}
|
|
|
|
void clock_tick()
|
|
{
|
|
// struct timeval now;
|
|
uint64_t delta;
|
|
|
|
if (clockdiv != 0) {
|
|
// gettimeofday(&now, NULL);
|
|
delta = (time.tv_sec - last_jiffy.tv_sec)*10000 + (time.tv_usec - last_jiffy.tv_usec)/100;
|
|
if (delta >= 166) {
|
|
jiffies++;
|
|
last_jiffy.tv_sec = time.tv_sec; last_jiffy.tv_usec = time.tv_usec;
|
|
if (jiffies >= clockdiv) {
|
|
jiffies = 0;
|
|
clock_ticks++;
|
|
// printf("Tick!\n");
|
|
if (intmsg != 0) {
|
|
intq_push(intmsg);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void clock_irqh()
|
|
{
|
|
// printf("Clock: A=0x%04x B=0x%04x\n", ra, rb);
|
|
switch (ra) {
|
|
case 0:
|
|
clockdiv = rb;
|
|
clock_ticks = 0;
|
|
break;
|
|
case 1:
|
|
rc = clock_ticks;
|
|
break;
|
|
case 2:
|
|
intmsg = rb;
|
|
break;
|
|
}
|
|
}
|
|
|
|
struct dev_entry clock_dev = { 0x6d53647c, 0x12d0b402, 0x0000, "Clock", clock_irqh, clock_init, NULL, clock_tick }; /* Clock device */
|