1 | /* |
---|
2 | * nvdisk.c -- Non-volatile disk block device implementation |
---|
3 | * |
---|
4 | * Copyright (C) 2007 Chris Johns |
---|
5 | * |
---|
6 | * The license and distribution terms for this file may be |
---|
7 | * found in the file LICENSE in this distribution or at |
---|
8 | * http://www.rtems.com/license/LICENSE. |
---|
9 | * |
---|
10 | * $Id$ |
---|
11 | */ |
---|
12 | |
---|
13 | #if HAVE_CONFIG_H |
---|
14 | #include "config.h" |
---|
15 | #endif |
---|
16 | |
---|
17 | #include <rtems.h> |
---|
18 | #include <rtems/libio.h> |
---|
19 | #include <errno.h> |
---|
20 | #include <stdlib.h> |
---|
21 | #include <stdio.h> |
---|
22 | #include <string.h> |
---|
23 | #include <inttypes.h> |
---|
24 | |
---|
25 | #include "rtems/blkdev.h" |
---|
26 | #include "rtems/diskdevs.h" |
---|
27 | #include "rtems/nvdisk.h" |
---|
28 | |
---|
29 | /** |
---|
30 | * @note |
---|
31 | * |
---|
32 | * The use of pages can vary. The rtems_nvdisk_*_page set |
---|
33 | * routines use an absolute page number relative to the segment |
---|
34 | * while all other page numbera are relative to the number of |
---|
35 | * page descriptor pages a segment has. You need to add the |
---|
36 | * number of page descriptor pages (pages_desc) to the page number |
---|
37 | * when call the rtems_nvdisk_*_page functions. |
---|
38 | * |
---|
39 | * You must always show the page number as relative in any trace |
---|
40 | * or error message as device-page and if you have to |
---|
41 | * the page number as absolute use device~page. This |
---|
42 | * can be seen in the page copy routine. |
---|
43 | * |
---|
44 | * The code is like this to avoid needing the pass the pages_desc |
---|
45 | * value around. It is only used in selected places and so the |
---|
46 | * extra parameter was avoided. |
---|
47 | */ |
---|
48 | |
---|
49 | /** |
---|
50 | * Control tracing. It can be compiled out of the code for small |
---|
51 | * footprint targets. Leave in by default. |
---|
52 | */ |
---|
53 | #if !defined (RTEMS_NVDISK_TRACE) |
---|
54 | #define RTEMS_NVDISK_TRACE 0 |
---|
55 | #endif |
---|
56 | |
---|
57 | /** |
---|
58 | * NV Device Control holds the segment controls |
---|
59 | */ |
---|
60 | typedef struct rtems_nvdisk_device_ctl |
---|
61 | { |
---|
62 | /** |
---|
63 | * The device this segment resides on. |
---|
64 | */ |
---|
65 | uint32_t device; |
---|
66 | |
---|
67 | /** |
---|
68 | * Total number of pages in the device. |
---|
69 | */ |
---|
70 | uint32_t pages; |
---|
71 | |
---|
72 | /** |
---|
73 | * Number of pages used for page checksums. |
---|
74 | */ |
---|
75 | uint32_t pages_desc; |
---|
76 | |
---|
77 | /** |
---|
78 | * First block number for this device. |
---|
79 | */ |
---|
80 | uint32_t block_base; |
---|
81 | |
---|
82 | /** |
---|
83 | * Device descriptor. |
---|
84 | */ |
---|
85 | const rtems_nvdisk_device_desc* descriptor; |
---|
86 | } rtems_nvdisk_device_ctl; |
---|
87 | |
---|
88 | /** |
---|
89 | * The NV disk control structure for a single disk. There is one |
---|
90 | * for each minor disk in the system. |
---|
91 | */ |
---|
92 | typedef struct rtems_mvdisk |
---|
93 | { |
---|
94 | rtems_device_major_number major; /**< The driver's major number. */ |
---|
95 | rtems_device_minor_number minor; /**< The driver's minor number. */ |
---|
96 | uint32_t flags; /**< configuration flags. */ |
---|
97 | uint32_t block_size; /**< The block size for this disk. */ |
---|
98 | uint32_t block_count; /**< The number of available blocks. */ |
---|
99 | rtems_nvdisk_device_ctl* devices; /**< The NV devices for this disk. */ |
---|
100 | uint32_t device_count; /**< The number of NV devices. */ |
---|
101 | uint32_t cs_pages; /**< The num of pages of checksums. */ |
---|
102 | rtems_id lock; /**< Mutex for threading protection.*/ |
---|
103 | uint32_t info_level; /**< The info trace level. */ |
---|
104 | } rtems_nvdisk; |
---|
105 | |
---|
106 | /** |
---|
107 | * The array of NV disks we support. |
---|
108 | */ |
---|
109 | static rtems_nvdisk* rtems_nvdisks; |
---|
110 | |
---|
111 | /** |
---|
112 | * The number of NV disks we have. |
---|
113 | */ |
---|
114 | static uint32_t rtems_nvdisk_count; |
---|
115 | |
---|
116 | /** |
---|
117 | * The CRC16 factor table. Created during initialisation. |
---|
118 | */ |
---|
119 | static uint16_t* rtems_nvdisk_crc16_factor; |
---|
120 | |
---|
121 | /** |
---|
122 | * Calculate the CRC16 checksum. |
---|
123 | * |
---|
124 | * @param _b The byte to checksum. |
---|
125 | * @param _c The current checksum. |
---|
126 | */ |
---|
127 | #define rtems_nvdisk_calc_crc16(_b, _c) \ |
---|
128 | rtems_nvdisk_crc16_factor[((_b) ^ ((_c) & 0xff)) & 0xff] ^ (((_c) >> 8) & 0xff) |
---|
129 | |
---|
130 | /** |
---|
131 | * Generate the CRC table. |
---|
132 | * |
---|
133 | * @param pattern The seed pattern for the table of factors. |
---|
134 | * @relval RTEMS_SUCCESSFUL The table was generated. |
---|
135 | * @retval RTEMS_NO_MEMORY The table could not be allocated from the heap. |
---|
136 | */ |
---|
137 | rtems_status_code |
---|
138 | rtems_nvdisk_crc16_gen_factors (uint16_t pattern) |
---|
139 | { |
---|
140 | uint32_t b; |
---|
141 | |
---|
142 | rtems_nvdisk_crc16_factor = malloc (sizeof (uint16_t) * 256); |
---|
143 | if (!rtems_nvdisk_crc16_factor) |
---|
144 | return RTEMS_NO_MEMORY; |
---|
145 | |
---|
146 | for (b = 0; b < 256; b++) |
---|
147 | { |
---|
148 | uint32_t i; |
---|
149 | uint16_t v = b; |
---|
150 | for (i = 8; i--;) |
---|
151 | v = v & 1 ? (v >> 1) ^ pattern : v >> 1; |
---|
152 | rtems_nvdisk_crc16_factor[b] = v & 0xffff; |
---|
153 | } |
---|
154 | return RTEMS_SUCCESSFUL; |
---|
155 | } |
---|
156 | |
---|
157 | #if RTEMS_NVDISK_TRACE |
---|
158 | /** |
---|
159 | * Print a message to the nvdisk output and flush it. |
---|
160 | * |
---|
161 | * @param nvd The nvdisk control structure. |
---|
162 | * @param format The format string. See printf for details. |
---|
163 | * @param ... The arguments for the format text. |
---|
164 | * @return int The number of bytes written to the output. |
---|
165 | */ |
---|
166 | static int |
---|
167 | rtems_nvdisk_printf (const rtems_nvdisk* nvd, const char *format, ...) |
---|
168 | { |
---|
169 | int ret = 0; |
---|
170 | if (nvd->info_level >= 3) |
---|
171 | { |
---|
172 | va_list args; |
---|
173 | va_start (args, format); |
---|
174 | fprintf (stdout, "nvdisk:"); |
---|
175 | ret = vfprintf (stdout, format, args); |
---|
176 | fprintf (stdout, "\n"); |
---|
177 | fflush (stdout); |
---|
178 | } |
---|
179 | return ret; |
---|
180 | } |
---|
181 | |
---|
182 | /** |
---|
183 | * Print a info message to the nvdisk output and flush it. |
---|
184 | * |
---|
185 | * @param nvd The nvdisk control structure. |
---|
186 | * @param format The format string. See printf for details. |
---|
187 | * @param ... The arguments for the format text. |
---|
188 | * @return int The number of bytes written to the output. |
---|
189 | */ |
---|
190 | static int |
---|
191 | rtems_nvdisk_info (const rtems_nvdisk* nvd, const char *format, ...) |
---|
192 | { |
---|
193 | int ret = 0; |
---|
194 | if (nvd->info_level >= 2) |
---|
195 | { |
---|
196 | va_list args; |
---|
197 | va_start (args, format); |
---|
198 | fprintf (stdout, "nvdisk:"); |
---|
199 | ret = vfprintf (stdout, format, args); |
---|
200 | fprintf (stdout, "\n"); |
---|
201 | fflush (stdout); |
---|
202 | } |
---|
203 | return ret; |
---|
204 | } |
---|
205 | |
---|
206 | /** |
---|
207 | * Print a warning to the nvdisk output and flush it. |
---|
208 | * |
---|
209 | * @param nvd The nvdisk control structure. |
---|
210 | * @param format The format string. See printf for details. |
---|
211 | * @param ... The arguments for the format text. |
---|
212 | * @return int The number of bytes written to the output. |
---|
213 | */ |
---|
214 | static int |
---|
215 | rtems_nvdisk_warning (const rtems_nvdisk* nvd, const char *format, ...) |
---|
216 | { |
---|
217 | int ret = 0; |
---|
218 | if (nvd->info_level >= 1) |
---|
219 | { |
---|
220 | va_list args; |
---|
221 | va_start (args, format); |
---|
222 | fprintf (stdout, "nvdisk:warning:"); |
---|
223 | ret = vfprintf (stdout, format, args); |
---|
224 | fprintf (stdout, "\n"); |
---|
225 | fflush (stdout); |
---|
226 | } |
---|
227 | return ret; |
---|
228 | } |
---|
229 | #endif |
---|
230 | |
---|
231 | /** |
---|
232 | * Print an error to the nvdisk output and flush it. |
---|
233 | * |
---|
234 | * @param format The format string. See printf for details. |
---|
235 | * @param ... The arguments for the format text. |
---|
236 | * @return int The number of bytes written to the output. |
---|
237 | */ |
---|
238 | static int |
---|
239 | rtems_nvdisk_error (const char *format, ...) |
---|
240 | { |
---|
241 | int ret; |
---|
242 | va_list args; |
---|
243 | va_start (args, format); |
---|
244 | fprintf (stderr, "nvdisk:error:"); |
---|
245 | ret = vfprintf (stderr, format, args); |
---|
246 | fprintf (stderr, "\n"); |
---|
247 | fflush (stderr); |
---|
248 | return ret; |
---|
249 | } |
---|
250 | |
---|
251 | /** |
---|
252 | * Get the descriptor for a device. |
---|
253 | */ |
---|
254 | static const rtems_nvdisk_device_desc* |
---|
255 | rtems_nvdisk_device_descriptor (const rtems_nvdisk* nvd, uint32_t device) |
---|
256 | { |
---|
257 | return nvd->devices[device].descriptor; |
---|
258 | } |
---|
259 | |
---|
260 | /** |
---|
261 | * Read a block of data from a device. |
---|
262 | */ |
---|
263 | static int |
---|
264 | rtems_nvdisk_device_read (const rtems_nvdisk* nvd, |
---|
265 | uint32_t device, |
---|
266 | uint32_t offset, |
---|
267 | void* buffer, |
---|
268 | uint32_t size) |
---|
269 | { |
---|
270 | const rtems_nvdisk_device_desc* dd; |
---|
271 | const rtems_nvdisk_driver_handlers* ops; |
---|
272 | dd = rtems_nvdisk_device_descriptor (nvd, device); |
---|
273 | ops = nvd->devices[device].descriptor->nv_ops; |
---|
274 | #if RTEMS_NVDISK_TRACE |
---|
275 | rtems_nvdisk_printf (nvd, " dev-read: %02d-%08x: s=%d", |
---|
276 | device, offset, size); |
---|
277 | #endif |
---|
278 | return ops->read (device, dd->flags, dd->base, offset, buffer, size); |
---|
279 | } |
---|
280 | |
---|
281 | /** |
---|
282 | * Write a block of data to a device. |
---|
283 | */ |
---|
284 | static int |
---|
285 | rtems_nvdisk_device_write (const rtems_nvdisk* nvd, |
---|
286 | uint32_t device, |
---|
287 | uint32_t offset, |
---|
288 | const void* buffer, |
---|
289 | uint32_t size) |
---|
290 | { |
---|
291 | const rtems_nvdisk_device_desc* dd; |
---|
292 | const rtems_nvdisk_driver_handlers* ops; |
---|
293 | dd = rtems_nvdisk_device_descriptor (nvd, device); |
---|
294 | ops = nvd->devices[device].descriptor->nv_ops; |
---|
295 | #if RTEMS_NVDISK_TRACE |
---|
296 | rtems_nvdisk_printf (nvd, " dev-write: %02d-%08x: s=%d", |
---|
297 | device, offset, size); |
---|
298 | #endif |
---|
299 | return ops->write (device, dd->flags, dd->base, offset, buffer, size); |
---|
300 | } |
---|
301 | |
---|
302 | #if NOT_USED |
---|
303 | /** |
---|
304 | * Verify the data with the data in a segment. |
---|
305 | */ |
---|
306 | static int |
---|
307 | rtems_nvdisk_device_verify (const rtems_nvdisk* nvd, |
---|
308 | uint32_t device, |
---|
309 | uint32_t offset, |
---|
310 | const void* buffer, |
---|
311 | uint32_t size) |
---|
312 | { |
---|
313 | const rtems_nvdisk_device_desc* dd; |
---|
314 | const rtems_nvdisk_driver_handlers* ops; |
---|
315 | dd = rtems_nvdisk_device_descriptor (nvd, device); |
---|
316 | ops = nvd->devices[device].descriptor->nv_ops; |
---|
317 | #if RTEMS_NVDISK_TRACE |
---|
318 | rtems_nvdisk_printf (nvd, " seg-verify: %02d-%08x: s=%d", |
---|
319 | device, offset, size); |
---|
320 | #endif |
---|
321 | return ops->verify (device, dd->flags, dd->base, offset, buffer, size); |
---|
322 | } |
---|
323 | #endif |
---|
324 | |
---|
325 | /** |
---|
326 | * Read a page of data from the device. |
---|
327 | */ |
---|
328 | static int |
---|
329 | rtems_nvdisk_read_page (const rtems_nvdisk* nvd, |
---|
330 | uint32_t device, |
---|
331 | uint32_t page, |
---|
332 | void* buffer) |
---|
333 | { |
---|
334 | return rtems_nvdisk_device_read (nvd, device, |
---|
335 | page * nvd->block_size, buffer, |
---|
336 | nvd->block_size); |
---|
337 | } |
---|
338 | |
---|
339 | /** |
---|
340 | * Write a page of data to a device. |
---|
341 | */ |
---|
342 | static int |
---|
343 | rtems_nvdisk_write_page (const rtems_nvdisk* nvd, |
---|
344 | uint32_t device, |
---|
345 | uint32_t page, |
---|
346 | const void* buffer) |
---|
347 | { |
---|
348 | return rtems_nvdisk_device_write (nvd, device, |
---|
349 | page * nvd->block_size, |
---|
350 | buffer, nvd->block_size); |
---|
351 | } |
---|
352 | |
---|
353 | /** |
---|
354 | * Read the checksum from the device. |
---|
355 | */ |
---|
356 | static int |
---|
357 | rtems_nvdisk_read_checksum (const rtems_nvdisk* nvd, |
---|
358 | uint32_t device, |
---|
359 | uint32_t page, |
---|
360 | uint16_t* cs) |
---|
361 | { |
---|
362 | return rtems_nvdisk_device_read (nvd, device, |
---|
363 | page * sizeof (uint16_t), |
---|
364 | cs, sizeof (uint16_t)); |
---|
365 | } |
---|
366 | |
---|
367 | /** |
---|
368 | * Write the checksum to the device. |
---|
369 | */ |
---|
370 | static int |
---|
371 | rtems_nvdisk_write_checksum (const rtems_nvdisk* nvd, |
---|
372 | uint32_t device, |
---|
373 | uint32_t page, |
---|
374 | const uint16_t cs) |
---|
375 | { |
---|
376 | return rtems_nvdisk_device_write (nvd, device, |
---|
377 | page * sizeof (uint16_t), |
---|
378 | &cs, sizeof (uint16_t)); |
---|
379 | } |
---|
380 | |
---|
381 | /** |
---|
382 | * Calculate the pages in a device give the device descriptor and the |
---|
383 | * page size. |
---|
384 | * |
---|
385 | * @param dd The device descriptor. |
---|
386 | * @param page_size The page size in bytes. |
---|
387 | */ |
---|
388 | static uint32_t |
---|
389 | rtems_nvdisk_pages_in_device (const rtems_nvdisk* nvd, |
---|
390 | const rtems_nvdisk_device_desc* dd) |
---|
391 | { |
---|
392 | return dd->size / nvd->block_size; |
---|
393 | } |
---|
394 | |
---|
395 | /** |
---|
396 | * Calculate the number of pages needed to hold the page descriptors. |
---|
397 | * The calculation need to round up. |
---|
398 | */ |
---|
399 | static uint32_t |
---|
400 | rtems_nvdisk_page_desc_pages (const rtems_nvdisk* nvd, |
---|
401 | const rtems_nvdisk_device_desc* dd) |
---|
402 | { |
---|
403 | uint32_t pages = rtems_nvdisk_pages_in_device (nvd, dd); |
---|
404 | uint32_t bytes = pages * sizeof (uint16_t); |
---|
405 | return ((bytes - 1) / nvd->block_size) + 1; |
---|
406 | } |
---|
407 | |
---|
408 | /** |
---|
409 | * Calculate the checksum of a page. |
---|
410 | */ |
---|
411 | static uint16_t |
---|
412 | rtems_nvdisk_page_checksum (const uint8_t* buffer, uint32_t page_size) |
---|
413 | { |
---|
414 | uint16_t cs = 0xffff; |
---|
415 | uint32_t i; |
---|
416 | |
---|
417 | for (i = 0; i < page_size; i++, buffer++) |
---|
418 | cs = rtems_nvdisk_calc_crc16 (cs, *buffer); |
---|
419 | |
---|
420 | return cs; |
---|
421 | } |
---|
422 | |
---|
423 | /** |
---|
424 | * Map a block to a device. |
---|
425 | */ |
---|
426 | static rtems_nvdisk_device_ctl* |
---|
427 | rtems_nvdisk_get_device (rtems_nvdisk* nvd, uint32_t block) |
---|
428 | { |
---|
429 | uint32_t device; |
---|
430 | |
---|
431 | if (block >= nvd->block_count) |
---|
432 | { |
---|
433 | rtems_nvdisk_error ("read-block: bad block: %d", block); |
---|
434 | return NULL; |
---|
435 | } |
---|
436 | |
---|
437 | for (device = 0; device < nvd->device_count; device++) |
---|
438 | { |
---|
439 | rtems_nvdisk_device_ctl* dc = &nvd->devices[device]; |
---|
440 | if ((block >= dc->block_base) && |
---|
441 | (block < (dc->block_base + dc->pages - dc->pages_desc))) |
---|
442 | return dc; |
---|
443 | } |
---|
444 | |
---|
445 | rtems_nvdisk_error ("map-block:%d: no device/page map found", block); |
---|
446 | |
---|
447 | return NULL; |
---|
448 | } |
---|
449 | |
---|
450 | /** |
---|
451 | * Get the page for a block in a device. |
---|
452 | */ |
---|
453 | static uint32_t |
---|
454 | rtems_nvdisk_get_page (rtems_nvdisk_device_ctl* dc, |
---|
455 | uint32_t block) |
---|
456 | { |
---|
457 | return block - dc->block_base; |
---|
458 | } |
---|
459 | |
---|
460 | /** |
---|
461 | * Read a block. The block is checked to see if the page referenced |
---|
462 | * is valid and the page has a valid crc. |
---|
463 | * |
---|
464 | * @param nvd The rtems_nvdisk control table. |
---|
465 | * @param block The block number to read. |
---|
466 | * @param buffer The buffer to write the data into. |
---|
467 | * @return 0 No error. |
---|
468 | * @return EIO Invalid block number or crc. |
---|
469 | */ |
---|
470 | static int |
---|
471 | rtems_nvdisk_read_block (rtems_nvdisk* nvd, uint32_t block, uint8_t* buffer) |
---|
472 | { |
---|
473 | rtems_nvdisk_device_ctl* dc; |
---|
474 | uint32_t page; |
---|
475 | uint16_t crc; |
---|
476 | uint16_t cs; |
---|
477 | int ret; |
---|
478 | |
---|
479 | dc = rtems_nvdisk_get_device (nvd, block); |
---|
480 | |
---|
481 | if (!dc) |
---|
482 | return EIO; |
---|
483 | |
---|
484 | page = rtems_nvdisk_get_page (dc, block); |
---|
485 | |
---|
486 | #if RTEMS_NVDISK_TRACE |
---|
487 | rtems_nvdisk_info (nvd, " read-block:%d=>%02d-%03d, cs:%04x", |
---|
488 | block, dc->device, page, crc); |
---|
489 | #endif |
---|
490 | |
---|
491 | ret = rtems_nvdisk_read_checksum (nvd, dc->device, page, &crc); |
---|
492 | |
---|
493 | if (ret) |
---|
494 | return ret; |
---|
495 | |
---|
496 | if (crc == 0xffff) |
---|
497 | { |
---|
498 | #if RTEMS_NVDISK_TRACE |
---|
499 | rtems_nvdisk_warning (nvd, "read-block: crc not set: %d", block); |
---|
500 | #endif |
---|
501 | memset (buffer, 0, nvd->block_size); |
---|
502 | return 0; |
---|
503 | } |
---|
504 | |
---|
505 | ret = rtems_nvdisk_read_page (nvd, dc->device, page + dc->pages_desc, buffer); |
---|
506 | |
---|
507 | if (ret) |
---|
508 | return ret; |
---|
509 | |
---|
510 | cs = rtems_nvdisk_page_checksum (buffer, nvd->block_size); |
---|
511 | |
---|
512 | if (cs != crc) |
---|
513 | { |
---|
514 | rtems_nvdisk_error ("read-block: crc failure: %d: buffer:%04x page:%04x", |
---|
515 | block, cs, crc); |
---|
516 | return EIO; |
---|
517 | } |
---|
518 | |
---|
519 | return 0; |
---|
520 | } |
---|
521 | |
---|
522 | /** |
---|
523 | * Write a block. |
---|
524 | * |
---|
525 | * @param nvd The rtems_nvdisk control table. |
---|
526 | * @param block The block number to read. |
---|
527 | * @param block_size The size of the block. Must match what we have. |
---|
528 | * @param buffer The buffer to write the data into. |
---|
529 | * @return 0 No error. |
---|
530 | * @return EIO Invalid block size, block number, segment pointer, crc, |
---|
531 | * page flags. |
---|
532 | */ |
---|
533 | static int |
---|
534 | rtems_nvdisk_write_block (rtems_nvdisk* nvd, |
---|
535 | uint32_t block, |
---|
536 | const unsigned char* buffer) |
---|
537 | { |
---|
538 | rtems_nvdisk_device_ctl* dc; |
---|
539 | uint32_t page; |
---|
540 | uint16_t cs; |
---|
541 | int ret; |
---|
542 | |
---|
543 | dc = rtems_nvdisk_get_device (nvd, block); |
---|
544 | |
---|
545 | if (!dc) |
---|
546 | return EIO; |
---|
547 | |
---|
548 | page = rtems_nvdisk_get_page (dc, block); |
---|
549 | |
---|
550 | cs = rtems_nvdisk_page_checksum (buffer, nvd->block_size); |
---|
551 | |
---|
552 | #if RTEMS_NVDISK_TRACE |
---|
553 | rtems_nvdisk_info (nvd, " write-block:%d=>%02d-%03d", block, dc->device, page); |
---|
554 | #endif |
---|
555 | |
---|
556 | ret = rtems_nvdisk_write_page (nvd, dc->device, page + dc->pages_desc, buffer); |
---|
557 | |
---|
558 | if (ret) |
---|
559 | return ret; |
---|
560 | |
---|
561 | return rtems_nvdisk_write_checksum (nvd, dc->device, page, cs); |
---|
562 | } |
---|
563 | |
---|
564 | /** |
---|
565 | * Disk READ request handler. This primitive copies data from the |
---|
566 | * flash disk to the supplied buffer and invoke the callout function |
---|
567 | * to inform upper layer that reading is completed. |
---|
568 | * |
---|
569 | * @param req Pointer to the READ block device request info. |
---|
570 | * @retval int The ioctl return value. |
---|
571 | */ |
---|
572 | static int |
---|
573 | rtems_nvdisk_read (rtems_nvdisk* nvd, rtems_blkdev_request* req) |
---|
574 | { |
---|
575 | rtems_blkdev_sg_buffer* sg = req->bufs; |
---|
576 | uint32_t b; |
---|
577 | int32_t remains; |
---|
578 | int ret = 0; |
---|
579 | |
---|
580 | #if RTEMS_NVDISK_TRACE |
---|
581 | rtems_nvdisk_info (nvd, "read: blocks=%d", req->bufnum); |
---|
582 | #endif |
---|
583 | |
---|
584 | remains = req->bufnum * nvd->block_size; |
---|
585 | |
---|
586 | for (b = 0; b < req->bufnum; b++, sg++) |
---|
587 | { |
---|
588 | uint32_t length = sg->length; |
---|
589 | |
---|
590 | if (remains <= 0) |
---|
591 | rtems_nvdisk_error ("nvdisk-read: remains size <= 0"); |
---|
592 | |
---|
593 | if (sg->length != nvd->block_size) |
---|
594 | { |
---|
595 | rtems_nvdisk_error ("nvdisk-read: length is not the block size: "\ |
---|
596 | "bd:%d nvd:%d", sg->length, nvd->block_size); |
---|
597 | |
---|
598 | if (length > nvd->block_size) |
---|
599 | length = nvd->block_size; |
---|
600 | } |
---|
601 | |
---|
602 | ret = rtems_nvdisk_read_block (nvd, sg->block, sg->buffer); |
---|
603 | |
---|
604 | if (ret) |
---|
605 | break; |
---|
606 | |
---|
607 | remains -= length; |
---|
608 | } |
---|
609 | |
---|
610 | req->req_done (req->done_arg, |
---|
611 | ret ? RTEMS_SUCCESSFUL : RTEMS_IO_ERROR, ret); |
---|
612 | |
---|
613 | return ret; |
---|
614 | } |
---|
615 | |
---|
616 | /** |
---|
617 | * Flash disk WRITE request handler. This primitive copies data from |
---|
618 | * supplied buffer to NV disk and invoke the callout function to inform |
---|
619 | * upper layer that writing is completed. |
---|
620 | * |
---|
621 | * @param req Pointers to the WRITE block device request info. |
---|
622 | * @retval int The ioctl return value. |
---|
623 | */ |
---|
624 | static int |
---|
625 | rtems_nvdisk_write (rtems_nvdisk* nvd, rtems_blkdev_request* req) |
---|
626 | { |
---|
627 | rtems_blkdev_sg_buffer* sg = req->bufs; |
---|
628 | uint32_t b; |
---|
629 | int ret = 0; |
---|
630 | |
---|
631 | #if RTEMS_NVDISK_TRACE |
---|
632 | rtems_nvdisk_info (nvd, "write: blocks=%d", req->bufnum); |
---|
633 | #endif |
---|
634 | |
---|
635 | for (b = 0; b < req->bufnum; b++, sg++) |
---|
636 | { |
---|
637 | if (sg->length != nvd->block_size) |
---|
638 | { |
---|
639 | rtems_nvdisk_error ("nvdisk-write: length is not the block size: " \ |
---|
640 | "bd:%d nvd:%d", sg->length, nvd->block_size); |
---|
641 | } |
---|
642 | |
---|
643 | ret = rtems_nvdisk_write_block (nvd, sg->block, sg->buffer); |
---|
644 | |
---|
645 | if (ret) |
---|
646 | break; |
---|
647 | } |
---|
648 | |
---|
649 | req->req_done (req->done_arg, |
---|
650 | ret ? RTEMS_SUCCESSFUL : RTEMS_IO_ERROR, ret); |
---|
651 | |
---|
652 | return 0; |
---|
653 | } |
---|
654 | |
---|
655 | /** |
---|
656 | * NV disk erase disk sets all the checksums for 0xffff. |
---|
657 | * |
---|
658 | * @param nvd The nvdisk data. |
---|
659 | * @retval int The ioctl return value. |
---|
660 | */ |
---|
661 | static int |
---|
662 | rtems_nvdisk_erase_disk (rtems_nvdisk* nvd) |
---|
663 | { |
---|
664 | uint32_t device; |
---|
665 | |
---|
666 | #if RTEMS_NVDISK_TRACE |
---|
667 | rtems_nvdisk_info (nvd, "erase-disk"); |
---|
668 | #endif |
---|
669 | |
---|
670 | for (device = 0; device < nvd->device_count; device++) |
---|
671 | { |
---|
672 | rtems_nvdisk_device_ctl* dc = &nvd->devices[device]; |
---|
673 | uint32_t page; |
---|
674 | for (page = 0; page < (dc->pages - dc->pages_desc); page++) |
---|
675 | { |
---|
676 | int ret = rtems_nvdisk_write_checksum (nvd, dc->device, page, 0xffff); |
---|
677 | if (ret) |
---|
678 | return ret; |
---|
679 | } |
---|
680 | } |
---|
681 | |
---|
682 | return 0; |
---|
683 | } |
---|
684 | |
---|
685 | /** |
---|
686 | * NV disk IOCTL handler. |
---|
687 | * |
---|
688 | * @param dev Device number (major, minor number). |
---|
689 | * @param req IOCTL request code. |
---|
690 | * @param argp IOCTL argument. |
---|
691 | * @retval The IOCTL return value |
---|
692 | */ |
---|
693 | static int |
---|
694 | rtems_nvdisk_ioctl (dev_t dev, uint32_t req, void* argp) |
---|
695 | { |
---|
696 | rtems_device_minor_number minor = rtems_filesystem_dev_minor_t (dev); |
---|
697 | rtems_blkdev_request* r = argp; |
---|
698 | rtems_status_code sc; |
---|
699 | |
---|
700 | if (minor >= rtems_nvdisk_count) |
---|
701 | { |
---|
702 | errno = ENODEV; |
---|
703 | return -1; |
---|
704 | } |
---|
705 | |
---|
706 | if (rtems_nvdisks[minor].device_count == 0) |
---|
707 | { |
---|
708 | errno = ENODEV; |
---|
709 | return -1; |
---|
710 | } |
---|
711 | |
---|
712 | errno = 0; |
---|
713 | |
---|
714 | sc = rtems_semaphore_obtain (rtems_nvdisks[minor].lock, RTEMS_WAIT, 0); |
---|
715 | if (sc != RTEMS_SUCCESSFUL) |
---|
716 | errno = EIO; |
---|
717 | else |
---|
718 | { |
---|
719 | switch (req) |
---|
720 | { |
---|
721 | case RTEMS_BLKIO_REQUEST: |
---|
722 | switch (r->req) |
---|
723 | { |
---|
724 | case RTEMS_BLKDEV_REQ_READ: |
---|
725 | errno = rtems_nvdisk_read (&rtems_nvdisks[minor], r); |
---|
726 | break; |
---|
727 | |
---|
728 | case RTEMS_BLKDEV_REQ_WRITE: |
---|
729 | errno = rtems_nvdisk_write (&rtems_nvdisks[minor], r); |
---|
730 | break; |
---|
731 | |
---|
732 | default: |
---|
733 | errno = EBADRQC; |
---|
734 | break; |
---|
735 | } |
---|
736 | break; |
---|
737 | |
---|
738 | case RTEMS_NVDISK_IOCTL_ERASE_DISK: |
---|
739 | errno = rtems_nvdisk_erase_disk (&rtems_nvdisks[minor]); |
---|
740 | break; |
---|
741 | |
---|
742 | case RTEMS_NVDISK_IOCTL_INFO_LEVEL: |
---|
743 | rtems_nvdisks[minor].info_level = (uint32_t) argp; |
---|
744 | break; |
---|
745 | |
---|
746 | default: |
---|
747 | errno = EBADRQC; |
---|
748 | break; |
---|
749 | } |
---|
750 | |
---|
751 | sc = rtems_semaphore_release (rtems_nvdisks[minor].lock); |
---|
752 | if (sc != RTEMS_SUCCESSFUL) |
---|
753 | errno = EIO; |
---|
754 | } |
---|
755 | |
---|
756 | return errno == 0 ? 0 : -1; |
---|
757 | } |
---|
758 | |
---|
759 | /** |
---|
760 | * NV disk device driver initialization. |
---|
761 | * |
---|
762 | * @todo Memory clean up on error is really badly handled. |
---|
763 | * |
---|
764 | * @param major NV disk major device number. |
---|
765 | * @param minor Minor device number, not applicable. |
---|
766 | * @param arg Initialization argument, not applicable. |
---|
767 | */ |
---|
768 | rtems_device_driver |
---|
769 | rtems_nvdisk_initialize (rtems_device_major_number major, |
---|
770 | rtems_device_minor_number minor, |
---|
771 | void* arg) |
---|
772 | { |
---|
773 | const rtems_nvdisk_config* c = rtems_nvdisk_configuration; |
---|
774 | rtems_nvdisk* nvd; |
---|
775 | rtems_status_code sc; |
---|
776 | |
---|
777 | sc = rtems_disk_io_initialize (); |
---|
778 | if (sc != RTEMS_SUCCESSFUL) |
---|
779 | return sc; |
---|
780 | |
---|
781 | sc = rtems_nvdisk_crc16_gen_factors (0x8408); |
---|
782 | if (sc != RTEMS_SUCCESSFUL) |
---|
783 | return sc; |
---|
784 | |
---|
785 | rtems_nvdisks = calloc (rtems_nvdisk_configuration_size, |
---|
786 | sizeof (rtems_nvdisk)); |
---|
787 | |
---|
788 | if (!rtems_nvdisks) |
---|
789 | return RTEMS_NO_MEMORY; |
---|
790 | |
---|
791 | for (minor = 0; minor < rtems_nvdisk_configuration_size; minor++, c++) |
---|
792 | { |
---|
793 | char name[sizeof (RTEMS_NVDISK_DEVICE_BASE_NAME) + 10]; |
---|
794 | dev_t dev = rtems_filesystem_make_dev_t (major, minor); |
---|
795 | uint32_t device; |
---|
796 | uint32_t blocks = 0; |
---|
797 | |
---|
798 | nvd = &rtems_nvdisks[minor]; |
---|
799 | |
---|
800 | snprintf (name, sizeof (name), |
---|
801 | RTEMS_NVDISK_DEVICE_BASE_NAME "%" PRIu32, minor); |
---|
802 | |
---|
803 | nvd->major = major; |
---|
804 | nvd->minor = minor; |
---|
805 | nvd->flags = c->flags; |
---|
806 | nvd->block_size = c->block_size; |
---|
807 | nvd->info_level = c->info_level; |
---|
808 | |
---|
809 | nvd->devices = calloc (c->device_count, sizeof (rtems_nvdisk_device_ctl)); |
---|
810 | if (!nvd->devices) |
---|
811 | return RTEMS_NO_MEMORY; |
---|
812 | |
---|
813 | for (device = 0; device < c->device_count; device++) |
---|
814 | { |
---|
815 | rtems_nvdisk_device_ctl* dc = &nvd->devices[device]; |
---|
816 | |
---|
817 | dc->device = device; |
---|
818 | dc->pages = rtems_nvdisk_pages_in_device (nvd, &c->devices[device]); |
---|
819 | dc->pages_desc = rtems_nvdisk_page_desc_pages (nvd, &c->devices[device]); |
---|
820 | dc->block_base = blocks; |
---|
821 | |
---|
822 | blocks += dc->pages - dc->pages_desc; |
---|
823 | |
---|
824 | dc->descriptor = &c->devices[device]; |
---|
825 | } |
---|
826 | |
---|
827 | nvd->block_count = blocks; |
---|
828 | nvd->device_count = c->device_count; |
---|
829 | |
---|
830 | sc = rtems_disk_create_phys(dev, c->block_size, blocks, |
---|
831 | rtems_nvdisk_ioctl, name); |
---|
832 | if (sc != RTEMS_SUCCESSFUL) |
---|
833 | { |
---|
834 | rtems_nvdisk_error ("disk create phy failed"); |
---|
835 | return sc; |
---|
836 | } |
---|
837 | |
---|
838 | sc = rtems_semaphore_create (rtems_build_name ('N', 'V', 'D', 'K'), 1, |
---|
839 | RTEMS_PRIORITY | RTEMS_BINARY_SEMAPHORE | |
---|
840 | RTEMS_INHERIT_PRIORITY, 0, &nvd->lock); |
---|
841 | if (sc != RTEMS_SUCCESSFUL) |
---|
842 | { |
---|
843 | rtems_nvdisk_error ("disk lock create failed"); |
---|
844 | return sc; |
---|
845 | } |
---|
846 | } |
---|
847 | |
---|
848 | rtems_nvdisk_count = rtems_nvdisk_configuration_size; |
---|
849 | |
---|
850 | return RTEMS_SUCCESSFUL; |
---|
851 | } |
---|