Changeset d9f4c76 in mainline for uspace/srv/bd/ata_bd/ata_bd.c

Timestamp:

2009-06-13T12:26:05Z (16 years ago)

Author:

Jiri Svoboda <jirik.svoboda@…>

Branches:

lfn, master, serial, ticket/834-toolchain-update, topic/msim-upgrade, topic/simplify-dev-export

Children:

2a18193c

Parents:

5481d1bb

Message:

Write support in ata_bd. Not used, not tested.

File:

• uspace/srv/bd/ata_bd/ata_bd.c (modified) (3 diffs)

uspace/srv/bd/ata_bd/ata_bd.c

@@ -77 +77 @@
 static int ata_bd_read_block(int disk_id, uint64_t blk_idx, size_t blk_cnt,
     void *buf);
+static int ata_bd_write_block(int disk_id, uint64_t blk_idx, size_t blk_cnt,
+    const void *buf);
 static int drive_identify(int drive_id, disk_t *d);
 
@@ -307 +309 @@
                         rc = ata_bd_read_block(disk_id, blk_idx, 1, buf);
                 else
-                        rc = ENOTSUP;
+                        rc = ata_bd_write_block(disk_id, blk_idx, 1, buf);
 
                 if (rc != EOK)
@@ -380 +382 @@
 }
 
+static int ata_bd_write_block(int disk_id, uint64_t blk_idx, size_t blk_cnt,
+    const void *buf)
+{
+        size_t i;
+        uint8_t status;
+        uint64_t c, h, s;
+        uint64_t idx;
+        uint8_t drv_head;
+        disk_t *d;
+
+        d = &disk[disk_id];
+
+        /* Check device bounds. */
+        if (blk_idx >= d->blocks)
+                return EINVAL;
+
+        /* Compute CHS. */
+        c = blk_idx / (d->heads * d->sectors);
+        idx = blk_idx % (d->heads * d->sectors);
+
+        h = idx / d->sectors;
+        s = 1 + (idx % d->sectors);
+
+        /* New value for Drive/Head register */
+        drv_head =
+            ((disk_id != 0) ? DHR_DRV : 0) |
+            (h & 0x0f);
+
+        futex_down(&dev_futex);
+
+        /* Program a Read Sectors operation. */
+
+        pio_write_8(&cmd->drive_head, drv_head);
+        pio_write_8(&cmd->sector_count, 1);
+        pio_write_8(&cmd->sector_number, s);
+        pio_write_8(&cmd->cylinder_low, c & 0xff);
+        pio_write_8(&cmd->cylinder_high, c >> 16);
+        pio_write_8(&cmd->command, CMD_WRITE_SECTORS);
+
+        /* Write data to the disk buffer. */
+
+        for (i = 0; i < block_size / 2; i++) {
+                do {
+                        status = pio_read_8(&cmd->status);
+                } while ((status & SR_DRDY) == 0);
+
+                pio_write_16(&cmd->data_port, ((uint16_t *) buf)[i]);
+        }
+
+        futex_up(&dev_futex);
+        return EOK;
+}
+
 
 /**