#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);
}