diff --git a/hid-libusb.c b/hid-libusb.c
index 5d61c2b..accc510 100644
--- a/hid-libusb.c
+++ b/hid-libusb.c
@@ -32,13 +32,15 @@
 #include <libusb-1.0/libusb.h>
 #include "util.h"
 
-static const unsigned char CMD_PRG[]  = "\2PROGRA";
-static const unsigned char CMD_PRG2[] = "M\2";
-static const unsigned char CMD_ACK[]  = "A";
-static const unsigned char CMD_READ[] = "Raan";
-static const unsigned char CMD_ENDR[] = "ENDR";
-static const unsigned char CMD_CWB0[] = "CWB\4\0\0\0\0";
-static const unsigned char CMD_CWB1[] = "CWB\4\0\1\0\0";
+static const unsigned char CMD_PRG[]   = "\2PROGRA";
+static const unsigned char CMD_PRG2[]  = "M\2";
+static const unsigned char CMD_ACK[]   = "A";
+static const unsigned char CMD_READ[]  = "Raan";
+static const unsigned char CMD_WRITE[] = "Waan...";
+static const unsigned char CMD_ENDR[]  = "ENDR";
+static const unsigned char CMD_ENDW[]  = "ENDW";
+static const unsigned char CMD_CWB0[]  = "CWB\4\0\0\0\0";
+static const unsigned char CMD_CWB1[]  = "CWB\4\0\1\0\0";
 
 static libusb_context *ctx = NULL;          // libusb context
 static libusb_device_handle *dev;           // libusb device
@@ -310,6 +312,46 @@ void hid_read_block(int bno, uint8_t *data, int nbytes)
     }
 }
 
+
+void hid_write_block(int bno, uint8_t *data, int nbytes)
+{
+    unsigned addr = bno * nbytes;
+    unsigned char ack, cmd[4+32];
+    int n;
+
+    if (addr < 0x10000 && offset != 0) {
+        offset = 0;
+        hid_send_recv(CMD_CWB0, 8, &ack, 1);
+        if (ack != CMD_ACK[0]) {
+            fprintf(stderr, "%s: Wrong acknowledge %#x, expected %#x\n",
+                __func__, ack, CMD_ACK[0]);
+            exit(-1);
+        }
+    } else if (addr >= 0x10000 && offset == 0) {
+        offset = 0x00010000;
+        hid_send_recv(CMD_CWB1, 8, &ack, 1);
+        if (ack != CMD_ACK[0]) {
+            fprintf(stderr, "%s: Wrong acknowledge %#x, expected %#x\n",
+                __func__, ack, CMD_ACK[0]);
+            exit(-1);
+        }
+    }
+
+    for (n=0; n<nbytes; n+=32) {
+        cmd[0] = CMD_WRITE[0];
+        cmd[1] = (addr + n) >> 8;
+        cmd[2] = addr + n;
+        cmd[3] = 32;
+        memcpy(cmd + 4, data + n, 32);
+        hid_send_recv(cmd, 4+32, &ack, 1);
+        if (ack != CMD_ACK[0]) {
+            fprintf(stderr, "%s: Wrong acknowledge %#x, expected %#x\n",
+                __func__, ack, CMD_ACK[0]);
+            exit(-1);
+        }
+    }
+}
+
 void hid_read_finish()
 {
     unsigned char ack;
@@ -320,3 +362,14 @@ void hid_read_finish()
             __func__, ack, CMD_ACK[0]);
     }
 }
+
+void hid_write_finish()
+{
+    unsigned char ack;
+
+    hid_send_recv(CMD_ENDW, 4, &ack, 1);
+    if (ack != CMD_ACK[0]) {
+        fprintf(stderr, "%s: Wrong acknowledge %#x, expected %#x\n",
+            __func__, ack, CMD_ACK[0]);
+    }
+}
diff --git a/rd5r.c b/rd5r.c
index dc18dc5..9caba60 100644
--- a/rd5r.c
+++ b/rd5r.c
@@ -381,7 +381,7 @@ static void rd5r_download(radio_device_t *radio)
             fflush(stderr);
         }
     }
-    hid_read_finish();
+    //hid_read_finish();
 
     // Add header.
     memset(&radio_mem[0], 0xff, 128);
@@ -397,14 +397,15 @@ static void rd5r_download(radio_device_t *radio)
 //
 static void rd5r_upload(radio_device_t *radio, int cont_flag)
 {
-    //TODO
-#if 0
     int bno;
 
-    dfu_erase(0, MEMSZ);
-
-    for (bno=0; bno<MEMSZ/1024; bno++) {
-        dfu_write_block(bno, &radio_mem[bno*1024], 1024);
+    // Write range 0x80...0x1e29f.
+    for (bno=1; bno<966; bno++) {
+        if (bno >= 248 && bno < 256) {
+            // Skip range 0x7c00...0x8000.
+            continue;
+        }
+        hid_write_block(bno, &radio_mem[bno*128], 128);
 
         ++radio_progress;
         if (radio_progress % 32 == 0) {
@@ -412,7 +413,7 @@ static void rd5r_upload(radio_device_t *radio, int cont_flag)
             fflush(stderr);
         }
     }
-#endif
+    hid_write_finish();
 }
 
 //
diff --git a/util.h b/util.h
index 1290759..661a092 100644
--- a/util.h
+++ b/util.h
@@ -73,6 +73,8 @@ void hid_close(void);
 int hid_write_read(const unsigned char *data, unsigned length, unsigned char *reply, unsigned rlength);
 void hid_read_block(int bno, unsigned char *data, int nbytes);
 void hid_read_finish(void);
+void hid_write_block(int bno, unsigned char *data, int nbytes);
+void hid_write_finish(void);
 
 //
 // Delay in milliseconds.