.286P
	INCLUDE x86SYS.INC
        INCLUDE DOS.INC
iodelay TEXTEQU <out 0edh,al>

DGROUP  GROUP _DATA,_TEXT

_BIOS   SEGMENT AT 0040h
        ORG 67h
RMTAR   dword ?
_BIOS   ENDS

_DATA   SEGMENT PARA PUBLIC 'DATA'
        ;       Save Area
Save_IDTR       fword ?
Save_GDTR       fword ?
Save_RM_SPTR    dd ?
Save_MSK_PIC1 db ?
Save_MSK_PIC2 db ?


        ; START OF INITIALIZED DATA
NA      db 8 dup(0)     ; for getting zeroes

        ; GDT
GDT     dw SIZE_GDT
        dd 0
        dw 0
        descriptor <0ffffh,0,0,10011010b,0> ; code segment descr.
        descriptor <0ffffh,0,0,10010010b,0> ; data segment descr.
        descriptor <0ffffh-1024,0,0,10010110b,0> ; stack segment descr.
SIZE_GDT EQU ($-GDT)

        ; IDT
IDT     dd 0    ; for now...
_DATA   ENDS

_TEXT   SEGMENT BYTE PUBLIC 'DATA'
_TEXT   ENDS

_STACK  SEGMENT PARA STACK 'STACK'
	db 1024 DUP (?)
_STACK  ENDS

_CODE   SEGMENT PARA PUBLIC 'CODE'
        ASSUME DS:_DATA
start:  mov ax,DGROUP
        mov ds,ax

        ; Save system pointers. Probably useless, but we want to exit clean
        sidt [Save_IDTR]
        sgdt [Save_GDTR]

        mov word ptr [Save_RM_SPTR],sp    ; Save Stack Pointer
        mov word ptr [Save_RM_SPTR+2],ss  ; This is the RM Stack

        push es                           ; Prepare RESET handler
        mov ax,SEG _BIOS
        mov es,ax
        mov bx,OFFSET RMTAR
        mov es:[bx],OFFSET RmRet
        mov es:[bx+2],cs
        pop es

        cli
        mov al,8Fh    ; disable NMI + write register 0Fh
        out 70h,al
        mov al,05h    ; shutdown code 5
        iodelay
        out 71h,al

        in al,21h               ; get IMR for PIC1
        mov Save_MSK_PIC1,al
        iodelay
        in al,0a1h              ; get IMR for PIC2
        mov Save_MSK_PIC2,al

        mov bx,cs               ; Prepare CS descriptor
        xor ax,ax
        call realtolin
        mov di,OFFSET GDT+8
        mov (descriptor ptr [di]).base_l,ax
        mov (descriptor ptr [di]).base_h,dl

        mov bx,ds               ; Prepare DS descriptor
        xor ax,ax
        call realtolin
        add di,8
        mov (descriptor ptr [di]).base_l,ax
        mov (descriptor ptr [di]).base_h,dl
        
        mov bx,ss               ; Prepare SS descriptor
        xor ax,ax
        call realtolin
        add ax,400h             ; BASE = LA + LIMIT
        adc dl,0
	dec dl			; according to guide: - modulus!
        add di,8
        mov (descriptor ptr [di]).base_l,ax
        mov (descriptor ptr [di]).base_h,dl

        mov di,OFFSET GDT
        mov ax,di
        mov bx,ds
        call realtolin
        mov word ptr [GDT+2],ax
        mov byte ptr [GDT+4],dl
        ; Size already filled in at assembly-time

        lgdt fword ptr [GDT]
        lidt fword ptr [NA]     ; load NULL-IDT (prepare triple fault)

        ;TEMP CHECK
        smsw ax
        or ax,1
        lmsw ax
        jmpf 8,@F
@@:
        mov ax,10h
        mov ds,ax
        mov es,ax
        mov ax,18h
        mov ss,ax
        add sp,0ffffh-400h      ; adjust SP
        
        ; CHECK IF IT  WORKS
        ;push 0FCE2h

        .386P                  ; Back to RM
        mov eax,cr0            ; Will lead to triple fault on 80286
        and al,0feh            ; 80386+ will fall through
        mov cr0,eax
        .286P

;        .686                   ; TESTING: RESET on 80386+
;        ud2
;        .286P

        ; Return point without RESET
        jmpf _CODE,@F
@@:     mov ax,DGROUP
        mov ds,ax
        mov es,ax

        lgdt [Save_GDTR]                 ; Restore system pointers
        lidt [Save_IDTR]
        mov ss,word ptr [Save_RM_SPTR+2] ; Restore stack pointer
        mov sp,word ptr [Save_RM_SPTR]

        mov al,0            ; re-enable NMI
        out 70h,al
        sti

        dosprint "Back to Real Mode without RESET!"
        mov ax,4c00h
	int 21h

RMRet:  mov ax,DGROUP
        mov ds,ax
        mov es,ax

        lgdt [Save_GDTR]                 ; Restore system pointers
        lidt [Save_IDTR]
        mov ss,word ptr [Save_RM_SPTR+2] ; Restore stack pointer
        mov sp,word ptr [Save_RM_SPTR]

        mov al,Save_MSK_PIC1             ; Restore IMR
        out 21h,al
        iodelay
        mov al,Save_MSK_PIC2
        out 0a1h,al

        mov al,0            ; re-enable NMI
        out 70h,al
        sti
        
        dosprint "Back to Real Mode after RESET!"
        mov ax,4c00h
	int 21h
_CODE   ENDS
	END start