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