From b8ce1c9686da19cac3094e7d754dc6a642b87cef Mon Sep 17 00:00:00 2001 From: Kevin Roscom Date: Tue, 22 Feb 2022 18:39:26 -0700 Subject: [PATCH] toyota EPS ocelot gateway --- board/SConscript | 25 +- board/crc.h | 29 ++ board/eps_gw/.gitignore | 1 + board/eps_gw/can.h | 370 ++++++++++++++++++++ board/eps_gw/flash_can.sh | 8 + board/eps_gw/main.c | 582 +++++++++++++++++++++++++++++++ board/eps_gw/main_declarations.h | 17 + board/eps_gw/recover.sh | 11 + board/eps_gw/recover_usb.sh | 12 + board/ibst/main.c | 4 +- board/ibst/recover.sh | 2 +- board/smart_dsu/recover.sh | 2 +- board/smart_dsu/recover_usb.sh | 12 + 13 files changed, 1053 insertions(+), 22 deletions(-) create mode 100644 board/eps_gw/.gitignore create mode 100644 board/eps_gw/can.h create mode 100755 board/eps_gw/flash_can.sh create mode 100644 board/eps_gw/main.c create mode 100644 board/eps_gw/main_declarations.h create mode 100755 board/eps_gw/recover.sh create mode 100755 board/eps_gw/recover_usb.sh create mode 100755 board/smart_dsu/recover_usb.sh diff --git a/board/SConscript b/board/SConscript index 2c498d6..c727c6b 100644 --- a/board/SConscript +++ b/board/SConscript @@ -21,26 +21,15 @@ if os.getenv("PEDAL"): "-DPEDAL", ] -if os.getenv("IBST"): +if os.getenv("GATEWAY"): PROJECT = "panda" STARTUP_FILE = "startup_stm32f413xx.s" - MAIN = "ibst/main.c" - PROJECT_FLAGS = [ - "-mcpu=cortex-m4", - "-mhard-float", - "-DSTM32F4", - "-DSTM32F413xx", - "-mfpu=fpv4-sp-d16", - "-fsingle-precision-constant", - "-Os", - "-g", - "-DGATEWAY", - ] - -if os.getenv("SDSU"): - PROJECT = "panda" - STARTUP_FILE = "startup_stm32f413xx.s" - MAIN = "smart_dsu/main.c" + if os.getenv("IBST"): + MAIN = "ibst/main.c" + elif os.getenv("SDSU"): + MAIN = "smart_dsu/main.c" + elif os.getenv("EPS_GW"): + MAIN = "eps_gw/main.c" PROJECT_FLAGS = [ "-mcpu=cortex-m4", "-mhard-float", diff --git a/board/crc.h b/board/crc.h index ab969e5..3598de9 100644 --- a/board/crc.h +++ b/board/crc.h @@ -14,3 +14,32 @@ uint8_t crc_checksum(uint8_t *dat, int len, const uint8_t poly) { } return crc; } + +void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]) { + + uint8_t j; + uint16_t i; + + for (i = 0; i < 256; i++) { + uint8_t crc; + crc = i; + for (j = 0; j < 8; j++) { + if ((crc & 0x80) != 0) + crc = (uint8_t)((crc << 1) ^ poly); + else + crc <<= 1; + } + crc_lut[i] = crc; + } +} + +uint8_t lut_checksum(uint8_t *d, int l, uint8_t *table) { + uint8_t crc = 0xFF; // Standard init value for CRC8 + // CRC the payload, skipping over the first byte where the CRC lives. + for (int i = 1; i < l; i++) { + crc ^= d[i] & 0xFF; + crc = table[crc] ^ crc<<8; + } + crc = crc ^ 0xFF; //final xor + return crc; +} \ No newline at end of file diff --git a/board/eps_gw/.gitignore b/board/eps_gw/.gitignore new file mode 100644 index 0000000..94053f2 --- /dev/null +++ b/board/eps_gw/.gitignore @@ -0,0 +1 @@ +obj/* diff --git a/board/eps_gw/can.h b/board/eps_gw/can.h new file mode 100644 index 0000000..8c33386 --- /dev/null +++ b/board/eps_gw/can.h @@ -0,0 +1,370 @@ +// 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 + 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 + 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 + puts("CAN TX ERROR!\n"); + #endif + } + + if ((CAN->TSR & CAN_TSR_ALST0) == CAN_TSR_ALST0) { + #ifdef DEBUG + 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 + 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/eps_gw/flash_can.sh b/board/eps_gw/flash_can.sh new file mode 100755 index 0000000..014b810 --- /dev/null +++ b/board/eps_gw/flash_can.sh @@ -0,0 +1,8 @@ +#!/usr/bin/env sh +set -e + +cd .. +IBST=1 scons -u +cd ibst + +../../tests/gateway/enter_canloader.py ../obj/panda.bin.signed diff --git a/board/eps_gw/main.c b/board/eps_gw/main.c new file mode 100644 index 0000000..23d2740 --- /dev/null +++ b/board/eps_gw/main.c @@ -0,0 +1,582 @@ +// ********************* 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 EPS_GW_USB +#define DEBUG + +#ifdef EPS_GW_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 EPS_GW_USB + +#include "eps_gw/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 0x23F //bootloader +#define COUNTER_CYCLE 0xFU +#define LKA_COUNTER_CYCLE = 0x3FU + +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; + +// Toyota Checksum algorithm +uint8_t toyota_checksum(int addr, uint8_t *dat, int len){ + int cksum = 0; + for(int ii = 0; ii < (len - 1); ii++){ + cksum = (cksum + dat[ii]); + } + cksum += len; + cksum += ((addr >> 8U) & 0xFF); // idh + cksum += ((addr) & 0xFF); // idl + return cksum & 0xFF; +} + +// OUTPUTS +//--------------------------------- +#define LKA_INPUT 0x2E4 +uint16_t torque_req = 0; +uint8_t lka_counter = 0; +bool lka_req = 0; +uint8_t lka_checksum = 0; + +#define CAN_ID 0x22F +bool eps_ok = 0; + +// INPUTS +//--------------------------------- +#define CAN_INPUT 0x22E +uint8_t mode = 0; +uint16_t rel_input = 0; +uint16_t pos_input = 0; + +#define STEER_TORQUE_SENSOR 0x260 +uint16_t steer_torque_driver = 0; +uint16_t steer_torque_eps = 0; +bool steer_override = 0; + +#define EPS_STATUS 0x262 +uint8_t lka_state = 0; + +// COUNTERS +uint8_t can1_count_out = 0; +uint8_t can1_count_in; +uint8_t can2_count_out = 0; + +#define MAX_TIMEOUT 50U +uint32_t timeout = 0; + +#define NO_FAULT 0U +#define FAULT_BAD_CHECKSUM 1U +#define FAULT_SEND 2U +#define FAULT_SCE 3U +#define FAULT_STARTUP 4U +#define FAULT_TIMEOUT 5U +#define FAULT_INVALID 6U +#define FAULT_COUNTER 7U + +uint8_t state = FAULT_STARTUP; + +const uint8_t crc_poly = 0x1D; // standard crc8 SAE J1850 +uint8_t crc8_lut_1d[256]; + +void CAN1_RX0_IRQ_Handler(void) { + while ((CAN1->RF0R & CAN_RF0R_FMP0) != 0) { + 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 CAN_INPUT: ; + uint8_t dat[6]; + for (int i=0; i<6; i++) { + dat[i] = GET_BYTE(&CAN1->sFIFOMailBox[0], i); + } + uint8_t index = dat[1] & COUNTER_CYCLE; + if(dat[0] == lut_checksum(dat, 6, crc8_lut_1d)) { + if (((can1_count_in + 1U) & COUNTER_CYCLE) == index) { + //if counter and checksum valid accept commands + mode = ((dat[1] >> 4U) & 3U); + if (mode != 0){ + lka_req = 1; + } else { + lka_req = 0; + } + pos_input = ((dat[3] & 0xFU) << 8U) | dat[2]; + rel_input = ((dat[5] << 8U) | dat[4]); + // TODO: safety? scaling? + torque_req = rel_input; + can1_count_in++; + } + else { + state = FAULT_COUNTER; + } + state = NO_FAULT; + timeout = 0; + } + else { + state = FAULT_BAD_CHECKSUM; + puts("checksum fail 0x22E \n"); + puts("DATA: "); + for(int ii = 0; ii < 6; ii++){ + puth2(dat[ii]); + } + puts("\n"); + puts("expected: "); + puth2(lut_checksum(dat, 6, crc8_lut_1d)); + puts(" got: "); + puth2(dat[0]); + puts("\n"); + } + break; + default: ; + } + can_rx(0); + // next + // CAN1->RF0R |= CAN_RF0R_RFOM0; + } +} + +void CAN1_SCE_IRQ_Handler(void) { + state = FAULT_SCE; + can_sce(CAN1); + llcan_clear_send(CAN1); +} + +void CAN2_RX0_IRQ_Handler(void) { + while ((CAN2->RF0R & CAN_RF0R_FMP0) != 0) { + uint16_t address = CAN2->sFIFOMailBox[0].RIR >> 21; + #ifdef DEBUG_CAN + puts("CAN2 RX: "); + puth(address); + puts("\n"); + #else + UNUSED(address); + #endif + // next + can_rx(1); + } +} + +void CAN2_SCE_IRQ_Handler(void) { + state = FAULT_SCE; + 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"); + #endif + switch (address) { + case STEER_TORQUE_SENSOR: ; + uint8_t dat[8]; + for (int i=0; i<8; i++) { + dat[i] = GET_BYTE(&CAN3->sFIFOMailBox[0], i); + } + if(dat[7] == toyota_checksum(address, dat, 8)) { + steer_override = dat[0] & 1U; + steer_torque_driver = (dat[1] << 8U) | dat[2]; + steer_torque_eps = (dat[5] << 8U) | dat[6]; + } + else { + state = FAULT_BAD_CHECKSUM; + } + break; + case EPS_STATUS: ; + uint8_t dat2[5]; + for (int i=0; i<5; i++) { + dat2[i] = GET_BYTE(&CAN3->sFIFOMailBox[0], i); + } + if(dat2[4] == toyota_checksum(address, dat2, 5)) { + lka_state = dat2[3] >> 1U; + } + else { + state = FAULT_BAD_CHECKSUM; + } + break; + default: ; + } + // next + can_rx(2); + } +} + +void CAN3_SCE_IRQ_Handler(void) { + state = FAULT_SCE; + can_sce(CAN3); + llcan_clear_send(CAN3); +} + +int to_signed(int d, int bits) { + int d_signed = d; + if (d >= (1 << MAX((bits - 1), 0))) { + d_signed = d - (1 << MAX(bits, 0)); + } + return d_signed; +} + +void TIM3_IRQ_Handler(void) { + // cmain loop for sending 100hz messages + + if ((CAN2->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) { + uint8_t dat[8]; // LKA_INPUT + dat[0] = (1 << 7U) | (can2_count_out << 1U) | lka_req; + dat[1] = (torque_req >> 8U); + dat[2] = (torque_req & 0xFF); + dat[3] = 0x0; + dat[4] = toyota_checksum(LKA_INPUT, dat, 5); + + CAN_FIFOMailBox_TypeDef to_send; + to_send.RDLR = dat[0] | (dat[1] << 8) | (dat[2] << 16) | (dat[3] << 24); + to_send.RDHR = dat[4]; + to_send.RDTR = 5; + to_send.RIR = (LKA_INPUT << 21) | 1U; + can_send(&to_send, 2, false); + + } + else { + // old can packet hasn't sent! + state = FAULT_SEND; + #ifdef DEBUG_CAN + puts("CAN2 MISS1\n"); + #endif + } + + //send to EON + if ((CAN1->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) { + uint8_t dat[7]; + + dat[6] = (steer_torque_eps & 0xFF); + dat[5] = (steer_torque_eps >> 8U); + dat[4] = (steer_torque_driver & 0xFF); + dat[3] = (steer_torque_driver >> 8U); + dat[2] = eps_ok; + dat[1] = ((state & 0xFU) << 4) | can1_count_out; + dat[0] = lut_checksum(dat, 7, crc8_lut_1d); + + CAN_FIFOMailBox_TypeDef to_send; + 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); + to_send.RDTR = 7; + to_send.RIR = (CAN_ID << 21) | 1U; + can_send(&to_send, 0, false); + + can1_count_out++; + can1_count_out &= COUNTER_CYCLE; + + } + else { + // old can packet hasn't sent! + state = FAULT_SEND; + #ifdef DEBUG_CAN + puts("CAN1 MISS1\n"); + #endif + } + // blink the LED + + TIM3->SR = 0; + + // up timeout for gas set + if (timeout == MAX_TIMEOUT) { + state = FAULT_TIMEOUT; + torque_req = 0; + mode = 0; + } else { + timeout += 1U; + } + + #ifdef DEBUG + puts("MODE: "); + puth(mode << 1U); + puts(" EPS: "); + puts("\n"); + #endif +} + +// ***************************** main code ***************************** + + +void ibst(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) + + // Should run at around 732Hz (see init below) + REGISTER_INTERRUPT(TIM3_IRQn, TIM3_IRQ_Handler, 1000U, FAULT_INTERRUPT_RATE_TIM3) + + disable_interrupts(); + + // init devices + clock_init(); + peripherals_init(); + detect_configuration(); + detect_board_type(); + + // init board + current_board->init(); + // enable USB + #ifdef EPS_GW_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); + + gen_crc_lookup_table(crc_poly, crc8_lut_1d); + + // 48mhz / 65536 ~= 732 + timer_init(TIM3, 7); + NVIC_EnableIRQ(TIM3_IRQn); + + // power on EPS + set_gpio_mode(GPIOB, 12, MODE_OUTPUT); + set_gpio_output_type(GPIOB, 12, OUTPUT_TYPE_PUSH_PULL); + set_gpio_output(GPIOB, 12, 1); + + watchdog_init(); + + puts("**** INTERRUPTS ON ****\n"); + enable_interrupts(); + + // main pedal loop + while (1) { + ibst(); + } + + return 0; +} \ No newline at end of file diff --git a/board/eps_gw/main_declarations.h b/board/eps_gw/main_declarations.h new file mode 100644 index 0000000..ade4aca --- /dev/null +++ b/board/eps_gw/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; diff --git a/board/eps_gw/recover.sh b/board/eps_gw/recover.sh new file mode 100755 index 0000000..bef86ad --- /dev/null +++ b/board/eps_gw/recover.sh @@ -0,0 +1,11 @@ +#!/usr/bin/env sh +set -e + +DFU_UTIL="dfu-util" + +cd .. +GATEWAY=1 EPS_GW=1 scons -u +cd eps_gw + +$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D ../obj/panda.bin.signed +$DFU_UTIL -d 0483:df11 -a 0 -s 0x08000000:leave -D ../obj/bootstub.panda.bin diff --git a/board/eps_gw/recover_usb.sh b/board/eps_gw/recover_usb.sh new file mode 100755 index 0000000..2ca52be --- /dev/null +++ b/board/eps_gw/recover_usb.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env sh +set -e + +DFU_UTIL="dfu-util" + +cd .. +GATEWAY=1 EPS_GW=1 scons -u +PYTHONPATH=.. python3 -c "from python import Panda; Panda().reset(enter_bootstub=True); Panda().reset(enter_bootloader=True)" || true +cd eps_gw + +$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D ../obj/panda.bin.signed +$DFU_UTIL -d 0483:df11 -a 0 -s 0x08000000:leave -D ../obj/bootstub.panda.bin diff --git a/board/ibst/main.c b/board/ibst/main.c index 5fe5503..9928db8 100644 --- a/board/ibst/main.c +++ b/board/ibst/main.c @@ -334,12 +334,12 @@ void CAN1_RX0_IRQ_Handler(void) { state = FAULT_BAD_CHECKSUM; puts("checksum fail 0x20E \n"); puts("DATA: "); - for(int ii = 0; ii < 5; ii++){ + for(int ii = 0; ii < 6; ii++){ puth2(dat[ii]); } puts("\n"); puts("expected: "); - puth2(lut_checksum(dat, 5, crc8_lut_1d)); + puth2(lut_checksum(dat, 6, crc8_lut_1d)); puts(" got: "); puth2(dat[0]); puts("\n"); diff --git a/board/ibst/recover.sh b/board/ibst/recover.sh index 7056840..fad0e29 100755 --- a/board/ibst/recover.sh +++ b/board/ibst/recover.sh @@ -4,7 +4,7 @@ set -e DFU_UTIL="dfu-util" cd .. -IBST=1 scons -u +GATEWAY=1 IBST=1 scons -u cd ibst $DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D ../obj/panda.bin.signed diff --git a/board/smart_dsu/recover.sh b/board/smart_dsu/recover.sh index 68c3ea2..5898414 100755 --- a/board/smart_dsu/recover.sh +++ b/board/smart_dsu/recover.sh @@ -4,7 +4,7 @@ set -e DFU_UTIL="dfu-util" cd .. -SDSU=1 scons -u +GATEWAY=1 SDSU=1 scons -u cd smart_dsu $DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D ../obj/panda.bin.signed diff --git a/board/smart_dsu/recover_usb.sh b/board/smart_dsu/recover_usb.sh new file mode 100755 index 0000000..79f41a8 --- /dev/null +++ b/board/smart_dsu/recover_usb.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env sh +set -e + +DFU_UTIL="dfu-util" + +cd .. +GATEWAY=1 SDSU=1 scons -u +PYTHONPATH=.. python3 -c "from python import Panda; Panda().reset(enter_bootstub=True); Panda().reset(enter_bootloader=True)" || true +cd smart_dsu + +$DFU_UTIL -d 0483:df11 -a 0 -s 0x08004000 -D ../obj/panda.bin.signed +$DFU_UTIL -d 0483:df11 -a 0 -s 0x08000000:leave -D ../obj/bootstub.panda.bin