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

        }

}