From acdb5735d21ebd67f2373858f3196b73a6646c53 Mon Sep 17 00:00:00 2001
From: Hubert Denkmair <hubert@denkmair.de>
Date: Tue, 12 Apr 2016 22:32:37 +0200
Subject: [PATCH] less copy, fewer bugs, maybe better performance

---
 include/can.h |  4 ++-
 src/can.c     | 71 ++++++++++++++++++++++++++++++++++++++++++++++-----
 src/main.c    | 34 +++++++-----------------
 3 files changed, 77 insertions(+), 32 deletions(-)

diff --git a/include/can.h b/include/can.h
index a5937f4..6c0648b 100644
--- a/include/can.h
+++ b/include/can.h
@@ -2,12 +2,14 @@
 
 #include <stdbool.h>
 #include "stm32f0xx_hal.h"
+#include <gs_usb.h>
 
 void can_init(CAN_HandleTypeDef *hcan, CAN_TypeDef *instance);
 void can_set_bittiming(CAN_HandleTypeDef *hcan, uint16_t brp, uint8_t phase_seg1, uint8_t phase_seg2, uint8_t sjw);
 void can_enable(CAN_HandleTypeDef *hcan, bool loop_back, bool listen_only, bool one_shot);
 void can_disable(CAN_HandleTypeDef *hcan);
 
-bool can_send(CAN_HandleTypeDef *hcan, CanTxMsgTypeDef *tx_msg, uint32_t timeout);
 bool can_receive(CAN_HandleTypeDef *hcan, CanRxMsgTypeDef *rx_msg, uint32_t timeout);
 bool can_is_rx_pending(CAN_HandleTypeDef *hcan);
+
+bool can_send(CAN_HandleTypeDef *hcan, struct gs_host_frame *frame);
diff --git a/src/can.c b/src/can.c
index a65ddb6..a8ae1d0 100644
--- a/src/can.c
+++ b/src/can.c
@@ -65,12 +65,6 @@ void can_disable(CAN_HandleTypeDef *hcan)
 	HAL_CAN_DeInit(hcan);
 }
 
-bool can_send(CAN_HandleTypeDef *hcan, CanTxMsgTypeDef *tx_msg, uint32_t timeout)
-{
-	hcan->pTxMsg = tx_msg;
-	return HAL_CAN_Transmit(hcan, timeout) == HAL_OK;
-}
-
 bool can_receive(CAN_HandleTypeDef *hcan, CanRxMsgTypeDef *rx_msg, uint32_t timeout)
 {
 	hcan->pRxMsg = rx_msg;
@@ -81,3 +75,68 @@ bool can_is_rx_pending(CAN_HandleTypeDef *hcan)
 {
 	return (__HAL_CAN_MSG_PENDING(hcan, CAN_FIFO0) > 0);
 }
+
+static CAN_TxMailBox_TypeDef *can_find_free_mailbox(CAN_HandleTypeDef *hcan)
+{
+	uint32_t tsr = hcan->Instance->TSR;
+	if ( tsr & CAN_TSR_TME0 ) {
+		return &hcan->Instance->sTxMailBox[0];
+	} else if ( tsr & CAN_TSR_TME1 ) {
+		return &hcan->Instance->sTxMailBox[1];
+	} else if ( tsr & CAN_TSR_TME2 ) {
+		return &hcan->Instance->sTxMailBox[2];
+	} else {
+		return 0;
+	}
+}
+
+bool can_send(CAN_HandleTypeDef *hcan, struct gs_host_frame *frame)
+{
+	bool retval;
+
+	__HAL_LOCK(hcan);
+
+	CAN_TxMailBox_TypeDef *mb = can_find_free_mailbox(hcan);
+	if (mb != 0) {
+
+		/* first, clear transmission request */
+		mb->TIR &= CAN_TI0R_TXRQ;
+
+		if (frame->can_id & CAN_EFF_FLAG) { // extended id
+			mb->TIR = CAN_ID_EXT | (frame->can_id & 0x1FFFFFFF) << 3;
+		} else {
+			mb->TIR = (frame->can_id & 0x7FF) << 21;
+		}
+
+		if (frame->can_id & CAN_RTR_FLAG) {
+			mb->TIR |= CAN_RTR_REMOTE;
+		}
+
+		mb->TDTR &= 0xFFFFFFF0;
+		mb->TDTR |= frame->can_dlc & 0x0F;
+
+		mb->TDLR =
+			  ( frame->data[3] << 24 )
+			| ( frame->data[2] << 16 )
+			| ( frame->data[1] <<  8 )
+			| ( frame->data[0] <<  0 );
+
+		mb->TDHR =
+			  ( frame->data[7] << 24 )
+			| ( frame->data[6] << 16 )
+			| ( frame->data[5] <<  8 )
+			| ( frame->data[4] <<  0 );
+
+		/* request transmission */
+		mb->TIR |= CAN_TI0R_TXRQ;
+
+		retval = true;
+	} else {
+		retval = false;
+	}
+
+	__HAL_UNLOCK(hcan);
+
+	return retval;
+}
+
diff --git a/src/main.c b/src/main.c
index 806dd7f..a0c668d 100644
--- a/src/main.c
+++ b/src/main.c
@@ -7,7 +7,7 @@
 #include <gs_usb.h>
 #include <can.h>
 
-#define CAN_QUEUE_SIZE 32
+#define CAN_QUEUE_SIZE 64
 
 void SystemClock_Config(void);
 static void MX_GPIO_Init(void);
@@ -32,6 +32,7 @@ bool send_to_host_or_enqueue(struct gs_host_frame *frame)
 	return retval;
 }
 
+
 int main(void)
 {
 
@@ -65,32 +66,15 @@ int main(void)
 	while (1) {
 
 
-		if (queue_size(q_from_host)>0) { // send can message from host
-			CanTxMsgTypeDef tx_msg;
-			struct gs_host_frame *frame = queue_pop_front(q_from_host);
+		struct gs_host_frame *frame = queue_pop_front(q_from_host);
+		if (frame != 0) { // send can message from host
 
-			if (frame != 0) {
-				if ((frame->can_id & CAN_EFF_FLAG) != 0) {
-					tx_msg.IDE = CAN_ID_EXT;
-					tx_msg.ExtId = frame->can_id & 0x1FFFFFFF;
-				} else {
-					tx_msg.IDE = CAN_ID_STD;
-					tx_msg.StdId = frame->can_id & 0x7FF;
-				}
-
-				if ((frame->can_id & CAN_RTR_FLAG) != 0) {
-					tx_msg.RTR = CAN_RTR_REMOTE;
-				}
-
-				tx_msg.DLC = MIN(8,frame->can_dlc);
-				memcpy(tx_msg.Data, frame->data, tx_msg.DLC);
-
-				if (can_send(&hCAN, &tx_msg, 10)) {
-					send_to_host_or_enqueue(frame);
-				} else {
-					queue_push_front(q_from_host, frame); // retry later
-				}
+			if (can_send(&hCAN, frame)) {
+				send_to_host_or_enqueue(frame);
+			} else {
+				queue_push_front(q_from_host, frame); // retry later
 			}
+
 		}
 
 		if (USBD_GS_CAN_TxReady(&hUSB)) {