source: rtems/cpukit/libblock/src/nvdisk.c @ 0d15414e

4.104.115
Last change on this file since 0d15414e was 918a0d8, checked in by Joel Sherrill <joel.sherrill@…>, on 06/12/09 at 17:38:53

2009-06-12 Joel Sherrill <joel.sherrill@…>

  • libblock/src/flashdisk.c, libblock/src/nvdisk.c, libblock/src/ramdisk.c, libfs/src/dosfs/fat.c, libfs/src/dosfs/msdos_format.c: Eliminate using the GNU/Linux specific error numbers EBADRQC and ENOTBLK. Switch to EINVAL and ENOTTY as appropriate.
  • Property mode set to 100644
File size: 22.1 KB
Line 
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 */
60typedef 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 */
92typedef 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 */
109static rtems_nvdisk* rtems_nvdisks;
110
111/**
112 * The number of NV disks we have.
113 */
114static uint32_t rtems_nvdisk_count;
115
116/**
117 * The CRC16 factor table. Created during initialisation.
118 */
119static 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 */
137rtems_status_code
138rtems_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 */
166static int
167rtems_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 */
190static int
191rtems_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 */
214static int
215rtems_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 */
238static int
239rtems_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 */
254static const rtems_nvdisk_device_desc*
255rtems_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 */
263static int
264rtems_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 */
284static int
285rtems_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 */
306static int
307rtems_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 */
328static int
329rtems_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 */
342static int
343rtems_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 */
356static int
357rtems_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 */
370static int
371rtems_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 */
388static uint32_t
389rtems_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 */
399static uint32_t
400rtems_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 */
411static uint16_t
412rtems_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 */
426static rtems_nvdisk_device_ctl*
427rtems_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 */
453static uint32_t
454rtems_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 */
470static int
471rtems_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 */
533static int
534rtems_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 */
572static int
573rtems_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 */
624static int
625rtems_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 */
661static int
662rtems_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 */
693static int
694rtems_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 = EINVAL;
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 = EINVAL;
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 */
768rtems_device_driver
769rtems_nvdisk_initialize (rtems_device_major_number major,
770                        rtems_device_minor_number minor,
771                        void*                     arg __attribute__((unused)))
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[] = RTEMS_NVDISK_DEVICE_BASE_NAME "a";
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    name [sizeof(RTEMS_NVDISK_DEVICE_BASE_NAME)] += minor;
801 
802    nvd->major        = major;
803    nvd->minor        = minor;
804    nvd->flags        = c->flags;
805    nvd->block_size   = c->block_size;
806    nvd->info_level   = c->info_level;
807
808    nvd->devices = calloc (c->device_count, sizeof (rtems_nvdisk_device_ctl));
809    if (!nvd->devices)
810      return RTEMS_NO_MEMORY;
811
812    for (device = 0; device < c->device_count; device++)
813    {
814      rtems_nvdisk_device_ctl* dc = &nvd->devices[device];
815
816      dc->device     = device;
817      dc->pages      = rtems_nvdisk_pages_in_device (nvd, &c->devices[device]);
818      dc->pages_desc = rtems_nvdisk_page_desc_pages (nvd, &c->devices[device]);
819      dc->block_base = blocks;
820     
821      blocks += dc->pages - dc->pages_desc;
822
823      dc->descriptor = &c->devices[device];
824    }
825
826    nvd->block_count  = blocks;
827    nvd->device_count = c->device_count;
828
829    sc = rtems_disk_create_phys(dev, c->block_size, blocks,
830                                rtems_nvdisk_ioctl, name);
831    if (sc != RTEMS_SUCCESSFUL)
832    {
833      rtems_nvdisk_error ("disk create phy failed");
834      return sc;
835    }
836 
837    sc = rtems_semaphore_create (rtems_build_name ('N', 'V', 'D', 'K'), 1,
838                                 RTEMS_PRIORITY | RTEMS_BINARY_SEMAPHORE |
839                                 RTEMS_INHERIT_PRIORITY, 0, &nvd->lock);
840    if (sc != RTEMS_SUCCESSFUL)
841    {
842      rtems_nvdisk_error ("disk lock create failed");
843      return sc;
844    }
845  }
846
847  rtems_nvdisk_count = rtems_nvdisk_configuration_size;
848
849  return RTEMS_SUCCESSFUL;
850}
Note: See TracBrowser for help on using the repository browser.