pwmgen.c

CNC 的开放码,EMC2 V2.2.8版

//
//    Copyright (C) 2007-2008 Sebastian Kuzminsky
//
//    This program is free software; you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation; either version 2 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program; if not, write to the Free Software
//    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
//

#include <linux/slab.h>

#include "rtapi.h"
#include "rtapi_app.h"
#include "rtapi_string.h"
#include "rtapi_math.h"

#include "hal.h"

#include "hal/drivers/mesa-hostmot2/hostmot2.h"




void hm2_pwmgen_handle_pwm_frequency(hostmot2_t *hm2) {
    u32 dds;


    if (hm2->pwmgen.hal->param.pwm_frequency < 1) {
        ERR("pwmgen.pwm_frequency %d is too low, setting to 1\n", hm2->pwmgen.hal->param.pwm_frequency);
        hm2->pwmgen.hal->param.pwm_frequency = 1;
    }


    // 
    // hal->param.pwm_frequency is the user's desired PWM frequency in Hz
    //
    // We get to play with PWMClock (frequency at which the PWM counter
    // runs) and PWMBits (number of bits of the PWM Value Register that
    // are used to hold the count, this affects resolution).
    //
    // PWMClock is controlled by the 16-bit PWM Master Rate DDS Register:
    //     PWMClock = ClockHigh * DDS / 65536
    //
    // PWMBits is 9-12.  More is better (higher resolution).
    //
    // The key equation is:
    //     PWMFreq = PWMClock / (2^PWMBits)
    //
    // This can be rewritten as:
    //     PWMFreq = (ClockHigh * DDS / 65536) / (2^PWMBits)
    //     PWMFreq = (ClockHigh * DDS) / (65536 * 2^PWMBits)
    //
    // Solve for DDS:
    //     PWMFreq * (65536 * 2^PWMBits) = ClockHigh * DDS
    //     DDS = (PWMFreq * 65536 * 2^PWMBits) / ClockHigh
    //

    // can we do it with 12 bits?
    dds = ((double)hm2->pwmgen.hal->param.pwm_frequency * 65536.0 * 4096.0) / (double)hm2->pwmgen.clock_frequency;
    if (dds < 65536) {
        hm2->pwmgen.pwmgen_master_rate_dds_reg = dds;
        hm2->pwmgen.pwm_bits = 12;
        return;
    }

    // try 11 bits
    dds = ((double)hm2->pwmgen.hal->param.pwm_frequency * 65536.0 * 2048.0) / (double)hm2->pwmgen.clock_frequency;
    if (dds < 65536) {
        hm2->pwmgen.pwmgen_master_rate_dds_reg = dds;
        hm2->pwmgen.pwm_bits = 11;
        return;
    }

    // try 10 bits
    dds = ((double)hm2->pwmgen.hal->param.pwm_frequency * 65536.0 * 1024.0) / (double)hm2->pwmgen.clock_frequency;
    if (dds < 65536) {
        hm2->pwmgen.pwmgen_master_rate_dds_reg = dds;
        hm2->pwmgen.pwm_bits = 10;
        return;
    }

    // try 9 bits
    dds = ((double)hm2->pwmgen.hal->param.pwm_frequency * 65536.0 * 512.0) / (double)hm2->pwmgen.clock_frequency;
    if (dds < 65536) {
        hm2->pwmgen.pwmgen_master_rate_dds_reg = dds;
        hm2->pwmgen.pwm_bits = 9;
        return;
    }

    // no joy, lower frequency until it'll work with 9 bits
    // From above:
    //     PWMFreq = (ClockHigh * DDS) / (65536 * 2^PWMBits)
    hm2->pwmgen.hal->param.pwm_frequency = ((double)hm2->pwmgen.clock_frequency * 65535.0) / (65536.0 * 512.0);
    ERR("max PWM frequency is %d Hz\n", hm2->pwmgen.hal->param.pwm_frequency);
    hm2->pwmgen.pwmgen_master_rate_dds_reg = 65535;
    hm2->pwmgen.pwm_bits = 9;
}


void hm2_pwmgen_handle_pdm_frequency(hostmot2_t *hm2) {
    u32 dds;


    if (hm2->pwmgen.hal->param.pdm_frequency < 1) {
        ERR("pwmgen.pdm_frequency %d is too low, setting to 1\n", hm2->pwmgen.hal->param.pdm_frequency);
        hm2->pwmgen.hal->param.pdm_frequency = 1;
    }


    // 
    // hal->param.pdm_frequency is the user's desired PDM frequency in Hz
    //
    // We get to play with PDMClock (frequency at which the PDM counter
    // runs) only - PDMBits (number of bits of the PDM Value Register that
    // are used to hold the count, this affects resolution) is fixed at 12.
    //
    // PDMClock is controlled by the 16-bit PDM Master Rate DDS Register:
    //     PDMClock = ClockHigh * DDS / 65536
    //
    // PDMBits is 12.
    //
    // The key equation is:
    //     PDMFreq = PDMClock / (2^PDMBits)
    //
    // This can be rewritten as:
    //     PDMFreq = (ClockHigh * DDS / 65536) / (2^PDMBits)
    //     PDMFreq = (ClockHigh * DDS) / (65536 * 2^PDMBits)
    //     PDMFreq = (ClockHigh * DDS) / (65536 * 4096)
    //
    // The PDMFreq is the frequency at which the 4096-pulse pattern
    // repeats.  The pulse frequency is 4096 times higher:
    //
    //     PulseFreq = PDMFreq * 4096
    //     PulseFreq = (ClockHigh * DDS) / 65536
    //
    // Solve for DDS:
    //     PDMFreq * (65536 * 4096) = ClockHigh * DDS
    //     DDS = (PDMFreq * 65536 * 4096) / ClockHigh
    //
    //     PulseFreq = (ClockHigh * DDS) / 65536
    //     DDS = (PulseFreq * 65536) / ClockHigh
    //

    // can we do it with 12 bits?
    dds = ((double)hm2->pwmgen.hal->param.pdm_frequency * 65536.0) / (double)hm2->pwmgen.clock_frequency;
    if (dds == 0) {
        // too slow, set frequency to minimum
        // From above:
        //     PulseFreq = (ClockHigh * DDS) / 65536
        dds = 1;
        hm2->pwmgen.hal->param.pdm_frequency = ((double)hm2->pwmgen.clock_frequency * (double)dds) / 65536.0;
        ERR("min PDM frequency is %d Hz\n", hm2->pwmgen.hal->param.pdm_frequency);
        hm2->pwmgen.pdmgen_master_rate_dds_reg = 1;
        return;
    }

    if (dds < 65536) {
        // ok
        hm2->pwmgen.pdmgen_master_rate_dds_reg = dds;
        return;
    }

    // user wants too much, lower frequency until it'll work with 12 bits
    // From above:
    //     PulseFreq = (ClockHigh * DDS) / 65536
    hm2->pwmgen.hal->param.pdm_frequency = ((double)hm2->pwmgen.clock_frequency * 65535.0) / 65536.0;
    ERR("max PDM frequency is %d Hz\n", hm2->pwmgen.hal->param.pdm_frequency);
    hm2->pwmgen.pdmgen_master_rate_dds_reg = 65535;
}


void hm2_pwmgen_force_write(hostmot2_t *hm2) {
    int i;
    u32 pwm_width;

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

    hm2_pwmgen_handle_pwm_frequency(hm2);
    hm2_pwmgen_handle_pdm_frequency(hm2);

    switch (hm2->pwmgen.pwm_bits) {
        case 9: {
            pwm_width = 0x00;
            break;
        }
        case 10: {
            pwm_width = 0x01;
            break;
        }
        case 11: {
            pwm_width = 0x02;
            break;
        }
        case 12: {
            pwm_width = 0x03;
            break;
        }
        default: {
            // this should never happen
            ERR("invalid pwmgen.bits=%d, resetting to 9\n", hm2->pwmgen.pwm_bits);
            hm2->pwmgen.pwm_bits = 9;
            pwm_width = 0x00;
            break;
        }
    }


    for (i = 0; i < hm2->pwmgen.num_instances; i ++) {
        u32 double_buffered;

        hm2->pwmgen.pwm_mode_reg[i] = pwm_width;

        switch (hm2->pwmgen.instance[i].hal.param.output_type) {
            case HM2_PWMGEN_OUTPUT_TYPE_PWM: {
                // leave the Output Mode bits 0
                double_buffered = 1;
                break;
            }

            case HM2_PWMGEN_OUTPUT_TYPE_UP_DOWN: {
                hm2->pwmgen.pwm_mode_reg[i] |= 0x2 << 3;
                double_buffered = 1;
                break;
            }

            case HM2_PWMGEN_OUTPUT_TYPE_PDM: {
                hm2->pwmgen.pwm_mode_reg[i] |= 0x3 << 3;
                double_buffered = 0;
                break;
            }

            case HM2_PWMGEN_OUTPUT_TYPE_PWM_SWAPPED: {  // Dir & PWM (ie with the output pins swapped), "for locked antiphase"
                hm2->pwmgen.pwm_mode_reg[i] |= 0x1 << 3;
                double_buffered = 1;
                break;
            }

            default: {  // unknown pwm mode!  complain and switch to pwm/dir
                ERR(
                    "invalid pwmgen output_type %d requested\n",
                    hm2->pwmgen.instance[i].hal.param.output_type
                ); 
                ERR(
                    "supported .output-type values are: %d (PWM & Dir), %d (Up & Down), %d (PDM & Dir), and %d (Dir & PWM)\n",
                    HM2_PWMGEN_OUTPUT_TYPE_PWM,
                    HM2_PWMGEN_OUTPUT_TYPE_UP_DOWN,
                    HM2_PWMGEN_OUTPUT_TYPE_PDM,
                    HM2_PWMGEN_OUTPUT_TYPE_PWM_SWAPPED
                ); 
                ERR("switching to 1 (PWM & Dir)\n"); 
                hm2->pwmgen.instance[i].hal.param.output_type = HM2_PWMGEN_OUTPUT_TYPE_PWM;
                double_buffered = 1;
                // leave the Output Mode bits 0
                break;
            }
        }

        hm2->pwmgen.pwm_mode_reg[i] |= (double_buffered << 5);
    }


    // update enable register
    hm2->pwmgen.enable_reg = 0;
    for (i = 0; i < hm2->pwmgen.num_instances; i ++) {
        if (*(hm2->pwmgen.instance[i].hal.pin.enable)) {
            hm2->pwmgen.enable_reg |= (1 << i);
        }
    }


    hm2->llio->write(hm2->llio, hm2->pwmgen.pwm_mode_addr, hm2->pwmgen.pwm_mode_reg, (hm2->pwmgen.num_instances * sizeof(u32)));
    hm2->llio->write(hm2->llio, hm2->pwmgen.enable_addr, &hm2->pwmgen.enable_reg, sizeof(u32));
    hm2->llio->write(hm2->llio, hm2->pwmgen.pwmgen_master_rate_dds_addr, &hm2->pwmgen.pwmgen_master_rate_dds_reg, sizeof(u32));
    hm2->llio->write(hm2->llio, hm2->pwmgen.pdmgen_master_rate_dds_addr, &hm2->pwmgen.pdmgen_master_rate_dds_reg, sizeof(u32));

    if ((*hm2->llio->io_error) != 0) return;

    for (i = 0; i < hm2->pwmgen.num_instances; i ++) {
        hm2->pwmgen.instance[i].written_output_type = hm2->pwmgen.instance[i].hal.param.output_type;
        hm2->pwmgen.instance[i].written_enable = *hm2->pwmgen.instance[i].hal.pin.enable;
    }

    hm2->pwmgen.written_pwm_frequency = hm2->pwmgen.hal->param.pwm_frequency;
    hm2->pwmgen.written_pdm_frequency = hm2->pwmgen.hal->param.pdm_frequency;
}




//
// Update the PWM Mode Registers of all pwmgen instances that need it
//

void hm2_pwmgen_write(hostmot2_t *hm2) {