Merge branch 'for-5.7/logitech' into for-linus
[linux-2.6-microblaze.git] / drivers / md / dm-dust.c
1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright (c) 2018 Red Hat, Inc.
4  *
5  * This is a test "dust" device, which fails reads on specified
6  * sectors, emulating the behavior of a hard disk drive sending
7  * a "Read Medium Error" sense.
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
17 struct badblock {
18         struct rb_node node;
19         sector_t bb;
20         unsigned char wr_fail_cnt;
21 };
22
23 struct 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
36 static 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
54 static 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
78 static 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
105 static 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
141 static 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
157 static 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
167 static 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
183 static 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
206 static 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
222 static 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
238 static 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
262 static 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  * Target parameters:
285  *
286  * <device_path> <offset> <blksz>
287  *
288  * device_path: path to the block device
289  * offset: offset to data area from start of device_path
290  * blksz: block size (minimum 512, maximum 1073741824, must be a power of 2)
291  */
292 static 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          * Whether to fail a read on a "bad" block.
354          * Defaults to false; enabled later by message.
355          */
356         dd->fail_read_on_bb = false;
357
358         /*
359          * Initialize bad block list rbtree.
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
376 static 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
385 static 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
481 static 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
501 static 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          * Only pass ioctls through if the device sizes match exactly.
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
518 static 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
526 static 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
539 static 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
549 static void __exit dm_dust_exit(void)
550 {
551         dm_unregister_target(&dust_target);
552 }
553
554 module_init(dm_dust_init);
555 module_exit(dm_dust_exit);
556
557 MODULE_DESCRIPTION(DM_NAME " dust test target");
558 MODULE_AUTHOR("Bryan Gurney <dm-devel@redhat.com>");
559 MODULE_LICENSE("GPL");