rtems_status_code bsp_interrupt_facility_initialize(void)
{
rtems_vector_number i = 0;
uint32_t processor_id = ppc_processor_id();
if (ppc_exc_set_handler(ASM_EXT_VECTOR, qoriq_external_exception_handler)) {
return RTEMS_IO_ERROR;
}
if (processor_id == 0) {
/* Core 0 must do the basic initialization */
pic_reset();
for (i = BSP_INTERRUPT_VECTOR_MIN; i <= BSP_INTERRUPT_VECTOR_MAX; ++i) {
volatile uint32_t *base = (volatile uint32_t *) &qoriq.pic;
int offs = vpr_and_dr_offsets [i] << 2;
volatile uint32_t *vpr = base + offs;
*vpr = VPR_MSK | VPR_P | VPR_PRIORITY(1) | VPR_VECTOR(i);
if (!pic_is_ipi(i)) {
volatile uint32_t *dr = base + offs + 4;
*dr = 0x1;
}
}
qoriq.pic.mer03 = 0xf;
qoriq.pic.mer47 = 0xf;
qoriq.pic.svr = SPURIOUS;
qoriq.pic.gcr = GCR_M;
pic_global_timer_init();
}
qoriq.pic.ctpr = 0;
for (i = BSP_INTERRUPT_VECTOR_MIN; i <= BSP_INTERRUPT_VECTOR_MAX; ++i) {
qoriq.pic.iack;
qoriq.pic.eoi = 0;
qoriq.pic.whoami;
}
return RTEMS_SUCCESSFUL;
}
rtems_status_code bsp_interrupt_facility_initialize(void)
{
rtems_vector_number i = 0;
uint32_t processor_id = ppc_processor_id();
#ifndef PPC_EXC_CONFIG_USE_FIXED_HANDLER
if (ppc_exc_set_handler(ASM_EXT_VECTOR, qoriq_external_exception_handler)) {
return RTEMS_IO_ERROR;
}
#endif
if (processor_id == 0) {
/* Core 0 must do the basic initialization */
pic_reset();
for (i = BSP_INTERRUPT_VECTOR_MIN; i <= BSP_INTERRUPT_VECTOR_MAX; ++i) {
volatile qoriq_pic_src_cfg *src_cfg = get_src_cfg(i);
src_cfg->vpr = VPR_MSK | VPR_P | VPR_PRIORITY(1) | VPR_VECTOR(i);
if (!pic_is_ipi(i)) {
src_cfg->dr = 0x1;
}
}
qoriq.pic.mer03 = 0xf;
qoriq.pic.mer47 = 0xf;
qoriq.pic.svr = SPURIOUS;
qoriq.pic.gcr = GCR_M;
pic_global_timer_init();
}
qoriq.pic.ctpr = 0;
for (i = BSP_INTERRUPT_VECTOR_MIN; i <= BSP_INTERRUPT_VECTOR_MAX; ++i) {
qoriq.pic.iack;
qoriq.pic.eoi = 0;
qoriq.pic.whoami;
}
return RTEMS_SUCCESSFUL;
}
void pc_reset()
{
cpu_set();
resetx86();
mem_updatecache();
//timer_reset();
dma_reset();
fdc_reset();
pic_reset();
pit_reset();
serial_reset();
setpitclock(models[model].cpu[cpu_manufacturer].cpus[cpu].rspeed);
// sb_reset();
ali1429_reset();
// video_init();
}
void
pic_decode_16E(_pic * pic)
{
unsigned short temp;
unsigned short opc;
unsigned short bank= (pic->ram[P16E_BSR] & 0x003F)<<7;
unsigned char * status = &pic->ram[bank|(P16E_STATUS & 0x007F)];
unsigned char * intcon = &pic->ram[bank|(P16E_INTCON & 0x007F)];
unsigned short afsr[2];
short jrange;
int afsrpos=0;
if(pic->pc != 0x2004)
opc=pic->prog[pic->pc];
else
opc=pic->id[4];
pic->lram=0x8000;
pic->rram=0x8000;
if(pic->sleep == 1)
{
if(pic->print)printf("sleep WDT=%i wdt=%f ms=%i\n",((pic->config[0] & 0x04) == 0x04 ),pic->twdt,pic->wdt);
return;
}
if(pic->s2 == 1)
{
pic->s2=0;
if(pic->jpc != 0xFFFFF)
{
pic->pc=pic->jpc;
pic->jpc=0xFFFFF;
}
return;
};
//print (Address)
if(pic->print)
{
printf("pc=%#06X\t",pic->pc);
if(pic->pc != 0x2004)
printf("prog=%#06X\t",pic->prog[pic->pc]);
else
printf("prog=%#06X\t",pic->id[4]);
}
pic->pc++;
if(pic->pc >= pic->ROMSIZE) pic->pc=0;
//pc
temp = pic->pc&0x00FF;
pic->ram[(0x0000)|(P16E_PCL & 0x007F)]=temp;
pic->ram[(0x0080)|(P16E_PCL & 0x007F)]=temp;
if(pic->processor != P16F84A)
{
pic->ram[(0x0100)|(P16E_PCL & 0x007F)]=temp;
pic->ram[(0x0180)|(P16E_PCL & 0x007F)]=temp;
}
//pc_ant = temp;
ReadIndf(pic, afsr);
switch(opc & 0x3000)
{
case 0x0000:
switch(opc & 0x0F00)
{
case 0x0000:
switch(opc & 0x0080)
{
case 0x0000:
switch(opc & 0x007F)
{
case 0x0000:
//NOP -- No Operation 1 0000000 0000000
if(pic->print)printf("NOP\n");
break;
case 0x0001:
//RESET - Software device Reset 1 00 0000 0000 0001
if(pic->print)printf("RESET\n");
pic_reset(pic,0);
break;
case 0x0008:
///RETURN -- Return from Subroutine 2 0000000 0001000
if(pic->print)printf("RETURN\n");
pic->jpc=pic->stack[0];
for(temp=0;temp<15;temp++)
pic->stack[temp]=pic->stack[temp+1];
pic->stack[15]=0;
pic->s2=1;
// if((pic->ram[ICKBUG] & 0x80)== 0x80)printf("Out DEBUG mode\n");
pic->debug=0;
pic->sstep=0;
pic->dbg=0;
//TODO P16E DEBUG
//pic->ram[P16E_ICKBUG]&=~0x80;
break;
case 0x0009:
//RETFIE -- Return from interrupt 2 0000000 0001001
if(pic->print)printf("RETFIE\n");
pic->jpc=pic->stack[0];
for(temp=0;temp<15;temp++)
pic->stack[temp]=pic->stack[temp+1];
pic->stack[15]=0;
pic->s2=1;
*intcon|=0x80;
pic->lram=P16E_INTCON;
//restore shadow
int bk;
int offset;
pic->ram[P16E_STATUS]=pic->ram[P16E_STATUS_SHAD];
offset=0x007F&P16E_STATUS;
temp=pic->ram[P16E_STATUS];
for(bk=0;bk<32;bk++)
pic->ram[(0x0080*bk)|offset]=temp;
pic->ram[P16E_WREG]=pic->ram[P16E_WREG_SHAD];
offset=0x007F&P16E_WREG;
temp=pic->ram[P16E_WREG];
for(bk=0;bk<32;bk++)
pic->ram[(0x0080*bk)|offset]=temp;
pic->ram[P16E_BSR]= pic->ram[P16E_BSR_SHAD];
offset=0x007F&P16E_BSR;
temp=pic->ram[P16E_BSR];
for(bk=0;bk<32;bk++)
pic->ram[(0x0080*bk)|offset]=temp;
pic->ram[P16E_PCLATH]=pic->ram[P16E_PCLATH_SHAD];
offset=0x007F&P16E_PCLATH;
temp=pic->ram[P16E_PCLATH];
for(bk=0;bk<32;bk++)
pic->ram[(0x0080*bk)|offset]=temp;
pic->ram[P16E_FSR0L]=pic->ram[P16E_FSR0L_SHAD];
offset=0x007F&P16E_FSR0L;
temp=pic->ram[P16E_FSR0L];
for(bk=0;bk<32;bk++)
pic->ram[(0x0080*bk)|offset]=temp;
pic->ram[P16E_FSR0H]=pic->ram[P16E_FSR0H_SHAD];
offset=0x007F&P16E_FSR0H;
temp=pic->ram[P16E_FSR0H];
for(bk=0;bk<32;bk++)
pic->ram[(0x0080*bk)|offset]=temp;
pic->ram[P16E_FSR1L]=pic->ram[P16E_FSR1L_SHAD];
offset=0x007F&P16E_FSR1L;
temp=pic->ram[P16E_FSR1L];
for(bk=0;bk<32;bk++)
pic->ram[(0x0080*bk)|offset]=temp;
pic->ram[P16E_FSR1H]=pic->ram[P16E_FSR1H_SHAD];
offset=0x007F&P16E_FSR1H;
temp=pic->ram[P16E_FSR1H];
for(bk=0;bk<32;bk++)
pic->ram[(0x0080*bk)|offset]=temp;
break;
case 0x000A:
//CALLW - Call Subroutine with W 2 00 0000 0000 1010
if(pic->print)printf("CALLW\n");
for(temp=15;temp>0;temp--)
pic->stack[temp]=pic->stack[temp-1]; //TODO P16E implement acess to stack and STKPTR
pic->stack[0]=pic->pc;
pic->jpc=((pic->ram[P16E_PCLATH]&0x1F)<<8)|pic->ram[P16E_WREG];
pic->s2=1;
break;
case 0x000B:
//BRW - Relative Branch with W 2 00 0000 0000 1011
if(pic->print)printf("BRW\n");
//TODO P16E DEBUG
/*
if(((pic->config[0] & 0x0800) == 0)&&(((pic->ram[P16E_ICKBUG])&0x80)== 0x80) )//DEBUG ON
// pic->jpc=((0x18)<<8)|(opc & 0x07FF);
pic->jpc=(pic->ROMSIZE-1)&(((0xF8)<<8)|(opc & 0x07FF));
else
*/
pic->jpc=(pic->pc+pic->ram[P16E_WREG])&((pic->ROMSIZE<<1)-1);
pic->s2=1;
break;
case 0x0010 ... 0x0017:
//MOVIW n,m Move Indirect FSRn to W with 1 00 0000 0001 0nmm Z
// pre/post inc/dec modifier, mm
if(pic->print)printf("MOVIW FSR%i, %i\n",(opc & 0x0004) != 0,opc & 0x0003 );
pic->lram=P16E_WREG;
if(opc & 0x0004)//INDF1
{
switch(opc &0x0003)
{
case 0://pre inc
afsr[1]++;
pic->ram[P16E_FSR1H]=(afsr[1] & 0xFF00)>>8;
pic->ram[P16E_FSR1L]=afsr[1] & 0x00FF;
ReadIndf(pic, afsr);
pic->rram=afsr[1];
pic->ram[P16E_WREG]=pic->ram[P16E_INDF1];
break;
case 1://pre dec
afsr[1]--;
pic->ram[P16E_FSR1H]=(afsr[1] & 0xFF00)>>8;
pic->ram[P16E_FSR1L]=afsr[1] & 0x00FF;
ReadIndf(pic, afsr);
pic->rram=afsr[1];
pic->ram[P16E_WREG]=pic->ram[P16E_INDF1];
break;
case 2://post inc
pic->ram[P16E_WREG]=pic->ram[P16E_INDF1];
pic->rram=afsr[1];
afsr[1]++;
pic->ram[P16E_FSR1H]=(afsr[1] & 0xFF00)>>8;
pic->ram[P16E_FSR1L]=afsr[1] & 0x00FF;
break;
case 3://post dec
pic->ram[P16E_WREG]=pic->ram[P16E_INDF1];
pic->rram=afsr[1];
afsr[1]--;
pic->ram[P16E_FSR1H]=(afsr[1] & 0xFF00)>>8;
pic->ram[P16E_FSR1L]=afsr[1] & 0x00FF;
break;
}
}
else//INDF0
{
switch(opc &0x0003)
{
case 0://pre inc
afsr[0]++;
pic->ram[P16E_FSR0H]=(afsr[0] & 0xFF00)>>8;
pic->ram[P16E_FSR0L]=afsr[0] & 0x00FF;
ReadIndf(pic, afsr);
pic->rram=afsr[0];
pic->ram[P16E_WREG]=pic->ram[P16E_INDF0];
break;
case 1://pre dec
afsr[0]--;
pic->ram[P16E_FSR0H]=(afsr[0] & 0xFF00)>>8;
pic->ram[P16E_FSR0L]=afsr[0] & 0x00FF;
ReadIndf(pic, afsr);
pic->rram=afsr[0];
pic->ram[P16E_WREG]=pic->ram[P16E_INDF0];
break;
case 2://post inc
pic->ram[P16E_WREG]=pic->ram[P16E_INDF0];
pic->rram=afsr[0];
afsr[0]++;
pic->ram[P16E_FSR0H]=(afsr[0] & 0xFF00)>>8;
pic->ram[P16E_FSR0L]=afsr[0] & 0x00FF;
break;
case 3://post dec
pic->ram[P16E_WREG]=pic->ram[P16E_INDF0];
pic->rram=afsr[0];
afsr[0]--;
pic->ram[P16E_FSR1H]=(afsr[0] & 0xFF00)>>8;
pic->ram[P16E_FSR1L]=afsr[0] & 0x00FF;
break;
}
}
break;
case 0x0018 ... 0x001F:
//MOVWI n,m Move W to Indirect FSRn with 1 00 0000 0001 1nmm
// pre/post inc/dec modifier, mm
if(pic->print)printf("MOVWI FSR%i, %i\n",(opc & 0x0004) != 0,opc & 0x0003 );
pic->rram=P16E_WREG;
if(opc & 0x0004)//INDF1
{
switch(opc &0x0003)
{
case 0://pre inc
afsr[1]++;
pic->ram[P16E_FSR1H]=(afsr[1] & 0xFF00)>>8;
pic->ram[P16E_FSR1L]=afsr[1] & 0x00FF;
ReadIndf(pic, afsr);
pic->lram=P16E_INDF1;
pic->ram[P16E_INDF1]=pic->ram[P16E_WREG];
break;
case 1://pre dec
afsr[1]--;
pic->ram[P16E_FSR1H]=(afsr[1] & 0xFF00)>>8;
pic->ram[P16E_FSR1L]=afsr[1] & 0x00FF;
ReadIndf(pic, afsr);
pic->lram=P16E_INDF1;
pic->ram[P16E_INDF1]=pic->ram[P16E_WREG];
break;
case 2://post inc
pic->ram[P16E_INDF1]=pic->ram[P16E_WREG];
pic->lram=P16E_INDF1;
afsrpos=+1;
break;
case 3://post dec
pic->ram[P16E_INDF1]=pic->ram[P16E_WREG];
pic->lram=P16E_INDF1;
afsrpos=-1;
break;
}
}
else//INDF0
{
switch(opc &0x0003)
{
case 0://pre inc
afsr[0]++;
pic->ram[P16E_FSR0H]=(afsr[0] & 0xFF00)>>8;
pic->ram[P16E_FSR0L]=afsr[0] & 0x00FF;
ReadIndf(pic, afsr);
pic->lram=P16E_INDF0;
pic->ram[P16E_INDF0]=pic->ram[P16E_WREG];
break;
case 1://pre dec
afsr[0]--;
pic->ram[P16E_FSR0H]=(afsr[0] & 0xFF00)>>8;
pic->ram[P16E_FSR0L]=afsr[0] & 0x00FF;
ReadIndf(pic, afsr);
pic->lram=P16E_INDF0;
pic->ram[P16E_INDF0]=pic->ram[P16E_WREG];
break;
case 2://post inc
pic->ram[P16E_INDF0]=pic->ram[P16E_WREG];
pic->lram=P16E_INDF0;
afsrpos=+1;
break;
case 3://post dec
pic->ram[P16E_INDF0]=pic->ram[P16E_WREG];
pic->lram=P16E_INDF0;
afsrpos=-1;
break;
}
}
break;
case 0x0020 ... 0x003F:
//MOVLB k Move literal to BSR 1 00 0000 001k kkkk
if(pic->print)printf("MOVLB %#04X\n",opc & 0x001F);
pic->ram[P16E_BSR]= (opc & 0x001F);
pic->lram=P16E_BSR;
break;
case 0x0063:
//SLEEP -- Go into Standby mode 1 0000000 1100011 TO,PD
if(pic->print)printf("SLEEP\n");
pic->wdt=0;
pic->sleep=1;
*status&=~0x08;
*status|=0x10;
break;
case 0x0064:
//CLRWDT -- Clear Watchdog Timer 1 0000000 1100100 TO,PD
if(pic->print)printf("CLRWDT\n");
pic->wdt=0;
*status|=0x08;
*status|=0x10;
break;
case 0x0065 ... 0x0067:
//TRIS f Load TRIS register with W 1 00 0000 0110 01ff
if(pic->print)printf("TRIS %#04X\n",opc & 0x0007);
switch(opc&0x0003)
{
case 1:
pic->ram[P16E_TRISA]= pic->ram[P16E_WREG];
pic->lram=P16E_TRISA;
break;
case 2:
pic->ram[P16E_TRISB]= pic->ram[P16E_WREG];
pic->lram=P16E_TRISB;
break;
case 3:
pic->ram[P16E_TRISC]= pic->ram[P16E_WREG];
pic->lram=P16E_TRISC;
break;
}
break;
case 0x006A:
//OPTION - Load OPTION_REG register with W 1 00 0000 0110 0010
if(pic->print)printf("OPTION\n");
pic->ram[P16E_OPTION_REG]= pic->ram[P16E_WREG];
pic->lram=P16E_OPTION_REG;
break;
default:
printf("unknown opcode 0x%04X!\n",opc);
break;
};
break;
case 0x0080:
//MOVWF f Move W to f 1 0000001 fffffff
if(pic->print)printf("MOVWF %#04X\n",opc & 0x007F);
pic->ram[bank|(opc & 0x007F)]=pic->ram[P16E_WREG];
pic->lram=bank|(opc & 0x007F);
break;
default:
printf("unknown opcode 0x%04X!\n",opc);
break;
}
break;
case 0x0100:
switch(opc & 0x0080)
{
case 0x0000:
//CLRW -- Clear W 1 0000010 00000xx Z
if(pic->print)printf("CLRW\n");
pic->ram[P16E_WREG]=0;
pic->lram=P16E_WREG;
break;
case 0x0080:
//CLRF f Clear f 1 0000011 fffffff Z 2
if(pic->print)printf("CLRF %#04X\n",opc & 0x007F);
pic->ram[bank|(opc & 0x007F) ]=0;
pic->lram=bank|(opc & 0x007F);
break;
default:
printf("unknown opcode 0x%04X!\n",opc);
break;
};
break;
case 0x0200:
//SUBWF f, d Subtract W from f 1 000010 dfffffff C,DC,Z 1, 2
if(pic->print)printf("SUBWF %#04X,%d\n",opc & 0x007F,(opc & 0x0080)>>7);
if((0xF0&((0x0F&(~pic->ram[P16E_WREG]))+1+(0x0F&pic->ram[bank|(opc & 0x007F)]))) !=0)
*status|=0x02;
else
*status&=~0x02;
temp=(~pic->ram[P16E_WREG]+1)+pic->ram[bank|(opc & 0x007F)];
if ((0xFF00&temp) != 0)
*status&=~0x01;
else
*status|=0x01;
if((opc & 0x0080) == 0 )
{
pic->ram[P16E_WREG] = (unsigned char)(0x00FF &temp);
pic->lram=P16E_WREG;
if (pic->ram[P16E_WREG] == 0)
*status|=0x04;
else
*status&=~0x04;
}
else
{
pic->ram[bank|(opc & 0x007F)]=(unsigned char)(0x00FF &temp);
pic->lram=bank|(opc & 0x007F);
if (pic->ram[bank|(opc & 0x007F) ] == 0)
*status|=0x04;
else
*status&=~0x04;
};
break;
case 0x0300:
//DECF f, d Decrement f 1 000011 dfffffff Z 1, 2
if(pic->print)printf("DECF %#04X,%d\n",opc & 0x007F,(opc & 0x0080)>>7);
if((opc & 0x0080) == 0 )
{
pic->ram[P16E_WREG]=pic->ram[bank|(opc & 0x007F)]-1;
pic->lram=P16E_WREG;
if (pic->ram[P16E_WREG] == 0)
*status|=0x04;
else
*status&=~0x04;
}
else
{
pic->ram[bank|(opc & 0x007F) ]--;
pic->lram=bank|(opc & 0x007F);
if (pic->ram[bank|(opc & 0x007F) ] == 0)
*status|=0x04;
else
*status&=~0x04;
};
break;
case 0x0400:
//IORWF f, d Inclusive OR W with f 1 000100 dfffffff Z 1, 2
if(pic->print)printf("IORWF %#04X,%d\n",opc & 0x007F,(opc & 0x0080)>>7);
if((opc & 0x0080) == 0 )
{
pic->ram[P16E_WREG]= pic->ram[P16E_WREG] | pic->ram[bank|(opc & 0x007F)];
pic->lram=P16E_WREG;
if (pic->ram[P16E_WREG] == 0)
*status|=0x04;
else
*status&=~0x04;
}
else
{
pic->ram[bank|(opc & 0x007F) ]= pic->ram[P16E_WREG] | pic->ram[bank|(opc & 0x007F)];
pic->lram=bank|(opc & 0x007F);
if (pic->ram[bank|(opc & 0x007F) ] == 0)
*status|=0x04;
else
*status&=~0x04;
};
break;
case 0x0500:
//ANDWF f, d AND W with f 1 000101 dfffffff Z 1, 2
if(pic->print)printf("ANDWF %#04X,%d\n",opc & 0x007F,(opc & 0x0080)>>7);
if((opc & 0x0080) == 0 )
{
pic->ram[P16E_WREG]= pic->ram[P16E_WREG] & pic->ram[bank|(opc & 0x007F)];
pic->lram=P16E_WREG;
if (pic->ram[P16E_WREG] == 0)
*status|=0x04;
else
*status&=~0x04;
}
else
{
pic->ram[bank|(opc & 0x007F) ]= pic->ram[P16E_WREG] & pic->ram[bank|(opc & 0x007F)];
pic->lram=bank|(opc & 0x007F);
if (pic->ram[bank|(opc & 0x007F) ] == 0)
*status|=0x04;
else
*status&=~0x04;
};
break;
case 0x0600:
//XORWF f, d Exclusive OR W with f 1 000110 dfffffff Z 1, 2
if(pic->print)printf("XORWF %#04X,%d\n",opc & 0x007F,(opc & 0x0080)>>7);
if((opc & 0x0080) == 0 )
{
pic->ram[P16E_WREG]= pic->ram[P16E_WREG] ^ pic->ram[bank|(opc & 0x007F)];
pic->lram=P16E_WREG;
if (pic->ram[P16E_WREG] == 0)
*status|=0x04;
else
*status&=~0x04;
}
else
{
pic->ram[bank|(opc & 0x007F) ]= pic->ram[P16E_WREG] ^ pic->ram[bank|(opc & 0x007F)];
pic->lram=bank|(opc & 0x007F);
if (pic->ram[bank|(opc & 0x007F) ] == 0)
*status|=0x04;
else
*status&=~0x04;
};
break;
case 0x0700:
//ADDWF f, d Add W and f 1 000111 dfffffff C,DC,Z 1, 2
if(pic->print)printf("ADDWF %#04X,%d\n",opc & 0x007F,(opc & 0x0080)>>7);
if((0xF0&((0x0F&pic->ram[P16E_WREG])+(0x0F&pic->ram[bank|(opc & 0x007F)]))) !=0)
*status|=0x02;
else
*status&=~0x02;
temp=pic->ram[P16E_WREG]+pic->ram[bank|(opc & 0x007F)];
if ((0xFF00&temp) != 0)
*status|=0x01;
else
*status&=~0x01;
if((opc & 0x0080) == 0 )
{
pic->ram[P16E_WREG] = (unsigned char)(0x00FF &temp);
pic->lram=P16E_WREG;
if (pic->ram[P16E_WREG] == 0)
*status|=0x04;
else
*status&=~0x04;
}
else
{
pic->ram[bank|(opc & 0x007F)]=(unsigned char)(0x00FF &temp);
pic->lram=bank|(opc & 0x007F);
if (pic->ram[bank|(opc & 0x007F) ] == 0)
*status|=0x04;
else
*status&=~0x04;
};
break;
case 0x0800:
//MOVF f, d Move f 1 001000 dfffffff Z 1, 2
if(pic->print)printf("MOVF %#04X,%d\n",opc & 0x007F,(opc & 0x0080)>>7);
if((opc & 0x0080) == 0 )
{
pic->ram[P16E_WREG]=pic->ram[bank|(opc & 0x007F)];
if (pic->ram[P16E_WREG] == 0)
*status|=0x04;
else
*status&=~0x04;
pic->rram=bank|(opc & 0x007F);
pic->lram=P16E_WREG;
}
else
{
if (pic->ram[bank|(opc & 0x007F) ] == 0)
*status|=0x04;
else
*status&=~0x04;
pic->rram=bank|(opc & 0x007F);
pic->lram=bank|(opc & 0x007F);
};
break;
case 0x0900:
//COMF f, d Complement f 1 001001 dfffffff Z 1, 2
if(pic->print)printf("COMF %#04X,%d\n",opc & 0x007F,(opc & 0x0080)>>7);
if((opc & 0x0080) == 0 )
{
pic->ram[P16E_WREG]=~pic->ram[bank|(opc & 0x007F)];
pic->lram=P16E_WREG;
if (pic->ram[P16E_WREG] == 0)
*status|=0x04;
else
*status&=~0x04;
}
else
{
pic->ram[bank|(opc & 0x007F) ]=~pic->ram[bank|(opc & 0x007F) ];
pic->lram=bank|(opc & 0x007F);
if (pic->ram[bank|(opc & 0x007F) ] == 0)
*status|=0x04;
else
*status&=~0x04;
};
break;
case 0x0A00:
//INCF f, d Increment f 1 001010 dfffffff Z 1, 2
if(pic->print)printf("INCF %#04X,%d\n",opc & 0x007F,(opc & 0x0080)>>7);
if((opc & 0x0080) == 0 )
{
pic->ram[P16E_WREG]=pic->ram[bank|(opc & 0x007F)]+1;
pic->lram=P16E_WREG;
if (pic->ram[P16E_WREG] == 0)
*status|=0x04;
else
*status&=~0x04;
}
else
{
pic->ram[bank|(opc & 0x007F) ]++;
pic->lram=bank|(opc & 0x007F);
if (pic->ram[bank|(opc & 0x007F) ] == 0)
*status|=0x04;
else
*status&=~0x04;
};
break;
case 0x0B00:
//DECFSZ f, d Decrement f, Skip if 0 1(2) 001011 dfffffff 1, 2, 3
if(pic->print)printf("DECFSZ %#04X,%d\n",opc & 0x007F,(opc & 0x0080)>>7);
if((opc & 0x0080) == 0 )
{
pic->ram[P16E_WREG]=pic->ram[bank|(opc & 0x007F)]-1;
pic->lram=P16E_WREG;
if (pic->ram[P16E_WREG] == 0)
{
pic->jpc=pic->pc+1;
pic->s2=1;
};
}
else
{
pic->ram[bank|(opc & 0x007F) ]--;
pic->lram=bank|(opc & 0x007F);
if (pic->ram[bank|(opc & 0x007F) ] == 0)
{
pic->jpc=pic->pc+1;
pic->s2=1;
};
};
break;
case 0x0C00:
//RRF f, d Rotate Right f through Carry 1 001100 dfffffff C 1, 2
if(pic->print)printf("RRF %#04X,%d\n",opc & 0x007F,(opc & 0x0080)>>7);
temp=(pic->ram[bank|(opc & 0x007F)]);
if((*status&0x01) ==0x01)temp|=0x0100;
if((temp&0x0001) ==0x01)
*status|=0x01;
else
*status&=~0x01;
temp=temp>>1;
if((opc & 0x0080) == 0 )
{
pic->ram[P16E_WREG]=0x00FF&temp;
pic->lram=P16E_WREG;
}
else
{
pic->ram[bank|(opc & 0x007F) ]=0x00FF&temp;
pic->lram=bank|(opc & 0x007F);
};
break;
case 0x0D00:
//RLF f, d Rotate Left f through Carry 1 001101 dfffffff C 1, 2
if(pic->print)printf("RLF %#04X,%d\n",opc & 0x007F,(opc & 0x0080)>>7);
temp=(pic->ram[bank|(opc & 0x007F)]);
temp=temp<<1;
if((*status&0x01) ==0x01)temp|=0x0001;
if((temp&0x0100) ==0x0100)
*status|=0x0001;
else
*status&=~0x0001;
if((opc & 0x0080) == 0 )
{
pic->ram[P16E_WREG]=0x00FF & temp;
pic->lram=P16E_WREG;
}
else
{
pic->ram[bank|(opc & 0x007F) ]= 0x00FF & temp;
pic->lram=bank|(opc & 0x007F);
};
break;
case 0x0E00:
//SWAPF f, d Swap nibbles in f 1 001110 dfffffff 1, 2
if(pic->print)printf("SWAPF %#04X,%d\n",opc & 0x007F,(opc & 0x0080)>>7);
if((opc & 0x0080) == 0 )
{
pic->ram[P16E_WREG]=((pic->ram[bank|(opc & 0x007F)]&0x0F)<<4)|
((pic->ram[bank|(opc & 0x007F)]&0xF0)>>4);
pic->lram=P16E_WREG;
}
static void pic_ioport_write(void *opaque, target_phys_addr_t addr64,
uint64_t val64, unsigned size)
{
PicState *s = opaque;
uint32_t addr = addr64;
uint32_t val = val64;
int priority, cmd, irq;
DPRINTF("write: addr=0x%02x val=0x%02x\n", addr, val);
if (addr == 0) {
if (val & 0x10) {
/* init */
pic_reset(s);
/* deassert a pending interrupt */
qemu_irq_lower(s->pics_state->parent_irq);
s->init_state = 1;
s->init4 = val & 1;
s->single_mode = val & 2;
if (val & 0x08)
hw_error("level sensitive irq not supported");
} else if (val & 0x08) {
if (val & 0x04)
s->poll = 1;
if (val & 0x02)
s->read_reg_select = val & 1;
if (val & 0x40)
s->special_mask = (val >> 5) & 1;
} else {
cmd = val >> 5;
switch(cmd) {
case 0:
case 4:
s->rotate_on_auto_eoi = cmd >> 2;
break;
case 1: /* end of interrupt */
case 5:
priority = get_priority(s, s->isr);
if (priority != 8) {
irq = (priority + s->priority_add) & 7;
s->isr &= ~(1 << irq);
if (cmd == 5)
s->priority_add = (irq + 1) & 7;
pic_update_irq(s->pics_state);
}
break;
case 3:
irq = val & 7;
s->isr &= ~(1 << irq);
pic_update_irq(s->pics_state);
break;
case 6:
s->priority_add = (val + 1) & 7;
pic_update_irq(s->pics_state);
break;
case 7:
irq = val & 7;
s->isr &= ~(1 << irq);
s->priority_add = (irq + 1) & 7;
pic_update_irq(s->pics_state);
break;
default:
/* no operation */
break;
}
}
} else {
