[cdi-devel] [PATCH] usb: Handle multiple packets in usb_do_packet
Kevin Wolf ·
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; } -- 1.6.0.2