1.1 --- a/stage2/handlers.S	Wed Jan 27 15:51:32 2016 +0100
     1.2 +++ b/stage2/handlers.S	Mon Feb 22 17:07:10 2016 +0100
     1.3 @@ -2,7 +2,7 @@
     1.4   * Handler routines.
     1.5   *
     1.6   * Copyright (C) 2015 Nicholas FitzRoy-Dale <wzdd.code@lardcave.net>
     1.7 - * Copyright (C) 2015 Paul Boddie <paul@boddie.org.uk>
     1.8 + * Copyright (C) 2015, 2016 Paul Boddie <paul@boddie.org.uk>
     1.9   *
    1.10   * This program is free software: you can redistribute it and/or modify
    1.11   * it under the terms of the GNU General Public License as published by
    1.12 @@ -25,6 +25,15 @@
    1.13  .set noat
    1.14  
    1.15  interrupt_handler:
    1.16 +
    1.17 +	/* Save the status, mask interrupts. */
    1.18 +
    1.19 +	mfc0 $k0, $12 /* CP0_STATUS */
    1.20 +	li $k1, 0xffff03ff
    1.21 +	and $k1, $k0, $k1
    1.22 +	mtc0 $k1, $12
    1.23 +	sw $k0, -120($sp)
    1.24 +
    1.25  	sw $at, -4($sp)
    1.26  	sw $v0, -8($sp)
    1.27  	sw $v1, -12($sp)
    1.28 @@ -54,14 +63,18 @@
    1.29  	sw $fp, -108($sp)
    1.30  	sw $ra, -112($sp)
    1.31  
    1.32 -	addi $sp, $sp, -112
    1.33 +	mfc0 $k0, $14 /* CP0_EPC */
    1.34 +	nop
    1.35 +	sw $k0, -116($sp)
    1.36 +
    1.37 +	addi $sp, $sp, -120
    1.38  
    1.39  	/* Invoke the handler. */
    1.40  
    1.41  	jal     irq_handle
    1.42  	nop
    1.43  
    1.44 -	addi $sp, $sp, 112
    1.45 +	addi $sp, $sp, 120
    1.46  
    1.47  	lw $at, -4($sp)
    1.48  	lw $v0, -8($sp)
    1.49 @@ -92,6 +105,14 @@
    1.50  	lw $fp, -108($sp)
    1.51  	lw $ra, -112($sp)
    1.52  
    1.53 +	lw $k0, -116($sp)
    1.54 +	mtc0 $k0, $14 /* CP0_EPC */
    1.55 +	nop
    1.56 +
    1.57 +	lw $k0, -120($sp)
    1.58 +	mtc0 $k0, $12 /* CP0_STATUS */
    1.59 +	nop
    1.60 +
    1.61  	eret
    1.62  	nop
    1.63