Support for building more modules with Linux

Added more queue support to linux/px4_layer.

Use virt char devices for ms5611, and mavlink.

Added more HRT functionality. uORB latency test
now fails. Likely due to bad HRT impl for Linux.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/platforms/linux/include/queue.h

@@ -96,14 +96,6 @@ struct dq_queue_s
 };
 typedef struct dq_queue_s dq_queue_t;
 
-// FIXME - hack for mavlink until kernel work queue can be removed
-#define LPWORK 1
-typedef struct {
-	dq_entry_t dq;
-} work_s;
-inline void work_queue(int worktype, work_s *dummy, void (*_worker_trampoline)(void *arg), void *req, int dummy2);
-inline void work_queue(int worktype, work_s *dummy, void (*_worker_trampoline)(void *arg), void *req, int dummy2) {}
-
 /************************************************************************
  * Global Function Prototypes
  ************************************************************************/

src/platforms/linux/main.cpp

@@ -41,6 +41,7 @@
 #include <fstream>
 #include <string>
 #include <sstream>
+#include <vector>
 
 using namespace std;
 
@@ -48,16 +49,22 @@ typedef int (*px4_main_t)(int argc, char *argv[]);
 
 #include "apps.h"
 
-// FIXME - the code below only passes 1 arg for now
-void run_cmd(const string &command, const string &apparg);
-void run_cmd(const string &command, const string &apparg) {
-	const char *arg[3];
-
+void run_cmd(const vector<string> &appargs);
+void run_cmd(const vector<string> &appargs) {
+	// command is appargs[0]
+	string command = appargs[0];
+	cout << "appargs.size() = " << appargs.size() << endl;
 	if (apps.find(command) != apps.end()) {
-		arg[0] = (char *)command.c_str();
-		arg[1] = (char *)apparg.c_str();
-		arg[2] = (char *)0;
-		apps[command](2,(char **)arg);
+		const char *arg[appargs.size()+2];
+
+		unsigned int i = 0;
+		while (i < appargs.size() && appargs[i] != "") {
+			arg[i] = (char *)appargs[i].c_str();
+			++i;
+		}
+		arg[i] = (char *)0;
+		cout << command << " " << i  << endl;
+		apps[command](i,(char **)arg);
 	}
 	else
 	{
@@ -65,12 +72,22 @@ void run_cmd(const string &command, const string &apparg) {
 	}
 }
 
+void process_line(string &line)
+{
+	vector<string> appargs(5);
+
+	stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4];
+	cout << "Command " << appargs[0] << endl;
+	unsigned int i = 1;
+	while ( i < appargs.size() && appargs[i] != "") {
+		cout << "  appargs[" << i << "] = " << appargs[i] << endl;
+		++i;
+	}
+	run_cmd(appargs);
+}
+
 int main(int argc, char **argv)
 {
-	string mystr;
-	string command;
-	string apparg;
-	
 	// Execute a command list of provided
 	if (argc == 2) {
 		ifstream infile(argv[1]);
@@ -78,21 +95,21 @@ int main(int argc, char **argv)
 		//vector<string> tokens;
 
 		for (string line; getline(infile, line, '\n'); ) {
-  			stringstream(line) >> command >> apparg;
-			cout << "Command " << command << ", apparg " << apparg << endl;
-			run_cmd(command, apparg);
+			process_line(line);
 		}
 	}
 
+	string mystr;
+	vector<string> appargs(2);
+
+	appargs[0] = "list_builtins";
+	
 	while(1) {
-		run_cmd("list_builtins", "");
+		run_cmd(appargs);
 
 		cout << "Enter a command and its args:" << endl;
 		getline (cin,mystr);
-  		stringstream(mystr) >> command >> apparg;
-		run_cmd(command, apparg);
+		process_line(mystr);
 		mystr = "";
-		command = "";
-		apparg = "";
 	}
 }

src/platforms/linux/px4_layer/dq_addlast.c

@@ -0,0 +1,74 @@
+/************************************************************
+ * 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/linux/px4_layer/dq_remfirst.c

@@ -0,0 +1,82 @@
+/************************************************************
+ * 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/linux/px4_layer/drv_hrt.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 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
@@ -32,14 +32,16 @@
  ****************************************************************************/
 
 /**
- * @file drv_hrt.h
+ * @file drv_hrt.c
  *
  * High-resolution timer with callouts and timekeeping.
  */
 
 #include <drivers/drv_hrt.h>
+#include <time.h>
+#include <string.h>
 
-static hrt_abstime dummy_timer = 0;
+static struct sq_queue_s	callout_queue;
 
 /* latency histogram */
 #define LATENCY_BUCKET_COUNT 8
@@ -47,12 +49,20 @@ __EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
 __EXPORT const uint16_t	latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
 __EXPORT uint32_t	latency_counters[LATENCY_BUCKET_COUNT + 1];
 
+static void		hrt_call_reschedule(void);
+
+#define HRT_INTERVAL_MIN	50
+#define HRT_INTERVAL_MAX	50000
+
 /*
  * Get absolute time.
  */
 hrt_abstime hrt_absolute_time(void)
 {
-	return dummy_timer;
+	struct timespec ts;
+
+	clock_gettime(CLOCK_MONOTONIC, &ts);
+	return ts_to_abstime(&ts);
 }
 
 /*
@@ -60,7 +70,12 @@ hrt_abstime hrt_absolute_time(void)
  */
 hrt_abstime ts_to_abstime(struct timespec *ts)
 {
-	return dummy_timer;
+	hrt_abstime	result;
+
+	result = (hrt_abstime)(ts->tv_sec) * 1000000;
+	result += ts->tv_nsec / 1000;
+
+	return result;
 }
 
 /*
@@ -77,7 +92,8 @@ void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
  */
 hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then)
 {
-	return dummy_timer;
+	hrt_abstime delta = hrt_absolute_time() - *then;
+	return delta;
 }
 
 /*
@@ -87,35 +103,10 @@ hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then)
  */
 hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now)
 {
-	return dummy_timer;
+	hrt_abstime ts = hrt_absolute_time();
+	return ts;
 }
 
-/*
- * Call callout(arg) after delay has elapsed.
- *
- * If callout is NULL, this can be used to implement a timeout by testing the call
- * with hrt_called().
- */
-void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
-{
-}
-
-/*
- * Call callout(arg) at absolute time calltime.
- */
-void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
-{
-}
-
-/*
- * Call callout(arg) after delay, and then after every interval.
- *
- * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
- * jitter but should not drift.
- */
-void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
-{
-}
 
 /*
  * If this returns true, the entry has been invoked and removed from the callout list,
@@ -125,7 +116,7 @@ void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime inter
  */
 bool	hrt_called(struct hrt_call *entry)
 {
-	return true;
+	return (entry->deadline == 0);
 }
 
 /*
@@ -133,6 +124,15 @@ bool	hrt_called(struct hrt_call *entry)
  */
 void	hrt_cancel(struct hrt_call *entry)
 {
+	// FIXME - need a lock
+	sq_rem(&entry->link, &callout_queue);
+	entry->deadline = 0;
+
+	/* if this is a periodic call being removed by the callout, prevent it from
+	 * being re-entered when the callout returns.
+	 */
+	entry->period = 0;
+	// endif
 }
 
 /*
@@ -140,6 +140,7 @@ void	hrt_cancel(struct hrt_call *entry)
  */
 void	hrt_call_init(struct hrt_call *entry)
 {
+	memset(entry, 0, sizeof(*entry));
 }
 
 /*
@@ -151,6 +152,7 @@ void	hrt_call_init(struct hrt_call *entry)
  */
 void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
 {
+	entry->deadline = hrt_absolute_time() + delay;
 }
 
 /*
@@ -158,5 +160,132 @@ void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
  */
 void	hrt_init(void)
 {
+	sq_init(&callout_queue);
+}
+
+static void
+hrt_call_enter(struct hrt_call *entry)
+{
+	struct hrt_call	*call, *next;
+
+	call = (struct hrt_call *)sq_peek(&callout_queue);
+
+	if ((call == NULL) || (entry->deadline < call->deadline)) {
+		sq_addfirst(&entry->link, &callout_queue);
+		//lldbg("call enter at head, reschedule\n");
+		/* we changed the next deadline, reschedule the timer event */
+		hrt_call_reschedule();
+
+	} else {
+		do {
+			next = (struct hrt_call *)sq_next(&call->link);
+
+			if ((next == NULL) || (entry->deadline < next->deadline)) {
+				//lldbg("call enter after head\n");
+				sq_addafter(&call->link, &entry->link, &callout_queue);
+				break;
+			}
+		} while ((call = next) != NULL);
+	}
+
+	//lldbg("scheduled\n");
+}
+
+/**
+ * Reschedule the next timer interrupt.
+ *
+ * This routine must be called with interrupts disabled.
+ */
+static void
+hrt_call_reschedule()
+{
+	hrt_abstime	now = hrt_absolute_time();
+	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
+	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
+
+	/*
+	 * Determine what the next deadline will be.
+	 *
+	 * Note that we ensure that this will be within the counter
+	 * period, so that when we truncate all but the low 16 bits
+	 * the next time the compare matches it will be the deadline
+	 * we want.
+	 *
+	 * It is important for accurate timekeeping that the compare
+	 * interrupt fires sufficiently often that the base_time update in
+	 * hrt_absolute_time runs at least once per timer period.
+	 */
+	if (next != NULL) {
+		//lldbg("entry in queue\n");
+		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
+			//lldbg("pre-expired\n");
+			/* set a minimal deadline so that we call ASAP */
+			deadline = now + HRT_INTERVAL_MIN;
+
+		} else if (next->deadline < deadline) {
+			//lldbg("due soon\n");
+			deadline = next->deadline;
+		}
+	}
+}
+
+static void
+hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	/* if the entry is currently queued, remove it */
+	/* note that we are using a potentially uninitialised
+	   entry->link here, but it is safe as sq_rem() doesn't
+	   dereference the passed node unless it is found in the
+	   list. So we potentially waste a bit of time searching the
+	   queue for the uninitialised entry->link but we don't do
+	   anything actually unsafe.
+	*/
+	if (entry->deadline != 0)
+		sq_rem(&entry->link, &callout_queue);
+
+	entry->deadline = deadline;
+	entry->period = interval;
+	entry->callout = callout;
+	entry->arg = arg;
+
+	hrt_call_enter(entry);
+}
+
+/*
+ * Call callout(arg) after delay has elapsed.
+ *
+ * If callout is NULL, this can be used to implement a timeout by testing the call
+ * with hrt_called().
+ */
+void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  0,
+			  callout,
+			  arg);
+}
+
+/*
+ * Call callout(arg) after delay, and then after every interval.
+ *
+ * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may
+ * jitter but should not drift.
+ */
+void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  interval,
+			  callout,
+			  arg);
+}
+
+/*
+ * Call callout(arg) at absolute time calltime.
+ */
+void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry, calltime, 0, callout, arg);
 }
 

src/platforms/linux/px4_layer/module.mk

@@ -41,7 +41,10 @@ SRCS		 = 	\
 			lib_crc32.c \
 			drv_hrt.c \
 			queue.c \
+			dq_addlast.c \
+			dq_remfirst.c \
 			sq_addlast.c \
-			sq_remfirst.c
+			sq_remfirst.c \
+			sq_addafter.c
 
 MAXOPTIMIZATION	 = -Os

src/platforms/linux/px4_layer/px4_linux_impl.cpp

@@ -37,9 +37,12 @@
  * PX4 Middleware Wrapper Linux Implementation
  */
 
+#include <px4_defines.h>
 #include <px4_middleware.h>
+#include <px4_workqueue.h>
 #include <stdint.h>
 #include <stdio.h>
+#include <unistd.h>
 #include "systemlib/param/param.h"
 
 __BEGIN_DECLS
@@ -49,6 +52,8 @@ struct param_info_s      param_array[256];
 struct param_info_s      *param_info_base;
 struct param_info_s      *param_info_limit;
 
+long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK);
+
 __END_DECLS
 
 namespace px4
@@ -92,3 +97,19 @@ void init(int argc, char *argv[], const char *app_name)
 }
 
 }
+
+
+
+
+int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay)
+{
+	printf("work_queue: UNIMPLEMENTED\n");
+	return PX4_OK;
+}
+
+int work_cancel(int qid, struct work_s *work)
+{
+	printf("work_cancel: UNIMPLEMENTED\n");
+	return PX4_OK;
+}
+

src/platforms/linux/px4_layer/px4_linux_tasks.c

@@ -151,21 +151,14 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 int px4_task_delete(px4_task_t id)
 {
 	int rv = 0;
-	int i; 
 	pthread_t pid;
 	//printf("Called px4_task_delete\n");
 
-	// Get pthread ID from the opaque ID
-	for (i=0; i<PX4_MAX_TASKS; ++i) {
-		// FIXME - precludes pthread task to have an ID of 0
-		if (taskmap[i] == id) {
-			pid = taskmap[i];
-			break;
-		}
-	}
-	if (i>=PX4_MAX_TASKS) {
+	if (id < PX4_MAX_TASKS && taskmap[id] != 0)
+		pid = taskmap[id];
+	else
 		return -EINVAL;
-	}
+
 	// If current thread then exit, otherwise cancel
         if (pthread_self() == pid) {
 		pthread_exit(0);
@@ -178,6 +171,25 @@ int px4_task_delete(px4_task_t id)
 	return rv;
 }
 
+void px4_task_exit(int ret)
+{
+	int i; 
+	pthread_t pid = pthread_self();
+
+	// Get pthread ID from the opaque ID
+	for (i=0; i<PX4_MAX_TASKS; ++i) {
+		// FIXME - precludes pthread task to have an ID of 0
+		if (taskmap[i] == pid) {
+			taskmap[i] = 0;
+			break;
+		}
+	}
+	if (i>=PX4_MAX_TASKS) 
+		printf("px4_task_exit: self task not found!\n");
+
+	pthread_exit((void *)(unsigned long)ret);
+}
+
 void px4_killall(void)
 {
 	//printf("Called px4_killall\n");

src/platforms/linux/px4_layer/sq_addafter.c

@@ -0,0 +1,71 @@
+/************************************************************
+ * 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;
+    }
+}