diff --git a/lib/CPUs/MIPS/bsp/examples/cfiflash.c b/lib/CPUs/MIPS/bsp/examples/cfiflash.c
index 7ab15be..bbfbe43 100644
--- a/lib/CPUs/MIPS/bsp/examples/cfiflash.c
+++ b/lib/CPUs/MIPS/bsp/examples/cfiflash.c
@@ -91,7 +91,7 @@ UINT32 cfi_find(flash_t *pObj)
 	for (i=0; i < size; i++)
 		pObj->info.wbuf_size *= 2;
 
-	pObj->info.nblocks = pObj->info.num_flash * ((UINT32)(pF16[2*0x2E] << 8 | pF16[2*0x2D]) + 1);
+	pObj->info.nblocks = (UINT32)(pF16[2*0x2E] << 8 | pF16[2*0x2D]) + 1;
 	pObj->info.blocksize = pObj->info.num_flash * ((UINT32)(pF16[2*0x30] << 8 | pF16[2*0x2F]) * 256);
 
 	*pF = 0x00FF00FF;
@@ -345,6 +345,9 @@ UINT32 flash_erase(flash_t *pObj, UINT32 offset, UINT32 size)
 	int i;
 	UINT32 offset_end, error;
 
+	if (size % 4)
+		size = 4*(size/4 + 1);
+
 	offset_end = offset + size;
 
 	for (i=offset; i < offset_end; i += pObj->info.blocksize)
@@ -366,6 +369,9 @@ UINT32 flash_erase(flash_t *pObj, UINT32 offset, UINT32 size)
 
 UINT32 flash_program(flash_t *pObj, UINT32 offset, UINT8 *pData, UINT32 size)
 {
+	if (size % 4)
+		size = 4*(size/4 + 1);
+
 	return cfi_program_multi(pObj, offset/4, (UINT32*)pData, size/4);
 }