1 files changed, 92 insertions, 15 deletions

diff --git a/drivers/firewire/fw-ohci.c b/drivers/firewire/fw-ohci.c
index 76ac70a3854..9b9ea0f3c41 100644
--- a/drivers/firewire/fw-ohci.c
+++ b/drivers/firewire/fw-ohci.c
@@ -1267,16 +1267,34 @@ ohci_enable_phys_dma(struct fw_card *card, int node_id, int generation)
 	spin_unlock_irqrestore(&ohci->lock, flags);
 	return retval;
 }
+ 
+static int handle_ir_bufferfill_packet(struct context *context,
+				       struct descriptor *d,
+				       struct descriptor *last)
+{
+	struct iso_context *ctx =
+		container_of(context, struct iso_context, context);
 
-static int handle_ir_packet(struct context *context,
-			    struct descriptor *d,
-			    struct descriptor *last)
+	if (d->res_count > 0)
+		return 0;
+
+	if (le16_to_cpu(last->control) & descriptor_irq_always)
+		ctx->base.callback(&ctx->base,
+				   le16_to_cpu(last->res_count),
+				   0, NULL, ctx->base.callback_data);
+
+	return 1;
+}
+
+static int handle_ir_dualbuffer_packet(struct context *context,
+				       struct descriptor *d,
+				       struct descriptor *last)
 {
 	struct iso_context *ctx =
 		container_of(context, struct iso_context, context);
 	struct db_descriptor *db = (struct db_descriptor *) d;
 	size_t header_length;
- 
+
 	if (db->first_res_count > 0 && db->second_res_count > 0)
 		/* This descriptor isn't done yet, stop iteration. */
 		return 0;
@@ -1317,7 +1335,7 @@ static int handle_it_packet(struct context *context,
 }
 
 static struct fw_iso_context *
-ohci_allocate_iso_context(struct fw_card *card, int type)
+ohci_allocate_iso_context(struct fw_card *card, int type, size_t header_size)
 {
 	struct fw_ohci *ohci = fw_ohci(card);
 	struct iso_context *ctx, *list;
@@ -1333,7 +1351,10 @@ ohci_allocate_iso_context(struct fw_card *card, int type)
 	} else {
  		mask = &ohci->ir_context_mask;
  		list = ohci->ir_context_list;
-		callback = handle_ir_packet;
+		if (header_size > 0)
+			callback = handle_ir_dualbuffer_packet;
+		else
+			callback = handle_ir_bufferfill_packet;
 	}
 
 	spin_lock_irqsave(&ohci->lock, flags);
@@ -1378,7 +1399,7 @@ static int ohci_start_iso(struct fw_iso_context *base, s32 cycle)
 {
  	struct iso_context *ctx = container_of(base, struct iso_context, base);
 	struct fw_ohci *ohci = ctx->context.ohci;
-	u32 cycle_match = 0;
+	u32 cycle_match = 0, mode;
 	int index;
 
 	if (ctx->base.type == FW_ISO_CONTEXT_TRANSMIT) {
@@ -1393,11 +1414,15 @@ static int ohci_start_iso(struct fw_iso_context *base, s32 cycle)
 	} else {
 		index = ctx - ohci->ir_context_list;
 
+		if (ctx->base.header_size > 0)
+			mode = IR_CONTEXT_DUAL_BUFFER_MODE;
+		else
+			mode = IR_CONTEXT_BUFFER_FILL;
 		reg_write(ohci, OHCI1394_IsoRecvIntEventClear, 1 << index);
 		reg_write(ohci, OHCI1394_IsoRecvIntMaskSet, 1 << index);
 		reg_write(ohci, context_match(ctx->context.regs),
 			  0xf0000000 | ctx->base.channel);
-		context_run(&ctx->context, IR_CONTEXT_DUAL_BUFFER_MODE);
+		context_run(&ctx->context, mode);
 	}
 
 	return 0;
@@ -1544,10 +1569,10 @@ ohci_queue_iso_transmit(struct fw_iso_context *base,
 }
 
 static int
-ohci_queue_iso_receive(struct fw_iso_context *base,
-		       struct fw_iso_packet *packet,
-		       struct fw_iso_buffer *buffer,
-		       unsigned long payload)
+ohci_queue_iso_receive_dualbuffer(struct fw_iso_context *base,
+				  struct fw_iso_packet *packet,
+				  struct fw_iso_buffer *buffer,
+				  unsigned long payload)
 {
 	struct iso_context *ctx = container_of(base, struct iso_context, base);
 	struct db_descriptor *db = NULL;
@@ -1604,10 +1629,58 @@ ohci_queue_iso_receive(struct fw_iso_context *base,
 
 	if (p->interrupt)
 		db->control |= cpu_to_le16(descriptor_irq_always);
+
+	return 0;
+}
  
- 	return 0;
- }
+static int
+ohci_queue_iso_receive_bufferfill(struct fw_iso_context *base,
+				  struct fw_iso_packet *packet,
+				  struct fw_iso_buffer *buffer,
+				  unsigned long payload)
+{
+	struct iso_context *ctx = container_of(base, struct iso_context, base);
+	struct descriptor *d = NULL;
+	dma_addr_t d_bus, page_bus;
+	u32 length, rest;
+	int page, offset;
  
+	page   = payload >> PAGE_SHIFT;
+	offset = payload & ~PAGE_MASK;
+	rest   = packet->payload_length;
+
+	while (rest > 0) {
+		d = context_get_descriptors(&ctx->context, 1, &d_bus);
+		if (d == NULL)
+			return -ENOMEM;
+
+		d->control = cpu_to_le16(descriptor_input_more |
+					 descriptor_status |
+					 descriptor_branch_always);
+		
+		if (offset + rest < PAGE_SIZE)
+			length = rest;
+		else
+			length = PAGE_SIZE - offset;
+
+		page_bus = page_private(buffer->pages[page]);
+		d->data_address = cpu_to_le32(page_bus + offset);
+		d->req_count = cpu_to_le16(length);
+		d->res_count = cpu_to_le16(length);
+
+		context_append(&ctx->context, d, 1, 0);
+
+		offset = (offset + length) & ~PAGE_MASK;
+		rest -= length;
+		page++;
+	}
+
+	if (packet->interrupt)
+		d->control |= cpu_to_le16(descriptor_irq_always);
+
+	return 0;
+}
+
 static int
 ohci_queue_iso(struct fw_iso_context *base,
 	       struct fw_iso_packet *packet,
@@ -1616,8 +1689,12 @@ ohci_queue_iso(struct fw_iso_context *base,
 {
 	if (base->type == FW_ISO_CONTEXT_TRANSMIT)
 		return ohci_queue_iso_transmit(base, packet, buffer, payload);
+	else if (base->header_size == 0)
+		return ohci_queue_iso_receive_bufferfill(base, packet,
+							 buffer, payload);
 	else
-		return ohci_queue_iso_receive(base, packet, buffer, payload);
+		return ohci_queue_iso_receive_dualbuffer(base, packet,
+							 buffer, payload);
 }
 
 static const struct fw_card_driver ohci_driver = {