Add pwm led code from sniffonly branch

git-svn-id: https://svn.openpcd.org:2342/trunk@435 6dc7ffe9-61d6-0310-9af1-9938baff3ed1

4 files changed, 128 insertions, 19 deletions

diff --git a/openpicc/application/gammatable.inc b/openpicc/application/gammatable.inc
new file mode 100644
index 0000000..0b6486f
--- /dev/null
+++ b/openpicc/application/gammatable.inc
@@ -0,0 +1,4 @@
+/* Autogenerated from gammatable.py */
+const u_int16_t gammatable[] = {0, 0, 0, 0, 1, 1, 2, 3, 4, 5, 6, 7, 9, 11, 12, 14, 16, 18, 21, 23, 26, 28, 31, 34, 37, 40, 44, 47, 51, 55, 58, 62, 67, 71, 75, 80, 84, 89, 94, 99, 104, 110, 115, 121, 126, 132, 138, 144, 150, 157, 163, 170, 177, 184, 191, 198, 205, 212, 220, 228, 235, 243, 251, 260, 268, 276, 285, 294, 303, 312, 321, 330, 339, 349, 358, 368, 378, 388, 398, 408, 419, 429, 440, 451, 462, 473, 484, 496, 507, 519, 530, 542, 554, 566, 579, 591, 603, 616, 629, 642, 655, 668, 681, 695, 708, 722, 736, 750, 764, 778, 792, 807, 822, 836, 851, 866, 881, 897, 912, 928, 943, 959, 975, 991, 1007, 1023, 1040, 1056, 1073, 1090, 1107, 1124, 1141, 1159, 1176, 1194, 1212, 1230, 1248, 1266, 1284, 1302, 1321, 1340, 1358, 1377, 1396, 1416, 1435, 1454, 1474, 1494, 1514, 1534, 1554, 1574, 1594, 1615, 1635, 1656, 1677, 1698, 1719, 1741, 1762, 1784, 1805, 1827, 1849, 1871, 1893, 1916, 1938, 1961, 1984, 2006, 2029, 2053, 2076, 2099, 2123, 2146, 2170, 2194, 2218, 2242, 2267, 2291, 2316, 2340, 2365, 2390, 2415, 2441, 2466, 2491, 2517, 2543, 2569, 2595, 2621, 2647, 2674, 2700, 2727, 2754, 2781, 2808, 2835, 2862, 2890, 2917, 2945, 2973, 3001, 3029, 3057, 3085, 3114, 3143, 3171, 3200, 3229, 3258, 3288, 3317, 3347, 3376, 3406, 3436, 3466, 3496, 3527, 3557, 3588, 3619, 3649, 3680, 3712, 3743, 3774, 3806, 3837, 3869, 3901, 3933, 3965, 3998, 4030, 4063, 4095, 4128, 4161, 4194, 4227, 4261, 4294, 4328, 4362, 4396, 4430, 4464, 4498, 4532, 4567, 4602, 4636, 4671, 4706, 4742, 4777, 4812, 4848, 4884, 4920, 4956, 4992, 5028, 5064, 5101, 5137, 5174, 5211, 5248, 5285, 5322, 5360, 5397, 5435, 5473, 5511, 5549, 5587, 5626, 5664, 5703, 5741, 5780, 5819, 5858, 5898, 5937, 5976, 6016, 6056, 6096, 6136, 6176, 6216, 6257, 6297, 6338, 6379, 6420, 6461, 6502, 6543, 6585, 6627, 6668, 6710, 6752, 6794, 6837, 6879, 6922, 6964, 7007, 7050, 7093, 7136, 7179, 7223, 7266, 7310, 7354, 7398, 7442, 7486, 7531, 7575, 7620, 7665, 7710, 7755, 7800, 7845, 7890, 7936, 7982, 8027, 8073, 8119, 8166, 8212, 8258, 8305, 8352, 8399, 8446, 8493, 8540, 8587, 8635, 8682, 8730, 8778, 8826, 8874, 8923, 8971, 9020, 9068, 9117, 9166, 9215, 9264, 9314, 9363, 9413, 9463, 9512, 9562, 9613, 9663, 9713, 9764, 9814, 9865, 9916, 9967, 10018, 10070, 10121, 10173, 10224, 10276, 10328, 10380, 10433, 10485, 10537, 10590, 10643, 10696, 10749, 10802, 10855, 10909, 10962, 11016, 11070, 11124, 11178, 11232, 11286, 11341, 11395, 11450, 11505, 11560, 11615, 11670, 11725, 11781, 11837, 11892, 11948, 12004, 12060, 12117, 12173, 12230, 12286, 12343, 12400, 12457, 12514, 12572, 12629, 12687, 12745, 12802, 12860, 12919, 12977, 13035, 13094, 13152, 13211, 13270, 13329, 13388, 13448, 13507, 13567, 13626, 13686, 13746, 13806, 13866, 13927, 13987, 14048, 14109, 14170, 14231, 14292, 14353, 14414, 14476, 14538, 14599, 14661, 14723, 14786, 14848, 14910, 14973, 15036, 15099, 15162, 15225, 15288, 15351, 15415, 15478, 15542, 15606, 15670, 15734, 15799, 15863, 15927, 15992, 16057, 16122, 16187, 16252, 16318, 16383, 16449, 16514, 16580, 16646, 16712, 16779, 16845, 16911, 16978, 17045, 17112, 17179, 17246, 17313, 17381, 17448, 17516, 17584, 17652, 17720, 17788, 17856, 17925, 17994, 18062, 18131, 18200, 18269, 18339, 18408, 18478, 18547, 18617, 18687, 18757, 18827, 18897, 18968, 19039, 19109, 19180, 19251, 19322, 19393, 19465, 19536, 19608, 19680, 19752, 19824, 19896, 19968, 20040, 20113, 20186, 20258, 20331, 20404, 20478, 20551, 20624, 20698, 20772, 20846, 20920, 20994, 21068, 21142, 21217, 21291, 21366, 21441, 21516, 21591, 21667, 21742, 21818, 21893, 21969, 22045, 22121, 22197, 22274, 22350, 22427, 22504, 22580, 22657, 22735, 22812, 22889, 22967, 23044, 23122, 23200, 23278, 23356, 23435, 23513, 23592, 23670, 23749, 23828, 23907, 23987, 24066, 24145, 24225, 24305, 24385, 24465, 24545, 24625, 24706, 24786, 24867, 24948, 25029, 25110, 25191, 25272, 25354, 25435, 25517, 25599, 25681, 25763, 25845, 25927, 26010, 26093, 26175, 26258, 26341, 26424, 26508, 26591, 26675, 26758, 26842, 26926, 27010, 27094, 27179, 27263, 27348, 27433, 27517, 27602, 27688, 27773, 27858, 27944, 28029, 28115, 28201, 28287, 28373, 28460, 28546, 28633, 28719, 28806, 28893, 28980, 29067, 29155, 29242, 29330, 29418, 29506, 29594, 29682, 29770, 29858, 29947, 30036, 30124, 30213, 30302, 30392, 30481, 30570, 30660, 30750, 30840, 30930, 31020, 31110, 31200, 31291, 31381, 31472, 31563, 31654, 31745, 31837, 31928, 32019, 32111, 32203, 32295, 32387, 32479, 32572, 32664, 32757, 32849, 32942, 33035, 33128, 33222, 33315, 33408, 33502, 33596, 33690, 33784, 33878, 33972, 34067, 34161, 34256, 34351, 34446, 34541, 34636, 34731, 34827, 34923, 35018, 35114, 35210, 35306, 35403, 35499, 35596, 35692, 35789, 35886, 35983, 36080, 36177, 36275, 36373, 36470, 36568, 36666, 36764, 36862, 36961, 37059, 37158, 37257, 37356, 37455, 37554, 37653, 37752, 37852, 37952, 38051, 38151, 38251, 38352, 38452, 38552, 38653, 38754, 38855, 38956, 39057, 39158, 39259, 39361, 39463, 39564, 39666, 39768, 39870, 39973, 40075, 40178, 40280, 40383, 40486, 40589, 40692, 40796, 40899, 41003, 41107, 41210, 41314, 41419, 41523, 41627, 41732, 41836, 41941, 42046, 42151, 42256, 42362, 42467, 42573, 42678, 42784, 42890, 42996, 43103, 43209, 43315, 43422, 43529, 43636, 43743, 43850, 43957, 44065, 44172, 44280, 44388, 44496, 44604, 44712, 44820, 44929, 45037, 45146, 45255, 45364, 45473, 45582, 45691, 45801, 45911, 46020, 46130, 46240, 46350, 46461, 46571, 46682, 46792, 46903, 47014, 47125, 47236, 47348, 47459, 47571, 47683, 47794, 47906, 48019, 48131, 48243, 48356, 48468, 48581, 48694, 48807, 48920, 49034, 49147, 49261, 49374, 49488, 49602, 49716, 49831, 49945, 50059, 50174, 50289, 50404, 50519, 50634, 50749, 50864, 50980, 51096, 51211, 51327, 51443, 51560, 51676, 51792, 51909, 52026, 52143, 52260, 52377, 52494, 52611, 52729, 52846, 52964, 53082, 53200, 53318, 53437, 53555, 53673, 53792, 53911, 54030, 54149, 54268, 54388, 54507, 54627, 54746, 54866, 54986, 55106, 55227, 55347, 55467, 55588, 55709, 55830, 55951, 56072, 56193, 56315, 56436, 56558, 56680, 56802, 56924, 57046, 57168, 57291, 57414, 57536, 57659, 57782, 57905, 58029, 58152, 58276, 58399, 58523, 58647, 58771, 58895, 59019, 59144, 59269, 59393, 59518, 59643, 59768, 59893, 60019, 60144, 60270, 60396, 60522, 60648, 60774, 60900, 61026, 61153, 61280, 61406, 61533, 61660, 61788, 61915, 62042, 62170, 62298, 62426, 62554, 62682, 62810, 62938, 63067, 63196, 63324, 63453, 63582, 63711, 63841, 63970, 64100, 64229, 64359, 64489, 64619, 64749, 64880, 65010, 65141, 65272, 65402, 65534};
+#define GAMMA_MAX 65535
+#define GAMMA_LEN 1001
diff --git a/openpicc/application/gammatable.py b/openpicc/application/gammatable.py
new file mode 100644
index 0000000..a2e59cd
--- /dev/null
+++ b/openpicc/application/gammatable.py
@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+a = range(1001)
+
+#b = [ (pow(10, (float(e)/500))) * 202  - 202 for e in a]
+b = [ 65534 * (pow((float(e)/1000), 2)) for e in a]
+maximum = max(b)+1
+
+print "/* Autogenerated from gammatable.py */"
+print "const u_int16_t gammatable[] = {" + ", ".join( [str(int(round(int(e)))) for e in b]  ) + "};"
+print "#define GAMMA_MAX %i" % maximum
+print "#define GAMMA_LEN %i" % len(b)
+
diff --git a/openpicc/application/led.c b/openpicc/application/led.c
index cab12b1..df13d21 100644
--- a/openpicc/application/led.c
+++ b/openpicc/application/led.c
@@ -3,6 +3,7 @@
  * OpenBeacon.org - LED support
  *
  * Copyright 2007 Milosch Meriac <meriac@openbeacon.de>
+ * Copyright 2008 Henryk Plötz <henryk@ploetzli.ch>
  *
  ***************************************************************
 
@@ -27,43 +28,102 @@
 #include <board.h>
 #include <task.h>
 #include "led.h"
+
+#include "gammatable.inc"
+
 /**********************************************************************/
 
 #define BLINK_TIME 5
+#define PWM_PERIOD GAMMA_MAX
+const struct channel_definition {
+	int channel_no;
+	enum { PERIPH_NONE=0, PERIPH_A, PERIPH_B} periph;
+	AT91PS_PWMC_CH channel;
+} 
+pwm_channels[] = {
+		[0] =  {0, PERIPH_A,  AT91C_BASE_PWMC_CH0},
+		[1] =  {1, PERIPH_A,  AT91C_BASE_PWMC_CH1},
+		[2] =  {2, PERIPH_A,  AT91C_BASE_PWMC_CH2},
+		[7] =  {3, PERIPH_B,  AT91C_BASE_PWMC_CH3},
+		[11] = {0, PERIPH_B,  AT91C_BASE_PWMC_CH0},
+		[12] = {1, PERIPH_B,  AT91C_BASE_PWMC_CH1},
+		[13] = {2, PERIPH_B,  AT91C_BASE_PWMC_CH2},
+		[14] = {3, PERIPH_B,  AT91C_BASE_PWMC_CH3},
+		[23] = {0, PERIPH_B,  AT91C_BASE_PWMC_CH0},
+		[24] = {1, PERIPH_B,  AT91C_BASE_PWMC_CH1},
+		[25] = {2, PERIPH_B,  AT91C_BASE_PWMC_CH2},
+};
 
-void vLedSetRed(bool_t on)
+static const struct channel_definition* find_channel(unsigned int led)
+{
+	unsigned int pos = ffs(led);
+	if(pos==0) return 0;
+	
+	if(pos-1 >= sizeof(pwm_channels) / sizeof(pwm_channels[0]) ) return 0;
+	if(pwm_channels[pos-1].periph == PERIPH_NONE) return 0;
+	return &pwm_channels[pos-1];
+}
+
+// Brightness is in per thousand
+void vLedSetBrightness(unsigned int led, int brightness)
+{
+	const struct channel_definition *c = find_channel(led);
+	if(c==0) return;
+	
+	if(brightness < 0) brightness = 0;
+	if(brightness > GAMMA_LEN-1) brightness = GAMMA_LEN-1;
+	
+	int duty = gammatable[brightness];
+	if(duty > 1) {
+		c->channel->PWMC_CUPDR = duty;
+		if(c->periph == PERIPH_A) {
+			AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, led, 0);
+		} else if(c->periph == PERIPH_B) {
+			AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, 0, led);
+		}
+	} else {
+	    vLedSet(led, 0);
+	}
+}
+
+void vLedSet(int led, bool_t on)
 {
     if(on)
-	AT91F_PIO_ClearOutput( AT91C_BASE_PIOA, LED_RED );
+    	AT91F_PIO_ClearOutput( AT91C_BASE_PIOA, led );
     else
-        AT91F_PIO_SetOutput( AT91C_BASE_PIOA, LED_RED );
+        AT91F_PIO_SetOutput( AT91C_BASE_PIOA, led );
+    AT91F_PIO_CfgOutput( AT91C_BASE_PIOA, led );
+}
+
+void vLedBlink(int led)
+{
+	volatile int i=0;
+	vLedSet(led, 1);
+	for(i=0; i<BLINK_TIME; i++) {vLedSet(led,0);vLedSet(led,1);}
+	vLedSet(led, 0);	
 }
+
 /**********************************************************************/
+void vLedSetRed(bool_t on)
+{
+	vLedSet(LED_RED, on);
+}
 
 void vLedBlinkRed(void)
 {
-	volatile int i=0;
-	vLedSetRed(1);
-	for(i=0; i<BLINK_TIME; i++) {vLedSetRed(0);vLedSetRed(1);}
-	vLedSetRed(0);
+	vLedBlink(LED_RED);
 }
 /**********************************************************************/
 
 void vLedSetGreen(bool_t on)
 {
-    if(on)
-	AT91F_PIO_ClearOutput( AT91C_BASE_PIOA, LED_GREEN );
-    else
-        AT91F_PIO_SetOutput( AT91C_BASE_PIOA, LED_GREEN );
+	vLedSet(LED_GREEN, on);
 }
 /**********************************************************************/
 
 void vLedBlinkGreen(void)
 {
-	volatile int i=0;
-	vLedSetGreen(1);
-	for(i=0; i<BLINK_TIME; i++) {vLedSetGreen(0);vLedSetGreen(1);}
-	vLedSetGreen(0);
+	vLedBlink(LED_GREEN);
 }
 /**********************************************************************/
 
@@ -72,7 +132,7 @@ void vLedHaltBlinking(int reason)
     volatile u_int32_t i=0;
     while(1)
     {
-        AT91F_PIO_ClearOutput( AT91C_BASE_PIOA, LED_MASK );
+        AT91F_PIO_ClearOutput( AT91C_BASE_PIOA, LED_RED | LED_GREEN );
         for(i=0; i<MCK/40; i++) ;
 	
 	switch(reason) {
@@ -96,15 +156,44 @@ void vLedHaltBlinking(int reason)
 	}
         for(i=0; i<MCK/20; i++) ;
 	
-        AT91F_PIO_SetOutput( AT91C_BASE_PIOA, LED_MASK );
+        AT91F_PIO_SetOutput( AT91C_BASE_PIOA, LED_RED | LED_GREEN );
         for(i=0; i<MCK/40; i++) ;
     }
 }
 /**********************************************************************/
 
+static void init_led_pwm(unsigned int led)
+{
+	const struct channel_definition *c = find_channel(led);
+	
+	if(c==0) return;
+	
+	// Set PIO mapping
+	if(c->periph == PERIPH_A) {
+		AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, led, 0);
+	} else if(c->periph == PERIPH_B) {
+		AT91F_PIO_CfgPeriph(AT91C_BASE_PIOA, 0, led);
+	} else
+		return;
+	
+	// Configure channel: clock = MCK, period = PWM_PERIOD, CPOL=1, duty cycle variable
+	c->channel->PWMC_CMR = 0x0 | AT91C_PWMC_CPOL;
+	c->channel->PWMC_CDTYR = 1;
+	c->channel->PWMC_CPRDR = PWM_PERIOD;
+	
+	AT91C_BASE_PWMC->PWMC_ENA = (1<<(c->channel_no));
+	vLedSetBrightness(led, 0);
+}
+
 void vLedInit(void)
 {
     // turn off LED's 
-    AT91F_PIO_CfgOutput( AT91C_BASE_PIOA, LED_MASK );
-    AT91F_PIO_SetOutput( AT91C_BASE_PIOA, LED_MASK );
+    AT91F_PIO_CfgOutput( AT91C_BASE_PIOA, LED_RED | LED_GREEN );
+    AT91F_PIO_SetOutput( AT91C_BASE_PIOA, LED_RED | LED_GREEN );
+	
+    AT91F_PMC_EnablePeriphClock(AT91C_BASE_PMC,
+				    ((unsigned int) 1 << AT91C_ID_PWMC));
+    
+    init_led_pwm(LED_GREEN);
+    init_led_pwm(LED_RED);
 }
diff --git a/openpicc/application/led.h b/openpicc/application/led.h
index 56699be..b1f00c9 100644
--- a/openpicc/application/led.h
+++ b/openpicc/application/led.h
@@ -25,6 +25,9 @@
 
 #include "openpicc.h"
 
+extern void vLedSetBrightness(unsigned int led, int brightness);
+extern void vLedSet(int led, bool_t on);
+extern void vLedBlink(int led);
 extern void vLedSetRed(bool_t on);
 extern void vLedBlinkRed(void);
 extern void vLedSetGreen(bool_t on);