1 #include "lolevel.h"
2 #include "platform.h"
3 #include "core.h"
4 #include "conf.h"
5 #include "keyboard.h"
6 #include "kbd_common.h"
7
8 long kbd_new_state[3] = { 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF };
9 long kbd_prev_state[3] = { 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF };
10 long kbd_mod_state[3] = { 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF };
11
12 KeyMap keymap[] = {
13 /* tiny bug: key order matters. see kbd_get_pressed_key()
14 * for example
15 */
16 {1, KEY_UP , 0x00000020 },
17 {1, KEY_DOWN , 0x00000040 },
18 {1, KEY_LEFT , 0x00000080 },
19 {1, KEY_RIGHT , 0x00000100 },
20 {1, KEY_SET , 0x00000200 },
21 {1, KEY_SHOOT_FULL , 0x00000006 }, // note 6 here!
22 {1, KEY_SHOOT_FULL_ONLY , 0x00000004 },
23 {1, KEY_SHOOT_HALF , 0x00000002 },
24 {1, KEY_ZOOM_IN , 0x00000008 },
25 {1, KEY_ZOOM_OUT , 0x00000010 },
26 {1, KEY_MENU , 0x00000400 },
27 {1, KEY_DISPLAY , 0x00000800 },
28 {1, KEY_PRINT , 0x00002000 },
29 {0, 0, 0 }
30 };
31
32 //get some vxworks defines for semaphore stuff
33 #define STATUS int
34 #define BOOL int
35 #define OK 0
36 #define ERROR (-1)
37
38 #define SEM_Q_FIFO 0x0
39 #define SEM_Q_PRIORITY 0x1
40 #define SEM_DELETE_SAFE 0x4
41 #define SEM_INVERSION_SAFE 0x8
42 #define SEM_OPTION_MASK (SEM_Q_FIFO|SEM_Q_PRIORITY|SEM_DELETE_SAFE|SEM_INVERSION_SAFE)
43
44 #define NO_WAIT 0
45 #define WAIT_FOREVER (-1)
46
47 typedef enum {
48
49 SEM_EMPTY =0,
50 SEM_FULL =1
51
52 } SEM_B_STATE;
53
54 typedef int SEM_ID;
55 extern SEM_ID _semBCreate(int options, SEM_B_STATE initialState);
56 //extern STATUS _TakeSemaphore(SEM_ID, int);
57 //extern STATUS _GiveSemaphore(SEM_ID);
58 SEM_ID semBinary;
59
60 static int kbd_data_process_request_data=0;
61
62 // extern void _platformsub_kbd_fetch_data(long*);
63 long __attribute__((naked)) wrap_kbd_p1_f();
64
65 //KBD HACK
66 // long kbd_process_copy();
67 // static int key_pressed;
68 // extern void script_start();
69 // extern void script_end();
70 // extern void process_script();
71 extern void msleep(long);
72 // static int kbd_blocked;
73
74
75 // extern void h_kbd_p2_f();
76
77
78 void __attribute__((naked,noinline)) mykbd_task_proceed()
79 {
80 asm volatile
81 (
82 "STMFD SP!, {R4-R8,LR}\n"
83 "LDR R3, =0x20F8\n"
84 "SUB SP, SP, #8\n"
85 "LDR R2, [R3]\n"
86 "CMP R2, #2\n"
87 "BNE loc_FF830870\n"
88 "LDR R6, =0x11A48\n"
89 "MOV R8, SP\n"
90 "ADD R7, SP, #0x20-0x1C\n"
91
92 "loc_FF83075C:\n"
93 "MOV R0, R8\n"
94 "BL _GetSystemTime\n"
95 "LDR R1, =wrap_kbd_p1_f\n" // (0xff8308e8)Replaced call to my own function...
96 "LDR R0, [SP,#0x20-0x20]\n"
97 "MOV R3, R7\n"
98 "ADD R0, R0, #0xA\n"
99 "MOV R2, R1\n"
100 "BL _SetTimerWhen\n"
101 "CMP R0, #0x15\n"
102 "MVN R3, R0\n"
103 "ANDNE R0, R3, #1\n"
104 "MOVEQ R0, #1\n"
105 "CMP R0, #0\n"
106 "BNE loc_FF8307A8\n"
107 "MOV R2, #0x1F0\n"
108 "LDR R0, =0xFF8306F4\n" //aError_tHtimer1
109 "LDR R1, =0xFF830000\n" //=aPhyswmngr_c
110 "ADD R2, R2, #2\n"
111 "BL sub_FF814C10\n"
112
113 "loc_FF8307A8:\n"
114 "LDR R3, =0x11AA0\n"
115 "LDR R4, =0x11728\n"
116 "LDR R5, =0x11A60\n"
117 "LDR R0, [R3]\n"
118 "MOV R1, #0\n"
119 "BL _TakeSemaphore\n"
120 "LDR R2, [R4,#8]\n"
121 "LDR R3, [R5,#0x2C]\n"
122 "TST R2, R3\n"
123 "BEQ loc_FF8307E0\n"
124 "LDR R0, =0x119A8\n"
125 "LDR R1, [R6,#0xC]\n"
126 "BL sub_FF830C5C\n"
127 "LDR R2, [R4,#8]\n"
128
129 "loc_FF8307E0:\n"
130 "LDR R3, [R5,#0x20]\n"
131 "TST R2, R3\n"
132 "BEQ loc_FF8307F8\n"
133 "LDR R0, =0x11958\n"
134 "LDR R1, [R6,#8]\n"
135 "BL sub_FF830C5C\n"
136
137 "loc_FF8307F8:\n"
138 "LDR R0, [R4,#4]\n"
139 "LDR R3, [R5,#0x10]\n"
140 "TST R0, R3\n"
141 "BEQ loc_FF830818\n"
142 "LDR R0, =0x11908\n"
143 "LDR R1, [R6,#4]\n"
144 "BL sub_FF830D10\n"
145 "LDR R0, [R4,#4]\n"
146
147 "loc_FF830818:\n"
148 "LDR R1, [R5,#4]\n"
149 "LDR R2, [R4,#8]\n"
150 "LDR R3, [R5,#8]\n"
151 "AND R1, R0, R1\n"
152 "AND R2, R2, R3\n"
153 "ORRS R1, R1, R2\n"
154 "BEQ loc_FF830844\n"
155 "LDR R0, =0x118b8\n"
156 "LDR R1, [R6]\n"
157 "BL sub_FF830C5C\n"
158 "LDR R0, [R4,#4]\n"
159
160 "loc_FF830844:\n"
161 "LDR R3, [R5,#0x34]\n"
162 "TST R0, R3\n"
163 "BEQ loc_FF83085C\n"
164 "LDR R0, =0x119f8\n"
165 "LDR R1, [R6,#0x10]\n"
166 "BL sub_FF830C5C\n"
167
168 "loc_FF83085C:\n"
169 "BL sub_FF8310C8\n"
170 "LDR R3, =0x20F8\n"
171 "LDR R2, [R3]\n"
172 "CMP R2, #2\n"
173 "BEQ loc_FF83075C\n"
174
175 "loc_FF830870:\n"
176 "LDR R3, =0x11A9C\n"
177 "LDR R1, =0x20F8\n"
178 "MOV R2, #3\n"
179 "LDR R0, [R3]\n"
180 "STR R2, [R1]\n"
181 "BL sub_FF823144\n"
182 "BL _ExitTask\n"
183 "ADD SP, SP, #8\n"
184 "LDMFD SP!, {R4-R8,PC}\n"
185 );
186 }//ixus30 ok
187
188 // no stack manipulation needed here, this task does not call kbd_process()
189 void __attribute__((noinline))
190 mykbd_task(long ua, long ub, long uc, long ud, long ue, long uf)
191 {
192 (void)ua; (void)ub; (void)uc; (void)ud; (void)ue; (void)uf;
193
194 //create semaphore, empty = not available
195 semBinary = _semBCreate(SEM_Q_FIFO|SEM_INVERSION_SAFE, SEM_EMPTY);
196 //lock until 2nd rtask frees it
197 //_TakeSemaphore(semBinary,WAIT_FOREVER);
198
199 mykbd_task_proceed();
200
201 _ExitTask();
202 }
203
204
205
206 long __attribute__((naked,noinline)) wrap_kbd_p1_f()
207 {
208 // kbd_prev_state[0] = kbd_new_state[0];
209 kbd_prev_state[1] = kbd_new_state[1];
210 // kbd_prev_state[2] = kbd_new_state[2];
211
212 asm volatile
213 (
214 "STMFD SP!, {R4,R5,LR}\n" //@ff8308e8
215 "LDR R3, =0x20F4\n"
216 "SUB SP, SP, #8\n"
217 "LDR R2, [R3]\n"
218 "CMP R2, #1\n"
219 "BEQ loc_FF830914\n"
220 "MOV R2, #0x210\n"
221 "LDR R0, =0xFF8308D0\n"
222 "LDR R1, =0xFF830000\n"
223 "ADD R2, R2, #3\n"
224 "BL sub_FF814C10\n"
225
226 "loc_FF830914:\n"
227 "LDR R1, =0x2108\n"
228 "LDR R2, [R1]\n"
229 "ADD R3, R2, #0x32\n"
230 "BIC R4, R3, #0xFF000000\n"
231 "BIC R4, R4, #0xF00000\n"
232 "CMP R2, R4\n"
233 "BCS loc_FF830954\n"
234 "MOV R5, R1\n"
235
236 "loc_FF830934:\n"
237 "BL _GetCurrentMachineTime\n"
238 "CMP R0, R4\n"
239 "BCS loc_FF830984\n"
240 "BL _GetCurrentMachineTime\n"
241 "LDR R3, [R5]\n"
242 "CMP R3, R0\n"
243 "BCC loc_FF830934\n"
244 "B loc_FF830984\n"
245
246 "loc_FF830954:\n"
247 "LDR R2, =0x2108\n"
248 "LDR R3, [R2]\n"
249 "CMP R3, R4\n"
250 "BLS loc_FF830984\n"
251 "MOV R5, R2\n"
252
253 "loc_FF830968:\n"
254 "BL _GetCurrentMachineTime\n"
255 "CMP R0, R4\n"
256 "BCC loc_FF830968\n"
257 "BL _GetCurrentMachineTime\n"
258 "LDR R3, [R5]\n"
259 "CMP R3, R0\n"
260 "BCC loc_FF830968\n"
261
262 "loc_FF830984:\n"
263 "LDR R4, =0x11728\n"
264 "MOV R0, R4\n"
265 "BL platformsub_kbd_fetch_data_my\n" // (sub_ff830e90) Replaced to a call to my own function..
266 "BL sub_FF830AB4\n"
267 "BL sub_FF830B1C\n"
268 "LDR R3, =0x20F4\n"
269 "MOV R2, #0\n"
270 "STR R2, [R3]\n"
271 "BL sub_FF83103C\n"
272 "LDR R3, [R4]\n"
273 "CMP R3, #0\n"
274 "BNE loc_FF8309CC\n"
275 "LDR R3, [R4,#4]\n"
276 "CMP R3, #0\n"
277 "BNE loc_FF8309CC\n"
278 "LDR R3, [R4,#8]\n"
279 "CMP R3, #0\n"
280 "BEQ loc_FF830A0C\n"
281
282 "loc_FF8309CC:\n"
283 "LDR R3, =0x11728\n" // Address of key structure physw_status[]
284 "ADD R1, R3, #0xC\n"
285 "LDMIA R1, {R1,R12}\n"
286 "LDR R2, [R3,#0x14]\n"
287 "LDMIA R3, {R0,LR}\n"
288 "LDR R4, [R3,#8]\n"
289 "LDR R5, =0x11AA0\n"
290 "EOR R1, R1, R0\n"
291 "EOR R12, R12, LR\n"
292 "EOR R2, R2, R4\n"
293 "LDR R0, [R5]\n"
294 "STR R2, [R3,#0x14]\n"
295 "STR R1, [R3,#0xC]\n"
296 "STR R12, [R3,#0x10]\n"
297 "BL sub_FF823144\n"
298 "B loc_FF830A54\n"
299
300 "loc_FF830A0C:\n"
301 "CMP R0, #1\n"
302 "BEQ loc_FF830A24\n"
303 "LDR R3, =0x20F8\n"
304 "LDR R2, [R3]\n"
305 "CMP R2, #2\n"
306 "BEQ loc_FF830A34\n"
307
308 "loc_FF830A24:\n"
309 "LDR R3, =0x11AA0\n"
310 "LDR R0, [R3]\n"
311 "BL sub_FF823144\n"
312 "B loc_FF830A54\n"
313
314 "loc_FF830A34:\n"
315 "MOV R0, SP\n"
316 "BL _GetSystemTime\n"
317 "LDR R1, =wrap_kbd_p1_f\n" // Replaced with a call to my own function ---------------------------------------------------------------------
318 "LDR R0, [SP,#0x14-0x14]\n"
319 "MOV R2, R1\n"
320 "ADD R0, R0, #0xA\n"
321 "ADD R3, SP, #0x14-0x10\n"
322 "BL _SetTimerWhen\n"
323
324 "loc_FF830A54:\n"
325 "ADD SP, SP, #8\n"
326 "LDMFD SP!, {R4,R5,PC}\n"
327 );
328 return 0; // shut up the compiler
329 }
330
331
332 long my_kbd_read_keys(long x){
333 kbd_new_state[1]=x & 0xFFFF;
334
335 //give semaphore away
336 _GiveSemaphore(semBinary);
337 //this should allow other tasks to grab it: (maybe not necessary)
338 msleep(0);
339
340 //take semaphore again
341 _TakeSemaphore(semBinary, WAIT_FOREVER);
342 //*p2=0;
343
344 //data is now in kbd_data_process_request_data :)
345
346 //if (kbd_process_copy() == 0)
347 if (kbd_data_process_request_data == 0){
348 return x;
349 }else{
350 return (kbd_new_state[1]&~KEYS_MASK1) | (kbd_mod_state[1] & KEYS_MASK1);
351 }
352
353 }
354
355 void __attribute__((naked,noinline)) platformsub_kbd_fetch_data_my()
356 {
357 asm volatile
358 (
359 "STMFD SP!, {R4-R7,LR}\n" //@ff830e90
360 "MOV R12, #0x220000\n"
361 "ORR R3, R12, #0x204\n"
362 "SUB SP, SP, #0xC\n"
363 "ORR R3, R3, #0xC0000000\n"
364 //"LDR R2, [R3]\n"
365
366 //**********************************************************
367 //WE HAVE READ 0xC0220204
368 //SEND IT TO usb_power_status_override():
369 "MOV R4, R0\n"
370 "LDR R0, [R3]\n"
371 "STMFD SP!, {R1-R11}\n"
372 "BL usb_power_status_override\n"
373 "LDMFD SP!, {R1-R11}\n"
374 "MOV R2, R0\n"
375 "MOV R0, R4\n"
376 "MOV R12, #0x220000\n" //restore R12
377 //EOF CUSTOM CODE
378 //**********************************************************
379
380 //this is original code, process data somehow
381 "LDR LR, =0x11AB0\n"
382 "LDR R3, [R0,#0x28]\n"
383 "LDR R4, [R0,#0x1C]\n"
384 "LDR R5, [LR,#8]\n"
385 "LDR R1, [LR,#0xC]\n"
386 "EOR R2, R2, R3\n"
387 "AND R2, R2, R4\n"
388 "LDR R3, [R0,#0x10]\n"
389 "EOR R7, R2, R1\n"
390 "EOR R6, R2, R5\n"
391 "ORR R1, R6, R7\n"
392 "EOR R3, R3, R2\n"
393 "BIC R3, R3, R1\n"
394 "STR R3, [R0,#4]\n"
395 "STR R2, [LR,#8]\n"
396 "STR R2, [SP,#0x20-0x1C]\n"
397 "STR R5, [LR,#0xC]\n"
398 "ORR R3, R12, #0x208\n"
399 "ORR R3, R3, #0xC0000000\n"
400 "LDR R1, [R3]\n" // The data read goes into R1 here...
401
402 //my code inserted there to fool around with the keyboard data values...
403 "MOV R2, R0\n"
404 "MOV R0, R1\n" // Pass the keyboard data (in R1) to my function via R0
405 "STMFD SP!, {R1-R11}\n"
406 "BL my_kbd_read_keys\n"
407 "LDMFD SP!, {R1-R11}\n"
408 "MOV R1,R0\n"
409 "MOV R0,R2\n" // restore registers
410 "MOV R12, #0x220000\n"
411 "LDR LR, =0x11AB0\n"
412
413 // Original code continues
414 "MOV R2, #0xFF00\n"
415 "ADD R2, R2, #0xFF\n"
416 "AND R1, R1, R2\n"
417 "STR R1, [SP,#0x20-0x18]\n"
418 "ADD R12, R12, #0x3000\n"
419 "ADD R2, R12, #0xC000001C\n"
420 "LDR R3, [R2]\n"
421 "AND R3, R3, #1\n"
422 "ORR R1, R1, R3,LSL#16\n"
423 "STR R1, [SP,#0x20-0x18]\n"
424 "ADD R12, R12, #0xC0000024\n"
425 "LDR R3, [R12]\n"
426 "LDR R2, [R0,#0x2C]\n"
427 //"AND R3, R3, #1\n"
428 "AND R3, R3, #0\n" // - clear SD card R/O flag
429 "ORR R1, R1, R3,LSL#17\n"
430 "LDR R12, [R0,#0x20]\n"
431 "LDR R4, [LR,#0x10]\n"
432 "EOR R1, R1, R2\n"
433 "LDR R2, [LR,#0x14]\n"
434 "AND R1, R1, R12\n"
435 "LDR R3, [R0,#0x14]\n"
436 "EOR R7, R1, R2\n"
437 "EOR R6, R1, R4\n"
438 "ORR R2, R6, R7\n"
439 "EOR R3, R3, R1\n"
440 "BIC R3, R3, R2\n"
441 "STR R3, [R0,#8]\n"
442 "STR R1, [LR,#0x10]\n"
443 "STR R4, [LR,#0x14]\n"
444 "STR R1, [SP,#0x20-0x18]\n"
445 "ADD SP, SP, #0xC\n"
446 "LDMFD SP!, {R4-R7,PC}\n"
447 );
448 }
449
450
451 /****************/
452 //hack to get a second thread for the keyboard:
453 //purpose: check if kbd_new_data flag is set
454 void kbd_process_task()
455 {
456 while(1){
457 //try to get semaphore
458 //volatile long *p; p=(void*) 0xc02200E4; *p=0x46;
459 _TakeSemaphore(semBinary, WAIT_FOREVER);
460 //*p=0x0;
461
462 //process data & store result
463 kbd_data_process_request_data = kbd_process(); //kbd_process_copy();
464
465 //give semaphore
466 //volatile long *p2; p2=(void*) 0xc02200E0; *p2=0x46;
467 _GiveSemaphore(semBinary);
468 //*p2=0x0;
469
470 //sleep to allow other task to grab it
471 msleep(10);
472 }
473 }