Me@19: /* Me@19: * Copyright 2009 OpenSourceStewardshipFoundation.org Me@19: * Licensed under GNU General Public License version 2 Me@19: * Me@19: * NOTE: this version of SRSW correct as of April 25, 2010 Me@19: * Me@19: * Author: seanhalle@yahoo.com Me@19: */ Me@19: Me@19: Me@19: #include Me@19: #include Me@19: #include Me@19: #include Me@19: Me@19: #include "PrivateQueue.h" Me@19: Me@19: Me@19: Me@19: //=========================================================================== Me@19: Me@19: /*This kind of queue is private to a single core at a time -- has no Me@19: * synchronizations Me@19: */ Me@19: Me@19: PrivQueueStruc* makePrivQ() Me@19: { Me@19: PrivQueueStruc* retQ; seanhalle@34: //This malloc is not safe to use inside VMS-language! Me@19: retQ = (PrivQueueStruc *) malloc( sizeof( PrivQueueStruc ) ); Me@19: seanhalle@34: //This malloc is not safe to use inside VMS-language! Me@19: retQ->startOfData = malloc( 1024 * sizeof(void *) ); Me@19: memset( retQ->startOfData, 0, 1024 * sizeof(void *) ); Me@19: retQ->extractPos = &(retQ->startOfData[0]); //side by side == empty Me@19: retQ->insertPos = &(retQ->startOfData[1]); // so start pos's have to be Me@19: retQ->endOfData = &(retQ->startOfData[1023]); Me@19: Me@19: return retQ; Me@19: } Me@19: Me@19: Me@19: void Me@19: enlargePrivQ( PrivQueueStruc *Q ) seanhalle@34: { int32 oldSize, newSize; seanhalle@34: int8 *insertPos, *extractPos; seanhalle@34: int8 *oldStartOfData, *oldEndOfData, *newStartOfData, *newEndOfData; seanhalle@34: int8 *insertOffsetBytes, *extractOffsetBytes; Me@19: seanhalle@34: oldStartOfData = (int8 *)Q->startOfData; seanhalle@34: oldEndOfData = (int8 *)Q->endOfData; seanhalle@34: insertPos = (int8 *)Q->insertPos; seanhalle@34: extractPos = (int8 *)Q->extractPos; Me@19: seanhalle@34: //TODO: verify these get number of bytes correct seanhalle@34: insertOffsetBytes = insertPos - oldStartOfData; seanhalle@34: extractOffsetBytes = extractPos - oldStartOfData); seanhalle@34: seanhalle@34: oldSize = endOfData - startOfData + 1; //in bytes seanhalle@34: newSize = 2 * oldSize; seanhalle@34: seanhalle@34: //This malloc is not safe to use inside VMS-language! seanhalle@34: Q->startOfData = (void **)malloc( newSize ); seanhalle@34: newStartOfData = (int8 *)Q->startOfData; seanhalle@34: newEndOfData = newStartOfData + newSize; //all calcs in Bytes seanhalle@34: Q->endOfData = (void **)newEndOfData; seanhalle@34: seanhalle@34: //TODO: test all of this, for both cases seanhalle@34: seanhalle@34: //Moving the data and pointers to the new array is seanhalle@34: //a little trickier than at first it seems.. the top part seanhalle@34: // of old queue must be moved to the top part of new queue, while seanhalle@34: // bottom part of old to bottom part of new, then the new insert seanhalle@34: // and extract positions calculated by offset from top and bottom seanhalle@34: //UNLESS the one case where old extract was at bottom and insert seanhalle@34: // was at top. seanhalle@34: //TODO: check that this is correct! seanhalle@34: if( extractPos == startOfData && insertPos == endOfData ) seanhalle@34: { seanhalle@34: memcpy( newStartOfData, oldStartOfData, oldSize ); //oldSize is bytes seanhalle@34: Q->extractPos = Q->startOfData; //start of valid data seanhalle@34: Q->insertPos = Q->startOfData + oldSize - 1; //end of valid data seanhalle@34: } seanhalle@34: else //have to copy two parts separately, then calc positions seanhalle@34: { //TODO: check end-addr, sizes, and new positions carefully seanhalle@34: seanhalle@34: //copy top part, starting at extract up until end of data, seanhalle@34: // into top of new array seanhalle@34: topPartSize = oldEndOfData - extractPos + 1; //+1 includes extractPos seanhalle@34: copyStartAddr = newEndOfData - topPartSize + 1;//+1 cancels other seanhalle@34: memcpy( copyStartAddr, Q->extractPos, topPartSize ); seanhalle@34: Q->extractPos = (void **)copyStartAddr; //extract just-copied data seanhalle@34: seanhalle@34: //copy bottom part, from old start up to old insert, seanhalle@34: // into bottom of new array seanhalle@34: bottPartSize = oldSize - topPartSize - 1; //-1 for empty insertPos seanhalle@34: memcpy( newStartOfData, oldStartOfData, bottPartSize ); seanhalle@34: Q->insertPos = (void **)(newStartOfData + bottPartSize); seanhalle@34: } seanhalle@34: //This free is not safe to use inside VMS-language! seanhalle@34: free(oldStartOfData); Me@19: } Me@19: Me@19: Me@19: /*Returns NULL when queue is empty Me@19: */ Me@19: void* readPrivQ( PrivQueueStruc* Q ) Me@19: { void *out = 0; Me@19: void **startOfData = Q->startOfData; Me@19: void **endOfData = Q->endOfData; Me@19: Me@19: void **insertPos = Q->insertPos; Me@19: void **extractPos = Q->extractPos; Me@19: Me@19: //if not empty -- (extract is just below insert when empty) Me@19: if( insertPos - extractPos != 1 && Me@19: !(extractPos == endOfData && insertPos == startOfData)) Me@19: { //move before read Me@19: if( extractPos == endOfData ) //write new pos exactly once, correctly Me@19: { Q->extractPos = startOfData; //can't overrun then fix it 'cause Me@19: } // other thread might read bad pos Me@19: else Me@19: { Q->extractPos++; Me@19: } Me@19: out = *(Q->extractPos); Me@19: return out; Me@19: } Me@19: //Q is empty Me@19: return NULL; Me@19: } Me@19: Me@19: Me@19: /*Expands the queue size automatically when it's full Me@19: */ Me@19: void Me@19: writePrivQ( void * in, PrivQueueStruc* Q ) Me@19: { Me@19: void **startOfData = Q->startOfData; Me@19: void **endOfData = Q->endOfData; Me@19: Me@19: void **insertPos = Q->insertPos; Me@19: void **extractPos = Q->extractPos; Me@19: Me@19: tryAgain: Me@19: //Full? (insert is just below extract when full) Me@19: if( extractPos - insertPos != 1 && Me@19: !(insertPos == endOfData && extractPos == startOfData)) Me@19: { *(Q->insertPos) = in; //insert before move Me@19: if( insertPos == endOfData ) //write new pos exactly once, correctly Me@19: { Q->insertPos = startOfData; Me@19: } Me@19: else Me@19: { Q->insertPos++; Me@19: } Me@19: return; Me@19: } Me@19: //Q is full Me@19: enlargePrivQ( Q ); Me@19: goto tryAgain; Me@19: } Me@19: Me@19: Me@19: /*Returns false when the queue was full. Me@19: * have option of calling make_larger_PrivQ to make more room, then try again Me@19: */ Me@19: int writeIfSpacePrivQ( void * in, PrivQueueStruc* Q ) Me@19: { Me@19: void **startOfData = Q->startOfData; Me@19: void **endOfData = Q->endOfData; Me@19: Me@19: void **insertPos = Q->insertPos; Me@19: void **extractPos = Q->extractPos; Me@19: Me@19: if( extractPos - insertPos != 1 && Me@19: !(insertPos == endOfData && extractPos == startOfData)) Me@19: { *(Q->insertPos) = in; //insert before move Me@19: if( insertPos == endOfData ) //write new pos exactly once, correctly Me@19: { Q->insertPos = startOfData; Me@19: } Me@19: else Me@19: { Q->insertPos++; Me@19: } Me@19: return TRUE; Me@19: } Me@19: //Q is full Me@19: return FALSE; Me@19: } seanhalle@34: seanhalle@34: int32 seanhalle@34: numInPrivQ( PrivQueueStruc *Q ) seanhalle@34: { int32 size, numIn; seanhalle@34: seanhalle@34: if( Q->insertPos < Q->extractPos ) seanhalle@34: { //insert has wrapped around so numIn is: seanhalle@34: // insertPos + size - extractPos -- Consider, is empty when seanhalle@34: // extractPos = endOfData and insert = start -- correctly get zero seanhalle@34: size = Q->endOfData - Q->startOfData + 1; //sz of 10 is 0..9 seanhalle@34: numIn = Q->insertPos - Q->extractPos + size - 1; //-1 bec insrt empty seanhalle@34: } seanhalle@34: else seanhalle@34: { seanhalle@34: numIn = Q->insertPos - Q->extractPos -1;//-1 bec insertPos empty seanhalle@34: } seanhalle@34: return numIn; seanhalle@34: } seanhalle@34: seanhalle@34: seanhalle@34: /*Treats queue as a stack -- no matter contents, if read done right after seanhalle@34: * a push, then the pushed item is what comes out. seanhalle@34: * Expands the queue size automatically when it's full. seanhalle@34: */ seanhalle@34: void seanhalle@34: pushPrivQ( void * in, PrivQueueStruc* Q ) seanhalle@34: { seanhalle@34: while(1){ seanhalle@34: void **startOfData = Q->startOfData; seanhalle@34: void **endOfData = Q->endOfData; seanhalle@34: seanhalle@34: void **insertPos = Q->insertPos; seanhalle@34: void **extractPos = Q->extractPos; seanhalle@34: seanhalle@34: //Full? (insert is just below extract when full) seanhalle@34: if( extractPos - insertPos != 1 && seanhalle@34: !(insertPos == endOfData && extractPos == startOfData)) seanhalle@34: { //insert -- but go backwards, inserting at read position then seanhalle@34: // move read pos backwards seanhalle@34: *(Q->extractPos) = in; seanhalle@34: if( extractPos == startOfData ) //write new pos exactly once, correctly seanhalle@34: { Q->extractPos = endOfData; //can't overrun then fix it 'cause seanhalle@34: } // other thread might read bad pos seanhalle@34: else seanhalle@34: { Q->extractPos--; seanhalle@34: } seanhalle@34: return; seanhalle@34: } seanhalle@34: //Q is full seanhalle@34: enlargePrivQ( Q ); seanhalle@34: } seanhalle@34: } seanhalle@34: seanhalle@34: seanhalle@34: void seanhalle@34: freePrivQ( PrivQueueStruc *Q ) seanhalle@34: { seanhalle@34: //This free is not safe to use inside VMS-language! seanhalle@34: free( Q->startOfData ); seanhalle@34: free( Q ); seanhalle@34: }