diff --git a/modules/sensors/SensorEventQueue.cpp b/modules/sensors/SensorEventQueue.cpp
index 00013de..6928034 100644
--- a/modules/sensors/SensorEventQueue.cpp
+++ b/modules/sensors/SensorEventQueue.cpp
@@ -81,8 +81,12 @@
     mStart = (mStart + 1) % mCapacity;
 }
 
-void SensorEventQueue::waitForSpace(pthread_mutex_t* mutex) {
+// returns true if it waited, or false if it was a no-op.
+bool SensorEventQueue::waitForSpace(pthread_mutex_t* mutex) {
+    bool waited = false;
     while (mSize == mCapacity) {
+        waited = true;
         pthread_cond_wait(&mSpaceAvailableCondition, mutex);
     }
+    return waited;
 }
diff --git a/modules/sensors/SensorEventQueue.h b/modules/sensors/SensorEventQueue.h
index 969d018..11e1f41 100644
--- a/modules/sensors/SensorEventQueue.h
+++ b/modules/sensors/SensorEventQueue.h
@@ -69,7 +69,8 @@
     void dequeue();
 
     // Blocks until space is available. No-op if there is already space.
-    void waitForSpace(pthread_mutex_t* mutex);
+    // Returns true if it had to wait.
+    bool waitForSpace(pthread_mutex_t* mutex);
 };
 
 #endif // SENSOREVENTQUEUE_H_
diff --git a/modules/sensors/multihal.cpp b/modules/sensors/multihal.cpp
index 52588c7..f0e979e 100644
--- a/modules/sensors/multihal.cpp
+++ b/modules/sensors/multihal.cpp
@@ -97,9 +97,9 @@
 }
 
 static int get_module_index(int global_handle) {
-    ALOGD("get_module_index for global_handle %d", global_handle);
     FullHandle f = global_to_full[global_handle];
-    ALOGD("FullHandle moduleIndex %d, localHandle %d", f.moduleIndex, f.localHandle);
+    ALOGD("FullHandle for global_handle %d: moduleIndex %d, localHandle %d",
+            global_handle, f.moduleIndex, f.localHandle);
     return f.moduleIndex;
 }
 
@@ -118,11 +118,10 @@
     sensors_event_t* buffer;
     int eventsPolled;
     while (1) {
-        ALOGD("writerTask before lock 1");
         pthread_mutex_lock(&queue_mutex);
-        ALOGD("writerTask before waitForSpace");
-        queue->waitForSpace(&queue_mutex);
-        ALOGD("writerTask after waitForSpace");
+        if (queue->waitForSpace(&queue_mutex)) {
+            ALOGD("writerTask waited for space");
+        }
         int bufferSize = queue->getWritableRegion(SENSOR_EVENT_QUEUE_CAPACITY, &buffer);
         // Do blocking poll outside of lock
         pthread_mutex_unlock(&queue_mutex);
@@ -130,8 +129,9 @@
         ALOGD("writerTask before poll() - bufferSize = %d", bufferSize);
         eventsPolled = device->poll(device, buffer, bufferSize);
         ALOGD("writerTask poll() got %d events.", eventsPolled);
-
-        ALOGD("writerTask before lock 2");
+        if (eventsPolled == 0) {
+            continue;
+        }
         pthread_mutex_lock(&queue_mutex);
         queue->markAsWritten(eventsPolled);
         ALOGD("writerTask wrote %d events", eventsPolled);
@@ -200,9 +200,7 @@
 }
 
 sensors_poll_device_t* sensors_poll_context_t::get_v0_device_by_handle(int handle) {
-    ALOGD("get_v0_device_by_handle(%d)", handle);
     int sub_index = get_module_index(handle);
-    ALOGD("sub_index: %d", sub_index);
     return (sensors_poll_device_t*) this->sub_hw_devices[sub_index];
 }
 
@@ -260,7 +258,6 @@
     while (eventsRead == 0) {
         while (empties < queueCount && eventsRead < maxReads) {
             SensorEventQueue* queue = this->queues.at(this->nextReadIndex);
-            ALOGD("queue size: %d", queue->getSize());
             sensors_event_t* event = queue->peek();
             if (event == NULL) {
                 empties++;
@@ -272,18 +269,16 @@
             this->nextReadIndex = (this->nextReadIndex + 1) % queueCount;
         }
         if (eventsRead == 0) {
-            // The queues have been scanned and none contain data.
-            // Wait for any of them to signal that there's data.
+            // The queues have been scanned and none contain data, so wait.
             ALOGD("poll stopping to wait for data");
             waiting_for_data = true;
             pthread_cond_wait(&data_available_cond, &queue_mutex);
             waiting_for_data = false;
             empties = 0;
-            ALOGD("poll done waiting for data");
         }
     }
     pthread_mutex_unlock(&queue_mutex);
-    ALOGD("...poll's blocking read ends. Returning %d events.", eventsRead);
+    ALOGD("poll returning %d events.", eventsRead);
 
     return eventsRead;
 }
diff --git a/modules/sensors/tests/SensorEventQueue_test.cpp b/modules/sensors/tests/SensorEventQueue_test.cpp
index cbe4377..3218bb9 100644
--- a/modules/sensors/tests/SensorEventQueue_test.cpp
+++ b/modules/sensors/tests/SensorEventQueue_test.cpp
@@ -26,14 +26,22 @@
 bool checkSize(SensorEventQueue* queue, int expected) {
     int actual = queue->getSize();
     if (actual != expected) {
-        printf("Expected queue size was %d; actual was %d", expected, actual);
+        printf("Expected queue size was %d; actual was %d\n", expected, actual);
+        return false;
+    }
+    return true;
+}
+
+bool checkInt(char* msg, int expected, int actual) {
+    if (actual != expected) {
+        printf("%s; expected %d; actual was %d\n", msg, expected, actual);
         return false;
     }
     return true;
 }
 
 bool testSimpleWriteSizeCounts() {
-    printf("TEST testSimpleWriteSizeCounts\n");
+    printf("testSimpleWriteSizeCounts\n");
     SensorEventQueue* queue = new SensorEventQueue(10);
     if (!checkSize(queue, 0)) return false;
     if (!checkWritableBufferSize(queue, 11, 10)) return false;
@@ -55,7 +63,7 @@
 }
 
 bool testWrappingWriteSizeCounts() {
-    printf("TEST testWrappingWriteSizeCounts\n");
+    printf("testWrappingWriteSizeCounts\n");
     SensorEventQueue* queue = new SensorEventQueue(10);
     queue->markAsWritten(9);
     if (!checkSize(queue, 9)) return false;
@@ -80,9 +88,109 @@
     return true;
 }
 
+
+
+struct TaskContext {
+  bool success;
+  SensorEventQueue* queue;
+};
+
+static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
+static pthread_cond_t dataAvailableCond = PTHREAD_COND_INITIALIZER;
+
+int FULL_QUEUE_CAPACITY = 5;
+int FULL_QUEUE_EVENT_COUNT = 31;
+
+void *fullQueueWriterTask(void* ptr) {
+    TaskContext* ctx = (TaskContext*)ptr;
+    SensorEventQueue* queue = ctx->queue;
+    ctx->success = true;
+    int totalWaits = 0;
+    int totalWrites = 0;
+    sensors_event_t* buffer;
+
+    while (totalWrites < FULL_QUEUE_EVENT_COUNT) {
+        pthread_mutex_lock(&mutex);
+        if (queue->waitForSpace(&mutex)) {
+            totalWaits++;
+            printf(".");
+        }
+        int writableSize = queue->getWritableRegion(FULL_QUEUE_CAPACITY, &buffer);
+        queue->markAsWritten(writableSize);
+        totalWrites += writableSize;
+        for (int i = 0; i < writableSize; i++) {
+            printf("w");
+        }
+        pthread_cond_broadcast(&dataAvailableCond);
+        pthread_mutex_unlock(&mutex);
+    }
+    printf("\n");
+
+    ctx->success =
+            checkInt("totalWrites", FULL_QUEUE_EVENT_COUNT, totalWrites) &&
+            checkInt("totalWaits", FULL_QUEUE_EVENT_COUNT - FULL_QUEUE_CAPACITY, totalWaits);
+    return NULL;
+}
+
+bool fullQueueReaderShouldRead(int queueSize, int totalReads) {
+    if (queueSize == 0) {
+        return false;
+    }
+    int totalWrites = totalReads + queueSize;
+    return queueSize == FULL_QUEUE_CAPACITY || totalWrites == FULL_QUEUE_EVENT_COUNT;
+}
+
+void* fullQueueReaderTask(void* ptr) {
+    TaskContext* ctx = (TaskContext*)ptr;
+    SensorEventQueue* queue = ctx->queue;
+    int totalReads = 0;
+    while (totalReads < FULL_QUEUE_EVENT_COUNT) {
+        pthread_mutex_lock(&mutex);
+        // Only read if there are events,
+        // and either the queue is full, or if we're reading the last few events.
+        while (!fullQueueReaderShouldRead(queue->getSize(), totalReads)) {
+            pthread_cond_wait(&dataAvailableCond, &mutex);
+        }
+        queue->dequeue();
+        totalReads++;
+        printf("r");
+        pthread_mutex_unlock(&mutex);
+    }
+    printf("\n");
+    ctx->success = ctx->success && checkInt("totalreads", FULL_QUEUE_EVENT_COUNT, totalReads);
+    return NULL;
+}
+
+// Test internal queue-full waiting and broadcasting.
+bool testFullQueueIo() {
+    printf("testFullQueueIo\n");
+    SensorEventQueue* queue = new SensorEventQueue(FULL_QUEUE_CAPACITY);
+
+    TaskContext readerCtx;
+    readerCtx.success = true;
+    readerCtx.queue = queue;
+
+    TaskContext writerCtx;
+    writerCtx.success = true;
+    writerCtx.queue = queue;
+
+    pthread_t writer, reader;
+    pthread_create(&reader, NULL, fullQueueReaderTask, &readerCtx);
+    pthread_create(&writer, NULL, fullQueueWriterTask, &writerCtx);
+
+    pthread_join(writer, NULL);
+    pthread_join(reader, NULL);
+
+    if (!readerCtx.success || !writerCtx.success) return false;
+    printf("passed\n");
+    return true;
+}
+
+
 int main(int argc, char **argv) {
     if (testSimpleWriteSizeCounts() &&
-            testWrappingWriteSizeCounts()) {
+            testWrappingWriteSizeCounts() &&
+            testFullQueueIo()) {
         printf("ALL PASSED\n");
     } else {
         printf("SOMETHING FAILED\n");