diff --git a/.vscode/launch.json b/.vscode/launch.json
index d2c158491..338474895 100644
--- a/.vscode/launch.json
+++ b/.vscode/launch.json
@@ -263,7 +263,7 @@
 				"target/stm32g4x.cfg"
 			],
 			"searchDir": [],
-			"preLaunchTask": "CMake: configure and build G4ADCTESTING",
+			"preLaunchTask": "CMake: configure and build G4CANTESTING",
 			"showDevDebugOutput": "raw",
 			"svdPath": "${workspaceFolder}/Lib/Vendor/CMSIS_5/SVD/STM32G474.svd",
 			"swoConfig": {
diff --git a/ECU/Core/Src/main.c b/ECU/Core/Src/main.c
index 3ea450de8..77db94d2a 100644
--- a/ECU/Core/Src/main.c
+++ b/ECU/Core/Src/main.c
@@ -271,17 +271,17 @@ void CAN_Configure()
 	canCfg.hal_fdcan_init.NominalSyncJumpWidth = 16;
 	canCfg.hal_fdcan_init.NominalTimeSeg1 = 127; // Updated for 170MHz: (1+127+42)*1 = 170 ticks -> 1 Mbps
 	canCfg.hal_fdcan_init.NominalTimeSeg2 = 42;
-	canCfg.hal_fdcan_init.DataPrescaler = 8;
+	canCfg.hal_fdcan_init.DataPrescaler = 2;
 	canCfg.hal_fdcan_init.DataSyncJumpWidth = 16;
-	canCfg.hal_fdcan_init.DataTimeSeg1 = 15; // Updated for 170MHz: (1+15+5)*8 = 168 ticks -> ~5 Mbps
-	canCfg.hal_fdcan_init.DataTimeSeg2 = 5;
+	canCfg.hal_fdcan_init.DataTimeSeg1 = 12; // Updated for 170MHz: 170 MHz/((1+12+4)*2) = 5 Mbps
+	canCfg.hal_fdcan_init.DataTimeSeg2 = 4;
 	canCfg.hal_fdcan_init.StdFiltersNbr = 1;
 	canCfg.hal_fdcan_init.ExtFiltersNbr = 0;
 
 	canCfg.rx_callback = NULL;
 	canCfg.rx_interrupt_priority = 15; // TODO: Maybe make these not hardcoded
 	canCfg.tx_interrupt_priority = 15;
-	canCfg.tx_buffer_length = CAN_TX_BUFFER_LENGTH;
+	// canCfg.tx_buffer_length = CAN_TX_BUFFER_LENGTH;
 
 	// RX shared settings
 	canCfg.init_rx_gpio.Mode = GPIO_MODE_AF_PP;
diff --git a/Lib/Peripherals/CAN/Inc/can.h b/Lib/Peripherals/CAN/Inc/can.h
index c20cbedae..dbdae9863 100644
--- a/Lib/Peripherals/CAN/Inc/can.h
+++ b/Lib/Peripherals/CAN/Inc/can.h
@@ -24,7 +24,7 @@ typedef struct {
 	uint32_t tx_interrupt_priority;
 
 	// Circular Buffer
-	uint32_t tx_buffer_length;
+	// uint32_t tx_buffer_capacity;
 
 	GPIO_TypeDef *rx_gpio;	       // Instance name, like GPIOA, GPIOB, etc.
 	GPIO_InitTypeDef init_rx_gpio; // GPIO Parameters - set correct Alternate Function, no pullup/pulldown, high/very_high frequency
@@ -34,17 +34,37 @@ typedef struct {
 	// additional parameters
 } CANConfig;
 
+#define FDCAN_MAX_DATA_BYTES 64
+typedef struct {
+	FDCAN_TxHeaderTypeDef tx_header;
+	uint8_t data[FDCAN_MAX_DATA_BYTES];
+} FDCANTxMessage;
+typedef struct {
+	FDCAN_RxHeaderTypeDef rx_header;
+	uint8_t data[FDCAN_MAX_DATA_BYTES];
+} FDCANRxMessage;
+
 // FDCAN peripheral for STM32G4
 typedef struct {
-	FDCAN_HandleTypeDef *hal_fdcanP;
-	CircularBuffer *tx_buffer;
-	uint32_t tx_buffer_length;
+	FDCAN_HandleTypeDef *hal_fdcanP; // DO NOT REORDER THIS
+
+	// TX buffer
+	volatile FDCANTxMessage *tx_buffer;
+	volatile uint32_t tx_capacity;
+	volatile uint32_t tx_tail;
+	volatile uint32_t tx_elements;
 
+	// RX Callback
 	CAN_RXCallback rx_callback;
 
+	uint32_t rx_interrupt_priority;
+	uint32_t tx_interrupt_priority;
+
 	// for release
 	GPIO_TypeDef *rx_gpio;
+	uint32_t rx_pin;
 	GPIO_TypeDef *tx_gpio;
+	uint32_t tx_pin;
 	uint32_t Clock_Source;
 
 	// state
@@ -54,16 +74,6 @@ typedef struct {
 	// error states
 } CANHandle;
 
-#define FDCAN_MAX_DATA_BYTES 64
-typedef struct {
-	FDCAN_TxHeaderTypeDef tx_header;
-	uint8_t data[FDCAN_MAX_DATA_BYTES];
-} FDCANTxMessage;
-typedef struct {
-	FDCAN_RxHeaderTypeDef rx_header;
-	uint8_t data[FDCAN_MAX_DATA_BYTES];
-} FDCANRxMessage;
-
 CANHandle *can_init(const CANConfig *config); // user must supply an rx callback function
 int can_start(CANHandle *handle);
 int can_stop(CANHandle *handle);
diff --git a/Lib/Peripherals/CAN/README.md b/Lib/Peripherals/CAN/README.md
index 0bbd70084..879bc771e 100644
--- a/Lib/Peripherals/CAN/README.md
+++ b/Lib/Peripherals/CAN/README.md
@@ -35,6 +35,8 @@ Verify ISR safety, no race conditions, atomic read/writes
 -Shouldn't disable GPIOs in the MSP layers when releasing, might affect other peripherals
 
 IDEAS for other features:
+-
+- DMA support for copying 64 bytes from circular buffer
 - abstract to different STM families besides STM32G4
 - Rx Buffering
 - TX Buffering policy, do we spread them out over multiple TX buffers
@@ -43,7 +45,6 @@ IDEAS for other features:
 - TX FIFO vs Queue policy (only allow FIFOS)
 - Add support for RXFifo1
 
-
 TESTING- ----------------------------------------------
 USE LOGOMATIC for return status -
 either returns through semihosting or debug cores
@@ -55,7 +56,6 @@ Testing framework
 - All tests are run from the top level function in can_test.c
 
 - can_test.c should initialize everything properly.
-- May have to create platform specific asserts when testing state
 - use LOGOMATIC to return errors or throw asserts
 
 - Platform testing, such as in G4PERTESTING just needs include "can_test.h" and call top level function
@@ -70,6 +70,7 @@ Library Centric Testing:
 - Test the implementation in each library.
 
 HAL_Rewrite:
-- Alternatively, rewrite without using HAL, just use CMSIS definitions. 
-- PROS: Would look good on your Github. 
+- Alternatively, rewrite without using HAL, just use CMSIS definitions.
+- There isn't actually that much going on underneath HAL, it just looks painful because of CMSIS
+- PROS: Would look good on your Github.
 - CONS: takes too long
diff --git a/Lib/Peripherals/CAN/Src/can.c b/Lib/Peripherals/CAN/Src/can.c
index e989f071b..4de7d6c87 100644
--- a/Lib/Peripherals/CAN/Src/can.c
+++ b/Lib/Peripherals/CAN/Src/can.c
@@ -9,20 +9,30 @@
 
 // HAL handles
 // #ifdef USECAN1
+#define TX_BUFFER_1_SIZE 3
 static FDCAN_HandleTypeDef hal_fdcan1 = {.Instance = FDCAN1};
-static CANHandle CAN1 = {.hal_fdcanP = &hal_fdcan1};
+FDCANTxMessage tx_buffer_1[TX_BUFFER_1_SIZE] = {0};
+static CANHandle CAN1 = {.hal_fdcanP = &hal_fdcan1, .tx_buffer = tx_buffer_1};
 // #endif
 
 // #ifdef USECAN2
+#define TX_BUFFER_2_SIZE 3
 static FDCAN_HandleTypeDef hal_fdcan2 = {.Instance = FDCAN2};
-static CANHandle CAN2 = {.hal_fdcanP = &hal_fdcan2};
+FDCANTxMessage tx_buffer_2[TX_BUFFER_2_SIZE] = {0};
+static CANHandle CAN2 = {.hal_fdcanP = &hal_fdcan2, .tx_buffer = tx_buffer_2};
 // #endif
 
 // #ifdef USECAN3
+#define TX_BUFFER_3_SIZE 3
 static FDCAN_HandleTypeDef hal_fdcan3 = {.Instance = FDCAN3};
-static CANHandle CAN3 = {.hal_fdcanP = &hal_fdcan3};
+FDCANTxMessage tx_buffer_3[TX_BUFFER_3_SIZE] = {0};
+static CANHandle CAN3 = {.hal_fdcanP = &hal_fdcan3, .tx_buffer = tx_buffer_3};
 // #endif
 
+#define MIN(A, B) ((A < B) ? A : B)
+
+bool hardwareEnabled = false;
+
 // macro lore
 /*
 #define CAT(a,b) a##b
@@ -31,20 +41,20 @@ static CANHandle CAN3 = {.hal_fdcanP = &hal_fdcan3};
 #define CAT5(a, b,c,d,e) a##b##c##d##e
 
 #define ACTIVATE_FDCAN_HELPER(FDCANX, ITY, preirq, subirq)  \
-	do { \
-	    HAL_NVIC_SetPriority( CAT4(FDCANX##,_,ITY, _IRQn ) , preirq, subirq );  \
-	    HAL_NVIC_EnableIRQ( CAT4(FDCANX##,_,ITY, _IRQn ) ); \
-	} while(0)
+    do { \
+	HAL_NVIC_SetPriority( CAT4(FDCANX##,_,ITY, _IRQn ) , preirq, subirq );  \
+	HAL_NVIC_EnableIRQ( CAT4(FDCANX##,_,ITY, _IRQn ) ); \
+    } while(0)
 
 #define HAL_NVIC_ACTIVATE_FDCAN(FDCANX, ITY, preirq, subirq) \
     do { \
-	if (FDCANX == ##FDCAN1 && ITY == 0)      { ACTIVATE_FDCAN_HELPER(FDCAN1, IT0, preirq, subirq); } \
-	else if (FDCANX == FDCAN1 && ITY == 1) { ACTIVATE_FDCAN_HELPER(FDCAN1, IT1, preirq, subirq); } \
-	else if (FDCANX == FDCAN2 && ITY == 0) { ACTIVATE_FDCAN_HELPER(FDCAN2, IT0, preirq, subirq); } \
-	else if (FDCANX == FDCAN2 && ITY == 1) { ACTIVATE_FDCAN_HELPER(FDCAN2, IT1, preirq, subirq); } \
-	else if (FDCANX == FDCAN3 && ITY == 0) { ACTIVATE_FDCAN_HELPER(FDCAN3, IT0, preirq, subirq); } \
-	else if (FDCANX == FDCAN3 && ITY == 1) { ACTIVATE_FDCAN_HELPER(FDCAN3, IT1, preirq, subirq); } \
-	else { LOGOMATIC("Unrecognized FDCAN and Interrupt Line combination"); } \
+    if (FDCANX == ##FDCAN1 && ITY == 0)      { ACTIVATE_FDCAN_HELPER(FDCAN1, IT0, preirq, subirq); } \
+    else if (FDCANX == FDCAN1 && ITY == 1) { ACTIVATE_FDCAN_HELPER(FDCAN1, IT1, preirq, subirq); } \
+    else if (FDCANX == FDCAN2 && ITY == 0) { ACTIVATE_FDCAN_HELPER(FDCAN2, IT0, preirq, subirq); } \
+    else if (FDCANX == FDCAN2 && ITY == 1) { ACTIVATE_FDCAN_HELPER(FDCAN2, IT1, preirq, subirq); } \
+    else if (FDCANX == FDCAN3 && ITY == 0) { ACTIVATE_FDCAN_HELPER(FDCAN3, IT0, preirq, subirq); } \
+    else if (FDCANX == FDCAN3 && ITY == 1) { ACTIVATE_FDCAN_HELPER(FDCAN3, IT1, preirq, subirq); } \
+    else { LOGOMATIC("Unrecognized FDCAN and Interrupt Line combination"); } \
     } while(0)
 
 */
@@ -73,59 +83,26 @@ static CANHandle CAN3 = {.hal_fdcanP = &hal_fdcan3};
 			LOGOMATIC("BAD FDCAN GPIO Port");                                                                                                                                              \
 	} while (0)
 
+// TODO: Modify helpers to work across families
+// helpers =================
 static int fdcan_shared_clock_ref = 0;
-static inline void fdcan_enable_shared_clock(void)
-{
-	uint32_t primask = __get_PRIMASK();
-	__disable_irq();
-
-	if (fdcan_shared_clock_ref == 0) {
-		__HAL_RCC_FDCAN_CLK_ENABLE();
-	}
-	fdcan_shared_clock_ref++;
-
-	__set_PRIMASK(primask);
-}
-
-static inline void fdcan_disable_shared_clock(void)
-{
-	uint32_t primask = __get_PRIMASK();
-	__disable_irq();
-
-	if (fdcan_shared_clock_ref > 0) {
-		fdcan_shared_clock_ref--;
-		if (fdcan_shared_clock_ref == 0) {
-			__HAL_RCC_FDCAN_CLK_DISABLE();
-		}
-	}
-	__set_PRIMASK(primask);
-}
-
-static inline void gpio_clk_enable(GPIO_TypeDef *gpio)
-{
-	if (gpio == GPIOA) {
-		__HAL_RCC_GPIOA_CLK_ENABLE();
-	} else if (gpio == GPIOB) {
-		__HAL_RCC_GPIOB_CLK_ENABLE();
-	} else if (gpio == GPIOD) {
-		__HAL_RCC_GPIOD_CLK_ENABLE();
-	}
-}
-
-static inline void gpio_clk_disable(GPIO_TypeDef *gpio)
+static inline void fdcan_enable_shared_clock(void);
+static inline void fdcan_disable_shared_clock(void);
+static CANHandle *can_get_handle(FDCAN_HandleTypeDef *hfdcan);
+static int can_get_irqs(FDCAN_GlobalTypeDef *instance, IRQn_Type *it0, IRQn_Type *it1);
+inline void can_set_clksource(uint32_t clksource)
 {
-	if (gpio == GPIOA) {
-		__HAL_RCC_GPIOA_CLK_DISABLE();
-	} else if (gpio == GPIOB) {
-		__HAL_RCC_GPIOB_CLK_DISABLE();
-	} else if (gpio == GPIOD) {
-		__HAL_RCC_GPIOD_CLK_DISABLE();
-	}
+	LL_RCC_SetFDCANClockSource(clksource);
 }
+// static const char *can_get_instance_name(FDCAN_GlobalTypeDef *instance)
+// static inline void gpio_clk_enable(GPIO_TypeDef *gpio)
+// static inline void gpio_clk_disable(GPIO_TypeDef *gpio)
 
-static int can_get_irqs(FDCAN_GlobalTypeDef *instance, IRQn_Type *it0, IRQn_Type *it1);
+static int can_msp_init(CANHandle *canHandle, CANConfig *config);
+static int can_msp_deinit(CANHandle *canHandle);
+static void FDCAN_InstanceDeInit(FDCAN_HandleTypeDef *hfdcan);
 
-static int can_msp_init(CANHandle *handle, CANConfig *config);
+//================================================= API ========================================
 CANHandle *can_init(const CANConfig *config)
 {
 	// config validation?
@@ -140,6 +117,7 @@ CANHandle *can_init(const CANConfig *config)
 			return 0;
 		} else {
 			canHandle = &CAN1;
+			canHandle->tx_capacity = TX_BUFFER_1_SIZE;
 		}
 	}
 	// #endif
@@ -150,6 +128,7 @@ CANHandle *can_init(const CANConfig *config)
 			return 0;
 		} else {
 			canHandle = &CAN2;
+			canHandle->tx_capacity = TX_BUFFER_2_SIZE;
 		}
 	}
 	// #ifdef USECAN3
@@ -159,6 +138,7 @@ CANHandle *can_init(const CANConfig *config)
 			return 0;
 		} else {
 			canHandle = &CAN3;
+			canHandle->tx_capacity = TX_BUFFER_3_SIZE;
 		}
 	}
 	// #endif
@@ -181,23 +161,32 @@ CANHandle *can_init(const CANConfig *config)
 
 	canHandle->rx_gpio = config->rx_gpio;
 	canHandle->tx_gpio = config->tx_gpio;
+	canHandle->rx_pin = config->init_rx_gpio.Pin;
+	canHandle->tx_pin = config->init_tx_gpio.Pin;
+	canHandle->rx_interrupt_priority = config->rx_interrupt_priority;
+	canHandle->tx_interrupt_priority = config->tx_interrupt_priority;
+
 	canHandle->rx_callback = config->rx_callback;
-	canHandle->tx_buffer_length = config->tx_buffer_length;
+
+	// tx buffer
+	// canHandle->tx_capacity = TX_BUFFER_SIZE_1; //dependent on can instance
+	canHandle->tx_tail = 0;
+	canHandle->tx_elements = 0;
 
 	// alternately -> have can_msp_init setup state for HAL_FDCAN_MspInit to work correctly
 	// have can_msp_deinit setup state for HAL_FDCAN_MspDeInit to work correctly
 	// Then call HAL_FDCAN_Init() and HAL_FDCAN_DeInit()
 
 	// Current idea, redefine HAL_FDCAN_MspInit and MspDeInit do nothing at all, do all the work in can_msp_init()
-	if (can_msp_init(canHandle, (CANConfig *)config)) {
+	uint32_t failure = 0;
+	if (failure |= (can_msp_init(canHandle, (CANConfig *)config))) {
 		LOGOMATIC("CAN_init: could not initialize MSP resources");
-		can_release(canHandle);
 	}
 
 	// PROBLEM: HAL_FDCAN_Init expects HAL_FDCAN_MspInit() to be defined
 	if (HAL_FDCAN_Init(canHandle->hal_fdcanP) != HAL_OK) {
+		failure |= HAL_ERROR;
 		LOGOMATIC("CAN: HAL Could not initialize FDCAN peripheral");
-		return NULL;
 		// Error_Handler();
 	}
 
@@ -217,156 +206,179 @@ CANHandle *can_init(const CANConfig *config)
 
 	if (status & HAL_ERROR) {
 		LOGOMATIC("CAN: Could not activate rx and tx interrupts\n");
-		return NULL;
+		failure |= status;
 	}
 
 	// Circular Buffer
-	canHandle->tx_buffer = GR_CircularBuffer_Create(config->tx_buffer_length);
+	// canHandle->tx_buffer = GR_CircularBuffer_Create(config->tx_buffer_length);
+	// canHandle->tx_buffer = malloc(sizeof(FDCANTxMessage)*canHandle->tx_buffer_length);
 	if (!canHandle->tx_buffer) {
-		LOGOMATIC("CAN: Could not allocate circular buffer\n");
+		LOGOMATIC("tx_buffer isn't valid?");
+		failure |= 1;
+	}
+
+	if (failure) {
+		can_msp_deinit(canHandle);
+		FDCAN_InstanceDeInit(canHandle->hal_fdcanP);
+		memset(canHandle + sizeof(FDCAN_HandleTypeDef), 0, sizeof(*canHandle) - sizeof(FDCAN_HandleTypeDef)); // FIXME: Make sure instance is not being overwritten (FDCANx)
 		return 0;
 	}
 
+	/*if (!canHandle->tx_buffer) {
+	    LOGOMATIC("CAN: Could not allocate circular buffer\n");
+	    return 0;
+	}*/
+
 	canHandle->init = true;
 	canHandle->started = false;
 
 	return canHandle;
 }
 
-inline void can_set_clksource(uint32_t clksource)
+int can_release(CANHandle *canHandle)
 {
-	LL_RCC_SetFDCANClockSource(clksource);
-}
+	if (!canHandle) {
+		LOGOMATIC("CAN: Tried to release a null handle");
+		return -1;
+	}
 
-// only valid for #STM32G474x, must redefine for each family
-static int can_msp_init(CANHandle *canHandle, CANConfig *config)
-{
-	// MSP Init ------- This could be inside HAL_FDCAN_MspInit() instead
-	// FDCAN Clock Select
+	if (!canHandle->init) {
+		LOGOMATIC("CAN_release: can instance is already deinitialized");
+		return -1;
+	}
+	can_stop(canHandle); // try to prevent more interrupts from firing
 
-	fdcan_enable_shared_clock();
-	// Clock speed for FDCAN determined by APB1 clock speed and FDCAN prescaler
+	// No more interrupts should be firing that modify canHandle
 
-	// GPIOs init
-	GPIOx_CLK_ENABLE(config->rx_gpio);
-	GPIOx_CLK_ENABLE(config->tx_gpio);
+	can_msp_deinit(canHandle);
 
-	HAL_GPIO_Init(config->rx_gpio, &(config->init_rx_gpio));
-	HAL_GPIO_Init(config->tx_gpio, &(config->init_tx_gpio));
+	// reset FDCANx instance and message RAM and filters, clear interrupts
+	// HAL_FDCAN_DeInit(canHandle->hal_fdcanP); resets a little too hard
+	FDCAN_InstanceDeInit(canHandle->hal_fdcanP);
 
-	IRQn_Type rxit = -1;
-	IRQn_Type txit = -1;
-	can_get_irqs(canHandle->hal_fdcanP->Instance, &rxit, &txit);
+	// TODO: Not sure these actually do anything
+	//__DSB(); // Data Synchronization Barrier
+	//__ISB(); // Instruction Synchronization Barrier
 
-	// rxfifo0
-	HAL_NVIC_SetPriority(rxit, config->rx_interrupt_priority, 0);
-	HAL_NVIC_EnableIRQ(rxit);
+	// free circular buffer contents
+	// GR_CircularBuffer_Free(&(canHandle->tx_buffer));
+	memset((void *)canHandle->tx_buffer, 0, canHandle->tx_capacity * sizeof(FDCANTxMessage));
+	canHandle->tx_elements = 0;
+	canHandle->tx_tail = 0;
 
-	// tx
-	HAL_NVIC_SetPriority(txit, config->tx_interrupt_priority, 0);
-	HAL_NVIC_EnableIRQ(txit);
-	// End MSP Init --------------
+	// reset can handle
+	memset((void *)canHandle + sizeof(FDCAN_HandleTypeDef), 0, sizeof(*canHandle) - sizeof(FDCAN_HandleTypeDef));
 
 	return 0;
 }
-
-// valid only for STM32G4
-static int can_get_irqs(FDCAN_GlobalTypeDef *instance, IRQn_Type *it0, IRQn_Type *it1)
+// TODO: prevent races conditions on the circular buffer
+// TODO: Implement timer
+// lock access to Circular Buffer when sending and dequeuing
+static void can_tx_dequeue_helper(CANHandle *handle)
 {
-	if (instance == FDCAN1) {
-		*it0 = FDCAN1_IT0_IRQn;
-		*it1 = FDCAN1_IT1_IRQn;
-		return 0;
-	}
-	if (instance == FDCAN2) {
-		*it0 = FDCAN2_IT0_IRQn;
-		*it1 = FDCAN2_IT1_IRQn;
-		return 0;
-	}
-	if (instance == FDCAN3) {
-		*it0 = FDCAN3_IT0_IRQn;
-		*it1 = FDCAN3_IT1_IRQn;
-		return 0;
+	if (!handle || !handle->tx_buffer) {
+		LOGOMATIC("can_tx_buffer_helper: handle is invalid");
+		return;
 	}
 
-	return -1; // invalid instance
-}
+	// TODO: use interrupt masking in case any other ISRs need to lock the circular buffer
+	uint32_t basepri = __get_BASEPRI();
 
-// valid only for STM32G4
-// static const char *can_get_instance_name(FDCAN_GlobalTypeDef *instance)
-// {
-// 	if (instance == FDCAN1) {
-// 		return "FDCAN1";
-// 	} else if (instance == FDCAN2) {
-// 		return "FDCAN2";
-// 	} else if (instance == FDCAN3) {
-// 		return "FDCAN3";
-// 	}
-//  return "UNKNOWN";
-// }
-
-// valid only for STM32G4
-static CANHandle *can_get_buffer_handle(FDCAN_HandleTypeDef *hfdcan)
-{
-	// #ifdef STM32G474xx
-	if (hfdcan->Instance == FDCAN1) {
-		return &CAN1;
-	} else if (hfdcan->Instance == FDCAN2) {
-		return &CAN2;
-	} else if (hfdcan->Instance == FDCAN3) {
-		return &CAN3;
-	} else {
-		LOGOMATIC("CAN_get_buffer_handle: was given invalid FDCAN instance\n");
-		return 0;
+	__set_BASEPRI(handle->tx_interrupt_priority);
+	// single consumer shouldn't affect state of circular buffer too closely
+	if (handle->tx_elements == 0) {
+		__set_BASEPRI(basepri);
+		return;
 	}
-}
 
-static void can_tx_buffer_helper(CANHandle *handle)
-{
-	while (HAL_FDCAN_GetTxFifoFreeLevel(handle->hal_fdcanP) && !GR_CircularBuffer_IsEmpty(handle->tx_buffer)) {
-		FDCANTxMessage *msg = GR_CircularBuffer_Pop(handle->tx_buffer);
+	// uint32_t basepri = __get_basepri();
+	//__disable_irq();
+	// TODO: No need to lock circular buffer, as this ISR cannot interrupt the thread mode (can_send)
 
-		if (!msg) {
-			break;
-		}
+	// Can Add to Fifo Q
+	if (HAL_FDCAN_GetTxFifoFreeLevel(handle->hal_fdcanP)) {
+		// lock the Circular Buffer
+		FDCANTxMessage *msg = &handle->tx_buffer[handle->tx_tail];
 
+		// should call Tx Buffer Callback once complete
 		HAL_StatusTypeDef status = HAL_FDCAN_AddMessageToTxFifoQ(handle->hal_fdcanP, &msg->tx_header, msg->data);
 
 		if (status != HAL_OK) {
-			LOGOMATIC("CAN_tx_helper: failed to add message to FIFO\n");
-			free(msg); // Free the message we couldn't send
-			break;	   // Stop trying to send more
+			// LOGOMATIC("CAN_tx_helper: failed to add message to FIFO\n"); //FIXME: Logomatic may not work with interrupts disabled
+			__set_BASEPRI(basepri);
+			return; // Stop trying to send more
 		}
+		// free(msg); // Successfully sent, free the entry in the circular buffer (which is pointed to by tail)
+		handle->tx_tail = ++handle->tx_tail % handle->tx_capacity;
+		handle->tx_elements--;
 
-		free(msg); // Successfully sent, free the memory
-	}
-}
+	} else { // FIXME: call can_tx_dequeue_helper later with a timer, if this gets implemented, need to mask timer interrupts to allow atomic access
 
-void FDCAN1_IT0_IRQHandler(void)
-{
-	HAL_FDCAN_IRQHandler(&hal_fdcan1);
-}
-void FDCAN1_IT1_IRQHandler(void)
-{
-	HAL_FDCAN_IRQHandler(&hal_fdcan1);
-}
+	} // alternatively, if fifo is full, tx_dequeue should get called anyways, and we don't need the else statement
 
-void FDCAN2_IT0_IRQHandler(void)
-{
-	HAL_FDCAN_IRQHandler(&hal_fdcan2);
-}
-void FDCAN2_IT1_IRQHandler(void)
-{
-	HAL_FDCAN_IRQHandler(&hal_fdcan2);
+	__set_BASEPRI(basepri);
 }
 
-void FDCAN3_IT0_IRQHandler(void)
-{
-	HAL_FDCAN_IRQHandler(&hal_fdcan3);
-}
-void FDCAN3_IT1_IRQHandler(void)
+int can_send(CANHandle *canHandle, FDCANTxMessage *message)
 {
-	HAL_FDCAN_IRQHandler(&hal_fdcan3);
+	if (!canHandle || !message) {
+		LOGOMATIC("CAN_send: received null pointer\n");
+		return -1;
+	}
+
+	if (!canHandle->init || !canHandle->started) {
+		LOGOMATIC("CAN_send: CAN not initialized or started\n");
+		return -1;
+	}
+
+	// IF TX Fifos are not full, send directly to them
+	// If TX Fifos are full, append to circular buffer
+	// If circular buffer is full, return an error code
+
+	// stop can_tx_dequeue_helper from from interleaving
+	uint32_t basepri = __get_BASEPRI();
+	__set_BASEPRI(canHandle->tx_interrupt_priority);
+
+	// FIXME: get rid of this jank
+	// if (hardwareEnabled) {
+	if (HAL_FDCAN_GetTxFifoFreeLevel(canHandle->hal_fdcanP) > 0) {
+		HAL_StatusTypeDef status = HAL_FDCAN_AddMessageToTxFifoQ(canHandle->hal_fdcanP, &(message->tx_header), message->data);
+
+		uint32_t val = 0;
+		if (status != HAL_OK) {
+			LOGOMATIC("CAN_send: failed to add to HW FIFO\n");
+			val = -1;
+		} else {
+			val = 0;
+		}
+		__set_BASEPRI(basepri);
+		return val;
+	}
+	//}
+
+	// Hardware FIFO full, try software buffer
+	if (canHandle->tx_elements < canHandle->tx_capacity) {
+		// int result = GR_CircularBuffer_Push(canHandle->tx_buffer, message, sizeof(FDCANTxMessage));
+
+		uint32_t idx = (canHandle->tx_tail + canHandle->tx_elements) % canHandle->tx_capacity;
+		canHandle->tx_buffer[idx] = *message;
+		canHandle->tx_elements++;
+		// memcpy(&canHandle->tx_buffer[idx], message , sizeof(FDCANTxMessage) );
+
+		//__set_BASEPRI(primask);
+
+		/*if (result != 0) {
+		    LOGOMATIC("CAN_send: buffer push failed\n");
+		    return -1;
+		} else {
+		    return 0;
+		}*/
+	} else {
+		LOGOMATIC("CAN_send: all buffers full\n"); // p
+	}
+	__set_BASEPRI(basepri);
+	// Both buffers full
+	return -1;
 }
 
 void HAL_FDCAN_TxBufferCompleteCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndexes)
@@ -375,22 +387,20 @@ void HAL_FDCAN_TxBufferCompleteCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t Bu
 	// If circular buffer has elements, send to queue
 	// Otherwise do nothing
 	// #ifdef USECAN1
-	CANHandle *handle = can_get_buffer_handle(hfdcan);
-
-	if (!handle || !handle->tx_buffer) {
-		return;
-	}
-
-	if (GR_CircularBuffer_IsEmpty(handle->tx_buffer)) {
-		return;
-	}
-
+	CANHandle *handle = can_get_handle(hfdcan);
 	// see if you can pop any more from the buffer
-	can_tx_buffer_helper(handle);
+	can_tx_dequeue_helper(handle);
+}
+
+void HAL_FDCAN_TxFifoEmptyCallback(FDCAN_HandleTypeDef *hfdcan)
+{
+	CANHandle *handle = can_get_handle(hfdcan);
+	can_tx_dequeue_helper(handle);
 }
+
 void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
 {
-	CANHandle *handle = can_get_buffer_handle(hfdcan);
+	CANHandle *handle = can_get_handle(hfdcan);
 
 	if (!handle || !handle->init || !handle->rx_callback) {
 		return;
@@ -401,9 +411,9 @@ void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
 	    return;
 	} */ // no rx buffer at the moment
 
-	/*if (RxFifo0ITs & FDCAN_IT_RX_FIFO0_MESSAGE_LOST) {
-	    lost_rx++;
-	}*/
+	if (RxFifo0ITs & FDCAN_IT_RX_FIFO0_MESSAGE_LOST) {
+		// lost_rx++;
+	}
 
 	if (!(RxFifo0ITs & ~FDCAN_IT_RX_FIFO0_MESSAGE_LOST)) {
 		return;
@@ -425,18 +435,13 @@ void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
 		// GR_OLD_MSG_ID messageID = (rx_header.Identifier & (0xFFF << 8)) >> 8;
 		handle->rx_callback(rx_header.Identifier, rx_data, rx_header.DataLength);
 	}
-
-	/* whoopsie, don't need the rx buffer yet
-	while (HAL_FDCAN_GetRxFifoFillLevel(hfdcan, FDCAN_RX_FIFO0) & !GR_CircularBuffer_IsFull(handle->rx_buffer)) {
-	    FDCAN_RxHeaderTypeDef rx_header;
-	    uint8_t rx_data[64] = {0};
-	    HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &rx_header, &rx_data);
-
+	/*
 	    if (GR_CircularBuffer_IsEmpty(handle->rx_buffer)) handle->rx_callback(rx_data, rx_header.DataLength);
 	    else {
-		GR_CircularBuffer_Push(handle->rx_buffer, rx_data, rx_header.DataLength);
+	    GR_CircularBuffer_Push(handle->rx_buffer, rx_data, rx_header.DataLength);
 	    }
 	}*/
+	//__set_BASEPRI(prev_priority);
 }
 
 /*
@@ -472,19 +477,32 @@ int can_add_filter(CANHandle *canHandle, FDCAN_FilterTypeDef *filter)
 	// check that # of filters isn't exceeding max value
 }
 
-// Need to
 int can_start(CANHandle *canHandle)
 {
 	if (!canHandle || !canHandle->init) {
 		return -1;
 	}
 
+	if (canHandle->started) {
+		return 0;
+	}
+
+	IRQn_Type rx0it, txit;
+	can_get_irqs(canHandle->hal_fdcanP->Instance, &rx0it, &txit);
+
+	HAL_NVIC_ClearPendingIRQ(rx0it); // prevent a spurious interrupt
+	HAL_NVIC_ClearPendingIRQ(txit);
+
 	GPIOx_CLK_ENABLE(canHandle->rx_gpio);
 	GPIOx_CLK_ENABLE(canHandle->tx_gpio);
 
 	HAL_FDCAN_Start(canHandle->hal_fdcanP);
+
 	canHandle->started = true;
 
+	HAL_NVIC_EnableIRQ(rx0it);
+	HAL_NVIC_EnableIRQ(txit);
+
 	return 0;
 }
 
@@ -493,12 +511,27 @@ int can_stop(CANHandle *canHandle)
 	if (!canHandle || !canHandle->init) {
 		return -1;
 	}
+
 	if (!canHandle->started) {
 		return 0;
 	}
 
+	// stop can interrupts from activating
+	uint32_t prev_priority = __get_BASEPRI();
+	__set_BASEPRI(MIN(canHandle->rx_interrupt_priority, canHandle->tx_interrupt_priority));
+
 	HAL_FDCAN_Stop(canHandle->hal_fdcanP);
 
+	IRQn_Type rx0it, txit;
+	can_get_irqs(canHandle->hal_fdcanP->Instance, &rx0it, &txit);
+
+	HAL_NVIC_DisableIRQ(rx0it);
+	HAL_NVIC_DisableIRQ(txit);
+	HAL_NVIC_ClearPendingIRQ(rx0it);
+	HAL_NVIC_ClearPendingIRQ(txit);
+
+	__set_BASEPRI(prev_priority);
+
 	GPIOx_CLK_DISABLE(canHandle->rx_gpio);
 	GPIOx_CLK_DISABLE(canHandle->tx_gpio);
 
@@ -507,136 +540,213 @@ int can_stop(CANHandle *canHandle)
 	return 0;
 }
 
-// Valid only for STM32G474xE
-/*int can_msp_deinit(CANHandle* canHandle) {
-    //MSP DeInit
-    //turn off gpio clocks - can only turn off GPIOs if no other instances are using them
+// ==================================== HELPER FUNCTIONS ===============================================
+// TODO: Abstract across families
+
+static inline void fdcan_enable_shared_clock(void)
+{
+	if (fdcan_shared_clock_ref == 0) {
+		__HAL_RCC_FDCAN_CLK_ENABLE();
+	}
+	fdcan_shared_clock_ref++;
+}
+
+static inline void fdcan_disable_shared_clock(void)
+{
+	if (fdcan_shared_clock_ref > 0) {
+		fdcan_shared_clock_ref--;
+		if (fdcan_shared_clock_ref == 0) {
+			__HAL_RCC_FDCAN_CLK_DISABLE();
+		}
+	}
+}
+
+// valid only for STM32G4
+static int can_get_irqs(FDCAN_GlobalTypeDef *instance, IRQn_Type *it0, IRQn_Type *it1)
+{
+	if (instance == FDCAN1) {
+		*it0 = FDCAN1_IT0_IRQn;
+		*it1 = FDCAN1_IT1_IRQn;
+		return 0;
+	}
+	if (instance == FDCAN2) {
+		*it0 = FDCAN2_IT0_IRQn;
+		*it1 = FDCAN2_IT1_IRQn;
+		return 0;
+	}
+	if (instance == FDCAN3) {
+		*it0 = FDCAN3_IT0_IRQn;
+		*it1 = FDCAN3_IT1_IRQn;
+		return 0;
+	}
+
+	return -1; // invalid instance
+}
+
+// valid only for STM32G4
+static CANHandle *can_get_handle(FDCAN_HandleTypeDef *hfdcan)
+{
+	// #ifdef STM32G474xx
+	if (hfdcan->Instance == FDCAN1) {
+		return &CAN1;
+	} else if (hfdcan->Instance == FDCAN2) {
+		return &CAN2;
+	} else if (hfdcan->Instance == FDCAN3) {
+		return &CAN3;
+	} else {
+		LOGOMATIC("CAN_get_handle: was given invalid FDCAN instance\n");
+		return 0;
+	}
+}
 
+/*
+static inline void gpio_clk_enable(GPIO_TypeDef *gpio)
+{
+    if (gpio == GPIOA) {
+	__HAL_RCC_GPIOA_CLK_ENABLE();
+    } else if (gpio == GPIOB) {
+	__HAL_RCC_GPIOB_CLK_ENABLE();
+    } else if (gpio == GPIOD) {
+	__HAL_RCC_GPIOD_CLK_ENABLE();
+    }
+}
 
-    return 0;
+static inline void gpio_clk_disable(GPIO_TypeDef *gpio)
+{
+    if (gpio == GPIOA) {
+	__HAL_RCC_GPIOA_CLK_DISABLE();
+    } else if (gpio == GPIOB) {
+	__HAL_RCC_GPIOB_CLK_DISABLE();
+    } else if (gpio == GPIOD) {
+	__HAL_RCC_GPIOD_CLK_DISABLE();
+    }
 }*/
 
-static void FDCAN_InstanceDeInit(FDCAN_HandleTypeDef *hfdcan)
+// only valid for #STM32G474x, must redefine for each family
+static int can_msp_init(CANHandle *canHandle, CANConfig *config)
 {
-	// Enter INIT mode
-	hfdcan->Instance->CCCR |= FDCAN_CCCR_INIT;
-	while (!(hfdcan->Instance->CCCR & FDCAN_CCCR_INIT))
-		;
+	// MSP Init ------- This could be inside HAL_FDCAN_MspInit() instead
+	// FDCAN Clock Select
 
-	// Clear filters
-	// TODO: fix magic numbers
-	memset((void *)hfdcan->msgRam.StandardFilterSA, 0, 0x0070);
-	memset((void *)hfdcan->msgRam.ExtendedFilterSA, 0, 0x0050);
+	fdcan_enable_shared_clock();
 
-	// Optionally reset FIFOs / buffers
+	// Clock speed for FDCAN determined by APB1 clock speed and FDCAN prescaler
 
-	// Disable interrupts
-	__HAL_FDCAN_DISABLE_IT(hfdcan, FDCAN_IT_LIST_RX_FIFO0 | FDCAN_IT_LIST_RX_FIFO1 | FDCAN_IT_LIST_SMSG | FDCAN_IT_LIST_TX_FIFO_ERROR | FDCAN_IT_LIST_MISC | FDCAN_IT_LIST_BIT_LINE_ERROR |
-					   FDCAN_IT_LIST_PROTOCOL_ERROR);
+	// GPIOs init
+	GPIOx_CLK_ENABLE(config->rx_gpio);
+	GPIOx_CLK_ENABLE(config->tx_gpio);
 
-	// Exit INIT mode
-	hfdcan->Instance->CCCR &= ~FDCAN_CCCR_INIT;
-	while (hfdcan->Instance->CCCR & FDCAN_CCCR_INIT)
-		;
+	HAL_GPIO_Init(config->rx_gpio, &(config->init_rx_gpio));
+	HAL_GPIO_Init(config->tx_gpio, &(config->init_tx_gpio));
 
-	// Update handle state
-	hfdcan->State = HAL_FDCAN_STATE_RESET;
-}
+	IRQn_Type rxit = -1;
+	IRQn_Type txit = -1;
+	can_get_irqs(canHandle->hal_fdcanP->Instance, &rxit, &txit);
 
-int can_release(CANHandle *canHandle)
-{
-	if (!canHandle) {
-		LOGOMATIC("CAN: Tried to release a null handle");
-		return -1;
-	}
+	// rxfifo0
+	HAL_NVIC_SetPriority(rxit, config->rx_interrupt_priority, 0);
 
-	if (!canHandle->init) {
-		LOGOMATIC("CAN_release: can instance is already deinitialized");
-		return -1;
-	}
-	can_stop(canHandle); // try to prevent more interrupts from firing
+	// tx
+	HAL_NVIC_SetPriority(txit, config->tx_interrupt_priority, 0);
+	// End MSP Init --------------
+
+	// Call can_start() to enable interrupts
 
-	// must disable NVIC IRQs before freeing circular buffer
+	return 0;
+}
+
+// Valid only for STM32G474xE
+static int can_msp_deinit(CANHandle *canHandle)
+{
+	// MSP DeInit
+	//  must disable NVIC IRQs before freeing circular buffer
 
-	// turn off NVIC resources
+	// NVIC
 	IRQn_Type rx0it = -1;
 	IRQn_Type txit = -1;
 	can_get_irqs(canHandle->hal_fdcanP->Instance, &rx0it, &txit);
 	HAL_NVIC_DisableIRQ(rx0it);
 	HAL_NVIC_DisableIRQ(txit);
 
-	// need to check if other pins are using before disabling - do this after mvp
-	// GPIOx_CLK_DISABLE(canHandle->rx_gpio);
-	// GPIOx_CLK_DISABLE(canHandle->tx_gpio);
-
-	// reset FDCANx instance and message RAM and filters, clear interrupts
-	// HAL_FDCAN_DeInit(canHandle->hal_fdcanP); resets a little too hard
-	FDCAN_InstanceDeInit(canHandle->hal_fdcanP);
+	// TODO: turn off gpio clocks if no other peripherals are using them??? Could implement a shared GPIO layer
+	HAL_GPIO_DeInit(canHandle->rx_gpio, canHandle->rx_pin);
+	HAL_GPIO_DeInit(canHandle->tx_gpio, canHandle->tx_pin);
 
-	__DSB(); // Data Synchronization Barrier
-	__ISB(); // Instruction Synchronization Barrier
+	// MSP shared layer for GPIOs
+	// TODO: used to disable GPIOs clocks, but that might affect other peripherals
 
-	// free circular buffer contents
-	GR_CircularBuffer_Free(&(canHandle->tx_buffer));
-
-	// reset can instance
-	FDCAN_HandleTypeDef *temp = canHandle->hal_fdcanP;
-	memset(canHandle, 0, sizeof(*canHandle));
-	canHandle->hal_fdcanP = temp;
-
-	fdcan_disable_shared_clock(); // only turns off clock if no other instances are running.
+	// RCC
+	fdcan_disable_shared_clock();
 
 	return 0;
 }
 
-int can_send(CANHandle *canHandle, FDCANTxMessage *message)
+static void FDCAN_InstanceDeInit(FDCAN_HandleTypeDef *hfdcan)
 {
-	if (!canHandle || !message) {
-		LOGOMATIC("CAN_send: received null pointer\n");
-		return -1;
-	}
+	// Enter INIT mode
+	hfdcan->Instance->CCCR |= FDCAN_CCCR_INIT;
+	while (!(hfdcan->Instance->CCCR & FDCAN_CCCR_INIT))
+		;
 
-	if (!canHandle->init || !canHandle->started) {
-		LOGOMATIC("CAN_send: CAN not initialized or started\n");
-		return -1;
-	}
+	// Disable interrupts
+	__HAL_FDCAN_DISABLE_IT(hfdcan, FDCAN_IT_LIST_RX_FIFO0 | FDCAN_IT_LIST_RX_FIFO1 | FDCAN_IT_LIST_SMSG | FDCAN_IT_LIST_TX_FIFO_ERROR | FDCAN_IT_LIST_MISC | FDCAN_IT_LIST_BIT_LINE_ERROR |
+					   FDCAN_IT_LIST_PROTOCOL_ERROR);
 
-	uint32_t primask = __get_PRIMASK();
-	__disable_irq();
-	// IF TX Fifos are not full, send directly to them
-	// If TX Fifos are full, append to circular buffer
-	// If circular buffer is full, return an error code
-	uint32_t free = HAL_FDCAN_GetTxFifoFreeLevel(canHandle->hal_fdcanP);
+	// Clear filters
+	// TODO: fix magic numbers
+	memset((void *)hfdcan->msgRam.StandardFilterSA, 0, 0x0070);
+	memset((void *)hfdcan->msgRam.ExtendedFilterSA, 0, 0x0050);
 
-	if (free > 0) {
-		HAL_StatusTypeDef status = HAL_FDCAN_AddMessageToTxFifoQ(canHandle->hal_fdcanP, &(message->tx_header),
-									 message->data // Not &message->data if data is array
-		);
+	// Optionally clear FIFOs / buffers
 
-		__set_PRIMASK(primask);
+	// Exit INIT mode
+	hfdcan->Instance->CCCR &= ~FDCAN_CCCR_INIT;
+	while (hfdcan->Instance->CCCR & FDCAN_CCCR_INIT)
+		;
 
-		if (status != HAL_OK) {
-			LOGOMATIC("CAN_send: failed to add to HW FIFO\n");
-			return -1;
-		}
-		return 0;
-	}
+	// Update handle state
+	hfdcan->State = HAL_FDCAN_STATE_RESET;
+}
 
-	// Hardware FIFO full, try software buffer
-	if (!GR_CircularBuffer_IsFull(canHandle->tx_buffer)) {
-		int result = GR_CircularBuffer_Push(canHandle->tx_buffer, message, sizeof(FDCANTxMessage));
+// valid only for STM32G4
+/*static const char *can_get_instance_name(FDCAN_GlobalTypeDef *instance)
+{
+ if (instance == FDCAN1) {
+     return "FDCAN1";
+ } else if (instance == FDCAN2) {
+     return "FDCAN2";
+ } else if (instance == FDCAN3) {
+     return "FDCAN3";
+ }
+ return "UNKNOWN";
+}*/
 
-		__set_PRIMASK(primask);
+// ===================================== HAL Callbacks ================================
+// TODO: Implement Family Checks
+// Probably is safe from races
+void FDCAN1_IT0_IRQHandler(void)
+{
+	HAL_FDCAN_IRQHandler(&hal_fdcan1);
+}
+void FDCAN1_IT1_IRQHandler(void)
+{
+	HAL_FDCAN_IRQHandler(&hal_fdcan1);
+}
 
-		if (result != 0) {
-			LOGOMATIC("CAN_send: buffer push failed\n");
-			return -1;
-		}
-		return 0;
-	}
+void FDCAN2_IT0_IRQHandler(void)
+{
+	HAL_FDCAN_IRQHandler(&hal_fdcan2);
+}
+void FDCAN2_IT1_IRQHandler(void)
+{
+	HAL_FDCAN_IRQHandler(&hal_fdcan2);
+}
 
-	// Both buffers full
-	__set_PRIMASK(primask);
-	LOGOMATIC("CAN_send: all buffers full\n");
-	return -1;
+void FDCAN3_IT0_IRQHandler(void)
+{
+	HAL_FDCAN_IRQHandler(&hal_fdcan3);
+}
+void FDCAN3_IT1_IRQHandler(void)
+{
+	HAL_FDCAN_IRQHandler(&hal_fdcan3);
 }
diff --git a/Lib/Peripherals/CAN/Test/can_tests.c b/Lib/Peripherals/CAN/Test/can_tests.c
index 240d3300c..d8310f065 100644
--- a/Lib/Peripherals/CAN/Test/can_tests.c
+++ b/Lib/Peripherals/CAN/Test/can_tests.c
@@ -28,16 +28,16 @@ void can_test_rx_callback1(uint32_t id, void *data, uint32_t size)
 	return;
 }
 
+// TODO: G4 tests are dependent on the System clock configuration
 int can_test(void)
 {
-
 	CANConfig canCfg;
 	// canCfg.fdcan_instance = FDCAN2;
 
 	canCfg.hal_fdcan_init.ClockDivider = FDCAN_CLOCK_DIV1;
 	canCfg.hal_fdcan_init.FrameFormat = FDCAN_FRAME_FD_NO_BRS;
 	canCfg.hal_fdcan_init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION;
-	canCfg.hal_fdcan_init.Mode = FDCAN_MODE_NORMAL;
+	canCfg.hal_fdcan_init.Mode = FDCAN_MODE_INTERNAL_LOOPBACK;
 	canCfg.hal_fdcan_init.AutoRetransmission = ENABLE;
 	canCfg.hal_fdcan_init.TransmitPause = DISABLE;
 	canCfg.hal_fdcan_init.ProtocolException = ENABLE;
@@ -53,9 +53,8 @@ int can_test(void)
 	canCfg.hal_fdcan_init.ExtFiltersNbr = 0;
 
 	canCfg.rx_callback = NULL;	  // PLEASE SET
-	canCfg.rx_interrupt_priority = 0; // PLEASE SET
-	canCfg.tx_interrupt_priority = 0; // PLEASE SET
-	canCfg.tx_buffer_length = 3;	  // PLEASE SET
+	canCfg.rx_interrupt_priority = 5; // PLEASE SET
+	canCfg.tx_interrupt_priority = 5; // PLEASE SET
 
 	// canCfg.rx_gpio = GPIOB;
 	// canCfg.init_rx_gpio.Pin = GPIO_PIN_12;
@@ -91,14 +90,13 @@ int can_test(void)
 	};
 
 	FDCANTxMessage msg;
-	msg.data[0] = 0x80;
 	memset(&(msg.data), 0, sizeof(msg.data));
+	msg.data[0] = 0x80;
 	msg.tx_header = TxHeader;
 
 	can_set_clksource(LL_RCC_FDCAN_CLKSOURCE_PCLK1);
 
 #ifdef FDCAN1
-
 	canCfg.fdcan_instance = FDCAN1;
 	canCfg.rx_gpio = GPIOA;
 	canCfg.init_rx_gpio.Pin = GPIO_PIN_11;
@@ -117,7 +115,6 @@ int can_test(void)
 
 #endif
 #ifdef FDCAN2
-
 	canCfg.fdcan_instance = FDCAN2;
 	canCfg.rx_gpio = GPIOB;
 	canCfg.init_rx_gpio.Pin = GPIO_PIN_12;