<commit_before>use {Point3, Projective3};
use alga::general::SubsetOf;

/// A scan position image.
#[derive(Clone, Copy, Debug)]
pub struct Image {
    cop: Projective3,
    mounting_matrix: Projective3,
}

impl Image {
    /// Sets this image's mounting matrix.
    pub fn set_mounting_matrix<T>(&mut self, mounting_matrix: T)
        where T: SubsetOf<Projective3>
    {
        self.mounting_matrix = mounting_matrix.to_superset();
    }

    /// Sets this image's camera's own position matrix.
    pub fn set_cop<T>(&mut self, cop: T)
        where T: SubsetOf<Projective3>
    {
        self.cop = cop.to_superset();
    }

    /// Converts a point in the project's coordinate system to the camera's coordinate system.
    ///
    /// # Examples
    ///
    /// ```
    /// use riscan_pro::{Image, Point3};
    /// let image = Image::default();
    /// let input = Point3::new(1., 2., 3.);
    /// let output = image.prcs_to_cmcs(input);
    /// assert_eq!(output, input);
    /// ```
    pub fn prcs_to_cmcs(&self, point: Point3) -> Point3 {
        use alga::linear::Transformation;

        (self.mounting_matrix * self.cop.inverse()).transform_point(&point)
    }
}

impl Default for Image {
    fn default() -> Image {
        Image {
            cop: Projective3::identity(),
            mounting_matrix: Projective3::identity(),
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use nalgebra::Rotation3;
    use std::f64::consts::PI;

    #[test]
    fn prcs_to_cmcs_identity() {
        let image = Image::default();
        let input = Point3::new(1., 2., 3.);
        let output = image.prcs_to_cmcs(input);
        assert_eq!(output, input);
    }

    #[test]
    fn prcs_to_cmcs_mounting_matrix() {
        let mut image = Image::default();
        image.set_mounting_matrix(Rotation3::from_euler_angles(0., 0., PI / 2.));
        let input = Point3::new(1., 2., 3.);
        let output = image.prcs_to_cmcs(input);
        assert_relative_eq!(Point3::new(-2., 1., 3.), output);
    }

    #[test]
    fn prcs_to_cmcs_cop() {
        let mut image = Image::default();
        image.set_cop(Rotation3::from_euler_angles(0., 0., PI / 2.));
        let input = Point3::new(1., 2., 3.);
        let output = image.prcs_to_cmcs(input);
        assert_relative_eq!(Point3::new(2., -1., 3.), output);
    }
}
<commit_msg>Add sop to math<commit_after>use {Point3, Projective3};
use alga::general::SubsetOf;

/// A scan position image.
#[derive(Clone, Copy, Debug)]
pub struct Image {
    cop: Projective3,
    mounting_matrix: Projective3,
    sop: Projective3,
}

impl Image {
    /// Sets this image's mounting matrix.
    pub fn set_mounting_matrix<T>(&mut self, mounting_matrix: T)
        where T: SubsetOf<Projective3>
    {
        self.mounting_matrix = mounting_matrix.to_superset();
    }

    /// Sets this image's camera's own position matrix.
    pub fn set_cop<T>(&mut self, cop: T)
        where T: SubsetOf<Projective3>
    {
        self.cop = cop.to_superset();
    }

    /// Sets this image's scanner's own position matrix.
    pub fn set_sop<T>(&mut self, sop: T)
        where T: SubsetOf<Projective3>
    {
        self.sop = sop.to_superset();
    }

    /// Converts a point in the project's coordinate system to the camera's coordinate system.
    ///
    /// # Examples
    ///
    /// ```
    /// use riscan_pro::{Image, Point3};
    /// let image = Image::default();
    /// let input = Point3::new(1., 2., 3.);
    /// let output = image.prcs_to_cmcs(input);
    /// assert_eq!(output, input);
    /// ```
    pub fn prcs_to_cmcs(&self, point: Point3) -> Point3 {
        use alga::linear::Transformation;

        (self.mounting_matrix * self.cop.inverse() * self.sop.inverse()).transform_point(&point)
    }
}

impl Default for Image {
    fn default() -> Image {
        Image {
            cop: Projective3::identity(),
            mounting_matrix: Projective3::identity(),
            sop: Projective3::identity(),
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use nalgebra::Rotation3;
    use std::f64::consts::PI;

    #[test]
    fn prcs_to_cmcs_identity() {
        let image = Image::default();
        let input = Point3::new(1., 2., 3.);
        let output = image.prcs_to_cmcs(input);
        assert_eq!(output, input);
    }

    #[test]
    fn prcs_to_cmcs_mounting_matrix() {
        let mut image = Image::default();
        image.set_mounting_matrix(Rotation3::from_euler_angles(0., 0., PI / 2.));
        let input = Point3::new(1., 2., 3.);
        let output = image.prcs_to_cmcs(input);
        assert_relative_eq!(Point3::new(-2., 1., 3.), output);
    }

    #[test]
    fn prcs_to_cmcs_cop() {
        let mut image = Image::default();
        image.set_cop(Rotation3::from_euler_angles(0., 0., PI / 2.));
        let input = Point3::new(1., 2., 3.);
        let output = image.prcs_to_cmcs(input);
        assert_relative_eq!(Point3::new(2., -1., 3.), output);
    }

    #[test]
    fn prcs_to_cmcs_sop() {
        let mut image = Image::default();
        image.set_sop(Rotation3::from_euler_angles(0., 0., PI / 2.));
        let input = Point3::new(1., 2., 3.);
        let output = image.prcs_to_cmcs(input);
        assert_relative_eq!(Point3::new(2., -1., 3.), output);
    }
}
<|endoftext|>


<commit_before>
use std::boxed::Box;
use std::ops::Neg;
use std::sync::Arc;
use num::traits::{One, Zero};

use na::{Iso2, Norm, Rot2, Translation, Vec1, Vec2};
use ncollide::inspection::{Repr2};
use ncollide::shape::{Cone, Cuboid, Cylinder};
use nphysics::object::{RigidBody};

use constants::*;
use tagtree;

enum Part {
    Vessel { width: f64, length: f64 },
    FuelTank { radius: f64, length: f64 },
    Engine { radius: f64, length: f64, group: i32 }
}

struct PointMass {
    center: Vec2<f64>,
    mass: f64
}

type Shape = Box<Repr2<f64>>;
type ArcShape = Arc<Shape>;

impl PointMass {

    fn mass_contrib (&self) -> Vec2<f64> {
        self.center * self.mass
    }

    fn moment_contrib (&self) -> f64 {
        self.mass * self.center.sqnorm()
    }

}

impl Part {

    fn mass (&self) -> f64 {
        match self {
            &Part::Vessel {width, length} => {
                VESSEL_DENSITY * width * length
            }
            &Part::FuelTank {radius, length} => {
                FUELTANK_DENSITY * radius * length
            }
            &Part::Engine {radius, length, group} => {
                let _group = group;
                ENGINE_DENSITY * radius * length * 0.5
            }
        }
    }

    fn geometry (&self) -> ArcShape {
        Arc::new(match self {
            &Part::Vessel {width, length} => {
                Box::new(Cuboid::new(Vec2::new(width, length))) as Shape
            }
            &Part::FuelTank {radius, length} => {
                Box::new(Cylinder::new(length, radius)) as Shape
            }
            &Part::Engine {radius, length, group} => {
                let _group = group;
                Box::new(Cone::new(length, radius)) as Shape
            }
        })
    }

    fn object (&self) -> RigidBody {
        match self {
            &Part::Vessel {width, length} => {
                let geom = Cuboid::new(Vec2::new(width, length));
                RigidBody::new_dynamic(geom, VESSEL_DENSITY, 1.0, 1.0)
            }
            &Part::FuelTank {radius, length} => {
                let geom = Cylinder::new(length, radius);
                RigidBody::new_dynamic(geom, FUELTANK_DENSITY, 1.0, 1.0)
            }
            &Part::Engine {radius, length, group} => {
                let _group = group;
                let geom = Cone::new(length, radius);
                RigidBody::new_dynamic(geom, ENGINE_DENSITY, 1.0, 1.0)
            }
        }
    }

    fn point_mass (&self) -> PointMass {
        PointMass { center: Vec2::new(0.0,0.0), mass: self.mass() }
    }

}

#[test]
fn simple_parts () {

    let p1 = Part::Vessel { width: 2.0, length: 4.0 };
    let _p2 = Part::FuelTank { radius: 1.0, length: 5.0};
    let _p3 = Part::Engine { radius: 1.0, length: 2.0, group: 3};

    let _m1 = p1.mass();
    let _pm1 = p1.point_mass();

}

struct PartObjectCache {
    part: Part,
    object: RigidBody,
    mass: f64
}

struct Beam {
    length: f64
}

impl Beam {

    fn object (&self) -> RigidBody {
        let geom = Cuboid::new(Vec2::new(BEAM_WIDTH, self.length));
        RigidBody::new_dynamic(geom, BEAM_DENSITY, 1.0, 1.0)
    }

    fn mass (&self) -> f64 {
        self.length * BEAM_DENSITY
    }

    fn point_mass (&self) -> PointMass {
        let midpoint = self.length / 2.0;
        PointMass { center: Vec2::new(midpoint, 0.0), mass: self.mass() }
    }

}

struct Attach {
    location: f64,
    rotation: f64
}

type Structure = tagtree::TagTree<Part,Beam,Attach>;

impl Structure {

    fn mass(&self) -> f64 {
        match self {
            &tagtree::TagTree::Leaf(ref part) => {
                part.mass()
            }
            &tagtree::TagTree::Node(ref beam, _) => {
                beam.mass()
            }
        }
    }

    fn total_mass(&self) -> f64 {
        let mut total_mass: f64 = 0.0;
        for item in self.iter() {
            total_mass += item.structure.mass();
        }
        total_mass
    }

    fn point_masses(&self) -> Vec<PointMass> {
        // This smells, could probably return an iterator?
        self.iter().fold(Vec::new(), |mut acc, item| {
            acc.push(PointMass{ center: item.context.translation, mass: item.structure.mass() });
            acc
        })
    }

    fn center_of_mass(&self) -> Vec2<f64> {
        self.point_masses().iter().fold(Vec2::zero(), |acc, pm| {
            acc + pm.mass_contrib()
        })
    }

    fn total_moment(&self) -> f64 {
        let mut point_masses = self.point_masses();
        let neg_center = self.center_of_mass().neg();
        for mut pm in point_masses.iter_mut() {
            pm.center.append_translation_mut(&neg_center);
        }
        point_masses.iter().fold(0.0, |acc, pm| {
            acc + pm.moment_contrib()
        })
    }

    // fn compound_shape(&self) -> Compound
    // Compound::new(...)
}

// Opportunity to move into tagtree if Attach is a monoid
struct StructureContextItem<'a> {
    context: Iso2<f64>,
    structure: &'a Structure,
}

struct StructureWorkItem<'a> {
    context: Iso2<f64>,
    structure: &'a Structure,
}

struct StructureIter<'a> {
    work: Vec<StructureWorkItem<'a>>,
}

impl Structure {
    pub fn iter(&self) -> StructureIter {
        let root_work_item = StructureWorkItem {
            context: Iso2::one(),
            structure: self,
        };
        StructureIter { work: vec!(root_work_item) }
    }
}

impl<'a> Iterator for StructureIter<'a> {

    type Item = StructureContextItem<'a>;

    fn next(&mut self) -> Option<Self::Item> {

        match self.work.pop() {
            None => { None }
            Some(ref curr_work) => {
                let context = curr_work.context;

                match curr_work.structure {
                    &tagtree::TagTree::Leaf(_) => {}
                    &tagtree::TagTree::Node(_, ref attachments) => {
                        // This smells, should probably rewrite as a normal for loop
                        attachments.iter().fold((), |_, &(ref attach, ref attachment)| {
                            let trn = Vec2::new(attach.location, 0.0);
                            let rot = Rot2::new(Vec1::new(attach.rotation));
                            let iso = Iso2::new_with_rotmat(trn, rot);
                            let next_context = context * iso;
                            let next_work = StructureWorkItem {
                                context: next_context,
                            structure: &**attachment
                            };

                            self.work.push(next_work);
                        })
                    }
                }

                Some(StructureContextItem { context: context, structure: curr_work.structure })

            }
        }

    }

}

fn part(attrs: Part) -> Structure {
    tagtree::TagTree::Leaf(attrs)
}

fn beam(length: f64, parts: Vec<(Attach, Box<Structure>)>) -> Structure {
    tagtree::TagTree::Node(Beam {length: length}, parts)
}

#[test]
fn simple_structures () {
    let p1 = part(Part::Vessel { width: 2.0, length: 4.0 });
    let a1 = Attach{location: 2.0, rotation: 0.0};
    let p2 = part(Part::FuelTank { radius: 1.0, length: 5.0});
    let a2 = Attach{location: 8.0, rotation: 0.0};
    let p3 = part(Part::Engine { radius: 1.0, length: 2.0, group: 3});
    let a3 = Attach{location: 10.0, rotation: 0.0};
    let b1 = beam(5.0, vec![(a1, box p1),(a2, box p2),(a3, box p3)]);
    b1.mass();
}

#[test]
fn simple_ships () {
    beam(8.0, vec![
         (Attach {location: 2.0, rotation: 0.0},
          box part(Part::Vessel {width: 2.0, length: 4.0})
         ),
         (Attach {location: 6.0, rotation: 0.0},
          box part(Part::FuelTank {radius: 1.0, length: 1.5})
         ),
         (Attach {location: 8.0, rotation: 0.0},
          box part(Part::Engine {radius: 1.5, length: 0.5, group: 1})
         )
         ]
        );
}
<commit_msg>structure compound shapes<commit_after>
use std::boxed::Box;
use std::ops::Neg;
use std::sync::Arc;
use num::traits::{One, Zero};

use na::{Iso2, Norm, Rot2, Translation, Vec1, Vec2};
use ncollide::inspection::{Repr2};
use ncollide::shape::{Compound, Cone, Cuboid, Cylinder};
use nphysics::object::{RigidBody};

use constants::*;
use tagtree;

enum Part {
    Vessel { width: f64, length: f64 },
    FuelTank { radius: f64, length: f64 },
    Engine { radius: f64, length: f64, group: i32 }
}

struct PointMass {
    center: Vec2<f64>,
    mass: f64
}

type Shaped = Box<Repr2<f64>>;
type ArcShaped = Arc<Shaped>;

impl PointMass {

    fn mass_contrib (&self) -> Vec2<f64> {
        self.center * self.mass
    }

    fn moment_contrib (&self) -> f64 {
        self.mass * self.center.sqnorm()
    }

}

impl Part {

    fn mass (&self) -> f64 {
        match self {
            &Part::Vessel {width, length} => {
                VESSEL_DENSITY * width * length
            }
            &Part::FuelTank {radius, length} => {
                FUELTANK_DENSITY * radius * length
            }
            &Part::Engine {radius, length, group} => {
                let _group = group;
                ENGINE_DENSITY * radius * length * 0.5
            }
        }
    }

    fn geometry (&self) -> ArcShaped {
        Arc::new(match self {
            &Part::Vessel {width, length} => {
                Box::new(Cuboid::new(Vec2::new(width, length))) as Shaped
            }
            &Part::FuelTank {radius, length} => {
                Box::new(Cylinder::new(length, radius)) as Shaped
            }
            &Part::Engine {radius, length, group} => {
                let _group = group;
                Box::new(Cone::new(length, radius)) as Shaped
            }
        })
    }

    fn object (&self) -> RigidBody {
        match self {
            &Part::Vessel {width, length} => {
                let geom = Cuboid::new(Vec2::new(width, length));
                RigidBody::new_dynamic(geom, VESSEL_DENSITY, 1.0, 1.0)
            }
            &Part::FuelTank {radius, length} => {
                let geom = Cylinder::new(length, radius);
                RigidBody::new_dynamic(geom, FUELTANK_DENSITY, 1.0, 1.0)
            }
            &Part::Engine {radius, length, group} => {
                let _group = group;
                let geom = Cone::new(length, radius);
                RigidBody::new_dynamic(geom, ENGINE_DENSITY, 1.0, 1.0)
            }
        }
    }

    fn point_mass (&self) -> PointMass {
        PointMass { center: Vec2::new(0.0,0.0), mass: self.mass() }
    }

}

#[test]
fn simple_parts () {

    let p1 = Part::Vessel { width: 2.0, length: 4.0 };
    let _p2 = Part::FuelTank { radius: 1.0, length: 5.0};
    let _p3 = Part::Engine { radius: 1.0, length: 2.0, group: 3};

    let _m1 = p1.mass();
    let _pm1 = p1.point_mass();

}

struct PartObjectCache {
    part: Part,
    object: RigidBody,
    mass: f64
}

struct Beam {
    length: f64
}

impl Beam {

    fn geometry (&self) -> ArcShaped {
        let geom = Cuboid::new(Vec2::new(BEAM_WIDTH, self.length));
        Arc::new(Box::new(geom))
    }

    fn object (&self) -> RigidBody {
        let geom = Cuboid::new(Vec2::new(BEAM_WIDTH, self.length));
        RigidBody::new_dynamic(geom, BEAM_DENSITY, 1.0, 1.0)
    }

    fn mass (&self) -> f64 {
        self.length * BEAM_DENSITY
    }

    fn point_mass (&self) -> PointMass {
        let midpoint = self.length / 2.0;
        PointMass { center: Vec2::new(midpoint, 0.0), mass: self.mass() }
    }

}

struct Attach {
    location: f64,
    rotation: f64
}

type Structure = tagtree::TagTree<Part,Beam,Attach>;

impl Structure {

    fn mass(&self) -> f64 {
        match self {
            &tagtree::TagTree::Leaf(ref part) => {
                part.mass()
            }
            &tagtree::TagTree::Node(ref beam, _) => {
                beam.mass()
            }
        }
    }

    fn geometry (&self) -> ArcShaped {
        match self {
            &tagtree::TagTree::Leaf(ref part) => {
                part.geometry()
            }
            &tagtree::TagTree::Node(ref beam, _) => {
                beam.geometry()
            }
        }
    }

    fn total_mass(&self) -> f64 {
        let mut total_mass: f64 = 0.0;
        for item in self.iter() {
            total_mass += item.structure.mass();
        }
        total_mass
    }

    fn point_masses(&self) -> Vec<PointMass> {
        // This smells, could probably return an iterator?
        self.iter().fold(Vec::new(), |mut acc, item| {
            acc.push(PointMass{ center: item.context.translation, mass: item.structure.mass() });
            acc
        })
    }

    fn center_of_mass(&self) -> Vec2<f64> {
        self.point_masses().iter().fold(Vec2::zero(), |acc, pm| {
            acc + pm.mass_contrib()
        })
    }

    fn total_moment(&self) -> f64 {
        let mut point_masses = self.point_masses();
        let neg_center = self.center_of_mass().neg();
        for mut pm in point_masses.iter_mut() {
            pm.center.append_translation_mut(&neg_center);
        }
        point_masses.iter().fold(0.0, |acc, pm| {
            acc + pm.moment_contrib()
        })
    }

    fn compound_shape(&self) -> ArcShaped {
        let mut acc = Vec::<(Iso2<f64>,ArcShaped)>::new();
        for item in self.iter() {
            acc.push((item.context, item.structure.geometry()));
        }
        Arc::new(Box::new(Compound::new(acc)))
    }
}

// Opportunity to move into tagtree if Attach is a monoid
struct StructureContextItem<'a> {
    context: Iso2<f64>,
    structure: &'a Structure,
}

struct StructureWorkItem<'a> {
    context: Iso2<f64>,
    structure: &'a Structure,
}

struct StructureIter<'a> {
    work: Vec<StructureWorkItem<'a>>,
}

impl Structure {
    pub fn iter(&self) -> StructureIter {
        let root_work_item = StructureWorkItem {
            context: Iso2::one(),
            structure: self,
        };
        StructureIter { work: vec!(root_work_item) }
    }
}

impl<'a> Iterator for StructureIter<'a> {

    type Item = StructureContextItem<'a>;

    fn next(&mut self) -> Option<Self::Item> {

        match self.work.pop() {
            None => { None }
            Some(ref curr_work) => {
                let context = curr_work.context;

                match curr_work.structure {
                    &tagtree::TagTree::Leaf(_) => {}
                    &tagtree::TagTree::Node(_, ref attachments) => {
                        // This smells, should probably rewrite as a normal for loop
                        attachments.iter().fold((), |_, &(ref attach, ref attachment)| {
                            let trn = Vec2::new(attach.location, 0.0);
                            let rot = Rot2::new(Vec1::new(attach.rotation));
                            let iso = Iso2::new_with_rotmat(trn, rot);
                            let next_context = context * iso;
                            let next_work = StructureWorkItem {
                                context: next_context,
                            structure: &**attachment
                            };

                            self.work.push(next_work);
                        })
                    }
                }

                Some(StructureContextItem { context: context, structure: curr_work.structure })

            }
        }

    }

}

fn part(attrs: Part) -> Structure {
    tagtree::TagTree::Leaf(attrs)
}

fn beam(length: f64, parts: Vec<(Attach, Box<Structure>)>) -> Structure {
    tagtree::TagTree::Node(Beam {length: length}, parts)
}

#[test]
fn simple_structures () {
    let p1 = part(Part::Vessel { width: 2.0, length: 4.0 });
    let a1 = Attach{location: 2.0, rotation: 0.0};
    let p2 = part(Part::FuelTank { radius: 1.0, length: 5.0});
    let a2 = Attach{location: 8.0, rotation: 0.0};
    let p3 = part(Part::Engine { radius: 1.0, length: 2.0, group: 3});
    let a3 = Attach{location: 10.0, rotation: 0.0};
    let b1 = beam(5.0, vec![(a1, box p1),(a2, box p2),(a3, box p3)]);
    b1.mass();
}

#[test]
fn simple_ships () {
    beam(8.0, vec![
         (Attach {location: 2.0, rotation: 0.0},
          box part(Part::Vessel {width: 2.0, length: 4.0})
         ),
         (Attach {location: 6.0, rotation: 0.0},
          box part(Part::FuelTank {radius: 1.0, length: 1.5})
         ),
         (Attach {location: 8.0, rotation: 0.0},
          box part(Part::Engine {radius: 1.5, length: 0.5, group: 1})
         )
         ]
        );
}
<|endoftext|>


<commit_before>use std::fmt;
use std::mem;

use openssl::bn;

bf!(RegCnt[u32] {
    busy: 0:0,
    keyslot: 4:5,
    little_endian: 8:8,
    normal_order: 9:9
});

bf!(RegSlotCnt[u32] {
    key_set: 0:0,
    key_prot: 1:1
});

pub struct RsaKeyslot {
    write_pos: usize,
    buf: [u8; 0x100]
}

impl Clone for RsaKeyslot {
    fn clone(&self) -> RsaKeyslot {
        let mut new_buf = [0u8; 0x100];
        new_buf.copy_from_slice(&self.buf[..]);

        RsaKeyslot {
            write_pos: self.write_pos,
            buf: new_buf
        }
    }
}

pub struct RsaDeviceState {
    slots: [RsaKeyslot; 4],
    modulus: [u8; 0x100],
    message: [u8; 0x100],
}

impl Default for RsaDeviceState {
    fn default() -> RsaDeviceState {
        let new_keyslot = RsaKeyslot {
            write_pos: 0,
            buf: [0; 0x100],
        };
        RsaDeviceState {
            slots: [new_keyslot.clone(), new_keyslot.clone(),
                    new_keyslot.clone(), new_keyslot],
            modulus: [0; 0x100],
            message: [0; 0x100],
        }
    }
}

impl fmt::Debug for RsaDeviceState {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(f, "RsaDeviceState {{ }}")
    }
}

fn get_keydata(dev: &RsaDevice, keyslot: usize) -> (RegSlotCnt::Bf, u32) {
    match keyslot {
        0 => (RegSlotCnt::new(dev.slot0_cnt.get()), dev.slot0_len.get()),
        1 => (RegSlotCnt::new(dev.slot1_cnt.get()), dev.slot1_len.get()),
        2 => (RegSlotCnt::new(dev.slot2_cnt.get()), dev.slot2_len.get()),
        3 => (RegSlotCnt::new(dev.slot3_cnt.get()), dev.slot3_len.get()),
        _ => unreachable!()
    }
}

fn reg_slot_cnt_update(dev: &mut RsaDevice, keyslot: usize) {
    let (slot_cnt, _) = get_keydata(dev, keyslot);
    if slot_cnt.key_set.get() == 1 {
        assert_eq!(dev._internal_state.slots[keyslot].write_pos, 0x100);
    } else {
        dev._internal_state.slots[keyslot].write_pos = 0;
    }
}

fn reg_cnt_update(dev: &mut RsaDevice) {
    let cnt = RegCnt::new(dev.cnt.get());
    trace!("Wrote 0x{:08X} to RSA CNT register!", cnt.val);

    if cnt.busy.get() == 1 {
        let keyslot = cnt.keyslot.get() as usize;
        let (slot_cnt, _) = get_keydata(dev, keyslot);
        assert_eq!(slot_cnt.key_set.get(), 1);

        info!("Performing RSA arithmetic!");

        let mut base_buf = [0u8; 0x100];
        base_buf.copy_from_slice(&dev._internal_state.message[..]);
        let mut modulus_buf = [0u8; 0x100];
        modulus_buf.copy_from_slice(&dev._internal_state.modulus[..]);

        match (cnt.little_endian.get(), cnt.normal_order.get()) {
            (1, 1) => {},
            (0, 0) => {
                base_buf.reverse();
                modulus_buf.reverse();
            }
            _ => unimplemented!()
        }

        let base = bn::BigNum::from_slice(&base_buf[..]).unwrap();
        let exponent = bn::BigNum::from_slice(&dev._internal_state.slots[keyslot].buf[..]).unwrap();
        let modulus = bn::BigNum::from_slice(&modulus_buf[..]).unwrap();

        let mut res = bn::BigNum::new().unwrap();
        res.mod_exp(&base, &exponent, &modulus, &mut bn::BigNumContext::new().unwrap()).unwrap();

        for b in dev._internal_state.message.iter_mut() {
            *b = 0;
        }
        let res_vec = res.to_vec();
        // Copy result to the back of the buffer
        dev._internal_state.message[0x100 - res_vec.len() .. 0x100].copy_from_slice(res_vec.as_slice());

        let cnt_ref = RegCnt::alias_mut(dev.cnt.ref_mut());
        cnt_ref.busy.set(0);
    }
}

fn reg_mod_read(dev: &mut RsaDevice, buf_pos: usize, dest: &mut [u8]) {
    trace!("Reading {} bytes from RSA MOD at +0x{:X}", dest.len(), buf_pos);
    let src_slice = &dev._internal_state.modulus[buf_pos .. buf_pos + dest.len()];
    dest.clone_from_slice(src_slice);
}

fn reg_mod_write(dev: &mut RsaDevice, buf_pos: usize, src: &[u8]) {
    trace!("Writing {} bytes to RSA MOD at +0x{:X}", src.len(), buf_pos);
    let dst_slice = &mut dev._internal_state.modulus[buf_pos .. buf_pos + src.len()];
    dst_slice.clone_from_slice(src);
}

fn reg_txt_read(dev: &mut RsaDevice, buf_pos: usize, dest: &mut [u8]) {
    trace!("Reading {} bytes from RSA TXT at +0x{:X}", dest.len(), buf_pos);
    let src_slice = &dev._internal_state.message[buf_pos .. buf_pos + dest.len()];
    dest.clone_from_slice(src_slice);
}

fn reg_txt_write(dev: &mut RsaDevice, buf_pos: usize, src: &[u8]) {
    trace!("Writing {} bytes to RSA TXT at +0x{:X}", src.len(), buf_pos);
    let dst_slice = &mut dev._internal_state.message[buf_pos .. buf_pos + src.len()];
    dst_slice.clone_from_slice(src);
}

fn reg_exp_fifo_write(dev: &mut RsaDevice) {
    let cnt = RegCnt::new(dev.cnt.get());
    let keyslot = cnt.keyslot.get() as usize;
    let (slot_cnt, _) = get_keydata(dev, keyslot);

    assert_eq!(slot_cnt.key_set.get(), 0);
    assert_eq!(slot_cnt.key_prot.get(), 0);

    let write_pos = dev._internal_state.slots[keyslot].write_pos;
    if write_pos == 0 { // Just starting to update key, clear previous key
        for b in dev._internal_state.slots[keyslot].buf.iter_mut() {
            *b = 0;
        }
    }
    let word_bytes: [u8; 4] = unsafe { mem::transmute(dev.exp_fifo.get()) };
    dev._internal_state.slots[keyslot].buf[write_pos .. write_pos + 4].copy_from_slice(&word_bytes[..]);

    trace!("Writing bytes {:02X},{:02X},{:02X},{:02X} to RSA exponent FIFO!",
        word_bytes[0], word_bytes[1], word_bytes[2], word_bytes[3]);

    dev._internal_state.slots[keyslot].write_pos += 4;
}

iodevice!(RsaDevice, {
    internal_state: RsaDeviceState;
    regs: {
        0x000 => cnt: u32 { write_effect = reg_cnt_update; }
        0x0F0 => unk: u32 { }
        0x100 => slot0_cnt: u32 { write_effect = |dev: &mut RsaDevice| reg_slot_cnt_update(dev, 0); }
        0x104 => slot0_len: u32 { }
        0x110 => slot1_cnt: u32 { write_effect = |dev: &mut RsaDevice| reg_slot_cnt_update(dev, 1); }
        0x114 => slot1_len: u32 { }
        0x120 => slot2_cnt: u32 { write_effect = |dev: &mut RsaDevice| reg_slot_cnt_update(dev, 2); }
        0x124 => slot2_len: u32 { }
        0x130 => slot3_cnt: u32 { write_effect = |dev: &mut RsaDevice| reg_slot_cnt_update(dev, 3); }
        0x134 => slot3_len: u32 { }
        0x200 => exp_fifo: u32 { write_effect = reg_exp_fifo_write; }
    }
    ranges: {
        0x204;0xFC => { } // A bug in bootrom causes it to write all over this area
        0x400;0x100 => {
            read_effect = reg_mod_read;
            write_effect = reg_mod_write;
        }
        0x800;0x100 => {
            read_effect = reg_txt_read;
            write_effect = reg_txt_write;
        }
    }
});
<commit_msg>Finally implement all the correct RSA modes<commit_after>use std::fmt;
use std::mem;

use openssl::bn;

bf!(RegCnt[u32] {
    busy: 0:0,
    keyslot: 4:5,
    little_endian: 8:8,
    normal_order: 9:9
});

bf!(RegSlotCnt[u32] {
    key_set: 0:0,
    key_prot: 1:1
});

pub struct RsaKeyslot {
    write_pos: usize,
    buf: [u8; 0x100]
}

impl Clone for RsaKeyslot {
    fn clone(&self) -> RsaKeyslot {
        let mut new_buf = [0u8; 0x100];
        new_buf.copy_from_slice(&self.buf[..]);

        RsaKeyslot {
            write_pos: self.write_pos,
            buf: new_buf
        }
    }
}

pub struct RsaDeviceState {
    slots: [RsaKeyslot; 4],
    modulus: [u8; 0x100],
    message: [u8; 0x100],
}

impl Default for RsaDeviceState {
    fn default() -> RsaDeviceState {
        let new_keyslot = RsaKeyslot {
            write_pos: 0,
            buf: [0; 0x100],
        };
        RsaDeviceState {
            slots: [new_keyslot.clone(), new_keyslot.clone(),
                    new_keyslot.clone(), new_keyslot],
            modulus: [0; 0x100],
            message: [0; 0x100],
        }
    }
}

impl fmt::Debug for RsaDeviceState {
    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
        write!(f, "RsaDeviceState {{ }}")
    }
}

fn get_keydata(dev: &RsaDevice, keyslot: usize) -> (RegSlotCnt::Bf, u32) {
    match keyslot {
        0 => (RegSlotCnt::new(dev.slot0_cnt.get()), dev.slot0_len.get()),
        1 => (RegSlotCnt::new(dev.slot1_cnt.get()), dev.slot1_len.get()),
        2 => (RegSlotCnt::new(dev.slot2_cnt.get()), dev.slot2_len.get()),
        3 => (RegSlotCnt::new(dev.slot3_cnt.get()), dev.slot3_len.get()),
        _ => unreachable!()
    }
}

fn reg_slot_cnt_update(dev: &mut RsaDevice, keyslot: usize) {
    let (slot_cnt, _) = get_keydata(dev, keyslot);
    if slot_cnt.key_set.get() == 1 {
        assert_eq!(dev._internal_state.slots[keyslot].write_pos, 0x100);
    } else {
        dev._internal_state.slots[keyslot].write_pos = 0;
    }
}

fn word_swap(buf: &mut [u8;256]) -> &mut [u8;256] {
    {
        let mut chunks = buf.chunks_exact_mut(4);
        loop {
            match (chunks.next(), chunks.next_back()) {
                (Some(front), Some(back)) => {
                    let mut tmp = [0u8;4];
                    tmp.copy_from_slice(back);
                    back.copy_from_slice(front);
                    front.copy_from_slice(&tmp);
                }
                _ => break
            }
        }
    }
    buf
}

fn byte_swap_inner(buf: &mut [u8;256]) -> &mut [u8;256] {
    for chunk in buf.chunks_exact_mut(4) {
        chunk.reverse();
    }
    buf
}

fn reg_cnt_update(dev: &mut RsaDevice) {
    let cnt = RegCnt::new(dev.cnt.get());
    trace!("Wrote 0x{:08X} to RSA CNT register!", cnt.val);

    if cnt.busy.get() == 1 {
        let keyslot = cnt.keyslot.get() as usize;
        let (slot_cnt, _) = get_keydata(dev, keyslot);
        assert_eq!(slot_cnt.key_set.get(), 1);

        info!("Performing RSA arithmetic!");

        let mut base_buf = [0u8; 0x100];
        base_buf.copy_from_slice(&dev._internal_state.message[..]);
        let mut exponent_buf = [0u8; 0x100];
        exponent_buf.copy_from_slice(&dev._internal_state.slots[keyslot].buf[..]);
        let mut modulus_buf = [0u8; 0x100];
        modulus_buf.copy_from_slice(&dev._internal_state.modulus[..]);

        if cnt.little_endian.get() == 0 {
            byte_swap_inner(&mut modulus_buf);
            byte_swap_inner(&mut base_buf);
            byte_swap_inner(&mut exponent_buf);
        }
        if cnt.normal_order.get() == 0 {
            word_swap(&mut modulus_buf);
            word_swap(&mut base_buf);
        }

        let mut base = bn::BigNum::from_slice(&base_buf[..]).unwrap();
        let exponent = bn::BigNum::from_slice(&exponent_buf[..]).unwrap();
        let modulus = bn::BigNum::from_slice(&modulus_buf[..]).unwrap();

        // The AES hardware doesn't like even moduli, and will output 0 when it finds them
        if !modulus.is_bit_set(0) {
            base.clear();
        }

        let mut res = bn::BigNum::new().unwrap();
        res.mod_exp(&base, &exponent, &modulus, &mut bn::BigNumContext::new().unwrap()).unwrap();

        for b in dev._internal_state.message.iter_mut() {
            *b = 0;
        }
        let res_vec = res.to_vec();

        // Copy result to the back of the buffer
        dev._internal_state.message[0x100 - res_vec.len() .. 0x100].copy_from_slice(res_vec.as_slice());

        if cnt.little_endian.get() == 0 {
            byte_swap_inner(&mut dev._internal_state.message);
        }
        if cnt.normal_order.get() == 0 {
            word_swap(&mut dev._internal_state.message);
        }

        let cnt_ref = RegCnt::alias_mut(dev.cnt.ref_mut());
        cnt_ref.busy.set(0);
    }
}

fn reg_mod_read(dev: &mut RsaDevice, buf_pos: usize, dest: &mut [u8]) {
    trace!("Reading {} bytes from RSA MOD at +0x{:X}", dest.len(), buf_pos);
    let src_slice = &dev._internal_state.modulus[buf_pos .. buf_pos + dest.len()];
    dest.clone_from_slice(src_slice);
}

fn reg_mod_write(dev: &mut RsaDevice, buf_pos: usize, src: &[u8]) {
    trace!("Writing {} bytes to RSA MOD at +0x{:X}", src.len(), buf_pos);
    let dst_slice = &mut dev._internal_state.modulus[buf_pos .. buf_pos + src.len()];
    dst_slice.clone_from_slice(src);
}

fn reg_txt_read(dev: &mut RsaDevice, buf_pos: usize, dest: &mut [u8]) {
    trace!("Reading {} bytes from RSA TXT at +0x{:X}", dest.len(), buf_pos);
    let src_slice = &dev._internal_state.message[buf_pos .. buf_pos + dest.len()];
    dest.clone_from_slice(src_slice);
}

fn reg_txt_write(dev: &mut RsaDevice, buf_pos: usize, src: &[u8]) {
    trace!("Writing {} bytes to RSA TXT at +0x{:X}", src.len(), buf_pos);
    let dst_slice = &mut dev._internal_state.message[buf_pos .. buf_pos + src.len()];
    dst_slice.clone_from_slice(src);
}

fn reg_exp_fifo_write(dev: &mut RsaDevice) {
    let cnt = RegCnt::new(dev.cnt.get());
    let keyslot = cnt.keyslot.get() as usize;
    let (slot_cnt, _) = get_keydata(dev, keyslot);

    assert_eq!(slot_cnt.key_set.get(), 0);
    assert_eq!(slot_cnt.key_prot.get(), 0);

    let write_pos = dev._internal_state.slots[keyslot].write_pos;
    if write_pos == 0 { // Just starting to update key, clear previous key
        for b in dev._internal_state.slots[keyslot].buf.iter_mut() {
            *b = 0;
        }
    }
    let word_bytes: [u8; 4] = unsafe { mem::transmute(dev.exp_fifo.get()) };
    dev._internal_state.slots[keyslot].buf[write_pos .. write_pos + 4].copy_from_slice(&word_bytes[..]);

    trace!("Writing bytes {:02X},{:02X},{:02X},{:02X} to RSA exponent FIFO!",
        word_bytes[0], word_bytes[1], word_bytes[2], word_bytes[3]);

    dev._internal_state.slots[keyslot].write_pos += 4;
}

iodevice!(RsaDevice, {
    internal_state: RsaDeviceState;
    regs: {
        0x000 => cnt: u32 { write_effect = reg_cnt_update; }
        0x0F0 => unk: u32 { }
        0x100 => slot0_cnt: u32 { write_effect = |dev: &mut RsaDevice| reg_slot_cnt_update(dev, 0); }
        0x104 => slot0_len: u32 { }
        0x110 => slot1_cnt: u32 { write_effect = |dev: &mut RsaDevice| reg_slot_cnt_update(dev, 1); }
        0x114 => slot1_len: u32 { }
        0x120 => slot2_cnt: u32 { write_effect = |dev: &mut RsaDevice| reg_slot_cnt_update(dev, 2); }
        0x124 => slot2_len: u32 { }
        0x130 => slot3_cnt: u32 { write_effect = |dev: &mut RsaDevice| reg_slot_cnt_update(dev, 3); }
        0x134 => slot3_len: u32 { }
        0x200 => exp_fifo: u32 { write_effect = reg_exp_fifo_write; }
    }
    ranges: {
        0x204;0xFC => { } // A bug in bootrom causes it to write all over this area
        0x400;0x100 => {
            read_effect = reg_mod_read;
            write_effect = reg_mod_write;
        }
        0x800;0x100 => {
            read_effect = reg_txt_read;
            write_effect = reg_txt_write;
        }
    }
});
<|endoftext|>


<commit_before>use document;
use document::YamlNodeData;
use ffi;
use parser::YamlMark;

use std::from_str::from_str;
use std::result;
use std::int;
use std::f64;
use std::u32;
use std::char;
use std::strbuf::StrBuf;

pub trait YamlConstructor<T, E> {
    fn construct_scalar(&self, scalar: document::YamlScalarData) -> Result<T, E>;
    fn construct_sequence(&self, sequence: document::YamlSequenceData) -> Result<T, E>;
    fn construct_mapping(&self, mapping: document::YamlMappingData) -> Result<T, E>;

    fn construct<'r>(&self, node: document::YamlNode<'r>) -> Result<T, E> {
        match node {
            document::YamlScalarNode(scalar) => self.construct_scalar(scalar),
            document::YamlSequenceNode(sequence) => self.construct_sequence(sequence),
            document::YamlMappingNode(mapping) => self.construct_mapping(mapping)
        }
    }
}

#[deriving(Eq)]
#[deriving(Show)]
pub enum YamlStandardData {
    YamlInteger(int),
    YamlFloat(f64),
    YamlString(~str),
    YamlNull,
    YamlBool(bool),
    YamlSequence(~[YamlStandardData]),
    YamlMapping(~[(YamlStandardData, YamlStandardData)]),
}

pub struct YamlStandardConstructor;

impl YamlStandardConstructor {
    pub fn new() -> YamlStandardConstructor {
        YamlStandardConstructor
    }

    pub fn parse_double_quoted(value: &str, mark: &YamlMark) -> Result<~str, ~str> {
        let mut buf = StrBuf::new();
        let mut it = value.chars();

        loop {
            match it.next() {
                None => return Ok(buf.into_owned()),
                Some('\\') => {
                    // escape sequences
                    match it.next() {
                        None => return Err(format!("invalid escape sequence at line {:u}, col {:u}", mark.line, mark.column)),
                        Some('0') => buf.push_char('\x00'),
                        Some('a') => buf.push_char('\x07'),
                        Some('b') => buf.push_char('\x08'),
                        Some('t') | Some('\t') => buf.push_char('\t'),
                        Some('n') => buf.push_char('\n'),
                        Some('v') => buf.push_char('\x0b'),
                        Some('f') => buf.push_char('\x0c'),
                        Some('r') => buf.push_char('\x0d'),
                        Some('e') => buf.push_char('\x1b'),
                        Some('N') => buf.push_char('\x85'),
                        Some('_') => buf.push_char('\xa0'),
                        Some('L') => buf.push_char('\u2028'),
                        Some('P') => buf.push_char('\u2029'),
                        Some('x') => {
                            let code:~str = it.take(2).collect();
                            match parse_escape_sequence(code, 2) {
                                Some(c) => buf.push_char(c),
                                None => return Err(format!("invalid x escape sequence at line {:u}, col {:u}", mark.line, mark.column))
                            }
                        },
                        Some('u') => {
                            let code:~str = it.take(4).collect();
                            match parse_escape_sequence(code, 4) {
                                Some(c) => buf.push_char(c),
                                None => return Err(format!("invalid x escape sequence at line {:u}, col {:u}", mark.line, mark.column))
                            }
                        },
                        Some('U') => {
                            let code:~str = it.take(8).collect();
                            match parse_escape_sequence(code, 8) {
                                Some(c) => buf.push_char(c),
                                None => return Err(format!("invalid x escape sequence at line {:u}, col {:u}", mark.line, mark.column))
                            }
                        },
                        Some(c) => buf.push_char(c)
                    }
                },
                Some(c) => buf.push_char(c)
            }
        }
    }
}

fn parse_escape_sequence(rep: ~str, expected_len: uint) -> Option<char> {
    match u32::parse_bytes(rep.as_bytes(), 16) {
        Some(code) if rep.len() == expected_len => char::from_u32(code),
        _ => None
    }
}

impl YamlConstructor<YamlStandardData, ~str> for YamlStandardConstructor {
    fn construct_scalar(&self, scalar: document::YamlScalarData) -> Result<YamlStandardData, ~str> {
        let dec_int = regex!(r"^[-+]?[0-9]+$");
        let oct_int = regex!(r"^0o[0-7]+$");
        let hex_int = regex!(r"^0x[0-9a-fA-F]+$");
        let float_pattern = regex!(r"^[-+]?(\.[0-9]+|[0-9]+(\.[0-9]*)?)([eE][-+]?[0-9]+)?$");
        let pos_inf = regex!(r"^[+]?(\.inf|\.Inf|\.INF)$");
        let neg_inf = regex!(r"^-(\.inf|\.Inf|\.INF)$");
        let nan = regex!(r"^(\.nan|\.NaN|\.NAN)$");
        let null = regex!(r"^(null|Null|NULL|~)$");
        let true_pattern = regex!(r"^(true|True|TRUE|yes|Yes|YES)$");
        let false_pattern = regex!(r"^(false|False|FALSE|no|No|NO)$");

        let value = scalar.get_value();
        let mark = scalar.start_mark();

        match scalar.style() {
            ffi::YamlPlainScalarStyle => {
                if dec_int.is_match(value) {
                    Ok(YamlInteger(from_str(value).unwrap()))
                } else if oct_int.is_match(value) {
                    let num_part = value.slice_from(2);
                    Ok(YamlInteger(int::parse_bytes(num_part.as_bytes(), 8).unwrap()))
                } else if hex_int.is_match(value) {
                    let num_part = value.slice_from(2);
                    Ok(YamlInteger(int::parse_bytes(num_part.as_bytes(), 16).unwrap()))
                } else if float_pattern.is_match(value) {
                    Ok(YamlFloat(from_str(value).unwrap()))
                } else if pos_inf.is_match(value) {
                    Ok(YamlFloat(f64::INFINITY))
                } else if neg_inf.is_match(value) {
                    Ok(YamlFloat(f64::NEG_INFINITY))
                } else if nan.is_match(value) {
                    Ok(YamlFloat(f64::NAN))
                } else if null.is_match(value) {
                    Ok(YamlNull)
                } else if true_pattern.is_match(value) {
                    Ok(YamlBool(true))
                } else if false_pattern.is_match(value) {
                    Ok(YamlBool(false))
                } else {
                    Ok(YamlString(value))
                }
            },
            ffi::YamlDoubleQuotedScalarStyle => {
                YamlStandardConstructor::parse_double_quoted(value, &mark).map(YamlString)
            },
            _ => {
                Ok(YamlString(value))
            }
        }
    }

    fn construct_sequence(&self, sequence: document::YamlSequenceData) -> Result<YamlStandardData, ~str> {
        result::collect(sequence.values().map(|node| { self.construct(node) })).map(|list| YamlSequence(list))
    }

    fn construct_mapping(&self, mapping: document::YamlMappingData) -> Result<YamlStandardData, ~str> {
        let pairs = mapping.pairs().map(|(key_node, value_node)| {
            match self.construct(key_node) {
                Ok(key) => match self.construct(value_node) {
                    Ok(value) => Ok((key, value)),
                    Err(e) => Err(e)
                },
                Err(e) => Err(e)
            }
        });
        result::collect(pairs).map(YamlMapping)
    }
}

#[cfg(test)]
mod test {
    use parser::{YamlParser, YamlByteParser};
    use constructor::*;
    use std::f64;

    #[test]
    fn test_standard_constructor() {
        let data = "[1, 2, 3]";
        let parser = YamlByteParser::init(data.as_bytes());

        match parser.load().next_document() {
            Ok(doc) => {
                let ctor = YamlStandardConstructor::new();
                assert_eq!(Ok(YamlSequence(~[YamlInteger(1), YamlInteger(2), YamlInteger(3)])), ctor.construct(doc.root()))
            }
            Err(err) => fail!("{:?}", err)
        }
    }

    #[test]
    fn test_integer_parser() {
        let data = "[0o10, 0x21, -30]";
        let parser = YamlByteParser::init(data.as_bytes());

        match parser.load().next_document() {
            Ok(doc) => {
                let ctor = YamlStandardConstructor::new();
                assert_eq!(Ok(YamlSequence(~[YamlInteger(0o10), YamlInteger(0x21), YamlInteger(-30)])), ctor.construct(doc.root()))
            }
            Err(err) => fail!("{:?}", err)
        }
    }

    #[test]
    fn test_float_parser() {
        let data = "[0.3, -.4, 1e+2, -1.2e-3]";
        let parser = YamlByteParser::init(data.as_bytes());

        match parser.load().next_document() {
            Ok(doc) => {
                let ctor = YamlStandardConstructor::new();
                let value = ctor.construct(doc.root());
                match value {
                    Ok(YamlSequence(seq)) => {
                        match seq.as_slice() {
                            [YamlFloat(f1), YamlFloat(f2), YamlFloat(f3), YamlFloat(f4)] => {
                                assert!((f1 - 0.3).abs() < 1.0e-6);
                                assert!((f2 + 0.4).abs() < 1.0e-6);
                                assert!((f3 - 1e+2).abs() < 1.0e-6);
                                assert!((f4 + 1.2e-3).abs() < 1.0e-6);
                            },
                            _ => fail!("unexpected sequence: {:?}", seq)
                        }
                    },
                    _ => fail!("unexpected result: {:?}", value)
                }
            }
            Err(err) => fail!("document parse failure: {:?}", err)
        }
    }

    #[test]
    fn test_inf_parser() {
        let data = "[.inf, -.INF]";
        let parser = YamlByteParser::init(data.as_bytes());

        match parser.load().next_document() {
            Ok(doc) => {
                let ctor = YamlStandardConstructor::new();
                assert_eq!(Ok(YamlSequence(~[YamlFloat(f64::INFINITY), YamlFloat(f64::NEG_INFINITY)])), ctor.construct(doc.root()))
            }
            Err(err) => fail!("document parse failure: {:?}", err)
        }
    }

    #[test]
    fn test_misc_parser() {
        let data = "[yes, False, ~]";
        let parser = YamlByteParser::init(data.as_bytes());

        match parser.load().next_document() {
            Ok(doc) => {
                let ctor = YamlStandardConstructor::new();
                assert_eq!(Ok(YamlSequence(~[YamlBool(true), YamlBool(false), YamlNull])), ctor.construct(doc.root()))
            }
            Err(err) => fail!("document parse failure: {:?}", err)
        }
    }

    #[test]
    fn test_double_quoted_parser() {
        let data = r#""hello, \"world\"""#;
        let parser = YamlByteParser::init(data.as_bytes());

        match parser.load().next_document() {
            Ok(doc) => {
                let ctor = YamlStandardConstructor::new();
                assert_eq!(Ok(YamlString("hello, \"world\"".to_owned())), ctor.construct(doc.root()))
            }
            Err(err) => fail!("document parse failure: {:?}", err)
        }
    }
}
<commit_msg>Single quoted scalar test<commit_after>use document;
use document::YamlNodeData;
use ffi;
use parser::YamlMark;

use std::from_str::from_str;
use std::result;
use std::int;
use std::f64;
use std::u32;
use std::char;
use std::strbuf::StrBuf;

pub trait YamlConstructor<T, E> {
    fn construct_scalar(&self, scalar: document::YamlScalarData) -> Result<T, E>;
    fn construct_sequence(&self, sequence: document::YamlSequenceData) -> Result<T, E>;
    fn construct_mapping(&self, mapping: document::YamlMappingData) -> Result<T, E>;

    fn construct<'r>(&self, node: document::YamlNode<'r>) -> Result<T, E> {
        match node {
            document::YamlScalarNode(scalar) => self.construct_scalar(scalar),
            document::YamlSequenceNode(sequence) => self.construct_sequence(sequence),
            document::YamlMappingNode(mapping) => self.construct_mapping(mapping)
        }
    }
}

#[deriving(Eq)]
#[deriving(Show)]
pub enum YamlStandardData {
    YamlInteger(int),
    YamlFloat(f64),
    YamlString(~str),
    YamlNull,
    YamlBool(bool),
    YamlSequence(~[YamlStandardData]),
    YamlMapping(~[(YamlStandardData, YamlStandardData)]),
}

pub struct YamlStandardConstructor;

impl YamlStandardConstructor {
    pub fn new() -> YamlStandardConstructor {
        YamlStandardConstructor
    }

    pub fn parse_double_quoted(value: &str, mark: &YamlMark) -> Result<~str, ~str> {
        let mut buf = StrBuf::new();
        let mut it = value.chars();

        loop {
            match it.next() {
                None => return Ok(buf.into_owned()),
                Some('\\') => {
                    // escape sequences
                    match it.next() {
                        None => return Err(format!("invalid escape sequence at line {:u}, col {:u}", mark.line, mark.column)),
                        Some('0') => buf.push_char('\x00'),
                        Some('a') => buf.push_char('\x07'),
                        Some('b') => buf.push_char('\x08'),
                        Some('t') | Some('\t') => buf.push_char('\t'),
                        Some('n') => buf.push_char('\n'),
                        Some('v') => buf.push_char('\x0b'),
                        Some('f') => buf.push_char('\x0c'),
                        Some('r') => buf.push_char('\x0d'),
                        Some('e') => buf.push_char('\x1b'),
                        Some('N') => buf.push_char('\x85'),
                        Some('_') => buf.push_char('\xa0'),
                        Some('L') => buf.push_char('\u2028'),
                        Some('P') => buf.push_char('\u2029'),
                        Some('x') => {
                            let code:~str = it.take(2).collect();
                            match parse_escape_sequence(code, 2) {
                                Some(c) => buf.push_char(c),
                                None => return Err(format!("invalid x escape sequence at line {:u}, col {:u}", mark.line, mark.column))
                            }
                        },
                        Some('u') => {
                            let code:~str = it.take(4).collect();
                            match parse_escape_sequence(code, 4) {
                                Some(c) => buf.push_char(c),
                                None => return Err(format!("invalid x escape sequence at line {:u}, col {:u}", mark.line, mark.column))
                            }
                        },
                        Some('U') => {
                            let code:~str = it.take(8).collect();
                            match parse_escape_sequence(code, 8) {
                                Some(c) => buf.push_char(c),
                                None => return Err(format!("invalid x escape sequence at line {:u}, col {:u}", mark.line, mark.column))
                            }
                        },
                        Some(c) => buf.push_char(c)
                    }
                },
                Some(c) => buf.push_char(c)
            }
        }
    }
}

fn parse_escape_sequence(rep: ~str, expected_len: uint) -> Option<char> {
    match u32::parse_bytes(rep.as_bytes(), 16) {
        Some(code) if rep.len() == expected_len => char::from_u32(code),
        _ => None
    }
}

impl YamlConstructor<YamlStandardData, ~str> for YamlStandardConstructor {
    fn construct_scalar(&self, scalar: document::YamlScalarData) -> Result<YamlStandardData, ~str> {
        let dec_int = regex!(r"^[-+]?[0-9]+$");
        let oct_int = regex!(r"^0o[0-7]+$");
        let hex_int = regex!(r"^0x[0-9a-fA-F]+$");
        let float_pattern = regex!(r"^[-+]?(\.[0-9]+|[0-9]+(\.[0-9]*)?)([eE][-+]?[0-9]+)?$");
        let pos_inf = regex!(r"^[+]?(\.inf|\.Inf|\.INF)$");
        let neg_inf = regex!(r"^-(\.inf|\.Inf|\.INF)$");
        let nan = regex!(r"^(\.nan|\.NaN|\.NAN)$");
        let null = regex!(r"^(null|Null|NULL|~)$");
        let true_pattern = regex!(r"^(true|True|TRUE|yes|Yes|YES)$");
        let false_pattern = regex!(r"^(false|False|FALSE|no|No|NO)$");

        let value = scalar.get_value();
        let mark = scalar.start_mark();

        match scalar.style() {
            ffi::YamlPlainScalarStyle => {
                if dec_int.is_match(value) {
                    Ok(YamlInteger(from_str(value).unwrap()))
                } else if oct_int.is_match(value) {
                    let num_part = value.slice_from(2);
                    Ok(YamlInteger(int::parse_bytes(num_part.as_bytes(), 8).unwrap()))
                } else if hex_int.is_match(value) {
                    let num_part = value.slice_from(2);
                    Ok(YamlInteger(int::parse_bytes(num_part.as_bytes(), 16).unwrap()))
                } else if float_pattern.is_match(value) {
                    Ok(YamlFloat(from_str(value).unwrap()))
                } else if pos_inf.is_match(value) {
                    Ok(YamlFloat(f64::INFINITY))
                } else if neg_inf.is_match(value) {
                    Ok(YamlFloat(f64::NEG_INFINITY))
                } else if nan.is_match(value) {
                    Ok(YamlFloat(f64::NAN))
                } else if null.is_match(value) {
                    Ok(YamlNull)
                } else if true_pattern.is_match(value) {
                    Ok(YamlBool(true))
                } else if false_pattern.is_match(value) {
                    Ok(YamlBool(false))
                } else {
                    Ok(YamlString(value))
                }
            },
            ffi::YamlDoubleQuotedScalarStyle => {
                YamlStandardConstructor::parse_double_quoted(value, &mark).map(YamlString)
            },
            _ => {
                Ok(YamlString(value))
            }
        }
    }

    fn construct_sequence(&self, sequence: document::YamlSequenceData) -> Result<YamlStandardData, ~str> {
        result::collect(sequence.values().map(|node| { self.construct(node) })).map(|list| YamlSequence(list))
    }

    fn construct_mapping(&self, mapping: document::YamlMappingData) -> Result<YamlStandardData, ~str> {
        let pairs = mapping.pairs().map(|(key_node, value_node)| {
            match self.construct(key_node) {
                Ok(key) => match self.construct(value_node) {
                    Ok(value) => Ok((key, value)),
                    Err(e) => Err(e)
                },
                Err(e) => Err(e)
            }
        });
        result::collect(pairs).map(YamlMapping)
    }
}

#[cfg(test)]
mod test {
    use parser::{YamlParser, YamlByteParser};
    use constructor::*;
    use std::f64;

    #[test]
    fn test_standard_constructor() {
        let data = "[1, 2, 3]";
        let parser = YamlByteParser::init(data.as_bytes());

        match parser.load().next_document() {
            Ok(doc) => {
                let ctor = YamlStandardConstructor::new();
                assert_eq!(Ok(YamlSequence(~[YamlInteger(1), YamlInteger(2), YamlInteger(3)])), ctor.construct(doc.root()))
            }
            Err(err) => fail!("{:?}", err)
        }
    }

    #[test]
    fn test_integer_parser() {
        let data = "[0o10, 0x21, -30]";
        let parser = YamlByteParser::init(data.as_bytes());

        match parser.load().next_document() {
            Ok(doc) => {
                let ctor = YamlStandardConstructor::new();
                assert_eq!(Ok(YamlSequence(~[YamlInteger(0o10), YamlInteger(0x21), YamlInteger(-30)])), ctor.construct(doc.root()))
            }
            Err(err) => fail!("{:?}", err)
        }
    }

    #[test]
    fn test_float_parser() {
        let data = "[0.3, -.4, 1e+2, -1.2e-3]";
        let parser = YamlByteParser::init(data.as_bytes());

        match parser.load().next_document() {
            Ok(doc) => {
                let ctor = YamlStandardConstructor::new();
                let value = ctor.construct(doc.root());
                match value {
                    Ok(YamlSequence(seq)) => {
                        match seq.as_slice() {
                            [YamlFloat(f1), YamlFloat(f2), YamlFloat(f3), YamlFloat(f4)] => {
                                assert!((f1 - 0.3).abs() < 1.0e-6);
                                assert!((f2 + 0.4).abs() < 1.0e-6);
                                assert!((f3 - 1e+2).abs() < 1.0e-6);
                                assert!((f4 + 1.2e-3).abs() < 1.0e-6);
                            },
                            _ => fail!("unexpected sequence: {:?}", seq)
                        }
                    },
                    _ => fail!("unexpected result: {:?}", value)
                }
            }
            Err(err) => fail!("document parse failure: {:?}", err)
        }
    }

    #[test]
    fn test_inf_parser() {
        let data = "[.inf, -.INF]";
        let parser = YamlByteParser::init(data.as_bytes());

        match parser.load().next_document() {
            Ok(doc) => {
                let ctor = YamlStandardConstructor::new();
                assert_eq!(Ok(YamlSequence(~[YamlFloat(f64::INFINITY), YamlFloat(f64::NEG_INFINITY)])), ctor.construct(doc.root()))
            }
            Err(err) => fail!("document parse failure: {:?}", err)
        }
    }

    #[test]
    fn test_misc_parser() {
        let data = "[yes, False, ~]";
        let parser = YamlByteParser::init(data.as_bytes());

        match parser.load().next_document() {
            Ok(doc) => {
                let ctor = YamlStandardConstructor::new();
                assert_eq!(Ok(YamlSequence(~[YamlBool(true), YamlBool(false), YamlNull])), ctor.construct(doc.root()))
            }
            Err(err) => fail!("document parse failure: {:?}", err)
        }
    }

    #[test]
    fn test_double_quoted_parser() {
        let data = r#""hello, \"world\"""#;
        let parser = YamlByteParser::init(data.as_bytes());

        match parser.load().next_document() {
            Ok(doc) => {
                let ctor = YamlStandardConstructor::new();
                assert_eq!(Ok(YamlString("hello, \"world\"".to_owned())), ctor.construct(doc.root()))
            }
            Err(err) => fail!("document parse failure: {:?}", err)
        }
    }

    #[test]
    fn test_single_quoted_parser() {
        let data = r#"'here''s to "quotes"'"#;
        let parser = YamlByteParser::init(data.as_bytes());

        match parser.load().next_document() {
            Ok(doc) => {
                let ctor = YamlStandardConstructor::new();
                assert_eq!(Ok(YamlString(r#"here's to "quotes""#.to_owned())), ctor.construct(doc.root()))
            }
            Err(err) => fail!("document parse failure: {:?}", err)
        }
    }
}
<|endoftext|>


<commit_before>// Copyright 2022 The Chromium OS Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

use std::cell::RefCell;
use std::collections::BTreeMap;
use std::fs::{File, OpenOptions};
use std::os::unix::fs::FileExt;
use std::rc::Rc;

use anyhow::Context;

use arch::{MsrAction, MsrConfig, MsrExitHandler, MsrExitHandlerError, MsrRWType, MsrValueFrom};
use base::{debug, error};

use remain::sorted;
use thiserror::Error as ThisError;

#[sorted]
#[derive(ThisError, Debug)]
pub enum Error {
    #[error("Unable to open host msr file: {0}")]
    HostMsrGetError(anyhow::Error),
    #[error("Unable to get metadata of dev file for msr: {0}")]
    HostMsrGetMetadataError(std::io::Error),
    #[error("Unable to read host msr: {0}")]
    HostMsrReadError(std::io::Error),
    #[error("Unable to set permissions of dev file for msr: {0}")]
    HostMsrSetPermsError(std::io::Error),
    #[error("Unable to write host msr: {0}")]
    HostMsrWriteError(std::io::Error),
    #[error("Not set msr action parameter")]
    InvalidAction,
}

pub type Result<T> = std::result::Result<T, Error>;

/// Wrap for userspace MSR file descriptor (/dev/cpu/*/msr).
pub struct MsrDevFile {
    dev_msr: File,
}

impl MsrDevFile {
    /// Create a new MSR file descriptor.
    ///
    /// "Passthrough" handler will create file descriptor with both read and write
    /// permissions. MsrHandlers controls read/write with MsrRWType. This avoids
    /// the corner case that some MSRs are read-only while other MSRs need write
    /// permission.
    /// "Emulate" handler will create read-only file descriptor. This read-only
    /// descriptor will only be used once to initialize MSR value and "Emulate"
    /// handler won't store its descriptor at MsrHandlers level.
    fn new(cpu_id: usize, read_only: bool) -> Result<Self> {
        let filename = format!("/dev/cpu/{}/msr", cpu_id);
        let dev_msr = OpenOptions::new()
            .read(true)
            .write(!read_only)
            .open(&filename)
            .context(format!("Cannot open {}, are you root?", filename))
            .map_err(Error::HostMsrGetError)?;
        Ok(MsrDevFile { dev_msr })
    }

    fn read(&self, index: u32) -> Result<u64> {
        let mut data = [0; 8];
        self.dev_msr
            .read_exact_at(&mut data, index.into())
            .map_err(Error::HostMsrReadError)?;
        Ok(u64::from_ne_bytes(data))
    }

    // In fact, only "passthrough" will write into MSR file.
    fn write(&self, index: u32, data: u64) -> Result<()> {
        self.dev_msr
            .write_all_at(&data.to_ne_bytes(), index.into())
            .map_err(Error::HostMsrWriteError)?;
        Ok(())
    }
}

/// Wrap for general RDMSR/WRMSR handling.
///
/// Each specific handler needs to implement this trait.
pub trait MsrHandling {
    fn read(&self) -> Result<u64>;
    // For "emulate" handler, it need to update MSR value which is stored in
    // `msr_data` of MsrEmulate. So declare `self` as mutable.
    fn write(&mut self, data: u64) -> Result<()>;
}

/// MsrPassthroughHandler - passthrough handler that will handle RDMSR/WRMSR
///                         by reading/writing MSR file directly.
/// For RDMSR, this handler will give Guest the current MSR value on Host.
/// For WRMSR, this handler will directly pass the change desired by the Guest
/// to the host, and expect the change to take effect on the MSR of the host.
struct MsrPassthroughHandler {
    /// MSR index.
    index: u32,
    /// MSR source CPU, CPU 0 or running CPU.
    from: MsrValueFrom,
    /// Reference of MSR file descriptors.
    msr_file: Rc<RefCell<BTreeMap<usize, Rc<MsrDevFile>>>>,
}

impl MsrPassthroughHandler {
    fn new(
        index: u32,
        msr_config: &MsrConfig,
        msr_file: &Rc<RefCell<BTreeMap<usize, Rc<MsrDevFile>>>>,
    ) -> Result<Self> {
        let pass = MsrPassthroughHandler {
            index,
            from: msr_config.from,
            msr_file: Rc::clone(msr_file), // Clone first, and then modify it.
        };
        pass.get_msr_dev()?;
        Ok(pass)
    }

    /// A helper interface to get MSR file descriptor.
    fn get_msr_dev(&self) -> Result<Rc<MsrDevFile>> {
        let cpu_id = self.from.get_cpu_id();
        let mut msr_file = self.msr_file.borrow_mut();
        // First, check if the descriptor is stored before.
        if let Some(dev_msr) = msr_file.get(&cpu_id) {
            Ok(Rc::clone(dev_msr))
        } else {
            // If descriptor isn't found, create new one.
            let new_dev_msr = Rc::new(MsrDevFile::new(cpu_id, false)?);
            // Note: For MsrValueFrom::RWFromRunningCPU case, just store
            // the new descriptor and don't remove the previous.
            // This is for convenience, since the most decriptor number is
            // same as Host CPU count.
            msr_file.insert(cpu_id, Rc::clone(&new_dev_msr));
            Ok(new_dev_msr)
        }
    }
}

impl MsrHandling for MsrPassthroughHandler {
    fn read(&self) -> Result<u64> {
        let index = self.index;
        self.get_msr_dev()?.read(index)
    }

    fn write(&mut self, data: u64) -> Result<()> {
        let index = self.index;
        self.get_msr_dev()?.write(index, data)
    }
}

/// MsrPassthroughHandler - emulate handler that will handle RDMSR/WRMSR
///                         with a dummy MSR value other than access to real
///                         MSR.
/// This Handler will initialize a value(`msr_data`) with the corresponding
/// Host MSR value, then handle the RDMSR/WRMSR based on this "value".
///
/// For RDMSR, this handler will give Guest the stored `msr_data`.
/// For WRMSR, this handler will directly change `msr_data` without the
/// modification on real Host MSR. The change will not take effect on the
/// real MSR of Host.
///
/// 'emulate' Handler is used in the case, that some driver need to control
/// MSR and user just wants to make WRMSR successful and doesn't care about
/// if WRMSR really works. This handlers make Guest's control of CPU not
/// affect the host
struct MsrEmulateHandler {
    /// Only initialize msr_data with MSR source pCPU, and will not update
    /// msr value changes on host cpu into msr_data.
    msr_data: u64,
}

impl MsrEmulateHandler {
    fn new(
        index: u32,
        msr_config: &MsrConfig,
        msr_file: &Rc<RefCell<BTreeMap<usize, Rc<MsrDevFile>>>>,
    ) -> Result<Self> {
        let cpu_id = msr_config.from.get_cpu_id();
        let msr_file_map = msr_file.borrow();
        let dev_msr = msr_file_map.get(&cpu_id);

        let msr_data: u64 = if dev_msr.is_some() {
            dev_msr.unwrap().read(index)?
        } else {
            // Don't allow to write. Only read the value to initialize
            // `msr_data` and won't store in MsrHandlers level.
            MsrDevFile::new(cpu_id, true)?.read(index)?
        };

        Ok(MsrEmulateHandler { msr_data })
    }
}

impl MsrHandling for MsrEmulateHandler {
    fn read(&self) -> Result<u64> {
        Ok(self.msr_data)
    }

    fn write(&mut self, data: u64) -> Result<()> {
        self.msr_data = data;
        Ok(())
    }
}

/// MSR handler configuration. Per-cpu.
#[derive(Default)]
pub struct MsrHandlers {
    /// Store read/write permissions to control read/write brfore calling
    /// MsrHandling trait.
    pub handler: BTreeMap<u32, (MsrRWType, Rc<RefCell<Box<dyn MsrHandling>>>)>,
    /// Store file descriptor here to avoid cache duplicate descriptors
    /// for each MSR.
    /// Only collect descriptor of 'passthrough' handler, since 'emulate'
    /// uses descriptor only once during initialization.
    pub msr_file: Option<Rc<RefCell<BTreeMap<usize, Rc<MsrDevFile>>>>>,
}

impl MsrExitHandler for MsrHandlers {
    fn read(&self, index: u32) -> Option<u64> {
        if let Some((rw_type, handler)) = self.handler.get(&index) {
            // It's not error. This means user does't want to handle
            // RDMSR. Just log it.
            if !rw_type.read_allow {
                debug!("RDMSR is not allowed for msr: {:#x}", index);
                return None;
            }

            match handler.borrow().read() {
                Ok(data) => Some(data),
                Err(e) => {
                    error!("MSR host read failed {:#x} {:?}", index, e);
                    None
                }
            }
        } else {
            None
        }
    }

    fn write(&self, index: u32, data: u64) -> Option<()> {
        if let Some((rw_type, handler)) = self.handler.get(&index) {
            // It's not error. This means user does't want to handle
            // WRMSR. Just log it.
            if !rw_type.write_allow {
                debug!("WRMSR is not allowed for msr: {:#x}", index);
                return None;
            }

            match handler.borrow_mut().write(data) {
                Ok(_) => Some(()),
                Err(e) => {
                    error!("MSR host write failed {:#x} {:?}", index, e);
                    None
                }
            }
        } else {
            None
        }
    }

    fn add_handler(
        &mut self,
        index: u32,
        msr_config: MsrConfig,
        cpu_id: usize,
    ) -> std::result::Result<(), MsrExitHandlerError> {
        if msr_config.action.is_none() {
            return Err(MsrExitHandlerError::InvalidParam);
        }

        let msr_file = Rc::new(RefCell::new(BTreeMap::new()));
        match msr_config.action.as_ref().unwrap() {
            MsrAction::MsrPassthrough => {
                let msr_handler: Rc<RefCell<Box<dyn MsrHandling>>> =
                    match MsrPassthroughHandler::new(index, &msr_config, &msr_file) {
                        Ok(r) => Rc::new(RefCell::new(Box::new(r))),
                        Err(e) => {
                            error!(
                                "failed to create MSR passthrough handler for vcpu {}: {:#}",
                                cpu_id, e
                            );
                            return Err(MsrExitHandlerError::HandlerCreateFailed);
                        }
                    };
                self.handler
                    .insert(index, (msr_config.rw_type, msr_handler));
            }
            MsrAction::MsrEmulate => {
                let msr_handler: Rc<RefCell<Box<dyn MsrHandling>>> =
                    match MsrEmulateHandler::new(index, &msr_config, &msr_file) {
                        Ok(r) => Rc::new(RefCell::new(Box::new(r))),
                        Err(e) => {
                            error!(
                                "failed to create MSR emulate handler for vcpu {}: {:#}",
                                cpu_id, e
                            );
                            return Err(MsrExitHandlerError::HandlerCreateFailed);
                        }
                    };
                self.handler
                    .insert(index, (msr_config.rw_type, msr_handler));
            }
        };
        // Empty only when no 'passthrough' handler exists.
        if !msr_file.borrow().is_empty() {
            self.msr_file = Some(msr_file);
        }
        Ok(())
    }
}
<commit_msg>x86_64: Fix File descriptor sharing in MSR handler<commit_after>// Copyright 2022 The Chromium OS Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

use std::cell::RefCell;
use std::collections::BTreeMap;
use std::fs::{File, OpenOptions};
use std::os::unix::fs::FileExt;
use std::rc::Rc;

use anyhow::Context;

use arch::{MsrAction, MsrConfig, MsrExitHandler, MsrExitHandlerError, MsrRWType, MsrValueFrom};
use base::{debug, error};

use remain::sorted;
use thiserror::Error as ThisError;

#[sorted]
#[derive(ThisError, Debug)]
pub enum Error {
    #[error("Unable to open host msr file: {0}")]
    HostMsrGetError(anyhow::Error),
    #[error("Unable to get metadata of dev file for msr: {0}")]
    HostMsrGetMetadataError(std::io::Error),
    #[error("Unable to read host msr: {0}")]
    HostMsrReadError(std::io::Error),
    #[error("Unable to set permissions of dev file for msr: {0}")]
    HostMsrSetPermsError(std::io::Error),
    #[error("Unable to write host msr: {0}")]
    HostMsrWriteError(std::io::Error),
    #[error("Not set msr action parameter")]
    InvalidAction,
}

pub type Result<T> = std::result::Result<T, Error>;

/// Wrap for userspace MSR file descriptor (/dev/cpu/*/msr).
pub struct MsrDevFile {
    dev_msr: File,
}

impl MsrDevFile {
    /// Create a new MSR file descriptor.
    ///
    /// "Passthrough" handler will create file descriptor with both read and write
    /// permissions. MsrHandlers controls read/write with MsrRWType. This avoids
    /// the corner case that some MSRs are read-only while other MSRs need write
    /// permission.
    /// "Emulate" handler will create read-only file descriptor. This read-only
    /// descriptor will only be used once to initialize MSR value and "Emulate"
    /// handler won't store its descriptor at MsrHandlers level.
    fn new(cpu_id: usize, read_only: bool) -> Result<Self> {
        let filename = format!("/dev/cpu/{}/msr", cpu_id);
        let dev_msr = OpenOptions::new()
            .read(true)
            .write(!read_only)
            .open(&filename)
            .context(format!("Cannot open {}, are you root?", filename))
            .map_err(Error::HostMsrGetError)?;
        Ok(MsrDevFile { dev_msr })
    }

    fn read(&self, index: u32) -> Result<u64> {
        let mut data = [0; 8];
        self.dev_msr
            .read_exact_at(&mut data, index.into())
            .map_err(Error::HostMsrReadError)?;
        Ok(u64::from_ne_bytes(data))
    }

    // In fact, only "passthrough" will write into MSR file.
    fn write(&self, index: u32, data: u64) -> Result<()> {
        self.dev_msr
            .write_all_at(&data.to_ne_bytes(), index.into())
            .map_err(Error::HostMsrWriteError)?;
        Ok(())
    }
}

/// Wrap for general RDMSR/WRMSR handling.
///
/// Each specific handler needs to implement this trait.
pub trait MsrHandling {
    fn read(&self) -> Result<u64>;
    // For "emulate" handler, it need to update MSR value which is stored in
    // `msr_data` of MsrEmulate. So declare `self` as mutable.
    fn write(&mut self, data: u64) -> Result<()>;
}

/// MsrPassthroughHandler - passthrough handler that will handle RDMSR/WRMSR
///                         by reading/writing MSR file directly.
/// For RDMSR, this handler will give Guest the current MSR value on Host.
/// For WRMSR, this handler will directly pass the change desired by the Guest
/// to the host, and expect the change to take effect on the MSR of the host.
struct MsrPassthroughHandler {
    /// MSR index.
    index: u32,
    /// MSR source CPU, CPU 0 or running CPU.
    from: MsrValueFrom,
    /// Reference of MSR file descriptors.
    msr_file: Rc<RefCell<BTreeMap<usize, Rc<MsrDevFile>>>>,
}

impl MsrPassthroughHandler {
    fn new(
        index: u32,
        msr_config: &MsrConfig,
        msr_file: &Rc<RefCell<BTreeMap<usize, Rc<MsrDevFile>>>>,
    ) -> Result<Self> {
        let pass = MsrPassthroughHandler {
            index,
            from: msr_config.from,
            msr_file: Rc::clone(msr_file), // Clone first, and then modify it.
        };
        pass.get_msr_dev()?;
        Ok(pass)
    }

    /// A helper interface to get MSR file descriptor.
    fn get_msr_dev(&self) -> Result<Rc<MsrDevFile>> {
        let cpu_id = self.from.get_cpu_id();
        let mut msr_file = self.msr_file.borrow_mut();
        // First, check if the descriptor is stored before.
        if let Some(dev_msr) = msr_file.get(&cpu_id) {
            Ok(Rc::clone(dev_msr))
        } else {
            // If descriptor isn't found, create new one.
            let new_dev_msr = Rc::new(MsrDevFile::new(cpu_id, false)?);
            // Note: For MsrValueFrom::RWFromRunningCPU case, just store
            // the new descriptor and don't remove the previous.
            // This is for convenience, since the most decriptor number is
            // same as Host CPU count.
            msr_file.insert(cpu_id, Rc::clone(&new_dev_msr));
            Ok(new_dev_msr)
        }
    }
}

impl MsrHandling for MsrPassthroughHandler {
    fn read(&self) -> Result<u64> {
        let index = self.index;
        self.get_msr_dev()?.read(index)
    }

    fn write(&mut self, data: u64) -> Result<()> {
        let index = self.index;
        self.get_msr_dev()?.write(index, data)
    }
}

/// MsrPassthroughHandler - emulate handler that will handle RDMSR/WRMSR
///                         with a dummy MSR value other than access to real
///                         MSR.
/// This Handler will initialize a value(`msr_data`) with the corresponding
/// Host MSR value, then handle the RDMSR/WRMSR based on this "value".
///
/// For RDMSR, this handler will give Guest the stored `msr_data`.
/// For WRMSR, this handler will directly change `msr_data` without the
/// modification on real Host MSR. The change will not take effect on the
/// real MSR of Host.
///
/// 'emulate' Handler is used in the case, that some driver need to control
/// MSR and user just wants to make WRMSR successful and doesn't care about
/// if WRMSR really works. This handlers make Guest's control of CPU not
/// affect the host
struct MsrEmulateHandler {
    /// Only initialize msr_data with MSR source pCPU, and will not update
    /// msr value changes on host cpu into msr_data.
    msr_data: u64,
}

impl MsrEmulateHandler {
    fn new(
        index: u32,
        msr_config: &MsrConfig,
        msr_file: &Rc<RefCell<BTreeMap<usize, Rc<MsrDevFile>>>>,
    ) -> Result<Self> {
        let cpu_id = msr_config.from.get_cpu_id();
        let msr_file_map = msr_file.borrow();
        let dev_msr = msr_file_map.get(&cpu_id);

        let msr_data: u64 = if dev_msr.is_some() {
            dev_msr.unwrap().read(index)?
        } else {
            // Don't allow to write. Only read the value to initialize
            // `msr_data` and won't store in MsrHandlers level.
            MsrDevFile::new(cpu_id, true)?.read(index)?
        };

        Ok(MsrEmulateHandler { msr_data })
    }
}

impl MsrHandling for MsrEmulateHandler {
    fn read(&self) -> Result<u64> {
        Ok(self.msr_data)
    }

    fn write(&mut self, data: u64) -> Result<()> {
        self.msr_data = data;
        Ok(())
    }
}

/// MSR handler configuration. Per-cpu.
#[derive(Default)]
pub struct MsrHandlers {
    /// Store read/write permissions to control read/write brfore calling
    /// MsrHandling trait.
    pub handler: BTreeMap<u32, (MsrRWType, Rc<RefCell<Box<dyn MsrHandling>>>)>,
    /// Store file descriptor here to avoid cache duplicate descriptors
    /// for each MSR.
    /// Only collect descriptor of 'passthrough' handler, since 'emulate'
    /// uses descriptor only once during initialization.
    pub msr_file: Option<Rc<RefCell<BTreeMap<usize, Rc<MsrDevFile>>>>>,
}

impl MsrExitHandler for MsrHandlers {
    fn read(&self, index: u32) -> Option<u64> {
        if let Some((rw_type, handler)) = self.handler.get(&index) {
            // It's not error. This means user does't want to handle
            // RDMSR. Just log it.
            if !rw_type.read_allow {
                debug!("RDMSR is not allowed for msr: {:#x}", index);
                return None;
            }

            match handler.borrow().read() {
                Ok(data) => Some(data),
                Err(e) => {
                    error!("MSR host read failed {:#x} {:?}", index, e);
                    None
                }
            }
        } else {
            None
        }
    }

    fn write(&self, index: u32, data: u64) -> Option<()> {
        if let Some((rw_type, handler)) = self.handler.get(&index) {
            // It's not error. This means user does't want to handle
            // WRMSR. Just log it.
            if !rw_type.write_allow {
                debug!("WRMSR is not allowed for msr: {:#x}", index);
                return None;
            }

            match handler.borrow_mut().write(data) {
                Ok(_) => Some(()),
                Err(e) => {
                    error!("MSR host write failed {:#x} {:?}", index, e);
                    None
                }
            }
        } else {
            None
        }
    }

    fn add_handler(
        &mut self,
        index: u32,
        msr_config: MsrConfig,
        cpu_id: usize,
    ) -> std::result::Result<(), MsrExitHandlerError> {
        if msr_config.action.is_none() {
            return Err(MsrExitHandlerError::InvalidParam);
        }

        let new_msr_file = Rc::new(RefCell::new(BTreeMap::new()));
        let msr_file = match &self.msr_file {
            Some(old_msr_file) => old_msr_file,
            None => &new_msr_file,
        };

        match msr_config.action.as_ref().unwrap() {
            MsrAction::MsrPassthrough => {
                let msr_handler: Rc<RefCell<Box<dyn MsrHandling>>> =
                    match MsrPassthroughHandler::new(index, &msr_config, msr_file) {
                        Ok(r) => Rc::new(RefCell::new(Box::new(r))),
                        Err(e) => {
                            error!(
                                "failed to create MSR passthrough handler for vcpu {}: {:#}",
                                cpu_id, e
                            );
                            return Err(MsrExitHandlerError::HandlerCreateFailed);
                        }
                    };
                self.handler
                    .insert(index, (msr_config.rw_type, msr_handler));
            }
            MsrAction::MsrEmulate => {
                let msr_handler: Rc<RefCell<Box<dyn MsrHandling>>> =
                    match MsrEmulateHandler::new(index, &msr_config, msr_file) {
                        Ok(r) => Rc::new(RefCell::new(Box::new(r))),
                        Err(e) => {
                            error!(
                                "failed to create MSR emulate handler for vcpu {}: {:#}",
                                cpu_id, e
                            );
                            return Err(MsrExitHandlerError::HandlerCreateFailed);
                        }
                    };
                self.handler
                    .insert(index, (msr_config.rw_type, msr_handler));
            }
        };
        // Empty only when no 'passthrough' handler exists.
        if self.msr_file.is_none() && !msr_file.borrow().is_empty() {
            self.msr_file = Some(new_msr_file);
        }
        Ok(())
    }
}
<|endoftext|>


<commit_before>use std::fs;
use std::fs::File;
use std::io;
use std::os::unix::io::AsRawFd;
use std::mem;
use std::slice::bytes::copy_memory;
use std::io::Error;
use std::io::ErrorKind::Other;

use byteorder::{LittleEndian, ByteOrder};

use nix::sys::ioctl;

use dm_ioctl as dmi;
use lv::LV;
use vg::VG;
use util::align_to;
use pv;

const DM_IOCTL: u8 = 0xfd;
const DM_CTL_PATH: &'static str= "/dev/mapper/control";

const DM_VERSION_MAJOR: u32 = 4;
const DM_VERSION_MINOR: u32 = 30;
const DM_VERSION_PATCHLEVEL: u32 = 0;

pub struct DM<'a> {
    file: File,
    vg: &'a VG,
}

impl <'a> DM<'a> {
    pub fn new(vg: &'a VG) -> io::Result<Self> {
        Ok(DM {
            file: try!(File::open(DM_CTL_PATH)),
            vg: vg,
        })
    }

    fn get_version(&self) -> io::Result<(u32, u32, u32)> {

        let mut hdr: dmi::Struct_dm_ioctl = Default::default();
        hdr.version[0] = DM_VERSION_MAJOR;
        hdr.version[1] = DM_VERSION_MINOR;
        hdr.version[2] = DM_VERSION_PATCHLEVEL;

        let op = ioctl::op_read_write(DM_IOCTL, dmi::DM_VERSION_CMD as u8,
                                      mem::size_of::<dmi::Struct_dm_ioctl>());

        match unsafe { ioctl::read_into(self.file.as_raw_fd(), op, &mut hdr) } {
            Err(_) => return Err((io::Error::last_os_error())),
            _ => {},
        };

        Ok((hdr.version[0], hdr.version[1], hdr.version[2]))
    }

    fn list_devices(&self) -> io::Result<Vec<(String, u64)>> {

        let mut devs = Vec::new();

        let mut buf = [0u8; 10240];

        let hdr: &mut dmi::Struct_dm_ioctl = unsafe {mem::transmute(&mut buf)};

        let hdr_size = mem::size_of::<dmi::Struct_dm_ioctl>();
        hdr.version[0] = DM_VERSION_MAJOR;
        hdr.version[1] = DM_VERSION_MINOR;
        hdr.version[2] = DM_VERSION_PATCHLEVEL;
        hdr.data_size = buf.len() as u32;
        hdr.data_start = hdr_size as u32;

        let op = ioctl::op_read_write(DM_IOCTL, dmi::DM_LIST_DEVICES_CMD as u8, buf.len());

        match unsafe { ioctl::read_into(self.file.as_raw_fd(), op, &mut buf) } {
            Err(_) => return Err((io::Error::last_os_error())),
            _ => {},
        };

        if (hdr.data_size - hdr_size as u32) != 0 {
            let mut result = &buf[hdr_size..];

            loop {
                let slc = slice_to_null(&result[12..]).expect("Bad data from ioctl");
                let devno = LittleEndian::read_u64(&result[..8]);
                devs.push((String::from_utf8_lossy(slc).into_owned(), devno));

                let next = LittleEndian::read_u32(&result[8..12]);
                if next == 0 { break }

                result = &result[next as usize..];
            }
        }

        Ok(devs)
    }

    fn initialize_hdr(lv: &LV, hdr: &mut dmi::Struct_dm_ioctl) -> () {
        hdr.version[0] = DM_VERSION_MAJOR;
        hdr.version[1] = DM_VERSION_MINOR;
        hdr.version[2] = DM_VERSION_PATCHLEVEL;

        // Transmute [i8; 128] to [u8; 128]
        let name_dest: &mut [u8; 128] = unsafe { mem::transmute(&mut hdr.name) };
        copy_memory(&lv.name.as_bytes(), &mut name_dest[..]);

        let uuid_dest: &mut [u8; 129] = unsafe { mem::transmute(&mut hdr.uuid) };
        copy_memory(&lv.id.as_bytes(), &mut uuid_dest[..]);
    }

    fn create_device(&self, lv: &mut LV) -> io::Result<()> {
        let mut hdr: dmi::Struct_dm_ioctl = Default::default();

        Self::initialize_hdr(lv, &mut hdr);

        let op = ioctl::op_read_write(DM_IOCTL, dmi::DM_DEV_CREATE_CMD as u8,
                                      mem::size_of::<dmi::Struct_dm_ioctl>());

        match unsafe { ioctl::read_into(self.file.as_raw_fd(), op, &mut hdr) } {
            Err(_) => return Err((io::Error::last_os_error())),
            _ => { }
        };

        lv.device = Some(pv::Device::from(hdr.dev as i64));

        Ok(())
    }

    fn load_device(&self, lv: &LV) -> io::Result<()> {
        let mut buf = [0u8; 10240];
        let hdr_len = mem::size_of::<dmi::Struct_dm_ioctl>();
        let sectors_per_extent = self.vg.extent_size;
        let mut targets_len = 0;
        let mut target_count = 0;

        // Fill in targets
        {
            let mut data = &mut buf[hdr_len..];

            for seg in &lv.segments {
                for &(ref pvname, ref loc) in &seg.stripes {
                    let err = || Error::new(Other, "dm load_device error");
                    let pv = try!(self.vg.pvs.get(pvname).ok_or(err()));

                    let params = format!("{}:{} {}",
                                         pv.device.major,
                                         pv.device.minor,
                                         (loc * sectors_per_extent) + pv.pe_start);

                    let table_size = mem::size_of::<dmi::Struct_dm_target_spec>();
                    let entry_size = table_size
                        + align_to(params.as_bytes().len() + 1usize, 8usize);

                    {
                        let mut sp: &mut dmi::Struct_dm_target_spec = unsafe {
                            mem::transmute(&mut data)
                        };

                        sp.sector_start = seg.start_extent * sectors_per_extent;
                        sp.length = seg.extent_count * sectors_per_extent;
                        sp.status = 0;

                        let mut dst: &mut [u8] = unsafe {
                            mem::transmute(&mut sp.target_type[..])
                        };

                        copy_memory(b"linear", &mut dst);

                        sp.next = entry_size as u32;
                    }

                    copy_memory(&params.as_bytes(), &mut data[table_size..]);

                    targets_len += entry_size;
                    target_count += 1;
                    let mut data = &mut data[entry_size..];
                }
            }
        }

        // Fill in header 2nd, now that we know overall size etc.
        {
            let mut hdr: &mut dmi::Struct_dm_ioctl = unsafe {mem::transmute(&mut buf)};

            Self::initialize_hdr(lv, &mut hdr);

            hdr.data_start = hdr_len as u32;
            hdr.data_size = hdr_len as u32 + targets_len as u32;
            hdr.target_count = target_count;
        }

        let op = ioctl::op_read_write(DM_IOCTL, dmi::DM_TABLE_LOAD_CMD as u8, buf.len());

        match unsafe { ioctl::read_into(self.file.as_raw_fd(), op, &mut buf) } {
            Err(_) => return Err((io::Error::last_os_error())),
            _ => Ok(())
        }
    }

    fn resume_device(&self, lv: &LV) -> io::Result<()> {
        let mut hdr: dmi::Struct_dm_ioctl = Default::default();

        Self::initialize_hdr(lv, &mut hdr);

        // TODO: broken, need to pass some flags
        let op = ioctl::op_read_write(DM_IOCTL, dmi::DM_DEV_SUSPEND_CMD as u8,
                                      mem::size_of::<dmi::Struct_dm_ioctl>());

        match unsafe { ioctl::read_into(self.file.as_raw_fd(), op, &mut hdr) } {
            Err(_) => return Err((io::Error::last_os_error())),
            _ => { }
        };

        Ok(())
    }

    pub fn activate_device(&self, lv: &mut LV) -> io::Result<()> {

        // TODO: name/uuid mangle?

        self.create_device(lv);

        self.load_device(lv);

        self.resume_device(lv);

        Ok(())
    }
}

//
// Return up to the first \0, or None
//
fn slice_to_null(slc: &[u8]) -> Option<&[u8]> {
    for (i, c) in slc.iter().enumerate() {
        if *c == b'\0' { return Some(&slc[..i]) };
    }
    None
}
<commit_msg>rewrite load_devices()<commit_after>use std::fs;
use std::fs::File;
use std::io;
use std::os::unix::io::AsRawFd;
use std::mem;
use std::slice;
use std::slice::bytes::copy_memory;
use std::io::Error;
use std::io::ErrorKind::Other;

use byteorder::{LittleEndian, ByteOrder};

use nix::sys::ioctl;

use dm_ioctl as dmi;
use lv::LV;
use vg::VG;
use util::align_to;
use pv;

const DM_IOCTL: u8 = 0xfd;
const DM_CTL_PATH: &'static str= "/dev/mapper/control";

const DM_VERSION_MAJOR: u32 = 4;
const DM_VERSION_MINOR: u32 = 30;
const DM_VERSION_PATCHLEVEL: u32 = 0;

pub struct DM<'a> {
    file: File,
    vg: &'a VG,
}

impl <'a> DM<'a> {
    pub fn new(vg: &'a VG) -> io::Result<Self> {
        Ok(DM {
            file: try!(File::open(DM_CTL_PATH)),
            vg: vg,
        })
    }

    fn get_version(&self) -> io::Result<(u32, u32, u32)> {

        let mut hdr: dmi::Struct_dm_ioctl = Default::default();
        hdr.version[0] = DM_VERSION_MAJOR;
        hdr.version[1] = DM_VERSION_MINOR;
        hdr.version[2] = DM_VERSION_PATCHLEVEL;

        let op = ioctl::op_read_write(DM_IOCTL, dmi::DM_VERSION_CMD as u8,
                                      mem::size_of::<dmi::Struct_dm_ioctl>());

        match unsafe { ioctl::read_into(self.file.as_raw_fd(), op, &mut hdr) } {
            Err(_) => return Err((io::Error::last_os_error())),
            _ => {},
        };

        Ok((hdr.version[0], hdr.version[1], hdr.version[2]))
    }

    fn list_devices(&self) -> io::Result<Vec<(String, u64)>> {

        let mut devs = Vec::new();

        let mut buf = [0u8; 10240];

        let hdr: &mut dmi::Struct_dm_ioctl = unsafe {mem::transmute(&mut buf)};

        let hdr_size = mem::size_of::<dmi::Struct_dm_ioctl>();
        hdr.version[0] = DM_VERSION_MAJOR;
        hdr.version[1] = DM_VERSION_MINOR;
        hdr.version[2] = DM_VERSION_PATCHLEVEL;
        hdr.data_size = buf.len() as u32;
        hdr.data_start = hdr_size as u32;

        let op = ioctl::op_read_write(DM_IOCTL, dmi::DM_LIST_DEVICES_CMD as u8, buf.len());

        match unsafe { ioctl::read_into(self.file.as_raw_fd(), op, &mut buf) } {
            Err(_) => return Err((io::Error::last_os_error())),
            _ => {},
        };

        if (hdr.data_size - hdr_size as u32) != 0 {
            let mut result = &buf[hdr_size..];

            loop {
                let slc = slice_to_null(&result[12..]).expect("Bad data from ioctl");
                let devno = LittleEndian::read_u64(&result[..8]);
                devs.push((String::from_utf8_lossy(slc).into_owned(), devno));

                let next = LittleEndian::read_u32(&result[8..12]);
                if next == 0 { break }

                result = &result[next as usize..];
            }
        }

        Ok(devs)
    }

    fn initialize_hdr(lv: &LV, hdr: &mut dmi::Struct_dm_ioctl) -> () {
        hdr.version[0] = DM_VERSION_MAJOR;
        hdr.version[1] = DM_VERSION_MINOR;
        hdr.version[2] = DM_VERSION_PATCHLEVEL;

        // Transmute [i8; 128] to [u8; 128]
        let name_dest: &mut [u8; 128] = unsafe { mem::transmute(&mut hdr.name) };
        copy_memory(&lv.name.as_bytes(), &mut name_dest[..]);

        let uuid_dest: &mut [u8; 129] = unsafe { mem::transmute(&mut hdr.uuid) };
        copy_memory(&lv.id.as_bytes(), &mut uuid_dest[..]);
    }

    fn create_device(&self, lv: &mut LV) -> io::Result<()> {
        let mut hdr: dmi::Struct_dm_ioctl = Default::default();

        Self::initialize_hdr(lv, &mut hdr);

        let op = ioctl::op_read_write(DM_IOCTL, dmi::DM_DEV_CREATE_CMD as u8,
                                      mem::size_of::<dmi::Struct_dm_ioctl>());

        match unsafe { ioctl::read_into(self.file.as_raw_fd(), op, &mut hdr) } {
            Err(_) => return Err((io::Error::last_os_error())),
            _ => { }
        };

        lv.device = Some(pv::Device::from(hdr.dev as i64));

        Ok(())
    }

    fn load_device(&self, lv: &LV) -> io::Result<()> {
        let sectors_per_extent = self.vg.extent_size;
        let mut tgt = Vec::new();

        for seg in &lv.segments {
            for &(ref pvname, ref loc) in &seg.stripes {
                let err = || Error::new(Other, "dm load_device error");
                let pv = try!(self.vg.pvs.get(pvname).ok_or(err()));

                let mut ts: dmi::Struct_dm_target_spec = Default::default();
                ts.sector_start = seg.start_extent * sectors_per_extent;
                ts.length = seg.extent_count * sectors_per_extent;
                ts.status = 0;

                let mut dst: &mut [u8] = unsafe {
                    mem::transmute(&mut ts.target_type[..])
                };
                copy_memory(b"linear", &mut dst);

                let mut params = Vec::new();
                params.extend(format!("{}:{} {}",
                                      pv.device.major,
                                      pv.device.minor,
                                      (loc * sectors_per_extent) + pv.pe_start).as_bytes());

                let pad_bytes = align_to(
                    params.len() + 1usize, 8usize) - params.len();
                params.extend(vec![0; pad_bytes]);

                ts.next = (mem::size_of::<dmi::Struct_dm_target_spec>()
                           + params.len()) as u32;

                tgt.push((ts, params));
            }
        }

        let targets_len: u32 = tgt
            .iter()
            .map(|&(ts, _)| ts.next)
            .sum();

        let mut hdr: dmi::Struct_dm_ioctl = Default::default();
        let hdr_len = mem::size_of::<dmi::Struct_dm_ioctl>() as u32;

        Self::initialize_hdr(lv, &mut hdr);

        hdr.data_start = hdr_len as u32;
        hdr.data_size = hdr_len + targets_len;
        hdr.target_count = tgt.len() as u32;

        let mut buf: Vec<u8> = Vec::with_capacity(hdr.data_size as usize);
        unsafe {
            let ptr: *mut u8 = mem::transmute(&mut hdr);
            let slc = slice::from_raw_parts(ptr, hdr_len as usize);
            buf.extend(slc);
        }

        for (ts, param) in tgt {
            unsafe {
                let ptr: *mut u8 = mem::transmute(&ts);
                let slc = slice::from_raw_parts(
                    ptr, mem::size_of::<dmi::Struct_dm_target_spec>());
                buf.extend(slc);
            }

            buf.extend(&param);
        }

        println!("FF {} {}", hdr_len, buf.len());

        let op = ioctl::op_read_write(DM_IOCTL, dmi::DM_TABLE_LOAD_CMD as u8, buf.len());

        match unsafe { ioctl::read_into(self.file.as_raw_fd(), op, &mut buf) } {
            Err(_) => return Err((io::Error::last_os_error())),
            _ => Ok(())
        }
    }

    fn resume_device(&self, lv: &LV) -> io::Result<()> {
        let mut hdr: dmi::Struct_dm_ioctl = Default::default();

        Self::initialize_hdr(lv, &mut hdr);

        // TODO: broken, need to pass some flags
        let op = ioctl::op_read_write(DM_IOCTL, dmi::DM_DEV_SUSPEND_CMD as u8,
                                      mem::size_of::<dmi::Struct_dm_ioctl>());

        match unsafe { ioctl::read_into(self.file.as_raw_fd(), op, &mut hdr) } {
            Err(_) => return Err((io::Error::last_os_error())),
            _ => { }
        };

        Ok(())
    }

    pub fn activate_device(&self, lv: &mut LV) -> io::Result<()> {

        // TODO: name/uuid mangle?

        try!(self.get_version());

        try!(self.list_devices());

        try!(self.create_device(lv));

        try!(self.load_device(lv));

        try!(self.resume_device(lv));

        Ok(())
    }
}

//
// Return up to the first \0, or None
//
fn slice_to_null(slc: &[u8]) -> Option<&[u8]> {
    for (i, c) in slc.iter().enumerate() {
        if *c == b'\0' { return Some(&slc[..i]) };
    }
    None
}
<|endoftext|>