/**************************************************************************
MODULE:    USER - User System Functions
CONTAINS:  MicroCANopen hardware specific system functions
COPYRIGHT: (c) Embedded Systems Academy (EmSA) 2002-2019
           All rights reserved. www.em-sa.com/nxp
DISCLAIM:  Read and understand our disclaimer before using this code!
           www.esacademy.com/disclaim.htm
           This software was written in accordance to the guidelines at
           www.esacademy.com/software/softwarestyleguide.pdf
LICENSE:   THIS IS THE NXP SDK VERSION OF MICROCANOPEN PLUS
           Licensed under a modified BSD License. See LICENSE.INFO
           file in the project root for full license information.
VERSION:   7.01, EmSA 02-APR-20
           $LastChangedDate: 2020-04-21 00:39:38 -0700 (Tue, 21 Apr 2020) $
           $LastChangedRevision: 4947 $
***************************************************************************/ 

#include "fsl_device_registers.h" // Peripheral Registers
#include "fsl_wwdt.h"
#if !defined(FSL_FEATURE_WWDT_HAS_NO_PDCFG) || (!FSL_FEATURE_WWDT_HAS_NO_PDCFG)
#include "fsl_power.h"
#endif

#include "mcop_inc.h"

#include "stackinit.h"

// watchdog clock frequency
#if (defined(LPC55S16_SERIES))
 #define WDT_CLK_FREQ CLOCK_GetWdtClkFreq()
#else
#define WDT_CLK_FREQ CLOCK_GetFreq(kCLOCK_WdtOsc)
#endif

/**************************************************************************
GLOBAL FUNCTIONS
***************************************************************************/ 

/**************************************************************************
DOES:    Call-back function for occurrence of a fatal error.
         Stops operation and displays blinking error pattern on LED
**************************************************************************/
void MCOUSER_FatalError (uint16_t ErrCode)
{
uint16_t timeout;

#if USE_CANOPEN_FD
  PRINTF("\r\nCANopen FD Library Event - Fatal Error: 0x%04X ",ErrCode);
#else
  PRINTF("\r\nCANopen Library Event - Fatal Error: 0x%04X ",ErrCode);
#endif

  // Remember last error for stack
  gMCOConfig.last_fatal = ErrCode;

#if USE_LEDS
  LED_ERR_ON;
  gMCOConfig.LEDErr = LED_ON;
#endif

#if USE_EMCY
  MCOP_PushEMCY(0x6100,ErrCode >> 8,ErrCode,0,0,0);
#endif // USE_EMCY

  // Wait 10ms
  timeout = MCOHW_GetTime() + 10;
  while (!MCOHW_IsTimeExpired(timeout))
  { // Wait until timeout
  }

  if (ErrCode >= ERR_FATAL)
  { // Fatal error, should abort/reset
    MCOUSER_ResetApplication();
  }

  // Warning only, simply timeout, then continue
}


/**************************************************************************
DOES:    Call-back function for reset application.
         Starts the watchdog and waits until watchdog causes a reset.
**************************************************************************/
void MCOUSER_ResetApplication (void)
{
#if USE_CANOPEN_FD
  PRINTF("\r\nCANopen Library Event - Reset Application ");
#else
  PRINTF("\r\nCANopen Library Event - Reset Application ");
#endif

  {
  wwdt_config_t config;

#if !defined(FSL_FEATURE_WWDT_HAS_NO_PDCFG) || (!FSL_FEATURE_WWDT_HAS_NO_PDCFG)
  POWER_DisablePD(kPDRUNCFG_PD_WDT_OSC);
#endif

  WWDT_GetDefaultConfig(&config);

  config.timeoutValue = 2;
  config.warningValue = 1;
  config.windowValue = 0xFFFFFF;
  // Configure WWDT to reset on timeout
  config.enableWatchdogReset = true;
  // Setup watchdog clock frequency(Hz)
  config.clockFreq_Hz = WDT_CLK_FREQ;

  WWDT_Init(WWDT, &config);
  
  while (1) // Now wait until Watchdog hits
  {
  }
  }
}


/**************************************************************************
DOES:    Call-back function for reset communication.
         Re-initializes the process image and the entire MicroCANopen
         communication.
**************************************************************************/
void MCOUSER_ResetCommunication (void)
{
  uint16_t can_bps;
#if USE_CANOPEN_FD
  uint16_t can_data_bps;
#endif
  uint8_t node_id;

  node_id = NODEID_DCF;
  can_bps = CAN_BITRATE_DCF;
#if USE_CANOPEN_FD
  can_data_bps = CAN_BRS_BITRATE;
  PRINTF("\r\nCANopen FD Library Event - Reset Communication, ");
  PRINTF("nominal bitrate %dkbps, data bitrate %dkbps, node id %d ",can_bps,can_data_bps,node_id);
#else
  PRINTF("\r\nCANopen Library Event - Reset Communication, ");
  PRINTF("bitrate %dkbps, node id %d ",can_bps,node_id);
#endif


  if (MCO_Init(
    can_bps,
#if USE_CANOPEN_FD
    can_data_bps,
#endif
    node_id,
    DEFAULT_HEARTBEAT))
  {
    //Initialization of PDOs comes from EDS
    INITPDOS_CALLS

    // Initialization of heartbeat consumers from EDS
    INITHBCONSUMER_CALLS

#if USE_STORE_PARAMETERS
    MCOSP_GetStoredParameters();
#endif
  }

}


#if USECB_NMTCHANGE
/**************************************************************************
DOES:    Called when the NMT state of the stack changes
RETURNS: nothing
**************************************************************************/
void MCOUSER_NMTChange
  (
  uint8_t nmtstate    // new nmt state of stack
  )
{
  // unused
#if USE_CANOPEN_FD
	PRINTF("\r\nCANopen FD Library Event - NMT Change: 0x%02X ",nmtstate);
#else
	PRINTF("\r\nCANopen Library Event - NMT Change: 0x%02X ",nmtstate);
#endif
	if (nmtstate == NMTSTATE_BOOT)
		PRINTF("boot ");
	else if (nmtstate == NMTSTATE_PREOP)
		PRINTF("pre-operational ");
	else if (nmtstate == NMTSTATE_OP)
		PRINTF("operational ");
	else if (nmtstate == NMTSTATE_STOP)
		PRINTF("stopped ");
}
#endif


#if USECB_ODSERIAL
/**************************************************************************
DOES:    This function is called upon read requests to Object Dictionary
         entry [1018h,4] - serial number
RETURNS: The 32bit serial number
**************************************************************************/
uint32_t MCOUSER_GetSerial (
  void
  )
{
uint32_t serial = 0;
  // replace with code to read serial number, for example from 
  // non-volatile memory
#if USE_CANOPEN_FD
  PRINTF("\r\nCANopen FD Library Event - Request serial nr: 0x%08X ",serial);
#else
  PRINTF("\r\nCANopen Library Event - Request serial nr: 0x%08X ",serial);
#endif
  return serial;
}
#endif // USECB_ODSERIAL


/**************************************************************************
DOES:    Call-back function, heartbeat lost, timeout occured.
         Gets called when a heartbeat timeout occured for a node.
RETURNS: Nothing
**************************************************************************/
void MCOUSER_HeartbeatLost (
  uint8_t node_id
  )
{
#if USE_CANOPEN_FD
  PRINTF("\r\nCANopen FD Library Event - Heartbeat lost node: %d ",node_id);
#else
  PRINTF("\r\nCANopen Library Event - Heartbeat lost node: %d ",node_id);
#endif
  // Add code to react on the loss of heartbeat,
  // if node is essential, switch to pre-operational mode
  MCO_HandleNMTRequest(NMTMSG_PREOP);
}


#if USECB_TIMEOFDAY
/**************************************************************************
DOES:    This function is called if the message with the time object has
         been received. This example implementtion calculates the current
         time in hours, minutes, seconds.
RETURNS: nothing
**************************************************************************/
// Global variables holding clock info
uint32_t hours;
uint32_t minutes;
uint32_t seconds;

void MCOUSER_TimeOfDay (
  uint32_t millis, // Milliseconds since midnight
  uint16_t days  // Number of days since January 1st, 1984
  )
{
  if (millis < (1000UL * 60 * 60 * 24))
  { // less Milliseconds as one day has
    // calculate hours, minutes & seconds since midnight
    hours = millis / (1000UL * 60 * 60);
    minutes = (millis - (hours * 1000UL * 60 * 60)) / (1000UL * 60);
    seconds = (millis - (hours * 1000UL * 60 * 60) - (minutes * 1000UL * 60)) / 1000;
  }
}
#endif // USECB_TIMEOFDAY


/**************************************************************************
END-OF-FILE 
***************************************************************************/ 

