eth1394.c

来自「Linux Kernel 2.6.9 for OMAP1710」· C语言 代码 · 共 1,839 行 · 第 1/4 页

 * speed, and unicast FIFO address information between the sender_unique_id
 * and the IP addresses.
 */
static inline void ether1394_arp_to_1394arp(struct sk_buff *skb,
					    struct net_device *dev)
{
	struct eth1394_priv *priv = (struct eth1394_priv *)(dev->priv);

	struct arphdr *arp = (struct arphdr *)skb->data;
	unsigned char *arp_ptr = (unsigned char *)(arp + 1);
	struct eth1394_arp *arp1394 = (struct eth1394_arp *)skb->data;

	/* Believe it or not, all that need to happen is sender IP get moved
	 * and set hw_addr_len, max_rec, sspd, fifo_hi and fifo_lo.  */
	arp1394->hw_addr_len	= 16;
	arp1394->sip		= *(u32*)(arp_ptr + ETH1394_ALEN);
	arp1394->max_rec	= priv->host->csr.max_rec;
	arp1394->sspd		= priv->host->csr.lnk_spd;
	arp1394->fifo_hi	= htons (priv->local_fifo >> 32);
	arp1394->fifo_lo	= htonl (priv->local_fifo & ~0x0);

	return;
}

/* We need to encapsulate the standard header with our own. We use the
 * ethernet header's proto for our own. */
static inline unsigned int ether1394_encapsulate_prep(unsigned int max_payload,
						      int proto,
						      union eth1394_hdr *hdr,
						      u16 dg_size, u16 dgl)
{
	unsigned int adj_max_payload = max_payload - hdr_type_len[ETH1394_HDR_LF_UF];

	/* Does it all fit in one packet? */
	if (dg_size <= adj_max_payload) {
		hdr->uf.lf = ETH1394_HDR_LF_UF;
		hdr->uf.ether_type = proto;
	} else {
		hdr->ff.lf = ETH1394_HDR_LF_FF;
		hdr->ff.ether_type = proto;
		hdr->ff.dg_size = dg_size - 1;
		hdr->ff.dgl = dgl;
		adj_max_payload = max_payload - hdr_type_len[ETH1394_HDR_LF_FF];
	}
	return((dg_size + (adj_max_payload - 1)) / adj_max_payload);
}

static inline unsigned int ether1394_encapsulate(struct sk_buff *skb,
						 unsigned int max_payload,
						 union eth1394_hdr *hdr)
{
	union eth1394_hdr *bufhdr;
	int ftype = hdr->common.lf;
	int hdrsz = hdr_type_len[ftype];
	unsigned int adj_max_payload = max_payload - hdrsz;

	switch(ftype) {
	case ETH1394_HDR_LF_UF:
		bufhdr = (union eth1394_hdr *)skb_push(skb, hdrsz);
		bufhdr->words.word1 = htons(hdr->words.word1);
		bufhdr->words.word2 = hdr->words.word2;
		break;

	case ETH1394_HDR_LF_FF:
		bufhdr = (union eth1394_hdr *)skb_push(skb, hdrsz);
		bufhdr->words.word1 = htons(hdr->words.word1);
		bufhdr->words.word2 = hdr->words.word2;
		bufhdr->words.word3 = htons(hdr->words.word3);
		bufhdr->words.word4 = 0;

		/* Set frag type here for future interior fragments */
		hdr->common.lf = ETH1394_HDR_LF_IF;
		hdr->sf.fg_off = 0;
		break;

	default:
		hdr->sf.fg_off += adj_max_payload;
		bufhdr = (union eth1394_hdr *)skb_pull(skb, adj_max_payload);
		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 = hpsb_alloc_packet(0);
	if (p) {
		p->host = host;
		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 + 3) & ~3;
	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] = __constant_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->bc_sspd;
}

static inline void ether1394_free_packet(struct hpsb_packet *packet)
{
	if (packet->tcode != TCODE_STREAM_DATA)
		hpsb_free_tlabel(packet);
	hpsb_free_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)) {
		hpsb_free_packet(packet);
		return -1;
	}

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

	if (hpsb_send_packet(packet) < 0) {
		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 = 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 = 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 eth1394_node_ref *node;
	struct eth1394_node_info *node_info = NULL;

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

	/* XXX Ignore this for now. Noticed that when MacOSX is the IRM,
	 * it does not set our validity bit. We need to compensate for
	 * that somewhere else, but not in eth1394. */
#if 0
	if ((priv->host->csr.broadcast_channel & 0xc0000000) != 0xc0000000) {
		ret = -EAGAIN;
		goto fail;
	}
#endif

	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);

	proto = eth->h_proto;
	dg_size = skb->len;

	/* 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;
		dest_node = LOCAL_BUS | ALL_NODES;
		max_payload = priv->bc_maxpayload - ETHER1394_GASP_OVERHEAD;
		BUG_ON(max_payload < (512 - ETHER1394_GASP_OVERHEAD));
		dgl = priv->bc_dgl;
		if (max_payload < dg_size + hdr_type_len[ETH1394_HDR_LF_UF])
			priv->bc_dgl++;
	} else {
		node = eth1394_find_node_guid(&priv->ip_node_list,
					      be64_to_cpu(*(u64*)eth->h_dest));
		if (!node) {
			ret = -EAGAIN;
			goto fail;
		}
		node_info = (struct eth1394_node_info*)node->ud->device.driver_data;
		if (node_info->fifo == ETHER1394_INVALID_ADDR) {
			ret = -EAGAIN;
			goto fail;
		}

		dest_node = node->ud->ne->nodeid;
		max_payload = node_info->maxpayload;
		BUG_ON(max_payload < (512 - ETHER1394_GASP_OVERHEAD));

		dgl = node_info->dgl;
		if (max_payload < dg_size + hdr_type_len[ETH1394_HDR_LF_UF])
			node_info->dgl++;
		tx_type = ETH1394_WRREQ;
	}

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

	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;

		spin_lock_irqsave(&priv->lock, flags);
		addr = node_info->fifo;
		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, 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 __user *useraddr)
{
	u32 ethcmd;

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

	switch (ethcmd) {
		case ETHTOOL_GDRVINFO: {
			struct ethtool_drvinfo info = { ETHTOOL_GDRVINFO };
			strcpy (info.driver, driver_name);
			strcpy (info.version, "$Rev: 1224 $");
			/* 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;
}


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);

	return hpsb_register_protocol(&eth1394_proto_driver);
}

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

module_init(ether1394_init_module);
module_exit(ether1394_exit_module);