diff --git a/main.c b/main.c
index 6388b14..50bbf17 100644
--- a/main.c
+++ b/main.c
@@ -87,6 +87,7 @@ int old_c_main(int argc, char **argv)
     setvbuf(stdout, 0, _IOLBF, 0);
     setvbuf(stderr, 0, _IOLBF, 0);
 
+    /*
     if (write_flag) {
         // Restore image file to device.
         if (argc != 1)
@@ -98,7 +99,7 @@ int old_c_main(int argc, char **argv)
         radio_upload(0);
         radio_disconnect();
 
-    } else if (config_flag) {
+    } else */if (config_flag) {
         if (argc != 1 && argc != 2)
             usage();
 
diff --git a/src/lib.rs b/src/lib.rs
index 7f4ec9a..f341843 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,3 +1,4 @@
+use std::ffi::CString;
 use libc::{c_int, c_char};
 use getopts::Options;
 use std::process::exit;
@@ -9,6 +10,11 @@ const VERSION: Option<&'static str> = option_env!("VERSION");
 
 extern {
   fn old_c_main(argc: c_int, argv: *const *const c_char) -> c_int;
+  fn radio_connect();
+  fn radio_read_image(filename: *const c_char);
+  fn radio_print_version(stdout: *const libc::FILE);
+  fn radio_upload(cont_flag: c_int);
+  fn radio_disconnect();
 }
 
 #[no_mangle]
@@ -88,6 +94,21 @@ pub extern "C" fn rust_main(argc: c_int, argv: *const *const c_char) -> c_int {
     print_usage();
   }
 
+  if write_flag {
+    if matches.free.len() != 1 {
+      print_usage();
+    }
+    unsafe {
+      radio_connect();
+      let filename = CString::new(matches.free[0].clone()).unwrap();
+      radio_read_image(filename.as_ptr());
+      let mode = CString::new("w").unwrap();
+      radio_print_version(libc::fdopen(libc::STDOUT_FILENO, mode.as_ptr()));
+      radio_upload(0);
+      radio_disconnect();
+    }
+  }
+
   unsafe {
     old_c_main(argc, argv)
   }