From 1898613e4ff3926250bc98e9917fc57b078f48f0 Mon Sep 17 00:00:00 2001
From: Michel Pollet <buserror@gmail.com>
Date: Fri, 4 Dec 2009 22:27:46 +0000
Subject: [PATCH] Many more changes, timed callbacks etc

Now have functions to convert from/to cycles & usecs, use them for
implementing the new "one shot" timer callbacks.
IO modules now use "one shots" to implement "call later" subsystems,
like eeprom, uart, timers and so on.

Signed-off-by: Michel Pollet <buserror@gmail.com>
---
 simavr/Makefile             |   5 +-
 simavr/sim/avr_eeprom.c     |  33 +++++------
 simavr/sim/avr_eeprom.h     |   3 -
 simavr/sim/avr_ioport.c     |   4 +-
 simavr/sim/avr_spi.c        |   5 +-
 simavr/sim/avr_timer8.c     |  70 +++++++++++++---------
 simavr/sim/avr_timer8.h     |   4 +-
 simavr/sim/avr_uart.c       |  30 +++++-----
 simavr/sim/avr_uart.h       |   1 -
 simavr/sim/sim_avr.c        | 113 ++++++++++++++++++++++++++++++++----
 simavr/sim/sim_avr.h        |  35 +++++++++--
 simavr/sim/sim_core.c       |  10 +++-
 simavr/sim/sim_core.h       |   1 -
 simavr/sim/sim_gdb.c        |   4 +-
 simavr/sim/sim_gdb.h        |   2 +-
 simavr/sim/sim_interrupts.c |  14 +++--
 simavr/sim/sim_interrupts.h |   2 +
 simavr/sim/sim_irq.c        |   4 ++
 tests/atmega88_uart_echo.c  |   2 +-
 19 files changed, 239 insertions(+), 103 deletions(-)

diff --git a/simavr/Makefile b/simavr/Makefile
index eba24e0..72bf44b 100644
--- a/simavr/Makefile
+++ b/simavr/Makefile
@@ -66,8 +66,9 @@ obj/%.o: %.c
 
 libsimavr.a	:	${cores_o}
 libsimavr.a	:	${sim_o}
-	ar cru $@ $^
-	ranlib $@
+	@echo AR $@
+	@ar cru $@ $^
+	@ranlib $@
 
 ${target}	:	${cores_o}
 ${target}	:	${sim_o}
diff --git a/simavr/sim/avr_eeprom.c b/simavr/sim/avr_eeprom.c
index eebdf31..7cbd5a6 100644
--- a/simavr/sim/avr_eeprom.c
+++ b/simavr/sim/avr_eeprom.c
@@ -26,23 +26,18 @@
 #include <string.h>
 #include "avr_eeprom.h"
 
-static void avr_eeprom_run(avr_io_t * port)
+static avr_cycle_count_t avr_eempe_clear(struct avr_t * avr, avr_cycle_count_t when, void * param)
 {
-	avr_eeprom_t * p = (avr_eeprom_t *)port;
-	avr_t * avr = p->io.avr;
-	//printf("%s\n", __FUNCTION__);
-	if (p->eempe_clear_timer) {
-		p->eempe_clear_timer--;
-		if (p->eempe_clear_timer == 0) {
-			avr_regbit_clear(avr, p->eempe);
-		}
-	}
-	if (p->ready_raise_timer) {
-		p->ready_raise_timer--;
-		if (p->ready_raise_timer == 0) {
-			avr_raise_interrupt(avr, &p->ready);
-		}
-	}
+	avr_eeprom_t * p = (avr_eeprom_t *)param;
+	avr_regbit_clear(p->io.avr, p->eempe);
+	return 0;
+}
+
+static avr_cycle_count_t avr_eei_raise(struct avr_t * avr, avr_cycle_count_t when, void * param)
+{
+	avr_eeprom_t * p = (avr_eeprom_t *)param;
+	avr_raise_interrupt(p->io.avr, &p->ready);
+	return 0;
 }
 
 static void avr_eeprom_write(avr_t * avr, uint8_t addr, uint8_t v, void * param)
@@ -53,7 +48,7 @@ static void avr_eeprom_write(avr_t * avr, uint8_t addr, uint8_t v, void * param)
 	avr_core_watch_write(avr, addr, v);
 
 	if (!eempe && avr_regbit_get(avr, p->eempe)) {
-		p->eempe_clear_timer = 4;	// auto clear, later
+		avr_cycle_timer_register(avr, 4, avr_eempe_clear, p);
 	}
 	
 	if (eempe && avr_regbit_get(avr, p->eepe)) {	// write operation
@@ -61,10 +56,9 @@ static void avr_eeprom_write(avr_t * avr, uint8_t addr, uint8_t v, void * param)
 	//	printf("eeprom write %04x <- %02x\n", addr, avr->data[p->r_eedr]);
 		p->eeprom[addr] = avr->data[p->r_eedr];	
 		// Automatically clears that bit (?)
-		p->eempe_clear_timer = 0;
 		avr_regbit_clear(avr, p->eempe);
 
-		p->ready_raise_timer = 1024; // make a avr_milliseconds_to_cycle(...) 3.4ms here
+		avr_cycle_timer_register_usec(avr, 3400, avr_eei_raise, p); // 3.4ms here
 	}
 	if (avr_regbit_get(avr, p->eere)) {	// read operation
 		uint16_t addr = avr->data[p->r_eearl] | (avr->data[p->r_eearh] << 8);
@@ -113,7 +107,6 @@ static int avr_eeprom_ioctl(struct avr_io_t * port, uint32_t ctl, void * io_para
 
 static	avr_io_t	_io = {
 	.kind = "eeprom",
-	.run = avr_eeprom_run,
 	.ioctl = avr_eeprom_ioctl,
 };
 
diff --git a/simavr/sim/avr_eeprom.h b/simavr/sim/avr_eeprom.h
index 89678fe..d44826e 100644
--- a/simavr/sim/avr_eeprom.h
+++ b/simavr/sim/avr_eeprom.h
@@ -42,9 +42,6 @@ typedef struct avr_eeprom_t {
 	avr_regbit_t 	eere;	// eeprom read enable
 	
 	avr_int_vector_t ready;	// EERIE vector
-
-	uint32_t		eempe_clear_timer;
-	uint32_t		ready_raise_timer;
 } avr_eeprom_t;
 
 void avr_eeprom_init(avr_t * avr, avr_eeprom_t * port);
diff --git a/simavr/sim/avr_ioport.c b/simavr/sim/avr_ioport.c
index 2b3eaae..6797a97 100644
--- a/simavr/sim/avr_ioport.c
+++ b/simavr/sim/avr_ioport.c
@@ -44,10 +44,10 @@ static void avr_ioport_write(struct avr_t * avr, uint8_t addr, uint8_t v, void *
 	uint8_t oldv = avr->data[addr];
 
 	if (addr == p->r_port) {
-	//	printf("PORT%c(%02x) = %02x (was %02x)\n", p->name, addr, v, oldv);
 
 		avr_core_watch_write(avr, addr, v);
 		if (v != oldv) {
+			//	printf("PORT%c(%02x) = %02x (was %02x)\n", p->name, addr, v, oldv);
 			int mask = v ^ oldv;
 
 			// raise the internal IRQ callbacks
@@ -68,6 +68,8 @@ void avr_ioport_irq_notify(struct avr_irq_t * irq, uint32_t value, void * param)
 {
 	avr_ioport_t * p = (avr_ioport_t *)param;
 	avr_t * avr = p->io.avr;
+
+	//	printf("pcint port%c pcint %02x:%02x\n", p->name, p->r_pcint, avr->data[p->r_pcint]);
 	if (p->r_pcint) {
 		uint8_t mask = 1 << irq->irq;
 		// set the real PIN bit. ddr doesn't matter here as it's masked when read.
diff --git a/simavr/sim/avr_spi.c b/simavr/sim/avr_spi.c
index 1ef1ee9..567bf79 100644
--- a/simavr/sim/avr_spi.c
+++ b/simavr/sim/avr_spi.c
@@ -27,7 +27,7 @@ static uint8_t avr_spi_read(struct avr_t * avr, uint8_t addr, void * param)
 	avr_spi_t * p = (avr_spi_t *)param;
 	uint8_t v = p->input_data_register;
 	p->input_data_register = 0;
-//	printf("** PIN%c = %02x\n", p->name, v);
+//	printf("avr_spi_read = %02x\n", v);
 	return v;
 }
 
@@ -36,7 +36,7 @@ static void avr_spi_write(struct avr_t * avr, uint8_t addr, uint8_t v, void * pa
 	avr_spi_t * p = (avr_spi_t *)param;
 
 	if (addr == p->r_spdr) {
-	//	printf("UDR%c(%02x) = %02x\n", p->name, addr, v);
+//		printf("avr_spi_write = %02x\n", v);
 		avr_core_watch_write(avr, addr, v);
 
 		if (avr_regbit_get(avr, p->spe)) {
@@ -83,6 +83,7 @@ void avr_spi_init(avr_t * avr, avr_spi_t * p)
 {
 	p->io = _io;
 	avr_register_io(avr, &p->io);
+	avr_register_vector(avr, &p->spi);
 
 	printf("%s SPI%c init\n", __FUNCTION__, p->name);
 
diff --git a/simavr/sim/avr_timer8.c b/simavr/sim/avr_timer8.c
index 1e209f9..4725ea5 100644
--- a/simavr/sim/avr_timer8.c
+++ b/simavr/sim/avr_timer8.c
@@ -25,32 +25,33 @@
 #include <stdio.h>
 #include "avr_timer8.h"
 
-static void avr_timer8_run(avr_io_t * port)
+static avr_cycle_count_t avr_timer8_compa(struct avr_t * avr, avr_cycle_count_t when, void * param)
 {
-	avr_timer8_t * p = (avr_timer8_t *)port;
-	avr_t * avr = p->io.avr;
-
-	if (p->compa_cycles) {
-		if (p->compa_next == 0) {
-			p->compa_next = avr->cycle + p->compa_cycles;
-		}
-		if (avr->cycle >= p->compa_next) {
-		//	printf("timer a firea %d\n", p->compa_next);
-			fflush(stdout);
-			p->compa_next += p->compa_cycles;						
-			avr_raise_interrupt(avr, &p->compa);
-		} 
-	}
+	avr_timer8_t * p = (avr_timer8_t *)param;
+	avr_raise_interrupt(avr, &p->compa);
+	return p->compa_cycles ? when + p->compa_cycles : 0;
+}
+
+static avr_cycle_count_t avr_timer8_compb(struct avr_t * avr, avr_cycle_count_t when, void * param)
+{
+	avr_timer8_t * p = (avr_timer8_t *)param;
+	avr_raise_interrupt(avr, &p->compb);
+	return p->compb_cycles ? when + p->compb_cycles : 0;
 }
 
+static uint8_t avr_timer8_tcnt_read(struct avr_t * avr, uint8_t addr, void * param)
+{
+	//avr_timer8_t * p = (avr_timer8_t *)param;
+	// made to trigger potential watchpoints
+	return avr_core_watch_read(avr, addr);
+}
 
 static void avr_timer8_write(struct avr_t * avr, uint8_t addr, uint8_t v, void * param)
 {
 	avr_timer8_t * p = (avr_timer8_t *)param;
-//	uint8_t oldv = avr->data[addr];
 
 	p->compa_cycles = 0;
-	p->compa_next = 0;
+	p->compb_cycles = 0;
 
 	avr_core_watch_write(avr, addr, v);
 	long clock = avr->frequency;
@@ -59,7 +60,8 @@ static void avr_timer8_write(struct avr_t * avr, uint8_t addr, uint8_t v, void *
 	uint8_t cs = avr_regbit_get_array(avr, p->cs, ARRAY_SIZE(p->cs));
 	if (cs == 0) {
 		printf("%s-%c clock turned off\n", __FUNCTION__, p->name);		
-		p->compa_cycles = 0;
+		avr_cycle_timer_cancel(avr, avr_timer8_compa, p);
+		avr_cycle_timer_cancel(avr, avr_timer8_compb, p);
 		return;
 	}
 	uint8_t mode = avr_regbit_get_array(avr, p->wgm, ARRAY_SIZE(p->wgm));
@@ -67,25 +69,33 @@ static void avr_timer8_write(struct avr_t * avr, uint8_t addr, uint8_t v, void *
 	uint16_t ocra = avr->data[p->r_ocra];
 	uint16_t ocrb = avr->data[p->r_ocrb];
 	long f = clock >> cs_div;
-	long fa = f / 2 / (ocra+1), fb = f / 2 / (ocrb+1);
-
-	printf("%s-%c clock f=%ld cs=%02x (div %d) = %ldhz\n", __FUNCTION__, p->name, clock, cs, 1 << cs_div, f);
-	printf("%s-%c wgm %d OCRA=%3d = %ldhz\n", __FUNCTION__, p->name, mode, ocra, fa);
-	printf("%s-%c wgm %d OCRB=%3d = %ldhz\n", __FUNCTION__, p->name, mode, ocrb, fb);	
-
-	long cocra = ocra ? avr->frequency / fa : 0;
-	p->compa_cycles = cocra;
-	printf("%s-%c A %ld/%ld = cycles = %ld\n", __FUNCTION__, p->name, (long)avr->frequency, fa, cocra);
-	
+	long fa = f / (ocra+1), fb = f / (ocrb+1);
+
+//	printf("%s-%c clock f=%ld cs=%02x (div %d) = %ldhz\n", __FUNCTION__, p->name, clock, cs, 1 << cs_div, f);
+	if (ocra) printf("%s-%c wgm %d OCRA=%3d = %ldhz\n", __FUNCTION__, p->name, mode, ocra, fa);
+	if (ocrb) printf("%s-%c wgm %d OCRB=%3d = %ldhz\n", __FUNCTION__, p->name, mode, ocrb, fb);
+
+	p->compa_cycles = avr_hz_to_cycles(avr, fa);
+	p->compb_cycles = avr_hz_to_cycles(avr, fb);
+	if (p->compa_cycles)
+		avr_cycle_timer_register(avr, p->compa_cycles, avr_timer8_compa, p);
+	if (p->compb_cycles)
+		avr_cycle_timer_register(avr, p->compb_cycles, avr_timer8_compb, p);
+//	printf("%s-%c A %ld/%ld = cycles = %d\n", __FUNCTION__, p->name, (long)avr->frequency, fa, (int)p->compa_cycles);
+//	printf("%s-%c B %ld/%ld = cycles = %d\n", __FUNCTION__, p->name, (long)avr->frequency, fb, (int)p->compb_cycles);
 }
 
 static void avr_timer8_reset(avr_io_t * port)
 {
+	avr_timer8_t * p = (avr_timer8_t *)port;
+	avr_cycle_timer_cancel(p->io.avr, avr_timer8_compa, p);
+	avr_cycle_timer_cancel(p->io.avr, avr_timer8_compb, p);
+	p->compa_cycles = 0;
+	p->compb_cycles = 0;
 }
 
 static	avr_io_t	_io = {
 	.kind = "timer8",
-	.run = avr_timer8_run,
 	.reset = avr_timer8_reset,
 };
 
@@ -100,4 +110,6 @@ void avr_timer8_init(avr_t * avr, avr_timer8_t * p)
 	avr_register_io_write(avr, p->cs[0].reg, avr_timer8_write, p);
 	avr_register_io_write(avr, p->r_ocra, avr_timer8_write, p);
 	avr_register_io_write(avr, p->r_ocrb, avr_timer8_write, p);
+
+	avr_register_io_read(avr, p->r_tcnt, avr_timer8_tcnt_read, p);
 }
diff --git a/simavr/sim/avr_timer8.h b/simavr/sim/avr_timer8.h
index 1f3ecca..04a2d34 100644
--- a/simavr/sim/avr_timer8.h
+++ b/simavr/sim/avr_timer8.h
@@ -40,8 +40,8 @@ typedef struct avr_timer8_t {
 	avr_int_vector_t compb;	// comparator A
 	avr_int_vector_t overflow;	// overflow
 
-
-	uint64_t		compa_cycles, compa_next;
+	uint32_t		compa_cycles;
+	uint32_t		compb_cycles;
 } avr_timer8_t;
 
 void avr_timer8_init(avr_t * avr, avr_timer8_t * port);
diff --git a/simavr/sim/avr_uart.c b/simavr/sim/avr_uart.c
index e268cf7..3f000f2 100644
--- a/simavr/sim/avr_uart.c
+++ b/simavr/sim/avr_uart.c
@@ -28,17 +28,12 @@
 
 DEFINE_FIFO(uint8_t, uart_fifo, 128);
 
-static void avr_uart_run(avr_io_t * port)
+static avr_cycle_count_t avr_uart_rxc_raise(struct avr_t * avr, avr_cycle_count_t when, void * param)
 {
-	avr_uart_t * p = (avr_uart_t *)port;
-	avr_t * avr = p->io.avr;
-	if (p->input_cycle_timer) {
-		p->input_cycle_timer--;
-		if (p->input_cycle_timer == 0) {
-			if (avr_regbit_get(avr, p->rxen))
-				avr_raise_interrupt(avr, &p->rxc);
-		}
-	}
+	avr_uart_t * p = (avr_uart_t *)param;
+	if (avr_regbit_get(avr, p->rxen))
+		avr_raise_interrupt(avr, &p->rxc);
+	return 0;
 }
 
 static uint8_t avr_uart_read(struct avr_t * avr, uint8_t addr, void * param)
@@ -56,7 +51,10 @@ static uint8_t avr_uart_read(struct avr_t * avr, uint8_t addr, void * param)
 	avr->data[addr] = v;
 	// made to trigger potential watchpoints
 	v = avr_core_watch_read(avr, addr);
-	p->input_cycle_timer = uart_fifo_isempty(&p->input) ? 0 : 10;
+
+	if (!uart_fifo_isempty(&p->input))
+		avr_cycle_timer_register_usec(avr, 100, avr_uart_rxc_raise, p); // should be uart speed dependent
+
 	return v;
 }
 
@@ -65,7 +63,6 @@ static void avr_uart_write(struct avr_t * avr, uint8_t addr, uint8_t v, void * p
 	avr_uart_t * p = (avr_uart_t *)param;
 
 	if (addr == p->r_udr) {
-	//	printf("UDR%c(%02x) = %02x\n", p->name, addr, v);
 		avr_core_watch_write(avr, addr, v);
 
 		// if the interrupts are not used, still raised the UDRE and TXC flaga
@@ -80,6 +77,7 @@ static void avr_uart_write(struct avr_t * avr, uint8_t addr, uint8_t v, void * p
 			l = 0;
 			printf("\e[32m%s\e[0m\n", buf);
 		}
+//		printf("UDR%c(%02x) = %02x\n", p->name, addr, v);
 		// tell other modules we are "outputing" a byte
 		if (avr_regbit_get(avr, p->txen))
 			avr_raise_irq(p->io.irq + UART_IRQ_OUTPUT, v);
@@ -107,10 +105,9 @@ static void avr_uart_irq_input(struct avr_irq_t * irq, uint32_t value, void * pa
 	if (!avr_regbit_get(avr, p->rxen))
 		return;
 
+	if (uart_fifo_isempty(&p->input))
+		avr_cycle_timer_register_usec(avr, 100, avr_uart_rxc_raise, p); // should be uart speed dependent
 	uart_fifo_write(&p->input, value); // add to fifo
-	// raise interrupt, if it was not there
-	if (p->input_cycle_timer == 0)
-		p->input_cycle_timer = 10;	// random number, should be proportional to speed
 }
 
 
@@ -120,13 +117,12 @@ void avr_uart_reset(struct avr_io_t *io)
 	avr_t * avr = p->io.avr;
 	avr_regbit_set(avr, p->udrc.raised);
 	avr_irq_register_notify(p->io.irq + UART_IRQ_INPUT, avr_uart_irq_input, p);
-	p->input_cycle_timer = 0;
+	avr_cycle_timer_cancel(avr, avr_uart_rxc_raise, p);
 	uart_fifo_reset(&p->input);
 }
 
 static	avr_io_t	_io = {
 	.kind = "uart",
-	.run = avr_uart_run,
 	.reset = avr_uart_reset,
 };
 
diff --git a/simavr/sim/avr_uart.h b/simavr/sim/avr_uart.h
index 81b4e13..b748446 100644
--- a/simavr/sim/avr_uart.h
+++ b/simavr/sim/avr_uart.h
@@ -57,7 +57,6 @@ typedef struct avr_uart_t {
 	avr_int_vector_t udrc;	
 
 	uart_fifo_t	input;
-	uint16_t	input_cycle_timer;
 } avr_uart_t;
 
 void avr_uart_init(avr_t * avr, avr_uart_t * port);
diff --git a/simavr/sim/sim_avr.c b/simavr/sim/sim_avr.c
index a4fa2b2..fc3a672 100644
--- a/simavr/sim/sim_avr.c
+++ b/simavr/sim/sim_avr.c
@@ -115,6 +115,99 @@ uint8_t avr_core_watch_read(avr_t *avr, uint16_t addr)
 	return avr->data[addr];
 }
 
+// converts a number of usec to a number of machine cycles, at current speed
+uint64_t avr_usec_to_cycles(avr_t * avr, uint32_t usec)
+{
+	return avr->frequency * (uint64_t)usec / 1000000;
+}
+
+uint32_t avr_cycles_to_usec(avr_t * avr, uint64_t cycles)
+{
+	return 1000000 * cycles / avr->frequency;
+}
+
+// converts a number of hz (to megahertz etc) to a number of cycle
+uint64_t avr_hz_to_cycles(avr_t * avr, uint32_t hz)
+{
+	return avr->frequency / hz;
+}
+
+void avr_cycle_timer_register(avr_t * avr, uint64_t when, avr_cycle_timer_t timer, void * param)
+{
+	avr_cycle_timer_cancel(avr, timer, param);
+
+	if (avr->cycle_timer_map == 0xffffffff) {
+		fprintf(stderr, "avr_cycle_timer_register is full!\n");
+		return;
+	}
+	when += avr->cycle;
+	for (int i = 0; i < 32; i++)
+		if (!(avr->cycle_timer_map & (1 << i))) {
+			avr->cycle_timer[i].timer = timer;
+			avr->cycle_timer[i].param = param;
+			avr->cycle_timer[i].when = when;
+			avr->cycle_timer_map |= (1 << i);
+			return;
+		}
+}
+
+void avr_cycle_timer_register_usec(avr_t * avr, uint32_t when, avr_cycle_timer_t timer, void * param)
+{
+	avr_cycle_timer_register(avr, avr_usec_to_cycles(avr, when), timer, param);
+}
+
+void avr_cycle_timer_cancel(avr_t * avr, avr_cycle_timer_t timer, void * param)
+{
+	if (!avr->cycle_timer_map)
+		return;
+	for (int i = 0; i < 32; i++)
+		if ((avr->cycle_timer_map & (1 << i)) &&
+				avr->cycle_timer[i].timer == timer &&
+				avr->cycle_timer[i].param == param) {
+			avr->cycle_timer[i].timer = NULL;
+			avr->cycle_timer[i].param = NULL;
+			avr->cycle_timer[i].when = 0;
+			avr->cycle_timer_map &= ~(1 << i);
+			return;
+		}
+}
+
+/*
+ * run thru all the timers, call the ones that needs it,
+ * clear the ones that wants it, and calculate the next
+ * potential cycle we could sleep for...
+ */
+static uint64_t avr_cycle_timer_check(avr_t * avr)
+{
+	if (!avr->cycle_timer_map)
+		return (uint32_t)-1;
+
+	uint64_t min = (uint64_t)-1;
+
+	for (int i = 0; i < 32; i++) {
+		if (!(avr->cycle_timer_map & (1 << i)))
+			continue;
+
+		if (avr->cycle_timer[i].when <= avr->cycle) {
+			// call it
+			avr->cycle_timer[i].when =
+					avr->cycle_timer[i].timer(avr,
+							avr->cycle_timer[i].when,
+							avr->cycle_timer[i].param);
+			if (avr->cycle_timer[i].when == 0) {
+				// clear it
+				avr->cycle_timer[i].timer = NULL;
+				avr->cycle_timer[i].param = NULL;
+				avr->cycle_timer[i].when = 0;
+				avr->cycle_timer_map &= ~(1 << i);
+				continue;
+			}
+		}
+		if (avr->cycle_timer[i].when < min)
+			min = avr->cycle_timer[i].when;
+	}
+	return min - avr->cycle;
+}
 
 int avr_run(avr_t * avr)
 {
@@ -123,7 +216,7 @@ int avr_run(avr_t * avr)
 	if (avr->state == cpu_Stopped)
 		return avr->state;
 
-	// if we are stepping one insruction, we "run" for one..
+	// if we are stepping one instruction, we "run" for one..
 	int step = avr->state == cpu_Step;
 	if (step) {
 		avr->state = cpu_Running;
@@ -134,11 +227,8 @@ int avr_run(avr_t * avr)
 	if (avr->state == cpu_Running) {
 		new_pc = avr_run_one(avr);
 		avr_dump_state(avr);
-	} else
-		avr->cycle ++;
+	}
 
-	// re-synth the SREG
-	//SREG();
 	// if we just re-enabled the interrupts...
 	if (avr->sreg[S_I] && !(avr->data[R_SREG] & (1 << S_I))) {
 	//	printf("*** %s: Renabling interrupts\n", __FUNCTION__);
@@ -150,22 +240,25 @@ int avr_run(avr_t * avr)
 			port->run(port);
 		port = port->next;
 	}
+	avr_cycle_count_t sleep = avr_cycle_timer_check(avr);
 
 	avr->pc = new_pc;
 
 	if (avr->state == cpu_Sleeping) {
 		if (!avr->sreg[S_I]) {
-			printf("simavr: sleeping with interrupts off, quitting gracefuly\n");
+			printf("simavr: sleeping with interrupts off, quitting gracefully\n");
 			exit(0);
 		}
+		/*
+		 * try to sleep for as long as we can (?)
+		 */
+		uint32_t usec = avr_cycles_to_usec(avr, sleep);
 		if (avr->gdb) {
-			while (avr_gdb_processor(avr, 1))
+			while (avr_gdb_processor(avr, usec))
 				;
 		} else
-			usleep(500);
-		long sleep = (float)avr->frequency * (1.0f / 500.0f);
+			usleep(usec);
 		avr->cycle += sleep;
-	//	avr->state = cpu_Running;
 	}
 	// Interrupt servicing might change the PC too
 	if (avr->state == cpu_Running || avr->state == cpu_Sleeping) {
diff --git a/simavr/sim/sim_avr.h b/simavr/sim/sim_avr.h
index 0af4836..88026f7 100644
--- a/simavr/sim/sim_avr.h
+++ b/simavr/sim/sim_avr.h
@@ -24,9 +24,12 @@
 
 #include <stdint.h>
 
+typedef uint64_t avr_cycle_count_t;
+
 struct avr_t;
 typedef uint8_t (*avr_io_read_t)(struct avr_t * avr, uint8_t addr, void * param);
 typedef void (*avr_io_write_t)(struct avr_t * avr, uint8_t addr, uint8_t v, void * param);
+typedef avr_cycle_count_t (*avr_cycle_timer_t)(struct avr_t * avr, avr_cycle_count_t when, void * param);
 
 enum {
 	// SREG bit indexes
@@ -74,11 +77,11 @@ typedef struct avr_t {
 	uint8_t		fuse[4];
 
 	// filled by the ELF data, this allow tracking of invalid jumps
-	uint32_t	codeend;
+	uint32_t			codeend;
 
-	int			state;		// stopped, running, sleeping
-	uint32_t	frequency;	// frequency we are running at
-	uint64_t	cycle;		// current cycle
+	int					state;		// stopped, running, sleeping
+	uint32_t			frequency;	// frequency we are running at
+	avr_cycle_count_t	cycle;		// current cycle
 	
 	// called at init time
 	void (*init)(struct avr_t * avr);
@@ -118,6 +121,17 @@ typedef struct avr_t {
 	// queue of io modules
 	struct avr_io_t *io_port;
 
+	// cycle timers are callbacks that will be called when "when" cycle is reached
+	// the bitmap allows quick knowledge of whether there is anything to call
+	// these timers are one shots, then get cleared if the timer function returns zero,
+	// they get reset if the callback function returns a new cycle number
+	uint32_t	cycle_timer_map;
+	struct {
+		avr_cycle_count_t	when;
+		avr_cycle_timer_t	timer;
+		void * param;
+	} cycle_timer[32];
+
 	// interrupt vectors, and their enable/clear registers
 	struct avr_int_vector_t * vector[64];
 	uint8_t		pending_wait;	// number of cycles to wait for pending
@@ -188,6 +202,19 @@ int avr_run(avr_t * avr);
 // load code in the "flash"
 void avr_loadcode(avr_t * avr, uint8_t * code, uint32_t size, uint32_t address);
 
+// converts a nunber of usec to a nunber of machine cycles, at current speed
+avr_cycle_count_t avr_usec_to_cycles(avr_t * avr, uint32_t usec);
+// converts a number of hz (to megahertz etc) to a number of cycle
+avr_cycle_count_t avr_hz_to_cycles(avr_t * avr, uint32_t hz);
+// converts back a number of cycles to usecs (for usleep)
+uint32_t avr_cycles_to_usec(avr_t * avr, avr_cycle_count_t cycles);
+
+// register for calling 'timer' in 'when' cycles
+void avr_cycle_timer_register(avr_t * avr, avr_cycle_count_t when, avr_cycle_timer_t timer, void * param);
+// register a timer to call in 'when' usec
+void avr_cycle_timer_register_usec(avr_t * avr, uint32_t when, avr_cycle_timer_t timer, void * param);
+// cancel a previously set timer
+void avr_cycle_timer_cancel(avr_t * avr, avr_cycle_timer_t timer, void * param);
 
 /*
  * these are accessors for avr->data but allows watchpoints to be set for gdb
diff --git a/simavr/sim/sim_core.c b/simavr/sim/sim_core.c
index b70edd6..55a0fab 100644
--- a/simavr/sim/sim_core.c
+++ b/simavr/sim/sim_core.c
@@ -142,7 +142,7 @@ static inline uint8_t _avr_get_ram(avr_t * avr, uint16_t addr)
 }
 
 /*
- * Stack oush accessors. Push/pop 8 and 16 bits
+ * Stack push accessors. Push/pop 8 and 16 bits
  */
 static inline void _avr_push8(avr_t * avr, uint16_t v)
 {
@@ -717,8 +717,14 @@ uint16_t avr_run_one(avr_t * avr)
 				}	break;
 				case 0x9598: { // BREAK
 					STATE("break\n");
-					if (avr->gdb)
+					if (avr->gdb) {
+						// if gdb is on, we break here as in here
+						// and we do so until gdb restores the instruction
+						// that was here before
 						avr->state = cpu_StepDone;
+						new_pc = avr->pc;
+						cycle = 0;
+					}
 				}	break;
 				case 0x95a8: { // WDR
 					STATE("wdr\n");
diff --git a/simavr/sim/sim_core.h b/simavr/sim/sim_core.h
index e5e2a96..17a65ed 100644
--- a/simavr/sim/sim_core.h
+++ b/simavr/sim/sim_core.h
@@ -39,7 +39,6 @@ void _avr_push16(avr_t * avr, uint16_t v);
  */
 const char * avr_regname(uint8_t reg);
 
-
 /* 
  * DEBUG bits follow 
  * These will diseapear when gdb arrives
diff --git a/simavr/sim/sim_gdb.c b/simavr/sim/sim_gdb.c
index 751f511..43225c8 100644
--- a/simavr/sim/sim_gdb.c
+++ b/simavr/sim/sim_gdb.c
@@ -304,7 +304,7 @@ static void gdb_handle_command(avr_gdb_t * g, char * cmd)
 	}
 }
 
-static int gdb_network_handler(avr_gdb_t * g, int dosleep)
+static int gdb_network_handler(avr_gdb_t * g, uint32_t dosleep)
 {
 	fd_set read_set;
 	int max;
@@ -317,7 +317,7 @@ static int gdb_network_handler(avr_gdb_t * g, int dosleep)
 		FD_SET(g->listen, &read_set);
 		max = g->listen + 1;
 	}
-	struct timeval timo = { 0, dosleep ? 500 : 0 };	// short, but not too short interval
+	struct timeval timo = { 0, dosleep };	// short, but not too short interval
 	int ret = select(max, &read_set, NULL, NULL, &timo);
 
 	if (ret == 0)
diff --git a/simavr/sim/sim_gdb.h b/simavr/sim/sim_gdb.h
index 548c59c..49a5645 100644
--- a/simavr/sim/sim_gdb.h
+++ b/simavr/sim/sim_gdb.h
@@ -25,6 +25,6 @@
 int avr_gdb_init(avr_t * avr);
 
 // call from the main AVR decoder thread
-int avr_gdb_processor(avr_t * avr, int sleep);
+int avr_gdb_processor(avr_t * avr, uint32_t sleep);
 
 #endif
diff --git a/simavr/sim/sim_interrupts.c b/simavr/sim/sim_interrupts.c
index bf620bb..41e2f40 100644
--- a/simavr/sim/sim_interrupts.c
+++ b/simavr/sim/sim_interrupts.c
@@ -46,8 +46,9 @@ int avr_raise_interrupt(avr_t * avr, avr_int_vector_t * vector)
 {
 	if (!vector || !vector->vector)
 		return 0;
-//	printf("%s raising %d\n", __FUNCTION__, vector->vector);
-	// always mark the 'raised' flag to one, even if the interuot is disabled
+	if (vector->trace)
+		printf("%s raising %d\n", __FUNCTION__, vector->vector);
+	// always mark the 'raised' flag to one, even if the interrupt is disabled
 	// this allow "pooling" for the "raised" flag, like for non-interrupt
 	// driven UART and so so. These flags are often "write one to clear"
 	if (vector->raised.reg)
@@ -62,7 +63,8 @@ int avr_raise_interrupt(avr_t * avr, avr_int_vector_t * vector)
 		avr->pending[vector->vector >> 5] |= (1 << (vector->vector & 0x1f));
 
 		if (avr->state != cpu_Running) {
-		//	printf("Waking CPU due to interrupt\n");
+			if (vector->trace)
+				printf("Waking CPU due to interrupt\n");
 			avr->state = cpu_Running;	// in case we were sleeping
 		}
 	}
@@ -76,7 +78,8 @@ void avr_clear_interrupt(avr_t * avr, int v)
 	avr->pending[v >> 5] &= ~(1 << (v & 0x1f));
 	if (!vector)
 		return;
-//	printf("%s cleared %d\n", __FUNCTION__, vector->vector);
+	if (vector->trace)
+		printf("%s cleared %d\n", __FUNCTION__, vector->vector);
 	if (vector->raised.reg)
 		avr_regbit_clear(avr, vector->raised);
 }
@@ -101,7 +104,8 @@ void avr_service_interrupts(avr_t * avr)
 
 							int v = (bi * 32) + ii;	// vector
 
-						//	printf("%s calling %d\n", __FUNCTION__, v);
+							if (avr->vector[v] && avr->vector[v]->trace)
+								printf("%s calling %d\n", __FUNCTION__, v);
 							_avr_push16(avr, avr->pc >> 1);
 							avr->sreg[S_I] = 0;
 							avr->pc = v * avr->vector_size;
diff --git a/simavr/sim/sim_interrupts.h b/simavr/sim/sim_interrupts.h
index c602e8e..26017cb 100644
--- a/simavr/sim/sim_interrupts.h
+++ b/simavr/sim/sim_interrupts.h
@@ -30,6 +30,8 @@ typedef struct avr_int_vector_t {
 
 	avr_regbit_t enable;	// IO register index for the "interrupt enable" flag for this vector
 	avr_regbit_t raised;	// IO register index for the register where the "raised" flag is (optional)
+
+	uint8_t			trace;		// only for debug of a vector
 } avr_int_vector_t;
 
 
diff --git a/simavr/sim/sim_irq.c b/simavr/sim/sim_irq.c
index 5daa824..f3377fd 100644
--- a/simavr/sim/sim_irq.c
+++ b/simavr/sim/sim_irq.c
@@ -86,5 +86,9 @@ static void _avr_irq_connect(avr_irq_t * irq, uint32_t value, void * param)
 
 void avr_connect_irq(avr_irq_t * src, avr_irq_t * dst)
 {
+	if (!src || !dst) {
+		printf("avr_connect_irq invalid irq %p/%p", src, dst);
+		return;
+	}
 	avr_irq_register_notify(src, _avr_irq_connect, dst);
 }
diff --git a/tests/atmega88_uart_echo.c b/tests/atmega88_uart_echo.c
index 4d7cccf..c2b4ba3 100644
--- a/tests/atmega88_uart_echo.c
+++ b/tests/atmega88_uart_echo.c
@@ -43,7 +43,7 @@ ISR(USART_RX_vect)
 	uint8_t b = UDR0;
 	buffer[bindex++] = b;
 	buffer[bindex] = 0;
-	if (b == '\r')
+	if (b == '\n')
 		done++;
 }
 
-- 
2.39.5