diff --git a/drivers/mtd/ubi/build.c b/drivers/mtd/ubi/build.c
index baf4e2d..d81bd43 100644
--- a/drivers/mtd/ubi/build.c
+++ b/drivers/mtd/ubi/build.c
@@ -1060,15 +1060,7 @@ int ubi_attach_mtd_dev(struct mtd_info *mtd, int ubi_num,
 #ifndef __UBOOT__
 	wake_up_process(ubi->bgt_thread);
 #else
-	/*
-	 * U-Boot special: We have no bgt_thread in U-Boot!
-	 * So just call do_work() here directly.
-	 */
-	err = do_work(ubi);
-	if (err) {
-		ubi_err(ubi, "%s: work failed with error code %d",
-			ubi->bgt_name, err);
-	}
+	ubi_do_worker(ubi);
 #endif
 
 	spin_unlock(&ubi->wl_lock);
diff --git a/drivers/mtd/ubi/ubi.h b/drivers/mtd/ubi/ubi.h
index 540f721..3337201 100644
--- a/drivers/mtd/ubi/ubi.h
+++ b/drivers/mtd/ubi/ubi.h
@@ -1119,6 +1119,6 @@ static inline int idx2vol_id(const struct ubi_device *ubi, int idx)
 }
 
 #ifdef __UBOOT__
-int do_work(struct ubi_device *ubi);
+void ubi_do_worker(struct ubi_device *ubi);
 #endif
 #endif /* !__UBI_UBI_H__ */
diff --git a/drivers/mtd/ubi/wl.c b/drivers/mtd/ubi/wl.c
index e823ca5..0de2a4a 100644
--- a/drivers/mtd/ubi/wl.c
+++ b/drivers/mtd/ubi/wl.c
@@ -191,11 +191,7 @@ static void wl_entry_destroy(struct ubi_device *ubi, struct ubi_wl_entry *e)
  * This function returns zero in case of success and a negative error code in
  * case of failure.
  */
-#ifndef __UBOOT__
 static int do_work(struct ubi_device *ubi)
-#else
-int do_work(struct ubi_device *ubi)
-#endif
 {
 	int err;
 	struct ubi_work *wrk;
@@ -528,6 +524,33 @@ repeat:
 	spin_unlock(&ubi->wl_lock);
 }
 
+#ifdef __UBOOT__
+void ubi_do_worker(struct ubi_device *ubi)
+{
+	int err;
+
+	if (list_empty(&ubi->works) || ubi->ro_mode ||
+	    !ubi->thread_enabled || ubi_dbg_is_bgt_disabled(ubi))
+		return;
+
+	spin_lock(&ubi->wl_lock);
+	while (!list_empty(&ubi->works)) {
+		/*
+		 * call do_work, which executes exactly one work form the queue,
+		 * including removeing it from the work queue.
+		 */
+		spin_unlock(&ubi->wl_lock);
+		err = do_work(ubi);
+		spin_lock(&ubi->wl_lock);
+		if (err) {
+			ubi_err(ubi, "%s: work failed with error code %d",
+				ubi->bgt_name, err);
+		}
+	}
+	spin_unlock(&ubi->wl_lock);
+}
+#endif
+
 /**
  * __schedule_ubi_work - schedule a work.
  * @ubi: UBI device description object
@@ -545,17 +568,6 @@ static void __schedule_ubi_work(struct ubi_device *ubi, struct ubi_work *wrk)
 #ifndef __UBOOT__
 	if (ubi->thread_enabled && !ubi_dbg_is_bgt_disabled(ubi))
 		wake_up_process(ubi->bgt_thread);
-#else
-	int err;
-	/*
-	 * U-Boot special: We have no bgt_thread in U-Boot!
-	 * So just call do_work() here directly.
-	 */
-	err = do_work(ubi);
-	if (err) {
-		ubi_err(ubi, "%s: work failed with error code %d",
-			ubi->bgt_name, err);
-	}
 #endif
 	spin_unlock(&ubi->wl_lock);
 }
@@ -610,6 +622,10 @@ static int schedule_erase(struct ubi_device *ubi, struct ubi_wl_entry *e,
 	wl_wrk->torture = torture;
 
 	schedule_ubi_work(ubi, wl_wrk);
+
+#ifdef __UBOOT__
+	ubi_do_worker(ubi);
+#endif
 	return 0;
 }
 
@@ -1011,8 +1027,15 @@ static int ensure_wear_leveling(struct ubi_device *ubi, int nested)
 	wrk->func = &wear_leveling_worker;
 	if (nested)
 		__schedule_ubi_work(ubi, wrk);
+#ifndef __UBOOT__
 	else
 		schedule_ubi_work(ubi, wrk);
+#else
+	else {
+		schedule_ubi_work(ubi, wrk);
+		ubi_do_worker(ubi);
+	}
+#endif
 	return err;
 
 out_cancel: