feat(usb): migrate to libusb 1.0

Replace the old libusb 0.1 API with libusb 1.0. - Update device detection and USB transfers. - Update USB interface and resource handling. - Use pkg-config to find the library. - Update build dependencies and installation instructions.
image: Fix image_read() function when image has padding
2026-10-23 00:13:05 +04:00 · 2023-05-07 22:38:36 +02:00 · 2023-04-16 15:48:43 +02:00 · 2023-04-16 00:24:25 +02:00 · 2023-04-15 23:09:03 +02:00 · 2023-04-15 22:21:16 +02:00
38 changed files with 3087 additions and 1035 deletions
@@ -0,0 +1,37 @@
+sudo: false
+
+language: c
+
+arch:
+  - AMD64
+  - ppc64le
+  
+compiler:
+  - gcc
+  - clang
+
+addons: &addons
+  apt:
+    packages:
+      - libusb-1.0-0-dev
+      - pkg-config
+
+matrix:
+  include:
+    - compiler: gcc
+      if: (branch = master) AND (type != pull_request)
+      env:
+        - secure: "f+qBVyDcDe/9IUmWcqlhL81s4QdY/eHeJW81Zfkfulu0Iai20Ro/lnj0PxVHeSo12cldUTG0XhY1GURYLe2RNdtB2ooe3d9JGn9uFrkyDNhmHcOuJQKY/g2XQ0gJQ9tpWkZhSzENl2C9H/uz87oGqSqwlCLdwixekk8vrmH5kpnNXtbgfK3EKWbhGN0Zub70+qeLa7jTShMN3HgJbrkDYgxUPMofqOCHp8W1xPIvUjNS4yQtN/Ahl6g7z79xopgCvGI91ZkAigSHJGBeHoq3rxG+ecoK+AprhiXuIZ8tD8bHv5IpKEi6mFGf4Fo0mQw+1b8g0HLBwWoiR5Yz7fv6fRgDhuK2AqZZmK8J6q13Lxl/DZS93hTeSJPk9FBEvJsPRwDUgy0P6HObmmEnmclERd9aqcK4RxrQkTjWK+rhHtoRg1kB2QiLz0dNpXQ/CTImv5jooYibo2TCyH6nBcEKzIxSQABvorEwpn7rx7UqL+rFqCQgLrqcRgCna615XpxpS+/W8VeScp2y1MzClwuudBaOiEAK5b6xa6HWk7RDQPkUAYmsnlFZ3zDbTiMzpKRYp/I2N6CPD50PX8lRJpxDSn6qL0HC0qCaYZqlKA+kKxl+OWbtUWM9mE9MJE3V8RRQO8g5p1mJ8NpjdOx7wwYk09BIp6YYPLXLj32hKn4kfXo="
+      addons:
+        <<: *addons
+        coverity_scan:
+          project:
+            name: "pali/0xFFFF"
+            description: "Build submitted via Travis CI"
+          notification_email: pali.rohar@gmail.com
+          build_command: make
+          branch_pattern: master
+
+script:
+  - if [ -n "$COVERITY_SCAN_TOKEN" ]; then exit 0; fi
+  - make
@@ -1,9 +1,15 @@
 Installation instructions:
 ==========================

-To build the flasher you will need gcc, make and libusb. For crosscompiling
-to arm you can use scratchbox. But you will probably need the libusb.so of
-the scratchbox environment to the device.
+To build the flasher you will need gcc, make and libusb-1.0 (version 1.0.16
+or newer). Compiler and linker flags for libusb-1.0 are detected by pkg-config;
+without it set LIBUSB_CFLAGS and LIBUSB_LIBS manually (see config.mk), e.g.:
+
+  $ make LIBUSB_CFLAGS=-I/opt/homebrew/opt/libusb/include/libusb-1.0 \
+         LIBUSB_LIBS="-L/opt/homebrew/opt/libusb/lib -lusb-1.0"
+
+For crosscompiling to arm you can use scratchbox. But you will probably need
+the libusb-1.0.so of the scratchbox environment to the device.

 Just type:

@@ -10,29 +10,26 @@
               _/ ___|_.|__|
                '`

-0xFFFF is Open Free Fiasco Firmware Flasher for Maemo devices. It support
-generating and unpacking FIASCO images on local computer. Useful for editing
-Maemo firmware package for future flash. It support via USB flashing any image
-type to Maemo device and also \"cold\" flashing which means flashing dead device
-with erased bootloader. There is support for booting kernel via USB without
-flashing to NAND and also changing configuration of Maemo device (enable/disable
-R&D mode, changing HW revision strings, ...).
+0xFFFF is the Open Free Fiasco Firmware Flasher for Maemo devices. It supports
+generating and unpacking FIASCO images. It is useful for editing Maemo firmware
+packages before flashing. It supports flashing via USB any image type to a
+Maemo device and also \"cold\" flashing which means flashing dead device with
+erased bootloader. There is support for booting kernel via USB without flashing
+to NAND and also changing configuration of Maemo device (enable/disable R&D
+mode, changing HW revision strings, ...).

-0xFFFF is alternative tool to proprietary Nokia flasher-3.5 and fiasco-gen.
-0xFFFF generate compatible FIASCO images and also accept FIASCO images generated
-by Nokia fiasco-gen.
+0xFFFF is an alternative tool to proprietary Nokia flasher-3.5 and fiasco-gen.
+0xFFFF generates compatible FIASCO images and also accepts FIASCO images
+generated by Nokia fiasco-gen.

 Use it with CARE. This is an experimental tool and it can brick your device.
-It's not suposed to be stable, so nokia will probably release incompatible
+It's not supposed to be stable, so Nokia will probably release incompatible
 bootloaders to break this tool. USE IT AT YOUR OWN RISK.

-PLEASE. Read carefully all the documentation inside doc/* for more information
-before building or using the flasher to avoid questions, problems or so.
+PLEASE. Read all the documentation inside doc/* carefully before building or
+using the flasher to avoid questions, problems or so.

-Feel free to join to the mailing list and visit the homepage for more info:
-
-  0xffff@lists.nopcode.org
-  http://www.nopcode.org/0xFFFF
+  https://github.com/pali/0xFFFF

 -----------8<---------------------------------------------------------------

@@ -4,11 +4,8 @@ cal:
 cold-flash:
 * Detect device from asic id

-device:
- * Support for Nokia N950 and Nokia N9
-
 disk:
- * Everything (work with eMMC images in USB Mass storage mode)
+ * Badblock checking

 fiasco:
 * Support for Harmattan images
@@ -17,18 +14,11 @@ image:
 * Support for Harmattan images

 local:
- * Support for flashing (on device)
+ * Write versions
+ * Badblock checking

 mkii:
- * Everything (flashing protocol for eMMC images)
+ * Support for flashing

 nolo:
- * Support for dumping images (it is possible?)
 * Implement functions for setting nolo, kernel, initfs and content versions (it is possible?)
-
-operations:
- * Enable FLASH_DISK protocol when implemented
-
-usb-device:
- * Autodetect devices from product string
- * Enable FLASH_DISK and FLASH_MKII protocols when implemented
@@ -1,6 +1,11 @@
-VERSION = 0.6
+VERSION = 0.10
 PREFIX = /usr/local

 # NetBSD stuff
 #CPPFLAGS += -I/usr/pkg/include
 #LDFLAGS += -L/usr/pkg/lib -Wl,-R/usr/pkg/lib
+
+# libusb-1.0 flags are detected by pkg-config, set them manually without it
+# (e.g. macOS with Homebrew libusb on Apple Silicon)
+#LIBUSB_CFLAGS = -I/opt/homebrew/opt/libusb/include/libusb-1.0
+#LIBUSB_LIBS = -L/opt/homebrew/opt/libusb/lib -lusb-1.0
@@ -1,5 +0,0 @@
-0xffff (0.6-1) unstable; urgency=low
-
-  * This is new rewritten version of original 0xFFFF 0.5 tool
-
- -- Pali Rohár <pali.rohar@gmail.com>  Sun, 07 Apr 2013 15:01:50 +0200
@@ -1 +0,0 @@
-7
@@ -1,25 +0,0 @@
-Source: 0xffff
-Section: utils
-Priority: extra
-Maintainer: Pali Rohár <pali.rohar@gmail.com>
-Build-Depends: debhelper (>= 7), libusb-dev
-Standards-Version: 3.9.3
-
-Package: 0xffff
-Architecture: any
-Depends: ${shlibs:Depends}, ${misc:Depends}
-Description: Open Free Fiasco Firmware Flasher
- 0xFFFF is Open Free Fiasco Firmware Flasher for Maemo devices. It support
- generating and unpacking FIASCO images on local computer. Useful for editing
- Maemo firmware package for future flash. It support via USB flashing any image
- type to Maemo device and also "cold" flashing which means flashing dead
- device with erased bootloader. There is support for booting kernel via USB
- without flashing to NAND and also changing configuration of Maemo device
- (enable/disable R&D mode, changing HW revision strings, ...).
- .
- 0xFFFF is alternative tool to proprietary Nokia flasher-3.5 and fiasco-gen.
- 0xFFFF generate compatible FIASCO images and also accept FIASCO images
- generated by Nokia fiasco-gen.
- .
- Supported Maemo devices:
- Nokia 770, Nokia N800, Nokia N810, Nokia N810 WiMAX, Nokia N900
@@ -1,29 +0,0 @@
-Format: http://www.debian.org/doc/packaging-manuals/copyright-format/1.0/
-Upstream-Name: 0xffff
-
-Files: *
-Copyright: 2007-2012 pancake <pancake@youterm.com>
-           2011      Michael Buesch <mb@bu3sch.de>
-           2011-2013 Pali Rohár <pali.rohar@gmail.com>
-License: GPL-3.0+
-
-Files: debian/*
-Copyright: 2013 Pali Rohár <pali.rohar@gmail.com>
-License: GPL-3.0+
-
-License: GPL-3.0+
- This program is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 3 of the License, or
- (at your option) any later version.
- .
- This package is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- GNU General Public License for more details.
- .
- You should have received a copy of the GNU General Public License
- along with this program. If not, see <http://www.gnu.org/licenses/>.
- .
- On Debian systems, the complete text of the GNU General
- Public License version 3 can be found in "/usr/share/common-licenses/GPL-3".
@@ -1,2 +0,0 @@
-README
-doc/*
@@ -1,11 +0,0 @@
-#!/usr/bin/make -f
-# -*- makefile -*-
-
-# Uncomment this to turn on verbose mode.
-#export DH_VERBOSE=1
-
-%:
-	dh $@
-
-override_dh_auto_install:
-	dh_auto_install -- PREFIX=/usr
@@ -1 +0,0 @@
-3.0 (quilt)
@@ -30,12 +30,10 @@ Technical details:
 For dumping mtd partition is used tool nanddump. Here is example how to dump
 kernel image without padding to file zImage:

- $ nanddump -i -o -b -s 0x00000800 -l 0x001FF800 -f zImage /dev/mtd2
+ $ nanddump --omitoob -s 0x00000800 -l 0x001FF800 -f zImage /dev/mtd2

 Params means:
-i - "Ignore errors"
-o - "Omit oob data"
-b - "Omit bad blocks"
+--omitoob - "Omit oob data"
 -s - "Start address"
 -l - "Length"
 -f - "Output file"
@@ -38,3 +38,6 @@ $ 0xFFFF -M <file> -u

 Generate new FIASCO image image.fiasco from files xloader.bin, nolo.bin, zImage, rootfs and append device&version information (xloader for RX-51 hw revision: 2101 and 2102, version 1.0)
 $ 0xFFFF -m RX-51:2101,2102:1.0:xloader:xloader.bin -m RX-51:2101,2102:1.0:secondary:nolo.bin -m 2.6.28:kernel:zImage -m rootfs -g image.fiasco
+
+Generate new FIASCO mmc image mmc_image.fiasco from layout file layout.txt and two partition files mydocs_fat32.bin (with name mydocs) and home.tar (without name) for device RX-51 with image version 1.0
+$ 0xFFFF -m RX-51:1.0:mmc:mydocs_fat32.bin@mydocs#home.tar%layout.txt -g mmc_image.fiasco
@@ -37,21 +37,75 @@ FW HEADER
 IMAGE

   1 byte = 0x54    --  signature
-   1 byte           --  number of subsection blocks + 1
-   5 bytes          --  unknown (always 0x2e 0x19 0x01 0x01 0x00)
-   2 bytes          --  checksum for the image contents (xorpair) (big endian)
-  12 bytes          --  image name type (first byte is FF if is the last image)
-   4 bytes          --  length of image data (big endian)
-   4 bytes          --  unknown (always 0x00 0x00 0x00 0x00)
+   1 byte           --  number of subsection blocks
             block {
   1 byte           --  type of subsection block
-                          '1' - version
-                          '2' - device & hw revisions (size of device is 16, hw revision 8)
-                          '3' - layout
-                          '4' - unknown
-                          '/' - unknown
+                          0x2E - file data
+                          0x2F - partition info
+                          0x31 - version
+                          0x32 - device & hw revisions (size of device is 16, hw revision 8)
+                          0x33 - layout
+                          0x34 - image data part block (one block for one data part)
   1 byte           --  length of subsection block
   N bytes          --  subsection block data
             }
-   1 byte           --  unknown (0x00 is accepted, maybe end of subsections?)
+   1 byte           --  checksum of subsection data without signature (0xFF - xorpair)
   N bytes          --  image data
+
+
+FILE DATA BLOCK
+
+   1 byte           --  asic index (always APE - 0x01)
+   1 byte           --  device type (always NAND - 0x01)
+   1 byte           --  device index (always 0x00)
+   2 bytes          --  checksum for the image contents (xorpair) (big endian)
+  12 bytes          --  image name type (first byte is FF if is the last image)
+   4 bytes          --  length of image data (big endian)
+   4 bytes          --  load address of image data (big endian, unused always zero)
+
+
+PARTITION INFO BLOCK
+
+   1 byte           --  asic index (always APE - 0x01)
+   1 byte           --  device type (always NAND - 0x01)
+   1 byte           --  device index (always 0x00)
+   4 bytes          --  partition version (always 0x00 0x00 0x00 0x00)
+   4 bytes          --  spare (always 0xFF 0xFF 0xFF 0xFF)
+   4 bytes          --  number of partition blocks (big endian)
+             block {
+   1 byte           --  partition type (0x03 - kernel, 0x04 - initfs, 0x05 - rootfs)
+   1 byte           --  partition attribute (0x01 - read/write, 0x02 - read-only)
+   4 bytes          --  partition size (big endian, 0 - remaining space)
+             }
+
+
+IMAGE DATA PART BLOCK
+
+   4 bytes          --  unknown (always 0x00 0x00 0x00 0x00)
+   4 bytes          --  offset (big endian)
+   4 bytes          --  unknown (always 0x00 0x00 0x00 0x00)
+   4 bytes          --  size (big endian)
+   N bytes          --  partition name (prefix) in layout (may be omitted)
+
+
+LAYOUT DATA BLOCK
+
+   N bytes of text data, e.g.:
+
+mmc {
+        name = "internal";
+        partition {
+                fs_type = "vfat";
+                prefix = "mydocs";
+                no_create = true;
+        }
+        partition {
+                size = 2048;
+                fs_type = "ext3";
+                prefix = "home";
+        }
+        partition {
+                size = 768;
+                fs_type = "swap";
+        }
+}
@@ -1,4 +1,4 @@
- Copyright (C) 2012  Pali Rohár <pali.rohar@gmail.com>
+ Copyright (C) 2012-2014  Pali Rohár <pali.rohar@gmail.com>

 Mk II protocol is the only protocol which can be used to flash eMMC images.
 NOLO does not support eMMC, so flashing eMMC is done in Maemo system. NOLO
@@ -27,6 +27,10 @@ Over usb are used only these functions for communication:
 usb_bulk_write (ep=1, timeout=5000)
 usb_bulk_read (ep=129, timeout=5000)

+And this function for sending raw data:
+
+ usb_bulk_write (ep=2, timeout=1000)
+
 For every (request) message which is send by host, server send back response.
 Format of message every message is same. It has 6 bytes header and (at least)
 4 bytes body.
@@ -40,51 +44,113 @@ HEADER

 BODY

-   4 bytes          --  type of message
+   2 bytes          --  unknown (always zero)
+   1 byte           --  or. num of message (starting with zero)
+   1 byte           --  type of message
   N bytes          --  data

 Reply message data always starts with char 0x00 (except pong response).

+Message types:
+
+0x00 - PING
+0x01 - GET
+0x02 - TELL
+0x0C - REBOOT
+
+0x20 - RESPONCE
+
 Here are some sniffed messages from Nokia RX-51. First two messages seems to
 must be always protocol version exchange (first host ask for protocol version of
 server and then host send its protocol version). On RX-51 is used version "2".

 Ping:
-   req_type = 0x00000000
-   res_type = 0x20000000
+   req_type = 0x00
+   res_type = 0x20

 Get protocol version:
-   req_type = 0x01010000
+   req_type = 0x01
   req_data = "/update/protocol_version"
-   res_type = 0x21010000
+   res_type = 0x21
   res_data = 0x00 "2"

 Tell our protocol version:
-   req_type = 0x02020000
+   req_type = 0x02
   req_data = "/update/host_protocol_version" 0x00 "2"
-   res_type = 0x22020000
+   res_type = 0x22
   res_data = 0x00

 Get device:
-   req_type = 0x01030000
+   req_type = 0x01
   req_data = "/device/product_code"
-   res_type = 0x21030000
+   res_type = 0x21
   res_data = 0x00 "RX-51"

 Get hwrev:
-   req_type = 0x01040000
+   req_type = 0x01
   req_data = "/device/hw_build"
-   res_type = 0x21040000
+   res_type = 0x21
   res_data = 0x00 "2101"

 Get image types:
-   req_type = 0x01050000
+   req_type = 0x01
   req_data = "/update/supported_images"
-   res_type = 0x21050000
+   res_type = 0x21
   res_data = 0x00 "xloader,secondary,kernel,mmc,cmt-2nd,cmt-algo,cmt-mcusw"

 Reboot device:
-   req_type = 0x0C060000
-   req_data = "reboot"
-   res_type = 0x2C060000
+   req_type = 0x0C
+   req_data = "reboot" 0x00
+   res_type = 0x2C
   res_data = 0x00
+
+ Send image (mmc):
+   req_type = 0x03
+   res_type = 0x23
+   res_data = 0x00
+
+   req_type = 0x04
+   req_data = fiasco subimage header
+   res_type = 0x24
+   res_data = 0x00 0x00 0x00 0x00 0x00 0x00 0x02 0x00 0x00
+
+   req_type = 0x05
+   req_data = 0x00 0x00 0x00 0x00 "usb:raw"
+   res_type = 0x25
+   res_data = 0x00
+
+   req_type = 0x06
+   req_data = 0x00 0x00 0x00 0x00
+   res_type = 0x26
+   res_data = 0x00 0x00 0x00 0x01 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
+
+   req_type = 0x0B
+   req_data = 0x00 0x00 0x00 0x64
+   res_type = 0x2B
+   res_data = 0x00 0x00 0x00 0x01 0x00 0x00 0x00 0x00 0x02 0x00 0x00 0x00 0x00
+
+   req_type = 0x08
+   req_data = 0x00 0x00 0x00 0x00 0x00 0x10 0x00 0x00
+   res_type = 0x28
+   res_data = 0x00
+
+   (raw data on ep=2 size=1048576)
+
+   req_type = 0x06
+   req_data = 0x00 0x00 0x00 0x00
+   res_type = 0x26
+   res_data = 0x00 0x00 0x00 0x03 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x0F 0x9F 0x2C 0x00 0x00
+
+   req_type = 0x0B
+   req_data = 0x00 0x00 0x00 0x64
+   res_type = 0x2B
+   res_data = 0x00 0x00 0x00 0x01 0x00 0x00 0x00 0x00 0x01 0xF0 0x00 0x00 0x00
+
+   req_type = 0x08
+   req_data = 0x00 0x00 0x00 0x00 0x00 0x10 0x00 0x00
+   res_type = 0x28
+   res_data = 0x00
+
+   (raw data on ep=2 size=1048576)
+
+   ...
@@ -3,13 +3,19 @@ include ../config.mk
 PREFIX ?= /usr/local
 INSTALL ?= install

-CC = gcc
-CROSS_CC = $(CROSS_COMPILE)$(CC)
-HOST_CC = $(HOST_COMPILE)$(CC)
+BUILD_DATE ?= $(shell LC_ALL=C date '+%b %e %Y')

-CPPFLAGS += -DVERSION=\"$(VERSION)\" -D_GNU_SOURCE
+CC ?= cc
+CROSS_CC ?= $(CROSS_COMPILE)$(CC)
+HOST_CC ?= $(HOST_COMPILE)$(CC)
+
+PKG_CONFIG ?= pkg-config
+LIBUSB_CFLAGS ?= $(shell $(PKG_CONFIG) --cflags libusb-1.0 2>/dev/null)
+LIBUSB_LIBS ?= $(shell $(PKG_CONFIG) --libs libusb-1.0 2>/dev/null || echo -lusb-1.0)
+
+CPPFLAGS += -DVERSION=\"$(VERSION)\" -DBUILD_DATE="\"$(BUILD_DATE)\"" -D_POSIX_C_SOURCE=200809L -D_FILE_OFFSET_BITS=64
 CFLAGS += -W -Wall -O2 -pedantic -std=c99
-LIBS += -lusb
+LIBS += $(LIBUSB_LIBS)

 DEPENDS = Makefile ../config.mk

@@ -31,13 +37,13 @@ $(BIN).1: $(BIN) $(MANGEN) $(DEPENDS) ../doc/examples
 	mv $@.tmp $@

 libusb-sniff-32.so: libusb-sniff.c $(DEPENDS)
-	$(CC) $(CFLAGS) $(LDFLAGS) -fPIC -ldl -shared -m32 -o $@ $<
+	$(CC) $(CFLAGS) $(LDFLAGS) -fPIC $< -ldl -shared -m32 -o $@

 libusb-sniff-64.so: libusb-sniff.c $(DEPENDS)
-	$(CC) $(CFLAGS) $(LDFLAGS) -fPIC -ldl -shared -m64 -o $@ $<
+	$(CC) $(CFLAGS) $(LDFLAGS) -fPIC $< -ldl -shared -m64 -o $@

 %.o: %.c $(DEPENDS)
-	$(CROSS_CC) $(CFLAGS) $(CPPFLAGS) -c -o $@ $<
+	$(CROSS_CC) $(CFLAGS) $(CPPFLAGS) $(LIBUSB_CFLAGS) -c -o $@ $<

 install: $(BIN) $(BIN).1
 	$(INSTALL) -D -m 755 $(BIN) $(DESTDIR)$(PREFIX)/bin/$(BIN)
@@ -27,14 +27,17 @@
 #include <errno.h>
 #include <string.h>

-#include <sys/ioctl.h>
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <unistd.h>
 #include <fcntl.h>

+#ifdef __linux__
+#include <sys/ioctl.h>
 #include <linux/fs.h>
 #include <mtd/mtd-user.h>
+#include <sys/sysmacros.h>
+#endif

 #include "cal.h"

@@ -43,7 +46,6 @@
 #define HDR_MAGIC	"ConF"

 struct cal {
-	int fd;
 	ssize_t size;
 	void * mem;
 };
@@ -68,30 +70,51 @@ int cal_init_file(const char * file, struct cal ** cal_out) {
 	void * mem = NULL;
 	struct cal * cal = NULL;
 	struct stat st;
+#ifdef __linux__
 	mtd_info_t mtd_info;
-
-	if ( stat(file, &st) != 0 )
-		return -1;
+#else
+	off_t lsize = 0;
+#endif

 	fd = open(file, O_RDONLY);

 	if ( fd < 0 )
 		return -1;

+	if ( fstat(fd, &st) != 0 )
+		goto err;
+
 	if ( S_ISREG(st.st_mode) )
 		size = st.st_size;
 	else if ( S_ISBLK(st.st_mode) ) {
+#ifdef __linux__
 		if ( ioctl(fd, BLKGETSIZE64, &blksize) != 0 )
 			goto err;
+#else
+		lsize = lseek(fd, 0, SEEK_END);
+		if ( lsize == (off_t)-1 )
+			goto err;
+		if ( lseek(fd, 0, SEEK_SET) == (off_t)-1 )
+			goto err;
+		blksize = lsize;
+#endif
+#ifdef SSIZE_MAX
 		if ( blksize > SSIZE_MAX )
 			goto err;
+#endif
 		size = blksize;
-	} else if ( S_ISCHR(st.st_mode) && major(st.st_rdev) == 90 ) {
-		if ( ioctl(fd, MEMGETINFO, &mtd_info) != 0 )
-			goto err;
-		size = mtd_info.size;
 	} else {
+#ifdef __linux__
+		if ( S_ISCHR(st.st_mode) && major(st.st_rdev) == 90 ) {
+			if ( ioctl(fd, MEMGETINFO, &mtd_info) != 0 )
+				goto err;
+			size = mtd_info.size;
+		} else {
+			goto err;
+		}
+#else
 		goto err;
+#endif
 	}

 	if ( size == 0 || size > MAX_SIZE )
@@ -110,16 +133,18 @@ int cal_init_file(const char * file, struct cal ** cal_out) {
 	if ( ! cal )
 		goto err;

-	cal->fd = fd;
 	cal->mem = mem;
 	cal->size = size;

+	close(fd);
+
 	*cal_out = cal;
 	return 0;

 err:
 	close(fd);
 	free(mem);
+	free(cal);
 	return -1;

 }
@@ -170,8 +195,8 @@ static int is_header(void *data, size_t size) {
 		return 0;

 	if ( memcmp(hdr->magic, HDR_MAGIC, sizeof(hdr->magic)) != 0 )
-
 		return 0;
+
 	return 1;

 }
@@ -206,10 +231,10 @@ static int64_t find_section(void *start, uint64_t count, int want_index, const c
 		memcpy(sectname, hdr->name, sizeof(hdr->name));

 		if ( want_index == INDEX_LAST ) {
-			if ((int)hdr->index <= previous_index)
+			if ( (int)hdr->index <= previous_index )
 				goto next;
 		} else {
-			if (want_index >= 0 && want_index != hdr->index)
+			if ( want_index >= 0 && want_index != hdr->index )
 				goto next;
 		}

@@ -239,6 +264,7 @@ int cal_read_block(struct cal * cal, const char * name, void ** ptr, unsigned lo
 	uint64_t filelen = cal->size;
 	uint8_t * data = cal->mem;
 	struct header * hdr;
+	void * offset;

 	find_offset = find_section(data, filelen, INDEX_LAST, name);
 	if ( find_offset < 0 )
@@ -252,12 +278,18 @@ int cal_read_block(struct cal * cal, const char * name, void ** ptr, unsigned lo
 	if ( crc32(0, hdr, sizeof(*hdr) - 4) != hdr->hdrsum )
 		return -1;

-	*ptr = data + find_offset + sizeof(struct header);
-	*len = hdr->length;
+	offset = data + find_offset + sizeof(struct header);

-	if ( crc32(0, *ptr, *len) != hdr->datasum )
+	if ( crc32(0, offset, hdr->length) != hdr->datasum )
 		return -1;

+	*ptr = malloc(hdr->length);
+	if (!*ptr)
+		return -1;
+
+	memcpy(*ptr, offset, hdr->length);
+	*len = hdr->length;
+
 	return 0;

 }
@@ -21,17 +21,13 @@
 #include <stdint.h>
 #include <string.h>
 #include <errno.h>
-#include <usb.h>

 #include "global.h"
-
 #include "cold-flash.h"
 #include "image.h"
 #include "usb-device.h"
 #include "printf-utils.h"

-#define READ_DEV		0x81
-#define WRITE_DEV		0x01
 #define READ_TIMEOUT		500
 #define WRITE_TIMEOUT		3000

@@ -49,7 +45,7 @@ static void crc32_gentab(void) {

 		for ( j = 8; j > 0; j-- ) {

-			if (crc & 1)
+			if ( crc & 1 )
 				crc = (crc >> 1) ^ poly;
 			else
 				crc >>= 1;
@@ -85,6 +81,8 @@ static uint32_t crc32(unsigned char * bytes, size_t size, uint32_t crc) {
 /* Omap Peripheral boot message */
 static const uint32_t omap_peripheral_msg = 0xF0030002;

+/* Unused */
+#if 0
 /* Omap Void (no device) boot message */
 static const uint32_t omap_void_msg = 0xF0030006;

@@ -117,6 +115,7 @@ static const uint32_t omap_hsusb_msg = 0xF0031106;

 /* Omap next device boot message */
 static const uint32_t omap_next_msg = 0xFFFFFFFF;
+#endif

 /* Omap memory boot message */
 static const uint32_t omap_memory_msg = 0;
@@ -130,7 +129,7 @@ struct xloader_msg {
 	uint32_t size; /* 4 bytes - size of file */
 	uint32_t crc1; /* 4 bytes - crc32 of file */
 	uint32_t crc2; /* 4 bytes - crc32 of first 12 bytes of message */
-};
+} __attribute__((__packed__));

 #define XLOADER_MSG_TYPE_PING	0x6301326E
 #define XLOADER_MSG_TYPE_SEND	0x6302326E
@@ -138,7 +137,7 @@ struct xloader_msg {
 struct xloader_msg xloader_msg_create(uint32_t type, struct image * image) {

 	struct xloader_msg msg;
-	uint32_t need, readed;
+	uint32_t need, sent;
 	int ret;
 	uint8_t buffer[1024];

@@ -149,16 +148,16 @@ struct xloader_msg xloader_msg_create(uint32_t type, struct image * image) {
 	if ( image ) {
 		msg.size = image->size;
 		image_seek(image, 0);
-		readed = 0;
-		while ( readed < image->size ) {
-			need = image->size - readed;
+		sent = 0;
+		while ( sent < image->size ) {
+			need = image->size - sent;
 			if ( need > sizeof(buffer) )
 				need = sizeof(buffer);
 			ret = image_read(image, buffer, need);
 			if ( ret == 0 )
 				break;
 			msg.crc1 = crc32(buffer, ret, msg.crc1);
-			readed += ret;
+			sent += ret;
 		}
 	}

@@ -168,12 +167,12 @@ struct xloader_msg xloader_msg_create(uint32_t type, struct image * image) {

 }

-static int read_asic(usb_dev_handle * udev, uint8_t * asic_buffer, int size, int asic_size) {
+static int read_asic(libusb_device_handle * udev, uint8_t * asic_buffer, int size, int asic_size) {

 	int ret;

 	printf("Waiting for ASIC ID...\n");
-	ret = usb_bulk_read(udev, READ_DEV, (char *)asic_buffer, size, READ_TIMEOUT);
+	ret = usb_bulk_recv(udev, USB_READ_EP, asic_buffer, size, READ_TIMEOUT);
 	if ( ret != asic_size )
 		ERROR_RETURN("Invalid size of ASIC ID", -1);

@@ -181,86 +180,88 @@ static int read_asic(usb_dev_handle * udev, uint8_t * asic_buffer, int size, int

 }

-static int send_2nd(usb_dev_handle * udev, struct image * image) {
+static int send_2nd(libusb_device_handle * udev, struct image * image) {

 	uint8_t buffer[1024];
-	uint32_t need, readed;
+	uint32_t need, sent;
 	int ret;

 	printf("Sending OMAP peripheral boot message...\n");
-	ret = usb_bulk_write(udev, WRITE_DEV, (char *)&omap_peripheral_msg, sizeof(omap_peripheral_msg), WRITE_TIMEOUT);
-	usleep(5000);
+	ret = usb_bulk_send(udev, USB_WRITE_EP, &omap_peripheral_msg, sizeof(omap_peripheral_msg), WRITE_TIMEOUT);
 	if ( ret != sizeof(omap_peripheral_msg) )
 		ERROR_RETURN("Sending OMAP peripheral boot message failed", -1);

+	MSLEEP(5);
+
 	printf("Sending 2nd X-Loader image size...\n");
-	ret = usb_bulk_write(udev, WRITE_DEV, (char *)&image->size, 4, WRITE_TIMEOUT);
-	usleep(5000);
+	ret = usb_bulk_send(udev, USB_WRITE_EP, &image->size, 4, WRITE_TIMEOUT);
 	if ( ret != 4 )
 		ERROR_RETURN("Sending 2nd X-Loader image size failed", -1);

+	MSLEEP(5);
+
 	printf("Sending 2nd X-Loader image...\n");
 	printf_progressbar(0, image->size);
 	image_seek(image, 0);
-	readed = 0;
-	while ( readed < image->size ) {
-		need = image->size - readed;
+	sent = 0;
+	while ( sent < image->size ) {
+		need = image->size - sent;
 		if ( need > sizeof(buffer) )
 			need = sizeof(buffer);
 		ret = image_read(image, buffer, need);
 		if ( ret == 0 )
 			break;
-		if ( usb_bulk_write(udev, WRITE_DEV, (char *)buffer, ret, WRITE_TIMEOUT) != ret )
+		if ( usb_bulk_send(udev, USB_WRITE_EP, buffer, ret, WRITE_TIMEOUT) != ret )
 			PRINTF_ERROR_RETURN("Sending 2nd X-Loader image failed", -1);
-		readed += ret;
-		printf_progressbar(readed, image->size);
+		sent += ret;
+		printf_progressbar(sent, image->size);
 	}
-	usleep(50000);

+	MSLEEP(50);
 	return 0;

 }

-static int send_secondary(usb_dev_handle * udev, struct image * image) {
+static int send_secondary(libusb_device_handle * udev, struct image * image) {

 	struct xloader_msg init_msg;
 	uint8_t buffer[1024];
-	uint32_t need, readed;
+	uint32_t need, sent;
 	int ret;

 	init_msg = xloader_msg_create(XLOADER_MSG_TYPE_SEND, image);

 	printf("Sending X-Loader init message...\n");
-	ret = usb_bulk_write(udev, WRITE_DEV, (char *)&init_msg, sizeof(init_msg), WRITE_TIMEOUT);
-	usleep(5000);
+	ret = usb_bulk_send(udev, USB_WRITE_EP, &init_msg, sizeof(init_msg), WRITE_TIMEOUT);
 	if ( ret != sizeof(init_msg) )
 		ERROR_RETURN("Sending X-Loader init message failed", -1);

 	printf("Waiting for X-Loader response...\n");
-	ret = usb_bulk_read(udev, READ_DEV, (char *)&buffer, 4, READ_TIMEOUT); /* 4 bytes - dummy value */
+	MSLEEP(5);
+	ret = usb_bulk_recv(udev, USB_READ_EP, &buffer, 4, READ_TIMEOUT); /* 4 bytes - dummy value */
 	if ( ret != 4 )
 		ERROR_RETURN("No response", -1);

 	printf("Sending Secondary image...\n");
 	printf_progressbar(0, image->size);
 	image_seek(image, 0);
-	readed = 0;
-	while ( readed < image->size ) {
-		need = image->size - readed;
+	sent = 0;
+	while ( sent < image->size ) {
+		need = image->size - sent;
 		if ( need > sizeof(buffer) )
 			need = sizeof(buffer);
 		ret = image_read(image, buffer, need);
 		if ( ret == 0 )
 			break;
-		if ( usb_bulk_write(udev, WRITE_DEV, (char *)buffer, ret, WRITE_TIMEOUT) != ret )
+		if ( usb_bulk_send(udev, USB_WRITE_EP, buffer, ret, WRITE_TIMEOUT) != ret )
 			PRINTF_ERROR_RETURN("Sending Secondary image failed", -1);
-		readed += ret;
-		printf_progressbar(readed, image->size);
+		sent += ret;
+		printf_progressbar(sent, image->size);
 	}
-	usleep(5000);

 	printf("Waiting for X-Loader response...\n");
-	ret = usb_bulk_read(udev, READ_DEV, (char *)&buffer, 4, READ_TIMEOUT); /* 4 bytes - dummy value */
+	MSLEEP(5);
+	ret = usb_bulk_recv(udev, USB_READ_EP, &buffer, 4, READ_TIMEOUT); /* 4 bytes - dummy value */
 	if ( ret != 4 )
 		ERROR_RETURN("No response", -1);

@@ -268,7 +269,7 @@ static int send_secondary(usb_dev_handle * udev, struct image * image) {

 }

-static int ping_timeout(usb_dev_handle * udev) {
+static int ping_timeout(libusb_device_handle * udev) {

 	int ret;
 	int pong = 0;
@@ -280,7 +281,7 @@ static int ping_timeout(usb_dev_handle * udev) {
 		int try_read = 4;

 		printf("Sending X-Loader ping message\n");
-		ret = usb_bulk_write(udev, WRITE_DEV, (char *)&ping_msg, sizeof(ping_msg), WRITE_TIMEOUT);
+		ret = usb_bulk_send(udev, USB_WRITE_EP, &ping_msg, sizeof(ping_msg), WRITE_TIMEOUT);
 		if ( ret != sizeof(ping_msg) )
 			ERROR_RETURN("Sending X-Loader ping message failed", -1);

@@ -288,14 +289,14 @@ static int ping_timeout(usb_dev_handle * udev) {
 		while ( try_read > 0 ) {

 			uint32_t ping_read;
-			ret = usb_bulk_read(udev, READ_DEV, (char *)&ping_read, sizeof(ping_read), READ_TIMEOUT);
+			ret = usb_bulk_recv(udev, USB_READ_EP, &ping_read, sizeof(ping_read), READ_TIMEOUT);
 			if ( ret == sizeof(ping_read) ) {
 				printf("Got it\n");
 				pong = 1;
 				break;
 			}

-			usleep(5000);
+			MSLEEP(5);
 			--try_read;

 		}
@@ -308,7 +309,7 @@ static int ping_timeout(usb_dev_handle * udev) {

 	}

-	if (pong)
+	if ( pong )
 		return 0;
 	else
 		return -1;
@@ -320,6 +321,7 @@ int init_cold_flash(struct usb_device_info * dev) {
 	uint8_t asic_buffer[127];
 	int asic_size = 69;
 	const char * chip = NULL;
+	int revision;
 	int i;

 	if ( dev->flash_device->protocol != FLASH_COLD )
@@ -343,11 +345,11 @@ int init_cold_flash(struct usb_device_info * dev) {
 	if ( asic_buffer[0] != 0x05 )
 		ERROR_RETURN("Invalid ASIC ID", -1);

-	/* ID Subblock - header */
+	/* 1. ID Subblock - header */
 	if ( memcmp(asic_buffer+1, "\x01\x05\x01", 3) != 0 )
 		ERROR_RETURN("Invalid ASIC ID", -1);

-	/* ID Subblock - OMAP chip version (check for OMAP3430 or 3630) */
+	/* 1. ID Subblock - OMAP chip version (check for OMAP3430 or 3630) */
 	if ( memcmp(asic_buffer+4, "\x34\x30\x07", 3) == 0 )
 		chip = "OMAP3430";
 	else if ( memcmp(asic_buffer+4, "\x36\x30\x07", 3) == 0 )
@@ -355,23 +357,26 @@ int init_cold_flash(struct usb_device_info * dev) {
 	else
 		ERROR_RETURN("Invalid ASIC ID", -1);

-	/* Reserved1 - header */
+	/* 1. ID Subblock - OMAP chip revision */
+	revision = asic_buffer[7];
+
+	/* 2. Secure Mode Subblock - header */
 	if ( memcmp(asic_buffer+8, "\x13\x02\x01", 3) != 0 )
 		ERROR_RETURN("Invalid ASIC ID", -1);

-	/* 2nd ID Subblock - header */
+	/* 3. 2nd ID Subblock - header */
 	if ( memcmp(asic_buffer+12, "\x12\x15\x01", 3) != 0 )
 		ERROR_RETURN("Invalid ASIC ID", -1);

-	/* Reserved2 - header */
+	/* 4. Root Key Hash Subblock - header */
 	if ( memcmp(asic_buffer+35, "\x14\x15\x01", 3) != 0 )
 		ERROR_RETURN("Invalid ASIC ID", -1);

-	/* Checksum subblock - header */
+	/* 5. Checksum subblock - header */
 	if ( memcmp(asic_buffer+58, "\x15\x09\x01", 3) != 0 )
 		ERROR_RETURN("Invalid ASIC ID", -1);

-	printf("Detected %s chip\n", chip);
+	printf("Detected %s chip (revision %d)\n", chip, revision);

 	return 0;

@@ -404,12 +409,11 @@ int leave_cold_flash(struct usb_device_info * dev) {
 	int ret;

 	printf("Sending OMAP memory boot message...\n");
-	ret = usb_bulk_write(dev->udev, WRITE_DEV, (char *)&omap_memory_msg, sizeof(omap_memory_msg), WRITE_TIMEOUT);
-	usleep(5000);
+	ret = usb_bulk_send(dev->udev, USB_WRITE_EP, &omap_memory_msg, sizeof(omap_memory_msg), WRITE_TIMEOUT);
 	if ( ret != sizeof(omap_memory_msg) )
 		ERROR_RETURN("Sending OMAP memory boot message failed", -1);

-	usleep(250000);
+	MSLEEP(250);
 	return 0;

 }
@@ -31,6 +31,8 @@ static const char * devices[] = {
 	[DEVICE_RX_44] = "RX-44",
 	[DEVICE_RX_48] = "RX-48",
 	[DEVICE_RX_51] = "RX-51",
+	[DEVICE_RM_680] = "RM-680",
+	[DEVICE_RM_696] = "RM-696",
 };

 enum device device_from_string(const char * device) {
@@ -63,6 +65,8 @@ static const char * long_devices[] = {
 	[DEVICE_RX_44] = "Nokia N810",
 	[DEVICE_RX_48] = "Nokia N810 Wimax",
 	[DEVICE_RX_51] = "Nokia N900",
+	[DEVICE_RM_680] = "Nokia N950",
+	[DEVICE_RM_696] = "Nokia N9",
 };

 const char * device_to_long_string(enum device device) {
@@ -180,9 +184,11 @@ char ** device_list_alloc_to_bufs(const struct device_list * device_list) {
 			continue;
 		}

-		for ( i = 0; device_first->hwrevs[i] != -1; ++i )
-			if ( device_first->hwrevs[i] >= 0 && device_first->hwrevs[i] <= 9999 )
-				++local;
+		if ( device_first->hwrevs ) {
+			for ( i = 0; device_first->hwrevs[i] != -1; ++i )
+				if ( device_first->hwrevs[i] >= 0 && device_first->hwrevs[i] <= 9999 )
+					++local;
+		}

 		size += (1+16+(MAX_HWREVS+1)*8)*(local/MAX_HWREVS+1);
 		count += local/MAX_HWREVS;
@@ -211,7 +217,7 @@ char ** device_list_alloc_to_bufs(const struct device_list * device_list) {
 			continue;
 		}

-		while ( device_first->hwrevs[i+1] != -1 ) {
+		do {

 			uint8_t len = 0;
 			ret[j] = ++last_ptr;
@@ -224,7 +230,7 @@ char ** device_list_alloc_to_bufs(const struct device_list * device_list) {

 			for ( k = 0; k < MAX_HWREVS; ++k ) {

-				if ( device_first->hwrevs[i+1] == -1 )
+				if ( ! device_first->hwrevs || device_first->hwrevs[i+1] == -1 )
 					break;

 				++i;
@@ -242,7 +248,7 @@ char ** device_list_alloc_to_bufs(const struct device_list * device_list) {

 			++j;

-		}
+		} while ( device_first->hwrevs && device_first->hwrevs[i+1] != -1 );

 		device_first = device_first->next;

@@ -30,6 +30,8 @@ enum device {
 	DEVICE_RX_44, /* Nokia N810 */
 	DEVICE_RX_48, /* Nokia N810 WiMax */
 	DEVICE_RX_51, /* Nokia N900 */
+	DEVICE_RM_680, /* Nokia N950 */
+	DEVICE_RM_696, /* Nokia N9 */
 	DEVICE_COUNT,
 };

@@ -18,13 +18,20 @@

 #include <fcntl.h>
 #include <libgen.h>
+#include <unistd.h>
+#include <stdlib.h>

 #include <sys/types.h>
 #include <sys/stat.h>
-#include <sys/ioctl.h>
 #include <sys/statvfs.h>

+#ifdef __linux__
+#include <sys/sysmacros.h>
+#include <sys/ioctl.h>
 #include <linux/fs.h>
+#include <dirent.h>
+#include <errno.h>
+#endif

 #include "disk.h"
 #include "global.h"
@@ -33,94 +40,207 @@
 #include "usb-device.h"
 #include "printf-utils.h"

-static char global_buf[1 << 22]; /* 4MB */
+static char global_buf[1UL << 22]; /* 4MB */

-int disk_init(struct usb_device_info * dev) {
+int disk_open_dev(int maj, int min, int partition, int readonly) {

-	ERROR("RAW mode is not implemented yet");
-	(void)dev;
-	return -1;
+#ifdef __linux__

-}
+	int fd;
+	struct stat st;
+	DIR * dir;
+	struct dirent * dirent;
+	int found;
+	int old_errno;
+	size_t len;
+	char blkdev[1024];

-enum device disk_get_device(struct usb_device_info * dev) {
+	dir = opendir("/dev/");
+	if ( ! dir ) {
+		ERROR_INFO("Cannot open '/dev/' directory");
+		return -1;
+	}
+
+	found = 0;
+
+	while ( ( dirent = readdir(dir) ) ) {
+
+		if ( strncmp(dirent->d_name, ".", sizeof(".")) == 0 || strncmp(dirent->d_name, "..", sizeof("..")) == 0 )
+			continue;
+
+		if ( snprintf(blkdev, sizeof(blkdev), "/dev/%s", dirent->d_name) <= 0 )
+			continue;
+
+		if ( stat(blkdev, &st) != 0 )
+			continue;
+
+		if ( ! S_ISBLK(st.st_mode) )
+			continue;
+
+		if ( makedev(maj, min) != st.st_rdev )
+			continue;
+
+		found = 1;
+		break;
+
+	}
+
+	closedir(dir);
+
+	if ( ! found ) {
+		ERROR("Cannot find block device with id %d:%d", maj, min);
+		return -1;
+	}
+
+	printf("Found block device %s with id %d:%d\n", blkdev, maj, min);
+
+	if ( partition == -1 ) {
+
+		/* Check if block device does not have partitions */
+
+		len = strlen(blkdev);
+		if ( sizeof(blkdev) <= len+2 ) {
+			ERROR("Block device name is too long");
+			return -1;
+		}
+
+		memcpy(blkdev+len, "p1", 3);
+		if ( stat(blkdev, &st) == 0 ) {
+			ERROR("Block device has partitions");
+			return -1;
+		}
+
+		memcpy(blkdev+len, "1", 2);
+		if ( stat(blkdev, &st) == 0 ) {
+			ERROR("Block device has partitions");
+			return -1;
+		}
+
+		blkdev[len] = 0;
+
+		fd = open(blkdev, ((simulate || readonly) ? O_RDONLY : O_RDWR) | O_EXCL | O_NONBLOCK);
+		if ( fd < 0 ) {
+			ERROR_INFO("Cannot open block device %s", blkdev);
+			return -1;
+		} else {
+			if ( fstat(fd, &st) != 0 || ! S_ISBLK(st.st_mode) || makedev(maj, min) != st.st_rdev ) {
+				ERROR("Block device %s does not have id %d:%d\n", blkdev, maj, min);
+				close(fd);
+				return -1;
+			}
+		}
+
+	} else if ( partition > 0 ) {
+
+		/* Select partition */
+
+		len = strlen(blkdev);
+		if ( sizeof(blkdev) <= len+2 ) {
+			ERROR("Block device name is too long");
+			return -1;
+		}
+
+		if ( snprintf(blkdev+len, sizeof(blkdev)-len, "p%d", partition) >= (int)(sizeof(blkdev)-len) ) {
+			ERROR("Block device name is too long");
+			return -1;
+		}
+		fd = open(blkdev, ((simulate || readonly) ? O_RDONLY : O_RDWR) | O_EXCL);
+		if ( fd < 0 && errno == ENOENT ) {
+			if ( snprintf(blkdev+len, sizeof(blkdev)-len, "%d", partition) >= (int)(sizeof(blkdev)-len) ) {
+				ERROR("Block device name is too long");
+				return -1;
+			}
+			fd = open(blkdev, ((simulate || readonly) ? O_RDONLY : O_RDWR) | O_EXCL);
+		}
+		if ( fd < 0 && errno == ENOENT ) {
+			blkdev[len] = 0;
+			fd = open(blkdev, O_RDONLY | O_NONBLOCK);
+			if ( fd < 0 ) {
+				ERROR_INFO("Cannot open block device %s", blkdev);
+			} else {
+				close(fd);
+				ERROR("Block device %s does not have partitions", blkdev);
+			}
+			return -1;
+		}
+		if ( fd < 0 )
+			old_errno = errno;
+
+		printf("Found block device %s for partition %d\n", blkdev, partition);
+
+		if ( fd < 0 ) {
+			errno = old_errno;
+			ERROR_INFO("Cannot open block device %s", blkdev);
+		} else if ( fstat(fd, &st) != 0 || ! S_ISBLK(st.st_mode) ) {
+			ERROR("Block device %s is not block device\n", blkdev);
+			close(fd);
+			return -1;
+		}
+
+	} else {
+
+		ERROR("Invalid partition %d for block device %s", partition, blkdev);
+		fd = -1;
+
+	}
+
+	return fd;
+
+#else

 	ERROR("Not implemented yet");
-	(void)dev;
-	return DEVICE_UNKNOWN;
-
-}
-
-int disk_flash_raw(const char * blkdev, const char * file) {
-
-	ERROR("Not implemented yet");
-	(void)blkdev;
-	(void)file;
+	(void)min;
+	(void)maj;
+	(void)partition;
+	(void)readonly;
 	return -1;

-}
-
-int disk_flash_image(struct usb_device_info * dev, struct image * image) {
-
-	ERROR("Not implemented yet");
-	(void)dev;
-	(void)image;
-	return -1;
+#endif

 }

-int disk_dump_raw(const char * blkdev, const char * file) {
+int disk_dump_dev(int fd, const char * file) {

-	int fd1, fd2;
+	int fd2 = -1;
 	int ret;
 	char * path;
 	uint64_t blksize;
-	size_t need, readed;
+	size_t need, sent;
 	ssize_t size;
-	struct stat st;
 	struct statvfs buf;

-	printf("Dump block device %s to file %s...\n", blkdev, file);
+	printf("Dump block device to file %s...\n", file);

-	if ( stat(blkdev, &st) != 0 ) {
-		ERROR_INFO("Cannot stat block device %s", blkdev);
+#ifdef __linux__
+
+	if ( ioctl(fd, BLKGETSIZE64, &blksize) != 0 ) {
+		ERROR_INFO("Cannot get size of block device");
 		return -1;
 	}

-	if ( ! S_ISBLK(st.st_mode) ) {
-		ERROR("Invalid block device %s", blkdev);
+#else
+
+	blksize = lseek(fd, 0, SEEK_END);
+	if ( (off_t)blksize == (off_t)-1 ) {
+		ERROR_INFO("Cannot get size of block device");
 		return -1;
 	}

-	fd1 = open(blkdev, O_RDONLY);
-
-	if ( fd1 < 0 ) {
-		ERROR_INFO("Cannot open block device %s", blkdev);
+	if ( lseek(fd, 0, SEEK_SET) == (off_t)-1 ) {
+		ERROR_INFO("Cannot seek to begin of block device");
 		return -1;
 	}

-	if ( ioctl(fd1, BLKGETSIZE64, &blksize) != 0 ) {
-		ERROR_INFO("Cannot get size of block device %s", blkdev);
-		close(fd1);
-		return -1;
-	}
-
-	if ( blksize > ULLONG_MAX ) {
-		ERROR("Block device %s is too big", blkdev);
-		close(fd1);
-		return -1;
-	}
+#endif

 	if ( blksize == 0 ) {
-		ERROR("Block device %s has zero size", blkdev);
-		close(fd1);
+		ERROR("Block device has zero size");
 		return -1;
 	}

 	path = strdup(file);
 	if ( ! path ) {
 		ALLOC_ERROR();
-		close(fd1);
 		return -1;
 	}

@@ -130,51 +250,282 @@ int disk_dump_raw(const char * blkdev, const char * file) {

 	if ( ret == 0 && buf.f_bsize * buf.f_bfree < blksize ) {
 		ERROR("Not enough free space (have: %llu, need: %llu)", (unsigned long long int)(buf.f_bsize) * buf.f_bfree, (unsigned long long int)blksize);
-		close(fd1);
 		return -1;
 	}

-	fd2 = creat(file, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
+	if ( ! simulate ) {

-	if ( fd2 < 0 ) {
-		ERROR_INFO("Cannot create file %s", file);
-		close(fd1);
-		return -1;
-	}
+		fd2 = creat(file, S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);

-	readed = 0;
-	printf_progressbar(0, blksize);
-
-	while ( readed < blksize ) {
-		need = blksize - readed;
-		if ( need > sizeof(global_buf) )
-			need = sizeof(global_buf);
-		size = read(fd1, global_buf, need);
-		if ( size == 0 )
-			break;
-		if ( write(fd2, global_buf, size) != size ) {
-			PRINTF_ERROR("Dumping image failed");
-			close(fd1);
-			close(fd2);
+		if ( fd2 < 0 ) {
+			ERROR_INFO("Cannot create file %s", file);
 			return -1;
 		}
-		readed += size;
-		printf_progressbar(readed, blksize);
+
 	}

-	close(fd1);
-	close(fd2);
+	sent = 0;
+	printf_progressbar(0, blksize);
+
+	while ( sent < blksize ) {
+		need = blksize - sent;
+		if ( need > sizeof(global_buf) )
+			need = sizeof(global_buf);
+		size = read(fd, global_buf, need);
+		if ( size == 0 )
+			break;
+		if ( size < 0 ) {
+			PRINTF_ERROR("Reading from block device failed");
+			if ( ! simulate )
+				close(fd2);
+			return -1;
+		}
+		if ( ! simulate ) {
+			if ( write(fd2, global_buf, size) != size ) {
+				PRINTF_ERROR("Dumping image failed");
+				close(fd2);
+				return -1;
+			}
+		}
+		sent += size;
+		printf_progressbar(sent, blksize);
+	}
+
+	if ( ! simulate )
+		close(fd2);
 	return 0;

 }

+int disk_flash_dev(int fd, struct image * image) {
+
+	uint64_t blksize;
+	size_t need, sent;
+	ssize_t size;
+
+	if ( image->type != IMAGE_MMC )
+		ERROR_RETURN("Only mmc images are supported", -1);
+
+	printf("Writing image to block device...\n");
+
+#ifdef __linux__
+
+	if ( ioctl(fd, BLKGETSIZE64, &blksize) != 0 ) {
+		ERROR_INFO("Cannot get size of block device");
+		return -1;
+	}
+
+#else
+
+	blksize = lseek(fd, 0, SEEK_END);
+	if ( (off_t)blksize == (off_t)-1 ) {
+		ERROR_INFO("Cannot get size of block device");
+		return -1;
+	}
+
+	if ( lseek(fd, 0, SEEK_SET) == (off_t)-1 ) {
+		ERROR_INFO("Cannot seek to begin of block device");
+		return -1;
+	}
+
+#endif
+
+	if ( blksize == 0 )
+		ERROR_RETURN("Block device has zero size", -1);
+
+	if ( image->size > blksize )
+		ERROR_RETURN("Image is too big", -1);
+
+	sent = 0;
+	printf_progressbar(0, image->size);
+
+	while ( sent < image->size ) {
+		need = image->size - sent;
+		if ( need > sizeof(global_buf) )
+			need = sizeof(global_buf);
+		size = image_read(image, global_buf, need);
+		if ( size == 0 ) {
+			PRINTF_ERROR("Failed to read image");
+			return -1;
+		}
+		if ( ! simulate ) {
+			if ( write(fd, global_buf, size) != size ) {
+				PRINTF_ERROR("Writing image failed");
+				return -1;
+			}
+		}
+		sent += size;
+		printf_progressbar(sent, image->size);
+	}
+
+	return 0;
+
+}
+
+int disk_init(struct usb_device_info * dev) {
+
+#ifdef __linux__
+
+	int fd;
+	int maj1;
+	int maj2;
+	int min1;
+	int min2;
+	int tmp;
+
+	maj1 = -1;
+	maj2 = -1;
+	min1 = -1;
+	min2 = -1;
+
+	FILE * f;
+	DIR * dir;
+	struct dirent * dirent;
+	char buf[1024];
+	unsigned int devnum;
+	unsigned int busnum;
+
+	libusb_device * device;
+
+	device = libusb_get_device(dev->udev);
+
+	dir = opendir("/sys/dev/block/");
+	if ( ! dir ) {
+		ERROR_INFO("Cannot open '/sys/dev/block/' directory");
+		return -1;
+	}
+
+	while ( ( dirent = readdir(dir) ) ) {
+
+		if ( strncmp(dirent->d_name, ".", sizeof(".")) == 0 || strncmp(dirent->d_name, "..", sizeof("..")) == 0 )
+			continue;
+
+		if ( snprintf(buf, sizeof(buf), "/sys/dev/block/%s/device/../../../../busnum", dirent->d_name) <= 0 )
+			continue;
+
+		f = fopen(buf, "r");
+		if ( ! f )
+			continue;
+
+		if ( fscanf(f, "%u", &busnum) != 1 ) {
+			fclose(f);
+			continue;
+		}
+
+		fclose(f);
+
+		if ( snprintf(buf, sizeof(buf), "/sys/dev/block/%s/device/../../../../devnum", dirent->d_name) <= 0 )
+			continue;
+
+		f = fopen(buf, "r");
+		if ( ! f )
+			continue;
+
+		if ( fscanf(f, "%u", &devnum) != 1 ) {
+			fclose(f);
+			continue;
+		}
+
+		fclose(f);
+
+		if ( (unsigned int)libusb_get_device_address(device) != devnum )
+			continue;
+
+		if ( (unsigned int)libusb_get_bus_number(device) != busnum )
+			continue;
+
+		if ( sscanf(dirent->d_name, "%d:%d", &maj2, &min2) != 2 ) {
+			maj2 = -1;
+			min2 = -1;
+			continue;
+		}
+
+		if ( maj1 != -1 && min1 != -1 && maj2 != -1 && min2 != -1 )
+			break;
+
+		maj1 = maj2;
+		min1 = min2;
+		maj2 = -1;
+		min2 = -1;
+
+	}
+
+	closedir(dir);
+
+	if ( maj1 == -1 || min1 == -1 ) {
+		ERROR("Cannot find id for mmc block disk device");
+		return -1;
+	}
+
+	/* Ensure that maj1:min1 is first device and maj2:min2 is second device */
+	if ( min2 != -1 && min2 < min1 ) {
+		tmp = min1;
+		min1 = min2;
+		min2 = tmp;
+		tmp = maj1;
+		maj1 = maj2;
+		maj2 = tmp;
+	}
+
+	/* RX-51, RM-680 and RM-696 export MyDocs in first usb device and just first partion, so host system see whole device without MBR table */
+	if ( dev->device == DEVICE_RX_51 || dev->device == DEVICE_RM_680 || dev->device == DEVICE_RM_696 )
+		fd = disk_open_dev(maj1, min1, -1, simulate ? 1 : 0);
+	/* Other devices can export SD card as first partition and export whole mmc device, so host system will see MBR table */
+	else if ( maj2 != -1 && min2 != -1 )
+		fd = disk_open_dev(maj2, min2, 1, simulate ? 1 : 0);
+	else
+		fd = disk_open_dev(maj1, min1, 1, simulate ? 1 : 0);
+
+	if ( fd < 0 )
+		return -1;
+
+	dev->data = fd;
+	return 0;
+
+#else
+
+	ERROR("Not implemented yet");
+	(void)dev;
+	return -1;
+
+#endif
+
+}
+
+void disk_exit(struct usb_device_info * dev) {
+
+	if ( dev->data >= 0 )
+		close(dev->data);
+
+}
+
+enum device disk_get_device(struct usb_device_info * dev) {
+
+	return dev->device;
+
+}
+
+int disk_flash_image(struct usb_device_info * dev, struct image * image) {
+
+	int ret;
+
+	printf("Flash image:\n");
+	image_print_info(image);
+
+	ret = disk_flash_dev(dev->data, image);
+	if ( ret == 0 )
+		printf("Done\n");
+
+	return ret;
+
+}
+
 int disk_dump_image(struct usb_device_info * dev, enum image_type image, const char * file) {

-	ERROR("Not implemented yet");
-	(void)dev;
-	(void)image;
-	(void)file;
-	return -1;
+	if ( image != IMAGE_MMC )
+		ERROR_RETURN("Only mmc images are supported", -1);
+
+	return disk_dump_dev(dev->data, file);

 }

@@ -24,11 +24,13 @@
 #include "usb-device.h"

 int disk_init(struct usb_device_info * dev);
+void disk_exit(struct usb_device_info * dev);

 enum device disk_get_device(struct usb_device_info * dev);

-int disk_flash_raw(const char * blkdev, const char * file);
-int disk_dump_raw(const char * blkdev, const char * file);
+int disk_open_dev(int maj, int min, int partition, int readonly);
+int disk_dump_dev(int fd, const char * file);
+int disk_flash_dev(int fd, struct image * image);

 int disk_flash_image(struct usb_device_info * dev, struct image * image);
 int disk_dump_image(struct usb_device_info * dev, enum image_type image, const char * file);
@@ -34,11 +34,13 @@
 #include "image.h"
 #include "fiasco.h"

+#define CHECKSUM(checksum, buf, size) do { size_t _i; for ( _i = 0; _i < size; _i++ ) checksum += ((unsigned char *)buf)[_i]; } while (0)
 #define FIASCO_READ_ERROR(fiasco, ...) do { ERROR_INFO(__VA_ARGS__); fiasco_free(fiasco); return NULL; } while (0)
 #define FIASCO_WRITE_ERROR(file, fd, ...) do { ERROR_INFO_STR(file, __VA_ARGS__); if ( fd >= 0 ) close(fd); return -1; } while (0)
 #define READ_OR_FAIL(fiasco, buf, size) do { if ( read(fiasco->fd, buf, size) != size ) { FIASCO_READ_ERROR(fiasco, "Cannot read %d bytes", size); } } while (0)
-#define READ_OR_RETURN(fiasco, buf, size) do { if ( read(fiasco->fd, buf, size) != size ) return fiasco; } while (0)
-#define WRITE_OR_FAIL(file, fd, buf, size) do { if ( ! simulate ) { if ( write(fd, buf, size) != (ssize_t)size ) { FIASCO_WRITE_ERROR(file, fd, "Cannot write %d bytes", size); } } } while (0)
+#define READ_OR_RETURN(fiasco, checksum, buf, size) do { if ( read(fiasco->fd, buf, size) != size ) return fiasco; CHECKSUM(checksum, buf, size); } while (0)
+#define WRITE_OR_FAIL_FREE(file, fd, buf, size, var) do { if ( ! simulate ) { if ( write(fd, buf, size) != (ssize_t)size ) { free(var); FIASCO_WRITE_ERROR(file, fd, "Cannot write %d bytes", size); } } } while (0)
+#define WRITE_OR_FAIL(file, fd, buf, size) WRITE_OR_FAIL_FREE(file, fd, buf, size, NULL)

 struct fiasco * fiasco_alloc_empty(void) {

@@ -64,9 +66,16 @@ struct fiasco * fiasco_alloc_from_file(const char * file) {
 	char hwrevs[1024];
 	char version[257];
 	char layout[257];
+	uint8_t asicidx;
+	uint8_t devicetype;
+	uint8_t deviceidx;
+	uint8_t checksum;
+	uint32_t address;
 	uint16_t hash;
 	off_t offset;
 	struct image * image;
+	struct image_part * image_part;
+	struct image_part * image_parts;

 	char hwrev[9];
 	unsigned char buf[512];
@@ -103,11 +112,11 @@ struct fiasco * fiasco_alloc_from_file(const char * file) {
 		READ_OR_FAIL(fiasco, buf, length8);
 		if ( byte == 0xe8 ) {
 			memset(fiasco->name, 0, sizeof(fiasco->name));
-			strncpy(fiasco->name, (char *)buf, length8);
+			memcpy(fiasco->name, buf, length8);
 			VERBOSE("Fiasco name: %s\n", fiasco->name);
 		} else if ( byte == 0x31 ) {
 			memset(fiasco->swver, 0, sizeof(fiasco->swver));
-			strncpy(fiasco->swver, (char *)buf, length8);
+			memcpy(fiasco->swver, buf, length8);
 			VERBOSE("SW version: %s\n", fiasco->swver);
 		} else {
 			VERBOSE("Unknown header %#x\n", byte);
@@ -119,23 +128,41 @@ struct fiasco * fiasco_alloc_from_file(const char * file) {
 	while ( 1 ) {

 		/* If end of file, return fiasco image */
-		READ_OR_RETURN(fiasco, buf, 7);
+		checksum = 0x00;
+		READ_OR_RETURN(fiasco, checksum, buf, 1);

-		/* Header of next image */
-		if ( ! buf[0] == 0x54 && buf[2] == 0x2E && buf[3] == 0x19 && buf[4] == 0x01 && buf[5] == 0x01 && buf[6] == 0x00 ) {
+		/* Header of next image (0x54) */
+		if ( buf[0] != 0x54 ) {
 			ERROR("Invalid next image header");
 			return fiasco;
 		}

-		count8 = buf[1];
-		if ( count8 > 0 )
-			--count8;
+		checksum = 0x00;

-		READ_OR_RETURN(fiasco, &hash, 2);
+		READ_OR_RETURN(fiasco, checksum, &count8, 1);
+
+		if ( count8 == 0 ) {
+			ERROR("No section in image header");
+			return fiasco;
+		}
+
+		READ_OR_RETURN(fiasco, checksum, buf, 2);
+
+		/* File data section (0x2E) with length of 25 bytes */
+		if ( buf[0] != 0x2E || buf[1] != 25 ) {
+			ERROR("First section in image header is not file data with length of 25 bytes");
+			return fiasco;
+		}
+
+		READ_OR_RETURN(fiasco, checksum, &asicidx, 1);
+		READ_OR_RETURN(fiasco, checksum, &devicetype, 1);
+		READ_OR_RETURN(fiasco, checksum, &deviceidx, 1);
+
+		READ_OR_RETURN(fiasco, checksum, &hash, 2);
 		hash = ntohs(hash);

 		memset(type, 0, sizeof(type));
-		READ_OR_RETURN(fiasco, type, 12);
+		READ_OR_RETURN(fiasco, checksum, type, 12);

 		byte = type[0];
 		if ( byte == 0xFF )
@@ -143,26 +170,35 @@ struct fiasco * fiasco_alloc_from_file(const char * file) {

 		VERBOSE(" %s\n", type);

-		READ_OR_RETURN(fiasco, &length, 4);
+		READ_OR_RETURN(fiasco, checksum, &length, 4);
 		length = ntohl(length);

-		/* unknown */
-		READ_OR_RETURN(fiasco, buf, 4);
+		/* load address (unused) */
+		READ_OR_RETURN(fiasco, checksum, &address, 4);
+
+		/* end of file data section */
+		--count8;

 		VERBOSE("   size:    %d bytes\n", length);
-		VERBOSE("   hash:    %#04x\n", hash);
+		VERBOSE("   address: 0x%04x\n", address);
+		VERBOSE("   hash:    0x%04x\n", hash);
+		VERBOSE("   asic idx:    %d\n", asicidx);
+		VERBOSE("   device type: %d\n", devicetype);
+		VERBOSE("   device idx:  %d\n", deviceidx);
 		VERBOSE("   subsections: %d\n", count8);

 		memset(device, 0, sizeof(device));
 		memset(hwrevs, 0, sizeof(hwrevs));
 		memset(version, 0, sizeof(version));
 		memset(layout, 0, sizeof(layout));
+		image_part = NULL;
+		image_parts = NULL;

 		while ( count8 > 0 ) {

-			READ_OR_RETURN(fiasco, &byte, 1);
-			READ_OR_RETURN(fiasco, &length8, 1);
-			READ_OR_RETURN(fiasco, buf, length8);
+			READ_OR_RETURN(fiasco, checksum, &byte, 1);
+			READ_OR_RETURN(fiasco, checksum, &length8, 1);
+			READ_OR_RETURN(fiasco, checksum, buf, length8);

 			VERBOSE("   subinfo\n");
 			VERBOSE("     length: %d\n", length8);
@@ -170,14 +206,14 @@ struct fiasco * fiasco_alloc_from_file(const char * file) {

 			if ( byte == '1' ) {
 				memset(version, 0, sizeof(version));
-				strncpy(version, (char *)buf, length8);
+				memcpy(version, buf, length8);
 				VERBOSE("version string\n");
 				VERBOSE("       version: %s\n", version);
 			} else if ( byte == '2' ) {
 				int tmp = length8;
 				if ( tmp > 16 ) tmp = 16;
 				memset(device, 0, sizeof(device));
-				strncpy(device, (char *)buf, tmp);
+				memcpy(device, buf, tmp);
 				VERBOSE("hw revision\n");
 				VERBOSE("       device: %s\n", device);
 				pbuf = buf + strlen(device) + 1;
@@ -188,51 +224,128 @@ struct fiasco * fiasco_alloc_from_file(const char * file) {
 					tmp = buf + length8 - pbuf;
 					if ( tmp > 8 ) tmp = 8;
 					memset(hwrev, 0, sizeof(hwrev));
-					strncpy(hwrev, (char *)pbuf, tmp);
+					memcpy(hwrev, pbuf, tmp);
 					if ( ! hwrevs[0] )
 						strcpy(hwrevs, hwrev);
 					else {
-						/* TODO: check if hwrevs has enough size */
-						strcat(hwrevs, ",");
-						strcat(hwrevs, hwrev);
+						size_t len1 = strlen(hwrevs);
+						size_t len2 = strlen(hwrev);
+						if ( len1 + len2 + 2 < sizeof(hwrevs) ) {
+							hwrevs[len1] = ',';
+							memcpy(hwrevs+len1+1, hwrev, len2+1);
+						}
 					}
 					VERBOSE("       hw revision: %s\n", hwrev);
 					pbuf += strlen(hwrev) + 1;
 				}
 			} else if ( byte == '3' ) {
 				memset(layout, 0, sizeof(layout));
-				strncpy(layout, (char *)buf, length8);
+				memcpy(layout, buf, length8);
 				VERBOSE("layout\n");
+			} else if ( byte == '4' ) {
+				VERBOSE("data part\n");
+				if ( length8 < 16 ) {
+					VERBOSE("       (damaged)\n");
+				} else {
+					if ( image_parts ) {
+						image_part->next = calloc(1, sizeof(struct image_part));
+						if ( ! image_part->next )
+							FIASCO_READ_ERROR(fiasco, "Cannot allocate image");
+						image_part = image_part->next;
+					} else {
+						image_parts = calloc(1, sizeof(struct image_part));
+						if ( ! image_parts )
+							FIASCO_READ_ERROR(fiasco, "Cannot allocate image");
+						image_part = image_parts;
+					}
+					image_part->offset = ntohl(*(uint32_t *)&buf[4]);
+					image_part->size = ntohl(*(uint32_t *)&buf[12]);
+					if ( length8 > 16 ) {
+						image_part->name = calloc(1, length8-16+1);
+						if ( image_part->name )
+							memcpy(image_part->name, &buf[16], length8-16);
+					}
+					VERBOSE("       unknown: 0x%02x 0x%02x 0x%02x 0x%02x\n", buf[0], buf[1], buf[2], buf[3]);
+					VERBOSE("       offset: %u bytes\n", image_part->offset);
+					VERBOSE("       unknown: 0x%02x 0x%02x 0x%02x 0x%02x\n", buf[8], buf[9], buf[10], buf[11]);
+					VERBOSE("       size: %u bytes\n", image_part->size);
+					if ( image_part->name )
+						VERBOSE("       partition name: %s\n", image_part->name);
+				}
+			} else if ( byte == 0x2F ) {
+				VERBOSE("partition info\n");
+				if ( length8 < 15 ) {
+					VERBOSE("       (damaged)\n");
+				} else {
+					unsigned int i;
+					unsigned int part_type;
+					unsigned int part_attr;
+					unsigned int part_size;
+					unsigned int num_parts = ((unsigned)buf[11] << 24) | ((unsigned)buf[12] << 16) | ((unsigned)buf[13] << 8) | ((unsigned)buf[14] << 0);
+					VERBOSE("       asic idx:    %d\n", buf[0]);
+					VERBOSE("       device type: %d\n", buf[1]);
+					VERBOSE("       device idx:  %d\n", buf[2]);
+					VERBOSE("       version:     0x%02x%02x%02x%02x\n", buf[3], buf[4], buf[5], buf[6]);
+					VERBOSE("       spare:       0x%02x%02x%02x%02x\n", buf[7], buf[8], buf[9], buf[10]);
+					VERBOSE("       partitions:  %d\n", num_parts);
+					for ( i = 0; i < num_parts; i++ ) {
+						VERBOSE("       partition\n");
+						if ( 20 + i*6 >= length8 ) {
+							VERBOSE("          (damaged)\n");
+							continue;
+						}
+						part_type = buf[15 + i*6];
+						part_attr = buf[16 + i*6];
+						part_size = ((unsigned)buf[17 + i*6] << 24) | ((unsigned)buf[18 + i*6] << 16) | ((unsigned)buf[19 + i*6] << 8) | ((unsigned)buf[20 + i*6] << 0);
+						VERBOSE("          type: 0x%02x (%s)\n", part_type, part_type == 0x03 ? "kernel" : part_type == 0x04 ? "initfs" : part_type == 0x05 ? "rootfs" : "unknown");
+						VERBOSE("          attribute: 0x%02x (%s)\n", part_attr, part_attr == 0x1 ? "read/write" : part_attr == 0x2 ? "read-only" : "unknown");
+						if ( part_size == 0 )
+							VERBOSE("          size: remaining space\n");
+						else
+							VERBOSE("          size: %u bytes\n", part_size);
+					}
+				}
 			} else {
-				VERBOSE("unknown ('%c':%#x)\n", byte, byte);
+				int i;
+				VERBOSE("unknown (%#x)\n", byte);
+				VERBOSE("       hexdump:");
+				for ( i = 0; i < length8; i++ ) VERBOSE(" 0x%02x", buf[i]);
+				VERBOSE("\n");
 			}

 			--count8;
 		}

-		/* unknown */
-		READ_OR_RETURN(fiasco, buf, 1);
+		/* checksum */
+		READ_OR_RETURN(fiasco, checksum, buf, 1);
+		VERBOSE("   subinfo checksum: 0x%02x\n", buf[0]);
+
+		if ( ! noverify && buf[0] != 0x00 && checksum != 0xFF ) {
+			ERROR("Image header subinfo checksum mishmash (counted 0x%02x, got 0x%02x)", (0xFF - checksum + buf[0]) & 0xFF, buf[0]);
+			return fiasco;
+		}

 		offset = lseek(fiasco->fd, 0, SEEK_CUR);
+		if ( offset == (off_t)-1 )
+			FIASCO_READ_ERROR(fiasco, "Cannot get offset of file");

 		VERBOSE("   version: %s\n", version);
 		VERBOSE("   device: %s\n", device);
 		VERBOSE("   hwrevs: %s\n", hwrevs);
 		VERBOSE("   data at: %#08x\n", (unsigned int)offset);

-		image = image_alloc_from_shared_fd(fiasco->fd, length, offset, hash, type, device, hwrevs, version, layout);
+		image = image_alloc_from_shared_fd(fiasco->fd, length, offset, hash, type, device, hwrevs, version, layout, image_parts);

 		if ( ! image )
 			FIASCO_READ_ERROR(fiasco, "Cannot allocate image");

 		fiasco_add_image(fiasco, image);

-		lseek(fiasco->fd, offset+length, SEEK_SET);
+		if ( lseek(fiasco->fd, offset+length, SEEK_SET) == (off_t)-1 )
+			FIASCO_READ_ERROR(fiasco, "Cannot seek to next image in file");

 	}

-	return fiasco;
-
 }

 void fiasco_free(struct fiasco * fiasco) {
@@ -269,10 +382,12 @@ int fiasco_write_to_file(struct fiasco * fiasco, const char * file) {
 	uint32_t length;
 	uint16_t hash;
 	uint8_t length8;
+	uint8_t checksum;
 	char ** device_hwrevs_bufs;
 	const char * str;
 	const char * type;
 	struct image_list * image_list;
+	struct image_part * image_part;
 	struct image * image;
 	unsigned char buf[4096];

@@ -284,10 +399,10 @@ int fiasco_write_to_file(struct fiasco * fiasco, const char * file) {
 	if ( ! fiasco->first )
 		FIASCO_WRITE_ERROR(file, fd, "Nothing to write");

-	if ( fiasco->name && strlen(fiasco->name)+1 > UINT8_MAX )
+	if ( strlen(fiasco->name)+1 > UINT8_MAX )
 		FIASCO_WRITE_ERROR(file, fd, "Fiasco name string is too long");

-	if ( fiasco->swver && strlen(fiasco->swver)+1 > UINT8_MAX )
+	if ( strlen(fiasco->swver)+1 > UINT8_MAX )
 		FIASCO_WRITE_ERROR(file, fd, "SW version string is too long");

 	if ( ! simulate ) {
@@ -350,12 +465,6 @@ int fiasco_write_to_file(struct fiasco * fiasco, const char * file) {

 		type = image_type_to_string(image->type);

-		device_hwrevs_bufs = device_list_alloc_to_bufs(image->devices);
-
-		device_count = 0;
-		if ( device_hwrevs_bufs && device_hwrevs_bufs[0] )
-			for ( ; device_hwrevs_bufs[device_count]; ++device_count );
-
 		if ( ! type )
 			FIASCO_WRITE_ERROR(file, fd, "Unknown image type");

@@ -365,10 +474,18 @@ int fiasco_write_to_file(struct fiasco * fiasco, const char * file) {
 		if ( image->layout && strlen(image->layout) > UINT8_MAX )
 			FIASCO_WRITE_ERROR(file, fd, "Image layout is too long");

+		device_hwrevs_bufs = device_list_alloc_to_bufs(image->devices);
+
+		device_count = 0;
+		if ( device_hwrevs_bufs && device_hwrevs_bufs[0] )
+			for ( ; device_hwrevs_bufs[device_count]; ++device_count );
+
 		printf("Writing image header...\n");

 		/* signature */
-		WRITE_OR_FAIL(file, fd, "T", 1);
+		WRITE_OR_FAIL_FREE(file, fd, "T", 1, device_hwrevs_bufs);
+
+		checksum = 0x00;

 		/* number of subsections */
 		length8 = device_count+1;
@@ -376,53 +493,109 @@ int fiasco_write_to_file(struct fiasco * fiasco, const char * file) {
 			++length8;
 		if ( image->layout )
 			++length8;
-		WRITE_OR_FAIL(file, fd, &length8, 1);
+		if ( image->parts ) {
+			for ( image_part = image->parts; image_part; image_part = image_part->next )
+				++length8;
+		} else {
+			++length;
+		}
+		WRITE_OR_FAIL_FREE(file, fd, &length8, 1, device_hwrevs_bufs);
+		CHECKSUM(checksum, &length8, 1);

-		/* unknown */
-		WRITE_OR_FAIL(file, fd, "\x2e\x19\x01\x01\x00", 5);
+		/* file data: asic index: APE (0x01), device type: NAND (0x01), device index: 0 */
+		WRITE_OR_FAIL_FREE(file, fd, "\x2e\x19\x01\x01\x00", 5, device_hwrevs_bufs);
+		CHECKSUM(checksum, "\x2e\x19\x01\x01\x00", 5);

 		/* checksum */
 		hash = htons(image->hash);
-		WRITE_OR_FAIL(file, fd, &hash, 2);
+		WRITE_OR_FAIL_FREE(file, fd, &hash, 2, device_hwrevs_bufs);
+		CHECKSUM(checksum, &hash, 2);

 		/* image type name */
 		memset(buf, 0, 12);
 		strncpy((char *)buf, type, 12);
-		WRITE_OR_FAIL(file, fd, buf, 12);
+		WRITE_OR_FAIL_FREE(file, fd, buf, 12, device_hwrevs_bufs);
+		CHECKSUM(checksum, buf, 12);

 		/* image size */
 		size = htonl(image->size);
-		WRITE_OR_FAIL(file, fd, &size, 4);
+		WRITE_OR_FAIL_FREE(file, fd, &size, 4, device_hwrevs_bufs);
+		CHECKSUM(checksum, &size, 4);

-		/* unknown */
-		WRITE_OR_FAIL(file, fd, "\x00\x00\x00\x00", 4);
+		/* image load address (unused always zero) */
+		WRITE_OR_FAIL_FREE(file, fd, "\x00\x00\x00\x00", 4, device_hwrevs_bufs);
+		CHECKSUM(checksum, "\x00\x00\x00\x00", 4);

 		/* append version subsection */
 		if ( image->version ) {
-			WRITE_OR_FAIL(file, fd, "1", 1); /* 1 - version */
-			length8 = strlen(image->version)+1;
-			WRITE_OR_FAIL(file, fd, &length8, 1);
-			WRITE_OR_FAIL(file, fd, image->version, length8);
+			WRITE_OR_FAIL_FREE(file, fd, "1", 1, device_hwrevs_bufs); /* 1 - version */
+			length8 = strlen(image->version)+1; /* +1 for NULL term */
+			WRITE_OR_FAIL_FREE(file, fd, &length8, 1, device_hwrevs_bufs);
+			WRITE_OR_FAIL_FREE(file, fd, image->version, length8, device_hwrevs_bufs);
+			CHECKSUM(checksum, "1", 1);
+			CHECKSUM(checksum, &length8, 1);
+			CHECKSUM(checksum, image->version, length8);
 		}

 		/* append device & hwrevs subsection */
 		for ( i = 0; i < device_count; ++i ) {
-			WRITE_OR_FAIL(file, fd, "2", 1); /* 2 - device & hwrevs */
-			WRITE_OR_FAIL(file, fd, &device_hwrevs_bufs[i][0], 1);
-			WRITE_OR_FAIL(file, fd, device_hwrevs_bufs[i]+1, ((uint8_t *)(device_hwrevs_bufs[i]))[0]);
+			WRITE_OR_FAIL_FREE(file, fd, "2", 1, device_hwrevs_bufs); /* 2 - device & hwrevs */
+			length8 = ((uint8_t *)(device_hwrevs_bufs[i]))[0];
+			WRITE_OR_FAIL_FREE(file, fd, &length8, 1, device_hwrevs_bufs);
+			WRITE_OR_FAIL_FREE(file, fd, device_hwrevs_bufs[i]+1, length8, device_hwrevs_bufs);
+			CHECKSUM(checksum, "2", 1);
+			CHECKSUM(checksum, &length8, 1);
+			CHECKSUM(checksum, device_hwrevs_bufs[i]+1, length8);
 		}
 		free(device_hwrevs_bufs);

 		/* append layout subsection */
 		if ( image->layout ) {
-			length8 = strlen(image->layout);
 			WRITE_OR_FAIL(file, fd, "3", 1); /* 3 - layout */
+			length8 = strlen(image->layout);
 			WRITE_OR_FAIL(file, fd, &length8, 1);
 			WRITE_OR_FAIL(file, fd, image->layout, length8);
+			CHECKSUM(checksum, "3", 1);
+			CHECKSUM(checksum, &length8, 1);
+			CHECKSUM(checksum, image->layout, length8);
 		}

-		/* dummy byte - end of all subsections */
-		WRITE_OR_FAIL(file, fd, "\x00", 1);
+		if ( image->parts ) {
+			/* for each image part append subsection */
+			for ( image_part = image->parts; image_part; image_part = image_part->next ) {
+				WRITE_OR_FAIL(file, fd, "4", 1); /* 4 - image data part */
+				CHECKSUM(checksum, "4", 1);
+				length = 16 + (image_part->name ? strlen(image_part->name) : 0);
+				length8 = length <= UINT8_MAX ? length : UINT8_MAX;
+				WRITE_OR_FAIL(file, fd, &length8, 1);
+				CHECKSUM(checksum, &length8, 1);
+				WRITE_OR_FAIL(file, fd, "\x00\x00\x00\x00", 4); /* unknown */
+				CHECKSUM(checksum, "\x00\x00\x00\x00", 4);
+				size = htonl(image_part->offset);
+				WRITE_OR_FAIL(file, fd, &size, 4);
+				CHECKSUM(checksum, &size, 4);
+				WRITE_OR_FAIL(file, fd, "\x00\x00\x00\x00", 4); /* unknown */
+				CHECKSUM(checksum, "\x00\x00\x00\x00", 4);
+				size = htonl(image_part->size);
+				WRITE_OR_FAIL(file, fd, &size, 4);
+				CHECKSUM(checksum, &size, 4);
+				if ( image_part->name ) {
+					WRITE_OR_FAIL(file, fd, image_part->name, length8-16);
+					CHECKSUM(checksum, image_part->name, (size_t)length8-16);
+				}
+			}
+		} else {
+			/* append one image data part subsection */;
+			WRITE_OR_FAIL(file, fd, "4\x10\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 14);
+			CHECKSUM(checksum, "4\x10\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 14);
+			size = htonl(image->size);
+			WRITE_OR_FAIL(file, fd, &size, 4);
+			CHECKSUM(checksum, &size, 4);
+		}
+
+		/* checksum of header */
+		checksum = 0xFF - checksum;
+		WRITE_OR_FAIL(file, fd, &checksum, 1);

 		printf("Writing image data...\n");

@@ -441,7 +614,9 @@ int fiasco_write_to_file(struct fiasco * fiasco, const char * file) {

 	}

-	close(fd);
+	if ( ! simulate )
+		close(fd);
+
 	printf("\nDone\n\n");
 	return 0;

@@ -449,12 +624,14 @@ int fiasco_write_to_file(struct fiasco * fiasco, const char * file) {

 int fiasco_unpack(struct fiasco * fiasco, const char * dir) {

-	int fd = -1;
-	char * name = NULL;
-	char * layout_name = NULL;
+	int fd;
+	char * name;
+	char * layout_name;
 	struct image * image;
+	struct image_part * image_part;
 	struct image_list * image_list;
-	uint32_t size;
+	uint32_t offset, size, need, total_size, written;
+	int part_num;
 	char cwd[256];
 	unsigned char buf[4096];

@@ -480,11 +657,9 @@ int fiasco_unpack(struct fiasco * fiasco, const char * dir) {

 	while ( image_list ) {

-		image = image_list->image;
+		fd = -1;

-		name = image_name_alloc_from_values(image);
-		if ( ! name )
-			return -1;
+		image = image_list->image;

 		printf("\n");
 		printf("Unpacking image...\n");
@@ -492,56 +667,108 @@ int fiasco_unpack(struct fiasco * fiasco, const char * dir) {

 		if ( image->layout ) {

-			layout_name = calloc(1, strlen(name) + strlen(".layout") + 1);
-			if ( ! layout_name )
+			name = image_name_alloc_from_values(image, -1);
+			if ( ! name )
 				ALLOC_ERROR_RETURN(-1);

-			sprintf(layout_name, "%s.layout", name);
+			layout_name = calloc(1, strlen(name) + sizeof("_layout.txt")-1 + 1);
+			if ( ! layout_name ) {
+				free(name);
+				ALLOC_ERROR_RETURN(-1);
+			}
+
+			sprintf(layout_name, "%s_layout.txt", name);
+			free(name);

 			printf("    Layout file: %s\n", layout_name);

-		}
-
-		printf("    Output file: %s\n", name);
-
-		if ( ! simulate ) {
-			fd = open(name, O_RDWR|O_CREAT|O_TRUNC, 0644);
-			if ( fd < 0 ) {
-				ERROR_INFO("Cannot create output file %s", name);
-				return -1;
-			}
-		}
-
-		free(name);
-
-		image_seek(image, 0);
-		while ( 1 ) {
-			size = image_read(image, buf, sizeof(buf));
-			if ( size == 0 )
-				break;
-			WRITE_OR_FAIL(name, fd, buf, size);
-		}
-
-		close(fd);
-
-		if ( image->layout ) {
-
 			if ( ! simulate ) {
 				fd = open(layout_name, O_RDWR|O_CREAT|O_TRUNC, 0644);
 				if ( fd < 0 ) {
 					ERROR_INFO("Cannot create layout file %s", layout_name);
+					free(layout_name);
+					return -1;
+				}
+
+				size = strlen(image->layout);
+
+				if ( write(fd, image->layout, size) != (ssize_t)size ) {
+					ERROR_INFO_STR(layout_name, "Cannot write %d bytes", size);
+					close(fd);
+					free(layout_name);
 					return -1;
 				}
 			}

 			free(layout_name);

-			WRITE_OR_FAIL(layout_name, fd, image->layout, (int)strlen(image->layout));
-
-			close(fd);
+			if ( ! simulate ) {
+				close(fd);
+				fd = -1;
+			}

 		}

+		part_num = 0;
+		image_part = image->parts;
+
+		do {
+
+			offset = image_part ? image_part->offset : 0;
+			total_size = image_part ? image_part->size : image->size;
+
+			name = image_name_alloc_from_values(image, image_part ? part_num : -1);
+			if ( ! name )
+				ALLOC_ERROR_RETURN(-1);
+
+			if ( image_part && ( part_num > 0 || image_part->next ) )
+				printf("    Output file part %d: %s\n", part_num+1, name);
+			else
+				printf("    Output file: %s\n", name);
+
+			if ( ! simulate ) {
+				fd = open(name, O_RDWR|O_CREAT|O_TRUNC, 0644);
+				if ( fd < 0 ) {
+					ERROR_INFO("Cannot create output file %s", name);
+					free(name);
+					return -1;
+				}
+			}
+
+			written = 0;
+			image_seek(image, offset);
+			while ( written < total_size ) {
+				need = total_size - written;
+				if ( need > sizeof(buf) )
+					need = sizeof(buf);
+				size = image_read(image, buf, need);
+				if ( size == 0 )
+					break;
+				if ( ! simulate ) {
+					if ( write(fd, buf, size) != (ssize_t)size ) {
+						ERROR_INFO_STR(name, "Cannot write %d bytes", size);
+						close(fd);
+						free(name);
+						return -1;
+					}
+				}
+				written += size;
+			}
+
+			free(name);
+
+			if ( ! simulate ) {
+				close(fd);
+				fd = -1;
+			}
+
+			if ( image_part ) {
+				image_part = image_part->next;
+				part_num++;
+			}
+
+		} while ( image_part );
+
 		image_list = image_list->next;

 	}
@@ -5,6 +5,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <errno.h>
+#include <time.h>

 extern int simulate;
 extern int noverify;
@@ -21,4 +22,19 @@ extern int verbose;
 #define ALLOC_ERROR() do { ERROR("Cannot allocate memory"); } while (0)
 #define ALLOC_ERROR_RETURN(...) do { ALLOC_ERROR(); return __VA_ARGS__; } while (0)

+#define MSLEEP(msec) do { nanosleep(&(struct timespec){ (msec / 1000), (1000L * 1000L * (msec % 1000)) }, NULL); } while (0)
+
+static inline void * MEMMEM(void *haystack, size_t haystacklen, const void *needle, size_t needlelen) {
+	for ( size_t i = 0; i < haystacklen; ++i ) {
+		for ( size_t j = 0; j < needlelen; ++j ) {
+			if ( ((char *)haystack)[i+j] != ((const char *)needle)[j] )
+				break;
+			if ( j != needlelen - 1 )
+				continue;
+			return (char *)haystack + i;
+		}
+	}
+	return NULL;
+}
+
 #endif
@@ -30,9 +30,6 @@
 #include "device.h"
 #include "image.h"

-#define IMAGE_STORE_CUR(image) do { if ( image->is_shared_fd ) { image->cur = lseek(image->fd, 0, SEEK_CUR) - image->offset; if ( image->cur > image->size ) image->cur = image->size; } } while (0)
-#define IMAGE_RESTORE_CUR(image) do { if ( image->is_shared_fd ) { if ( image->cur <= image->size ) lseek(image->fd, image->offset + image->cur, SEEK_SET); else lseek(image->fd, image->offset + image->size, SEEK_SET); } } while (0)
-
 /* format: type-device:hwrevs_version */
 static void image_missing_values_from_name(struct image * image, const char * name) {

@@ -76,7 +73,7 @@ static void image_missing_values_from_name(struct image * image, const char * na
 		image->type = image_type_from_string(type);
 	free(type);

-	if ( ! image->devices || image->devices->device || image->devices->device == DEVICE_ANY ) {
+	if ( ! image->devices || ! image->devices->device || image->devices->device == DEVICE_ANY ) {
 		new_device = device_from_string(device);
 		if ( new_device ) {
 			if ( ! image->devices ) image->devices = calloc(1, sizeof(struct device_list));
@@ -88,20 +85,19 @@ static void image_missing_values_from_name(struct image * image, const char * na

 	if ( image->devices && image->devices->device && ! image->devices->hwrevs )
 		image->devices->hwrevs = hwrevs_alloc_from_string(hwrevs);
-	else
-		free(hwrevs);

 	if ( ! image->version )
 		image->version = version;
 	else
 		free(version);

+	free(hwrevs);
 	free(str);

 }

 /* format: type-device:hwrevs_version */
-char * image_name_alloc_from_values(struct image * image) {
+char * image_name_alloc_from_values(struct image * image, int part_num) {

 	char * name;
 	char * ptr;
@@ -109,6 +105,8 @@ char * image_name_alloc_from_values(struct image * image) {
 	size_t length;
 	const char * type;
 	const char * device;
+	struct image_part * part;
+	int i;

 	type = image_type_to_string(image->type);

@@ -125,6 +123,14 @@ char * image_name_alloc_from_values(struct image * image) {
 	else
 		hwrevs = NULL;

+	part = image->parts;
+
+	if ( part && ( !part->next || part_num < 0 ) )
+		part = NULL;
+
+	for ( i = 0; i < part_num && part; i++ )
+		part = part->next;
+
 	length = 1 + strlen(type);

 	if ( device )
@@ -133,10 +139,16 @@ char * image_name_alloc_from_values(struct image * image) {
 		length += 1 + strlen(hwrevs);
 	if ( image->version )
 		length += 1 + strlen(image->version);
+	if ( part )
+		length += 4 + 3; /* 3 <= strlen(part_num) */
+	if ( part && part->name )
+		length += 1 + strlen(part->name);

 	name = calloc(1, length);
-	if ( ! name )
+	if ( ! name ) {
+		free(hwrevs);
 		ALLOC_ERROR_RETURN(NULL);
+	}

 	strcpy(name, type);
 	ptr = name + strlen(name);
@@ -148,21 +160,29 @@ char * image_name_alloc_from_values(struct image * image) {
 	if ( image->version )
 		ptr += sprintf(ptr, "_%s", image->version);

+	if ( part ) {
+		ptr += sprintf(ptr, "_part%d", part_num+1);
+		if ( part->name )
+			ptr += sprintf(ptr, "_%s", part->name);
+	}
+
 	free(hwrevs);

 	return name;

 }

-static int image_append(struct image * image, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout) {
+static int image_append(struct image * image, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout, struct image_part * parts) {

 	enum image_type detected_type;

 	image->hash = image_hash_from_data(image);

 	image->devices = calloc(1, sizeof(struct device_list));
-	if ( ! image->devices )
-		return -1;
+	if ( ! image->devices ) {
+		image_free(image);
+		ALLOC_ERROR_RETURN(-1);
+	}

 	image->devices->device = DEVICE_ANY;

@@ -207,29 +227,42 @@ static int image_append(struct image * image, const char * type, const char * de
 	else
 		image->layout = NULL;

+	image->parts = parts;
+
 	return 0;

 }

 static void image_align(struct image * image) {

-	size_t align = 0;
+	struct image_fd * image_fd = image->fds;
+	size_t align, aligned_size;
+	uint32_t total_size;

 	if ( image->type == IMAGE_MMC )
 		align = 8;
 	else
 		align = 7;

-	if ( align == 0 )
+	if ( image_fd && ! image_fd->next && image->size == image_fd->size && ( image->size & ( ( 1ULL << align ) - 1 ) ) == 0 )
 		return;

-	if ( ( image->size & ( ( 1ULL << align ) - 1 ) ) == 0 )
-		return;
+	total_size = 0;

-	align = ((image->size >> align) + 1) << align;
+	while ( image_fd ) {

-	image->align = align - image->size;
-	image->size = align;
+		if ( ( image_fd->size & ( ( 1ULL << align ) - 1 ) ) != 0 ) {
+			aligned_size = ((image_fd->size >> align) + 1) << align;
+			image_fd->align = aligned_size - image_fd->size;
+			image_fd->size = aligned_size;
+		}
+
+		total_size += image_fd->size;
+		image_fd = image_fd->next;
+
+	}
+
+	image->size = total_size;

 	image->hash = image_hash_from_data(image);

@@ -244,31 +277,100 @@ static struct image * image_alloc(void) {

 }

-struct image * image_alloc_from_file(const char * file, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout) {
+struct image * image_alloc_from_file(const char * file, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout, struct image_part * parts) {

+	return image_alloc_from_files(&file, 1, type, device, hwrevs, version, layout, parts);
+
+}
+
+struct image * image_alloc_from_files(const char ** files, int count, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout, struct image_part * parts) {
+
+	struct image * image;
+	int i;
+	int * fds = calloc(count, sizeof(int));
+	if ( ! fds )
+		ALLOC_ERROR_RETURN(NULL);
+
+	for ( i = 0; i < count; ++i ) {
+		fds[i] = open(files[i], O_RDONLY);
+		if ( fds[i] < 0 ) {
+			ERROR_INFO("Cannot open image file %s", files[i]);
+			free(fds);
+			return NULL;
+		}
+	}
+
+	image = image_alloc_from_fds(fds, files, count, type, device, hwrevs, version, layout, parts);
+	free(fds);
+	return image;
+
+}
+
+struct image * image_alloc_from_fd(int fd, const char * orig_filename, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout, struct image_part * parts) {
+
+	return image_alloc_from_fds(&fd, &orig_filename, 1, type, device, hwrevs, version, layout, parts);
+
+}
+
+struct image * image_alloc_from_fds(int * fds, const char ** orig_filenames, int count, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout, struct image_part * parts) {
+
+	int i;
 	struct image * image = image_alloc();
-	if ( ! image )
-		return NULL;
-
-	image->is_shared_fd = 0;
-	image->fd = open(file, O_RDONLY);
-	if ( image->fd < 0 ) {
-		ERROR_INFO("Cannot open image file %s", file);
-		free(image);
+	if ( ! image ) {
+		for ( i = 0; i < count; ++i )
+			close(fds[i]);
 		return NULL;
 	}

-	image->size = lseek(image->fd, 0, SEEK_END);
-	image->offset = 0;
-	image->cur = 0;
-	image->orig_filename = strdup(file);
-	lseek(image->fd, 0, SEEK_SET);
+	for ( i = 0; i < count; ++i ) {

-	if ( image_append(image, type, device, hwrevs, version, layout) < 0 )
+		off_t offset;
+		struct image_fd * image_fds = image->fds;
+		struct image_fd * image_fd = calloc(1, sizeof(struct image_fd));
+		if ( ! image_fd ) {
+			image_free(image);
+			return NULL;
+		}
+
+		if ( ! image_fds ) {
+			image->fds = image_fd;
+		} else {
+			while ( image_fds->next )
+				image_fds = image_fds->next;
+			image_fds->next = image_fd;
+		}
+
+		image_fd->is_shared_fd = 0;
+		image_fd->fd = fds[i];
+
+		offset = lseek(image_fd->fd, 0, SEEK_END);
+		if ( offset == (off_t)-1 ) {
+			ERROR_INFO("Cannot seek to end of file %s", orig_filenames[i]);
+			image_free(image);
+			return NULL;
+		}
+
+		image_fd->size = offset;
+		image_fd->offset = 0;
+		image_fd->is_shared_fd = 0;
+		image_fd->shared_cur = 0;
+		image_fd->orig_filename = strdup(orig_filenames[i]);
+
+		image->size += image_fd->size;
+
+		if ( lseek(image_fd->fd, 0, SEEK_SET) == (off_t)-1 ) {
+			ERROR_INFO("Cannot seek to begin of file %s", orig_filenames[i]);
+			image_free(image);
+			return NULL;
+		}
+
+	}
+
+	if ( image_append(image, type, device, hwrevs, version, layout, parts) < 0 )
 		return NULL;

 	if ( ( ! type || ! type[0] ) && ( ! device || ! device[0] ) && ( ! hwrevs || ! hwrevs[0] ) && ( ! version || ! version[0] ) )
-		image_missing_values_from_name(image, file);
+		image_missing_values_from_name(image, count > 0 ? orig_filenames[0] : NULL);

 	image_align(image);

@@ -276,19 +378,25 @@ struct image * image_alloc_from_file(const char * file, const char * type, const

 }

-struct image * image_alloc_from_shared_fd(int fd, size_t size, size_t offset, uint16_t hash, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout) {
+struct image * image_alloc_from_shared_fd(int fd, size_t size, size_t offset, uint16_t hash, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout, struct image_part * parts) {

 	struct image * image = image_alloc();
-	if ( ! image )
+	struct image_fd * image_fd = calloc(1, sizeof(struct image_fd));
+	if ( ! image || ! image_fd ) {
+		free(image);
+		free(image_fd);
 		return NULL;
+	}

-	image->is_shared_fd = 1;
-	image->fd = fd;
-	image->size = size;
-	image->offset = offset;
-	image->cur = 0;
+	image_fd->is_shared_fd = 1;
+	image_fd->fd = fd;
+	image_fd->size = size;
+	image_fd->offset = offset;
+	image_fd->shared_cur = 0;
+	image->fds = image_fd;
+	image->size = image_fd->size;

-	if ( image_append(image, type, device, hwrevs, version, layout) < 0 )
+	if ( image_append(image, type, device, hwrevs, version, layout, parts) < 0 )
 		return NULL;

 	if ( ! noverify && image->hash != hash ) {
@@ -308,9 +416,13 @@ void image_free(struct image * image) {
 	if ( ! image )
 		return;

-	if ( ! image->is_shared_fd ) {
-		close(image->fd);
-		image->fd = -1;
+	while ( image->fds ) {
+		struct image_fd * next = image->fds->next;
+		if ( ! image->fds->is_shared_fd )
+			close(image->fds->fd);
+		free(image->fds->orig_filename);
+		free(image->fds);
+		image->fds = next;
 	}

 	while ( image->devices ) {
@@ -320,9 +432,15 @@ void image_free(struct image * image) {
 		image->devices = next;
 	}

+	while ( image->parts ) {
+		struct image_part * next = image->parts->next;
+		free(image->parts->name);
+		free(image->parts);
+		image->parts = next;
+	}
+
 	free(image->version);
 	free(image->layout);
-	free(image->orig_filename);

 	free(image);

@@ -330,63 +448,123 @@ void image_free(struct image * image) {

 void image_seek(struct image * image, size_t whence) {

-	if ( whence > image->size )
-		return;
+	off_t offset;
+	size_t start = 0;
+	struct image_fd * image_fd = image->fds;

-	if ( whence >= image->size - image->align ) {
-		lseek(image->fd, image->size - image->align - 1, SEEK_SET);
-		image->acur = whence - ( image->size - image->align );
-	} else {
-		lseek(image->fd, image->offset + whence, SEEK_SET);
-		image->acur = 0;
+	image->cur = whence;
+
+	if ( whence > image->size ) {
+		ERROR("Seek in image failed: Position end of the image");
+		return;
 	}

-	IMAGE_STORE_CUR(image);
+	while ( image_fd ) {
+		if ( ( whence >= start && whence < start + image_fd->size ) || ! image_fd->next )
+			break;
+		start += image_fd->size;
+		image_fd = image_fd->next;
+	}
+
+	if ( whence - start >= image_fd->size - image_fd->align ) {
+		offset = lseek(image_fd->fd, image_fd->size - image_fd->align, SEEK_SET);
+	} else {
+		offset = lseek(image_fd->fd, image_fd->offset + whence - start, SEEK_SET);
+	}
+
+	if ( offset == (off_t)-1 )
+		ERROR_INFO("Seek in file %s failed", (image_fd->orig_filename ? image_fd->orig_filename : "(unknown)"));
+	else if ( image_fd->is_shared_fd )
+		image_fd->shared_cur = offset;

 }

 size_t image_read(struct image * image, void * buf, size_t count) {

-	size_t cur;
 	ssize_t ret;
+	off_t offset;
 	size_t new_count = 0;
 	size_t ret_count = 0;
+	size_t start = 0;
+	struct image_fd * image_fd = image->fds;

-	IMAGE_RESTORE_CUR(image);
-
-	if ( ! image->is_shared_fd || image->cur < image->size - image->align ) {
-
-		if ( image->is_shared_fd && image->cur + count > image->size - image->align )
-			new_count = image->size - image->align - image->cur;
-		else
-			new_count = count;
-
-		ret = read(image->fd, buf, new_count);
-		if ( ret > 0 )
-			ret_count += ret;
-
-		IMAGE_STORE_CUR(image);
-
-		if ( ret < 0 )
-			return 0;
-
+	while ( image_fd ) {
+		if ( ( image->cur >= start && image->cur < start + image_fd->size ) || ! image_fd->next )
+			break;
+		start += image_fd->size;
+		image_fd = image_fd->next;
 	}

-	if ( ret_count == count )
-		return ret_count;
+	if ( image_fd->is_shared_fd ) {
+		if ( lseek(image_fd->fd, image_fd->shared_cur, SEEK_SET) != image_fd->shared_cur ) {
+			ERROR_INFO("Cannot restore offset of file %s", (image_fd->orig_filename ? image_fd->orig_filename : "(unknown)"));
+			return 0;
+		}
+	}

-	cur = lseek(image->fd, 0, SEEK_CUR) - image->offset;
+	while ( count > 0 ) {

-	if ( image->align && cur == image->size - image->align && image->acur < image->align ) {
+		if ( image->cur < start + image_fd->size - image_fd->align ) {

-		if ( image->acur + count - ret_count > image->align )
-			new_count = image->align - image->acur;
-		else
-			new_count = count - ret_count;
+			if ( image->cur + count > start + image_fd->size - image_fd->align )
+				new_count = start + image_fd->size - image_fd->align - image->cur;
+			else
+				new_count = count;

-		memset((unsigned char *)buf+ret_count, 0xFF, new_count);
-		ret_count += new_count;
-		image->acur += new_count;
+			ret = read(image_fd->fd, buf, new_count);
+			if ( ret <= 0 )
+				break;
+
+			count -= ret;
+			buf = (unsigned char *)buf + ret;
+			ret_count += ret;
+			image->cur += ret;
+
+			if ( image_fd->is_shared_fd )
+				image_fd->shared_cur += ret;
+
+			if ( (size_t)ret != new_count )
+				continue;
+
+		}
+
+		if ( image_fd->align > 0 && count > 0 && image->cur < start + image_fd->size ) {
+
+			if ( count > image_fd->align )
+				new_count = image_fd->align;
+			else
+				new_count = count;
+
+			if ( image->cur + new_count > start + image_fd->size )
+				new_count = start + image_fd->size - image->cur;
+
+			memset(buf, 0xFF, new_count);
+			ret = new_count;
+
+			count -= ret;
+			buf = (unsigned char *)buf + ret;
+			ret_count += ret;
+			image->cur += ret;
+
+		}
+
+		if ( image->cur == start + image_fd->size ) {
+
+			start += image_fd->size;
+			image_fd = image_fd->next;
+
+			if ( ! image_fd )
+				break;
+
+			offset = lseek(image_fd->fd, image_fd->offset, SEEK_SET);
+			if ( offset == (off_t)-1 ) {
+				ERROR_INFO("Seek in file %s failed", (image_fd->orig_filename ? image_fd->orig_filename : "(unknown)"));
+				break;
+			} else if ( image_fd->is_shared_fd ) {
+				image_fd->shared_cur = offset;
+			}
+
+		}

 	}

@@ -477,46 +655,50 @@ static const char * image_types[] = {
 	[IMAGE_CMT_2ND] = "cmt-2nd",
 	[IMAGE_CMT_ALGO] = "cmt-algo",
 	[IMAGE_CMT_MCUSW] = "cmt-mcusw",
+	[IMAGE_1ST] = "1st",
+	[IMAGE_CERT_SW] = "cert-sw",
+	[IMAGE_APE_ALGO] = "ape-algo",
 };

 enum image_type image_type_from_data(struct image * image) {

 	unsigned char buf[512];
+	size_t size;

 	memset(buf, 0, sizeof(buf));
 	image_seek(image, 0);
-	image_read(image, buf, sizeof(buf));
+	size = image_read(image, buf, sizeof(buf));

-	if ( memcmp(buf+52, "2NDAPE", 6) == 0 )
+	if ( size >= 58 && memcmp(buf+52, "2NDAPE", 6) == 0 )
 		return IMAGE_2ND;
-	else if ( memcmp(buf+20, "2ND", 3) == 0 )
+	else if ( size >= 23 && memcmp(buf+20, "2ND", 3) == 0 )
 		return IMAGE_2ND;
-	else if ( memcmp(buf+4, "NOLOScnd", 8) == 0 )
+	else if ( size >= 8 && memcmp(buf+4, "NOLOScnd", 8) == 0 )
 		return IMAGE_SECONDARY;
-	else if ( memcmp(buf+20, "X-LOADER", 8) == 0 )
+	else if ( size >= 28 && memcmp(buf+20, "X-LOADER", 8) == 0 )
 		return IMAGE_XLOADER;
-	else if ( memcmp(buf+12, "NOLOXldr", 8) == 0 )
+	else if ( size >= 20 && memcmp(buf+12, "NOLOXldr", 8) == 0 )
 		return IMAGE_XLOADER;
-	else if ( memcmp(buf+4, "NOLOXldr", 8) == 0 )
+	else if ( size >= 12 && memcmp(buf+4, "NOLOXldr", 8) == 0 )
 		return IMAGE_2ND;
-	else if ( memcmp(buf+36, "\x18\x28\x6f\x01", 4) == 0 ) /* ARM Linux kernel magic number */
+	else if ( size >= 40 && memcmp(buf+36, "\x18\x28\x6f\x01", 4) == 0 ) /* ARM Linux kernel magic number */
 		return IMAGE_KERNEL;
-	else if ( memcmp(buf, "\x14\x00\x00\xea", 4) == 0 ) /* ARM U-Boot - instruction branch +0x50 */
+	else if ( size >= 4 && memcmp(buf+1, "\x00\x00\xea", 3) == 0 ) /* ARM U-Boot - instruction branch */
 		return IMAGE_KERNEL;
-	else if ( memcmp(buf, "UBI#", 4) == 0 ) /* UBI EC header */
+	else if ( size >= 4 && memcmp(buf, "UBI#", 4) == 0 ) /* UBI EC header */
 		return IMAGE_ROOTFS;
-	else if ( memcmp(buf+510, "\x55\xaa", 2) == 0 ) /* FAT boot sector signature */
+	else if ( size >= 512 && memcmp(buf+510, "\x55\xaa", 2) == 0 ) /* FAT boot sector signature */
 		return IMAGE_MMC;
-	else if ( memcmp(buf, "\xb0\x00\x01\x03\x9d\x00\x00\x00", 8) == 0 )
+	else if ( size >= 8 && memcmp(buf, "\xb0\x00\x01\x03\x9d\x00\x00\x00", 8) == 0 )
 		return IMAGE_CMT_2ND;
-	else if ( memcmp(buf, "\xb1\x00\x00\x00\x82\x00\x00\x00", 8) == 0 )
+	else if ( size >= 8 && memcmp(buf, "\xb1\x00\x00\x00\x82\x00\x00\x00", 8) == 0 )
 		return IMAGE_CMT_ALGO;
-	else if ( memcmp(buf, "\xb2\x00\x00\x01\x44\x00\x00\x00", 8) == 0 )
+	else if ( size >= 8 && memcmp(buf, "\xb2\x00\x00\x01\x44\x00\x00\x00", 8) == 0 )
 		return IMAGE_CMT_MCUSW;
-	else if ( memcmp(buf, "\x45\x3d\xcd\x28", 4) == 0 ) /* CRAMFS MAGIC */
+	else if ( size >= 4 && memcmp(buf, "\x45\x3d\xcd\x28", 4) == 0 ) /* CRAMFS MAGIC */
 		return IMAGE_INITFS;
-	else if ( memcmp(buf, "\x85\x19", 2) == 0 ) { /* JFFS2 MAGIC */
-		if ( image->size < 0x300000 )
+	else if ( size >= 2 && memcmp(buf, "\x85\x19", 2) == 0 ) { /* JFFS2 MAGIC */
+		if ( image->size < 0x1000000 )
 			return IMAGE_INITFS;
 		else
 			return IMAGE_ROOTFS;
@@ -566,11 +748,18 @@ int image_hwrev_is_valid(struct image * image, int16_t hwrev) {

 void image_print_info(struct image * image) {

+	int i = 0;
 	const char * str;
+	struct image_fd * image_fd = image->fds;
+	struct image_part * part = image->parts;
 	struct device_list * device = image->devices;

-	if ( image->orig_filename )
-		printf("File: %s\n", image->orig_filename);
+	if ( image->fds && image->fds->orig_filename ) {
+		if ( image->fds->next )
+			printf("Multifile image:\n");
+		else
+			printf("File: %s\n", image->fds->orig_filename);
+	}

 	str = image_type_to_string(image->type);
 	printf("    Image type: %s\n", str ? str : "unknown");
@@ -582,6 +771,20 @@ void image_print_info(struct image * image) {
 	if ( image->layout )
 		printf("    Image layout: included\n");

+	while ( part ) {
+
+		if ( image->fds && image->fds->orig_filename && image->fds->next )
+			printf("File: %s\n", (image_fd && image_fd->orig_filename) ? image_fd->orig_filename : "(unknown)");
+
+		printf("    Image part %d name: %s\n", i, part->name ? part->name : "(empty)");
+		printf("    Image part %d size: %d bytes\n", i, part->size);
+
+		part = part->next;
+		image_fd = image_fd ? image_fd->next : NULL;
+		++i;
+
+	}
+
 	while ( device ) {

 		char * str2 = NULL;
@@ -37,9 +37,30 @@ enum image_type {
 	IMAGE_CMT_2ND,
 	IMAGE_CMT_ALGO,
 	IMAGE_CMT_MCUSW,
+	IMAGE_1ST,
+	IMAGE_CERT_SW,
+	IMAGE_APE_ALGO,
 	IMAGE_COUNT,
 };

+struct image_part {
+	struct image_part * next;
+	uint32_t offset;
+	uint32_t size;
+	char * name;
+};
+
+struct image_fd {
+	struct image_fd * next;
+	int fd;
+	int is_shared_fd;
+	off_t shared_cur;
+	uint32_t size;
+	uint32_t align;
+	size_t offset;
+	char * orig_filename;
+};
+
 struct image {
 	enum image_type type;
 	struct device_list * devices;
@@ -47,14 +68,9 @@ struct image {
 	char * layout;
 	uint16_t hash;
 	uint32_t size;
-
-	int fd;
-	int is_shared_fd;
-	uint32_t align;
-	size_t offset;
+	struct image_part * parts;
+	struct image_fd * fds;
 	size_t cur;
-	size_t acur;
-	char * orig_filename;
 };

 struct image_list {
@@ -63,8 +79,11 @@ struct image_list {
 	struct image_list * next;
 };

-struct image * image_alloc_from_file(const char * file, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout);
-struct image * image_alloc_from_shared_fd(int fd, size_t size, size_t offset, uint16_t hash, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout);
+struct image * image_alloc_from_file(const char * file, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout, struct image_part * parts);
+struct image * image_alloc_from_files(const char ** files, int count, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout, struct image_part * parts);
+struct image * image_alloc_from_fd(int fd, const char * orig_filename, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout, struct image_part * parts);
+struct image * image_alloc_from_fds(int * fds, const char ** orig_filenames, int count, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout, struct image_part * parts);
+struct image * image_alloc_from_shared_fd(int fd, size_t size, size_t offset, uint16_t hash, const char * type, const char * device, const char * hwrevs, const char * version, const char * layout, struct image_part * parts);
 void image_free(struct image * image);
 void image_seek(struct image * image, size_t whence);
 size_t image_read(struct image * image, void * buf, size_t count);
@@ -75,7 +94,7 @@ void image_list_unlink(struct image_list * list);

 uint16_t image_hash_from_data(struct image * image);
 enum image_type image_type_from_data(struct image * image);
-char * image_name_alloc_from_values(struct image * image);
+char * image_name_alloc_from_values(struct image * image, int part_num);
 enum image_type image_type_from_string(const char * type);
 const char * image_type_to_string(enum image_type type);
 int image_hwrev_is_valid(struct image * image, int16_t hwrev);
@@ -21,10 +21,20 @@
 /* usage: sudo USBSNIFF_WAIT=1 LD_PRELOAD=./libusb-sniff.so flasher-3.5 ... */
 /* usage: sudo USBSNIFF_SKIP_READ=1 USBSNIFF_SKIP_WRITE=1 LD_PRELOAD=./libusb-sniff.so flasher-3.5 ... */

+/* Enable RTLD_NEXT for glibc */
+#ifndef _GNU_SOURCE
 #define _GNU_SOURCE
+#endif
+
 #include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
 #include <dlfcn.h>
-#include <usb.h>
+
+struct usb_dev_handle;
+struct libusb_device_handle;
+typedef struct usb_dev_handle usb_dev_handle;
+typedef struct libusb_device_handle libusb_device_handle;

 static char to_ascii(char c) {

@@ -68,7 +78,7 @@ int usb_bulk_write(usb_dev_handle * dev, int ep, const char * bytes, int size, i
 	static int (*real_usb_bulk_write)(usb_dev_handle * dev, int ep, const char * bytes, int size, int timeout) = NULL;

 	if ( ! real_usb_bulk_write )
-		real_usb_bulk_write = dlsym(RTLD_NEXT, "usb_bulk_write");
+		*(void **)(&real_usb_bulk_write) = dlsym(RTLD_NEXT, "usb_bulk_write");

 	if ( ! getenv("USBSNIFF_SKIP_WRITE") ) {

@@ -92,7 +102,7 @@ int usb_bulk_read(usb_dev_handle * dev, int ep, char * bytes, int size, int time
 	int ret;

 	if ( ! real_usb_bulk_read )
-		real_usb_bulk_read = dlsym(RTLD_NEXT, "usb_bulk_read");
+		*(void **)(&real_usb_bulk_read) = dlsym(RTLD_NEXT, "usb_bulk_read");

 	ret = real_usb_bulk_read(dev, ep, bytes, size, timeout);

@@ -114,13 +124,61 @@ int usb_bulk_read(usb_dev_handle * dev, int ep, char * bytes, int size, int time

 }

+int libusb_bulk_transfer(libusb_device_handle *dev, unsigned char ep, unsigned char *bytes, int size, int *actual_length, unsigned int timeout) {
+
+	static int (*real_libusb_bulk_transfer)(libusb_device_handle *dev, unsigned char ep, unsigned char *bytes, int size, int *actual_length, unsigned int timeout) = NULL;
+	int ret;
+
+	if ( ! real_libusb_bulk_transfer )
+		*(void **)(&real_libusb_bulk_transfer) = dlsym(RTLD_NEXT, "libusb_bulk_transfer");
+
+	if ( ep == 0x81 ) {
+
+		ret = real_libusb_bulk_transfer(dev, ep, bytes, size, actual_length, timeout);
+
+		if ( ! getenv("USBSNIFF_SKIP_READ") ) {
+
+			printf("\n==== usb_bulk_read (ep=%d size=%d timeout=%d) ret = %d ====\n", ep, size, timeout, (ret < 0) ? ret : *actual_length);
+			if ( ret == 0 ) {
+				dump_bytes((char*) bytes, *actual_length);
+				printf("====\n");
+			}
+
+			if ( getenv("USBSNIFF_WAIT") ) {
+				printf("Press ENTER"); fflush(stdout); getchar();
+			}
+
+		}
+
+		return ret;
+
+	} else {
+
+		if ( ! getenv("USBSNIFF_SKIP_WRITE") ) {
+
+			printf("\n==== usb_bulk_write (ep=%d size=%d timeout=%d) ====\n", ep, size, timeout);
+			dump_bytes((char*) bytes, size);
+			printf("====\n");
+
+			if ( getenv("USBSNIFF_WAIT") ) {
+				printf("Press ENTER"); fflush(stdout); getchar();
+			}
+
+		}
+
+		return real_libusb_bulk_transfer(dev, ep, bytes, size, actual_length, timeout);
+
+	}
+
+}
+
 int usb_control_msg(usb_dev_handle *dev, int requesttype, int request, int value, int index, char *bytes, int size, int timeout) {

 	static int (*real_usb_control_msg)(usb_dev_handle *dev, int requesttype, int request, int value, int index, char *bytes, int size, int timeout) = NULL;
 	int ret;

 	if ( ! real_usb_control_msg )
-		real_usb_control_msg = dlsym(RTLD_NEXT, "usb_control_msg");
+		*(void **)(&real_usb_control_msg) = dlsym(RTLD_NEXT, "usb_control_msg");

 	if ( requesttype == 64 && ! getenv("USBSNIFF_SKIP_CONTROL") ) {

@@ -154,12 +212,65 @@ int usb_control_msg(usb_dev_handle *dev, int requesttype, int request, int value

 }

+int libusb_control_transfer(libusb_device_handle *dev, uint8_t requesttype, uint8_t request, uint16_t value, uint16_t index, unsigned char *bytes, uint16_t size, unsigned int timeout) {
+
+	static int (*real_libusb_control_transfer)(libusb_device_handle *dev, uint8_t requesttype, uint8_t request, uint16_t value, uint16_t index, unsigned char *bytes, uint16_t size, unsigned int timeout) = NULL;
+	int ret;
+
+	if ( ! real_libusb_control_transfer )
+		*(void **)(&real_libusb_control_transfer) = dlsym(RTLD_NEXT, "libusb_control_transfer");
+
+	if ( requesttype == 64 && ! getenv("USBSNIFF_SKIP_CONTROL") ) {
+
+		printf("\n==== usb_control_msg(requesttype=%d, request=%d, value=%d, index=%d, size=%d, timeout=%d) ====\n", (int)requesttype, (int)request, (int)value, (int)index, (int)size, (int)timeout);
+		dump_bytes((char*) bytes, size);
+		printf("====\n");
+
+		if ( getenv("USBSNIFF_WAIT") ) {
+			printf("Press ENTER"); fflush(stdout); getchar();
+		}
+
+	}
+
+	ret = real_libusb_control_transfer(dev, requesttype, request, value, index, bytes, size, timeout);
+
+	if ( requesttype != 64 && ! getenv("USBSNIFF_SKIP_CONTROL") ) {
+
+		printf("\n==== usb_control_msg(requesttype=%d, request=%d, value=%d, index=%d, size=%d, timeout=%d) ret = %d ====\n", (int)requesttype, (int)request, (int)value, (int)index, (int)size, (int)timeout, ret);
+		if ( ret > 0 ) {
+			dump_bytes((char*) bytes, ret);
+			printf("====\n");
+		}
+
+		if ( getenv("USBSNIFF_WAIT") ) {
+			printf("Press ENTER"); fflush(stdout); getchar();
+		}
+
+	}
+
+	return ret;
+
+}
+
 int usb_set_configuration(usb_dev_handle *dev, int configuration) {

 	static int (*real_usb_set_configuration)(usb_dev_handle *dev, int configuration) = NULL;

 	if ( ! real_usb_set_configuration )
-		real_usb_set_configuration = dlsym(RTLD_NEXT, "usb_set_configuration");
+		*(void **)(&real_usb_set_configuration) = dlsym(RTLD_NEXT, "usb_set_configuration");
+
+	printf("\n==== usb_set_configuration (configuration=%d) ====\n", configuration);
+
+	return real_usb_set_configuration(dev, configuration);
+
+}
+
+int libusb_set_configuration(libusb_device_handle *dev, int configuration) {
+
+	static int (*real_usb_set_configuration)(libusb_device_handle *dev, int configuration) = NULL;
+
+	if ( ! real_usb_set_configuration )
+		*(void **)(&real_usb_set_configuration) = dlsym(RTLD_NEXT, "libusb_set_configuration");

 	printf("\n==== usb_set_configuration (configuration=%d) ====\n", configuration);

@@ -172,7 +283,20 @@ int usb_claim_interface(usb_dev_handle *dev, int interface) {
 	static int (*real_usb_claim_interface)(usb_dev_handle *dev, int interface) = NULL;

 	if ( ! real_usb_claim_interface )
-		real_usb_claim_interface = dlsym(RTLD_NEXT, "usb_claim_interface");
+		*(void **)(&real_usb_claim_interface) = dlsym(RTLD_NEXT, "usb_claim_interface");
+
+	printf("\n==== usb_claim_interface (interface=%d) ====\n", interface);
+
+	return real_usb_claim_interface(dev, interface);
+
+}
+
+int libusb_claim_interface(libusb_device_handle *dev, int interface) {
+
+	static int (*real_usb_claim_interface)(libusb_device_handle *dev, int interface) = NULL;
+
+	if ( ! real_usb_claim_interface )
+		*(void **)(&real_usb_claim_interface) = dlsym(RTLD_NEXT, "libusb_claim_interface");

 	printf("\n==== usb_claim_interface (interface=%d) ====\n", interface);

@@ -185,10 +309,23 @@ int usb_set_altinterface(usb_dev_handle *dev, int alternate) {
 	static int (*real_usb_set_altinterface)(usb_dev_handle *dev, int alternate) = NULL;

 	if ( ! real_usb_set_altinterface )
-		real_usb_set_altinterface = dlsym(RTLD_NEXT, "usb_set_altinterface");
+		*(void **)(&real_usb_set_altinterface) = dlsym(RTLD_NEXT, "usb_set_altinterface");

 	printf("\n==== usb_set_altinterface (alternate=%d) ====\n", alternate);

 	return real_usb_set_altinterface(dev, alternate);

 }
+
+int libusb_set_interface_alt_setting(libusb_device_handle *dev, int interface, int alternate) {
+
+	static int (*real_usb_set_altinterface)(libusb_device_handle *dev, int interface, int alternate) = NULL;
+
+	if ( ! real_usb_set_altinterface )
+		*(void **)(&real_usb_set_altinterface) = dlsym(RTLD_NEXT, "libusb_set_interface_alt_setting");
+
+	printf("\n==== usb_set_altinterface (alternate=%d) ====\n", alternate);
+
+	return real_usb_set_altinterface(dev, interface, alternate);
+
+}
@@ -19,6 +19,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <signal.h>

 #include <sys/statvfs.h>
 #include <sys/types.h>
@@ -53,7 +54,9 @@ static int root_device = -1;
 #define min(a, b) (a < b ? a : b)
 #define local_cal_copy(dest, from, len) strncpy(dest, from, min(len, sizeof(dest)-1))
 #define local_cal_read(cal, str, ptr, len) ( cal_read_block(cal, str, &ptr, &len, 0) == 0 && ptr )
-#define local_cal_readcopy(cal, str, dest) do { void * ptr; unsigned long int len; if ( local_cal_read(cal, str, ptr, len) ) local_cal_copy(dest, ptr, len); } while ( 0 )
+#define local_cal_readcopy(cal, str, dest) do { void * ptr; unsigned long int len; if ( local_cal_read(cal, str, ptr, len) ) { local_cal_copy(dest, ptr, len); free(ptr); } } while ( 0 )
+
+#if defined(__linux__) && defined(__arm__)

 static void local_cal_parse(void) {

@@ -109,16 +112,22 @@ static void local_cal_parse(void) {

 }

+#endif
+
 int local_init(void) {

+#if defined(__linux__) && defined(__arm__)
 	char buf[1024];
 	char * ptr;
 	char * ptr2;
 	FILE * file;
+#endif

 	if ( failed )
 		return -1;

+#if defined(__linux__) && defined(__arm__)
+
 	file = fopen("/proc/cpuinfo", "r");
 	if ( ! file ) {
 		failed = 1;
@@ -127,9 +136,9 @@ int local_init(void) {

 	while ( fgets(buf, sizeof(buf), file) ) {

-		if ( strncmp(buf, "Hardware", strlen("Hardware")) == 0 ) {
+		if ( strncmp(buf, "Hardware", sizeof("Hardware")-1) == 0 ) {

-			ptr = buf + strlen("Hardware");
+			ptr = buf + sizeof("Hardware")-1;

 			while ( ptr < buf + sizeof(buf) && *ptr > 0 && *ptr <= 32 )
 				++ptr;
@@ -174,9 +183,12 @@ int local_init(void) {

 	}

+	fclose(file);
+
+#endif
+
 	failed = 1;
 	printf("Not a local device\n");
-	fclose(file);
 	return -1;

 }
@@ -187,14 +199,6 @@ enum device local_get_device(void) {

 }

-int local_flash_image(struct image * image) {
-
-	ERROR("Not implemented yet");
-	(void)image;
-	return -1;
-
-}
-
 static int local_nanddump(const char * file, int mtd, int offset, int length) {

 	struct statvfs buf;
@@ -212,19 +216,22 @@ static int local_nanddump(const char * file, int mtd, int offset, int length) {
 	free(path);

 	if ( ret == 0 && buf.f_bsize * buf.f_bfree < (long unsigned int)length ) {
-		ERROR("Not enough free space (have: %lu, need: %d)", buf.f_bsize * buf.f_bfree, length);
+		ERROR("Not enough free space (have: %ju, need: %d)", (intmax_t)(buf.f_bsize * buf.f_bfree), length);
 		return 1;
 	}

-	size = snprintf(NULL, 0, "nanddump -i -o -b -s %d -l %d -f %s /dev/mtd%dro", offset, length, file, mtd);
+	size = snprintf(NULL, 0, "nanddump --omitoob -s %d -l %d -f %s /dev/mtd%d", offset, length, file, mtd);

 	command = malloc(size+1);
 	if ( ! command )
 		return 1;

-	snprintf(command, size+1, "nanddump -i -o -b -s %d -l %d -f %s /dev/mtd%dro", offset, length, file, mtd);
+	snprintf(command, size+1, "nanddump --omitoob -s %d -l %d -f %s /dev/mtd%d", offset, length, file, mtd);

-	ret = system(command);
+	if ( ! simulate )
+		ret = system(command);
+	else
+		ret = 0;

 	free(command);

@@ -232,72 +239,177 @@ static int local_nanddump(const char * file, int mtd, int offset, int length) {

 }

-struct nanddump_args {
-	int valid;
-	int mtd;
-	int offset;
-	int length;
+static FILE * local_nandwrite(int mtd, int offset) {
+
+	char * command;
+	FILE * stream;
+	size_t size;
+
+	size = snprintf(NULL, 0, "nandwrite -a -s %d -p /dev/mtd%d -", offset, mtd);
+
+	command = malloc(size+1);
+	if ( ! command )
+		return NULL;
+
+	snprintf(command, size+1, "nandwrite -a -s %d -p /dev/mtd%d -", offset, mtd);
+
+	if ( ! simulate )
+		stream = popen(command, "w");
+	else
+		stream = NULL;
+
+	free(command);
+
+	return stream;
+
+}
+
+struct nandpart_args {
+	unsigned int mtd;
+	unsigned int offset;
+	unsigned int length;
+	unsigned int header;
 };

-static struct nanddump_args nanddump_rx51[] = {
-	[IMAGE_XLOADER]   = { 1, 0, 0x00000000, 0x00004000 },
-	[IMAGE_SECONDARY] = { 1, 0, 0x00004000, 0x0001C000 },
-	[IMAGE_KERNEL]    = { 1, 3, 0x00000800, 0x001FF800 },
-	[IMAGE_INITFS]    = { 1, 4, 0x00000000, 0x00200000 },
-	[IMAGE_ROOTFS]    = { 1, 5, 0x00000000, 0x0fb40000 },
+static struct nandpart_args nandpart_rx51[] = {
+	[IMAGE_XLOADER]   = { 0, 0x00000000, 0x00004000, 0x00000000 },
+	[IMAGE_SECONDARY] = { 0, 0x00004000, 0x0001C000, 0x00000000 },
+	[IMAGE_KERNEL]    = { 3, 0x00000000, 0x00200000, 0x00000800 },
+	[IMAGE_INITFS]    = { 4, 0x00000000, 0x00200000, 0x00000000 },
+	[IMAGE_ROOTFS]    = { 5, 0x00000000, 0x0fb40000, 0x00000000 },
 };

 /* FIXME: Is this table correct? */
-static struct nanddump_args nanddump_rx4x[] = {
-	[IMAGE_XLOADER]   = { 1, 0, 0x00000200, 0x00003E00 },
-	[IMAGE_SECONDARY] = { 1, 0, 0x00004000, 0x0001C000 },
-	[IMAGE_KERNEL]    = { 1, 2, 0x00000800, 0x0021F800 },
-	[IMAGE_INITFS]    = { 1, 3, 0x00000000, 0x00400000 },
-	[IMAGE_ROOTFS]    = { 1, 4, 0x00000000, 0x0f960000 },
+static struct nandpart_args nandpart_rx4x[] = {
+	[IMAGE_XLOADER]   = { 0, 0x00000200, 0x00003E00, 0x00000000 },
+	[IMAGE_SECONDARY] = { 0, 0x00004000, 0x0001C000, 0x00000000 },
+	[IMAGE_KERNEL]    = { 2, 0x00000000, 0x00220000, 0x00000800 },
+	[IMAGE_INITFS]    = { 3, 0x00000000, 0x00400000, 0x00000000 },
+	[IMAGE_ROOTFS]    = { 4, 0x00000000, 0x0f960000, 0x00000000 },
 };

 /* FIXME: Is this table correct? */
-static struct nanddump_args nanddump_old[] = {
-	[IMAGE_XLOADER]   = { 1, 0, 0x00000200, 0x00003E00 },
-	[IMAGE_SECONDARY] = { 1, 0, 0x00004000, 0x0001C000 },
-	[IMAGE_KERNEL]    = { 1, 2, 0x00000800, 0x001FF800 },
-	[IMAGE_INITFS]    = { 1, 3, 0x00000000, 0x00200000 },
-	[IMAGE_ROOTFS]    = { 1, 4, 0x00000000, 0x0fb80000 },
+static struct nandpart_args nandpart_old[] = {
+	[IMAGE_XLOADER]   = { 0, 0x00000200, 0x00003E00, 0x00000000 },
+	[IMAGE_SECONDARY] = { 0, 0x00004000, 0x0001C000, 0x00000000 },
+	[IMAGE_KERNEL]    = { 2, 0x00000000, 0x00200000, 0x00000800 },
+	[IMAGE_INITFS]    = { 3, 0x00000000, 0x00200000, 0x00000000 },
+	[IMAGE_ROOTFS]    = { 4, 0x00000000, 0x0fb80000, 0x00000000 },
 };

-struct nanddump_device {
+struct nand_device {
 	size_t count;
-	struct nanddump_args * args;
+	struct nandpart_args * args;
 };

-#define NANDDUMP(device, array) [device] = { .count = sizeof(array)/sizeof(array[0]), .args = array }
+#define NAND_DEVICE(device, array) [device] = { .count = sizeof(array)/sizeof(array[0]), .args = array }

-static struct nanddump_device nanddump[] = {
-	NANDDUMP(DEVICE_SU_18, nanddump_old),
-	NANDDUMP(DEVICE_RX_34, nanddump_old),
-	NANDDUMP(DEVICE_RX_44, nanddump_rx4x),
-	NANDDUMP(DEVICE_RX_48, nanddump_rx4x),
-	NANDDUMP(DEVICE_RX_51, nanddump_rx51),
+static struct nand_device nand_device[] = {
+	NAND_DEVICE(DEVICE_SU_18, nandpart_old),
+	NAND_DEVICE(DEVICE_RX_34, nandpart_old),
+	NAND_DEVICE(DEVICE_RX_44, nandpart_rx4x),
+	NAND_DEVICE(DEVICE_RX_48, nandpart_rx4x),
+	NAND_DEVICE(DEVICE_RX_51, nandpart_rx51),
 };

 #undef NANDDUMP

-int local_dump_image(enum image_type image, const char * file) {
+static void local_find_internal_mydocs(int * maj, int * min) {

-	int ret = -1;
-	int fd = -1;
-	unsigned char * addr = NULL;
-	off_t nlen, len;
-	int align;
+#ifdef __linux__
+
+	int fd;
 	DIR * dir;
 	DIR * dir2;
 	FILE * f;
 	struct dirent * dirent;
 	struct dirent * dirent2;
-	struct stat st;
-	int maj, min;
 	char buf[1024];
-	char blk[1024];
+
+	/* Find min & maj id for block device MyDocs (mmc device, partition 1) */
+
+	dir = opendir("/sys/class/mmc_host/");
+	if ( ! dir ) {
+		ERROR("Cannot find MyDocs mmc device: Opening '/sys/class/mmc_host/' failed");
+		return;
+	}
+
+	while ( ( dirent = readdir(dir) ) ) {
+
+		if ( strncmp(dirent->d_name, ".", sizeof(".")) == 0 || strncmp(dirent->d_name, "..", sizeof("..")) == 0 )
+			continue;
+
+		if ( snprintf(buf, sizeof(buf), "/sys/class/mmc_host/%s/slot_name", dirent->d_name) <= 0 )
+			continue;
+
+		fd = open(buf, O_RDONLY);
+		if ( fd < 0 )
+			continue;
+
+		memset(buf, 0, sizeof(buf));
+		if ( read(fd, buf, sizeof(buf)-1) < 0 )
+			buf[0] = 0;
+		close(fd);
+		fd = -1;
+
+		if ( strncmp(buf, "internal", sizeof("internal")-1) != 0 )
+			continue;
+
+		if ( snprintf(buf, sizeof(buf), "/sys/class/mmc_host/%s/%s:0001/", dirent->d_name, dirent->d_name) <= 0 )
+			continue;
+
+		dir2 = opendir(buf);
+		if ( ! dir2 )
+			continue;
+
+		while ( ( dirent2 = readdir(dir2) ) ) {
+
+			if ( strncmp(dirent2->d_name, "block:mmcblk", sizeof("block:mmcblk")-1) != 0 )
+				continue;
+
+			if ( snprintf(buf, sizeof(buf), "/sys/class/mmc_host/%s/%s:0001/%s/dev", dirent->d_name, dirent->d_name, dirent2->d_name) <= 0 )
+				continue;
+
+			f = fopen(buf, "r");
+			if ( ! f )
+				continue;
+
+			if ( fscanf(f, "%d:%d", maj, min) != 2 ) {
+				*maj = -1;
+				*min = -1;
+				fclose(f);
+				continue;
+			}
+
+			fclose(f);
+			break;
+
+		}
+
+		closedir(dir2);
+
+		if ( *maj != -1 && *min != -1 )
+			break;
+
+	}
+
+	closedir(dir);
+
+#endif
+
+}
+
+int local_dump_image(enum image_type image, const char * file) {
+
+	unsigned char buf[20];
+	int ret = -1;
+	int fd = -1;
+	int header = 0;
+	unsigned char * addr = NULL;
+	off_t nlen = (off_t)-1;
+	off_t len;
+	int align;
+	int maj, min;

 	printf("Dump %s image to file %s...\n", image_type_to_string(image), file);

@@ -306,129 +418,70 @@ int local_dump_image(enum image_type image, const char * file) {
 		maj = -1;
 		min = -1;

-		/* Find block device in /dev/ for MyDocs (mmc device, partition 1) */
-
-		dir = opendir("/sys/class/mmc_host/");
-		if ( ! dir ) {
-			ERROR("Cannot find MyDocs mmc device: Opening '/sys/class/mmc_host/' failed");
-			goto clean;
-		}
-
-		while ( ( dirent = readdir(dir) ) ) {
-
-			snprintf(buf, sizeof(buf), "/sys/class/mmc_host/%s/slot_name", dirent->d_name);
-
-			fd = open(buf, O_RDONLY);
-			if ( fd < 0 )
-				continue;
-
-			buf[0] = 0;
-			if ( read(fd, buf, sizeof(buf)) < 0 )
-				buf[0] = 0;
-			close(fd);
-
-			if ( strncmp(buf, "internal", strlen("internal")) != 0 )
-				continue;
-
-			snprintf(buf, sizeof(buf), "/sys/class/mmc_host/%s/%s:0001/", dirent->d_name, dirent->d_name);
-
-			dir2 = opendir(buf);
-			if ( ! dir2 )
-				continue;
-
-			while ( ( dirent2 = readdir(dir2) ) ) {
-
-				if ( strncmp(dirent2->d_name, "block:mmcblk", strlen("block:mmcblk")) != 0 )
-					continue;
-
-				snprintf(buf, sizeof(buf), "/sys/class/mmc_host/%s/%s:0001/%s/dev", dirent->d_name, dirent->d_name, dirent2->d_name);
-
-				f = fopen(buf, "r");
-				if ( ! f )
-					continue;
-
-				if ( fscanf(f, "%d:%d", &maj, &min) != 2 ) {
-					maj = -1;
-					min = -1;
-					fclose(f);
-					continue;
-				}
-
-				fclose(f);
-				break;
-
-			}
-
-			closedir(dir2);
-
-			if ( maj != -1 && min != -1 )
-				break;
-
-		}
-
-		closedir(dir);
-
+		local_find_internal_mydocs(&maj, &min);
 		if ( maj == -1 || min == -1 ) {
 			ERROR("Cannot find MyDocs mmc device: Slot 'internal' was not found");
 			goto clean;
 		}

-		VERBOSE("Detected internal mmc device: major=%d minor=%d\n", maj, min);
+		VERBOSE("Detected internal MyDocs mmc device: major=%d minor=%d\n", maj, min);

-		blk[0] = 0;
-
-		dir = opendir("/dev/");
-		if ( ! dir ) {
-			ERROR("Cannot find MyDocs mmc device: Opening '/dev/' failed");
+		fd = disk_open_dev(maj, min, 1, 1);
+		if ( fd < 0 ) {
+			ERROR("Cannot open MyDocs mmc device in /dev/");
 			goto clean;
 		}

-		while ( ( dirent = readdir(dir) ) ) {
+		ret = disk_dump_dev(fd, file);

-			snprintf(buf, sizeof(buf), "/dev/%s", dirent->d_name);
-
-			if ( stat(buf, &st) != 0 )
-				continue;
-
-			if ( ! S_ISBLK(st.st_mode) )
-				continue;
-
-			if ( makedev(maj, min) != st.st_rdev )
-				continue;
-
-			strcpy(blk, buf);
-			break;
-
-		}
-
-		closedir(dir);
-
-		if ( ! blk[0] ) {
-			ERROR("Cannot find MyDocs mmc device: Block device in /dev/ was not found");
-			goto clean;
-		}
-
-		VERBOSE("Detected internal mmc device: '%s'\n", blk);
-
-		strncat(blk, "p1", sizeof(blk));
-
-		printf("Using MyDocs mmc device: '%s'\n", blk);
-
-		ret = disk_dump_raw(blk, file);
+		close(fd);
+		fd = -1;

 	} else {

-		if ( device >= sizeof(nanddump)/sizeof(nanddump[0]) ) {
+		if ( device >= sizeof(nand_device)/sizeof(nand_device[0]) ) {
 			ERROR("Unsupported device");
 			goto clean;
 		}

-		if ( image >= nanddump[device].count ) {
+		if ( image >= nand_device[device].count ) {
 			ERROR("Unsupported image type: %s", image_type_to_string(image));
 			goto clean;
 		}

-		ret = local_nanddump(file, nanddump[device].args[image].mtd, nanddump[device].args[image].offset, nanddump[device].args[image].length);
+		header = nand_device[device].args[image].header;
+
+		if ( header > 0 ) {
+
+			ret = local_nanddump(file, nand_device[device].args[image].mtd, nand_device[device].args[image].offset, header);
+			if ( ret != 0 ) {
+				if ( ! simulate )
+					unlink(file);
+				ret = -1;
+				goto clean;
+			}
+
+			if ( ! simulate ) {
+
+				fd = open(file, O_RDONLY);
+				if ( fd >= 0 ) {
+					if ( read(fd, buf, 20) == 20 ) {
+						if ( memcmp(buf, "NOLO!img\x02\x00\x00\x00\x00\x00\x00\x00", 16) == 0 )
+							nlen = ((unsigned int)buf[16] << 0) | ((unsigned int)buf[17] << 8) | ((unsigned int)buf[18] << 16) | ((unsigned int)buf[19] << 24);
+						else if ( memcmp(buf, "NOLO img", 8) == 0 )
+							nlen = ((unsigned int)buf[8] << 0) | ((unsigned int)buf[9] << 8) | ((unsigned int)buf[10] << 16) | ((unsigned int)buf[11] << 24);
+					}
+					close(fd);
+					fd = -1;
+				}
+
+				unlink(file);
+
+			}
+
+		}
+
+		ret = local_nanddump(file, nand_device[device].args[image].mtd, nand_device[device].args[image].offset + header, nand_device[device].args[image].length - header);

 	}

@@ -437,6 +490,9 @@ int local_dump_image(enum image_type image, const char * file) {
 		goto clean;
 	}

+	if ( simulate )
+		goto clean;
+
 	fd = open(file, O_RDWR);
 	if ( fd < 0 )
 		goto clean;
@@ -445,27 +501,35 @@ int local_dump_image(enum image_type image, const char * file) {
 	if ( len == (off_t)-1 || len == 0 )
 		goto clean;

-	addr = mmap(NULL, len, PROT_READ, MAP_PRIVATE, fd, 0);
+	if ( header <= 0 || nlen == (off_t)-1 ) {

-	if ( addr == MAP_FAILED )
-		addr = NULL;
+		addr = (unsigned char *)mmap(NULL, len, PROT_READ, MAP_PRIVATE, fd, 0);

-	if ( ! addr )
-		goto clean;
+		if ( addr == MAP_FAILED )
+			addr = NULL;

-	for ( nlen = len; nlen > 0; --nlen )
-		if ( addr[nlen-1] != 0xFF )
-			break;
+		if ( ! addr )
+			goto clean;

-	if ( image == IMAGE_MMC )
-		align = 8;
-	else
-		align = 7;
+		for ( nlen = len; nlen > 0; --nlen )
+			if ( addr[nlen-1] != 0xFF )
+				break;

-	if ( ( nlen & ( ( 1ULL << align ) - 1 ) ) != 0 )
-		nlen = ((nlen >> align) + 1) << align;
+		for ( ; nlen > 0; --nlen )
+			if ( addr[nlen-1] != 0x00 )
+				break;

-	if ( nlen == 0 ) {
+		if ( image == IMAGE_MMC )
+			align = 8;
+		else
+			align = 7;
+
+		if ( ( nlen & ( ( 1ULL << align ) - 1 ) ) != 0 )
+			nlen = ((nlen >> align) + 1) << align;
+
+	}
+
+	if ( header <= 0 && nlen == 0 ) {
 		printf("File %s is empty, removing it...\n", file);
 		unlink(file);
 	} else if ( nlen != len ) {
@@ -476,7 +540,7 @@ int local_dump_image(enum image_type image, const char * file) {

 clean:
 	if ( addr )
-		munmap(addr, len);
+		munmap((void *)addr, len);

 	if ( fd >= 0 )
 		close(fd);
@@ -486,6 +550,153 @@ clean:

 }

+int local_flash_image(struct image * image) {
+
+	unsigned char buf[0x20000];
+	unsigned int remaining, size;
+	void (*sighandler)(int);
+	int min, maj, fd;
+	FILE * stream;
+	int ret;
+
+	printf("Flash image:\n");
+	image_print_info(image);
+
+	if ( image->type == IMAGE_MMC ) {
+
+		maj = -1;
+		min = -1;
+
+		local_find_internal_mydocs(&maj, &min);
+		if ( maj == -1 || min == -1 )
+			ERROR_RETURN("Cannot find MyDocs mmc device: Slot 'internal' was not found", -1);
+
+		VERBOSE("Detected internal MyDocs mmc device: major=%d minor=%d\n", maj, min);
+
+		fd = disk_open_dev(maj, min, 1, simulate ? 1 : 0);
+		if ( fd < 0 )
+			ERROR_RETURN("Cannot open MyDocs mmc device in /dev/", -1);
+
+		ret = disk_flash_dev(fd, image);
+
+		close(fd);
+
+	} else {
+
+		if ( device >= sizeof(nand_device)/sizeof(nand_device[0]) )
+			ERROR_RETURN("Unsupported device", -1);
+
+		if ( image->type >= nand_device[device].count ) {
+			ERROR("Unsupported image type: %s", image_type_to_string(image->type));
+			return -1;
+		}
+
+		if ( image->size > nand_device[device].args[image->type].length - nand_device[device].args[image->type].header )
+			ERROR_RETURN("Image is too big", -1);
+
+		printf("Using nandwrite for flashing %s image...\n", image_type_to_string(image->type));
+
+		stream = local_nandwrite(nand_device[device].args[image->type].mtd, nand_device[device].args[image->type].offset);
+		if ( ! simulate && ! stream )
+			ERROR_RETURN("Cannot start nandwrite process", -1);
+
+		ret = 0;
+		sighandler = signal(SIGPIPE, SIG_IGN);
+
+		/* Write NOLO!img or NOLO img header with size */
+		if ( nand_device[device].args[image->type].header > 0 ) {
+			if ( device == DEVICE_RX_51 ) {
+				memcpy(buf, "NOLO!img\x02\x00\x00\x00\x00\x00\x00\x00", 16);
+				buf[16] = (image->size >>  0) & 0xFF;
+				buf[17] = (image->size >>  8) & 0xFF;
+				buf[18] = (image->size >> 16) & 0xFF;
+				buf[19] = (image->size >> 24) & 0xFF;
+				size = 20;
+			} else {
+				memcpy(buf, "NOLO img", 8);
+				buf[ 8] = (image->size >>  0) & 0xFF;
+				buf[ 9] = (image->size >>  8) & 0xFF;
+				buf[10] = (image->size >> 16) & 0xFF;
+				buf[11] = (image->size >> 24) & 0xFF;
+				size = 12;
+			}
+
+			if ( ! simulate ) {
+				if ( fwrite(buf, size, 1, stream) != 1 ) {
+					ret = -1;
+					goto clean;
+				}
+			}
+
+			memset(buf, 0, sizeof(buf));
+			remaining = nand_device[device].args[image->type].header - size;
+			while ( remaining > 0 ) {
+				size = remaining < sizeof(buf) ? remaining : sizeof(buf);
+				if ( ! simulate ) {
+					if ( fwrite(buf, size, 1, stream) != 1 ) {
+						ret = -1;
+						goto clean;
+					}
+				}
+				remaining -= size;
+			}
+		}
+
+		/* Write image data */
+		image_seek(image, 0);
+		remaining = image->size;
+		while ( remaining > 0 ) {
+			size = remaining < sizeof(buf) ? remaining : sizeof(buf);
+			size = image_read(image, buf, size);
+			if ( size == 0 ) {
+				ERROR("Failed to read image");
+				ret = -1;
+				goto clean;
+			}
+			if ( ! simulate ) {
+				if ( fwrite(buf, size, 1, stream) != 1 ) {
+					ret = -1;
+					goto clean;
+				}
+			}
+			remaining -= size;
+		}
+
+		/* Write filler zeros to clear previous data */
+		memset(buf, 0, sizeof(buf));
+		remaining = nand_device[device].args[image->type].length - (image->size + nand_device[device].args[image->type].header);
+		while ( remaining > 0 ) {
+			size = remaining < sizeof(buf) ? remaining : sizeof(buf);
+			if ( ! simulate ) {
+				if ( fwrite(buf, size, 1, stream) != 1 ) {
+					ret = -1;
+					goto clean;
+				}
+			}
+			remaining -= size;
+		}
+
+clean:
+		if ( ! simulate ) {
+			if ( ret == 0 )
+				ret = pclose(stream);
+			else
+				pclose(stream);
+		}
+
+		signal(SIGPIPE, sighandler);
+
+		if ( ret != 0 )
+			ERROR("Flashing failed");
+	}
+
+	if ( ret == 0 )
+		printf("Done\n");
+
+	return ret;
+
+}
+
 int local_check_badblocks(const char * device) {

 	ERROR("Not implemented yet");
@@ -532,7 +743,7 @@ int local_set_usb_host_mode(int enable) {

 int local_get_rd_mode(void) {

-	if ( strncmp(rd_mode, "master", strlen("master")) == 0 )
+	if ( strncmp(rd_mode, "master", sizeof("master")-1) == 0 )
 		return 1;
 	else
 		return 0;
@@ -551,8 +762,8 @@ int local_get_rd_flags(char * flags, size_t size) {

 	const char * ptr;

-	if ( strncmp(rd_mode, "master", strlen("master")) == 0 )
-		ptr = rd_mode + strlen("master");
+	if ( strncmp(rd_mode, "master", sizeof("master")-1) == 0 )
+		ptr = rd_mode + sizeof("master")-1;
 	else
 		ptr = rd_mode;

@@ -36,6 +36,9 @@
 #include "device.h"
 #include "operations.h"

+extern char *optarg;
+extern int optind, opterr, optopt;
+
 static void show_title(void) {
 	printf("0xFFFF v%s  // Open Free Fiasco Firmware Flasher\n", VERSION);
 }
@@ -53,8 +56,8 @@ static void show_usage(void) {
 		" -f              flash all specified images\n"
 		" -c              cold flash 2nd and secondary images\n"
 		" -x [/dev/mtd]   check for bad blocks on mtd device (default: all)\n"
-		" -E file         dump all device images to one fiasco image, see -t\n"
-		" -e [dir]        dump all device images to directory, see -t (default: current)\n"
+		" -E file         dump all device images to one fiasco image\n"
+		" -e [dir]        dump all device images (or one -t) to directory (default: current)\n"
 		"\n"

 		"Device configuration:\n"
@@ -74,17 +77,19 @@ static void show_usage(void) {
 		"Input image specification:\n"
 		" -M file         specify fiasco image\n"
 		" -m arg          specify normal image\n"
-		"                 arg is [[[dev:[hw:]]ver:]type:]file[%%lay]\n"
+		"                 arg is [[[dev:[hw:]]ver:]type:]file[@name][#file2[@name2]...][%%lay]\n"
 		"                   dev is device name string (default: empty)\n"
 		"                   hw are comma separated list of HW revisions (default: empty)\n"
 		"                   ver is image version string (default: empty)\n"
 		"                   type is image type (default: autodetect)\n"
 		"                   file is image file name\n"
+		"                   fileN is file name of the Nth image part\n"
+		"                   nameN is name of the Nth image part (default: none)\n"
 		"                   lay is layout file name (default: none)\n"
 		"\n"

 		"Image filters:\n"
-		" -t types        filter images by type\n"
+		" -t type         filter images by type\n"
 		" -d dev          filter images by device\n"
 		" -w hw           filter images by HW revision\n"
 		"\n"
@@ -140,32 +145,54 @@ int simulate;
 int noverify;
 int verbose;

-/* arg = [[[dev:[hw:]]ver:]type:]file[%%lay] */
+/* arg = [[[dev:[hw:]]ver:]type:]file[@name][#file2[@name2][#file3[@name3]...]][%lay] */
 static void parse_image_arg(char * arg, struct image_list ** image_first) {

 	struct stat st;
 	struct image * image;
+	struct image_fd * image_fd;
+	struct image_part * image_part;
+	struct image_part * image_parts;
 	char * file;
+	const char ** files;
 	char * type;
 	char * device;
 	char * hwrevs;
 	char * version;
 	char * layout;
+	char * part_files;
 	char * layout_file;
+	char * name;
+	char * ptr;
+	uint32_t offset;
+	int count;
+	int fd;

 	/* First check if arg is file, then try to parse arg format */
-	if ( stat(arg, &st) == 0 ) {
-		image = image_alloc_from_file(arg, NULL, NULL, NULL, NULL, NULL);
-		if ( ! image ) {
-			ERROR("Cannot load image file %s", arg);
-			exit(1);
+	fd = open(arg, O_RDONLY);
+	if ( fd >= 0 ) {
+		if ( fstat(fd, &st) == 0 && !S_ISDIR(st.st_mode) ) {
+			image = image_alloc_from_fd(fd, arg, NULL, NULL, NULL, NULL, NULL, NULL);
+			if ( ! image ) {
+				ERROR("Cannot load image file %s", arg);
+				exit(1);
+			}
+			image_list_add(image_first, image);
+			return;
 		}
-		image_list_add(image_first, image);
-		return;
+		close(fd);
+	} else if ( errno != ENOENT ) {
+		ERROR("Cannot load image file %s", arg);
+		exit(1);
 	}

-	layout_file = strchr(arg, '%');
-	if (layout_file)
+	part_files = strchr(arg, '#');
+	if ( part_files )
+		*(part_files++) = 0;
+
+	layout_file = part_files ? part_files : arg;
+	layout_file = strchr(layout_file, '%');
+	if ( layout_file )
 		*(layout_file++) = 0;

 	type = NULL;
@@ -175,16 +202,16 @@ static void parse_image_arg(char * arg, struct image_list ** image_first) {
 	layout = NULL;

 	file = strrchr(arg, ':');
-	if (file) {
+	if ( file ) {
 		*(file++) = 0;
 		type = strrchr(arg, ':');
-		if (type) {
+		if ( type ) {
 			*(type++) = 0;
 			version = strrchr(arg, ':');
-			if (version) {
+			if ( version ) {
 				*(version++) = 0;
 				hwrevs = strchr(arg, ':');
-				if (hwrevs)
+				if ( hwrevs )
 					*(hwrevs++) = 0;
 				device = arg;
 			} else {
@@ -209,28 +236,102 @@ static void parse_image_arg(char * arg, struct image_list ** image_first) {
 			ERROR_INFO("Cannot get size of file %s", layout_file);
 			exit(1);
 		}
-		lseek(fd, 0, SEEK_SET);
-		layout = malloc(len);
+		if ( lseek(fd, 0, SEEK_SET) == (off_t)-1 ) {
+			ERROR_INFO("Cannot seek to begin of file %s", layout_file);
+			exit(1);
+		}
+		layout = malloc(len+1);
 		if ( ! layout ) {
 			ALLOC_ERROR();
 			exit(1);
 		}
 		if ( read(fd, layout, len) != len ) {
-			ERROR_INFO("Cannot read %lu bytes from layout file %s", len, layout_file);
+			ERROR_INFO("Cannot read %ju bytes from layout file %s", (intmax_t)len, layout_file);
 			exit(1);
 		}
+		layout[len] = 0;
+		close(fd);
 	}

-	image = image_alloc_from_file(file, type, device, hwrevs, version, layout);
+	count = 0;
+	ptr = part_files;
+	while ( ptr ) {
+		count++;
+		ptr = strchr(ptr, '#');
+		if ( ptr )
+			ptr++;
+	}

-	if ( layout )
-		free(layout);
+	files = calloc(count+1, sizeof(*files));
+	if ( ! files ) {
+		ALLOC_ERROR();
+		exit(1);
+	}
+
+	name = strchr(file, '@');
+	if ( name ) {
+		*name = 0;
+		name++;
+	}
+
+	if ( count > 0 || name ) {
+		image_part = calloc(1, sizeof(*image_part));
+		if ( ! image_part ) {
+			ALLOC_ERROR();
+			exit(1);
+		}
+		image_part->name = name ? strdup(name) : NULL;
+		image_parts = image_part;
+	} else {
+		image_part = NULL;
+		image_parts = NULL;
+	}
+
+	count = 0;
+	files[count++] = file;
+
+	while ( part_files ) {
+		ptr = strchr(part_files, '#');
+		if ( ptr ) {
+			*ptr = 0;
+			ptr++;
+		}
+		name = strchr(part_files, '@');
+		if ( name ) {
+			*name = 0;
+			name++;
+		}
+		image_part->next = calloc(1, sizeof(*image_part));
+		if ( ! image_part->next ) {
+			ALLOC_ERROR();
+			exit(1);
+		}
+		image_part = image_part->next;
+		image_part->name = name ? strdup(name) : NULL;
+		files[count++] = part_files;
+		part_files = ptr;
+	}
+
+	image = image_alloc_from_files(files, count, type, device, hwrevs, version, layout, image_parts);
+	free(files);
+	free(layout);

 	if ( ! image ) {
 		ERROR("Cannot load image file %s", file);
 		exit(1);
 	}

+	image_part = image_parts;
+	image_fd = image->fds;
+	offset = 0;
+	while ( image_part && image_fd ) {
+		image_part->offset = offset;
+		image_part->size = image_fd->size;
+		offset += image_part->size;
+		image_part = image_part->next;
+		image_fd = image_fd->next;
+	}
+
 	image_list_add(image_first, image);

 }
@@ -405,9 +506,13 @@ int main(int argc, char **argv) {
 	enum device detected_device = DEVICE_UNKNOWN;
 	int16_t detected_hwrev = -1;

+	struct stat st;
+
 	int i;
 	char buf[512];
 	char * ptr = NULL;
+	char * ptr1 = NULL;
+	char * ptr2 = NULL;
 	char * tmp = NULL;

 	char nolo_ver[512];
@@ -661,6 +766,9 @@ int main(int argc, char **argv) {
 			goto clean;
 		}
 		filter_images_by_type(type, &image_first);
+		/* make sure that fiasco_in has valid images */
+		if ( fiasco_in )
+			fiasco_in->first = image_first;
 	}

 	/* filter images by device */
@@ -672,11 +780,18 @@ int main(int argc, char **argv) {
 			goto clean;
 		}
 		filter_images_by_device(device, &image_first);
+		/* make sure that fiasco_in has valid images */
+		if ( fiasco_in )
+			fiasco_in->first = image_first;
 	}

 	/* filter images by hwrev */
-	if ( filter_hwrev )
+	if ( filter_hwrev ) {
 		filter_images_by_hwrev(atoi(filter_hwrev_arg), &image_first);
+		/* make sure that fiasco_in has valid images */
+		if ( fiasco_in )
+			fiasco_in->first = image_first;
+	}

 	/* reorder images for flashing (first x-loader, second secondary) */
 	/* set 2nd and secondary images for cold-flashing */
@@ -740,11 +855,29 @@ int main(int argc, char **argv) {
 			image_ptr = next;
 		}

+		/* make sure that fiasco_in has valid images */
+		if ( fiasco_in )
+			fiasco_in->first = image_first;
+
 	}

-	/* make sure that fiasco_in has valid images*/
-	if ( fiasco_in )
-		fiasco_in->first = image_first;
+	/* remove 2nd image when doing normal flash */
+	if ( dev_flash ) {
+		image_ptr = image_first;
+		while ( image_ptr ) {
+			struct image_list * next = image_ptr->next;
+			if ( image_ptr->image->type == IMAGE_2ND ) {
+				if ( image_ptr == image_first )
+					image_first = next;
+				image_list_del(image_ptr);
+			}
+			image_ptr = next;
+		}
+
+		/* make sure that fiasco_in has valid images */
+		if ( fiasco_in )
+			fiasco_in->first = image_first;
+	}

 	/* identify images */
 	if ( image_ident ) {
@@ -779,11 +912,12 @@ int main(int argc, char **argv) {
 	while ( image_ptr ) {
 		struct image_list * next = image_ptr->next;
 		if ( image_ptr->image->type == IMAGE_UNKNOWN ) {
-			WARNING("Removing unknown image (specified by %s %s)", image_ptr->image->orig_filename ? "file" : "fiasco", image_ptr->image->orig_filename ? image_ptr->image->orig_filename : "image");
+			WARNING("Removing unknown image (specified by %s %s)", (image_ptr->image->fds && image_ptr->image->fds->orig_filename) ? "file" : "fiasco", (image_ptr->image->fds && image_ptr->image->fds->orig_filename) ? image_ptr->image->fds->orig_filename : "image");
 			if ( image_ptr == image_first )
 				image_first = next;
-			image_list_unlink(image_ptr);
-			free(image_ptr);
+			if ( fiasco_in && image_ptr == fiasco_in->first )
+				fiasco_in->first = fiasco_in->first->next;
+			image_list_del(image_ptr);
 		}
 		image_ptr = next;
 	}
@@ -797,9 +931,16 @@ int main(int argc, char **argv) {
 		char * swver = strchr(fiasco_gen_arg, '%');
 		if ( swver )
 			*(swver++) = 0;
+		if ( swver && strlen(swver) >= sizeof(fiasco_out->swver) ) {
+			ERROR("SW rel version is too long");
+			ret = 1;
+			goto clean;
+		}
 		fiasco_out = fiasco_alloc_empty();
 		if ( ! fiasco_out ) {
 			ERROR("Cannot write images to fiasco file %s", fiasco_gen_arg);
+			ret = 1;
+			goto clean;
 		} else {
 			if ( swver )
 				strcpy(fiasco_out->swver, swver);
@@ -956,6 +1097,7 @@ int main(int argc, char **argv) {
 			printf("\n");

 			if ( ret == 1 ) {
+				buf[0] = 0;
 				ret = dev_get_rd_flags(dev, buf, sizeof(buf));
 				printf("R&D flags: ");
 				if ( ret < 0 )
@@ -981,8 +1123,10 @@ int main(int argc, char **argv) {
 			/* filter images by device & hwrev */
 			if ( detected_device )
 				filter_images_by_device(dev->detected_device, &image_first);
-			if ( detected_hwrev )
+			if ( detected_hwrev > 0 )
 				filter_images_by_hwrev(dev->detected_hwrev, &image_first);
+			if ( fiasco_in && ( detected_device || detected_hwrev > 0 ) )
+				fiasco_in->first = image_first;

 			/* set kernel and initfs images for loading */
 			if ( dev_load ) {
@@ -1039,7 +1183,7 @@ int main(int argc, char **argv) {
 			if ( dev_load ) {
 				if ( image_kernel ) {
 					ret = dev_load_image(dev, image_kernel->image);
-					if ( ret < 0 )
+					if ( ret == -EAGAIN )
 						goto again;

 					if ( image_kernel == image_first )
@@ -1047,23 +1191,21 @@ int main(int argc, char **argv) {
 					if ( fiasco_in && image_kernel == fiasco_in->first )
 						fiasco_in->first = fiasco_in->first->next;

-					image_list_unlink(image_kernel);
-					free(image_kernel);
+					image_list_del(image_kernel);
 					image_kernel = NULL;
 				}

 				if ( image_initfs ) {
 					ret = dev_load_image(dev, image_initfs->image);
-					if ( ret < 0 )
+					if ( ret == -EAGAIN )
 						goto again;

 					if ( image_initfs == image_first )
 						image_first = image_first->next;
-					if ( fiasco_in && image_kernel == fiasco_in->first )
+					if ( fiasco_in && image_initfs == fiasco_in->first )
 						fiasco_in->first = fiasco_in->first->next;

-					image_list_unlink(image_initfs);
-					free(image_initfs);
+					image_list_del(image_initfs);
 					image_initfs = NULL;
 				}
 			}
@@ -1074,16 +1216,15 @@ int main(int argc, char **argv) {
 				while ( image_ptr ) {
 					struct image_list * next = image_ptr->next;
 					ret = dev_flash_image(dev, image_ptr->image);
-					if ( ret < 0 )
+					if ( ret == -EAGAIN )
 						goto again;

 					if ( image_ptr == image_first )
 						image_first = image_first->next;
-					if ( fiasco_in && image_kernel == fiasco_in->first )
+					if ( fiasco_in && image_ptr == fiasco_in->first )
 						fiasco_in->first = fiasco_in->first->next;

-					image_list_unlink(image_ptr);
-					free(image_ptr);
+					image_list_del(image_ptr);
 					image_ptr = next;
 				}
 			}
@@ -1175,14 +1316,28 @@ int main(int argc, char **argv) {
 						buf[0] = 0;
 				}

-				for ( i = 0; i < IMAGE_COUNT; ++i )
-					if ( image_tmp_name(i) )
-						dev_dump_image(dev, i, image_tmp_name(i));
+				if ( filter_type ) {
+					enum image_type type = image_type_from_string(filter_type_arg);
+					if ( ! type || ! image_tmp_name(type) ) {
+						ERROR("Specified unknown image type for filtering: %s", filter_type_arg);
+						ret = 1;
+						goto clean;
+					}
+					ret = dev_dump_image(dev, type, image_tmp_name(type));
+					if ( ret != 0 )
+						goto clean;
+				} else {
+					for ( i = 0; i < IMAGE_COUNT; ++i )
+						if ( image_tmp_name(i) )
+							dev_dump_image(dev, i, image_tmp_name(i));
+				}

 				if ( buf[0] )
 					if ( chdir(buf) < 0 )
 						ERROR_INFO("Cannot chdir back to %s", buf);

+				printf("Done\n");
+
 			}

 			/* dump fiasco */
@@ -1193,10 +1348,12 @@ int main(int argc, char **argv) {

 				for ( i = 0; i < IMAGE_COUNT; ++i ) {

-					if ( ! image_tmp_name(i) )
+					if ( ! image_tmp_name(i) || ( stat(image_tmp_name(i), &st) < 0 && errno == ENOENT ) )
 						continue;

-					sprintf(buf, "%hd", dev->detected_hwrev);
+					buf[0] = 0;
+					if ( dev->detected_hwrev > 0 )
+						snprintf(buf, sizeof(buf), "%hd", dev->detected_hwrev);

 					switch ( i ) {
 						case IMAGE_2ND:
@@ -1226,7 +1383,9 @@ int main(int argc, char **argv) {
 							break;
 					}

-					image_dump = image_alloc_from_file(image_tmp_name(i), image_type_to_string(i), device_to_string(dev->detected_device), buf, ptr, NULL);
+					/* TODO: add support for dumping mmc layout and also other mmc partitions as image data parts */
+
+					image_dump = image_alloc_from_file(image_tmp_name(i), image_type_to_string(i), device_to_string(dev->detected_device), buf, ptr, NULL, NULL);

 					if ( ! image_dump )
 						continue;
@@ -1262,14 +1421,12 @@ int main(int argc, char **argv) {

 				for ( i = 0; i < IMAGE_COUNT; ++i ) {

-					struct stat st;
+					int rename_ret;
+					int rename_errno;

 					if ( ! image_tmp_name(i) )
 						continue;

-					if ( stat(image_tmp_name(i), &st) != 0 )
-						continue;
-
 					switch ( i ) {
 						case IMAGE_2ND:
 						case IMAGE_XLOADER:
@@ -1298,19 +1455,50 @@ int main(int argc, char **argv) {
 							break;
 					}

-					sprintf(buf, "%s-%s:%hd_%s", image_type_to_string(i), device_to_string(dev->detected_device), dev->detected_hwrev, ptr);
+					buf[0] = 0;
+					snprintf(buf, sizeof(buf), "%s", image_type_to_string(i));
+					ptr1 = buf + strlen(buf);
+
+					if ( dev->detected_device ) {
+						snprintf(ptr1, sizeof(buf)-(ptr1-buf), "-%s", device_to_string(dev->detected_device));
+						ptr1 += strlen(ptr1);
+					}
+
+					if ( dev->detected_hwrev > 0 )
+						snprintf(ptr1, sizeof(buf)-(ptr1-buf), ":%hd", dev->detected_hwrev);
+					ptr2 = ptr1 + strlen(ptr1);
+
+					if ( ptr && ptr[0] )
+						snprintf(ptr2, sizeof(buf)-(ptr2-buf), "_%s", ptr);
+
+					rename_ret = rename(image_tmp_name(i), buf);
+					rename_errno = errno;
+
+					if ( rename_ret < 0 && rename_errno == ENOENT )
+						continue;
+
 					printf("Renaming %s image file to %s...\n", image_type_to_string(i), buf);

-					if ( rename(image_tmp_name(i), buf) < 0 ) {
+					if ( rename_ret < 0 ) {

+						errno = rename_errno;
 						ERROR_INFO("Renaming failed");

-						sprintf(buf, "%s-%s_%s", image_type_to_string(i), device_to_string(dev->detected_device), ptr);
+						*ptr2 = 0;
 						printf("Trying to rename %s image file to %s...\n", image_type_to_string(i), buf);

-						if ( rename(image_tmp_name(i), buf) < 0 )
+						if ( rename(image_tmp_name(i), buf) < 0 ) {
+
 							ERROR_INFO("Renaming failed");

+							*ptr1 = 0;
+							printf("Trying to rename %s image file to %s...\n", image_type_to_string(i), buf);
+
+							if ( rename(image_tmp_name(i), buf) < 0 )
+								ERROR_INFO("Renaming failed");
+
+						}
+
 					}

 				}
@@ -25,6 +25,10 @@
 #define INFO NAME " \\- Open Free Fiasco Firmware Flasher, version " VERSION
 #define DESCRIPTION "0xFFFF is Open Free Fiasco Firmware Flasher for Maemo devices. It support generating and unpacking FIASCO images on local computer. Useful for editing Maemo firmware package for future flash. It support via USB flashing any image type to Maemo device and also \"cold\" flashing which means flashing dead device with erased bootloader. There is support for booting kernel via USB without flashing to NAND and also changing configuration of Maemo device (enable/disable R&D mode, changing HW revision strings, ...).\n\n0xFFFF is alternative tool to proprietary Nokia flasher-3.5 and fiasco-gen. 0xFFFF generate compatible FIASCO images and also accept FIASCO images generated by Nokia fiasco-gen."

+#ifndef BUILD_DATE
+#define BUILD_DATE __DATE__
+#endif
+
 int main() {

 	FILE * pipe;
@@ -34,7 +38,7 @@ int main() {
 	if ( ! pipe )
 		return 1;

-	puts(".TH " NAME " 1 \"" __DATE__ "\" \"" NAME " " VERSION "\"");
+	puts(".TH " NAME " 1 \"" BUILD_DATE "\" \"" NAME " " VERSION "\"");
 	puts("");

 	puts(".SH NAME");
@@ -17,42 +17,363 @@

 */

+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <arpa/inet.h>
+
 #include "mkii.h"
 #include "global.h"
 #include "image.h"
 #include "device.h"
 #include "usb-device.h"

+#define MKII_OUT	0x8810001B
+#define MKII_IN		0x8800101B
+
+#define MKII_PING	0x00
+#define MKII_GET	0x01
+#define MKII_TELL	0x02
+#define MKII_REBOOT	0x0C
+#define MKII_RESPONCE	0x20
+
+struct mkii_message {
+	uint32_t header;
+	uint16_t size;
+	uint16_t zero;
+	uint8_t num;
+	uint8_t type;
+	char data[];
+} __attribute__((__packed__));
+
+
+static int mkii_send_receive(libusb_device_handle * udev, uint8_t type, struct mkii_message * in_msg, size_t data_size, struct mkii_message * out_msg, size_t out_size) {
+
+	int ret;
+	static uint8_t number = 0;
+
+	in_msg->header = MKII_OUT;
+	in_msg->size = htons(data_size + 4);
+	in_msg->zero = 0;
+	in_msg->num = number++;
+	in_msg->type = type;
+
+	ret = usb_bulk_send(udev, USB_WRITE_EP, in_msg, data_size + sizeof(*in_msg), 5000);
+	if ( ret < 0 )
+		return ret;
+	if ( (size_t)ret != data_size + sizeof(*in_msg) )
+		return -1;
+
+	ret = usb_bulk_recv(udev, USB_READ_EP, out_msg, out_size, 5000);
+	if ( ret < 0 )
+		return ret;
+
+	if ( out_msg->header != MKII_IN )
+		return -1;
+
+	if ( out_msg->type != (type | MKII_RESPONCE) )
+		return -1;
+
+	if ( (size_t)ret < sizeof(*out_msg) )
+		return -1;
+
+	if ( ntohs(out_msg->size) != ret - sizeof(*out_msg) + 4 )
+		return -1;
+
+	return ret - sizeof(*out_msg);
+
+}
+
 int mkii_init(struct usb_device_info * dev) {

-	ERROR("Mk II protocol is not implemented yet");
-	(void)dev;
-	return -1;
+	char buf[2048];
+	struct mkii_message * msg;
+	struct libusb_config_descriptor * config;
+	enum device device;
+	int ret;
+	char * newptr;
+	char * ptr;
+	enum image_type type;
+
+	printf("Initializing Mk II protocol...\n");
+
+	msg = (struct mkii_message *)buf;
+
+	ret = mkii_send_receive(dev->udev, MKII_PING, msg, 0, msg, sizeof(buf));
+	if ( ret != 0 )
+		ERROR_RETURN("Cannot ping device", -1);
+
+	memcpy(msg->data, "/update/protocol_version", sizeof("/update/protocol_version")-1);
+	ret = mkii_send_receive(dev->udev, MKII_GET, msg, sizeof("/update/protocol_version")-1, msg, sizeof(buf));
+	if ( ret < 2 || msg->data[0] != 0 )
+		ERROR_RETURN("Cannot get Mk II protocol version", -1);
+
+	msg->data[ret] = 0;
+
+	if ( ret == 2 && msg->data[1] == 0x32 )
+		dev->data |= MKII_SUPPORT_SW_RELEASE;
+
+	printf("Detected Mk II protocol version: %s\n", msg->data);
+
+	memcpy(msg->data, "/update/host_protocol_version\x00\x32", sizeof("/update/host_protocol_version\x00\x32")-1);
+	ret = mkii_send_receive(dev->udev, MKII_TELL, msg, sizeof("/update/host_protocol_version\x00\x32")-1, msg, sizeof(buf));
+	if ( ret != 1 || msg->data[0] != 0 )
+		ERROR_RETURN("Cannot send our protocol version", -1);
+
+	device = mkii_get_device(dev);
+
+	if ( ! dev->device )
+		dev->device = device;
+
+	if ( dev->device && device && dev->device != device ) {
+		ERROR("Device mishmash, expected %s, got %s", device_to_string(dev->device), device_to_string(device));
+		return -1;
+	}
+
+	dev->hwrev = mkii_get_hwrev(dev);
+
+	memcpy(msg->data, "/update/supported_images", sizeof("/update/supported_images")-1);
+	ret = mkii_send_receive(dev->udev, MKII_GET, msg, sizeof("/update/supported_images")-1, msg, sizeof(buf));
+	if ( ret < 2 || msg->data[0] != 0 )
+		ERROR_RETURN("Cannot get supported image types", -1);
+
+	msg->data[ret] = 0;
+	ptr = msg->data + 1;
+
+	printf("Supported images by current device configuration:");
+
+	while ( ptr && *ptr ) {
+		newptr = strchr(ptr, ',');
+		if ( newptr ) {
+			*newptr = 0;
+			++newptr;
+		}
+		type = image_type_from_string(ptr);
+		if ( type != IMAGE_UNKNOWN ) {
+			dev->data |= (1UL << type);
+			printf(" %s", ptr);
+		}
+		ptr = newptr;
+	}
+
+	printf("\n");
+
+	memset(buf, 0, sizeof(buf));
+	if ( libusb_get_config_descriptor(libusb_get_device(dev->udev), 0, &config) == 0 ) {
+		libusb_get_string_descriptor_ascii(dev->udev, config->iConfiguration, (unsigned char *)buf, sizeof(buf));
+		libusb_free_config_descriptor(config);
+	}
+	if ( strncmp(buf, "Firmware Upgrade Configuration", sizeof("Firmware Upgrade Configuration")) == 0 )
+		dev->data |= MKII_UPDATE_MODE;
+
+	printf("Mode: %s\n", (dev->data & MKII_UPDATE_MODE) ? "Update" : "PC Suite");
+
+	return 0;

 }

 enum device mkii_get_device(struct usb_device_info * dev) {

-	ERROR("Not implemented yet");
-	(void)dev;
-	return DEVICE_UNKNOWN;
+	char buf[2048];
+	struct mkii_message * msg;
+	int ret;
+
+	msg = (struct mkii_message *)buf;
+
+	memcpy(msg->data, "/device/product_code", sizeof("/device/product_code")-1);
+	ret = mkii_send_receive(dev->udev, MKII_GET, msg, sizeof("/device/product_code")-1, msg, sizeof(buf));
+	if ( ret < 2 || msg->data[0] != 0 || msg->data[1] == 0 )
+		return DEVICE_UNKNOWN;
+
+	msg->data[ret] = 0;
+	return device_from_string((char *)msg->data+1);

 }

 int mkii_flash_image(struct usb_device_info * dev, struct image * image) {

+	char buf1[512];
+	char buf[2048];
+	struct mkii_message * msg1;
+	struct mkii_message * msg;
+	char * ptr;
+	const char * type;
+	uint8_t len;
+	uint16_t hash;
+	uint32_t size;
+	int ret;
+
 	ERROR("Not implemented yet");
-	(void)dev;
-	(void)image;
 	return -1;

+	if ( ! ( dev->data & (1UL << image->type) ) ) {
+		ERROR("Flashing image %s is not supported in current device configuration", image_type_to_string(image->type));
+		return -1;
+	}
+
+	msg = (struct mkii_message *)buf;
+	msg1 = (struct mkii_message *)buf1;
+	ptr = msg->data;
+
+	/* File data header */
+	memcpy(ptr, "\x2E", 1);
+	ptr += 1;
+
+	/* Length of file data */
+	memcpy(ptr, "\x19", 1);
+	ptr += 1;
+
+	/* File data (ignored): asic index: APE (0x01), device type: NAND (0x01), device index: 0 */
+	memcpy(ptr, "\x01\x01\x00", 3);
+	ptr += 3;
+
+	/* Hash */
+	hash = htons(image->hash);
+	memcpy(ptr, &hash, 2);
+	ptr += 2;
+
+	/* Type */
+	type = image_type_to_string(image->type);
+	if ( ! type )
+		ERROR_RETURN("Unknown image type", -1);
+	memset(ptr, 0, 12);
+	strncpy(ptr, type, 12);
+	ptr += 12;
+
+	/* Size */
+	size = htonl(image->size);
+	memcpy(ptr, &size, 4);
+	ptr += 4;
+
+	/* Load address (ignored) */
+	memcpy(ptr, "\x00\x00\x00\x00", 4);
+	ptr += 4;
+
+	/* Device & hwrev */
+	if ( image->devices ) {
+
+		int i;
+		uint8_t len;
+		char buf[9];
+		char ** bufs = NULL;
+		struct device_list * device = image->devices;
+
+		while ( device ) {
+			if ( device->device == dev->device && hwrev_is_valid(device->hwrevs, dev->hwrev) )
+				break;
+			device = device->next;
+		}
+
+		if ( device )
+			bufs = device_list_alloc_to_bufs(device);
+
+		if ( bufs ) {
+
+			memset(buf, 0, sizeof(buf));
+			snprintf(buf, 8+1, "%d", dev->hwrev);
+
+			for ( i = 0; bufs[i]; ++i ) {
+				len = ((uint8_t*)bufs[i])[0];
+				if ( MEMMEM(bufs[i]+1, len, buf, strlen(buf)) )
+					break;
+			}
+
+			if ( bufs[i] ) {
+				/* Device & hwrev string header */
+				memcpy(ptr, "\x32", 1);
+				ptr += 1;
+				/* Device & hwrev string size */
+				memcpy(ptr, &len, 1);
+				ptr += 1;
+				/* Device & hwrev string */
+				memcpy(ptr, bufs[i]+1, len);
+				ptr += len;
+			}
+
+			free(bufs);
+
+		}
+
+	}
+
+	/* Version */
+	if ( image->version ) {
+		len = strnlen(image->version, 255) + 1;
+		/* Version string header */
+		memcpy(ptr, "\x31", 1);
+		ptr += 1;
+		/* Version string size */
+		memcpy(ptr, &len, 1);
+		ptr += 1;
+		/* Version string */
+		memcpy(ptr, image->version, len);
+		ptr += len;
+	}
+
+	/* append layout subsection */
+	if ( image->layout ) {
+		len = strlen(image->layout);
+		/* Layout header */
+		memcpy(ptr, "\x33", 1);
+		ptr += 1;
+		/* Layout size */
+		memcpy(ptr, &len, 1);
+		ptr += 1;
+		/* Layout string */
+		memcpy(ptr, image->layout, len);
+		ptr += len;
+	}
+
+	/* end */
+	memcpy(ptr, "\x00", 1);
+	ptr += 1;
+
+	ret = mkii_send_receive(dev->udev, 0x03, msg1, 0, msg1, sizeof(buf1));
+	if ( ret != 1 || msg1->data[0] != 0 )
+		return -1;
+
+	ret = mkii_send_receive(dev->udev, 0x04, msg, ptr - msg->data, msg, sizeof(buf));
+	if ( ret != 9 )
+		return -1;
+
+	/* TODO: send image itself */
+
+	return 0;
+
 }

-int mkii_reboot_device(struct usb_device_info * dev) {
+int mkii_reboot_device(struct usb_device_info * dev, int update) {

-	ERROR("Not implemented yet");
-	(void)dev;
-	return -1;
+	char buf[2048];
+	struct mkii_message * msg;
+	const char * str;
+	int len;
+	int ret;
+
+	msg = (struct mkii_message *)buf;
+
+	if ( update ) {
+		printf("Rebooting device to Update mode...\n");
+		len = sizeof("reboot=update");
+		str = "reboot=update";
+	} else {
+		printf("Rebooting device...\n");
+		len = sizeof("reboot");
+		str = "reboot";
+	}
+
+	memcpy(msg->data, str, len);
+	ret = mkii_send_receive(dev->udev, MKII_REBOOT, msg, len, msg, sizeof(buf));
+	if ( ret != 1 || msg->data[0] != 0 )
+		ERROR_RETURN("Cannot send reboot command", -1);
+
+	if (dev->data & MKII_UPDATE_MODE)
+		MSLEEP(100);
+	else
+		MSLEEP(3000);
+
+	return 0;

 }

@@ -128,9 +449,19 @@ int mkii_set_rd_flags(struct usb_device_info * dev, const char * flags) {

 int16_t mkii_get_hwrev(struct usb_device_info * dev) {

-	ERROR("Not implemented yet");
-	(void)dev;
-	return -1;
+	char buf[2048];
+	struct mkii_message * msg;
+	int ret;
+
+	msg = (struct mkii_message *)buf;
+
+	memcpy(msg->data, "/device/hw_build", sizeof("/device/hw_build")-1);
+	ret = mkii_send_receive(dev->udev, MKII_GET, msg, sizeof("/device/hw_build")-1, msg, sizeof(buf));
+	if ( ret < 2 || msg->data[0] != 0 || msg->data[1] == 0 )
+		ERROR_RETURN("Cannot get hw revision", -1);
+
+	msg->data[ret] = 0;
+	return atoi(msg->data+1);

 }

@@ -202,11 +533,24 @@ int mkii_set_nolo_ver(struct usb_device_info * dev, const char * ver) {

 int mkii_get_sw_ver(struct usb_device_info * dev, char * ver, size_t size) {

-	ERROR("Not implemented yet");
-	(void)dev;
-	(void)ver;
-	(void)size;
-	return -1;
+	char buf[2048];
+	struct mkii_message * msg;
+	int ret;
+
+	if ( ! ( dev->data & MKII_SUPPORT_SW_RELEASE ) )
+		return -1;
+
+	msg = (struct mkii_message *)buf;
+
+	memcpy(msg->data, "/version/sw_release", sizeof("/version/sw_release")-1);
+	ret = mkii_send_receive(dev->udev, MKII_GET, msg, sizeof("/version/sw_release")-1, msg, sizeof(buf));
+	if ( ret < 2 || msg->data[0] != 0 || msg->data[1] == 0 )
+		ERROR_RETURN("Cannot get sw release", -1);
+
+	msg->data[ret] = 0;
+	strncpy(ver, msg->data+1, size);
+	ver[size-1] = 0;
+	return strlen(ver);

 }

@@ -24,12 +24,15 @@
 #include "device.h"
 #include "usb-device.h"

+#define MKII_SUPPORT_SW_RELEASE	(1UL << 30)
+#define MKII_UPDATE_MODE	(1UL << 31)
+
 int mkii_init(struct usb_device_info * dev);

 enum device mkii_get_device(struct usb_device_info * dev);

 int mkii_flash_image(struct usb_device_info * dev, struct image * image);
-int mkii_reboot_device(struct usb_device_info * dev);
+int mkii_reboot_device(struct usb_device_info * dev, int update);

 int mkii_get_root_device(struct usb_device_info * dev);
 int mkii_set_root_device(struct usb_device_info * dev, int device);
@@ -22,8 +22,6 @@
 #include <stdlib.h>
 #include <arpa/inet.h>

-#include <usb.h>
-
 #include "nolo.h"
 #include "image.h"
 #include "global.h"
@@ -84,12 +82,14 @@ static void nolo_error_log(struct usb_device_info * dev, int only_clear) {

 	char buf[2048];
 	size_t i, count;
+	int ret;

 	for ( count = 0; count < 20; ++count ) {

 		memset(buf, 0, sizeof(buf));

-		if ( usb_control_msg(dev->udev, NOLO_QUERY, NOLO_ERROR_LOG, 0, 0, buf, sizeof(buf), 2000) <= 0 )
+		ret = libusb_control_transfer(dev->udev, NOLO_QUERY, NOLO_ERROR_LOG, 0, 0, (unsigned char *)buf, sizeof(buf), 2000);
+		if ( ret < 0 )
 			break;

 		if ( ! only_clear ) {
@@ -103,6 +103,9 @@ static void nolo_error_log(struct usb_device_info * dev, int only_clear) {

 		}

+		if ( (size_t)ret < sizeof(buf) )
+			break;
+
 	}

 }
@@ -115,11 +118,14 @@ static int nolo_identify_string(struct usb_device_info * dev, const char * str,

 	memset(buf, 0, sizeof(buf));

-	ret = usb_control_msg(dev->udev, NOLO_QUERY, NOLO_IDENTIFY, 0, 0, (char *)buf, sizeof(buf), 2000);
+	ret = libusb_control_transfer(dev->udev, NOLO_QUERY, NOLO_IDENTIFY, 0, 0, (unsigned char *)buf, sizeof(buf), 2000);
 	if ( ret < 0 )
 		NOLO_ERROR_RETURN("NOLO_IDENTIFY failed", -1);

-	ptr = memmem(buf, ret, str, strlen(str));
+	if ( (size_t)ret > sizeof(buf) )
+		ret = sizeof(buf);
+
+	ptr = MEMMEM(buf, ret, str, strlen(str));
 	if ( ! ptr )
 		ERROR_RETURN("Substring was not found", -1);

@@ -142,10 +148,10 @@ static int nolo_set_string(struct usb_device_info * dev, char * str, char * arg)
 	if ( simulate )
 		return 0;

-	if ( usb_control_msg(dev->udev, NOLO_WRITE, NOLO_STRING, 0, 0, str, strlen(str), 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_WRITE, NOLO_STRING, 0, 0, (unsigned char *)str, strlen(str), 2000) < 0 )
 		NOLO_ERROR_RETURN("NOLO_STRING failed", -1);

-	if ( usb_control_msg(dev->udev, NOLO_WRITE, NOLO_SET_STRING, 0, 0, arg, strlen(arg), 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_WRITE, NOLO_SET_STRING, 0, 0, (unsigned char *)arg, strlen(arg), 2000) < 0 )
 		NOLO_ERROR_RETURN("NOLO_SET_STRING failed", -1);

 	return 0;
@@ -156,12 +162,15 @@ static int nolo_get_string(struct usb_device_info * dev, char * str, char * out,

 	int ret = 0;

-	if ( usb_control_msg(dev->udev, NOLO_WRITE, NOLO_STRING, 0, 0, str, strlen(str), 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_WRITE, NOLO_STRING, 0, 0, (unsigned char *)str, strlen(str), 2000) < 0 )
 		return -1;

-	if ( ( ret = usb_control_msg(dev->udev, NOLO_QUERY, NOLO_GET_STRING, 0, 0, out, size-1, 2000) ) < 0 )
+	if ( ( ret = libusb_control_transfer(dev->udev, NOLO_QUERY, NOLO_GET_STRING, 0, 0, (unsigned char *)out, size-1, 2000) ) < 0 )
 		return -1;

+	if ( (size_t)ret > size-1 )
+		ret = size-1;
+
 	out[size-1] = 0;
 	out[ret] = 0;
 	return strlen(out);
@@ -176,14 +185,14 @@ static int nolo_get_version_string(struct usb_device_info * dev, const char * st
 	if ( strlen(str) > 500 )
 		return -1;

-	sprintf(buf, "version:%s", str);
+	if ( sprintf(buf, "version:%s", str) <= 0 )
+		return -1;

 	ret = nolo_get_string(dev, buf, out, size);
-
-	nolo_error_log(dev, 1);
-
-	if ( ret < 0 )
+	if ( ret < 0 ) {
+		nolo_error_log(dev, 1);
 		return ret;
+	}

 	if ( ! out[0] )
 		return -1;
@@ -200,7 +209,7 @@ int nolo_init(struct usb_device_info * dev) {
 	printf("Initializing NOLO...\n");

 	while ( val != 0 )
-		if ( usb_control_msg(dev->udev, NOLO_QUERY, NOLO_STATUS, 0, 0, (char *)&val, 4, 2000) == -1 )
+		if ( libusb_control_transfer(dev->udev, NOLO_QUERY, NOLO_STATUS, 0, 0, (unsigned char *)&val, 4, 2000) < 0 )
 			NOLO_ERROR_RETURN("NOLO_STATUS failed", -1);

 	/* clear error log */
@@ -242,7 +251,7 @@ static int nolo_send_image(struct usb_device_info * dev, struct image * image, i
 	uint16_t hash;
 	uint32_t size;
 	uint32_t need;
-	uint32_t readed;
+	uint32_t sent;
 	int request;
 	int ret;

@@ -260,14 +269,18 @@ static int nolo_send_image(struct usb_device_info * dev, struct image * image, i

 	ptr = buf;

-	/* Signature */
-	memcpy(ptr, "\x2E\x19\x01\x01", 4);
-	ptr += 4;
-
-	/* Space */
-	memcpy(ptr, "\x00", 1);
+	/* File data header */
+	memcpy(ptr, "\x2E", 1);
 	ptr += 1;

+	/* Length of file data */
+	memcpy(ptr, "\x19", 1);
+	ptr += 1;
+
+	/* File data (ignored by NOLO): asic index: APE (0x01), device type: NAND (0x01), device index: 0 */
+	memcpy(ptr, "\x01\x01\x00", 3);
+	ptr += 3;
+
 	/* Hash */
 	hash = htons(image->hash);
 	memcpy(ptr, &hash, 2);
@@ -286,7 +299,7 @@ static int nolo_send_image(struct usb_device_info * dev, struct image * image, i
 	memcpy(ptr, &size, 4);
 	ptr += 4;

-	/* Space */
+	/* Load address (ignored by NOLO) */
 	memcpy(ptr, "\x00\x00\x00\x00", 4);
 	ptr += 4;

@@ -311,11 +324,11 @@ static int nolo_send_image(struct usb_device_info * dev, struct image * image, i
 		if ( bufs ) {

 			memset(buf, 0, sizeof(buf));
-			snprintf(buf, 8, "%d", dev->hwrev);
+			snprintf(buf, 8+1, "%d", dev->hwrev);

 			for ( i = 0; bufs[i]; ++i ) {
 				len = ((uint8_t*)bufs[i])[0];
-				if ( memmem(bufs[i]+1, len, buf, strlen(buf)) )
+				if ( MEMMEM(bufs[i]+1, len, buf, strlen(buf)) )
 					break;
 			}

@@ -359,7 +372,7 @@ static int nolo_send_image(struct usb_device_info * dev, struct image * image, i
 	printf("Sending image header...\n");

 	if ( ! simulate ) {
-		if ( usb_control_msg(dev->udev, NOLO_WRITE, request, 0, 0, buf, ptr-buf, 2000) < 0 )
+		if ( libusb_control_transfer(dev->udev, NOLO_WRITE, request, 0, 0, (unsigned char *)buf, ptr-buf, 2000) < 0 )
 			NOLO_ERROR_RETURN("Sending image header failed", -1);
 	}

@@ -369,28 +382,28 @@ static int nolo_send_image(struct usb_device_info * dev, struct image * image, i
 		printf("Sending image...\n");
 	printf_progressbar(0, image->size);
 	image_seek(image, 0);
-	readed = 0;
-	while ( readed < image->size ) {
-		need = image->size - readed;
+	sent = 0;
+	while ( sent < image->size ) {
+		need = image->size - sent;
 		if ( need > sizeof(buf) )
 			need = sizeof(buf);
 		ret = image_read(image, buf, need);
 		if ( ret == 0 )
 			break;
 		if ( ! simulate ) {
-			if ( usb_bulk_write(dev->udev, 2, buf, ret, 5000) != ret ) {
+			if ( usb_bulk_send(dev->udev, USB_WRITE_DATA_EP, buf, ret, 5000) != ret ) {
 				PRINTF_END();
 				NOLO_ERROR_RETURN("Sending image failed", -1);
 			}
 		}
-		readed += ret;
-		printf_progressbar(readed, image->size);
+		sent += ret;
+		printf_progressbar(sent, image->size);
 	}

 	if ( flash ) {
 		printf("Finishing flashing...\n");
 		if ( ! simulate ) {
-			if ( usb_control_msg(dev->udev, NOLO_WRITE, NOLO_SEND_FLASH_FINISH, 0, 0, NULL, 0, 30000) < 0 )
+			if ( libusb_control_transfer(dev->udev, NOLO_WRITE, NOLO_SEND_FLASH_FINISH, 0, 0, NULL, 0, 30000) < 0 )
 				NOLO_ERROR_RETURN("Finishing failed", -1);
 		}
 	}
@@ -418,8 +431,8 @@ int nolo_flash_image(struct usb_device_info * dev, struct image * image) {
 	unsigned long long int part;
 	unsigned long long int total;
 	unsigned long long int last_total;
-	char status[20];
 	char buf[128];
+	char * ptr;

 	if ( image->type == IMAGE_ROOTFS )
 		flash = 1;
@@ -447,7 +460,7 @@ int nolo_flash_image(struct usb_device_info * dev, struct image * image) {
 		printf("Flashing image...\n");

 		if ( ! simulate ) {
-			if ( usb_control_msg(dev->udev, NOLO_WRITE, NOLO_FLASH_IMAGE, 0, index, NULL, 0, 10000) )
+			if ( libusb_control_transfer(dev->udev, NOLO_WRITE, NOLO_FLASH_IMAGE, 0, index, NULL, 0, 10000) )
 				NOLO_ERROR_RETURN("Flashing failed", -1);
 		}

@@ -463,7 +476,7 @@ int nolo_flash_image(struct usb_device_info * dev, struct image * image) {
 		if ( nolo_get_string(dev, "cmt:status", buf, sizeof(buf)) < 0 )
 			NOLO_ERROR_RETURN("cmt:status failed", -1);

-		if ( strncmp(buf, "idle", strlen("idle")) == 0 )
+		if ( strncmp(buf, "idle", sizeof("idle")-1) == 0 )
 			state = 4;
 		else
 			printf("Erasing CMT...\n");
@@ -475,7 +488,7 @@ int nolo_flash_image(struct usb_device_info * dev, struct image * image) {
 				NOLO_ERROR_RETURN("cmt:status failed", -1);
 			}

-			if ( strncmp(buf, "finished", strlen("finished")) == 0 ) {
+			if ( strncmp(buf, "finished", sizeof("finished")-1) == 0 ) {

 				if ( state <= 0 ) {
 					printf_progressbar(last_total, last_total);
@@ -490,18 +503,29 @@ int nolo_flash_image(struct usb_device_info * dev, struct image * image) {

 				state = 4;

+			} else if ( strncmp(buf, "error", sizeof("error")-1) == 0 ) {
+
+				PRINTF_ERROR_RETURN("cmt:status error", -1);
+
 			} else {

-				if ( sscanf(buf, "%s:%llu/%llu", status, &part, &total) != 3 )
+				ptr = strchr(buf, ':');
+				if ( ! ptr )
 					PRINTF_ERROR_RETURN("cmt:status unknown", -1);

-				if ( strcmp(status, "program") == 0 && state <= 0 ) {
+				*ptr = 0;
+				ptr++;
+
+				if ( sscanf(ptr, "%llu/%llu", &part, &total) != 2 )
+					PRINTF_ERROR_RETURN("cmt:status unknown", -1);
+
+				if ( strcmp(buf, "program") == 0 && state <= 0 ) {
 					printf_progressbar(last_total, last_total);
 					printf("Done\n");
 					state = 1;
 				}

-				if ( strcmp(status, "program") == 0 && state <= 1 ) {
+				if ( strcmp(buf, "program") == 0 && state <= 1 ) {
 					printf("Programming CMT...\n");
 					state = 2;
 				}
@@ -509,19 +533,19 @@ int nolo_flash_image(struct usb_device_info * dev, struct image * image) {
 				printf_progressbar(part, total);
 				last_total = total;

-				if ( strcmp(status, "erase") == 0 && state <= 0 && part == total ) {
+				if ( strcmp(buf, "erase") == 0 && state <= 0 && part == total ) {
 					printf("Done\n");
 					state = 1;
 				}

-				if ( strcmp(status, "program") == 0 && state <= 2 && part == total ) {
+				if ( strcmp(buf, "program") == 0 && state <= 2 && part == total ) {
 					printf("Done\n");
 					state = 3;
 				}

 			}

-			usleep(0xc350); // 0.5s
+			MSLEEP(50);

 		}

@@ -536,9 +560,9 @@ int nolo_boot_device(struct usb_device_info * dev, const char * cmdline) {
 	int size = 0;
 	int mode = NOLO_BOOT_MODE_NORMAL;

-	if ( cmdline && strncmp(cmdline, "update", strlen("update")) == 0 && cmdline[strlen("update")] <= 32 ) {
+	if ( cmdline && strncmp(cmdline, "update", sizeof("update")-1) == 0 && cmdline[sizeof("update")-1] <= 32 ) {
 		mode = NOLO_BOOT_MODE_UPDATE;
-		cmdline += strlen("update");
+		cmdline += sizeof("update")-1;
 		if ( *cmdline ) ++cmdline;
 		while ( *cmdline && *cmdline <= 32 )
 			++cmdline;
@@ -557,7 +581,7 @@ int nolo_boot_device(struct usb_device_info * dev, const char * cmdline) {
 		cmdline = NULL;
 	}

-	if ( usb_control_msg(dev->udev, NOLO_WRITE, NOLO_BOOT, mode, 0, (char *)cmdline, size, 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_WRITE, NOLO_BOOT, mode, 0, (unsigned char *)cmdline, size, 2000) < 0 )
 		NOLO_ERROR_RETURN("Booting failed", -1);

 	return 0;
@@ -567,7 +591,7 @@ int nolo_boot_device(struct usb_device_info * dev, const char * cmdline) {
 int nolo_reboot_device(struct usb_device_info * dev) {

 	printf("Rebooting device...\n");
-	if ( usb_control_msg(dev->udev, NOLO_WRITE, NOLO_REBOOT, 0, 0, NULL, 0, 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_WRITE, NOLO_REBOOT, 0, 0, NULL, 0, 2000) < 0 )
 		NOLO_ERROR_RETURN("NOLO_REBOOT failed", -1);
 	return 0;

@@ -576,7 +600,7 @@ int nolo_reboot_device(struct usb_device_info * dev) {
 int nolo_get_root_device(struct usb_device_info * dev) {

 	uint8_t device = 0;
-	if ( usb_control_msg(dev->udev, NOLO_QUERY, NOLO_GET, 0, NOLO_ROOT_DEVICE, (char *)&device, 1, 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_QUERY, NOLO_GET, 0, NOLO_ROOT_DEVICE, (unsigned char *)&device, 1, 2000) < 0 )
 		NOLO_ERROR_RETURN("Cannot get root device", -1);
 	return device;

@@ -587,7 +611,7 @@ int nolo_set_root_device(struct usb_device_info * dev, int device) {
 	printf("Setting root device to %d...\n", device);
 	if ( simulate )
 		return 0;
-	if ( usb_control_msg(dev->udev, NOLO_WRITE, NOLO_SET, device, NOLO_ROOT_DEVICE, NULL, 0, 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_WRITE, NOLO_SET, device, NOLO_ROOT_DEVICE, NULL, 0, 2000) < 0 )
 		NOLO_ERROR_RETURN("Cannot set root device", -1);
 	return 0;

@@ -596,7 +620,7 @@ int nolo_set_root_device(struct usb_device_info * dev, int device) {
 int nolo_get_usb_host_mode(struct usb_device_info * dev) {

 	uint32_t enabled = 0;
-	if ( usb_control_msg(dev->udev, NOLO_QUERY, NOLO_GET, 0, NOLO_USB_HOST_MODE, (void *)&enabled, 4, 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_QUERY, NOLO_GET, 0, NOLO_USB_HOST_MODE, (unsigned char *)&enabled, 4, 2000) < 0 )
 		NOLO_ERROR_RETURN("Cannot get USB host mode status", -1);
 	return enabled ? 1 : 0;

@@ -607,7 +631,7 @@ int nolo_set_usb_host_mode(struct usb_device_info * dev, int enable) {
 	printf("%s USB host mode...\n", enable ? "Enabling" : "Disabling");
 	if ( simulate )
 		return 0;
-	if ( usb_control_msg(dev->udev, NOLO_WRITE, NOLO_SET, enable, NOLO_USB_HOST_MODE, NULL, 0, 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_WRITE, NOLO_SET, enable, NOLO_USB_HOST_MODE, NULL, 0, 2000) < 0 )
 		NOLO_ERROR_RETURN("Cannot change USB host mode status", -1);
 	return 0;

@@ -616,7 +640,7 @@ int nolo_set_usb_host_mode(struct usb_device_info * dev, int enable) {
 int nolo_get_rd_mode(struct usb_device_info * dev) {

 	uint8_t enabled = 0;
-	if ( usb_control_msg(dev->udev, NOLO_QUERY, NOLO_GET, 0, NOLO_RD_MODE, (char *)&enabled, 1, 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_QUERY, NOLO_GET, 0, NOLO_RD_MODE, (unsigned char *)&enabled, 1, 2000) < 0 )
 		NOLO_ERROR_RETURN("Cannot get R&D mode status", -1);
 	return enabled ? 1 : 0;

@@ -627,7 +651,7 @@ int nolo_set_rd_mode(struct usb_device_info * dev, int enable) {
 	printf("%s R&D mode...\n", enable ? "Enabling" : "Disabling");
 	if ( simulate )
 		return 0;
-	if ( usb_control_msg(dev->udev, NOLO_WRITE, NOLO_SET, enable, NOLO_RD_MODE, NULL, 0, 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_WRITE, NOLO_SET, enable, NOLO_RD_MODE, NULL, 0, 2000) < 0 )
 		NOLO_ERROR_RETURN("Cannot change R&D mode status", -1);
 	return 0;

@@ -640,7 +664,7 @@ int nolo_get_rd_flags(struct usb_device_info * dev, char * flags, size_t size) {
 	uint16_t add_flags = 0;
 	char * ptr = flags;

-	if ( usb_control_msg(dev->udev, NOLO_QUERY, NOLO_GET, 0, NOLO_ADD_RD_FLAGS, (char *)&add_flags, 2, 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_QUERY, NOLO_GET, 0, NOLO_ADD_RD_FLAGS, (unsigned char *)&add_flags, 2, 2000) < 0 )
 		NOLO_ERROR_RETURN("Cannot get R&D flags", -1);

 	if ( add_flags & NOLO_RD_FLAG_NO_OMAP_WD )
@@ -740,10 +764,10 @@ int nolo_set_rd_flags(struct usb_device_info * dev, const char * flags) {
 	if ( simulate )
 		return 0;

-	if ( usb_control_msg(dev->udev, NOLO_WRITE, NOLO_SET, add_flags, NOLO_ADD_RD_FLAGS, NULL, 0, 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_WRITE, NOLO_SET, add_flags, NOLO_ADD_RD_FLAGS, NULL, 0, 2000) < 0 )
 		NOLO_ERROR_RETURN("Cannot add R&D flags", -1);

-	if ( usb_control_msg(dev->udev, NOLO_WRITE, NOLO_SET, del_flags, NOLO_DEL_RD_FLAGS, NULL, 0, 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_WRITE, NOLO_SET, del_flags, NOLO_DEL_RD_FLAGS, NULL, 0, 2000) < 0 )
 		NOLO_ERROR_RETURN("Cannot del R&D flags", -1);

 	return 0;
@@ -763,7 +787,7 @@ int nolo_set_hwrev(struct usb_device_info * dev, int16_t hwrev) {

 	char buf[9];
 	memset(buf, 0, sizeof(buf));
-	snprintf(buf, 8, "%d", hwrev);
+	snprintf(buf, sizeof(buf), "%d", hwrev);
 	printf("Setting HW revision to: %s\n", buf);
 	return nolo_set_string(dev, "hw_rev", buf);

@@ -803,7 +827,7 @@ int nolo_get_nolo_ver(struct usb_device_info * dev, char * ver, size_t size) {

 	uint32_t version = 0;

-	if ( usb_control_msg(dev->udev, NOLO_QUERY, NOLO_GET_NOLO_VERSION, 0, 0, (char *)&version, 4, 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_QUERY, NOLO_GET_NOLO_VERSION, 0, 0, (unsigned char *)&version, 4, 2000) < 0 )
 		NOLO_ERROR_RETURN("Cannot get NOLO version", -1);

 	if ( (version & 255) > 1 )
@@ -862,7 +886,7 @@ int nolo_set_sw_ver(struct usb_device_info * dev, const char * ver) {
 	memcpy(ptr, ver, len);
 	ptr += len;

-	if ( usb_control_msg(dev->udev, NOLO_WRITE, NOLO_SET_SW_RELEASE, 0, 0, buf, ptr-buf, 2000) < 0 )
+	if ( libusb_control_transfer(dev->udev, NOLO_WRITE, NOLO_SET_SW_RELEASE, 0, 0, (unsigned char *)buf, ptr-buf, 2000) < 0 )
 		NOLO_ERROR_RETURN("NOLO_SET_SW_RELEASE failed", -1);

 	return 0;
@@ -17,6 +17,8 @@

 */

+#include <stdlib.h>
+
 #include "global.h"
 #include "device.h"
 #include "usb-device.h"
@@ -80,7 +82,7 @@ struct device_info * dev_detect(void) {
 	}

 clean:
-	if (usb)
+	if ( usb )
 		usb_close_device(usb);
 	free(dev);
 	return NULL;
@@ -89,8 +91,11 @@ clean:

 void dev_free(struct device_info * dev) {

-	if ( dev->usb )
+	if ( dev->usb ) {
+		if ( dev->usb->flash_device->protocol == FLASH_DISK )
+			disk_exit(dev->usb);
 		usb_close_device(dev->usb);
+	}
 	free(dev);

 }
@@ -110,8 +115,8 @@ enum device dev_get_device(struct device_info * dev) {
 			return nolo_get_device(dev->usb);
 		else if ( protocol == FLASH_MKII )
 			return mkii_get_device(dev->usb);
-/*		else if ( protocol == FLASH_DISK )
-			return disk_get_device(dev->usb);*/
+		else if ( protocol == FLASH_DISK )
+			return disk_get_device(dev->usb);

 	}

@@ -174,15 +179,19 @@ int dev_flash_image(struct device_info * dev, struct image * image) {

 		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;

-		if ( protocol == FLASH_NOLO )
-			return nolo_flash_image(dev->usb, image);
-		else if ( protocol == FLASH_MKII )
-			return mkii_flash_image(dev->usb, image);
-		else {
-			usb_switch_to_nolo(dev->usb);
+		if ( protocol == FLASH_NOLO ) {
+			if ( image->type != IMAGE_MMC )
+				return nolo_flash_image(dev->usb, image);
+			usb_switch_to_update(dev->usb);
 			return -EAGAIN;
+		} else if ( protocol == FLASH_MKII ) {
+			if ( dev->usb->data & (1UL << image->type) )
+				return mkii_flash_image(dev->usb, image);
 		}

+		usb_switch_to_nolo(dev->usb);
+		return -EAGAIN;
+
 	}

 	return -1;
@@ -195,8 +204,15 @@ int dev_dump_image(struct device_info * dev, enum image_type image, const char *
 		return local_dump_image(image, file);

 	if ( dev->method == METHOD_USB ) {
-		ERROR("Dump image via USB is not supported");
+
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_DISK )
+			return disk_dump_image(dev->usb, image, file);
+
+		ERROR("Dump image via USB not in Mass Storage Mode is not supported");
 		return -1;
+
 	}

 	return -1;
@@ -230,10 +246,11 @@ int dev_boot_device(struct device_info * dev, const char * cmdline) {

 		if ( protocol == FLASH_NOLO )
 			return nolo_boot_device(dev->usb, cmdline);
-		else {
-			usb_switch_to_nolo(dev->usb);
-			return 0;
-		}
+		else if ( protocol == FLASH_MKII && cmdline && strcmp(cmdline, "update") == 0 )
+			return mkii_reboot_device(dev->usb, 1);
+
+		usb_switch_to_nolo(dev->usb);
+		return -EAGAIN;

 	}

@@ -255,10 +272,10 @@ int dev_reboot_device(struct device_info * dev) {
 		else if ( protocol == FLASH_NOLO )
 			return nolo_reboot_device(dev->usb);
 		else if ( protocol == FLASH_MKII )
-			return mkii_reboot_device(dev->usb);
+			return mkii_reboot_device(dev->usb, 0);
 		else {
-			ERROR("Rebooting device in RAW disk mode is not supported");
-			return -1;
+			usb_switch_to_nolo(dev->usb);
+			return -EAGAIN;
 		}

 	}
@@ -292,13 +309,13 @@ int dev_set_root_device(struct device_info * dev, int device) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_set_root_device(dev->usb, device);

-		if ( dev->usb->flash_device->protocol == FLASH_COLD ) {
-			usb_switch_to_nolo(dev->usb);
-			return -EAGAIN;
-		}
+		usb_switch_to_nolo(dev->usb);
+		return -EAGAIN;

 	}

@@ -313,7 +330,9 @@ int dev_get_usb_host_mode(struct device_info * dev) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_get_usb_host_mode(dev->usb);

 	}
@@ -329,13 +348,13 @@ int dev_set_usb_host_mode(struct device_info * dev, int enable) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_set_usb_host_mode(dev->usb, enable);

-		if ( dev->usb->flash_device->protocol == FLASH_COLD ) {
-			usb_switch_to_nolo(dev->usb);
-			return -EAGAIN;
-		}
+		usb_switch_to_nolo(dev->usb);
+		return -EAGAIN;

 	}

@@ -350,7 +369,9 @@ int dev_get_rd_mode(struct device_info * dev) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_get_rd_mode(dev->usb);

 	}
@@ -366,13 +387,13 @@ int dev_set_rd_mode(struct device_info * dev, int enable) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_set_rd_mode(dev->usb, enable);

-		if ( dev->usb->flash_device->protocol == FLASH_COLD ) {
-			usb_switch_to_nolo(dev->usb);
-			return -EAGAIN;
-		}
+		usb_switch_to_nolo(dev->usb);
+		return -EAGAIN;

 	}

@@ -387,7 +408,9 @@ int dev_get_rd_flags(struct device_info * dev, char * flags, size_t size) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_get_rd_flags(dev->usb, flags, size);

 	}
@@ -403,13 +426,13 @@ int dev_set_rd_flags(struct device_info * dev, const char * flags) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_set_rd_flags(dev->usb, flags);

-		if ( dev->usb->flash_device->protocol == FLASH_COLD ) {
-			usb_switch_to_nolo(dev->usb);
-			return -EAGAIN;
-		}
+		usb_switch_to_nolo(dev->usb);
+		return -EAGAIN;

 	}

@@ -424,8 +447,12 @@ int16_t dev_get_hwrev(struct device_info * dev) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_get_hwrev(dev->usb);
+		else if ( protocol == FLASH_MKII )
+			return mkii_get_hwrev(dev->usb);

 	}

@@ -440,13 +467,13 @@ int dev_set_hwrev(struct device_info * dev, int16_t hwrev) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_set_hwrev(dev->usb, hwrev);

-		if ( dev->usb->flash_device->protocol == FLASH_COLD ) {
-			usb_switch_to_nolo(dev->usb);
-			return -EAGAIN;
-		}
+		usb_switch_to_nolo(dev->usb);
+		return -EAGAIN;

 	}

@@ -461,7 +488,9 @@ int dev_get_kernel_ver(struct device_info * dev, char * ver, size_t size) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_get_kernel_ver(dev->usb, ver, size);

 	}
@@ -477,13 +506,13 @@ int dev_set_kernel_ver(struct device_info * dev, const char * ver) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_set_kernel_ver(dev->usb, ver);

-		if ( dev->usb->flash_device->protocol == FLASH_COLD ) {
-			usb_switch_to_nolo(dev->usb);
-			return -EAGAIN;
-		}
+		usb_switch_to_nolo(dev->usb);
+		return -EAGAIN;

 	}

@@ -498,7 +527,9 @@ int dev_get_initfs_ver(struct device_info * dev, char * ver, size_t size) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_get_initfs_ver(dev->usb, ver, size);

 	}
@@ -514,13 +545,13 @@ int dev_set_initfs_ver(struct device_info * dev, const char * ver) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_set_initfs_ver(dev->usb, ver);

-		if ( dev->usb->flash_device->protocol == FLASH_COLD ) {
-			usb_switch_to_nolo(dev->usb);
-			return -EAGAIN;
-		}
+		usb_switch_to_nolo(dev->usb);
+		return -EAGAIN;

 	}

@@ -535,7 +566,9 @@ int dev_get_nolo_ver(struct device_info * dev, char * ver, size_t size) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_get_nolo_ver(dev->usb, ver, size);

 	}
@@ -551,13 +584,13 @@ int dev_set_nolo_ver(struct device_info * dev, const char * ver) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_set_nolo_ver(dev->usb, ver);

-		if ( dev->usb->flash_device->protocol == FLASH_COLD ) {
-			usb_switch_to_nolo(dev->usb);
-			return -EAGAIN;
-		}
+		usb_switch_to_nolo(dev->usb);
+		return -EAGAIN;

 	}

@@ -572,8 +605,12 @@ int dev_get_sw_ver(struct device_info * dev, char * ver, size_t size) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_get_sw_ver(dev->usb, ver, size);
+		else if ( protocol == FLASH_MKII )
+			return mkii_get_sw_ver(dev->usb, ver, size);

 	}

@@ -588,13 +625,13 @@ int dev_set_sw_ver(struct device_info * dev, const char * ver) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_set_sw_ver(dev->usb, ver);

-		if ( dev->usb->flash_device->protocol == FLASH_COLD ) {
-			usb_switch_to_nolo(dev->usb);
-			return -EAGAIN;
-		}
+		usb_switch_to_nolo(dev->usb);
+		return -EAGAIN;

 	}

@@ -609,7 +646,9 @@ int dev_get_content_ver(struct device_info * dev, char * ver, size_t size) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_get_content_ver(dev->usb, ver, size);

 	}
@@ -625,13 +664,13 @@ int dev_set_content_ver(struct device_info * dev, const char * ver) {

 	if ( dev->method == METHOD_USB ) {

-		if ( dev->usb->flash_device->protocol == FLASH_NOLO )
+		enum usb_flash_protocol protocol = dev->usb->flash_device->protocol;
+
+		if ( protocol == FLASH_NOLO )
 			return nolo_set_content_ver(dev->usb, ver);

-		if ( dev->usb->flash_device->protocol == FLASH_COLD ) {
-			usb_switch_to_nolo(dev->usb);
-			return -EAGAIN;
-		}
+		usb_switch_to_nolo(dev->usb);
+		return -EAGAIN;

 	}

@@ -39,20 +39,20 @@ void printf_progressbar(unsigned long long part, unsigned long long total) {
 	int tmp, cols = 80;

 	/* percentage calculation */
-	pc = (int)(part*100/total);
-	(pc<0)?pc=0:(pc>100)?pc=100:0;
+	pc = total == 0 ? 100 : (int)(part*100/total);
+	( pc < 0 ) ? pc = 0 : ( pc > 100 ) ? pc = 100 : 0;

 	PRINTF_BACK();
 	PRINTF_ADD("\x1b[K  %3d%% [", pc);
-	if (columns)
+	if ( columns )
 		cols = atoi(columns);
-	if (cols > 115)
+	if ( cols > 115 )
 		cols = 115;
 	cols-=15;
-	for(tmp=cols*pc/100;tmp;tmp--) PRINTF_ADD("#");
-	for(tmp=cols-(cols*pc/100);tmp;tmp--) PRINTF_ADD("-");
+	for ( tmp = cols*pc/100; tmp; tmp-- ) PRINTF_ADD("#");
+	for ( tmp = cols-(cols*pc/100); tmp; tmp-- ) PRINTF_ADD("-");
 	PRINTF_ADD("]");
-	if (part == total) PRINTF_END();
+	if ( part == total ) PRINTF_END();
 	fflush(stdout);

 }
@@ -25,8 +25,6 @@
 #include <ctype.h>
 #include <signal.h>

-#include <usb.h>
-
 #include "global.h"
 #include "device.h"
 #include "usb-device.h"
@@ -35,20 +33,33 @@
 #include "cold-flash.h"
 #include "mkii.h"

+#define USB_ERROR(str, err) do { PRINTF_END(); ERROR("%s: %s", str, libusb_strerror(err)); } while (0)
+
+static libusb_context * usb_ctx;
+
 static struct usb_flash_device usb_devices[] = {
-	{ 0x0421, 0x0105,  2,  1, -1, FLASH_NOLO, { DEVICE_SU_18, DEVICE_RX_44, DEVICE_RX_48, DEVICE_RX_51, 0 } },
-	{ 0x0421, 0x0106,  0, -1, -1, FLASH_COLD, { DEVICE_RX_51, 0 } },
-/*	{ 0x0421, 0x01c7,  0, -1, -1, FLASH_DISK, { DEVICE_RX_51, 0 } }, */
-/*	{ 0x0421, 0x01c8,  1,  1, -1, FLASH_MKII, { DEVICE_RX_51, 0 } }, */
-/*	{ 0x0421, 0x0431,  0, -1, -1, FLASH_DISK, { DEVICE_SU_18, DEVICE_RX_34, 0 } }, */
-	{ 0x0421, 0x3f00,  2,  1, -1, FLASH_NOLO, { DEVICE_RX_34, 0 } },
+	/* vend    prod    i   a   protocol     devices */
+	{ 0x0421, 0x0096, -1, -1, FLASH_DISK, { DEVICE_RX_44, 0 } },
+	{ 0x0421, 0x0105,  2,  1, FLASH_NOLO, { DEVICE_SU_18, DEVICE_RX_34, DEVICE_RX_44, DEVICE_RX_48, DEVICE_RX_51, DEVICE_RM_680, DEVICE_RM_696, 0 } },
+	{ 0x0421, 0x0106,  0,  0, FLASH_COLD, { DEVICE_RX_51, DEVICE_RM_680, DEVICE_RM_696, 0 } },
+	{ 0x0421, 0x0189, -1, -1, FLASH_DISK, { DEVICE_RX_48, 0 } },
+	{ 0x0421, 0x01c7, -1, -1, FLASH_DISK, { DEVICE_RX_51, 0 } },
+	{ 0x0421, 0x01c8,  1,  1, FLASH_MKII, { DEVICE_RX_51, DEVICE_RM_680, 0 } },
+	{ 0x0421, 0x01c8,  1,  0, FLASH_UNKN, { DEVICE_RX_51, 0 } }, /* U-Boot mode (TTY via USB) */
+	{ 0x0421, 0x03d1, -1, -1, FLASH_DISK, { DEVICE_RM_680, 0 } },
+	{ 0x0421, 0x03d2,  1,  1, FLASH_MKII, { DEVICE_RM_680, 0 } },
+	{ 0x0421, 0x0431, -1, -1, FLASH_DISK, { DEVICE_SU_18, DEVICE_RX_34, 0 } },
+	{ 0x0421, 0x04c3, -1, -1, FLASH_DISK, { DEVICE_RX_34, 0 } },
+	{ 0x0421, 0x0518, -1, -1, FLASH_DISK, { DEVICE_RM_696, 0 } },
+	{ 0x0421, 0x0519, -1, -1, FLASH_UNKN, { DEVICE_RM_696, 0 } }, /* RNDIS/Ethernet mode */
+	{ 0x0421, 0x051a,  2,  1, FLASH_UNKN, { DEVICE_RM_696, 0 } }, /* Sync mode (ADL protocol) */
 };

 static const char * usb_flash_protocols[] = {
 	[FLASH_NOLO] = "NOLO",
 	[FLASH_COLD] = "Cold flashing",
-/*	[FLASH_MKII] = "Mk II protocol", */
-/*	[FLASH_DISK] = "RAW disk", */
+	[FLASH_MKII] = "Mk II protocol",
+	[FLASH_DISK] = "RAW disk",
 };

 const char * usb_flash_protocol_to_string(enum usb_flash_protocol protocol) {
@@ -73,7 +84,20 @@ static void usb_flash_device_info_print(const struct usb_flash_device * dev) {

 }

-static void usb_descriptor_info_print(usb_dev_handle * udev, struct usb_device * dev, char * product, size_t size) {
+static void usb_reattach_kernel_driver(libusb_device_handle * udev, int interface, int detached) {
+
+	if ( interface < 0 )
+		return;
+
+	libusb_release_interface(udev, interface);
+
+	/* Reattach only driver detached by us, on macOS reattaching re-enumerates (resets) device */
+	if ( detached )
+		libusb_attach_kernel_driver(udev, interface);
+
+}
+
+static void usb_descriptor_info_print(libusb_device_handle * udev, const struct libusb_device_descriptor * desc, char * product, size_t size) {

 	char buf[1024];
 	char buf2[1024];
@@ -82,7 +106,7 @@ static void usb_descriptor_info_print(usb_dev_handle * udev, struct usb_device *
 	int i;

 	memset(buf, 0, sizeof(buf));
-	usb_get_string_simple(udev, dev->descriptor.iProduct, buf, sizeof(buf));
+	libusb_get_string_descriptor_ascii(udev, desc->iProduct, (unsigned char *)buf, sizeof(buf));
 	PRINTF_LINE("USB device product string: %s", buf[0] ? buf : "(not detected)");
 	PRINTF_END();

@@ -91,12 +115,12 @@ static void usb_descriptor_info_print(usb_dev_handle * udev, struct usb_device *

 	memset(buf, 0, sizeof(buf));
 	memset(buf2, 0, sizeof(buf2));
-	ret = usb_get_string_simple(udev, dev->descriptor.iSerialNumber, buf, sizeof(buf));
+	ret = libusb_get_string_descriptor_ascii(udev, desc->iSerialNumber, (unsigned char *)buf, sizeof(buf));
 	if ( ! isalnum(buf[0]) )
 		buf[0] = 0;
 	for ( i = 0; i < ret; i+=2 ) {
 		sscanf(buf+i, "%2x", &x);
-		if ( x > 32 && x < 128 )
+		if ( x > 31 && x < 127 )
 			buf2[i/2] = x;
 		else {
 			buf2[0] = 0;
@@ -110,15 +134,51 @@ static void usb_descriptor_info_print(usb_dev_handle * udev, struct usb_device *

 }

-static struct usb_device_info * usb_device_is_valid(struct usb_device * dev) {
+static int usb_device_has_interface(libusb_device * dev, int interface, int alternate) {

-	int i;
+	struct libusb_config_descriptor * config;
+	int ret = 1;
+
+	if ( interface == -1 )
+		return 1;
+
+	if ( libusb_get_config_descriptor(dev, 0, &config) < 0 )
+		return 0;
+
+	if ( interface >= config->bNumInterfaces )
+		ret = 0;
+	else if ( alternate != -1 && alternate >= config->interface[interface].num_altsetting )
+		ret = 0;
+
+	libusb_free_config_descriptor(config);
+	return ret;
+
+}
+
+static struct usb_device_info * usb_device_is_valid(libusb_device * dev) {
+
+	size_t i;
+	int err;
+	int detached = 0;
 	char product[1024];
+	struct libusb_device_descriptor desc;
 	struct usb_device_info * ret = NULL;

-	for ( i = 0; usb_devices[i].vendor; ++i ) {
+	if ( libusb_get_device_descriptor(dev, &desc) < 0 )
+		return NULL;

-		if ( dev->descriptor.idVendor == usb_devices[i].vendor && dev->descriptor.idProduct == usb_devices[i].product ) {
+	for ( i = 0; i < sizeof(usb_devices)/sizeof(usb_devices[0]); ++i ) {
+
+		if ( desc.idVendor == usb_devices[i].vendor && desc.idProduct == usb_devices[i].product ) {
+
+			if ( usb_devices[i].protocol == FLASH_UNKN )
+				continue;
+
+			if ( desc.bNumConfigurations < 1 )
+				continue;
+
+			if ( ! usb_device_has_interface(dev, usb_devices[i].interface, usb_devices[i].alternate) )
+				continue;

 			printf("\b\b  ");
 			PRINTF_END();
@@ -127,44 +187,50 @@ static struct usb_device_info * usb_device_is_valid(struct usb_device * dev) {
 			PRINTF_END();

 			PRINTF_LINE("Opening USB...");
-			usb_dev_handle * udev = usb_open(dev);
-			if ( ! udev ) {
-				PRINTF_ERROR("usb_open failed");
+			libusb_device_handle * udev;
+			err = libusb_open(dev, &udev);
+			if ( err < 0 ) {
+				USB_ERROR("libusb_open failed", err);
 				fprintf(stderr, "\n");
 				return NULL;
 			}

-			usb_descriptor_info_print(udev, dev, product, sizeof(product));
+			memset(product, 0, sizeof(product));
+			usb_descriptor_info_print(udev, &desc, product, sizeof(product));

-#ifdef LIBUSB_HAS_DETACH_KERNEL_DRIVER_NP
-			PRINTF_LINE("Detaching kernel from USB interface...");
-			usb_detach_kernel_driver_np(udev, usb_devices[i].interface);
-#endif
+			if ( usb_devices[i].interface >= 0 ) {
+
+				if ( libusb_kernel_driver_active(udev, usb_devices[i].interface) == 1 ) {
+					PRINTF_LINE("Detaching kernel from USB interface...");
+					err = libusb_detach_kernel_driver(udev, usb_devices[i].interface);
+					if ( err == 0 )
+						detached = 1;
+					else {
+						PRINTF_END();
+						WARNING("Cannot detach kernel driver: %s", libusb_strerror(err));
+					}
+				}
+
+				PRINTF_LINE("Claiming USB interface...");
+				err = libusb_claim_interface(udev, usb_devices[i].interface);
+				if ( err < 0 ) {
+					USB_ERROR("libusb_claim_interface failed", err);
+					fprintf(stderr, "\n");
+					usb_reattach_kernel_driver(udev, usb_devices[i].interface, detached);
+					libusb_close(udev);
+					return NULL;
+				}

-			PRINTF_LINE("Claiming USB interface...");
-			if ( usb_claim_interface(udev, usb_devices[i].interface) < 0 ) {
-				PRINTF_ERROR("usb_claim_interface failed");
-				fprintf(stderr, "\n");
-				usb_close(udev);
-				return NULL;
 			}

 			if ( usb_devices[i].alternate >= 0 ) {
 				PRINTF_LINE("Setting alternate USB interface...");
-				if ( usb_set_altinterface(udev, usb_devices[i].alternate) < 0 ) {
-					PRINTF_ERROR("usb_claim_interface failed");
+				err = libusb_set_interface_alt_setting(udev, usb_devices[i].interface, usb_devices[i].alternate);
+				if ( err < 0 ) {
+					USB_ERROR("libusb_set_interface_alt_setting failed", err);
 					fprintf(stderr, "\n");
-					usb_close(udev);
-					return NULL;
-				}
-			}
-
-			if ( usb_devices[i].configuration >= 0 ) {
-				PRINTF_LINE("Setting USB configuration...");
-				if ( usb_set_configuration(udev, usb_devices[i].configuration) < 0 ) {
-					PRINTF_ERROR("usb_set_configuration failed");
-					fprintf(stderr, "\n");
-					usb_close(udev);
+					usb_reattach_kernel_driver(udev, usb_devices[i].interface, detached);
+					libusb_close(udev);
 					return NULL;
 				}
 			}
@@ -172,24 +238,46 @@ static struct usb_device_info * usb_device_is_valid(struct usb_device * dev) {
 			ret = calloc(1, sizeof(struct usb_device_info));
 			if ( ! ret ) {
 				ALLOC_ERROR();
-				usb_close(udev);
+				usb_reattach_kernel_driver(udev, usb_devices[i].interface, detached);
+				libusb_close(udev);
 				return NULL;
 			}

-			if ( strstr(product, "N900") )
+			if ( strcmp(product, "Nokia 770") == 0 || strcmp(product, "Nokia 770 (Update mode)") == 0 )
+				ret->device = DEVICE_SU_18;
+			else if ( strcmp(product, "Nokia N800 Internet Tablet") == 0 || strcmp(product, "Nokia N800 (Update mode)") == 0 )
+				ret->device = DEVICE_RX_34;
+			else if ( strcmp(product, "Nokia N810 Internet Tablet") == 0 || strcmp(product, "Nokia N810 (Update mode)") == 0 )
+				ret->device = DEVICE_RX_44;
+			else if ( strcmp(product, "Nokia N810 Internet Tablet WiMAX Edition") == 0 || strcmp(product, "Nokia-RX48 (Update mode)") == 0 )
+				ret->device = DEVICE_RX_48;
+			else if ( strcmp(product, "N900 (Storage Mode)") == 0 || strcmp(product, "Nokia N900 (Update mode)") == 0 || strcmp(product, "N900 (PC-Suite Mode)") == 0 || strcmp(product, "N900 (U-Boot)") == 0 )
 				ret->device = DEVICE_RX_51;
+			else if ( strcmp(product, "Nokia N950") == 0 || strcmp(product, "N950 (Update mode)") == 0 )
+				ret->device = DEVICE_RM_680;
+			else if ( strcmp(product, "Nokia N9") == 0 || strcmp(product, "N9 (Update mode)") == 0 || strcmp(product, "Nokia N9 RNDIS/Ethernet") == 0 )
+				ret->device = DEVICE_RM_696;
+			else if ( strcmp(product, "Nokia USB ROM") == 0 || strcmp(product, "Sync Mode") == 0 || strcmp(product, "Nxy (Update mode)") == 0 )
+				ret->device = DEVICE_ANY;
 			else
 				ret->device = DEVICE_UNKNOWN;

-			/* TODO: Autodetect more devices */
-
 			if ( device_to_string(ret->device) )
 				PRINTF_LINE("Detected USB device: %s", device_to_string(ret->device));
 			else
 				PRINTF_LINE("Detected USB device: (not detected)");
 			PRINTF_END();

-			if ( ret->device ) {
+			if ( ! noverify && ret->device == DEVICE_UNKNOWN ) {
+				ERROR("Device detection failed");
+				fprintf(stderr, "\n");
+				usb_reattach_kernel_driver(udev, usb_devices[i].interface, detached);
+				libusb_close(udev);
+				free(ret);
+				return NULL;
+			}
+
+			if ( ! noverify && ret->device != DEVICE_ANY ) {
 				enum device * device;
 				for ( device = usb_devices[i].devices; *device; ++device )
 					if ( *device == ret->device )
@@ -197,7 +285,9 @@ static struct usb_device_info * usb_device_is_valid(struct usb_device * dev) {
 				if ( ! *device ) {
 					ERROR("Device mishmash");
 					fprintf(stderr, "\n");
-					usb_close(udev);
+					usb_reattach_kernel_driver(udev, usb_devices[i].interface, detached);
+					libusb_close(udev);
+					free(ret);
 					return NULL;
 				}
 			}
@@ -205,6 +295,7 @@ static struct usb_device_info * usb_device_is_valid(struct usb_device * dev) {
 			ret->hwrev = -1;
 			ret->flash_device = &usb_devices[i];
 			ret->udev = udev;
+			ret->detached = detached;
 			break;
 		}
 	}
@@ -213,28 +304,6 @@ static struct usb_device_info * usb_device_is_valid(struct usb_device * dev) {

 }

-static struct usb_device_info * usb_search_device(struct usb_device * dev, int level) {
-
-	int i;
-	struct usb_device_info * ret = NULL;
-
-	if ( ! dev )
-		return NULL;
-
-	ret = usb_device_is_valid(dev);
-	if ( ret )
-		return ret;
-
-	for ( i = 0; i < dev->num_children; i++ ) {
-		ret = usb_search_device(dev->children[i], level + 1);
-		if ( ret )
-			break;
-	}
-
-	return ret;
-
-}
-
 static volatile sig_atomic_t signal_quit;

 static void signal_handler(int signum) {
@@ -244,16 +313,33 @@ static void signal_handler(int signum) {

 }

+static void usb_exit(void) {
+
+	libusb_exit(usb_ctx);
+	usb_ctx = NULL;
+
+}
+
 struct usb_device_info * usb_open_and_wait_for_device(void) {

-	struct usb_bus * bus;
+	libusb_device ** devs;
+	ssize_t count;
+	ssize_t j;
 	struct usb_device_info * ret = NULL;
 	int i = 0;
+	int err;
 	void (*prev)(int);
 	static char progress[] = {'/','-','\\', '|'};

-	usb_init();
-	usb_find_busses();
+	/* Context is kept until exit, so device list stays up to date between waits */
+	if ( ! usb_ctx ) {
+		err = libusb_init(&usb_ctx);
+		if ( err < 0 ) {
+			USB_ERROR("libusb_init failed", err);
+			return NULL;
+		}
+		atexit(usb_exit);
+	}

 	PRINTF_BACK();
 	printf("\n");
@@ -266,30 +352,25 @@ struct usb_device_info * usb_open_and_wait_for_device(void) {

 		PRINTF_LINE("Waiting for USB device... %c", progress[++i%sizeof(progress)]);

-		usb_find_devices();
-
-		for ( bus = usb_get_busses(); bus; bus = bus->next ) {
-
-			if ( bus->root_dev )
-				ret = usb_search_device(bus->root_dev, 0);
-			else {
-				struct usb_device *dev;
-				for ( dev = bus->devices; dev; dev = dev->next ) {
-					ret = usb_search_device(dev, 0);
-					if ( ret )
-						break;
-				}
-			}
+		count = libusb_get_device_list(usb_ctx, &devs);
+		if ( count < 0 ) {
+			USB_ERROR("libusb_get_device_list failed", (int)count);
+			break;
+		}

+		for ( j = 0; j < count; ++j ) {
+			ret = usb_device_is_valid(devs[j]);
 			if ( ret )
 				break;
-
 		}

+		/* Opened device handle holds its own reference */
+		libusb_free_device_list(devs, 1);
+
 		if ( ret )
 			break;

-		usleep(0xc350); // 0.5s
+		MSLEEP(50);

 	}

@@ -308,11 +389,66 @@ struct usb_device_info * usb_open_and_wait_for_device(void) {

 void usb_close_device(struct usb_device_info * dev) {

-	usb_close(dev->udev);
+	if ( dev->flash_device->protocol != FLASH_COLD )
+		usb_reattach_kernel_driver(dev->udev, dev->flash_device->interface, dev->detached);
+	libusb_close(dev->udev);
 	free(dev);

 }

+/* Translate libusb error, so error messages which print errno stay meaningful */
+static int usb_error_to_errno(int err) {
+
+	switch ( err ) {
+	case LIBUSB_ERROR_INVALID_PARAM: return EINVAL;
+	case LIBUSB_ERROR_ACCESS: return EACCES;
+	case LIBUSB_ERROR_NO_DEVICE: return ENODEV;
+	case LIBUSB_ERROR_NOT_FOUND: return ENOENT;
+	case LIBUSB_ERROR_BUSY: return EBUSY;
+	case LIBUSB_ERROR_TIMEOUT: return ETIMEDOUT;
+	case LIBUSB_ERROR_OVERFLOW: return EOVERFLOW;
+	case LIBUSB_ERROR_PIPE: return EPIPE;
+	case LIBUSB_ERROR_INTERRUPTED: return EINTR;
+	case LIBUSB_ERROR_NO_MEM: return ENOMEM;
+	case LIBUSB_ERROR_NOT_SUPPORTED: return ENOTSUP;
+	default: return EIO;
+	}
+
+}
+
+/* Returns number of transferred bytes or negative libusb error code */
+static int usb_bulk_transfer(libusb_device_handle * udev, unsigned char endpoint, unsigned char * data, int size, unsigned int timeout) {
+
+	int transferred = 0;
+	int ret;
+
+	ret = libusb_bulk_transfer(udev, endpoint, data, size, &transferred, timeout);
+	if ( ret == 0 )
+		return transferred;
+
+	errno = usb_error_to_errno(ret);
+
+	/* Timed out transfer which moved some data is reported as short transfer, callers check its size */
+	if ( ret == LIBUSB_ERROR_TIMEOUT && transferred > 0 )
+		return transferred;
+
+	return ret;
+
+}
+
+int usb_bulk_send(libusb_device_handle * udev, unsigned char endpoint, const void * data, int size, unsigned int timeout) {
+
+	/* libusb does not modify buffer of OUT transfer */
+	return usb_bulk_transfer(udev, endpoint, (unsigned char *)data, size, timeout);
+
+}
+
+int usb_bulk_recv(libusb_device_handle * udev, unsigned char endpoint, void * data, int size, unsigned int timeout) {
+
+	return usb_bulk_transfer(udev, endpoint, data, size, timeout);
+
+}
+
 void usb_switch_to_nolo(struct usb_device_info * dev) {

 	printf("\nSwitching to NOLO mode...\n");
@@ -320,7 +456,7 @@ void usb_switch_to_nolo(struct usb_device_info * dev) {
 	if ( dev->flash_device->protocol == FLASH_COLD )
 		leave_cold_flash(dev);
 	else if ( dev->flash_device->protocol == FLASH_MKII )
-		mkii_reboot_device(dev);
+		mkii_reboot_device(dev, 0);
 	else if ( dev->flash_device->protocol == FLASH_DISK )
 		printf_and_wait("Unplug USB cable, turn device off, press ENTER and plug USB cable again");

@@ -333,13 +469,13 @@ void usb_switch_to_cold(struct usb_device_info * dev) {
 	if ( dev->flash_device->protocol == FLASH_NOLO )
 		nolo_reboot_device(dev);
 	else if ( dev->flash_device->protocol == FLASH_MKII )
-		mkii_reboot_device(dev);
+		mkii_reboot_device(dev, 0);
 	else if ( dev->flash_device->protocol == FLASH_DISK )
 		printf_and_wait("Unplug USB cable, turn device off, press ENTER and plug USB cable again");

 }

-void usb_switch_to_mkii(struct usb_device_info * dev) {
+void usb_switch_to_update(struct usb_device_info * dev) {

 	printf("\nSwitching to Update mode...\n");

@@ -347,6 +483,8 @@ void usb_switch_to_mkii(struct usb_device_info * dev) {
 		leave_cold_flash(dev);
 	else if ( dev->flash_device->protocol == FLASH_NOLO )
 		nolo_boot_device(dev, "update");
+	else if ( dev->flash_device->protocol == FLASH_MKII && ! ( dev->data & MKII_UPDATE_MODE ) )
+		mkii_reboot_device(dev, 1);
 	else if ( dev->flash_device->protocol == FLASH_DISK )
 		printf_and_wait("Unplug USB cable, turn device off, press ENTER and plug USB cable again");

@@ -361,7 +499,11 @@ void usb_switch_to_disk(struct usb_device_info * dev) {
 	else if ( dev->flash_device->protocol == FLASH_NOLO ) {
 		nolo_boot_device(dev, NULL);
 		printf_and_wait("Wait until device start, choose USB Mass Storage Mode and press ENTER");
-	} else if ( dev->flash_device->protocol == FLASH_MKII )
-		mkii_reboot_device(dev);
+	} else if ( dev->flash_device->protocol == FLASH_MKII ) {
+		if ( dev->data & MKII_UPDATE_MODE )
+			mkii_reboot_device(dev, 0);
+		else
+			printf_and_wait("Unplug USB cable, plug again, choose USB Mass Storage Mode and press ENTER");
+	}

 }
@@ -20,7 +20,13 @@
 #ifndef USB_DEVICE_H
 #define USB_DEVICE_H

-#include <usb.h>
+#include <stdint.h>
+
+#include <libusb.h>
+
+#define USB_READ_EP		(LIBUSB_ENDPOINT_IN | 0x1)
+#define USB_WRITE_EP		(LIBUSB_ENDPOINT_OUT | 0x1)
+#define USB_WRITE_DATA_EP	(LIBUSB_ENDPOINT_OUT | 0x2)

 #include "device.h"

@@ -38,7 +44,6 @@ struct usb_flash_device {
 	uint16_t product;
 	int interface;
 	int alternate;
-	int configuration;
 	enum usb_flash_protocol protocol;
 	enum device devices[DEVICE_COUNT];
 };
@@ -47,16 +52,21 @@ struct usb_device_info {
 	enum device device;
 	int16_t hwrev;
 	const struct usb_flash_device * flash_device;
-	usb_dev_handle * udev;
+	libusb_device_handle * udev;
+	int detached;
+	int data;
 };

 const char * usb_flash_protocol_to_string(enum usb_flash_protocol protocol);
 struct usb_device_info * usb_open_and_wait_for_device(void);
 void usb_close_device(struct usb_device_info * dev);

+int usb_bulk_send(libusb_device_handle * udev, unsigned char endpoint, const void * data, int size, unsigned int timeout);
+int usb_bulk_recv(libusb_device_handle * udev, unsigned char endpoint, void * data, int size, unsigned int timeout);
+
 void usb_switch_to_nolo(struct usb_device_info * dev);
 void usb_switch_to_cold(struct usb_device_info * dev);
-void usb_switch_to_mkii(struct usb_device_info * dev);
+void usb_switch_to_update(struct usb_device_info * dev);
 void usb_switch_to_disk(struct usb_device_info * dev);

 #endif