Skip to content

Commit da8e00f

Browse files
TX message guaranteed delivery (#421)
* wait for tx slots before clearing nak * fix bootstub * Fixed misra * Cleanup * Added bulk write test to test USB NAK on bulk CAN messages * Added automated bulk tx test * Fixed linter * Fixed latency test influence * Added timeout to python API * Disabled can write timeout in bulk write test Co-authored-by: Robbe <robbe.derks@gmail.com>
1 parent d8f6184 commit da8e00f

File tree

8 files changed

+143
-8
lines changed

8 files changed

+143
-8
lines changed

board/drivers/can.h

+27
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@ void can_set_forwarding(int from, int to);
2828

2929
void can_init(uint8_t can_number);
3030
void can_init_all(void);
31+
bool can_tx_check_min_slots_free(uint32_t min);
3132
void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook);
3233
bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem);
3334

@@ -107,6 +108,20 @@ bool can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
107108
return ret;
108109
}
109110

111+
uint32_t can_slots_empty(can_ring *q) {
112+
uint32_t ret = 0;
113+
114+
ENTER_CRITICAL();
115+
if (q->w_ptr >= q->r_ptr) {
116+
ret = q->fifo_size - 1U - q->w_ptr + q->r_ptr;
117+
} else {
118+
ret = q->r_ptr - q->w_ptr - 1U;
119+
}
120+
EXIT_CRITICAL();
121+
122+
return ret;
123+
}
124+
110125
void can_clear(can_ring *q) {
111126
ENTER_CRITICAL();
112127
q->w_ptr = 0;
@@ -317,6 +332,10 @@ void process_can(uint8_t can_number) {
317332
CAN->sTxMailBox[0].TDHR = to_send.RDHR;
318333
CAN->sTxMailBox[0].TDTR = to_send.RDTR;
319334
CAN->sTxMailBox[0].TIR = to_send.RIR;
335+
336+
if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) {
337+
usb_outep3_resume_if_paused();
338+
}
320339
}
321340
}
322341

@@ -405,6 +424,14 @@ void CAN3_TX_IRQ_Handler(void) { process_can(2); }
405424
void CAN3_RX0_IRQ_Handler(void) { can_rx(2); }
406425
void CAN3_SCE_IRQ_Handler(void) { can_sce(CAN3); }
407426

427+
bool can_tx_check_min_slots_free(uint32_t min) {
428+
return
429+
(can_slots_empty(&can_tx1_q) >= min) &&
430+
(can_slots_empty(&can_tx2_q) >= min) &&
431+
(can_slots_empty(&can_tx3_q) >= min) &&
432+
(can_slots_empty(&can_txgmlan_q) >= min);
433+
}
434+
408435
void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook) {
409436
if (skip_tx_hook || safety_tx_hook(to_push) != 0) {
410437
if (bus_number < BUS_MAX) {

board/drivers/usb.h

+19-4
Original file line numberDiff line numberDiff line change
@@ -23,12 +23,16 @@ typedef union _USB_Setup {
2323
}
2424
USB_Setup_TypeDef;
2525

26+
#define MAX_CAN_MSGS_PER_BULK_TRANSFER 4U
27+
2628
void usb_init(void);
2729
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired);
2830
int usb_cb_ep1_in(void *usbdata, int len, bool hardwired);
2931
void usb_cb_ep2_out(void *usbdata, int len, bool hardwired);
3032
void usb_cb_ep3_out(void *usbdata, int len, bool hardwired);
33+
void usb_cb_ep3_out_complete(void);
3134
void usb_cb_enumeration_complete(void);
35+
void usb_outep3_resume_if_paused(void);
3236

3337
// **** supporting defines ****
3438

@@ -380,6 +384,7 @@ USB_Setup_TypeDef setup;
380384
uint8_t usbdata[0x100];
381385
uint8_t* ep0_txdata = NULL;
382386
uint16_t ep0_txlen = 0;
387+
bool outep3_processing = false;
383388

384389
// Store the current interface alt setting.
385390
int current_int0_alt_setting = 0;
@@ -744,6 +749,7 @@ void usb_irqhandler(void) {
744749
}
745750

746751
if (endpoint == 3) {
752+
outep3_processing = true;
747753
usb_cb_ep3_out(usbdata, len, 1);
748754
}
749755
} else if (status == STS_SETUP_UPDT) {
@@ -816,15 +822,17 @@ void usb_irqhandler(void) {
816822
#ifdef DEBUG_USB
817823
puts(" OUT3 PACKET XFRC\n");
818824
#endif
819-
USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U;
820-
USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
825+
// NAK cleared by process_can (if tx buffers have room)
826+
outep3_processing = false;
827+
usb_cb_ep3_out_complete();
821828
} else if ((USBx_OUTEP(3)->DOEPINT & 0x2000) != 0) {
822829
#ifdef DEBUG_USB
823830
puts(" OUT3 PACKET WTF\n");
824831
#endif
825832
// if NAK was set trigger this, unknown interrupt
826-
USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U;
827-
USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
833+
// TODO: why was this here? fires when TX buffers when we can't clear NAK
834+
// USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U;
835+
// USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
828836
} else if ((USBx_OUTEP(3)->DOEPINT) != 0) {
829837
puts("OUTEP3 error ");
830838
puth(USBx_OUTEP(3)->DOEPINT);
@@ -932,6 +940,13 @@ void usb_irqhandler(void) {
932940
//USBx->GINTMSK = 0xFFFFFFFF & ~(USB_OTG_GINTMSK_NPTXFEM | USB_OTG_GINTMSK_PTXFEM | USB_OTG_GINTSTS_SOF | USB_OTG_GINTSTS_EOPF);
933941
}
934942

943+
void usb_outep3_resume_if_paused() {
944+
if (!outep3_processing && (USBx_OUTEP(3)->DOEPCTL & USB_OTG_DOEPCTL_NAKSTS) != 0) {
945+
USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U;
946+
USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
947+
}
948+
}
949+
935950
void OTG_FS_IRQ_Handler(void) {
936951
NVIC_DisableIRQ(OTG_FS_IRQn);
937952
//__disable_irq();

board/main.c

+6
Original file line numberDiff line numberDiff line change
@@ -235,6 +235,12 @@ void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
235235
}
236236
}
237237

238+
void usb_cb_ep3_out_complete() {
239+
if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) {
240+
usb_outep3_resume_if_paused();
241+
}
242+
}
243+
238244
void usb_cb_enumeration_complete() {
239245
puts("USB enumeration complete\n");
240246
is_enumerated = 1;

board/pedal/main.c

+1
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,7 @@ void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
7676
UNUSED(len);
7777
UNUSED(hardwired);
7878
}
79+
void usb_cb_ep3_out_complete(void) {}
7980
void usb_cb_enumeration_complete(void) {}
8081

8182
int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) {

board/spi_flasher.h

+1
Original file line numberDiff line numberDiff line change
@@ -110,6 +110,7 @@ void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
110110
UNUSED(len);
111111
UNUSED(hardwired);
112112
}
113+
void usb_cb_ep3_out_complete(void) {}
113114

114115
int is_enumerated = 0;
115116
void usb_cb_enumeration_complete(void) {

python/__init__.py

+9-4
Original file line numberDiff line numberDiff line change
@@ -478,7 +478,12 @@ def set_uart_callback(self, uart, install):
478478

479479
# ******************* can *******************
480480

481-
def can_send_many(self, arr):
481+
# The panda will NAK CAN writes when there is CAN congestion.
482+
# libusb will try to send it again, with a max timeout.
483+
# Timeout is in ms. If set to 0, the timeout is infinite.
484+
CAN_SEND_TIMEOUT_MS = 10
485+
486+
def can_send_many(self, arr, timeout=CAN_SEND_TIMEOUT_MS):
482487
snds = []
483488
transmit = 1
484489
extended = 4
@@ -501,13 +506,13 @@ def can_send_many(self, arr):
501506
for s in snds:
502507
self._handle.bulkWrite(3, s)
503508
else:
504-
self._handle.bulkWrite(3, b''.join(snds))
509+
self._handle.bulkWrite(3, b''.join(snds), timeout=timeout)
505510
break
506511
except (usb1.USBErrorIO, usb1.USBErrorOverflow):
507512
print("CAN: BAD SEND MANY, RETRYING")
508513

509-
def can_send(self, addr, dat, bus):
510-
self.can_send_many([[addr, None, dat, bus]])
514+
def can_send(self, addr, dat, bus, timeout=CAN_SEND_TIMEOUT_MS):
515+
self.can_send_many([[addr, None, dat, bus]], timeout=timeout)
511516

512517
def can_recv(self):
513518
dat = bytearray()

tests/automated/7_can_loopback.py

+42
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
import os
22
import time
33
import random
4+
import threading
45
from panda import Panda
56
from nose.tools import assert_equal, assert_less, assert_greater
67
from .helpers import panda_jungle, start_heartbeat_thread, reset_pandas, time_many_sends, test_all_pandas, test_all_gen2_pandas, clear_can_buffers, panda_connect_and_init
@@ -200,3 +201,44 @@ def test(p_send, p_recv):
200201
finally:
201202
# Set back to silent mode
202203
p.set_safety_mode(Panda.SAFETY_SILENT)
204+
205+
@test_all_pandas
206+
@panda_connect_and_init
207+
def test_bulk_write(p):
208+
# The TX buffers on pandas is 0x100 in length.
209+
NUM_MESSAGES_PER_BUS = 10000
210+
211+
def flood_tx(panda):
212+
print('Sending!')
213+
msg = b"\xaa"*4
214+
packet = [[0xaa, None, msg, 0], [0xaa, None, msg, 1], [0xaa, None, msg, 2]] * NUM_MESSAGES_PER_BUS
215+
216+
# Disable timeout
217+
panda.can_send_many(packet, timeout=0)
218+
print(f"Done sending {3*NUM_MESSAGES_PER_BUS} messages!")
219+
220+
# Start heartbeat
221+
start_heartbeat_thread(p)
222+
223+
# Set safety mode and power saving
224+
p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
225+
p.set_power_save(False)
226+
227+
# Start transmisson
228+
threading.Thread(target=flood_tx, args=(p,)).start()
229+
230+
# Receive as much as we can in a few second time period
231+
rx = []
232+
old_len = 0
233+
start_time = time.time()
234+
while time.time() - start_time < 2 or len(rx) > old_len:
235+
old_len = len(rx)
236+
rx.extend(panda_jungle.can_recv())
237+
print(f"Received {len(rx)} messages")
238+
239+
# All messages should have been received
240+
if len(rx) != 3*NUM_MESSAGES_PER_BUS:
241+
Exception("Did not receive all messages!")
242+
243+
# Set back to silent mode
244+
p.set_safety_mode(Panda.SAFETY_SILENT)

tests/bulk_write_test.py

+38
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
#!/usr/bin/env python3
2+
import time
3+
import threading
4+
5+
from panda import Panda
6+
7+
# The TX buffers on pandas is 0x100 in length.
8+
NUM_MESSAGES_PER_BUS = 10000
9+
10+
def flood_tx(panda):
11+
print('Sending!')
12+
msg = b"\xaa"*4
13+
packet = [[0xaa, None, msg, 0], [0xaa, None, msg, 1], [0xaa, None, msg, 2]] * NUM_MESSAGES_PER_BUS
14+
panda.can_send_many(packet)
15+
print(f"Done sending {3*NUM_MESSAGES_PER_BUS} messages!")
16+
17+
if __name__ == "__main__":
18+
serials = Panda.list()
19+
if len(serials) != 2:
20+
raise Exception("Connect two pandas to perform this test!")
21+
22+
sender = Panda(serials[0])
23+
receiver = Panda(serials[1])
24+
25+
sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
26+
receiver.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
27+
28+
# Start transmisson
29+
threading.Thread(target=flood_tx, args=(sender,)).start()
30+
31+
# Receive as much as we can in a few second time period
32+
rx = []
33+
old_len = 0
34+
start_time = time.time()
35+
while time.time() - start_time < 2 or len(rx) > old_len:
36+
old_len = len(rx)
37+
rx.extend(receiver.can_recv())
38+
print(f"Received {len(rx)} messages")

0 commit comments

Comments
 (0)