ath5k: Fix and clean mode initialization, prefer G for AR5212

index 0997577..29559df 100644
@@ -1917,67 +1917,74 @@ static void ath_dump_modes(struct ieee80211_hw_mode *modes)
 static inline void ath_dump_modes(struct ieee80211_hw_mode *modes) {}
 #endif
 
+#define REGISTER_MODE(mode) do { \
+	if (modes[mode].num_channels) { \
+		ret = ieee80211_register_hwmode(hw, &modes[mode]); \
+		if (ret) { \
+			printk(KERN_ERR "can't register hwmode %u\n", mode); \
+			goto err; \
+		} \
+	} \
+} while (0)
+
 static int ath_getchannels(struct ieee80211_hw *hw)
 {
 	struct ath_softc *sc = hw->priv;
 	struct ath_hw *ah = sc->ah;
 	struct ieee80211_hw_mode *modes = sc->modes;
-	unsigned int i, max;
+	unsigned int i, max_r, max_c;
 	int ret;
-	enum {
-		A = MODE_IEEE80211A,
-		B = MODE_IEEE80211G, /* this is not a typo, but workaround */
-		G = MODE_IEEE80211B, /* to prefer g over b */
-		T = MODE_ATHEROS_TURBO,
-		TG = MODE_ATHEROS_TURBOG,
-	};
 
 	BUILD_BUG_ON(ARRAY_SIZE(sc->modes) < 3);
 
 	ah->ah_country_code = countrycode;
 
-	modes[A].mode = MODE_IEEE80211A;
-	modes[B].mode = MODE_IEEE80211B;
-	modes[G].mode = MODE_IEEE80211G;
-
-	max = ARRAY_SIZE(sc->rates);
-	modes[A].rates = sc->rates;
-	max -= modes[A].num_rates = ath_copy_rates(modes[A].rates,
-			ath5k_hw_get_rate_table(ah, MODE_IEEE80211A), max);
-	modes[B].rates = &modes[A].rates[modes[A].num_rates];
-	max -= modes[B].num_rates = ath_copy_rates(modes[B].rates,
-			ath5k_hw_get_rate_table(ah, MODE_IEEE80211B), max);
-	modes[G].rates = &modes[B].rates[modes[B].num_rates];
-	max -= modes[G].num_rates = ath_copy_rates(modes[G].rates,
-			ath5k_hw_get_rate_table(ah, MODE_IEEE80211G), max);
-
-	if (!max)
-		printk(KERN_WARNING "yet another rates found, but there is not "
-				"sufficient space to store them\n");
-
-	max = ARRAY_SIZE(sc->channels);
-	modes[A].channels = sc->channels;
-	max -= modes[A].num_channels = ath_copy_channels(ah, modes[A].channels,
-			MODE_IEEE80211A, max);
-	modes[B].channels = &modes[A].channels[modes[A].num_channels];
-	max -= modes[B].num_channels = ath_copy_channels(ah, modes[B].channels,
-			MODE_IEEE80211B, max);
-	modes[G].channels = &modes[B].channels[modes[B].num_channels];
-	max -= modes[G].num_channels = ath_copy_channels(ah, modes[G].channels,
-			MODE_IEEE80211G, max);
-
-	if (!max)
-		printk(KERN_WARNING "yet another modes found, but there is not "
-				"sufficient space to store them\n");
-
-	for (i = 0; i < ARRAY_SIZE(sc->modes); i++)
-		if (modes[i].num_channels) {
-			ret = ieee80211_register_hwmode(hw, &modes[i]);
-			if (ret) {
-				printk(KERN_ERR "can't register hwmode %u\n",i);
-				goto err;
-			}
+	modes[0].mode = MODE_IEEE80211G;
+	modes[1].mode = MODE_IEEE80211B;
+	modes[2].mode = MODE_IEEE80211A;
+
+	max_r = ARRAY_SIZE(sc->rates);
+	max_c = ARRAY_SIZE(sc->channels);
+	modes[0].rates = sc->rates;
+
+	for (i=0; i <= 2; i++) {
+		struct ieee80211_hw_mode *mode = &modes[i];
+		const struct ath5k_rate_table *hw_rates;
+
+		if (i == 0 ) {
+			mode->rates	= sc->rates;
+			modes->channels	= sc->channels;
+		}
+		else {
+			struct ieee80211_hw_mode *prev_mode = &modes[i-1];
+			int prev_num_r	= prev_mode->num_rates;
+			int prev_num_c	= prev_mode->num_channels;
+			mode->rates	= &prev_mode->rates[prev_num_r];
+			mode->channels	= &prev_mode->channels[prev_num_c];
 		}
+
+		hw_rates = ath5k_hw_get_rate_table(ah, mode->mode);
+		mode->num_rates    = ath_copy_rates(mode->rates, hw_rates,
+			max_r);
+		mode->num_channels = ath_copy_channels(ah, mode->channels, 
+			mode->mode, max_c);
+		max_r -= modes->num_rates;
+		max_c -= mode->num_channels;
+	}
+
+	switch(ah->ah_version) {
+		case AR5K_AR5210:
+			REGISTER_MODE(MODE_IEEE80211A);
+			break;
+		case AR5K_AR5211:
+			REGISTER_MODE(MODE_IEEE80211B);
+			break;
+		case AR5K_AR5212:
+			if (!ah->ah_single_chip)
+				REGISTER_MODE(MODE_IEEE80211A);
+			REGISTER_MODE(MODE_IEEE80211G);
+				break;
+	}
 	ath_dump_modes(modes);
 
 	return 0;