int16_t
parse_cmd_adc_get(char *cmd, char *output, uint16_t len)
{
uint16_t adc;
uint8_t channel;
uint8_t ret = 0;
if (cmd[0] && cmd[1])
{
channel = cmd[1] - '0';
if (channel < ADC_CHANNELS)
{
adc = adc_get(channel);
channel = ADC_CHANNELS;
goto adc_out;
}
else
return ECMD_ERR_PARSE_ERROR;
}
for (channel = 0; channel < ADC_CHANNELS; channel++)
{
adc = adc_get(channel);
adc_out:
output[0] = NIBBLE_TO_HEX(LO4(HI8(adc)));
output[1] = NIBBLE_TO_HEX(HI4(LO8(adc)));
output[2] = NIBBLE_TO_HEX(LO4(adc));
output[3] = ' ';
output[4] = 0;
ret += 4;
output += 4;
}
return ECMD_FINAL(ret);
}
void do_if_stage()
{
byte_t instr = HPACK(I_NOP, F_NONE);
byte_t regids = HPACK(REG_NONE, REG_NONE);
word_t valc = 0;
word_t valp = f_pc = gen_f_pc();
/* Ready to fetch instruction. Speculatively fetch register byte
and immediate word
*/
imem_error = !get_byte_val(mem, valp+START_PLACE, &instr);
imem_icode = HI4(instr);
imem_ifun = LO4(instr);
if (!imem_error) {
byte_t junk;
/* Make sure can read maximum length instruction */
imem_error = !get_byte_val(mem, valp+5 + START_PLACE, &junk);
}
if_id_next->icode = gen_f_icode();
if_id_next->ifun = gen_f_ifun();
if (!imem_error) {
sim_log("\tFetch: f_pc = 0x%x, imem_instr = %s, f_instr = %s\n",
f_pc, iname(instr),
iname(HPACK(if_id_next->icode, if_id_next->ifun)));
}
instr_valid = gen_instr_valid();
if (!instr_valid)
sim_log("\tFetch: Instruction code 0x%x invalid\n", instr);
if_id_next->status = gen_f_stat();
valp++;
if (gen_need_regids()) {
get_byte_val(mem, valp + START_PLACE, ®ids);
valp ++;
}
if_id_next->ra = HI4(regids);
if_id_next->rb = LO4(regids);
if (gen_need_valC()) {
get_word_val(mem, valp + START_PLACE, &valc);
valp+= 4;
}
if_id_next->valp = valp;
if_id_next->valc = valc;
/*
if(gen_need_regids() && if_id_next->valc)
printf("##### vap: %d %d %d\n", if_id_next->ra, if_id_next->rb, if_id_next->valc);
*/
pc_next->pc = gen_f_predPC();
pc_next->status = (if_id_next->status == STAT_AOK) ? STAT_AOK : STAT_BUB;
if_id_next->stage_pc = f_pc;
}
void output_byte(uint8_t rs, uint8_t data, uint8_t en)
{
output_nibble(rs, HI4(data), en);
output_nibble(rs, LO4(data), en);
#ifdef HD44780_READBACK
/* wait until command is executed by checking busy flag, with timeout */
/* max execution time is for return home command,
* which takes at most 1.52ms = 1520us */
uint8_t busy, timeout = 200;
do {
busy = input_byte(0,en) & _BV(BUSY_FLAG);
_delay_us(10);
timeout--;
} while (busy && timeout > 0);
#ifdef DEBUG
if (timeout == 0)
debug_printf("lcd timeout!\n");
#endif
#else
/* just wait the maximal time a command can take... */
_delay_ms(2);
#endif
}
uint8_t byte2hex (uint8_t value, char *string)
{
// convert high nibble into hex ascii
string[0] = NIBBLE_TO_HEX(HI4(value));
// convert low nibble into hex ascii
string[1] = NIBBLE_TO_HEX(LO4(value));
return (2);
}
// Look up or generate waveform for ProTracker vibrato/tremolo oscillator
static int8_t do_osc(pto_t *p_osc) {
int8_t sample = 0;
int16_t mul;
switch (p_osc->mode & 0x03) {
case 0: // Sine
sample = pgm_read_byte(&sine_tbl[(p_osc->offset) & 0x1F]);
if (p_osc->offset & 0x20) sample = -sample;
break;
case 1: // Saw
sample = -(p_osc->offset << 2);
break;
case 2: // Square
sample = (p_osc->offset & 0x20) ? 127 : -128;
break;
case 3: // Noise (random)
sample = rand();
break;
}
mul = sample * LO4(p_osc->fxp);
p_osc->offset = (p_osc->offset + HI4(p_osc->fxp));
return mul >> 6;
}
// Look up or generate waveform for ProTracker vibrato/tremolo oscillator
static int8_t do_osc( osc_t *p_osc ) {
int8_t sample = 0;
int16_t mul;
switch( p_osc->mode & 0x03 ) {
case 0: // Sine
sample = ROM_READB( &sine[ ( p_osc->offset ) & 0x3F ] );
if( sample > 127 ) sample -= 256;
break;
case 1: // Square
sample = ( p_osc->offset & 0x20 ) ? 127 : -128;
break;
case 2: // Saw
sample = ( ( p_osc->offset << 2 ) & 0xFF ) - 128;
break;
case 3: // Noise (random)
sample = rand() & 0xFF;
break;
}
mul = sample * LO4( p_osc->fxp );
p_osc->offset = ( p_osc->offset + HI4( p_osc->fxp ) - 1 ) & 0xFF;
return mul / 64;
}
char *amqp_util_encode(char *key, char *dest) {
char *p, *end;
if ((strlen(key) == 1) && (key[0] == '#' || key[0] == '*')) {
*dest++ = key[0];
*dest = '\0';
return dest;
}
for (p = key, end = key + strlen(key); p < end; p++) {
if (KEY_SAFE(*p)) {
*dest++ = *p;
} else if (*p == '.') {
memcpy(dest, "%2E", 3);
dest += 3;
} else if (*p == ' ') {
*dest++ = '+';
} else {
*dest++ = '%';
sprintf(dest, "%c%c", hexint(HI4(*p)), hexint(LO4(*p)));
dest += 2;
}
}
*dest = '\0';
return dest;
}
// Progress module by one tick
static void play_module() {
uint8_t ch, fx, fxp;
uint8_t temp_b;
uint16_t temp_w;
fxm_t *p_fxm;
dma_t *p_dma;
bool pattern_jump = false;
uint8_t ix_period;
uint8_t ix_sample;
uint8_t *p_ptn;
// Advance tick
if( ++tick == speed ) tick = 0;
// Handle row delay
if( delay ) {
if( tick == 0 ) delay--;
return;
}
// Advance playback
if( tick == 0 ) {
if( ++ix_row == 64 ) {
ix_row = 0;
if( ++ix_order == ROM_READB( &p_mod->order_count ) ) ix_order = 0;
}
// Forced order/row
if( ix_nextorder != 0xFF ) {
ix_order = ix_nextorder;
ix_nextorder = 0xFF;
}
if( ix_nextrow != 0xFF ) {
ix_row = ix_nextrow;
ix_nextrow = 0xFF;
}
}
// Set up pointers
p_ptn = ( ( uint8_t* )p_mod )
+ sizeof( ptm_t )
+ ( ROM_READB( &p_mod->order[ ix_order ] ) << 10 )
+ ( ix_row << 4 );
p_fxm = fxm;
p_dma = dma;
for( ch = 0; ch != 4; ch++ ) {
// Deconstruct cell (what the hell were they smoking?)
temp_b = ROM_READB( p_ptn++ ); // sample.msb and period.msb
temp_w = ( temp_b & 0x0F ) << 8;
ix_sample = temp_b & 0xF0;
temp_w |= ROM_READB( p_ptn++ ); // period.lsb
temp_b = ROM_READB( p_ptn++ ); // sample.lsb and effect
ix_sample |= HI4( temp_b );
fx = LO4( temp_b ) << 4;
fxp = ROM_READB( p_ptn++ ); // parameters
if( fx == 0xE0 ) {
fx |= HI4( fxp ); // extended parameters
fxp &= 0x0F;
}
ix_period = 0x7F; // period index
if( temp_w ) {
for( temp_b = 0; temp_b != 36; temp_b++ ) {
if( ROM_READW( &period_tbl[ 0 ][ temp_b ] ) == temp_w ) {
ix_period = temp_b;
break;
}
}
}
// General effect parameter memory
// NOTE: Unsure if this is true to the original ProTracker, but alot of
// modern players and trackers do implement this functionality.
if( fx == 0x10 || fx == 0x20 || fx == 0xE1 || fx == 0xE2 || fx == 0x50 || fx == 0x60 || fx == 0xA0 ) {
if( fxp ) {
p_fxm->param = fxp;
} else {
fxp = p_fxm->param;
}
}
if( tick == ( fx == 0xED ? fxp : 0 ) ) {
if( ix_sample != 0 ) {
// Cell has sample
temp_b = ix_sample - 1;
p_fxm->sample = temp_b;
p_fxm->volume = sample[ temp_b ].volume;
// Reset volume
p_dma->volume = sample[ temp_b ].volume;
}
// Set tuning
if( fx == 0xE5 ) sample[ p_fxm->sample ].tuning = fxp;
// Reset oscillators
if( ( p_fxm->vibr.mode & 0x4 ) == 0x0 ) p_fxm->vibr.offset = 0;
if( ( p_fxm->trem.mode & 0x4 ) == 0x0 ) p_fxm->trem.offset = 0;
if( ix_period != 0x7F ) {
// Cell has note
if( fx == 0x30 || fx == 0x50 ) {
// Tone-portamento effect setup
p_fxm->port_target = ROM_READW( &period_tbl[ sample[ ix_sample ].tuning ][ ix_period ] );
} else {
// Start note
temp_b = p_fxm->sample;
note_start( p_dma, temp_b, ix_period, ( fx == 0x90 ? fxp : 0 ) );
// Set required effect memory parameters
p_fxm->period = ROM_READW( &period_tbl[ sample[ temp_b ].tuning ][ ix_period ] );
}
}
// Effects processed when tick = 0
switch( fx ) {
case 0x30: // Portamento
if( fxp ) p_fxm->port_speed = fxp;
break;
case 0xB0: // Jump to pattern
ix_nextorder = ( fxp >= ROM_READB( &p_mod->order_count ) ? 0x00 : fxp );
ix_nextrow = 0;
pattern_jump = true;
break;
case 0xC0: // Set volume
p_fxm->volume = MIN( fxp, 0x40 );
p_dma->volume = p_fxm->volume;
break;
case 0xD0: // Jump to row
fxp = HI4( fxp ) * 10 + LO4( fxp );
if( !pattern_jump ) ix_nextorder = ( ( ix_order + 1 ) >= ROM_READB( &p_mod->order_count ) ? 0x00 : ix_order + 1 );
pattern_jump = true;
ix_nextrow = ( fxp > 63 ? 0 : fxp );
break;
case 0xF0: // Set speed
if( fxp > 0x20 ) {
cia = SAMPLE_RATE / ( ( 24 * fxp ) / 60 );
} else {
speed = fxp;
}
break;
case 0x40: // Vibrato
if( fxp ) p_fxm->vibr.fxp = fxp;
break;
case 0x70: // Tremolo
if( fxp ) p_fxm->trem.fxp = fxp;
break;
case 0xE1: // Fine slide up
p_fxm->period = MAX( p_fxm->period - fxp, PERIOD_MIN );
p_dma->rate = SYS_FREQ / p_fxm->period;
break;
case 0xE2: // Fine slide down
p_fxm->period = MIN( p_fxm->period + fxp, PERIOD_MAX );
p_dma->rate = SYS_FREQ / p_fxm->period;
break;
case 0xE3: // Glissando control
p_fxm->glissando = ( fxp != 0 );
break;
case 0xE4: // Set vibrato waveform
p_fxm->vibr.mode = fxp;
break;
case 0xE6: // Loop-back (advanced looping)
if( fxp == 0x0 ) {
p_fxm->loop_order = ix_order;
p_fxm->loop_row = ix_row;
} else {
p_fxm->loop_count = ( p_fxm->loop_count ? p_fxm->loop_count - 1 : fxp );
if( p_fxm->loop_count ) {
ix_nextorder = p_fxm->loop_order;
ix_nextrow = p_fxm->loop_row;
}
}
break;
case 0xE7: // Set tremolo waveform
p_fxm->trem.mode = fxp;
break;
case 0xEA: // Fine volume slide up
p_fxm->volume = MIN( p_fxm->volume + fxp, 0x40 );
p_dma->volume = p_fxm->volume;
break;
case 0xEB: // Fine volume slide down
p_fxm->volume = MAX( p_fxm->volume - fxp, 0 );
p_dma->volume = p_fxm->volume;
break;
case 0xEE: // Delay
delay = fxp;
break;
}
} else {
// Effects processed when tick > 0
switch( fx ) {
case 0x10: // Slide up
p_fxm->period = MAX( p_fxm->period - fxp, PERIOD_MIN );
p_dma->rate = SYS_FREQ / p_fxm->period;
break;
case 0x20: // Slide down
p_fxm->period = MIN( p_fxm->period + fxp, PERIOD_MAX );
p_dma->rate = SYS_FREQ / p_fxm->period;
break;
case 0xE9: // Retrigger note
temp_b = tick; while( temp_b >= fxp ) temp_b -= fxp;
if( temp_b == 0 ) note_start( p_dma, p_fxm->sample, ix_period, ( fx == 0x90 ? fxp : 0 ) );
break;
case 0xEC: // Note cut
if( fxp == tick ) p_dma->volume = 0x00;
break;
default: // Multi-effect processing
// Portamento
if( fx == 0x30 || fx == 0x50 ) {
if( p_fxm->period < p_fxm->port_target ) p_fxm->period = MIN( p_fxm->period + p_fxm->port_speed, p_fxm->port_target );
else p_fxm->period = MAX( p_fxm->period - p_fxm->port_speed, p_fxm->port_target );
if( p_fxm->glissando ) p_dma->rate = SYS_FREQ / glissando( ch );
else p_dma->rate = SYS_FREQ / p_fxm->period;
}
// Volume slide
if( fx == 0x50 || fx == 0x60 || fx == 0xA0 ) {
if( ( fxp & 0xF0 ) == 0 ) p_fxm->volume -= ( LO4( fxp ) );
if( ( fxp & 0x0F ) == 0 ) p_fxm->volume += ( HI4( fxp ) );
p_fxm->volume = MAX( MIN( p_fxm->volume, 0x40 ), 0 );
p_dma->volume = p_fxm->volume;
}
}
}
// Normal play and arpeggio
if( fx == 0x00 ) {
temp_b = tick; while( temp_b > 2 ) temp_b -= 2;
if( temp_b == 0 ) {
// Reset
p_dma->rate = SYS_FREQ / p_fxm->period;
} else if( fxp ) {
// Arpeggio
p_dma->rate = SYS_FREQ / arpeggio( ch, ( temp_b == 1 ? HI4( fxp ) : LO4( fxp ) ) );
}
} else if( fx == 0x40 || fx == 0x60 ) {
// Vibrato
p_dma->rate = SYS_FREQ / ( p_fxm->period + do_osc( &p_fxm->vibr ) );
} else if( fx == 0x70 ) {
// Tremolo
temp_b = p_fxm->volume + do_osc( &p_fxm->trem );
p_dma->volume = MAX( MIN( temp_b, 0x40 ), 0 );
}
p_fxm++;
p_dma++;
}
}
/* Execute single instruction. Return exception condition. */
exc_t step_state(state_ptr s, FILE *error_file)
{
word_t argA, argB;
byte_t byte0 = 0;
byte_t byte1 = 0;
itype_t hi0;
alu_t lo0;
reg_id_t hi1 = REG_NONE;
reg_id_t lo1 = REG_NONE;
bool_t ok1 = TRUE;
word_t cval;
word_t okc = TRUE;
word_t val, dval;
bool_t need_regids;
bool_t need_imm;
word_t ftpc = s->pc;
if (!get_byte_val(s->m, ftpc, &byte0)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return EXC_ADDR;
}
ftpc++;
hi0 = HI4(byte0);
lo0 = LO4(byte0);
need_regids =
(hi0 == I_RRMOVL || hi0 == I_ALU || hi0 == I_PUSHL ||
hi0 == I_POPL || hi0 == I_IRMOVL || hi0 == I_RMMOVL ||
hi0 == I_MRMOVL || hi0 == I_ALUI);
if (need_regids) {
ok1 = get_byte_val(s->m, ftpc, &byte1);
ftpc++;
hi1 = HI4(byte1);
lo1 = LO4(byte1);
}
need_imm =
(hi0 == I_IRMOVL || hi0 == I_RMMOVL || hi0 == I_MRMOVL ||
hi0 == I_JXX || hi0 == I_CALL || hi0 == I_ALUI);
if (need_imm) {
okc = get_word_val(s->m, ftpc, &cval);
ftpc += 4;
}
switch (hi0) {
case I_NOP:
s->pc = ftpc;
break;
case I_HALT:
s->pc = ftpc;
return EXC_HALT;
break;
case I_RRMOVL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return EXC_ADDR;
}
if (hi1 >= 8) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n",
s->pc, hi1);
return EXC_INSTR;
}
if (lo1 >= 8) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n",
s->pc, lo1);
return EXC_INSTR;
}
val = get_reg_val(s->r, hi1);
set_reg_val(s->r, lo1, val);
s->pc = ftpc;
break;
#if 0
case I_IRMOVL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return EXC_ADDR;
}
if (!okc) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address",
s->pc);
return EXC_INSTR;
}
if (lo1 >= 8) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n",
s->pc, lo1);
return EXC_INSTR;
}
set_reg_val(s->r, lo1, cval);
s->pc = ftpc;
break;
#endif
case I_RMMOVL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return EXC_ADDR;
}
if (!okc) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return EXC_INSTR;
}
if (hi1 >= 8) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n",
s->pc, hi1);
return EXC_INSTR;
}
if (lo1 < 8)
cval += get_reg_val(s->r, lo1);
val = get_reg_val(s->r, hi1);
if (!set_word_val(s->m, cval, val)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid data address 0x%x\n",
s->pc, cval);
return EXC_ADDR;
}
s->pc = ftpc;
break;
case I_MRMOVL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return EXC_ADDR;
}
if (!okc) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction addres\n", s->pc);
return EXC_INSTR;
}
if (hi1 >= 8) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n",
s->pc, hi1);
return EXC_INSTR;
}
if (lo1 < 8)
cval += get_reg_val(s->r, lo1);
if (!get_word_val(s->m, cval, &val))
return EXC_ADDR;
set_reg_val(s->r, hi1, val);
s->pc = ftpc;
break;
case I_ALU:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return EXC_ADDR;
}
argA = get_reg_val(s->r, hi1);
argB = get_reg_val(s->r, lo1);
val = compute_alu(lo0, argA, argB);
set_reg_val(s->r, lo1, val);
s->cc = compute_cc(lo0, argA, argB);
s->pc = ftpc;
break;
case I_JXX:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return EXC_ADDR;
}
if (!okc) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return EXC_ADDR;
}
if (take_branch(s->cc, lo0))
s->pc = cval;
else
s->pc = ftpc;
break;
#if 0
case I_CALL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return EXC_ADDR;
}
if (!okc) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return EXC_ADDR;
}
val = get_reg_val(s->r, REG_ESP) - 4;
set_reg_val(s->r, REG_ESP, val);
if (!set_word_val(s->m, val, ftpc)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid stack address 0x%x\n", s->pc, val);
return EXC_ADDR;
}
s->pc = cval;
break;
case I_RET:
/* Return Instruction. Pop address from stack */
dval = get_reg_val(s->r, REG_ESP);
if (!get_word_val(s->m, dval, &val)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid stack address 0x%x\n",
s->pc, dval);
return EXC_ADDR;
}
set_reg_val(s->r, REG_ESP, dval + 4);
s->pc = val;
break;
#endif
case I_PUSHL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return EXC_ADDR;
}
if (hi1 >= 8) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n", s->pc, hi1);
return EXC_INSTR;
}
val = get_reg_val(s->r, hi1);
dval = get_reg_val(s->r, REG_ESP) - 4;
set_reg_val(s->r, REG_ESP, dval);
if (!set_word_val(s->m, dval, val)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid stack address 0x%x\n", s->pc, dval);
return EXC_ADDR;
}
s->pc = ftpc;
break;
case I_POPL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return EXC_ADDR;
}
if (hi1 >= 8) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n", s->pc, hi1);
return EXC_INSTR;
}
dval = get_reg_val(s->r, REG_ESP);
set_reg_val(s->r, REG_ESP, dval+4);
if (!get_word_val(s->m, dval, &val)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid stack address 0x%x\n",
s->pc, dval);
return EXC_ADDR;
}
set_reg_val(s->r, hi1, val);
s->pc = ftpc;
break;
case I_LEAVE:
dval = get_reg_val(s->r, REG_EBP);
set_reg_val(s->r, REG_ESP, dval+4);
if (!get_word_val(s->m, dval, &val)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid stack address 0x%x\n",
s->pc, dval);
return EXC_ADDR;
}
set_reg_val(s->r, REG_EBP, val);
s->pc = ftpc;
break;
#if 0
case I_ALUI:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return EXC_ADDR;
}
if (!okc) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address",
s->pc);
return EXC_INSTR;
}
if (lo1 >= 8) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n",
s->pc, lo1);
return EXC_INSTR;
}
argB = get_reg_val(s->r, lo1);
val = argB + cval;
set_reg_val(s->r, lo1, val);
s->cc = compute_cc(A_ADD, cval, argB);
s->pc = ftpc;
break;
#endif
default:
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction %.2x\n", s->pc, byte0);
return EXC_INSTR;
}
return EXC_NONE;
}
/* Return resulting exception status */
static exc_t sim_step()
{
word_t aluA;
word_t aluB;
word_t alufun;
exc_t status = update_state(); /* Update state from last cycle */
if (plusmode) {
pc = gen_pc();
}
valp = pc;
if (get_byte_val(mem, valp, &instr)) {
icode = HI4(instr);
ifun = LO4(instr);
} else {
instr = HPACK(I_NOP,0);
icode = I_NOP;
ifun = 0;
status = EXC_ADDR;
sim_log("Couldn't fetch at address 0x%x\n", valp);
}
valp++;
if (gen_need_regids()) {
byte_t regids;
if (get_byte_val(mem, valp, ®ids)) {
ra = GET_RA(regids);
rb = GET_RB(regids);
} else {
ra = REG_NONE;
rb = REG_NONE;
status = EXC_ADDR;
sim_log("Couldn't fetch at address 0x%x\n", valp);
}
valp++;
} else {
ra = REG_NONE;
rb = REG_NONE;
}
if (gen_need_valC()) {
if (get_word_val(mem, valp, &valc)) {
} else {
valc = 0;
status = EXC_ADDR;
sim_log("Couldn't fetch at address 0x%x\n", valp);
}
valp+=4;
} else {
valc = 0;
}
if (status == EXC_NONE && !gen_instr_valid()) {
status = EXC_INSTR;
}
sim_log("IF: Fetched %s at 0x%x. ra=%s, rb=%s, valC = 0x%x\n",
iname(HPACK(icode,ifun)), pc, reg_name(ra), reg_name(rb), valc);
if (status == EXC_NONE && icode == I_HALT) {
status = EXC_HALT;
}
srcA = gen_srcA();
if (srcA != REG_NONE) {
vala = get_reg_val(reg, srcA);
} else {
vala = 0;
}
srcB = gen_srcB();
if (srcB != REG_NONE) {
valb = get_reg_val(reg, srcB);
} else {
valb = 0;
}
destE = gen_dstE();
destM = gen_dstM();
aluA = gen_aluA();
aluB = gen_aluB();
alufun = gen_alufun();
vale = compute_alu(alufun, aluA, aluB);
cc_in = cc;
if (gen_set_cc())
cc_in = compute_cc(alufun, aluA, aluB);
bcond = (icode == I_JMP) && take_branch(cc, ifun);
mem_addr = gen_mem_addr();
mem_data = gen_mem_data();
if (status == EXC_NONE && gen_mem_read()) {
if (!get_word_val(mem, mem_addr, &valm)) {
sim_log("Couldn't read at address 0x%x\n", mem_addr);
return EXC_ADDR;
}
} else
valm = 0;
mem_write = status == EXC_NONE && gen_mem_write();
if (plusmode) {
prev_icode_in = icode;
prev_ifun_in = ifun;
prev_valc_in = valc;
prev_valm_in = valm;
prev_valp_in = valp;
prev_bcond_in = bcond;
} else {
/* Update PC */
pc_in = gen_new_pc();
}
sim_report();
return status;
}
void do_if_stage()
{
exc_t nstatus = EXC_NONE;
word_t fetchpc = gen_f_pc();
word_t valp = fetchpc;
bool_t fetch_ok;
byte_t instr;
byte_t regids = HPACK(REG_NONE, REG_NONE);
word_t valc = 0;
f_pc = fetchpc;
if (fetchpc == 0) {
sim_log("Fetch: Fetch pc = 0, nominal pc = 0x%x\n",
pc_curr->pc);
}
/* Ready to fetch instruction. Speculatively fetch register byte
and immediate word
*/
fetch_ok = get_byte_val(mem, valp, &instr);
if (fetch_ok) {
if_id_next->icode = GET_ICODE(instr);
if_id_next->ifun = GET_FUN(instr);
} else {
if_id_next->icode = I_NOP;
if_id_next->ifun = 0;
nstatus = EXC_ADDR;
}
valp++;
if (fetch_ok && gen_need_regids()) {
fetch_ok = get_byte_val(mem, valp, ®ids);
valp ++;
}
if_id_next->ra = HI4(regids);
if_id_next->rb = LO4(regids);
if (fetch_ok && gen_need_valC()) {
fetch_ok = get_word_val(mem, valp, &valc);
valp+= 4;
}
if_id_next->valp = valp;
if_id_next->valc = valc;
pc_next->pc = gen_new_F_predPC();
if (!gen_instr_valid())
{
byte_t instr = HPACK(if_id_next->icode, if_id_next->ifun);
sim_log("Fetch: Instruction code %s (0x%x) invalid\n",
iname(instr), instr);
nstatus = EXC_INSTR;
}
pc_next->exception = (nstatus == EXC_NONE) ? EXC_NONE : EXC_BUBBLE;
if_id_next->stage_pc = fetchpc;
if_id_next->exception = nstatus;
/* Recompute icode for one-write implementation of popl */
if_id_next->icode = gen_new_D_icode();
sim_log("Fetch: Fetched %s at 0x%x, ra = %s, rb = %s, valp = 0x%x, status = %s\n",
iname(HPACK(if_id_next->icode, if_id_next->ifun)),
if_id_next->stage_pc,
reg_name(if_id_next->ra), reg_name(if_id_next->rb),
if_id_next->valp,
exc_name(nstatus));
}
/* Execute single instruction. Return status. */
stat_t step_state(state_ptr s, FILE *error_file)
{
word_t argA, argB;
byte_t byte0 = 0;
byte_t byte1 = 0;
itype_t hi0;
alu_t lo0;
reg_id_t hi1 = REG_NONE;
reg_id_t lo1 = REG_NONE;
bool_t ok1 = TRUE;
word_t cval = 0;
word_t okc = TRUE;
word_t val, dval;
bool_t need_regids;
bool_t need_imm;
word_t ftpc = s->pc; /* Fall-through PC */
if (!get_byte_val(s->m, ftpc, &byte0)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_ADR;
}
ftpc++;
hi0 = HI4(byte0);
lo0 = LO4(byte0);
need_regids =
(hi0 == I_RRMOVL || hi0 == I_ALU || hi0 == I_PUSHL ||
hi0 == I_POPL || hi0 == I_IRMOVL || hi0 == I_RMMOVL ||
hi0 == I_MRMOVL || hi0 == I_IADDL || hi0 == I_ISUBL);
if (need_regids) {
ok1 = get_byte_val(s->m, ftpc, &byte1);
ftpc++;
hi1 = HI4(byte1);
lo1 = LO4(byte1);
}
need_imm =
(hi0 == I_IRMOVL || hi0 == I_RMMOVL || hi0 == I_MRMOVL ||
hi0 == I_JMP || hi0 == I_CALL || hi0 == I_IADDL || hi0 == I_ISUBL);
if (need_imm) {
okc = get_word_val(s->m, ftpc, &cval);
ftpc += 4;
}
switch (hi0) {
case I_NOP:
s->pc = ftpc;
break;
case I_HALT:
return STAT_HLT;
break;
case I_RRMOVL: /* Both unconditional and conditional moves */
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_ADR;
}
if (!reg_valid(hi1)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n",
s->pc, hi1);
return STAT_INS;
}
if (!reg_valid(lo1)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n",
s->pc, lo1);
return STAT_INS;
}
val = get_reg_val(s->r, hi1);
if (cond_holds(s->cc, lo0))
set_reg_val(s->r, lo1, val);
s->pc = ftpc;
break;
case I_IRMOVL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_ADR;
}
if (!okc) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address",
s->pc);
return STAT_INS;
}
if (!reg_valid(lo1)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n",
s->pc, lo1);
return STAT_INS;
}
set_reg_val(s->r, lo1, cval);
s->pc = ftpc;
break;
case I_RMMOVL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_ADR;
}
if (!okc) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_INS;
}
if (!reg_valid(hi1)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n",
s->pc, hi1);
return STAT_INS;
}
if (reg_valid(lo1))
cval += get_reg_val(s->r, lo1);
val = get_reg_val(s->r, hi1);
if (!set_word_val(s->m, cval, val)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid data address 0x%x\n",
s->pc, cval);
return STAT_ADR;
}
s->pc = ftpc;
break;
case I_MRMOVL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_ADR;
}
if (!okc) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction addres\n", s->pc);
return STAT_INS;
}
if (!reg_valid(hi1)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n",
s->pc, hi1);
return STAT_INS;
}
if (reg_valid(lo1))
cval += get_reg_val(s->r, lo1);
if (!get_word_val(s->m, cval, &val))
return STAT_ADR;
set_reg_val(s->r, hi1, val);
s->pc = ftpc;
break;
case I_ALU:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_ADR;
}
argA = get_reg_val(s->r, hi1);
argB = get_reg_val(s->r, lo1);
val = compute_alu(lo0, argA, argB);
set_reg_val(s->r, lo1, val);
s->cc = compute_cc(lo0, argA, argB);
s->pc = ftpc;
break;
case I_JMP:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_ADR;
}
if (!okc) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_ADR;
}
if (cond_holds(s->cc, lo0))
s->pc = cval;
else
s->pc = ftpc;
break;
case I_CALL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_ADR;
}
if (!okc) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_ADR;
}
val = get_reg_val(s->r, REG_ESP) - 4;
set_reg_val(s->r, REG_ESP, val);
if (!set_word_val(s->m, val, ftpc)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid stack address 0x%x\n", s->pc, val);
return STAT_ADR;
}
s->pc = cval;
break;
case I_RET:
/* Return Instruction. Pop address from stack */
dval = get_reg_val(s->r, REG_ESP);
if (!get_word_val(s->m, dval, &val)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid stack address 0x%x\n",
s->pc, dval);
return STAT_ADR;
}
set_reg_val(s->r, REG_ESP, dval + 4);
s->pc = val;
break;
case I_PUSHL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_ADR;
}
if (!reg_valid(hi1)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n", s->pc, hi1);
return STAT_INS;
}
val = get_reg_val(s->r, hi1);
dval = get_reg_val(s->r, REG_ESP) - 4;
set_reg_val(s->r, REG_ESP, dval);
if (!set_word_val(s->m, dval, val)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid stack address 0x%x\n", s->pc, dval);
return STAT_ADR;
}
s->pc = ftpc;
break;
case I_POPL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_ADR;
}
if (!reg_valid(hi1)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n", s->pc, hi1);
return STAT_INS;
}
dval = get_reg_val(s->r, REG_ESP);
set_reg_val(s->r, REG_ESP, dval+4);
if (!get_word_val(s->m, dval, &val)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid stack address 0x%x\n",
s->pc, dval);
return STAT_ADR;
}
set_reg_val(s->r, hi1, val);
s->pc = ftpc;
break;
case I_LEAVE:
dval = get_reg_val(s->r, REG_EBP);
set_reg_val(s->r, REG_ESP, dval+4);
if (!get_word_val(s->m, dval, &val)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid stack address 0x%x\n",
s->pc, dval);
return STAT_ADR;
}
set_reg_val(s->r, REG_EBP, val);
s->pc = ftpc;
break;
case I_IADDL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_ADR;
}
if (!okc) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address",
s->pc);
return STAT_INS;
}
if (!reg_valid(lo1)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n",
s->pc, lo1);
return STAT_INS;
}
argB = get_reg_val(s->r, lo1);
val = argB + cval;
set_reg_val(s->r, lo1, val);
s->cc = compute_cc(A_ADD, cval, argB);
s->pc = ftpc;
break;
case I_ISUBL:
if (!ok1) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address\n", s->pc);
return STAT_ADR;
}
if (!okc) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction address",
s->pc);
return STAT_INS;
}
if (!reg_valid(lo1)) {
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid register ID 0x%.1x\n",
s->pc, lo1);
return STAT_INS;
}
argB = get_reg_val(s->r, lo1);
val = argB - cval;
set_reg_val(s->r, lo1, val);
s->cc = compute_cc(A_SUB, cval, argB);
s->pc = ftpc;
break;
default:
if (error_file)
fprintf(error_file,
"PC = 0x%x, Invalid instruction %.2x\n", s->pc, byte0);
return STAT_INS;
}
return STAT_AOK;
}
/* Return resulting status */
static byte_t sim_step()
{
word_t aluA;
word_t aluB;
word_t alufun;
status = STAT_AOK;
imem_error = dmem_error = FALSE;
update_state(); /* Update state from last cycle */
if (plusmode) {
pc = gen_pc();
}
valp = pc;
instr = HPACK(I_NOP, F_NONE);
imem_error = !get_byte_val(mem, valp, &instr);
if (imem_error) {
sim_log("Couldn't fetch at address 0x%x\n", valp);
}
imem_icode = HI4(instr);
imem_ifun = LO4(instr);
icode = gen_icode();
ifun = gen_ifun();
instr_valid = gen_instr_valid();
valp++;
if (gen_need_regids()) {
byte_t regids;
if (get_byte_val(mem, valp, ®ids)) {
ra = GET_RA(regids);
rb = GET_RB(regids);
} else {
ra = REG_NONE;
rb = REG_NONE;
status = STAT_ADR;
sim_log("Couldn't fetch at address 0x%x\n", valp);
}
valp++;
} else {
ra = REG_NONE;
rb = REG_NONE;
}
if (gen_need_valC()) {
if (get_word_val(mem, valp, &valc)) {
} else {
valc = 0;
status = STAT_ADR;
sim_log("Couldn't fetch at address 0x%x\n", valp);
}
valp+=4;
} else {
valc = 0;
}
sim_log("IF: Fetched %s at 0x%x. ra=%s, rb=%s, valC = 0x%x\n",
iname(HPACK(icode,ifun)), pc, reg_name(ra), reg_name(rb), valc);
if (status == STAT_AOK && icode == I_HALT) {
status = STAT_HLT;
}
srcA = gen_srcA();
if (srcA != REG_NONE) {
vala = get_reg_val(reg, srcA);
} else {
vala = 0;
}
srcB = gen_srcB();
if (srcB != REG_NONE) {
valb = get_reg_val(reg, srcB);
} else {
valb = 0;
}
cond = cond_holds(cc, ifun);
destE = gen_dstE();
destM = gen_dstM();
aluA = gen_aluA();
aluB = gen_aluB();
alufun = gen_alufun();
vale = compute_alu(alufun, aluA, aluB);
cc_in = cc;
if (gen_set_cc())
cc_in = compute_cc(alufun, aluA, aluB);
bcond = cond && (icode == I_JMP);
mem_addr = gen_mem_addr();
mem_data = gen_mem_data();
if (gen_mem_read()) {
dmem_error = dmem_error || !get_word_val(mem, mem_addr, &valm);
if (dmem_error) {
sim_log("Couldn't read at address 0x%x\n", mem_addr);
}
} else
valm = 0;
mem_write = gen_mem_write();
if (mem_write) {
/* Do a test read of the data memory to make sure address is OK */
word_t junk;
dmem_error = dmem_error || !get_word_val(mem, mem_addr, &junk);
}
status = gen_Stat();
if (plusmode) {
prev_icode_in = icode;
prev_ifun_in = ifun;
prev_valc_in = valc;
prev_valm_in = valm;
prev_valp_in = valp;
prev_bcond_in = bcond;
} else {
/* Update PC */
pc_in = gen_new_pc();
}
sim_report();
return status;
}
void
bsbport_poll_cb(void)
{
BSBDEBUG("MQTT Poll");
for (uint8_t i = 0; i < BSBPORT_MESSAGE_BUFFER_LEN; i++)
{
if (bsbport_msg_buffer.msg[i].mqtt_new)
{
uint8_t len;
uint8_t topic_len;
char buf[DATA_LENGTH];
char topic[TOPIC_LENGTH];
bsbport_msg_buffer.msg[i].mqtt_new = 0;
// Topic
topic_len = snprintf_P(topic, TOPIC_LENGTH, publish_topic_format,
LO4(bsbport_msg_buffer.msg[i].src),
bsbport_msg_buffer.msg[i].p.data.p1,
bsbport_msg_buffer.msg[i].p.data.p2,
bsbport_msg_buffer.msg[i].p.data.p3,
bsbport_msg_buffer.msg[i].p.data.p4);
// RAW
strncat_P(topic, PSTR("RAW"), TOPIC_LENGTH - topic_len - 1);
len =
snprintf_P(buf, DATA_LENGTH, PSTR("%u"),
bsbport_msg_buffer.msg[i].value);
mqtt_construct_publish_packet(topic, buf, len, BSBPORT_MQTT_RETAIN);
BSBDEBUG("%s=%s", topic, buf);
// STA
topic[topic_len]=0;
strncat_P(topic, PSTR("STA"), TOPIC_LENGTH - topic_len - 1);
len =
snprintf_P(buf, DATA_LENGTH, PSTR("%u"),
HI8(bsbport_msg_buffer.msg[i].value));
mqtt_construct_publish_packet(topic, buf, len, BSBPORT_MQTT_RETAIN);
BSBDEBUG("%s=%s", topic, buf);
// TMP
topic[topic_len]=0;
strncat_P(topic, PSTR("TMP"), TOPIC_LENGTH - topic_len - 1);
len =
itoa_fixedpoint(((int32_t) bsbport_msg_buffer.msg[i].value * 100) /
64, 2, buf, DATA_LENGTH);
mqtt_construct_publish_packet(topic, buf, len, BSBPORT_MQTT_RETAIN);
BSBDEBUG("%s=%s", topic, buf);
// FP1
topic[topic_len]=0;
strncat_P(topic, PSTR("FP1"), TOPIC_LENGTH - topic_len - 1);
len =
itoa_fixedpoint(bsbport_msg_buffer.msg[i].value, 1, buf, DATA_LENGTH);
mqtt_construct_publish_packet(topic, buf, len, BSBPORT_MQTT_RETAIN);
BSBDEBUG("%s=%s", topic, buf);
// FP5
topic[topic_len]=0;
strncat_P(topic, PSTR("FP5"), TOPIC_LENGTH - topic_len - 1);
len =
itoa_fixedpoint(bsbport_msg_buffer.msg[i].value * 10 / 2, 1, buf,
DATA_LENGTH);
mqtt_construct_publish_packet(topic, buf, len, BSBPORT_MQTT_RETAIN);
BSBDEBUG("%s=%s", topic, buf);
}
}
}