X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=common%2Ftimer.c;fp=common%2Ftimer.c;h=491e4707d7e870898211f48405e9463392ed70cb;hb=3c070b8fc92fe0506e29d0c0491038be241f7107;hp=99625f8c2abd4b6df8ca0dd552b760499d09bf6e;hpb=912c9026215483f1755c2291370208f43e1a7b2c;p=model-railway-devices.git

diff --git a/common/timer.c b/common/timer.c
index 99625f8..491e470 100644
--- a/common/timer.c
+++ b/common/timer.c
@@ -10,9 +10,7 @@ static void timer_start(uint8_t num, uint32_t period)
 	if(num==0)
 	{
 		if(period<0x100)
-		{
 			cs = BIT(CS00);
-		}
 		else if(period<0x800)
 		{
 			cs = BIT(CS01);
@@ -40,12 +38,10 @@ static void timer_start(uint8_t num, uint32_t period)
 		OCR0A = period;
 		TIMSK0 = BIT(OCIE0A);
 	}
-	if(num==1)
+	else if(num==1)
 	{
 		if(period<0x10000)
-		{
 			cs = BIT(CS10);
-		}
 		else if(period<0x80000)
 		{
 			cs = BIT(CS11);
@@ -74,6 +70,47 @@ static void timer_start(uint8_t num, uint32_t period)
 		OCR1AL = period;
 		TIMSK1 = BIT(OCIE1A);
 	}
+	else if(num==2)
+	{
+		if(period<0x100)
+			cs = BIT(CS20);
+		else if(period<0x800)
+		{
+			cs = BIT(CS21);
+			period /= 8;
+		}
+		else if(period<0x2000)
+		{
+			cs = BIT(CS21) | BIT(CS20);
+			period /= 32;
+		}
+		else if(period<0x4000)
+		{
+			cs = BIT(CS22);
+			period /= 64;
+		}
+		else if(period<0x8000)
+		{
+			cs = BIT(CS22) | BIT(CS20);
+			period /= 128;
+		}
+		else if(period<0x10000)
+		{
+			cs = BIT(CS22) | BIT(CS21);
+			period /= 256;
+		}
+		else
+		{
+			cs = BIT(CS22) | BIT(CS21) | BIT(CS20);
+			period /= 1024;
+			if(period>0xFF)
+				period = 0xFF;
+		}
+		TCCR2A = BIT(WGM21);
+		TCCR2B = cs;
+		OCR2A = period;
+		TIMSK2 = BIT(OCIE2A);
+	}
 }
 
 void timer_start_hz(uint8_t num, uint32_t freq_p, uint8_t freq_q)
@@ -98,4 +135,9 @@ void timer_stop(uint8_t num)
 		TCCR1B = 0;
 		TIMSK1 = 0;
 	}
+	else if(num==2)
+	{
+		TCCR2B = 0;
+		TIMSK2 = 0;
+	}
 }