daemon/Fifo.cpp

   1 /**
   2  * Copyright (C) ARM Limited 2010-2012. All rights reserved.
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License version 2 as
   6  * published by the Free Software Foundation.
   7  */
   8
   9 #include <stdlib.h>
  10 #include <string.h>
  11 #include <unistd.h>
  12 #include "Fifo.h"
  13 #include "Logging.h"
  14
  15 extern void handleException();
  16
  17 Fifo::Fifo(int numBuffers, int bufferSize) {
  18         int     which;
  19
  20         if (numBuffers > FIFO_BUFFER_LIMIT) {
  21                 logg->logError(__FILE__, __LINE__, "Number of fifo buffers exceeds maximum");
  22                 handleException();
  23         }
  24         mNumBuffers = numBuffers;
  25         mBufferSize = bufferSize;
  26         mWriteCurrent = 0;
  27         mReadCurrent = mNumBuffers - 1; // (n-1) pipelined
  28
  29         for (which=0; which<mNumBuffers; which++) {
  30                 // initialized read-to-write sem to 1, so that first wait goes through; write-to-read init'd to 0
  31                 if (sem_init(&mReadToWriteSem[which], 0, 1) ||
  32                         sem_init(&mWriteToReadSem[which], 0, 0)) {
  33                         logg->logError(__FILE__, __LINE__, "sem_init(%d) failed", which);
  34                         handleException();
  35                 }
  36                 // page-align allocate buffers
  37                 mBuffer[which] = (char*)valloc(bufferSize);
  38                 if (mBuffer[which] == NULL) {
  39                         logg->logError(__FILE__, __LINE__, "failed to allocate %d bytes", bufferSize);
  40                         handleException();
  41                 }
  42                 // touch each page to fault it in
  43                 for (int i=0; i<bufferSize; i+= getpagesize()) {
  44                         *mBuffer[which] = 0;
  45                 }
  46         }
  47 }
  48
  49 Fifo::~Fifo() {
  50         for (int which=0; which<mNumBuffers; which++) {
  51                 if (mBuffer[which] != NULL) {
  52                         free(mBuffer[which]);
  53                         mBuffer[which] = NULL;
  54                 }
  55         }
  56 }
  57
  58 int Fifo::depth(void) {
  59         return mNumBuffers;
  60 }
  61
  62 int Fifo::numReadToWriteBuffersFilled() {
  63         int value;
  64         int numFilled = 0;
  65         for (int which=0; which<mNumBuffers; which++) {
  66                 if (sem_getvalue(&mReadToWriteSem[which], &value) == 0) numFilled += value;
  67         }
  68         return numFilled;
  69 }
  70
  71 int Fifo::numWriteToReadBuffersFilled() {
  72         int value;
  73         int numFilled = 0;
  74         for (int which=0; which<mNumBuffers; which++) {
  75                 if (sem_getvalue(&mWriteToReadSem[which], &value) == 0) numFilled += value;
  76         }
  77         return numFilled;
  78 }
  79
  80 char* Fifo::start() {
  81         sem_wait(&mReadToWriteSem[mWriteCurrent]);
  82         return mBuffer[mWriteCurrent];
  83 }
  84
  85 char* Fifo::write(int length) {
  86         mLength[mWriteCurrent] = length;
  87         sem_post(&mWriteToReadSem[mWriteCurrent]);
  88         mWriteCurrent = (mWriteCurrent + 1) % mNumBuffers;
  89         sem_wait(&mReadToWriteSem[mWriteCurrent]);
  90         return mBuffer[mWriteCurrent];
  91 }
  92
  93 char* Fifo::read(int* length) {
  94         static bool firstTime = true;
  95         if (!firstTime) {
  96                 sem_post(&mReadToWriteSem[mReadCurrent]);
  97         }
  98         firstTime = false;
  99         mReadCurrent = (mReadCurrent + 1) % mNumBuffers;
 100         sem_wait(&mWriteToReadSem[mReadCurrent]);
 101         *length = mLength[mReadCurrent];
 102         return mBuffer[mReadCurrent];
 103 }