int mypthread_create(mypthread_t *thread, const mypthread_attr_t *attr,
void *(*start_routine) (void *), void *arg)
{
if (count == MAXTHREADS)
{
printf("Maximum number of threads reached: %d\n", MAXTHREADS);
return -1;
}
static int firstRun = 1;
if (firstRun)
{
initThreads();
firstRun = 0;
mainSetup = 0;
}
int unused = getUnused();
if (getcontext(&(allThreads[unused].context)) != 0) {
perror("Error getting context.\n");
return -1;
}
allThreads[unused].context.uc_stack.ss_sp = malloc(STACKSIZE);
allThreads[unused].context.uc_stack.ss_size = STACKSIZE;
makecontext(&(allThreads[unused].context), (void*) start_routine, 1, arg);
allThreads[unused].status = READY;
*thread = allThreads[unused];
count++;
return 0;
}
void BZ2Stream::read(void* ptr, size_t size) {
BZ2_bzRead(&bzerror_, bzfile_, ptr, size);
advanceOffset(size);
switch (bzerror_) {
case BZ_OK: return;
case BZ_STREAM_END:
if (getUnused() || getUnusedLength() > 0)
ROS_ERROR("unused data already available");
else {
char* unused;
int nUnused;
BZ2_bzReadGetUnused(&bzerror_, bzfile_, (void**) &unused, &nUnused);
setUnused(unused);
setUnusedLength(nUnused);
}
return;
case BZ_IO_ERROR: throw BagIOException("BZ_IO_ERROR: error reading from compressed stream"); break;
case BZ_UNEXPECTED_EOF: throw BagIOException("BZ_UNEXPECTED_EOF: compressed stream ended before logical end-of-stream detected"); break;
case BZ_DATA_ERROR: throw BagIOException("BZ_DATA_ERROR: data integrity error detected in compressed stream"); break;
case BZ_DATA_ERROR_MAGIC: throw BagIOException("BZ_DATA_ERROR_MAGIC: stream does not begin with requisite header bytes"); break;
case BZ_MEM_ERROR: throw BagIOException("BZ_MEM_ERROR: insufficient memory available"); break;
}
}
void BZ2Stream::startRead() {
bzfile_ = BZ2_bzReadOpen(&bzerror_, getFilePointer(), verbosity_, 0, getUnused(), getUnusedLength());
switch (bzerror_) {
case BZ_OK: break;
default: {
BZ2_bzReadClose(&bzerror_, bzfile_);
throw BagException("Error opening file for reading compressed stream");
}
}
clearUnused();
}
int mypthread_yield(void)
{
if (!mainSetup)
{
int unused = getUnused();
if (getcontext(&(allThreads[unused].context)) != 0)
{
printf("Error getting context.\n");
exit(0);
}
allThreads[unused].context.uc_stack.ss_sp = malloc(STACKSIZE);
allThreads[unused].context.uc_stack.ss_size = STACKSIZE;
allThreads[unused].status = READY;
allThreads[threadIndex].status = RUNNING;
mainSetup = 1;
swapcontext(&(allThreads[unused].context), &(allThreads[threadIndex].context));
return 0;
}
allThreads[threadIndex].status = READY;
int next = getReady();
if (next < 0)
{
int waiting = getWaiting();
if (waiting < 0)
{
printf("No waiting threads found.\n");
exit(0);
}
next = allThreads[waiting].index;
}
int old = threadIndex;
threadIndex = next;
allThreads[threadIndex].status = RUNNING;
swapcontext(&(allThreads[old].context), &(allThreads[threadIndex].context));
return 0;
}
void UncompressedStream::read(void* ptr, size_t size) {
size_t nUnused = (size_t) getUnusedLength();
char* unused = getUnused();
if (nUnused > 0) {
// We have unused data from the last compressed read
if (nUnused == size) {
// Copy the unused data into the buffer
memcpy(ptr, unused, nUnused);
clearUnused();
}
else if (nUnused < size) {
// Copy the unused data into the buffer
memcpy(ptr, unused, nUnused);
// Still have data to read
size -= nUnused;
// Read the remaining data from the file
int result = fread((char*) ptr + nUnused, 1, size, getFilePointer());
if ((size_t) result != size)
throw BagIOException((format("Error reading from file + unused: wanted %1% bytes, read %2% bytes") % size % result).str());
advanceOffset(size);
clearUnused();
}
else {
// nUnused_ > size
memcpy(ptr, unused, size);
setUnused(unused + size);
setUnusedLength(nUnused - size);
}
}
// No unused data - read from stream
int result = fread(ptr, 1, size, getFilePointer());
if ((size_t) result != size)
throw BagIOException((format("Error reading from file: wanted %1% bytes, read %2% bytes") % size % result).str());
advanceOffset(size);
}
int mypthread_join(mypthread_t thread, void **retval)
{
if (!mainSetup)
{
int unused = getUnused();
if (getcontext(&(allThreads[unused].context)) != 0)
{
printf("Error getting context.\n");
exit(0);
}
allThreads[unused].context.uc_stack.ss_sp = malloc(STACKSIZE);
allThreads[unused].context.uc_stack.ss_size = STACKSIZE;
allThreads[thread.index].beingJoinedOnBy = allThreads[unused].index;
threadIndex = thread.index;
allThreads[threadIndex].status = RUNNING;
allThreads[unused].status = WAITING;
if (retval)
{
printf("retval detected in join call, setting return address in thread struct\n");
allThreads[threadIndex].retadd = retval;
printf("retadd set to %p\n", allThreads[threadIndex].retadd);
}
mainSetup = 1;
swapcontext(&(allThreads[unused].context), &(allThreads[threadIndex].context));
return 0;
}
if (threadIndex == thread.index)
{
printf("A thread cannot join on itself.\n");
return -1;
}
if (allThreads[thread.index].beingJoinedOnBy > -1)
{
printf("Thread is already being joined on.\n");
return -1;
}
if (retval)
{
printf("retval detected in join call (not in main setup), setting return address in thread struct\n");
allThreads[thread.index].retadd = retval;
printf("retadd set to %p\n", allThreads[threadIndex].retadd);
}
if (allThreads[threadIndex].status == EXITED)
{
allThreads[threadIndex].status = UNUSED;
free(allThreads[threadIndex].context.uc_stack.ss_sp);
allThreads[threadIndex].beingJoinedOnBy = -1;
count--;
return 0;
}
allThreads[threadIndex].status = WAITING;
allThreads[thread.index].beingJoinedOnBy = threadIndex;
int next = getReady();
if (next < 0)
{
int waiting = getWaiting();
if (waiting < 0)
{
printf("No waiting threads found.\n");
exit(0);
}
next = allThreads[waiting].index;
}
int old = threadIndex;
threadIndex = next;
allThreads[threadIndex].status = RUNNING;
swapcontext(&(allThreads[old].context), &(allThreads[threadIndex].context));
return 0;
}
