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; } } }