Fixed disabling gmlan.

board/main.c

@@ -512,13 +512,42 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
       }
       break;
     case 0xdb: // toggle GMLAN
-      if (setup->b.wIndex.w == 3) {
-        set_can_mode(2, 0);
-        set_can_mode(3, setup->b.wValue.w);
-      } else {
-        set_can_mode(3, 0);
-        set_can_mode(2, setup->b.wValue.w);
+      puts("Toggle GMLAN canid: ");
+
+      uint16_t canid = setup->b.wValue.w;
+      bool gmlan_enable = setup->b.wIndex.w;
+
+      puth(canid);
+      puts(" mode ");
+      puth(gmlan_enable);
+      puts("\n");
+
+      if (canid >= CAN_MAX){
+	puts("  Out of range!\n");
+	return -1;
       }
+
+      can_port_desc *port = &can_ports[canid];
+
+      //Fail if canid doesn't support gmlan
+      if(!port->gmlan_support)
+	return -1;
+
+      //ACK the USB pipe but don't do anything; nothing to do.
+      if(port->gmlan == gmlan_enable){
+	puts("The CAN bus is already in the requested gmlan config.\n");
+	break;
+      }
+
+      // Check to see if anyther canid is acting as gmlan, disable it.
+      if(gmlan_enable)
+	for(i = 0; i < CAN_MAX; i++)
+	  if(can_ports[i].gmlan){
+	    puts("Disable old gmlan mode\n");
+	    set_can_mode(i, 0);
+	  }
+
+      set_can_mode(canid, gmlan_enable);
       break;
     case 0xdc: // set controls allowed
       controls_allowed = setup->b.wValue.w == 0x1337;

panda/__init__.py

@@ -157,8 +157,8 @@ class Panda(object):
   def get_can_baud(self, bus):
     return struct.unpack("I", self._handle.controlRead(Panda.REQUEST_TYPE, 0xdf, bus, 0, 4))[0]
 
-  def set_gmlan(self, on, bus=2):
-    self._handle.controlWrite(Panda.REQUEST_TYPE, 0xdb, 1, bus, b'')
+  def set_gmlan(self, bus, on):
+    self._handle.controlWrite(Panda.REQUEST_TYPE, 0xdb, bus, bool(on), b'')
 
   def set_uart_baud(self, uart, rate):
     self._handle.controlWrite(Panda.REQUEST_TYPE, 0xe1, uart, rate, b'')

tests/loopback_test.py

@@ -4,6 +4,7 @@ import os
 import sys
 import time
 import random
+import argparse
 
 from hexdump import hexdump
 from itertools import permutations
@@ -14,7 +15,7 @@ from panda import Panda
 def get_test_string():
   return b"test"+os.urandom(10)
 
-def run_test():
+def run_test(can_speeds, gmlan_speeds, sleep_duration=0):
   pandas = Panda.list()
   print(pandas)
 
@@ -25,9 +26,9 @@ def run_test():
   if len(pandas) == 1:
     # if we only have one on USB, assume the other is on wifi
     pandas.append("WIFI")
-  run_test_w_pandas(pandas)
+  run_test_w_pandas(pandas, can_speeds, gmlan_speeds, sleep_duration)
 
-def run_test_w_pandas(pandas):
+def run_test_w_pandas(pandas, can_speeds, gmlan_speeds, sleep_duration=0):
   h = list(map(lambda x: Panda(x), pandas))
   print("H", h)
 
@@ -60,19 +61,30 @@ def run_test_w_pandas(pandas):
       hexdump(ret)
       assert st == ret
       print("K/L pass", bus, ho, "\n")
+      time.sleep(sleep_duration)
 
     # **** test can line loopback ****
-    for bus, gmlan in [(0, False), (1, False), (2, False), (1, True), (2, True)]:
+    for bus, gmlan in [(0, None), (1, False), (2, False), (1, True), (2, True)]:
       panda0 = h[ho[0]]
       panda1 = h[ho[1]]
-      print("\ntest can", bus)
+      print("\ntest can", bus, "gmlan" if gmlan else "")
       # flush
       cans_echo = panda0.can_recv()
       cans_loop = panda1.can_recv()
 
       # set GMLAN mode
-      panda0.set_gmlan(gmlan, bus)
-      panda1.set_gmlan(gmlan, bus)
+      if(gmlan is not None):
+        panda0.set_gmlan(bus, gmlan)
+        panda1.set_gmlan(bus, gmlan)
+
+      if gmlan:
+        print("Setting GMLAN %d Speed to %d" % (bus, gmlan_speeds[bus]))
+        panda0.set_can_baud(bus, gmlan_speeds[bus])
+        panda1.set_can_baud(bus, gmlan_speeds[bus])
+      else:
+        print("Setting CanBus %d Speed to %d" % (bus, can_speeds[bus]))
+        panda0.set_can_baud(bus, can_speeds[bus])
+        panda1.set_can_baud(bus, can_speeds[bus])
 
       # send the characters
       # pick addresses high enough to not conflict with honda code
@@ -100,16 +112,27 @@ def run_test_w_pandas(pandas):
       if cans_loop[0][3] != bus:
         print("EXPECTED %d GOT %d" % (bus, cans_loop[0][3]))
       assert cans_loop[0][3] == bus
+      time.sleep(sleep_duration)
 
       print("CAN pass", bus, ho)
 
 if __name__ == "__main__":
-  if len(sys.argv) > 1:
-    for i in range(int(sys.argv[1])):
-      run_test()
-  else :
-    i = 0
+  parser = argparse.ArgumentParser()
+  parser.add_argument("-n", type=int, help="Number of test iterations to run")
+  parser.add_argument("-can1baud", type=int, help="Baud Rate of CAN1", default=500000)
+  parser.add_argument("-can2baud", type=int, help="Baud Rate of CAN2", default=500000)
+  parser.add_argument("-can3baud", type=int, help="Baud Rate of CAN3", default=500000)
+  parser.add_argument("-gmlan2baud", type=int, help="Baud Rate of GMLAN2", default=33333)
+  parser.add_argument("-gmlan3baud", type=int, help="Baud Rate of GMLAN3", default=33333)
+  parser.add_argument("-sleep", type=int, help="Sleep time between tests", default=0)
+  args = parser.parse_args()
+
+  can_speeds = (args.can1baud, args.can2baud, args.can3baud)
+  gmlan_speeds = (None, args.gmlan2baud, args.gmlan2baud)
+
+  if args.n is None:
     while True:
-      print("************* testing %d" % i)
-      run_test()
-      i += 1
+      run_test(can_speeds, gmlan_speeds, sleep_duration=args.sleep)
+  else:
+    for i in range(args.n):
+      run_test(can_speeds, gmlan_speeds, sleep_duration=args.sleep)