|
@@ -0,0 +1,584 @@
|
|
|
|
|
+#include "s600_can.h"
|
|
|
|
|
+#include "s600_iap.h"
|
|
|
|
|
+#include "string.h"
|
|
|
|
|
+#include "stdlib.h"
|
|
|
|
|
+
|
|
|
|
|
+#ifndef CONFIG_CAN_ID
|
|
|
|
|
+#error CONFIG_CAN_ID not set
|
|
|
|
|
+#endif
|
|
|
|
|
+
|
|
|
|
|
+#define S600_CAN_LOOPBACK 0
|
|
|
|
|
+
|
|
|
|
|
+static s600_can_packet_t s600_can_tx_packets[5];
|
|
|
|
|
+static s600_can_packet_t *s600_can_tx_front;
|
|
|
|
|
+
|
|
|
|
|
+static s600_can_packet_t s600_can_rx_packets[10];
|
|
|
|
|
+static s600_can_packet_t *s600_can_rx_fronts[10];
|
|
|
|
|
+static u8 s600_can_rx_front_len;
|
|
|
|
|
+
|
|
|
|
|
+static u8 s600_can_tx_alloc_buff[NELEM(s600_can_tx_packets) + 1];
|
|
|
|
|
+static byte_queue_t s600_can_tx_allocator;
|
|
|
|
|
+
|
|
|
|
|
+static u8 s600_can_rx_alloc_buff[NELEM(s600_can_rx_packets) + 1];
|
|
|
|
|
+static byte_queue_t s600_can_rx_allocator;
|
|
|
|
|
+
|
|
|
|
|
+static u8 s600_can_tx_buff[NELEM(s600_can_tx_packets) + 1];
|
|
|
|
|
+static byte_queue_t s600_can_tx_queue = { s600_can_tx_buff, NELEM(s600_can_tx_buff), 0, 0 };
|
|
|
|
|
+
|
|
|
|
|
+static u8 s600_can_rx_buff[NELEM(s600_can_rx_packets) + 1];
|
|
|
|
|
+static byte_queue_t s600_can_rx_queue = { s600_can_rx_buff, NELEM(s600_can_rx_buff), 0, 0 };
|
|
|
|
|
+
|
|
|
|
|
+// ================================================================================
|
|
|
|
|
+
|
|
|
|
|
+static u32 s600_can_write_position;
|
|
|
|
|
+
|
|
|
|
|
+__STATIC_INLINE int s600_can_process_cmd_write_begin(const u8 *buff, u16 length)
|
|
|
|
|
+{
|
|
|
|
|
+ s600_can_write_position = 0;
|
|
|
|
|
+ s600_iap_write_begin();
|
|
|
|
|
+ return 0;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+__STATIC_INLINE int s600_can_process_cmd_write_end(const u8 *buff, u16 length)
|
|
|
|
|
+{
|
|
|
|
|
+ u32 size, checksum;
|
|
|
|
|
+
|
|
|
|
|
+ if (length < 7) {
|
|
|
|
|
+ return -1;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ size = s600_decode_u24(buff);
|
|
|
|
|
+ checksum = s600_decode_u32(buff + 3);
|
|
|
|
|
+
|
|
|
|
|
+ if (s600_iap_write_end(size, checksum)) {
|
|
|
|
|
+ return 0;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ return -1;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+__STATIC_INLINE int s600_can_process_cmd_write_slow(u8 can, u8 *buff, u16 length)
|
|
|
|
|
+{
|
|
|
|
|
+ u32 position;
|
|
|
|
|
+
|
|
|
|
|
+ if (length < 3) {
|
|
|
|
|
+ return -1;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ position = s600_decode_u24(buff);
|
|
|
|
|
+
|
|
|
|
|
+ if (s600_can_write_position == position) {
|
|
|
|
|
+ length -= 3;
|
|
|
|
|
+ s600_iap_write(buff + 3, length);
|
|
|
|
|
+ s600_can_write_position += length;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ s600_encode_u24(buff + 1, s600_can_write_position);
|
|
|
|
|
+
|
|
|
|
|
+ return 3;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+__STATIC_INLINE void s600_can_process_command(u8 can, u8 *command, u16 length)
|
|
|
|
|
+{
|
|
|
|
|
+ int rsplen;
|
|
|
|
|
+ u8 *args;
|
|
|
|
|
+ u16 key;
|
|
|
|
|
+
|
|
|
|
|
+ if (length < 2) {
|
|
|
|
|
+ return;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ key = *(u16 *) command;
|
|
|
|
|
+
|
|
|
|
|
+ if ((key & 0xFFF0) != S600_CAN_BUILD_CMD(0xF0)) {
|
|
|
|
|
+ return;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ s600_iap_100ms = 0;
|
|
|
|
|
+ args = command + 2;
|
|
|
|
|
+ length -= 2;
|
|
|
|
|
+
|
|
|
|
|
+ switch (key)
|
|
|
|
|
+ {
|
|
|
|
|
+ case S600_CAN_BUILD_CMD(0xF0):
|
|
|
|
|
+ rsplen = 0;
|
|
|
|
|
+ break;
|
|
|
|
|
+
|
|
|
|
|
+ case S600_CAN_BUILD_CMD(0xF1):
|
|
|
|
|
+ rsplen = s600_can_process_cmd_write_begin(args, length);
|
|
|
|
|
+ break;
|
|
|
|
|
+
|
|
|
|
|
+ case S600_CAN_BUILD_CMD(0xF2):
|
|
|
|
|
+ rsplen = s600_can_process_cmd_write_end(args, length);
|
|
|
|
|
+ break;
|
|
|
|
|
+
|
|
|
|
|
+ case S600_CAN_BUILD_CMD(0xF3):
|
|
|
|
|
+ s600_iap_write(args, length);
|
|
|
|
|
+ return;
|
|
|
|
|
+
|
|
|
|
|
+ case S600_CAN_BUILD_CMD(0xF4):
|
|
|
|
|
+ rsplen = s600_can_process_cmd_write_slow(can, args, length);
|
|
|
|
|
+ break;
|
|
|
|
|
+
|
|
|
|
|
+ case S600_CAN_BUILD_CMD(0xF5):
|
|
|
|
|
+ s600_iap_boot(false);
|
|
|
|
|
+ rsplen = -1;
|
|
|
|
|
+ break;
|
|
|
|
|
+
|
|
|
|
|
+ default:
|
|
|
|
|
+ rsplen = -1;
|
|
|
|
|
+ break;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ if (rsplen < 0) {
|
|
|
|
|
+ rsplen = 0;
|
|
|
|
|
+ args[0] = 0x01;
|
|
|
|
|
+ } else {
|
|
|
|
|
+ args[0] = 0x00;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ s600_can_send_response(can, command, rsplen + 3);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+// ================================================================================
|
|
|
|
|
+
|
|
|
|
|
+__STATIC_INLINE void s600_can_rcu_config(void)
|
|
|
|
|
+{
|
|
|
|
|
+ byte_queue_alloc_init(&s600_can_tx_allocator, s600_can_tx_alloc_buff, sizeof(s600_can_tx_alloc_buff));
|
|
|
|
|
+ byte_queue_alloc_init(&s600_can_rx_allocator, s600_can_rx_alloc_buff, sizeof(s600_can_rx_alloc_buff));
|
|
|
|
|
+
|
|
|
|
|
+#if CONFIG_CAN_PIN_MAP == CAN_PIN_MAP_PA11_PA12
|
|
|
|
|
+ rcu_periph_clock_enable(RCU_GPIOA);
|
|
|
|
|
+#elif CONFIG_CAN_PIN_MAP == CAN_PIN_MAP_PB8_PB9
|
|
|
|
|
+ rcu_periph_clock_enable(RCU_GPIOB);
|
|
|
|
|
+#elif CONFIG_CAN_PIN_MAP == CAN_PIN_MAP_PD0_PD1
|
|
|
|
|
+ rcu_periph_clock_enable(RCU_GPIOD);
|
|
|
|
|
+#else
|
|
|
|
|
+#error "Invalid CONFIG_CAN_PIN_MAP"
|
|
|
|
|
+#endif
|
|
|
|
|
+
|
|
|
|
|
+ rcu_periph_clock_enable(RCU_AF);
|
|
|
|
|
+ rcu_periph_clock_enable(RCU_CAN0);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+__STATIC_INLINE void s600_can_gpio_config(void)
|
|
|
|
|
+{
|
|
|
|
|
+#if CONFIG_CAN_PIN_MAP == CAN_PIN_MAP_PA11_PA12
|
|
|
|
|
+ gpio_init(GPIOA, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, GPIO_PIN_11);
|
|
|
|
|
+ gpio_init(GPIOA, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_12);
|
|
|
|
|
+#elif CONFIG_CAN_PIN_MAP == CAN_PIN_MAP_PB8_PB9
|
|
|
|
|
+ gpio_init(GPIOB, GPIO_MODE_IPU, GPIO_OSPEED_50MHZ, GPIO_PIN_8);
|
|
|
|
|
+ gpio_init(GPIOB, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_9);
|
|
|
|
|
+#ifdef GPIO_CAN0_PARTIAL_REMAP
|
|
|
|
|
+ gpio_pin_remap_config(GPIO_CAN0_PARTIAL_REMAP, ENABLE);
|
|
|
|
|
+#else
|
|
|
|
|
+ gpio_pin_remap_config(GPIO_CAN_PARTIAL_REMAP, ENABLE);
|
|
|
|
|
+#endif
|
|
|
|
|
+#elif CONFIG_CAN_PIN_MAP == CAN_PIN_MAP_PD0_PD1
|
|
|
|
|
+ gpio_init(GPIOD, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_0);
|
|
|
|
|
+ gpio_init(GPIOD, GPIO_MODE_AF_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_1);
|
|
|
|
|
+ gpio_pin_remap_config(GPIO_CAN_FULL_REMAP, ENABLE);
|
|
|
|
|
+#else
|
|
|
|
|
+#error "Invalid CONFIG_CAN_PIN_MAP"
|
|
|
|
|
+#endif
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void s600_can_device_config(void)
|
|
|
|
|
+{
|
|
|
|
|
+ can_parameter_struct can_parameter;
|
|
|
|
|
+ can_filter_parameter_struct can_filter;
|
|
|
|
|
+
|
|
|
|
|
+ s600_can_rcu_config();
|
|
|
|
|
+ s600_can_gpio_config();
|
|
|
|
|
+
|
|
|
|
|
+ // can_struct_para_init(CAN_INIT_STRUCT, &can_parameter);
|
|
|
|
|
+ // can_struct_para_init(CAN_FILTER_STRUCT, &can_filter);
|
|
|
|
|
+
|
|
|
|
|
+ can_deinit(CAN0);
|
|
|
|
|
+
|
|
|
|
|
+ /* initialize CAN parameters */
|
|
|
|
|
+ can_parameter.time_triggered = DISABLE;
|
|
|
|
|
+ can_parameter.auto_bus_off_recovery = DISABLE;
|
|
|
|
|
+ can_parameter.auto_wake_up = DISABLE;
|
|
|
|
|
+ can_parameter.auto_retrans = DISABLE;
|
|
|
|
|
+ can_parameter.rec_fifo_overwrite = DISABLE;
|
|
|
|
|
+ can_parameter.trans_fifo_order = ENABLE;
|
|
|
|
|
+ can_parameter.resync_jump_width = CAN_BT_SJW_1TQ;
|
|
|
|
|
+ can_parameter.time_segment_1 = CAN_BT_BS1_5TQ;
|
|
|
|
|
+ can_parameter.time_segment_2 = CAN_BT_BS2_3TQ;
|
|
|
|
|
+
|
|
|
|
|
+#if S600_CAN_LOOPBACK
|
|
|
|
|
+ can_parameter.working_mode = CAN_LOOPBACK_MODE;
|
|
|
|
|
+#else
|
|
|
|
|
+ can_parameter.working_mode = CAN_NORMAL_MODE;
|
|
|
|
|
+#endif
|
|
|
|
|
+
|
|
|
|
|
+#if S600_CAN_BAUDRATE == 1000
|
|
|
|
|
+ can_parameter.prescaler = 6;
|
|
|
|
|
+#elif S600_CAN_BAUDRATE == 500
|
|
|
|
|
+ can_parameter.prescaler = 12;
|
|
|
|
|
+#elif S600_CAN_BAUDRATE == 250
|
|
|
|
|
+ can_parameter.prescaler = 24;
|
|
|
|
|
+#elif S600_CAN_BAUDRATE == 125
|
|
|
|
|
+ can_parameter.prescaler = 48;
|
|
|
|
|
+#elif S600_CAN_BAUDRATE == 100
|
|
|
|
|
+ can_parameter.prescaler = 60;
|
|
|
|
|
+#elif S600_CAN_BAUDRATE == 50
|
|
|
|
|
+ can_parameter.prescaler = 120;
|
|
|
|
|
+#elif S600_CAN_BAUDRATE == 20
|
|
|
|
|
+ can_parameter.prescaler = 300;
|
|
|
|
|
+#else
|
|
|
|
|
+ #error "Invalid Baudrate"
|
|
|
|
|
+#endif
|
|
|
|
|
+
|
|
|
|
|
+ /* initialize CAN */
|
|
|
|
|
+ can_init(CAN0, &can_parameter);
|
|
|
|
|
+
|
|
|
|
|
+#ifdef CONFIG_GD32E10X
|
|
|
|
|
+ can_frequency_set(CAN0, S600_CAN_BAUDRATE * 1000);
|
|
|
|
|
+#endif
|
|
|
|
|
+
|
|
|
|
|
+ /* initialize filter */
|
|
|
|
|
+ can_filter.filter_number = 0;
|
|
|
|
|
+ can_filter.filter_mode = CAN_FILTERMODE_MASK;
|
|
|
|
|
+ can_filter.filter_bits = CAN_FILTERBITS_32BIT;
|
|
|
|
|
+ can_filter.filter_list_high = 0x0000;
|
|
|
|
|
+ can_filter.filter_list_low = S600_CAN_DEST_ID(CONFIG_CAN_ID) << 3 | 1 << 2;
|
|
|
|
|
+ can_filter.filter_mask_high = 0x0000;
|
|
|
|
|
+ can_filter.filter_mask_low = S600_CAN_DEST_ID(S600_CAN_ALL_ID) << 3 | 1 << 2;
|
|
|
|
|
+ can_filter.filter_fifo_number = CAN_FIFO1;
|
|
|
|
|
+ can_filter.filter_enable = ENABLE;
|
|
|
|
|
+ can_filter_init(&can_filter);
|
|
|
|
|
+
|
|
|
|
|
+ nvic_irq_enable(CAN0_RX1_IRQn, 0, 0);
|
|
|
|
|
+ can_interrupt_enable(CAN0, CAN_INTEN_RFNEIE1);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void s600_can_device_deinit(void)
|
|
|
|
|
+{
|
|
|
|
|
+ nvic_irq_disable(CAN0_RX1_IRQn);
|
|
|
|
|
+ can_deinit(CAN0);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+u8 s600_can_find_mailbox(void)
|
|
|
|
|
+{
|
|
|
|
|
+ u32 value = CAN_TSTAT(CAN0);
|
|
|
|
|
+
|
|
|
|
|
+ if ((value & CAN_TSTAT_TME0) != 0) {
|
|
|
|
|
+ return CAN_MAILBOX0;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ if ((value & CAN_TSTAT_TME1) != 0) {
|
|
|
|
|
+ return CAN_MAILBOX1;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ if ((value & CAN_TSTAT_TME2) != 0) {
|
|
|
|
|
+ return CAN_MAILBOX2;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ return CAN_NOMAILBOX;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+u8 s600_can_wait_mailbox(void)
|
|
|
|
|
+{
|
|
|
|
|
+ u32 count;
|
|
|
|
|
+
|
|
|
|
|
+ for (count = 0; count < 1000000; count++) {
|
|
|
|
|
+ u8 mailbox = s600_can_find_mailbox();
|
|
|
|
|
+
|
|
|
|
|
+ if (mailbox != CAN_NOMAILBOX) {
|
|
|
|
|
+ return mailbox;
|
|
|
|
|
+ }
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ return CAN_NOMAILBOX;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void s600_can_send_frame(u8 mailbox, u32 efid, const u8 *buff, u8 length)
|
|
|
|
|
+{
|
|
|
|
|
+ /* set the data length */
|
|
|
|
|
+ CAN_TMP(CAN0, mailbox) &= ~CAN_TMP_DLENC;
|
|
|
|
|
+ CAN_TMP(CAN0, mailbox) |= length;
|
|
|
|
|
+
|
|
|
|
|
+ /* set the data */
|
|
|
|
|
+ CAN_TMDATA0(CAN0, mailbox) = ((const u32 *) buff)[0];
|
|
|
|
|
+
|
|
|
|
|
+ if (length > 4) {
|
|
|
|
|
+ CAN_TMDATA1(CAN0, mailbox) = ((const u32 *) buff)[1];
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ /* enable transmission */
|
|
|
|
|
+ CAN_TMI(CAN0, mailbox) = TMI_EFID(efid) | CAN_FF_EXTENDED | CAN_FT_DATA | CAN_TMI_TEN;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+bool s600_can_tx_packet(s600_can_packet_t *packet)
|
|
|
|
|
+{
|
|
|
|
|
+ u8 total, index;
|
|
|
|
|
+ u8 mailbox;
|
|
|
|
|
+ u16 length;
|
|
|
|
|
+ u32 efid;
|
|
|
|
|
+
|
|
|
|
|
+ mailbox = s600_can_find_mailbox();
|
|
|
|
|
+ if (mailbox == CAN_NOMAILBOX) {
|
|
|
|
|
+ return false;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ total = (packet->length + 7) / 8;
|
|
|
|
|
+ index = packet->offset / 8 + 1;
|
|
|
|
|
+
|
|
|
|
|
+ efid = 6 << 26 | U32(packet->type) << 24
|
|
|
|
|
+ | U32(total & 0x1F) << 19 | U32(index & 0x1F) << 14
|
|
|
|
|
+ | S600_CAN_DEST_ID(packet->dest) | S600_CAN_SRC_ID(packet->src);
|
|
|
|
|
+
|
|
|
|
|
+ length = packet->length - packet->offset;
|
|
|
|
|
+ if (length > 8) {
|
|
|
|
|
+ length = 8;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ s600_can_send_frame(mailbox, efid, packet->data + packet->offset, length);
|
|
|
|
|
+ packet->offset += length;
|
|
|
|
|
+
|
|
|
|
|
+ return true;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+__STATIC_INLINE s600_can_packet_t *s600_can_rx_front_get(u8 addr)
|
|
|
|
|
+{
|
|
|
|
|
+ u8 index;
|
|
|
|
|
+
|
|
|
|
|
+ for (index = 0; index < s600_can_rx_front_len; index++) {
|
|
|
|
|
+ s600_can_packet_t *packet = s600_can_rx_fronts[index];
|
|
|
|
|
+
|
|
|
|
|
+ if (packet->src == addr) {
|
|
|
|
|
+ return packet;
|
|
|
|
|
+ }
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ if (s600_can_rx_front_len < NELEM(s600_can_rx_fronts)) {
|
|
|
|
|
+ u8 index = byte_queue_alloc(&s600_can_rx_allocator);
|
|
|
|
|
+ s600_can_packet_t *packet;
|
|
|
|
|
+
|
|
|
|
|
+ if (index == 0xFF) {
|
|
|
|
|
+ return NULL;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ packet = s600_can_rx_packets + index;
|
|
|
|
|
+ packet->src = addr;
|
|
|
|
|
+
|
|
|
|
|
+#if S600_CAN_USE_MASK
|
|
|
|
|
+ packet->mask = 0;
|
|
|
|
|
+#else
|
|
|
|
|
+ packet->index = 0xFF;
|
|
|
|
|
+#endif
|
|
|
|
|
+
|
|
|
|
|
+ s600_can_rx_fronts[s600_can_rx_front_len] = packet;
|
|
|
|
|
+ s600_can_rx_front_len++;
|
|
|
|
|
+ return packet;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ return NULL;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+__STATIC_INLINE bool s600_can_rx_front_put(s600_can_packet_t *packet)
|
|
|
|
|
+{
|
|
|
|
|
+ u8 index;
|
|
|
|
|
+
|
|
|
|
|
+ for (index = 0; index < s600_can_rx_front_len; index++) {
|
|
|
|
|
+ if (s600_can_rx_fronts[index] == packet) {
|
|
|
|
|
+ s600_can_rx_front_len--;
|
|
|
|
|
+
|
|
|
|
|
+ while (index < s600_can_rx_front_len) {
|
|
|
|
|
+ s600_can_rx_fronts[index] = s600_can_rx_fronts[index + 1];
|
|
|
|
|
+ index++;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ return true;
|
|
|
|
|
+ }
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ return false;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+bool s600_can_rx_packet(u8 fifo)
|
|
|
|
|
+{
|
|
|
|
|
+ s600_can_packet_t *packet;
|
|
|
|
|
+ u8 index, total;
|
|
|
|
|
+ u32 *buff;
|
|
|
|
|
+ u32 value;
|
|
|
|
|
+ u32 efid;
|
|
|
|
|
+
|
|
|
|
|
+ value = CAN_RFIFOMI(CAN0, fifo);
|
|
|
|
|
+ if ((value & (CAN_RFIFOMI_FT | CAN_RFIFOMI_FF)) != (CAN_FT_DATA | CAN_FF_EXTENDED)) {
|
|
|
|
|
+ return true;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ efid = GET_RFIFOMI_EFID(value);
|
|
|
|
|
+
|
|
|
|
|
+ value = CAN_RFIFOMP(CAN0, fifo);
|
|
|
|
|
+
|
|
|
|
|
+#ifdef CAN_RFIFOMP_FDF
|
|
|
|
|
+ if ((value & CAN_RFIFOMP_FDF) != CAN_FDF_CLASSIC) {
|
|
|
|
|
+ return true;
|
|
|
|
|
+ }
|
|
|
|
|
+#endif
|
|
|
|
|
+
|
|
|
|
|
+ packet = s600_can_rx_front_get((efid >> 7) & 0x7F);
|
|
|
|
|
+ if (packet == NULL) {
|
|
|
|
|
+ return false;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ index = (efid >> 14) & 0x1F;
|
|
|
|
|
+ total = (efid >> 19) & 0x1F;
|
|
|
|
|
+
|
|
|
|
|
+#if S600_CAN_USE_MASK
|
|
|
|
|
+ if (index == total) {
|
|
|
|
|
+ packet->dest = efid & 0x7F;
|
|
|
|
|
+ packet->type = (efid >> 24) & 0x03;
|
|
|
|
|
+ packet->length = ((total - 1) & 0x1F) << 3;
|
|
|
|
|
+ packet->length += GET_RFIFOMP_DLENC(value);
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ index = (index - 1) & 0x1F;
|
|
|
|
|
+
|
|
|
|
|
+ if (index == 0) {
|
|
|
|
|
+ packet->mask = 1;
|
|
|
|
|
+ } else {
|
|
|
|
|
+ packet->mask |= 1 << index;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ buff = (u32 *) packet->datas[index];
|
|
|
|
|
+ buff[0] = CAN_RFIFOMDATA0(CAN0, fifo);
|
|
|
|
|
+ buff[1] = CAN_RFIFOMDATA1(CAN0, fifo);
|
|
|
|
|
+
|
|
|
|
|
+ if (total == 0) {
|
|
|
|
|
+ value = 0xFFFFFFFF;
|
|
|
|
|
+ } else {
|
|
|
|
|
+ value = (1 << total) - 1;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ if ((packet->mask & value) == value) {
|
|
|
|
|
+ byte_queue_write_byte(&s600_can_rx_queue, packet - s600_can_rx_packets);
|
|
|
|
|
+ s600_can_rx_front_put(packet);
|
|
|
|
|
+ }
|
|
|
|
|
+#else
|
|
|
|
|
+ if (index != packet->index) {
|
|
|
|
|
+ if (index != 1) {
|
|
|
|
|
+ return true;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ packet->type = (efid >> 24) & 0x03;
|
|
|
|
|
+ packet->dest = efid & 0x7F;
|
|
|
|
|
+ packet->offset = 0;
|
|
|
|
|
+ packet->index = 2;
|
|
|
|
|
+ } else {
|
|
|
|
|
+ packet->index = (index + 1) & 0x1F;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ buff = (u32 *) (packet->data + packet->offset);
|
|
|
|
|
+ packet->offset += GET_RFIFOMP_DLENC(value);
|
|
|
|
|
+ buff[0] = CAN_RFIFOMDATA0(CAN0, fifo);
|
|
|
|
|
+ buff[1] = CAN_RFIFOMDATA1(CAN0, fifo);
|
|
|
|
|
+
|
|
|
|
|
+ if (total == index) {
|
|
|
|
|
+ packet->length = packet->offset;
|
|
|
|
|
+ byte_queue_write_byte(&s600_can_rx_queue, packet - s600_can_rx_packets);
|
|
|
|
|
+ s600_can_rx_front_put(packet);
|
|
|
|
|
+ }
|
|
|
|
|
+#endif
|
|
|
|
|
+
|
|
|
|
|
+ return true;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void s600_can_transmit(void)
|
|
|
|
|
+{
|
|
|
|
|
+ while (1) {
|
|
|
|
|
+ u8 index;
|
|
|
|
|
+
|
|
|
|
|
+ if (s600_can_tx_front) {
|
|
|
|
|
+ if (s600_can_tx_front->offset < s600_can_tx_front->length) {
|
|
|
|
|
+ s600_can_tx_packet(s600_can_tx_front);
|
|
|
|
|
+ break;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ byte_queue_free(&s600_can_tx_allocator, s600_can_tx_front - s600_can_tx_packets);
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ if (byte_queue_read(&s600_can_tx_queue, &index, 1) > 0) {
|
|
|
|
|
+ s600_can_tx_front = s600_can_tx_packets + index;
|
|
|
|
|
+ } else {
|
|
|
|
|
+ s600_can_tx_front = NULL;
|
|
|
|
|
+ break;
|
|
|
|
|
+ }
|
|
|
|
|
+ }
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+__STATIC_INLINE void s600_can_process_packet(s600_can_packet_t *packet)
|
|
|
|
|
+{
|
|
|
|
|
+ s600_can_process_command(packet->src, packet->data, packet->length);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void s600_can_poll(void)
|
|
|
|
|
+{
|
|
|
|
|
+ u8 index;
|
|
|
|
|
+
|
|
|
|
|
+ s600_can_transmit();
|
|
|
|
|
+
|
|
|
|
|
+ while (byte_queue_read(&s600_can_rx_queue, &index, 1) > 0) {
|
|
|
|
|
+ s600_can_process_packet(s600_can_rx_packets + index);
|
|
|
|
|
+ byte_queue_free(&s600_can_rx_allocator, index);
|
|
|
|
|
+ }
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+bool s600_can_send_packet(u8 dest, u8 src, u8 type, const void *data, u16 length)
|
|
|
|
|
+{
|
|
|
|
|
+ s600_can_packet_t *packet;
|
|
|
|
|
+ u8 index;
|
|
|
|
|
+
|
|
|
|
|
+ index = byte_queue_alloc(&s600_can_tx_allocator);
|
|
|
|
|
+ if (index == 0xFF) {
|
|
|
|
|
+ return false;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ packet = s600_can_tx_packets + index;
|
|
|
|
|
+
|
|
|
|
|
+ packet->offset = 0;
|
|
|
|
|
+ packet->type = type;
|
|
|
|
|
+ packet->dest = dest;
|
|
|
|
|
+ packet->src = src;
|
|
|
|
|
+ packet->length = length;
|
|
|
|
|
+ memcpy(packet->data, data, length);
|
|
|
|
|
+
|
|
|
|
|
+ byte_queue_write_byte(&s600_can_tx_queue, index);
|
|
|
|
|
+ s600_can_transmit();
|
|
|
|
|
+
|
|
|
|
|
+ return true;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void s600_can_send_packet_block(u8 dest, u8 src, u8 type, const void *data, u16 length)
|
|
|
|
|
+{
|
|
|
|
|
+ while (!s600_can_send_packet(dest, src, type, data, length)) {
|
|
|
|
|
+ s600_can_transmit();
|
|
|
|
|
+ }
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+bool s600_can_send_command(u8 addr, const void *command, u16 length)
|
|
|
|
|
+{
|
|
|
|
|
+ return s600_can_send_packet(addr, CONFIG_CAN_ID, 1, command, length);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+bool s600_can_send_response(u8 addr, const void *response, u16 length)
|
|
|
|
|
+{
|
|
|
|
|
+ return s600_can_send_packet(addr, CONFIG_CAN_ID, 2, response, length);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+bool s600_can_send_event(u8 addr, const void *event, u16 length)
|
|
|
|
|
+{
|
|
|
|
|
+ return s600_can_send_packet(addr, CONFIG_CAN_ID, 3, event, length);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+// ================================================================================
|
|
|
|
|
+
|
|
|
|
|
+void CAN0_RX1_IRQHandler(void)
|
|
|
|
|
+{
|
|
|
|
|
+ s600_can_rx_packet(CAN_FIFO1);
|
|
|
|
|
+ CAN_RFIFO1(CAN0) |= CAN_RFIFO1_RFD1;
|
|
|
|
|
+}
|