+#define FREQ_OFFSET_BOUND 0x5f
+
+static void rt2800_adjust_freq_offset(struct rt2x00_dev *rt2x00dev)
+{
+ u8 freq_offset, prev_freq_offset;
+ u8 rfcsr, prev_rfcsr;
+
+ freq_offset = rt2x00_get_field8(rt2x00dev->freq_offset, RFCSR17_CODE);
+ freq_offset = min_t(u8, freq_offset, FREQ_OFFSET_BOUND);
+
+ rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
+ prev_rfcsr = rfcsr;
+
+ rt2x00_set_field8(&rfcsr, RFCSR17_CODE, freq_offset);
+ if (rfcsr == prev_rfcsr)
+ return;
+
+ if (rt2x00_is_usb(rt2x00dev)) {
+ rt2800_mcu_request(rt2x00dev, MCU_FREQ_OFFSET, 0xff,
+ freq_offset, prev_rfcsr);
+ return;
+ }
+
+ prev_freq_offset = rt2x00_get_field8(prev_rfcsr, RFCSR17_CODE);
+ while (prev_freq_offset != freq_offset) {
+ if (prev_freq_offset < freq_offset)
+ prev_freq_offset++;
+ else
+ prev_freq_offset--;
+
+ rt2x00_set_field8(&rfcsr, RFCSR17_CODE, prev_freq_offset);
+ rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
+
+ usleep_range(1000, 1500);
+ }
+}
+