aos/linux_code/ipc_lib/queue.cc - RealtimeRoboticsGroup/test - Gitiles

 #include "aos/linux_code/ipc_lib/queue.h"

 #include <stdio.h>
 #include <string.h>
 #include <errno.h>
 #include <assert.h>

 #include <memory>
 #include <algorithm>

 #include "aos/common/logging/logging.h"
 #include "aos/common/type_traits.h"
 #include "aos/linux_code/ipc_lib/core_lib.h"

 namespace aos {
 namespace {

 static_assert(shm_ok<RawQueue>::value,
               "RawQueue instances go into shared memory");

 const bool kReadDebug = false;
 const bool kWriteDebug = false;
 const bool kRefDebug = false;
 const bool kFetchDebug = false;

 // The number of extra messages the pool associated with each queue will be able
 // to hold (for readers who are slow about freeing them or who leak one when
 // they get killed).
 const int kExtraMessages = 20;

 }  // namespace

 const int RawQueue::kPeek;
 const int RawQueue::kFromEnd;
 const int RawQueue::kNonBlock;
 const int RawQueue::kBlock;
 const int RawQueue::kOverride;

 struct RawQueue::MessageHeader {
   int ref_count;
   int index;  // in pool_
   // Gets the message header immediately preceding msg.
   static MessageHeader *Get(const void *msg) {
     return reinterpret_cast<MessageHeader *>(__builtin_assume_aligned(
         static_cast<uint8_t *>(const_cast<void *>(msg)) - sizeof(MessageHeader),
         alignof(MessageHeader)));
   }
   void Swap(MessageHeader *other) {
     MessageHeader temp;
     memcpy(&temp, other, sizeof(temp));
     memcpy(other, this, sizeof(*other));
     memcpy(this, &temp, sizeof(*this));
   }
 };
 static_assert(shm_ok<RawQueue::MessageHeader>::value,
               "the whole point is to stick it in shared memory");

 struct RawQueue::ReadData {
   bool writable_start;
 };

 // TODO(brians) maybe do this with atomic integer instructions so it doesn't
 //   have to lock/unlock pool_lock_
 void RawQueue::DecrementMessageReferenceCount(const void *msg) {
   MutexLocker locker(&pool_lock_);
   MessageHeader *header = MessageHeader::Get(msg);
   --header->ref_count;
   assert(header->ref_count >= 0);
   if (kRefDebug) {
     printf("ref_dec_count: %p count=%d\n", msg, header->ref_count);
   }
   if (header->ref_count == 0) {
     DoFreeMessage(msg);
   }
 }

 RawQueue::RawQueue(const char *name, size_t length, int hash, int queue_length)
     : readable_(&data_lock_), writable_(&data_lock_) {
   const size_t name_size = strlen(name) + 1;
   char *temp = static_cast<char *>(shm_malloc(name_size));
   memcpy(temp, name, name_size);
   name_ = temp;
   length_ = length;
   hash_ = hash;
   queue_length_ = queue_length;

   next_ = NULL;
   recycle_ = NULL;

   if (kFetchDebug) {
     printf("initializing name=%s, length=%zd, hash=%d, queue_length=%d\n",
            name, length, hash, queue_length);
   }

   data_length_ = queue_length + 1;
   if (data_length_ < 2) {  // TODO(brians) when could this happen?
     data_length_ = 2;
   }
   data_ = static_cast<void **>(shm_malloc(sizeof(void *) * data_length_));
   data_start_ = 0;
   data_end_ = 0;
   messages_ = 0;

   mem_length_ = queue_length + kExtraMessages;
   pool_length_ = 0;
   messages_used_ = 0;
   msg_length_ = length + sizeof(MessageHeader);
   pool_ = static_cast<MessageHeader **>(
       shm_malloc(sizeof(MessageHeader *) * mem_length_));

   if (kFetchDebug) {
     printf("made queue %s\n", name);
   }
 }
 RawQueue *RawQueue::Fetch(const char *name, size_t length, int hash,
                     int queue_length) {
   if (kFetchDebug) {
     printf("fetching queue %s\n", name);
   }
   if (mutex_lock(&global_core->mem_struct->queues.lock) != 0) {
     return NULL;
   }
   RawQueue *current = static_cast<RawQueue *>(
       global_core->mem_struct->queues.pointer);
   if (current != NULL) {
     while (true) {
       // If we found a matching queue.
       if (strcmp(current->name_, name) == 0 && current->length_ == length &&
           current->hash_ == hash && current->queue_length_ == queue_length) {
         mutex_unlock(&global_core->mem_struct->queues.lock);
         return current;
       } else {
         if (kFetchDebug) {
           printf("rejected queue %s strcmp=%d target=%s\n", current->name_,
                  strcmp(current->name_, name), name);
         }
       }
       // If this is the last one.
       if (current->next_ == NULL) break;
       current = current->next_;
     }
   }

   RawQueue *r = new (shm_malloc(sizeof(RawQueue)))
       RawQueue(name, length, hash, queue_length);
   if (current == NULL) {  // if we don't already have one
     global_core->mem_struct->queues.pointer = r;
   } else {
     current->next_ = r;
   }

   mutex_unlock(&global_core->mem_struct->queues.lock);
   return r;
 }
 RawQueue *RawQueue::Fetch(const char *name, size_t length, int hash,
                     int queue_length,
                     int recycle_hash, int recycle_length, RawQueue **recycle) {
   RawQueue *r = Fetch(name, length, hash, queue_length);
   r->recycle_ = Fetch(name, length, recycle_hash, recycle_length);
   if (r == r->recycle_) {
     fprintf(stderr, "queue: r->recycle_(=%p) == r(=%p)\n", r->recycle_, r);
     printf("see stderr\n");
     r->recycle_ = NULL;
     abort();
   }
   *recycle = r->recycle_;
   return r;
 }

 void RawQueue::DoFreeMessage(const void *msg) {
   MessageHeader *header = MessageHeader::Get(msg);
   if (pool_[header->index] != header) {  // if something's messed up
     fprintf(stderr, "queue: something is very very wrong with queue %p."
             " pool_(=%p)[header->index(=%d)] != header(=%p)\n",
             this, pool_, header->index, header);
     printf("queue: see stderr\n");
     abort();
   }
   if (kRefDebug) {
     printf("ref free: %p\n", msg);
   }
   --messages_used_;

   if (recycle_ != NULL) {
     void *const new_msg = recycle_->GetMessage();
     if (new_msg == NULL) {
       fprintf(stderr, "queue: couldn't get a message"
               " for recycle queue %p\n", recycle_);
     } else {
       // Take a message from recycle_ and switch its
       // header with the one being freed, which effectively
       // switches which queue each message belongs to.
       MessageHeader *const new_header = MessageHeader::Get(new_msg);
       // Also switch the messages between the pools.
       pool_[header->index] = new_header;
       {
         MutexLocker locker(&recycle_->pool_lock_);
         recycle_->pool_[new_header->index] = header;
         // Swap the information in both headers.
         header->Swap(new_header);
         // Don't unlock the other pool until all of its messages are valid.
       }
       // use the header for new_msg which is now for this pool
       header = new_header;
       if (!recycle_->WriteMessage(const_cast<void *>(msg), kOverride)) {
         fprintf(stderr, "queue: %p->WriteMessage(%p, kOverride) failed."
                 " aborting\n", recycle_, msg);
         printf("see stderr\n");
         abort();
       }
       msg = new_msg;
     }
   }

   // Where the one we're freeing was.
   int index = header->index;
   header->index = -1;
   if (index != messages_used_) {  // if we're not freeing the one on the end
     // Put the last one where the one we're freeing was.
     header = pool_[index] = pool_[messages_used_];
     // Put the one we're freeing at the end.
     pool_[messages_used_] = MessageHeader::Get(msg);
     // Update the former last one's index.
     header->index = index;
   }
 }

 bool RawQueue::WriteMessage(void *msg, int options) {
   if (kWriteDebug) {
     printf("queue: %p->WriteMessage(%p, %x)\n", this, msg, options);
   }
   if (msg == NULL || msg < reinterpret_cast<void *>(global_core->mem_struct) ||
       msg > static_cast<void *>((
               reinterpret_cast<char *>(global_core->mem_struct) +
               global_core->size))) {
     fprintf(stderr, "queue: attempt to write bad message %p to %p. aborting\n",
             msg, this);
     printf("see stderr\n");
     abort();
   }
   {
     MutexLocker locker(&data_lock_);
     bool writable_waited = false;

     int new_end;
     while (true) {
       new_end = (data_end_ + 1) % data_length_;
       // If there is room in the queue right now.
       if (new_end != data_start_) break;
       if (options & kNonBlock) {
         if (kWriteDebug) {
           printf("queue: not blocking on %p. returning false\n", this);
         }
         DecrementMessageReferenceCount(msg);
         return false;
       } else if (options & kOverride) {
         if (kWriteDebug) {
           printf("queue: overriding on %p\n", this);
         }
         // Avoid leaking the message that we're going to overwrite.
         DecrementMessageReferenceCount(data_[data_start_]);
         data_start_ = (data_start_ + 1) % data_length_;
       } else {  // kBlock
         if (kWriteDebug) {
           printf("queue: going to wait for writable_ of %p\n", this);
         }
         writable_.Wait();
         writable_waited = true;
       }
     }
     data_[data_end_] = msg;
     ++messages_;
     data_end_ = new_end;

     if (kWriteDebug) {
       printf("queue: broadcasting to readable_ of %p\n", this);
     }
     readable_.Broadcast();

     // If we got a signal on writable_ here and it's still writable, then we
     // need to signal the next person in line (if any).
     if (writable_waited && is_writable()) {
       if (kWriteDebug) {
         printf("queue: resignalling writable_ of %p\n", this);
       }
       writable_.Signal();
     }
   }
   if (kWriteDebug) {
     printf("queue: write returning true on queue %p\n", this);
   }
   return true;
 }

 void RawQueue::ReadCommonEnd(ReadData *read_data) {
   if (is_writable()) {
     if (kReadDebug) {
       printf("queue: %ssignalling writable_ of %p\n",
              read_data->writable_start ? "not " : "", this);
     }
     if (!read_data->writable_start) writable_.Signal();
   }
 }
 bool RawQueue::ReadCommonStart(int options, int *index, ReadData *read_data) {
   read_data->writable_start = is_writable();
   while (data_start_ == data_end_ || ((index != NULL) && messages_ <= *index)) {
     if (options & kNonBlock) {
       if (kReadDebug) {
         printf("queue: not going to block waiting on %p\n", this);
       }
       return false;
     } else {  // kBlock
       if (kReadDebug) {
         printf("queue: going to wait for readable_ of %p\n", this);
       }
       // Wait for a message to become readable.
       readable_.Wait();
       if (kReadDebug) {
         printf("queue: done waiting for readable_ of %p\n", this);
       }
     }
   }
   if (kReadDebug) {
     printf("queue: %p->read(%p) start=%d end=%d\n", this, index, data_start_,
            data_end_);
   }
   return true;
 }
 void *RawQueue::ReadPeek(int options, int start) {
   void *ret;
   if (options & kFromEnd) {
     int pos = data_end_ - 1;
     if (pos < 0) {  // if it needs to wrap
       pos = data_length_ - 1;
     }
     if (kReadDebug) {
       printf("queue: %p reading from line %d: %d\n", this, __LINE__, pos);
     }
     ret = data_[pos];
   } else {
     if (kReadDebug) {
       printf("queue: %p reading from line %d: %d\n", this, __LINE__, start);
     }
     ret = data_[start];
   }
   MessageHeader *const header = MessageHeader::Get(ret);
   ++header->ref_count;
   if (kRefDebug) {
     printf("ref inc count: %p\n", ret);
   }
   return ret;
 }
 const void *RawQueue::ReadMessage(int options) {
   if (kReadDebug) {
     printf("queue: %p->ReadMessage(%x)\n", this, options);
   }
   void *msg = NULL;

   MutexLocker locker(&data_lock_);

   ReadData read_data;
   if (!ReadCommonStart(options, NULL, &read_data)) {
     if (kReadDebug) {
       printf("queue: %p common returned false\n", this);
     }
     return NULL;
   }

   if (options & kPeek) {
     msg = ReadPeek(options, data_start_);
   } else {
     if (options & kFromEnd) {
       while (true) {
         if (kReadDebug) {
           printf("queue: %p start of c2\n", this);
         }
         // This loop pulls each message out of the buffer.
         const int pos = data_start_;
         data_start_ = (data_start_ + 1) % data_length_;
         // If this is the last one.
         if (data_start_ == data_end_) {
           if (kReadDebug) {
             printf("queue: %p reading from c2: %d\n", this, pos);
           }
           msg = data_[pos];
           break;
         }
         // This message is not going to be in the queue any more.
         DecrementMessageReferenceCount(data_[pos]);
       }
     } else {
       if (kReadDebug) {
         printf("queue: %p reading from d2: %d\n", this, data_start_);
       }
       msg = data_[data_start_];
       data_start_ = (data_start_ + 1) % data_length_;
     }
   }
   ReadCommonEnd(&read_data);
   if (kReadDebug) {
     printf("queue: %p read returning %p\n", this, msg);
   }
   return msg;
 }
 const void *RawQueue::ReadMessageIndex(int options, int *index) {
   if (kReadDebug) {
     printf("queue: %p->ReadMessageIndex(%x, %p(*=%d))\n",
            this, options, index, *index);
   }
   void *msg = NULL;

   MutexLocker locker(&data_lock_);

   ReadData read_data;
   if (!ReadCommonStart(options, index, &read_data)) {
     if (kReadDebug) {
       printf("queue: %p common returned false\n", this);
     }
     return NULL;
   }

   // TODO(parker): Handle integer wrap on the index.

   // Where we're going to start reading.
   int my_start;

   const int unread_messages = messages_ - *index;
   int current_messages = data_end_ - data_start_;
   if (current_messages < 0) current_messages += data_length_ - 1;
   if (unread_messages > current_messages) {  // If we're behind the available messages.
     // Catch index up to the last available message.
     *index = messages_ - current_messages;
     // And that's the one we're going to read.
     my_start = data_start_;
   } else {
     // Just start reading at the first available message that we haven't yet
     // read.
     my_start = (data_end_ - unread_messages) % data_length_;
     if (my_start < 0) my_start = data_start_ + unread_messages - 1;
   }

   if (options & kPeek) {
     msg = ReadPeek(options, my_start);
   } else {
     if (options & kFromEnd) {
       if (kReadDebug) {
         printf("queue: %p start of c1\n", this);
       }
       int pos = data_end_ - 1;
       if (pos < 0) {  // If it wrapped.
         pos = data_length_ - 1;  // Unwrap it.
       }
       if (kReadDebug) {
         printf("queue: %p reading from c1: %d\n", this, pos);
       }
       msg = data_[pos];
       *index = messages_;
     } else {
       if (kReadDebug) {
         printf("queue: %p reading from d1: %d\n", this, my_start);
       }
 #if 0
       // TODO(brians): Do this check right? (make sure full queue works etc)
       // This assert checks that we're either within both endpoints (duh) or
       // outside of both of them (if the queue is wrapped around).
       assert((my_start >= data_start_ && my_start < data_end_) ||
              (my_start > data_end_ && my_start <= data_start_));
 #endif
       msg = data_[my_start];
       ++(*index);
     }
     MessageHeader *const header = MessageHeader::Get(msg);
     ++header->ref_count;
     if (kRefDebug) {
       printf("ref_inc_count: %p\n", msg);
     }
   }
   ReadCommonEnd(&read_data);
   return msg;
 }

 void *RawQueue::GetMessage() {
   MutexLocker locker(&pool_lock_);
   MessageHeader *header;
   if (pool_length_ - messages_used_ > 0) {
     header = pool_[messages_used_];
   } else {
     if (pool_length_ >= mem_length_) {
       LOG(FATAL, "overused pool of queue %p\n", this);
     }
     header = pool_[pool_length_] =
         static_cast<MessageHeader *>(shm_malloc(msg_length_));
     ++pool_length_;
   }
   void *msg = reinterpret_cast<uint8_t *>(header) + sizeof(MessageHeader);
   header->ref_count = 1;
   if (kRefDebug) {
     printf("%p ref alloc: %p\n", this, msg);
   }
   header->index = messages_used_;
   ++messages_used_;
   return msg;
 }

 }  // namespace aos
	#include "aos/linux_code/ipc_lib/queue.h"

	#include <stdio.h>
	#include <string.h>
	#include <errno.h>
	#include <assert.h>

	#include <memory>
	#include <algorithm>

	#include "aos/common/logging/logging.h"
	#include "aos/common/type_traits.h"
	#include "aos/linux_code/ipc_lib/core_lib.h"

	namespace aos {
	namespace {

	static_assert(shm_ok<RawQueue>::value,
	"RawQueue instances go into shared memory");

	const bool kReadDebug = false;
	const bool kWriteDebug = false;
	const bool kRefDebug = false;
	const bool kFetchDebug = false;

	// The number of extra messages the pool associated with each queue will be able
	// to hold (for readers who are slow about freeing them or who leak one when
	// they get killed).
	const int kExtraMessages = 20;

	} // namespace

	const int RawQueue::kPeek;
	const int RawQueue::kFromEnd;
	const int RawQueue::kNonBlock;
	const int RawQueue::kBlock;
	const int RawQueue::kOverride;

	struct RawQueue::MessageHeader {
	int ref_count;
	int index; // in pool_
	// Gets the message header immediately preceding msg.
	static MessageHeader Get(const void msg) {
	return reinterpret_cast<MessageHeader *>(__builtin_assume_aligned(
	static_cast<uint8_t >(const_cast<void >(msg)) - sizeof(MessageHeader),
	alignof(MessageHeader)));
	}
	void Swap(MessageHeader *other) {
	MessageHeader temp;
	memcpy(&temp, other, sizeof(temp));
	memcpy(other, this, sizeof(*other));
	memcpy(this, &temp, sizeof(*this));
	}
	};
	static_assert(shm_ok<RawQueue::MessageHeader>::value,
	"the whole point is to stick it in shared memory");

	struct RawQueue::ReadData {
	bool writable_start;
	};

	// TODO(brians) maybe do this with atomic integer instructions so it doesn't
	// have to lock/unlock pool_lock_
	void RawQueue::DecrementMessageReferenceCount(const void *msg) {
	MutexLocker locker(&pool_lock_);
	MessageHeader *header = MessageHeader::Get(msg);
	--header->ref_count;
	assert(header->ref_count >= 0);
	if (kRefDebug) {
	printf("ref_dec_count: %p count=%d\n", msg, header->ref_count);
	}
	if (header->ref_count == 0) {
	DoFreeMessage(msg);
	}
	}

	RawQueue::RawQueue(const char *name, size_t length, int hash, int queue_length)
	: readable_(&data_lock_), writable_(&data_lock_) {
	const size_t name_size = strlen(name) + 1;
	char temp = static_cast<char >(shm_malloc(name_size));
	memcpy(temp, name, name_size);
	name_ = temp;
	length_ = length;
	hash_ = hash;
	queue_length_ = queue_length;

	next_ = NULL;
	recycle_ = NULL;

	if (kFetchDebug) {
	printf("initializing name=%s, length=%zd, hash=%d, queue_length=%d\n",
	name, length, hash, queue_length);
	}

	data_length_ = queue_length + 1;
	if (data_length_ < 2) { // TODO(brians) when could this happen?
	data_length_ = 2;
	}
	data_ = static_cast<void *>(shm_malloc(sizeof(void ) * data_length_));
	data_start_ = 0;
	data_end_ = 0;
	messages_ = 0;

	mem_length_ = queue_length + kExtraMessages;
	pool_length_ = 0;
	messages_used_ = 0;
	msg_length_ = length + sizeof(MessageHeader);
	pool_ = static_cast<MessageHeader **>(
	shm_malloc(sizeof(MessageHeader ) mem_length_));

	if (kFetchDebug) {
	printf("made queue %s\n", name);
	}
	}
	RawQueue RawQueue::Fetch(const char name, size_t length, int hash,
	int queue_length) {
	if (kFetchDebug) {
	printf("fetching queue %s\n", name);
	}
	if (mutex_lock(&global_core->mem_struct->queues.lock) != 0) {
	return NULL;
	}
	RawQueue current = static_cast<RawQueue >(
	global_core->mem_struct->queues.pointer);
	if (current != NULL) {
	while (true) {
	// If we found a matching queue.
	if (strcmp(current->name_, name) == 0 && current->length_ == length &&
	current->hash_ == hash && current->queue_length_ == queue_length) {
	mutex_unlock(&global_core->mem_struct->queues.lock);
	return current;
	} else {
	if (kFetchDebug) {
	printf("rejected queue %s strcmp=%d target=%s\n", current->name_,
	strcmp(current->name_, name), name);
	}
	}
	// If this is the last one.
	if (current->next_ == NULL) break;
	current = current->next_;
	}
	}

	RawQueue *r = new (shm_malloc(sizeof(RawQueue)))
	RawQueue(name, length, hash, queue_length);
	if (current == NULL) { // if we don't already have one
	global_core->mem_struct->queues.pointer = r;
	} else {
	current->next_ = r;
	}

	mutex_unlock(&global_core->mem_struct->queues.lock);
	return r;
	}
	RawQueue RawQueue::Fetch(const char name, size_t length, int hash,
	int queue_length,
	int recycle_hash, int recycle_length, RawQueue **recycle) {
	RawQueue *r = Fetch(name, length, hash, queue_length);
	r->recycle_ = Fetch(name, length, recycle_hash, recycle_length);
	if (r == r->recycle_) {
	fprintf(stderr, "queue: r->recycle_(=%p) == r(=%p)\n", r->recycle_, r);
	printf("see stderr\n");
	r->recycle_ = NULL;
	abort();
	}
	*recycle = r->recycle_;
	return r;
	}

	void RawQueue::DoFreeMessage(const void *msg) {
	MessageHeader *header = MessageHeader::Get(msg);
	if (pool_[header->index] != header) { // if something's messed up
	fprintf(stderr, "queue: something is very very wrong with queue %p."
	" pool_(=%p)[header->index(=%d)] != header(=%p)\n",
	this, pool_, header->index, header);
	printf("queue: see stderr\n");
	abort();
	}
	if (kRefDebug) {
	printf("ref free: %p\n", msg);
	}
	--messages_used_;

	if (recycle_ != NULL) {
	void *const new_msg = recycle_->GetMessage();
	if (new_msg == NULL) {
	fprintf(stderr, "queue: couldn't get a message"
	" for recycle queue %p\n", recycle_);
	} else {
	// Take a message from recycle_ and switch its
	// header with the one being freed, which effectively
	// switches which queue each message belongs to.
	MessageHeader *const new_header = MessageHeader::Get(new_msg);
	// Also switch the messages between the pools.
	pool_[header->index] = new_header;
	{
	MutexLocker locker(&recycle_->pool_lock_);
	recycle_->pool_[new_header->index] = header;
	// Swap the information in both headers.
	header->Swap(new_header);
	// Don't unlock the other pool until all of its messages are valid.
	}
	// use the header for new_msg which is now for this pool
	header = new_header;
	if (!recycle_->WriteMessage(const_cast<void *>(msg), kOverride)) {
	fprintf(stderr, "queue: %p->WriteMessage(%p, kOverride) failed."
	" aborting\n", recycle_, msg);
	printf("see stderr\n");
	abort();
	}
	msg = new_msg;
	}
	}

	// Where the one we're freeing was.
	int index = header->index;
	header->index = -1;
	if (index != messages_used_) { // if we're not freeing the one on the end
	// Put the last one where the one we're freeing was.
	header = pool_[index] = pool_[messages_used_];
	// Put the one we're freeing at the end.
	pool_[messages_used_] = MessageHeader::Get(msg);
	// Update the former last one's index.
	header->index = index;
	}
	}

	bool RawQueue::WriteMessage(void *msg, int options) {
	if (kWriteDebug) {
	printf("queue: %p->WriteMessage(%p, %x)\n", this, msg, options);
	}
	if (msg == NULL \|\| msg < reinterpret_cast<void *>(global_core->mem_struct) \|\|
	msg > static_cast<void *>((
	reinterpret_cast<char *>(global_core->mem_struct) +
	global_core->size))) {
	fprintf(stderr, "queue: attempt to write bad message %p to %p. aborting\n",
	msg, this);
	printf("see stderr\n");
	abort();
	}
	{
	MutexLocker locker(&data_lock_);
	bool writable_waited = false;

	int new_end;
	while (true) {
	new_end = (data_end_ + 1) % data_length_;
	// If there is room in the queue right now.
	if (new_end != data_start_) break;
	if (options & kNonBlock) {
	if (kWriteDebug) {
	printf("queue: not blocking on %p. returning false\n", this);
	}
	DecrementMessageReferenceCount(msg);
	return false;
	} else if (options & kOverride) {
	if (kWriteDebug) {
	printf("queue: overriding on %p\n", this);
	}
	// Avoid leaking the message that we're going to overwrite.
	DecrementMessageReferenceCount(data_[data_start_]);
	data_start_ = (data_start_ + 1) % data_length_;
	} else { // kBlock
	if (kWriteDebug) {
	printf("queue: going to wait for writable_ of %p\n", this);
	}
	writable_.Wait();
	writable_waited = true;
	}
	}
	data_[data_end_] = msg;
	++messages_;
	data_end_ = new_end;

	if (kWriteDebug) {
	printf("queue: broadcasting to readable_ of %p\n", this);
	}
	readable_.Broadcast();

	// If we got a signal on writable_ here and it's still writable, then we
	// need to signal the next person in line (if any).
	if (writable_waited && is_writable()) {
	if (kWriteDebug) {
	printf("queue: resignalling writable_ of %p\n", this);
	}
	writable_.Signal();
	}
	}
	if (kWriteDebug) {
	printf("queue: write returning true on queue %p\n", this);
	}
	return true;
	}

	void RawQueue::ReadCommonEnd(ReadData *read_data) {
	if (is_writable()) {
	if (kReadDebug) {
	printf("queue: %ssignalling writable_ of %p\n",
	read_data->writable_start ? "not " : "", this);
	}
	if (!read_data->writable_start) writable_.Signal();
	}
	}
	bool RawQueue::ReadCommonStart(int options, int index, ReadData read_data) {
	read_data->writable_start = is_writable();
	while (data_start_ == data_end_ \|\| ((index != NULL) && messages_ <= *index)) {
	if (options & kNonBlock) {
	if (kReadDebug) {
	printf("queue: not going to block waiting on %p\n", this);
	}
	return false;
	} else { // kBlock
	if (kReadDebug) {
	printf("queue: going to wait for readable_ of %p\n", this);
	}
	// Wait for a message to become readable.
	readable_.Wait();
	if (kReadDebug) {
	printf("queue: done waiting for readable_ of %p\n", this);
	}
	}
	}
	if (kReadDebug) {
	printf("queue: %p->read(%p) start=%d end=%d\n", this, index, data_start_,
	data_end_);
	}
	return true;
	}
	void *RawQueue::ReadPeek(int options, int start) {
	void *ret;
	if (options & kFromEnd) {
	int pos = data_end_ - 1;
	if (pos < 0) { // if it needs to wrap
	pos = data_length_ - 1;
	}
	if (kReadDebug) {
	printf("queue: %p reading from line %d: %d\n", this, __LINE__, pos);
	}
	ret = data_[pos];
	} else {
	if (kReadDebug) {
	printf("queue: %p reading from line %d: %d\n", this, __LINE__, start);
	}
	ret = data_[start];
	}
	MessageHeader *const header = MessageHeader::Get(ret);
	++header->ref_count;
	if (kRefDebug) {
	printf("ref inc count: %p\n", ret);
	}
	return ret;
	}
	const void *RawQueue::ReadMessage(int options) {
	if (kReadDebug) {
	printf("queue: %p->ReadMessage(%x)\n", this, options);
	}
	void *msg = NULL;

	MutexLocker locker(&data_lock_);

	ReadData read_data;
	if (!ReadCommonStart(options, NULL, &read_data)) {
	if (kReadDebug) {
	printf("queue: %p common returned false\n", this);
	}
	return NULL;
	}

	if (options & kPeek) {
	msg = ReadPeek(options, data_start_);
	} else {
	if (options & kFromEnd) {
	while (true) {
	if (kReadDebug) {
	printf("queue: %p start of c2\n", this);
	}
	// This loop pulls each message out of the buffer.
	const int pos = data_start_;
	data_start_ = (data_start_ + 1) % data_length_;
	// If this is the last one.
	if (data_start_ == data_end_) {
	if (kReadDebug) {
	printf("queue: %p reading from c2: %d\n", this, pos);
	}
	msg = data_[pos];
	break;
	}
	// This message is not going to be in the queue any more.
	DecrementMessageReferenceCount(data_[pos]);
	}
	} else {
	if (kReadDebug) {
	printf("queue: %p reading from d2: %d\n", this, data_start_);
	}
	msg = data_[data_start_];
	data_start_ = (data_start_ + 1) % data_length_;
	}
	}
	ReadCommonEnd(&read_data);
	if (kReadDebug) {
	printf("queue: %p read returning %p\n", this, msg);
	}
	return msg;
	}
	const void RawQueue::ReadMessageIndex(int options, int index) {
	if (kReadDebug) {
	printf("queue: %p->ReadMessageIndex(%x, %p(*=%d))\n",
	this, options, index, *index);
	}
	void *msg = NULL;

	MutexLocker locker(&data_lock_);

	ReadData read_data;
	if (!ReadCommonStart(options, index, &read_data)) {
	if (kReadDebug) {
	printf("queue: %p common returned false\n", this);
	}
	return NULL;
	}

	// TODO(parker): Handle integer wrap on the index.

	// Where we're going to start reading.
	int my_start;

	const int unread_messages = messages_ - *index;
	int current_messages = data_end_ - data_start_;
	if (current_messages < 0) current_messages += data_length_ - 1;
	if (unread_messages > current_messages) { // If we're behind the available messages.
	// Catch index up to the last available message.
	*index = messages_ - current_messages;
	// And that's the one we're going to read.
	my_start = data_start_;
	} else {
	// Just start reading at the first available message that we haven't yet
	// read.
	my_start = (data_end_ - unread_messages) % data_length_;
	if (my_start < 0) my_start = data_start_ + unread_messages - 1;
	}

	if (options & kPeek) {
	msg = ReadPeek(options, my_start);
	} else {
	if (options & kFromEnd) {
	if (kReadDebug) {
	printf("queue: %p start of c1\n", this);
	}
	int pos = data_end_ - 1;
	if (pos < 0) { // If it wrapped.
	pos = data_length_ - 1; // Unwrap it.
	}
	if (kReadDebug) {
	printf("queue: %p reading from c1: %d\n", this, pos);
	}
	msg = data_[pos];
	*index = messages_;
	} else {
	if (kReadDebug) {
	printf("queue: %p reading from d1: %d\n", this, my_start);
	}
	#if 0
	// TODO(brians): Do this check right? (make sure full queue works etc)
	// This assert checks that we're either within both endpoints (duh) or
	// outside of both of them (if the queue is wrapped around).
	assert((my_start >= data_start_ && my_start < data_end_) \|\|
	(my_start > data_end_ && my_start <= data_start_));
	#endif
	msg = data_[my_start];
	++(*index);
	}
	MessageHeader *const header = MessageHeader::Get(msg);
	++header->ref_count;
	if (kRefDebug) {
	printf("ref_inc_count: %p\n", msg);
	}
	}
	ReadCommonEnd(&read_data);
	return msg;
	}

	void *RawQueue::GetMessage() {
	MutexLocker locker(&pool_lock_);
	MessageHeader *header;
	if (pool_length_ - messages_used_ > 0) {
	header = pool_[messages_used_];
	} else {
	if (pool_length_ >= mem_length_) {
	LOG(FATAL, "overused pool of queue %p\n", this);
	}
	header = pool_[pool_length_] =
	static_cast<MessageHeader *>(shm_malloc(msg_length_));
	++pool_length_;
	}
	void msg = reinterpret_cast<uint8_t >(header) + sizeof(MessageHeader);
	header->ref_count = 1;
	if (kRefDebug) {
	printf("%p ref alloc: %p\n", this, msg);
	}
	header->index = messages_used_;
	++messages_used_;
	return msg;
	}

	} // namespace aos