From 26904c8992e45a3dbbafae273bef81e90d7d9692 Mon Sep 17 00:00:00 2001 From: John Hodge Date: Tue, 8 Oct 2013 16:14:01 +0800 Subject: [PATCH] Modules/UDI - PIO and better instance startup --- .../Interfaces/UDI/include/udi_internal.h | 4 +- .../Interfaces/UDI/include/udi_internal_ma.h | 19 +- KernelLand/Modules/Interfaces/UDI/main.c | 5 +- .../Modules/Interfaces/UDI/management_agent.c | 178 ++++--- .../Modules/Interfaces/UDI/trans/bus_pci.c | 3 + .../Modules/Interfaces/UDI/udi_lib/core/cb.c | 31 +- .../Modules/Interfaces/UDI/udi_lib/core/imc.c | 5 +- .../Interfaces/UDI/udi_lib/core/meta_mgmt.c | 12 +- .../Interfaces/UDI/udi_lib/physio/pio.c | 497 +++++++++++++++++- 9 files changed, 659 insertions(+), 95 deletions(-) diff --git a/KernelLand/Modules/Interfaces/UDI/include/udi_internal.h b/KernelLand/Modules/Interfaces/UDI/include/udi_internal.h index 6476c2f6..e07b8d41 100644 --- a/KernelLand/Modules/Interfaces/UDI/include/udi_internal.h +++ b/KernelLand/Modules/Interfaces/UDI/include/udi_internal.h @@ -142,6 +142,8 @@ struct sUDI_DriverInstance udi_channel_t ManagementChannel; tUDI_DriverInstance *Parent; tUDI_ChildBinding *ParentChildBinding; + + int CurState; // Current MA state tUDI_ChildBinding *FirstChild; tUDI_DriverRegion *Regions[]; @@ -170,7 +172,6 @@ struct sUDI_ChildBinding // --- Metalanguages --- extern tUDI_MetaLang cMetaLang_Management; - // --- Index to pointer translation --- extern udi_ops_init_t *UDI_int_GetOps(tUDI_DriverInstance *Inst, udi_index_t index); extern tUDI_MetaLang *UDI_int_GetMetaLang(tUDI_DriverInstance *Inst, udi_index_t meta_idx); @@ -206,6 +207,7 @@ extern void UDI_int_MakeDeferredCbS(udi_cb_t *cb, udi_op_t *handler, udi_status_ // --- CBs --- extern void *udi_cb_alloc_internal(tUDI_DriverInstance *Inst, udi_ubit8_t bind_cb_idx, udi_channel_t channel); +extern udi_cb_t *udi_cb_alloc_internal_v(tUDI_MetaLang *Meta, udi_index_t MetaCBNum, size_t inline_size, size_t scratch_size, udi_channel_t channel); // --- Attribute Management --- extern udi_instance_attr_type_t udi_instance_attr_get_internal(udi_cb_t *gcb, const char *attr_name, udi_ubit32_t child_ID, void *attr_value, udi_size_t attr_length, udi_size_t *actual_length); diff --git a/KernelLand/Modules/Interfaces/UDI/include/udi_internal_ma.h b/KernelLand/Modules/Interfaces/UDI/include/udi_internal_ma.h index dd49b92f..27f67581 100644 --- a/KernelLand/Modules/Interfaces/UDI/include/udi_internal_ma.h +++ b/KernelLand/Modules/Interfaces/UDI/include/udi_internal_ma.h @@ -9,7 +9,8 @@ #define _UDI_MA_H_ extern void UDI_MA_BindParents(tUDI_DriverModule *Module); -extern tUDI_DriverInstance *UDI_MA_CreateInstance(tUDI_DriverModule *DriverModule); +extern tUDI_DriverInstance *UDI_MA_CreateInstance(tUDI_DriverModule *DriverModule, + tUDI_DriverInstance *ParentInstance, tUDI_ChildBinding *ChildBinding); extern tUDI_DriverRegion *UDI_MA_InitRegion(tUDI_DriverInstance *Inst, udi_ubit16_t Index, udi_ubit16_t Type, size_t RDataSize); extern void UDI_MA_BeginEnumeration(tUDI_DriverInstance *Inst); @@ -20,6 +21,22 @@ extern const udi_cb_init_t cUDI_MgmtCbInitList[]; extern tUDI_DriverModule *gpUDI_LoadedModules; +enum eUDI_MAState +{ + UDI_MASTATE_USAGEIND, + UDI_MASTATE_SECBIND, + UDI_MASTATE_PARENTBIND, + UDI_MASTATE_ENUMCHILDREN, + UDI_MASTATE_ACTIVE +}; + +extern void UDI_MA_TransitionState(tUDI_DriverInstance *Inst, enum eUDI_MAState Src, enum eUDI_MAState Dst); + +enum { + UDI_MGMT_ENUMERATE_CB_NUM, + UDI_MGMT_USAGE_CB_NUM, + UDI_MGMT_CHANNEL_EVENT_CB_NUM, +}; #endif diff --git a/KernelLand/Modules/Interfaces/UDI/main.c b/KernelLand/Modules/Interfaces/UDI/main.c index 1d76678e..bb24d19c 100644 --- a/KernelLand/Modules/Interfaces/UDI/main.c +++ b/KernelLand/Modules/Interfaces/UDI/main.c @@ -681,10 +681,7 @@ tUDI_DriverModule *UDI_int_LoadDriver(void *LoadBase, udi_init_t *info, const ch // Check for orphan drivers, and create an instance of them when loaded if( driver_module->nParents == 0 ) { - tUDI_DriverInstance *inst = UDI_MA_CreateInstance(driver_module); - - // Enumerate so any pre-loaded drivers are detected - UDI_MA_BeginEnumeration(inst); + UDI_MA_CreateInstance(driver_module, NULL, NULL); } else { diff --git a/KernelLand/Modules/Interfaces/UDI/management_agent.c b/KernelLand/Modules/Interfaces/UDI/management_agent.c index 0e1c87e7..512fda3c 100644 --- a/KernelLand/Modules/Interfaces/UDI/management_agent.c +++ b/KernelLand/Modules/Interfaces/UDI/management_agent.c @@ -12,23 +12,15 @@ #include // === 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; @@ -44,6 +36,13 @@ tUDI_DriverInstance *UDI_MA_CreateInstance(tUDI_DriverModule *DriverModule) } } + 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); @@ -53,27 +52,20 @@ tUDI_DriverInstance *UDI_MA_CreateInstance(tUDI_DriverModule *DriverModule) 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; } @@ -93,10 +85,10 @@ tUDI_DriverRegion *UDI_MA_InitRegion(tUDI_DriverInstance *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; @@ -214,7 +206,7 @@ void UDI_MA_AddChild(udi_enumerate_cb_t *cb, udi_index_t ops_idx) } if( best_module != NULL ) { - UDI_MA_CreateChildInstance(best_module, inst, child); + UDI_MA_CreateInstance(best_module, inst, child); } } @@ -239,59 +231,93 @@ void UDI_MA_BindParents(tUDI_DriverModule *Module) // 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; } diff --git a/KernelLand/Modules/Interfaces/UDI/trans/bus_pci.c b/KernelLand/Modules/Interfaces/UDI/trans/bus_pci.c index ab09aa4a..e51db758 100644 --- a/KernelLand/Modules/Interfaces/UDI/trans/bus_pci.c +++ b/KernelLand/Modules/Interfaces/UDI/trans/bus_pci.c @@ -5,6 +5,7 @@ * trans/bus_pci.c * - PCI Bus Driver */ +#define DEBUG 1 #include #include #include @@ -284,6 +285,8 @@ void pci_intr_handle__trans_done(udi_cb_t *gcb, udi_buf_t *new_buf, udi_status_t udi_status_t pci_pio_do_io(uint32_t child_ID, udi_ubit32_t regset_idx, udi_ubit32_t ofs, udi_ubit8_t len, void *data, bool isOutput) { + LOG("child_ID=%i, regset_idx=%i,ofs=0x%x,len=%i,data=%p,isOutput=%b", + child_ID, regset_idx, ofs, len, data, isOutput); tPCIDev pciid = child_ID; // TODO: Cache child mappings diff --git a/KernelLand/Modules/Interfaces/UDI/udi_lib/core/cb.c b/KernelLand/Modules/Interfaces/UDI/udi_lib/core/cb.c index 39822aa6..ae8c155d 100644 --- a/KernelLand/Modules/Interfaces/UDI/udi_lib/core/cb.c +++ b/KernelLand/Modules/Interfaces/UDI/udi_lib/core/cb.c @@ -10,17 +10,29 @@ #include // for cUDI_MgmtCbInitList // === CODE === +udi_cb_t *udi_cb_alloc_internal_v(tUDI_MetaLang *Meta, udi_index_t MetaCBNum, + size_t inline_size, size_t scratch_size, udi_channel_t channel) +{ + ASSERTC(MetaCBNum, <, Meta->nCbTypes); + size_t base = Meta->CbTypes[MetaCBNum].Size; + ASSERTC(base, >=, sizeof(udi_cb_t)); + base -= sizeof(udi_cb_t); + LOG("+ %i + %i + %i", base, inline_size, scratch_size); + udi_cb_t *ret = NEW(udi_cb_t, + base + inline_size + scratch_size); + ret->channel = channel; + ret->scratch = (void*)ret + sizeof(udi_cb_t) + base + inline_size; + return ret; +} void *udi_cb_alloc_internal(tUDI_DriverInstance *Inst, udi_ubit8_t bind_cb_idx, udi_channel_t channel) { const udi_cb_init_t *cb_init; LOG("Inst=%p, bind_cb_idx=%i, channel=%p", Inst, bind_cb_idx, channel); - if(Inst) { - ASSERT(Inst->Module); - ASSERT(Inst->Module->InitInfo); - ASSERT(Inst->Module->InitInfo->cb_init_list); - } - cb_init = Inst ? Inst->Module->InitInfo->cb_init_list : cUDI_MgmtCbInitList; - for( ; cb_init->cb_idx; cb_init ++ ) + ASSERT(Inst); + ASSERT(Inst->Module); + ASSERT(Inst->Module->InitInfo); + ASSERT(Inst->Module->InitInfo->cb_init_list); + + for( cb_init = Inst->Module->InitInfo->cb_init_list; cb_init->cb_idx; cb_init ++ ) { if( cb_init->cb_idx == bind_cb_idx ) { @@ -31,6 +43,9 @@ void *udi_cb_alloc_internal(tUDI_DriverInstance *Inst, udi_ubit8_t bind_cb_idx, Inst->Module->ModuleName, bind_cb_idx, cb_init->meta_idx); return NULL; } + return udi_cb_alloc_internal_v(metalang, cb_init->meta_cb_num, + cb_init->inline_size, cb_init->scratch_requirement, channel); + #if 0 ASSERTC(cb_init->meta_cb_num, <, metalang->nCbTypes); size_t base = metalang->CbTypes[cb_init->meta_cb_num].Size; ASSERTC(base, >=, sizeof(udi_cb_t)); @@ -39,7 +54,9 @@ void *udi_cb_alloc_internal(tUDI_DriverInstance *Inst, udi_ubit8_t bind_cb_idx, udi_cb_t *ret = NEW(udi_cb_t, + base + cb_init->inline_size + cb_init->scratch_requirement); ret->channel = channel; + ret->scratch = (void*)ret + sizeof(udi_cb_t) + base + cb_init->inline_size; return ret; + #endif } } Log_Warning("UDI", "Cannot find CB init def %i for '%s'", diff --git a/KernelLand/Modules/Interfaces/UDI/udi_lib/core/imc.c b/KernelLand/Modules/Interfaces/UDI/udi_lib/core/imc.c index 365f8393..e6529dae 100644 --- a/KernelLand/Modules/Interfaces/UDI/udi_lib/core/imc.c +++ b/KernelLand/Modules/Interfaces/UDI/udi_lib/core/imc.c @@ -6,6 +6,7 @@ #include #include #include +#include // === EXPORTS === EXPORT(udi_channel_anchor); @@ -101,5 +102,7 @@ void udi_channel_event_ind(udi_channel_event_cb_t *cb) void udi_channel_event_complete(udi_channel_event_cb_t *cb, udi_status_t status) { - UNIMPLEMENTED(); + LOG("cb=%p,status=%i", cb, status); + UDI_MA_TransitionState( UDI_GCB(cb)->initiator_context, UDI_MASTATE_PARENTBIND, UDI_MASTATE_ENUMCHILDREN ); + udi_cb_free( UDI_GCB(cb) ); } diff --git a/KernelLand/Modules/Interfaces/UDI/udi_lib/core/meta_mgmt.c b/KernelLand/Modules/Interfaces/UDI/udi_lib/core/meta_mgmt.c index ddaf5146..ab8f68a5 100644 --- a/KernelLand/Modules/Interfaces/UDI/udi_lib/core/meta_mgmt.c +++ b/KernelLand/Modules/Interfaces/UDI/udi_lib/core/meta_mgmt.c @@ -23,9 +23,11 @@ EXPORT(udi_final_cleanup_ack); tUDI_MetaLang cMetaLang_Management = { "udi_mgmt", - 1, + 3, { - {sizeof(udi_enumerate_cb_t), NULL} + {sizeof(udi_enumerate_cb_t), NULL}, + {sizeof(udi_usage_cb_t), NULL}, + {sizeof(udi_channel_event_cb_t), NULL}, } }; @@ -54,6 +56,8 @@ void udi_usage_res(udi_usage_cb_t *cb) { // TODO: Update trace mask from cb LOG("cb=%p{cb->trace_mask=%x}", cb, cb->trace_mask); + UDI_MA_TransitionState(UDI_GCB(cb)->initiator_context, UDI_MASTATE_USAGEIND, UDI_MASTATE_SECBIND); + udi_cb_free( UDI_GCB(cb) ); } void udi_enumerate_req(udi_enumerate_cb_t *cb, udi_ubit8_t enumeration_level) @@ -102,8 +106,12 @@ void udi_enumerate_ack(udi_enumerate_cb_t *cb, udi_ubit8_t enumeration_result, u UDI_MA_AddChild(cb, ops_idx); udi_enumerate_req(cb, UDI_ENUMERATE_NEXT); return ; + case UDI_ENUMERATE_LEAF: case UDI_ENUMERATE_DONE: // All done. Chain terminates + UDI_MA_TransitionState(UDI_GCB(cb)->initiator_context, + UDI_MASTATE_ENUMCHILDREN, UDI_MASTATE_ACTIVE); + udi_cb_free( UDI_GCB(cb) ); return ; default: Log_Notice("UDI", "Unknown enumeration_result %i", enumeration_result); diff --git a/KernelLand/Modules/Interfaces/UDI/udi_lib/physio/pio.c b/KernelLand/Modules/Interfaces/UDI/udi_lib/physio/pio.c index 97dd36d9..c2c229c1 100644 --- a/KernelLand/Modules/Interfaces/UDI/udi_lib/physio/pio.c +++ b/KernelLand/Modules/Interfaces/UDI/udi_lib/physio/pio.c @@ -2,7 +2,7 @@ * \file udi_lib/physio/pio.c * \author John Hodge (thePowersGang) */ -#define DEBUG 1 +#define DEBUG 0 #include #include #include @@ -16,8 +16,11 @@ struct udi_pio_handle_s { 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; @@ -29,6 +32,7 @@ struct udi_pio_handle_s udi_index_t Domain; // TODO: Cached labels +// size_t Labels[]; }; // === IMPORTS === @@ -73,6 +77,8 @@ void udi_pio_map(udi_pio_map_call_t *callback, udi_cb_t *gcb, 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; @@ -103,11 +109,496 @@ void udi_pio_abort_sequence(udi_pio_handle_t pio_handle, udi_size_t scratch_requ 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<> 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<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<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 <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 <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 <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 <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 <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<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<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, -- 2.20.1