static int at_context_queue_packet()

in ohci.c [1284:1412]

• 104 lines of code
• 21 McCabe index (conditional complexity)

static int at_context_queue_packet(struct context *ctx,
				   struct fw_packet *packet)
{
	struct fw_ohci *ohci = ctx->ohci;
	dma_addr_t d_bus, payload_bus;
	struct driver_data *driver_data;
	struct descriptor *d, *last;
	__le32 *header;
	int z, tcode;

	d = context_get_descriptors(ctx, 4, &d_bus);
	if (d == NULL) {
		packet->ack = RCODE_SEND_ERROR;
		return -1;
	}

	d[0].control   = cpu_to_le16(DESCRIPTOR_KEY_IMMEDIATE);
	d[0].res_count = cpu_to_le16(packet->timestamp);

	/*
	 * The DMA format for asynchronous link packets is different
	 * from the IEEE1394 layout, so shift the fields around
	 * accordingly.
	 */

	tcode = (packet->header[0] >> 4) & 0x0f;
	header = (__le32 *) &d[1];
	switch (tcode) {
	case TCODE_WRITE_QUADLET_REQUEST:
	case TCODE_WRITE_BLOCK_REQUEST:
	case TCODE_WRITE_RESPONSE:
	case TCODE_READ_QUADLET_REQUEST:
	case TCODE_READ_BLOCK_REQUEST:
	case TCODE_READ_QUADLET_RESPONSE:
	case TCODE_READ_BLOCK_RESPONSE:
	case TCODE_LOCK_REQUEST:
	case TCODE_LOCK_RESPONSE:
		header[0] = cpu_to_le32((packet->header[0] & 0xffff) |
					(packet->speed << 16));
		header[1] = cpu_to_le32((packet->header[1] & 0xffff) |
					(packet->header[0] & 0xffff0000));
		header[2] = cpu_to_le32(packet->header[2]);

		if (TCODE_IS_BLOCK_PACKET(tcode))
			header[3] = cpu_to_le32(packet->header[3]);
		else
			header[3] = (__force __le32) packet->header[3];

		d[0].req_count = cpu_to_le16(packet->header_length);
		break;

	case TCODE_LINK_INTERNAL:
		header[0] = cpu_to_le32((OHCI1394_phy_tcode << 4) |
					(packet->speed << 16));
		header[1] = cpu_to_le32(packet->header[1]);
		header[2] = cpu_to_le32(packet->header[2]);
		d[0].req_count = cpu_to_le16(12);

		if (is_ping_packet(&packet->header[1]))
			d[0].control |= cpu_to_le16(DESCRIPTOR_PING);
		break;

	case TCODE_STREAM_DATA:
		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;
	}

	BUILD_BUG_ON(sizeof(struct driver_data) > sizeof(struct descriptor));
	driver_data = (struct driver_data *) &d[3];
	driver_data->packet = packet;
	packet->driver_data = driver_data;

	if (packet->payload_length > 0) {
		if (packet->payload_length > sizeof(driver_data->inline_data)) {
			payload_bus = dma_map_single(ohci->card.device,
						     packet->payload,
						     packet->payload_length,
						     DMA_TO_DEVICE);
			if (dma_mapping_error(ohci->card.device, payload_bus)) {
				packet->ack = RCODE_SEND_ERROR;
				return -1;
			}
			packet->payload_bus	= payload_bus;
			packet->payload_mapped	= true;
		} else {
			memcpy(driver_data->inline_data, packet->payload,
			       packet->payload_length);
			payload_bus = d_bus + 3 * sizeof(*d);
		}

		d[2].req_count    = cpu_to_le16(packet->payload_length);
		d[2].data_address = cpu_to_le32(payload_bus);
		last = &d[2];
		z = 3;
	} else {
		last = &d[0];
		z = 2;
	}

	last->control |= cpu_to_le16(DESCRIPTOR_OUTPUT_LAST |
				     DESCRIPTOR_IRQ_ALWAYS |
				     DESCRIPTOR_BRANCH_ALWAYS);

	/* FIXME: Document how the locking works. */
	if (ohci->generation != packet->generation) {
		if (packet->payload_mapped)
			dma_unmap_single(ohci->card.device, payload_bus,
					 packet->payload_length, DMA_TO_DEVICE);
		packet->ack = RCODE_GENERATION;
		return -1;
	}

	context_append(ctx, d, z, 4 - z);

	if (ctx->running)
		reg_write(ohci, CONTROL_SET(ctx->regs), CONTEXT_WAKE);
	else
		context_run(ctx, 0);

	return 0;
}