1 files changed, 37 insertions, 47 deletions

diff --git a/drivers/rtc/rtc-m41t80.c b/drivers/rtc/rtc-m41t80.c
index f4c070ea8384..c90fba3ed861 100644
--- a/drivers/rtc/rtc-m41t80.c
+++ b/drivers/rtc/rtc-m41t80.c
@@ -154,6 +154,8 @@ struct m41t80_data {
 	struct rtc_device *rtc;
 #ifdef CONFIG_COMMON_CLK
 	struct clk_hw sqw;
+	unsigned long freq;
+	unsigned int sqwe;
 #endif
 };
 
@@ -443,43 +445,40 @@ static SIMPLE_DEV_PM_OPS(m41t80_pm, m41t80_suspend, m41t80_resume);
 #ifdef CONFIG_COMMON_CLK
 #define sqw_to_m41t80_data(_hw) container_of(_hw, struct m41t80_data, sqw)
 
-static unsigned long m41t80_sqw_recalc_rate(struct clk_hw *hw,
-					    unsigned long parent_rate)
+static unsigned long m41t80_decode_freq(int setting)
+{
+	return (setting == 0) ? 0 : (setting == 1) ? M41T80_SQW_MAX_FREQ :
+		M41T80_SQW_MAX_FREQ >> setting;
+}
+
+static unsigned long m41t80_get_freq(struct m41t80_data *m41t80)
 {
-	struct m41t80_data *m41t80 = sqw_to_m41t80_data(hw);
 	struct i2c_client *client = m41t80->client;
 	int reg_sqw = (m41t80->features & M41T80_FEATURE_SQ_ALT) ?
 		M41T80_REG_WDAY : M41T80_REG_SQW;
 	int ret = i2c_smbus_read_byte_data(client, reg_sqw);
-	unsigned long val = M41T80_SQW_MAX_FREQ;
 
 	if (ret < 0)
 		return 0;
+	return m41t80_decode_freq(ret >> 4);
+}
 
-	ret >>= 4;
-	if (ret == 0)
-		val = 0;
-	else if (ret > 1)
-		val = val / (1 << ret);
-
-	return val;
+static unsigned long m41t80_sqw_recalc_rate(struct clk_hw *hw,
+					    unsigned long parent_rate)
+{
+	return sqw_to_m41t80_data(hw)->freq;
 }
 
 static long m41t80_sqw_round_rate(struct clk_hw *hw, unsigned long rate,
 				  unsigned long *prate)
 {
-	int i, freq = M41T80_SQW_MAX_FREQ;
-
-	if (freq <= rate)
-		return freq;
-
-	for (i = 2; i <= ilog2(M41T80_SQW_MAX_FREQ); i++) {
-		freq /= 1 << i;
-		if (freq <= rate)
-			return freq;
-	}
-
-	return 0;
+	if (rate >= M41T80_SQW_MAX_FREQ)
+		return M41T80_SQW_MAX_FREQ;
+	if (rate >= M41T80_SQW_MAX_FREQ / 4)
+		return M41T80_SQW_MAX_FREQ / 4;
+	if (!rate)
+		return 0;
+	return 1 << ilog2(rate);
 }
 
 static int m41t80_sqw_set_rate(struct clk_hw *hw, unsigned long rate,
@@ -491,17 +490,12 @@ static int m41t80_sqw_set_rate(struct clk_hw *hw, unsigned long rate,
 		M41T80_REG_WDAY : M41T80_REG_SQW;
 	int reg, ret, val = 0;
 
-	if (rate) {
-		if (!is_power_of_2(rate))
-			return -EINVAL;
-		val = ilog2(rate);
-		if (val == ilog2(M41T80_SQW_MAX_FREQ))
-			val = 1;
-		else if (val < (ilog2(M41T80_SQW_MAX_FREQ) - 1))
-			val = ilog2(M41T80_SQW_MAX_FREQ) - val;
-		else
-			return -EINVAL;
-	}
+	if (rate >= M41T80_SQW_MAX_FREQ)
+		val = 1;
+	else if (rate >= M41T80_SQW_MAX_FREQ / 4)
+		val = 2;
+	else if (rate)
+		val = 15 - ilog2(rate);
 
 	reg = i2c_smbus_read_byte_data(client, reg_sqw);
 	if (reg < 0)
@@ -510,10 +504,9 @@ static int m41t80_sqw_set_rate(struct clk_hw *hw, unsigned long rate,
 	reg = (reg & 0x0f) | (val << 4);
 
 	ret = i2c_smbus_write_byte_data(client, reg_sqw, reg);
-	if (ret < 0)
-		return ret;
-
-	return -EINVAL;
+	if (!ret)
+		m41t80->freq = m41t80_decode_freq(val);
+	return ret;
 }
 
 static int m41t80_sqw_control(struct clk_hw *hw, bool enable)
@@ -530,7 +523,10 @@ static int m41t80_sqw_control(struct clk_hw *hw, bool enable)
 	else
 		ret &= ~M41T80_ALMON_SQWE;
 
-	return i2c_smbus_write_byte_data(client, M41T80_REG_ALARM_MON, ret);
+	ret = i2c_smbus_write_byte_data(client, M41T80_REG_ALARM_MON, ret);
+	if (!ret)
+		m41t80->sqwe = enable;
+	return ret;
 }
 
 static int m41t80_sqw_prepare(struct clk_hw *hw)
@@ -545,14 +541,7 @@ static void m41t80_sqw_unprepare(struct clk_hw *hw)
 
 static int m41t80_sqw_is_prepared(struct clk_hw *hw)
 {
-	struct m41t80_data *m41t80 = sqw_to_m41t80_data(hw);
-	struct i2c_client *client = m41t80->client;
-	int ret = i2c_smbus_read_byte_data(client, M41T80_REG_ALARM_MON);
-
-	if (ret < 0)
-		return ret;
-
-	return !!(ret & M41T80_ALMON_SQWE);
+	return sqw_to_m41t80_data(hw)->sqwe;
 }
 
 static const struct clk_ops m41t80_sqw_ops = {
@@ -587,6 +576,7 @@ static struct clk *m41t80_sqw_register_clk(struct m41t80_data *m41t80)
 	init.parent_names = NULL;
 	init.num_parents = 0;
 	m41t80->sqw.init = &init;
+	m41t80->freq = m41t80_get_freq(m41t80);
 
 	/* optional override of the clockname */
 	of_property_read_string(node, "clock-output-names", &init.name);