@@ -1590,33 +1590,42 @@ uint8_t modSPIRead(uint32_t offset, uint32_t size, uint8_t *dst)
15901590 uint32_t toAlign ;
15911591
15921592 if (offset & 3 ) { // long align offset
1593- toAlign = 4 - (offset & 3 );
1594- if (toAlign > size )
1595- toAlign = size ;
15961593 if (SPI_FLASH_RESULT_OK != spi_flash_read (offset & ~3 , (uint32_t * )temp , 4 ))
15971594 return 0 ;
1598- if (size < toAlign ) {
1599- c_memcpy (dst , temp + 4 - toAlign , size );
1600- toAlign = size ;
1601- }
1602- else
1603- c_memcpy (dst , temp + 4 - toAlign , toAlign );
1595+
1596+ toAlign = 4 - (offset & 3 );
1597+ c_memcpy (dst , temp + 4 - toAlign , (size < toAlign ) ? size : toAlign );
1598+
1599+ if (size <= toAlign )
1600+ return 1 ;
1601+
16041602 dst += toAlign ;
16051603 offset += toAlign ;
16061604 size -= toAlign ;
1607- if (!size )
1608- return 1 ;
16091605 }
16101606
16111607 toAlign = size & ~3 ;
16121608 if (toAlign ) {
1613- if (SPI_FLASH_RESULT_OK != spi_flash_read (offset , (uint32_t * )temp , toAlign ))
1614- return 0 ;
1615- c_memmove (dst , temp , toAlign );
1616-
1617- dst += toAlign ;
1618- offset += toAlign ;
16191609 size -= toAlign ;
1610+ if (((uintptr_t )dst ) & ~3 ) { // dst is not long aligned, copy through stack
1611+ while (toAlign ) {
1612+ uint32_t use = (toAlign > sizeof (temp )) ? sizeof (temp ) : toAlign ;
1613+
1614+ if (SPI_FLASH_RESULT_OK != spi_flash_read (offset , (uint32_t * )temp , use ))
1615+ return 0 ;
1616+ c_memmove (dst , temp , use );
1617+
1618+ toAlign -= use ;
1619+ dst += use ;
1620+ offset += use ;
1621+ }
1622+ }
1623+ else {
1624+ if (SPI_FLASH_RESULT_OK != spi_flash_read (offset , (uint32_t * )dst , toAlign ))
1625+ return 0 ;
1626+ dst += toAlign ;
1627+ offset += toAlign ;
1628+ }
16201629 }
16211630
16221631 if (size ) { // long align tail
@@ -1630,35 +1639,34 @@ uint8_t modSPIRead(uint32_t offset, uint32_t size, uint8_t *dst)
16301639
16311640uint8_t modSPIWrite (uint32_t offset , uint32_t size , const uint8_t * src )
16321641{
1633- uint8_t temp [4 ] __attribute__ ((aligned (4 )));
1642+ uint8_t temp [512 ] __attribute__ ((aligned (4 )));
16341643 uint32_t toAlign ;
16351644
16361645 if (offset & 3 ) { // long align offset
16371646 toAlign = 4 - (offset & 3 );
1638- if (toAlign > size )
1639- toAlign = size ;
1640- c_memset (temp , 0xFF , sizeof (temp ));
1641- c_memcpy (temp + 4 - toAlign , src , toAlign );
1642- if (SPI_FLASH_RESULT_OK != spi_flash_write (offset & ~3 , (uint32_t * )temp , sizeof (temp )))
1647+ c_memset (temp , 0xFF , 4 );
1648+ c_memcpy (temp + 4 - toAlign , src , (size < toAlign ) ? size : toAlign );
1649+ if (SPI_FLASH_RESULT_OK != spi_flash_write (offset & ~3 , (uint32_t * )temp , 4 ))
16431650 return 0 ;
16441651
1652+ if (size <= toAlign )
1653+ return 1 ;
1654+
16451655 src += toAlign ;
16461656 offset += toAlign ;
16471657 size -= toAlign ;
1648- if (!size )
1649- return 1 ;
16501658 }
16511659
1652- if ( size >= 4 ) {
1653- toAlign = size & ~ 3 ;
1660+ toAlign = size & ~ 3 ;
1661+ if ( toAlign ) {
16541662 size -= toAlign ;
16551663 if (((uintptr_t )src ) & ~3 ) { // src is not long aligned, copy through stack
1656- while (toAlign >= 4 ) {
1657- uint8_t scratch [512 ] __attribute__ ((aligned (4 )));;
1658- uint32_t use = (toAlign > sizeof (scratch )) ? sizeof (scratch ) : toAlign ;
1659- c_memcpy (scratch , src , use );
1660- if (SPI_FLASH_RESULT_OK != spi_flash_write (offset , (uint32_t * )scratch , use ))
1664+ while (toAlign ) {
1665+ uint32_t use = (toAlign > sizeof (temp )) ? sizeof (temp ) : toAlign ;
1666+ c_memcpy (temp , src , use );
1667+ if (SPI_FLASH_RESULT_OK != spi_flash_write (offset , (uint32_t * )temp , use ))
16611668 return 0 ;
1669+
16621670 toAlign -= use ;
16631671 src += use ;
16641672 offset += use ;
@@ -1667,16 +1675,15 @@ uint8_t modSPIWrite(uint32_t offset, uint32_t size, const uint8_t *src)
16671675 else {
16681676 if (SPI_FLASH_RESULT_OK != spi_flash_write (offset , (uint32_t * )src , toAlign ))
16691677 return 0 ;
1670-
16711678 src += toAlign ;
16721679 offset += toAlign ;
16731680 }
16741681 }
16751682
16761683 if (size ) { // long align tail
1677- c_memset (temp , 0xFF , sizeof ( temp ) );
1684+ c_memset (temp , 0xFF , 4 );
16781685 c_memcpy (temp , src , size );
1679- if (SPI_FLASH_RESULT_OK != spi_flash_write (offset , (uint32_t * )temp , sizeof ( temp ) ))
1686+ if (SPI_FLASH_RESULT_OK != spi_flash_write (offset , (uint32_t * )temp , 4 ))
16801687 return 0 ;
16811688 }
16821689
0 commit comments