Changeset 5048be7 in mainline


Ignore:
Timestamp:
2009-08-23T20:41:51Z (15 years ago)
Author:
Jiri Svoboda <jiri@…>
Branches:
lfn, master, serial, ticket/834-toolchain-update, topic/msim-upgrade, topic/simplify-dev-export
Children:
21d8020
Parents:
1c1657c
Message:

Move code to compute and program block coordinates to separate functions to avoid duplication.

Location:
uspace/srv/bd/ata_bd
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • uspace/srv/bd/ata_bd/ata_bd.c

    r1c1657c r5048be7  
    9595static int drive_identify(int drive_id, void *buf);
    9696static void disk_print_summary(disk_t *d);
     97static int coord_calc(disk_t *d, uint64_t blk_idx, block_coord_t *bc);
     98static void coord_sc_program(const block_coord_t *bc, uint16_t scnt);
    9799static int wait_status(unsigned set, unsigned n_reset, uint8_t *pstatus,
    98100    unsigned timeout);
    … …  
    476478        uint16_t data;
    477479        uint8_t status;
    478         uint64_t c, h, s, c1, s1;
    479         uint64_t idx;
    480480        uint8_t drv_head;
    481481        disk_t *d;
     482        block_coord_t bc;
    482483
    483484        d = &disk[disk_id];
     485        bc.h = 0;       /* Silence warning. */
     486
     487        /* Compute block coordinates. */
     488        if (coord_calc(d, blk_idx, &bc) != EOK)
     489                return EINVAL;
     490
     491        /* New value for Drive/Head register */
     492        drv_head =
     493            ((disk_id != 0) ? DHR_DRV : 0) |
     494            ((d->amode != am_chs) ? DHR_LBA : 0) |
     495            (bc.h & 0x0f);
     496
     497        fibril_mutex_lock(&d->lock);
     498
     499        /* Program a Read Sectors operation. */
     500
     501        if (wait_status(0, ~SR_BSY, NULL, TIMEOUT_BSY) != EOK) {
     502                fibril_mutex_unlock(&d->lock);
     503                return EIO;
     504        }
     505
     506        pio_write_8(&cmd->drive_head, drv_head);
     507
     508        if (wait_status(SR_DRDY, ~SR_BSY, NULL, TIMEOUT_DRDY) != EOK) {
     509                fibril_mutex_unlock(&d->lock);
     510                return EIO;
     511        }
     512
     513        /* Program block coordinates into the device. */
     514        coord_sc_program(&bc, 1);
     515
     516        pio_write_8(&cmd->command, d->amode == am_lba48 ?
     517            CMD_READ_SECTORS_EXT : CMD_READ_SECTORS);
     518
     519        if (wait_status(0, ~SR_BSY, &status, TIMEOUT_BSY) != EOK) {
     520                fibril_mutex_unlock(&d->lock);
     521                return EIO;
     522        }
     523
     524        if ((status & SR_DRQ) != 0) {
     525                /* Read data from the device buffer. */
     526
     527                for (i = 0; i < block_size / 2; i++) {
     528                        data = pio_read_16(&cmd->data_port);
     529                        ((uint16_t *) buf)[i] = data;
     530                }
     531        }
     532
     533        if ((status & SR_ERR) != 0)
     534                return EIO;
     535
     536        fibril_mutex_unlock(&d->lock);
     537        return EOK;
     538}
     539
     540/** Write a physical block to the device.
     541 *
     542 * @param disk_id       Device index (0 or 1)
     543 * @param blk_idx       Index of the first block.
     544 * @param blk_cnt       Number of blocks to transfer.
     545 * @param buf           Buffer holding the data to write.
     546 *
     547 * @return EOK on success, EIO on error.
     548 */
     549static int ata_bd_write_block(int disk_id, uint64_t blk_idx, size_t blk_cnt,
     550    const void *buf)
     551{
     552        size_t i;
     553        uint8_t status;
     554        uint8_t drv_head;
     555        disk_t *d;
     556        block_coord_t bc;
     557
     558        d = &disk[disk_id];
     559        bc.h = 0;       /* Silence warning. */
     560
     561        /* Compute block coordinates. */
     562        if (coord_calc(d, blk_idx, &bc) != EOK)
     563                return EINVAL;
     564
     565        /* New value for Drive/Head register */
     566        drv_head =
     567            ((disk_id != 0) ? DHR_DRV : 0) |
     568            ((d->amode != am_chs) ? DHR_LBA : 0) |
     569            (bc.h & 0x0f);
     570
     571        fibril_mutex_lock(&d->lock);
     572
     573        /* Program a Write Sectors operation. */
     574
     575        if (wait_status(0, ~SR_BSY, NULL, TIMEOUT_BSY) != EOK) {
     576                fibril_mutex_unlock(&d->lock);
     577                return EIO;
     578        }
     579
     580        pio_write_8(&cmd->drive_head, drv_head);
     581
     582        if (wait_status(SR_DRDY, ~SR_BSY, NULL, TIMEOUT_DRDY) != EOK) {
     583                fibril_mutex_unlock(&d->lock);
     584                return EIO;
     585        }
     586
     587        /* Program block coordinates into the device. */
     588        coord_sc_program(&bc, 1);
     589
     590        pio_write_8(&cmd->command, d->amode == am_lba48 ?
     591            CMD_WRITE_SECTORS_EXT : CMD_WRITE_SECTORS);
     592
     593        if (wait_status(0, ~SR_BSY, &status, TIMEOUT_BSY) != EOK) {
     594                fibril_mutex_unlock(&d->lock);
     595                return EIO;
     596        }
     597
     598        if ((status & SR_DRQ) != 0) {
     599                /* Write data to the device buffer. */
     600
     601                for (i = 0; i < block_size / 2; i++) {
     602                        pio_write_16(&cmd->data_port, ((uint16_t *) buf)[i]);
     603                }
     604        }
     605
     606        fibril_mutex_unlock(&d->lock);
     607
     608        if (status & SR_ERR)
     609                return EIO;
     610
     611        return EOK;
     612}
     613
     614/** Calculate block coordinates.
     615 *
     616 * Calculates block coordinates in the best coordinate system supported
     617 * by the device. These can be later programmed into the device using
     618 * @c coord_sc_program().
     619 *
     620 * @return EOK on success or EINVAL if block index is past end of device.
     621 */
     622static int coord_calc(disk_t *d, uint64_t blk_idx, block_coord_t *bc)
     623{
     624        uint64_t c;
     625        uint64_t idx;
    484626
    485627        /* Check device bounds. */
    486628        if (blk_idx >= d->blocks)
    487629                return EINVAL;
     630
     631        bc->amode = d->amode;
    488632
    489633        switch (d->amode) {
    … …  
    493637                idx = blk_idx % (d->geom.heads * d->geom.sectors);
    494638
    495                 h = idx / d->geom.sectors;
    496                 s = 1 + (idx % d->geom.sectors);
     639                bc->cyl_lo = c & 0xff;
     640                bc->cyl_hi = (c >> 8) & 0xff;
     641                bc->h      = (idx / d->geom.sectors) & 0x0f;
     642                bc->sector = (1 + (idx % d->geom.sectors)) & 0xff;
    497643                break;
    498644
    499645        case am_lba28:
    500646                /* Compute LBA-28 coordinates. */
    501                 s = blk_idx & 0xff;             /* bits 0-7 */
    502                 c = (blk_idx >> 8) & 0xffff;    /* bits 8-23 */
    503                 h = (blk_idx >> 24) & 0x0f;     /* bits 24-27 */
     647                bc->c0 = blk_idx & 0xff;                /* bits 0-7 */
     648                bc->c1 = (blk_idx >> 8) & 0xff;         /* bits 8-15 */
     649                bc->c2 = (blk_idx >> 16) & 0xff;        /* bits 16-23 */
     650                bc->h  = (blk_idx >> 24) & 0x0f;        /* bits 24-27 */
    504651                break;
    505652
    506653        case am_lba48:
    507654                /* Compute LBA-48 coordinates. */
    508                 s = blk_idx & 0xff;             /* bits 0-7 */
    509                 c = (blk_idx >> 8) & 0xffff;    /* bits 8-23 */
    510                 s1 = (blk_idx >> 24) & 0xff;    /* bits 24-31 */
    511                 c1 = (blk_idx >> 32) & 0xffff;  /* bits 32-47 */
    512                 h = 0;
     655                bc->c0 = blk_idx & 0xff;                /* bits 0-7 */
     656                bc->c1 = (blk_idx >> 8) & 0xff;         /* bits 8-15 */
     657                bc->c2 = (blk_idx >> 16) & 0xff;        /* bits 16-23 */
     658                bc->c3 = (blk_idx >> 24) & 0xff;        /* bits 24-31 */
     659                bc->c4 = (blk_idx >> 32) & 0xff;        /* bits 32-39 */
     660                bc->c5 = (blk_idx >> 40) & 0xff;        /* bits 40-47 */
     661                bc->h  = 0;
    513662                break;
    514663        }
    515664
    516         /* New value for Drive/Head register */
    517         drv_head =
    518             ((disk_id != 0) ? DHR_DRV : 0) |
    519             ((d->amode != am_chs) ? DHR_LBA : 0) |
    520             (h & 0x0f);
    521 
    522         fibril_mutex_lock(&d->lock);
    523 
    524         /* Program a Read Sectors operation. */
    525 
    526         if (wait_status(0, ~SR_BSY, NULL, TIMEOUT_BSY) != EOK) {
    527                 fibril_mutex_unlock(&d->lock);
    528                 return EIO;
    529         }
    530 
    531         pio_write_8(&cmd->drive_head, drv_head);
    532 
    533         if (wait_status(SR_DRDY, ~SR_BSY, NULL, TIMEOUT_DRDY) != EOK) {
    534                 fibril_mutex_unlock(&d->lock);
    535                 return EIO;
    536         }
    537 
    538         if (d->amode == am_lba48) {
     665        return EOK;
     666}
     667
     668/** Program block coordinates and sector count into ATA registers.
     669 *
     670 * Note that bc->h must be programmed separately into the device/head register.
     671 */
     672static void coord_sc_program(const block_coord_t *bc, uint16_t scnt)
     673{
     674        if (bc->amode == am_lba48) {
    539675                /* Write high-order bits. */
    540                 pio_write_8(&cmd->sector_count, 0);
    541                 pio_write_8(&cmd->sector_number, s1);
    542                 pio_write_8(&cmd->cylinder_low, c1 & 0xff);
    543                 pio_write_8(&cmd->cylinder_high, c1 >> 16);
     676                pio_write_8(&cmd->sector_count, scnt >> 8);
     677                pio_write_8(&cmd->sector_number, bc->c3);
     678                pio_write_8(&cmd->cylinder_low, bc->c4);
     679                pio_write_8(&cmd->cylinder_high, bc->c5);
    544680        }
    545681
    546682        /* Write low-order bits. */
    547         pio_write_8(&cmd->sector_count, 1);
    548         pio_write_8(&cmd->sector_number, s);
    549         pio_write_8(&cmd->cylinder_low, c & 0xff);
    550         pio_write_8(&cmd->cylinder_high, c >> 16);
    551 
    552         pio_write_8(&cmd->command, d->amode == am_lba48 ?
    553             CMD_READ_SECTORS_EXT : CMD_READ_SECTORS);
    554 
    555         if (wait_status(0, ~SR_BSY, &status, TIMEOUT_BSY) != EOK) {
    556                 fibril_mutex_unlock(&d->lock);
    557                 return EIO;
    558         }
    559 
    560         if ((status & SR_DRQ) != 0) {
    561                 /* Read data from the device buffer. */
    562 
    563                 for (i = 0; i < block_size / 2; i++) {
    564                         data = pio_read_16(&cmd->data_port);
    565                         ((uint16_t *) buf)[i] = data;
    566                 }
    567         }
    568 
    569         if ((status & SR_ERR) != 0)
    570                 return EIO;
    571 
    572         fibril_mutex_unlock(&d->lock);
    573         return EOK;
    574 }
    575 
    576 /** Write a physical block to the device.
    577  *
    578  * @param disk_id       Device index (0 or 1)
    579  * @param blk_idx       Index of the first block.
    580  * @param blk_cnt       Number of blocks to transfer.
    581  * @param buf           Buffer holding the data to write.
    582  *
    583  * @return EOK on success, EIO on error.
    584  */
    585 static int ata_bd_write_block(int disk_id, uint64_t blk_idx, size_t blk_cnt,
    586     const void *buf)
    587 {
    588         size_t i;
    589         uint8_t status;
    590         uint64_t c, h, s, c1, s1;
    591         uint64_t idx;
    592         uint8_t drv_head;
    593         disk_t *d;
    594 
    595         d = &disk[disk_id];
    596 
    597         /* Check device bounds. */
    598         if (blk_idx >= d->blocks)
    599                 return EINVAL;
    600 
    601         switch (d->amode) {
    602         case am_chs:
    603                 /* Compute CHS coordinates. */
    604                 c = blk_idx / (d->geom.heads * d->geom.sectors);
    605                 idx = blk_idx % (d->geom.heads * d->geom.sectors);
    606 
    607                 h = idx / d->geom.sectors;
    608                 s = 1 + (idx % d->geom.sectors);
    609                 break;
    610 
    611         case am_lba28:
    612                 /* Compute LBA-28 coordinates. */
    613                 s = blk_idx & 0xff;             /* bits 0-7 */
    614                 c = (blk_idx >> 8) & 0xffff;    /* bits 8-23 */
    615                 h = (blk_idx >> 24) & 0x0f;     /* bits 24-27 */
    616                 break;
    617 
    618         case am_lba48:
    619                 /* Compute LBA-48 coordinates. */
    620                 s = blk_idx & 0xff;             /* bits 0-7 */
    621                 c = (blk_idx >> 8) & 0xffff;    /* bits 8-23 */
    622                 s1 = (blk_idx >> 24) & 0xff;    /* bits 24-31 */
    623                 c1 = (blk_idx >> 32) & 0xffff;  /* bits 32-47 */
    624                 h = 0;
    625                 break;
    626         }
    627 
    628         /* New value for Drive/Head register */
    629         drv_head =
    630             ((disk_id != 0) ? DHR_DRV : 0) |
    631             ((d->amode != am_chs) ? DHR_LBA : 0) |
    632             (h & 0x0f);
    633 
    634         fibril_mutex_lock(&d->lock);
    635 
    636         /* Program a Write Sectors operation. */
    637 
    638         if (wait_status(0, ~SR_BSY, NULL, TIMEOUT_BSY) != EOK) {
    639                 fibril_mutex_unlock(&d->lock);
    640                 return EIO;
    641         }
    642 
    643         pio_write_8(&cmd->drive_head, drv_head);
    644 
    645         if (wait_status(SR_DRDY, ~SR_BSY, NULL, TIMEOUT_DRDY) != EOK) {
    646                 fibril_mutex_unlock(&d->lock);
    647                 return EIO;
    648         }
    649 
    650         if (d->amode == am_lba48) {
    651                 /* Write high-order bits. */
    652                 pio_write_8(&cmd->sector_count, 0);
    653                 pio_write_8(&cmd->sector_number, s1);
    654                 pio_write_8(&cmd->cylinder_low, c1 & 0xff);
    655                 pio_write_8(&cmd->cylinder_high, c1 >> 16);
    656         }
    657 
    658         /* Write low-order bits. */
    659         pio_write_8(&cmd->sector_count, 1);
    660         pio_write_8(&cmd->sector_number, s);
    661         pio_write_8(&cmd->cylinder_low, c & 0xff);
    662         pio_write_8(&cmd->cylinder_high, c >> 16);
    663 
    664         pio_write_8(&cmd->command, d->amode == am_lba48 ?
    665             CMD_WRITE_SECTORS_EXT : CMD_WRITE_SECTORS);
    666 
    667         if (wait_status(0, ~SR_BSY, &status, TIMEOUT_BSY) != EOK) {
    668                 fibril_mutex_unlock(&d->lock);
    669                 return EIO;
    670         }
    671 
    672         if ((status & SR_DRQ) != 0) {
    673                 /* Write data to the device buffer. */
    674 
    675                 for (i = 0; i < block_size / 2; i++) {
    676                         pio_write_16(&cmd->data_port, ((uint16_t *) buf)[i]);
    677                 }
    678         }
    679 
    680         fibril_mutex_unlock(&d->lock);
    681 
    682         if (status & SR_ERR)
    683                 return EIO;
    684 
    685         return EOK;
     683        pio_write_8(&cmd->sector_count, scnt & 0x00ff);
     684        pio_write_8(&cmd->sector_number, bc->c0);
     685        pio_write_8(&cmd->cylinder_low, bc->c1);
     686        pio_write_8(&cmd->cylinder_high, bc->c2);
    686687}
    687688

  • uspace/srv/bd/ata_bd/ata_bd.h

    r1c1657c r5048be7  
    242242};
    243243
     244/** Block coordinates */
     245typedef struct {
     246        /** Addressing mode used */
     247        enum addr_mode amode;
     248
     249        union {
     250                /** CHS coordinates */
     251                struct {
     252                        uint8_t sector;
     253                        uint8_t cyl_lo;
     254                        uint8_t cyl_hi;
     255                };
     256                /** LBA coordinates */
     257                struct {
     258                        uint8_t c0;
     259                        uint8_t c1;
     260                        uint8_t c2;
     261                        uint8_t c3;
     262                        uint8_t c4;
     263                        uint8_t c5;
     264                };
     265        };
     266
     267        /** Lower 4 bits for device/head register */
     268        uint8_t h;
     269} block_coord_t;
     270
    244271typedef struct {
    245272        bool present;
Note: See TracChangeset for help on using the changeset viewer.