3 files changed, 40 insertions, 36 deletions

diff --git a/etc/init.rc b/etc/init.rc
index 8a12b7cfd..e8106f391 100644
--- a/etc/init.rc
+++ b/etc/init.rc
@@ -42,9 +42,9 @@ on fs
 
     write /sys/class/android_usb/android0/enable 0
     write /sys/class/android_usb/android0/idVendor 18D1
-    write /sys/class/android_usb/android0/idProduct D002
+    write /sys/class/android_usb/android0/idProduct 4EE2
     write /sys/class/android_usb/android0/f_ffs/aliases adb
-    write /sys/class/android_usb/android0/functions adb
+    write /sys/class/android_usb/android0/functions mtp,adb
     write /sys/class/android_usb/android0/iManufacturer ${ro.product.manufacturer}
     write /sys/class/android_usb/android0/iProduct ${ro.product.model}
     write /sys/class/android_usb/android0/iSerial ${ro.serialno}
@@ -86,7 +86,7 @@ on property:sys.storage.ums_enabled=1
 
 on property:sys.storage.ums_enabled=0
     write /sys/class/android_usb/android0/enable 0
-    write /sys/class/android_usb/android0/functions adb
+    write /sys/class/android_usb/android0/functions ${sys.usb.config}
     write /sys/class/android_usb/android0/enable ${service.adb.root}
 
 on property:sys.usb.config=none
diff --git a/partitionmanager.cpp b/partitionmanager.cpp
index ea61d57d7..5d0fb018e 100644
--- a/partitionmanager.cpp
+++ b/partitionmanager.cpp
@@ -1903,8 +1903,6 @@ bool TWPartitionManager::Enable_MTP(void) {
 	}
 	//Launch MTP Responder
 	LOGINFO("Starting MTP\n");
-	char vendor[PROPERTY_VALUE_MAX];
-	char product[PROPERTY_VALUE_MAX];
 	int count = 0;
 
 	int mtppipe[2];
@@ -1914,14 +1912,20 @@ bool TWPartitionManager::Enable_MTP(void) {
 		return false;
 	}
 
-	property_set("sys.usb.config", "none");
-	property_get("usb.vendor", vendor, "18D1");
-	property_get("usb.product.mtpadb", product, "4EE2");
-	string vendorstr = vendor;
-	string productstr = product;
-	TWFunc::write_file("/sys/class/android_usb/android0/idVendor", vendorstr);
-	TWFunc::write_file("/sys/class/android_usb/android0/idProduct", productstr);
-	property_set("sys.usb.config", "mtp,adb");
+	char old_value[PROPERTY_VALUE_MAX];
+	property_get("sys.usb.config", old_value, "error");
+	if (strcmp(old_value, "error") != 0 && strcmp(old_value, "mtp,adb") != 0) {
+		char vendor[PROPERTY_VALUE_MAX];
+		char product[PROPERTY_VALUE_MAX];
+		property_set("sys.usb.config", "none");
+		property_get("usb.vendor", vendor, "18D1");
+		property_get("usb.product.mtpadb", product, "4EE2");
+		string vendorstr = vendor;
+		string productstr = product;
+		TWFunc::write_file("/sys/class/android_usb/android0/idVendor", vendorstr);
+		TWFunc::write_file("/sys/class/android_usb/android0/idProduct", productstr);
+		property_set("sys.usb.config", "mtp,adb");
+	}
 	std::vector<TWPartition*>::iterator iter;
 	/* To enable MTP debug, use the twrp command line feature to
 	 * twrp set tw_mtp_debug 1
@@ -1960,17 +1964,21 @@ bool TWPartitionManager::Enable_MTP(void) {
 }
 
 bool TWPartitionManager::Disable_MTP(void) {
+	char old_value[PROPERTY_VALUE_MAX];
+	property_get("sys.usb.config", old_value, "error");
+	if (strcmp(old_value, "adb") != 0) {
+		char vendor[PROPERTY_VALUE_MAX];
+		char product[PROPERTY_VALUE_MAX];
+		property_set("sys.usb.config", "none");
+		property_get("usb.vendor", vendor, "18D1");
+		property_get("usb.product.adb", product, "D002");
+		string vendorstr = vendor;
+		string productstr = product;
+		TWFunc::write_file("/sys/class/android_usb/android0/idVendor", vendorstr);
+		TWFunc::write_file("/sys/class/android_usb/android0/idProduct", productstr);
+		usleep(2000);
+	}
 #ifdef TW_HAS_MTP
-	char vendor[PROPERTY_VALUE_MAX];
-	char product[PROPERTY_VALUE_MAX];
-	property_set("sys.usb.config", "none");
-	property_get("usb.vendor", vendor, "18D1");
-	property_get("usb.product.adb", product, "D002");
-	string vendorstr = vendor;
-	string productstr = product;
-	TWFunc::write_file("/sys/class/android_usb/android0/idVendor", vendorstr);
-	TWFunc::write_file("/sys/class/android_usb/android0/idProduct", productstr);
-	usleep(2000);
 	if (mtppid) {
 		LOGINFO("Disabling MTP\n");
 		int status;
@@ -1981,14 +1989,13 @@ bool TWPartitionManager::Disable_MTP(void) {
 		close(mtp_write_fd);
 		mtp_write_fd = -1;
 	}
+#endif
 	property_set("sys.usb.config", "adb");
+#ifdef TW_HAS_MTP
 	DataManager::SetValue("tw_mtp_enabled", 0);
 	return true;
-#else
-	LOGERR("MTP support not included\n");
-	DataManager::SetValue("tw_mtp_enabled", 0);
-	return false;
 #endif
+	return false;
 }
 
 TWPartition* TWPartitionManager::Find_Partition_By_MTP_Storage_ID(unsigned int Storage_ID) {
diff --git a/twrp.cpp b/twrp.cpp
index 53aab7f66..e0b335975 100644
--- a/twrp.cpp
+++ b/twrp.cpp
@@ -311,26 +311,23 @@ int main(int argc, char **argv) {
 	property_get("mtp.crash_check", mtp_crash_check, "0");
 	if (strcmp(mtp_crash_check, "0") == 0) {
 		property_set("mtp.crash_check", "1");
-		if (DataManager::GetIntValue(TW_IS_ENCRYPTED) != 0) {
-			if (DataManager::GetIntValue(TW_IS_DECRYPTED) != 0 && DataManager::GetIntValue("tw_mtp_enabled") == 1) {
-				LOGINFO("Enabling MTP during startup\n");
-				if (!PartitionManager.Enable_MTP())
-					PartitionManager.Disable_MTP();
-				else
-					gui_print("MTP Enabled\n");
-			}
-		} else if (DataManager::GetIntValue("tw_mtp_enabled") == 1) {
+		if (DataManager::GetIntValue("tw_mtp_enabled") == 1 && ((DataManager::GetIntValue(TW_IS_ENCRYPTED) != 0 && DataManager::GetIntValue(TW_IS_DECRYPTED) != 0) || DataManager::GetIntValue(TW_IS_ENCRYPTED) == 0)) {
 			LOGINFO("Enabling MTP during startup\n");
 			if (!PartitionManager.Enable_MTP())
 				PartitionManager.Disable_MTP();
 			else
 				gui_print("MTP Enabled\n");
+		} else {
+			PartitionManager.Disable_MTP();
 		}
 		property_set("mtp.crash_check", "0");
 	} else {
 		gui_print_color("warning", "MTP Crashed, not starting MTP on boot.\n");
 		DataManager::SetValue("tw_mtp_enabled", 0);
+		PartitionManager.Disable_MTP();
 	}
+#else
+	PartitionManager.Disable_MTP();
 #endif
 
 	// Launch the main GUI