/***********************license start************************************
 * Copyright (c) 2008 Cavium Networks (support@cavium.com). All rights
 * reserved.
 *
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *     * Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *     * Neither the name of Cavium Networks nor the names of
 *       its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written
 *       permission.
 *
 * TO THE MAXIMUM EXTENT PERMITTED BY LAW, THE SOFTWARE IS PROVIDED "AS IS"
 * AND WITH ALL FAULTS AND CAVIUM NETWORKS MAKES NO PROMISES, REPRESENTATIONS
 * OR WARRANTIES, EITHER EXPRESS, IMPLIED, STATUTORY, OR OTHERWISE, WITH
 * RESPECT TO THE SOFTWARE, INCLUDING ITS CONDITION, ITS CONFORMITY TO ANY
 * REPRESENTATION OR DESCRIPTION, OR THE EXISTENCE OF ANY LATENT OR PATENT
 * DEFECTS, AND CAVIUM SPECIFICALLY DISCLAIMS ALL IMPLIED (IF ANY) WARRANTIES
 * OF TITLE, MERCHANTABILITY, NONINFRINGEMENT, FITNESS FOR A PARTICULAR
 * PURPOSE, LACK OF VIRUSES, ACCURACY OR COMPLETENESS, QUIET ENJOYMENT, QUIET
 * POSSESSION OR CORRESPONDENCE TO DESCRIPTION.  THE ENTIRE RISK ARISING OUT
 * OF USE OR PERFORMANCE OF THE SOFTWARE LIES WITH YOU.
 *
 *
 * For any questions regarding licensing please contact marketing@caviumnetworks.com
 *
 ***********************license end**************************************/
#include <common.h>
#include <command.h>

#if (CONFIG_COMMANDS & CFG_CMD_OCTEON_NAND)

#include <watchdog.h>
#include <malloc.h>
#include <nand.h>
#include <asm/byteorder.h>
#include <octeon_boot.h>
#include <octeon_nand.h>



#ifdef DEBUG
#define debug_printf    printf
#else
#define debug_printf(...)
#endif

#include "cvmx.h"
#include "cvmx-nand.h"
#include "cvmx-bootloader.h"
/* Special command for dealing with Octeon specific NAND boot */

#if CONFIG_OCTEON_NAND_STAGE2
static int chip __attribute__ ((section (".data_in_text"))) = -1;  /* Current chip selected */
static uint64_t buf_storage[CVMX_NAND_MAX_PAGE_AND_OOB_SIZE/8] __attribute__ ((section (".data_in_text")));
#else
static int chip = -1;  /* Current chip selected */
static uint64_t buf_storage[CVMX_NAND_MAX_PAGE_AND_OOB_SIZE/8];
#endif

int oct_nand_get_cur_chip(void)
{
    return chip;
}
int oct_nand_set_cur_chip(int chip_select)
{
    chip = chip_select;
    return chip;
}



/* Copies from OCTEON boot ECC protected memory (doing ECC decode) to a supplied
** memory buffer */
int oct_nand_copy_from_boot_nand(void *dest_addr, uint64_t nand_addr, int len)
{
    int bytes_remaining = len;
    int bytes;
    int read_size = CVMX_NAND_BOOT_ECC_BLOCK_SIZE + CVMX_NAND_BOOT_ECC_ECC_SIZE;
    unsigned char *buf=(unsigned char *)buf_storage;
    int errors;
    int page_size = cvmx_nand_get_page_size(chip);

    int crc_skip = nand_addr & (CVMX_NAND_BOOT_ECC_BLOCK_SIZE - 1);
    nand_addr &= ~(CVMX_NAND_BOOT_ECC_BLOCK_SIZE - 1);  /* Start reading at an aligned address */

    for (;bytes_remaining > 0; nand_addr += CVMX_NAND_BOOT_ECC_BLOCK_SIZE)
    {
        /* Adjust the read address based on location in page */
        int offset = ((nand_addr & (page_size - 1))/CVMX_NAND_BOOT_ECC_BLOCK_SIZE) * CVMX_NAND_BOOT_ECC_ECC_SIZE;

        bytes = cvmx_nand_page_read(chip, nand_addr + offset, cvmx_ptr_to_phys(buf), read_size);
        if (bytes != read_size)
        {
            return OCT_NAND_READ_ERROR;
        }
        errors = cvmx_nand_correct_boot_ecc(buf);
        if (errors <= 1)
        {
            int copy_size = MIN(CVMX_NAND_BOOT_ECC_BLOCK_SIZE - crc_skip, bytes_remaining);
            memcpy(dest_addr, buf + crc_skip, copy_size);
            bytes_remaining -= CVMX_NAND_BOOT_ECC_BLOCK_SIZE - crc_skip;
            dest_addr += copy_size;
            crc_skip = 0;
        }
        else
            return OCT_NAND_ECC_UNCOR;
    }

    return(len);
}


/**
 * Checks to see if address is part of a bad block.
 * This does the 'standard' bad block checks, but also takes
 * into account the Octeon NAND boot ECC format.  Most blocks
 * with this format will appear to be bad, so we do extra checks
 * for blocks that fail the standard bad block test.  If a 'bad' block
 * passes Octeon ECC, then it is treated as good.
 * One complication is that some bad blocks are all 0x00 bytes, which
 * also happens to be a valid Octeon ECC encoding.  This special case is
 * checked for, and treated as a bad block.
 * 
 * @param chip_select
 *                  Chip select for operation
 * @param nand_addr Address of block to check.  (Any address within block is OK.)
 * @param check_octeon_ecc
 *                  Do extra checks for Octeon ECC.  Without these checks, many Octeon NAND boot
 *                  blocks will be reported as bad.
 * 
 * @return 1 if bad
 *         0 if OK
 */
int oct_nand_block_is_bad(int chip_select, uint64_t nand_addr, int check_octeon_ecc)
{
    /* Cache the results of the most recent check,
    ** so that we can answer quickly about all pages in 
    ** a block.*/
    static uint64_t cached_block_addr = ~0ull;
    static int cached_block_is_bad = 0;

    unsigned char *buf=(unsigned char *)buf_storage;
    int page_size = cvmx_nand_get_page_size(chip_select);
    int pages_per_block = cvmx_nand_get_pages_per_block(chip_select);
    int block_size = page_size * pages_per_block;
    int read_size = page_size + octeon_nand_get_oob_size(chip_select);
    int block_is_bad = 0;
    int bytes;
    int i;
    int page_to_check;
    uint64_t nand_addr_read;


    /* Set the bad block position */
    int bad_block_pos = page_size + (page_size > 512 ?
            NAND_LARGE_BADBLOCK_POS : NAND_SMALL_BADBLOCK_POS);

    /* Since NAND is marked bad in terms of blocks, we only
    ** care about what block the supplied address is in */
    nand_addr &= ~(block_size - 1);

    /* Return cached value if we have a match */
    if (nand_addr == cached_block_addr)
        return cached_block_is_bad;

    nand_addr_read = nand_addr;
    for (page_to_check = 0; page_to_check < 2; page_to_check++)
    {
        /* Check first and last pages of block */
        bytes = cvmx_nand_page_read(chip_select, nand_addr_read, cvmx_ptr_to_phys(buf), read_size);
        if (bytes != read_size)
        {
            printf("ERROR: %s: error reading NAND.\n", __FUNCTION__);
            return 1;
        }
        if (buf[bad_block_pos] != 0xFF)
            block_is_bad = 1;

        if (block_is_bad)
        {
            if (check_octeon_ecc)
            {
                /* Check to see if there is a valid Octeon ECC encoded block at the beginning of the page.
                 * If so, the block is good, otherwise it is bad */
                if (!cvmx_nand_correct_boot_ecc(buf))
                {
                    /* No ECC errors detected */
                    /* Some bad blocks are set to all 0x00, including OOB.  This meets
                    ** all bad block marking requirements, but is also a valid Octeon 
                    ** BOOT bus encoding.  Treat this as a bad block */
                    for (i = 0;i < read_size;i++)
                        if (buf[i])
                            break;
                    if (i == read_size)
                    {
                        block_is_bad = 1;
                        goto page_is_bad_exit;
                    }
                    block_is_bad = 0;
                }
            }
            else
                goto page_is_bad_exit;
        }
        nand_addr_read += block_size - page_size;
    }

page_is_bad_exit:
    /* Cache most recent lookup so looking up pages in same block is fast */
    cached_block_addr = nand_addr;
    cached_block_is_bad = block_is_bad;
    return block_is_bad;
}

/* Checks to see if the page (including OOB) at given address is blank
** returns: 1 if blank
**          0 if !blank
*/
int oct_nand_page_is_blank(uint64_t nand_addr)
{
    unsigned char *buf=(unsigned char *)buf_storage;
    int page_size = cvmx_nand_get_page_size(chip);
    int read_size = page_size + octeon_nand_get_oob_size(chip);
    int bytes;
    int i;


    if (nand_addr & (page_size -1))
        printf("WARNING: non page aligned address passed to %s\n", __FUNCTION__);

    nand_addr &= ~(page_size -1);

    /* Check to see if the page/block is bad.
    ** Treat bad blocks as non-blank, so they won't get written to */
    if (oct_nand_block_is_bad(chip, nand_addr, 1))
    {
        printf("page at addr 0x%llx is bad, returning !blank\n", nand_addr);
        return 0;
    }

    bytes = cvmx_nand_page_read(chip, nand_addr, cvmx_ptr_to_phys(buf), read_size);
    if (bytes != read_size)
    {
        printf("ERROR: %s: error reading NAND.\n", __FUNCTION__);
        return 0;
    }
    for (i = 0; i < read_size;i++)
    {
        if (buf[i] != 0xff)
            return(0);
    }
    return(1);
}

/* Checks to see if the block (including OOB) at given address is blank
** returns: 1 if blank
**          0 if !blank
*/
int oct_nand_block_is_blank(uint64_t nand_addr)
{
    int page_size = cvmx_nand_get_page_size(chip);
    int pages_per_block = cvmx_nand_get_pages_per_block(chip);
    int block_size = page_size * pages_per_block;
    int page;

    if (nand_addr & (block_size -1))
    {
        printf("ERROR: %s: nand_addr is not block aligned.\n", __FUNCTION__);
        return 0;
    }
    for (page = 0; page < pages_per_block; page++)
    {
        if (!oct_nand_page_is_blank(nand_addr + page_size * page))
            return(0);
    }
    return(1);
}

void dump_hex(unsigned char * buffer)
{
    int i;
    for (i = 0; i < 256;i++)
    {
        printf("%02x ", buffer[i]);
        if (i%16==15)
            printf("\n");
    }
}
/* Writes to NAND using Octeon NAND-boot ECC */
oct_nand_status_t oct_nand_boot_write(uint64_t nand_addr, void *data, int len, uint32_t flags)
{
    uint64_t tmp_addr;
    int input_remaining;
    uint64_t buf_storage[CVMX_NAND_MAX_PAGE_AND_OOB_SIZE/8];
    unsigned char *buf = (unsigned char *)buf_storage;
    int page_size = cvmx_nand_get_page_size(chip);
    int oob_size = octeon_nand_get_oob_size(chip);

//    printf("oct_nand_boot_write(0x%llx, %p, %d, 0x%x)\n", nand_addr, data, len, flags);
    if (nand_addr & (page_size - 1))
    {
        printf("ERROR: write start must be page aligned.\n");
        return OCT_NAND_BAD_PARAM;
    }


    /* Round length up to page size */
    len = (len + page_size - 1) & ~(page_size - 1);



    /* First check to make sure all pages are erased.  We don't count the OOB data against the length
    ** supplied, as that is where the ECC data will go.
    ** BAD blocks are returned as !blank, so we will also skip bad blocks */
    for (tmp_addr = nand_addr; tmp_addr  < nand_addr + len;tmp_addr += page_size)
    {
        if (oct_nand_block_is_bad(chip, tmp_addr, 1))
        {
            printf("ERROR: Block containing addr 0x%llx is bad, unable to write to page\n", tmp_addr);
            return OCT_NAND_ERROR;
        }
        if (!oct_nand_page_is_blank(tmp_addr))
        {
            printf("ERROR: page at addr 0x%llx is not blank, unable to write to page\n", tmp_addr);
            return OCT_NAND_ERROR;
        }
    }


    /* Now we have verified that the required flash is erased (and not bad), now generate the ECC and
    ** write the data to the flash. */
    int offset;  /* offset within NAND page while computing ECC (may limit size of final write) */
    input_remaining = len;
    for (tmp_addr = nand_addr; tmp_addr  < nand_addr + len;tmp_addr += page_size)
    {
        uint64_t buf1_storage[CVMX_NAND_MAX_PAGE_AND_OOB_SIZE/8];
        unsigned char *buf1 = (unsigned char *)buf1_storage;
        memset(buf, 0, sizeof(buf));
        offset = 0;
        while (input_remaining > 0 && offset <= (page_size + oob_size - CVMX_NAND_BOOT_ECC_BLOCK_SIZE - CVMX_NAND_BOOT_ECC_ECC_SIZE))
        {
            memcpy(buf + offset, data, MIN(CVMX_NAND_BOOT_ECC_BLOCK_SIZE, input_remaining));
            cvmx_nand_compute_boot_ecc(buf + offset, buf + offset + CVMX_NAND_BOOT_ECC_BLOCK_SIZE);
            offset += CVMX_NAND_BOOT_ECC_BLOCK_SIZE + CVMX_NAND_BOOT_ECC_ECC_SIZE;
            data += CVMX_NAND_BOOT_ECC_BLOCK_SIZE;
            input_remaining -= CVMX_NAND_BOOT_ECC_BLOCK_SIZE;
        }
        /* We have filled up a page, so write it */
        if (cvmx_nand_page_write(chip, tmp_addr, cvmx_ptr_to_phys(buf)))
        {
            printf("cvmx_nand_page_write() failed, address 0x%08x\n", tmp_addr);
            return OCT_NAND_WRITE_ERROR;
        }
        /* Now read the block to make sure it matches */
        if (cvmx_nand_page_read(chip, tmp_addr, cvmx_ptr_to_phys(buf1), offset) != offset)
        {
            printf("ERROR reading back NAND to verify write.\n");
            return OCT_NAND_READ_ERROR;
        }
        if (memcmp(buf, buf1, offset))
        {
            printf("ERROR: write verify failed.\n");
            return OCT_NAND_WRITE_ERROR;
        }
    }
    return OCT_NAND_SUCCESS;
}








/* Validates the header that is at nand_addr, and returns the
** size in bytes.  If invalid:
** -1 : wrong magic
** -2 : header CRC incorrect
** -3 : data CRC incorrect
** -4 : uncorrectable ECC error
** -5 : NAND read error
*/
int oct_nand_validate_image(uint64_t nand_addr)
{
    int errors = 0;
    int read_size = CVMX_NAND_BOOT_ECC_BLOCK_SIZE + CVMX_NAND_BOOT_ECC_ECC_SIZE;
    int bytes;
    unsigned char *buf=(unsigned char *)buf_storage;
    const bootloader_header_t *header = (const bootloader_header_t *)buf;
    uint32_t dcrc;
    uint32_t crc;
    int page_size = cvmx_nand_get_page_size(chip);
    int data_length;
    int crc_bytes_remaining;
    int crc_skip;

    /* Read first block, which should contain header */

    bytes = cvmx_nand_page_read(chip, nand_addr, cvmx_ptr_to_phys(buf), read_size);

    if (cvmx_nand_correct_boot_ecc(buf) > 1)
        return OCT_NAND_ECC_UNCOR;

    if ((header->magic != BOOTLOADER_HEADER_MAGIC))
        return OCT_NAND_BAD_MAGIC;

    crc = crc32(0, (void *)header, offsetof(bootloader_header_t,hcrc));
    crc = crc32(crc,  (void *)header + offsetof(bootloader_header_t,hlen), header->hlen - offsetof(bootloader_header_t,hlen));
    if (crc != header->hcrc)
        return OCT_NAND_BAD_HCRC;

    /* Save data crc for later use */
    dcrc = header->dcrc;

    /* Data starts at nand_addr + header->hlen */
    nand_addr += header->hlen;

    /* We need to always read CVMX_NAND_BOOT_ECC_BLOCK_SIZE size blocks so that we can do ECC.
    ** The data may start at an offset into this first block */
    data_length = crc_bytes_remaining = header->dlen;

    crc_skip = nand_addr & (CVMX_NAND_BOOT_ECC_BLOCK_SIZE - 1);
    nand_addr &= ~(CVMX_NAND_BOOT_ECC_BLOCK_SIZE - 1);  /* Start reading at an aligned address */

    crc = 0;
    for (;crc_bytes_remaining > 0; nand_addr += CVMX_NAND_BOOT_ECC_BLOCK_SIZE)
    {
        /* Adjust the read address based on location in page */
        int offset = ((nand_addr & (page_size - 1))/CVMX_NAND_BOOT_ECC_BLOCK_SIZE) * CVMX_NAND_BOOT_ECC_ECC_SIZE;

        bytes = cvmx_nand_page_read(chip, nand_addr + offset, cvmx_ptr_to_phys(buf), read_size);
        if (bytes != read_size)
        {
            return OCT_NAND_READ_ERROR;
        }
        errors = cvmx_nand_correct_boot_ecc(buf);
        if (errors <= 1)
        {
            if (errors == 1)
                debug_printf("Correctable ECC error at NAND addr: 0x%qx\n", nand_addr + offset);
            crc = crc32(crc,  (void *)buf + crc_skip, MIN(CVMX_NAND_BOOT_ECC_BLOCK_SIZE - crc_skip, crc_bytes_remaining));
            crc_bytes_remaining -= CVMX_NAND_BOOT_ECC_BLOCK_SIZE - crc_skip;
            if (crc_skip)
                crc_skip = 0;
        }
        else
        {
            debug_printf("Uncorrectable ECC error at NAND addr: 0x%qx\n", nand_addr + offset);
            return OCT_NAND_ECC_UNCOR;
    }
    }

    if (dcrc != crc)
        return OCT_NAND_BAD_DCRC;

    return(data_length);
}

int oct_nand_probe(void);

int do_oct_nand(cmd_tbl_t * cmdtp, int flag, int argc, char *argv[])
{
    uint64_t buf_storage[CVMX_NAND_MAX_PAGE_AND_OOB_SIZE/8];
    unsigned char *buf = (unsigned char *)buf_storage;

    char *cmd;

    if (argc < 2)
        goto usage;

    cmd = argv[1];

    if (!strcmp("probe", cmd))
    {
        return(oct_nand_probe());
    }
    else if (!strcmp("dump", cmd))
    {
        /* Only dump full pages.... */
        uint64_t page, end;
        uint64_t start_addr, len;
        int page_size = cvmx_nand_get_page_size(chip);
        int oob_size = cvmx_nand_get_oob_size(chip);
        if (argc <3)
            goto usage;
        start_addr = simple_strtoull(argv[2], NULL, 16);
        if (argc == 4)
            len =   simple_strtoull(argv[3], NULL, 16);
        else
            len = 0;

        if (!page_size)
        {
            printf("ERROR: No NAND configured (run probe)\n");
            return 1;
        }

        /* Convert from addresses to pages */
        page = start_addr/page_size;
        if (!len)
            end = page;
        else
            end = (start_addr + len)/page_size;

        while (page <= end)
        {
            /* Read the next block */
            int i;
            int read_size = page_size + oob_size;
            /* Read more than required in cases when total read size
            ** is not a multiple of 8.
            */
            read_size = (read_size + 7) & ~0x7;  /* Size must be multiple of 8 */

            int bytes = cvmx_nand_page_read(chip, page_size * page, cvmx_ptr_to_phys(buf), read_size);
            int pages_per_block = cvmx_nand_get_pages_per_block(chip);
            if (bytes != read_size)
            {
                printf("Error reading page %d, bytes read: %d\n", page, bytes);
                return 1;
            }
            /* Dump page data */
            printf("Address 0x%llx, (Block 0x%x, page 0x%x) data:\n", page * page_size, page/pages_per_block, page%pages_per_block);
            for (i = 0; i < page_size; i++)
            {
                if (i%16 == 0)
                    printf("0x%04x:", page*page_size + i);
                printf(" %02x", buf[i]);
                if (i%16 == 15)
                    printf("\n");

            }
            /* Dump OOB data */
            printf("Address 0x%llx, (Block 0x%x, page 0x%x) OOB:\n", page * page_size, page/pages_per_block, page%pages_per_block);
            for (i = 0; i < oob_size; i++)
            {
                if (i%16 == 0)
                    printf("0x%04x:", i);
                printf(" %02x", buf[page_size + i]);
                if (i%16 == 15)
                    printf("\n");
            }
            printf("\n");

            page++;
        }
        return 0;
    }
    else if (!strcmp("erase", cmd))
    {
        uint64_t start_addr, length;
        int force = 0;
        uint64_t i;
        int page_size = cvmx_nand_get_page_size(chip);
        int pages_per_block = cvmx_nand_get_pages_per_block(chip);
        if (argc < 3 || argc > 5)
            goto usage;
        start_addr = simple_strtoull(argv[2], NULL, 16);
        if (argc >= 4)
            length =   simple_strtoull(argv[3], NULL, 16);
        else
            length = page_size*pages_per_block;

        if (argc == 5)
        {
            if (strcmp(argv[4], "force"))
                goto usage;
            printf("WARNING: Forced erase: erasing bad blocks.\n");
            force = 1;
        }
        if (!page_size)
        {
            printf("ERROR: No NAND configured (run probe)\n");
            return 1;
        }


        if (start_addr & ((uint64_t)page_size*pages_per_block - 1))
        {
            printf("ERROR: erase start not at block boundary (maybe want: 0x%llx or 0x%llx)\n",
                   start_addr & ~((uint64_t)page_size*pages_per_block - 1),
                   (start_addr + page_size*pages_per_block) & ~((uint64_t)page_size*pages_per_block - 1));
            return 1;

        }
        if ((length & (page_size*pages_per_block - 1)))
        {
            printf("ERROR: erase length not multiple of block size (maybe want: 0x%llx)\n",
                   (length + (page_size*pages_per_block - 1)) & ~(page_size*pages_per_block - 1));
            return 1;

        }

        for (i = start_addr; i < start_addr + length; i += page_size*pages_per_block)
        {
            if (oct_nand_block_is_bad(chip, i, 1))
            {
                if (force)
                {
                    printf("WARNING: Erasing bad bock at address 0x%llx\n", i);
                }
                else
                {
                    printf("Not erasing bad block at address 0x%llx\n", i);
                    continue;
                }
            }
            if (cvmx_nand_block_erase(chip, i))
            {
                printf("cvmx_nand_block_erase() failed, addr 0x%08llx\n", i);
                return 1;
            }
        }

        return 0;
    }
    else if (!strcmp("write", cmd))
    {
        uint64_t nand_addr;
        uint32_t dram_addr, length;
        int page_size = cvmx_nand_get_page_size(chip);

        if (!page_size)
        {
            printf("ERROR: No NAND configured (run probe)\n");
            return 1;
        }
        if (argc != 3 && argc != 5)
            goto usage;

        nand_addr = simple_strtoull(argv[2], NULL, 16);
        if (argc == 5)
        {
            dram_addr = simple_strtoul(argv[3], NULL, 16);
            length =   simple_strtoul(argv[4], NULL, 16);
        }
        else
        {
            /* We look up the length/address from environment variables.... */
            /* filesize=3410
            ** fileaddr=20000000
            */

            if (getenv("filesize") && getenv("fileaddr"))
            {
                dram_addr = simple_strtoul (getenv("fileaddr"), NULL, 16);
                length = simple_strtoul (getenv("filesize"), NULL, 16);

                /* Round length up to pagesize */
                length = (length + page_size - 1) & ~(page_size -1);
            }
            else
            {
                printf("ERROR: filesize and fileaddr environment variables\nmust be set if address and size are not given\n");
                goto usage;
            }

        }
        unsigned char * data_ptr = (void *)dram_addr;


        if (nand_addr & (page_size - 1))
        {
            printf("ERROR: write address not at page boundary (maybe want: 0x%08x)\n",
                   nand_addr & ~(page_size - 1));
            return 1;
        }

        return(oct_nand_boot_write(nand_addr, data_ptr, length,0));
    }
    else if (!strcmp("scan", cmd))
    {
        uint64_t nand_addr;
        bootloader_header_t header_copy;
        int page_size = cvmx_nand_get_page_size(chip);
        int pages_per_block = cvmx_nand_get_pages_per_block(chip);
        int block_size = page_size * pages_per_block;
        int errors = 0;
        int read_size = CVMX_NAND_BOOT_ECC_BLOCK_SIZE + CVMX_NAND_BOOT_ECC_ECC_SIZE;
        if (!page_size)
        {
            printf("ERROR: No NAND configured (run probe)\n");
            return 1;
        }
        printf("Scanning NAND for recognized images\n");
        for (nand_addr = 0; nand_addr < MAX_NAND_SEARCH_ADDR; nand_addr += page_size)
        {

            /* Adjust the read address based on location in page */
            int offset = ((nand_addr & (page_size - 1))/CVMX_NAND_BOOT_ECC_BLOCK_SIZE) * CVMX_NAND_BOOT_ECC_ECC_SIZE;
            int bytes = cvmx_nand_page_read(chip, nand_addr + offset, cvmx_ptr_to_phys(buf), read_size);
            if (bytes != read_size)
            {
                printf("Error reading NAND addr %d (bytes_read: %d, expected: %d)\n", nand_addr, bytes, read_size);
                return 1;
            }
            errors = cvmx_nand_correct_boot_ecc(buf);
            if (errors <= 1)
            {
                const bootloader_header_t *header = (const bootloader_header_t *)buf;

                /* Block is good, see if it contains a bootloader image header */
                if ((header->magic == BOOTLOADER_HEADER_MAGIC))
                {
                    int rval;
        
                    /* Check the CRC of the header */
                    uint32_t crc;
                    crc = crc32(0, (void *)header, 12);
                    crc = crc32(crc,  (void *)header + 16, header->hlen - 16);
                    
                    if (crc != header->hcrc)
                    {
                        printf("ERROR: 0x%llx Header CRC mismatch, expected: 0x%08x, found: 0x%08x\n", nand_addr, header->hcrc, crc);
                    }
                    else
                    {
                        header_copy = *header;
                        printf("\nImage header at addr: 0x%llx, size: 0x%x, crc: 0x%x ", nand_addr, header->dlen, header->dcrc);
                        if ((rval = oct_nand_validate_image(nand_addr)) < 0)
                        {
                            printf("Invalid image(%d).\n", rval);
                        }
                        else
                        {
                            printf("Valid image.\n");
                            if (header_copy.flags & BL_HEADER_FLAG_FAILSAFE)
                                printf("Failsafe ");
                            else
                                printf("Normal ");
                            if (header_copy.image_type == BL_HEADER_IMAGE_STAGE2)
                                printf("NAND stage2 ");
                            else if (header_copy.image_type == BL_HEADER_IMAGE_STAGE3)
                                printf("NAND stage3 ");
                            else if (header_copy.image_type == BL_HEADER_IMAGE_UBOOT_ENV)
                                printf("U-boot environment (version: %d)\n", (int)header_copy.address);

                            if (header_copy.board_type)
                                printf("image for %s board\n",cvmx_board_type_to_string(header_copy.board_type));
                            else
                                printf("\n");
                            /* Skip the image, as we have validated it */
                            nand_addr += ((header->dlen + page_size - 1) & ~(page_size - 1)) - page_size;
                        }
                    }
                    
                }
            }
            else
            {
                /* Uncorrectable errors detected. */

                /* Check to see if we have a bad block */
                if (oct_nand_block_is_bad(chip, nand_addr, 1))
                {
                    printf("Bad NAND block at addr: 0x%llx\n", nand_addr & ~(block_size -1));
                    /* Skip rest of block */
                    nand_addr += block_size - (nand_addr & (block_size -1)) - page_size;
                }

            }


        }

        printf("\n");
        return 0;
    }

usage:
	printf("Usage:\n%s\n", cmdtp->usage);
	return 1;
}






/* This function is used to scan for the environment before the serial port
** is initialized.  In order to enable these debug prints the 'env_init'
** function must be called after the serial port is configured.
*/

#if 1
#define special_debug_printf(...)
#else
#define special_debug_printf    printf
#endif
/* Look for an image of a given type in the NAND address range given.  If an image type
** of 0 is passed, matches all valid images */
uint64_t oct_nand_image_search(uint64_t start_addr, uint64_t max_addr, uint16_t image_type)
{
    unsigned char *buf = (unsigned char *)buf_storage;

    int page_size;
    int chip = oct_nand_get_cur_chip();
    uint64_t nand_addr;
    bootloader_header_t header_copy;
    
    if (chip < 0)
    {
        special_debug_printf("No nand chips found\n");
        return 0;
    }

    page_size = cvmx_nand_get_page_size(chip);

    /* Round start address up to a page boundary */
    start_addr = (start_addr + page_size - 1) & ~(page_size - 1);

    special_debug_printf("Using NAND chip %d, page size: %d\n", chip, page_size);
    int errors = 0;
    int read_size = CVMX_NAND_BOOT_ECC_BLOCK_SIZE + CVMX_NAND_BOOT_ECC_ECC_SIZE;
    if (!page_size)
    {
        special_debug_printf("ERROR: No NAND configured (run probe)\n");
        return 0;
    }
    special_debug_printf("Scanning NAND for for image type: %d\n", image_type);
    for (nand_addr = start_addr; nand_addr < max_addr; nand_addr += page_size)
    {
        /* Adjust the read address based on location in page */
        int offset = ((nand_addr & (page_size - 1))/CVMX_NAND_BOOT_ECC_BLOCK_SIZE) * CVMX_NAND_BOOT_ECC_ECC_SIZE;
        int bytes = cvmx_nand_page_read(chip, nand_addr + offset, cvmx_ptr_to_phys(buf), read_size);
        if (bytes != read_size)
        {
            special_debug_printf("Error reading NAND addr %d (bytes_read: %d, expected: %d)\n", nand_addr, bytes, read_size);
            return -1;
        }
        errors = cvmx_nand_correct_boot_ecc(buf);
        if (errors <= 1)
        {
            if (errors == 1)
                special_debug_printf("Correctable ECC error at NAND address: 0x%qx\n", nand_addr + offset);

            const bootloader_header_t *header = (const bootloader_header_t *)buf;

            /* Block is good, see if it contains a bootloader image header */
            if ((header->magic == BOOTLOADER_HEADER_MAGIC))
            {
                special_debug_printf("\nFound a header at addr 0x%llx\n", nand_addr);
    
                /* Check the CRC of the header */
                uint32_t crc;
                crc = crc32(0, (void *)header, 12);
                crc = crc32(crc,  (void *)header + 16, header->hlen - 16);
                
                if (crc == header->hcrc)
                {
                    /* Copy header, as validate_image() call will overwrite global buffer */
                    header_copy = *header;
                    if (oct_nand_validate_image(nand_addr) >= 0)
                    {
                        if (!image_type || header_copy.image_type == image_type)
                        {
                            /* We found a valid image of the type we were looking for, so
                            ** return the address of it. */
                            special_debug_printf("Image type match at addr 0x%llx\n", nand_addr);
                            return(nand_addr);
                        }
                        /* Skip the image, as we have validated it */
                        special_debug_printf("\nSkipping image type: %d\n", header_copy.image_type);
                        nand_addr += ((header_copy.hlen + header_copy.dlen + page_size - 1) & ~(page_size - 1)) - page_size;
                    }
                    else
                    {
                        special_debug_printf("Image CRC failed\n");
                    }
                }
                else
                {
                    special_debug_printf("Header CRC failed\n");
                }
            }
        }
        else
        {
            special_debug_printf("Uncorrectable ECC error at NAND address: 0x%qx\n", nand_addr + offset);
        }
    }

    special_debug_printf("Done looking for image type: %d, none found.\n", image_type);
    return(0);

}




U_BOOT_CMD(octnand, 5, 0, do_oct_nand,
	"octnand   - NAND operations related to NAND boot\n",
           "probe - probes for NAND on Octeon boot bus\n"
	   "erase <nand addr> [length] [force] - erase blocks of NAND. addr must be block aligned\n"
           "                                     force must be set to erase blocks that are bad\n"
           "scan - scans for recognized images in NAND\n"
           "write <nand dest addr> [<dram src addr> <length in bytes>]\n"
           "      Adds ECC encoding and writes to NAND\n"
           "      Uses fileaddr and filesize env. variable if only 1 arg\n"
           "dump <nand addr> <length in bytes> - dumps raw nand DATA (always full pages)\n"
           "\n"
           "This command only writes to contiguous sections of good blocks, as the first stage\n"
           "of NAND boot does not support skipping of bad blocks.\n"
           );

#endif
