/** @file
  Default exception handler

  Copyright (c) 2008-2010, Apple Inc. All rights reserved.
  
  All rights reserved. This program and the accompanying materials
  are licensed and made available under the terms and conditions of the BSD License
  which accompanies this distribution.  The full text of the license may be found at
  http://opensource.org/licenses/bsd-license.php

  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.

**/

#include <Base.h>
#include <Library/BaseLib.h>
#include <Library/UefiLib.h>
#include <Library/PrintLib.h>


CHAR8 *gCondition[] = {
  "EQ",
  "NE",
  "CS",
  "CC",
  "MI",
  "PL",
  "VS",
  "VC",
  "HI",
  "LS",
  "GE",
  "LT",
  "GT",
  "LE",
  "",
  "2"
};

#define COND(_a)  gCondition[(_a) >> 28]

CHAR8 *gReg[] = {
  "r0",
  "r1",
  "r2",
  "r3",
  "r4",
  "r5",
  "r6",
  "r7",
  "r8",
  "r9",
  "r10",
  "r11",
  "r12",
  "sp",
  "lr",
  "pc"
};

CHAR8 *gLdmAdr[] = {
  "DA",
  "IA",
  "DB",
  "IB"
};

CHAR8 *gLdmStack[] = {
  "FA",
  "FD",
  "EA",
  "ED"
};

#define LDM_EXT(_reg, _off) ((_reg == 13) ? gLdmStack[(_off)] : gLdmAdr[(_off)])


#define SIGN(_U)  ((_U) ? "" : "-")
#define WRITE(_W) ((_W) ? "!" : "")
#define BYTE(_B)  ((_B) ? "B":"")
#define USER(_B)  ((_B) ? "^" : "")

CHAR8 mMregListStr[4*15 + 1];

CHAR8 *
MRegList (
  UINT32  OpCode
  )
{
  UINTN     Index, Start, End;
  CHAR8     *Str;
  BOOLEAN   First;
 
  Str = mMregListStr;
  *Str = '\0';
  AsciiStrCat  (Str, "{");
  for (Index = 0, First = TRUE; Index <= 15; Index++) {
    if ((OpCode & (1 << Index)) != 0) {
      Start = End = Index;
      for (Index++; ((OpCode & (1 << Index)) != 0) && Index <= 15; Index++) {
        End = Index;
      }
      
      if (!First) {
        AsciiStrCat  (Str, ",");
      } else {
        First = FALSE;
      }
      
      if (Start == End) {
        AsciiStrCat  (Str, gReg[Start]);
        AsciiStrCat  (Str, ", ");
      } else {
        AsciiStrCat  (Str, gReg[Start]);
        AsciiStrCat  (Str, "-");
        AsciiStrCat  (Str, gReg[End]);
      }
    }
  }
  if (First) {
    AsciiStrCat  (Str, "ERROR");
  }
  AsciiStrCat  (Str, "}");
  
  // BugBug: Make caller pass in buffer it is cleaner
  return mMregListStr;
}

CHAR8 *
FieldMask (
  IN  UINT32  Mask
  )
{
  return "";
}

UINT32
RotateRight (
  IN UINT32 Op,
  IN UINT32 Shift
  )
{
  return (Op >> Shift) | (Op << (32 - Shift));
}


/**
  DEBUG print the faulting instruction. We cheat and only decode instructions that access 
  memory. If the instruction is not found we dump the instruction in hex.
   
  @param  Insturction   ARM instruction to disassemble.  
  
**/
VOID
DisassembleArmInstruction (
  IN  UINT32    *OpCodePtr,
  OUT CHAR8     *Buf,
  OUT UINTN     Size
  )
{
  UINT32    OpCode = *OpCodePtr;
  CHAR8     *Type, *Root;
  BOOLEAN   I, P, U, B, W, L, S, H;
  UINT32    Rn, Rd, Rm;
  UINT32    imode, offset_8, offset_12;
  UINT32    Index;
  UINT32    shift_imm, shift;

  I = (OpCode & BIT25) == BIT25;
  P = (OpCode & BIT24) == BIT24;
  U = (OpCode & BIT23) == BIT23;
  B = (OpCode & BIT22) == BIT22;  // Also called S
  W = (OpCode & BIT21) == BIT21;  
  L = (OpCode & BIT20) == BIT20;
  S = (OpCode & BIT6) == BIT6;
  H = (OpCode & BIT5) == BIT5;
  Rn = (OpCode >> 16) & 0xf;
  Rd = (OpCode >> 12) & 0xf;
  Rm = (OpCode & 0xf);

  // LDREX, STREX
  if ((OpCode  & 0x0fe000f0) == 0x01800090) {
    if (L) {
      // A4.1.27  LDREX{<cond>} <Rd>, [<Rn>] 
      AsciiSPrint (Buf, Size, "LDREX%a %a, [%a]", COND (OpCode), gReg[Rd], gReg[Rn]);   
    } else {
     // A4.1.103  STREX{<cond>} <Rd>, <Rm>, [<Rn>]
      AsciiSPrint (Buf, Size, "STREX%a %a, %a, [%a]", COND (OpCode), gReg[Rd], gReg[Rn], gReg[Rn]);   
    } 
    return;
  }
  
  // LDM/STM
  if ((OpCode  & 0x0e000000) == 0x08000000) {
    if (L) {
      // A4.1.20 LDM{<cond>}<addressing_mode> <Rn>{!}, <registers>
      // A4.1.21 LDM{<cond>}<addressing_mode> <Rn>, <registers_without_pc>^
      // A4.1.22 LDM{<cond>}<addressing_mode> <Rn>{!}, <registers_and_pc>^
      AsciiSPrint (Buf, Size, "LDM%a%a, %a%a, %a", COND (OpCode), LDM_EXT (Rn ,(OpCode >> 23) & 3), gReg[Rn], WRITE (W), MRegList (OpCode), USER (B));   
    } else {
      // A4.1.97 STM{<cond>}<addressing_mode> <Rn>{!}, <registers>
      // A4.1.98 STM{<cond>}<addressing_mode> <Rn>, <registers>^
      AsciiSPrint (Buf, Size, "STM%a%a, %a%a, %a", COND (OpCode), LDM_EXT (Rn ,(OpCode >> 23) & 3), gReg[Rn], WRITE (W), MRegList (OpCode), USER (B));   
    }   
    return;
  }

  // LDR/STR Address Mode 2
  if ( ((OpCode  & 0x0c000000) == 0x04000000) || ((OpCode & 0xfd70f000 ) == 0xf550f000) ) {
    offset_12 = OpCode & 0xfff;
    if ((OpCode & 0xfd70f000 ) == 0xf550f000) {
      Index = AsciiSPrint (Buf, Size, "PLD");
    } else {
      Index = AsciiSPrint (Buf, Size, "%a%a%a%a %a, ", L ? "LDR" : "STR", COND (OpCode), BYTE (B), (!P & W) ? "T":"", gReg[Rd]); 
    }
    if (P) {
      if (!I) {
        // A5.2.2 [<Rn>, #+/-<offset_12>]
        // A5.2.5 [<Rn>, #+/-<offset_12>]
        AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a0x%x]%a", gReg[Rn], SIGN (U), offset_12, WRITE (W));
      } else if ((OpCode & 0x03000ff0) == 0x03000000) {
        // A5.2.3 [<Rn>, +/-<Rm>]
        // A5.2.6 [<Rn>, +/-<Rm>]!
        AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a%a]%a", gReg[Rn], SIGN (U), WRITE (W));
      } else {
        // A5.2.4 [<Rn>, +/-<Rm>, LSL #<shift_imm>]
        // A5.2.7 [<Rn>, +/-<Rm>, LSL #<shift_imm>]!
        shift_imm = (OpCode >> 7) & 0x1f;
        shift = (OpCode >> 5) & 0x3;
        if (shift == 0x0) {
          Type = "LSL";
        } else if (shift == 0x1) {
          Type = "LSR";
          if (shift_imm == 0) {
            shift_imm = 32;
          }
        } else if (shift == 0x12) {
          Type = "ASR";
        } else if (shift_imm == 0) {
          AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a%a, %a, RRX]%a", gReg[Rn], SIGN (U), gReg[Rm], WRITE (W));
          return;
        } else {
          Type = "ROR";
        }
          
        AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a%a, %a, #%d]%a", gReg[Rn], SIGN (U), gReg[Rm], Type, shift_imm, WRITE (W));
      }
    } else {  // !P
      if (!I) {
        // A5.2.8  [<Rn>], #+/-<offset_12>
        AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a0x%x", gReg[Rn], SIGN (U), offset_12);
      } else if ((OpCode & 0x03000ff0) == 0x03000000) {
        // A5.2.9  [<Rn>], +/-<Rm>
        AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a%a", gReg[Rn], SIGN (U), gReg[Rm]);
      } else {
        // A5.2.10 [<Rn>], +/-<Rm>, LSL #<shift_imm>
        shift_imm = (OpCode >> 7) & 0x1f;
        shift = (OpCode >> 5) & 0x3;

        if (shift == 0x0) {
          Type = "LSL";
        } else if (shift == 0x1) {
          Type = "LSR";
          if (shift_imm == 0) {
            shift_imm = 32;
          }
        } else if (shift == 0x12) {
          Type = "ASR";
        } else if (shift_imm == 0) {
          AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a%a, %a, RRX", gReg[Rn], SIGN (U), gReg[Rm]);
          // FIx me
          return;
        } else {
          Type = "ROR";
        }
          
        AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a%a, %a, #%d", gReg[Rn], SIGN (U), gReg[Rm], Type, shift_imm);
      }
    }
    return;  
  }
  
  if ((OpCode  & 0x0e000000) == 0x00000000) {
    // LDR/STR address mode 3
    // LDR|STR{<cond>}H|SH|SB|D <Rd>, <addressing_mode>
    if (L) {
      if (!S) {
        Root = "LDR%aH %a, ";
      } else if (!H) {
        Root = "LDR%aSB %a, ";
      } else {
        Root = "LDR%aSH %a, ";
      }
    } else {
      if (!S) {
        Root = "STR%aH %a ";
      } else if (!H) {
        Root = "LDR%aD %a ";
      } else {
        Root = "STR%aD %a ";
      }
    }
  
    Index = AsciiSPrint (Buf, Size, Root, COND (OpCode), gReg[Rd]); 

    S = (OpCode & BIT6) == BIT6;
    H = (OpCode & BIT5) == BIT5;
    offset_8 = ((OpCode >> 4) | (OpCode * 0xf)) & 0xff;
    if (P & !W) {
      // Immediate offset/index
      if (B) {
        // A5.3.2  [<Rn>, #+/-<offset_8>]
        // A5.3.4  [<Rn>, #+/-<offset_8>]!
        AsciiSPrint  (&Buf[Index], Size - Index, "[%a, #%a%d]%a", gReg[Rn], SIGN (U), offset_8, WRITE (W));
      } else {
        // A5.3.3  [<Rn>, +/-<Rm>]
        // A5.3.5  [<Rn>, +/-<Rm>]!
        AsciiSPrint  (&Buf[Index], Size - Index, "[%a, #%a%]a", gReg[Rn], SIGN (U), gReg[Rm], WRITE (W));
      }
    } else {
      // Register offset/index
      if (B) {
        // A5.3.6 [<Rn>], #+/-<offset_8>
        AsciiSPrint  (&Buf[Index], Size - Index, "[%a], #%a%d", gReg[Rn], SIGN (U), offset_8);
      } else {
        // A5.3.7 [<Rn>], +/-<Rm>
        AsciiSPrint  (&Buf[Index], Size - Index, "[%a], #%a%a", gReg[Rn], SIGN (U), gReg[Rm]);
      }
    }
    return;
  }

  if ((OpCode  & 0x0fb000f0) == 0x01000050) {
    // A4.1.108  SWP   SWP{<cond>}B <Rd>, <Rm>, [<Rn>]
    // A4.1.109  SWPB  SWP{<cond>}B <Rd>, <Rm>, [<Rn>]
    AsciiSPrint (Buf, Size, "SWP%a%a %a, %a, [%a]", COND (OpCode), BYTE (B), gReg[Rd], gReg[Rm], gReg[Rn]);
    return;
  }
  
  if ((OpCode  & 0xfe5f0f00) == 0xf84d0500) {
    // A4.1.90 SRS SRS<addressing_mode> #<mode>{!}
    AsciiSPrint (Buf, Size, "SRS%a #0x%x%a", gLdmStack[(OpCode >> 23) & 3], OpCode & 0x1f, WRITE (W));
    return;
  }

  if ((OpCode  & 0xfe500f00) == 0xf8100500) {
    // A4.1.59 RFE<addressing_mode> <Rn>{!}
    AsciiSPrint (Buf, Size, "RFE%a %a", gLdmStack[(OpCode >> 23) & 3], gReg[Rn], WRITE (W));
    return;
  }
  
  if ((OpCode  & 0xfff000f0) == 0xe1200070) {
    // A4.1.7 BKPT <immed_16>
    AsciiSPrint (Buf, Size, "BKPT %x", ((OpCode >> 8) | (OpCode & 0xf)) & 0xffff);
    return;
  }  
  
  if ((OpCode  & 0xfff10020) == 0xf1000000) {
    // A4.1.16 CPS<effect> <iflags> {, #<mode>}
    if (((OpCode >> 6) & 0x7) == 0) {
      AsciiSPrint (Buf, Size, "CPS #0x%x", (OpCode & 0x2f));
    } else {
      imode = (OpCode >> 18) & 0x3;
      Index = AsciiSPrint (Buf, Size, "CPS%a %a%a%a", (imode == 3) ? "ID":"IE", (OpCode & BIT8) ? "A":"", (OpCode & BIT7) ? "I":"", (OpCode & BIT6) ? "F":"");
      if ((OpCode & BIT17) != 0) {
        AsciiSPrint (&Buf[Index], Size - Index, ", #0x%x", OpCode & 0x1f);
      }
    }
    return;
  }  
    
  if ((OpCode  & 0x0f000000) == 0x0f000000) {
    // A4.1.107 SWI{<cond>} <immed_24>
    AsciiSPrint (Buf, Size, "SWI%a %x", COND (OpCode), OpCode & 0x00ffffff);
    return;
  }  

  if ((OpCode  & 0x0fb00000) == 0x01000000) {
    // A4.1.38 MRS{<cond>} <Rd>, CPSR  MRS{<cond>} <Rd>, SPSR
    AsciiSPrint (Buf, Size, "MRS%a %a, %a", COND (OpCode), gReg[Rd], B ? "SPSR" : "CPSR");
    return;
  }  


  if ((OpCode  & 0x0db00000) == 0x03200000) {
    // A4.1.38 MSR{<cond>} CPSR_<fields>, #<immediate> MSR{<cond>} CPSR_<fields>, <Rm>
    if (I) {
      // MSR{<cond>} CPSR_<fields>, #<immediate>
      AsciiSPrint (Buf, Size, "MRS%a %a_%a, #0x%x", COND (OpCode),  B ? "SPSR" : "CPSR", FieldMask ((OpCode >> 16) & 0xf), RotateRight (OpCode & 0xf, ((OpCode >> 8) & 0xf) *2));
    } else {
      // MSR{<cond>} CPSR_<fields>, <Rm>
      AsciiSPrint (Buf, Size, "MRS%a %a_%a, %a", COND (OpCode), B ? "SPSR" : "CPSR", gReg[Rd]);
    }
    return;
  }  

  if ((OpCode  & 0xff000010) == 0xfe000000) {
    // A4.1.13 CDP{<cond>} <coproc>, <opcode_1>, <CRd>, <CRn>, <CRm>, <opcode_2>
    AsciiSPrint (Buf, Size, "CDP%a 0x%x, 0x%x, CR%d, CR%d, CR%d, 0x%x", COND (OpCode), (OpCode >> 8) & 0xf, (OpCode >> 20) & 0xf, Rn, Rd, Rm, (OpCode >> 5) &0x7);
    return;
  }
  
  if ((OpCode  & 0x0e000000) == 0x0c000000) {
    // A4.1.19 LDC and A4.1.96 SDC
    if ((OpCode & 0xf0000000) == 0xf0000000) {
      Index = AsciiSPrint (Buf, Size, "%a2 0x%x, CR%d, ", L ? "LDC":"SDC", (OpCode >> 8) & 0xf, Rd);
    } else {
      Index = AsciiSPrint (Buf, Size, "%a%a 0x%x, CR%d, ",  L ? "LDC":"SDC", COND (OpCode), (OpCode >> 8) & 0xf, Rd);
    }
    
    if (!P) {
      if (!W) { 
        // A5.5.5.5 [<Rn>], <option>
      AsciiSPrint (&Buf[Index], Size - Index, "[%a], {0x%x}", gReg[Rn], OpCode & 0xff); 
      } else {
        // A.5.5.4  [<Rn>], #+/-<offset_8>*4
      AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a0x%x*4", gReg[Rn], SIGN (U), OpCode & 0xff); 
      }
    } else {
      // A5.5.5.2 [<Rn>, #+/-<offset_8>*4 ]!
      AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a0x%x*4]%a", gReg[Rn], SIGN (U), OpCode & 0xff, WRITE (W)); 
    }
    
  }
  
  if ((OpCode  & 0x0f000010) == 0x0e000010) {
    // A4.1.32 MRC2, MCR2 
    AsciiSPrint (Buf, Size, "%a%a 0x%x, 0x%x, %a, CR%d, CR%d, 0x%x", L ? "MRC":"MCR", COND (OpCode), (OpCode >> 8) & 0xf, (OpCode >> 20) & 0xf, gReg[Rd], Rn, Rm, (OpCode >> 5) &0x7);
    return;    
  }

  if ((OpCode  & 0x0ff00000) == 0x0c400000) {
    // A4.1.33 MRRC2, MCRR2 
    AsciiSPrint (Buf, Size, "%a%a 0x%x, 0x%x, %a, %a, CR%d", L ? "MRRC":"MCRR", COND (OpCode), (OpCode >> 4) & 0xf, (OpCode >> 20) & 0xf, gReg[Rd], gReg[Rn], Rm);
    return;    
  }

  AsciiSPrint (Buf, Size, "Faulting OpCode 0x%08x", OpCode);
  return;
}