fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 08:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Platform cleanup - remove duplicate files

Browse Source 
The POSIX and QURT platforms contain several duplicate files.
These files have been factored out into platforms/posix/work_queue.
The config files have been updated to include the
platforms/posix/work_queue module.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2015-07-28 20:02:39 -07:00
parent 08c50cac5e
commit a96db1580f
38 changed files with 69 additions and 1733 deletions
Download Patch File Download Diff File Expand all files Collapse all files
makefiles/posix/config_posix_sitl.mk
+2 -1
View File
@@ -72,9 +72,10 @@ MODULES += lib/geo_lookup
MODULES += lib/conversion
#
# Linux port
# POSIX port
#
MODULES += platforms/posix/px4_layer
MODULES += platforms/posix/work_queue
MODULES += platforms/posix/drivers/accelsim
MODULES += platforms/posix/drivers/gyrosim
MODULES += platforms/posix/drivers/adcsim
makefiles/qurt/config_qurt_default.mk
+1
View File
@@ -63,6 +63,7 @@ MODULES += lib/conversion
# QuRT port
#
MODULES += platforms/qurt/px4_layer
MODULES += platforms/posix/work_queue
MODULES += platforms/posix/drivers/accelsim
MODULES += platforms/posix/drivers/gyrosim
MODULES += platforms/posix/drivers/adcsim
makefiles/qurt/config_qurt_hello.mk
+1
View File
@@ -58,6 +58,7 @@ MODULES += modules/uORB
# QuRT port
#
MODULES += platforms/qurt/px4_layer
MODULES += platforms/posix/work_queue
#MODULES += platforms/posix/drivers/accelsim
#MODULES += platforms/posix/drivers/gyrosim
#MODULES += platforms/posix/drivers/adcsim
makefiles/qurt/config_qurt_hil.mk
+1
View File
@@ -62,6 +62,7 @@ MODULES += lib/conversion
# QuRT port
#
MODULES += platforms/qurt/px4_layer
MODULES += platforms/posix/work_queue
#MODULES += platforms/posix/drivers/accelsim
#MODULES += platforms/posix/drivers/gyrosim
#MODULES += platforms/posix/drivers/adcsim
makefiles/qurt/config_qurt_muorb_test.mk
+1
View File
@@ -60,6 +60,7 @@ MODULES += lib/conversion
# QuRT port
#
MODULES += platforms/qurt/px4_layer
MODULES += platforms/posix/work_queue
MODULES += platforms/posix/drivers/accelsim
MODULES += platforms/posix/drivers/gyrosim
MODULES += platforms/posix/drivers/adcsim
src/platforms/posix/px4_layer/module.mk
+1 -15
View File
@@ -32,27 +32,13 @@
############################################################################
#
# NuttX / uORB adapter library
# POSIX platform dependent files
#
SRCS = \
px4_posix_impl.cpp \
px4_posix_tasks.cpp \
hrt_thread.c \
hrt_queue.c \
hrt_work_cancel.c \
work_thread.c \
work_lock.c \
work_queue.c \
work_cancel.c \
lib_crc32.c \
drv_hrt.c \
queue.c \
dq_addlast.c \
dq_remfirst.c \
sq_addlast.c \
sq_remfirst.c \
sq_addafter.c \
dq_rem.c
MAXOPTIMIZATION = -Os
src/platforms/posix/px4_layer/work_lock.c
-19
View File
@@ -1,19 +0,0 @@
//#pragma once
#include <semaphore.h>
#include <stdio.h>
#include "work_lock.h"
extern sem_t _work_lock[];
void work_lock(int id)
{
//printf("work_lock %d\n", id);
sem_wait(&_work_lock[id]);
}
void work_unlock(int id)
{
//printf("work_unlock %d\n", id);
sem_post(&_work_lock[id]);
}
src/platforms/posix/px4_layer/work_thread.c
-328
View File
@@ -1,328 +0,0 @@
/****************************************************************************
* libc/wqueue/work_thread.c
*
* Copyright (C) 2009-2013 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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 <px4_config.h>
#include <px4_defines.h>
#include <stdint.h>
#include <stdio.h>
#include <unistd.h>
#include <queue.h>
#include <px4_workqueue.h>
#include <drivers/drv_hrt.h>
#include "work_lock.h"
#ifdef CONFIG_SCHED_WORKQUEUE
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Type Declarations
****************************************************************************/
/****************************************************************************
* Public Variables
****************************************************************************/
/* The state of each work queue. */
struct wqueue_s g_work[NWORKERS];
/****************************************************************************
* Private Variables
****************************************************************************/
sem_t _work_lock[NWORKERS];
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: work_process
*
* Description:
* This is the logic that performs actions placed on any work list.
*
* Input parameters:
* wqueue - Describes the work queue to be processed
*
* Returned Value:
* None
*
****************************************************************************/
static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
{
volatile FAR struct work_s *work;
worker_t worker;
FAR void *arg;
uint64_t elapsed;
uint32_t remaining;
uint32_t next;
/* Then process queued work. We need to keep interrupts disabled while
* we process items in the work list.
*/
next = CONFIG_SCHED_WORKPERIOD;
work_lock(lock_id);
work = (FAR struct work_s *)wqueue->q.head;
while (work)
{
/* Is this work ready? It is ready if there is no delay or if
* the delay has elapsed. qtime is the time that the work was added
* to the work queue. It will always be greater than or equal to
* zero. Therefore a delay of zero will always execute immediately.
*/
elapsed = USEC2TICK(clock_systimer() - work->qtime);
//PX4_INFO("work_process: queue=%p in ticks elapsed=%lu delay=%u time=%u", wqueue, elapsed, work->delay,clock_systimer());
if (elapsed >= work->delay)
{
/* Remove the ready-to-execute work from the list */
(void)dq_rem((struct dq_entry_s *)work, &wqueue->q);
/* Extract the work description from the entry (in case the work
* instance by the re-used after it has been de-queued).
*/
worker = work->worker;
arg = work->arg;
/* Mark the work as no longer being queued */
work->worker = NULL;
/* Do the work. Re-enable interrupts while the work is being
* performed... we don't have any idea how long that will take!
*/
work_unlock(lock_id);
if (!worker) {
PX4_ERR("MESSED UP: worker = 0");
}
else {
worker(arg);
}
/* Now, unfortunately, since we re-enabled interrupts we don't
* know the state of the work list and we will have to start
* back at the head of the list.
*/
work_lock(lock_id);
work = (FAR struct work_s *)wqueue->q.head;
}
else
{
/* This one is not ready.. will it be ready before the next
* scheduled wakeup interval?
*/
/* Here: elapsed < work->delay */
remaining = USEC_PER_TICK*(work->delay - elapsed);
if (remaining < next)
{
/* Yes.. Then schedule to wake up when the work is ready */
next = remaining;
}
/* Then try the next in the list. */
work = (FAR struct work_s *)work->dq.flink;
}
}
/* Wait awhile to check the work list. We will wait here until either
* the time elapses or until we are awakened by a signal.
*/
work_unlock(lock_id);
usleep(next);
}
/****************************************************************************
* Public Functions
****************************************************************************/
void work_queues_init(void)
{
sem_init(&_work_lock[HPWORK], 0, 1);
sem_init(&_work_lock[LPWORK], 0, 1);
#ifdef CONFIG_SCHED_USRWORK
sem_init(&_work_lock[USRWORK], 0, 1);
#endif
// Create high priority worker thread
g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX-1,
2000,
work_hpthread,
(char* const*)NULL);
// Create low priority worker thread
g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low",
SCHED_DEFAULT,
SCHED_PRIORITY_MIN,
2000,
work_lpthread,
(char* const*)NULL);
}
/****************************************************************************
* Name: work_hpthread, work_lpthread, and work_usrthread
*
* Description:
* These are the worker threads that performs actions placed on the work
* lists.
*
* work_hpthread and work_lpthread: These are the kernel mode work queues
* (also build in the flat build). One of these threads also performs
* periodic garbage collection (that is otherwise performed by the idle
* thread if CONFIG_SCHED_WORKQUEUE is not defined).
*
* These worker threads are started by the OS during normal bringup.
*
* work_usrthread: This is a user mode work queue. It must be built into
* the applicatino blob during the user phase of a kernel build. The
* user work thread will then automatically be started when the system
* boots by calling through the pointer found in the header on the user
* space blob.
*
* All of these entrypoints are referenced by OS internally and should not
* not be accessed by application logic.
*
* Input parameters:
* argc, argv (not used)
*
* Returned Value:
* Does not return
*
****************************************************************************/
#ifdef CONFIG_SCHED_HPWORK
int work_hpthread(int argc, char *argv[])
{
/* Loop forever */
for (;;)
{
/* First, perform garbage collection. This cleans-up memory de-allocations
* that were queued because they could not be freed in that execution
* context (for example, if the memory was freed from an interrupt handler).
* NOTE: If the work thread is disabled, this clean-up is performed by
* the IDLE thread (at a very, very low priority).
*/
#ifndef CONFIG_SCHED_LPWORK
sched_garbagecollection();
#endif
/* Then process queued work. We need to keep interrupts disabled while
* we process items in the work list.
*/
work_process(&g_work[HPWORK], HPWORK);
}
return PX4_OK; /* To keep some compilers happy */
}
#ifdef CONFIG_SCHED_LPWORK
int work_lpthread(int argc, char *argv[])
{
/* Loop forever */
for (;;)
{
/* First, perform garbage collection. This cleans-up memory de-allocations
* that were queued because they could not be freed in that execution
* context (for example, if the memory was freed from an interrupt handler).
* NOTE: If the work thread is disabled, this clean-up is performed by
* the IDLE thread (at a very, very low priority).
*/
//sched_garbagecollection();
/* Then process queued work. We need to keep interrupts disabled while
* we process items in the work list.
*/
work_process(&g_work[LPWORK], LPWORK);
}
return PX4_OK; /* To keep some compilers happy */
}
#endif /* CONFIG_SCHED_LPWORK */
#endif /* CONFIG_SCHED_HPWORK */
#ifdef CONFIG_SCHED_USRWORK
int work_usrthread(int argc, char *argv[])
{
/* Loop forever */
for (;;)
{
/* Then process queued work. We need to keep interrupts disabled while
* we process items in the work list.
*/
work_process(&g_work[USRWORK], USRWORK);
}
return PX4_OK; /* To keep some compilers happy */
}
#endif /* CONFIG_SCHED_USRWORK */
uint32_t clock_systimer()
{
//printf("clock_systimer: %0lx\n", hrt_absolute_time());
return (0x00000000ffffffff & hrt_absolute_time());
}
#endif /* CONFIG_SCHED_WORKQUEUE */
src/platforms/posix/px4_layer/dq_addlast.c → src/platforms/posix/work_queue/dq_addlast.c
View File
src/platforms/posix/px4_layer/dq_rem.c → src/platforms/posix/work_queue/dq_rem.c
View File
src/platforms/posix/px4_layer/dq_remfirst.c → src/platforms/posix/work_queue/dq_remfirst.c
View File
src/platforms/posix/px4_layer/hrt_queue.c → src/platforms/posix/work_queue/hrt_queue.c
+4
View File
@@ -121,7 +121,11 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t del
//PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime);
dq_addlast((dq_entry_t *)work, &wqueue->q);
#ifdef __PX4_QURT
px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */
#else
px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */
#endif
hrt_work_unlock();
return PX4_OK;
src/platforms/posix/px4_layer/hrt_thread.c → src/platforms/posix/work_queue/hrt_thread.c
View File
src/platforms/posix/px4_layer/hrt_work_cancel.c → src/platforms/posix/work_queue/hrt_work_cancel.c
View File
src/platforms/posix/work_queue/module.mk
+54
View File
@@ -0,0 +1,54 @@
############################################################################
#
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
#
# 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 PX4 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.
#
############################################################################
#
# POSIX compatible queue and work_queue implementation
#
SRCS = \
hrt_thread.c \
hrt_queue.c \
hrt_work_cancel.c \
work_thread.c \
work_lock.c \
work_queue.c \
work_cancel.c \
queue.c \
dq_addlast.c \
dq_remfirst.c \
sq_addlast.c \
sq_remfirst.c \
sq_addafter.c \
dq_rem.c
MAXOPTIMIZATION = -Os
src/platforms/posix/px4_layer/queue.c → src/platforms/posix/work_queue/queue.c
View File
src/platforms/posix/px4_layer/sq_addafter.c → src/platforms/posix/work_queue/sq_addafter.c
View File
src/platforms/posix/px4_layer/sq_addlast.c → src/platforms/posix/work_queue/sq_addlast.c
View File
src/platforms/posix/px4_layer/sq_remfirst.c → src/platforms/posix/work_queue/sq_remfirst.c
View File
src/platforms/posix/px4_layer/work_cancel.c → src/platforms/posix/work_queue/work_cancel.c
View File
src/platforms/qurt/px4_layer/work_lock.c → src/platforms/posix/work_queue/work_lock.c
View File
src/platforms/posix/px4_layer/work_lock.h → src/platforms/posix/work_queue/work_lock.h
View File
src/platforms/posix/px4_layer/work_queue.c → src/platforms/posix/work_queue/work_queue.c
+4
View File
@@ -124,7 +124,11 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_
work->qtime = clock_systimer(); /* Time work queued */
dq_addlast((dq_entry_t *)work, &wqueue->q);
#ifdef __PX4_QURT
px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */
#else
px4_task_kill(wqueue->pid, SIGCONT); /* Wake up the worker thread */
#endif
work_unlock(qid);
return PX4_OK;
src/platforms/qurt/px4_layer/work_thread.c → src/platforms/posix/work_queue/work_thread.c
View File
src/platforms/qurt/px4_layer/dq_addlast.c
-74
View File
@@ -1,74 +0,0 @@
/************************************************************
* libc/queue/dq_addlast.c
*
* Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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.
*
************************************************************/
/************************************************************
* Compilation Switches
************************************************************/
/************************************************************
* Included Files
************************************************************/
#include <queue.h>
/************************************************************
* Public Functions
************************************************************/
/************************************************************
* Name: dq_addlast
*
* Description
* dq_addlast adds 'node' to the end of 'queue'
*
************************************************************/
void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue)
{
node->flink = NULL;
node->blink = queue->tail;
if (!queue->head)
{
queue->head = node;
queue->tail = node;
}
else
{
queue->tail->flink = node;
queue->tail = node;
}
}
src/platforms/qurt/px4_layer/dq_rem.c
-84
View File
@@ -1,84 +0,0 @@
/************************************************************
* libc/queue/dq_rem.c
*
* Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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.
*
************************************************************/
/************************************************************
* Compilation Switches
************************************************************/
/************************************************************
* Included Files
************************************************************/
#include <queue.h>
/************************************************************
* Public Functions
************************************************************/
/************************************************************
* Name: dq_rem
*
* Descripton:
* dq_rem removes 'node' from 'queue'
*
************************************************************/
void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue)
{
FAR dq_entry_t *prev = node->blink;
FAR dq_entry_t *next = node->flink;
if (!prev)
{
queue->head = next;
}
else
{
prev->flink = next;
}
if (!next)
{
queue->tail = prev;
}
else
{
next->blink = prev;
}
node->flink = NULL;
node->blink = NULL;
}
src/platforms/qurt/px4_layer/dq_remfirst.c
-82
View File
@@ -1,82 +0,0 @@
/************************************************************
* libc/queue/dq_remfirst.c
*
* Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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.
*
************************************************************/
/************************************************************
* Compilation Switches
************************************************************/
/************************************************************
* Included Files
************************************************************/
#include <queue.h>
/************************************************************
* Public Functions
************************************************************/
/************************************************************
* Name: dq_remfirst
*
* Description:
* dq_remfirst removes 'node' from the head of 'queue'
*
************************************************************/
FAR dq_entry_t *dq_remfirst(dq_queue_t *queue)
{
FAR dq_entry_t *ret = queue->head;
if (ret)
{
FAR dq_entry_t *next = ret->flink;
if (!next)
{
queue->head = NULL;
queue->tail = NULL;
}
else
{
queue->head = next;
next->blink = NULL;
}
ret->flink = NULL;
ret->blink = NULL;
}
return ret;
}
src/platforms/qurt/px4_layer/hrt_queue.c
-129
View File
@@ -1,129 +0,0 @@
/****************************************************************************
* libc/wqueue/work_queue.c
*
* Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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 <px4_config.h>
#include <px4_defines.h>
#include <signal.h>
#include <stdint.h>
#include <queue.h>
#include <stdio.h>
#include <semaphore.h>
#include <drivers/drv_hrt.h>
#include <px4_workqueue.h>
#include "hrt_work.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Type Declarations
****************************************************************************/
/****************************************************************************
* Public Variables
****************************************************************************/
/****************************************************************************
* Private Variables
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: hrt_work_queue
*
* Description:
* Queue work to be performed at a later time. All queued work will be
* performed on the worker thread of of execution (not the caller's).
*
* The work structure is allocated by caller, but completely managed by
* the work queue logic. The caller should never modify the contents of
* the work queue structure; the caller should not call work_queue()
* again until either (1) the previous work has been performed and removed
* from the queue, or (2) work_cancel() has been called to cancel the work
* and remove it from the work queue.
*
* Input parameters:
* work - The work structure to queue
* worker - The worker callback to be invoked. The callback will invoked
* on the worker thread of execution.
* arg - The argument that will be passed to the workder callback when
* int is invoked.
* delay - Delay (in microseconds) from the time queue until the worker
* is invoked. Zero means to perform the work immediately.
*
* Returned Value:
* Zero on success, a negated errno on failure
*
****************************************************************************/
int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay)
{
struct wqueue_s *wqueue = &g_hrt_work;
/* First, initialize the work structure */
work->worker = worker; /* Work callback */
work->arg = arg; /* Callback argument */
work->delay = delay; /* Delay until work performed */
/* Now, time-tag that entry and put it in the work queue. This must be
* done with interrupts disabled. This permits this function to be called
* from with task logic or interrupt handlers.
*/
hrt_work_lock();
work->qtime = hrt_absolute_time(); /* Time work queued */
//PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime);
dq_addlast((dq_entry_t *)work, &wqueue->q);
px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */
hrt_work_unlock();
return PX4_OK;
}
src/platforms/qurt/px4_layer/hrt_thread.c
-256
View File
@@ -1,256 +0,0 @@
/****************************************************************************
* libc/wqueue/work_thread.c
*
* Copyright (C) 2009-2013 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
* Modified by: Mark Charlebois to use hrt compatible times
*
* 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 <px4_config.h>
#include <px4_defines.h>
#include <stdint.h>
#include <stdio.h>
#include <unistd.h>
#include <queue.h>
#include <px4_workqueue.h>
#include <drivers/drv_hrt.h>
#include "hrt_work.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Type Declarations
****************************************************************************/
/****************************************************************************
* Public Variables
****************************************************************************/
/* The state of each work queue. */
struct wqueue_s g_hrt_work;
/****************************************************************************
* Private Variables
****************************************************************************/
sem_t _hrt_work_lock;
/****************************************************************************
* Private Functions
****************************************************************************/
static void hrt_work_process(void);
/****************************************************************************
* Name: work_process
*
* Description:
* This is the logic that performs actions placed on any work list.
*
* Input parameters:
* wqueue - Describes the work queue to be processed
*
* Returned Value:
* None
*
****************************************************************************/
static void hrt_work_process()
{
struct wqueue_s *wqueue = &g_hrt_work;
volatile FAR struct work_s *work;
worker_t worker;
FAR void *arg;
uint64_t elapsed;
uint32_t remaining;
uint32_t next;
/* Then process queued work. We need to keep interrupts disabled while
* we process items in the work list.
*/
/* Default to sleeping for 1 sec */
next = 1000000;
hrt_work_lock();
work = (FAR struct work_s *)wqueue->q.head;
while (work)
{
/* Is this work ready? It is ready if there is no delay or if
* the delay has elapsed. qtime is the time that the work was added
* to the work queue. It will always be greater than or equal to
* zero. Therefore a delay of zero will always execute immediately.
*/
elapsed = hrt_absolute_time() - work->qtime;
//PX4_INFO("hrt work_process: in usec elapsed=%lu delay=%u work=%p", elapsed, work->delay, work);
if (elapsed >= work->delay)
{
/* Remove the ready-to-execute work from the list */
(void)dq_rem((struct dq_entry_s *)work, &wqueue->q);
//PX4_INFO("Dequeued work=%p", work);
/* Extract the work description from the entry (in case the work
* instance by the re-used after it has been de-queued).
*/
worker = work->worker;
arg = work->arg;
/* Mark the work as no longer being queued */
work->worker = NULL;
/* Do the work. Re-enable interrupts while the work is being
* performed... we don't have any idea how long that will take!
*/
hrt_work_unlock();
if (!worker) {
PX4_ERR("MESSED UP: worker = 0");
}
else {
worker(arg);
}
/* Now, unfortunately, since we re-enabled interrupts we don't
* know the state of the work list and we will have to start
* back at the head of the list.
*/
hrt_work_lock();
work = (FAR struct work_s *)wqueue->q.head;
}
else
{
/* This one is not ready.. will it be ready before the next
* scheduled wakeup interval?
*/
/* Here: elapsed < work->delay */
remaining = work->delay - elapsed;
//PX4_INFO("remaining=%u delay=%u elapsed=%lu", remaining, work->delay, elapsed);
if (remaining < next)
{
/* Yes.. Then schedule to wake up when the work is ready */
next = remaining;
}
/* Then try the next in the list. */
work = (FAR struct work_s *)work->dq.flink;
//PX4_INFO("next %u work %p", next, work);
}
}
/* Wait awhile to check the work list. We will wait here until either
* the time elapses or until we are awakened by a signal.
*/
hrt_work_unlock();
/* might sleep less if a signal received and new item was queued */
//PX4_INFO("Sleeping for %u usec", next);
usleep(next);
}
/****************************************************************************
* Name: work_hrtthread
*
* Description:
* This is the worker threads that performs actions placed on the ISR work
* list.
*
* work_hpthread and work_lpthread: These are the kernel mode work queues
* (also build in the flat build). One of these threads also performs
* periodic garbage collection (that is otherwise performed by the idle
* thread if CONFIG_SCHED_WORKQUEUE is not defined).
*
* These worker threads are started by the OS during normal bringup.
*
* All of these entrypoints are referenced by OS internally and should not
* not be accessed by application logic.
*
* Input parameters:
* argc, argv (not used)
*
* Returned Value:
* Does not return
*
****************************************************************************/
static int work_hrtthread(int argc, char *argv[])
{
/* Loop forever */
for (;;)
{
/* First, perform garbage collection. This cleans-up memory de-allocations
* that were queued because they could not be freed in that execution
* context (for example, if the memory was freed from an interrupt handler).
* NOTE: If the work thread is disabled, this clean-up is performed by
* the IDLE thread (at a very, very low priority).
*/
/* Then process queued work. We need to keep interrupts disabled while
* we process items in the work list.
*/
hrt_work_process();
}
return PX4_OK; /* To keep some compilers happy */
}
/****************************************************************************
* Public Functions
****************************************************************************/
void hrt_work_queue_init(void)
{
sem_init(&_hrt_work_lock, 0, 1);
// Create high priority worker thread
g_hrt_work.pid = px4_task_spawn_cmd("wkr_hrt",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
2000,
work_hrtthread,
(char* const*)NULL);
}
src/platforms/qurt/px4_layer/hrt_work_cancel.c
-112
View File
@@ -1,112 +0,0 @@
/****************************************************************************
* libc/wqueue/work_cancel.c
*
* Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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 <px4_config.h>
#include <px4_defines.h>
#include <queue.h>
#include <semaphore.h>
#include <px4_workqueue.h>
#include "hrt_work.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Type Declarations
****************************************************************************/
/****************************************************************************
* Public Variables
****************************************************************************/
/****************************************************************************
* Private Variables
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: hrt_work_cancel
*
* Description:
* Cancel previously queued work. This removes work from the work queue.
* After work has been canceled, it may be re-queue by calling
* hrt_work_queue() again.
*
* Input parameters:
* work - The previously queue work structure to cancel
*
****************************************************************************/
void hrt_work_cancel(struct work_s *work)
{
struct wqueue_s *wqueue = &g_hrt_work;
//DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
/* Cancelling the work is simply a matter of removing the work structure
* from the work queue. This must be done with interrupts disabled because
* new work is typically added to the work queue from interrupt handlers.
*/
hrt_work_lock();
if (work->worker != NULL)
{
/* A little test of the integrity of the work queue */
//DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail);
//DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head);
/* Remove the entry from the work queue and make sure that it is
* mark as availalbe (i.e., the worker field is nullified).
*/
dq_rem((FAR dq_entry_t *)work, &wqueue->q);
work->worker = NULL;
}
hrt_work_unlock();
}
src/platforms/qurt/px4_layer/module.mk
-16
View File
@@ -42,25 +42,9 @@ SRCDIR=$(dir $(MODULE_MK))
SRCS = \
px4_qurt_impl.cpp \
px4_qurt_tasks.cpp \
hrt_thread.c \
hrt_queue.c \
hrt_work_cancel.c \
work_thread.c \
work_queue.c \
work_lock.c \
work_cancel.c \
lib_crc32.c \
drv_hrt.c \
queue.c \
dq_addlast.c \
dq_remfirst.c \
sq_addlast.c \
sq_remfirst.c \
sq_addafter.c \
dq_rem.c \
hrt_work.c \
qurt_stubs.c \
qurt_hacks.c \
main.cpp
ifeq ($(CONFIG),qurt_hello)
SRCS += commands_hello.c
src/platforms/qurt/px4_layer/queue.c
-102
View File
@@ -1,102 +0,0 @@
/************************************************************************
*
* Copyright (C) 2007-2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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.
*
* Modified for use in Linux by Mark Charlebois
*
************************************************************************/
// FIXME - need px4_queue
#include <platforms/qurt/include/queue.h>
#include <stddef.h>
__EXPORT void sq_rem(sq_entry_t *node, sq_queue_t *queue);
sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue);
void sq_rem(sq_entry_t *node, sq_queue_t *queue)
{
if (queue->head && node)
{
if (node == queue->head)
{
queue->head = node->flink;
if (node == queue->tail)
{
queue->tail = NULL;
}
}
else
{
sq_entry_t *prev;
for(prev = (sq_entry_t*)queue->head;
prev && prev->flink != node;
prev = prev->flink);
if (prev)
{
sq_remafter(prev, queue);
}
}
}
}
sq_entry_t *sq_remafter(sq_entry_t *node, sq_queue_t *queue)
{
sq_entry_t *ret = node->flink;
if (queue->head && ret)
{
if (queue->tail == ret)
{
queue->tail = node;
node->flink = NULL;
}
else
{
node->flink = ret->flink;
}
ret->flink = NULL;
}
return ret;
}
__EXPORT void sq_addfirst(sq_entry_t *node, sq_queue_t *queue);
void sq_addfirst(sq_entry_t *node, sq_queue_t *queue)
{
node->flink = queue->head;
if (!queue->head)
{
queue->tail = node;
}
queue->head = node;
}
src/platforms/qurt/px4_layer/sq_addafter.c
-71
View File
@@ -1,71 +0,0 @@
/************************************************************
* libc/queue/sq_addafter.c
*
* Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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.
*
************************************************************/
/************************************************************
* Compilation Switches
************************************************************/
/************************************************************
* Included Files
************************************************************/
#include <queue.h>
/************************************************************
* Public Functions
************************************************************/
/************************************************************
* Name: sq_addafter.c
*
* Description:
* The sq_addafter function adds 'node' after 'prev' in the
* 'queue.'
*
************************************************************/
void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node,
sq_queue_t *queue)
{
if (!queue->head || prev == queue->tail)
{
sq_addlast(node, queue);
}
else
{
node->flink = prev->flink;
prev->flink = node;
}
}
src/platforms/qurt/px4_layer/sq_addlast.c
-72
View File
@@ -1,72 +0,0 @@
/************************************************************
* libc/queue/sq_addlast.c
*
* Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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.
*
************************************************************/
/************************************************************
* Compilation Switches
************************************************************/
/************************************************************
* Included Files
************************************************************/
#include <queue.h>
/************************************************************
* Public Functions
************************************************************/
/************************************************************
* Name: sq_addlast
*
* Description:
* The sq_addlast function places the 'node' at the tail of
* the 'queue'
************************************************************/
void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue)
{
node->flink = NULL;
if (!queue->head)
{
queue->head = node;
queue->tail = node;
}
else
{
queue->tail->flink = node;
queue->tail = node;
}
}
src/platforms/qurt/px4_layer/sq_remfirst.c
-76
View File
@@ -1,76 +0,0 @@
/************************************************************
* libc/queue/sq_remfirst.c
*
* Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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.
*
************************************************************/
/************************************************************
* Compilation Switches
************************************************************/
/************************************************************
* Included Files
************************************************************/
#include <queue.h>
/************************************************************
* Public Functions
************************************************************/
/************************************************************
* Name: sq_remfirst
*
* Description:
* sq_remfirst function removes the first entry from
* 'queue'
*
************************************************************/
FAR sq_entry_t *sq_remfirst(sq_queue_t *queue)
{
FAR sq_entry_t *ret = queue->head;
if (ret)
{
queue->head = ret->flink;
if (!queue->head)
{
queue->tail = NULL;
}
ret->flink = NULL;
}
return ret;
}
src/platforms/qurt/px4_layer/work_cancel.c
-120
View File
@@ -1,120 +0,0 @@
/****************************************************************************
* libc/wqueue/work_cancel.c
*
* Copyright (C) 2009-2010, 2012-2013 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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 <px4_config.h>
#include <px4_defines.h>
#include <queue.h>
#include <px4_workqueue.h>
#ifdef CONFIG_SCHED_WORKQUEUE
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Type Declarations
****************************************************************************/
/****************************************************************************
* Public Variables
****************************************************************************/
/****************************************************************************
* Private Variables
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: work_cancel
*
* Description:
* Cancel previously queued work. This removes work from the work queue.
* After work has been canceled, it may be re-queue by calling work_queue()
* again.
*
* Input parameters:
* qid - The work queue ID
* work - The previously queue work structure to cancel
*
* Returned Value:
* Zero on success, a negated errno on failure
*
****************************************************************************/
int work_cancel(int qid, struct work_s *work)
{
struct wqueue_s *wqueue = &g_work[qid];
//irqstate_t flags;
//DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
/* Cancelling the work is simply a matter of removing the work structure
* from the work queue. This must be done with interrupts disabled because
* new work is typically added to the work queue from interrupt handlers.
*/
//flags = irqsave();
if (work->worker != NULL)
{
/* A little test of the integrity of the work queue */
//DEBUGASSERT(work->dq.flink ||(FAR dq_entry_t *)work == wqueue->q.tail);
//DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head);
/* Remove the entry from the work queue and make sure that it is
* mark as availalbe (i.e., the worker field is nullified).
*/
dq_rem((FAR dq_entry_t *)work, &wqueue->q);
work->worker = NULL;
}
//irqrestore(flags);
return PX4_OK;
}
#endif /* CONFIG_SCHED_WORKQUEUE */
src/platforms/qurt/px4_layer/work_lock.h
-43
View File
@@ -1,43 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* 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 PX4 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 _work_lock_h_
#define _work_lock_h_
//#pragma once
void work_lock(int id);
void work_unlock(int id);
#endif // _work_lock_h_
src/platforms/qurt/px4_layer/work_queue.c
-133
View File
@@ -1,133 +0,0 @@
/****************************************************************************
* libc/wqueue/work_queue.c
*
* Copyright (C) 2009-2011, 2013 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* 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 <px4_config.h>
#include <px4_defines.h>
#include <signal.h>
#include <stdint.h>
#include <queue.h>
#include <stdio.h>
#include <semaphore.h>
#include <px4_workqueue.h>
#include "work_lock.h"
#ifdef CONFIG_SCHED_WORKQUEUE
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/****************************************************************************
* Private Type Declarations
****************************************************************************/
/****************************************************************************
* Public Variables
****************************************************************************/
/****************************************************************************
* Private Variables
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: work_queue
*
* Description:
* Queue work to be performed at a later time. All queued work will be
* performed on the worker thread of of execution (not the caller's).
*
* The work structure is allocated by caller, but completely managed by
* the work queue logic. The caller should never modify the contents of
* the work queue structure; the caller should not call work_queue()
* again until either (1) the previous work has been performed and removed
* from the queue, or (2) work_cancel() has been called to cancel the work
* and remove it from the work queue.
*
* Input parameters:
* qid - The work queue ID (index)
* work - The work structure to queue
* worker - The worker callback to be invoked. The callback will invoked
* on the worker thread of execution.
* arg - The argument that will be passed to the workder callback when
* int is invoked.
* delay - Delay (in clock ticks) from the time queue until the worker
* is invoked. Zero means to perform the work immediately.
*
* Returned Value:
* Zero on success, a negated errno on failure
*
****************************************************************************/
int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay)
{
struct wqueue_s *wqueue = &g_work[qid];
//DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
/* First, initialize the work structure */
work->worker = worker; /* Work callback */
work->arg = arg; /* Callback argument */
work->delay = delay; /* Delay until work performed */
/* Now, time-tag that entry and put it in the work queue. This must be
* done with interrupts disabled. This permits this function to be called
* from with task logic or interrupt handlers.
*/
work_lock(qid);
work->qtime = clock_systimer(); /* Time work queued */
dq_addlast((dq_entry_t *)work, &wqueue->q);
px4_task_kill(wqueue->pid, SIGALRM); /* Wake up the worker thread */
work_unlock(qid);
return PX4_OK;
}
#endif /* CONFIG_SCHED_WORKQUEUE */