Added a SensorEventQueue, a circular buffer meant for reading with one thread

and polling a subhal with another. The writing thread gets access to pointers
in the internal buffer. This design avoids a memcpy on write when the multihal
fetches subhal events using poll().

Unit-tests include multithreaded reading and writing lots of events, in
random-sized chunks.

This is not used by the multihal yet. That will be a different CL.

Change-Id: I58418d69eebebeb96befb08ba3aed080f0f08551

5 files changed, 389 insertions, 7 deletions

diff --git a/modules/sensors/Android.mk b/modules/sensors/Android.mk
index 6298ca0..5b787d4 100644
--- a/modules/sensors/Android.mk
+++ b/modules/sensors/Android.mk
@@ -14,10 +14,10 @@
 # limitations under the License.
 #
 
-ifeq ($(USE_SENSOR_MULTI_HAL),true)
-
 LOCAL_PATH := $(call my-dir)
 
+ifeq ($(USE_SENSOR_MULTI_HAL),true)
+
 include $(CLEAR_VARS)
 
 LOCAL_MODULE := sensors.$(TARGET_DEVICE)
@@ -25,24 +25,28 @@ LOCAL_MODULE := sensors.$(TARGET_DEVICE)
 LOCAL_MODULE_PATH := $(TARGET_OUT_SHARED_LIBRARIES)/hw
 
 LOCAL_CFLAGS := -DLOG_TAG=\"MultiHal\"
-LOCAL_SRC_FILES := multihal.cpp
+
+LOCAL_SRC_FILES := \
+    multihal.cpp \
+    SensorEventQueue.h \
+    SensorEventQueue.cpp \
 
 LOCAL_SHARED_LIBRARIES := \
-    liblog \
     libcutils \
     libdl \
+    liblog \
     libstlport \
     libutils \
-    libcutils \
-    liblog \
 
 LOCAL_PRELINK_MODULE := false
 LOCAL_STRIP_MODULE := false
 
 LOCAL_C_INCLUDES := \
     external/stlport/stlport \
-    bionic /
+    bionic \
 
 include $(BUILD_SHARED_LIBRARY)
 
 endif # USE_SENSOR_MULTI_HAL
+
+include $(call all-subdir-makefiles)
diff --git a/modules/sensors/SensorEventQueue.cpp b/modules/sensors/SensorEventQueue.cpp
new file mode 100644
index 0000000..c139944
--- /dev/null
+++ b/modules/sensors/SensorEventQueue.cpp
@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) 2013 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <hardware/sensors.h>
+#include <algorithm>
+#include <pthread.h>
+
+#include <linux/input.h>
+#include <cutils/atomic.h>
+#include <cutils/log.h>
+
+#include "SensorEventQueue.h"
+
+SensorEventQueue::SensorEventQueue(int capacity) {
+    mCapacity = capacity;
+    mStart = 0;
+    mSize = 0;
+    mData = new sensors_event_t[mCapacity];
+    pthread_cond_init(&mDataAvailableCondition, NULL);
+    pthread_cond_init(&mSpaceAvailableCondition, NULL);
+    pthread_mutex_init(&mMutex, NULL);
+}
+
+SensorEventQueue::~SensorEventQueue() {
+    delete[] mData;
+    mData = NULL;
+    pthread_cond_destroy(&mDataAvailableCondition);
+    pthread_cond_destroy(&mSpaceAvailableCondition);
+    pthread_mutex_destroy(&mMutex);
+}
+
+void SensorEventQueue::lock() {
+    pthread_mutex_lock(&mMutex);
+}
+
+void SensorEventQueue::unlock() {
+    pthread_mutex_unlock(&mMutex);
+}
+
+void SensorEventQueue::waitForSpaceAndLock() {
+    lock();
+    while (mSize >= mCapacity) {
+        pthread_cond_wait(&mSpaceAvailableCondition, &mMutex);
+    }
+}
+
+void SensorEventQueue::waitForDataAndLock() {
+    lock();
+    while (mSize <= 0) {
+        pthread_cond_wait(&mDataAvailableCondition, &mMutex);
+    }
+}
+
+int SensorEventQueue::getWritableRegion(int requestedLength, sensors_event_t** out) {
+    if (mSize >= mCapacity || requestedLength <= 0) {
+        *out = NULL;
+        return 0;
+    }
+    // Start writing after the last readable record.
+    int firstWritable = (mStart + mSize) % mCapacity;
+
+    int lastWritable = firstWritable + requestedLength - 1;
+
+    // Don't go past the end of the data array.
+    if (lastWritable > mCapacity - 1) {
+        lastWritable = mCapacity - 1;
+    }
+    // Don't go into the readable region.
+    if (firstWritable < mStart && lastWritable >= mStart) {
+        lastWritable = mStart - 1;
+    }
+    *out = &mData[firstWritable];
+    return lastWritable - firstWritable + 1;
+}
+
+void SensorEventQueue::markAsWritten(int count) {
+    mSize += count;
+    if (mSize) {
+        pthread_cond_broadcast(&mDataAvailableCondition);
+    }
+}
+
+int SensorEventQueue::getSize() {
+    return mSize;
+}
+
+sensors_event_t* SensorEventQueue::peek() {
+    if (mSize <= 0) return NULL;
+    return &mData[mStart];
+}
+
+void SensorEventQueue::dequeue() {
+    if (mSize <= 0) return;
+    mSize--;
+    mStart = (mStart + 1) % mCapacity;
+    pthread_cond_broadcast(&mSpaceAvailableCondition);
+}
diff --git a/modules/sensors/SensorEventQueue.h b/modules/sensors/SensorEventQueue.h
new file mode 100644
index 0000000..fd833fa
--- /dev/null
+++ b/modules/sensors/SensorEventQueue.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2013 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef SENSOREVENTQUEUE_H_
+#define SENSOREVENTQUEUE_H_
+
+#include <hardware/sensors.h>
+#include <pthread.h>
+
+/*
+ * Fixed-size circular queue, with an API developed around the sensor HAL poll() method.
+ * Poll() takes a pointer to a buffer, which is written by poll() before it returns.
+ * This class can provide a pointer to a spot in its internal buffer for poll() to
+ * write to, instead of using an intermediate buffer and a memcpy.
+ *
+ * Thread safety:
+ * Reading can be done safely after grabbing the mutex lock, while poll() writing in a separate
+ * thread without a mutex lock. But there can only be one writer at a time.
+ */
+class SensorEventQueue {
+    int mCapacity;
+    int mStart; // start of readable region
+    int mSize; // number of readable items
+    sensors_event_t* mData;
+    pthread_cond_t mDataAvailableCondition;
+    pthread_cond_t mSpaceAvailableCondition;
+    pthread_mutex_t mMutex;
+
+public:
+    SensorEventQueue(int capacity);
+    ~SensorEventQueue();
+    void lock();
+    void unlock();
+    void waitForSpaceAndLock();
+    void waitForDataAndLock();
+
+    // Returns length of region, between zero and min(capacity, requestedLength). If there is any
+    // writable space, it will return a region of at least one. Because it must return
+    // a pointer to a contiguous region, it may return smaller regions as we approach the end of
+    // the data array.
+    // Only call while holding the lock.
+    // The region is not marked internally in any way. Subsequent calls may return overlapping
+    // regions. This class expects there to be exactly one writer at a time.
+    int getWritableRegion(int requestedLength, sensors_event_t** out);
+
+    // After writing to the region returned by getWritableRegion(), call this to indicate how
+    // many records were actually written.
+    // This increases size() by count.
+    // Only call while holding the lock.
+    void markAsWritten(int count);
+
+    // Gets the number of readable records.
+    // Only call while holding the lock.
+    int getSize();
+
+    // Returns pointer to the first readable record, or NULL if size() is zero.
+    // Only call this while holding the lock.
+    sensors_event_t* peek();
+
+    // This will decrease the size by one, freeing up the oldest readable event's slot for writing.
+    // Only call while holding the lock.
+    void dequeue();
+};
+
+#endif // SENSOREVENTQUEUE_H_
diff --git a/modules/sensors/tests/Android.mk b/modules/sensors/tests/Android.mk
new file mode 100644
index 0000000..010bb90
--- /dev/null
+++ b/modules/sensors/tests/Android.mk
@@ -0,0 +1,17 @@
+LOCAL_PATH := $(call my-dir)
+
+include $(CLEAR_VARS)
+
+LOCAL_SRC_FILES := \
+	SensorEventQueue_test.cpp
+
+#LOCAL_CFLAGS := -g
+LOCAL_MODULE := sensorstests
+
+LOCAL_STATIC_LIBRARIES := libcutils libutils
+
+LOCAL_C_INCLUDES := $(LOCAL_PATH)/.. bionic
+
+LOCAL_LDLIBS += -lpthread
+
+include $(BUILD_HOST_EXECUTABLE)
diff --git a/modules/sensors/tests/SensorEventQueue_test.cpp b/modules/sensors/tests/SensorEventQueue_test.cpp
new file mode 100644
index 0000000..3b89964
--- /dev/null
+++ b/modules/sensors/tests/SensorEventQueue_test.cpp
@@ -0,0 +1,173 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <hardware/sensors.h>
+#include <pthread.h>
+#include "SensorEventQueue.cpp"
+
+// Unit tests for the SensorEventQueue.
+
+// Run it like this:
+//
+// make sensorstests -j32 && \
+// out/host/linux-x86/obj/EXECUTABLES/sensorstests_intermediates/sensorstests
+
+bool checkWritableBufferSize(SensorEventQueue* queue, int requested, int expected) {
+    sensors_event_t* buffer;
+    int actual = queue->getWritableRegion(requested, &buffer);
+    if (actual != expected) {
+        printf("Expected buffer size was %d; actual was %d\n", expected, actual);
+        return false;
+    }
+    return true;
+}
+
+bool checkSize(SensorEventQueue* queue, int expected) {
+    int actual = queue->getSize();
+    if (actual != expected) {
+        printf("Expected queue size was %d; actual was %d", expected, actual);
+        return false;
+    }
+    return true;
+}
+
+bool testSimpleWriteSizeCounts() {
+    printf("TEST testSimpleWriteSizeCounts\n");
+    SensorEventQueue* queue = new SensorEventQueue(10);
+    if (!checkSize(queue, 0)) return false;
+    if (!checkWritableBufferSize(queue, 11, 10)) return false;
+    if (!checkWritableBufferSize(queue, 10, 10)) return false;
+    if (!checkWritableBufferSize(queue, 9, 9)) return false;
+
+    queue->markAsWritten(7);
+    if (!checkSize(queue, 7)) return false;
+    if (!checkWritableBufferSize(queue, 4, 3)) return false;
+    if (!checkWritableBufferSize(queue, 3, 3)) return false;
+    if (!checkWritableBufferSize(queue, 2, 2)) return false;
+
+    queue->markAsWritten(3);
+    if (!checkSize(queue, 10)) return false;
+    if (!checkWritableBufferSize(queue, 1, 0)) return false;
+
+    printf("passed\n");
+    return true;
+}
+
+bool testWrappingWriteSizeCounts() {
+    printf("TEST testWrappingWriteSizeCounts\n");
+    SensorEventQueue* queue = new SensorEventQueue(10);
+    queue->markAsWritten(9);
+    if (!checkSize(queue, 9)) return false;
+
+    // dequeue from the front
+    queue->dequeue();
+    queue->dequeue();
+    if (!checkSize(queue, 7)) return false;
+    if (!checkWritableBufferSize(queue, 100, 1)) return false;
+
+    // Write all the way to the end.
+    queue->markAsWritten(1);
+    if (!checkSize(queue, 8)) return false;
+    // Now the two free spots in the front are available.
+    if (!checkWritableBufferSize(queue, 100, 2)) return false;
+
+    // Fill the queue again
+    queue->markAsWritten(2);
+    if (!checkSize(queue, 10)) return false;
+
+    printf("passed\n");
+    return true;
+}
+
+static const int TTOQ_EVENT_COUNT = 10000;
+
+struct TaskContext {
+  bool success;
+  SensorEventQueue* queue;
+};
+
+void* writerTask(void* ptr) {
+    printf("writerTask starts\n");
+    TaskContext* ctx = (TaskContext*)ptr;
+    SensorEventQueue* queue = ctx->queue;
+    int totalWrites = 0;
+    sensors_event_t* buffer;
+    while (totalWrites < TTOQ_EVENT_COUNT) {
+        queue->waitForSpaceAndLock();
+        int writableSize = queue->getWritableRegion(rand() % 10 + 1, &buffer);
+        queue->unlock();
+        for (int i = 0; i < writableSize; i++) {
+            // serialize the events
+            buffer[i].timestamp = totalWrites++;
+        }
+        queue->lock();
+        queue->markAsWritten(writableSize);
+        queue->unlock();
+    }
+    printf("writerTask ends normally\n");
+    return NULL;
+}
+
+void* readerTask(void* ptr) {
+    printf("readerTask starts\n");
+    TaskContext* ctx = (TaskContext*)ptr;
+    SensorEventQueue* queue = ctx->queue;
+    int totalReads = 0;
+    while (totalReads < TTOQ_EVENT_COUNT) {
+        queue->waitForDataAndLock();
+        int maxReads = rand() % 20 + 1;
+        int reads = 0;
+        while (queue->getSize() && reads < maxReads) {
+            sensors_event_t* event = queue->peek();
+            if (totalReads != event->timestamp) {
+                printf("FAILURE: readerTask expected timestamp %d; actual was %d\n",
+                        totalReads, (int)(event->timestamp));
+                ctx->success = false;
+                return NULL;
+            }
+            queue->dequeue();
+            totalReads++;
+            reads++;
+        }
+        queue->unlock();
+    }
+    printf("readerTask ends normally\n");
+    return NULL;
+}
+
+
+// Create a short queue, and write and read a ton of data through it.
+// Write serial timestamps into the events, and expect to read them in the right order.
+bool testTwoThreadsOneQueue() {
+    printf("TEST testTwoThreadsOneQueue\n");
+    SensorEventQueue* queue = new SensorEventQueue(100);
+
+    TaskContext readerCtx;
+    readerCtx.success = true;
+    readerCtx.queue = queue;
+
+    TaskContext writerCtx;
+    writerCtx.success = true;
+    writerCtx.queue = queue;
+
+    pthread_t writer, reader;
+    pthread_create(&reader, NULL, readerTask, &readerCtx);
+    pthread_create(&writer, NULL, writerTask, &writerCtx);
+
+    pthread_join(writer, NULL);
+    pthread_join(reader, NULL);
+
+    printf("testTwoThreadsOneQueue done\n");
+    return readerCtx.success && writerCtx.success;
+}
+
+
+int main(int argc, char **argv) {
+    if (testSimpleWriteSizeCounts() &&
+            testWrappingWriteSizeCounts() &&
+            testTwoThreadsOneQueue()) {
+        printf("ALL PASSED\n");
+    } else {
+        printf("SOMETHING FAILED\n");
+    }
+    return EXIT_SUCCESS;
+}