sysflashmem.c

vxworks MPC8541 BSP

    SYS_FLASH_HW_ENABLE();

    overSize = pos % dataSize;
    curPos = (UINT32)(pos & ~(dataSize - 1));

    while (1)
        {
        dataBuf = buf + scanLen;

        if (overSize != 0)
            {
            memcpy(tempBuf, (char *)(dev->devAddr + curPos), dataSize);
            if (len < (dataSize - overSize))
                memcpy(tempBuf + overSize, dataBuf, len);
            else
                memcpy(tempBuf + overSize, dataBuf, dataSize - overSize);
            dataBuf = tempBuf;
            }
        else if (len < dataSize)
            {
            memcpy(tempBuf, (char *)(dev->devAddr + curPos), dataSize);
            memcpy(tempBuf, dataBuf, len);
            dataBuf = tempBuf;
            }
        else if ((UINT32)dataBuf % dataSize)
            {
            memcpy(tempBuf, dataBuf, dataSize);
            dataBuf = tempBuf;
            }

        if ((*dev->write)(dev, curPos, dataBuf) == ERROR)
            {
            SYS_FLASH_HW_DISABLE();
            semGive(dev->devSem);
            return ERROR;
            }

        if (len <= (dataSize - overSize))
            break;
        else
            {
            len -= dataSize - overSize;
            scanLen += dataSize - overSize;
            }
        overSize = 0;
        curPos += dataSize;
        }

    SYS_FLASH_HW_DISABLE();
    semGive(dev->devSem);

    return OK;
    }

/*******************************************************************************
*
* flashProgram - 
*
*/
STATUS flashProgram(FLASH_DEV *dev, UINT32 pos, char *buf, UINT32 len)
    {
    FLASH_PART *part;
    char *blkBuf = NULL;
    UINT32 blkPos = pos;
    UINT32 partPos = 0;
    UINT32 overSize = 0;
    UINT32 underSize = 0;
    UINT32 dataSize = 0;
    UINT32 scanLen = 0;
    UINT32 stepSize = dev->writeBufMod ? WRITE_BUF_SIZE * dev->bankCount : dev->portSize;
    UINT32 i, j;

    if (!dev->inited)
        {
        if (flashInit() == ERROR)
            return ERROR;
        }

    if (pos + len > dev->devSize)
        {
        printf("flash program position out of range!\n");
        return ERROR;
        }

    if (len == 0)
        return OK;

    semTake(dev->devSem, WAIT_FOREVER);
    SYS_FLASH_HW_ENABLE();

    for (i = 0; i < dev->partCount; i++)
        {
        part = dev->devPart + i;
        partPos += dev->bankCount * part->blockSize * part->blockCount;
        if (blkPos >= partPos)
            continue;

        blkPos &= (UINT32)~(dev->bankCount * part->blockSize - 1);
        overSize = pos + scanLen - blkPos;

        do
            {
            if (scanLen + dev->bankCount * part->blockSize - overSize > len)
                underSize = scanLen + dev->bankCount * part->blockSize - overSize - len;

            if ((overSize != 0) || (underSize != 0) || ((UINT32)(buf + scanLen) % stepSize))
                {
                if ((blkBuf = (char *)malloc(dev->bankCount * part->blockSize)) == NULL)
                    {
                    SYS_FLASH_HW_DISABLE();
                    semGive(dev->devSem);
                    return ERROR;
                    }

                memcpy(blkBuf, (char *)(dev->devAddr + blkPos),
                       dev->bankCount * part->blockSize);
                dataSize = dev->bankCount * part->blockSize - overSize - underSize;
                memcpy(blkBuf + overSize, buf + scanLen, dataSize);

                if ((*dev->erase)(dev, blkPos) == ERROR)
                    {
                    SYS_FLASH_HW_DISABLE();
                    semGive(dev->devSem);
                    free(blkBuf);
                    return ERROR;
                    }
                for (j = 0; j < dev->bankCount * part->blockSize; j += stepSize)
                    {
                    if ((*dev->write)(dev, blkPos + j, blkBuf + j) == ERROR)
                        {
                        SYS_FLASH_HW_DISABLE();
                        semGive(dev->devSem);
                        free(blkBuf);
                        return ERROR;
                        }
                    }

                free(blkBuf);
                }
            else
                {
                if ((*dev->erase)(dev, blkPos) == ERROR)
                    {
                    SYS_FLASH_HW_DISABLE();
                    semGive(dev->devSem);
                    return ERROR;
                    }
                for (j = 0; j < dev->bankCount * part->blockSize; j += stepSize)
                    {
                    if ((*dev->write)(dev, blkPos + j, buf + scanLen + j) == ERROR)
                        {
                        SYS_FLASH_HW_DISABLE();
                        semGive(dev->devSem);
                        return ERROR;
                        }
                    }
                dataSize = dev->bankCount * part->blockSize;
                }

            scanLen += dataSize;
            overSize = 0;
            blkPos += dev->bankCount * part->blockSize;
            } while ((scanLen != len) && (blkPos < partPos));

        if (scanLen == len)
            break;
        }

    SYS_FLASH_HW_DISABLE();
    semGive(dev->devSem);

    return OK;
    }

/*******************************************************************************
*
* flashErase - 
*
*/
STATUS flashErase(FLASH_DEV *dev, UINT32 pos, UINT32 len)
    {
    FLASH_PART *part;
    UINT32 blkPos = pos;
    UINT32 partPos = 0;
    UINT32 i;

    if (!dev->inited)
        {
        if (flashInit() == ERROR)
            return ERROR;
        }

    if (pos + len > dev->devSize)
        {
        printf("flash erase position out of range!\n");
        return ERROR;
        }

    if (len == 0)
        return OK;

    semTake(dev->devSem, WAIT_FOREVER);
    SYS_FLASH_HW_ENABLE();

    for (i = 0; i < dev->partCount; i++)
        {
        part = dev->devPart + i;
        partPos += dev->bankCount * part->blockSize * part->blockCount;
        if (blkPos >= partPos)
            continue;

        blkPos &= (UINT32)~(dev->bankCount * part->blockSize - 1);
        do
            {
            if ((*dev->erase)(dev, blkPos) == ERROR)
                {
                SYS_FLASH_HW_DISABLE();
                semGive(dev->devSem);
                return ERROR;
                }
            blkPos += dev->bankCount * part->blockSize;
            } while ((blkPos < pos + len) && (blkPos < partPos));

        if (blkPos >= pos + len)
            break;
        }

    SYS_FLASH_HW_DISABLE();
    semGive(dev->devSem);

    return OK;
    }

/*******************************************************************************
*
* flashEraseAll - 
*
*/
STATUS flashEraseAll(FLASH_DEV *dev)
    {
    return flashErase(dev, 0, dev->devSize);
    }

/*******************************************************************************
*
* flashWritable - 
*
*/
STATUS flashWritable(FLASH_DEV *dev, UINT32 pos, UINT32 len)
    {
    UINT8 buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};

    if (!dev->inited)
        {
        if (flashInit() == ERROR)
            return ERROR;
        }

    if (pos + len > dev->devSize)
        {
        printf("flash write position out of range!\n");
        return ERROR;
        }

    if (len == 0)
        return OK;

    if ((pos | len) & (dev->portSize - 1))
        {
        printf("flash unaligned parameter!\n");
        return ERROR;
        }

    while (len)
        {
        (*dev->get)(dev, pos, buf);
        if (*(UINT64 *)buf != 0xFFFFFFFFFFFFFFFFLL)
            return FALSE;
        pos += dev->portSize;
        len -= dev->portSize;
        }

    return TRUE;
    }

/*******************************************************************************
*
* sysNvRamGet - 
*
*/
STATUS sysNvRamGet(char *string, int strLen, int offset)
    {
    FLASH_DEV *dev = FLASH_DEV_NVRAM;

    if (!dev->inited)
        {
        if (flashInit() == ERROR)
            return ERROR;
        }

    if (offset < 0 || strLen < 0 || offset + strLen > NV_RAM_SIZE)
        return ERROR;

    if (flashRead(dev, offset, string, strLen) == ERROR)
        return ERROR;

    return OK;
    }

/*******************************************************************************
*
* sysNvRamSet - 
*
*/
STATUS sysNvRamSet(char *string, int strLen, int offset)
    {
    FLASH_DEV *dev = FLASH_DEV_NVRAM;

    if (!dev->inited)
        {
        if (flashInit() == ERROR)
            return ERROR;
        }

    if (offset < 0 || strLen < 0 || offset + strLen > NV_RAM_SIZE)
        return ERROR;

    if (memcmp((char *)(dev->devAddr + offset), string, strLen) == 0)
        return OK;

    if (flashProgram(dev, offset, string, strLen) == ERROR)
        return ERROR;

    return OK;
    }

int WriteBootCfgPara(char *pvCfgPara, int length, unsigned long paraType)
{
    UINT offset = 0;
    FLASH_DEV *dev = FLASH_DEV_NVRAM;

    if (!dev->inited)
        {
        if (flashInit() == ERROR)
            return ERROR;
        }
 
    if (paraType == 1)
        offset = NV_RAM_SIZE;

    if (length < 0 || length > 0x200)
        return ERROR;
 
    return (flashProgram(dev,offset,pvCfgPara, length));
}


int ReadBootCfgPara (char *pvCfgPara, int length, UINT paraType)
{
    UINT offset = 0;
    FLASH_DEV *dev = FLASH_DEV_NVRAM;

    if (!dev->inited)
        {
        if (flashInit() == ERROR)
            return ERROR;
        }
 
    if (paraType == 1)
       offset = NV_RAM_SIZE;

    if (length < 0 || length > 0x100)
        return ERROR;

    return (flashRead(dev, offset,pvCfgPara, length));
}




/*
 *------------------------------------------------------------------------------
 * flashTest -
 *------------------------------------------------------------------------------
 */
/*lint -e449*/
STATUS flashTest(FLASH_DEV *dev)
    {
    FLASH_PART *part;
    UINT32 *buf = 0;
    UINT32 pos;
    int scanLen = 0;
    UINT32 i, j, k;
    int retVal = ERROR;

    if (flashCheck(dev) == ERROR)
        return ERROR;

    printf("\nErase dev%d...", (int)(dev - flashDev));

    if (flashEraseAll(dev) == ERROR)
        {
        printf("failed!\n");
        goto done;
        }
    else
        printf("done.\n");

    for (i = 0; i < dev->partCount; i++)
        {
        printf("Write part%d", i);
        part = dev->devPart + i;

        buf = malloc(dev->bankCount * part->blockSize);
        if (buf == 0)
            {
            printf("\nOut of memory!\n");
            goto done;
            }

        for (j = 0; j < part->blockCount; j++)
            {
            for (k = 0; k < dev->bankCount * part->blockSize / 4; k++)
                buf[k] = j << 24 | k;

            pos = dev->bankCount * part->blockSize * j + scanLen;
            if (flashWrite(dev, pos, (char *)buf, dev->bankCount * part->blockSize)
                == ERROR)
                {
                printf("\nWrite failed (part:%d, block:%d)\n", i, j);
                goto done;
                }

            printf(".");
            }

        scanLen += dev->bankCount * part->blockSize * part->blockCount;
        free(buf);
        printf("done.\n");
        }

    scanLen = 0;

    for (i = 0; i < dev->partCount; i++)
        {
        printf("Verify part%d", i);
        part = dev->devPart + i;

        buf = malloc(dev->bankCount * part->blockSize);
        if (buf == 0)
            {
            printf("\nOut of memory!\n");
            goto done;
            }

        for (j = 0; j < part->blockCount; j++)
            {
            pos = dev->bankCount * part->blockSize * j + scanLen;
            if (flashRead(dev, pos, (char *)buf, dev->bankCount * part->blockSize)
                == ERROR)
                {
                printf("\nRead failed (part:%d, block:%d)\n", i, j);
                goto done;
                }

            for (k = 0; k < dev->bankCount * part->blockSize / 4; k++)
                if (buf[k] != (j << 24 | k))
                    {
                    printf("\nVerify failed (part:%d, block:%d)\n", i, j);
                    goto done;
                    }

            printf(".");
            }

        scanLen += dev->bankCount * part->blockSize * part->blockCount;
        free(buf);
        printf("ok.\n");
        }

    printf("Erase dev%d...", (int)(dev - flashDev));

    if (flashEraseAll(dev) == ERROR)
        {
        printf("failed!\n");
        goto done;
        }
    else
        printf("done.\n");

    retVal = OK;

done:
    if (buf && retVal != OK)
        free(buf);

    if (retVal == OK)
        printf("Flash test device %d passed!\n", (int)(dev - flashDev));
    else
        printf("Flash test device %d failed!\n", (int)(dev - flashDev));

    return retVal;
    }
/*lint +e449*/
STATUS flashTestAll(void)
    {
    int i;
    int retVal = OK;

    printf("\nFlash test all: devices = %d\n", devCount);

    if (flashInit() == ERROR)
        return ERROR;

    for (i = 1; i < devCount; i++)
        {
        FLASH_DEV *dev = &flashDev[i];

        if (dev->inited && flashTest(dev) == ERROR)
            retVal = ERROR;
        }

    if (retVal == OK)
        printf("\nFlash test all passed!\n");
    else
        printf("\nFailed because of earlier errors!\n");

    return retVal;
    }