1338b8dc3SIngo Weinhold
2338b8dc3SIngo Weinhold#include <ctype.h>
3338b8dc3SIngo Weinhold#include <errno.h>
4338b8dc3SIngo Weinhold#include <fcntl.h>
5338b8dc3SIngo Weinhold#include <stdint.h>
6338b8dc3SIngo Weinhold#include <stdio.h>
7338b8dc3SIngo Weinhold#include <stdlib.h>
8338b8dc3SIngo Weinhold#include <string.h>
9338b8dc3SIngo Weinhold#include <unistd.h>
10338b8dc3SIngo Weinhold
11338b8dc3SIngo Weinhold#define	int8	int8_t
12338b8dc3SIngo Weinhold#define	int16	int16_t
13338b8dc3SIngo Weinhold#define	int32	int32_t
14338b8dc3SIngo Weinhold#define	int64	int64_t
15338b8dc3SIngo Weinhold#define	uint8	uint8_t
16338b8dc3SIngo Weinhold#define	uint16	uint16_t
17338b8dc3SIngo Weinhold#define	uint32	uint32_t
18338b8dc3SIngo Weinhold#define	uint64	uint64_t
19338b8dc3SIngo Weinhold
20338b8dc3SIngo Weinholdtypedef int64 bfs_off_t;
21338b8dc3SIngo Weinhold
22338b8dc3SIngo Weinholdstruct block_run {
23338b8dc3SIngo Weinhold	int32		allocation_group;
24338b8dc3SIngo Weinhold	uint16		start;
25338b8dc3SIngo Weinhold	uint16		length;
26338b8dc3SIngo Weinhold};
27338b8dc3SIngo Weinhold
28338b8dc3SIngo Weinholdtypedef block_run inode_addr;
29338b8dc3SIngo Weinhold
30338b8dc3SIngo Weinhold#define BFS_DISK_NAME_LENGTH	32
31338b8dc3SIngo Weinhold
32338b8dc3SIngo Weinholdstruct disk_super_block {
33338b8dc3SIngo Weinhold	char		name[BFS_DISK_NAME_LENGTH];
34338b8dc3SIngo Weinhold	int32		magic1;
35338b8dc3SIngo Weinhold	int32		fs_byte_order;
36338b8dc3SIngo Weinhold	uint32		block_size;
37338b8dc3SIngo Weinhold	uint32		block_shift;
38338b8dc3SIngo Weinhold	bfs_off_t	num_blocks;
39338b8dc3SIngo Weinhold	bfs_off_t	used_blocks;
40338b8dc3SIngo Weinhold	int32		inode_size;
41338b8dc3SIngo Weinhold	int32		magic2;
42338b8dc3SIngo Weinhold	int32		blocks_per_ag;
43338b8dc3SIngo Weinhold	int32		ag_shift;
44338b8dc3SIngo Weinhold	int32		num_ags;
45338b8dc3SIngo Weinhold	int32		flags;
46338b8dc3SIngo Weinhold	block_run	log_blocks;
47338b8dc3SIngo Weinhold	bfs_off_t	log_start;
48338b8dc3SIngo Weinhold	bfs_off_t	log_end;
49338b8dc3SIngo Weinhold	int32		magic3;
50338b8dc3SIngo Weinhold	inode_addr	root_dir;
51338b8dc3SIngo Weinhold	inode_addr	indices;
52338b8dc3SIngo Weinhold	int32		pad[8];
53338b8dc3SIngo Weinhold};
54338b8dc3SIngo Weinhold
55338b8dc3SIngo Weinhold#define SUPER_BLOCK_MAGIC1			'BFS1'		/* BFS1 */
56338b8dc3SIngo Weinhold
57338b8dc3SIngo Weinholdconst char *kHexDigits = "0123456789abcdef";
58338b8dc3SIngo Weinhold
59338b8dc3SIngo Weinhold
60338b8dc3SIngo Weinholdstatic void
61338b8dc3SIngo Weinholdread_from(int fd, off_t pos, void *buffer, size_t size)
62338b8dc3SIngo Weinhold{
63338b8dc3SIngo Weinhold	if (lseek(fd, pos, SEEK_SET) < 0
64338b8dc3SIngo Weinhold		|| read(fd, buffer, size) != (int)size) {
65338b8dc3SIngo Weinhold		fprintf(stderr, "Error: Failed to read %lu bytes at offset %lld\n",
66338b8dc3SIngo Weinhold			size, pos);
67338b8dc3SIngo Weinhold		exit(1);
68338b8dc3SIngo Weinhold	}
69338b8dc3SIngo Weinhold}
70338b8dc3SIngo Weinhold
71338b8dc3SIngo Weinhold
72338b8dc3SIngo Weinholdstatic void
73338b8dc3SIngo Weinholdwrite_to(int fd, off_t pos, const void *buffer, size_t size)
74338b8dc3SIngo Weinhold{
75338b8dc3SIngo Weinhold	if (lseek(fd, pos, SEEK_SET) < 0
76338b8dc3SIngo Weinhold		|| write(fd, buffer, size) != (int)size) {
77338b8dc3SIngo Weinhold		fprintf(stderr, "Error: Failed to write %lu bytes at offset %lld\n",
78338b8dc3SIngo Weinhold			size, pos);
79338b8dc3SIngo Weinhold		exit(1);
80338b8dc3SIngo Weinhold	}
81338b8dc3SIngo Weinhold}
82338b8dc3SIngo Weinhold
83338b8dc3SIngo Weinholdstatic int
84338b8dc3SIngo Weinholdcount_bits(int value)
85338b8dc3SIngo Weinhold{
86338b8dc3SIngo Weinhold	int count = 0;
87338b8dc3SIngo Weinhold
88338b8dc3SIngo Weinhold	while (value) {
89338b8dc3SIngo Weinhold		if (value & 1)
90338b8dc3SIngo Weinhold			count++;
91338b8dc3SIngo Weinhold		value >>= 1;
92338b8dc3SIngo Weinhold	}
935644d283SMatt Madia
94338b8dc3SIngo Weinhold	return count;
95338b8dc3SIngo Weinhold}
96338b8dc3SIngo Weinhold
97338b8dc3SIngo Weinholdint
98338b8dc3SIngo Weinholdmain(int argc, const char *const *argv)
99338b8dc3SIngo Weinhold{
100338b8dc3SIngo Weinhold	if (argc < 2 || argc > 3) {
101338b8dc3SIngo Weinhold		fprintf(stderr, "Usage: %s <image> [ <hex OR pattern> ]\n", argv[0]);
102338b8dc3SIngo Weinhold		exit(1);
103338b8dc3SIngo Weinhold	}
104338b8dc3SIngo Weinhold
1055644d283SMatt Madia	const char *fileName = argv[1];
106338b8dc3SIngo Weinhold	const char *patternString = (argc >= 3 ? argv[2] : "0f");
107338b8dc3SIngo Weinhold
108338b8dc3SIngo Weinhold	// skip leading "0x" in the pattern string
109338b8dc3SIngo Weinhold	if (strncmp(patternString, "0x", 2) == 0)
110338b8dc3SIngo Weinhold		patternString += 2;
111338b8dc3SIngo Weinhold
112338b8dc3SIngo Weinhold	// translate the pattern
113338b8dc3SIngo Weinhold	uint8 *pattern = new uint8[strlen(patternString) / 2 + 1];
114338b8dc3SIngo Weinhold	int patternLen = 0;
115338b8dc3SIngo Weinhold	for (; *patternString; patternString++) {
116338b8dc3SIngo Weinhold		if (!isspace(*patternString)) {
117338b8dc3SIngo Weinhold			char c = *patternString;
118338b8dc3SIngo Weinhold			const char *digitPos = strchr(kHexDigits, tolower(c));
119338b8dc3SIngo Weinhold			if (!digitPos) {
120338b8dc3SIngo Weinhold				fprintf(stderr, "Error: Invalid pattern character: \"%c\"\n",
121338b8dc3SIngo Weinhold					c);
122338b8dc3SIngo Weinhold				exit(1);
123338b8dc3SIngo Weinhold			}
124338b8dc3SIngo Weinhold			int digit = digitPos - kHexDigits;
125338b8dc3SIngo Weinhold
126338b8dc3SIngo Weinhold			if (patternLen & 1)
127338b8dc3SIngo Weinhold				pattern[patternLen / 2] |= digit;
128338b8dc3SIngo Weinhold			else
129338b8dc3SIngo Weinhold				pattern[patternLen / 2] = digit << 4;
130338b8dc3SIngo Weinhold			patternLen++;
131338b8dc3SIngo Weinhold		}
132338b8dc3SIngo Weinhold	}
133338b8dc3SIngo Weinhold
134338b8dc3SIngo Weinhold	if (patternLen == 0 || (patternLen & 1)) {
135338b8dc3SIngo Weinhold		fprintf(stderr, "Error: Invalid pattern! Must have even number (>= 2) "
136338b8dc3SIngo Weinhold			"of hex digits.\n");
137338b8dc3SIngo Weinhold		exit(1);
138338b8dc3SIngo Weinhold	}
139338b8dc3SIngo Weinhold	patternLen /= 2;	// is now length in bytes
140338b8dc3SIngo Weinhold
141338b8dc3SIngo Weinhold	printf("bfs_fragmenter: using pattern: 0x");
142338b8dc3SIngo Weinhold	for (int i = 0; i < patternLen; i++)
143338b8dc3SIngo Weinhold		printf("%02x", pattern[i]);
144338b8dc3SIngo Weinhold	printf("\n");
145338b8dc3SIngo Weinhold
146338b8dc3SIngo Weinhold	// open file
147338b8dc3SIngo Weinhold	int fd = open(fileName, O_RDWR);
148338b8dc3SIngo Weinhold	if (fd < 0) {
149338b8dc3SIngo Weinhold		fprintf(stderr, "Error: Failed to open \"%s\": %s\n", fileName,
150338b8dc3SIngo Weinhold			strerror(errno));
151338b8dc3SIngo Weinhold		exit(1);
152338b8dc3SIngo Weinhold	}
153338b8dc3SIngo Weinhold
1545644d283SMatt Madia	// read superblock
155338b8dc3SIngo Weinhold	disk_super_block superBlock;
156338b8dc3SIngo Weinhold	read_from(fd, 512, &superBlock, sizeof(superBlock));
157338b8dc3SIngo Weinhold
158338b8dc3SIngo Weinhold	if (superBlock.magic1 != SUPER_BLOCK_MAGIC1) {
1595644d283SMatt Madia		fprintf(stderr, "Error: Bad superblock magic. This is probably not a "
160338b8dc3SIngo Weinhold			"BFS image.\n");
161338b8dc3SIngo Weinhold		exit(1);
162338b8dc3SIngo Weinhold	}
163338b8dc3SIngo Weinhold	int blockSize = superBlock.block_size;
164338b8dc3SIngo Weinhold
165338b8dc3SIngo Weinhold	// stat the image
166338b8dc3SIngo Weinhold	struct stat st;
167338b8dc3SIngo Weinhold	if (fstat(fd, &st) < 0) {
168338b8dc3SIngo Weinhold		fprintf(stderr, "Error: Failed to stat image: %s\n", strerror(errno));
169338b8dc3SIngo Weinhold		exit(1);
170338b8dc3SIngo Weinhold	}
171338b8dc3SIngo Weinhold
172338b8dc3SIngo Weinhold	int64 blockCount = st.st_size / blockSize;
173338b8dc3SIngo Weinhold	if (blockCount != superBlock.num_blocks) {
174338b8dc3SIngo Weinhold		fprintf(stderr, "Error: Number of blocks in image and the number in "
1755644d283SMatt Madia			"the superblock differ: %lld vs. %lld\n", blockCount,
176338b8dc3SIngo Weinhold			superBlock.num_blocks);
177338b8dc3SIngo Weinhold		exit(1);
178338b8dc3SIngo Weinhold	}
1795644d283SMatt Madia
1805644d283SMatt Madia
181338b8dc3SIngo Weinhold	// iterate through the block bitmap blocks and or the bytes with 0x0f
182338b8dc3SIngo Weinhold	uint8 *block = new uint8[blockSize];
183338b8dc3SIngo Weinhold	int64 occupiedBlocks = 0;
184338b8dc3SIngo Weinhold	int64 bitmapByteCount = blockCount / 8;		// round down, we ignore the
185338b8dc3SIngo Weinhold												// last partial byte
186338b8dc3SIngo Weinhold	int64 bitmapBlockCount = (bitmapByteCount + blockSize - 1) / blockSize;
187338b8dc3SIngo Weinhold
188338b8dc3SIngo Weinhold	int bitmapByteIndex = 0;
189338b8dc3SIngo Weinhold	for (int i = 1; i < bitmapBlockCount + 1; i++) {
190338b8dc3SIngo Weinhold		off_t offset = i * blockSize;
191338b8dc3SIngo Weinhold		read_from(fd, offset, block, blockSize);
192338b8dc3SIngo Weinhold
193338b8dc3SIngo Weinhold		int toProcess = blockSize;
194338b8dc3SIngo Weinhold		if (toProcess > bitmapByteCount - bitmapByteIndex)
195338b8dc3SIngo Weinhold			toProcess = (bitmapByteCount - bitmapByteIndex);
196338b8dc3SIngo Weinhold
197338b8dc3SIngo Weinhold		for (int k = 0; k < toProcess; k++) {
198338b8dc3SIngo Weinhold			uint8 patternChar = pattern[bitmapByteIndex % patternLen];
199338b8dc3SIngo Weinhold			occupiedBlocks += count_bits(~block[k] & patternChar);
200338b8dc3SIngo Weinhold			block[k] |= patternChar;
201338b8dc3SIngo Weinhold			bitmapByteIndex++;
202338b8dc3SIngo Weinhold		}
203338b8dc3SIngo Weinhold
204338b8dc3SIngo Weinhold		write_to(fd, offset, block, blockSize);
205338b8dc3SIngo Weinhold	}
206338b8dc3SIngo Weinhold
207338b8dc3SIngo Weinhold	printf("bfs_fragmenter: marked %lld blocks used\n", occupiedBlocks);
208338b8dc3SIngo Weinhold
2095644d283SMatt Madia	// write back the superblock
210338b8dc3SIngo Weinhold	superBlock.used_blocks += occupiedBlocks;
211338b8dc3SIngo Weinhold	write_to(fd, 512, &superBlock, sizeof(superBlock));
2125644d283SMatt Madia
213338b8dc3SIngo Weinhold	return 0;
214338b8dc3SIngo Weinhold}
215338b8dc3SIngo Weinhold
216