From ecf85cf6104daf3e5a48449efcb7971afb040870 Mon Sep 17 00:00:00 2001
From: paul nehlich <paul.nehlich@irs.uni-stuttgart.de>
Date: Mon, 17 Jun 2024 12:32:07 +0200
Subject: [PATCH] Added can test code

---
 bsp_z7/ps7_cortexa9_0/include/xcanps.h    | 576 ++++++++++++++++++++++
 bsp_z7/ps7_cortexa9_0/include/xcanps_hw.h | 347 +++++++++++++
 mission/can_rust_helper.c                 |  20 +-
 mission_rust/src/fsrc/osal/mod.rs         |   5 +-
 mission_rust/src/lib.rs                   |  17 +
 5 files changed, 959 insertions(+), 6 deletions(-)
 create mode 100644 bsp_z7/ps7_cortexa9_0/include/xcanps.h
 create mode 100644 bsp_z7/ps7_cortexa9_0/include/xcanps_hw.h

diff --git a/bsp_z7/ps7_cortexa9_0/include/xcanps.h b/bsp_z7/ps7_cortexa9_0/include/xcanps.h
new file mode 100644
index 0000000..b9e2cd3
--- /dev/null
+++ b/bsp_z7/ps7_cortexa9_0/include/xcanps.h
@@ -0,0 +1,576 @@
+/******************************************************************************
+* Copyright (C) 2010 - 2021 Xilinx, Inc.  All rights reserved.
+* Copyright (C) 2023 Advanced Micro Devices, Inc. All Rights Reserved.
+* SPDX-License-Identifier: MIT
+******************************************************************************/
+
+/*****************************************************************************/
+/**
+*
+* @file xcanps.h
+* @addtogroup canps Overview
+* @{
+* @details
+*
+* The Xilinx CAN driver component.  This component supports the Xilinx
+* CAN Controller.
+*
+* The CAN Controller supports the following features:
+*	- Confirms to the ISO 11898-1, CAN 2.0A and CAN 2.0B standards.
+*	- Supports both Standard (11 bit Identifier) and Extended (29 bit
+*	  Identifier) frames.
+*	- Supports Bit Rates up to 1 Mbps.
+*	- Transmit message object FIFO with a user configurable depth of
+*	  up to 64 message objects.
+*	- Transmit prioritization through one TX High Priority Buffer.
+*	- Receive message object FIFO with a user configurable depth of
+*	  up to 64 message objects.
+*	- Watermark interrupts for Rx FIFO with configurable Watermark.
+*	- Acceptance filtering with 4 acceptance filters.
+*	- Sleep mode with automatic wake up.
+*	- Loop Back mode for diagnostic applications.
+*	- Snoop mode for diagnostic applications.
+*	- Maskable Error and Status Interrupts.
+*	- Readable Error Counters.
+*	- External PHY chip required.
+*	- Receive Timestamp.
+*
+* The device driver supports all the features listed above, if applicable.
+*
+* <b>Driver Description</b>
+*
+* The device driver enables higher layer software (e.g., an application) to
+* communicate to the CAN. The driver handles transmission and reception of
+* CAN frames, as well as configuration of the controller. The driver is simply a
+* pass-through mechanism between a protocol stack and the CAN. A single device
+* driver can support multiple CANs.
+*
+* Since the driver is a simple pass-through mechanism between a protocol stack
+* and the CAN, no assembly or disassembly of CAN frames is done at the
+* driver-level. This assumes that the protocol stack passes a correctly
+* formatted CAN frame to the driver for transmission, and that the driver
+* does not validate the contents of an incoming frame
+*
+* <b>Operation Modes</b>
+*
+* The CAN controller supports the following modes of operation:
+*   - <b>Configuration Mode</b>: In this mode the CAN timing parameters and
+*	 Baud Rate Pre-scalar parameters can be changed. In this mode the CAN
+*	 controller loses synchronization with the CAN bus and drives a
+*	 constant recessive bit on the bus line. The Error Counter Register are
+*	 reset. The CAN controller does not receive or transmit any messages
+*	 even if there are pending transmit requests from the TX FIFO or the TX
+*	 High Priority Buffer. The Storage FIFOs and the CAN configuration
+*	 registers are still accessible.
+*   - <b>Normal Mode</b>:In Normal Mode the CAN controller participates in bus
+*	 communication, by transmitting and receiving messages.
+*   - <b>Sleep Mode</b>: In Sleep Mode the CAN Controller does not transmit any
+*	 messages. However, if any other node transmits a message, then the CAN
+*	 Controller receives the transmitted message and exits from Sleep Mode.
+*	 If there are new transmission requests from either the TX FIFO or the
+*	 TX High Priority Buffer when the CAN Controller is in Sleep Mode, these
+*	 requests are not serviced, and the CAN Controller continues to remain
+*	 in Sleep Mode. Interrupts are generated when the CAN controller enters
+*	Sleep mode or Wakes up from Sleep mode.
+*   - <b>Loop Back Mode</b>: In Loop Back mode, the CAN controller transmits a
+*	 recessive bit stream on to the CAN Bus. Any message that is transmitted
+*	 is looped back to the �Rx� line and acknowledged. The CAN controller
+*	 thus receives any message that it transmits. It does not participate in
+*	 normal bus communication and does not receive any messages that are
+*	 transmitted by other CAN nodes. This mode is used for diagnostic
+*	 purposes.
+*   - <b>Snoop Mode</b>: In Snoop mode, the CAN controller transmits a
+*	 recessive bit stream on to the CAN Bus and does not participate
+*	 in normal bus communication but receives messages that are transmitted
+*	 by other CAN nodes. This mode is used for diagnostic purposes.
+*
+*
+* <b>Buffer Alignment</b>
+*
+* It is important to note that frame buffers passed to the driver must be
+* 32-bit aligned.
+*
+* <b>Receive Address Filtering</b>
+*
+* The device can be set to accept frames whose Identifiers match any of the
+* 4 filters set in the Acceptance Filter Mask/ID registers.
+*
+* The incoming Identifier is masked with the bits in the Acceptance Filter Mask
+* Register. This value is compared with the result of masking the bits in the
+* Acceptance Filter ID Register with the Acceptance Filter Mask Register. If
+* both these values are equal, the message will be stored in the RX FIFO.
+*
+* Acceptance Filtering is performed by each of the defined acceptance filters.
+* If the incoming identifier passes through any acceptance filter then the
+* frame is stored in the RX FIFO.
+*
+* If the Accpetance Filters are not set up then all the received messages are
+* stroed in the RX FIFO.
+*
+* <b>PHY Communication</b>
+*
+* This driver does not provide any mechanism for directly programming PHY.
+*
+* <b>Interrupts</b>
+*
+* The driver has no dependencies on the interrupt controller. The driver
+* provides an interrupt handler. User of this driver needs to provide
+* callback functions. An interrupt handler example is available with
+* the driver.
+*
+* <b>Threads</b>
+*
+* This driver is not thread safe.  Any needs for threads or thread mutual
+* exclusion must be satisfied by the layer above this driver.
+*
+* <b>Device Reset</b>
+*
+* Bus Off interrupt that can occur in the device requires a device reset.
+* The user is responsible for resetting the device and re-configuring it
+* based on its needs (the driver does not save the current configuration).
+* When integrating into an RTOS, these reset and re-configure obligations are
+* taken care of by the OS adapter software if it exists for that RTOS.
+*
+* <b>Device Configuration</b>
+*
+* The device can be configured in various ways during the FPGA implementation
+* process. Configuration parameters are stored in the xcanps_g.c files.
+* A table is defined where each entry contains configuration information
+* for a CAN device.  This information includes such things as the base address
+* of the memory-mapped device.
+*
+* <b>Asserts</b>
+*
+* Asserts are used within all Xilinx drivers to enforce constraints on argument
+* values. Asserts can be turned off on a system-wide basis by defining, at
+* compile time, the NDEBUG identifier. By default, asserts are turned on and it
+* is recommended that users leave asserts on during development.
+*
+* <b>Building the driver</b>
+*
+* The XCanPs driver is composed of several source files. This allows the user
+* to build and link only those parts of the driver that are necessary.
+* <br><br>
+*
+* <pre>
+* MODIFICATION HISTORY:
+*
+* Ver   Who    Date	Changes
+* ----- -----  -------- -----------------------------------------------
+* 1.00a xd/sv  01/12/10 First release
+* 1.01a bss    12/27/11 Added the APIs XCanPs_SetTxIntrWatermark and
+* 			XCanPs_GetTxIntrWatermark.
+*			Updated the Register/bit definitions
+*                       Changed XCANPS_RXFWIR_RXFLL_MASK to XCANPS_WIR_FW_MASK
+*                       Changed XCANPS_RXWIR_OFFSET to XCANPS_WIR_OFFSET
+*			Added XCANPS_IXR_TXFEMP_MASK for Tx Fifo Empty
+*			Changed XCANPS_IXR_RXFLL_MASK to
+*			XCANPS_IXR_RXFWMFLL_MASK
+* 			Changed
+*			XCANPS_TXBUF_ID_OFFSET to XCANPS_TXHPB_ID_OFFSET
+* 			XCANPS_TXBUF_DLC_OFFSET to XCANPS_TXHPB_DLC_OFFSET
+*			XCANPS_TXBUF_DW1_OFFSET to XCANPS_TXHPB_DW1_OFFSET
+*			XCANPS_TXBUF_DW2_OFFSET to XCANPS_TXHPB_DW2_OFFSET
+* 2.1 adk 		23/08/14 Fixed CR:798792 Peripheral test for CANPS IP in
+*			SDK claims a 40kbps baud rate but it's not.
+* 3.0 adk     09/12/14  Added support for Zynq Ultrascale Mp.Also code
+*			modified for MISRA-C:2012 compliance.
+* 3.1 adk     10/11/15  Fixed CR#911958 Add support for Tx Watermark example.
+*			Data mismatch while sending data less than 8 bytes.
+* 3.1 nsk     12/21/15  Updated XCanPs_IntrHandler in xcanps_intr.c to handle
+*			error interrupts correctly. CR#925615
+*     ms      03/17/17  Added readme.txt file in examples folder for doxygen
+*                       generation.
+* 3.3 sne     08/06/19	Fixed coverity warnings.
+* 3.5 sne     06/29/20  Fixed MISRA-C violations.
+* 3.5 sne     06/29/20  Fix multiple packets send issue CR-1066438.
+* 3.5 sne     08/28/20  Modify Makefile to support parallel make execution.
+* 3.7 ht      06/28/23  Added support for system device-tree flow.
+* </pre>
+*
+******************************************************************************/
+#ifndef XCANPS_H			/* prevent circular inclusions */
+#define XCANPS_H			/**< by using protection macros */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/***************************** Include Files *********************************/
+
+#include "xstatus.h"
+#include "xcanps_hw.h"
+#include "xil_types.h"
+
+/************************** Constant Definitions *****************************/
+
+/** @name CAN operation modes
+ *  @{
+ */
+#define XCANPS_MODE_CONFIG	0x00000001U /**< Configuration mode */
+#define XCANPS_MODE_NORMAL	0x00000002U /**< Normal mode */
+#define XCANPS_MODE_LOOPBACK	0x00000004U /**< Loop Back mode */
+#define XCANPS_MODE_SLEEP	0x00000008U /**< Sleep mode */
+#define XCANPS_MODE_SNOOP	0x00000010U /**< Snoop mode */
+/** @} */
+
+/** @name Callback identifiers used as parameters to XCanPs_SetHandler()
+ *  @{
+ */
+#define XCANPS_HANDLER_SEND 1U /**< Handler type for frame sending interrupt */
+#define XCANPS_HANDLER_RECV 2U /**< Handler type for frame reception interrupt*/
+#define XCANPS_HANDLER_ERROR  3U /**< Handler type for error interrupt */
+#define XCANPS_HANDLER_EVENT  4U /**< Handler type for all other interrupts */
+/** @} */
+
+/**************************** Type Definitions *******************************/
+
+/**
+ * This typedef contains configuration information for a device.
+ */
+typedef struct {
+#ifndef SDT
+	u16 DeviceId;		/**< Unique ID of device */
+#else
+	char *Name;		/**< Unique name of the device */
+#endif
+	UINTPTR BaseAddr;	/**< Register base address */
+#ifdef SDT
+	u16 IntrId;             /** Bits[11:0] Interrupt-id Bits[15:12]
+				  * trigger type and level flags */
+	UINTPTR IntrParent;     /** Bit[0] Interrupt parent type Bit[64/32:1]
+				  * Parent base address */
+#endif
+} XCanPs_Config;
+
+/******************************************************************************/
+/**
+ * Callback type for frame sending and reception interrupts.
+ *
+ * @param 	CallBackRef is a callback reference passed in by the upper layer
+ *		when setting the callback functions, and passed back to the
+ *		upper layer when the callback is invoked.
+*******************************************************************************/
+typedef void (*XCanPs_SendRecvHandler) (void *CallBackRef);
+
+/******************************************************************************/
+/**
+ * Callback type for error interrupt.
+ *
+ * @param	CallBackRef is a callback reference passed in by the upper layer
+ *		when setting the callback functions, and passed back to the
+ *		upper layer when the callback is invoked.
+ * @param	ErrorMask is a bit mask indicating the cause of the error. Its
+ *		value equals 'OR'ing one or more XCANPS_ESR_* values defined in
+ *		xcanps_hw.h
+*******************************************************************************/
+typedef void (*XCanPs_ErrorHandler) (void *CallBackRef, u32 ErrorMask);
+
+/******************************************************************************/
+/**
+ * Callback type for all kinds of interrupts except sending frame interrupt,
+ * receiving frame interrupt, and error interrupt.
+ *
+ * @param	CallBackRef is a callback reference passed in by the upper layer
+ *		when setting the callback functions, and passed back to the
+ *		upper layer when the callback is invoked.
+ * @param	Mask is a bit mask indicating the pending interrupts. Its value
+ *		equals 'OR'ing one or more XCANPS_IXR_* defined in xcanps_hw.h
+*******************************************************************************/
+typedef void (*XCanPs_EventHandler) (void *CallBackRef, u32 Mask);
+
+/**
+ * The XCanPs driver instance data. The user is required to allocate a
+ * variable of this type for every CAN device in the system. A pointer
+ * to a variable of this type is then passed to the driver API functions.
+ */
+typedef struct {
+	XCanPs_Config CanConfig; 	/**< Device configuration */
+	u32 IsReady;			/**< Device is initialized and ready */
+
+	/**
+	 * Callback and callback reference for TXOK interrupt.
+	 */
+	XCanPs_SendRecvHandler SendHandler;
+	void *SendRef;		/**< Reference Pointer for Send */
+
+	/**
+	 * Callback and callback reference for RXOK/RXNEMP/RXFLL interrupts.
+	 */
+	XCanPs_SendRecvHandler RecvHandler;
+	void *RecvRef;		/**< Reference Pointer for Receive */
+
+	/**
+	 * Callback and callback reference for ERROR interrupt.
+	 */
+	XCanPs_ErrorHandler ErrorHandler;
+	void *ErrorRef;		/**< Reference Pointer for Error */
+
+	/**
+	 * Callback  and callback reference for RXOFLW/RXUFLW/TXBFLL/TXFLL/
+	 * Wakeup/Sleep/Bus off/ARBLST interrupts.
+	 */
+	XCanPs_EventHandler EventHandler;
+	void *EventRef;		 /**< Reference Pointer for Event */
+	u32 IsBusy;              /**< A transfer is in progress (state) */
+
+} XCanPs;
+
+
+/************************** Variable Definitions *****************************/
+
+extern XCanPs_Config XCanPs_ConfigTable[];
+
+/***************** Macros (Inline Functions) Definitions *********************/
+
+/****************************************************************************/
+/**
+*
+* This macro checks if the transmission is complete.
+*
+* @param	InstancePtr is a pointer to the XCanPs instance.
+*
+* @return
+*		- TRUE if the transmission is done.
+*		- FALSE if the transmission is not done.
+*
+* @note		C-Style signature:
+*		int XCanPs_IsTxDone(XCanPs *InstancePtr)
+*
+*******************************************************************************/
+#define XCanPs_IsTxDone(InstancePtr) \
+	(((XCanPs_ReadReg(((InstancePtr)->CanConfig.BaseAddr),		\
+			  XCANPS_ISR_OFFSET) & XCANPS_IXR_TXOK_MASK) != (u32)0) ? TRUE : FALSE)
+
+
+/****************************************************************************/
+/**
+*
+* This macro checks if the transmission FIFO is full.
+*
+* @param	InstancePtr is a pointer to the XCanPs instance.
+*
+* @return
+*		- TRUE if TX FIFO is full.
+*		- FALSE if the TX FIFO is NOT full.
+*
+* @note		C-Style signature:
+*		int XCanPs_IsTxFifoFull(XCanPs *InstancePtr)
+*
+*****************************************************************************/
+#define XCanPs_IsTxFifoFull(InstancePtr) \
+	(((XCanPs_ReadReg(((InstancePtr)->CanConfig.BaseAddr), 	\
+			  XCANPS_SR_OFFSET) & XCANPS_SR_TXFLL_MASK) != (u32)0) ? TRUE : FALSE)
+
+
+/****************************************************************************/
+/**
+*
+* This macro checks if the Transmission High Priority Buffer is full.
+*
+* @param	InstancePtr is a pointer to the XCanPs instance.
+*
+* @return
+*		- TRUE if the TX High Priority Buffer is full.
+*		- FALSE if the TX High Priority Buffer is NOT full.
+*
+* @note		C-Style signature:
+*		int XCanPs_IsHighPriorityBufFull(XCanPs *InstancePtr)
+*
+*****************************************************************************/
+#define XCanPs_IsHighPriorityBufFull(InstancePtr) \
+	(((XCanPs_ReadReg(((InstancePtr)->CanConfig.BaseAddr), 	\
+			  XCANPS_SR_OFFSET) & XCANPS_SR_TXBFLL_MASK) != (u32)0) ? TRUE : FALSE)
+
+
+/****************************************************************************/
+/**
+*
+* This macro checks if the receive FIFO is empty.
+*
+* @param	InstancePtr is a pointer to the XCanPs instance.
+*
+* @return
+*		- TRUE if RX FIFO is empty.
+*		- FALSE if the RX FIFO is NOT empty.
+*
+* @note		C-Style signature:
+*		int XCanPs_IsRxEmpty(XCanPs *InstancePtr)
+*
+*****************************************************************************/
+#define XCanPs_IsRxEmpty(InstancePtr) \
+	(((XCanPs_ReadReg(((InstancePtr)->CanConfig.BaseAddr), 	\
+			  XCANPS_ISR_OFFSET) & XCANPS_IXR_RXNEMP_MASK) != (u32)0) ? FALSE : TRUE)
+
+
+/****************************************************************************/
+/**
+*
+* This macro checks if the CAN device is ready for the driver to change
+* Acceptance Filter Identifier Registers (AFIR) and Acceptance Filter Mask
+* Registers (AFMR).
+*
+* AFIR and AFMR for a filter are changeable only after the filter is disabled
+* and this routine returns FALSE. The filter can be disabled using the
+* XCanPs_AcceptFilterDisable function.
+*
+* Use the XCanPs_Accept_* functions for configuring the acceptance filters.
+*
+* @param	InstancePtr is a pointer to the XCanPs instance.
+*
+* @return
+*		- TRUE if the device is busy and NOT ready to accept writes to
+*		AFIR and AFMR.
+*		- FALSE if the device is ready to accept writes to AFIR and
+*		AFMR.
+*
+* @note		C-Style signature:
+*		int XCanPs_IsAcceptFilterBusy(XCanPs *InstancePtr)
+*
+*****************************************************************************/
+#define XCanPs_IsAcceptFilterBusy(InstancePtr) 		\
+	(((XCanPs_ReadReg(((InstancePtr)->CanConfig.BaseAddr), 	\
+			  XCANPS_SR_OFFSET) & XCANPS_SR_ACFBSY_MASK) != (u32)0) ? TRUE : FALSE)
+
+
+/****************************************************************************/
+/**
+*
+* This macro calculates CAN message identifier value given identifier field
+* values.
+*
+* @param	StandardId contains Standard Message ID value.
+* @param	SubRemoteTransReq contains Substitute Remote Transmission
+*		Request value.
+* @param	IdExtension contains Identifier Extension value.
+* @param	ExtendedId contains Extended Message ID value.
+* @param	RemoteTransReq contains Remote Transmission Request value.
+*
+* @return	Message Identifier value.
+*
+* @note		C-Style signature:
+*		u32 XCanPs_CreateIdValue(u32 StandardId,
+*					u32 SubRemoteTransReq,
+*					u32 IdExtension, u32 ExtendedId,
+*					u32 RemoteTransReq)
+*
+*		Read the CAN specification for meaning of each parameter.
+*
+*****************************************************************************/
+#define XCanPs_CreateIdValue(StandardId, SubRemoteTransReq, IdExtension, \
+			     ExtendedId, RemoteTransReq) 				\
+((((StandardId) << XCANPS_IDR_ID1_SHIFT) & XCANPS_IDR_ID1_MASK) |	\
+ (((SubRemoteTransReq) << XCANPS_IDR_SRR_SHIFT) & XCANPS_IDR_SRR_MASK)|\
+ (((IdExtension) << XCANPS_IDR_IDE_SHIFT) & XCANPS_IDR_IDE_MASK) |	\
+ (((ExtendedId) << XCANPS_IDR_ID2_SHIFT) & XCANPS_IDR_ID2_MASK) |	\
+ ((RemoteTransReq) & XCANPS_IDR_RTR_MASK))
+
+
+/****************************************************************************/
+/**
+*
+* This macro calculates value for Data Length Code register given Data
+* Length Code value.
+*
+* @param	DataLengCode indicates Data Length Code value.
+*
+* @return	Value that can be assigned to Data Length Code register.
+*
+* @note		C-Style signature:
+*		u32 XCanPs_CreateDlcValue(u32 DataLengCode)
+*
+*		Read the CAN specification for meaning of Data Length Code.
+*
+*****************************************************************************/
+#define XCanPs_CreateDlcValue(DataLengCode) \
+	(((DataLengCode) << XCANPS_DLCR_DLC_SHIFT) & XCANPS_DLCR_DLC_MASK)
+
+
+/****************************************************************************/
+/**
+*
+* This macro clears the timestamp in the Timestamp Control Register.
+*
+* @param	InstancePtr is a pointer to the XCanPs instance.
+*
+* @return	None.
+*
+* @note		C-Style signature:
+*		void XCanPs_ClearTimestamp(XCanPs *InstancePtr)
+*
+*****************************************************************************/
+#define XCanPs_ClearTimestamp(InstancePtr) 			\
+	XCanPs_WriteReg((InstancePtr)->CanConfig.BaseAddr, 		\
+			XCANPS_TCR_OFFSET, XCANPS_TCR_CTS_MASK)
+
+/************************** Function Prototypes ******************************/
+
+/*
+ * Functions in xcanps.c
+ */
+s32 XCanPs_CfgInitialize(XCanPs *InstancePtr, XCanPs_Config *ConfigPtr,
+			 UINTPTR EffectiveAddr);
+
+void XCanPs_Reset(XCanPs *InstancePtr);
+u8 XCanPs_GetMode(XCanPs *InstancePtr);
+void XCanPs_EnterMode(XCanPs *InstancePtr, u8 OperationMode);
+u32 XCanPs_GetStatus(XCanPs *InstancePtr);
+void XCanPs_GetBusErrorCounter(XCanPs *InstancePtr, u8 *RxErrorCount,
+			       u8 *TxErrorCount);
+u32 XCanPs_GetBusErrorStatus(XCanPs *InstancePtr);
+void XCanPs_ClearBusErrorStatus(XCanPs *InstancePtr, u32 Mask);
+s32 XCanPs_Send(XCanPs *InstancePtr, u32 *FramePtr);
+s32 XCanPs_Recv(XCanPs *InstancePtr, u32 *FramePtr);
+s32 XCanPs_SendHighPriority(XCanPs *InstancePtr, u32 *FramePtr);
+void XCanPs_AcceptFilterEnable(XCanPs *InstancePtr, u32 FilterIndexes);
+void XCanPs_AcceptFilterDisable(XCanPs *InstancePtr, u32 FilterIndexes);
+u32 XCanPs_AcceptFilterGetEnabled(XCanPs *InstancePtr);
+s32 XCanPs_AcceptFilterSet(XCanPs *InstancePtr, u32 FilterIndex,
+			   u32 MaskValue, u32 IdValue);
+void XCanPs_AcceptFilterGet(XCanPs *InstancePtr, u32 FilterIndex,
+			    u32 *MaskValue, u32 *IdValue);
+
+s32 XCanPs_SetBaudRatePrescaler(XCanPs *InstancePtr, u8 Prescaler);
+u8 XCanPs_GetBaudRatePrescaler(XCanPs *InstancePtr);
+s32 XCanPs_SetBitTiming(XCanPs *InstancePtr, u8 SyncJumpWidth,
+			u8 TimeSegment2, u8 TimeSegment1);
+void XCanPs_GetBitTiming(XCanPs *InstancePtr, u8 *SyncJumpWidth,
+			 u8 *TimeSegment2, u8 *TimeSegment1);
+
+s32 XCanPs_SetRxIntrWatermark(XCanPs *InstancePtr, u8 Threshold);
+u8 XCanPs_GetRxIntrWatermark(XCanPs *InstancePtr);
+s32 XCanPs_SetTxIntrWatermark(XCanPs *InstancePtr, u8 Threshold);
+u8 XCanPs_GetTxIntrWatermark(XCanPs *InstancePtr);
+
+/*
+ * Diagnostic functions in xcanps_selftest.c
+ */
+s32 XCanPs_SelfTest(XCanPs *InstancePtr);
+
+/*
+ * Functions in xcanps_intr.c
+ */
+void XCanPs_IntrEnable(XCanPs *InstancePtr, u32 Mask);
+void XCanPs_IntrDisable(XCanPs *InstancePtr, u32 Mask);
+u32 XCanPs_IntrGetEnabled(XCanPs *InstancePtr);
+u32 XCanPs_IntrGetStatus(XCanPs *InstancePtr);
+void XCanPs_IntrClear(XCanPs *InstancePtr, u32 Mask);
+void XCanPs_IntrHandler(void *InstancePtr);
+s32 XCanPs_SetHandler(XCanPs *InstancePtr, u32 HandlerType,
+		      void *CallBackFunc, void *CallBackRef);
+
+/*
+ * Functions in xcanps_sinit.c
+ */
+#ifndef SDT
+XCanPs_Config *XCanPs_LookupConfig(u16 DeviceId);
+#else
+XCanPs_Config *XCanPs_LookupConfig(u32 BaseAddress);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* end of protection macro */
+/** @} */
diff --git a/bsp_z7/ps7_cortexa9_0/include/xcanps_hw.h b/bsp_z7/ps7_cortexa9_0/include/xcanps_hw.h
new file mode 100644
index 0000000..2ecca1c
--- /dev/null
+++ b/bsp_z7/ps7_cortexa9_0/include/xcanps_hw.h
@@ -0,0 +1,347 @@
+/******************************************************************************
+* Copyright (C) 2010 - 2021 Xilinx, Inc.  All rights reserved.
+* Copyright (C) 2023 Advanced Micro Devices, Inc. All Rights Reserved.
+* SPDX-License-Identifier: MIT
+******************************************************************************/
+
+/*****************************************************************************/
+/**
+*
+* @file xcanps_hw.h
+* @addtogroup canps Overview
+* @{
+*
+* This header file contains the identifiers and basic driver functions (or
+* macros) that can be used to access the device. Other driver functions
+* are defined in xcanps.h.
+*
+* <pre>
+* MODIFICATION HISTORY:
+*
+* Ver   Who    Date	Changes
+* ----- -----  -------- -----------------------------------------------
+* 1.00a xd/sv  01/12/10 First release
+* 1.01a sbs    12/27/11 Updated the Register/bit definitions
+*                       Changed XCANPS_RXFWIR_RXFLL_MASK to XCANPS_WIR_FW_MASK
+*                       Changed XCANPS_RXWIR_OFFSET to XCANPS_WIR_OFFSET
+*			Added XCANPS_IXR_TXFEMP_MASK for Tx Fifo Empty
+*			Changed XCANPS_IXR_RXFLL_MASK to
+*			XCANPS_IXR_RXFWMFLL_MASK
+* 			Changed
+*			XCANPS_TXBUF_ID_OFFSET to XCANPS_TXHPB_ID_OFFSET
+* 			XCANPS_TXBUF_DLC_OFFSET to XCANPS_TXHPB_DLC_OFFSET
+*			XCANPS_TXBUF_DW1_OFFSET  to XCANPS_TXHPB_DW1_OFFSET
+*			XCANPS_TXBUF_DW2_OFFSET  to XCANPS_TXHPB_DW2_OFFSET
+* 1.02a adk   08/08/13  Updated for including the function prototype
+* 3.00  kvn   02/13/15  Modified code for MISRA-C:2012 compliance.
+* 3.5	sne   07/01/20  Fixed MISRAC warnings.
+* </pre>
+*
+******************************************************************************/
+
+#ifndef XCANPS_HW_H		/* prevent circular inclusions */
+#define XCANPS_HW_H		/**< by using protection macros */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/***************************** Include Files *********************************/
+
+#include "xil_types.h"
+#include "xil_assert.h"
+#include "xil_io.h"
+
+/************************** Constant Definitions *****************************/
+
+/** @name Register offsets for the CAN. Each register is 32 bits.
+ *  @{
+ */
+#define XCANPS_SRR_OFFSET	  	0x00000000U /**< Software Reset Register */
+#define XCANPS_MSR_OFFSET	  	0x00000004U /**< Mode Select Register */
+#define XCANPS_BRPR_OFFSET	  	0x00000008U /**< Baud Rate Prescaler */
+#define XCANPS_BTR_OFFSET	  	0x0000000CU /**< Bit Timing Register */
+#define XCANPS_ECR_OFFSET	  	0x00000010U /**< Error Counter Register */
+#define XCANPS_ESR_OFFSET	  	0x00000014U /**< Error Status Register */
+#define XCANPS_SR_OFFSET	  	0x00000018U /**< Status Register */
+
+#define XCANPS_ISR_OFFSET	  	0x0000001CU /**< Interrupt Status Register */
+#define XCANPS_IER_OFFSET	  	0x00000020U /**< Interrupt Enable Register */
+#define XCANPS_ICR_OFFSET	  	0x00000024U /**< Interrupt Clear Register */
+#define XCANPS_TCR_OFFSET	  	0x00000028U /**< Timestamp Control Register */
+#define XCANPS_WIR_OFFSET	  	0x0000002CU /**< Watermark Interrupt Reg */
+
+#define XCANPS_TXFIFO_ID_OFFSET  	0x00000030U /**< TX FIFO ID */
+#define XCANPS_TXFIFO_DLC_OFFSET 	0x00000034U /**< TX FIFO DLC */
+#define XCANPS_TXFIFO_DW1_OFFSET	0x00000038U /**< TX FIFO Data Word 1 */
+#define XCANPS_TXFIFO_DW2_OFFSET 	0x0000003CU /**< TX FIFO Data Word 2 */
+
+#define XCANPS_TXHPB_ID_OFFSET   	0x00000040U /**< TX High Priority Buffer ID */
+#define XCANPS_TXHPB_DLC_OFFSET  	0x00000044U /**< TX High Priority Buffer DLC */
+#define XCANPS_TXHPB_DW1_OFFSET  	0x00000048U /**< TX High Priority Buf Data 1 */
+#define XCANPS_TXHPB_DW2_OFFSET  	0x0000004CU /**< TX High Priority Buf Data Word 2 */
+
+#define XCANPS_RXFIFO_ID_OFFSET  	0x00000050U /**< RX FIFO ID */
+#define XCANPS_RXFIFO_DLC_OFFSET 	0x00000054U /**< RX FIFO DLC */
+#define XCANPS_RXFIFO_DW1_OFFSET 	0x00000058U /**< RX FIFO Data Word 1 */
+#define XCANPS_RXFIFO_DW2_OFFSET 	0x0000005CU /**< RX FIFO Data Word 2 */
+
+#define XCANPS_AFR_OFFSET	  	0x00000060U /**< Acceptance Filter Register */
+#define XCANPS_AFMR1_OFFSET	  	0x00000064U /**< Acceptance Filter Mask 1 */
+#define XCANPS_AFIR1_OFFSET	  	0x00000068U /**< Acceptance Filter ID  1 */
+#define XCANPS_AFMR2_OFFSET	  	0x0000006CU /**< Acceptance Filter Mask  2 */
+#define XCANPS_AFIR2_OFFSET	  	0x00000070U /**< Acceptance Filter ID 2 */
+#define XCANPS_AFMR3_OFFSET	  	0x00000074U /**< Acceptance Filter Mask 3 */
+#define XCANPS_AFIR3_OFFSET	  	0x00000078U /**< Acceptance Filter ID 3 */
+#define XCANPS_AFMR4_OFFSET	  	0x0000007CU /**< Acceptance Filter Mask  4 */
+#define XCANPS_AFIR4_OFFSET	  	0x00000080U /**< Acceptance Filter ID 4 */
+/** @} */
+
+/** @name Software Reset Register (SRR) Bit Definitions and Masks
+ *  @{
+ */
+#define XCANPS_SRR_CEN_MASK		0x00000002U  /**< Can Enable */
+#define XCANPS_SRR_SRST_MASK	0x00000001U  /**< Reset */
+/** @} */
+
+/** @name Mode Select Register (MSR) Bit Definitions and Masks
+ *  @{
+ */
+#define XCANPS_MSR_SNOOP_MASK	0x00000004U /**< Snoop Mode Select */
+#define XCANPS_MSR_LBACK_MASK	0x00000002U /**< Loop Back Mode Select */
+#define XCANPS_MSR_SLEEP_MASK	0x00000001U /**< Sleep Mode Select */
+/** @} */
+
+/** @name Baud Rate Prescaler register (BRPR) Bit Definitions and Masks
+ *  @{
+ */
+#define XCANPS_BRPR_BRP_MASK	0x000000FFU /**< Baud Rate Prescaler */
+/** @} */
+
+/** @name Bit Timing Register (BTR) Bit Definitions and Masks
+ *  @{
+ */
+#define XCANPS_BTR_SJW_MASK	0x00000180U /**< Synchronization Jump Width */
+#define XCANPS_BTR_SJW_SHIFT	7U	    /**< Shift Value for SJW */
+#define XCANPS_BTR_TS2_MASK	0x00000070U /**< Time Segment 2 */
+#define XCANPS_BTR_TS2_SHIFT	4U	    /**< Shift Value for TS2 */
+#define XCANPS_BTR_TS1_MASK	0x0000000FU /**< Time Segment 1 */
+/** @} */
+
+/** @name Error Counter Register (ECR) Bit Definitions and Masks
+ *  @{
+ */
+#define XCANPS_ECR_REC_MASK	0x0000FF00U /**< Receive Error Counter */
+#define XCANPS_ECR_REC_SHIFT		 8U /**< Shift Value for REC */
+#define XCANPS_ECR_TEC_MASK	0x000000FFU /**< Transmit Error Counter */
+/** @} */
+
+/** @name Error Status Register (ESR) Bit Definitions and Masks
+ *  @{
+ */
+#define XCANPS_ESR_ACKER_MASK	0x00000010U /**< ACK Error */
+#define XCANPS_ESR_BERR_MASK	0x00000008U /**< Bit Error */
+#define XCANPS_ESR_STER_MASK	0x00000004U /**< Stuff Error */
+#define XCANPS_ESR_FMER_MASK	0x00000002U /**< Form Error */
+#define XCANPS_ESR_CRCER_MASK	0x00000001U /**< CRC Error */
+/** @} */
+
+/** @name Status Register (SR) Bit Definitions and Masks
+ *  @{
+ */
+#define XCANPS_SR_SNOOP_MASK	0x00001000U /**< Snoop Mask */
+#define XCANPS_SR_ACFBSY_MASK	0x00000800U /**< Acceptance Filter busy */
+#define XCANPS_SR_TXFLL_MASK	0x00000400U /**< TX FIFO is full */
+#define XCANPS_SR_TXBFLL_MASK	0x00000200U /**< TX High Priority Buffer full */
+#define XCANPS_SR_ESTAT_MASK	0x00000180U /**< Error Status */
+#define XCANPS_SR_ESTAT_SHIFT	7U	    /**< Shift value for ESTAT */
+#define XCANPS_SR_ERRWRN_MASK	0x00000040U /**< Error Warning */
+#define XCANPS_SR_BBSY_MASK		0x00000020U /**< Bus Busy */
+#define XCANPS_SR_BIDLE_MASK	0x00000010U /**< Bus Idle */
+#define XCANPS_SR_NORMAL_MASK	0x00000008U /**< Normal Mode */
+#define XCANPS_SR_SLEEP_MASK	0x00000004U /**< Sleep Mode */
+#define XCANPS_SR_LBACK_MASK	0x00000002U /**< Loop Back Mode */
+#define XCANPS_SR_CONFIG_MASK	0x00000001U /**< Configuration Mode */
+/** @} */
+
+/** @name Interrupt Status/Enable/Clear Register Bit Definitions and Masks
+ *  @{
+ */
+#define XCANPS_IXR_TXFEMP_MASK   0x00004000U /**< Tx Fifo Empty Interrupt */
+#define XCANPS_IXR_TXFWMEMP_MASK 0x00002000U /**< Tx Fifo Watermark Empty */
+#define XCANPS_IXR_RXFWMFLL_MASK 0x00001000U /**< Rx FIFO Watermark Full */
+#define XCANPS_IXR_WKUP_MASK	 0x00000800U /**< Wake up Interrupt */
+#define XCANPS_IXR_SLP_MASK		0x00000400U /**< Sleep Interrupt */
+#define XCANPS_IXR_BSOFF_MASK	0x00000200U /**< Bus Off Interrupt */
+#define XCANPS_IXR_ERROR_MASK	0x00000100U /**< Error Interrupt */
+#define XCANPS_IXR_RXNEMP_MASK	0x00000080U /**< RX FIFO Not Empty Interrupt */
+#define XCANPS_IXR_RXOFLW_MASK	0x00000040U /**< RX FIFO Overflow Interrupt */
+#define XCANPS_IXR_RXUFLW_MASK	0x00000020U /**< RX FIFO Underflow Interrupt */
+#define XCANPS_IXR_RXOK_MASK	0x00000010U /**< New Message Received Intr */
+#define XCANPS_IXR_TXBFLL_MASK	0x00000008U /**< TX High Priority Buf Full */
+#define XCANPS_IXR_TXFLL_MASK	0x00000004U /**< TX FIFO Full Interrupt */
+#define XCANPS_IXR_TXOK_MASK	0x00000002U /**< TX Successful Interrupt */
+#define XCANPS_IXR_ARBLST_MASK	0x00000001U /**< Arbitration Lost Interrupt */
+#define XCANPS_IXR_ALL		((u32)XCANPS_IXR_RXFWMFLL_MASK | \
+				(u32)XCANPS_IXR_WKUP_MASK   | \
+				(u32)XCANPS_IXR_SLP_MASK	| \
+				(u32)XCANPS_IXR_BSOFF_MASK  | \
+				(u32)XCANPS_IXR_ERROR_MASK  | \
+				(u32)XCANPS_IXR_RXNEMP_MASK | \
+				(u32)XCANPS_IXR_RXOFLW_MASK | \
+				(u32)XCANPS_IXR_RXUFLW_MASK | \
+				(u32)XCANPS_IXR_RXOK_MASK   | \
+				(u32)XCANPS_IXR_TXBFLL_MASK | \
+				(u32)XCANPS_IXR_TXFLL_MASK  | \
+				(u32)XCANPS_IXR_TXOK_MASK   | \
+				(u32)XCANPS_IXR_ARBLST_MASK)
+				/**< Basic interrupts */
+/** @} */
+
+/** @name CAN Timestamp Control Register (TCR) Bit Definitions and Masks
+ *  @{
+ */
+#define XCANPS_TCR_CTS_MASK	0x00000001U /**< Clear Timestamp counter mask */
+/** @} */
+
+/** @name CAN Watermark Register (WIR) Bit Definitions and Masks
+ *  @{
+ */
+#define XCANPS_WIR_FW_MASK   	0x0000003FU /**< Rx Full Threshold mask */
+#define XCANPS_WIR_EW_MASK 	0x00003F00U /**< Tx Empty Threshold mask */
+#define XCANPS_WIR_EW_SHIFT 	0x00000008U /**< Tx Empty Threshold shift */
+
+/** @} */
+
+/** @name CAN Frame Identifier (TX High Priority Buffer/TX/RX/Acceptance Filter
+				Mask/Acceptance Filter ID)
+ *  @{
+ */
+#define XCANPS_IDR_ID1_MASK	0xFFE00000U /**< Standard Messg Identifier */
+#define XCANPS_IDR_ID1_SHIFT	21U	    /**< Shift Value for ID1 */
+#define XCANPS_IDR_SRR_MASK	0x00100000U /**< Substitute Remote TX Req */
+#define XCANPS_IDR_SRR_SHIFT	20U	    /**< Shift Value for SRR */
+#define XCANPS_IDR_IDE_MASK	0x00080000U /**< Identifier Extension */
+#define XCANPS_IDR_IDE_SHIFT	19U	    /**< Shift Value for IDE */
+#define XCANPS_IDR_ID2_MASK	0x0007FFFEU /**< Extended Message Ident */
+#define XCANPS_IDR_ID2_SHIFT	1U	    /**< Shift Value for ID2 */
+#define XCANPS_IDR_RTR_MASK	0x00000001U /**< Remote TX Request */
+/** @} */
+
+/** @name CAN Frame Data Length Code (TX High Priority Buffer/TX/RX)
+ *  @{
+ */
+#define XCANPS_DLCR_DLC_MASK	 0xF0000000U	/**< Data Length Code */
+#define XCANPS_DLCR_DLC_SHIFT	 28U		/**< Shift Value for DLC */
+#define XCANPS_DLCR_TIMESTAMP_MASK 0x0000FFFFU	/**< Timestamp Mask (Rx only) */
+
+/** @} */
+
+/** @name CAN Frame Data Word 1 (TX High Priority Buffer/TX/RX)
+ *  @{
+ */
+#define XCANPS_DW1R_DB0_MASK	0xFF000000U /**< Data Byte 0 */
+#define XCANPS_DW1R_DB0_SHIFT	24U	    /**< Shift Value for Data Byte 0 */
+#define XCANPS_DW1R_DB1_MASK	0x00FF0000U /**< Data Byte 1 */
+#define XCANPS_DW1R_DB1_SHIFT	16U	    /**< Shift Value for Data Byte 1 */
+#define XCANPS_DW1R_DB2_MASK	0x0000FF00U /**< Data Byte 2 */
+#define XCANPS_DW1R_DB2_SHIFT	8U	    /**< Shift Value for Data Byte 2 */
+#define XCANPS_DW1R_DB3_MASK	0x000000FFU /**< Data Byte 3 */
+/** @} */
+
+/** @name CAN Frame Data Word 2 (TX High Priority Buffer/TX/RX)
+ *  @{
+ */
+#define XCANPS_DW2R_DB4_MASK	0xFF000000U /**< Data Byte 4 */
+#define XCANPS_DW2R_DB4_SHIFT	24U	    /**< Shift Value for Data Byte 4 */
+#define XCANPS_DW2R_DB5_MASK	0x00FF0000U /**< Data Byte 5 */
+#define XCANPS_DW2R_DB5_SHIFT	16U	    /**< Shift Value for Data Bbyet 5 */
+#define XCANPS_DW2R_DB6_MASK	0x0000FF00U /**< Data Byte 6 */
+#define XCANPS_DW2R_DB6_SHIFT	8U	    /**< Shift Value for Data Byte 6 */
+#define XCANPS_DW2R_DB7_MASK	0x000000FFU /**< Data Byte 7 */
+/** @} */
+
+/** @name Acceptance Filter Register (AFR) Bit Definitions and Masks
+ *  @{
+ */
+#define XCANPS_AFR_UAF4_MASK	0x00000008U /**< Use Acceptance Filter No.4 */
+#define XCANPS_AFR_UAF3_MASK	0x00000004U /**< Use Acceptance Filter No.3 */
+#define XCANPS_AFR_UAF2_MASK	0x00000002U /**< Use Acceptance Filter No.2 */
+#define XCANPS_AFR_UAF1_MASK	0x00000001U /**< Use Acceptance Filter No.1 */
+#define XCANPS_AFR_UAF_ALL_MASK	((u32)XCANPS_AFR_UAF4_MASK | \
+					(u32)XCANPS_AFR_UAF3_MASK | \
+					(u32)XCANPS_AFR_UAF2_MASK | \
+					(u32)XCANPS_AFR_UAF1_MASK)
+					/**< Mask for Acceptance Filers */
+/** @} */
+
+/** @name CAN frame length constants
+ *  @{
+ */
+#define XCANPS_MAX_FRAME_SIZE sizeof(u32)*16U /**< Maximum CAN frame length in bytes */
+/** @} */
+
+/* For backwards compatibility */
+#define XCANPS_TXBUF_ID_OFFSET   XCANPS_TXHPB_ID_OFFSET	 /**< TX High Priority Buffer ID */
+#define XCANPS_TXBUF_DLC_OFFSET  XCANPS_TXHPB_DLC_OFFSET /**< TX High Priority Buffer DLC */
+#define XCANPS_TXBUF_DW1_OFFSET  XCANPS_TXHPB_DW1_OFFSET /**< TX High Priority Buf Data 1 */
+#define XCANPS_TXBUF_DW2_OFFSET  XCANPS_TXHPB_DW2_OFFSET /**< TX High Priority Buf Data Word 2 */
+
+#define XCANPS_RXFWIR_RXFLL_MASK XCANPS_WIR_FW_MASK  /**< Rx Full Threshold mask */
+#define XCANPS_RXWIR_OFFSET 	 XCANPS_WIR_OFFSET  /**< Watermark Interrupt Reg */
+#define XCANPS_IXR_RXFLL_MASK 	 XCANPS_IXR_RXFWMFLL_MASK /**< Rx FIFO Watermark Full */
+
+
+
+
+/**************************** Type Definitions *******************************/
+
+/***************** Macros (Inline Functions) Definitions *********************/
+
+/****************************************************************************/
+/**
+*
+* This macro reads the given register.
+*
+* @param	BaseAddr is the base address of the device.
+* @param	RegOffset is the register offset to be read.
+*
+* @return	The 32-bit value of the register
+*
+* @note		None.
+*
+*****************************************************************************/
+#define XCanPs_ReadReg(BaseAddr, RegOffset) \
+		Xil_In32((BaseAddr) + (u32)(RegOffset))
+
+
+/****************************************************************************/
+/**
+*
+* This macro writes the given register.
+*
+* @param	BaseAddr is the base address of the device.
+* @param	RegOffset is the register offset to be written.
+* @param	Data is the 32-bit value to write to the register.
+*
+* @return	None.
+*
+* @note		None.
+*
+*****************************************************************************/
+#define XCanPs_WriteReg(BaseAddr, RegOffset, Data) \
+		Xil_Out32((BaseAddr) + (u32)(RegOffset), (u32)(Data))
+
+/************************** Function Prototypes ******************************/
+/*
+ * Perform reset operation to the CanPs interface
+ */
+void XCanPs_ResetHw(UINTPTR BaseAddr);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* end of protection macro */
+/** @} */
diff --git a/mission/can_rust_helper.c b/mission/can_rust_helper.c
index cada7e4..078d7f8 100644
--- a/mission/can_rust_helper.c
+++ b/mission/can_rust_helper.c
@@ -110,7 +110,7 @@ int canps_poll_example(void)
 	 */
 
 	Status = CanPsPolledExample(&Can, XPAR_XCANPS_0_BASEADDR);
-
+	xil_printf("114 \r\n");
 	if (Status != XST_SUCCESS) {
 		xil_printf("CAN Polled Mode Test Failed\r\n");
 		return XST_FAILURE;
@@ -149,16 +149,24 @@ int CanPsPolledExample(XCanPs *CanInstancePtr, UINTPTR BaseAddress)
 	/*
 	 * Initialize the Can device.
 	 */
-     
+	xil_printf("Initialize the Can device.\r\n");
+	xil_printf("BaseAdress 0x%p\r\n", BaseAddress);
+
 	ConfigPtr = XCanPs_LookupConfig(BaseAddress);
 
 	if (CanInstPtr == NULL) {
+
+		xil_printf("NULL\r\n");
+     
 		return XST_FAILURE;
 	}
+
+	xil_printf("Status = XCanPs_CfgInitialize()\r\n");
 	Status = XCanPs_CfgInitialize(CanInstPtr,
 				      ConfigPtr,
 				      ConfigPtr->BaseAddr);
 	if (Status != XST_SUCCESS) {
+		xil_printf("Status != XST_SUCCESS\r\n");
 		return XST_FAILURE;
 	}
 
@@ -176,12 +184,15 @@ int CanPsPolledExample(XCanPs *CanInstancePtr, UINTPTR BaseAddress)
 	 * Register (BRPR) and Bit Timing Register (BTR).
 	 */
 	XCanPs_EnterMode(CanInstPtr, XCANPS_MODE_CONFIG);
+	xil_printf("ENTER MODE\r\n");
 	while (XCanPs_GetMode(CanInstPtr) != XCANPS_MODE_CONFIG);
-
 	/*
 	 * Setup Baud Rate Prescaler Register (BRPR) and
 	 * Bit Timing Register (BTR).
 	 */
+	
+	xil_printf("Setup Baud Rate Prescaler Register (BRPR) and Bit Timing Register (BTR).\r\n");
+
 	XCanPs_SetBaudRatePrescaler(CanInstPtr, TEST_BRPR_BAUD_PRESCALAR);
 	XCanPs_SetBitTiming(CanInstPtr, TEST_BTR_SYNCJUMPWIDTH,
 			    TEST_BTR_SECOND_TIMESEGMENT,
@@ -191,6 +202,7 @@ int CanPsPolledExample(XCanPs *CanInstancePtr, UINTPTR BaseAddress)
 	/*
 	 * Enter Loop Back Mode.
 	 */
+	xil_printf("Enter Loop Back Mode\r\n");
 	XCanPs_EnterMode(CanInstPtr, XCANPS_MODE_LOOPBACK);
 	while (XCanPs_GetMode(CanInstPtr) != XCANPS_MODE_LOOPBACK);
 
@@ -198,6 +210,8 @@ int CanPsPolledExample(XCanPs *CanInstancePtr, UINTPTR BaseAddress)
 	 * Send a frame, receive the frame via the loop back and verify its
 	 * contents.
 	 */
+
+	xil_printf("Send a frame, receive the frame via the loop back and verify its contents.\r\n");
 	Status = SendFrame(CanInstPtr);
 	if (Status != XST_SUCCESS) {
 		return Status;
diff --git a/mission_rust/src/fsrc/osal/mod.rs b/mission_rust/src/fsrc/osal/mod.rs
index 4f4d346..a890e9e 100644
--- a/mission_rust/src/fsrc/osal/mod.rs
+++ b/mission_rust/src/fsrc/osal/mod.rs
@@ -52,16 +52,15 @@ extern "C" {
     // TODO: Is u32 compatible to uint32_t?
     
 
-
     /// Reads the value of a gpio pin
+    ///
     pub fn gpio_read_pin(pin: u32) -> u32;
     pub fn gpio_toggle_pin(pin: u32) -> ();
 
-
     // CAN Samples
 
 
-    fn canps_poll_example() -> u32;
+    pub fn canps_poll_example() -> i32;
 
 }
 
diff --git a/mission_rust/src/lib.rs b/mission_rust/src/lib.rs
index 514ad7a..a936755 100644
--- a/mission_rust/src/lib.rs
+++ b/mission_rust/src/lib.rs
@@ -173,6 +173,23 @@ impl SystemObjectIF for HandlerSender {
 fn mission() {
     sifln!("Mission enter");
 
+
+    use crate::osal::*;
+    unsafe {
+    sifln!("Setup GPIO");
+    gpio_setup();
+    sifln!(";");
+    sifln!("✅");
+    gpio_toggle_pin(0x00000070);
+    sifln!("✅");
+    
+
+    sifln!("Call canps poll example()");
+
+    canps_poll_example();
+    }
+    sifln!("Called canps poll example()");
+
     let mut h1 = Handler {
         id: 1,
         command_queue: queues::MessageQueue::new(),