From 7a98dc66cb6e11f13d2b4f055dd8c97c1143678a Mon Sep 17 00:00:00 2001
From: Mike Playle <mike@mythik.co.uk>
Date: Thu, 1 Feb 2018 22:24:37 +0000
Subject: [PATCH] Add support for software-triggered timer matches via the
 "force output compare" bits.

ATmega32U4 only.
---
 simavr/cores/sim_mega32u4.c |  2 ++
 simavr/sim/avr_timer.c      | 19 +++++++++++++++++++
 simavr/sim/avr_timer.h      |  1 +
 3 files changed, 22 insertions(+)

diff --git a/simavr/cores/sim_mega32u4.c b/simavr/cores/sim_mega32u4.c
index ea79fac..357123e 100644
--- a/simavr/cores/sim_mega32u4.c
+++ b/simavr/cores/sim_mega32u4.c
@@ -331,6 +331,7 @@ const struct mcu_t {
 				.r_ocrh = OCR1AH,	// 16 bits timers have two bytes of it
 				.com = AVR_IO_REGBITS(TCCR1A, COM1A0, 0x3),
 				.com_pin = AVR_IO_REGBIT(PORTB, 5),
+                                .foc = AVR_IO_REGBIT(TCCR1C, FOC1A),
 				.interrupt = {
 					.enable = AVR_IO_REGBIT(TIMSK1, OCIE1A),
 					.raised = AVR_IO_REGBIT(TIFR1, OCF1A),
@@ -342,6 +343,7 @@ const struct mcu_t {
 				.r_ocrh = OCR1BH,
 				.com = AVR_IO_REGBITS(TCCR1A, COM1B0, 0x3),
 				.com_pin = AVR_IO_REGBIT(PORTB, 6),
+                                .foc = AVR_IO_REGBIT(TCCR1C, FOC1B),
 				.interrupt = {
 					.enable = AVR_IO_REGBIT(TIMSK1, OCIE1B),
 					.raised = AVR_IO_REGBIT(TIFR1, OCF1B),
diff --git a/simavr/sim/avr_timer.c b/simavr/sim/avr_timer.c
index 2eb2009..96b40e1 100644
--- a/simavr/sim/avr_timer.c
+++ b/simavr/sim/avr_timer.c
@@ -704,6 +704,23 @@ avr_timer_write(
 	}
 }
 
+/*
+ * write to the "force output compare" bits
+ */
+static void avr_timer_write_foc(struct avr_t * avr, avr_io_addr_t addr, uint8_t v, void * param)
+{
+	avr_timer_t * p = (avr_timer_t *)param;
+
+        /* These are strobe writes, so just decode them, don't store them */
+
+	for (int compi = 0; compi < AVR_TIMER_COMP_COUNT; compi++) {
+                if ((addr == p->comp[compi].foc.reg) &&
+                    (v & (1 << p->comp[compi].foc.bit))) {
+                        avr_timer_comp(p, avr->cycle, compi);
+                }
+        }
+}
+
 /*
  * write to the TIFR register. Watch for code that writes "1" to clear
  * pending interrupts.
@@ -936,6 +953,8 @@ avr_timer_init(
 
 		if (p->comp[compi].r_ocr) // not all timers have all comparators
 			avr_register_io_write(avr, p->comp[compi].r_ocr, avr_timer_write_ocr, &p->comp[compi]);
+                if (p->comp[compi].foc.reg)
+                        avr_register_io_write(avr, p->comp[compi].foc.reg, avr_timer_write_foc, p);
 	}
 	avr_register_io_write(avr, p->r_tcnt, avr_timer_tcnt_write, p);
 	avr_register_io_read(avr, p->r_tcnt, avr_timer_tcnt_read, p);
diff --git a/simavr/sim/avr_timer.h b/simavr/sim/avr_timer.h
index 8e99857..ce52592 100644
--- a/simavr/sim/avr_timer.h
+++ b/simavr/sim/avr_timer.h
@@ -114,6 +114,7 @@ typedef struct avr_timer_comp_t {
 		avr_regbit_t		com;			// comparator output mode registers
 		avr_regbit_t		com_pin;		// where comparator output is connected
 		uint64_t			comp_cycles;
+                avr_regbit_t            foc;                    // "force compare match" strobe
 } avr_timer_comp_t, *avr_timer_comp_p;
 
 enum {
-- 
2.39.5