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
|
/**
* 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];
}
|
