p54: implement power save management

diff -Nurp a/drivers/net/wireless/p54/p54common.c b/drivers/net/wireless/p54/p54common.c
@@ -1747,6 +1747,41 @@ static int p54_set_edcf(struct ieee80211
 	return 0;
 }
 
+static int p54_powersave(struct ieee80211_hw *dev)
+{
+	struct p54_common *priv = dev->priv;
+	struct sk_buff *skb;
+	struct p54_psm *psm;
+	u16 mode = P54_PSM_OFF;
+	int i;
+
+	skb = p54_alloc_skb(dev, P54_HDR_FLAG_CONTROL_OPSET, sizeof(*psm) +
+			sizeof(struct p54_hdr), P54_CONTROL_TYPE_PSM,
+			GFP_ATOMIC);
+	if (!skb)
+		return -ENOMEM;
+
+	if (dev->conf.flags & IEEE80211_CONF_PS)
+		mode = cpu_to_le16(P54_PSM | P54_PSM_DTIM | P54_PSM_MCBC);
+
+	psm = (struct p54_psm *)skb_put(skb, sizeof(*psm));
+	psm->aid = cpu_to_le16(priv->aid);
+	psm->mode = cpu_to_le16(mode);
+	for (i = 0; i < ARRAY_SIZE(psm->intervals); i++) {
+		psm->intervals[i].interval =
+			cpu_to_le16(dev->conf.listen_interval);
+		psm->intervals[i].periods = 1;
+	}
+
+	psm->beacon_rssi_skip_max = 60;
+	psm->rssi_delta_threshold = 0;
+	psm->nr = 0;
+
+	priv->tx(dev, skb);
+
+	return 0;
+}
+
 static int p54_beacon_tim(struct sk_buff *skb)
 {
 	/*

@@ -1939,6 +1974,11 @@ static int p54_config(struct ieee80211_h
 		if (ret)
 			goto out;
 	}
+	if (changed & IEEE80211_CONF_CHANGE_PS) {
+		ret = p54_powersave(dev);
+		if (ret)
+			goto out;
+	}
 
 out:
 	mutex_unlock(&priv->conf_mutex);

diff -Nurp a/drivers/net/wireless/p54/p54common.h b/drivers/net/wireless/p54/p54common.h
@@ -545,6 +545,7 @@ struct p54_psm_interval {
 	__le16 periods;
 } __attribute__ ((packed));
 
+#define P54_PSM_OFF			0
 #define P54_PSM				BIT(0)
 #define P54_PSM_DTIM			BIT(1)
 #define P54_PSM_MCBC			BIT(2)