#include <udi_internal_ma.h>
// === CONSTANTS ===
-enum {
- MGMT_CB_ENUM = 1,
-};
-const udi_cb_init_t cUDI_MgmtCbInitList[] = {
- {MGMT_CB_ENUM,0,0, 0,0,NULL},
- {0}
-};
// === PROTOTYPES ===
-tUDI_DriverInstance *UDI_MA_CreateChildInstance(tUDI_DriverModule *Module,
- tUDI_DriverInstance *ParentInstance, tUDI_ChildBinding *ChildBinding);
// === GLOBALS ===
tUDI_DriverInstance *gpUDI_ActiveInstances;
// === CODE ===
-tUDI_DriverInstance *UDI_MA_CreateInstance(tUDI_DriverModule *DriverModule)
+tUDI_DriverInstance *UDI_MA_CreateInstance(tUDI_DriverModule *DriverModule,
+ tUDI_DriverInstance *ParentInstance, tUDI_ChildBinding *ChildBinding)
{
tUDI_DriverInstance *inst = NEW_wA(tUDI_DriverInstance, Regions, DriverModule->nRegions);
udi_primary_init_t *pri_init = DriverModule->InitInfo->primary_init_info;
}
}
+ if( ParentInstance ) {
+ ASSERT(ChildBinding);
+ ChildBinding->BoundInstance = inst;
+ }
+ inst->Parent = ParentInstance;
+ inst->ParentChildBinding = ChildBinding;
+
inst->ManagementChannel = UDI_CreateChannel_Blank(&cMetaLang_Management);
UDI_BindChannel_Raw(inst->ManagementChannel, true,
inst, 0, 0, inst->Regions[0]->InitContext, pri_init->mgmt_ops);
for( int i = 0; i < DriverModule->nRegions; i ++ )
Log("Rgn %i: %p", i, inst->Regions[i]);
- // Send usage indication
- char scratch[pri_init->mgmt_scratch_requirement];
- {
- udi_usage_cb_t ucb;
- memset(&ucb, 0, sizeof(ucb));
- ucb.gcb.scratch = scratch;
- ucb.gcb.channel = inst->ManagementChannel;
- udi_usage_ind(&ucb, UDI_RESOURCES_NORMAL);
- // TODO: Ensure that udi_usage_res is called
- }
-
- for( int i = 1; i < DriverModule->nRegions; i ++ )
- {
- // TODO: Bind secondaries to primary
- Log_Warning("UDI", "TODO: Bind secondary channels");
- //inst->Regions[i]->PriChannel = UDI_CreateChannel_Blank(
- }
+ inst->CurState = UDI_MASTATE_USAGEIND;
+ // Next State: _SECBIND
// Add to global list of active instances
inst->Next = gpUDI_ActiveInstances;
gpUDI_ActiveInstances = inst;
+
+ // Send usage indication
+ udi_usage_cb_t *cb = (void*)udi_cb_alloc_internal_v(&cMetaLang_Management, UDI_MGMT_USAGE_CB_NUM,
+ 0, pri_init->mgmt_scratch_requirement, inst->ManagementChannel
+ );
+ UDI_GCB(cb)->initiator_context = inst;
+ udi_usage_ind(cb, UDI_RESOURCES_NORMAL);
+ // udi_usage_res causes next state transition
return inst;
}
void UDI_MA_BeginEnumeration(tUDI_DriverInstance *Inst)
{
udi_primary_init_t *pri_init = Inst->Module->InitInfo->primary_init_info;
- udi_enumerate_cb_t *ecb = udi_cb_alloc_internal(NULL, MGMT_CB_ENUM, Inst->ManagementChannel);
- memset(ecb, 0, sizeof(ecb));
- ecb->gcb.scratch = malloc(pri_init->mgmt_scratch_requirement);
- ecb->gcb.channel = Inst->ManagementChannel;
+ udi_enumerate_cb_t *ecb = (void*)udi_cb_alloc_internal_v(
+ &cMetaLang_Management, UDI_MGMT_ENUMERATE_CB_NUM,
+ 0, pri_init->mgmt_scratch_requirement, Inst->ManagementChannel);
+ UDI_GCB(ecb)->initiator_context = Inst;
ecb->child_data = malloc(pri_init->child_data_size);
ecb->attr_list = NEW(udi_instance_attr_list_t, *pri_init->enumeration_attr_list_length);
ecb->attr_valid_length = 0;
}
if( best_module != NULL )
{
- UDI_MA_CreateChildInstance(best_module, inst, child);
+ UDI_MA_CreateInstance(best_module, inst, child);
}
}
// No match: Continue
if( level == 0 )
continue ;
- UDI_MA_CreateChildInstance(Module, inst, child);
+ // Found a match, so create an instance (binding happens async)
+ UDI_MA_CreateInstance(Module, inst, child);
}
}
}
}
-tUDI_DriverInstance *UDI_MA_CreateChildInstance(tUDI_DriverModule *Module,
- tUDI_DriverInstance *ParentInstance, tUDI_ChildBinding *ChildBinding)
+void UDI_MA_TransitionState(tUDI_DriverInstance *Inst, enum eUDI_MAState Src, enum eUDI_MAState Dst)
{
- // Found a match, so create an instance and bind it
- tUDI_DriverInstance *inst = UDI_MA_CreateInstance(Module);
- ChildBinding->BoundInstance = inst;
- inst->Parent = ParentInstance;
- inst->ParentChildBinding = ChildBinding;
-
- // TODO: Handle multi-parent drivers
- ASSERTC(Module->nParents, ==, 1);
-
- // Bind to parent
- tUDI_BindOps *parent = &Module->Parents[0];
- udi_channel_t channel = UDI_CreateChannel_Blank( UDI_int_GetMetaLang(inst, parent->meta_idx) );
+ ASSERT(Inst);
+ if( Inst->CurState != Src )
+ return ;
- UDI_BindChannel(channel,true, inst, parent->ops_idx, parent->region_idx, NULL,false,0);
- UDI_BindChannel(channel,false,
- ParentInstance, ChildBinding->Ops->ops_idx, ChildBinding->BindOps->region_idx,
- NULL, true, ChildBinding->ChildID);
-
- udi_channel_event_cb_t ev_cb;
- memset(&ev_cb, 0, sizeof(ev_cb));
- int n_handles = Module->InitInfo->primary_init_info->per_parent_paths;
- udi_buf_path_t handles[n_handles];
- ev_cb.gcb.channel = channel;
- ev_cb.event = UDI_CHANNEL_BOUND;
- if( parent->bind_cb_idx == 0 )
- ev_cb.params.parent_bound.bind_cb = NULL;
- else {
- ev_cb.params.parent_bound.bind_cb = udi_cb_alloc_internal(inst, parent->bind_cb_idx, channel);
- if( !ev_cb.params.parent_bound.bind_cb ) {
- Log_Warning("UDI", "Bind CB index is invalid");
- return NULL;
+ switch(Dst)
+ {
+ case UDI_MASTATE_USAGEIND:
+ ASSERT(Dst != UDI_MASTATE_USAGEIND);
+ break;
+ case UDI_MASTATE_SECBIND:
+ Inst->CurState = UDI_MASTATE_SECBIND;
+ for( int i = 1; i < Inst->Module->nRegions; i ++ )
+ {
+ // TODO: Bind secondaries to primary
+ Log_Warning("UDI", "TODO: Bind secondary channels");
+ //inst->Regions[i]->PriChannel = UDI_CreateChannel_Blank(
}
- UDI_int_ChannelFlip( ev_cb.params.parent_bound.bind_cb );
- }
+ //UDI_MA_TransitionState(Inst, UDI_MASTATE_SECBIND, UDI_MASTATE_PARENTBIND);
+ //break;
+ case UDI_MASTATE_PARENTBIND:
+ Inst->CurState = UDI_MASTATE_PARENTBIND;
+ if( Inst->Parent )
+ {
+ tUDI_DriverModule *Module = Inst->Module;
+ tUDI_ChildBinding *parent_bind = Inst->ParentChildBinding;
+ // TODO: Handle multi-parent drivers
+ ASSERTC(Module->nParents, ==, 1);
+
+ // Bind to parent
+ tUDI_BindOps *parent = &Module->Parents[0];
+ udi_channel_t channel = UDI_CreateChannel_Blank(UDI_int_GetMetaLang(Inst, parent->meta_idx));
+
+ UDI_BindChannel(channel,true, Inst, parent->ops_idx, parent->region_idx, NULL,false,0);
+ UDI_BindChannel(channel,false,
+ Inst->Parent, parent_bind->Ops->ops_idx, parent_bind->BindOps->region_idx,
+ NULL, true, parent_bind->ChildID);
- ev_cb.params.parent_bound.parent_ID = 1;
- ev_cb.params.parent_bound.path_handles = handles;
-
- for( int i = 0; i < n_handles; i ++ ) {
- //handles[i] = udi_buf_path_alloc_internal(inst);
- handles[i] = 0;
+ udi_cb_t *bind_cb;
+ if( parent->bind_cb_idx == 0 )
+ bind_cb = NULL;
+ else {
+ bind_cb = udi_cb_alloc_internal(Inst, parent->bind_cb_idx, channel);
+ if( !bind_cb ) {
+ Log_Warning("UDI", "Bind CB index is invalid");
+ break;
+ }
+ UDI_int_ChannelFlip( bind_cb );
+ }
+
+ int n_handles = Module->InitInfo->primary_init_info->per_parent_paths;
+ udi_buf_path_t handles[n_handles];
+ for( int i = 0; i < n_handles; i ++ ) {
+ //handles[i] = udi_buf_path_alloc_internal(Inst);
+ handles[i] = 0;
+ }
+
+ udi_channel_event_cb_t *ev_cb = (void*)udi_cb_alloc_internal_v(&cMetaLang_Management,
+ UDI_MGMT_CHANNEL_EVENT_CB_NUM, 0, 0, channel);
+ UDI_GCB(ev_cb)->initiator_context = Inst;
+ ev_cb->event = UDI_CHANNEL_BOUND;
+ ev_cb->params.parent_bound.bind_cb = bind_cb;
+ ev_cb->params.parent_bound.parent_ID = 1;
+ ev_cb->params.parent_bound.path_handles = handles;
+
+ udi_channel_event_ind(ev_cb);
+ break;
+ }
+ //UDI_MA_TransitionState(Inst, UDI_MASTATE_PARENTBIND, UDI_MASTATE_ENUMCHILDREN);
+ //break;
+ case UDI_MASTATE_ENUMCHILDREN:
+ Inst->CurState = UDI_MASTATE_ENUMCHILDREN;
+ UDI_MA_BeginEnumeration(Inst);
+ break;
+ case UDI_MASTATE_ACTIVE:
+ Inst->CurState = UDI_MASTATE_ACTIVE;
+ Log_Log("UDI", "Driver instance %s %p entered active state",
+ Inst->Module->ModuleName, Inst);
+ break;
}
-
- udi_channel_event_ind(&ev_cb);
- return inst;
}
* \file udi_lib/physio/pio.c
* \author John Hodge (thePowersGang)
*/
-#define DEBUG 1
+#define DEBUG 0
#include <udi.h>
#include <udi_physio.h>
#include <acess.h>
{
tUDI_DriverInstance *Inst;
udi_index_t RegionIdx;
-
+
_udi_pio_do_io_op_t *IOFunc;
+ udi_ubit32_t ChildID;
+ udi_index_t RegSet;
+
udi_ubit32_t Offset;
udi_ubit32_t Length;
udi_index_t Domain;
// TODO: Cached labels
+// size_t Labels[];
};
// === IMPORTS ===
udi_pio_handle_t ret = malloc( sizeof(struct udi_pio_handle_s) );
ret->Inst = UDI_int_ChannelGetInstance(gcb, false, &ret->RegionIdx);
+ ret->ChildID = ret->Inst->ParentChildBinding->ChildID;
+ ret->RegSet = regset_idx;
ret->IOFunc = io_op;
ret->Offset = base_offset;
ret->Length = length;
UNIMPLEMENTED();
}
+size_t _get_label(udi_pio_handle_t pio_handle, udi_index_t label)
+{
+ udi_pio_trans_t *ops = pio_handle->TransOps;
+ size_t ip = 0;
+ if( label == 0 )
+ return 0;
+ for( ; ip < pio_handle->nTransOps; ip ++ ) {
+ if( ops[ip].pio_op == UDI_PIO_LABEL && ops[ip].operand == label )
+ return ip;
+ }
+ return 0;
+}
+
+typedef union
+{
+ udi_ubit32_t words[32/4];;
+} _trans_value_t;
+
+void _zero_upper(int sizelog2, _trans_value_t *val)
+{
+ switch(sizelog2)
+ {
+ case UDI_PIO_1BYTE:
+ val->words[0] &= 0x00FF;
+ case UDI_PIO_2BYTE:
+ val->words[0] &= 0xFFFF;
+ case UDI_PIO_4BYTE:
+ val->words[1] = 0;
+ case UDI_PIO_8BYTE:
+ val->words[2] = 0;
+ val->words[3] = 0;
+ case UDI_PIO_16BYTE:
+ val->words[4] = 0;
+ val->words[5] = 0;
+ val->words[6] = 0;
+ val->words[7] = 0;
+ case UDI_PIO_32BYTE:
+ break;
+ }
+}
+
+int _compare_zero(_trans_value_t *val, int sizelog2)
+{
+ int is_z=1;
+ int is_n=0;
+ switch(sizelog2)
+ {
+ case UDI_PIO_32BYTE:
+ is_z = is_z && (val->words[7] == 0);
+ is_z = is_z && (val->words[6] == 0);
+ is_z = is_z && (val->words[5] == 0);
+ is_z = is_z && (val->words[4] == 0);
+ case UDI_PIO_16BYTE:
+ is_z = is_z && (val->words[3] == 0);
+ is_z = is_z && (val->words[2] == 0);
+ case UDI_PIO_8BYTE:
+ is_z = is_z && (val->words[1] == 0);
+ case UDI_PIO_4BYTE:
+ is_z = is_z && (val->words[0] == 0);
+ is_n = (val->words[ (1<<sizelog2)/4-1 ] >> 31) != 0;
+ break;
+ case UDI_PIO_2BYTE:
+ is_z = (val->words[0] & 0xFFFF) == 0;
+ is_n = (val->words[0] >> 15) != 0;
+ break;
+ case UDI_PIO_1BYTE:
+ is_z = (val->words[0] & 0xFFFF) == 0;
+ is_n = (val->words[0] >> 7) != 0;
+ break;
+ }
+
+ return is_z | (is_n << 1);
+}
+
+static inline void _operation_shift_left(int sizelog2, _trans_value_t *dstval, int count)
+{
+ const int nwords = (1<<sizelog2)/4;
+ ASSERTC(count, <=, 32);
+ if( sizelog2 <= UDI_PIO_4BYTE ) {
+ dstval->words[0] <<= count;
+ }
+ else if( count == 0 ) {
+ // nop
+ }
+ else if( count == 32 ) {
+ for(int i = nwords - 1; i --; )
+ dstval->words[i+1] = dstval->words[i];
+ dstval->words[0] = 0;
+ }
+ else {
+ for( int i = nwords - 1; i --; )
+ dstval->words[i+1] = (dstval->words[i+1] << count)
+ | (dstval->words[i] >> (32 - count));
+ dstval->words[0] = dstval->words[0] << count;
+ }
+ _zero_upper(sizelog2, dstval);
+}
+static inline void _operation_shift_right(int sizelog2, _trans_value_t *dstval, int count)
+{
+ const int nwords = (1<<sizelog2)/4;
+ ASSERTC(count, <=, 32);
+ if( sizelog2 <= UDI_PIO_4BYTE ) {
+ dstval->words[0] >>= count;
+ }
+ else if( count == 0 ) {
+ // nop
+ }
+ else if( count == 32 ) {
+ for(int i = 0; i < nwords-1; i++ )
+ dstval->words[i] = dstval->words[i+1];
+ dstval->words[nwords-1] = 0;
+ }
+ else {
+ for(int i = 0; i < nwords-1; i++ )
+ dstval->words[i] = (dstval->words[i] >> count)
+ | (dstval->words[i+1] << (32 - count));
+ dstval->words[nwords-1] = dstval->words[nwords-1] >> count;
+ }
+ _zero_upper(sizelog2, dstval);
+}
+static inline void _operation_and(int sizelog2, _trans_value_t *dstval, const _trans_value_t *srcval)
+{
+ for( int i = 0; i < ((1 <<sizelog2)+3)/4; i ++ )
+ dstval->words[i] &= srcval->words[i];
+ _zero_upper(sizelog2, dstval);
+}
+static inline void _operation_or(int sizelog2, _trans_value_t *dstval, const _trans_value_t *srcval)
+{
+ for( int i = 0; i < ((1 <<sizelog2)+3)/4; i ++ )
+ dstval->words[i] |= srcval->words[i];
+ _zero_upper(sizelog2, dstval);
+}
+static inline void _operation_xor(int sizelog2, _trans_value_t *dstval, const _trans_value_t *srcval)
+{
+ for( int i = 0; i < ((1 <<sizelog2)+3)/4; i ++ )
+ dstval->words[i] ^= srcval->words[i];
+ _zero_upper(sizelog2, dstval);
+}
+static inline void _operation_add(int sizelog2, _trans_value_t *dstval, const _trans_value_t *srcval)
+{
+ int c = 0;
+ for( int i = 0; i < ((1 <<sizelog2)+3)/4; i ++ ) {
+ int new_c = (dstval->words[i] + srcval->words[i]) < srcval->words[i];
+ dstval->words[i] += srcval->words[i] + c;
+ c = new_c;
+ }
+ _zero_upper(sizelog2, dstval);
+}
+static inline void _operation_sub(int sizelog2, _trans_value_t *dstval, const _trans_value_t *srcval)
+{
+ if( sizelog2 <= UDI_PIO_4BYTE ) {
+ // Single word doesn't need the borrow logic
+ // - including it would cause bugs
+ dstval->words[0] -= srcval->words[0];
+ }
+ else {
+ int b = 0;
+ for( int i = ((1 <<sizelog2)+3)/4; i --; ) {
+ int new_b = (dstval->words[i] < srcval->words[i] + b);
+ dstval->words[i] -= srcval->words[i] + b;
+ b = new_b;
+ }
+ }
+ _zero_upper(sizelog2, dstval);
+}
+
+#if DEBUG
+static const char *caMEM_MODES[] = {"DIRECT","SCRATCH","BUF","MEM"};
+#endif
+static inline int _read_mem(udi_cb_t *gcb, udi_buf_t *buf, void *mem_ptr,
+ udi_ubit8_t op, int sizelog2,
+ const _trans_value_t *reg, _trans_value_t *val)
+{
+ udi_ubit32_t ofs = reg->words[0];
+ udi_size_t size = 1 << sizelog2;
+ switch(op)
+ {
+ case UDI_PIO_DIRECT:
+ memcpy(val, reg, size);
+ _zero_upper(sizelog2, val);
+ break;
+ case UDI_PIO_SCRATCH:
+ ASSERTCR( (ofs & (size-1)), ==, 0, 1);
+ memcpy(val, gcb->scratch + ofs, size);
+ break;
+ case UDI_PIO_BUF:
+ udi_buf_read(buf, ofs, size, val);
+ break;
+ case UDI_PIO_MEM:
+ ASSERTCR( (ofs & (size-1)), ==, 0, 1 );
+ memcpy(val, mem_ptr + ofs, size);
+ break;
+ }
+ return 0;
+}
+
+static inline int _write_mem(udi_cb_t *gcb, udi_buf_t *buf, void *mem_ptr,
+ udi_ubit8_t op, int sizelog2,
+ _trans_value_t *reg, const _trans_value_t *val)
+{
+ udi_ubit32_t ofs = reg->words[0];
+ udi_size_t size = 1 << sizelog2;
+ switch(op)
+ {
+ case UDI_PIO_DIRECT:
+ memcpy(reg, val, size);
+ _zero_upper(sizelog2, reg);
+ break;
+ case UDI_PIO_SCRATCH:
+ ASSERTCR( (ofs & (size-1)), ==, 0, 1);
+ memcpy(gcb->scratch + ofs, val, size);
+ break;
+ case UDI_PIO_BUF:
+ udi_buf_write(NULL,NULL, val, size, buf, ofs, size, UDI_NULL_BUF_PATH);
+ break;
+ case UDI_PIO_MEM:
+ ASSERTCR( (ofs & (size-1)), ==, 0, 1);
+ memcpy(mem_ptr + ofs, val, size);
+ break;
+ }
+ return 0;
+}
+
void udi_pio_trans(udi_pio_trans_call_t *callback, udi_cb_t *gcb,
udi_pio_handle_t pio_handle, udi_index_t start_label,
udi_buf_t *buf, void *mem_ptr)
{
- UNIMPLEMENTED();
+ 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);
}
void udi_pio_probe(udi_pio_probe_call_t *callback, udi_cb_t *gcb,
git.ucc.asn.au / tpg / acess2.git / commitdiff
UCC git Repository :: git.ucc.asn.au