/* For free support for VSIDE, please visit www.vsdsp-forum.com */

/* FiFoBuf implementation */

#include <vo_stdio.h>
#include <string.h>
#include <kernel.h>

#include <math.h>
#include <ringbuf.h>
#include <sysmemory.h>

#include <mutex.h>

#include "fifobuf.h"

void FifoDoRead(register struct fifoBuf *fifo, union fifoPointer dest,
		     register u_int16 words, register u_int16 readToY) {
	if (words == 0) {
		return;
	}
	if (readToY) {
		if (fifo->flags & FIFO_BUFFER_IN_Y) {
			fifo->rd.y =
				RING_YY_S(ringcpyYY(dest.y, 1, fifo->rd.y, fifo->modifier, words));
		} else {
			fifo->rd.x =
				RING_XY_S(ringcpyXY(dest.y, 1, fifo->rd.x, fifo->modifier, words));
		}
	} else {
		if (fifo->flags & FIFO_BUFFER_IN_Y) {
			fifo->rd.y =
				RING_YX_S(ringcpyYX(dest.x, 1, fifo->rd.y, fifo->modifier, words));
		} else {
			fifo->rd.x =
				RING_XX_S(ringcpyXX(dest.x, 1, fifo->rd.x, fifo->modifier, words));
		}
	}
}

u_int16 FifoReadSome(register struct fifoBuf *fifo, union fifoPointer dest,
		     register u_int16 words, register u_int16 readToY) {
	u_int16 frags = words % fifo->elementSize, uf = 1; /* VSDSP is happy to /0 */
	if ((fifo->flags & FIFO_READ_SIZE_ONLY) &&
	    fifo->elementSize && frags)
	{
		return 0;
	}
	if (fifo->elementSize) {
		words -= frags;
	}
	if (words == 0) {
		return 0;
	}
	if (fifo->flags & FIFO_READ_NON_BLOCK) {
		u_int16 toRead;
		if (AttemptMutex(&(fifo->mutex))) {
			/* No mutex available */
			return 0;
		}
		toRead = FifoFill(fifo);
		if (toRead  < words) {
			fifo->underflows++;
			if (fifo->flags & FIFO_READ_PARTIAL) {
				if (fifo->elementSize) {
					toRead = toRead / fifo->elementSize * fifo->elementSize;
				}
				/* Else whatever there is */
			} else {
				ReleaseMutex(&(fifo->mutex));
				return 0;
			}
		} else {
			/* More than enough */
			toRead = words;
		}
		FifoDoRead(fifo, dest, toRead, readToY);
		ReleaseMutex(&(fifo->mutex));
		return toRead;
	} else {
		u_int16 left = words, toRead;
		while (left) {
			ObtainMutex(&(fifo->mutex));
			toRead = FifoFill(fifo);
			if (toRead == 0) {
			retry:
				ReleaseMutex(&(fifo->mutex));
				Delay(1);
				continue;
			} else if (toRead  < left) {
				fifo->underflows += uf;
				uf = 0;
				if (fifo->flags & FIFO_READ_PARTIAL) {
					if (fifo->elementSize) {
						toRead = toRead / fifo->elementSize * fifo->elementSize;
					}
					/* Else read whatever is available */
				} else {
					goto retry;
				}
			} else {
				toRead = left;
			}
			FifoDoRead(fifo, dest, toRead, readToY);
			ReleaseMutex(&(fifo->mutex));
			left -= toRead;
			dest.u += toRead;
		}
		return words;
	}
}
u_int16 FifoReadY(register struct fifoBuf *fifo, register __mem_y u_int16 *dest, register u_int16 words) {
	union fifoPointer d;
	d.y = dest;
	return FifoReadSome(fifo, d, words, -1);
}
u_int16 FifoRead(register struct fifoBuf *fifo, register u_int16 *dest, register u_int16 words) {
	union fifoPointer d;
	d.x = dest;
	return FifoReadSome(fifo, d, words, 0);
}
void FifoDoWrite(register struct fifoBuf *fifo, union fifoPointer source,
		      register u_int16 words, register u_int16 writeToY)
{
	if (words == 0) {
		return;
	}
	if (writeToY) {
		if (fifo->flags & FIFO_BUFFER_IN_Y) {
			fifo->wr.y =
				RING_YY_D(ringcpyYY(fifo->wr.y, fifo->modifier, source.y, 1, words));
		} else {
			fifo->wr.x =
				RING_YX_D(ringcpyYX(fifo->wr.x, fifo->modifier, source.y, 1, words));
		}
	} else {
		if (fifo->flags & FIFO_BUFFER_IN_Y) {
			fifo->wr.y =
				RING_XY_D(ringcpyXY(fifo->wr.y, fifo->modifier, source.x, 1, words));
		} else {
			fifo->wr.x =
				RING_XX_D(ringcpyXX(fifo->wr.x, fifo->modifier, source.x, 1, words));
		}
	}
}
void PushReadPointer(register struct fifoBuf *fifo, register u_int16 atLeastWords) {
	u_int16 advance;
	if (fifo->elementSize) {
		advance = (atLeastWords + fifo->elementSize - 1) / fifo->elementSize;
		advance *= fifo->elementSize;
	} else {
		advance = atLeastWords;
	}
	fifo->rd.u = fifo->buf.u +
		((fifo->rd.u - fifo->buf.u + advance) % fifo->bufSize);
}
u_int16 FifoWriteSome(register struct fifoBuf *fifo, union fifoPointer source,
		      register u_int16 words, register u_int16 writeToY) {
	u_int16 frags = words % fifo->elementSize, of = 1;
	if ((fifo->flags & FIFO_WRITE_SIZE_ONLY) &&
	      fifo->elementSize && frags)
	{
		return 0;
	}
	if (fifo->elementSize) {
		words -= frags;
	}
	/* Nothing or too impossible */
	if (words == 0 || words >= fifo->bufSize) {
		return 0;
	}
	if (fifo->flags & FIFO_WRITE_NON_BLOCK) {
		u_int16 toWrite;
		if (AttemptMutex(&(fifo->mutex))) {
			return 0;
		}
		toWrite = FifoFree(fifo);
		if (words > toWrite) {
			fifo->overflows ++;
			if (fifo->flags & FIFO_WRITE_OVER) {
				PushReadPointer(fifo, words - toWrite );
				toWrite = words;
			} else if (fifo->flags & FIFO_WRITE_PARTIAL) {
				if (fifo->elementSize) {
					toWrite = toWrite / fifo->elementSize;
					toWrite *= fifo->elementSize;
				}
			} else {
				/* Can't write now */
				ReleaseMutex(&(fifo->mutex));
				return 0;
			}
		} else {
			/* More than enough space */
			toWrite = words;
		}
		FifoDoWrite(fifo, source, toWrite, writeToY);
		ReleaseMutex(&(fifo->mutex));
		return toWrite;
	} else { /* Blocking mode */
		u_int16 left = words, toWrite;
		while (left) {
			ObtainMutex(&(fifo->mutex));
		retry:
			toWrite = FifoFree(fifo);
			if (left > toWrite) {
				fifo->overflows += of;
				of = 0; /* Write overflows once. */;
				if (fifo->flags & FIFO_WRITE_OVER) {
					u_int16 advance = 1;
					/* Push read further to make some room. */
					if (fifo->flags & FIFO_WRITE_PARTIAL) {
						if (fifo->elementSize > 1) {
							advance = fifo->elementSize;
							toWrite += advance;
							if (toWrite > left) {
								toWrite = left;
							}
							/* Advance is larger than 1 elementSize */
							toWrite = toWrite / advance;
							toWrite *= advance; /* 1 or 2 elements */
						} else {
							toWrite++; /* Max == left */
						}
						PushReadPointer(fifo, advance);
					} else {
						PushReadPointer(fifo, left - toWrite );
						/* Now there is enough space for everything */
						printf("#OW\n");
						goto retry;
					}
				} else if (fifo->flags & FIFO_WRITE_PARTIAL) {
					if (fifo->elementSize) {
						toWrite = toWrite / fifo->elementSize;
						toWrite *= fifo->elementSize;
					}
					/* Else write whatever space there may be. */
				}
			} else {
				/* More than enough space */
				toWrite = left;
			}

			FifoDoWrite(fifo, source, toWrite, writeToY);
			ReleaseMutex(&(fifo->mutex));
			left -= toWrite;
			source.u += toWrite;

			if (left) {
				/* Allow reader to read */
				Delay(1);
			}
		}
		return words;
	}
}
u_int16 FifoWriteY(register struct fifoBuf *fifo, register __mem_y u_int16 *source, register u_int16 words) {
	union fifoPointer s;
	s.y = source;
	return FifoWriteSome(fifo, s, words, -1);
}
/* write words to fifo from X-mem source if there is space in the fifo, return written words */
u_int16 FifoWrite(register struct fifoBuf *fifo, register u_int16 *source, register u_int16 words) {
	union fifoPointer s;
	s.x = source;
	return FifoWriteSome(fifo, s, words, 0);
}
/* Status functions */
u_int16 FifoFill(register struct fifoBuf *fifo) {
	s_int16 fill = fifo->wr.u - fifo->rd.u;
	if (fill < 0) {
		fill += fifo->bufSize;
	}
	return (u_int16)fill;
}
u_int16 FifoFree(register struct fifoBuf *fifo) {
	s_int16 avail = fifo->rd.u - fifo->wr.u;
	if (avail <= 0) {
		avail += fifo->bufSize;
	}
	avail--;
	return (u_int16) avail;
}
/* Blocking access */
/* Get word from fifo. Delay(1) until word is available. */
u_int16 FifoGet(register struct fifoBuf *fifo) {
	u_int16 data;
	while (!FifoFill(fifo)) {
		Delay(1);
	}
	if (fifo->flags & FIFO_BUFFER_IN_Y) {
		data = *(fifo->rd.y);
		fifo->rd.y = ringmodY(fifo->rd.y, fifo->modifier);
	} else {
		data = *(fifo->rd.x);
		fifo->rd.x = ringmodX(fifo->rd.x, fifo->modifier);
	}
}
/* Put data to fifo. Delay(1) until space for data is available. */
void FifoPut(register struct fifoBuf *fifo, register u_int16 data) {
	while(!FifoFree(fifo)) {
		Delay(1);
	}
	if (fifo->flags & FIFO_BUFFER_IN_Y) {
		*(fifo->wr.y) = data;
		fifo->wr.y = ringmodY(fifo->wr.y, fifo->modifier);
	} else {
		*(fifo->wr.x) = data;
		fifo->wr.x = ringmodX(fifo->wr.x, fifo->modifier);
	}
}
/* if fifo NULL, create fifo, realloc buffer to size, 0 to free, useY selects buffer memory */
struct fifoBuf *FifoRealloc(register struct fifoBuf *fifo, register u_int16 size, register u_int16 useY) {
	if (fifo) {
		if (size) {
			u_int16 memChange = 0;
			if (useY) {
				memChange = FIFO_BUFFER_IN_Y;
			}
			memChange ^= fifo->flags & FIFO_BUFFER_IN_Y;
			if ((size == fifo->bufSize) && (memChange == 0)) {
				/* No change to memory */
				return fifo;
			}
			if (fifo->flags & FIFO_BUFFER_IN_Y) {
				FreeMemY(fifo->buf.y, fifo->bufSize);
			} else {
				FreeMemX(fifo->buf.x, fifo->bufSize);
			}
		allocate_buf:
			fifo->bufSize = size;
			size = CeilPow2(size); /* Alignment */
			if (useY) {
				fifo->flags |= FIFO_BUFFER_IN_Y;
				fifo->buf.y = AllocMemY(fifo->bufSize, size);
			} else {
				fifo->flags &= ~FIFO_BUFFER_IN_Y;
				fifo->buf.x = AllocMemX(fifo->bufSize, size);
			}
			/* Reset write and read pointers */
			fifo->wr.u = fifo->buf.u;
			fifo->rd.u = fifo->buf.u;
			if (fifo->buf.u == 0) {
				/* Allocation returned NULL pointer */
				goto release_struct;
			}
			fifo->modifier = MAKEMODF(fifo->bufSize);
			return fifo;
		} else {
			if (fifo->flags & FIFO_BUFFER_IN_Y) {
				FreeMemY(fifo->buf.y, fifo->bufSize);
			} else {
				FreeMemX(fifo->buf.x, fifo->bufSize);
			}
		release_struct:
			fifo->rd.u = 0;
			fifo->rd.u = 0;
			fifo->buf.u = 0;
			free(fifo);
			return NULL;
		}
	} else {
		if (size == 0) {
			return NULL;
		}
		fifo = malloc(sizeof(struct fifoBuf));
		if (fifo) {
			memset(fifo, 0, sizeof(struct fifoBuf));
			InitMutex(&(fifo->mutex));
			goto allocate_buf;
		}
		return NULL;
	}
}

u_int16 FifoFeatures(register struct fifoBuf *fifo, register u_int16 *pUserFlags, register u_int16 *pSize) {
	u_int16 flags = fifo->flags;
	/* Getter */
	if ((pUserFlags == NULL) && (pSize == NULL)) {
		return flags & FIFO_USER_MASK;
	}
	if (pUserFlags) {
		flags &= ~FIFO_USER_MASK;
		flags |= (FIFO_USER_MASK & *pUserFlags);
	}
	ObtainMutex(&(fifo->mutex));
	fifo->flags = flags;
	if (pSize) {
		fifo->elementSize = *pSize;
	}
	ReleaseMutex(&(fifo->mutex));
	return flags & FIFO_USER_MASK;
}
