utils.c

   1 #include <stdio.h>
   2 #include <fcntl.h>
   3 #include <unistd.h>
   4 #include <sys/stat.h>
   5
   6 #include "tfs.h"
   7
   8 static int fd;
   9
  10 /*
  11  * Open the fs image and return the size of the fs image
  12  */
  13 uint32_t open_fs(char *fs)
  14 {
  15         struct stat stat;
  16
  17         fd = open(fs, O_RDWR);
  18         if (fd < 0) {
  19                 printf("open error\n");
  20                 return;
  21         }
  22         fstat(fd, &stat);
  23
  24         return stat.st_size;
  25 }
  26
  27 /*
  28  * Set the bit and return it's original value
  29  */
  30 int set_bit(void * addr, unsigned int nr)
  31 {
  32         int             mask, retval;
  33         unsigned char   *ADDR = (unsigned char *) addr;
  34
  35         ADDR += nr >> 3;
  36         mask = 1 << (nr & 0x07);
  37         retval = mask & *ADDR;
  38         *ADDR |= mask;
  39         return retval;
  40 }
  41
  42 /*
  43  * Clear the bit and return it's original value
  44  */
  45 int clear_bit(void * addr, unsigned int nr)
  46 {
  47         int             mask, retval;
  48         unsigned char   *ADDR = (unsigned char *) addr;
  49
  50         ADDR += nr >> 3;
  51         mask = 1 << (nr & 0x07);
  52         retval = mask & *ADDR;
  53         *ADDR &= ~mask;
  54         return retval;
  55 }
  56
  57 int test_bit(const void * addr, unsigned int nr)
  58 {
  59         int                     mask;
  60         const unsigned char     *ADDR = (const unsigned char *) addr;
  61
  62         ADDR += nr >> 3;
  63         mask = 1 << (nr & 0x07);
  64         return (mask & *ADDR);
  65 }
  66
  67 uint32_t find_first_zero(void *buf, void *end)
  68 {
  69         uint32_t *p = (uint32_t *)buf;
  70         uint32_t block = 0;
  71         int i;
  72
  73         while (*p == 0xffffffff && (void *)p < end) {
  74                 p++;
  75                 block += 32;
  76         }
  77         if ((void *)p >= end)
  78                 return -1;
  79
  80         for (i = 0; i < 32; i++)
  81                 if (test_bit(p, i) == 0)
  82                         return block + i;
  83 }
  84
  85 int read_sector(uint32_t sector, void *buf, int counts)
  86 {
  87         if (lseek(fd, sector << 9, SEEK_SET) < 0) {
  88                 printf("seeking sector: %d error...\n", sector);
  89                 return;
  90         }
  91
  92         return read(fd, buf, 512 * counts);
  93 }
  94
  95 int write_sector(uint32_t sector, void *buf, int counts)
  96 {
  97         if (lseek(fd, sector << 9, SEEK_SET) < 0) {
  98                 printf("seeking sector: %d error...\n", sector);
  99                 return;
 100         }
 101
 102         return write(fd, buf, 512 * counts);
 103 }
 104
 105 int tfs_bread(struct tfs_sb_info *sbi, uint32_t block, void *buf)
 106 {
 107         uint32_t sector = (block << (sbi->s_block_shift - 9)) + sbi->s_offset;
 108
 109         return read_sector(sector, buf, 1 << (sbi->s_block_shift - 9));
 110 }
 111
 112 int tfs_bwrite(struct tfs_sb_info *sbi, uint32_t block, void *buf)
 113 {
 114         uint32_t sector = (block << (sbi->s_block_shift - 9)) + sbi->s_offset;
 115
 116         return write_sector(sector, buf, 1 << (sbi->s_block_shift - 9));
 117 }
 118