source: rtems/cpukit/libblock/src/nvdisk.c @ 5c587596

4.104.115
Last change on this file since 5c587596 was 5c587596, checked in by Thomas Doerfler <Thomas.Doerfler@…>, on 01/19/10 at 09:10:03

libblock API update

  • Property mode set to 100644
File size: 21.8 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                bufs;
577  int                     ret = 0;
578
579#if RTEMS_NVDISK_TRACE
580  rtems_nvdisk_info (nvd, "read: blocks=%d", req->bufnum);
581#endif
582
583  for (bufs = 0; (ret == 0) && (bufs < req->bufnum); bufs++, sg++)
584  {
585    uint8_t* data;
586    uint32_t nvb;
587    uint32_t b;
588    nvb = sg->length / nvd->block_size;
589    data = sg->buffer;
590    for (b = 0; b < nvb; b++, data += nvd->block_size)
591    {
592      ret = rtems_nvdisk_read_block (nvd, sg->block + b, data);
593      if (ret)
594        break;
595    }
596  }
597
598  req->req_done (req->done_arg, ret ? RTEMS_SUCCESSFUL : RTEMS_IO_ERROR);
599
600  return ret;
601}
602
603/**
604 * Flash disk WRITE request handler. This primitive copies data from
605 * supplied buffer to NV disk and invoke the callout function to inform
606 * upper layer that writing is completed.
607 *
608 * @param req Pointers to the WRITE block device request info.
609 * @retval int The ioctl return value.
610 */
611static int
612rtems_nvdisk_write (rtems_nvdisk* nvd, rtems_blkdev_request* req)
613{
614  rtems_blkdev_sg_buffer* sg = req->bufs;
615  uint32_t                bufs;
616  int                     ret = 0;
617
618#if RTEMS_NVDISK_TRACE
619  rtems_nvdisk_info (nvd, "write: blocks=%d", req->bufnum);
620#endif
621
622  for (bufs = 0; (ret == 0) && (bufs < req->bufnum); bufs++, sg++)
623  {
624    uint8_t* data;
625    uint32_t nvb;
626    uint32_t b;
627    nvb = sg->length / nvd->block_size;
628    data = sg->buffer;
629    for (b = 0; b < nvb; b++, data += nvd->block_size)
630    {
631      ret = rtems_nvdisk_write_block (nvd, sg->block + b, data);
632      if (ret)
633        break;
634    }
635  }
636
637  req->req_done (req->done_arg, ret ? RTEMS_SUCCESSFUL : RTEMS_IO_ERROR);
638
639  return 0;
640}
641
642/**
643 * NV disk erase disk sets all the checksums for 0xffff.
644 *
645 * @param nvd The nvdisk data.
646 * @retval int The ioctl return value.
647 */
648static int
649rtems_nvdisk_erase_disk (rtems_nvdisk* nvd)
650{
651  uint32_t device;
652
653#if RTEMS_NVDISK_TRACE
654  rtems_nvdisk_info (nvd, "erase-disk");
655#endif
656
657  for (device = 0; device < nvd->device_count; device++)
658  {
659    rtems_nvdisk_device_ctl* dc = &nvd->devices[device];
660    uint32_t                 page;
661    for (page = 0; page < (dc->pages - dc->pages_desc); page++)
662    {
663      int ret = rtems_nvdisk_write_checksum (nvd, dc->device, page, 0xffff);
664      if (ret)
665        return ret;
666    }
667  }
668
669  return 0;
670}
671
672/**
673 * NV disk IOCTL handler.
674 *
675 * @param dd Disk device.
676 * @param req IOCTL request code.
677 * @param argp IOCTL argument.
678 * @retval The IOCTL return value
679 */
680static int
681rtems_nvdisk_ioctl (rtems_disk_device *dd, uint32_t req, void* argp)
682{
683  dev_t                     dev = rtems_disk_get_device_identifier (dd);
684  rtems_device_minor_number minor = rtems_filesystem_dev_minor_t (dev);
685  rtems_blkdev_request*     r = argp;
686  rtems_status_code         sc;
687
688  if (minor >= rtems_nvdisk_count)
689  {
690    errno = ENODEV;
691    return -1;
692  }
693
694  if (rtems_nvdisks[minor].device_count == 0)
695  {
696    errno = ENODEV;
697    return -1;
698  }
699
700  errno = 0;
701
702  sc = rtems_semaphore_obtain (rtems_nvdisks[minor].lock, RTEMS_WAIT, 0);
703  if (sc != RTEMS_SUCCESSFUL)
704    errno = EIO;
705  else
706  {
707    errno = 0;
708    switch (req)
709    {
710      case RTEMS_BLKIO_REQUEST:
711        switch (r->req)
712        {
713          case RTEMS_BLKDEV_REQ_READ:
714            errno = rtems_nvdisk_read (&rtems_nvdisks[minor], r);
715            break;
716
717          case RTEMS_BLKDEV_REQ_WRITE:
718            errno = rtems_nvdisk_write (&rtems_nvdisks[minor], r);
719            break;
720
721          default:
722            errno = EINVAL;
723            break;
724        }
725        break;
726
727      case RTEMS_NVDISK_IOCTL_ERASE_DISK:
728        errno = rtems_nvdisk_erase_disk (&rtems_nvdisks[minor]);
729        break;
730
731      case RTEMS_NVDISK_IOCTL_INFO_LEVEL:
732        rtems_nvdisks[minor].info_level = (uint32_t) argp;
733        break;
734
735      default:
736        rtems_blkdev_ioctl (dd, req, argp);
737        break;
738    }
739
740    sc = rtems_semaphore_release (rtems_nvdisks[minor].lock);
741    if (sc != RTEMS_SUCCESSFUL)
742      errno = EIO;
743  }
744
745  return errno == 0 ? 0 : -1;
746}
747
748/**
749 * NV disk device driver initialization.
750 *
751 * @todo Memory clean up on error is really badly handled.
752 *
753 * @param major NV disk major device number.
754 * @param minor Minor device number, not applicable.
755 * @param arg Initialization argument, not applicable.
756 */
757rtems_device_driver
758rtems_nvdisk_initialize (rtems_device_major_number major,
759                        rtems_device_minor_number minor,
760                        void*                     arg __attribute__((unused)))
761{
762  const rtems_nvdisk_config* c = rtems_nvdisk_configuration;
763  rtems_nvdisk*              nvd;
764  rtems_status_code          sc;
765
766  sc = rtems_disk_io_initialize ();
767  if (sc != RTEMS_SUCCESSFUL)
768    return sc;
769
770  sc = rtems_nvdisk_crc16_gen_factors (0x8408);
771  if (sc != RTEMS_SUCCESSFUL)
772      return sc;
773
774  rtems_nvdisks = calloc (rtems_nvdisk_configuration_size,
775                          sizeof (rtems_nvdisk));
776
777  if (!rtems_nvdisks)
778    return RTEMS_NO_MEMORY;
779
780  for (minor = 0; minor < rtems_nvdisk_configuration_size; minor++, c++)
781  {
782    char     name[] = RTEMS_NVDISK_DEVICE_BASE_NAME "a";
783    dev_t    dev = rtems_filesystem_make_dev_t (major, minor);
784    uint32_t device;
785    uint32_t blocks = 0;
786
787    nvd = &rtems_nvdisks[minor];
788
789    name [sizeof(RTEMS_NVDISK_DEVICE_BASE_NAME)] += minor;
790
791    nvd->major        = major;
792    nvd->minor        = minor;
793    nvd->flags        = c->flags;
794    nvd->block_size   = c->block_size;
795    nvd->info_level   = c->info_level;
796
797    nvd->devices = calloc (c->device_count, sizeof (rtems_nvdisk_device_ctl));
798    if (!nvd->devices)
799      return RTEMS_NO_MEMORY;
800
801    for (device = 0; device < c->device_count; device++)
802    {
803      rtems_nvdisk_device_ctl* dc = &nvd->devices[device];
804
805      dc->device     = device;
806      dc->pages      = rtems_nvdisk_pages_in_device (nvd, &c->devices[device]);
807      dc->pages_desc = rtems_nvdisk_page_desc_pages (nvd, &c->devices[device]);
808      dc->block_base = blocks;
809
810      blocks += dc->pages - dc->pages_desc;
811
812      dc->descriptor = &c->devices[device];
813    }
814
815    nvd->block_count  = blocks;
816    nvd->device_count = c->device_count;
817
818    sc = rtems_disk_create_phys(dev, c->block_size, blocks,
819                                rtems_nvdisk_ioctl, NULL, name);
820    if (sc != RTEMS_SUCCESSFUL)
821    {
822      rtems_nvdisk_error ("disk create phy failed");
823      return sc;
824    }
825
826    sc = rtems_semaphore_create (rtems_build_name ('N', 'V', 'D', 'K'), 1,
827                                 RTEMS_PRIORITY | RTEMS_BINARY_SEMAPHORE |
828                                 RTEMS_INHERIT_PRIORITY, 0, &nvd->lock);
829    if (sc != RTEMS_SUCCESSFUL)
830    {
831      rtems_nvdisk_error ("disk lock create failed");
832      return sc;
833    }
834  }
835
836  rtems_nvdisk_count = rtems_nvdisk_configuration_size;
837
838  return RTEMS_SUCCESSFUL;
839}
Note: See TracBrowser for help on using the repository browser.