capsules_extra/
lsm303dlhc.rs

1// Licensed under the Apache License, Version 2.0 or the MIT License.
2// SPDX-License-Identifier: Apache-2.0 OR MIT
3// Copyright Tock Contributors 2022.
4
5//! SyscallDriver for the LSM303DLHC 3D accelerometer and 3D magnetometer sensor.
6//!
7//! May be used with NineDof and Temperature
8//!
9//! I2C Interface
10//!
11//! <https://www.st.com/en/mems-and-sensors/lsm303dlhc.html>
12//!
13//! The syscall interface is described in [lsm303dlhc.md](https://github.com/tock/tock/tree/master/doc/syscalls/70006_lsm303dlhc.md)
14//!
15//! Usage
16//! -----
17//!
18//! ```rust,ignore
19//! let mux_i2c = components::i2c::I2CMuxComponent::new(&stm32f3xx::i2c::I2C1)
20//!     .finalize(components::i2c_mux_component_helper!());
21//!
22//! let lsm303dlhc = components::lsm303dlhc::Lsm303dlhcI2CComponent::new()
23//!    .finalize(components::lsm303dlhc_i2c_component_helper!(mux_i2c));
24//!
25//! lsm303dlhc.configure(
26//!    lsm303dlhc::Lsm303AccelDataRate::DataRate25Hz,
27//!    false,
28//!    lsm303dlhc::Lsm303Scale::Scale2G,
29//!    false,
30//!    true,
31//!    lsm303dlhc::Lsm303MagnetoDataRate::DataRate3_0Hz,
32//!    lsm303dlhc::Lsm303Range::Range4_7G,
33//!);
34//! ```
35//!
36//! NideDof Example
37//!
38//! ```rust,ignore
39//! let grant_cap = create_capability!(capabilities::MemoryAllocationCapability);
40//! let grant_ninedof = board_kernel.create_grant(&grant_cap);
41//!
42//! // use as primary NineDof Sensor
43//! let ninedof = static_init!(
44//!    capsules::ninedof::NineDof<'static>,
45//!    capsules::ninedof::NineDof::new(lsm303dlhc, grant_ninedof)
46//! );
47//!
48//! hil::sensors::NineDof::set_client(lsm303dlhc, ninedof);
49//!
50//! // use as secondary NineDof Sensor
51//! let lsm303dlhc_secondary = static_init!(
52//!    capsules::ninedof::NineDofNode<'static, &'static dyn hil::sensors::NineDof>,
53//!    capsules::ninedof::NineDofNode::new(lsm303dlhc)
54//! );
55//! ninedof.add_secondary_driver(lsm303dlhc_secondary);
56//! hil::sensors::NineDof::set_client(lsm303dlhc, ninedof);
57//! ```
58//!
59//! Temperature Example
60//!
61//! ```rust,ignore
62//! let grant_cap = create_capability!(capabilities::MemoryAllocationCapability);
63//! let grant_temp = board_kernel.create_grant(&grant_cap);
64//!
65//! lsm303dlhc.configure(
66//!    lsm303dlhc::Lsm303AccelDataRate::DataRate25Hz,
67//!    false,
68//!    lsm303dlhc::Lsm303Scale::Scale2G,
69//!    false,
70//!    true,
71//!    lsm303dlhc::Lsm303MagnetoDataRate::DataRate3_0Hz,
72//!    lsm303dlhc::Lsm303Range::Range4_7G,
73//!);
74//! let temp = static_init!(
75//! capsules::temperature::TemperatureSensor<'static>,
76//!     capsules::temperature::TemperatureSensor::new(lsm303dlhc, grant_temperature));
77//! kernel::hil::sensors::TemperatureDriver::set_client(lsm303dlhc, temp);
78//! ```
79//!
80//! Author: Alexandru Radovici <msg4alex@gmail.com>
81//!
82
83#![allow(non_camel_case_types)]
84
85use core::cell::Cell;
86
87use enum_primitive::cast::FromPrimitive;
88use enum_primitive::enum_from_primitive;
89
90use kernel::grant::{AllowRoCount, AllowRwCount, Grant, UpcallCount};
91use kernel::hil::i2c;
92use kernel::hil::sensors;
93use kernel::syscall::{CommandReturn, SyscallDriver};
94use kernel::utilities::cells::{OptionalCell, TakeCell};
95use kernel::{ErrorCode, ProcessId};
96
97use crate::lsm303xx::{
98    AccelerometerRegisters, Lsm303AccelDataRate, Lsm303MagnetoDataRate, Lsm303Range, Lsm303Scale,
99    CTRL_REG1, CTRL_REG4, RANGE_FACTOR_X_Y, RANGE_FACTOR_Z, SCALE_FACTOR,
100};
101
102use capsules_core::driver;
103
104/// Syscall driver number.
105pub const DRIVER_NUM: usize = driver::NUM::Lsm303dlch as usize;
106
107/// Register values
108const REGISTER_AUTO_INCREMENT: u8 = 0x80;
109
110enum_from_primitive! {
111    enum MagnetometerRegisters {
112        CRA_REG_M = 0x00,
113        CRB_REG_M = 0x01,
114        OUT_X_H_M = 0x03,
115        OUT_X_L_M = 0x04,
116        OUT_Z_H_M = 0x05,
117        OUT_Z_L_M = 0x06,
118        OUT_Y_H_M = 0x07,
119        OUT_Y_L_M = 0x08,
120        TEMP_OUT_H_M = 0x31,
121        TEMP_OUT_L_M = 0x32,
122    }
123}
124
125// Experimental
126const TEMP_OFFSET: i32 = 17;
127
128#[derive(Clone, Copy, PartialEq)]
129enum State {
130    Idle,
131    IsPresent,
132    SetPowerMode,
133    SetScaleAndResolution,
134    ReadAccelerationXYZ,
135    SetTemperatureDataRate,
136    SetRange,
137    ReadTemperature,
138    ReadMagnetometerXYZ,
139}
140
141pub struct Lsm303dlhcI2C<'a, I: i2c::I2CDevice> {
142    config_in_progress: Cell<bool>,
143    i2c_accelerometer: &'a I,
144    i2c_magnetometer: &'a I,
145    state: Cell<State>,
146    accel_scale: Cell<Lsm303Scale>,
147    mag_range: Cell<Lsm303Range>,
148    accel_high_resolution: Cell<bool>,
149    mag_data_rate: Cell<Lsm303MagnetoDataRate>,
150    accel_data_rate: Cell<Lsm303AccelDataRate>,
151    low_power: Cell<bool>,
152    temperature: Cell<bool>,
153    buffer: TakeCell<'static, [u8]>,
154    nine_dof_client: OptionalCell<&'a dyn sensors::NineDofClient>,
155    temperature_client: OptionalCell<&'a dyn sensors::TemperatureClient>,
156    current_process: OptionalCell<ProcessId>,
157    apps: Grant<App, UpcallCount<1>, AllowRoCount<0>, AllowRwCount<0>>,
158}
159
160#[derive(Default)]
161pub struct App {}
162
163impl<'a, I: i2c::I2CDevice> Lsm303dlhcI2C<'a, I> {
164    pub fn new(
165        i2c_accelerometer: &'a I,
166        i2c_magnetometer: &'a I,
167        buffer: &'static mut [u8],
168        grant: Grant<App, UpcallCount<1>, AllowRoCount<0>, AllowRwCount<0>>,
169    ) -> Lsm303dlhcI2C<'a, I> {
170        // setup and return struct
171        Lsm303dlhcI2C {
172            config_in_progress: Cell::new(false),
173            i2c_accelerometer,
174            i2c_magnetometer,
175            state: Cell::new(State::Idle),
176            accel_scale: Cell::new(Lsm303Scale::Scale2G),
177            mag_range: Cell::new(Lsm303Range::Range1G),
178            accel_high_resolution: Cell::new(false),
179            mag_data_rate: Cell::new(Lsm303MagnetoDataRate::DataRate0_75Hz),
180            accel_data_rate: Cell::new(Lsm303AccelDataRate::DataRate1Hz),
181            low_power: Cell::new(false),
182            temperature: Cell::new(false),
183            buffer: TakeCell::new(buffer),
184            nine_dof_client: OptionalCell::empty(),
185            temperature_client: OptionalCell::empty(),
186            current_process: OptionalCell::empty(),
187            apps: grant,
188        }
189    }
190
191    pub fn configure(
192        &self,
193        accel_data_rate: Lsm303AccelDataRate,
194        low_power: bool,
195        accel_scale: Lsm303Scale,
196        accel_high_resolution: bool,
197        temperature: bool,
198        mag_data_rate: Lsm303MagnetoDataRate,
199        mag_range: Lsm303Range,
200    ) -> Result<(), ErrorCode> {
201        if self.state.get() == State::Idle {
202            self.config_in_progress.set(true);
203
204            self.accel_scale.set(accel_scale);
205            self.accel_high_resolution.set(accel_high_resolution);
206            self.temperature.set(temperature);
207            self.mag_data_rate.set(mag_data_rate);
208            self.mag_range.set(mag_range);
209            self.accel_data_rate.set(accel_data_rate);
210            self.low_power.set(low_power);
211
212            self.set_power_mode(accel_data_rate, low_power)
213        } else {
214            Err(ErrorCode::BUSY)
215        }
216    }
217
218    fn is_present(&self) -> Result<(), ErrorCode> {
219        if self.state.get() != State::Idle {
220            self.state.set(State::IsPresent);
221            self.buffer.take().map_or(Err(ErrorCode::NOMEM), |buf| {
222                // turn on i2c to send commands
223                buf[0] = 0x0F;
224                self.i2c_magnetometer.enable();
225                if let Err((error, buf)) = self.i2c_magnetometer.write_read(buf, 1, 1) {
226                    self.buffer.replace(buf);
227                    self.state.set(State::Idle);
228                    Err(error.into())
229                } else {
230                    Ok(())
231                }
232            })
233        } else {
234            Err(ErrorCode::BUSY)
235        }
236    }
237
238    fn set_power_mode(
239        &self,
240        data_rate: Lsm303AccelDataRate,
241        low_power: bool,
242    ) -> Result<(), ErrorCode> {
243        if self.state.get() == State::Idle {
244            self.state.set(State::SetPowerMode);
245            self.buffer.take().map_or(Err(ErrorCode::NOMEM), |buf| {
246                buf[0] = AccelerometerRegisters::CTRL_REG1 as u8;
247                buf[1] = (CTRL_REG1::ODR.val(data_rate as u8)
248                    + CTRL_REG1::LPEN.val(low_power as u8)
249                    + CTRL_REG1::ZEN::SET
250                    + CTRL_REG1::YEN::SET
251                    + CTRL_REG1::XEN::SET)
252                    .value;
253                self.i2c_accelerometer.enable();
254                if let Err((error, buf)) = self.i2c_accelerometer.write(buf, 2) {
255                    self.state.set(State::Idle);
256                    self.buffer.replace(buf);
257                    Err(error.into())
258                } else {
259                    Ok(())
260                }
261            })
262        } else {
263            Err(ErrorCode::BUSY)
264        }
265    }
266
267    fn set_scale_and_resolution(
268        &self,
269        scale: Lsm303Scale,
270        high_resolution: bool,
271    ) -> Result<(), ErrorCode> {
272        if self.state.get() == State::Idle {
273            self.state.set(State::SetScaleAndResolution);
274            // TODO move these in completed
275            self.accel_scale.set(scale);
276            self.accel_high_resolution.set(high_resolution);
277            self.buffer.take().map_or(Err(ErrorCode::NOMEM), |buf| {
278                buf[0] = AccelerometerRegisters::CTRL_REG4 as u8;
279                buf[1] = (CTRL_REG4::FS.val(scale as u8)
280                    + CTRL_REG4::HR.val(high_resolution as u8))
281                .value;
282                self.i2c_accelerometer.enable();
283                if let Err((error, buf)) = self.i2c_accelerometer.write(buf, 2) {
284                    self.state.set(State::Idle);
285                    self.buffer.replace(buf);
286                    Err(error.into())
287                } else {
288                    Ok(())
289                }
290            })
291        } else {
292            Err(ErrorCode::BUSY)
293        }
294    }
295
296    fn read_acceleration_xyz(&self) -> Result<(), ErrorCode> {
297        if self.state.get() == State::Idle {
298            self.state.set(State::ReadAccelerationXYZ);
299            self.buffer.take().map_or(Err(ErrorCode::NOMEM), |buf| {
300                buf[0] = AccelerometerRegisters::OUT_X_L_A as u8 | REGISTER_AUTO_INCREMENT;
301                self.i2c_accelerometer.enable();
302                if let Err((error, buf)) = self.i2c_accelerometer.write_read(buf, 1, 6) {
303                    self.state.set(State::Idle);
304                    self.buffer.replace(buf);
305                    Err(error.into())
306                } else {
307                    Ok(())
308                }
309            })
310        } else {
311            Err(ErrorCode::BUSY)
312        }
313    }
314
315    fn set_temperature_and_magneto_data_rate(
316        &self,
317        temperature: bool,
318        data_rate: Lsm303MagnetoDataRate,
319    ) -> Result<(), ErrorCode> {
320        if self.state.get() == State::Idle {
321            self.state.set(State::SetTemperatureDataRate);
322            self.buffer.take().map_or(Err(ErrorCode::NOMEM), |buf| {
323                buf[0] = MagnetometerRegisters::CRA_REG_M as u8;
324                buf[1] = ((data_rate as u8) << 2) | if temperature { 1 << 7 } else { 0 };
325                self.i2c_magnetometer.enable();
326                if let Err((error, buf)) = self.i2c_magnetometer.write(buf, 2) {
327                    self.state.set(State::Idle);
328                    self.buffer.replace(buf);
329                    Err(error.into())
330                } else {
331                    Ok(())
332                }
333            })
334        } else {
335            Err(ErrorCode::BUSY)
336        }
337    }
338
339    fn set_range(&self, range: Lsm303Range) -> Result<(), ErrorCode> {
340        if self.state.get() == State::Idle {
341            self.state.set(State::SetRange);
342            // TODO move these in completed
343            self.mag_range.set(range);
344            self.buffer.take().map_or(Err(ErrorCode::NOMEM), |buf| {
345                buf[0] = MagnetometerRegisters::CRB_REG_M as u8;
346                buf[1] = (range as u8) << 5;
347                buf[2] = 0;
348                self.i2c_magnetometer.enable();
349                if let Err((error, buf)) = self.i2c_magnetometer.write(buf, 3) {
350                    self.state.set(State::Idle);
351                    self.buffer.replace(buf);
352                    Err(error.into())
353                } else {
354                    Ok(())
355                }
356            })
357        } else {
358            Err(ErrorCode::BUSY)
359        }
360    }
361
362    fn read_temperature(&self) -> Result<(), ErrorCode> {
363        if self.state.get() == State::Idle {
364            self.state.set(State::ReadTemperature);
365            self.buffer.take().map_or(Err(ErrorCode::NOMEM), |buf| {
366                buf[0] = MagnetometerRegisters::TEMP_OUT_H_M as u8;
367                self.i2c_magnetometer.enable();
368                if let Err((error, buf)) = self.i2c_magnetometer.write_read(buf, 1, 2) {
369                    self.state.set(State::Idle);
370                    self.buffer.replace(buf);
371                    Err(error.into())
372                } else {
373                    Ok(())
374                }
375            })
376        } else {
377            Err(ErrorCode::BUSY)
378        }
379    }
380
381    fn read_magnetometer_xyz(&self) -> Result<(), ErrorCode> {
382        if self.state.get() == State::Idle {
383            self.state.set(State::ReadMagnetometerXYZ);
384            self.buffer.take().map_or(Err(ErrorCode::NOMEM), |buf| {
385                buf[0] = MagnetometerRegisters::OUT_X_H_M as u8;
386                self.i2c_magnetometer.enable();
387                if let Err((error, buf)) = self.i2c_magnetometer.write_read(buf, 1, 6) {
388                    self.state.set(State::Idle);
389                    self.buffer.replace(buf);
390                    Err(error.into())
391                } else {
392                    Ok(())
393                }
394            })
395        } else {
396            Err(ErrorCode::BUSY)
397        }
398    }
399}
400
401impl<I: i2c::I2CDevice> i2c::I2CClient for Lsm303dlhcI2C<'_, I> {
402    fn command_complete(&self, buffer: &'static mut [u8], status: Result<(), i2c::Error>) {
403        match self.state.get() {
404            State::IsPresent => {
405                let present = status.is_ok() && buffer[0] == 60;
406
407                self.current_process.map(|process_id| {
408                    let _ = self.apps.enter(process_id, |_grant, upcalls| {
409                        let _ = upcalls.schedule_upcall(0, (usize::from(present), 0, 0));
410                    });
411                });
412
413                self.buffer.replace(buffer);
414                self.i2c_magnetometer.disable();
415                self.state.set(State::Idle);
416            }
417            State::SetPowerMode => {
418                let set_power = status == Ok(());
419
420                self.current_process.map(|process_id| {
421                    let _ = self.apps.enter(process_id, |_grant, upcalls| {
422                        let _ = upcalls.schedule_upcall(0, (usize::from(set_power), 0, 0));
423                    });
424                });
425
426                self.buffer.replace(buffer);
427                self.i2c_accelerometer.disable();
428                self.state.set(State::Idle);
429                if self.config_in_progress.get() {
430                    let _ = self.set_scale_and_resolution(
431                        self.accel_scale.get(),
432                        self.accel_high_resolution.get(),
433                    );
434                }
435            }
436            State::SetScaleAndResolution => {
437                let set_scale_and_resolution = status == Ok(());
438
439                self.current_process.map(|process_id| {
440                    let _ = self.apps.enter(process_id, |_grant, upcalls| {
441                        let _ = upcalls
442                            .schedule_upcall(0, (usize::from(set_scale_and_resolution), 0, 0));
443                    });
444                });
445
446                self.buffer.replace(buffer);
447                self.i2c_accelerometer.disable();
448                self.state.set(State::Idle);
449                if self.config_in_progress.get() {
450                    if let Err(_error) = self.set_temperature_and_magneto_data_rate(
451                        self.temperature.get(),
452                        self.mag_data_rate.get(),
453                    ) {
454                        self.config_in_progress.set(false);
455                    }
456                }
457            }
458            State::ReadAccelerationXYZ => {
459                let mut x: usize = 0;
460                let mut y: usize = 0;
461                let mut z: usize = 0;
462                let values = if status == Ok(()) {
463                    self.nine_dof_client.map(|client| {
464                        // compute using only integers
465                        let scale_factor = self.accel_scale.get() as usize;
466                        x = (((buffer[0] as i16 | ((buffer[1] as i16) << 8)) as i32)
467                            * (SCALE_FACTOR[scale_factor] as i32)
468                            * 1000
469                            / 32768) as usize;
470                        y = (((buffer[2] as i16 | ((buffer[3] as i16) << 8)) as i32)
471                            * (SCALE_FACTOR[scale_factor] as i32)
472                            * 1000
473                            / 32768) as usize;
474                        z = (((buffer[4] as i16 | ((buffer[5] as i16) << 8)) as i32)
475                            * (SCALE_FACTOR[scale_factor] as i32)
476                            * 1000
477                            / 32768) as usize;
478                        client.callback(x, y, z);
479                    });
480
481                    x = (buffer[0] as i16 | ((buffer[1] as i16) << 8)) as usize;
482                    y = (buffer[2] as i16 | ((buffer[3] as i16) << 8)) as usize;
483                    z = (buffer[4] as i16 | ((buffer[5] as i16) << 8)) as usize;
484                    true
485                } else {
486                    self.nine_dof_client.map(|client| {
487                        client.callback(0, 0, 0);
488                    });
489                    false
490                };
491
492                self.current_process.map(|process_id| {
493                    let _ = self.apps.enter(process_id, |_grant, upcalls| {
494                        if values {
495                            let _ = upcalls.schedule_upcall(0, (x, y, z));
496                        } else {
497                            let _ = upcalls.schedule_upcall(0, (0, 0, 0));
498                        }
499                    });
500                });
501
502                self.buffer.replace(buffer);
503                self.i2c_accelerometer.disable();
504                self.state.set(State::Idle);
505            }
506            State::SetTemperatureDataRate => {
507                let set_temperature_and_magneto_data_rate = status == Ok(());
508
509                self.current_process.map(|process_id| {
510                    let _ = self.apps.enter(process_id, |_grant, upcalls| {
511                        let _ = upcalls.schedule_upcall(
512                            0,
513                            (usize::from(set_temperature_and_magneto_data_rate), 0, 0),
514                        );
515                    });
516                });
517
518                self.buffer.replace(buffer);
519                self.i2c_magnetometer.disable();
520                self.state.set(State::Idle);
521                if self.config_in_progress.get() {
522                    if let Err(_error) = self.set_range(self.mag_range.get()) {
523                        self.config_in_progress.set(false);
524                    }
525                }
526            }
527            State::SetRange => {
528                let set_range = status == Ok(());
529
530                self.current_process.map(|process_id| {
531                    let _ = self.apps.enter(process_id, |_grant, upcalls| {
532                        let _ = upcalls.schedule_upcall(0, (usize::from(set_range), 0, 0));
533                    });
534                });
535
536                if self.config_in_progress.get() {
537                    self.config_in_progress.set(false);
538                }
539                self.buffer.replace(buffer);
540                self.i2c_magnetometer.disable();
541                self.state.set(State::Idle);
542            }
543            State::ReadTemperature => {
544                let values = match status {
545                    Ok(()) => Ok(
546                        ((buffer[1] as i16 | ((buffer[0] as i16) << 8)) >> 4) as i32 / 8
547                            + TEMP_OFFSET,
548                    ),
549                    Err(i2c_error) => Err(i2c_error.into()),
550                };
551                self.temperature_client.map(|client| {
552                    client.callback(values);
553                });
554
555                self.current_process.map(|process_id| {
556                    let _ = self.apps.enter(process_id, |_grant, upcalls| {
557                        if let Ok(temp) = values {
558                            let _ = upcalls.schedule_upcall(0, (temp as usize, 0, 0));
559                        } else {
560                            let _ = upcalls.schedule_upcall(0, (0, 0, 0));
561                        }
562                    });
563                });
564
565                self.buffer.replace(buffer);
566                self.i2c_magnetometer.disable();
567                self.state.set(State::Idle);
568            }
569            State::ReadMagnetometerXYZ => {
570                let mut x: usize = 0;
571                let mut y: usize = 0;
572                let mut z: usize = 0;
573                let values = if status == Ok(()) {
574                    self.nine_dof_client.map(|client| {
575                        // compute using only integers
576                        let range = self.mag_range.get() as usize;
577                        x = (((buffer[1] as i16 | ((buffer[0] as i16) << 8)) as i32) * 100
578                            / RANGE_FACTOR_X_Y[range] as i32) as usize;
579                        z = (((buffer[3] as i16 | ((buffer[2] as i16) << 8)) as i32) * 100
580                            / RANGE_FACTOR_X_Y[range] as i32) as usize;
581                        y = (((buffer[5] as i16 | ((buffer[4] as i16) << 8)) as i32) * 100
582                            / RANGE_FACTOR_Z[range] as i32) as usize;
583                        client.callback(x, y, z);
584                    });
585
586                    x = ((buffer[1] as u16 | ((buffer[0] as u16) << 8)) as i16) as usize;
587                    z = ((buffer[3] as u16 | ((buffer[2] as u16) << 8)) as i16) as usize;
588                    y = ((buffer[5] as u16 | ((buffer[4] as u16) << 8)) as i16) as usize;
589                    true
590                } else {
591                    self.nine_dof_client.map(|client| {
592                        client.callback(0, 0, 0);
593                    });
594                    false
595                };
596
597                self.current_process.map(|process_id| {
598                    let _ = self.apps.enter(process_id, |_grant, upcalls| {
599                        if values {
600                            let _ = upcalls.schedule_upcall(0, (x, y, z));
601                        } else {
602                            let _ = upcalls.schedule_upcall(0, (0, 0, 0));
603                        }
604                    });
605                });
606
607                self.buffer.replace(buffer);
608                self.i2c_magnetometer.disable();
609                self.state.set(State::Idle);
610            }
611            _ => {
612                self.i2c_magnetometer.disable();
613                self.i2c_accelerometer.disable();
614                self.buffer.replace(buffer);
615            }
616        }
617    }
618}
619
620impl<I: i2c::I2CDevice> SyscallDriver for Lsm303dlhcI2C<'_, I> {
621    fn command(
622        &self,
623        command_num: usize,
624        data1: usize,
625        data2: usize,
626        process_id: ProcessId,
627    ) -> CommandReturn {
628        if command_num == 0 {
629            // Handle this first as it should be returned
630            // unconditionally
631            return CommandReturn::success();
632        }
633
634        // Check if this non-virtualized driver is already in use by
635        // some (alive) process
636        let match_or_empty_or_nonexistant = self.current_process.map_or(true, |current_process| {
637            self.apps
638                .enter(current_process, |_, _| current_process == process_id)
639                .unwrap_or(true)
640        });
641
642        if match_or_empty_or_nonexistant {
643            self.current_process.set(process_id);
644        } else {
645            return CommandReturn::failure(ErrorCode::NOMEM);
646        }
647
648        match command_num {
649            // Check is sensor is correctly connected
650            1 => {
651                if self.state.get() == State::Idle {
652                    match self.is_present() {
653                        Ok(()) => CommandReturn::success(),
654                        Err(error) => CommandReturn::failure(error),
655                    }
656                } else {
657                    CommandReturn::failure(ErrorCode::BUSY)
658                }
659            }
660            // Set Accelerometer Power Mode
661            2 => {
662                if self.state.get() == State::Idle {
663                    if let Some(data_rate) = Lsm303AccelDataRate::from_usize(data1) {
664                        match self.set_power_mode(data_rate, data2 != 0) {
665                            Ok(()) => CommandReturn::success(),
666                            Err(error) => CommandReturn::failure(error),
667                        }
668                    } else {
669                        CommandReturn::failure(ErrorCode::INVAL)
670                    }
671                } else {
672                    CommandReturn::failure(ErrorCode::BUSY)
673                }
674            }
675            // Set Accelerometer Scale And Resolution
676            3 => {
677                if self.state.get() == State::Idle {
678                    if let Some(scale) = Lsm303Scale::from_usize(data1) {
679                        match self.set_scale_and_resolution(scale, data2 != 0) {
680                            Ok(()) => CommandReturn::success(),
681                            Err(error) => CommandReturn::failure(error),
682                        }
683                    } else {
684                        CommandReturn::failure(ErrorCode::INVAL)
685                    }
686                } else {
687                    CommandReturn::failure(ErrorCode::BUSY)
688                }
689            }
690            // Set Magnetometer Temperature Enable and Data Rate
691            4 => {
692                if self.state.get() == State::Idle {
693                    if let Some(data_rate) = Lsm303MagnetoDataRate::from_usize(data1) {
694                        match self.set_temperature_and_magneto_data_rate(data2 != 0, data_rate) {
695                            Ok(()) => CommandReturn::success(),
696                            Err(error) => CommandReturn::failure(error),
697                        }
698                    } else {
699                        CommandReturn::failure(ErrorCode::INVAL)
700                    }
701                } else {
702                    CommandReturn::failure(ErrorCode::BUSY)
703                }
704            }
705            // Set Magnetometer Range
706            5 => {
707                if self.state.get() == State::Idle {
708                    if let Some(range) = Lsm303Range::from_usize(data1) {
709                        match self.set_range(range) {
710                            Ok(()) => CommandReturn::success(),
711                            Err(error) => CommandReturn::failure(error),
712                        }
713                    } else {
714                        CommandReturn::failure(ErrorCode::INVAL)
715                    }
716                } else {
717                    CommandReturn::failure(ErrorCode::BUSY)
718                }
719            }
720            // default
721            _ => CommandReturn::failure(ErrorCode::NOSUPPORT),
722        }
723    }
724
725    fn allocate_grant(&self, processid: ProcessId) -> Result<(), kernel::process::Error> {
726        self.apps.enter(processid, |_, _| {})
727    }
728}
729
730impl<'a, I: i2c::I2CDevice> sensors::NineDof<'a> for Lsm303dlhcI2C<'a, I> {
731    fn set_client(&self, nine_dof_client: &'a dyn sensors::NineDofClient) {
732        self.nine_dof_client.replace(nine_dof_client);
733    }
734
735    fn read_accelerometer(&self) -> Result<(), ErrorCode> {
736        self.read_acceleration_xyz()
737    }
738
739    fn read_magnetometer(&self) -> Result<(), ErrorCode> {
740        self.read_magnetometer_xyz()
741    }
742}
743
744impl<'a, I: i2c::I2CDevice> sensors::TemperatureDriver<'a> for Lsm303dlhcI2C<'a, I> {
745    fn set_client(&self, temperature_client: &'a dyn sensors::TemperatureClient) {
746        self.temperature_client.replace(temperature_client);
747    }
748
749    fn read_temperature(&self) -> Result<(), ErrorCode> {
750        self.read_temperature()
751    }
752}