simplelink: dpl: update SwiP in dpl to enable RF driver

Also updated HwiP in order to support SwiP, and added QueueP which is
a dependency.

Signed-off-by: Vincent Wan <vincent.wan@linaro.org>

simplelink/CMakeLists.txt

@@ -73,6 +73,7 @@ elseif(CONFIG_HAS_CC13X2_CC26X2_SDK)
     kernel/zephyr/dpl/HwiP_zephyr.c
     kernel/zephyr/dpl/SwiP_zephyr.c
     kernel/zephyr/dpl/SemaphoreP_zephyr.c
+    kernel/zephyr/dpl/QueueP_zephyr.c
     )
 
   if(CONFIG_SOC_CC1352R)

simplelink/kernel/zephyr/dpl/HwiP_zephyr.c

@@ -128,12 +128,20 @@ void HwiP_delete(HwiP_Handle handle)
 	irq_disable(interruptNum - 16);
 }
 #elif defined(CONFIG_SOC_SERIES_CC13X2_CC26X2)
+/* INT_PENDSV is already taken by Zephyr, so let's use INT_SWEV0 to support
+ * SwiP since this line is usually unused. Change this to a different
+ * interrupt if desired.
+ */
+int HwiP_swiPIntNum = INT_SWEV0;
+
 typedef struct _HwiP_Obj {
     uint32_t intNum;
+    struct sl_isr_args * cb;
 } HwiP_Obj;
 
 static struct sl_isr_args sl_OSC_COMB_cb = {NULL, 0};
 static struct sl_isr_args sl_AUX_COMB_cb = {NULL, 0};
+static struct sl_isr_args sl_SWEV0_cb = {NULL, 0};
 
 /*
  *  ======== HwiP_construct ========
@@ -143,32 +151,70 @@ HwiP_Handle HwiP_construct(HwiP_Struct *handle, int interruptNum,
 {
 	HwiP_Obj *obj = (HwiP_Obj *)handle;
 	uintptr_t arg = 0;
+	uint8_t priority = INT_PRI_LEVEL7; /* default to lowest priority */
 	
 	if (handle == NULL) {
 		return NULL;
 	}
 
 	if (params) {
+		priority = params->priority;
 		arg = params->arg;
 	}
 
 	/*
-	 * Currently only support INT_OSC_COMB and INT_AUX_COMB
+	 * Currently only support INT_OSC_COMB, INT_AUX_COMB, INT_SWEV0
 	 */
-	__ASSERT(INT_OSC_COMB == interruptNum || INT_AUX_COMB == interruptNum,
+	__ASSERT(INT_OSC_COMB == interruptNum || INT_AUX_COMB == interruptNum
+		|| INT_SWEV0 == interruptNum,
 		 "Unexpected interruptNum: %d\r\n",
 		 interruptNum);
 
+	/*
+	 * Priority expected is either:
+	 *    INT_PRI_LEVEL1 to INT_PRI_LEVEL7,
+	 *    or ~0 or 255 (meaning lowest priority)
+	 *    ~0 and 255 are meant to be the same as INT_PRI_LEVEL7.
+	 *    For ~0 or 255, we want 7; but Zephyr IRQ_CONNECT adds +1
+	 *    to reserve INT_PRI_LEVEL0,
+	 *    so we pass 6 for those TI drivers passing prio = ~0.
+	 */
+	__ASSERT((INT_PRI_LEVEL7 == priority) ||
+		(INT_PRI_LEVEL6 == priority) ||
+		(INT_PRI_LEVEL5 == priority) ||
+		(INT_PRI_LEVEL4 == priority) ||
+		(INT_PRI_LEVEL3 == priority) ||
+		(INT_PRI_LEVEL2 == priority) ||
+		(INT_PRI_LEVEL1 == priority) ||
+		(0xFF == priority),
+		"Unexpected priority level, got: 0x%x\r\n",
+		(unsigned int)priority);
+
+	if (0xFF == priority) {
+		priority = INT_PRI_LEVEL7;
+	}
+
+	/* The priority for IRQ_CONNECT is encoded in the top 3 bits */ 
+	priority = (priority >> 5) - 1;
+
 	switch(interruptNum) {
 	case INT_OSC_COMB:
 		sl_OSC_COMB_cb.cb = hwiFxn;
 		sl_OSC_COMB_cb.arg = arg;
-		IRQ_CONNECT(INT_OSC_COMB - 16, 6, sl_isr, &sl_OSC_COMB_cb, 0);
+		obj->cb = &sl_OSC_COMB_cb;
+		IRQ_CONNECT(INT_OSC_COMB - 16, priority, sl_isr, &sl_OSC_COMB_cb, 0);
 		break;
 	case INT_AUX_COMB:
 		sl_AUX_COMB_cb.cb = hwiFxn;
 		sl_AUX_COMB_cb.arg = arg;
-		IRQ_CONNECT(INT_AUX_COMB - 16, 6, sl_isr, &sl_AUX_COMB_cb, 0);
+		obj->cb = &sl_AUX_COMB_cb;
+		IRQ_CONNECT(INT_AUX_COMB - 16, priority, sl_isr, &sl_AUX_COMB_cb, 0);
+		break;
+	case INT_SWEV0:
+		sl_SWEV0_cb.cb = hwiFxn;
+		sl_SWEV0_cb.arg = arg;
+		obj->cb = &sl_SWEV0_cb;
+		IRQ_CONNECT(INT_SWEV0 - 16, priority, sl_isr, &sl_SWEV0_cb, 0);
 		break;
 	default:
 		return(NULL);
@@ -221,15 +267,22 @@ void HwiP_restore(uintptr_t key)
 	irq_unlock(key);
 }
 
-void HwiP_post(int interruptNum) {
-    ARG_UNUSED(interruptNum);
-    STUB("");
+#if defined(CONFIG_SOC_SERIES_CC13X2_CC26X2)
+void HwiP_post(int interruptNum)
+{
+	IntPendSet((uint32_t)interruptNum);
 }
-void HwiP_setFunc(HwiP_Handle hwiP, HwiP_Fxn fxn, uintptr_t arg) {
-    ARG_UNUSED(hwiP);
-    ARG_UNUSED(fxn);
-    ARG_UNUSED(arg);
-    STUB("");
+
+void HwiP_setFunc(HwiP_Handle hwiP, HwiP_Fxn fxn, uintptr_t arg)
+{
+	HwiP_Obj *obj = (HwiP_Obj *)hwiP;
+
+	uintptr_t key = HwiP_disable();
+
+	obj->cb->cb = fxn;
+	obj->cb->arg = arg;
+
+	HwiP_restore(key);
 }
 
 void HwiP_destruct(HwiP_Struct *hwiP)
@@ -244,3 +297,4 @@ void HwiP_destruct(HwiP_Struct *hwiP)
 	obj->cb->arg = (uintptr_t)NULL;
 	obj->cb = NULL;
 }
+#endif

simplelink/kernel/zephyr/dpl/QueueP.h

@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2017, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  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.
+ *
+ * *  Neither the name of Texas Instruments Incorporated 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 OWNER 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.
+ */
+/*
+ *  ======== QueueP.h ========
+ */
+
+typedef struct _QueueP_Elem {
+    struct _QueueP_Elem *volatile next;
+    struct _QueueP_Elem *volatile prev;
+} QueueP_Elem;
+
+typedef struct _QueueP_Obj {
+    QueueP_Elem elem;
+} QueueP_Obj;
+
+typedef QueueP_Obj *QueueP_Handle;
+
+void QueueP_init(QueueP_Obj *obj);
+uintptr_t QueueP_head(QueueP_Obj *obj);
+uintptr_t QueueP_next(QueueP_Elem *qelem);
+uintptr_t QueueP_prev(QueueP_Elem *qelem);
+uintptr_t QueueP_get(QueueP_Obj *obj);
+void QueueP_put(QueueP_Obj *obj, QueueP_Elem *elem);
+void QueueP_remove(QueueP_Elem *qelem) ;
+bool QueueP_empty(QueueP_Obj *obj);

simplelink/kernel/zephyr/dpl/QueueP_zephyr.c

@@ -0,0 +1,199 @@
+/*
+ * Copyright (c) 2017, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * *  Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *
+ * *  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.
+ *
+ * *  Neither the name of Texas Instruments Incorporated 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 OWNER 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.
+ */
+/*
+ *  ======== QueueP_zephyr.c ========
+ */
+
+#include <ti/drivers/dpl/HwiP.h>
+#include "QueueP.h"
+
+/*
+ *  ======== QueueP_init ========
+ */
+void QueueP_init(QueueP_Obj *obj)
+{
+    obj->elem.next = obj->elem.prev = &(obj->elem);
+}
+
+/*
+ *  ======== QueueP_empty ========
+ */
+bool QueueP_empty(QueueP_Obj *obj)
+{
+    return (obj->elem.next == &(obj->elem));
+}
+
+/*
+ *  ======== QueueP_get ========
+ */
+uintptr_t QueueP_get(QueueP_Obj *obj)
+{
+    QueueP_Elem *elem;
+    uintptr_t key;
+
+    key = HwiP_disable();
+
+    elem = obj->elem.next;
+
+    obj->elem.next = elem->next;
+    elem->next->prev = &(obj->elem);
+
+    HwiP_restore(key);
+
+    return ((uintptr_t)elem);
+
+}
+
+/*
+ *  ======== QueueP_getTail ========
+ */
+uintptr_t QueueP_getTail(QueueP_Obj *obj)
+{
+    QueueP_Elem *elem;
+    uintptr_t key;
+
+    key = HwiP_disable();
+
+    elem = obj->elem.prev;
+
+    obj->elem.prev = elem->prev;
+    elem->prev->next = &(obj->elem);
+
+    HwiP_restore(key);
+
+    return ((uintptr_t)elem);
+
+}
+
+/*
+ *  ======== QueueP_head ========
+ */
+uintptr_t QueueP_head(QueueP_Obj *obj)
+{
+    return ((uintptr_t)(obj->elem.next));
+}
+
+/*
+ *  ======== elemClear ========
+ */
+void QueueP_elemClear(QueueP_Elem *qelem)
+{
+    qelem->next = qelem->prev = qelem;
+}
+
+/*
+ *  ======== insert ========
+ */
+void QueueP_insert(QueueP_Elem *qelem, QueueP_Elem *elem)
+{
+    QueueP_put((QueueP_Obj *)qelem, elem);
+}
+
+/*
+ *  ======== next ========
+ */
+uintptr_t QueueP_next(QueueP_Elem *qelem)
+{
+    return ((uintptr_t)qelem->next);
+}
+
+/*
+ *  ======== QueueP_prev ========
+ */
+uintptr_t QueueP_prev(QueueP_Elem *qelem)
+{
+    return ((uintptr_t)qelem->prev);
+}
+
+/*
+ *  ======== QueueP_put ========
+ */
+void QueueP_put(QueueP_Obj *obj, QueueP_Elem *elem)
+{
+    uintptr_t key;
+
+    key = HwiP_disable();
+
+    elem->next = &(obj->elem);
+    elem->prev = obj->elem.prev;
+    obj->elem.prev->next = elem;
+    obj->elem.prev = elem;
+
+    HwiP_restore(key);
+}
+
+/*
+ *  ======== QueueP_putHead ========
+ */
+void QueueP_putHead(QueueP_Obj *obj, QueueP_Elem *elem)
+{
+    uintptr_t key;
+
+    key = HwiP_disable();
+
+    elem->prev = &(obj->elem);
+    elem->next = obj->elem.next;
+    obj->elem.next->prev = elem;
+    obj->elem.next = elem;
+
+    HwiP_restore(key);
+}
+
+/*
+ *  ======== QueueP_remove ========
+ */
+void QueueP_remove(QueueP_Elem *qelem)
+{
+#if defined(__IAR_SYSTEMS_ICC__)
+    QueueP_Elem *temp;
+    temp = qelem->next;
+    qelem->prev->next = temp;
+    temp = qelem->prev;
+    qelem->next->prev = temp;
+#else
+    qelem->prev->next = qelem->next;
+    qelem->next->prev = qelem->prev;
+#endif
+}
+
+/*
+ *  ======== isQueued ========
+ */
+bool QueueP_isQueued(QueueP_Elem *qelem)
+{
+    if ((qelem->prev == qelem) && (qelem->next == qelem)) {
+        return (false);
+    }
+    else {
+        return (true);
+    }
+}

simplelink/kernel/zephyr/dpl/SwiP_zephyr.c

@@ -6,17 +6,273 @@
 
 #include <zephyr.h>
 
+#include <ti/drivers/dpl/HwiP.h>
 #include <ti/drivers/dpl/SwiP.h>
 
+#include "QueueP.h"
+
+/* Lowest priority interrupt */
+#define INT_PRI_LEVEL7 0xe0
+
+#define NUMPRI 4
+
+typedef enum {
+    SwiP_State_Idle = 0,
+    SwiP_State_Posted,
+    SwiP_State_Running,
+    SwiP_State_Interrupted
+} SwiP_State;
+
+typedef uint32_t SwiP_LockState;
+
+enum {
+    SwiP_LockState_Locked,
+    SwiP_LockState_Unlocked
+};
+
+typedef struct SwiP_Obj {
+    QueueP_Elem    elem;
+    QueueP_Handle  readyList;
+    SwiP_Params    params;
+    SwiP_Fxn       fxn;
+    uint32_t       trigger;
+    uint32_t       state;
+} SwiP_Obj;
+
+static void SwiP_handleHwi();
+
+static const SwiP_Params SwiP_defaultParams = {
+    .arg0 = (uintptr_t) NULL,
+    .arg1 = (uintptr_t) NULL,
+    .priority = ~0,  /* max priority */
+    .trigger = 0,
+};
+
+static volatile int SwiP_readyMask;
+static volatile SwiP_LockState SwiP_lockState;
+static volatile uint32_t SwiP_currentTrigger;
+static volatile bool SwiP_schedulerRunning;
+static volatile bool SwiP_initialized = false;
+static HwiP_Struct SwiP_hwiStruct;
+static QueueP_Obj SwiP_readyList[NUMPRI];
+
+/* don't call with n == 0 */
+static int maxbit(int n)
+{
+    int mask = 1 << (NUMPRI - 1);
+    int max = NUMPRI - 1;
+
+    while (mask) {
+        if (n & mask) {
+            return max;
+        }
+        max--;
+        mask >>= 1;
+    }
+
+    return 0;
+}
+
+/*
+ *  ======== SwiP_Params_init ========
+ */
+void SwiP_Params_init(SwiP_Params *params) {
+    /* structure copy */
+    *params = SwiP_defaultParams;
+}
+
+/*
+ *  ======== SwiP_construct ========
+ */
+SwiP_Handle SwiP_construct(SwiP_Struct *handle, SwiP_Fxn swiFxn,
+    SwiP_Params *params)
+{
+    SwiP_Obj *swi = (SwiP_Obj *)handle;
+    HwiP_Params hwiParams;
+    uintptr_t hwiKey;
+    uint32_t priority;
+    int i;
+
+    if (handle != NULL) {
+        hwiKey = HwiP_disable();
+
+        if (SwiP_initialized == false) {
+            for (i = 0; i < NUMPRI; i++) {
+                QueueP_init(&SwiP_readyList[i]);
+            }
+            SwiP_readyMask = 0;
+            SwiP_currentTrigger = 0;
+            SwiP_lockState = SwiP_LockState_Unlocked;
+            SwiP_schedulerRunning = false;
+
+            HwiP_Params_init(&hwiParams);
+            hwiParams.priority = INT_PRI_LEVEL7;  // use the lowest priority
+            HwiP_construct(&SwiP_hwiStruct, HwiP_swiPIntNum, SwiP_handleHwi,
+                           &hwiParams);
+
+            SwiP_initialized = true;
+        }
+
+        HwiP_restore(hwiKey);
+
+        if (params == NULL) {
+            params = (SwiP_Params *)&SwiP_defaultParams;
+        }
+
+        if (params->priority == (~0)) {
+            priority = NUMPRI - 1;
+        }
+        else {
+            priority = params->priority;
+        }
+
+        if (priority >= NUMPRI) {
+            return NULL;
+        }
+        else {
+            QueueP_init((QueueP_Obj *)&swi->elem);
+            swi->params = *params;
+            swi->params.priority = priority;
+            swi->fxn = swiFxn;
+            swi->trigger = swi->params.trigger;
+            swi->state = SwiP_State_Idle;
+            swi->readyList = &SwiP_readyList[priority];
+        }
+    }
+
+    return ((SwiP_Handle)swi);
+}
+
+/*
+ *  ======== SwiP_destruct ========
+ */
+void SwiP_destruct(SwiP_Struct *handle)
+{
+    SwiP_Obj *swi = (SwiP_Obj *)handle;
+    uintptr_t hwiKey = HwiP_disable();
+
+    /* if on SwiP_readyList, remove it */
+    QueueP_remove(&swi->elem);
+    if (QueueP_empty(swi->readyList)) {
+        SwiP_readyMask &= ~(1 << swi->params.priority);
+    }
+
+    HwiP_restore(hwiKey);
+}
 
 /*
  *  ======== SwiP_disable ========
  */
 uintptr_t SwiP_disable(void)
 {
-    k_sched_lock();
+    uintptr_t previousHwiState = HwiP_disable();
 
-    return 0;
+    SwiP_LockState previousLockState = SwiP_lockState;
+    SwiP_lockState = SwiP_LockState_Locked;
+
+    HwiP_restore(previousHwiState);
+
+    return previousLockState;
+}
+
+/*
+ * This is a non-preemptive fixed-priority scheduler implementation.
+ * It runs with interrupts disabled, but enables them for each swi.
+ */
+void SwiP_dispatch(uintptr_t hwiKey)
+{
+    SwiP_Obj *swi;
+    int maxpri;
+
+    while (SwiP_readyMask && (SwiP_lockState == SwiP_LockState_Unlocked)) {
+        maxpri = maxbit(SwiP_readyMask);
+        swi = (SwiP_Obj *)QueueP_get(&SwiP_readyList[maxpri]);
+
+        if (QueueP_empty(&SwiP_readyList[maxpri])) {
+            SwiP_readyMask &= ~(1 << maxpri);
+        }
+
+        /*
+         * Prepare the swi for execution.  The trigger has to be saved
+         * because the swi might re-post itself with another trigger value.
+         */
+        swi->state = SwiP_State_Running;
+        SwiP_currentTrigger = swi->trigger;
+        swi->trigger = swi->params.trigger;
+
+        /* run the swi with interrupts enabled */
+        HwiP_restore(hwiKey);
+        swi->fxn(swi->params.arg0, swi->params.arg1);
+        hwiKey = HwiP_disable();
+
+        /* If the swi didn't get re-posted, set it to idle now */
+        if (swi->state == SwiP_State_Running) {
+            swi->state = SwiP_State_Idle;
+        }
+    }
+
+    /* Scheduler was set to running immediately after posting the interrupt */
+    SwiP_schedulerRunning = false;
+}
+
+static void SwiP_handleHwi()
+{
+    uintptr_t hwiKey = HwiP_disable();
+
+    SwiP_dispatch(hwiKey);
+
+    HwiP_restore(hwiKey);
+}
+
+/*
+ *  ======== SwiP_getTrigger ========
+ */
+uint32_t SwiP_getTrigger()
+{
+    return (SwiP_currentTrigger);
+}
+
+
+/*
+ *  ======== SwiP_or ========
+ */
+void SwiP_or(SwiP_Handle handle, uint32_t mask)
+{
+    SwiP_Obj *swi = (SwiP_Obj *)handle;
+    uintptr_t hwiKey = HwiP_disable();
+
+    swi->trigger |= mask;
+
+    HwiP_restore(hwiKey);
+    SwiP_post(swi);
+}
+
+/*
+ *  ======== SwiP_post ========
+ */
+void SwiP_post(SwiP_Handle handle)
+{
+    SwiP_Obj *swi = (SwiP_Obj *)handle;
+    uintptr_t hwiKey = HwiP_disable();
+
+    /* (Re-)post an swi only once */
+    if (swi->state != SwiP_State_Posted) {
+        swi->state = SwiP_State_Posted;
+
+        QueueP_put(&SwiP_readyList[swi->params.priority],
+                   (QueueP_Elem *)&swi->elem);
+        SwiP_readyMask |= 1 << swi->params.priority;
+    }
+
+    /* Activate the scheduler when not already running */
+    if ((SwiP_schedulerRunning == false) &&
+        (SwiP_lockState == SwiP_LockState_Unlocked)) {
+        HwiP_post(HwiP_swiPIntNum);
+        /* Set the scheduler into running state to avoid double posts */
+        SwiP_schedulerRunning = true;
+    }
+
+    HwiP_restore(hwiKey);
 }
 
 /*
@@ -24,6 +280,17 @@ uintptr_t SwiP_disable(void)
  */
 void SwiP_restore(uintptr_t key)
 {
-    ARG_UNUSED(key);
-    k_sched_unlock();
+    uintptr_t hwiKey = HwiP_disable();
+
+    SwiP_lockState = key;
+
+    /* Determine whether the scheduler needs to run */
+    if (SwiP_readyMask && (key == SwiP_LockState_Unlocked) &&
+        (SwiP_schedulerRunning == false)) {
+        HwiP_post(HwiP_swiPIntNum);
+        /* Set the scheduler into running state to avoid double posts */
+        SwiP_schedulerRunning = true;
+    }
+
+    HwiP_restore(hwiKey);
 }