ieee1394_core.c

1394在linux下单独的驱动程序代码

* Return value: 0 for success or error number otherwise.
*/
int hpsb_send_phy_config(struct hpsb_host *host, int rootid, int gapcnt)
{
	struct hpsb_packet *packet;
	int retval = 0;
	
	if (rootid >= ALL_NODES || rootid < -1 || gapcnt > 0x3f || gapcnt < -1 ||
		(rootid == -1 && gapcnt == -1)) {
		HPSB_DEBUG("Invalid Parameter: rootid = %d   gapcnt = %d",
			rootid, gapcnt);
		return -EINVAL;
	}
	
	packet = alloc_hpsb_packet(0);
	if (!packet)
		return -ENOMEM;
	
	packet->host = host;
	packet->header_size = 8;
	packet->data_size = 0;
	packet->expect_response = 0;
	packet->no_waiter = 0;
	packet->type = hpsb_raw;
	packet->header[0] = 0;
	if (rootid != -1)
		packet->header[0] |= rootid << 24 | 1 << 23;
	if (gapcnt != -1)
		packet->header[0] |= gapcnt << 16 | 1 << 22;
	
	packet->header[1] = ~packet->header[0];
	
	packet->generation = get_hpsb_generation(host);
	
	if (!hpsb_send_packet(packet)) {
		retval = -EINVAL;
		goto fail;
	}
	
	down(&packet->state_change);
	down(&packet->state_change);
	
fail:
	free_hpsb_packet(packet);
	
	return retval;
}

/**
* hpsb_send_packet - transmit a packet on the bus
* @packet: packet to send
*
* The packet is sent through the host specified in the packet->host field.
* Before sending, the packet's transmit speed is automatically determined using
* the local speed map when it is an async, non-broadcast packet.
*
* Possibilities for failure are that host is either not initialized, in bus
* reset, the packet's generation number doesn't match the current generation
* number or the host reports a transmit error.
*
* Return value: False (0) on failure, true (1) otherwise.
*/
int hpsb_send_packet(struct hpsb_packet *packet)
{
	struct hpsb_host *host = packet->host;
	
	if (host->is_shutdown || host->in_bus_reset
		|| (packet->generation != get_hpsb_generation(host))) {
		return 0;
	}
	
	packet->state = hpsb_queued;
	
	if (packet->node_id == host->node_id)
	{ /* it is a local request, so handle it locally */
		quadlet_t *data;
		size_t size=packet->data_size+packet->header_size;
		
		data = kmalloc(packet->header_size + packet->data_size, GFP_ATOMIC);
		if (!data) {
			HPSB_ERR("unable to allocate memory for concatenating header and data");
			return 0;
		}
		
		memcpy(data, packet->header, packet->header_size);
		
		if (packet->data_size)
		{
			if (packet->data_be) {
				memcpy(((u8*)data)+packet->header_size, packet->data, packet->data_size);
			} else {
				int i;
				quadlet_t *my_data=(quadlet_t*) ((u8*) data + packet->data_size);
				for (i=0; i < packet->data_size/4; i++) {
					my_data[i] = cpu_to_be32(packet->data[i]);
				}
			}
		}
		
		dump_packet("send packet local:", packet->header,
			packet->header_size);
		
		hpsb_packet_sent(host, packet,  packet->expect_response?ACK_PENDING:ACK_COMPLETE);
		hpsb_packet_received(host, data, size, 0);
		
		kfree(data);
		
		return 1;
	}
	
	if (packet->type == hpsb_async && packet->node_id != ALL_NODES) {
		packet->speed_code =
			host->speed_map[NODEID_TO_NODE(host->node_id) * 64
			+ NODEID_TO_NODE(packet->node_id)];
	}
	
	switch (packet->speed_code) {
	case 2:
		dump_packet("send packet 400:", packet->header,
			packet->header_size);
		break;
	case 1:
		dump_packet("send packet 200:", packet->header,
			packet->header_size);
		break;
	default:
		dump_packet("send packet 100:", packet->header,
			packet->header_size);
	}
	
	return host->driver->transmit_packet(host, packet);
}

static void send_packet_nocare(struct hpsb_packet *packet)
{
	if (!hpsb_send_packet(packet)) {
		free_hpsb_packet(packet);
	}
}


void handle_packet_response(struct hpsb_host *host, int tcode, quadlet_t *data,
                            size_t size)
{
	struct hpsb_packet *packet = NULL;
	struct list_head *lh;
	int tcode_match = 0;
	int tlabel;
	unsigned long flags;
	
	tlabel = (data[0] >> 10) & 0x3f;
	
	spin_lock_irqsave(&host->pending_pkt_lock, flags);
	
	list_for_each(lh, &host->pending_packets) {
		packet = list_entry(lh, struct hpsb_packet, list);
		if ((packet->tlabel == tlabel)
			&& (packet->node_id == (data[1] >> 16))){
			break;
		}
	}
	
	if (lh == &host->pending_packets) {
		HPSB_DEBUG("unsolicited response packet received - no tlabel match");
		dump_packet("contents:", data, 16);
		spin_unlock_irqrestore(&host->pending_pkt_lock, flags);
		return;
	}
	
	switch (packet->tcode) {
	case TCODE_WRITEQ:
	case TCODE_WRITEB:
		if (tcode == TCODE_WRITE_RESPONSE) tcode_match = 1;
		break;
	case TCODE_READQ:
		if (tcode == TCODE_READQ_RESPONSE) tcode_match = 1;
		break;
	case TCODE_READB:
		if (tcode == TCODE_READB_RESPONSE) tcode_match = 1;
		break;
	case TCODE_LOCK_REQUEST:
		if (tcode == TCODE_LOCK_RESPONSE) tcode_match = 1;
		break;
	}
	
	if (!tcode_match || (packet->tlabel != tlabel)
		|| (packet->node_id != (data[1] >> 16))) {
		HPSB_INFO("unsolicited response packet received - tcode mismatch");
		dump_packet("contents:", data, 16);
		
		spin_unlock_irqrestore(&host->pending_pkt_lock, flags);
		return;
	}
	
	list_del(&packet->list);
	
	spin_unlock_irqrestore(&host->pending_pkt_lock, flags);
	
	/* FIXME - update size fields? */
	switch (tcode) {
	case TCODE_WRITE_RESPONSE:
		memcpy(packet->header, data, 12);
		break;
	case TCODE_READQ_RESPONSE:
		memcpy(packet->header, data, 16);
		break;
	case TCODE_READB_RESPONSE:
		memcpy(packet->header, data, 16);
		memcpy(packet->data, data + 4, size - 16);
		break;
	case TCODE_LOCK_RESPONSE:
		memcpy(packet->header, data, 16);
		memcpy(packet->data, data + 4, (size - 16) > 8 ? 8 : size - 16);
		break;
	}
	
	packet->state = hpsb_complete;
	up(&packet->state_change);
	run_packet_complete(packet);
}


static struct hpsb_packet *create_reply_packet(struct hpsb_host *host,
											   quadlet_t *data, size_t dsize)
{
	struct hpsb_packet *p;
	
	dsize += (dsize % 4 ? 4 - (dsize % 4) : 0);
	
	p = alloc_hpsb_packet(dsize);
	if (p == NULL) {
		/* FIXME - send data_error response */
		return NULL;
	}
	
	p->type = hpsb_async;
	p->state = hpsb_unused;
	p->host = host;
	p->node_id = data[1] >> 16;
	p->tlabel = (data[0] >> 10) & 0x3f;
	p->no_waiter = 1;
	
	p->generation = get_hpsb_generation(host);
	
	if (dsize % 4) {
		p->data[dsize / 4] = 0;
	}
	
	return p;
}

#define PREP_ASYNC_HEAD_RCODE(tc) \
	packet->tcode = tc; \
	packet->header[0] = (packet->node_id << 16) | (packet->tlabel << 10) \
	| (1 << 8) | (tc << 4); \
	packet->header[1] = (packet->host->node_id << 16) | (rcode << 12); \
packet->header[2] = 0

static void fill_async_readquad_resp(struct hpsb_packet *packet, int rcode,
									 quadlet_t data)
{
	PREP_ASYNC_HEAD_RCODE(TCODE_READQ_RESPONSE);
	packet->header[3] = data;
	packet->header_size = 16;
	packet->data_size = 0;
}

static void fill_async_readblock_resp(struct hpsb_packet *packet, int rcode,
									  int length)
{
	if (rcode != RCODE_COMPLETE)
		length = 0;
	
	PREP_ASYNC_HEAD_RCODE(TCODE_READB_RESPONSE);
	packet->header[3] = length << 16;
	packet->header_size = 16;
	packet->data_size = length + (length % 4 ? 4 - (length % 4) : 0);
}

static void fill_async_write_resp(struct hpsb_packet *packet, int rcode)
{
	PREP_ASYNC_HEAD_RCODE(TCODE_WRITE_RESPONSE);
	packet->header[2] = 0;
	packet->header_size = 12;
	packet->data_size = 0;
}

static void fill_async_lock_resp(struct hpsb_packet *packet, int rcode, int extcode,
								 int length)
{
	if (rcode != RCODE_COMPLETE)
		length = 0;
	
	PREP_ASYNC_HEAD_RCODE(TCODE_LOCK_RESPONSE);
	packet->header[3] = (length << 16) | extcode;
	packet->header_size = 16;
	packet->data_size = length;
}

#define PREP_REPLY_PACKET(length) \
	packet = create_reply_packet(host, data, length); \
if (packet == NULL) break

static void handle_incoming_packet(struct hpsb_host *host, int tcode,
								   quadlet_t *data, size_t size, int write_acked)
{
	struct hpsb_packet *packet;
	int length, rcode, extcode;
	quadlet_t buffer;
	nodeid_t source = data[1] >> 16;
	nodeid_t dest = data[0] >> 16;
	u16 flags = (u16) data[0];
	u64 addr;
	
	/* big FIXME - no error checking is done for an out of bounds length */
	
	switch (tcode) {
	case TCODE_WRITEQ:
		addr = (((u64)(data[1] & 0xffff)) << 32) | data[2];
		rcode = highlevel_write(host, source, dest, data+3,
			addr, 4, flags);
		
		if (!write_acked
			&& (NODEID_TO_NODE(data[0] >> 16) != NODE_MASK)
			&& (rcode >= 0)) {
			/* not a broadcast write, reply */
			PREP_REPLY_PACKET(0);
			fill_async_write_resp(packet, rcode);
			send_packet_nocare(packet);
		}
		break;
		
	case TCODE_WRITEB:
		addr = (((u64)(data[1] & 0xffff)) << 32) | data[2];
		rcode = highlevel_write(host, source, dest, data+4,
			addr, data[3]>>16, flags);
		
		if (!write_acked
			&& (NODEID_TO_NODE(data[0] >> 16) != NODE_MASK)
			&& (rcode >= 0)) {
			/* not a broadcast write, reply */
			PREP_REPLY_PACKET(0);
			fill_async_write_resp(packet, rcode);
			send_packet_nocare(packet);
		}
		break;
		
	case TCODE_READQ:
		addr = (((u64)(data[1] & 0xffff)) << 32) | data[2];
		rcode = highlevel_read(host, source, &buffer, addr, 4, flags);
		
		if (rcode >= 0) {
			PREP_REPLY_PACKET(0);
			fill_async_readquad_resp(packet, rcode, buffer);
			send_packet_nocare(packet);
		}
		break;
		
	case TCODE_READB:
		length = data[3] >> 16;
		PREP_REPLY_PACKET(length);
		
		addr = (((u64)(data[1] & 0xffff)) << 32) | data[2];
		rcode = highlevel_read(host, source, packet->data, addr,
			length, flags);
		
		if (rcode >= 0) {
			fill_async_readblock_resp(packet, rcode, length);
			send_packet_nocare(packet);
		} else {
			free_hpsb_packet(packet);
		}
		break;
		
	case TCODE_LOCK_REQUEST:
		length = data[3] >> 16;
		extcode = data[3] & 0xffff;
		addr = (((u64)(data[1] & 0xffff)) << 32) | data[2];
		
		PREP_REPLY_PACKET(8);
		
		if ((extcode == 0) || (extcode >= 7)) {
			/* let switch default handle error */
			length = 0;
		}
		
		switch (length) {
		case 4:
			rcode = highlevel_lock(host, source, packet->data, addr,
				data[4], 0, extcode,flags);
			fill_async_lock_resp(packet, rcode, extcode, 4);
			break;
		case 8:
			if ((extcode != EXTCODE_FETCH_ADD) 
				&& (extcode != EXTCODE_LITTLE_ADD)) {
				rcode = highlevel_lock(host, source,
					packet->data, addr,
					data[5], data[4], 
					extcode, flags);
				fill_async_lock_resp(packet, rcode, extcode, 4);
			} else {
				rcode = highlevel_lock64(host, source,
					(octlet_t *)packet->data, addr,
					*(octlet_t *)(data + 4), 0ULL,
					extcode, flags);
				fill_async_lock_resp(packet, rcode, extcode, 8);
			}
			break;
		case 16:
			rcode = highlevel_lock64(host, source,
				(octlet_t *)packet->data, addr,
				*(octlet_t *)(data + 6),
				*(octlet_t *)(data + 4), 
				extcode, flags);
			fill_async_lock_resp(packet, rcode, extcode, 8);
			break;
		default:
			rcode = RCODE_TYPE_ERROR;
			fill_async_lock_resp(packet, rcode,
				extcode, 0);
		}
		
		if (rcode >= 0) {
			send_packet_nocare(packet);
		} else {
			free_hpsb_packet(packet);
		}
		break;
        }
		
}
#undef PREP_REPLY_PACKET


void hpsb_packet_received(struct hpsb_host *host, quadlet_t *data, size_t size,
                          int write_acked)
{
	int tcode;
	
	if (host->in_bus_reset) {
		HPSB_INFO("received packet during reset; ignoring");
		return;
	}
	
	dump_packet("received packet:", data, size);
	
	tcode = (data[0] >> 4) & 0xf;
	
	switch (tcode) {
	case TCODE_WRITE_RESPONSE:
	case TCODE_READQ_RESPONSE:
	case TCODE_READB_RESPONSE: