/*static char *SCCSID = "@(#)hyberldr.c  6.1 95/03/01";*/
/*
 *      IBM Confidential
 *
 *      Copyright (c) IBM Corporation 1995
 *
 *      All Rights Reserved
 */

typedef unsigned long ulong_t;
typedef unsigned short ushort_t;
typedef unsigned char uchar_t;
#include "hybernat.h"
#include "hyberhd.h"

ulong_t doit(struct mydata *mydata);
ulong_t startio(struct mydata *mydata, unsigned long addr, long len);
void setMultiple(int m);
ulong_t hdReadDisk(struct mydata *mydata, uchar_t *address, ulong_t len);
void restart(hyberInfoHdr_t *pHyberInfoHdr);

#define min(x, y)	(x <= y ? x : y)
#define PAGEDIRSHIFT	22
#define COM_PORT	pHyberInfoHdr->hih_debugport

void
Main(hyberInfoHdr_t *pHyberInfoHdr) {
	ulong_t rslt;
	ulong_t *paddr;
	mydata_t Mydata;
	mydata_t *mydata;

	mydata = &Mydata;
	maxblk = pHyberInfoHdr->hih_maxblk;
	SpT = pHyberInfoHdr->hih_SpT;
	SpC = pHyberInfoHdr->hih_SpC;
	disklist = (struct dlist_s *)((ulong_t)pHyberInfoHdr->dlist + // offset
					(ulong_t)pHyberInfoHdr);
	pagelist = (struct plist_s *)((ulong_t)pHyberInfoHdr->plist + // offset
					(ulong_t)pHyberInfoHdr);
	dlSiz = pHyberInfoHdr->hih_dli;
	plSiz = pHyberInfoHdr->hih_pli;

	/* setup for the I/O */
	setMultiple(maxblk);
	maxio = MAXIO;
	rslt = doit(mydata);
	while(rslt && (maxio > 0)) {
		rslt |= maxio << 24;
		msgOut(rslt, COM_PORT, 0);
		maxio /= 2;
		rslt = doit(mydata);
	}
	if (rslt) {
		// once more
		rslt |= maxio << 24;
		msgOut(rslt, COM_PORT, 0);
		setMultiple(0);
		maxblk = 1;
		maxio = 1;
		rslt = doit(mydata);
		if (rslt) {
			rslt |= maxio << 24;
			msgOut(rslt, COM_PORT, 0);
			reboot();
		}
	}

	/* provide a page directory entry for the space we are now using */
	paddr = (ulong_t *)pHyberInfoHdr->hih_cr3;
	paddr[0] = paddr[HYBERPHYSMEM >> PAGEDIRSHIFT];
	restart(pHyberInfoHdr);			// doesn't return
}

ulong_t
doit(struct mydata *mydata)
{
	int dcount;		/* tracks sectors completed per dlist entry */
	int dgrpsize;		/* sectors per dlist entry */
	int iosize;		/* in sectors */
	int sector;		/* sector read address */
	int sectorsRead;
	int mgrpsize = 0;	/* size of current memory group */
	int mcount = 0;		/* bytes completed in this group */
	int i, j, k;
	ulong_t address;	/* destination address */
	unsigned long status;

	hdWaitNotBusy();
	for (i=j=0; i<=dlSiz; i++) {
		dgrpsize = disklist[i].dl_count;
		dcount = 0;
		sector = disklist[i].dl_sector;
		while (dgrpsize != dcount) {
			iosize = min(maxio, dgrpsize - dcount);
			sectorsRead = 0;
			status = startio(mydata, sector + dcount, iosize);
			if (status & HD_ERR) {
				status |= 0x100;
				status = status << 8 | inb(HD_C0 + HD_ER);
				goto out;
			}
			dcount += iosize;

			/* memory loop */
			while (iosize != sectorsRead) {
				if (mgrpsize == mcount) {
				    address = pagelist[j].pl_frame * PAGESIZE;
				    mgrpsize = pagelist[j].pl_count * PAGESIZE;
				    j++;
				    mcount = 0;
				}
				k = min(mgrpsize-mcount,
					(iosize-sectorsRead) * SECTORSIZE);
				status = hdReadDisk(mydata,
					((char *)address)+mcount, k);
				if (status & HD_ERR) {
					status |= 0x400;
					status = status << 8 | inb(HD_C0+HD_ER);
					goto out;
				}
				mcount += k;
				sectorsRead += k / SECTORSIZE;
			}
			status = hdWaitNotBusy();
			if (status & HD_ERR) {
				status |= 0x200;
				status = status << 8 | inb(HD_C0 + HD_ER);
				goto out;
			}
			if (status & HD_DRQ) {
				// some devices are slow posting the REAL status
				for (k=0; status & HD_DRQ && k < 500; k++)
					status = hdWaitNotBusy();
				if (status & HD_DRQ) {
					status |= 0x300;
					status = status << 8;
					goto out;
				}
			}
		}
	}
	status = 0;
out:
	return(status);
}

ulong_t
startio(struct mydata *mydata, unsigned long addr, long len)
{
	long cylnum;
	long cyl;

	cylnum = addr / SpC;
	cyl = addr % SpC;
	outb((cyl / SpT) | HD_DRV0, HD_C0 + HD_DHR);	/* head/drv */
	hdWaitReady();
	outb(len & 0xff, HD_C0 + HD_SCR);		/* sector count */
	outb(cylnum & 0xff, HD_C0 + HD_CLR);		/* cylinder low */
	outb((cylnum >> 8) & 0xff, HD_C0 + HD_CHR);	/* cylinder high */
	outb((cyl % SpT) + 1, HD_C0 + HD_SNR);		/* sector */
	if (maxblk == 1)
		outb(HD_READ, HD_C0 + HD_CR);
	else
		outb(HD_READM, HD_C0 + HD_CR);
	sectorCount = len < maxblk ? len : maxblk;	// thinkpad fix
	return(hdWaitNotBusy());
}

void
setMultiple(int m) {

	if (m == 1)
		return;
	outb(HD_DRV0, HD_C0 + HD_DHR);
	hdWaitReady();
	outb(m, HD_C0 + HD_SCR);
	outb(HD_SMM, HD_C0 + HD_CR);
	hdWaitNotBusy();
}

