Support blksize2 option (non-standard)

1 files changed, 31 insertions, 0 deletions

diff --git a/src/lib.rs b/src/lib.rs
index 9299eea..ec092a4 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -93,6 +93,20 @@ fn octet_to_netascii(buf: &[u8]) -> Vec<u8> {
     out
 }
 
+fn blksize2(mut size: usize) -> usize {
+    if size == 0 {
+        return 0;
+    }
+
+    let mut msb = 0;
+    while size > 0 {
+        size >>= 1;
+        msb += 1;
+    }
+    1 << (msb - 1)
+}
+
+
 impl Default for Tftp {
     fn default() -> Tftp {
         Tftp {
@@ -309,6 +323,14 @@ impl Tftp {
                     }
                     _ => false,
                 },
+                "blksize2" => match val.parse() {
+                    Ok(b) if b >= 8 && b <= 32768 => {
+                        /* select 2^x lower or equal the requested size */
+                        self.options.blksize = blksize2(b);
+                        true
+                    }
+                    _ => false,
+                },
                 "timeout" => match val.parse() {
                     Ok(t) if t >= 1 => {
                         self.options.timeout = t;
@@ -663,4 +685,13 @@ mod tests {
         assert_eq!(octet_to_netascii(b"\r\0\r\n"), b"\r\0\0\r\0\r\n");
         assert_eq!(octet_to_netascii(b""), b"");
     }
+
+    #[test]
+    fn test_blksize2() {
+        assert_eq!(blksize2(16), 16);
+        assert_eq!(blksize2(17), 16);
+        assert_eq!(blksize2(15), 8);
+        assert_eq!(blksize2(1), 1);
+        assert_eq!(blksize2(0), 0);
+    }
 }