[U-Boot] [U-BOOT][PATCH 4/4] mflash : Support word unaligned access

[U-Boot] [U-BOOT][PATCH 4/4] mflash : Support word unaligned access

[unsik Kim]

Signed-off-by: unsik Kim <donari75@gmail.com>
---
  drivers/block/mg_disk.c     |   38 ++++++++++++++++++++++++++------------
  drivers/block/mg_disk_prv.h |    5 +++++
  2 files changed, 31 insertions(+), 12 deletions(-)

diff --git a/drivers/block/mg_disk.c b/drivers/block/mg_disk.c
index 113aa79..b74307a 100644
--- a/drivers/block/mg_disk.c
+++ b/drivers/block/mg_disk.c
@@ -240,6 +240,7 @@ static unsigned int mg_do_read_sects(void *buff, u32 sect_num, u32 sect_cnt)
  {
  	u32 i, j, err;
  	u8 *buff_ptr = buff;
+	union mg_uniwb uniwb;

  	err = mg_out(sect_num, sect_cnt, MG_CMD_RD);
  	if (err)
@@ -250,13 +251,20 @@ static unsigned int mg_do_read_sects(void *buff, u32 sect_num, u32 sect_cnt)
  		if (err)
  			return err;

-		/* TODO : u16 unaligned case */
-		for(j = 0; j < MG_SECTOR_SIZE >> 1; j++) {
-			*(u16 *)buff_ptr =
-				readw(mg_base() + MG_BUFF_OFFSET + (j << 1));
-			buff_ptr += 2;
+		if ((u32)buff_ptr & 1) {
+			for (j = 0; j < MG_SECTOR_SIZE >> 1; j++) {
+				uniwb.w = readw(mg_base() + MG_BUFF_OFFSET
+						+ (j << 1));
+				*buff_ptr++ = uniwb.b[0];
+				*buff_ptr++ = uniwb.b[1];
+			}
+		} else {
+			for(j = 0; j < MG_SECTOR_SIZE >> 1; j++) {
+				*(u16 *)buff_ptr = readw(mg_base() +
+						MG_BUFF_OFFSET + (j << 1));
+				buff_ptr += 2;
+			}
  		}
-
  		writeb(MG_CMD_RD_CONF, mg_base() + MG_REG_COMMAND);

  		MG_DBG("%u (0x%8.8x) sector read", sect_num + i,
@@ -372,6 +380,7 @@ static int mg_do_write_sects(void *buff, u32 sect_num, u32 sect_cnt)
  {
  	u32 i, j, err;
  	u8 *buff_ptr = buff;
+	union mg_uniwb uniwb;

  	err = mg_out(sect_num, sect_cnt, MG_CMD_WR);
  	if (err)
@@ -382,13 +391,18 @@ static int mg_do_write_sects(void *buff, u32 sect_num, u32 sect_cnt)
  		if (err)
  			return err;

-		/* TODO : u16 unaligned case */
-		for(j = 0; j < MG_SECTOR_SIZE >> 1; j++) {
-			writew(*(u16 *)buff_ptr,
-				mg_base() + MG_BUFF_OFFSET + (j << 1));
-			buff_ptr += 2;
+		if ((u32)buff_ptr & 1) {
+			uniwb.b[0] = *buff_ptr++;
+			uniwb.b[1] = *buff_ptr++;
+			writew(uniwb.w, mg_base() + MG_BUFF_OFFSET + (j << 1));
+		} else {
+			for(j = 0; j < MG_SECTOR_SIZE >> 1; j++) {
+				writew(*(u16 *)buff_ptr,
+						mg_base() + MG_BUFF_OFFSET +
+						(j << 1));
+				buff_ptr += 2;
+			}
  		}
-
  		writeb(MG_CMD_WR_CONF, mg_base() + MG_REG_COMMAND);

  		MG_DBG("%u (0x%8.8x) sector write",
diff --git a/drivers/block/mg_disk_prv.h b/drivers/block/mg_disk_prv.h
index a6b7299..8f22e28 100644
--- a/drivers/block/mg_disk_prv.h
+++ b/drivers/block/mg_disk_prv.h
@@ -120,6 +120,11 @@
  #define MG_CMD_WR_CONF	0x3C
  #define MG_CMD_RD_CONF	0x40

+union mg_uniwb{
+	u16 w;
+	u8 b[2];
+};
+
  /* main structure for mflash driver */
  struct mg_host {
  	struct mg_drv_data *drv_data;
-- 
1.5.6.6