Subversion Repositories s6soc

////////////////////////////////////////////////////////////////////////////////
//
// Filename: 	syspipe.c
//
// Project:	CMod S6 System on a Chip, ZipCPU demonstration project
//
// Purpose:	This "device" handles the primary device level interaction of
//		almost all devices on the ZipOS: the pipe.  A pipe, as defined
//	here, is an O/S supported FIFO.  Information written to the FIFO will
//	be read from the FIFO in the order it was received.  Attempts to read
//	from an empty FIFO, or equivalently to write to a full FIFO, will block
//	the reading (writing) process until memory is available.
//
// Creator:	Dan Gisselquist, Ph.D.
//		Gisselquist Technology, LLC
//
////////////////////////////////////////////////////////////////////////////////
//
// Copyright (C) 2015-2016, Gisselquist Technology, LLC
//
// This program is free software (firmware): you can redistribute it and/or
// modify it under the terms of  the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License, or (at
// your option) any later version.
//
// This program is distributed in the hope that it will be useful, but WITHOUT
// ANY WARRANTY; without even the implied warranty of MERCHANTIBILITY or
// FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
// for more details.
//
// You should have received a copy of the GNU General Public License along
// with this program.  (It's in the $(ROOT)/doc directory, run make with no
// target there if the PDF file isn't present.)  If not, see
// <http://www.gnu.org/licenses/> for a copy.
//
// License:	GPL, v3, as defined and found on www.gnu.org,
//		http://www.gnu.org/licenses/gpl.html
//
//
////////////////////////////////////////////////////////////////////////////////
//
//
#include "errno.h"
#include "board.h"
#include "taskp.h"
#include "syspipe.h"
#include "zipsys.h"
#include "ktraps.h"
 
#ifndef	NULL
#define	NULL	(void *)0
#endif
 
static	void	clear_syspipe(SYSPIPE *p) {
	p->m_head  = 0;
	p->m_tail  = 0;
	p->m_error = 0;
 
	for(int i=0; i<=(int)p->m_mask; i++)
		p->m_buf[i] = 0;
 
	if ((p->m_rdtask)&&(p->m_rdtask != INTERRUPT_READ_TASK)) {
		p->m_rdtask->context[1] = p->m_nread;
		if (p->m_nread == 0)
			p->m_rdtask->errno = -EFAULT;
		p->m_rdtask->state = SCHED_READY;
	} else if (p->m_wrtask) {
		p->m_wrtask->context[1] = p->m_nwritten;
		if (p->m_nwritten == 0)
			p->m_wrtask->errno = -EFAULT;
		p->m_wrtask->state = SCHED_READY;
	}
 
	if (p->m_rdtask != INTERRUPT_READ_TASK)
		p->m_rdtask   = 0;
	p->m_wrtask   = 0;
	p->m_nread    = 0;
	p->m_nwritten = 0;
}
 
void	kpush_syspipe(SYSPIPE *pipe, int val) {
	int	tst = (pipe->m_head+1)&pipe->m_mask;
	if (tst != pipe->m_tail) {
		pipe->m_buf[pipe->m_head] = val;
		pipe->m_head = tst;		// Increment the head pointer
		if ((pipe->m_rdtask)&&(pipe->m_rdtask != INTERRUPT_READ_TASK))
			pipe->m_rdtask->state = SCHED_READY;
	} else pipe->m_error = 1;
}
 
void	txchr(char v) {
	volatile IOSPACE	*sys = (IOSPACE *)IOADDR;
	if (v < 10)
		return;
	v &= 0x0ff;
	sys->io_pic = INT_UARTTX;
	while((sys->io_pic&INT_UARTTX)==0)
		;
	sys->io_uart = v;
}
 
void	txstr(const char *str) {
	const char *ptr = str;
	while(*ptr) {
		txchr(*ptr++);
	}
}
 
void	txhex(int num) {
	for(int ds=28; ds>=0; ds-=4) {
		int	ch;
		ch = (num>>ds)&0x0f;
		if (ch >= 10)
			ch = 'A'+ch-10;
		else
			ch += '0';
		txchr(ch);
	} txstr("\r\n");
}
 
void	pipe_panic(SYSPIPE *pipe) {
	extern void	kpanic(void);
	volatile IOSPACE	*sys = (IOSPACE *)IOADDR;
 
	sys->io_spio = 0x0fa;
 
	txstr("SYSPIPE PANIC!\r\n");
	txstr("ADDR: "); txhex((int)pipe);
	txstr("MASK: "); txhex(pipe->m_mask);
	txstr("HEAD: "); txhex(pipe->m_head);
	txstr("TAIL: "); txhex(pipe->m_tail);
	kpanic();
}
 
int	kpop_syspipe(SYSPIPE *pipe, int *vl) {
	if (pipe->m_head != pipe->m_tail) {
		*vl = pipe->m_buf[pipe->m_tail];
		pipe->m_tail++;
		if ((unsigned)pipe->m_tail > pipe->m_mask)
			pipe->m_tail = 0;
		if (pipe->m_wrtask)
			pipe->m_wrtask->state = SCHED_READY;
		return 0;
	} return 1; // Error condition
}
 
SYSPIPE *new_syspipe(const unsigned int len) {
	unsigned	msk;
 
	for(msk=2; msk<len; msk<<=1)
		;
	SYSPIPE *pipe = sys_malloc(sizeof(SYSPIPE)-1+msk);
	pipe->m_mask = msk-1;
	pipe->m_rdtask = pipe->m_wrtask = 0;
	clear_syspipe(pipe);
	return pipe;
}
 
int	len_syspipe(SYSPIPE *p) {
	return (p->m_head-p->m_tail) & p->m_mask;
}
int	num_avail_syspipe(SYSPIPE *p) {
	return (p->m_mask + p->m_tail-p->m_head) & p->m_mask;
}
 
// This will be called from a user context.
// Another task may write to the pipe during this call.  If the pipe becomes
// full, that task will block.
//
static int	uread_syspipe(TASKP tsk __attribute__((__unused__)), SYSPIPE *p, int *dst, int len) {
	int nleft= len, h;
	if (len == 0) {
		// We'll only get here if we were released from within a 
		// writing task.
		return p->m_nread;
	} else do {
		// We have a valid read request, for a new process.  Continue
		// 'reading' until we have fulfilled the request.
		//
		// We can read from head, just not write to it
		// As for the tail pointer -- we own it, no one else can touch
		// it.
		h = ((volatile SYSPIPE *)p)->m_head;
		if (h < p->m_tail) {
			// The buffer wraps around the end.  Thus, we first
			// read anything between the tail pointer and the end
			int ln1 = p->m_mask+1 - p->m_tail; // Navail to be read
			ln1 = (ln1 > nleft) ? nleft : ln1;
			if (ln1 > 0) {
				register int *src = &p->m_buf[p->m_tail];
				for(int i=0; i<ln1; i++)
					*dst++ = *src++;
 
				p->m_nread += ln1;
				nleft -= ln1;
				p->m_tail += ln1;
				if ((unsigned)p->m_tail > p->m_mask)
					p->m_tail = 0;
			}
 
			// nleft is either zero, or tail
			if (nleft & -2)
				exit(nleft);
			else if (p->m_nread & -2)
				exit(p->m_nread);
		}
 
		// Then repeat with the second half of the buffer, from the
		// beginning to the head--unless we've exhausted our buffer.
		if (nleft > 0) {
			// Still need to do more, wrap around our buffer and
			// restart
			int ln1 = h - p->m_tail;
			ln1 = (ln1 < nleft) ? ln1 : nleft;
 
			int *src = &p->m_buf[p->m_tail];
			for(int i=0; i<ln1; i++)
				*dst++ = *src++;
 
			p->m_nread += ln1;
			nleft -= ln1;
			p->m_tail += ln1;
			if (p->m_tail == (int)p->m_mask+1)
				p->m_tail = 0;
 
			if (nleft & -2)
				exit(nleft);
			else if (p->m_nread & -2)
				exit(p->m_nread);
		}
 
		if (nleft == 0)
			break;
 
		// We didn't finish our read, check for a blocked writing
		// process to copy directly from.  Note that we don't need
		// to check the status of the write task--if it is set and
		// we are active, then it is blocked and waiting for us to
		// complete.  Note also that this is treated as a volatile
		// pointer.  It can change from one time through our loop
		// to the next.
		if (((volatile SYSPIPE *)p)->m_wrtask) {
			int	*src, ln;
 
			// If the head changed before the write task blocked,
			// then go around again and copy some more before
			// getting started
			//
			// This should never happen, however.  If a write task
			// gets assigned while a read task exists, it doesn't
			// write its values into the buffer, it just waits.
			// therefore we don't need to check for this.
			//
			// if (p->m_head != h)
			//	continue;
 
			ln = nleft;
			if (p->m_wrtask->context[4] < nleft)
				ln = p->m_wrtask->context[4];
			src = (int *)p->m_wrtask->context[3];
 
			for(int i=0; i<ln; i++)
				*dst++ = *src++;
 
			p->m_nwritten += ln;
			p->m_nread    += ln;
 
			nleft -= ln;
			p->m_wrtask->context[4] -= ln;
			p->m_wrtask->context[3]  = (int)src;
 
			// We have exhausted the write task.  Release it
			if (p->m_wrtask->context[4] == 0) { // wr_len == 0
				// Release the write task, it has exhausted
				// its buffer
				TASKP	w = p->m_wrtask;
				// Now we allow other tasks to write into our
				// pipe
				p->m_wrtask = 0;
				// And here we actually release the writing
				// task
				w->state = SCHED_READY;
			}
		}
 
		// Realistically, we need to block here 'till more data is
		// available.  Need to determine how to do that.  Until then,
		// we'll just tell the scheduler to yield.  This will in
		// effect create a busy wait--not what we want, but it'll work.
		if (nleft > 0) {
			DISABLE_INTS();
			h = ((volatile SYSPIPE *)p)->m_head;
			if (h == p->m_tail)
				wait(0,-1);
			else
				ENABLE_INTS();
		}
	} while(nleft > 0);
 
	len = p->m_nread;
	p->m_nread = 0;
	// Release our ownership of the read end of the pipe
	DISABLE_INTS();
	p->m_rdtask = NULL;
	if (((volatile SYSPIPE *)p)->m_wrtask)
		p->m_wrtask->state = SCHED_READY;
	ENABLE_INTS();
 
	// We have accomplished our read
	//
	return len;
}
 
static int	uwrite_syspipe(TASKP tsk __attribute__((__unused__)), SYSPIPE *p, int *src, int len) {
	int nleft = len;
 
	// The kernel guarantees, before we come into here, that we have a 
	// valid write request.  
	do {
		// We try to fill this request without going through the pipes
		// memory at all.  Hence, if there is a read task that is
		// waiting/suspended, waiting on a write (this must've happened
		// since we started)--write directly into the read buffer first.
 
		// If there is a read task blocked, the pipe must be empty
		TASKP rdtask = ((volatile SYSPIPE *)p)->m_rdtask;
		if (rdtask == INTERRUPT_READ_TASK) {
			// We need to copy everything to the buffer
		} else if (rdtask) {
// #warning "The previous code should have worked"
		// if (((unsigned)rdtask+1) & -2)
			int ln = nleft;
			if (ln > p->m_rdtask->context[4])
				ln = p->m_rdtask->context[4];
			int *dst = (int *)p->m_rdtask->context[3];
			for(int i=0; i<ln; i++)
				*dst++ = *src++;
			p->m_nread += ln;
			p->m_rdtask->context[3]+= ln;
			p->m_rdtask->context[4]-= ln;
			nleft -= ln;
			p->m_nwritten += ln;
 
			// Realistically, we always need to wake up the reader
			// at this point.  Either 1) we exhausted the readers
			// buffer, or 2) we exhausted our own and the reader
			// needs to take over.  Here, we only handle the first
			// case, leaving the rest for later.
			if (p->m_rdtask->context[4] == 0) {
				TASKP	r = p->m_rdtask;
				// Detach the reader task
				p->m_rdtask = 0;
				// Wake up the reader
				r->state = SCHED_READY;
			}
 
			// While it might appear that we might close our loop
			// here, that's not quite the case.  It may be that the
			// pipe is read from an interrupt context.  In that
			// case, there will never be any reader tasks, but we
			// will still need to loop.
 
			// Now that we've filled any existing reader task, we
			// check whether or not we fit into the buffer.  The
			// rule is: don't write into the buffer unless
			// everything will fit.  Why?  Well, if you have to
			// block anyway, why not see if you can't avoid a
			// double copy?
			if (nleft == 0)
				break;
		}
 
		// Copy whatever we have into the pipe's buffer
		if ((nleft <= num_avail_syspipe(p))||(rdtask == INTERRUPT_READ_TASK)) {
			// Either there is no immediate reader task, or
			// the reader has been exhausted, but we've go
			// more to write.
			//
			// Note that we no longer need to check what
			// will fit into the pipe.  We know the entire
			// rest of our buffer will fit.
 
			{ // Write into the first half of the pipe
				int ln = p->m_mask+1-p->m_head;
				int *dst = &p->m_buf[p->m_head];
				if (ln > nleft) ln = nleft;
 
				for(int i=0; i<ln; i++)
					*dst++ = *src++;
 
				p->m_head += ln;
				nleft -= ln;
				p->m_nwritten += ln;
				if (p->m_head > (int)p->m_mask)
					p->m_head = 0;
			}
 
			// Write into the rest of the pipe
			if (nleft > 0) {
				int ln = num_avail_syspipe(p);
				if (nleft < ln)
					ln = nleft;
				int *dst = &p->m_buf[p->m_head];
 
				for(int i=0; i<ln; i++)
					*dst++ = *src++;
 
				p->m_head += ln;
				p->m_nwritten += ln;
				nleft -= ln;
			}
		}
 
		if (nleft > 0) {
			if (rdtask == INTERRUPT_READ_TASK) {
				DISABLE_INTS();
				if (num_avail_syspipe(p)==0)
					wait(0,-1);
				else ENABLE_INTS();
			} else {
				DISABLE_INTS();
				if (!((volatile SYSPIPE *)p)->m_rdtask)
					wait(0,-1); // Should really be a wait
					// on JIFFIES and if JIFFIES expired
					// (i.e. write timeout) then break;
				else ENABLE_INTS();
			}
		}
	} while(nleft > 0);
 
	int	nw= p->m_nwritten;
	p->m_wrtask = 0;
	return nw;
}
 
// This will be called from a kernel (interrupt) context
void	kread_syspipe(TASKP tsk, int dev, int *dst, int len) {
	SYSPIPE	*p = (SYSPIPE *)dev;
	if (p->m_rdtask != NULL) {
		// If the pipe already has a read task, then we fail
		tsk->context[1] = -EBUSY;
		zip_halt();
	} else if (p->m_error) {
		// If there's been an overrun, let the reader know on the
		// next read--i.e. this one.  Also, clear the error condition
		// so that the following read will succeed.
		tsk->context[1] = -EIO;
		p->m_tail = p->m_head;
		p->m_error = 0;
	} else if (len <= 0) {
		tsk->context[1] = -EFAULT;
		zip_halt();
	} else if (!valid_ram_region(dst, len)) {
		// Bad parameters
		tsk->context[1] = -EFAULT;
		zip_halt();
	} else {
		// Take  ownership of the read end of the pipe
		p->m_rdtask = tsk;
		p->m_nread  = 0;
		tsk->context[1] = (int)tsk;
		tsk->context[2] = (int)p;
		// These are  already set, else we'd set them again
		// tsk->context[3] = (int)dst;
		// tsk->context[4] = len;
		tsk->context[15] = (int)uread_syspipe;
 
		// If there is already a write task, make sure it is awake
		if (p->m_wrtask) {
			tsk->state = SCHED_WAITING;
			p->m_wrtask->state = SCHED_READY;
		} else if (p->m_head == p->m_tail)
			// If the pipe is empty, block the read task
			tsk->state = SCHED_WAITING;
 
		// On return, this will bring us back to user space, inside our
		// user space version of the read system call
	}
}
 
void	kwrite_syspipe(TASKP tsk, int dev, int *src, int len) {
	SYSPIPE	*p = (SYSPIPE *)dev;
	if (p->m_wrtask != NULL) {
		// If the pipe already has a write task, then we fail
		tsk->context[1] = -EBUSY;
	} else if (len <= 0) {
		tsk->context[1] = -EFAULT;
	} else if (!valid_mem_region(src, len)) {
		// Bad parameters
		tsk->context[1] = -EFAULT;
		zip_halt();
	} else {
		// Take  ownership of the write end of the pipe
		p->m_wrtask    = tsk;
		p->m_nwritten = 0;
		tsk->context[1] = (int)tsk;
		tsk->context[2] = (int)p;
		// These are  already set, else we'd set them again
		// tsk->context[3] = (int)src;
		// tsk->context[4] = len;
		tsk->context[15] = (int)uwrite_syspipe;
 
		// If a reader task currently exists, then block until that
		// task either finishes or releases us
		if ((p->m_rdtask)&&(p->m_rdtask != INTERRUPT_READ_TASK)) {
			tsk->state = SCHED_WAITING;
			p->m_rdtask->state = SCHED_READY;
		} else if (((p->m_head+1)&p->m_mask) == (unsigned)p->m_tail)
			// If the pipe is empty, block until there's data
			tsk->state = SCHED_WAITING;
 
		// On return, this will bring us back to user space, in our
		// user space write call
	}
}