projects / GitHub / mt8127 / android_kernel_alcatel_ttab.git / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
blame | history | raw | HEAD
Merge tag 'v3.10.79' into update
[GitHub/mt8127/android_kernel_alcatel_ttab.git] / kernel / power / swap.c
1 /*
2 * linux/kernel/power/swap.c
3 *
4 * This file provides functions for reading the suspend image from
5 * and writing it to a swap partition.
6 *
7 * Copyright (C) 1998,2001-2005 Pavel Machek <pavel@ucw.cz>
8 * Copyright (C) 2006 Rafael J. Wysocki <rjw@sisk.pl>
9 * Copyright (C) 2010-2012 Bojan Smojver <bojan@rexursive.com>
10 *
11 * This file is released under the GPLv2.
12 *
13 */
14
15 #include <linux/module.h>
16 #include <linux/file.h>
17 #include <linux/delay.h>
18 #include <linux/bitops.h>
19 #include <linux/genhd.h>
20 #include <linux/device.h>
21 #include <linux/bio.h>
22 #include <linux/blkdev.h>
23 #include <linux/swap.h>
24 #include <linux/swapops.h>
25 #include <linux/pm.h>
26 #include <linux/slab.h>
27 #include <linux/lzo.h>
28 #include <linux/vmalloc.h>
29 #include <linux/cpumask.h>
30 #include <linux/atomic.h>
31 #include <linux/kthread.h>
32 #include <linux/crc32.h>
33
34 #include "power.h"
35
36 #define HIBERNATE_SIG "S1SUSPEND"
37
38 /*
39 * The swap map is a data structure used for keeping track of each page
40 * written to a swap partition. It consists of many swap_map_page
41 * structures that contain each an array of MAP_PAGE_ENTRIES swap entries.
42 * These structures are stored on the swap and linked together with the
43 * help of the .next_swap member.
44 *
45 * The swap map is created during suspend. The swap map pages are
46 * allocated and populated one at a time, so we only need one memory
47 * page to set up the entire structure.
48 *
49 * During resume we pick up all swap_map_page structures into a list.
50 */
51
52 #define MAP_PAGE_ENTRIES (PAGE_SIZE / sizeof(sector_t) - 1)
53
54 /*
55 * Number of free pages that are not high.
56 */
57 static inline unsigned long low_free_pages(void)
58 {
59 return nr_free_pages() - nr_free_highpages();
60 }
61
62 /*
63 * Number of pages required to be kept free while writing the image. Always
64 * half of all available low pages before the writing starts.
65 */
66 static inline unsigned long reqd_free_pages(void)
67 {
68 return low_free_pages() / 2;
69 }
70
71 struct swap_map_page {
72 sector_t entries[MAP_PAGE_ENTRIES];
73 sector_t next_swap;
74 };
75
76 struct swap_map_page_list {
77 struct swap_map_page *map;
78 struct swap_map_page_list *next;
79 };
80
81 /**
82 * The swap_map_handle structure is used for handling swap in
83 * a file-alike way
84 */
85
86 struct swap_map_handle {
87 struct swap_map_page *cur;
88 struct swap_map_page_list *maps;
89 sector_t cur_swap;
90 sector_t first_sector;
91 unsigned int k;
92 unsigned long reqd_free_pages;
93 u32 crc32;
94 };
95
96 struct swsusp_header {
97 char reserved[PAGE_SIZE - 20 - sizeof(sector_t) - sizeof(int) -
98 sizeof(u32)];
99 u32 crc32;
100 sector_t image;
101 unsigned int flags; /* Flags to pass to the "boot" kernel */
102 char orig_sig[10];
103 char sig[10];
104 } __attribute__((packed));
105
106 static struct swsusp_header *swsusp_header;
107
108 /**
109 * The following functions are used for tracing the allocated
110 * swap pages, so that they can be freed in case of an error.
111 */
112
113 struct swsusp_extent {
114 struct rb_node node;
115 unsigned long start;
116 unsigned long end;
117 };
118
119 static struct rb_root swsusp_extents = RB_ROOT;
120
121 static int swsusp_extents_insert(unsigned long swap_offset)
122 {
123 struct rb_node **new = &(swsusp_extents.rb_node);
124 struct rb_node *parent = NULL;
125 struct swsusp_extent *ext;
126
127 /* Figure out where to put the new node */
128 while (*new) {
129 ext = rb_entry(*new, struct swsusp_extent, node);
130 parent = *new;
131 if (swap_offset < ext->start) {
132 /* Try to merge */
133 if (swap_offset == ext->start - 1) {
134 ext->start--;
135 return 0;
136 }
137 new = &((*new)->rb_left);
138 } else if (swap_offset > ext->end) {
139 /* Try to merge */
140 if (swap_offset == ext->end + 1) {
141 ext->end++;
142 return 0;
143 }
144 new = &((*new)->rb_right);
145 } else {
146 /* It already is in the tree */
147 return -EINVAL;
148 }
149 }
150 /* Add the new node and rebalance the tree. */
151 ext = kzalloc(sizeof(struct swsusp_extent), GFP_KERNEL);
152 if (!ext)
153 return -ENOMEM;
154
155 ext->start = swap_offset;
156 ext->end = swap_offset;
157 rb_link_node(&ext->node, parent, new);
158 rb_insert_color(&ext->node, &swsusp_extents);
159 return 0;
160 }
161
162 /**
163 * alloc_swapdev_block - allocate a swap page and register that it has
164 * been allocated, so that it can be freed in case of an error.
165 */
166
167 sector_t alloc_swapdev_block(int swap)
168 {
169 unsigned long offset;
170
171 offset = swp_offset(get_swap_page_of_type(swap));
172 if (offset) {
173 if (swsusp_extents_insert(offset))
174 swap_free(swp_entry(swap, offset));
175 else
176 return swapdev_block(swap, offset);
177 }
178 return 0;
179 }
180
181 /**
182 * free_all_swap_pages - free swap pages allocated for saving image data.
183 * It also frees the extents used to register which swap entries had been
184 * allocated.
185 */
186
187 void free_all_swap_pages(int swap)
188 {
189 struct rb_node *node;
190
191 while ((node = swsusp_extents.rb_node)) {
192 struct swsusp_extent *ext;
193 unsigned long offset;
194
195 ext = container_of(node, struct swsusp_extent, node);
196 rb_erase(node, &swsusp_extents);
197 for (offset = ext->start; offset <= ext->end; offset++)
198 swap_free(swp_entry(swap, offset));
199
200 kfree(ext);
201 }
202 }
203
204 int swsusp_swap_in_use(void)
205 {
206 return (swsusp_extents.rb_node != NULL);
207 }
208
209 /*
210 * General things
211 */
212
213 static unsigned short root_swap = 0xffff;
214 struct block_device *hib_resume_bdev;
215
216 /*
217 * Saving part
218 */
219
220 static int mark_swapfiles(struct swap_map_handle *handle, unsigned int flags)
221 {
222 int error;
223
224 hib_bio_read_page(swsusp_resume_block, swsusp_header, NULL);
225 if (!memcmp("SWAP-SPACE",swsusp_header->sig, 10) ||
226 !memcmp("SWAPSPACE2",swsusp_header->sig, 10)) {
227 memcpy(swsusp_header->orig_sig,swsusp_header->sig, 10);
228 memcpy(swsusp_header->sig, HIBERNATE_SIG, 10);
229 swsusp_header->image = handle->first_sector;
230 swsusp_header->flags = flags;
231 if (flags & SF_CRC32_MODE)
232 swsusp_header->crc32 = handle->crc32;
233 error = hib_bio_write_page(swsusp_resume_block,
234 swsusp_header, NULL);
235 #if 0 // FIXME: confirm if required
236 hib_bio_read_page(128, swsusp_header, NULL);
237 error = hib_bio_write_page(128, swsusp_header, NULL);
238 #endif
239 } else {
240 printk(KERN_ERR "PM: Swap header not found!\n");
241 error = -ENODEV;
242 }
243 return error;
244 }
245
246 /**
247 * swsusp_swap_check - check if the resume device is a swap device
248 * and get its index (if so)
249 *
250 * This is called before saving image
251 */
252 static int swsusp_swap_check(void)
253 {
254 int res;
255
256 res = swap_type_of(swsusp_resume_device, swsusp_resume_block,
257 &hib_resume_bdev);
258 if (res < 0)
259 return res;
260
261 root_swap = res;
262 res = blkdev_get(hib_resume_bdev, FMODE_WRITE, NULL);
263 if (res)
264 return res;
265
266 res = set_blocksize(hib_resume_bdev, PAGE_SIZE);
267 if (res < 0)
268 blkdev_put(hib_resume_bdev, FMODE_WRITE);
269
270 return res;
271 }
272
273 /**
274 * write_page - Write one page to given swap location.
275 * @buf: Address we're writing.
276 * @offset: Offset of the swap page we're writing to.
277 * @bio_chain: Link the next write BIO here
278 */
279
280 static int write_page(void *buf, sector_t offset, struct bio **bio_chain)
281 {
282 void *src;
283 int ret;
284
285 if (!offset)
286 return -ENOSPC;
287
288 if (bio_chain) {
289 src = (void *)__get_free_page(__GFP_WAIT | __GFP_NOWARN |
290 __GFP_NORETRY);
291 if (src) {
292 copy_page(src, buf);
293 } else {
294 ret = hib_wait_on_bio_chain(bio_chain); /* Free pages */
295 if (ret)
296 return ret;
297 src = (void *)__get_free_page(__GFP_WAIT |
298 __GFP_NOWARN |
299 __GFP_NORETRY);
300 if (src) {
301 copy_page(src, buf);
302 } else {
303 WARN_ON_ONCE(1);
304 bio_chain = NULL; /* Go synchronous */
305 src = buf;
306 }
307 }
308 } else {
309 src = buf;
310 }
311 return hib_bio_write_page(offset, src, bio_chain);
312 }
313
314 static void release_swap_writer(struct swap_map_handle *handle)
315 {
316 if (handle->cur)
317 free_page((unsigned long)handle->cur);
318 handle->cur = NULL;
319 }
320
321 static int get_swap_writer(struct swap_map_handle *handle)
322 {
323 int ret;
324
325 ret = swsusp_swap_check();
326 if (ret) {
327 if (ret != -ENOSPC)
328 printk(KERN_ERR "PM: Cannot find swap device, try "
329 "swapon -a.\n");
330 return ret;
331 }
332 handle->cur = (struct swap_map_page *)get_zeroed_page(GFP_KERNEL);
333 if (!handle->cur) {
334 ret = -ENOMEM;
335 goto err_close;
336 }
337 handle->cur_swap = alloc_swapdev_block(root_swap);
338 if (!handle->cur_swap) {
339 ret = -ENOSPC;
340 goto err_rel;
341 }
342 handle->k = 0;
343 handle->reqd_free_pages = reqd_free_pages();
344 handle->first_sector = handle->cur_swap;
345 return 0;
346 err_rel:
347 release_swap_writer(handle);
348 err_close:
349 swsusp_close(FMODE_WRITE);
350 return ret;
351 }
352
353 static int swap_write_page(struct swap_map_handle *handle, void *buf,
354 struct bio **bio_chain)
355 {
356 int error = 0;
357 sector_t offset;
358
359 if (!handle->cur)
360 return -EINVAL;
361 offset = alloc_swapdev_block(root_swap);
362 error = write_page(buf, offset, bio_chain);
363 if (error)
364 return error;
365 handle->cur->entries[handle->k++] = offset;
366 if (handle->k >= MAP_PAGE_ENTRIES) {
367 offset = alloc_swapdev_block(root_swap);
368 if (!offset)
369 return -ENOSPC;
370 handle->cur->next_swap = offset;
371 error = write_page(handle->cur, handle->cur_swap, bio_chain);
372 if (error)
373 goto out;
374 clear_page(handle->cur);
375 handle->cur_swap = offset;
376 handle->k = 0;
377
378 if (bio_chain && low_free_pages() <= handle->reqd_free_pages) {
379 error = hib_wait_on_bio_chain(bio_chain);
380 if (error)
381 goto out;
382 /*
383 * Recalculate the number of required free pages, to
384 * make sure we never take more than half.
385 */
386 handle->reqd_free_pages = reqd_free_pages();
387 }
388 }
389 out:
390 return error;
391 }
392
393 static int flush_swap_writer(struct swap_map_handle *handle)
394 {
395 if (handle->cur && handle->cur_swap)
396 return write_page(handle->cur, handle->cur_swap, NULL);
397 else
398 return -EINVAL;
399 }
400
401 static int swap_writer_finish(struct swap_map_handle *handle,
402 unsigned int flags, int error)
403 {
404 if (!error) {
405 flush_swap_writer(handle);
406 printk(KERN_INFO "PM: S");
407 error = mark_swapfiles(handle, flags);
408 printk("|\n");
409 }
410
411 if (error)
412 free_all_swap_pages(root_swap);
413 release_swap_writer(handle);
414 swsusp_close(FMODE_WRITE);
415
416 return error;
417 }
418
419 /* We need to remember how much compressed data we need to read. */
420 #define LZO_HEADER sizeof(size_t)
421
422 /* Number of pages/bytes we'll compress at one time. */
423 #define LZO_UNC_PAGES 32
424 #define LZO_UNC_SIZE (LZO_UNC_PAGES * PAGE_SIZE)
425
426 /* Number of pages/bytes we need for compressed data (worst case). */
427 #define LZO_CMP_PAGES DIV_ROUND_UP(lzo1x_worst_compress(LZO_UNC_SIZE) + \
428 LZO_HEADER, PAGE_SIZE)
429 #define LZO_CMP_SIZE (LZO_CMP_PAGES * PAGE_SIZE)
430
431 /* Maximum number of threads for compression/decompression. */
432 #define LZO_THREADS 3
433
434 /* Minimum/maximum number of pages for read buffering. */
435 #define LZO_MIN_RD_PAGES 1024
436 #define LZO_MAX_RD_PAGES 8192
437
438
439 /**
440 * save_image - save the suspend image data
441 */
442
443 static int save_image(struct swap_map_handle *handle,
444 struct snapshot_handle *snapshot,
445 unsigned int nr_to_write)
446 {
447 unsigned int m;
448 int ret;
449 int nr_pages;
450 int err2;
451 struct bio *bio;
452 struct timeval start;
453 struct timeval stop;
454
455 printk(KERN_INFO "PM: Saving image data pages (%u pages)...\n",
456 nr_to_write);
457 m = nr_to_write / 10;
458 if (!m)
459 m = 1;
460 nr_pages = 0;
461 bio = NULL;
462 do_gettimeofday(&start);
463 while (1) {
464 ret = snapshot_read_next(snapshot);
465 if (ret <= 0)
466 break;
467 ret = swap_write_page(handle, data_of(*snapshot), &bio);
468 if (ret)
469 break;
470 if (!(nr_pages % m))
471 printk(KERN_INFO "PM: Image saving progress: %3d%%\n",
472 nr_pages / m * 10);
473 nr_pages++;
474 }
475 err2 = hib_wait_on_bio_chain(&bio);
476 do_gettimeofday(&stop);
477 if (!ret)
478 ret = err2;
479 if (!ret)
480 printk(KERN_INFO "PM: Image saving done.\n");
481 swsusp_show_speed(&start, &stop, nr_to_write, "Wrote");
482 return ret;
483 }
484
485 /**
486 * Structure used for CRC32.
487 */
488 struct crc_data {
489 struct task_struct *thr; /* thread */
490 atomic_t ready; /* ready to start flag */
491 atomic_t stop; /* ready to stop flag */
492 unsigned run_threads; /* nr current threads */
493 wait_queue_head_t go; /* start crc update */
494 wait_queue_head_t done; /* crc update done */
495 u32 *crc32; /* points to handle's crc32 */
496 size_t *unc_len[LZO_THREADS]; /* uncompressed lengths */
497 unsigned char *unc[LZO_THREADS]; /* uncompressed data */
498 };
499
500 /**
501 * CRC32 update function that runs in its own thread.
502 */
503 static int crc32_threadfn(void *data)
504 {
505 struct crc_data *d = data;
506 unsigned i;
507
508 while (1) {
509 wait_event(d->go, atomic_read(&d->ready) ||
510 kthread_should_stop());
511 if (kthread_should_stop()) {
512 d->thr = NULL;
513 atomic_set(&d->stop, 1);
514 wake_up(&d->done);
515 break;
516 }
517 atomic_set(&d->ready, 0);
518
519 for (i = 0; i < d->run_threads; i++)
520 *d->crc32 = crc32_le(*d->crc32,
521 d->unc[i], *d->unc_len[i]);
522 atomic_set(&d->stop, 1);
523 wake_up(&d->done);
524 }
525 return 0;
526 }
527 /**
528 * Structure used for LZO data compression.
529 */
530 struct cmp_data {
531 struct task_struct *thr; /* thread */
532 atomic_t ready; /* ready to start flag */
533 atomic_t stop; /* ready to stop flag */
534 int ret; /* return code */
535 wait_queue_head_t go; /* start compression */
536 wait_queue_head_t done; /* compression done */
537 size_t unc_len; /* uncompressed length */
538 size_t cmp_len; /* compressed length */
539 unsigned char unc[LZO_UNC_SIZE]; /* uncompressed buffer */
540 unsigned char cmp[LZO_CMP_SIZE]; /* compressed buffer */
541 unsigned char wrk[LZO1X_1_MEM_COMPRESS]; /* compression workspace */
542 };
543
544 /**
545 * Compression function that runs in its own thread.
546 */
547 static int lzo_compress_threadfn(void *data)
548 {
549 struct cmp_data *d = data;
550
551 while (1) {
552 wait_event(d->go, atomic_read(&d->ready) ||
553 kthread_should_stop());
554 if (kthread_should_stop()) {
555 d->thr = NULL;
556 d->ret = -1;
557 atomic_set(&d->stop, 1);
558 wake_up(&d->done);
559 break;
560 }
561 atomic_set(&d->ready, 0);
562
563 d->ret = lzo1x_1_compress(d->unc, d->unc_len,
564 d->cmp + LZO_HEADER, &d->cmp_len,
565 d->wrk);
566 atomic_set(&d->stop, 1);
567 wake_up(&d->done);
568 }
569 return 0;
570 }
571
572 /**
573 * save_image_lzo - Save the suspend image data compressed with LZO.
574 * @handle: Swap mam handle to use for saving the image.
575 * @snapshot: Image to read data from.
576 * @nr_to_write: Number of pages to save.
577 */
578 static int save_image_lzo(struct swap_map_handle *handle,
579 struct snapshot_handle *snapshot,
580 unsigned int nr_to_write)
581 {
582 unsigned int m;
583 int ret = 0;
584 int nr_pages;
585 int err2;
586 struct bio *bio;
587 struct timeval start;
588 struct timeval stop;
589 size_t off;
590 unsigned thr, run_threads, nr_threads;
591 unsigned char *page = NULL;
592 struct cmp_data *data = NULL;
593 struct crc_data *crc = NULL;
594
595 /*
596 * We'll limit the number of threads for compression to limit memory
597 * footprint.
598 */
599 nr_threads = num_online_cpus() - 1;
600 nr_threads = clamp_val(nr_threads, 1, LZO_THREADS);
601
602 page = (void *)__get_free_page(__GFP_WAIT | __GFP_HIGH);
603 if (!page) {
604 printk(KERN_ERR "PM: Failed to allocate LZO page\n");
605 ret = -ENOMEM;
606 goto out_clean;
607 }
608
609 data = vmalloc(sizeof(*data) * nr_threads);
610 if (!data) {
611 printk(KERN_ERR "PM: Failed to allocate LZO data\n");
612 ret = -ENOMEM;
613 goto out_clean;
614 }
615 for (thr = 0; thr < nr_threads; thr++)
616 memset(&data[thr], 0, offsetof(struct cmp_data, go));
617
618 crc = kmalloc(sizeof(*crc), GFP_KERNEL);
619 if (!crc) {
620 printk(KERN_ERR "PM: Failed to allocate crc\n");
621 ret = -ENOMEM;
622 goto out_clean;
623 }
624 memset(crc, 0, offsetof(struct crc_data, go));
625
626 /*
627 * Start the compression threads.
628 */
629 for (thr = 0; thr < nr_threads; thr++) {
630 init_waitqueue_head(&data[thr].go);
631 init_waitqueue_head(&data[thr].done);
632
633 data[thr].thr = kthread_run(lzo_compress_threadfn,
634 &data[thr],
635 "image_compress/%u", thr);
636 if (IS_ERR(data[thr].thr)) {
637 data[thr].thr = NULL;
638 printk(KERN_ERR
639 "PM: Cannot start compression threads\n");
640 ret = -ENOMEM;
641 goto out_clean;
642 }
643 }
644
645 /*
646 * Start the CRC32 thread.
647 */
648 init_waitqueue_head(&crc->go);
649 init_waitqueue_head(&crc->done);
650
651 handle->crc32 = 0;
652 crc->crc32 = &handle->crc32;
653 for (thr = 0; thr < nr_threads; thr++) {
654 crc->unc[thr] = data[thr].unc;
655 crc->unc_len[thr] = &data[thr].unc_len;
656 }
657
658 crc->thr = kthread_run(crc32_threadfn, crc, "image_crc32");
659 if (IS_ERR(crc->thr)) {
660 crc->thr = NULL;
661 printk(KERN_ERR "PM: Cannot start CRC32 thread\n");
662 ret = -ENOMEM;
663 goto out_clean;
664 }
665
666 /*
667 * Adjust the number of required free pages after all allocations have
668 * been done. We don't want to run out of pages when writing.
669 */
670 handle->reqd_free_pages = reqd_free_pages();
671
672 printk(KERN_INFO
673 "PM: Using %u thread(s) for compression.\n"
674 "PM: Compressing and saving image data (%u pages)...\n",
675 nr_threads, nr_to_write);
676 m = nr_to_write / 10;
677 if (!m)
678 m = 1;
679 nr_pages = 0;
680 bio = NULL;
681 do_gettimeofday(&start);
682 for (;;) {
683 for (thr = 0; thr < nr_threads; thr++) {
684 for (off = 0; off < LZO_UNC_SIZE; off += PAGE_SIZE) {
685 ret = snapshot_read_next(snapshot);
686 if (ret < 0)
687 goto out_finish;
688
689 if (!ret)
690 break;
691
692 memcpy(data[thr].unc + off,
693 data_of(*snapshot), PAGE_SIZE);
694
695 if (!(nr_pages % m))
696 printk(KERN_INFO
697 "PM: Image saving progress: "
698 "%3d%%\n",
699 nr_pages / m * 10);
700 nr_pages++;
701 }
702 if (!off)
703 break;
704
705 data[thr].unc_len = off;
706
707 atomic_set(&data[thr].ready, 1);
708 wake_up(&data[thr].go);
709 }
710
711 if (!thr)
712 break;
713
714 crc->run_threads = thr;
715 atomic_set(&crc->ready, 1);
716 wake_up(&crc->go);
717
718 for (run_threads = thr, thr = 0; thr < run_threads; thr++) {
719 wait_event(data[thr].done,
720 atomic_read(&data[thr].stop));
721 atomic_set(&data[thr].stop, 0);
722
723 ret = data[thr].ret;
724
725 if (ret < 0) {
726 printk(KERN_ERR "PM: LZO compression failed\n");
727 goto out_finish;
728 }
729
730 if (unlikely(!data[thr].cmp_len ||
731 data[thr].cmp_len >
732 lzo1x_worst_compress(data[thr].unc_len))) {
733 printk(KERN_ERR
734 "PM: Invalid LZO compressed length\n");
735 ret = -1;
736 goto out_finish;
737 }
738
739 *(size_t *)data[thr].cmp = data[thr].cmp_len;
740
741 /*
742 * Given we are writing one page at a time to disk, we
743 * copy that much from the buffer, although the last
744 * bit will likely be smaller than full page. This is
745 * OK - we saved the length of the compressed data, so
746 * any garbage at the end will be discarded when we
747 * read it.
748 */
749 for (off = 0;
750 off < LZO_HEADER + data[thr].cmp_len;
751 off += PAGE_SIZE) {
752 memcpy(page, data[thr].cmp + off, PAGE_SIZE);
753
754 ret = swap_write_page(handle, page, &bio);
755 if (ret)
756 goto out_finish;
757 }
758 }
759
760 wait_event(crc->done, atomic_read(&crc->stop));
761 atomic_set(&crc->stop, 0);
762 }
763
764 out_finish:
765 err2 = hib_wait_on_bio_chain(&bio);
766 do_gettimeofday(&stop);
767 if (!ret)
768 ret = err2;
769 if (!ret)
770 printk(KERN_INFO "PM: Image saving done.\n");
771 swsusp_show_speed(&start, &stop, nr_to_write, "Wrote");
772 out_clean:
773 if (crc) {
774 if (crc->thr)
775 kthread_stop(crc->thr);
776 kfree(crc);
777 }
778 if (data) {
779 for (thr = 0; thr < nr_threads; thr++)
780 if (data[thr].thr)
781 kthread_stop(data[thr].thr);
782 vfree(data);
783 }
784 if (page) free_page((unsigned long)page);
785
786 return ret;
787 }
788
789 /**
790 * enough_swap - Make sure we have enough swap to save the image.
791 *
792 * Returns TRUE or FALSE after checking the total amount of swap
793 * space avaiable from the resume partition.
794 */
795
796 static int enough_swap(unsigned int nr_pages, unsigned int flags)
797 {
798 unsigned int free_swap = count_swap_pages(root_swap, 1);
799 unsigned int required;
800
801 pr_debug("PM: Free swap pages: %u\n", free_swap);
802
803 required = PAGES_FOR_IO + nr_pages;
804 return free_swap > required;
805 }
806
807 /**
808 * swsusp_write - Write entire image and metadata.
809 * @flags: flags to pass to the "boot" kernel in the image header
810 *
811 * It is important _NOT_ to umount filesystems at this point. We want
812 * them synced (in case something goes wrong) but we DO not want to mark
813 * filesystem clean: it is not. (And it does not matter, if we resume
814 * correctly, we'll mark system clean, anyway.)
815 */
816
817 int swsusp_write(unsigned int flags)
818 {
819 struct swap_map_handle handle;
820 struct snapshot_handle snapshot;
821 struct swsusp_info *header;
822 unsigned long pages;
823 int error;
824
825 pages = snapshot_get_image_size();
826 error = get_swap_writer(&handle);
827 if (error) {
828 printk(KERN_ERR "PM: Cannot get swap writer\n");
829 return error;
830 }
831 if (flags & SF_NOCOMPRESS_MODE) {
832 if (!enough_swap(pages, flags)) {
833 printk(KERN_ERR "PM: Not enough free swap\n");
834 error = -ENOSPC;
835 goto out_finish;
836 }
837 }
838 memset(&snapshot, 0, sizeof(struct snapshot_handle));
839 error = snapshot_read_next(&snapshot);
840 if (error < PAGE_SIZE) {
841 if (error >= 0)
842 error = -EFAULT;
843
844 goto out_finish;
845 }
846 header = (struct swsusp_info *)data_of(snapshot);
847 error = swap_write_page(&handle, header, NULL);
848 if (!error) {
849 error = (flags & SF_NOCOMPRESS_MODE) ?
850 save_image(&handle, &snapshot, pages - 1) :
851 save_image_lzo(&handle, &snapshot, pages - 1);
852 }
853 out_finish:
854 error = swap_writer_finish(&handle, flags, error);
855 return error;
856 }
857
858 /**
859 * The following functions allow us to read data using a swap map
860 * in a file-alike way
861 */
862
863 static void release_swap_reader(struct swap_map_handle *handle)
864 {
865 struct swap_map_page_list *tmp;
866
867 while (handle->maps) {
868 if (handle->maps->map)
869 free_page((unsigned long)handle->maps->map);
870 tmp = handle->maps;
871 handle->maps = handle->maps->next;
872 kfree(tmp);
873 }
874 handle->cur = NULL;
875 }
876
877 static int get_swap_reader(struct swap_map_handle *handle,
878 unsigned int *flags_p)
879 {
880 int error;
881 struct swap_map_page_list *tmp, *last;
882 sector_t offset;
883
884 *flags_p = swsusp_header->flags;
885
886 if (!swsusp_header->image) /* how can this happen? */
887 return -EINVAL;
888
889 handle->cur = NULL;
890 last = handle->maps = NULL;
891 offset = swsusp_header->image;
892 while (offset) {
893 tmp = kmalloc(sizeof(*handle->maps), GFP_KERNEL);
894 if (!tmp) {
895 release_swap_reader(handle);
896 return -ENOMEM;
897 }
898 memset(tmp, 0, sizeof(*tmp));
899 if (!handle->maps)
900 handle->maps = tmp;
901 if (last)
902 last->next = tmp;
903 last = tmp;
904
905 tmp->map = (struct swap_map_page *)
906 __get_free_page(__GFP_WAIT | __GFP_HIGH);
907 if (!tmp->map) {
908 release_swap_reader(handle);
909 return -ENOMEM;
910 }
911
912 error = hib_bio_read_page(offset, tmp->map, NULL);
913 if (error) {
914 release_swap_reader(handle);
915 return error;
916 }
917 offset = tmp->map->next_swap;
918 }
919 handle->k = 0;
920 handle->cur = handle->maps->map;
921 return 0;
922 }
923
924 static int swap_read_page(struct swap_map_handle *handle, void *buf,
925 struct bio **bio_chain)
926 {
927 sector_t offset;
928 int error;
929 struct swap_map_page_list *tmp;
930
931 if (!handle->cur)
932 return -EINVAL;
933 offset = handle->cur->entries[handle->k];
934 if (!offset)
935 return -EFAULT;
936 error = hib_bio_read_page(offset, buf, bio_chain);
937 if (error)
938 return error;
939 if (++handle->k >= MAP_PAGE_ENTRIES) {
940 handle->k = 0;
941 free_page((unsigned long)handle->maps->map);
942 tmp = handle->maps;
943 handle->maps = handle->maps->next;
944 kfree(tmp);
945 if (!handle->maps)
946 release_swap_reader(handle);
947 else
948 handle->cur = handle->maps->map;
949 }
950 return error;
951 }
952
953 static int swap_reader_finish(struct swap_map_handle *handle)
954 {
955 release_swap_reader(handle);
956
957 return 0;
958 }
959
960 /**
961 * load_image - load the image using the swap map handle
962 * @handle and the snapshot handle @snapshot
963 * (assume there are @nr_pages pages to load)
964 */
965
966 static int load_image(struct swap_map_handle *handle,
967 struct snapshot_handle *snapshot,
968 unsigned int nr_to_read)
969 {
970 unsigned int m;
971 int ret = 0;
972 struct timeval start;
973 struct timeval stop;
974 struct bio *bio;
975 int err2;
976 unsigned nr_pages;
977
978 printk(KERN_INFO "PM: Loading image data pages (%u pages)...\n",
979 nr_to_read);
980 m = nr_to_read / 10;
981 if (!m)
982 m = 1;
983 nr_pages = 0;
984 bio = NULL;
985 do_gettimeofday(&start);
986 for ( ; ; ) {
987 ret = snapshot_write_next(snapshot);
988 if (ret <= 0)
989 break;
990 ret = swap_read_page(handle, data_of(*snapshot), &bio);
991 if (ret)
992 break;
993 if (snapshot->sync_read)
994 ret = hib_wait_on_bio_chain(&bio);
995 if (ret)
996 break;
997 if (!(nr_pages % m))
998 printk(KERN_INFO "PM: Image loading progress: %3d%%\n",
999 nr_pages / m * 10);
1000 nr_pages++;
1001 }
1002 err2 = hib_wait_on_bio_chain(&bio);
1003 do_gettimeofday(&stop);
1004 if (!ret)
1005 ret = err2;
1006 if (!ret) {
1007 printk(KERN_INFO "PM: Image loading done.\n");
1008 snapshot_write_finalize(snapshot);
1009 if (!snapshot_image_loaded(snapshot))
1010 ret = -ENODATA;
1011 }
1012 swsusp_show_speed(&start, &stop, nr_to_read, "Read");
1013 return ret;
1014 }
1015
1016 /**
1017 * Structure used for LZO data decompression.
1018 */
1019 struct dec_data {
1020 struct task_struct *thr; /* thread */
1021 atomic_t ready; /* ready to start flag */
1022 atomic_t stop; /* ready to stop flag */
1023 int ret; /* return code */
1024 wait_queue_head_t go; /* start decompression */
1025 wait_queue_head_t done; /* decompression done */
1026 size_t unc_len; /* uncompressed length */
1027 size_t cmp_len; /* compressed length */
1028 unsigned char unc[LZO_UNC_SIZE]; /* uncompressed buffer */
1029 unsigned char cmp[LZO_CMP_SIZE]; /* compressed buffer */
1030 };
1031
1032 /**
1033 * Deompression function that runs in its own thread.
1034 */
1035 static int lzo_decompress_threadfn(void *data)
1036 {
1037 struct dec_data *d = data;
1038
1039 while (1) {
1040 wait_event(d->go, atomic_read(&d->ready) ||
1041 kthread_should_stop());
1042 if (kthread_should_stop()) {
1043 d->thr = NULL;
1044 d->ret = -1;
1045 atomic_set(&d->stop, 1);
1046 wake_up(&d->done);
1047 break;
1048 }
1049 atomic_set(&d->ready, 0);
1050
1051 d->unc_len = LZO_UNC_SIZE;
1052 d->ret = lzo1x_decompress_safe(d->cmp + LZO_HEADER, d->cmp_len,
1053 d->unc, &d->unc_len);
1054 atomic_set(&d->stop, 1);
1055 wake_up(&d->done);
1056 }
1057 return 0;
1058 }
1059
1060 /**
1061 * load_image_lzo - Load compressed image data and decompress them with LZO.
1062 * @handle: Swap map handle to use for loading data.
1063 * @snapshot: Image to copy uncompressed data into.
1064 * @nr_to_read: Number of pages to load.
1065 */
1066 static int load_image_lzo(struct swap_map_handle *handle,
1067 struct snapshot_handle *snapshot,
1068 unsigned int nr_to_read)
1069 {
1070 unsigned int m;
1071 int ret = 0;
1072 int eof = 0;
1073 struct bio *bio;
1074 struct timeval start;
1075 struct timeval stop;
1076 unsigned nr_pages;
1077 size_t off;
1078 unsigned i, thr, run_threads, nr_threads;
1079 unsigned ring = 0, pg = 0, ring_size = 0,
1080 have = 0, want, need, asked = 0;
1081 unsigned long read_pages = 0;
1082 unsigned char **page = NULL;
1083 struct dec_data *data = NULL;
1084 struct crc_data *crc = NULL;
1085
1086 /*
1087 * We'll limit the number of threads for decompression to limit memory
1088 * footprint.
1089 */
1090 nr_threads = num_online_cpus() - 1;
1091 nr_threads = clamp_val(nr_threads, 1, LZO_THREADS);
1092
1093 page = vmalloc(sizeof(*page) * LZO_MAX_RD_PAGES);
1094 if (!page) {
1095 printk(KERN_ERR "PM: Failed to allocate LZO page\n");
1096 ret = -ENOMEM;
1097 goto out_clean;
1098 }
1099
1100 data = vmalloc(sizeof(*data) * nr_threads);
1101 if (!data) {
1102 printk(KERN_ERR "PM: Failed to allocate LZO data\n");
1103 ret = -ENOMEM;
1104 goto out_clean;
1105 }
1106 for (thr = 0; thr < nr_threads; thr++)
1107 memset(&data[thr], 0, offsetof(struct dec_data, go));
1108
1109 crc = kmalloc(sizeof(*crc), GFP_KERNEL);
1110 if (!crc) {
1111 printk(KERN_ERR "PM: Failed to allocate crc\n");
1112 ret = -ENOMEM;
1113 goto out_clean;
1114 }
1115 memset(crc, 0, offsetof(struct crc_data, go));
1116
1117 /*
1118 * Start the decompression threads.
1119 */
1120 for (thr = 0; thr < nr_threads; thr++) {
1121 init_waitqueue_head(&data[thr].go);
1122 init_waitqueue_head(&data[thr].done);
1123
1124 data[thr].thr = kthread_run(lzo_decompress_threadfn,
1125 &data[thr],
1126 "image_decompress/%u", thr);
1127 if (IS_ERR(data[thr].thr)) {
1128 data[thr].thr = NULL;
1129 printk(KERN_ERR
1130 "PM: Cannot start decompression threads\n");
1131 ret = -ENOMEM;
1132 goto out_clean;
1133 }
1134 }
1135
1136 /*
1137 * Start the CRC32 thread.
1138 */
1139 init_waitqueue_head(&crc->go);
1140 init_waitqueue_head(&crc->done);
1141
1142 handle->crc32 = 0;
1143 crc->crc32 = &handle->crc32;
1144 for (thr = 0; thr < nr_threads; thr++) {
1145 crc->unc[thr] = data[thr].unc;
1146 crc->unc_len[thr] = &data[thr].unc_len;
1147 }
1148
1149 crc->thr = kthread_run(crc32_threadfn, crc, "image_crc32");
1150 if (IS_ERR(crc->thr)) {
1151 crc->thr = NULL;
1152 printk(KERN_ERR "PM: Cannot start CRC32 thread\n");
1153 ret = -ENOMEM;
1154 goto out_clean;
1155 }
1156
1157 /*
1158 * Set the number of pages for read buffering.
1159 * This is complete guesswork, because we'll only know the real
1160 * picture once prepare_image() is called, which is much later on
1161 * during the image load phase. We'll assume the worst case and
1162 * say that none of the image pages are from high memory.
1163 */
1164 if (low_free_pages() > snapshot_get_image_size())
1165 read_pages = (low_free_pages() - snapshot_get_image_size()) / 2;
1166 read_pages = clamp_val(read_pages, LZO_MIN_RD_PAGES, LZO_MAX_RD_PAGES);
1167
1168 for (i = 0; i < read_pages; i++) {
1169 page[i] = (void *)__get_free_page(i < LZO_CMP_PAGES ?
1170 __GFP_WAIT | __GFP_HIGH :
1171 __GFP_WAIT | __GFP_NOWARN |
1172 __GFP_NORETRY);
1173
1174 if (!page[i]) {
1175 if (i < LZO_CMP_PAGES) {
1176 ring_size = i;
1177 printk(KERN_ERR
1178 "PM: Failed to allocate LZO pages\n");
1179 ret = -ENOMEM;
1180 goto out_clean;
1181 } else {
1182 break;
1183 }
1184 }
1185 }
1186 want = ring_size = i;
1187
1188 printk(KERN_INFO
1189 "PM: Using %u thread(s) for decompression.\n"
1190 "PM: Loading and decompressing image data (%u pages)...\n",
1191 nr_threads, nr_to_read);
1192 m = nr_to_read / 10;
1193 if (!m)
1194 m = 1;
1195 nr_pages = 0;
1196 bio = NULL;
1197 do_gettimeofday(&start);
1198
1199 ret = snapshot_write_next(snapshot);
1200 if (ret <= 0)
1201 goto out_finish;
1202
1203 for(;;) {
1204 for (i = 0; !eof && i < want; i++) {
1205 ret = swap_read_page(handle, page[ring], &bio);
1206 if (ret) {
1207 /*
1208 * On real read error, finish. On end of data,
1209 * set EOF flag and just exit the read loop.
1210 */
1211 if (handle->cur &&
1212 handle->cur->entries[handle->k]) {
1213 goto out_finish;
1214 } else {
1215 eof = 1;
1216 break;
1217 }
1218 }
1219 if (++ring >= ring_size)
1220 ring = 0;
1221 }
1222 asked += i;
1223 want -= i;
1224
1225 /*
1226 * We are out of data, wait for some more.
1227 */
1228 if (!have) {
1229 if (!asked)
1230 break;
1231
1232 ret = hib_wait_on_bio_chain(&bio);
1233 if (ret)
1234 goto out_finish;
1235 have += asked;
1236 asked = 0;
1237 if (eof)
1238 eof = 2;
1239 }
1240
1241 if (crc->run_threads) {
1242 wait_event(crc->done, atomic_read(&crc->stop));
1243 atomic_set(&crc->stop, 0);
1244 crc->run_threads = 0;
1245 }
1246
1247 for (thr = 0; have && thr < nr_threads; thr++) {
1248 data[thr].cmp_len = *(size_t *)page[pg];
1249 if (unlikely(!data[thr].cmp_len ||
1250 data[thr].cmp_len >
1251 lzo1x_worst_compress(LZO_UNC_SIZE))) {
1252 printk(KERN_ERR
1253 "PM: Invalid LZO compressed length\n");
1254 ret = -1;
1255 goto out_finish;
1256 }
1257
1258 need = DIV_ROUND_UP(data[thr].cmp_len + LZO_HEADER,
1259 PAGE_SIZE);
1260 if (need > have) {
1261 if (eof > 1) {
1262 ret = -1;
1263 goto out_finish;
1264 }
1265 break;
1266 }
1267
1268 for (off = 0;
1269 off < LZO_HEADER + data[thr].cmp_len;
1270 off += PAGE_SIZE) {
1271 memcpy(data[thr].cmp + off,
1272 page[pg], PAGE_SIZE);
1273 have--;
1274 want++;
1275 if (++pg >= ring_size)
1276 pg = 0;
1277 }
1278
1279 atomic_set(&data[thr].ready, 1);
1280 wake_up(&data[thr].go);
1281 }
1282
1283 /*
1284 * Wait for more data while we are decompressing.
1285 */
1286 if (have < LZO_CMP_PAGES && asked) {
1287 ret = hib_wait_on_bio_chain(&bio);
1288 if (ret)
1289 goto out_finish;
1290 have += asked;
1291 asked = 0;
1292 if (eof)
1293 eof = 2;
1294 }
1295
1296 for (run_threads = thr, thr = 0; thr < run_threads; thr++) {
1297 wait_event(data[thr].done,
1298 atomic_read(&data[thr].stop));
1299 atomic_set(&data[thr].stop, 0);
1300
1301 ret = data[thr].ret;
1302
1303 if (ret < 0) {
1304 printk(KERN_ERR
1305 "PM: LZO decompression failed\n");
1306 goto out_finish;
1307 }
1308
1309 if (unlikely(!data[thr].unc_len ||
1310 data[thr].unc_len > LZO_UNC_SIZE ||
1311 data[thr].unc_len & (PAGE_SIZE - 1))) {
1312 printk(KERN_ERR
1313 "PM: Invalid LZO uncompressed length\n");
1314 ret = -1;
1315 goto out_finish;
1316 }
1317
1318 for (off = 0;
1319 off < data[thr].unc_len; off += PAGE_SIZE) {
1320 memcpy(data_of(*snapshot),
1321 data[thr].unc + off, PAGE_SIZE);
1322
1323 if (!(nr_pages % m))
1324 printk(KERN_INFO
1325 "PM: Image loading progress: "
1326 "%3d%%\n",
1327 nr_pages / m * 10);
1328 nr_pages++;
1329
1330 ret = snapshot_write_next(snapshot);
1331 if (ret <= 0) {
1332 crc->run_threads = thr + 1;
1333 atomic_set(&crc->ready, 1);
1334 wake_up(&crc->go);
1335 goto out_finish;
1336 }
1337 }
1338 }
1339
1340 crc->run_threads = thr;
1341 atomic_set(&crc->ready, 1);
1342 wake_up(&crc->go);
1343 }
1344
1345 out_finish:
1346 if (crc->run_threads) {
1347 wait_event(crc->done, atomic_read(&crc->stop));
1348 atomic_set(&crc->stop, 0);
1349 }
1350 do_gettimeofday(&stop);
1351 if (!ret) {
1352 printk(KERN_INFO "PM: Image loading done.\n");
1353 snapshot_write_finalize(snapshot);
1354 if (!snapshot_image_loaded(snapshot))
1355 ret = -ENODATA;
1356 if (!ret) {
1357 if (swsusp_header->flags & SF_CRC32_MODE) {
1358 if(handle->crc32 != swsusp_header->crc32) {
1359 printk(KERN_ERR
1360 "PM: Invalid image CRC32!\n");
1361 ret = -ENODATA;
1362 }
1363 }
1364 }
1365 }
1366 swsusp_show_speed(&start, &stop, nr_to_read, "Read");
1367 out_clean:
1368 for (i = 0; i < ring_size; i++)
1369 free_page((unsigned long)page[i]);
1370 if (crc) {
1371 if (crc->thr)
1372 kthread_stop(crc->thr);
1373 kfree(crc);
1374 }
1375 if (data) {
1376 for (thr = 0; thr < nr_threads; thr++)
1377 if (data[thr].thr)
1378 kthread_stop(data[thr].thr);
1379 vfree(data);
1380 }
1381 if (page) vfree(page);
1382
1383 return ret;
1384 }
1385
1386 /**
1387 * swsusp_read - read the hibernation image.
1388 * @flags_p: flags passed by the "frozen" kernel in the image header should
1389 * be written into this memory location
1390 */
1391
1392 int swsusp_read(unsigned int *flags_p)
1393 {
1394 int error;
1395 struct swap_map_handle handle;
1396 struct snapshot_handle snapshot;
1397 struct swsusp_info *header;
1398
1399 memset(&snapshot, 0, sizeof(struct snapshot_handle));
1400 error = snapshot_write_next(&snapshot);
1401 if (error < PAGE_SIZE)
1402 return error < 0 ? error : -EFAULT;
1403 header = (struct swsusp_info *)data_of(snapshot);
1404 error = get_swap_reader(&handle, flags_p);
1405 if (error)
1406 goto end;
1407 if (!error)
1408 error = swap_read_page(&handle, header, NULL);
1409 if (!error) {
1410 error = (*flags_p & SF_NOCOMPRESS_MODE) ?
1411 load_image(&handle, &snapshot, header->pages - 1) :
1412 load_image_lzo(&handle, &snapshot, header->pages - 1);
1413 }
1414 swap_reader_finish(&handle);
1415 end:
1416 if (!error)
1417 pr_debug("PM: Image successfully loaded\n");
1418 else
1419 pr_debug("PM: Error %d resuming\n", error);
1420 return error;
1421 }
1422
1423 /**
1424 * swsusp_check - Check for swsusp signature in the resume device
1425 */
1426
1427 int swsusp_check(void)
1428 {
1429 int error;
1430
1431 hib_resume_bdev = blkdev_get_by_dev(swsusp_resume_device,
1432 FMODE_READ, NULL);
1433 if (!IS_ERR(hib_resume_bdev)) {
1434 set_blocksize(hib_resume_bdev, PAGE_SIZE);
1435 clear_page(swsusp_header);
1436 error = hib_bio_read_page(swsusp_resume_block,
1437 swsusp_header, NULL);
1438 if (error)
1439 goto put;
1440
1441 if (!memcmp(HIBERNATE_SIG, swsusp_header->sig, 10)) {
1442 memcpy(swsusp_header->sig, swsusp_header->orig_sig, 10);
1443 /* Reset swap signature now */
1444 error = hib_bio_write_page(swsusp_resume_block,
1445 swsusp_header, NULL);
1446 } else {
1447 error = -EINVAL;
1448 }
1449
1450 put:
1451 if (error)
1452 blkdev_put(hib_resume_bdev, FMODE_READ);
1453 else
1454 pr_debug("PM: Image signature found, resuming\n");
1455 } else {
1456 error = PTR_ERR(hib_resume_bdev);
1457 }
1458
1459 if (error)
1460 pr_debug("PM: Image not found (code %d)\n", error);
1461
1462 return error;
1463 }
1464
1465 /**
1466 * swsusp_close - close swap device.
1467 */
1468
1469 void swsusp_close(fmode_t mode)
1470 {
1471 if (IS_ERR(hib_resume_bdev)) {
1472 pr_debug("PM: Image device not initialised\n");
1473 return;
1474 }
1475
1476 blkdev_put(hib_resume_bdev, mode);
1477 }
1478
1479 /**
1480 * swsusp_unmark - Unmark swsusp signature in the resume device
1481 */
1482
1483 #ifdef CONFIG_SUSPEND
1484 int swsusp_unmark(void)
1485 {
1486 int error;
1487
1488 hib_bio_read_page(swsusp_resume_block, swsusp_header, NULL);
1489 if (!memcmp(HIBERNATE_SIG,swsusp_header->sig, 10)) {
1490 memcpy(swsusp_header->sig,swsusp_header->orig_sig, 10);
1491 error = hib_bio_write_page(swsusp_resume_block,
1492 swsusp_header, NULL);
1493 } else {
1494 printk(KERN_ERR "PM: Cannot find swsusp signature!\n");
1495 error = -ENODEV;
1496 }
1497
1498 /*
1499 * We just returned from suspend, we don't need the image any more.
1500 */
1501 free_all_swap_pages(root_swap);
1502
1503 return error;
1504 }
1505 #endif
1506
1507 static int swsusp_header_init(void)
1508 {
1509 swsusp_header = (struct swsusp_header*) __get_free_page(GFP_KERNEL);
1510 if (!swsusp_header)
1511 panic("Could not allocate memory for swsusp_header\n");
1512 return 0;
1513 }
1514
1515 core_initcall(swsusp_header_init);
kernel source for telekom puls (alcatel/mediatek)