Merge branch 'master' into maemo

28 files changed, 1461 insertions, 481 deletions

diff --git a/.travis.yml b/.travis.yml
index 3e46a37..b623f9a 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -1,10 +1,16 @@
 language: c
 compiler:
   - gcc
+  - clang
 before_install:
   - sudo apt-get update -qq
   - sudo apt-get install -qq -y libusb-dev
-script: if [ ${COVERITY_SCAN_BRANCH} != 1 ]; then make ; fi
+script: if [ "${COVERITY_SCAN_BRANCH}" != 1 ]; then make ; fi
+
+matrix:
+  exclude:
+    - compiler: clang
+    - env: COVERITY_SCAN_BRANCH=1
 
 env:
   global:
@@ -16,5 +22,5 @@ addons:
       name: "pali/0xFFFF"
       description: "Build submitted via Travis CI"
     notification_email: pali.rohar@gmail.com
-    build_command: "make"
+    build_command: make
     branch_pattern: master
diff --git a/README b/README
index 8bbecca..76367fa 100644
--- a/README
+++ b/README
@@ -29,10 +29,7 @@ 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.
 
-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<---------------------------------------------------------------
 
diff --git a/TODO b/TODO
index 915a005..292b372 100644
--- a/TODO
+++ b/TODO
@@ -4,11 +4,9 @@ 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)
+ * Support for flashing mmc images
+ * Badblock checking
 
 fiasco:
  * Support for Harmattan images
@@ -18,17 +16,11 @@ image:
 
 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
diff --git a/config.mk b/config.mk
index 822eb02..1d5f3ad 100644
--- a/config.mk
+++ b/config.mk
@@ -1,4 +1,4 @@
-VERSION = 0.6.1
+VERSION = 0.7
 PREFIX = /usr/local
 
 # NetBSD stuff
diff --git a/doc/dumping b/doc/dumping
index 2d7ce25..4a629cd 100644
--- a/doc/dumping
+++ b/doc/dumping
@@ -30,10 +30,9 @@ 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 -o -b -s 0x00000800 -l 0x001FF800 -f zImage /dev/mtd2
 
 Params means:
--i - "Ignore errors"
 -o - "Omit oob data"
 -b - "Omit bad blocks"
 -s - "Start address"
diff --git a/doc/fiasco b/doc/fiasco
index f7462dd..941a2da 100644
--- a/doc/fiasco
+++ b/doc/fiasco
@@ -37,12 +37,16 @@ FW HEADER
 IMAGE
 
    1 byte = 0x54    --  signature
-   1 byte           --  number of subsection blocks + 1
-   5 bytes          --  unknown (always 0x2e 0x19 0x01 0x01 0x00)
+   1 byte           --  number of subsection blocks
+                        (start of data block)
+   1 byte           --  type of subsection block (data - always 0x2e)
+   1 byte           --  length of subsection block (data - always 25)
+   3 bytes          --  unknown (always 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)
+                        (end of data block)
              block {
    1 byte           --  type of subsection block
                           '1' - version
diff --git a/doc/mkii b/doc/mkii
index f18f5b3..0d3ecc2 100644
--- a/doc/mkii
+++ b/doc/mkii
@@ -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)
+
+   ...
diff --git a/src/Makefile b/src/Makefile
index b436ee6..be4ade6 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -3,13 +3,15 @@ include ../config.mk
 PREFIX ?= /usr/local
 INSTALL ?= install
 
+BUILD_DATE ?= $(shell LC_ALL=C date '+%b %e %Y')
+
 CC = gcc
 CROSS_CC = $(CROSS_COMPILE)$(CC)
 HOST_CC = $(HOST_COMPILE)$(CC)
 
-CPPFLAGS += -DVERSION=\"$(VERSION)\" -D_GNU_SOURCE
+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 += -lusb -ldl
 
 DEPENDS = Makefile ../config.mk
 
diff --git a/src/cal.c b/src/cal.c
index 3a46abd..4d07e5e 100644
--- a/src/cal.c
+++ b/src/cal.c
@@ -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,7 +70,11 @@ 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;
+#else
+	off_t lsize = 0;
+#endif
 
 	if ( stat(file, &st) != 0 )
 		return -1;
@@ -81,17 +87,34 @@ int cal_init_file(const char * file, struct cal ** cal_out) {
 	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;
 
 }
diff --git a/src/cold-flash.c b/src/cold-flash.c
index 8874b32..6338670 100644
--- a/src/cold-flash.c
+++ b/src/cold-flash.c
@@ -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;
@@ -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;
 		}
 	}
 
@@ -173,7 +172,7 @@ static int read_asic(usb_dev_handle * udev, uint8_t * asic_buffer, int size, int
 	int ret;
 
 	printf("Waiting for ASIC ID...\n");
-	ret = usb_bulk_read(udev, READ_DEV, (char *)asic_buffer, size, READ_TIMEOUT);
+	ret = usb_bulk_read(udev, USB_READ_EP, (char *)asic_buffer, size, READ_TIMEOUT);
 	if ( ret != asic_size )
 		ERROR_RETURN("Invalid size of ASIC ID", -1);
 
@@ -184,39 +183,41 @@ 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) {
 
 	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_write(udev, USB_WRITE_EP, (char *)&omap_peripheral_msg, sizeof(omap_peripheral_msg), WRITE_TIMEOUT);
 	if ( ret != sizeof(omap_peripheral_msg) )
 		ERROR_RETURN("Sending OMAP peripheral boot message failed", -1);
 
+	SLEEP(5000);
+
 	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_write(udev, USB_WRITE_EP, (char *)&image->size, 4, WRITE_TIMEOUT);
 	if ( ret != 4 )
 		ERROR_RETURN("Sending 2nd X-Loader image size failed", -1);
 
+	SLEEP(5000);
+
 	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_write(udev, USB_WRITE_EP, (char *)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);
 
+	SLEEP(50000);
 	return 0;
 
 }
@@ -225,42 +226,42 @@ static int send_secondary(usb_dev_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_write(udev, USB_WRITE_EP, (char *)&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 */
+	SLEEP(5000);
+	ret = usb_bulk_read(udev, USB_READ_EP, (char *)&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_write(udev, USB_WRITE_EP, (char *)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 */
+	SLEEP(5000);
+	ret = usb_bulk_read(udev, USB_READ_EP, (char *)&buffer, 4, READ_TIMEOUT); /* 4 bytes - dummy value */
 	if ( ret != 4 )
 		ERROR_RETURN("No response", -1);
 
@@ -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_write(udev, USB_WRITE_EP, (char *)&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_read(udev, USB_READ_EP, (char *)&ping_read, sizeof(ping_read), READ_TIMEOUT);
 			if ( ret == sizeof(ping_read) ) {
 				printf("Got it\n");
 				pong = 1;
 				break;
 			}
 
-			usleep(5000);
+			SLEEP(5000);
 			--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_write(dev->udev, USB_WRITE_EP, (char *)&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);
+	SLEEP(1000000);
 	return 0;
 
 }
diff --git a/src/device.c b/src/device.c
index 2ac5cb6..db2c6a7 100644
--- a/src/device.c
+++ b/src/device.c
@@ -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) {
diff --git a/src/device.h b/src/device.h
index 4ea3f0f..8d38e39 100644
--- a/src/device.h
+++ b/src/device.h
@@ -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,
 };
 
diff --git a/src/disk.c b/src/disk.c
index eb63211..6bdd974 100644
--- a/src/disk.c
+++ b/src/disk.c
@@ -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,186 @@
 #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;
+	size_t len;
+	char blkdev[1024];
+
+	dir = opendir("/dev/");
+	if ( ! dir ) {
+		ERROR_INFO("Cannot open '/dev/' directory");
+		return -1;
+	}
 
-enum device disk_get_device(struct usb_device_info * dev) {
+	found = 0;
 
-	ERROR("Not implemented yet");
-	(void)dev;
-	return DEVICE_UNKNOWN;
+	while ( ( dirent = readdir(dir) ) ) {
 
-}
+		if ( strncmp(dirent->d_name, ".", sizeof(".")) == 0 || strncmp(dirent->d_name, "..", sizeof("..")) == 0 )
+			continue;
 
-int disk_flash_raw(const char * blkdev, const char * file) {
+		if ( snprintf(blkdev, sizeof(blkdev), "/dev/%s", dirent->d_name) <= 0 )
+			continue;
 
-	ERROR("Not implemented yet");
-	(void)blkdev;
-	(void)file;
-	return -1;
+		if ( stat(blkdev, &st) != 0 )
+			continue;
 
-}
+		if ( ! S_ISBLK(st.st_mode) )
+			continue;
 
-int disk_flash_image(struct usb_device_info * dev, struct image * image) {
+		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;
+
+	} else if ( partition > 0 ) {
+
+		/* Select partition */
+
+		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 || ! S_ISBLK(st.st_mode) ) {
+			memcpy(blkdev+len, "1", 2);
+			if ( stat(blkdev, &st) != 0 || ! S_ISBLK(st.st_mode) ) {
+				blkdev[len] = 0;
+				fd = open(blkdev, O_RDONLY);
+				if ( fd < 0 ) {
+					if ( errno != ENOMEDIUM ) {
+						ERROR_INFO("Cannot open block device %s", blkdev);
+						return -1;
+					}
+				} else {
+					close(fd);
+					ERROR("Block device does not have partitions");
+					return -1;
+				}
+			}
+		}
+
+		printf("Found block device %s for partition %d\n", blkdev, partition);
+
+	}
+
+	fd = open(blkdev, (readonly ? O_RDONLY : O_RDWR) | O_EXCL);
+
+	if ( fd < 0 ) {
+		if ( errno != ENOMEDIUM )
+			ERROR_INFO("Cannot open block device %s", blkdev);
+		return -1;
+	}
+
+	return fd;
+
+#else
 
 	ERROR("Not implemented yet");
-	(void)dev;
-	(void)image;
+	(void)min;
+	(void)maj;
+	(void)partition;
+	(void)readonly;
 	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;
 	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);
-		return -1;
-	}
+#ifdef __linux__
 
-	if ( ! S_ISBLK(st.st_mode) ) {
-		ERROR("Invalid block device %s", blkdev);
+	if ( ioctl(fd, BLKGETSIZE64, &blksize) != 0 ) {
+		ERROR_INFO("Cannot get size of block device");
 		return -1;
 	}
 
-	fd1 = open(blkdev, O_RDONLY);
+#else
 
-	if ( fd1 < 0 ) {
-		ERROR_INFO("Cannot open block device %s", blkdev);
+	blksize = lseek(fd, 0, SEEK_END);
+	if ( blksize == (off_t)-1 ) {
+		ERROR_INFO("Cannot get size of block device");
 		return -1;
 	}
 
-	if ( ioctl(fd1, BLKGETSIZE64, &blksize) != 0 ) {
-		ERROR_INFO("Cannot get size of block device %s", blkdev);
-		close(fd1);
+	if ( lseek(fd, 0, SEEK_SET) == (off_t)-1 ) {
+		ERROR_INFO("Cannot seek to begin of block device");
 		return -1;
 	}
 
+#endif
+
 	if ( blksize > ULLONG_MAX ) {
-		ERROR("Block device %s is too big", blkdev);
-		close(fd1);
+		ERROR("Block device is too big");
 		return -1;
 	}
 
 	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,7 +229,6 @@ 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;
 	}
 
@@ -138,46 +236,188 @@ int disk_dump_raw(const char * blkdev, const char * file) {
 
 	if ( fd2 < 0 ) {
 		ERROR_INFO("Cannot create file %s", file);
-		close(fd1);
 		return -1;
 	}
 
-	readed = 0;
+	sent = 0;
 	printf_progressbar(0, blksize);
 
-	while ( readed < blksize ) {
-		need = blksize - readed;
+	while ( sent < blksize ) {
+		need = blksize - sent;
 		if ( need > sizeof(global_buf) )
 			need = sizeof(global_buf);
-		size = read(fd1, global_buf, need);
+		size = read(fd, global_buf, need);
 		if ( size == 0 )
 			break;
 		if ( write(fd2, global_buf, size) != size ) {
 			PRINTF_ERROR("Dumping image failed");
-			close(fd1);
 			close(fd2);
 			return -1;
 		}
-		readed += size;
-		printf_progressbar(readed, blksize);
+		sent += size;
+		printf_progressbar(sent, blksize);
 	}
 
-	close(fd1);
 	close(fd2);
 	return 0;
 
 }
 
-int disk_dump_image(struct usb_device_info * dev, enum image_type image, const char * file) {
+int disk_flash_dev(int fd, const char * file) {
+
+	ERROR("Not implemented yet");
+	(void)fd;
+	(void)file;
+	return -1;
+
+}
+
+int disk_init(struct usb_device_info * dev) {
+
+#ifdef __linux__
+
+	int fd;
+	int maj1;
+	int maj2;
+	int min1;
+	int min2;
+
+	maj1 = -1;
+	maj2 = -1;
+	min1 = -1;
+	min2 = -1;
+
+	FILE * f;
+	DIR * dir;
+	struct dirent * dirent;
+	char buf[1024];
+	unsigned int devnum;
+	unsigned int busnum;
+
+	struct usb_device * device;
+
+	device = usb_device(dev->udev);
+	if ( ! device || ! device->bus ) {
+		ERROR_INFO("Cannot read usb devnum and busnum");
+		return -1;
+	}
+
+	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 ( devnum != device->devnum || device->bus->location != 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;
+	}
+
+	/* TODO: change 1 to 0 when disk_flash_dev will be implemented */
+
+	/* RX-51 and RM-680 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 )
+		fd = disk_open_dev(maj1, min1, -1, 1);
+	/* 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, 1);
+	else
+		fd = disk_open_dev(maj1, min1, 1, 1);
+
+	if ( fd < 0 && errno != ENOMEDIUM )
+		return -1;
+
+	dev->data = fd;
+	return 0;
+
+#else
+
+	ERROR("Not implemented yet");
+	(void)dev;
+	return -1;
+
+#endif
+
+}
+
+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) {
 
 	ERROR("Not implemented yet");
 	(void)dev;
 	(void)image;
-	(void)file;
 	return -1;
 
 }
 
+int disk_dump_image(struct usb_device_info * dev, enum image_type image, const char * file) {
+
+	if ( image != IMAGE_MMC )
+		ERROR_RETURN("Only mmc images are supported", -1);
+
+	return disk_dump_dev(dev->data, file);
+
+}
+
 int disk_check_badblocks(struct usb_device_info * dev, const char * device) {
 
 	ERROR("Not implemented yet");
diff --git a/src/disk.h b/src/disk.h
index d67b208..d5f367d 100644
--- a/src/disk.h
+++ b/src/disk.h
@@ -27,8 +27,9 @@ int disk_init(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, const char * file);
 
 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);
diff --git a/src/fiasco.c b/src/fiasco.c
index a2cd55a..caa0a9f 100644
--- a/src/fiasco.c
+++ b/src/fiasco.c
@@ -38,7 +38,8 @@
 #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 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) {
 
@@ -122,7 +123,7 @@ struct fiasco * fiasco_alloc_from_file(const char * file) {
 		READ_OR_RETURN(fiasco, buf, 7);
 
 		/* Header of next image */
-		if ( ! buf[0] == 0x54 && buf[2] == 0x2E && buf[3] == 0x19 && buf[4] == 0x01 && buf[5] == 0x01 && buf[6] == 0x00 ) {
+		if ( ! ( buf[0] == 0x54 && buf[2] == 0x2E && buf[3] == 0x19 && buf[4] == 0x01 && buf[5] == 0x01 && buf[6] == 0x00 ) ) {
 			ERROR("Invalid next image header");
 			return fiasco;
 		}
@@ -217,6 +218,8 @@ struct fiasco * fiasco_alloc_from_file(const char * file) {
 		READ_OR_RETURN(fiasco, buf, 1);
 
 		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);
@@ -230,12 +233,11 @@ struct fiasco * fiasco_alloc_from_file(const char * file) {
 
 		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) {
@@ -287,10 +289,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 ) {
@@ -353,12 +355,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");
 
@@ -368,10 +364,16 @@ 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);
 
 		/* number of subsections */
 		length8 = device_count+1;
@@ -379,48 +381,48 @@ int fiasco_write_to_file(struct fiasco * fiasco, const char * file) {
 			++length8;
 		if ( image->layout )
 			++length8;
-		WRITE_OR_FAIL(file, fd, &length8, 1);
+		WRITE_OR_FAIL_FREE(file, fd, &length8, 1, device_hwrevs_bufs);
 
 		/* unknown */
-		WRITE_OR_FAIL(file, fd, "\x2e\x19\x01\x01\x00", 5);
+		WRITE_OR_FAIL_FREE(file, fd, "\x2e\x19\x01\x01\x00", 5, device_hwrevs_bufs);
 
 		/* checksum */
 		hash = htons(image->hash);
-		WRITE_OR_FAIL(file, fd, &hash, 2);
+		WRITE_OR_FAIL_FREE(file, fd, &hash, 2, device_hwrevs_bufs);
 
 		/* 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);
 
 		/* image size */
 		size = htonl(image->size);
-		WRITE_OR_FAIL(file, fd, &size, 4);
+		WRITE_OR_FAIL_FREE(file, fd, &size, 4, device_hwrevs_bufs);
 
 		/* unknown */
-		WRITE_OR_FAIL(file, fd, "\x00\x00\x00\x00", 4);
+		WRITE_OR_FAIL_FREE(file, fd, "\x00\x00\x00\x00", 4, device_hwrevs_bufs);
 
 		/* 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);
 		}
 
 		/* 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]);
-			/* FIXME: memory leak: device_hwrevs_bufs */
+			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);
 		}
 		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);
 		}
@@ -445,7 +447,9 @@ int fiasco_write_to_file(struct fiasco * fiasco, const char * file) {
 
 	}
 
-	close(fd);
+	if ( ! simulate )
+		close(fd);
+
 	printf("\nDone\n\n");
 	return 0;
 
diff --git a/src/global.h b/src/global.h
index e1f0365..a26c88b 100644
--- a/src/global.h
+++ b/src/global.h
@@ -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 SLEEP(usec) do { struct timespec _t = { 0, (usec) }; nanosleep(&_t, 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
diff --git a/src/image.c b/src/image.c
index 48d80d8..9cb3e41 100644
--- a/src/image.c
+++ b/src/image.c
@@ -76,7 +76,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));
@@ -214,16 +214,13 @@ static int image_append(struct image * image, const char * type, const char * de
 
 static void image_align(struct image * image) {
 
-	size_t align = 0;
+	size_t align;
 
 	if ( image->type == IMAGE_MMC )
 		align = 8;
 	else
 		align = 7;
 
-	if ( align == 0 )
-		return;
-
 	if ( ( image->size & ( ( 1ULL << align ) - 1 ) ) == 0 )
 		return;
 
@@ -247,6 +244,7 @@ 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) {
 
+	off_t offset;
 	struct image * image = image_alloc();
 	if ( ! image )
 		return NULL;
@@ -259,11 +257,26 @@ struct image * image_alloc_from_file(const char * file, const char * type, const
 		return NULL;
 	}
 
-	image->size = lseek(image->fd, 0, SEEK_END);
+	offset = lseek(image->fd, 0, SEEK_END);
+	if ( offset == (off_t)-1 ) {
+		ERROR_INFO("Cannot seek to end of file %s", file);
+		close(image->fd);
+		free(image);
+		return NULL;
+	}
+
+	image->size = offset;
 	image->offset = 0;
 	image->cur = 0;
 	image->orig_filename = strdup(file);
-	lseek(image->fd, 0, SEEK_SET);
+
+	if ( lseek(image->fd, 0, SEEK_SET) == (off_t)-1 ) {
+		ERROR_INFO("Cannot seek to begin of file %s", file);
+		close(image->fd);
+		free(image->orig_filename);
+		free(image);
+		return NULL;
+	}
 
 	if ( image_append(image, type, device, hwrevs, version, layout) < 0 )
 		return NULL;
@@ -331,17 +344,22 @@ void image_free(struct image * image) {
 
 void image_seek(struct image * image, size_t whence) {
 
+	off_t offset;
+
 	if ( whence > image->size )
 		return;
 
 	if ( whence >= image->size - image->align ) {
-		lseek(image->fd, image->size - image->align - 1, SEEK_SET);
+		offset = 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);
+		offset = lseek(image->fd, image->offset + whence, SEEK_SET);
 		image->acur = 0;
 	}
 
+	if ( offset == (off_t)-1 )
+		ERROR_INFO("Seek in file %s failed", (image->orig_filename ? image->orig_filename : "(unknown)"));
+
 	IMAGE_STORE_CUR(image);
 
 }
@@ -350,6 +368,7 @@ 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;
 
@@ -376,7 +395,13 @@ size_t image_read(struct image * image, void * buf, size_t count) {
 	if ( ret_count == count )
 		return ret_count;
 
-	cur = lseek(image->fd, 0, SEEK_CUR) - image->offset;
+	offset = lseek(image->fd, 0, SEEK_CUR);
+	if ( offset == (off_t)-1 ) {
+		ERROR_INFO("Cannot get offset of file %s", (image->orig_filename ? image->orig_filename : "(unknown)"));
+		return 0;
+	}
+
+	cur = offset - image->offset;
 
 	if ( image->align && cur == image->size - image->align && image->acur < image->align ) {
 
@@ -483,41 +508,42 @@ static const char * image_types[] = {
 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;
diff --git a/src/libusb-sniff.c b/src/libusb-sniff.c
index ce66772..91f7872 100644
--- a/src/libusb-sniff.c
+++ b/src/libusb-sniff.c
@@ -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);
+
+}
diff --git a/src/local.c b/src/local.c
index c7f3ad1..3813910 100644
--- a/src/local.c
+++ b/src/local.c
@@ -53,7 +53,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 +111,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;
@@ -174,9 +182,12 @@ int local_init(void) {
 
 	}
 
+	fclose(file);
+
+#endif
+
 	failed = 1;
 	printf("Not a local device\n");
-	fclose(file);
 	return -1;
 
 }
@@ -212,17 +223,17 @@ 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 -o -b -s %d -l %d -f %s /dev/mtd%dro", 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 -o -b -s %d -l %d -f %s /dev/mtd%dro", offset, length, file, mtd);
 
 	ret = system(command);
 
@@ -282,143 +293,125 @@ static struct nanddump_device nanddump[] = {
 
 #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];
 
-	printf("Dump %s image to file %s...\n", image_type_to_string(image), file);
+	/* Find min & maj id for block device MyDocs (mmc device, partition 1) */
 
-	if ( image == IMAGE_MMC ) {
+	dir = opendir("/sys/class/mmc_host/");
+	if ( ! dir ) {
+		ERROR("Cannot find MyDocs mmc device: Opening '/sys/class/mmc_host/' failed");
+		return;
+	}
 
-		maj = -1;
-		min = -1;
+	while ( ( dirent = readdir(dir) ) ) {
 
-		/* Find block device in /dev/ for MyDocs (mmc device, partition 1) */
+		if ( strncmp(dirent->d_name, ".", sizeof(".")) == 0 || strncmp(dirent->d_name, "..", sizeof("..")) == 0 )
+			continue;
 
-		dir = opendir("/sys/class/mmc_host/");
-		if ( ! dir ) {
-			ERROR("Cannot find MyDocs mmc device: Opening '/sys/class/mmc_host/' failed");
-			goto clean;
-		}
+		if ( snprintf(buf, sizeof(buf), "/sys/class/mmc_host/%s/slot_name", dirent->d_name) <= 0 )
+			continue;
 
-		while ( ( dirent = readdir(dir) ) ) {
+		fd = open(buf, O_RDONLY);
+		if ( fd < 0 )
+			continue;
 
-			if ( snprintf(buf, sizeof(buf), "/sys/class/mmc_host/%s/slot_name", dirent->d_name) <= 0 )
-				continue;
+		memset(buf, 0, sizeof(buf));
+		if ( read(fd, buf, sizeof(buf)-1) < 0 )
+			buf[0] = 0;
+		close(fd);
+		fd = -1;
 
-			fd = open(buf, O_RDONLY);
-			if ( fd < 0 )
-				continue;
+		if ( strncmp(buf, "internal", sizeof("internal")-1) != 0 )
+			continue;
 
-			memset(buf, 0, sizeof(buf));
-			if ( read(fd, buf, sizeof(buf)-1) < 0 )
-				buf[0] = 0;
-			close(fd);
+		if ( snprintf(buf, sizeof(buf), "/sys/class/mmc_host/%s/%s:0001/", dirent->d_name, dirent->d_name) <= 0 )
+			continue;
 
-			if ( strncmp(buf, "internal", sizeof("internal")-1) != 0 )
-				continue;
+		dir2 = opendir(buf);
+		if ( ! dir2 )
+			continue;
 
-			if ( snprintf(buf, sizeof(buf), "/sys/class/mmc_host/%s/%s:0001/", dirent->d_name, dirent->d_name) <= 0 )
-				continue;
+		while ( ( dirent2 = readdir(dir2) ) ) {
 
-			dir2 = opendir(buf);
-			if ( ! dir2 )
+			if ( strncmp(dirent2->d_name, "block:mmcblk", sizeof("block:mmcblk")-1) != 0 )
 				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 ( snprintf(buf, sizeof(buf), "/sys/class/mmc_host/%s/%s:0001/%s/dev", dirent->d_name, dirent->d_name, dirent2->d_name) <= 0 )
+				continue;
 
-				if ( fscanf(f, "%d:%d", &maj, &min) != 2 ) {
-					maj = -1;
-					min = -1;
-					fclose(f);
-					continue;
-				}
+			f = fopen(buf, "r");
+			if ( ! f )
+				continue;
 
+			if ( fscanf(f, "%d:%d", maj, min) != 2 ) {
+				*maj = -1;
+				*min = -1;
 				fclose(f);
-				break;
-
+				continue;
 			}
 
-			closedir(dir2);
-
-			if ( maj != -1 && min != -1 )
-				break;
+			fclose(f);
+			break;
 
 		}
 
-		closedir(dir);
+		closedir(dir2);
 
-		if ( maj == -1 || min == -1 ) {
-			ERROR("Cannot find MyDocs mmc device: Slot 'internal' was not found");
-			goto clean;
-		}
+		if ( *maj != -1 && *min != -1 )
+			break;
 
-		VERBOSE("Detected internal mmc device: major=%d minor=%d\n", maj, min);
+	}
 
-		blk[0] = 0;
+	closedir(dir);
 
-		dir = opendir("/dev/");
-		if ( ! dir ) {
-			ERROR("Cannot find MyDocs mmc device: Opening '/dev/' failed");
-			goto clean;
-		}
+#endif
 
-		while ( ( dirent = readdir(dir) ) ) {
+}
 
-			if ( snprintf(buf, sizeof(buf), "/dev/%s", dirent->d_name) <= 0 )
-				continue;
+int local_dump_image(enum image_type image, const char * file) {
 
-			if ( stat(buf, &st) != 0 )
-				continue;
+	int ret = -1;
+	int fd = -1;
+	unsigned char * addr = NULL;
+	off_t nlen, len;
+	int align;
+	int maj, min;
 
-			if ( ! S_ISBLK(st.st_mode) )
-				continue;
+	printf("Dump %s image to file %s...\n", image_type_to_string(image), file);
 
-			if ( makedev(maj, min) != st.st_rdev )
-				continue;
+	if ( image == IMAGE_MMC ) {
 
-			strcpy(blk, buf);
-			break;
+		maj = -1;
+		min = -1;
 
+		local_find_internal_mydocs(&maj, &min);
+		if ( maj == -1 || min == -1 ) {
+			ERROR("Cannot find MyDocs mmc device: Slot 'internal' was not found");
+			goto clean;
 		}
 
-		closedir(dir);
+		VERBOSE("Detected internal MyDocs mmc device: major=%d minor=%d\n", maj, min);
 
-		if ( ! blk[0] ) {
-			ERROR("Cannot find MyDocs mmc device: Block device in /dev/ was not found");
+		fd = disk_open_dev(maj, min, 1, 1);
+		if ( fd < 0 ) {
+			ERROR("Cannot open MyDocs mmc device in /dev/");
 			goto clean;
 		}
 
-		VERBOSE("Detected internal mmc device: '%s'\n", blk);
-
-		strncat(blk, "p1", sizeof(blk)-strlen(blk)-1);
+		ret = disk_dump_dev(fd, file);
 
-		printf("Using MyDocs mmc device: '%s'\n", blk);
-
-		ret = disk_dump_raw(blk, file);
+		close(fd);
+		fd = -1;
 
 	} else {
 
@@ -449,7 +442,7 @@ 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);
+	addr = (unsigned char *)mmap(NULL, len, PROT_READ, MAP_PRIVATE, fd, 0);
 
 	if ( addr == MAP_FAILED )
 		addr = NULL;
@@ -484,7 +477,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);
diff --git a/src/main.c b/src/main.c
index 0552593..40f367a 100644
--- a/src/main.c
+++ b/src/main.c
@@ -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);
 }
@@ -165,7 +168,7 @@ static void parse_image_arg(char * arg, struct image_list ** image_first) {
 	}
 
 	layout_file = strchr(arg, '%');
-	if (layout_file)
+	if ( layout_file )
 		*(layout_file++) = 0;
 
 	type = NULL;
@@ -175,16 +178,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,14 +212,17 @@ 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);
+		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;
@@ -828,9 +834,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);
@@ -987,6 +1000,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 )
diff --git a/src/mangen.c b/src/mangen.c
index d2bf89a..6f5a2fa 100644
--- a/src/mangen.c
+++ b/src/mangen.c
@@ -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");
diff --git a/src/mkii.c b/src/mkii.c
index 81e860d..6c8a0d6 100644
--- a/src/mkii.c
+++ b/src/mkii.c
@@ -17,42 +17,351 @@
 
 */
 
+#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(usb_dev_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_write(udev, USB_WRITE_EP, (char *)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_read(udev, USB_READ_EP, (char *)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;
+	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 ( usb_device(dev->udev)->descriptor.bNumConfigurations >= 1 )
+		usb_get_string_simple(dev->udev, usb_device(dev->udev)->config[0].iConfiguration, buf, sizeof(buf));
+	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;
+
+	/* Signature */
+	memcpy(ptr, "\x2E\x19\x01\x01", 4);
+	ptr += 4;
+
+	/* Space */
+	memcpy(ptr, "\x00", 1);
+	ptr += 1;
+
+	/* 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;
+
+	/* Space */
+	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);
+
+	return 0;
 
 }
 
@@ -128,9 +437,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 +521,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);
 
 }
 
diff --git a/src/mkii.h b/src/mkii.h
index d4d5c31..2731eed 100644
--- a/src/mkii.h
+++ b/src/mkii.h
@@ -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);
diff --git a/src/nolo.c b/src/nolo.c
index e437ef7..9faea56 100644
--- a/src/nolo.c
+++ b/src/nolo.c
@@ -22,8 +22,6 @@
 #include <stdlib.h>
 #include <arpa/inet.h>
 
-#include <usb.h>
-
 #include "nolo.h"
 #include "image.h"
 #include "global.h"
@@ -124,7 +122,10 @@ static int nolo_identify_string(struct usb_device_info * dev, const char * str,
 	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);
 
@@ -167,6 +168,9 @@ static int nolo_get_string(struct usb_device_info * dev, char * str, char * out,
 	if ( ( ret = usb_control_msg(dev->udev, NOLO_QUERY, NOLO_GET_STRING, 0, 0, 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);
@@ -247,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;
 
@@ -320,7 +324,7 @@ static int nolo_send_image(struct usb_device_info * dev, struct image * image, i
 
 			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;
 			}
 
@@ -374,22 +378,22 @@ 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_write(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 ) {
@@ -537,7 +541,7 @@ int nolo_flash_image(struct usb_device_info * dev, struct image * image) {
 
 			}
 
-			usleep(0xc350); // 0.5s
+			SLEEP(0xc350); // 0.5s
 
 		}
 
diff --git a/src/operations.c b/src/operations.c
index 2c28228..cd7ef44 100644
--- a/src/operations.c
+++ b/src/operations.c
@@ -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;
@@ -110,8 +112,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 +176,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 +201,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 +243,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 +269,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 +306,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 +327,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 +345,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 +366,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 +384,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 +405,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 +423,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 +444,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 +464,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 +485,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 +503,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 +524,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 +542,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 +563,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 +581,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 +602,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 +622,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 +643,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 +661,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;
 
 	}
 
diff --git a/src/printf-utils.c b/src/printf-utils.c
index 3520d0b..d43c063 100644
--- a/src/printf-utils.c
+++ b/src/printf-utils.c
@@ -39,20 +39,20 @@ void printf_progressbar(unsigned long long part, unsigned long long total) {
 	int tmp, cols = 80;
 
 	/* percentage calculation */
-	pc = total==0?100:(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);
 
 }
diff --git a/src/usb-device.c b/src/usb-device.c
index bf1241e..5379b82 100644
--- a/src/usb-device.c
+++ b/src/usb-device.c
@@ -17,6 +17,11 @@
 
 */
 
+/* Enable RTLD_DEFAULT for glibc */
+#ifndef _GNU_SOURCE
+#define _GNU_SOURCE
+#endif
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <stdint.h>
@@ -24,12 +29,7 @@
 #include <errno.h>
 #include <ctype.h>
 #include <signal.h>
-
-#include <usb.h>
-
-#ifdef LIBUSB_HAS_DETACH_KERNEL_DRIVER_NP
-#include <sys/ioctl.h>
-#endif
+#include <dlfcn.h>
 
 #include "global.h"
 #include "device.h"
@@ -39,20 +39,30 @@
 #include "cold-flash.h"
 #include "mkii.h"
 
+#ifdef __linux__
+#ifdef LIBUSB_HAS_DETACH_KERNEL_DRIVER_NP
+#include <sys/ioctl.h>
+#endif
+#endif
+
 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, 0x0096, -1, -1, -1, FLASH_DISK, { DEVICE_RX_44, 0 } },
+	{ 0x0421, 0x0105,  2,  1, -1, FLASH_NOLO, { DEVICE_SU_18, DEVICE_RX_44, DEVICE_RX_48, DEVICE_RX_51, DEVICE_RM_680, DEVICE_RM_696, 0 } },
+	{ 0x0421, 0x0106,  0, -1, -1, FLASH_COLD, { DEVICE_RX_51, DEVICE_RM_680, DEVICE_RM_696, 0 } },
+	{ 0x0421, 0x0189, -1, -1, -1, FLASH_DISK, { DEVICE_RX_48, 0 } },
+	{ 0x0421, 0x01c7, -1, -1, -1, FLASH_DISK, { DEVICE_RX_51, 0 } },
+	{ 0x0421, 0x01c8,  1,  1, -1, FLASH_MKII, { DEVICE_RX_51, DEVICE_RM_680, 0 } },
+	{ 0x0421, 0x03d1, -1, -1, -1, FLASH_DISK, { DEVICE_RM_680, 0 } },
+	{ 0x0421, 0x0431, -1, -1, -1, FLASH_DISK, { DEVICE_SU_18, DEVICE_RX_34, 0 } },
+	{ 0x0421, 0x04c3, -1, -1, -1, FLASH_DISK, { DEVICE_RX_34, 0 } },
 	{ 0x0421, 0x3f00,  2,  1, -1, FLASH_NOLO, { DEVICE_RX_34, 0 } },
 };
 
 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) {
@@ -79,6 +89,7 @@ static void usb_flash_device_info_print(const struct usb_flash_device * dev) {
 
 static void usb_reattach_kernel_driver(usb_dev_handle * udev, int interface) {
 
+#ifdef __linux__
 #ifdef LIBUSB_HAS_DETACH_KERNEL_DRIVER_NP
 	struct {
 		int ifno;
@@ -90,9 +101,13 @@ static void usb_reattach_kernel_driver(usb_dev_handle * udev, int interface) {
 		.data = NULL,
 	};
 
+	if ( interface < 0 )
+		return;
+
 	usb_release_interface(udev, interface);
 	ioctl(*((int *)udev), _IOWR('U', 18, command), &command);
 #endif
+#endif
 
 }
 
@@ -159,18 +174,22 @@ static struct usb_device_info * usb_device_is_valid(struct usb_device * dev) {
 
 			usb_descriptor_info_print(udev, dev, product, sizeof(product));
 
+			if ( usb_devices[i].interface >= 0 ) {
+
 #ifdef LIBUSB_HAS_DETACH_KERNEL_DRIVER_NP
-			PRINTF_LINE("Detaching kernel from USB interface...");
-			usb_detach_kernel_driver_np(udev, usb_devices[i].interface);
+				PRINTF_LINE("Detaching kernel from USB interface...");
+				usb_detach_kernel_driver_np(udev, usb_devices[i].interface);
 #endif
 
-			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_reattach_kernel_driver(udev, usb_devices[i].interface);
-				usb_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_reattach_kernel_driver(udev, usb_devices[i].interface);
+					usb_close(udev);
+					return NULL;
+				}
+
 			}
 
 			if ( usb_devices[i].alternate >= 0 ) {
@@ -203,20 +222,41 @@ static struct usb_device_info * usb_device_is_valid(struct usb_device * dev) {
 				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 )
 				ret->device = DEVICE_RX_51;
+			else if ( strcmp(product, "Nokia N950") == 0 || strcmp(product, "Sync Mode") == 0 || strcmp(product, "N950 (Update mode)") == 0 )
+				ret->device = DEVICE_RM_680;
+			else if ( strcmp(product, "N9 (Update mode)") == 0 || strcmp(product, "Nxy (Update mode)") == 0 )
+				ret->device = DEVICE_RM_696;
+			else if ( strcmp(product, "Nokia USB ROM") == 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);
+				usb_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 )
@@ -281,6 +321,9 @@ struct usb_device_info * usb_open_and_wait_for_device(void) {
 	void (*prev)(int);
 	static char progress[] = {'/','-','\\', '|'};
 
+	if ( dlsym(RTLD_DEFAULT, "libusb_init") )
+		ERROR_RETURN("You are trying to use broken libusb-1.0 library (either directly or via wrapper) which has slow listing of usb devices. It cannot be used for flashing or cold-flashing. Please use libusb 0.1.", NULL);
+
 	usb_init();
 	usb_find_busses();
 
@@ -318,7 +361,7 @@ struct usb_device_info * usb_open_and_wait_for_device(void) {
 		if ( ret )
 			break;
 
-		usleep(0xc350); // 0.5s
+		SLEEP(0xc350); // 0.5s
 
 	}
 
@@ -337,7 +380,8 @@ struct usb_device_info * usb_open_and_wait_for_device(void) {
 
 void usb_close_device(struct usb_device_info * dev) {
 
-	usb_reattach_kernel_driver(dev->udev, dev->flash_device->interface);
+	if ( dev->flash_device->protocol != FLASH_COLD )
+		usb_reattach_kernel_driver(dev->udev, dev->flash_device->interface);
 	usb_close(dev->udev);
 	free(dev);
 
@@ -350,7 +394,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");
 
@@ -363,13 +407,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");
 
@@ -377,6 +421,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");
 
@@ -391,7 +437,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");
+	}
 
 }
diff --git a/src/usb-device.h b/src/usb-device.h
index 20a073c..082bf73 100644
--- a/src/usb-device.h
+++ b/src/usb-device.h
@@ -20,8 +20,19 @@
 #ifndef USB_DEVICE_H
 #define USB_DEVICE_H
 
+#include <stdint.h>
+
+/* u_int*_t types are not defined without _GNU_SOURCE but usb.h needs them */
+#define u_int8_t uint8_t
+#define u_int16_t uint16_t
+#define u_int32_t uint32_t
+
 #include <usb.h>
 
+#define USB_READ_EP		(USB_ENDPOINT_IN | 0x1)
+#define USB_WRITE_EP		(USB_ENDPOINT_OUT | 0x1)
+#define USB_WRITE_DATA_EP	(USB_ENDPOINT_OUT | 0x2)
+
 #include "device.h"
 
 enum usb_flash_protocol {
@@ -48,6 +59,7 @@ struct usb_device_info {
 	int16_t hwrev;
 	const struct usb_flash_device * flash_device;
 	usb_dev_handle * udev;
+	int data;
 };
 
 const char * usb_flash_protocol_to_string(enum usb_flash_protocol protocol);
@@ -56,7 +68,7 @@ void usb_close_device(struct usb_device_info * dev);
 
 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