int hm2_dpll_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, 7, 4, 0x0000)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
if (hm2->config.num_dplls == 0) return 0;
if (hm2->config.num_dplls > md->instances) {
hm2->dpll.num_instances = md->instances;
HM2_ERR( "There are only %d dplls on this board type, using %d\n",
md->instances, md->instances );
} else if (hm2->config.num_dplls == -1) {
hm2->dpll.num_instances = md->instances;
} else hm2->dpll.num_instances = hm2->config.num_dplls;
//
// looks good, start initializing
//
hm2->dpll.clock_frequency = md->clock_freq;
hm2->dpll.base_rate_addr = md->base_address + 0 * md->register_stride;
hm2->dpll.phase_err_addr = md->base_address + 1 * md->register_stride;
hm2->dpll.control_reg0_addr = md->base_address + 2 * md->register_stride;
hm2->dpll.control_reg1_addr = md->base_address + 3 * md->register_stride;
hm2->dpll.timer_12_addr = md->base_address + 4 * md->register_stride;
hm2->dpll.timer_34_addr = md->base_address + 5 * md->register_stride;
hm2->dpll.hm2_dpll_sync_addr = md->base_address + 6 * md->register_stride;
// export to HAL
hm2->dpll.pins = hal_malloc(sizeof(hm2_dpll_pins_t));
r = hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->time1_us),
hm2->llio->comp_id, "%s.dpll.01.timer-us", hm2->llio->name);
r += hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->time2_us),
hm2->llio->comp_id, "%s.dpll.02.timer-us", hm2->llio->name);
r += hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->time3_us),
hm2->llio->comp_id, "%s.dpll.03.timer-us", hm2->llio->name);
r += hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->time4_us),
hm2->llio->comp_id, "%s.dpll.04.timer-us", hm2->llio->name);
r += hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->base_freq),
hm2->llio->comp_id, "%s.dpll.base-freq-khz", hm2->llio->name);
r += hal_pin_float_newf(HAL_OUT, &(hm2->dpll.pins->phase_error),
hm2->llio->comp_id, "%s.dpll.phase-error-us", hm2->llio->name);
r += hal_pin_u32_newf(HAL_IN, &(hm2->dpll.pins->time_const),
hm2->llio->comp_id, "%s.dpll.time-const", hm2->llio->name);
r += hal_pin_u32_newf(HAL_IN, &(hm2->dpll.pins->plimit),
hm2->llio->comp_id, "%s.dpll.plimit", hm2->llio->name);
r += hal_pin_u32_newf(HAL_OUT, &(hm2->dpll.pins->ddssize),
hm2->llio->comp_id, "%s.dpll.ddsize", hm2->llio->name);
r += hal_pin_u32_newf(HAL_OUT, &(hm2->dpll.pins->prescale),
hm2->llio->comp_id, "%s.dpll.prescale", hm2->llio->name);
if (r < 0) {
HM2_ERR("error adding hm2_dpll timer pins, Aborting\n");
goto fail0;
}
*hm2->dpll.pins->time1_us = 100.0;
*hm2->dpll.pins->time2_us = 100.0;
*hm2->dpll.pins->time3_us = 100.0;
*hm2->dpll.pins->time4_us = 100.0;
*hm2->dpll.pins->prescale = 1;
*hm2->dpll.pins->base_freq = -1; // An indication it needs init
*hm2->dpll.pins->time_const = 0xA000;
*hm2->dpll.pins->plimit = 0x400000;
r = hm2_register_tram_read_region(hm2, hm2->dpll.hm2_dpll_sync_addr,
sizeof(u32), &hm2->dpll.hm2_dpll_sync_reg);
if (r < 0) {
HM2_ERR("Error registering tram synch write. Aborting\n");
goto fail0;
}
r = hm2_register_tram_read_region(hm2, hm2->dpll.control_reg1_addr,
sizeof(u32), &hm2->dpll.control_reg1_read);
if (r < 0) {
HM2_ERR("Error registering dpll control reg 1. Aborting\n");
goto fail0;
}
return hm2->dpll.num_instances;
fail0:
return r;
}
int hm2_resolver_parse_md(hostmot2_t *hm2, int md_index) {
hm2_module_descriptor_t *md = &hm2->md[md_index];
int i, r = 0;
int resolvers_per_instance;
//
// some standard sanity checks
//
if ( ! hm2_md_is_consistent_or_complain(hm2, md_index, 0, 5, 4, 0x001F)) {
HM2_ERR("inconsistent resolver Module Descriptor!\n");
return -EINVAL;
}
if (hm2->resolver.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;
}
resolvers_per_instance = hm2_resolver_get_param(2); // just returns 6 at the moment
if (hm2->config.num_resolvers > (md->instances * resolvers_per_instance)) {
HM2_ERR(
"config.num_resolvers=%d, but only %d are available, not loading driver\n",
hm2->config.num_resolvers,
md->instances * resolvers_per_instance);
return -EINVAL;
}
if (hm2->config.num_resolvers == 0) {
return 0;
}
//
// looks good, start initializing
//
/*At the moment it is not clear if there will ever be more than one resolver
instance. If there were to be more than one then they would need to have
a different base-address, and this code would need to be re-enterable.
A bridge to cross when we come to it */
if (hm2->config.num_resolvers == -1) {
hm2->resolver.num_resolvers = md->instances * resolvers_per_instance;
hm2->resolver.num_instances = md->instances;
} else {
hm2->resolver.num_resolvers = hm2->config.num_resolvers;
hm2->resolver.num_instances = md->instances;
}
hm2->resolver.hal = (hm2_resolver_global_t *)hal_malloc(
sizeof(hm2_resolver_global_t));
if (hm2->resolver.hal == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->resolver.instance = (hm2_resolver_instance_t *)hal_malloc(
hm2->resolver.num_resolvers * sizeof(hm2_resolver_instance_t));
if (hm2->resolver.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
for (i = 0 ; i < hm2->resolver.num_instances ; i++ ){
hm2->resolver.stride = md->register_stride;
hm2->resolver.clock_frequency = md->clock_freq;
hm2->resolver.version = md->version;
hm2->resolver.command_addr = md->base_address + (0 * md->register_stride);
hm2->resolver.data_addr = md->base_address + (1 * md->register_stride);
hm2->resolver.status_addr = md->base_address + (2 * md->register_stride);
hm2->resolver.velocity_addr = md->base_address + (3 * md->register_stride);
hm2->resolver.position_addr = md->base_address + (4 * md->register_stride);
// If there were multiple resolver function instances, this would need
// to be the number of resolvers for that particular instance
r = hm2_register_tram_read_region(hm2, hm2->resolver.status_addr,
sizeof(u32),
&hm2->resolver.status_reg);
r += hm2_register_tram_read_region(hm2, hm2->resolver.position_addr,
(hm2->resolver.num_resolvers * sizeof(u32)),
&hm2->resolver.position_reg);
r += hm2_register_tram_read_region(hm2, hm2->resolver.velocity_addr,
(hm2->resolver.num_resolvers * sizeof(u32)),
(u32**)&hm2->resolver.velocity_reg);
if (r < 0) {
HM2_ERR("error registering tram read region for Resolver "
"register (%d)\n", i);
goto fail1;
}
}
// export the resolvers to HAL
{
int i;
int ret;
char name[HAL_NAME_LEN + 1];
rtapi_snprintf(name, sizeof(name), "%s.resolver.excitation-khz",
hm2->llio->name);
ret= hal_pin_float_new(name, HAL_IO,
&(hm2->resolver.hal->pin.excitation_khz),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
for (i = 0; i < hm2->resolver.num_resolvers; i ++) {
// pins
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.position",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.position),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.angle",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.angle),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.velocity",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.velocity),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.count",
hm2->llio->name, i);
ret= hal_pin_s32_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.count),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.rawcounts",
hm2->llio->name, i);
ret= hal_pin_s32_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.rawcounts),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.reset",
hm2->llio->name, i);
ret= hal_pin_bit_new(name, HAL_IN,
&(hm2->resolver.instance[i].hal.pin.reset),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.index-enable",
hm2->llio->name, i);
ret= hal_pin_bit_new(name, HAL_IO,
&(hm2->resolver.instance[i].hal.pin.index_enable),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.error",
hm2->llio->name, i);
ret= hal_pin_bit_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.error),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
// parameters
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.scale",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_IO,
&(hm2->resolver.instance[i].hal.pin.scale),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.velocity-scale",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_IO,
&(hm2->resolver.instance[i].hal.pin.vel_scale),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.index-divisor",
hm2->llio->name, i);
ret= hal_pin_u32_new(name, HAL_RW,
&(hm2->resolver.instance[i].hal.pin.index_div),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
//
// init the hal objects that need it
// the things not initialized here will be set by hm2_resolver_tram_init()
//
*hm2->resolver.instance[i].hal.pin.reset = 0;
*hm2->resolver.instance[i].hal.pin.scale = 1.0;
*hm2->resolver.instance[i].hal.pin.vel_scale = 1.0;
*hm2->resolver.instance[i].hal.pin.index_div = 1;
*hm2->resolver.hal->pin.excitation_khz = -1; // don't-write
hm2->resolver.kHz = (hm2->resolver.clock_frequency / 5000);
}
}
return hm2->resolver.num_instances;
fail1:
// This is where we would kfree anything kmalloced.
fail0:
hm2->resolver.num_instances = 0;
return r;
}
int hm2_led_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, 1, 4, 0x0000)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
// LEDs were enumerated during llio setup
if (hm2->llio->num_leds == 0 || hm2->config.num_leds == 0) return 0;
if (hm2->config.num_leds > hm2->llio->num_leds) {
hm2->config.num_leds = hm2->llio->num_leds;
HM2_ERR( "There are only %d LEDs on this board type, defaulting to %d\n",
hm2->llio->num_leds, hm2->config.num_leds );
}
else if (hm2->config.num_leds == -1) {
hm2->config.num_leds = hm2->llio->num_leds;
}
//
// looks good, start initializing
//
// allocate the module-global HAL shared memory
hm2->led.instance = (hm2_led_instance_t *)hal_malloc(hm2->config.num_leds * sizeof(hm2_led_instance_t));
if (hm2->led.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->led.led_reg = (u32 *)kmalloc( sizeof(u32), GFP_KERNEL);
if (hm2->led.led_reg == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->led.led_addr = md->base_address;
// export to HAL
{
int i;
char name[HAL_NAME_LEN+1];
for (i = 0 ; i < hm2->config.num_leds ; i++) {
rtapi_snprintf(name, sizeof(name), "%s.led.CR%02d", hm2->llio->name, i + 1 );
r = hal_pin_bit_new(name, HAL_IN, &(hm2->led.instance[i].led), hm2->llio->comp_id);
if (r < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
}
return 1;
fail1:
kfree(hm2->led.led_reg);
fail0:
return r;
}
}
int hm2_bspi_parse_md(hostmot2_t *hm2, int md_index)
{
// All this function actually does is allocate memory
// and give the bspi modules names.
//
// some standard sanity checks
//
int i, j, r = -EINVAL;
hm2_module_descriptor_t *md = &hm2->md[md_index];
if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 3, 0x40, 0x0007)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
if (hm2->bspi.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_bspis > md->instances) {
HM2_ERR(
"config defines %d bspis, but only %d are available, "
"not loading driver\n",
hm2->config.num_bspis,
md->instances
);
return -EINVAL;
}
if (hm2->config.num_bspis == 0) {
return 0;
}
//
// looks good, start initializing
//
if (hm2->config.num_bspis == -1) {
hm2->bspi.num_instances = md->instances;
} else {
hm2->bspi.num_instances = hm2->config.num_bspis;
}
hm2->bspi.instance = (hm2_bspi_instance_t *)hal_malloc(hm2->bspi.num_instances
* sizeof(hm2_bspi_instance_t));
if (hm2->bspi.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
for (i = 0 ; i < hm2->bspi.num_instances ; i++){
hm2_bspi_instance_t *chan = &hm2->bspi.instance[i];
chan->clock_freq = md->clock_freq;
r = sprintf(chan->name, "%s.bspi.%01d", hm2->llio->name, i);
HM2_PRINT("created Buffered SPI function %s.\n", chan->name);
chan->base_address = md->base_address + i * md->instance_stride;
chan->register_stride = md->register_stride;
chan->instance_stride = md->instance_stride;
chan->cd_addr = md->base_address + md->register_stride + i * sizeof(rtapi_u32);
chan->count_addr = md->base_address + 2 * md->register_stride + i * sizeof(rtapi_u32);
for (j = 0 ; j < 16 ; j++ ){
chan->addr[j] = chan->base_address + j * sizeof(rtapi_u32);
}
}
return hm2->bspi.num_instances;
fail0:
return r;
}
int hm2_watchdog_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, 3, 4, 0)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
if (hm2->watchdog.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;
}
//
// special sanity checks for watchdog
//
if (md->instances != 1) {
HM2_PRINT("MD declares %d watchdogs! only using the first one...\n", md->instances);
}
//
// looks good, start initializing
//
hm2->watchdog.num_instances = 1;
hm2->watchdog.instance = (hm2_watchdog_instance_t *)hal_malloc(hm2->watchdog.num_instances * sizeof(hm2_watchdog_instance_t));
if (hm2->watchdog.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->watchdog.clock_frequency = md->clock_freq;
hm2->watchdog.version = md->version;
hm2->watchdog.timer_addr = md->base_address + (0 * md->register_stride);
hm2->watchdog.status_addr = md->base_address + (1 * md->register_stride);
hm2->watchdog.reset_addr = md->base_address + (2 * md->register_stride);
r = hm2_register_tram_read_region(hm2, hm2->watchdog.status_addr, (hm2->watchdog.num_instances * sizeof(u32)), &hm2->watchdog.status_reg);
if (r < 0) {
HM2_ERR("error registering tram read region for watchdog (%d)\n", r);
goto fail0;
}
r = hm2_register_tram_write_region(hm2, hm2->watchdog.reset_addr, sizeof(u32), &hm2->watchdog.reset_reg);
if (r < 0) {
HM2_ERR("error registering tram write region for watchdog (%d)!\n", r);
goto fail0;
}
//
// allocate memory for register buffers
//
hm2->watchdog.timer_reg = (u32 *)kmalloc(hm2->watchdog.num_instances * sizeof(u32), GFP_KERNEL);
if (hm2->watchdog.timer_reg == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
//
// export to HAL
//
// pins
r = hal_pin_bit_newf(
HAL_IO,
&(hm2->watchdog.instance[0].hal.pin.has_bit),
hm2->llio->comp_id,
"%s.watchdog.has_bit",
hm2->llio->name
);
if (r < 0) {
HM2_ERR("error adding pin, aborting\n");
r = -EINVAL;
goto fail1;
}
// params
r = hal_param_u32_newf(
HAL_RW,
&(hm2->watchdog.instance[0].hal.param.timeout_ns),
hm2->llio->comp_id,
"%s.watchdog.timeout_ns",
hm2->llio->name
);
if (r < 0) {
HM2_ERR("error adding param, aborting\n");
r = -EINVAL;
goto fail1;
}
// the function
{
hal_export_xfunct_args_t xfunct_args = {
.type = FS_XTHREADFUNC,
.funct.x = hm2_pet_watchdog,
.arg = hm2,
.uses_fp = 0,
.reentrant = 0,
.owner_id = hm2->llio->comp_id
};
if ((r = hal_export_xfunctf(&xfunct_args,
"%s.pet_watchdog",
hm2->llio->name)) != 0) {
HM2_ERR("hal_export pet_watchdog failed - %d\n", r);
r = -EINVAL;
goto fail1;
}
}
//
// initialize the watchdog
//
*hm2->watchdog.instance[0].hal.pin.has_bit = 0;
hm2->watchdog.instance[0].hal.param.timeout_ns = 5 * 1000 * 1000; // default timeout is 5 milliseconds
hm2->watchdog.instance[0].enable = 0; // the first pet_watchdog will turn it on
return hm2->watchdog.num_instances;
fail1:
kfree(hm2->watchdog.timer_reg);
fail0:
hm2->watchdog.num_instances = 0;
return r;
}
void hm2_watchdog_print_module(hostmot2_t *hm2) {
int i;
HM2_PRINT("Watchdog: %d\n", hm2->watchdog.num_instances);
if (hm2->watchdog.num_instances <= 0) return;
HM2_PRINT(" clock_frequency: %d Hz (%s MHz)\n", hm2->watchdog.clock_frequency, hm2_hz_to_mhz(hm2->watchdog.clock_frequency));
HM2_PRINT(" version: %d\n", hm2->watchdog.version);
HM2_PRINT(" timer_addr: 0x%04X\n", hm2->watchdog.timer_addr);
HM2_PRINT(" status_addr: 0x%04X\n", hm2->watchdog.status_addr);
HM2_PRINT(" reset_addr: 0x%04X\n", hm2->watchdog.reset_addr);
for (i = 0; i < hm2->watchdog.num_instances; i ++) {
HM2_PRINT(" instance %d:\n", i);
HM2_PRINT(" param timeout_ns = %u\n", hm2->watchdog.instance[i].hal.param.timeout_ns);
HM2_PRINT(" pin has_bit = %d\n", (*hm2->watchdog.instance[i].hal.pin.has_bit));
HM2_PRINT(" reg timer = 0x%08X\n", hm2->watchdog.timer_reg[i]);
}
}
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_or_complain(hm2, md_index, 0, 5, 4, 0x0003)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
if (hm2->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_pwmgens > md->instances) {
HM2_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) {
HM2_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) {
HM2_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(rtapi_u32)), &hm2->pwmgen.pwm_value_reg);
if (r < 0) {
HM2_ERR("error registering tram write region for PWM Value register (%d)\n", r);
goto fail0;
}
hm2->pwmgen.pwm_mode_reg = (rtapi_u32 *)rtapi_kmalloc(hm2->pwmgen.num_instances * sizeof(rtapi_u32), RTAPI_GFP_KERNEL);
if (hm2->pwmgen.pwm_mode_reg == NULL) {
HM2_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 + 1];
// 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 < 0) {
HM2_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 < 0) {
HM2_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, sizeof(name), "%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 < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%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 < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
// parameters
rtapi_snprintf(name, sizeof(name), "%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 < 0) {
HM2_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 < 0) {
HM2_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:
rtapi_kfree(hm2->pwmgen.pwm_mode_reg);
fail0:
hm2->pwmgen.num_instances = 0;
return r;
}
int hm2_uart_parse_md(hostmot2_t *hm2, int md_index)
{
// All this function actually does is allocate memory
// and give the uart modules names.
//
// some standard sanity checks
//
int i, r = -EINVAL;
hm2_module_descriptor_t *md = &hm2->md[md_index];
static int last_gtag = -1;
//The UART declares a TX and RX module separately
if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 4, 0x10, 0x000F)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
if (hm2->uart.num_instances > 1 && last_gtag == md->gtag) {
HM2_ERR(
"found duplicate Module Descriptor for %s (inconsistent "
"firmware), not loading driver %i %i\n",
hm2_get_general_function_name(md->gtag), md->gtag, last_gtag
);
return -EINVAL;
}
last_gtag = md->gtag;
if (hm2->config.num_uarts > md->instances) {
HM2_ERR(
"config defines %d uarts, but only %d are available, "
"not loading driver\n",
hm2->config.num_uarts,
md->instances
);
return -EINVAL;
}
if (hm2->config.num_uarts == 0) {
return 0;
}
//
// looks good, start, or continue, initializing
//
if (hm2->uart.num_instances == 0){
if (hm2->config.num_uarts == -1) {
hm2->uart.num_instances = md->instances;
} else {
hm2->uart.num_instances = hm2->config.num_uarts;
}
hm2->uart.instance = (hm2_uart_instance_t *)hal_malloc(hm2->uart.num_instances
* sizeof(hm2_uart_instance_t));
if (hm2->uart.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
}
for (i = 0 ; i < hm2->uart.num_instances ; i++){
hm2_uart_instance_t *inst = &hm2->uart.instance[i];
// For the time being we assume that all UARTS come on pairs
if (inst->clock_freq == 0){
inst->clock_freq = md->clock_freq;
r = sprintf(inst->name, "%s.uart.%01d", hm2->llio->name, i);
HM2_PRINT("created UART Interface function %s.\n", inst->name);
}
if (md->gtag == HM2_GTAG_UART_TX){
inst->tx1_addr = md->base_address + i * md->instance_stride;
inst->tx2_addr = md->base_address + i * md->instance_stride + 0x4;
inst->tx3_addr = md->base_address + i * md->instance_stride + 0x8;
inst->tx4_addr = md->base_address + i * md->instance_stride + 0xC;
inst->tx_fifo_count_addr = (md->base_address
+ md->register_stride
+ i * md->instance_stride);
inst->tx_bitrate_addr = (md->base_address
+ 2 * md->register_stride
+ i * md->instance_stride);
inst->tx_mode_addr = (md->base_address
+ 3 * md->register_stride
+i * md->instance_stride);
}
else if (md->gtag == HM2_GTAG_UART_RX){
inst->rx1_addr = md->base_address + i * md->instance_stride;
inst->rx2_addr = md->base_address + i * md->instance_stride + 0x4;
inst->rx3_addr = md->base_address + i * md->instance_stride + 0x8;
inst->rx4_addr = md->base_address + i * md->instance_stride + 0xC;
inst->rx_fifo_count_addr = (md->base_address
+ md->register_stride
+ i * md->instance_stride);
inst->rx_bitrate_addr = (md->base_address
+ 2 * md->register_stride
+ i * md->instance_stride);
inst->rx_mode_addr = (md->base_address
+ 3 * md->register_stride
+i * md->instance_stride);
}
else{
HM2_ERR("Something very wierd happened");
goto fail0;
}
}
return hm2->uart.num_instances;
fail0:
return r;
}
int hm2_watchdog_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, 3, 4, 0)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
if (hm2->watchdog.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;
}
//
// special sanity checks for watchdog
//
if (md->instances != 1) {
HM2_PRINT("MD declares %d watchdogs! only using the first one...\n", md->instances);
}
//
// looks good, start initializing
//
hm2->watchdog.num_instances = 1;
hm2->watchdog.instance = (hm2_watchdog_instance_t *)hal_malloc(hm2->watchdog.num_instances * sizeof(hm2_watchdog_instance_t));
if (hm2->watchdog.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->watchdog.clock_frequency = md->clock_freq;
hm2->watchdog.version = md->version;
hm2->watchdog.timer_addr = md->base_address + (0 * md->register_stride);
hm2->watchdog.status_addr = md->base_address + (1 * md->register_stride);
hm2->watchdog.reset_addr = md->base_address + (2 * md->register_stride);
//
// allocate memory for register buffers
//
hm2->watchdog.status_reg = (u32 *)kmalloc(hm2->watchdog.num_instances * sizeof(u32), GFP_KERNEL);
if (hm2->watchdog.status_reg == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->watchdog.reset_reg = (u32 *)kmalloc(hm2->watchdog.num_instances * sizeof(u32), GFP_KERNEL);
if (hm2->watchdog.reset_reg == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail1;
}
hm2->watchdog.timer_reg = (u32 *)kmalloc(hm2->watchdog.num_instances * sizeof(u32), GFP_KERNEL);
if (hm2->watchdog.timer_reg == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail2;
}
//
// export to HAL
//
// pins
r = hal_pin_bit_newf(
HAL_IO,
&(hm2->watchdog.instance[0].hal.pin.has_bit),
hm2->llio->comp_id,
"%s.watchdog.has_bit",
hm2->llio->name
);
if (r < 0) {
HM2_ERR("error adding pin, aborting\n");
r = -EINVAL;
goto fail3;
}
// params
r = hal_param_u32_newf(
HAL_RW,
&(hm2->watchdog.instance[0].hal.param.timeout_ns),
hm2->llio->comp_id,
"%s.watchdog.timeout_ns",
hm2->llio->name
);
if (r < 0) {
HM2_ERR("error adding param, aborting\n");
r = -EINVAL;
goto fail3;
}
// the function
{
char name[HAL_NAME_LEN + 1];
rtapi_snprintf(name, sizeof(name), "%s.pet_watchdog", hm2->llio->name);
r = hal_export_funct(name, hm2_pet_watchdog, hm2, 0, 0, hm2->llio->comp_id);
if (r != 0) {
HM2_ERR("error %d exporting pet_watchdog function %s\n", r, name);
r = -EINVAL;
goto fail3;
}
}
//
// initialize the watchdog
//
*hm2->watchdog.instance[0].hal.pin.has_bit = 0;
hm2->watchdog.instance[0].hal.param.timeout_ns = 5 * 1000 * 1000; // default timeout is 5 milliseconds
hm2->watchdog.instance[0].enable = 0; // the first pet_watchdog will turn it on
hm2->watchdog.instance[0].warned_about_short_timeout = 0;
hm2->watchdog.reset_reg[0] = 0x5a000000;
hm2->watchdog.status_reg[0] = 0;
return hm2->watchdog.num_instances;
fail3:
kfree(hm2->watchdog.timer_reg);
fail2:
kfree(hm2->watchdog.reset_reg);
fail1:
kfree(hm2->watchdog.status_reg);
fail0:
hm2->watchdog.num_instances = 0;
return r;
}
