dev/i2c_eeprom.c: Use negative offset when addr>0xff is mangled with dev addr

On some I2C eeproms which require addresses >1B, the remaining bits are mangled together with the i2c device address. Thus a negative offset_bytes is introduced to cover these cases. The absolute value refers to the amount of mangled bits. E.g. -2 would mean, that the address consists of 10 bits, with two being joined with the i2c device address.

dev/i2c_eeprom.c

@@ -13,6 +13,21 @@
 #include "dev/bb_i2c.h"
 #include "dev/i2c_eeprom.h"
 
+static uint8_t i2c_get_address_for_virtual_devices( struct i2c_eeprom_device *dev, int offset)
+{
+	uint8_t addr = dev->addr;
+
+	/*
+	 * See explanation in i2c_eeprom.h for negative offsets
+	 */
+	if (dev->offset_bytes < 0) {
+		uint8_t offset_bits_mask = (1 << (-dev->offset_bytes)) - 1;
+		addr = dev->addr | ((offset >> 8) & offset_bits_mask);
+	}
+
+	return addr;
+}
+
 int i2c_eeprom_create( struct i2c_eeprom_device *dev, struct i2c_bus *bus, uint8_t i2c_addr, int offset_bytes )
 {
 	dev->bus = bus;
@@ -25,17 +40,7 @@ int i2c_eeprom_read(struct i2c_eeprom_device *dev, int offset, void *buf, int co
 {
 	int i;
 	unsigned char *cb = buf;
-	uint8_t addr = dev->addr;
-
-	/* Some eeproms use only 1 address byte, but mangle the
-	 * remaining address bits together with the address.
-	 * By using a negative offset specifing the number of
-	 * mangled bits, this driver is compatible with these
-	 * eeproms. E.g. -2 for a 10bit eeprom with 2 mangled
-	 * bits
-	 */
-	if (dev->offset_bytes < 0)
-		addr |= (offset >> 8) & (abs(dev->offset_bytes) + 1);
+	uint8_t addr = i2c_get_address_for_virtual_devices(dev, offset);
 
 	bb_i2c_start(dev->bus);
 	if (bb_i2c_put_byte(dev->bus, addr << 1) < 0) {
@@ -64,21 +69,13 @@ int i2c_eeprom_write(struct i2c_eeprom_device *dev, int offset, void *buf, int c
 {
 	int i, busy;
 	unsigned char *cb = buf;
-	uint8_t addr = dev->addr;
-
-	/* Some eeproms use only 1 address byte, but mangle the
-	 * remaining address bits together with the address.
-	 * By using a negative offset specifing the number of
-	 * mangled bits, this driver is compatible with these
-	 * eeproms. E.g. -2 for a 10bit eeprom with 2 mangled
-	 * bits
-	 */
-	if (dev->offset_bytes < 0)
-		addr |= (offset >> 8) & (abs(dev->offset_bytes) + 1);
+	uint8_t addr;
 
 	for (i = 0; i < count; i++) {
 		bb_i2c_start(dev->bus);
 
+		addr = i2c_get_address_for_virtual_devices(dev, offset);
+
 		if (bb_i2c_put_byte(dev->bus, addr << 1) < 0) {
 			bb_i2c_stop(dev->bus);
 			return -1;
@@ -105,21 +102,13 @@ int i2c_eeprom_write(struct i2c_eeprom_device *dev, int offset, void *buf, int c
 int i2c_eeprom_erase(struct i2c_eeprom_device *dev, int offset, int count)
 {
 	int i, busy;
-	uint8_t addr = dev->addr;
-
-	/* Some eeproms use only 1 address byte, but mangle the
-	 * remaining address bits together with the address.
-	 * By using a negative offset specifing the number of
-	 * mangled bits, this driver is compatible with these
-	 * eeproms. E.g. -2 for a 10bit eeprom with 2 mangled
-	 * bits
-	 */
-	if (dev->offset_bytes < 0)
-		addr |= (offset >> 8) & (abs(dev->offset_bytes) + 1);
+	uint8_t addr;
 
 	for (i = 0; i < count; i++) {
 		bb_i2c_start(dev->bus);
 
+		addr = i2c_get_address_for_virtual_devices(dev, offset);
+
 		if (bb_i2c_put_byte(dev->bus, addr << 1) < 0) {
 			bb_i2c_stop(dev->bus);
 			return -1;