blob: f672e92a6807403c43274ad3705cc16a2dd2aee6 (
plain) (
blame)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
|
/**
* Copyright (C) ARM Limited 2010-2014. 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.
*/
#include "Fifo.h"
#include <stdlib.h>
#ifdef WIN32
#define valloc malloc
#endif
#include "Logging.h"
// bufferSize is the amount of data to be filled
// singleBufferSize is the maximum size that may be filled during a single write
// (bufferSize + singleBufferSize) will be allocated
Fifo::Fifo(int singleBufferSize, int bufferSize, sem_t* readerSem) {
mWrite = mRead = mReadCommit = mRaggedEnd = 0;
mWrapThreshold = bufferSize;
mSingleBufferSize = singleBufferSize;
mReaderSem = readerSem;
mBuffer = (char*)valloc(bufferSize + singleBufferSize);
mEnd = false;
if (mBuffer == NULL) {
logg->logError(__FILE__, __LINE__, "failed to allocate %d bytes", bufferSize + singleBufferSize);
handleException();
}
if (sem_init(&mWaitForSpaceSem, 0, 0)) {
logg->logError(__FILE__, __LINE__, "sem_init() failed");
handleException();
}
}
Fifo::~Fifo() {
free(mBuffer);
sem_destroy(&mWaitForSpaceSem);
}
int Fifo::numBytesFilled() const {
return mWrite - mRead + mRaggedEnd;
}
char* Fifo::start() const {
return mBuffer;
}
bool Fifo::isEmpty() const {
return mRead == mWrite && mRaggedEnd == 0;
}
bool Fifo::isFull() const {
return willFill(0);
}
// Determines if the buffer will fill assuming 'additional' bytes will be added to the buffer
// 'full' means there is less than singleBufferSize bytes available contiguously; it does not mean there are zero bytes available
bool Fifo::willFill(int additional) const {
if (mWrite > mRead) {
if (numBytesFilled() + additional < mWrapThreshold) {
return false;
}
} else {
if (numBytesFilled() + additional < mWrapThreshold - mSingleBufferSize) {
return false;
}
}
return true;
}
// This function will stall until contiguous singleBufferSize bytes are available
char* Fifo::write(int length) {
if (length <= 0) {
length = 0;
mEnd = true;
}
// update the write pointer
mWrite += length;
// handle the wrap-around
if (mWrite >= mWrapThreshold) {
mRaggedEnd = mWrite;
mWrite = 0;
}
// send a notification that data is ready
sem_post(mReaderSem);
// wait for space
while (isFull()) {
sem_wait(&mWaitForSpaceSem);
}
return &mBuffer[mWrite];
}
void Fifo::release() {
// update the read pointer now that the data has been handled
mRead = mReadCommit;
// handle the wrap-around
if (mRead >= mWrapThreshold) {
mRaggedEnd = mRead = mReadCommit = 0;
}
// send a notification that data is free (space is available)
sem_post(&mWaitForSpaceSem);
}
// This function will return null if no data is available
char* Fifo::read(int *const length) {
// wait for data
if (isEmpty() && !mEnd) {
return NULL;
}
// obtain the length
do {
mReadCommit = mRaggedEnd ? mRaggedEnd : mWrite;
*length = mReadCommit - mRead;
} while (*length < 0); // plugs race condition without using semaphores
return &mBuffer[mRead];
}
|
