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/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;
+}
+