libogc/pad.c

   1 #include <stdlib.h>
   2 #include <stdio.h>
   3 #include <string.h>
   4 #include <time.h>
   5 #include <irq.h>
   6 #include <video.h>
   7 #include <system.h>
   8 #include "asm.h"
   9 #include "processor.h"
  10 #include "si.h"
  11 #include "pad.h"
  12
  13 //#define _PAD_DEBUG
  14
  15 #define PAD_PRODPADS            6
  16
  17 #define _SHIFTL(v, s, w)        \
  18     ((u32) (((u32)(v) & ((0x01 << (w)) - 1)) << (s)))
  19 #define _SHIFTR(v, s, w)        \
  20     ((u32)(((u32)(v) >> (s)) & ((0x01 << (w)) - 1)))
  21
  22 #define PAD_ENABLEDMASK(chan)           (0x80000000>>chan);
  23
  24 typedef struct _keyinput {
  25         s8 stickX;
  26         s8 stickY;
  27         s8 substickX;
  28         s8 substickY;
  29         u8 triggerL;
  30         u8 triggerR;
  31         u16 up;
  32         u16 down;
  33         u16 state;
  34         u32 chan;
  35 } keyinput;
  36
  37 typedef void (*SPECCallback)(u32,u32*,PADStatus*);
  38
  39 static sampling_callback __pad_samplingcallback = NULL;
  40 static SPECCallback __pad_makestatus = NULL;
  41 static u32 __pad_initialized = 0;
  42 static u32 __pad_enabledbits = 0;
  43 static u32 __pad_resettingbits = 0;
  44 static u32 __pad_recalibratebits = 0;
  45 static u32 __pad_waitingbits = 0;
  46 static u32 __pad_pendingbits = 0;
  47 static u32 __pad_checkingbits = 0;
  48 static u32 __pad_resettingchan = 32;
  49 static u32 __pad_spec = 5;
  50
  51 static u32 __pad_analogmode = 0x00000300;
  52 static u32 __pad_cmdreadorigin = 0x41000000;
  53 static u32 __pad_cmdcalibrate = 0x42000000;
  54 static u32 __pad_xpatchbits = 0xf0000000;
  55
  56 static u32 __pad_recalibrated$207 = 0;
  57
  58 static u32 __pad_type[PAD_CHANMAX];
  59 static s8 __pad_origin[PAD_CHANMAX][12];
  60 static u32 __pad_cmdprobedevice[PAD_CHANMAX];
  61
  62 static keyinput __pad_keys[PAD_CHANMAX];
  63 static u8 __pad_clampregion[8] = {30, 180, 15, 72, 40, 15, 59, 31};
  64
  65 static vu32* const _siReg = (u32*)0xCC006400;
  66 static vu16* const _viReg = (u16*)0xCC002000;
  67
  68 extern u32 __PADFixBits;
  69
  70 static void __pad_enable(u32 chan);
  71 static void __pad_disable(u32 chan);
  72 static void __pad_doreset();
  73 static s32 __pad_onreset(s32 final);
  74
  75 static sys_resetinfo pad_resetinfo = {
  76         {},
  77         __pad_onreset,
  78         127
  79 };
  80
  81 extern void udelay(int);
  82
  83 static s32 __pad_onreset(s32 final)
  84 {
  85         u32 ret;
  86
  87         if(__pad_samplingcallback!=NULL) PAD_SetSamplingCallback(NULL);
  88
  89         if(final==FALSE) {
  90                 ret = PAD_Sync();
  91                 if(__pad_recalibrated$207==0 && ret) {
  92                         __pad_recalibrated$207 = PAD_Recalibrate(0xf0000000);
  93                         return 0;
  94                 }
  95                 return ret;
  96         }
  97         __pad_recalibrated$207 = 0;
  98         return 1;
  99 }
 100
 101 static void SPEC0_MakeStatus(u32 chan,u32 *data,PADStatus *status)
 102 {
 103         status->button = 0;
 104
 105         if(data[0]&0x00080000) status->button |= 0x0100;
 106         if(data[0]&0x00200000) status->button |= 0x0200;
 107         if(data[0]&0x01000000) status->button |= 0x0400;
 108         if(data[0]&0x00010000) status->button |= 0x0800;
 109         if(data[0]&0x00100000) status->button |= 0x1000;
 110
 111         status->stickX = (s8)(data[1]>>16);
 112         status->stickY = (s8)(data[1]>>24);
 113         status->substickX = (s8)data[1];
 114         status->substickY = (s8)(data[1]>>8);
 115         status->triggerL = (u8)_SHIFTR(data[0],8,8);
 116         status->triggerR = (u8)(data[0]&0xff);
 117         status->analogA = 0;
 118         status->analogB = 0;
 119
 120         if(status->triggerL>=0xaa) status->button |= 0x40;
 121         if(status->triggerR>=0xaa) status->button |= 0x20;
 122
 123         status->stickX -= 128;
 124         status->stickY -= 128;
 125         status->substickX -= 128;
 126         status->substickY -= 128;
 127 }
 128
 129 static void SPEC1_MakeStatus(u32 chan,u32 *data,PADStatus *status)
 130 {
 131         status->button = 0;
 132
 133         if(data[0]&0x00800000) status->button |= 0x0100;
 134         if(data[0]&0x01000000) status->button |= 0x0200;
 135         if(data[0]&0x00200000) status->button |= 0x0400;
 136         if(data[0]&0x00100000) status->button |= 0x0800;
 137         if(data[0]&0x02000000) status->button |= 0x1000;
 138
 139         status->stickX = (s8)(data[1]>>16);
 140         status->stickY = (s8)(data[1]>>24);
 141         status->substickX = (s8)data[1];
 142         status->substickY = (s8)(data[1]>>8);
 143         status->triggerL = (u8)_SHIFTR(data[0],8,8);
 144         status->triggerR = (u8)data[0]&0xff;
 145         status->analogA = 0;
 146         status->analogB = 0;
 147
 148         if(status->triggerL>=0xaa) status->button |= 0x40;
 149         if(status->triggerR>=0xaa) status->button |= 0x20;
 150
 151         status->stickX -= 128;
 152         status->stickY -= 128;
 153         status->substickX -= 128;
 154         status->substickY -= 128;
 155 }
 156
 157 static s8 __pad_clampS8(s8 var,s8 org)
 158 {
 159         s32 siorg;
 160
 161 #ifdef _PAD_DEBUG
 162         u32 tmp = var;
 163         printf("__pad_clampS8(%d,%d)\n",var,org);
 164 #endif
 165         siorg = (s32)org;
 166         if(siorg>0) {
 167                 siorg -= 128;
 168                 if((s32)var<siorg) var = siorg;
 169         } else if(siorg<0) {
 170                 siorg += 127;
 171                 if(siorg<(s32)var) var = siorg;
 172         }
 173 #ifdef _PAD_DEBUG
 174         printf("__pad_clampS8(%d,%d,%d,%d)\n",tmp,var,org,(var-org));
 175 #endif
 176         return (var-org);
 177 }
 178
 179 static u8 __pad_clampU8(u8 var,u8 org)
 180 {
 181         if(var<org) var = org;
 182         return (var-org);
 183 }
 184
 185 static void SPEC2_MakeStatus(u32 chan,u32 *data,PADStatus *status)
 186 {
 187         u32 mode;
 188
 189         status->button = _SHIFTR(data[0],16,14);
 190
 191         status->stickX = (s8)(data[0]>>8);
 192         status->stickY = (s8)data[0];
 193 #ifdef _PAD_DEBUG
 194         printf("SPEC2_MakeStatus(%d,%p,%p)",chan,data,status);
 195 #endif
 196         mode = __pad_analogmode&0x0700;
 197         if(mode==0x100) {
 198                 status->substickX = (s8)((data[1]>>24)&0xf0);
 199                 status->substickY = (s8)((data[1]>>8)&0xff);
 200                 status->triggerL = (u8)((data[1]>>16)&0xff);
 201                 status->triggerR = (u8)((data[1]>>8)&0xff);
 202                 status->analogA = (u8)(data[1]&0xf0);
 203                 status->analogB = (u8)((data[1]<<4)&0xf0);
 204         } else if(mode==0x200) {
 205                 status->substickX = (s8)((data[1]>>24)&0xf0);
 206                 status->substickY = (s8)((data[1]>>20)&0xf0);
 207                 status->triggerL = (u8)((data[1]>>16)&0xf0);
 208                 status->triggerR = (u8)((data[1]>>12)&0xf0);
 209                 status->analogA = (u8)((data[1]>>8)&0xff);
 210                 status->analogB = (s8)data[1]&0xff;
 211         } else if(mode==0x300) {
 212                 status->substickX = (s8)((data[1]>>24)&0xff);
 213                 status->substickY = (s8)((data[1]>>16)&0xff);
 214                 status->triggerL = (u8)((data[1]>>8)&0xff);
 215                 status->triggerR = (u8)data[1]&0xff;
 216                 status->analogA = 0;
 217                 status->analogB = 0;
 218         } else if(mode==0x400) {
 219                 status->substickX = (s8)((data[1]>>24)&0xff);
 220                 status->substickY = (s8)((data[1]>>16)&0xff);
 221                 status->triggerL = 0;
 222                 status->triggerR = 0;
 223                 status->analogA = (u8)((data[1]>>8)&0xff);
 224                 status->analogB = (u8)data[1]&0xff ;
 225         } else if(!mode || mode==0x500 || mode==0x600 || mode==0x700) {
 226                 status->substickX = (s8)((data[1]>>24)&0xff);
 227                 status->substickY = (s8)((data[1]>>16)&0xff);
 228                 status->triggerL = (u8)((data[1]>>8)&0xf0);
 229                 status->triggerR = (u8)((data[1]>>4)&0xf0);
 230                 status->analogA = (u8)(data[1]&0xf0);
 231                 status->analogB = (u8)((data[1]<<4)&0xf0);
 232         }
 233
 234         status->stickX -= 128;
 235         status->stickY -= 128;
 236         status->substickX -= 128;
 237         status->substickY -= 128;
 238         status->stickX = __pad_clampS8(status->stickX,__pad_origin[chan][2]);
 239         status->stickY = __pad_clampS8(status->stickY,__pad_origin[chan][3]);
 240         status->substickX = __pad_clampS8(status->substickX,__pad_origin[chan][4]);
 241         status->substickY = __pad_clampS8(status->substickY,__pad_origin[chan][5]);
 242         status->triggerL = __pad_clampU8(status->triggerL,__pad_origin[chan][6]);
 243         status->triggerR = __pad_clampU8(status->triggerR,__pad_origin[chan][7]);
 244 }
 245
 246 static void __pad_clampstick(s8 *px,s8 *py,s8 max,s8 xy,s8 min)
 247 {
 248         s32 x,y,signX,signY,d;
 249
 250         x = *px;
 251         y = *py;
 252         if(x>=0) signX = 1;
 253         else { signX = -1; x = -(x); }
 254
 255         if(y>=0) signY = 1;
 256         else { signY = -1; y = -(y); }
 257
 258         if(x<=min) x = 0;
 259         else x -= min;
 260
 261         if(y<=min) y = 0;
 262         else y -= min;
 263
 264         if(x!=0 || y!=0) {
 265                 s32 xx,yy,maxy;
 266
 267                 xx = (x * xy);
 268                 yy= (y * xy);
 269                 maxy = (max * xy);
 270                 if(yy<=xx) {
 271                         d = ((x * xy) + (y * (max - xy)));
 272                         if(maxy<d) {
 273                                 x = ((x * maxy)/d);
 274                                 y = ((y * maxy)/d);
 275                         }
 276                 } else {
 277                         d = ((y * xy) + (x * (max - xy)));
 278                         if(maxy<d) {
 279                                 x = ((x * maxy)/d);
 280                                 y = ((y * maxy)/d);
 281                         }
 282                 }
 283                 *px = (s8)(x * signX);
 284                 *py = (s8)(y * signY);
 285         } else
 286                 *px = *py = 0;
 287 }
 288
 289 static void __pad_clamptrigger(u8 *trigger)
 290 {
 291         u8 min, max;
 292
 293         min = __pad_clampregion[0];
 294         max = __pad_clampregion[1];
 295         if(min>*trigger) *trigger = 0;
 296         else if(max<*trigger) *trigger = (max - min);
 297         else *trigger -= min;
 298 }
 299
 300 static void __pad_updateorigin(s32 chan)
 301 {
 302         u32 mode,mask,type;
 303
 304         mask = PAD_ENABLEDMASK(chan);
 305         mode = __pad_analogmode&0x0700;
 306         if(mode==0x0100) {
 307                 __pad_origin[chan][4] &= ~0x0f;
 308                 __pad_origin[chan][5] &= ~0x0f;
 309                 __pad_origin[chan][8] &= ~0x0f;
 310                 __pad_origin[chan][9] &= ~0x0f;
 311         } else if(mode==0x200) {
 312                 __pad_origin[chan][4] &= ~0x0f;
 313                 __pad_origin[chan][5] &= ~0x0f;
 314                 __pad_origin[chan][6] &= ~0x0f;
 315                 __pad_origin[chan][7] &= ~0x0f;
 316         }
 317
 318         __pad_origin[chan][2] -= 128;
 319         __pad_origin[chan][3] -= 128;
 320         __pad_origin[chan][4] -= 128;
 321         __pad_origin[chan][5] -= 128;
 322
 323         if(__pad_xpatchbits&mask && (s32)__pad_origin[chan][2]>64) {
 324                 type = SI_GetType(chan)&~0xffff;
 325                 if(!(type&~0x09ffffff)) __pad_origin[chan][2] = 0;
 326         }
 327 }
 328
 329 static void __pad_probecallback(s32 chan,u32 type)
 330 {
 331         if(!(type&0x0f)) {
 332                 __pad_enable(__pad_resettingchan);
 333                 __pad_waitingbits |= PAD_ENABLEDMASK(__pad_resettingchan);
 334         }
 335         __pad_doreset();
 336 }
 337
 338 static void __pad_origincallback(s32 chan,u32 type)
 339 {
 340 #ifdef _PAD_DEBUG
 341         printf("__pad_origincallback(%d,%08x)\n",chan,type);
 342 #endif
 343         if(!(type&0x0f)) {
 344                 __pad_updateorigin(__pad_resettingchan);
 345                 __pad_enable(__pad_resettingchan);
 346         }
 347         __pad_doreset();
 348 }
 349
 350 static void __pad_originupdatecallback(s32 chan,u32 type)
 351 {
 352         u32 en_bits = __pad_enabledbits&PAD_ENABLEDMASK(chan);
 353
 354         if(en_bits) {
 355                 if(!(type&0x0f)) __pad_updateorigin(chan);
 356                 if(type&SI_ERROR_NO_RESPONSE) __pad_disable(chan);
 357         }
 358 }
 359
 360 static void __pad_typeandstatuscallback(s32 chan,u32 type)
 361 {
 362         u32 recal_bits,mask,ret = 0;
 363 #ifdef _PAD_DEBUG
 364         printf("__pad_typeandstatuscallback(%d,%08x)\n",chan,type);
 365 #endif
 366         mask = PAD_ENABLEDMASK(__pad_resettingchan);
 367         recal_bits = __pad_recalibratebits&mask;
 368         __pad_recalibratebits &= ~mask;
 369
 370         if(type&0x0f) {
 371                 __pad_doreset();
 372                 return;
 373         }
 374
 375         __pad_type[__pad_resettingchan] = (type&~0xff);
 376         if(((type&SI_TYPE_MASK)-SI_TYPE_GC)
 377                 || !(type&SI_GC_STANDARD)) {
 378                 __pad_doreset();
 379                 return;
 380         }
 381
 382         if(__pad_spec<2) {
 383                 __pad_enable(__pad_resettingchan);
 384                 __pad_doreset();
 385                 return;
 386         }
 387
 388         if(!(type&SI_GC_WIRELESS) || type&SI_WIRELESS_IR) {
 389                 if(recal_bits) ret = SI_Transfer(__pad_resettingchan,&__pad_cmdcalibrate,3,__pad_origin[__pad_resettingchan],10,__pad_origincallback,0);
 390                 else ret = SI_Transfer(__pad_resettingchan,&__pad_cmdreadorigin,1,__pad_origin[__pad_resettingchan],10,__pad_origincallback,0);
 391         } else if(type&SI_WIRELESS_FIX_ID && !(type&SI_WIRELESS_CONT_MASK) && !(type&SI_WIRELESS_LITE)) {
 392                 if(type&SI_WIRELESS_RECEIVED) ret = SI_Transfer(__pad_resettingchan,&__pad_cmdreadorigin,1,__pad_origin[__pad_resettingchan],10,__pad_origincallback,0);
 393                 else ret = SI_Transfer(__pad_resettingchan,&__pad_cmdprobedevice[__pad_resettingchan],3,__pad_origin[__pad_resettingchan],8,__pad_probecallback,0);
 394         }
 395         if(!ret) {
 396                 __pad_pendingbits |= mask;
 397                 __pad_doreset();
 398         }
 399 }
 400
 401 static void __pad_receivecheckcallback(s32 chan,u32 type)
 402 {
 403         u32 mask,tmp;
 404 #ifdef _PAD_DEBUG
 405         printf("__pad_receivecheckcallback(%d,%08x)\n",chan,type);
 406 #endif
 407         mask = PAD_ENABLEDMASK(chan);
 408         if(__pad_enabledbits&mask) {
 409                 tmp = type&0xff;
 410                 type &= ~0xff;
 411                 __pad_waitingbits &= ~mask;
 412                 __pad_checkingbits &= ~mask;
 413                 if(!(tmp&0x0f)
 414                         && (type&SI_GC_WIRELESS) && (type&SI_WIRELESS_RECEIVED) && (type&SI_WIRELESS_FIX_ID)
 415                         && !(type&SI_WIRELESS_IR) && !(type&SI_WIRELESS_CONT_MASK) && !(type&SI_WIRELESS_LITE))  SI_Transfer(chan,&__pad_cmdreadorigin,1,__pad_origin[chan],10,__pad_originupdatecallback,0);
 416                 else __pad_disable(chan);
 417         }
 418 }
 419
 420 static void __pad_enable(u32 chan)
 421 {
 422         u32 buf[2];
 423 #ifdef _PAD_DEBUG
 424         printf("__pad_enable(%d)\n",chan);
 425 #endif
 426         __pad_enabledbits |= PAD_ENABLEDMASK(chan);
 427         SI_GetResponse(chan,(void*)buf);
 428         SI_SetCommand(chan,(__pad_analogmode|0x00400000));
 429         SI_EnablePolling(__pad_enabledbits);
 430 }
 431
 432 static void __pad_disable(u32 chan)
 433 {
 434         u32 level,mask;
 435 #ifdef _PAD_DEBUG
 436         printf("__pad_disable(%d)\n",chan);
 437 #endif
 438         _CPU_ISR_Disable(level);
 439         mask = PAD_ENABLEDMASK(chan);
 440         SI_DisablePolling(mask);
 441         __pad_enabledbits &= ~mask;
 442         __pad_waitingbits &= ~mask;
 443         __pad_pendingbits &= ~mask;
 444         __pad_checkingbits &= ~mask;
 445         SYS_SetWirelessID(chan,0);
 446         _CPU_ISR_Restore(level);
 447 }
 448
 449 static void __pad_doreset()
 450 {
 451         __pad_resettingchan = cntlzw(__pad_resettingbits);
 452         if(__pad_resettingchan==32) return;
 453 #ifdef _PAD_DEBUG
 454         printf("__pad_doreset(%d)\n",__pad_resettingchan);
 455 #endif
 456         __pad_resettingbits &= ~PAD_ENABLEDMASK(__pad_resettingchan);
 457
 458         memset(__pad_origin[__pad_resettingchan],0,12);
 459         SI_GetTypeAsync(__pad_resettingchan,__pad_typeandstatuscallback);
 460 }
 461
 462 static void __pad_samplinghandler(u32 irq,void *ctx)
 463 {
 464 }
 465
 466 u32 __PADDisableRecalibration(s32 disable)
 467 {
 468         u32 level,ret;
 469         u8 *ram_recaldis = (u8*)0x800030e3;
 470
 471         _CPU_ISR_Disable(level);
 472
 473         ret = 0;
 474         if(ram_recaldis[0]&0x40) ret = 1;
 475
 476         ram_recaldis[0] &= 0xbf;
 477         if(disable) ram_recaldis[0] |= 0x40;
 478
 479         _CPU_ISR_Restore(level);
 480
 481         return ret;
 482 }
 483
 484 u32 PAD_Init()
 485 {
 486         u32 chan;
 487         u16 prodpads = PAD_PRODPADS;
 488 #ifdef _PAD_DEBUG
 489         printf("PAD_Init()\n");
 490 #endif
 491         if(__pad_initialized) return 1;
 492
 493         if(__pad_spec) PAD_SetSpec(__pad_spec);
 494
 495         memset(__pad_keys,0,sizeof(keyinput)*PAD_CHANMAX);
 496
 497         __pad_recalibratebits = 0xf0000000;
 498
 499         chan = 0;
 500         while(chan<4) {
 501                 __pad_keys[chan].chan = -1;
 502                 __pad_cmdprobedevice[chan] = 0x4d000000|(chan<<22)|_SHIFTL(prodpads,8,14);
 503                 chan++;
 504         }
 505
 506         SI_RefreshSamplingRate();
 507         SYS_RegisterResetFunc(&pad_resetinfo);
 508
 509         __pad_initialized = 1;
 510         return PAD_Reset(0xf0000000);
 511 }
 512
 513 u32 PAD_Read(PADStatus *status)
 514 {
 515         u32 chan,mask,ret;
 516         u32 level,sistatus,type;
 517         u32 buf[2];
 518 #ifdef _PAD_DEBUG
 519         printf("PAD_Read(%p)\n",status);
 520 #endif
 521         _CPU_ISR_Disable(level);
 522         chan = 0;
 523         ret = 0;
 524         while(chan<4) {
 525                 mask = PAD_ENABLEDMASK(chan);
 526 #ifdef _PAD_DEBUG
 527                 printf("PAD_Read(%d,%d,%08x,%08x,%08x)\n",chan,__pad_resettingchan,(__pad_pendingbits&mask),(__pad_resettingbits&mask),(__pad_enabledbits&mask));
 528 #endif
 529                 if(__pad_pendingbits&mask) {
 530                         PAD_Reset(0);
 531                         memset(&status[chan],0,sizeof(PADStatus));
 532                         status[chan].err = PAD_ERR_NOT_READY;
 533                 } else if(__pad_resettingbits&mask || __pad_resettingchan==chan) {
 534                         memset(&status[chan],0,sizeof(PADStatus));
 535                         status[chan].err = PAD_ERR_NOT_READY;
 536                 } else if(!(__pad_enabledbits&mask)) {
 537                         memset(&status[chan],0,sizeof(PADStatus));
 538                         status[chan].err = PAD_ERR_NO_CONTROLLER;
 539                 } else {
 540                         if(SI_IsChanBusy(chan)) {
 541                                 memset(&status[chan],0,sizeof(PADStatus));
 542                                 status[chan].err = PAD_ERR_TRANSFER;
 543                         } else {
 544                                 sistatus = SI_GetStatus(chan);
 545 #ifdef _PAD_DEBUG
 546                                 printf("PAD_Read(%08x)\n",sistatus);
 547 #endif
 548                                 if(sistatus&SI_ERROR_NO_RESPONSE) {
 549 #ifdef _PAD_DEBUG
 550                                         printf("PAD_Read(%08x)\n",sistatus);
 551 #endif
 552                                         SI_GetResponse(chan,(void*)buf);
 553                                         if(!(__pad_waitingbits&mask)) {
 554                                                 memset(&status[chan],0,sizeof(PADStatus));
 555                                                 status[chan].err = PAD_ERR_NONE;
 556                                                 if(!(__pad_checkingbits&mask)) {
 557                                                         __pad_checkingbits |= mask;
 558                                                         SI_GetTypeAsync(chan,__pad_receivecheckcallback);
 559                                                 }
 560                                         } else {
 561                                                 __pad_disable(chan);
 562                                                 memset(&status[chan],0,sizeof(PADStatus));
 563                                                 status[chan].err = PAD_ERR_NO_CONTROLLER;
 564                                         }
 565                                 } else {
 566                                         type = SI_GetType(chan);
 567 #ifdef _PAD_DEBUG
 568                                         printf("PAD_Read(%08x)\n",type);
 569 #endif
 570                                         if(!(type&SI_WIRELESS_STATE)) ret |= mask;
 571                                         if(!SI_GetResponse(chan,buf)
 572                                                 || buf[0]&0x80000000) {
 573 #ifdef _PAD_DEBUG
 574                                                 printf("PAD_Read(%08x %08x)\n",buf[0],buf[1]);
 575 #endif
 576                                                 memset(&status[chan],0,sizeof(PADStatus));
 577                                                 status[chan].err = PAD_ERR_TRANSFER;
 578                                         } else {
 579 #ifdef _PAD_DEBUG
 580                                                 printf("PAD_Read(%08x %08x)\n",buf[0],buf[1]);
 581 #endif
 582                                                 __pad_makestatus(chan,buf,&status[chan]);
 583 #ifdef _PAD_DEBUG
 584                                                 printf("PAD_Read(%08x)\n",status[chan].button);
 585 #endif
 586                                                 if(status[chan].button&0x00002000) {
 587                                                         memset(&status[chan],0,sizeof(PADStatus));
 588                                                         status[chan].err = PAD_ERR_TRANSFER;
 589                                                         SI_Transfer(chan,&__pad_cmdreadorigin,1,__pad_origin[chan],10,__pad_originupdatecallback,0);
 590                                                 } else {
 591                                                         status[chan].err = PAD_ERR_NONE;
 592                                                         status[chan].button &= ~0x80;
 593                                                 }
 594                                         }
 595                                 }
 596                         }
 597                 }
 598                 chan++;
 599
 600         }
 601         _CPU_ISR_Restore(level);
 602
 603         return ret;
 604 }
 605
 606 u32 PAD_Reset(u32 mask)
 607 {
 608         u32 level;
 609         u32 pend_bits,en_bits;
 610
 611         _CPU_ISR_Disable(level);
 612         pend_bits = (__pad_pendingbits|mask);
 613         __pad_pendingbits = 0;
 614
 615         pend_bits &= ~(__pad_waitingbits|__pad_checkingbits);
 616         __pad_resettingbits |= pend_bits;
 617
 618         en_bits = (__pad_resettingbits&__pad_enabledbits);
 619         __pad_enabledbits &= ~pend_bits;
 620
 621         if(__pad_spec==4) __pad_recalibratebits |= pend_bits;
 622
 623         SI_DisablePolling(en_bits);
 624         if(__pad_resettingchan==32) __pad_doreset();
 625         _CPU_ISR_Restore(level);
 626
 627         return 1;
 628 }
 629
 630 u32 PAD_Recalibrate(u32 mask)
 631 {
 632         u32 level;
 633
 634         _CPU_ISR_Disable(level);
 635
 636         _CPU_ISR_Restore(level);
 637         return 1;
 638 }
 639
 640 u32 PAD_Sync()
 641 {
 642         u32 ret = 0;
 643
 644         if(!__pad_resettingbits && __pad_resettingchan==32) {
 645                 if(SI_Busy()==0) ret = 1;
 646         }
 647         return ret;
 648 }
 649
 650 void PAD_SetSpec(u32 spec)
 651 {
 652         if(__pad_initialized) return;
 653
 654         __pad_spec = 0;
 655         if(spec==0) __pad_makestatus = SPEC0_MakeStatus;
 656         else if(spec==1) __pad_makestatus = SPEC1_MakeStatus;
 657         else if(spec<6) __pad_makestatus = SPEC2_MakeStatus;
 658
 659         __pad_spec = spec;
 660 }
 661
 662 void PAD_ControlMotor(s32 chan,u32 cmd)
 663 {
 664         u32 level;
 665         u32 mask,type;
 666
 667         _CPU_ISR_Disable(level);
 668
 669         mask = PAD_ENABLEDMASK(chan);
 670         if(__pad_enabledbits&mask) {
 671                 type = SI_GetType(chan);
 672                 if(!(type&SI_GC_NOMOTOR)) {
 673                         if(__pad_spec<2 && cmd==PAD_MOTOR_STOP_HARD) cmd = 0;
 674
 675                         cmd = 0x00400000|__pad_analogmode|(cmd&0x03);
 676                         SI_SetCommand(chan,cmd);
 677                         SI_TransferCommands();
 678                 }
 679         }
 680         _CPU_ISR_Restore(level);
 681 }
 682
 683 sampling_callback PAD_SetSamplingCallback(sampling_callback cb)
 684 {
 685         sampling_callback ret;
 686
 687         ret = __pad_samplingcallback;
 688         __pad_samplingcallback = cb;
 689         if(cb) {
 690                 SI_RegisterPollingHandler(__pad_samplinghandler);
 691         } else {
 692                 SI_UnregisterPollingHandler(__pad_samplinghandler);
 693         }
 694
 695         return ret;
 696 }
 697
 698 void PAD_Clamp(PADStatus *status)
 699 {
 700         s32 i;
 701
 702         for(i=0;i<PAD_CHANMAX;i++) {
 703                 if(status[i].err==PAD_ERR_NONE) {
 704                         __pad_clampstick(&status[i].stickX,&status[i].stickY,__pad_clampregion[3],__pad_clampregion[4],__pad_clampregion[2]);
 705                         __pad_clampstick(&status[i].substickX,&status[i].substickY,__pad_clampregion[6],__pad_clampregion[7],__pad_clampregion[5]);
 706                         __pad_clamptrigger(&status[i].triggerL);
 707                         __pad_clamptrigger(&status[i].triggerR);
 708                 }
 709         }
 710 }
 711
 712 u32 PAD_ScanPads()
 713 {
 714         s32 i;
 715         u32 resetBits;
 716         u32 padBit,connected;
 717         u16 state,oldstate;
 718         PADStatus padstatus[PAD_CHANMAX];
 719
 720         resetBits = 0;
 721         connected = 0;
 722
 723         PAD_Read(padstatus);
 724         //PAD_Clamp(padstatus);
 725         for(i=0;i<PAD_CHANMAX;i++) {
 726                 padBit = (PAD_CHAN0_BIT>>i);
 727
 728                 switch(padstatus[i].err) {
 729                 case PAD_ERR_NONE:
 730                         oldstate                                = __pad_keys[i].state;
 731                         state                                   = padstatus[i].button;
 732                         __pad_keys[i].stickX    = padstatus[i].stickX;
 733                         __pad_keys[i].stickY    = padstatus[i].stickY;
 734                         __pad_keys[i].substickX = padstatus[i].substickX;
 735                         __pad_keys[i].substickY = padstatus[i].substickY;
 736                         __pad_keys[i].triggerL  = padstatus[i].triggerL;
 737                         __pad_keys[i].triggerR  = padstatus[i].triggerR;
 738                         __pad_keys[i].up                = oldstate & ~state;
 739                         __pad_keys[i].down              = state & (state ^ oldstate);
 740                         __pad_keys[i].state             = state;
 741                         __pad_keys[i].chan              = i;
 742
 743                         connected |= (1<<i);
 744                         break;
 745
 746                 case PAD_ERR_NO_CONTROLLER:
 747                         if(__pad_keys[i].chan!=-1) memset(&__pad_keys[i],0,sizeof(keyinput));
 748                         __pad_keys[i].chan = -1;
 749                         resetBits |= padBit;
 750                         break;
 751
 752                 default:
 753                         break;
 754                 }
 755         }
 756 #ifdef _PAD_DEBUG
 757         printf("resetBits = %08x\n",resetBits);
 758 #endif
 759         if(resetBits) {
 760                 PAD_Reset(resetBits);
 761         }
 762         return connected;
 763 }
 764
 765
 766 u16 PAD_ButtonsUp(int pad)
 767 {
 768         if(pad<PAD_CHAN0 || pad>PAD_CHAN3 || __pad_keys[pad].chan==-1) return 0;
 769         return __pad_keys[pad].up;
 770 }
 771
 772 u16 PAD_ButtonsDown(int pad)
 773 {
 774         if(pad<PAD_CHAN0 || pad>PAD_CHAN3 || __pad_keys[pad].chan==-1) return 0;
 775         return __pad_keys[pad].down;
 776 }
 777
 778 u16 PAD_ButtonsHeld(int pad)
 779 {
 780         if(pad<PAD_CHAN0 || pad>PAD_CHAN3 || __pad_keys[pad].chan==-1) return 0;
 781         return __pad_keys[pad].state;
 782 }
 783
 784 s8 PAD_SubStickX(int pad)
 785 {
 786         if(pad<PAD_CHAN0 || pad>PAD_CHAN3 || __pad_keys[pad].chan==-1) return 0;
 787         return __pad_keys[pad].substickX;
 788 }
 789
 790 s8 PAD_SubStickY(int pad)
 791 {
 792         if(pad<PAD_CHAN0 || pad>PAD_CHAN3 || __pad_keys[pad].chan==-1) return 0;
 793         return __pad_keys[pad].substickY;
 794 }
 795
 796 s8 PAD_StickX(int pad)
 797 {
 798         if(pad<PAD_CHAN0 || pad>PAD_CHAN3 || __pad_keys[pad].chan==-1) return 0;
 799         return __pad_keys[pad].stickX;
 800 }
 801
 802 s8 PAD_StickY(int pad)
 803 {
 804         if(pad<PAD_CHAN0 || pad>PAD_CHAN3 || __pad_keys[pad].chan==-1) return 0;
 805         return __pad_keys[pad].stickY;
 806 }
 807
 808 u8 PAD_TriggerL(int pad)
 809 {
 810         if(pad<PAD_CHAN0 || pad>PAD_CHAN3 || __pad_keys[pad].chan==-1) return 0;
 811         return __pad_keys[pad].triggerL;
 812 }
 813
 814 u8 PAD_TriggerR(int pad)
 815 {
 816         if(pad<PAD_CHAN0 || pad>PAD_CHAN3 || __pad_keys[pad].chan==-1) return 0;
 817         return __pad_keys[pad].triggerR;
 818 }