vhost-user-i2c: Implement low level I2C helpers

This patch implements the low level I2C helpers responsible for
transferring data and parsing the adapters and their clients.

Signed-off-by: Viresh Kumar <viresh.kumar@linaro.org>

1 files changed, 298 insertions, 3 deletions

diff --git a/src/i2c.rs b/src/i2c.rs
index 7f0b4bf..adeb4bf 100644
--- a/src/i2c.rs
+++ b/src/i2c.rs
@@ -5,7 +5,86 @@
 //
 // SPDX-License-Identifier: Apache-2.0
 
+use libc::{open, ioctl, O_RDWR};
+use log::{error};
 use std::io::{Result};
+use std::os::raw::{c_char, c_ulong};
+use std::process::exit;
+
+// Standard I2C definitions
+// I2C IOCTL commands
+const I2C_SLAVE: u64 = 0x0703; // Use this slave address
+const I2C_FUNCS: u64 = 0x0705; // Get the adapter functionality mask
+const I2C_RDWR: u64 = 0x0707; // Combined R/W transfer (one STOP only)
+const I2C_SMBUS: u64 = 0x0720; // SMBus transfer
+
+// I2C/SMBUS Functions
+const I2C_FUNC_I2C: u64 = 0x00000001;
+const I2C_FUNC_SMBUS_READ_BYTE: u64 = 0x00020000;
+const I2C_FUNC_SMBUS_WRITE_BYTE: u64 = 0x00040000;
+const I2C_FUNC_SMBUS_READ_BYTE_DATA: u64 = 0x00080000;
+const I2C_FUNC_SMBUS_WRITE_BYTE_DATA: u64 = 0x00100000;
+const I2C_FUNC_SMBUS_READ_WORD_DATA: u64 = 0x00200000;
+const I2C_FUNC_SMBUS_WRITE_WORD_DATA: u64 = 0x00400000;
+
+const I2C_FUNC_SMBUS_BYTE: u64 = I2C_FUNC_SMBUS_READ_BYTE | I2C_FUNC_SMBUS_WRITE_BYTE;
+const I2C_FUNC_SMBUS_BYTE_DATA: u64 = I2C_FUNC_SMBUS_READ_BYTE_DATA | I2C_FUNC_SMBUS_WRITE_BYTE_DATA;
+const I2C_FUNC_SMBUS_WORD_DATA: u64 = I2C_FUNC_SMBUS_READ_WORD_DATA | I2C_FUNC_SMBUS_WRITE_WORD_DATA;
+
+// SMBUS definitions
+// SMBUS read or write markers
+const I2C_SMBUS_WRITE: u8 = 0;
+const I2C_SMBUS_READ: u8 = 1;
+
+// SMBus transaction types (size parameter in the above functions)
+const I2C_SMBUS_QUICK: u32 = 0;
+const I2C_SMBUS_BYTE: u32 = 1;
+const I2C_SMBUS_BYTE_DATA: u32 = 2;
+const I2C_SMBUS_WORD_DATA: u32 = 3;
+
+// As specified in SMBus standard
+const I2C_SMBUS_BLOCK_MAX: usize = 32;
+
+union I2cSmbusData {
+	byte: u8,
+	word: u16,
+	block: [u8; I2C_SMBUS_BLOCK_MAX + 2]
+}
+
+#[repr(C)]
+struct I2cSmbusIoctlData {
+	read_write: u8,
+	command: u8,
+	size: u32,
+	data: *mut I2cSmbusData
+}
+
+// I2C definitions
+pub const I2C_M_RD: u16 = 0x0001; // read data, from slave to master
+
+#[derive(Copy, Clone)]
+#[repr(C)]
+pub struct I2cMsg {
+	pub addr: u16,
+	pub flags: u16,
+	pub len: u16,
+	pub buf: *mut u8
+}
+
+#[repr(C)]
+struct I2cRdwrIoctlData {
+    msgs: *mut I2cMsg,
+	nmsgs: u32
+}
+
+#[repr(C)]
+pub struct I2cReq {
+	pub addr: u16,
+	pub flags: u16,
+	pub len: u16,
+	pub buf: Vec<u8>
+}
+
 
 // Local I2C definitions
 const MAX_I2C_VDEV: usize = 1 << 7;
@@ -23,11 +102,227 @@ pub struct I2cBackend {
     adapter_map: [u32; MAX_I2C_VDEV],
 }
 
+fn vi2c_set_client_addr(fd: i32, addr: usize) -> bool {
+    let ret = unsafe {ioctl(fd, I2C_SLAVE, addr as c_ulong)};
+    if ret == -1 {
+        println!("Failed to set client addr to {:x}", addr);
+        false
+    } else {
+        true
+    }
+}
+
+fn i2c_xfer(adapter: &I2cAdapter, reqs: &mut [I2cReq]) -> bool {
+    let mut msgs: Vec<I2cMsg> = Vec::with_capacity(reqs.len());
+
+    for i in 0..reqs.len() {
+        msgs.push(
+            I2cMsg {
+                addr: reqs[i].addr,
+                flags: reqs[i].flags,
+                len: reqs[i].len,
+                buf: reqs[i].buf.as_mut_ptr()
+            }
+            );
+    }
+
+    let data = I2cRdwrIoctlData {
+        msgs: msgs.as_mut_ptr(),
+        nmsgs: reqs.len() as u32
+    };
+
+    let ret = unsafe {ioctl(adapter.fd, I2C_RDWR, &data)};
+    if ret == -1 {
+        println!("Failed to transfer i2c data to client addr to {:x}", reqs[0].addr);
+        false
+    } else {
+        true
+    }
+}
+
+// Based on Linux's drivers/i2c/i2c-core-smbus.c:i2c_smbus_xfer_emulated().
+//
+// This function tries to reverse what Linux does, only support basic modes
+// (up to word transfer).
+fn smbus_xfer(adapter: &I2cAdapter, reqs: &mut [I2cReq]) -> bool {
+    let mut data = I2cSmbusData {block: [0; I2C_SMBUS_BLOCK_MAX + 2]};
+	let read_write: u8;
+	let size: u32;
+
+    match reqs.len() {
+        1 => {
+            if (reqs[0].flags & I2C_M_RD) != 0 {
+                if reqs[0].len > 1 {
+                    println!("Incorrect message length for read operation: {}",
+                             reqs[0].len);
+                    return false;
+                }
+                read_write = I2C_SMBUS_READ;
+            } else {
+                read_write = I2C_SMBUS_WRITE;
+            }
+
+            if reqs[0].len == 0 {
+                size = I2C_SMBUS_QUICK;
+            } else if reqs[0].len == 1 {
+                size = I2C_SMBUS_BYTE;
+            } else if reqs[0].len == 2 {
+                size = I2C_SMBUS_BYTE_DATA;
+                data.byte = reqs[0].buf[1];
+            } else if reqs[0].len == 3 {
+                size = I2C_SMBUS_WORD_DATA;
+                data.word = reqs[0].buf[1] as u16 | ((reqs[0].buf[2] as u16) << 8);
+            } else {
+                println!("Message length not supported for write operation: {}",
+                         reqs[0].len);
+                return false;
+            }
+        }
+
+        2 => {
+            if ((reqs[0].flags & I2C_M_RD) != 0) ||
+               ((reqs[1].flags & I2C_M_RD) == 0) ||
+               (reqs[0].len != 1) || (reqs[1].len > 2) {
+                println!("Expecting a valid read smbus transfer: {:?}",
+                         (reqs.len(), reqs[0].len, reqs[1].len));
+                return false;
+            }
+            read_write = I2C_SMBUS_READ;
+
+            if reqs[1].len == 1 {
+                size = I2C_SMBUS_BYTE_DATA;
+            } else {
+                size = I2C_SMBUS_WORD_DATA;
+            }
+        }
+
+        _ => {
+            println!("Invalid number of messages for smbus xfer: {}", reqs.len());
+            return false;
+        }
+    }
+
+    let smbus_data = I2cSmbusIoctlData {
+        read_write: read_write,
+        command: reqs[0].buf[0],
+        size: size,
+        data: &mut data
+    };
+
+    let ret = unsafe {
+        ioctl(adapter.fd, I2C_SMBUS, &smbus_data)
+    };
+
+    if ret == -1 {
+        println!("Failed to transfer smbus data to client addr to {:x}", reqs[0].addr);
+        return false;
+    }
+
+    if smbus_data.read_write == I2C_SMBUS_READ {
+        unsafe {
+            match smbus_data.size {
+                I2C_SMBUS_BYTE => reqs[0].buf[0] = data.byte,
+                I2C_SMBUS_BYTE_DATA => reqs[1].buf[0] = data.byte,
+                I2C_SMBUS_WORD_DATA => {
+                    reqs[1].buf[0] = (data.word & 0xff) as u8;
+                    reqs[1].buf[1] = (data.word >> 8) as u8;
+                }
+
+                _ => {
+                    println!("Invalid SMBUS command: {}", smbus_data.size);
+                    return false;
+                }
+            }
+        }
+    }
+    true
+}
+
+pub fn vi2c_xfer(backend: &I2cBackend, reqs: &mut [I2cReq]) -> bool {
+    let client = reqs[0].addr as usize;
+    let index = backend.adapter_map[client];
+
+    if index == I2C_INVALID_ADAPTER {
+        println!("Adapter not found for client address: {}", client);
+        return false;
+    }
+
+    let adapter = backend.adapters[index as usize];
+
+    // Set client's address
+    if !vi2c_set_client_addr(adapter.fd, client) {
+        return false;
+    }
+
+    if adapter.smbus {
+        return smbus_xfer(&adapter, reqs);
+    } else {
+        return i2c_xfer(&adapter, reqs);
+    }
+}
+
 impl I2cBackend {
-    pub fn new(_list: &str) -> Result<I2cBackend> {
-        let adapter_map: [u32; MAX_I2C_VDEV] = [I2C_INVALID_ADAPTER; MAX_I2C_VDEV];
-        let adapters: Vec<I2cAdapter> = Vec::new();
+    pub fn new(list: &str) -> Result<I2cBackend> {
+        let mut adapter_map: [u32; MAX_I2C_VDEV] = [I2C_INVALID_ADAPTER; MAX_I2C_VDEV];
+        let mut adapters: Vec<I2cAdapter> = Vec::new();
+        let devices: Vec<&str> = list.split(',').collect();
+        let count: usize = devices.len();
+        let mut smbus;
+
+        for i in 0..count {
+            let list: Vec<&str> = devices[i].split(':').collect();
+            let bus = list[0].parse::<u32>().unwrap();
+            let clients = &list[1..];
+            let mut dev = String::from("/dev/i2c-");
+            let funcs: u64 = 0;
+
+            dev.push_str(&bus.to_string());
+
+            let fd = unsafe { open(dev.as_ptr() as *const c_char, O_RDWR) };
+            assert!(fd != -1);
+
+            let ret = unsafe {
+                ioctl(fd, I2C_FUNCS, &funcs)
+            };
+            assert!(ret != -1);
+
+            if (funcs & I2C_FUNC_I2C) != 0 {
+                smbus = false;
+            } else if (funcs & (I2C_FUNC_SMBUS_WORD_DATA | I2C_FUNC_SMBUS_BYTE |
+                                I2C_FUNC_SMBUS_BYTE_DATA)) != 0 {
+                smbus = true;
+            } else {
+                error!("Invalid functionality {:x}", funcs);
+                exit(-1);
+            }
+
+            for j in 1..clients.len() {
+                let client = clients[j].parse::<usize>().unwrap();
+
+                if client > MAX_I2C_VDEV {
+                    error!("Invalid client address {}", client);
+                    exit(-1);
+                }
+
+				if adapter_map[client] != I2C_INVALID_ADAPTER {
+                    error!("Client address {} is already used by {}", client, adapter_map[client]);
+                    exit(-1);
+                }
+
+                assert!(vi2c_set_client_addr(fd, client));
+                adapter_map[client] = i as u32;
+            }
+
+            adapters.push(
+                I2cAdapter {
+                    fd: fd,
+                    bus: bus,
+                    smbus: smbus
+                }
+                );
 
+            println!("Added I2C master with bus id: {:x} for clients: {:?}", bus, clients);
+        }
 
         Ok(I2cBackend {
             adapters,