diff --git a/radio.c b/radio.c
index 4ed3555..f8aecec 100644
--- a/radio.c
+++ b/radio.c
@@ -81,26 +81,6 @@ void radio_print_version(radio_device_t* dev, FILE *out)
     dev->print_version(dev, out);
 }
 
-//
-// Write firmware image to the device.
-//
-void radio_upload(radio_device_t* dev, int cont_flag)
-{
-    // Check for compatibility.
-    if (! dev->is_compatible(dev)) {
-        fprintf(stderr, "Incompatible image - cannot upload.\n");
-        exit(-1);
-    }
-    if (! trace_flag) {
-        fprintf(stderr, "Write device: ");
-        fflush(stderr);
-    }
-    dev->upload(dev, cont_flag);
-
-    if (! trace_flag)
-        fprintf(stderr, " done.\n");
-}
-
 //
 // Read firmware image from the binary file.
 //
diff --git a/src/lib.rs b/src/lib.rs
index c449578..beb9424 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -98,7 +98,7 @@ pub extern "C" fn rust_main(_argc: c_int, _argv: *const *const c_char) -> c_int
       let device = radio::connect();
       radio::read_image(&matches.free[0]);
       radio::print_version(&device);
-      radio::upload(&device, 0);
+      radio::upload(&device, 0, trace_flag);
       radio::disconnect();
   } else if config_flag {
     let conf_args = matches.free.len();
@@ -127,7 +127,7 @@ pub extern "C" fn rust_main(_argc: c_int, _argv: *const *const c_char) -> c_int
         radio::save_image(&device, "device.img");
         radio::parse_config(&device, &config_filename);
         radio::verify_config(&device);
-        radio::upload(&device, 1);
+        radio::upload(&device, 1, trace_flag);
         radio::disconnect();
     }
   } else if verify_flag {
diff --git a/src/radio.rs b/src/radio.rs
index 5105ec4..e5dbd56 100644
--- a/src/radio.rs
+++ b/src/radio.rs
@@ -12,14 +12,13 @@ pub struct Radio {
 #[repr(C)]
 pub struct radio_tab_t {
     ident: *const c_char,
-    device: *const radio_device_t,
+    device: *mut radio_device_t,
 }
 
 extern {
 
   fn set_active_device(device: *const radio_device_t);
 
-  fn radio_upload(device: *const radio_device_t, cont_flag: c_int);
   fn radio_verify_config(device: *const radio_device_t);
   fn radio_print_version(device: *const radio_device_t, stdout: *const libc::FILE);
   fn radio_print_config(device: *const radio_device_t, file: *const libc::FILE, verbose: c_int);
@@ -99,7 +98,7 @@ pub fn connect() -> Radio {
     println!("Connect to {}", name);
 
     set_active_device(device);
-    return Radio { ptr: device };
+    return Radio { ptr: device as *mut radio_device_t };
   }
 }
 
@@ -119,7 +118,7 @@ pub fn disconnect() {
 /// Read firmware image from the device
 pub fn download(radio: &Radio, trace: bool) {
 
-  let device = radio.ptr;
+  let device = radio.ptr as *mut radio_device_t;
 
   if !trace {
     eprint!("Read device: ");
@@ -127,8 +126,7 @@ pub fn download(radio: &Radio, trace: bool) {
 
   unsafe {
     let download_fn = (*device).download.unwrap();
-    let device_mut = device as *mut radio_device_t;
-    download_fn(device_mut);
+    download_fn(device);
   }
 
   if !trace {
@@ -137,10 +135,25 @@ pub fn download(radio: &Radio, trace: bool) {
 }
 
 /// Write firmware image to the device.
-pub fn upload(radio: &Radio, cont_flag: c_int) {
-  let device = radio.ptr;
+pub fn upload(radio: &Radio, cont_flag: c_int, trace: bool) {
+  let device = radio.ptr as *mut radio_device_t;
   unsafe {
-    radio_upload(device, cont_flag)
+    let is_compatible_fn = (*device).is_compatible.unwrap();
+    if is_compatible_fn(device) == 0 {
+      eprintln!("Incompatible image - cannot upload.");
+      exit(-1);
+    }
+
+    if !trace {
+      eprint!("Write device: ");
+    }
+
+    let upload_fn = (*device).upload.unwrap();
+    upload_fn(device, cont_flag);
+
+    if !trace {
+      eprintln!(" done.");
+    }
   }
 }
 
@@ -180,7 +193,7 @@ pub fn read_image(filename: &str) -> Radio {
   let ptr = unsafe {
     radio_read_image(filename.as_ptr())
   };
-  Radio { ptr }
+  Radio { ptr: ptr as *mut radio_device_t }
 }
 
 /// Save firmware image to the binary file.