spm.c

KaOS is a real-time operating system that has been implemented with the basic real-time constraints

/*
#asm
.cseg
.org mainflash
#endasm
*/
void ReadPage(UINT pageStart, UINT *instArr, UCHAR instCount)
{
    register int i;
    for (i = 0; i < instCount; ++i)
    {
        instArr[i] = ReadInst(pageStart + (i << 1));
    }
}
void WriteSector(UINT sectorStart, UINT *instArr)
{
    WritePage(sectorStart + 0, instArr+0, 64);
    WritePage(sectorStart + 128, instArr+64, 64);
    WritePage(sectorStart + 256, instArr+128, 64);
    WritePage(sectorStart + 384, instArr+192, 64);
}
/*
#asm
.org bootloader
#endasm
*/

// This is the only code that really needs to reside in the bootloader
// since it is reading/writing from flash
UINT ReadInst(UINT instAddr)
{
    g_instaddr = instAddr;
    #asm
        movw r30, r6       ; // move instAddr to Z pointer  
        lpm r2, Z+         ; // read LSB
        lpm r3, Z          ; // read MSB
    #endasm
    return g_instdata;
}
// instCount must be <= 64 (instructions) (128 bytes)
void WritePage(UINT pageStart, UINT *instArr, UCHAR instCount)
{
    register UCHAR i;
    #asm("cli");
    g_pageaddr = pageStart;

    for (i = 0; i < instCount; ++i)
    {
        // do some big-endian / little-endian crap
        g_instdata = (instArr[i] >> 8) | (instArr[i] << 8);
        while (SPMCR&1); //wait for spm complete
        g_instaddr = pageStart + (i << 1);

        g_spmcrval=1;
        #asm 
        movw r30, r6        ;//move CurrentAddress to Z pointer   
        mov r1, r3          ;//move Pagedata MSB reg 1
        mov r0, r2          ;//move Pagedata LSB reg 0  
        sts SpmcrAddr, r10  ;//move spmcrval to SPM control register
        spm                 ;//store program memory
        #endasm
    }	
	
    while (SPMCR&1);    //wait for spm complete
    g_spmcrval=3;       //erase page cmd
    #asm 
    movw r30, r4       ;//move pageStart to Z pointer
    sts SpmcrAddr, r10 ;//move spmcrval to SPM control register              
    spm                ;//erase page
    #endasm

    while (SPMCR&1);    //wait for spm complete
    g_spmcrval=5;       //write page cmd
    #asm 
    movw r30, r4       ;//move PageAddress to Z pointer
    sts SpmcrAddr, r10 ;//move spmcrval to SPM control register              
    spm                ;//write page
    #endasm

    while (SPMCR&1);    //wait for spm complete
    g_spmcrval=0x11;    //enableRWW

    #asm 
    sts SpmcrAddr, r10 ;//move spmcrval to SPMCR              
    spm   
    #endasm

    #asm("sei");
}