hostmot2.c

CNC 的开放码,EMC2 V2.2.8版

        }
        if (r != 0) {
            ERR("request for firmware %s failed, aborting hm2_register (r=%d)\n", hm2->config.firmware, r);
            goto fail0;
        }

        r = bitfile_parse_and_verify(fw, &bitfile);
        if (r != 0) {
            ERR("firmware %s fails verification, aborting hm2_register\n", hm2->config.firmware);
            release_firmware(fw);
            goto fail0;
        }

        INFO("firmware %s:\n", hm2->config.firmware);
        INFO("    %s %s %s\n", bitfile.a.data, bitfile.c.data, bitfile.d.data);
        INFO("    Part Name: %s\n", bitfile.b.data);
        INFO("    FPGA Config: %d bytes\n", bitfile.e.size);

        if (llio->fpga_part_number == NULL) {
            ERR("llio did not provide an FPGA part number, cannot verify firmware part number\n");
        } else {
            if (strcmp(llio->fpga_part_number, bitfile.b.data) != 0) {
                ERR(
                    "board has FPGA '%s', but the firmware in %s is for FPGA '%s'\n",
                    llio->fpga_part_number,
                    hm2->config.firmware,
                    bitfile.b.data
                );
                release_firmware(fw);
                r = -EINVAL;
                goto fail0;
            }
        }

        bitfile_reverse_bits_of_chunk(&bitfile.e);

        if (llio->reset != NULL) {
            r = llio->reset(llio);
            if (r != 0) {
                release_firmware(fw);
                ERR("failed to reset fpga, aborting hm2_register\n");
                goto fail0;
            }
        }

        r = llio->program_fpga(llio, &bitfile);
        release_firmware(fw);
        if (r != 0) {
            ERR("failed to program fpga, aborting hm2_register\n");
            goto fail0;
        }
    }


    // 
    // export a parameter to deal with communication errors
    // NOTE: this is really only useful for EPP boards, PCI doesnt use it
    //

    {
        int r;
        char name[HAL_NAME_LEN + 2];

        llio->io_error = (hal_bit_t *)hal_malloc(sizeof(hal_bit_t));
        if (llio->io_error == NULL) {
            ERR("out of memory!\n");
            r = -ENOMEM;
            goto fail0;
        }

        (*llio->io_error) = 0;

        rtapi_snprintf(name, HAL_NAME_LEN, "%s.io_error", llio->name);
        r = hal_param_bit_new(name, HAL_RW, llio->io_error, llio->comp_id);
        if (r != HAL_SUCCESS) {
            ERR("error adding param '%s', aborting\n", name);
            r = -EINVAL;
            goto fail0;
        }
    }


    // 
    // read & verify FPGA firmware IOCookie
    // 

    {
        uint32_t cookie;

        if (!llio->read(llio, HM2_ADDR_IOCOOKIE, &cookie, 4)) {
            ERR("error reading hm2 cookie\n"); 
            r = -EIO;
            goto fail0;
        }

        if (cookie != HM2_IOCOOKIE) {
            ERR("invalid cookie, got 0x%08X, expected 0x%08X\n", cookie, HM2_IOCOOKIE); 
            ERR("FPGA failed to initialize, or unexpected firmware?\n"); 
            r = -EINVAL;
            goto fail0;
        }
    }


    // 
    // read & verify FPGA firmware ConfigName
    // 

    {
        char name[9];  // read 8, plus 1 for the NULL

        if (!llio->read(llio, HM2_ADDR_CONFIGNAME, name, 8)) {
            ERR("error reading HM2 Config Name\n");
            r = -EIO;
            goto fail0;
        }
        name[8] = '\0';

        if (strncmp(name, HM2_CONFIGNAME, 9) != 0) {
            ERR("invalid config name, got '%s', expected '%s'\n", name, HM2_CONFIGNAME); 
            r = -EINVAL;
            goto fail0;
        }
    }


    // 
    // Looks like HostMot2 alright, go ahead an initialize it
    // 


    // 
    // read the IDROM Header, the Pin Descriptors, and the Module Descriptors
    // 

    r = hm2_read_idrom(hm2);
    if (r != 0) {
        goto fail0;
    }

    r = hm2_read_pin_descriptors(hm2);
    if (r != 0) {
        goto fail0;
    }

    r = hm2_read_module_descriptors(hm2);
    if (r != 0) {
        goto fail0;
    }


    // 
    // process the Module Descriptors and initialize the HostMot2 Modules found
    // 

    r = hm2_parse_module_descriptors(hm2);
    if (r != 0) {
        goto fail1;
    }


    // 
    // allocate memory for the PC's copy of the HostMot2's registers
    //

    r = hm2_allocate_tram_regions(hm2);
    if (r < 0) {
        ERR("error allocating memory for HostMot2 registers\n");
        goto fail1;
    }


    //
    // At this point, all register buffers have been allocated.
    // All non-TRAM register buffers except for the IOPort registers have
    // been initialized. All HAL objects except for the GPIOs have been
    // allocated & exported to HAL.
    //


    // 
    // set IOPorts based on detected, enabled Modules
    // all in-use module instances get all their pins, the unused pins are left as GPIOs
    // 

    hm2_configure_pins(hm2);

    r = hm2_ioport_gpio_export_hal(hm2);
    if (r != 0) {
        goto fail1;
    }


    // 
    // the "raw" interface lets you peek and poke the HostMot2 registers from HAL
    //

    r = hm2_raw_setup(hm2);
    if (r != 0) {
        goto fail1;
    }


    //
    // At this point, all non-TRAM register buffers have been initialized
    // and all HAL objects have been allocated and exported to HAL.
    //


    // 
    // Write out all the non-TRAM register buffers to the FPGA.
    // 
    // This initializes the FPGA to the default load-time state chosen by
    // the hostmot2 driver.  Users can change the state later via HAL.
    //

    hm2_force_write(hm2);


    // 
    // read the TRAM one first time
    //

    r = hm2_tram_read(hm2);
    if (r != 0) {
        goto fail1;
    }

    // set HAL gpio input pins based on FPGA pins
    hm2_ioport_gpio_process_tram_read(hm2);

    // initialize encoder count & pos to 0
    hm2_encoder_tram_init(hm2);
    hm2_encoder_process_tram_read(hm2, 1000);

    // initialize step accumulator, hal count & position to 0
    hm2_stepgen_tram_init(hm2);
    hm2_stepgen_process_tram_read(hm2, 1000);


    // 
    // write the TRAM one first time
    //

    // set gpio output pins (tho there should be none yet) based on their HAL objects
    hm2_ioport_gpio_tram_write_init(hm2);
    hm2_ioport_gpio_prepare_tram_write(hm2);

    // tell stepgen not to move
    // NOTE: the 1000 is the fake "amount of time since function last ran"
    hm2_stepgen_prepare_tram_write(hm2, 1000);

    // tell pwmgen not to move
    hm2_pwmgen_prepare_tram_write(hm2);

    r = hm2_tram_write(hm2);
    if (r != 0) {
        goto fail1;
    }


    //
    // final check for comm errors
    //

    if ((*hm2->llio->io_error) != 0) {
        ERR("comm errors while initializing firmware!\n");
        goto fail1;
    }


    //
    // all initialized show what pins & modules we ended up with
    //

    hm2_print_pin_usage(hm2);

    if (debug_modules) {
        PRINT("HM2 Modules used:\n");
        hm2_print_modules(hm2);
    }


    //
    // export the main read/write functions
    //

    {
        char name[HAL_NAME_LEN + 2];

        rtapi_snprintf(name, HAL_NAME_LEN, "%s.read", hm2->llio->name);
        r = hal_export_funct(name, hm2_read, hm2, 1, 0, hm2->llio->comp_id);
        if (r != 0) {
            ERR("error %d exporting read function %s\n", r, name);
            r = -EINVAL;
            goto fail1;
        }

        rtapi_snprintf(name, HAL_NAME_LEN, "%s.write", hm2->llio->name);
        r = hal_export_funct(name, hm2_write, hm2, 1, 0, hm2->llio->comp_id);
        if (r != 0) {
            ERR("error %d exporting write function %s\n", r, name);
            r = -EINVAL;
            goto fail1;
        }
    }


    //
    // if the llio claims to be threadsafe, export the gpio read/write functions
    //

    if (hm2->llio->threadsafe) {
        char name[HAL_NAME_LEN + 2];

        rtapi_snprintf(name, HAL_NAME_LEN, "%s.read_gpio", hm2->llio->name);
        r = hal_export_funct(name, hm2_read_gpio, hm2, 1, 0, hm2->llio->comp_id);
        if (r != 0) {
            ERR("error %d exporting gpio_read function %s\n", r, name);
            r = -EINVAL;
            goto fail1;
        }

        rtapi_snprintf(name, HAL_NAME_LEN, "%s.write_gpio", hm2->llio->name);
        r = hal_export_funct(name, hm2_write_gpio, hm2, 1, 0, hm2->llio->comp_id);
        if (r != 0) {
            ERR("error %d exporting gpio_write function %s\n", r, name);
            r = -EINVAL;
            goto fail1;
        }
    }


    //
    // found one!
    //

    PRINT("registered\n");

    return 0;


fail1:
    hm2_cleanup(hm2);  // undoes the kmallocs from hm2_parse_module_descriptors()

fail0:
    list_del(&hm2->list);
    kfree(hm2);
    return r;
}




EXPORT_SYMBOL_GPL(hm2_unregister);
void hm2_unregister(hm2_lowlevel_io_t *llio) {
    struct list_head *ptr;

    // kill the watchdog?  nah, let it bite us and make the board safe

    // FIXME: make sure to stop the stepgens & pwmgens

    list_for_each(ptr, &hm2_list) {
        hostmot2_t *hm2 = list_entry(ptr, hostmot2_t, list);
        if (hm2->llio != llio) continue;

        PRINT("unregistered\n");

        hm2_cleanup(hm2);

        list_del(ptr);
        kfree(hm2);

        return;
    }

    PRINT_NO_LL("ignoring request to unregister %s: not found\n", llio->name);
    return;
}




//
// setup and cleanup code
//

int rtapi_app_main(void) {
    PRINT_NO_LL("loading Mesa HostMot2 driver version %s\n", HM2_VERSION);

    comp_id = hal_init("hostmot2");
    if(comp_id < 0) return comp_id;
    INIT_LIST_HEAD(&hm2_list);

    hal_ready(comp_id);

    return 0;
}


void rtapi_app_exit(void) {
    PRINT_NO_LL("unloading\n");
    hal_exit(comp_id);
}




// this pushes our idea of what things are like into the FPGA's poor little mind
void hm2_force_write(hostmot2_t *hm2) {
    hm2_watchdog_force_write(hm2);
    hm2_ioport_force_write(hm2);
    hm2_encoder_force_write(hm2);
    hm2_pwmgen_force_write(hm2);
    hm2_stepgen_force_write(hm2);
}