1 | #include "controller.h"
|
---|
2 |
|
---|
3 | using namespace pacpus;
|
---|
4 |
|
---|
5 | DualshockAPI::DualshockAPI()
|
---|
6 | {
|
---|
7 | uid_t uid = getuid();
|
---|
8 | if(uid)
|
---|
9 | throw "Run as root and you will be happy :)";
|
---|
10 | bdaddr_t any = {{0, 0, 0, 0, 0, 0}};
|
---|
11 | csk = l2cap_listen(&any, L2CAP_PSM_HIDP_CTRL);
|
---|
12 | isk = l2cap_listen(&any, L2CAP_PSM_HIDP_INTR);
|
---|
13 | if(csk < 0 || isk < 0)
|
---|
14 | throw "Unable to listen on l2cap socket";
|
---|
15 | quit = 0;
|
---|
16 | controller.paired = 0;
|
---|
17 | shmem = new ShMem("dualshock", sizeof(struct dualshockButtons_s));
|
---|
18 | struct dualshockButtons_s d;
|
---|
19 | d.available = 0;
|
---|
20 | d.time = road_time();
|
---|
21 | shmem->write((void*)&d, sizeof(struct dualshockButtons_s), 0);
|
---|
22 | }
|
---|
23 |
|
---|
24 | int DualshockAPI::l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm)
|
---|
25 | {
|
---|
26 | int sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
|
---|
27 | if(sk < 0)
|
---|
28 | throw "Unable to open a socket";
|
---|
29 | struct sockaddr_l2 addr;
|
---|
30 | memset(&addr, 0, sizeof(addr));
|
---|
31 | addr.l2_family = AF_BLUETOOTH;
|
---|
32 | addr.l2_bdaddr = *bdaddr;//(bdaddr_t) {{0, 0, 0, 0, 0, 0}};
|
---|
33 | addr.l2_psm = htobs(psm);
|
---|
34 | if (bind(sk, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
|
---|
35 | close(sk);
|
---|
36 | throw "Bind error";
|
---|
37 | }
|
---|
38 | if (listen(sk, 5) < 0)
|
---|
39 | throw "Listen error";
|
---|
40 | return sk;
|
---|
41 | }
|
---|
42 |
|
---|
43 | void DualshockAPI::update_shared_memory()
|
---|
44 | {
|
---|
45 | struct dualshockButtons_s d;
|
---|
46 | d.available = controller.paired;
|
---|
47 | d.buttons = controller.buttons;
|
---|
48 | d.time = road_time();
|
---|
49 | shmem->write((void*)&d, sizeof(struct dualshockButtons_s), 0);
|
---|
50 | dbtFile.writeRecord(d.time, 0, (const char*)&d,
|
---|
51 | sizeof(struct dualshockButtons_s));
|
---|
52 | }
|
---|
53 |
|
---|
54 | void DualshockAPI::main_loop()
|
---|
55 | {
|
---|
56 | //std::cerr << "Main loop\n";
|
---|
57 | while(!quit) {
|
---|
58 | fd_set fds;
|
---|
59 | FD_ZERO(&fds);
|
---|
60 | FD_SET(STDIN_FILENO, &fds);
|
---|
61 | int fdmax = 0;
|
---|
62 | FD_SET(csk, &fds);
|
---|
63 | FD_SET(isk, &fds);
|
---|
64 | fdmax = csk > isk ? csk : isk;
|
---|
65 | if(controller.paired) {
|
---|
66 | FD_SET(controller.csk, &fds);
|
---|
67 | if (controller.csk > fdmax)
|
---|
68 | fdmax = controller.csk;
|
---|
69 | FD_SET(controller.isk, &fds);
|
---|
70 | if (controller.isk > fdmax)
|
---|
71 | fdmax = controller.isk;
|
---|
72 | }
|
---|
73 | //std::cerr << " elem\n";
|
---|
74 | //std::cerr << "Select\n";
|
---|
75 | struct timeval tv = {2, 0}; //timeout = 2s
|
---|
76 | if (select(fdmax + 1, &fds, NULL, NULL, &tv) < 0)
|
---|
77 | throw "Select";
|
---|
78 |
|
---|
79 | if (FD_ISSET(csk, &fds))
|
---|
80 | handle_connection();
|
---|
81 | if (controller.paired && FD_ISSET(controller.isk, &fds)) {
|
---|
82 | unsigned char report[256];
|
---|
83 | int nr = recv(controller.isk, report, sizeof(report), 0);
|
---|
84 | if (nr <= 0)
|
---|
85 | handle_disconnection();
|
---|
86 | else
|
---|
87 | handle_report(report, nr);
|
---|
88 | }
|
---|
89 | }
|
---|
90 | quit = 2;
|
---|
91 | }
|
---|
92 |
|
---|
93 | void DualshockAPI::stop()
|
---|
94 | {
|
---|
95 | quit = 1;
|
---|
96 | while(quit != 2) //avoid a race condition
|
---|
97 | ;
|
---|
98 | dbtFile.close();
|
---|
99 | }
|
---|
100 |
|
---|
101 | void DualshockAPI::handle_connection()
|
---|
102 | {
|
---|
103 | std::cerr << "Connection !\n";
|
---|
104 | static int currentIndex = 0;
|
---|
105 | int cs, is;
|
---|
106 | bdaddr_t baddr;
|
---|
107 |
|
---|
108 | if((cs = accept(csk, NULL, NULL)) < 0)
|
---|
109 | throw "Accept";
|
---|
110 | if((is = accept(isk, NULL, NULL)) < 0)
|
---|
111 | throw "Accept";
|
---|
112 |
|
---|
113 | struct sockaddr_l2 addr;
|
---|
114 | socklen_t addrlen = sizeof(addr);
|
---|
115 | if (getpeername(is, (struct sockaddr *)&addr, &addrlen) < 0)
|
---|
116 | throw "Getpeername";
|
---|
117 | baddr = addr.l2_bdaddr;
|
---|
118 | unsigned char resp[64];
|
---|
119 | char get03f2[] = { HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_FEATURE | 8,
|
---|
120 | '\xf2', sizeof(resp), sizeof(resp)>>8 };
|
---|
121 | (void)send(cs, get03f2, sizeof(get03f2), 0);
|
---|
122 | (void)recv(cs, resp, sizeof(resp), 0);
|
---|
123 |
|
---|
124 | if(controller.paired == 1) {
|
---|
125 | close(controller.csk);
|
---|
126 | close(controller.isk);
|
---|
127 | }
|
---|
128 | controller.index = currentIndex++;
|
---|
129 | controller.addr = baddr;
|
---|
130 | controller.paired = 1;
|
---|
131 | controller.csk = cs;
|
---|
132 | controller.isk = is;
|
---|
133 | setup_device(controller.csk);
|
---|
134 | }
|
---|
135 |
|
---|
136 | void DualshockAPI::setup_device(int sk)
|
---|
137 | {
|
---|
138 | int index = 1; //FIXME
|
---|
139 | char set03f4[] = { HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE,
|
---|
140 | '\xf4', '\x42', '\x03', '\00', '\00' };
|
---|
141 | send(sk, set03f4, sizeof(set03f4), 0);
|
---|
142 | unsigned char ack;
|
---|
143 | int nr = recv(sk, &ack, sizeof(ack), 0);
|
---|
144 | if (nr != 1 || ack != 0)
|
---|
145 | throw "Device is not responding";
|
---|
146 | static const char ledmask[10] = { 1, 2, 4, 8, 6, 7, 11, 13, 14, 15 };
|
---|
147 | #define LED_PERMANENT '\xff', '\x27', '\00', '\00', '\x32'
|
---|
148 | char set0201[] = {
|
---|
149 | HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_OUTPUT, 0x01,
|
---|
150 | '\00', '\00', '\00', '\00', '\00', '\00', '\00', '\00',
|
---|
151 | '\00', static_cast<char>(ledmask[index % 10] << 1),
|
---|
152 | LED_PERMANENT,
|
---|
153 | LED_PERMANENT,
|
---|
154 | LED_PERMANENT,
|
---|
155 | LED_PERMANENT,
|
---|
156 | '\00', '\00', '\00', '\00', '\00', '\00', '\00', '\00', '\00', '\00'
|
---|
157 | };
|
---|
158 | set0201[3] = set0201[5] = 4;
|
---|
159 | set0201[5] = 0;
|
---|
160 | set0201[6] = 0x70;
|
---|
161 | send(sk, set0201, sizeof(set0201), 0);
|
---|
162 | nr = recv(sk, &ack, sizeof(ack), 0);
|
---|
163 | if (nr != 1 || ack != 0)
|
---|
164 | throw "Device is not responding";
|
---|
165 | }
|
---|
166 |
|
---|
167 | void DualshockAPI::handle_disconnection()
|
---|
168 | {
|
---|
169 | std::cerr << "Disconnection\n";
|
---|
170 | controller.paired = 0;
|
---|
171 | }
|
---|
172 |
|
---|
173 | void DualshockAPI::handle_report(unsigned char buf[], int len)
|
---|
174 | {
|
---|
175 | //std::cerr << "Data\n";
|
---|
176 | (void)len;
|
---|
177 | if (buf[0] != 0xa1)
|
---|
178 | return;
|
---|
179 |
|
---|
180 | /* Digital */
|
---|
181 | controller.buttons.digital.select = (buf[3] & 0x01) >> 0;
|
---|
182 | controller.buttons.digital.l3 = (buf[3] & 0x02) >> 1;
|
---|
183 | controller.buttons.digital.r3 = (buf[3] & 0x04) >> 2;
|
---|
184 | controller.buttons.digital.start = (buf[3] & 0x08) >> 3;
|
---|
185 | controller.buttons.digital.up = (buf[3] & 0x10) >> 4;
|
---|
186 | controller.buttons.digital.right = (buf[3] & 0x20) >> 5;
|
---|
187 | controller.buttons.digital.down = (buf[3] & 0x40) >> 6;
|
---|
188 | controller.buttons.digital.left = (buf[3] & 0x80) >> 7;
|
---|
189 | controller.buttons.digital.l2 = (buf[4] & 0x01) >> 1;
|
---|
190 | controller.buttons.digital.r2 = (buf[4] & 0x02) >> 2;
|
---|
191 | controller.buttons.digital.l1 = (buf[4] & 0x04) >> 3;
|
---|
192 | controller.buttons.digital.r1 = (buf[4] & 0x08) >> 4;
|
---|
193 | controller.buttons.digital.triangle = (buf[4] & 0x10) >> 5;
|
---|
194 | controller.buttons.digital.circle = (buf[4] & 0x20) >> 6;
|
---|
195 | controller.buttons.digital.cross = (buf[4] & 0x40) >> 7;
|
---|
196 | controller.buttons.digital.square = (buf[4] & 0x80) >> 8;
|
---|
197 |
|
---|
198 | /* Sticks */
|
---|
199 | controller.buttons.stick.leftStick_x = buf[7];
|
---|
200 | controller.buttons.stick.leftStick_y = buf[8];
|
---|
201 | controller.buttons.stick.rightStick_x = buf[9];
|
---|
202 | controller.buttons.stick.rightStick_y = buf[10];
|
---|
203 |
|
---|
204 | /* Analog */
|
---|
205 | controller.buttons.analog.l2 = buf[19];
|
---|
206 | controller.buttons.analog.r2 = buf[20];
|
---|
207 | controller.buttons.analog.l1 = buf[21];
|
---|
208 | controller.buttons.analog.r1 = buf[22];
|
---|
209 | controller.buttons.analog.triangle = buf[23];
|
---|
210 | controller.buttons.analog.circle = buf[24];
|
---|
211 | controller.buttons.analog.cross = buf[25];
|
---|
212 | controller.buttons.analog.square = buf[26];
|
---|
213 |
|
---|
214 | /* Axis */
|
---|
215 | controller.buttons.axis.x = buf[43];
|
---|
216 | controller.buttons.axis.y = buf[45];
|
---|
217 | controller.buttons.axis.z = buf[47];
|
---|
218 | controller.buttons.axis.gZ = buf[47];
|
---|
219 |
|
---|
220 | update_shared_memory();
|
---|
221 | }
|
---|
222 |
|
---|
223 | void DualshockAPI::run()
|
---|
224 | {
|
---|
225 | dbtFile.open("dualshock.dbt", WriteMode, FILE_DBT_UNKNOWN,
|
---|
226 | sizeof(struct dualshockButtons_s));
|
---|
227 | quit = 0;
|
---|
228 | main_loop();
|
---|
229 | }
|
---|
230 |
|
---|
231 | DualshockAPI::~DualshockAPI()
|
---|
232 | {
|
---|
233 | delete(shmem);
|
---|
234 | }
|
---|