diff options
Diffstat (limited to 'Kernel/BlkDev/DskFDC.HC')
| -rw-r--r-- | Kernel/BlkDev/DskFDC.HC | 30 |
1 files changed, 20 insertions, 10 deletions
diff --git a/Kernel/BlkDev/DskFDC.HC b/Kernel/BlkDev/DskFDC.HC index 8a68b21..7517469 100644 --- a/Kernel/BlkDev/DskFDC.HC +++ b/Kernel/BlkDev/DskFDC.HC @@ -144,29 +144,39 @@ U0 FDCMotorTask(CBlkDev *bd) { // Motor timeout task F64 timeout = tS()+3.0; // 3 sec timeout - U8 dor; - while(tS() != timeout) { + U64 dor; + while(tS()<timeout) { if (bd->mtr==FDC_MOTOR_ON) return; Yield; } dor=InU8(bd->base0+FDC_DOR); - OutU8(bd->base0+FDC_DOR,dor^(1<<4+bd->unit)); + dor&=~(1<<(4+bd->unit)); + OutU8(bd->base0+FDC_DOR,dor); + bd->mtr=FDC_MOTOR_OFF; } U0 FDCMotor(CBlkDev *bd, Bool onoff) { // Motor control F64 timeout; - U8 dor=InU8(bd->base0+FDC_DOR); + U64 dor; if (onoff) { - OutU8(bd->base0+FDC_DOR,dor^(1<<4+bd->unit)|(1<<4+bd->unit)); - timeout=tS()+0.5; - while (tS()<timeout) Yield; - bd->mtr=FDC_MOTOR_ON; + if (bd->mtr!=FDC_MOTOR_ON) { + if (!bd->mtr) { + dor=InU8(bd->base0+FDC_DOR); + dor|=1<<(4+bd->unit); + OutU8(bd->base0+FDC_DOR,dor); + timeout=tS()+0.5; + while (tS()<timeout) Yield; + } + bd->mtr=FDC_MOTOR_ON; + } } else { - bd->mtr == FDC_MOTOR_WAIT; - Spawn(&FDCMotorTask,bd,"FDC Motor"); + if (bd->mtr) { + bd->mtr == FDC_MOTOR_WAIT; + Spawn(&FDCMotorTask,bd,"FDC Motor"); + } } } |