5 files changed, 153 insertions, 15 deletions

diff --git a/firmware/export/config/sansaconnect.h b/firmware/export/config/sansaconnect.h
index 5668d579fc..fa929f3c10 100644
--- a/firmware/export/config/sansaconnect.h
+++ b/firmware/export/config/sansaconnect.h
@@ -152,7 +152,15 @@
 #define BATTERY_CAPACITY_INC 100     /* capacity increment */
 #define BATTERY_TYPES_COUNT  1       /* only one type */
 
-#define CONFIG_BATTERY_MEASURE PERCENTAGE_MEASURE
+/* bq27000 provides voltage, percentage and time measure.
+ * Voltage reading is available every 2.56 seconds and does not need filtering.
+ * Read the measured voltage every 3 seconds so we are guaranteed to not read
+ * the same value twice (do not try to read every 2.56 seconds as clocks are
+ * not synchronized).
+ */
+#define CONFIG_BATTERY_MEASURE (VOLTAGE_MEASURE|PERCENTAGE_MEASURE|TIME_MEASURE)
+#define BATT_AVE_SAMPLES 1
+#define POWER_THREAD_STEP_TICKS (3*HZ)
 
 /* define current usage levels */
 #if 0
diff --git a/firmware/powermgmt.c b/firmware/powermgmt.c
index a05e0aeb68..51ea99f6fa 100644
--- a/firmware/powermgmt.c
+++ b/firmware/powermgmt.c
@@ -88,16 +88,12 @@ static long last_event_tick = 0;
 #if (CONFIG_BATTERY_MEASURE & PERCENTAGE_MEASURE) == PERCENTAGE_MEASURE
 #ifdef SIMULATOR
 int _battery_level(void) { return -1; }
-int _battery_voltage(void);
-extern const unsigned short percent_to_volt_discharge[BATTERY_TYPES_COUNT][11];
-extern const unsigned short percent_to_volt_charge[11];
-#else
-int _battery_voltage(void) { return -1; }
-const unsigned short percent_to_volt_discharge[BATTERY_TYPES_COUNT][11];
-const unsigned short percent_to_volt_charge[11];
 #endif
-#elif (CONFIG_BATTERY_MEASURE & VOLTAGE_MEASURE) == VOLTAGE_MEASURE
+#else
 int _battery_level(void) { return -1; }
+#endif
+
+#if (CONFIG_BATTERY_MEASURE & VOLTAGE_MEASURE) == VOLTAGE_MEASURE
 /*
  * Average battery voltage and charger voltage, filtered via a digital
  * exponential filter (aka. exponential moving average, scaled):
@@ -106,10 +102,8 @@ int _battery_level(void) { return -1; }
 static unsigned int avgbat;
 /* filtered battery voltage, millivolts */
 static unsigned int battery_millivolts;
-#elif (CONFIG_BATTERY_MEASURE == 0)
+#else
 int _battery_voltage(void) { return -1; }
-int _battery_level(void) { return -1; }
-
 const unsigned short percent_to_volt_discharge[BATTERY_TYPES_COUNT][11];
 const unsigned short percent_to_volt_charge[11];
 #endif
@@ -156,9 +150,9 @@ void battery_read_info(int *voltage, int *level)
         *voltage = millivolts;
 
     if (level)  {
-        percent = voltage_to_battery_level(millivolts);
+        percent = _battery_level();
         if (percent < 0)
-            percent = _battery_level();
+            percent = voltage_to_battery_level(millivolts);
         *level = percent;
     }
 }
diff --git a/firmware/target/arm/tms320dm320/sansa-connect/avr-sansaconnect.c b/firmware/target/arm/tms320dm320/sansa-connect/avr-sansaconnect.c
index 8ebba3a8d5..ed7dab1572 100644
--- a/firmware/target/arm/tms320dm320/sansa-connect/avr-sansaconnect.c
+++ b/firmware/target/arm/tms320dm320/sansa-connect/avr-sansaconnect.c
@@ -104,8 +104,15 @@
 #define SYS_CTRL_EN_TS_THERM    0x06
 #define SYS_CTRL_FRESET         0x80
 
+/* HDQ status codes */
+#define HDQ_STATUS_OK           0x00
+#define HDQ_STATUS_NOT_READY    0x01
+#define HDQ_STATUS_TIMEOUT      0x02
+
 /* protects spi avr commands from concurrent access */
 static struct mutex avr_mtx;
+/* serializes hdq read/write and status retrieval */
+static struct mutex hdq_mtx;
 
 /* AVR thread events */
 #define INPUT_INTERRUPT          1
@@ -498,6 +505,7 @@ void avr_hid_init(void)
     IO_SERIAL1_MODE = 0x6DB;
 
     mutex_init(&avr_mtx);
+    mutex_init(&hdq_mtx);
 }
 
 int _battery_level(void)
@@ -510,6 +518,17 @@ int _battery_level(void)
     return avr_battery_level & BATTERY_LEVEL_PERCENTAGE_MASK;
 }
 
+int _battery_voltage(void)
+{
+    return avr_hid_hdq_read_short(HDQ_REG_VOLT);
+}
+
+int _battery_time(void)
+{
+    /* HDQ_REG_TTE reads as 65535 when charging */
+    return avr_hid_hdq_read_short(HDQ_REG_TTE);
+}
+
 unsigned int power_input_status(void)
 {
     if (avr_battery_status & BATTERY_STATUS_CHARGER_CONNECTED)
@@ -524,6 +543,70 @@ bool charging_state(void)
     return (avr_battery_status & BATTERY_STATUS_CHARGING) != 0;
 }
 
+static int avr_hid_hdq_read_byte_internal(uint8_t address)
+{
+    uint8_t result[2];
+
+    if (!avr_execute_command(CMD_HDQ_READ, &address, sizeof(address)))
+    {
+        return -1;
+    }
+
+    do
+    {
+        mdelay(10);
+        if (!avr_execute_command(CMD_HDQ_STATUS, result, sizeof(result)))
+        {
+            return -1;
+        }
+    }
+    while (result[0] == HDQ_STATUS_NOT_READY);
+
+    if (result[0] != HDQ_STATUS_OK)
+    {
+        logf("HDQ read %d status %d", address, result[0]);
+        return -1;
+    }
+
+    return result[1];
+}
+
+int avr_hid_hdq_read_byte(uint8_t address)
+{
+    int retry;
+    int value = -1;
+    for (retry = 0; (retry < 3) && (value < 0); retry++)
+    {
+        mutex_lock(&hdq_mtx);
+        value = avr_hid_hdq_read_byte_internal(address);
+        mutex_unlock(&hdq_mtx);
+    }
+    return value;
+}
+
+int avr_hid_hdq_read_short(uint8_t address)
+{
+    int old_hi = -1, old_lo = -1, hi = -2, lo = -2;
+    /* Keep reading until we read the same value twice.
+     * There's no atomic 16-bit value retrieval, so keep reading
+     * until we read the same value twice. HDQ registers update
+     * no more than once per 2.56 seconds so usually there will
+     * be 4 reads and sometimes 6 reads.
+     */
+    while ((old_hi != hi) || (old_lo != lo))
+    {
+        old_hi = hi;
+        old_lo = lo;
+        hi = avr_hid_hdq_read_byte(address + 1);
+        lo = avr_hid_hdq_read_byte(address);
+    }
+    if ((hi < 0) || (lo < 0))
+    {
+        return -1;
+    }
+    return (hi << 8) | lo;
+}
+
 static void avr_hid_enable_wheel(void)
 {
     uint8_t enable = 0x01;
diff --git a/firmware/target/arm/tms320dm320/sansa-connect/avr-sansaconnect.h b/firmware/target/arm/tms320dm320/sansa-connect/avr-sansaconnect.h
index baa7083760..ed23e84936 100644
--- a/firmware/target/arm/tms320dm320/sansa-connect/avr-sansaconnect.h
+++ b/firmware/target/arm/tms320dm320/sansa-connect/avr-sansaconnect.h
@@ -24,8 +24,38 @@
 
 #include "config.h"
 
+/* HDQ (bq27000) RAM registers */
+#define HDQ_REG_CTRL            0x00
+#define HDQ_REG_MODE            0x01
+#define HDQ_REG_AR              0x02
+#define HDQ_REG_ARTTE           0x04
+#define HDQ_REG_TEMP            0x06
+#define HDQ_REG_VOLT            0x08
+#define HDQ_REG_FLAGS           0x0A
+#define HDQ_REG_RSOC            0x0B
+#define HDQ_REG_NAC             0x0C
+#define HDQ_REG_CACD            0x0E
+#define HDQ_REG_CACT            0x10
+#define HDQ_REG_LMD             0x12
+#define HDQ_REG_AI              0x14
+#define HDQ_REG_TTE             0x16
+#define HDQ_REG_TTF             0x18
+#define HDQ_REG_SI              0x1A
+#define HDQ_REG_STTE            0x1C
+#define HDQ_REG_MLI             0x1E
+#define HDQ_REG_MLTTE           0x20
+#define HDQ_REG_SAE             0x22
+#define HDQ_REG_AP              0x24
+#define HDQ_REG_TTECP           0x26
+#define HDQ_REG_CYCL            0x28
+#define HDQ_REG_CYCT            0x2A
+#define HDQ_REG_CSOC            0x2C
+
 void avr_hid_init(void);
 
+int avr_hid_hdq_read_byte(uint8_t address);
+int avr_hid_hdq_read_short(uint8_t address);
+
 void avr_hid_enable_charger(void);
 
 void avr_hid_wifi_pd(int high);
diff --git a/firmware/target/arm/tms320dm320/sansa-connect/power-sansaconnect.c b/firmware/target/arm/tms320dm320/sansa-connect/power-sansaconnect.c
index ccd91c6d2f..597fb6b7e0 100644
--- a/firmware/target/arm/tms320dm320/sansa-connect/power-sansaconnect.c
+++ b/firmware/target/arm/tms320dm320/sansa-connect/power-sansaconnect.c
@@ -31,6 +31,29 @@
 #include "i2c-dm320.h"
 #include "logf.h"
 
+
+const unsigned short battery_level_dangerous[BATTERY_TYPES_COUNT] =
+{
+    3450
+};
+
+const unsigned short battery_level_shutoff[BATTERY_TYPES_COUNT] =
+{
+    3400
+};
+
+/* voltages (millivolt) of 0%, 10%, ... 100% when charging disabled */
+const unsigned short percent_to_volt_discharge[BATTERY_TYPES_COUNT][11] =
+{
+    { 3400, 3508, 3630, 3703, 3727, 3750, 3803, 3870, 3941, 4026, 4142 }
+};
+
+/* voltages (millivolt) of 0%, 10%, ... 100% when charging enabled */
+const unsigned short percent_to_volt_charge[11] =
+{
+    3540, 3788, 3860, 3890, 3916, 3956, 4016, 4085, 4164, 4180, 4190
+};
+
 /* (7-bit) address is 0x48, the LSB is read/write flag */
 #define TPS65021_ADDR (0x48 << 1)
 
@@ -54,7 +77,7 @@ void power_init(void)
 
     /* PWM mode */
     tps65021_write_reg(0x04, 0xB2);
- 
+
     /* Set core voltage to 1.5V */
     tps65021_write_reg(0x06, 0x1C);