[PATCH] USB storage: Enhance sddr09 to work with 64 MB SmartMedia cards

This patch was written by Andries Brouwer.  It adds to sddr09 the ability
to use 64 MB SmartMedia cards.  I have added a few minor alterations to
make it fit in with my sequence of other patches.

drivers/usb/storage/sddr09.c

@@ -66,7 +66,7 @@ struct nand_flash_dev {
  * NAND Flash Manufacturer ID Codes
  */
 #define NAND_MFR_AMD		0x01
-#define NAND_MFR_NS		0x8f
+#define NAND_MFR_NATSEMI	0x8f
 #define NAND_MFR_TOSHIBA	0x98
 #define NAND_MFR_SAMSUNG	0xec
 
@@ -74,8 +74,8 @@ static inline char *nand_flash_manufacturer(int manuf_id) {
 	switch(manuf_id) {
 	case NAND_MFR_AMD:
 		return "AMD";
-	case NAND_MFR_NS:
-		return "NS";
+	case NAND_MFR_NATSEMI:
+		return "NATSEMI";
 	case NAND_MFR_TOSHIBA:
 		return "Toshiba";
 	case NAND_MFR_SAMSUNG:
@@ -302,8 +302,7 @@ sddr09_request_sense(struct us_data *us, unsigned char *sensebuf, int buflen) {
 	if (result != USB_STOR_XFER_GOOD) {
 		US_DEBUGP("request sense bulk in failed\n");
 		return USB_STOR_TRANSPORT_ERROR;
-	}
-	else {
+	} else {
 		US_DEBUGP("request sense worked\n");
 		return USB_STOR_TRANSPORT_GOOD;
 	}
@@ -469,6 +468,8 @@ sddr09_erase(struct us_data *us, unsigned long Eaddress) {
 	unsigned char *command = us->iobuf;
 	int result;
 
+	US_DEBUGP("sddr09_erase: erase address %lu\n", Eaddress);
+
 	memset(command, 0, 12);
 	command[0] = 0xEA;
 	command[1] = LUNBITS;
@@ -757,17 +758,27 @@ sddr09_read_data(struct us_data *us,
 	return result;
 }
 
-/* we never free blocks, so lastpba can only increase */
 static unsigned int
-sddr09_find_unused_pba(struct sddr09_card_info *info) {
+sddr09_find_unused_pba(struct sddr09_card_info *info, unsigned int lba) {
 	static unsigned int lastpba = 1;
-	int numblocks = info->capacity >> (info->blockshift + info->pageshift);
-	int i;
+	int zonestart, end, i;
+
+	zonestart = (lba/1000) << 10;
+	end = info->capacity >> (info->blockshift + info->pageshift);
+	end -= zonestart;
+	if (end > 1024)
+		end = 1024;
 
-	for (i = lastpba+1; i < numblocks; i++) {
-		if (info->pba_to_lba[i] == UNDEF) {
+	for (i = lastpba+1; i < end; i++) {
+		if (info->pba_to_lba[zonestart+i] == UNDEF) {
 			lastpba = i;
-			return i;
+			return zonestart+i;
+		}
+	}
+	for (i = 0; i <= lastpba; i++) {
+		if (info->pba_to_lba[zonestart+i] == UNDEF) {
+			lastpba = i;
+			return zonestart+i;
 		}
 	}
 	return 0;
@@ -784,21 +795,23 @@ sddr09_write_lba(struct us_data *us, unsigned int lba,
 	unsigned int pagelen, blocklen;
 	unsigned char *blockbuffer, *bptr, *cptr, *xptr;
 	unsigned char ecc[3];
-	int i, result;
+	int i, result, isnew;
 
-	lbap = ((lba & 0x3ff) << 1) | 0x1000;
+	lbap = ((lba % 1000) << 1) | 0x1000;
 	if (parity[MSB_of(lbap) ^ LSB_of(lbap)])
 		lbap ^= 1;
 	pba = info->lba_to_pba[lba];
+	isnew = 0;
 
 	if (pba == UNDEF) {
-		pba = sddr09_find_unused_pba(info);
+		pba = sddr09_find_unused_pba(info, lba);
 		if (!pba) {
 			printk("sddr09_write_lba: Out of unused blocks\n");
 			return USB_STOR_TRANSPORT_ERROR;
 		}
 		info->pba_to_lba[pba] = lba;
 		info->lba_to_pba[lba] = pba;
+		isnew = 1;
 	}
 
 	if (pba == 1) {
@@ -823,8 +836,8 @@ sddr09_write_lba(struct us_data *us, unsigned int lba,
 	if (result != USB_STOR_TRANSPORT_GOOD)
 		goto err;
 
-	/* check old contents */
-	for (i = 0; i < info->blockshift; i++) {
+	/* check old contents and fill lba */
+	for (i = 0; i < info->blocksize; i++) {
 		bptr = blockbuffer + i*pagelen;
 		cptr = bptr + info->pagesize;
 		nand_compute_ecc(bptr, ecc);
@@ -839,6 +852,8 @@ sddr09_write_lba(struct us_data *us, unsigned int lba,
 				  i, pba);
 			nand_store_ecc(cptr+8, ecc);
 		}
+		cptr[6] = cptr[11] = MSB_of(lbap);
+		cptr[7] = cptr[12] = LSB_of(lbap);
 	}
 
 	/* copy in new stuff and compute ECC */
@@ -852,8 +867,6 @@ sddr09_write_lba(struct us_data *us, unsigned int lba,
 		nand_store_ecc(cptr+13, ecc);
 		nand_compute_ecc(bptr+(info->pagesize / 2), ecc);
 		nand_store_ecc(cptr+8, ecc);
-		cptr[6] = cptr[11] = MSB_of(lbap);
-		cptr[7] = cptr[12] = LSB_of(lbap);
 	}
 
 	US_DEBUGP("Rewrite PBA %d (LBA %d)\n", pba, lba);
@@ -947,10 +960,11 @@ sddr09_read_control(struct us_data *us,
 		unsigned char *content,
 		int use_sg) {
 
-	US_DEBUGP("Read control address %08lX blocks %04X\n",
+	US_DEBUGP("Read control address %lu, blocks %d\n",
 		address, blocks);
 
-	return sddr09_read21(us, address, blocks, CONTROL_SHIFT, content, use_sg);
+	return sddr09_read21(us, address, blocks,
+			     CONTROL_SHIFT, content, use_sg);
 }
 
 /*
@@ -997,7 +1011,7 @@ sddr09_get_wp(struct us_data *us, struct sddr09_card_info *info) {
 		US_DEBUGP("sddr09_get_wp: read_status fails\n");
 		return result;
 	}
-	US_DEBUGP("sddr09_get_wp: status %02X", status);
+	US_DEBUGP("sddr09_get_wp: status 0x%02X", status);
 	if ((status & 0x80) == 0) {
 		info->flags |= SDDR09_WP;	/* write protected */
 		US_DEBUGP(" WP");
@@ -1114,8 +1128,11 @@ sddr09_read_map(struct us_data *us) {
 	alloc_blocks = min(numblocks, SDDR09_READ_MAP_BUFSZ >> CONTROL_SHIFT);
 	alloc_len = (alloc_blocks << CONTROL_SHIFT);
 	buffer = kmalloc(alloc_len, GFP_NOIO);
-	if (buffer == NULL)
-		return 0;
+	if (buffer == NULL) {
+		printk("sddr09_read_map: out of memory\n");
+		result = -1;
+		goto done;
+	}
 	buffer_end = buffer + alloc_len;
 
 #undef SDDR09_READ_MAP_BUFSZ
@@ -1141,15 +1158,18 @@ sddr09_read_map(struct us_data *us) {
 	for (i = 0; i < numblocks; i++) {
 		ptr += (1 << CONTROL_SHIFT);
 		if (ptr >= buffer_end) {
-			ptr = buffer;
+			unsigned long address;
+
+			address = i << (info->pageshift + info->blockshift);
 			result = sddr09_read_control(
-				us, i << (info->blockshift + 8),
+				us, address>>1,
 				min(alloc_blocks, numblocks - i),
 				buffer, 0);
 			if (result != USB_STOR_TRANSPORT_GOOD) {
 				result = -1;
 				goto done;
 			}
+			ptr = buffer;
 		}
 
 		if (i == 0 || i == 1) {
@@ -1162,7 +1182,7 @@ sddr09_read_map(struct us_data *us) {
 			if (ptr[j] != 0)
 				goto nonz;
 		info->pba_to_lba[i] = UNUSABLE;
-		printk("sddr09: PBA %04X has no logical mapping\n", i);
+		printk("sddr09: PBA %d has no logical mapping\n", i);
 		continue;
 
 	nonz:
@@ -1175,7 +1195,7 @@ sddr09_read_map(struct us_data *us) {
 	nonff:
 		/* normal PBAs start with six FFs */
 		if (j < 6) {
-			printk("sddr09: PBA %04X has no logical mapping: "
+			printk("sddr09: PBA %d has no logical mapping: "
 			       "reserved area = %02X%02X%02X%02X "
 			       "data status %02X block status %02X\n",
 			       i, ptr[0], ptr[1], ptr[2], ptr[3],
@@ -1185,7 +1205,7 @@ sddr09_read_map(struct us_data *us) {
 		}
 
 		if ((ptr[6] >> 4) != 0x01) {
-			printk("sddr09: PBA %04X has invalid address field "
+			printk("sddr09: PBA %d has invalid address field "
 			       "%02X%02X/%02X%02X\n",
 			       i, ptr[6], ptr[7], ptr[11], ptr[12]);
 			info->pba_to_lba[i] = UNUSABLE;
@@ -1194,7 +1214,7 @@ sddr09_read_map(struct us_data *us) {
 
 		/* check even parity */
 		if (parity[ptr[6] ^ ptr[7]]) {
-			printk("sddr09: Bad parity in LBA for block %04X"
+			printk("sddr09: Bad parity in LBA for block %d"
 			       " (%02X %02X)\n", i, ptr[6], ptr[7]);
 			info->pba_to_lba[i] = UNUSABLE;
 			continue;
@@ -1213,27 +1233,32 @@ sddr09_read_map(struct us_data *us) {
 		 */
 
 		if (lba >= 1000) {
-			unsigned long address;
-
-			printk("sddr09: Bad LBA %04X for block %04X\n",
+			printk("sddr09: Bad low LBA %d for block %d\n",
 			       lba, i);
-			info->pba_to_lba[i] = UNDEF /* UNUSABLE */;
-			if (erase_bad_lba_entries) {
-				/* some cameras cannot erase a card if it has
-				   bad entries, so we supply this function */
-				address = (i << (info->pageshift + info->blockshift));
-				sddr09_erase(us, address>>1);
-			}
-			continue;
+			goto possibly_erase;
 		}
 
 		lba += 1000*(i/0x400);
 
-		if (lba<0x10 || (lba >= 0x3E0 && lba < 0x3EF))
-			US_DEBUGP("LBA %04X <-> PBA %04X\n", lba, i);
+		if (info->lba_to_pba[lba] != UNDEF) {
+			printk("sddr09: LBA %d seen for PBA %d and %d\n",
+			       lba, info->lba_to_pba[lba], i);
+			goto possibly_erase;
+		}
 
 		info->pba_to_lba[i] = lba;
 		info->lba_to_pba[lba] = i;
+		continue;
+
+	possibly_erase:
+		if (erase_bad_lba_entries) {
+			unsigned long address;
+
+			address = (i << (info->pageshift + info->blockshift));
+			sddr09_erase(us, address>>1);
+			info->pba_to_lba[i] = UNDEF;
+		} else
+			info->pba_to_lba[i] = UNUSABLE;
 	}
 
 	/*
@@ -1410,6 +1435,7 @@ int sddr09_transport(Scsi_Cmnd *srb, struct us_data *us)
 		cardinfo = sddr09_get_cardinfo(us, info->flags);
 		if (!cardinfo) {
 			/* probably no media */
+		init_error:
 			sensekey = 0x02;	/* not ready */
 			sensecode = 0x3a;	/* medium not present */
 			return USB_STOR_TRANSPORT_FAILED;
@@ -1423,7 +1449,10 @@ int sddr09_transport(Scsi_Cmnd *srb, struct us_data *us)
 		info->blockmask = info->blocksize - 1;
 
 		// map initialization, must follow get_cardinfo()
-		sddr09_read_map(us);
+		if (sddr09_read_map(us)) {
+			/* probably out of memory */
+			goto init_error;
+		}
 
 		// Report capacity
 
@@ -1444,12 +1473,13 @@ int sddr09_transport(Scsi_Cmnd *srb, struct us_data *us)
 		return USB_STOR_TRANSPORT_GOOD;
 	}
 
-	if (srb->cmnd[0] == MODE_SENSE) {
+	if (srb->cmnd[0] == MODE_SENSE || srb->cmnd[0] == MODE_SENSE_10) {
 		int modepage = (srb->cmnd[2] & 0x3F);
 		int len;
 
 		/* They ask for the Read/Write error recovery page,
 		   or for all pages. Give as much as they have room for. */
+		/* %% We should check DBD %% */
 		if (modepage == 0x01 || modepage == 0x3F) {
 
 			US_DEBUGP("SDDR09: Dummy up request for "
@@ -1473,17 +1503,9 @@ int sddr09_transport(Scsi_Cmnd *srb, struct us_data *us)
 		return USB_STOR_TRANSPORT_FAILED;
 	}
 
-	if (srb->cmnd[0] == ALLOW_MEDIUM_REMOVAL) {
-
-		US_DEBUGP(
-			"SDDR09: %s medium removal. Not that I can do"
-			" anything about it...\n",
-			(srb->cmnd[4]&0x03) ? "Prevent" : "Allow");
-
+	if (srb->cmnd[0] == ALLOW_MEDIUM_REMOVAL)
 		return USB_STOR_TRANSPORT_GOOD;
 
-	}
-
 	havefakesense = 0;
 
 	if (srb->cmnd[0] == READ_10) {
@@ -1531,8 +1553,7 @@ int sddr09_transport(Scsi_Cmnd *srb, struct us_data *us)
 	for (i=0; i<12; i++)
 		sprintf(string+strlen(string), "%02X ", srb->cmnd[i]);
 
-	US_DEBUGP("SDDR09: Send control for command %s\n",
-		  string);
+	US_DEBUGP("SDDR09: Send control for command %s\n", string);
 
 	result = sddr09_send_scsi_command(us, srb->cmnd, 12);
 	if (result != USB_STOR_TRANSPORT_GOOD) {