static int Q6Polling (U32 adr) {
U32 fsreg;
U32 dqold;
fsreg = M16(adr);
do {
dqold = fsreg & DQ6;
fsreg = M16(adr);
if ((fsreg & DQ6) == dqold) {
/* OK, Done. */
return (0);
}
dqold = fsreg & DQ6;
} while ((fsreg & DQ5) < DQ5);
fsreg = M16(adr);
dqold = fsreg & DQ6;
fsreg = M16(adr);
if ((fsreg & DQ6) == dqold) {
/* OK, Done */
return (0);
}
/* Error, Reset Flash Device */
M16(adr) = RESET;
return (1);
} /* end of Q6Polling */
int fs_ProgramPage (U32 adr, U32 sz, U8 *buf) {
for ( ; sz; sz -= 2, adr += 2, buf += 2) {
M16(base_adr | 0xAAA) = 0xAA;
M16(base_adr | 0x554) = 0x55;
M16(base_adr | 0xAAA) = PROGRAM;
/* 'buf' might be unaligned. */
M16(adr) = *(__packed U16 *)buf;
/* Wait until Programming completed */
if (Q6Polling (adr)) {
return (1);
}
}
return (0);
} /* end of fs_ProgramPage */
int ProgramPage (unsigned long adr, unsigned long sz, unsigned char *buf) {
sz = (sz + 1) & ~1; // Adjust size for Half Words
FLASH->SR |= FLASH_PGERR; // Reset Error Flags
while (sz) {
FLASH->CR |= FLASH_OPTPG; // Option Byte Programming Enabled
M16(adr) = *((u16 *)buf); // Program Half Word
while (FLASH->SR & FLASH_BSY) {
IWDG->KR = 0xAAAA; // Reload IWDG
}
FLASH->CR &= ~FLASH_OPTPG; // Option Byte Programming Disabled
if (FLASH->SR & FLASH_ERR) { // Check for Errors
FLASH->SR |= FLASH_ERR;
return (1); // Failed
}
adr += 2; // Go to next Half Word
buf += 2;
sz -= 2;
}
return (0); // Done
}
int EraseChip (void) {
FLASH->SR |= FLASH_ERR; // Reset Error Flags
FLASH->CR |= FLASH_OPTER; // Option Byte Erase Enabled
FLASH->CR |= FLASH_STRT; // Start Erase
while (FLASH->SR & FLASH_BSY) {
IWDG->KR = 0xAAAA; // Reload IWDG
}
FLASH->CR &= ~FLASH_OPTER; // Option Byte Erase Disabled
// Unprotect Flash
FLASH->CR |= FLASH_OPTPG; // Option Byte Programming Enabled
M16(0x1FFFF800) = RDPRT_KEY; // Program Half Word: RDPRT Key
while (FLASH->SR & FLASH_BSY) {
IWDG->KR = 0xAAAA; // Reload IWDG
}
FLASH->CR &= ~FLASH_OPTPG; // Option Byte Programming Disabled
if (FLASH->SR & FLASH_ERR) { // Check for Errors
FLASH->SR |= FLASH_ERR;
return (1); // Failed
}
return (0); // Done
}
void code2(int mode, LPSYMBOL s1, LPSYMBOL s2)
{
switch (mode) {
case AM_R8:
R8(s1->instr, s2->val8);
break;
case AM_R16:
R16(s1->instr, s2->val8);
break;
case AM_M8:
M8(s1->instr, s2->val8);
break;
case AM_M16:
M16(s1->instr, s2->val8);
break;
case AM_A8:
A8(s1->instr, s2->val16);
break;
case AM_A16:
A16(s1->instr, s2->val16);
break;
case AM_I16:
I16(s1->instr, s2->val16);
break;
case AM_I8:
I8(s1->instr, s2->val8);
break;
default:
break;
}
}
static void cvtSM1616K(const unsigned char* src, int ns, unsigned char* dst)
{
TRACE("(%p, %d, %p)\n", src, ns, dst);
while (ns--) {
W16(dst, M16(R16(src),R16(src+2))); dst += 2;
src += 4;
}
}
static void cvtSM168K(const unsigned char* src, int ns, unsigned char* dst)
{
TRACE("(%p, %d, %p)\n", src, ns, dst);
while (ns--) {
*dst++ = C168(M16(R16(src), R16(src + 2)));
src += 4;
}
}
static void cvtSM816K(const unsigned char* src, int ns, unsigned char* dst)
{
short v;
TRACE("(%p, %d, %p)\n", src, ns, dst);
while (ns--) {
v = M16(C816(src[0]), C816(src[1]));
src += 2;
W16(dst, v); dst += 2;
}
}
static void cvtSM1616C(DWORD srcRate, const unsigned char* src, LPDWORD nsrc,
DWORD dstRate, unsigned char* dst, LPDWORD ndst)
{
DWORD error = dstRate / 2;
TRACE("(%d, %p, %p, %d, %p, %p)\n", srcRate, src, nsrc, dstRate, dst, ndst);
while ((*ndst)--) {
W16(dst, M16(R16(src), R16(src + 2))); dst += 2;
error = error + srcRate;
while (error > dstRate) {
src += 4;
(*nsrc)--;
if (*nsrc == 0)
return;
error = error - dstRate;
}
}
}
int fs_EraseSector (U32 adr) {
U32 fsreg;
M16(base_adr | 0xAAA) = 0xAA;
M16(base_adr | 0x554) = 0x55;
M16(base_adr | 0xAAA) = ERASE;
M16(base_adr | 0xAAA) = 0xAA;
M16(base_adr | 0x554) = 0x55;
M16(adr) = ERA_SECT;
/* Wait for Sector Erase Timeout. */
do {
fsreg = M16(adr);
} while ((fsreg & DQ3) < DQ3);
/* Wait until Erase Completed */
return (Q6Polling (adr));
} /* end of fs_EraseSector */
