1 files changed, 15 insertions, 3 deletions

diff --git a/firmware/drivers/rtc.c b/firmware/drivers/rtc.c
index 2882c975b7..c8e19aab53 100644
--- a/firmware/drivers/rtc.c
+++ b/firmware/drivers/rtc.c
@@ -28,9 +28,21 @@
 void rtc_init(void)
 {
     unsigned char data;
-    data = (unsigned int)rtc_read(0x0c);
-    data &= 0xbf;
-    rtc_write(0x0c,data);
+    rtc_write(0x13, 0x10); /* 32 kHz square wave */
+
+    /* Clear the Stop bit if it is set */
+    data = rtc_read(0x01);
+    if(data & 0x80)
+        rtc_write(0x01, 0x00);
+
+    /* Clear the HT bit if it is set */
+    data = rtc_read(0x0c);
+
+    if(data & 0x40)
+    {
+        data &= ~0x40;
+        rtc_write(0x0c,data);
+    }
 }
 
 int rtc_write(unsigned char address, unsigned char value)