From 7e1e02b8a0fc6f448892b82203a4a77606eb83c9 Mon Sep 17 00:00:00 2001 From: patacongo Date: Sun, 4 Sep 2011 19:24:27 +0000 Subject: Add a basic power management framework git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@3936 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/drivers/Makefile | 1 + nuttx/drivers/pm/Make.defs | 50 +++++++ nuttx/drivers/pm/pm_activity.c | 166 ++++++++++++++++++++++ nuttx/drivers/pm/pm_changestate.c | 223 +++++++++++++++++++++++++++++ nuttx/drivers/pm/pm_checkstate.c | 161 +++++++++++++++++++++ nuttx/drivers/pm/pm_initialize.c | 112 +++++++++++++++ nuttx/drivers/pm/pm_internal.h | 210 ++++++++++++++++++++++++++++ nuttx/drivers/pm/pm_register.c | 112 +++++++++++++++ nuttx/drivers/pm/pm_update.c | 287 ++++++++++++++++++++++++++++++++++++++ 9 files changed, 1322 insertions(+) create mode 100755 nuttx/drivers/pm/Make.defs create mode 100644 nuttx/drivers/pm/pm_activity.c create mode 100644 nuttx/drivers/pm/pm_changestate.c create mode 100644 nuttx/drivers/pm/pm_checkstate.c create mode 100644 nuttx/drivers/pm/pm_initialize.c create mode 100644 nuttx/drivers/pm/pm_internal.h create mode 100644 nuttx/drivers/pm/pm_register.c create mode 100644 nuttx/drivers/pm/pm_update.c (limited to 'nuttx/drivers') diff --git a/nuttx/drivers/Makefile b/nuttx/drivers/Makefile index d1361e3b5..8c86feff6 100644 --- a/nuttx/drivers/Makefile +++ b/nuttx/drivers/Makefile @@ -56,6 +56,7 @@ include mmcsd/Make.defs include mtd/Make.defs include net/Make.defs include pipes/Make.defs +include pm/Make.defs include sensors/Make.defs include serial/Make.defs include usbdev/Make.defs diff --git a/nuttx/drivers/pm/Make.defs b/nuttx/drivers/pm/Make.defs new file mode 100755 index 000000000..6204973f5 --- /dev/null +++ b/nuttx/drivers/pm/Make.defs @@ -0,0 +1,50 @@ +############################################################################ +# drivers/pm/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. 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. +# 3. Neither the name NuttX 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. +# +############################################################################ + +# Has power management support been initialized? + +ifeq ($(CONFIG_PM),y) + +# Include power management sources + +CSRCS += pm_activity.c pm_changestate.c pm_checkstate.c pm_initialize.c pm_register.c pm_update.c + +# Include power management in the build + +DEPPATH += --dep-path pm +VPATH += :pm +CFLAGS += ${shell $(TOPDIR)/tools/incdir.sh $(INCDIROPT) "$(CC)" $(TOPDIR)/drivers/pm} +endif + diff --git a/nuttx/drivers/pm/pm_activity.c b/nuttx/drivers/pm/pm_activity.c new file mode 100644 index 000000000..f1518520b --- /dev/null +++ b/nuttx/drivers/pm/pm_activity.c @@ -0,0 +1,166 @@ +/**************************************************************************** + * drivers/pm/pm_activity.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include "pm_internal.h" + +#ifdef CONFIG_PM + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: pm_activity + * + * Description: + * This function is called by a device driver to indicate that it is + * performing meaningful activities (non-idle). This increment an activty + * cound and/or will restart a idle timer and prevent entering IDLE + * power states. + * + * Input Parameters: + * priority - activity priority, range 0-9. Larger values correspond to + * higher priorities. Higher priority activity can prevent the system + * fromentering reduced power states for a longer period of time. + * + * As an example, a button press might be higher priority activity because + * it means that the user is actively interacting with the device. + * + * Returned Value: + * None. + * + * Assumptions: + * This function may be called from an interrupt handler (this is the ONLY + * PM function that may be called from an interrupt handler!). + * + ****************************************************************************/ + +void pm_activity(int priority) +{ + uint32_t now; + uint32_t accum; + irqstate_t flags; + + /* Just increment the activity count in the current time slice. The priority + * is simply the number of counts that are added. + */ + + if (priority > 0) + { + /* Add the priority to the accumulated counts in a critical section. */ + + flags = irqsave(); + accum = (uint32_t)g_pmglobals.accum + priority; + + /* Make sure that we do not overflow the underlying uint16_t representation */ + + if (accum > UINT16_MAX) + { + accum = UINT16_MAX; + } + + /* Save the updated count */ + + g_pmglobals.accum = accum; + + /* Check the elapsed time. In periods of low activity, time slicing is + * controlled by IDLE loop polling; in periods of higher activity, time + * slicing is controlled by driver activity. In either case, the duration + * of the time slice is only approximate; during times of heavy activity, + * time slices may be become longer and the activity level may be over- + * estimated. + */ + + now = clock_systimer(); + if (now - g_pmglobals.stime >= TIME_SLICE_TICKS) + { + uint16_t tmp; + + /* Sample the count, reset the time and count, and assess the PM + * state. This is an atomic operation because interrupts are + * still disabled. + */ + + tmp = g_pmglobals.accum; + g_pmglobals.stime = now; + g_pmglobals.accum = 0; + + /* Reassessing the PM state may require some computation. However, + * the work will actually be performed on a worker thread at a user- + * controlled priority. + */ + + (void)pm_update(accum); + } + + irqrestore(flags); + } +} + +#endif /* CONFIG_PM */ \ No newline at end of file diff --git a/nuttx/drivers/pm/pm_changestate.c b/nuttx/drivers/pm/pm_changestate.c new file mode 100644 index 000000000..e98e46236 --- /dev/null +++ b/nuttx/drivers/pm/pm_changestate.c @@ -0,0 +1,223 @@ +/**************************************************************************** + * drivers/pm/pm_changestate.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include "pm_internal.h" + +#ifdef CONFIG_PM + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: pm_prepall + * + * Description: + * Prepare every driver for the state change. + * + * Input Parameters: + * newstate - Idenfifies the new PM state + * + * Returned Value: + * 0 (OK) means that the callback function for all registered drivers + * returned OK (meaning that they accept the state change). Non-zero + * means that one of the drivers refused the state change. In this case, + * the system will revert to the preceding state. + * + * Assumptions: + * Interrupts are disabled. + * + ****************************************************************************/ + +static int pm_prepall(enum pm_state_e newstate) +{ + FAR sq_entry_t *entry; + int ret = OK; + + /* Visit each registered callback structure. */ + + for (entry = sq_peek(&g_pmglobals.registry); + entry && ret == OK; + entry = sq_next(entry)) + { + /* Is the prepare callback supported? */ + + FAR struct pm_callback_s *cb = (FAR struct pm_callback_s *)entry; + if (cb->prepare) + { + /* Yes.. prepare the driver */ + + ret = cb->prepare(cb, newstate); + } + } + + return ret; +} + +/**************************************************************************** + * Name: pm_changeall + * + * Description: + * Inform all drivers of the state change. + * + * Input Parameters: + * newstate - Idenfifies the new PM state + * + * Returned Value: + * None + * + * Assumptions: + * Interrupts are disabled. + * + ****************************************************************************/ + +static inline void pm_changeall(enum pm_state_e newstate) +{ + FAR sq_entry_t *entry; + + /* Visit each registered callback structure. */ + + for (entry = sq_peek(&g_pmglobals.registry); entry; entry = sq_next(entry)) + { + /* Is the notification callback supported? */ + + FAR struct pm_callback_s *cb = (FAR struct pm_callback_s *)entry; + if (cb->notify) + { + /* Yes.. notify the driver */ + + (void)cb->notify(cb, newstate); + } + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: pm_changestate + * + * Description: + * This function is used to platform-specific power managmeent logic. It + * will announce the power management power management state change to all + * drivers that have registered for power management event callbacks. + * + * Input Parameters: + * newstate - Idenfifies the new PM state + * + * Returned Value: + * 0 (OK) means that the callback function for all registered drivers + * returned OK (meaning that they accept the state change). Non-zero + * means that one of the drivers refused the state change. In this case, + * the system will revert to the preceding state. + * + * Assumptions: + * It is assumed that interrupts are disabled when this function is + * called. This function is probably called from the IDLE loop... the + * lowest priority task in the system. Changing driver power management + * states may result in renewed system activity and, as a result, can + * suspend the IDLE thread before it completes the entire state change + * unless interrupts are disabled throughout the state change. + * + ****************************************************************************/ + +int pm_changestate(enum pm_state_e newstate) +{ + irqstate_t flags; + int ret; + + /* Disable interrupts throught this operation... changing driver states + * could cause additional driver activity that might interfere with the + * state change. When the state change is complete, interrupts will be + * re-enabled. + */ + + flags = irqsave(); + + /* First, prepare the drivers for the state change. In this phase, + * drivers may refuse the state state change. + */ + + ret = pm_prepall(newstate); + if (ret != OK) + { + /* One or more drivers is not ready for this state change. Revert to + * the preceding state. + */ + + newstate = g_pmglobals.state; + (void)pm_prepall(newstate); + } + + /* All drivers have agreed to the state change (or, one or more have + * disagreed and the state has been reverted). Set the new state. + */ + + pm_changeall(newstate); + g_pmglobals.state = newstate; + return ret; +} + +#endif /* CONFIG_PM */ diff --git a/nuttx/drivers/pm/pm_checkstate.c b/nuttx/drivers/pm/pm_checkstate.c new file mode 100644 index 000000000..6b23ad05c --- /dev/null +++ b/nuttx/drivers/pm/pm_checkstate.c @@ -0,0 +1,161 @@ +/**************************************************************************** + * drivers/pm/pm_checkstate.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include "pm_internal.h" + +#ifdef CONFIG_PM + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: pm_checkstate + * + * Description: + * This function is called from the MCU-specific IDLE loop to monitor the + * the power management conditions. This function returns the "recommended" + * power management state based on the PM configuration and activity + * reported in the last sampling periods. The power management state is + * not automatically changed, however. The IDLE loop must call + * pm_changestate() in order to make the state change. + * + * These two steps are separated because the plaform-specific IDLE loop may + * have additional situational information that is not available to the + * the PM sub-system. For example, the IDLE loop may know that the + * battery charge level is very low and may force lower power states + * even if there is activity. + * + * NOTE: That these two steps are separated in time and, hence, the IDLE + * could be suspended for a long period of time between calling + * pm_checkstate() and pm_changestate(). There it is recommended that + * the IDLE loop make these calls atomic by either disabling interrupts + * until the state change is completed. + * + * Input Parameters: + * None + * + * Returned Value: + * The recommended power management state. + * + ****************************************************************************/ + +enum pm_state_e pm_checkstate(void) +{ + uint32_t now; + irqstate_t flags; + + /* Check for the end of the current time slice. This must be performed + * with interrupts disabled so that it does not conflict with the similar + * logic in pm_activity(). + */ + + flags = irqsave(); + + /* Check the elapsed time. In periods of low activity, time slicing is + * controlled by IDLE loop polling; in periods of higher activity, time + * slicing is controlled by driver activity. In either case, the duration + * of the time slice is only approximate; during times of heavy activity, + * time slices may be become longer and the activity level may be over- + * estimated. + */ + + now = clock_systimer(); + if (now - g_pmglobals.stime >= TIME_SLICE_TICKS) + { + uint16_t accum; + + /* Sample the count, reset the time and count, and assess the PM + * state. This is an atomic operation because interrupts are + * still disabled. + */ + + accum = g_pmglobals.accum; + g_pmglobals.stime = now; + g_pmglobals.accum = 0; + + /* Reassessing the PM state may require some computation. However, + * the work will actually be performed on a worker thread at a user- + * controlled priority. + */ + + (void)pm_update(accum); + } + irqrestore(flags); + + /* Return the recommended state. Assuming that we are called from the + * IDLE thread at the lowest priority level, any updates scheduled on the + * worker thread above should have already been peformed and the recommended + * state should be current: + */ + + return g_pmglobals.recommended; +} + +#endif /* CONFIG_PM */ diff --git a/nuttx/drivers/pm/pm_initialize.c b/nuttx/drivers/pm/pm_initialize.c new file mode 100644 index 000000000..70cd8d7e6 --- /dev/null +++ b/nuttx/drivers/pm/pm_initialize.c @@ -0,0 +1,112 @@ +/**************************************************************************** + * drivers/pm/pm_initialize.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include + +#include "pm_internal.h" + +#ifdef CONFIG_PM + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* All PM global data: */ + +struct pm_global_s g_pmglobals; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: pm_initialize + * + * Description: + * This function is called by MCU-specific one-time at power on reset in + * order to initialize the power management capabilities. This function + * must be called *very* early in the intialization sequence *before* any + * other device drivers are initialize (since they may attempt to register + * with the power management subsystem). + * + * Input parameters: + * None. + * + * Returned value: + * None. + * + ****************************************************************************/ + +void pm_initialize(void) +{ + /* Initialize the registry and the PM global data structures. The PM + * global data structure resides in .bss which is zeroed at boot time. So + * it is only required to initialize non-zero elements of the PM global + * data structure here. + */ + + sq_init(&g_pmglobals.registry); + sem_init(&g_pmglobals.regsem, 0, 1); +} + +#endif /* CONFIG_PM */ \ No newline at end of file diff --git a/nuttx/drivers/pm/pm_internal.h b/nuttx/drivers/pm/pm_internal.h new file mode 100644 index 000000000..e0cb3fb1a --- /dev/null +++ b/nuttx/drivers/pm/pm_internal.h @@ -0,0 +1,210 @@ +/**************************************************************************** + * drivers/pm/pm_internal.h + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +#ifndef __DRIVERS_PM_PM_INTERNAL_H +#define __DRIVERS_PM_PM_INTERNAL_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include +#include + +#ifdef CONFIG_PM + +/**************************************************************************** + * Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ + +#ifndef CONFIG_SCHED_WORKQUEUE +# warning "Worker thread support is required (CONFIG_SCHED_WORKQUEUE)" +#endif + +/* Convert the time slice interval into system clock ticks. + * + * CONFIG_PM_SLICEMS provides the duration of one time slice in milliseconds. + * CLOCKS_PER_SEC provides the number of timer ticks in one second. + * + * slice ticks = (CONFIG_PM_SLICEMS msec / 1000 msec/sec) / + * (CLOCKS_PER_SEC ticks/sec) + */ + +#define TIME_SLICE_TICKS ((CONFIG_PM_SLICEMS * CLOCKS_PER_SEC) / 1000) + +/* Function-like macros *****************************************************/ +/**************************************************************************** + * Name: pm_lock + * + * Descripton: + * Lock the power management registry. NOTE: This function may return + * an error if a signal is received while what (errno == EINTR). + * + ****************************************************************************/ + +#define pm_lock() sem_wait(&g_pmglobals.regsem); + +/**************************************************************************** + * Name: pm_unlock + * + * Descripton: + * Unlock the power management registry. + * + ****************************************************************************/ + +#define pm_unlock() sem_post(&g_pmglobals.regsem); + +/**************************************************************************** + * Public Types + ****************************************************************************/ +/* This structure encapsulates all of the global data used by the PM module */ + +struct pm_global_s +{ + /* state - The current state (as determined by an explicit call to + * pm_changestate() + * recommended - The recommended state based on the PM algorithm in + * function pm_update(). + * mndex - The index to the next slot in the memory[] arry to use. + * mcnt - A tiny counter used only at start up. The actual + * algorithm cannot be applied until CONFIG_PM_MEMORY + * samples have been collected. + */ + + uint8_t state; + uint8_t recommended; + uint8_t mndx; + uint8_t mcnt; + + /* accum - The accumulated counts in this time interval + * thrcnt - The number of below threshold counts seen. + */ + + uint16_t accum; + uint16_t thrcnt; + + /* This is the averaging "memory." The averaging algorithm is simply: + * Y = (An*X + SUM(Ai*Yi))/SUM(Aj), where i = 1..n-1 and j= 1..n, n is the + * length of the "memory", Ai is the weight applied to each value, and X is + * the current activity. + * + * CONFIG_PM_MEMORY provides the memory for the algorithm. Default: 2 + * CONFIG_PM_COEFn provides weight for each sample. Default: 1 + */ + +#if CONFIG_PM_MEMORY > 1 + uint16_t memory[CONFIG_PM_MEMORY-1]; +#endif + + /* stime - The time (in ticks) at the start of the current time slice */ + + uint32_t stime; + + /* This semaphore manages mutually exclusive access to the power management + * registry. It must be initialized to the value 1. + */ + + sem_t regsem; + + /* For work that has been deferred to the worker thread */ + + struct work_s work; + + /* registry is a singly-linked list of registered power management + * callback structures. To ensure mutually exclusive access, this list + * must be locked by calling pm_lock() before it is accessed. + */ + + sq_queue_t registry; +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#undef EXTERN +#if defined(__cplusplus) +# define EXTERN extern "C" +extern "C" +{ +#else +# define EXTERN extern +#endif + +/* All PM global data: */ + +EXTERN struct pm_global_s g_pmglobals; + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ + +/**************************************************************************** + * Name: pm_update + * + * Description: + * This internal function is called at the end of a time slice in order to + * update driver activity metrics and recommended states. + * + * Input Parameters: + * accum - The value of the activity accumulator at the end of the time + * slice. + * + * Returned Value: + * None. + * + * Assumptions: + * This function may be called from a driver, perhaps even at the interrupt + * level. It may also be called from the IDLE loop at the lowest possible + * priority level. To reconcile these various conditions, all work is + * performed on the worker thread at a user-selectable priority. + * + ****************************************************************************/ + +EXTERN void pm_update(uint16_t accum); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* CONFIG_PM */ +#endif /* #define __DRIVERS_PM_PM_INTERNAL_H */ diff --git a/nuttx/drivers/pm/pm_register.c b/nuttx/drivers/pm/pm_register.c new file mode 100644 index 000000000..41d4bb836 --- /dev/null +++ b/nuttx/drivers/pm/pm_register.c @@ -0,0 +1,112 @@ +/**************************************************************************** + * drivers/pm/pm_register.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include + +#include "pm_internal.h" + +#ifdef CONFIG_PM + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: pm_register + * + * Description: + * This function is called by a device driver in order to register to + * receive power management event callbacks. + * + * Input parameters: + * callbacks - An instance of struct pm_callback_s providing the driver + * callback functions. + * + * Returned value: + * Zero (OK) on success; otherwise a negater errno value is returned. + * + ****************************************************************************/ + +int pm_register(FAR struct pm_callback_s *callbacks) +{ + int ret; + + DEBUGASSERT(callbacks); + + /* Add the new entry to the end of the list of registered callbacks */ + + ret = pm_lock(); + if (ret == OK) + { + sq_addlast(&callbacks->entry, &g_pmglobals.registry); + pm_unlock(); + } + return ret; +} + +#endif /* CONFIG_PM */ \ No newline at end of file diff --git a/nuttx/drivers/pm/pm_update.c b/nuttx/drivers/pm/pm_update.c new file mode 100644 index 000000000..d3ff75090 --- /dev/null +++ b/nuttx/drivers/pm/pm_update.c @@ -0,0 +1,287 @@ +/**************************************************************************** + * drivers/pm/pm_update.c + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include +#include + +#include "pm_internal.h" + +#ifdef CONFIG_PM + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ +/* CONFIG_PM_MEMORY is the total number of time slices (including the current + * time slice. The histor or previous values is then CONFIG_PM_MEMORY-1. + */ + +#if CONFIG_PM_MEMORY > 1 +static const uint16_t g_pmcoeffs[CONFIG_PM_MEMORY-1] = +{ + CONFIG_PM_COEF1 +#if CONFIG_PM_MEMORY > 2 + , CONFIG_PM_COEF2 +#endif +#if CONFIG_PM_MEMORY > 3 + , CONFIG_PM_COEF3 +#endif +#if CONFIG_PM_MEMORY > 4 + , CONFIG_PM_COEF4 +#endif +#if CONFIG_PM_MEMORY > 5 + , CONFIG_PM_COEF5 +#endif +#if CONFIG_PM_MEMORY > 6 +# warning "This logic needs to be extended" +#endif +}; +#endif + +static const uint16_t g_pmthresh[3] = +{ + CONFIG_PM_IDLEENTER_THRESH, + CONFIG_PM_STANDBYENTER_THRESH, + CONFIG_PM_SLEEPENTER_THRESH +}; + +static const uint16_t g_pmcount[3] = +{ + CONFIG_PM_IDLEENTER_COUNT, + CONFIG_PM_STANDBYENTER_COUNT, + CONFIG_PM_SLEEPENTER_COUNT +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: pm_worker + * + * Description: + * This worker function is queue at the end of a time slice in order to + * update driver activity metrics and recommended states. + * + * Input Parameters: + * arg - The value of the activity accumulator at the end of the time + * slice. + * + * Returned Value: + * None. + * + * Assumptions: + * This function runs on the worker thread. + * + ****************************************************************************/ + +void pm_worker(FAR void *arg) +{ + uint16_t accum = (uint16_t)((uintptr_t)arg); + uint32_t Y; + +#if CONFIG_PM_MEMORY > 1 + uint32_t denom; + int i, j; + + /* We won't bother to do anything until we have accumulated + * CONFIG_PM_MEMORY-1 samples. + */ + + if (g_pmglobals.mcnt < CONFIG_PM_MEMORY-1) + { + g_pmglobals.memory[g_pmglobals.mcnt] = accum; + g_pmglobals.mcnt++; + return; + } + + /* The averaging algorithm is simply: Y = (An*X + SUM(Ai*Yi))/SUM(Aj), where + * i = 1..n-1 and j= 1..n, n is the length of the "memory", Ai is the + * weight applied to each value, and X is the current activity. + * + * CONFIG_PM_MEMORY provides the memory for the algorithm. Default: 2 + * CONFIG_PM_COEFn provides weight for each sample. Default: 1 + * + * First, calclate Y = An*X + */ + + Y = CONFIG_PM_COEFN * accum; + denom = CONFIG_PM_COEFN; + + /* Then calculate Y += SUM(Ai*Yi), i = 1..n-1. The oldest sample will + * reside at g_pmglobals.mndx (and this is the value that we will overwrite + * with the new value). + */ + + for (i = 0, j = g_pmglobals.mndx; i < CONFIG_PM_MEMORY-1; i++, j++) + { + if (j >= CONFIG_PM_MEMORY-1) + { + j = 0; + } + + Y += g_pmcoeffs[i] * g_pmglobals.memory[j]; + denom += g_pmcoeffs[i]; + } + + /* Compute and save the new activity value */ + + Y /= denom; + g_pmglobals.memory[g_pmglobals.mndx] = Y; + g_pmglobals.mndx++; + if (g_pmglobals.mndx >= CONFIG_PM_MEMORY-1) + { + g_pmglobals.mndx = 0; + } + +#else + + /* No smoothing */ + + Y = accum; + +#endif + + /* Now, compare this new activity level to the thresholds and counts for + * the next state. Determine if it is appropriate to switch to a new, + * lower power consumption state. + * + * If we are already in the SLEEP state, then there is nothing more to be + * done (in fact, I would be surprised to be executing!). + */ + + if (g_pmglobals.state < PM_SLEEP) + { + unsigned int nextstate; + int index; + + /* Get the next state and the table index for the next state (which will + * be the current state) + */ + + index = g_pmglobals.state; + nextstate = g_pmglobals.state + 1; + + /* Has the threshold for the next state been exceeded? */ + + if (Y > g_pmthresh[index]) + { + /* No... reset the count and recommend the current state */ + + g_pmglobals.thrcnt = 0; + g_pmglobals.recommended = g_pmglobals.state; + } + + /* Yes.. have we already recommended this state? If so, do nothing */ + + else if (g_pmglobals.recommended < nextstate) + { + /* No.. increment the count. Has is passed the the count required + * for a state transition? + */ + + if (++g_pmglobals.thrcnt >= g_pmcount[index]) + { + /* Yes, recommend the new state and set up for the next + * transition. + */ + + g_pmglobals.thrcnt = 0; + g_pmglobals.recommended = nextstate; + } + } + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: pm_update + * + * Description: + * This internal function is called at the end of a time slice in order to + * update driver activity metrics and recommended states. + * + * Input Parameters: + * accum - The value of the activity accumulator at the end of the time + * slice. + * + * Returned Value: + * None. + * + * Assumptions: + * This function may be called from a driver, perhaps even at the interrupt + * level. It may also be called from the IDLE loop at the lowest possible + * priority level. To reconcile these various conditions, all work is + * performed on the worker thread at a user-selectable priority. This will + * also serialize all of the updates and eliminate any need for additional + * protection. + * + ****************************************************************************/ + +void pm_update(uint16_t accum) +{ + /* The work will be performed on the worker thread */ + + DEBUGASSERT(g_pmglobals.work.worker == NULL); + (void)work_queue(&g_pmglobals.work, pm_worker, (FAR void*)((uintptr_t)accum), 0); +} + +#endif /* CONFIG_PM */ \ No newline at end of file -- cgit v1.2.3