/** @file
  Default exception handler

  Copyright (c) 2008 - 2010, Apple Inc. All rights reserved.<BR>
  Copyright (c) 2021, Arm Limited. All rights reserved.<BR>

  SPDX-License-Identifier: BSD-2-Clause-Patent

**/

#include <Base.h>
#include <Library/BaseLib.h>
#include <Library/PrintLib.h>
#include <Library/ArmDisassemblerLib.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(_Write)  ((_Write) ? "!" : "")
#define BYTE(_B)       ((_B) ? "B":"")
#define USER(_B)       ((_B) ? "^" : "")

CHAR8  mMregListStr[4*15 + 1];

CHAR8 *
MRegList (
  UINT32  OpCode
  )
{
  UINTN    Index, Start, End;
  BOOLEAN  First;

  mMregListStr[0] = '\0';
  AsciiStrCatS (mMregListStr, sizeof mMregListStr, "{");
  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) {
        AsciiStrCatS (mMregListStr, sizeof mMregListStr, ",");
      } else {
        First = FALSE;
      }

      if (Start == End) {
        AsciiStrCatS (mMregListStr, sizeof mMregListStr, gReg[Start]);
        AsciiStrCatS (mMregListStr, sizeof mMregListStr, ", ");
      } else {
        AsciiStrCatS (mMregListStr, sizeof mMregListStr, gReg[Start]);
        AsciiStrCatS (mMregListStr, sizeof mMregListStr, "-");
        AsciiStrCatS (mMregListStr, sizeof mMregListStr, gReg[End]);
      }
    }
  }

  if (First) {
    AsciiStrCatS (mMregListStr, sizeof mMregListStr, "ERROR");
  }

  AsciiStrCatS (mMregListStr, sizeof mMregListStr, "}");

  // 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));
}

/**
  Place a disassembly of **OpCodePtr into buffer, and update OpCodePtr to
  point to next instruction.

  We cheat and only decode instructions that access
  memory. If the instruction is not found we dump the instruction in hex.

  @param  OpCodePtr   Pointer to pointer of ARM instruction to disassemble.
  @param  Buf         Buffer to sprintf disassembly into.
  @param  Size        Size of Buf in bytes.
  @param  Extended    TRUE dump hex for instruction too.

**/
VOID
DisassembleArmInstruction (
  IN  UINT32   **OpCodePtr,
  OUT CHAR8    *Buf,
  OUT UINTN    Size,
  IN  BOOLEAN  Extended
  )
{
  UINT32   OpCode;
  CHAR8    *Type;
  CHAR8    *Root;
  BOOLEAN  Imm, Pre, Up, WriteBack, Write, Load, Sign, Half;
  UINT32   Rn, Rd, Rm;
  UINT32   IMod, Offset8, Offset12;
  UINT32   Index;
  UINT32   ShiftImm, Shift;

  OpCode = **OpCodePtr;

  Imm       = (OpCode & BIT25) == BIT25; // I
  Pre       = (OpCode & BIT24) == BIT24; // P
  Up        = (OpCode & BIT23) == BIT23; // U
  WriteBack = (OpCode & BIT22) == BIT22; // B, also called S
  Write     = (OpCode & BIT21) == BIT21; // W
  Load      = (OpCode & BIT20) == BIT20; // L
  Sign      = (OpCode & BIT6) == BIT6;   // S
  Half      = (OpCode & BIT5) == BIT5;   // H
  Rn        = (OpCode >> 16) & 0xf;
  Rd        = (OpCode >> 12) & 0xf;
  Rm        = (OpCode & 0xf);

  if (Extended) {
    Index = AsciiSPrint (Buf, Size, "0x%08x   ", OpCode);
    Buf  += Index;
    Size -= Index;
  }

  // LDREX, STREX
  if ((OpCode  & 0x0fe000f0) == 0x01800090) {
    if (Load) {
      // 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 (Load) {
      // 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 (Write), MRegList (OpCode), USER (WriteBack));
    } 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 (Write), MRegList (OpCode), USER (WriteBack));
    }

    return;
  }

  // LDR/STR Address Mode 2
  if (((OpCode  & 0x0c000000) == 0x04000000) || ((OpCode & 0xfd70f000) == 0xf550f000)) {
    Offset12 = OpCode & 0xfff;
    if ((OpCode & 0xfd70f000) == 0xf550f000) {
      Index = AsciiSPrint (Buf, Size, "PLD");
    } else {
      Index = AsciiSPrint (Buf, Size, "%a%a%a%a %a, ", Load ? "LDR" : "STR", COND (OpCode), BYTE (WriteBack), (!(Pre) && Write) ? "T" : "", gReg[Rd]);
    }

    if (Pre) {
      if (!Imm) {
        // A5.2.2 [<Rn>, #+/-<offset_12>]
        // A5.2.5 [<Rn>, #+/-<offset_12>]
        AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a0x%x]%a", gReg[Rn], SIGN (Up), Offset12, WRITE (Write));
      } 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 (Up), WRITE (Write));
      } else {
        // A5.2.4 [<Rn>, +/-<Rm>, LSL #<shift_imm>]
        // A5.2.7 [<Rn>, +/-<Rm>, LSL #<shift_imm>]!
        ShiftImm = (OpCode >> 7) & 0x1f;
        Shift    = (OpCode >> 5) & 0x3;
        if (Shift == 0x0) {
          Type = "LSL";
        } else if (Shift == 0x1) {
          Type = "LSR";
          if (ShiftImm == 0) {
            ShiftImm = 32;
          }
        } else if (Shift == 0x2) {
          Type = "ASR";
        } else if (ShiftImm == 0) {
          AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a%a, %a, RRX]%a", gReg[Rn], SIGN (Up), gReg[Rm], WRITE (Write));
          return;
        } else {
          Type = "ROR";
        }

        AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a%a, %a, #%d]%a", gReg[Rn], SIGN (Up), gReg[Rm], Type, ShiftImm, WRITE (Write));
      }
    } else {
      // !Pre
      if (!Imm) {
        // A5.2.8  [<Rn>], #+/-<offset_12>
        AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a0x%x", gReg[Rn], SIGN (Up), Offset12);
      } else if ((OpCode & 0x03000ff0) == 0x03000000) {
        // A5.2.9  [<Rn>], +/-<Rm>
        AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a%a", gReg[Rn], SIGN (Up), gReg[Rm]);
      } else {
        // A5.2.10 [<Rn>], +/-<Rm>, LSL #<shift_imm>
        ShiftImm = (OpCode >> 7) & 0x1f;
        Shift    = (OpCode >> 5) & 0x3;

        if (Shift == 0x0) {
          Type = "LSL";
        } else if (Shift == 0x1) {
          Type = "LSR";
          if (ShiftImm == 0) {
            ShiftImm = 32;
          }
        } else if (Shift == 0x2) {
          Type = "ASR";
        } else if (ShiftImm == 0) {
          AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a%a, %a, RRX", gReg[Rn], SIGN (Up), gReg[Rm]);
          // FIx me
          return;
        } else {
          Type = "ROR";
        }

        AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a%a, %a, #%d", gReg[Rn], SIGN (Up), gReg[Rm], Type, ShiftImm);
      }
    }

    return;
  }

  if ((OpCode  & 0x0e000000) == 0x00000000) {
    // LDR/STR address mode 3
    // LDR|STR{<cond>}H|SH|SB|D <Rd>, <addressing_mode>
    if (Load) {
      if (!Sign) {
        Root = "LDR%aH %a, ";
      } else if (!Half) {
        Root = "LDR%aSB %a, ";
      } else {
        Root = "LDR%aSH %a, ";
      }
    } else {
      if (!Sign) {
        Root = "STR%aH %a ";
      } else if (!Half) {
        Root = "LDR%aD %a ";
      } else {
        Root = "STR%aD %a ";
      }
    }

    Index = AsciiSPrint (Buf, Size, Root, COND (OpCode), gReg[Rd]);

    Sign    = (OpCode & BIT6) == BIT6;
    Half    = (OpCode & BIT5) == BIT5;
    Offset8 = ((OpCode >> 4) | (OpCode * 0xf)) & 0xff;
    if (Pre & !Write) {
      // Immediate offset/index
      if (WriteBack) {
        // A5.3.2  [<Rn>, #+/-<offset_8>]
        // A5.3.4  [<Rn>, #+/-<offset_8>]!
        AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a%d]%a", gReg[Rn], SIGN (Up), Offset8, WRITE (Write));
      } else {
        // A5.3.3  [<Rn>, +/-<Rm>]
        // A5.3.5  [<Rn>, +/-<Rm>]!
        AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a%]a", gReg[Rn], SIGN (Up), gReg[Rm], WRITE (Write));
      }
    } else {
      // Register offset/index
      if (WriteBack) {
        // A5.3.6 [<Rn>], #+/-<offset_8>
        AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a%d", gReg[Rn], SIGN (Up), Offset8);
      } else {
        // A5.3.7 [<Rn>], +/-<Rm>
        AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a%a", gReg[Rn], SIGN (Up), 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 (WriteBack), 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 (Write));
    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 (Write));
    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 {
      IMod  = (OpCode >> 18) & 0x3;
      Index = AsciiSPrint (
                Buf,
                Size,
                "CPS%a %a%a%a",
                (IMod == 3) ? "ID" : "IE",
                ((OpCode & BIT8) != 0) ? "A" : "",
                ((OpCode & BIT7) != 0) ? "I" : "",
                ((OpCode & BIT6) != 0) ? "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], WriteBack ? "SPSR" : "CPSR");
    return;
  }

  if ((OpCode  & 0x0db00000) == 0x01200000) {
    // A4.1.38 MSR{<cond>} CPSR_<fields>, #<immediate> MSR{<cond>} CPSR_<fields>, <Rm>
    if (Imm) {
      // MSR{<cond>} CPSR_<fields>, #<immediate>
      AsciiSPrint (Buf, Size, "MRS%a %a_%a, #0x%x", COND (OpCode), WriteBack ? "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), WriteBack ? "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, ", Load ? "LDC" : "SDC", (OpCode >> 8) & 0xf, Rd);
    } else {
      Index = AsciiSPrint (Buf, Size, "%a%a 0x%x, CR%d, ", Load ? "LDC" : "SDC", COND (OpCode), (OpCode >> 8) & 0xf, Rd);
    }

    if (!Pre) {
      if (!Write) {
        // 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 (Up), OpCode & 0xff);
      }
    } else {
      // A5.5.5.2 [<Rn>, #+/-<offset_8>*4 ]!
      AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a0x%x*4]%a", gReg[Rn], SIGN (Up), OpCode & 0xff, WRITE (Write));
    }
  }

  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", Load ? "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", Load ? "MRRC" : "MCRR", COND (OpCode), (OpCode >> 4) & 0xf, (OpCode >> 20) & 0xf, gReg[Rd], gReg[Rn], Rm);
    return;
  }

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

  *OpCodePtr += 1;
  return;
}