diff --git a/include/can.h b/include/can.h
index 6c0648b..c236f31 100644
--- a/include/can.h
+++ b/include/can.h
@@ -9,7 +9,7 @@ void can_set_bittiming(CAN_HandleTypeDef *hcan, uint16_t brp, uint8_t phase_seg1
 void can_enable(CAN_HandleTypeDef *hcan, bool loop_back, bool listen_only, bool one_shot);
 void can_disable(CAN_HandleTypeDef *hcan);
 
-bool can_receive(CAN_HandleTypeDef *hcan, CanRxMsgTypeDef *rx_msg, uint32_t timeout);
+bool can_receive(CAN_HandleTypeDef *hcan, struct gs_host_frame *rx_frame);
 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 a8ae1d0..51c4d6e 100644
--- a/src/can.c
+++ b/src/can.c
@@ -65,15 +65,48 @@ void can_disable(CAN_HandleTypeDef *hcan)
 	HAL_CAN_DeInit(hcan);
 }
 
-bool can_receive(CAN_HandleTypeDef *hcan, CanRxMsgTypeDef *rx_msg, uint32_t timeout)
-{
-	hcan->pRxMsg = rx_msg;
-	return HAL_CAN_Receive(hcan, CAN_FIFO0, timeout) == HAL_OK;
-}
-
 bool can_is_rx_pending(CAN_HandleTypeDef *hcan)
 {
-	return (__HAL_CAN_MSG_PENDING(hcan, CAN_FIFO0) > 0);
+	return ((hcan->Instance->RF0R & CAN_RF0R_FMP0)!=0);
+}
+
+bool can_receive(CAN_HandleTypeDef *hcan, struct gs_host_frame *rx_frame)
+{
+	bool retval = false;
+
+	__HAL_LOCK(hcan);
+	if (can_is_rx_pending(hcan)) {
+
+		CAN_FIFOMailBox_TypeDef *fifo = &hcan->Instance->sFIFOMailBox[0];
+
+		if (fifo->RIR &  CAN_RI0R_IDE) {
+			rx_frame->can_id =  CAN_EFF_FLAG | ((fifo->RIR >> 3) & 0x1FFFFFFF);
+		} else {
+			rx_frame->can_id = (fifo->RIR >> 21) & 0x7FF;
+		}
+
+		if (fifo->RIR & CAN_RI0R_RTR)  {
+			rx_frame->can_id |= CAN_RTR_FLAG;
+		}
+
+		rx_frame->can_dlc = fifo->RDTR & CAN_RDT0R_DLC;
+
+		rx_frame->data[0] = (fifo->RDLR >>  0) & 0xFF;
+		rx_frame->data[1] = (fifo->RDLR >>  8) & 0xFF;
+		rx_frame->data[2] = (fifo->RDLR >> 16) & 0xFF;
+		rx_frame->data[3] = (fifo->RDLR >> 24) & 0xFF;
+		rx_frame->data[4] = (fifo->RDHR >>  0) & 0xFF;
+		rx_frame->data[5] = (fifo->RDHR >>  8) & 0xFF;
+		rx_frame->data[6] = (fifo->RDHR >> 16) & 0xFF;
+		rx_frame->data[7] = (fifo->RDHR >> 24) & 0xFF;
+
+		hcan->Instance->RF0R |= CAN_RF0R_RFOM0; // release FIFO
+
+	    retval = true;
+	}
+	__HAL_UNLOCK(hcan);
+
+	return retval;
 }
 
 static CAN_TxMailBox_TypeDef *can_find_free_mailbox(CAN_HandleTypeDef *hcan)
diff --git a/src/main.c b/src/main.c
index a0c668d..9ebfa98 100644
--- a/src/main.c
+++ b/src/main.c
@@ -91,35 +91,18 @@ int main(void)
 
 		if (can_is_rx_pending(&hCAN)) {
 			struct gs_host_frame *frame = queue_pop_front(q_frame_pool);
-			if (frame) {
-				CanRxMsgTypeDef rx_msg;
-				if (can_receive(&hCAN, &rx_msg, 0)) {
+			if ((frame != 0) && can_receive(&hCAN, frame)) {
 
-					frame->echo_id = 0xFFFFFFFF; // not a echo frame
+				frame->echo_id = 0xFFFFFFFF; // not a echo frame
+				frame->channel = 0;
+				frame->flags = 0;
+				frame->reserved = 0;
+				send_to_host_or_enqueue(frame);
 
-					frame->can_dlc = MIN(8, rx_msg.DLC);
-					frame->channel = 0;
-					frame->flags = 0;
-					frame->reserved = 0;
-
-					if (rx_msg.IDE) {
-						frame->can_id = rx_msg.ExtId | CAN_EFF_FLAG;
-					} else {
-						frame->can_id = rx_msg.StdId;
-					}
-
-					if (rx_msg.RTR) {
-						frame->can_id |= CAN_RTR_FLAG;
-					}
-
-					memcpy(frame->data, rx_msg.Data, frame->can_dlc);
-
-					send_to_host_or_enqueue(frame);
-
-				} else {
-					queue_push_back(q_frame_pool, frame);
-				}
+			} else {
+				queue_push_back(q_frame_pool, frame);
 			}
+
 		}
 
 		if (HAL_GetTick() >= t_next_send) {
diff --git a/src/usbd_conf.c b/src/usbd_conf.c
index 3482589..2bcbbe5 100644
--- a/src/usbd_conf.c
+++ b/src/usbd_conf.c
@@ -274,10 +274,12 @@ USBD_StatusTypeDef  USBD_LL_Init (USBD_HandleTypeDef *pdev)
 
   HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , 0x00 , PCD_SNG_BUF, 0x18);
   HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , 0x80 , PCD_SNG_BUF, 0x58);
-  HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , 0x81 , PCD_SNG_BUF, 0xC0);
-  HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , 0x01 , PCD_SNG_BUF, 0x110);
-  HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , 0x82 , PCD_SNG_BUF, 0x100);
-  HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , 0x02 , PCD_SNG_BUF, 0x200);
+
+  //HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , 0x81 , PCD_DBL_BUF, 0x01000140);
+  //HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , 0x02 , PCD_DBL_BUF, 0x018001C0);
+  HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , 0x81 , PCD_SNG_BUF, 0x0100);
+  HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , 0x02 , PCD_SNG_BUF, 0x0180);
+
   return USBD_OK;
 }