1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
|
#include "ph_ps2_phy.h"
#include "ph_ps2_phy.pio.h"
void ph_ps2_phy_init(ph_ps2_phy* this, PIO pio, u8 data_pin, rx_callback rx) {
queue_init(&this->qbytes, sizeof(u8), 9);
queue_init(&this->qpacks, sizeof(u8) * 9, 16);
this->pio = pio;
this->sm = pio_claim_unused_sm(this->pio, true);
ps2phy_program_init(this->pio, this->sm, pio_add_program(this->pio, &ps2phy_program), data_pin);
this->sent = 0;
this->rx = rx;
this->last_rx = 0;
this->last_tx = 0;
}
u32 ph_ps2_frame(u8 byte) {
u8 parity = 1;
for (u8 i = 0; i < 8; i++) {
parity = parity ^ (byte >> i & 1);
}
return ((1 << 10) | (parity << 9) | (byte << 1)) ^ 0x7ff;
}
void ph_ps2_phy_task(ph_ps2_phy* this) {
u8 i = 0;
u8 byte;
u8 pack[9];
if (!queue_is_empty(&this->qbytes)) {
while (i < 9 && queue_try_remove(&this->qbytes, &byte)) {
i++;
pack[i] = byte;
}
pack[0] = i;
queue_try_add(&this->qpacks, &pack);
}
if (!queue_is_empty(&this->qpacks) && pio_sm_is_tx_fifo_empty(this->pio, this->sm) && !pio_interrupt_get(this->pio, this->sm * 2 + 0)) {
if (queue_try_peek(&this->qpacks, &pack)) {
if (this->sent == pack[0]) {
this->sent = 0;
queue_try_remove(&this->qpacks, &pack);
} else {
this->sent++;
this->last_tx = pack[this->sent];
pio_sm_put(this->pio, this->sm, ph_ps2_frame(this->last_tx));
}
}
}
if (pio_interrupt_get(this->pio, this->sm * 2 + 1)) {
this->sent = 0;
pio_sm_drain_tx_fifo(this->pio, this->sm);
pio_interrupt_clear(this->pio, this->sm * 2 + 1);
}
if (!pio_sm_is_rx_fifo_empty(this->pio, this->sm)) {
u32 fifo = pio_sm_get(this->pio, this->sm);
fifo = fifo >> 23;
u8 parity = 1;
for (i = 0; i < 8; i++) {
parity = parity ^ (fifo >> i & 1);
}
if (parity != fifo >> 8) {
pio_sm_put(this->pio, this->sm, ph_ps2_frame(0xfe));
return;
}
while(queue_try_remove(&this->qbytes, &byte));
while(queue_try_remove(&this->qpacks, &pack));
(*this->rx)(fifo, this->last_rx);
this->last_rx = fifo;
}
}
|
