2 files changed, 97 insertions, 7 deletions

diff --git a/drivers/net/wireless/b43/phy_lp.c b/drivers/net/wireless/b43/phy_lp.c
index 1a57d3390e92..80f245c04042 100644
--- a/drivers/net/wireless/b43/phy_lp.c
+++ b/drivers/net/wireless/b43/phy_lp.c
@@ -234,19 +234,15 @@ static void lpphy_baseband_rev0_1_init(struct b43_wldev *dev)
 	if ((bus->sprom.boardflags_lo & B43_BFL_FEM) &&
 	   ((b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) ||
 	   (bus->sprom.boardflags_hi & B43_BFH_PAREF))) {
-		/* TODO:
-		 * Set the LDO voltage to 0x0028 - FIXME: What is this?
-		 * Call sb_pmu_set_ldo_voltage with 4 and the LDO voltage
-		 *      as arguments
-		 * Call sb_pmu_paref_ldo_enable with argument TRUE
-		 */
+		ssb_pmu_set_ldo_voltage(&bus->chipco, LDO_PAREF, 0x28);
+		ssb_pmu_set_ldo_paref(&bus->chipco, true);
 		if (dev->phy.rev == 0) {
 			b43_phy_maskset(dev, B43_LPPHY_LP_RF_SIGNAL_LUT,
 					0xFFCF, 0x0010);
 		}
 		b43_lptab_write(dev, B43_LPTAB16(11, 7), 60);
 	} else {
-		//TODO: Call ssb_pmu_paref_ldo_enable with argument FALSE
+		ssb_pmu_set_ldo_paref(&bus->chipco, false);
 		b43_phy_maskset(dev, B43_LPPHY_LP_RF_SIGNAL_LUT,
 				0xFFCF, 0x0020);
 		b43_lptab_write(dev, B43_LPTAB16(11, 7), 100);
diff --git a/drivers/ssb/driver_chipcommon_pmu.c b/drivers/ssb/driver_chipcommon_pmu.c
index 4aaddeec55a2..64abd11f6fbb 100644
--- a/drivers/ssb/driver_chipcommon_pmu.c
+++ b/drivers/ssb/driver_chipcommon_pmu.c
@@ -28,6 +28,21 @@ static void ssb_chipco_pll_write(struct ssb_chipcommon *cc,
 	chipco_write32(cc, SSB_CHIPCO_PLLCTL_DATA, value);
 }
 
+static void ssb_chipco_regctl_maskset(struct ssb_chipcommon *cc,
+				   u32 offset, u32 mask, u32 set)
+{
+	u32 value;
+
+	chipco_read32(cc, SSB_CHIPCO_REGCTL_ADDR);
+	chipco_write32(cc, SSB_CHIPCO_REGCTL_ADDR, offset);
+	chipco_read32(cc, SSB_CHIPCO_REGCTL_ADDR);
+	value = chipco_read32(cc, SSB_CHIPCO_REGCTL_DATA);
+	value &= mask;
+	value |= set;
+	chipco_write32(cc, SSB_CHIPCO_REGCTL_DATA, value);
+	chipco_read32(cc, SSB_CHIPCO_REGCTL_DATA);
+}
+
 struct pmu0_plltab_entry {
 	u16 freq;	/* Crystal frequency in kHz.*/
 	u8 xf;		/* Crystal frequency value for PMU control */
@@ -506,3 +521,82 @@ void ssb_pmu_init(struct ssb_chipcommon *cc)
 	ssb_pmu_pll_init(cc);
 	ssb_pmu_resources_init(cc);
 }
+
+void ssb_pmu_set_ldo_voltage(struct ssb_chipcommon *cc,
+			     enum ssb_pmu_ldo_volt_id id, u32 voltage)
+{
+	struct ssb_bus *bus = cc->dev->bus;
+	u32 addr, shift, mask;
+
+	switch (bus->chip_id) {
+	case 0x4328:
+	case 0x5354:
+		switch (id) {
+		case LDO_VOLT1:
+			addr = 2;
+			shift = 25;
+			mask = 0xF;
+			break;
+		case LDO_VOLT2:
+			addr = 3;
+			shift = 1;
+			mask = 0xF;
+			break;
+		case LDO_VOLT3:
+			addr = 3;
+			shift = 9;
+			mask = 0xF;
+			break;
+		case LDO_PAREF:
+			addr = 3;
+			shift = 17;
+			mask = 0x3F;
+			break;
+		default:
+			SSB_WARN_ON(1);
+			return;
+		}
+		break;
+	case 0x4312:
+		if (SSB_WARN_ON(id != LDO_PAREF))
+			return;
+		addr = 0;
+		shift = 21;
+		mask = 0x3F;
+		break;
+	default:
+		return;
+	}
+
+	ssb_chipco_regctl_maskset(cc, addr, ~(mask << shift),
+				  (voltage & mask) << shift);
+}
+
+void ssb_pmu_set_ldo_paref(struct ssb_chipcommon *cc, bool on)
+{
+	struct ssb_bus *bus = cc->dev->bus;
+	int ldo;
+
+	switch (bus->chip_id) {
+	case 0x4312:
+		ldo = SSB_PMURES_4312_PA_REF_LDO;
+		break;
+	case 0x4328:
+		ldo = SSB_PMURES_4328_PA_REF_LDO;
+		break;
+	case 0x5354:
+		ldo = SSB_PMURES_5354_PA_REF_LDO;
+		break;
+	default:
+		return;
+	}
+
+	if (on)
+		chipco_set32(cc, SSB_CHIPCO_PMU_MINRES_MSK, 1 << ldo);
+	else
+		chipco_mask32(cc, SSB_CHIPCO_PMU_MINRES_MSK, ~(1 << ldo));
+	chipco_read32(cc, SSB_CHIPCO_PMU_MINRES_MSK); //SPEC FIXME found via mmiotrace - dummy read?
+}
+
+EXPORT_SYMBOL(ssb_pmu_set_ldo_voltage);
+EXPORT_SYMBOL(ssb_pmu_set_ldo_paref);