diff --git a/mtp/MtpDatabase.h b/mtp/MtpDatabase.h
index c25e9b2..a0ff8da 100755
--- a/mtp/MtpDatabase.h
+++ b/mtp/MtpDatabase.h
@@ -61,6 +61,7 @@
     virtual MtpDevicePropertyList*  getSupportedDeviceProperties() = 0;
 
 	virtual void 					createDB(MtpStorage* storage, MtpStorageID storageID) = 0;
+	virtual void 					destroyDB(MtpStorageID storageID) = 0;
 
     virtual MtpResponseCode         getObjectPropertyValue(MtpObjectHandle handle,
                                             MtpObjectProperty property,
diff --git a/mtp/MtpMessage.hpp b/mtp/MtpMessage.hpp
new file mode 100644
index 0000000..272da17
--- /dev/null
+++ b/mtp/MtpMessage.hpp
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2010 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *	  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * Copyright (C) 2014 TeamWin - bigbiff and Dees_Troy mtp database conversion to C++
+ */
+
+#ifndef _MTPMESSAGE_HPP
+#define _MTPMESSAGE_HPP
+
+#define MTP_MESSAGE_ADD_STORAGE    1
+#define MTP_MESSAGE_REMOVE_STORAGE 2
+
+struct mtpmsg {
+	int message_type; // 1 is add, 2 is remove, see above
+	unsigned int storage_id;
+	const char* display;
+	const char* path;
+	uint64_t maxFileSize;
+};
+
+#endif //_MTPMESSAGE_HPP
diff --git a/mtp/MtpServer.cpp b/mtp/MtpServer.cpp
index f4af2b9..f99554b 100755
--- a/mtp/MtpServer.cpp
+++ b/mtp/MtpServer.cpp
@@ -116,9 +116,13 @@
 }
 
 void MtpServer::addStorage(MtpStorage* storage) {
-	MTPD("addStorage(): storage: %x\n", storage);
-	mDatabase->createDB(storage, storage->getStorageID());
 	android::Mutex::Autolock autoLock(mMutex);
+	MTPD("addStorage(): storage: %x\n", storage);
+	if (getStorage(storage->getStorageID()) != NULL) {
+		MTPE("MtpServer::addStorage Storage for storage ID %i already exists.\n", storage->getStorageID());
+		return;
+	}
+	mDatabase->createDB(storage, storage->getStorageID());
 	mStorages.push(storage);
 	sendStoreAdded(storage->getStorageID());
 }
@@ -128,11 +132,31 @@
 
 	for (size_t i = 0; i < mStorages.size(); i++) {
 		if (mStorages[i] == storage) {
+			MTPD("MtpServer::removeStorage calling sendStoreRemoved\n");
+			// First lock the mutex so that the inotify thread and main
+			// thread do not do anything while we remove the storage
+			// item, and to make sure we don't remove the item while an
+			// operation is in progress
+			mDatabase->lockMutex();
+			// Grab the storage ID before we delete the item from the
+			// database
+			MtpStorageID storageID = storage->getStorageID();
+			// Remove the item from the mStorages from the vector. At
+			// this point the main thread will no longer be able to find
+			// this storage item anymore.
 			mStorages.removeAt(i);
-			sendStoreRemoved(storage->getStorageID());
+			// Destroy the storage item, free up all the memory, kill
+			// the inotify thread.
+			mDatabase->destroyDB(storageID);
+			// Tell the host OS that the storage item is gone.
+			sendStoreRemoved(storageID);
+			// Unlock any remaining mutexes on other storage devices.
+			// If no storage devices exist anymore this will do nothing.
+			mDatabase->unlockMutex();
 			break;
 		}
 	}
+	MTPD("MtpServer::removeStorage DONE\n");
 }
 
 MtpStorage* MtpServer::getStorage(MtpStorageID id) {
@@ -275,6 +299,7 @@
 void MtpServer::sendStoreRemoved(MtpStorageID id) {
 	MTPD("sendStoreRemoved %08X\n", id);
 	sendEvent(MTP_EVENT_STORE_REMOVED, id);
+	MTPD("MtpServer::sendStoreRemoved done\n");
 }
 
 void MtpServer::sendEvent(MtpEventCode code, uint32_t param1) {
diff --git a/mtp/MtpStorage.cpp b/mtp/MtpStorage.cpp
index 319be09..ab4f8e0 100755
--- a/mtp/MtpStorage.cpp
+++ b/mtp/MtpStorage.cpp
@@ -36,6 +36,7 @@
 #include <signal.h>
 #include <sys/inotify.h>
 #include <fcntl.h>
+#include "tw_sys_atomics.h"
 
 #define WATCH_FLAGS ( IN_CREATE | IN_DELETE | IN_MOVE | IN_MODIFY )
 
@@ -54,6 +55,8 @@
 	MTPI("MtpStorage id: %d path: %s\n", id, filePath);
 	inotify_thread = 0;
 	inotify_fd = -1;
+	// Threading has not started yet so we should be safe to set these directly instead of using atomics
+	inotify_thread_kill = 0;
 	sendEvents = false;
 	handleCurrentlySending = 0;
 	use_mutex = true;
@@ -63,25 +66,32 @@
 	}
 	if (pthread_mutex_init(&inMutex, NULL) != 0) {
 		MTPE("Failed to init inMutex\n");
+		pthread_mutex_destroy(&mtpMutex);
 		use_mutex = false;
 	}
-	
 }
 
 MtpStorage::~MtpStorage() {
 	if (inotify_thread) {
-		// TODO: what does this do? manpage says it does not kill the thread
-		pthread_kill(inotify_thread, 0);
+		__tw_atomic_cmpxchg(0, 1, &inotify_thread_kill);
+		//inotify_thread_kill = 1;
+		MTPD("joining inotify_thread after sending the kill notification.\n");
+		pthread_join(inotify_thread, NULL); // There's not much we can do if there's an error here
+		inotify_thread = 0;
+		MTPD("~MtpStorage removing inotify watches and closing inotify_fd\n");
 		for (std::map<int, Tree*>::iterator i = inotifymap.begin(); i != inotifymap.end(); i++) {
 			inotify_rm_watch(inotify_fd, i->first);
 		}
 		close(inotify_fd);
+		inotifymap.clear();
 	}
-	for (iter i = mtpmap.begin(); i != mtpmap.end(); i++) {
-		delete i->second;
-	}
+	// Deleting the root tree causes a cascade in btree.cpp that ends up
+	// deleting all of the trees and nodes.
+	delete mtpmap[0];
+	mtpmap.clear();
 	if (use_mutex) {
 		use_mutex = false;
+		MTPD("~MtpStorage destroying mutexes\n");
 		pthread_mutex_destroy(&mtpMutex);
 		pthread_mutex_destroy(&inMutex);
 	}
@@ -566,9 +576,22 @@
 
 pthread_t MtpStorage::inotify(void) {
 	pthread_t thread;
+	pthread_attr_t tattr;
+
+	if (pthread_attr_init(&tattr)) {
+		MTPE("Unable to pthread_attr_init\n");
+		return 0;
+	}
+	if (pthread_attr_setdetachstate(&tattr, PTHREAD_CREATE_JOINABLE)) {
+		MTPE("Error setting pthread_attr_setdetachstate\n");
+		return 0;
+	}
 	ThreadPtr inotifyptr = &MtpStorage::inotify_t;
 	PThreadPtr p = *(PThreadPtr*)&inotifyptr;
-	pthread_create(&thread, NULL, p, this);
+	pthread_create(&thread, &tattr, p, this);
+	if (pthread_attr_destroy(&tattr)) {
+		MTPE("Failed to pthread_attr_destroy\n");
+	}
 	return thread;
 }
 
@@ -669,10 +692,20 @@
 	#define EVENT_SIZE ( sizeof(struct inotify_event) )
 	#define EVENT_BUF_LEN ( 1024 * ( EVENT_SIZE + 16) )
 	char buf[EVENT_BUF_LEN];
+	fd_set fdset;
+	struct timeval seltmout;
+	int sel_ret;
 
 	MTPD("inotify thread starting.\n");
 
-	while (true) {
+	while (__tw_atomic_cmpxchg(0, inotify_thread_kill, &inotify_thread_kill) == 0) {
+		FD_ZERO(&fdset);
+		FD_SET(inotify_fd, &fdset);
+		seltmout.tv_sec = 0;
+		seltmout.tv_usec = 25000;
+		sel_ret = select(inotify_fd + 1, &fdset, NULL, NULL, &seltmout);
+		if (sel_ret == 0)
+			continue;
 		int i = 0;
 		int len = read(inotify_fd, buf, EVENT_BUF_LEN);
 
@@ -682,7 +715,7 @@
 			MTPE("inotify_t Can't read inotify events\n");
 		}
 
-		while (i < len) {
+		while (i < len && __tw_atomic_cmpxchg(0, inotify_thread_kill, &inotify_thread_kill) == 0) {
 			struct inotify_event *event = (struct inotify_event *) &buf[i];
 			if (event->len) {
 				MTPD("inotify event: wd: %i, mask: %x, name: %s\n", event->wd, event->mask, event->name);
@@ -693,11 +726,12 @@
 			i += EVENT_SIZE + event->len;
 		}
 	}
-
-	for (std::map<int, Tree*>::iterator i = inotifymap.begin(); i != inotifymap.end(); i++) {
+	MTPD("inotify_thread_kill received!\n");
+	// This cleanup is handled in the destructor.
+	/*for (std::map<int, Tree*>::iterator i = inotifymap.begin(); i != inotifymap.end(); i++) {
 		inotify_rm_watch(inotify_fd, i->first);
 	}
-	close(inotify_fd);
+	close(inotify_fd);*/
 	return 0;
 }
 
diff --git a/mtp/MtpStorage.h b/mtp/MtpStorage.h
index 418e3db..cdbb73b 100755
--- a/mtp/MtpStorage.h
+++ b/mtp/MtpStorage.h
@@ -113,6 +113,7 @@
 	bool use_mutex;
 	pthread_mutex_t inMutex; // inotify mutex
 	pthread_mutex_t mtpMutex; // main mtp mutex
+	int inotify_thread_kill;
 };
 
 #endif // _MTP_STORAGE_H
diff --git a/mtp/btree.cpp b/mtp/btree.cpp
index 3a5648d..e53afab 100755
--- a/mtp/btree.cpp
+++ b/mtp/btree.cpp
@@ -28,6 +28,7 @@
 Tree::~Tree() {
 	for (std::map<MtpObjectHandle, Node*>::iterator it = entries.begin(); it != entries.end(); ++it)
 		delete it->second;
+	entries.clear();
 }
 
 int Tree::getCount(void) {
diff --git a/mtp/mtp_MtpDatabase.cpp b/mtp/mtp_MtpDatabase.cpp
index 05bb5d9..17053f1 100755
--- a/mtp/mtp_MtpDatabase.cpp
+++ b/mtp/mtp_MtpDatabase.cpp
@@ -233,10 +233,17 @@
 }
 
 void MyMtpDatabase::createDB(MtpStorage* storage, MtpStorageID storageID) {
+	MTPD("MyMtpDatabase::createDB called\n");
 	storagemap[storageID] = storage;
 	storage->createDB();
 }
 
+void MyMtpDatabase::destroyDB(MtpStorageID storageID) {
+	MtpStorage* storage = storagemap[storageID];
+	storagemap.erase(storageID);
+	delete storage;
+}
+
 MtpObjectHandleList* MyMtpDatabase::getObjectList(MtpStorageID storageID,
 									MtpObjectFormat format,
 									MtpObjectHandle parent) {
diff --git a/mtp/mtp_MtpDatabase.hpp b/mtp/mtp_MtpDatabase.hpp
index cc8097b..49e5913 100755
--- a/mtp/mtp_MtpDatabase.hpp
+++ b/mtp/mtp_MtpDatabase.hpp
@@ -64,6 +64,7 @@
     virtual                         ~MyMtpDatabase();
 
 	void					createDB(MtpStorage* storage, MtpStorageID storageID);
+	void					destroyDB(MtpStorageID storageID);
     virtual MtpObjectHandle         beginSendObject(const char* path,
                                             MtpObjectFormat format,
                                             MtpObjectHandle parent,
diff --git a/mtp/mtp_MtpServer.cpp b/mtp/mtp_MtpServer.cpp
index 17facdd..f49270f 100755
--- a/mtp/mtp_MtpServer.cpp
+++ b/mtp/mtp_MtpServer.cpp
@@ -25,11 +25,13 @@
 #include <fcntl.h>
 #include <vector>
 #include <utils/threads.h>
+#include <pthread.h>
 
 #include "mtp_MtpServer.hpp"
 #include "MtpServer.h"
 #include "MtpStorage.h"
 #include "MtpDebug.h"
+#include "MtpMessage.hpp"
 
 #include <string>
 
@@ -37,6 +39,11 @@
 {
 	if (setup() == 0) {
 		add_storage();
+		MTPD("Starting add / remove mtppipe monitor thread\n");
+		pthread_t thread;
+		ThreadPtr mtpptr = &twmtp_MtpServer::mtppipe_thread;
+		PThreadPtr p = *(PThreadPtr*)&mtpptr;
+		pthread_create(&thread, NULL, p, this);
 		server->run();
 	}
 }
@@ -140,9 +147,52 @@
 	if (server) {
 		MtpStorage* storage = server->getStorage(storageId);
 		if (storage) {
+			MTPD("twmtp_MtpServer::remove_storage calling removeStorage\n");
 			server->removeStorage(storage);
-			delete storage;
 		}
 	} else
 		MTPD("server is null in remove_storage");
+	MTPD("twmtp_MtpServer::remove_storage DONE\n");
+}
+
+int twmtp_MtpServer::mtppipe_thread(void)
+{
+	if (mtp_read_pipe == -1) {
+		MTPD("mtppipe_thread exiting because mtp_read_pipe not set\n");
+		return 0;
+	}
+	MTPD("Starting twmtp_MtpServer::mtppipe_thread\n");
+	int read_count;
+	struct mtpmsg mtp_message;
+	while (1) {
+		read_count = ::read(mtp_read_pipe, &mtp_message, sizeof(mtp_message));
+		MTPD("read %i from mtppipe\n", read_count);
+		if (read_count == sizeof(mtp_message)) {
+			if (mtp_message.message_type == MTP_MESSAGE_ADD_STORAGE) {
+				MTPI("mtppipe add storage %i '%s'\n", mtp_message.storage_id, mtp_message.path);
+				long reserveSpace = 1;
+				bool removable = false;
+				MtpStorage* storage = new MtpStorage(mtp_message.storage_id, mtp_message.path, mtp_message.display, reserveSpace, removable, mtp_message.maxFileSize, refserver);
+				server->addStorage(storage);
+				MTPD("mtppipe done adding storage\n");
+			} else if (mtp_message.message_type == MTP_MESSAGE_REMOVE_STORAGE) {
+				MTPI("mtppipe remove storage %i\n", mtp_message.storage_id);
+				remove_storage(mtp_message.storage_id);
+				MTPD("mtppipe done removing storage\n");
+			} else {
+				MTPE("Unknown mtppipe message value: %i\n", mtp_message.message_type);
+			}
+		} else {
+			MTPE("twmtp_MtpServer::mtppipe_thread unexpected read_count %i\n", read_count);
+			close(mtp_read_pipe);
+			break;
+		}
+	}
+	MTPD("twmtp_MtpServer::mtppipe_thread closing\n");
+	return 0;
+}
+
+void twmtp_MtpServer::set_read_pipe(int pipe)
+{
+	mtp_read_pipe = pipe;
 }
diff --git a/mtp/mtp_MtpServer.hpp b/mtp/mtp_MtpServer.hpp
index 3153e80..622ed6d 100755
--- a/mtp/mtp_MtpServer.hpp
+++ b/mtp/mtp_MtpServer.hpp
@@ -52,11 +52,16 @@
 		void add_storage();
 		void remove_storage(int storageId);
 		void set_storages(storages* mtpstorages);
+		void set_read_pipe(int pipe);
 		storages *stores;
 	private:
+		typedef int (twmtp_MtpServer::*ThreadPtr)(void);
+		typedef void* (*PThreadPtr)(void *);
+		int mtppipe_thread(void);
 		bool usePtp;
 		MtpServer* server;
 		MtpServer* refserver;
+		int mtp_read_pipe;
 
 };
 #endif
diff --git a/mtp/tw_sys_atomics.h b/mtp/tw_sys_atomics.h
new file mode 100644
index 0000000..6349a93
--- /dev/null
+++ b/mtp/tw_sys_atomics.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright (C) 2008 The Android Open Source Project
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
+ * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+#ifndef _TW_SYS_ATOMICS_H
+#define _TW_SYS_ATOMICS_H
+
+#include <sys/cdefs.h>
+#include <sys/time.h>
+
+__BEGIN_DECLS
+
+/* Note: atomic operations that were exported by the C library didn't
+ *       provide any memory barriers, which created potential issues on
+ *       multi-core devices. We now define them as inlined calls to
+ *       GCC sync builtins, which always provide a full barrier.
+ *
+ *       NOTE: The C library still exports atomic functions by the same
+ *              name to ensure ABI stability for existing NDK machine code.
+ *
+ *       If you are an NDK developer, we encourage you to rebuild your
+ *       unmodified sources against this header as soon as possible.
+ */
+
+/* This was kanged from Android 4.4 bionic/libc/include/sys/atomics.h
+ * This header was removed in Android 5.0 in favor of stdatomics.h but
+ * to maintain compatibility across multiple trees, we are including our
+ * own copy.
+ */
+
+#ifndef __ATOMIC_INLINE__
+#define __ATOMIC_INLINE__ static __inline__ __attribute__((always_inline))
+#endif
+
+__ATOMIC_INLINE__ int
+__tw_atomic_cmpxchg(int old_value, int new_value, volatile int* ptr)
+{
+    /* We must return 0 on success */
+    return __sync_val_compare_and_swap(ptr, old_value, new_value) != old_value;
+}
+
+__END_DECLS
+
+#endif /* _TW_SYS_ATOMICS_H */
diff --git a/mtp/twrpMtp.cpp b/mtp/twrpMtp.cpp
index d9db424..d47b8fa 100755
--- a/mtp/twrpMtp.cpp
+++ b/mtp/twrpMtp.cpp
@@ -72,12 +72,14 @@
 	if (debug_enabled)
 		MtpDebug::enableDebug();
 	mtpstorages = new storages;
+	mtp_read_pipe = -1;
 }
 
 int twrpMtp::start(void) {
 	MTPI("Starting MTP\n");
 	twmtp_MtpServer *mtp = new twmtp_MtpServer();
 	mtp->set_storages(mtpstorages);
+	mtp->set_read_pipe(mtp_read_pipe);
 	mtp->start();
 	return 0;
 }
@@ -90,7 +92,7 @@
 	return thread;
 }
 
-pid_t twrpMtp::forkserver(void) {
+pid_t twrpMtp::forkserver(int mtppipe[2]) {
 	pid_t pid;
 	if ((pid = fork()) == -1) {
 		MTPE("MTP fork failed.\n");
@@ -98,8 +100,11 @@
 	}
 	if (pid == 0) {
 		// Child process
+		close(mtppipe[1]); // Child closes write side
+		mtp_read_pipe = mtppipe[0];
 		start();
 		MTPD("MTP child process exited.\n");
+		close(mtppipe[0]);
 		_exit(0);
 	} else {
 		return pid;
diff --git a/mtp/twrpMtp.hpp b/mtp/twrpMtp.hpp
index 3aaa964..ec7cd4b 100755
--- a/mtp/twrpMtp.hpp
+++ b/mtp/twrpMtp.hpp
@@ -35,7 +35,7 @@
 	public:
 		twrpMtp(int debug_enabled /* = 0 */);
 		pthread_t threadserver(void);
-		pid_t forkserver(void);
+		pid_t forkserver(int mtppipe[2]);
 		void addStorage(std::string display, std::string path, int mtpid, uint64_t maxFileSize);
 	private:
 		int start(void);
@@ -43,5 +43,6 @@
 		typedef void* (*PThreadPtr)(void *);
 		storages *mtpstorages;
 		storage *s;
+		int mtp_read_pipe;
 };
 #endif