4 files changed, 75 insertions, 16 deletions

diff --git a/drivers/usb/mon/mon_bin.c b/drivers/usb/mon/mon_bin.c
index 10f3205798e8..ddf7f9a1b336 100644
--- a/drivers/usb/mon/mon_bin.c
+++ b/drivers/usb/mon/mon_bin.c
@@ -16,6 +16,8 @@
 #include <linux/compat.h>
 #include <linux/mm.h>
 #include <linux/smp_lock.h>
+#include <linux/scatterlist.h>
+#include <linux/slab.h>
 
 #include <asm/uaccess.h>
 
@@ -221,7 +223,7 @@ static void mon_free_buff(struct mon_pgmap *map, int npages);
 /*
  * This is a "chunked memcpy". It does not manipulate any counters.
  */
-static void mon_copy_to_buff(const struct mon_reader_bin *this,
+static unsigned int mon_copy_to_buff(const struct mon_reader_bin *this,
     unsigned int off, const unsigned char *from, unsigned int length)
 {
         unsigned int step_len;
@@ -246,6 +248,7 @@ static void mon_copy_to_buff(const struct mon_reader_bin *this,
                 from += step_len;
                 length -= step_len;
         }
+        return off;
 }
 
 /*
@@ -394,14 +397,44 @@ static inline char mon_bin_get_setup(unsigned char *setupb,
         return 0;
 }
 
-static char mon_bin_get_data(const struct mon_reader_bin *rp,
-    unsigned int offset, struct urb *urb, unsigned int length)
+static unsigned int mon_bin_get_data(const struct mon_reader_bin *rp,
+    unsigned int offset, struct urb *urb, unsigned int length,
+    char *flag)
 {
+        int i;
+        struct scatterlist *sg;
+        unsigned int this_len;
+
+        *flag = 0;
+        if (urb->num_sgs == 0) {
+                if (urb->transfer_buffer == NULL) {
+                        *flag = 'Z';
+                        return length;
+                }
+                mon_copy_to_buff(rp, offset, urb->transfer_buffer, length);
+                length = 0;
 
-        if (urb->transfer_buffer == NULL)
-                return 'Z';
-        mon_copy_to_buff(rp, offset, urb->transfer_buffer, length);
-        return 0;
+        } else {
+                /* If IOMMU coalescing occurred, we cannot trust sg_page */
+                if (urb->sg->nents != urb->num_sgs) {
+                        *flag = 'D';
+                        return length;
+                }
+
+                /* Copy up to the first non-addressable segment */
+                for_each_sg(urb->sg->sg, sg, urb->num_sgs, i) {
+                        if (length == 0 || PageHighMem(sg_page(sg)))
+                                break;
+                        this_len = min_t(unsigned int, sg->length, length);
+                        offset = mon_copy_to_buff(rp, offset, sg_virt(sg),
+                                        this_len);
+                        length -= this_len;
+                }
+                if (i == 0)
+                        *flag = 'D';
+        }
+
+        return length;
 }
 
 static void mon_bin_get_isodesc(const struct mon_reader_bin *rp,
@@ -428,8 +461,8 @@ static void mon_bin_event(struct mon_reader_bin *rp, struct urb *urb,
     char ev_type, int status)
 {
         const struct usb_endpoint_descriptor *epd = &urb->ep->desc;
-        unsigned long flags;
         struct timeval ts;
+        unsigned long flags;
         unsigned int urb_length;
         unsigned int offset;
         unsigned int length;
@@ -536,8 +569,9 @@ static void mon_bin_event(struct mon_reader_bin *rp, struct urb *urb,
         }
 
         if (length != 0) {
-                ep->flag_data = mon_bin_get_data(rp, offset, urb, length);
-                if (ep->flag_data != 0) {       /* Yes, it's 0x00, not '0' */
+                length = mon_bin_get_data(rp, offset, urb, length,
+                                &ep->flag_data);
+                if (length > 0) {
                         delta = (ep->len_cap + PKT_ALIGN-1) & ~(PKT_ALIGN-1);
                         ep->len_cap -= length;
                         delta -= (ep->len_cap + PKT_ALIGN-1) & ~(PKT_ALIGN-1);
@@ -567,10 +601,13 @@ static void mon_bin_complete(void *data, struct urb *urb, int status)
 static void mon_bin_error(void *data, struct urb *urb, int error)
 {
         struct mon_reader_bin *rp = data;
+        struct timeval ts;
         unsigned long flags;
         unsigned int offset;
         struct mon_bin_hdr *ep;
 
+        do_gettimeofday(&ts);
+
         spin_lock_irqsave(&rp->b_lock, flags);
 
         offset = mon_buff_area_alloc(rp, PKT_SIZE);
@@ -590,6 +627,8 @@ static void mon_bin_error(void *data, struct urb *urb, int error)
         ep->devnum = urb->dev->devnum;
         ep->busnum = urb->dev->bus->busnum;
         ep->id = (unsigned long) urb;
+        ep->ts_sec = ts.tv_sec;
+        ep->ts_usec = ts.tv_usec;
         ep->status = error;
 
         ep->flag_setup = '-';
diff --git a/drivers/usb/mon/mon_main.c b/drivers/usb/mon/mon_main.c
index e0c2db3b767b..e4af18b93c7d 100644
--- a/drivers/usb/mon/mon_main.c
+++ b/drivers/usb/mon/mon_main.c
@@ -9,6 +9,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/usb.h>
+#include <linux/slab.h>
 #include <linux/notifier.h>
 #include <linux/mutex.h>
 
diff --git a/drivers/usb/mon/mon_stat.c b/drivers/usb/mon/mon_stat.c
index ac8b0d5ce7f8..1becdc3837e6 100644
--- a/drivers/usb/mon/mon_stat.c
+++ b/drivers/usb/mon/mon_stat.c
@@ -8,6 +8,7 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/slab.h>
 #include <linux/usb.h>
 #include <linux/fs.h>
 #include <asm/uaccess.h>
diff --git a/drivers/usb/mon/mon_text.c b/drivers/usb/mon/mon_text.c
index 9f1a9227ebe6..4d0be130f49b 100644
--- a/drivers/usb/mon/mon_text.c
+++ b/drivers/usb/mon/mon_text.c
@@ -7,9 +7,11 @@
 #include <linux/kernel.h>
 #include <linux/list.h>
 #include <linux/usb.h>
+#include <linux/slab.h>
 #include <linux/time.h>
 #include <linux/mutex.h>
 #include <linux/debugfs.h>
+#include <linux/scatterlist.h>
 #include <asm/uaccess.h>
 
 #include "usb_mon.h"
@@ -137,6 +139,8 @@ static inline char mon_text_get_setup(struct mon_event_text *ep,
 static inline char mon_text_get_data(struct mon_event_text *ep, struct urb *urb,
     int len, char ev_type, struct mon_bus *mbus)
 {
+        void *src;
+
         if (len <= 0)
                 return 'L';
         if (len >= DATA_MAX)
@@ -150,10 +154,24 @@ static inline char mon_text_get_data(struct mon_event_text *ep, struct urb *urb,
                         return '>';
         }
 
-        if (urb->transfer_buffer == NULL)
-                return 'Z';     /* '0' would be not as pretty. */
+        if (urb->num_sgs == 0) {
+                src = urb->transfer_buffer;
+                if (src == NULL)
+                        return 'Z';     /* '0' would be not as pretty. */
+        } else {
+                struct scatterlist *sg = urb->sg->sg;
+
+                /* If IOMMU coalescing occurred, we cannot trust sg_page */
+                if (urb->sg->nents != urb->num_sgs ||
+                                PageHighMem(sg_page(sg)))
+                        return 'D';
 
-        memcpy(ep->data, urb->transfer_buffer, len);
+                /* For the text interface we copy only the first sg buffer */
+                len = min_t(int, sg->length, len);
+                src = sg_virt(sg);
+        }
+
+        memcpy(ep->data, src, len);
         return 0;
 }
 
@@ -163,7 +181,7 @@ static inline unsigned int mon_get_timestamp(void)
         unsigned int stamp;
 
         do_gettimeofday(&tval);
-        stamp = tval.tv_sec & 0xFFFF;   /* 2^32 = 4294967296. Limit to 4096s. */
+        stamp = tval.tv_sec & 0xFFF;    /* 2^32 = 4294967296. Limit to 4096s. */
         stamp = stamp * 1000000 + tval.tv_usec;
         return stamp;
 }
@@ -256,12 +274,12 @@ static void mon_text_error(void *data, struct urb *urb, int error)
 
         ep->type = 'E';
         ep->id = (unsigned long) urb;
-        ep->busnum = 0;
+        ep->busnum = urb->dev->bus->busnum;
         ep->devnum = urb->dev->devnum;
         ep->epnum = usb_endpoint_num(&urb->ep->desc);
         ep->xfertype = usb_endpoint_type(&urb->ep->desc);
         ep->is_in = usb_urb_dir_in(urb);
-        ep->tstamp = 0;
+        ep->tstamp = mon_get_timestamp();
         ep->length = 0;
         ep->status = error;