hostmot2.c

CNC 的开放码,EMC2 V2.2.8版

            ERR("error reading Module Descriptor %d (at 0x%04x)\n", hm2->num_mds, addr); 
            return -EIO;
        }

        md->gtag = d[0] & 0x000000FF;
        if (md->gtag == 0) {
            // done
            return 0;
        }

        md->version   = (d[0] >>  8) & 0x000000FF;
        md->clock_tag = (d[0] >> 16) & 0x000000FF;
        md->instances = (d[0] >> 24) & 0x000000FF;

        if (md->clock_tag == 1) {
            md->clock_freq = hm2->idrom.clock_low;
        } else if (md->clock_tag == 2) {
            md->clock_freq = hm2->idrom.clock_high;
        } else {
            ERR("Module Descriptor %d (at 0x%04x) has invalid ClockTag %d\n", hm2->num_mds, addr, md->clock_tag); 
            return -EINVAL;
        }

        md->base_address = (d[1] >> 00) & 0x0000FFFF;
        md->num_registers = (d[1] >> 16) & 0x000000FF;

        md->register_stride = (d[1] >> 24) & 0x0000000F;
        if (md->register_stride == 0) {
            md->register_stride = hm2->idrom.register_stride_0;
        } else if (md->register_stride == 1) {
            md->register_stride = hm2->idrom.register_stride_1;
        } else {
            ERR("Module Descriptor %d (at 0x%04x) has invalid RegisterStride %d\n", hm2->num_mds, addr, md->register_stride); 
            return -EINVAL;
        }

        md->instance_stride = (d[1] >> 28) & 0x0000000F;
        if (md->instance_stride == 0) {
            md->instance_stride = hm2->idrom.instance_stride_0;
        } else if (md->instance_stride == 1) {
            md->instance_stride = hm2->idrom.instance_stride_1;
        } else {
            ERR("Module Descriptor %d (at 0x%04x) has invalid InstanceStride %d\n", hm2->num_mds, addr, md->instance_stride); 
            return -EINVAL;
        }

        md->multiple_registers = d[2];

        if (debug_module_descriptors) {
            PRINT("Module Descriptor %d at 0x%04X:\n", hm2->num_mds, addr);
            PRINT("    General Function Tag: %d (%s)\n", md->gtag, hm2_get_general_function_name(md->gtag));
            PRINT("    Version: %d\n", md->version);
            PRINT("    Clock Tag: %d (%s MHz)\n", md->clock_tag, hm2_hz_to_mhz(md->clock_freq));
            PRINT("    Instances: %d\n", md->instances);
            PRINT("    Base Address: 0x%04X\n", md->base_address);
            PRINT("    -- Num Registers: %d\n", md->num_registers);
            PRINT("    Register Stride: 0x%08X\n", md->register_stride);
            PRINT("    -- Instance Stride: 0x%08X\n", md->instance_stride);
            PRINT("    -- Multiple Registers: 0x%08X\n", md->multiple_registers);
        }
    }
    
    return 0;
}




// 
// Here comes the code to "parse" the Module Descriptors, turn them into
// internal-to-the-driver representations, and export those internal
// representations to the HAL.
// 
// There's a general function that walks the MD list and tries to parse
// each MD in turn, and there's a special function to parse each GTag
// (aka Function)
// 
// The per-Module parsers return the number of instances accepted, which
// may be less than the number of instances available, or even 0, if the
// user has disabled some instances using modparams.  The per-Module
// parsers return -1 on error, which causes the module load to fail.
// 


int hm2_md_is_consistent(
    hostmot2_t *hm2,
    int md_index,
    u8 version,
    u8 num_registers,
    u32 instance_stride,
    u32 multiple_registers
) {
    hm2_module_descriptor_t *md = &hm2->md[md_index];

    if (
        (md->num_registers == num_registers)
        && (md->version == version)
        && (md->instance_stride == instance_stride)
        && (md->multiple_registers == multiple_registers)
    ) {
        return 1;
    }

    ERR(
        "inconsistent Module Descriptor for %s, not loading driver\n",
        hm2_get_general_function_name(md->gtag)
    );

    ERR(
        "    Version = %d, expected %d\n",
        md->version,
        version
    );

    ERR(
        "    NumRegisters = %d, expected %d\n",
        md->num_registers,
        num_registers
    );

    ERR(
        "    InstanceStride = 0x%08X, expected 0x%08X\n",
        md->instance_stride,
        instance_stride
    );

    ERR(
        "    MultipleRegisters = 0x%08X, expected 0x%08X\n",
        md->multiple_registers,
        multiple_registers
    );

    return 0;
}




static int hm2_parse_module_descriptors(hostmot2_t *hm2) {
    int md_index;

    for (md_index = 0; md_index < hm2->num_mds; md_index ++) {
        hm2_module_descriptor_t *md = &hm2->md[md_index];
        int md_accepted;

        if (md->gtag == 0) {
            // done
            return 0;
        }

        md_accepted = 0;  // will be set by the switch

        switch (md->gtag) {

            case HM2_GTAG_IOPORT:
                md_accepted = hm2_ioport_parse_md(hm2, md_index);
                break;

            case HM2_GTAG_ENCODER:
                md_accepted = hm2_encoder_parse_md(hm2, md_index);
                break;

            case HM2_GTAG_PWMGEN:
                md_accepted = hm2_pwmgen_parse_md(hm2, md_index);
                break;

            case HM2_GTAG_STEPGEN:
                md_accepted = hm2_stepgen_parse_md(hm2, md_index);
                break;

            case HM2_GTAG_WATCHDOG:
                md_accepted = hm2_watchdog_parse_md(hm2, md_index);
                break;

            default:
                WARN(
                    "MD %d: %dx %s v%d: ignored\n",
                    md_index,
                    md->instances,
                    hm2_get_general_function_name(md->gtag),
                    md->version
                );
                continue;

        }

        if ((*hm2->llio->io_error) != 0) {
            ERR("IO error while parsing Module Descriptor %d\n", md_index);
            return -EIO;
        }

        if (md_accepted >= 0)  {
            INFO(
                "MD %d: %dx %s v%d: accepted, using %d\n",
                md_index,
                md->instances,
                hm2_get_general_function_name(md->gtag),
                md->version,
                md_accepted
            );
        } else {
            ERR("failed to parse Module Descriptor %d\n", md_index);
            return md_accepted;
        }

    }

    return 0;  // success!
}




//
// These functions free all the memory kmalloc'ed in hm2_parse_module_descriptors()
//

static void hm2_cleanup(hostmot2_t *hm2) {
    // clean up the Modules
    hm2_ioport_cleanup(hm2);
    hm2_encoder_cleanup(hm2);
    hm2_watchdog_cleanup(hm2);
    hm2_pwmgen_cleanup(hm2);

    // free all the tram entries
    hm2_tram_cleanup(hm2);
}




void hm2_print_modules(hostmot2_t *hm2) {
    hm2_encoder_print_module(hm2);
    hm2_pwmgen_print_module(hm2);
    hm2_stepgen_print_module(hm2);
    hm2_ioport_print_module(hm2);
    hm2_watchdog_print_module(hm2);
}




// 
// register and unregister, for the low-level I/O drivers to add and remove boards to this hostmot2 driver
//


static void hm2_release_device(struct device *dev) {
    // nothing to do here
}


EXPORT_SYMBOL_GPL(hm2_register);

int hm2_register(hm2_lowlevel_io_t *llio, char *config_string) {
    int r;
    hostmot2_t *hm2;


    // 
    // first a pile of sanity checks
    //

    if (llio == NULL) {
        ERR_NO_LL("NULL llio passed in\n");
        return -EINVAL;
    }


    //
    // verify llio->name
    //

    {
        int i;

        for (i = 0; i < HAL_NAME_LEN; i ++) {
            if (llio->name[i] == '\0') break;
            if (!isprint(llio->name[i])) {
                ERR_NO_LL("invalid llio name passed in (contains non-printable character)\n");
                return -EINVAL;
            }
        }
        if (i == HAL_NAME_LEN) {
            ERR_NO_LL("invalid llio name passed in (not NULL terminated)\n");
            return -EINVAL;
        }
        if (i == 0) {
            ERR_NO_LL("invalid llio name passed in (zero length)\n");
            return -EINVAL;
        }
    }


    //
    // verify llio ioport connector names
    // 

    if ((llio->num_ioport_connectors < 1) || (llio->num_ioport_connectors > ANYIO_MAX_IOPORT_CONNECTORS)) {
        ERR_NO_LL("llio reports invalid number of I/O connectors (%d)\n", llio->num_ioport_connectors);
        return -EINVAL;
    }

    {
        int port;

        for (port = 0; port < llio->num_ioport_connectors; port ++) {
            int i;

            if (llio->ioport_connector_name[port] == NULL) {
                ERR_NO_LL("llio ioport connector name %d is NULL\n", port);
                return -EINVAL;
            }

            for (i = 0; i < HAL_NAME_LEN; i ++) {
                if (llio->ioport_connector_name[port][i] == '\0') break;
                if (!isprint(llio->ioport_connector_name[port][i])) {
                    ERR_NO_LL("invalid llio ioport connector name %d passed in (contains non-printable character)\n", port);
                    return -EINVAL;
                }
            }
            if (i == HAL_NAME_LEN) {
                ERR_NO_LL("invalid llio ioport connector name %d passed in (not NULL terminated)\n", port);
                return -EINVAL;
            }
            if (i == 0) {
                ERR_NO_LL("invalid llio ioport connector name %d passed in (zero length)\n", port);
                return -EINVAL;
            }
        }
    }


    // 
    // verify llio functions
    // 

    if (llio->read == NULL) {
        ERR_NO_LL("NULL llio->read passed in\n");
        return -EINVAL;
    }

    if (llio->write == NULL) {
        ERR_NO_LL("NULL llio->write passed in\n");
        return -EINVAL;
    }

    // NOTE: reset and program_fpga are allowed be NULL


    if (config_string == NULL) {
        PRINT_NO_LL("no firmware specified in config modparam!  the board had better have firmware configured already, or this won't work\n");
    }


    // 
    // make a hostmot2_t struct to represent this device
    //

    hm2 = kmalloc(sizeof(hostmot2_t), GFP_KERNEL);
    if (hm2 == NULL) {
        PRINT_NO_LL("out of memory!\n");
        return -ENOMEM;
    }

    memset(hm2, 0, sizeof(hostmot2_t));

    hm2->llio = llio;

    INIT_LIST_HEAD(&hm2->tram_read_entries);
    INIT_LIST_HEAD(&hm2->tram_write_entries);

    // tentatively add it to the hm2 list
    list_add_tail(&hm2->list, &hm2_list);


    // 
    // parse the config string
    // 

    r = hm2_parse_config_string(hm2, config_string);
    if (r != 0) {
        goto fail0;
    }


    //
    // if programming of the fpga is supported by the board and the user
    // requested a firmware file, fetch it from userspace and program
    // the board
    //

    if ((llio->program_fpga != NULL) && (hm2->config.firmware != NULL)) {
        const struct firmware *fw;
        bitfile_t bitfile;
        struct device dev;

        // check firmware name length
        if (strlen(hm2->config.firmware) > FIRMWARE_NAME_MAX) {
            ERR("requested firmware name '%s' is too long (max length is %d)\n", hm2->config.firmware, FIRMWARE_NAME_MAX);
            r = -ENAMETOOLONG;
            goto fail0;
        }

        memset(&dev, '\0', sizeof(dev));
        strncpy(dev.bus_id, hm2->llio->name, BUS_ID_SIZE);
        dev.bus_id[BUS_ID_SIZE - 1] = '\0';
        dev.release = hm2_release_device;
        r = device_register(&dev);
        if (r != 0) {
            ERR("error with device_register\n");
            goto fail0;
        }

        r = request_firmware(&fw, hm2->config.firmware, &dev);
        device_unregister(&dev);
        if (r == -ENOENT) {
            ERR("firmware %s not found\n", hm2->config.firmware);
            ERR("install the package containing the firmware, or link your RIP sandbox into /lib/firmware manually\n");
            goto fail0;