1 files changed, 107 insertions, 9 deletions

diff --git a/drivers/usb/storage/realtek_cr.c b/drivers/usb/storage/realtek_cr.c
index 6fd13068481b..0ce5f79197e7 100644
--- a/drivers/usb/storage/realtek_cr.c
+++ b/drivers/usb/storage/realtek_cr.c
@@ -292,6 +292,52 @@ static int rts51x_bulk_transport(struct us_data *us, u8 lun,
         return USB_STOR_TRANSPORT_ERROR;
 }
 
+static int rts51x_bulk_transport_special(struct us_data *us, u8 lun,
+                                 u8 *cmd, int cmd_len, u8 *buf, int buf_len,
+                                 enum dma_data_direction dir, int *act_len)
+{
+        struct bulk_cb_wrap *bcb = (struct bulk_cb_wrap *) us->iobuf;
+        struct bulk_cs_wrap *bcs = (struct bulk_cs_wrap *) us->iobuf;
+        int result;
+        unsigned int cswlen;
+        unsigned int cbwlen = US_BULK_CB_WRAP_LEN;
+
+        /* set up the command wrapper */
+        bcb->Signature = cpu_to_le32(US_BULK_CB_SIGN);
+        bcb->DataTransferLength = cpu_to_le32(buf_len);
+        bcb->Flags = (dir == DMA_FROM_DEVICE) ? 1 << 7 : 0;
+        bcb->Tag = ++us->tag;
+        bcb->Lun = lun;
+        bcb->Length = cmd_len;
+
+        /* copy the command payload */
+        memset(bcb->CDB, 0, sizeof(bcb->CDB));
+        memcpy(bcb->CDB, cmd, bcb->Length);
+
+        /* send it to out endpoint */
+        result = usb_stor_bulk_transfer_buf(us, us->send_bulk_pipe,
+                                bcb, cbwlen, NULL);
+        if (result != USB_STOR_XFER_GOOD)
+                return USB_STOR_TRANSPORT_ERROR;
+
+        /* DATA STAGE */
+        /* send/receive data payload, if there is any */
+
+        if (buf && buf_len) {
+                unsigned int pipe = (dir == DMA_FROM_DEVICE) ?
+                                us->recv_bulk_pipe : us->send_bulk_pipe;
+                result = usb_stor_bulk_transfer_buf(us, pipe,
+                                buf, buf_len, NULL);
+                if (result == USB_STOR_XFER_ERROR)
+                        return USB_STOR_TRANSPORT_ERROR;
+        }
+
+        /* get CSW for device status */
+        result = usb_bulk_msg(us->pusb_dev, us->recv_bulk_pipe, bcs,
+                        US_BULK_CS_WRAP_LEN, &cswlen, 250);
+        return result;
+}
+
 /* Determine what the maximum LUN supported is */
 static int rts51x_get_max_lun(struct us_data *us)
 {
@@ -319,6 +365,11 @@ static int rts51x_read_mem(struct us_data *us, u16 addr, u8 *data, u16 len)
 {
         int retval;
         u8 cmnd[12] = { 0 };
+        u8 *buf;
+
+        buf = kmalloc(len, GFP_NOIO);
+        if (buf == NULL)
+                return USB_STOR_TRANSPORT_ERROR;
 
         US_DEBUGP("%s, addr = 0x%x, len = %d\n", __func__, addr, len);
 
@@ -330,10 +381,14 @@ static int rts51x_read_mem(struct us_data *us, u16 addr, u8 *data, u16 len)
         cmnd[5] = (u8) len;
 
         retval = rts51x_bulk_transport(us, 0, cmnd, 12,
-                                       data, len, DMA_FROM_DEVICE, NULL);
-        if (retval != USB_STOR_TRANSPORT_GOOD)
+                                       buf, len, DMA_FROM_DEVICE, NULL);
+        if (retval != USB_STOR_TRANSPORT_GOOD) {
+                kfree(buf);
                 return -EIO;
+        }
 
+        memcpy(data, buf, len);
+        kfree(buf);
         return 0;
 }
 
@@ -341,6 +396,12 @@ static int rts51x_write_mem(struct us_data *us, u16 addr, u8 *data, u16 len)
 {
         int retval;
         u8 cmnd[12] = { 0 };
+        u8 *buf;
+
+        buf = kmalloc(len, GFP_NOIO);
+        if (buf == NULL)
+                return USB_STOR_TRANSPORT_ERROR;
+        memcpy(buf, data, len);
 
         US_DEBUGP("%s, addr = 0x%x, len = %d\n", __func__, addr, len);
 
@@ -352,7 +413,8 @@ static int rts51x_write_mem(struct us_data *us, u16 addr, u8 *data, u16 len)
         cmnd[5] = (u8) len;
 
         retval = rts51x_bulk_transport(us, 0, cmnd, 12,
-                                       data, len, DMA_TO_DEVICE, NULL);
+                                       buf, len, DMA_TO_DEVICE, NULL);
+        kfree(buf);
         if (retval != USB_STOR_TRANSPORT_GOOD)
                 return -EIO;
 
@@ -364,6 +426,11 @@ static int rts51x_read_status(struct us_data *us,
 {
         int retval;
         u8 cmnd[12] = { 0 };
+        u8 *buf;
+
+        buf = kmalloc(len, GFP_NOIO);
+        if (buf == NULL)
+                return USB_STOR_TRANSPORT_ERROR;
 
         US_DEBUGP("%s, lun = %d\n", __func__, lun);
 
@@ -371,10 +438,14 @@ static int rts51x_read_status(struct us_data *us,
         cmnd[1] = 0x09;
 
         retval = rts51x_bulk_transport(us, lun, cmnd, 12,
-                                       status, len, DMA_FROM_DEVICE, actlen);
-        if (retval != USB_STOR_TRANSPORT_GOOD)
+                                       buf, len, DMA_FROM_DEVICE, actlen);
+        if (retval != USB_STOR_TRANSPORT_GOOD) {
+                kfree(buf);
                 return -EIO;
+        }
 
+        memcpy(status, buf, len);
+        kfree(buf);
         return 0;
 }
 
@@ -433,6 +504,29 @@ static int enable_oscillator(struct us_data *us)
         return 0;
 }
 
+static int __do_config_autodelink(struct us_data *us, u8 *data, u16 len)
+{
+        int retval;
+        u16 addr = 0xFE47;
+        u8 cmnd[12] = {0};
+
+        US_DEBUGP("%s, addr = 0x%x, len = %d\n", __FUNCTION__, addr, len);
+
+        cmnd[0] = 0xF0;
+        cmnd[1] = 0x0E;
+        cmnd[2] = (u8)(addr >> 8);
+        cmnd[3] = (u8)addr;
+        cmnd[4] = (u8)(len >> 8);
+        cmnd[5] = (u8)len;
+
+        retval = rts51x_bulk_transport_special(us, 0, cmnd, 12, data, len, DMA_TO_DEVICE, NULL);
+        if (retval != USB_STOR_TRANSPORT_GOOD) {
+                return -EIO;
+        }
+
+        return 0;
+}
+
 static int do_config_autodelink(struct us_data *us, int enable, int force)
 {
         int retval;
@@ -453,7 +547,8 @@ static int do_config_autodelink(struct us_data *us, int enable, int force)
 
         US_DEBUGP("In %s,set 0xfe47 to 0x%x\n", __func__, value);
 
-        retval = rts51x_write_mem(us, 0xFE47, &value, 1);
+        /* retval = rts51x_write_mem(us, 0xFE47, &value, 1); */
+        retval = __do_config_autodelink(us, &value, 1);
         if (retval < 0)
                 return -EIO;
 
@@ -485,7 +580,8 @@ static int config_autodelink_after_power_on(struct us_data *us)
 
                 SET_BIT(value, 7);
 
-                retval = rts51x_write_mem(us, 0xFE47, &value, 1);
+                /* retval = rts51x_write_mem(us, 0xFE47, &value, 1); */
+                retval = __do_config_autodelink(us, &value, 1);
                 if (retval < 0)
                         return -EIO;
 
@@ -506,7 +602,8 @@ static int config_autodelink_after_power_on(struct us_data *us)
                         CLR_BIT(value, 7);
                 }
 
-                retval = rts51x_write_mem(us, 0xFE47, &value, 1);
+                /* retval = rts51x_write_mem(us, 0xFE47, &value, 1); */
+                retval = __do_config_autodelink(us, &value, 1);
                 if (retval < 0)
                         return -EIO;
 
@@ -583,7 +680,8 @@ static int config_autodelink_before_power_down(struct us_data *us)
                         if (CHECK_ID(chip, 0x0138, 0x3882))
                                 SET_BIT(value, 2);
 
-                        retval = rts51x_write_mem(us, 0xFE47, &value, 1);
+                        /* retval = rts51x_write_mem(us, 0xFE47, &value, 1); */
+                        retval = __do_config_autodelink(us, &value, 1);
                         if (retval < 0)
                                 return -EIO;
                 }