Update 802.11 wireless support:

o major overhaul of the way channels are handled: channels are now
  fully enumerated and uniquely identify the operating characteristics;
  these changes are visible to user applications which require changes
o make scanning support independent of the state machine to enable
  background scanning and roaming
o move scanning support into loadable modules based on the operating
  mode to enable different policies and reduce the memory footprint
  on systems w/ constrained resources
o add background scanning in station mode (no support for adhoc/ibss
  mode yet)
o significantly speedup sta mode scanning with a variety of techniques
o add roaming support when background scanning is supported; for now
  we use a simple algorithm to trigger a roam: we threshold the rssi
  and tx rate, if either drops too low we try to roam to a new ap
o add tx fragmentation support
o add first cut at 802.11n support: this code works with forthcoming
  drivers but is incomplete; it's included now to establish a baseline
  for other drivers to be developed and for user applications
o adjust max_linkhdr et. al. to reflect 802.11 requirements; this eliminates
  prepending mbufs for traffic generated locally
o add support for Atheros protocol extensions; mainly the fast frames
  encapsulation (note this can be used with any card that can tx+rx
  large frames correctly)
o add sta support for ap's that beacon both WPA1+2 support
o change all data types from bsd-style to posix-style
o propagate noise floor data from drivers to net80211 and on to user apps
o correct various issues in the sta mode state machine related to handling
  authentication and association failures
o enable the addition of sta mode power save support for drivers that need
  net80211 support (not in this commit)
o remove old WI compatibility ioctls (wicontrol is officially dead)
o change the data structures returned for get sta info and get scan
  results so future additions will not break user apps
o fixed tx rate is now maintained internally as an ieee rate and not an
  index into the rate set; this needs to be extended to deal with
  multi-mode operation
o add extended channel specifications to radiotap to enable 11n sniffing

Drivers:
o ath: add support for bg scanning, tx fragmentation, fast frames,
       dynamic turbo (lightly tested), 11n (sniffing only and needs
       new hal)
o awi: compile tested only
o ndis: lightly tested
o ipw: lightly tested
o iwi: add support for bg scanning (well tested but may have some
       rough edges)
o ral, ural, rum: add suppoort for bg scanning, calibrate rssi data
o wi: lightly tested

This work is based on contributions by Atheros, kmacy, sephe, thompsa,
mlaier, kevlo, and others.  Much of the scanning work was supported by
Atheros.  The 11n work was supported by Marvell.

7 files changed, 391 insertions, 276 deletions

diff --git a/sys/dev/ral/if_ral_pci.c b/sys/dev/ral/if_ral_pci.c
index 3d9976d..2ceafe4 100644
--- a/sys/dev/ral/if_ral_pci.c
+++ b/sys/dev/ral/if_ral_pci.c
@@ -87,8 +87,8 @@ static struct ral_opns {
 }  ral_rt2560_opns = {
 	rt2560_attach,
 	rt2560_detach,
-	rt2560_shutdown,
-	rt2560_suspend,
+	rt2560_stop,
+	rt2560_stop,
 	rt2560_resume,
 	rt2560_intr
 
@@ -192,7 +192,8 @@ ral_pci_attach(device_t dev)
 
 	sc->sc_st = rman_get_bustag(psc->mem);
 	sc->sc_sh = rman_get_bushandle(psc->mem);
-
+	sc->sc_invalid = 1;
+	
 	psc->irq_rid = 0;
 	psc->irq = bus_alloc_resource_any(dev, SYS_RES_IRQ, &psc->irq_rid,
 	    RF_ACTIVE | RF_SHAREABLE);
@@ -214,7 +215,8 @@ ral_pci_attach(device_t dev)
 		device_printf(dev, "could not set up interrupt\n");
 		return error;
 	}
-
+	sc->sc_invalid = 0;
+	
 	return 0;
 }
 
@@ -222,7 +224,11 @@ static int
 ral_pci_detach(device_t dev)
 {
 	struct ral_pci_softc *psc = device_get_softc(dev);
-
+	struct rt2560_softc *sc = &psc->u.sc_rt2560;
+	
+	/* check if device was removed */
+	sc->sc_invalid = !bus_child_present(dev);
+	
 	(*psc->sc_opns->detach)(psc);
 
 	bus_generic_detach(dev);
diff --git a/sys/dev/ral/rt2560.c b/sys/dev/ral/rt2560.c
index 36a849d..f09c4f3 100644
--- a/sys/dev/ral/rt2560.c
+++ b/sys/dev/ral/rt2560.c
@@ -53,6 +53,7 @@ __FBSDID("$FreeBSD$");
 
 #include <net80211/ieee80211_var.h>
 #include <net80211/ieee80211_radiotap.h>
+#include <net80211/ieee80211_regdomain.h>
 
 #include <netinet/in.h>
 #include <netinet/in_systm.h>
@@ -64,6 +65,10 @@ __FBSDID("$FreeBSD$");
 #include <dev/ral/rt2560reg.h>
 #include <dev/ral/rt2560var.h>
 
+#define RT2560_RSSI(sc, rssi)					\
+	((rssi) > (RT2560_NOISE_FLOOR + (sc)->rssi_corr) ?	\
+	 ((rssi) - RT2560_NOISE_FLOOR - (sc)->rssi_corr) : 0)
+
 #ifdef RAL_DEBUG
 #define DPRINTF(x)	do { if (ral_debug > 0) printf x; } while (0)
 #define DPRINTFN(n, x)	do { if (ral_debug >= (n)) printf x; } while (0)
@@ -90,7 +95,6 @@ static void		rt2560_free_rx_ring(struct rt2560_softc *,
 static struct		ieee80211_node *rt2560_node_alloc(
 			    struct ieee80211_node_table *);
 static int		rt2560_media_change(struct ifnet *);
-static void		rt2560_next_scan(void *);
 static void		rt2560_iter_func(void *, struct ieee80211_node *);
 static void		rt2560_update_rssadapt(void *);
 static int		rt2560_newstate(struct ieee80211com *,
@@ -105,6 +109,9 @@ static void		rt2560_beacon_expire(struct rt2560_softc *);
 static void		rt2560_wakeup_expire(struct rt2560_softc *);
 static uint8_t		rt2560_rxrate(struct rt2560_rx_desc *);
 static int		rt2560_ack_rate(struct ieee80211com *, int);
+static void		rt2560_scan_start(struct ieee80211com *);
+static void		rt2560_scan_end(struct ieee80211com *);
+static void		rt2560_set_channel(struct ieee80211com *);
 static uint16_t		rt2560_txtime(int, int, uint32_t);
 static uint8_t		rt2560_plcp_signal(int);
 static void		rt2560_setup_tx_desc(struct rt2560_softc *,
@@ -137,7 +144,7 @@ static void		rt2560_update_plcp(struct rt2560_softc *);
 static void		rt2560_update_slot(struct ifnet *);
 static void		rt2560_set_basicrates(struct rt2560_softc *);
 static void		rt2560_update_led(struct rt2560_softc *, int, int);
-static void		rt2560_set_bssid(struct rt2560_softc *, uint8_t *);
+static void		rt2560_set_bssid(struct rt2560_softc *, const uint8_t *);
 static void		rt2560_set_macaddr(struct rt2560_softc *, uint8_t *);
 static void		rt2560_get_macaddr(struct rt2560_softc *, uint8_t *);
 static void		rt2560_update_promisc(struct rt2560_softc *);
@@ -147,7 +154,6 @@ static int		rt2560_bbp_init(struct rt2560_softc *);
 static void		rt2560_set_txantenna(struct rt2560_softc *, int);
 static void		rt2560_set_rxantenna(struct rt2560_softc *, int);
 static void		rt2560_init(void *);
-static void		rt2560_stop(void *);
 static int		rt2560_raw_xmit(struct ieee80211_node *, struct mbuf *,
 				const struct ieee80211_bpf_params *);
 
@@ -187,7 +193,7 @@ rt2560_attach(device_t dev, int id)
 	struct rt2560_softc *sc = device_get_softc(dev);
 	struct ieee80211com *ic = &sc->sc_ic;
 	struct ifnet *ifp;
-	int error, i;
+	int error, bands;
 
 	sc->sc_dev = dev;
 
@@ -195,7 +201,6 @@ rt2560_attach(device_t dev, int id)
 	    MTX_DEF | MTX_RECURSE);
 
 	callout_init_mtx(&sc->watchdog_ch, &sc->sc_mtx, 0);
-	callout_init(&sc->scan_ch, debug_mpsafenet ? CALLOUT_MPSAFE : 0);
 	callout_init(&sc->rssadapt_ch, CALLOUT_MPSAFE);
 
 	/* retrieve RT2560 rev. no */
@@ -272,37 +277,20 @@ rt2560_attach(device_t dev, int id)
 	    IEEE80211_C_TXPMGT |	/* tx power management */
 	    IEEE80211_C_SHPREAMBLE |	/* short preamble supported */
 	    IEEE80211_C_SHSLOT |	/* short slot time supported */
+	    IEEE80211_C_BGSCAN |	/* bg scanning support */
 	    IEEE80211_C_WPA;		/* 802.11i */
 
-	if (sc->rf_rev == RT2560_RF_5222) {
-		/* set supported .11a channels */
-		for (i = 36; i <= 64; i += 4) {
-			ic->ic_channels[i].ic_freq =
-			    ieee80211_ieee2mhz(i, IEEE80211_CHAN_5GHZ);
-			ic->ic_channels[i].ic_flags = IEEE80211_CHAN_A;
-		}
-		for (i = 100; i <= 140; i += 4) {
-			ic->ic_channels[i].ic_freq =
-			    ieee80211_ieee2mhz(i, IEEE80211_CHAN_5GHZ);
-			ic->ic_channels[i].ic_flags = IEEE80211_CHAN_A;
-		}
-		for (i = 149; i <= 161; i += 4) {
-			ic->ic_channels[i].ic_freq =
-			    ieee80211_ieee2mhz(i, IEEE80211_CHAN_5GHZ);
-			ic->ic_channels[i].ic_flags = IEEE80211_CHAN_A;
-		}
-	}
-
-	/* set supported .11b and .11g channels (1 through 14) */
-	for (i = 1; i <= 14; i++) {
-		ic->ic_channels[i].ic_freq =
-		    ieee80211_ieee2mhz(i, IEEE80211_CHAN_2GHZ);
-		ic->ic_channels[i].ic_flags =
-		    IEEE80211_CHAN_CCK | IEEE80211_CHAN_OFDM |
-		    IEEE80211_CHAN_DYN | IEEE80211_CHAN_2GHZ;
-	}
+	bands = 0;
+	setbit(&bands, IEEE80211_MODE_11B);
+	setbit(&bands, IEEE80211_MODE_11G);
+	if (sc->rf_rev == RT2560_RF_5222)
+		setbit(&bands, IEEE80211_MODE_11A);
+	ieee80211_init_channels(ic, 0, CTRY_DEFAULT, bands, 0, 1);
 
 	ieee80211_ifattach(ic);
+	ic->ic_scan_start = rt2560_scan_start;
+	ic->ic_scan_end = rt2560_scan_end;
+	ic->ic_set_channel = rt2560_set_channel;
 	ic->ic_node_alloc = rt2560_node_alloc;
 	ic->ic_updateslot = rt2560_update_slot;
 	ic->ic_reset = rt2560_reset;
@@ -365,10 +353,9 @@ rt2560_detach(void *xsc)
 	struct rt2560_softc *sc = xsc;
 	struct ieee80211com *ic = &sc->sc_ic;
 	struct ifnet *ifp = ic->ic_ifp;
-
+	
 	rt2560_stop(sc);
 	callout_stop(&sc->watchdog_ch);
-	callout_stop(&sc->scan_ch);
 	callout_stop(&sc->rssadapt_ch);
 
 	bpfdetach(ifp);
@@ -388,22 +375,6 @@ rt2560_detach(void *xsc)
 }
 
 void
-rt2560_shutdown(void *xsc)
-{
-	struct rt2560_softc *sc = xsc;
-
-	rt2560_stop(sc);
-}
-
-void
-rt2560_suspend(void *xsc)
-{
-	struct rt2560_softc *sc = xsc;
-
-	rt2560_stop(sc);
-}
-
-void
 rt2560_resume(void *xsc)
 {
 	struct rt2560_softc *sc = xsc;
@@ -733,28 +704,13 @@ rt2560_media_change(struct ifnet *ifp)
 	int error;
 
 	error = ieee80211_media_change(ifp);
-	if (error != ENETRESET)
-		return error;
-
-	if ((ifp->if_flags & IFF_UP) &&
-	    (ifp->if_drv_flags & IFF_DRV_RUNNING))
-		rt2560_init(sc);
-
-	return 0;
-}
-
-/*
- * This function is called periodically (every 200ms) during scanning to
- * switch from one channel to another.
- */
-static void
-rt2560_next_scan(void *arg)
-{
-	struct rt2560_softc *sc = arg;
-	struct ieee80211com *ic = &sc->sc_ic;
 
-	if (ic->ic_state == IEEE80211_S_SCAN)
-		ieee80211_next_scan(ic);
+	if (error == ENETRESET) {
+		if ((ifp->if_flags & IFF_UP) &&
+		    (ifp->if_drv_flags & IFF_DRV_RUNNING))
+		        rt2560_init(sc);
+	}
+	return error;
 }
 
 /*
@@ -796,7 +752,6 @@ rt2560_newstate(struct ieee80211com *ic, enum ieee80211_state nstate, int arg)
 	int error = 0;
 
 	ostate = ic->ic_state;
-	callout_stop(&sc->scan_ch);
 
 	switch (nstate) {
 	case IEEE80211_S_INIT:
@@ -810,24 +765,7 @@ rt2560_newstate(struct ieee80211com *ic, enum ieee80211_state nstate, int arg)
 			rt2560_update_led(sc, 0, 0);
 		}
 		break;
-
-	case IEEE80211_S_SCAN:
-		rt2560_set_chan(sc, ic->ic_curchan);
-		callout_reset(&sc->scan_ch, (sc->dwelltime * hz) / 1000,
-		    rt2560_next_scan, sc);
-		break;
-
-	case IEEE80211_S_AUTH:
-		rt2560_set_chan(sc, ic->ic_curchan);
-		break;
-
-	case IEEE80211_S_ASSOC:
-		rt2560_set_chan(sc, ic->ic_curchan);
-		break;
-
 	case IEEE80211_S_RUN:
-		rt2560_set_chan(sc, ic->ic_curchan);
-
 		ni = ic->ic_bss;
 
 		if (ic->ic_opmode != IEEE80211_M_MONITOR) {
@@ -862,6 +800,10 @@ rt2560_newstate(struct ieee80211com *ic, enum ieee80211_state nstate, int arg)
 			rt2560_enable_tsf_sync(sc);
 		}
 		break;
+	case IEEE80211_S_SCAN:
+	case IEEE80211_S_AUTH:
+	case IEEE80211_S_ASSOC:
+		break;
 	}
 
 	return (error != 0) ? error : sc->sc_newstate(ic, nstate, arg);
@@ -1060,6 +1002,9 @@ rt2560_prio_intr(struct rt2560_softc *sc)
 	struct ifnet *ifp = ic->ic_ifp;
 	struct rt2560_tx_desc *desc;
 	struct rt2560_tx_data *data;
+	struct ieee80211_node *ni;
+	struct mbuf *m;
+	int flags;
 
 	bus_dmamap_sync(sc->prioq.desc_dmat, sc->prioq.desc_map,
 	    BUS_DMASYNC_POSTREAD);
@@ -1068,18 +1013,18 @@ rt2560_prio_intr(struct rt2560_softc *sc)
 		desc = &sc->prioq.desc[sc->prioq.next];
 		data = &sc->prioq.data[sc->prioq.next];
 
-		if ((le32toh(desc->flags) & RT2560_TX_BUSY) ||
-		    !(le32toh(desc->flags) & RT2560_TX_VALID))
+		flags = le32toh(desc->flags);
+		if ((flags & RT2560_TX_BUSY) || (flags & RT2560_TX_VALID) == 0)
 			break;
 
-		switch (le32toh(desc->flags) & RT2560_TX_RESULT_MASK) {
+		switch (flags & RT2560_TX_RESULT_MASK) {
 		case RT2560_TX_SUCCESS:
 			DPRINTFN(10, ("mgt frame sent successfully\n"));
 			break;
 
 		case RT2560_TX_SUCCESS_RETRY:
 			DPRINTFN(9, ("mgt frame sent after %u retries\n",
-			    (le32toh(desc->flags) >> 5) & 0x7));
+			    (flags >> 5) & 0x7));
 			break;
 
 		case RT2560_TX_FAIL_RETRY:
@@ -1091,15 +1036,17 @@ rt2560_prio_intr(struct rt2560_softc *sc)
 		case RT2560_TX_FAIL_OTHER:
 		default:
 			device_printf(sc->sc_dev, "sending mgt frame failed "
-			    "0x%08x\n", le32toh(desc->flags));
+			    "0x%08x\n", flags);
+			break;
 		}
 
 		bus_dmamap_sync(sc->prioq.data_dmat, data->map,
 		    BUS_DMASYNC_POSTWRITE);
 		bus_dmamap_unload(sc->prioq.data_dmat, data->map);
-		m_freem(data->m);
+
+		m = data->m;
 		data->m = NULL;
-		ieee80211_free_node(data->ni);
+		ni = data->ni;
 		data->ni = NULL;
 
 		/* descriptor is no longer valid */
@@ -1109,6 +1056,13 @@ rt2560_prio_intr(struct rt2560_softc *sc)
 
 		sc->prioq.queued--;
 		sc->prioq.next = (sc->prioq.next + 1) % RT2560_PRIO_RING_COUNT;
+
+		if (m->m_flags & M_TXCB)
+			ieee80211_process_callback(ni, m,
+				(flags & RT2560_TX_RESULT_MASK) &~
+				(RT2560_TX_SUCCESS | RT2560_TX_SUCCESS_RETRY));
+		m_freem(m);
+		ieee80211_free_node(ni);
 	}
 
 	bus_dmamap_sync(sc->prioq.desc_dmat, sc->prioq.desc_map,
@@ -1227,25 +1181,31 @@ rt2560_decryption_intr(struct rt2560_softc *sc)
 			tap->wr_chan_freq = htole16(ic->ic_curchan->ic_freq);
 			tap->wr_chan_flags = htole16(ic->ic_curchan->ic_flags);
 			tap->wr_antenna = sc->rx_ant;
-			tap->wr_antsignal = desc->rssi;
+			tap->wr_antsignal = RT2560_RSSI(sc, desc->rssi);
 
 			bpf_mtap2(sc->sc_drvbpf, tap, sc->sc_rxtap_len, m);
 		}
 
+		sc->sc_flags |= RAL_INPUT_RUNNING;
+		RAL_UNLOCK(sc);
 		wh = mtod(m, struct ieee80211_frame *);
 		ni = ieee80211_find_rxnode(ic,
 		    (struct ieee80211_frame_min *)wh);
 
 		/* send the frame to the 802.11 layer */
-		ieee80211_input(ic, m, ni, desc->rssi, 0);
+		ieee80211_input(ic, m, ni, RT2560_RSSI(sc, desc->rssi),
+				RT2560_NOISE_FLOOR, 0);
 
 		/* give rssi to the rate adatation algorithm */
 		rn = (struct rt2560_node *)ni;
-		ral_rssadapt_input(ic, ni, &rn->rssadapt, desc->rssi);
+		ral_rssadapt_input(ic, ni, &rn->rssadapt,
+				   RT2560_RSSI(sc, desc->rssi));
 
 		/* node is no longer needed */
 		ieee80211_free_node(ni);
 
+		RAL_LOCK(sc);
+		sc->sc_flags &= ~RAL_INPUT_RUNNING;
 skip:		desc->flags = htole32(RT2560_RX_BUSY);
 
 		DPRINTFN(15, ("decryption done idx=%u\n", sc->rxq.cur_decrypt));
@@ -1324,9 +1284,14 @@ rt2560_beacon_expire(struct rt2560_softc *sc)
 
 	if (ic->ic_opmode != IEEE80211_M_IBSS &&
 	    ic->ic_opmode != IEEE80211_M_HOSTAP)
-		return;
+		return;	
 
 	data = &sc->bcnq.data[sc->bcnq.next];
+	/*
+	 * Don't send beacon if bsschan isn't set
+	 */
+	if (data->ni == NULL)
+	        return;
 
 	bus_dmamap_sync(sc->bcnq.data_dmat, data->map, BUS_DMASYNC_POSTWRITE);
 	bus_dmamap_unload(sc->bcnq.data_dmat, data->map);
@@ -1808,7 +1773,6 @@ rt2560_tx_data(struct rt2560_softc *sc, struct mbuf *m0,
 	struct rt2560_tx_desc *desc;
 	struct rt2560_tx_data *data;
 	struct rt2560_node *rn;
-	struct ieee80211_rateset *rs;
 	struct ieee80211_frame *wh;
 	struct ieee80211_key *k;
 	struct mbuf *mnew;
@@ -1820,9 +1784,10 @@ rt2560_tx_data(struct rt2560_softc *sc, struct mbuf *m0,
 	wh = mtod(m0, struct ieee80211_frame *);
 
 	if (ic->ic_fixed_rate != IEEE80211_FIXED_RATE_NONE) {
-		rs = &ic->ic_sup_rates[ic->ic_curmode];
-		rate = rs->rs_rates[ic->ic_fixed_rate];
+		rate = ic->ic_fixed_rate;
 	} else {
+		struct ieee80211_rateset *rs;
+
 		rs = &ni->ni_rates;
 		rn = (struct rt2560_node *)ni;
 		ni->ni_txrate = ral_rssadapt_choose(&rn->rssadapt, rs, wh,
@@ -2020,9 +1985,10 @@ rt2560_start(struct ifnet *ifp)
 			if (bpf_peers_present(ic->ic_rawbpf))
 				bpf_mtap(ic->ic_rawbpf, m0);
 
-			if (rt2560_tx_mgt(sc, m0, ni) != 0)
+			if (rt2560_tx_mgt(sc, m0, ni) != 0) {
+				ieee80211_free_node(ni);
 				break;
-
+			}
 		} else {
 			if (ic->ic_state != IEEE80211_S_RUN)
 				break;
@@ -2045,6 +2011,28 @@ rt2560_start(struct ifnet *ifp)
 				m_freem(m0);
 				continue;
 			}
+			if ((ni->ni_flags & IEEE80211_NODE_PWR_MGT) &&
+			    (m0->m_flags & M_PWR_SAV) == 0) {
+				/*
+				 * Station in power save mode; pass the frame
+				 * to the 802.11 layer and continue.  We'll get
+				 * the frame back when the time is right.
+				 */
+				ieee80211_pwrsave(ni, m0);
+				/*
+				 * If we're in power save mode 'cuz of a bg
+				 * scan cancel it so the traffic can flow.
+				 * The packet we just queued will automatically
+				 * get sent when we drop out of power save.
+				 * XXX locking
+				 */
+				if (ic->ic_flags & IEEE80211_F_SCAN)
+					ieee80211_cancel_scan(ic);
+				ieee80211_free_node(ni);
+				continue;
+
+			}
+
 			BPF_MTAP(ifp, m0);
 
 			m0 = ieee80211_encap(ic, m0, ni);
@@ -2052,7 +2040,7 @@ rt2560_start(struct ifnet *ifp)
 				ieee80211_free_node(ni);
 				continue;
 			}
-
+			
 			if (bpf_peers_present(ic->ic_rawbpf))
 				bpf_mtap(ic->ic_rawbpf, m0);
 
@@ -2074,7 +2062,6 @@ static void
 rt2560_watchdog(void *arg)
 {
 	struct rt2560_softc *sc = arg;
-	struct ieee80211com *ic = &sc->sc_ic;
 
 	if (sc->sc_tx_timer > 0) {
 		if (--sc->sc_tx_timer == 0) {
@@ -2085,8 +2072,6 @@ rt2560_watchdog(void *arg)
 		}
 		callout_reset(&sc->watchdog_ch, hz, rt2560_watchdog, sc);
 	}
-
-	ieee80211_watchdog(ic);
 }
 
 /*
@@ -2115,19 +2100,22 @@ rt2560_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 	struct ieee80211com *ic = &sc->sc_ic;
 	int error = 0;
 
-	RAL_LOCK(sc);
+
 
 	switch (cmd) {
 	case SIOCSIFFLAGS:
 		if (ifp->if_flags & IFF_UP) {
+			RAL_LOCK(sc);
 			if (ifp->if_drv_flags & IFF_DRV_RUNNING)
 				rt2560_update_promisc(sc);
 			else
 				rt2560_init(sc);
+			RAL_UNLOCK(sc);
 		} else {
 			if (ifp->if_drv_flags & IFF_DRV_RUNNING)
 				rt2560_stop(sc);
 		}
+
 		break;
 
 	default:
@@ -2142,7 +2130,6 @@ rt2560_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 		error = 0;
 	}
 
-	RAL_UNLOCK(sc);
 
 	return error;
 }
@@ -2295,6 +2282,8 @@ rt2560_set_chan(struct rt2560_softc *sc, struct ieee80211_channel *c)
 		rt2560_rf_write(sc, RAL_RF3, power << 7 | 0x00040);
 		rt2560_rf_write(sc, RAL_RF4, rt2560_rf5222[i].r4);
 		break;
+	default: 
+ 	        printf("unknown ral rev=%d\n", sc->rf_rev);
 	}
 
 	if (ic->ic_state != IEEE80211_S_SCAN) {
@@ -2312,6 +2301,18 @@ rt2560_set_chan(struct rt2560_softc *sc, struct ieee80211_channel *c)
 	}
 }
 
+static void
+rt2560_set_channel(struct ieee80211com *ic)
+{
+	struct ifnet *ifp = ic->ic_ifp;
+	struct rt2560_softc *sc = ifp->if_softc;
+
+	RAL_LOCK(sc);
+	rt2560_set_chan(sc, ic->ic_curchan);
+	RAL_UNLOCK(sc);
+
+}
+
 #if 0
 /*
  * Disable RF auto-tuning.
@@ -2456,7 +2457,7 @@ rt2560_update_led(struct rt2560_softc *sc, int led1, int led2)
 }
 
 static void
-rt2560_set_bssid(struct rt2560_softc *sc, uint8_t *bssid)
+rt2560_set_bssid(struct rt2560_softc *sc, const uint8_t *bssid)
 {
 	uint32_t tmp;
 
@@ -2559,6 +2560,37 @@ rt2560_read_eeprom(struct rt2560_softc *sc)
 		sc->txpow[i * 2] = val >> 8;
 		sc->txpow[i * 2 + 1] = val & 0xff;
 	}
+
+	val = rt2560_eeprom_read(sc, RT2560_EEPROM_CALIBRATE);
+	if ((val & 0xff) == 0xff)
+		sc->rssi_corr = RT2560_DEFAULT_RSSI_CORR;
+	else
+		sc->rssi_corr = val & 0xff;
+	DPRINTF(("rssi correction %d, calibrate 0x%02x\n",
+		 sc->rssi_corr, val));
+}
+
+
+static void
+rt2560_scan_start(struct ieee80211com *ic)
+{
+	struct ifnet *ifp = ic->ic_ifp;
+	struct rt2560_softc *sc = ifp->if_softc;
+
+	/* abort TSF synchronization */
+	RAL_WRITE(sc, RT2560_CSR14, 0);
+	rt2560_set_bssid(sc, ifp->if_broadcastaddr);
+}
+
+static void
+rt2560_scan_end(struct ieee80211com *ic)
+{
+	struct ifnet *ifp = ic->ic_ifp;
+	struct rt2560_softc *sc = ifp->if_softc;
+
+	rt2560_enable_tsf_sync(sc);
+	/* XXX keep local copy */
+	rt2560_set_bssid(sc, ic->ic_bss->ni_bssid);
 }
 
 static int
@@ -2653,10 +2685,11 @@ rt2560_init(void *priv)
 	uint32_t tmp;
 	int i;
 
-	RAL_LOCK(sc);
+
 
 	rt2560_stop(sc);
 
+	RAL_LOCK(sc);
 	/* setup tx rings */
 	tmp = RT2560_PRIO_RING_COUNT << 24 |
 	      RT2560_ATIM_RING_COUNT << 16 |
@@ -2739,36 +2772,44 @@ rt2560_init(void *priv)
 }
 
 void
-rt2560_stop(void *priv)
+rt2560_stop(void *arg)
 {
-	struct rt2560_softc *sc = priv;
+	struct rt2560_softc *sc = arg;
 	struct ieee80211com *ic = &sc->sc_ic;
 	struct ifnet *ifp = ic->ic_ifp;
+	volatile int *flags = &sc->sc_flags;
 
-	sc->sc_tx_timer = 0;
-	ifp->if_drv_flags &= ~(IFF_DRV_RUNNING | IFF_DRV_OACTIVE);
-
-	ieee80211_new_state(ic, IEEE80211_S_INIT, -1);
-
-	/* abort Tx */
-	RAL_WRITE(sc, RT2560_TXCSR0, RT2560_ABORT_TX);
-
-	/* disable Rx */
-	RAL_WRITE(sc, RT2560_RXCSR0, RT2560_DISABLE_RX);
-
-	/* reset ASIC (imply reset BBP) */
-	RAL_WRITE(sc, RT2560_CSR1, RT2560_RESET_ASIC);
-	RAL_WRITE(sc, RT2560_CSR1, 0);
-
-	/* disable interrupts */
-	RAL_WRITE(sc, RT2560_CSR8, 0xffffffff);
+	while (*flags & RAL_INPUT_RUNNING) {
+		tsleep(sc, 0, "ralrunning", hz/10);
+	}
 
-	/* reset Tx and Rx rings */
-	rt2560_reset_tx_ring(sc, &sc->txq);
-	rt2560_reset_tx_ring(sc, &sc->atimq);
-	rt2560_reset_tx_ring(sc, &sc->prioq);
-	rt2560_reset_tx_ring(sc, &sc->bcnq);
-	rt2560_reset_rx_ring(sc, &sc->rxq);
+	RAL_LOCK(sc);
+	if (ifp->if_drv_flags & IFF_DRV_RUNNING) {
+		ieee80211_new_state(ic, IEEE80211_S_INIT, -1);
+		sc->sc_tx_timer = 0;
+		ifp->if_drv_flags &= ~(IFF_DRV_RUNNING | IFF_DRV_OACTIVE);
+
+		/* abort Tx */
+		RAL_WRITE(sc, RT2560_TXCSR0, RT2560_ABORT_TX);
+		
+		/* disable Rx */
+		RAL_WRITE(sc, RT2560_RXCSR0, RT2560_DISABLE_RX);
+
+		/* reset ASIC (imply reset BBP) */
+		RAL_WRITE(sc, RT2560_CSR1, RT2560_RESET_ASIC);
+		RAL_WRITE(sc, RT2560_CSR1, 0);
+
+		/* disable interrupts */
+		RAL_WRITE(sc, RT2560_CSR8, 0xffffffff);
+		
+		/* reset Tx and Rx rings */
+		rt2560_reset_tx_ring(sc, &sc->txq);
+		rt2560_reset_tx_ring(sc, &sc->atimq);
+		rt2560_reset_tx_ring(sc, &sc->prioq);
+		rt2560_reset_tx_ring(sc, &sc->bcnq);
+		rt2560_reset_rx_ring(sc, &sc->rxq);
+	}
+	RAL_UNLOCK(sc);
 }
 
 static int
@@ -2828,3 +2869,4 @@ bad:
 	RAL_UNLOCK(sc);
 	return EIO;		/* XXX */
 }
+
diff --git a/sys/dev/ral/rt2560reg.h b/sys/dev/ral/rt2560reg.h
index 6b1cf0d..3871ed3 100644
--- a/sys/dev/ral/rt2560reg.h
+++ b/sys/dev/ral/rt2560reg.h
@@ -17,6 +17,9 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#define RT2560_DEFAULT_RSSI_CORR	0x79
+#define RT2560_NOISE_FLOOR		-95
+
 #define RT2560_TX_RING_COUNT		48
 #define RT2560_ATIM_RING_COUNT		4
 #define RT2560_PRIO_RING_COUNT		16
@@ -296,6 +299,7 @@ struct rt2560_rx_desc {
 #define RT2560_EEPROM_CONFIG0	16
 #define RT2560_EEPROM_BBP_BASE	19
 #define RT2560_EEPROM_TXPOWER	35
+#define RT2560_EEPROM_CALIBRATE	62
 
 /*
  * control and status registers access macros
diff --git a/sys/dev/ral/rt2560var.h b/sys/dev/ral/rt2560var.h
index 50112b5..c01abb1 100644
--- a/sys/dev/ral/rt2560var.h
+++ b/sys/dev/ral/rt2560var.h
@@ -109,14 +109,18 @@ struct rt2560_softc {
 	struct mtx		sc_mtx;
 
 	struct callout		watchdog_ch;
-	struct callout		scan_ch;
 	struct callout		rssadapt_ch;
 
 	int			sc_tx_timer;
-
+	int                     sc_invalid;
+/*
+ * The same in both up to here
+ * ------------------------------------------------
+ */
 	uint32_t		asic_rev;
 	uint32_t		eeprom_rev;
 	uint8_t			rf_rev;
+	uint8_t			rssi_corr;
 
 	struct rt2560_tx_ring	txq;
 	struct rt2560_tx_ring	prioq;
@@ -157,12 +161,13 @@ struct rt2560_softc {
 	}			sc_txtapu;
 #define sc_txtap	sc_txtapu.th
 	int			sc_txtap_len;
+#define                 RAL_INPUT_RUNNING       1
+	int                     sc_flags;
 };
 
 int	rt2560_attach(device_t, int);
 int	rt2560_detach(void *);
-void	rt2560_shutdown(void *);
-void	rt2560_suspend(void *);
+void	rt2560_stop(void *);
 void	rt2560_resume(void *);
 void	rt2560_intr(void *);
 
diff --git a/sys/dev/ral/rt2661.c b/sys/dev/ral/rt2661.c
index 8f13eb6..224da32 100644
--- a/sys/dev/ral/rt2661.c
+++ b/sys/dev/ral/rt2661.c
@@ -53,6 +53,7 @@ __FBSDID("$FreeBSD$");
 
 #include <net80211/ieee80211_var.h>
 #include <net80211/ieee80211_radiotap.h>
+#include <net80211/ieee80211_regdomain.h>
 
 #include <netinet/in.h>
 #include <netinet/in_systm.h>
@@ -92,7 +93,6 @@ static void		rt2661_free_rx_ring(struct rt2661_softc *,
 static struct		ieee80211_node *rt2661_node_alloc(
 			    struct ieee80211_node_table *);
 static int		rt2661_media_change(struct ifnet *);
-static void		rt2661_next_scan(void *);
 static int		rt2661_newstate(struct ieee80211com *,
 			    enum ieee80211_state, int);
 static uint16_t		rt2661_eeprom_read(struct rt2661_softc *, uint8_t);
@@ -104,6 +104,9 @@ static void		rt2661_mcu_beacon_expire(struct rt2661_softc *);
 static void		rt2661_mcu_wakeup(struct rt2661_softc *);
 static void		rt2661_mcu_cmd_intr(struct rt2661_softc *);
 static int		rt2661_ack_rate(struct ieee80211com *, int);
+static void		rt2661_scan_start(struct ieee80211com *);
+static void		rt2661_scan_end(struct ieee80211com *);
+static void		rt2661_set_channel(struct ieee80211com *);
 static uint16_t		rt2661_txtime(int, int, uint32_t);
 static uint8_t		rt2661_rxrate(struct rt2661_rx_desc *);
 static uint8_t		rt2661_plcp_signal(int);
@@ -148,6 +151,7 @@ static void		rt2661_read_eeprom(struct rt2661_softc *);
 static int		rt2661_bbp_init(struct rt2661_softc *);
 static void		rt2661_init(void *);
 static void		rt2661_stop(void *);
+static void             rt2661_stop_locked(struct rt2661_softc *);
 static int		rt2661_load_microcode(struct rt2661_softc *,
 			    const uint8_t *, int);
 #ifdef notyet
@@ -190,7 +194,7 @@ rt2661_attach(device_t dev, int id)
 	struct ifnet *ifp;
 	uint32_t val;
 	const uint8_t *ucode = NULL;
-	int error, i, ac, ntries, size = 0;
+	int bands, error, ac, ntries, size = 0;
 
 	sc->sc_dev = dev;
 
@@ -198,7 +202,6 @@ rt2661_attach(device_t dev, int id)
 	    MTX_DEF | MTX_RECURSE);
 
 	callout_init_mtx(&sc->watchdog_ch, &sc->sc_mtx, 0);
-	callout_init(&sc->scan_ch, debug_mpsafenet ? CALLOUT_MPSAFE : 0);
 	callout_init(&sc->rssadapt_ch, CALLOUT_MPSAFE);
 
 	/* wait for NIC to initialize */
@@ -302,39 +305,22 @@ rt2661_attach(device_t dev, int id)
 #ifdef notyet
 	    IEEE80211_C_WME |		/* 802.11e */
 #endif
+	    IEEE80211_C_BGSCAN |	/* bg scanning support */
 	    IEEE80211_C_WPA;		/* 802.11i */
 
-	if (sc->rf_rev == RT2661_RF_5225 || sc->rf_rev == RT2661_RF_5325) {
-		/* set supported .11a channels */
-		for (i = 36; i <= 64; i += 4) {
-			ic->ic_channels[i].ic_freq =
-			    ieee80211_ieee2mhz(i, IEEE80211_CHAN_5GHZ);
-			ic->ic_channels[i].ic_flags = IEEE80211_CHAN_A;
-		}
-		for (i = 100; i <= 140; i += 4) {
-			ic->ic_channels[i].ic_freq =
-			    ieee80211_ieee2mhz(i, IEEE80211_CHAN_5GHZ);
-			ic->ic_channels[i].ic_flags = IEEE80211_CHAN_A;
-		}
-		for (i = 149; i <= 165; i += 4) {
-			ic->ic_channels[i].ic_freq =
-			    ieee80211_ieee2mhz(i, IEEE80211_CHAN_5GHZ);
-			ic->ic_channels[i].ic_flags = IEEE80211_CHAN_A;
-		}
-	}
-
-	/* set supported .11b and .11g channels (1 through 14) */
-	for (i = 1; i <= 14; i++) {
-		ic->ic_channels[i].ic_freq =
-		    ieee80211_ieee2mhz(i, IEEE80211_CHAN_2GHZ);
-		ic->ic_channels[i].ic_flags =
-		    IEEE80211_CHAN_CCK | IEEE80211_CHAN_OFDM |
-		    IEEE80211_CHAN_DYN | IEEE80211_CHAN_2GHZ;
-	}
+	bands = 0;
+	setbit(&bands, IEEE80211_MODE_11B);
+	setbit(&bands, IEEE80211_MODE_11G);
+	if (sc->rf_rev == RT2661_RF_5225 || sc->rf_rev == RT2661_RF_5325) 
+		setbit(&bands, IEEE80211_MODE_11A);
+	ieee80211_init_channels(ic, 0, CTRY_DEFAULT, bands, 0, 1);
 
 	ieee80211_ifattach(ic);
 	ic->ic_node_alloc = rt2661_node_alloc;
 /*	ic->ic_wme.wme_update = rt2661_wme_update;*/
+	ic->ic_scan_start = rt2661_scan_start;
+	ic->ic_scan_end = rt2661_scan_end;
+	ic->ic_set_channel = rt2661_set_channel;
 	ic->ic_updateslot = rt2661_update_slot;
 	ic->ic_reset = rt2661_reset;
 	/* enable s/w bmiss handling in sta mode */
@@ -356,6 +342,7 @@ rt2661_attach(device_t dev, int id)
 	sc->sc_txtap.wt_ihdr.it_len = htole16(sc->sc_txtap_len);
 	sc->sc_txtap.wt_ihdr.it_present = htole32(RT2661_TX_RADIOTAP_PRESENT);
 
+	
 	/*
 	 * Add a few sysctl knobs.
 	 */
@@ -376,7 +363,6 @@ fail3:	rt2661_free_tx_ring(sc, &sc->mgtq);
 fail2:	while (--ac >= 0)
 		rt2661_free_tx_ring(sc, &sc->txq[ac]);
 fail1:	mtx_destroy(&sc->sc_mtx);
-
 	return error;
 }
 
@@ -386,10 +372,9 @@ rt2661_detach(void *xsc)
 	struct rt2661_softc *sc = xsc;
 	struct ieee80211com *ic = &sc->sc_ic;
 	struct ifnet *ifp = ic->ic_ifp;
-
+	
 	rt2661_stop(sc);
 	callout_stop(&sc->watchdog_ch);
-	callout_stop(&sc->scan_ch);
 	callout_stop(&sc->rssadapt_ch);
 
 	bpfdetach(ifp);
@@ -759,20 +744,6 @@ rt2661_media_change(struct ifnet *ifp)
 }
 
 /*
- * This function is called periodically (every 200ms) during scanning to
- * switch from one channel to another.
- */
-static void
-rt2661_next_scan(void *arg)
-{
-	struct rt2661_softc *sc = arg;
-	struct ieee80211com *ic = &sc->sc_ic;
-
-	if (ic->ic_state == IEEE80211_S_SCAN)
-		ieee80211_next_scan(ic);
-}
-
-/*
  * This function is called for each node present in the node station table.
  */
 static void
@@ -811,7 +782,6 @@ rt2661_newstate(struct ieee80211com *ic, enum ieee80211_state nstate, int arg)
 	int error = 0;
 
 	ostate = ic->ic_state;
-	callout_stop(&sc->scan_ch);
 
 	switch (nstate) {
 	case IEEE80211_S_INIT:
@@ -823,21 +793,7 @@ rt2661_newstate(struct ieee80211com *ic, enum ieee80211_state nstate, int arg)
 			RAL_WRITE(sc, RT2661_TXRX_CSR9, tmp & ~0x00ffffff);
 		}
 		break;
-
-	case IEEE80211_S_SCAN:
-		rt2661_set_chan(sc, ic->ic_curchan);
-		callout_reset(&sc->scan_ch, (sc->dwelltime * hz) / 1000,
-		    rt2661_next_scan, sc);
-		break;
-
-	case IEEE80211_S_AUTH:
-	case IEEE80211_S_ASSOC:
-		rt2661_set_chan(sc, ic->ic_curchan);
-		break;
-
 	case IEEE80211_S_RUN:
-		rt2661_set_chan(sc, ic->ic_curchan);
-
 		ni = ic->ic_bss;
 
 		if (ic->ic_opmode != IEEE80211_M_MONITOR) {
@@ -859,6 +815,10 @@ rt2661_newstate(struct ieee80211com *ic, enum ieee80211_state nstate, int arg)
 			rt2661_enable_tsf_sync(sc);
 		}
 		break;
+	case IEEE80211_S_SCAN:
+	case IEEE80211_S_AUTH:
+	case IEEE80211_S_ASSOC:
+		break;
 	}	
 
 	return (error != 0) ? error : sc->sc_newstate(ic, nstate, arg);
@@ -934,6 +894,9 @@ rt2661_tx_intr(struct rt2661_softc *sc)
 	int qid, retrycnt;
 
 	for (;;) {
+		struct ieee80211_node *ni;
+		struct mbuf *m;
+
 		val = RAL_READ(sc, RT2661_STA_CSR4);
 		if (!(val & RT2661_TX_STAT_VALID))
 			break;
@@ -944,12 +907,17 @@ rt2661_tx_intr(struct rt2661_softc *sc)
 
 		/* retrieve rate control algorithm context */
 		data = &txq->data[txq->stat];
-		rn = (struct rt2661_node *)data->ni;
+		m = data->m;
+		data->m = NULL;
+		ni = data->ni;
+		data->ni = NULL;
 
 		/* if no frame has been sent, ignore */
-		if (rn == NULL)
+		if (ni == NULL)
 			continue;
 
+		rn = (struct rt2661_node *)ni;
+
 		switch (RT2661_TX_RESULT(val)) {
 		case RT2661_TX_SUCCESS:
 			retrycnt = RT2661_TX_RETRYCNT(val);
@@ -967,7 +935,7 @@ rt2661_tx_intr(struct rt2661_softc *sc)
 			DPRINTFN(9, ("sending data frame failed (too much "
 			    "retries)\n"));
 			if (data->id.id_node != NULL) {
-				ral_rssadapt_lower_rate(ic, data->ni,
+				ral_rssadapt_lower_rate(ic, ni,
 				    &rn->rssadapt, &data->id);
 			}
 			ifp->if_oerrors++;
@@ -980,14 +948,17 @@ rt2661_tx_intr(struct rt2661_softc *sc)
 			ifp->if_oerrors++;
 		}
 
-		ieee80211_free_node(data->ni);
-		data->ni = NULL;
-
 		DPRINTFN(15, ("tx done q=%d idx=%u\n", qid, txq->stat));
 
 		txq->queued--;
 		if (++txq->stat >= txq->count)	/* faster than % count */
 			txq->stat = 0;
+
+		if (m->m_flags & M_TXCB)
+			ieee80211_process_callback(ni, m,
+				RT2661_TX_RESULT(val) != RT2661_TX_SUCCESS);
+		m_freem(m);
+		ieee80211_free_node(ni);
 	}
 
 	sc->sc_tx_timer = 0;
@@ -1014,9 +985,6 @@ rt2661_tx_dma_intr(struct rt2661_softc *sc, struct rt2661_tx_ring *txq)
 		bus_dmamap_sync(txq->data_dmat, data->map,
 		    BUS_DMASYNC_POSTWRITE);
 		bus_dmamap_unload(txq->data_dmat, data->map);
-		m_freem(data->m);
-		data->m = NULL;
-		/* node reference is released in rt2661_tx_intr() */
 
 		/* descriptor is no longer valid */
 		desc->flags &= ~htole32(RT2661_TX_VALID);
@@ -1048,6 +1016,8 @@ rt2661_rx_intr(struct rt2661_softc *sc)
 	    BUS_DMASYNC_POSTREAD);
 
 	for (;;) {
+		int rssi;
+
 		desc = &sc->rxq.desc[sc->rxq.cur];
 		data = &sc->rxq.data[sc->rxq.cur];
 
@@ -1120,6 +1090,8 @@ rt2661_rx_intr(struct rt2661_softc *sc)
 		m->m_pkthdr.len = m->m_len =
 		    (le32toh(desc->flags) >> 16) & 0xfff;
 
+		rssi = rt2661_get_rssi(sc, desc->rssi);
+
 		if (bpf_peers_present(sc->sc_drvbpf)) {
 			struct rt2661_rx_radiotap_header *tap = &sc->sc_rxtap;
 			uint32_t tsf_lo, tsf_hi;
@@ -1134,22 +1106,28 @@ rt2661_rx_intr(struct rt2661_softc *sc)
 			tap->wr_rate = rt2661_rxrate(desc);
 			tap->wr_chan_freq = htole16(ic->ic_curchan->ic_freq);
 			tap->wr_chan_flags = htole16(ic->ic_curchan->ic_flags);
-			tap->wr_antsignal = desc->rssi;
+			tap->wr_antsignal = rssi < 0 ? 0 : rssi;
 
 			bpf_mtap2(sc->sc_drvbpf, tap, sc->sc_rxtap_len, m);
 		}
-
+		sc->sc_flags |= RAL_INPUT_RUNNING;
+		RAL_UNLOCK(sc);
 		wh = mtod(m, struct ieee80211_frame *);
 		ni = ieee80211_find_rxnode(ic,
 		    (struct ieee80211_frame_min *)wh);
 
+		/* Error happened during RSSI conversion. */
+		if (rssi < 0)
+			rssi = ni->ni_rssi;
+
 		/* send the frame to the 802.11 layer */
-		ieee80211_input(ic, m, ni, desc->rssi, 0);
+		ieee80211_input(ic, m, ni, rssi, RT2661_NOISE_FLOOR, 0);
 
 		/* give rssi to the rate adatation algorithm */
 		rn = (struct rt2661_node *)ni;
-		ral_rssadapt_input(ic, ni, &rn->rssadapt,
-		    rt2661_get_rssi(sc, desc->rssi));
+		RAL_LOCK(sc);
+		sc->sc_flags &= ~RAL_INPUT_RUNNING;
+		ral_rssadapt_input(ic, ni, &rn->rssadapt, rssi);
 
 		/* node is no longer needed */
 		ieee80211_free_node(ni);
@@ -1556,7 +1534,6 @@ rt2661_tx_data(struct rt2661_softc *sc, struct mbuf *m0,
 	struct rt2661_tx_desc *desc;
 	struct rt2661_tx_data *data;
 	struct rt2661_node *rn;
-	struct ieee80211_rateset *rs;
 	struct ieee80211_frame *wh;
 	struct ieee80211_key *k;
 	const struct chanAccParams *cap;
@@ -1569,9 +1546,10 @@ rt2661_tx_data(struct rt2661_softc *sc, struct mbuf *m0,
 	wh = mtod(m0, struct ieee80211_frame *);
 
 	if (ic->ic_fixed_rate != IEEE80211_FIXED_RATE_NONE) {
-		rs = &ic->ic_sup_rates[ic->ic_curmode];
-		rate = rs->rs_rates[ic->ic_fixed_rate];
+		rate = ic->ic_fixed_rate;
 	} else {
+		struct ieee80211_rateset *rs;
+
 		rs = &ni->ni_rates;
 		rn = (struct rt2661_node *)ni;
 		ni->ni_txrate = ral_rssadapt_choose(&rn->rssadapt, rs,
@@ -1753,7 +1731,7 @@ rt2661_start(struct ifnet *ifp)
 	RAL_LOCK(sc);
 
 	/* prevent management frames from being sent if we're not ready */
-	if (!(ifp->if_drv_flags & IFF_DRV_RUNNING)) {
+	if (!(ifp->if_drv_flags & IFF_DRV_RUNNING) || sc->sc_invalid) {
 		RAL_UNLOCK(sc);
 		return;
 	}
@@ -1773,9 +1751,10 @@ rt2661_start(struct ifnet *ifp)
 			if (bpf_peers_present(ic->ic_rawbpf))
 				bpf_mtap(ic->ic_rawbpf, m0);
 
-			if (rt2661_tx_mgt(sc, m0, ni) != 0)
+			if (rt2661_tx_mgt(sc, m0, ni) != 0) {
+				ieee80211_free_node(ni);
 				break;
-
+			}
 		} else {
 			if (ic->ic_state != IEEE80211_S_RUN)
 				break;
@@ -1812,6 +1791,7 @@ rt2661_start(struct ifnet *ifp)
 				/* there is no place left in this ring */
 				IFQ_DRV_PREPEND(&ifp->if_snd, m0);
 				ifp->if_drv_flags |= IFF_DRV_OACTIVE;
+				ieee80211_free_node(ni);
 				break;
 			}
 
@@ -1845,9 +1825,8 @@ static void
 rt2661_watchdog(void *arg)
 {
 	struct rt2661_softc *sc = (struct rt2661_softc *)arg;
-	struct ieee80211com *ic = &sc->sc_ic;
 
-	if (sc->sc_tx_timer > 0) {
+	if (sc->sc_tx_timer > 0 && !sc->sc_invalid) {
 		if (--sc->sc_tx_timer == 0) {
 			device_printf(sc->sc_dev, "device timeout\n");
 			rt2661_init(sc);
@@ -1856,8 +1835,6 @@ rt2661_watchdog(void *arg)
 		}
 		callout_reset(&sc->watchdog_ch, hz, rt2661_watchdog, sc);
 	}
-
-	ieee80211_watchdog(ic);
 }
 
 /*
@@ -1886,8 +1863,6 @@ rt2661_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 	struct ieee80211com *ic = &sc->sc_ic;
 	int error = 0;
 
-	RAL_LOCK(sc);
-
 	switch (cmd) {
 	case SIOCSIFFLAGS:
 		if (ifp->if_flags & IFF_UP) {
@@ -1913,8 +1888,6 @@ rt2661_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 		error = 0;
 	}
 
-	RAL_UNLOCK(sc);
-
 	return error;
 }
 
@@ -2066,6 +2039,12 @@ rt2661_set_txpreamble(struct rt2661_softc *sc)
 	RAL_WRITE(sc, RT2661_TXRX_CSR4, tmp);
 }
 
+/*
+ * Supported rates for 802.11g. XXX should use ic_sup_rates.
+ */
+static const struct ieee80211_rateset rt2661_rateset_11g =
+	{ 12, { 2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108 } };
+
 static void
 rt2661_set_basicrates(struct rt2661_softc *sc,
     const struct ieee80211_rateset *rs)
@@ -2373,10 +2352,18 @@ rt2661_read_eeprom(struct rt2661_softc *sc)
 	if ((val & 0xff) != 0xff)
 		sc->rssi_2ghz_corr = (int8_t)(val & 0xff);	/* signed */
 
+	/* Only [-10, 10] is valid */
+	if (sc->rssi_2ghz_corr < -10 || sc->rssi_2ghz_corr > 10)
+		sc->rssi_2ghz_corr = 0;
+
 	val = rt2661_eeprom_read(sc, RT2661_EEPROM_RSSI_5GHZ_OFFSET);
 	if ((val & 0xff) != 0xff)
 		sc->rssi_5ghz_corr = (int8_t)(val & 0xff);	/* signed */
 
+	/* Only [-10, 10] is valid */
+	if (sc->rssi_5ghz_corr < -10 || sc->rssi_5ghz_corr > 10)
+		sc->rssi_5ghz_corr = 0;
+
 	/* adjust RSSI correction for external low-noise amplifier */
 	if (sc->ext_2ghz_lna)
 		sc->rssi_2ghz_corr -= 14;
@@ -2465,7 +2452,7 @@ rt2661_init(void *priv)
 
 	RAL_LOCK(sc);
 
-	rt2661_stop(sc);
+	rt2661_stop_locked(sc);
 
 	/* initialize Tx rings */
 	RAL_WRITE(sc, RT2661_AC1_BASE_CSR, sc->txq[1].physaddr);
@@ -2525,13 +2512,13 @@ rt2661_init(void *priv)
 	}
 	if (ntries == 1000) {
 		printf("timeout waiting for BBP/RF to wakeup\n");
-		rt2661_stop(sc);
+		rt2661_stop_locked(sc);
 		RAL_UNLOCK(sc);
 		return;
 	}
 
 	if (rt2661_bbp_init(sc) != 0) {
-		rt2661_stop(sc);
+		rt2661_stop_locked(sc);
 		RAL_UNLOCK(sc);
 		return;
 	}
@@ -2572,6 +2559,7 @@ rt2661_init(void *priv)
 
 	/* kick Rx */
 	RAL_WRITE(sc, RT2661_RX_CNTL_CSR, 1);
+	RAL_UNLOCK(sc);
 
 	ifp->if_drv_flags &= ~IFF_DRV_OACTIVE;
 	ifp->if_drv_flags |= IFF_DRV_RUNNING;
@@ -2582,7 +2570,7 @@ rt2661_init(void *priv)
 	} else
 		ieee80211_new_state(ic, IEEE80211_S_RUN, -1);
 
-	RAL_UNLOCK(sc);
+
 #undef N
 }
 
@@ -2590,41 +2578,57 @@ void
 rt2661_stop(void *priv)
 {
 	struct rt2661_softc *sc = priv;
+
+	RAL_LOCK(sc);
+	rt2661_stop_locked(sc);
+	RAL_UNLOCK(sc);
+}
+
+void
+rt2661_stop_locked(struct rt2661_softc *sc)
+{
 	struct ieee80211com *ic = &sc->sc_ic;
 	struct ifnet *ifp = ic->ic_ifp;
 	uint32_t tmp;
-
-	sc->sc_tx_timer = 0;
-	ifp->if_drv_flags &= ~(IFF_DRV_RUNNING | IFF_DRV_OACTIVE);
-
-	ieee80211_new_state(ic, IEEE80211_S_INIT, -1);
-
-	/* abort Tx (for all 5 Tx rings) */
-	RAL_WRITE(sc, RT2661_TX_CNTL_CSR, 0x1f << 16);
-
-	/* disable Rx (value remains after reset!) */
-	tmp = RAL_READ(sc, RT2661_TXRX_CSR0);
-	RAL_WRITE(sc, RT2661_TXRX_CSR0, tmp | RT2661_DISABLE_RX);
-
-	/* reset ASIC */
-	RAL_WRITE(sc, RT2661_MAC_CSR1, 3);
-	RAL_WRITE(sc, RT2661_MAC_CSR1, 0);
-
-	/* disable interrupts */
-	RAL_WRITE(sc, RT2661_INT_MASK_CSR, 0xffffffff);
-	RAL_WRITE(sc, RT2661_MCU_INT_MASK_CSR, 0xffffffff);
-
-	/* clear any pending interrupt */
-	RAL_WRITE(sc, RT2661_INT_SOURCE_CSR, 0xffffffff);
-	RAL_WRITE(sc, RT2661_MCU_INT_SOURCE_CSR, 0xffffffff);
-
-	/* reset Tx and Rx rings */
-	rt2661_reset_tx_ring(sc, &sc->txq[0]);
-	rt2661_reset_tx_ring(sc, &sc->txq[1]);
-	rt2661_reset_tx_ring(sc, &sc->txq[2]);
-	rt2661_reset_tx_ring(sc, &sc->txq[3]);
-	rt2661_reset_tx_ring(sc, &sc->mgtq);
-	rt2661_reset_rx_ring(sc, &sc->rxq);
+	volatile int *flags = &sc->sc_flags;
+
+	while (*flags & RAL_INPUT_RUNNING) {
+		msleep(sc, &sc->sc_mtx, 0, "ralrunning", hz/10);
+	}
+
+	if (ifp->if_drv_flags & IFF_DRV_RUNNING) {
+		sc->sc_tx_timer = 0;
+		ifp->if_drv_flags &= ~(IFF_DRV_RUNNING | IFF_DRV_OACTIVE);
+		
+		ieee80211_new_state(ic, IEEE80211_S_INIT, -1);
+		
+		/* abort Tx (for all 5 Tx rings) */
+		RAL_WRITE(sc, RT2661_TX_CNTL_CSR, 0x1f << 16);
+		
+		/* disable Rx (value remains after reset!) */
+		tmp = RAL_READ(sc, RT2661_TXRX_CSR0);
+		RAL_WRITE(sc, RT2661_TXRX_CSR0, tmp | RT2661_DISABLE_RX);
+		
+		/* reset ASIC */
+		RAL_WRITE(sc, RT2661_MAC_CSR1, 3);
+		RAL_WRITE(sc, RT2661_MAC_CSR1, 0);
+		
+		/* disable interrupts */
+		RAL_WRITE(sc, RT2661_INT_MASK_CSR, 0xffffffff);
+		RAL_WRITE(sc, RT2661_MCU_INT_MASK_CSR, 0xffffffff);
+		
+		/* clear any pending interrupt */
+		RAL_WRITE(sc, RT2661_INT_SOURCE_CSR, 0xffffffff);
+		RAL_WRITE(sc, RT2661_MCU_INT_SOURCE_CSR, 0xffffffff);
+		
+		/* reset Tx and Rx rings */
+		rt2661_reset_tx_ring(sc, &sc->txq[0]);
+		rt2661_reset_tx_ring(sc, &sc->txq[1]);
+		rt2661_reset_tx_ring(sc, &sc->txq[2]);
+		rt2661_reset_tx_ring(sc, &sc->txq[3]);
+		rt2661_reset_tx_ring(sc, &sc->mgtq);
+		rt2661_reset_rx_ring(sc, &sc->rxq);
+	}
 }
 
 static int
@@ -2858,7 +2862,17 @@ rt2661_get_rssi(struct rt2661_softc *sc, uint8_t raw)
 	lna = (raw >> 5) & 0x3;
 	agc = raw & 0x1f;
 
-	rssi = 2 * agc;
+	if (lna == 0) {
+		/*
+		 * No mapping available.
+		 *
+		 * NB: Since RSSI is relative to noise floor, -1 is
+		 *     adequate for caller to know error happened.
+		 */
+		return -1;
+	}
+
+	rssi = (2 * agc) - RT2661_NOISE_FLOOR;
 
 	if (IEEE80211_IS_CHAN_2GHZ(sc->sc_curchan)) {
 		rssi += sc->rssi_2ghz_corr;
@@ -2881,3 +2895,39 @@ rt2661_get_rssi(struct rt2661_softc *sc, uint8_t raw)
 	}
 	return rssi;
 }
+
+static void
+rt2661_scan_start(struct ieee80211com *ic)
+{
+	struct ifnet *ifp = ic->ic_ifp;
+	struct rt2661_softc *sc = ifp->if_softc;
+	uint32_t tmp;
+
+	/* abort TSF synchronization */
+	tmp = RAL_READ(sc, RT2661_TXRX_CSR9);
+	RAL_WRITE(sc, RT2661_TXRX_CSR9, tmp & ~0xffffff);
+	rt2661_set_bssid(sc, ifp->if_broadcastaddr);
+}
+
+static void
+rt2661_scan_end(struct ieee80211com *ic)
+{
+	struct ifnet *ifp = ic->ic_ifp;
+	struct rt2661_softc *sc = ifp->if_softc;
+
+	rt2661_enable_tsf_sync(sc);
+	/* XXX keep local copy */
+	rt2661_set_bssid(sc, ic->ic_bss->ni_bssid);
+}
+
+static void
+rt2661_set_channel(struct ieee80211com *ic)
+{
+	struct ifnet *ifp = ic->ic_ifp;
+	struct rt2661_softc *sc = ifp->if_softc;
+
+	RAL_LOCK(sc);
+	rt2661_set_chan(sc, ic->ic_curchan);
+	RAL_UNLOCK(sc);
+
+}
diff --git a/sys/dev/ral/rt2661reg.h b/sys/dev/ral/rt2661reg.h
index d79f926..b4325b0 100644
--- a/sys/dev/ral/rt2661reg.h
+++ b/sys/dev/ral/rt2661reg.h
@@ -17,6 +17,8 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#define RT2661_NOISE_FLOOR	-95
+
 #define RT2661_TX_RING_COUNT	32
 #define RT2661_MGT_RING_COUNT	32
 #define RT2661_RX_RING_COUNT	64
diff --git a/sys/dev/ral/rt2661var.h b/sys/dev/ral/rt2661var.h
index fab2892..9f12a15 100644
--- a/sys/dev/ral/rt2661var.h
+++ b/sys/dev/ral/rt2661var.h
@@ -102,11 +102,15 @@ struct rt2661_softc {
 	struct mtx			sc_mtx;
 
 	struct callout			watchdog_ch;
-	struct callout			scan_ch;
 	struct callout			rssadapt_ch;
 
 	int				sc_tx_timer;
-
+	int                             sc_invalid;
+/*
+ * The same in both up to here
+ * ------------------------------------------------
+ */
+	
 	struct ieee80211_channel	*sc_curchan;
 
 	uint8_t				rf_rev;
@@ -159,6 +163,8 @@ struct rt2661_softc {
 	}				sc_txtapu;
 #define sc_txtap	sc_txtapu.th
 	int				sc_txtap_len;
+#define                 RAL_INPUT_RUNNING       1
+	int                             sc_flags;
 };
 
 int	rt2661_attach(device_t, int);