initial low power mode implementation

8 files changed, 854 insertions, 7 deletions

diff --git a/examples/imx7_colibri_m4/low_power_demo/FreeRTOSConfig.h b/examples/imx7_colibri_m4/low_power_demo/FreeRTOSConfig.h
index 45eccbb..2027d64 100644
--- a/examples/imx7_colibri_m4/low_power_demo/FreeRTOSConfig.h
+++ b/examples/imx7_colibri_m4/low_power_demo/FreeRTOSConfig.h
@@ -84,10 +84,12 @@
     #include <stdint.h>
 #endif
 
+extern unsigned long configCPU_CLOCK_HZ;
+
 #define configUSE_PREEMPTION              1
 #define configUSE_IDLE_HOOK               1
 #define configUSE_TICK_HOOK               0
-#define configCPU_CLOCK_HZ                (240000000ul)
+#define configCPU_CLOCK_HZ_DEFAULT        (240000000ul)
 #define configTICK_RATE_HZ                ((TickType_t)1000)
 #define configMAX_PRIORITIES              (5)
 #define configMINIMAL_STACK_SIZE          ((unsigned short)130)
@@ -125,6 +127,7 @@ to exclude the API function. */
 #define INCLUDE_vTaskSuspend              1
 #define INCLUDE_vTaskDelayUntil           0
 #define INCLUDE_vTaskDelay                1
+#define INCLUDE_xTaskGetSchedulerState    1
 
 /* Cortex-M specific definitions. */
 #ifdef __NVIC_PRIO_BITS
diff --git a/examples/imx7_colibri_m4/low_power_demo/armgcc/CMakeLists.txt b/examples/imx7_colibri_m4/low_power_demo/armgcc/CMakeLists.txt
index 3c9ab46..ebe868a 100644
--- a/examples/imx7_colibri_m4/low_power_demo/armgcc/CMakeLists.txt
+++ b/examples/imx7_colibri_m4/low_power_demo/armgcc/CMakeLists.txt
@@ -139,6 +139,9 @@ ADD_EXECUTABLE(low_power_demo
     "${ProjDirPath}/../gpio_pins.h"
     "${ProjDirPath}/../ugui/ugui.h"
     "${ProjDirPath}/../ugui/ugui.c"
+    "${ProjDirPath}/../lpm_mcore.c"
+    "${ProjDirPath}/../lpm_mcore.h"
+    "${ProjDirPath}/../main.c"
     "${ProjDirPath}/../main.c"
     "${BspRootDirPath}/platform/drivers/src/gpio_imx.c"
     "${BspRootDirPath}/platform/drivers/inc/gpio_imx.h"
diff --git a/examples/imx7_colibri_m4/low_power_demo/gpc.c b/examples/imx7_colibri_m4/low_power_demo/gpc.c
new file mode 100644
index 0000000..d21b5cd
--- /dev/null
+++ b/examples/imx7_colibri_m4/low_power_demo/gpc.c
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2015, Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * o Redistributions of source code must retain the above copyright notice, this list
+ *   of conditions and the following disclaimer.
+ *
+ * o Redistributions in binary form must reproduce the above copyright notice, this
+ *   list of conditions and the following disclaimer in the documentation and/or
+ *   other materials provided with the distribution.
+ *
+ * o Neither the name of Freescale Semiconductor, Inc. nor the names of its
+ *   contributors may be used to endorse or promote products derived from this
+ *   software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "gpc.h"
+
+/*
+ * Initialize GPC settings for dual core low power management
+ */
+void GPC_Init(GPC_Type * base)
+{
+    /*
+     * Disable all M4 interrupt as GPC wakeup source
+     */
+    base->IMR1_M4 = GPC_IMR1_M4_IMR1_M4_MASK;
+    base->IMR2_M4 = GPC_IMR2_M4_IMR2_M4_MASK;
+    base->IMR3_M4 = GPC_IMR3_M4_IMR3_M4_MASK;
+    base->IMR4_M4 = GPC_IMR4_M4_IMR4_M4_MASK;
+
+    /*
+     * Initialize the GPC settings for M4
+     */
+    base->LPCR_M4 |= GPC_LPCR_M4_EN_M4_PUP_MASK |
+        GPC_LPCR_M4_EN_M4_PDN_MASK;  /* EN_M4_PUP, EN_M4_PDN*/
+    base->PGC_ACK_SEL_M4 = GPC_PGC_ACK_SEL_M4_M4_VIRTUAL_PGC_PUP_ACK_MASK |
+        GPC_PGC_ACK_SEL_M4_M4_VIRTUAL_PGC_PDN_ACK_MASK; /* change dummy to virtual*/
+    base->MISC |= GPC_MISC_M4_PDN_REQ_MASK_MASK; /* not mask M4 power down*/
+
+    /*
+     * M4 Virtual domain and Fast Mega domain must use the same PUP/PDN slot
+     * Align to A7, SLT 1 and SLT 5 are used for Fast Mega and M4 Virtual domain
+     */
+    base->SLT_CFG[1] = GPC_SLT_CFG_M4_VIRTUAL_PDN_SLOT_CONTROL_MASK | GPC_SLT_CFG_FASTMEGA_PDN_SLOT_CONTROL_MASK;
+    base->SLT_CFG[5] = GPC_SLT_CFG_M4_VIRTUAL_PUP_SLOT_CONTROL_MASK | GPC_SLT_CFG_FASTMEGA_PUP_SLOT_CONTROL_MASK;
+
+    /* 
+     * Fast map to both A7 and M4
+     *      - M4 only have access to byte 1, A7 only have access to byte 0
+     *      - set to 1 for "map" 
+     */
+    base->PGC_CPU_MAPPING = GPC_PGC_CPU_MAPPING_FASTMEGA_M4_DOMAIN_MASK; 
+}
+
+/*
+ * Enable an interrupt source as GPC wakeup source for M4 core
+ */
+void GPC_EnableM4WakeupIRQ(GPC_Type* base, uint32_t irq_no, GPC_IRQ_WAKEUP_MODE wakeup_mode)
+{
+    uint32_t reg_index, reg_offset;
+    volatile uint32_t* target_reg;
+
+    if (irq_no < TOTAL_IRQ_NUM) {
+        reg_index = irq_no / IRQ_PER_REGISTER;
+        reg_offset = irq_no % IRQ_PER_REGISTER;
+        target_reg = &base->IMR1_M4 + reg_index;
+        if (wakeup_mode == GPC_IRQ_WAKEUP_ENABLE) {
+            /*enable the IRQ as wakeup source*/
+            *target_reg &= ~(1 << reg_offset);
+        }
+        else {
+            /*disable the IRQ as wakeup source*/
+            *target_reg |= 1 << reg_offset;
+        }
+    }
+}
+
+/*
+ * Configure LPCR_M4_LPM, the configured LPM state will be entered
+ * the next time WFI is executed
+ */
+void GPC_SetM4NextLPM(GPC_Type* base, uint32_t lpm_val)
+{
+    if (lpm_val < 4) {
+        base->LPCR_M4 = (base->LPCR_M4 & ~GPC_LPCR_M4_LPM0_MASK) | lpm_val;
+    }
+}
diff --git a/examples/imx7_colibri_m4/low_power_demo/gpc.h b/examples/imx7_colibri_m4/low_power_demo/gpc.h
new file mode 100644
index 0000000..8c61adf
--- /dev/null
+++ b/examples/imx7_colibri_m4/low_power_demo/gpc.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (c) 2015, Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * o Redistributions of source code must retain the above copyright notice, this list
+ *   of conditions and the following disclaimer.
+ *
+ * o Redistributions in binary form must reproduce the above copyright notice, this
+ *   list of conditions and the following disclaimer in the documentation and/or
+ *   other materials provided with the distribution.
+ *
+ * o Neither the name of Freescale Semiconductor, Inc. nor the names of its
+ *   contributors may be used to endorse or promote products derived from this
+ *   software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __GPC_H__
+#define __GPC_H__
+#include "device_imx.h"
+
+#define TOTAL_IRQ_NUM   128
+#define IRQ_PER_REGISTER 32
+
+typedef enum gpc_irq_wakeup_mode {
+    GPC_IRQ_WAKEUP_ENABLE,
+    GPC_IRQ_WAKEUP_DISABLE,
+} GPC_IRQ_WAKEUP_MODE;
+
+void GPC_Init(GPC_Type* base);
+
+void GPC_EnableM4WakeupIRQ(GPC_Type* base, uint32_t irq_no, GPC_IRQ_WAKEUP_MODE wakeup_mode);
+
+void GPC_SetM4NextLPM(GPC_Type* base, uint32_t lpm_val);
+
+#endif
diff --git a/examples/imx7_colibri_m4/low_power_demo/hardware_init.c b/examples/imx7_colibri_m4/low_power_demo/hardware_init.c
index 3658b64..aa077b5 100644
--- a/examples/imx7_colibri_m4/low_power_demo/hardware_init.c
+++ b/examples/imx7_colibri_m4/low_power_demo/hardware_init.c
@@ -33,6 +33,8 @@
 
 void hardware_init(void)
 {
+    int i;
+
     /* Board specific RDC settings */
     BOARD_RdcInit();
     /* Board specific clock settings */
@@ -40,6 +42,23 @@ void hardware_init(void)
     /* initialize debug uart */
     dbg_uart_init();
 
+    /*
+     * In order to wakeup M4 from LPM, some PLLCTRLs need to be set to "NeededRun"
+     */
+    CCM_BASE_PTR->PLL_CTRL[0].PLL_CTRL = ccmClockNeededRun;
+    CCM_BASE_PTR->PLL_CTRL[6].PLL_CTRL = ccmClockNeededRun;
+    CCM_BASE_PTR->PLL_CTRL[7].PLL_CTRL = ccmClockNeededRun;
+    CCM_BASE_PTR->PLL_CTRL[8].PLL_CTRL = ccmClockNeededRun;
+    CCM_BASE_PTR->PLL_CTRL[9].PLL_CTRL = ccmClockNeededRun;
+    CCM_BASE_PTR->PLL_CTRL[10].PLL_CTRL = ccmClockNeededRun;
+    CCM_BASE_PTR->PLL_CTRL[11].PLL_CTRL = ccmClockNeededRun;
+    CCM_BASE_PTR->PLL_CTRL[12].PLL_CTRL = ccmClockNeededRun;
+    CCM_BASE_PTR->PLL_CTRL[13].PLL_CTRL = ccmClockNeededRun;
+    CCM_BASE_PTR->PLL_CTRL[14].PLL_CTRL = ccmClockNeededRun;
+    CCM_BASE_PTR->PLL_CTRL[15].PLL_CTRL = ccmClockNeededRun;
+
+    /* Enable clock gate for wakeup mix*/
+    CCM_ControlGate(CCM, BOARD_SIM_WAKEUP_CCGR, ccmClockNeededAll);
     /* In this demo, we need to share board GPIO without RDC SEMAPHORE */
     RDC_SetPdapAccess(RDC, rdcPdapGpio1, 0xFF, false, false);
     RDC_SetPdapAccess(RDC, rdcPdapGpio4, 0xFF, false, false);
@@ -58,6 +77,9 @@ void hardware_init(void)
     CCM_ControlGate(CCM, BOARD_ECSPI_CCM_CCGR, ccmClockNeededAll);
     /* Configure ecspi pin IOMUX */
     configure_ecspi_pins(BOARD_ECSPI_BASEADDR);
+
+    /* Enable MU clock*/
+    CCM_ControlGate(CCM, BOARD_MU_CCM_CCGR, ccmClockNeededAll);
 }
 
 /*******************************************************************************
diff --git a/examples/imx7_colibri_m4/low_power_demo/lpm_mcore.c b/examples/imx7_colibri_m4/low_power_demo/lpm_mcore.c
new file mode 100644
index 0000000..96857b8
--- /dev/null
+++ b/examples/imx7_colibri_m4/low_power_demo/lpm_mcore.c
@@ -0,0 +1,523 @@
+/*
+ * Copyright (c) 2015-2016, Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * o Redistributions of source code must retain the above copyright notice, this list
+ *   of conditions and the following disclaimer.
+ *
+ * o Redistributions in binary form must reproduce the above copyright notice, this
+ *   list of conditions and the following disclaimer in the documentation and/or
+ *   other materials provided with the distribution.
+ *
+ * o Neither the name of Freescale Semiconductor, Inc. nor the names of its
+ *   contributors may be used to endorse or promote products derived from this
+ *   software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <string.h>
+#include "board.h"
+#include "lpm_mcore.h"
+#include "debug_console_imx.h"
+#include "ccm_imx7d.h"
+#include "mu_imx.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "gpt.h"
+
+unsigned long configCPU_CLOCK_HZ = configCPU_CLOCK_HZ_DEFAULT;
+extern void vPortSetupTimerInterrupt(void);
+
+#define MAXIMUM_24M_DIV 15
+
+static LPM_POWER_STATUS_M4 m4_lpm_state = LPM_M4_STATE_RUN;
+
+static P_WAKEUP_INT_ELE g_wakeup_int_list;
+
+/*
+ * Send Message to A7
+ */
+static void LPM_MCORE_SendMessage(uint32_t msg)
+{
+    while (0 == (MUB_SR & MU_SR_TEn(0x8 >> LPM_MCORE_MU_CHANNEL)));
+    MUB->TR[LPM_MCORE_MU_CHANNEL] = msg;
+    while ((MUB_SR & MU_SR_EP_MASK) != 0);
+}
+
+#if (defined(LPM_MCORE_PRINT_DEBUG_INFO) && (LPM_MCORE_PRINT_DEBUG_INFO))
+/*
+ * Provide a spinning delay, the actual delay time is dependent on the CPU freq
+ */
+static void my_delay(void)
+{
+    uint32_t i, j, k;
+    for (i=0; i!=DELAY_CNT; i++)
+        for (j=0; j!=DELAY_CNT; j++)
+            for (k=0; k!=DELAY_CNT; k++)
+                __NOP();
+    return;
+}
+
+/*
+ * Use the delay function to demostrate current CPU running freq
+ */
+static void verify_clock_speed(void)
+{
+    uint32_t i;
+    for (i=0; i!=DELAY_LOOP_CNT_LOW_SPEED; i++) {
+        my_delay();
+        PRINTF("\rVerify M4 Speed : %2d of %d ... ", i+1, DELAY_LOOP_CNT_LOW_SPEED);
+    }
+    PRINTF("Done.\r\n");
+}
+#endif
+
+/*
+ * initialize the wakeup interrupt list
+ */
+static void lpm_init_wakeup_interrupt_list() {
+    g_wakeup_int_list = NULL;
+}
+
+/*
+ * add a new irq to wakeup interrupt link list
+ */
+static void lpm_add_wakeup_interrupt_list(uint32_t irq_no)
+{
+    P_WAKEUP_INT_ELE cur_ele = g_wakeup_int_list;
+    P_WAKEUP_INT_ELE p;
+
+    if (cur_ele == NULL) {
+        /*
+         * first element to add
+         */
+        p = pvPortMalloc(sizeof(WAKEUP_INT_ELE));
+        p->irq_no = irq_no;
+        p->next = NULL;
+        g_wakeup_int_list = p;
+    } else {
+        for (;;) {
+            if (cur_ele->irq_no == irq_no) {
+                /*
+                 * already in the link list
+                 *      - return directly
+                 */
+                break;
+            }
+            else if (cur_ele->next == NULL) {
+                /*
+                 * can't find the element
+                 *      - insert into the end
+                 */
+                p = pvPortMalloc(sizeof(WAKEUP_INT_ELE));
+                p->irq_no = irq_no;
+                p->next = NULL;
+                cur_ele->next = p;
+            } else {
+                cur_ele = cur_ele->next;
+            }
+        }
+    }
+}
+
+/*
+ * remove an exsiting irq to wakeup interrupt link list
+ */
+static void lpm_del_wakeup_interrupt_list(uint32_t irq_no)
+{
+    P_WAKEUP_INT_ELE cur_ele = g_wakeup_int_list;
+    P_WAKEUP_INT_ELE p;
+
+    if (cur_ele != NULL) {
+        if (cur_ele->irq_no == irq_no) {
+            /*first element is the target*/
+            p = g_wakeup_int_list;
+            g_wakeup_int_list = p->next;
+            vPortFree(p);
+        } else {
+            for (;;) {
+                p = cur_ele->next;
+                if (p == NULL) {
+                    /*
+                     * can't find the element
+                     *      - return directly
+                     */
+                    break;
+                } else {
+                    if (p->irq_no == irq_no) {
+                        /*
+                         * Find the target "p"
+                         */
+                        cur_ele->next = p->next;
+                        vPortFree(p);
+                        break;
+                    } else {
+                        cur_ele = cur_ele->next;
+                    }
+                }
+            }
+        }
+    }
+}
+
+
+/*
+ * register a IRQ source as M4 wakeup source
+ */
+void LPM_MCORE_RegisterWakeupInterrupt(GPC_Type * base, uint32_t irq_no, GPC_IRQ_WAKEUP_MODE wakeup_mode)
+{
+    /*register wakeup interrupt for M4 in GPC*/
+    GPC_EnableM4WakeupIRQ(base, irq_no, wakeup_mode);
+
+    if (wakeup_mode == GPC_IRQ_WAKEUP_ENABLE) {
+        /*add an element to link list*/
+        lpm_add_wakeup_interrupt_list(irq_no);
+    } else {
+        /*delete an element to link list*/
+        lpm_del_wakeup_interrupt_list(irq_no);
+    }
+}
+
+
+/*
+ * Low Power Management initialization
+ */
+void LPM_MCORE_Init(GPC_Type * base)
+{
+    // Init GPC
+    GPC_Init(base);
+
+    // Init the wakeup interrupt link list
+    lpm_init_wakeup_interrupt_list();
+}
+
+
+/*
+ * get the current m4 LPM state
+ */
+LPM_POWER_STATUS_M4 LPM_MCORE_GetPowerStatus(GPC_Type * base)
+{
+    return m4_lpm_state;
+} 
+
+/*
+ * on-the-fly change m4 parent clock between 24MHz and 240MHz
+ */
+void LPM_MCORE_ChangeM4Clock(LPM_M4_CLOCK_SPEED target)
+{
+    // change CCM Root to change M4 clock
+    switch (target) {
+        case LPM_M4_LOW_FREQ:
+            if (CCM_GetRootMux(CCM, ccmRootM4) != ccmRootmuxM4Osc24m) {
+            #if (defined(LPM_MCORE_PRINT_DEBUG_INFO) && (LPM_MCORE_PRINT_DEBUG_INFO))
+                PRINTF("Change M4 clock freq to 24M\r\n");
+            #endif
+                CCM_SetRootMux(CCM, ccmRootM4, ccmRootmuxM4Osc24m);
+            }
+            configCPU_CLOCK_HZ = 24000000ul;
+            break;
+        case LPM_M4_HIGH_FREQ:
+            if (CCM_GetRootMux(CCM, ccmRootM4) != ccmRootmuxM4SysPllDiv2) {
+            #if (defined(LPM_MCORE_PRINT_DEBUG_INFO) && (LPM_MCORE_PRINT_DEBUG_INFO))
+                PRINTF("Change M4 clock freq to SysPLL Div2 (240M)\r\n");
+            #endif
+                CCM_SetRootMux(CCM, ccmRootM4, ccmRootmuxM4SysPllDiv2);
+            }
+            configCPU_CLOCK_HZ = 240000000ul;
+            break;
+        default:
+            break;
+    }
+    if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED)
+        vPortSetupTimerInterrupt();
+#if (defined(LPM_MCORE_PRINT_DEBUG_INFO) && (LPM_MCORE_PRINT_DEBUG_INFO))
+    verify_clock_speed();
+#endif
+}
+
+/*
+ * cycle M4 low power mode to next state, the state machine is
+ *
+ *        +---> "RUN" ---> "WAIT" ---> "STOP" ---+
+ *        |                                      |
+ *        +--------------------------------------+
+ */
+void LPM_MCORE_SetPowerStatus(GPC_Type * base, LPM_POWER_STATUS_M4 m4_next_lpm)
+{
+    uint32_t next_lpm = GPC_LPCR_M4_LPM0(0);
+    switch (m4_next_lpm) {
+        case LPM_M4_STATE_RUN:
+            next_lpm = GPC_LPCR_M4_LPM0(0);
+            break;
+        case LPM_M4_STATE_WAIT:
+            next_lpm = GPC_LPCR_M4_LPM0(1);
+            break;
+        case LPM_M4_STATE_STOP:
+            next_lpm = GPC_LPCR_M4_LPM0(2);
+            break;
+        default:
+            break;
+    }
+
+    /*
+     * Patch, let GPC-M4 observe the GPR0 interrupt for a period as long
+     * as 5 32KHz clock cycle before set it to a Low power status
+     */
+    if (m4_next_lpm != LPM_M4_STATE_RUN)
+    {
+        uint32_t i;
+        LPM_MCORE_RegisterWakeupInterrupt(GPC, GPT4_IRQn, GPC_IRQ_WAKEUP_ENABLE);
+        for (i=0; i!=GPC_SYNC_DELAY_CNT; i++)
+            __NOP();
+        LPM_MCORE_RegisterWakeupInterrupt(GPC, GPT4_IRQn, GPC_IRQ_WAKEUP_DISABLE);
+    }
+
+    GPC_SetM4NextLPM(base, next_lpm);
+
+    /*change lpm state variable*/
+    m4_lpm_state = m4_next_lpm;
+}
+
+
+
+/*
+ * Give readable string of current M4 lpm state
+ */
+const char* LPM_MCORE_GetPowerStatusString(void)
+{
+    switch (m4_lpm_state) {
+        case LPM_M4_STATE_RUN:
+            return "RUN";
+        case LPM_M4_STATE_WAIT:
+            return "WAIT";
+        case LPM_M4_STATE_STOP:
+            return "STOP";
+        default:
+            return "UNKNOWN";
+    }
+}
+
+/*
+ * Check if A7 LPM Driver is ready, an "Once Ready, Always Ready" logic is used
+ */
+uint32_t LPM_MCORE_CheckPeerReady(void) 
+{
+    static uint32_t a7_ready = 0;
+    if (!a7_ready) {
+        a7_ready = MU_GetFlags(MUB) & MU_SR_Fn(1);
+    }
+    return a7_ready;
+}
+
+/*
+ * Use MU Flag to indicate to A7 that low power management in M4 is ready
+ */
+void LPM_MCORE_SetSelfReady(void)
+{
+    MU_SetFlags(MUB, MU_CR_Fn(1));
+}
+
+/*
+ * This function modify BASEPRI to configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY and
+ * wakeup interrupt's NVIC->Priority to configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY - 1
+ * The effect is all non-wakeup interrupt gets mute
+ * The original basepri settings are stored into pBasepriBackup
+ * The original wakeup interrupt nvic->priority settings are stored into linklist
+ */
+void lpm_disable_non_wakeup_interrupt(uint32_t* pBasepriBackup)
+{
+    P_WAKEUP_INT_ELE ele;
+    uint32_t irq_no;
+#if defined(__CC_ARM)
+    register uint32_t __regBasePri __ASM("basepri");
+    *pBasepriBackup = __regBasePri;
+    __regBasePri  = (configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY << (8 - __NVIC_PRIO_BITS));
+#else
+    *pBasepriBackup = __get_BASEPRI();
+    __set_BASEPRI(configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY << (8 - __NVIC_PRIO_BITS));
+#endif
+    /*
+     * Make exceptions to wakeup interrupts, they are stored in "g_wakeup_int_list"
+     */
+    ele = g_wakeup_int_list;
+    for (;;) {
+        if (ele == NULL)
+            break;
+
+        /* 
+         * Store the current Priority into ele backup field
+         * Change the Priority to "configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY - 1"
+         */
+        irq_no = ele->irq_no;
+        ele->irq_priority_backup = NVIC->IP[irq_no];
+        NVIC->IP[irq_no] = (configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY - 1) << (8 - __NVIC_PRIO_BITS);
+
+        /*
+         * Move to next
+         */
+        ele = ele->next;
+    }
+    __DSB();
+    __ISB();
+}
+
+/*
+ * This function restores BASEPRI and wakeup interrupt nvic priority settings
+ * It recover interrupt settings made by lpm_disable_non_wakeup_interrupt
+ */
+void lpm_enable_non_wakeup_interrupt(uint32_t basePriBackup)
+{
+    P_WAKEUP_INT_ELE ele;
+    uint32_t irq_no;
+#if defined(__CC_ARM)
+    register uint32_t __regBasePri __ASM("basepri");
+#endif
+    /*
+     * first restore wakeup interrupt priority
+     */
+    ele = g_wakeup_int_list;
+    for (;;) {
+        if (ele == NULL)
+            break;
+
+        /* 
+         * Restore the original priority
+         */
+        irq_no = ele->irq_no;
+        NVIC->IP[irq_no] = ele->irq_priority_backup;
+
+        /*
+         * Move to next
+         */
+        ele = ele->next;
+    }
+#if defined(__CC_ARM)
+    __regBasePri = basePriBackup & 0xFF;
+#else
+    __set_BASEPRI(basePriBackup);
+#endif
+    // infinite_loop();
+    // Are these necessary?
+    __DSB();
+    __ISB();
+}
+
+
+/*
+ * The sleep function inserted into FreeRTOS idle task hook
+ */
+void LPM_MCORE_WaitForInt(void)
+{
+    uint32_t priMaskBackup;
+
+    /* 
+     * Only when 
+     *      1. A7 peer is ready
+     *      2. safe sleep function has been put into TCM
+     *      3. m4 true sleep mode has been allowed
+     *      4. m4 current lpm mode is wait / stop
+     *  The "power save wfi" routine will be executed
+     *  Otherwise "normal wfi" will be executed
+     *
+     *  In Power Save WFI
+     *      - PRIMASK is set, so all interrupt handler won't be executed
+     *      - BASEPRI and NVIC->Priority is modified so that only wakeup interrupt can
+     *        wake up M4 from WFI
+     *      - After M4 wake up, NVIC->Priority, BASEPRI and PRIMASK are restored so that
+     *        the system return to normal mode
+     *
+     *  There is a critical section code which is in "runInRAM", inside it M4 will
+     *  inform A7 it release the high bus. A7 can then shutdown the high bus which
+     *  will make all highbus related peripherals losing functionality, including
+     *  DDR, so code in "runInRAM" should run in TCM and don't have any access to
+     *  other part of memory
+     */
+    if (LPM_MCORE_CheckPeerReady() && false)
+    {
+        volatile uint32_t* reg_lpcr_m4 = &GPC->LPCR_M4;
+
+        uint32_t next_lpm_mode = *reg_lpcr_m4 & GPC_LPCR_M4_LPM0_MASK;
+        uint32_t basePriBackup;
+
+        /* Save current PRIMASK value. */
+        priMaskBackup = __get_PRIMASK();
+
+        /* 
+         * Set PRIMASK to avoid execution of any enabled ISR. 
+         *      Note : PRIMASK will not prevent interrupt to wake up M4 from WFI
+         *             but it will prevent interrupt handler from running
+         */
+        __set_PRIMASK(1);
+        /*
+         * Some of the code should be moved out of "runInRAM"
+         */
+        switch (next_lpm_mode) {
+            case LPCR_M4_RUN:
+                /*
+                 * STOP -> RUN
+                 */
+                /*
+                 * tell A7 the next LPM mode is RUN
+                 */
+                /*
+                 * the WFI will be wakeup by any enabled interrupt
+                 */
+                __WFI();
+                break;
+            case LPCR_M4_WAIT:
+            case LPCR_M4_STOP:
+                /*
+                 * RUN -> WAIT or WAIT -> STOP
+                 */
+                /*
+                 * tell A7 the next LPM mode is WAIT/STOP
+                 */
+                if (next_lpm_mode == LPCR_M4_WAIT)
+                    LPM_MCORE_SendMessage(MSG_LPM_M4_WAIT);
+                else if (next_lpm_mode == LPCR_M4_STOP)
+                    LPM_MCORE_SendMessage(MSG_LPM_M4_STOP);
+                /* 
+                 * do modification to BASEPRI and NVIC->Priority settings so that
+                 * all interrupt except wakeup interrupt are disabled
+                 */
+                lpm_disable_non_wakeup_interrupt(&basePriBackup);
+
+                /*
+                 * Inside "runInRAM", M4 will inform A7 that it release the highbus. Later
+                 * when M4 is waken up, it will request A7 to resume highbus. This section
+                 * of code must run in TCM to avoid accessing highbus dependent resouces
+                 */
+                __WFI();
+
+                // Restore Basepri and NVIC->Priority settings
+                lpm_enable_non_wakeup_interrupt(basePriBackup);
+                break;
+            default:
+                break;
+        }
+        /* 
+         * Recover PRIMASK register value. this will enable the wakeup interrupt
+         * handler and will activate the main task immediately
+         */
+        __set_PRIMASK(priMaskBackup);
+    }
+    else {
+        /*
+         * Normal WFI which will be wakeup by any enabled interrupt
+         */
+        __WFI();
+    }
+}
+
diff --git a/examples/imx7_colibri_m4/low_power_demo/lpm_mcore.h b/examples/imx7_colibri_m4/low_power_demo/lpm_mcore.h
new file mode 100644
index 0000000..8e72585
--- /dev/null
+++ b/examples/imx7_colibri_m4/low_power_demo/lpm_mcore.h
@@ -0,0 +1,132 @@
+/*
+ * Copyright (c) 2015, Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * o Redistributions of source code must retain the above copyright notice, this list
+ *   of conditions and the following disclaimer.
+ *
+ * o Redistributions in binary form must reproduce the above copyright notice, this
+ *   list of conditions and the following disclaimer in the documentation and/or
+ *   other materials provided with the distribution.
+ *
+ * o Neither the name of Freescale Semiconductor, Inc. nor the names of its
+ *   contributors may be used to endorse or promote products derived from this
+ *   software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __LPM_MCORE_H__
+#define __LPM_MCORE_H__
+
+#include "gpc.h"
+#include "device_imx.h"
+
+#define DELAY_LOOP_CNT_LOW_SPEED 10
+#define DELAY_CNT 100
+
+#define LPCR_M4_RUN  0
+#define LPCR_M4_WAIT 1
+#define LPCR_M4_STOP 2
+
+#define LPM_MCORE_MU_CHANNEL    0
+
+#define LPM_MCORE_PRINT_DEBUG_INFO      1
+
+#define MSG_LPM_M4_RUN                  0x5A5A0001
+#define MSG_LPM_M4_WAIT                 0x5A5A0002
+#define MSG_LPM_M4_STOP                 0x5A5A0003
+
+#define MSG_LPM_M4_REQUEST_HIGHBUS      0x2222CCCC
+#define MSG_LPM_M4_RELEASE_HIGHBUS      0x2222BBBB
+
+#define MSG_LPM_A7_HIGHBUS_READY        0xFFFF6666
+
+#define GPC_SYNC_DELAY_CNT              65536
+
+/*
+ * LPM state of M4 core
+ */
+typedef enum lpm_power_status_m4 {
+    LPM_M4_STATE_RUN,
+    LPM_M4_STATE_WAIT,
+    LPM_M4_STATE_STOP,
+} LPM_POWER_STATUS_M4;
+
+/*
+ * Clock Speed of M4 core
+ */
+typedef enum lpm_m4_clock_speed {
+    LPM_M4_HIGH_FREQ,
+    LPM_M4_LOW_FREQ
+} LPM_M4_CLOCK_SPEED;
+
+/*
+ * Linklist of wakeup interrupt
+ */
+typedef struct wakeup_int_ele WAKEUP_INT_ELE, *P_WAKEUP_INT_ELE;
+struct wakeup_int_ele {
+    P_WAKEUP_INT_ELE next;
+    uint32_t irq_no;
+    uint32_t irq_priority_backup;
+};
+
+/*
+ * low power driver initialization
+ */
+void LPM_MCORE_Init(GPC_Type * base);
+
+/*
+ * get the current lpm state of M4 core
+ */
+LPM_POWER_STATUS_M4 LPM_MCORE_GetPowerStatus(GPC_Type * base);
+
+/*
+ * set the next lpm state of M4 core, the state will be entered
+ * next time WFI is executed
+ */
+void LPM_MCORE_SetPowerStatus(GPC_Type * base, LPM_POWER_STATUS_M4 m4_next_lpm);
+
+/*
+ * provide readable information of current m4 core lpm state
+ */
+const char* LPM_MCORE_GetPowerStatusString(void);
+
+/*
+ * register/unregister a peripherail interrupt to M4 core wakeup interrupt
+ */
+void LPM_MCORE_RegisterWakeupInterrupt(GPC_Type * base, uint32_t irq_no, GPC_IRQ_WAKEUP_MODE wakeup_mode);
+
+/*
+ * change the m4 core clock between 24MHz(OSC) and 240MHz (SysPllDiv2)
+ */
+void LPM_MCORE_ChangeM4Clock(LPM_M4_CLOCK_SPEED target);
+
+/*
+ * Check if A7 LPM driver is ready
+ */
+uint32_t LPM_MCORE_CheckPeerReady(void);
+
+/*
+ * Set M4 LPM driver ready flag to A7 Peer
+ */
+void LPM_MCORE_SetSelfReady(void);
+
+/*
+ * Function to inserted into FreeRTOS idletask hook
+ */
+void LPM_MCORE_WaitForInt(void);
+
+#endif
diff --git a/examples/imx7_colibri_m4/low_power_demo/main.c b/examples/imx7_colibri_m4/low_power_demo/main.c
index 24f2e66..2ccac97 100644
--- a/examples/imx7_colibri_m4/low_power_demo/main.c
+++ b/examples/imx7_colibri_m4/low_power_demo/main.c
@@ -38,6 +38,7 @@
 #include "gpio_pins.h"
 #include "gpio_imx.h"
 #include "ugui/ugui.h"
+#include "lpm_mcore.h"
 
 TaskHandle_t xLcdTaskHandle;
 
@@ -183,7 +184,7 @@ static void LCD_SetXY(int x, int y)
 {
 	unsigned char cmd[3];
 
-	PRINTF("LCD_SetXY(%d,%d)\n\r", x, y);
+	//PRINTF("LCD_SetXY(%d,%d)\n\r", x, y);
 	CLAMP(x, 0, LCDWIDTH-1);
 	CLAMP(y, 0, LCDPAGES-1);
 
@@ -236,6 +237,7 @@ void LCD_Task(void *pvParameters)
 	};
 
 	/* GPIO module initialize, configure "LED" as output and button as interrupt mode. */
+	PRINTF("Before init\n\r");
 	LCD_Init();
 
 	LCD_SendBytes(LCD_init_seq, sizeof(LCD_init_seq), LCD_COMMAND);
@@ -278,17 +280,27 @@ void LCD_Task(void *pvParameters)
 	LCD_SendFB(fb);
 
 	int x = 0;
+	char control_char;
 	while (true) {
-	//	data = ~data;
-/*
 		LCD_SetPixel(x, 7, C_BLACK);
 		LCD_SetPixel(x, 8, C_BLACK);
-*/
+
 		x++;
 		LCD_SendFB(fb);
-		vTaskDelay(100);
+		//vTaskDelay(100);
 		if (x >= LCDWIDTH)
 			x = 0;
+
+		PRINTF("\r\nCPU delaying for 5s: ");
+		vTaskDelay(5000);
+		PRINTF("done");
+
+		PRINTF("\r\nCPU spinning, press any character: ");
+		control_char = GETCHAR();
+		PRINTF("%c", control_char);/*
+		if ((control_char == 's') || (control_char == 'S')) {
+			break;
+		}*/
 	}
 /*
 	uint8_t page[128];
@@ -324,7 +336,7 @@ void LCD_Task(void *pvParameters)
 void vApplicationIdleHook(void)
 {
 	/* Waiting for Wake up event. */
-	__WFI();
+	LPM_MCORE_WaitForInt();
 }
 
 /*!
@@ -334,6 +346,7 @@ int main(void)
 {
 	/* hardware initialiize, include RDC, IOMUX and UART debug */
 	hardware_init();
+	LPM_MCORE_ChangeM4Clock(LPM_M4_LOW_FREQ);
 	PRINTF("\n\r=> Low Power Demo\n\r");
 
 	xTaskCreate(LCD_Task, "LCD Task", configMINIMAL_STACK_SIZE,