From cfb6b67fbbcce558185f68d9b17e3455a60440ea Mon Sep 17 00:00:00 2001 From: ReFil <31960031+ReFil@users.noreply.github.com> Date: Fri, 17 Nov 2023 10:36:18 +0000 Subject: [PATCH] Add smart roadster interceptor --- board/board.h | 8 + board/boards/kombi.h | 125 ++++++++++ board/kombi/.gitignore | 1 + board/kombi/Makefile | 81 ++++++ board/kombi/can.h | 369 ++++++++++++++++++++++++++++ board/kombi/main.c | 422 ++++++++++++++++++++++++++++++++ board/kombi/main_declarations.h | 17 ++ 7 files changed, 1023 insertions(+) create mode 100644 board/boards/kombi.h create mode 100644 board/kombi/.gitignore create mode 100644 board/kombi/Makefile create mode 100644 board/kombi/can.h create mode 100644 board/kombi/main.c create mode 100644 board/kombi/main_declarations.h diff --git a/board/board.h b/board/board.h index f80ac3a39b..9f4dea98aa 100644 --- a/board/board.h +++ b/board/board.h @@ -22,6 +22,9 @@ #ifdef IBST #include "boards/ibst.h" #endif +#ifdef KOMBI + #include "boards/kombi.h" +#endif void detect_board_type(void) { #ifdef PANDA @@ -56,6 +59,10 @@ void detect_board_type(void) { #ifdef IBST hw_type = HW_TYPE_IBST; current_board = &board_ibst; + #else + #ifdef KOMBI + hw_type = HW_TYPE_KOMBI; + current_board = &board_kombi; #else hw_type = HW_TYPE_UNKNOWN; puts("Hardware type is UNKNOWN!\n"); @@ -63,6 +70,7 @@ void detect_board_type(void) { #endif #endif #endif + #endif } diff --git a/board/boards/kombi.h b/board/boards/kombi.h new file mode 100644 index 0000000000..a6fc9703a8 --- /dev/null +++ b/board/boards/kombi.h @@ -0,0 +1,125 @@ +// /////////// // +// KOMBI gateway // +// /////////// // + +void kombi_enable_can_transceiver(uint8_t transceiver, bool enabled) { + switch (transceiver){ + case 1U: + set_gpio_output(GPIOC, 1, !enabled); + break; + case 2U: + set_gpio_output(GPIOC, 13, !enabled); + break; + case 3U: + set_gpio_output(GPIOA, 0, !enabled); + break; + default: + puts("Invalid CAN transceiver ("); puth(transceiver); puts("): enabling failed\n"); + break; + } +} + +void kombi_enable_can_transceivers(bool enabled) { + uint8_t t1 = enabled ? 1U : 2U; // leave transceiver 1 enabled to detect CAN ignition + for(uint8_t i=t1; i<=3U; i++) { + kombi_enable_can_transceiver(i, enabled); + } +} + +void kombi_set_led(uint8_t color, bool enabled) { + UNUSED(color); + UNUSED(enabled); +} + +void kombi_set_usb_power_mode(uint8_t mode){ + UNUSED(mode); +} + +void kombi_set_gps_mode(uint8_t mode) { + UNUSED(mode); +} + +void kombi_set_can_mode(uint8_t mode){ + switch (mode) { + case CAN_MODE_NORMAL: + set_gpio_alternate(GPIOB, 8, GPIO_AF8_CAN1); + set_gpio_alternate(GPIOB, 9, GPIO_AF8_CAN1); + // B5,B6: normal CAN2 mode + set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2); + set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2); + + // A8,A15: normal CAN3 mode + set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3); + set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3); + break; + default: + puts("Tried to set unsupported CAN mode: "); puth(mode); puts("\n"); + break; + } +} + +uint32_t kombi_read_current(void){ + return 0U; +} + +void kombi_usb_power_mode_tick(uint32_t uptime){ + UNUSED(uptime); +} + +void kombi_set_ir_power(uint8_t percentage){ + UNUSED(percentage); +} + +void kombi_set_fan_power(uint8_t percentage){ + UNUSED(percentage); +} + +bool kombi_check_ignition(void){ + // ignition is on PA1 + return 0U; +} + +void kombi_set_phone_power(bool enabled){ + UNUSED(enabled); +} + +void kombi_set_clock_source_mode(uint8_t mode){ + UNUSED(mode); +} + +void kombi_set_siren(bool enabled){ + UNUSED(enabled); +} + +void kombi_init(void) { + common_init_gpio(); + + // Enable CAN transceivers + kombi_enable_can_transceivers(true); + // Set normal CAN mode + kombi_set_can_mode(CAN_MODE_NORMAL); +} + +const harness_configuration kombi_harness_config = { + .has_harness = false +}; + +const board board_kombi = { + .board_type = "White", + .harness_config = &kombi_harness_config, + .init = kombi_init, + .enable_can_transceiver = kombi_enable_can_transceiver, + .enable_can_transceivers = kombi_enable_can_transceivers, + .set_led = kombi_set_led, + .set_usb_power_mode = kombi_set_usb_power_mode, + .set_gps_mode = kombi_set_gps_mode, + .set_can_mode = kombi_set_can_mode, + .usb_power_mode_tick = kombi_usb_power_mode_tick, + .check_ignition = kombi_check_ignition, + .read_current = kombi_read_current, + .set_fan_power = kombi_set_fan_power, + .set_ir_power = kombi_set_ir_power, + .set_phone_power = kombi_set_phone_power, + .set_clock_source_mode = kombi_set_clock_source_mode, + .set_siren = kombi_set_siren +}; diff --git a/board/kombi/.gitignore b/board/kombi/.gitignore new file mode 100644 index 0000000000..94053f2925 --- /dev/null +++ b/board/kombi/.gitignore @@ -0,0 +1 @@ +obj/* diff --git a/board/kombi/Makefile b/board/kombi/Makefile new file mode 100644 index 0000000000..f46e01497c --- /dev/null +++ b/board/kombi/Makefile @@ -0,0 +1,81 @@ +PROJ_NAME = panda +CFLAGS = -g -Wall -Wextra -Wstrict-prototypes -Werror + +CFLAGS += -mlittle-endian -mthumb -mcpu=cortex-m4 +CFLAGS += -mhard-float -DSTM32F4 -DSTM32F413xx -mfpu=fpv4-sp-d16 -fsingle-precision-constant -DIBST +STARTUP_FILE = startup_stm32f413xx + +CFLAGS += -I ../inc -I ../ -I ../../ -nostdlib -fno-builtin -std=gnu11 -Os + +CFLAGS += -T../stm32_flash.ld + +DFU_UTIL = "dfu-util" + +#COMPILER_PATH = /home/batman/Downloads/gcc-arm-none-eabi-9-2020-q2-update/bin/ +CC = $(COMPILER_PATH)arm-none-eabi-gcc +OBJCOPY = $(COMPILER_PATH)arm-none-eabi-objcopy +OBJDUMP = $(COMPILER_PATH)arm-none-eabi-objdump + +CERT = ../../certs/debug +CFLAGS += "-DALLOW_DEBUG" + + + +DEPDIR = ../generated_dependencies +$(shell mkdir -p -m 777 $(DEPDIR) >/dev/null) +DEPFLAGS = -MT $@ -MMD -MP -MF $(DEPDIR)/$*.Td +POSTCOMPILE = @mv -f $(DEPDIR)/$*.Td $(DEPDIR)/$*.d && touch $@ + +# this no longer pushes the bootstub +canflash: obj/$(PROJ_NAME).bin + ../../tests/ctrls/enter_canloader.py $< + +usbflash: obj/$(PROJ_NAME).bin + ../../tests/ibst/enter_canloader.py; sleep 0.5 + PYTHONPATH=../../ python -c "from python import Panda; p = [x for x in [Panda(x) for x in Panda.list()] if x.bootstub]; assert(len(p)==1); p[0].flash('obj/$(PROJ_NAME).bin', reconnect=False)" + +recover: obj/bootstub.bin obj/$(PROJ_NAME).bin + ../tools/enter_download_mode.py $< + sleep 1.0 + $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08004000 -D obj/$(PROJ_NAME).bin + $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.bin + +recover_can: obj/bootstub.bin obj/$(PROJ_NAME).bin + ../../tests/ibst/enter_canloader.py --recover; sleep 0.5 + $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08004000 -D obj/$(PROJ_NAME).bin + $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.bin + +include ../../common/version.mk + +obj/cert.h: ../../crypto/getcertheader.py + ../../crypto/getcertheader.py ../../certs/debug.pub ../../certs/release.pub > $@ + +obj/main.o: main.c ../*.h + mkdir -p obj + $(CC) $(CFLAGS) -o $@ -c $< + +obj/bootstub.o: ../bootstub.c ../*.h obj/gitversion.h obj/cert.h + mkdir -p obj + mkdir -p ../obj + cp obj/gitversion.h ../obj/gitversion.h + cp obj/cert.h ../obj/cert.h + $(CC) $(CFLAGS) -o $@ -c $< + +obj/$(STARTUP_FILE).o: ../$(STARTUP_FILE).s + $(CC) $(CFLAGS) -o $@ -c $< + +obj/%.o: ../../crypto/%.c + $(CC) $(CFLAGS) -o $@ -c $< + +obj/$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/main.o + # hack + $(CC) -Wl,--section-start,.isr_vector=0x8004000 $(CFLAGS) -o obj/$(PROJ_NAME).elf $^ + $(OBJCOPY) -v -O binary obj/$(PROJ_NAME).elf obj/code.bin + SETLEN=1 ../../crypto/sign.py obj/code.bin $@ $(CERT) + +obj/bootstub.bin: obj/$(STARTUP_FILE).o obj/bootstub.o obj/sha.o obj/rsa.o + $(CC) $(CFLAGS) -o obj/bootstub.$(PROJ_NAME).elf $^ + $(OBJCOPY) -v -O binary obj/bootstub.$(PROJ_NAME).elf $@ + +clean: + rm -f obj/* diff --git a/board/kombi/can.h b/board/kombi/can.h new file mode 100644 index 0000000000..43a19985f6 --- /dev/null +++ b/board/kombi/can.h @@ -0,0 +1,369 @@ +// cut down version of the can.h driver. allows pedal-like devices to work like Pandas + +// IRQs: CAN1_TX, CAN1_RX0, CAN1_SCE +// CAN2_TX, CAN2_RX0, CAN2_SCE +// CAN3_TX, CAN3_RX0, CAN3_SCE + +typedef struct { + volatile uint32_t w_ptr; + volatile uint32_t r_ptr; + uint32_t fifo_size; + CAN_FIFOMailBox_TypeDef *elems; +} can_ring; + +#define CAN_BUS_RET_FLAG 0x80U +#define CAN_BUS_NUM_MASK 0x7FU + +#define BUS_MAX 4U + +uint32_t can_rx_errs = 0; +uint32_t can_send_errs = 0; +uint32_t can_fwd_errs = 0; +extern int can_live, pending_can_live; + +// must reinit after changing these +extern int can_loopback, can_silent; +extern uint32_t can_speed[4]; + +void can_set_forwarding(int from, int to); + +bool can_init(uint8_t can_number); +void can_init_all(void); +bool can_tx_check_min_slots_free(uint32_t min); +void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook); +bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem); + +// Ignition detected from CAN meessages +bool ignition_can = false; +bool ignition_cadillac = false; +uint32_t ignition_can_cnt = 0U; + +// end API + +#define ALL_CAN_SILENT 0xFF +#define ALL_CAN_LIVE 0 + +int can_live = 0, pending_can_live = 0, can_loopback = 0, can_silent = ALL_CAN_SILENT; + +// ********************* instantiate queues ********************* + +#define can_buffer(x, size) \ + CAN_FIFOMailBox_TypeDef elems_##x[size]; \ + can_ring can_##x = { .w_ptr = 0, .r_ptr = 0, .fifo_size = (size), .elems = ((CAN_FIFOMailBox_TypeDef *)&elems_##x) }; + +can_buffer(rx_q, 0x1000) +can_buffer(tx1_q, 0x100) +can_buffer(tx2_q, 0x100) +can_buffer(tx3_q, 0x100) +can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q}; + +// global CAN stats +int can_rx_cnt = 0; +int can_tx_cnt = 0; +int can_txd_cnt = 0; +int can_err_cnt = 0; +int can_overflow_cnt = 0; + +// ********************* interrupt safe queue ********************* + +bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) { + bool ret = 0; + + ENTER_CRITICAL(); + if (q->w_ptr != q->r_ptr) { + *elem = q->elems[q->r_ptr]; + if ((q->r_ptr + 1U) == q->fifo_size) { + q->r_ptr = 0; + } else { + q->r_ptr += 1U; + } + ret = 1; + } + EXIT_CRITICAL(); + + return ret; +} + +bool can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) { + bool ret = false; + uint32_t next_w_ptr; + + ENTER_CRITICAL(); + if ((q->w_ptr + 1U) == q->fifo_size) { + next_w_ptr = 0; + } else { + next_w_ptr = q->w_ptr + 1U; + } + if (next_w_ptr != q->r_ptr) { + q->elems[q->w_ptr] = *elem; + q->w_ptr = next_w_ptr; + ret = true; + } + EXIT_CRITICAL(); + if (!ret) { + can_overflow_cnt++; + #ifdef DEBUG_CAN + puts("can_push failed!\n"); + #endif + } + return ret; +} + +uint32_t can_slots_empty(can_ring *q) { + uint32_t ret = 0; + + ENTER_CRITICAL(); + if (q->w_ptr >= q->r_ptr) { + ret = q->fifo_size - 1U - q->w_ptr + q->r_ptr; + } else { + ret = q->r_ptr - q->w_ptr - 1U; + } + EXIT_CRITICAL(); + + return ret; +} + +void can_clear(can_ring *q) { + ENTER_CRITICAL(); + q->w_ptr = 0; + q->r_ptr = 0; + EXIT_CRITICAL(); +} + +// assign CAN numbering +// bus num: Can bus number on ODB connector. Sent to/from USB +// Min: 0; Max: 127; Bit 7 marks message as receipt (bus 129 is receipt for but 1) +// cans: Look up MCU can interface from bus number +// can number: numeric lookup for MCU CAN interfaces (0 = CAN1, 1 = CAN2, etc); +// bus_lookup: Translates from 'can number' to 'bus number'. +// can_num_lookup: Translates from 'bus number' to 'can number'. +// can_forwarding: Given a bus num, lookup bus num to forward to. -1 means no forward. + +// Panda: Bus 0=CAN1 Bus 1=CAN2 Bus 2=CAN3 +CAN_TypeDef *cans[] = {CAN1, CAN2, CAN3}; +uint8_t bus_lookup[] = {0,1,2}; +uint8_t can_num_lookup[] = {0,1,2,-1}; +int8_t can_forwarding[] = {-1,-1,-1,-1}; +uint32_t can_speed[] = {5000, 5000, 5000, 333}; +#define CAN_MAX 3U + +#define CANIF_FROM_CAN_NUM(num) (cans[num]) +#define CAN_NUM_FROM_CANIF(CAN) ((CAN)==CAN1 ? 0 : ((CAN) == CAN2 ? 1 : 2)) +#define BUS_NUM_FROM_CAN_NUM(num) (bus_lookup[num]) +#define CAN_NUM_FROM_BUS_NUM(num) (can_num_lookup[num]) + +void process_can(uint8_t can_number); + +bool can_set_speed(uint8_t can_number) { + bool ret = true; + CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); + uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); + + ret &= llcan_set_speed(CAN, can_speed[bus_number], can_loopback, (unsigned int)(can_silent) & (1U << can_number)); + return ret; +} + +void can_init_all(void) { + bool ret = true; + for (uint8_t i=0U; i < CAN_MAX; i++) { + can_clear(can_queues[i]); + ret &= can_init(i); + } + UNUSED(ret); +} + +void can_flip_buses(uint8_t bus1, uint8_t bus2){ + bus_lookup[bus1] = bus2; + bus_lookup[bus2] = bus1; + can_num_lookup[bus1] = bus2; + can_num_lookup[bus2] = bus1; +} + + +// CAN error +void can_sce(CAN_TypeDef *CAN) { + ENTER_CRITICAL(); + + #ifdef DEBUG_CAN + if (CAN==CAN1) puts("CAN1: "); + if (CAN==CAN2) puts("CAN2: "); + #ifdef CAN3 + if (CAN==CAN3) puts("CAN3: "); + #endif + puts("MSR:"); + puth(CAN->MSR); + puts(" TSR:"); + puth(CAN->TSR); + puts(" RF0R:"); + puth(CAN->RF0R); + puts(" RF1R:"); + puth(CAN->RF1R); + puts(" ESR:"); + puth(CAN->ESR); + puts("\n"); + #endif + + can_err_cnt += 1; + llcan_clear_send(CAN); + EXIT_CRITICAL(); +} + +// ***************************** CAN ***************************** + +void process_can(uint8_t can_number) { + if (can_number != 0xffU) { + + ENTER_CRITICAL(); + + CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); + uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); + + // check for empty mailbox + CAN_FIFOMailBox_TypeDef to_send; + if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) { + // add successfully transmitted message to my fifo + if ((CAN->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) { + can_txd_cnt += 1; + + if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) { + CAN_FIFOMailBox_TypeDef to_push; + to_push.RIR = CAN->sTxMailBox[0].TIR; + to_push.RDTR = (CAN->sTxMailBox[0].TDTR & 0xFFFF000FU) | ((CAN_BUS_RET_FLAG | bus_number) << 4); + to_push.RDLR = CAN->sTxMailBox[0].TDLR; + to_push.RDHR = CAN->sTxMailBox[0].TDHR; + can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U; + } + + if ((CAN->TSR & CAN_TSR_TERR0) == CAN_TSR_TERR0) { + #ifdef DEBUG_CAN + puts("CAN TX ERROR!\n"); + #endif + } + + if ((CAN->TSR & CAN_TSR_ALST0) == CAN_TSR_ALST0) { + #ifdef DEBUG_CAN + puts("CAN TX ARBITRATION LOST!\n"); + #endif + } + + // clear interrupt + // careful, this can also be cleared by requesting a transmission + CAN->TSR |= CAN_TSR_RQCP0; + } + + if (can_pop(can_queues[bus_number], &to_send)) { + can_tx_cnt += 1; + // only send if we have received a packet + CAN->sTxMailBox[0].TDLR = to_send.RDLR; + CAN->sTxMailBox[0].TDHR = to_send.RDHR; + CAN->sTxMailBox[0].TDTR = to_send.RDTR; + CAN->sTxMailBox[0].TIR = to_send.RIR; + + if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) { + usb_outep3_resume_if_paused(); + } + } + } + + EXIT_CRITICAL(); + } +} + +void ignition_can_hook(CAN_FIFOMailBox_TypeDef *to_push) { + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + int len = GET_LEN(to_push); + + ignition_can_cnt = 0U; // reset counter + + if (bus == 0) { + // TODO: verify on all supported GM models that we can reliably detect ignition using only this signal, + // since the 0x1F1 signal can briefly go low immediately after ignition + if ((addr == 0x160) && (len == 5)) { + // this message isn't all zeros when ignition is on + ignition_cadillac = GET_BYTES_04(to_push) != 0; + } + // GM exception + if ((addr == 0x1F1) && (len == 8)) { + // Bit 5 is ignition "on" + bool ignition_gm = ((GET_BYTE(to_push, 0) & 0x20) != 0); + ignition_can = ignition_gm || ignition_cadillac; + } + // Tesla exception + if ((addr == 0x348) && (len == 8)) { + // GTW_status + ignition_can = (GET_BYTE(to_push, 0) & 0x1) != 0; + } + } +} + +// CAN receive handlers +// blink blue when we are receiving CAN messages +void can_rx(uint8_t can_number) { + CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); + uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); + while ((CAN->RF0R & CAN_RF0R_FMP0) != 0) { + can_rx_cnt += 1; + + // can is live + pending_can_live = 1; + + // add to my fifo + CAN_FIFOMailBox_TypeDef to_push; + to_push.RIR = CAN->sFIFOMailBox[0].RIR; + to_push.RDTR = CAN->sFIFOMailBox[0].RDTR; + to_push.RDLR = CAN->sFIFOMailBox[0].RDLR; + to_push.RDHR = CAN->sFIFOMailBox[0].RDHR; + + // modify RDTR for our API + to_push.RDTR = (to_push.RDTR & 0xFFFF000F) | (bus_number << 4); + can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U; + + // next + CAN->RF0R |= CAN_RF0R_RFOM0; + } +} + +bool can_tx_check_min_slots_free(uint32_t min) { + return + (can_slots_empty(&can_tx1_q) >= min) && + (can_slots_empty(&can_tx2_q) >= min) && + (can_slots_empty(&can_tx3_q) >= min); +} + +void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook) { + if (!skip_tx_hook) { + #ifdef DEBUG_CAN + puts("TX CAN"); + puth2(bus_number); + puts(" "); + puth(to_push->RIR >> 21); + puts("\n"); + #endif + if (bus_number < BUS_MAX) { + // add CAN packet to send queue + // bus number isn't passed through + to_push->RDTR &= 0xF; + can_fwd_errs += can_push(can_queues[bus_number], to_push) ? 0U : 1U; + process_can(CAN_NUM_FROM_BUS_NUM(bus_number)); + + } + } +} + +void can_set_forwarding(int from, int to) { + can_forwarding[from] = to; +} + +bool can_init(uint8_t can_number) { + bool ret = false; + + if (can_number != 0xffU) { + CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); + ret &= can_set_speed(can_number); + ret &= llcan_init(CAN); + // in case there are queued up messages + process_can(can_number); + } + return ret; +} diff --git a/board/kombi/main.c b/board/kombi/main.c new file mode 100644 index 0000000000..de272e499d --- /dev/null +++ b/board/kombi/main.c @@ -0,0 +1,422 @@ +// ********************* Includes ********************* +#include "../config.h" +#include "libc.h" + +#include "main_declarations.h" +#include "critical.h" +#include "faults.h" + +#include "drivers/registers.h" +#include "drivers/interrupts.h" +#include "drivers/llcan.h" +#include "drivers/llgpio.h" +#include "drivers/adc.h" + +#include "board.h" + +#include "drivers/clock.h" +#include "drivers/timer.h" + +#include "gpio.h" +#include "crc.h" + +// uncomment for usb debugging via debug_console.py +#define KOMBI_USB +#define DEBUG + +#ifdef KOMBI_USB + #include "drivers/uart.h" + #include "drivers/usb.h" +#else + // no serial either + void puts(const char *a) { + UNUSED(a); + } + void puth(unsigned int i) { + UNUSED(i); + } + void puth2(unsigned int i) { + UNUSED(i); + } +#endif + +#define ENTER_BOOTLOADER_MAGIC 0xdeadbeef +uint32_t enter_bootloader_mode; + +// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck +void __initialize_hardware_early(void) { + early(); +} + +#ifdef KOMBI_USB + +#include "kombi/can.h" + +// ********************* usb debugging ********************* +void debug_ring_callback(uart_ring *ring) { + char rcv; + while (getc(ring, &rcv) != 0) { + (void)putc(ring, rcv); + } +} + +int usb_cb_ep1_in(void *usbdata, int len, bool hardwired) { + UNUSED(hardwired); + CAN_FIFOMailBox_TypeDef *reply = (CAN_FIFOMailBox_TypeDef *)usbdata; + int ilen = 0; + while (ilen < MIN(len/0x10, 4) && can_pop(&can_rx_q, &reply[ilen])) { + ilen++; + } + return ilen*0x10; +} +// send on serial, first byte to select the ring +void usb_cb_ep2_out(void *usbdata, int len, bool hardwired) { + UNUSED(hardwired); + uint8_t *usbdata8 = (uint8_t *)usbdata; + uart_ring *ur = get_ring_by_number(usbdata8[0]); + if ((len != 0) && (ur != NULL)) { + if ((usbdata8[0] < 2U)) { + for (int i = 1; i < len; i++) { + while (!putc(ur, usbdata8[i])) { + // wait + } + } + } + } +} +// send on CAN +void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) { + UNUSED(usbdata); + UNUSED(len); + UNUSED(hardwired); +} +void usb_cb_ep3_out_complete() { + if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) { + usb_outep3_resume_if_paused(); + } +} + +void usb_cb_enumeration_complete() { + puts("USB enumeration complete\n"); + is_enumerated = 1; +} + +int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) { + UNUSED(hardwired); + unsigned int resp_len = 0; + uart_ring *ur = NULL; + switch (setup->b.bRequest) { + // **** 0xd1: enter bootloader mode + case 0xd1: + // this allows reflashing of the bootstub + // so it's blocked over wifi + switch (setup->b.wValue.w) { + case 0: + // only allow bootloader entry on debug builds + #ifdef ALLOW_DEBUG + if (hardwired) { + puts("-> entering bootloader\n"); + enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; + NVIC_SystemReset(); + } + #endif + break; + case 1: + puts("-> entering softloader\n"); + enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; + NVIC_SystemReset(); + break; + default: + puts("Bootloader mode invalid\n"); + break; + } + break; + // **** 0xd8: reset ST + case 0xd8: + NVIC_SystemReset(); + break; + // **** 0xe0: uart read + case 0xe0: + ur = get_ring_by_number(setup->b.wValue.w); + if (!ur) { + break; + } + // read + while ((resp_len < MIN(setup->b.wLength.w, MAX_RESP_LEN)) && + getc(ur, (char*)&resp[resp_len])) { + ++resp_len; + } + break; + // **** 0xf1: Clear CAN ring buffer. + case 0xf1: + if (setup->b.wValue.w == 0xFFFFU) { + puts("Clearing CAN Rx queue\n"); + can_clear(&can_rx_q); + } else if (setup->b.wValue.w < BUS_MAX) { + puts("Clearing CAN Tx queue\n"); + can_clear(can_queues[setup->b.wValue.w]); + } else { + puts("Clearing CAN CAN ring buffer failed: wrong bus number\n"); + } + break; + // **** 0xf2: Clear UART ring buffer. + case 0xf2: + { + uart_ring * rb = get_ring_by_number(setup->b.wValue.w); + if (rb != NULL) { + puts("Clearing UART queue.\n"); + clear_uart_buff(rb); + } + break; + } + default: + puts("NO HANDLER "); + puth(setup->b.bRequest); + puts("\n"); + break; + } + return resp_len; +} + +#endif + +// ***************************** can port ***************************** +#define CAN_UPDATE 0x341 //bootloader + +void CAN1_TX_IRQ_Handler(void) { + process_can(0); +} + +void CAN2_TX_IRQ_Handler(void) { + // CAN2->TSR |= CAN_TSR_RQCP0; + process_can(1); +} + +void CAN3_TX_IRQ_Handler(void) { + process_can(2); +} + +bool sent; + +uint16_t rpm; +uint16_t intermediary_rpm; +uint8_t scaled_rpm; + + + +void CAN1_RX0_IRQ_Handler(void) { + while ((CAN1->RF0R & CAN_RF0R_FMP0) != 0) { + CAN_FIFOMailBox_TypeDef to_send; + uint8_t dat[8]; + uint16_t address = CAN1->sFIFOMailBox[0].RIR >> 21; + #ifdef DEBUG_CAN + puts("CAN1 RX: "); + puth(address); + puts("\n"); + #endif + switch (address) { + case CAN_UPDATE: + if (GET_BYTES_04(&CAN1->sFIFOMailBox[0]) == 0xdeadface) { + if (GET_BYTES_48(&CAN1->sFIFOMailBox[0]) == 0x0ab00b1e) { + enter_bootloader_mode = ENTER_SOFTLOADER_MAGIC; + NVIC_SystemReset(); + } else if (GET_BYTES_48(&CAN1->sFIFOMailBox[0]) == 0x02b00b1e) { + enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC; + NVIC_SystemReset(); + } else { + puts("Failed entering Softloader or Bootloader\n"); + } + } + break; + case 0x300: ; //RPM SIGNAL + + //Get can messages + for (int i=0; i<8; i++) { + dat[i] = GET_BYTE(&CAN1->sFIFOMailBox[0], i); + } + rpm = dat[1] | (uint16_t)(dat[2] << 8); //Read RPM signal + intermediary_rpm = (rpm/80) * 51; //Scale RPM signal for our purposes + scaled_rpm = intermediary_rpm/25; //Apply DBC scale factor for cluster + #ifdef DEBUG_CAN + puts("Decoded RPM: "); + puth(rpm); + puts("Mathed RPM: "); + puth(intermediary_rpm); + puts("Scaled RPM: "); + puth(scaled_rpm); + puts("\n"); + #endif + //Forward message unmolested + + to_send.RDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24); + to_send.RDHR = dat[4] | (dat[5] << 8) | (dat[6] << 16) | (dat[7] << 24); + to_send.RDTR = 8; + to_send.RIR = (0x300 << 21) | 1U; + can_send(&to_send, 1, false); + can_send(&to_send, 2, false); + break; + + case 0x190: ; //The message we manipulate + //Get can messages + for (int i=0; i<8; i++) { + dat[i] = GET_BYTE(&CAN1->sFIFOMailBox[0], i); + } + //original message with spoofed RPM + to_send.RDLR = dat[0] | (dat[1] << 8) | (scaled_rpm << 16) | (dat[3] << 24); + to_send.RDHR = dat[4] | (dat[5] << 8) | (dat[6] << 16) | (dat[7] << 24); + to_send.RDTR = 8; + to_send.RIR = (0x300 << 21) | 1U; + can_send(&to_send, 1, false); + //forward message to bus with original rpm (probably unnecessary) + to_send.RDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24); + can_send(&to_send, 2, false); + break; + + default: ; //forward to cluster and esp unmolested + to_send.RIR = CAN1->sFIFOMailBox[0].RIR | 1U; + to_send.RDTR = CAN1->sFIFOMailBox[0].RDTR; + to_send.RDLR = CAN1->sFIFOMailBox[0].RDLR; + to_send.RDHR = CAN1->sFIFOMailBox[0].RDHR; + can_send(&to_send, 1, false); + can_send(&to_send, 2, false); + break; + } + can_rx(0); + // next + // CAN1->RF0R |= CAN_RF0R_RFOM0; + } +} + +void CAN1_SCE_IRQ_Handler(void) { + can_sce(CAN1); + llcan_clear_send(CAN1); +} + +void CAN2_RX0_IRQ_Handler(void) { + //All messages from cluster shoot through to both buses no fuckery + while ((CAN2->RF0R & CAN_RF0R_FMP0) != 0) { + #ifdef DEBUG_CAN + puts("CAN2 RX: "); + puth(address); + puts("\n"); + #endif + + CAN_FIFOMailBox_TypeDef to_send; + to_send.RIR = CAN2->sFIFOMailBox[0].RIR | 1U; + to_send.RDTR = CAN2->sFIFOMailBox[0].RDTR; + to_send.RDLR = CAN2->sFIFOMailBox[0].RDLR; + to_send.RDHR = CAN2->sFIFOMailBox[0].RDHR; + can_send(&to_send, 0, false); + can_send(&to_send, 2, false); + + // next + can_rx(1); + } +} + +void CAN2_SCE_IRQ_Handler(void) { + can_sce(CAN2); + llcan_clear_send(CAN2); +} + +void CAN3_RX0_IRQ_Handler(void) { + while ((CAN3->RF0R & CAN_RF0R_FMP0) != 0) { + uint16_t address = CAN3->sFIFOMailBox[0].RIR >> 21; + #ifdef DEBUG_CAN + puts("CAN3 RX: "); + puth(address); + puts("\n"); + #else + UNUSED(address); + #endif + CAN_FIFOMailBox_TypeDef to_send; + to_send.RIR = CAN3->sFIFOMailBox[0].RIR | 1U; + to_send.RDTR = CAN3->sFIFOMailBox[0].RDTR; + to_send.RDLR = CAN3->sFIFOMailBox[0].RDLR; + to_send.RDHR = CAN3->sFIFOMailBox[0].RDHR; + can_send(&to_send, 0, false); + can_send(&to_send, 1, false); + // next + can_rx(2); + } +} + +void CAN3_SCE_IRQ_Handler(void) { + can_sce(CAN3); + llcan_clear_send(CAN3); +} + + +// ***************************** main code ***************************** + + +void kombi(void) { + // read/write + watchdog_feed(); + +} + +int main(void) { + // Init interrupt table + init_interrupts(true); + + REGISTER_INTERRUPT(CAN1_TX_IRQn, CAN1_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1) + REGISTER_INTERRUPT(CAN1_RX0_IRQn, CAN1_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1) + REGISTER_INTERRUPT(CAN1_SCE_IRQn, CAN1_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1) + REGISTER_INTERRUPT(CAN2_TX_IRQn, CAN2_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2) + REGISTER_INTERRUPT(CAN2_RX0_IRQn, CAN2_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2) + REGISTER_INTERRUPT(CAN2_SCE_IRQn, CAN2_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2) + REGISTER_INTERRUPT(CAN3_TX_IRQn, CAN3_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3) + REGISTER_INTERRUPT(CAN3_RX0_IRQn, CAN3_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3) + REGISTER_INTERRUPT(CAN3_SCE_IRQn, CAN3_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3) + + disable_interrupts(); + + // init devices + clock_init(); + peripherals_init(); + detect_configuration(); + detect_board_type(); + + // init board + current_board->init(); + // enable USB + #ifdef KOMBI_USB + USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOVAL; + USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOEN; + usb_init(); + #endif + + // init can + bool llcan_speed_set = llcan_set_speed(CAN1, 5000, false, false); + if (!llcan_speed_set) { + puts("Failed to set llcan1 speed"); + } + llcan_speed_set = llcan_set_speed(CAN2, 5000, false, false); + if (!llcan_speed_set) { + puts("Failed to set llcan2 speed"); + } + llcan_speed_set = llcan_set_speed(CAN3, 5000, false, false); + if (!llcan_speed_set) { + puts("Failed to set llcan3 speed"); + } + + bool ret = llcan_init(CAN1); + UNUSED(ret); + ret = llcan_init(CAN2); + UNUSED(ret); + ret = llcan_init(CAN3); + UNUSED(ret); + + watchdog_init(); + + puts("**** INTERRUPTS ON ****\n"); + enable_interrupts(); + + // main pedal loop + while (1) { + kombi(); + } + + return 0; +} diff --git a/board/kombi/main_declarations.h b/board/kombi/main_declarations.h new file mode 100644 index 0000000000..ade4acac37 --- /dev/null +++ b/board/kombi/main_declarations.h @@ -0,0 +1,17 @@ +// ******************** Prototypes ******************** +void puts(const char *a); +void puth(unsigned int i); +void puth2(unsigned int i); +typedef struct board board; +typedef struct harness_configuration harness_configuration; +void can_flip_buses(uint8_t bus1, uint8_t bus2); +void can_set_obd(uint8_t harness_orientation, bool obd); + +// ********************* Globals ********************** +uint8_t hw_type = 0; +const board *current_board; +bool is_enumerated = 0; +uint32_t heartbeat_counter = 0; +uint32_t uptime_cnt = 0; +bool siren_enabled = false; +bool green_led_enabled = false;