ata: fix read/write on 2nd/4th disk

This commit is contained in:
Mathieu Maret 2023-11-21 00:06:09 +01:00 committed by Mathieu Maret
parent 8a4e0ff10f
commit 01f5b872f2

View File

@ -455,6 +455,10 @@ int ATAGetDeviceInfo(struct ata_device *dev)
outb(ctl->base + ATA_PIO_DRIVE,
ATA_PIO_DRIVE_IBM | (dev->isSlave ? ATA_PIO_DRIVE_SLAVE : ATA_PIO_DRIVE_MASTER));
outb(ctl->base + ATA_PIO_SEC_COUNT, 0);
outb(ctl->base + ATA_PIO_LBALO, 0);
outb(ctl->base + ATA_PIO_LBAMID, 0);
outb(ctl->base + ATA_PIO_LBAHI, 0);
outb(ctl->base + ATA_PIO_CMD, ATA_PIO_CMD_IDENTIFY);
/* Wait for command completion (wait while busy bit is set) */
@ -613,41 +617,42 @@ int ATAReadPartitionSector(struct ata_partition *part, int offset, uint nbSector
int ATAReadSector(struct ata_device *dev, int lba, uint nb_sector, void *buf)
{
struct ata_controller *ctl = dev->ctl;
struct ata_controller *ctl = dev->ctl;
mutexLock(&ctl->mutex);
mutexLock(&ctl->mutex);
if (ctl->last_device_used != dev->id) {
outb(ctl->base + ATA_PIO_DRIVE, ATA_PIO_DRIVE_IBM | dev->isSlave << 4);
while (inb(ctl->base + ATA_PIO_STATUS) & ATA_PIO_STATUS_DRIVE_BUSY) {
}
ctl->last_device_used = dev->id;
}
outb(ctl->base + ATA_PIO_DRIVE, lba >> 24 | ATA_PIO_DRIVE_IBM | ATA_PIO_DRIVE_LBA);
outb(ctl->base + ATA_PIO_SEC_COUNT, nb_sector);
outb(ctl->base + ATA_PIO_LBALO, lba & 0xff);
outb(ctl->base + ATA_PIO_LBAMID, (lba >> 8) & 0xff);
outb(ctl->base + ATA_PIO_LBAHI, (lba >> 16) & 0xff);
outb(ctl->base + ATA_PIO_CMD, ATA_PIO_CMD_READ);
if (ctl->last_device_used != dev->id) {
outb(ctl->base + ATA_PIO_DRIVE,
ATA_PIO_DRIVE_IBM | (dev->isSlave ? ATA_PIO_DRIVE_SLAVE : ATA_PIO_DRIVE_MASTER));
while (inb(ctl->base + ATA_PIO_STATUS) & ATA_PIO_STATUS_DRIVE_BUSY) {
}
ctl->last_device_used = dev->id;
}
outb(ctl->base + ATA_PIO_DRIVE, lba >> 24 | ATA_PIO_DRIVE_IBM | ATA_PIO_DRIVE_LBA | (dev->isSlave ? ATA_PIO_DRIVE_SLAVE : ATA_PIO_DRIVE_MASTER));
outb(ctl->base + ATA_PIO_SEC_COUNT, nb_sector);
outb(ctl->base + ATA_PIO_LBALO, lba & 0xff);
outb(ctl->base + ATA_PIO_LBAMID, (lba >> 8) & 0xff);
outb(ctl->base + ATA_PIO_LBAHI, (lba >> 16) & 0xff);
outb(ctl->base + ATA_PIO_CMD, ATA_PIO_CMD_READ);
uint16_t *ptr = (uint16_t *)buf;
for (uint sector = 0; sector < nb_sector; sector++) {
while (inb(ctl->base + ATA_PIO_STATUS) & ATA_PIO_STATUS_DRIVE_BUSY) {
}
if (inb(ctl->base + ATA_PIO_STATUS) & ATA_PIO_STATUS_REG_ERR) {
mutexUnlock(&ctl->mutex);
printf("ATA read error at sector %d\n", sector);
return -1;
}
for (uint i = 0; i < (DISK_SECTOR_SIZE / sizeof(uint16_t)); i++) {
*ptr = inw(ctl->base + ATA_PIO_DATA);
ptr++;
}
}
uint16_t *ptr = (uint16_t *)buf;
for (uint sector = 0; sector < nb_sector; sector++) {
while (inb(ctl->base + ATA_PIO_STATUS) & ATA_PIO_STATUS_DRIVE_BUSY) {
}
if (inb(ctl->base + ATA_PIO_STATUS) & ATA_PIO_STATUS_REG_ERR) {
mutexUnlock(&ctl->mutex);
printf("ATA read error at sector %d\n", sector);
return -1;
}
for (uint i = 0; i < (DISK_SECTOR_SIZE / sizeof(uint16_t)); i++) {
*ptr = inw(ctl->base + ATA_PIO_DATA);
ptr++;
}
}
mutexUnlock(&ctl->mutex);
mutexUnlock(&ctl->mutex);
return 0;
return 0;
}
int ATAWriteSector(struct ata_device *dev, int lba, uint nb_sector, void *buf)
@ -663,7 +668,7 @@ int ATAWriteSector(struct ata_device *dev, int lba, uint nb_sector, void *buf)
}
ctl->last_device_used = dev->id;
}
outb(ctl->base + ATA_PIO_DRIVE, lba >> 24 | ATA_PIO_DRIVE_IBM | ATA_PIO_DRIVE_LBA);
outb(ctl->base + ATA_PIO_DRIVE, lba >> 24 | ATA_PIO_DRIVE_IBM | ATA_PIO_DRIVE_LBA | (dev->isSlave ? ATA_PIO_DRIVE_SLAVE : ATA_PIO_DRIVE_MASTER));
outb(ctl->base + ATA_PIO_SEC_COUNT, nb_sector);
outb(ctl->base + ATA_PIO_LBALO, lba & 0xff);
outb(ctl->base + ATA_PIO_LBAMID, (lba >> 8) & 0xff);