Распечатка 4.3 Имитация функции write

Распечатка 4.3

static inline int dummy_flash_write_oneword(struct map_info *map,

                  struct flchip *chip, loff_t addr, __u32 datum)

{

  DECLARE_WAITQUEUE(wait, current);

again:

  spin_lock(chip->mutex);

  if(chip->state != FL_READY)

  {

    set_current_state(TASK_UNINTERRUPTIBLE);

    add_wait_queue(&chip->wq, &wait);

    spin_unlock(chip->mutex);

    schedule();

    remove_wait_queue(&chip->wq, &wait);

    if(signal_pending(current))

      return -EINTR;

    goto again;

  }

  addr += chip->start;

  chip->state = FL_WRITING;

  WRITE_FLASH_ONE_WORD(map, chip->start, addr, datum);

  chip->state = FL_READY;

  wake_up(&chip->wq);

  spin_unlock(chip->mutex);

  return 0;

}

static int dummy_flash_write(struct mtd_info *mtd, loff_t from,

      size_t len,size_t *retlen, const u_char *buf)

{

  struct map_info *map = mtd->priv;

  struct dummy_private_info_struct *priv = map->fldrv_priv;

  int chipnum = 0;

  union {

    unsigned int idata;

    char cdata[4]; }

  wbuf;

  unsigned int ofs;

  int ret;

  *retlen = 0;

  chipnum = (from >> priv->chipshift);

  ofs = from & ((1 << priv->chipshift) - 1);

  /* Сначала проверяем, выровнено ли первое слово для записи */

  if(ofs & 3)

  {

    unsigned int from_offset = ofs & (~3);

    unsigned int orig_copy_num = ofs - from_offset;

    unsigned int to_copy_num = (4 - orig_copy_num);

    unsigned int i, len;

    map_copy_from(map, wbuf.cdata, from_offset +

                  priv->chips[chipnum].start, 4);

    /* Перезаписываем новым содержимым из buf[] */

    for(i=0; i < to_copy_num; i++)

      wbuf.cdata[orig_copy_num + i] = buf[i];

    if((ret = dummy_flash_write_oneword(map, &priv->chips[chipnum],

        from_offset, wbuf.idata)) < 0)

      return ret;

    ofs += i;

    buf += i;

    *retlen += i;

    len -= i;

    if(ofs >> priv->chipshift)

    {

      chipnum++;

      ofs = 0;

    }

  }

  /* Теперь записываем все выровненные слова */

  while(len / 4)

  {

    memcpy(wbuf.cdata, buf, 4);

    if((ret = dummy_flash_write_oneword(map, &priv->chips[chipnum],

                                            ofs, wbuf.idata)) < 0)

      return ret;

    ofs += 4;

    buf += 4;

    *retlen += 4;

    len -= 4;

    if(ofs >> priv->chipshift)

    {

      chipnum++;

      ofs = 0;

    }

  }

  /* Записываем последнее слово */

  if(len)

  {

    unsigned int i=0;

    map_copy_from(map, wbuf.cdata, ofs + priv->chips[chipnum].start,

                  4);

    for(; i<len; i++)

      wbuf.cdata[i] = buf[i];

    if((ret = dummy_flash_write_oneword(map, &priv->chips[chipnum],

                                            ofs, wbuf.idata)) < 0)

      return ret;

    *retlen += i;

  }

  return 0;

}