m7i43_hm2.comp

CNC 的开放码,EMC2 V2.2.8版

    }

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

    PRINT(
        RTAPI_MSG_ERR,
        "    Version = %d, expected %d\n",
        m->version,
        version
    );

    PRINT(
        RTAPI_MSG_ERR,
        "    NumRegisters = %d, expected %d\n",
        m->num_registers,
        num_registers
    );

    PRINT(
        RTAPI_MSG_ERR,
        "    InstanceStride = 0x%08X, expected 0x%08X\n",
        m->instance_stride,
        instance_stride
    );

    PRINT(
        RTAPI_MSG_ERR,
        "    MultipleRegisters = 0x%08X, expected 0x%08X\n",
        m->multiple_registers,
        multiple_registers
    );

    return 0;
}


// reads the Module Descriptors
// doesnt do any validation or parsing or anything, that's in hm2_parse_module_descriptors(), which comes next
static void m7i43_hm2_read_module_descriptors(m7i43_t *board) {
    int addr = board->idrom_offset + board->idrom.offset_to_modules;

    m7i43_epp_addr16(addr + M7I43_HM2_ADDR_AUTOINCREMENT);

    // we keep track of addr just for debugging, the actual board is using address auto-increment
    for (
        board->num_mds = 0;
        board->num_mds < M7I43_HM2_MAX_MODULE_DESCRIPTORS;
        board->num_mds ++, addr += 12
    ) {
        __u32 d[3];
        hm2_module_descriptor_t *m = &board->md[board->num_mds];


        d[0] = m7i43_epp_read32();
        d[1] = m7i43_epp_read32();
        d[2] = m7i43_epp_read32();

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

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

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

        m->register_stride = (d[1] >> 24) & 0x0000000F;
        if (m->register_stride == 0) {
            m->register_stride = board->idrom.register_stride_0;
        } else {
            m->register_stride = board->idrom.register_stride_1;
        }

        m->instance_stride = (d[1] >> 28) & 0x0000000F;
        if (m->instance_stride == 0) {
            m->instance_stride = board->idrom.instance_stride_0;
        } else {
            m->instance_stride = board->idrom.instance_stride_1;
        }

        m->multiple_registers = d[2];

        if (debug_module_descriptors) {
            int freq_hz;

            if (m->clock_tag == 1) {
                freq_hz = board->idrom.clock_low;
            } else if (m->clock_tag == 2) {
                freq_hz = board->idrom.clock_high;
            } else {
                freq_hz = 0;
            }

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




// 
// 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-Function 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-Function
// parsers return -1 on error, which causes the module load to fail.
// 

static int hm2_md_parse_encoder(m7i43_t *board, hm2_module_descriptor_t *m) {
    if (!hm2_md_is_consistent(m, 2, 5, 4, 0x0003)) {
        PRINT(RTAPI_MSG_ERR, "inconsistent Module Descriptor!\n");
        return -1;
    }

    if (board->encoder.num_instances != 0) {
        PRINT(
            RTAPI_MSG_ERR,
            "Module Descriptor contains duplicate %s (inconsistent firmware), not loading driver\n",
            hm2_get_general_function_name(m->gtag)
        );
        return -1;
    }

    if (num_encoders == -1) {
        board->encoder.num_instances = m->instances;
    } else if (num_encoders > m->instances) {
        PRINT(
            RTAPI_MSG_ERR,
            "num_encoders=%d, but only %d are available, not loading driver\n",
            num_encoders,
            m->instances
        );
        return -1;
    } else {
        board->encoder.num_instances = num_encoders;
    }

    board->encoder.instance = (hm2_encoder_instance_t *)hal_malloc(board->encoder.num_instances * sizeof(hm2_encoder_instance_t));
    if (board->encoder.instance == NULL) {
        PRINT(RTAPI_MSG_ERR, "out of memory!\n");
        return -1;
    }

    if (m->clock_tag == 1) {
        board->encoder.clock_frequency = board->idrom.clock_low;
    } else if (m->clock_tag == 2) {
        board->encoder.clock_frequency = board->idrom.clock_high;
    } else {
        PRINT(RTAPI_MSG_ERR, "%s MD has invalid Clock Tag %d\n", hm2_get_general_function_name(m->gtag), m->clock_tag);
    }

    board->encoder.version = m->version;

    board->encoder.counter_addr = m->base_address + (0 * m->register_stride);
    board->encoder.latch_control_addr = m->base_address + (1 * m->register_stride);
    board->encoder.timestamp_div_addr = m->base_address + (2 * m->register_stride);
    board->encoder.timestamp_count_addr = m->base_address + (3 * m->register_stride);
    board->encoder.filter_rate_addr = m->base_address + (4 * m->register_stride);

    // export the encoders to HAL
    {
        int i;
        int r;
        char name[HAL_NAME_LEN + 2];

        for (i = 0; i < board->encoder.num_instances; i ++) {
            // pins
            rtapi_snprintf(name, HAL_NAME_LEN, "m7i43_hm2.%d.encoder.%02d.count", 0, i);
            r = hal_pin_s32_new(name, HAL_OUT, &(board->encoder.instance[i].hal.pin.count), comp_id);
            if (r != HAL_SUCCESS) {
                PRINT(RTAPI_MSG_ERR, "error adding pin '%s', aborting\n", name);
                return -1;
            }

            rtapi_snprintf(name, HAL_NAME_LEN, "m7i43_hm2.%d.encoder.%02d.position", 0, i);
            r = hal_pin_float_new(name, HAL_OUT, &(board->encoder.instance[i].hal.pin.position), comp_id);
            if (r != HAL_SUCCESS) {
                PRINT(RTAPI_MSG_ERR, "error adding pin '%s', aborting\n", name);
                return -1;
            }

            // parameters
            rtapi_snprintf(name, HAL_NAME_LEN, "m7i43_hm2.%d.encoder.%02d.scale", 0, i);
            r = hal_param_float_new(name, HAL_RW, &(board->encoder.instance[i].hal.param.scale), comp_id);
            if (r != HAL_SUCCESS) {
                PRINT(RTAPI_MSG_ERR, "error adding param '%s', aborting\n", name);
                return -1;
            }

            // init
            board->encoder.instance[i].hw.count = 0;
            board->encoder.instance[i].hw.prev_count = 0;

            board->encoder.instance[i].hw.timestamp = 0;
            board->encoder.instance[i].hw.prev_timestamp = 0;

            board->encoder.instance[i].hw.control = 0x0800;  // quad filter = 15 clocks
            // board->encoder.instance[i].hw.control = 0x0000;  // quad filter = 3 clocks

            *(board->encoder.instance[i].hal.pin.count) = 0;
            *(board->encoder.instance[i].hal.pin.position) = 0.0;
            board->encoder.instance[i].hal.param.scale = 1.0;
        }
    }


    //
    // write the encoder control registers
    //

    {
        int i;

        m7i43_epp_addr16(board->encoder.latch_control_addr + M7I43_HM2_ADDR_AUTOINCREMENT);
        for (i = 0; i < board->encoder.num_instances; i ++) {
            m7i43_epp_write32(board->encoder.instance[i].hw.control);
        }
    }


    return board->encoder.num_instances;
}


static int hm2_md_parse_pwmgen(m7i43_t *board, hm2_module_descriptor_t *m) {
    if (!hm2_md_is_consistent(m, 0, 5, 4, 0x0003)) {
        PRINT(RTAPI_MSG_ERR, "inconsistent Module Descriptor!\n");
        return -1;
    }

    if (board->pwmgen.num_instances != 0) {
        PRINT(
            RTAPI_MSG_ERR,
            "Module Descriptor contains duplicate %s (inconsistent firmware), not loading driver\n",
            hm2_get_general_function_name(m->gtag)
        );
        return -1;
    }

    if (num_pwmgens == -1) {
        board->pwmgen.num_instances = m->instances;
    } else if (num_pwmgens > m->instances) {
        PRINT(
            RTAPI_MSG_ERR,
            "num_pwmgens=%d, but only %d are available, not loading driver\n",
            num_pwmgens,
            m->instances
        );
        return -1;
    } else {
        board->pwmgen.num_instances = num_pwmgens;
    }

    board->pwmgen.instance = (hm2_pwmgen_instance_t *)hal_malloc(board->pwmgen.num_instances * sizeof(hm2_pwmgen_instance_t));
    if (board->pwmgen.instance == NULL) {
        PRINT(RTAPI_MSG_ERR, "out of memory!\n");
        return -1;
    }

    if (m->clock_tag == 1) {
        board->pwmgen.clock_frequency = board->idrom.clock_low;
    } else if (m->clock_tag == 2) {
        board->pwmgen.clock_frequency = board->idrom.clock_high;
    } else {
        PRINT(RTAPI_MSG_ERR, "%s MD has invalid Clock Tag %d\n", hm2_get_general_function_name(m->gtag), m->clock_tag);
    }

    board->pwmgen.version = m->version;

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

    // export to HAL
    {
        int i;
        int r;
        char name[HAL_NAME_LEN + 2];

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

            rtapi_snprintf(name, HAL_NAME_LEN, "m7i43_hm2.%d.pwmgen.%02d.enable", 0, i);
            r = hal_pin_bit_new(name, HAL_IN, &(board->pwmgen.instance[i].hal.pin.enable), comp_id);
            if (r != HAL_SUCCESS) {
                PRINT(RTAPI_MSG_ERR, "error adding pin '%s', aborting\n", name);
                return -1;
            }

            // parameters

            rtapi_snprintf(name, HAL_NAME_LEN, "m7i43_hm2.%d.pwmgen.%02d.scale", 0, i);
            r = hal_param_float_new(name, HAL_RW, &(board->pwmgen.instance[i].hal.param.scale), comp_id);
            if (r != HAL_SUCCESS) {
                PRINT(RTAPI_MSG_ERR, "error adding param '%s', aborting\n", name);
                return -1;
            }

            r = hal_param_s32_newf(
                HAL_RW,
                &(board->pwmgen.instance[i].hal.param.output_type),
                comp_id,
                "m7i43_hm2.%d.pwmgen.%02d.output-type",
                0,
                i
            );
            if (r != HAL_SUCCESS) {
                PRINT(RTAPI_MSG_ERR, "error adding param, aborting\n");
                return -1;
            }

            // init
            *(board->pwmgen.instance[i].hal.pin.enable) = 0;
            *(board->pwmgen.instance[i].hal.pin.value) = 0.0;
            board->pwmgen.instance[i].hal.param.scale = 1.0;
            board->pwmgen.instance[i].hal.param.output_type = 1;  // default to PWM+Dir
            board->pwmgen.instance[i].hal.param.written_output_type = -666;  // force at update at the start

            // these are the fields of the PWM Mode Register that are not exposed to HAL
            // FIXME: let the user choose some of these bits?
            board->pwmgen.instance[i].pwm_width_select = 0x3;
            board->pwmgen.instance[i].pwm_mode_select = 0;
            board->pwmgen.instance[i].pwm_double_buffered = 1;
        }
    }


    hm2_pwmgen_update_mode_registers(board);


    // 
    // set the PWM frequency
    // FIXME: this is broken and should be exported to HAL
    // 
    // 16 bit PWM gen master rate DDS (PWMCLOCK = CLKHIGH*Rate/65536)
    // PWM rate will be PWMCLOCK/(2^PWMBITS) for normal and interleaved PWM
    // and PWMCLOCK/(2^(PWMBITS+1)) for symmetrical mode PWM.
    // 
    // ClockHigh = 100 MHz
    // 7i30 max PWM rate is 50 KHz
    // 
    // PWMClock = 100 MHz * (val / 65536)
    // PWMRate = PWMClock / 2^PWMBits = PWMClock / 2^12 = PWMClock / 4096
    // 
    // PWMRate = 50 Khz = PWMClock / 4096
    // PWMClock = 205 MHz!?
    // 
    // PWMClock = 100 MHz * 65536 / 65536 = 100 MHz
    // PWMRate = 100 MHz / 4096 = 24 KHz