// ------------------------------------------------------------------------------------------------
static uint UsbHubResetPort(UsbHub *hub, uint port)
{
UsbDevice *dev = hub->dev;
// Reset the port
if (!UsbDevRequest(dev,
RT_HOST_TO_DEV | RT_CLASS | RT_OTHER,
REQ_SET_FEATURE, F_PORT_RESET, port + 1,
0, 0))
{
return 0;
}
// Wait 100ms for port to enable (TODO - remove after dynamic port detection)
u32 status = 0;
for (uint i = 0; i < 10; ++i)
{
// Delay
PitWait(10);
// Get current status
if (!UsbDevRequest(dev,
RT_DEV_TO_HOST | RT_CLASS | RT_OTHER,
REQ_GET_STATUS, 0, port + 1,
sizeof(status), &status))
{
return 0;
}
// Check if device is attached to port
if (~status & PORT_CONNECTION)
{
break;
}
/*
// Acknowledge change in status
if (status & (PORT_ENABLE_CHANGE | PORT_CONNECTION_CHANGE))
{
port_clr(reg, PORT_ENABLE_CHANGE | PORT_CONNECTION_CHANGE);
continue;
}*/
// Check if device is enabled
if (status & PORT_ENABLE)
{
break;
}
}
return status;
}
// ------------------------------------------------------------------------------------------------
static void UsbHubProbe(UsbHub *hub)
{
UsbDevice *dev = hub->dev;
uint portCount = hub->desc.portCount;
// Enable power if needed
if ((hub->desc.chars & HUB_POWER_MASK) == HUB_POWER_INDIVIDUAL)
{
for (uint port = 0; port < portCount; ++port)
{
if (!UsbDevRequest(dev,
RT_HOST_TO_DEV | RT_CLASS | RT_OTHER,
REQ_SET_FEATURE, F_PORT_POWER, port + 1,
0, 0))
{
return;
}
}
PitWait(hub->desc.portPowerTime * 2);
}
// Reset ports
for (uint port = 0; port < portCount; ++port)
{
uint status = UsbHubResetPort(hub, port);
if (status & PORT_ENABLE)
{
uint speed = (status & PORT_SPEED_MASK) >> PORT_SPEED_SHIFT;
UsbDevice *dev = UsbDevCreate();
if (dev)
{
dev->parent = hub->dev;
dev->hc = hub->dev->hc;
dev->port = port;
dev->speed = speed;
dev->maxPacketSize = 8;
dev->hcControl = hub->dev->hcControl;
dev->hcIntr = hub->dev->hcIntr;
if (!UsbDevInit(dev))
{
// TODO - cleanup
}
}
}
}
void smp_init()
{
int i;
//uint8_t nop = 0x90;
//for(i=0x100000;i<0x100810;i++)
//{
//memcpy(i,&nop,0x1);
//}
for( i = 0; i<cpu_count;i++)
{
gdt_insert_tss(5+i*2, &tss64, sizeof(tss_t),0);
}
memcpy(phys_to_virt(0x9000),phys_to_virt(0x00101000),0x800);
terminal_write("cpus available: ");
terminal_set_color(0,9);
terminal_write(itoa(cpu_count,0,10));
terminal_set_color(0,15);
terminal_write("\n");
uint16_t cpu_id_limit = cpu_count;// - 1;
uint16_t localid = localapic_getid();
for(i = 0; i < cpu_id_limit; ++i)
{
cpu_ready = 0;
if(i != localid)
{
uint16_t apicid = acpi_cpuids[i];
localapic_sendinit(apicid);
PitWait(10);
localapic_sendstartup(apicid, 0x9);
PitWait(10);
if(!cpu_ready)
{
localapic_sendstartup(apicid, 0x9);//0xA);//
PitWait(100);
if(!cpu_ready)
{
terminal_set_color(0,12);
terminal_write("Error: ");
terminal_set_color(0,15);
terminal_write("Failed to start cpu ");
terminal_write(itoa(apicid,0,10));
terminal_write("\n");
}
else
{
cpu_ready_count++;
}
}
else
{
cpu_ready_count++;
}
}
}
terminal_write("cpus started: ");
terminal_set_color(0,9);
terminal_write(itoa(cpu_ready_count,0,10));
terminal_set_color(0,15);
terminal_write("\n");
}