static void flash_make_cmd(flash_info_t * info, uint8_t cmd, void *cmdbuf)
{
int32_t i;
#if defined(__LITTLE_ENDIAN)
uint16_t stmpw;
#endif
uint8_t *cp = (uint8_t *) cmdbuf;
if (info->chipwidth < FLASH_CFI_BY32) {
for (i = 0; i < info->portwidth; i++)
*cp++ = ((i + 1) % info->chipwidth) ? '\0' : cmd;
} else {
uint16_t *ccp = (uint16_t *) cmdbuf;
uint16_t cmd_16 = cmd + cmd * 256;
for (i = 0; i < info->portwidth; i = i + 2)
*ccp++ = ((i + 2) % info->chipwidth) ? '\0' : cmd_16;
}
#if defined(__LITTLE_ENDIAN)
switch (info->portwidth) {
case FLASH_CFI_8BIT:
break;
case FLASH_CFI_16BIT:
stmpw = *(uint16_t *) cmdbuf;
*(uint16_t *) cmdbuf = __swab16(stmpw);
break;
default:
printf("WARNING: flash_make_cmd: unsuppported LittleEndian mode\n");
break;
}
#endif
}
int memory_display(const void *addr, loff_t offs, unsigned nbytes, int size, int swab)
{
ulong linebytes, i;
u_char *cp;
/* Print the lines.
*
* We buffer all read data, so we can make sure data is read only
* once, and all accesses are with the specified bus width.
*/
do {
char linebuf[DISP_LINE_LEN];
uint32_t *uip = (uint *)linebuf;
uint16_t *usp = (ushort *)linebuf;
uint8_t *ucp = (u_char *)linebuf;
unsigned count = 52;
printf("%08llx:", offs);
linebytes = (nbytes > DISP_LINE_LEN) ? DISP_LINE_LEN : nbytes;
for (i = 0; i < linebytes; i += size) {
if (size == 4) {
u32 res;
res = (*uip++ = *((uint *)addr));
if (swab)
res = __swab32(res);
count -= printf(" %08x", res);
} else if (size == 2) {
u16 res;
res = (*usp++ = *((ushort *)addr));
if (swab)
res = __swab16(res);
count -= printf(" %04x", res);
} else {
count -= printf(" %02x", (*ucp++ = *((u_char *)addr)));
}
addr += size;
offs += size;
}
while (count--)
putchar(' ');
cp = (uint8_t *)linebuf;
for (i = 0; i < linebytes; i++) {
if ((*cp < 0x20) || (*cp > 0x7e))
putchar('.');
else
printf("%c", *cp);
cp++;
}
putchar('\n');
nbytes -= linebytes;
if (ctrlc())
return -EINTR;
} while (nbytes > 0);
return 0;
}
int tmp75_read_value(int fd, int addr, __u8 reg){
if( ioctl(fd, I2C_SLAVE, addr) < 0 ){
printf("Failed to configure the device; %s\n", strerror(errno));
return EXIT_FAILURE;
}
if(reg == TMP75_REG_CONFIG)
return i2c_smbus_read_byte_data(fd, reg);
else
return __swab16(i2c_smbus_read_word_data(fd, reg));
}
int tmp75_write_value(int fd, int addr, __u8 reg, __u16 value){
if( ioctl(fd, I2C_SLAVE, addr) < 0 ){
printf("Failed to configure the device; %s\n", strerror(errno));
return EXIT_FAILURE;
}
if(reg == TMP75_REG_CONFIG){
__u8 tmp = (__u8) value;
return i2c_smbus_write_byte_data(fd, reg, tmp);
}
else
return i2c_smbus_write_word_data(fd, reg, __swab16(value));
}
void IMUReader::readSensors(IMUSensorsData& data, GyroState& gyroState,
bool calibration)
{
short x, y, z;
unsigned char buf[16];
selectDevice(file, HMC5883L_I2C_ADDR, "HMC5883L");
writeToDevice(file, "\x03", 1);
if (read(file, buf, 6) != 6)
{
printf("Unable to read from HMC5883L\n");
}
else
{
x = __swab16(*(short*) &buf[0]);
z = __swab16(*(short*) &buf[2]);
y = __swab16(*(short*) &buf[4]);
data.rawCompassX = x;
data.rawCompassY = y;
data.rawCompassZ = z;
}
selectDevice(file, ADXL345_I2C_ADDR, "ADXL345");
writeToDevice(file, "\x32", 1);
if (read(file, buf, 6) != 6)
{
printf("Unable to read from ADXL345\n");
}
else
{
x = buf[1] << 8 | buf[0];
y = buf[3] << 8 | buf[2];
z = buf[5] << 8 | buf[4];
data.rawAccelX = x;
data.rawAccelY = y;
data.rawAccelZ = z;
}
selectDevice(file, ITG3200_I2C_ADDR, "ITG3200");
writeToDevice(file, "\x1D", 1);
if (read(file, buf, 6) != 6)
{
printf("Unable to read from ITG3200\n");
}
else
{
x = __swab16(*(short*) &buf[0]);
y = __swab16(*(short*) &buf[2]);
z = __swab16(*(short*) &buf[4]);
data.rawGyroX = x;
data.rawGyroY = y;
data.rawGyroZ = z;
if (calibration)
{
gyroState.offsetX += x;
gyroState.offsetY += y;
gyroState.offsetZ += z;
}
}
}
