[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>