diff --git a/wrc_ptp.c b/wrc_ptp.c
index e5fbedf5a4ec1a028cdf80422192085c96f0b917..ec7a54f96de0c938d1bb6ff508907d6c74452a29 100644
--- a/wrc_ptp.c
+++ b/wrc_ptp.c
@@ -80,7 +80,7 @@ int wrc_ptp_set_mode(int mode)
 
 	ptp_mode = 0;
 	
-	ptp_enabled = 0;
+  wrc_ptp_stop();
 	
 	switch(mode)
 	{
@@ -147,6 +147,7 @@ int wrc_ptp_get_mode()
 int wrc_ptp_start()
 {
   ptpPortDS->linkUP = FALSE;
+  wr_servo_reset();
   initDataClock(&rtOpts, &ptpClockDS);
 
   ptp_enabled = 1;
@@ -156,6 +157,7 @@ int wrc_ptp_start()
 int wrc_ptp_stop()
 {
 	ptp_enabled = 0;
+  wr_servo_reset();
 	return 0;
 }