blob: acfc002ceba4d81b5861256622d197ec51be6cc5 [file] [log] [blame]
Tobias Waldekranz4f76dd32023-02-16 16:33:49 +01001// SPDX-License-Identifier: GPL-2.0+
2/*
3 * Copyright (c) 2023 Addiva Elektronik
4 * Author: Tobias Waldekranz <tobias@waldekranz.com>
5 */
6
7#include <common.h>
8#include <blk.h>
9#include <blkmap.h>
10#include <dm.h>
11#include <malloc.h>
12#include <mapmem.h>
13#include <part.h>
14#include <dm/device-internal.h>
15#include <dm/lists.h>
16#include <dm/root.h>
17
18struct blkmap;
19
20/**
21 * struct blkmap_slice - Region mapped to a blkmap
22 *
23 * Common data for a region mapped to a blkmap, specialized by each
24 * map type.
25 *
26 * @node: List node used to associate this slice with a blkmap
27 * @blknr: Start block number of the mapping
28 * @blkcnt: Number of blocks covered by this mapping
29 */
30struct blkmap_slice {
31 struct list_head node;
32
33 lbaint_t blknr;
34 lbaint_t blkcnt;
35
36 /**
37 * @read: - Read from slice
38 *
39 * @read.bm: Blkmap to which this slice belongs
40 * @read.bms: This slice
41 * @read.blknr: Start block number to read from
42 * @read.blkcnt: Number of blocks to read
43 * @read.buffer: Buffer to store read data to
44 */
45 ulong (*read)(struct blkmap *bm, struct blkmap_slice *bms,
46 lbaint_t blknr, lbaint_t blkcnt, void *buffer);
47
48 /**
49 * @write: - Write to slice
50 *
51 * @write.bm: Blkmap to which this slice belongs
52 * @write.bms: This slice
53 * @write.blknr: Start block number to write to
54 * @write.blkcnt: Number of blocks to write
55 * @write.buffer: Data to be written
56 */
57 ulong (*write)(struct blkmap *bm, struct blkmap_slice *bms,
58 lbaint_t blknr, lbaint_t blkcnt, const void *buffer);
59
60 /**
61 * @destroy: - Tear down slice
62 *
63 * @read.bm: Blkmap to which this slice belongs
64 * @read.bms: This slice
65 */
66 void (*destroy)(struct blkmap *bm, struct blkmap_slice *bms);
67};
68
69/**
70 * struct blkmap - Block map
71 *
72 * Data associated with a blkmap.
73 *
74 * @label: Human readable name of this blkmap
75 * @blk: Underlying block device
76 * @slices: List of slices associated with this blkmap
77 */
78struct blkmap {
79 char *label;
80 struct udevice *blk;
81 struct list_head slices;
82};
83
84static bool blkmap_slice_contains(struct blkmap_slice *bms, lbaint_t blknr)
85{
86 return (blknr >= bms->blknr) && (blknr < (bms->blknr + bms->blkcnt));
87}
88
89static bool blkmap_slice_available(struct blkmap *bm, struct blkmap_slice *new)
90{
91 struct blkmap_slice *bms;
92 lbaint_t first, last;
93
94 first = new->blknr;
95 last = new->blknr + new->blkcnt - 1;
96
97 list_for_each_entry(bms, &bm->slices, node) {
98 if (blkmap_slice_contains(bms, first) ||
99 blkmap_slice_contains(bms, last) ||
100 blkmap_slice_contains(new, bms->blknr) ||
101 blkmap_slice_contains(new, bms->blknr + bms->blkcnt - 1))
102 return false;
103 }
104
105 return true;
106}
107
108static int blkmap_slice_add(struct blkmap *bm, struct blkmap_slice *new)
109{
110 struct blk_desc *bd = dev_get_uclass_plat(bm->blk);
111 struct list_head *insert = &bm->slices;
112 struct blkmap_slice *bms;
113
114 if (!blkmap_slice_available(bm, new))
115 return -EBUSY;
116
117 list_for_each_entry(bms, &bm->slices, node) {
118 if (bms->blknr < new->blknr)
119 continue;
120
121 insert = &bms->node;
122 break;
123 }
124
125 list_add_tail(&new->node, insert);
126
127 /* Disk might have grown, update the size */
128 bms = list_last_entry(&bm->slices, struct blkmap_slice, node);
129 bd->lba = bms->blknr + bms->blkcnt;
130 return 0;
131}
132
133static ulong blkmap_blk_read_slice(struct blkmap *bm, struct blkmap_slice *bms,
134 lbaint_t blknr, lbaint_t blkcnt,
135 void *buffer)
136{
137 lbaint_t nr, cnt;
138
139 nr = blknr - bms->blknr;
140 cnt = (blkcnt < bms->blkcnt) ? blkcnt : bms->blkcnt;
141 return bms->read(bm, bms, nr, cnt, buffer);
142}
143
144static ulong blkmap_blk_read(struct udevice *dev, lbaint_t blknr,
145 lbaint_t blkcnt, void *buffer)
146{
147 struct blk_desc *bd = dev_get_uclass_plat(dev);
148 struct blkmap *bm = dev_get_plat(dev->parent);
149 struct blkmap_slice *bms;
150 lbaint_t cnt, total = 0;
151
152 list_for_each_entry(bms, &bm->slices, node) {
153 if (!blkmap_slice_contains(bms, blknr))
154 continue;
155
156 cnt = blkmap_blk_read_slice(bm, bms, blknr, blkcnt, buffer);
157 blknr += cnt;
158 blkcnt -= cnt;
159 buffer += cnt << bd->log2blksz;
160 total += cnt;
161 }
162
163 return total;
164}
165
166static ulong blkmap_blk_write_slice(struct blkmap *bm, struct blkmap_slice *bms,
167 lbaint_t blknr, lbaint_t blkcnt,
168 const void *buffer)
169{
170 lbaint_t nr, cnt;
171
172 nr = blknr - bms->blknr;
173 cnt = (blkcnt < bms->blkcnt) ? blkcnt : bms->blkcnt;
174 return bms->write(bm, bms, nr, cnt, buffer);
175}
176
177static ulong blkmap_blk_write(struct udevice *dev, lbaint_t blknr,
178 lbaint_t blkcnt, const void *buffer)
179{
180 struct blk_desc *bd = dev_get_uclass_plat(dev);
181 struct blkmap *bm = dev_get_plat(dev->parent);
182 struct blkmap_slice *bms;
183 lbaint_t cnt, total = 0;
184
185 list_for_each_entry(bms, &bm->slices, node) {
186 if (!blkmap_slice_contains(bms, blknr))
187 continue;
188
189 cnt = blkmap_blk_write_slice(bm, bms, blknr, blkcnt, buffer);
190 blknr += cnt;
191 blkcnt -= cnt;
192 buffer += cnt << bd->log2blksz;
193 total += cnt;
194 }
195
196 return total;
197}
198
199static const struct blk_ops blkmap_blk_ops = {
200 .read = blkmap_blk_read,
201 .write = blkmap_blk_write,
202};
203
204U_BOOT_DRIVER(blkmap_blk) = {
205 .name = "blkmap_blk",
206 .id = UCLASS_BLK,
207 .ops = &blkmap_blk_ops,
208};
209
210int blkmap_dev_bind(struct udevice *dev)
211{
212 struct blkmap *bm = dev_get_plat(dev);
213 struct blk_desc *bd;
214 int err;
215
216 err = blk_create_devicef(dev, "blkmap_blk", "blk", UCLASS_BLKMAP,
217 dev_seq(dev), 512, 0, &bm->blk);
218 if (err)
219 return log_msg_ret("blk", err);
220
221 INIT_LIST_HEAD(&bm->slices);
222
223 bd = dev_get_uclass_plat(bm->blk);
224 snprintf(bd->vendor, BLK_VEN_SIZE, "U-Boot");
225 snprintf(bd->product, BLK_PRD_SIZE, "blkmap");
226 snprintf(bd->revision, BLK_REV_SIZE, "1.0");
227
228 /* EFI core isn't keen on zero-sized disks, so we lie. This is
229 * updated with the correct size once the user adds a
230 * mapping.
231 */
232 bd->lba = 1;
233
234 return 0;
235}
236
237int blkmap_dev_unbind(struct udevice *dev)
238{
239 struct blkmap *bm = dev_get_plat(dev);
240 struct blkmap_slice *bms, *tmp;
241 int err;
242
243 list_for_each_entry_safe(bms, tmp, &bm->slices, node) {
244 list_del(&bms->node);
245 free(bms);
246 }
247
248 err = device_remove(bm->blk, DM_REMOVE_NORMAL);
249 if (err)
250 return err;
251
252 return device_unbind(bm->blk);
253}
254
255U_BOOT_DRIVER(blkmap_root) = {
256 .name = "blkmap_dev",
257 .id = UCLASS_BLKMAP,
258 .bind = blkmap_dev_bind,
259 .unbind = blkmap_dev_unbind,
260 .plat_auto = sizeof(struct blkmap),
261};
262
263struct udevice *blkmap_from_label(const char *label)
264{
265 struct udevice *dev;
266 struct uclass *uc;
267 struct blkmap *bm;
268
269 uclass_id_foreach_dev(UCLASS_BLKMAP, dev, uc) {
270 bm = dev_get_plat(dev);
271 if (bm->label && !strcmp(label, bm->label))
272 return dev;
273 }
274
275 return NULL;
276}
277
278int blkmap_create(const char *label, struct udevice **devp)
279{
280 char *hname, *hlabel;
281 struct udevice *dev;
282 struct blkmap *bm;
283 size_t namelen;
284 int err;
285
286 dev = blkmap_from_label(label);
287 if (dev) {
288 err = -EBUSY;
289 goto err;
290 }
291
292 hlabel = strdup(label);
293 if (!hlabel) {
294 err = -ENOMEM;
295 goto err;
296 }
297
298 namelen = strlen("blkmap-") + strlen(label) + 1;
299 hname = malloc(namelen);
300 if (!hname) {
301 err = -ENOMEM;
302 goto err_free_hlabel;
303 }
304
305 strlcpy(hname, "blkmap-", namelen);
306 strlcat(hname, label, namelen);
307
308 err = device_bind_driver(dm_root(), "blkmap_dev", hname, &dev);
309 if (err)
310 goto err_free_hname;
311
312 device_set_name_alloced(dev);
313 bm = dev_get_plat(dev);
314 bm->label = hlabel;
315
316 if (devp)
317 *devp = dev;
318
319 return 0;
320
321err_free_hname:
322 free(hname);
323err_free_hlabel:
324 free(hlabel);
325err:
326 return err;
327}
328
329int blkmap_destroy(struct udevice *dev)
330{
331 int err;
332
333 err = device_remove(dev, DM_REMOVE_NORMAL);
334 if (err)
335 return err;
336
337 return device_unbind(dev);
338}
339
340UCLASS_DRIVER(blkmap) = {
341 .id = UCLASS_BLKMAP,
342 .name = "blkmap",
343};