From dff85ddb0a1b80f5792664960b9967760d6e436d Mon Sep 17 00:00:00 2001
From: Shunzhou Jiang <shunzhou.jiang@amlogic.com>
Date: Mon, 1 Apr 2019 18:56:14 +0800
Subject: [PATCH] power: sm1/tm2: add sleep and iso mask function [1/1]

PD#SWPL-6186

Problem:
add mask function

Solution:
add sleep and iso mask function

Verify:
sm1_skt

Change-Id: Ifbb7329fd1eb6e333d0332f53c809e7b2e55181d
Signed-off-by: Shunzhou Jiang <shunzhou.jiang@amlogic.com>
---
 drivers/amlogic/power/power_ctrl.c | 52 ++++++++++++++++++++++++++++++
 include/linux/amlogic/power_ctrl.h |  4 +++
 2 files changed, 56 insertions(+)

diff --git a/drivers/amlogic/power/power_ctrl.c b/drivers/amlogic/power/power_ctrl.c
index 9654f6a5ca6f..ad0b7845d942 100644
--- a/drivers/amlogic/power/power_ctrl.c
+++ b/drivers/amlogic/power/power_ctrl.c
@@ -62,6 +62,32 @@ int power_ctrl_sleep(bool power_on, unsigned int shift)
 }
 EXPORT_SYMBOL(power_ctrl_sleep);
 
+int power_ctrl_sleep_mask(bool power_on,
+			  unsigned int mask_val, unsigned int shift)
+{
+	unsigned int val;
+	unsigned long flags;
+
+	if (!probe_done)
+		return -ENXIO;
+
+	if (power_on) {
+		spin_lock_irqsave(&ctrl.sleep_lock, flags);
+		val = readl(ctrl.sleep_addr);
+		val = val & (~(mask_val << shift));
+		writel(val, ctrl.sleep_addr);
+		spin_unlock_irqrestore(&ctrl.sleep_lock, flags);
+	} else {
+		spin_lock_irqsave(&ctrl.sleep_lock, flags);
+		val = readl(ctrl.sleep_addr);
+		val = val | (mask_val << shift);
+		writel(val, ctrl.sleep_addr);
+		spin_unlock_irqrestore(&ctrl.sleep_lock, flags);
+	}
+	return 0;
+}
+EXPORT_SYMBOL(power_ctrl_sleep_mask);
+
 int power_ctrl_iso(bool power_on, unsigned int shift)
 {
 	unsigned int val;
@@ -87,6 +113,32 @@ int power_ctrl_iso(bool power_on, unsigned int shift)
 }
 EXPORT_SYMBOL(power_ctrl_iso);
 
+int power_ctrl_iso_mask(bool power_on,
+			unsigned int mask_val, unsigned int shift)
+{
+	unsigned int val;
+	unsigned long flags;
+
+	if (!probe_done)
+		return -ENXIO;
+
+	if (power_on) {
+		spin_lock_irqsave(&(ctrl.iso_lock), flags);
+		val = readl(ctrl.iso_addr);
+		val = val & (~(mask_val << shift));
+		writel(val, ctrl.iso_addr);
+		spin_unlock_irqrestore(&(ctrl.iso_lock), flags);
+	} else {
+		spin_lock_irqsave((&ctrl.iso_lock), flags);
+		val = readl(ctrl.iso_addr);
+		val = val | (mask_val << shift);
+		writel(val, ctrl.iso_addr);
+		spin_unlock_irqrestore(&(ctrl.iso_lock), flags);
+	}
+	return 0;
+}
+EXPORT_SYMBOL(power_ctrl_iso_mask);
+
 int power_ctrl_mempd0(bool power_on, unsigned int mask_val, unsigned int shift)
 {
 	unsigned int val;
diff --git a/include/linux/amlogic/power_ctrl.h b/include/linux/amlogic/power_ctrl.h
index 5b6f7213539e..f0ab6117fbb2 100644
--- a/include/linux/amlogic/power_ctrl.h
+++ b/include/linux/amlogic/power_ctrl.h
@@ -21,7 +21,11 @@
 
 #ifdef CONFIG_AMLOGIC_POWER
 int power_ctrl_sleep(bool power_on, unsigned int shift);
+int power_ctrl_sleep_mask(bool power_on,
+			  unsigned int mask_val, unsigned int shift);
 int power_ctrl_iso(bool power_on, unsigned int shift);
+int power_ctrl_iso_mask(bool power_on,
+			unsigned int mask_val, unsigned int shift);
 int power_ctrl_mempd0(bool power_on, unsigned int mask_val, unsigned int shift);
 #else
 static inline int power_ctrl_sleep(bool power_on, unsigned int shift)