unit dsPIC33_Traps;

// ******************************************************************************
//
// * Copyright:
//     (c) Mustangpeak Software 2012.
//
//     The contents of this file are subject to the GNU GPL v3 licence/ you maynot use
//     this file except in compliance with the License. You may obtain a copy of the
//     License at http://www.gnu.org/licenses/gpl.html
//
// * Revision History:
//     2012-02-01:   Created
//     2012-10-07:   Version 1.0
//
// * Description:

//
// *****************************************************************************

{$I Options.inc}

{$DEFINE ENABLE_TRAPS}
{$DEFINE ENABLE_TOGGLE_PIN_ON_TRAP}
{$DEFINE ENABLE_LOOP_FOREVER_ON_TRAP}
{$DEFINE PRINT_TRAP}


{$IFDEF ENABLE_TRAPS}
  {$IFDEF ENABLE_TOGGLE_PIN_ON_TRAP}
  var
    TrapFlagPin: sbit; sfr; external;
    Trap_PC_Lo,
    Trap_PC_Hi: Word;
    Trap_0,
    Trap_1,
    Trap_2,
    Trap_3,
    Trap_4,
    Trap_5,
    Trap_6: Word;
  {$ENDIF}
{$ENDIF}

implementation

{$IFDEF ENABLE_TRAPS}
  {$IFDEF PRINT_TRAP}
  var
    s1: string[32];
  {$ENDIF}
{$ENDIF}

{$IFDEF ENABLE_TRAPS}

procedure Trap_OscillatorFail; iv IVT_ADDR_OSCILLATORFAIL;
begin
  {$IFDEF PRINT_TRAP}UART1_Write_Text('Oscillator Error');{$ENDIF}
  asm
 //   MOV [w15-36], w13
 //   MOV w13, _Addr_Minus36
  //  MOV [w15-34], w13
  //  MOV w13, _Addr_Minus34
 //   MOV [w15-32], w13
 //   MOV w13, _Addr_Minus32
  end;
  OSCFAIL_bit := 0;
  {$IFDEF ENABLE_LOOP_FOREVER_ON_TRAP} while True do
  begin
    {$IFDEF ENABLE_TOGGLE_PIN_ON_TRAP} TrapFlagPin := not TrapFlagPin; Delay_us(100); TrapFlagPin := not TrapFlagPin; {$ENDIF ENABLE_TOGGLE_PIN_ON_TRAP}
  end
  {$ENDIF ENABLE_LOOP_FOREVER_ON_TRAP}
end;

procedure Trap_AddressError; iv IVT_ADDR_ADDRESSERROR; ics ICS_OFF;
type
  PWord = ^Word;
var
  i: Integer;
 // WordPtr: ^Word;
begin
//  {$IFDEF PRINT_TRAP}UART1_Write_Text('Address Error');{$ENDIF}
  asm
    MOV #50, w0
    PUSH w0
    MOV [w15-50], w13
    MOV w13, _Trap_PC_Lo
    MOV [w15-48], w13
    MOV w13, _Trap_PC_Hi
    
    MOV [w15-58], w13
    MOV w13, _Trap_0
    MOV [w15-56], w13
    MOV w13, _Trap_1
    MOV [w15-64], w13
    MOV w13, _Trap_2
    MOV [w15-62], w13
    MOV w13, _Trap_3
  {  MOV [w15-44], w13
    MOV w13, _Trap_4
    MOV [w15-2], w13
    MOV w13, _Trap_5  }

  end;

  {$IFDEF PRINT_TRAP}
  
  WordToHex(Trap_PC_Lo and $FFFE, s1);
  UART1_Write_Text(s1 + #13+#10);
  IntToHex(Trap_PC_Hi and $007F, s1);
  UART1_Write_Text(s1 + #13+#10+#13+#10);
  
  WordToHex(Trap_0 and $FFFE, s1);
  UART1_Write_Text(s1 + #13+#10);
  IntToHex(Trap_1 and $007F, s1);
  UART1_Write_Text(s1 + #13+#10+#13+#10);
  
  WordToHex(Trap_2 and $FFFE, s1);
  UART1_Write_Text(s1 + #13+#10);
  IntToHex(Trap_3 and $007F, s1);
  UART1_Write_Text(s1 + #13+#10+#13+#10);

  {$ENDIF}
  
   ADDRERR_bit := 0;
  
  
  {$IFDEF ENABLE_LOOP_FOREVER_ON_TRAP} while True do
  begin
    {$IFDEF ENABLE_TOGGLE_PIN_ON_TRAP} TrapFlagPin := not TrapFlagPin; Delay_us(200); TrapFlagPin := not TrapFlagPin; {$ENDIF ENABLE_TOGGLE_PIN_ON_TRAP}
  end
  {$ENDIF ENABLE_LOOP_FOREVER_ON_TRAP}
end;

procedure Trap_StackError; iv IVT_ADDR_STACKERROR;
begin
  {$IFDEF PRINT_TRAP}UART1_Write_Text('Stack Error');{$ENDIF}
  asm
  //  MOV [w15-36], w13
  //  MOV w13, _Addr_Minus36
  //  MOV [w15-34], w13
  //  MOV w13, _Addr_Minus34
//    MOV [w15-32], w13
//    MOV w13, _Addr_Minus32
  end;
  STKERR_bit := 0;
{$IFDEF ENABLE_LOOP_FOREVER_ON_TRAP} while True do
  begin
    {$IFDEF ENABLE_TOGGLE_PIN_ON_TRAP} TrapFlagPin := not TrapFlagPin; Delay_us(300); TrapFlagPin := not TrapFlagPin; {$ENDIF ENABLE_TOGGLE_PIN_ON_TRAP}
  end
  {$ENDIF ENABLE_LOOP_FOREVER_ON_TRAP}
end;

procedure Trap_MathError; iv IVT_ADDR_MATHERROR;
begin
  {$IFDEF PRINT_TRAP}UART1_Write_Text('Math Error');{$ENDIF}
  asm
  //  MOV [w15-36], w13
  //  MOV w13, _Addr_Minus36
  //  MOV [w15-34], w13
  //  MOV w13, _Addr_Minus34
 //   MOV [w15-32], w13
//    MOV w13, _Addr_Minus32
  end;
  MATHERR_bit := 0;
{$IFDEF ENABLE_LOOP_FOREVER_ON_TRAP} while True do
  begin
    {$IFDEF ENABLE_TOGGLE_PIN_ON_TRAP} TrapFlagPin := not TrapFlagPin; Delay_us(400); TrapFlagPin := not TrapFlagPin; {$ENDIF ENABLE_TOGGLE_PIN_ON_TRAP}
  end
  {$ENDIF ENABLE_LOOP_FOREVER_ON_TRAP}end;
{$ENDIF}

end.