diff options
| -rw-r--r-- | drivers/block/mg_disk.c | 89 |
1 files changed, 47 insertions, 42 deletions
diff --git a/drivers/block/mg_disk.c b/drivers/block/mg_disk.c index 6440d5945414..19917d5481bd 100644 --- a/drivers/block/mg_disk.c +++ b/drivers/block/mg_disk.c @@ -36,7 +36,6 @@ /* Register offsets */ #define MG_BUFF_OFFSET 0x8000 -#define MG_STORAGE_BUFFER_SIZE 0x200 #define MG_REG_OFFSET 0xC000 #define MG_REG_FEATURE (MG_REG_OFFSET + 2) /* write case */ #define MG_REG_ERROR (MG_REG_OFFSET + 2) /* read case */ @@ -477,9 +476,18 @@ static unsigned int mg_out(struct mg_host *host, return MG_ERR_NONE; } +static void mg_read_one(struct mg_host *host, struct request *req) +{ + u16 *buff = (u16 *)req->buffer; + u32 i; + + for (i = 0; i < MG_SECTOR_SIZE >> 1; i++) + *buff++ = inw((unsigned long)host->dev_base + MG_BUFF_OFFSET + + (i << 1)); +} + static void mg_read(struct request *req) { - u32 j; struct mg_host *host = req->rq_disk->private_data; if (mg_out(host, blk_rq_pos(req), blk_rq_sectors(req), @@ -490,26 +498,33 @@ static void mg_read(struct request *req) blk_rq_sectors(req), blk_rq_pos(req), req->buffer); do { - u16 *buff = (u16 *)req->buffer; - if (mg_wait(host, ATA_DRQ, MG_TMAX_WAIT_RD_DRQ) != MG_ERR_NONE) { mg_bad_rw_intr(host); return; } - for (j = 0; j < MG_SECTOR_SIZE >> 1; j++) - *buff++ = inw((unsigned long)host->dev_base + - MG_BUFF_OFFSET + (j << 1)); + + mg_read_one(host, req); outb(MG_CMD_RD_CONF, (unsigned long)host->dev_base + MG_REG_COMMAND); } while (mg_end_request(host, 0, MG_SECTOR_SIZE)); } +static void mg_write_one(struct mg_host *host, struct request *req) +{ + u16 *buff = (u16 *)req->buffer; + u32 i; + + for (i = 0; i < MG_SECTOR_SIZE >> 1; i++) + outw(*buff++, (unsigned long)host->dev_base + MG_BUFF_OFFSET + + (i << 1)); +} + static void mg_write(struct request *req) { - u32 j; struct mg_host *host = req->rq_disk->private_data; + bool rem; if (mg_out(host, blk_rq_pos(req), blk_rq_sectors(req), MG_CMD_WR, NULL) != MG_ERR_NONE) { @@ -520,27 +535,37 @@ static void mg_write(struct request *req) MG_DBG("requested %d sects (from %ld), buffer=0x%p\n", blk_rq_sectors(req), blk_rq_pos(req), req->buffer); - do { - u16 *buff = (u16 *)req->buffer; + if (mg_wait(host, ATA_DRQ, + MG_TMAX_WAIT_WR_DRQ) != MG_ERR_NONE) { + mg_bad_rw_intr(host); + return; + } + + mg_write_one(host, req); + + outb(MG_CMD_WR_CONF, (unsigned long)host->dev_base + MG_REG_COMMAND); - if (mg_wait(host, ATA_DRQ, MG_TMAX_WAIT_WR_DRQ) != MG_ERR_NONE) { + do { + if (blk_rq_sectors(req) > 1 && + mg_wait(host, ATA_DRQ, + MG_TMAX_WAIT_WR_DRQ) != MG_ERR_NONE) { mg_bad_rw_intr(host); return; } - for (j = 0; j < MG_SECTOR_SIZE >> 1; j++) - outw(*buff++, (unsigned long)host->dev_base + - MG_BUFF_OFFSET + (j << 1)); - outb(MG_CMD_WR_CONF, (unsigned long)host->dev_base + - MG_REG_COMMAND); - } while (mg_end_request(host, 0, MG_SECTOR_SIZE)); + rem = mg_end_request(host, 0, MG_SECTOR_SIZE); + if (rem) + mg_write_one(host, req); + + outb(MG_CMD_WR_CONF, + (unsigned long)host->dev_base + MG_REG_COMMAND); + } while (rem); } static void mg_read_intr(struct mg_host *host) { struct request *req = host->req; u32 i; - u16 *buff; /* check status */ do { @@ -558,13 +583,7 @@ static void mg_read_intr(struct mg_host *host) return; ok_to_read: - /* get current segment of request */ - buff = (u16 *)req->buffer; - - /* read 1 sector */ - for (i = 0; i < MG_SECTOR_SIZE >> 1; i++) - *buff++ = inw((unsigned long)host->dev_base + MG_BUFF_OFFSET + - (i << 1)); + mg_read_one(host, req); MG_DBG("sector %ld, remaining=%ld, buffer=0x%p\n", blk_rq_pos(req), blk_rq_sectors(req) - 1, req->buffer); @@ -583,8 +602,7 @@ ok_to_read: static void mg_write_intr(struct mg_host *host) { struct request *req = host->req; - u32 i, j; - u16 *buff; + u32 i; bool rem; /* check status */ @@ -605,12 +623,7 @@ static void mg_write_intr(struct mg_host *host) ok_to_write: if ((rem = mg_end_request(host, 0, MG_SECTOR_SIZE))) { /* write 1 sector and set handler if remains */ - buff = (u16 *)req->buffer; - for (j = 0; j < MG_STORAGE_BUFFER_SIZE >> 1; j++) { - outw(*buff, (unsigned long)host->dev_base + - MG_BUFF_OFFSET + (j << 1)); - buff++; - } + mg_write_one(host, req); MG_DBG("sector %ld, remaining=%ld, buffer=0x%p\n", blk_rq_pos(req), blk_rq_sectors(req), req->buffer); host->mg_do_intr = mg_write_intr; @@ -675,9 +688,6 @@ static unsigned int mg_issue_req(struct request *req, unsigned int sect_num, unsigned int sect_cnt) { - u16 *buff; - u32 i; - switch (rq_data_dir(req)) { case READ: if (mg_out(host, sect_num, sect_cnt, MG_CMD_RD, &mg_read_intr) @@ -701,12 +711,7 @@ static unsigned int mg_issue_req(struct request *req, mg_bad_rw_intr(host); return host->error; } - buff = (u16 *)req->buffer; - for (i = 0; i < MG_SECTOR_SIZE >> 1; i++) { - outw(*buff, (unsigned long)host->dev_base + - MG_BUFF_OFFSET + (i << 1)); - buff++; - } + mg_write_one(host, req); mod_timer(&host->timer, jiffies + 3 * HZ); outb(MG_CMD_WR_CONF, (unsigned long)host->dev_base + MG_REG_COMMAND); |