static VOID XmsLocalDisableA20(VOID)
{
UCHAR Result = XMS_STATUS_SUCCESS;
/* Disable A20 only if we can do so, otherwise make the caller believe we disabled it */
if (!CanChangeA20) goto Quit;
/* If the count is already zero, fail */
if (A20EnableCount == 0) goto Fail;
--A20EnableCount;
/* The count is zero so disable A20 */
if (A20EnableCount == 0)
EmulatorSetA20(FALSE); // Result = XMS_STATUS_SUCCESS;
else
Result = XMS_STATUS_A20_STILL_ENABLED;
Quit:
setAX(0x0001); /* Line successfully disabled */
setBL(Result);
return;
Fail:
setAX(0x0000); /* Line failed to be disabled */
setBL(XMS_STATUS_A20_ERROR);
return;
}
static VOID XmsLocalEnableA20(VOID)
{
/* Enable A20 only if we can do so, otherwise make the caller believe we enabled it */
if (!CanChangeA20) goto Quit;
/* The count is zero so enable A20 */
if (A20EnableCount == 0) EmulatorSetA20(TRUE);
++A20EnableCount;
Quit:
setAX(0x0001); /* Line successfully enabled */
setBL(XMS_STATUS_SUCCESS);
return;
}
int main()
{
while (true) {
//thrust PID
error = setpoint - getHeight();
integral = integral + error * dt;
derivative = (error - previous_error);
thrust = Kp * error + Ki * integral + Kd * derivative;
//Pitch PID
error_P = 0 - getPitch();
integral_P = integral_P + error_P * dt;
derivative_P = (error_P - pre_error_P);
pitch = Kp * error_P + Ki * integral_P + Kd * derivative_P;
//Roll PID
error_R = 0 - getRoll();
integral_R = integral_R + error_R * dt;
derivative_R = (error_R - pre_error_R);
roll = Kp * error_R + Ki * integral_P + Kd * derivative_R;
//Motor controls
if (thrust > 255) thrust = 255;
else if (thrust < 0) thrust = 0;
setFR(makeValid(thrust + pitch + roll)); // front right
setFL(makeValid(thrust + pitch - roll)); // front left
setBR(makeValid(thrust - pitch + roll)); // back right
setBL(makeValid(thrust - pitch - roll)); // back left
//set previous errors
previous_error = error;
pre_error_P = error_P;
pre_error_R = error_R;
Sleep(1000 * dt);
}
}
VOID BiosCharPrint(CHAR Character)
{
/* Save AX and BX */
USHORT AX = getAX();
USHORT BX = getBX();
/*
* Set the parameters:
* AL contains the character to print,
* BL contains the character attribute,
* BH contains the video page to use.
*/
setAL(Character);
setBL(DEFAULT_ATTRIBUTE);
setBH(Bda->VideoPage);
/* Call the BIOS INT 10h, AH=0Eh "Teletype Output" */
setAH(0x0E);
Int32Call(&DosContext, BIOS_VIDEO_INTERRUPT);
/* Restore AX and BX */
setBX(BX);
setAX(AX);
}
static VOID WINAPI XmsBopProcedure(LPWORD Stack)
{
switch (getAH())
{
/* Get XMS Version */
case 0x00:
{
setAX(0x0300); /* XMS version 3.00 */
setBX(0x0301); /* Driver version 3.01 */
setDX(0x0001); /* HMA present */
break;
}
/* Request HMA */
case 0x01:
{
/* Check whether HMA is already reserved */
if (IsHmaReserved)
{
/* It is, bail out */
setAX(0x0000);
setBL(XMS_STATUS_HMA_IN_USE);
break;
}
// NOTE: We implicitely suppose that we always have HMA.
// If not, we should fail there with the XMS_STATUS_HMA_DOES_NOT_EXIST
// error code.
/* Check whether the requested size is above the minimal allowed one */
if (getDX() < HmaMinSize)
{
/* It is not, bail out */
setAX(0x0000);
setBL(XMS_STATUS_HMA_MIN_SIZE);
break;
}
/* Reserve it */
IsHmaReserved = TRUE;
setAX(0x0001);
setBL(XMS_STATUS_SUCCESS);
break;
}
/* Release HMA */
case 0x02:
{
/* Check whether HMA was reserved */
if (!IsHmaReserved)
{
/* It was not, bail out */
setAX(0x0000);
setBL(XMS_STATUS_HMA_NOT_ALLOCATED);
break;
}
/* Release it */
IsHmaReserved = FALSE;
setAX(0x0001);
setBL(XMS_STATUS_SUCCESS);
break;
}
/* Global Enable A20 */
case 0x03:
{
/* Enable A20 if needed */
if (!IsA20Enabled)
{
XmsLocalEnableA20();
if (getAX() != 0x0001)
{
/* XmsLocalEnableA20 failed and already set AX and BL to their correct values */
break;
}
IsA20Enabled = TRUE;
}
setAX(0x0001); /* Line successfully enabled */
setBL(XMS_STATUS_SUCCESS);
break;
}
/* Global Disable A20 */
case 0x04:
{
UCHAR Result = XMS_STATUS_SUCCESS;
/* Disable A20 if needed */
if (IsA20Enabled)
{
XmsLocalDisableA20();
if (getAX() != 0x0001)
{
/* XmsLocalDisableA20 failed and already set AX and BL to their correct values */
break;
}
IsA20Enabled = FALSE;
Result = getBL();
}
setAX(0x0001); /* Line successfully disabled */
setBL(Result);
break;
}
/* Local Enable A20 */
case 0x05:
{
/* This call sets AX and BL to their correct values */
XmsLocalEnableA20();
break;
}
/* Local Disable A20 */
case 0x06:
{
/* This call sets AX and BL to their correct values */
XmsLocalDisableA20();
break;
}
/* Query A20 State */
case 0x07:
{
setAX(EmulatorGetA20());
setBL(XMS_STATUS_SUCCESS);
break;
}
/* Query Free Extended Memory */
case 0x08:
{
setAX(XmsGetLargestFreeBlock());
setDX(FreeBlocks);
if (FreeBlocks > 0)
setBL(XMS_STATUS_SUCCESS);
else
setBL(XMS_STATUS_OUT_OF_MEMORY);
break;
}
/* Allocate Extended Memory Block */
case 0x09:
{
WORD Handle;
UCHAR Result = XmsAlloc(getDX(), &Handle);
if (Result == XMS_STATUS_SUCCESS)
{
setAX(1);
setDX(Handle);
}
else
{
setAX(0);
setBL(Result);
}
break;
}
/* Free Extended Memory Block */
case 0x0A:
{
UCHAR Result = XmsFree(getDX());
setAX(Result == XMS_STATUS_SUCCESS);
setBL(Result);
break;
}
/* Move Extended Memory Block */
case 0x0B:
{
PVOID SourceAddress, DestAddress;
PXMS_COPY_DATA CopyData = (PXMS_COPY_DATA)SEG_OFF_TO_PTR(getDS(), getSI());
PXMS_HANDLE HandleEntry;
if (CopyData->SourceHandle)
{
HandleEntry = GetHandleRecord(CopyData->SourceHandle);
if (!ValidateHandle(HandleEntry))
{
setAX(0);
setBL(XMS_STATUS_BAD_SRC_HANDLE);
break;
}
if (CopyData->SourceOffset >= HandleEntry->Size * XMS_BLOCK_SIZE)
{
setAX(0);
setBL(XMS_STATUS_BAD_SRC_OFFSET);
}
SourceAddress = (PVOID)REAL_TO_PHYS(HandleEntry->Address + CopyData->SourceOffset);
}
else
{
/* The offset is actually a 16-bit segment:offset pointer */
SourceAddress = FAR_POINTER(CopyData->SourceOffset);
}
if (CopyData->DestHandle)
{
HandleEntry = GetHandleRecord(CopyData->DestHandle);
if (!ValidateHandle(HandleEntry))
{
setAX(0);
setBL(XMS_STATUS_BAD_DEST_HANDLE);
break;
}
if (CopyData->DestOffset >= HandleEntry->Size * XMS_BLOCK_SIZE)
{
setAX(0);
setBL(XMS_STATUS_BAD_DEST_OFFSET);
}
DestAddress = (PVOID)REAL_TO_PHYS(HandleEntry->Address + CopyData->DestOffset);
}
else
{
/* The offset is actually a 16-bit segment:offset pointer */
DestAddress = FAR_POINTER(CopyData->DestOffset);
}
/* Perform the move */
RtlMoveMemory(DestAddress, SourceAddress, CopyData->Count);
setAX(1);
setBL(XMS_STATUS_SUCCESS);
break;
}
/* Lock Extended Memory Block */
case 0x0C:
{
DWORD Address;
UCHAR Result = XmsLock(getDX(), &Address);
if (Result == XMS_STATUS_SUCCESS)
{
setAX(1);
/* Store the LINEAR address in DX:BX */
setDX(HIWORD(Address));
setBX(LOWORD(Address));
}
else
{
setAX(0);
setBL(Result);
}
break;
}
/* Unlock Extended Memory Block */
case 0x0D:
{
UCHAR Result = XmsUnlock(getDX());
setAX(Result == XMS_STATUS_SUCCESS);
setBL(Result);
break;
}
/* Get Handle Information */
case 0x0E:
{
PXMS_HANDLE HandleEntry = GetHandleRecord(getDX());
UINT i;
UCHAR Handles = 0;
if (!ValidateHandle(HandleEntry))
{
setAX(0);
setBL(XMS_STATUS_INVALID_HANDLE);
break;
}
for (i = 0; i < XMS_MAX_HANDLES; i++)
{
if (HandleTable[i].Handle == 0) Handles++;
}
setAX(1);
setBH(HandleEntry->LockCount);
setBL(Handles);
setDX(HandleEntry->Size);
break;
}
/* Reallocate Extended Memory Block */
case 0x0F:
{
UCHAR Result = XmsRealloc(getDX(), getBX());
setAX(Result == XMS_STATUS_SUCCESS);
setBL(Result);
break;
}
/* Request UMB */
case 0x10:
{
BOOLEAN Result;
USHORT Segment = 0x0000; /* No preferred segment */
USHORT Size = getDX(); /* Size is in paragraphs */
Result = UmaDescReserve(&Segment, &Size);
if (Result)
setBX(Segment);
else
setBL(Size > 0 ? XMS_STATUS_SMALLER_UMB : XMS_STATUS_OUT_OF_UMBS);
setDX(Size);
setAX(Result);
break;
}
/* Release UMB */
case 0x11:
{
BOOLEAN Result;
USHORT Segment = getDX();
Result = UmaDescRelease(Segment);
if (!Result)
setBL(XMS_STATUS_INVALID_UMB);
setAX(Result);
break;
}
/* Reallocate UMB */
case 0x12:
{
BOOLEAN Result;
USHORT Segment = getDX();
USHORT Size = getBX(); /* Size is in paragraphs */
Result = UmaDescReallocate(Segment, &Size);
if (!Result)
{
if (Size > 0)
{
setBL(XMS_STATUS_SMALLER_UMB);
setDX(Size);
}
else
{
setBL(XMS_STATUS_INVALID_UMB);
}
}
setAX(Result);
break;
}
default:
{
DPRINT1("XMS command AH = 0x%02X NOT IMPLEMENTED\n", getAH());
setBL(XMS_STATUS_NOT_IMPLEMENTED);
}
}
}
void MechanumDrive(int bottomThresh, int topThresh, int lowGear, int cutOff)
{
//these variables allow you to change which channels conrtol different movements
if(controllerState == 0)
{
XVal = 0; //left/right
ZVal = vexRT[Ch3]; //forward/back
RotateVal = vexRT[Ch1]; //rotational
}
else if(controllerState == 1)
{
XVal = 0;
ZVal = vexRT[Ch3Xmtr2];
RotateVal = vexRT[Ch1Xmtr2];
}
/*
accelChange = abs(ZVal - oldZVal);
//if accel is in progress continue and aim for ZVal always
if(accel)
{
ZVal = accelRun(ZVal);
}
//if the ZVal has changed enough turn on accel
if(abs(ZVal - oldZVal) >= accelCutOff)
{
if(ZVal > oldZVal)
{
accelDirection = 1
}
else
{
accelDirection = -1
}
ZVal = accelRun(ZVal);
accel = true;
}
oldZVal = ZVal;
*/
//these manipulate the channel values going into the main function
//*see top and bottom thresh above
if(abs(ZVal)< bottomThresh)
{
ZVal = 0;
}
else if(abs(ZVal) < topThresh)
{
ZVal = ZVal/lowGear;
}
if(abs(XVal)< bottomThresh)
{
XVal = 0;
}
else if(abs(XVal) < topThresh)
{
XVal = XVal/lowGear;
}
if(abs(RotateVal)< bottomThresh)
{
RotateVal = 0;
}
else if(abs(RotateVal) < topThresh)
{
RotateVal = RotateVal/lowGear;
}
weightMechanum(XVal, ZVal, RotateVal);
//Set powers to motors
setFR(FRPower, cutOff);
setFL(FLPower, cutOff);
setBL(BLPower, cutOff);
setBR(BRPower, cutOff);
}
//go sideways for distance(in degrees) in a straight line at heading goal(front of robot)
//distance is always a positive integer
//goal should be 0 coming out of a turn so it finishes adjustments it couldn't do because of the motor cutoff
//alternately the goal could be the current gyro heading
//base power is positive or negative depending on desired direction-negative = left; positive = right
void sideways(int distance, int goal, int basePower, int cutOff)
{
//adjusts distance
distance = abs(distance);
distance = (int)(distance*(360/sideConversion));
int direction;
//sets direction for use later
if(basePower <= 0)
{
direction = -1;
}
else
{
direction = 1;
}
//error variables for use later
float error;
float oldError = 0;
float change = 0;
//resets variables for use later
proportion = 0;
integral = 0;
derivative = 0;
//set constants for PID
float Kp = 0.5;
float Ki = 0.054;
float Kd = 1.5;
//prepares goal for use with gyro
goal *= 10;
//stores gyro value
int gyroValue;
//will loop while the average of the sides is less than the direction
while((direction * NMotorEncoder[BL] + direction * (-1*NMotorEncoder[BR]))/2 < distance)
{
//preparing error...mostly magic
gyroValue = SensorValue[gyro];
gyroValue = centerGyro(goal, gyroValue);
error = adjustGyro(gyroValue);
change = PID(Kp, Ki, Kd, error, oldError);
change = change * -1;
weightMechanum(basePower, 0, change);
setFR(FRPower, cutOff);
setFL(FLPower, cutOff);
setBL(BLPower, cutOff);
setBR(BRPower, cutOff);
//sets the current error to be the old error for the next iteration
oldError = error;
//waits so that calculations are at even intervals
wait1Msec(50);
}
stopDrive();
}
int main()
{
//Declare variables for equation use
//Sensor Variables
int8_t height1;
int8_t pitch1;
int8_t roll1;
uint8_t height2;
int8_t pitch2;
int8_t roll2;
//Target Variables
int8_t pitchTar = 0;
int8_t rollTar = 0;
uint8_t heightTar = 127;
//Rate of change variables
double derPitch;
double derRoll;
double derHeight;
//Calculated Thrust Variables
double pitchThrust;
double rollThrust;
double heightThrust;
double adjustHeightThrust;
//Motor thrust before height adjustment variables
double FRTNoAdjustment;
double FLTNoAdjustment;
double BRTNoAdjustment;
double BLTNoAdjustment;
//Motor thrust after height adjustment variablefs
uint8_t FRT;
uint8_t FLT;
uint8_t BRT;
uint8_t BLT;
//Detect Pitch, Roll, and Height and store as variables
pitch1 = getPitch();
roll1 = getRoll();
height1 = getHeight();
//Calculate initial motor strength guess based on Pitch, Roll, and Height
FRT = (int)pitch1 + (int)roll1 + (int)heightTar - (int)height1;
FLT = (int)pitch1 - (int)roll1 + (int)heightTar - (int)height1;
BRT = -(int)pitch1 + (int)roll1 + (int)heightTar - (int)height1;
BLT = -(int)pitch1 - (int)roll1 + (int)heightTar - (int)height1;
//Set the motor strength
setFR(FRT);
setFL(FLT);
setBR(BRT);
setBL(BLT);
//Delay
Sleep(1000);
//Infinite loop
while (true)
{
//Detect position again
pitch2 = getPitch();
roll2 = getRoll();
height2 = getHeight();
//Calculate rate of change based on thrust and pitch
if ((int)FRT + (int)FLT - (int)BRT - (int)BLT != 0)
{
derPitch = ((int)pitch2 - (int)pitch1) / ((int)FRT + (int)FLT - (int)BRT - (int)BLT);
if (derPitch != 0)
{
//Calculate desired thrust based on rate of change in pitch
pitchThrust = ((int)pitchTar - (int)pitch2) / derPitch;
}
}
//Calculate rate of change based on thrust and roll
if ((int)FRT + (int)BRT - (int)FLT - (int)BLT != 0)
{
derRoll = ((int)roll2 - (int)roll1) / ((int)FRT + (int)BRT - (int)FLT - (int)BLT);
if (derRoll != 0)
{
//Calculate desired thrust based on rate of change in roll
rollThrust = ((int)rollTar - (int)roll2) / derRoll;
}
}
//Calculate rate of change based on thrust and height
if ((int)FRT + (int)FLT + (int)BRT + (int)BLT != 0)
{
derHeight = ((int)height2 - (int)height1) / ((int)FRT + (int)FLT + (int)BRT + (int)BLT);
if (derHeight != 0)
{
//Calculate desired thrust based on rate of change in height
heightThrust = ((int)heightTar - (int)height2) / derHeight;
}
}
//Calculate what thrust would be with only pitch and roll taken in to account
FRTNoAdjustment = pitchThrust + rollThrust;
FLTNoAdjustment = pitchThrust - rollThrust;
BRTNoAdjustment = -pitchThrust + rollThrust;
BLTNoAdjustment = -pitchThrust - rollThrust;
//Calculate change in thrust required to get to target height
adjustHeightThrust = (heightThrust - (FRTNoAdjustment + FLTNoAdjustment + BRTNoAdjustment + BLTNoAdjustment)) / 4;
//Calculate thrust based on pitch roll and height
FRT = pitchThrust + rollThrust + adjustHeightThrust;
FLT = pitchThrust - rollThrust + adjustHeightThrust;
BRT = -pitchThrust + rollThrust + adjustHeightThrust;
BLT = -pitchThrust - rollThrust + adjustHeightThrust;
//Set the motor strength
setFR(FRT);
setFL(FLT);
setBR(BRT);
setBL(BLT);
//Save old positionf
pitch1 = pitch2;
roll1 = roll2;
height1 = height2;
//Delay
Sleep(1000);
}
return 0;
}
void rs232_io()
{
#ifdef BIT_ORDER2
union {
half_word all;
struct {
HALF_WORD_BIT_FIELD word_length:2;
HALF_WORD_BIT_FIELD stop_bit:1;
HALF_WORD_BIT_FIELD parity:2;
HALF_WORD_BIT_FIELD baud_rate:3;
} bit;
} parameters;
#endif
#ifdef BIT_ORDER1
union {
half_word all;
struct {
HALF_WORD_BIT_FIELD baud_rate:3;
HALF_WORD_BIT_FIELD parity:2;
HALF_WORD_BIT_FIELD stop_bit:1;
HALF_WORD_BIT_FIELD word_length:2;
} bit;
} parameters;
#endif
DIVISOR_LATCH divisor_latch;
int j;
half_word timeout;
sys_addr timeout_location;
/* clear com/lpt idle flag */
IDLE_comlpt ();
setIF(1);
/*
* Which adapter?
*/
switch (getDX ())
{
case 0:
port = RS232_COM1_PORT_START;
timeout_location = RS232_COM1_TIMEOUT;
break;
case 1:
port = RS232_COM2_PORT_START;
timeout_location = RS232_COM2_TIMEOUT;
break;
case 2:
port = RS232_COM3_PORT_START;
timeout_location = RS232_COM3_TIMEOUT;
break;
case 3:
port = RS232_COM4_PORT_START;
timeout_location = RS232_COM4_TIMEOUT;
break;
default:
break;
}
/*
* Determine function
*/
switch (getAH ())
{
case 0:
/*
* Initialise the communication port
*/
value = 0x80; /* set DLAB */
outb(port + (io_addr) RS232_LCR, value);
/*
* Set baud rate
*/
parameters.all = getAL();
divisor_latch.all = divisors[parameters.bit.baud_rate];
outb(port + (io_addr) RS232_IER, divisor_latch.byte.MSByte);
outb(port + (io_addr) RS232_TX_RX, divisor_latch.byte.LSByte);
/*
* Set word length, stop bits and parity
*/
parameters.bit.baud_rate = 0;
outb(port + (io_addr) RS232_LCR, parameters.all);
/*
* Disable interrupts
*/
value = 0;
outb(port + (io_addr) RS232_IER, value);
return_status();
break;
case 1:
/*
* Send char over the comms line
*/
/*
* Set DTR and RTS
*/
outb(port + (io_addr) RS232_MCR, 3);
/*
* Real BIOS checks CTS and DSR - we know DSR ok.
* Real BIOS check THRE - we know it's ok.
* We only check CTS - this is supported on a few ports, eg. Macintosh.
*/
/*
* Wait for CTS to go high, or timeout
*/
sas_load(timeout_location, &timeout);
for ( j = 0; j < timeout; j++)
{
inb(port + (io_addr) RS232_MSR, &value);
if(value & 0x10)break; /* CTS High, all is well */
}
if(j < timeout)
{
outb(port + (io_addr) RS232_TX_RX, getAL()); /* Send byte */
inb(port + (io_addr) RS232_LSR, &value);
setAH(value); /* Return Line Status Reg in AH */
}
else
{
setAH(value | 0x80); /* Indicate time out */
}
break;
case 2:
/*
* Receive char over the comms line
*/
/*
* Set DTR
*/
value = 1;
outb(port + (io_addr) RS232_MCR, value);
/*
* Real BIOS checks DSR - we know it's ok.
*/
/*
* Wait for data to appear, or timeout(just an empirical guess)
*/
sas_load(timeout_location, &timeout);
for ( j = 0; j < timeout; j++)
{
inb(port + (io_addr) RS232_LSR, &value);
if ( (value & 1) == 1 )
{
/*
* Data ready go read it
*/
value &= 0x1e; /* keep error bits only */
setAH(value);
inb(port + (io_addr) RS232_TX_RX, &value);
setAL(value);
return;
}
}
/*
* Set timeout
*/
value |= 0x80;
setAH(value);
break;
case 3:
/*
* Return the communication port status
*/
return_status();
break;
case 4:
/*
* EXTENDED (PS/2) Initialise the communication port
*/
value = 0x80; /* set DLAB */
outb(port + (io_addr) RS232_LCR, value);
parameters.bit.word_length = getCH();
parameters.bit.stop_bit = getBL();
parameters.bit.parity = getBH();
parameters.bit.baud_rate = getCL();
/*
Set baud rate
*/
divisor_latch.all = divisors[parameters.bit.baud_rate];
outb(port + (io_addr) RS232_IER, divisor_latch.byte.MSByte);
outb(port + (io_addr) RS232_TX_RX, divisor_latch.byte.LSByte);
/*
* Set word length, stop bits and parity
*/
parameters.bit.baud_rate = 0;
outb(port + (io_addr) RS232_LCR, parameters.all);
/*
* Disable interrupts
*/
value = 0;
outb(port + (io_addr) RS232_IER, value);
return_status();
break;
case 5: /* EXTENDED Comms Port Control */
switch( getAL() )
{
case 0: /* Read modem control register */
inb( port + (io_addr) RS232_MCR, &value);
setBL(value);
break;
case 1: /* Write modem control register */
outb( port + (io_addr) RS232_MCR, getBL());
break;
}
/*
Return the communication port status
*/
return_status();
break;
default:
/*
** Yes both XT and AT BIOS's really do this.
*/
setAH( getAH()-3 );
break;
}
}
/*static*/
VOID WINAPI BiosDiskService(LPWORD Stack)
{
BYTE Drive;
PDISK_IMAGE DiskImage;
switch (getAH())
{
/* Disk -- Reset Disk System */
case 0x00:
{
Drive = getDL();
if (Drive & 0x80)
{
AllDisksReset();
/* Return success */
setAH(0x00);
Stack[STACK_FLAGS] &= ~EMULATOR_FLAG_CF;
break;
}
Drive &= ~0x80;
if (Drive >= ARRAYSIZE(FloppyDrive))
{
DPRINT1("BiosDiskService(0x00): Drive number 0x%02X invalid\n", Drive);
/* Return error */
setAH(0x01);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
break;
}
// TODO: Reset drive
/* Return success */
setAH(0x00);
Stack[STACK_FLAGS] &= ~EMULATOR_FLAG_CF;
break;
}
/* Disk -- Get Status of Last Operation */
case 0x01:
{
BYTE LastOperationStatus = 0x00;
Drive = getDL();
DiskImage = GetDisk(Drive);
if (!DiskImage || !IsDiskPresent(DiskImage))
{
DPRINT1("BiosDiskService(0x01): Disk number 0x%02X invalid\n", Drive);
/* Return error */
setAH(0x01);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
break;
}
LastOperationStatus = DiskImage->LastOperationStatus;
if (Drive & 0x80)
Bda->LastDiskOperation = LastOperationStatus;
else
Bda->LastDisketteOperation = LastOperationStatus;
/* Return last error */
setAH(LastOperationStatus);
if (LastOperationStatus == 0x00)
Stack[STACK_FLAGS] &= ~EMULATOR_FLAG_CF;
else
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
break;
}
/* Disk -- Read Sectors into Memory */
case 0x02:
{
BYTE Status;
BYTE Head = getDH();
BYTE NumSectors = getAL();
// CH: Low eight bits of cylinder number
// CL: High two bits of cylinder (bits 6-7, hard disk only)
WORD Cylinder = MAKEWORD(getCH(), (getCL() >> 6) & 0x02);
// CL: Sector number 1-63 (bits 0-5)
BYTE Sector = (getCL() & 0x3F); // 1-based
Drive = getDL();
DiskImage = GetDisk(Drive);
if (!DiskImage || !IsDiskPresent(DiskImage))
{
DPRINT1("BiosDiskService(0x02): Disk number 0x%02X invalid\n", Drive);
/* Return error */
setAH(0x01);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
break;
}
/* Read the sectors */
Status = ReadDisk(DiskImage, Cylinder, Head, Sector, NumSectors);
if (Status == 0x00)
{
/* Return success */
setAH(0x00);
Stack[STACK_FLAGS] &= ~EMULATOR_FLAG_CF;
}
else
{
DPRINT1("BiosDiskService(0x02): Error when reading from disk number 0x%02X (0x%02X)\n", Drive, Status);
/* Return error */
setAH(Status);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
}
break;
}
/* Disk -- Write Disk Sectors */
case 0x03:
{
BYTE Status;
BYTE Head = getDH();
BYTE NumSectors = getAL();
// CH: Low eight bits of cylinder number
// CL: High two bits of cylinder (bits 6-7, hard disk only)
WORD Cylinder = MAKEWORD(getCH(), (getCL() >> 6) & 0x02);
// CL: Sector number 1-63 (bits 0-5)
BYTE Sector = (getCL() & 0x3F); // 1-based
Drive = getDL();
DiskImage = GetDisk(Drive);
if (!DiskImage || !IsDiskPresent(DiskImage))
{
DPRINT1("BiosDiskService(0x03): Disk number 0x%02X invalid\n", Drive);
/* Return error */
setAH(0x01);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
break;
}
/* Write the sectors */
Status = WriteDisk(DiskImage, Cylinder, Head, Sector, NumSectors);
if (Status == 0x00)
{
/* Return success */
setAH(0x00);
Stack[STACK_FLAGS] &= ~EMULATOR_FLAG_CF;
}
else
{
DPRINT1("BiosDiskService(0x03): Error when writing to disk number 0x%02X (0x%02X)\n", Drive, Status);
/* Return error */
setAH(Status);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
}
break;
}
/* Disk -- Verify Disk Sectors */
case 0x04:
/* Floppy/Fixed Disk -- Format Track */
case 0x05:
/* Fixed Disk -- Format Track and Set Bad Sector Flags */
case 0x06:
/* Fixed Disk -- Format Drive starting at Given Track */
case 0x07:
goto Default;
/* Disk -- Get Drive Parameters */
case 0x08:
{
WORD MaxCylinders;
BYTE MaxHeads;
BYTE PresentDrives = 0;
BYTE i;
Drive = getDL();
DiskImage = GetDisk(Drive);
if (!DiskImage || !IsDiskPresent(DiskImage))
{
DPRINT1("BiosDiskService(0x08): Disk number 0x%02X invalid\n", Drive);
/* Return error */
setAH(0x01);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
break;
}
// Minus 2 because it's the maximum cylinder number (not count),
// and the last cylinder is reserved (for compatibility with BIOSes
// which reserve it for testing purposes).
MaxCylinders = DiskImage->DiskInfo.Cylinders - 2;
// Minus 1 because it's the maximum head number (not count).
MaxHeads = DiskImage->DiskInfo.Heads - 1;
// CL: Sector number 1-63 (bits 0-5)
// High two bits of cylinder (bits 6-7, hard disk only)
setCL((DiskImage->DiskInfo.Sectors & 0x3F) |
((HIBYTE(MaxCylinders) & 0x02) << 6));
// CH: Low eight bits of cylinder number
setCH(LOBYTE(MaxCylinders));
setDH(MaxHeads);
if (Drive & 0x80)
{
/* Count the number of active HDDs */
for (i = 0; i < ARRAYSIZE(HardDrive); ++i)
{
if (IsDiskPresent(HardDrive[i]))
++PresentDrives;
}
/* Reset ES:DI to NULL */
// FIXME: NONONO!! Apps expect (for example, MS-DOS kernel)
// that this function does not modify ES:DI if it was called
// for a HDD.
// setES(0x0000);
// setDI(0x0000);
}
else
{
/* Count the number of active floppies */
for (i = 0; i < ARRAYSIZE(FloppyDrive); ++i)
{
if (IsDiskPresent(FloppyDrive[i]))
++PresentDrives;
}
/* ES:DI points to the floppy parameter table */
setES(HIWORD(((PULONG)BaseAddress)[0x1E]));
setDI(LOWORD(((PULONG)BaseAddress)[0x1E]));
}
setDL(PresentDrives);
setBL(DiskImage->DiskType); // DiskGeometryList[DiskImage->DiskType].biosval
setAL(0x00);
/* Return success */
setAH(0x00);
Stack[STACK_FLAGS] &= ~EMULATOR_FLAG_CF;
break;
}
/* Hard Disk -- Initialize Controller with Drive Parameters */
case 0x09:
/* Hard Disk -- Read Long Sectors */
case 0x0A:
/* Hard Disk -- Write Long Sectors */
case 0x0B:
goto Default;
/* Hard Disk -- Seek to Cylinder */
case 0x0C:
{
BYTE Status;
BYTE Head = getDH();
// CH: Low eight bits of cylinder number
// CL: High two bits of cylinder (bits 6-7, hard disk only)
WORD Cylinder = MAKEWORD(getCH(), (getCL() >> 6) & 0x02);
// CL: Sector number 1-63 (bits 0-5)
BYTE Sector = (getCL() & 0x3F); // 1-based
Drive = getDL();
if (!(Drive & 0x80))
{
DPRINT1("BiosDiskService(0x0C): Disk number 0x%02X is not a HDD\n", Drive);
/* Return error */
setAH(0x01);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
break;
}
DiskImage = GetDisk(Drive);
if (!DiskImage || !IsDiskPresent(DiskImage))
{
DPRINT1("BiosDiskService(0x0C): Disk number 0x%02X invalid\n", Drive);
/* Return error */
setAH(0x01);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
break;
}
/* Set position */
Status = SeekDisk(DiskImage, Cylinder, Head, Sector);
if (Status == 0x00)
{
/* Return success */
setAH(0x00);
Stack[STACK_FLAGS] &= ~EMULATOR_FLAG_CF;
}
else
{
DPRINT1("BiosDiskService(0x0C): Error when seeking in disk number 0x%02X (0x%02X)\n", Drive, Status);
/* Return error */
setAH(Status);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
}
break;
}
/* Hard Disk -- Reset Hard Disks */
case 0x0D:
{
// FIXME: Should do what 0x11 does.
UNIMPLEMENTED;
}
/* Hard Disk -- Read Sector Buffer (XT only) */
case 0x0E:
/* Hard Disk -- Write Sector Buffer (XT only) */
case 0x0F:
goto Default;
/* Hard Disk -- Check if Drive is ready */
case 0x10:
{
Drive = getDL();
if (!(Drive & 0x80))
{
DPRINT1("BiosDiskService(0x10): Disk number 0x%02X is not a HDD\n", Drive);
/* Return error */
setAH(0x01);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
break;
}
DiskImage = GetDisk(Drive);
if (!DiskImage || !IsDiskPresent(DiskImage))
{
DPRINT1("BiosDiskService(0x10): Disk number 0x%02X invalid\n", Drive);
/* Return error */
setAH(0x01);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
break;
}
/* Return success */
setAH(0x00);
Stack[STACK_FLAGS] &= ~EMULATOR_FLAG_CF;
break;
}
/* Hard Disk -- Recalibrate Drive */
case 0x11:
{
BYTE Status;
Drive = getDL();
if (!(Drive & 0x80))
{
DPRINT1("BiosDiskService(0x11): Disk number 0x%02X is not a HDD\n", Drive);
/* Return error */
setAH(0x01);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
break;
}
DiskImage = GetDisk(Drive);
if (!DiskImage || !IsDiskPresent(DiskImage))
{
DPRINT1("BiosDiskService(0x11): Disk number 0x%02X invalid\n", Drive);
/* Return error */
setAH(0x01);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
break;
}
/* Set position to zero */
Status = SeekDisk(DiskImage, /*Cylinder*/ 0, /*Head*/ 0, /*Sector*/ 1);
if (Status == 0x00)
{
/* Return success */
setAH(0x00);
Stack[STACK_FLAGS] &= ~EMULATOR_FLAG_CF;
}
else
{
DPRINT1("BiosDiskService(0x11): Error when recalibrating disk number 0x%02X (0x%02X)\n", Drive, Status);
/* Return error */
setAH(Status);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
}
break;
}
/* Hard Disk -- Controller RAM Diagnostic */
case 0x12:
/* Hard Disk -- Drive Diagnostic */
case 0x13:
/* Hard Disk -- Controller Internal Diagnostic */
case 0x14:
goto Default;
/* Disk -- Get Disk Type */
case 0x15:
{
Drive = getDL();
DiskImage = GetDisk(Drive);
if (!DiskImage || !IsDiskPresent(DiskImage))
{
DPRINT1("BiosDiskService(0x15): Disk number 0x%02X invalid\n", Drive);
/* Return error */
setAH(0x01);
Stack[STACK_FLAGS] |= EMULATOR_FLAG_CF;
break;
}
if (Drive & 0x80)
{
ULONG NumSectors;
/* Hard disk */
setAH(0x03);
/* Number of 512-byte sectors in CX:DX */
NumSectors = (ULONG)((ULONG)DiskImage->DiskInfo.Cylinders * DiskImage->DiskInfo.Heads)
* DiskImage->DiskInfo.Sectors;
setCX(HIWORD(NumSectors));
setDX(LOWORD(NumSectors));
}
else
{
/* Floppy */
setAH(0x01);
}
/* Return success */
Stack[STACK_FLAGS] &= ~EMULATOR_FLAG_CF;
break;
}
/* Floppy Disk -- Detect Disk Change */
case 0x16:
/* Floppy Disk -- Set Disk Type for Format */
case 0x17:
/* Disk -- Set Media Type for Format */
case 0x18:
goto Default;
default: Default:
{
DPRINT1("BIOS Function INT 13h, AH = 0x%02X, AL = 0x%02X, BH = 0x%02X NOT IMPLEMENTED\n",
getAH(), getAL(), getBH());
}
}
}