Signed-off-by: Kevin Wolf <kevin@xxxxxxxxxx>
---
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;
}