pwmgen.c

CNC 的开放码,EMC2 V2.2.8版

    int i;

    if (hm2->pwmgen.num_instances == 0) return;

    // check output type
    for (i = 0; i < hm2->pwmgen.num_instances; i ++) {
        if (hm2->pwmgen.instance[i].hal.param.output_type != hm2->pwmgen.instance[i].written_output_type) {
            goto force_write;
        }
    }

    // check pwm & pdm frequency
    if (hm2->pwmgen.hal->param.pwm_frequency != hm2->pwmgen.written_pwm_frequency) goto force_write;
    if (hm2->pwmgen.hal->param.pdm_frequency != hm2->pwmgen.written_pdm_frequency) goto force_write;

    // update enable register?
    for (i = 0; i < hm2->pwmgen.num_instances; i ++) {
        if (*(hm2->pwmgen.instance[i].hal.pin.enable) != hm2->pwmgen.instance[i].written_enable) {
            goto force_write;
        }
    }

    return;

force_write:
    hm2_pwmgen_force_write(hm2);
}




int hm2_pwmgen_parse_md(hostmot2_t *hm2, int md_index) {
    hm2_module_descriptor_t *md = &hm2->md[md_index];
    int r;


    // 
    // some standard sanity checks
    //

    if (!hm2_md_is_consistent(hm2, md_index, 0, 5, 4, 0x0003)) {
        ERR("inconsistent Module Descriptor!\n");
        return -EINVAL;
    }

    if (hm2->pwmgen.num_instances != 0) {
        ERR(
            "found duplicate Module Descriptor for %s (inconsistent firmware), not loading driver\n",
            hm2_get_general_function_name(md->gtag)
        );
        return -EINVAL;
    }

    if (hm2->config.num_pwmgens > md->instances) {
        ERR(
            "config.num_pwmgens=%d, but only %d are available, not loading driver\n",
            hm2->config.num_pwmgens,
            md->instances
        );
        return -EINVAL;
    }

    if (hm2->config.num_pwmgens == 0) {
        return 0;
    }


    // 
    // looks good, start initializing
    // 


    if (hm2->config.num_pwmgens == -1) {
        hm2->pwmgen.num_instances = md->instances;
    } else {
        hm2->pwmgen.num_instances = hm2->config.num_pwmgens;
    }


    // allocate the module-global HAL shared memory
    hm2->pwmgen.hal = (hm2_pwmgen_module_global_t *)hal_malloc(sizeof(hm2_pwmgen_module_global_t));
    if (hm2->pwmgen.hal == NULL) {
        ERR("out of memory!\n");
        r = -ENOMEM;
        goto fail0;
    }


    hm2->pwmgen.instance = (hm2_pwmgen_instance_t *)hal_malloc(hm2->pwmgen.num_instances * sizeof(hm2_pwmgen_instance_t));
    if (hm2->pwmgen.instance == NULL) {
        ERR("out of memory!\n");
        r = -ENOMEM;
        goto fail0;
    }

    hm2->pwmgen.clock_frequency = md->clock_freq;
    hm2->pwmgen.version = md->version;

    hm2->pwmgen.pwm_value_addr = md->base_address + (0 * md->register_stride);
    hm2->pwmgen.pwm_mode_addr = md->base_address + (1 * md->register_stride);
    hm2->pwmgen.pwmgen_master_rate_dds_addr = md->base_address + (2 * md->register_stride);
    hm2->pwmgen.pdmgen_master_rate_dds_addr = md->base_address + (3 * md->register_stride);
    hm2->pwmgen.enable_addr = md->base_address + (4 * md->register_stride);

    r = hm2_register_tram_write_region(hm2, hm2->pwmgen.pwm_value_addr, (hm2->pwmgen.num_instances * sizeof(u32)), &hm2->pwmgen.pwm_value_reg);
    if (r < 0) {
        ERR("error registering tram write region for PWM Value register (%d)\n", r);
        goto fail0;
    }

    hm2->pwmgen.pwm_mode_reg = (u32 *)kmalloc(hm2->pwmgen.num_instances * sizeof(u32), GFP_KERNEL);
    if (hm2->pwmgen.pwm_mode_reg == NULL) {
        ERR("out of memory!\n");
        r = -ENOMEM;
        goto fail0;
    }

    // export to HAL
    // FIXME: r hides the r in enclosing function, and it returns the wrong thing
    {
        int i;
        int r;
        char name[HAL_NAME_LEN + 2];


        // these hal parameters affect all pwmgen instances
        r = hal_param_u32_newf(
            HAL_RW,
            &(hm2->pwmgen.hal->param.pwm_frequency),
            hm2->llio->comp_id,
            "%s.pwmgen.pwm_frequency",
            hm2->llio->name
        );
        if (r != HAL_SUCCESS) {
            ERR("error adding pwmgen.pwm_frequency param, aborting\n");
            goto fail1;
        }
        hm2->pwmgen.hal->param.pwm_frequency = 20000;
        hm2->pwmgen.written_pwm_frequency = 0;

        r = hal_param_u32_newf(
            HAL_RW,
            &(hm2->pwmgen.hal->param.pdm_frequency),
            hm2->llio->comp_id,
            "%s.pwmgen.pdm_frequency",
            hm2->llio->name
        );
        if (r != HAL_SUCCESS) {
            ERR("error adding pwmgen.pdm_frequency param, aborting\n");
            goto fail1;
        }
        hm2->pwmgen.hal->param.pdm_frequency = 20000;
        hm2->pwmgen.written_pdm_frequency = 0;


        for (i = 0; i < hm2->pwmgen.num_instances; i ++) {
            // pins
            rtapi_snprintf(name, HAL_NAME_LEN, "%s.pwmgen.%02d.value", hm2->llio->name, i);
            r = hal_pin_float_new(name, HAL_IN, &(hm2->pwmgen.instance[i].hal.pin.value), hm2->llio->comp_id);
            if (r != HAL_SUCCESS) {
                ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }

            rtapi_snprintf(name, HAL_NAME_LEN, "%s.pwmgen.%02d.enable", hm2->llio->name, i);
            r = hal_pin_bit_new(name, HAL_IN, &(hm2->pwmgen.instance[i].hal.pin.enable), hm2->llio->comp_id);
            if (r != HAL_SUCCESS) {
                ERR("error adding pin '%s', aborting\n", name);
                goto fail1;
            }

            // parameters

            rtapi_snprintf(name, HAL_NAME_LEN, "%s.pwmgen.%02d.scale", hm2->llio->name, i);
            r = hal_param_float_new(name, HAL_RW, &(hm2->pwmgen.instance[i].hal.param.scale), hm2->llio->comp_id);
            if (r != HAL_SUCCESS) {
                ERR("error adding param '%s', aborting\n", name);
                goto fail1;
            }

            r = hal_param_s32_newf(
                HAL_RW,
                &(hm2->pwmgen.instance[i].hal.param.output_type),
                hm2->llio->comp_id,
                "%s.pwmgen.%02d.output-type",
                hm2->llio->name,
                i
            );
            if (r != HAL_SUCCESS) {
                ERR("error adding param, aborting\n");
                goto fail1;
            }

            // init hal objects
            *(hm2->pwmgen.instance[i].hal.pin.enable) = 0;
            *(hm2->pwmgen.instance[i].hal.pin.value) = 0.0;
            hm2->pwmgen.instance[i].hal.param.scale = 1.0;
            hm2->pwmgen.instance[i].hal.param.output_type = HM2_PWMGEN_OUTPUT_TYPE_PWM;

            hm2->pwmgen.instance[i].written_output_type = -666;  // force an update at the start
            hm2->pwmgen.instance[i].written_enable = -666;       // force an update at the start
        }
    }


    return hm2->pwmgen.num_instances;


fail1:
    kfree(hm2->pwmgen.pwm_mode_reg);

fail0:
    hm2->pwmgen.num_instances = 0;
    return r;
}




void hm2_pwmgen_cleanup(hostmot2_t *hm2) {
    if (hm2->pwmgen.num_instances <= 0) return;
    if (hm2->pwmgen.pwm_mode_reg != NULL) {
        kfree(hm2->pwmgen.pwm_mode_reg);
        hm2->pwmgen.pwm_mode_reg = NULL;
    }
    hm2->pwmgen.num_instances = 0;
}




void hm2_pwmgen_print_module(hostmot2_t *hm2) {
    int i;
    PRINT("PWMGen: %d\n", hm2->pwmgen.num_instances);
    if (hm2->pwmgen.num_instances <= 0) return;
    PRINT("    clock_frequency: %d Hz (%s MHz)\n", hm2->pwmgen.clock_frequency, hm2_hz_to_mhz(hm2->pwmgen.clock_frequency));
    PRINT("    version: %d\n", hm2->pwmgen.version);
    PRINT("    pwmgen_master_rate_dds: 0x%08X (%d)\n", hm2->pwmgen.pwmgen_master_rate_dds_reg, hm2->pwmgen.pwmgen_master_rate_dds_reg);
    PRINT("    pdmgen_master_rate_dds: 0x%08X (%d)\n", hm2->pwmgen.pdmgen_master_rate_dds_reg, hm2->pwmgen.pdmgen_master_rate_dds_reg);
    PRINT("    enable: 0x%08X\n", hm2->pwmgen.enable_reg);
    PRINT("    pwm_value_addr: 0x%04X\n", hm2->pwmgen.pwm_value_addr);
    PRINT("    pwm_mode_addr: 0x%04X\n", hm2->pwmgen.pwm_mode_addr);
    PRINT("    pwmgen_master_rate_dds_addr: 0x%04X\n", hm2->pwmgen.pwmgen_master_rate_dds_addr);
    PRINT("    pdmgen_master_rate_dds_addr: 0x%04X\n", hm2->pwmgen.pdmgen_master_rate_dds_addr);
    PRINT("    enable_addr: 0x%04X\n", hm2->pwmgen.enable_addr);
    for (i = 0; i < hm2->pwmgen.num_instances; i ++) {
        PRINT("    instance %d:\n", i);
        PRINT("        hw:\n");
        PRINT(
            "            pwm_val = 0x%08X (%s%d)\n",
            hm2->pwmgen.pwm_value_reg[i],
            ((hm2->pwmgen.pwm_value_reg[i] & 0x80000000) ? "-" : ""),
            ((hm2->pwmgen.pwm_value_reg[i]>>16) & 0x7fff)
        );
        PRINT("            pwm_mode = 0x%08X\n", hm2->pwmgen.pwm_mode_reg[i]);
    }
}




void hm2_pwmgen_prepare_tram_write(hostmot2_t *hm2) {
    int i;

    if (hm2->pwmgen.num_instances == 0) return;

    for (i = 0; i < hm2->pwmgen.num_instances; i ++) {
        double scaled_value;
        double abs_duty_cycle;
        int bits;

        // Normally the PWM & Dir IO pins of the pwmgen instance keep doing
        // their thing even if the enable bit is low.  This is because the
        // downstream equipment *should* ignore PWM & Dir if /Enable is
        // high (this is how it handles bootup & watchdog bites).
        // However, there apparently is equipment that does not behave this
        // way, and that benefits from having PWM & Dir go low when /Enable
        // goes high.  So...
        if (*hm2->pwmgen.instance[i].hal.pin.enable == 0) {
            hm2->pwmgen.pwm_value_reg[i] = 0;
            continue;
        }

        scaled_value = *hm2->pwmgen.instance[i].hal.pin.value / hm2->pwmgen.instance[i].hal.param.scale;

        abs_duty_cycle = fabs(scaled_value);
        if (abs_duty_cycle > 1.0) abs_duty_cycle = 1.0;

        // duty_cycle goes from 0.0 to 1.0, and needs to be puffed out to pwm_bits (if it's pwm) or 12 (if it's pdm)
        if (hm2->pwmgen.instance[i].hal.param.output_type == HM2_PWMGEN_OUTPUT_TYPE_PDM) {
            bits = 12;
        } else {
            bits = hm2->pwmgen.pwm_bits;
        }
        hm2->pwmgen.pwm_value_reg[i] = abs_duty_cycle * (double)((1 << bits) - 1);
        hm2->pwmgen.pwm_value_reg[i] <<= 16;
        if (scaled_value < 0) {
            hm2->pwmgen.pwm_value_reg[i] |= (1 << 31);
        }
    }
}