| #include <core.h> |
| #include <com32.h> |
| #include <fs.h> |
| #include <ilog2.h> |
| |
| #define RETRY_COUNT 6 |
| |
| static inline sector_t chs_max(const struct disk *disk) |
| { |
| return (sector_t)disk->secpercyl << 10; |
| } |
| |
| struct edd_rdwr_packet { |
| uint16_t size; |
| uint16_t blocks; |
| far_ptr_t buf; |
| uint64_t lba; |
| }; |
| |
| struct edd_disk_params { |
| uint16_t len; |
| uint16_t flags; |
| uint32_t phys_c; |
| uint32_t phys_h; |
| uint32_t phys_s; |
| uint64_t sectors; |
| uint16_t sector_size; |
| far_ptr_t dpte; |
| uint16_t devpath_key; |
| uint8_t devpath_len; |
| uint8_t _pad1[3]; |
| char bus_type[4]; |
| char if_type[8]; |
| uint8_t if_path[8]; |
| uint8_t dev_path[16]; |
| uint8_t _pad2; |
| uint8_t devpath_csum; /* Depends on devpath_len! */ |
| } __attribute__((packed)); |
| |
| static inline bool is_power_of_2(uint32_t x) |
| { |
| return !(x & (x-1)); |
| } |
| |
| static int chs_rdwr_sectors(struct disk *disk, void *buf, |
| sector_t lba, size_t count, bool is_write) |
| { |
| char *ptr = buf; |
| char *tptr; |
| size_t chunk, freeseg; |
| int sector_shift = disk->sector_shift; |
| uint32_t xlba = lba + disk->part_start; /* Truncated LBA (CHS is << 2 TB) */ |
| uint32_t t; |
| uint32_t c, h, s; |
| com32sys_t ireg, oreg; |
| size_t done = 0; |
| size_t bytes; |
| int retry; |
| uint32_t maxtransfer = disk->maxtransfer; |
| |
| if (lba + disk->part_start >= chs_max(disk)) |
| return 0; /* Impossible CHS request */ |
| |
| memset(&ireg, 0, sizeof ireg); |
| |
| ireg.eax.b[1] = 0x02 + is_write; |
| ireg.edx.b[0] = disk->disk_number; |
| |
| while (count) { |
| chunk = count; |
| if (chunk > maxtransfer) |
| chunk = maxtransfer; |
| |
| freeseg = (0x10000 - ((size_t)ptr & 0xffff)) >> sector_shift; |
| |
| if ((size_t)buf <= 0xf0000 && freeseg) { |
| /* Can do a direct load */ |
| tptr = ptr; |
| } else { |
| /* Either accessing high memory or we're crossing a 64K line */ |
| tptr = core_xfer_buf; |
| freeseg = (0x10000 - ((size_t)tptr & 0xffff)) >> sector_shift; |
| } |
| if (chunk > freeseg) |
| chunk = freeseg; |
| |
| s = xlba % disk->s; |
| t = xlba / disk->s; |
| h = t % disk->h; |
| c = t / disk->h; |
| |
| if (chunk > (disk->s - s)) |
| chunk = disk->s - s; |
| |
| bytes = chunk << sector_shift; |
| |
| if (tptr != ptr && is_write) |
| memcpy(tptr, ptr, bytes); |
| |
| ireg.eax.b[0] = chunk; |
| ireg.ecx.b[1] = c; |
| ireg.ecx.b[0] = ((c & 0x300) >> 2) | (s+1); |
| ireg.edx.b[1] = h; |
| ireg.ebx.w[0] = OFFS(tptr); |
| ireg.es = SEG(tptr); |
| |
| retry = RETRY_COUNT; |
| |
| for (;;) { |
| if (c < 1024) { |
| dprintf("CHS[%02x]: %u @ %llu (%u/%u/%u) %04x:%04x %s %p\n", |
| ireg.edx.b[0], chunk, xlba, c, h, s+1, |
| ireg.es, ireg.ebx.w[0], |
| (ireg.eax.b[1] & 1) ? "<-" : "->", |
| ptr); |
| |
| __intcall(0x13, &ireg, &oreg); |
| if (!(oreg.eflags.l & EFLAGS_CF)) |
| break; |
| |
| dprintf("CHS: error AX = %04x\n", oreg.eax.w[0]); |
| |
| if (retry--) |
| continue; |
| |
| /* |
| * For any starting value, this will always end with |
| * ..., 1, 0 |
| */ |
| chunk >>= 1; |
| if (chunk) { |
| maxtransfer = chunk; |
| retry = RETRY_COUNT; |
| ireg.eax.b[0] = chunk; |
| continue; |
| } |
| } |
| |
| printf("CHS: Error %04x %s sector %llu (%u/%u/%u)\n", |
| oreg.eax.w[0], |
| is_write ? "writing" : "reading", |
| lba, c, h, s+1); |
| return done; /* Failure */ |
| } |
| |
| bytes = chunk << sector_shift; |
| |
| if (tptr != ptr && !is_write) |
| memcpy(ptr, tptr, bytes); |
| |
| /* If we dropped maxtransfer, it eventually worked, so remember it */ |
| disk->maxtransfer = maxtransfer; |
| |
| ptr += bytes; |
| xlba += chunk; |
| count -= chunk; |
| done += chunk; |
| } |
| |
| return done; |
| } |
| |
| static int edd_rdwr_sectors(struct disk *disk, void *buf, |
| sector_t lba, size_t count, bool is_write) |
| { |
| static __lowmem struct edd_rdwr_packet pkt; |
| char *ptr = buf; |
| char *tptr; |
| size_t chunk, freeseg; |
| int sector_shift = disk->sector_shift; |
| com32sys_t ireg, oreg, reset; |
| size_t done = 0; |
| size_t bytes; |
| int retry; |
| uint32_t maxtransfer = disk->maxtransfer; |
| |
| memset(&ireg, 0, sizeof ireg); |
| |
| ireg.eax.b[1] = 0x42 + is_write; |
| ireg.edx.b[0] = disk->disk_number; |
| ireg.ds = SEG(&pkt); |
| ireg.esi.w[0] = OFFS(&pkt); |
| |
| memset(&reset, 0, sizeof reset); |
| |
| lba += disk->part_start; |
| while (count) { |
| chunk = count; |
| if (chunk > maxtransfer) |
| chunk = maxtransfer; |
| |
| freeseg = (0x10000 - ((size_t)ptr & 0xffff)) >> sector_shift; |
| |
| if ((size_t)ptr <= 0xf0000 && freeseg) { |
| /* Can do a direct load */ |
| tptr = ptr; |
| } else { |
| /* Either accessing high memory or we're crossing a 64K line */ |
| tptr = core_xfer_buf; |
| freeseg = (0x10000 - ((size_t)tptr & 0xffff)) >> sector_shift; |
| } |
| if (chunk > freeseg) |
| chunk = freeseg; |
| |
| bytes = chunk << sector_shift; |
| |
| if (tptr != ptr && is_write) |
| memcpy(tptr, ptr, bytes); |
| |
| retry = RETRY_COUNT; |
| |
| for (;;) { |
| pkt.size = sizeof pkt; |
| pkt.blocks = chunk; |
| pkt.buf = FAR_PTR(tptr); |
| pkt.lba = lba; |
| |
| dprintf("EDD[%02x]: %u @ %llu %04x:%04x %s %p\n", |
| ireg.edx.b[0], pkt.blocks, pkt.lba, |
| pkt.buf.seg, pkt.buf.offs, |
| (ireg.eax.b[1] & 1) ? "<-" : "->", |
| ptr); |
| |
| __intcall(0x13, &ireg, &oreg); |
| if (!(oreg.eflags.l & EFLAGS_CF)) |
| break; |
| |
| dprintf("EDD: error AX = %04x\n", oreg.eax.w[0]); |
| |
| if (retry--) |
| continue; |
| |
| /* |
| * Some systems seem to get "stuck" in an error state when |
| * using EBIOS. Doesn't happen when using CBIOS, which is |
| * good, since some other systems get timeout failures |
| * waiting for the floppy disk to spin up. |
| */ |
| __intcall(0x13, &reset, NULL); |
| |
| /* For any starting value, this will always end with ..., 1, 0 */ |
| chunk >>= 1; |
| if (chunk) { |
| maxtransfer = chunk; |
| retry = RETRY_COUNT; |
| continue; |
| } |
| |
| /* |
| * Total failure. There are systems which identify as |
| * EDD-capable but aren't; the known such systems return |
| * error code AH=1 (invalid function), but let's not |
| * assume that for now. |
| * |
| * Try to fall back to CHS. If the LBA is absurd, the |
| * chs_max() test in chs_rdwr_sectors() will catch it. |
| */ |
| done = chs_rdwr_sectors(disk, buf, lba - disk->part_start, |
| count, is_write); |
| if (done == (count << sector_shift)) { |
| /* Successful, assume this is a CHS disk */ |
| disk->rdwr_sectors = chs_rdwr_sectors; |
| return done; |
| } |
| printf("EDD: Error %04x %s sector %llu\n", |
| oreg.eax.w[0], |
| is_write ? "writing" : "reading", |
| lba); |
| return done; /* Failure */ |
| } |
| |
| bytes = chunk << sector_shift; |
| |
| if (tptr != ptr && !is_write) |
| memcpy(ptr, tptr, bytes); |
| |
| /* If we dropped maxtransfer, it eventually worked, so remember it */ |
| disk->maxtransfer = maxtransfer; |
| |
| ptr += bytes; |
| lba += chunk; |
| count -= chunk; |
| done += chunk; |
| } |
| return done; |
| } |
| |
| struct disk *bios_disk_init(void *private) |
| { |
| static struct disk disk; |
| struct bios_disk_private *priv = (struct bios_disk_private *)private; |
| com32sys_t *regs = priv->regs; |
| static __lowmem struct edd_disk_params edd_params; |
| com32sys_t ireg, oreg; |
| uint8_t devno = regs->edx.b[0]; |
| bool cdrom = regs->edx.b[1]; |
| sector_t part_start = regs->ecx.l | ((sector_t)regs->ebx.l << 32); |
| uint16_t bsHeads = regs->esi.w[0]; |
| uint16_t bsSecPerTrack = regs->edi.w[0]; |
| uint32_t MaxTransfer = regs->ebp.l; |
| bool ebios; |
| int sector_size; |
| unsigned int hard_max_transfer; |
| |
| memset(&ireg, 0, sizeof ireg); |
| ireg.edx.b[0] = devno; |
| |
| if (cdrom) { |
| /* |
| * The query functions don't work right on some CD-ROM stacks. |
| * Known affected systems: ThinkPad T22, T23. |
| */ |
| sector_size = 2048; |
| ebios = true; |
| hard_max_transfer = 32; |
| } else { |
| sector_size = 512; |
| ebios = false; |
| hard_max_transfer = 63; |
| |
| /* CBIOS parameters */ |
| disk.h = bsHeads; |
| disk.s = bsSecPerTrack; |
| |
| if ((int8_t)devno < 0) { |
| /* Get hard disk geometry from BIOS */ |
| |
| ireg.eax.b[1] = 0x08; |
| __intcall(0x13, &ireg, &oreg); |
| |
| if (!(oreg.eflags.l & EFLAGS_CF)) { |
| disk.h = oreg.edx.b[1] + 1; |
| disk.s = oreg.ecx.b[0] & 63; |
| } |
| } |
| |
| memset(&ireg, 0, sizeof ireg); |
| /* Get EBIOS support */ |
| ireg.eax.b[1] = 0x41; |
| ireg.ebx.w[0] = 0x55aa; |
| ireg.edx.b[0] = devno; |
| ireg.eflags.b[0] = 0x3; /* CF set */ |
| |
| __intcall(0x13, &ireg, &oreg); |
| |
| if (!(oreg.eflags.l & EFLAGS_CF) && |
| oreg.ebx.w[0] == 0xaa55 && (oreg.ecx.b[0] & 1)) { |
| ebios = true; |
| hard_max_transfer = 127; |
| |
| /* Query EBIOS parameters */ |
| /* The memset() is needed once this function can be called |
| more than once */ |
| /* memset(&edd_params, 0, sizeof edd_params); */ |
| edd_params.len = sizeof edd_params; |
| |
| memset(&ireg, 0, sizeof ireg); |
| ireg.eax.b[1] = 0x48; |
| ireg.edx.b[0] = devno; |
| ireg.ds = SEG(&edd_params); |
| ireg.esi.w[0] = OFFS(&edd_params); |
| __intcall(0x13, &ireg, &oreg); |
| |
| if (!(oreg.eflags.l & EFLAGS_CF) && oreg.eax.b[1] == 0) { |
| if (edd_params.len < sizeof edd_params) |
| memset((char *)&edd_params + edd_params.len, 0, |
| sizeof edd_params - edd_params.len); |
| |
| if (edd_params.sector_size >= 512 && |
| is_power_of_2(edd_params.sector_size)) |
| sector_size = edd_params.sector_size; |
| } |
| } |
| |
| } |
| |
| disk.disk_number = devno; |
| disk.sector_size = sector_size; |
| disk.sector_shift = ilog2(sector_size); |
| disk.part_start = part_start; |
| disk.secpercyl = disk.h * disk.s; |
| disk.rdwr_sectors = ebios ? edd_rdwr_sectors : chs_rdwr_sectors; |
| |
| if (!MaxTransfer || MaxTransfer > hard_max_transfer) |
| MaxTransfer = hard_max_transfer; |
| |
| disk.maxtransfer = MaxTransfer; |
| |
| dprintf("disk %02x cdrom %d type %d sector %u/%u offset %llu limit %u\n", |
| devno, cdrom, ebios, sector_size, disk.sector_shift, |
| part_start, disk.maxtransfer); |
| |
| disk.private = private; |
| return &disk; |
| } |
| |
| void pm_fs_init(com32sys_t *regs) |
| { |
| static struct bios_disk_private priv; |
| |
| priv.regs = regs; |
| fs_init((const struct fs_ops **)regs->eax.l, (void *)&priv); |
| } |