From da87a52f0078185c5a3df9d2279181216ec442a1 Mon Sep 17 00:00:00 2001
From: Baldanos <balda@balda.ch>
Date: Wed, 27 Jan 2016 22:40:26 +0100
Subject: [PATCH] Added CAN mode in bbIO

---
 hydrabus/hydrabus_bbio.c | 137 +++++++++++++++++++++++++++++++++++++++
 hydrabus/hydrabus_bbio.h |  15 +++++
 2 files changed, 152 insertions(+)

diff --git a/hydrabus/hydrabus_bbio.c b/hydrabus/hydrabus_bbio.c
index e4519787..50980f33 100644
--- a/hydrabus/hydrabus_bbio.c
+++ b/hydrabus/hydrabus_bbio.c
@@ -26,8 +26,17 @@
 #include <ctype.h>
 
 #include "bsp_spi.h"
+#include "bsp_can.h"
 #include "hydrabus_mode_jtag.h"
 
+static void print_raw_uint32(t_hydra_console *con, uint32_t num)
+{
+	cprintf(con, "%c%c%c%c",((num>>24)&0xFF),
+		((num>>16)&0xFF),
+		((num>>8)&0xFF),
+		(num&0xFF));
+}
+
 static void bbio_spi_init_proto_default(t_hydra_console *con)
 {
 	mode_config_proto_t* proto = &con->mode->proto;
@@ -201,6 +210,130 @@ static void bbio_mode_spi(t_hydra_console *con)
 	}
 }
 
+static void bbio_mode_can(t_hydra_console *con)
+{
+	uint8_t bbio_subcommand;
+	bsp_status_t status;
+	mode_config_proto_t* proto = &con->mode->proto;
+
+	uint8_t rx_buff[10], i, to_tx;
+	CanTxMsgTypeDef tx_msg;
+	CanRxMsgTypeDef rx_msg;
+	uint32_t can_id;
+	uint32_t filter_low=0, filter_high=0;
+
+	proto->dev_num = 0;
+	proto->dev_speed = 500000;
+
+	bsp_can_init(proto->dev_num, proto);
+	bsp_can_init_filter(proto->dev_num, proto);
+
+	while(!USER_BUTTON) {
+		if(chSequentialStreamRead(con->sdu, &bbio_subcommand, 1) == 1) {
+			switch(bbio_subcommand) {
+			case BBIO_RESET:
+				bsp_can_deinit(proto->dev_num);
+				return;
+			case BBIO_CAN_ID:
+				chSequentialStreamRead(con->sdu, rx_buff, 4);
+				can_id =  rx_buff[0] << 24;
+				can_id += rx_buff[1] << 16;
+				can_id += rx_buff[2] << 8;
+				can_id += rx_buff[3];
+				cprint(con, "\x01", 1);
+				break;
+			case BBIO_CAN_FILTER_OFF:
+				status = bsp_can_init_filter(proto->dev_num, proto);
+					if(status == BSP_OK) {
+						cprint(con, "\x01", 1);
+					} else {
+						cprint(con, "\x00", 1);
+					}
+				break;
+			case BBIO_CAN_FILTER_ON:
+				status = bsp_can_set_filter(proto->dev_num, proto, filter_low, filter_high);
+				if(status == BSP_OK) {
+					cprint(con, "\x01", 1);
+				} else {
+					cprint(con, "\x00", 1);
+				}
+				break;
+			case BBIO_CAN_READ:
+				status = bsp_can_read(proto->dev_num, &rx_msg);
+				if(status == BSP_OK) {
+					cprint(con, "\x01", 1);
+					if(rx_msg.IDE == CAN_ID_STD) {
+						print_raw_uint32(con, (uint32_t)rx_msg.StdId);
+					}else{
+						print_raw_uint32(con, (uint32_t)rx_msg.ExtId);
+					}
+					cprintf(con, "%c", rx_msg.DLC);
+					for(i=0; i<rx_msg.DLC; i++){
+						cprintf(con, "%c",
+							rx_msg.Data[i]);
+					}
+
+				}else{
+					cprint(con, "\x00", 1);
+				}
+				break;
+			default:
+				if ((bbio_subcommand & BBIO_CAN_WRITE) == BBIO_CAN_WRITE) {
+
+					if (can_id < 0b11111111111) {
+						tx_msg.StdId = can_id;
+						tx_msg.IDE = CAN_ID_STD;
+					} else {
+						tx_msg.ExtId = can_id;
+						tx_msg.IDE = CAN_ID_EXT;
+					}
+
+					to_tx = (bbio_subcommand & 0b111)+1;
+
+					tx_msg.RTR = CAN_RTR_DATA;
+					tx_msg.DLC = to_tx;
+
+					chSequentialStreamRead(con->sdu, rx_buff,
+							       to_tx);
+
+					for(i=0; i<to_tx; i++) {
+						tx_msg.Data[i] = rx_buff[i];
+					}
+
+					status = bsp_can_write(proto->dev_num, &tx_msg);
+
+					if(status == BSP_OK) {
+						cprint(con, "\x01", 1);
+					} else {
+						cprint(con, "\x00", 1);
+					}
+				} else if((bbio_subcommand & BBIO_CAN_FILTER) == BBIO_CAN_FILTER) {
+					chSequentialStreamRead(con->sdu, rx_buff, 4);
+					if(bbio_subcommand & 1) {
+						filter_high =  rx_buff[0] << 24;
+						filter_high += rx_buff[1] << 16;
+						filter_high += rx_buff[2] << 8;
+						filter_high += rx_buff[3];
+					} else {
+						filter_low =  rx_buff[0] << 24;
+						filter_low += rx_buff[1] << 16;
+						filter_low += rx_buff[2] << 8;
+						filter_low += rx_buff[3];
+					}
+					status = bsp_can_set_filter(proto->dev_num, proto,
+							   filter_low, filter_high);
+					if(status == BSP_OK) {
+						cprint(con, "\x01", 1);
+					} else {
+						cprint(con, "\x00", 1);
+					}
+
+				}
+
+			}
+		}
+	}
+}
 
 int cmd_bbio(t_hydra_console *con)
 {
@@ -235,6 +368,10 @@ int cmd_bbio(t_hydra_console *con)
 				cprint(con, "OCD1", 4);
 				openOCD(con);
 				break;
+			case BBIO_CAN:
+				cprint(con, "CAN1", 4);
+				bbio_mode_can(con);
+				break;
 			case BBIO_RESET_HW:
 				return TRUE;
 			default:
diff --git a/hydrabus/hydrabus_bbio.h b/hydrabus/hydrabus_bbio.h
index 3fde3e77..d76d7cca 100644
--- a/hydrabus/hydrabus_bbio.h
+++ b/hydrabus/hydrabus_bbio.h
@@ -27,6 +27,9 @@
 #define BBIO_RAWWIRE	0b00000101
 #define BBIO_JTAG	0b00000110
 
+//Hydrabus specific
+#define BBIO_CAN	0b00001000
+
 #define BBIO_RESET_HW	0b00001111
 #define BBIO_PWM	0b00010010
 #define BBIO_PWM_CLEAR	0b00010010
@@ -34,6 +37,7 @@
 #define BBIO_VOLT_CONT	0b00010101
 #define BBIO_FREQ	0b00010110
 
+
 /*
  * SPI-specific commands
  */
@@ -50,4 +54,15 @@
 #define BBIO_SPI_SET_SPEED	0b01100000
 #define BBIO_SPI_CONFIG		0b10000000
 
+/*
+ * CAN-specific commands
+ */
+#define BBIO_CAN_READ		0b00000010
+#define BBIO_CAN_ID		0b00000011
+#define BBIO_CAN_FILTER_OFF	0b00000100
+#define BBIO_CAN_FILTER_ON	0b00000101
+#define BBIO_CAN_FILTER		0b00000110
+#define BBIO_CAN_WRITE		0b00001000
+
+
 int cmd_bbio(t_hydra_console *con);