From b25a894e4c37b5b87f02f3f6dc14c638a83a4b77 Mon Sep 17 00:00:00 2001
From: bravochar <4663128+bravochar@users.noreply.github.com>
Date: Sat, 3 Aug 2024 13:04:16 -0400
Subject: [PATCH 1/9] LKAS model from jnewb

---
 board/safety/safety_subaru.h | 85 +++++++++++++++++++++++++++++++++++-
 python/__init__.py           |  1 +
 tests/safety/test_subaru.py  | 22 +++++++++-
 3 files changed, 105 insertions(+), 3 deletions(-)

diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
index 290150657e..390c9c6a7c 100644
--- a/board/safety/safety_subaru.h
+++ b/board/safety/safety_subaru.h
@@ -20,6 +20,31 @@
     .has_steer_req_tolerance = true,                                                  \
   }
 
+const SteeringLimits SUBARU_STEERING_LIMITS      = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
+const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);
+
+const SteeringLimits SUBARU_ANGLE_STEERING_LIMITS = {
+  .angle_deg_to_can = 100.,
+  .angle_rate_up_lookup = {
+    {0., 15., 15.},
+    {5.,  .8,  .8}
+  },
+  .angle_rate_down_lookup = {
+    {0., 15., 15.},
+    {5.,  .4,  .4}
+  },
+};
+
+const LongitudinalLimits SUBARU_LONG_LIMITS = {
+  .min_gas = 808,       // appears to be engine braking
+  .max_gas = 3400,      // approx  2 m/s^2 when maxing cruise_rpm and cruise_throttle
+  .inactive_gas = 1818, // this is zero acceleration
+  .max_brake = 600,     // approx -3.5 m/s^2
+
+  .min_transmission_rpm = 0,
+  .max_transmission_rpm = 3600,
+};
+
 #define MSG_SUBARU_Brake_Status          0x13c
 #define MSG_SUBARU_CruiseControl         0x240
 #define MSG_SUBARU_Throttle              0x40
@@ -71,6 +96,46 @@
 static bool subaru_gen2 = false;
 static bool subaru_longitudinal = false;
 
+const CanMsg SUBARU_TX_MSGS[] = {
+  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
+};
+
+const CanMsg SUBARU_LONG_TX_MSGS[] = {
+  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
+  SUBARU_COMMON_LONG_TX_MSGS(SUBARU_MAIN_BUS)
+};
+
+const CanMsg SUBARU_GEN2_TX_MSGS[] = {
+  SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
+};
+
+const CanMsg SUBARU_LKAS_ANGLE_TX_MSGS[] = {
+  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
+};
+
+const CanMsg SUBARU_GEN2_LONG_TX_MSGS[] = {
+  SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
+  SUBARU_COMMON_LONG_TX_MSGS(SUBARU_ALT_BUS)
+  SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()
+};
+
+RxCheck subaru_rx_checks[] = {
+  SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS)
+};
+
+RxCheck subaru_gen2_rx_checks[] = {
+  SUBARU_COMMON_RX_CHECKS(SUBARU_ALT_BUS)
+};
+
+
+const uint16_t SUBARU_PARAM_GEN2 = 1;
+const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
+const uint16_t SUBARU_PARAM_LKAS_ANGLE = 4;
+
+bool subaru_gen2 = false;
+bool subaru_longitudinal = false;
+bool subaru_lkas_angle = false;
+
 static uint32_t subaru_get_checksum(const CANPacket_t *to_push) {
   return (uint8_t)GET_BYTE(to_push, 0);
 }
@@ -92,6 +157,7 @@ static uint32_t subaru_compute_checksum(const CANPacket_t *to_push) {
 static void subaru_rx_hook(const CANPacket_t *to_push) {
   const int bus = GET_BUS(to_push);
   const int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
+  const int stock_lkas_msg = subaru_lkas_angle ? MSG_SUBARU_ES_LKAS_ANGLE : MSG_SUBARU_ES_LKAS;
 
   int addr = GET_ADDR(to_push);
   if ((addr == MSG_SUBARU_Steering_Torque) && (bus == SUBARU_MAIN_BUS)) {
@@ -133,6 +199,7 @@ static void subaru_rx_hook(const CANPacket_t *to_push) {
   }
 
   generic_rx_checks((addr == MSG_SUBARU_ES_LKAS) && (bus == SUBARU_MAIN_BUS));
+  generic_rx_checks((addr == stock_lkas_msg) && (bus == SUBARU_MAIN_BUS));
 }
 
 static bool subaru_tx_hook(const CANPacket_t *to_send) {
@@ -164,6 +231,14 @@ static bool subaru_tx_hook(const CANPacket_t *to_send) {
     violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits);
   }
 
+  if (addr == MSG_SUBARU_ES_LKAS_ANGLE) {
+    int desired_angle = GET_BYTES(to_send, 5, 3) & 0x1FFFFU;
+    desired_angle = -1 * to_signed(desired_angle, 17);
+    bool lkas_request = GET_BIT(to_send, 12U);
+
+    violation |= steer_angle_cmd_checks(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS);
+  }
+
   // check es_brake brake_pressure limits
   if (addr == MSG_SUBARU_ES_Brake) {
     int es_brake_pressure = GET_BYTES(to_send, 2, 2);
@@ -210,13 +285,15 @@ static bool subaru_tx_hook(const CANPacket_t *to_send) {
 static int subaru_fwd_hook(int bus_num, int addr) {
   int bus_fwd = -1;
 
+  const int stock_lkas_msg = subaru_lkas_angle ? MSG_SUBARU_ES_LKAS_ANGLE : MSG_SUBARU_ES_LKAS;
+
   if (bus_num == SUBARU_MAIN_BUS) {
     bus_fwd = SUBARU_CAM_BUS;  // to the eyesight camera
   }
 
   if (bus_num == SUBARU_CAM_BUS) {
     // Global platform
-    bool block_lkas = ((addr == MSG_SUBARU_ES_LKAS) ||
+    bool block_lkas = ((addr == stock_lkas_msg) ||
                        (addr == MSG_SUBARU_ES_DashStatus) ||
                        (addr == MSG_SUBARU_ES_LKAS_State) ||
                        (addr == MSG_SUBARU_ES_Infotainment));
@@ -268,11 +345,15 @@ static safety_config subaru_init(uint16_t param) {
 
 #ifdef ALLOW_DEBUG
   const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
+  subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE);
   subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
 #endif
 
   safety_config ret;
-  if (subaru_gen2) {
+  if (subaru_lkas_angle) {
+    ret = BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_LKAS_ANGLE_TX_MSGS);
+  }
+  else if (subaru_gen2) {
     ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_LONG_TX_MSGS) : \
                                 BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_TX_MSGS);
   } else {
diff --git a/python/__init__.py b/python/__init__.py
index 88ccf897a1..83c870d26c 100644
--- a/python/__init__.py
+++ b/python/__init__.py
@@ -214,6 +214,7 @@ class Panda:
 
   FLAG_SUBARU_GEN2 = 1
   FLAG_SUBARU_LONG = 2
+  FLAG_SUBARU_LKAS_ANGLE = 4
 
   FLAG_SUBARU_PREGLOBAL_REVERSED_DRIVER_TORQUE = 1
 
diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py
index 7d07f79aaf..d3635ec418 100755
--- a/tests/safety/test_subaru.py
+++ b/tests/safety/test_subaru.py
@@ -68,7 +68,7 @@ class TestSubaruSafetyBase(common.PandaCarSafetyTest):
   ALT_MAIN_BUS = SUBARU_MAIN_BUS
   ALT_CAM_BUS = SUBARU_CAM_BUS
 
-  DEG_TO_CAN = 100
+  DEG_TO_CAN = 100.
 
   INACTIVE_GAS = 1818
 
@@ -167,6 +167,26 @@ def _torque_cmd_msg(self, torque, steer_req=1):
     return self.packer.make_can_msg_panda("ES_LKAS", SUBARU_MAIN_BUS, values)
 
 
+class TestSubaruAngleSafetyBase(TestSubaruSafetyBase, common.AngleSteeringSafetyTest):
+  TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS, SubaruMsg.ES_LKAS_ANGLE)
+  RELAY_MALFUNCTION_ADDRS = {0: (SubaruMsg.ES_LKAS_ANGLE,)}
+  FWD_BLACKLISTED_ADDRS = fwd_blacklisted_addr(SubaruMsg.ES_LKAS_ANGLE)
+
+  FLAGS = Panda.FLAG_SUBARU_LKAS_ANGLE
+
+  ANGLE_RATE_BP = [0, 15]
+  ANGLE_RATE_UP = [5, 0.15]
+  ANGLE_RATE_DOWN = [5, 0.4]
+
+  def _angle_cmd_msg(self, angle, enabled=1):
+    values = {"LKAS_Output": angle, "LKAS_Request": enabled}
+    return self.packer.make_can_msg_panda("ES_LKAS_ANGLE", 0, values)
+
+  def _angle_meas_msg(self, angle):
+    values = {"Steering_Angle": angle}
+    return self.packer.make_can_msg_panda("Steering_Torque", 0, values)
+
+
 class TestSubaruGen1TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):
   FLAGS = 0
   TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS)

From 58205a1e81f6a7135272179e2ae81200d6477a1d Mon Sep 17 00:00:00 2001
From: Charlie <4663128+bravochar@users.noreply.github.com>
Date: Fri, 13 Sep 2024 12:12:17 -0400
Subject: [PATCH 2/9] Remove LKAS Angle from debug ifdef

---
 board/safety/safety_subaru.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
index 390c9c6a7c..b9f008f474 100644
--- a/board/safety/safety_subaru.h
+++ b/board/safety/safety_subaru.h
@@ -342,10 +342,10 @@ static safety_config subaru_init(uint16_t param) {
   const uint16_t SUBARU_PARAM_GEN2 = 1;
 
   subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
+  subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE);
 
 #ifdef ALLOW_DEBUG
   const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
-  subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE);
   subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);
 #endif
 

From 0565c2910add6b8e5b3e2e67e025f036a31d1796 Mon Sep 17 00:00:00 2001
From: Charlie <4663128+bravochar@users.noreply.github.com>
Date: Fri, 13 Sep 2024 13:41:04 -0400
Subject: [PATCH 3/9] Add LKAS Angle safety tests

---
 tests/safety/common.py      | 4 ++++
 tests/safety/test_subaru.py | 6 +++++-
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/tests/safety/common.py b/tests/safety/common.py
index 884f21a360..67325d88a9 100644
--- a/tests/safety/common.py
+++ b/tests/safety/common.py
@@ -759,6 +759,10 @@ def test_tx_hook_on_wrong_safety_mode(self):
               continue
             if attr.startswith('TestSubaruGen') and current_test.startswith('TestSubaruGen'):
               continue
+            if attr.startswith('TestSubaruAngle') and current_test.startswith('TestSubaruGen'):
+              continue
+            if attr.startswith('TestSubaruGen') and current_test.startswith('TestSubaruAngle'):
+              continue
             if attr.startswith('TestSubaruPreglobal') and current_test.startswith('TestSubaruPreglobal'):
               continue
             if {attr, current_test}.issubset({'TestVolkswagenPqSafety', 'TestVolkswagenPqStockSafety', 'TestVolkswagenPqLongSafety'}):
diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py
index d3635ec418..e7dcdfcc0d 100755
--- a/tests/safety/test_subaru.py
+++ b/tests/safety/test_subaru.py
@@ -169,7 +169,7 @@ def _torque_cmd_msg(self, torque, steer_req=1):
 
 class TestSubaruAngleSafetyBase(TestSubaruSafetyBase, common.AngleSteeringSafetyTest):
   TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS, SubaruMsg.ES_LKAS_ANGLE)
-  RELAY_MALFUNCTION_ADDRS = {0: (SubaruMsg.ES_LKAS_ANGLE,)}
+  RELAY_MALFUNCTION_ADDRS = {SUBARU_MAIN_BUS: (SubaruMsg.ES_LKAS_ANGLE, SubaruMsg.ES_LKAS,)}
   FWD_BLACKLISTED_ADDRS = fwd_blacklisted_addr(SubaruMsg.ES_LKAS_ANGLE)
 
   FLAGS = Panda.FLAG_SUBARU_LKAS_ANGLE
@@ -201,6 +201,10 @@ class TestSubaruGen2TorqueSafetyBase(TestSubaruTorqueSafetyBase):
   MAX_TORQUE = 1000
 
 
+class TestSubaruAngleSafety(TestSubaruAngleSafetyBase):
+  FLAGS = Panda.FLAG_SUBARU_LKAS_ANGLE
+
+
 class TestSubaruGen2TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruGen2TorqueSafetyBase):
   FLAGS = Panda.FLAG_SUBARU_GEN2
   TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS)

From e0e61cceaeb956443781c1e3b38be5cec444078e Mon Sep 17 00:00:00 2001
From: Charlie <4663128+bravochar@users.noreply.github.com>
Date: Fri, 13 Sep 2024 14:21:26 -0400
Subject: [PATCH 4/9] Update cruise activated signal for LKAS Angle subaru

---
 board/safety/safety_subaru.h | 5 +++++
 tests/safety/test_subaru.py  | 5 +++++
 2 files changed, 10 insertions(+)

diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
index b9f008f474..a292eb1dba 100644
--- a/board/safety/safety_subaru.h
+++ b/board/safety/safety_subaru.h
@@ -176,6 +176,11 @@ static void subaru_rx_hook(const CANPacket_t *to_push) {
   if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) {
     bool cruise_engaged = GET_BIT(to_push, 41U);
     pcm_cruise_check(cruise_engaged);
+
+  // LKAS Angle cars use different message
+  } else if (subaru_lkas_angle && (addr == MSG_SUBARU_ES_DashStatus) && (bus == alt_main_bus)) {
+    bool cruise_engaged = GET_BIT(to_push, 36U);
+    pcm_cruise_check(cruise_engaged);
   }
 
   // update vehicle moving with any non-zero wheel speed
diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py
index e7dcdfcc0d..8145dee199 100755
--- a/tests/safety/test_subaru.py
+++ b/tests/safety/test_subaru.py
@@ -186,6 +186,11 @@ def _angle_meas_msg(self, angle):
     values = {"Steering_Angle": angle}
     return self.packer.make_can_msg_panda("Steering_Torque", 0, values)
 
+  # need to use ES_DashStatus Message
+  def _pcm_status_msg(self, enable):
+    values = {"Cruise_Activated": enable}
+    return self.packer.make_can_msg_panda("ES_DashStatus", self.ALT_MAIN_BUS, values)
+
 
 class TestSubaruGen1TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):
   FLAGS = 0

From 61c7076c4d7296e0598ba68273b62bb462ca813b Mon Sep 17 00:00:00 2001
From: Charlie <4663128+bravochar@users.noreply.github.com>
Date: Fri, 13 Sep 2024 14:50:05 -0400
Subject: [PATCH 5/9] Detect cruise on cam bus

---
 board/safety/safety_subaru.h | 2 +-
 tests/safety/test_subaru.py  | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
index a292eb1dba..fc5cef065f 100644
--- a/board/safety/safety_subaru.h
+++ b/board/safety/safety_subaru.h
@@ -178,7 +178,7 @@ static void subaru_rx_hook(const CANPacket_t *to_push) {
     pcm_cruise_check(cruise_engaged);
 
   // LKAS Angle cars use different message
-  } else if (subaru_lkas_angle && (addr == MSG_SUBARU_ES_DashStatus) && (bus == alt_main_bus)) {
+  } else if (subaru_lkas_angle && (addr == MSG_SUBARU_ES_DashStatus) && (bus == SUBARU_CAM_BUS)) {
     bool cruise_engaged = GET_BIT(to_push, 36U);
     pcm_cruise_check(cruise_engaged);
   }
diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py
index 8145dee199..440899dab0 100755
--- a/tests/safety/test_subaru.py
+++ b/tests/safety/test_subaru.py
@@ -189,7 +189,7 @@ def _angle_meas_msg(self, angle):
   # need to use ES_DashStatus Message
   def _pcm_status_msg(self, enable):
     values = {"Cruise_Activated": enable}
-    return self.packer.make_can_msg_panda("ES_DashStatus", self.ALT_MAIN_BUS, values)
+    return self.packer.make_can_msg_panda("ES_DashStatus", self.ALT_CAM_BUS, values)
 
 
 class TestSubaruGen1TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):

From a296efdc815c1eac19c074ea40a0e2df79c7bf05 Mon Sep 17 00:00:00 2001
From: Charlie <4663128+bravochar@users.noreply.github.com>
Date: Fri, 13 Sep 2024 14:59:36 -0400
Subject: [PATCH 6/9] Add long tests?

---
 board/safety/safety_subaru.h | 5 +++--
 tests/safety/test_subaru.py  | 2 +-
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
index fc5cef065f..f39dcc101e 100644
--- a/board/safety/safety_subaru.h
+++ b/board/safety/safety_subaru.h
@@ -177,8 +177,9 @@ static void subaru_rx_hook(const CANPacket_t *to_push) {
     bool cruise_engaged = GET_BIT(to_push, 41U);
     pcm_cruise_check(cruise_engaged);
 
-  // LKAS Angle cars use different message
-  } else if (subaru_lkas_angle && (addr == MSG_SUBARU_ES_DashStatus) && (bus == SUBARU_CAM_BUS)) {
+  }
+  else if (subaru_lkas_angle && (addr == MSG_SUBARU_ES_DashStatus) && (bus == SUBARU_CAM_BUS)) {
+    // LKAS Angle cars use different message
     bool cruise_engaged = GET_BIT(to_push, 36U);
     pcm_cruise_check(cruise_engaged);
   }
diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py
index 440899dab0..92f25911a5 100755
--- a/tests/safety/test_subaru.py
+++ b/tests/safety/test_subaru.py
@@ -206,7 +206,7 @@ class TestSubaruGen2TorqueSafetyBase(TestSubaruTorqueSafetyBase):
   MAX_TORQUE = 1000
 
 
-class TestSubaruAngleSafety(TestSubaruAngleSafetyBase):
+class TestSubaruAngleSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase):
   FLAGS = Panda.FLAG_SUBARU_LKAS_ANGLE
 
 

From 1e385a24b2197d2bc3b5c01c0adb552a7ead5ea4 Mon Sep 17 00:00:00 2001
From: Charlie <4663128+bravochar@users.noreply.github.com>
Date: Fri, 13 Sep 2024 16:19:22 -0400
Subject: [PATCH 7/9] Merge Gen2 and Angle together for LKAS Angle test

---
 board/safety/safety_subaru.h | 26 +++++++++++++++-----------
 tests/safety/test_subaru.py  | 13 +++++++++----
 2 files changed, 24 insertions(+), 15 deletions(-)

diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
index f39dcc101e..c0dc34b897 100644
--- a/board/safety/safety_subaru.h
+++ b/board/safety/safety_subaru.h
@@ -110,7 +110,8 @@ const CanMsg SUBARU_GEN2_TX_MSGS[] = {
 };
 
 const CanMsg SUBARU_LKAS_ANGLE_TX_MSGS[] = {
-  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
+  SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
+  SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
 };
 
 const CanMsg SUBARU_GEN2_LONG_TX_MSGS[] = {
@@ -156,7 +157,7 @@ static uint32_t subaru_compute_checksum(const CANPacket_t *to_push) {
 
 static void subaru_rx_hook(const CANPacket_t *to_push) {
   const int bus = GET_BUS(to_push);
-  const int alt_main_bus = subaru_gen2 ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
+  const int alt_main_bus = (subaru_gen2 || subaru_lkas_angle) ? SUBARU_ALT_BUS : SUBARU_MAIN_BUS;
   const int stock_lkas_msg = subaru_lkas_angle ? MSG_SUBARU_ES_LKAS_ANGLE : MSG_SUBARU_ES_LKAS;
 
   int addr = GET_ADDR(to_push);
@@ -173,15 +174,18 @@ static void subaru_rx_hook(const CANPacket_t *to_push) {
   }
 
   // enter controls on rising edge of ACC, exit controls on ACC off
-  if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) {
-    bool cruise_engaged = GET_BIT(to_push, 41U);
-    pcm_cruise_check(cruise_engaged);
-
-  }
-  else if (subaru_lkas_angle && (addr == MSG_SUBARU_ES_DashStatus) && (bus == SUBARU_CAM_BUS)) {
+  if (subaru_lkas_angle) {
     // LKAS Angle cars use different message
-    bool cruise_engaged = GET_BIT(to_push, 36U);
-    pcm_cruise_check(cruise_engaged);
+    if ((addr == MSG_SUBARU_ES_DashStatus) && (bus == SUBARU_CAM_BUS)) {
+      bool cruise_engaged = GET_BIT(to_push, 36U);
+      pcm_cruise_check(cruise_engaged);
+    }
+  } else {
+    if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) {
+      bool cruise_engaged = GET_BIT(to_push, 41U);
+      pcm_cruise_check(cruise_engaged);
+
+    }
   }
 
   // update vehicle moving with any non-zero wheel speed
@@ -357,7 +361,7 @@ static safety_config subaru_init(uint16_t param) {
 
   safety_config ret;
   if (subaru_lkas_angle) {
-    ret = BUILD_SAFETY_CFG(subaru_rx_checks, SUBARU_LKAS_ANGLE_TX_MSGS);
+    ret = BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_LKAS_ANGLE_TX_MSGS);
   }
   else if (subaru_gen2) {
     ret = subaru_longitudinal ? BUILD_SAFETY_CFG(subaru_gen2_rx_checks, SUBARU_GEN2_LONG_TX_MSGS) : \
diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py
index 92f25911a5..77167946b9 100755
--- a/tests/safety/test_subaru.py
+++ b/tests/safety/test_subaru.py
@@ -168,6 +168,8 @@ def _torque_cmd_msg(self, torque, steer_req=1):
 
 
 class TestSubaruAngleSafetyBase(TestSubaruSafetyBase, common.AngleSteeringSafetyTest):
+  ALT_MAIN_BUS = SUBARU_ALT_BUS
+
   TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS, SubaruMsg.ES_LKAS_ANGLE)
   RELAY_MALFUNCTION_ADDRS = {SUBARU_MAIN_BUS: (SubaruMsg.ES_LKAS_ANGLE, SubaruMsg.ES_LKAS,)}
   FWD_BLACKLISTED_ADDRS = fwd_blacklisted_addr(SubaruMsg.ES_LKAS_ANGLE)
@@ -206,15 +208,18 @@ class TestSubaruGen2TorqueSafetyBase(TestSubaruTorqueSafetyBase):
   MAX_TORQUE = 1000
 
 
-class TestSubaruAngleSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase):
-  FLAGS = Panda.FLAG_SUBARU_LKAS_ANGLE
-
-
 class TestSubaruGen2TorqueStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruGen2TorqueSafetyBase):
   FLAGS = Panda.FLAG_SUBARU_GEN2
   TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS)
 
 
+class TestSubaruGen2AngleStockLongitudinalSafety(TestSubaruStockLongitudinalSafetyBase, TestSubaruAngleSafetyBase):
+  ALT_MAIN_BUS = SUBARU_ALT_BUS
+
+  FLAGS = Panda.FLAG_SUBARU_GEN2 | Panda.FLAG_SUBARU_LKAS_ANGLE
+  TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS) + lkas_tx_msgs(SUBARU_MAIN_BUS, SubaruMsg.ES_LKAS_ANGLE)
+
+
 class TestSubaruGen1LongitudinalSafety(TestSubaruLongitudinalSafetyBase, TestSubaruTorqueSafetyBase):
   FLAGS = Panda.FLAG_SUBARU_LONG
   TX_MSGS = lkas_tx_msgs(SUBARU_MAIN_BUS) + long_tx_msgs(SUBARU_MAIN_BUS)

From 6057481608ab5d602cb420aae91d90f169ce3747 Mon Sep 17 00:00:00 2001
From: bravochar <4663128+bravochar@users.noreply.github.com>
Date: Sat, 23 Nov 2024 11:25:27 -0500
Subject: [PATCH 8/9] Fix rebase bugs

---
 board/safety/safety_subaru.h | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
index c0dc34b897..6c660d3300 100644
--- a/board/safety/safety_subaru.h
+++ b/board/safety/safety_subaru.h
@@ -52,6 +52,7 @@ const LongitudinalLimits SUBARU_LONG_LIMITS = {
 #define MSG_SUBARU_Wheel_Speeds          0x13a
 
 #define MSG_SUBARU_ES_LKAS               0x122
+#define MSG_SUBARU_ES_LKAS_ANGLE         0x124
 #define MSG_SUBARU_ES_Brake              0x220
 #define MSG_SUBARU_ES_Distance           0x221
 #define MSG_SUBARU_ES_Status             0x222
@@ -95,6 +96,7 @@ const LongitudinalLimits SUBARU_LONG_LIMITS = {
 
 static bool subaru_gen2 = false;
 static bool subaru_longitudinal = false;
+static bool subaru_lkas_angle = false;
 
 const CanMsg SUBARU_TX_MSGS[] = {
   SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
@@ -133,10 +135,6 @@ const uint16_t SUBARU_PARAM_GEN2 = 1;
 const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
 const uint16_t SUBARU_PARAM_LKAS_ANGLE = 4;
 
-bool subaru_gen2 = false;
-bool subaru_longitudinal = false;
-bool subaru_lkas_angle = false;
-
 static uint32_t subaru_get_checksum(const CANPacket_t *to_push) {
   return (uint8_t)GET_BYTE(to_push, 0);
 }

From 1c7ac6b2e822c6701dacd6d68454c1971b311e76 Mon Sep 17 00:00:00 2001
From: bravochar <4663128+bravochar@users.noreply.github.com>
Date: Sat, 23 Nov 2024 12:11:12 -0500
Subject: [PATCH 9/9] Fix misra erros

---
 board/safety/safety_subaru.h | 80 ++++++++----------------------------
 1 file changed, 18 insertions(+), 62 deletions(-)

diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h
index 6c660d3300..41ce5fb7dc 100644
--- a/board/safety/safety_subaru.h
+++ b/board/safety/safety_subaru.h
@@ -20,31 +20,6 @@
     .has_steer_req_tolerance = true,                                                  \
   }
 
-const SteeringLimits SUBARU_STEERING_LIMITS      = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
-const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);
-
-const SteeringLimits SUBARU_ANGLE_STEERING_LIMITS = {
-  .angle_deg_to_can = 100.,
-  .angle_rate_up_lookup = {
-    {0., 15., 15.},
-    {5.,  .8,  .8}
-  },
-  .angle_rate_down_lookup = {
-    {0., 15., 15.},
-    {5.,  .4,  .4}
-  },
-};
-
-const LongitudinalLimits SUBARU_LONG_LIMITS = {
-  .min_gas = 808,       // appears to be engine braking
-  .max_gas = 3400,      // approx  2 m/s^2 when maxing cruise_rpm and cruise_throttle
-  .inactive_gas = 1818, // this is zero acceleration
-  .max_brake = 600,     // approx -3.5 m/s^2
-
-  .min_transmission_rpm = 0,
-  .max_transmission_rpm = 3600,
-};
-
 #define MSG_SUBARU_Brake_Status          0x13c
 #define MSG_SUBARU_CruiseControl         0x240
 #define MSG_SUBARU_Throttle              0x40
@@ -98,43 +73,6 @@ static bool subaru_gen2 = false;
 static bool subaru_longitudinal = false;
 static bool subaru_lkas_angle = false;
 
-const CanMsg SUBARU_TX_MSGS[] = {
-  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
-};
-
-const CanMsg SUBARU_LONG_TX_MSGS[] = {
-  SUBARU_COMMON_TX_MSGS(SUBARU_MAIN_BUS, MSG_SUBARU_ES_LKAS)
-  SUBARU_COMMON_LONG_TX_MSGS(SUBARU_MAIN_BUS)
-};
-
-const CanMsg SUBARU_GEN2_TX_MSGS[] = {
-  SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
-};
-
-const CanMsg SUBARU_LKAS_ANGLE_TX_MSGS[] = {
-  SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
-  SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
-};
-
-const CanMsg SUBARU_GEN2_LONG_TX_MSGS[] = {
-  SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
-  SUBARU_COMMON_LONG_TX_MSGS(SUBARU_ALT_BUS)
-  SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()
-};
-
-RxCheck subaru_rx_checks[] = {
-  SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS)
-};
-
-RxCheck subaru_gen2_rx_checks[] = {
-  SUBARU_COMMON_RX_CHECKS(SUBARU_ALT_BUS)
-};
-
-
-const uint16_t SUBARU_PARAM_GEN2 = 1;
-const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
-const uint16_t SUBARU_PARAM_LKAS_ANGLE = 4;
-
 static uint32_t subaru_get_checksum(const CANPacket_t *to_push) {
   return (uint8_t)GET_BYTE(to_push, 0);
 }
@@ -214,6 +152,18 @@ static bool subaru_tx_hook(const CANPacket_t *to_send) {
   const SteeringLimits SUBARU_STEERING_LIMITS      = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
   const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);
 
+  const SteeringLimits SUBARU_ANGLE_STEERING_LIMITS = {
+    .angle_deg_to_can = 100.,
+    .angle_rate_up_lookup = {
+      {0., 15., 15.},
+      {5.,  .8,  .8}
+    },
+    .angle_rate_down_lookup = {
+      {0., 15., 15.},
+      {5.,  .4,  .4}
+    },
+  };
+
   const LongitudinalLimits SUBARU_LONG_LIMITS = {
     .min_gas = 808,       // appears to be engine braking
     .max_gas = 3400,      // approx  2 m/s^2 when maxing cruise_rpm and cruise_throttle
@@ -339,6 +289,11 @@ static safety_config subaru_init(uint16_t param) {
     SUBARU_GEN2_LONG_ADDITIONAL_TX_MSGS()
   };
 
+  static const CanMsg SUBARU_LKAS_ANGLE_TX_MSGS[] = {
+    SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS_ANGLE)
+    SUBARU_COMMON_TX_MSGS(SUBARU_ALT_BUS, MSG_SUBARU_ES_LKAS)
+  };
+
   static RxCheck subaru_rx_checks[] = {
     SUBARU_COMMON_RX_CHECKS(SUBARU_MAIN_BUS)
   };
@@ -348,6 +303,7 @@ static safety_config subaru_init(uint16_t param) {
   };
 
   const uint16_t SUBARU_PARAM_GEN2 = 1;
+  const uint16_t SUBARU_PARAM_LKAS_ANGLE = 4;
 
   subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
   subaru_lkas_angle = GET_FLAG(param, SUBARU_PARAM_LKAS_ANGLE);