7 files changed, 386 insertions, 271 deletions

diff --git a/sys/dev/ral/if_ral_pci.c b/sys/dev/ral/if_ral_pci.c
index 3d9976d64743..2ceafe4a97ae 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 36a849d94f27..f09c4f35d199 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);
+	while (*flags & RAL_INPUT_RUNNING) {
+		tsleep(sc, 0, "ralrunning", hz/10);
+	}
 
-	/* disable Rx */
-	RAL_WRITE(sc, RT2560_RXCSR0, RT2560_DISABLE_RX);
+	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);
 
-	/* reset ASIC (imply reset BBP) */
-	RAL_WRITE(sc, RT2560_CSR1, RT2560_RESET_ASIC);
-	RAL_WRITE(sc, RT2560_CSR1, 0);
+		/* abort Tx */
+		RAL_WRITE(sc, RT2560_TXCSR0, RT2560_ABORT_TX);
+		
+		/* disable Rx */
+		RAL_WRITE(sc, RT2560_RXCSR0, RT2560_DISABLE_RX);
 
-	/* disable interrupts */
-	RAL_WRITE(sc, RT2560_CSR8, 0xffffffff);
+		/* reset ASIC (imply reset BBP) */
+		RAL_WRITE(sc, RT2560_CSR1, RT2560_RESET_ASIC);
+		RAL_WRITE(sc, RT2560_CSR1, 0);
 
-	/* 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);
+		/* 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 6b1cf0d09bd3..3871ed3f28be 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 50112b5f2c23..c01abb1093d2 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 8f13eb6eca1e..224da32c657a 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;
+	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 (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);
+	while (*flags & RAL_INPUT_RUNNING) {
+		msleep(sc, &sc->sc_mtx, 0, "ralrunning", hz/10);
+	}
 
-	/* 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);
+	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 d79f9264260e..b4325b053d33 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 fab28924c6c1..9f12a15fca61 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);