/** * Print a little info about the driver. */ int info(enum LIS3MDL_BUS busid) { struct lis3mdl_bus_option &bus = find_bus(busid); warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath); bus.dev->print_info(); exit(0); }
/** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled * and automatic modes. */ void test(enum QMC5883_BUS busid) { struct qmc5883_bus_option &bus = find_bus(busid); struct mag_report report; ssize_t sz; int ret; const char *path = bus.devpath; int fd = open(path, O_RDONLY); if (fd < 0) { err(1, "%s open failed (try 'qmc5883 start')", path); } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { err(1, "immediate read failed"); } print_message(report); /* check if mag is onboard or external */ if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) { errx(1, "failed to get if mag is onboard or external"); } warnx("device active: %s", ret ? "external" : "onboard"); /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { struct pollfd fds; /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; ret = poll(&fds, 1, 2000); if (ret != 1) { errx(1, "timed out waiting for sensor data"); } /* now go get it */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { err(1, "periodic read failed"); } print_message(report); } PX4_INFO("PASS"); exit(0); }
/** * Print a little info about the driver. */ int info(enum IST8310_BUS busid) { struct ist8310_bus_option &bus = find_bus(busid); PX4_INFO("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath); bus.dev->print_info(); exit(0); }
/** * Print a little info about the driver. */ int info(enum QMC5883_BUS busid) { struct qmc5883_bus_option &bus = find_bus(busid); warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath); bus.dev->print_info(); exit(0); }
/** * deliberately produce an error to test recovery */ void testerror(enum MPU9250_BUS busid) { struct mpu9250_bus_option &bus = find_bus(busid); if (bus.dev == nullptr) { errx(1, "driver not running"); } bus.dev->test_error(); exit(0); }
/** * Print a little info about the driver. */ void info(enum MPU9250_BUS busid) { struct mpu9250_bus_option &bus = find_bus(busid); if (bus.dev == nullptr) { errx(1, "driver not running"); } printf("state @ %p\n", bus.dev); bus.dev->print_info(); exit(0); }
/** * Dump the register information */ void regdump(enum MPU9250_BUS busid) { struct mpu9250_bus_option &bus = find_bus(busid); if (bus.dev == nullptr) { errx(1, "driver not running"); } printf("regdump @ %p\n", bus.dev); bus.dev->print_registers(); exit(0); }
/** * Calculate actual MSL pressure given current altitude */ void calibrate(unsigned altitude, enum LPS25H_BUS busid) { struct lps25h_bus_option &bus = find_bus(busid); const char *path = bus.devpath; int fd = open(path, O_RDONLY); if (fd < 0) { err(1, "%s open failed (try 'lps25h start' if the driver is not running", path); } // TODO: Implement calibration close(fd); }
void stop(enum MPU9250_BUS busid) { struct mpu9250_bus_option &bus = find_bus(busid); if (bus.dev != nullptr) { delete bus.dev; bus.dev = nullptr; } else { /* warn, but not an error */ warnx("already stopped."); } exit(0); }
/** * enable/disable temperature compensation */ int temp_enable(enum HMC5883_BUS busid, bool enable) { struct hmc5883_bus_option &bus = find_bus(busid); const char *path = bus.devpath; int fd = open(path, O_RDONLY); if (fd < 0) err(1, "failed "); if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) err(1, "set temperature compensation failed"); close(fd); return 0; }
static struct scsi_device * edd_find_matching_scsi_device(struct edd_device *edev) { struct edd_match_data data; struct bus_type * scsi_bus = find_bus("scsi"); if (!scsi_bus) { return NULL; } data.edev = edev; if (edd_dev_is_type(edev, "SCSI")) { if (bus_for_each_dev(scsi_bus,NULL,&data,edd_match_scsidev)) return data.sd; } return NULL; }
/** * Reset the driver. */ void reset(enum HMC5883_BUS busid) { struct hmc5883_bus_option &bus = find_bus(busid); const char *path = bus.devpath; int fd = open(path, O_RDONLY); if (fd < 0) err(1, "failed "); if (ioctl(fd, SENSORIOCRESET, 0) < 0) err(1, "driver reset failed"); if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); exit(0); }
/** * Automatic scale calibration. * * Basic idea: * * output = (ext field +- 1.1 Ga self-test) * scale factor * * and consequently: * * 1.1 Ga = (excited - normal) * scale factor * scale factor = (excited - normal) / 1.1 Ga * * sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3 * sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3 * * By subtracting the non-excited measurement the pure 1.1 Ga reading * can be extracted and the sensitivity of all axes can be matched. * * SELF TEST OPERATION * To check the HMC5883L for proper operation, a self test feature in incorporated * in which the sensor offset straps are excited to create a nominal field strength * (bias field) to be measured. To implement self test, the least significant bits * (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias) * or 10 (negetive bias), e.g. 0x11 or 0x12. * Then, by placing the mode register into single-measurement mode (0x01), * two data acquisition cycles will be made on each magnetic vector. * The first acquisition will be a set pulse followed shortly by measurement * data of the external field. The second acquisition will have the offset strap * excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create * about a ±1.1 gauss self test field plus the external field. The first acquisition * values will be subtracted from the second acquisition, and the net measurement * will be placed into the data output registers. * Since self test adds ~1.1 Gauss additional field to the existing field strength, * using a reduced gain setting prevents sensor from being saturated and data registers * overflowed. For example, if the configuration register B is set to 0x60 (Gain=3), * values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data * output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data * output register. To leave the self test mode, change MS1 and MS0 bit of the * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10. * Using the self test method described above, the user can scale sensor */ int calibrate(enum HMC5883_BUS busid) { int ret; struct hmc5883_bus_option &bus = find_bus(busid); const char *path = bus.devpath; int fd = open(path, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path); if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { warnx("failed to enable sensor calibration mode"); } close(fd); return ret; }
/** * Automatic scale calibration. * * Basic idea: * * output = (ext field +- 1.1 Ga self-test) * scale factor * * and consequently: * * 1.1 Ga = (excited - normal) * scale factor * scale factor = (excited - normal) / 1.1 Ga * * sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3 * sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3 * * By subtracting the non-excited measurement the pure 1.1 Ga reading * can be extracted and the sensitivity of all axes can be matched. * * SELF TEST OPERATION * To check the IST8310L for proper operation, a self test feature in incorporated * in which the sensor will change the polarity on all 3 axis. The values with and * with and without selftest on shoult be compared and if the absolete value are equal * the IC is functional. */ int calibrate(enum IST8310_BUS busid) { int ret; struct ist8310_bus_option &bus = find_bus(busid); const char *path = bus.devpath; int fd = open(path, O_RDONLY); if (fd < 0) { err(1, "%s open failed (try 'ist8310 start' if the driver is not running", path); } if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { PX4_WARN("failed to enable sensor calibration mode"); } close(fd); return ret; }
/** * Reset the driver. */ void reset(enum MPU9250_BUS busid) { struct mpu9250_bus_option &bus = find_bus(busid); int fd = open(bus.accelpath, O_RDONLY); if (fd < 0) { err(1, "failed "); } if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); } close(fd); exit(0); }
/** * Reset the driver. */ void reset(enum IST8310_BUS busid) { struct ist8310_bus_option &bus = find_bus(busid); const char *path = bus.devpath; int fd = open(path, O_RDONLY); if (fd < 0) { err(1, "failed "); } if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); } // Relay on at_)exit to close handel exit(0); }
/** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled * and automatic modes. */ void test(enum LIS3MDL_BUS busid) { struct lis3mdl_bus_option &bus = find_bus(busid); struct mag_report report; ssize_t sz; int ret; const char *path = bus.devpath; int fd = open(path, O_RDONLY); if (fd < 0) { err(1, "%s open failed (try 'lis3mdl start')", path); } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { err(1, "immediate read failed"); } print_message(report); /* check if mag is onboard or external */ if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) { errx(1, "failed to get if mag is onboard or external"); } /* set the queue depth to 5 */ if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { errx(1, "failed to set queue depth"); } /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { struct pollfd fds; /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; ret = poll(&fds, 1, 2000); if (ret != 1) { errx(1, "timed out waiting for sensor data"); } /* now go get it */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { err(1, "periodic read failed"); } print_message(report); } errx(0, "PASS"); }
static bus_t * add_bus(struct search_args *args, di_node_t node, di_minor_t minor, controller_t *cp) { char *btype; char *devpath; bus_t *bp; char kstat_name[MAXPATHLEN]; di_node_t pnode; if (node == DI_NODE_NIL) { return (NULL); } if ((btype = bus_type(node, minor, args->ph)) == NULL) { return (add_bus(args, di_parent_node(node), di_minor_next(di_parent_node(node), NULL), cp)); } devpath = di_devfs_path(node); if ((bp = find_bus(args, devpath)) != NULL) { di_devfs_path_free((void *) devpath); if (cp != NULL) { if (add_ptr2array(cp, (void ***)&bp->controllers) != 0) { args->dev_walk_status = ENOMEM; return (NULL); } } return (bp); } /* Special handling for root node. */ if (strcmp(devpath, "/") == 0) { di_devfs_path_free((void *) devpath); return (NULL); } if (dm_debug) { (void) fprintf(stderr, "INFO: add_bus %s\n", devpath); } bp = (bus_t *)calloc(1, sizeof (bus_t)); if (bp == NULL) { return (NULL); } bp->name = strdup(devpath); di_devfs_path_free((void *) devpath); if (bp->name == NULL) { args->dev_walk_status = ENOMEM; cache_free_bus(bp); return (NULL); } bp->btype = strdup(btype); if (bp->btype == NULL) { args->dev_walk_status = ENOMEM; cache_free_bus(bp); return (NULL); } (void) snprintf(kstat_name, sizeof (kstat_name), "%s%d", di_node_name(node), di_instance(node)); if ((bp->kstat_name = strdup(kstat_name)) == NULL) { args->dev_walk_status = ENOMEM; cache_free_bus(bp); return (NULL); } /* if parent node is a bus, get its name */ if ((pnode = get_parent_bus(node, args)) != NULL) { devpath = di_devfs_path(pnode); bp->pname = strdup(devpath); di_devfs_path_free((void *) devpath); if (bp->pname == NULL) { args->dev_walk_status = ENOMEM; cache_free_bus(bp); return (NULL); } } else { bp->pname = NULL; } bp->freq = get_prom_int("clock-frequency", node, args->ph); bp->controllers = (controller_t **)calloc(1, sizeof (controller_t *)); if (bp->controllers == NULL) { args->dev_walk_status = ENOMEM; cache_free_bus(bp); return (NULL); } bp->controllers[0] = NULL; if (cp != NULL) { if (add_ptr2array(cp, (void ***)&bp->controllers) != 0) { args->dev_walk_status = ENOMEM; return (NULL); } } bp->next = args->bus_listp; args->bus_listp = bp; return (bp); }
/** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled * and automatic modes. */ void test(enum MPU9250_BUS busid) { struct mpu9250_bus_option &bus = find_bus(busid); accel_report a_report; gyro_report g_report; mag_report m_report; ssize_t sz; /* get the driver */ int fd = open(bus.accelpath, O_RDONLY); if (fd < 0) { err(1, "%s open failed (try 'm start')", bus.accelpath); } /* get the driver */ int fd_gyro = open(bus.gyropath, O_RDONLY); if (fd_gyro < 0) { err(1, "%s open failed", bus.gyropath); } /* get the driver */ int fd_mag = open(bus.magpath, O_RDONLY); if (fd_mag < 0) { err(1, "%s open failed", bus.magpath); } /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { err(1, "reset to manual polling"); } /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); if (sz != sizeof(a_report)) { warnx("ret: %d, expected: %d", sz, sizeof(a_report)); err(1, "immediate acc read failed"); } warnx("single read"); warnx("time: %lld", a_report.timestamp); warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x); warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y); warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z); warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw); warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, (double)(a_report.range_m_s2 / MPU9250_ONE_G)); /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); if (sz != sizeof(g_report)) { warnx("ret: %d, expected: %d", sz, sizeof(g_report)); err(1, "immediate gyro read failed"); } warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z); warnx("gyro x: \t%d\traw", (int)g_report.x_raw); warnx("gyro y: \t%d\traw", (int)g_report.y_raw); warnx("gyro z: \t%d\traw", (int)g_report.z_raw); warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); /* do a simple demand read */ sz = read(fd_mag, &m_report, sizeof(m_report)); if (sz != sizeof(m_report)) { warnx("ret: %d, expected: %d", sz, sizeof(m_report)); err(1, "immediate mag read failed"); } warnx("mag x: \t% 9.5f\trad/s", (double)m_report.x); warnx("mag y: \t% 9.5f\trad/s", (double)m_report.y); warnx("mag z: \t% 9.5f\trad/s", (double)m_report.z); warnx("mag x: \t%d\traw", (int)m_report.x_raw); warnx("mag y: \t%d\traw", (int)m_report.y_raw); warnx("mag z: \t%d\traw", (int)m_report.z_raw); warnx("mag range: %8.4f Ga", (double)m_report.range_ga); warnx("mag temp: %8.4f\tdeg celsius", (double)m_report.temperature); /* reset to default polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "reset to default polling"); } close(fd); close(fd_gyro); close(fd_mag); /* XXX add poll-rate tests here too */ reset(busid); errx(0, "PASS"); }
/** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled * and automatic modes. */ void test(enum LPS25H_BUS busid) { struct lps25h_bus_option &bus = find_bus(busid); struct baro_report report; ssize_t sz; int ret; int fd; fd = open(bus.devpath, O_RDONLY); if (fd < 0) { err(1, "open failed (try 'lps25h start' if the driver is not running)"); } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { err(1, "immediate read failed"); } warnx("single read"); warnx("pressure: %10.4f", (double)report.pressure); warnx("altitude: %11.4f", (double)report.altitude); warnx("temperature: %8.4f", (double)report.temperature); warnx("time: %lld", report.timestamp); /* set the queue depth to 10 */ if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { errx(1, "failed to set queue depth"); } /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { struct pollfd fds; /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; ret = poll(&fds, 1, 2000); if (ret != 1) { errx(1, "timed out waiting for sensor data"); } /* now go get it */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { err(1, "periodic read failed"); } warnx("periodic read %u", i); warnx("pressure: %10.4f", (double)report.pressure); warnx("altitude: %11.4f", (double)report.altitude); warnx("temperature K: %8.4f", (double)report.temperature); warnx("time: %lld", report.timestamp); } close(fd); errx(0, "PASS"); }
/** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled * and automatic modes. */ void test(enum HMC5883_BUS busid) { struct hmc5883_bus_option &bus = find_bus(busid); struct mag_report report; ssize_t sz; int ret; const char *path = bus.devpath; int fd = open(path, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'hmc5883 start')", path); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) err(1, "immediate read failed"); warnx("single read"); warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); warnx("time: %lld", report.timestamp); /* check if mag is onboard or external */ if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) errx(1, "failed to get if mag is onboard or external"); warnx("device active: %s", ret ? "external" : "onboard"); /* set the queue depth to 5 */ if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) errx(1, "failed to set queue depth"); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) errx(1, "failed to set 2Hz poll rate"); /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { struct pollfd fds; /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; ret = poll(&fds, 1, 2000); if (ret != 1) errx(1, "timed out waiting for sensor data"); /* now go get it */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) err(1, "periodic read failed"); warnx("periodic read %u", i); warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); warnx("time: %lld", report.timestamp); } errx(0, "PASS"); }
static uint32_t reg_script_read_res(struct reg_script_context *ctx) { struct resource *res; uint32_t val = 0; const struct reg_script *step = reg_script_get_step(ctx); res = reg_script_get_resource(ctx); if (res == NULL) return val; if (res->flags & IORESOURCE_IO) { const struct reg_script io_step = { .size = step->size, .reg = res->base + step->reg, }; reg_script_set_step(ctx, &io_step); val = reg_script_read_io(ctx); } else if (res->flags & IORESOURCE_MEM) { const struct reg_script mmio_step = { .size = step->size, .reg = res->base + step->reg, }; reg_script_set_step(ctx, &mmio_step); val = reg_script_read_mmio(ctx); } reg_script_set_step(ctx, step); return val; } static void reg_script_write_res(struct reg_script_context *ctx) { struct resource *res; const struct reg_script *step = reg_script_get_step(ctx); res = reg_script_get_resource(ctx); if (res == NULL) return; if (res->flags & IORESOURCE_IO) { const struct reg_script io_step = { .size = step->size, .reg = res->base + step->reg, .value = step->value, }; reg_script_set_step(ctx, &io_step); reg_script_write_io(ctx); } else if (res->flags & IORESOURCE_MEM) { const struct reg_script mmio_step = { .size = step->size, .reg = res->base + step->reg, .value = step->value, }; reg_script_set_step(ctx, &mmio_step); reg_script_write_mmio(ctx); } reg_script_set_step(ctx, step); } #if HAS_IOSF static uint32_t reg_script_read_iosf(struct reg_script_context *ctx) { const struct reg_script *step = reg_script_get_step(ctx); switch (step->id) { case IOSF_PORT_AUNIT: return iosf_aunit_read(step->reg); case IOSF_PORT_CPU_BUS: return iosf_cpu_bus_read(step->reg); case IOSF_PORT_BUNIT: return iosf_bunit_read(step->reg); case IOSF_PORT_DUNIT_CH0: return iosf_dunit_ch0_read(step->reg); case IOSF_PORT_PMC: return iosf_punit_read(step->reg); case IOSF_PORT_USBPHY: return iosf_usbphy_read(step->reg); case IOSF_PORT_SEC: return iosf_sec_read(step->reg); case IOSF_PORT_0x45: return iosf_port45_read(step->reg); case IOSF_PORT_0x46: return iosf_port46_read(step->reg); case IOSF_PORT_0x47: return iosf_port47_read(step->reg); case IOSF_PORT_SCORE: return iosf_score_read(step->reg); case IOSF_PORT_0x55: return iosf_port55_read(step->reg); case IOSF_PORT_0x58: return iosf_port58_read(step->reg); case IOSF_PORT_0x59: return iosf_port59_read(step->reg); case IOSF_PORT_0x5a: return iosf_port5a_read(step->reg); case IOSF_PORT_USHPHY: return iosf_ushphy_read(step->reg); case IOSF_PORT_SCC: return iosf_scc_read(step->reg); case IOSF_PORT_LPSS: return iosf_lpss_read(step->reg); case IOSF_PORT_0xa2: return iosf_porta2_read(step->reg); case IOSF_PORT_CCU: return iosf_ccu_read(step->reg); case IOSF_PORT_SSUS: return iosf_ssus_read(step->reg); default: printk(BIOS_DEBUG, "No read support for IOSF port 0x%x.\n", step->id); break; } return 0; } static void reg_script_write_iosf(struct reg_script_context *ctx) { const struct reg_script *step = reg_script_get_step(ctx); switch (step->id) { case IOSF_PORT_AUNIT: iosf_aunit_write(step->reg, step->value); break; case IOSF_PORT_CPU_BUS: iosf_cpu_bus_write(step->reg, step->value); break; case IOSF_PORT_BUNIT: iosf_bunit_write(step->reg, step->value); break; case IOSF_PORT_DUNIT_CH0: iosf_dunit_write(step->reg, step->value); break; case IOSF_PORT_PMC: iosf_punit_write(step->reg, step->value); break; case IOSF_PORT_USBPHY: iosf_usbphy_write(step->reg, step->value); break; case IOSF_PORT_SEC: iosf_sec_write(step->reg, step->value); break; case IOSF_PORT_0x45: iosf_port45_write(step->reg, step->value); break; case IOSF_PORT_0x46: iosf_port46_write(step->reg, step->value); break; case IOSF_PORT_0x47: iosf_port47_write(step->reg, step->value); break; case IOSF_PORT_SCORE: iosf_score_write(step->reg, step->value); break; case IOSF_PORT_0x55: iosf_port55_write(step->reg, step->value); break; case IOSF_PORT_0x58: iosf_port58_write(step->reg, step->value); break; case IOSF_PORT_0x59: iosf_port59_write(step->reg, step->value); break; case IOSF_PORT_0x5a: iosf_port5a_write(step->reg, step->value); break; case IOSF_PORT_USHPHY: iosf_ushphy_write(step->reg, step->value); break; case IOSF_PORT_SCC: iosf_scc_write(step->reg, step->value); break; case IOSF_PORT_LPSS: iosf_lpss_write(step->reg, step->value); break; case IOSF_PORT_0xa2: iosf_porta2_write(step->reg, step->value); break; case IOSF_PORT_CCU: iosf_ccu_write(step->reg, step->value); break; case IOSF_PORT_SSUS: iosf_ssus_write(step->reg, step->value); break; default: printk(BIOS_DEBUG, "No write support for IOSF port 0x%x.\n", step->id); break; } } #endif /* HAS_IOSF */ static uint64_t reg_script_read_msr(struct reg_script_context *ctx) { #if CONFIG_ARCH_X86 const struct reg_script *step = reg_script_get_step(ctx); msr_t msr = rdmsr(step->reg); uint64_t value = msr.hi; value = msr.hi; value <<= 32; value |= msr.lo; return value; #endif } static void reg_script_write_msr(struct reg_script_context *ctx) { #if CONFIG_ARCH_X86 const struct reg_script *step = reg_script_get_step(ctx); msr_t msr; msr.hi = step->value >> 32; msr.lo = step->value & 0xffffffff; wrmsr(step->reg, msr); #endif } #ifndef __PRE_RAM__ /* Default routine provided for systems without platform specific busses */ const struct reg_script_bus_entry *__attribute__((weak)) platform_bus_table(size_t *table_entries) { /* No platform bus type table supplied */ *table_entries = 0; return NULL; } /* Locate the structure containing the platform specific bus access routines */ static const struct reg_script_bus_entry *find_bus(const struct reg_script *step) { const struct reg_script_bus_entry *bus; size_t table_entries; size_t i; /* Locate the platform specific bus */ bus = platform_bus_table(&table_entries); for (i = 0; i < table_entries; i++) { if (bus[i].type == step->type) return &bus[i]; } /* Bus not found */ return NULL; } #endif static uint64_t reg_script_read(struct reg_script_context *ctx) { const struct reg_script *step = reg_script_get_step(ctx); switch (step->type) { case REG_SCRIPT_TYPE_PCI: return reg_script_read_pci(ctx); case REG_SCRIPT_TYPE_IO: return reg_script_read_io(ctx); case REG_SCRIPT_TYPE_MMIO: return reg_script_read_mmio(ctx); case REG_SCRIPT_TYPE_RES: return reg_script_read_res(ctx); case REG_SCRIPT_TYPE_MSR: return reg_script_read_msr(ctx); #if HAS_IOSF case REG_SCRIPT_TYPE_IOSF: return reg_script_read_iosf(ctx); #endif /* HAS_IOSF */ default: #ifndef __PRE_RAM__ { const struct reg_script_bus_entry *bus; /* Read from the platform specific bus */ bus = find_bus(step); if (NULL != bus) return bus->reg_script_read(ctx); } #endif printk(BIOS_ERR, "Unsupported read type (0x%x) for this device!\n", step->type); break; } return 0; } static void reg_script_write(struct reg_script_context *ctx) { const struct reg_script *step = reg_script_get_step(ctx); switch (step->type) { case REG_SCRIPT_TYPE_PCI: reg_script_write_pci(ctx); break; case REG_SCRIPT_TYPE_IO: reg_script_write_io(ctx); break; case REG_SCRIPT_TYPE_MMIO: reg_script_write_mmio(ctx); break; case REG_SCRIPT_TYPE_RES: reg_script_write_res(ctx); break; case REG_SCRIPT_TYPE_MSR: reg_script_write_msr(ctx); break; #if HAS_IOSF case REG_SCRIPT_TYPE_IOSF: reg_script_write_iosf(ctx); break; #endif /* HAS_IOSF */ default: #ifndef __PRE_RAM__ { const struct reg_script_bus_entry *bus; /* Write to the platform specific bus */ bus = find_bus(step); if (NULL != bus) { bus->reg_script_write(ctx); return; } } #endif printk(BIOS_ERR, "Unsupported write type (0x%x) for this device!\n", step->type); break; } } static void reg_script_rmw(struct reg_script_context *ctx) { uint64_t value; const struct reg_script *step = reg_script_get_step(ctx); struct reg_script write_step = *step; value = reg_script_read(ctx); value &= step->mask; value |= step->value; write_step.value = value; reg_script_set_step(ctx, &write_step); reg_script_write(ctx); reg_script_set_step(ctx, step); }
/** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled * and automatic modes. */ void test(enum MPU9250_BUS busid) { struct mpu9250_bus_option &bus = find_bus(busid); accel_report a_report; gyro_report g_report; mag_report m_report; ssize_t sz; /* get the driver */ int fd = open(bus.accelpath, O_RDONLY); if (fd < 0) { err(1, "%s open failed (try 'm start')", bus.accelpath); } /* get the driver */ int fd_gyro = open(bus.gyropath, O_RDONLY); if (fd_gyro < 0) { err(1, "%s open failed", bus.gyropath); } /* get the driver */ int fd_mag = open(bus.magpath, O_RDONLY); if (fd_mag < 0) { err(1, "%s open failed", bus.magpath); } /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { err(1, "reset to manual polling"); } /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); if (sz != sizeof(a_report)) { warnx("ret: %d, expected: %d", sz, sizeof(a_report)); err(1, "immediate acc read failed"); } print_message(a_report); /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); if (sz != sizeof(g_report)) { warnx("ret: %d, expected: %d", sz, sizeof(g_report)); err(1, "immediate gyro read failed"); } print_message(g_report); /* do a simple demand read */ sz = read(fd_mag, &m_report, sizeof(m_report)); if (sz != sizeof(m_report)) { warnx("ret: %d, expected: %d", sz, sizeof(m_report)); err(1, "immediate mag read failed"); } print_message(m_report); /* reset to default polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "reset to default polling"); } close(fd); close(fd_gyro); close(fd_mag); /* XXX add poll-rate tests here too */ reset(busid); errx(0, "PASS"); }