openwrt-owl/target/linux/omap24xx/patches-2.6.37/900-n810-battery-management.patch

---
 arch/arm/mach-omap2/board-n8x0.c |   13 +
 drivers/cbus/Kconfig             |   12 +
 drivers/cbus/Makefile            |    3 
 drivers/cbus/lipocharge.c        |   63 ++++++
 drivers/cbus/lipocharge.h        |   50 ++++
 drivers/cbus/n810bm_main.c       |  397 +++++++++++++++++++++++++++++++++++++++
 drivers/cbus/retu.c              |    4 
 drivers/cbus/retu.h              |    3 
 drivers/cbus/tahvo.h             |    6 
 9 files changed, 548 insertions(+), 3 deletions(-)

Index: linux-2.6.37/drivers/cbus/Kconfig
===================================================================
--- linux-2.6.37.orig/drivers/cbus/Kconfig	2011-02-06 14:05:49.838388760 +0100
+++ linux-2.6.37/drivers/cbus/Kconfig	2011-02-06 14:05:49.885395646 +0100
@@ -94,4 +94,12 @@
 	  to Retu/Vilma. Detection state and events are exposed through
 	  sysfs.
 
+config N810BM
+	depends on CBUS_RETU && CBUS_TAHVO
+	tristate "Nokia n810 battery management"
+	---help---
+	  Nokia n810 device battery management.
+
+	  If unsure, say N.
+
 endmenu
Index: linux-2.6.37/drivers/cbus/Makefile
===================================================================
--- linux-2.6.37.orig/drivers/cbus/Makefile	2011-02-06 14:05:49.829387442 +0100
+++ linux-2.6.37/drivers/cbus/Makefile	2011-02-06 14:05:49.885395646 +0100
@@ -12,3 +12,6 @@
 obj-$(CONFIG_CBUS_TAHVO_USER)	+= tahvo-user.o
 obj-$(CONFIG_CBUS_RETU_USER)	+= retu-user.o
 obj-$(CONFIG_CBUS_RETU_HEADSET)	+= retu-headset.o
+n810bm-y			+= n810bm_main.o
+n810bm-y			+= lipocharge.o
+obj-$(CONFIG_N810BM)		+= n810bm.o
Index: linux-2.6.37/drivers/cbus/n810bm_main.c
===================================================================
--- /dev/null	1970-01-01 00:00:00.000000000 +0000
+++ linux-2.6.37/drivers/cbus/n810bm_main.c	2011-02-09 13:47:23.831144291 +0100
@@ -0,0 +1,1169 @@
+/*
+ *   Nokia n810 battery management
+ *
+ *   WARNING: This driver is based on unconfirmed documentation.
+ *            It is possibly dangerous to use this software.
+ *            Use this software at your own risk!
+ *
+ *   Copyright (c) 2010-2011 Michael Buesch <mb@bu3sch.de>
+ *
+ *   This program is free software; you can redistribute it and/or
+ *   modify it under the terms of the GNU General Public License
+ *   as published by the Free Software Foundation; either version 2
+ *   of the License, or (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ */
+
+#define DEBUG
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/mutex.h>
+#include <linux/timer.h>
+#include <linux/firmware.h>
+#include <linux/bitops.h>
+#include <linux/workqueue.h>
+#include <linux/delay.h>
+
+#include "cbus.h"
+#include "retu.h"
+#include "tahvo.h"
+#include "lipocharge.h"
+
+
+#define N810BM_PMM_BLOCK_FILENAME	"n810-cal-bme-pmm.fw"
+#define N810BM_PMM_BLOCK_SIZE		0x600
+
+#define N810BM_CHECK_INTERVAL		(HZ * 2)
+#define N810BM_MIN_VOLTAGE_THRES	3300 /* Absolute minimum voltage threshold */
+
+
+/* RETU_ADC_BSI
+ * The battery size indicator ADC measures the resistance between
+ * the battery BSI pin and ground. This is used to detect the battery
+ * capacity, as the BSI resistor is related to capacity.
+ *
+ * Manually measured lookup table.
+ * Hard to measure, thus not very accurate.
+ *
+ * Resistance  |  ADC value
+ * ========================
+ * 120k        |  0x3AC
+ * 110k        |  0x37C
+ * 100k        |  0x351
+ *  90k        |  0x329
+ */
+
+/* RETU_ADC_BATTVOLT
+ * Manually measured lookup table.
+ * Hard to measure, thus not very accurate.
+ *
+ * Voltage  |  ADC value
+ * =====================
+ * 2.80V    |  0x037
+ * 2.90V    |  0x05E
+ * 3.00V    |  0x090
+ * 3.10V    |  0x0A4
+ * 3.20V    |  0x0CC
+ * 3.30V    |  0x0EF
+ * 3.40V    |  0x115
+ * 3.50V    |  0x136
+ * 3.60V    |  0x15C
+ * 3.70V    |  0x187
+ * 3.80V    |  0x1A5
+ * 3.90V    |  0x1C9
+ * 4.00V    |  0x1ED
+ * 4.10V    |  0x212
+ * 4.20V    |  0x236
+ */
+
+
+enum n810bm_capacity {
+	N810BM_CAP_UNKNOWN	= -1,
+	N810BM_CAP_NONE		= 0,
+	N810BM_CAP_1500MAH	= 1500,	/* 1500 mAh battery */
+};
+
+enum n810bm_notify_flags {
+	N810BM_NOTIFY_battery_charging,
+	N810BM_NOTIFY_charger_pwm,
+};
+
+struct n810bm {
+	bool battery_present;			/* A battery is inserted */
+	bool charger_present;			/* The charger is connected */
+	enum n810bm_capacity capacity;		/* The capacity of the inserted battery (if any) */
+
+	bool charger_enabled;			/* Want to charge? */
+	struct lipocharge charger;		/* Charger subsystem */
+	unsigned int active_current_pwm;	/* Active value of TAHVO_REG_CHGCURR */
+	int current_measure_enabled;		/* Current measure enable refcount */
+
+	struct platform_device *pdev;
+	const struct firmware *pmm_block;	/* CAL PMM block */
+
+	bool verbose_charge_log;		/* Verbose charge logging */
+
+	unsigned long notify_flags;
+	struct work_struct notify_work;
+	struct work_struct currmeas_irq_work;
+	struct delayed_work periodic_check_work;
+
+	bool initialized;			/* The hardware was initialized */
+	struct mutex mutex;
+};
+
+static void n810bm_notify_battery_charging(struct n810bm *bm);
+static void n810bm_notify_charger_pwm(struct n810bm *bm);
+
+
+static inline struct n810bm * device_to_n810bm(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct n810bm *bm = platform_get_drvdata(pdev);
+
+	return bm;
+}
+
+static inline bool n810bm_known_battery_present(struct n810bm *bm)
+{
+	return bm->battery_present &&
+	       bm->capacity != N810BM_CAP_UNKNOWN &&
+	       bm->capacity != N810BM_CAP_NONE;
+}
+
+static NORET_TYPE void n810bm_emergency(struct n810bm *bm, const char *message) ATTRIB_NORET;
+static void n810bm_emergency(struct n810bm *bm, const char *message)
+{
+	printk(KERN_EMERG "n810 battery management fatal fault: %s\n", message);
+	cbus_emergency();
+}
+
+static u16 tahvo_read(struct n810bm *bm, unsigned int reg)
+{
+	int ret;
+	unsigned long flags;
+
+	spin_lock_irqsave(&tahvo_lock, flags);
+	ret = tahvo_read_reg(reg);
+	spin_unlock_irqrestore(&tahvo_lock, flags);
+	if (ret < 0 || ret > 0xFFFF)
+		n810bm_emergency(bm, "tahvo_read");
+
+	return ret;
+}
+
+static void tahvo_maskset(struct n810bm *bm, unsigned int reg, u16 mask, u16 set)
+{
+	int ret;
+	unsigned long flags;
+	u16 value;
+
+	spin_lock_irqsave(&tahvo_lock, flags);
+	if (~mask) {
+		ret = tahvo_read_reg(reg);
+		if (ret < 0 || ret > 0xFFFF)
+			goto fatal_unlock;
+		value = ret;
+	} else
+		value = 0;
+	value &= ~mask;
+	value |= set;
+	ret = tahvo_write_reg(reg, value);
+	if (ret)
+		goto fatal_unlock;
+	spin_unlock_irqrestore(&tahvo_lock, flags);
+
+	return;
+
+fatal_unlock:
+	spin_unlock_irqrestore(&tahvo_lock, flags);
+	n810bm_emergency(bm, "tahvo_maskset");
+}
+
+static inline void tahvo_write(struct n810bm *bm, unsigned int reg, u16 value)
+{
+	tahvo_maskset(bm, reg, 0xFFFF, value);
+}
+
+static inline void tahvo_set(struct n810bm *bm, unsigned int reg, u16 mask)
+{
+	tahvo_maskset(bm, reg, mask, mask);
+}
+
+static inline void tahvo_clear(struct n810bm *bm, unsigned int reg, u16 mask)
+{
+	tahvo_maskset(bm, reg, mask, 0);
+}
+
+static u16 retu_read(struct n810bm *bm, unsigned int reg)
+{
+	int ret;
+	unsigned long flags;
+
+	spin_lock_irqsave(&retu_lock, flags);
+	ret = retu_read_reg(reg);
+	spin_unlock_irqrestore(&retu_lock, flags);
+	if (ret < 0 || ret > 0xFFFF)
+		n810bm_emergency(bm, "retu_read");
+
+	return ret;
+}
+
+static void retu_maskset(struct n810bm *bm, unsigned int reg, u16 mask, u16 set)
+{
+	int ret;
+	unsigned long flags;
+	u16 value;
+
+	spin_lock_irqsave(&retu_lock, flags);
+	if (~mask) {
+		ret = retu_read_reg(reg);
+		if (ret < 0 || ret > 0xFFFF)
+			goto fatal_unlock;
+		value = ret;
+	} else
+		value = 0;
+	value &= ~mask;
+	value |= set;
+	ret = retu_write_reg(reg, value);
+	if (ret)
+		goto fatal_unlock;
+	spin_unlock_irqrestore(&retu_lock, flags);
+
+	return;
+
+fatal_unlock:
+	spin_unlock_irqrestore(&retu_lock, flags);
+	n810bm_emergency(bm, "retu_maskset");
+}
+
+static inline void retu_write(struct n810bm *bm, unsigned int reg, u16 value)
+{
+	retu_maskset(bm, reg, 0xFFFF, value);
+}
+
+static int retu_adc_average(struct n810bm *bm, unsigned int chan,
+			    unsigned int nr_passes)
+{
+	unsigned int i, value = 0;
+	int ret;
+
+	if (WARN_ON(!nr_passes))
+		return 0;
+	for (i = 0; i < nr_passes; i++) {
+		ret = retu_read_adc(chan);
+		if (ret < 0)
+			return ret;
+		value += ret;
+	}
+	value /= nr_passes;
+
+	return value;
+}
+
+/* Set the current measure timer that triggers on Tahvo IRQ 7
+ * An interval of zero disables the timer. */
+static void n810bm_set_current_measure_timer(struct n810bm *bm,
+					     u16 millisec_interval)
+{
+	u16 value = millisec_interval;
+
+	if (value <= 0xF905) {
+		value = ((u64)0x10624DD3 * (u64)(value + 0xF9)) >> 32;
+		value /= 16;
+	} else
+		value = 0xFF;
+
+	tahvo_write(bm, TAHVO_REG_BATCURRTIMER, value & 0xFF);
+
+	tahvo_set(bm, TAHVO_REG_CHGCTL,
+		  TAHVO_REG_CHGCTL_CURTIMRST);
+	tahvo_clear(bm, TAHVO_REG_CHGCTL,
+		    TAHVO_REG_CHGCTL_CURTIMRST);
+
+	if (millisec_interval)
+		tahvo_enable_irq(TAHVO_INT_BATCURR);
+	else
+		tahvo_disable_irq(TAHVO_INT_BATCURR);
+
+	//TODO also do a software timer for safety.
+}
+
+static void n810bm_enable_current_measure(struct n810bm *bm)
+{
+	WARN_ON(bm->current_measure_enabled < 0);
+	if (!bm->current_measure_enabled) {
+		/* Enable the current measurement circuitry */
+		tahvo_set(bm, TAHVO_REG_CHGCTL,
+			  TAHVO_REG_CHGCTL_CURMEAS);
+		dev_dbg(&bm->pdev->dev,
+			"Current measurement circuitry enabled");
+	}
+	bm->current_measure_enabled++;
+}
+
+static void n810bm_disable_current_measure(struct n810bm *bm)
+{
+	bm->current_measure_enabled--;
+	WARN_ON(bm->current_measure_enabled < 0);
+	if (!bm->current_measure_enabled) {
+		/* Disable the current measurement circuitry */
+		tahvo_clear(bm, TAHVO_REG_CHGCTL,
+			    TAHVO_REG_CHGCTL_CURMEAS);
+		dev_dbg(&bm->pdev->dev,
+			"Current measurement circuitry disabled");
+	}
+}
+
+/* Measure the actual battery current. Returns a signed value in mA.
+ * Does only work, if current measurement was enabled. */
+static int n810bm_measure_batt_current(struct n810bm *bm)
+{
+	u16 retval;
+	int adc = 0, ma, i;
+
+	if (WARN_ON(bm->current_measure_enabled <= 0))
+		return 0;
+	for (i = 0; i < 3; i++) {
+		retval = tahvo_read(bm, TAHVO_REG_BATCURR);
+		adc += (s16)retval; /* Value is signed */
+	}
+	adc /= 3;
+
+	//TODO convert to mA
+	ma = adc;
+
+	return ma;
+}
+
+/* Requires bm->mutex locked */
+static int n810bm_measure_batt_current_async(struct n810bm *bm)
+{
+	int ma;
+	bool charging = lipocharge_is_charging(&bm->charger);
+
+	n810bm_enable_current_measure(bm);
+	if (!charging)
+		WARN_ON(bm->active_current_pwm != 0);
+	tahvo_maskset(bm, TAHVO_REG_CHGCTL,
+		      TAHVO_REG_CHGCTL_EN |
+		      TAHVO_REG_CHGCTL_PWMOVR |
+		      TAHVO_REG_CHGCTL_PWMOVRZERO,
+		      TAHVO_REG_CHGCTL_EN |
+		      TAHVO_REG_CHGCTL_PWMOVR |
+		      (charging ? 0 : TAHVO_REG_CHGCTL_PWMOVRZERO));
+	ma = n810bm_measure_batt_current(bm);
+	tahvo_maskset(bm, TAHVO_REG_CHGCTL,
+		      TAHVO_REG_CHGCTL_EN |
+		      TAHVO_REG_CHGCTL_PWMOVR |
+		      TAHVO_REG_CHGCTL_PWMOVRZERO,
+		      (charging ? TAHVO_REG_CHGCTL_EN : 0));
+	n810bm_disable_current_measure(bm);
+
+	return ma;
+}
+
+static int adc_sanity_check(struct n810bm *bm, unsigned int channel)
+{
+	int value;
+
+	value = retu_read_adc(channel);
+	if (value < 0) {
+		dev_err(&bm->pdev->dev, "Failed to read GND ADC channel %u",
+			channel);
+		return -EIO;
+	}
+	dev_dbg(&bm->pdev->dev,
+		"GND ADC channel %u sanity check got value: %d",
+		channel, value);
+	if (value > 5) {
+		n810bm_emergency(bm, "GND ADC sanity check failed");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int n810bm_check_adc_sanity(struct n810bm *bm)
+{
+	int err;
+
+	/* Discard one conversion */
+	retu_write(bm, RETU_REG_ADCSCR, 0);
+	retu_read_adc(RETU_ADC_GND2);
+
+	err = adc_sanity_check(bm, RETU_ADC_GND2);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+/* Measure the battery voltage. Returns the value in mV (or negative value on error). */
+static int n810bm_measure_batt_voltage(struct n810bm *bm)
+{
+	int adc;
+	unsigned int mv;
+	const unsigned int scale = 1000;
+
+	adc = retu_adc_average(bm, RETU_ADC_BATTVOLT, 5);
+	if (adc < 0)
+		return adc;
+	if (adc <= 0x37)
+		return 2800;
+	mv = 2800 + ((adc - 0x37) * (((4200 - 2800) * scale) / (0x236 - 0x37))) / scale;
+
+	//TODO compensate for power consumption
+	//TODO honor calibration values
+
+	return mv;
+}
+
+/* Measure the charger voltage. Returns the value in mV (or negative value on error). */
+static int n810bm_measure_charger_voltage(struct n810bm *bm)
+{
+	int adc;
+	unsigned int mv;
+
+	adc = retu_adc_average(bm, RETU_ADC_CHGVOLT, 5);
+	if (adc < 0)
+		return adc;
+	//TODO convert to mV
+	mv = adc;
+
+	return mv;
+}
+
+/* Measure backup battery voltage. Returns the value in mV (or negative value on error). */
+static int n810bm_measure_backup_batt_voltage(struct n810bm *bm)
+{
+	int adc;
+	unsigned int mv;
+
+	adc = retu_adc_average(bm, RETU_ADC_BKUPVOLT, 3);
+	if (adc < 0)
+		return adc;
+	//TODO convert to mV
+	mv = adc;
+
+	return mv;
+}
+
+/* Measure the battery temperature. Returns the value in K (or negative value on error). */
+static int n810bm_measure_batt_temp(struct n810bm *bm)
+{
+	int adc;
+	unsigned int k;
+
+	adc = retu_adc_average(bm, RETU_ADC_BATTEMP, 3);
+	if (adc < 0)
+		return adc;
+	//TODO convert to K
+	k = adc;
+
+	return k;
+}
+
+/* Read the battery capacity via BSI pin. */
+static enum n810bm_capacity n810bm_read_batt_capacity(struct n810bm *bm)
+{
+	int adc;
+	const unsigned int hyst = 20;
+
+	adc = retu_adc_average(bm, RETU_ADC_BSI, 5);
+	if (adc < 0) {
+		dev_err(&bm->pdev->dev, "Failed to read BSI ADC");
+		return N810BM_CAP_UNKNOWN;
+	}
+
+	if (adc >= 0x3B5 - hyst && adc <= 0x3B5 + hyst)
+		return N810BM_CAP_1500MAH;
+
+	dev_err(&bm->pdev->dev, "Capacity indicator 0x%X unknown", adc);
+
+	return N810BM_CAP_UNKNOWN;
+}
+
+/* Convert a battery voltage (in mV) to percentage. */
+static unsigned int n810bm_mvolt2percent(unsigned int mv)
+{
+	const unsigned int minv = 3700;
+	const unsigned int maxv = 4150;
+	unsigned int percent;
+
+	mv = clamp(mv, minv, maxv);
+	percent = (mv - minv) * 100 / (maxv - minv);
+
+	return percent;
+}
+
+static void n810bm_start_charge(struct n810bm *bm)
+{
+	int err;
+
+	WARN_ON(!bm->battery_present);
+	WARN_ON(!bm->charger_present);
+
+	/* Set PWM to zero */
+	bm->active_current_pwm = 0;
+	tahvo_write(bm, TAHVO_REG_CHGCURR, bm->active_current_pwm);
+
+	/* Charge global enable */
+	tahvo_maskset(bm, TAHVO_REG_CHGCTL,
+		      TAHVO_REG_CHGCTL_EN |
+		      TAHVO_REG_CHGCTL_PWMOVR |
+		      TAHVO_REG_CHGCTL_PWMOVRZERO,
+		      TAHVO_REG_CHGCTL_EN);
+
+	WARN_ON((int)bm->capacity <= 0);
+	bm->charger.capacity = bm->capacity;
+	err = lipocharge_start(&bm->charger);
+	WARN_ON(err);
+
+	/* Initialize current measurement circuitry */
+	n810bm_enable_current_measure(bm);
+	n810bm_set_current_measure_timer(bm, 250);
+
+	dev_info(&bm->pdev->dev, "Charging battery");
+	n810bm_notify_charger_pwm(bm);
+	n810bm_notify_battery_charging(bm);
+}
+
+static void n810bm_stop_charge(struct n810bm *bm)
+{
+	if (lipocharge_is_charging(&bm->charger)) {
+		n810bm_set_current_measure_timer(bm, 0);
+		n810bm_disable_current_measure(bm);
+	}
+	lipocharge_stop(&bm->charger);
+
+	/* Set PWM to zero */
+	bm->active_current_pwm = 0;
+	tahvo_write(bm, TAHVO_REG_CHGCURR, bm->active_current_pwm);
+
+	/* Charge global disable */
+	tahvo_maskset(bm, TAHVO_REG_CHGCTL,
+		      TAHVO_REG_CHGCTL_EN |
+		      TAHVO_REG_CHGCTL_PWMOVR |
+		      TAHVO_REG_CHGCTL_PWMOVRZERO,
+		      0);
+
+	dev_info(&bm->pdev->dev, "Not charging battery");
+	n810bm_notify_charger_pwm(bm);
+	n810bm_notify_battery_charging(bm);
+}
+
+/* Periodic check */
+static void n810bm_periodic_check_work(struct work_struct *work)
+{
+	struct n810bm *bm = container_of(to_delayed_work(work),
+					 struct n810bm, periodic_check_work);
+	u16 status;
+	bool battery_was_present, charger_was_present;
+	bool force_charge = 0;
+	int mv;
+
+	mutex_lock(&bm->mutex);
+
+	status = retu_read(bm, RETU_REG_STATUS);
+	battery_was_present = bm->battery_present;
+	charger_was_present = bm->charger_present;
+	bm->battery_present = !!(status & RETU_REG_STATUS_BATAVAIL);
+	bm->charger_present = !!(status & RETU_REG_STATUS_CHGPLUG);
+
+	if (bm->battery_present != battery_was_present) {
+		/* Battery state changed */
+		if (bm->battery_present) {
+			bm->capacity = n810bm_read_batt_capacity(bm);
+			if (bm->capacity == N810BM_CAP_UNKNOWN) {
+				dev_err(&bm->pdev->dev, "Unknown battery detected");
+			} else {
+				dev_info(&bm->pdev->dev, "Detected %u mAh battery",
+					 (unsigned int)bm->capacity);
+			}
+		} else {
+			bm->capacity = N810BM_CAP_NONE;
+			dev_info(&bm->pdev->dev, "The main battery was removed");
+			//TODO disable charging
+		}
+	}
+
+	if (bm->charger_present != charger_was_present) {
+		/* Charger state changed */
+		dev_info(&bm->pdev->dev, "The charger was %s",
+			 bm->charger_present ? "plugged in" : "removed");
+	}
+
+	if (bm->battery_present && !lipocharge_is_charging(&bm->charger)) {
+		/* We're draining the battery */
+		mv = n810bm_measure_batt_voltage(bm);
+		if (mv < 0)
+			n810bm_emergency(bm, "check timer: Failed to measure");
+		if (!bm->charger_present) {
+			if (mv < N810BM_MIN_VOLTAGE_THRES) {
+				n810bm_emergency(bm, "check timer: "
+					"Minimum voltage threshold reached");
+			}
+			force_charge = 1;
+		}
+	}
+
+	if (bm->charger_present && n810bm_known_battery_present(bm)) {
+		/* Known battery and charger are connected */
+		if (bm->charger_enabled || force_charge) {
+			/* Charger is enabled */
+			if (!lipocharge_is_charging(&bm->charger)) {
+				//TODO start charging, if battery is below some threshold
+				n810bm_start_charge(bm);
+			}
+		}
+	}
+
+	if (lipocharge_is_charging(&bm->charger) && !bm->charger_present) {
+		/* Charger was unplugged. */
+		n810bm_stop_charge(bm);
+	}
+
+	mutex_unlock(&bm->mutex);
+	schedule_delayed_work(&bm->periodic_check_work,
+			      round_jiffies_relative(N810BM_CHECK_INTERVAL));
+}
+
+static void n810bm_adc_irq_handler(unsigned long data)
+{
+	struct n810bm *bm = (struct n810bm *)data;
+
+	retu_ack_irq(RETU_INT_ADCS);
+	//TODO
+dev_info(&bm->pdev->dev, "ADC interrupt triggered\n");
+}
+
+static void n810bm_tahvo_current_measure_work(struct work_struct *work)
+{
+	struct n810bm *bm = container_of(work, struct n810bm, currmeas_irq_work);
+	int res, ma, mv, temp;
+
+	mutex_lock(&bm->mutex);
+	if (!lipocharge_is_charging(&bm->charger))
+		goto out_unlock;
+
+	tahvo_maskset(bm, TAHVO_REG_CHGCTL,
+		      TAHVO_REG_CHGCTL_PWMOVR |
+		      TAHVO_REG_CHGCTL_PWMOVRZERO,
+		      TAHVO_REG_CHGCTL_PWMOVR);
+	ma = n810bm_measure_batt_current(bm);
+	tahvo_maskset(bm, TAHVO_REG_CHGCTL,
+		      TAHVO_REG_CHGCTL_PWMOVR |
+		      TAHVO_REG_CHGCTL_PWMOVRZERO,
+		      TAHVO_REG_CHGCTL_PWMOVR |
+		      TAHVO_REG_CHGCTL_PWMOVRZERO);
+	msleep(10);
+	mv = n810bm_measure_batt_voltage(bm);
+	tahvo_maskset(bm, TAHVO_REG_CHGCTL,
+		      TAHVO_REG_CHGCTL_PWMOVR |
+		      TAHVO_REG_CHGCTL_PWMOVRZERO,
+		      0);
+	temp = n810bm_measure_batt_temp(bm);
+	if (WARN_ON(mv < 0))
+		goto out_unlock;
+	if (WARN_ON(temp < 0))
+		goto out_unlock;
+
+	if (bm->verbose_charge_log) {
+		dev_info(&bm->pdev->dev,
+			 "Battery charge state: %d mV, %d mA (%s)",
+			 mv, ma,
+			 (ma <= 0) ? "discharging" : "charging");
+	}
+	res = lipocharge_update_state(&bm->charger, mv, ma, temp);
+	if (res) {
+		if (res > 0)
+			dev_info(&bm->pdev->dev, "Battery fully charged");
+		n810bm_stop_charge(bm);
+	}
+out_unlock:
+	mutex_unlock(&bm->mutex);
+}
+
+static void n810bm_tahvo_current_measure_irq_handler(unsigned long data)
+{
+	struct n810bm *bm = (struct n810bm *)data;
+
+	tahvo_ack_irq(TAHVO_INT_BATCURR);
+	schedule_work(&bm->currmeas_irq_work);
+}
+
+#define DEFINE_ATTR_NOTIFY(attr_name)						\
+	void n810bm_notify_##attr_name(struct n810bm *bm)			\
+	{									\
+		set_bit(N810BM_NOTIFY_##attr_name, &bm->notify_flags);		\
+		wmb();								\
+		schedule_work(&bm->notify_work);				\
+	}
+
+#define DEFINE_SHOW_INT_FUNC(name, member)					\
+	static ssize_t n810bm_attr_##name##_show(struct device *dev,		\
+						 struct device_attribute *attr,	\
+						 char *buf)			\
+	{									\
+		struct n810bm *bm = device_to_n810bm(dev);			\
+		ssize_t count;							\
+										\
+		mutex_lock(&bm->mutex);						\
+		count = snprintf(buf, PAGE_SIZE, "%d\n", (int)(bm->member));	\
+		mutex_unlock(&bm->mutex);					\
+										\
+		return count;							\
+	}
+
+#define DEFINE_STORE_INT_FUNC(name, member)					\
+	static ssize_t n810bm_attr_##name##_store(struct device *dev,		\
+						  struct device_attribute *attr,\
+						  const char *buf, size_t count)\
+	{									\
+		struct n810bm *bm = device_to_n810bm(dev);			\
+		long val;							\
+		int err;							\
+										\
+		mutex_lock(&bm->mutex);						\
+		err = strict_strtol(buf, 0, &val);				\
+		if (!err)							\
+			bm->member = (typeof(bm->member))val;			\
+		mutex_unlock(&bm->mutex);					\
+										\
+		return err ? err : count;					\
+	}
+
+#define DEFINE_ATTR_SHOW_INT(name, member)					\
+	DEFINE_SHOW_INT_FUNC(name, member)					\
+	static DEVICE_ATTR(name, S_IRUGO,					\
+			   n810bm_attr_##name##_show, NULL);
+
+#define DEFINE_ATTR_SHOW_STORE_INT(name, member)				\
+	DEFINE_SHOW_INT_FUNC(name, member)					\
+	DEFINE_STORE_INT_FUNC(name, member)					\
+	static DEVICE_ATTR(name, S_IRUGO | S_IWUSR,				\
+			   n810bm_attr_##name##_show,				\
+			   n810bm_attr_##name##_store);
+
+DEFINE_ATTR_SHOW_INT(battery_present, battery_present);
+DEFINE_ATTR_SHOW_INT(charger_present, charger_present);
+DEFINE_ATTR_SHOW_INT(charger_pwm, active_current_pwm);
+static DEFINE_ATTR_NOTIFY(charger_pwm);
+DEFINE_ATTR_SHOW_STORE_INT(charger_enable, charger_enabled);
+DEFINE_ATTR_SHOW_STORE_INT(charger_verbose, verbose_charge_log);
+
+static ssize_t n810bm_attr_battery_charging(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf)
+{
+	struct n810bm *bm = device_to_n810bm(dev);
+	ssize_t count;
+
+	mutex_lock(&bm->mutex);
+	count = snprintf(buf, PAGE_SIZE, "%d\n",
+			 (int)lipocharge_is_charging(&bm->charger));
+	mutex_unlock(&bm->mutex);
+
+	return count;
+}
+static DEVICE_ATTR(battery_charging, S_IRUGO,
+		   n810bm_attr_battery_charging, NULL);
+static DEFINE_ATTR_NOTIFY(battery_charging);
+
+static ssize_t n810bm_attr_battery_level_show(struct device *dev,
+					      struct device_attribute *attr,
+					      char *buf)
+{
+	struct n810bm *bm = device_to_n810bm(dev);
+	ssize_t count = -ENODEV;
+	int millivolt;
+
+	mutex_lock(&bm->mutex);
+	if (!bm->battery_present || lipocharge_is_charging(&bm->charger))
+		millivolt = 0;
+	else
+		millivolt = n810bm_measure_batt_voltage(bm);
+	if (millivolt >= 0) {
+		count = snprintf(buf, PAGE_SIZE, "%u\n",
+				 n810bm_mvolt2percent(millivolt));
+	}
+	mutex_unlock(&bm->mutex);
+
+	return count;
+}
+static DEVICE_ATTR(battery_level, S_IRUGO,
+		   n810bm_attr_battery_level_show, NULL);
+
+static ssize_t n810bm_attr_battery_capacity_show(struct device *dev,
+						 struct device_attribute *attr,
+						 char *buf)
+{
+	struct n810bm *bm = device_to_n810bm(dev);
+	ssize_t count;
+	int capacity = 0;
+
+	mutex_lock(&bm->mutex);
+	if (n810bm_known_battery_present(bm))
+		capacity = (int)bm->capacity;
+	count = snprintf(buf, PAGE_SIZE, "%d\n", capacity);
+	mutex_unlock(&bm->mutex);
+
+	return count;
+}
+static DEVICE_ATTR(battery_capacity, S_IRUGO,
+		   n810bm_attr_battery_capacity_show, NULL);
+
+static ssize_t n810bm_attr_battery_temp_show(struct device *dev,
+					     struct device_attribute *attr,
+					     char *buf)
+{
+	struct n810bm *bm = device_to_n810bm(dev);
+	ssize_t count = -ENODEV;
+	int k;
+
+	mutex_lock(&bm->mutex);
+	k = n810bm_measure_batt_temp(bm);
+	if (k >= 0)
+		count = snprintf(buf, PAGE_SIZE, "%d\n", k);
+	mutex_unlock(&bm->mutex);
+
+	return count;
+}
+static DEVICE_ATTR(battery_temp, S_IRUGO,
+		   n810bm_attr_battery_temp_show, NULL);
+
+static ssize_t n810bm_attr_charger_voltage_show(struct device *dev,
+						struct device_attribute *attr,
+						char *buf)
+{
+	struct n810bm *bm = device_to_n810bm(dev);
+	ssize_t count = -ENODEV;
+	int mv = 0;
+
+	mutex_lock(&bm->mutex);
+	if (bm->charger_present)
+		mv = n810bm_measure_charger_voltage(bm);
+	if (mv >= 0)
+		count = snprintf(buf, PAGE_SIZE, "%d\n", mv);
+	mutex_unlock(&bm->mutex);
+
+	return count;
+}
+static DEVICE_ATTR(charger_voltage, S_IRUGO,
+		   n810bm_attr_charger_voltage_show, NULL);
+
+static ssize_t n810bm_attr_backup_battery_voltage_show(struct device *dev,
+						       struct device_attribute *attr,
+						       char *buf)
+{
+	struct n810bm *bm = device_to_n810bm(dev);
+	ssize_t count = -ENODEV;
+	int mv;
+
+	mutex_lock(&bm->mutex);
+	mv = n810bm_measure_backup_batt_voltage(bm);
+	if (mv >= 0)
+		count = snprintf(buf, PAGE_SIZE, "%d\n", mv);
+	mutex_unlock(&bm->mutex);
+
+	return count;
+}
+static DEVICE_ATTR(backup_battery_voltage, S_IRUGO,
+		   n810bm_attr_backup_battery_voltage_show, NULL);
+
+static ssize_t n810bm_attr_battery_current_show(struct device *dev,
+						struct device_attribute *attr,
+						char *buf)
+{
+	struct n810bm *bm = device_to_n810bm(dev);
+	ssize_t count = -ENODEV;
+	int ma = 0;
+
+	mutex_lock(&bm->mutex);
+	if (bm->battery_present)
+		ma = n810bm_measure_batt_current_async(bm);
+	count = snprintf(buf, PAGE_SIZE, "%d\n", ma);
+	mutex_unlock(&bm->mutex);
+
+	return count;
+}
+static DEVICE_ATTR(battery_current, S_IRUGO,
+		   n810bm_attr_battery_current_show, NULL);
+
+static const struct device_attribute *n810bm_attrs[] = {
+	&dev_attr_battery_present,
+	&dev_attr_battery_level,
+	&dev_attr_battery_charging,
+	&dev_attr_battery_current,
+	&dev_attr_battery_capacity,
+	&dev_attr_battery_temp,
+	&dev_attr_backup_battery_voltage,
+	&dev_attr_charger_present,
+	&dev_attr_charger_verbose,
+	&dev_attr_charger_voltage,
+	&dev_attr_charger_enable,
+	&dev_attr_charger_pwm,
+};
+
+static void n810bm_notify_work(struct work_struct *work)
+{
+	struct n810bm *bm = container_of(work, struct n810bm, notify_work);
+	unsigned long notify_flags;
+
+	notify_flags = xchg(&bm->notify_flags, 0);
+	mb();
+
+#define do_notify(attr_name)						\
+	do {								\
+		if (notify_flags & (1 << N810BM_NOTIFY_##attr_name)) {	\
+			sysfs_notify(&bm->pdev->dev.kobj, NULL,		\
+				     dev_attr_##attr_name.attr.name);	\
+		}							\
+	} while (0)
+
+	do_notify(battery_charging);
+	do_notify(charger_pwm);
+}
+
+static int n810bm_charger_set_current_pwm(struct lipocharge *c,
+					  unsigned int duty_cycle)
+{
+	struct n810bm *bm = container_of(c, struct n810bm, charger);
+	int err = -EINVAL;
+
+	WARN_ON(!mutex_is_locked(&bm->mutex));
+	if (WARN_ON(duty_cycle > 0xFF))
+		goto out;
+	if (WARN_ON(!bm->charger_enabled))
+		goto out;
+	if (WARN_ON(!bm->battery_present || !bm->charger_present))
+		goto out;
+
+	if (duty_cycle != bm->active_current_pwm) {
+		bm->active_current_pwm = duty_cycle;
+		tahvo_write(bm, TAHVO_REG_CHGCURR, duty_cycle);
+		n810bm_notify_charger_pwm(bm);
+	}
+
+	err = 0;
+out:
+
+	return err;
+}
+
+static void n810bm_charger_emergency(struct lipocharge *c)
+{
+	struct n810bm *bm = container_of(c, struct n810bm, charger);
+
+	n810bm_emergency(bm, "Battery charger fault");
+}
+
+static void n810bm_hw_exit(struct n810bm *bm)
+{
+	n810bm_stop_charge(bm);
+	retu_write(bm, RETU_REG_ADCSCR, 0);
+}
+
+static int n810bm_hw_init(struct n810bm *bm)
+{
+	int err;
+
+	err = n810bm_check_adc_sanity(bm);
+	if (err)
+		return err;
+
+	n810bm_stop_charge(bm);
+
+	return 0;
+}
+
+static void n810bm_cancel_and_flush_work(struct n810bm *bm)
+{
+	cancel_delayed_work_sync(&bm->periodic_check_work);
+	cancel_work_sync(&bm->notify_work);
+	cancel_work_sync(&bm->currmeas_irq_work);
+	flush_scheduled_work();
+}
+
+static int n810bm_device_init(struct n810bm *bm)
+{
+	int attr_index;
+	int err;
+
+	bm->charger.rate = LIPORATE_p6C;
+	bm->charger.top_voltage = 4100;
+	bm->charger.duty_cycle_max = 0xFF;
+	bm->charger.set_current_pwm = n810bm_charger_set_current_pwm;
+	bm->charger.emergency = n810bm_charger_emergency;
+	lipocharge_init(&bm->charger, &bm->pdev->dev);
+
+	err = n810bm_hw_init(bm);
+	if (err)
+		goto error;
+	for (attr_index = 0; attr_index < ARRAY_SIZE(n810bm_attrs); attr_index++) {
+		err = device_create_file(&bm->pdev->dev, n810bm_attrs[attr_index]);
+		if (err)
+			goto err_unwind_attrs;
+	}
+	err = retu_request_irq(RETU_INT_ADCS,
+			       n810bm_adc_irq_handler,
+			       (unsigned long)bm, "n810bm");
+	if (err)
+		goto err_unwind_attrs;
+	err = tahvo_request_irq(TAHVO_INT_BATCURR,
+				n810bm_tahvo_current_measure_irq_handler,
+				(unsigned long)bm, "n810bm");
+	if (err)
+		goto err_free_retu_irq;
+	tahvo_disable_irq(TAHVO_INT_BATCURR);
+
+	schedule_delayed_work(&bm->periodic_check_work,
+			      round_jiffies_relative(N810BM_CHECK_INTERVAL));
+
+	bm->initialized = 1;
+	dev_info(&bm->pdev->dev, "Battery management initialized");
+
+	return 0;
+
+err_free_retu_irq:
+	retu_free_irq(RETU_INT_ADCS);
+err_unwind_attrs:
+	for (attr_index--; attr_index >= 0; attr_index--)
+		device_remove_file(&bm->pdev->dev, n810bm_attrs[attr_index]);
+/*err_exit:*/
+	n810bm_hw_exit(bm);
+error:
+	n810bm_cancel_and_flush_work(bm);
+
+	return err;
+}
+
+static void n810bm_device_exit(struct n810bm *bm)
+{
+	int i;
+
+	if (!bm->initialized)
+		return;
+
+	lipocharge_exit(&bm->charger);
+	tahvo_free_irq(TAHVO_INT_BATCURR);
+	retu_free_irq(RETU_INT_ADCS);
+	for (i = 0; i < ARRAY_SIZE(n810bm_attrs); i++)
+		device_remove_file(&bm->pdev->dev, n810bm_attrs[i]);
+
+	n810bm_cancel_and_flush_work(bm);
+
+	n810bm_hw_exit(bm);
+	release_firmware(bm->pmm_block);
+
+	bm->initialized = 0;
+}
+
+static void n810bm_pmm_block_found(const struct firmware *fw, void *context)
+{
+	struct n810bm *bm = context;
+	int err;
+
+	if (!fw) {
+		dev_err(&bm->pdev->dev,
+			"CAL PMM block image file not found");
+		goto error;
+	}
+	if (fw->size != N810BM_PMM_BLOCK_SIZE ||
+	    memcmp(fw->data, "BME-PMM-BLOCK01", 15) != 0) {
+		dev_err(&bm->pdev->dev,
+			"CAL PMM block image file has an invalid format");
+		goto error;
+	}
+
+	bm->pmm_block = fw;
+	err = n810bm_device_init(bm);
+	if (err) {
+		dev_err(&bm->pdev->dev,
+			"Failed to initialized battery management (%d)", err);
+		goto error;
+	}
+
+	return;
+error:
+	release_firmware(fw);
+}
+
+static int __devinit n810bm_probe(struct platform_device *pdev)
+{
+	struct n810bm *bm;
+	int err;
+
+	bm = kzalloc(sizeof(*bm), GFP_KERNEL);
+	if (!bm)
+		return -ENOMEM;
+	bm->pdev = pdev;
+	platform_set_drvdata(pdev, bm);
+	mutex_init(&bm->mutex);
+	INIT_DELAYED_WORK(&bm->periodic_check_work, n810bm_periodic_check_work);
+	INIT_WORK(&bm->notify_work, n810bm_notify_work);
+	INIT_WORK(&bm->currmeas_irq_work, n810bm_tahvo_current_measure_work);
+
+	dev_info(&bm->pdev->dev, "Requesting CAL BME PMM block firmware file "
+		 N810BM_PMM_BLOCK_FILENAME);
+	err = request_firmware_nowait(THIS_MODULE, 1,
+				      N810BM_PMM_BLOCK_FILENAME,
+				      &bm->pdev->dev, GFP_KERNEL,
+				      bm, n810bm_pmm_block_found);
+	if (err) {
+		dev_err(&bm->pdev->dev,
+			"Failed to request CAL PMM block image file (%d)", err);
+		goto err_free;
+	}
+
+	return 0;
+
+err_free:
+	kfree(bm);
+
+	return err;
+}
+
+static int __devexit n810bm_remove(struct platform_device *pdev)
+{
+	struct n810bm *bm = platform_get_drvdata(pdev);
+
+	n810bm_device_exit(bm);
+
+	kfree(bm);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver n810bm_driver = {
+	.remove		= __devexit_p(n810bm_remove),
+	.driver		= {
+		.name	= "n810bm",
+	}
+};
+
+static int __init n810bm_modinit(void)
+{
+	return platform_driver_probe(&n810bm_driver, n810bm_probe);
+}
+module_init(n810bm_modinit);
+
+static void __exit n810bm_modexit(void)
+{
+	platform_driver_unregister(&n810bm_driver);
+}
+module_exit(n810bm_modexit);
+
+MODULE_DESCRIPTION("Nokia n810 battery management");
+MODULE_FIRMWARE(N810BM_PMM_BLOCK_FILENAME);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Michael Buesch");
Index: linux-2.6.37/drivers/cbus/retu.c
===================================================================
--- linux-2.6.37.orig/drivers/cbus/retu.c	2011-02-06 14:05:49.829387442 +0100
+++ linux-2.6.37/drivers/cbus/retu.c	2011-02-08 17:33:18.981369017 +0100
@@ -85,10 +85,10 @@
  *
  * This function writes a value to the specified register
  */
-void retu_write_reg(int reg, u16 val)
+int retu_write_reg(int reg, u16 val)
 {
 	BUG_ON(!retu_initialized);
-	cbus_write_reg(cbus_host, RETU_ID, reg, val);
+	return cbus_write_reg(cbus_host, RETU_ID, reg, val);
 }
 
 void retu_set_clear_reg_bits(int reg, u16 set, u16 clear)
@@ -459,6 +459,7 @@
 EXPORT_SYMBOL(retu_ack_irq);
 EXPORT_SYMBOL(retu_read_reg);
 EXPORT_SYMBOL(retu_write_reg);
+EXPORT_SYMBOL(retu_read_adc);
 
 subsys_initcall(retu_init);
 module_exit(retu_exit);
Index: linux-2.6.37/drivers/cbus/retu.h
===================================================================
--- linux-2.6.37.orig/drivers/cbus/retu.h	2011-02-06 14:05:49.829387442 +0100
+++ linux-2.6.37/drivers/cbus/retu.h	2011-02-06 14:05:49.886395793 +0100
@@ -40,6 +40,8 @@
 #define RETU_REG_CTRL_CLR	0x0f	/* Regulator clear register */
 #define RETU_REG_CTRL_SET	0x10	/* Regulator set register */
 #define RETU_REG_STATUS		0x16	/* Status register */
+#define  RETU_REG_STATUS_BATAVAIL	0x0100 /* Battery available */
+#define  RETU_REG_STATUS_CHGPLUG	0x1000 /* Charger is plugged in */
 #define RETU_REG_WATCHDOG	0x17	/* Watchdog register */
 #define RETU_REG_AUDTXR		0x18	/* Audio Codec Tx register */
 #define RETU_REG_MAX		0x1f
@@ -57,8 +59,25 @@
 
 #define	MAX_RETU_IRQ_HANDLERS	16
 
+/* ADC channels */
+#define RETU_ADC_GND		0x00 /* Ground */
+#define RETU_ADC_BSI		0x01 /* Battery Size Indicator */
+#define RETU_ADC_BATTEMP	0x02 /* Battery temperature */
+#define RETU_ADC_CHGVOLT	0x03 /* Charger voltage */
+#define RETU_ADC_HEADSET	0x04 /* Headset detection */
+#define RETU_ADC_HOOKDET	0x05 /* Hook detection */
+#define RETU_ADC_RFGP		0x06 /* RF GP */
+#define RETU_ADC_WBTX		0x07 /* Wideband Tx detection */
+#define RETU_ADC_BATTVOLT	0x08 /* Battery voltage measurement */
+#define RETU_ADC_GND2		0x09 /* Ground */
+#define RETU_ADC_LIGHTSENS	0x0A /* Light sensor */
+#define RETU_ADC_LIGHTTEMP	0x0B /* Light sensor temperature */
+#define RETU_ADC_BKUPVOLT	0x0C /* Backup battery voltage */
+#define RETU_ADC_TEMP		0x0D /* RETU temperature */
+
+
 int retu_read_reg(int reg);
-void retu_write_reg(int reg, u16 val);
+int retu_write_reg(int reg, u16 val);
 void retu_set_clear_reg_bits(int reg, u16 set, u16 clear);
 int retu_read_adc(int channel);
 int retu_request_irq(int id, void *irq_handler, unsigned long arg, char *name);
Index: linux-2.6.37/arch/arm/mach-omap2/board-n8x0.c
===================================================================
--- linux-2.6.37.orig/arch/arm/mach-omap2/board-n8x0.c	2011-02-06 14:05:49.815385390 +0100
+++ linux-2.6.37/arch/arm/mach-omap2/board-n8x0.c	2011-02-06 14:05:49.886395793 +0100
@@ -907,6 +907,17 @@
 				    ARRAY_SIZE(n8x0_gpio_switches));
 }
 
+static struct platform_device n810_bm_device = {
+	.name		= "n810bm",
+	.id		= -1,
+};
+
+static void __init n810_bm_init(void)
+{
+	if (platform_device_register(&n810_bm_device))
+		BUG();
+}
+
 static void __init n8x0_init_machine(void)
 {
 	omap2420_mux_init(board_mux, OMAP_PACKAGE_ZAC);
@@ -933,6 +944,8 @@
 	n8x0_onenand_init();
 	n8x0_mmc_init();
 	n8x0_usb_init();
+
+	n810_bm_init();
 }
 
 MACHINE_START(NOKIA_N800, "Nokia N800")
Index: linux-2.6.37/drivers/cbus/lipocharge.c
===================================================================
--- /dev/null	1970-01-01 00:00:00.000000000 +0000
+++ linux-2.6.37/drivers/cbus/lipocharge.c	2011-02-09 12:42:58.243147563 +0100
@@ -0,0 +1,183 @@
+/*
+ *   Generic LIPO battery charger
+ *
+ *   Copyright (c) 2010-2011 Michael Buesch <mb@bu3sch.de>
+ *
+ *   This program is free software; you can redistribute it and/or
+ *   modify it under the terms of the GNU General Public License
+ *   as published by the Free Software Foundation; either version 2
+ *   of the License, or (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ */
+
+#define DEBUG
+
+#include "lipocharge.h"
+
+#include <linux/slab.h>
+
+
+/* Hysteresis constants */
+#define CURRENT_HYST		30 /* mA */
+#define VOLTAGE_HYST		10 /* mV */
+
+/* Threshold constants */
+#define FINISH_CURRENT_PERCENT	3
+
+
+/* Returns the requested first-stage charge current in mA */
+static inline unsigned int get_stage1_charge_current(struct lipocharge *c)
+{
+	/* current = (capacity * C) */
+	return c->capacity * c->rate / 1000;
+}
+
+void lipocharge_init(struct lipocharge *c, struct device *dev)
+{
+	c->dev = dev;
+	c->state = LIPO_IDLE;
+}
+
+void lipocharge_exit(struct lipocharge *c)
+{
+	c->state = LIPO_IDLE;
+}
+
+int lipocharge_start(struct lipocharge *c)
+{
+	int err;
+
+	if (c->state != LIPO_IDLE)
+		return -EBUSY;
+	if (!c->set_current_pwm || !c->emergency)
+		return -EINVAL;
+	if (!c->top_voltage || c->top_voltage > 4200)
+		return -EINVAL;
+
+	c->active_duty_cycle = 0;
+	err = c->set_current_pwm(c, c->active_duty_cycle);
+	if (err)
+		return err;
+	c->state = LIPO_FIRST_STAGE;
+
+	return 0;
+}
+
+void lipocharge_stop(struct lipocharge *c)
+{
+	if (c->state == LIPO_IDLE)
+		return;
+	c->state = LIPO_IDLE;
+}
+
+static int lipocharge_increase_current(struct lipocharge *c,
+				       unsigned int inc_permille)
+{
+	int old_pwm, new_pwm;
+
+	if (c->active_duty_cycle >= c->duty_cycle_max)
+		return 0;
+
+	old_pwm = c->active_duty_cycle;
+	new_pwm = old_pwm + (c->duty_cycle_max * inc_permille / 1000);
+	new_pwm = min(new_pwm, (int)c->duty_cycle_max);
+	c->active_duty_cycle = new_pwm;
+
+	dev_dbg(c->dev, "lipo: Increasing duty_cycle by "
+		"%u permille (0x%02X -> 0x%02X)",
+		inc_permille, old_pwm, new_pwm);
+
+	return c->set_current_pwm(c, c->active_duty_cycle);
+}
+
+static int lipocharge_decrease_current(struct lipocharge *c,
+				       unsigned int dec_permille)
+{
+	int old_pwm, new_pwm;
+
+	if (c->active_duty_cycle <= 0)
+		return 0;
+
+	old_pwm = c->active_duty_cycle;
+	new_pwm = old_pwm - (c->duty_cycle_max * dec_permille / 1000);
+	new_pwm = max(0, new_pwm);
+	c->active_duty_cycle = new_pwm;
+
+	dev_dbg(c->dev, "lipo: Decreasing duty_cycle by "
+		"%u permille (0x%02X -> 0x%02X)",
+		dec_permille, old_pwm, new_pwm);
+
+	return c->set_current_pwm(c, c->active_duty_cycle);
+}
+
+/** lipocharge_update_state - Update the charge state
+ * @c: The context.
+ * @voltage_mV: The measured battery voltage.
+ * @current_mA: The measured charge current.
+ *		negative -> drain.
+ *		positive -> charge.
+ * @temp_K: Battery temperature in K.
+ *
+ * Returns 0 on success, -1 on error.
+ * Returns 1, if the charging process is finished.
+ */
+int lipocharge_update_state(struct lipocharge *c,
+			    unsigned int voltage_mV,
+			    int current_mA,
+			    unsigned int temp_K)
+{
+	int requested_current, current_diff;
+	int err;
+	unsigned int permille;
+
+	//TODO temp
+
+restart:
+	switch (c->state) {
+	case LIPO_IDLE:
+		dev_err(c->dev, "%s: called while idle", __func__);
+		return -EINVAL;
+	case LIPO_FIRST_STAGE:	/* Constant current */
+//printk("GOT %u %d %u\n", voltage_mV, current_mA, temp_K);
+		if (voltage_mV >= c->top_voltage) {
+			/* Float voltage reached.
+			 * Switch charger mode to "constant current" */
+			c->state = LIPO_SECOND_STAGE;
+			dev_dbg(c->dev, "Switched to second charging stage.");
+			goto restart;
+		}
+		/* Float voltage not reached, yet.
+		 * Try to get the requested constant current. */
+		requested_current = get_stage1_charge_current(c);
+		if (current_mA < 0)
+			current_mA = 0;
+		current_diff = requested_current - current_mA;
+		if (abs(requested_current - current_mA) > CURRENT_HYST) {
+			if (current_diff > 0) {
+				/* Increase current */
+				permille = current_diff * 1000 / requested_current;
+				permille /= 2;
+				err = lipocharge_increase_current(c, permille);
+				if (err)
+					return err;
+			} else {
+				/* Decrease current */
+				permille = (-current_diff) * 1000 / requested_current;
+				permille /= 2;
+				err = lipocharge_decrease_current(c, permille);
+				if (err)
+					return err;
+			}
+		}
+		break;
+	case LIPO_SECOND_STAGE:	/* Constant voltage */
+		//TODO
+		break;
+	}
+
+	return 0;
+}
Index: linux-2.6.37/drivers/cbus/lipocharge.h
===================================================================
--- /dev/null	1970-01-01 00:00:00.000000000 +0000
+++ linux-2.6.37/drivers/cbus/lipocharge.h	2011-02-07 20:07:29.669098631 +0100
@@ -0,0 +1,60 @@
+#ifndef LIPOCHARGE_H_
+#define LIPOCHARGE_H_
+
+#include <linux/types.h>
+#include <linux/device.h>
+
+
+#define LIPORATE(a,b)	(((a) * 1000) + ((b) * 100))
+#define LIPORATE_p6C	LIPORATE(0,6)	/* 0.6C */
+
+enum lipocharge_state {
+	LIPO_IDLE,		/* Not charging */
+	LIPO_FIRST_STAGE,	/* Charging: constant current */
+	LIPO_SECOND_STAGE,	/* Charging: constant voltage */
+};
+
+/** struct lipocharge - A generic LIPO charger
+ *
+ * @capacity: Battery capacity in mAh.
+ * @rate: Charge rate.
+ * @top_voltage: Fully charged voltage, in mV.
+ * @duty_cycle_max: Max value for duty_cycle.
+ *
+ * @set_charge_current: Set the charge current PWM duty cycle.
+ * @emergency: Something went wrong. Force shutdown.
+ */
+struct lipocharge {
+	unsigned int capacity;
+	unsigned int rate;
+	unsigned int top_voltage;
+	unsigned int duty_cycle_max;
+
+	int (*set_current_pwm)(struct lipocharge *c, unsigned int duty_cycle);
+	void (*emergency)(struct lipocharge *c);
+
+	/* internal */
+	struct device *dev;
+	enum lipocharge_state state;
+	unsigned int active_duty_cycle;
+
+	//TODO implement timer to cut power after maximum charge time.
+};
+
+void lipocharge_init(struct lipocharge *c, struct device *dev);
+void lipocharge_exit(struct lipocharge *c);
+
+int lipocharge_start(struct lipocharge *c);
+void lipocharge_stop(struct lipocharge *c);
+
+int lipocharge_update_state(struct lipocharge *c,
+			    unsigned int voltage_mV,
+			    int current_mA,
+			    unsigned int temp_K);
+
+static inline bool lipocharge_is_charging(struct lipocharge *c)
+{
+	return (c->state != LIPO_IDLE);
+}
+
+#endif /* LIPOCHARGE_H_ */
Index: linux-2.6.37/drivers/cbus/tahvo.h
===================================================================
--- linux-2.6.37.orig/drivers/cbus/tahvo.h	2011-02-06 14:05:49.830387588 +0100
+++ linux-2.6.37/drivers/cbus/tahvo.h	2011-02-06 16:22:25.902331536 +0100
@@ -30,17 +30,28 @@
 #define TAHVO_REG_IDR		0x01	/* Interrupt ID */
 #define TAHVO_REG_IDSR		0x02	/* Interrupt status */
 #define TAHVO_REG_IMR		0x03	/* Interrupt mask */
+#define TAHVO_REG_CHGCURR	0x04	/* Charge current control PWM (8-bit) */
 #define TAHVO_REG_LEDPWMR	0x05	/* LED PWM */
 #define TAHVO_REG_USBR		0x06	/* USB control */
+#define TAHVO_REG_CHGCTL	0x08	/* Charge control register */
+#define  TAHVO_REG_CHGCTL_EN		0x0001	/* Global charge enable */
+#define  TAHVO_REG_CHGCTL_PWMOVR	0x0004	/* PWM override. Force charge PWM to 0%/100% duty cycle. */
+#define  TAHVO_REG_CHGCTL_PWMOVRZERO	0x0008	/* If set, PWM override is 0% (If unset -> 100%) */
+#define  TAHVO_REG_CHGCTL_CURMEAS	0x0040	/* Enable battery current measurement. */
+#define  TAHVO_REG_CHGCTL_CURTIMRST	0x0080	/* Current measure timer reset. */
+#define TAHVO_REG_BATCURRTIMER	0x0c	/* Battery current measure timer (8-bit) */
+#define TAHVO_REG_BATCURR	0x0d	/* Battery (dis)charge current (signed 16-bit) */
+
 #define TAHVO_REG_MAX		0x0d
 
 /* Interrupt sources */
 #define TAHVO_INT_VBUSON	0
+#define TAHVO_INT_BATCURR	7 /* Battery current measure timer */
 
 #define MAX_TAHVO_IRQ_HANDLERS	8
 
 int tahvo_read_reg(int reg);
-void tahvo_write_reg(int reg, u16 val);
+int tahvo_write_reg(int reg, u16 val);
 void tahvo_set_clear_reg_bits(int reg, u16 set, u16 clear);
 int tahvo_request_irq(int id, void *irq_handler, unsigned long arg, char *name);
 void tahvo_free_irq(int id);
Index: linux-2.6.37/drivers/cbus/tahvo.c
===================================================================
--- linux-2.6.37.orig/drivers/cbus/tahvo.c	2011-02-06 14:05:49.830387588 +0100
+++ linux-2.6.37/drivers/cbus/tahvo.c	2011-02-06 14:05:49.886395793 +0100
@@ -85,10 +85,10 @@
  *
  * This function writes a value to the specified register
  */
-void tahvo_write_reg(int reg, u16 val)
+int tahvo_write_reg(int reg, u16 val)
 {
 	BUG_ON(!tahvo_initialized);
-	cbus_write_reg(cbus_host, TAHVO_ID, reg, val);
+	return cbus_write_reg(cbus_host, TAHVO_ID, reg, val);
 }
 
 /**
Index: linux-2.6.37/drivers/cbus/cbus.c
===================================================================
--- linux-2.6.37.orig/drivers/cbus/cbus.c	2011-02-08 17:34:34.988926130 +0100
+++ linux-2.6.37/drivers/cbus/cbus.c	2011-02-08 17:38:16.980407594 +0100
@@ -31,6 +31,7 @@
 #include <linux/gpio.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
+#include <linux/reboot.h>
 
 #include <asm/io.h>
 #include <asm/mach-types.h>
@@ -301,6 +302,13 @@
 }
 module_exit(cbus_bus_exit);
 
+void cbus_emergency(void)
+{
+	machine_power_off();
+	panic("cbus: Failed to halt machine in emergency state\n");
+}
+EXPORT_SYMBOL(cbus_emergency);
+
 MODULE_DESCRIPTION("CBUS serial protocol");
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Juha Yrj<72>l<EFBFBD>");
Index: linux-2.6.37/drivers/cbus/cbus.h
===================================================================
--- linux-2.6.37.orig/drivers/cbus/cbus.h	2011-02-08 17:36:40.172074049 +0100
+++ linux-2.6.37/drivers/cbus/cbus.h	2011-02-08 17:41:32.680647478 +0100
@@ -33,4 +33,6 @@
 extern int cbus_read_reg(struct cbus_host *host, int dev, int reg);
 extern int cbus_write_reg(struct cbus_host *host, int dev, int reg, u16 val);
 
+NORET_TYPE void cbus_emergency(void) ATTRIB_NORET;
+
 #endif /* __DRIVERS_CBUS_CBUS_H */