Index: uspace/drv/bus/usb/ar9271/ar9271.c
===================================================================
--- uspace/drv/bus/usb/ar9271/ar9271.c	(revision 4cb0148f513840557c24fabe8b6c9b7fe5b13ef0)
+++ uspace/drv/bus/usb/ar9271/ar9271.c	(revision 9e5a51c3f68bce51a13b957d870bf35d97aa6fad)
@@ -99,8 +99,10 @@
 static int ar9271_add_device(ddf_dev_t *);
 
-/* IEEE802.11 callbacks */
+/* IEEE 802.11 callbacks */
 static int ar9271_ieee80211_start(ieee80211_dev_t *ieee80211_dev);
 static int ar9271_ieee80211_tx_handler(ieee80211_dev_t *ieee80211_dev,
 	void *buffer, size_t buffer_size);
+static int ar9271_ieee80211_set_freq(ieee80211_dev_t *ieee80211_dev,
+	uint16_t freq);
 
 static driver_ops_t ar9271_driver_ops = {
@@ -115,43 +117,7 @@
 static ieee80211_ops_t ar9271_ieee80211_ops = {
 	.start = ar9271_ieee80211_start,
-	.tx_handler = ar9271_ieee80211_tx_handler
-};
-
-static int ar9271_ieee80211_tx_handler(ieee80211_dev_t *ieee80211_dev,
-	void *buffer, size_t buffer_size)
-{
-	/* TODO: Process TX message properly */
-	
-	size_t complete_size, offset;
-	void *complete_buffer;
-	int endpoint;
-	
-	ar9271_t *ar9271 = (ar9271_t *) ieee80211_dev->driver_data;
-	
-	ieee80211_header_t *ieee80211_header = (ieee80211_header_t *) buffer;
-	
-	if(ieee80211_is_data_frame(ieee80211_header)) {
-		offset = sizeof(htc_frame_header_t);
-		complete_size = buffer_size + offset;
-		complete_buffer = malloc(complete_size);
-		endpoint = ar9271->htc_device->endpoints.data_be_endpoint;
-	} else {
-		offset = sizeof(htc_tx_management_header_t) +
-			sizeof(htc_frame_header_t);
-		complete_size = buffer_size + offset;
-		complete_buffer = malloc(complete_size);
-		endpoint = ar9271->htc_device->endpoints.mgmt_endpoint;
-	}
-	
-	/* Copy IEEE802.11 data to new allocated buffer with HTC headers. */
-	memcpy(complete_buffer + offset, buffer, buffer_size);
-	
-	htc_send_data_message(ar9271->htc_device, complete_buffer,
-		complete_size, endpoint);
-	
-	free(complete_buffer);
-	
-	return EOK;
-}
+	.tx_handler = ar9271_ieee80211_tx_handler,
+	.set_freq = ar9271_ieee80211_set_freq
+};
 
 static int ar9271_data_polling(void *arg)
@@ -162,8 +128,7 @@
 	
 	while(true) {
-		int rc = 
 		htc_read_data_message(ar9271->htc_device, buffer, buffer_size,
 			NULL);
-		usb_log_info("XXXXXXXXXXXXXXXXXXXXXXXXXXXX RC is %d.\n", rc);
+		usb_log_info("Incoming data message.\n");
 		
 		/* TODO: Process RX message */
@@ -173,4 +138,5 @@
 }
 
+/*
 static int ar9271_diag_polling(void *arg)
 {
@@ -183,5 +149,5 @@
 		wmi_reg_read(ar9271->htc_device, 0x782C, &result);
 		usb_log_info("0x782C: %x\n", result);
-		wmi_reg_read(ar9271->htc_device, 0x8048, &result);
+		wmi_reg_read(ar9271->htc_device, AR9271_DIAG, &result);
 		usb_log_info("Diag reg.: %x\n", result);
 		wmi_reg_read(ar9271->htc_device, 0x8088, &result);
@@ -204,4 +170,5 @@
 	return EOK;
 }
+ */
 
 static int ar9271_register_polling_fibrils(ar9271_t *ar9271)
@@ -215,4 +182,5 @@
 	
 	/* Add debug polling fibril. */
+	/*
 	fibril = fibril_create(ar9271_diag_polling, ar9271);
 	if (fibril == 0) {
@@ -220,4 +188,81 @@
 	}
 	fibril_add_ready(fibril);
+	 */
+	
+	return EOK;
+}
+
+/** 
+ * IEEE 802.11 handlers. 
+ */
+
+static int ar9271_ieee80211_set_freq(ieee80211_dev_t *ieee80211_dev,
+	uint16_t freq)
+{
+	ar9271_t *ar9271 = (ar9271_t *) ieee80211_dev->driver_data;
+	
+	wmi_send_command(ar9271->htc_device, WMI_DISABLE_INTR, NULL, 0, NULL);
+	wmi_send_command(ar9271->htc_device, WMI_DRAIN_TXQ_ALL, NULL, 0, NULL);
+	wmi_send_command(ar9271->htc_device, WMI_STOP_RECV, NULL, 0, NULL);
+	
+	int rc = hw_freq_switch(ar9271, freq);
+	if(rc != EOK) {
+		usb_log_error("Failed to HW switch frequency.\n");
+		return rc;
+	}
+	
+	wmi_send_command(ar9271->htc_device, WMI_START_RECV, NULL, 0, NULL);
+	
+	rc = hw_rx_init(ar9271);
+	if(rc != EOK) {
+		usb_log_error("Failed to initialize RX.\n");
+		return rc;
+	}
+	
+	uint16_t htc_mode = host2uint16_t_be(1);
+	wmi_send_command(ar9271->htc_device, WMI_SET_MODE, 
+		(uint8_t *) &htc_mode, sizeof(htc_mode), NULL);
+	wmi_send_command(ar9271->htc_device, WMI_ENABLE_INTR, NULL, 0, NULL);
+	
+	return EOK;
+}
+
+static int ar9271_ieee80211_tx_handler(ieee80211_dev_t *ieee80211_dev,
+	void *buffer, size_t buffer_size)
+{
+	size_t complete_size, offset;
+	void *complete_buffer;
+	int endpoint;
+	
+	ar9271_t *ar9271 = (ar9271_t *) ieee80211_dev->driver_data;
+	
+	uint16_t frame_ctrl = uint16_t_le2host(*((uint16_t *) buffer));
+	if(frame_ctrl & IEEE80211_DATA_FRAME) {
+		offset = sizeof(htc_frame_header_t);
+		complete_size = buffer_size + offset;
+		complete_buffer = malloc(complete_size);
+		endpoint = ar9271->htc_device->endpoints.data_be_endpoint;
+	} else {
+		offset = sizeof(htc_tx_management_header_t) +
+			sizeof(htc_frame_header_t);
+		complete_size = buffer_size + offset;
+		complete_buffer = malloc(complete_size);
+		memset(complete_buffer, 0, complete_size);
+		
+		htc_tx_management_header_t *mgmt_header =
+			(htc_tx_management_header_t *) 
+			(complete_buffer + sizeof(htc_frame_header_t));
+		mgmt_header->keyix = 0xFF;
+		
+		endpoint = ar9271->htc_device->endpoints.mgmt_endpoint;
+	}
+	
+	/* Copy IEEE802.11 data to new allocated buffer with HTC headers. */
+	memcpy(complete_buffer + offset, buffer, buffer_size);
+	
+	htc_send_data_message(ar9271->htc_device, complete_buffer,
+		complete_size, endpoint);
+	
+	free(complete_buffer);
 	
 	return EOK;
@@ -228,12 +273,7 @@
 	ar9271_t *ar9271 = (ar9271_t *) ieee80211_dev->driver_data;
 	
-	int rc = wmi_send_command(ar9271->htc_device, WMI_FLUSH_RECV, NULL, 0, 
-		NULL);
-	if(rc != EOK) {
-		usb_log_error("Failed to flush receiving buffer.\n");
-		return rc;
-	}
-	
-	rc = hw_reset(ar9271);
+	wmi_send_command(ar9271->htc_device, WMI_FLUSH_RECV, NULL, 0, NULL);
+	
+	int rc = hw_reset(ar9271);
 	if(rc != EOK) {
 		usb_log_error("Failed to do HW reset.\n");
@@ -242,24 +282,9 @@
 	
 	uint16_t htc_mode = host2uint16_t_be(1);
-	rc = wmi_send_command(ar9271->htc_device, WMI_SET_MODE, 
+	wmi_send_command(ar9271->htc_device, WMI_SET_MODE, 
 		(uint8_t *) &htc_mode, sizeof(htc_mode), NULL);
-	if(rc != EOK) {
-		usb_log_error("Failed to set HTC mode.\n");
-		return rc;
-	}
-	
-	rc = wmi_send_command(ar9271->htc_device, WMI_ATH_INIT, NULL, 0, 
-		NULL);
-	if(rc != EOK) {
-		usb_log_error("Failed to send ath init command.\n");
-		return rc;
-	}
-	
-	rc = wmi_send_command(ar9271->htc_device, WMI_START_RECV, NULL, 0, 
-		NULL);
-	if(rc != EOK) {
-		usb_log_error("Failed to send receiving init command.\n");
-		return rc;
-	}
+	wmi_send_command(ar9271->htc_device, WMI_ATH_INIT, NULL, 0, NULL);
+	wmi_send_command(ar9271->htc_device, WMI_START_RECV, NULL, 0, NULL);
+	wmi_send_command(ar9271->htc_device, WMI_ENABLE_INTR, NULL, 0, NULL);
 	
 	rc = hw_rx_init(ar9271);
@@ -269,4 +294,14 @@
 	}
 	
+	/* Send capability message to target. */
+	htc_cap_msg_t cap_msg;
+	cap_msg.ampdu_limit = host2uint32_t_be(0xFFFF);
+	cap_msg.ampdu_subframes = 0xFF;
+	cap_msg.enable_coex = 0;
+	cap_msg.tx_chainmask = 0x1;
+	
+	wmi_send_command(ar9271->htc_device, WMI_TARGET_IC_UPDATE,
+		(uint8_t *) &cap_msg, sizeof(cap_msg), NULL);
+	
 	rc = htc_init_new_vif(ar9271->htc_device);
 	if(rc != EOK) {
@@ -281,4 +316,6 @@
 	}
 	
+	ar9271->starting_up = false;
+	
 	return EOK;
 }
@@ -286,4 +323,5 @@
 static int ar9271_init(ar9271_t *ar9271, usb_device_t *usb_device)
 {
+	ar9271->starting_up = true;
 	ar9271->usb_device = usb_device;
 	
Index: uspace/drv/bus/usb/ar9271/ar9271.h
===================================================================
--- uspace/drv/bus/usb/ar9271/ar9271.h	(revision 4cb0148f513840557c24fabe8b6c9b7fe5b13ef0)
+++ uspace/drv/bus/usb/ar9271/ar9271.h	(revision 9e5a51c3f68bce51a13b957d870bf35d97aa6fad)
@@ -98,4 +98,5 @@
 	AR9271_BSSID_MASK1 = 0x80E4,		/**< BSSID Mask Upper 16 Bits */
 	AR9271_STATION_ID1_MASK = 0x0000FFFF,
+	AR9271_STATION_ID1_POWER_SAVING = 0x00040000,
 		
 	/* RX filtering register */
@@ -111,10 +112,17 @@
 	AR9271_MULTICAST_FILTER1 = 0x8040,
 	AR9271_MULTICAST_FILTER2 = 0x8044,	
+	AR9271_DIAG = 0x8048,
 		
 	/* Physical layer registers */
+		
 	AR9271_PHY_ACTIVE = 0x981C,
 	AR9271_ADC_CONTROL = 0x982C,
 	AR9271_AGC_CONTROL = 0x9860,
 	AR9271_PHY_SYNTH_CONTROL = 0x9874, 
+	AR9271_PHY_SPECTRAL_SCAN = 0x9910,
+	AR9271_PHY_RADAR0 = 0x9954,
+	AR9271_PHY_RADAR0_FFT_ENABLED = 0x80000000,
+	AR9271_PHY_RFBUS_KILL = 0x997C,
+	AR9271_PHY_RFBUS_GRANT = 0x9C20,
 	AR9271_PHY_MODE = 0xA200,
 	AR9271_PHY_CCK_TX_CTRL = 0xA204,
@@ -127,5 +135,4 @@
 	AR9271_AGC_CONTROL_TX_CALIB = 0x00010000,
 	AR9271_AGC_CONTROL_NF_NOT_UPDATE = 0x00020000,
-	AR9271_PHY_MODE_2G = 0x02,
 	AR9271_PHY_MODE_DYNAMIC = 0x04,
 	AR9271_PHY_CCK_TX_CTRL_JAPAN = 0x00000010,
@@ -156,4 +163,7 @@
 /** AR9271 device data */
 typedef struct {
+	/** Whether device is starting up. */
+	bool starting_up;
+	
 	/** Backing DDF device */
 	ddf_dev_t *ddf_dev;
@@ -187,8 +197,8 @@
 	{0x0000801c, 0x12e0002b},
 	{0x00008318, 0x00003440},
-	{0x00009804, 0x00000300},
+	{0x00009804, 0x000003c0},/*< note: overridden */
 	{0x00009820, 0x02020200},
 	{0x00009824, 0x01000e0e},
-	{0x00009828, 0x3a020001},
+	{0x00009828, 0x0a020001},/*< note: overridden */
 	{0x00009834, 0x00000e0e},
 	{0x00009838, 0x00000007},
@@ -486,4 +496,45 @@
 
 /**
+ * AR9271 TX init values for 2GHz mode operation.
+ * 
+ * Taken from Linux sources.
+ */
+static const uint32_t ar9271_2g_tx_array[][2] = {
+	{0x0000a300, 0x00010000},
+	{0x0000a304, 0x00016200},
+	{0x0000a308, 0x00018201},
+	{0x0000a30c, 0x0001b240},
+	{0x0000a310, 0x0001d241},
+	{0x0000a314, 0x0001f600},
+	{0x0000a318, 0x00022800},
+	{0x0000a31c, 0x00026802},
+	{0x0000a320, 0x0002b805},
+	{0x0000a324, 0x0002ea41},
+	{0x0000a328, 0x00038b00},
+	{0x0000a32c, 0x0003ab40},
+	{0x0000a330, 0x0003cd80},
+	{0x0000a334, 0x000368de},
+	{0x0000a338, 0x0003891e},
+	{0x0000a33c, 0x0003a95e},
+	{0x0000a340, 0x0003e9df},
+	{0x0000a344, 0x0003e9df},
+	{0x0000a348, 0x0003e9df},
+	{0x0000a34c, 0x0003e9df},
+	{0x0000a350, 0x0003e9df},
+	{0x0000a354, 0x0003e9df},
+	{0x00007838, 0x0000002b},
+	{0x00007824, 0x00d8a7ff},
+	{0x0000786c, 0x08609eba},
+	{0x00007820, 0x00000c00},
+	{0x0000a274, 0x0a214652},
+	{0x0000a278, 0x0e739ce7},
+	{0x0000a27c, 0x05018063},
+	{0x0000a394, 0x06318c63},
+	{0x0000a398, 0x00000063},
+	{0x0000a3dc, 0x06318c63},
+	{0x0000a3e0, 0x00000063}
+};
+
+/**
  * AR9271 hardware init values.
  * 
@@ -584,5 +635,5 @@
 	{0x00008058, 0x00000000},
 	{0x0000805c, 0x000fc78f},
-	{0x00008060, 0x0000000f},
+	{0x00008060, 0xc7ff000f},
 	{0x00008064, 0x00000000},
 	{0x00008070, 0x00000000},
@@ -680,5 +731,5 @@
 	{0x0000833c, 0x00000000},
 	{0x00008340, 0x00010380},
-	{0x00008344, 0x00581003},/*< note: disabled ADHOC_MCAST_KEYID feature */
+	{0x00008344, 0x00481083},/*< note: disabled ADHOC_MCAST_KEYID feature */
 	{0x00007010, 0x00000030},
 	{0x00007034, 0x00000002},
Index: uspace/drv/bus/usb/ar9271/ath_usb.c
===================================================================
--- uspace/drv/bus/usb/ar9271/ath_usb.c	(revision 4cb0148f513840557c24fabe8b6c9b7fe5b13ef0)
+++ uspace/drv/bus/usb/ar9271/ath_usb.c	(revision 9e5a51c3f68bce51a13b957d870bf35d97aa6fad)
@@ -145,4 +145,12 @@
 	size_t buffer_size)
 {
+	size_t complete_buffer_size = buffer_size + DATA_HEADER_SIZE;
+	void *complete_buffer = malloc(buffer_size + DATA_HEADER_SIZE);
+	memcpy(complete_buffer + DATA_HEADER_SIZE, buffer, buffer_size);
+	
+	uint16_t *it = (uint16_t *) complete_buffer;
+	*it++ = host2uint16_t_le(buffer_size);
+	*it++ = host2uint16_t_le(TX_STREAM_MODE_TAG);
+	
 	ath_usb_t *ath_usb = (ath_usb_t *) ath->specific_data;
 	usb_pipe_t *pipe = 
@@ -150,5 +158,10 @@
 		pipe;
 	
-	return usb_pipe_write(pipe, buffer, buffer_size);
+	int ret_val = usb_pipe_write(pipe, complete_buffer, 
+		complete_buffer_size);
+	
+	free(complete_buffer);
+	
+	return ret_val;
 }
 
Index: uspace/drv/bus/usb/ar9271/ath_usb.h
===================================================================
--- uspace/drv/bus/usb/ar9271/ath_usb.h	(revision 4cb0148f513840557c24fabe8b6c9b7fe5b13ef0)
+++ uspace/drv/bus/usb/ar9271/ath_usb.h	(revision 9e5a51c3f68bce51a13b957d870bf35d97aa6fad)
@@ -40,4 +40,8 @@
 #include "ath.h"
 
+#define DATA_HEADER_SIZE 4
+#define RX_STREAM_MODE_TAG 0x4e00
+#define TX_STREAM_MODE_TAG 0x697e
+
 /** Atheros USB wifi device structure */
 typedef struct {
Index: uspace/drv/bus/usb/ar9271/htc.c
===================================================================
--- uspace/drv/bus/usb/ar9271/htc.c	(revision 4cb0148f513840557c24fabe8b6c9b7fe5b13ef0)
+++ uspace/drv/bus/usb/ar9271/htc.c	(revision 9e5a51c3f68bce51a13b957d870bf35d97aa6fad)
@@ -71,4 +71,7 @@
 	htc_sta_msg_t sta_msg;
 	
+	memset(&vif_msg, 0, sizeof(htc_vif_msg_t));
+	memset(&sta_msg, 0, sizeof(htc_sta_msg_t));
+	
 	nic_address_t addr;
 	nic_t *nic = nic_get_from_ddf_dev(htc_device->ieee80211_dev->ddf_dev);
@@ -99,22 +102,14 @@
 	vif_msg.rts_thres = host2uint16_t_be(HTC_RTS_THRESHOLD);
 	
-	int rc = wmi_send_command(htc_device, WMI_VAP_CREATE, 
-		(uint8_t *) &vif_msg, sizeof(vif_msg), NULL);
-	if(rc != EOK) {
-		usb_log_error("Failed to send VAP create command.\n");
-		return rc;
-	}
+	wmi_send_command(htc_device, WMI_VAP_CREATE, (uint8_t *) &vif_msg, 
+		sizeof(vif_msg), NULL);
 	
 	sta_msg.is_vif_sta = 1;
-	sta_msg.max_ampdu = host2uint16_t_be(HTC_MAX_AMPDU);
+	sta_msg.max_ampdu = host2uint16_t_be(0xFFFF);
 	sta_msg.sta_index = 0;
 	sta_msg.vif_index = 0;
 	
-	rc = wmi_send_command(htc_device, WMI_NODE_CREATE, 
-		(uint8_t *) &sta_msg, sizeof(sta_msg), NULL);
-	if(rc != EOK) {
-		usb_log_error("Failed to send NODE create command.\n");
-		return rc;
-	}
+	wmi_send_command(htc_device, WMI_NODE_CREATE, (uint8_t *) &sta_msg, 
+		sizeof(sta_msg), NULL);
 	
 	/* Write first 4 bytes of MAC address. */
@@ -122,19 +117,14 @@
 	memcpy(&id0, &addr.address, 4);
 	id0 = host2uint32_t_le(id0);
-	rc = wmi_reg_write(htc_device, AR9271_STATION_ID0, id0);
-	if(rc != EOK)
-		return rc;
-	
+	wmi_reg_write(htc_device, AR9271_STATION_ID0, id0);
+
 	/* Write last 2 bytes of MAC address (and preserve existing data). */
 	uint32_t id1;
-	rc = wmi_reg_read(htc_device, AR9271_STATION_ID1, &id1);
-	if(rc != EOK)
-		return rc;
+	wmi_reg_read(htc_device, AR9271_STATION_ID1, &id1);
+
 	uint16_t id1_addr;
 	memcpy(&id1_addr, &addr.address[4], 2);
 	id1 = (id1 & ~AR9271_STATION_ID1_MASK) | host2uint16_t_le(id1_addr);
-	rc = wmi_reg_write(htc_device, AR9271_STATION_ID1, id1);
-	if(rc != EOK)
-		return rc;
+	wmi_reg_write(htc_device, AR9271_STATION_ID1, id1);
 	
 	/* TODO: Set BSSID mask for AP mode. */
@@ -146,13 +136,9 @@
 	size_t buffer_size, uint8_t endpoint_id)
 {
+	memset(header, 0, sizeof(htc_frame_header_t));
+	
 	header->endpoint_id = endpoint_id;
-	header->flags = 0;
 	header->payload_length = 
 		host2uint16_t_be(buffer_size - sizeof(htc_frame_header_t));
-	
-	header->control_bytes[0] = 0x02;
-	header->control_bytes[1] = 0x88;
-	header->control_bytes[2] = 0xFF;
-	header->control_bytes[3] = 0xFF;
 }
 
@@ -257,4 +243,5 @@
 		sizeof(htc_service_msg_t);
 	void *buffer = malloc(buffer_size);
+	memset(buffer, 0, buffer_size);
 	
 	/* Fill service message structure. */
Index: uspace/drv/bus/usb/ar9271/htc.h
===================================================================
--- uspace/drv/bus/usb/ar9271/htc.h	(revision 4cb0148f513840557c24fabe8b6c9b7fe5b13ef0)
+++ uspace/drv/bus/usb/ar9271/htc.h	(revision 9e5a51c3f68bce51a13b957d870bf35d97aa6fad)
@@ -45,5 +45,4 @@
 
 #define HTC_RTS_THRESHOLD 2304
-#define HTC_MAX_AMPDU 0xFFFF
 
 /**
@@ -220,4 +219,15 @@
 	uint8_t pad;
 } __attribute__((packed)) htc_sta_msg_t;
+
+/**
+ * HTC message to inform target about available capabilities.
+ */
+typedef struct {
+	uint32_t ampdu_limit;	/**< Big Endian value! */
+	uint8_t ampdu_subframes;
+	uint8_t enable_coex;
+	uint8_t tx_chainmask;
+	uint8_t pad;
+} __attribute__((packed)) htc_cap_msg_t;
 
 /**
Index: uspace/drv/bus/usb/ar9271/hw.c
===================================================================
--- uspace/drv/bus/usb/ar9271/hw.c	(revision 4cb0148f513840557c24fabe8b6c9b7fe5b13ef0)
+++ uspace/drv/bus/usb/ar9271/hw.c	(revision 9e5a51c3f68bce51a13b957d870bf35d97aa6fad)
@@ -60,11 +60,5 @@
 		udelay(HW_WAIT_TIME_US);
 
-		int rc = wmi_reg_read(ar9271->htc_device, offset, &result);
-		if(rc != EOK) {
-			usb_log_debug("Failed to read register. Error %d\n",
-				rc);
-			continue;
-		}
-		
+		wmi_reg_read(ar9271->htc_device, offset, &result);
 		if((result & mask) == value) {
 			return EOK;
@@ -93,31 +87,18 @@
 	};
 	
-	int rc = wmi_reg_buffer_write(ar9271->htc_device, buffer,
+	wmi_reg_buffer_write(ar9271->htc_device, buffer,
 		sizeof(buffer) / sizeof(wmi_reg_t));
-	if(rc != EOK) {
-		usb_log_error("Failed to set RT FORCE WAKE register.\n");
-		return rc;
-	}
 	
 	udelay(2);
 	
-	rc = wmi_reg_write(ar9271->htc_device, AR9271_RC, 0);
-	if(rc != EOK) {
-		usb_log_error("Failed to reset MAC AHB register.\n");
-		return rc;
-	}
-	
-	rc = wmi_reg_write(ar9271->htc_device, AR9271_RTC_RESET, 1);
-	if(rc != EOK) {
-		usb_log_error("Failed to bring up RTC register.\n");
-		return rc;
-	}
-	
-	rc = hw_read_wait(ar9271, 
+	wmi_reg_write(ar9271->htc_device, AR9271_RC, 0);
+	wmi_reg_write(ar9271->htc_device, AR9271_RTC_RESET, 1);
+	
+	int rc = hw_read_wait(ar9271, 
 		AR9271_RTC_STATUS, 
 		AR9271_RTC_STATUS_MASK, 
 		AR9271_RTC_STATUS_ON);
 	if(rc != EOK) {
-		usb_log_error("Failed to read wake up RTC register.\n");
+		usb_log_error("Failed to wait for RTC wake up register.\n");
 		return rc;
 	}
@@ -150,30 +131,20 @@
 	};
 	
-	int rc = wmi_reg_buffer_write(ar9271->htc_device, buffer,
+	wmi_reg_buffer_write(ar9271->htc_device, buffer, 
 		sizeof(buffer) / sizeof(wmi_reg_t));
-	if(rc != EOK) {
-		usb_log_error("Failed to set warm reset register.\n");
-		return rc;
-	}
 	
 	udelay(100);
 	
-	rc = wmi_reg_write(ar9271->htc_device, AR9271_RTC_RC, 0);
-	if(rc != EOK) {
-		usb_log_error("Failed to reset RTC RC register.\n");
-		return rc;
-	}
-	
-	rc = hw_read_wait(ar9271, AR9271_RTC_RC, AR9271_RTC_RC_MASK, 0);
-	if(rc != EOK) {
-		usb_log_error("Failed to read RTC RC register.\n");
-		return rc;
-	}
-	
-	rc = wmi_reg_write(ar9271->htc_device, AR9271_RC, 0);
-	if(rc != EOK) {
-		usb_log_error("Failed to reset MAC AHB register.\n");
-		return rc;
-	}
+	wmi_reg_write(ar9271->htc_device, AR9271_RTC_RC, 0);
+	
+	int rc = hw_read_wait(ar9271, AR9271_RTC_RC, AR9271_RTC_RC_MASK, 0);
+	if(rc != EOK) {
+		usb_log_error("Failed to wait for RTC RC register.\n");
+		return rc;
+	}
+	
+	wmi_reg_write(ar9271->htc_device, AR9271_RC, 0);
+	wmi_reg_clear_bit(ar9271->htc_device, AR9271_STATION_ID1, 
+		AR9271_STATION_ID1_POWER_SAVING);
 	
 	return EOK;
@@ -182,18 +153,11 @@
 static int hw_addr_init(ar9271_t *ar9271)
 {
-	int rc;
 	uint32_t value;
 	nic_address_t ar9271_address;
 	
 	for(int i = 0; i < 3; i++) {
-		rc = wmi_reg_read(ar9271->htc_device, 
+		wmi_reg_read(ar9271->htc_device, 
 			AR9271_EEPROM_MAC_ADDR_START + i*4,
 			&value);
-		
-		if(rc != EOK) {
-			usb_log_error("Failed to read %d. byte of MAC address."
-				"\n", i);
-			return rc;
-		}
 		
 		uint16_t two_bytes = uint16_t_be2host(value);
@@ -204,8 +168,8 @@
 	nic_t *nic = nic_get_from_ddf_dev(ar9271->ddf_dev);
 	
-	rc = nic_report_address(nic, &ar9271_address);
+	int rc = nic_report_address(nic, &ar9271_address);
 	if(rc != EOK) {
 		usb_log_error("Failed to report NIC HW address.\n");
-			return rc;
+		return rc;
 	}
 	
@@ -227,29 +191,17 @@
 	gpio_shift = (gpio % 6) * 5;
 	
-	int rc = wmi_reg_read(ar9271->htc_device, address, &temp);
-	if(rc != EOK) {
-		usb_log_error("Failed to read GPIO output mux.\n");
-		return rc;
-	}
-	
+	wmi_reg_read(ar9271->htc_device, address, &temp);
+
 	temp = ((temp & 0x1F0) << 1) | (temp & ~0x1F0);
 	temp &= ~(0x1f << gpio_shift);
 	temp |= (type << gpio_shift);
-	
-	rc = wmi_reg_write(ar9271->htc_device, address, temp);
-	if(rc != EOK) {
-		usb_log_error("Failed to write GPIO output mux.\n");
-		return rc;
-	}
+
+	wmi_reg_write(ar9271->htc_device, address, temp);
 	
 	gpio_shift = 2 * gpio;
 	
-	rc = wmi_reg_set_clear_bit(ar9271->htc_device, AR9271_GPIO_OE_OUT,
+	wmi_reg_set_clear_bit(ar9271->htc_device, AR9271_GPIO_OE_OUT,
 		AR9271_GPIO_OE_OUT_ALWAYS << gpio_shift, 
 		AR9271_GPIO_OE_OUT_ALWAYS << gpio_shift);
-	if(rc != EOK) {
-		usb_log_error("Failed to config GPIO as output.\n");
-		return rc;
-	}
 	
 	return EOK;
@@ -258,11 +210,6 @@
 static int hw_gpio_set_value(ar9271_t *ar9271, uint32_t gpio, uint32_t value)
 {
-	int rc = wmi_reg_set_clear_bit(ar9271->htc_device, AR9271_GPIO_IN_OUT,
+	wmi_reg_set_clear_bit(ar9271->htc_device, AR9271_GPIO_IN_OUT,
 		(~value & 1) << gpio, 1 << gpio);
-	if(rc != EOK) {
-		usb_log_error("Failed to set GPIO.\n");
-		return rc;
-	}
-	
 	return EOK;
 }
@@ -321,8 +268,10 @@
 	uint32_t set_bit = 0x10000000;
 	
-	/* NOTICE: Fall-through switch statement! */
 	switch(op_mode) {
 		case IEEE80211_OPMODE_ADHOC:
 			set_bit |= AR9271_OPMODE_ADHOC_MASK;
+			wmi_reg_set_bit(ar9271->htc_device, AR9271_CONFIG,
+				AR9271_CONFIG_ADHOC);
+			break;
 		case IEEE80211_OPMODE_MESH:
 		case IEEE80211_OPMODE_AP:
@@ -342,4 +291,29 @@
 }
 
+static int hw_reset_operating_mode(ar9271_t *ar9271)
+{
+	int rc = hw_set_operating_mode(ar9271, IEEE80211_OPMODE_STATION);
+	if(rc != EOK) {
+		usb_log_error("Failed to set opmode to station.\n");
+		return rc;
+	}
+	
+	return EOK;
+}
+
+static int hw_noise_floor_calibration(ar9271_t *ar9271)
+{
+	wmi_reg_set_bit(ar9271->htc_device, AR9271_AGC_CONTROL, 
+		AR9271_AGC_CONTROL_NF_CALIB_EN);
+	
+	wmi_reg_clear_bit(ar9271->htc_device, AR9271_AGC_CONTROL, 
+		AR9271_AGC_CONTROL_NF_NOT_UPDATE);
+	
+	wmi_reg_set_bit(ar9271->htc_device, AR9271_AGC_CONTROL, 
+		AR9271_AGC_CONTROL_NF_CALIB);
+	
+	return EOK;
+}
+
 static int hw_set_freq(ar9271_t *ar9271, uint16_t freq)
 {
@@ -373,24 +347,42 @@
 }
 
+int hw_freq_switch(ar9271_t *ar9271, uint16_t freq)
+{
+	wmi_reg_write(ar9271->htc_device, AR9271_PHY_RFBUS_KILL, 0x1);
+	
+	int rc = hw_read_wait(ar9271, AR9271_PHY_RFBUS_GRANT, 0x1, 0x1);
+	if(rc != EOK) {
+		usb_log_error("Failed to kill RF bus.\n");
+		return rc;
+	}
+	
+	rc = hw_set_freq(ar9271, freq);
+	if(rc != EOK) {
+		usb_log_error("Failed to HW set frequency.\n");
+		return rc;
+	}
+	
+	udelay(1000);
+	wmi_reg_write(ar9271->htc_device, AR9271_PHY_RFBUS_KILL, 0x0);
+	
+	rc = hw_noise_floor_calibration(ar9271);
+	if(rc != EOK) {
+		usb_log_error("Failed to do NF calibration.\n");
+		return rc;
+	}
+	
+	return EOK;
+}
+
 static int hw_set_rx_filter(ar9271_t *ar9271)
 {
 	uint32_t filter_bits;
-	int rc = wmi_reg_read(ar9271->htc_device, AR9271_RX_FILTER, 
-		&filter_bits);
-	if(rc != EOK) {
-		usb_log_error("Failed to read RX filter.\n");
-		return EINVAL;
-	}
 	
 	/* TODO: Do proper filtering here. */
 	
-	filter_bits |= AR9271_RX_FILTER_UNI | AR9271_RX_FILTER_MULTI | 
-		AR9271_RX_FILTER_BROAD | AR9271_RX_FILTER_PROMISCUOUS;
-	
-	rc = wmi_reg_write(ar9271->htc_device, AR9271_RX_FILTER, filter_bits);
-	if(rc != EOK) {
-		usb_log_error("Failed to write RX filter.\n");
-		return EINVAL;
-	}
+	filter_bits = AR9271_RX_FILTER_UNI | AR9271_RX_FILTER_MULTI | 
+		AR9271_RX_FILTER_BROAD | AR9271_RX_FILTER_BEACON;
+	
+	wmi_reg_write(ar9271->htc_device, AR9271_RX_FILTER, filter_bits);
 	
 	return EOK;
@@ -399,15 +391,11 @@
 int hw_rx_init(ar9271_t *ar9271)
 {
-	int rc = wmi_reg_write(ar9271->htc_device, AR9271_COMMAND, 
+	wmi_reg_write(ar9271->htc_device, AR9271_COMMAND, 
 		AR9271_COMMAND_RX_ENABLE);
-	if(rc != EOK) {
-		usb_log_error("Failed to send RX enable command.\n");
-		return EINVAL;
-	}
-	
-	rc = hw_set_rx_filter(ar9271);
+	
+	int rc = hw_set_rx_filter(ar9271);
 	if(rc != EOK) {
 		usb_log_error("Failed to set RX filtering.\n");
-		return EINVAL;
+		return rc;
 	}
 	
@@ -415,4 +403,7 @@
 	wmi_reg_write(ar9271->htc_device, AR9271_MULTICAST_FILTER2, ~0);
 	
+	/* Disable RX blocking. */
+	wmi_reg_clear_bit(ar9271->htc_device, AR9271_DIAG, (0x20 | 0x02000000));
+	
 	return EOK;
 }
@@ -420,10 +411,5 @@
 static int hw_activate_phy(ar9271_t *ar9271)
 {
-	int rc = wmi_reg_write(ar9271->htc_device, AR9271_PHY_ACTIVE, 1);
-	if(rc != EOK) {
-		usb_log_error("Failed to activate set PHY active.\n");
-		return rc;
-	}
-	
+	wmi_reg_write(ar9271->htc_device, AR9271_PHY_ACTIVE, 1);
 	udelay(1000);
 	
@@ -435,10 +421,11 @@
 	uint32_t pll;
 	
-	/* Some magic here. */
+	/* Some magic here (set for 2GHz channels). But VERY important :-) */
 	pll = (0x5 << 10) & 0x00003C00;
-	pll |= (0x2 << 14) & 0x0000C000; /**< 0x2 ~ quarter rate (0x1 half) */
 	pll |= 0x2C & 0x000003FF;
 	
-	return wmi_reg_write(ar9271->htc_device, AR9271_RTC_PLL_CONTROL, pll);
+	wmi_reg_write(ar9271->htc_device, AR9271_RTC_PLL_CONTROL, pll);
+	
+	return EOK;
 }
 
@@ -453,4 +440,12 @@
 		reg_offset = ar9271_2g_mode_array[i][0];
 		reg_value = ar9271_2g_mode_array[i][1];
+		wmi_reg_write(ar9271->htc_device, reg_offset, reg_value);
+	}
+	
+	size = sizeof(ar9271_2g_tx_array) / sizeof(ar9271_2g_tx_array[0]);
+	
+	for(int i = 0; i < size; i++) {
+		reg_offset = ar9271_2g_tx_array[i][0];
+		reg_value = ar9271_2g_tx_array[i][1];
 		wmi_reg_write(ar9271->htc_device, reg_offset, reg_value);
 	}
@@ -497,40 +492,25 @@
 }
 
-static int hw_noise_floor_calibration(ar9271_t *ar9271)
-{
-	wmi_reg_set_bit(ar9271->htc_device, AR9271_AGC_CONTROL, 
-		AR9271_AGC_CONTROL_NF_CALIB_EN);
-	
-	wmi_reg_clear_bit(ar9271->htc_device, AR9271_AGC_CONTROL, 
-		AR9271_AGC_CONTROL_NF_NOT_UPDATE);
-	
-	wmi_reg_set_bit(ar9271->htc_device, AR9271_AGC_CONTROL, 
-		AR9271_AGC_CONTROL_NF_CALIB);
-	
-	return EOK;
-}
-
 int hw_reset(ar9271_t *ar9271) 
 {
 	/* Set physical layer as deactivated. */
-	int rc = wmi_reg_write(ar9271->htc_device, AR9271_PHY_ACTIVE, 0);
-	if(rc != EOK) {
-		usb_log_error("Failed to set PHY deactivated.\n");
-		return rc;
-	}
-	
-	rc = wmi_reg_write(ar9271->htc_device, 
-		AR9271_RESET_POWER_DOWN_CONTROL,
-		AR9271_RADIO_RF_RESET);
-	if(rc != EOK) {
-		usb_log_error("Failed to reset radio rf.\n");
-		return rc;
-	}
-	
-	udelay(50);
-	
-	/* TODO: There should be cold reset only if RX or TX is enabled. */
-	
-	rc = hw_init_pll(ar9271);
+	wmi_reg_write(ar9271->htc_device, AR9271_PHY_ACTIVE, 0);
+	
+	if(ar9271->starting_up) {
+		wmi_reg_write(ar9271->htc_device, 
+			AR9271_RESET_POWER_DOWN_CONTROL,
+			AR9271_RADIO_RF_RESET);
+
+		udelay(50);
+	}
+	
+	/* Cold reset when RX is enabled. */
+	uint32_t config_reg;
+	wmi_reg_read(ar9271->htc_device, AR9271_COMMAND, &config_reg);
+	if(config_reg & AR9271_COMMAND_RX_ENABLE) {
+		hw_set_reset(ar9271, true);
+	}
+	
+	int rc = hw_init_pll(ar9271);
 	if(rc != EOK) {
 		usb_log_error("Failed to init PLL.\n");
@@ -540,24 +520,16 @@
 	udelay(500);
 	
-	rc = wmi_reg_write(ar9271->htc_device, 
-		AR9271_CLOCK_CONTROL,
+	wmi_reg_write(ar9271->htc_device, AR9271_CLOCK_CONTROL,	
 		AR9271_MAX_CPU_CLOCK);
-	if(rc != EOK) {
-		usb_log_error("Failed to set CPU clock.\n");
-		return rc;
-	}
 	
 	udelay(100);
 	
-	rc = wmi_reg_write(ar9271->htc_device, 
-		AR9271_RESET_POWER_DOWN_CONTROL,
-		AR9271_GATE_MAC_CONTROL);
-	if(rc != EOK) {
-		usb_log_error("Failed to set the gate reset controls for MAC."
-			"\n");
-		return rc;
-	}
-	
-	udelay(50);
+	if(ar9271->starting_up) {
+		wmi_reg_write(ar9271->htc_device, 
+			AR9271_RESET_POWER_DOWN_CONTROL,
+			AR9271_GATE_MAC_CONTROL);
+
+		udelay(50);
+	}
 	
 	rc = hw_set_init_values(ar9271);
@@ -567,23 +539,17 @@
 	}
 	
-	/* TODO: There should probably be TX power settings. */
-	
 	/* Set physical layer mode. */
-	rc = wmi_reg_write(ar9271->htc_device, AR9271_PHY_MODE, 
+	wmi_reg_write(ar9271->htc_device, AR9271_PHY_MODE, 
 		AR9271_PHY_MODE_DYNAMIC);
-	if(rc != EOK) {
-		usb_log_error("Failed to set physical layer mode.\n");
-		return rc;
-	}
-	
-	/* Set device operating mode. */
-	rc = hw_set_operating_mode(ar9271, IEEE80211_OPMODE_STATION);
-	if(rc != EOK) {
-		usb_log_error("Failed to set opmode to station.\n");
-		return rc;
-	}
-	
-	/* Set channel frequency. */
-	rc = hw_set_freq(ar9271, 2437);
+	
+	/* Reset device operating mode. */
+	rc = hw_reset_operating_mode(ar9271);
+	if(rc != EOK) {
+		usb_log_error("Failed to reset operating mode.\n");
+		return rc;
+	}
+	
+	/* Set initial channel frequency. */
+	rc = hw_set_freq(ar9271, IEEE80211_FIRST_FREQ);
 	if(rc != EOK) {
 		usb_log_error("Failed to set channel.\n");
@@ -593,15 +559,8 @@
 	/* Initialize transmission queues. */
 	for(int i = 0; i < AR9271_QUEUES_COUNT; i++) {
-		rc = wmi_reg_write(ar9271->htc_device, 
+		wmi_reg_write(ar9271->htc_device, 
 			AR9271_QUEUE_BASE_MASK + (i << 2),
 			1 << i);
-		if(rc != EOK) {
-			usb_log_error("Failed to initialize transmission queue."
-				"\n");
-			return rc;
-		}
-	}
-	
-	/* TODO: Maybe resetting TX queues will be necessary afterwards here. */
+	}
 	
 	/* Activate physical layer. */
@@ -624,4 +583,7 @@
 		return rc;
 	}
+	
+	/* Byteswap TX and RX data buffer words. */
+	wmi_reg_write(ar9271->htc_device, AR9271_CONFIG, 0xA);
 	
 	usb_log_info("HW reset done.\n");
Index: uspace/drv/bus/usb/ar9271/hw.h
===================================================================
--- uspace/drv/bus/usb/ar9271/hw.h	(revision 4cb0148f513840557c24fabe8b6c9b7fe5b13ef0)
+++ uspace/drv/bus/usb/ar9271/hw.h	(revision 9e5a51c3f68bce51a13b957d870bf35d97aa6fad)
@@ -42,4 +42,5 @@
 
 extern int hw_init(ar9271_t *ar9271);
+extern int hw_freq_switch(ar9271_t *, uint16_t freq);
 extern int hw_rx_init(ar9271_t *ar9271);
 extern int hw_reset(ar9271_t *ar9271);
Index: uspace/lib/net/ieee80211/ieee80211.c
===================================================================
--- uspace/lib/net/ieee80211/ieee80211.c	(revision 4cb0148f513840557c24fabe8b6c9b7fe5b13ef0)
+++ uspace/lib/net/ieee80211/ieee80211.c	(revision 9e5a51c3f68bce51a13b957d870bf35d97aa6fad)
@@ -42,4 +42,23 @@
 #include <ieee80211.h>
 
+/** Broadcast MAC address used to spread probe request through channel. */
+static const uint8_t ieee80211_broadcast_mac_addr[] = {
+	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
+};
+
+/** IEEE 802.11 b/g supported data rates in units of 500 kb/s. */
+static const uint8_t ieee80211bg_data_rates[] = {
+	2, 4, 11, 12, 18, 22, 24, 36
+};
+
+/** IEEE 802.11 b/g extended supported data rates in units of 500 kb/s.
+ * 
+ *  These are defined separately, because probe request message can
+ *  only handle up to 8 data rates in supported rates IE. 
+ */
+static const uint8_t ieee80211bg_ext_data_rates[] = {
+	48, 72, 96, 108
+};
+
 /** Network interface options for IEEE 802.11 driver. */
 static nic_iface_t ieee80211_nic_iface;
@@ -47,11 +66,4 @@
 /** Basic driver operations for IEEE 802.11 NIC driver. */
 static driver_ops_t ieee80211_nic_driver_ops;
-
-bool ieee80211_is_data_frame(ieee80211_header_t *header)
-{
-	return (header->frame_ctrl & 
-		host2uint16_t_le(IEEE80211_FRAME_CTRL_FRAME_TYPE)) ==
-		host2uint16_t_le(IEEE80211_FRAME_CTRL_DATA_FRAME);
-}
 
 static int ieee80211_open(ddf_fun_t *fun)
@@ -70,9 +82,7 @@
 		return rc;
 	
-	/*
 	rc = ieee80211_dev->ops->scan(ieee80211_dev);
 	if(rc != EOK)
 		return rc;
-	 */
 	
 	return EOK;
@@ -93,4 +103,8 @@
 	/* IEEE802.11 TX handler must be implemented. */
 	if(!ieee80211_ops->tx_handler)
+		return EINVAL;
+	
+	/* IEEE802.11 set frequency handler must be implemented. */
+	if(!ieee80211_ops->set_freq)
 		return EINVAL;
 	
@@ -119,4 +133,7 @@
 	ieee80211_dev->current_op_mode = IEEE80211_OPMODE_STATION;
 	
+	memcpy(ieee80211_dev->bssid_mask, ieee80211_broadcast_mac_addr, 
+		ETH_ADDR);
+	
 	/* Bind NIC to device */
 	nic_t *nic = nic_create_and_bind(ddf_dev);
@@ -175,4 +192,106 @@
 }
 
+static uint8_t ieee80211_freq_to_channel(uint16_t freq)
+{
+	return (freq - IEEE80211_FIRST_FREQ) / IEEE80211_CHANNEL_GAP + 1;
+}
+
+int ieee80211_probe_request(ieee80211_dev_t *ieee80211_dev)
+{
+	nic_t *nic = nic_get_from_ddf_dev(ieee80211_dev->ddf_dev);
+	nic_address_t nic_address;
+	nic_query_address(nic, &nic_address);
+	
+	size_t data_rates_size = 
+		sizeof(ieee80211bg_data_rates) / 
+		sizeof(ieee80211bg_data_rates[0]);
+	
+	size_t ext_data_rates_size = 
+		sizeof(ieee80211bg_ext_data_rates) / 
+		sizeof(ieee80211bg_ext_data_rates[0]);
+	
+	/* 3 headers - (rates, ext rates, current channel) and their data
+	 * lengths + pad. 
+	 */
+	size_t payload_size = 
+		sizeof(ieee80211_ie_header_t) * 3 +
+		data_rates_size + ext_data_rates_size + sizeof(uint8_t) + 2;
+	
+	size_t buffer_size = sizeof(ieee80211_mgmt_header_t) + payload_size;
+	void *buffer = malloc(buffer_size);
+	memset(buffer, 0, buffer_size);
+	
+	ieee80211_mgmt_header_t *mgmt_header = 
+		(ieee80211_mgmt_header_t *) buffer;
+	
+	mgmt_header->frame_ctrl = host2uint16_t_le(
+		IEEE80211_MGMT_FRAME | 
+		IEEE80211_MGMT_PROBE_REQ_FRAME
+		);
+	memcpy(mgmt_header->dest_addr, ieee80211_broadcast_mac_addr, ETH_ADDR);
+	memcpy(mgmt_header->src_addr, nic_address.address, ETH_ADDR);
+	memcpy(mgmt_header->bssid, ieee80211_broadcast_mac_addr, ETH_ADDR);
+	
+	/* Jump to payload -> header + padding. */
+	uint8_t *it = buffer + sizeof(ieee80211_mgmt_header_t) + 2;
+	
+	*it++ = IEEE80211_RATES_IE;
+	*it++ = data_rates_size;
+	memcpy(it, ieee80211bg_data_rates, data_rates_size);
+	it += data_rates_size;
+	
+	*it++ = IEEE80211_EXT_RATES_IE;
+	*it++ = ext_data_rates_size;
+	memcpy(it, ieee80211bg_ext_data_rates, ext_data_rates_size);
+	it += ext_data_rates_size;
+	
+	*it++ = IEEE80211_CHANNEL_IE;
+	*it++ = 1;
+	*it = ieee80211_freq_to_channel(ieee80211_dev->current_freq);
+	
+	ieee80211_dev->ops->tx_handler(ieee80211_dev, buffer, buffer_size);
+	
+	free(buffer);
+	
+	return EOK;
+}
+
+int ieee80211_probe_auth(ieee80211_dev_t *ieee80211_dev)
+{
+	uint8_t test_bssid[] = {0x14, 0xF6, 0x5A, 0xAF, 0x5E, 0xB7};
+	
+	nic_t *nic = nic_get_from_ddf_dev(ieee80211_dev->ddf_dev);
+	nic_address_t nic_address;
+	nic_query_address(nic, &nic_address);
+	
+	size_t buffer_size = sizeof(ieee80211_mgmt_header_t) + 
+		sizeof(ieee80211_auth_body_t);
+	void *buffer = malloc(buffer_size);
+	memset(buffer, 0, buffer_size);
+	
+	ieee80211_mgmt_header_t *mgmt_header = 
+		(ieee80211_mgmt_header_t *) buffer;
+	
+	mgmt_header->frame_ctrl = host2uint16_t_le(
+		IEEE80211_MGMT_FRAME | 
+		IEEE80211_MGMT_AUTH_FRAME
+		);
+	memcpy(mgmt_header->dest_addr, test_bssid, ETH_ADDR);
+	memcpy(mgmt_header->src_addr, nic_address.address, ETH_ADDR);
+	memcpy(mgmt_header->bssid, test_bssid, ETH_ADDR);
+	
+	ieee80211_auth_body_t *auth_body =
+		(ieee80211_auth_body_t *) 
+		(buffer + sizeof(ieee80211_mgmt_header_t));
+	auth_body->auth_alg = host2uint16_t_le(0);
+	auth_body->auth_trans_no = host2uint16_t_le(0);
+	
+	ieee80211_dev->ops->tx_handler(ieee80211_dev, buffer, buffer_size);
+	
+	free(buffer);
+	
+	return EOK;
+}
+
 /** @}
  */
Index: uspace/lib/net/ieee80211/ieee80211_impl.c
===================================================================
--- uspace/lib/net/ieee80211/ieee80211_impl.c	(revision 4cb0148f513840557c24fabe8b6c9b7fe5b13ef0)
+++ uspace/lib/net/ieee80211/ieee80211_impl.c	(revision 9e5a51c3f68bce51a13b957d870bf35d97aa6fad)
@@ -40,25 +40,4 @@
 #include <ieee80211_impl.h>
 
-static int ieee80211_freq_to_channel(uint16_t freq)
-{
-	return (freq - IEEE80211_FIRST_FREQ) / IEEE80211_CHANNEL_GAP + 1;
-}
-
-static int ieee80211_probe_request(ieee80211_dev_t *ieee80211_dev)
-{
-	size_t buffer_size = sizeof(ieee80211_header_t);
-	void *buffer = malloc(buffer_size);
-	
-	/* TODO */
-	
-	ieee80211_freq_to_channel(ieee80211_dev->current_freq);
-	
-	ieee80211_dev->ops->tx_handler(ieee80211_dev, buffer, buffer_size);
-	
-	free(buffer);
-	
-	return EOK;
-}
-
 /**
  * Default implementation of IEEE802.11 scan function.
@@ -70,8 +49,19 @@
 int ieee80211_scan_impl(ieee80211_dev_t *ieee80211_dev)
 {
-	/* TODO */
-	int rc = ieee80211_probe_request(ieee80211_dev);
-	if(rc != EOK)
-		return rc;
+	uint16_t orig_freq = ieee80211_dev->current_freq;
+	
+	for(uint16_t freq = IEEE80211_FIRST_FREQ;
+		freq <= IEEE80211_MAX_FREQ; 
+		freq += IEEE80211_CHANNEL_GAP) {
+		ieee80211_dev->ops->set_freq(ieee80211_dev, freq);
+		ieee80211_probe_request(ieee80211_dev);
+		
+		/* Wait for probe responses. */
+		usleep(100000);
+	}
+	
+	ieee80211_dev->ops->set_freq(ieee80211_dev, orig_freq);
+	
+	/* TODO: Collect results. */
 	
 	return EOK;
Index: uspace/lib/net/include/ieee80211.h
===================================================================
--- uspace/lib/net/include/ieee80211.h	(revision 4cb0148f513840557c24fabe8b6c9b7fe5b13ef0)
+++ uspace/lib/net/include/ieee80211.h	(revision 9e5a51c3f68bce51a13b957d870bf35d97aa6fad)
@@ -52,7 +52,4 @@
 #define IEEE80211_CHANNEL_GAP 5
 
-#define IEEE80211_FRAME_CTRL_FRAME_TYPE 0x000C
-#define IEEE80211_FRAME_CTRL_DATA_FRAME 0x0008
-
 struct ieee80211_dev;
 
@@ -65,4 +62,34 @@
 } ieee80211_operating_mode_t;
 
+/** IEEE 802.11 frame types. */
+typedef enum {
+	IEEE80211_MGMT_FRAME = 0x0,
+	IEEE80211_CTRL_FRAME = 0x4,
+	IEEE80211_DATA_FRAME = 0x8,
+	IEEE80211_EXT_FRAME = 0xC
+} ieee80211_frame_type_t;
+
+/** IEEE 802.11 frame subtypes. */
+typedef enum {
+	IEEE80211_MGMT_ASSOC_REQ_FRAME = 0x00,
+	IEEE80211_MGMT_ASSOC_RESP_FRAME = 0x10,
+	IEEE80211_MGMT_REASSOC_REQ_FRAME = 0x20,
+	IEEE80211_MGMT_REASSOC_RESP_FRAME = 0x30,
+	IEEE80211_MGMT_PROBE_REQ_FRAME = 0x40,
+	IEEE80211_MGMT_PROBE_RESP_FRAME = 0x50,
+	IEEE80211_MGMT_BEACON_FRAME = 0x80,
+	IEEE80211_MGMT_DIASSOC_FRAME = 0xA0,
+	IEEE80211_MGMT_AUTH_FRAME = 0xB0,
+	IEEE80211_MGMT_DEAUTH_FRAME = 0xC0,
+} ieee80211_frame_subtype_t;
+
+/** IEEE 802.11 information element types. */
+typedef enum {
+	IEEE80211_SSID_IE = 0,		/**< Target SSID. */
+	IEEE80211_RATES_IE = 1,		/**< Supported data rates. */
+	IEEE80211_CHANNEL_IE = 3,	/**< Current channel number. */
+	IEEE80211_EXT_RATES_IE = 50	/**< Extended data rates. */
+} ieee80211_ie_type_t;
+
 /** IEEE 802.11 functions. */
 typedef struct {
@@ -70,4 +97,5 @@
 	int (*scan)(struct ieee80211_dev *);
 	int (*tx_handler)(struct ieee80211_dev *, void *, size_t);
+	int (*set_freq)(struct ieee80211_dev *, uint16_t);
 } ieee80211_ops_t;
 
@@ -89,4 +117,7 @@
 	ieee80211_operating_mode_t current_op_mode;
 	
+	/** BSSIDs we listen to. */
+	uint8_t bssid_mask[ETH_ADDR];
+	
 	/* TODO: Probably to be removed later - nic.open function is now 
 	 * executed multiple times, have to find out reason and fix it. 
@@ -96,5 +127,15 @@
 } ieee80211_dev_t;
 
-/** IEEE 802.11 header structure. */
+/** IEEE 802.11 management header structure. */
+typedef struct {
+	uint16_t frame_ctrl;		/**< Little Endian value! */
+	uint16_t duration_id;		/**< Little Endian value! */
+	uint8_t dest_addr[ETH_ADDR];
+	uint8_t src_addr[ETH_ADDR];
+	uint8_t bssid[ETH_ADDR];
+	uint16_t seq_ctrl;		/**< Little Endian value! */
+} __attribute__((packed)) __attribute__ ((aligned(2))) ieee80211_mgmt_header_t;
+
+/** IEEE 802.11 data header structure. */
 typedef struct {
 	uint16_t frame_ctrl;		/**< Little Endian value! */
@@ -105,11 +146,31 @@
 	uint16_t seq_ctrl;		/**< Little Endian value! */
 	uint8_t address4[ETH_ADDR];
-} __attribute__((packed)) __attribute__ ((aligned(2))) ieee80211_header_t;
+	uint16_t qos_ctrl;		/**< Little Endian value! */
+} __attribute__((packed)) __attribute__ ((aligned(2))) ieee80211_data_header_t;
 
-extern bool ieee80211_is_data_frame(ieee80211_header_t *header);
+/** IEEE 802.11 information element header. */
+typedef struct {
+	uint8_t element_id;
+	uint8_t length;
+} __attribute__((packed)) __attribute__ ((aligned(2))) ieee80211_ie_header_t;
+
+/** IEEE 802.11 authentication frame body. */
+typedef struct {
+	uint16_t auth_alg;		/**< Little Endian value! */
+	uint16_t auth_trans_no;		/**< Little Endian value! */
+	uint16_t status;		/**< Little Endian value! */
+} __attribute__((packed)) __attribute__ ((aligned(2))) ieee80211_auth_body_t;
+
+typedef struct {
+	uint8_t bssid[ETH_ADDR];
+	uint16_t auth_alg;
+} __attribute__((packed)) __attribute__ ((aligned(2))) ieee80211_auth_data_t;
+
 extern int ieee80211_device_init(ieee80211_dev_t *ieee80211_dev, 
 	void *driver_data, ddf_dev_t *ddf_dev);
 extern int ieee80211_init(ieee80211_dev_t *ieee80211_dev, 
 	ieee80211_ops_t *ieee80211_ops);
+extern int ieee80211_probe_request(ieee80211_dev_t *ieee80211_dev);
+extern int ieee80211_probe_auth(ieee80211_dev_t *ieee80211_dev);
 
 #endif /* LIBNET_IEEE80211_H */