2 files changed, 62 insertions, 71 deletions

diff --git a/drivers/misc/tifm_7xx1.c b/drivers/misc/tifm_7xx1.c
index e051f9d..3563869 100644
--- a/drivers/misc/tifm_7xx1.c
+++ b/drivers/misc/tifm_7xx1.c
@@ -136,7 +136,6 @@ static void tifm_7xx1_switch_media(struct work_struct *work)
 					       media_switcher);
 	unsigned long flags;
 	unsigned char media_id;
-	char *card_name = "xx";
 	int cnt;
 	struct tifm_dev *sock;
 	unsigned int socket_change_set;
@@ -153,68 +152,45 @@ static void tifm_7xx1_switch_media(struct work_struct *work)
 		return;
 	}
 
-		for (cnt = 0; cnt < fm->num_sockets; cnt++) {
-			if (!(socket_change_set & (1 << cnt)))
-				continue;
-			sock = fm->sockets[cnt];
-			if (sock) {
-				printk(KERN_INFO DRIVER_NAME
-				       ": demand removing card from socket %d\n",
-				       cnt);
-				fm->sockets[cnt] = NULL;
-				spin_unlock_irqrestore(&fm->lock, flags);
-				device_unregister(&sock->dev);
-				spin_lock_irqsave(&fm->lock, flags);
-				writel(0x0e00,
-				       tifm_7xx1_sock_addr(fm->addr, cnt)
-				       + SOCK_CONTROL);
-			}
-
+	for (cnt = 0; cnt < fm->num_sockets; cnt++) {
+		if (!(socket_change_set & (1 << cnt)))
+			continue;
+		sock = fm->sockets[cnt];
+		if (sock) {
+			printk(KERN_INFO
+			       "%s : demand removing card from socket %u:%u\n",
+			       fm->cdev.class_id, fm->id, cnt);
+			fm->sockets[cnt] = NULL;
 			spin_unlock_irqrestore(&fm->lock, flags);
-			media_id = tifm_7xx1_toggle_sock_power(
-					tifm_7xx1_sock_addr(fm->addr, cnt));
-			if (media_id) {
-				sock = tifm_alloc_device(fm);
-				if (sock) {
-					sock->addr = tifm_7xx1_sock_addr(fm->addr,
-									 cnt);
-					sock->type = media_id;
-					sock->socket_id = cnt;
-					switch (media_id) {
-					case 1:
-						card_name = "xd";
-						break;
-					case 2:
-						card_name = "ms";
-						break;
-					case 3:
-						card_name = "sd";
-						break;
-					default:
-						tifm_free_device(&sock->dev);
-						spin_lock_irqsave(&fm->lock, flags);
-						continue;
-					}
-					snprintf(sock->dev.bus_id, BUS_ID_SIZE,
-						 "tifm_%s%u:%u", card_name,
-						 fm->id, cnt);
-					printk(KERN_INFO DRIVER_NAME
-					       ": %s card detected in socket %d\n",
-					       card_name, cnt);
-					if (!device_register(&sock->dev)) {
-						spin_lock_irqsave(&fm->lock, flags);
-						if (!fm->sockets[cnt]) {
-							fm->sockets[cnt] = sock;
-							sock = NULL;
-						}
-						spin_unlock_irqrestore(&fm->lock, flags);
-					}
-					if (sock)
-						tifm_free_device(&sock->dev);
-				}
+			device_unregister(&sock->dev);
+			spin_lock_irqsave(&fm->lock, flags);
+			writel(0x0e00, tifm_7xx1_sock_addr(fm->addr, cnt)
+			       + SOCK_CONTROL);
+		}
+
+		spin_unlock_irqrestore(&fm->lock, flags);
+
+		media_id = tifm_7xx1_toggle_sock_power(
+				tifm_7xx1_sock_addr(fm->addr, cnt));
+
+		// tifm_alloc_device will check if media_id is valid
+		sock = tifm_alloc_device(fm, cnt, media_id);
+		if (sock) {
+			sock->addr = tifm_7xx1_sock_addr(fm->addr, cnt);
+
+			if (!device_register(&sock->dev)) {
 				spin_lock_irqsave(&fm->lock, flags);
+				if (!fm->sockets[cnt]) {
+					fm->sockets[cnt] = sock;
+					sock = NULL;
+				}
+				spin_unlock_irqrestore(&fm->lock, flags);
 			}
+			if (sock)
+				tifm_free_device(&sock->dev);
 		}
+		spin_lock_irqsave(&fm->lock, flags);
+	}
 
 	writel(TIFM_IRQ_FIFOMASK(socket_change_set)
 	       | TIFM_IRQ_CARDMASK(socket_change_set),
diff --git a/drivers/misc/tifm_core.c b/drivers/misc/tifm_core.c
index f0cce2a..1e59198 100644
--- a/drivers/misc/tifm_core.c
+++ b/drivers/misc/tifm_core.c
@@ -232,25 +232,40 @@ EXPORT_SYMBOL(tifm_free_adapter);
 
 void tifm_free_device(struct device *dev)
 {
-	struct tifm_dev *fm_dev = container_of(dev, struct tifm_dev, dev);
-	kfree(fm_dev);
+	struct tifm_dev *sock = container_of(dev, struct tifm_dev, dev);
+	kfree(sock);
 }
 EXPORT_SYMBOL(tifm_free_device);
 
-struct tifm_dev *tifm_alloc_device(struct tifm_adapter *fm)
+struct tifm_dev *tifm_alloc_device(struct tifm_adapter *fm, unsigned int id,
+				   unsigned char type)
 {
-	struct tifm_dev *dev = kzalloc(sizeof(struct tifm_dev), GFP_KERNEL);
+	struct tifm_dev *sock = NULL;
+
+	if (!tifm_media_type_name(type, 0))
+		return sock;
 
-	if (dev) {
-		spin_lock_init(&dev->lock);
+	sock = kzalloc(sizeof(struct tifm_dev), GFP_KERNEL);
+	if (sock) {
+		spin_lock_init(&sock->lock);
+		sock->type = type;
+		sock->socket_id = id;
+		sock->card_event = tifm_dummy_event;
+		sock->data_event = tifm_dummy_event;
 
-		dev->dev.parent = fm->cdev.dev;
-		dev->dev.bus = &tifm_bus_type;
-		dev->dev.release = tifm_free_device;
-		dev->card_event = tifm_dummy_event;
-		dev->data_event = tifm_dummy_event;
+		sock->dev.parent = fm->cdev.dev;
+		sock->dev.bus = &tifm_bus_type;
+		sock->dev.dma_mask = fm->cdev.dev->dma_mask;
+		sock->dev.release = tifm_free_device;
+
+		snprintf(sock->dev.bus_id, BUS_ID_SIZE,
+			 "tifm_%s%u:%u", tifm_media_type_name(type, 2),
+			 fm->id, id);
+		printk(KERN_INFO DRIVER_NAME
+		       ": %s card detected in socket %u:%u\n",
+		       tifm_media_type_name(type, 0), fm->id, id);
 	}
-	return dev;
+	return sock;
 }
 EXPORT_SYMBOL(tifm_alloc_device);