[cdi-devel] [PATCH] usb: Handle multiple packets in usb_do_packet

Max Reitz ·

On Sat, Aug 29 14:22, Kevin Wolf wrote:
> Signed-off-by: Kevin Wolf <kevin at tyndur.org>
> ---
>  usb/include/usb.h |    2 +-
>  usb/main.c        |   93 ++++++++++++++++++++++++++++++++++++++++++-----------
>  usb/msd.c         |   10 +++---
>  3 files changed, 80 insertions(+), 25 deletions(-)
> 
> diff --git a/usb/include/usb.h b/usb/include/usb.h
> index 3882601..5f0f2cc 100644
> --- a/usb/include/usb.h
> +++ b/usb/include/usb.h
> @@ -334,7 +334,7 @@ struct usb_pipe {
>  #define HCI_STRUCT_SIZE max(sizeof(struct ohci), sizeof(struct uhci))
>  
>  
> -int usb_do_packet(struct usb_packet* packet);
> +int usb_do_packet(struct usb_packet* packets, int num_packets);
>  void enumerate_hci(struct hci*);
>  struct cdi_driver* init_uhcd(void);
>  void init_msc_driver(void);
> diff --git a/usb/main.c b/usb/main.c
> index cfab9a5..da767cb 100644
> --- a/usb/main.c
> +++ b/usb/main.c
> @@ -99,20 +99,38 @@ static const int next_data_type[9] = {
>      "STATUS"
>     };*/
>  
> +#define MIN(a, b) ((a) < (b) ? (a) : (b))
> +
>  /**
>    * Verarbeitet ein USB-Paket.
>    *
>    * @param packet Das Paket
>    */
>  
> -int usb_do_packet(struct usb_packet* packet)
> +int usb_do_packet(struct usb_packet* packets, int num_packets)
>  {
>      int error = USB_NAK;
> -    int tod_short, tod = packet->type_of_data;
> +    int tod_short, tod;
>      int i = 0, mps;
> -    struct usb_pipe* pipe = packet->pipe;
> -    struct usb_device* device = pipe->device;
> +    int num_out_packets;
> +    struct usb_pipe* pipe;
> +    struct usb_device* device;
> +    struct usb_packet* out_packets;
> +
> +    if (num_packets < 1) {
> +        return USB_TRIVIAL_ERROR;
> +    }
> +
> +    // Alle Pakete muessen vom selben Typ sein und dieselbe Pipe benutzen
> +    tod = packets[0].type_of_data;
> +    pipe = packets[0].pipe;
> +    for (i = 1; i < num_packets; i++) {
> +        if ((packets[i].type_of_data != tod) || (packets[i].pipe != pipe)) {
> +            return USB_TRIVIAL_ERROR;
> +        }
> +    }
>  
> +    device = pipe->device;
>      if (!(device->expects & tod)) {
>          dprintf("0x%04X erwartet, 0x%04X bekommen...?\n", device->expects, tod);
>          // FIXME Das ist keine elegante Loesung des Problems
> @@ -125,31 +143,63 @@ int usb_do_packet(struct usb_packet* packet)
>      }
>      //dprintf("TOD: %s\n", tod_name[tod_short]);
>      device->expects = next_data_type[tod_short];
> -    packet->type &= 0xFF;
>  
> -    struct usb_packet send_packet = {
> -        .pipe      = packet->pipe,
> -        .type      = packet->type,
> -        .data      = packet->data
> -    };
> +    // Pruefen, wie viele Pakete wir draus machen muessen
>      mps = pipe->endpoint->max_packet_size;
> +    num_out_packets = 0;
> +    for (i = 0; i < num_packets; i++) {
> +        int num = (packets[i].length + mps - 1) / mps;
> +        if (num > 0) {
> +            num_out_packets += num;
> +        } else {
> +            // Pakete ohne Daten wollen auch gesendet werden
> +            num_out_packets++;
> +        }
> +    }
> +
> +    // Ggf. Pakete kopieren und splitten
> +    if (num_out_packets == num_packets) {
> +        out_packets = packets;
> +    } else {
> +        int j;
> +        int cur = 0;
> +
> +        out_packets = calloc(num_out_packets, sizeof(*out_packets));
> +        for (i = 0; i < num_packets; i++) {
> +            int num = (packets[i].length + mps - 1) / mps;
> +            if (num == 0) {
> +                num = 1;
> +            }
> +
> +            for (j = 0; j < num; j++) {
> +                out_packets[cur].pipe = pipe;
> +                out_packets[cur].type = packets[i].type;
> +                out_packets[cur].data = packets[i].data;
> +                out_packets[cur].length = MIN(packets[i].length, mps);
> +                out_packets[cur].type_of_data = tod;
>  
> -    for (i = 0; (i < packet->length) || (!i && !packet->length); i += mps) {
> -        send_packet.length =
> -            (packet->length - i > mps) ? mps : (packet->length - i);
> +                packets[i].data += out_packets[cur].length;
> +                packets[i].length -= out_packets[cur].length;
> +                cur++;
> +            }
> +        }
> +    }
> +
> +    // Pakete (FIXME: einzeln) senden
> +    for (i = 0; i < num_out_packets; i++) {
>          error = USB_NAK;
>          while (error == USB_NAK) {
> -            error = device->hci->do_packet(&send_packet);
> +            error = device->hci->do_packet(&out_packets[i]);
>              if (error == USB_NAK) {
>                  cdi_sleep_ms(5);
>              }
>          }
>          if (error != USB_NO_ERROR) {
> -            i -= mps;
> +            i--;
>          } else {
>              pipe->data_toggle ^= 1;
> -            send_packet.data += mps;
>          }
> +
>          if (error == USB_STALLED) {
>              printf("[usb] ENDPOINT 0x%02X DES GER�TS %i STALLED!\n",
>                  pipe->endpoint->endpoint_address,
> @@ -159,6 +209,11 @@ int usb_do_packet(struct usb_packet* packet)
>                  pipe->endpoint->endpoint_address);
>          }
>      }
> +
> +    if (out_packets != packets) {
> +        free(out_packets);
> +    }
> +
>      return error;
>  }
>  
> @@ -191,7 +246,7 @@ static void* do_control(struct usb_device* device, int direction, void* buffer,
>          .type_of_data = USB_TOD_SETUP,
>      };
>  
> -    if (usb_do_packet(&setup_packet)) {
> +    if (usb_do_packet(&setup_packet, 1)) {
>          return NULL;
>      }
>  
> @@ -209,7 +264,7 @@ static void* do_control(struct usb_device* device, int direction, void* buffer,
>                   DEV_TO_HOST ? USB_TOD_SETUP_DATA_IN : USB_TOD_SETUP_DATA_OUT),
>          };
>  
> -        rval = usb_do_packet(&data_packet);
> +        rval = usb_do_packet(&data_packet, 1);
>      }
>  
>      if (rval == USB_NO_ERROR) {
> @@ -228,7 +283,7 @@ static void* do_control(struct usb_device* device, int direction, void* buffer,
>              ack_packet.type_of_data = USB_TOD_SETUP_ACK_OUT;
>          }
>  
> -        rval = usb_do_packet(&ack_packet);
> +        rval = usb_do_packet(&ack_packet, 1);
>      }
>      return (rval == USB_NO_ERROR) ? buffer : NULL;
>  }
> diff --git a/usb/msd.c b/usb/msd.c
> index f91df4d..4191002 100644
> --- a/usb/msd.c
> +++ b/usb/msd.c
> @@ -241,7 +241,7 @@ static int write_cmd(struct usb_device* usbdev, void* src)
>          .type_of_data = USB_TOD_COMMAND,
>      };
>  
> -    return usb_do_packet(&cmd_packet);
> +    return usb_do_packet(&cmd_packet, 1);
>  }
>  
>  /**
> @@ -268,7 +268,7 @@ static int read_status(struct usb_device* usbdev, uint32_t expected_tag)
>          .type_of_data = USB_TOD_STATUS,
>      };
>  
> -    error = usb_do_packet(&status_packet);
> +    error = usb_do_packet(&status_packet, 1);
>      if (error != USB_NO_ERROR) {
>          return error;
>      }
> @@ -321,7 +321,7 @@ static int msd_get_capacity(struct usb_device* usbdev, uint32_t* block_size,
>          .type_of_data = USB_TOD_DATA_IN,
>      };
>  
> -    if (usb_do_packet(&in_packet) != USB_NO_ERROR) {
> +    if (usb_do_packet(&in_packet, 1) != USB_NO_ERROR) {
>          return 0;
>      }
>  
> @@ -463,7 +463,7 @@ static uint32_t msd_read(struct usb_device* usbdev, uint32_t lba,
>          .type_of_data = USB_TOD_DATA_IN,
>      };
>  
> -    error = usb_do_packet(&in_packet);
> +    error = usb_do_packet(&in_packet, 1);
>      if (error != USB_NO_ERROR) {
>          return error;
>      }
> @@ -564,7 +564,7 @@ static uint32_t msd_write(struct usb_device* usbdev, uint32_t lba,
>          .type_of_data = USB_TOD_DATA_OUT,
>      };
>  
> -    error = usb_do_packet(&out_packet);
> +    error = usb_do_packet(&out_packet, 1);
>      if (error != USB_NO_ERROR) {
>          return error;
>      }
> 

Acked-by: Max Reitz <max at tyndur.org>