1#![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
104pub const DRIVER_NUM: usize = driver::NUM::Lsm303dlch as usize;
106
107const 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
125const 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 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 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 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 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 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 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 return CommandReturn::success();
632 }
633
634 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 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 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 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 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 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 _ => 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}