+static void wl1251_ps_work(struct work_struct *work)
+{
+ struct delayed_work *dwork;
+ struct wl1251 *wl;
+ unsigned long diff, wait;
+ bool need_ps;
+ bool have_ps;
+ int ret;
+ int i;
+
+ dwork = container_of(work, struct delayed_work, work);
+ wl = container_of(dwork, struct wl1251, ps_work);
+
+ mutex_lock(&wl->mutex);
+
+ /* don't change PS modes while still transitioning, to avoid possbile
+ * fw bugs (it normally takes ~130ms to enable and ~10ms to disable) */
+ if (wl->ps_transitioning) {
+ diff = jiffies - wl->ps_change_jiffies;
+ if (diff > msecs_to_jiffies(500)) {
+ wl1251_error("PS change taking too long: %lu", diff);
+ wl->ps_transitioning = false;
+ } else {
+ //wl1251_error("PS still transitioning");
+ ieee80211_queue_delayed_work(wl->hw, &wl->ps_work,
+ msecs_to_jiffies(50));
+ goto out;
+ }
+ }
+
+ have_ps = wl->station_mode == STATION_POWER_SAVE_MODE;
+ need_ps = wl->psm_requested && !wl->bss_lost
+ && wl->rate < wl->ps_rate_threshold;
+
+ if (need_ps == have_ps) {
+ //wl1251_info("ps: already in mode %d", have_ps);
+ goto out;
+ }
+
+ /* don't enter PS if there was recent activity */
+ if (need_ps) {
+ wait = 0;
+
+ diff = jiffies - wl->last_no_ps_jiffies[1];
+ if (diff < msecs_to_jiffies(1000))
+ wait = msecs_to_jiffies(1000) - diff + 1;
+
+ diff = jiffies - wl->last_no_ps_jiffies[0];
+ if (diff < msecs_to_jiffies(3000))
+ wait += msecs_to_jiffies(1000);
+
+ for (i = 0; i < ARRAY_SIZE(wl->tx_frames); i++) {
+ if (wl->tx_frames[i] != NULL) {
+ //wl1251_error(" frm %d busy", i);
+ if (wait < msecs_to_jiffies(50))
+ wait = msecs_to_jiffies(50);
+ break;
+ }
+ }
+
+ if (wait > 0) {
+ ieee80211_queue_delayed_work(wl->hw, &wl->ps_work, wait);
+ goto out;
+ }
+ }
+
+ ret = wl1251_ps_elp_wakeup(wl);
+ if (ret < 0)
+ goto out;
+
+ if (need_ps) {
+ wl1251_acx_wr_tbtt_and_dtim(wl, wl->beacon_int,
+ wl->dtim_period);
+ }
+ ret = wl1251_ps_set_mode(wl,
+ need_ps ? STATION_POWER_SAVE_MODE : STATION_ACTIVE_MODE);
+ if (ret < 0)
+ goto out_sleep;
+
+ // wl1251_info("psm %d, r %u", need_ps, wl->rate);
+
+out_sleep:
+ wl1251_ps_elp_sleep(wl);
+
+out:
+ mutex_unlock(&wl->mutex);
+}
+
+static bool wl1251_can_do_pm(struct ieee80211_conf *conf, struct wl1251 *wl)
+{
+ return (conf->flags & IEEE80211_CONF_PS) && !wl->monitor_present;
+}
+