From 539878fb3d12a425032c67ed4fde1883c65dffd5 Mon Sep 17 00:00:00 2001
From: jcar <devnull@localhost>
Date: Tue, 23 Apr 2013 16:23:28 +0200
Subject: [PATCH] Fixed ticket #2231

---
 arch/ppc/mpc55xx/drivers/Pwm.c   | 53 +++++++++++++++++++++++---------
 system/EcuM/EcuM_Callout_Stubs.c |  4 +--
 2 files changed, 40 insertions(+), 17 deletions(-)

diff --git a/arch/ppc/mpc55xx/drivers/Pwm.c b/arch/ppc/mpc55xx/drivers/Pwm.c
index 7f798ef9..b2cf360e 100644
--- a/arch/ppc/mpc55xx/drivers/Pwm.c
+++ b/arch/ppc/mpc55xx/drivers/Pwm.c
@@ -281,6 +281,21 @@ void Pwm_Init(const Pwm_ConfigType* ConfigPtr) {
     for (channel_iterator = 0; channel_iterator < PWM_NUMBER_OF_CHANNELS; channel_iterator++) {
     	const Pwm_ChannelConfigurationType* channelConfig = &ConfigPtr->Channels[channel_iterator];
     	Pwm_ChannelType channel = channelConfig->channel;
+    	volatile struct EMIOS_tag *emiosHw;
+
+#if defined(CFG_MPC560X)
+		if(channel <= PWM_NUMBER_OF_EACH_EMIOS-1) {
+			emiosHw = &EMIOS_0;
+		} else {
+			emiosHw = &EMIOS_1;
+			channel -= PWM_NUMBER_OF_EACH_EMIOS;
+		}
+#else
+		emiosHw = &EMIOS;
+#endif
+
+		// Clear the disable bit for this channel
+		emiosHw->UCDIS.R &= ~(1 << channel);
 
     	configureChannel( channelConfig );
 
@@ -523,27 +538,35 @@ void Pwm_Init(const Pwm_ConfigType* ConfigPtr) {
 
 		#endif
     }
+
+	/* Enable module */
+	#if defined(CFG_MPC5516) || defined(CFG_MPC5567)
+		EMIOS.MCR.B.MDIS = 0;
+    #elif defined(CFG_MPC560X)
+		EMIOS_0.MCR.B.MDIS = 0;
+		EMIOS_1.MCR.B.MDIS = 0;
+	#endif
 }
 
 #if PWM_DE_INIT_API==STD_ON
 
 void inline Pwm_DeInitChannel(Pwm_ChannelType Channel) {
-    Pwm_SetOutputToIdle(Channel);
+	volatile struct EMIOS_tag *emiosHw;
+#if defined(CFG_MPC560X)
+	if(Channel <= PWM_NUMBER_OF_EACH_EMIOS-1) {
+		emiosHw = &EMIOS_0;
+	} else {
+		emiosHw = &EMIOS_1;
+		Channel -= PWM_NUMBER_OF_EACH_EMIOS;
+	}
+#else
+	emiosHw = &EMIOS;
+#endif
 
-	#if defined(CFG_MPC5516)
-        // Set the disable bit for this channel
-    	EMIOS.UCDIS.R |= (1 << (31 - Channel));
-    #elif defined(CFG_MPC560X)
-        // Set the disable bit for this channel
-        if(Channel <= PWM_NUMBER_OF_EACH_EMIOS-1)
-        {
-        	EMIOS_0.UCDIS.R |= (1 << (Channel));
-        }
-        else
-        {
-        	EMIOS_1.UCDIS.R |= (1 << (Channel-PWM_NUMBER_OF_EACH_EMIOS));
-        }
-	#endif
+	emiosHw->CH[Channel].CADR.R = 0;
+
+	// Set the disable bit for this channel
+	emiosHw->UCDIS.R |= (1 << Channel);
 
     /*
      * PWM052: The function Pwm_DeInit shall disable all notifications.
diff --git a/system/EcuM/EcuM_Callout_Stubs.c b/system/EcuM/EcuM_Callout_Stubs.c
index e209b595..13205aa1 100644
--- a/system/EcuM/EcuM_Callout_Stubs.c
+++ b/system/EcuM/EcuM_Callout_Stubs.c
@@ -603,13 +603,13 @@ void EcuM_GenerateRamHash(void)
 	 */
 	VALIDATE_STATE( ECUM_STATE_SLEEP );
 	 
-#if defined(USE_GPT)
+#if defined(USE_GPT) && (GPT_DEINIT_API == STD_ON)
 	Gpt_DeInit();
 #endif
 #if defined(USE_ADC) && (ADC_DEINIT_API == STD_ON)
 	Adc_DeInit();
 #endif
-#if defined(USE_PWM)
+#if defined(USE_PWM) && (PWM_DE_INIT_API == STD_ON)
 	Pwm_DeInit();
 #endif
 #if defined(USE_SPI)
-- 
2.39.2