1
2#include <stdint.h>
3#include <stdlib.h>
4#include <stdio.h>
5#include <stddef.h>
6#include <strings.h>
7#include <sys/mman.h>
8#include <fcntl.h>
9#include <mach/mach_types.h>
10#include <mach/vm_param.h>
11#include <IOKit/IOHibernatePrivate.h>
12
13
14#define ptoa_64(p)	  (((uint64_t)p) << PAGE_SHIFT)
15#define ptoa_32(p)	  ptoa_64(p)
16#define atop_64(p)	  ((p) >> PAGE_SHIFT)
17#define pal_hib_map(a, p) (p)
18
19enum { kIOHibernateTagLength    = 0x00001fff };
20
21static IOHibernateImageHeader _hibernateHeader;
22IOHibernateImageHeader * gIOHibernateCurrentHeader = &_hibernateHeader;
23
24ppnum_t gIOHibernateHandoffPages[64];
25uint32_t gIOHibernateHandoffPageCount = sizeof(gIOHibernateHandoffPages)
26					/ sizeof(gIOHibernateHandoffPages[0]);
27
28/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
29
30static uint32_t
31store_one_page(uint32_t * src, uint32_t compressedSize,
32		uint32_t * buffer, uint32_t ppnum)
33{
34//    printf("ppnum 0x%x\n", ppnum);
35}
36
37
38/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
39
40uint32_t
41hibernate_sum_page(uint8_t *buf, uint32_t ppnum)
42{
43    return (((uint32_t *)buf)[((PAGE_SIZE >> 2) - 1) & ppnum]);
44}
45
46/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
47
48static hibernate_bitmap_t *
49hibernate_page_bitmap(hibernate_page_list_t * list, uint32_t page)
50{
51    uint32_t             bank;
52    hibernate_bitmap_t * bitmap = &list->bank_bitmap[0];
53
54    for (bank = 0; bank < list->bank_count; bank++)
55    {
56	if ((page >= bitmap->first_page) && (page <= bitmap->last_page))
57	    break;
58	bitmap = (hibernate_bitmap_t *) &bitmap->bitmap[bitmap->bitmapwords];
59    }
60    if (bank == list->bank_count)
61	bitmap = NULL;
62
63    return (bitmap);
64}
65
66hibernate_bitmap_t *
67hibernate_page_bitmap_pin(hibernate_page_list_t * list, uint32_t * pPage)
68{
69    uint32_t             bank, page = *pPage;
70    hibernate_bitmap_t * bitmap = &list->bank_bitmap[0];
71
72    for (bank = 0; bank < list->bank_count; bank++)
73    {
74	if (page <= bitmap->first_page)
75	{
76	    *pPage = bitmap->first_page;
77	    break;
78	}
79	if (page <= bitmap->last_page)
80	    break;
81	bitmap = (hibernate_bitmap_t *) &bitmap->bitmap[bitmap->bitmapwords];
82    }
83    if (bank == list->bank_count)
84	bitmap = NULL;
85
86    return (bitmap);
87}
88
89void
90hibernate_page_bitset(hibernate_page_list_t * list, boolean_t set, uint32_t page)
91{
92    hibernate_bitmap_t * bitmap;
93
94    bitmap = hibernate_page_bitmap(list, page);
95    if (bitmap)
96    {
97	page -= bitmap->first_page;
98	if (set)
99	    bitmap->bitmap[page >> 5] |= (0x80000000 >> (page & 31));
100	    //setbit(page - bitmap->first_page, (int *) &bitmap->bitmap[0]);
101	else
102	    bitmap->bitmap[page >> 5] &= ~(0x80000000 >> (page & 31));
103	    //clrbit(page - bitmap->first_page, (int *) &bitmap->bitmap[0]);
104    }
105}
106
107boolean_t
108hibernate_page_bittst(hibernate_page_list_t * list, uint32_t page)
109{
110    boolean_t		 result = TRUE;
111    hibernate_bitmap_t * bitmap;
112
113    bitmap = hibernate_page_bitmap(list, page);
114    if (bitmap)
115    {
116	page -= bitmap->first_page;
117	result = (0 != (bitmap->bitmap[page >> 5] & (0x80000000 >> (page & 31))));
118    }
119    return (result);
120}
121
122// count bits clear or set (set == TRUE) starting at page.
123uint32_t
124hibernate_page_bitmap_count(hibernate_bitmap_t * bitmap, uint32_t set, uint32_t page)
125{
126    uint32_t index, bit, bits;
127    uint32_t count;
128
129    count = 0;
130
131    index = (page - bitmap->first_page) >> 5;
132    bit = (page - bitmap->first_page) & 31;
133
134    bits = bitmap->bitmap[index];
135    if (set)
136	bits = ~bits;
137    bits = (bits << bit);
138    if (bits)
139	count += __builtin_clz(bits);
140    else
141    {
142	count += 32 - bit;
143	while (++index < bitmap->bitmapwords)
144	{
145	    bits = bitmap->bitmap[index];
146	    if (set)
147		bits = ~bits;
148	    if (bits)
149	    {
150		count += __builtin_clz(bits);
151		break;
152	    }
153	    count += 32;
154	}
155    }
156
157    if ((page + count) > (bitmap->last_page + 1)) count = (bitmap->last_page + 1) - page;
158
159    return (count);
160}
161
162static ppnum_t
163hibernate_page_list_grab(hibernate_page_list_t * list, uint32_t * pNextFree)
164{
165    uint32_t		 nextFree = *pNextFree;
166    uint32_t		 nextFreeInBank;
167    hibernate_bitmap_t * bitmap;
168
169    nextFreeInBank = nextFree + 1;
170    while ((bitmap = hibernate_page_bitmap_pin(list, &nextFreeInBank)))
171    {
172	nextFreeInBank += hibernate_page_bitmap_count(bitmap, FALSE, nextFreeInBank);
173	if (nextFreeInBank <= bitmap->last_page)
174	{
175	    *pNextFree = nextFreeInBank;
176	    break;
177	}
178    }
179
180    if (!bitmap)
181    {
182	exit(1);
183	nextFree = 0;
184    }
185
186    return (nextFree);
187}
188
189/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
190
191void process_image(const void * image)
192{
193    uint64_t headerPhys;
194    uint64_t mapPhys;
195    uint64_t srcPhys;
196    uint64_t imageReadPhys;
197    uint64_t pageIndexPhys;
198    uint32_t idx;
199    uint32_t * pageIndexSource;
200    hibernate_page_list_t * map;
201    uint32_t stage;
202    uint32_t count;
203    uint32_t ppnum;
204    uint32_t page;
205    uint32_t conflictCount;
206    uint32_t compressedSize;
207    uint32_t uncompressedPages;
208    uint32_t copyPageListHeadPage;
209    uint32_t pageListPage;
210    uint32_t * copyPageList;
211    uint32_t * src;
212    uint32_t copyPageIndex;
213    uint32_t sum;
214    uint32_t pageSum;
215    uint32_t nextFree;
216    uint32_t lastImagePage;
217    uint32_t lastMapPage;
218    uint32_t lastPageIndexPage;
219    uint32_t handoffPages;
220    uint32_t handoffPageCount;
221
222
223    headerPhys = (uint64_t) image;
224
225    bcopy(image, gIOHibernateCurrentHeader, sizeof(IOHibernateImageHeader));
226
227    mapPhys = headerPhys
228             + (offsetof(IOHibernateImageHeader, fileExtentMap)
229	     + gIOHibernateCurrentHeader->fileExtentMapSize
230	     + ptoa_32(gIOHibernateCurrentHeader->restore1PageCount)
231	     + gIOHibernateCurrentHeader->previewSize);
232
233    map = (hibernate_page_list_t *) pal_hib_map(BITMAP_AREA, mapPhys);
234
235    lastImagePage = atop_64(headerPhys + gIOHibernateCurrentHeader->image1Size);
236    lastMapPage = atop_64(mapPhys + gIOHibernateCurrentHeader->bitmapSize);
237
238    handoffPages     = gIOHibernateCurrentHeader->handoffPages;
239    handoffPageCount = gIOHibernateCurrentHeader->handoffPageCount;
240
241    nextFree = 0;
242    hibernate_page_list_grab(map, &nextFree);
243
244    sum = gIOHibernateCurrentHeader->actualRestore1Sum;
245    gIOHibernateCurrentHeader->diag[0] = atop_64(headerPhys);
246    gIOHibernateCurrentHeader->diag[1] = sum;
247
248    uncompressedPages    = 0;
249    conflictCount        = 0;
250    copyPageListHeadPage = 0;
251    copyPageList         = 0;
252    copyPageIndex        = PAGE_SIZE >> 2;
253
254    compressedSize       = PAGE_SIZE;
255    stage                = 2;
256    count                = 0;
257    srcPhys              = 0;
258
259
260    if (gIOHibernateCurrentHeader->previewSize)
261    {
262	pageIndexPhys     = headerPhys
263	                   + (offsetof(IOHibernateImageHeader, fileExtentMap)
264			   + gIOHibernateCurrentHeader->fileExtentMapSize
265			   + ptoa_32(gIOHibernateCurrentHeader->restore1PageCount));
266	imageReadPhys     = (pageIndexPhys + gIOHibernateCurrentHeader->previewPageListSize);
267	lastPageIndexPage = atop_64(imageReadPhys);
268	pageIndexSource   = (uint32_t *) pal_hib_map(IMAGE2_AREA, pageIndexPhys);
269    }
270    else
271    {
272	pageIndexPhys     = 0;
273	lastPageIndexPage = 0;
274	imageReadPhys     = (mapPhys + gIOHibernateCurrentHeader->bitmapSize);
275    }
276
277
278    while (1)
279    {
280	switch (stage)
281	{
282	    case 2:
283		// copy handoff data
284		count = srcPhys ? 0 : handoffPageCount;
285		if (!count)
286		    break;
287		if (count > gIOHibernateHandoffPageCount) count = gIOHibernateHandoffPageCount;
288		srcPhys = ptoa_64(handoffPages);
289		break;
290
291	    case 1:
292		// copy pageIndexSource pages == preview image data
293		if (!srcPhys)
294		{
295		    if (!pageIndexPhys) break;
296		    srcPhys = imageReadPhys;
297		}
298		ppnum = pageIndexSource[0];
299		count = pageIndexSource[1];
300		pageIndexSource += 2;
301		pageIndexPhys   += 2 * sizeof(pageIndexSource[0]);
302		imageReadPhys = srcPhys;
303		break;
304
305	    case 0:
306		// copy pages
307		if (!srcPhys) srcPhys = (mapPhys + gIOHibernateCurrentHeader->bitmapSize);
308		src = (uint32_t *) pal_hib_map(IMAGE_AREA, srcPhys);
309		ppnum = src[0];
310		count = src[1];
311		srcPhys += 2 * sizeof(*src);
312		imageReadPhys = srcPhys;
313		break;
314	}
315
316
317	if (!count)
318	{
319	    if (!stage)
320	        break;
321	    stage--;
322	    srcPhys = 0;
323	    continue;
324	}
325
326if (!stage) printf("phys 0x%x, 0x%x\n", ppnum, count);
327
328	for (page = 0; page < count; page++, ppnum++)
329	{
330	    uint32_t tag;
331	    int conflicts;
332
333	    src = (uint32_t *) pal_hib_map(IMAGE_AREA, srcPhys);
334
335	    if (2 == stage) ppnum = gIOHibernateHandoffPages[page];
336	    else if (!stage)
337	    {
338		tag = *src++;
339		srcPhys += sizeof(*src);
340		compressedSize = kIOHibernateTagLength & tag;
341	    }
342
343	    conflicts = (ppnum >= atop_64(mapPhys)) && (ppnum <= lastMapPage);
344
345	    conflicts |= ((ppnum >= atop_64(imageReadPhys)) && (ppnum <= lastImagePage));
346
347	    if (stage >= 2)
348 		conflicts |= ((ppnum >= atop_64(srcPhys)) && (ppnum <= (handoffPages + handoffPageCount - 1)));
349
350	    if (stage >= 1)
351 		conflicts |= ((ppnum >= atop_64(pageIndexPhys)) && (ppnum <= lastPageIndexPage));
352
353//	    if (!conflicts)
354	    {
355//              if (compressedSize)
356		pageSum = store_one_page(src, compressedSize, 0, ppnum);
357		if (stage != 2)
358		    sum += pageSum;
359		uncompressedPages++;
360	    }
361
362	    srcPhys += ((compressedSize + 3) & ~3);
363	    src     += ((compressedSize + 3) >> 2);
364
365	}
366    }
367}
368
369
370/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
371
372int main(int argc, char * argv[])
373{
374    int fd;
375    void * map;
376
377    fd = open("/var/vm/sleepimage", O_RDONLY);
378    map = mmap(NULL, 1*1024*1024*1024, PROT_READ, MAP_FILE|MAP_PRIVATE, fd, 0);
379
380    printf("map %p\n", map);
381    process_image(map);
382}
383
384