cmdr-joystick/rp2040/src/usb_joystick_device.rs

//! Project: CMtec CMDR joystick 24
//! Date:    2023-08-01
//! Author:  Christoffer Martinsson
//! Email:   cm@cmtec.se
//! License: Please refer to LICENSE in root directory

use core::default::Default;
use fugit::ExtU32;
// use packed_struct::prelude::*;
use usb_device::bus::UsbBus;
use usb_device::class_prelude::UsbBusAllocator;
use usbd_human_interface_device::usb_class::prelude::*;

macro_rules! unwrap {
    ($arg:expr) => {
        match $crate::usb_joystick_device::Try::into_result($arg) {
            ::core::result::Result::Ok(t) => t,
            ::core::result::Result::Err(e) => {
                ::core::panic!("unwrap of `{}` failed: {:?}", ::core::stringify!($arg), e);
            }
        }
    };
    ($arg:expr, $($msg:expr),+ $(,)? ) => {
        match $crate::usb_joystick_device::Try::into_result($arg) {
            ::core::result::Result::Ok(t) => t,
            ::core::result::Result::Err(e) => {
                ::core::panic!("unwrap of `{}` failed: {}: {:?}", ::core::stringify!($arg), ::core::format_args!($($msg,)*), e);
            }
        }
    }
}

#[derive(Debug, Copy, Clone, Eq, PartialEq)]
pub struct NoneError;

pub trait Try {
    type Ok;
    type Error;
    fn into_result(self) -> Result<Self::Ok, Self::Error>;
}

impl<T> Try for Option<T> {
    type Ok = T;
    type Error = NoneError;

    #[inline]
    fn into_result(self) -> Result<T, NoneError> {
        self.ok_or(NoneError)
    }
}

impl<T, E> Try for Result<T, E> {
    type Ok = T;
    type Error = E;

    #[inline]
    fn into_result(self) -> Self {
        self
    }
}

// Based on example device from https://github.com/dlkj/usbd-human-interface-device/blob/main/src/device/joystick.rs
// Updated to 6x 12bit axis, 32x buttons and 4x hat switches
#[rustfmt::skip]
pub const JOYSTICK_DESCRIPTOR: &[u8] = &[
    0x05, 0x01,        // Usage Page (Generic Desktop)
    0x09, 0x04,        // Usage (Joystick)
    0xA1, 0x01,        // Collection (Application)

    // 8 signed 16-bit axes: X, Y, Z, Rx, Ry, Rz, Slider, Dial
    0x09, 0x30,        //   Usage (X)
    0x09, 0x31,        //   Usage (Y)
    0x09, 0x32,        //   Usage (Z)
    0x09, 0x33,        //   Usage (Rx)
    0x09, 0x34,        //   Usage (Ry)
    0x09, 0x35,        //   Usage (Rz)
    0x09, 0x36,        //   Usage (Slider)
    0x09, 0x37,        //   Usage (Dial)
    0x16, 0x00, 0x80,  //   Logical Minimum (-32768)
    0x26, 0xFF, 0x7F,  //   Logical Maximum (32767)
    0x75, 0x10,        //   Report Size (16)
    0x95, 0x08,        //   Report Count (8)
    0x81, 0x02,        //   Input (Data,Var,Abs)

    // 26 Buttons (1-bit each)
    0x05, 0x09,        //   Usage Page (Button)
    0x19, 0x01,        //   Usage Minimum (1)
    0x29, 0x1A,        //   Usage Maximum (26)
    0x15, 0x00,        //   Logical Minimum (0)
    0x25, 0x01,        //   Logical Maximum (1)
    0x75, 0x01,        //   Report Size (1)
    0x95, 0x1A,        //   Report Count (26)
    0x81, 0x02,        //   Input (Data,Var,Abs)

    // Padding to align buttons to byte boundary (26 bits → +6 bits padding)
    0x75, 0x01,        //   Report Size (1)
    0x95, 0x06,        //   Report Count (6)
    0x81, 0x03,        //   Input (Const,Var,Abs)

    0xC0               // End Collection
];

#[derive(Clone, Copy, Debug, Eq, PartialEq, Default)]
pub struct JoystickReport {
    pub x: i16,       // 16bit
    pub y: i16,       // 16bit
    pub z: i16,       // 16bit
    pub rx: i16,      // 16bit
    pub ry: i16,      // 16bit
    pub rz: i16,      // 16bit
    pub slider: i16,  // 16bit
    pub dial: i16,    // 16bit
    pub buttons: u32, // 32bit
}

pub struct Joystick<'a, B: UsbBus> {
    interface: Interface<'a, B, InBytes32, OutNone, ReportSingle>,
}

impl<B: UsbBus> Joystick<'_, B> {
    pub fn write_report(&mut self, report: &JoystickReport) -> Result<(), UsbHidError> {
        let mut data: [u8; 20] = [0; 20];

        // Did not make the packed struct work, so doing it manually
        data[0] = report.x as u8;
        data[1] = (report.x >> 8) as u8;
        data[2] = report.y as u8;
        data[3] = (report.y >> 8) as u8;
        data[4] = report.z as u8;
        data[5] = (report.z >> 8) as u8;
        data[6] = report.rx as u8;
        data[7] = (report.rx >> 8) as u8;
        data[8] = report.ry as u8;
        data[9] = (report.ry >> 8) as u8;
        data[10] = report.rz as u8;
        data[11] = (report.rz >> 8) as u8;
        data[12] = report.slider as u8;
        data[13] = (report.slider >> 8) as u8;
        data[14] = report.dial as u8;
        data[15] = (report.dial >> 8) as u8;
        data[16] = report.buttons as u8;
        data[17] = (report.buttons >> 8) as u8;
        data[18] = (report.buttons >> 16) as u8;
        data[19] = (report.buttons >> 24) as u8;

        self.interface
            .write_report(&data)
            .map(|_| ())
            .map_err(UsbHidError::from)
    }
}

impl<'a, B: UsbBus> DeviceClass<'a> for Joystick<'a, B> {
    type I = Interface<'a, B, InBytes32, OutNone, ReportSingle>;

    fn interface(&mut self) -> &mut Self::I {
        &mut self.interface
    }

    fn reset(&mut self) {}

    fn tick(&mut self) -> Result<(), UsbHidError> {
        Ok(())
    }
}

pub struct JoystickConfig<'a> {
    interface: InterfaceConfig<'a, InBytes32, OutNone, ReportSingle>,
}

impl Default for JoystickConfig<'_> {
    #[must_use]
    fn default() -> Self {
        Self::new(
            unwrap!(
                unwrap!(InterfaceBuilder::new(JOYSTICK_DESCRIPTOR))
                    .boot_device(InterfaceProtocol::None)
                    .description("Joystick")
                    .in_endpoint(10.millis())
            )
            .without_out_endpoint()
            .build(),
        )
    }
}

impl<'a> JoystickConfig<'a> {
    #[must_use]
    pub fn new(interface: InterfaceConfig<'a, InBytes32, OutNone, ReportSingle>) -> Self {
        Self { interface }
    }
}

impl<'a, B: UsbBus + 'a> UsbAllocatable<'a, B> for JoystickConfig<'a> {
    type Allocated = Joystick<'a, B>;

    fn allocate(self, usb_alloc: &'a UsbBusAllocator<B>) -> Self::Allocated {
        Self::Allocated {
            interface: Interface::new(usb_alloc, self.interface),
        }
    }
}