summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--nuttx/fs/romfs/Make.defs39
-rw-r--r--nuttx/fs/romfs/fs_romfs.c1000
-rw-r--r--nuttx/fs/romfs/fs_romfs.h221
-rw-r--r--nuttx/fs/romfs/fs_romfsutil.c775
4 files changed, 2035 insertions, 0 deletions
diff --git a/nuttx/fs/romfs/Make.defs b/nuttx/fs/romfs/Make.defs
new file mode 100644
index 000000000..172eb27b9
--- /dev/null
+++ b/nuttx/fs/romfs/Make.defs
@@ -0,0 +1,39 @@
+############################################################################
+# fs/romfs/Make.defs
+#
+# Copyright (C) 2008 Gregory Nutt. All rights reserved.
+# Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name Nuttx nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+ifeq ($(CONFIG_FS_ROMFS),y)
+ASRCS +=
+CSRCS += fs_romfs.c fs_romfsutil.c
+endif
diff --git a/nuttx/fs/romfs/fs_romfs.c b/nuttx/fs/romfs/fs_romfs.c
new file mode 100644
index 000000000..5400b5d00
--- /dev/null
+++ b/nuttx/fs/romfs/fs_romfs.c
@@ -0,0 +1,1000 @@
+/****************************************************************************
+ * rm/romfs/fs_romfs.h
+ *
+ * Copyright (C) 2008 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * References: Linux/Documentation/filesystems/romfs.txt
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <sys/statfs.h>
+#include <sys/stat.h>
+
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <fcntl.h>
+#include <limits.h>
+#include <assert.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/fs.h>
+
+#include "fs_romfs.h"
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int romfs_open(FAR struct file *filep, const char *relpath,
+ int oflags, mode_t mode);
+static int romfs_close(FAR struct file *filep);
+static ssize_t romfs_read(FAR struct file *filep, char *buffer, size_t buflen);
+static off_t romfs_seek(FAR struct file *filep, off_t offset, int whence);
+
+static int romfs_opendir(struct inode *mountpt, const char *relpath,
+ struct internal_dir_s *dir);
+static int romfs_readdir(struct inode *mountpt, struct internal_dir_s *dir);
+static int romfs_rewinddir(struct inode *mountpt, struct internal_dir_s *dir);
+
+static int romfs_bind(FAR struct inode *blkdriver, const void *data,
+ void **handle);
+static int romfs_unbind(void *handle, FAR struct inode **blkdriver);
+static int romfs_statfs(struct inode *mountpt, struct statfs *buf);
+
+static int romfs_stat(struct inode *mountpt, const char *relpath, struct stat *buf);
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/* See fs_mount.c -- this structure is explicitly externed there.
+ * We use the old-fashioned kind of initializers so that this will compile
+ * with any compiler.
+ */
+
+const struct mountpt_operations romfs_operations =
+{
+ romfs_open, /* open */
+ romfs_close, /* close */
+ romfs_read, /* read */
+ NULL, /* write */
+ romfs_seek, /* seek */
+ NULL, /* ioctl */
+ NULL, /* sync */
+
+ romfs_opendir, /* opendir */
+ NULL, /* closedir */
+ romfs_readdir, /* readdir */
+ romfs_rewinddir, /* rewinddir */
+
+ romfs_bind, /* bind */
+ romfs_unbind, /* unbind */
+ romfs_statfs, /* statfs */
+
+ NULL, /* unlink */
+ NULL, /* mkdir */
+ NULL, /* rmdir */
+ NULL, /* rename */
+ romfs_stat /* stat */
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: romfs_open
+ ****************************************************************************/
+
+static int romfs_open(FAR struct file *filep, const char *relpath,
+ int oflags, mode_t mode)
+{
+ struct romfs_dirinfo_s dirinfo;
+ struct inode *inode;
+ struct romfs_mountpt_s *rm;
+ struct romfs_file_s *rf;
+ int ret;
+
+ /* Sanity checks */
+
+ DEBUGASSERT(filep->f_priv == NULL && filep->f_inode != NULL);
+
+ /* Get the mountpoint inode reference from the file structure and the
+ * mountpoint private data from the inode structure
+ */
+
+ inode = filep->f_inode;
+ rm = (struct romfs_mountpt_s*)inode->i_private;
+
+ DEBUGASSERT(rm != NULL);
+
+ /* Check if the mount is still healthy */
+
+ romfs_semtake(rm);
+ ret = romfs_checkmount(rm);
+ if (ret != OK)
+ {
+ goto errout_with_semaphore;
+ }
+
+ /* ROMFS is read-only. Any attempt to open with any kind of write
+ * access is not permitted.
+ */
+
+ if ((oflags & O_WRONLY) != 0 || (oflags & O_RDONLY) == 0)
+ {
+ ret = -EACCES;
+ goto errout_with_semaphore;
+ }
+
+ /* Initialize the directory info structure */
+
+ memset(&dirinfo, 0, sizeof(struct romfs_dirinfo_s));
+
+ /* Locate the directory entry for this path */
+
+ ret = romfs_finddirentry(rm, &dirinfo, relpath);
+ if (ret < 0)
+ {
+ goto errout_with_semaphore;
+ }
+
+ /* The full path exists -- but is the final component a file
+ * or a directory?
+ */
+
+ if (IS_DIRECTORY(dirinfo.rd_next))
+ {
+ /* It is a directory */
+
+ ret = -EISDIR;
+ goto errout_with_semaphore;
+ }
+
+#ifdef CONFIG_FILE_MODE
+# warning "Missing check for privileges based on inode->i_mode"
+#endif
+
+ /* Create an instance of the file private date to describe the opened
+ * file.
+ */
+
+ rf = (struct romfs_file_s *)zalloc(sizeof(struct romfs_file_s));
+ if (!rf)
+ {
+ ret = -ENOMEM;
+ goto errout_with_semaphore;
+ }
+
+ /* Create a file buffer to support partial sector accesses */
+
+ rf->rf_buffer = (ubyte*)malloc(rm->rm_hwsectorsize);
+ if (!rf->rf_buffer)
+ {
+ ret = -ENOMEM;
+ goto errout_with_struct;
+ }
+
+ /* Initialize the file private data (only need to initialize non-zero elements) */
+
+ rf->rf_open = TRUE;
+ rf->rf_diroffset = dirinfo.rd_dir.fr_diroffset;
+ rf->rf_startoffset = dirinfo.rd_dir.fr_curroffset;
+ rf->rf_size = dirinfo.rd_size;
+ rf->rf_cachesector = (uint32)-1;
+
+ /* Attach the private date to the struct file instance */
+
+ filep->f_priv = rf;
+
+ /* Then insert the new instance into the mountpoint structure.
+ * It needs to be there (1) to handle error conditions that effect
+ * all files, and (2) to inform the umount logic that we are busy
+ * (but a simple reference count could have done that).
+ */
+
+ rf->rf_next = rm->rm_head;
+ rm->rm_head = rf->rf_next;
+
+ romfs_semgive(rm);
+ return OK;
+
+ /* Error exits -- goto's are nasty things, but they sure can make error
+ * handling a lot simpler.
+ */
+
+errout_with_struct:
+ free(rf);
+
+errout_with_semaphore:
+ romfs_semgive(rm);
+ return ret;
+}
+
+/****************************************************************************
+ * Name: romfs_close
+ ****************************************************************************/
+
+static int romfs_close(FAR struct file *filep)
+{
+ struct inode *inode;
+ struct romfs_mountpt_s *rm;
+ struct romfs_file_s *rf;
+ int ret = OK;
+
+ /* Sanity checks */
+
+ DEBUGASSERT(filep->f_priv != NULL && filep->f_inode != NULL);
+
+ /* Recover our private data from the struct file instance */
+
+ rf = filep->f_priv;
+ inode = filep->f_inode;
+ rm = inode->i_private;
+
+ DEBUGASSERT(rm != NULL);
+
+ /* Do not check if the mount is healthy. We must support closing of
+ * the file even when there is healthy mount.
+ */
+
+ /* Deallocate the memory structures created when the open method
+ * was called.
+ *
+ * Free the sector buffer that was used to manage partial sector
+ * accesses.
+ */
+
+ if (rf->rf_buffer)
+ {
+ free(rf->rf_buffer);
+ }
+
+ /* Then free the file structure itself. */
+
+ free(rf);
+ filep->f_priv = NULL;
+ return ret;
+}
+
+/****************************************************************************
+ * Name: romfs_read
+ ****************************************************************************/
+
+static ssize_t romfs_read(FAR struct file *filep, char *buffer, size_t buflen)
+{
+ struct inode *inode;
+ struct romfs_mountpt_s *rm;
+ struct romfs_file_s *rf;
+ unsigned int bytesread;
+ unsigned int readsize;
+ unsigned int nsectors;
+ size_t bytesleft;
+ off_t sector;
+ ubyte *userbuffer = (ubyte*)buffer;
+ int sectorndx;
+ int ret;
+
+ /* Sanity checks */
+
+ DEBUGASSERT(filep->f_priv != NULL && filep->f_inode != NULL);
+
+ /* Recover our private data from the struct file instance */
+
+ rf = filep->f_priv;
+ inode = filep->f_inode;
+ rm = inode->i_private;
+
+ DEBUGASSERT(rm != NULL);
+
+ /* Make sure that the mount is still healthy */
+
+ romfs_semtake(rm);
+ ret = romfs_checkmount(rm);
+ if (ret != OK)
+ {
+ goto errout_with_semaphore;
+ }
+
+ /* Get the number of bytes left in the file */
+
+ bytesleft = rf->rf_size - filep->f_pos;
+
+ /* Truncate read count so that it does not exceed the number
+ * of bytes left in the file.
+ */
+
+ if (buflen > bytesleft)
+ {
+ buflen = bytesleft;
+ }
+
+ /* Get the first sector and index to read from. */
+
+ sector = SEC_NSECTORS(rm, filep->f_pos);
+ sectorndx = filep->f_pos & SEC_NDXMASK(rm);
+
+ /* Loop until either (1) all data has been transferred, or (2) an
+ * error occurs.
+ */
+
+ readsize = 0;
+ while (buflen > 0)
+ {
+ bytesread = 0;
+
+ /* Check if the user has provided a buffer large enough to
+ * hold one or more complete sectors -AND- the read is
+ * aligned to a sector boundary.
+ */
+
+ nsectors = buflen / rm->rm_hwsectorsize;
+ if (nsectors > 0 && sectorndx == 0)
+ {
+ /* Read maximum contiguous sectors directly to the user's
+ * buffer without using our tiny read buffer.
+ */
+
+ /* Read all of the sectors directly into user memory */
+
+ ret = romfs_hwread(rm, userbuffer, sector, nsectors);
+ if (ret < 0)
+ {
+ goto errout_with_semaphore;
+ }
+
+ sector += nsectors;
+ bytesread = nsectors * rm->rm_hwsectorsize;
+ }
+ else
+ {
+ /* We are reading a partial sector. First, read the whole sector
+ * into the file data buffer. This is a caching buffer so if
+ * it is already there then all is well.
+ */
+
+ ret = romfs_filecacheread(rm, rf, sector);
+ if (ret < 0)
+ {
+ goto errout_with_semaphore;
+ }
+
+ /* Copy the partial sector into the user buffer */
+
+ bytesread = rm->rm_hwsectorsize - sectorndx;
+ if (bytesread > buflen)
+ {
+ /* We will not read to the end of the buffer */
+
+ bytesread = buflen;
+ }
+ else
+ {
+ /* We will read to the end of the buffer (or beyond) */
+
+ sector++;
+ }
+
+ memcpy(userbuffer, &rf->rf_buffer[sectorndx], bytesread);
+ }
+
+ /* Set up for the next sector read */
+
+ userbuffer += bytesread;
+ filep->f_pos += bytesread;
+ readsize += bytesread;
+ buflen -= bytesread;
+ sectorndx = filep->f_pos & SEC_NDXMASK(rm);
+ }
+
+ romfs_semgive(rm);
+ return readsize;
+
+errout_with_semaphore:
+ romfs_semgive(rm);
+ return ret;
+}
+
+/****************************************************************************
+ * Name: romfs_seek
+ ****************************************************************************/
+
+static off_t romfs_seek(FAR struct file *filep, off_t offset, int whence)
+{
+ struct inode *inode;
+ struct romfs_mountpt_s *rm;
+ struct romfs_file_s *rf;
+ ssize_t position;
+ int ret;
+
+ /* Sanity checks */
+
+ DEBUGASSERT(filep->f_priv != NULL && filep->f_inode != NULL);
+
+ /* Recover our private data from the struct file instance */
+
+ rf = filep->f_priv;
+ inode = filep->f_inode;
+ rm = inode->i_private;
+
+ DEBUGASSERT(rm != NULL);
+
+ /* Map the offset according to the whence option */
+ switch (whence)
+ {
+ case SEEK_SET: /* The offset is set to offset bytes. */
+ position = offset;
+ break;
+
+ case SEEK_CUR: /* The offset is set to its current location plus
+ * offset bytes. */
+
+ position = offset + filep->f_pos;
+ break;
+
+ case SEEK_END: /* The offset is set to the size of the file plus
+ * offset bytes. */
+
+ position = offset + rf->rf_size;
+ break;
+
+ default:
+ return -EINVAL;
+ }
+
+ /* Make sure that the mount is still healthy */
+
+ romfs_semtake(rm);
+ ret = romfs_checkmount(rm);
+ if (ret != OK)
+ {
+ goto errout_with_semaphore;
+ }
+
+ /* Limit positions to the end of the file. */
+
+ if (position > rf->rf_size)
+ {
+ /* Otherwise, the position is limited to the file size */
+
+ position = rf->rf_size;
+ }
+
+ /* Set file position and return success */
+
+ filep->f_pos = position;
+
+ romfs_semgive(rm);
+ return OK;
+
+errout_with_semaphore:
+ romfs_semgive(rm);
+ return ret;
+}
+
+/****************************************************************************
+ * Name: romfs_opendir
+ *
+ * Description: Open a directory for read access
+ *
+ ****************************************************************************/
+
+static int romfs_opendir(struct inode *mountpt, const char *relpath,
+ struct internal_dir_s *dir)
+{
+ struct romfs_mountpt_s *rm;
+ struct romfs_dirinfo_s dirinfo;
+ int ret;
+
+ /* Sanity checks */
+
+ DEBUGASSERT(mountpt != NULL && mountpt->i_private != NULL);
+
+ /* Recover our private data from the inode instance */
+
+ rm = mountpt->i_private;
+
+ /* Make sure that the mount is still healthy */
+
+ romfs_semtake(rm);
+ ret = romfs_checkmount(rm);
+ if (ret != OK)
+ {
+ goto errout_with_semaphore;
+ }
+
+ /* Find the requested directory */
+
+ ret = romfs_finddirentry(rm, &dirinfo, relpath);
+ if (ret < 0)
+ {
+ goto errout_with_semaphore;
+ }
+
+ /* Verify that it is some kind of directory */
+
+ if (!IS_DIRECTORY(dirinfo.rd_next))
+ {
+ /* The entry is not a directory */
+
+ ret = -ENOTDIR;
+ goto errout_with_semaphore;
+ }
+
+ /* The entry is a directory */
+
+ memcpy(&dir->u.romfs, &dirinfo.rd_dir, sizeof(struct fs_romfsdir_s));
+ romfs_semgive(rm);
+ return OK;
+
+errout_with_semaphore:
+ romfs_semgive(rm);
+ return ERROR;
+}
+
+/****************************************************************************
+ * Name: romfs_readdir
+ *
+ * Description: Read the next directory entry
+ *
+ ****************************************************************************/
+
+static int romfs_readdir(struct inode *mountpt, struct internal_dir_s *dir)
+{
+ struct romfs_mountpt_s *rm;
+ uint32 linkoffset;
+ uint32 next;
+ uint32 info;
+ uint32 size;
+ int ret;
+
+ /* Sanity checks */
+
+ DEBUGASSERT(mountpt != NULL && mountpt->i_private != NULL);
+
+ /* Recover our private data from the inode instance */
+
+ rm = mountpt->i_private;
+
+ /* Make sure that the mount is still healthy */
+
+ romfs_semtake(rm);
+ ret = romfs_checkmount(rm);
+ if (ret != OK)
+ {
+ goto errout_with_semaphore;
+ }
+
+ /* Loop, skipping over unsupported items in the file system */
+
+ for (;;)
+ {
+ /* Have we reached the end of the directory */
+
+ if (!dir->u.romfs.fr_curroffset)
+ {
+ /* We signal the end of the directory by returning the
+ * special error -ENOENT
+ */
+
+ ret = -ENOENT;
+ goto errout_with_semaphore;
+ }
+
+ /* Parse the directory entry */
+
+ ret = romfs_parsedirentry(rm, dir->u.romfs.fr_curroffset, &linkoffset,
+ &next, &info, &size);
+ if (ret < 0)
+ {
+ goto errout_with_semaphore;
+ }
+
+ /* Save the filename */
+
+ ret = romfs_parsefilename(rm, dir->u.romfs.fr_curroffset, dir->fd_dir.d_name);
+ if (ret < 0)
+ {
+ goto errout_with_semaphore;
+ }
+
+ /* Set up the next directory entry offset */
+
+ dir->u.romfs.fr_curroffset = next & RFNEXT_OFFSETMASK;
+
+ /* Check the file type */
+
+ if (IS_DIRECTORY(next))
+ {
+ dir->fd_dir.d_type = DTYPE_DIRECTORY;
+ break;
+ }
+ else if (IS_DIRECTORY(next))
+ {
+ dir->fd_dir.d_type = DTYPE_FILE;
+ break;
+ }
+ }
+
+errout_with_semaphore:
+ romfs_semgive(rm);
+ return ret;
+}
+
+/****************************************************************************
+ * Name: romfs_rewindir
+ *
+ * Description: Reset directory read to the first entry
+ *
+ ****************************************************************************/
+
+static int romfs_rewinddir(struct inode *mountpt, struct internal_dir_s *dir)
+{
+ struct romfs_mountpt_s *rm;
+ int ret;
+
+ /* Sanity checks */
+
+ DEBUGASSERT(mountpt != NULL && mountpt->i_private != NULL);
+
+ /* Recover our private data from the inode instance */
+
+ rm = mountpt->i_private;
+
+ /* Make sure that the mount is still healthy */
+
+ romfs_semtake(rm);
+ ret = romfs_checkmount(rm);
+ if (ret == OK)
+ {
+ dir->u.romfs.fr_curroffset = dir->u.romfs.fr_firstoffset;
+ }
+
+ romfs_semgive(rm);
+ return ret;
+}
+
+/****************************************************************************
+ * Name: romfs_bind
+ *
+ * Description: This implements a portion of the mount operation. This
+ * function allocates and initializes the mountpoint private data and
+ * binds the blockdriver inode to the filesystem private data. The final
+ * binding of the private data (containing the blockdriver) to the
+ * mountpoint is performed by mount().
+ *
+ ****************************************************************************/
+
+static int romfs_bind(FAR struct inode *blkdriver, const void *data,
+ void **handle)
+{
+ struct romfs_mountpt_s *rm;
+ int ret;
+
+ /* Open the block driver */
+
+ if (!blkdriver || !blkdriver->u.i_bops)
+ {
+ return -ENODEV;
+ }
+
+ if (blkdriver->u.i_bops->open &&
+ blkdriver->u.i_bops->open(blkdriver) != OK)
+ {
+ return -ENODEV;
+ }
+
+ /* Create an instance of the mountpt state structure */
+
+ rm = (struct romfs_mountpt_s *)zalloc(sizeof(struct romfs_mountpt_s));
+ if (!rm)
+ {
+ return -ENOMEM;
+ }
+
+ /* Initialize the allocated mountpt state structure. The filesystem is
+ * responsible for one reference ont the blkdriver inode and does not
+ * have to addref() here (but does have to release in ubind().
+ */
+
+ sem_init(&rm->rm_sem, 0, 0); /* Initialize the semaphore that controls access */
+ rm->rm_blkdriver = blkdriver; /* Save the block driver reference */
+ rm->rm_cachesector = (uint32)-1; /* No sector in the cache */
+
+ /* Get the hardware configuration */
+
+ ret = romfs_getgeometry(rm);
+ if (ret < 0)
+ {
+ goto errout_with_sem;
+ }
+
+ /* Allocate the device cache buffer */
+
+ rm->rm_buffer = (ubyte*)malloc(rm->rm_hwsectorsize);
+ if (!rm->rm_buffer)
+ {
+ ret = -ENOMEM;
+ goto errout_with_sem;
+ }
+
+ /* Then complete the mount */
+
+ ret = romfs_mount(rm);
+ if (ret < 0)
+ {
+ goto errout_with_buffer;
+ }
+
+ /* Mounted */
+
+ *handle = (void*)rm;
+ romfs_semgive(rm);
+ return OK;
+
+errout_with_buffer:
+ free(rm->rm_buffer);
+errout_with_sem:
+ sem_destroy(&rm->rm_sem);
+ free(rm);
+ return ret;
+}
+
+/****************************************************************************
+ * Name: romfs_unbind
+ *
+ * Description: This implements the filesystem portion of the umount
+ * operation.
+ *
+ ****************************************************************************/
+
+static int romfs_unbind(void *handle, FAR struct inode **blkdriver)
+{
+ struct romfs_mountpt_s *rm = (struct romfs_mountpt_s*)handle;
+ int ret;
+
+ if (!rm)
+ {
+ return -EINVAL;
+ }
+
+ /* Check if there are sill any files opened on the filesystem. */
+
+ ret = OK; /* Assume success */
+ romfs_semtake(rm);
+ if (rm->rm_head)
+ {
+ /* We cannot unmount now.. there are open files */
+
+ ret = -EBUSY;
+ }
+ else
+ {
+ /* Unmount ... close the block driver */
+
+ if (rm->rm_blkdriver)
+ {
+ struct inode *inode = rm->rm_blkdriver;
+ if (inode)
+ {
+ if (inode->u.i_bops && inode->u.i_bops->close)
+ {
+ (void)inode->u.i_bops->close(inode);
+ }
+
+ /* We hold a reference to the block driver but should
+ * not but mucking with inodes in this context. So, we will just return
+ * our contained reference to the block driver inode and let the umount
+ * logic dispose of it.
+ */
+
+ if (blkdriver)
+ {
+ *blkdriver = inode;
+ }
+ }
+ }
+
+ /* Release the mountpoint private data */
+
+ if (rm->rm_buffer)
+ {
+ free(rm->rm_buffer);
+ }
+
+ sem_destroy(&rm->rm_sem);
+ free(rm);
+ }
+
+ romfs_semgive(rm);
+ return ret;
+}
+
+/****************************************************************************
+ * Name: romfs_statfs
+ *
+ * Description: Return filesystem statistics
+ *
+ ****************************************************************************/
+
+static int romfs_statfs(struct inode *mountpt, struct statfs *buf)
+{
+ struct romfs_mountpt_s *rm;
+ int ret;
+
+ /* Sanity checks */
+
+ DEBUGASSERT(mountpt && mountpt->i_private);
+
+ /* Get the mountpoint private data from the inode structure */
+
+ rm = mountpt->i_private;
+
+ /* Check if the mount is still healthy */
+
+ romfs_semtake(rm);
+ ret = romfs_checkmount(rm);
+ if (ret < 0)
+ {
+ goto errout_with_semaphore;
+ }
+
+ /* Fill in the statfs info */
+
+ memset(buf, 0, sizeof(struct statfs));
+ buf->f_type = ROMFS_MAGIC;
+
+ /* We will claim that the optimal transfer size is the size of one sector */
+
+ buf->f_bsize = rm->rm_hwsectorsize;
+
+ /* Everything else follows in units of sectors */
+
+ buf->f_blocks = SEC_NSECTORS(rm, rm->rm_volsize + SEC_NDXMASK(rm));
+ buf->f_bfree = 0;
+ buf->f_bavail = 0;
+ buf->f_namelen = NAME_MAX;
+
+ romfs_semgive(rm);
+ return OK;
+
+errout_with_semaphore:
+ romfs_semgive(rm);
+ return ret;
+}
+
+/****************************************************************************
+ * Name: romfs_stat
+ *
+ * Description: Return information about a file or directory
+ *
+ ****************************************************************************/
+
+static int romfs_stat(struct inode *mountpt, const char *relpath, struct stat *buf)
+{
+ struct romfs_mountpt_s *rm;
+ struct romfs_dirinfo_s dirinfo;
+ uint16 date;
+ uint16 date2;
+ uint16 time;
+ ubyte attribute;
+ int ret;
+
+ /* Sanity checks */
+
+ DEBUGASSERT(mountpt && mountpt->i_private);
+
+ /* Get the mountpoint private data from the inode structure */
+
+ rm = mountpt->i_private;
+
+ /* Check if the mount is still healthy */
+
+ romfs_semtake(rm);
+ ret = romfs_checkmount(rm);
+ if (ret != OK)
+ {
+ goto errout_with_semaphore;
+ }
+
+ /* Find the directory entry corresponding to relpath. */
+
+ ret = romfs_finddirentry(rm, &dirinfo, relpath);
+
+ /* If nothing was found, then we fail with EEXIST */
+
+ if (ret < 0)
+ {
+ goto errout_with_semaphore;
+ }
+
+ memset(buf, 0, sizeof(struct stat));
+ if (IS_DIRECTORY(dirinfo.rd_next))
+ {
+ /* It's a read-only directory name */
+
+ buf->st_mode = S_IFDIR|S_IROTH|S_IRGRP|S_IRUSR;
+ if (IS_EXECUTABLE(dirinfo.rd_next))
+ {
+ buf->st_mode |= S_IXOTH|S_IXGRP|S_IXUSR;
+ }
+ }
+ else if (IS_FILE(dirinfo.rd_next))
+ {
+ /* It's a read-only file name */
+
+ buf->st_mode = S_IFREG|S_IROTH|S_IRGRP|S_IRUSR;
+ if (IS_EXECUTABLE(dirinfo.rd_next))
+ {
+ buf->st_mode |= S_IXOTH|S_IXGRP|S_IXUSR;
+ }
+ }
+ else
+ {
+ /* Otherwise, pretend like the unsupported node does not exist */
+
+ ret = -ENOENT;
+ goto errout_with_semaphore;
+ }
+
+ /* File/directory size, access block size */
+
+ buf->st_size = dirinfo.rd_size;
+ buf->st_blksize = rm->rm_hwsectorsize;
+ buf->st_blocks = (buf->st_size + buf->st_blksize - 1) / buf->st_blksize;
+
+ ret = OK;
+
+errout_with_semaphore:
+ romfs_semgive(rm);
+ return ret;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
diff --git a/nuttx/fs/romfs/fs_romfs.h b/nuttx/fs/romfs/fs_romfs.h
new file mode 100644
index 000000000..40e166f84
--- /dev/null
+++ b/nuttx/fs/romfs/fs_romfs.h
@@ -0,0 +1,221 @@
+/****************************************************************************
+ * fs/romfs/fs_romfs.h
+ *
+ * Copyright (C) 2008 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * References: Linux/Documentation/filesystems/romfs.txt
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef __FS_ROMFS_FS_ROMFS_H
+#define __FS_ROMFS_FS_ROMFS_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <sys/types.h>
+#include "../fs_internal.h"
+
+/****************************************************************************
+ * Definitions
+ ****************************************************************************/
+
+/* Volume header (multi-byte values are big-endian) */
+
+#define ROMFS_VHDR_ROM1FS 0 /* 0-7: "-rom1fs-" */
+#define ROMFS_VHDR_SIZE 8 /* 8-11: Number of accessible bytes in this fs. */
+#define ROMFS_VHDR_CHKSUM 12 /* 12-15: Checksum of the first 512 bytes. */
+#define ROMFS_VHDR_VOLNAME 16 /* 16-..: Zero terminated volume name, padded to
+ * 16 byte boundary. */
+
+#define ROMFS_VHDR_MAGIC "-rom1fs-"
+
+/* File header offset (multi-byte values are big-endian) */
+
+#define ROMFS_FHDR_NEXT 0 /* 0-3: Offset of the next file header
+ * (zero if no more files) */
+#define ROMFS_FHDR_INFO 4 /* 4-7: Info for directories/hard links/
+ * devices */
+#define ROMFS_FHDR_SIZE 8 /* 8-11: Size of this file in bytes */
+#define ROMFS_FHDR_CHKSUM 12 /* 12-15: Checksum covering the meta data,
+ * including the file name, and
+ * padding. */
+#define ROMFS_FHDR_NAME 16 /* 16-..: Zero terminated volume name, padded
+ * to 16 byte boundary. */
+
+/* Bits 0-3 of the rf_next offset provide mode information. These are the
+ * values specified in */
+
+#define RFNEXT_MODEMASK 7 /* Bits 0-2: Mode; bit 3: Executable */
+#define RFNEXT_OFFSETMASK (~7) /* Bits n-3: Offset to next entry */
+
+#define RFNEXT_HARDLINK 0 /* rf_info = Link destination file header */
+#define RFNEXT_DIRECTORY 1 /* rf_info = First file's header */
+#define RFNEXT_FILE 2 /* rf_info = Unused, must be zero */
+#define RFNEXT_SOFTLINK 3 /* rf_info = Unused, must be zero */
+#define RFNEXT_BLOCKDEV 4 /* rf_info = 16/16 bits major/minor number */
+#define RFNEXT_CHARDEV 5 /* rf_info = 16/16 bits major/minor number */
+#define RFNEXT_SOCKET 6 /* rf_info = Unused, must be zero */
+#define RFNEXT_FIFO 7 /* rf_info = Unused, must be zero */
+#define RFNEXT_EXEC 8 /* Modifier of RFNEXT_DIRECTORY and RFNEXT_FILE */
+
+#define IS_MODE(rfn,mode) ((((uint32)(rfn))&RFNEXT_MODEMASK)==(mode))
+#define IS_HARDLINK(rfn) IS_MODE(rfn,RFNEXT_HARDLINK)
+#define IS_DIRECTORY(rfn) IS_MODE(rfn,RFNEXT_DIRECTORY)
+#define IS_FILE(rfn) IS_MODE(rfn,RFNEXT_FILE)
+#define IS_EXECUTABLE(rfn) (((rfn) & RFNEXT_EXEC) != 0)
+
+/* RFNEXT_SOFTLINK, RFNEXT_BLOCKDEV, RFNEXT_CHARDEV, RFNEXT_SOCKET, and
+ * RFNEXT_FIFO are not presently supported in NuttX.
+ */
+
+/* Alignment macros */
+
+#define ROMFS_ALIGNMENT 16
+#define ROMFS_MAXPADDING (ROMFS_ALIGNMENT-1)
+#define ROMFS_ALIGNMASK (~ROMFS_MAXPADDING)
+#define ROMFS_ALIGNUP(addr) ((((uint32)(addr))+ROMFS_MAXPADDING)&ROMFS_ALIGNMASK)
+#define ROMFS_ALIGNDOWN(addr) (((uint32)(addr))&ROMFS_ALIGNMASK)
+
+/* Offset and sector conversions */
+
+#define SEC_NDXMASK(r) ((r)->rm_hwsectorsize - 1)
+#define SEC_NSECTORS(r,o) ((o) / (r)->rm_hwsectorsize)
+
+/* Maximum numbr of links that will be followed before we decide that there
+ * is a problem.
+ */
+
+#define ROMF_MAX_LINKS 64
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+/* This structure represents the overall mountpoint state. An instance of this
+ * structure is retained as inode private data on each mountpoint that is
+ * mounted with a fat32 filesystem.
+ */
+
+struct romfs_file_s;
+struct romfs_mountpt_s
+{
+ struct inode *rm_blkdriver; /* The block driver inode that hosts the FAT32 fs */
+ struct romfs_file_s *rm_head; /* A list to all files opened on this mountpoint */
+
+ boolean rm_mounted; /* TRUE: The file system is ready */
+ uint16 rm_hwsectorsize; /* HW: Sector size reported by block driver*/
+ sem_t rm_sem; /* Used to assume thread-safe access */
+ uint32 rm_rootoffset; /* Saved offset to the first root directory entry */
+ uint32 rm_hwnsectors; /* HW: The number of sectors reported by the hardware */
+ uint32 rm_volsize; /* Size of the ROMFS volume */
+ uint32 rm_cachesector; /* Current sector in the rm_buffer */
+ ubyte *rm_buffer; /* Device sector buffer */
+};
+
+/* This structure represents on open file under the mountpoint. An instance
+ * of this structure is retained as struct file specific information on each
+ * opened file.
+ */
+
+struct romfs_file_s
+{
+ struct romfs_file_s *rf_next; /* Retained in a singly linked list */
+ boolean rf_open; /* TRUE: The file is (still) open */
+ uint32 rf_diroffset; /* Offset to the parent directory entry */
+ uint32 rf_startoffset; /* Offset to the start of the file */
+ uint32 rf_size; /* Size of the file in bytes */
+ uint32 rf_cachesector; /* Current sector in the rf_buffer */
+ ubyte *rf_buffer; /* File sector buffer */
+};
+
+/* This structure is used internally for describing the result of
+ * walking a path
+ */
+
+struct romfs_dirinfo_s
+{
+ /* These values describe the directory containing the terminal
+ * path component (of the terminal component itself if it is
+ * a directory.
+ */
+
+ struct fs_romfsdir_s rd_dir; /* Describes directory. */
+
+ /* Values from the ROMFS file entry */
+
+ uint32 rd_next; /* Offset of the next file header+flags */
+ uint32 rd_size; /* Size (if file) */
+};
+
+/****************************************************************************
+ * Global Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+EXTERN void romfs_semtake(struct romfs_mountpt_s *rm);
+EXTERN void romfs_semgive(struct romfs_mountpt_s *rm);
+EXTERN int romfs_hwread(struct romfs_mountpt_s *rm, ubyte *buffer,
+ uint32 sector, unsigned int nsectors);
+EXTERN int romfs_devcacheread(struct romfs_mountpt_s *rm, uint32 sector);
+EXTERN int romfs_filecacheread(struct romfs_mountpt_s *rm,
+ struct romfs_file_s *rf, uint32 sector);
+EXTERN int romfs_getgeometry(struct romfs_mountpt_s *rm);
+EXTERN int romfs_mount(struct romfs_mountpt_s *rm);
+EXTERN int romfs_checkmount(struct romfs_mountpt_s *rm);
+EXTERN int romfs_finddirentry(struct romfs_mountpt_s *rm,
+ struct romfs_dirinfo_s *dirinfo,
+ const char *path);
+EXTERN int romfs_parsedirentry(struct romfs_mountpt_s *rm,
+ uint32 offset, uint32 *poffset, uint32 *pnext,
+ uint32 *pinfo, uint32 *psize);
+EXTERN int romfs_parsefilename(struct romfs_mountpt_s *rm, uint32 offset,
+ char *pname);
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __FS_ROMFS_FS_ROMFS_H */
diff --git a/nuttx/fs/romfs/fs_romfsutil.c b/nuttx/fs/romfs/fs_romfsutil.c
new file mode 100644
index 000000000..cbd10e3bd
--- /dev/null
+++ b/nuttx/fs/romfs/fs_romfsutil.c
@@ -0,0 +1,775 @@
+/****************************************************************************
+ * rm/romfs/fs_romfsutil.h
+ *
+ * Copyright (C) 2008 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * References: Linux/Documentation/filesystems/romfs.txt
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <sys/types.h>
+
+#include <string.h>
+#include <assert.h>
+#include <errno.h>
+#include <debug.h>
+
+#include "fs_romfs.h"
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Variables
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: romfs_swap32
+ *
+ * Desciption:
+ * Convert the 32-bit big endian value to little endian
+ *
+ ****************************************************************************/
+
+#ifndef CONFIG_ENDIAN_BIG
+static inline uint32 romfs_swap32(uint32 value)
+{
+ return ((((value) & 0x000000ff) << 24) | (((value) & 0x0000ff00) << 8) |
+ (((value) & 0x00ff0000) >> 8) | (((value) & 0xff000000) >> 24));
+}
+#endif
+
+/****************************************************************************
+ * Name: romfs_devread32
+ *
+ * Desciption:
+ * Read the big-endian 32-bit value from the mount device buffer
+ *
+ * Assumption:
+ * All values are aligned to 32-bit boundaries
+ *
+ ****************************************************************************/
+
+static uint32 romfs_devread32(struct romfs_mountpt_s *rm, int ndx)
+{
+ /* Extract the value */
+
+ uint32 value = *(uint32*)&rm->rm_buffer[ndx];
+
+ /* Value is begin endian -- return the native host endian-ness. */
+#ifdef CONFIG_ENDIAN_BIG
+ return value;
+#else
+ return romfs_swap32(value);
+#endif
+}
+
+/****************************************************************************
+ * Name: romfs_checkentry
+ *
+ * Desciption:
+ * Check if the entry at offset is a directory or file path segment
+ *
+ ****************************************************************************/
+
+static inline int romfs_checkentry(struct romfs_mountpt_s *rm, uint32 offset,
+ const char *entryname, int entrylen,
+ struct romfs_dirinfo_s *dirinfo)
+{
+ char name[NAME_MAX+1];
+ uint32 linkoffset;
+ uint32 next;
+ uint32 info;
+ uint32 size;
+ int ret;
+
+ /* Parse the directory entry at this offset (which may be re-directed
+ * to some other entry if HARLINKED).
+ */
+
+ ret = romfs_parsedirentry(rm, offset, &linkoffset, &next, &info, &size);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Now we are pointing to the real entry of interest. Is it a
+ * directory? Or a file?
+ */
+
+ if (IS_DIRECTORY(next) || IS_FILE(next))
+ {
+ /* Get the name of the directory entry. */
+
+ ret = romfs_parsefilename(rm, offset, name);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Then check if this the name segment we are looking for. The
+ * string comparison is awkward because there is no terminator
+ * on entryname (there is a terminator on name, however)
+ */
+
+ if (memcmp(entryname, name, entrylen) == 0 &&
+ strlen(name) == entrylen)
+ {
+ /* Found it -- save the component info and return success */
+
+ if (IS_DIRECTORY(next))
+ {
+ dirinfo->rd_dir.fr_diroffset = linkoffset;
+ dirinfo->rd_dir.fr_firstoffset = info;
+ dirinfo->rd_dir.fr_curroffset = info;
+ dirinfo->rd_size = 0;
+ }
+ else
+ {
+ dirinfo->rd_dir.fr_curroffset = offset;
+ dirinfo->rd_size = size;
+ }
+ dirinfo->rd_next = next;
+ return OK;
+ }
+ }
+
+ /* The entry is not a directory or it does not have the matching name */
+
+ return -ENOENT;
+}
+
+/****************************************************************************
+ * Name: romfs_searchdir
+ *
+ * Desciption:
+ * This is part of the romfs_finddirentry log. Search the directory
+ * beginning at dirinfo->fr_firstoffset for entryname.
+ *
+ ****************************************************************************/
+
+static inline int romfs_searchdir(struct romfs_mountpt_s *rm,
+ const char *entryname, int entrylen,
+ struct romfs_dirinfo_s *dirinfo)
+{
+ uint32 offset;
+ uint32 sector;
+ uint32 next;
+ uint16 ndx;
+ int ret;
+
+ /* Then loop through the current directory until the directory
+ * with the matching name is found.
+ */
+
+ offset = dirinfo->rd_dir.fr_firstoffset;
+ for (;;)
+ {
+ /* Convert the offset into sector + index */
+
+ sector = SEC_NSECTORS(rm, offset);
+ ndx = offset & SEC_NDXMASK(rm);
+
+ /* Read the sector into memory (do this before calling
+ * romfs_checkentry() so we won't have to read the sector
+ * twice in the event that the offset refers to a hardlink).
+ */
+
+ ret = romfs_devcacheread(rm, sector);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Because everything is chunked and aligned to 16-bit boundaries,
+ * we know that most the basic node info fits into the sector.
+ */
+
+ next = romfs_devread32(rm, ndx + ROMFS_FHDR_NEXT) & RFNEXT_OFFSETMASK;
+
+ /* Check if the name this entry is a directory with the matching
+ * name
+ */
+
+ ret = romfs_checkentry(rm, offset, entryname, entrylen, dirinfo);
+ if (ret == OK)
+ {
+ /* Its a match! Return success */
+
+ return OK;
+ }
+
+ /* No match... select the offset to the next entry */
+
+ offset += next;
+ }
+ while (next != 0)
+
+ /* There is nothing in this directoy with that name */
+
+ return -ENOENT;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: romfs_semtake
+ ****************************************************************************/
+
+void romfs_semtake(struct romfs_mountpt_s *rm)
+{
+ /* Take the semaphore (perhaps waiting) */
+
+ while (sem_wait(&rm->rm_sem) != 0)
+ {
+ /* The only case that an error should occur here is if
+ * the wait was awakened by a signal.
+ */
+
+ ASSERT(*get_errno_ptr() == EINTR);
+ }
+}
+
+/****************************************************************************
+ * Name: romfs_semgive
+ ****************************************************************************/
+
+void romfs_semgive(struct romfs_mountpt_s *rm)
+{
+ sem_post(&rm->rm_sem);
+}
+
+/****************************************************************************
+ * Name: romfs_hwread
+ *
+ * Desciption: Read the specified sector into the sector buffer
+ *
+ ****************************************************************************/
+
+int romfs_hwread(struct romfs_mountpt_s *rm, ubyte *buffer, uint32 sector,
+ unsigned int nsectors)
+{
+ struct inode *inode;;
+ ssize_t nsectorsread;
+ int ret = -ENODEV;
+
+ if (rm && rm->rm_blkdriver)
+ {
+ inode = rm->rm_blkdriver;
+ if (inode && inode->u.i_bops && inode->u.i_bops->read)
+ {
+ nsectorsread =
+ inode->u.i_bops->read(inode, buffer, sector, nsectors);
+
+ if (nsectorsread == (ssize_t)nsectors)
+ {
+ ret = OK;
+ }
+ else if (nsectorsread < 0)
+ {
+ ret = nsectorsread;
+ }
+ }
+ }
+ return ret;
+}
+
+/****************************************************************************
+ * Name: romfs_devcacheread
+ *
+ * Desciption:
+ * Read the specified sector into the sector cache
+ *
+ ****************************************************************************/
+
+int romfs_devcacheread(struct romfs_mountpt_s *rm, uint32 sector)
+{
+ int ret;
+
+ /* rm->rm_cachesector holds the current sector that is buffered in
+ * rm->tm_buffer. If the requested sector is the same as this sector, then
+ * we do nothing. Otherwise, we will have to read the new sector.
+ */
+
+ if (rm->rm_cachesector != sector)
+ {
+ ret = romfs_hwread(rm, rm->rm_buffer, sector, 1);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Update the cached sector number */
+
+ rm->rm_cachesector = sector;
+ }
+ return OK;
+}
+
+/****************************************************************************
+ * Name: romfs_filecacheread
+ *
+ * Desciption:
+ * Read the specified sector into the sector cache
+ *
+ ****************************************************************************/
+
+int romfs_filecacheread(struct romfs_mountpt_s *rm, struct romfs_file_s *rf, uint32 sector)
+{
+ int ret;
+
+ /* rf->rf_cachesector holds the current sector that is buffered in
+ * rf->rf_buffer. If the requested sector is the same as this sector, then
+ * we do nothing. Otherwise, we will have to read the new sector.
+ */
+
+ if (rf->rf_cachesector != sector)
+ {
+ ret = romfs_hwread(rm, rf->rf_buffer, sector, 1);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Update the cached sector number */
+
+ rf->rf_cachesector = sector;
+ }
+ return OK;
+}
+
+/****************************************************************************
+ * Name: romfs_getgeometry
+ *
+ * Desciption:
+ * Get the geometry of the device (part of the mount operation)
+ *
+ ****************************************************************************/
+
+int romfs_getgeometry(struct romfs_mountpt_s *rm)
+{
+ struct inode *inode = rm->rm_blkdriver;
+ struct geometry geo;
+ int ret;
+
+ /* Get the underlying device geometry */
+
+ if (!inode || !inode->u.i_bops || !inode->u.i_bops->geometry)
+ {
+ return -ENODEV;
+ }
+
+ ret = inode->u.i_bops->geometry(inode, &geo);
+ if (ret != OK)
+ {
+ return ret;
+ }
+
+ if (!geo.geo_available)
+ {
+ return -EBUSY;
+ }
+
+ /* Save that information in the mount structure */
+
+ rm->rm_hwsectorsize = geo.geo_sectorsize;
+ rm->rm_hwnsectors = geo.geo_nsectors;
+ return OK;
+}
+
+/****************************************************************************
+ * Name: romfs_mount
+ *
+ * Desciption:
+ * Setup ROMFS on the block driver
+ *
+ ****************************************************************************/
+
+int romfs_mount(struct romfs_mountpt_s *rm)
+{
+ const char *name;
+ int ret;
+
+ /* Then get information about the ROMFS filesystem on the devices managed
+ * by this block driver. Read sector zero which contains the volume header.
+ */
+
+ ret = romfs_devcacheread(rm, 0);
+ if (ret != 0)
+ {
+ return ret;
+ }
+
+ /* Verify the magic number at that identifies this as a ROMFS filesystem */
+
+ if (memcmp(rm->rm_buffer, ROMFS_VHDR_MAGIC, 8) != 0)
+ {
+ return -EINVAL;
+ }
+
+ /* Then extract the values we need from the header and return success */
+
+ rm->rm_volsize = romfs_devread32(rm, ROMFS_VHDR_SIZE);
+
+ /* The root directory entry begins right after the header */
+
+ name = (const char*)&rm->rm_buffer[ROMFS_VHDR_VOLNAME];
+ rm->rm_rootoffset = ROMFS_ALIGNUP(ROMFS_VHDR_VOLNAME + strlen(name) + 1);
+
+ /* and return success */
+
+ rm->rm_mounted = TRUE;
+ return OK;
+}
+
+/****************************************************************************
+ * Name: romfs_checkmount
+ *
+ * Desciption: Check if the mountpoint is still valid.
+ *
+ * The caller should hold the mountpoint semaphore
+ *
+ ****************************************************************************/
+
+int romfs_checkmount(struct romfs_mountpt_s *rm)
+{
+ /* If the fs_mounted flag is FALSE, then we have already handled the loss
+ * of the mount.
+ */
+
+ if (rm && rm->rm_mounted)
+ {
+ struct romfs_file_s *file;
+
+ /* We still think the mount is healthy. Check an see if this is
+ * still the case
+ */
+
+ if (rm->rm_blkdriver)
+ {
+ struct inode *inode = rm->rm_blkdriver;
+ if (inode && inode->u.i_bops && inode->u.i_bops->geometry)
+ {
+ struct geometry geo;
+ int errcode = inode->u.i_bops->geometry(inode, &geo);
+ if (errcode == OK && geo.geo_available && !geo.geo_mediachanged)
+ {
+ return OK;
+ }
+ }
+ }
+
+ /* If we get here, the mount is NOT healthy */
+
+ rm->rm_mounted = FALSE;
+
+ /* Make sure that this is flagged in every opened file */
+
+ for (file = rm->rm_head; file; file = file->rf_next)
+ {
+ file->rf_open = FALSE;
+ }
+ }
+ return -ENODEV;
+}
+
+/****************************************************************************
+ * Name: romfs_finddirentry
+ *
+ * Desciption:
+ * Given a path to something that may or may not be in the file system,
+ * return the directory entry of the item.
+ *
+ ****************************************************************************/
+
+int romfs_finddirentry(struct romfs_mountpt_s *rm, struct romfs_dirinfo_s *dirinfo,
+ const char *path)
+{
+ const char *entryname;
+ const char *terminator;
+ int entrylen;
+ int ret;
+
+ /* Start with the first element after the root directory */
+
+ dirinfo->rd_dir.fr_diroffset = 0;
+ dirinfo->rd_dir.fr_firstoffset = rm->rm_rootoffset;
+ dirinfo->rd_dir.fr_curroffset = rm->rm_rootoffset;
+ dirinfo->rd_next = 0;
+ dirinfo->rd_size = 0;
+
+ /* Then loop for each directory/file component in the full path */
+
+ entryname = path;
+ terminator = NULL;
+
+ for (;;)
+ {
+ /* Find the start of the next path component */
+
+ while (*entryname == '/') entryname++;
+
+ /* Find the end of the next path component */
+
+ terminator = strchr(entryname, '/');
+ if (!terminator)
+ {
+ entrylen = strlen(entryname);
+ }
+ else
+ {
+ entrylen = terminator - entryname;
+ }
+
+ /* Long path segment names will be truncated to NAME_MAX */
+
+ if (entrylen > NAME_MAX)
+ {
+ entrylen = NAME_MAX;
+ }
+
+ /* Then find the entry in the current directory with the
+ * matching name.
+ */
+
+ ret = romfs_searchdir(rm, entryname, entrylen, dirinfo);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Was that the last path component? */
+
+ if (!terminator)
+ {
+ /* Yes.. return success */
+
+ return OK;
+ }
+
+ /* No... If that was not the last path component, then it had
+ * better have been a directory
+ */
+
+ if (!IS_DIRECTORY(dirinfo->rd_next))
+ {
+ return -ENOTDIR;
+ }
+
+ /* Setup to search the next directory for the next component
+ * of the path
+ */
+
+ entryname = terminator;
+ }
+
+ return ERROR; /* Won't get here */
+}
+
+/****************************************************************************
+ * Name: romfs_parsedirentry
+ *
+ * Desciption:
+ * Return the directory entry at this offset. If rf is NULL, then the
+ * mount device resources are used. Otherwise, file resources are used.
+ *
+ ****************************************************************************/
+
+int romfs_parsedirentry(struct romfs_mountpt_s *rm, uint32 offset, uint32 *poffset,
+ uint32 *pnext, uint32 *pinfo, uint32 *psize)
+{
+ uint32 sector;
+ uint32 save;
+ uint32 next;
+ uint32 info;
+ uint32 size;
+ uint16 ndx;
+ int ret;
+ int i;
+
+ /* Loop while we are redirected by hardlinks */
+
+ for (i = 0; i < ROMF_MAX_LINKS; i++)
+ {
+ /* Convert the offset into sector + index */
+
+ sector = SEC_NSECTORS(rm, offset);
+ ndx = offset & SEC_NDXMASK(rm);
+
+ /* Read the sector into memory */
+
+ ret = romfs_devcacheread(rm, sector);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Because everything is chunked and aligned to 16-bit boundaries,
+ * we know that most the basic node info fits into the sector. The
+ * associated name may not, however.
+ */
+
+ next = romfs_devread32(rm, ndx + ROMFS_FHDR_NEXT);
+ info = romfs_devread32(rm, ndx + ROMFS_FHDR_INFO);
+ size = romfs_devread32(rm, ndx + ROMFS_FHDR_SIZE);
+
+ /* Is this the first offset we have tried? */
+
+ if (i == 0)
+ {
+ /* Yes.. Save the first 'next' value. That has the offset needed to
+ * traverse the parent directory.
+ */
+
+ save = next;
+ }
+
+ /* Is this a hardlink? */
+
+ if (IS_HARDLINK(next))
+ {
+ /* Yes.. then the info field is the offset to the actual entry
+ * that we are interested in.
+ */
+
+ offset = info;
+ }
+ else
+ {
+ /* No... then break returning the directory information.
+ * Retaining the original offset
+ */
+
+ *poffset = offset;
+ *pnext = (save & RFNEXT_OFFSETMASK) | (next & RFNEXT_MODEMASK);
+ *pinfo = info;
+ *psize = size;
+ return OK;
+ }
+ }
+
+ /* Too many hard links -- probably an infinite loop */
+
+ return -ELOOP;
+}
+
+/****************************************************************************
+ * Name: romfs_parsefilename
+ *
+ * Desciption:
+ * Return the filename from directory entry at this offset
+ *
+ ****************************************************************************/
+
+int romfs_parsefilename(struct romfs_mountpt_s *rm, uint32 offset, char *pname)
+{
+ uint32 sector;
+ uint16 ndx;
+ uint16 namelen;
+ uint16 chunklen;
+ boolean done;
+ int ret;
+
+ /* Loop until the whole name is obtained or until NAME_MAX characters
+ * of the name have been parsed.
+ */
+
+ offset += ROMFS_FHDR_NAME;
+ for (namelen = 0, done = FALSE; namelen < NAME_MAX && !done;)
+ {
+ /* Convert the offset into sector + index */
+
+ sector = SEC_NSECTORS(rm, offset);
+ ndx = offset & SEC_NDXMASK(rm);
+
+ /* Read the sector into memory */
+
+ ret = romfs_devcacheread(rm, sector);
+ if (ret < 0)
+ {
+ return ret;
+ }
+
+ /* Is the name terminated in this 16-byte block */
+
+ if (rm->rm_buffer[ndx + 15] == '\0')
+ {
+ /* Yes.. then this chunk is less than 16 */
+
+ chunklen = strlen((char*)&rm->rm_buffer[ndx]);
+ done = TRUE;
+ }
+ else
+ {
+ /* No.. then this chunk is 16 bytes in length */
+
+ chunklen = 16;
+ }
+
+ /* Check if we would exceed the NAME_MAX */
+
+ if (namelen + chunklen > NAME_MAX)
+ {
+ chunklen = NAME_MAX - namelen;
+ done = TRUE;
+ }
+
+ /* Copy the chunk */
+
+ memcpy(&pname[namelen], &rm->rm_buffer[ndx], chunklen);
+ namelen += chunklen;
+ }
+
+ /* Terminate the name (NAME_MAX+1 chars total) and return success */
+
+ pname[namelen] = '\0';
+ return OK;
+}