static void* run_master(void * arg) {
ni = init_ec((char*)arg);
init_slave_db();
if(ni != 0) {
printf("Attach netif \n\n");
EtherCAT_DataLinkLayer::instance()->attach(ni);
printf("Master initializing \n\n");
EtherCAT_Master * EM = EtherCAT_Master::instance();
printf("Getting slave handler\n");
EtherCAT_SlaveHandler * sh_ek1100 = EM->get_slave_handler(0x03e9);
//sleep(1);
printf("Setting EK1100 to OP STATE\n");
if(sh_ek1100->to_state(EC_OP_STATE))
printf("EK1100 succesfully set in OP STATE\n");
else
printf("\nfailed to set EK1100 in OP STATE\n");
printf("Getting slave handler\n");
EtherCAT_SlaveHandler * sh_el4102 = EM->get_slave_handler(0x03ea);
printf("Setting EL4102 to OP STATE\n");
if(sh_el4102->to_state(EC_OP_STATE))
printf("EL4102 succesfully set in OP STATE\n");
else
printf("\nfailed to set EL4102 in OP STATE!!\n");
printf("AL initializing \n\n");
EtherCAT_AL * AL = EtherCAT_AL::instance();
///Set Channel 1 to 5V
unsigned char msg[2] = {0xff, 0x3f};
if(AL->isReady()) {
printf("Test: Set Channel 1 to 5V: \n\n");
int count = 0;
while(count<100000) {
EM->txandrx_PD(sizeof(msg),msg);
count++;
}
printf("Test done.\n");
}
close_socket(ni);
}
}
static void it8716f_init(device_t dev)
{
struct superio_ite_it8716f_config *conf = dev->chip_info;
struct resource *res0;
if (!dev->enabled)
return;
/* TODO: FDC, PP, KBCM, MIDI, GAME, IR. */
switch (dev->path.pnp.device) {
case IT8716F_EC:
res0 = find_resource(dev, PNP_IDX_IO0);
#define EC_INDEX_PORT 5
init_ec(res0->base + EC_INDEX_PORT);
break;
case IT8716F_KBCK:
pc_keyboard_init(&conf->keyboard);
break;
}
}
void init(char *interface)
{
struct netif *ni;
// Initialize network interface
if ((ni = init_ec(interface)) == NULL)
{
fprintf(stderr, "Unable to initialize interface: %s", interface);
exit(-1);
}
// Initialize Application Layer (AL)
EtherCAT_DataLinkLayer::instance()->attach(ni);
EtherCAT_AL *al;
if ((al = EtherCAT_AL::instance()) == NULL)
{
fprintf(stderr, "Unable to initialize Application Layer (AL): %08x", (int)al);
exit(-1);
}
uint32_t num_slaves = al->get_num_slaves();
if (num_slaves == 0)
{
fprintf(stderr, "Unable to locate any slaves");
exit(-1);
}
// Initialize Master
EtherCAT_Master *em;
if ((em = EtherCAT_Master::instance()) == NULL)
{
fprintf(stderr, "Unable to initialize EtherCAT_Master: %08x", int(em));
exit(-1);
}
static int startAddress = 0x00010000;
for (unsigned int slave = 0; slave < num_slaves; ++slave)
{
EC_FixedStationAddress fsa(slave + 1);
EtherCAT_SlaveHandler *sh = em->get_slave_handler(fsa);
if (sh == NULL)
{
fprintf(stderr, "Unable to get slave handler #%d", slave);
exit(-1);
}
if (sh->get_product_code() == WG05::PRODUCT_CODE)
{
printf("found a WG05 at #%d\n", slave);
WG05 *dev = new WG05();
dev->configure(startAddress, sh);
devices.push_back(dev);
}
else if (sh->get_product_code() == WG06::PRODUCT_CODE)
{
printf("found a WG06 at #%d\n", slave);
WG06 *dev = new WG06();
dev->configure(startAddress, sh);
devices.push_back(dev);
}
else
{
printf("found a %08x at #%d\n", sh->get_product_code(), slave);
devices.push_back(NULL);
}
}
BOOST_FOREACH(WG0X *device, devices)
{
Actuator a;
if (!device) continue;
if (!device->sh_->to_state(EC_OP_STATE))
{
fprintf(stderr, "Unable set device %d into OP_STATE", device->sh_->get_ring_position());
}
device->initialize(&a, true);
}