Minor cleanup, commented-out beginnings of elf support

Cargo.lock

@@ -156,7 +156,7 @@ dependencies = [
  "heck",
  "proc-macro2",
  "quote",
- "syn 2.0.38",
+ "syn 2.0.39",
 ]
 
 [[package]]
@@ -324,9 +324,9 @@ checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646"
 
 [[package]]
 name = "libc"
-version = "0.2.149"
+version = "0.2.150"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a08173bc88b7955d1b3145aa561539096c421ac8debde8cbc3612ec635fee29b"
+checksum = "89d92a4743f9a61002fae18374ed11e7973f530cb3a3255fb354818118b2203c"
 
 [[package]]
 name = "linux-raw-sys"
@@ -565,9 +565,9 @@ dependencies = [
 
 [[package]]
 name = "syn"
-version = "2.0.38"
+version = "2.0.39"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "e96b79aaa137db8f61e26363a0c9b47d8b4ec75da28b7d1d614c2303e232408b"
+checksum = "23e78b90f2fcf45d3e842032ce32e3f2d1545ba6636271dcbf24fa306d87be7a"
 dependencies = [
  "proc-macro2",
  "quote",
@@ -795,29 +795,29 @@ checksum = "ed94fce61571a4006852b7389a063ab983c02eb1bb37b47f8272ce92d06d9538"
 
 [[package]]
 name = "winnow"
-version = "0.5.18"
+version = "0.5.19"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "176b6138793677221d420fd2f0aeeced263f197688b36484660da767bca2fa32"
+checksum = "829846f3e3db426d4cee4510841b71a8e58aa2a76b1132579487ae430ccd9c7b"
 dependencies = [
  "memchr",
 ]
 
 [[package]]
 name = "zerocopy"
-version = "0.7.24"
+version = "0.7.25"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "092cd76b01a033a9965b9097da258689d9e17c69ded5dcf41bca001dd20ebc6d"
+checksum = "8cd369a67c0edfef15010f980c3cbe45d7f651deac2cd67ce097cd801de16557"
 dependencies = [
  "zerocopy-derive",
 ]
 
 [[package]]
 name = "zerocopy-derive"
-version = "0.7.24"
+version = "0.7.25"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "a13a20a7c6a90e2034bcc65495799da92efcec6a8dd4f3fcb6f7a48988637ead"
+checksum = "c2f140bda219a26ccc0cdb03dba58af72590c53b22642577d88a927bc5c87d6b"
 dependencies = [
  "proc-macro2",
  "quote",
- "syn 2.0.38",
+ "syn 2.0.39",
 ]

src/file.rs

@@ -1,14 +1,10 @@
-extern crate object;
-
 use std::fs::File;
 use std::io::prelude::*;
 use std::iter::repeat;
-use std::path::Path;
 use std::path::PathBuf;
 
 use clap::ValueEnum;
-use log::debug;
+use log::{debug, warn};
-use log::warn;
 use object::elf;
 use object::Endianness;
 
@@ -23,13 +19,20 @@ pub enum MemoryFileType {
     Bin,
 }
 
+pub struct MemorySection {
+    pub address: u32,
+    pub data: Vec<u8>,
+}
+
 const M33_FLAGS: u32 = elf::EF_ARM_EABI_VER5 | elf::EF_ARM_ABI_FLOAT_HARD; // processor has hard float, even if code may not use it
 
 struct ElfWriter;
 struct IhexWriter;
 struct BinWriter;
 
-struct ElfReader;
+struct ElfReader {
+    file_contents: Vec<u8>,
+}
 struct IhexReader {
     file_contents: String,
 }
@@ -50,11 +53,13 @@ impl MemoryFileType {
             _ => None,
         }
     }
-    pub fn object_file_reader(
+    pub fn object_file_reader_auto(
         filename: &PathBuf,
     ) -> Result<Box<dyn ObjectFileReader>, std::io::Error> {
         match MemoryFileType::type_from_filename(filename.to_str().unwrap_or("")) {
-            Some(MemoryFileType::Elf) => unimplemented!(),
+            Some(MemoryFileType::Elf) => {
+                unimplemented!("Sorry ELF reading support is coming later")
+            }
             Some(MemoryFileType::Ihex) => IhexReader::new(filename),
             Some(MemoryFileType::Bin) => BinReader::new(filename),
             None => {
@@ -63,6 +68,16 @@ impl MemoryFileType {
             }
         }
     }
+    pub fn object_file_reader(
+        &self,
+        filename: &PathBuf,
+    ) -> Result<Box<dyn ObjectFileReader>, std::io::Error> {
+        Ok(match *self {
+            MemoryFileType::Elf => unimplemented!("Sorry ELF reading support is coming later"),
+            MemoryFileType::Ihex => IhexReader::new(filename)?,
+            MemoryFileType::Bin => BinReader::new(filename)?,
+        })
+    }
     pub fn mem_writer(&self) -> Box<dyn MemoryWriter> {
         match *self {
             MemoryFileType::Elf => Box::new(ElfWriter),
@@ -91,7 +106,7 @@ pub trait ObjectFileReader {
     /// Return the base address and entire data of `text` sections in the object file,
     /// copied into a Vec<u8>. This will allocate the necessary space for the entire memory
     /// map of the `text` sections, which in some cases is probably a horrible idea.
-    fn read_all(&mut self, size: Option<u32>) -> Result<Vec<u8>, Box<dyn std::error::Error>>;
+    fn read_all(&mut self, size: Option<u32>) -> Result<MemorySection, Box<dyn std::error::Error>>;
 }
 
 /// An object that can write a block of memory to a file
@@ -245,13 +260,16 @@ impl ObjectFileReader for BinReader {
         Ok(Box::new(BinReader { file, filesize }))
     }
 
-    fn read_all(&mut self, size: Option<u32>) -> Result<Vec<u8>, Box<dyn std::error::Error>> {
+    fn read_all(&mut self, size: Option<u32>) -> Result<MemorySection, Box<dyn std::error::Error>> {
         // The casting here is a bit janky...
         let size = std::cmp::min(size.unwrap_or(self.filesize as u32) as usize, self.filesize);
         let mut buf = Vec::from_iter(std::iter::repeat(0).take(size));
         self.file.read_exact(&mut buf)?;
 
-        Ok(buf)
+        Ok(MemorySection {
+            data: buf,
+            address: 0, // Bin file has no address information
+        })
     }
 }
 
@@ -265,10 +283,12 @@ impl ObjectFileReader for IhexReader {
 
         Ok(Box::new(IhexReader { file_contents }))
     }
-    fn read_all(&mut self, size: Option<u32>) -> Result<Vec<u8>, Box<dyn std::error::Error>> {
+    fn read_all(&mut self, size: Option<u32>) -> Result<MemorySection, Box<dyn std::error::Error>> {
         let reader = ihex::Reader::new(&self.file_contents);
+        let mut file_base = None;
         let mut cur_base = 0;
         let mut data = Vec::new();
+
         for rec in reader {
             if let Ok(record) = rec {
                 match record {
@@ -285,11 +305,27 @@ impl ObjectFileReader for IhexReader {
 
                     ihex::Record::Data { offset, mut value } => {
                         let record_base = cur_base + (offset as u32);
+                        // File base comes from the address of the first record
+                        if file_base.is_none() {
+                            file_base = Some(record_base);
+                        }
                         if (data.len() as u32) < record_base {
-                            debug!("Extending vec by {} to take up slack space", (record_base as usize) - data.len());
+                            // We're going to coalesce disjoint 'sections' in Ihex files, because they don't support 'real' sections to identify
+                            // what should or should not be programmed
+                            debug!(
+                                "Extending vec by {} to take up slack space",
+                                (record_base as usize) - data.len()
+                            );
                             data.extend(repeat(0xff).take((record_base as usize) - data.len()));
                         }
                         data.append(&mut value);
+                        if size.is_some() && data.len() >= size.unwrap() as usize {
+                            data.truncate(size.unwrap() as usize);
+                            return Ok(MemorySection {
+                                data,
+                                address: file_base.unwrap_or(0),
+                            });
+                        }
                     }
                     ihex::Record::EndOfFile => continue,
                     _ => unimplemented!(
@@ -306,6 +342,58 @@ impl ObjectFileReader for IhexReader {
             }
         }
 
-        Ok(data)
+        Ok(MemorySection {
+            data,
+            address: file_base.unwrap_or(0),
+        })
     }
 }
+
+//TODO: This is getting a bit hairy to figure out which sections to include, so forget about it for now.
+
+// impl ObjectFileReader for ElfReader {
+//     fn new(file: &PathBuf) -> Result<Box<dyn ObjectFileReader>, std::io::Error>
+//     where
+//         Self: Sized,
+//     {
+//         let file_contents = std::fs::read(file)?;
+//         Ok(Box::new(ElfReader { file_contents }))
+//     }
+//     fn read_all(&mut self, size: Option<u32>) -> Result<MemorySection, Box<dyn std::error::Error>> {
+//         let object = object::File::parse(&*self.file_contents).unwrap();
+//         let data = Vec::new();
+//         let file_base = None;
+
+//         debug!(
+//             "Opened object file, found sections: {:?}",
+//             object
+//                 .sections()
+//                 .map(|x| x.name().unwrap().to_owned())
+//                 .collect::<Vec<String>>()
+//         );
+
+//         for sec in object.sections() {
+//             if let SectionFlags::Elf{ sh_flags: flags } = sec.flags() {
+//                 if flags & elf::SHT_PROGBITS {
+//                     if file_base.is_none() {
+//                         file_base = Some(sec.address());
+//                     } else if sec.address() < file_base.unwrap() {
+//                         // Make space at the beginning of data
+//                         data.
+//                         file_base = Some(sec.address());
+
+//                     }
+
+//                 }
+//             }
+//         }
+
+//         Ok(MemorySection {
+//             data: section
+//                 .data_range(section.address(), size)?
+//                 .ok_or("section is empty or smaller than given size")?
+//                 .to_vec(),
+//             address: section.address() as u32,
+//         })
+//     }
+// }

src/main.rs

@@ -1,5 +1,7 @@
 use core::fmt;
-use std::{ffi::CString, fs::File, io::BufWriter, os::unix::prelude::OsStrExt, path::PathBuf};
+use std::{
+    ffi::CString, fmt::Write, fs::File, io::BufWriter, os::unix::prelude::OsStrExt, path::PathBuf,
+};
 
 use anyhow::{anyhow, Context, Error};
 use clap_num::maybe_hex;
@@ -102,7 +104,7 @@ enum ObjectFileType {
     Elf,
     Ihex,
     Bin,
-    /// Derive file type from file name
+    /// Derive file type from file name (not magic)
     Auto,
 }
 
@@ -112,7 +114,7 @@ enum WriteEraseMethod {
     All,
     /// Erase only the area to be written
     Region,
-    /// Don't do an erase. Memory to be written must be erased already if it's flash.
+    /// Don't erase (assume memory is erased)
     None,
 }
 
@@ -127,14 +129,17 @@ impl ObjectFileType {
     }
     fn file_reader(&self, filename: &PathBuf) -> Result<Box<dyn ObjectFileReader>, std::io::Error> {
         match self {
-            Self::Auto => MemoryFileType::object_file_reader(filename),
+            Self::Auto => MemoryFileType::object_file_reader_auto(filename),
-            _ => unimplemented!(),
+            Self::Bin => MemoryFileType::Bin.object_file_reader(filename),
+            Self::Ihex => MemoryFileType::Ihex.object_file_reader(filename),
+            Self::Elf => MemoryFileType::Elf.object_file_reader(filename),
         }
     }
 }
 
 #[derive(Args, Debug)]
 struct ReadArgs {
+    /// Object file to write
     file: PathBuf,
     #[arg(short = 't', long = "type", value_enum, default_value_t = ObjectFileType::Auto)]
     /// Type of object file to generate
@@ -143,9 +148,11 @@ struct ReadArgs {
     /// USB device to act on
     #[command(flatten)]
     devspec: UsbDeviceSpecifier,
+
     /// Base memory address on microcontroller (defaults to start of flash)
     #[arg(short = 'a', long = "base-address", value_parser=maybe_hex::<u32>)]
     addr: Option<u32>,
+
     /// Size to read in bytes (defaults to size of flash)
     #[arg(short, long, value_parser=maybe_hex::<u32>)]
     size: Option<u32>,
@@ -153,6 +160,7 @@ struct ReadArgs {
 
 #[derive(Args, Debug)]
 struct WriteArgs {
+    /// Object file to read
     file: PathBuf,
     #[arg(short = 't', long = "type", value_enum, default_value_t = ObjectFileType::Auto)]
     filetype: ObjectFileType,
@@ -160,17 +168,21 @@ struct WriteArgs {
     /// USB device to act on
     #[command(flatten)]
     devspec: UsbDeviceSpecifier,
+
     /// Base memory address on microcontroller to write to (defaults to start of flash)
     #[arg(short = 'a', long = "write-address", value_parser=maybe_hex::<u32>)]
     addr: Option<u32>,
+
     /// Size to write in bytes (defaults to size of flash)
     #[arg(short, long, value_parser=maybe_hex::<u32>)]
     size: Option<u32>,
+
     /// Don't reset the microcontroller after writing
     #[arg(short, long, action=ArgAction::SetFalse)]
     no_reset: bool,
-    #[arg(short, long, default_value = "region")]
+
     /// Erase method to use before writing
+    #[arg(short, long, default_value = "region")]
     erase: WriteEraseMethod,
 }
 
@@ -194,6 +206,7 @@ struct EraseArgs {
     /// Address to start erasing at
     #[arg(short = 'a', long="base-address", value_parser=maybe_hex::<u32>, conflicts_with="erase_all")]
     addr: Option<u32>,
+
     /// Number of bytes to erase
     #[arg(short, long, value_parser=maybe_hex::<u32>, conflicts_with="erase_all")]
     size: Option<u32>,
@@ -210,6 +223,9 @@ enum Commands {
     /// Read microcontroller memory to a file
     Read(ReadArgs),
     /// Write a file to microcontroller memory
+    ///
+    /// Read the object file, ignoring address mapping and relocation, and write its
+    /// read-only sections to the given address (or flash)
     Write(WriteArgs),
     /// Erase the microcontroller's flash
     Erase(EraseArgs),
@@ -227,9 +243,35 @@ fn read_write_style() -> ProgressStyle {
     .progress_chars("##-")
 }
 
+fn do_erase(
+    isp: &protocol::UsbIsp,
+    all: bool,
+    start_addr: Option<u32>,
+    size: Option<u32>,
+) -> Result<(), Error> {
+    let erase_pb = ProgressBar::new(size.unwrap_or(0).into())
+        .with_style(read_write_style())
+        .with_prefix("Erasing flash");
+    if all {
+        erase_pb.set_length(isp.GetFlashSizeInBytes()?.into());
+        isp.flash_erase_all(None)?;
+        erase_pb.finish();
+    } else {
+        let flash_start = isp.GetFlashStartAddress()?;
+        let flash_size = isp.GetFlashSizeInBytes()?;
+        let start_addr = start_addr.unwrap_or(flash_start);
+        let size = size.unwrap_or(flash_size);
+        if start_addr + size > flash_start + flash_size {
+            warn!("Looks like you're attempting to erase beyond the end of flash (0x{:x}-0x{:x})! This will probably fail.", start_addr, start_addr+size);
+        }
+        isp.flash_erase_region(None, start_addr, size)?;
+        erase_pb.finish();
+    }
+    Ok(())
+}
+
 fn write_file_to_flash(args: &WriteArgs) -> Result<(), Error> {
-    let api = HidApi::new()?;
+    let isp = connect_device(&args.devspec)?;
-    let isp = connect_device(&api, &args.devspec)?;
 
     let mut infile = args
         .filetype
@@ -240,41 +282,57 @@ fn write_file_to_flash(args: &WriteArgs) -> Result<(), Error> {
         .read_all(None)
         .or_else(|e| Err(anyhow!("Unable to read file contents: {}", e)))?;
 
-    println!("Input file first section contents:");
+    debug!(
-    for (i, line) in contents.chunks(16).enumerate() {
+        "Input file read (origin 0x{:08x} size 0x{:08x}):",
-        print!("{:08x} ", i * 16);
+        contents.address,
-        for block in line.chunks(8) {
+        contents.data.len()
-            for s in block.iter().map(|x| format!("{:02x} ", x)) {
+    );
-                print!("{}", s);
-            }
-            print!(" ");
-        }
-        println!();
-    }
 
     let flash_start = args.addr.unwrap_or(isp.GetFlashStartAddress()?);
-    let flash_size = args.size.unwrap_or(isp.GetFlashSizeInBytes()?);
+    let flash_size = args.size.unwrap_or(contents.data.len().try_into()?);
 
-    // if !args.no_reset {
+    debug!(
-    //     isp.reset()?;
+        "Will start write/erase at 0x{:08x} for 0x{:08x} bytes",
-    // }
+        flash_start, flash_size
+    );
+
+    match args.erase {
+        WriteEraseMethod::All => {
+            do_erase(&isp, true, None, None).with_context(|| "Erasing all flash")?
+        }
+        WriteEraseMethod::Region => do_erase(&isp, false, Some(flash_start), Some(flash_size))
+            .with_context(|| {
+                format!(
+                    "Erasing flash region 0x{:08x} size 0x{:08x}",
+                    flash_start, flash_size
+                )
+            })?,
+        WriteEraseMethod::None => (),
+    }
+    debug!(
+        "Beginning flash write at 0x{:08x} for 0x{:08x}",
+        flash_start, flash_size
+    );
+    let mut data_to_write = &contents.data[..flash_size as usize];
+    isp.WriteMemory(
+        &mut data_to_write,
+        flash_start,
+        flash_size,
+        Some(protocol::MemoryId::Internal),
+    )?;
+
+    if !args.no_reset {
+        isp.reset()?;
+    }
 
     Ok(())
 }
 
 fn read_flash_to_file(args: &ReadArgs) -> Result<(), Error> {
-    let api = hidapi::HidApi::new()?; // is this free, or should it be passed?
+    let isp = connect_device(&args.devspec)?;
-    let isp = connect_device(&api, &args.devspec)?;
 
-    let flash_start = match args.addr {
+    let flash_start = args.addr.unwrap_or(isp.GetFlashStartAddress()?);
-        None => isp.GetFlashStartAddress()?,
+    let flash_size = args.size.unwrap_or(isp.GetFlashSizeInBytes()?);
-        Some(addr) => addr,
-    };
-
-    let flash_size = match args.size {
-        None => isp.GetFlashSizeInBytes()?,
-        Some(size) => size,
-    };
 
     let mut buf = Vec::with_capacity(flash_size as usize);
 
@@ -319,32 +377,12 @@ fn read_flash_to_file(args: &ReadArgs) -> Result<(), Error> {
 }
 
 fn erase_flash(args: &EraseArgs) -> Result<(), Error> {
-    let api = hidapi::HidApi::new()?;
+    let isp = connect_device(&args.devspec)?;
-    let isp = connect_device(&api, &args.devspec)?;
+    do_erase(&isp, args.erase_all, args.addr, args.size)
-
-    let erase_pb = ProgressBar::new(1)
-        .with_style(read_write_style())
-        .with_prefix("Erasing flash");
-    if args.erase_all {
-        isp.flash_erase_all(None)?;
-        erase_pb.finish();
-    } else {
-        let flash_start = isp.GetFlashStartAddress()?;
-        let flash_size = isp.GetFlashSizeInBytes()?;
-        let start_addr = args.addr.unwrap_or(flash_start);
-        let size = args.addr.unwrap_or(flash_size);
-        if start_addr + size > flash_start + flash_size {
-            warn!("Looks like you're attempting to erase beyond the end of flash (0x{:x}-0x{:x})! This will probably fail.", start_addr, start_addr+size);
-        }
-        isp.flash_erase_region(None, start_addr, size)?;
-        erase_pb.finish();
-    }
-    Ok(())
 }
 
 fn reset(args: &ResetArgs) -> Result<(), Error> {
-    let api = hidapi::HidApi::new()?;
+    let isp = connect_device(&args.devspec)?;
-    let isp = connect_device(&api, &args.devspec)?;
 
     isp.reset()
 }
@@ -375,11 +413,9 @@ fn get_matching_devices<'a>(
     }
 }
 
-fn connect_device(
+fn connect_device(devspec: &UsbDeviceSpecifier) -> Result<protocol::UsbIsp, Error> {
-    api: &hidapi::HidApi,
+    let api = HidApi::new()?;
-    devspec: &UsbDeviceSpecifier,
+    let matches = get_matching_devices(&api, devspec)?;
-) -> Result<protocol::UsbIsp, Error> {
-    let matches = get_matching_devices(api, devspec)?;
     if matches.len() < 1 {
         return Err(NoDevicesError {}.into());
     } else if matches.len() > 1 {
@@ -391,7 +427,7 @@ fn connect_device(
         matches[0].product_string().unwrap_or("device"),
         matches[0].path().to_str()?
     );
-    let dev = matches[0].open_device(api)?;
+    let dev = matches[0].open_device(&api)?;
     let proto = protocol::UsbIsp::new(dev);
     debug!("Connected to device {}", proto.GetUniqueDeviceId()?);
     Ok(proto)

src/protocol.rs

@@ -6,7 +6,7 @@ use hidapi::HidDevice;
 use log::debug;
 use num_enum::{FromPrimitive, IntoPrimitive};
 use std::fmt::{Debug, Display};
-use std::io::Write;
+use std::io::{Read, Write};
 use strum::IntoEnumIterator;
 use strum_macros::EnumIter;
 use uuid::Uuid;
@@ -17,6 +17,9 @@ use crate::packet::{
 };
 use crate::packet::{CommandFlags, CommandPacket, CommandTag};
 
+const DATA_PACKET_MAX: usize = 56; //TODO: Need to read this from the chip
+const READ_TIMEOUT: i32 = 1000;
+
 #[repr(u32)]
 #[derive(IntoPrimitive, EnumIter, Debug, PartialEq, Clone)]
 pub enum MemoryId {
@@ -309,6 +312,7 @@ pub struct UsbIsp {
 
 impl UsbIsp {
     pub fn new(device: HidDevice) -> Self {
+        device.set_blocking_mode(false).unwrap();
         Self { device }
     }
     pub fn send_command(&self, p: CommandPacket) -> Result<ResponsePacket, Error> {
@@ -316,30 +320,52 @@ impl UsbIsp {
         self.write_packet(Packet::CommandPacket(p))
     }
     pub fn write_packet(&self, p: Packet) -> Result<ResponsePacket, Error> {
+        let mut expecting_response = true;
+        debug!("Writing packet {:?}", &p);
         let usb_packet = match p {
             Packet::CommandPacket(p) => {
                 UsbPacket::new(packet::ReportId::CommandOut, Packet::CommandPacket(p))
             }
             Packet::ResponsePacket(_) => panic!("We should not be writing responses"),
             Packet::DataPacket(p) => {
+                expecting_response = false;
                 UsbPacket::new(packet::ReportId::DataOut, Packet::DataPacket(p))
             }
         };
+
         let mut buf = Vec::with_capacity(usb_packet.length());
-        usb_packet.write(&mut buf)?;
+        usb_packet.write(&mut buf)?; // Write packet to buffer
-
+        debug!("raw packet: {:x?}", buf);
-        //FIXME: don't expect a response on data packets
         let _ = self.device.write(&buf)?;
-        let mut rbuf = [0u8; 64];
-        let res_size = self.device.read(&mut rbuf[..])?;
-        let parsed = packet::usb_packet(&rbuf[..res_size]).unwrap(); //TODO: handle more gracefully, but requires ownership of I
 
-        match parsed.1 {
+        let resp_timeout = if expecting_response { READ_TIMEOUT } else { 0 };
-            Packet::ResponsePacket(p) => Ok(p),
+
-            Packet::CommandPacket(_) => Err(anyhow!("Unexpected command packet on IN stream")),
+        let mut rbuf = [0u8; 64];
-            Packet::DataPacket(_) => {
+        let res_size = self.device.read_timeout(&mut rbuf[..], resp_timeout)?;
-                Err(anyhow!("Data IN packets not expected in reply to commands"))
+        if res_size > 0 {
+            let parsed = packet::usb_packet(&rbuf[..res_size]).unwrap(); //TODO: handle more gracefully, but requires ownership of I
+            debug!("Got response {:?}", parsed);
+
+            match parsed.1 {
+                Packet::ResponsePacket(p) => Ok(p),
+                Packet::CommandPacket(_) => Err(anyhow!("Unexpected command packet on IN stream")),
+                Packet::DataPacket(_) => {
+                    Err(anyhow!("Data IN packets not expected in reply to commands"))
+                }
             }
+        } else if !expecting_response {
+            Ok(ResponsePacket {
+                flags: CommandFlags::empty(),
+                tag: packet::ResponseTag::GenericResponse,
+                reserved: 0,
+                param_count: 2,
+                params: ResponseParameters::GenericResponse(packet::GenericResponseParams {
+                    status: StatusCode::Success,
+                    command: CommandTag::WriteMemory,
+                }),
+            })
+        } else {
+            Err(anyhow!("Expected a response and didn't get one"))
         }
     }
 
@@ -347,7 +373,7 @@ impl UsbIsp {
         let mut rbuf = [0u8; 64];
         let mut read = 0;
         while read < byte_count {
-            let res_size = self.device.read(&mut rbuf[..])?;
+            let res_size = self.device.read_timeout(&mut rbuf[..], READ_TIMEOUT)?;
             let parsed = packet::usb_packet(&rbuf[..res_size]).unwrap(); //TODO: handle more gracefully, but requires ownership of I
             match parsed.1 {
                 Packet::DataPacket(p) => {
@@ -371,6 +397,43 @@ impl UsbIsp {
         Ok(())
     }
 
+    pub fn write_data(&self, buf: &mut impl Read, byte_count: usize) -> Result<u32, Error> {
+        debug!("Writing {} bytes", byte_count);
+        let mut tmp_buf = [0u8; DATA_PACKET_MAX];
+        let mut written = 0;
+        while written < byte_count {
+            let to_write = buf.read(&mut tmp_buf)?;
+            debug!("{}..{}", written, written + to_write);
+            // Avoid the unnecessary copies somehow?
+            let resp = self
+                .write_packet(Packet::DataPacket(packet::DataPacket {
+                    bytes: tmp_buf[..to_write].to_vec(),
+                }))?
+                .params;
+
+            match resp {
+                ResponseParameters::GenericResponse(_) => (),
+                _ => Err(anyhow!("Unexpected response during data phase: {:?}", resp))?,
+            };
+
+            written += to_write;
+        }
+        debug!("Wrote total of {} bytes", written);
+
+        // After we're done we expect a GenericResponse with success.
+        // For some reason we need to send an empty data packet to finish the write
+        let resp = self.write_packet(Packet::DataPacket(packet::DataPacket { bytes: vec![] }))?;
+        let params = &resp.params;
+
+        match params {
+            ResponseParameters::GenericResponse(rp) if rp.status == StatusCode::Success => Ok(written as u32),
+            _ => Err(anyhow!(
+                "Got an error response after data phase: {:?}",
+                resp
+            )),
+        }
+    }
+
     pub fn get_last_error(&self) -> u32 {
         // Same as get_property but less checking
         let command = CommandPacket {
@@ -597,10 +660,7 @@ impl UsbIsp {
             params: vec![
                 start_address,
                 byte_count,
-                match memory_id {
+                memory_id.unwrap_or(MemoryId::Internal).into(),
-                    Some(v) => v.into(),
-                    None => MemoryId::Internal.into(),
-                },
             ],
         };
         let pr = self.send_command(command)?;
@@ -615,7 +675,7 @@ impl UsbIsp {
 
         // We expect a GenericResponse with success after the transfer
         let mut rbuf = vec![0u8; 64];
-        let res_size = self.device.read(&mut rbuf[..])?;
+        let res_size = self.device.read_timeout(&mut rbuf[..], READ_TIMEOUT)?;
         let parsed = packet::usb_packet(&rbuf[..res_size]).unwrap().1; //TODO: handle more gracefully, but requires ownership of I
 
         match parsed {
@@ -632,4 +692,34 @@ impl UsbIsp {
             )),
         }
     }
+
+    pub fn WriteMemory(
+        &self,
+        buf: &mut impl std::io::Read,
+        start_address: u32,
+        byte_count: u32,
+        memory_id: Option<MemoryId>,
+    ) -> Result<u32, Error> {
+        let command = CommandPacket {
+            tag: CommandTag::WriteMemory,
+            flags: CommandFlags::DATA_FOLLOWS,
+            reserved: 0,
+            param_count: 3,
+            params: vec![
+                start_address,
+                byte_count,
+                memory_id.unwrap_or(MemoryId::Internal).into(),
+            ],
+        };
+        let pr = self.write_packet(Packet::CommandPacket(command))?;
+
+        match pr.params {
+            ResponseParameters::GenericResponse(params) => match params.status {
+                StatusCode::Success => (),
+                _ => return Err(anyhow!("Error returned from device: {:?}", params.status)),
+            },
+            _ => return Err(anyhow!("Unexpected reply to WriteMemory {:?}", pr.params)),
+        };
+        Ok(self.write_data(buf, byte_count as usize)?)
+    }
 }