speaker: Fix beep

Finally beeps. XXX: nope, it does not.

1 files changed, 20 insertions, 9 deletions

diff --git a/speaker.c b/speaker.c
index ec5da36..e96afcf 100644
--- a/speaker.c
+++ b/speaker.c
@@ -11,8 +11,16 @@
 
 #include "speaker.h"
 
-ISR(TIMER2_OVF_vect) {
+static volatile uint16_t count;
+
+/*	Compare interrupt
+ */
+ISR(TIMER2_COMPA_vect) {
 	PORTD ^= (1 << PD6);
+	--count;
+	if (count == 0) {
+		speakerStop ();
+	}
 }
 
 void speakerInit () {
@@ -23,21 +31,24 @@ void speakerInit () {
 }
 
 void speakerStart (const speakerMode mode) {
+	/* 12.8ms */
+	count = 100;
+
+	/* compare value (hit on every tick) */
+	OCR2A = 1;
 	/* reset timer value */
 	TCNT2 = 0;
-	/* set normal mode */
-	TCCR2A = 0;
+	/* set ctc mode */
+	TCCR2A = (1 << WGM21);
 	/* enable overflow interrupt */
-	TIMSK2 = (1 << TOIE2);
-	/* io clock with 1024 prescaler */
-	TCCR2B = ((TCCR2B & ~(1 << CS21)) | (1 << CS22) | (1 << CS20));
-	_delay_ms (50);
-	speakerStop ();
+	TIMSK2 = (1 << OCIE2A);
+	/* io clock with 1024 prescaler -> ~4kHz tone, ctc part2 */
+	TCCR2B = (1 << CS22) | (1 << CS21) | (1 << CS20);
 }
 
 void speakerStop () {
 	/* zero clock source */
-	TCCR2B = 0;
+	TCCR2B &= ~(0x7);
 	/* turn off */
 	PORTD = PORTD & ~(1 << PD6);
 }