[PATCH 3/3] gta02 led fixes

[Holger Freyther]

From 3b6b0073cf689920ea66ea5896959d0d3436c188 Mon Sep 17 00:00:00 2001
From: Holger Freyther <zecke at openmoko.org>
Date: Wed, 9 Apr 2008 18:58:51 +0200
Subject: [PATCH] [led] Do not turn off one LED when turning on another
     We can only write the LED status not read it. Trying to set just one
     GPIO will result in unsetting the others to zero and enabling only the
     new one.

    Keep track of the state and do not turn off the other LEDs when turning
    on one.

    Every access to the GPBXYZ is going to potentially turn off the LEDs, e.g
    turning on the vibrator will turn off the LEDs.
---
 drivers/leds/leds-neo1973-gta02.c |   59 
+++++++++++++++++++++++++++++++++----
 1 files changed, 53 insertions(+), 6 deletions(-)

diff --git a/drivers/leds/leds-neo1973-gta02.c 
b/drivers/leds/leds-neo1973-gta02.c
index 122831c..737e524 100644
--- a/drivers/leds/leds-neo1973-gta02.c
+++ b/drivers/leds/leds-neo1973-gta02.c
@@ -35,7 +35,10 @@ struct gta02_led_priv
 
 struct gta02_led_bundle
 {
+	struct mutex mutex;
 	int num_leds;
+	unsigned long gpio_mask;
+	unsigned long gpio_state;
 	struct gta02_led_priv led[MAX_LEDS];
 };
 
@@ -49,6 +52,39 @@ static inline struct gta02_led_bundle *to_bundle(struct 
led_classdev *led_cdev)
 	return dev_get_drvdata(led_cdev->dev->parent);
 }
 
+static void gta02led_bundle_update(struct gta02_led_bundle *bundle)
+{
+	/* we know the LEDs are on bank B */
+	void __iomem *base = S3C24XX_GPIO_BASE(S3C2410_GPB0);
+	unsigned long flags;
+	unsigned long dat;
+
+	mutex_lock(&bundle->mutex);
+
+	local_irq_save(flags);
+	dat = __raw_readl(base + 0x04);
+	dat &= ~bundle->gpio_mask;
+	dat |= bundle->gpio_state;
+	__raw_writel(dat, base + 0x04);
+	local_irq_restore(flags);
+
+	mutex_unlock(&bundle->mutex);
+}
+
+static void gta02led_bundle_set(struct gta02_led_bundle *bundle, int gpio, 
int value)
+{
+	unsigned long offset = S3C2410_GPIO_OFFSET(gpio);
+	unsigned long state = value != 0;
+
+	mutex_lock(&bundle->mutex);
+
+	bundle->gpio_state &= ~(1L << offset);
+	bundle->gpio_state |= state << offset;
+
+	mutex_unlock(&bundle->mutex);
+}
+
+
 static void gta02led_set(struct led_classdev *led_cdev,
 		enum led_brightness value)
 {
@@ -60,14 +96,14 @@ static void gta02led_set(struct led_classdev *led_cdev,
  	 * value == 0 -> 0% duty cycle (zero power)
  	 */
  	mutex_lock(&lp->mutex);
+
 	if (lp->has_pwm) {
-			s3c2410_pwm_duty_cycle(value, &lp->pwm);
+		s3c2410_pwm_duty_cycle(value, &lp->pwm);
 	} else {
-		if (value)
-			s3c2410_gpio_setpin(lp->gpio, 1);
-		else
-			s3c2410_gpio_setpin(lp->gpio, 0);
+		gta02led_bundle_set(to_bundle(led_cdev), lp->gpio, value);
+		gta02led_bundle_update(to_bundle(led_cdev));
 	}
+
 	mutex_unlock(&lp->mutex);
 }
 
@@ -106,11 +142,13 @@ static int __init gta02led_probe(struct platform_device 
*pdev)
 	bundle = kzalloc(sizeof(struct gta02_led_bundle), GFP_KERNEL);
 	if (!bundle)
 		return -ENOMEM;
+	mutex_init(&bundle->mutex);
 	platform_set_drvdata(pdev, bundle);
 
 	for (i = 0; i < pdev->num_resources; i++) {
 		struct gta02_led_priv *lp;
 		struct resource *r;
+		unsigned long offset;
 
 		if (i >= MAX_LEDS)
 			break;
@@ -164,7 +202,10 @@ static int __init gta02led_probe(struct platform_device 
*pdev)
 		case S3C2410_GPB3:
 			lp->has_pwm = 0;
 			s3c2410_gpio_cfgpin(lp->gpio, S3C2410_GPIO_OUTPUT);
-			s3c2410_gpio_setpin(lp->gpio, 0);
+
+			offset = S3C2410_GPIO_OFFSET(lp->gpio);
+			bundle->gpio_mask |= 1L << offset;
+			gta02led_bundle_set(bundle, lp->gpio, 0);
 			break;
 		default:
 			break;
@@ -175,6 +216,7 @@ static int __init gta02led_probe(struct platform_device 
*pdev)
 	}
 
 	bundle->num_leds = i;
+	gta02led_bundle_update(bundle);
 
 	return 0;
 }
@@ -193,7 +235,12 @@ static int gta02led_remove(struct platform_device *pdev)
 		mutex_destroy(&lp->mutex);
 	}
 
+	/* turn off the leds */
+	bundle->gpio_state = 0;
+	gta02led_bundle_update(bundle);
+
 	platform_set_drvdata(pdev, NULL);
+	mutex_destroy(&bundle->mutex);
 	kfree(bundle);
 
 	return 0;
-- 
1.5.3