eurosim-obsw/src/can.rs

use crate::power_handler::DeviceState;

use crate::can_ids::{
    can_id_to_package_id, package_id_to_can_id, value_to_package_id, DeviceId, PackageId,
    PackageModel, SenderReceiverThread, ThreadId,
};
use embedded_can::{self, Frame};
use log::{debug, error, info, trace, warn};
use serde::{Deserialize, Serialize};
use serde_json;
use socketcan::{errors, frame, socket, CanFrame, Socket};
use std::collections::HashMap;
use std::ffi::c_void;
use std::io;
use std::mem::size_of;
use std::sync::mpsc::Sender;

pub struct CanRxHandler {
    interface: &'static str,
    socket: socket::CanSocket,
    //frame_id_to_sender_id: HashMap<embedded_can::Id, u32>, // double hash map: frame id -> receiver id -> sender handle
    can_senders: HashMap<ThreadId, Sender<PackageModel>>,
    //dismissed_ids: Vec<embedded_can::Id>,
    package_map: HashMap<PackageId, SenderReceiverThread>,
    //packet_id_to_sender_id: HashMap<PackageId, ThreadId>,
}

impl CanRxHandler {
    pub fn new_socket(
        interface: &'static str,
        //frame_id_to_sender_id: HashMap<embedded_can::Id, u32>,
        can_senders: HashMap<ThreadId, Sender<PackageModel>>,
        package_map: HashMap<PackageId, SenderReceiverThread>,
    ) -> Result<CanRxHandler, ()> {
        let socket = socket::CanSocket::open(&interface);
        if let Ok(socket) = socket {
            Ok(CanRxHandler {
                interface,
                socket,
                can_senders,
                package_map,
            })
        } else {
            Err(())
        }
    }

    /*
    pub fn new_socket_with_filter(
        interface: &'static str,
        can_id_to_sender_id: HashMap<embedded_can::Id, u32>,
        can_senders: HashMap<u32, Sender<Vec<u8>>>,
        can_filters: &[socket::CanFilter],
    ) -> Result<CanRxHandler, ()> {
        let can_wrapper = Self::new_socket(interface, can_id_to_sender_id, can_senders)?;
        let filter_result = can_wrapper.socket.set_filters(can_filters);
        if let Err(e) = filter_result {
            warn!("Can Bus set filter error: {}", e);
        }
        Ok(can_wrapper)
    }
    */

    pub fn read_frame(&self) -> io::Result<frame::CanFrame> {
        let frame = self.socket.read_frame();
        if let Err(e) = &frame {
            warn!("CAN bus read error: {}", e);
        }
        frame
    }

    pub fn rx_socket(&self) -> Option<CanFrame> {
        let frame = self.socket.read_frame().ok()?;
        info!("Can Frame read: {:?}.", frame);
        return Some(frame);
        /*
        if let Ok(frame) = frame {
            println!("Frame received: {:?}", frame);
            return Some(frame);
        }
        None
         */
    }

    pub fn forward_frame(&self, frame: CanFrame) {
        let frame_id = can_id_to_package_id(frame.id());
        info!("Frame forwarding with id: {:?}", frame_id);
        if let Some(frame_id) = frame_id {
            if self.package_map.contains_key(&frame_id) {
                let value = self.package_map.get(&frame_id).unwrap();
                if value.get_sender() != DeviceId::OBC {
                    let message_sender = self.can_senders.get(&value.get_thread()).unwrap();
                    let data = frame.data();
                    let message =
                        PackageModel::new(frame_id, data).expect("Error generating message.");
                    message_sender.send(message).expect(&*format!(
                        "Failure sending can bus frame to thread{:?}, frame id {:?}",
                        value.get_thread(),
                        frame_id
                    ));
                }
            }
        }
    }

    /*
    pub fn set_filter(&self, filters: &[socket::CanFilter]) -> io::Result<()> {
        info!("Setting filter with filter {:?}", filters);
        let result = self.socket.set_filters(filters);
        if let Err(e) = result {
            warn!("Can bus socket filter set error: {}", e);
        }
        Ok(())
    }
    */
}

pub struct CanTxHandler {
    interface: &'static str,
    socket: socket::CanSocket,
    thread_id: ThreadId,
    package_map: HashMap<PackageId, SenderReceiverThread>,
}

impl CanTxHandler {
    pub fn new_socket(
        interface: &'static str,
        thread_id: ThreadId,
        package_map: HashMap<PackageId, SenderReceiverThread>,
    ) -> Result<CanTxHandler, ()> {
        let socket = socket::CanSocket::open(&interface);

        if let Ok(socket) = socket {
            socket.filter_drop_all().unwrap(); // tx nodes cannot receive data
            Ok(CanTxHandler {
                interface,
                socket,
                thread_id,
                package_map,
            })
        } else {
            Err(())
        }
    }

    pub fn tx_socket(&self, package_id: PackageId, data: &[u8]) {
        if self.package_map.contains_key(&package_id) {
            let value = self.package_map.get(&package_id).unwrap();
            if value.get_sender() == DeviceId::OBC {
                //if value.get_thread() == self.thread_id {
                if data.len() <= 8 {
                    let frame_id = package_id_to_can_id(&package_id);
                    let frame = CanFrame::new(frame_id, data);
                    if let Some(frame) = frame {
                        self.socket
                            .write_frame(&frame)
                            .expect("Error writing frame.");
                    }
                } else {
                    warn!(
                        "Message dismissed, data length ({:?}) exceeds 8 bytes",
                        data.len()
                    );
                }
                /*} else {
                    warn!(
                        "Message dismissed, mismatched thread id: {:?}",
                        value.get_thread()
                    );
                }

                 */
            } else {
                warn!(
                    "Message dismissed, wrong sender id: {:?}",
                    value.get_sender()
                );
            }
        } else {
            warn!("Message dismissed, wrong package id: {:?}", package_id);
        }
    }

    /*
    pub fn tx_socket_from_frame(&self, frame: frame::CanFrame) -> io::Result<()> {
        let frame_id = frame.id();
        if !self.allowed_frame_ids.contains(&frame_id) {
            warn!(
                "Requested frame Id {:?} not allowed for current thread",
                frame.id()
            );
        } else if let Err(e) = self.socket.write_frame(&frame) {
            warn!("CAN bus write error: {}", e);
        }
        return Ok(());
    }

    pub fn tx_socket_from_data(
        &self,
        frame_id: embedded_can::Id,
        frame_data: &[u8],
    ) -> io::Result<()> {
        let frame = frame::CanFrame::new(frame_id, &frame_data).expect(&*format!(
            "Failure sending can bus frame with id {:?}",
            frame_id
        ));
        self.tx_socket_from_frame(frame)
    }
    */
}

pub fn open_socket(interface: &str) -> Result<socket::CanSocket, errors::CanSocketOpenError> {
    let socket = socket::CanSocket::open(&interface);
    return socket;
}