/**
* 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");
}
