zencan-node 0.0.4

//! Implementation of LSS slave functionality
use defmt_or_log::info;
use zencan_common::AtomicCell;
use zencan_common::{
    lss::{
        LssConfigureError, LssIdentity, LssRequest, LssResponse, LssState, LSS_FASTSCAN_CONFIRM,
    },
    messages::MessageError,
    NodeId,
};

/// Events which can be generated by the LSS slave to the higher level node
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
#[allow(dead_code)]
pub enum LssEvent {
    /// Event to trigger configuration to be stored persistently
    StoreConfiguration,
    /// Activate a baud rate setting
    ActivateBitTiming {
        /// The table used for baud rate lookup
        table: u8,
        /// The index into the table
        index: u8,
        /// Specifies the delay that should be waited before applying the change
        delay: u16,
    },
    /// Set the node ID to the provided value
    ConfigureNodeId {
        /// The new node ID
        node_id: NodeId,
    },
}

/// A Sync structure providing message handling for message receive thread
pub(crate) struct LssReceiver {
    selected_identity: AtomicCell<LssIdentity>,
    rx_req: AtomicCell<Option<LssRequest>>,
}

impl Default for LssReceiver {
    fn default() -> Self {
        Self::new()
    }
}

impl LssReceiver {
    /// Create a new LssReceiver
    pub const fn new() -> Self {
        Self {
            selected_identity: AtomicCell::new(LssIdentity {
                vendor_id: 0,
                product_code: 0,
                revision: 0,
                serial: 0,
            }),
            rx_req: AtomicCell::new(None),
        }
    }

    /// Handle a received LSS request
    ///
    /// Returns true if the request requires further processing in the node 'process' thread
    pub fn handle_req(&self, req: LssRequest) -> bool {
        // Certain messages we store here for fast handling. Others, are stored to be handled during
        // process call
        match req {
            LssRequest::SwitchStateProduct { product_code } => {
                let mut selected_identity = self.selected_identity.load();
                selected_identity.product_code = product_code;
                self.selected_identity.store(selected_identity);
                false
            }
            LssRequest::SwitchStateVendor { vendor_id } => {
                let mut selected_identity = self.selected_identity.load();
                selected_identity.vendor_id = vendor_id;
                self.selected_identity.store(selected_identity);
                false
            }
            LssRequest::SwitchStateRevision { revision } => {
                let mut selected_identity = self.selected_identity.load();
                selected_identity.revision = revision;
                self.selected_identity.store(selected_identity);
                false
            }
            _ => {
                self.rx_req.store(Some(req));
                true
            }
        }
    }
}

pub struct LssConfig {
    /// The identity of the device
    pub identity: LssIdentity,
    /// The node ID of the device
    pub node_id: NodeId,
    /// Indicates the device supports storing of config
    pub store_supported: bool,
}

/// Implements LSS slave functionality
pub(crate) struct LssSlave {
    state: LssState,
    config: LssConfig,
    /// The current subindex for fast scan
    fast_scan_sub: u8,
    /// The identity selected by LSS master for configuration
    pending_node_id: NodeId,
    store_config_flag: bool,
}

impl LssSlave {
    /// Create a new LssSlave
    ///
    /// # Args
    /// - `config`: Application configuration
    pub fn new(config: LssConfig) -> Self {
        let pending_node_id = config.node_id;
        Self {
            state: LssState::Waiting,
            config,
            fast_scan_sub: 0,
            pending_node_id,
            store_config_flag: false,
        }
    }

    /// Create a new LssSlave using the existing slave's state, but with a new application config
    ///
    /// This should be called when config items are changed, e.g. when the node has changed it's ID
    pub fn update_config(&mut self, config: LssConfig) {
        self.pending_node_id = config.node_id;
        self.config = config;
    }

    /// Check for a pending event
    ///
    /// Should be called by the node after
    pub fn pending_event(&mut self) -> Option<LssEvent> {
        if self.pending_node_id != self.config.node_id {
            Some(LssEvent::ConfigureNodeId {
                node_id: self.pending_node_id,
            })
        } else if self.store_config_flag {
            self.store_config_flag = false;
            Some(LssEvent::StoreConfiguration)
        } else {
            None
        }
    }

    /// Process an LSS request, updating the state of the slave
    ///
    /// When a response is generated, it will be returned and should be transmitted to the CAN bus
    pub fn process(&mut self, receiver: &LssReceiver) -> Result<Option<LssResponse>, MessageError> {
        let request = match receiver.rx_req.take() {
            Some(req) => req,
            None => return Ok(None),
        };

        match request {
            LssRequest::SwitchModeGlobal { mode } => {
                // TODO: Device should enter NMT reset communication state when node ID is configured
                self.state = LssState::from_byte(mode)?;
                Ok(None)
            }

            LssRequest::ConfigureNodeId { node_id } => {
                if self.state == LssState::Configuring {
                    let mut error = 0;
                    if let Ok(node_id) = NodeId::try_from(node_id) {
                        self.pending_node_id = node_id;
                    } else {
                        error = LssConfigureError::NodeIdOutOfRange as u8;
                    }
                    Ok(Some(LssResponse::ConfigureNodeIdAck {
                        error,
                        spec_error: 0,
                    }))
                } else {
                    Ok(None)
                }
            }

            LssRequest::ConfigureBitTiming { table: _, index: _ } => {
                // Configuring bit timing is not supported
                if self.state == LssState::Configuring {
                    Ok(Some(LssResponse::ConfigureBitTimingAck {
                        error: 1,
                        spec_error: 0,
                    }))
                } else {
                    Ok(None)
                }
            }

            LssRequest::StoreConfiguration => {
                if self.state == LssState::Configuring {
                    if self.config.store_supported {
                        self.store_config_flag = true;
                        Ok(Some(LssResponse::StoreConfigurationAck {
                            error: 0,
                            spec_error: 0,
                        }))
                    } else {
                        Ok(Some(LssResponse::StoreConfigurationAck {
                            error: 1,
                            spec_error: 0,
                        }))
                    }
                } else {
                    Ok(None)
                }
            }

            // Other switch state commands (vendor, product, revision) are handled directly in the
            // receiver
            LssRequest::SwitchStateSerial { serial } => {
                if self.state == LssState::Waiting {
                    let mut selected_identity = receiver.selected_identity.load();
                    selected_identity.serial = serial;
                    if self.config.identity == selected_identity {
                        // If the identity matches, we are selected and enter the configuration state
                        self.state = LssState::Configuring;
                        Ok(Some(LssResponse::SwitchStateResponse))
                    } else {
                        Ok(None)
                    }
                } else {
                    Ok(None)
                }
            }

            LssRequest::InquireVendor => {
                if self.state == LssState::Configuring {
                    Ok(Some(LssResponse::InquireVendorAck {
                        vendor_id: self.config.identity.vendor_id,
                    }))
                } else {
                    Ok(None)
                }
            }
            LssRequest::InquireProduct => {
                if self.state == LssState::Configuring {
                    Ok(Some(LssResponse::InquireProductAck {
                        product_code: self.config.identity.product_code,
                    }))
                } else {
                    Ok(None)
                }
            }
            LssRequest::InquireRev => {
                if self.state == LssState::Configuring {
                    Ok(Some(LssResponse::InquireRevAck {
                        revision: self.config.identity.revision,
                    }))
                } else {
                    Ok(None)
                }
            }
            LssRequest::InquireSerial => {
                if self.state == LssState::Configuring {
                    Ok(Some(LssResponse::InquireSerialAck {
                        serial_number: self.config.identity.serial,
                    }))
                } else {
                    Ok(None)
                }
            }
            LssRequest::InquireNodeId => {
                if self.state == LssState::Configuring {
                    Ok(Some(LssResponse::InquireNodeIdAck {
                        node_id: self.config.node_id.into(),
                    }))
                } else {
                    Ok(None)
                }
            }
            LssRequest::FastScan {
                id,
                bit_check,
                sub,
                next,
            } => {
                if self.config.node_id == NodeId::Unconfigured && self.state == LssState::Waiting {
                    if bit_check == LSS_FASTSCAN_CONFIRM {
                        // Reset state machine and confirm
                        self.fast_scan_sub = 0;
                        Ok(Some(LssResponse::IdentifySlave))
                    } else if self.fast_scan_sub == sub {
                        let mask = 0xFFFFFFFFu32 << bit_check;
                        if self.config.identity.by_addr(sub) & mask == (id & mask) {
                            self.fast_scan_sub = next;
                            if bit_check == 0 && next < sub {
                                // All bits matched, enter configuration state
                                info!("Fast scan complete, entering configuration state");
                                self.state = LssState::Configuring;
                            }
                            Ok(Some(LssResponse::IdentifySlave))
                        } else {
                            Ok(None)
                        }
                    } else {
                        Ok(None)
                    }
                } else {
                    Ok(None)
                }
            }
            _ => Ok(None),
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_fast_scan_simple() {
        const VENDOR_ID: u32 = 0x0;
        const PRODUCT_CODE: u32 = 0x1;
        const REVISION: u32 = 0x2;
        const SERIAL_NUMBER: u32 = 0x3;
        const IDENTITY: LssIdentity = LssIdentity {
            vendor_id: VENDOR_ID,
            product_code: PRODUCT_CODE,
            revision: REVISION,
            serial: SERIAL_NUMBER,
        };

        // Create new LSS slave with unconfigured node_id
        let mut slave = LssSlave::new(LssConfig {
            node_id: NodeId::Unconfigured,
            identity: IDENTITY,
            store_supported: true,
        });

        let rx = LssReceiver::new();

        // Send confirmation message, and it should always ACK
        rx.rx_req.store(Some(LssRequest::FastScan {
            id: 0x00000000,
            bit_check: LSS_FASTSCAN_CONFIRM,
            sub: 0,
            next: 1,
        }));
        assert_eq!(slave.process(&rx), Ok(Some(LssResponse::IdentifySlave)));

        // 0 Matches
        rx.rx_req.store(Some(LssRequest::FastScan {
            id: 0x00000000,
            bit_check: 31,
            sub: 0,
            next: 1,
        }));
        assert_eq!(slave.process(&rx), Ok(Some(LssResponse::IdentifySlave)));

        // 1 does not match
        rx.rx_req.store(Some(LssRequest::FastScan {
            id: 0x00000001,
            bit_check: 31,
            sub: 0,
            next: 1,
        }));
        assert_eq!(slave.process(&rx), Ok(None));
    }

    /// Make sure that the slave goes into the configuration state after a complete scan
    #[test]
    fn test_fast_scan_configure() {
        const VENDOR_ID: u32 = 0x0;
        const PRODUCT_CODE: u32 = 0x1;
        const REVISION: u32 = 0x2;
        const SERIAL_NUMBER: u32 = 0x3;
        const IDENTITY: LssIdentity = LssIdentity {
            vendor_id: VENDOR_ID,
            product_code: PRODUCT_CODE,
            revision: REVISION,
            serial: SERIAL_NUMBER,
        };

        let mut slave = LssSlave::new(LssConfig {
            node_id: NodeId::Unconfigured,
            identity: IDENTITY,
            store_supported: true,
        });

        let mut id = [0, 0, 0, 0];
        let mut sub = 0;
        let mut next = 0;
        let mut bit_check;

        fn send_fs(slave: &mut LssSlave, id: &[u32; 4], bit_check: u8, sub: u8, next: u8) -> bool {
            let rx = LssReceiver::new();
            rx.rx_req.store(Some(LssRequest::FastScan {
                id: id[sub as usize],
                bit_check,
                sub,
                next,
            }));
            let resp = slave.process(&rx).unwrap();

            matches!(resp, Some(LssResponse::IdentifySlave))
        }

        // The first message resets the LSS state machines, and a response confirms that there is at
        // least one unconfigured slave to discover
        assert!(
            send_fs(&mut slave, &id, LSS_FASTSCAN_CONFIRM, sub, next),
            "No confirmation response"
        );

        while sub < 4 {
            bit_check = 32;
            while bit_check > 0 {
                bit_check -= 1;
                if !send_fs(&mut slave, &id, bit_check, sub, next) {
                    id[sub as usize] |= 1 << bit_check;
                }
            }
            next = (sub + 1) % 4;
            assert!(
                send_fs(&mut slave, &id, bit_check, sub, next),
                "No ack after completing sub {}, id: {:?}",
                sub,
                id
            );

            sub += 1;
        }

        assert_eq!(id, [0x0, 0x1, 0x2, 0x3]);
        assert_eq!(slave.state, LssState::Configuring);
    }

    #[test]
    fn test_node_id_configuration_with_store() {
        const VENDOR_ID: u32 = 0x0;
        const PRODUCT_CODE: u32 = 0x1;
        const REVISION: u32 = 0x2;
        const SERIAL_NUMBER: u32 = 0x3;
        const IDENTITY: LssIdentity = LssIdentity {
            vendor_id: VENDOR_ID,
            product_code: PRODUCT_CODE,
            revision: REVISION,
            serial: SERIAL_NUMBER,
        };

        let mut slave = LssSlave::new(LssConfig {
            node_id: NodeId::Unconfigured,
            identity: IDENTITY,
            store_supported: true,
        });

        let rx = LssReceiver::new();

        let node_id = NodeId::new(10).unwrap();

        // Put the slave into Configuring mode
        rx.rx_req
            .store(Some(LssRequest::SwitchModeGlobal { mode: 1 }));
        let _ = slave.process(&rx).unwrap();

        // Send command to set the node ID
        rx.rx_req.store(Some(LssRequest::ConfigureNodeId {
            node_id: node_id.raw(),
        }));
        let resp = slave.process(&rx).unwrap();
        assert_eq!(
            Some(LssResponse::ConfigureNodeIdAck {
                error: 0,
                spec_error: 0
            }),
            resp
        );
        // Slave should return event to application
        assert_eq!(
            Some(LssEvent::ConfigureNodeId { node_id }),
            slave.pending_event()
        );
        // Update with new application config
        slave.update_config(LssConfig {
            node_id: NodeId::new(10).unwrap(),
            identity: IDENTITY,
            store_supported: true,
        });
        // Event should be cleared
        assert_eq!(None, slave.pending_event());

        // send command to store config
        rx.rx_req.store(Some(LssRequest::StoreConfiguration));
        let resp = slave.process(&rx).unwrap();
        assert_eq!(
            Some(LssResponse::StoreConfigurationAck {
                error: 0,
                spec_error: 0
            }),
            resp
        );
        // Event should be returned
        assert_eq!(Some(LssEvent::StoreConfiguration), slave.pending_event());
        // Event should be automatically cleared after being returned once
        assert_eq!(None, slave.pending_event());
    }

    #[test]
    fn test_node_id_configuration_without_store() {
        const VENDOR_ID: u32 = 0x0;
        const PRODUCT_CODE: u32 = 0x1;
        const REVISION: u32 = 0x2;
        const SERIAL_NUMBER: u32 = 0x3;
        const IDENTITY: LssIdentity = LssIdentity {
            vendor_id: VENDOR_ID,
            product_code: PRODUCT_CODE,
            revision: REVISION,
            serial: SERIAL_NUMBER,
        };

        let mut slave = LssSlave::new(LssConfig {
            node_id: NodeId::Unconfigured,
            identity: IDENTITY,
            store_supported: false,
        });

        let rx = LssReceiver::new();

        // Put the slave into Configuring mode
        rx.rx_req
            .store(Some(LssRequest::SwitchModeGlobal { mode: 1 }));
        let _ = slave.process(&rx).unwrap();

        rx.rx_req
            .store(Some(LssRequest::ConfigureNodeId { node_id: 10 }));
        let resp = slave.process(&rx).unwrap();
        assert_eq!(
            Some(LssResponse::ConfigureNodeIdAck {
                error: 0,
                spec_error: 0
            }),
            resp
        );
        slave.update_config(LssConfig {
            node_id: NodeId::new(10).unwrap(),
            identity: IDENTITY,
            store_supported: false,
        });

        rx.rx_req.store(Some(LssRequest::StoreConfiguration));
        let resp = slave.process(&rx).unwrap();
        assert_eq!(
            Some(LssResponse::StoreConfigurationAck {
                error: 1,
                spec_error: 0
            }),
            resp
        );
        // No events
        assert_eq!(None, slave.pending_event());
    }
}