zd_mac.c

linux内核源码

	return 0;
}

static int zd_mac_tx(struct zd_mac *mac, struct ieee80211_txb *txb, int pri)
{
	int i, r;
	struct ieee80211_device *ieee = zd_mac_to_ieee80211(mac);

	for (i = 0; i < txb->nr_frags; i++) {
		struct sk_buff *skb = txb->fragments[i];

		r = fill_ctrlset(mac, txb, i);
		if (r) {
			ieee->stats.tx_dropped++;
			return r;
		}
		r = zd_usb_tx(&mac->chip.usb, skb->data, skb->len);
		if (r) {
			ieee->stats.tx_dropped++;
			return r;
		}
	}

	/* FIXME: shouldn't this be handled by the upper layers? */
	mac->netdev->trans_start = jiffies;

	ieee80211_txb_free(txb);
	return 0;
}

struct zd_rt_hdr {
	struct ieee80211_radiotap_header rt_hdr;
	u8  rt_flags;
	u8  rt_rate;
	u16 rt_channel;
	u16 rt_chbitmask;
} __attribute__((packed));

static void fill_rt_header(void *buffer, struct zd_mac *mac,
	                   const struct ieee80211_rx_stats *stats,
			   const struct rx_status *status)
{
	struct zd_rt_hdr *hdr = buffer;

	hdr->rt_hdr.it_version = PKTHDR_RADIOTAP_VERSION;
	hdr->rt_hdr.it_pad = 0;
	hdr->rt_hdr.it_len = cpu_to_le16(sizeof(struct zd_rt_hdr));
	hdr->rt_hdr.it_present = cpu_to_le32((1 << IEEE80211_RADIOTAP_FLAGS) |
		                 (1 << IEEE80211_RADIOTAP_CHANNEL) |
				 (1 << IEEE80211_RADIOTAP_RATE));

	hdr->rt_flags = 0;
	if (status->decryption_type & (ZD_RX_WEP64|ZD_RX_WEP128|ZD_RX_WEP256))
		hdr->rt_flags |= IEEE80211_RADIOTAP_F_WEP;

	hdr->rt_rate = stats->rate / 5;

	/* FIXME: 802.11a */
	hdr->rt_channel = cpu_to_le16(ieee80211chan2mhz(
		                             _zd_chip_get_channel(&mac->chip)));
	hdr->rt_chbitmask = cpu_to_le16(IEEE80211_CHAN_2GHZ |
		((status->frame_status & ZD_RX_FRAME_MODULATION_MASK) ==
		ZD_RX_OFDM ? IEEE80211_CHAN_OFDM : IEEE80211_CHAN_CCK));
}

/* Returns 1 if the data packet is for us and 0 otherwise. */
static int is_data_packet_for_us(struct ieee80211_device *ieee,
	                         struct ieee80211_hdr_4addr *hdr)
{
	struct net_device *netdev = ieee->dev;
	u16 fc = le16_to_cpu(hdr->frame_ctl);

	ZD_ASSERT(WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA);

	switch (ieee->iw_mode) {
	case IW_MODE_ADHOC:
		if ((fc & (IEEE80211_FCTL_TODS|IEEE80211_FCTL_FROMDS)) != 0 ||
		    compare_ether_addr(hdr->addr3, ieee->bssid) != 0)
			return 0;
		break;
	case IW_MODE_AUTO:
	case IW_MODE_INFRA:
		if ((fc & (IEEE80211_FCTL_TODS|IEEE80211_FCTL_FROMDS)) !=
		    IEEE80211_FCTL_FROMDS ||
		    compare_ether_addr(hdr->addr2, ieee->bssid) != 0)
			return 0;
		break;
	default:
		ZD_ASSERT(ieee->iw_mode != IW_MODE_MONITOR);
		return 0;
	}

	return compare_ether_addr(hdr->addr1, netdev->dev_addr) == 0 ||
	       (is_multicast_ether_addr(hdr->addr1) &&
		compare_ether_addr(hdr->addr3, netdev->dev_addr) != 0) ||
	       (netdev->flags & IFF_PROMISC);
}

/* Filters received packets. The function returns 1 if the packet should be
 * forwarded to ieee80211_rx(). If the packet should be ignored the function
 * returns 0. If an invalid packet is found the function returns -EINVAL.
 *
 * The function calls ieee80211_rx_mgt() directly.
 *
 * It has been based on ieee80211_rx_any.
 */
static int filter_rx(struct ieee80211_device *ieee,
	             const u8 *buffer, unsigned int length,
		     struct ieee80211_rx_stats *stats)
{
	struct ieee80211_hdr_4addr *hdr;
	u16 fc;

	if (ieee->iw_mode == IW_MODE_MONITOR)
		return 1;

	hdr = (struct ieee80211_hdr_4addr *)buffer;
	fc = le16_to_cpu(hdr->frame_ctl);
	if ((fc & IEEE80211_FCTL_VERS) != 0)
		return -EINVAL;

	switch (WLAN_FC_GET_TYPE(fc)) {
	case IEEE80211_FTYPE_MGMT:
		if (length < sizeof(struct ieee80211_hdr_3addr))
			return -EINVAL;
		ieee80211_rx_mgt(ieee, hdr, stats);
		return 0;
	case IEEE80211_FTYPE_CTL:
		return 0;
	case IEEE80211_FTYPE_DATA:
		/* Ignore invalid short buffers */
		if (length < sizeof(struct ieee80211_hdr_3addr))
			return -EINVAL;
		return is_data_packet_for_us(ieee, hdr);
	}

	return -EINVAL;
}

static void update_qual_rssi(struct zd_mac *mac,
			     const u8 *buffer, unsigned int length,
			     u8 qual_percent, u8 rssi_percent)
{
	unsigned long flags;
	struct ieee80211_hdr_3addr *hdr;
	int i;

	hdr = (struct ieee80211_hdr_3addr *)buffer;
	if (length < offsetof(struct ieee80211_hdr_3addr, addr3))
		return;
	if (compare_ether_addr(hdr->addr2, zd_mac_to_ieee80211(mac)->bssid) != 0)
		return;

	spin_lock_irqsave(&mac->lock, flags);
	i = mac->stats_count % ZD_MAC_STATS_BUFFER_SIZE;
	mac->qual_buffer[i] = qual_percent;
	mac->rssi_buffer[i] = rssi_percent;
	mac->stats_count++;
	spin_unlock_irqrestore(&mac->lock, flags);
}

static int fill_rx_stats(struct ieee80211_rx_stats *stats,
	                 const struct rx_status **pstatus,
		         struct zd_mac *mac,
			 const u8 *buffer, unsigned int length)
{
	const struct rx_status *status;

	*pstatus = status = (struct rx_status *)
		(buffer + (length - sizeof(struct rx_status)));
	if (status->frame_status & ZD_RX_ERROR) {
		struct ieee80211_device *ieee = zd_mac_to_ieee80211(mac);
		ieee->stats.rx_errors++;
		if (status->frame_status & ZD_RX_TIMEOUT_ERROR)
			ieee->stats.rx_missed_errors++;
		else if (status->frame_status & ZD_RX_FIFO_OVERRUN_ERROR)
			ieee->stats.rx_fifo_errors++;
		else if (status->frame_status & ZD_RX_DECRYPTION_ERROR)
			ieee->ieee_stats.rx_discards_undecryptable++;
		else if (status->frame_status & ZD_RX_CRC32_ERROR) {
			ieee->stats.rx_crc_errors++;
			ieee->ieee_stats.rx_fcs_errors++;
		}
		else if (status->frame_status & ZD_RX_CRC16_ERROR)
			ieee->stats.rx_crc_errors++;
		return -EINVAL;
	}

	memset(stats, 0, sizeof(struct ieee80211_rx_stats));
	stats->len = length - (ZD_PLCP_HEADER_SIZE + IEEE80211_FCS_LEN +
		               + sizeof(struct rx_status));
	/* FIXME: 802.11a */
	stats->freq = IEEE80211_24GHZ_BAND;
	stats->received_channel = _zd_chip_get_channel(&mac->chip);
	stats->rssi = zd_rx_strength_percent(status->signal_strength);
	stats->signal = zd_rx_qual_percent(buffer,
		                          length - sizeof(struct rx_status),
		                          status);
	stats->mask = IEEE80211_STATMASK_RSSI | IEEE80211_STATMASK_SIGNAL;
	stats->rate = zd_rx_rate(buffer, status);
	if (stats->rate)
		stats->mask |= IEEE80211_STATMASK_RATE;

	return 0;
}

static void zd_mac_rx(struct zd_mac *mac, struct sk_buff *skb)
{
	int r;
	struct ieee80211_device *ieee = zd_mac_to_ieee80211(mac);
	struct ieee80211_rx_stats stats;
	const struct rx_status *status;

	if (skb->len < ZD_PLCP_HEADER_SIZE + IEEE80211_1ADDR_LEN +
	               IEEE80211_FCS_LEN + sizeof(struct rx_status))
	{
		ieee->stats.rx_errors++;
		ieee->stats.rx_length_errors++;
		goto free_skb;
	}

	r = fill_rx_stats(&stats, &status, mac, skb->data, skb->len);
	if (r) {
		/* Only packets with rx errors are included here.
		 * The error stats have already been set in fill_rx_stats.
		 */
		goto free_skb;
	}

	__skb_pull(skb, ZD_PLCP_HEADER_SIZE);
	__skb_trim(skb, skb->len -
		        (IEEE80211_FCS_LEN + sizeof(struct rx_status)));

	ZD_ASSERT(IS_ALIGNED((unsigned long)skb->data, 4));

	update_qual_rssi(mac, skb->data, skb->len, stats.signal,
		         status->signal_strength);

	r = filter_rx(ieee, skb->data, skb->len, &stats);
	if (r <= 0) {
		if (r < 0) {
			ieee->stats.rx_errors++;
			dev_dbg_f(zd_mac_dev(mac), "Error in packet.\n");
		}
		goto free_skb;
	}

	if (ieee->iw_mode == IW_MODE_MONITOR)
		fill_rt_header(skb_push(skb, sizeof(struct zd_rt_hdr)), mac,
			       &stats, status);

	r = ieee80211_rx(ieee, skb, &stats);
	if (r)
		return;
free_skb:
	/* We are always in a soft irq. */
	dev_kfree_skb(skb);
}

static void do_rx(unsigned long mac_ptr)
{
	struct zd_mac *mac = (struct zd_mac *)mac_ptr;
	struct sk_buff *skb;

	while ((skb = skb_dequeue(&mac->rx_queue)) != NULL)
		zd_mac_rx(mac, skb);
}

int zd_mac_rx_irq(struct zd_mac *mac, const u8 *buffer, unsigned int length)
{
	struct sk_buff *skb;
	unsigned int reserved =
		ALIGN(max_t(unsigned int,
		            sizeof(struct zd_rt_hdr), ZD_PLCP_HEADER_SIZE), 4) -
		ZD_PLCP_HEADER_SIZE;

	skb = dev_alloc_skb(reserved + length);
	if (!skb) {
		struct ieee80211_device *ieee = zd_mac_to_ieee80211(mac);
		dev_warn(zd_mac_dev(mac), "Could not allocate skb.\n");
		ieee->stats.rx_dropped++;
		return -ENOMEM;
	}
	skb_reserve(skb, reserved);
	memcpy(__skb_put(skb, length), buffer, length);
	skb_queue_tail(&mac->rx_queue, skb);
	tasklet_schedule(&mac->rx_tasklet);
	return 0;
}

static int netdev_tx(struct ieee80211_txb *txb, struct net_device *netdev,
		     int pri)
{
	return zd_mac_tx(zd_netdev_mac(netdev), txb, pri);
}

static void set_security(struct net_device *netdev,
			 struct ieee80211_security *sec)
{
	struct ieee80211_device *ieee = zd_netdev_ieee80211(netdev);
	struct ieee80211_security *secinfo = &ieee->sec;
	int keyidx;

	dev_dbg_f(zd_mac_dev(zd_netdev_mac(netdev)), "\n");

	for (keyidx = 0; keyidx<WEP_KEYS; keyidx++)
		if (sec->flags & (1<<keyidx)) {
			secinfo->encode_alg[keyidx] = sec->encode_alg[keyidx];
			secinfo->key_sizes[keyidx] = sec->key_sizes[keyidx];
			memcpy(secinfo->keys[keyidx], sec->keys[keyidx],
			       SCM_KEY_LEN);
		}

	if (sec->flags & SEC_ACTIVE_KEY) {
		secinfo->active_key = sec->active_key;
		dev_dbg_f(zd_mac_dev(zd_netdev_mac(netdev)),
			"   .active_key = %d\n", sec->active_key);
	}
	if (sec->flags & SEC_UNICAST_GROUP) {
		secinfo->unicast_uses_group = sec->unicast_uses_group;
		dev_dbg_f(zd_mac_dev(zd_netdev_mac(netdev)),
			"   .unicast_uses_group = %d\n",
			sec->unicast_uses_group);
	}
	if (sec->flags & SEC_LEVEL) {
		secinfo->level = sec->level;
		dev_dbg_f(zd_mac_dev(zd_netdev_mac(netdev)),
			"   .level = %d\n", sec->level);
	}
	if (sec->flags & SEC_ENABLED) {
		secinfo->enabled = sec->enabled;
		dev_dbg_f(zd_mac_dev(zd_netdev_mac(netdev)),
			"   .enabled = %d\n", sec->enabled);
	}
	if (sec->flags & SEC_ENCRYPT) {
		secinfo->encrypt = sec->encrypt;
		dev_dbg_f(zd_mac_dev(zd_netdev_mac(netdev)),
			"   .encrypt = %d\n", sec->encrypt);
	}
	if (sec->flags & SEC_AUTH_MODE) {
		secinfo->auth_mode = sec->auth_mode;
		dev_dbg_f(zd_mac_dev(zd_netdev_mac(netdev)),
			"   .auth_mode = %d\n", sec->auth_mode);
	}
}

static void ieee_init(struct ieee80211_device *ieee)
{
	ieee->mode = IEEE_B | IEEE_G;
	ieee->freq_band = IEEE80211_24GHZ_BAND;
	ieee->modulation = IEEE80211_OFDM_MODULATION | IEEE80211_CCK_MODULATION;
	ieee->tx_headroom = sizeof(struct zd_ctrlset);
	ieee->set_security = set_security;
	ieee->hard_start_xmit = netdev_tx;

	/* Software encryption/decryption for now */
	ieee->host_build_iv = 0;
	ieee->host_encrypt = 1;
	ieee->host_decrypt = 1;

	/* FIXME: default to managed mode, until ieee80211 and zd1211rw can
	 * correctly support AUTO */
	ieee->iw_mode = IW_MODE_INFRA;
}

static void softmac_init(struct ieee80211softmac_device *sm)
{
	sm->set_channel = set_channel;
	sm->bssinfo_change = bssinfo_change;
}

struct iw_statistics *zd_mac_get_wireless_stats(struct net_device *ndev)
{
	struct zd_mac *mac = zd_netdev_mac(ndev);
	struct iw_statistics *iw_stats = &mac->iw_stats;
	unsigned int i, count, qual_total, rssi_total;

	memset(iw_stats, 0, sizeof(struct iw_statistics));
	/* We are not setting the status, because ieee->state is not updated
	 * at all and this driver doesn't track authentication state.
	 */
	spin_lock_irq(&mac->lock);
	count = mac->stats_count < ZD_MAC_STATS_BUFFER_SIZE ?
		mac->stats_count : ZD_MAC_STATS_BUFFER_SIZE;
	qual_total = rssi_total = 0;
	for (i = 0; i < count; i++) {
		qual_total += mac->qual_buffer[i];
		rssi_total += mac->rssi_buffer[i];
	}
	spin_unlock_irq(&mac->lock);
	iw_stats->qual.updated = IW_QUAL_NOISE_INVALID;
	if (count > 0) {
		iw_stats->qual.qual = qual_total / count;
		iw_stats->qual.level = rssi_total / count;
		iw_stats->qual.updated |=
			IW_QUAL_QUAL_UPDATED|IW_QUAL_LEVEL_UPDATED;
	} else {
		iw_stats->qual.updated |=
			IW_QUAL_QUAL_INVALID|IW_QUAL_LEVEL_INVALID;
	}
	/* TODO: update counter */
	return iw_stats;
}

#define LINK_LED_WORK_DELAY HZ

static void link_led_handler(struct work_struct *work)
{
	struct zd_mac *mac =
		container_of(work, struct zd_mac, housekeeping.link_led_work.work);
	struct zd_chip *chip = &mac->chip;
	struct ieee80211softmac_device *sm = ieee80211_priv(mac->netdev);
	int is_associated;
	int r;

	spin_lock_irq(&mac->lock);
	is_associated = sm->associnfo.associated != 0;
	spin_unlock_irq(&mac->lock);

	r = zd_chip_control_leds(chip,
		                 is_associated ? LED_ASSOCIATED : LED_SCANNING);
	if (r)
		dev_err(zd_mac_dev(mac), "zd_chip_control_leds error %d\n", r);

	queue_delayed_work(zd_workqueue, &mac->housekeeping.link_led_work,
		           LINK_LED_WORK_DELAY);
}

static void housekeeping_init(struct zd_mac *mac)
{
	INIT_DELAYED_WORK(&mac->housekeeping.link_led_work, link_led_handler);
}

static void housekeeping_enable(struct zd_mac *mac)
{
	dev_dbg_f(zd_mac_dev(mac), "\n");
	queue_delayed_work(zd_workqueue, &mac->housekeeping.link_led_work,
			   0);
}

static void housekeeping_disable(struct zd_mac *mac)
{
	dev_dbg_f(zd_mac_dev(mac), "\n");
	cancel_rearming_delayed_workqueue(zd_workqueue,
		&mac->housekeeping.link_led_work);
	zd_chip_control_leds(&mac->chip, LED_OFF);
}