copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/aos/atom_code/README.txt b/aos/atom_code/README.txt
new file mode 100644
index 0000000..5cf8182
--- /dev/null
+++ b/aos/atom_code/README.txt
@@ -0,0 +1,6 @@
+see ../README.txt for stuff affecting crio and atom code
+
+[NOTES]
+Any code should call aos::Init() (or aos::InitNRT() for processes that don't need to be realtime) before making any calls to any of the aos functions.
+Making calls to any of the aos functions (including aos::Init()) from more than 1 thread per process is not supported, but using fork(2) after some aos functions have been called and then continuing to make aos function calls (without calling one of the exec(3) functions) in both processes is supported.
+
diff --git a/aos/atom_code/async_action/AsyncAction.cpp.inc b/aos/atom_code/async_action/AsyncAction.cpp.inc
new file mode 100644
index 0000000..911bee8
--- /dev/null
+++ b/aos/atom_code/async_action/AsyncAction.cpp.inc
@@ -0,0 +1,405 @@
+#include <sys/syscall.h>
+#include <errno.h>
+#include <math.h>
+
+namespace aos {
+
+template<class T, class S> AsyncAction<T, S>::AsyncAction(const std::string name) : local_count(0), local_done_status(0), local_pid(0),
+ done_index(0), status_index(0), next_status_count(0){
+ aos_type_sig async_action_status_sig = {sizeof(async_action_status<S>), 1234, 1};
+ status_queue = aos_fetch_queue(name.c_str(), &async_action_status_sig);
+ aos_type_sig async_action_start_sig = {sizeof(async_action_start<T>), 4321, 1};
+ start_queue = aos_fetch_queue(name.c_str(), &async_action_start_sig);
+}
+
+template<class T, class S> uint16_t AsyncAction<T, S>::Start(T &args){
+ CheckStop();
+ // write something to the start queue and increment the count in the status queue
+ async_action_start<S> *start = (async_action_start<S> *)aos_queue_get_msg(start_queue);
+ if(start == NULL)
+ return 0;
+ memcpy(&start->args, &args, sizeof(T));
+ async_action_status<T> *status = (async_action_status<T> *)aos_queue_read_msg(status_queue, 0); // wait until one starts it if didn't already
+ uint16_t r = 0;
+ aos_resource_entity *entity;
+ if(status == NULL)
+ goto err;
+ r = status->count + 1;
+ local_pid = status->pid;
+ start->count = r;
+ entity = status->resource_entity;
+ aos_queue_free_msg(status_queue, status);
+ status = (async_action_status<T> *)aos_queue_get_msg(status_queue);
+ status->count = r;
+ status->done_status = 0;
+ status->pid = local_pid;
+ status->resource_entity = entity; // the resource_entity of the action, not the caller!
+ start->parent = resource_entity; // if it's NULL, it'll get fixed when starting
+ if(aos_queue_write_msg(status_queue, status, OVERRIDE) < 0){
+ goto err;
+ } else {
+ status = NULL;
+ }
+ if(aos_queue_write_msg(start_queue, start, OVERRIDE) < 0){
+ goto err;
+ } else {
+ start = NULL;
+ }
+err:
+ if(start != NULL)
+ aos_queue_free_msg(start_queue, start);
+ if(status != NULL)
+ aos_queue_free_msg(status_queue, status);
+ local_count = r;
+ return r;
+}
+template<class T, class S> void AsyncAction<T, S>::Stop(int32_t count){
+ CheckStop();
+ async_action_status<S> *status = (async_action_status<S> *)aos_queue_read_msg(status_queue, PEEK | NON_BLOCK);
+ if(status == NULL)
+ return;
+ local_pid = status->pid;
+ local_count = status->count;
+ aos_queue_free_msg(status_queue, status);
+ if(local_count != GetCount(count)) // if it's the wrong one
+ return;
+ async_action_start<S> *start = (async_action_start<S> *)aos_queue_read_msg(start_queue, PEEK | NON_BLOCK);
+ if(start == NULL) // if there isn't something in the start queue (aka one running)
+ return;
+ aos_queue_free_msg(start_queue, start);
+ union sigval sival;
+ sival.sival_int = 0;
+ if(sigqueue(local_pid, STOP_SIGNAL, sival) < 0){ // if sending the signal failed
+ fprintf(stderr, "sigqueue STOP_SIGNAL (which is %d) with pid %d failed with errno %d: ", STOP_SIGNAL, local_pid, errno);
+ perror(NULL);
+ }
+}
+
+template<class T, class S> bool AsyncAction<T, S>::IsDone(int32_t count){
+ CheckStop();
+ async_action_status<S> *status = (async_action_status<S> *)aos_queue_read_msg(status_queue, PEEK | NON_BLOCK);
+ // below = there is one running && it's the right one && it's done
+ bool r = status != NULL && status->count == GetCount(count) && ((status->done_status & 0x02) != 0);
+ aos_queue_free_msg(status_queue, status);
+ return r;
+}
+template<class T, class S> uint16_t AsyncAction<T, S>::Join(int32_t count_in){
+ const uint16_t count = GetCount(count_in);
+ if(count == 0){
+ fprintf(stderr, "not joining non-existent run 0\n");
+ return 0;
+ }
+ async_action_status<S> *status = NULL;
+ done_index = 0; // the queue's message numbering might have been reset
+ do {
+ aos_queue_free_msg(status_queue, status);
+ CheckStop();
+ status = (async_action_status<S> *)aos_queue_read_msg_index(status_queue, FROM_END, &done_index);
+ } while(status != NULL && (status->done_status & 0x02) == 0 && (status->count == count));
+ if(status == NULL){
+ fprintf(stderr, "bad news at %s: %d\n", __FILE__, __LINE__);
+ return 0;
+ }
+ aos_queue_free_msg(status_queue, status);
+ return count;
+}
+template<class T, class S> bool AsyncAction<T, S>::GetNextStatus(S &status_out, int32_t count_in){
+ async_action_status<S> *status = NULL;
+start:
+ CheckStop();
+ const uint16_t count = GetCount(count_in);
+ if(count != next_status_count){ // reset the index if another one gets started in case the queue indexes get reset
+ next_status_count = count;
+ status_index = 0;
+ }
+ status = (async_action_status<S> *)aos_queue_read_msg_index(status_queue, FROM_END, &status_index);
+ if(status == NULL)
+ goto start;
+ if(status->count != count){
+ aos_queue_free_msg(status_queue, status);
+ return false;
+ }
+ bool r = (status->done_status & 0x01) != 0;
+ memcpy(&status_out, &status->status, sizeof(S));
+ aos_queue_free_msg(status_queue, status);
+ return r;
+}
+template<class T, class S> bool AsyncAction<T, S>::GetStatus(S &status_out, int32_t count){
+ CheckStop();
+ async_action_status<S> *status = (async_action_status<S> *)aos_queue_read_msg(status_queue, PEEK | NON_BLOCK);
+ if(status == NULL)
+ return false;
+ if(status->count != GetCount(count)){
+ aos_queue_free_msg(status_queue, status);
+ return false;
+ }
+ bool r = (status->done_status & 0x01) != 0;
+ memcpy(&status_out, &status->status, sizeof(S));
+ aos_queue_free_msg(status_queue, status);
+ return r;
+}
+
+template<class T, class S> void AsyncAction<T, S>::PostStatus(S &status_in){
+ CheckStop();
+ async_action_status<S> *status = (async_action_status<S> *)aos_queue_get_msg(status_queue);
+ if(status == NULL)
+ return;
+ memcpy(&local_status, &status_in, sizeof(S));
+ memcpy(&status->status, &status_in, sizeof(S));
+ status->done_status = 0x01;
+ local_done_status = 0x01;
+ status->pid = local_pid;
+ status->resource_entity = resource_entity;
+ if(aos_queue_write_msg(status_queue, status, OVERRIDE) < 0){
+ local_done_status = 0;
+ memset(&local_status, 0x00, sizeof(S));
+ aos_queue_free_msg(status_queue, status);
+ }
+}
+template<class T, class S> template<int (*O)(aos_resource_entity *, aos_resource *)> inline bool AsyncAction<T, S>::ResourceOp(uint16_t resource){
+ CheckStop();
+ if(resource_entity == NULL){
+ fprintf(stderr, "no started AsyncAction detected in this process\n");
+ return false;
+ }
+ switch(O(resource_entity, aos_resource_get(resource))){
+ case 0:
+ break;
+ case 1:
+ return false;
+ case -1:
+ throw resourceexception();
+ }
+ return true;
+}
+template<class T, class S> void AsyncAction<T, S>::RequestResource(uint16_t resource){
+ if(ResourceOp<aos_resource_request>(resource)){
+ resources[resource] = 1;
+ }else{
+ throw resourceexception(); // if we can't get a resource, we just want to stop
+ }
+}
+template<class T, class S> bool AsyncAction<T, S>::TryRequestResource(uint16_t resource){
+ if(ResourceOp<aos_resource_request>(resource)){
+ resources[resource] = 1;
+ return true;
+ }else{
+ return false;
+ }
+}
+template<class T, class S> void AsyncAction<T, S>::ReleaseResource(uint16_t resource){
+ if(ResourceOp<aos_resource_release>(resource)){
+ if(resources.erase(resource) != 1)
+ fprintf(stderr, "warning: value for resource %d wasn't 1\n", resource);
+ }
+}
+
+#if 0
+// from gcc's (used 4.6.2) libjava/include/i386-signal.h
+/* We use kernel_sigaction here because we're calling the kernel
+ directly rather than via glibc. The sigaction structure that the
+ syscall uses is a different shape from the one in userland and not
+ visible to us in a header file so we define it here. */
+extern "C"
+{
+ struct kernel_sigaction
+ {
+ void (*k_sa_sigaction)(int,siginfo_t *,void *);
+ unsigned long k_sa_flags;
+ void (*k_sa_restorer) (void);
+ sigset_t k_sa_mask;
+ };
+}
+#define RESTORE(name, syscall) RESTORE2 (name, syscall)
+#define RESTORE2(name, syscall) \
+asm \
+ ( \
+ ".text\n" \
+ ".byte 0 # Yes, this really is necessary\n" \
+ " .align 16\n" \
+ "__" #name ":\n" \
+ " movl $" #syscall ", %eax\n" \
+ " int $0x80" \
+ );
+/* The return code for realtime-signals. */
+RESTORE (restore_rt, __NR_rt_sigreturn)
+void restore_rt (void) asm ("__restore_rt")
+ __attribute__ ((visibility ("hidden")));
+#define INIT_SIGNAL(signal, sigaction) \
+do \
+ { \
+ struct kernel_sigaction act; \
+ act.k_sa_sigaction = sigaction; \
+ sigemptyset (&act.k_sa_mask); \
+ act.k_sa_flags = SA_SIGINFO|0x4000000; \
+ act.k_sa_restorer = restore_rt; \
+ syscall (SYS_rt_sigaction, signal, &act, NULL, _NSIG / 8); \
+ } \
+while (0)
+/* Unblock a signal. Unless we do this, the signal may only be sent
+ once. */
+static void
+unblock_signal (int signum __attribute__ ((__unused__)))
+{
+ sigset_t sigs;
+
+ sigemptyset (&sigs);
+ sigaddset (&sigs, signum);
+ sigprocmask (SIG_UNBLOCK, &sigs, NULL);
+}
+#endif
+
+template<class T, class S> void AsyncAction<T, S>::sig_action(int signum, siginfo_t *, void *){
+ // TODO really shouldn't be using stdio in here (check before changing)
+ /*unblock_signal(signum);
+ // MAKE_THROW_FRAME(exception)
+ std::exception *exception = new std::exception;
+ throw exception;*/
+ fprintf(stderr, "received signal %d\n", signum);
+ if(signum == STOP_SIGNAL)
+ interrupt |= 0x01;
+ else if(signum == RESOURCE_KILL_SIGNAL)
+ interrupt |= 0x02;
+ else if(signum == SIGINT || signum == SIGTERM)
+ interrupt |= 0x04;
+ else
+ fprintf(stderr, "unknown signal %d\n", signum);
+}
+template<class T, class S> int AsyncAction<T, S>::Run(uint8_t priority) {
+ interrupt = 0;
+ struct sigaction sigact;
+ sigact.sa_sigaction = sig_action;
+ sigact.sa_flags = 0;
+ sigaction(STOP_SIGNAL, &sigact, NULL);
+ sigaction(RESOURCE_KILL_SIGNAL, &sigact, NULL);
+ sigaction(SIGINT, &sigact, NULL);
+ sigaction(SIGTERM, &sigact, NULL); // kill from the command line default
+
+ if(resource_entity != NULL){
+ fprintf(stderr, "resource_entity isn't already null, which means that this is the second Run being called in this process or something (which isn't allowed...)\n");
+ return -1;
+ }
+ resource_entity = aos_resource_entity_create(priority);
+
+ async_action_status<S> *status = (async_action_status<S> *)aos_queue_get_msg(status_queue);
+ if(status == NULL)
+ return -1;
+ //memset(status + offsetof(aync_action_status<S>, status->status), 0x00, sizeof(S));
+ status->count = 1;
+ status->done_status = 0;
+ local_done_status = 0;
+ local_pid = getpid();
+ fprintf(stderr, "local_pid=%d STOP_SIGNAL is currently %d\n", local_pid, STOP_SIGNAL);
+ status->pid = local_pid;
+ status->resource_entity = resource_entity;
+ // put the initial status in
+ if(aos_queue_write_msg(status_queue, status, OVERRIDE) < 0){
+ aos_queue_free_msg(status_queue, status);
+ return -1;
+ }
+
+ // clear out any pending start messages
+ async_action_start<T> *start = (async_action_start<T> *)aos_queue_read_msg(start_queue, NON_BLOCK);
+ aos_queue_free_msg(start_queue, start);
+
+ OnStart();
+
+ T args;
+ uint16_t current_count;
+ while(true){
+ interrupt = 0;
+ // wait for a new start message
+ start = (async_action_start<T> *)aos_queue_read_msg(start_queue, PEEK);
+ if(start == NULL){
+ if(interrupt & 0x04)
+ break;
+ continue;
+ }
+ memcpy(&args, &start->args, sizeof(T));
+ current_count = start->count;
+ if(start->parent == NULL){ // Start isn't getting called from a process with an AsyncAction implementation running in it
+ start->parent = aos_resource_entity_root_get();
+ }
+ aos_resource_entity_set_parent(resource_entity, start->parent);
+ aos_queue_free_msg(start_queue, start);
+ status = (async_action_status<S> *)aos_queue_get_msg(status_queue);
+ if(status == NULL){
+ fprintf(stderr, "%s: %d: could not get a status message to write initial status to\n", __FILE__, __LINE__);
+ continue;
+ }
+ status->done_status = local_done_status = 0;
+ status->pid = local_pid;
+ status->count = current_count;
+ status->resource_entity = resource_entity;
+ if(aos_queue_write_msg(status_queue, status, OVERRIDE) < 0)
+ aos_queue_free_msg(status_queue, status);
+
+ try {
+ DoAction(args);
+ } catch (stopexception &e) {
+ fprintf(stderr, "caught stopexception\n");
+ } catch (resourceexception &e) {
+ fprintf(stderr, "caught resourceexception\n");
+ } catch (...) {
+ fprintf(stderr, "caught another exception... (bad news)\n");
+ }
+
+ start = (async_action_start<T> *)aos_queue_read_msg(start_queue, NON_BLOCK);
+ if(start == NULL){
+ fprintf(stderr, "somebody else consumed the start message (at %s: %d)\n", __FILE__, __LINE__);
+ } else {
+ aos_queue_free_msg(start_queue, start);
+ }
+
+ status = (async_action_status<S> *)aos_queue_get_msg(status_queue);
+ if(status == NULL){
+ fprintf(stderr, "couldn't get a message to write that we're finished in to %s: %d\n", __FILE__, __LINE__);
+ continue;
+ }
+ memcpy(&status->status, &local_status, sizeof(S));
+ status->done_status = local_done_status | 0x02;
+ status->pid = local_pid;
+ status->count = current_count;
+ status->resource_entity = resource_entity;
+ if(aos_queue_write_msg(status_queue, status, OVERRIDE) < 0)
+ aos_queue_free_msg(status_queue, status);
+
+ std::map<uint16_t, uint8_t>::iterator it;
+ for(it = resources.begin(); it != resources.end(); ++it){
+ ReleaseResource(it->first);
+ }
+ if(!resources.empty()){
+ fprintf(stderr, "resources isn't empty after releasing everything in it. clearing it\n");
+ resources.clear();
+ }
+
+ if(interrupt & 0x04)
+ break;
+ }
+ OnEnd();
+ return 0;
+}
+
+template<class T, class S> void AsyncAction<T, S>::Sleep(double seconds){
+ timespec ts;
+ clock_gettime(CLOCK_MONOTONIC, &ts);
+ ts.tv_sec += (time_t)floor(seconds);
+ ts.tv_nsec += (long)((seconds - floor(seconds)) * 1000000000);
+ if(ts.tv_nsec > 1000000000){
+ ts.tv_nsec -= 1000000000;
+ ts.tv_sec += 1;
+ }
+ int rv;
+ do {
+ CheckStop();
+ rv = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, NULL);
+ } while(rv);
+}
+
+template<class T, class S> void AsyncAction<T, S>::DoAction(__attribute__((unused)) T &args){
+ fprintf(stderr, "this (at %s: %d) should never get run\n", __FILE__, __LINE__);
+ *((int *)NULL) = 0;
+}
+
+} // namespace aos
+
diff --git a/aos/atom_code/async_action/AsyncAction.h b/aos/atom_code/async_action/AsyncAction.h
new file mode 100644
index 0000000..31be206
--- /dev/null
+++ b/aos/atom_code/async_action/AsyncAction.h
@@ -0,0 +1,155 @@
+#ifndef _AOS_ASYNC_ACTION_H_
+#define _AOS_ASYNC_ACTION_H_
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <signal.h>
+
+#include <string>
+#include <map>
+#include <type_traits>
+
+#include "aos/aos_core.h"
+#include "aos/common/type_traits.h"
+
+class AsyncActionTest;
+
+namespace aos {
+ // S is status type T is parameter type
+
+ template<class S> struct async_action_status {
+ S status;
+ uint8_t done_status; // 1 = valid status, 2 = done
+ uint16_t count;
+ pid_t pid;
+ aos_resource_entity *resource_entity;
+ };
+ template<class T> struct async_action_start {
+ T args;
+ uint16_t count;
+ aos_resource_entity *parent;
+ };
+
+ class AsyncActionRunner;
+ class ResourceAction_t;
+
+ class AsyncActionStatics {
+ friend class ResourceAction_t; // a testing AsyncAction that has to mess with resource stuff
+ protected:
+ class stopexception : public std::exception {
+ virtual const char* what() const throw() {
+ return "This exception indicates that the AsyncAction was stopped. This message should never show up anywhere.";
+ }
+ public:
+ stopexception() : std::exception() {}
+ };
+ class resourceexception : public std::exception {
+ virtual const char* what() const throw() {
+ return "This exception indicates that the AsyncAction was stopped due to resource contention. This message should never show up anywhere.";
+ }
+ public:
+ resourceexception() : std::exception() {}
+ };
+
+ // 1 = stop current DoAction
+ // 2 = stop corrent DoAction because of resource issues
+ // 4 = SIGINT pending (nicely close everything down first)
+ static volatile uint8_t interrupt;
+ static const int STOP_SIGNAL;
+ static aos_resource_entity *resource_entity;
+
+ // throw any exceptions indicated by signals
+ static inline void CheckStop(){
+ if(interrupt & (0x01 | 0x04)) {
+ throw stopexception();
+ fprintf(stderr, "the code should never ever get here (%s: %d)\n", __FILE__, __LINE__);
+ }else if(interrupt & 0x02) {
+ throw resourceexception();
+ fprintf(stderr, "the code should never ever get here (%s: %d)\n", __FILE__, __LINE__);
+ }
+ }
+ };
+
+ // S and T have to be structs
+ // T is sTart and S is status
+ // public functions (except for constructor) should be called on this AsyncAction
+ // in processes other than the one Run ing this AsyncAction
+ // vice versa for protected ones
+ template<class T, class S> class AsyncAction : public AsyncActionStatics {
+ static_assert(shm_ok<async_action_start<T>>::value,
+ "T must go through shared memory");
+ static_assert(shm_ok<async_action_status<S>>::value,
+ "S must go through shared memory");
+ friend class AsyncActionRunner;
+ friend class ::AsyncActionTest;
+ public:
+ AsyncAction(const std::string name);
+
+ // returns what the count will be throughout this run
+ // return of 0 indicates error (didn't start)
+ uint16_t Start(T &args);
+
+ // -1 for count means use the static one
+ // aka !IsRunning()
+ bool IsDone(int32_t count = -1);
+ // returns which one it joined
+ uint16_t Join(int32_t count = -1);
+ // return is whether there is an actual status or just garbage
+ bool GetStatus(S &status_out, int32_t count = -1) __attribute__ ((warn_unused_result));
+ // waits for a new good status
+ bool GetNextStatus(S &status_out, int32_t count = -1) __attribute__ ((warn_unused_result));
+
+ void Stop(int32_t count = -1);
+ protected:
+ // starts infinite loop that waits for Starts
+ // returns 0 for success, negative for error
+ // gets called by generated code
+ int Run(uint8_t priority);
+
+ virtual void DoAction(T &args);
+ // should only be called from DoAction
+ void PostStatus(S &status_in);
+ void RequestResource(uint16_t resource);
+ // returns whether succeeded
+ bool TryRequestResource(uint16_t resource);
+ void ReleaseResource(uint16_t resource);
+
+ // called at the beginning and end of Run
+ virtual void OnStart() {}
+ virtual void OnEnd() {}
+
+ // this should be the only sleep (or variant thereof) that gets called
+ void Sleep(double seconds);
+ private:
+ aos_queue *status_queue, *start_queue;
+
+ uint16_t local_count;
+ S local_status;
+ uint8_t local_done_status;
+ pid_t local_pid;
+
+ template<int (*O)(aos_resource_entity *, aos_resource *)> bool ResourceOp(uint16_t resource);
+ std::map<uint16_t, uint8_t> resources;
+
+ // for read_msg_index
+ int done_index, status_index;
+ uint16_t next_status_count; // uses it to figure out when to reset status_index
+
+ // return the default if appropriate
+ inline uint16_t GetCount(int32_t in){
+ if(in < 0)
+ return local_count;
+ else
+ return (uint16_t)in;
+ }
+
+ static void sig_action(int, siginfo_t *, void *);
+ };
+
+} // namespace aos
+
+#include "AsyncAction.cpp.inc" // to make the template stuff work
+
+#endif
diff --git a/aos/atom_code/async_action/AsyncActionHandle.h b/aos/atom_code/async_action/AsyncActionHandle.h
new file mode 100644
index 0000000..8088848
--- /dev/null
+++ b/aos/atom_code/async_action/AsyncActionHandle.h
@@ -0,0 +1,17 @@
+#ifndef __AOS__ASYNC_ACTION_HANDLE_H_
+#define __AOS__ASYNC_ACTION_HANDLE_H_
+
+namespace aos {
+
+ class AsyncActionHandle {
+ public:
+ virtual bool IsDone() = 0;
+ virtual uint16_t Join() = 0;
+ virtual uint16_t Join(int32_t count) = 0;
+ virtual void Stop() = 0;
+ virtual void Stop(int32_t count) = 0;
+ };
+
+} // namespace aos
+
+#endif
diff --git a/aos/atom_code/async_action/AsyncActionRunner.h b/aos/atom_code/async_action/AsyncActionRunner.h
new file mode 100644
index 0000000..148d13a
--- /dev/null
+++ b/aos/atom_code/async_action/AsyncActionRunner.h
@@ -0,0 +1,15 @@
+#ifndef __AOS_ASYNC_ACTION_RUNNER_H_
+#define __AOS_ASYNC_ACTION_RUNNER_H_
+
+#include "aos/atom_code/async_action/AsyncAction.h"
+
+namespace aos {
+ class AsyncActionRunner {
+ public:
+ template<class T, class S> inline static int Run(AsyncAction<T, S> &action, uint8_t priority) {
+ return action.Run(priority);
+ }
+ };
+}
+
+#endif
diff --git a/aos/atom_code/async_action/AsyncAction_real.cpp b/aos/atom_code/async_action/AsyncAction_real.cpp
new file mode 100644
index 0000000..9a0be1d
--- /dev/null
+++ b/aos/atom_code/async_action/AsyncAction_real.cpp
@@ -0,0 +1,10 @@
+#include "AsyncAction.h"
+
+using namespace aos;
+
+volatile uint8_t AsyncActionStatics::interrupt = 0;
+
+const int AsyncActionStatics::STOP_SIGNAL = SIGRTMIN + 4;
+
+aos_resource_entity *AsyncActionStatics::resource_entity = NULL;
+
diff --git a/aos/atom_code/async_action/AsyncAction_test.cpp b/aos/atom_code/async_action/AsyncAction_test.cpp
new file mode 100644
index 0000000..bdd4002
--- /dev/null
+++ b/aos/atom_code/async_action/AsyncAction_test.cpp
@@ -0,0 +1,115 @@
+#include "TestAction.h"
+#include "ResourceAction.h"
+extern "C"{
+#include <resource_internal.h>
+}
+
+#include <gtest/gtest.h>
+
+#include "sharedmem_test_setup.h"
+
+using namespace aos;
+
+class AsyncActionTest : public ExecVeTestSetup{
+ protected:
+ virtual void SetUp(){
+ AddProcess("../bin/TestAction");
+ AddProcess("../bin/ResourceAction");
+ //AddProcess("/home/brians/bin/wait_5s.sh");
+ AddProcess("../bin/ResourceActionChild");
+
+ ExecVeTestSetup::SetUp();
+ }
+
+ template<class H, class S, class T> bool HasResource(const uint16_t resource, H &handle){
+ /*bool r = AOS_RESOURCE_STATE_GET_HAS_IT(resource, handle.GetInstance().resource_entity);
+ EXPECT_EQ(r, handle.GetInstance().resources.count(resource)) << "the AsyncAction doesn't know whether it has resource " << resource << " or not";
+ return r;*/
+
+ /*const AsyncAction<S, T> &action = handle.GetInstance();
+ async_action_start<S> *start = (async_action_start<S> *)aos_queue_read_message(action.start_queue, PEEK | NON_BLOCK);
+ ASSERT_NE(NULL, start);
+ bool r = AOS_RESOURCE_STATE_GET_HAS_IT(resource, start->parent);
+ aos_queue_free_msg(action.start_queue, start);
+ return r;*/
+
+ AsyncAction<S, T> &action = (AsyncAction<S, T> &)handle.GetInstance();
+ async_action_status<T> *status = (async_action_status<T> *)aos_queue_read_msg(action.status_queue, PEEK | NON_BLOCK);
+ EXPECT_TRUE(status != NULL) << "if this failed, we're going to segfault next";
+ bool r = AOS_RESOURCE_STATE_GET_HAS_IT(resource, status->resource_entity);
+ aos_queue_free_msg(action.status_queue, status);
+ return r;
+ }
+
+ // tests from the google doc (https://docs.google.com/document/d/1gzRrVcqL2X9VgNQUI5DrvLVVVziIH7c5ZerATVbiS7U/edit?hl=en_US) (referenced by the numbers in the drawing)
+ // up here so they can be called with multiple inputs and checked for correct outputs
+ // return = number of sub-action call it failed at (1 = top level one etc) (0 = succeeded)
+ int GoogleDocTest1(uint8_t in_state){
+ return ResourceAction.Execute(0x01, 0x01, 1, in_state).failed;
+ }
+
+ virtual void TearDown(){
+ TestAction.Free();
+ LeftDrive.Free();
+ ResourceAction.Free();
+ ResourceAction.Free();
+
+ ExecVeTestSetup::TearDown();
+ }
+};
+
+TEST_F(AsyncActionTest, StartStop){
+ TestAction.Start(5, 3);
+ TestAction.Stop();
+ PercolatePause();
+ EXPECT_TRUE(TestAction.IsDone());
+}
+TEST_F(AsyncActionTest, AlternateName){
+ EXPECT_FALSE(LeftDrive.IsDone());
+}
+
+TEST_F(AsyncActionTest, Join){
+ TestAction.Start(0.1, 3);
+ EXPECT_FALSE(TestAction.IsDone());
+ TestAction.Join();
+ EXPECT_TRUE(TestAction.IsDone());
+}
+TEST_F(AsyncActionTest, JoinAgain){
+ uint16_t instance = TestAction.Start(0.1, 3);
+ TestAction.Join();
+ EXPECT_TRUE(TestAction.IsDone());
+ timespec ts;
+ clock_gettime(CLOCK_MONOTONIC, &ts);
+ TestAction.Join(instance);
+ timespec ts2;
+ clock_gettime(CLOCK_MONOTONIC, &ts2);
+ long diff = ts2.tv_nsec - ts.tv_nsec;
+ diff += (ts2.tv_sec - ts.tv_sec) * 1000000000;
+ EXPECT_LT(diff, 50000);
+}
+TEST_F(AsyncActionTest, Execute){
+ TestAction.Execute(0.06, 3);
+ EXPECT_TRUE(TestAction.IsDone());
+}
+
+TEST_F(AsyncActionTest, Release){
+ TestAction.Start(0.1, -4);
+ while(!TestAction.IsDone()){
+ if(TestAction.GetNextStatus())
+ EXPECT_EQ(TestAction.GetLastStatus().loops > 2, !(HasResource<TestActionHandle, testing_status, testing_args>(test_resource1, TestAction)));
+ }
+}
+TEST_F(AsyncActionTest, ReleaseBad){
+ TestAction.Start(-0.01, 5);
+}
+TEST_F(AsyncActionTest, ImplicitRelease){
+ TestAction.Execute(0.02, 4);
+ EXPECT_FALSE((HasResource<TestActionHandle, testing_status, testing_args>(test_resource1, TestAction)));
+}
+
+//TODO test killing or not based on priority (and inherited priority...)
+
+TEST_F(AsyncActionTest, GoogleDoc111){
+ EXPECT_EQ(3, GoogleDocTest1(3));
+}
+
diff --git a/aos/atom_code/async_action/ResourceAction.act b/aos/atom_code/async_action/ResourceAction.act
new file mode 100644
index 0000000..46e22fb
--- /dev/null
+++ b/aos/atom_code/async_action/ResourceAction.act
@@ -0,0 +1,13 @@
+args resource_args {
+ // 1 = request, 2 = release, 4 = child run, 8 = child request, 16 = child release, all others 0
+ uint8_t first;
+ uint8_t second;
+
+ int number; // top level is 1
+
+ uint8_t state_to_set; // first 2 bits are this one, next 2 bits are for parent
+};
+status resource_status {
+ int failed; // 0 means none have failed yet
+};
+priority 100;
diff --git a/aos/atom_code/async_action/ResourceActionChild.act b/aos/atom_code/async_action/ResourceActionChild.act
new file mode 100644
index 0000000..5e8226a
--- /dev/null
+++ b/aos/atom_code/async_action/ResourceActionChild.act
@@ -0,0 +1,8 @@
+args resource_child_args{
+ // 1 = request, 2 = release
+ uint8_t actions;
+}
+status resource_child_status{
+ bool success;
+}
+priority 70;
diff --git a/aos/atom_code/async_action/ResourceActionChild_t.cpp b/aos/atom_code/async_action/ResourceActionChild_t.cpp
new file mode 100644
index 0000000..c1a2601
--- /dev/null
+++ b/aos/atom_code/async_action/ResourceActionChild_t.cpp
@@ -0,0 +1,13 @@
+#include "ResourceActionChild.h"
+
+void ResourceActionChild_t::DoAction(uint8_t actions){
+ if(actions & 0x01)
+ if(TryRequestResource(test_resource1)){
+ PostStatus(false);
+ return;
+ }
+ if(actions & 0x02)
+ ReleaseResource(test_resource1);
+ PostStatus(true);
+}
+
diff --git a/aos/atom_code/async_action/ResourceAction_t.cpp b/aos/atom_code/async_action/ResourceAction_t.cpp
new file mode 100644
index 0000000..99d9c7e
--- /dev/null
+++ b/aos/atom_code/async_action/ResourceAction_t.cpp
@@ -0,0 +1,40 @@
+#define AOS_ResourceAction_t_HEADER_FRAG int DoSub(uint8_t slot, int in_number);
+
+#include "ResourceAction.h"
+#include "ResourceActionChild.h"
+extern "C"{
+#include <resource_internal.h>
+}
+
+int ResourceAction_t::DoSub(uint8_t slot, int in_number){
+ if(slot & 0x01)
+ if(TryRequestResource(test_resource1)){
+ PostStatus(in_number + 1);
+ return 0;
+ }
+ if(slot & 0x02)
+ ReleaseResource(test_resource1);
+ if(slot & 0x04){
+ if(!ResourceActionChild.Execute(slot << 3).success){
+ PostStatus(in_number + 2);
+ return 0;
+ }
+ }
+ return in_number + ((slot & (0x01 | 0x02)) ? 1 : 0) + ((slot & 0x04) ? 1 : 0);
+}
+void ResourceAction_t::DoAction(uint8_t first, uint8_t second, int number, uint8_t state_to_set){
+ printf("start of ResourceAction.DoAction\n");
+ AOS_RESOURCE_STATE_SET_ON((AOS_RESOURCE_STATE_WANTS_IT | AOS_RESOURCE_STATE_HAS_PASSED_DOWN) & state_to_set, test_resource1, ::AsyncActionStatics::resource_entity);
+ AOS_RESOURCE_STATE_SET_OFF((AOS_RESOURCE_STATE_WANTS_IT | AOS_RESOURCE_STATE_HAS_PASSED_DOWN) & state_to_set, test_resource1, ::AsyncActionStatics::resource_entity);
+ AOS_RESOURCE_STATE_SET_ON((AOS_RESOURCE_STATE_WANTS_IT | AOS_RESOURCE_STATE_HAS_PASSED_DOWN) & (state_to_set >> 2), test_resource1, aos_resource_entity_root_get());
+ AOS_RESOURCE_STATE_SET_OFF((AOS_RESOURCE_STATE_WANTS_IT | AOS_RESOURCE_STATE_HAS_PASSED_DOWN) & state_to_set >> 2, test_resource1, aos_resource_entity_root_get());
+ printf("set state\n");
+
+ number = DoSub(first, number);
+ printf("did first\n");
+ if(number == 0) // error
+ return;
+ number = DoSub(second, number);
+ printf("did second\n");
+}
+
diff --git a/aos/atom_code/async_action/TestAction.act b/aos/atom_code/async_action/TestAction.act
new file mode 100644
index 0000000..96c940b
--- /dev/null
+++ b/aos/atom_code/async_action/TestAction.act
@@ -0,0 +1,13 @@
+args testing_args {
+ double seconds;
+ int loops;
+};
+status testing_status {
+ int loops;
+ double total_seconds;
+};
+async_queue LeftDrive;
+async_queue TestAction;
+//has OnEnd;
+//has OnStart;
+priority 50;
diff --git a/aos/atom_code/async_action/TestAction_t.cpp b/aos/atom_code/async_action/TestAction_t.cpp
new file mode 100644
index 0000000..1e99529
--- /dev/null
+++ b/aos/atom_code/async_action/TestAction_t.cpp
@@ -0,0 +1,35 @@
+#include "TestAction.h"
+#include <AsyncActionRunner.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+
+//void TestingAction_t::OnStart() {printf("started\n");}
+//void TestingAction_t::OnEnd() {printf("ended\n");}
+void TestAction_t::DoAction(double seconds, int loops) {
+ printf("start of DoAction\n");
+ RequestResource(test_resource1);
+ bool release = loops < 0;
+ if(release)
+ loops *= -1;
+ bool release_bad = seconds < 0;
+ if(release_bad)
+ seconds *= -1;
+ int i;
+ for(i = 0; i < loops; ++i) {
+ Sleep(seconds);
+ printf("posting for loop %d\n", i);
+ PostStatus(i, seconds * i);
+ printf("posted for loop %d\n", i);
+ if(release && i > loops / 2){
+ printf("releasing resource\n");
+ ReleaseResource(test_resource1);
+ }
+ if(release_bad && i > loops / 2){
+ printf("releasing resource which has not been requested\n");
+ ReleaseResource(test_resource2);
+ }
+ }
+ printf("end of DoAction\n");
+}
+
diff --git a/aos/atom_code/async_action/async_action.gyp b/aos/atom_code/async_action/async_action.gyp
new file mode 100644
index 0000000..5f5bf55
--- /dev/null
+++ b/aos/atom_code/async_action/async_action.gyp
@@ -0,0 +1,13 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'AsyncAction_test',
+ 'type': 'executable',
+ 'sources': [
+ 'AsyncACtion_test.cpp',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ ],
+ ],
+},
diff --git a/aos/atom_code/camera/Buffers.cpp b/aos/atom_code/camera/Buffers.cpp
new file mode 100644
index 0000000..22b7337
--- /dev/null
+++ b/aos/atom_code/camera/Buffers.cpp
@@ -0,0 +1,160 @@
+#include "Buffers.h"
+#include "V4L2.h"
+
+#include <sys/mman.h>
+
+namespace aos {
+namespace camera {
+
+// Represents an actual v4l2 buffer.
+struct Buffers::Buffer {
+ void *start;
+ size_t length; // for munmap
+};
+const std::string Buffers::kFDServerName("/tmp/aos_fd_server");
+const std::string Buffers::kQueueName("CameraBufferQueue");
+const aos_type_sig Buffers::kSignature{sizeof(Message), 971, 1};
+
+int Buffers::CreateSocket(int (*bind_connect)(int, const sockaddr *, socklen_t)) {
+ union af_unix_sockaddr {
+ sockaddr_un un;
+ sockaddr addr;
+ } addr;
+ const int r = socket(AF_UNIX, SOCK_STREAM, 0);
+ if (r == -1) {
+ LOG(ERROR, "socket(AF_UNIX, SOCK_STREAM, 0) failed with %d: %s\n",
+ errno, strerror(errno));
+ return -1;
+ }
+ addr.un.sun_family = AF_UNIX;
+ memset(addr.un.sun_path, 0, sizeof(addr.un.sun_path));
+ strcpy(addr.un.sun_path, kFDServerName.c_str());
+ if (bind_connect(r, &addr.addr, sizeof(addr.un)) == -1) {
+ LOG(ERROR, "bind_connect(=%p)(%d, %p, %zd) failed with %d: %s\n",
+ bind_connect, r, &addr.addr, sizeof(addr.un), errno, strerror(errno));
+ close(r); // what are we going to do about an error?
+ return -1;
+ }
+ return r;
+}
+
+void Buffers::MMap() {
+ buffers_ = new Buffer[kNumBuffers];
+ v4l2_buffer buf;
+ for (unsigned int n = 0; n < kNumBuffers; ++n) {
+ memset(&buf, 0, sizeof(buf));
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = n;
+ if (xioctl(fd_, VIDIOC_QUERYBUF, &buf) == -1) {
+ LOG(FATAL, "ioctl VIDIOC_QUERYBUF(%d, %p) failed with %d: %s\n",
+ fd_, &buf, errno, strerror(errno));
+ }
+ buffers_[n].length = buf.length;
+ buffers_[n].start = mmap(NULL, buf.length,
+ PROT_READ | PROT_WRITE, MAP_SHARED,
+ fd_, buf.m.offset);
+ if (buffers_[n].start == MAP_FAILED) {
+ LOG(FATAL, "mmap(NULL, %zd, PROT_READ | PROT_WRITE, MAP_SHARED, %d, %jd)"
+ " failed with %d: %s\n", buf.length, fd_, static_cast<intmax_t>(buf.m.offset),
+ errno, strerror(errno));
+ }
+ }
+}
+
+void Buffers::Release() {
+ if (message_ != NULL) {
+ aos_queue_free_msg(queue_, message_);
+ message_ = NULL;
+ }
+}
+const void *Buffers::GetNext(bool block,
+ uint32_t *bytesused, timeval *timestamp, uint32_t *sequence) {
+ Release();
+
+ // TODO(brians) make sure the camera reader process hasn't died
+ do {
+ if (block) {
+ message_ = static_cast<const Message *>(aos_queue_read_msg(queue_, PEEK | BLOCK));
+ } else {
+ static int index = 0;
+ message_ = static_cast<const Message *>(aos_queue_read_msg_index(queue_, BLOCK,
+ &index));
+ }
+ } while (block && message_ == NULL);
+ if (message_ != NULL) {
+ if (bytesused != NULL) memcpy(bytesused, &message_->bytesused, sizeof(*bytesused));
+ if (timestamp != NULL) memcpy(timestamp, &message_->timestamp, sizeof(*timestamp));
+ if (sequence != NULL) memcpy(sequence, &message_->sequence, sizeof(*sequence));
+ return buffers_[message_->index].start;
+ } else {
+ return NULL;
+ }
+}
+
+int Buffers::FetchFD() {
+ int myfds[Buffers::kNumFDs]; // where to retrieve the fds into
+ char buf[CMSG_SPACE(sizeof(myfds))]; // ancillary data buffer
+
+ iovec data;
+ memset(&data, 0, sizeof(data));
+ char dummy;
+ data.iov_base = &dummy;
+ data.iov_len = sizeof(dummy);
+ msghdr msg;
+ memset(&msg, 0, sizeof(msg));
+ msg.msg_iov = &data;
+ msg.msg_iovlen = 1;
+ msg.msg_control = buf;
+ msg.msg_controllen = sizeof(buf);
+
+ switch (recvmsg(server_, &msg, 0)) {
+ case 0: // "the peer has performed an orderly shutdown"
+ LOG(FATAL, "the fd server shut down (connected on %d)\n", server_);
+ case -1:
+ LOG(FATAL, "recvmsg(server_(=%d), %p, 0) failed with %d: %s\n",
+ server_, &msg, errno, strerror(errno));
+ }
+ const cmsghdr *const cmsg = CMSG_FIRSTHDR(&msg);
+ if (cmsg == NULL) {
+ LOG(FATAL, "no headers in message\n");
+ }
+ if (cmsg->cmsg_len != CMSG_LEN(sizeof(myfds))) {
+ LOG(FATAL, "got wrong size. got %d but expected %zd\n",
+ cmsg->cmsg_len, CMSG_LEN(sizeof(myfds)));
+ }
+ if (cmsg->cmsg_level != SOL_SOCKET) {
+ LOG(FATAL, "cmsg_level=%d. expected SOL_SOCKET(=%d)\n", cmsg->cmsg_level, SOL_SOCKET);
+ }
+ if (cmsg->cmsg_type != SCM_RIGHTS) {
+ LOG(FATAL, "cmsg_type=%d. expected SCM_RIGHTS(=%d)\n", cmsg->cmsg_type, SCM_RIGHTS);
+ }
+ memcpy(myfds, CMSG_DATA(cmsg), sizeof(myfds));
+
+ return myfds[0];
+}
+Buffers::Buffers() : server_(CreateSocket(connect)), fd_(FetchFD()), message_(NULL) {
+ MMap();
+ queue_ = aos_fetch_queue(kQueueName.c_str(), &kSignature);
+}
+
+Buffers::~Buffers() {
+ Release();
+
+ for (unsigned i = 0; i < kNumBuffers; ++i) {
+ if (munmap(buffers_[i].start, buffers_[i].length) == -1) {
+ LOG(WARNING, "munmap(%p, %zd) for destruction failed with %d: %s\n",
+ buffers_[i].start, buffers_[i].length, errno, strerror(errno));
+ }
+ }
+ delete[] buffers_;
+
+ if (close(fd_) == -1) {
+ LOG(WARNING, "close(%d) for destruction failed with %d: %s\n",
+ fd_, errno, strerror(errno));
+ }
+}
+
+} // namespace camera
+} // namespace aos
+
diff --git a/aos/atom_code/camera/Buffers.h b/aos/atom_code/camera/Buffers.h
new file mode 100644
index 0000000..7f1206d
--- /dev/null
+++ b/aos/atom_code/camera/Buffers.h
@@ -0,0 +1,93 @@
+#ifndef AOS_ATOM_CODE_CAMERA_CAMERA_BUFFERS_H_
+#define AOS_ATOM_CODE_CAMERA_CAMERA_BUFFERS_H_
+
+#include <sys/socket.h>
+#include <sys/un.h>
+
+#include <string>
+
+#include "aos/aos_core.h"
+#include "aos/common/type_traits.h"
+
+namespace aos {
+namespace camera {
+
+class Reader;
+class Buffers {
+ // It has to do a lot of the same things as all the other ones, but it gets
+ // the information from different places (some of it gets sent out by it).
+ friend class Reader;
+ // Not an abstract name so that an existing one can just be unlinked without
+ // disturbing it if necessary (like with shm_link).
+ static const std::string kFDServerName;
+ // How many v4l2 buffers and other things that depend on that.
+ static const unsigned int kNumBuffers = 10;
+ // How many fds to transfer from the fd server.
+ // Used to make it clear which 1s are 1 and which are this.
+ static const size_t kNumFDs = 1;
+ // Creates a socket and calls either bind or connect.
+ // Returns a bound/connected socket or -1 (the reason for which will already
+ // have been logged at ERROR).
+ static int CreateSocket(int (*bind_connect)(int, const sockaddr *, socklen_t));
+
+ // File descriptor connected to the fd server. Used for detecting if the
+ // camera reading process has died.
+ // A value of -2 means don't check.
+ const int server_;
+ // Gets the fd (using server_).
+ int FetchFD();
+ // File descriptor for the v4l2 device (that's valid in this process of
+ // course).
+ const int fd_;
+
+ struct Buffer;
+ // Buffer[kNumBuffers]
+ Buffer *buffers_;
+ struct Message {
+ uint32_t index;
+ uint32_t bytesused;
+ timeval timestamp;
+ uint32_t sequence;
+ };
+ static_assert(shm_ok<Message>::value, "it's going through queues");
+ // The current one. Sometimes NULL.
+ const Message *message_;
+ static const std::string kQueueName;
+ static const aos_type_sig kSignature;
+ // NULL for the Reader one.
+ aos_queue *queue_;
+ // Make the actual mmap calls.
+ // Called by Buffers() automatically.
+ void MMap();
+ public:
+ Buffers();
+ // Will clean everything up.
+ // So that HTTPStreamer can create/destroy one for each client to make it
+ // simpler.
+ ~Buffers();
+
+ // Retrieves the next image. Will return the current one if it hasn't yet.
+ // Calls Release() at the beginning.
+ // NOTE: this means that the caller can't keep using references to the old
+ // return value after calling this function again
+ // block is whether to return NULL or wait for a new one
+ // the last 3 output parameters will be filled in straight from the
+ // v4l2_buffer if they're not NULL
+ // (see <http://v4l2spec.bytesex.org/spec/x5953.htm#V4L2-BUFFER> for details)
+ // NOTE: guaranteed to return a valid pointer if block is true
+ const void *GetNext(bool block,
+ uint32_t *bytesused, timeval *timestamp, uint32_t *sequence);
+ // Releases the most recent frame buffer. Automatically called by GetNext and
+ // the destructor. Safe to call multiple times without getting frames in
+ // between.
+ void Release();
+
+ // How big images are.
+ static const int32_t kWidth = 640, kHeight = 480;
+};
+
+} // namespace camera
+} // namespace aos
+
+#endif
+
diff --git a/aos/atom_code/camera/HTTPStreamer.cpp b/aos/atom_code/camera/HTTPStreamer.cpp
new file mode 100644
index 0000000..ad339a8
--- /dev/null
+++ b/aos/atom_code/camera/HTTPStreamer.cpp
@@ -0,0 +1,388 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <malloc.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <sys/mman.h>
+#include <netinet/ip.h>
+#include <sys/socket.h>
+#include <inttypes.h>
+
+#include <vector>
+
+#include "aos/common/Configuration.h"
+#include "aos/aos_core.h"
+#include "aos/atom_code/camera/Buffers.h"
+
+namespace aos {
+namespace camera {
+
+namespace {
+
+// doesn't like being a static class member
+static const unsigned char dht_table[] = {
+ 0xff, 0xc4, 0x01, 0xa2, 0x00, 0x00, 0x01, 0x05, 0x01, 0x01, 0x01, 0x01,
+ 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02,
+ 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x01, 0x00, 0x03,
+ 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09,
+ 0x0a, 0x0b, 0x10, 0x00, 0x02, 0x01, 0x03, 0x03, 0x02, 0x04, 0x03, 0x05,
+ 0x05, 0x04, 0x04, 0x00, 0x00, 0x01, 0x7d, 0x01, 0x02, 0x03, 0x00, 0x04,
+ 0x11, 0x05, 0x12, 0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07, 0x22,
+ 0x71, 0x14, 0x32, 0x81, 0x91, 0xa1, 0x08, 0x23, 0x42, 0xb1, 0xc1, 0x15,
+ 0x52, 0xd1, 0xf0, 0x24, 0x33, 0x62, 0x72, 0x82, 0x09, 0x0a, 0x16, 0x17,
+ 0x18, 0x19, 0x1a, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2a, 0x34, 0x35, 0x36,
+ 0x37, 0x38, 0x39, 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a,
+ 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5a, 0x63, 0x64, 0x65, 0x66,
+ 0x67, 0x68, 0x69, 0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7a,
+ 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8a, 0x92, 0x93, 0x94, 0x95,
+ 0x96, 0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa8,
+ 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba, 0xc2,
+ 0xc3, 0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4, 0xd5,
+ 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe1, 0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7,
+ 0xe8, 0xe9, 0xea, 0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9,
+ 0xfa, 0x11, 0x00, 0x02, 0x01, 0x02, 0x04, 0x04, 0x03, 0x04, 0x07, 0x05,
+ 0x04, 0x04, 0x00, 0x01, 0x02, 0x77, 0x00, 0x01, 0x02, 0x03, 0x11, 0x04,
+ 0x05, 0x21, 0x31, 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71, 0x13, 0x22,
+ 0x32, 0x81, 0x08, 0x14, 0x42, 0x91, 0xa1, 0xb1, 0xc1, 0x09, 0x23, 0x33,
+ 0x52, 0xf0, 0x15, 0x62, 0x72, 0xd1, 0x0a, 0x16, 0x24, 0x34, 0xe1, 0x25,
+ 0xf1, 0x17, 0x18, 0x19, 0x1a, 0x26, 0x27, 0x28, 0x29, 0x2a, 0x35, 0x36,
+ 0x37, 0x38, 0x39, 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a,
+ 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5a, 0x63, 0x64, 0x65, 0x66,
+ 0x67, 0x68, 0x69, 0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7a,
+ 0x82, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8a, 0x92, 0x93, 0x94,
+ 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7,
+ 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba,
+ 0xc2, 0xc3, 0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4,
+ 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7,
+ 0xe8, 0xe9, 0xea, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa
+};
+
+const char kFirstHeader[] = "HTTP/1.0 200 OK\r\n"
+"Connection: close\r\n"
+"Server: AOS/0.0 Camera\r\n"
+"Cache-Control: no-store, no-cache, must-revalidate, pre-check=0, "
+"post-check=0, max-age=0\r\n" // this and above from mjpg-streamer
+"Pragma: no-cache\r\n"
+"Expires: Mon, 3 Jan 2000 12:34:56 GMT\r\n" // also from mjpg-streamer
+"Content-Type: multipart/x-mixed-replace; boundary=boundarydonotcross\r\n";
+
+} // namespace
+
+class HTTPStreamer {
+ // Represents a single client. Handles all reading and writing of sockets and
+ // queues.
+ class Client {
+ enum class State {
+ kReadHeaders,
+ kWriteHeaders,
+ kWriteBoundary, // these last 2 loop to each other
+ kWriteImage,
+ kWriteDHT, // happens in the middle of kWriteImage
+ };
+ const int sock_;
+ State state_;
+ inline fd_set *GetFDSetForCurrentState(fd_set *read_fds,
+ fd_set *write_fds) {
+ if (state_ == State::kReadHeaders) {
+ return read_fds;
+ } else {
+ return write_fds;
+ }
+ }
+ // MUST BE LONG ENOUGH TO HOLD kBoundaryText WITH A BIGGISH # IN IT
+ char scratch_[4096];
+ int to_write_;
+ int zero_reads_;
+ static const int kMaxZeroReads = 2000;
+ size_t pos_, dht_pos_, dht_start_;
+ Buffers buffers_;
+ const void *current_;
+ uint32_t size_;
+
+ Client(const Client &);
+ void operator=(const Client &);
+
+ public:
+ explicit Client(int sock) : sock_(sock), state_(State::kReadHeaders),
+ zero_reads_(0), pos_(0) {}
+ ~Client() {
+ LOG(DEBUG, "closing socket %d\n", sock_);
+ if (close(sock_) == -1) {
+ LOG(INFO, "closing socket %d for destruction failed with %d: %s\n",
+ sock_, errno, strerror(errno));
+ }
+ }
+ // Set any fds necessary into the 2 arguments.
+ void FDSet(fd_set *read_fds, fd_set *write_fds) {
+ FD_SET(sock_, GetFDSetForCurrentState(read_fds, write_fds));
+ }
+ // The arguments are the same as the last FDSet call (after a successful
+ // select).
+ // Return value is whether or not to keep this one around.
+ bool Process(fd_set *read_fds, fd_set *write_fds) {
+ // if the socket we're waiting on isn't ready
+ if (!FD_ISSET(sock_, GetFDSetForCurrentState(read_fds, write_fds))) {
+ return true;
+ }
+
+ ssize_t ret;
+ switch (state_) {
+ case State::kReadHeaders:
+ if (pos_ >= sizeof(scratch_)) {
+ LOG(WARNING, "read too many bytes of headers on sock %d."
+ " somebody should increase the size of scratch_\n", sock_);
+ return false;
+ }
+ if (zero_reads_ > kMaxZeroReads) {
+ LOG(WARNING, "read 0 bytes %d times on sock %d. giving up\n",
+ zero_reads_, sock_);
+ return false;
+ }
+ ret = read(sock_, scratch_ + pos_, sizeof(scratch_) - pos_);
+ if (ret == -1) {
+ LOG(WARNING, "read(%d, %p, %zd) failed with %d: %s\n",
+ sock_, scratch_ + pos_, sizeof(scratch_) - pos_,
+ errno, strerror(errno));
+ return false;
+ }
+ pos_ += ret;
+ // if we just received \r\n\r\n (the end of the headers)
+ if (scratch_[pos_ - 4] == '\r' && scratch_[pos_ - 3] == '\n' &&
+ scratch_[pos_ - 2] == '\r' && scratch_[pos_ - 1] == '\n') {
+ LOG(INFO, "entering state kWriteHeaders"
+ " after %zd bytes of headers read\n", pos_ - 1);
+ pos_ = 0;
+ state_ = State::kWriteHeaders;
+ }
+ scratch_[pos_] = '\0';
+ if (ret == 0) {
+ ++zero_reads_;
+ } else {
+ zero_reads_ = 0;
+ LOG(DEBUG, "read %zd bytes of headers scratch_=%s\n",
+ ret, scratch_);
+ }
+ break;
+ case State::kWriteHeaders:
+ // this intentionally doesn't write the terminating \0 on the string
+ ret = write(sock_, kFirstHeader + pos_, sizeof(kFirstHeader) - pos_);
+ if (ret == -1) {
+ LOG(WARNING, "write(%d, %p, %zd) failed with %d: %s\n",
+ sock_, kFirstHeader + pos_, sizeof(kFirstHeader) - pos_,
+ errno, strerror(errno));
+ } else {
+ pos_ += ret;
+ if (pos_ >= sizeof(kFirstHeader)) {
+ current_ = NULL;
+ state_ = State::kWriteBoundary;
+ }
+ }
+ break;
+ case State::kWriteBoundary:
+ if (current_ == NULL) {
+ timeval timestamp;
+ current_ = buffers_.GetNext(false, &size_, ×tamp, NULL);
+
+ /*static int skip = 0;
+ if (current_ != NULL) skip = (skip + 1) % 30;
+ if (!skip) current_ = NULL;
+ if (current_ == NULL) break;*/
+
+#if 0
+ // set pos_ to where the first header starts
+ for (pos_ = 0; static_cast<const uint8_t *>(current_)[pos_] != 0xFF;
+ ++pos_);
+#else
+ pos_ = 0;
+#endif
+#if 0
+ // go through the frame looking for the start of frame marker
+ for (dht_start_ = 0;
+ static_cast<const uint8_t *>(current_)[dht_start_ + 0] !=
+ 0xFF &&
+ static_cast<const uint8_t *>(current_)[dht_start_ + 1] !=
+ 0xC0 &&
+ dht_start_ < size_; ++dht_start_)
+ printf("[%zd]=%"PRIx8" ", dht_start_,
+ static_cast<const uint8_t *>(current_)[dht_start_]);
+ if (dht_start_ >= size_) {
+ LOG(WARNING, "couldn't find start of frame marker\n");
+ return false;
+ }
+#else
+ dht_start_ = 0;
+#endif
+ dht_pos_ = 0;
+
+ // aos.ChannelImageGetter depends on the exact format of this
+ to_write_ = snprintf(scratch_, sizeof(scratch_),
+ "\r\n--boundarydonotcross\r\n"
+ "Content-Type: image/jpeg\r\n"
+ "Content-Length: %"PRId32"\r\n"
+ "X-Timestamp: %ld.%06ld\r\n"
+ "\r\n",
+ size_,
+ timestamp.tv_sec, timestamp.tv_usec);
+ }
+ ret = write(sock_, scratch_ + pos_, to_write_ - pos_);
+ if (ret == -1) {
+ LOG(WARNING, "write(%d, %p, %zd) failed with %d: %s\n",
+ sock_, scratch_ + pos_, to_write_ - pos_,
+ errno, strerror(errno));
+ return false;
+ } else {
+ pos_ += ret;
+ if (static_cast<ssize_t>(pos_) >= to_write_) {
+ pos_ = 0;
+ state_ = State::kWriteImage;
+ }
+ }
+ break;
+ case State::kWriteImage:
+ ret = write(sock_, static_cast<const char *>(current_) + pos_,
+ ((dht_start_ == 0) ? size_ : dht_start_) - pos_);
+ if (ret == -1) {
+ LOG(WARNING, "write(%d, %p, %zd) failed with %d: %s\n",
+ sock_, static_cast<const char *>(current_) + pos_,
+ ((dht_start_ == 0) ? size_ : dht_start_) - pos_,
+ errno, strerror(errno));
+ return false;
+ } else {
+ pos_ += ret;
+ if (dht_start_ == 0) {
+ if (pos_ >= size_) {
+ buffers_.Release();
+ current_ = NULL;
+ state_ = State::kWriteBoundary;
+ }
+ } else {
+ if (pos_ >= dht_start_) {
+ dht_start_ = 0;
+ state_ = State::kWriteDHT;
+ }
+ }
+ }
+ break;
+ case State::kWriteDHT:
+ ret = write(sock_, dht_table + dht_pos_,
+ sizeof(dht_table) - dht_pos_);
+ if (ret == -1) {
+ LOG(WARNING, "write(%d, %p, %zd) failed with %d: %s\n",
+ sock_, dht_table + dht_pos_, sizeof(dht_table) - dht_pos_,
+ errno, strerror(errno));
+ return false;
+ } else {
+ dht_pos_ += ret;
+ if (dht_pos_ >= sizeof(dht_table)) {
+ state_ = State::kWriteImage;
+ }
+ }
+ break;
+ default:
+ LOG(FATAL, "something weird happened\n");
+ }
+ return true;
+ }
+ };
+
+ const int bind_socket_;
+
+ public:
+ HTTPStreamer() : bind_socket_(socket(AF_INET, SOCK_STREAM, 0)) {
+ if (bind_socket_ < 0) {
+ LOG(FATAL, "socket(AF_INET, SOCK_STREAM, 0) failed with %d: %s\n",
+ errno, strerror(errno));
+ }
+
+ union {
+ sockaddr_in in;
+ sockaddr addr;
+ } bind_sockaddr;
+ memset(&bind_sockaddr, 0, sizeof(bind_sockaddr));
+ bind_sockaddr.in.sin_family = AF_INET;
+ bind_sockaddr.in.sin_port =
+ htons(static_cast<uint16_t>(aos::NetworkPort::kCameraStreamer));
+ bind_sockaddr.in.sin_addr.s_addr = htonl(INADDR_ANY);
+ int optval = 1;
+ setsockopt(bind_socket_, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));
+ if (bind(bind_socket_, &bind_sockaddr.addr,
+ sizeof(bind_sockaddr.addr)) == -1) {
+ LOG(FATAL, "bind(%d, %p) failed because of %d: %s\n",
+ bind_socket_, &bind_sockaddr.addr, errno, strerror(errno));
+ }
+
+ if (listen(bind_socket_, 10) == -1) {
+ LOG(FATAL, "listen(%d, 10) failed because of %d: %s\n", bind_socket_,
+ errno, strerror(errno));
+ }
+ const int flags = fcntl(bind_socket_, F_GETFL, 0);
+ if (flags == -1) {
+ LOG(FATAL, "fcntl(%d, F_GETFL, 0) failed because of %d: %s\n",
+ bind_socket_, errno, strerror(errno));
+ }
+ if (fcntl(bind_socket_, F_SETFL, flags | O_NONBLOCK) == -1) {
+ LOG(FATAL, "fcntl(%d, F_SETFL, %x) failed because of %d: %s\n",
+ bind_socket_, flags | O_NONBLOCK, errno, strerror(errno));
+ }
+ }
+ void Run() {
+ signal(SIGPIPE, SIG_IGN);
+
+ std::vector<Client *> clients;
+ fd_set read_fds, write_fds;
+ while (true) {
+ FD_ZERO(&read_fds);
+ FD_ZERO(&write_fds);
+ FD_SET(bind_socket_, &read_fds);
+ for (auto it = clients.begin(); it != clients.end(); ++it) {
+ (*it)->FDSet(&read_fds, &write_fds);
+ }
+ switch (select(FD_SETSIZE, &read_fds, &write_fds,
+ NULL, // err
+ NULL)) { // timeout
+ case -1:
+ LOG(ERROR, "select(FD_SETSIZE(=%d), %p, %p, NULL, NULL) failed"
+ " because of %d: %s\n", FD_SETSIZE, &read_fds, &write_fds,
+ errno, strerror(errno));
+ continue;
+ case 0:
+ LOG(ERROR, "select with NULL timeout timed out...\n");
+ continue;
+ }
+
+ if (FD_ISSET(bind_socket_, &read_fds)) {
+ const int sock = accept4(bind_socket_, NULL, NULL, SOCK_NONBLOCK);
+ if (sock == -1) {
+ LOG(ERROR, "accept4(%d, NULL, NULL, SOCK_NONBLOCK(=%d) failed"
+ " because of %d: %s\n",
+ bind_socket_, SOCK_NONBLOCK, errno, strerror(errno));
+ } else {
+ clients.push_back(new Client(sock));
+ }
+ }
+
+ std::vector<std::vector<Client *>::iterator> to_remove;
+ for (auto it = clients.begin(); it != clients.end(); ++it) {
+ if (!(*it)->Process(&read_fds, &write_fds)) {
+ to_remove.push_back(it);
+ delete *it;
+ }
+ }
+ for (auto it = to_remove.rbegin(); it != to_remove.rend(); ++it) {
+ LOG(INFO, "removing client\n");
+ clients.erase(*it);
+ }
+ }
+ }
+};
+
+} // namespace camera
+} // namespace aos
+
+AOS_RUN_NRT(aos::camera::HTTPStreamer)
diff --git a/aos/atom_code/camera/Reader.cpp b/aos/atom_code/camera/Reader.cpp
new file mode 100644
index 0000000..9cfee2a
--- /dev/null
+++ b/aos/atom_code/camera/Reader.cpp
@@ -0,0 +1,362 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <malloc.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <sys/mman.h>
+
+#include <string>
+#include <inttypes.h>
+
+#include "aos/aos_core.h"
+#include "aos/atom_code/camera/V4L2.h"
+#include "aos/atom_code/camera/Buffers.h"
+
+#define CLEAR(x) memset(&(x), 0, sizeof(x))
+
+namespace aos {
+namespace camera {
+
+class Reader {
+ static const char *const dev_name;
+
+ // of the camera
+ int fd_;
+ // the bound socket listening for fd requests
+ int server_fd_;
+
+ static const aos_type_sig kRecycleSignature;
+ aos_queue *queue_, *recycle_queue_;
+ // the number of buffers currently queued in v4l2
+ uint32_t queued_;
+ public:
+ Reader() {
+ struct stat st;
+ if (stat(dev_name, &st) == -1) {
+ LOG(FATAL, "Cannot identify '%s' because of %d: %s\n",
+ dev_name, errno, strerror(errno));
+ }
+ if (!S_ISCHR(st.st_mode)) {
+ LOG(FATAL, "%s is no device\n", dev_name);
+ }
+
+ fd_ = open(dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);
+ if (fd_ == -1) {
+ LOG(FATAL, "Cannot open '%s' because of %d: %s\n",
+ dev_name, errno, strerror(errno));
+ }
+
+ queue_ = aos_fetch_queue_recycle(Buffers::kQueueName.c_str(), &Buffers::kSignature,
+ &kRecycleSignature, &recycle_queue_);
+ // read off any existing recycled messages
+ while (aos_queue_read_msg(recycle_queue_, NON_BLOCK) != NULL);
+ queued_ = 0;
+
+ InitServer();
+ Init();
+ }
+ private:
+ void InitServer() {
+ if (unlink(Buffers::kFDServerName.c_str()) == -1 && errno != ENOENT) {
+ LOG(WARNING, "unlink(kFDServerName(='%s')) failed with %d: %s\n",
+ Buffers::kFDServerName.c_str(), errno, strerror(errno));
+ }
+ if ((server_fd_ = Buffers::CreateSocket(bind)) == -1) {
+ LOG(FATAL, "creating the IPC socket failed\n");
+ }
+ if (listen(server_fd_, 10) == -1) {
+ LOG(FATAL, "listen(%d, 10) failed with %d: %s\n",
+ server_fd_, errno, strerror(errno));
+ }
+ }
+ void SendFD(const int sock) {
+ int myfds[Buffers::kNumFDs]; /* Contains the file descriptors to pass. */
+ myfds[0] = fd_;
+ char buf[CMSG_SPACE(sizeof(myfds))]; /* ancillary data buffer */
+
+ iovec data;
+ memset(&data, 0, sizeof(data));
+ char dummy = 'A';
+ data.iov_base = &dummy;
+ data.iov_len = sizeof(dummy);
+ msghdr msg;
+ memset(&msg, 0, sizeof(msg));
+ msg.msg_iov = &data;
+ msg.msg_iovlen = 1;
+ msg.msg_control = buf;
+ msg.msg_controllen = sizeof(buf);
+ cmsghdr *const cmsg = CMSG_FIRSTHDR(&msg);
+ cmsg->cmsg_level = SOL_SOCKET;
+ cmsg->cmsg_type = SCM_RIGHTS;
+ cmsg->cmsg_len = CMSG_LEN(sizeof(myfds));
+ /* Initialize the payload: */
+ memcpy(CMSG_DATA(cmsg), myfds, sizeof(myfds));
+ if (sendmsg(sock, &msg, 0) == -1) {
+ LOG(ERROR, "sendmsg(%d, %p, 0) failed with %d: %s\n",
+ sock, &msg, errno, strerror(errno));
+ }
+ // leave it open so that the other end can tell if this process dies
+ }
+
+#if 0
+ // if we ever do want to do any of these things, this is how
+ void Stop() {
+ const v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_STREAMOFF, &type) == -1) {
+ errno_exit("VIDIOC_STREAMOFF");
+ }
+ }
+ void Close() {
+ if (close(fd_) == -1)
+ errno_exit("close");
+ fd_ = -1;
+ }
+#endif
+
+ void QueueBuffer(v4l2_buffer *buf) {
+ if (xioctl(fd_, VIDIOC_QBUF, buf) == -1) {
+ LOG(WARNING, "ioctl VIDIOC_QBUF(%d, %p) failed with %d: %s. losing buf #%"PRIu32"\n",
+ fd_, &buf, errno, strerror(errno), buf->index);
+ } else {
+ LOG(DEBUG, "put buf #%"PRIu32" into driver's queue\n", buf->index);
+ ++queued_;
+ }
+ }
+ void ReadFrame() {
+ v4l2_buffer buf;
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+
+ const Buffers::Message *read;
+ do {
+ read = static_cast<const Buffers::Message *>(
+ // we block waiting for one if we can't dequeue one without leaving
+ // the driver <= 2 (to be safe)
+ aos_queue_read_msg(recycle_queue_, (queued_ <= 2) ? BLOCK : NON_BLOCK));
+ if (read != NULL) {
+ buf.index = read->index;
+ aos_queue_free_msg(recycle_queue_, read);
+ QueueBuffer(&buf);
+ }
+ } while (read != NULL);
+
+ if (xioctl(fd_, VIDIOC_DQBUF, &buf) == -1) {
+ if (errno != EAGAIN) {
+ LOG(ERROR, "ioctl VIDIOC_DQBUF(%d, %p) failed with %d: %s\n",
+ fd_, &buf, errno, strerror(errno));
+ }
+ return;
+ }
+ --queued_;
+ if (buf.index >= Buffers::kNumBuffers) {
+ LOG(ERROR, "buf.index (%"PRIu32") is >= kNumBuffers (%u)\n",
+ buf.index, Buffers::kNumBuffers);
+ return;
+ }
+
+ Buffers::Message *const msg = static_cast<Buffers::Message *>(
+ aos_queue_get_msg(queue_));
+ if (msg == NULL) {
+ LOG(WARNING, "couldn't get a message to send buf #%"PRIu32" from queue %p."
+ " re-queueing now\n", buf.index, queue_);
+ QueueBuffer(&buf);
+ return;
+ }
+ msg->index = buf.index;
+ msg->bytesused = buf.bytesused;
+ memcpy(&msg->timestamp, &buf.timestamp, sizeof(msg->timestamp));
+ msg->sequence = buf.sequence;
+ if (aos_queue_write_msg_free(queue_, msg, OVERRIDE) == -1) {
+ LOG(WARNING, "sending message %p with buf #%"PRIu32" to queue %p failed."
+ " re-queueing now\n", msg, buf.index, queue_);
+ QueueBuffer(&buf);
+ return;
+ } else {
+ LOG(DEBUG, "sent message off to queue %p with buffer #%"PRIu32"\n",
+ queue_, buf.index);
+ }
+ }
+
+ void init_mmap() {
+ v4l2_requestbuffers req;
+ CLEAR(req);
+ req.count = Buffers::kNumBuffers;
+ req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ req.memory = V4L2_MEMORY_MMAP;
+ if (xioctl(fd_, VIDIOC_REQBUFS, &req) == -1) {
+ if (EINVAL == errno) {
+ LOG(FATAL, "%s does not support memory mapping\n", dev_name);
+ } else {
+ LOG(FATAL, "ioctl VIDIOC_REQBUFS(%d, %p) failed with %d: %s\n",
+ fd_, &req, errno, strerror(errno));
+ }
+ }
+ queued_ = Buffers::kNumBuffers;
+ if (req.count < Buffers::kNumBuffers) {
+ LOG(FATAL, "Insufficient buffer memory on %s\n", dev_name);
+ }
+ }
+
+ void Init() {
+ v4l2_capability cap;
+ if (xioctl(fd_, VIDIOC_QUERYCAP, &cap) == -1) {
+ if (EINVAL == errno) {
+ LOG(FATAL, "%s is no V4L2 device\n",
+ dev_name);
+ } else {
+ LOG(FATAL, "ioctl VIDIOC_QUERYCAP(%d, %p) failed with %d: %s\n",
+ fd_, &cap, errno, strerror(errno));
+ }
+ }
+ if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
+ LOG(FATAL, "%s is no video capture device\n",
+ dev_name);
+ }
+ if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
+ LOG(FATAL, "%s does not support streaming i/o\n",
+ dev_name);
+ }
+
+ /* Select video input, video standard and tune here. */
+
+ v4l2_cropcap cropcap;
+ CLEAR(cropcap);
+ cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_CROPCAP, &cropcap) == 0) {
+ v4l2_crop crop;
+ crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ crop.c = cropcap.defrect; /* reset to default */
+
+ if (xioctl(fd_, VIDIOC_S_CROP, &crop) == -1) {
+ switch (errno) {
+ case EINVAL:
+ /* Cropping not supported. */
+ break;
+ default:
+ /* Errors ignored. */
+ break;
+ }
+ }
+ } else {
+ /* Errors ignored. */
+ }
+
+ v4l2_format fmt;
+ CLEAR(fmt);
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ fmt.fmt.pix.width = Buffers::kWidth;
+ fmt.fmt.pix.height = Buffers::kHeight;
+ fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
+ fmt.fmt.pix.field = V4L2_FIELD_ANY;
+ //fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+ //fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
+ if (xioctl(fd_, VIDIOC_S_FMT, &fmt) == -1) {
+ LOG(FATAL, "ioctl VIDIC_S_FMT(%d, %p) failed with %d: %s\n",
+ fd_, &fmt, errno, strerror(errno));
+ }
+ /* Note VIDIOC_S_FMT may change width and height. */
+
+ /* Buggy driver paranoia. */
+ unsigned int min = fmt.fmt.pix.width * 2;
+ if (fmt.fmt.pix.bytesperline < min)
+ fmt.fmt.pix.bytesperline = min;
+ min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
+ if (fmt.fmt.pix.sizeimage < min)
+ fmt.fmt.pix.sizeimage = min;
+
+#if 0
+ // set framerate
+ struct v4l2_streamparm *setfps;
+ setfps = (struct v4l2_streamparm *) calloc(1, sizeof(struct v4l2_streamparm));
+ memset(setfps, 0, sizeof(struct v4l2_streamparm));
+ setfps->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ setfps->parm.capture.timeperframe.numerator = 1;
+ setfps->parm.capture.timeperframe.denominator = 20;
+ if (xioctl(fd_, VIDIOC_S_PARM, setfps) == -1) {
+ LOG(ERROR, "ioctl VIDIOC_S_PARM(%d, %p) failed with %d: %s\n",
+ fd_, setfps, errno, strerror(errno));
+ exit(EXIT_FAILURE);
+ }
+ LOG(INFO, "framerate ended up at %d/%d\n",
+ setfps->parm.capture.timeperframe.numerator,
+ setfps->parm.capture.timeperframe.denominator);
+#endif
+
+ init_mmap();
+ }
+
+ void Start() {
+ LOG(DEBUG, "queueing buffers for the first time\n");
+ v4l2_buffer buf;
+ for (unsigned int i = 0; i < Buffers::kNumBuffers; ++i) {
+ CLEAR(buf);
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = i;
+ QueueBuffer(&buf);
+ }
+ LOG(DEBUG, "done with first queue\n");
+
+ v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (xioctl(fd_, VIDIOC_STREAMON, &type) == -1) {
+ LOG(FATAL, "ioctl VIDIOC_STREAMON(%d, %p) failed with %d: %s\n",
+ fd_, &type, errno, strerror(errno));
+ }
+ }
+
+ public:
+ void Run() {
+ Start();
+
+ fd_set fds;
+ timeval tv;
+ while (true) {
+ // HAVE TO DO THIS EACH TIME THROUGH THE LOOP
+ tv.tv_sec = 2;
+ tv.tv_usec = 0;
+
+ FD_ZERO(&fds);
+ FD_SET(fd_, &fds);
+ FD_SET(server_fd_, &fds);
+ switch (select(std::max(fd_, server_fd_) + 1, &fds, NULL, NULL, &tv)) {
+ case -1:
+ if (errno != EINTR) {
+ LOG(ERROR, "select(%d, %p, NULL, NULL, %p) failed with %d: %s\n",
+ std::max(fd_, server_fd_) + 1, &fds, &tv, errno, strerror(errno));
+ }
+ continue;
+ case 0:
+ LOG(WARNING, "select timed out\n");
+ continue;
+ }
+
+ if (FD_ISSET(fd_, &fds)) {
+ ReadFrame();
+ }
+ if (FD_ISSET(server_fd_, &fds)) {
+ const int sock = accept4(server_fd_, NULL, NULL, SOCK_NONBLOCK);
+ if (sock == -1) {
+ LOG(ERROR, "accept4(%d, NULL, NULL, SOCK_NONBLOCK(=%d) failed with %d: %s\n",
+ server_fd_, SOCK_NONBLOCK, errno, strerror(errno));
+ } else {
+ SendFD(sock);
+ }
+ }
+ }
+ }
+};
+const char *const Reader::dev_name = "/dev/video0";
+const aos_type_sig Reader::kRecycleSignature{
+ sizeof(Buffers::Message), 1, Buffers::kNumBuffers};
+
+} // namespace camera
+} // namespace aos
+
+AOS_RUN_NRT(aos::camera::Reader)
+
diff --git a/aos/atom_code/camera/V4L2.h b/aos/atom_code/camera/V4L2.h
new file mode 100644
index 0000000..bbafe3e
--- /dev/null
+++ b/aos/atom_code/camera/V4L2.h
@@ -0,0 +1,27 @@
+#ifndef AOS_ATOM_CODE_CAMREA_V4L2_H_
+#define AOS_ATOM_CODE_CAMREA_V4L2_H_
+
+// This file handles including everything needed to use V4L2 and has some
+// utility functions.
+
+#include <sys/ioctl.h>
+
+#include <asm/types.h> /* for videodev2.h */
+#include <linux/videodev2.h>
+
+namespace aos {
+namespace camera {
+
+static inline int xioctl(int fd, int request, void *arg) {
+ int r;
+ do {
+ r = ioctl(fd, request, reinterpret_cast<uintptr_t>(arg));
+ } while (r == -1 && errno == EINTR);
+ return r;
+}
+
+} // namespace camera
+} // namespace aos
+
+#endif
+
diff --git a/aos/atom_code/camera/aos.jar_manifest b/aos/atom_code/camera/aos.jar_manifest
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/aos/atom_code/camera/aos.jar_manifest
@@ -0,0 +1 @@
+
diff --git a/aos/atom_code/camera/camera.gyp b/aos/atom_code/camera/camera.gyp
new file mode 100644
index 0000000..e94a6ac
--- /dev/null
+++ b/aos/atom_code/camera/camera.gyp
@@ -0,0 +1,66 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'aos_camera',
+ 'type': 'loadable_module',
+ 'sources': [
+ 'jni.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:aos_shared_lib',
+ '<(AOS)/common/network/network.gyp:socket_so',
+ '<(AOS)/common/common.gyp:timing_so',
+ '<(AOS)/atom_code/messages/messages.gyp:messages_so',
+ 'private_aos_camera_jar',
+ '<(EXTERNALS):libjpeg',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:aos_shared_lib',
+ '<(AOS)/common/network/network.gyp:socket_so',
+ '<(AOS)/common/common.gyp:timing_so',
+ '<(AOS)/atom_code/messages/messages.gyp:messages_so',
+ 'private_aos_camera_jar',
+ ],
+ },
+ {
+ 'target_name': 'private_aos_camera_jar',
+ 'dependencies': [
+ '<(EXTERNALS):javacv',
+ ],
+ 'variables': {
+ 'srcdirs': ['java'],
+ 'gen_headers': ['aos.Natives'],
+ },
+ 'export_dependent_settings': [
+ '<(EXTERNALS):javacv',
+ ],
+ 'direct_dependent_settings': {
+ 'variables': {
+ 'jni_libs': ['aos_camera'],
+ },
+ },
+ 'includes': ['../../build/java.gypi'],
+ 'hard_dependency': 1,
+ },
+ {
+ 'target_name': 'CameraHTTPStreamer',
+ 'type': 'executable',
+ 'sources': [
+ 'HTTPStreamer.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ {
+ 'target_name': 'CameraReader',
+ 'type': 'executable',
+ 'sources': [
+ 'Reader.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ ],
+}
diff --git a/aos/atom_code/camera/java/aos/CameraProcessor.java b/aos/atom_code/camera/java/aos/CameraProcessor.java
new file mode 100644
index 0000000..4f6c68d
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/CameraProcessor.java
@@ -0,0 +1,67 @@
+package aos;
+
+import java.io.IOException;
+import java.net.Inet4Address;
+import java.net.InetSocketAddress;
+import java.net.UnknownHostException;
+import java.nio.ByteBuffer;
+import java.nio.channels.ReadableByteChannel;
+import java.nio.channels.SocketChannel;
+import java.util.logging.Level;
+import java.util.logging.Logger;
+
+import com.googlecode.javacv.cpp.opencv_core;
+
+/**
+ * Makes implementing code that processes frames from a camera easy.
+ */
+public abstract class CameraProcessor {
+ private static final Logger LOG = Logger.getLogger(CameraProcessor.class.getName());
+ protected final ImageGetter getter;
+ protected final ServableImage start = new ServableImage(ImageGetter.width, ImageGetter.height, opencv_core.IPL_DEPTH_8U, 3);
+
+ /**
+ * Parses any arguments it recognizes out of {@code args} and initializes stuff appropriately.
+ * This includes using {@link QueueLogHandler} for all exceptions and {@link Thread#setDefaultUncaughtExceptionHandler}ing.
+ * @param args from {@code main}
+ */
+ protected CameraProcessor(String[] args) throws UnknownHostException, IOException {
+ QueueLogHandler.UseForAll();
+ ReadableByteChannel channel = null;
+ for (int i = 0; i < args.length; ++i) {
+ final String c = args[i];
+ if (c.equals("--host")) {
+ String host = args[++i];
+ final SocketChannel socketChannel = SocketChannel.open(new InetSocketAddress(Inet4Address.getByName(host), 9714));
+ socketChannel.write(ByteBuffer.wrap(new byte[] {'\r', '\n', '\r', '\n'})); // get it past the read headers stage
+ channel = socketChannel;
+ } else {
+ System.err.println("CameraProcessor: warning: unrecognized argument '" + c + "'. ignoring");
+ }
+ }
+
+ if (channel != null) {
+ getter = new ChannelImageGetter(channel);
+ } else {
+ System.out.println("creating QueueImageGetter");
+ getter = new QueueImageGetter();
+ System.out.println("done");
+ }
+
+ LOG.log(Level.INFO, "CameraProcessor is up");
+ System.err.println("CameraProcessor is up (on stderr)");
+ }
+
+ protected abstract void RunIteration();
+
+ protected void Run() {
+ while (true) {
+ if (!getter.get(start.getImage())) {
+ LOG.log(Level.WARNING, "getting image failed");
+ continue;
+ }
+ RunIteration();
+ start.releaseImage();
+ }
+ }
+}
diff --git a/aos/atom_code/camera/java/aos/ChannelImageGetter.java b/aos/atom_code/camera/java/aos/ChannelImageGetter.java
new file mode 100644
index 0000000..511b55b
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/ChannelImageGetter.java
@@ -0,0 +1,158 @@
+package aos;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+import java.nio.channels.ReadableByteChannel;
+import java.nio.channels.SelectableChannel;
+import java.nio.channels.SelectionKey;
+import java.nio.channels.Selector;
+import java.util.Map;
+import java.util.TreeMap;
+import java.util.logging.Level;
+import java.util.logging.Logger;
+
+/**
+ * Retrieves images from a {@link InputChannel}. Expects the images in mjpg form.
+ * For now, only accepts streams formatted pretty closely to how aos::camera::HTTPStreamer does it.
+ */
+public class ChannelImageGetter extends JPEGImageGetter {
+ /**
+ * What to multiply each length by when it needs to allocate a larger buffer to fit an image.
+ */
+ private static final double extraLength = 1.2;
+
+ private static final Logger LOG = Logger.getLogger(ChannelImageGetter.class
+ .getName());
+ private final ReadableByteChannel channel;
+ private final Selector selector = Selector.open();
+ private String separator = "--boundarydonotcross\r\n";
+ private ByteBuffer current;
+ private final ByteBuffer headerBuffer = ByteBuffer.allocateDirect(30);
+ private final Map<String, String> headers = new TreeMap<String, String>(String.CASE_INSENSITIVE_ORDER);
+
+ public ChannelImageGetter(ReadableByteChannel channel) throws IOException {
+ this.channel = channel;
+ if (channel instanceof SelectableChannel) {
+ ((SelectableChannel)channel).configureBlocking(false);
+ }
+ }
+
+ @Override
+ public ByteBuffer getJPEG() {
+ try {
+ if (!parseHeaders()) {
+ return null;
+ }
+ LOG.log(Level.FINE, "parsed headers " + headers.toString());
+ try {
+ final int length = Integer.parseInt(headers.get("Content-Length"));
+ if (current == null || current.capacity() < length) {
+ LOG.log(Level.INFO, "allocating a new direct buffer of length " + length * extraLength);
+ current = ByteBuffer.allocateDirect((int) (length * extraLength));
+ } else {
+ current.rewind();
+ current.limit(length);
+ }
+ } catch (NumberFormatException e) {
+ LOG.log(Level.WARNING, "couldn't parse '" + headers.get("Content-Length") + "' as a number");
+ return null;
+ }
+ current.put(headerBuffer); // copy out any of the image that got buffered with the headers
+ while (current.hasRemaining()) {
+ channel.read(current);
+ }
+ current.flip();
+ } catch (IOException e) {
+ LOG.log(Level.WARNING, "reading the headers and/or image failed", e);
+ return null;
+ }
+ return current;
+ }
+ // returns success
+ private boolean parseHeaders() throws IOException {
+ // Reads chunks into headerBuffer and parses out headers.
+ // Looks for separator first.
+
+ headerBuffer.clear();
+ headers.clear();
+ final byte[] separatorBytes = separator.getBytes();
+ int separatorIndex = 0; // how much of the separator has been matched
+ while (headerBuffer.hasRemaining() || headerBuffer.limit() < headerBuffer.capacity()) {
+ if (channel instanceof SelectableChannel) {
+ ((SelectableChannel)channel).register(selector, SelectionKey.OP_READ);
+ selector.select();
+ }
+ headerBuffer.limit(headerBuffer.capacity());
+ channel.read(headerBuffer);
+ headerBuffer.flip();
+ if (separatorIndex < separatorBytes.length) {
+ // make sure we don't get part of the way through
+ while (headerBuffer.remaining() >= (separatorBytes.length - separatorIndex)) {
+ final byte c = headerBuffer.get();
+ if (separatorBytes[separatorIndex++] != c) {
+ separatorIndex = 0;
+ }
+ if (separatorIndex == separatorBytes.length) {
+ break;
+ }
+ }
+ headerBuffer.compact();
+ } else {
+ int keyEnd = 0, valueStart = 0;
+ boolean foundEndR = false; // found the end \r
+ while (headerBuffer.hasRemaining()) {
+ final byte c = headerBuffer.get();
+ if (foundEndR) {
+ if (c != '\n') {
+ LOG.log(Level.WARNING, "found \r\n\r but no \n afterwards");
+ } else {
+ return true;
+ }
+ } else if (keyEnd == 0) {
+ if (c == ':') {
+ keyEnd = headerBuffer.position() - 1;
+ } else if (c == '\r') {
+ foundEndR = true;
+ }
+ } else if (valueStart == 0) {
+ if (c != ' ') {
+ valueStart = headerBuffer.position() - 1;
+ }
+ } else {
+ if (c == '\r') {
+ final int valueEnd = headerBuffer.position();
+ final byte[] key = new byte[keyEnd];
+ headerBuffer.position(0);
+ headerBuffer.get(key);
+ final byte[] value = new byte[valueEnd - valueStart - 1];
+ headerBuffer.position(valueStart);
+ headerBuffer.get(value);
+ headers.put(new String(key), new String(value));
+
+ headerBuffer.get(); // get the \r
+ headerBuffer.get(); // get the \n
+
+ headerBuffer.compact();
+ headerBuffer.flip();
+
+ keyEnd = valueStart = 0;
+ }
+ }
+ }
+ }
+ }
+ // if we got here, then it doesn't have space left and we haven't finished
+ LOG.log(Level.WARNING, "got a header that was too long. headerBuffer should be made bigger");
+ return false;
+ }
+
+ @Override
+ public double getTimestamp() {
+ if (headers.containsKey("X-Timestamp")) {
+ return Double.parseDouble(headers.get("X-Timestamp"));
+ } else {
+ throw new UnsupportedOperationException("source stream doesn't have X-Timestamp headers");
+ }
+ }
+
+}
diff --git a/aos/atom_code/camera/java/aos/DebugServer.java b/aos/atom_code/camera/java/aos/DebugServer.java
new file mode 100644
index 0000000..398cb11
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/DebugServer.java
@@ -0,0 +1,357 @@
+package aos;
+
+import java.io.ByteArrayOutputStream;
+import java.io.IOException;
+import java.net.InetSocketAddress;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.nio.channels.ClosedChannelException;
+import java.nio.channels.SelectionKey;
+import java.nio.channels.Selector;
+import java.nio.channels.ServerSocketChannel;
+import java.nio.channels.SocketChannel;
+import java.util.ArrayList;
+import java.util.Collection;
+import java.util.HashMap;
+import java.util.Iterator;
+import java.util.Map;
+import java.util.logging.Level;
+import java.util.logging.Logger;
+import java.util.regex.Matcher;
+import java.util.regex.Pattern;
+
+import com.googlecode.javacv.cpp.opencv_core.IplImage;
+
+/**
+ * A server that serves {@link ServableImage}s.
+ */
+public class DebugServer {
+ private static final String initialHeaderString = "HTTP/1.0 200 OK\r\n"
+ + "Connection: close\r\n"
+ + "Server: AOS/0.0 Vision Code Debug\r\n"
+ + "Cache-Control: no-store, no-cache, must-revalidate, pre-check=0, post-check=0, max-age=0\r\n"
+ + "Pragma: no-cache\r\n"
+ + "Expires: Mon, 3 Jan 2000 12:34:56 GMT\r\n"
+ + "Content-Type: multipart/x-mixed-replace; boundary=boundarydonotcross\r\n";
+ private static final String intermediateHeaderFormat = "\r\n--boundarydonotcross\r\n"
+ + "Content-Type: image/bmp\r\n"
+ + "Content-Length: %d\r\n"
+ + "X-Timestamp: %f\r\n"
+ + "\r\n";
+ private static final ByteBuffer initialHeader;
+ private static final Pattern headerPattern;
+ static {
+ initialHeader = ByteBuffer.wrap(initialHeaderString.getBytes())
+ .asReadOnlyBuffer();
+ headerPattern = Pattern.compile("^GET ([^? ]+)(?:\\?i=(\\S*))? HTTP/.*\r\n.*$",
+ Pattern.DOTALL);
+ }
+
+ private static final Logger LOG = Logger.getLogger(DebugServer.class
+ .getName());
+ private final ServerSocketChannel server;
+ private final Collection<Client> clients = new ArrayList<Client>();
+ private final Map<String, ServableImage> images = new HashMap<String, ServableImage>();
+ private final Map<String, Palette> palettes = new HashMap<String, Palette>();
+ private double timestamp;
+
+ public static class Palette {
+ private final ByteArrayOutputStream out = new ByteArrayOutputStream();
+ private int entries = 0;
+
+ /**
+ * Adds a new color. All 4 arguments are unsigned bytes.
+ * @param r red
+ * @param g green
+ * @param b blue
+ * @param a alpha (doesn't seem to work)
+ * @return this
+ */
+ public Palette add(int r, int g, int b, int a) {
+ out.write(b);
+ out.write(g);
+ out.write(r);
+ out.write(a);
+ ++entries;
+ return this;
+ }
+
+ private int entries() {
+ return entries;
+ }
+ private int bytes() {
+ return entries * 4;
+ }
+ private void writeTo(ByteBuffer buffer) {
+ buffer.put(out.toByteArray());
+ }
+ }
+
+ private class Client {
+ private final int bmpHeaderSize;
+ private int bmpType;
+ private Palette palette;
+
+ private final SocketChannel channel;
+ private ServableImage image = null;
+ private IplImage img;
+ private int index;
+ private final ByteBuffer[] buffers;
+ private final ByteBuffer initial = initialHeader.duplicate();
+ private final ByteBuffer read = ByteBuffer.allocate(1024);
+
+ public Client(SocketChannel channel) throws IOException {
+ this.channel = channel;
+ channel.configureBlocking(false);
+
+ if (bmpType == 3) {
+ bmpHeaderSize = 122;
+ } else if (bmpType == 0) {
+ bmpHeaderSize = 54;
+ } else {
+ throw new AssertionError("unknown bmpType value " + bmpType);
+ }
+ // [0] gets filled in by createBmpHeader which writes the header into [1]
+ // [2] gets set to the image buffer
+ buffers = new ByteBuffer[] { null, ByteBuffer.allocate(2048), null };
+ }
+
+ public void register(Selector selector) throws ClosedChannelException {
+ channel.register(
+ selector,
+ (image == null) ? SelectionKey.OP_READ
+ : SelectionKey.OP_WRITE, this);
+ }
+
+ public void close() {
+ if (image != null) {
+ image.setDebugging(false);
+ }
+ if (channel != null) {
+ try {
+ channel.close();
+ } catch (IOException e) {
+ LOG.log(Level.WARNING,
+ "encountered error when closing channel", e);
+ }
+ }
+ }
+
+ private void createBmpHeader(ByteBuffer buffer) {
+ // <http://en.wikipedia.org/wiki/BMP_file_format#File_structure>
+ // explains what these all are
+ // signed/unsigned numbers don't matter because they'd better not be
+ // that big
+ final int paletteSize = (palette == null) ? 0 : palette.bytes();
+ buffers[0] = ByteBuffer.wrap(String.format(intermediateHeaderFormat,
+ bmpHeaderSize + paletteSize + image.imageSize(), timestamp).getBytes());
+ buffer.order(ByteOrder.LITTLE_ENDIAN);
+ buffer.put((byte) 'B').put((byte) 'M');
+ buffer.putInt(bmpHeaderSize + paletteSize + image.imageSize());
+ buffer.putInt(0); // skip ahead 4 bytes
+ buffer.putInt(bmpHeaderSize + paletteSize); // offset to start of image data
+ buffer.putInt(bmpHeaderSize - 14); // size of the rest of the header
+ // BMPs expect image data bottom to top, so -height to fix that
+ buffer.putInt(ImageGetter.width).putInt(-ImageGetter.height);
+ buffer.putShort((short) 1).putShort(image.bitsPerPixel());
+ buffer.putInt(bmpType);
+ buffer.putInt(image.imageSize()); // number of bytes in the actual
+ // image
+ buffer.putInt(2835).putInt(2835); // physical resolution; don't
+ // think it matters
+ buffer.putInt((palette == null) ? 0 : palette.entries());
+ buffer.putInt(0); // # of important colors (0 means all)
+ if (palette != null) {
+ palette.writeTo(buffer);
+ }
+ final int expected;
+ if (bmpType == 0) { // BI_RGB
+ expected = bmpHeaderSize + paletteSize;
+ } else if (bmpType == 3) { // BI_BITFIELDS
+ buffer.putInt(0x0000FF00).putInt(0x00FF0000).putInt(0xFF000000)
+ .putInt(0); // RGBA bitmasks
+ buffer.putInt(0x57696E20); // LCS_WINDOWS_COLOR_SPACE
+ expected = bmpHeaderSize - 48;
+ } else {
+ throw new AssertionError("unknown bmpType value " + bmpType);
+ }
+ if (buffer.position() != expected) {
+ throw new AssertionError(
+ "header ended up the wrong size. expected "
+ + expected + " but got "
+ + buffer.position());
+ }
+ buffer.limit(bmpHeaderSize + paletteSize);
+ buffer.rewind();
+ }
+
+ /**
+ * Does anything that this one can right now.
+ *
+ * @return whether or not to {@link #close()} and remove this one
+ */
+ public boolean run() throws IOException {
+ if (image == null) {
+ final int bytesRead = channel.read(read);
+ final String readString = new String(read.array(), 0,
+ read.position());
+ LOG.log(Level.INFO, "read " + bytesRead
+ + " header bytes position=" + read.position()
+ + " string='" + readString + "'");
+
+ final Matcher matcher = headerPattern.matcher(readString);
+ if (matcher.matches()) {
+ final String url = matcher.group(1);
+ image = images.get(url);
+ if (image == null) {
+ LOG.log(Level.INFO, "couldn't find an image for url '"
+ + url + "'. dropping client");
+ return true;
+ } else {
+ LOG.log(Level.INFO, "found an image for url '"
+ + url + "'");
+ }
+ palette = palettes.get(url);
+ bmpType = 0; // could change this in the future
+ createBmpHeader(buffers[1]);
+ image.setDebugging(true);
+ final String indexString = matcher.group(2);
+ if (indexString != null) {
+ index = Integer.parseInt(indexString);
+ } else {
+ index = 0;
+ }
+ LOG.log(Level.INFO, "using index " + index);
+ } else if (!read.hasRemaining()) {
+ read.flip();
+ LOG.log(Level.WARNING,
+ "ran out of buffer space reading the header. currently have '"
+ + readString + "'. dropping connection");
+ return true;
+ } else if (bytesRead == -1) {
+ read.flip();
+ LOG.log(Level.WARNING,
+ "reached end of stream for headers without getting anything valid. currently have "
+ + read.limit()
+ + " bytes ='"
+ + readString
+ + "'. dropping connection");
+ return true;
+ }
+ } else if (initial.hasRemaining()) {
+ channel.write(initial);
+ } else {
+ if (buffers[2] == null) {
+ img = image.getSnapshot(index);
+ if (img == null) {
+ return false;
+ } else {
+ buffers[2] = img.getByteBuffer();
+ LOG.log(Level.FINE, "got " + buffers[2]
+ + " from the image");
+ }
+ }
+ channel.write(buffers);
+ boolean remaining = false;
+ for (ByteBuffer c : buffers) {
+ if (c.hasRemaining()) {
+ remaining = true;
+ }
+ }
+ if (!remaining) {
+ for (ByteBuffer c : buffers) {
+ c.rewind();
+ }
+ buffers[2] = null;
+ image.releaseSnapshot(index, img);
+ }
+ }
+ return false;
+ }
+ }
+
+ public DebugServer(int port) throws IOException {
+ server = ServerSocketChannel.open();
+ server.configureBlocking(false);
+ server.socket().bind(new InetSocketAddress(port), 10);
+ new Thread("DebugServer") {
+ @Override
+ public void run() {
+ try {
+ loop();
+ } catch (Throwable e) {
+ LOG.log(Level.SEVERE,
+ "trouble running the server loop", e);
+ System.exit(1);
+ }
+ }
+ }.start();
+ }
+
+ public void addImage(String name, ServableImage image) {
+ if (image.bitsPerPixel() != 24) {
+ throw new IllegalArgumentException("only 24-bit images are supported");
+ // could support 16 and 32 bpp images by using bmpType of 3
+ }
+ images.put(name, image);
+ }
+ public void addImage(String name, ServableImage image, Palette palette) {
+ if (image.bitsPerPixel() != 8) {
+ throw new IllegalArgumentException("only 8-bit images are supported");
+ // everything should work for 1, 2, and 4 bpp ones too except for padding etc
+ }
+ if (palette.entries() > (1 << image.bitsPerPixel())) {
+ throw new IllegalArgumentException("too many colors in the palette");
+ }
+ images.put(name, image);
+ palettes.put(name, palette);
+ }
+ /**
+ * This timestamp is written out in the debugging images.
+ * @param timestamp the current timestamp
+ */
+ public void setTimestamp(double timestamp) {
+ this.timestamp = timestamp;
+ }
+
+ private void loop() throws IOException {
+ final Selector selector = Selector.open();
+ server.register(selector, SelectionKey.OP_ACCEPT);
+ while (true) {
+ try {
+ for (Client c : clients) {
+ c.register(selector);
+ }
+ selector.select();
+ for (final Iterator<SelectionKey> i = selector.selectedKeys()
+ .iterator(); i.hasNext();) {
+ final SelectionKey c = i.next();
+ if (c.isAcceptable()) {
+ // there's only 1 socket there that can accept
+ final SocketChannel channel = server.accept();
+ if (channel != null) {
+ clients.add(new Client(channel));
+ }
+ } else {
+ final Client client = (Client) c.attachment();
+ try {
+ if (client.run()) {
+ client.close();
+ clients.remove(client);
+ }
+ } catch (Exception e) {
+ LOG.log(Level.INFO, "dropping client " + client
+ + " because it's run() threw an exception",
+ e);
+ client.close();
+ clients.remove(client);
+ }
+ }
+ i.remove();
+ }
+ } catch (IOException e) {
+ LOG.log(Level.WARNING, "trouble running the server loop", e);
+ }
+ }
+ }
+}
diff --git a/aos/atom_code/camera/java/aos/ImageGetter.java b/aos/atom_code/camera/java/aos/ImageGetter.java
new file mode 100644
index 0000000..f0f1063
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/ImageGetter.java
@@ -0,0 +1,24 @@
+package aos;
+
+import com.googlecode.javacv.cpp.opencv_core;
+import com.googlecode.javacv.cpp.opencv_core.IplImage;
+
+/**
+ * An object that can retrieve images from somewhere.
+ */
+public interface ImageGetter {
+ public static int width = 640, height = 480;
+
+ /**
+ * Gets an image.
+ * @param out Where to write the image to. Must be a 3-channel {@link opencv_core#IPL_DEPTH_8U} image.
+ * @return whether it succeeded or not
+ */
+ public boolean get(IplImage out);
+ /**
+ * Only valid after a successful {@link #get()}.
+ * @return The timestamp from the most recent frame. Will be in seconds with at least ms accuracy.
+ */
+ public double getTimestamp();
+}
+
diff --git a/aos/atom_code/camera/java/aos/JPEGDecoder.java b/aos/atom_code/camera/java/aos/JPEGDecoder.java
new file mode 100644
index 0000000..63d12fd
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/JPEGDecoder.java
@@ -0,0 +1,29 @@
+package aos;
+
+import java.nio.ByteBuffer;
+
+import com.googlecode.javacv.cpp.opencv_core;
+import com.googlecode.javacv.cpp.opencv_core.IplImage;
+
+/**
+ * Efficiently decodes a JPEG image from a @{class ByteBuffer} into an @{class IplImage}.
+ * Instances are not safe for use from multiple threads.
+ * The first use of an instance allocates some largish buffers which are never freed.
+ */
+public class JPEGDecoder {
+ private final long[] state = new long[1];
+
+ /**
+ * @param in Must be direct. The {@link ByteBuffer#limit()} of it will be respected.
+ * @param out Where to write the decoded image to. Must be a 3-channel {@link opencv_core#IPL_DEPTH_8U} image.
+ * Will be written in the RGB color space.
+ * @return Whether or not it succeeded. If not, {@code out} is undefined.
+ */
+ public boolean decode(ByteBuffer in, IplImage out) {
+ if (out.nChannels() != 3 || out.depth() != opencv_core.IPL_DEPTH_8U) {
+ throw new IllegalArgumentException("out is invalid");
+ }
+ return Natives.decodeJPEG(state, in, in.limit(), out.getByteBuffer());
+ }
+}
+
diff --git a/aos/atom_code/camera/java/aos/JPEGImageGetter.java b/aos/atom_code/camera/java/aos/JPEGImageGetter.java
new file mode 100644
index 0000000..84296e7
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/JPEGImageGetter.java
@@ -0,0 +1,26 @@
+package aos;
+
+import java.nio.ByteBuffer;
+
+import com.googlecode.javacv.cpp.opencv_core.IplImage;
+
+/**
+ * Helper class for {@link ImageGetter}s that return JPEG images.
+ */
+public abstract class JPEGImageGetter implements ImageGetter {
+
+ private final JPEGDecoder decoder = new JPEGDecoder();
+
+ @Override
+ public boolean get(IplImage out) {
+ final ByteBuffer jpeg = getJPEG();
+ if (jpeg == null) return false;
+ final boolean r = decoder.decode(jpeg, out);
+ release();
+ return r;
+ }
+
+ protected abstract ByteBuffer getJPEG();
+ protected void release() {}
+
+}
diff --git a/aos/atom_code/camera/java/aos/JavaCVImageGetter.java b/aos/atom_code/camera/java/aos/JavaCVImageGetter.java
new file mode 100644
index 0000000..67398f0
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/JavaCVImageGetter.java
@@ -0,0 +1,56 @@
+package aos;
+
+import java.util.logging.Level;
+import java.util.logging.Logger;
+
+import com.googlecode.javacv.FrameGrabber;
+import com.googlecode.javacv.cpp.opencv_core.IplImage;
+
+
+/**
+ * Adapts between the JavaCV {@link FrameGrabber} and {@link ImageGetter}.
+ * There is (at least) 1 extra copy involved, so this shouldn't be used if you care about speed.
+ */
+public class JavaCVImageGetter implements ImageGetter {
+ private static final Logger LOG = Logger.getLogger(JavaCVImageGetter.class
+ .getName());
+ private final FrameGrabber grabber;
+
+ public JavaCVImageGetter(FrameGrabber grabber) {
+ this.grabber = grabber;
+ if (grabber.getImageWidth() != width || grabber.getImageHeight() != height) {
+ if (grabber.getImageWidth() == 0 && grabber.getImageHeight() == 0) {
+ LOG.log(Level.WARNING, "grabber says it will give 0x0 images at the start. ignoring");
+ } else {
+ throw new IllegalArgumentException("grabber says it will give images that are the wrong size!!");
+ }
+ }
+ }
+
+ @Override
+ public boolean get(IplImage out) {
+ try {
+ final IplImage frame = grabber.grab();
+ if (grabber.getImageWidth() != width || grabber.getImageHeight() != height) {
+ LOG.log(Level.SEVERE, "grabber says it will give the wrong size images");
+ return false;
+ }
+ if (out.imageSize() != frame.imageSize()) {
+ LOG.log(Level.SEVERE, "the grabber gave a " + frame.imageSize() + "-byte image" +
+ "but a " + out.imageSize() + "-byte image was passed in");
+ return false;
+ }
+ out.getByteBuffer().put(frame.getByteBuffer());
+ return true;
+ } catch (FrameGrabber.Exception e) {
+ LOG.log(Level.WARNING, "grabber.grab() threw an exception", e);
+ return false;
+ }
+ }
+
+ @Override
+ public double getTimestamp() {
+ // grabber.getTimestamp seems to be in ms (all the implementations are at least)
+ return grabber.getTimestamp() / 1000.0;
+ }
+}
diff --git a/aos/atom_code/camera/java/aos/NativeBufferError.java b/aos/atom_code/camera/java/aos/NativeBufferError.java
new file mode 100644
index 0000000..41c794d
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/NativeBufferError.java
@@ -0,0 +1,22 @@
+package aos;
+
+public class NativeBufferError extends NativeError {
+ private static final long serialVersionUID = -5298480149664213316L;
+
+ public NativeBufferError() {
+ super();
+ }
+
+ public NativeBufferError(String message, Throwable cause) {
+ super(message, cause);
+ }
+
+ public NativeBufferError(String message) {
+ super(message);
+ }
+
+ public NativeBufferError(Throwable cause) {
+ super(cause);
+ }
+
+}
diff --git a/aos/atom_code/camera/java/aos/NativeError.java b/aos/atom_code/camera/java/aos/NativeError.java
new file mode 100644
index 0000000..d410234
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/NativeError.java
@@ -0,0 +1,25 @@
+package aos;
+
+/**
+ * Represents an error from native code.
+ */
+public class NativeError extends Error {
+ private static final long serialVersionUID = 7394872852984037261L;
+
+ public NativeError() {
+ super();
+ }
+
+ public NativeError(String message, Throwable cause) {
+ super(message, cause);
+ }
+
+ public NativeError(String message) {
+ super(message);
+ }
+
+ public NativeError(Throwable cause) {
+ super(cause);
+ }
+
+}
diff --git a/aos/atom_code/camera/java/aos/NativeLoader.java b/aos/atom_code/camera/java/aos/NativeLoader.java
new file mode 100644
index 0000000..d4fe7a8
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/NativeLoader.java
@@ -0,0 +1,14 @@
+package aos;
+
+/**
+ * Provides support for dealing with loading native code.
+ */
+public class NativeLoader {
+ /**
+ * Loads a native library.
+ * @param name the name of the gyp shared_library or loadable_module target to load
+ */
+ public static void load(String name) {
+ System.load(System.getProperty("one-jar.expand.dir") + "/so_libs/lib" + name + ".so");
+ }
+}
diff --git a/aos/atom_code/camera/java/aos/Natives.java b/aos/atom_code/camera/java/aos/Natives.java
new file mode 100644
index 0000000..8ea4e01
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/Natives.java
@@ -0,0 +1,82 @@
+package aos;
+
+import com.googlecode.javacv.cpp.opencv_core;
+
+import java.nio.ByteBuffer;
+import java.util.logging.Level;
+
+/**
+ * <p>Package-private class that has all of the native functions in it to make them easier to implement.</p>
+ * <p>WARNING: The raw native functions are <b>NOT</b> thread-safe!!!!! Any java functions that need to be thread safe MUST be synchronized in JAVA!</p>
+ */
+class Natives {
+ static {
+ NativeLoader.load("aos_camera");
+ nativeInit(ImageGetter.width, ImageGetter.height);
+ }
+ private static native void nativeInit(int width, int height);
+ /**
+ * Empty function to make sure the class gets loaded (which means loading the native library).
+ */
+ public static void ensureLoaded() {}
+
+ /**
+ * Decodes a JPEG from in into out. Both buffers must be direct.
+ * @param state a long[1] for storing thread-local state
+ * @param in the JPEG to decode
+ * @param inBytes how many bytes long the JPEG is
+ * @param out the buffer to write the decoded image into
+ * @return Whether or not it succeeded. If not, {@code out} is undefined.
+ */
+ public static native boolean decodeJPEG(long[] state, ByteBuffer in, int inBytes, ByteBuffer out);
+
+ /**
+ * Thresholds in into out. Both buffers must be direct.
+ * All of the short arguments should be unsigned bytes. The min and max parameters specify what colors to accept.
+ * @param in The image to threshold. Must be a 3-channel {@link opencv_core#IPL_DEPTH_8U} image buffer in the HSV color space.
+ * @param out Where to write the thresholded image to. Must be a 1-channel {@link opencv_core#IPL_DEPTH_8U} image buffer.
+ * @param hoffset An offset to be added to the hue value before comparing it to {@code hmin} and {@code hmax}.
+ * The addition will be performed to a {@code uint8_t}, which will wrap around. This means that it must be positive.
+ * Useful for finding red values.
+ */
+ public static native void threshold(ByteBuffer in, ByteBuffer out, short hoffset, char hmin, char hmax,
+ char smin, char smax, char vmin, char vmax);
+
+ /**
+ * Converts the colors from in to the format required for dumping them into a BMP image.
+ * @param in The image to convert. Must be a 3-channel {@link opencv_core#IPL_DEPTH_8U} image buffer in the regular (BGR I think...) color space.
+ * @param out Where to write the converted image to. Must be a 3-channel {@link opencv_core#IPL_DEPTH_8U} image buffer.
+ */
+ public static native void convertBGR2BMP(ByteBuffer in, ByteBuffer out);
+
+ /**
+ * Retrieves a JPEG image from the queue system. Will block until a new one is ready.
+ * @param id from {@link #queueInit()}
+ * @return Will be direct. This buffer <b>must not EVER</b> be written to.
+ */
+ public static native ByteBuffer queueGetJPEG(long id);
+ /**
+ * Retrieves the latest frame timestamp from the queue system. Must only be called between {@link #queueGetJPEG} and {@link #queueReleaseJPEG}.
+ * @param id from {@link #queueInit()}
+ * @return a timestamp
+ */
+ public static native double queueGetTimestamp(long id);
+ /**
+ * Releases the last image retrieved from the queue system. The result of the last {@link #queueGetJPEG()} will now be invalid.
+ * @param id from {@link #queueInit()}
+ */
+ public static native void queueReleaseJPEG(long id);
+ /**
+ * Prepares to start retrieving JPEGs from the queues.
+ * @return the ID to pass to the other queue functions
+ */
+ public static native long queueInit();
+
+ /**
+ * Puts the given message into the logging framework.
+ * @param message the complete log message
+ * @param level the level (from {@link Level#intValue()}
+ */
+ public static native void LOG(String message, int level);
+}
+
diff --git a/aos/atom_code/camera/java/aos/QueueImageGetter.java b/aos/atom_code/camera/java/aos/QueueImageGetter.java
new file mode 100644
index 0000000..7ed2f65
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/QueueImageGetter.java
@@ -0,0 +1,34 @@
+package aos;
+
+import java.nio.ByteBuffer;
+
+/**
+ * Retrieves images from the queue system.<br>
+ * {@link #getTimestamp()} returns the value from the v4l2 driver,
+ * which is a CLOCK_MONOTONIC time for most (all?) of them and definitely uvcvideo.
+ */
+public class QueueImageGetter extends JPEGImageGetter {
+ private final long nativeID;
+ public QueueImageGetter() {
+ nativeID = Natives.queueInit();
+ }
+
+ @Override
+ public ByteBuffer getJPEG() {
+ final ByteBuffer buf = Natives.queueGetJPEG(nativeID);
+ if (buf == null) {
+ return null;
+ }
+ return buf.asReadOnlyBuffer();
+ }
+
+ @Override
+ public void release() {
+ Natives.queueReleaseJPEG(nativeID);
+ }
+
+ @Override
+ public double getTimestamp() {
+ return Natives.queueGetTimestamp(nativeID);
+ }
+}
diff --git a/aos/atom_code/camera/java/aos/QueueLogHandler.java b/aos/atom_code/camera/java/aos/QueueLogHandler.java
new file mode 100644
index 0000000..3eb8938
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/QueueLogHandler.java
@@ -0,0 +1,119 @@
+package aos;
+
+import java.lang.Thread.UncaughtExceptionHandler;
+import java.util.logging.Formatter;
+import java.util.logging.Handler;
+import java.util.logging.Level;
+import java.util.logging.LogRecord;
+import java.util.logging.Logger;
+import java.util.logging.SimpleFormatter;
+
+/**
+ * <p>Sends messages to the AOS queue-based logging system. Also sends anything that's at least a {@link Level#WARNING} to {@link System#err}.</p>
+ * <p>Writes out each stack frame of exceptions as a separate line after the first one with a \t at the beginning.</p>
+ */
+public class QueueLogHandler extends Handler {
+ private Formatter defaultFormatter;
+
+ /**
+ * Sets up the logging framework to use an instance of this class for all logging and returns the newly created instance.
+ */
+ public static QueueLogHandler UseForAll() {
+ Natives.ensureLoaded();
+ final Logger top = Logger.getLogger("");
+ for (Handler c : top.getHandlers()) {
+ top.removeHandler(c);
+ }
+ QueueLogHandler instance = new QueueLogHandler();
+ top.addHandler(instance);
+ top.setLevel(Level.ALL);
+
+ Thread.setDefaultUncaughtExceptionHandler(new UncaughtExceptionHandler() {
+ @Override
+ public void uncaughtException(Thread thread, Throwable e) {
+ top.log(Level.SEVERE, "uncaught exception in thread " + thread, e);
+ System.exit(1);
+ }
+ });
+
+ return instance;
+ }
+
+ @Override
+ public void close() throws SecurityException {
+ }
+ @Override
+ public void flush() {
+ }
+
+ private Formatter findFormatter() {
+ final Formatter r = getFormatter();
+ if (r != null) {
+ return r;
+ }
+ if (defaultFormatter != null) {
+ return defaultFormatter;
+ }
+ return defaultFormatter = new SimpleFormatter();
+ }
+ @Override
+ public void publish(LogRecord record) {
+ /*final StringBuilder thrownString = new StringBuilder(0);
+ if (record.getThrown() != null) {
+ thrownString.append(": ");
+ thrownString.append(record.getThrown().toString());
+ for (StackTraceElement c : record.getThrown().getStackTrace()) {
+ thrownString.append(" > ");
+ thrownString.append(c.getClassName());
+ thrownString.append('.');
+ thrownString.append(c.getMethodName());
+ thrownString.append('(');
+ thrownString.append(c.getFileName());
+ thrownString.append(':');
+ thrownString.append(c.getLineNumber());
+ thrownString.append(')');
+ }
+ }
+ Natives.LOG(record.getSourceClassName() + ": " + record.getSourceMethodName() + ": " +
+ findFormatter().formatMessage(record) + thrownString, record.getLevel().intValue());*/
+ if (record.getThrown() instanceof UnsatisfiedLinkError || record.getThrown().getCause() instanceof UnsatisfiedLinkError) {
+ record.setThrown(new UnsatisfiedLinkError("are you running a JVM of the correct bitness?").initCause(record.getThrown()));
+ }
+ Natives.LOG(record.getSourceClassName() + ": " + record.getSourceMethodName() + ": " +
+ findFormatter().formatMessage(record), record.getLevel().intValue());
+ if (record.getThrown() != null) {
+ logException(record.getThrown(), record.getLevel().intValue(), false);
+ }
+
+ if (record.getLevel().intValue() >= Level.WARNING.intValue()) {
+ System.err.println(findFormatter().format(record));
+ }
+ }
+ private void logException(Throwable e, int level, boolean caused_by) {
+ final StringBuilder thrownString = new StringBuilder();
+ if (caused_by) {
+ thrownString.append("Caused by: ");
+ }
+ thrownString.append(e.getClass().getName());
+ thrownString.append(": ");
+ thrownString.append(e.getLocalizedMessage());
+ Natives.LOG(thrownString.toString(), level);
+ for (StackTraceElement c : e.getStackTrace()) {
+ thrownString.setLength(0);
+ thrownString.append("\t");
+ thrownString.append(c.getClassName());
+ thrownString.append('.');
+ thrownString.append(c.getMethodName());
+ thrownString.append('(');
+ thrownString.append(c.getFileName());
+ thrownString.append(':');
+ thrownString.append(c.getLineNumber());
+ thrownString.append(')');
+ Natives.LOG(thrownString.toString(), level);
+ }
+ if (e.getCause() != null) {
+ logException(e.getCause(), level, true);
+ }
+ }
+
+}
diff --git a/aos/atom_code/camera/java/aos/ServableImage.java b/aos/atom_code/camera/java/aos/ServableImage.java
new file mode 100644
index 0000000..c5db252
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/ServableImage.java
@@ -0,0 +1,145 @@
+package aos;
+
+import java.util.ArrayDeque;
+import java.util.ArrayList;
+import java.util.Queue;
+import java.util.logging.Logger;
+
+import com.googlecode.javacv.cpp.opencv_core.IplImage;
+
+/**
+ * Provides {@link IplImage}s that can be used with a {@link DebugServer}.
+ */
+public class ServableImage {
+ @SuppressWarnings("unused")
+ private static final Logger LOG = Logger.getLogger(ServableImage.class
+ .getName());
+ private final int width, height, depth, channels;
+ private final ArrayList<Queue<IplImage>> queues = new ArrayList<Queue<IplImage>>();
+ private final ArrayList<IplImage> snapshots = new ArrayList<IplImage>();
+ private final IplImage current;
+ private int debugging = 0;
+
+ public ServableImage(int width, int height, int depth, int channels) {
+ this.width = width;
+ this.height = height;
+ this.depth = depth;
+ this.channels = channels;
+
+ current = IplImage.create(width, height, depth, channels);
+ }
+
+ /**
+ * @return the number of bytes in each image
+ */
+ public int imageSize() {
+ return width * height * depth * channels / 8;
+ }
+
+ /**
+ * @return the number of bits in each pixel
+ */
+ public short bitsPerPixel() {
+ return (short) (depth * channels);
+ }
+
+ /**
+ * Retrieves an image that should be used for debugging. It clears the value
+ * when called. {@link #releastSnapshot} MUST be called with the result.
+ *
+ * @param i
+ * Which snapshot to retrieve. 0 means the most recent final
+ * image.
+ * @return The most recent image at this index. {@code null} if there isn't
+ * a new one since last time this function was called. Will be in
+ * the correct color space to dump into a BMP image if this is a
+ * 3-channel image.
+ */
+ public synchronized IplImage getSnapshot(int i) {
+ if (snapshots.size() > i) {
+ return snapshots.get(i);
+ } else {
+ return null;
+ }
+ }
+
+ public synchronized void releaseSnapshot(int i, IplImage image) {
+ queues.get(i).add(image);
+ }
+
+ /**
+ * This function will return the same image if called repeatedly until
+ * {@link #releaseImage} is called.
+ *
+ * @return the current image
+ */
+ public synchronized IplImage getImage() {
+ return current;
+ }
+
+ /**
+ * Releases the current image (to be potentially sent out for debugging and
+ * then reused).
+ */
+ public synchronized void releaseImage() {
+ recordSnapshot(0);
+ }
+
+ /**
+ * Records a copy of the current image for debugging. It will be accessible
+ * at <{@code <base path>?i=<the value>}>. Does nothing unless
+ * {@link #isDebugging()}. This method <i>should</i> get called regardless
+ * of {@link #isDebugging()} to avoid outputting old debugging images. Note:
+ * 0 is not a valid snapshot number.
+ *
+ * @param i
+ * which snapshot this is
+ */
+ public synchronized void recordSnapshot(int i) {
+ while (queues.size() <= i) {
+ queues.add(null);
+ }
+ if (queues.get(i) == null) {
+ queues.set(i, new ArrayDeque<IplImage>());
+ }
+ while (snapshots.size() <= i) {
+ snapshots.add(null);
+ }
+ if (snapshots.get(i) != null) {
+ releaseSnapshot(i, snapshots.get(i));
+ }
+ if (isDebugging()) {
+ IplImage snapshot = queues.get(i).poll();
+ if (snapshot == null) {
+ snapshot = IplImage.create(width, height, depth, channels);
+ }
+ if (channels == 3) {
+ Natives.convertBGR2BMP(current.getByteBuffer(),
+ snapshot.getByteBuffer());
+ } else {
+ snapshot.getByteBuffer().put(current.getByteBuffer());
+ }
+ snapshots.set(i, snapshot);
+ } else {
+ snapshots.set(i, null);
+ }
+ }
+
+ /**
+ * @return whether or not to do extra debug work with the current image
+ */
+ public synchronized boolean isDebugging() {
+ return debugging > 0;
+ }
+
+ /**
+ * Whoever turns this on should turn it off when they're done.
+ */
+ synchronized void setDebugging(boolean debugging) {
+ if (debugging) {
+ ++this.debugging;
+ } else {
+ --this.debugging;
+ }
+ }
+}
diff --git a/aos/atom_code/camera/java/aos/Thresholder.java b/aos/atom_code/camera/java/aos/Thresholder.java
new file mode 100644
index 0000000..adf1b49
--- /dev/null
+++ b/aos/atom_code/camera/java/aos/Thresholder.java
@@ -0,0 +1,28 @@
+package aos;
+
+import com.googlecode.javacv.cpp.opencv_core;
+import com.googlecode.javacv.cpp.opencv_core.IplImage;
+
+public class Thresholder {
+ /**
+ * Thresholds in into out.
+ * All of the int arguments should be unsigned bytes except hoffset, which should be a signed byte.
+ * The min and max parameters specify what colors to accept.
+ * @param in The image to threshold. Must be a 3-channel {@link opencv_core#IPL_DEPTH_8U} image in the HSV color space.
+ * @param out Where to write the thresholded image to. Must be a 1-channel {@link opencv_core#IPL_DEPTH_8U} image.
+ * @param hoffset An offset to be added to the hue value before comparing it to {@code hmin} and {@code hmax}.
+ * The addition will be performed to a {@code uint8_t}, which will wrap around. This means that it must be positive.
+ * Useful for finding red values.
+ */
+ public static void threshold(IplImage in, IplImage out, int hoffset, int hmin, int hmax,
+ int smin, int smax, int vmin, int vmax) {
+ if (in.nChannels() != 3 || in.depth() != opencv_core.IPL_DEPTH_8U) {
+ throw new IllegalArgumentException("in is invalid");
+ }
+ if (out.nChannels() != 1 || out.depth() != opencv_core.IPL_DEPTH_8U) {
+ throw new IllegalArgumentException("out is invalid");
+ }
+ Natives.threshold(in.getByteBuffer(), out.getByteBuffer(), (short)hoffset,
+ (char)hmin, (char)hmax, (char)smin, (char)smax, (char)vmin, (char)vmax);
+ }
+}
diff --git a/aos/atom_code/camera/jni.cpp b/aos/atom_code/camera/jni.cpp
new file mode 100644
index 0000000..7b7eafa
--- /dev/null
+++ b/aos/atom_code/camera/jni.cpp
@@ -0,0 +1,257 @@
+#include <setjmp.h>
+
+#include "jni/aos_Natives.h"
+#include "aos/atom_code/camera/Buffers.h"
+#include "aos/externals/libjpeg/include/jpeglib.h"
+
+using aos::camera::Buffers;
+
+namespace {
+
+jclass nativeError, bufferError, outOfMemoryError;
+bool findClass(JNIEnv *env, const char *name, jclass *out) {
+ jclass local = env->FindClass(name);
+ if (out == NULL) return true;
+ *out = static_cast<jclass>(env->NewGlobalRef(local));
+ if (out == NULL) return true;
+ env->DeleteLocalRef(local);
+ return false;
+}
+
+// Checks that the size is correct and retrieves the address.
+// An expected_size of 0 means don't check it.
+// If this function returns NULL, a java exception will already have been
+// thrown.
+void *getBufferAddress(JNIEnv *env, jobject obj, jlong expected_size) {
+ if (obj == NULL) {
+ env->ThrowNew(nativeError, "null buffer");
+ return NULL;
+ }
+ if (expected_size != 0 &&
+ expected_size != env->GetDirectBufferCapacity(obj)) {
+ char *str;
+ if (asprintf(&str, "wrong size. expected %lld but got %lld",
+ expected_size, env->GetDirectBufferCapacity(obj)) < 0) {
+ env->ThrowNew(bufferError, "creating message failed");
+ return NULL;
+ }
+ env->ThrowNew(bufferError, str);
+ free(str);
+ return NULL;
+ }
+ void *const r = env->GetDirectBufferAddress(obj);
+ if (r == NULL) {
+ env->ThrowNew(bufferError, "couldn't get address");
+ }
+ return r;
+}
+
+const int kImagePixels = Buffers::kWidth * Buffers::kHeight;
+
+void jpeg_log_message(jpeg_common_struct *cinfo, log_level level) {
+ char buf[LOG_MESSAGE_LEN];
+ cinfo->err->format_message(cinfo, buf);
+ log_do(level, "libjpeg: %s\n", buf);
+}
+void jpeg_error_exit(jpeg_common_struct *cinfo) __attribute__((noreturn));
+void jpeg_error_exit(jpeg_common_struct *cinfo) {
+ jpeg_log_message(cinfo, ERROR);
+ longjmp(*static_cast<jmp_buf *>(cinfo->client_data), 1);
+}
+void jpeg_emit_message(jpeg_common_struct *cinfo, int msg_level) {
+ if (msg_level < 0) {
+ jpeg_log_message(cinfo, WARNING);
+ longjmp(*static_cast<jmp_buf *>(cinfo->client_data), 2);
+ }
+ // this spews a lot of messages out
+ //jpeg_log_message(cinfo, DEBUG);
+}
+
+// The structure used to hold all of the state for the functions that deal with
+// a Buffers. A pointer to this structure is stored java-side.
+struct BuffersHolder {
+ Buffers buffers;
+ timeval timestamp;
+ BuffersHolder() : buffers() {}
+};
+
+} // namespace
+
+void Java_aos_Natives_nativeInit(JNIEnv *env, jclass, jint width, jint height) {
+ if (findClass(env, "aos/NativeError", &nativeError)) return;
+ if (findClass(env, "aos/NativeBufferError", &bufferError)) return;
+ if (findClass(env, "java/lang/OutOfMemoryError", &outOfMemoryError)) return;
+
+ aos::InitNRT();
+
+ if (width != Buffers::kWidth || height != Buffers::kHeight) {
+ env->ThrowNew(nativeError, "dimensions mismatch");
+ return;
+ }
+
+ LOG(INFO, "nativeInit finished\n");
+}
+
+static_assert(sizeof(jlong) >= sizeof(void *),
+ "can't stick pointers into jlongs");
+
+jboolean Java_aos_Natives_decodeJPEG(JNIEnv *env, jclass, jlongArray stateArray,
+ jobject inobj, jint inLength,
+ jobject outobj) {
+ unsigned char *const in = static_cast<unsigned char *>(
+ getBufferAddress(env, inobj, 0));
+ if (in == NULL) return false;
+ if (env->GetDirectBufferCapacity(inobj) < inLength) {
+ env->ThrowNew(bufferError, "in is too small");
+ return false;
+ }
+ unsigned char *const out = static_cast<unsigned char *>(
+ getBufferAddress(env, outobj, kImagePixels * 3));
+ if (out == NULL) return false;
+
+ jpeg_decompress_struct *volatile cinfo; // volatile because of the setjmp call
+
+ jlong state;
+ env->GetLongArrayRegion(stateArray, 0, 1, &state);
+ if (env->ExceptionCheck()) return false;
+ if (state == 0) {
+ cinfo = static_cast<jpeg_decompress_struct *>(malloc(sizeof(*cinfo)));
+ if (cinfo == NULL) {
+ env->ThrowNew(outOfMemoryError, "malloc for jpeg_decompress_struct");
+ return false;
+ }
+ cinfo->err = jpeg_std_error(static_cast<jpeg_error_mgr *>(
+ malloc(sizeof(*cinfo->err))));
+ cinfo->client_data = malloc(sizeof(jmp_buf));
+ cinfo->err->error_exit = jpeg_error_exit;
+ cinfo->err->emit_message = jpeg_emit_message;
+ // if the error handler sees a failure, it needs to clean up
+ // (jpeg_abort_decompress) and then return the failure
+ // set cinfo->client_data to the jmp_buf
+ jpeg_create_decompress(cinfo);
+ state = reinterpret_cast<intptr_t>(cinfo);
+ env->SetLongArrayRegion(stateArray, 0, 1, &state);
+ if (env->ExceptionCheck()) return false;
+ } else {
+ cinfo = reinterpret_cast<jpeg_decompress_struct *>(state);
+ }
+
+ // set up the jump buffer
+ // this has to happen each time
+ if (setjmp(*static_cast<jmp_buf *>(cinfo->client_data))) {
+ jpeg_abort_decompress(cinfo);
+ return false;
+ }
+
+ jpeg_mem_src(cinfo, in, inLength);
+ jpeg_read_header(cinfo, TRUE);
+ if (cinfo->image_width != static_cast<unsigned int>(Buffers::kWidth) ||
+ cinfo->image_height != static_cast<unsigned int>(Buffers::kHeight)) {
+ LOG(WARNING, "got (%ux%u) image but expected (%dx%d)\n", cinfo->image_width,
+ cinfo->image_height, Buffers::kWidth, Buffers::kHeight);
+ jpeg_abort_decompress(cinfo);
+ return false;
+ }
+ cinfo->out_color_space = JCS_RGB;
+ jpeg_start_decompress(cinfo);
+ if (cinfo->output_components != 3) {
+ LOG(WARNING, "libjpeg wants to return %d color components instead of 3\n",
+ cinfo->out_color_components);
+ jpeg_abort_decompress(cinfo);
+ return false;
+ }
+ if (cinfo->output_width != static_cast<unsigned int>(Buffers::kWidth) ||
+ cinfo->output_height != static_cast<unsigned int>(Buffers::kHeight)) {
+ LOG(WARNING, "libjpeg wants to return a (%ux%u) image but need (%dx%d)\n",
+ cinfo->output_width, cinfo->output_height,
+ Buffers::kWidth, Buffers::kHeight);
+ jpeg_abort_decompress(cinfo);
+ return false;
+ }
+
+ unsigned char *buffers[Buffers::kHeight];
+ for (int i = 0; i < Buffers::kHeight; ++i) {
+ buffers[i] = &out[i * Buffers::kWidth * 3];
+ }
+ while (cinfo->output_scanline < cinfo->output_height) {
+ jpeg_read_scanlines(cinfo, &buffers[cinfo->output_scanline],
+ Buffers::kHeight - cinfo->output_scanline);
+ }
+
+ jpeg_finish_decompress(cinfo);
+ return true;
+}
+
+void Java_aos_Natives_threshold(JNIEnv *env, jclass, jobject inobj,
+ jobject outobj, jshort hoffset, jchar hmin,
+ jchar hmax, jchar smin, jchar smax, jchar vmin,
+ jchar vmax) {
+ const unsigned char *__restrict__ const in = static_cast<unsigned char *>(
+ getBufferAddress(env, inobj, kImagePixels * 3));
+ if (in == NULL) return;
+ char *__restrict__ const out = static_cast<char *>(
+ getBufferAddress(env, outobj, kImagePixels));
+ if (out == NULL) return;
+
+ for (int i = 0; i < kImagePixels; ++i) {
+ const uint8_t h = in[i * 3] + static_cast<uint8_t>(hoffset);
+ out[i] = h > hmin && h < hmax &&
+ in[i * 3 + 1] > smin && in[i * 3 + 1] < smax &&
+ in[i * 3 + 2] > vmin && in[i * 3 + 2] < vmax;
+ }
+}
+void Java_aos_Natives_convertBGR2BMP(JNIEnv *env, jclass,
+ jobject inobj, jobject outobj) {
+ const char *__restrict__ const in = static_cast<char *>(
+ getBufferAddress(env, inobj, kImagePixels * 3));
+ if (in == NULL) return;
+ char *__restrict__ const out = static_cast<char *>(
+ getBufferAddress(env, outobj, kImagePixels * 3));
+ if (out == NULL) return;
+
+ for (int i = 0; i < kImagePixels; ++i) {
+ out[i * 3 + 0] = in[i * 3 + 2];
+ out[i * 3 + 1] = in[i * 3 + 1];
+ out[i * 3 + 2] = in[i * 3 + 0];
+ }
+}
+
+jlong Java_aos_Natives_queueInit(JNIEnv *, jclass) {
+ return reinterpret_cast<intptr_t>(new BuffersHolder());
+}
+void Java_aos_Natives_queueReleaseJPEG(JNIEnv *, jclass, jlong ptr) {
+ reinterpret_cast<BuffersHolder *>(ptr)->buffers.Release();
+}
+jobject Java_aos_Natives_queueGetJPEG(JNIEnv *env, jclass, jlong ptr) {
+ uint32_t size;
+ BuffersHolder *const holder = reinterpret_cast<BuffersHolder *>(ptr);
+ const void *const r = holder->buffers.GetNext(true, &size,
+ &holder->timestamp, NULL);
+ if (r == NULL) return NULL;
+ return env->NewDirectByteBuffer(const_cast<void *>(r), size);
+}
+jdouble Java_aos_Natives_queueGetTimestamp(JNIEnv *, jclass, jlong ptr) {
+ const BuffersHolder *const holder = reinterpret_cast<BuffersHolder *>(ptr);
+ return holder->timestamp.tv_sec + holder->timestamp.tv_usec / 1000000.0;
+}
+
+void Java_aos_Natives_LOG(JNIEnv *env, jclass, jstring message, jint jlevel) {
+ log_level level;
+ if (jlevel >= 1000) {
+ // Don't want to use FATAL because the uncaught java exception that is
+ // likely to come next will be useful.
+ level = ERROR;
+ } else if (jlevel >= 900) {
+ level = WARNING;
+ } else if (jlevel >= 800) {
+ level = INFO;
+ } else {
+ level = DEBUG;
+ }
+ // can't use Get/ReleaseStringCritical because log_do might block waiting to
+ // put its message into the queue
+ const char *const message_chars = env->GetStringUTFChars(message, NULL);
+ if (message_chars == NULL) return;
+ log_do(level, "%s\n", message_chars);
+ env->ReleaseStringUTFChars(message, message_chars);
+}
diff --git a/aos/atom_code/core/BinaryLogReader.cpp b/aos/atom_code/core/BinaryLogReader.cpp
new file mode 100644
index 0000000..703a219
--- /dev/null
+++ b/aos/atom_code/core/BinaryLogReader.cpp
@@ -0,0 +1,123 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <time.h>
+#include <string.h>
+#include <string>
+#include <unistd.h>
+#include <sys/types.h>
+#include <pwd.h>
+#include <fcntl.h>
+
+#include <map>
+
+#include "aos/aos_core.h"
+#include "aos/atom_code/core/LogFileCommon.h"
+
+static const char *const kCRIOName = "CRIO";
+
+int main() {
+ aos::InitNRT();
+
+ char *folder_tmp;
+ if (asprintf(&folder_tmp, "%s/tmp/robot_logs", getpwuid(getuid())->pw_dir) == -1) {
+ LOG(ERROR, "couldn't figure out what folder to use because of %d (%s)\n",
+ errno, strerror(errno));
+ return EXIT_FAILURE;
+ }
+ std::string hack("/home/driver/tmp/robot_logs"); // TODO(brians) remove this hack
+ const char *folder = hack.c_str();
+ if (access(folder, R_OK | W_OK) == -1) {
+ fprintf(stderr,
+ "LogReader: error: folder '%s' does not exist. please create it\n",
+ folder);
+ return EXIT_FAILURE;
+ }
+ LOG(INFO, "logging to folder '%s'\n", folder);
+
+ const time_t t = time(NULL);
+ char *tmp;
+ if (asprintf(&tmp, "%s/aos_log-%jd", folder, static_cast<uintmax_t>(t)) == -1) {
+ fprintf(stderr, "BinaryLogReader: couldn't create final name because of %d (%s)."
+ " exiting\n", errno, strerror(errno));
+ return EXIT_FAILURE;
+ }
+ char *tmp2;
+ if (asprintf(&tmp2, "%s/aos_log-current", folder) == -1) {
+ fprintf(stderr, "BinaryLogReader: couldn't create symlink name because of %d (%s)."
+ " not creating current symlink\n", errno, strerror(errno));
+ } else {
+ if (unlink(tmp2) == -1 && (errno != EROFS && errno != ENOENT)) {
+ fprintf(stderr, "BinaryLogReader: warning: unlink('%s') failed because of %d (%s)\n",
+ tmp2, errno, strerror(errno));
+ }
+ if (symlink(tmp, tmp2) == -1) {
+ fprintf(stderr, "BinaryLogReader: warning: symlink('%s', '%s') failed"
+ " because of %d (%s)\n", tmp, tmp2, errno, strerror(errno));
+ }
+ free(tmp2);
+ }
+ int fd = open(tmp, O_SYNC | O_APPEND | O_RDWR | O_CREAT,
+ S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH | S_IWOTH);
+ free(tmp);
+ if (fd == -1) {
+ fprintf(stderr, "BinaryLogReader: couldn't open file '%s' because of %d (%s)."
+ " exiting\n", tmp, errno, strerror(errno));
+ return EXIT_FAILURE;
+ }
+ aos::LogFileAccessor writer(fd, true);
+
+ struct timespec timespec;
+ time_t last_sec = 0;
+ while (true) {
+ clock_gettime(CLOCK_MONOTONIC, ×pec);
+ if (last_sec != timespec.tv_sec) {
+ LOG(INFO, "msyncing output\n");
+ last_sec = timespec.tv_sec;
+ writer.Sync();
+ }
+
+ const log_message *const msg = log_read_next();
+ if (msg == NULL) continue;
+ const log_crio_message *const crio_msg = reinterpret_cast<const log_crio_message *>(
+ msg);
+
+ size_t name_size, message_size;
+ if (msg->source == -1) {
+ name_size = strlen(kCRIOName);
+ message_size = strlen(crio_msg->message);
+ } else {
+ name_size = strlen(msg->name);
+ message_size = strlen(msg->message);
+ }
+ // add on size for terminating '\0'
+ name_size += 1;
+ message_size += 1;
+
+ aos::LogFileMessageHeader *const output = writer.GetWritePosition(
+ sizeof(aos::LogFileMessageHeader) + name_size + message_size);
+ char *output_strings = reinterpret_cast<char *>(output) + sizeof(*output);
+ output->name_size = name_size;
+ output->message_size = message_size;
+ output->source = msg->source;
+ if (msg->source == -1) {
+ output->level = crio_msg->level;
+ // TODO(brians) figure out what time to put in
+ output->sequence = crio_msg->sequence;
+ memcpy(output_strings, kCRIOName, name_size);
+ memcpy(output_strings + name_size, crio_msg->message, message_size);
+ } else {
+ output->level = msg->level;
+ output->time_sec = msg->time.tv_sec;
+ output->time_nsec = msg->time.tv_nsec;
+ output->sequence = msg->sequence;
+ memcpy(output_strings, msg->name, name_size);
+ memcpy(output_strings + name_size, msg->message, message_size);
+ }
+ condition_set(&output->marker);
+
+ log_free_message(msg);
+ }
+
+ aos::Cleanup();
+}
diff --git a/aos/atom_code/core/CRIOLogReader.cpp b/aos/atom_code/core/CRIOLogReader.cpp
new file mode 100644
index 0000000..f32e21d
--- /dev/null
+++ b/aos/atom_code/core/CRIOLogReader.cpp
@@ -0,0 +1,129 @@
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netinet/tcp.h>
+#include <sys/select.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <sys/time.h>
+#include <fcntl.h>
+#include <arpa/inet.h>
+#include <errno.h>
+
+#include <map>
+
+#include "aos/aos_core.h"
+#include "aos/common/Configuration.h"
+#include "aos/common/byteorder.h"
+
+struct log_buffer {
+ log_crio_message msg;
+ size_t used;
+
+ log_buffer() {
+ Clear();
+ }
+ void Clear() {
+ used = 0;
+ }
+ // returns whether msg is now complete
+ bool ReceiveFrom(int sock) {
+ const ssize_t ret = recv(sock, reinterpret_cast<uint8_t *>(&msg) + used,
+ sizeof(msg) - used, 0);
+ if (ret == -1) {
+ LOG(ERROR, "recv(%d, %p, %d) failed because of %d: %s\n",
+ sock, reinterpret_cast<uint8_t *>(&msg) + used, sizeof(msg) - used,
+ errno, strerror(errno));
+ return false;
+ } else {
+ used += ret;
+ if (used > sizeof(msg)) {
+ LOG(WARNING, "used(%zd) is > sizeof(msg)(%zd)\n", used, sizeof(msg));
+ }
+ return used >= sizeof(msg);
+ }
+ }
+};
+
+int main() {
+ aos::InitNRT();
+
+ const int sock = socket(AF_INET, SOCK_STREAM, 0);
+ if (sock == -1) {
+ LOG(ERROR, "creating TCP socket failed because of %d: %s\n",
+ errno, strerror(errno));
+ return EXIT_FAILURE;
+ }
+ union {
+ sockaddr_in in;
+ sockaddr addr;
+ } bind_sockaddr;
+ memset(&bind_sockaddr, 0, sizeof(bind_sockaddr));
+ bind_sockaddr.in.sin_family = AF_INET;
+ bind_sockaddr.in.sin_port = htons(static_cast<uint16_t>(aos::NetworkPort::kLogs));
+ bind_sockaddr.in.sin_addr.s_addr = htonl(INADDR_ANY);
+ if (bind(sock, &bind_sockaddr.addr, sizeof(bind_sockaddr.addr)) == -1) {
+ LOG(ERROR, "bind(%d, %p) failed because of %d: %s\n", sock, &bind_sockaddr.addr,
+ errno, strerror(errno));
+ return EXIT_FAILURE;
+ }
+ if (listen(sock, 10) == -1) {
+ LOG(ERROR, "listen(%d) failed because of %d: %s\n", sock,
+ errno, strerror(errno));
+ return EXIT_FAILURE;
+ }
+ const int flags = fcntl(sock, F_GETFL, 0);
+ if (flags == -1) {
+ LOG(ERROR, "fcntl(%d, F_GETFL, 0) failed because of %d: %s\n", sock,
+ errno, strerror(errno));
+ return EXIT_FAILURE;
+ }
+ if (fcntl(sock, F_SETFL, flags | O_NONBLOCK) == -1) {
+ LOG(ERROR, "fcntl(%d, F_SETFL, %x) failed because of %d: %s\n",
+ sock, flags | O_NONBLOCK, errno, strerror(errno));
+ return EXIT_FAILURE;
+ }
+
+ std::map<int, log_buffer> socks;
+ fd_set read_fds;
+ while (true) {
+ FD_ZERO(&read_fds);
+ FD_SET(sock, &read_fds);
+ for (auto it = socks.begin(); it != socks.end(); ++it) {
+ FD_SET(it->first, &read_fds);
+ }
+ switch (select(FD_SETSIZE, &read_fds, NULL /*write*/, NULL /*err*/,
+ NULL /*timeout*/)) {
+ case -1:
+ LOG(ERROR, "select(FD_SETSIZE, %p, NULL, NULL, NULL) failed "
+ "because of %d: %s\n", &read_fds, errno, strerror(errno));
+ continue;
+ case 0:
+ LOG(ERROR, "select with NULL timeout timed out...\n");
+ continue;
+ }
+
+ if (FD_ISSET(sock, &read_fds)) {
+ const int new_sock = accept4(sock, NULL, NULL, SOCK_NONBLOCK);
+ if (new_sock == -1) {
+ LOG(ERROR, "accept4(%d, NULL, NULL, SOCK_NONBLOCK(=%d)) failed "
+ "because of %d: %s\n",
+ sock, SOCK_NONBLOCK, errno, strerror(errno));
+ } else {
+ socks[new_sock]; // creates using value's default constructor
+ }
+ }
+
+ for (auto it = socks.begin(); it != socks.end(); ++it) {
+ if (FD_ISSET(it->first, &read_fds)) {
+ if (it->second.ReceiveFrom(it->first)) {
+ it->second.msg.identifier = -1;
+ it->second.msg.time = aos::ntoh(it->second.msg.time);
+ log_crio_message_send(it->second.msg);
+ it->second.Clear();
+ }
+ }
+ }
+ }
+
+ aos::Cleanup();
+}
diff --git a/aos/atom_code/core/LogDisplayer.cpp b/aos/atom_code/core/LogDisplayer.cpp
new file mode 100644
index 0000000..9dee08f
--- /dev/null
+++ b/aos/atom_code/core/LogDisplayer.cpp
@@ -0,0 +1,157 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <getopt.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <inttypes.h>
+#include <errno.h>
+
+#include "aos/atom_code/core/LogFileCommon.h"
+#include "aos/aos_core.h"
+
+namespace {
+
+const char *kArgsHelp = "[OPTION]... [FILE]\n"
+ "Display log file FILE (created by BinaryLogReader) to stdout.\n"
+ "FILE is \"aos_log-current\" by default.\n"
+ "\n"
+ " -n, --name NAME only display entries from processes named NAME\n"
+ " -l, --level LEVEL "
+ "only display log entries at least as important as LEVEL\n"
+ " // -p, --pid PID only display log entries from process PID\n"
+ " -f, --follow "
+ "wait when the end of the file is reached (implies --end)\n"
+ " -t, --terminate stop when the end of file is reached (default)\n"
+ " -b, --beginning start at the beginning of the file (default)\n"
+ " -e, --end start at the end of the file\n"
+ " -s, --skip NUMBER skip NUMBER matching logs\n"
+ " // -m, --max NUMBER only display up to NUMBER logs\n"
+ " // -o, --format FORMAT use FORMAT to display log entries\n"
+ "\n"
+ "LEVEL must be DEBUG, INFO, WARNING, ERROR, or FATAL.\n"
+ " It defaults to INFO.\n"
+ "\n"
+ "TODO(brians) implement the commented out ones and changing FILE\n";
+
+void PrintHelpAndExit() {
+ fprintf(stderr, "Usage: %s %s", program_invocation_name, kArgsHelp);
+
+ exit(EXIT_SUCCESS);
+}
+
+} // namespace
+
+int main(int argc, char **argv) {
+ const char *filter_name = NULL;
+ log_level filter_level = INFO;
+ bool follow = false, start_at_beginning = true;
+ const char *filename = "aos_log-current";
+
+ while (true) {
+ static struct option long_options[] = {
+ {"name", required_argument, NULL, 'n'},
+ {"level", required_argument, NULL, 'l'},
+ {"pid", required_argument, NULL, 'p'},
+
+ {"follow", no_argument, NULL, 'f'},
+ {"terminate", no_argument, NULL, 't'},
+ {"beginning", no_argument, NULL, 'b'},
+ {"end", no_argument, NULL, 'e'},
+ {"skip", required_argument, NULL, 's'},
+ {"max", required_argument, NULL, 'm'},
+
+ {"format", required_argument, NULL, 'o'},
+
+ {"help", no_argument, NULL, 'h'},
+ {0, 0, 0, 0}
+ };
+ int option_index = 0;
+
+ const int c = getopt_long(argc, argv, "n:l:p:fts:m:o:h",
+ long_options, &option_index);
+ if (c == -1) { // if we're at the end
+ break;
+ }
+ switch (c) {
+ case 0:
+ fprintf(stderr, "LogDisplayer: got a 0 option but didn't set up any\n");
+ abort();
+ case 'n':
+ filter_name = optarg;
+ break;
+ case 'l':
+ filter_level = str_log(optarg);
+ if (filter_level == LOG_UNKNOWN) {
+ fprintf(stderr, "LogDisplayer: unknown log level '%s'\n", optarg);
+ exit(EXIT_FAILURE);
+ }
+ break;
+ case 'p':
+ abort();
+ break;
+ case 'f':
+ follow = true;
+ start_at_beginning = false;
+ break;
+ case 't':
+ follow = false;
+ break;
+ case 'b':
+ start_at_beginning = true;
+ break;
+ case 'e':
+ start_at_beginning = false;
+ break;
+ case 'm':
+ abort();
+ break;
+ case 'o':
+ abort();
+ break;
+ case 'h':
+ PrintHelpAndExit();
+ break;
+ case '?':
+ break;
+ default:
+ fprintf(stderr, "LogDisplayer: in a bad spot (%s: %d)\n", __FILE__, __LINE__);
+ abort();
+ }
+ }
+
+ fprintf(stderr, "displaying down to level %s from file '%s'\n",
+ log_str(filter_level), filename);
+ if (optind < argc) {
+ fprintf(stderr, "non-option ARGV-elements: ");
+ while (optind < argc) {
+ fprintf(stderr, "%s\n", argv[optind++]);
+ }
+ }
+
+ int fd = open(filename, O_RDONLY);
+ if (fd == -1) {
+ fprintf(stderr, "error: couldn't open file '%s' for reading because of %s\n",
+ filename, strerror(errno));
+ exit(EXIT_FAILURE);
+ }
+ aos::LogFileAccessor accessor(fd, false);
+ if (!start_at_beginning) {
+ accessor.MoveToEnd();
+ }
+ const aos::LogFileMessageHeader *msg;
+ do {
+ msg = accessor.ReadNextMessage(follow);
+ if (msg == NULL) continue;
+ if (log_gt_important(filter_level, msg->level)) continue;
+ if (filter_name != NULL &&
+ strcmp(filter_name, reinterpret_cast<const char *>(msg) + sizeof(*msg)) != 0) {
+ continue;
+ }
+ printf("%s(%"PRId32")(%03"PRId8"): %s at %010"PRIu64"s%09"PRIu64"ns: %s",
+ reinterpret_cast<const char *>(msg) + sizeof(*msg), msg->source,
+ msg->sequence, log_str(msg->level), msg->time_sec, msg->time_nsec,
+ reinterpret_cast<const char *>(msg) + sizeof(*msg) + msg->name_size);
+ } while (msg != NULL);
+}
+
diff --git a/aos/atom_code/core/LogFileCommon.h b/aos/atom_code/core/LogFileCommon.h
new file mode 100644
index 0000000..235c55b
--- /dev/null
+++ b/aos/atom_code/core/LogFileCommon.h
@@ -0,0 +1,192 @@
+#ifndef AOS_ATOM_CODE_CORE_LOG_FILE_COMMON_H_
+#define AOS_ATOM_CODE_CORE_LOG_FILE_COMMON_H_
+
+#include <stdio.h>
+#include <errno.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+
+#include <algorithm>
+
+#include "aos/aos_core.h"
+
+namespace aos {
+
+// File format: {
+// LogFileMessageHeader header;
+// char *name; // of the process that wrote the message
+// char *message;
+// } not crossing kPageSize boundaries into the file.
+//
+// Field sizes designed to fit the various values from log_message even on
+// other machines (hopefully) because they're baked into the files.
+struct __attribute__((aligned)) LogFileMessageHeader {
+ // gets condition_set once this one has been written
+ // for readers keeping up with a live writer
+ //
+ // gets initialized to 0 by ftruncate
+ //
+ // there will be something here after the last log on a "page" set to 2
+ // (by the condition_set) to indicate that the next log is on the next page
+ mutex marker;
+ static_assert(sizeof(marker) == 4, "mutex changed size!");
+ log_level level;
+ static_assert(sizeof(level) == 1, "log_level changed size!");
+
+ uint64_t time_sec;
+ static_assert(sizeof(time_sec) >= sizeof(log_message::time.tv_sec), "tv_sec won't fit");
+ uint64_t time_nsec;
+ static_assert(sizeof(time_nsec) >= sizeof(log_message::time.tv_nsec),
+ "tv_nsec won't fit");
+
+ int32_t source; // pid or -1 for crio
+ static_assert(sizeof(source) >= sizeof(log_message::source), "PIDs won't fit");
+ uint8_t sequence;
+ static_assert(sizeof(sequence) == sizeof(log_crio_message::sequence),
+ "something changed");
+ static_assert(sizeof(sequence) == sizeof(log_message::sequence),
+ "something changed");
+
+ // both including the terminating '\0'
+ uint32_t name_size;
+ uint32_t message_size;
+};
+static_assert(std::is_pod<LogFileMessageHeader>::value,
+ "LogFileMessageHeader will to get dumped to a file");
+
+// Handles the mmapping and munmapping for reading and writing log files.
+class LogFileAccessor {
+ private:
+ // The size of the chunks that get mmaped/munmapped together. Large enough so
+ // that not too much space is wasted and it's hopefully bigger than and a
+ // multiple of the system page size but small enough so that really large chunks
+ // of memory don't have to get mapped at the same time.
+ static const size_t kPageSize = 32768;
+ // What to align messages to. Necessary for futexes to work.
+ static const size_t kAlignment = 64;
+ static_assert(kAlignment >= __alignof__(mutex), "futexes will complain");
+
+ const int fd_;
+ const bool writable_;
+
+ off_t offset_; // into the file. will be aligned to kPageSize
+ char *current_;
+ size_t position_;
+
+ inline unsigned long SystemPageSize() {
+ static unsigned long r = sysconf(_SC_PAGESIZE);
+ return r;
+ }
+ void MapNextPage() {
+ if (writable_) {
+ if (ftruncate(fd_, offset_ + kPageSize) == -1) {
+ fprintf(stderr, "ftruncate(%d, %zd) failed with %d: %s. aborting\n",
+ fd_, kPageSize, errno, strerror(errno));
+ printf("see stderr\n");
+ abort();
+ }
+ }
+ current_ = static_cast<char *>(mmap(NULL, kPageSize,
+ PROT_READ | (writable_ ? PROT_WRITE : 0),
+ MAP_SHARED, fd_, offset_));
+ if (current_ == MAP_FAILED) {
+ fprintf(stderr, "mmap(NULL, %zd, PROT_READ | PROT_WRITE, MAP_SHARED, %d, %jd)"
+ " failed with %d: %s. aborting\n", kPageSize, fd_,
+ static_cast<intmax_t>(offset_), errno, strerror(errno));
+ printf("see stderr\n");
+ abort();
+ }
+ offset_ += kPageSize;
+ }
+ void Unmap(void *location) {
+ if (munmap(location, kPageSize) == -1) {
+ fprintf(stderr, "munmap(%p, %zd) failed with %d: %s. aborting\n",
+ location, kPageSize, errno, strerror(errno));
+ printf("see stderr\n");
+ abort();
+ }
+ }
+ public:
+ LogFileAccessor(int fd, bool writable) : fd_(fd), writable_(writable),
+ offset_(0), current_(0), position_(0) {
+ // check to make sure that mmap will allow mmaping in chunks of kPageSize
+ if (SystemPageSize() > kPageSize || (kPageSize % SystemPageSize()) != 0) {
+ fprintf(stderr, "LogFileCommon: system page size (%lu)"
+ " not compatible with kPageSize (%zd). aborting\n",
+ SystemPageSize(), kPageSize);
+ printf("see stderr\n");
+ abort();
+ }
+
+ MapNextPage();
+ }
+ // message_size should be the total number of bytes needed for the message
+ LogFileMessageHeader *GetWritePosition(size_t message_size) {
+ if (position_ + message_size + (kAlignment - (message_size % kAlignment)) +
+ sizeof(mutex) > kPageSize) {
+ char *const temp = current_;
+ MapNextPage();
+ if (condition_set_value(reinterpret_cast<mutex *>(&temp[position_]), 2) != 0) {
+ fprintf(stderr, "LogFileCommon: condition_set_value(%p, 2) failed with %d: %s."
+ " readers will hang\n", &temp[position_], errno, strerror(errno));
+ }
+ Unmap(temp);
+ position_ = 0;
+ }
+ LogFileMessageHeader *const r = reinterpret_cast<LogFileMessageHeader *>(
+ ¤t_[position_]);
+ position_ += message_size;
+ // keep it aligned for next time
+ position_ += kAlignment - (position_ % kAlignment);
+ return r;
+ }
+ // may only return NULL if wait is false
+ const LogFileMessageHeader *ReadNextMessage(bool wait) {
+ LogFileMessageHeader *r;
+ do {
+ r = reinterpret_cast<LogFileMessageHeader *>(¤t_[position_]);
+ if (wait) {
+ if (condition_wait(&r->marker) != 0) continue;
+ }
+ if (r->marker == 2) {
+ Unmap(current_);
+ MapNextPage();
+ position_ = 0;
+ r = reinterpret_cast<LogFileMessageHeader *>(current_);
+ }
+ } while (wait && r->marker == 0);
+ if (r->marker == 0) {
+ return NULL;
+ }
+ position_ += sizeof(LogFileMessageHeader) + r->name_size + r->message_size;
+ // keep it aligned for next time
+ position_ += kAlignment - (position_ % kAlignment);
+ return r;
+ }
+
+ // asynchronously syncs all open mappings
+ void Sync() {
+ msync(current_, kPageSize, MS_ASYNC | MS_INVALIDATE);
+ }
+
+ void MoveToEnd() {
+ Unmap(current_);
+ struct stat info;
+ if (fstat(fd_, &info) == -1) {
+ fprintf(stderr, "LOgFileCommon: fstat(%d, %p) failed with %d: %s\n",
+ fd_, &info, errno, strerror(errno));
+ printf("see stderr\n");
+ abort();
+ }
+ offset_ = info.st_size - kPageSize;
+ MapNextPage();
+ }
+};
+
+};
+
+#endif
+
diff --git a/aos/atom_code/core/LogStreamer.cpp b/aos/atom_code/core/LogStreamer.cpp
new file mode 100644
index 0000000..ceec18d
--- /dev/null
+++ b/aos/atom_code/core/LogStreamer.cpp
@@ -0,0 +1,44 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <time.h>
+#include <string.h>
+#include <string>
+#include <unistd.h>
+#include <sys/types.h>
+#include <pwd.h>
+#include <fcntl.h>
+#include <inttypes.h>
+
+#include "aos/aos_core.h"
+#include "aos/atom_code/core/LogFileCommon.h"
+
+static const char *const kCRIOName = "CRIO";
+
+int main() {
+ aos::InitNRT();
+
+ const time_t t = time(NULL);
+ printf("starting at %jd----------------------------------\n", static_cast<uintmax_t>(t));
+
+ int index = 0;
+ while (true) {
+ const log_message *const msg = log_read_next2(BLOCK, &index);
+ if (msg == NULL) continue;
+ const log_crio_message *const crio_msg = reinterpret_cast<const log_crio_message *>(
+ msg);
+
+ if (msg->source == -1) {
+ printf("CRIO(%03"PRId8"): %s at %f: %s", crio_msg->sequence,
+ log_str(crio_msg->level), crio_msg->time, crio_msg->message);
+ } else {
+ printf("%s(%"PRId32")(%03"PRId8"): %s at %010jus%09luns: %s",
+ msg->name, msg->source, msg->sequence, log_str(msg->level),
+ static_cast<uintmax_t>(msg->time.tv_sec), msg->time.tv_nsec, msg->message);
+ }
+
+ log_free_message(msg);
+ }
+
+ aos::Cleanup();
+}
diff --git a/aos/atom_code/core/core.cc b/aos/atom_code/core/core.cc
new file mode 100644
index 0000000..b34169b
--- /dev/null
+++ b/aos/atom_code/core/core.cc
@@ -0,0 +1,15 @@
+// This is the core scheduling system
+//
+// Purposes: All shared memory gets allocated here.
+//
+
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include "aos/aos_core.h"
+
+int main() {
+ aos::InitCreate();
+ select(0, NULL, NULL, NULL, NULL); // wait forever
+ aos::Cleanup();
+}
diff --git a/aos/atom_code/core/core.gyp b/aos/atom_code/core/core.gyp
new file mode 100644
index 0000000..04d3cc8
--- /dev/null
+++ b/aos/atom_code/core/core.gyp
@@ -0,0 +1,54 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'core',
+ 'type': 'executable',
+ 'sources': [
+ 'core.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ {
+ 'target_name': 'BinaryLogReader',
+ 'type': 'executable',
+ 'sources': [
+ 'BinaryLogReader.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ {
+ 'target_name': 'LogStreamer',
+ 'type': 'executable',
+ 'sources': [
+ 'LogStreamer.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ {
+ 'target_name': 'LogDisplayer',
+ 'type': 'executable',
+ 'sources': [
+ 'LogDisplayer.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ {
+ 'target_name': 'CRIOLogReader',
+ 'type': 'executable',
+ 'sources': [
+ 'CRIOLogReader.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ ],
+}
diff --git a/aos/atom_code/init.cc b/aos/atom_code/init.cc
new file mode 100644
index 0000000..4776261
--- /dev/null
+++ b/aos/atom_code/init.cc
@@ -0,0 +1,115 @@
+#include "aos/atom_code/init.h"
+
+#include <stdio.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <errno.h>
+#include <sched.h>
+#include <sys/resource.h>
+#include <asm-generic/resource.h> // for RLIMIT_RTTIME
+#include <sys/types.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdint.h>
+
+#include "aos/aos_core.h"
+#include "aos/common/die.h"
+
+namespace aos {
+
+namespace {
+
+void SetSoftRLimit(int resource, rlim64_t soft) {
+ // If we're root, then we don't need to change the soft limits because none of
+ // the ones we care about get checked for root.
+ if (getuid() != 0) {
+ struct rlimit64 rlim;
+ if (getrlimit64(resource, &rlim) == -1) {
+ Die("%s-init: getrlimit64(%d) failed with %d (%s)\n",
+ program_invocation_short_name, resource, errno, strerror(errno));
+ }
+ rlim.rlim_cur = soft;
+ if (setrlimit64(resource, &rlim) == -1) {
+ Die("%s-init: setrlimit64(%d, {cur=%jd,max=%jd})"
+ " failed with %d (%s)\n", program_invocation_short_name,
+ resource, (intmax_t)rlim.rlim_cur, (intmax_t)rlim.rlim_max,
+ errno, strerror(errno));
+ }
+ }
+}
+
+// Common stuff that needs to happen at the beginning of both the realtime and
+// non-realtime initialization sequences. May be called twice.
+void InitStart() {
+ // Allow locking as much as we want into RAM.
+ SetSoftRLimit(RLIMIT_MEMLOCK, RLIM_INFINITY);
+}
+
+int LockAllMemory() {
+ InitStart();
+ if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
+ Die("%s-init: mlockall failed with %d (%s)\n",
+ program_invocation_short_name, errno, strerror(errno));
+ }
+
+ // Forces the memory pages for all the stack space that we're ever going to
+ // use to be loaded into memory (so it can be locked there).
+ uint8_t data[4096 * 8];
+ // Not 0 because linux might optimize that to a 0-filled page.
+ memset(data, 1, sizeof(data));
+
+ return 0;
+}
+
+// Do the initialization code that is necessary for both realtime and
+// non-realtime processes.
+void DoInitNRT(aos_core_create create) {
+ InitStart();
+ if (aos_core_create_shared_mem(create)) {
+ Die("%s-init: creating shared memory reference failed\n",
+ program_invocation_short_name);
+ }
+ if (log_init(program_invocation_short_name) != 0) {
+ Die("%s-init: initializing logging failed\n",
+ program_invocation_short_name);
+ }
+}
+
+const char *const kNoRealtimeEnvironmentVariable = "AOS_NO_REALTIME";
+
+} // namespace
+
+void InitNRT() { DoInitNRT(aos_core_create::reference); }
+void InitCreate() { DoInitNRT(aos_core_create::create); }
+void Init() {
+ if (getenv(kNoRealtimeEnvironmentVariable) == NULL) { // if nobody set it
+ LockAllMemory();
+ // Only let rt processes run for 1 second straight.
+ SetSoftRLimit(RLIMIT_RTTIME, 1000000);
+ // Allow rt processes up to priority 40.
+ SetSoftRLimit(RLIMIT_RTPRIO, 40);
+ // Set our process to priority 40.
+ struct sched_param param;
+ param.sched_priority = 40;
+ if (sched_setscheduler(0, SCHED_FIFO, ¶m) != 0) {
+ Die("%s-init: setting SCHED_FIFO failed with %d (%s)\n",
+ program_invocation_short_name, errno, strerror(errno));
+ }
+ } else {
+ fprintf(stderr, "%s not doing realtime initialization because environment"
+ " variable %s is set\n", program_invocation_short_name,
+ kNoRealtimeEnvironmentVariable);
+ printf("no realtime for %s. see stderr\n", program_invocation_short_name);
+ }
+
+ InitNRT();
+}
+
+void Cleanup() {
+ if (aos_core_free_shared_mem()) {
+ Die("%s-init: freeing shared mem failed\n",
+ program_invocation_short_name);
+ }
+}
+
+} // namespace aos
diff --git a/aos/atom_code/init.h b/aos/atom_code/init.h
new file mode 100644
index 0000000..ffb7afc
--- /dev/null
+++ b/aos/atom_code/init.h
@@ -0,0 +1,19 @@
+#ifndef AOS_ATOM_CODE_INIT_H_
+#define AOS_ATOM_CODE_INIT_H_
+
+namespace aos {
+
+// Does the non-realtime parts of the initialization process.
+void InitNRT();
+// Initializes everything, including the realtime stuff.
+void Init();
+// Same as InitNRT, except will remove an existing shared memory file and create
+// a new one.
+void InitCreate();
+// Cleans up (probably not going to get called very often because few things can
+// exit gracefully).
+void Cleanup();
+
+} // namespace aos
+
+#endif // AOS_ATOM_CODE_INIT_H_
diff --git a/aos/atom_code/input/FRCComm.h b/aos/atom_code/input/FRCComm.h
new file mode 100644
index 0000000..3d2f4c0
--- /dev/null
+++ b/aos/atom_code/input/FRCComm.h
@@ -0,0 +1,115 @@
+/*************************************************************
+ * NOTICE
+ *
+ * These are the only externally exposed functions to the
+ * NetworkCommunication library
+ *
+ * This is an implementation of FRC Spec for Comm Protocol
+ * Revision 4.5, June 30, 2008
+ *
+ * Copyright (c) National Instruments 2008. All Rights Reserved.
+ *
+ *************************************************************/
+
+#ifndef __FRC_COMM_H__
+#define __FRC_COMM_H__
+
+#include <stdint.h>
+
+typedef uint64_t UINT64;
+typedef uint32_t UINT32;
+typedef uint16_t UINT16;
+typedef uint8_t UINT8;
+typedef int8_t INT8;
+
+struct FRCCommonControlData{
+ UINT16 packetIndex;
+ union {
+ UINT8 control;
+ struct {
+ /*the order of these are flipped on the fit pc side to make it work*/
+ UINT8 fpgaChkSum :1;
+ UINT8 cRIOChkSum :1;
+ UINT8 resync : 1;
+ UINT8 fmsAttached:1;
+ UINT8 autonomous : 1;
+ UINT8 enabled : 1;
+ UINT8 notEStop : 1;
+ UINT8 reset : 1;
+ };
+ };
+ UINT8 dsDigitalIn;
+ UINT16 teamID;
+
+ char dsID_Alliance;
+ char dsID_Position;
+
+ union {
+ INT8 stick0Axes[6];
+ struct {
+ INT8 stick0Axis1;
+ INT8 stick0Axis2;
+ INT8 stick0Axis3;
+ INT8 stick0Axis4;
+ INT8 stick0Axis5;
+ INT8 stick0Axis6;
+ };
+ };
+ UINT16 stick0Buttons; // Left-most 4 bits are unused
+
+ union {
+ INT8 stick1Axes[6];
+ struct {
+ INT8 stick1Axis1;
+ INT8 stick1Axis2;
+ INT8 stick1Axis3;
+ INT8 stick1Axis4;
+ INT8 stick1Axis5;
+ INT8 stick1Axis6;
+ };
+ };
+ UINT16 stick1Buttons; // Left-most 4 bits are unused
+
+ union {
+ INT8 stick2Axes[6];
+ struct {
+ INT8 stick2Axis1;
+ INT8 stick2Axis2;
+ INT8 stick2Axis3;
+ INT8 stick2Axis4;
+ INT8 stick2Axis5;
+ INT8 stick2Axis6;
+ };
+ };
+ UINT16 stick2Buttons; // Left-most 4 bits are unused
+
+ union {
+ INT8 stick3Axes[6];
+ struct {
+ INT8 stick3Axis1;
+ INT8 stick3Axis2;
+ INT8 stick3Axis3;
+ INT8 stick3Axis4;
+ INT8 stick3Axis5;
+ INT8 stick3Axis6;
+ };
+ };
+ UINT16 stick3Buttons; // Left-most 4 bits are unused
+
+ //Analog inputs are 10 bit right-justified
+ UINT16 analog1;
+ UINT16 analog2;
+ UINT16 analog3;
+ UINT16 analog4;
+
+ UINT64 cRIOChecksum;
+ UINT32 FPGAChecksum0;
+ UINT32 FPGAChecksum1;
+ UINT32 FPGAChecksum2;
+ UINT32 FPGAChecksum3;
+
+ char versionData[8];
+};
+
+
+#endif
diff --git a/aos/atom_code/input/JoystickInput.cpp b/aos/atom_code/input/JoystickInput.cpp
new file mode 100644
index 0000000..6aeca4b
--- /dev/null
+++ b/aos/atom_code/input/JoystickInput.cpp
@@ -0,0 +1,66 @@
+#include "aos/atom_code/input/JoystickInput.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/Configuration.h"
+#include "aos/common/network/ReceiveSocket.h"
+#include "aos/common/messages/RobotState.q.h"
+
+namespace aos {
+
+void JoystickInput::SetupButtons() {
+ for (int i = 0; i < 4; ++i) {
+ old_buttons[i] = buttons[i];
+ }
+ buttons[0] = control_data_.stick0Buttons;
+ buttons[1] = control_data_.stick1Buttons;
+ buttons[2] = control_data_.stick2Buttons;
+ buttons[3] = control_data_.stick3Buttons;
+
+ buttons[0] |= (control_data_.enabled << (ENABLED - 9)) |
+ (control_data_.autonomous << (AUTONOMOUS - 9)) |
+ (control_data_.fmsAttached << (FMS_ATTACHED - 9));
+
+ for (int j = 0; j < 4; ++j) {
+ for (int k = 1; k <= 12; ++k) {
+ if (PosEdge(j, k)) {
+ LOG(INFO, "PosEdge(%d, %d)\n", j, k);
+ }
+ if (NegEdge(j, k)) {
+ LOG(INFO, "NegEdge(%d, %d)\n", j, k);
+ }
+ }
+ }
+ if (PosEdge(0, ENABLED)) LOG(INFO, "PosEdge(ENABLED)\n");
+ if (NegEdge(0, ENABLED)) LOG(INFO, "NegEdge(ENABLED)\n");
+ if (PosEdge(0, AUTONOMOUS)) LOG(INFO, "PosEdge(AUTONOMOUS)\n");
+ if (NegEdge(0, AUTONOMOUS)) LOG(INFO, "NegEdge(AUTONOMOUS)\n");
+ if (PosEdge(0, FMS_ATTACHED)) LOG(INFO, "PosEdge(FMS_ATTACHED)\n");
+ if (NegEdge(0, FMS_ATTACHED)) LOG(INFO, "NegEdge(FMS_ATTACHED)\n");
+}
+
+void JoystickInput::Run() {
+ ReceiveSocket sock(NetworkPort::kDS);
+ while (true) {
+ if (sock.Recv(&control_data_, sizeof(control_data_)) == -1) {
+ LOG(WARNING, "socket receive failed\n");
+ continue;
+ }
+ SetupButtons();
+ if (!robot_state.MakeWithBuilder().enabled(Pressed(0, ENABLED)).
+ autonomous(Pressed(0, AUTONOMOUS)).
+ team_id(ntohs(control_data_.teamID)).Send()) {
+ LOG(WARNING, "sending robot_state failed\n");
+ }
+ if (robot_state.FetchLatest()) {
+ char state[1024];
+ robot_state->Print(state, sizeof(state));
+ LOG(DEBUG, "robot_state={%s}\n", state);
+ } else {
+ LOG(WARNING, "fetching robot_state failed\n");
+ }
+ RunIteration();
+ }
+}
+
+} // namespace aos
+
diff --git a/aos/atom_code/input/JoystickInput.h b/aos/atom_code/input/JoystickInput.h
new file mode 100644
index 0000000..de5de04
--- /dev/null
+++ b/aos/atom_code/input/JoystickInput.h
@@ -0,0 +1,45 @@
+#ifndef AOS_INPUT_JOYSTICK_INPUT_H_
+#define AOS_INPUT_JOYSTICK_INPUT_H_
+
+#include "FRCComm.h"
+
+namespace aos {
+
+// Class for implementing atom code that reads the joystick values from the
+// cRIO.
+// Designed for a subclass that implements RunIteration to be instantiated and
+// Runed.
+class JoystickInput {
+ private:
+ uint16_t buttons[4], old_buttons[4];
+ inline uint16_t MASK(int button) {
+ return 1 << ((button > 8) ? (button - 9) : (button + 7));
+ }
+ void SetupButtons();
+ protected:
+ FRCCommonControlData control_data_;
+
+ // Constants that retrieve data when used with joystick 0.
+ static const int ENABLED = 13;
+ static const int AUTONOMOUS = 14;
+ static const int FMS_ATTACHED = 15;
+ bool Pressed(int stick, int button) {
+ return buttons[stick] & MASK(button);
+ }
+ bool PosEdge(int stick, int button) {
+ return !(old_buttons[stick] & MASK(button)) && (buttons[stick] & MASK(button));
+ }
+ bool NegEdge(int stick, int button) {
+ return (old_buttons[stick] & MASK(button)) && !(buttons[stick] & MASK(button));
+ }
+
+ virtual void RunIteration() = 0;
+ public:
+ // Enters an infinite loop that reads values and calls RunIteration.
+ void Run();
+};
+
+} // namespace aos
+
+#endif
+
diff --git a/aos/atom_code/input/input.gyp b/aos/atom_code/input/input.gyp
new file mode 100644
index 0000000..e77fc47
--- /dev/null
+++ b/aos/atom_code/input/input.gyp
@@ -0,0 +1,14 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'joystick',
+ 'type': 'static_library',
+ 'sources': [
+ 'JoystickInput.cpp'
+ ],
+ 'dependencies': [
+ '<(AOS)/common/messages/messages.gyp:aos_queues',
+ ]
+ },
+ ],
+}
diff --git a/aos/atom_code/ipc_lib/aos_sync.c b/aos/atom_code/ipc_lib/aos_sync.c
new file mode 100644
index 0000000..a9a4779
--- /dev/null
+++ b/aos/atom_code/ipc_lib/aos_sync.c
@@ -0,0 +1,135 @@
+#include <stdio.h>
+#include <linux/futex.h>
+#include <unistd.h>
+#include <sys/syscall.h>
+#include <errno.h>
+#include "aos_sync.h"
+#include "cmpxchg.h"
+#include <stdint.h>
+#include <limits.h>
+#include <string.h>
+
+// this code is based on something that appears to be based on <http://www.akkadia.org/drepper/futex.pdf>, which also has a lot of useful information
+// should probably use <http://lxr.linux.no/linux+v2.6.34/Documentation/robust-futexes.txt> once it becomes available
+// <http://locklessinc.com/articles/futex_cheat_sheet/> and <http://locklessinc.com/articles/mutex_cv_futex/> are useful
+// <http://lwn.net/Articles/360699/> has a nice overview of futexes in late 2009 (fairly recent compared to everything else...)
+// can't use PRIVATE futex operations because they use the pid (or something) as part of the hash
+//
+// Values for a mutex:
+// 0 = unlocked
+// 1 = locked, not contended
+// 2 = locked, probably contended
+// Values for a condition:
+// 0 = unset
+// 1 = set
+
+static inline int sys_futex(mutex *addr1, int op, int val1, const struct timespec *timeout,
+ void *addr2, int val3) {
+ return syscall(SYS_futex, addr1, op, val1, timeout, addr2, val3);
+}
+
+static inline int mutex_get(mutex *m, uint8_t signals_fail, const
+ struct timespec *timeout) {
+ int c;
+ c = cmpxchg(m, 0, 1);
+ if (!c) return 0;
+ /* The lock is now contended */
+ if (c == 1) c = xchg(m, 2);
+ while (c) {
+ /* Wait in the kernel */
+ //printf("sync here %d\n", __LINE__);
+ if (sys_futex(m, FUTEX_WAIT, 2, timeout, NULL, 0) == -1) {
+ if (signals_fail && errno == EINTR) {
+ return 1;
+ }
+ if (timeout != NULL && errno == ETIMEDOUT) {
+ return 2;
+ }
+ }
+ //printf("sync here %d\n", __LINE__);
+ c = xchg(m, 2);
+ }
+ return 0;
+}
+int mutex_lock(mutex *m) {
+ return mutex_get(m, 1, NULL);
+}
+int mutex_lock_timeout(mutex *m, const struct timespec *timeout) {
+ return mutex_get(m, 1, timeout);
+}
+int mutex_grab(mutex *m) {
+ return mutex_get(m, 0, NULL);
+}
+
+int mutex_unlock(mutex *m) {
+ /* Unlock, and if not contended then exit. */
+ //printf("mutex_unlock(%p) => %d \n",m,*m);
+ switch (xchg(m, 0)) {
+ case 0:
+ fprintf(stderr, "sync: multiple unlock of %p. aborting\n", m);
+ printf("see stderr\n");
+ abort();
+ case 1:
+ //printf("mutex_unlock return(%p) => %d \n",m,*m);
+ return 0;
+ case 2:
+ if (sys_futex(m, FUTEX_WAKE, 1, NULL, NULL, 0) == -1) {
+ fprintf(stderr, "sync: waking 1 from %p failed with %d: %s\n",
+ m, errno, strerror(errno));
+ return -1;
+ } else {
+ return 0;
+ }
+ default:
+ fprintf(stderr, "sync: got a garbage value from mutex %p. aborting\n",
+ m);
+ printf("see stderr\n");
+ abort();
+ }
+}
+int mutex_trylock(mutex *m) {
+ /* Try to take the lock, if is currently unlocked */
+ unsigned c = cmpxchg(m, 0, 1);
+ if (!c) return 0;
+ return 1;
+}
+
+int condition_wait(mutex *m) {
+ if (*m) {
+ return 0;
+ }
+ if (sys_futex(m, FUTEX_WAIT, 0, NULL, NULL, 0) == -1) {
+ if (errno == EINTR) {
+ return 1;
+ } else if (errno != EWOULDBLOCK) {
+ return -1;
+ }
+ }
+ return 0;
+}
+int condition_wait_force(mutex *m) {
+ while (1) {
+ if (sys_futex(m, FUTEX_WAIT, *m, NULL, NULL, 0) == -1) {
+ if (errno != EWOULDBLOCK) { // if it was an actual problem
+ if (errno == EINTR) {
+ return 1;
+ } else {
+ return -1;
+ }
+ }
+ } else {
+ return 0;
+ }
+ }
+}
+inline int condition_set_value(mutex *m, mutex value) {
+ xchg(m, value);
+ return sys_futex(m, FUTEX_WAKE, INT_MAX, NULL, NULL, 0);
+}
+int condition_set(mutex *m) {
+ return condition_set_value(m, 1);
+}
+int condition_unset(mutex *m) {
+ return !xchg(m, 0);
+}
+
diff --git a/aos/atom_code/ipc_lib/aos_sync.h b/aos/atom_code/ipc_lib/aos_sync.h
new file mode 100644
index 0000000..3c66264
--- /dev/null
+++ b/aos/atom_code/ipc_lib/aos_sync.h
@@ -0,0 +1,63 @@
+#ifndef AOS_IPC_LIB_SYNC_H_
+#define AOS_IPC_LIB_SYNC_H_
+
+#include <stdlib.h>
+#include <signal.h>
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif // __cplusplus
+
+// TODO(brians) add client requests to make helgrind useful with this code
+// <http://www.valgrind.org/docs/manual/hg-manual.html#hg-manual.client-requests>
+// and <http://www.valgrind.org/docs/manual/drd-manual.html#drd-manual.clientreqs>
+// list the interesting ones
+
+// Have to align structs containing it to to sizeof(int).
+// Valid initial values for use with mutex_ functions are 0 (unlocked) and 1 (locked).
+// Valid initial values for use with condition_ functions are 0 (unset) and 1 (set).
+// The value should not be changed after multiple processes have started
+// accessing an instance except through the functions declared in this file.
+typedef volatile uint32_t mutex __attribute__((aligned(sizeof(int))));
+
+// All return -1 for other error (which will be in errno from futex(2)).
+
+// Returns 1 if interrupted by a signal.
+int mutex_lock(mutex *m) __attribute__((warn_unused_result));
+// Returns 2 if it timed out or 1 if interrupted by a signal.
+int mutex_lock_timeout(mutex *m, const struct timespec *timeout)
+ __attribute__((warn_unused_result));
+// Ignores signals. Can not fail.
+int mutex_grab(mutex *m);
+// Returns 1 for multiple unlocking and -1 if something bad happened and
+// whoever's waiting didn't get woken up.
+int mutex_unlock(mutex *m);
+// Returns 0 when successful in locking the mutex and 1 if somebody else has it
+// locked.
+int mutex_trylock(mutex *m) __attribute__((warn_unused_result));
+
+// The condition_ functions are similar to the mutex_ ones but different.
+// They are designed for signalling when something happens (possibly to
+// multiple listeners). A mutex manipulated with them can only be set or unset.
+
+// Wait for the condition to be set. Will return immediately if it's already set.
+// Returns 0 if successful or it was already set, 1 if interrupted by a signal, or -1.
+int condition_wait(mutex *m) __attribute__((warn_unused_result));
+// Will wait for the next condition_set, even if the condition is already set.
+// Returns 0 if successful, 1 if interrupted by a signal, or -1.
+int condition_wait_force(mutex *m) __attribute__((warn_unused_result));
+// Set the condition and wake up anybody waiting on it.
+// Returns the number that were woken or -1.
+int condition_set(mutex *m);
+// Same as above except lets something other than 1 be used as the final value.
+int condition_set_value(mutex *m, mutex value);
+// Unsets the condition.
+// Returns 0 if it was set before and 1 if it wasn't.
+int condition_unset(mutex *m);
+
+#ifdef __cplusplus
+}
+#endif // __cplusplus
+
+#endif
diff --git a/aos/atom_code/ipc_lib/binheap.c b/aos/atom_code/ipc_lib/binheap.c
new file mode 100644
index 0000000..8eb024d
--- /dev/null
+++ b/aos/atom_code/ipc_lib/binheap.c
@@ -0,0 +1,125 @@
+#include "binheap.h"
+#include <stdlib.h>
+#include <stdio.h>
+
+#ifndef TESTING_ASSERT
+#define TESTING_ASSERT(...)
+#endif
+#define Error(x) TESTING_ASSERT(0, x)
+
+#define MinData (0)
+
+void Initialize( int Elements, PriorityQueue H )
+{
+ H->Capacity = Elements - 1;
+ H->Size = 0;
+ H->Elements[ 0 ] = MinData;
+}
+
+int Insert( ElementType X, PriorityQueue H )
+{
+ int i;
+
+ if( IsFull( H ) )
+ {
+ return -1;
+ }
+
+ for( i = ++H->Size; H->Elements[ i / 2 ] > X; i /= 2 )
+ H->Elements[ i ] = H->Elements[ i / 2 ];
+ H->Elements[ i ] = X;
+ return 0;
+}
+
+void Remove( ElementType X, PriorityQueue H )
+{
+ int i, Child, removed = 0;
+ ElementType LastElement;
+
+ for ( i = 1; i <= H->Size; ++i )
+ {
+ if( H->Elements[ i ] == X )
+ {
+ removed = i;
+ break;
+ }
+ }
+ if( removed == 0 )
+ {
+ fprintf(stderr, "could not find element %d to remove. not removing any\n", X);
+ return;
+ }
+
+ LastElement = H->Elements[ H->Size-- ];
+
+ for( i = removed; i * 2 <= H->Size; i = Child )
+ {
+ /* Find smaller child */
+ Child = i * 2;
+ if( Child != H->Size && H->Elements[ Child + 1 ]
+ < H->Elements[ Child ] )
+ Child++;
+
+ /* Percolate one level */
+ if( LastElement > H->Elements[ Child ] )
+ H->Elements[ i ] = H->Elements[ Child ];
+ else
+ break;
+ }
+ H->Elements[ i ] = LastElement;
+}
+
+ElementType DeleteMin( PriorityQueue H )
+{
+ int i, Child;
+ ElementType MinElement, LastElement;
+
+ if( IsEmpty( H ) )
+ {
+ Error( "Priority queue is empty" );
+ return H->Elements[ 0 ];
+ }
+ MinElement = H->Elements[ 1 ];
+ LastElement = H->Elements[ H->Size-- ];
+
+ for( i = 1; i * 2 <= H->Size; i = Child )
+ {
+ /* Find smaller child */
+ Child = i * 2;
+ if( Child != H->Size && H->Elements[ Child + 1 ]
+ < H->Elements[ Child ] )
+ Child++;
+
+ /* Percolate one level */
+ if( LastElement > H->Elements[ Child ] )
+ H->Elements[ i ] = H->Elements[ Child ];
+ else
+ break;
+ }
+ H->Elements[ i ] = LastElement;
+ return MinElement;
+}
+
+ElementType GetMin( PriorityQueue H )
+{
+ if( !IsEmpty( H ) )
+ return H->Elements[ 1 ];
+ Error( "Priority Queue is Empty" );
+ return H->Elements[ 0 ];
+}
+
+int IsEmpty( PriorityQueue H )
+{
+ return H->Size == 0;
+}
+
+int IsFull( PriorityQueue H )
+{
+ return H->Size == H->Capacity;
+}
+
+int GetSize( PriorityQueue H )
+{
+ return H->Size;
+}
+
diff --git a/aos/atom_code/ipc_lib/binheap.h b/aos/atom_code/ipc_lib/binheap.h
new file mode 100644
index 0000000..8c26f9f
--- /dev/null
+++ b/aos/atom_code/ipc_lib/binheap.h
@@ -0,0 +1,29 @@
+#ifndef _BinHeap_H
+#define _BinHeap_H
+
+#include <stdint.h>
+
+typedef uint8_t ElementType;
+struct HeapStruct;
+typedef struct HeapStruct *PriorityQueue;
+
+struct HeapStruct
+{
+ int Capacity;
+ int Size;
+ ElementType *Elements;
+};
+
+// Elements is the number allocated at H->Elements
+void Initialize( int Elements, PriorityQueue H );
+// 0 if successful, -1 if full
+int Insert( ElementType X, PriorityQueue H );
+void Remove( ElementType X, PriorityQueue H );
+ElementType DeleteMin( PriorityQueue H );
+ElementType GetMin( PriorityQueue H );
+int IsEmpty( PriorityQueue H );
+int IsFull( PriorityQueue H );
+int GetSize( PriorityQueue H );
+
+#endif
+
diff --git a/aos/atom_code/ipc_lib/binheap_test.cpp b/aos/atom_code/ipc_lib/binheap_test.cpp
new file mode 100644
index 0000000..62eecd4
--- /dev/null
+++ b/aos/atom_code/ipc_lib/binheap_test.cpp
@@ -0,0 +1,65 @@
+extern "C" {
+#include "binheap.h"
+}
+
+#include <gtest/gtest.h>
+
+class BinHeapTest : public testing::Test{
+ protected:
+ static const int TEST_ELEMENTS = 57;
+ PriorityQueue queue;
+ virtual void SetUp(){
+ queue = new HeapStruct();
+ queue->Elements = new uint8_t[TEST_ELEMENTS];
+ Initialize(TEST_ELEMENTS, queue);
+ }
+ virtual void TearDown(){
+ delete[] queue->Elements;
+ delete queue;
+ }
+};
+
+std::ostream& operator<< (std::ostream& o, uint8_t c){
+ return o<<(int)c;
+}
+
+testing::AssertionResult Contains(PriorityQueue queue, const uint8_t expected[], size_t length){
+ for(size_t i = 0; i < length; ++i){
+ //printf("expected[%d]=%d\n", i, expected[i]);
+ if(DeleteMin(queue) != expected[i]){
+ return testing::AssertionFailure() << "queue[" << i << "] != " << expected[i];
+ }
+ }
+ if(!IsEmpty(queue))
+ return testing::AssertionFailure() << "queue is longer than " << length;
+ return ::testing::AssertionSuccess();
+}
+
+TEST_F(BinHeapTest, SingleElement){
+ Insert(87, queue);
+ EXPECT_EQ(87, DeleteMin(queue));
+ EXPECT_TRUE(IsEmpty(queue));
+}
+TEST_F(BinHeapTest, MultipleElements){
+ Insert(54, queue);
+ Insert(1, queue);
+ Insert(0, queue);
+ Insert(255, queue);
+ Insert(123, queue);
+ uint8_t expected[] = {0, 1, 54, 123, 255};
+ EXPECT_TRUE(Contains(queue, expected, sizeof(expected)));
+}
+TEST_F(BinHeapTest, Removals){
+ Insert(54, queue);
+ Insert(1, queue);
+ Insert(0, queue);
+ Insert(255, queue);
+ Insert(123, queue);
+ Remove(255, queue);
+ Remove(0, queue);
+ Insert(222, queue);
+ Insert(67, queue);
+ uint8_t expected[] = {1, 54, 67, 123, 222};
+ EXPECT_TRUE(Contains(queue, expected, sizeof(expected)));
+}
+
diff --git a/aos/atom_code/ipc_lib/cmpxchg.h b/aos/atom_code/ipc_lib/cmpxchg.h
new file mode 100644
index 0000000..acb4a3c
--- /dev/null
+++ b/aos/atom_code/ipc_lib/cmpxchg.h
@@ -0,0 +1,153 @@
+#ifndef __ASM_CMPXCHG_H
+#define __ASM_CMPXCHG_H
+
+#include <stdint.h>
+
+//TODO implement xchg using gcc's atomic builtins (http://gcc.gnu.org/onlinedocs/gcc-4.1.1/gcc/Atomic-Builtins.html)
+//or maybe http://gcc.gnu.org/onlinedocs/gcc/_005f_005fatomic-Builtins.html
+//__atomic_fetch_sub looks promising
+
+#define cmpxchg(ptr, o, n) __sync_val_compare_and_swap(ptr, o, n)
+/*#define xchg(ptr, n) ({typeof(*ptr) r; \
+ do{ \
+ r = *ptr; \
+ }while(!__sync_bool_compare_and_swap(ptr, r, n)); \
+ r; \
+})*/
+
+# define LOCK "lock;"
+# define LOCK_PREFIX "lock;"
+
+#define xchg(ptr,v) ((__typeof__(*(ptr)))__xchg((unsigned long)(v),(ptr),sizeof(*(ptr))))
+
+#define __xg(x) ((volatile long long *)(x))
+
+/*static inline void set_64bit(volatile unsigned long *ptr, unsigned long val)
+{
+ *ptr = val;
+}
+
+#define _set_64bit set_64bit*/
+
+/*
+ * Note: no "lock" prefix even on SMP: xchg always implies lock anyway
+ * Note 2: xchg has side effect, so that attribute volatile is necessary,
+ * but generally the primitive is invalid, *ptr is output argument. --ANK
+ */
+static inline unsigned long __xchg(unsigned long x, volatile void * ptr, int size)
+{
+ switch (size) {
+ case 1:
+ __asm__ __volatile__("xchgb %b0,%1"
+ :"=q" (x)
+ :"m" (*__xg(ptr)), "0" (x)
+ :"memory");
+ break;
+ case 2:
+ __asm__ __volatile__("xchgw %w0,%1"
+ :"=r" (x)
+ :"m" (*__xg(ptr)), "0" (x)
+ :"memory");
+ break;
+ case 4:
+ __asm__ __volatile__("xchgl %k0,%1"
+ :"=r" (x)
+ :"m" (*__xg(ptr)), "0" (x)
+ :"memory");
+ break;
+ case 8:
+ __asm__ __volatile__("xchg %0,%1"
+ :"=r" (x)
+ :"m" (*__xg(ptr)), "0" (x)
+ :"memory");
+ break;
+ }
+ return x;
+}
+
+/*
+ * Atomic compare and exchange. Compare OLD with MEM, if identical,
+ * store NEW in MEM. Return the initial value in MEM. Success is
+ * indicated by comparing RETURN with OLD.
+ */
+
+#if 0
+
+#define __HAVE_ARCH_CMPXCHG 1
+
+static inline unsigned long __cmpxchg(volatile void *ptr, unsigned long old,
+ unsigned long new, int size)
+{
+ int32_t prev;
+ switch (size) {
+ case 1:
+ __asm__ __volatile__(LOCK_PREFIX "cmpxchgb %b1,%2"
+ : "=a"(prev)
+ : "q"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ case 2:
+ __asm__ __volatile__(LOCK_PREFIX "cmpxchgw %w1,%2"
+ : "=a"(prev)
+ : "r"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ case 4:
+ __asm__ __volatile__(LOCK_PREFIX "cmpxchgl %k1,%2"
+ : "=a"(prev)
+ : "r"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ case 8:
+ __asm__ __volatile__("lock; cmpxchg %1,%2"
+ : "=a"(prev)
+ : "q"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ }
+ return old;
+}
+
+/*
+static inline unsigned long __cmpxchg_local(volatile void *ptr,
+ unsigned long old, unsigned long new, int size)
+{
+ unsigned long prev;
+ switch (size) {
+ case 1:
+ __asm__ __volatile__("cmpxchgb %b1,%2"
+ : "=a"(prev)
+ : "q"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ case 2:
+ __asm__ __volatile__("cmpxchgw %w1,%2"
+ : "=a"(prev)
+ : "r"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ case 4:
+ __asm__ __volatile__("cmpxchgl %k1,%2"
+ : "=a"(prev)
+ : "r"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ case 8:
+ __asm__ __volatile__("cmpxchgq %1,%2"
+ : "=a"(prev)
+ : "r"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ }
+ return old;
+}*/
+
+#define cmpxchg(ptr,o,n)\
+ ((__typeof__(*(ptr)))__cmpxchg((ptr),(unsigned long)(o),\
+ (unsigned long)(n),sizeof(*(ptr))))
+/*#define cmpxchg_local(ptr,o,n)\
+ ((__typeof__(*(ptr)))__cmpxchg((ptr),(unsigned long)(o),\
+ (unsigned long)(n),sizeof(*(ptr))))*/
+#endif
+
+#endif
diff --git a/aos/atom_code/ipc_lib/core_lib.c b/aos/atom_code/ipc_lib/core_lib.c
new file mode 100644
index 0000000..d4988cc
--- /dev/null
+++ b/aos/atom_code/ipc_lib/core_lib.c
@@ -0,0 +1,53 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include "shared_mem.h"
+#include "core_lib.h"
+#include <time.h>
+
+void init_shared_mem_core(aos_shm_core *shm_core) {
+ clock_gettime(CLOCK_REALTIME, &shm_core->identifier);
+ shm_core->queues.alloc_flag = 0;
+ shm_core->msg_alloc_lock = 0;
+ shm_core->queues.queue_list = NULL;
+ shm_core->queues.alloc_lock = 0;
+ aos_resource_entity_root_create();
+ for(int i = 0; i < AOS_RESOURCE_NUM; ++i){
+ aos_resource_init(i);
+ }
+}
+static inline uint8_t aos_8max(uint8_t l, uint8_t r) {
+ return (l > r) ? l : r;
+}
+void *shm_malloc_aligned(size_t length, uint8_t alignment) {
+ // minimum alignments from <http://software.intel.com/en-us/articles/data-alignment-when-migrating-to-64-bit-intel-architecture/>
+ if (length <= 1) {
+ alignment = aos_8max(alignment, 1);
+ } else if (length <= 2) {
+ alignment = aos_8max(alignment, 2);
+ } else if (length <= 4) {
+ alignment = aos_8max(alignment, 4);
+ } else if (length <= 8) {
+ alignment = aos_8max(alignment, 8);
+ } else if (length <= 16) {
+ alignment = aos_8max(alignment, 16);
+ } else {
+ alignment = aos_8max(alignment, (length >= 64) ? 64 : 16);
+ }
+
+ void *msg = NULL;
+ aos_shm_core *shm_core = global_core->mem_struct;
+ mutex_grab(&shm_core->msg_alloc_lock);
+ shm_core->msg_alloc = (uint8_t *)shm_core->msg_alloc - length;
+ const uint8_t align_extra = (uintptr_t)shm_core->msg_alloc % alignment;
+ shm_core->msg_alloc = (uint8_t *)shm_core->msg_alloc - align_extra;
+ msg = shm_core->msg_alloc;
+ if (msg <= global_core->shared_mem) {
+ fprintf(stderr, "core_lib: RAN OUT OF SHARED MEMORY!!!----------------------------------------------------------\n");
+ printf("if you didn't see the stderr output just then, you should\n");
+ abort();
+ }
+ //printf("alloc %p\n", msg);
+ mutex_unlock(&shm_core->msg_alloc_lock);
+ return msg;
+}
+
diff --git a/aos/atom_code/ipc_lib/core_lib.h b/aos/atom_code/ipc_lib/core_lib.h
new file mode 100644
index 0000000..1983f7a
--- /dev/null
+++ b/aos/atom_code/ipc_lib/core_lib.h
@@ -0,0 +1,55 @@
+#ifndef _AOS_CORE_LIB_H_
+#define _AOS_CORE_LIB_H_
+
+// required by resource.h
+// defined in shared_mem.c
+#ifdef __cplusplus
+extern "C" {
+#endif // __cplusplus
+extern struct aos_core *global_core;
+#ifdef __cplusplus
+}
+#endif // __cplusplus
+
+#include "aos_sync.h"
+#include "queue.h"
+#include <stdint.h>
+#include "resource_core.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif // __cplusplus
+
+struct aos_queue_list_t;
+typedef struct aos_queue_hash_t {
+ int alloc_flag;
+ mutex alloc_lock;
+ struct aos_queue_list_t *queue_list;
+} aos_queue_hash;
+
+typedef struct aos_shm_core_t {
+ // clock_gettime(CLOCK_REALTIME, &identifier) gets called to identify
+ // this shared memory area
+ struct timespec identifier;
+ // gets 0-initialized at the start (as part of shared memory) and
+ // the owner sets as soon as it finishes setting stuff up
+ mutex creation_condition;
+ mutex msg_alloc_lock;
+ void *msg_alloc;
+ aos_queue_hash queues;
+ aos_resource_list resources;
+} aos_shm_core;
+
+void init_shared_mem_core(aos_shm_core *shm_core);
+
+void *shm_malloc_aligned(size_t length, uint8_t alignment);
+static void *shm_malloc(size_t length);
+static inline void *shm_malloc(size_t length) {
+ return shm_malloc_aligned(length, 0);
+}
+
+#ifdef __cplusplus
+}
+#endif // __cplusplus
+
+#endif
diff --git a/aos/atom_code/ipc_lib/ipc_lib.gyp b/aos/atom_code/ipc_lib/ipc_lib.gyp
new file mode 100644
index 0000000..4dd0fa9
--- /dev/null
+++ b/aos/atom_code/ipc_lib/ipc_lib.gyp
@@ -0,0 +1,55 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'ipc_lib',
+ 'type': 'static_library',
+ 'sources': [
+ 'aos_sync.c',
+ 'binheap.c',
+ 'core_lib.c',
+ 'queue.c',
+ 'resource.c',
+ 'shared_mem.c',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:aos/ResourceList.h',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:aos/ResourceList.h',
+ ],
+ },
+ {
+ 'target_name': 'binheap_test',
+ 'type': 'executable',
+ 'sources': [
+ 'binheap_test.cpp',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ {
+ 'target_name': 'resource_test',
+ 'type': 'executable',
+ 'sources': [
+ 'resource_test.cpp',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ {
+ 'target_name': 'ipc_queue_test',
+ 'type': 'executable',
+ 'sources': [
+ 'queue_test.cpp',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ ],
+}
diff --git a/aos/atom_code/ipc_lib/mutex.cpp b/aos/atom_code/ipc_lib/mutex.cpp
new file mode 100644
index 0000000..d1f0ef2
--- /dev/null
+++ b/aos/atom_code/ipc_lib/mutex.cpp
@@ -0,0 +1,37 @@
+#include "aos/common/mutex.h"
+
+#include <inttypes.h>
+#include <errno.h>
+
+#include "aos/aos_core.h"
+#include "aos/common/type_traits.h"
+
+namespace aos {
+
+Mutex::Mutex() : impl_(0) {
+ static_assert(shm_ok<Mutex>::value,
+ "Mutex is not safe for use in shared memory.");
+}
+
+// Lock and Unlock use the return values of mutex_lock/mutex_unlock
+// to determine whether the lock/unlock succeeded.
+
+void Mutex::Lock() {
+ if (mutex_grab(&impl_) != 0) {
+ LOG(FATAL, "mutex_grab(%p(=%"PRIu32")) failed because of %d: %s\n",
+ &impl_, impl_, errno, strerror(errno));
+ }
+}
+
+void Mutex::Unlock() {
+ if (mutex_unlock(&impl_) != 0) {
+ LOG(FATAL, "mutex_unlock(%p(=%"PRIu32")) failed because of %d: %s\n",
+ &impl_, impl_, errno, strerror(errno));
+ }
+}
+
+bool Mutex::TryLock() {
+ return mutex_trylock(&impl_) == 0;
+}
+
+} // namespace aos
diff --git a/aos/atom_code/ipc_lib/queue.c b/aos/atom_code/ipc_lib/queue.c
new file mode 100644
index 0000000..5cfd2ac
--- /dev/null
+++ b/aos/atom_code/ipc_lib/queue.c
@@ -0,0 +1,510 @@
+#include "aos/atom_code/ipc_lib/queue.h"
+#include "aos/atom_code/ipc_lib/queue_internal.h"
+
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <assert.h>
+
+#define READ_DEBUG 0
+#define WRITE_DEBUG 0
+#define REF_DEBUG 0
+
+static inline aos_msg_header *get_header(void *msg) {
+ return (aos_msg_header *)((uint8_t *)msg - sizeof(aos_msg_header));
+}
+static inline aos_queue *aos_core_alloc_queue() {
+ return shm_malloc_aligned(sizeof(aos_queue), sizeof(int));
+}
+static inline void *aos_alloc_msg(aos_msg_pool *pool) {
+ return shm_malloc(pool->msg_length);
+}
+
+// actually free the given message
+static inline int aos_free_msg(aos_msg_pool *pool, void *msg, aos_queue *queue) {
+#if REF_DEBUG
+ if (pool->pool_lock == 0) {
+ //LOG(WARNING, "unprotected\n");
+ }
+#endif
+ aos_msg_header *header = get_header(msg);
+ if (pool->pool[header->index] != header) { // if something's messed up
+ fprintf(stderr, "queue: something is very very wrong with queue %p."
+ " pool->pool(=%p)[header->index(=%d)] != header(=%p)\n",
+ queue, pool->pool, header->index, header);
+ printf("queue: see stderr\n");
+ abort();
+ }
+#if REF_DEBUG
+ printf("ref_free_count: %p\n", msg);
+#endif
+ --pool->used;
+
+ if (queue->recycle != NULL) {
+ void *const new_msg = aos_queue_get_msg(queue->recycle);
+ if (new_msg == NULL) {
+ fprintf(stderr, "queue: couldn't get a message"
+ " for recycle queue %p\n", queue->recycle);
+ } else {
+ // Take a message from recycle_queue and switch its
+ // header with the one being freed, which effectively
+ // switches which queue each message belongs to.
+ aos_msg_header *const new_header = get_header(new_msg);
+ // also switch the messages between the pools
+ pool->pool[header->index] = new_header;
+ if (mutex_lock(&queue->recycle->pool.pool_lock)) {
+ return -1;
+ }
+ queue->recycle->pool.pool[new_header->index] = header;
+ // swap the information in both headers
+ header_swap(header, new_header);
+ // don't unlock the other pool until all of its messages are valid
+ mutex_unlock(&queue->recycle->pool.pool_lock);
+ // use the header for new_msg which is now for this pool
+ header = new_header;
+ if (aos_queue_write_msg_free(queue->recycle,
+ (void *)msg, OVERRIDE) != 0) {
+ printf("queue: warning aos_queue_write_msg("
+ "%p(=queue(=%p)->recycle), %p, OVERRIDE)"
+ " failed\n",
+ queue->recycle, queue, msg);
+ }
+ msg = new_msg;
+ }
+ }
+
+ // where the one we're freeing was
+ int index = header->index;
+ header->index = -1;
+ if (index != pool->used) { // if we're not freeing the one on the end
+ // put the last one where the one we're freeing was
+ header = pool->pool[index] = pool->pool[pool->used];
+ // put the one we're freeing at the end
+ pool->pool[pool->used] = get_header(msg);
+ // update the former last one's index
+ header->index = index;
+ }
+ return 0;
+}
+// TODO(brians) maybe do this with atomic integer instructions so it doesn't have to lock/unlock pool_lock
+static inline int msg_ref_dec(void *msg, aos_msg_pool *pool, aos_queue *queue) {
+ if (msg == NULL) {
+ return 0;
+ }
+
+ int rv = 0;
+ if (mutex_lock(&pool->pool_lock)) {
+ return -1;
+ }
+ aos_msg_header *const header = get_header(msg);
+ header->ref_count --;
+ assert(header->ref_count >= 0);
+#if REF_DEBUG
+ printf("ref_dec_count: %p count=%d\n", msg, header->ref_count);
+#endif
+ if (header->ref_count == 0) {
+ rv = aos_free_msg(pool, msg, queue);
+ }
+ mutex_unlock(&pool->pool_lock);
+ return rv;
+}
+
+static inline int sigcmp(const aos_type_sig *sig1, const aos_type_sig *sig2) {
+ if (sig1->length != sig2->length) {
+ //LOG(DEBUG, "length mismatch 1=%d 2=%d\n", sig1->length, sig2->length);
+ return 0;
+ }
+ if (sig1->queue_length != sig2->queue_length) {
+ //LOG(DEBUG, "queue_length mismatch 1=%d 2=%d\n", sig1->queue_length, sig2->queue_length);
+ return 0;
+ }
+ if (sig1->hash != sig2->hash) {
+ //LOG(DEBUG, "hash mismatch 1=%d 2=%d\n", sig1->hash, sig2->hash);
+ return 0;
+ }
+ //LOG(DEBUG, "signature match\n");
+ return 1;
+}
+static inline aos_queue *aos_create_queue(const aos_type_sig *sig) {
+ aos_queue *const queue = aos_core_alloc_queue();
+ aos_msg_pool *const pool = &queue->pool;
+ pool->mem_length = sig->queue_length + EXTRA_MESSAGES;
+ pool->length = 0;
+ pool->used = 0;
+ pool->msg_length = sig->length + sizeof(aos_msg_header);
+ pool->pool = shm_malloc(sizeof(void *) * pool->mem_length);
+ aos_ring_buf *const buf = &queue->buf;
+ buf->length = sig->queue_length + 1;
+ if (buf->length < 2) { // TODO(brians) when could this happen?
+ buf->length = 2;
+ }
+ buf->data = shm_malloc(buf->length * sizeof(void *));
+ buf->start = 0;
+ buf->end = 0;
+ buf->msgs = 0;
+ buf->writable = 1;
+ buf->readable = 0;
+ buf->buff_lock = 0;
+ pool->pool_lock = 0;
+ queue->recycle = NULL;
+ return queue;
+}
+aos_queue *aos_fetch_queue(const char *name, const aos_type_sig *sig) {
+ //LOG(DEBUG, "Fetching the stupid queue: %s\n", name);
+ mutex_grab(&global_core->mem_struct->queues.alloc_lock);
+ aos_queue_list *list = global_core->mem_struct->queues.queue_list;
+ aos_queue_list *last = NULL;
+ while (list != NULL) {
+ // if we found a matching queue
+ if (strcmp(list->name, name) == 0 && sigcmp(&list->sig, sig)) {
+ mutex_unlock(&global_core->mem_struct->queues.alloc_lock);
+ return list->queue;
+ } else {
+ //LOG(DEBUG, "rejected queue %s strcmp=%d target=%s\n", (*list)->name, strcmp((*list)->name, name), name);
+ }
+ last = list;
+ list = list->next;
+ }
+ list = shm_malloc(sizeof(aos_queue_list));
+ if (last == NULL) {
+ global_core->mem_struct->queues.queue_list = list;
+ } else {
+ last->next = list;
+ }
+ list->sig = *sig;
+ const size_t name_size = strlen(name) + 1;
+ list->name = shm_malloc(name_size);
+ memcpy(list->name, name, name_size);
+ //LOG(INFO, "creating queue{name=%s, sig.length=%zd, sig.hash=%d, sig.queue_length=%d}\n", name, sig->length, sig->hash, sig->queue_length);
+ list->queue = aos_create_queue(sig);
+ //LOG(DEBUG, "Made the stupid queue: %s happy?\n", name);
+ list->next = NULL;
+ mutex_unlock(&global_core->mem_struct->queues.alloc_lock);
+ return list->queue;
+}
+aos_queue *aos_fetch_queue_recycle(const char *name, const aos_type_sig *sig,
+ const aos_type_sig *recycle_sig, aos_queue **recycle) {
+ if (sig->length != recycle_sig->length || sig->hash == recycle_sig->hash) {
+ *recycle = NULL;
+ return NULL;
+ }
+ aos_queue *const r = aos_fetch_queue(name, sig);
+ r->recycle = aos_fetch_queue(name, recycle_sig);
+ if (r == r->recycle) {
+ fprintf(stderr, "queue: r->recycle(=%p) == r(=%p)\n", r->recycle, r);
+ printf("see stderr\n");
+ abort();
+ }
+ *recycle = r->recycle;
+ return r;
+}
+
+int aos_queue_write_msg(aos_queue *queue, void *msg, int opts) {
+#if WRITE_DEBUG
+ printf("queue: write_msg(%p, %p, %d)\n", queue, msg, opts);
+#endif
+ int rv = 0;
+ if (msg == NULL || msg < (void *)global_core->mem_struct ||
+ msg > (void *)((intptr_t)global_core->mem_struct + global_core->size)) {
+ fprintf(stderr, "queue: attempt to write bad message %p to %p. aborting\n",
+ msg, queue);
+ printf("see stderr\n");
+ abort();
+ }
+ aos_ring_buf *const buf = &queue->buf;
+ if (mutex_lock(&buf->buff_lock)) {
+#if WRITE_DEBUG
+ printf("queue: locking buff_lock of %p failed\n", buf);
+#endif
+ return -1;
+ }
+ int new_end = (buf->end + 1) % buf->length;
+ while (new_end == buf->start) {
+ if (opts & NON_BLOCK) {
+#if WRITE_DEBUG
+ printf("queue: not blocking on %p. returning -1\n", queue);
+#endif
+ mutex_unlock(&buf->buff_lock);
+ return -1;
+ } else if (opts & OVERRIDE) {
+#if WRITE_DEBUG
+ printf("queue: overriding on %p\n", queue);
+#endif
+ // avoid leaking the message that we're going to overwrite
+ msg_ref_dec(buf->data[buf->start], &queue->pool, queue);
+ buf->start = (buf->start + 1) % buf->length;
+ } else { // BLOCK
+ mutex_unlock(&buf->buff_lock);
+#if WRITE_DEBUG
+ printf("queue: going to wait for writable(=%p) of %p\n",
+ &buf->writable, queue);
+#endif
+ if (condition_wait(&buf->writable)) {
+#if WRITE_DEBUG
+ printf("queue: waiting for writable(=%p) of %p failed\n",
+ &buf->writable, queue);
+#endif
+ return -1;
+ }
+#if WRITE_DEBUG
+ printf("queue: going to re-lock buff_lock of %p to write\n", queue);
+#endif
+ if (mutex_lock(&buf->buff_lock)) {
+#if WRITE_DEBUG
+ printf("queue: error locking buff_lock of %p\n", queue);
+#endif
+ return -1;
+ }
+ }
+ new_end = (buf->end + 1) % buf->length;
+ }
+ buf->data[buf->end] = msg;
+ ++buf->msgs;
+ buf->end = new_end;
+ mutex_unlock(&buf->buff_lock);
+#if WRITE_DEBUG
+ printf("queue: setting readable(=%p) of %p\n", &buf->readable, queue);
+#endif
+ condition_set(&buf->readable);
+ if (((buf->end + 1) % buf->length) == buf->start) { // if it's now full
+ condition_unset(&buf->writable);
+ }
+#if WRITE_DEBUG
+ printf("queue: write returning %d on queue %p\n", rv, queue);
+#endif
+ return rv;
+}
+
+int aos_queue_free_msg(aos_queue *queue, const void *msg) {
+ // TODO(brians) get rid of this
+ void *msg_temp;
+ memcpy(&msg_temp, &msg, sizeof(msg_temp));
+ return msg_ref_dec(msg_temp, &queue->pool, queue);
+}
+// Deals with setting/unsetting readable and writable.
+// Should be called after buff_lock has been unlocked.
+// read is whether or not this read call read one off the queue
+static inline void aos_read_msg_common_end(aos_ring_buf *const buf, int read) {
+ if (read) {
+ condition_set(&buf->writable);
+ if (buf->start == buf->end) {
+ condition_unset(&buf->readable);
+ }
+ }
+}
+// Returns with buff_lock locked and a readable message in buf.
+// Returns -1 for error (if it returns -1, buff_lock will be unlocked).
+static inline int aos_queue_read_msg_common(int opts, aos_ring_buf *const buf,
+ aos_queue *const queue, int *index) {
+#if !READ_DEBUG
+ (void)queue;
+#endif
+ if (mutex_lock(&buf->buff_lock)) {
+#if READ_DEBUG
+ printf("queue: couldn't lock buff_lock of %p\n", queue);
+#endif
+ return -1;
+ }
+ while (buf->start == buf->end || ((index != NULL) && buf->msgs <= *index)) {
+ mutex_unlock(&buf->buff_lock);
+ if (opts & NON_BLOCK) {
+#if READ_DEBUG
+ printf("queue: not going to block waiting on %p\n", queue);
+#endif
+ return -1;
+ } else { // BLOCK
+#if READ_DEBUG
+ printf("queue: going to wait for readable(=%p) of %p\n",
+ &buf->readable, queue);
+#endif
+ // wait for a message to become readable
+ if ((index == NULL) ? condition_wait(&buf->readable) :
+ condition_wait_force(&buf->readable)) {
+#if READ_DEBUG
+ printf("queue: waiting for readable(=%p) of %p failed\n",
+ &buf->readable, queue);
+#endif
+ return -1;
+ }
+ }
+#if READ_DEBUG
+ printf("queue: going to re-lock buff_lock of %p to read\n", queue);
+#endif
+ if (mutex_lock(&buf->buff_lock)) {
+#if READ_DEBUG
+ printf("couldn't re-lock buff_lock of %p\n", queue);
+#endif
+ return -1;
+ }
+ }
+#if READ_DEBUG
+ printf("queue: read start=%d end=%d from %p\n", buf->start, buf->end, queue);
+#endif
+ return 0;
+}
+// handles reading with PEEK
+static inline void *read_msg_peek(aos_ring_buf *const buf, int opts, int start) {
+ void *ret;
+ if (opts & FROM_END) {
+ int pos = buf->end - 1;
+ if (pos < 0) { // if it needs to wrap
+ pos = buf->length - 1;
+ }
+#if READ_DEBUG
+ printf("queue: reading from line %d: %d\n", __LINE__, pos);
+#endif
+ ret = buf->data[pos];
+ } else {
+#if READ_DEBUG
+ printf("queue: reading from line %d: %d\n", __LINE__, start);
+#endif
+ ret = buf->data[start];
+ }
+ aos_msg_header *const header = get_header(ret);
+ header->ref_count ++;
+#if REF_DEBUG
+ printf("ref inc count: %p\n", ret);
+#endif
+ return ret;
+}
+const void *aos_queue_read_msg(aos_queue *queue, int opts) {
+#if READ_DEBUG
+ printf("queue: read_msg(%p, %d)\n", queue, opts);
+#endif
+ void *msg = NULL;
+ aos_ring_buf *const buf = &queue->buf;
+ if (aos_queue_read_msg_common(opts, buf, queue, NULL) == -1) {
+#if READ_DEBUG
+ printf("queue: common returned -1 for %p\n", queue);
+#endif
+ return NULL;
+ }
+ if (opts & PEEK) {
+ msg = read_msg_peek(buf, opts, buf->start);
+ } else {
+ if (opts & FROM_END) {
+ while (1) {
+#if READ_DEBUG
+ printf("queue: start of c2 of %p\n", queue);
+#endif
+ // This loop pulls each message out of the buffer.
+ const int pos = buf->start;
+ buf->start = (buf->start + 1) % buf->length;
+ // if this is the last one
+ if (buf->start == buf->end) {
+#if READ_DEBUG
+ printf("queue: reading from c2: %d\n", pos);
+#endif
+ msg = buf->data[pos];
+ break;
+ }
+ // it's not going to be in the queue any more
+ msg_ref_dec(buf->data[pos], &queue->pool, queue);
+ }
+ } else {
+#if READ_DEBUG
+ printf("queue: reading from d2: %d\n", buf->start);
+#endif
+ msg = buf->data[buf->start];
+ buf->start = (buf->start + 1) % buf->length;
+ }
+ }
+ mutex_unlock(&buf->buff_lock);
+ aos_read_msg_common_end(buf, !(opts & PEEK));
+#if READ_DEBUG
+ printf("queue: read returning %p\n", msg);
+#endif
+ return msg;
+}
+const void *aos_queue_read_msg_index(aos_queue *queue, int opts, int *index) {
+#if READ_DEBUG
+ printf("queue: read_msg_index(%p, %d, %p(*=%d))\n", queue, opts, index, *index);
+#endif
+ void *msg = NULL;
+ aos_ring_buf *const buf = &queue->buf;
+ if (aos_queue_read_msg_common(opts, buf, queue, index) == -1) {
+#if READ_DEBUG
+ printf("queue: common returned -1\n");
+#endif
+ return NULL;
+ }
+ // TODO(parker): Handle integer wrap on the index.
+ const int offset = buf->msgs - *index;
+ int my_start = buf->end - offset;
+ if (offset >= buf->length) { // if we're behind the available messages
+ // catch index up to the last available message
+ *index += buf->start - my_start;
+ // and that's the one we're going to read
+ my_start = buf->start;
+ }
+ if (my_start < 0) { // if we want to read off the end of the buffer
+ // unwrap where we're going to read from
+ my_start += buf->length;
+ }
+ if (opts & PEEK) {
+ msg = read_msg_peek(buf, opts, my_start);
+ } else {
+ if (opts & FROM_END) {
+#if READ_DEBUG
+ printf("queue: start of c1 of %p\n", queue);
+#endif
+ int pos = buf->end - 1;
+ if (pos < 0) { // if it wrapped
+ pos = buf->length - 1; // unwrap it
+ }
+#if READ_DEBUG
+ printf("queue: reading from c1: %d\n", pos);
+#endif
+ msg = buf->data[pos];
+ *index = buf->msgs;
+ } else {
+#if READ_DEBUG
+ printf("queue: reading from d1: %d\n", my_start);
+#endif
+ msg = buf->data[my_start];
+ ++(*index);
+ }
+ aos_msg_header *const header = get_header(msg);
+ ++header->ref_count;
+#if REF_DEBUG
+ printf("ref_inc_count: %p\n", msg);
+#endif
+ }
+ mutex_unlock(&buf->buff_lock);
+ // this function never consumes one off the queue
+ aos_read_msg_common_end(buf, 0);
+ return msg;
+}
+static inline void *aos_pool_get_msg(aos_msg_pool *pool) {
+ if (mutex_lock(&pool->pool_lock)) {
+ return NULL;
+ }
+ void *msg;
+ if (pool->length - pool->used > 0) {
+ msg = pool->pool[pool->used];
+ } else {
+ if (pool->length >= pool->mem_length) {
+ //TODO(brians) log this if it isn't the log queue
+ fprintf(stderr, "queue: overused_pool\n");
+ msg = NULL;
+ goto exit;
+ }
+ msg = pool->pool[pool->length] = aos_alloc_msg(pool);
+ ++pool->length;
+ }
+ aos_msg_header *const header = msg;
+ msg = (uint8_t *)msg + sizeof(aos_msg_header);
+ header->ref_count = 1;
+#if REF_DEBUG
+ printf("ref alloc: %p\n", msg);
+#endif
+ header->index = pool->used;
+ ++pool->used;
+exit:
+ mutex_unlock(&pool->pool_lock);
+ return msg;
+}
+void *aos_queue_get_msg(aos_queue *queue) {
+ return aos_pool_get_msg(&queue->pool);
+}
+
diff --git a/aos/atom_code/ipc_lib/queue.h b/aos/atom_code/ipc_lib/queue.h
new file mode 100644
index 0000000..4c279e1
--- /dev/null
+++ b/aos/atom_code/ipc_lib/queue.h
@@ -0,0 +1,134 @@
+#ifndef AOS_IPC_LIB_QUEUE_H_
+#define AOS_IPC_LIB_QUEUE_H_
+
+#include "shared_mem.h"
+#include "aos_sync.h"
+
+// TODO(brians) add valgrind client requests to the queue and shared_mem_malloc
+// code to make checking for leaks work better
+// <http://www.valgrind.org/docs/manual/mc-manual.html#mc-manual.mempools>
+// describes how
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Queues are the primary way to use shared memory. Basic use consists of
+// initializing an aos_type_sig and then calling aos_fetch_queue on it.
+// This aos_queue* can then be used to get a message and write it or to read a
+// message.
+// Queues (as the name suggests) are a FIFO stack of messages. Each combination
+// of name and aos_type_sig will result in a different queue, which means that
+// if you only recompile some code that uses differently sized messages, it will
+// simply use a different queue than the old code.
+//
+// Any pointers returned from these functions can be safely passed to other
+// processes because they are all shared memory pointers.
+// IMPORTANT: Any message pointer must be passed back in some way
+// (aos_queue_free_msg and aos_queue_write_msg are common ones) or the
+// application will leak shared memory.
+// NOTE: Taking a message from read_msg and then passing it to write_msg might
+// work, but it is not guaranteed to.
+
+typedef struct aos_type_sig_t {
+ size_t length; // sizeof(message)
+ int hash; // can differentiate multiple otherwise identical queues
+ int queue_length; // how many messages the queue can hold
+} aos_type_sig;
+
+// Structures that are opaque to users (defined in queue_internal.h).
+typedef struct aos_queue_list_t aos_queue_list;
+typedef struct aos_queue_t aos_queue;
+
+// Retrieves (and creates if necessary) a queue. Each combination of name and
+// signature refers to a completely independent queue.
+aos_queue *aos_fetch_queue(const char *name, const aos_type_sig *sig);
+// Same as above, except sets up the returned queue so that it will put messages
+// on *recycle (retrieved with recycle_sig) when they are freed (after they have
+// been released by all other readers/writers and are not in the queue).
+// The length of recycle_sig determines how many freed messages will be kept.
+// Other code can retrieve recycle_sig and sig separately. However, any frees
+// made using aos_fetch_queue with only sig before the recycle queue has been
+// associated with it will not go on to the recyce queue.
+// Will return NULL for both queues if sig->length != recycle_sig->length or
+// sig->hash == recycle_sig->hash (just to be safe).
+// NOTE: calling this function with the same sig but multiple recycle_sig s
+// will result in each freed message being put onto an undefined recycle_sig.
+aos_queue *aos_fetch_queue_recycle(const char *name, const aos_type_sig *sig,
+ const aos_type_sig *recycle_sig, aos_queue **recycle);
+
+// Constants for passing to opts arguments.
+// #defines so that c code can use queues
+// The non-conflicting ones can be combined with bitwise-or.
+// TODO(brians) prefix these?
+//
+// Causes the returned message to be left in the queue.
+// For reading only.
+#define PEEK 0x0001
+// Reads the last message in the queue instead of just the next one.
+// NOTE: This removes all of the messages until the last one from the queue
+// (which means that nobody else will read them). However, PEEK means to not
+// remove any from the queue, including the ones that are skipped.
+// For reading only.
+#define FROM_END 0x0002
+// Causes reads to return NULL and writes to fail instead of waiting.
+// For reading and writing.
+#define NON_BLOCK 0x0004
+// Causes things to block.
+// IMPORTANT: #defined to 0 so that it is the default. This has to stay.
+// For reading and writing.
+#define BLOCK 0x0000
+// Causes writes to overwrite the oldest message in the queue instead of
+// blocking.
+// For writing only.
+#define OVERRIDE 0x0008
+
+// Frees a message. Does nothing if msg is NULL.
+int aos_queue_free_msg(aos_queue *queue, const void *msg);
+
+// Writes a message into the queue.
+// NOTE: msg must point to at least the length of this queue's worth of valid
+// data to write
+// IMPORTANT: if this returns -1, then the caller must do something with msg
+// (like free it)
+int aos_queue_write_msg(aos_queue *queue, void *msg, int opts);
+// Exactly the same as aos_queue_write_msg, except it automatically frees the
+// message if writing fails.
+static inline int aos_queue_write_msg_free(aos_queue *queue, void *msg, int opts) {
+ const int ret = aos_queue_write_msg(queue, msg, opts);
+ if (ret != 0) {
+ aos_queue_free_msg(queue, msg);
+ }
+ return ret;
+}
+
+// Reads a message out of the queue.
+// The return value will have at least the length of this queue's worth of valid
+// data where it's pointing to.
+// The return value is const because other people might be viewing the same
+// messsage. Do not cast the const away!
+// IMPORTANT: The return value (if not NULL) must eventually be passed to
+// aos_queue_free_msg.
+const void *aos_queue_read_msg(aos_queue *buf, int opts);
+// Exactly the same as aos_queue_read_msg, except it will never return the same
+// message twice with the same index argument. However, it may not return some
+// messages that pass through the queue.
+// *index should start as 0. index does not have to be in shared memory, but it
+// can be
+const void *aos_queue_read_msg_index(aos_queue *queue, int opts, int *index);
+
+// Retrieves ("allocates") a message that can then be written to the queue.
+// NOTE: the return value will be completely uninitialized
+// The return value will have at least the length of this queue's worth of valid
+// data where it's pointing to.
+// Returns NULL for error.
+// IMPORTANT: The return value (if not NULL) must eventually be passed to
+// aos_queue_free_msg.
+void *aos_queue_get_msg(aos_queue *queue);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
diff --git a/aos/atom_code/ipc_lib/queue_internal.h b/aos/atom_code/ipc_lib/queue_internal.h
new file mode 100644
index 0000000..e6b23ef
--- /dev/null
+++ b/aos/atom_code/ipc_lib/queue_internal.h
@@ -0,0 +1,62 @@
+#ifndef AOS_IPC_LIB_QUEUE_INTERNAL_H_
+#define AOS_IPC_LIB_QUEUE_INTERNAL_H_
+
+#include "shared_mem.h"
+#include "aos_sync.h"
+
+// Should only be used by queue.c. Contains definitions of the structures
+// it uses.
+
+// The number of extra messages the pool associated with each queue will be able
+// to hold (for readers who are slow about freeing them).
+#define EXTRA_MESSAGES 20
+
+typedef struct aos_msg_header_t {
+ int ref_count;
+ int index; // in the pool
+} aos_msg_header;
+static inline void header_swap(aos_msg_header *l, aos_msg_header *r) {
+ aos_msg_header tmp;
+ tmp.ref_count = l->ref_count;
+ tmp.index = l->index;
+ l->ref_count = r->ref_count;
+ l->index = r->index;
+ r->ref_count = tmp.ref_count;
+ r->index = tmp.index;
+}
+
+struct aos_queue_list_t {
+ char *name;
+ aos_type_sig sig;
+ aos_queue *queue;
+ aos_queue_list *next;
+};
+
+typedef struct aos_ring_buf_t {
+ mutex buff_lock; // the main lock protecting operations on this buffer
+ // conditions
+ mutex writable;
+ mutex readable;
+ int length; // max index into data + 1
+ int start; // is an index into data
+ int end; // is an index into data
+ int msgs; // that have passed through
+ void **data; // array of messages (w/ headers)
+} aos_ring_buf;
+
+typedef struct aos_msg_pool_t {
+ mutex pool_lock;
+ size_t msg_length;
+ int mem_length; // the number of messages
+ int used; // number of messages
+ int length; // number of allocated messages
+ void **pool; // array of messages
+} aos_msg_pool;
+
+struct aos_queue_t {
+ aos_msg_pool pool;
+ aos_ring_buf buf;
+ aos_queue *recycle;
+};
+
+#endif
diff --git a/aos/atom_code/ipc_lib/queue_test.cpp b/aos/atom_code/ipc_lib/queue_test.cpp
new file mode 100644
index 0000000..d4eb700
--- /dev/null
+++ b/aos/atom_code/ipc_lib/queue_test.cpp
@@ -0,0 +1,404 @@
+#include <unistd.h>
+#include <sys/mman.h>
+#include <inttypes.h>
+
+#include <ostream>
+#include <memory>
+#include <map>
+
+#include "gtest/gtest.h"
+
+#include "aos/aos_core.h"
+#include "aos/atom_code/ipc_lib/sharedmem_test_setup.h"
+#include "aos/common/type_traits.h"
+
+using testing::AssertionResult;
+using testing::AssertionSuccess;
+using testing::AssertionFailure;
+
+// IMPORTANT: Some of the functions that do test predicate functions allocate
+// shared memory (and don't free it).
+class QueueTest : public SharedMemTestSetup {
+ protected:
+ static const size_t kFailureSize = 400;
+ static char *fatal_failure;
+ private:
+ // This gets registered right after the fork, so it will get run before any
+ // exit handlers that had already been registered.
+ static void ExitExitHandler() {
+ _exit(EXIT_SUCCESS);
+ }
+ enum class ResultType : uint8_t {
+ NotCalled,
+ Called,
+ Returned,
+ };
+ const std::string ResultTypeString(volatile const ResultType &result) {
+ switch (result) {
+ case ResultType::Returned:
+ return "Returned";
+ case ResultType::Called:
+ return "Called";
+ case ResultType::NotCalled:
+ return "NotCalled";
+ default:
+ return std::string("unknown(" + static_cast<uint8_t>(result)) + ")";
+ }
+ }
+ static_assert(aos::shm_ok<ResultType>::value, "this will get put in shared memory");
+ // Gets allocated in shared memory so it has to be volatile.
+ template<typename T> struct FunctionToCall {
+ ResultType result;
+ bool expected;
+ void (*function)(T*, char*);
+ T *arg;
+ volatile char failure[kFailureSize];
+ };
+ template<typename T> static void Hangs_(volatile FunctionToCall<T> *const to_call) {
+ to_call->result = ResultType::Called;
+ to_call->function(to_call->arg, const_cast<char *>(to_call->failure));
+ to_call->result = ResultType::Returned;
+ }
+
+ static const long kMsToNs = 1000000;
+ // The number of ms after which a function is considered to have hung.
+ // Must be < 1000.
+ static const long kHangTime = 10;
+ static const unsigned int kForkSleep = 0; // how many seconds to sleep after forking
+
+ // Represents a process that has been forked off. The destructor kills the
+ // process and wait(2)s for it.
+ class ForkedProcess {
+ const pid_t pid_;
+ mutex *const lock_;
+ public:
+ ForkedProcess(pid_t pid, mutex *lock) : pid_(pid), lock_(lock) {};
+ ~ForkedProcess() {
+ if (kill(pid_, SIGINT) == -1) {
+ if (errno == ESRCH) {
+ printf("process %jd was already dead\n", static_cast<intmax_t>(pid_));
+ } else {
+ fprintf(stderr, "kill(SIGKILL, %jd) failed with %d: %s\n",
+ static_cast<intmax_t>(pid_), errno, strerror(errno));
+ }
+ return;
+ }
+ const pid_t ret = wait(NULL);
+ if (ret == -1) {
+ fprintf(stderr, "wait(NULL) failed."
+ " child %jd might still be alive\n",
+ static_cast<intmax_t>(pid_));
+ } else if (ret == 0) {
+ fprintf(stderr, "child %jd wasn't waitable. it might still be alive\n",
+ static_cast<intmax_t>(pid_));
+ } else if (ret != pid_) {
+ fprintf(stderr, "child %d is dead, but child %jd might still be alive\n",
+ ret, static_cast<intmax_t>(pid_));
+ }
+ }
+
+ enum class JoinResult {
+ Finished, Hung, Error
+ };
+ JoinResult Join(long timeout = kHangTime) {
+ timespec ts{kForkSleep, timeout * kMsToNs};
+ switch (mutex_lock_timeout(lock_, &ts)) {
+ case 2:
+ return JoinResult::Hung;
+ case 0:
+ return JoinResult::Finished;
+ default:
+ return JoinResult::Error;
+ }
+ }
+ } __attribute__((unused));
+
+ // Member variables for HangsFork and HangsCheck.
+ typedef uint8_t ChildID;
+ static void ReapExitHandler() {
+ for (auto it = children_.begin(); it != children_.end(); ++it) {
+ delete it->second;
+ }
+ }
+ static std::map<ChildID, ForkedProcess *> children_;
+ std::map<ChildID, volatile FunctionToCall<void> *> to_calls_;
+
+ void SetUp() {
+ SharedMemTestSetup::SetUp();
+ fatal_failure = reinterpret_cast<char *>(shm_malloc(sizeof(fatal_failure)));
+ static bool registered = false;
+ if (!registered) {
+ atexit(ReapExitHandler);
+ registered = true;
+ }
+ }
+
+ protected:
+ // Function gets called with arg in a forked process.
+ // Leaks shared memory.
+ // the attribute is in the middle to make gcc happy
+ template<typename T> __attribute__((warn_unused_result))
+ std::unique_ptr<ForkedProcess> ForkExecute(void (*function)(T*), T *arg) {
+ mutex *lock = reinterpret_cast<mutex *>(shm_malloc_aligned(
+ sizeof(*lock), sizeof(int)));
+ *lock = 1;
+ const pid_t pid = fork();
+ switch (pid) {
+ case 0: // child
+ if (kForkSleep != 0) {
+ printf("pid %jd sleeping for %u\n", static_cast<intmax_t>(getpid()),
+ kForkSleep);
+ sleep(kForkSleep);
+ }
+ atexit(ExitExitHandler);
+ function(arg);
+ mutex_unlock(lock);
+ exit(EXIT_SUCCESS);
+ case -1: // parent failure
+ printf("fork() failed with %d: %s\n", errno, strerror(errno));
+ return std::unique_ptr<ForkedProcess>();
+ default: // parent
+ return std::unique_ptr<ForkedProcess>(new ForkedProcess(pid, lock));
+ }
+ }
+
+ // Checks whether or not the given function hangs.
+ // expected is whether to return success or failure if the function hangs
+ // NOTE: There are other reasons for it to return a failure than the function
+ // doing the wrong thing.
+ // Leaks shared memory.
+ template<typename T> AssertionResult Hangs(void (*function)(T*, char*), T *arg,
+ bool expected) {
+ AssertionResult fork_result(HangsFork<T>(function, arg, expected, 0));
+ if (!fork_result) {
+ return fork_result;
+ }
+ return HangsCheck(0);
+ }
+ // Starts the first part of Hangs.
+ // Use HangsCheck to get the result.
+ // Returns whether the fork succeeded or not, NOT whether or not the hang
+ // check succeeded.
+ template<typename T> AssertionResult HangsFork(void (*function)(T*, char *), T *arg,
+ bool expected, ChildID id) {
+ static_assert(aos::shm_ok<FunctionToCall<T>>::value,
+ "this is going into shared memory");
+ volatile FunctionToCall<T> *const to_call = reinterpret_cast<FunctionToCall<T> *>(
+ shm_malloc_aligned(sizeof(*to_call), sizeof(int)));
+ to_call->result = ResultType::NotCalled;
+ to_call->function = function;
+ to_call->arg = arg;
+ to_call->expected = expected;
+ to_call->failure[0] = '\0';
+ static_cast<volatile char *>(fatal_failure)[0] = '\0';
+ children_[id] = ForkExecute(Hangs_, to_call).release();
+ if (!children_[id]) return AssertionFailure() << "ForkExecute failed";
+ to_calls_[id] = reinterpret_cast<volatile FunctionToCall<void> *>(to_call);
+ return AssertionSuccess();
+ }
+ // Checks whether or not a function hung like it was supposed to.
+ // Use HangsFork first.
+ // NOTE: calls to HangsFork and HangsCheck with the same id argument will
+ // correspond, but they do not nest. Also, id 0 is used by Hangs.
+ // Return value is the same as Hangs.
+ AssertionResult HangsCheck(ChildID id) {
+ std::unique_ptr<ForkedProcess> child(children_[id]);
+ children_.erase(id);
+ const ForkedProcess::JoinResult result = child->Join();
+ if (to_calls_[id]->failure[0] != '\0') {
+ return AssertionFailure() << "function says: "
+ << const_cast<char *>(to_calls_[id]->failure);
+ }
+ if (result == ForkedProcess::JoinResult::Finished) {
+ return !to_calls_[id]->expected ? AssertionSuccess() : (AssertionFailure()
+ << "something happened and the the test only got to "
+ << ResultTypeString(to_calls_[id]->result));
+ } else {
+ if (to_calls_[id]->result == ResultType::Called) {
+ return to_calls_[id]->expected ? AssertionSuccess() : AssertionFailure();
+ } else {
+ return AssertionFailure() << "something weird happened";
+ }
+ }
+ }
+#define EXPECT_HANGS(function, arg) \
+ EXPECT_HANGS_COND(function, arg, true, EXPECT_TRUE)
+#define EXPECT_RETURNS(function, arg) \
+ EXPECT_HANGS_COND(function, arg, false, EXPECT_TRUE)
+#define EXPECT_RETURNS_FAILS(function, arg) \
+ EXPECT_HANGS_COND(function, arg, false, EXPECT_FALSE)
+#define EXPECT_HANGS_COND(function, arg, hangs, cond) do { \
+ cond(Hangs(function, arg, hangs)); \
+ if (fatal_failure[0] != '\0') { \
+ FAIL() << fatal_failure; \
+ } \
+} while (false)
+
+ struct TestMessage {
+ int16_t data; // don't really want to test empty messages
+ };
+ struct MessageArgs {
+ aos_queue *const queue;
+ int flags;
+ int16_t data; // -1 means NULL expected
+ };
+ static void WriteTestMessage(MessageArgs *args, char *failure) {
+ TestMessage *msg = reinterpret_cast<TestMessage *>(aos_queue_get_msg(args->queue));
+ if (msg == NULL) {
+ snprintf(fatal_failure, kFailureSize, "couldn't get_msg from %p", args->queue);
+ return;
+ }
+ msg->data = args->data;
+ if (aos_queue_write_msg_free(args->queue, msg, args->flags) == -1) {
+ snprintf(failure, kFailureSize, "write_msg_free(%p, %p, %d) failed",
+ args->queue, msg, args->flags);
+ }
+ }
+ static void ReadTestMessage(MessageArgs *args, char *failure) {
+ const TestMessage *msg = reinterpret_cast<const TestMessage *>(
+ aos_queue_read_msg(args->queue, args->flags));
+ if (msg == NULL) {
+ if (args->data != -1) {
+ snprintf(failure, kFailureSize, "expected data of %"PRId16" but got NULL message",
+ args->data);
+ }
+ } else {
+ if (args->data != msg->data) {
+ snprintf(failure, kFailureSize,
+ "expected data of %"PRId16" but got %"PRId16" instead",
+ args->data, msg->data);
+ }
+ aos_queue_free_msg(args->queue, msg);
+ }
+ }
+};
+char *QueueTest::fatal_failure;
+std::map<QueueTest::ChildID, QueueTest::ForkedProcess *> QueueTest::children_;
+
+TEST_F(QueueTest, Reading) {
+ static const aos_type_sig signature{sizeof(TestMessage), 1, 1};
+ aos_queue *const queue = aos_fetch_queue("Queue", &signature);
+ MessageArgs args{queue, 0, -1};
+
+ EXPECT_EQ(BLOCK, 0);
+ EXPECT_EQ(BLOCK | FROM_END, FROM_END);
+
+ args.flags = NON_BLOCK;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = NON_BLOCK | PEEK;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = 0;
+ EXPECT_HANGS(ReadTestMessage, &args);
+ args.flags = PEEK;
+ EXPECT_HANGS(ReadTestMessage, &args);
+ args.data = 254;
+ args.flags = BLOCK;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = PEEK;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = PEEK;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = PEEK | NON_BLOCK;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = 0;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = 0;
+ args.data = -1;
+ EXPECT_HANGS(ReadTestMessage, &args);
+ args.flags = NON_BLOCK;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = 0;
+ args.data = 971;
+ EXPECT_RETURNS_FAILS(ReadTestMessage, &args);
+}
+TEST_F(QueueTest, Writing) {
+ static const aos_type_sig signature{sizeof(TestMessage), 1, 1};
+ aos_queue *const queue = aos_fetch_queue("Queue", &signature);
+ MessageArgs args{queue, 0, 973};
+
+ args.flags = BLOCK;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = BLOCK;
+ EXPECT_HANGS(WriteTestMessage, &args);
+ args.flags = NON_BLOCK;
+ EXPECT_RETURNS_FAILS(WriteTestMessage, &args);
+ args.flags = NON_BLOCK;
+ EXPECT_RETURNS_FAILS(WriteTestMessage, &args);
+ args.flags = PEEK;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.data = 971;
+ args.flags = OVERRIDE;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = OVERRIDE;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = 0;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = NON_BLOCK;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = 0;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ args.flags = OVERRIDE;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = 0;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+}
+
+TEST_F(QueueTest, MultiRead) {
+ static const aos_type_sig signature{sizeof(TestMessage), 1, 1};
+ aos_queue *const queue = aos_fetch_queue("Queue", &signature);
+ MessageArgs args{queue, 0, 1323};
+
+ args.flags = BLOCK;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ args.flags = BLOCK;
+ ASSERT_TRUE(HangsFork(ReadTestMessage, &args, true, 1));
+ ASSERT_TRUE(HangsFork(ReadTestMessage, &args, true, 2));
+ EXPECT_TRUE(HangsCheck(1) != HangsCheck(2));
+ // TODO(brians) finish this
+}
+
+TEST_F(QueueTest, Recycle) {
+ // TODO(brians) basic test of recycle queue
+ // include all of the ways a message can get into the recycle queue
+ static const aos_type_sig signature{sizeof(TestMessage), 1, 2},
+ recycle_signature{sizeof(TestMessage), 2, 2};
+ aos_queue *recycle_queue = reinterpret_cast<aos_queue *>(23);
+ aos_queue *const queue = aos_fetch_queue_recycle("Queue", &signature,
+ &recycle_signature, &recycle_queue);
+ ASSERT_NE(reinterpret_cast<aos_queue *>(23), recycle_queue);
+ MessageArgs args{queue, 0, 973}, recycle{recycle_queue, 0, 973};
+
+ args.flags = BLOCK;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+ args.data = 254;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+ args.data = 971;
+ args.flags = OVERRIDE;
+ EXPECT_RETURNS(WriteTestMessage, &args);
+ recycle.flags = BLOCK;
+ EXPECT_RETURNS(ReadTestMessage, &recycle);
+
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+
+ TestMessage *msg = static_cast<TestMessage *>(aos_queue_get_msg(queue));
+ ASSERT_TRUE(msg != NULL);
+ msg->data = 341;
+ aos_queue_free_msg(queue, msg);
+ recycle.data = 341;
+ EXPECT_RETURNS(ReadTestMessage, &recycle);
+
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+
+ args.data = 254;
+ args.flags = PEEK;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ recycle.flags = BLOCK;
+ EXPECT_HANGS(ReadTestMessage, &recycle);
+ args.flags = BLOCK;
+ EXPECT_RETURNS(ReadTestMessage, &args);
+ recycle.data = 254;
+ EXPECT_RETURNS(ReadTestMessage, &recycle);
+}
+
diff --git a/aos/atom_code/ipc_lib/resource.c b/aos/atom_code/ipc_lib/resource.c
new file mode 100644
index 0000000..472191f
--- /dev/null
+++ b/aos/atom_code/ipc_lib/resource.c
@@ -0,0 +1,199 @@
+//#define TESTING_ASSERT(...)
+#define TESTING_ASSERT(cond, desc, args...) if(!(cond)){fprintf(stderr, "error: " desc " at " __FILE__ ": %d\n", ##args, __LINE__);}
+#define TESTING_ASSERT_RETURN(cond, desc, args...) TESTING_ASSERT(cond, desc, ##args); if(!(cond)){return 1;}
+// leave TESTING_ASSERT_RETURN (segfaults result otherwise)
+
+#include "resource_internal.h"
+#include <stdlib.h>
+#include <stdio.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <signal.h>
+#include <string.h>
+#include <errno.h>
+
+int RESOURCE_KILL_SIGNAL;
+
+__attribute__((constructor)) void __constructor(){
+ RESOURCE_KILL_SIGNAL = SIGRTMIN + 5;
+}
+
+void aos_resource_init(uint16_t num){
+ aos_resource *resource = aos_resource_get(num);
+ resource->num = num;
+ resource->resource_mutex = 0;
+ resource->modify = 0;
+ resource->request = 0;
+ resource->kill = 0;
+ resource->owner = 0;
+ resource->priorities = shm_malloc(sizeof(struct HeapStruct) + AOS_RESOURCE_PRIORITY_STACK_LENGTH * sizeof(uint8_t));
+ resource->priorities->Elements = (uint8_t *)((uintptr_t)resource->priorities + sizeof(struct HeapStruct)); // only do 1 shm_malloc (directly above)
+ Initialize(AOS_RESOURCE_PRIORITY_STACK_LENGTH, resource->priorities);
+ AOS_RESOURCE_STATE_SET_ON(AOS_RESOURCE_STATE_WANTS_IT, num, aos_resource_entity_root_get());
+}
+void aos_resource_entity_root_create(){
+ global_core->mem_struct->resources.root = aos_resource_entity_create(0);
+}
+inline aos_resource_entity *aos_resource_entity_root_get(){
+ return global_core->mem_struct->resources.root;
+}
+inline aos_resource *aos_resource_get(uint16_t num){
+ return &global_core->mem_struct->resources.resources[num];
+}
+
+aos_resource_entity *aos_resource_entity_create(uint8_t base_priority){
+ aos_resource_entity *local = shm_malloc(sizeof(aos_resource_entity));
+ memset(local->state, 0x00, sizeof(local->state));
+ local->base_priority = base_priority;
+ local->parent = NULL; // for the root entity
+ return local;
+}
+int aos_resource_entity_set_parent(aos_resource_entity *local, aos_resource_entity *parent){
+ TESTING_ASSERT_RETURN(local != NULL, "do not have a local entity");
+ TESTING_ASSERT_RETURN(parent != local, "can't set parent to self");
+ TESTING_ASSERT_RETURN(parent != NULL, "have to have a parent to set to");
+ local->parent = parent;
+ if(parent->parent == NULL){
+ local->root_action = getpid();
+ }else{
+ local->root_action = parent->root_action;
+ }
+ if(parent->priority > local->base_priority){
+ local->priority = parent->priority;
+ }else{
+ local->priority = local->base_priority;
+ }
+ if(local->state[0] != 0 || memcmp(local->state, local->state + 1, sizeof(local->state) - 1)){ // if it's not all 0s
+ TESTING_ASSERT(0, "local->state isn't all 0s when changing parents (fixing it)");
+ memset(local->state, 0x00, sizeof(local->state));
+ }
+ return 0;
+}
+
+void aos_resource_kill(pid_t action){
+ union sigval sival;
+ sival.sival_int = 0;
+ if(sigqueue(action, RESOURCE_KILL_SIGNAL, sival) < 0){ // if sending the signal failed
+ fprintf(stderr, "sigqueue RESOURCE_KILL_SIGNAL (which is %d) with pid %d failed with errno %d ", RESOURCE_KILL_SIGNAL, action, errno);
+ perror(NULL);
+ }
+}
+int aos_resource_request(aos_resource_entity *local, aos_resource *resource){
+ TESTING_ASSERT_RETURN(local != NULL, "do not have a local entity");
+
+ if(mutex_lock(&resource->request))
+ return 1;
+ if(mutex_lock(&resource->kill)){
+ mutex_unlock(&resource->request);
+ return 1;
+ }
+ if(mutex_lock(&resource->modify)){
+ mutex_unlock(&resource->kill);
+ mutex_unlock(&resource->request);
+ return 1;
+ }
+
+ aos_resource_entity *c = local;
+ while(c->parent != NULL && !AOS_RESOURCE_STATE_GET_HAS_IT(resource->num, c)){
+ c = c->parent;
+ }
+ TESTING_ASSERT((c->parent == NULL) == (c == aos_resource_entity_root_get()), "found a resource with no parent that isn't the root")
+ if(c->parent == NULL && !AOS_RESOURCE_STATE_GET_WANTS_IT(resource->num, c)){ // if c is the root entity and doesn't want it
+ TESTING_ASSERT(0, "root entity does not want resource %d (will fix it)", resource->num);
+ *((int *)NULL) = 0;
+ AOS_RESOURCE_STATE_SET_ON(AOS_RESOURCE_STATE_WANTS_IT, resource->num, c);
+ }
+ uint8_t locked_resource_mutex = 0;
+ if(AOS_RESOURCE_STATE_GET_HAS_PASSED_DOWN(resource->num, c)){
+ if(c->parent == NULL){
+ if(GetMin(resource->priorities) >= local->priority){
+ mutex_unlock(&resource->modify);
+ aos_resource_kill(resource->owner);
+ if(mutex_lock(&resource->kill)){ // got released by one that got killed (after unlocking resource_mutex)
+ mutex_unlock(&resource->resource_mutex);
+ mutex_unlock(&resource->request);
+ return 1;
+ }
+ if(mutex_lock(&resource->resource_mutex)){ // wait for the other process to release it
+ mutex_unlock(&resource->request);
+ return 1;
+ }
+ locked_resource_mutex = 1;
+ if(mutex_lock(&resource->modify)){
+ mutex_unlock(&resource->request);
+ return 1;
+ }
+ }else{
+ mutex_unlock(&resource->modify);
+ mutex_unlock(&resource->request);
+ return -1;
+ }
+ }else{
+ fprintf(stderr, "PROGRAMMER ERROR!!!!! 2 sub-actions both requested resource %d!!! stopping the root action\n", resource->num);
+ mutex_unlock(&resource->modify);
+ mutex_unlock(&resource->request);
+ return -1;
+ }
+ }
+ AOS_RESOURCE_STATE_SET_ON(AOS_RESOURCE_STATE_WANTS_IT, resource->num, local);
+ aos_resource_entity *c2 = local;
+ do{
+ c2 = c2->parent;
+ TESTING_ASSERT_RETURN(c2 != NULL, "couldn't find the parent that has resource %d", resource->num)
+ AOS_RESOURCE_STATE_SET_ON(AOS_RESOURCE_STATE_HAS_PASSED_DOWN, resource->num, c2);
+ } while(c2 != c);
+ TESTING_ASSERT(c2 == c, "c2 != c");
+ if(c->parent == NULL){ // if you found the root entity
+ resource->owner = local->root_action;
+ if(!locked_resource_mutex){
+ int rv = mutex_trylock(&resource->resource_mutex); // don't deadlock if somebody died or whatever
+ TESTING_ASSERT(rv == 0, "the resource_mutex was already locked when getting %d from the root", resource->num);
+ }
+ }else{
+ TESTING_ASSERT(resource->owner == local->root_action, "my action chain has resource %d, but my chain's root(%d) isn't the owner(%d)", resource->num, local->root_action, resource->owner);
+ TESTING_ASSERT(mutex_trylock(&resource->resource_mutex) != 0, "my action has the resource_mutex for %d, but the resource_mutex wasn't already locked", resource->num);
+ }
+ if(Insert(local->priority, resource->priorities) < 0){
+ fprintf(stderr, "BAD NEWS: ran out of space on the priority heap for resource %d. Increase the size of AOS_RESOURCE_PRIORITY_STACK_LENGTH in resource.h\n", resource->num);
+ mutex_unlock(&resource->modify);
+ mutex_unlock(&resource->request);
+ return -1;
+ }
+ mutex_unlock(&resource->modify);
+ mutex_unlock(&resource->kill);
+ mutex_unlock(&resource->request);
+ return 0;
+}
+
+int aos_resource_release(aos_resource_entity *local, aos_resource *resource){
+ TESTING_ASSERT_RETURN(local != NULL, "do not have a local entity");
+
+ if(mutex_lock(&resource->modify)){
+ return 1;
+ }
+
+ AOS_RESOURCE_STATE_SET_OFF(AOS_RESOURCE_STATE_WANTS_IT, resource->num, local);
+ if(!AOS_RESOURCE_STATE_GET_HAS_PASSED_DOWN(resource->num, local)){ // if we're actually supposed to go release it
+ aos_resource_entity *c = local;
+ while(c->parent != NULL && !AOS_RESOURCE_STATE_GET_WANTS_IT(resource->num, c)){
+ AOS_RESOURCE_STATE_SET_OFF(AOS_RESOURCE_STATE_HAS_PASSED_DOWN, resource->num, c);
+ c = c->parent;
+ }
+ if(c->parent == NULL && !AOS_RESOURCE_STATE_GET_WANTS_IT(resource->num, c)){ // if c is the root entity and doesn't want it
+ TESTING_ASSERT(0, "root entity does not want resource %d (will fix it)", resource->num);
+ AOS_RESOURCE_STATE_SET_ON(AOS_RESOURCE_STATE_WANTS_IT, resource->num, c);
+ }
+ AOS_RESOURCE_STATE_SET_OFF(AOS_RESOURCE_STATE_HAS_PASSED_DOWN, resource->num, c);
+ Remove(local->priority, resource->priorities);
+ TESTING_ASSERT(local->root_action == resource->owner, "freeing a resource (%d) whose owner(%d) isn't my chain's root(%d)", resource->num, resource->owner, local->root_action);
+ if(c->parent == NULL){ // if you gave it back to the root entity (c)
+ TESTING_ASSERT(IsEmpty(resource->priorities), "priority stack isn't empty (size=%d)", GetSize(resource->priorities));
+ resource->owner = 0;
+ mutex_unlock(&resource->resource_mutex);
+ mutex_unlock(&resource->kill);
+ }
+ } // else has passed it down
+ mutex_unlock(&resource->modify);
+ return 0;
+}
+
diff --git a/aos/atom_code/ipc_lib/resource.h b/aos/atom_code/ipc_lib/resource.h
new file mode 100644
index 0000000..de1de1a
--- /dev/null
+++ b/aos/atom_code/ipc_lib/resource.h
@@ -0,0 +1,28 @@
+#ifndef __AOS_RESOURCE_H_
+#define __AOS_RESOURCE_H_
+
+// notes at <https://docs.google.com/document/d/1gzRrVcqL2X9VgNQUI5DrvLVVVziIH7c5ZerATVbiS7U/edit?hl=en_US>
+
+#include <sys/types.h>
+#include "shared_mem.h"
+#include "binheap.h"
+#include "aos_sync.h"
+#include "core_lib.h"
+
+#define AOS_RESOURCE_PRIORITY_STACK_LENGTH 50
+
+extern int RESOURCE_KILL_SIGNAL;
+
+/* Return Values
+ 0 = success
+ -1 = you should stop (like if you got killed) (only for request)
+ 1 = error
+*/
+int aos_resource_request(aos_resource_entity *local, aos_resource *resource);
+int aos_resource_release(aos_resource_entity *local, aos_resource *resource);
+int aos_resource_entity_set_parent(aos_resource_entity *local, aos_resource_entity *parent);
+aos_resource_entity *aos_resource_entity_create(uint8_t base_priority);
+aos_resource *aos_resource_get(uint16_t num);
+aos_resource_entity *aos_resource_entity_root_get(void);
+
+#endif
diff --git a/aos/atom_code/ipc_lib/resource_core.h b/aos/atom_code/ipc_lib/resource_core.h
new file mode 100644
index 0000000..27d1af8
--- /dev/null
+++ b/aos/atom_code/ipc_lib/resource_core.h
@@ -0,0 +1,28 @@
+#ifndef AOS_ATOM_CODE_IPC_LIB_RESOURCE_CORE_H_
+#define AOS_ATOM_CODE_IPC_LIB_RESOURCE_CORE_H_
+// this file has to be separate due to #include order dependencies
+
+#include "aos/atom_code/ipc_lib/shared_mem.h"
+#include "aos/atom_code/ipc_lib/binheap.h"
+#include "aos/ResourceList.h"
+
+typedef struct aos_resource_t{
+ mutex resource_mutex; // gets locked whenever resource is taken from root entity and unlocked when its given back
+ mutex modify; // needs to be locked while somebody is requesting or releasing this resource
+ mutex request; // needs to be locked while somebody is requesting this resource (including possibly waiting for somebody else to release it after being killed)
+ mutex kill; // gets locked while somebody is dying for this resource (gets unlocked whenever this resource gets given back to the root entity)
+ pid_t owner;
+ PriorityQueue priorities; // lower number = higher priority
+
+ uint16_t num;
+} aos_resource;
+typedef struct aos_resource_entity_t aos_resource_entity;
+typedef struct aos_resource_list_t {
+ aos_resource resources[AOS_RESOURCE_NUM];
+ aos_resource_entity *root;
+} aos_resource_list;
+void aos_resource_init(uint16_t num);
+void aos_resource_entity_root_create(void);
+
+#endif
+
diff --git a/aos/atom_code/ipc_lib/resource_internal.h b/aos/atom_code/ipc_lib/resource_internal.h
new file mode 100644
index 0000000..eebecf9
--- /dev/null
+++ b/aos/atom_code/ipc_lib/resource_internal.h
@@ -0,0 +1,27 @@
+#ifndef __AOS_RESOURCE_INTERNAL_H_
+#define __AOS_RESOURCE_INTERNAL_H_
+
+#include "resource.h"
+
+#define AOS_RESOURCE_STATES_PER_BYTE 4
+#define AOS_RESOURCE_STATE_SHIFTER(num) ((num % AOS_RESOURCE_STATES_PER_BYTE) * (8 / AOS_RESOURCE_STATES_PER_BYTE))
+#define AOS_RESOURCE_STATE_GET(num, entity) (entity->state[num / AOS_RESOURCE_STATES_PER_BYTE] >> AOS_RESOURCE_STATE_SHIFTER(num))
+#define AOS_RESOURCE_STATE_SET_ON(mask, num, entity) (entity->state[num / AOS_RESOURCE_STATES_PER_BYTE] |= (mask << AOS_RESOURCE_STATE_SHIFTER(num)))
+#define AOS_RESOURCE_STATE_SET_OFF(mask, num, entity) (entity->state[num / AOS_RESOURCE_STATES_PER_BYTE] &= ~(mask << AOS_RESOURCE_STATE_SHIFTER(num)))
+#define AOS_RESOURCE_STATE_GET_HAS_IT(num, entity) (AOS_RESOURCE_STATE_GET_WANTS_IT(num, entity) || AOS_RESOURCE_STATE_GET_HAS_PASSED_DOWN(num, entity))
+#define AOS_RESOURCE_STATE_WANTS_IT 0x01
+#define AOS_RESOURCE_STATE_HAS_PASSED_DOWN 0x02
+#define AOS_RESOURCE_STATE_GET_WANTS_IT(num, entity) (AOS_RESOURCE_STATE_GET(num, entity) & AOS_RESOURCE_STATE_WANTS_IT)
+#define AOS_RESOURCE_STATE_GET_HAS_PASSED_DOWN(num, entity) (AOS_RESOURCE_STATE_GET(num, entity) & AOS_RESOURCE_STATE_HAS_PASSED_DOWN)
+
+struct aos_resource_entity_t{
+ aos_resource_entity *parent;
+ uint8_t state[(AOS_RESOURCE_NUM + (AOS_RESOURCE_STATES_PER_BYTE - 1)) / AOS_RESOURCE_STATES_PER_BYTE];
+ pid_t root_action;
+ uint8_t base_priority, priority;
+};
+
+inline void aos_resource_init(uint16_t num);
+
+#endif
+
diff --git a/aos/atom_code/ipc_lib/resource_list.txt b/aos/atom_code/ipc_lib/resource_list.txt
new file mode 100644
index 0000000..1aabdca
--- /dev/null
+++ b/aos/atom_code/ipc_lib/resource_list.txt
@@ -0,0 +1,4 @@
+test_resource1
+test_resource2
+drivetrain_left
+drivetrain_right
diff --git a/aos/atom_code/ipc_lib/resource_test.cpp b/aos/atom_code/ipc_lib/resource_test.cpp
new file mode 100644
index 0000000..9ce6e06
--- /dev/null
+++ b/aos/atom_code/ipc_lib/resource_test.cpp
@@ -0,0 +1,28 @@
+#include "aos/atom_code/ipc_lib/sharedmem_test_setup.h"
+#include "aos/aos_core.h"
+
+#include <gtest/gtest.h>
+#include <gtest/gtest-spi.h>
+
+class ResourceTest : public SharedMemTestSetup{
+};
+
+TEST_F(ResourceTest, GetResource){
+ aos_resource *first = aos_resource_get(1);
+ AllSharedMemAllocated();
+ aos_resource *second = aos_resource_get(1);
+ EXPECT_EQ(first, second);
+}
+TEST_F(ResourceTest, CheckLocal){
+ EXPECT_EQ(1, aos_resource_request(NULL, aos_resource_get(2)));
+}
+
+TEST_F(ResourceTest, LocalCreate){
+ EXPECT_TRUE(aos_resource_entity_create(56) != NULL);
+}
+TEST_F(ResourceTest, LocalSetParentSelf){
+ aos_resource_entity *local = aos_resource_entity_create(76);
+ ASSERT_TRUE(local != NULL);
+ EXPECT_EQ(1, aos_resource_entity_set_parent(local, local));
+}
+
diff --git a/aos/atom_code/ipc_lib/shared_mem.c b/aos/atom_code/ipc_lib/shared_mem.c
new file mode 100644
index 0000000..f631b78
--- /dev/null
+++ b/aos/atom_code/ipc_lib/shared_mem.c
@@ -0,0 +1,119 @@
+#include "shared_mem.h"
+
+#include <stdio.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <errno.h>
+
+// the path for the shared memory segment. see shm_open(3) for restrictions
+#define AOS_SHM_NAME "/aos_shared_mem"
+// Size of the shared mem segment.
+// set to the maximum number that worked
+#define SIZEOFSHMSEG (4096 * 27813)
+
+ptrdiff_t aos_core_get_mem_usage(void) {
+ return global_core->size -
+ ((ptrdiff_t)global_core->mem_struct->msg_alloc -
+ (ptrdiff_t)global_core->mem_struct);
+}
+
+struct aos_core global_core_data;
+struct aos_core *global_core = NULL;
+
+int aos_core_create_shared_mem(enum aos_core_create to_create) {
+ global_core = &global_core_data;
+ int shm;
+before:
+ if (to_create == create) {
+ printf("shared_mem: creating\n");
+ shm = shm_open(AOS_SHM_NAME, O_RDWR | O_CREAT | O_EXCL, 0666);
+ global_core->owner = 1;
+ if (shm == -1 && errno == EEXIST) {
+ printf("shared_mem: going to shm_unlink(" AOS_SHM_NAME ")\n");
+ if (shm_unlink(AOS_SHM_NAME) == -1) {
+ fprintf(stderr, "shared_mem: shm_unlink(" AOS_SHM_NAME ") failed with of %d: %s\n", errno, strerror(errno));
+ } else {
+ goto before;
+ }
+ }
+ } else {
+ printf("shared_mem: not creating\n");
+ shm = shm_open(AOS_SHM_NAME, O_RDWR, 0);
+ global_core->owner = 0;
+ }
+ if (shm == -1) {
+ fprintf(stderr, "shared_mem:"
+ " shm_open(" AOS_SHM_NAME ", O_RDWR [| O_CREAT | O_EXCL, 0|0666)"
+ " failed with %d: %s\n", errno, strerror(errno));
+ return -1;
+ }
+ if (global_core->owner) {
+ if (ftruncate(shm, SIZEOFSHMSEG) == -1) {
+ fprintf(stderr, "shared_mem: fruncate(%d, 0x%zx) failed with %d: %s\n",
+ shm, (size_t)SIZEOFSHMSEG, errno, strerror(errno));
+ return -1;
+ }
+ }
+ void *shm_address = mmap(
+ (void *)SHM_START, SIZEOFSHMSEG, PROT_READ | PROT_WRITE,
+ MAP_SHARED | MAP_FIXED | MAP_LOCKED | MAP_POPULATE, shm, 0);
+ if (shm_address == MAP_FAILED) {
+ fprintf(stderr, "shared_mem: mmap(%p, 0x%zx, stuff, stuff, %d, 0) failed"
+ " with %d: %s\n",
+ (void *)SHM_START, SIZEOFSHMSEG, shm, errno, strerror(errno));
+ return -1;
+ }
+ printf("shared_mem: shm at: %p\n", shm_address);
+ if (close(shm) == -1) {
+ printf("shared_mem: close(%d(=shm) failed with %d: %s\n",
+ shm, errno, strerror(errno));
+ }
+ if (shm_address != (void *)SHM_START) {
+ fprintf(stderr, "shared_mem: shm isn't at hard-coded %p. at %p instead\n",
+ (void *)SHM_START, shm_address);
+ return -1;
+ }
+ return aos_core_use_address_as_shared_mem(shm_address, SIZEOFSHMSEG);
+}
+
+int aos_core_use_address_as_shared_mem(void *address, size_t size) {
+ global_core->mem_struct = address;
+ global_core->size = size;
+ global_core->shared_mem = (uint8_t *)address + sizeof(*global_core->mem_struct);
+ if (global_core->owner) {
+ global_core->mem_struct->msg_alloc = (uint8_t *)address + global_core->size;
+ init_shared_mem_core(global_core->mem_struct);
+ }
+ if (global_core->owner) {
+ condition_set(&global_core->mem_struct->creation_condition);
+ } else {
+ if (condition_wait(&global_core->mem_struct->creation_condition) != 0) {
+ fprintf(stderr, "waiting on creation_condition failed\n");
+ return -1;
+ }
+ }
+ fprintf(stderr, "shared_mem: end of create_shared_mem owner=%d\n",
+ global_core->owner);
+ return 0;
+}
+
+int aos_core_free_shared_mem(){
+ void *shm_address = global_core->shared_mem;
+ if (munmap((void *)SHM_START, SIZEOFSHMSEG) == -1) {
+ fprintf(stderr, "shared_mem: munmap(%p, 0x%zx) failed with %d: %s\n",
+ shm_address, SIZEOFSHMSEG, errno, strerror(errno));
+ return -1;
+ }
+ if (global_core->owner) {
+ if (shm_unlink(AOS_SHM_NAME)) {
+ fprintf(stderr, "shared_mem: shm_unlink(" AOS_SHM_NAME ") failed with %d: %s\n",
+ errno, strerror(errno));
+ return -1;
+ }
+ }
+ return 0;
+}
+
diff --git a/aos/atom_code/ipc_lib/shared_mem.h b/aos/atom_code/ipc_lib/shared_mem.h
new file mode 100644
index 0000000..b1a2608
--- /dev/null
+++ b/aos/atom_code/ipc_lib/shared_mem.h
@@ -0,0 +1,46 @@
+#ifndef _SHARED_MEM_H_
+#define _SHARED_MEM_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "core_lib.h"
+#include <stddef.h>
+#include <unistd.h>
+
+// Where the shared memory segment starts in each process's address space.
+// Has to be the same in all of them so that stuff in shared memory
+// can have regular pointers to other stuff in shared memory.
+#define SHM_START 0x20000000
+
+enum aos_core_create {
+ create,
+ reference
+};
+struct aos_core {
+ int owner;
+ void *shared_mem;
+ // How large the chunk of shared memory is.
+ ptrdiff_t size;
+ aos_shm_core *mem_struct;
+};
+
+ptrdiff_t aos_core_get_mem_usage(void);
+
+// Takes the specified memory address and uses it as the shared memory.
+// address is the memory address, and size is the size of the memory.
+// global_core needs to point to an instance of struct aos_core, and owner
+// should be set correctly there.
+// The owner should verify that the first sizeof(mutex) of data is set to 0
+// before passing the memory to this function.
+int aos_core_use_address_as_shared_mem(void *address, size_t size);
+
+int aos_core_create_shared_mem(enum aos_core_create to_create);
+int aos_core_free_shared_mem(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/aos/atom_code/ipc_lib/sharedmem_test_setup.h b/aos/atom_code/ipc_lib/sharedmem_test_setup.h
new file mode 100644
index 0000000..e461c43
--- /dev/null
+++ b/aos/atom_code/ipc_lib/sharedmem_test_setup.h
@@ -0,0 +1,147 @@
+// defines a fixture (SharedMemTestSetup) that sets up shared memory
+
+extern "C" {
+#include "shared_mem.h"
+ extern struct aos_core *global_core;
+}
+
+#include <signal.h>
+
+#include <gtest/gtest.h>
+#include <sys/types.h>
+
+// TODO(brians) read logs from here
+class SharedMemTestSetup : public testing::Test{
+ protected:
+ pid_t core;
+ int start[2];
+ int memcheck[2];
+ static void signal_handler(int){
+ if(aos_core_free_shared_mem()){
+ exit(- 1);
+ }
+ exit(0);
+ }
+ static int get_mem_usage(){
+ return global_core->size - ((uint8_t *)global_core->mem_struct->msg_alloc - (uint8_t *)SHM_START);
+ }
+ bool checking_mem;
+
+ virtual void BeforeLocalShmSetup() {}
+ virtual void SetUp(){
+ ASSERT_EQ(0, pipe(start)) << "couldn't create start pipes";
+ ASSERT_EQ(0, pipe(memcheck)) << "couldn't create memcheck pipes";
+ checking_mem = false;
+ if((core = fork()) == 0){
+ close(start[0]);
+ close(memcheck[1]);
+ struct sigaction act;
+ act.sa_handler = signal_handler;
+ sigaction(SIGINT, &act, NULL);
+ if(aos_core_create_shared_mem(create)){
+ exit(-1);
+ }
+ write_pipe(start[1], "a", 1);
+ int usage = 0;
+ while(1){
+ char buf1;
+ read_pipe(memcheck[0], &buf1, 1);
+ if(usage == 0)
+ usage = get_mem_usage();
+ if(usage == get_mem_usage())
+ buf1 = 1;
+ else
+ buf1 = 0;
+ write_pipe(start[1], &buf1, 1);
+ }
+ }
+ close(start[1]);
+ close(memcheck[0]);
+ ASSERT_NE(-1, core) << "fork failed";
+ char buf;
+ read_pipe(start[0], &buf, 1);
+
+ BeforeLocalShmSetup();
+
+ ASSERT_EQ(0, aos_core_create_shared_mem(reference)) << "couldn't create shared mem reference";
+ }
+ virtual void TearDown(){
+ if(checking_mem){
+ write_pipe(memcheck[1], "a", 1);
+ char buf;
+ read_pipe(start[0], &buf, 1);
+ EXPECT_EQ(1, buf) << "memory got leaked";
+ }
+ EXPECT_EQ(0, aos_core_free_shared_mem()) << "issues freeing shared mem";
+ if(core > 0){
+ kill(core, SIGINT);
+ siginfo_t status;
+ ASSERT_EQ(0, waitid(P_PID, core, &status, WEXITED)) << "waiting for the core to finish failed";
+ EXPECT_EQ(CLD_EXITED, status.si_code) << "core died";
+ EXPECT_EQ(0, status.si_status) << "core exited with an error";
+ }
+ }
+ // if any more shared memory gets allocated after calling this and not freed by the end, it's an error
+ void AllSharedMemAllocated(){
+ checking_mem = true;
+ write_pipe(memcheck[1], "a", 1);
+ char buf;
+ read_pipe(start[0], &buf, 1);
+ }
+ private:
+ // Wrapper functions for pipes because they should never have errors.
+ void read_pipe(int fd, void *buf, size_t count) {
+ if (read(fd, buf, count) < 0) abort();
+ }
+ void write_pipe(int fd, const void *buf, size_t count) {
+ if (write(fd, buf, count) < 0) abort();
+ }
+};
+class ExecVeTestSetup : public SharedMemTestSetup {
+ protected:
+ std::vector<std::string> files;
+ std::vector<pid_t> pids;
+ virtual void BeforeLocalShmSetup(){
+ std::vector<std::string>::iterator it;
+ pid_t child;
+ for(it = files.begin(); it < files.end(); ++it){
+ if((child = fork()) == 0){
+ char *null = NULL;
+ execve(it->c_str(), &null, &null);
+ ADD_FAILURE() << "execve failed";
+ perror("execve");
+ exit(0);
+ }
+ if(child > 0)
+ pids.push_back(child);
+ else
+ ADD_FAILURE() << "fork failed return=" << child;
+ }
+ usleep(150000);
+ }
+ virtual void TearDown(){
+ std::vector<pid_t>::iterator it;
+ siginfo_t status;
+ for(it = pids.begin(); it < pids.end(); ++it){
+ printf("attempting to SIGINT(%d) %d\n", SIGINT, *it);
+ if(*it > 0){
+ kill(*it, SIGINT);
+ ASSERT_EQ(0, waitid(P_PID, *it, &status, WEXITED)) << "waiting for the AsyncAction(pid=" << *it << ") to finish failed";
+ EXPECT_EQ(CLD_EXITED, status.si_code) << "child died (killed by signal is " << (int)CLD_KILLED << ")";
+ EXPECT_EQ(0, status.si_status) << "child exited with an error";
+ }else{
+ FAIL();
+ }
+ }
+
+ SharedMemTestSetup::TearDown();
+ }
+ // call this _before_ ExecVeTestSetup::SetUp()
+ void AddProcess(const std::string file){
+ files.push_back(file);
+ }
+ void PercolatePause(){
+ usleep(50000);
+ }
+};
+
diff --git a/aos/atom_code/logging/atom_logging.cpp b/aos/atom_code/logging/atom_logging.cpp
new file mode 100644
index 0000000..e98078b
--- /dev/null
+++ b/aos/atom_code/logging/atom_logging.cpp
@@ -0,0 +1,259 @@
+#include <stdarg.h>
+#include <stdio.h>
+#include <string.h>
+#include <time.h>
+#include <sys/types.h>
+#include <errno.h>
+#include <unistd.h>
+#include <limits.h>
+
+#include <algorithm>
+
+#include "aos/aos_core.h"
+#include "aos/common/die.h"
+
+#define DECL_LEVEL(name, value) const log_level name = value;
+DECL_LEVELS
+#undef DECL_LEVEL
+
+log_level log_min = 0;
+
+static const aos_type_sig message_sig = {sizeof(log_queue_message), 1234, 1500};
+static const char *name;
+static size_t name_size;
+static aos_queue *queue;
+static log_message corked_message;
+static int cork_line_min, cork_line_max;
+bool log_initted = false;
+
+static inline void cork_init() {
+ corked_message.message[0] = '\0'; // make strlen of it 0
+ cork_line_min = INT_MAX;
+ cork_line_max = -1;
+}
+int log_init(const char *name_in){
+ if (log_initted) {
+ return 1;
+ }
+
+ const size_t name_in_len = strlen(name_in);
+ const char *last_slash = static_cast<const char *>(memrchr(name_in,
+ '/', name_in_len));
+ if (last_slash == NULL) {
+ name_size = name_in_len;
+ last_slash = name_in - 1;
+ } else {
+ name_size = name_in + name_in_len - last_slash;
+ }
+ if (name_size >= sizeof(log_message::name)) {
+ fprintf(stderr, "logging: error: name '%s' (going to use %zu bytes) is too long\n",
+ name_in, name_size);
+ return -1;
+ }
+ char *const tmp = static_cast<char *>(malloc(name_size + 1));
+ if (tmp == NULL) {
+ fprintf(stderr, "logging: error: couldn't malloc(%zd)\n", name_size + 1);
+ return -1;
+ }
+ name = tmp;
+ memcpy(tmp, last_slash + 1, name_size);
+ tmp[name_size] = 0;
+ queue = aos_fetch_queue("LoggingQueue", &message_sig);
+ if (queue == NULL) {
+ fprintf(stderr, "logging: error: couldn't fetch queue\n");
+ return -1;
+ }
+
+ cork_init();
+
+ log_initted = true;
+ return 0;
+}
+void log_uninit() {
+ free(const_cast<char *>(name));
+ name = NULL;
+ name_size = 0;
+ queue = NULL;
+ log_initted = false;
+}
+
+static inline void check_init() {
+ if (!log_initted) {
+ fprintf(stderr, "logging: warning: not initialized in %jd."
+ " initializing using \"<null>\" as name\n", static_cast<intmax_t>(getpid()));
+ log_init("<null>");
+ }
+}
+
+const log_message *log_read_next2(int flags, int *index) {
+ check_init();
+ return static_cast<const log_message *>(aos_queue_read_msg_index(queue, flags, index));
+}
+const log_message *log_read_next1(int flags) {
+ check_init();
+ const log_message *r = NULL;
+ do {
+ r = static_cast<const log_message *>(aos_queue_read_msg(queue, flags));
+ } while ((flags & BLOCK) && r == NULL); // not blocking means return a NULL if that's what it gets
+ return r;
+}
+void log_free_message(const log_message *msg) {
+ check_init();
+ aos_queue_free_msg(queue, msg);
+}
+
+int log_crio_message_send(log_crio_message &to_send) {
+ check_init();
+
+ log_crio_message *msg = static_cast<log_crio_message *>(aos_queue_get_msg(queue));
+ if (msg == NULL) {
+ fprintf(stderr, "logging: error: couldn't get a message to send '%s'\n",
+ to_send.message);
+ return -1;
+ }
+ //*msg = to_send;
+ static_assert(sizeof(to_send) == sizeof(*msg), "something is very wrong here");
+ memcpy(msg, &to_send, sizeof(to_send));
+ if (aos_queue_write_msg(queue, msg, OVERRIDE) < 0) {
+ fprintf(stderr, "logging: error: writing crio message '%s' failed\n", msg->message);
+ aos_queue_free_msg(queue, msg);
+ return -1;
+ }
+
+ return 0;
+}
+
+// Prints format (with ap) into output and correctly deals with the message
+// being too long etc.
+// Returns whether it succeeded or not.
+static inline bool vsprintf_in(char *output, size_t output_size,
+ const char *format, va_list ap) {
+ static const char *continued = "...\n";
+ const size_t size = output_size - strlen(continued);
+ const int ret = vsnprintf(output, size, format, ap);
+ if (ret < 0) {
+ fprintf(stderr, "logging: error: vsnprintf failed with %d (%s)\n",
+ errno, strerror(errno));
+ return false;
+ } else if (static_cast<uintmax_t>(ret) >= static_cast<uintmax_t>(size)) {
+ // overwrite the NULL at the end of the existing one and
+ // copy in the one on the end of continued
+ memcpy(&output[size - 1], continued, strlen(continued) + 1);
+ }
+ return true;
+}
+static inline bool write_message(log_message *msg, log_level level) {
+ msg->level = level;
+ msg->source = getpid();
+ memcpy(msg->name, name, name_size + 1);
+ if (clock_gettime(CLOCK_REALTIME, &msg->time) == -1) {
+ fprintf(stderr, "logging: warning: couldn't get the current time "
+ "because of %d (%s)\n", errno, strerror(errno));
+ msg->time.tv_sec = 0;
+ msg->time.tv_nsec = 0;
+ }
+
+ static uint8_t local_sequence = -1;
+ msg->sequence = ++local_sequence;
+
+ if (aos_queue_write_msg(queue, msg, OVERRIDE) < 0) {
+ fprintf(stderr, "logging: error: writing message '%s' failed\n", msg->message);
+ aos_queue_free_msg(queue, msg);
+ return false;
+ }
+ return true;
+}
+static inline int vlog_do(log_level level, const char *format, va_list ap) {
+ log_message *msg = static_cast<log_message *>(aos_queue_get_msg(queue));
+ if (msg == NULL) {
+ fprintf(stderr, "logging: error: couldn't get a message to send '%s'\n", format);
+ return -1;
+ }
+
+ if (!vsprintf_in(msg->message, sizeof(msg->message), format, ap)) {
+ return -1;
+ }
+
+ if (!write_message(msg, level)) {
+ return -1;
+ }
+
+ if (level == FATAL) {
+ aos::Die("%s", msg->message);
+ }
+
+ return 0;
+}
+int log_do(log_level level, const char *format, ...) {
+ check_init();
+ va_list ap;
+ va_start(ap, format);
+ const int ret = vlog_do(level, format, ap);
+ va_end(ap);
+ return ret;
+}
+
+static inline int vlog_cork(int line, const char *format, va_list ap) {
+ const size_t message_length = strlen(corked_message.message);
+ if (line > cork_line_max) cork_line_max = line;
+ if (line < cork_line_min) cork_line_min = line;
+ return vsprintf_in(corked_message.message + message_length,
+ sizeof(corked_message.message) - message_length, format, ap) ? 0 : -1;
+}
+int log_cork(int line, const char *format, ...) {
+ check_init();
+ va_list ap;
+ va_start(ap, format);
+ const int ret = vlog_cork(line, format, ap);
+ va_end(ap);
+ return ret;
+}
+static inline bool log_uncork_helper(char *output, size_t output_size,
+ const char *format, ...) {
+ check_init();
+ va_list ap;
+ va_start(ap, format);
+ const bool ret = vsprintf_in(output, output_size, format, ap);
+ va_end(ap);
+ return ret;
+}
+int log_uncork(int line, log_level level, const char *begin_format,
+ const char *format, ...) {
+ check_init();
+ va_list ap;
+ va_start(ap, format);
+ const int ret = vlog_cork(line, format, ap);
+ va_end(ap);
+ if (ret != 0) {
+ return ret;
+ }
+
+ log_message *msg = static_cast<log_message *>(aos_queue_get_msg(queue));
+ if (msg == NULL) {
+ fprintf(stderr, "logging: error: couldn't get a message to send '%s'\n", format);
+ cork_init();
+ return -1;
+ }
+
+ static char new_format[LOG_MESSAGE_LEN];
+ if (!log_uncork_helper(new_format, sizeof(new_format), begin_format,
+ cork_line_min, cork_line_max)) {
+ cork_init();
+ return -1;
+ }
+ const size_t new_length = strlen(new_format);
+ memcpy(msg->message, new_format, new_length);
+ memcpy(msg->message + new_length, corked_message.message,
+ std::min(strlen(corked_message.message) + 1,
+ sizeof(msg->message) - new_length));
+ // in case corked_message.message was too long, it'll still be NULL-terminated
+ msg->message[sizeof(msg->message) - 1] = '\0';
+ cork_init();
+
+ if (!write_message(msg, level)) {
+ return -1;
+ }
+
+ return 0;
+}
+
diff --git a/aos/atom_code/logging/atom_logging.h b/aos/atom_code/logging/atom_logging.h
new file mode 100644
index 0000000..6211e22
--- /dev/null
+++ b/aos/atom_code/logging/atom_logging.h
@@ -0,0 +1,109 @@
+#ifndef AOS_ATOM_CODE_LOGGING_LOGGING_H_
+#define AOS_ATOM_CODE_LOGGING_LOGGING_H_
+
+// IWYU pragma: private, include "aos/common/logging/logging.h"
+
+#ifndef AOS_COMMON_LOGGING_LOGGING_H_
+#error This file may only be #included through common/logging/logging.h!!!
+#endif
+
+#include "aos/aos_core.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int log_init(const char *name);
+// WARNING: THIS LEAKS MEMORY AND SHARED MEMORY
+void log_uninit(void);
+
+extern log_level log_min;
+
+// The basic structure that goes into the shared memory queue.
+// It is packed so the pid_t at the front is at the same location as
+// the one in log_crio_message.
+typedef struct log_message_t_ {
+ pid_t source;
+ log_level level;
+ char message[LOG_MESSAGE_LEN];
+ char name[40];
+ struct timespec time;
+ uint8_t sequence; // per process
+} __attribute__((packed)) log_message;
+
+#ifdef __cplusplus
+#define LOG_BOOL bool
+#else
+#define LOG_BOOL uint8_t
+#endif
+extern LOG_BOOL log_initted;
+#undef LOG_BOOL
+
+// Unless explicitly stated otherwise, format must always be a string constant
+// and args are printf-style arguments for format.
+// The validitiy of format and args together will be checked at compile time
+// using a gcc function attribute.
+
+// Logs the specified thing.
+#define LOG(level, format, args...) do { \
+ if (level >= log_min) { \
+ log_do(level, LOG_SOURCENAME ": " STRINGIFY(__LINE__) ": " format, ##args); \
+ } \
+} while (0)
+// Allows "bottling up" multiple log fragments which can then all be logged in
+// one message with LOG_UNCORK.
+// format does not have to be a constant
+#define LOG_CORK(format, args...) do { \
+ log_cork(__LINE__, format, ##args); \
+} while (0)
+// Actually logs all of the saved up log fragments (including format and args on
+// the end).
+#define LOG_UNCORK(level, format, args...) do { \
+ log_uncork(__LINE__, level, LOG_SOURCENAME ": %d-%d: ", format, ##args); \
+} while (0)
+// Makes a normal logging call if possible or just prints it out on stderr.
+#define LOG_IFINIT(level, format, args...) do{ \
+ if(log_initted) { \
+ LOG(level, format, args); \
+ } else { \
+ fprintf(stderr, "%s-noinit: " format, log_str(level), ##args); \
+ } \
+}while(0)
+
+// All functions return 0 for success and - for error.
+
+// Actually implements the basic logging call.
+// Does not check that level is valid.
+// TODO(brians): Fix this so it works with clang.
+int log_do(log_level level, const char *format, ...)
+ __attribute__((format(gnu_printf, 2, 3)));
+
+// TODO(brians): Fix this so it works with clang.
+int log_cork(int line, const char *format, ...)
+ __attribute__((format(gnu_printf, 2, 3)));
+// Implements the uncork logging call.
+// IMPORTANT: begin_format must have 2 %d formats as its only 2 format specifiers
+// which will get passed the minimum and maximum line numbers that have been
+// corked into this call.
+// TODO(brians): Fix this so it works with clang.
+int log_uncork(int line, log_level level, const char *begin_format,
+ const char *format, ...)
+ __attribute__((format(gnu_printf, 4, 5)));
+
+const log_message *log_read_next1(int flags);
+const log_message *log_read_next2(int flags, int *index);
+inline const log_message *log_read_next(void) { return log_read_next1(BLOCK); }
+void log_free_message(const log_message *msg);
+
+// The structure that is actually in the shared memory queue.
+union log_queue_message {
+ log_message atom;
+ log_crio_message crio;
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
diff --git a/aos/atom_code/logging/atom_logging_test.cpp b/aos/atom_code/logging/atom_logging_test.cpp
new file mode 100644
index 0000000..a97d1e9
--- /dev/null
+++ b/aos/atom_code/logging/atom_logging_test.cpp
@@ -0,0 +1,146 @@
+#include <string>
+
+#include "gtest/gtest.h"
+
+#include "aos/aos_core.h"
+#include "aos/atom_code/ipc_lib/sharedmem_test_setup.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/common/inttypes.h"
+#include "aos/common/time.h"
+
+using ::aos::time::Time;
+using ::testing::AssertionResult;
+using ::testing::AssertionSuccess;
+using ::testing::AssertionFailure;
+
+namespace aos {
+namespace testing {
+
+static const std::string kLoggingName = "LoggingTestName";
+
+class LoggingTest : public SharedMemTestSetup {
+ virtual void SetUp() {
+ SharedMemTestSetup::SetUp();
+ ASSERT_EQ(0, log_init(kLoggingName.c_str()));
+ }
+ virtual void TearDown() {
+ log_uninit();
+ SharedMemTestSetup::TearDown();
+ }
+
+ public:
+ AssertionResult WasAnythingLogged() {
+ const log_message *msg = log_read_next1(NON_BLOCK);
+ if (msg != NULL) {
+ char bad_msg[LOG_MESSAGE_LEN];
+ memcpy(bad_msg, msg->message, sizeof(bad_msg));
+ log_free_message(msg);
+ return AssertionSuccess() << "read message '" << bad_msg << "'";
+ }
+ return AssertionFailure();
+ }
+ AssertionResult WasLogged(log_level level, const std::string value) {
+ const log_message *msg = NULL;
+ char bad_msg[LOG_MESSAGE_LEN];
+ bad_msg[0] = '\0';
+ const pid_t owner = getpid();
+ while (true) {
+ if (msg != NULL) {
+ static_assert(sizeof(bad_msg) == sizeof(msg->message),
+ "something is wrong");
+ if (bad_msg[0] != '\0') {
+ printf("read bad message: %s", bad_msg);
+ }
+ memcpy(bad_msg, msg->message, sizeof(bad_msg));
+ log_free_message(msg);
+ msg = NULL;
+ }
+ msg = log_read_next1(NON_BLOCK);
+ if (msg == NULL) {
+ return AssertionFailure() << "last message read was '" << bad_msg << "'"
+ " instead of '" << value << "'";
+ }
+ if (msg->source != owner) continue;
+ if (msg->level != level) continue;
+ if (strcmp(msg->name, kLoggingName.c_str()) != 0) continue;
+ if (strcmp(msg->message + strlen(msg->message) - value.length(),
+ value.c_str()) != 0) continue;
+
+ // if it's gotten this far, then the message is correct
+ log_free_message(msg);
+ return AssertionSuccess();
+ }
+ }
+};
+typedef LoggingTest LoggingDeathTest;
+
+// Tests both basic logging functionality and that the test setup works
+// correctly.
+TEST_F(LoggingTest, Basic) {
+ EXPECT_FALSE(WasAnythingLogged());
+ LOG(INFO, "test log 1\n");
+ EXPECT_TRUE(WasLogged(INFO, "test log 1\n"));
+ LOG(INFO, "test log 1.5\n");
+ // there's a subtle typo on purpose...
+ EXPECT_FALSE(WasLogged(INFO, "test log 15\n"));
+ LOG(ERROR, "test log 2=%d\n", 55);
+ EXPECT_TRUE(WasLogged(ERROR, "test log 2=55\n"));
+ LOG(ERROR, "test log 3\n");
+ EXPECT_FALSE(WasLogged(WARNING, "test log 3\n"));
+ LOG(WARNING, "test log 4\n");
+ EXPECT_TRUE(WasAnythingLogged());
+}
+TEST_F(LoggingTest, Cork) {
+ static const int begin_line = __LINE__;
+ LOG_CORK("first part ");
+ LOG_CORK("second part (=%d) ", 19);
+ EXPECT_FALSE(WasAnythingLogged());
+ LOG_CORK("third part ");
+ static const int end_line = __LINE__;
+ LOG_UNCORK(WARNING, "last part %d\n", 5);
+ char *expected;
+ ASSERT_GT(asprintf(&expected, "atom_logging_test.cpp: %d-%d: "
+ "first part second part (=19) third part last part 5\n",
+ begin_line + 1, end_line + 1), 0);
+ EXPECT_TRUE(WasLogged(WARNING, std::string(expected)));
+}
+
+TEST_F(LoggingDeathTest, Fatal) {
+ ASSERT_EXIT(LOG(FATAL, "this should crash it\n"),
+ ::testing::KilledBySignal(SIGABRT),
+ "this should crash it");
+}
+
+TEST_F(LoggingTest, PrintfDirectives) {
+ LOG(INFO, "test log %%1 %%d\n");
+ EXPECT_TRUE(WasLogged(INFO, "test log %1 %d\n"));
+ LOG_DYNAMIC(WARNING, "test log %%2 %%f\n");
+ EXPECT_TRUE(WasLogged(WARNING, "test log %2 %f\n"));
+ LOG_CORK("log 3 part %%1 %%d ");
+ LOG_UNCORK(DEBUG, "log 3 part %%2 %%f\n");
+ EXPECT_TRUE(WasLogged(DEBUG, "log 3 part %1 %d log 3 part %2 %f\n"));
+}
+
+// For writing only.
+TEST_F(LoggingTest, Timing) {
+ static const long kTimingCycles = 5000;
+ Time start = Time::Now();
+ for (long i = 0; i < kTimingCycles; ++i) {
+ LOG(INFO, "a\n");
+ }
+ Time elapsed = Time::Now() - start;
+
+ printf("short message took %"PRId64" nsec for %ld\n", elapsed.ToNSec(),
+ kTimingCycles);
+
+ start = Time::Now();
+ for (long i = 0; i < kTimingCycles; ++i) {
+ LOG(INFO, "something longer than just \"a\" to log to test timing\n");
+ }
+ elapsed = Time::Now() - start;
+ printf("long message took %"PRId64" nsec for %ld\n", elapsed.ToNSec(),
+ kTimingCycles);
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/atom_code/logging/logging.gyp b/aos/atom_code/logging/logging.gyp
new file mode 100644
index 0000000..5d29da8
--- /dev/null
+++ b/aos/atom_code/logging/logging.gyp
@@ -0,0 +1,15 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'atom_logging_test',
+ 'type': 'executable',
+ 'sources': [
+ 'atom_logging_test.cpp',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ ],
+}
diff --git a/aos/atom_code/messages/DriverStationDisplay.cpp b/aos/atom_code/messages/DriverStationDisplay.cpp
new file mode 100644
index 0000000..e48a1eb
--- /dev/null
+++ b/aos/atom_code/messages/DriverStationDisplay.cpp
@@ -0,0 +1,54 @@
+#include "DriverStationDisplay.h"
+
+#include <stdarg.h>
+#include <stdio.h>
+
+using namespace aos;
+
+static const aos_type_sig signature = {sizeof(DriverStationDisplay), 1234, 10};
+aos_queue *DriverStationDisplay::queue = NULL;
+void DriverStationDisplay::GetQueue() {
+ if (queue == NULL) {
+ queue = aos_fetch_queue("DriverStationDisplay", &signature);
+ }
+}
+
+void DriverStationDisplay::Send(int line, const char *fmt, ...) {
+ GetQueue();
+ DriverStationDisplay *msg = static_cast<DriverStationDisplay *>(
+ aos_queue_get_msg(queue));
+ if (msg == NULL) {
+ LOG(WARNING, "could not get message to send '%s' to the DS queue\n", fmt);
+ return;
+ }
+ msg->line = static_cast<uint8_t>(line);
+
+ va_list ap;
+ va_start(ap, fmt);
+ int ret = vsnprintf(msg->data, sizeof(msg->data), fmt, ap);
+ va_end(ap);
+ if (ret < 0) {
+ LOG(WARNING, "could not format '%s' with arguments\n", fmt);
+ aos_queue_free_msg(queue, msg);
+ return;
+ } else if (static_cast<uintmax_t>(ret) >=
+ static_cast<uintmax_t>(sizeof(msg->data))) {
+ LOG(WARNING, "format '%s' ended up longer than the max size (%zd)\n",
+ fmt, sizeof(msg->data));
+ }
+
+ if (aos_queue_write_msg(queue, msg, NON_BLOCK) < 0) {
+ LOG(ERROR, "writing '%s' (line %hhd) failed\n", msg->data, msg->line);
+ aos_queue_free_msg(queue, msg);
+ }
+}
+
+const DriverStationDisplay *DriverStationDisplay::GetNext() {
+ GetQueue();
+ return static_cast<const DriverStationDisplay *>(aos_queue_read_msg(queue, NON_BLOCK));
+}
+void DriverStationDisplay::Free(const DriverStationDisplay *msg) {
+ GetQueue();
+ aos_queue_free_msg(queue, msg);
+}
+
diff --git a/aos/atom_code/messages/DriverStationDisplay.h b/aos/atom_code/messages/DriverStationDisplay.h
new file mode 100644
index 0000000..9ce3712
--- /dev/null
+++ b/aos/atom_code/messages/DriverStationDisplay.h
@@ -0,0 +1,31 @@
+#ifndef AOS_ATOM_CODE_MESSAGES_DRIVER_STATION_DISPLAY_H_
+#define AOS_ATOM_CODE_MESSAGES_DRIVER_STATION_DISPLAY_H_
+
+#include <stdint.h>
+#include <string.h>
+
+#include "aos/aos_core.h"
+#include "aos/common/type_traits.h"
+
+namespace aos {
+const size_t kLineLength = 21;
+
+struct DriverStationDisplay {
+ static void Send(int line, const char *fmt, ...)
+ __attribute__((format(printf, 2, 3)));
+ static const DriverStationDisplay *GetNext(); // returns NULL if there are no more
+ static void Free(const DriverStationDisplay *msg);
+
+ uint8_t line;
+ char data[kLineLength + 1];
+
+ private:
+ static void GetQueue();
+ static aos_queue *queue;
+};
+static_assert(shm_ok<DriverStationDisplay>::value,
+ "DriverStationDisplay will go through shared memory");
+} // namespace aos
+
+#endif
+
diff --git a/aos/atom_code/messages/messages.gyp b/aos/atom_code/messages/messages.gyp
new file mode 100644
index 0000000..ddbe9ec
--- /dev/null
+++ b/aos/atom_code/messages/messages.gyp
@@ -0,0 +1,38 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'messages_so',
+ 'type': 'shared_library',
+ 'variables': {'no_rsync': 1},
+ 'sources': [
+ 'DriverStationDisplay.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:aos_shared_lib',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:aos_shared_lib',
+ ],
+ 'direct_dependent_settings': {
+ 'variables': {
+ 'jni_libs': [
+ 'messages_so',
+ ],
+ },
+ },
+ },
+ {
+ 'target_name': 'messages',
+ 'type': 'static_library',
+ 'sources': [
+ 'DriverStationDisplay.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib'
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib'
+ ],
+ },
+ ],
+}
diff --git a/aos/atom_code/output/HTTPServer.cpp b/aos/atom_code/output/HTTPServer.cpp
new file mode 100644
index 0000000..88e3c6f
--- /dev/null
+++ b/aos/atom_code/output/HTTPServer.cpp
@@ -0,0 +1,153 @@
+#include "aos/atom_code/output/HTTPServer.h"
+
+#include <inttypes.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+#include <memory>
+
+#include "event2/event.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/scoped_fd.h"
+#include "aos/common/unique_malloc_ptr.h"
+
+namespace aos {
+namespace http {
+
+HTTPServer::HTTPServer(const char *directory, uint16_t port) :
+ directory_(directory), base_(event_base_new()), http_(evhttp_new(base_)) {
+ if (base_ == NULL) {
+ LOG(FATAL, "couldn't create an event_base\n");
+ }
+ if (http_ == NULL) {
+ LOG(FATAL, "couldn't create an evhttp\n");
+ }
+ if (evhttp_bind_socket(http_, "0.0.0.0", port) != 0) {
+ LOG(FATAL, "evhttp_bind_socket(%p, \"0.0.0.0\", %"PRIu16") failed\n",
+ http_, port);
+ }
+ evhttp_set_gencb(http_, StaticServeFile, this);
+}
+
+void HTTPServer::AddPage(const std::string &path,
+ void (*handler)(evhttp_request *, void *), void *data) {
+ switch (evhttp_set_cb(http_, path.c_str(), handler, data)) {
+ case 0:
+ LOG(DEBUG, "set callback handler for '%s'\n", path.c_str());
+ break;
+ case -1:
+ LOG(INFO, "changed callback handler for '%s'\n", path.c_str());
+ break;
+ default:
+ LOG(WARNING, "evhttp_set_cb(%p, %s, %p, %p) failed\n", http_, path.c_str(),
+ handler, data);
+ break;
+ }
+}
+
+void HTTPServer::AddStandardHeaders(evhttp_request *request) {
+ if (evhttp_add_header(evhttp_request_get_output_headers(request),
+ "Server", "aos::HTTPServer/0.0") == -1) {
+ LOG(WARNING, "adding Server header failed\n");
+ }
+}
+
+namespace {
+// All of these functions return false, NULL, or -1 if they fail (and send back
+// an error).
+
+// Returns the path of the file that is being requested.
+const char *GetPath(evhttp_request *request) {
+ // Docs are unclear whether this needs freeing, but it looks like it just
+ // returns an internal field of the request.
+ // Running valgrind with no freeing of uri or path doesn't report anything
+ // related to this code.
+ const evhttp_uri *uri = evhttp_request_get_evhttp_uri(request);
+ const char *path = evhttp_uri_get_path(uri);
+ if (path == NULL) {
+ evhttp_send_error(request, HTTP_BADREQUEST, "need a path");
+ return NULL;
+ }
+ if (strstr(path, "..") != NULL) {
+ evhttp_send_error(request, HTTP_NOTFOUND, "no .. allowed!!");
+ return NULL;
+ }
+ return path;
+}
+// Returns an fd open for reading for the file at "directory/path".
+int OpenFile(evhttp_request *request, const char *path,
+ const char *directory) {
+ char *temp;
+ if (asprintf(&temp, "%s/%s", directory, path) == -1) {
+ LOG(WARNING, "asprintf(%p, \"%%s/%%s\", %p, %p) failed with %d: %s\n",
+ &temp, directory, path, errno, strerror(errno));
+ evhttp_send_error(request, HTTP_INTERNAL, NULL);
+ return -1;
+ }
+ const unique_c_ptr<char> filename(temp);
+ ScopedFD file(open(filename.get(), O_RDONLY));
+ if (!file) {
+ if (errno == ENOENT) {
+ evhttp_send_error(request, HTTP_NOTFOUND, NULL);
+ return -1;
+ }
+ LOG(ERROR, "open('%s', 0) failed with %d: %s\n", filename.get(),
+ errno, strerror(errno));
+ evhttp_send_error(request, HTTP_INTERNAL, NULL);
+ return -1;
+ }
+ return file.release();
+}
+// Returns the size of the file specified by the given fd.
+off_t GetSize(int file) {
+ struct stat info;
+ if (fstat(file, &info) == -1) {
+ LOG(ERROR, "stat(%d, %p) failed with %d: %s\n", file, &info,
+ errno, strerror(errno));
+ return -1;
+ }
+ return info.st_size;
+}
+bool SendFileResponse(evhttp_request *request, int file_num) {
+ ScopedFD file(file_num);
+ const off_t size = GetSize(file.get());
+ if (size == -1) {
+ evhttp_send_error(request, HTTP_INTERNAL, NULL);
+ return false;
+ }
+ evbuffer *const buf = evhttp_request_get_output_buffer(request);
+ if (evbuffer_add_file(buf, file.get(), 0, size) == -1) {
+ LOG(WARNING, "evbuffer_add_file(%p, %d, 0, %jd) failed\n", buf,
+ file.get(), static_cast<intmax_t>(size));
+ evhttp_send_error(request, HTTP_INTERNAL, NULL);
+ return false;
+ } else {
+ // it succeeded, so evhttp takes ownership
+ file.release();
+ }
+ evhttp_send_reply(request, HTTP_OK, NULL, NULL);
+ return true;
+}
+
+} // namespace
+void HTTPServer::ServeFile(evhttp_request *request) {
+ AddStandardHeaders(request);
+
+ const char *path = GetPath(request);
+ if (path == NULL) return;
+
+ ScopedFD file(OpenFile(request, path, directory_));
+ if (!file) return;
+
+ if (!SendFileResponse(request, file.release())) return;
+}
+
+void HTTPServer::Run() {
+ event_base_dispatch(base_);
+ LOG(FATAL, "event_base_dispatch returned\n");
+}
+
+} // namespace http
+} // namespace aos
diff --git a/aos/atom_code/output/HTTPServer.h b/aos/atom_code/output/HTTPServer.h
new file mode 100644
index 0000000..99eb295
--- /dev/null
+++ b/aos/atom_code/output/HTTPServer.h
@@ -0,0 +1,58 @@
+#include "event2/buffer.h"
+#include "event2/http.h"
+
+#include <string>
+
+namespace aos {
+namespace http {
+
+// An HTTP server that serves files from a directory using libevent.
+// Also allows configuring certain URLs to be dynamically generated.
+class HTTPServer {
+ public:
+ HTTPServer(const char *directory, uint16_t port);
+ // Starts serving pages.
+ // Might not clean up everything before returning.
+ void Run();
+ protected:
+ template<class T> class MemberHandler {
+ public:
+ typedef void (T::*Handler)(evhttp_request *);
+ struct Holder {
+ T *self;
+ Handler handler;
+ };
+ static void Call(evhttp_request *request, void *handler_in) {
+ const Holder *const holder = static_cast<Holder *>(handler_in);
+ AddStandardHeaders(request);
+ ((holder->self)->*(holder->handler))(request);
+ }
+ };
+ void AddPage(const std::string &path, void (*handler)(evhttp_request *, void *),
+ void *data);
+ template<class T> void AddPage(const std::string &path,
+ typename MemberHandler<T>::Handler handler,
+ T *self) {
+ // have to put "typename" in, so the typedef makes it clearer
+ typedef typename MemberHandler<T>::Holder HolderType;
+ AddPage(path, MemberHandler<T>::Call, new HolderType{self, handler});
+ }
+ // This gets set up as the generic handler.
+ // It can also be called separately to serve the file that the request is
+ // requesting from the filesystem.
+ void ServeFile(evhttp_request *request);
+ private:
+ // The directory where files to be served come from.
+ const char *directory_;
+ // The main libevent structure.
+ event_base *const base_;
+ // The libevent HTTP server handle.
+ evhttp *const http_;
+ static void AddStandardHeaders(evhttp_request *request);
+ static void StaticServeFile(evhttp_request *request, void *self) {
+ static_cast<HTTPServer *>(self)->ServeFile(request);
+ }
+};
+
+} // namespace http
+} // namespace aos
diff --git a/aos/atom_code/output/MotorOutput.cpp b/aos/atom_code/output/MotorOutput.cpp
new file mode 100644
index 0000000..ecbdf8b
--- /dev/null
+++ b/aos/atom_code/output/MotorOutput.cpp
@@ -0,0 +1,78 @@
+#include "aos/atom_code/output/MotorOutput.h"
+
+#include <math.h>
+
+#include "aos/common/Configuration.h"
+#include "aos/aos_core.h"
+#include "aos/common/byteorder.h"
+#include "aos/common/control_loop/Timing.h"
+#include "aos/atom_code/messages/DriverStationDisplay.h"
+
+namespace aos {
+
+MotorOutput::MotorOutput() : sock(NetworkPort::kMotors,
+ configuration::GetIPAddress(configuration::NetworkDevice::kCRIO)) {}
+
+void MotorOutput::Run() {
+ while (true) {
+ time::PhasedLoopXMS(5, 1000);
+ RunIteration();
+
+ // doesn't matter if multiple instances end up running this loop at the same
+ // time because the queue handles the race conditions
+ while (true) {
+ const aos::DriverStationDisplay *line = aos::DriverStationDisplay::GetNext();
+ if (line != NULL) {
+ AddDSLine(line->line, line->data);
+ LOG(DEBUG, "got a line %hhd that said '%s'\n", line->line, line->data);
+ aos::DriverStationDisplay::Free(line);
+ } else {
+ break;
+ }
+ }
+
+ if (sock.SendHoldMsg() == -1) {
+ LOG(WARNING, "sending outputs failed\n");
+ continue;
+ } else {
+ LOG(DEBUG, "sent outputs\n");
+ }
+ }
+}
+
+int MotorOutput::AddMotor(char type, uint8_t num, float value) {
+ if (sock.hold_msg_len_ + 4 > sock.MAX_MSG) {
+ return -1;
+ }
+ sock.hold_msg_[sock.hold_msg_len_ ++] = type;
+ sock.hold_msg_[sock.hold_msg_len_ ++] = num;
+ to_network(&value, &sock.hold_msg_[sock.hold_msg_len_]);
+ sock.hold_msg_len_ += 4;
+ return 0;
+}
+int MotorOutput::AddSolenoid(uint8_t port, bool on) {
+ if (sock.hold_msg_len_ + 3 > sock.MAX_MSG) {
+ return -1;
+ }
+ sock.hold_msg_[sock.hold_msg_len_ ++] = 's';
+ sock.hold_msg_[sock.hold_msg_len_ ++] = port;
+ sock.hold_msg_[sock.hold_msg_len_ ++] = on ? 1 : 0;
+ return 0;
+}
+
+int MotorOutput::AddDSLine(uint8_t line, const char *data) {
+ size_t data_len = strlen(data); // doesn't include terminating NULL
+ if (sock.hold_msg_len_ + 3 + data_len > sock.MAX_MSG) {
+ return -1;
+ }
+
+ sock.hold_msg_[sock.hold_msg_len_ ++] = 'd';
+ sock.hold_msg_[sock.hold_msg_len_ ++] = line;
+ sock.hold_msg_[sock.hold_msg_len_ ++] = data_len;
+ memcpy(&sock.hold_msg_[sock.hold_msg_len_], data, data_len);
+ sock.hold_msg_len_ += data_len;
+ return 0;
+}
+
+} // namespace aos
+
diff --git a/aos/atom_code/output/MotorOutput.h b/aos/atom_code/output/MotorOutput.h
new file mode 100644
index 0000000..925139a
--- /dev/null
+++ b/aos/atom_code/output/MotorOutput.h
@@ -0,0 +1,74 @@
+#ifndef AOS_ATOM_CODE_OUTPUT_MOTOR_OUTPUT_H_
+#define AOS_ATOM_CODE_OUTPUT_MOTOR_OUTPUT_H_
+
+#include <stdint.h>
+#include <string.h>
+#include <algorithm>
+#include <string>
+
+#include "aos/common/network/SendSocket.h"
+#include "aos/common/byteorder.h"
+#include "aos/common/type_traits.h"
+
+namespace aos {
+
+class MotorOutput {
+ public:
+ MotorOutput();
+ void Run();
+
+ // Constants for the first argument of AddMotor.
+ static const char VICTOR = 'v';
+ static const char TALON = 't';
+
+ protected:
+ // num is 1-indexed
+ int AddMotor(char type, uint8_t num, float value);
+ int AddSolenoid(uint8_t num, bool on);
+ int AddDSLine(uint8_t line, const char *data);
+ // loop_queuegroup is expected to be a control loop queue group.
+ // This function will fetch the latest goal and serve it.
+ template <class T>
+ int AddControlLoopGoal(T *loop_queuegroup);
+
+ virtual void RunIteration() = 0;
+
+ private:
+ SendSocket sock;
+};
+
+template <class T>
+int MotorOutput::AddControlLoopGoal(T *loop_queuegroup) {
+ typedef typename std::remove_reference<
+ decltype(*(loop_queuegroup->goal.MakeMessage().get()))>::type GoalType;
+ // TODO(aschuh): This assumes a static serialized message size.
+ const uint16_t size = GoalType::Size();
+ if (sock.hold_msg_len_ + 4 + 1 + size > sock.MAX_MSG) {
+ return -1;
+ }
+
+ const bool zero = !loop_queuegroup->goal.FetchLatest();
+
+ sock.hold_msg_[sock.hold_msg_len_ ++] = 'g';
+ const uint32_t hash = loop_queuegroup->hash();
+
+ // Place the loop hash at the front.
+ to_network(&hash, &sock.hold_msg_[sock.hold_msg_len_]);
+ sock.hold_msg_len_ += 4;
+
+ if (zero) {
+ GoalType zero_message;
+ zero_message.Zero();
+ zero_message.Serialize(&sock.hold_msg_[sock.hold_msg_len_]);
+ sock.hold_msg_len_ += size;
+ return -1;
+ } else {
+ loop_queuegroup->goal->Serialize(&sock.hold_msg_[sock.hold_msg_len_]);
+ sock.hold_msg_len_ += size;
+ return 0;
+ }
+}
+
+} // namespace aos
+
+#endif
diff --git a/aos/atom_code/output/evhttp_ctemplate_emitter.cc b/aos/atom_code/output/evhttp_ctemplate_emitter.cc
new file mode 100644
index 0000000..ed2a83f
--- /dev/null
+++ b/aos/atom_code/output/evhttp_ctemplate_emitter.cc
@@ -0,0 +1,18 @@
+#include "aos/atom_code/output/evhttp_ctemplate_emitter.h"
+
+#include "aos/aos_core.h"
+
+namespace aos {
+namespace http {
+
+void EvhttpCtemplateEmitter::Emit(const char *s, size_t slen) {
+ if (error_) return;
+ if (evbuffer_add(buf_, s, slen) != 0) {
+ LOG(ERROR, "evbuffer_add(%p, %p, %zd) failed\n",
+ buf_, s, slen);
+ error_ = true;
+ }
+}
+
+} // namespace http
+} // namespace aos
diff --git a/aos/atom_code/output/evhttp_ctemplate_emitter.h b/aos/atom_code/output/evhttp_ctemplate_emitter.h
new file mode 100644
index 0000000..d20d2cb
--- /dev/null
+++ b/aos/atom_code/output/evhttp_ctemplate_emitter.h
@@ -0,0 +1,34 @@
+#ifndef AOS_ATOM_CODE_OUTPUT_EVHTTP_CTEMPLATE_EMITTER_H_
+#define AOS_ATOM_CODE_OUTPUT_EVHTTP_CTEMPLATE_EMITTER_H_
+
+#include <string.h>
+
+#include "event2/buffer.h"
+#include "ctemplate/template_emitter.h"
+
+namespace aos {
+namespace http {
+
+// Writes everything directly into an evbuffer*.
+// Handles errors by refusing to write anything else into the buffer and storing
+// the state (which can be retrieved with error()).
+class EvhttpCtemplateEmitter : public ctemplate::ExpandEmitter {
+ public:
+ EvhttpCtemplateEmitter(evbuffer *buf) : buf_(buf), error_(false) {}
+ virtual void Emit(char c) { Emit(&c, 1); };
+ virtual void Emit(const std::string& s) { Emit(s.data(), s.size()); };
+ virtual void Emit(const char* s) { Emit(s, strlen(s)); }
+ virtual void Emit(const char* s, size_t slen);
+ // Retrieves whether or not there has been an error. If true, the error will
+ // already have been logged.
+ bool error() { return error_; }
+
+ private:
+ evbuffer *const buf_;
+ bool error_;
+};
+
+} // namespace http
+} // namespace aos
+
+#endif // AOS_ATOM_CODE_OUTPUT_EVHTTP_CTEMPLATE_EMITTER_H_
diff --git a/aos/atom_code/output/output.gyp b/aos/atom_code/output/output.gyp
new file mode 100644
index 0000000..3e4abd4
--- /dev/null
+++ b/aos/atom_code/output/output.gyp
@@ -0,0 +1,35 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'http_server',
+ 'type': 'static_library',
+ 'sources': [
+ 'HTTPServer.cpp',
+ 'evhttp_ctemplate_emitter.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(EXTERNALS):libevent',
+ '<(EXTERNALS):ctemplate',
+ ],
+ 'export_dependent_settings': [
+# Our headers #include headers from both of these.
+ '<(EXTERNALS):libevent',
+ '<(EXTERNALS):ctemplate',
+ ],
+ },
+ {
+ 'target_name': 'motor_output',
+ 'type': 'static_library',
+ 'sources': [
+ 'MotorOutput.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ },
+ ],
+}
diff --git a/aos/atom_code/queue-tmpl.h b/aos/atom_code/queue-tmpl.h
new file mode 100644
index 0000000..2c6e4e4
--- /dev/null
+++ b/aos/atom_code/queue-tmpl.h
@@ -0,0 +1,263 @@
+namespace aos {
+
+template <class T>
+bool ScopedMessagePtr<T>::Send() {
+ assert(msg_ != NULL);
+ msg_->SetTimeToNow();
+ assert(queue_ != NULL);
+ bool return_value = aos_queue_write_msg_free(queue_, msg_, OVERRIDE) == 0;
+ msg_ = NULL;
+ return return_value;
+}
+
+template <class T>
+bool ScopedMessagePtr<T>::SendBlocking() {
+ assert(msg_ != NULL);
+ msg_->SetTimeToNow();
+ assert(queue_ != NULL);
+ bool return_value = aos_queue_write_msg_free(queue_, msg_, BLOCK) == 0;
+ msg_ = NULL;
+ return return_value;
+}
+
+template <class T>
+void ScopedMessagePtr<T>::reset(T *msg) {
+ if (queue_ != NULL && msg_ != NULL) {
+ aos_queue_free_msg(queue_, msg_);
+ }
+ msg_ = msg;
+}
+
+// A SafeScopedMessagePtr<> manages a message pointer.
+// It frees it properly when the ScopedMessagePtr<> goes out of scope or gets
+// sent. By design, there is no way to get the ScopedMessagePtr to release the
+// message pointer. When the message gets sent, it allocates a queue message,
+// copies the data into it, and then sends it. Copies copy the message.
+template <class T>
+class SafeScopedMessagePtr {
+ public:
+ // Returns a pointer to the message.
+ // This stays valid until Send or the destructor have been called.
+ T *get() { return msg_; }
+
+ T &operator*() {
+ T *msg = get();
+ assert(msg != NULL);
+ return *msg;
+ }
+
+ T *operator->() {
+ T *msg = get();
+ assert(msg != NULL);
+ return msg;
+ }
+
+#ifndef SWIG
+ operator bool() {
+ return msg_ != NULL;
+ }
+
+ const T *get() const { return msg_; }
+
+ const T &operator*() const {
+ const T *msg = get();
+ assert(msg != NULL);
+ return *msg;
+ }
+
+ const T *operator->() const {
+ const T *msg = get();
+ assert(msg != NULL);
+ return msg;
+ }
+#endif // SWIG
+
+ // Sends the message and removes our reference to it.
+ // If the queue is full, over-rides the oldest message in it with our new
+ // message.
+ // Returns true on success, and false otherwise.
+ // The message will be freed.
+ bool Send() {
+ assert(msg_ != NULL);
+ assert(queue_ != NULL);
+ msg_->SetTimeToNow();
+ T *shm_msg = static_cast<T *>(aos_queue_get_msg(queue_));
+ if (shm_msg == NULL) {
+ return false;
+ }
+ *shm_msg = *msg_;
+ bool return_value =
+ aos_queue_write_msg_free(queue_, shm_msg, OVERRIDE) == 0;
+ reset();
+ return return_value;
+ }
+
+ // Sends the message and removes our reference to it.
+ // If the queue is full, blocks until it is no longer full.
+ // Returns true on success, and false otherwise.
+ // Frees the message.
+ bool SendBlocking() {
+ assert(msg_ != NULL);
+ assert(queue_ != NULL);
+ msg_->SetTimeToNow();
+ T *shm_msg = static_cast<T *>(aos_queue_get_msg(queue_));
+ if (shm_msg == NULL) {
+ return false;
+ }
+ *shm_msg = *msg_;
+ bool return_value = aos_queue_write_msg_free(queue_, shm_msg, BLOCK) == 0;
+ reset();
+ return return_value;
+ }
+
+ // Frees the contained message.
+ ~SafeScopedMessagePtr() {
+ reset();
+ }
+
+#ifndef SWIG
+ // Implements a move constructor to take the message pointer from the
+ // temporary object to save work.
+ SafeScopedMessagePtr(SafeScopedMessagePtr<T> &&ptr)
+ : queue_(ptr.queue_),
+ msg_(ptr.msg_) {
+ ptr.msg_ = NULL;
+ }
+#endif // SWIG
+
+ // Copy constructor actually copies the data.
+ SafeScopedMessagePtr(const SafeScopedMessagePtr<T> &ptr)
+ : queue_(ptr.queue_),
+ msg_(NULL) {
+ reset(new T(*ptr.get()));
+ }
+#ifndef SWIG
+ // Equal operator copies the data.
+ void operator=(const SafeScopedMessagePtr<T> &ptr) {
+ queue_ = ptr.queue_;
+ reset(new T(*ptr.get()));
+ }
+#endif // SWIG
+
+ private:
+ // Provide access to private constructor.
+ friend class aos::Queue<typename std::remove_const<T>::type>;
+ friend class aos::SafeMessageBuilder<T>;
+
+ // Only Queue should be able to build a message pointer.
+ SafeScopedMessagePtr(aos_queue *queue)
+ : queue_(queue), msg_(new T()) {}
+
+ // Sets the pointer to msg, freeing the old value if it was there.
+ // This is private because nobody should be able to get a pointer to a message
+ // that needs to be scoped without using the queue.
+ void reset(T *msg = NULL) {
+ if (msg_) {
+ delete msg_;
+ }
+ msg_ = msg;
+ }
+
+ // Sets the queue that owns this message.
+ void set_queue(aos_queue *queue) { queue_ = queue; }
+
+ // The queue that the message is a part of.
+ aos_queue *queue_;
+ // The message or NULL.
+ T *msg_;
+};
+
+template <class T>
+void Queue<T>::Init() {
+ if (queue_ == NULL) {
+ // Signature of the message.
+ aos_type_sig kQueueSignature{sizeof(T), T::kHash, T::kQueueLength};
+
+ queue_ = aos_fetch_queue(queue_name_, &kQueueSignature);
+ queue_msg_.set_queue(queue_);
+ }
+}
+
+template <class T>
+bool Queue<T>::FetchNext() {
+ Init();
+ // TODO(aschuh): Use aos_queue_read_msg_index so that multiple readers
+ // reading don't randomly get only part of the messages.
+ // Document here the tradoffs that are part of each method.
+ const T *msg = static_cast<const T *>(aos_queue_read_msg(queue_,
+ NON_BLOCK));
+ // Only update the internal pointer if we got a new message.
+ if (msg != NULL) {
+ queue_msg_.reset(msg);
+ }
+ return msg != NULL;
+}
+
+template <class T>
+bool Queue<T>::FetchNextBlocking() {
+ Init();
+ const T *msg = static_cast<const T *>(aos_queue_read_msg(queue_, BLOCK));
+ queue_msg_.reset(msg);
+ assert (msg != NULL);
+ return true;
+}
+
+template <class T>
+bool Queue<T>::FetchLatest() {
+ Init();
+ const T *msg = static_cast<const T *>(aos_queue_read_msg(queue_,
+ FROM_END | NON_BLOCK | PEEK));
+ // Only update the internal pointer if we got a new message.
+ if (msg != NULL && msg != queue_msg_.get()) {
+ queue_msg_.reset(msg);
+ return true;
+ }
+ // The message has to get freed if we didn't use it (and aos_queue_free_msg is
+ // ok to call on NULL).
+ aos_queue_free_msg(queue_, msg);
+ return false;
+}
+
+template <class T>
+SafeScopedMessagePtr<T> Queue<T>::SafeMakeMessage() {
+ Init();
+ SafeScopedMessagePtr<T> safe_msg(queue_);
+ safe_msg->Zero();
+ return safe_msg;
+}
+
+template <class T>
+ScopedMessagePtr<T> Queue<T>::MakeMessage() {
+ Init();
+ return ScopedMessagePtr<T>(queue_, MakeRawMessage());
+}
+
+template <class T>
+T *Queue<T>::MakeRawMessage() {
+ return static_cast<T *>(aos_queue_get_msg(queue_));
+}
+
+template <class T>
+aos::MessageBuilder<T> Queue<T>::MakeWithBuilder() {
+ Init();
+ return aos::MessageBuilder<T>(queue_, MakeRawMessage());
+}
+
+
+// This builder uses the safe message pointer so that it can be safely copied
+// and used by SWIG or in places where it could be leaked.
+template <class T>
+class SafeMessageBuilder {
+ public:
+ typedef T Message;
+ bool Send();
+};
+
+template <class T>
+aos::SafeMessageBuilder<T> Queue<T>::SafeMakeWithBuilder() {
+ Init();
+ return aos::SafeMessageBuilder<T>(queue_);
+}
+
+
+} // namespace aos
diff --git a/aos/atom_code/starter/starter.cpp b/aos/atom_code/starter/starter.cpp
new file mode 100644
index 0000000..0cfddd5
--- /dev/null
+++ b/aos/atom_code/starter/starter.cpp
@@ -0,0 +1,354 @@
+#include "aos/atom_code/starter/starter.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/signalfd.h>
+#include <sys/types.h>
+#include <fcntl.h>
+#include <sys/inotify.h>
+#include <sys/stat.h>
+
+#include "aos/aos_core.h"
+
+void niceexit(int status);
+
+pid_t start(const char *cmd, uint8_t times) {
+ char *which_cmd, *which_cmd_stm;
+ if (asprintf(&which_cmd, "which %s", cmd) == -1) {
+ LOG_IFINIT(ERROR, "creating \"which %s\" failed with %d: %s\n",
+ cmd, errno, strerror(errno));
+ niceexit(EXIT_FAILURE);
+ }
+ if (asprintf(&which_cmd_stm, "which %s.stm", cmd) == -1) {
+ LOG_IFINIT(ERROR, "creating \"which %s.stm\" failed with %d: %s\n",
+ cmd, errno, strerror(errno));
+ niceexit(EXIT_FAILURE);
+ }
+ FILE *which = popen(which_cmd, "r");
+ char exe[CMDLEN + 5], orig_exe[CMDLEN];
+ size_t ret;
+ if ((ret = fread(orig_exe, 1, sizeof(orig_exe), which)) == CMDLEN) {
+ LOG_IFINIT(ERROR, "`which %s` was too long. not starting '%s'\n", cmd, cmd);
+ return 0;
+ }
+ orig_exe[ret] = '\0';
+ if (pclose(which) == -1) {
+ LOG_IFINIT(WARNING, "pclose failed with %d: %s\n", errno, strerror(errno));
+ }
+ free(which_cmd);
+ if (strlen(orig_exe) == 0) { // which returned nothing; check if stm exists
+ LOG_IFINIT(INFO, "%s didn't exist. trying %s.stm\n", cmd, cmd);
+ FILE *which_stm = popen(which_cmd_stm, "r");
+ if ((ret = fread(orig_exe, 1, sizeof(orig_exe), which_stm)) == CMDLEN) {
+ LOG_IFINIT(ERROR, "`which %s.stm` was too long. not starting %s\n", cmd, cmd);
+ return 0;
+ }
+ orig_exe[ret] = '\0';
+ if (pclose(which) == -1) {
+ LOG_IFINIT(WARNING, "pclose failed with %d: %s\n", errno, strerror(errno));
+ }
+ }
+ if (strlen(orig_exe) == 0) {
+ LOG_IFINIT(WARNING, "couldn't find file '%s[.stm]'. not going to start it\n",
+ cmd);
+ return 0;
+ }
+ if (orig_exe[strlen(orig_exe) - 1] != '\n') {
+ LOG_IFINIT(WARNING, "no \\n on the end of `which %s[.stm]` output ('%s')\n",
+ cmd, orig_exe);
+ } else {
+ orig_exe[strlen(orig_exe) - 1] = '\0'; // get rid of the \n
+ }
+ strncpy(exe, orig_exe, CMDLEN);
+
+ strcat(exe, ".stm");
+ struct stat st;
+ errno = 0;
+ if (stat(orig_exe, &st) != 0 && errno != ENOENT) {
+ LOG_IFINIT(ERROR, "killing everything because stat('%s') failed with %d: %s\n",
+ orig_exe, errno, strerror(errno));
+ niceexit(EXIT_FAILURE);
+ } else if (errno == ENOENT) {
+ LOG_IFINIT(WARNING, "binary '%s' doesn't exist. not starting it\n", orig_exe);
+ return 0;
+ }
+ struct stat st2;
+ // if we can confirm it's already 0 size
+ bool orig_zero = stat(orig_exe, &st2) == 0 && st2.st_size == 0;
+ if (!orig_zero) {
+ // if it failed and it wasn't because it was missing
+ if (unlink(exe) != 0 && (errno != EROFS && errno != ENOENT)) {
+ LOG_IFINIT(ERROR,
+ "killing everything because unlink('%s') failed with %d: %s\n",
+ exe, errno, strerror(errno));
+ niceexit(EXIT_FAILURE);
+ }
+ if (link(orig_exe, exe) != 0) {
+ LOG_IFINIT(ERROR,
+ "killing everything because link('%s', '%s') failed with %d: %s\n",
+ orig_exe, exe, errno, strerror(errno));
+ niceexit(EXIT_FAILURE);
+ }
+ }
+ if (errno == EEXIST) {
+ LOG_IFINIT(INFO, "exe ('%s') already existed\n", exe);
+ }
+
+ pid_t child;
+ if ((child = fork()) == 0) {
+ execlp(exe, orig_exe, static_cast<char *>(NULL));
+ LOG_IFINIT(ERROR,
+ "killing everything because execlp('%s', '%s', NULL) "
+ "failed with %d: %s\n",
+ exe, cmd, errno, strerror(errno));
+ _exit(EXIT_FAILURE); // don't niceexit or anything because this is the child!!
+ }
+ if (child == -1) {
+ LOG_IFINIT(WARNING, "fork on '%s' failed with %d: %s",
+ cmd, errno, strerror(errno));
+ if (times < 100) {
+ return start(cmd, times + 1);
+ } else {
+ LOG_IFINIT(ERROR, "tried to start '%s' too many times. giving up\n", cmd);
+ return 0;
+ }
+ } else {
+ children[child] = cmd;
+ files[child] = orig_exe;
+ int ret = inotify_add_watch(notifyfd, orig_exe, IN_ATTRIB | IN_MODIFY);
+ if (ret < 0) {
+ LOG_IFINIT(WARNING, "inotify_add_watch('%s') failed: "
+ "not going to watch for changes to it because of %d: %s\n",
+ orig_exe, errno, strerror(errno));
+ } else {
+ watches[ret] = child;
+ mtimes[ret] = st2.st_mtime;
+ }
+ return child;
+ }
+}
+
+static bool exited = false;
+void exit_handler() {
+ if(exited) {
+ return;
+ } else {
+ exited = true;
+ }
+ fputs("starter: killing all children for exit\n", stdout);
+ for (auto it = children.begin(); it != children.end(); ++it) {
+ printf("starter: killing child %d ('%s') for exit\n", it->first, it->second);
+ kill(it->first, SIGKILL);
+ }
+ if (sigfd != 0) {
+ close(sigfd);
+ }
+ if (notifyfd != 0) {
+ close(notifyfd);
+ }
+}
+void niceexit(int status) {
+ printf("starter: niceexit(%d) EXIT_SUCCESS=%d EXIT_FAILURE=%d\n",
+ status, EXIT_SUCCESS, EXIT_FAILURE);
+ exit_handler();
+ exit(status);
+}
+
+int main(int argc, char *argv[]) {
+ if (argc < 2) {
+ fputs("starter: error: need an argument specifying what file to use\n", stderr);
+ niceexit(EXIT_FAILURE);
+ } else if(argc > 2) {
+ fputs("starter: warning: too many arguments\n", stderr);
+ }
+
+ atexit(exit_handler);
+
+ notifyfd = inotify_init1(IN_NONBLOCK);
+
+ pid_t core = start("core", 0);
+ if (core == 0) {
+ fputs("starter: error: core didn't exist\n", stderr);
+ niceexit(EXIT_FAILURE);
+ }
+ fprintf(stderr, "starter: info: core's pid is %jd\n", static_cast<intmax_t>(core));
+ FILE *pid_file = fopen("/tmp/starter.pid", "w");
+ if (pid_file == NULL) {
+ perror("fopen(/tmp/starter.pid)");
+ } else {
+ if (fprintf(pid_file, "%d", core) == -1) {
+ fprintf(stderr, "starter: error: fprintf(pid_file, core(=%d)) failed "
+ "with %d: %s",
+ core, errno, strerror(errno));
+ }
+ fclose(pid_file);
+ }
+ sleep(1);
+ if (kill(core, 0) != 0) {
+ fprintf(stderr, "starter: couldn't kill(%jd(=core), 0) because of %d: %s\n",
+ static_cast<intmax_t>(core), errno, strerror(errno));
+ niceexit(EXIT_FAILURE);
+ }
+ fputs("starter: before init\n", stdout);
+ aos::InitNRT();
+ fputs("starter: after init\n", stdout);
+
+ FILE *list = fopen(argv[1], "re");
+ char line[CMDLEN + 1];
+ char *line_copy;
+ uint8_t too_long = 0;
+ while (fgets(line, sizeof(line), list) != NULL) {
+ if (line[strlen(line) - 1] != '\n') {
+ LOG(WARNING, "command segment '%s' is too long. "
+ "increase the size of the line char[] above " __FILE__ ": %d\n",
+ line, __LINE__);
+ too_long = 1;
+ continue;
+ }
+ if (too_long) {
+ too_long = 0;
+ LOG(WARNING, "\tgot last chunk of too long line: '%s'\n", line);
+ continue; // don't try running the last little chunk
+ }
+ line[strlen(line) - 1] = '\0'; // get rid of the \n
+ line_copy = new char[strlen(line) + 1];
+ memcpy(line_copy, line, strlen(line) + 1);
+ fprintf(stderr, "starter: info: going to start \"%s\"\n", line_copy);
+ start(line_copy, 0);
+ }
+ fclose(list);
+ LOG(INFO, "started everything\n");
+
+ sigset_t mask;
+ sigemptyset (&mask);
+ sigaddset (&mask, SIGCHLD);
+ sigprocmask (SIG_BLOCK, &mask, NULL);
+ sigfd = signalfd (-1, &mask, O_NONBLOCK);
+
+ fd_set readfds;
+ FD_ZERO(&readfds);
+ siginfo_t infop;
+ signalfd_siginfo fdsi;
+ inotify_event notifyevt;
+ int ret;
+ while (1) {
+ FD_SET(sigfd, &readfds);
+ FD_SET(notifyfd, &readfds);
+ timeval timeout;
+ timeout.tv_sec = restarts.empty() ? 2 : 0;
+ timeout.tv_usec = 100000;
+ ret = select (FD_SETSIZE, &readfds, NULL, NULL, &timeout);
+
+ if (ret == 0) { // timeout
+ auto it = restarts.begin();
+ // WARNING because the message about it dying will be
+ for (; it != restarts.end(); it++) {
+ LOG(WARNING, "restarting process %d ('%s') by giving it a SIGKILL(%d)\n",
+ *it, children[*it], SIGKILL);
+ kill(*it, SIGKILL);
+ }
+ restarts.clear();
+ }
+
+ if (FD_ISSET(notifyfd, &readfds)) {
+ if ((ret = read(notifyfd, ¬ifyevt, sizeof(notifyevt))) ==
+ sizeof(notifyevt)) {
+ if (watches.count(notifyevt.wd)) {
+ struct stat st;
+ if (!children.count(watches[notifyevt.wd]) ||
+ stat(files[watches[notifyevt.wd]], &st) == 0) {
+ if (mtimes[notifyevt.wd] == st.st_mtime) {
+ LOG(DEBUG, "ignoring trigger of watch id %d (file '%s')"
+ " because mtime didn't change\n",
+ notifyevt.wd, files[watches[notifyevt.wd]]);
+ } else if (children.count(watches[notifyevt.wd])) {
+ LOG(DEBUG, "adding process %d to the restart list\n",
+ watches[notifyevt.wd]);
+ restarts.insert(watches[notifyevt.wd]);
+ } else {
+ LOG(DEBUG, "children doesn't have entry for PID %d\n",
+ watches[notifyevt.wd]);
+ }
+ } else {
+ LOG(ERROR, "stat('%s') failed with %d: %s\n",
+ files[watches[notifyevt.wd]], errno, strerror(errno));
+ }
+ } else {
+ LOG(WARNING, "no PID for watch id %d\n", notifyevt.wd);
+ }
+ } else {
+ if (ret == -1) {
+ LOG(WARNING, "read(notifyfd) failed with %d: %s", errno, strerror(errno));
+ } else {
+ LOG(WARNING, "couldn't get a whole inotify_event(%d) (only got %d)\n",
+ sizeof(notifyevt), ret);
+ }
+ }
+ }
+
+ if (FD_ISSET(sigfd, &readfds)) {
+ while(read (sigfd, &fdsi, sizeof fdsi) > 0);
+ }
+ while (1) {
+ infop.si_pid = 0;
+ if (waitid(P_ALL, 0, &infop, WEXITED | WSTOPPED | WNOHANG) == 0) {
+ if (infop.si_pid == 0) {
+ goto after_loop; // no more child process changes pending
+ }
+ switch (infop.si_code) {
+ case CLD_EXITED:
+ LOG(WARNING, "child %d (%s) exited with status %d\n",
+ infop.si_pid, children[infop.si_pid], infop.si_status);
+ break;
+ case CLD_DUMPED:
+ LOG(INFO, "child %d actually dumped core. "
+ "falling through to killed by signal case\n", infop.si_pid);
+ case CLD_KILLED:
+ LOG(WARNING, "child %d (%s) was killed by signal %d (%s)\n",
+ infop.si_pid, children[infop.si_pid], infop.si_status,
+ strsignal(infop.si_status));
+ break;
+ case CLD_STOPPED:
+ LOG(WARNING, "child %d (%s) was stopped by signal %d "
+ "(giving it a SIGCONT(%d))\n",
+ infop.si_pid, children[infop.si_pid], infop.si_status, SIGCONT);
+ kill(infop.si_pid, SIGCONT);
+ continue;
+ default:
+ LOG(WARNING, "something happened to child %d (%s) (killing it)\n",
+ infop.si_pid, children[infop.si_pid]);
+ kill(infop.si_pid, SIGKILL);
+ continue;
+ }
+ if (infop.si_pid == core) {
+ fprintf(stderr, "starter: si_code=%d CLD_EXITED=%d CLD_DUMPED=%d "
+ "CLD_KILLED=%d CLD_STOPPED=%d si_status=%d (sig '%s')\n",
+ infop.si_code, CLD_EXITED, CLD_DUMPED, CLD_KILLED,
+ CLD_STOPPED, infop.si_status, strsignal(infop.si_status));
+ // core has died. logging is down too
+ fputs("starter: error: core died. exiting\n", stderr);
+ niceexit(EXIT_FAILURE);
+ }
+
+ /*// remove all of the watches assigned to the pid that just died
+ for (auto it = watches.begin(); it != watches.end(); ++it) {
+ if (it->second == infop.si_pid) {
+ watches_to_ignore.insert(it->first);
+ }
+ }
+ for (auto it = watches_to_ignore.begin();
+ it != watches_to_ignore.end(); ++it) {
+ LOG(DEBUG, "watch id %d was on PID %d\n", *it, infop.si_pid);
+ watches.erase(*it);
+ }*/
+
+ start(children[infop.si_pid], 0);
+ LOG(DEBUG, "erasing %d from children\n", infop.si_pid);
+ children.erase(infop.si_pid);
+ } else {
+ LOG(WARNING, "waitid failed with %d: %s", errno, strerror(errno));
+ }
+ }
+after_loop: ;
+ }
+}
diff --git a/aos/atom_code/starter/starter.gyp b/aos/atom_code/starter/starter.gyp
new file mode 100644
index 0000000..44e4b08
--- /dev/null
+++ b/aos/atom_code/starter/starter.gyp
@@ -0,0 +1,23 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'starter_exe',
+ 'type': 'executable',
+ 'sources': [
+ 'starter.cpp',
+ ],
+ 'dependencies': [
+ '<(AOS)/build/aos.gyp:libaos',
+ ],
+ 'copies': [
+ {
+ 'destination': '<(rsync_dir)',
+ 'files': [
+ 'starter.sh',
+ 'starter_loop.sh',
+ ],
+ },
+ ],
+ },
+ ],
+}
diff --git a/aos/atom_code/starter/starter.h b/aos/atom_code/starter/starter.h
new file mode 100644
index 0000000..16bbe01
--- /dev/null
+++ b/aos/atom_code/starter/starter.h
@@ -0,0 +1,28 @@
+#ifndef __AOS_STARTER_H_
+#define __AOS_STARTER_H_
+
+#include <map>
+#include <sys/types.h>
+#include <signal.h>
+#include <stdint.h>
+#include <string>
+#include <errno.h>
+#include <string.h>
+#include <sys/wait.h>
+#include <set>
+
+using namespace std;
+
+map<pid_t, const char *> children;
+map<pid_t, const char *> files; // the names of the actual files
+map<int, pid_t> watches;
+set<pid_t> restarts;
+map<int, time_t> mtimes;
+
+int sigfd = 0;
+int notifyfd = 0;
+
+const size_t CMDLEN = 5000;
+
+#endif
+
diff --git a/aos/atom_code/starter/starter.sh b/aos/atom_code/starter/starter.sh
new file mode 100755
index 0000000..103180e
--- /dev/null
+++ b/aos/atom_code/starter/starter.sh
@@ -0,0 +1,17 @@
+#!/bin/bash
+
+#echo $$ > /var/run/`basename $0`.pid IT FORKS AFTER THIS!!!!
+insmod /home/driver/robot_code/bin/aos_module.ko
+#chrt -p 45 `pidof sshd`
+chrt -o 0 bash -c "export PATH=$PATH:/home/driver/robot_code/bin; starter_loop.sh $*" &
+#chrt -o 0 bash -c "while true; do cd /home/driver/mjpg-streamer2; ./server.sh; sleep 5; done" &
+
+# Log everything from the serial port...
+#SERIAL_LOG_FILE=$(date "/home/driver/tmp/robot_logs/serial_log.%F_%H-%M-%S")
+#chrt -o 0 bash -c "( stty -echo -echoe -echok 9600; cat > ${SERIAL_LOG_FILE} ) < /dev/ttyUSB0" &
+
+# Wireshark _everything_ we can see...
+#DUMPCAP_LOG_FILE=$(date "/home/driver/tmp/robot_logs/dumpcap.%F_%H-%M-%S")
+#DUMPCAP_STDOUT_FILE=$(date "/home/driver/tmp/robot_logs/stdout_dumpcap.%F_%H-%M-%S")
+#chrt -o 0 bash -c "dumpcap -i eth0 -w ${DUMPCAP_LOG_FILE} -f 'not port 8080 and not net 10.9.71.13' > ${DUMPCAP_STDOUT_FILE}" &
+
diff --git a/aos/atom_code/starter/starter_loop.sh b/aos/atom_code/starter/starter_loop.sh
new file mode 100755
index 0000000..b4e1d5c
--- /dev/null
+++ b/aos/atom_code/starter/starter_loop.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+for ((i=1; 1; i++)); do
+ starter_exe $* 1>/tmp/starter${i}_stdout 2>/tmp/starter${i}_stderr
+ sleep 2
+done
+
diff --git a/aos/atom_code/starter/testing_list.txt b/aos/atom_code/starter/testing_list.txt
new file mode 100644
index 0000000..96d412d
--- /dev/null
+++ b/aos/atom_code/starter/testing_list.txt
@@ -0,0 +1,3 @@
+../bin/LogReader
+../bin/JoystickCode
+../bin/AutoMode