#include "nvic.h"
#include "scb.h"

void NVIC::SetPriority(IRQn_Type IRQn, uint32_t priority)
{
	// set Priority for device specific interrupts
	IP[(uint32_t)(IRQn)] = ((priority << (8 - NVIC_PRIO_BITS)) & 0xff);
}

//    This function reads the priority for the specified interrupt. The interrupt
//    number can be positive to specify an external (device specific)
//    interrupt, or negative to specify an internal (core) interrupt.

uint32_t NVIC::GetPriority(IRQn_Type IRQn)
{
	// get priority for device specific interrupts
	return((uint32_t)(IP[(uint32_t)(IRQn)] >> (8 - NVIC_PRIO_BITS)));
}

//    This function encodes the priority for an interrupt with the given priority group,
//    preemptive priority value and sub priority value.
//    In case of a conflict between priority grouping and available
//    priority bits (NVIC_PRIO_BITS) the samllest possible priority group is set.

//    The returned priority value can be used for NVIC_SetPriority(...) function

uint32_t NVIC::EncodePriority (uint32_t priorityGroup, uint32_t preemptPriority, uint32_t subPriority)
{
  uint32_t priorityGroupTmp = (priorityGroup & 0x07);          /* only values 0..7 are used          */
  uint32_t preemptPriorityBits;
  uint32_t subPriorityBits;

  preemptPriorityBits = ((7 - priorityGroupTmp) > NVIC_PRIO_BITS) ? NVIC_PRIO_BITS : 7 - priorityGroupTmp;
  subPriorityBits     = ((priorityGroupTmp + NVIC_PRIO_BITS) < 7) ? 0 : priorityGroupTmp - 7 + NVIC_PRIO_BITS;

  return (
           ((preemptPriority & ((1 << (preemptPriorityBits)) - 1)) << subPriorityBits) |
           ((subPriority     & ((1 << (subPriorityBits    )) - 1)))
         );
}

//    This function decodes an interrupt priority value with the given priority group to
//    preemptive priority value and sub priority value.
//    In case of a conflict between priority grouping and available
//    priority bits (NVIC_PRIO_BITS) the samllest possible priority group is set.
//
//    The priority value can be retrieved with NVIC_GetPriority(...) function

void NVIC::DecodePriority (uint32_t priority, uint32_t priorityGroup, uint32_t* preemptPriority, uint32_t* subPriority)
{
  uint32_t priorityGroupTmp = (priorityGroup & 0x07);          /* only values 0..7 are used          */
  uint32_t preemptPriorityBits;
  uint32_t subPriorityBits;

  preemptPriorityBits = ((7 - priorityGroupTmp) > NVIC_PRIO_BITS) ? NVIC_PRIO_BITS : 7 - priorityGroupTmp;
  subPriorityBits     = ((priorityGroupTmp + NVIC_PRIO_BITS) < 7) ? 0 : priorityGroupTmp - 7 + NVIC_PRIO_BITS;

  *preemptPriority = (priority >> subPriorityBits) & ((1 << (preemptPriorityBits)) - 1);
  *subPriority     = (priority                  ) & ((1 << (subPriorityBits    )) - 1);
}


#if 0
//    This function initiate a system reset request to reset the MCU.

void NVIC_SystemReset(void)
{
  __DSB();                                                     /* Ensure all outstanding memory accesses included
                                                                  buffered write are completed before reset */
  SCB->AIRCR  = ((0x5FA << SCB_AIRCR_VECTKEY_Pos)      |
                 (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) |
                 SCB_AIRCR_SYSRESETREQ_Msk);                   /* Keep priority group unchanged */
  __DSB();                                                     /* Ensure completion of memory access */
  while(1);                                                    /* wait until reset */
}
#endif