rt2800: add write_with_rx_chain function

Based on:
AsicBBPWriteWithRxChain()

from:
DPO_RT5572_LinuxSTA_2.6.1.3_20121022/chips/rtmp_chip.c
Signed-off-by: NStanislaw Gruszka <stf_xl@wp.pl>
Tested-by: NWanlong Gao <gaowanlong@cn.fujitsu.com>
Signed-off-by: NJohn W. Linville <linville@tuxdriver.com>

drivers/net/wireless/rt2x00/rt2800.h

@@ -1939,6 +1939,9 @@ struct mac_iveiv_entry {
 #define BBP4_BANDWIDTH			FIELD8(0x18)
 #define BBP4_MAC_IF_CTRL		FIELD8(0x40)
 
+/* BBP27 */
+#define BBP27_RX_CHAIN_SEL		FIELD8(0x60)
+
 /*
  * BBP 47: Bandwidth
  */

drivers/net/wireless/rt2x00/rt2800lib.c

@@ -2458,6 +2458,22 @@ static void rt2800_config_channel_rf55xx(struct rt2x00_dev *rt2x00dev,
 	rt2800_bbp_write(rt2x00dev, 196, (rf->channel <= 14) ? 0x19 : 0x7F);
 }
 
+static void rt2800_bbp_write_with_rx_chain(struct rt2x00_dev *rt2x00dev,
+					   const unsigned int word,
+					   const u8 value)
+{
+	u8 chain, reg;
+
+	for (chain = 0; chain < rt2x00dev->default_ant.rx_chain_num; chain++) {
+		rt2800_bbp_read(rt2x00dev, 27, &reg);
+		rt2x00_set_field8(&reg,  BBP27_RX_CHAIN_SEL, chain);
+		rt2800_bbp_write(rt2x00dev, 27, reg);
+
+		rt2800_bbp_write(rt2x00dev, word, value);
+	}
+}
+
+
 static void rt2800_iq_calibrate(struct rt2x00_dev *rt2x00dev, int channel)
 {
 	u8 cal;