components/
ninedof.rs

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
// Licensed under the Apache License, Version 2.0 or the MIT License.
// SPDX-License-Identifier: Apache-2.0 OR MIT
// Copyright Tock Contributors 2022.

//! Component for 9DOF
//!
//! Usage
//! -----
//!
//! ```rust
//! let ninedof = components::ninedof::NineDofComponent::new(board_kernel)
//!     .finalize(components::ninedof_component_static!(driver1, driver2, ...));
//! ```

use capsules_extra::ninedof::NineDof;
use core::mem::MaybeUninit;
use kernel::capabilities;
use kernel::component::Component;
use kernel::create_capability;

#[macro_export]
macro_rules! ninedof_component_static {
    ($($P:expr),+ $(,)?) => {{
        use kernel::count_expressions;

        const NUM_DRIVERS: usize = count_expressions!($($P),+);

        let drivers = kernel::static_init!(
            [&'static dyn kernel::hil::sensors::NineDof; NUM_DRIVERS],
            [
                $($P,)*
            ]
        );
        let ninedof = kernel::static_buf!(capsules_extra::ninedof::NineDof<'static>);
        (ninedof, drivers)
    };};
}

pub struct NineDofComponent {
    board_kernel: &'static kernel::Kernel,
    driver_num: usize,
}

impl NineDofComponent {
    pub fn new(board_kernel: &'static kernel::Kernel, driver_num: usize) -> NineDofComponent {
        NineDofComponent {
            board_kernel,
            driver_num,
        }
    }
}

impl Component for NineDofComponent {
    type StaticInput = (
        &'static mut MaybeUninit<NineDof<'static>>,
        &'static [&'static dyn kernel::hil::sensors::NineDof<'static>],
    );
    type Output = &'static capsules_extra::ninedof::NineDof<'static>;

    fn finalize(self, static_buffer: Self::StaticInput) -> Self::Output {
        let grant_cap = create_capability!(capabilities::MemoryAllocationCapability);
        let grant_ninedof = self.board_kernel.create_grant(self.driver_num, &grant_cap);

        let ninedof = static_buffer.0.write(capsules_extra::ninedof::NineDof::new(
            static_buffer.1,
            grant_ninedof,
        ));

        for driver in static_buffer.1 {
            kernel::hil::sensors::NineDof::set_client(*driver, ninedof);
        }

        ninedof
    }
}