diff --git a/kernel/intr.asm b/kernel/intr.asm
index 2d7eca8..e08b764 100644
--- a/kernel/intr.asm
+++ b/kernel/intr.asm
@@ -1,23 +1,34 @@
 ; Register a interrupt handler
-; in:	sp:bp+0  interrupt number
-;	     +2  handler offset
-; out:  sp:bp+4
+; in:	AL	interrupt number
+;	ES:DI	far ptr to routine/data
 intr_register:
-  ; use data stack
-  xchg sp, bp
-  ; ES := 0
+  ; backup original values
+  push ax
+  push es
+  push di
+
+  ; pop as ax later
+  push es
+  push di
+
+  ; DI = AL * 4
+  mov ah, 4
+  mul ah
+  mov di, ax
+
+  ; ES = 0
   xor ax, ax
   mov es, ax
-  ; DI := intnum * 4
-  pop di
-  sal di, 1
-  sal di, 1
-  ; copy offset from arguments
+
+  ; store offset
   pop ax
   stosw
-  ; copy our segment
-  mov ax, cs
+
+  ; store segment
+  pop ax
   stosw
-  ; use code stack
-  xchg sp, bp
-  ret
\ No newline at end of file
+
+  pop di
+  pop es
+  pop ax
+  ret
diff --git a/kernel/main.asm b/kernel/main.asm
index c3c6da9..f02d90b 100644
--- a/kernel/main.asm
+++ b/kernel/main.asm
@@ -37,12 +37,10 @@ str_product:
   db "Nero DOS ", 60, 234, 62, 0
 
 optrom_init:
-  ; setup data stack below code stack
-  mov bp, sp
-  sub bp, 0x80
-  ; intnum and offset to data stack
-  mov word [ss:bp], 0x18
-  mov word [ss:bp+2], start
+  push cs
+  pop es
+  mov di, start
+  mov ax, 0x18
   call intr_register
   retf