summaryrefslogtreecommitdiff
path: root/nuttx/sched
diff options
context:
space:
mode:
authorpatacongo <patacongo@42af7a65-404d-4744-a932-0658087f49c3>2007-03-21 17:21:26 +0000
committerpatacongo <patacongo@42af7a65-404d-4744-a932-0658087f49c3>2007-03-21 17:21:26 +0000
commit9539b686d25f82b5a9938725c6fe2c2ac862e632 (patch)
tree15bd94438dbb8fd1f655598700f34c79dd3af092 /nuttx/sched
parent7407828071d3d8f8d15f4e99e34114208e97bc33 (diff)
downloadpx4-nuttx-9539b686d25f82b5a9938725c6fe2c2ac862e632.tar.gz
px4-nuttx-9539b686d25f82b5a9938725c6fe2c2ac862e632.tar.bz2
px4-nuttx-9539b686d25f82b5a9938725c6fe2c2ac862e632.zip
Added support for POSIX timers
git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@111 42af7a65-404d-4744-a932-0658087f49c3
Diffstat (limited to 'nuttx/sched')
-rw-r--r--nuttx/sched/Makefile12
-rw-r--r--nuttx/sched/clock_abstime2ticks.c129
-rw-r--r--nuttx/sched/clock_internal.h32
-rw-r--r--nuttx/sched/clock_ticks2time.c94
-rw-r--r--nuttx/sched/clock_time2ticks.c100
-rw-r--r--nuttx/sched/os_start.c18
-rw-r--r--nuttx/sched/pthread_condtimedwait.c70
-rw-r--r--nuttx/sched/sched_releasetcb.c17
-rw-r--r--nuttx/sched/timer_create.c110
-rw-r--r--nuttx/sched/timer_delete.c60
-rw-r--r--nuttx/sched/timer_getoverrun.c4
-rw-r--r--nuttx/sched/timer_gettime.c27
-rw-r--r--nuttx/sched/timer_initialize.c163
-rw-r--r--nuttx/sched/timer_internal.h96
-rw-r--r--nuttx/sched/timer_settime.c233
-rw-r--r--nuttx/sched/wd_gettime.c116
16 files changed, 1177 insertions, 104 deletions
diff --git a/nuttx/sched/Makefile b/nuttx/sched/Makefile
index 4679a846d..727c4d9c4 100644
--- a/nuttx/sched/Makefile
+++ b/nuttx/sched/Makefile
@@ -55,10 +55,11 @@ SCHED_SRCS = sched_setparam.c sched_getparam.c \
sched_yield.c sched_rrgetinterval.c sched_foreach.c \
sched_getprioritymax.c sched_getprioritymin.c \
sched_lock.c sched_unlock.c sched_lockcount.c
-WDOG_SRCS = wd_initialize.c wd_create.c wd_start.c wd_cancel.c wd_delete.c
+WDOG_SRCS = wd_initialize.c wd_create.c wd_start.c wd_cancel.c wd_delete.c \
+ wd_gettime.c
TIME_SRCS = sched_processtimer.c sleep.c usleep.c
-CLOCK_SRCS = clock_initialize.c mktime.c gmtime_r.c clock_settime.c \
- clock_gettime.c clock_getres.c
+CLOCK_SRCS = clock_initialize.c mktime.c gmtime_r.c clock_settime.c clock_gettime.c \
+ clock_getres.c clock_time2ticks.c clock_abstime2ticks.c clock_ticks2time.c
SIGNAL_SRCS = sig_initialize.c \
sig_action.c sig_procmask.c sig_pending.c sig_suspend.c \
sig_kill.c sig_queue.c sig_waitinfo.c sig_timedwait.c \
@@ -99,8 +100,9 @@ SEM_SRCS = sem_initialize.c sem_init.c sem_destroy.c\
sem_open.c sem_close.c sem_unlink.c \
sem_wait.c sem_trywait.c sem_post.c sem_getvalue.c \
sem_waitirq.c sem_findnamed.c
-ifneq ($(CONFIG_DISABLE_POSIX_TIMERSA),y)
-TIMERS_SRCS = timer_create.c timer_delete.c timer_getoverrun.c timer_gettime.c timer_settime.c
+ifneq ($(CONFIG_DISABLE_POSIX_TIMERS),y)
+TIMER_SRCS = timer_initialize.c timer_create.c timer_delete.c timer_getoverrun.c \
+ timer_gettime.c timer_settime.c
endif
IRQ_SRCS = irq_initialize.c irq_attach.c irq_dispatch.c irq_unexpectedisr.c
diff --git a/nuttx/sched/clock_abstime2ticks.c b/nuttx/sched/clock_abstime2ticks.c
new file mode 100644
index 000000000..359bd2165
--- /dev/null
+++ b/nuttx/sched/clock_abstime2ticks.c
@@ -0,0 +1,129 @@
+/********************************************************************************
+ * clock_abstime2ticks.c
+ *
+ * Copyright (C) 2007 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * 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 Gregory Nutt 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 <nuttx/config.h>
+#include <sys/types.h>
+#include <time.h>
+#include <errno.h>
+#include <debug.h>
+#include "clock_internal.h"
+
+/********************************************************************************
+ * Definitions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Private Type Declarations
+ ********************************************************************************/
+
+/********************************************************************************
+ * Global Variables
+ ********************************************************************************/
+
+/********************************************************************************
+ * Private Variables
+ ********************************************************************************/
+
+/********************************************************************************
+ * Private Functions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Public Functions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Function: clock_abstime2ticks
+ *
+ * Description:
+ * Convert an absolute timespec delay to system timer ticks.
+ *
+ * Parameters:
+ * clockid - The timing source to use in the conversion
+ * reltime - Convert this absolue time to system clock ticks.
+ * ticks - Return the converted number of ticks here.
+ *
+ * Return Value:
+ * OK on success; A non-zero error number on failure;
+ *
+ * Assumptions:
+ * Interrupts should be disabled so that the time is not changing during the
+ * calculation
+ *
+ ********************************************************************************/
+
+extern int clock_abstime2ticks(clockid_t clockid, const struct timespec *abstime,
+ int *ticks)
+{
+ struct timespec currtime;
+ struct timespec reltime;
+ sint32 relusec;
+ int ret;
+
+ /* Convert the timespec to clock ticks. NOTE: Here we use
+ * internal knowledge that CLOCK_REALTIME is defined to be zero!
+ */
+
+ ret = clock_gettime(clockid, &currtime);
+ if (ret)
+ {
+ return EINVAL;
+ }
+
+ /* The relative time to wait is the absolute time minus the
+ * current time.
+ */
+
+ reltime.tv_nsec = (abstime->tv_nsec - currtime.tv_nsec);
+ reltime.tv_sec = (abstime->tv_sec - currtime.tv_sec);
+
+ /* Check if we were supposed to borrow from the seconds to
+ * borrow from the seconds
+ */
+
+ if (reltime.tv_nsec < 0)
+ {
+ reltime.tv_nsec += NSEC_PER_SEC;
+ reltime.tv_sec -= 1;
+ }
+
+ /* Convert this relative time into microseconds.*/
+
+ return clock_time2ticks(&reltime, ticks);
+}
diff --git a/nuttx/sched/clock_internal.h b/nuttx/sched/clock_internal.h
index e97c468b7..85cb13f06 100644
--- a/nuttx/sched/clock_internal.h
+++ b/nuttx/sched/clock_internal.h
@@ -1,4 +1,4 @@
-/************************************************************
+/********************************************************************************
* clock_internal.h
*
* Copyright (C) 2007 Gregory Nutt. All rights reserved.
@@ -31,21 +31,21 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
- ************************************************************/
+ ********************************************************************************/
#ifndef __CLOCK_INTERNAL_H
#define __CLOCK_INTERNAL_H
-/************************************************************
+/********************************************************************************
* Included Files
- ************************************************************/
+ ********************************************************************************/
#include <sys/types.h>
#include <nuttx/compiler.h>
-/************************************************************
+/********************************************************************************
* Definitions
- ************************************************************/
+ ********************************************************************************/
/* Timing constants */
@@ -59,7 +59,7 @@
#define MSEC_PER_TICK 10
#define USEC_PER_TICK (MSEC_PER_TICK * USEC_PER_MSEC)
#define NSEC_PER_TICK (MSEC_PER_TICK * NSEC_PER_MSEC)
-#define TICK_PER_SEC (MSEC_PER_SEC / MSEC_PER_TICK)
+#define TICK_PER_SEC (MSEC_PER_SEC / MSEC_PER_TICK)
#define MSEC2TICK(msec) (((msec)+(MSEC_PER_TICK/2))/MSEC_PER_TICK)
#define USEC2TICK(usec) (((usec)+(USEC_PER_TICK/2))/USEC_PER_TICK)
@@ -73,25 +73,29 @@
# define GREG_DAY 15
#endif /* CONFIG_JULIAN_TIME */
-/************************************************************
+/********************************************************************************
* Public Type Definitions
- ************************************************************/
+ ********************************************************************************/
-/************************************************************
+/********************************************************************************
* Global Variables
- ************************************************************/
+ ********************************************************************************/
extern volatile uint32 g_system_timer;
extern struct timespec g_basetime;
-extern uint32 g_tickbias;
+extern uint32 g_tickbias;
-/************************************************************
+/********************************************************************************
* Public Function Prototypes
- ************************************************************/
+ ********************************************************************************/
extern void weak_function clock_initialize(void);
extern void weak_function clock_timer(void);
extern time_t clock_calendar2utc(int year, int month, int day);
+extern int clock_abstime2ticks(clockid_t clockid, const struct timespec *abstime,
+ int *ticks);
+extern int clock_time2ticks(const struct timespec *reltime, int *ticks);
+extern int clock_ticks2time(int ticks, struct timespec *reltime);
#endif /* __CLOCK_INTERNAL_H */
diff --git a/nuttx/sched/clock_ticks2time.c b/nuttx/sched/clock_ticks2time.c
new file mode 100644
index 000000000..a7ef5dc9b
--- /dev/null
+++ b/nuttx/sched/clock_ticks2time.c
@@ -0,0 +1,94 @@
+/********************************************************************************
+ * clock_ticks2time.c
+ *
+ * Copyright (C) 2007 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * 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 Gregory Nutt 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 <nuttx/config.h>
+#include <sys/types.h>
+#include <time.h>
+#include "clock_internal.h"
+
+/********************************************************************************
+ * Definitions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Private Type Declarations
+ ********************************************************************************/
+
+/********************************************************************************
+ * Global Variables
+ ********************************************************************************/
+
+/********************************************************************************
+ * Private Variables
+ ********************************************************************************/
+
+/********************************************************************************
+ * Private Functions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Public Functions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Function: clock_ticks2time
+ *
+ * Description:
+ * Convert the system time tick value to a relative time.
+ *
+ * Parameters:
+ * ticks - The number of system time ticks to convert.
+ * reltime - Return the converted system time here.
+ *
+ * Return Value:
+ * Always returns OK
+ *
+ * Assumptions:
+ *
+ ********************************************************************************/
+
+int clock_ticks2time(int ticks, struct timespec *reltime)
+{
+ int remainder;
+
+ reltime->tv_sec = ticks / TICK_PER_SEC;
+ remainder = ticks - TICK_PER_SEC * reltime->tv_sec;
+ reltime->tv_nsec = remainder * NSEC_PER_TICK;
+ return OK;
+}
diff --git a/nuttx/sched/clock_time2ticks.c b/nuttx/sched/clock_time2ticks.c
new file mode 100644
index 000000000..91c5e976c
--- /dev/null
+++ b/nuttx/sched/clock_time2ticks.c
@@ -0,0 +1,100 @@
+/********************************************************************************
+ * clock_time2ticks.c
+ *
+ * Copyright (C) 2007 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * 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 Gregory Nutt 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 <nuttx/config.h>
+#include <sys/types.h>
+#include <time.h>
+#include "clock_internal.h"
+
+/********************************************************************************
+ * Definitions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Private Type Declarations
+ ********************************************************************************/
+
+/********************************************************************************
+ * Global Variables
+ ********************************************************************************/
+
+/********************************************************************************
+ * Private Variables
+ ********************************************************************************/
+
+/********************************************************************************
+ * Private Functions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Public Functions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Function: clock_time2ticks
+ *
+ * Description:
+ * Convert a timespec delay to system timer ticks. This function is suitable
+ * for calculating relative time delays and does not depend on the other
+ * clock_* logic.
+ *
+ * Parameters:
+ * reltime - Convert this relative time to system clock ticks.
+ * ticks - Return the converted number of ticks here.
+ *
+ * Return Value:
+ * Always returns OK
+ *
+ * Assumptions:
+ *
+ ********************************************************************************/
+
+int clock_time2ticks(const struct timespec *reltime, int *ticks)
+{
+ sint32 relusec;
+
+ /* Convert the relative time into microseconds.*/
+
+ relusec = reltime->tv_sec * USEC_PER_SEC + reltime->tv_nsec / NSEC_PER_USEC;
+
+ /* Convert microseconds to clock ticks */
+
+ *ticks = relusec / USEC_PER_TICK;
+ return OK;
+}
diff --git a/nuttx/sched/os_start.c b/nuttx/sched/os_start.c
index 740cffa10..4cb128d3c 100644
--- a/nuttx/sched/os_start.c
+++ b/nuttx/sched/os_start.c
@@ -57,6 +57,7 @@
# include "pthread_internal.h"
#endif
#include "clock_internal.h"
+#include "timer_internal.h"
#include "irq_internal.h"
/************************************************************
@@ -304,6 +305,15 @@ void os_start(void)
user_initialize();
}
+ /* Initialize the watchdog facility (if included in the link) */
+
+#ifdef CONFIG_HAVE_WEAKFUNCTIONS
+ if (wd_initialize != NULL)
+#endif
+ {
+ wd_initialize();
+ }
+
/* Initialize the POSIX timer facility (if included in the link) */
#ifndef CONFIG_DISABLE_CLOCK
@@ -315,14 +325,14 @@ void os_start(void)
}
#endif
- /* Initialize the watchdog facility (if included in the link) */
-
+#ifndef CONFIG_DISABLE_POSIX_TIMERS
#ifdef CONFIG_HAVE_WEAKFUNCTIONS
- if (wd_initialize != NULL)
+ if (timer_initialize != NULL)
#endif
{
- wd_initialize();
+ timer_initialize();
}
+#endif
/* Initialize the signal facility (if in link) */
diff --git a/nuttx/sched/pthread_condtimedwait.c b/nuttx/sched/pthread_condtimedwait.c
index 6cc6c34eb..dd8bfaffb 100644
--- a/nuttx/sched/pthread_condtimedwait.c
+++ b/nuttx/sched/pthread_condtimedwait.c
@@ -54,8 +54,6 @@
* Definitions
************************************************************/
-#define ECHO_COND_WAIT_SIGNO 3
-
/************************************************************
* Private Type Declarations
************************************************************/
@@ -106,70 +104,6 @@ static void pthread_condtimedout(int argc, uint32 pid, uint32 signo, ...)
}
/************************************************************
- * Function: pthread_timeoutticks
- *
- * Description:
- * Convert a timespec delay to system timer ticks.
- *
- * Parameters:
- * abstime - wait until this absolute time
- *
- * Return Value:
- * The relative number of ticks to wait (or ERROR on
- * failure;
- *
- * Assumptions:
- * Interrupts should be disabled so that the time is
- * not changing during the calculation
- *
- ************************************************************/
-
-int pthread_timeouticks(const struct timespec *abstime, int *ticks)
-{
- struct timespec currtime;
- struct timespec reltime;
- sint32 relusec;
- int ret;
-
- /* Convert the timespec to clock ticks. NOTE: Here we use
- * internal knowledge that CLOCK_REALTIME is defined to be zero!
- */
-
- ret = clock_gettime(0, &currtime);
- if (ret)
- {
- return EINVAL;
- }
-
- /* The relative time to wait is the absolute time minus the
- * current time.
- */
-
- reltime.tv_nsec = (abstime->tv_nsec - currtime.tv_nsec);
- reltime.tv_sec = (abstime->tv_sec - currtime.tv_sec);
-
- /* Check if we were supposed to borrow from the seconds to
- * borrow from the seconds
- */
-
- if (reltime.tv_nsec < 0)
- {
- reltime.tv_nsec += NSEC_PER_SEC;
- reltime.tv_sec -= 1;
- }
-
- /* Convert this relative time into microseconds.*/
-
- relusec = reltime.tv_sec * USEC_PER_SEC +
- reltime.tv_nsec / NSEC_PER_USEC;
-
- /* Convert microseconds to clock ticks */
-
- *ticks = relusec / USEC_PER_TICK;
- return OK;
-}
-
-/************************************************************
* Public Functions
************************************************************/
@@ -255,7 +189,7 @@ int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex,
* here so that this time stays valid until the wait begins.
*/
- ret = pthread_timeouticks(abstime, &ticks);
+ ret = clock_abstime2ticks(CLOCK_REALTIME, abstime, &ticks);
if (ret)
{
/* Restore interrupts (pre-emption will be enabled when
@@ -299,7 +233,7 @@ int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex,
/* Start the watchdog */
wd_start(wdog, ticks, (wdentry_t)pthread_condtimedout,
- 2, (uint32)mypid, (uint32)ECHO_COND_WAIT_SIGNO);
+ 2, (uint32)mypid, (uint32)SIGCONDTIMEDOUT);
/* Take the condition semaphore. Do not restore interrupts
* until we return from the wait. This is necessary to
diff --git a/nuttx/sched/sched_releasetcb.c b/nuttx/sched/sched_releasetcb.c
index e37e2bab3..5d3c8fd90 100644
--- a/nuttx/sched/sched_releasetcb.c
+++ b/nuttx/sched/sched_releasetcb.c
@@ -45,6 +45,7 @@
#include <errno.h>
#include <nuttx/arch.h>
#include "os_internal.h"
+#include "timer_internal.h"
/************************************************************
* Private Functions
@@ -87,6 +88,7 @@ static void sched_releasepid(pid_t pid)
* OK on success; ERROR on failure
*
* Assumptions:
+ * Interrupts are disabled.
*
************************************************************/
@@ -97,6 +99,21 @@ int sched_releasetcb(FAR _TCB *tcb)
if (tcb)
{
+ /* Relase any timers that the task might hold. We do this
+ * before release the PID because it may still be trying to
+ * deliver signals (although interrupts are should be
+ * disabled here).
+ */
+
+#ifndef CONFIG_DISABLE_POSIX_TIMERS
+#ifdef CONFIG_HAVE_WEAKFUNCTIONS
+ if (timer_deleteall != NULL)
+#endif
+ {
+ timer_deleteall(tcb->pid);
+ }
+#endif
+
/* Release the task's process ID if one was assigned. PID
* zero is reserved for the IDLE task. The TCB of the IDLE
* task is never release so a value of zero simply means that
diff --git a/nuttx/sched/timer_create.c b/nuttx/sched/timer_create.c
index b86d38d6d..40a660f50 100644
--- a/nuttx/sched/timer_create.c
+++ b/nuttx/sched/timer_create.c
@@ -38,10 +38,14 @@
********************************************************************************/
#include <nuttx/config.h>
+#include <stdlib.h>
+#include <unistd.h>
#include <time.h>
+#include <wdog.h>
#include <errno.h>
+#include "timer_internal.h"
-#ifdef CONFIG_POSIX_TIMERS
+#ifndef CONFIG_DISABLE_POSIX_TIMERS
/********************************************************************************
* Definitions
@@ -60,6 +64,52 @@
********************************************************************************/
/********************************************************************************
+ * Function: timer_allocate
+ *
+ * Description:
+ * Allocate one POSIX timer and place it into the allocated timer list.
+ *
+ ********************************************************************************/
+
+static struct posix_timer_s *timer_allocate(void)
+{
+ struct posix_timer_s *ret;
+ irqstate_t flags;
+
+ /* Try to get a preallocated timer from the free list */
+
+#if CONFIG_PREALLOC_TIMERS > 0
+ flags = irqsave();
+ ret = (struct posix_timer_s*)sq_remfirst((sq_queue_t*)&g_freetimers);
+ irqrestore(flags);
+
+ /* Did we get one? */
+
+ if (!ret)
+#endif
+ {
+ /* Allocate a new timer from the heap */
+
+ ret = (struct posix_timer_s*)malloc(sizeof(struct posix_timer_s));
+ if (ret)
+ {
+ ret->pt_flags = 0;
+ }
+ }
+
+ /* If we have a timer, then put it into the allocated timer list */
+
+ if (ret)
+ {
+ flags = irqsave();
+ sq_addlast((sq_entry_t*)ret, (sq_queue_t*)&g_alloctimers);
+ irqrestore(flags);
+ }
+
+ return ret;
+}
+
+/********************************************************************************
* Public Functions
********************************************************************************/
@@ -113,8 +163,60 @@
int timer_create(clockid_t clockid, FAR struct sigevent *evp, FAR timer_t *timerid)
{
-#warning "Not Implemented"
- return ENOTSUP;
+ struct posix_timer_s *ret;
+ WDOG_ID wdog;
+
+ /* Sanity checks. Also, we support only CLOCK_REALTIME */
+
+ if (!timerid || clockid != CLOCK_REALTIME)
+ {
+ *get_errno_ptr() = EINVAL;
+ return ERROR;
+ }
+
+ /* Allocate a watchdog to provide the underling CLOCK_REALTIME timer */
+
+ wdog = wd_create();
+ if (!wdog)
+ {
+ *get_errno_ptr() = EAGAIN;
+ return ERROR;
+ }
+
+ /* Allocate a timer instance to contain the watchdog */
+
+ ret = timer_allocate();
+ if (!ret)
+ {
+ *get_errno_ptr() = EAGAIN;
+ return ERROR;
+ }
+
+ /* Initialize the timer instance */
+
+ ret->pt_owner = getpid();
+ ret->pt_delay = 0;
+ ret->pt_wdog = wdog;
+
+ if (evp)
+ {
+ ret->pt_signo = evp->sigev_signo;
+#ifdef CONFIG_CAN_PASS_STRUCTS
+ ret->pt_value = evp->sigev_value;
+#else
+ ret->pt_value.sival_ptr = evp->sigev_value.sigval_ptr;
+#endif
+ }
+ else
+ {
+ ret->pt_signo = SIGALRM;
+ ret->pt_value.sival_ptr = ret;
+ }
+
+ /* Return the timer */
+
+ *timerid = ret;
+ return OK;
}
-#endif /* CONFIG_POSIX_TIMERS */
+#endif /* CONFIG_DISABLE_POSIX_TIMERS */
diff --git a/nuttx/sched/timer_delete.c b/nuttx/sched/timer_delete.c
index 4dc97a9bd..aed19f76a 100644
--- a/nuttx/sched/timer_delete.c
+++ b/nuttx/sched/timer_delete.c
@@ -39,7 +39,10 @@
#include <nuttx/config.h>
#include <time.h>
+#include <queue.h>
#include <errno.h>
+#include <nuttx/kmalloc.h>
+#include "timer_internal.h"
#ifndef CONFIG_DISABLE_POSIX_TIMERS
@@ -60,6 +63,43 @@
********************************************************************************/
/********************************************************************************
+ * Function: timer_free
+ *
+ * Description:
+ * Remove the timer from the allocated timer list and free it or return it to
+ * the free list (depending on whether or not the timer is one of the
+ * preallocated timers)
+ *
+ ********************************************************************************/
+
+static void timer_free(struct posix_timer_s *timer)
+{
+ irqstate_t flags;
+
+ /* Remove the timer from the allocated list */
+
+ flags = irqsave();
+ sq_rem((FAR sq_entry_t*)timer, (sq_queue_t*)&g_alloctimers);
+
+ /* Return it to the free list if it is one of the preallocated timers */
+
+#if CONFIG_PREALLOC_TIMERS > 0
+ if ((timer->pt_flags & PT_FLAGS_PREALLOCATED) != 0)
+ {
+ sq_addlast((FAR sq_entry_t*)&timer, (FAR sq_queue_t*)&g_freetimers);
+ irqrestore(flags);
+ }
+ else
+#endif
+ {
+ /* Otherwise, return it to the heap */
+
+ irqrestore(flags);
+ sched_free(timer);
+ }
+}
+
+/********************************************************************************
* Public Functions
********************************************************************************/
@@ -88,8 +128,24 @@
int timer_delete(timer_t timerid)
{
-#warning "Not Implemented"
- return ENOTSUP;
+ FAR struct posix_timer_s *timer = (FAR struct posix_timer_s *)timerid;
+
+ /* Some sanity checks */
+
+ if (!timer)
+ {
+ *get_errno_ptr() = EINVAL;
+ return ERROR;
+ }
+
+ /* Disarm the timer */
+
+ (void)wd_cancel(timer->pt_wdog);
+
+ /* Release the timer structure */
+
+ timer_free(timer);
+ return OK;
}
#endif /* CONFIG_DISABLE_POSIX_TIMERS */
diff --git a/nuttx/sched/timer_getoverrun.c b/nuttx/sched/timer_getoverrun.c
index 8dbd8da62..9b23136da 100644
--- a/nuttx/sched/timer_getoverrun.c
+++ b/nuttx/sched/timer_getoverrun.c
@@ -40,6 +40,7 @@
#include <nuttx/config.h>
#include <time.h>
#include <errno.h>
+#include "timer_internal.h"
#ifndef CONFIG_DISABLE_POSIX_TIMERS
@@ -102,7 +103,8 @@
int timer_getoverrun(timer_t timerid)
{
#warning "Not Implemented"
- return ENOTSUP;
+ *get_errno_ptr() = ENOSYS;
+ return ERROR;
}
#endif /* CONFIG_DISABLE_POSIX_TIMERS */
diff --git a/nuttx/sched/timer_gettime.c b/nuttx/sched/timer_gettime.c
index 948c6043c..91a5a60d0 100644
--- a/nuttx/sched/timer_gettime.c
+++ b/nuttx/sched/timer_gettime.c
@@ -40,6 +40,8 @@
#include <nuttx/config.h>
#include <time.h>
#include <errno.h>
+#include "clock_internal.h"
+#include "timer_internal.h"
#ifndef CONFIG_DISABLE_POSIX_TIMERS
@@ -87,14 +89,33 @@
* EINVAL - The timerid argument does not correspond to an ID returned by
* timer_create() but not yet deleted by timer_delete().
*
- * Assumptions:
+ * Assumptions/Limitations:
+ * Due to the asynchronous operation of this function, the time reported
+ * by this function could be significantly more than that actual time
+ * remaining on the timer at any time.
*
********************************************************************************/
int timer_gettime(timer_t timerid, FAR struct itimerspec *value)
{
-#warning "Not Implemented"
- return ENOTSUP;
+ FAR struct posix_timer_s *timer = (FAR struct posix_timer_s *)timerid;
+ int ticks;
+
+ if (!timer || !value)
+ {
+ *get_errno_ptr() = EINVAL;
+ return ERROR;
+ }
+
+ /* Get the number of ticks before the underlying watchdog expires */
+
+ ticks = wd_gettime(timer->pt_wdog);
+
+ /* Convert that to a struct timespec and return it */
+
+ (void)clock_ticks2time(ticks, &value->it_value);
+ (void)clock_ticks2time(timer->pt_delay, &value->it_interval);
+ return OK;
}
#endif /* CONFIG_DISABLE_POSIX_TIMERS */
diff --git a/nuttx/sched/timer_initialize.c b/nuttx/sched/timer_initialize.c
new file mode 100644
index 000000000..a32189e89
--- /dev/null
+++ b/nuttx/sched/timer_initialize.c
@@ -0,0 +1,163 @@
+/********************************************************************************
+ * timer_initialize.c
+ *
+ * Copyright (C) 2007 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * 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 Gregory Nutt 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 <nuttx/config.h>
+#include <time.h>
+#include <queue.h>
+#include <errno.h>
+#include <nuttx/compiler.h>
+#include "timer_internal.h"
+
+#ifndef CONFIG_DISABLE_POSIX_TIMERS
+
+/********************************************************************************
+ * Definitions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Private Data
+ ********************************************************************************/
+
+/* These are the preallocated times */
+
+#if CONFIG_PREALLOC_TIMERS > 0
+static struct posix_timer_s g_prealloctimers[CONFIG_PREALLOC_TIMERS];
+#endif
+
+/********************************************************************************
+ * Public Data
+ ********************************************************************************/
+
+/* This is a list of free, preallocated timer structures */
+
+#if CONFIG_PREALLOC_TIMERS > 0
+volatile sq_queue_t g_freetimers;
+#endif
+
+/* This is a list of instantiated timer structures -- active and inactive. The
+ * timers are place on this list by timer_create() and removed from the list by
+ * timer_delete() or when the owning thread exits.
+ */
+
+volatile sq_queue_t g_alloctimers;
+
+/********************************************************************************
+ * Private Functions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Public Functions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Function: timer_initialize
+ *
+ * Description:
+ * Boot up configuration of the POSIX timer facility.
+ *
+ * Parameters:
+ * None
+ *
+ * Return Value:
+ * None
+ *
+ * Assumptions:
+ *
+ ********************************************************************************/
+
+void weak_function timer_initialize(void)
+{
+ int i;
+
+ /* Place all of the pre-allocated timers into the free timer list */
+
+#if CONFIG_PREALLOC_TIMERS > 0
+ sq_init((sq_queue_t*)&g_freetimers);
+
+ for (i = 0; i < CONFIG_PREALLOC_TIMERS; i++)
+ {
+ g_prealloctimers[i].pt_flags = PT_FLAGS_PREALLOCATED;
+ sq_addlast((FAR sq_entry_t*)&g_prealloctimers[i], (FAR sq_queue_t*)&g_freetimers);
+ }
+#endif
+
+ /* Initialize the list of allocated timers */
+
+ sq_init((sq_queue_t*)&g_alloctimers);
+}
+
+/********************************************************************************
+ * Function: timer_deleteall
+ *
+ * Description:
+ * This function is called whenever a thread exits. Any timers owned by that
+ * thread are deleted as though called by timer_delete().
+ *
+ * It is provided in this file so that it can be weakly defined but also,
+ * like timer_intitialize(), be brought into the link whenever the timer
+ * resources are referenced.
+ *
+ * Parameters:
+ * pid - the task ID of the thread that exitted
+ *
+ * Return Value:
+ * None
+ *
+ * Assumptions:
+ *
+ ********************************************************************************/
+
+void weak_function timer_deleteall(pid_t pid)
+{
+ FAR struct posix_timer_s *timer;
+ FAR struct posix_timer_s *next;
+ irqstate_t flags;
+
+ flags = irqsave();
+ for (timer = g_alloctimers.head; timer; timer = next)
+ {
+ next = timer->flink;
+ if (timer->pt_owner = pid)
+ {
+ timer_delete((timer_t)timer);
+ }
+ }
+}
+
+#endif /* CONFIG_DISABLE_POSIX_TIMERS */
diff --git a/nuttx/sched/timer_internal.h b/nuttx/sched/timer_internal.h
new file mode 100644
index 000000000..1c1e8ec54
--- /dev/null
+++ b/nuttx/sched/timer_internal.h
@@ -0,0 +1,96 @@
+/********************************************************************************
+ * timer_internal.h
+ *
+ * Copyright (C) 2007 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * 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 Gregory Nutt 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 __TIMER_INTERNAL_H
+#define __TIMER_INTERNAL_H
+
+/********************************************************************************
+ * Included Files
+ ********************************************************************************/
+
+#include <nuttx/config.h>
+#include <sys/types.h>
+#include <wdog.h>
+#include <nuttx/compiler.h>
+
+/********************************************************************************
+ * Definitions
+ ********************************************************************************/
+
+#define PT_FLAGS_PREALLOCATED 0x01
+
+/********************************************************************************
+ * Public Types
+ ********************************************************************************/
+
+/* This structure represents one POSIX timer */
+
+struct posix_timer_s
+{
+ FAR struct posix_timer_s *flink;
+
+ ubyte pt_flags; /* See PT_FLAGS_* definitions */
+ ubyte pt_signo; /* Notification signal */
+ pid_t pt_owner; /* Creator of timer */
+ int pt_delay; /* If non-zero, used to reset repetitive timers */
+ WDOG_ID pt_wdog; /* The watchdog that provides the timing */
+ union sigval pt_value; /* Data passed with notification */
+};
+
+/********************************************************************************
+ * Public Data
+ ********************************************************************************/
+
+/* This is a list of free, preallocated timer structures */
+
+#if CONFIG_PREALLOC_TIMERS > 0
+extern volatile sq_queue_t g_freetimers;
+#endif
+
+/* This is a list of instantiated timer structures -- active and inactive. The
+ * timers are place on this list by timer_create() and removed from the list by
+ * timer_delete() or when the owning thread exits.
+ */
+
+extern volatile sq_queue_t g_alloctimers;
+
+/********************************************************************************
+ * Public Function Prototypes
+ ********************************************************************************/
+
+extern void weak_function timer_initialize(void);
+extern void weak_function timer_deleteall(pid_t pid);
+
+#endif /* __TIMER_INTERNAL_H */
diff --git a/nuttx/sched/timer_settime.c b/nuttx/sched/timer_settime.c
index 8df4c7e32..fda730f9b 100644
--- a/nuttx/sched/timer_settime.c
+++ b/nuttx/sched/timer_settime.c
@@ -40,6 +40,10 @@
#include <nuttx/config.h>
#include <time.h>
#include <errno.h>
+#include "os_internal.h"
+#include "clock_internal.h"
+#include "sig_internal.h"
+#include "timer_internal.h"
#ifndef CONFIG_DISABLE_POSIX_TIMERS
@@ -56,10 +60,145 @@
********************************************************************************/
/********************************************************************************
+ * Private Function Prototypes
+ ********************************************************************************/
+
+static void inline timer_sigqueue(FAR struct posix_timer_s *timer);
+static void inline timer_restart(FAR struct posix_timer_s *timer, uint32 itimer);
+static void timer_timeout(int argc, uint32 itimer, ...);
+
+/********************************************************************************
* Private Functions
********************************************************************************/
/********************************************************************************
+ * Function: timer_sigqueue
+ *
+ * Description:
+ * This function basically reimplements sigqueue() so that the si_code can
+ * be correctly set to SI_TIMER
+ *
+ * Parameters:
+ * timer - A reference to the POSIX timer that just timed out
+ *
+ * Return Value:
+ * None
+ *
+ * Assumptions:
+ * This function executes in the context of the watchod timer interrupt.
+ *
+ ********************************************************************************/
+
+static void inline timer_sigqueue(FAR struct posix_timer_s *timer)
+{
+ FAR _TCB *tcb;
+
+ /* Get the TCB of the receiving task */
+
+ tcb = sched_gettcb(timer->pt_owner);
+ if (tcb)
+ {
+ siginfo_t info;
+
+ /* Create the siginfo structure */
+
+ info.si_signo = timer->pt_signo;
+ info.si_code = SI_TIMER;
+#ifndef CONFIG_CAN_PASS_STRUCTS
+ info.si_value = timer->pt_value;
+#else
+ info.si_value.sival_ptr = timer->pt_value.sival_ptr;
+#endif
+
+ /* Send the signal */
+
+ (void)sig_received(tcb, &info);
+ }
+}
+
+/********************************************************************************
+ * Function: timer_restart
+ *
+ * Description:
+ * If a periodic timer has been selected, then restart the watchdog.
+ *
+ * Parameters:
+ * timer - A reference to the POSIX timer that just timed out
+ *
+ * Return Value:
+ * None
+ *
+ * Assumptions:
+ * This function executes in the context of the watchod timer interrupt.
+ *
+ ********************************************************************************/
+
+static void inline timer_restart(FAR struct posix_timer_s *timer, uint32 itimer)
+{
+ /* If this is a repetitive timer, then restart the watchdog */
+
+ if (timer->pt_delay)
+ {
+ (void)wd_start(timer->pt_wdog, timer->pt_delay, timer_timeout, 1, itimer);
+ }
+}
+
+/********************************************************************************
+ * Function: timer_timeout
+ *
+ * Description:
+ * This function is called if the timeout elapses before
+ * the condition is signaled.
+ *
+ * Parameters:
+ * argc - the number of arguments (should be 1)
+ * itimer - A reference to the POSIX timer that just timed out
+ * signo - The signal to use to wake up the task
+ *
+ * Return Value:
+ * None
+ *
+ * Assumptions:
+ * This function executes in the context of the watchod timer interrupt.
+ *
+ ********************************************************************************/
+
+static void timer_timeout(int argc, uint32 itimer, ...)
+{
+#ifndef CONFIG_CAN_PASS_STRUCTS
+ /* On many small machines, pointers are encoded and cannot be simply cast from
+ * uint32 to _TCB*. The following union works around this (see wdogparm_t).
+ */
+
+ union
+ {
+ FAR struct posix_timer_s *timer;
+ uint32 itimer;
+ } u;
+
+ u.itimer = itimer;
+
+ /* Send the specified signal to the specified task. */
+
+ timer_sigqueue(u.timer);
+
+ /* If this is a repetitive timer, the restart the watchdog */
+
+ timer_restart(u.timer, itimer);
+#else
+ FAR struct posix_timer_s *timer = (FAR struct posix_timer_s *)itimer;
+
+ /* Send the specified signal to the specified task. */
+
+ timer_sigqueue(timer);
+
+ /* If this is a repetitive timer, the restart the watchdog */
+
+ timer_restart(timer, itimer);
+#endif
+}
+
+/********************************************************************************
* Public Functions
********************************************************************************/
@@ -109,7 +248,7 @@
* flags - Specifie characteristics of the timer (see above)
* value - Specifies the timer value to set
* ovalue - A location in which to return the time remaining from the previous
- * timer setting.
+ * timer setting. (ignored)
*
* Return Value:
* If the timer_settime() succeeds, a value of 0 (OK) will be returned.
@@ -129,8 +268,96 @@
int timer_settime(timer_t timerid, int flags, FAR const struct itimerspec *value,
FAR struct itimerspec *ovalue)
{
-#warning "Not Implemented"
- return ENOTSUP;
+ FAR struct posix_timer_s *timer = (FAR struct posix_timer_s *)timerid;
+ irqstate_t state;
+ int delay;
+ int ret = OK;
+
+ /* Some sanity checks */
+
+ if (!timer || !value)
+ {
+ *get_errno_ptr() = EINVAL;
+ return ERROR;
+ }
+
+ /* Disarm the timer (in case the timer was already armed when timer_settime()
+ * is called).
+ */
+
+ (void)wd_cancel(timer->pt_wdog);
+
+ /* If the it_value member of value is zero, the timer will not be re-armed */
+
+ if (value->it_value.tv_sec <= 0 && value->it_value.tv_nsec <= 0)
+ {
+ return OK;
+ }
+
+ /* Setup up any repititive timer */
+
+ if (value->it_interval.tv_sec > 0 || value->it_interval.tv_nsec > 0)
+ {
+ (void)clock_time2ticks(&value->it_interval, &timer->pt_delay);
+ }
+ else
+ {
+ timer->pt_delay = 0;
+ }
+
+ /* We need to disable timer interrupts through the following section so
+ * that the system timer is stable.
+ */
+
+ state = irqsave();
+
+ /* Check if abstime is selected */
+
+ if ((flags & TIMER_ABSTIME) != 0)
+ {
+#ifdef CONFIG_DISABLE_CLOCK
+ /* Absolute timing depends upon having access to clock functionality */
+
+ *get_errno_ptr() = ENOSYS;
+ return ERROR;
+#else
+ /* Calculate a delay corresponding to the absolute time in 'value'.
+ * NOTE: We have internal knowledge the clock_abstime2ticks only
+ * returns an error if clockid != CLOCK_REALTIME.
+ */
+
+ (void)clock_abstime2ticks(CLOCK_REALTIME, &value->it_value, &delay);
+#endif
+ }
+ else
+ {
+ /* Calculate a delay assuming that 'value' holds the relative time
+ * to wait. We have internal knowledge that clock_time2ticks always
+ * returns success.
+ */
+
+ (void)clock_time2ticks(&value->it_value, &delay);
+ }
+
+ /* If the time is in the past or now, then set up the next interval
+ * instead.
+ */
+
+ if (delay <= 0)
+ {
+ delay = timer->pt_delay;
+ }
+
+ /* Then start the watchdog */
+
+
+ if (delay > 0)
+ {
+ ret = wd_start(timer->pt_wdog, timer->pt_delay, timer_timeout, 1, (uint32)timer);
+ }
+
+ irqrestore(state);
+ return ret;
}
#endif /* CONFIG_DISABLE_POSIX_TIMERS */
diff --git a/nuttx/sched/wd_gettime.c b/nuttx/sched/wd_gettime.c
new file mode 100644
index 000000000..77d71eb5c
--- /dev/null
+++ b/nuttx/sched/wd_gettime.c
@@ -0,0 +1,116 @@
+/********************************************************************************
+ * wd_gettime.c
+ *
+ * Copyright (C) 2007 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * 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 Gregory Nutt 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 <sys/types.h>
+#include <wdog.h>
+#include "os_internal.h"
+#include "wd_internal.h"
+
+/********************************************************************************
+ * Definitions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Private Types
+ ********************************************************************************/
+
+/********************************************************************************
+ * Global Variables
+ ********************************************************************************/
+
+/********************************************************************************
+ * Private Variables
+ ********************************************************************************/
+
+/********************************************************************************
+ * Private Functions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Public Functions
+ ********************************************************************************/
+
+/********************************************************************************
+ * Function: wd_gettime
+ *
+ * Description:
+ * This function returns the time remaining before the the specified watchdog
+ * expires.
+ *
+ * Parameters:
+ * wdog = watchdog ID
+ *
+ * Return Value:
+ * The time in system ticks remaining until the watchdog time expires. Zero
+ * means either that wdog is not valid or that the wdog has already expired.
+ *
+ * Assumptions:
+ *
+ ********************************************************************************/
+
+int wd_gettime(WDOG_ID wdog)
+{
+ irqstate_t flags;
+
+ /* Verify the wdog */
+
+ flags = irqsave();
+ if (wdog && wdog->active)
+ {
+ /* Traverse the watchdog list accumulating lag times until we find the wdog
+ * that we are looking for
+ */
+
+ wdog_t *curr;
+ int delay = 0;
+
+ for (curr = (wdog_t*)g_wdactivelist.head; curr; curr = curr->next)
+ {
+ delay += curr->lag;
+ if (curr == wdog)
+ {
+ irqrestore(flags);
+ return delay;
+ }
+ }
+ }
+
+ irqrestore(flags);
+ return 0;
+}