GCC Code Coverage Report

Directory: ./
File: safety/safety_subaru.h
Date: 2025-02-27 09:09:49
Exec Total Coverage
Lines: 91 91 100.0%
Functions: 7 7 100.0%
Branches: 89 94 94.7%
Line Branch Exec Source
1 #pragma once
2
3 #include "safety_declarations.h"
4
5 #define SUBARU_STEERING_LIMITS_GENERATOR(steer_max, rate_up, rate_down) \
6 { \
7 .max_steer = (steer_max), \
8 .max_rt_delta = 940, \
9 .max_rt_interval = 250000, \
10 .max_rate_up = (rate_up), \
11 .max_rate_down = (rate_down), \
12 .driver_torque_multiplier = 50, \
13 .driver_torque_allowance = 60, \
14 .type = TorqueDriverLimited, \
15 /* the EPS will temporary fault if the steering rate is too high, so we cut the \
16 the steering torque every 7 frames for 1 frame if the steering rate is high */ \
17 .min_valid_request_frames = 7, \
18 .max_invalid_request_frames = 1, \
19 .min_valid_request_rt_interval = 144000, /* 10% tolerance */ \
20 .has_steer_req_tolerance = true, \
21 }
22
23 #define MSG_SUBARU_Brake_Status 0x13c
24 #define MSG_SUBARU_CruiseControl 0x240
25 #define MSG_SUBARU_Throttle 0x40
26 #define MSG_SUBARU_Steering_Torque 0x119
27 #define MSG_SUBARU_Wheel_Speeds 0x13a
28
29 #define MSG_SUBARU_ES_LKAS 0x122
30 #define MSG_SUBARU_ES_Brake 0x220
31 #define MSG_SUBARU_ES_Distance 0x221
32 #define MSG_SUBARU_ES_Status 0x222
33 #define MSG_SUBARU_ES_DashStatus 0x321
34 #define MSG_SUBARU_ES_LKAS_State 0x322
35 #define MSG_SUBARU_ES_Infotainment 0x323
36
37 #define MSG_SUBARU_ES_UDS_Request 0x787
38
39 #define MSG_SUBARU_ES_HighBeamAssist 0x121
40 #define MSG_SUBARU_ES_STATIC_1 0x22a
41 #define MSG_SUBARU_ES_STATIC_2 0x325
42
43 #define SUBARU_MAIN_BUS 0
44 #define SUBARU_ALT_BUS 1
45 #define SUBARU_CAM_BUS 2
46
47 #define SUBARU_COMMON_TX_MSGS(alt_bus, lkas_msg) \
48 {lkas_msg, SUBARU_MAIN_BUS, 8}, \
49 {MSG_SUBARU_ES_Distance, alt_bus, 8}, \
50 {MSG_SUBARU_ES_DashStatus, SUBARU_MAIN_BUS, 8}, \
51 {MSG_SUBARU_ES_LKAS_State, SUBARU_MAIN_BUS, 8}, \
52 {MSG_SUBARU_ES_Infotainment, SUBARU_MAIN_BUS, 8}, \
53
54 #define SUBARU_COMMON_LONG_TX_MSGS(alt_bus) \
55 {MSG_SUBARU_ES_Brake, alt_bus, 8}, \
56 {MSG_SUBARU_ES_Status, alt_bus, 8}, \
57
58 #define SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS() \
59 {MSG_SUBARU_ES_UDS_Request, SUBARU_CAM_BUS, 8}, \
60 {MSG_SUBARU_ES_HighBeamAssist, SUBARU_MAIN_BUS, 8}, \
61 {MSG_SUBARU_ES_STATIC_1, SUBARU_MAIN_BUS, 8}, \
62 {MSG_SUBARU_ES_STATIC_2, SUBARU_MAIN_BUS, 8}, \
63
64 #define SUBARU_COMMON_RX_CHECKS(alt_bus) \
65 {.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}}, \
66 {.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
67 {.msg = {{MSG_SUBARU_Wheel_Speeds, alt_bus, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
68 {.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
69 {.msg = {{MSG_SUBARU_CruiseControl, alt_bus, 8, .check_checksum = true, .max_counter = 15U, .frequency = 20U}, { 0 }, { 0 }}}, \
70
71 static bool subaru_gen2 = false;
72 static bool subaru_longitudinal = false;
73
74 6260 static uint32_t subaru_get_checksum(const CANPacket_t *to_push) {
75 6260 return (uint8_t)GET_BYTE(to_push, 0);
76 }
77
78 6260 static uint8_t subaru_get_counter(const CANPacket_t *to_push) {
79 6260 return (uint8_t)(GET_BYTE(to_push, 1) & 0xFU);
80 }
81
82 6260 static uint32_t subaru_compute_checksum(const CANPacket_t *to_push) {
83 6260 int addr = GET_ADDR(to_push);
84 6260 int len = GET_LEN(to_push);
85 6260 uint8_t checksum = (uint8_t)(addr) + (uint8_t)((unsigned int)(addr) >> 8U);
86
2/2
✓ Branch 0 taken 43820 times.
✓ Branch 1 taken 6260 times.
 50080 for (int i = 1; i < len; i++) {
87 43820 checksum += (uint8_t)GET_BYTE(to_push, i);
88 }
89 6260 return checksum;
90 }
91
92 40012 static void subaru_rx_hook(const CANPacket_t *to_push) {
93 40012 const int bus = GET_BUS(to_push);
94 40012 const int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
95
96 40012 int addr = GET_ADDR(to_push);
97
4/4
✓ Branch 0 taken 6104 times.
✓ Branch 1 taken 33908 times.
✓ Branch 2 taken 6096 times.
✓ Branch 3 taken 8 times.
 40012 if ((addr == MSG_SUBARU_Steering_Torque) && (bus == SUBARU_MAIN_BUS)) {
98 int torque_driver_new;
99 6096 torque_driver_new = ((GET_BYTES(to_push, 0, 4) >> 16) & 0x7FFU);
100 6096 torque_driver_new = -1 * to_signed(torque_driver_new, 11);
101 6096 update_sample(&torque_driver, torque_driver_new);
102
103 6096 int angle_meas_new = (GET_BYTES(to_push, 4, 2) & 0xFFFFU);
104 // convert Steering_Torque -> Steering_Angle to centidegrees, to match the ES_LKAS_ANGLE angle request units
105 6096 angle_meas_new = ROUND(to_signed(angle_meas_new, 16) * -2.17);
106 6096 update_sample(&angle_meas, angle_meas_new);
107 }
108
109 // enter controls on rising edge of ACC, exit controls on ACC off
110
4/4
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 39976 times.
✓ Branch 2 taken 28 times.
✓ Branch 3 taken 8 times.
 40012 if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) {
111 28 bool cruise_engaged = GET_BIT(to_push, 41U);
112 28 pcm_cruise_check(cruise_engaged);
113 }
114
115 // update vehicle moving with any non-zero wheel speed
116
4/4
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 39976 times.
✓ Branch 2 taken 28 times.
✓ Branch 3 taken 8 times.
 40012 if ((addr == MSG_SUBARU_Wheel_Speeds) && (bus == alt_main_bus)) {
117 28 uint32_t fr = (GET_BYTES(to_push, 1, 3) >> 4) & 0x1FFFU;
118 28 uint32_t rr = (GET_BYTES(to_push, 3, 3) >> 1) & 0x1FFFU;
119 28 uint32_t rl = (GET_BYTES(to_push, 4, 3) >> 6) & 0x1FFFU;
120 28 uint32_t fl = (GET_BYTES(to_push, 6, 2) >> 3) & 0x1FFFU;
121
122
5/8
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 20 times.
 28 vehicle_moving = (fr > 0U) || (rr > 0U) || (rl > 0U) || (fl > 0U);
123
124 28 UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4U * 0.057);
125 }
126
127
4/4
✓ Branch 0 taken 56 times.
✓ Branch 1 taken 39956 times.
✓ Branch 2 taken 48 times.
✓ Branch 3 taken 8 times.
 40012 if ((addr == MSG_SUBARU_Brake_Status) && (bus == alt_main_bus)) {
128 48 brake_pressed = GET_BIT(to_push, 62U);
129 }
130
131
4/4
✓ Branch 0 taken 48 times.
✓ Branch 1 taken 39964 times.
✓ Branch 2 taken 40 times.
✓ Branch 3 taken 8 times.
 40012 if ((addr == MSG_SUBARU_Throttle) && (bus == SUBARU_MAIN_BUS)) {
132 40 gas_pressed = GET_BYTE(to_push, 4) != 0U;
133 }
134
135
4/4
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 40000 times.
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 8 times.
 40012 generic_rx_checks((addr == MSG_SUBARU_ES_LKAS) && (bus == SUBARU_MAIN_BUS));
136 40012 }
137
138 489689 static bool subaru_tx_hook(const CANPacket_t *to_send) {
139 const SteeringLimits SUBARU_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
140 const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);
141
142 const LongitudinalLimits SUBARU_LONG_LIMITS = {
143 .min_gas = 808, // appears to be engine braking
144 .max_gas = 3400, // approx 2 m/s^2 when maxing cruise_rpm and cruise_throttle
145 .inactive_gas = 1818, // this is zero acceleration
146 .max_brake = 600, // approx -3.5 m/s^2
147
148 .min_transmission_rpm = 0,
149 .max_transmission_rpm = 3600,
150 };
151
152 489689 bool tx = true;
153 489689 int addr = GET_ADDR(to_send);
154 489689 bool violation = false;
155
156 // steer cmd checks
157
2/2
✓ Branch 0 taken 63384 times.
✓ Branch 1 taken 426305 times.
 489689 if (addr == MSG_SUBARU_ES_LKAS) {
158 63384 int desired_torque = ((GET_BYTES(to_send, 0, 4) >> 16) & 0x1FFFU);
159 63384 desired_torque = -1 * to_signed(desired_torque, 13);
160
161 63384 bool steer_req = GET_BIT(to_send, 29U);
162
163
2/2
✓ Branch 0 taken 25412 times.
✓ Branch 1 taken 37972 times.
 63384 const SteeringLimits limits = subaru_gen2 ? SUBARU_GEN2_STEERING_LIMITS : SUBARU_STEERING_LIMITS;
164 63384 violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits);
165 }
166
167 // check es_brake brake_pressure limits
168
2/2
✓ Branch 0 taken 262154 times.
✓ Branch 1 taken 227535 times.
 489689 if (addr == MSG_SUBARU_ES_Brake) {
169 262154 int es_brake_pressure = GET_BYTES(to_send, 2, 2);
170 262154 violation |= longitudinal_brake_checks(es_brake_pressure, SUBARU_LONG_LIMITS);
171 }
172
173 // check es_distance cruise_throttle limits
174
2/2
✓ Branch 0 taken 65564 times.
✓ Branch 1 taken 424125 times.
 489689 if (addr == MSG_SUBARU_ES_Distance) {
175 65564 int cruise_throttle = (GET_BYTES(to_send, 2, 2) & 0x1FFFU);
176 65564 bool cruise_cancel = GET_BIT(to_send, 56U);
177
178
2/2
✓ Branch 0 taken 32778 times.
✓ Branch 1 taken 32786 times.
 65564 if (subaru_longitudinal) {
179 32778 violation |= longitudinal_gas_checks(cruise_throttle, SUBARU_LONG_LIMITS);
180 } else {
181 // If openpilot is not controlling long, only allow ES_Distance for cruise cancel requests,
182 // (when Cruise_Cancel is true, and Cruise_Throttle is inactive)
183 32786 violation |= (cruise_throttle != SUBARU_LONG_LIMITS.inactive_gas);
184 32786 violation |= (!cruise_cancel);
185 }
186 }
187
188 // check es_status transmission_rpm limits
189
2/2
✓ Branch 0 taken 32778 times.
✓ Branch 1 taken 456911 times.
 489689 if (addr == MSG_SUBARU_ES_Status) {
190 32778 int transmission_rpm = (GET_BYTES(to_send, 2, 2) & 0x1FFFU);
191 32778 violation |= longitudinal_transmission_rpm_checks(transmission_rpm, SUBARU_LONG_LIMITS);
192 }
193
194
2/2
✓ Branch 0 taken 65794 times.
✓ Branch 1 taken 423895 times.
 489689 if (addr == MSG_SUBARU_ES_UDS_Request) {
195 // tester present ('\x02\x3E\x80\x00\x00\x00\x00\x00') is allowed for gen2 longitudinal to keep eyesight disabled
196
3/4
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 65793 times.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 65794 bool is_tester_present = (GET_BYTES(to_send, 0, 4) == 0x00803E02U) && (GET_BYTES(to_send, 4, 4) == 0x0U);
197
198 // reading ES button data by identifier (b'\x03\x22\x11\x30\x00\x00\x00\x00') is also allowed (DID 0x1130)
199
3/4
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 65793 times.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 65794 bool is_button_rdbi = (GET_BYTES(to_send, 0, 4) == 0x30112203U) && (GET_BYTES(to_send, 4, 4) == 0x0U);
200
201
4/4
✓ Branch 0 taken 65793 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 65792 times.
✓ Branch 3 taken 1 times.
 65794 violation |= !(is_tester_present || is_button_rdbi);
202 }
203
204
2/2
✓ Branch 0 taken 447308 times.
✓ Branch 1 taken 42381 times.
 489689 if (violation){
205 447308 tx = false;
206 }
207 489689 return tx;
208 }
209
210 33792 static int subaru_fwd_hook(int bus_num, int addr) {
211 33792 int bus_fwd = -1;
212
213
2/2
✓ Branch 0 taken 11264 times.
✓ Branch 1 taken 22528 times.
 33792 if (bus_num == SUBARU_MAIN_BUS) {
214 11264 bus_fwd = SUBARU_CAM_BUS; // to the eyesight camera
215 }
216
217
2/2
✓ Branch 0 taken 11264 times.
✓ Branch 1 taken 22528 times.
 33792 if (bus_num == SUBARU_CAM_BUS) {
218 // Global platform
219
2/2
✓ Branch 0 taken 11256 times.
✓ Branch 1 taken 4 times.
 11260 bool block_lkas = ((addr == MSG_SUBARU_ES_LKAS) ||
220
2/2
✓ Branch 0 taken 11252 times.
✓ Branch 1 taken 4 times.
 11256 (addr == MSG_SUBARU_ES_DashStatus) ||
221
4/4
✓ Branch 0 taken 11260 times.
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 11248 times.
 22524 (addr == MSG_SUBARU_ES_LKAS_State) ||
222 (addr == MSG_SUBARU_ES_Infotainment));
223
224
2/2
✓ Branch 0 taken 11256 times.
✓ Branch 1 taken 4 times.
 11260 bool block_long = ((addr == MSG_SUBARU_ES_Brake) ||
225
4/4
✓ Branch 0 taken 11260 times.
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 11252 times.
 22524 (addr == MSG_SUBARU_ES_Distance) ||
226 (addr == MSG_SUBARU_ES_Status));
227
228
6/6
✓ Branch 0 taken 11248 times.
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 5624 times.
✓ Branch 3 taken 5624 times.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 5618 times.
 11264 bool block_msg = block_lkas || (subaru_longitudinal && block_long);
229
2/2
✓ Branch 0 taken 11242 times.
✓ Branch 1 taken 22 times.
 11264 if (!block_msg) {
230 11242 bus_fwd = SUBARU_MAIN_BUS; // Main CAN
231 }
232 }
233
234 33792 return bus_fwd;
235 }
236
237 139 static safety_config subaru_init(uint16_t param) {
238 static const CanMsg SUBARU_TX_MSGS[] = {
239 SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
240 };
241
242 static const CanMsg SUBARU_LONG_TX_MSGS[] = {
243 SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
244 SUBARU_COMMON_LONG_TX_MSGS(SUBARU_MAIN_BUS)
245 };
246
247 static const CanMsg SUBARU_GEN2_TX_MSGS[] = {
248 SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
249 };
250
251 static const CanMsg SUBARU_GEN2_LONG_TX_MSGS[] = {
252 SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
253 SUBARU_COMMON_LONG_TX_MSGS(SUBARU_ALT_BUS)
254 SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()
255 };
256
257 static RxCheck subaru_rx_checks[] = {
258 SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS)
259 };
260
261 static RxCheck subaru_gen2_rx_checks[] = {
262 SUBARU_COMMON_RX_CHECKS(SUBARU_ALT_BUS)
263 };
264
265 139 const uint16_t SUBARU_PARAM_GEN2 = 1;
266
267 139 subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
268
269 #ifdef ALLOW_DEBUG
270 139 const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
271 139 subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
272 #endif
273
274 safety_config ret;
275
2/2
✓ Branch 0 taken 70 times.
✓ Branch 1 taken 69 times.
 139 if (subaru_gen2) {
276
2/2
✓ Branch 0 taken 37 times.
✓ Branch 1 taken 33 times.
 70 ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_LONG_TX_MSGS) : \
277 BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_TX_MSGS);
278 } else {
279
2/2
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 33 times.
 69 ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_LONG_TX_MSGS) : \
280 BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_TX_MSGS);
281 }
282 139 return ret;
283 }
284
285 const safety_hooks subaru_hooks = {
286 .init = subaru_init,
287 .rx = subaru_rx_hook,
288 .tx = subaru_tx_hook,
289 .fwd = subaru_fwd_hook,
290 .get_counter = subaru_get_counter,
291 .get_checksum = subaru_get_checksum,
292 .compute_checksum = subaru_compute_checksum,
293 };
294