int
CGBASerial::multiBoot()
{
if(eMode_ != SIO_MULTI_MODE)
{
if(setMode(SIO_MULTI_MODE) == -1)
{
printk("unable to set multi mode\n");
return -1;
}
}
uint16_t slave0, slave1, slave2, slave3;
uint8_t client_bit(0);
// Detect slaves
for(int i(0); i < 15; i++)
{
sendMulti(0x6200, &slave0, &slave1, &slave2, &slave3);
if((slave1 & 0xfff0) == 0x7200)
client_bit |= (slave1 & 0x000f);
if((slave2 & 0xfff0) == 0x7200)
client_bit |= (slave2 & 0x000f);
if((slave3 & 0xfff0) == 0x7200)
client_bit |= (slave3 & 0x000f);
if(client_bit == 0)
{
// Wait 1/16 sec
WAIT_62_5_MS();
}
}
if(client_bit != 0)
{
printk("Detected slaves: ");
if(client_bit & 0x02) printk("1 ");
if(client_bit & 0x04) printk("2 ");
if(client_bit & 0x08) printk("3 ");
printk("\n");
}
else
{
printk("No slaves!\n");
return -1;
}
// Check all slaves
sendMulti(0x6100 + client_bit, &slave0, &slave1, &slave2, &slave3);
if(client_bit & (1<<1))
{
if(slave1 != (0x7200 + (1<<1)))
{
printk("Slave1 ACK error (0x%x)\n", slave1);
return -1;
}
}
if(client_bit & (1<<2))
{
if(slave2 != (0x7200 + (1<<2)))
{
printk("Slave2 ACK error (0x%x)\n", slave2);
return -1;
}
}
if(client_bit & (1<<3))
{
if(slave3 != (0x7200 + (1<<3)))
{
printk("Slave3 ACK error (0x%x)\n", slave3);
return -1;
}
}
printk("1 - ACK ok\n");
// Send multiboot header
sendMulti((pHeader[0*2+1] << 8) + pHeader[0*2], &slave0, &slave1, &slave2, &slave3);
sendMulti((pHeader[1*2+1] << 8) + pHeader[1*2], &slave0, &slave1, &slave2, &slave3);
for(int i(2); i < 0x60; i++)
{
sendMulti((goodHeader[i*2+1] << 8) + goodHeader[i*2], &slave0, &slave1, &slave2, &slave3);
if(client_bit & (1<<1))
{
if(slave1 != (((0x60 - i) << 8) | (1<<1)))
{
printk("Slave1 header invalid response (0x%x)\n", slave1);
return -1;
}
}
if(client_bit & (1<<2))
{
if(slave2 != (((0x60 - i) << 8) | (1<<2)))
{
printk("Slave2 header invalid response (0x%x)\n", slave2);
return -1;
}
}
if(client_bit & (1<<3))
{
if(slave3 != (((0x60 - i) << 8) | (1<<3)))
{
printk("Slave3 header invalid response (0x%x)\n", slave3);
return -1;
}
}
}
printk("Header sent ok\n");
// Check all slaves
sendMulti(0x6200, &slave0, &slave1, &slave2, &slave3);
if(client_bit & (1<<1))
{
if(slave1 != (0x0000 + (1<<1)))
{
printk("Slave1 ACK error (0x%x)\n", slave1);
return -1;
}
}
if(client_bit & (1<<2))
{
if(slave2 != (0x0000 + (1<<2)))
{
printk("Slave2 ACK error (0x%x)\n", slave2);
return -1;
}
}
if(client_bit & (1<<3))
{
if(slave3 != (0x0000 + (1<<3)))
{
printk("Slave3 ACK error (0x%x)\n", slave3);
return -1;
}
}
printk("2 - ACK ok\n");
// Check all slaves
sendMulti(0x6200 + client_bit, &slave0, &slave1, &slave2, &slave3);
if(client_bit & (1<<1))
{
if(slave1 != (0x7200 + (1<<1)))
{
printk("Slave1 ACK error (0x%x)\n", slave1);
return -1;
}
}
if(client_bit & (1<<2))
{
if(slave2 != (0x7200 + (1<<2)))
{
printk("Slave2 ACK error (0x%x)\n", slave2);
return -1;
}
}
if(client_bit & (1<<3))
{
if(slave3 != (0x7200 + (1<<3)))
{
printk("Slave3 ACK error (0x%x)\n", slave3);
return -1;
}
}
printk("3 - ACK ok\n");
// Send palette data and receive client data
uint8_t palette_data(0xc1);
uint8_t client_data[3] = {0xff, 0xff, 0xff};
uint8_t check = client_bit;
while(check != 0)
{
sendMulti(0x6300 + palette_data, &slave0, &slave1, &slave2, &slave3);
if(check & (1<<1))
{
if((slave1 & 0xff00) == 0x7300)
{
client_data[0] = slave0 & 0x00ff;
check &= ~(1<<1);
}
}
if(check & (1<<2))
{
if((slave2 & 0xff00) == 0x7300)
{
client_data[1] = slave0 & 0x00ff;
check &= ~(1<<2);
}
}
if(check & (1<<3))
{
if((slave3 & 0xff00) == 0x7300)
{
client_data[2] = slave0 & 0x00ff;
check &= ~(1<<3);
}
}
}
printk("4 - ACK ok\n");
uint8_t handshake_data = 0x11 + client_data[0] + client_data[1] + client_data[2];
sendMulti(0x6400 + handshake_data, &slave0, &slave1, &slave2, &slave3);
if(client_bit & (1<<1))
{
if((slave1 & 0xff00) != 0x7300)
{
printk("Slave1 ACK error (0x%x)\n", slave1);
return -1;
}
}
if(client_bit & (1<<2))
{
if((slave2 & 0xff00) != 0x7300)
{
printk("Slave2 ACK error (0x%x)\n", slave2);
return -1;
}
}
if(client_bit & (1<<3))
{
if((slave3 & 0xff00) != 0x7300)
{
printk("Slave3 ACK error (0x%x)\n", slave3);
return -1;
}
}
printk("5 - ACK ok\n");
uint32_t length = (256*1024) - 0xc0;
// length = (length + 0xf) & ~0xf; // 16 byte units
// if(length < 0x1c0)
// length = 0x1c0;
memset(&mbp, 0, sizeof(MultiBootParam));
mbp.handshake_data = handshake_data;
mbp.probe_count = 0xd1;
mbp.client_data[0] = client_data[0];
mbp.client_data[1] = client_data[1];
mbp.client_data[2] = client_data[2];
mbp.palette_data = palette_data;
mbp.client_bit = client_bit;
mbp.boot_srcp = pData;
mbp.boot_endp = pData + 0x3ff40; // length;
printk("Sending...");
uint32_t flags = local_save_flags();
local_irq_disable();
uint32_t status = MultiBoot(&mbp, MODE16_MULTI);
local_irq_restore(flags);
if(status == 0)
{
printk("ok\n");
}
else
{
printk("error (0x%x)\n", status);
return -1;
}
return 0;
}
|