toyota EPS ocelot gateway

master
Kevin Roscom 2022-02-22 18:39:26 -07:00
parent e59134fec0
commit b8ce1c9686
13 changed files with 1053 additions and 22 deletions

View File

@ -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",

View File

@ -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;
}

1
board/eps_gw/.gitignore vendored 100644
View File

@ -0,0 +1 @@
obj/*

370
board/eps_gw/can.h 100644
View File

@ -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;
}

View File

@ -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

582
board/eps_gw/main.c 100644
View File

@ -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;
}

View File

@ -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;

View File

@ -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

View File

@ -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

View File

@ -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");

View File

@ -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

View File

@ -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

View File

@ -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