eth1394.c

IEE1394 火线接口驱动 for linux

		if (max_payload >= skb->len)
			hdr->common.lf = ETH1394_HDR_LF_LF;
		bufhdr->words.word1 = htons(hdr->words.word1);
		bufhdr->words.word2 = htons(hdr->words.word2);
		bufhdr->words.word3 = htons(hdr->words.word3);
		bufhdr->words.word4 = 0;
	}

	return min(max_payload, skb->len);
}

static inline struct hpsb_packet *ether1394_alloc_common_packet(struct hpsb_host *host)
{
	struct hpsb_packet *p;

	p = alloc_hpsb_packet(0);
	if (p) {
		p->host = host;
		p->data = NULL;
		p->generation = get_hpsb_generation(host);
		p->type = hpsb_async;
	}
	return p;
}

static inline int ether1394_prep_write_packet(struct hpsb_packet *p,
					      struct hpsb_host *host,
					      nodeid_t node, u64 addr,
					      void * data, int tx_len)
{
	p->node_id = node;
	p->data = NULL;

	p->tcode = TCODE_WRITEB;
	p->header[1] = (host->node_id << 16) | (addr >> 32);
	p->header[2] = addr & 0xffffffff;

	p->header_size = 16;
	p->expect_response = 1;

	if (hpsb_get_tlabel(p)) {
		ETH1394_PRINT_G(KERN_ERR, "No more tlabels left while sending "
				"to node " NODE_BUS_FMT "\n", NODE_BUS_ARGS(host, node));
		return -1;
	}		
	p->header[0] = (p->node_id << 16) | (p->tlabel << 10)
		| (1 << 8) | (TCODE_WRITEB << 4);

	p->header[3] = tx_len << 16;
	p->data_size = tx_len + (tx_len % 4 ? 4 - (tx_len % 4) : 0);
	p->data = (quadlet_t*)data;

	return 0;
}

static inline void ether1394_prep_gasp_packet(struct hpsb_packet *p,
					      struct eth1394_priv *priv,
					      struct sk_buff *skb, int length)
{
	p->header_size = 4;
	p->tcode = TCODE_STREAM_DATA;

	p->header[0] = (length << 16) | (3 << 14)
		| ((priv->broadcast_channel) << 8)
		| (TCODE_STREAM_DATA << 4);
	p->data_size = length;
	p->data = ((quadlet_t*)skb->data) - 2;
	p->data[0] = cpu_to_be32((priv->host->node_id << 16) |
				      ETHER1394_GASP_SPECIFIER_ID_HI);
	p->data[1] = cpu_to_be32((ETHER1394_GASP_SPECIFIER_ID_LO << 24) |
				      ETHER1394_GASP_VERSION);

	/* Setting the node id to ALL_NODES (not LOCAL_BUS | ALL_NODES)
	 * prevents hpsb_send_packet() from setting the speed to an arbitrary
	 * value based on packet->node_id if packet->node_id is not set. */
	p->node_id = ALL_NODES;
	p->speed_code = priv->sspd[ALL_NODES];
}

static inline void ether1394_free_packet(struct hpsb_packet *packet)
{
	if (packet->tcode != TCODE_STREAM_DATA)
		hpsb_free_tlabel(packet);
	packet->data = NULL;
	free_hpsb_packet(packet);
}

static void ether1394_complete_cb(void *__ptask);

static int ether1394_send_packet(struct packet_task *ptask, unsigned int tx_len)
{
	struct eth1394_priv *priv = ptask->priv;
	struct hpsb_packet *packet = NULL;

	packet = ether1394_alloc_common_packet(priv->host);
	if (!packet)
		return -1;

	if (ptask->tx_type == ETH1394_GASP) {
		int length = tx_len + (2 * sizeof(quadlet_t));

		ether1394_prep_gasp_packet(packet, priv, ptask->skb, length);
	} else if (ether1394_prep_write_packet(packet, priv->host,
					       ptask->dest_node,
					       ptask->addr, ptask->skb->data,
					       tx_len)) {
		free_hpsb_packet(packet);
		return -1;
	}

	ptask->packet = packet;
	hpsb_set_packet_complete_task(ptask->packet, ether1394_complete_cb,
				      ptask);

	if (!hpsb_send_packet(packet)) {
		ether1394_free_packet(packet);
		return -1;
	}

	return 0;
}


/* Task function to be run when a datagram transmission is completed */
static inline void ether1394_dg_complete(struct packet_task *ptask, int fail)
{
	struct sk_buff *skb = ptask->skb;
	struct net_device *dev = skb->dev;
	struct eth1394_priv *priv = (struct eth1394_priv *)dev->priv;
        unsigned long flags;
		
	/* Statistics */
	spin_lock_irqsave(&priv->lock, flags);
	if (fail) {
		priv->stats.tx_dropped++;
		priv->stats.tx_errors++;
	} else {
		priv->stats.tx_bytes += skb->len;
		priv->stats.tx_packets++;
	}
	spin_unlock_irqrestore(&priv->lock, flags);

	dev_kfree_skb_any(skb);
	kmem_cache_free(packet_task_cache, ptask);
}


/* Callback for when a packet has been sent and the status of that packet is
 * known */
static void ether1394_complete_cb(void *__ptask)
{
	struct packet_task *ptask = (struct packet_task *)__ptask;
	struct hpsb_packet *packet = ptask->packet;
	int fail = 0;

	if (packet->tcode != TCODE_STREAM_DATA)
		fail = hpsb_packet_success(packet);

	ether1394_free_packet(packet);

	ptask->outstanding_pkts--;
	if (ptask->outstanding_pkts > 0 && !fail)
	{
		int tx_len;

		/* Add the encapsulation header to the fragment */
		tx_len = ether1394_encapsulate(ptask->skb, ptask->max_payload,
					       &ptask->hdr);
		if (ether1394_send_packet(ptask, tx_len))
			ether1394_dg_complete(ptask, 1);
	} else {
		ether1394_dg_complete(ptask, fail);
	}
}



/* Transmit a packet (called by kernel) */
static int ether1394_tx (struct sk_buff *skb, struct net_device *dev)
{
	int kmflags = in_interrupt() ? GFP_ATOMIC : GFP_KERNEL;
	struct eth1394hdr *eth;
	struct eth1394_priv *priv = (struct eth1394_priv *)dev->priv;
	int proto;
	unsigned long flags;
	nodeid_t dest_node;
	eth1394_tx_type tx_type;
	int ret = 0;
	unsigned int tx_len;
	unsigned int max_payload;
	u16 dg_size;
	u16 dgl;
	struct packet_task *ptask;
	struct node_entry *ne;

	ptask = kmem_cache_alloc(packet_task_cache, kmflags);
	if (ptask == NULL) {
		ret = -ENOMEM;
		goto fail;
	}

	spin_lock_irqsave (&priv->lock, flags);
	if (priv->bc_state == ETHER1394_BC_CLOSED) {
		ETH1394_PRINT(KERN_ERR, dev->name,
			      "Cannot send packet, no broadcast channel available.\n");
		ret = -EAGAIN;
		spin_unlock_irqrestore (&priv->lock, flags);
		goto fail;
	}

	if ((ret = ether1394_init_bc(dev))) {
		spin_unlock_irqrestore (&priv->lock, flags);
		goto fail;
	}

	spin_unlock_irqrestore (&priv->lock, flags);

	if ((skb = skb_share_check (skb, kmflags)) == NULL) {
		ret = -ENOMEM;
		goto fail;
	}

	/* Get rid of the fake eth1394 header, but save a pointer */
	eth = (struct eth1394hdr*)skb->data;
	skb_pull(skb, ETH1394_HLEN);

	ne = hpsb_guid_get_entry(be64_to_cpu(*(u64*)eth->h_dest));
	if (!ne)
		dest_node = LOCAL_BUS | ALL_NODES;
	else
		dest_node = ne->nodeid;

	proto = eth->h_proto;

	/* If this is an ARP packet, convert it */
	if (proto == __constant_htons (ETH_P_ARP))
		ether1394_arp_to_1394arp (skb, dev);

	max_payload = priv->maxpayload[NODEID_TO_NODE(dest_node)];

	/* This check should be unnecessary, but we'll keep it for safety for
	 * a while longer. */
	if (max_payload < 512) {
		ETH1394_PRINT(KERN_WARNING, dev->name,
			      "max_payload too small: %d   (setting to 512)\n",
			      max_payload);
		max_payload = 512;
	}

	/* Set the transmission type for the packet.  ARP packets and IP
	 * broadcast packets are sent via GASP. */
	if (memcmp(eth->h_dest, dev->broadcast, ETH1394_ALEN) == 0 ||
	    proto == __constant_htons(ETH_P_ARP) ||
	    (proto == __constant_htons(ETH_P_IP) &&
	     IN_MULTICAST(__constant_ntohl(skb->nh.iph->daddr)))) {
		tx_type = ETH1394_GASP;
                max_payload -= ETHER1394_GASP_OVERHEAD;
	} else {
		tx_type = ETH1394_WRREQ;
	}

	dg_size = skb->len;

	spin_lock_irqsave (&priv->lock, flags);
	dgl = priv->dgl[NODEID_TO_NODE(dest_node)];
	if (max_payload < dg_size + hdr_type_len[ETH1394_HDR_LF_UF])
		priv->dgl[NODEID_TO_NODE(dest_node)]++;
	spin_unlock_irqrestore (&priv->lock, flags);

	ptask->hdr.words.word1 = 0;
	ptask->hdr.words.word2 = 0;
	ptask->hdr.words.word3 = 0;
	ptask->hdr.words.word4 = 0;
	ptask->skb = skb;
	ptask->priv = priv;
	ptask->tx_type = tx_type;

	if (tx_type != ETH1394_GASP) {
		u64 addr;

		/* This test is just temporary until ConfigROM support has
		 * been added to eth1394.  Until then, we need an ARP packet
		 * after a bus reset from the current destination node so that
		 * we can get FIFO information. */
		if (priv->fifo[NODEID_TO_NODE(dest_node)] == 0ULL) {
			ret = -EAGAIN;
			goto fail;
		}

		spin_lock_irqsave(&priv->lock, flags);
		addr = priv->fifo[NODEID_TO_NODE(dest_node)];
		spin_unlock_irqrestore(&priv->lock, flags);

		ptask->addr = addr;
		ptask->dest_node = dest_node;
	}

	ptask->tx_type = tx_type;
	ptask->max_payload = max_payload;
        ptask->outstanding_pkts = ether1394_encapsulate_prep(max_payload, proto,
							     &ptask->hdr, dg_size,
							     dgl);

	/* Add the encapsulation header to the fragment */
	tx_len = ether1394_encapsulate(skb, max_payload, &ptask->hdr);
	dev->trans_start = jiffies;
	if (ether1394_send_packet(ptask, tx_len))
		goto fail;

	netif_wake_queue(dev);
	return 0;
fail:
	if (ptask)
		kmem_cache_free(packet_task_cache, ptask);

	if (skb != NULL)
		dev_kfree_skb(skb);

	spin_lock_irqsave (&priv->lock, flags);
	priv->stats.tx_dropped++;
	priv->stats.tx_errors++;
	spin_unlock_irqrestore (&priv->lock, flags);

	if (netif_queue_stopped(dev))
		netif_wake_queue(dev);

	return 0;  /* returning non-zero causes serious problems */
}

static int ether1394_do_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
{
	switch(cmd) {
		case SIOCETHTOOL:
			return ether1394_ethtool_ioctl(dev, (void *) ifr->ifr_data);

		case SIOCGMIIPHY:		/* Get address of MII PHY in use. */
		case SIOCGMIIREG:		/* Read MII PHY register. */
		case SIOCSMIIREG:		/* Write MII PHY register. */
		default:
			return -EOPNOTSUPP;
	}

	return 0;
}

static int ether1394_ethtool_ioctl(struct net_device *dev, void *useraddr)
{
	u32 ethcmd;

	if (get_user(ethcmd, (u32 *)useraddr))
		return -EFAULT;

	switch (ethcmd) {
		case ETHTOOL_GDRVINFO: {
			struct ethtool_drvinfo info = { ETHTOOL_GDRVINFO };
			strcpy (info.driver, driver_name);
			strcpy (info.version, "$Rev: 1043 $");
			/* FIXME XXX provide sane businfo */
			strcpy (info.bus_info, "ieee1394");
			if (copy_to_user (useraddr, &info, sizeof (info)))
				return -EFAULT;
			break;
		}
		case ETHTOOL_GSET:
		case ETHTOOL_SSET:
		case ETHTOOL_NWAY_RST:
		case ETHTOOL_GLINK:
		case ETHTOOL_GMSGLVL:
		case ETHTOOL_SMSGLVL:
		default:
			return -EOPNOTSUPP;
	}

	return 0;
}

/* Function for incoming 1394 packets */
static struct hpsb_address_ops addr_ops = {
	.write =	ether1394_write,
};

/* Ieee1394 highlevel driver functions */
static struct hpsb_highlevel eth1394_highlevel = {
	.name =		driver_name,
	.add_host =	ether1394_add_host,
	.remove_host =	ether1394_remove_host,
	.host_reset =	ether1394_host_reset,
};

static int __init ether1394_init_module (void)
{
	packet_task_cache = kmem_cache_create("packet_task", sizeof(struct packet_task),
					      0, 0, NULL, NULL);

	/* Register ourselves as a highlevel driver */
	hpsb_register_highlevel(&eth1394_highlevel);

	hpsb_register_addrspace(&eth1394_highlevel, &addr_ops, ETHER1394_REGION_ADDR,
				 ETHER1394_REGION_ADDR_END);

	return 0;
}

static void __exit ether1394_exit_module (void)
{
	hpsb_unregister_highlevel(&eth1394_highlevel);
	kmem_cache_destroy(packet_task_cache);
}

module_init(ether1394_init_module);
module_exit(ether1394_exit_module);