usb: f_mass_storage: sleep_thread: retrigger watchdog

[karo-tx-uboot.git] / drivers / usb / gadget / f_mass_storage.c

diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index b34068a0bb3862f508c2822078f5beb5850f6ed3..f30b4f812e6d4b8495e8b08fc0c59bb84e9ce851 100644 (file)
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -9,7 +9,6 @@
  * SPDX-License-Identifier: GPL-2.0+   BSD-3-Clause
  */
 
-
 /*
  * The Mass Storage Function acts as a USB Mass Storage device,
  * appearing to the host as a disk drive or as a CD-ROM drive.  In
@@ -153,7 +152,6 @@
  * <http://www.usb.org/developers/devclass_docs/usbmass-ufi10.pdf>.
  */
 
-
 /*
  *                             Driver Design
  *
@@ -245,6 +243,8 @@
 #include <config.h>
 #include <malloc.h>
 #include <common.h>
+#include <g_dnl.h>
+#include <watchdog.h>
 
 #include <linux/err.h>
 #include <linux/usb/ch9.h>
@@ -256,6 +256,7 @@
 #include <linux/usb/gadget.h>
 #include <linux/usb/composite.h>
 #include <usb/lin_gadget_compat.h>
+#include <g_dnl.h>
 
 /*------------------------------------------------------------------------*/
 
@@ -264,7 +265,6 @@
 
 static const char fsg_string_interface[] = "Mass Storage";
 
-
 #define FSG_NO_INTR_EP 1
 #define FSG_NO_DEVICE_STRINGS    1
 #define FSG_NO_OTG               1
@@ -444,7 +444,7 @@ static void set_bulk_out_req_length(struct fsg_common *common,
 
 /*-------------------------------------------------------------------------*/
 
-struct ums_board_info                  *ums_info;
+struct ums *ums;
 struct fsg_common *the_fsg_common;
 
 static int fsg_set_halt(struct fsg_dev *fsg, struct usb_ep *ep)
@@ -672,13 +672,27 @@ static int sleep_thread(struct fsg_common *common)
                if (common->thread_wakeup_needed)
                        break;
 
-               if (++i == 50000) {
+               if (++i == 20000) {
                        busy_indicator();
                        i = 0;
                        k++;
                }
 
-               usb_gadget_handle_interrupts();
+               if (k == 10) {
+                       /* Handle CTRL+C */
+                       if (ctrlc())
+                               return -EPIPE;
+
+                       WATCHDOG_RESET();
+
+                       /* Check cable connection */
+                       if (!g_dnl_board_usb_cable_connected())
+                               return -EIO;
+
+                       k = 0;
+               }
+
+               usb_gadget_handle_interrupts(0);
        }
        common->thread_wakeup_needed = 0;
        return rc;
@@ -760,14 +774,14 @@ static int do_read(struct fsg_common *common)
                }
 
                /* Perform the read */
-               nread = 0;
-               rc = ums_info->read_sector(&(ums_info->ums_dev),
-                                          file_offset / SECTOR_SIZE,
-                                          amount / SECTOR_SIZE,
-                                          (char __user *)bh->buf);
-               if (rc)
+               rc = ums->read_sector(ums,
+                                     file_offset / SECTOR_SIZE,
+                                     amount / SECTOR_SIZE,
+                                     (char __user *)bh->buf);
+               if (!rc)
                        return -EIO;
-               nread = amount;
+
+               nread = rc * SECTOR_SIZE;
 
                VLDBG(curlun, "file read %u @ %llu -> %d\n", amount,
                                (unsigned long long) file_offset,
@@ -934,13 +948,13 @@ static int do_write(struct fsg_common *common)
                        amount = bh->outreq->actual;
 
                        /* Perform the write */
-                       rc = ums_info->write_sector(&(ums_info->ums_dev),
+                       rc = ums->write_sector(ums,
                                               file_offset / SECTOR_SIZE,
                                               amount / SECTOR_SIZE,
                                               (char __user *)bh->buf);
-                       if (rc)
+                       if (!rc)
                                return -EIO;
-                       nwritten = amount;
+                       nwritten = rc * SECTOR_SIZE;
 
                        VLDBG(curlun, "file write %u @ %llu -> %d\n", amount,
                                        (unsigned long long) file_offset,
@@ -962,6 +976,8 @@ static int do_write(struct fsg_common *common)
 
                        /* If an error occurred, report it and its position */
                        if (nwritten < amount) {
+                               printf("nwritten:%zd amount:%u\n", nwritten,
+                                      amount);
                                curlun->sense_data = SS_WRITE_ERROR;
                                curlun->info_valid = 1;
                                break;
@@ -1048,14 +1064,13 @@ static int do_verify(struct fsg_common *common)
                }
 
                /* Perform the read */
-               nread = 0;
-               rc = ums_info->read_sector(&(ums_info->ums_dev),
-                                          file_offset / SECTOR_SIZE,
-                                          amount / SECTOR_SIZE,
-                                          (char __user *)bh->buf);
-               if (rc)
+               rc = ums->read_sector(ums,
+                                     file_offset / SECTOR_SIZE,
+                                     amount / SECTOR_SIZE,
+                                     (char __user *)bh->buf);
+               if (!rc)
                        return -EIO;
-               nread = amount;
+               nread = rc * SECTOR_SIZE;
 
                VLDBG(curlun, "file read %u @ %llu -> %d\n", amount,
                                (unsigned long long) file_offset,
@@ -1098,12 +1113,13 @@ static int do_inquiry(struct fsg_common *common, struct fsg_buffhd *bh)
 
        memset(buf, 0, 8);
        buf[0] = TYPE_DISK;
+       buf[1] = curlun->removable ? 0x80 : 0;
        buf[2] = 2;             /* ANSI SCSI level 2 */
        buf[3] = 2;             /* SCSI-2 INQUIRY data format */
        buf[4] = 31;            /* Additional length */
                                /* No special options */
        sprintf((char *) (buf + 8), "%-8s%-16s%04x", (char*) vendor_id ,
-                       ums_info->name, (u16) 0xffff);
+                       ums->name, (u16) 0xffff);
 
        return 36;
 }
@@ -2389,6 +2405,7 @@ static void handle_exception(struct fsg_common *common)
 
 int fsg_main_thread(void *common_)
 {
+       int ret;
        struct fsg_common       *common = the_fsg_common;
        /* The main loop */
        do {
@@ -2398,12 +2415,16 @@ int fsg_main_thread(void *common_)
                }
 
                if (!common->running) {
-                       sleep_thread(common);
+                       ret = sleep_thread(common);
+                       if (ret)
+                               return ret;
+
                        continue;
                }
 
-               if (get_next_command(common))
-                       continue;
+               ret = get_next_command(common);
+               if (ret)
+                       return ret;
 
                if (!exception_in_progress(common))
                        common->state = FSG_STATE_DATA_PHASE;
@@ -2445,12 +2466,12 @@ static struct fsg_common *fsg_common_init(struct fsg_common *common,
 
        /* Allocate? */
        if (!common) {
-               common = calloc(sizeof *common, 1);
+               common = calloc(sizeof(*common), 1);
                if (!common)
                        return ERR_PTR(-ENOMEM);
                common->free_storage_on_release = 1;
        } else {
-               memset(common, 0, sizeof common);
+               memset(common, 0, sizeof(*common));
                common->free_storage_on_release = 0;
        }
 
@@ -2499,7 +2520,7 @@ static struct fsg_common *fsg_common_init(struct fsg_common *common,
 buffhds_first_it:
                bh->inreq_busy = 0;
                bh->outreq_busy = 0;
-               bh->buf = kmalloc(FSG_BUFLEN, GFP_KERNEL);
+               bh->buf = memalign(CONFIG_SYS_CACHELINE_SIZE, FSG_BUFLEN);
                if (unlikely(!bh->buf)) {
                        rc = -ENOMEM;
                        goto error_release;
@@ -2606,7 +2627,7 @@ usb_copy_descriptors(struct usb_descriptor_header **src)
                bytes += (*tmp)->bLength;
        bytes += (n_desc + 1) * sizeof(*tmp);
 
-       mem = kmalloc(bytes, GFP_KERNEL);
+       mem = memalign(CONFIG_SYS_CACHELINE_SIZE, bytes);
        if (!mem)
                return NULL;
 
@@ -2629,8 +2650,6 @@ usb_copy_descriptors(struct usb_descriptor_header **src)
        return ret;
 }
 
-
-
 static void fsg_unbind(struct usb_configuration *c, struct usb_function *f)
 {
        struct fsg_dev          *fsg = fsg_from_func(f);
@@ -2758,9 +2777,11 @@ int fsg_add(struct usb_configuration *c)
        return fsg_bind_config(c->cdev, c, fsg_common);
 }
 
-int fsg_init(struct ums_board_info *ums)
+int fsg_init(struct ums *ums_dev)
 {
-       ums_info = ums;
+       ums = ums_dev;
 
        return 0;
 }
+
+DECLARE_GADGET_BIND_CALLBACK(usb_dnl_ums, fsg_add);