1
2
3
4
5
6
7
8
9
10
11#include <linux/device-mapper.h>
12#include <linux/module.h>
13#include <linux/rbtree.h>
14
15#define DM_MSG_PREFIX "dust"
16
17struct badblock {
18 struct rb_node node;
19 sector_t bb;
20 unsigned char wr_fail_cnt;
21};
22
23struct dust_device {
24 struct dm_dev *dev;
25 struct rb_root badblocklist;
26 unsigned long long badblock_count;
27 spinlock_t dust_lock;
28 unsigned int blksz;
29 int sect_per_block_shift;
30 unsigned int sect_per_block;
31 sector_t start;
32 bool fail_read_on_bb:1;
33 bool quiet_mode:1;
34};
35
36static struct badblock *dust_rb_search(struct rb_root *root, sector_t blk)
37{
38 struct rb_node *node = root->rb_node;
39
40 while (node) {
41 struct badblock *bblk = rb_entry(node, struct badblock, node);
42
43 if (bblk->bb > blk)
44 node = node->rb_left;
45 else if (bblk->bb < blk)
46 node = node->rb_right;
47 else
48 return bblk;
49 }
50
51 return NULL;
52}
53
54static bool dust_rb_insert(struct rb_root *root, struct badblock *new)
55{
56 struct badblock *bblk;
57 struct rb_node **link = &root->rb_node, *parent = NULL;
58 sector_t value = new->bb;
59
60 while (*link) {
61 parent = *link;
62 bblk = rb_entry(parent, struct badblock, node);
63
64 if (bblk->bb > value)
65 link = &(*link)->rb_left;
66 else if (bblk->bb < value)
67 link = &(*link)->rb_right;
68 else
69 return false;
70 }
71
72 rb_link_node(&new->node, parent, link);
73 rb_insert_color(&new->node, root);
74
75 return true;
76}
77
78static int dust_remove_block(struct dust_device *dd, unsigned long long block)
79{
80 struct badblock *bblock;
81 unsigned long flags;
82
83 spin_lock_irqsave(&dd->dust_lock, flags);
84 bblock = dust_rb_search(&dd->badblocklist, block);
85
86 if (bblock == NULL) {
87 if (!dd->quiet_mode) {
88 DMERR("%s: block %llu not found in badblocklist",
89 __func__, block);
90 }
91 spin_unlock_irqrestore(&dd->dust_lock, flags);
92 return -EINVAL;
93 }
94
95 rb_erase(&bblock->node, &dd->badblocklist);
96 dd->badblock_count--;
97 if (!dd->quiet_mode)
98 DMINFO("%s: badblock removed at block %llu", __func__, block);
99 kfree(bblock);
100 spin_unlock_irqrestore(&dd->dust_lock, flags);
101
102 return 0;
103}
104
105static int dust_add_block(struct dust_device *dd, unsigned long long block,
106 unsigned char wr_fail_cnt)
107{
108 struct badblock *bblock;
109 unsigned long flags;
110
111 bblock = kmalloc(sizeof(*bblock), GFP_KERNEL);
112 if (bblock == NULL) {
113 if (!dd->quiet_mode)
114 DMERR("%s: badblock allocation failed", __func__);
115 return -ENOMEM;
116 }
117
118 spin_lock_irqsave(&dd->dust_lock, flags);
119 bblock->bb = block;
120 bblock->wr_fail_cnt = wr_fail_cnt;
121 if (!dust_rb_insert(&dd->badblocklist, bblock)) {
122 if (!dd->quiet_mode) {
123 DMERR("%s: block %llu already in badblocklist",
124 __func__, block);
125 }
126 spin_unlock_irqrestore(&dd->dust_lock, flags);
127 kfree(bblock);
128 return -EINVAL;
129 }
130
131 dd->badblock_count++;
132 if (!dd->quiet_mode) {
133 DMINFO("%s: badblock added at block %llu with write fail count %hhu",
134 __func__, block, wr_fail_cnt);
135 }
136 spin_unlock_irqrestore(&dd->dust_lock, flags);
137
138 return 0;
139}
140
141static int dust_query_block(struct dust_device *dd, unsigned long long block)
142{
143 struct badblock *bblock;
144 unsigned long flags;
145
146 spin_lock_irqsave(&dd->dust_lock, flags);
147 bblock = dust_rb_search(&dd->badblocklist, block);
148 if (bblock != NULL)
149 DMINFO("%s: block %llu found in badblocklist", __func__, block);
150 else
151 DMINFO("%s: block %llu not found in badblocklist", __func__, block);
152 spin_unlock_irqrestore(&dd->dust_lock, flags);
153
154 return 0;
155}
156
157static int __dust_map_read(struct dust_device *dd, sector_t thisblock)
158{
159 struct badblock *bblk = dust_rb_search(&dd->badblocklist, thisblock);
160
161 if (bblk)
162 return DM_MAPIO_KILL;
163
164 return DM_MAPIO_REMAPPED;
165}
166
167static int dust_map_read(struct dust_device *dd, sector_t thisblock,
168 bool fail_read_on_bb)
169{
170 unsigned long flags;
171 int r = DM_MAPIO_REMAPPED;
172
173 if (fail_read_on_bb) {
174 thisblock >>= dd->sect_per_block_shift;
175 spin_lock_irqsave(&dd->dust_lock, flags);
176 r = __dust_map_read(dd, thisblock);
177 spin_unlock_irqrestore(&dd->dust_lock, flags);
178 }
179
180 return r;
181}
182
183static int __dust_map_write(struct dust_device *dd, sector_t thisblock)
184{
185 struct badblock *bblk = dust_rb_search(&dd->badblocklist, thisblock);
186
187 if (bblk && bblk->wr_fail_cnt > 0) {
188 bblk->wr_fail_cnt--;
189 return DM_MAPIO_KILL;
190 }
191
192 if (bblk) {
193 rb_erase(&bblk->node, &dd->badblocklist);
194 dd->badblock_count--;
195 kfree(bblk);
196 if (!dd->quiet_mode) {
197 sector_div(thisblock, dd->sect_per_block);
198 DMINFO("block %llu removed from badblocklist by write",
199 (unsigned long long)thisblock);
200 }
201 }
202
203 return DM_MAPIO_REMAPPED;
204}
205
206static int dust_map_write(struct dust_device *dd, sector_t thisblock,
207 bool fail_read_on_bb)
208{
209 unsigned long flags;
210 int r = DM_MAPIO_REMAPPED;
211
212 if (fail_read_on_bb) {
213 thisblock >>= dd->sect_per_block_shift;
214 spin_lock_irqsave(&dd->dust_lock, flags);
215 r = __dust_map_write(dd, thisblock);
216 spin_unlock_irqrestore(&dd->dust_lock, flags);
217 }
218
219 return r;
220}
221
222static int dust_map(struct dm_target *ti, struct bio *bio)
223{
224 struct dust_device *dd = ti->private;
225 int r;
226
227 bio_set_dev(bio, dd->dev->bdev);
228 bio->bi_iter.bi_sector = dd->start + dm_target_offset(ti, bio->bi_iter.bi_sector);
229
230 if (bio_data_dir(bio) == READ)
231 r = dust_map_read(dd, bio->bi_iter.bi_sector, dd->fail_read_on_bb);
232 else
233 r = dust_map_write(dd, bio->bi_iter.bi_sector, dd->fail_read_on_bb);
234
235 return r;
236}
237
238static bool __dust_clear_badblocks(struct rb_root *tree,
239 unsigned long long count)
240{
241 struct rb_node *node = NULL, *nnode = NULL;
242
243 nnode = rb_first(tree);
244 if (nnode == NULL) {
245 BUG_ON(count != 0);
246 return false;
247 }
248
249 while (nnode) {
250 node = nnode;
251 nnode = rb_next(node);
252 rb_erase(node, tree);
253 count--;
254 kfree(node);
255 }
256 BUG_ON(count != 0);
257 BUG_ON(tree->rb_node != NULL);
258
259 return true;
260}
261
262static int dust_clear_badblocks(struct dust_device *dd)
263{
264 unsigned long flags;
265 struct rb_root badblocklist;
266 unsigned long long badblock_count;
267
268 spin_lock_irqsave(&dd->dust_lock, flags);
269 badblocklist = dd->badblocklist;
270 badblock_count = dd->badblock_count;
271 dd->badblocklist = RB_ROOT;
272 dd->badblock_count = 0;
273 spin_unlock_irqrestore(&dd->dust_lock, flags);
274
275 if (!__dust_clear_badblocks(&badblocklist, badblock_count))
276 DMINFO("%s: no badblocks found", __func__);
277 else
278 DMINFO("%s: badblocks cleared", __func__);
279
280 return 0;
281}
282
283
284
285
286
287
288
289
290
291
292static int dust_ctr(struct dm_target *ti, unsigned int argc, char **argv)
293{
294 struct dust_device *dd;
295 unsigned long long tmp;
296 char dummy;
297 unsigned int blksz;
298 unsigned int sect_per_block;
299 sector_t DUST_MAX_BLKSZ_SECTORS = 2097152;
300 sector_t max_block_sectors = min(ti->len, DUST_MAX_BLKSZ_SECTORS);
301
302 if (argc != 3) {
303 ti->error = "Invalid argument count";
304 return -EINVAL;
305 }
306
307 if (kstrtouint(argv[2], 10, &blksz) || !blksz) {
308 ti->error = "Invalid block size parameter";
309 return -EINVAL;
310 }
311
312 if (blksz < 512) {
313 ti->error = "Block size must be at least 512";
314 return -EINVAL;
315 }
316
317 if (!is_power_of_2(blksz)) {
318 ti->error = "Block size must be a power of 2";
319 return -EINVAL;
320 }
321
322 if (to_sector(blksz) > max_block_sectors) {
323 ti->error = "Block size is too large";
324 return -EINVAL;
325 }
326
327 sect_per_block = (blksz >> SECTOR_SHIFT);
328
329 if (sscanf(argv[1], "%llu%c", &tmp, &dummy) != 1 || tmp != (sector_t)tmp) {
330 ti->error = "Invalid device offset sector";
331 return -EINVAL;
332 }
333
334 dd = kzalloc(sizeof(struct dust_device), GFP_KERNEL);
335 if (dd == NULL) {
336 ti->error = "Cannot allocate context";
337 return -ENOMEM;
338 }
339
340 if (dm_get_device(ti, argv[0], dm_table_get_mode(ti->table), &dd->dev)) {
341 ti->error = "Device lookup failed";
342 kfree(dd);
343 return -EINVAL;
344 }
345
346 dd->sect_per_block = sect_per_block;
347 dd->blksz = blksz;
348 dd->start = tmp;
349
350 dd->sect_per_block_shift = __ffs(sect_per_block);
351
352
353
354
355
356 dd->fail_read_on_bb = false;
357
358
359
360
361 dd->badblocklist = RB_ROOT;
362 dd->badblock_count = 0;
363 spin_lock_init(&dd->dust_lock);
364
365 dd->quiet_mode = false;
366
367 BUG_ON(dm_set_target_max_io_len(ti, dd->sect_per_block) != 0);
368
369 ti->num_discard_bios = 1;
370 ti->num_flush_bios = 1;
371 ti->private = dd;
372
373 return 0;
374}
375
376static void dust_dtr(struct dm_target *ti)
377{
378 struct dust_device *dd = ti->private;
379
380 __dust_clear_badblocks(&dd->badblocklist, dd->badblock_count);
381 dm_put_device(ti, dd->dev);
382 kfree(dd);
383}
384
385static int dust_message(struct dm_target *ti, unsigned int argc, char **argv,
386 char *result_buf, unsigned int maxlen)
387{
388 struct dust_device *dd = ti->private;
389 sector_t size = i_size_read(dd->dev->bdev->bd_inode) >> SECTOR_SHIFT;
390 bool invalid_msg = false;
391 int r = -EINVAL;
392 unsigned long long tmp, block;
393 unsigned char wr_fail_cnt;
394 unsigned int tmp_ui;
395 unsigned long flags;
396 char dummy;
397
398 if (argc == 1) {
399 if (!strcasecmp(argv[0], "addbadblock") ||
400 !strcasecmp(argv[0], "removebadblock") ||
401 !strcasecmp(argv[0], "queryblock")) {
402 DMERR("%s requires an additional argument", argv[0]);
403 } else if (!strcasecmp(argv[0], "disable")) {
404 DMINFO("disabling read failures on bad sectors");
405 dd->fail_read_on_bb = false;
406 r = 0;
407 } else if (!strcasecmp(argv[0], "enable")) {
408 DMINFO("enabling read failures on bad sectors");
409 dd->fail_read_on_bb = true;
410 r = 0;
411 } else if (!strcasecmp(argv[0], "countbadblocks")) {
412 spin_lock_irqsave(&dd->dust_lock, flags);
413 DMINFO("countbadblocks: %llu badblock(s) found",
414 dd->badblock_count);
415 spin_unlock_irqrestore(&dd->dust_lock, flags);
416 r = 0;
417 } else if (!strcasecmp(argv[0], "clearbadblocks")) {
418 r = dust_clear_badblocks(dd);
419 } else if (!strcasecmp(argv[0], "quiet")) {
420 if (!dd->quiet_mode)
421 dd->quiet_mode = true;
422 else
423 dd->quiet_mode = false;
424 r = 0;
425 } else {
426 invalid_msg = true;
427 }
428 } else if (argc == 2) {
429 if (sscanf(argv[1], "%llu%c", &tmp, &dummy) != 1)
430 return r;
431
432 block = tmp;
433 sector_div(size, dd->sect_per_block);
434 if (block > size) {
435 DMERR("selected block value out of range");
436 return r;
437 }
438
439 if (!strcasecmp(argv[0], "addbadblock"))
440 r = dust_add_block(dd, block, 0);
441 else if (!strcasecmp(argv[0], "removebadblock"))
442 r = dust_remove_block(dd, block);
443 else if (!strcasecmp(argv[0], "queryblock"))
444 r = dust_query_block(dd, block);
445 else
446 invalid_msg = true;
447
448 } else if (argc == 3) {
449 if (sscanf(argv[1], "%llu%c", &tmp, &dummy) != 1)
450 return r;
451
452 if (sscanf(argv[2], "%u%c", &tmp_ui, &dummy) != 1)
453 return r;
454
455 block = tmp;
456 if (tmp_ui > 255) {
457 DMERR("selected write fail count out of range");
458 return r;
459 }
460 wr_fail_cnt = tmp_ui;
461 sector_div(size, dd->sect_per_block);
462 if (block > size) {
463 DMERR("selected block value out of range");
464 return r;
465 }
466
467 if (!strcasecmp(argv[0], "addbadblock"))
468 r = dust_add_block(dd, block, wr_fail_cnt);
469 else
470 invalid_msg = true;
471
472 } else
473 DMERR("invalid number of arguments '%d'", argc);
474
475 if (invalid_msg)
476 DMERR("unrecognized message '%s' received", argv[0]);
477
478 return r;
479}
480
481static void dust_status(struct dm_target *ti, status_type_t type,
482 unsigned int status_flags, char *result, unsigned int maxlen)
483{
484 struct dust_device *dd = ti->private;
485 unsigned int sz = 0;
486
487 switch (type) {
488 case STATUSTYPE_INFO:
489 DMEMIT("%s %s %s", dd->dev->name,
490 dd->fail_read_on_bb ? "fail_read_on_bad_block" : "bypass",
491 dd->quiet_mode ? "quiet" : "verbose");
492 break;
493
494 case STATUSTYPE_TABLE:
495 DMEMIT("%s %llu %u", dd->dev->name,
496 (unsigned long long)dd->start, dd->blksz);
497 break;
498 }
499}
500
501static int dust_prepare_ioctl(struct dm_target *ti, struct block_device **bdev)
502{
503 struct dust_device *dd = ti->private;
504 struct dm_dev *dev = dd->dev;
505
506 *bdev = dev->bdev;
507
508
509
510
511 if (dd->start ||
512 ti->len != i_size_read(dev->bdev->bd_inode) >> SECTOR_SHIFT)
513 return 1;
514
515 return 0;
516}
517
518static int dust_iterate_devices(struct dm_target *ti, iterate_devices_callout_fn fn,
519 void *data)
520{
521 struct dust_device *dd = ti->private;
522
523 return fn(ti, dd->dev, dd->start, ti->len, data);
524}
525
526static struct target_type dust_target = {
527 .name = "dust",
528 .version = {1, 0, 0},
529 .module = THIS_MODULE,
530 .ctr = dust_ctr,
531 .dtr = dust_dtr,
532 .iterate_devices = dust_iterate_devices,
533 .map = dust_map,
534 .message = dust_message,
535 .status = dust_status,
536 .prepare_ioctl = dust_prepare_ioctl,
537};
538
539static int __init dm_dust_init(void)
540{
541 int r = dm_register_target(&dust_target);
542
543 if (r < 0)
544 DMERR("dm_register_target failed %d", r);
545
546 return r;
547}
548
549static void __exit dm_dust_exit(void)
550{
551 dm_unregister_target(&dust_target);
552}
553
554module_init(dm_dust_init);
555module_exit(dm_dust_exit);
556
557MODULE_DESCRIPTION(DM_NAME " dust test target");
558MODULE_AUTHOR("Bryan Gurney <dm-devel@redhat.com>");
559MODULE_LICENSE("GPL");
560
