Subversion Repositories s6soc

////////////////////////////////////////////////////////////////////////////////

//

// Filename:    flashdrvr.cpp

//

// Project:     CMod S6 System on a Chip, ZipCPU demonstration project

//

// Purpose:     Flash driver.  Encapsulate writing to the flash device.

//

// Creator:     Dan Gisselquist

//              Gisselquist Tecnology, LLC

//

////////////////////////////////////////////////////////////////////////////////

//

// Copyright (C) 2016-2017, 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 <stdio.h>

#include <stdlib.h>

#include <stdint.h>

#include <unistd.h>

#include <strings.h>

#include <ctype.h>

#include <string.h>

#include <signal.h>

#include <assert.h>

#include "devbus.h"

#include "regdefs.h"

#include "flashdrvr.h"

#include "byteswap.h"

const   bool    HIGH_SPEED = false;

void    FLASHDRVR::flwait(void) {

        DEVBUS::BUSW    v;

        v = m_fpga->readio(R_QSPI_EREG);

        if ((v&ERASEFLAG)==0)

                return;

        m_fpga->writeio(R_ICONTROL, ISPIF_DIS);

        m_fpga->clear();

        m_fpga->writeio(R_ICONTROL, ISPIF_EN);

        do {

                // Start by checking that we are still erasing.  The interrupt

                // may have been generated while we were setting things up and

                // disabling things, so this just double checks for us.  If

                // the interrupt was tripped, we're done.  If not, we can now

                // wait for an interrupt.

                v = m_fpga->readio(R_QSPI_EREG);

                if (v&ERASEFLAG) {

                        m_fpga->usleep(400);

                        if (m_fpga->poll()) {

                                m_fpga->clear();

                                m_fpga->writeio(R_ICONTROL, ISPIF_EN);

                        }

                }

        } while(v & ERASEFLAG);

}

bool    FLASHDRVR::erase_sector(const unsigned sector, const bool verify_erase) {

        DEVBUS::BUSW    page[SZPAGEW];

        if (m_debug) printf("Erasing sector: %08x\n", sector);

        m_fpga->writeio(R_QSPI_EREG, DISABLEWP);

        m_fpga->writeio(R_QSPI_EREG, ERASEFLAG + (sector>>2));

        // If we're in high speed mode and we want to verify the erase, then

        // we can skip waiting for the erase to complete by issueing a read

        // command immediately.  As soon as the erase completes the read will

        // begin sending commands back.  This allows us to recover the lost 

        // time between the interrupt and the next command being received.

        if  ((!HIGH_SPEED)||(!verify_erase)) {

                flwait();

                if (m_debug) {

                        printf("@%08x -> %08x\n", R_QSPI_EREG,

                                m_fpga->readio(R_QSPI_EREG));

                        printf("@%08x -> %08x\n", R_QSPI_SREG,

                                        m_fpga->readio(R_QSPI_SREG));

                        printf("@%08x -> %08x\n", sector,

                                m_fpga->readio(sector));

                }

        }

        // Now, let's verify that we erased the sector properly

        if (verify_erase) {

                for(int i=0; i<NPAGES; i++) {

                        m_fpga->readi(sector+i*SZPAGEW, SZPAGEW, page);

                        for(int i=0; i<SZPAGEW; i++)

                                if (page[i] != 0xffffffff)

                                        return false;

                }

        }

        return true;

}

bool    FLASHDRVR::page_program(const unsigned addr, const unsigned len,

                const char *data, const bool verify_write) {

        DEVBUS::BUSW    buf[SZPAGEW], bswapd[SZPAGEW];

        assert(len > 0);

        assert(len <= PGLENB);

        assert(PAGEOF(addr)==PAGEOF(addr+len-1));

        if (len <= 0)

                return true;

        bool    empty_page = true;

        for(unsigned i=0; i<len; i+=4) {

                DEVBUS::BUSW v;

                v = buildword((const unsigned char *)&data[i]);

                bswapd[(i>>2)] = v;

                if (v != 0xffffffff)

                        empty_page = false;

        }

        if (!empty_page) {

                // Write the page

                m_fpga->writeio(R_ICONTROL, ISPIF_DIS);

                m_fpga->clear();

                m_fpga->writeio(R_ICONTROL, ISPIF_EN);

                printf("Writing page: 0x%08x - 0x%08x\r", addr, addr+len-1);

                m_fpga->writeio(R_QSPI_EREG, DISABLEWP);

                m_fpga->writei(addr, (len>>2), bswapd);

                fflush(stdout);

                // If we're in high speed mode and we want to verify the write,

                // then we can skip waiting for the write to complete by

                // issueing a read command immediately.  As soon as the write

                // completes the read will begin sending commands back.  This

                // allows us to recover the lost time between the interrupt and

                // the next command being received.

                flwait();

        }

        // if ((!HIGH_SPEED)||(!verify_write)) { }

        if (verify_write) {

                // printf("Attempting to verify page\n");

                // NOW VERIFY THE PAGE

                m_fpga->readi(addr, len>>2, buf);

                for(unsigned i=0; i<(len>>2); i++) {

                        if (buf[i] != bswapd[i]) {

                                printf("\nVERIFY FAILS[%d]: %08x\n", i, (i<<2)+addr);

                                printf("\t(Flash[%d]) %08x != %08x (Goal[%08x])\n",

                                        (i<<2), buf[i], bswapd[i], (i<<2)+addr);

                                return false;

                        }

                } // printf("\nVerify success\n");

        } return true;

}

bool    FLASHDRVR::write(const unsigned addr, const unsigned len,

                const char *data, const bool verify) {

        // Work through this one sector at a time.

        // If this buffer is equal to the sector value(s), go on

        // If not, erase the sector

        for(unsigned s=SECTOROF(addr); s<SECTOROF(addr+len+SECTORSZB-1);

                        s+=SECTORSZB) {

                // Do we need to erase?

                bool    need_erase = false, need_program = false;

                unsigned newv = 0; // (s<addr)?addr:s;

                {

                        char *sbuf = new char[SECTORSZB];

                        const char *dp; // pointer to our "desired" buffer

                        unsigned        base,ln;

                        base = (addr>s)?addr:s;

                        ln=((addr+len>s+SECTORSZB)?(s+SECTORSZB):(addr+len))-base;

                        m_fpga->readi(base, ln>>2, (uint32_t *)sbuf);

                        byteswapbuf(ln>>2, (uint32_t *)sbuf);

                        dp = &data[base-addr];

                        for(unsigned i=0; i<ln; i++) {

                                if ((sbuf[i]&dp[i]) != dp[i]) {

                                        if (m_debug) {

                                                printf("\nNEED-ERASE @0x%08x ... %08x != %08x (Goal)\n",

                                                        i+base-addr, sbuf[i], dp[i]);

                                        }

                                        need_erase = true;

                                        newv = (i&-4)+base;

                                        break;

                                } else if ((sbuf[i] != dp[i])&&(newv == 0))

                                        newv = (i&-4)+base;

                        }

                }

                if (newv == 0)

                        continue; // This sector already matches

                // Erase the sector if necessary

                if (!need_erase) {

                        if (m_debug) printf("NO ERASE NEEDED\n");

                } else {

                        printf("ERASING SECTOR: %08x\n", s);

                        if (!erase_sector(s, verify)) {

                                printf("SECTOR ERASE FAILED!\n");

                                return false;

                        } newv = (s<addr) ? addr : s;

                }

                // Now walk through all of our pages in this sector and write

                // to them.

                for(unsigned p=newv; (p<s+SECTORSZB)&&(p<addr+len); p=PAGEOF(p+PGLENB)) {

                        unsigned start = p, len = addr+len-start;

                        // BUT! if we cross page boundaries, we need to clip

                        // our results to the page boundary

                        if (PAGEOF(start+len-1)!=PAGEOF(start))

                                len = PAGEOF(start+PGLENB)-start;

                        if (!page_program(start, len, &data[p-addr], verify)) {

                                printf("WRITE-PAGE FAILED!\n");

                                return false;

                        }

                } if ((need_erase)||(need_program))

                        printf("Sector 0x%08x: DONE%15s\n", s, "");

        }

        m_fpga->writeio(R_QSPI_EREG, ENABLEWP); // Re-enable write protection

        return true;

}