int hm2_tram_add_bspi_frame(char *name, int chan, rtapi_u32 **wbuff, rtapi_u32 **rbuff) { hostmot2_t *hm2; int i, r; i = hm2_get_bspi(&hm2, name); if (i < 0){ HM2_ERR_NO_LL("Can not find BSPI instance %s.\n", name); return -1; } if (hm2->bspi.instance[i].conf_flag[chan] != true){ HM2_ERR("The selected write channel (%i) on bspi instance %s.\n" "Has not been configured.\n", chan, name); return -1; } if (wbuff != NULL) { r = hm2_register_tram_write_region(hm2,hm2->bspi.instance[i].addr[chan], sizeof(rtapi_u32),wbuff); if (r < 0) { HM2_ERR("Failed to add TRAM write entry for %s.\n", name); return -1; } } else { HM2_ERR("SPI frame must have a write entry for channel (%i) on %s.\n", chan, name); return -1; } if (rbuff != NULL){ // Don't add a read entry for a no-echo channel if(!(hm2->bspi.instance[i].cd[chan] & 0x80000000)) { r = hm2_register_tram_read_region(hm2,hm2->bspi.instance[i].addr[0], sizeof(rtapi_u32),rbuff); if (r < 0) { HM2_ERR( "Failed to add TRAM read entry for %s\n", name); return -1; } } } return 0; }
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_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]); } }