mawei
/
panda
mirror of https://github.com/commaai/panda.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
panda/boardesp/elm327.c

1579 lines
50 KiB
C
Raw Blame History

#include "ets_sys.h"
#include "osapi.h"
#include "gpio.h"
#include "os_type.h"
#include "user_interface.h"
#include "espconn.h"
#include "mem.h"
#include "driver/uart.h"
//#define ELM_DEBUG
#define MIN(a,b) ((a) < (b) ? (a) : (b))
#define MAX(a,b) ((a) > (b) ? (a) : (b))
int ICACHE_FLASH_ATTR spi_comm(char *dat, int len, uint32_t *recvData, int recvDataLen);
#define ELM_PORT 35000
//Version 1.5 is an invalid version used by many pirate clones
//that only partially support 1.0.
#define IDENT_MSG "ELM327 v1.5\r\r"
#define DEVICE_DESC "Panda\n\n"
#define SHOW_CONNECTION(msg, p_conn) os_printf("%s %p, proto %p, %d.%d.%d.%d:%d disconnect\r\n", \
msg, p_conn, (p_conn)->proto.tcp, (p_conn)->proto.tcp->remote_ip[0], \
(p_conn)->proto.tcp->remote_ip[1], (p_conn)->proto.tcp->remote_ip[2], \
(p_conn)->proto.tcp->remote_ip[3], (p_conn)->proto.tcp->remote_port)
const static char hex_lookup[] = {'0', '1', '2', '3', '4', '5', '6', '7',
'8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
typedef struct __attribute__((packed)) {
bool tx : 1;
bool : 1;
bool ext : 1;
uint32_t addr : 29;
uint8_t len : 4;
uint8_t bus : 8;
uint8_t : 4; //unused
uint16_t ts : 16;
uint8_t data[8];
} panda_can_msg_t;
//TODO: Masking is likely unnecessary for these bit fields. Check.
#define panda_get_can_addr(recv) (((recv)->ext) ? ((recv)->addr & 0x1FFFFFFF) :\
(((recv)->addr >> 18) & 0x7FF))
#define PANDA_CAN_FLAG_TRANSMIT 1
#define PANDA_CAN_FLAG_EXTENDED 4
#define PANDA_USB_CAN_WRITE_BUS_NUM 3
#define PANDA_USB_LIN_WRITE_BUS_NUM 2
#define SAFETY_ELM327 3U
typedef struct _elm_tcp_conn {
struct espconn *conn;
struct _elm_tcp_conn *next;
} elm_tcp_conn_t;
typedef struct __attribute__((packed)) {
uint8_t len;
uint8_t dat[7]; //mode and data
} elm_can_obd_msg;
typedef struct __attribute__((packed)) {
uint8_t priority;
uint8_t receiver;
uint8_t sender;
uint8_t dat[8]; //mode, data, and checksum
} elm_lin_obd_msg;
typedef struct __attribute__((packed)) {
uint16_t usb_ep_num;
uint16_t payload_len;
uint8_t serial_port;
//uint8_t msg[8+3];
elm_lin_obd_msg msg;
} elm_lin_usb_msg;
static struct espconn elm_conn;
static esp_tcp elm_proto;
static elm_tcp_conn_t *connection_list = NULL;
static char stripped_msg[0x100];
static uint16 stripped_msg_len = 0;
static char in_msg[0x100];
static uint16 in_msg_len = 0;
static char rsp_buff[536]; //TCP min MTU
static uint16 rsp_buff_len = 0;
static uint8_t pandaSendData[0x14] = {0};
static uint32_t pandaRecvData[0x40] = {0};
static uint32_t pandaRecvDataDummy[0x40] = {0}; // Used for CAN write operations (no received data)
#define ELM_MODE_SELECTED_PROTOCOL_DEFAULT 6
#define ELM_MODE_TIMEOUT_DEFAULT 20;
#define ELM_MODE_KEEPALIVE_PERIOD_DEFAULT (0x92*20)
static bool elm_mode_echo = true;
static bool elm_mode_linefeed = false;
static bool elm_mode_additional_headers = false;
static bool elm_mode_auto_protocol = true;
static uint8_t elm_selected_protocol = ELM_MODE_SELECTED_PROTOCOL_DEFAULT;
static bool elm_mode_print_spaces = true;
static uint8_t elm_mode_adaptive_timing = 1;
static bool elm_mode_allow_long = false;
static uint16_t elm_mode_timeout = ELM_MODE_TIMEOUT_DEFAULT;
static uint16_t elm_mode_keepalive_period = ELM_MODE_KEEPALIVE_PERIOD_DEFAULT;
bool lin_bus_initialized = false;
/***********************************************
*** ELM CLI response functions ***
*** (for sending data back to the terminal) ***
***********************************************/
// All ELM operations are global, so send data out to all connections
void ICACHE_FLASH_ATTR elm_tcp_tx_flush() {
if(!rsp_buff_len) return; // Was causing small error messages
for(elm_tcp_conn_t *iter = connection_list; iter != NULL; iter = iter->next){
int8_t err = espconn_send(iter->conn, rsp_buff, rsp_buff_len);
if(err){
os_printf(" Wifi %p TX error code %d\n", iter->conn, err);
if(err == ESPCONN_ARG) {
if(iter == connection_list) {
connection_list = iter->next;
} else {
for(elm_tcp_conn_t *iter2 = connection_list; iter2 != NULL; iter2 = iter2->next)
if(iter2->next == iter) {
iter2->next = iter->next;
break;
}
}
os_printf(" deleting orphaned connection. iter: %p; conn: %p\n", iter, iter->conn);
os_free(iter);
}
}
}
rsp_buff_len = 0;
}
static void ICACHE_FLASH_ATTR elm_append_rsp(const char *data, uint16_t len) {
uint16_t overflow_len = 0;
if(rsp_buff_len + len > sizeof(rsp_buff)) {
overflow_len = rsp_buff_len + len - sizeof(rsp_buff);
len = sizeof(rsp_buff) - rsp_buff_len;
}
if(!elm_mode_linefeed) {
memcpy(rsp_buff + rsp_buff_len, data, len);
rsp_buff_len += len;
} else {
for(int i=0; i < len && rsp_buff_len < sizeof(rsp_buff); i++){
rsp_buff[rsp_buff_len++] = data[i];
if(data[i] == '\r' && rsp_buff_len < sizeof(rsp_buff))
rsp_buff[rsp_buff_len++] = '\n';
}
}
if(overflow_len) {
os_printf("Packet full, sending\n");
elm_tcp_tx_flush();
elm_append_rsp(data + len, overflow_len);
}
}
#define elm_append_rsp_const(str) elm_append_rsp(str, sizeof(str)-1)
static void ICACHE_FLASH_ATTR elm_append_rsp_hex_byte(uint8_t num) {
elm_append_rsp(&hex_lookup[num >> 4], 1);
elm_append_rsp(&hex_lookup[num & 0xF], 1);
if(elm_mode_print_spaces) elm_append_rsp_const(" ");
}
void ICACHE_FLASH_ATTR elm_append_rsp_can_msg_addr(const panda_can_msg_t *recv) {
//Show address
uint32_t addr = panda_get_can_addr(recv);
if(recv->ext){
elm_append_rsp_hex_byte(addr>>24);
elm_append_rsp_hex_byte(addr>>16);
elm_append_rsp_hex_byte(addr>>8);
elm_append_rsp_hex_byte(addr);
} else {
elm_append_rsp(&hex_lookup[addr>>8], 1);
elm_append_rsp_hex_byte(addr);
}
}
/***************************************
*** Panda communication functions ***
*** (for controlling the Panda MCU) ***
***************************************/
static int ICACHE_FLASH_ATTR panda_usbemu_ctrl_write(uint8_t request_type, uint8_t request,
uint16_t value, uint16_t index, uint16_t length) {
//self.sock.send(struct.pack("HHBBHHH", 0, 0, request_type, request, value, index, length));
*(uint16_t*)(pandaSendData) = 0;
*(uint16_t*)(pandaSendData+2) = 0;
pandaSendData[4] = request_type;
pandaSendData[5] = request;
*(uint16_t*)(pandaSendData+6) = value;
*(uint16_t*)(pandaSendData+8) = index;
*(uint16_t*)(pandaSendData+10) = length;
int returned_count = spi_comm(pandaSendData, 0x10, pandaRecvData, 0x40);
if(returned_count > 0x40 || returned_count < 0)
return -1;
return returned_count;
}
#define panda_set_can0_cbaud(cbps) panda_usbemu_ctrl_write(0x40, 0xde, 0, cbps, 0)
#define panda_set_can0_kbaud(kbps) panda_usbemu_ctrl_write(0x40, 0xde, 0, kbps*10, 0)
#define panda_set_safety_mode(mode) panda_usbemu_ctrl_write(0x40, 0xdc, mode, 0, 0)
#define panda_kline_wakeup_pulse() panda_usbemu_ctrl_write(0x40, 0xf0, 0, 0, 0)
#define panda_clear_can_rx() panda_usbemu_ctrl_write(0x40, 0xf1, 0xFFFF, 0, 0)
#define panda_clear_lin_txrx() panda_usbemu_ctrl_write(0x40, 0xf2, 2, 0, 0)
static int ICACHE_FLASH_ATTR panda_usbemu_can_read(panda_can_msg_t** can_msgs) {
int returned_count = spi_comm((uint8_t *)((const uint16 []){1,0}), 4, pandaRecvData, 0x40);
if(returned_count > 0x40 || returned_count < 0){
os_printf("CAN read got invalid length\n");
return -1;
}
*can_msgs = (panda_can_msg_t*)(pandaRecvData+1);
return returned_count/sizeof(panda_can_msg_t);
}
static int ICACHE_FLASH_ATTR panda_usbemu_can_write(bool ext, uint32_t addr,
char *candata, uint8_t canlen) {
uint32_t rir;
if(canlen > 8) return 0;
if(ext || addr >= 0x800){
rir = (addr << 3) | PANDA_CAN_FLAG_TRANSMIT | PANDA_CAN_FLAG_EXTENDED;
}else{
rir = (addr << 21) | PANDA_CAN_FLAG_TRANSMIT;
}
#define MAX_CAN_LEN 8
//Wifi USB Wrapper
*(uint16_t*)(pandaSendData) = PANDA_USB_CAN_WRITE_BUS_NUM; //USB Bulk Endpoint ID.
*(uint16_t*)(pandaSendData+2) = MAX_CAN_LEN;
//BULK MESSAGE
*(uint32_t*)(pandaSendData+4) = rir;
*(uint32_t*)(pandaSendData+8) = MAX_CAN_LEN | (0 << 4); //0 is CAN bus number.
//CAN DATA
memcpy(pandaSendData+12, candata, canlen);
memset(pandaSendData+12+canlen, 0, MAX_CAN_LEN-canlen);
for(int i = 12+canlen; i < 20; i++) pandaSendData[i] = 0; //Zero the rest
/* spi_comm will erase data in the recv buffer even if you are only
* interested in sending data that gets no response (like writing
* can data). This behavior becomes problematic when trying to send
* a can message while processsing received can messages. A dummy
* recv buffer is used here so received data is not overwritten. */
int returned_count = spi_comm(pandaSendData, 0x14, pandaRecvDataDummy, 0x40);
if(returned_count)
os_printf("ELM Can send expected 0 bytes back from panda. Got %d bytes instead\n", returned_count);
if(returned_count > 0x40) return 0;
return returned_count;
}
elm_lin_obd_msg lin_last_sent_msg;
uint16_t lin_last_sent_msg_len = 0;
bool lin_await_msg_echo = false;
static int ICACHE_FLASH_ATTR panda_usbemu_kline_read(uint16_t len) {
int returned_count = panda_usbemu_ctrl_write(0xC0, 0xE0, 2, 0, len);
if(returned_count > len || returned_count < 0){
os_printf("LIN read got invalid length\n");
return -1;
}
#ifdef ELM_DEBUG
if(returned_count) {
os_printf("LIN Received %d bytes\n", returned_count);
os_printf(" Data: ");
for(int i = 0; i < returned_count; i++)
os_printf("%02x ", ((char*)(pandaRecvData+1))[i]);
os_printf("\n");
}
#endif
return returned_count;
}
static int ICACHE_FLASH_ATTR panda_usbemu_kline_write(elm_lin_obd_msg *msg) {
elm_lin_usb_msg usb_msg = {};
usb_msg.usb_ep_num = PANDA_USB_LIN_WRITE_BUS_NUM; //USB Bulk Endpoint ID.
usb_msg.payload_len = (msg->priority & 0x07) + 4 + 1; //The +1 is for serial_port
usb_msg.serial_port = 2;
memcpy(&usb_msg.msg, msg, sizeof(elm_lin_obd_msg));
/* spi_comm will erase data in the recv buffer even if you are only
* interested in sending data that gets no response (like writing
* can data). This behavior becomes problematic when trying to send
* a can message while processsing received can messages. A dummy
* recv buffer is used here so received data is not overwritten. */
int returned_count = spi_comm((char*)&usb_msg, sizeof(elm_lin_usb_msg), pandaRecvDataDummy, 0x40);
if(returned_count)
os_printf("ELM LIN send expected 0 bytes back from panda. Got %d bytes instead\n", returned_count);
if(returned_count > 0x40) return 0;
return returned_count;
}
/****************************************
*** Ringbuffer ***
****************************************/
//LIN data is delivered in chunks of arbitrary size. Using a
//ringbuffer to handle it.
uint8_t lin_ringbuff[0x20];
uint8_t lin_ringbuff_start = 0;
uint8_t lin_ringbuff_end = 0;
#define lin_ringbuff_len \
(((sizeof(lin_ringbuff) + lin_ringbuff_end) - lin_ringbuff_start)% sizeof(lin_ringbuff))
#define lin_ringbuff_get(index) (lin_ringbuff[(lin_ringbuff_start + index) % sizeof(lin_ringbuff)])
#define lin_ringbuff_consume(len) lin_ringbuff_start = ((lin_ringbuff_start + len) % sizeof(lin_ringbuff))
#define lin_ringbuff_clear()\
{lin_ringbuff_start = 0; \
lin_ringbuff_end = 0;}
int ICACHE_FLASH_ATTR elm_LIN_ringbuff_memcmp(uint8_t *data, uint16_t len) {
if(len > lin_ringbuff_len) return 1;
for(int i = 0; i < len; i++)
if(lin_ringbuff_get(i) != data[i]) return 1;
return 0; // Going with memcpy ret format where 0 means 'equal'
}
uint16_t ICACHE_FLASH_ATTR elm_LIN_read_into_ringbuff() {
int bytelen = panda_usbemu_kline_read((sizeof(lin_ringbuff) - lin_ringbuff_len) - 1);
if(bytelen < 0) return 0;
for(int j = 0; j < bytelen; j++) {
lin_ringbuff[lin_ringbuff_end % sizeof(lin_ringbuff)] = ((char*)(pandaRecvData+1))[j];
lin_ringbuff_end = (lin_ringbuff_end + 1) % sizeof(lin_ringbuff);
if(lin_ringbuff_start == lin_ringbuff_end) lin_ringbuff_start++;
}
#ifdef ELM_DEBUG
if(bytelen){
os_printf(" RB Data (%d %d %d): ", lin_ringbuff_start, lin_ringbuff_end, lin_ringbuff_len);
for(int i = 0; i < sizeof(lin_ringbuff); i++)
os_printf("%02x ", lin_ringbuff[i]);
os_printf("\n");
}
#endif
return bytelen;
}
/****************************************
*** String parsing utility functions ***
****************************************/
static int8_t ICACHE_FLASH_ATTR elm_decode_hex_char(char b){
if(b >= '0' && b <= '9') return b - '0';
if(b >= 'A' && b <= 'F') return (b - 'A') + 10;
if(b >= 'a' && b <= 'f') return (b - 'a') + 10;
return -1;
}
static uint8_t ICACHE_FLASH_ATTR elm_decode_hex_byte(const char* data) {
return (elm_decode_hex_char(data[0]) << 4) | elm_decode_hex_char(data[1]);
}
static bool ICACHE_FLASH_ATTR elm_check_valid_hex_chars(const char* data, uint8_t len) {
for(int i = 0; i < len; i++){
char b = data[i];
if(!((b >= '0' && b <= '9') || (b >= 'A' && b <= 'F') || (b >= 'a' && b <= 'f')))
return 0;
}
return 1;
}
static uint16_t ICACHE_FLASH_ATTR elm_strip(const char *data, uint16_t lenin,
char *outbuff, uint16_t outbufflen) {
uint16_t count = 0;
for(uint16_t i = 0; i < lenin; i++) {
if(count >= outbufflen) break;
if(data[i] == ' ') continue;
if(data[i] >= 'a' && data[i] <= 'z'){
outbuff[count++] = data[i] - ('a' - 'A');
} else {
outbuff[count++] = data[i];
}
if(data[i] == '\r') break;
}
return count;
}
static int ICACHE_FLASH_ATTR elm_msg_find_cr_or_eos(char *data, uint16_t len){
uint16_t i;
for(i = 0; i < len; i++)
if(data[i] == '\r') {
i++;
break;
}
return i;
}
/*****************************************************
*** ELM protocol specification and implementation ***
*****************************************************/
typedef enum {
AUTO, LIN, CAN11, CAN29, NA
} elm_proto_type_t;
typedef struct elm_protocol {
bool supported;
elm_proto_type_t type;
uint16_t cbaud; //Centibaud (cbaud * 10 = kbaud)
void (*process_obd)(const struct elm_protocol*, const char*, uint16_t);
//init is used to init and de-init a protocol. Init functions should
//not do things that would leave a new protocol in an invalid state
//after the new protocol's init is called (e.g. No arming timers).
void (*init)(const struct elm_protocol*);
char* name;
} elm_protocol_t;
static const elm_protocol_t* ICACHE_FLASH_ATTR elm_current_proto();
void ICACHE_FLASH_ATTR elm_reset_aux_timer();
static void ICACHE_FLASH_ATTR elm_autodetect_cb(bool);
static const elm_protocol_t elm_protocols[];
//(sizeof(elm_protocols)/sizeof(elm_protocol_t))
#define ELM_PROTOCOL_COUNT 13
#define LOOPCOUNT_FULL 4
static int loopcount = 0;
static volatile os_timer_t elm_timeout;
static volatile os_timer_t elm_proto_aux_timeout;
static bool is_auto_detecting = false;
// Used only by elm_timer_cb, so not volatile
static bool did_multimessage = false;
static bool got_msg_this_run = false;
static bool can_tx_worked = false;
static uint8_t elm_msg_mode_ret_filter;
static uint8_t elm_msg_pid_ret_filter;
/*****************************************************
*** ELM protocol specification and implementation ***
*** -> SAE J1850 implementation (Unsupported) ***
*****************************************************/
static void ICACHE_FLASH_ATTR elm_process_obd_cmd_J1850(const elm_protocol_t* proto,
const char *cmd, uint16_t len) {
elm_append_rsp_const("NO DATA\r\r>");
}
/*****************************************************
*** ELM protocol specification and implementation ***
*** -> ISO 14230-4 implementation ***
*****************************************************/
const char *lin_cmd_backup = NULL; //Holds msg while bus init is done
uint16_t lin_cmd_backup_len = 0;
bool lin_waiting_keepalive_echo = false;
static void ICACHE_FLASH_ATTR elm_process_obd_cmd_LIN5baud(const elm_protocol_t* proto,
const char *cmd, uint16_t len) {
elm_append_rsp_const("BUS INIT: ...ERROR\r\r>");
}
bool ICACHE_FLASH_ATTR elm_lin_keepalive_echo() {
if(lin_waiting_keepalive_echo) {
for(int pass = 0; pass < 4 && lin_ringbuff_len < 5; pass++) {
elm_LIN_read_into_ringbuff();
}
lin_waiting_keepalive_echo = false;
//keepalive Echo should always come before other message echo.
if(lin_ringbuff_len >= 5 && !elm_LIN_ringbuff_memcmp("\xc1\x33\xf1\x3e\x23", 5)){
lin_ringbuff_consume(5);
return true;
} else {
os_printf("Keep alive echo failed\n");
return false;
}
}
return true;
}
void ICACHE_FLASH_ATTR elm_LINFast_keepalive_timer_cb(void *arg) {
if(!lin_bus_initialized) {
os_printf("WARNING! Elm LIN keepalive timer running while bus is not initialized\n");
return;
}
if(loopcount) {
os_printf("WARNING! Elm LIN keepalive timer during a tx/rx loop!\n");
return;
}
if(lin_ringbuff_len) {
os_printf("WARNING! lin_ringbuff_len should be 0 when a keepalive echo is processed.\n");
return;
}
if(!elm_lin_keepalive_echo()) {
lin_bus_initialized = false;
return;
}
elm_lin_obd_msg msg = {};
msg.priority = 0xC0 | 1;
msg.receiver = 0x33;
msg.sender = 0xF1;
msg.dat[0] = 0x3E;
msg.dat[1] = msg.dat[0] + msg.priority + msg.receiver + msg.sender; // checksum
#ifdef ELM_DEBUG
os_printf("Sending LIN KEEPALIVE: Priority: %02x; RecvAddr: %02x; SendAddr: %02x; (%02x); ",
msg.priority, msg.receiver, msg.sender, 1);
for(int i = 0; i < 2; i++) os_printf("%02x ", msg.dat[i]);
os_printf("\n");
#endif
lin_waiting_keepalive_echo = true;
panda_usbemu_kline_write(&msg);
elm_reset_aux_timer();
}
static void ICACHE_FLASH_ATTR elm_init_LINFast(const elm_protocol_t* proto){
os_timer_disarm(&elm_proto_aux_timeout);
os_timer_setfn(&elm_proto_aux_timeout, (os_timer_func_t *)elm_LINFast_keepalive_timer_cb, proto);
lin_bus_initialized = false;
lin_await_msg_echo = false;
lin_waiting_keepalive_echo = false;
lin_cmd_backup = NULL;
lin_cmd_backup_len = 0;
lin_ringbuff_clear();
panda_clear_lin_txrx();
}
int ICACHE_FLASH_ATTR elm_LINFast_process_echo() {
if(!elm_lin_keepalive_echo()) {
os_printf("Keepalive echo not detected.\n");
lin_ringbuff_clear();
return -1;
}
if(!lin_await_msg_echo) {
os_printf("Echo abort. Nothing waiting echo\n");
return 1;
}
for(int i = 0; i < 4; i++){
if(lin_ringbuff_len < lin_last_sent_msg_len) elm_LIN_read_into_ringbuff();
if(lin_ringbuff_len >= lin_last_sent_msg_len){
#ifdef ELM_DEBUG
os_printf("Got enough data %d\n", lin_last_sent_msg_len);
#endif
if(!elm_LIN_ringbuff_memcmp((uint8_t*)&lin_last_sent_msg, lin_last_sent_msg_len)) {
#ifdef ELM_DEBUG
os_printf("LIN data was sent successfully.\n");
#endif
lin_ringbuff_consume(lin_last_sent_msg_len);
lin_await_msg_echo = false;
return 1;
} else {
#ifdef ELM_DEBUG
os_printf("Echo not correct.\n");
os_printf(" RB Data (%d %d %d): ", lin_ringbuff_start, lin_ringbuff_end, lin_ringbuff_len);
for(int i = 0; i < sizeof(lin_ringbuff); i++)
os_printf("%02x ", lin_ringbuff[i]);
os_printf("\n");
os_printf(" MSG Data (%d): ", lin_last_sent_msg_len);
for(int i = 0; i < lin_last_sent_msg_len; i++)
os_printf("%02x ", ((uint8_t*)&lin_last_sent_msg)[i]);
os_printf("\n");
#endif
if(lin_bus_initialized || loopcount == 0 && i == 4) {
lin_ringbuff_clear();
return -1;
} else {
os_printf("Lin init echo misaligned? Consuming byte (%02x). Retry.\n", lin_ringbuff_get(0));
lin_ringbuff_consume(1);
continue;
}
}
}
}
return !lin_await_msg_echo; //true if echo handled
}
void ICACHE_FLASH_ATTR elm_LINFast_timer_cb(void *arg){
const elm_protocol_t* proto = (const elm_protocol_t*) arg;
loopcount--;
#ifdef ELM_DEBUG
os_printf("LIN CB call\n");
#endif
if(!lin_bus_initialized) {
os_printf("WARNING: LIN CB called without bus initialized!");
return; // TODO: shoulnd't ever happen. Handle?
}
int echo_result = elm_LINFast_process_echo();
if(echo_result == -1 || (echo_result == 0 && loopcount == 0)) {
if(!is_auto_detecting){
elm_append_rsp_const("BUS ERROR\r\r>");
elm_tcp_tx_flush();
}
loopcount = 0;
lin_bus_initialized = false;
return;
}
if(echo_result == 0) {
#ifdef ELM_DEBUG
os_printf("Not ready to process\n");
#endif
os_timer_arm(&elm_timeout, 30, 0);
return; // Not ready to go on
}
#ifdef ELM_DEBUG
os_printf("Processing ELM %d\n", lin_ringbuff_len);
#endif
if(loopcount>0) {
for(int pass = 0; pass < 16 && loopcount; pass++){
elm_LIN_read_into_ringbuff();
while(lin_ringbuff_len > 0){
//if(lin_ringbuff_len > 0){
if(lin_ringbuff_get(0) & 0x80 != 0x80){
os_printf("Resetting LIN bus due to bad first byte.\n");
loopcount = 0;
lin_bus_initialized = false;
lin_ringbuff_clear();
if(!is_auto_detecting){
elm_append_rsp_const("ERROR\r\r>");
elm_tcp_tx_flush();
}
return;
}
uint8_t newmsg_len = 4 + (lin_ringbuff_get(0) & 0x7);
if(lin_ringbuff_len >= newmsg_len) {
#ifdef ELM_DEBUG
os_printf("Processing LIN MSG. BuffLen %d; expect %d. Dat: ", lin_ringbuff_len, newmsg_len);
for(int i = 0; i < newmsg_len; i++) os_printf("%02x ", lin_ringbuff_get(i));
os_printf("\n");
#endif
got_msg_this_run = true;
loopcount = LOOPCOUNT_FULL;
if(!is_auto_detecting){
if(elm_mode_additional_headers){
for(int i = 0; i < newmsg_len; i++) elm_append_rsp_hex_byte(lin_ringbuff_get(i));
} else {
for(int i = 3; i < newmsg_len - 1; i++) elm_append_rsp_hex_byte(lin_ringbuff_get(i));
}
elm_append_rsp_const("\r");
}
lin_ringbuff_consume(newmsg_len);
//elm_reset_aux_timer();
} else {
break; //Stop consuming data if there is not enough data for the next msg.
}
}
}
os_timer_arm(&elm_timeout, 50, 0);
} else {
bool got_msg_this_run_backup = got_msg_this_run;
if(!got_msg_this_run) {
#ifdef ELM_DEBUG
os_printf(" No data collected\n");
#endif
if(!is_auto_detecting) {
elm_append_rsp_const("NO DATA\r");
}
}
got_msg_this_run = false;
if(!is_auto_detecting) {
elm_append_rsp_const("\r>");
elm_tcp_tx_flush();
} else {
elm_autodetect_cb(got_msg_this_run_backup);
}
//TX RX over, resume Keepalive timer
elm_reset_aux_timer();
}
}
void ICACHE_FLASH_ATTR elm_LINFast_businit_timer_cb(void *arg){
const elm_protocol_t* proto = (const elm_protocol_t*) arg;
loopcount--;
#ifdef ELM_DEBUG
os_printf("LIN INIT CB call\n");
#endif
int echo_result = elm_LINFast_process_echo();
if(echo_result == -1 || (echo_result == 0 && loopcount == 0)) {
#ifdef ELM_DEBUG
os_printf("Init failed with echo test\n");
#endif
loopcount = 0;
lin_bus_initialized = 0;
if(!is_auto_detecting){
if(echo_result == -1)
elm_append_rsp_const("BUS ERROR\r\r>");
else
elm_append_rsp_const("ERROR\r\r>");
elm_tcp_tx_flush();
} else {
elm_autodetect_cb(false);
}
return;
}
if(echo_result == 0) {
#ifdef ELM_DEBUG
os_printf("Not ready to process\n");
#endif
os_timer_arm(&elm_timeout, elm_mode_timeout, 0);
return; // Not ready to go on
}
#ifdef ELM_DEBUG
os_printf("Bus init ready to process %d bytes\n", lin_ringbuff_len);
#endif
if(lin_bus_initialized) return; // TODO: shoulnd't ever happen. Handle?
if(loopcount>0) {
//Keep waiting for response
for(int i = 0; i < 4; i++){
elm_LIN_read_into_ringbuff();
if(lin_ringbuff_len > 0){
if(lin_ringbuff_get(0) & 0x80 != 0x80){
os_printf("Resetting LIN bus due to bad first byte.\n");
loopcount = 0;
lin_ringbuff_clear();
if(!is_auto_detecting){
elm_append_rsp_const("ERROR\r\r>");
elm_tcp_tx_flush();
} else {
elm_autodetect_cb(false);
}
return;
}
uint8_t newmsg_len = 4 + (lin_ringbuff_get(0) & 0x7);
if(lin_ringbuff_len < newmsg_len) {
os_printf("Resetting LIN because returned init data was wrong.\n");
loopcount = 0;
lin_ringbuff_clear();
if(!is_auto_detecting){
elm_append_rsp_const("ERROR\r\r>");
elm_tcp_tx_flush();
} else {
elm_autodetect_cb(false);
}
return;
}
if(!elm_LIN_ringbuff_memcmp("\x83\xF1\x10\xC1\x8F\xE9\xBD", 7)) {
lin_ringbuff_consume(7);
lin_bus_initialized = true;
//lin_ringbuff_clear();
os_printf("BUS INITIALIZED\n");
elm_reset_aux_timer();
if(!is_auto_detecting) {
elm_append_rsp_const("OK\r");
//Do the send that was delayed
if(lin_cmd_backup_len) {
elm_tcp_tx_flush();
proto->process_obd(proto, lin_cmd_backup, lin_cmd_backup_len);
} else {
elm_append_rsp_const("\r>");
elm_tcp_tx_flush();
}
} else {
#ifdef ELM_DEBUG
os_printf("LIN success. Silent because in autodetect.\n");
#endif
elm_autodetect_cb(true);
// TODO: Since bus init is good, is it ok to skip sending the '0100' msg?
}
return;
}
}
}
os_timer_arm(&elm_timeout, elm_mode_timeout, 0);
} else {
#ifdef ELM_DEBUG
os_printf("Fall through on bus init\n");
#endif
if(!is_auto_detecting){
elm_append_rsp_const("ERROR\r\r>");
elm_tcp_tx_flush();
} else {
elm_autodetect_cb(false);
}
elm_reset_aux_timer();
}
}
static void ICACHE_FLASH_ATTR elm_process_obd_cmd_LINFast(const elm_protocol_t* proto,
const char *cmd, uint16_t len) {
elm_lin_obd_msg msg = {};
uint8_t bytelen = (len-1)/2;
if((bytelen > 7 && !elm_mode_allow_long) || bytelen > 8) {
elm_append_rsp_const("?\r\r>");
return;
}
os_timer_disarm(&elm_proto_aux_timeout);
if(!lin_bus_initialized) {
panda_clear_lin_txrx();
if(!is_auto_detecting)
elm_append_rsp_const("BUS INIT: ");
lin_cmd_backup = cmd;
lin_cmd_backup_len = len;
bytelen = 1;
msg.dat[0] = 0x81;
msg.dat[1] = 0x81; // checksum
panda_kline_wakeup_pulse();
} else {
bytelen = MIN(bytelen, 7);
for(int i = 0; i < bytelen; i++){
msg.dat[i] = elm_decode_hex_byte(&cmd[i*2]);
msg.dat[bytelen] += msg.dat[i];
}
elm_msg_mode_ret_filter = msg.dat[0];
elm_msg_pid_ret_filter = msg.dat[1];
}
msg.priority = 0xC0 | bytelen;
msg.receiver = 0x33;
msg.sender = 0xF1;
msg.dat[bytelen] += msg.priority + msg.receiver + msg.sender; // checksum
#ifdef ELM_DEBUG
os_printf("Sending LIN OBD: Priority: %02x; RecvAddr: %02x; SendAddr: %02x; (%02x); ",
msg.priority, msg.receiver, msg.sender, bytelen);
for(int i = 0; i < 8; i++) os_printf("%02x ", msg.dat[i]);
os_printf("\n");
#endif
lin_last_sent_msg_len = (msg.priority & 0x07) + 4;
memcpy(&lin_last_sent_msg, &msg, lin_last_sent_msg_len);
lin_await_msg_echo = true;
panda_usbemu_kline_write(&msg);
loopcount = LOOPCOUNT_FULL + 1;
os_timer_disarm(&elm_timeout);
if(lin_bus_initialized) {
os_timer_setfn(&elm_timeout, (os_timer_func_t *)elm_LINFast_timer_cb, proto);
elm_LINFast_timer_cb((void*)proto);
} else {
os_timer_setfn(&elm_timeout, (os_timer_func_t *)elm_LINFast_businit_timer_cb, proto);
elm_LINFast_businit_timer_cb((void*)proto);
}
}
/*****************************************************
*** ELM protocol specification and implementation ***
*** -> ISO 15765-4 implementation ***
*****************************************************/
void ICACHE_FLASH_ATTR elm_ISO15765_timer_cb(void *arg){
const elm_protocol_t* proto = (const elm_protocol_t*) arg;
loopcount--;
if(loopcount>0) {
for(int pass = 0; pass < 16 && loopcount; pass++){
panda_can_msg_t *can_msgs;
int num_can_msgs = panda_usbemu_can_read(&can_msgs);
#ifdef ELM_DEBUG
if(num_can_msgs) os_printf(" Received %d can messages\n", num_can_msgs);
#endif
if(num_can_msgs < 0) continue;
if(!num_can_msgs) break;
for(int i = 0; i < num_can_msgs; i++){
panda_can_msg_t *recv = &can_msgs[i];
#ifdef ELM_DEBUG
os_printf(" RECV: Bus: %d; Addr: %08x; ext: %d; tx: %d; Len: %d; ",
recv->bus, panda_get_can_addr(recv), recv->ext, recv->tx, recv->len);
for(int j = 0; j < recv->len; j++) os_printf("%02x ", recv->data[j]);
os_printf("Ts: %d\n", recv->ts);
#endif
if (recv->bus==0 && recv->len == 8 &&
(
(proto->type == CAN11 && !recv->ext && (panda_get_can_addr(recv) & 0x7F8) == 0x7E8) ||
(proto->type == CAN29 && recv->ext && (panda_get_can_addr(recv) & 0x1FFFFF00) == 0x18DAF100)
)
) {
if(recv->data[0] <= 7 &&
recv->data[1] == (0x40|elm_msg_mode_ret_filter) &&
recv->data[2] == elm_msg_pid_ret_filter) {
got_msg_this_run = true;
loopcount = LOOPCOUNT_FULL;
#ifdef ELM_DEBUG
os_printf(" CAN msg response, index: %d\n", i);
#endif
if(!is_auto_detecting){
if(elm_mode_additional_headers){
elm_append_rsp_can_msg_addr(recv);
for(int j = 0; j < recv->data[0]+1; j++) elm_append_rsp_hex_byte(recv->data[j]);
} else {
for(int j = 1; j < recv->data[0]+1; j++) elm_append_rsp_hex_byte(recv->data[j]);
}
elm_append_rsp_const("\r");
elm_tcp_tx_flush();
}
} else if((recv->data[0] & 0xF0) == 0x10 &&
recv->data[2] == (0x40|elm_msg_mode_ret_filter) &&
recv->data[3] == elm_msg_pid_ret_filter) {
got_msg_this_run = true;
loopcount = LOOPCOUNT_FULL;
panda_usbemu_can_write(0,
(proto->type==CAN11) ?
0x7E0 | (panda_get_can_addr(recv)&0x7) :
(0x18DA00F1 | (((panda_get_can_addr(recv))&0xFF)<<8)),
"\x30\x00\x00", 3);
did_multimessage = true;
#ifdef ELM_DEBUG
os_printf(" CAN multimsg start response, index: %d, len %d\n", i,
((recv->data[0]&0xF)<<8) | recv->data[1]);
#endif
if(!is_auto_detecting){
if(!elm_mode_additional_headers) {
elm_append_rsp(&hex_lookup[recv->data[0]&0xF], 1);
elm_append_rsp_hex_byte(recv->data[1]);
elm_append_rsp_const("\r0:");
if(elm_mode_print_spaces) elm_append_rsp_const(" ");
for(int j = 2; j < 8; j++) elm_append_rsp_hex_byte(recv->data[j]);
} else {
elm_append_rsp_can_msg_addr(recv);
for(int j = 0; j < 8; j++) elm_append_rsp_hex_byte(recv->data[j]);
}
elm_append_rsp_const("\r");
elm_tcp_tx_flush();
}
} else if (did_multimessage && (recv->data[0] & 0xF0) == 0x20) {
got_msg_this_run = true;
loopcount = LOOPCOUNT_FULL;
#ifdef ELM_DEBUG
os_printf(" CAN multimsg data response, index: %d\n", i);
#endif
if(!is_auto_detecting){
if(!elm_mode_additional_headers) {
elm_append_rsp(&hex_lookup[recv->data[0] & 0xF], 1);
elm_append_rsp_const(":");
if(elm_mode_print_spaces) elm_append_rsp_const(" ");
for(int j = 1; j < 8; j++) elm_append_rsp_hex_byte(recv->data[j]);
} else {
elm_append_rsp_can_msg_addr(recv);
for(int j = 0; j < 8; j++) elm_append_rsp_hex_byte(recv->data[j]);
}
elm_append_rsp_const("\r");
}
}
} else if (recv->bus == 0x80 && recv->len == 8 &&
(panda_get_can_addr(recv) == ((proto->type==CAN11) ? 0x7DF : 0x18DB33F1))
) {
//Can send receipt
#ifdef ELM_DEBUG
os_printf(" Got CAN tx receipt\n");
#endif
can_tx_worked = true;
}
}
}
os_timer_arm(&elm_timeout, elm_mode_timeout, 0);
} else {
bool got_msg_this_run_backup = got_msg_this_run;
if(did_multimessage) {
os_printf(" End of multi message\n");
} else if(!got_msg_this_run) {
os_printf(" No data collected\n");
if(!is_auto_detecting) {
if(can_tx_worked) {
elm_append_rsp_const("NO DATA\r");
} else {
elm_append_rsp_const("CAN ERROR\r");
}
}
}
did_multimessage = false;
got_msg_this_run = false;
can_tx_worked = false;
if(!is_auto_detecting) {
elm_append_rsp_const("\r>");
elm_tcp_tx_flush();
} else {
elm_autodetect_cb(got_msg_this_run_backup);
}
}
}
static void ICACHE_FLASH_ATTR elm_init_ISO15765(const elm_protocol_t* proto){
panda_set_can0_cbaud(proto->cbaud);
}
static void ICACHE_FLASH_ATTR elm_process_obd_cmd_ISO15765(const elm_protocol_t* proto,
const char *cmd, uint16_t len) {
elm_can_obd_msg msg = {};
msg.len = (len-1)/2;
if((msg.len > 7 && !elm_mode_allow_long) || msg.len > 8) {
elm_append_rsp_const("?\r\r>");
return;
}
msg.len = MIN(msg.len, 7);
for(int i = 0; i < msg.len; i++)
msg.dat[i] = elm_decode_hex_byte(&cmd[i*2]);
elm_msg_mode_ret_filter = msg.dat[0];
elm_msg_pid_ret_filter = msg.dat[1];
#ifdef ELM_DEBUG
os_printf("Sending CAN OBD: %02x; ", msg.len);
for(int i = 0; i < 7; i++)
os_printf("%02x ", msg.dat[i]);
os_printf("\n");
#endif
panda_clear_can_rx();
panda_usbemu_can_write(0, (proto->type==CAN11) ? 0x7DF : 0x18DB33F1,
(uint8_t*)&msg, msg.len+1);
#ifdef ELM_DEBUG
os_printf("Starting up timer\n");
#endif
loopcount = LOOPCOUNT_FULL;
os_timer_disarm(&elm_timeout);
os_timer_setfn(&elm_timeout, (os_timer_func_t *)elm_ISO15765_timer_cb, proto);
os_timer_arm(&elm_timeout, elm_mode_timeout, 0);
}
/*****************************************************
*** ELM protocol specification and implementation ***
*** -> Stuf for unsupported CAN protocols ***
*****************************************************/
static void ICACHE_FLASH_ATTR elm_process_obd_cmd_CANGen(const elm_protocol_t* proto,
const char *cmd, uint16_t len) {
elm_append_rsp_const("NO DATA\r\r>");
}
/*****************************************************
*** ELM protocol specification and implementation ***
*** -> AUTO Detect implementation ***
*****************************************************/
static int elm_autodetect_proto_iter;
static uint16_t elm_staged_auto_msg_len;
static const char* elm_staged_auto_msg;
static void ICACHE_FLASH_ATTR elm_autodetect_cb(bool proto_worked){
if(proto_worked) {
os_printf("Autodetect proto success\n");
is_auto_detecting = false;
elm_selected_protocol = elm_autodetect_proto_iter;
elm_current_proto()->process_obd(elm_current_proto(),
elm_staged_auto_msg, elm_staged_auto_msg_len);
} else {
for(elm_autodetect_proto_iter++; elm_autodetect_proto_iter < ELM_PROTOCOL_COUNT;
elm_autodetect_proto_iter++){
const elm_protocol_t *proto = &elm_protocols[elm_autodetect_proto_iter];
if(proto->supported && proto->type != AUTO) {
os_printf("*** AUTO trying '%s'\n", proto->name);
proto->init(proto);
proto->process_obd(proto, "0100\r", 5); // Try sending on the bus
return;
}
}
//if(elm_autodetect_main()) return;
is_auto_detecting = false;
os_printf("Autodetect failed\n");
elm_append_rsp_const("UNABLE TO CONNECT\r\r>");
elm_tcp_tx_flush();
}
}
static void ICACHE_FLASH_ATTR elm_process_obd_cmd_AUTO(const elm_protocol_t* proto,
const char *cmd, uint16_t len) {
elm_append_rsp_const("SEARCHING...\r");
elm_staged_auto_msg_len = len;
elm_staged_auto_msg = cmd;
is_auto_detecting = true;
elm_autodetect_proto_iter = 0;
elm_autodetect_cb(false);
}
/*****************************************************
*** ELM protocol specification and implementation ***
*** -> Protocol Registry and related functions. ***
*****************************************************/
static const elm_protocol_t elm_protocols[] = {
{true, AUTO, 0, elm_process_obd_cmd_AUTO, NULL, "AUTO", },
{false, NA, 416, elm_process_obd_cmd_J1850, NULL, "SAE J1850 PWM", },
{false, NA, 104, elm_process_obd_cmd_J1850, NULL, "SAE J1850 VPW", },
{false, LIN, 104, elm_process_obd_cmd_LIN5baud, NULL, "ISO 9141-2", },
{false, LIN, 104, elm_process_obd_cmd_LIN5baud, NULL, "ISO 14230-4 (KWP 5BAUD)", },
{true, LIN, 104, elm_process_obd_cmd_LINFast, NULL, "ISO 14230-4 (KWP FAST)", },
{true, CAN11, 5000, elm_process_obd_cmd_ISO15765, elm_init_ISO15765, "ISO 15765-4 (CAN 11/500)",},
{true, CAN29, 5000, elm_process_obd_cmd_ISO15765, elm_init_ISO15765, "ISO 15765-4 (CAN 29/500)",},
{true, CAN11, 2500, elm_process_obd_cmd_ISO15765, elm_init_ISO15765, "ISO 15765-4 (CAN 11/250)",},
{true, CAN29, 2500, elm_process_obd_cmd_ISO15765, elm_init_ISO15765, "ISO 15765-4 (CAN 29/250)",},
{false, CAN29, 2500, elm_process_obd_cmd_CANGen, NULL, "SAE J1939 (CAN 29/250)", },
{false, CAN11, 1250, elm_process_obd_cmd_CANGen, NULL, "USER1 (CAN 11/125)", },
{false, CAN11, 500, elm_process_obd_cmd_CANGen, NULL, "USER2 (CAN 11/50)", },
};
static const elm_protocol_t* ICACHE_FLASH_ATTR elm_current_proto() {
return &elm_protocols[elm_selected_protocol];
}
void ICACHE_FLASH_ATTR elm_reset_aux_timer() {
os_timer_disarm(&elm_proto_aux_timeout);
if(elm_mode_keepalive_period)
os_timer_arm(&elm_proto_aux_timeout, elm_mode_keepalive_period, 0);
}
void ICACHE_FLASH_ATTR elm_proto_reinit(const elm_protocol_t *proto) {
if(proto->init) proto->init(proto);
}
/*******************************************
*** ELM AT command parsing and handling ***
*******************************************/
enum at_cmd_ids_t { // FULL ELM 1.0 list
AT_INVALID, //Fake
AT_AMP1,
AT_AL,
AT_AT0, AT_AT1, AT_AT2, // Added ELM 1.2, expected by Torque
AT_BD,
AT_BI,
AT_CAF0, AT_CAF1,
AT_CF_8, AT_CF_3,
AT_CFC0, AT_CFC1,
AT_CM_8, AT_CM_3,
AT_CP,
AT_CS,
AT_CV,
AT_D,
AT_DP, AT_DPN,
AT_E0, AT_E1,
AT_H0, AT_H1,
AT_I,
AT_IB10,
AT_IB96,
AT_L0, AT_L1,
AT_M0, AT_M1, AT_MA,
AT_MR,
AT_MT,
AT_NL,
AT_PC,
AT_R0, AT_R1,
AT_RV,
AT_S0, AT_S1, // Added ELM 1.3, expected by Torque
AT_SH_6, AT_SH_3,
AT_SPA, AT_SP,
AT_ST,
AT_SW,
AT_TPA, AT_TP,
AT_WM_XYZA, AT_WM_XYZAB, AT_WM_XYZABC,
AT_WS,
AT_Z,
};
typedef struct {
char* name;
uint8_t name_len;
uint8_t cmd_len;
enum at_cmd_ids_t id;
} at_cmd_reg_t;
static const at_cmd_reg_t at_cmd_reg[] = {
{"@1", 2, 2, AT_AMP1},
{"AL", 2, 2, AT_AL},
{"AT0", 3, 3, AT_AT0}, // Added ELM 1.2, expected by Torque
{"AT1", 3, 3, AT_AT1}, // Added ELM 1.2, expected by Torque
{"AT2", 3, 3, AT_AT2}, // Added ELM 1.2, expected by Torque
{"DP", 2, 2, AT_DP},
{"DPN", 3, 3, AT_DPN},
{"E0", 2, 2, AT_E0},
{"E1", 2, 2, AT_E1},
{"H0", 2, 2, AT_H0},
{"H1", 2, 2, AT_H1},
{"I", 1, 1, AT_I},
{"L0", 2, 2, AT_L0},
{"L1", 2, 2, AT_L1},
{"M0", 2, 2, AT_M0},
//{"M1", 2, 2, AT_M1},
{"NL", 2, 2, AT_NL},
{"PC", 2, 2, AT_PC},
{"S0", 2, 2, AT_S0}, // Added ELM 1.3, expected by Torque
{"S1", 2, 2, AT_S1}, // Added ELM 1.3, expected by Torque
{"SP", 2, 3, AT_SP},
{"SPA", 3, 4, AT_SPA},
{"ST", 2, 4, AT_ST},
{"SW", 2, 4, AT_SW},
{"Z", 1, 1, AT_Z},
};
#define AT_CMD_REG_LEN (sizeof(at_cmd_reg)/sizeof(at_cmd_reg_t))
static enum at_cmd_ids_t ICACHE_FLASH_ATTR elm_parse_at_cmd(char *cmd, uint16_t len){
int i;
for(i=0; i<AT_CMD_REG_LEN; i++){
at_cmd_reg_t candidate = at_cmd_reg[i];
if(candidate.cmd_len == len-1 && !memcmp(cmd, candidate.name, candidate.name_len))
return candidate.id;
}
return AT_INVALID;
}
static void ICACHE_FLASH_ATTR elm_process_at_cmd(char *cmd, uint16_t len) {
uint8_t tmp;
os_printf("AT COMMAND ");
for(int i = 0; i < len; i++) os_printf("%c", cmd[i]);
os_printf("\r\n");
switch(elm_parse_at_cmd(cmd, len)){
case AT_AMP1: //RETURN DEVICE DESCRIPTION
elm_append_rsp_const(DEVICE_DESC);
return;
case AT_AL: //DISABLE LONG MESSAGE SUPPORT (>7 BYTES)
elm_mode_allow_long = true;
break;
case AT_AT0: //DISABLE ADAPTIVE TIMING
elm_mode_adaptive_timing = 0;
break;
case AT_AT1: //SET ADAPTIVE TIMING TO AUTO1
elm_mode_adaptive_timing = 1;
break;
case AT_AT2: //SET ADAPTIVE TIMING TO AUTO2
elm_mode_adaptive_timing = 2;
break;
case AT_DP: //DESCRIBE THE PROTOCOL BY NAME
if(elm_mode_auto_protocol && elm_selected_protocol != 0)
elm_append_rsp_const("AUTO, ");
elm_append_rsp(elm_current_proto()->name,
strlen(elm_current_proto()->name));
elm_append_rsp_const("\r\r");
return;
case AT_DPN: //DESCRIBE THE PROTOCOL BY NUMBER
//TODO: Required. Report currently selected protocol
if(elm_mode_auto_protocol)
elm_append_rsp_const("A");
elm_append_rsp(&hex_lookup[elm_selected_protocol], 1);
elm_append_rsp_const("\r\r");
return; // Don't display 'OK'
case AT_E0: //ECHO OFF
elm_mode_echo = false;
break;
case AT_E1: //ECHO ON
elm_mode_echo = true;
break;
case AT_H0: //SHOW FULL CAN HEADERS OFF
elm_mode_additional_headers = false;
break;
case AT_H1: //SHOW FULL CAN HEADERS ON
elm_mode_additional_headers = true;
break;
case AT_I: //IDENTIFY SELF
elm_append_rsp_const(IDENT_MSG);
return;
case AT_L0: //LINEFEED OFF
elm_mode_linefeed = false;
break;
case AT_L1: //LINEFEED ON
elm_mode_linefeed = true;
break;
case AT_M0: //DISABLE NONVOLATILE STORAGE
//Memory storage is likely unnecessary
break;
case AT_NL: //DISABLE LONG MESSAGE SUPPORT (>7 BYTES)
elm_mode_allow_long = false;
break;
case AT_PC: //PROTOCOL CANCEL (Stop timers and stuff)
{
//Init functions should idenpotently prepare the protocol to be used.
//Thus, the init function can be used as a protocol cancel function
elm_proto_reinit(elm_current_proto());
break;
}
case AT_S0: //DISABLE PRINTING SPACES IN ECU RESPONSES
elm_mode_print_spaces = false;
break;
case AT_S1: //ENABLE PRINTING SPACES IN ECU RESPONSES
elm_mode_print_spaces = true;
break;
case AT_SP: //SET PROTOCOL
tmp = elm_decode_hex_char(cmd[2]);
if(tmp == -1 || tmp >= ELM_PROTOCOL_COUNT) {
elm_append_rsp_const("?\r\r");
return;
}
//De-Init previous protocol
elm_proto_reinit(elm_current_proto());
elm_selected_protocol = tmp;
elm_mode_auto_protocol = (tmp == 0);
//Init new protocol
elm_proto_reinit(elm_current_proto());
break;
case AT_SPA: //SET PROTOCOL WITH AUTO FALLBACK
tmp = elm_decode_hex_char(cmd[3]);
if(tmp == -1 || tmp >= ELM_PROTOCOL_COUNT) {
elm_append_rsp_const("?\r\r");
return;
}
//De-Init previous protocol
elm_proto_reinit(elm_current_proto());
elm_selected_protocol = tmp;
elm_mode_auto_protocol = true;
//Init new protocol
elm_proto_reinit(elm_current_proto());
break;
case AT_ST: //SET TIMEOUT
if(!elm_check_valid_hex_chars(&cmd[2], 2)) {
elm_append_rsp_const("?\r\r");
return;
}
tmp = elm_decode_hex_byte(&cmd[2]);
//20 for CAN, 4 for LIN
elm_mode_timeout = tmp ? tmp*20 : ELM_MODE_TIMEOUT_DEFAULT;
break;
case AT_SW: //SET WAKEUP TIME INTERVAL
if(!elm_check_valid_hex_chars(&cmd[2], 2)) {
elm_append_rsp_const("?\r\r");
return;
}
tmp = elm_decode_hex_byte(&cmd[2]);
elm_mode_keepalive_period = tmp ? MAX(tmp, 0x20) * 20 : 0;
if(lin_bus_initialized){
os_timer_disarm(&elm_proto_aux_timeout);
if(elm_mode_keepalive_period)
os_timer_arm(&elm_proto_aux_timeout, elm_mode_keepalive_period, 0);
}
break;
case AT_Z: //RESET
elm_mode_echo = true;
elm_mode_linefeed = false;
elm_mode_additional_headers = false;
elm_mode_auto_protocol = true;
elm_selected_protocol = ELM_MODE_SELECTED_PROTOCOL_DEFAULT;
elm_mode_print_spaces = true;
elm_mode_adaptive_timing = 1;
elm_mode_allow_long = false;
elm_mode_timeout = ELM_MODE_TIMEOUT_DEFAULT;
elm_mode_keepalive_period = ELM_MODE_KEEPALIVE_PERIOD_DEFAULT;
elm_append_rsp_const("\r\r");
elm_append_rsp_const(IDENT_MSG);
panda_set_safety_mode(SAFETY_ELM327);
elm_proto_reinit(elm_current_proto());
return;
default:
elm_append_rsp_const("?\r\r");
return;
}
elm_append_rsp_const("OK\r\r");
}
/*************************************
*** Connection and cli management ***
*************************************/
static void ICACHE_FLASH_ATTR elm_append_in_msg(char *data, uint16_t len) {
if(in_msg_len + len > sizeof(in_msg))
len = sizeof(in_msg) - in_msg_len;
memcpy(in_msg + in_msg_len, data, len);
in_msg_len += len;
}
static int ICACHE_FLASH_ATTR elm_msg_is_at_cmd(char *data, uint16_t len){
return len >= 4 && data[0] == 'A' && data[1] == 'T';
}
static void ICACHE_FLASH_ATTR elm_rx_cb(void *arg, char *data, uint16_t len) {
#ifdef ELM_DEBUG
//os_printf("\nGot ELM Data In: '%s'\n", data);
#endif
rsp_buff_len = 0;
len = elm_msg_find_cr_or_eos(data, len);
if(loopcount){
os_timer_disarm(&elm_timeout);
loopcount = 0;
got_msg_this_run = false;
can_tx_worked = false;
did_multimessage = false;
os_printf("Interrupting operation, stopping timer. msg len: %d\n", len);
elm_append_rsp_const("STOPPED\r\r>");
if(len == 1 && data[0] == '\r') {
os_printf("Empty msg source of interrupt.\n");
elm_tcp_tx_flush();
return;
}
}
if(!(len == 1 && data[0] == '\r') && in_msg_len && in_msg[in_msg_len-1] == '\r'){
in_msg_len = 0;
}
if(!(len == 1 && data[0] == '\r' && in_msg_len && in_msg[in_msg_len-1] == '\r')) {
// Not Repeating last message
elm_append_in_msg(data, len); //Aim to remove this memcpy
}
if(elm_mode_echo)
elm_append_rsp(in_msg, in_msg_len);
if(in_msg_len > 0 && in_msg[in_msg_len-1] == '\r') { //Got a full line
stripped_msg_len = elm_strip(in_msg, in_msg_len, stripped_msg, sizeof(stripped_msg));
if(elm_msg_is_at_cmd(stripped_msg, stripped_msg_len)) {
elm_process_at_cmd(stripped_msg+2, stripped_msg_len-2);
elm_append_rsp_const(">");
} else if(elm_check_valid_hex_chars(stripped_msg, stripped_msg_len - 1)) {
elm_current_proto()->process_obd(elm_current_proto(), stripped_msg, stripped_msg_len);
} else {
elm_append_rsp_const("?\r\r>");
}
}
elm_tcp_tx_flush();
//Just clear the buffer if full with no termination
if(in_msg_len == sizeof(in_msg) && in_msg[in_msg_len-1] != '\r')
in_msg_len = 0;
}
void ICACHE_FLASH_ATTR elm_tcp_disconnect_cb(void *arg){
struct espconn *pesp_conn = (struct espconn *)arg;
elm_tcp_conn_t * prev = NULL;
for(elm_tcp_conn_t *iter = connection_list; iter != NULL; iter=iter->next){
struct espconn *conn = iter->conn;
//SHOW_CONNECTION("Considering Disconnecting", conn);
if(!memcmp(pesp_conn->proto.tcp->remote_ip, conn->proto.tcp->remote_ip, 4) &&
pesp_conn->proto.tcp->remote_port == conn->proto.tcp->remote_port){
os_printf("Deleting ELM Connection!\n");
if(prev){
prev->next = iter->next;
} else {
connection_list = iter->next;
}
os_free(iter);
break;
}
prev = iter;
}
if(connection_list == NULL) {
//If all clients are disconnected, reset the protocol (cancels
//keep alive timers). This will not detect inproperly killed
//connections. In this case, periodic events associated with the
//current protocol will continue until a new client attaches, a
//command is sent generating a response (ELM will try to responde
//to the dead connection, and remove it upon error), and finally,
//the new client disconnects. OFC a power cycle is also an option.
elm_proto_reinit(elm_current_proto());
}
}
void ICACHE_FLASH_ATTR elm_tcp_connect_cb(void *arg) {
struct espconn *pesp_conn = (struct espconn *)arg;
//SHOW_CONNECTION("New connection", pesp_conn);
espconn_set_opt(&elm_conn, ESPCONN_NODELAY);
espconn_regist_recvcb(pesp_conn, elm_rx_cb);
//Allow several sends to be queued at a time.
espconn_tcp_set_buf_count(pesp_conn, 3);
bool connection_address_already_there = false;
for(elm_tcp_conn_t *iter2 = connection_list; iter2 != NULL; iter2 = iter2->next)
if(iter2->conn == pesp_conn){connection_address_already_there = true; break;}
if(connection_address_already_there) {
os_printf("ELM WIFI: Memory reuse of recently killed connection\n");
} else {
os_printf("ELM WIFI: Adding connection\n");
elm_tcp_conn_t *newconn = os_malloc(sizeof(elm_tcp_conn_t));
if(!newconn) {
os_printf("Failed to allocate place for connection\n");
} else {
newconn->next = connection_list;
newconn->conn = pesp_conn;
connection_list = newconn;
}
}
}
void ICACHE_FLASH_ATTR elm327_init() {
// control listener
elm_proto.local_port = ELM_PORT;
elm_conn.type = ESPCONN_TCP;
elm_conn.state = ESPCONN_NONE;
elm_conn.proto.tcp = &elm_proto;
espconn_regist_connectcb(&elm_conn, elm_tcp_connect_cb);
espconn_regist_disconcb(&elm_conn, elm_tcp_disconnect_cb);
espconn_accept(&elm_conn);
espconn_regist_time(&elm_conn, 0, 0); // 60s timeout for all connections
}
Reference in New Issue View Git Blame Copy Permalink