diff --git a/SConstruct b/SConstruct index 78ca36a4c..d9b801454 100644 --- a/SConstruct +++ b/SConstruct @@ -410,6 +410,9 @@ SConscript(['selfdrive/locationd/SConscript']) SConscript(['selfdrive/sensord/SConscript']) SConscript(['selfdrive/ui/SConscript']) +if arch == "aarch64": + SConscript(['selfdrive/hardware/eon/SConscript']) + if arch != "Darwin": SConscript(['selfdrive/logcatd/SConscript']) diff --git a/selfdrive/hardware/.gitignore b/selfdrive/hardware/.gitignore new file mode 100644 index 000000000..980f09abf --- /dev/null +++ b/selfdrive/hardware/.gitignore @@ -0,0 +1 @@ +eon/rat diff --git a/selfdrive/hardware/eon/SConscript b/selfdrive/hardware/eon/SConscript new file mode 100644 index 000000000..d4d9edb70 --- /dev/null +++ b/selfdrive/hardware/eon/SConscript @@ -0,0 +1,5 @@ +Import('env', 'common') + +lenv = env.Clone() +lenv["CCFLAGS"] = [f for f in env["CCFLAGS"] if f != "-Wunused"] +lenv.Program('rat', ['rat.cc'], LIBS=[common, 'diag', 'time_genoff', 'zmq']) diff --git a/selfdrive/hardware/eon/libdiag.h b/selfdrive/hardware/eon/libdiag.h new file mode 100644 index 000000000..cb8e8f7dc --- /dev/null +++ b/selfdrive/hardware/eon/libdiag.h @@ -0,0 +1,33 @@ +#pragma once + +#include +#include + +extern "C" { + +#define DIAG_MAX_RX_PKT_SIZ 4096 + +bool Diag_LSM_Init(uint8_t* pIEnv); +bool Diag_LSM_DeInit(void); + +// DCI + +#define DIAG_CON_APSS 0x001 +#define DIAG_CON_MPSS 0x002 +#define DIAG_CON_LPASS 0x004 +#define DIAG_CON_WCNSS 0x008 + +enum { + DIAG_DCI_NO_ERROR = 1001, +} diag_dci_error_type; + +int diag_register_dci_client(int*, uint16_t*, int, void*); +int diag_log_stream_config(int client_id, int set_mask, uint16_t log_codes_array[], int num_codes); +int diag_register_dci_stream(void (*func_ptr_logs)(unsigned char *ptr, int len), void (*func_ptr_events)(unsigned char *ptr, int len)); +int diag_release_dci_client(int*); + +int diag_send_dci_async_req(int client_id, unsigned char buf[], int bytes, unsigned char *rsp_ptr, int rsp_len, + void (*func_ptr)(unsigned char *ptr, int len, void *data_ptr), void *data_ptr); + + +} diff --git a/selfdrive/hardware/eon/rat.cc b/selfdrive/hardware/eon/rat.cc new file mode 100644 index 000000000..0a00b3a26 --- /dev/null +++ b/selfdrive/hardware/eon/rat.cc @@ -0,0 +1,183 @@ +#include +#include +#include +#include +#include + +#include "json11.hpp" + +#include "libdiag.h" +#include "selfdrive/common/swaglog.h" + + +#define DIAG_NV_READ_F 38 +#define DIAG_NV_WRITE_F 39 + +#define DIAG_SUBSYS_CMD 75 +#define DIAG_SUBSYS_CMD_VER_2 128 + +#define DIAG_SUBSYS_FS 19 + +#define EFS2_DIAG_SYNC_NO_WAIT 48 + + +struct __attribute__((packed)) NvPacket { + uint8_t cmd_code; + uint16_t nv_id; + uint8_t data[128]; + uint16_t status; +}; + + +enum NvStatus { + NV_DONE, + NV_BUSY, + NV_FULL, + NV_FAIL, + NV_NOTACTIVE, + NV_BADPARAM, + NV_READONLY, + NV_BADRG, + NV_NOMEM, + NV_NOTALLOC, +}; + +struct __attribute__((packed)) Efs2DiagSyncReq { + uint8_t cmd_code; + uint8_t subsys_id; + uint16_t subsys_cmd_code; + uint16_t sequence_num; + char path[8]; +}; + +struct __attribute__((packed)) Efs2DiagSyncResp { + uint8_t cmd_code; + uint8_t subsys_id; + uint16_t subsys_cmd_code; + uint16_t sequence_num; + uint32_t sync_token; + int32_t diag_errno; +}; + + +struct SendDiagSyncState { + sem_t sem; + int len; +}; + +static void diag_send_sync_cb(unsigned char *ptr, int len, void *data_ptr) { + SendDiagSyncState *s = (SendDiagSyncState*)data_ptr; + s->len = len; + sem_post(&s->sem); +} + +static int diag_send_sync(int client_id, unsigned char* req_pkt, size_t pkt_len, + unsigned char* res_pkt, size_t res_pkt_size) { + + SendDiagSyncState s = {0}; + sem_init(&s.sem, 0, 0); + + int err = diag_send_dci_async_req(client_id, req_pkt, pkt_len, res_pkt, res_pkt_size, + diag_send_sync_cb, &s); + assert(err == DIAG_DCI_NO_ERROR); + + sem_wait(&s.sem); + return s.len; +} + +static void efs_sync(int client_id) { + unsigned char res_pkt[DIAG_MAX_RX_PKT_SIZ]; + + Efs2DiagSyncReq req_pkt = { + .cmd_code = DIAG_SUBSYS_CMD, + .subsys_id = DIAG_SUBSYS_FS, + .subsys_cmd_code = EFS2_DIAG_SYNC_NO_WAIT, + .sequence_num = (uint16_t)(rand() % 100), + }; + req_pkt.path[0] = '/'; + req_pkt.path[1] = 0; + + int res_len = diag_send_sync(client_id, (unsigned char*)&req_pkt, sizeof(req_pkt), + res_pkt, sizeof(res_pkt)); + Efs2DiagSyncResp *resp = (Efs2DiagSyncResp*)res_pkt; + + if (res_len != sizeof(Efs2DiagSyncResp) + || resp->cmd_code != DIAG_SUBSYS_CMD + || resp->subsys_id != DIAG_SUBSYS_FS + || resp->subsys_cmd_code != EFS2_DIAG_SYNC_NO_WAIT) { + LOGW("efs_sync: bad response!"); + return; + } + if (resp->diag_errno != 0) { + LOGW("efs_sync: error %d", resp->diag_errno); + } +} + +static int nv_read(int client_id, uint16_t nv_id, NvPacket &resp) { + NvPacket req = { + .cmd_code = DIAG_NV_READ_F, + .nv_id = nv_id, + }; + + int res_len = diag_send_sync(client_id, (unsigned char*)&req, sizeof(req), + (unsigned char*)&resp, sizeof(resp)); + + // hexdump((uint8_t*)&resp, res_len); + + if (resp.cmd_code != DIAG_NV_READ_F || resp.nv_id != nv_id) { + LOGW("nv_read: diag command failed"); + return -1; + } + + if (resp.status != NV_DONE) { + LOGW("nv_read: read failed: %d", resp.status); + return -1; + } + return 0; +} + +static uint32_t nv_read_u32(int client_id, uint16_t nv_id) { + NvPacket resp = {0}; + if (nv_read(client_id, nv_id, resp) < 0) { + return 0; + } + return *(uint32_t*)resp.data; +} + + + +int main() { + int err; + int client_id = 0; + + // setup diag + bool ok = Diag_LSM_Init(NULL); + assert(ok); + + uint16_t list = DIAG_CON_APSS | DIAG_CON_MPSS; + int signal_type = SIGCONT; + err = diag_register_dci_client(&client_id, &list, 0, &signal_type); + assert(err == DIAG_DCI_NO_ERROR); + + // log some stuff + std::map to_log = { + {"B13", 6502}, + {"B7", 6553}, + {"B17", 6606}, + {"B40", 6658}, + {"B1", 6710}, + }; + auto log = json11::Json::object {}; + for (auto const &kv : to_log) { + NvPacket resp = {0}; + nv_read(client_id, kv.second, resp); + log[kv.first] = *(uint8_t*)resp.data; + } + + printf("%s\n", json11::Json(log).dump().c_str()); + + // cleanup + err = diag_release_dci_client(&client_id); + assert(err == DIAG_DCI_NO_ERROR); + Diag_LSM_DeInit(); +} diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 820d5fae9..ff3be7c00 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 import datetime +import json import os +import subprocess import time from pathlib import Path from typing import Dict, Optional, Tuple @@ -10,6 +12,7 @@ from smbus2 import SMBus import cereal.messaging as messaging from cereal import log +from common.basedir import BASEDIR from common.filter_simple import FirstOrderFilter from common.numpy_fast import clip, interp from common.params import Params, ParamKeyType @@ -189,6 +192,15 @@ def thermald_thread(): pass cloudlog.event("CPR", data=cpr_data) + # modem logging + try: + binpath = os.path.join(BASEDIR, "selfdrive/hardware/eon/rat") + out = subprocess.check_output([binpath], encoding='utf8').strip() + dat = json.loads(out.splitlines()[1]) + cloudlog.event("NV data", data=dat) + except Exception: + pass + while 1: pandaState = messaging.recv_sock(pandaState_sock, wait=True) msg = read_thermal(thermal_config)