#ifndef PSR_H
#define PSR_H

#include <stdint.h>

enum execution_mode {
  MODE_USER       = 0b10000,
  MODE_FIQ        = 0b10001,
  MODE_IRQ        = 0b10010,
  MODE_SUPERVISOR = 0b10011,
  MODE_MONITOR    = 0b10110,
  MODE_ABORT      = 0b10111,
  MODE_HYPERVISOR = 0b11010,
  MODE_UNDEFINED  = 0b11011,
  MODE_SYSTEM     = 0b11111,
};

typedef union
{
  uint32_t raw;
  struct
  {
    uint32_t M_4_0      : 5; // bits 4:0
    uint32_t T          : 1; // bit  5
    uint32_t F          : 1; // bit  6
    uint32_t I          : 1; // bit  7
    uint32_t A          : 1; // bit  8
    uint32_t E          : 1; // bit  9
    uint32_t IT_7_2     : 6; // bits 15:10
    uint32_t GE_3_0     : 4; // bits 19:16
    uint32_t Bits_23_20 : 4; // bits 23:20
    uint32_t J          : 1; // bit  24
    uint32_t IT_1_0     : 2; // bits 26:25
    uint32_t Q          : 1; // bit  27
    uint32_t V          : 1; // bit  28
    uint32_t C          : 1; // bit  29
    uint32_t Z          : 1; // bit  30
    uint32_t N          : 1; // bit  31
#define PSR_MODE_4_0                    M_4_0
#define PSR_THUMB_BIT                   T
#define PSR_FIQ_MASKK_BIT               F
#define PSR_IRQ_MASK_BIT                I
#define PSR_ASYNC_ABORT_MASK_BIT        A
#define PSR_ENDIANNESS_BIT              E
#define PSR_IF_THEN_STATE_7_2           IT_7_2
#define PSR_GREATER_THAN_OR_EQUAL_FLAGS GE_3_0
    // bits 23:20 are reserved
#define PSR_JAZELLE_BIT                 J
#define PSR_IF_THEN_STATE_1_0           IT_1_0
#define PSR_CUMULATIVE_SATURATION_BIT   Q
#define PSR_OVERFLOW_CONDITION_BIT      V
#define PSR_CARRY_CONDITION_BIT         C
#define PSR_ZERO_CONDITION_BIT          Z
#define PSR_NEGATIVE_CONDITION_BIT      N
  } fields;
} PSR_t;

inline static PSR_t read_CPSR(void)
{
  PSR_t CPSR;
  // get content of current program status register
  asm("mrs %0, cpsr" : "=r" (CPSR.raw) :: "memory");
  
  return CPSR;
}

//// Write function not working for some reason... Assembly from gcc
//// looks ok. Needs checking on real hw.

//inline static void write_CPSR(PSR_t CPSR)
//{
  // write to current program status register and synchronize context
  //  asm("msr cpsr, %0\n\r"
  //      "isb" :: "r" (CPSR.raw) : "memory");
//}

inline static void set_system_mode(void)
{
  // hack to fix an unexplained bug; volatile needed in case of
  // compilation with optimizations
  volatile PSR_t CPSR __attribute__((unused)) = read_CPSR();

  //// there are 2 ways of changing mode, both with the same
  //// problem (see the long comment below)

  //// way 1
  // CPSR.fields.M_4_0 = MODE_SYSTEM;
  // write to current program status register and synchronize context
  // asm("msr cpsr, %0\n\r"
  //     "isb":: "r" (CPSR.raw) : "memory");  

  //// way 2
  asm("cps #0b11111\n\r"
      "isb" ::: "memory");
}

// The thing with writing to cpsr is weird. I used to have a single
// function that would set the system mode by:
//   1. reading the cpsr
//   2. modifying the value using bit shifts and logical or
//   3. writing the value back
// When introducing structs and bitfields I wanted to have separate
// functions for reading and writing the cpsr and have code up the
// call stack handle value modification. For some reason this didn't
// work - all would just hang when writing the CPSR. It turned out
// everything works if i call read_CPSR() from the same function, in
// which i write to cpsr. And I don't even need to use the value
// I read. I can just discard it (we're compiling without
// optimizations, so read_CPSR() is still called) and use the value
// passed as argument. I noticed an even weirder thing: the 2nd way
// of changing mode (cps instruction) doesn't work normally, but,
// just as msr, it does work when I call read_CPSR() before it. Even
// weirder - I cannot replace read_CPSR() with the actual assembly it
// does, because it stops working again! Asm generated by gcc looks
// ok, so Idk, maybe its some qemu issue (haven't tested on real RPi
// yet). I seem to have experienced uart printing inserted here and
// there do the same magic read_CPSR() does (but, again, not always).
// Unfortunately, nop repeated several hundreds times (or a recursive
// nop function, that just calls itself a given number of times)
// doesn't have this magical property :(

#endif // PSR_H