[ARM] tegra: nv: implement avp suspend/resume for LP0

Change-Id: I3ca52752d00cb603d9327d5a40b48e1cf56cb7c4
Signed-off-by: Dima Zavin <dima@android.com>

2 files changed, 175 insertions, 2 deletions

diff --git a/arch/arm/mach-tegra/nv/nvrm/core/ap15/ap15rm_avp_service.c b/arch/arm/mach-tegra/nv/nvrm/core/ap15/ap15rm_avp_service.c
index 53f503210cb8..d4094aee63a5 100644
--- a/arch/arm/mach-tegra/nv/nvrm/core/ap15/ap15rm_avp_service.c
+++ b/arch/arm/mach-tegra/nv/nvrm/core/ap15/ap15rm_avp_service.c
@@ -35,6 +35,9 @@
 #include <linux/miscdevice.h>
 #include <linux/fs.h>
 #include <linux/firmware.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
 
 #include "nvcommon.h"
 #include "nvassert.h"
@@ -56,6 +59,9 @@
 static NvU32 s_AvpInitialized = NV_FALSE;
 static NvRmLibraryHandle s_hAvpLibrary = NULL;
 
+#define _TEGRA_AVP_RESET_VECTOR_ADDR	\
+	(IO_ADDRESS(TEGRA_EXCEPTION_VECTORS_BASE) + EVP_COP_RESET_VECTOR_0)
+
 #define NV_USE_AOS 1
 
 #define AVP_EXECUTABLE_NAME "nvrm_avp.axf"
@@ -297,8 +303,8 @@ NvRmPrivInitAvp(NvRmDeviceHandle hDevice)
 
     resetVector = (NvU32)avpExecutionJumpAddress & 0xFFFFFFFE;
 
-    NV_WRITE32(IO_ADDRESS(TEGRA_EXCEPTION_VECTORS_BASE) +  EVP_COP_RESET_VECTOR_0, resetVector);
-    RegVal = NV_READ32(IO_ADDRESS(TEGRA_EXCEPTION_VECTORS_BASE) +  EVP_COP_RESET_VECTOR_0);
+    NV_WRITE32(_TEGRA_AVP_RESET_VECTOR_ADDR, resetVector);
+    RegVal = NV_READ32(_TEGRA_AVP_RESET_VECTOR_ADDR);
     NV_ASSERT( RegVal == resetVector );
 
     NvRmModuleReset(hDevice, NvRmModuleID_Avp);
@@ -317,3 +323,119 @@ NvRmPrivInitAvp(NvRmDeviceHandle hDevice)
 
     return err;
 }
+
+static void __iomem *iram_base = IO_ADDRESS(TEGRA_IRAM_BASE);
+static void __iomem *iram_backup;
+static dma_addr_t iram_backup_addr;
+static u32 iram_size = TEGRA_IRAM_SIZE;
+static u32 iram_backup_size = TEGRA_IRAM_SIZE + 4;
+static u32 avp_resume_addr;
+
+NvError
+NvRmPrivSuspendAvp(NvRmRPCHandle hRPCHandle)
+{
+    NvError err = NvSuccess;
+    NvRmMessage_InitiateLP0 lp0_msg;
+    void *avp_suspend_done = iram_backup + iram_size;
+    unsigned long timeout;
+
+    pr_info("%s()+\n", __func__);
+
+    if (!s_AvpInitialized)
+        goto done;
+    else if (!iram_backup_addr) {
+        /* XXX: should we return error? */
+        pr_warning("%s: iram backup ram missing, not suspending avp\n",
+                   __func__);
+        goto done;
+    }
+
+    NV_ASSERT(hRPCHandle->svcTransportHandle != NULL);
+
+    lp0_msg.msg = NvRmMsg_InitiateLP0;
+    lp0_msg.sourceAddr = (u32)TEGRA_IRAM_BASE;
+    lp0_msg.bufferAddr = (u32)iram_backup_addr;
+    lp0_msg.bufferSize = (u32)iram_size;
+
+    writel(0, avp_suspend_done);
+
+    NvOsMutexLock(hRPCHandle->RecvLock);
+    err = NvRmTransportSendMsg(hRPCHandle->svcTransportHandle, &lp0_msg,
+                               sizeof(lp0_msg), 1000);
+    NvOsMutexUnlock(hRPCHandle->RecvLock);
+
+    if (err != NvSuccess) {
+        pr_err("%s: cannot send AVP LP0 message\n", __func__);
+        goto done;
+    }
+
+    timeout = jiffies + msecs_to_jiffies(1000);
+    while (!readl(avp_suspend_done) && time_before(jiffies, timeout)) {
+        udelay(10);
+        cpu_relax();
+    }
+
+    if (!readl(avp_suspend_done)) {
+        pr_err("%s: AVP failed to suspend\n", __func__);
+        err = NvError_Timeout;
+        goto done;
+    }
+
+    avp_resume_addr = readl(iram_base);
+    if (!avp_resume_addr) {
+        pr_err("%s: AVP failed to set it's resume address\n", __func__);
+        err = NvError_InvalidState;
+        goto done;
+    }
+
+    pr_info("avp_suspend: resume_addr=%x\n", avp_resume_addr);
+    avp_resume_addr &= 0xFFFFFFFE;
+
+    pr_info("%s()-\n", __func__);
+
+done:
+    return err;
+}
+
+NvError
+NvRmPrivResumeAvp(NvRmRPCHandle hRPCHandle)
+{
+    NvError ret = NvSuccess;
+    u32 tmp;
+
+    pr_info("%s()+\n", __func__);
+    if (!s_AvpInitialized || !avp_resume_addr)
+        goto done;
+
+    writel(avp_resume_addr, _TEGRA_AVP_RESET_VECTOR_ADDR);
+    tmp = readl(_TEGRA_AVP_RESET_VECTOR_ADDR);
+    NV_ASSERT(tmp == resetVector);
+
+    NvRmModuleReset(hRPCHandle->hRmDevice, NvRmModuleID_Avp);
+
+    /// Resume AVP
+    tmp = NV_DRF_DEF(FLOW_CTLR, HALT_COP_EVENTS, MODE, FLOW_MODE_NONE);
+    writel(tmp, IO_ADDRESS(TEGRA_FLOW_CTRL_BASE) + FLOW_CTLR_HALT_COP_EVENTS_0);
+
+    /* clear the avp resume addr so that if suspend fails, we don't try to
+     * resume */
+    avp_resume_addr = 0;
+
+    pr_info("%s()-\n", __func__);
+
+done:
+    return ret;
+}
+
+int __init _avp_suspend_resume_init(void)
+{
+	/* allocate an iram sized chunk of ram to give to the AVP */
+	iram_backup = dma_alloc_coherent(NULL, iram_backup_size,
+					 &iram_backup_addr, GFP_KERNEL);
+	if (!iram_backup) {
+		pr_err("%s: Unable to allocate iram backup mem\n", __func__);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
diff --git a/arch/arm/mach-tegra/nv/nvrm/core/common/nvrm_moduleloader.c b/arch/arm/mach-tegra/nv/nvrm/core/common/nvrm_moduleloader.c
index 6de25c61608e..2616fe009dff 100644
--- a/arch/arm/mach-tegra/nv/nvrm/core/common/nvrm_moduleloader.c
+++ b/arch/arm/mach-tegra/nv/nvrm/core/common/nvrm_moduleloader.c
@@ -31,6 +31,7 @@
 #include <linux/fs.h>
 #include <linux/firmware.h>
 #include <linux/uaccess.h>
+#include <linux/platform_device.h>
 #include <asm/io.h>
 
 #include "nvcommon.h"
@@ -1631,14 +1632,64 @@ NvError NvRmPrivGetProcAddress(NvRmLibraryHandle Handle,
 	return Error;
 }
 
+NvError NvRmPrivSuspendAvp(NvRmRPCHandle hRPCHandle);
+NvError NvRmPrivResumeAvp(NvRmRPCHandle hRPCHandle);
+
+static int avp_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	NvError err;
+
+	err = NvRmPrivSuspendAvp(s_RPCHandle);
+	if (err != NvSuccess)
+		return -EIO;
+	return 0;
+}
+
+static int avp_resume(struct platform_device *pdev)
+{
+	NvError err;
+
+	err = NvRmPrivResumeAvp(s_RPCHandle);
+	if (err != NvSuccess)
+		return -EIO;
+	return 0;
+}
+
+static struct platform_driver avp_nvfw_driver = {
+	.suspend = avp_suspend,
+	.resume  = avp_resume,
+	.driver  = {
+		.name  = "nvfw-avp-device",
+		.owner = THIS_MODULE,
+	},
+};
+
+int __init _avp_suspend_resume_init(void);
+
 static int __init nvfw_init(void)
 {
 	int ret = 0;
+	struct platform_device *pdev;
 
 	NvOsDebugPrintf("%s: called\n", __func__);
 	ret = misc_register(&nvfw_dev);
 	if (ret) panic("%s: misc_register FAILED\n", __func__);
 
+	ret = _avp_suspend_resume_init();
+	if (ret)
+		goto err;
+	pdev = platform_create_bundle(&avp_nvfw_driver, NULL, NULL, 0, NULL, 0);
+	if (!pdev) {
+		pr_err("%s: Can't reg platform driver\n", __func__);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	pr_info("avp driver initialized\n");
+
+	return 0;
+
+err:
 	return ret;
 }