diff --git a/src/main/drivers/1-wire.c b/src/main/drivers/1-wire.c index 12af640637a..a0c04e634d0 100644 --- a/src/main/drivers/1-wire.c +++ b/src/main/drivers/1-wire.c @@ -1,4 +1,6 @@ +#include + #include "drivers/1-wire.h" #include "drivers/1-wire/ds2482.h" @@ -8,19 +10,22 @@ #ifdef USE_1WIRE #ifdef USE_1WIRE_DS2482 -bool ds2482_detected = false; -_1WireDev_t ds2482Dev; +static bool ds2482Detected = false; +static owDev_t ds2482Dev; #endif -void _1WireInit(void) +void owInit(void) { + memset(&ds2482Dev, 0, sizeof(ds2482Dev)); addBootlogEvent2(BOOT_EVENT_TEMP_SENSOR_DETECTION, BOOT_EVENT_FLAGS_NONE); #ifdef USE_1WIRE_DS2482 - if (ds2482Detect(&ds2482Dev)) { - ds2482_detected = true; - ds2482Init(&ds2482Dev); - } + if (ds2482Detect(&ds2482Dev)) ds2482Detected = true; #endif } +owDev_t *getOwDev(void) +{ + return ds2482Detected ? &ds2482Dev : NULL; +} + #endif /* USE_1WIRE */ diff --git a/src/main/drivers/1-wire.h b/src/main/drivers/1-wire.h index df054db7304..3726e04b2be 100644 --- a/src/main/drivers/1-wire.h +++ b/src/main/drivers/1-wire.h @@ -5,15 +5,53 @@ #ifdef USE_1WIRE -typedef struct _1WireDev_s { +#define OW_TRIPLET_FIRST_BIT(tripletResult) (tripletResult & (1 << 0)) +#define OW_TRIPLET_SECOND_BIT(tripletResult) (tripletResult & (1 << 1)) +#define OW_TRIPLET_DIRECTION_BIT(tripletResult) (tripletResult & (1 << 2)) + +#define OW_SINGLE_BIT_WRITE0 0 +#define OW_SINGLE_BIT_WRITE1_READ (1<<7) + +typedef struct owDev_s { busDevice_t *busDev; -} _1WireDev_t; -#if defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) -extern bool ds2482_detected; -extern _1WireDev_t ds2482Dev; -#endif + uint8_t status; + + bool (*reset)(struct owDev_s *owDev); + bool (*owResetCommand)(struct owDev_s *owDev); + bool (*owReset)(struct owDev_s *owDev); + bool (*waitForBus)(struct owDev_s *owDev); + bool (*readConfig)(struct owDev_s *owDev, uint8_t *config); + bool (*writeConfig)(struct owDev_s *owDev, uint8_t config); + bool (*readStatus)(struct owDev_s *owDev, uint8_t *status); + uint8_t (*getStatus)(struct owDev_s *owDev); + bool (*poll)(struct owDev_s *owDev, bool waitForBus, uint8_t *status); + bool (*owBusReady)(struct owDev_s *owDev); + + // 1-Wire ROM + bool (*owSearchRom)(struct owDev_s *owDev, uint8_t familyCode, uint64_t *romTable, uint8_t *romTableLen); + bool (*owMatchRomCommand)(struct owDev_s *owDev); + bool (*owMatchRom)(struct owDev_s *owDev, uint64_t rom); + bool (*owSkipRomCommand)(struct owDev_s *owDev); + bool (*owSkipRom)(struct owDev_s *owDev); + + // 1-Wire read/write + bool (*owWriteByteCommand)(struct owDev_s *owDev, uint8_t byte); + bool (*owWriteByte)(struct owDev_s *owDev, uint8_t byte); + bool (*owWriteBuf)(struct owDev_s *owDev, const uint8_t *buf, uint8_t len); + bool (*owReadByteCommand)(struct owDev_s *owDev); + bool (*owReadByteResult)(struct owDev_s *owDev, uint8_t *result); + bool (*owReadByte)(struct owDev_s *owDev, uint8_t *result); + bool (*owReadBuf)(struct owDev_s *owDev, uint8_t *buf, uint8_t len); + bool (*owSingleBitCommand)(struct owDev_s *owDev, uint8_t type); + bool (*owSingleBitResult)(struct owDev_s *owDev); + bool (*owSingleBit)(struct owDev_s *owDev, uint8_t type, bool *result); + bool (*owTripletCommand)(struct owDev_s *owDev, uint8_t direction); + uint8_t (*owTripletResult)(struct owDev_s *owDev); + bool (*owTriplet)(struct owDev_s *owDev, uint8_t direction, uint8_t *result); +} owDev_t; -void _1WireInit(void); +void owInit(void); +owDev_t *getOwDev(void); #endif /* defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) */ diff --git a/src/main/drivers/1-wire/ds2482.c b/src/main/drivers/1-wire/ds2482.c index e1c4b60a83f..f4fc6a3d5f4 100644 --- a/src/main/drivers/1-wire/ds2482.c +++ b/src/main/drivers/1-wire/ds2482.c @@ -32,6 +32,28 @@ #define DS2482_READ_DATA_REG_ADDR 0xE1 #define DS2482_CONFIG_REG_ADDR 0xC3 +#define DS2482_CONFIG_REG_APU (1<<0) // Active pull-up +#define DS2482_CONFIG_REG_SPU (1<<2) // Strong pull-up +#define DS2482_CONFIG_REG_WS (1<<3) // 1-Wire speed + +#define DS2482_STATUS_REG_1WB_POS 0 // 1-Wire busy +#define DS2482_STATUS_REG_PPD_POS 1 // Presense-pulse detect +#define DS2482_STATUS_REG_SD_POS 2 // Short detected +#define DS2482_STATUS_REG_LL_POS 3 // Logic level +#define DS2482_STATUS_REG_RST_POS 4 // Device reset +#define DS2482_STATUS_REG_SBR_POS 5 // Single bit result +#define DS2482_STATUS_REG_TSB_POS 6 // Triplet second bit +#define DS2482_STATUS_REG_DIR_POS 7 // Branch direction taken + +#define DS2482_1WIRE_BUSY(status) (status & (1 << DS2482_STATUS_REG_1WB_POS)) +#define DS2482_DEVICE_PRESENT(status) (status & (1 << DS2482_STATUS_REG_PPD_POS)) // True if a device have been detected on the bus after a bus reset +#define DS2482_SBR_VALUE(status) ((status >> DS2482_STATUS_REG_SBR_POS) & 1) // Extract single bit read value or triplet first bit from status register value +#define DS2482_TSB_VALUE(status) ((status >> DS2482_STATUS_REG_TSB_POS) & 1) // Extract triplet second bit value from status register value +#define DS2482_DIR_VALUE(status) ((status >> DS2482_STATUS_REG_DIR_POS) & 1) // Extract triplet chosen direction bit value from status register value + +#define DS2482_1WIRE_SINGLE_BIT_WRITE0 0 +#define DS2482_1WIRE_SINGLE_BIT_WRITE1_READ (1<<7) + #define DS2482_CONFIG_WRITE_BYTE(config) (config | ((~config & 0xF) << 4)) // Config's upper nibble should be the one's complement of lower nibble when writing #define DS2482_RESET_CMD 0xF0 @@ -49,172 +71,237 @@ #define _1WIRE_SKIP_ROM_CMD 0xCC - #if defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) -bool ds2482_reset(_1WireDev_t *_1WireDev) + +static bool ds2482Reset(owDev_t *owDev) { - return busWrite(_1WireDev->busDev, 0xFF, DS2482_RESET_CMD); + return busWrite(owDev->busDev, 0xFF, DS2482_RESET_CMD); } -static bool ds2482_set_read_ptr(_1WireDev_t *_1WireDev, uint8_t reg) +static bool ds2482SetReadPtr(owDev_t *owDev, uint8_t reg) { - return busWrite(_1WireDev->busDev, DS2482_SET_READ_PTR_CMD, reg); + return busWrite(owDev->busDev, DS2482_SET_READ_PTR_CMD, reg); } -static bool ds2482_read_reg(_1WireDev_t *_1WireDev, uint8_t reg, uint8_t *byte) +static bool ds2482ReadReg(owDev_t *owDev, uint8_t reg, uint8_t *byte) { - bool ack = ds2482_set_read_ptr(_1WireDev, reg); + bool ack = ds2482SetReadPtr(owDev, reg); if (!ack) return false; - return busRead(_1WireDev->busDev, 0xFF, byte); + return busRead(owDev->busDev, 0xFF, byte); +} + +static bool ds2482ReadByte(owDev_t *owDev, uint8_t *byte) +{ + return ds2482ReadReg(owDev, DS2482_READ_DATA_REG_ADDR, byte); } -bool ds2482_read_byte(_1WireDev_t *_1WireDev, uint8_t *byte) +static bool ds2482ReadConfig(owDev_t *owDev, uint8_t *config) { - return ds2482_read_reg(_1WireDev, DS2482_READ_DATA_REG_ADDR, byte); + return ds2482ReadReg(owDev, DS2482_CONFIG_REG_ADDR, config); } -bool ds2482_config(_1WireDev_t *_1WireDev, uint8_t *config) +static bool ds2482WriteConfig(owDev_t *owDev, uint8_t config) { - return ds2482_read_reg(_1WireDev, DS2482_CONFIG_REG_ADDR, config); + return busWrite(owDev->busDev, DS2482_WRITE_CONFIG_CMD, DS2482_CONFIG_WRITE_BYTE(config)); } -bool ds2482_status(_1WireDev_t *_1WireDev, uint8_t *status) +static bool ds2482ReadStatus(owDev_t *owDev, uint8_t *status) { - return ds2482_read_reg(_1WireDev, DS2482_STATUS_REG_ADDR, status); + bool ack = ds2482ReadReg(owDev, DS2482_STATUS_REG_ADDR, &owDev->status); + if (!ack) return false; + *status = owDev->status; + return true; } -bool ds2482_write_config(_1WireDev_t *_1WireDev, uint8_t config) +static uint8_t ds2482GetStatus(owDev_t *owDev) { - return busWrite(_1WireDev->busDev, DS2482_WRITE_CONFIG_CMD, DS2482_CONFIG_WRITE_BYTE(config)); + return owDev->status; } -bool ds2482_poll(_1WireDev_t *_1WireDev, bool wait_for_bus, uint8_t *status) +static bool ds2482Poll(owDev_t *owDev, bool waitForBus, uint8_t *status) { - uint8_t status_temp; do { - bool ack = busRead(_1WireDev->busDev, 0xFF, &status_temp); + bool ack = busRead(owDev->busDev, 0xFF, &owDev->status); if (!ack) return false; - } while (wait_for_bus && DS2482_1WIRE_BUSY(status_temp)); - if (status) *status = status_temp; + } while (waitForBus && DS2482_1WIRE_BUSY(owDev->status)); + if (status) *status = owDev->status; return true; } -bool ds2482_wait_for_bus(_1WireDev_t *_1WireDev) +static bool ds2482WaitForBus(owDev_t *owDev) { - return ds2482_poll(_1WireDev, true, NULL); + return ds2482Poll(owDev, true, NULL); } -bool ds2482_1wire_reset(_1WireDev_t *_1WireDev) +static bool ds2482OwBusReady(owDev_t *owDev) { - return busWrite(_1WireDev->busDev, 0xFF, DS2482_1WIRE_RESET_CMD); + bool ack = busRead(owDev->busDev, 0xFF, &owDev->status); + if (!ack) return false; + return !DS2482_1WIRE_BUSY(owDev->status); +} + +static bool ds2482OwResetCommand(owDev_t *owDev) +{ + return busWrite(owDev->busDev, 0xFF, DS2482_1WIRE_RESET_CMD); } -bool ds2482_1wire_reset_wait(_1WireDev_t *_1WireDev) +static bool ds2482OwReset(owDev_t *owDev) { - bool ack = ds2482_1wire_reset(_1WireDev); + bool ack = ds2482OwResetCommand(owDev); if (!ack) return false; - return ds2482_wait_for_bus(_1WireDev); + return ds2482WaitForBus(owDev); } -bool ds2482_1wire_write_byte(_1WireDev_t *_1WireDev, uint8_t byte) +static bool ds2482OwWriteByteCommand(owDev_t *owDev, uint8_t byte) { - return busWrite(_1WireDev->busDev, DS2482_1WIRE_WRITE_BYTE_CMD, byte); + return busWrite(owDev->busDev, DS2482_1WIRE_WRITE_BYTE_CMD, byte); } -bool ds2482_1wire_write_byte_wait(_1WireDev_t *_1WireDev, uint8_t byte) +static bool ds2482OwWriteByte(owDev_t *owDev, uint8_t byte) { - bool ack = ds2482_1wire_write_byte(_1WireDev, byte); + bool ack = ds2482OwWriteByteCommand(owDev, byte); if (!ack) return false; - return ds2482_wait_for_bus(_1WireDev); + return ds2482WaitForBus(owDev); } -bool ds2482_1wire_write_buf(_1WireDev_t *_1WireDev, const uint8_t *buf, uint8_t len) +static bool ds2482OwWriteBuf(owDev_t *owDev, const uint8_t *buf, uint8_t len) { for (uint8_t index = 0; index < len; ++index) { - bool ack = ds2482_1wire_write_byte_wait(_1WireDev, buf[index]); + bool ack = ds2482OwWriteByte(owDev, buf[index]); if (!ack) return false; } return true; } -bool ds2482_1wire_read_byte(_1WireDev_t *_1WireDev) +static bool ds2482OwReadByteCommand(owDev_t *owDev) +{ + return busWrite(owDev->busDev, 0xFF, DS2482_1WIRE_READ_BYTE_CMD); +} + +static bool ds2482OwReadByte(owDev_t *owDev, uint8_t *result) { - return busWrite(_1WireDev->busDev, 0xFF, DS2482_1WIRE_READ_BYTE_CMD); + bool ack = ds2482OwReadByteCommand(owDev); + if (!ack) return false; + + ack = ds2482WaitForBus(owDev); + if (!ack) return false; + + return ds2482ReadByte(owDev, result); } -bool ds2482_1wire_single_bit(_1WireDev_t *_1WireDev, uint8_t type) +static bool ds2482OwReadBuf(owDev_t *owDev, uint8_t *buf, uint8_t len) { - return busWrite(_1WireDev->busDev, DS2482_1WIRE_SINGLE_BIT_CMD, type); + for (uint8_t index = 0; index < len; ++index) { + bool ack = ds2482OwReadByte(owDev, buf + index); + if (!ack) return false; + } + return true; } -bool ds2482_1wire_triplet(_1WireDev_t *_1WireDev, uint8_t direction) +static bool ds2482OwSingleBitCommand(owDev_t *owDev, uint8_t type) { - return busWrite(_1WireDev->busDev, DS2482_1WIRE_TRIPLET_CMD, direction << 7); + return busWrite(owDev->busDev, DS2482_1WIRE_SINGLE_BIT_CMD, type); } -bool ds2482_1wire_triplet_wait(_1WireDev_t *_1WireDev, uint8_t direction) +static bool ds2482OwSingleBitResult(owDev_t *owDev) { - bool ack = ds2482_1wire_triplet(_1WireDev, direction << 7); + return DS2482_SBR_VALUE(owDev->status); +} + +static bool ds2482OwSingleBit(owDev_t *owDev, uint8_t type, bool *result) +{ + bool ack = ds2482OwSingleBitCommand(owDev, type); + + ack = ds2482WaitForBus(owDev); if (!ack) return false; - return ds2482_wait_for_bus(_1WireDev); + + if (result) *result = ds2482OwSingleBitResult(owDev); + return true; +} + +static bool ds2482OwTripletCommand(owDev_t *owDev, uint8_t direction) +{ + return busWrite(owDev->busDev, DS2482_1WIRE_TRIPLET_CMD, direction << 7); } -bool ds2482_1wire_skip_rom(_1WireDev_t *_1WireDev) +static uint8_t ds2482OwTripletResult(owDev_t *owDev) { - return ds2482_1wire_write_byte(_1WireDev, _1WIRE_SKIP_ROM_CMD); + return owDev->status >> 5; } -bool ds2482_1wire_skip_rom_wait(_1WireDev_t *_1WireDev) +static bool ds2482OwTriplet(owDev_t *owDev, uint8_t direction, uint8_t *result) { - bool ack = ds2482_1wire_write_byte(_1WireDev, _1WIRE_SKIP_ROM_CMD); + bool ack = ds2482OwTripletCommand(owDev, direction << 7); if (!ack) return false; - return ds2482_wait_for_bus(_1WireDev); + ack = ds2482Poll(owDev, true, NULL); + if (!ack) return false; + *result = ds2482OwTripletResult(owDev); + return true; } -bool ds2482_1wire_match_rom(_1WireDev_t *_1WireDev, uint64_t rom) +static bool ds2482OwSkipRomCommand(owDev_t *owDev) { - bool ack = ds2482_1wire_write_byte_wait(_1WireDev, _1WIRE_MATCH_ROM_CMD); + return ds2482OwWriteByte(owDev, _1WIRE_SKIP_ROM_CMD); +} + +static bool ds2482OwSkipRom(owDev_t *owDev) +{ + bool ack = ds2482OwReset(owDev); if (!ack) return false; - for (uint8_t rom_byte_index = 0; rom_byte_index < 8; ++rom_byte_index) { - ack = ds2482_1wire_write_byte_wait(_1WireDev, ((uint8_t *)&rom)[rom_byte_index]); - if (!ack) return false; - } + ack = ds2482OwSkipRomCommand(owDev); + if (!ack) return false; - return true; + return ds2482WaitForBus(owDev); } -bool ds2482_1wire_reset_and_match_rom(_1WireDev_t *_1WireDev, uint64_t rom) +static bool ds2482OwMatchRomCommand(owDev_t *owDev) { - bool ack = ds2482_1wire_reset_wait(_1WireDev); + return ds2482OwWriteByte(owDev, _1WIRE_MATCH_ROM_CMD); +} + +static bool ds2482OwMatchRom(owDev_t *owDev, uint64_t rom) +{ + bool ack = ds2482OwReset(owDev); + if (!ack) return false; + + ack = ds2482OwMatchRomCommand(owDev); + if (!ack) return false; + + ack = ds2482WaitForBus(owDev); if (!ack) return false; - return ds2482_1wire_match_rom(_1WireDev, rom); + + for (uint8_t romByteIndex = 0; romByteIndex < 8; ++romByteIndex) { + ack = ds2482OwWriteByte(owDev, ((uint8_t *)&rom)[romByteIndex]); + if (!ack) return false; + } + + return true; } -bool ds2482_1wire_search_rom(_1WireDev_t *_1WireDev, uint8_t family_code, uint64_t *rom_table, uint8_t *rom_table_len) +static bool ds2482OwSearchRom(owDev_t *owDev, uint8_t family_code, uint64_t *rom_table, uint8_t *rom_table_len) { bool ack; uint8_t last_collision_index = 0, rom_index = 0; do { - uint8_t status; uint8_t rom_byte_index = 0, rom_bit_index = 1, rom_byte_mask = 1; uint8_t dir_zero_last_index = 0; // Bit index where the 0 direction has been chosen after collision uint8_t *rom = (uint8_t *)&rom_table[rom_index]; - ack = ds2482_1wire_reset(_1WireDev); - if (!ack) goto ds2482_search_rom_return; + ack = ds2482OwReset(owDev); + if (!ack) goto ds2482SearchRomReturn; - ack = ds2482_poll(_1WireDev, true, &status); - if (!ack) goto ds2482_search_rom_return; + ack = ds2482Poll(owDev, true, NULL); + if (!ack) goto ds2482SearchRomReturn; - if (!DS2482_DEVICE_PRESENT(status)) - goto ds2482_search_rom_return; + if (!DS2482_DEVICE_PRESENT(owDev->status)) + goto ds2482SearchRomReturn; - ack = ds2482_1wire_write_byte_wait(_1WireDev, _1WIRE_SEARCH_ROM_CMD); - if (!ack) goto ds2482_search_rom_return; + ack = ds2482OwWriteByte(owDev, _1WIRE_SEARCH_ROM_CMD); + if (!ack) goto ds2482SearchRomReturn; do { @@ -228,20 +315,20 @@ bool ds2482_1wire_search_rom(_1WireDev_t *_1WireDev, uint8_t family_code, uint64 direction = rom_bit_index == last_collision_index; } - ack = ds2482_1wire_triplet(_1WireDev, direction); - if (!ack) goto ds2482_search_rom_return; + ack = ds2482OwTripletCommand(owDev, direction); + if (!ack) goto ds2482SearchRomReturn; - ack = ds2482_poll(_1WireDev, true, &status); - if (!ack) goto ds2482_search_rom_return; + ack = ds2482Poll(owDev, true, NULL); + if (!ack) goto ds2482SearchRomReturn; - uint8_t triplet_sbr = DS2482_SBR_VALUE(status); - uint8_t triplet_tsb = DS2482_TSB_VALUE(status); - uint8_t triplet_dir = DS2482_DIR_VALUE(status); + uint8_t triplet_sbr = DS2482_SBR_VALUE(owDev->status); + uint8_t triplet_tsb = DS2482_TSB_VALUE(owDev->status); + uint8_t triplet_dir = DS2482_DIR_VALUE(owDev->status); if (triplet_sbr && triplet_tsb) break; // Error, the device have been disconnected during the search, restart if (family_code && (rom_bit_index < 9) && (triplet_dir != direction)) - goto ds2482_search_rom_return; + goto ds2482SearchRomReturn; if (triplet_dir) rom[rom_byte_index] |= rom_byte_mask; @@ -267,7 +354,7 @@ bool ds2482_1wire_search_rom(_1WireDev_t *_1WireDev, uint8_t family_code, uint64 } while (rom_index < *rom_table_len); -ds2482_search_rom_return: +ds2482SearchRomReturn: *rom_table_len = rom_index; @@ -275,43 +362,69 @@ bool ds2482_1wire_search_rom(_1WireDev_t *_1WireDev, uint8_t family_code, uint64 } -bool ds2482_1wire_reset_and_skip_rom(_1WireDev_t *_1WireDev) + +static bool ds2482Init(owDev_t *owDev) { - bool ack = ds2482_1wire_reset_wait(_1WireDev); - if (!ack) return false; - return ds2482_1wire_skip_rom_wait(_1WireDev); + return ds2482WriteConfig(owDev, DS2482_CONFIG_REG_APU); } - #define DETECTION_MAX_RETRY_COUNT 5 -static bool deviceDetect(_1WireDev_t *_1WireDev) +static bool deviceDetect(owDev_t *owDev) { for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) { delay(10); - if (ds2482_reset(_1WireDev)) return true; + if (ds2482Reset(owDev)) return true; } return false; } -bool ds2482Detect(_1WireDev_t *_1WireDev) +bool ds2482Detect(owDev_t *owDev) { - _1WireDev->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_DS2482, 0, OWNER_1WIRE); - if (_1WireDev->busDev == NULL) { + owDev->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_DS2482, 0, OWNER_1WIRE); + if (owDev->busDev == NULL) { return false; } - if (!deviceDetect(_1WireDev)) { - busDeviceDeInit(_1WireDev->busDev); + if (!deviceDetect(owDev)) { + busDeviceDeInit(owDev->busDev); return false; } - return true; -} + ds2482Init(owDev); + + owDev->reset = ds2482Reset; + owDev->owResetCommand = ds2482OwResetCommand; + owDev->owReset = ds2482OwReset; + owDev->waitForBus = ds2482WaitForBus; + owDev->readConfig = ds2482ReadConfig; + owDev->writeConfig = ds2482WriteConfig; + owDev->readStatus = ds2482ReadStatus; + owDev->getStatus = ds2482GetStatus; + owDev->poll = ds2482Poll; + owDev->owBusReady = ds2482OwBusReady; + + owDev->owSearchRom = ds2482OwSearchRom; + owDev->owMatchRomCommand = ds2482OwMatchRomCommand; + owDev->owMatchRom = ds2482OwMatchRom; + owDev->owSkipRomCommand = ds2482OwSkipRomCommand; + owDev->owSkipRom = ds2482OwSkipRom; + + owDev->owWriteByteCommand = ds2482OwWriteByteCommand; + owDev->owWriteByte = ds2482OwWriteByte; + owDev->owWriteBuf = ds2482OwWriteBuf; + owDev->owReadByteCommand = ds2482OwReadByteCommand; + owDev->owReadByteResult = ds2482ReadByte; + owDev->owReadByte = ds2482OwReadByte; + owDev->owReadBuf = ds2482OwReadBuf; + owDev->owSingleBitCommand = ds2482OwSingleBitCommand; + owDev->owSingleBitResult = ds2482OwSingleBitResult; + owDev->owSingleBit = ds2482OwSingleBit; + owDev->owTripletCommand = ds2482OwTripletCommand; + owDev->owTripletResult = ds2482OwTripletResult; + owDev->owTriplet = ds2482OwTriplet; -bool ds2482Init(_1WireDev_t *_1WireDev) -{ - return ds2482_write_config(_1WireDev, DS2482_CONFIG_REG_APU); + return true; } #endif /* defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) */ diff --git a/src/main/drivers/1-wire/ds2482.h b/src/main/drivers/1-wire/ds2482.h index 999587a71e0..ad4d6177b2a 100644 --- a/src/main/drivers/1-wire/ds2482.h +++ b/src/main/drivers/1-wire/ds2482.h @@ -23,57 +23,6 @@ #if defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) -#define DS2482_CONFIG_REG_APU (1<<0) // Active pull-up -#define DS2482_CONFIG_REG_SPU (1<<2) // Strong pull-up -#define DS2482_CONFIG_REG_WS (1<<3) // 1-Wire speed - -#define DS2482_STATUS_REG_1WB_POS 0 // 1-Wire busy -#define DS2482_STATUS_REG_PPD_POS 1 // Presense-pulse detect -#define DS2482_STATUS_REG_SD_POS 2 // Short detected -#define DS2482_STATUS_REG_LL_POS 3 // Logic level -#define DS2482_STATUS_REG_RST_POS 4 // Device reset -#define DS2482_STATUS_REG_SBR_POS 5 // Single bit result -#define DS2482_STATUS_REG_TSB_POS 6 // Triplet second bit -#define DS2482_STATUS_REG_DIR_POS 7 // Branch direction taken - -#define DS2482_1WIRE_BUSY(status) (status & (1 << DS2482_STATUS_REG_1WB_POS)) -#define DS2482_DEVICE_PRESENT(status) (status & (1 << DS2482_STATUS_REG_PPD_POS)) // True if a device have been detected on the bus after a bus reset -#define DS2482_SBR_VALUE(status) ((status >> DS2482_STATUS_REG_SBR_POS) & 1) // Extract single bit read value or triplet first bit from status register value -#define DS2482_TSB_VALUE(status) ((status >> DS2482_STATUS_REG_TSB_POS) & 1) // Extract triplet second bit value from status register value -#define DS2482_DIR_VALUE(status) ((status >> DS2482_STATUS_REG_DIR_POS) & 1) // Extract triplet chosen direction bit value from status register value - -#define DS2482_1WIRE_SINGLE_BIT_WRITE0 0 -#define DS2482_1WIRE_SINGLE_BIT_WRITE1_READ (1<<7) - - -bool ds2482Detect(_1WireDev_t *dev); -bool ds2482Init(_1WireDev_t *_1WireDev); - -bool ds2482_reset(_1WireDev_t *_1WireDev); -bool ds2482_1wire_reset(_1WireDev_t *_1WireDev); -bool ds2482_1wire_reset_wait(_1WireDev_t *_1WireDev); -bool ds2482_wait_for_bus(_1WireDev_t *_1WireDev); -bool ds2482_config(_1WireDev_t *_1WireDev, uint8_t *config); -bool ds2482_write_config(_1WireDev_t *_1WireDev, uint8_t config); -bool ds2482_status(_1WireDev_t *_1WireDev, uint8_t *status); -bool ds2482_poll(_1WireDev_t *_1WireDev, bool wait_for_bus, uint8_t *status); -bool ds2482_read_byte(_1WireDev_t *_1WireDev, uint8_t *byte); - -// 1-Wire ROM -bool ds2482_1wire_search_rom(_1WireDev_t *_1WireDev, uint8_t family_code, uint64_t *rom_table, uint8_t *rom_table_len); -bool ds2482_1wire_match_rom(_1WireDev_t *_1WireDev, uint64_t rom); -bool ds2482_1wire_reset_and_match_rom(_1WireDev_t *_1WireDev, uint64_t rom); -bool ds2482_1wire_skip_rom(_1WireDev_t *_1WireDev); -bool ds2482_1wire_skip_rom_wait(_1WireDev_t *_1WireDev); -bool ds2482_1wire_reset_and_skip_rom(_1WireDev_t *_1WireDev); - -// 1-Wire read/write -bool ds2482_1wire_write_byte(_1WireDev_t *_1WireDev, uint8_t byte); -bool ds2482_1wire_write_byte_wait(_1WireDev_t *_1WireDev, uint8_t byte); -bool ds2482_1wire_write_buf(_1WireDev_t *_1WireDev, const uint8_t *buf, uint8_t len); -bool ds2482_1wire_read_byte(_1WireDev_t *_1WireDev); -bool ds2482_1wire_single_bit(_1WireDev_t *_1WireDev, uint8_t type); -bool ds2482_1wire_triplet(_1WireDev_t *_1WireDev, uint8_t direction); -bool ds2482_1wire_triplet_wait(_1WireDev_t *_1WireDev, uint8_t direction); +bool ds2482Detect(owDev_t *owDev); #endif /* defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) */ diff --git a/src/main/drivers/1-wire/ds_crc.c b/src/main/drivers/1-wire/ds_crc.c index 3ccc63525ae..8332ab0a5fa 100644 --- a/src/main/drivers/1-wire/ds_crc.c +++ b/src/main/drivers/1-wire/ds_crc.c @@ -19,7 +19,7 @@ uint8_t ds_crc8_byte_update(uint8_t crc8, uint8_t byte) return crc8; } -uint8_t ds_crc8(uint8_t *addr, uint8_t len) +uint8_t ds_crc8(const uint8_t *addr, uint8_t len) { uint8_t crc8 = 0; for (uint8_t i=0; i < len; ++i) crc8 = ds_crc8_byte_update(crc8, addr[i]); diff --git a/src/main/drivers/1-wire/ds_crc.h b/src/main/drivers/1-wire/ds_crc.h index 62e76716313..839a3e1e107 100644 --- a/src/main/drivers/1-wire/ds_crc.h +++ b/src/main/drivers/1-wire/ds_crc.h @@ -5,4 +5,4 @@ uint8_t ds_crc8_bit_update(uint8_t crc, uint8_t bit); uint8_t ds_crc8_byte_update(uint8_t crc, uint8_t byte); -uint8_t ds_crc8(uint8_t *addr, uint8_t len); +uint8_t ds_crc8(const uint8_t *addr, uint8_t len); diff --git a/src/main/drivers/temperature/ds18b20.c b/src/main/drivers/temperature/ds18b20.c index 9bd5be999fb..3b68c87c57f 100644 --- a/src/main/drivers/temperature/ds18b20.c +++ b/src/main/drivers/temperature/ds18b20.c @@ -38,104 +38,113 @@ #define DS18B20_RECALL_EEPROM_CMD 0xB8 #define DS18B20_READ_POWER_SUPPLY_CMD 0xB4 -#if defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) && defined(USE_TEMPERATURE_DS18B20) +#if defined(USE_1WIRE) && defined(USE_TEMPERATURE_DS18B20) -#define _1WireDev (&ds2482Dev) -bool ds18b20_enumerate(uint64_t *rom_table, uint8_t *rom_table_len) +bool ds18b20Enumerate(owDev_t *owDev, uint64_t *rom_table, uint8_t *rom_table_len) { - return ds2482_1wire_search_rom(_1WireDev, DS18B20_FAMILY_CODE, rom_table, rom_table_len); + return owDev->owSearchRom(owDev, DS18B20_FAMILY_CODE, rom_table, rom_table_len); } -bool ds18b20_configure(uint64_t rom, uint8_t config) +bool ds18b20Configure(owDev_t *owDev, uint64_t rom, uint8_t config) { - bool ack = ds2482_1wire_reset_and_match_rom(_1WireDev, rom); + bool ack = owDev->owMatchRom(owDev, rom); if (!ack) return false; uint8_t buf[4] = { DS18B20_WRITE_SCRATCHPAD_CMD, 0, 0, config }; - return ds2482_1wire_write_buf(_1WireDev, buf, sizeof(buf)); + return owDev->owWriteBuf(owDev, buf, sizeof(buf)); } -static bool ds18b20_parasitic_powered_present() +static bool readPowerSupply(owDev_t *owDev, bool *result) { - bool ack = ds2482_1wire_reset_and_skip_rom(_1WireDev); + bool ack = owDev->owWriteByte(owDev, DS18B20_READ_POWER_SUPPLY_CMD); if (!ack) return false; - ack = ds2482_1wire_write_byte(_1WireDev, DS18B20_READ_POWER_SUPPLY_CMD); + bool sbr; + ack = owDev->owSingleBit(owDev, OW_SINGLE_BIT_WRITE1_READ, &sbr); if (!ack) return false; - uint8_t status; - ack = ds2482_poll(_1WireDev, true, &status); + *result = !sbr; + return true; +} + +bool ds18b20ParasiticPoweredPresent(owDev_t *owDev, bool *result) +{ + bool ack = owDev->owSkipRom(owDev); if (!ack) return false; - return !DS2482_SBR_VALUE(status); + return readPowerSupply(owDev, result); } -static bool ds18b20_read_scratchpad() +bool ds18b20ReadPowerSupply(owDev_t *owDev, uint64_t rom, bool *parasiticPowered) { - return ds2482_1wire_write_byte(_1WireDev, DS18B20_READ_SCRATCHPAD_CMD); + bool ack = owDev->owMatchRom(owDev, rom); + if (!ack) return false; + + return readPowerSupply(owDev, parasiticPowered); } -static bool ds18b20_read_scratchpad_buf(uint8_t *buf, uint8_t len) +bool ds18b20ReadScratchpadCommand(owDev_t *owDev) { - ds18b20_read_scratchpad(_1WireDev); - ds2482_wait_for_bus(_1WireDev); - - for (uint8_t index = 0; index < len; ++index) { - bool ack = ds2482_1wire_read_byte(_1WireDev); - if (!ack) return false; + return owDev->owWriteByteCommand(owDev, DS18B20_READ_SCRATCHPAD_CMD); +} - ack = ds2482_wait_for_bus(_1WireDev); - if (!ack) return false; +static bool ds18b20ReadScratchpadBuf(owDev_t *owDev, uint8_t *buf, uint8_t len) +{ + bool ack = ds18b20ReadScratchpadCommand(owDev); + if (!ack) return false; + ack = owDev->waitForBus(owDev); + if (!ack) return false; + return owDev->owReadBuf(owDev, buf, len); +} - ack = ds2482_read_byte(_1WireDev, buf + index); - if (!ack) return false; - } - return true; +bool ds18b20StartConversionCommand(owDev_t *owDev) +{ + return owDev->owWriteByteCommand(owDev, DS18B20_START_CONVERSION_CMD); } -bool ds18b20_start_conversion() +// start conversion on all devices present on the bus +// note: parasitic power only supports one device converting at a time +bool ds18b20StartConversion(owDev_t *owDev) { - bool parasitic_power = ds18b20_parasitic_powered_present(_1WireDev); - if (parasitic_power) return false; - bool ack = ds2482_1wire_reset_and_skip_rom(_1WireDev); + bool ack = owDev->owSkipRom(owDev); if (!ack) return false; - return ds2482_1wire_write_byte(_1WireDev, DS18B20_START_CONVERSION_CMD); + return ds18b20StartConversionCommand(owDev); } -bool ds18b20_wait_for_conversion() +bool ds18b20WaitForConversion(owDev_t *owDev) { - bool ack; - uint8_t status, read_bit = 0; - while (!read_bit) { - ack = ds2482_wait_for_bus(_1WireDev); - if (!ack) return false; + bool ack = owDev->waitForBus(owDev); + if (!ack) return false; - ack = ds2482_1wire_single_bit(_1WireDev, DS2482_1WIRE_SINGLE_BIT_WRITE1_READ); + bool read_bit; + do { + ack = owDev->owSingleBit(owDev, OW_SINGLE_BIT_WRITE1_READ, &read_bit); if (!ack) return false; + } while (!read_bit); - while (1) { - ack = busRead(_1WireDev->busDev, 0xFF, &status); - if (!ack) return false; - if ((status & 1) == 0) break; - } - - read_bit = DS2482_SBR_VALUE(status); + return true; +} - } +bool ds18b20ReadTemperatureFromScratchPadBuf(const uint8_t *buf, int16_t *temperature) +{ + if (buf[8] != ds_crc8(buf, 8)) return false; + *temperature = (int16_t)(((buf[0] | (buf[1] << 8)) >> 3) | ((buf[1] & 0x80) ? 0xE000 : 0)) * 5; return true; } -bool ds18b20_read_temperature(uint64_t rom, int16_t *temperature) +bool ds18b20ReadTemperature(owDev_t *owDev, uint64_t rom, int16_t *temperature) { - uint8_t buf[9]; - bool ack = ds2482_1wire_reset_and_match_rom(_1WireDev, rom); + bool ack = owDev->owMatchRom(owDev, rom); if (!ack) return false; - ack = ds18b20_read_scratchpad_buf(buf, 9); + + uint8_t buf[9]; + ack = ds18b20ReadScratchpadBuf(owDev, buf, 9); if (!ack) return false; + if (buf[8] != ds_crc8(buf, 8)) return false; *temperature = (int16_t)(((buf[0] | (buf[1] << 8)) >> 3) | ((buf[1] & 0x80) ? 0xE000 : 0)) * 5; return true; } -#endif /* defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) && defined(USE_TEMPERATURE_DS18B20) */ +#endif /* defined(USE_1WIRE) && defined(USE_TEMPERATURE_DS18B20) */ diff --git a/src/main/drivers/temperature/ds18b20.h b/src/main/drivers/temperature/ds18b20.h index c1148dfcfdb..aaecf160d03 100644 --- a/src/main/drivers/temperature/ds18b20.h +++ b/src/main/drivers/temperature/ds18b20.h @@ -5,8 +5,9 @@ #include #include "drivers/1-wire.h" -#if defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) && defined(USE_TEMPERATURE_DS18B20) +#if defined(USE_1WIRE) && defined(USE_TEMPERATURE_DS18B20) +#define USE_TEMPERATURE_SENSOR #define DS18B20_DRIVER_AVAILABLE #define DS18B20_CONFIG_9BIT 0x1F @@ -21,11 +22,15 @@ #define DS18B20_12BIT_CONVERSION_TIME 750 -bool ds18b20_enumerate(uint64_t *rom_table, uint8_t *rom_table_len); -bool ds18b20_configure(uint64_t rom, uint8_t config); -bool ds18b20_wait_for_conversion(); -bool ds18b20_start_conversion(); -bool ds18b20_wait_for_conversion(); -bool ds18b20_read_temperature(uint64_t rom, int16_t *temperature); +bool ds18b20Enumerate(owDev_t *owDev, uint64_t *rom_table, uint8_t *rom_table_len); +bool ds18b20Configure(owDev_t *owDev, uint64_t rom, uint8_t config); +bool ds18b20ParasiticPoweredPresent(owDev_t *owDev, bool *result); +bool ds18b20ReadPowerSupply(owDev_t *owDev, uint64_t rom, bool *parasiticPowered); +bool ds18b20StartConversionCommand(owDev_t *owDev); +bool ds18b20StartConversion(owDev_t *owDev); +bool ds18b20WaitForConversion(owDev_t *owDev); +bool ds18b20ReadScratchpadCommand(owDev_t *owDev); +bool ds18b20ReadTemperatureFromScratchPadBuf(const uint8_t *buf, int16_t *temperature); +bool ds18b20ReadTemperature(owDev_t *owDev, uint64_t rom, int16_t *temperature); -#endif /* defined(USE_1WIRE) && defined(USE_1WIRE_DS2482) && defined(USE_TEMPERATURE_DS18B20) */ +#endif /* defined(USE_1WIRE) && defined(USE_TEMPERATURE_DS18B20) */ diff --git a/src/main/drivers/temperature/lm75.h b/src/main/drivers/temperature/lm75.h index 0419b953904..2fc4e9f9362 100644 --- a/src/main/drivers/temperature/lm75.h +++ b/src/main/drivers/temperature/lm75.h @@ -21,5 +21,9 @@ #include "drivers/temperature/temperature.h" #ifdef USE_TEMPERATURE_LM75 + +#define USE_TEMPERATURE_SENSOR + bool lm75Detect(temperatureDev_t *tempDev, uint8_t partial_address); -#endif + +#endif /* USE_TEMPERATURE_LM75 */ diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index c519ddaea73..3aab52166c4 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -534,7 +534,7 @@ void init(void) // 1-Wire IF chip #ifdef USE_1WIRE - _1WireInit(); + owInit(); #endif if (!sensorsAutodetect()) { diff --git a/src/main/sensors/temperature.c b/src/main/sensors/temperature.c index 09d6f3ba5a4..5192b20638b 100644 --- a/src/main/sensors/temperature.c +++ b/src/main/sensors/temperature.c @@ -63,6 +63,10 @@ static uint8_t sensorStatus[MAX_TEMP_SENSORS / 8 + (MAX_TEMP_SENSORS % 8 ? 1 : 0 static temperatureDev_t lm75Dev[8]; #endif +#ifdef DS18B20_DRIVER_AVAILABLE +static owDev_t *owDev; +#endif + static void newSensorCheckAndEnter(uint8_t type, uint64_t addr) { int8_t foundConfigIndex = -1, firstFreeConfigSlot = -1; @@ -108,10 +112,11 @@ void temperatureInit(void) #endif #ifdef DS18B20_DRIVER_AVAILABLE - if (ds2482_detected) { + owDev = getOwDev(); + if (owDev) { uint64_t ds18b20_rom_table[MAX_TEMP_SENSORS]; uint8_t ds18b20_rom_count = MAX_TEMP_SENSORS; - ds18b20_enumerate(ds18b20_rom_table, &ds18b20_rom_count); + ds18b20Enumerate(owDev, ds18b20_rom_table, &ds18b20_rom_count); for (uint8_t rom_index = 0; rom_index < ds18b20_rom_count; ++rom_index) newSensorCheckAndEnter(TEMP_SENSOR_DS18B20, ds18b20_rom_table[rom_index]); @@ -125,8 +130,10 @@ void temperatureInit(void) switch (configSlot->type) { #ifdef DS18B20_DRIVER_AVAILABLE case TEMP_SENSOR_DS18B20: - tempSensorValue[configIndex] = -1250; - ds18b20_configure(configSlot->address, DS18B20_CONFIG_9BIT); + if (owDev) { + tempSensorValue[configIndex] = -1250; + ds18b20Configure(owDev, configSlot->address, DS18B20_CONFIG_9BIT); + } break; #endif @@ -213,18 +220,20 @@ void temperatureUpdate(void) #endif break; - case TEMP_SENSOR_DS18B20:; + case TEMP_SENSOR_DS18B20: #ifdef DS18B20_DRIVER_AVAILABLE - int16_t temperature; - if (ds18b20_read_temperature(configSlot->address, &temperature)) { - if (temperatureSensorValueIsValid(sensorIndex) || (tempSensorValue[sensorIndex] == -1240)) { - tempSensorValue[sensorIndex] = temperature; - valueValid = true; - } else - tempSensorValue[sensorIndex] = -1240; + if (owDev) { + int16_t temperature; + if (ds18b20ReadTemperature(owDev, configSlot->address, &temperature)) { + if (temperatureSensorValueIsValid(sensorIndex) || (tempSensorValue[sensorIndex] == -1240)) { + tempSensorValue[sensorIndex] = temperature; + valueValid = true; + } else + tempSensorValue[sensorIndex] = -1240; - } else - tempSensorValue[sensorIndex] = -1250; + } else + tempSensorValue[sensorIndex] = -1250; + } #endif break; @@ -242,7 +251,7 @@ void temperatureUpdate(void) #endif #ifdef DS18B20_DRIVER_AVAILABLE - ds18b20_start_conversion(); + if (owDev) ds18b20StartConversion(owDev); #endif }