1 files changed, 104 insertions, 0 deletions
diff --git a/daemon/Fifo.cpp b/daemon/Fifo.cpp
new file mode 100644
index 0000000..bbc43a7
--- /dev/null
+++ b/daemon/Fifo.cpp
@@ -0,0 +1,104 @@
+/**
+ * Copyright (C) ARM Limited 2010-2011. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+typedef unsigned long long uint64_t;
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include "Fifo.h"
+#include "Logging.h"
+
+extern void handleException();
+
+Fifo::Fifo(int numBuffers, int bufferSize) {
+	int	which;
+
+	if (numBuffers > FIFO_BUFFER_LIMIT) {
+		logg->logError(__FILE__, __LINE__, "Number of fifo buffers exceeds maximum");
+		handleException();
+	}
+	mNumBuffers = numBuffers;
+	mBufferSize = bufferSize;
+	mWriteCurrent = 0;
+	mReadCurrent = mNumBuffers - 1; // (n-1) pipelined
+
+	for (which=0; which<mNumBuffers; which++) {
+		// initialized read-to-write sem to 1, so that first wait goes through; write-to-read init'd to 0
+		if (sem_init(&mReadToWriteSem[which], 0, 1) ||
+			sem_init(&mWriteToReadSem[which], 0, 0)) {
+			logg->logError(__FILE__, __LINE__, "sem_init(%d) failed", which);
+			handleException();
+		}
+		// page-align allocate buffers
+		mBuffer[which] = (char*)valloc(bufferSize);
+		if (mBuffer[which] == NULL) {
+			logg->logError(__FILE__, __LINE__, "failed to allocate %d bytes", bufferSize);
+			handleException();
+		}
+		// touch each page to fault it in
+		for (int i=0; i<bufferSize; i+= getpagesize()) {
+			*mBuffer[which] = 0;
+		}
+	}
+}
+
+Fifo::~Fifo() {
+	for (int which=0; which<mNumBuffers; which++) {
+		if (mBuffer[which] != NULL) {
+			free(mBuffer[which]);
+			mBuffer[which] = NULL;
+		}
+	}
+}
+
+int Fifo::depth(void) {
+	return mNumBuffers;
+}
+
+int Fifo::numReadToWriteBuffersFilled() {
+	int value;
+	int numFilled = 0;
+	for (int which=0; which<mNumBuffers; which++) {
+		if (sem_getvalue(&mReadToWriteSem[which], &value) == 0) numFilled += value;
+	}
+	return numFilled;
+}
+
+int Fifo::numWriteToReadBuffersFilled() {
+	int value;
+	int numFilled = 0;
+	for (int which=0; which<mNumBuffers; which++) {
+		if (sem_getvalue(&mWriteToReadSem[which], &value) == 0) numFilled += value;
+	}
+	return numFilled;
+}
+
+char* Fifo::start() {
+	sem_wait(&mReadToWriteSem[mWriteCurrent]);
+	return mBuffer[mWriteCurrent];
+}
+
+char* Fifo::write(int length) {
+	mLength[mWriteCurrent] = length;
+	sem_post(&mWriteToReadSem[mWriteCurrent]);
+	mWriteCurrent = (mWriteCurrent + 1) % mNumBuffers;
+	sem_wait(&mReadToWriteSem[mWriteCurrent]);
+	return mBuffer[mWriteCurrent];
+}
+
+char* Fifo::read(int* length) {
+	static bool firstTime = true;
+	if (!firstTime) {
+		sem_post(&mReadToWriteSem[mReadCurrent]);
+	}
+	firstTime = false;
+	mReadCurrent = (mReadCurrent + 1) % mNumBuffers;
+	sem_wait(&mWriteToReadSem[mReadCurrent]);
+	*length = mLength[mReadCurrent];
+	return mBuffer[mReadCurrent];
+}