4 files changed, 81 insertions, 2 deletions

diff --git a/drivers/firewire/fw-cdev.c b/drivers/firewire/fw-cdev.c
index 214e534efee5..539dae5eb5b2 100644
--- a/drivers/firewire/fw-cdev.c
+++ b/drivers/firewire/fw-cdev.c
@@ -1242,6 +1242,38 @@ static int ioctl_send_broadcast_request(struct client *client, void *buffer)
 	return init_request(client, request, LOCAL_BUS | 0x3f, SCODE_100);
 }
 
+struct stream_packet {
+	struct fw_packet packet;
+	u8 data[0];
+};
+
+static void send_stream_packet_done(struct fw_packet *packet,
+				    struct fw_card *card, int status)
+{
+	kfree(container_of(packet, struct stream_packet, packet));
+}
+
+static int ioctl_send_stream_packet(struct client *client, void *buffer)
+{
+	struct fw_cdev_send_stream_packet *request = buffer;
+	struct stream_packet *p;
+
+	p = kmalloc(sizeof(*p) + request->size, GFP_KERNEL);
+	if (p == NULL)
+		return -ENOMEM;
+
+	if (request->data &&
+	    copy_from_user(p->data, u64_to_uptr(request->data), request->size)) {
+		kfree(p);
+		return -EFAULT;
+	}
+	fw_send_stream_packet(client->device->card, &p->packet,
+			      request->generation, request->speed,
+			      request->channel, request->sy, request->tag,
+			      p->data, request->size, send_stream_packet_done);
+	return 0;
+}
+
 static int (* const ioctl_handlers[])(struct client *client, void *buffer) = {
 	ioctl_get_info,
 	ioctl_send_request,
@@ -1262,6 +1294,7 @@ static int (* const ioctl_handlers[])(struct client *client, void *buffer) = {
 	ioctl_deallocate_iso_resource_once,
 	ioctl_get_speed,
 	ioctl_send_broadcast_request,
+	ioctl_send_stream_packet,
 };
 
 static int dispatch_ioctl(struct client *client,
diff --git a/drivers/firewire/fw-ohci.c b/drivers/firewire/fw-ohci.c
index c92278374658..1180d0be0bb4 100644
--- a/drivers/firewire/fw-ohci.c
+++ b/drivers/firewire/fw-ohci.c
@@ -936,7 +936,9 @@ static int at_context_queue_packet(struct context *ctx,
 	 */
 
 	header = (__le32 *) &d[1];
-	if (packet->header_length > 8) {
+	switch (packet->header_length) {
+	case 16:
+	case 12:
 		header[0] = cpu_to_le32((packet->header[0] & 0xffff) |
 					(packet->speed << 16));
 		header[1] = cpu_to_le32((packet->header[1] & 0xffff) |
@@ -950,12 +952,27 @@ static int at_context_queue_packet(struct context *ctx,
 			header[3] = (__force __le32) packet->header[3];
 
 		d[0].req_count = cpu_to_le16(packet->header_length);
-	} else {
+		break;
+
+	case 8:
 		header[0] = cpu_to_le32((OHCI1394_phy_tcode << 4) |
 					(packet->speed << 16));
 		header[1] = cpu_to_le32(packet->header[0]);
 		header[2] = cpu_to_le32(packet->header[1]);
 		d[0].req_count = cpu_to_le16(12);
+		break;
+
+	case 4:
+		header[0] = cpu_to_le32((packet->header[0] & 0xffff) |
+					(packet->speed << 16));
+		header[1] = cpu_to_le32(packet->header[0] & 0xffff0000);
+		d[0].req_count = cpu_to_le16(8);
+		break;
+
+	default:
+		/* BUG(); */
+		packet->ack = RCODE_SEND_ERROR;
+		return -1;
 	}
 
 	driver_data = (struct driver_data *) &d[3];
diff --git a/drivers/firewire/fw-transaction.c b/drivers/firewire/fw-transaction.c
index 76938fe432a0..e3da58991960 100644
--- a/drivers/firewire/fw-transaction.c
+++ b/drivers/firewire/fw-transaction.c
@@ -37,6 +37,10 @@
 #include "fw-topology.h"
 #include "fw-device.h"
 
+#define HEADER_TAG(tag)			((tag) << 14)
+#define HEADER_CHANNEL(ch)		((ch) << 8)
+#define HEADER_SY(sy)			((sy) << 0)
+
 #define HEADER_PRI(pri)			((pri) << 0)
 #define HEADER_TCODE(tcode)		((tcode) << 4)
 #define HEADER_RETRY(retry)		((retry) << 8)
@@ -293,6 +297,27 @@ void fw_send_request(struct fw_card *card, struct fw_transaction *t, int tcode,
 }
 EXPORT_SYMBOL(fw_send_request);
 
+void fw_send_stream_packet(struct fw_card *card, struct fw_packet *p,
+		int generation, int speed, int channel, int sy, int tag,
+		void *payload, size_t length, fw_packet_callback_t callback)
+{
+	p->callback = callback;
+	p->header[0] =
+		  HEADER_DATA_LENGTH(length)
+		| HEADER_TAG(tag)
+		| HEADER_CHANNEL(channel)
+		| HEADER_TCODE(TCODE_STREAM_DATA)
+		| HEADER_SY(sy);
+	p->header_length = 4;
+	p->payload = payload;
+	p->payload_length = length;
+	p->speed = speed;
+	p->generation = generation;
+	p->ack = 0;
+
+	card->driver->send_request(card, p);
+}
+
 struct transaction_callback_data {
 	struct completion done;
 	void *payload;
diff --git a/drivers/firewire/fw-transaction.h b/drivers/firewire/fw-transaction.h
index 35d0a4bb6d5c..eed2e295eb3c 100644
--- a/drivers/firewire/fw-transaction.h
+++ b/drivers/firewire/fw-transaction.h
@@ -407,6 +407,10 @@ void fw_send_request(struct fw_card *card, struct fw_transaction *t,
 		int tcode, int destination_id, int generation, int speed,
 		unsigned long long offset, void *payload, size_t length,
 		fw_transaction_callback_t callback, void *callback_data);
+void fw_send_stream_packet(struct fw_card *card, struct fw_packet *p,
+		int generation, int speed, int channel, int sy, int tag,
+		void *payload, size_t length, fw_packet_callback_t callback);
+
 int fw_cancel_transaction(struct fw_card *card,
 			  struct fw_transaction *transaction);
 void fw_flush_transactions(struct fw_card *card);