+ LOG("pio_handle=%p,start_label=%i,buf=%p,mem_ptr=%p",
+ pio_handle, start_label, buf, mem_ptr);
+ _trans_value_t registers[8];
+ _trans_value_t tmpval;
+
+ ASSERT(pio_handle);
+
+ udi_ubit16_t ret_status = 0;
+
+ udi_pio_trans_t *ops = pio_handle->TransOps;
+ size_t ip = _get_label(pio_handle, start_label);
+
+ while( ip < pio_handle->nTransOps )
+ {
+ udi_ubit8_t pio_op = ops[ip].pio_op;
+ udi_ubit8_t tran_size = ops[ip].tran_size;
+ udi_ubit16_t operand = ops[ip].operand;
+ LOG("%2i: %02x %i 0x%04x", ip, pio_op, tran_size, operand);
+ ASSERTC(tran_size, <=, UDI_PIO_32BYTE);
+ ip ++;
+ if( !(pio_op & 0x80) )
+ {
+ // Class A
+ _trans_value_t *reg = ®isters[pio_op&7];
+ switch(pio_op & 0x60)
+ {
+ case UDI_PIO_IN:
+ pio_handle->IOFunc(pio_handle->ChildID, pio_handle->RegSet,
+ operand, tran_size, &tmpval, false);
+ _write_mem(gcb, buf, mem_ptr, (pio_op&0x18), tran_size, reg, &tmpval);
+ break;
+ case UDI_PIO_OUT:
+ _read_mem(gcb, buf, mem_ptr, (pio_op&0x18), tran_size, reg, &tmpval);
+ pio_handle->IOFunc(pio_handle->ChildID, pio_handle->RegSet,
+ operand, tran_size, &tmpval, true);
+ break;
+ case UDI_PIO_LOAD:
+ _read_mem(gcb, buf, mem_ptr, (pio_op&0x18), tran_size, reg, ®isters[operand]);
+ _zero_upper(tran_size, ®isters[operand]);
+ break;
+ case UDI_PIO_STORE:
+ _write_mem(gcb, buf, mem_ptr, (pio_op&0x18), tran_size, reg, ®isters[operand]);
+ break;
+ }
+ }
+ else if( pio_op < 0xF0 )
+ {
+ int reg_num = pio_op & 7;
+ _trans_value_t *reg = ®isters[reg_num];
+ // Class B
+ switch(pio_op & 0xF8)
+ {
+ case UDI_PIO_LOAD_IMM:
+ ASSERTC(ip + (1<<tran_size)/2-1, <=, pio_handle->nTransOps);
+ if( tran_size == UDI_PIO_1BYTE ) {
+ Log_Error("UDI", "udi_pio_trans - %p [%i] LOAD_IMM with 1 byte",
+ ops, ip-1);
+ goto error;
+ }
+ switch(tran_size)
+ {
+ case UDI_PIO_32BYTE:
+ reg->words[7] = (ops[ip+14].operand << 16) | ops[ip+13].operand;
+ reg->words[6] = (ops[ip+12].operand << 16) | ops[ip+11].operand;
+ reg->words[5] = (ops[ip+10].operand << 16) | ops[ip+ 9].operand;
+ reg->words[4] = (ops[ip+ 8].operand << 16) | ops[ip+ 7].operand;
+ case UDI_PIO_16BYTE:
+ reg->words[3] = (ops[ip+ 6].operand << 16) | ops[ip+ 5].operand;
+ reg->words[2] = (ops[ip+ 4].operand << 16) | ops[ip+ 3].operand;
+ case UDI_PIO_8BYTE:
+ reg->words[1] = (ops[ip+ 2].operand << 16) | ops[ip+ 1].operand;
+ case UDI_PIO_4BYTE:
+ reg->words[0] = (ops[ip+ 0].operand << 16) | operand;
+ case UDI_PIO_2BYTE:
+ case UDI_PIO_1BYTE:
+ reg->words[0] = operand;
+ break;
+ }
+ _zero_upper(tran_size, reg);
+ ip += (1<<tran_size)/2-1;
+ break;
+ case UDI_PIO_CSKIP: {
+ int cnd = _compare_zero(reg, tran_size);
+ switch(operand)
+ {
+ case UDI_PIO_NZ:
+ LOG("CSKIP NZ R%i", reg_num);
+ if( !(cnd & 1) )
+ ip ++;
+ break;
+ case UDI_PIO_Z:
+ LOG("CSKIP Z R%i", reg_num);
+ if( cnd & 1 )
+ ip ++;
+ break;
+ case UDI_PIO_NNEG:
+ LOG("CSKIP NNEG R%i", reg_num);
+ if( !(cnd & 2) )
+ ip ++;
+ case UDI_PIO_NEG:
+ LOG("CSKIP NEG R%i", reg_num);
+ if( cnd & 2 )
+ ip ++;
+ break;
+ }
+ break; }
+ case UDI_PIO_IN_IND:
+ pio_handle->IOFunc(pio_handle->ChildID, pio_handle->RegSet,
+ registers[operand].words[0], tran_size, reg, false);
+ _zero_upper(tran_size, reg);
+ break;
+ case UDI_PIO_OUT_IND:
+ pio_handle->IOFunc(pio_handle->ChildID, pio_handle->RegSet,
+ registers[operand].words[0], tran_size, reg, true);
+ break;
+ case UDI_PIO_SHIFT_LEFT:
+ _operation_shift_left(tran_size, reg, operand);
+ break;
+ case UDI_PIO_SHIFT_RIGHT:
+ _operation_shift_right(tran_size, reg, operand);
+ break;
+ case UDI_PIO_AND:
+ _operation_and(tran_size, reg, ®isters[operand]);
+ break;
+ case UDI_PIO_AND_IMM:
+ tmpval.words[0] = operand;
+ _zero_upper(UDI_PIO_2BYTE, &tmpval);
+ _operation_and(tran_size, reg, &tmpval);
+ break;
+ case UDI_PIO_OR:
+ _operation_or(tran_size, reg, ®isters[operand]);
+ break;
+ case UDI_PIO_OR_IMM:
+ tmpval.words[0] = operand;
+ _zero_upper(UDI_PIO_4BYTE, &tmpval);
+ _operation_or(tran_size, reg, &tmpval);
+ break;
+ case UDI_PIO_XOR:
+ _operation_xor(tran_size, reg, ®isters[operand]);
+ break;
+ case UDI_PIO_ADD:
+ _operation_add(tran_size, reg, ®isters[operand]);
+ break;
+ case UDI_PIO_ADD_IMM:
+ tmpval.words[0] = operand;
+ if( operand & (1 <<16) ) {
+ tmpval.words[0] |= 0xFFFF0000;
+ memset(&tmpval.words[1], 0xFF, 4*(8-1));
+ }
+ else {
+ _zero_upper(UDI_PIO_4BYTE, &tmpval);
+ }
+ _operation_add(tran_size, reg, &tmpval);
+ break;
+ case UDI_PIO_SUB:
+ _operation_sub(tran_size, reg, ®isters[operand]);
+ break;
+ default:
+ Log_Error("UDI", "udi_pio_trans - Class B %x unhandled!",
+ pio_op & 0xF8);
+ goto error;
+ }
+ }
+ else
+ {
+ // Class C
+ switch(pio_op)
+ {
+ case UDI_PIO_BRANCH:
+ ip = _get_label(pio_handle, operand);
+ break;
+ case UDI_PIO_LABEL:
+ // nop
+ break;
+ case UDI_PIO_REP_IN_IND:
+ case UDI_PIO_REP_OUT_IND: {
+ bool dir_is_out = (pio_op == UDI_PIO_REP_OUT_IND);
+ udi_ubit8_t mode = operand & 0x18;
+ udi_ubit32_t cnt = registers[(operand>>13) & 7].words[0];
+ int pio_stride = (operand>>10) & 3;
+ udi_ubit32_t pio_ofs = registers[(operand>>7) & 7].words[0];
+ int mem_stride = (operand>>5) & 3;
+ _trans_value_t *mem_reg=®isters[(operand>>0) & 7];
+ udi_ubit32_t saved_mem_reg = mem_reg->words[0];
+
+ if( pio_stride > 0 )
+ pio_stride = 1<<(tran_size+pio_stride-1);
+ if( mem_stride > 0 )
+ mem_stride = 1<<(tran_size+mem_stride-1);
+
+ LOG("REP_%s_IND %i IO=%x+%i %s Mem=%x+%i",
+ (dir_is_out ? "OUT": "IN"), cnt,
+ pio_ofs, pio_stride,
+ caMEM_MODES[mode>>3], saved_mem_reg, mem_stride
+ );
+
+ while( cnt -- )
+ {
+ if( dir_is_out )
+ _read_mem(gcb,buf,mem_ptr, mode, tran_size,
+ mem_reg, &tmpval);
+ pio_handle->IOFunc(pio_handle->ChildID, pio_handle->RegSet,
+ pio_ofs, tran_size, &tmpval, dir_is_out);
+ if( !dir_is_out )
+ _read_mem(gcb,buf,mem_ptr, mode, tran_size,
+ mem_reg, &tmpval);
+ pio_ofs += pio_stride;
+ if( mode != UDI_PIO_DIRECT )
+ mem_reg->words[0] += mem_stride;
+ }
+ if( mode != UDI_PIO_DIRECT )
+ mem_reg->words[0] = saved_mem_reg;
+
+ break; }
+ case UDI_PIO_DELAY:
+ LOG("DELAY %i", operand);
+ Log_Notice("UDI", "udi_pio_trans - TODO: DELAY");
+ break;
+ case UDI_PIO_BARRIER:
+ LOG("BARRIER");
+ Log_Notice("UDI", "udi_pio_trans - TODO: BARRIER");
+ break;
+ case UDI_PIO_SYNC:
+ LOG("SYNC");
+ Log_Notice("UDI", "udi_pio_trans - TODO: SYNC");
+ break;
+ case UDI_PIO_SYNC_OUT:
+ LOG("SYNC_OUT");
+ Log_Notice("UDI", "udi_pio_trans - TODO: SYNC_OUT");
+ break;
+ case UDI_PIO_DEBUG:
+ LOG("DEBUG %x", operand);
+ // nop
+ break;
+ case UDI_PIO_END:
+ ASSERTC(operand, <, 8);
+ ASSERTC(tran_size, <=, UDI_PIO_2BYTE);
+ if( tran_size == UDI_PIO_2BYTE )
+ ret_status = registers[operand].words[0] & 0xFFFF;
+ else
+ ret_status = registers[operand].words[0] & 0xFF;
+ goto end;
+ case UDI_PIO_END_IMM:
+ ASSERTC(tran_size, ==, UDI_PIO_2BYTE);
+ ret_status = operand;
+ goto end;
+ default:
+ Log_Error("UDI", "udi_pio_trans - Class C %x unimplemented",
+ pio_op);
+ goto error;
+ }
+ }
+ }
+
+ if( ip == pio_handle->nTransOps ) {
+ Log_Notice("UDI", "udi_pio_trans - %p: Overran transaction list",
+ ops);
+ }
+end:
+ callback(gcb, NULL, UDI_OK, ret_status);
+ return ;
+error:
+ callback(gcb, NULL, UDI_STAT_HW_PROBLEM, 0);