diff --git a/src/Makefile b/src/Makefile index e7e9810..4e1b7dd 100644 --- a/src/Makefile +++ b/src/Makefile @@ -605,6 +605,8 @@ obj-$(CONFIG_STEPGEN) += stepgen.o stepgen-objs := hal/components/stepgen.o $(MATHSTUB) obj-$(CONFIG_FREQGEN) += freqgen.o freqgen-objs := hal/components/freqgen.o $(MATHSTUB) +obj-$(CONFIG_TPPWMGEN) += tpwmgen.o +tppwmgen-objs := hal/components/tppwmgen.o $(MATHSTUB) obj-$(CONFIG_PWMGEN) += pwmgen.o pwmgen-objs := hal/components/pwmgen.o $(MATHSTUB) obj-$(CONFIG_SIGGEN) += siggen.o @@ -664,6 +666,7 @@ hostmot2-objs := \ hal/drivers/mesa-hostmot2/ioport.o \ hal/drivers/mesa-hostmot2/encoder.o \ hal/drivers/mesa-hostmot2/pwmgen.o \ + hal/drivers/mesa-hostmot2/tppwmgen.o \ hal/drivers/mesa-hostmot2/stepgen.o \ hal/drivers/mesa-hostmot2/watchdog.o \ hal/drivers/mesa-hostmot2/pins.o \ @@ -814,6 +817,7 @@ endif ../rtlib/stepgen$(MODULE_EXT): $(addprefix objects/rt,$(stepgen-objs)) ../rtlib/freqgen$(MODULE_EXT): $(addprefix objects/rt,$(freqgen-objs)) ../rtlib/pwmgen$(MODULE_EXT): $(addprefix objects/rt,$(pwmgen-objs)) +../rtlib/tppwmgen$(MODULE_EXT): $(addprefix objects/rt,$(tppwmgen-objs)) ../rtlib/siggen$(MODULE_EXT): $(addprefix objects/rt,$(siggen-objs)) ../rtlib/at_pid$(MODULE_EXT): $(addprefix objects/rt,$(at_pid-objs)) ../rtlib/pid$(MODULE_EXT): $(addprefix objects/rt,$(pid-objs)) diff --git a/src/hal/drivers/mesa-hostmot2/hostmot2.c b/src/hal/drivers/mesa-hostmot2/hostmot2.c index 18d7281..ea48248 100644 --- a/src/hal/drivers/mesa-hostmot2/hostmot2.c +++ b/src/hal/drivers/mesa-hostmot2/hostmot2.c @@ -83,6 +83,7 @@ static void hm2_read(void *void_hm2, long period) { if ((*hm2->llio->io_error) != 0) return; hm2_ioport_gpio_process_tram_read(hm2); + hm2_tp_pwmgen_process_tram_read(hm2); // check the status of the fault bits hm2_encoder_process_tram_read(hm2, period); hm2_stepgen_process_tram_read(hm2, period); @@ -101,6 +102,7 @@ static void hm2_write(void *void_hm2, long period) { hm2_ioport_gpio_prepare_tram_write(hm2); hm2_pwmgen_prepare_tram_write(hm2); + hm2_tp_pwmgen_prepare_tram_write(hm2); hm2_stepgen_prepare_tram_write(hm2, period); hm2_tram_write(hm2); @@ -109,9 +111,9 @@ static void hm2_write(void *void_hm2, long period) { hm2_ioport_write(hm2); // handles gpio.is_output but not gpio.out (that's done in tram_write() above) hm2_watchdog_write(hm2); // in case the user has written to the watchdog.timeout_ns param hm2_pwmgen_write(hm2); // update pwmgen registers if needed + hm2_tp_pwmgen_write(hm2); // update Three Phase PWM registers if needed hm2_stepgen_write(hm2); // update stepgen registers if needed hm2_encoder_write(hm2); // update ctrl register if needed - hm2_raw_write(hm2); } @@ -171,6 +173,7 @@ const char *hm2_get_general_function_name(int gtag) { case HM2_GTAG_STEPGEN: return "StepGen"; case HM2_GTAG_PWMGEN: return "PWMGen"; case HM2_GTAG_TRANSLATIONRAM: return "TranslationRAM"; + case HM2_GTAG_3PPWM: return "ThreePhasePWM"; default: { static char unknown[100]; rtapi_snprintf(unknown, 100, "(unknown-gtag-%d)", gtag); @@ -188,6 +191,7 @@ static int hm2_parse_config_string(hostmot2_t *hm2, char *config_string) { // default is to enable everything in the firmware hm2->config.num_encoders = -1; hm2->config.num_pwmgens = -1; + hm2->config.num_tp_pwmgens = -1; hm2->config.num_stepgens = -1; hm2->config.enable_raw = 0; hm2->config.firmware = NULL; @@ -214,7 +218,11 @@ static int hm2_parse_config_string(hostmot2_t *hm2, char *config_string) { } else if (strncmp(token, "num_pwmgens=", 12) == 0) { token += 12; hm2->config.num_pwmgens = simple_strtol(token, NULL, 0); - + + } else if (strncmp(token, "num_3p_pwmgens=", 15) == 0) { + token += 15; + hm2->config.num_tp_pwmgens = simple_strtol(token, NULL, 0); + } else if (strncmp(token, "num_stepgens=", 13) == 0) { token += 13; hm2->config.num_stepgens = simple_strtol(token, NULL, 0); @@ -238,6 +246,7 @@ static int hm2_parse_config_string(hostmot2_t *hm2, char *config_string) { HM2_DBG("final config:\n"); HM2_DBG(" num_encoders=%d\n", hm2->config.num_encoders); HM2_DBG(" num_pwmgens=%d\n", hm2->config.num_pwmgens); + HM2_DBG(" num_3Ppwmgens=%d\n", hm2->config.num_tp_pwmgens); HM2_DBG(" num_stepgens=%d\n", hm2->config.num_stepgens); HM2_DBG(" enable_raw=%d\n", hm2->config.enable_raw); HM2_DBG(" firmware=%s\n", hm2->config.firmware ? hm2->config.firmware : "(NULL)"); @@ -620,6 +629,10 @@ static int hm2_parse_module_descriptors(hostmot2_t *hm2) { case HM2_GTAG_WATCHDOG: md_accepted = hm2_watchdog_parse_md(hm2, md_index); break; + + case HM2_GTAG_3PPWM: + md_accepted = hm2_tp_pwmgen_parse_md(hm2, md_index); + break; default: HM2_WARN( @@ -673,7 +686,7 @@ static void hm2_cleanup(hostmot2_t *hm2) { hm2_encoder_cleanup(hm2); hm2_watchdog_cleanup(hm2); hm2_pwmgen_cleanup(hm2); - + hm2_tp_pwmgen_cleanup(hm2); // free all the tram entries hm2_tram_cleanup(hm2); } @@ -684,6 +697,7 @@ static void hm2_cleanup(hostmot2_t *hm2) { void hm2_print_modules(hostmot2_t *hm2) { hm2_encoder_print_module(hm2); hm2_pwmgen_print_module(hm2); + hm2_tp_pwmgen_print_module(hm2); hm2_stepgen_print_module(hm2); hm2_ioport_print_module(hm2); hm2_watchdog_print_module(hm2); @@ -1280,8 +1294,6 @@ void rtapi_app_exit(void) { } - - // 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); @@ -1289,5 +1301,6 @@ void hm2_force_write(hostmot2_t *hm2) { hm2_encoder_force_write(hm2); hm2_pwmgen_force_write(hm2); hm2_stepgen_force_write(hm2); + hm2_tp_pwmgen_force_write(hm2); } diff --git a/src/hal/drivers/mesa-hostmot2/hostmot2.h b/src/hal/drivers/mesa-hostmot2/hostmot2.h index 1a1ac18..d53456e 100644 --- a/src/hal/drivers/mesa-hostmot2/hostmot2.h +++ b/src/hal/drivers/mesa-hostmot2/hostmot2.h @@ -100,7 +100,7 @@ char **argv_split(gfp_t gfp, const char *str, int *argcp); #define HM2_GTAG_STEPGEN (5) #define HM2_GTAG_PWMGEN (6) #define HM2_GTAG_TRANSLATIONRAM (11) - +#define HM2_GTAG_3PPWM (19) @@ -390,6 +390,80 @@ typedef struct { } hm2_pwmgen_t; +// +// 3-Phase pwmgen +// + + +typedef struct { + + struct { + + struct { + hal_float_t *Avalue; + hal_float_t *Bvalue; + hal_float_t *Cvalue; + hal_bit_t *fault; + hal_bit_t *enable; + } pin; + + struct { + hal_float_t scale; + hal_float_t deadzone; + hal_bit_t faultpolarity; + hal_float_t sampletime; + } param; + + } hal; + + // these keep track of the written values of each register so we know if an update-write is needed + bool written_enable; + float written_deadzone; + bool written_faultpolarity; + float written_sampletime; + +} hm2_tp_pwmgen_instance_t; + + +// these hal params affect all pwmgen instances +typedef struct { + struct { + hal_u32_t pwm_frequency; + } param; +} hm2_tp_pwmgen_module_global_t; + + +typedef struct { + int num_instances; + + hm2_tp_pwmgen_instance_t *instance; + + u32 clock_frequency; + u8 version; + + // module-global HAL objects... + hm2_tp_pwmgen_module_global_t *hal; + + // these keep track of the most recent hal->param.p{d,w}m_frequency + // that we've told the FPGA about, so we know if we need to update it + u32 written_pwm_frequency; + + // number of bits of resolution of the PWM signal. Always 16 for Three Phase + int pwm_bits; + + u32 pwm_value_addr; // All three phases share a register (10 bits each) + u32 *pwm_value_reg; // Pointer to a memory block that holds the set. + + u32 setup_addr; // holds dead-time, fault polarity and ADC sample time + u32 *setup_reg; + + u32 enable_addr; // a 32-bit enable register for each tp_pwmgen. Seems excessive + u32 *enable_reg; + + u32 pwmgen_master_rate_dds_addr; + u32 pwmgen_master_rate_dds_reg; // one register for the whole Function + +} hm2_tp_pwmgen_t; // @@ -566,8 +640,7 @@ typedef struct { u32 *reset_reg; } hm2_watchdog_t; - - + // // raw peek/poke access @@ -615,6 +688,7 @@ typedef struct { struct { int num_encoders; int num_pwmgens; + int num_tp_pwmgens; int num_stepgens; int enable_raw; char *firmware; @@ -643,10 +717,10 @@ typedef struct { // the hostmot2 "Functions" hm2_encoder_t encoder; hm2_pwmgen_t pwmgen; + hm2_tp_pwmgen_t tp_pwmgen; hm2_stepgen_t stepgen; hm2_ioport_t ioport; hm2_watchdog_t watchdog; - hm2_raw_t *raw; struct list_head list; @@ -765,6 +839,17 @@ void hm2_pwmgen_force_write(hostmot2_t *hm2); void hm2_pwmgen_prepare_tram_write(hostmot2_t *hm2); +// +// Three Phase pwmgen functions +// + +int hm2_tp_pwmgen_parse_md(hostmot2_t *hm2, int md_index); +void hm2_tp_pwmgen_print_module(hostmot2_t *hm2); +void hm2_tp_pwmgen_cleanup(hostmot2_t *hm2); +void hm2_tp_pwmgen_write(hostmot2_t *hm2); +void hm2_tp_pwmgen_force_write(hostmot2_t *hm2); +void hm2_tp_pwmgen_prepare_tram_write(hostmot2_t *hm2); +void hm2_tp_pwmgen_process_tram_read(hostmot2_t *hm2); // @@ -793,9 +878,6 @@ void hm2_watchdog_cleanup(hostmot2_t *hm2); void hm2_watchdog_write(hostmot2_t *hm2); void hm2_watchdog_force_write(hostmot2_t *hm2); - - - // // the raw interface lets you peek and poke the hostmot2 instance from HAL // diff --git a/src/hal/drivers/mesa-hostmot2/pins.c b/src/hal/drivers/mesa-hostmot2/pins.c index 6bff378..990ae2a 100644 --- a/src/hal/drivers/mesa-hostmot2/pins.c +++ b/src/hal/drivers/mesa-hostmot2/pins.c @@ -63,6 +63,19 @@ static const char* hm2_get_pin_secondary_name(hm2_pin_t *pin) { case 3: return "Not-Enable"; } break; + + case HM2_GTAG_3PPWM: + switch (sec_pin) { + case 1: return "PWM A"; + case 2: return "PWM B"; + case 3: return "PWM C"; + case 4: return "PWM /A"; + case 5: return "PWM /B"; + case 6: return "PWM /C"; + case 7: return "Enable"; + case 8: return "Fault"; + } + break; case HM2_GTAG_STEPGEN: // FIXME: these depend on the stepgen mode @@ -335,6 +348,7 @@ void hm2_configure_pins(hostmot2_t *hm2) { // encoder and pwmgen just get all their enabled instances' pins hm2_pins_allocate_all(hm2, HM2_GTAG_ENCODER, hm2->encoder.num_instances); hm2_pins_allocate_all(hm2, HM2_GTAG_PWMGEN, hm2->pwmgen.num_instances); + hm2_pins_allocate_all(hm2, HM2_GTAG_3PPWM, hm2->tp_pwmgen.num_instances); } diff --git a/src/hal/drivers/mesa-hostmot2/tppwmgen.c b/src/hal/drivers/mesa-hostmot2/tppwmgen.c new file mode 100755 index 0000000..6337937 --- /dev/null +++ b/src/hal/drivers/mesa-hostmot2/tppwmgen.c @@ -0,0 +1,480 @@ +// +// Copyright (C) 2010 Andy Pugh +// Heavily based on pwmgen.c (C) 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 + +#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_tp_pwmgen_handle_pwm_frequency(hostmot2_t *hm2) { + u32 dds; + int deadtime; + int i; + + if (hm2->tp_pwmgen.hal->param.pwm_frequency < 1) { + HM2_ERR("pwmgen.pwm_frequency %d is too low, setting to 1\n", hm2->tp_pwmgen.hal->param.pwm_frequency); + hm2->tp_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) + // + // PWMClock is controlled by the 16-bit PWM Master Rate DDS Register: + // PWMClock = ClockHigh * DDS / 65536 + // + // PWMBits is fixed at 11 for 3-Phase PWM. + + // The key equation is: + // PWMFreq = PWMClock / 2048 + + // Solve for DDS: + // PWMFreq * (65536 * 2048) = ClockHigh * DDS + // DDS = (PWMFreq * 65536 * 2048) / ClockHigh + // + + // check that the frequency is achievable + dds = ((double)hm2->tp_pwmgen.hal->param.pwm_frequency * 65536.0 * 2048.0) / (double)hm2->tp_pwmgen.clock_frequency; + if (dds < 65536) { + hm2->tp_pwmgen.pwmgen_master_rate_dds_reg = dds; + } + else{ + // not possible so try the fastest we can + // PWMFreq = (ClockHigh * DDS) / (65536 * 2048) + dds = 65535; + hm2->tp_pwmgen.hal->param.pwm_frequency = ((double)hm2->tp_pwmgen.clock_frequency * 65535.0) / (65536.0 * 2048.0); + HM2_ERR("max PWM frequency is %d Hz\n", hm2->tp_pwmgen.hal->param.pwm_frequency); + hm2->tp_pwmgen.pwmgen_master_rate_dds_reg = dds; + } + + // Now recalculate the deadtime. + // This is specified in nS in the pin but 2/TPPWMCLK in the register. + // = (2*65536*1e9 / ClockHigh*dds)nS + + for (i =0 ; i < hm2->tp_pwmgen.num_instances ;i++) { + if (hm2->tp_pwmgen.instance[i].hal.param.sampletime > 1){ + HM2_ERR("Max sampletime is 1 (end of PWM cycle"); + hm2->tp_pwmgen.instance[i].hal.param.sampletime = 1; + } + else if (hm2->tp_pwmgen.instance[i].hal.param.sampletime < 0){ + HM2_ERR("Min sampletime is 0 (beginning of PWM cycle"); + hm2->tp_pwmgen.instance[i].hal.param.sampletime = 0; + } + + deadtime = (hm2->tp_pwmgen.instance[i].hal.param.deadzone * hm2->tp_pwmgen.clock_frequency * dds) / (2 * 65536 * 1e9); + + if (deadtime > 511){ + deadtime = 511; + hm2->tp_pwmgen.instance[i].hal.param.deadzone = ((float)deadtime * 2.0 * 65536.0 * 1.0e9)/((float)hm2->tp_pwmgen.clock_frequency * dds); + HM2_ERR("At this PWM frequency the maximum deadtime is %dnS\n", (int)hm2->tp_pwmgen.instance[i].hal.param.deadzone); + } + else if (deadtime < 0) { + HM2_ERR("Deadtime must be positive"); + deadtime = 0; + hm2->tp_pwmgen.instance[i].hal.param.deadzone = 0; + } + + // Now setup the control register + + hm2->tp_pwmgen.setup_reg[i] = ( + ((int)(hm2->tp_pwmgen.instance[i].hal.param.sampletime*1023) << 16) + +(hm2->tp_pwmgen.instance[i].hal.param.faultpolarity << 15) + +(deadtime) + ); + } +} + + +void hm2_tp_pwmgen_force_write(hostmot2_t *hm2) { + int i; + + if (hm2->tp_pwmgen.num_instances == 0) return; + + hm2_tp_pwmgen_handle_pwm_frequency(hm2); // This sets the pwmgen_dds register and also the deadtime/sample time register + + // update enable and setup registers + + for (i = 0; i < hm2->tp_pwmgen.num_instances; i ++) { + if (*hm2->tp_pwmgen.instance[i].hal.pin.enable) { + hm2->tp_pwmgen.enable_reg[i] = 1; // It is possibly tricksy that this register is read/write + } + } + //write out the values to their registers + hm2->llio->write(hm2->llio, hm2->tp_pwmgen.setup_addr, hm2->tp_pwmgen.setup_reg, (hm2->tp_pwmgen.num_instances * sizeof(u32))); + hm2->llio->write(hm2->llio, hm2->tp_pwmgen.enable_addr, hm2->tp_pwmgen.enable_reg, (hm2->tp_pwmgen.num_instances * sizeof(u32))); + hm2->llio->write(hm2->llio, hm2->tp_pwmgen.pwmgen_master_rate_dds_addr, &hm2->tp_pwmgen.pwmgen_master_rate_dds_reg, sizeof(u32)); + + if ((*hm2->llio->io_error) != 0) return; + + for (i = 0; i < hm2->tp_pwmgen.num_instances; i ++) { + hm2->tp_pwmgen.instance[i].written_enable = *hm2->tp_pwmgen.instance[i].hal.pin.enable; + hm2->tp_pwmgen.instance[i].written_faultpolarity = hm2->tp_pwmgen.instance[i].hal.param.faultpolarity; + hm2->tp_pwmgen.instance[i].written_deadzone = hm2->tp_pwmgen.instance[i].hal.param.deadzone; + hm2->tp_pwmgen.instance[i].written_sampletime = hm2->tp_pwmgen.instance[i].hal.param.sampletime; + } + + hm2->tp_pwmgen.written_pwm_frequency = hm2->tp_pwmgen.hal->param.pwm_frequency; +} + + +// +// Update the Registers of all Three Phase pwmgen instances that need it +// + +void hm2_tp_pwmgen_write(hostmot2_t *hm2) { + int i; + + if (hm2->tp_pwmgen.num_instances == 0) return; + + // check pwm frequency + if (hm2->tp_pwmgen.hal->param.pwm_frequency != hm2->tp_pwmgen.written_pwm_frequency) goto force_write; + + // update enable register? + for (i = 0; i < hm2->tp_pwmgen.num_instances; i ++) { + if ( + *(hm2->tp_pwmgen.instance[i].hal.pin.enable) != hm2->tp_pwmgen.instance[i].written_enable + || (hm2->tp_pwmgen.instance[i].hal.param.deadzone != hm2->tp_pwmgen.instance[i].written_deadzone) + || (hm2->tp_pwmgen.instance[i].hal.param.sampletime != hm2->tp_pwmgen.instance[i].written_sampletime) + || (hm2->tp_pwmgen.instance[i].hal.param.faultpolarity != hm2->tp_pwmgen.instance[i].written_faultpolarity) + ) + { + goto force_write; + } + } + + return; + +force_write: + hm2_tp_pwmgen_force_write(hm2); +} + + + +int hm2_tp_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_or_complain(hm2, md_index, 0, 4, 4, 0x0003)) { + HM2_ERR("inconsistent Module Descriptor!\n"); + return -EINVAL; + } + + if (hm2->tp_pwmgen.num_instances != 0) { + HM2_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_tp_pwmgens > md->instances) { + HM2_ERR( + "config.num_pwmgens=%d, but only %d are available, not loading driver\n", + hm2->config.num_tp_pwmgens, + md->instances + ); + return -EINVAL; + } + + if (hm2->config.num_tp_pwmgens == 0) { + return 0; + } + + // + // looks good, start initializing + // + + + if (hm2->config.num_tp_pwmgens == -1) { + hm2->tp_pwmgen.num_instances = md->instances; + } else { + hm2->tp_pwmgen.num_instances = hm2->config.num_tp_pwmgens; + } + + + // allocate the module-global HAL shared memory + hm2->tp_pwmgen.hal = (hm2_tp_pwmgen_module_global_t *)hal_malloc(sizeof(hm2_tp_pwmgen_module_global_t)); + if (hm2->tp_pwmgen.hal == NULL) { + HM2_ERR("out of memory!\n"); + r = -ENOMEM; + goto fail0; + } + + hm2->tp_pwmgen.instance = (hm2_tp_pwmgen_instance_t *)hal_malloc(hm2->tp_pwmgen.num_instances * sizeof(hm2_tp_pwmgen_instance_t)); + if (hm2->tp_pwmgen.instance == NULL) { + HM2_ERR("out of memory!\n"); + r = -ENOMEM; + goto fail0; + } + + hm2->tp_pwmgen.clock_frequency = md->clock_freq; + hm2->tp_pwmgen.version = md->version; + + hm2->tp_pwmgen.pwm_value_addr = md->base_address + (0 * md->register_stride); + hm2->tp_pwmgen.pwmgen_master_rate_dds_addr = md->base_address + (3 * md->register_stride); + hm2->tp_pwmgen.enable_addr = md->base_address + (1 * md->register_stride); + hm2->tp_pwmgen.setup_addr = md->base_address + (2 * md->register_stride); + + //Allocate some memory for the parameter registers. The value equivalent is handled in the tram section + hm2->tp_pwmgen.setup_reg = (u32 *)kmalloc(hm2->tp_pwmgen.num_instances * sizeof(u32), GFP_KERNEL); + if (hm2->tp_pwmgen.setup_reg == NULL) { + HM2_ERR("out of memory!\n"); + r = -ENOMEM; + goto fail0; + } + hm2->tp_pwmgen.enable_reg = (u32 *)kmalloc(hm2->tp_pwmgen.num_instances * sizeof(u32), GFP_KERNEL); + if (hm2->tp_pwmgen.enable_reg == NULL) { + HM2_ERR("out of memory!\n"); + r = -ENOMEM; + goto fail0; + } + + // Register the PWM values with the TRAM + r = hm2_register_tram_write_region(hm2, hm2->tp_pwmgen.pwm_value_addr, (hm2->tp_pwmgen.num_instances * sizeof(u32)), &hm2->tp_pwmgen.pwm_value_reg); + if (r < 0) { + HM2_ERR("error registering tram write region for PWM Value register (%d)\n", r); + goto fail0; + } + + r = hm2_register_tram_read_region(hm2, hm2->tp_pwmgen.enable_addr, (hm2->tp_pwmgen.num_instances * sizeof(u32)), &hm2->tp_pwmgen.enable_reg); + if (r < 0) { + HM2_ERR("error registering tram read region for PWM Value register (%d)\n", r); + 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 3-Phase pwmgen instances + r = hal_param_u32_newf( + HAL_RW, + &(hm2->tp_pwmgen.hal->param.pwm_frequency), + hm2->llio->comp_id, + "%s.tp_pwmgen.pwm_frequency", + hm2->llio->name + ); + if (r < 0) { + HM2_ERR("error adding Three-Phase pwmgen.pwm_frequency param, aborting\n"); + goto fail1; + } + + hm2->tp_pwmgen.hal->param.pwm_frequency = 20000; + hm2->tp_pwmgen.written_pwm_frequency = 0; // force a write + + + for (i = 0; i < hm2->tp_pwmgen.num_instances; i ++) { + // pins + rtapi_snprintf(name, HAL_NAME_LEN, "%s.tp_pwmgen.%02d.A-value", hm2->llio->name, i); + r = hal_pin_float_new(name, HAL_IN, &(hm2->tp_pwmgen.instance[i].hal.pin.Avalue), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding pin '%s', aborting\n", name); + goto fail1; + } + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.tp_pwmgen.%02d.B-value", hm2->llio->name, i); + r = hal_pin_float_new(name, HAL_IN, &(hm2->tp_pwmgen.instance[i].hal.pin.Bvalue), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding pin '%s', aborting\n", name); + goto fail1; + } + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.tp_pwmgen.%02d.C-value", hm2->llio->name, i); + r = hal_pin_float_new(name, HAL_IN, &(hm2->tp_pwmgen.instance[i].hal.pin.Cvalue), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding pin '%s', aborting\n", name); + goto fail1; + } + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.tp_pwmgen.%02d.enable", hm2->llio->name, i); + r = hal_pin_bit_new(name, HAL_IN, &(hm2->tp_pwmgen.instance[i].hal.pin.enable), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding pin '%s', aborting\n", name); + goto fail1; + } + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.tp_pwmgen.%02d.fault", hm2->llio->name, i); + r = hal_pin_bit_new(name, HAL_OUT, &(hm2->tp_pwmgen.instance[i].hal.pin.fault), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding pin '%s', aborting\n", name); + goto fail1; + } + + // parameters + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.tp_pwmgen.%02d.scale", hm2->llio->name, i); + r = hal_param_float_new(name, HAL_RW, &(hm2->tp_pwmgen.instance[i].hal.param.scale), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding param '%s', aborting\n", name); + goto fail1; + } + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.tp_pwmgen.%02d.deadtime", hm2->llio->name, i); + r = hal_param_float_new(name, HAL_RW, &(hm2->tp_pwmgen.instance[i].hal.param.deadzone), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding param '%s', aborting\n", name); + goto fail1; + } + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.tp_pwmgen.%02d.fault-invert", hm2->llio->name, i); + r = hal_param_bit_new(name, HAL_RW, &(hm2->tp_pwmgen.instance[i].hal.param.faultpolarity), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding param '%s', aborting\n", name); + goto fail1; + } + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.tp_pwmgen.%02d.sample-time", hm2->llio->name, i); + r = hal_param_float_new(name, HAL_RW, &(hm2->tp_pwmgen.instance[i].hal.param.sampletime), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding param '%s', aborting\n", name); + goto fail1; + } + + + // init hal objects + *(hm2->tp_pwmgen.instance[i].hal.pin.enable) = 0; + *(hm2->tp_pwmgen.instance[i].hal.pin.Avalue) = 0.0; + *(hm2->tp_pwmgen.instance[i].hal.pin.Bvalue) = 0.0; + *(hm2->tp_pwmgen.instance[i].hal.pin.Cvalue) = 0.0; + + hm2->tp_pwmgen.instance[i].hal.param.scale = 1.0; + hm2->tp_pwmgen.instance[i].hal.param.faultpolarity = 0.0; //active low + hm2->tp_pwmgen.instance[i].hal.param.deadzone = 5000; // 5000nS conservative deadzone for safety + + hm2->tp_pwmgen.instance[i].written_enable = -666; // force an update at the start + } + } + + return hm2->tp_pwmgen.num_instances; + +fail1: + + kfree(hm2->tp_pwmgen.setup_reg); + kfree(hm2->tp_pwmgen.enable_reg); + +fail0: + hm2->tp_pwmgen.num_instances = 0; + return r; + +} + +void hm2_tp_pwmgen_print_module(hostmot2_t *hm2) { + int i; + HM2_PRINT("3-phase PWMGen: %d\n", hm2->tp_pwmgen.num_instances); + if (hm2->tp_pwmgen.num_instances <= 0) return; + HM2_PRINT(" clock_frequency: %d Hz (%s MHz)\n", hm2->tp_pwmgen.clock_frequency, hm2_hz_to_mhz(hm2->tp_pwmgen.clock_frequency)); + HM2_PRINT(" version: %d\n", hm2->tp_pwmgen.version); + HM2_PRINT(" pwmgen_master_rate_dds: 0x%08X (%d)\n", hm2->tp_pwmgen.pwmgen_master_rate_dds_reg, hm2->tp_pwmgen.pwmgen_master_rate_dds_reg); + HM2_PRINT(" pwmgen_master_rate_dds_addr: 0x%04X\n", hm2->tp_pwmgen.pwmgen_master_rate_dds_addr); + + for (i = 0; i < hm2->tp_pwmgen.num_instances; i ++) { + HM2_PRINT(" instance %d:\n", i); + HM2_PRINT(" hw:\n"); + HM2_PRINT(" pwm_value_addr: 0x%04X\n", hm2->tp_pwmgen.pwm_value_addr); + HM2_PRINT(" pwm_val:0x%08X\n", hm2->tp_pwmgen.pwm_value_reg[i]); + HM2_PRINT(" enable: 0x%08X\n", hm2->tp_pwmgen.enable_reg[i]); + HM2_PRINT(" setup: 0x%08X\n", hm2->tp_pwmgen.setup_reg[i]); + } +} + +void hm2_tp_pwmgen_process_tram_read(hostmot2_t *hm2){ + int i; + if (hm2->tp_pwmgen.num_instances <= 0) return; + + for (i = 0 ; i < hm2->tp_pwmgen.num_instances; i ++) { + *hm2->tp_pwmgen.instance[i].hal.pin.fault = (hm2->tp_pwmgen.enable_reg[i] & 2); + } + +} + + +void hm2_tp_pwmgen_prepare_tram_write(hostmot2_t *hm2) { + int i; + + if (hm2->tp_pwmgen.num_instances <= 0) return; + + for (i = 0; i < hm2->tp_pwmgen.num_instances; i ++) { + double scaled_Avalue; + double scaled_Bvalue; + double scaled_Cvalue; + + // 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->tp_pwmgen.instance[i].hal.pin.enable == 0) { + hm2->tp_pwmgen.pwm_value_reg[i] = 0; + continue; + } + + scaled_Avalue = fabs(*hm2->tp_pwmgen.instance[i].hal.pin.Avalue / hm2->tp_pwmgen.instance[i].hal.param.scale); + scaled_Bvalue = fabs(*hm2->tp_pwmgen.instance[i].hal.pin.Bvalue / hm2->tp_pwmgen.instance[i].hal.param.scale); + scaled_Cvalue = fabs(*hm2->tp_pwmgen.instance[i].hal.pin.Cvalue / hm2->tp_pwmgen.instance[i].hal.param.scale); + + if (scaled_Avalue > 1.0) scaled_Avalue = 1.0; + if (scaled_Bvalue > 1.0) scaled_Bvalue = 1.0; + if (scaled_Cvalue > 1.0) scaled_Cvalue = 1.0; + + // duty_cycle goes from 0.0 to 1.0, and needs to be puffed out to 10 bits + + hm2->tp_pwmgen.pwm_value_reg[i] = ( + ((long)(scaled_Cvalue * (511.0) + 512.0) << 20) + + ((long)(scaled_Bvalue * (511.0) + 512.0) << 10) + + ((long)(scaled_Avalue * (511.0) + 512.0) << 0) + ); + + } + +} + + +void hm2_tp_pwmgen_cleanup(hostmot2_t *hm2) { + if (hm2->tp_pwmgen.num_instances <= 0) return; + if (hm2->tp_pwmgen.setup_reg != NULL) { + kfree(hm2->tp_pwmgen.setup_reg); + hm2->tp_pwmgen.enable_reg = NULL; + } + if (hm2->tp_pwmgen.enable_reg != NULL) { + kfree(hm2->tp_pwmgen.enable_reg); + hm2->tp_pwmgen.enable_reg = NULL; + } + hm2->tp_pwmgen.num_instances = 0; +} +