3 files changed, 97 insertions, 3 deletions

diff --git a/lib/cec.h b/lib/cec.h
index 7184ebb..32528c3 100644
--- a/lib/cec.h
+++ b/lib/cec.h
@@ -520,8 +520,9 @@ struct CEC_Device;
 
 struct CEC_Hardware
 {
-	struct CEC_Packet*	(*func_receive)(struct CEC_Device* device);
-	int			(*func_transmit)(struct CEC_Device* device, struct CEC_Packet* packet);
+	struct CEC_Packet*		(*func_receive)(struct CEC_Device* device);
+	int				(*func_transmit)(struct CEC_Device* device, struct CEC_Packet* packet);
+	struct CEC_Physical_Address	(*func_get_addr)(struct CEC_Device *device);
 };
 
 extern struct CEC_Hardware* MSP430_Open_Hardware(const char* device);
diff --git a/lib/device.c b/lib/device.c
index b0cfb5b..9202b4a 100644
--- a/lib/device.c
+++ b/lib/device.c
@@ -81,12 +81,23 @@ int CEC_Transmit(struct CEC_Device* device, struct CEC_Packet* packet)
 	return result;
 }
 
+int CEC_Addr_Equal(struct CEC_Physical_Address* addr1, struct CEC_Physical_Address* addr2)
+{
+	return !memcmp(addr1, addr2, sizeof(struct CEC_Physical_Address));
+}
+
 int CEC_Alloc_Addr(struct CEC_Device* device, enum CEC_Device_Type device_type)
 {
 	uint16_t allowed = device_type_addr[device_type];
 	uint8_t addr;
 
-	if(!memcmp(&device->physical_address, &invalid_addr, sizeof(struct CEC_Physical_Address))) {
+	if(CEC_Addr_Equal(&device->physical_address, &invalid_addr)) {
+		if(device->hardware && device->hardware->func_get_addr) {
+			device->physical_address = device->hardware->func_get_addr(device);
+		}
+	}
+
+	if(CEC_Addr_Equal(&device->physical_address, &invalid_addr)) {
 		device->logical_address = 0xF;
 		return 0;
 	}
diff --git a/lib/msp430.c b/lib/msp430.c
index cf70512..7ac3b52 100644
--- a/lib/msp430.c
+++ b/lib/msp430.c
@@ -29,6 +29,9 @@
 #define TX_BIT_DELAY	2400
 #define TX_RETRY	10
 
+#define EDID_BLOCK_SEL	0x30
+#define EDID_BYTE_SEL	0x50
+
 struct MSP430_Hardware
 {
 	struct CEC_Hardware	cec;
@@ -116,6 +119,84 @@ static int Transmit(struct CEC_Device* device, struct CEC_Packet* packet)
 	return 0;
 }
 
+static uint8_t Read_DDC(struct MSP430_Hardware* hw, uint16_t addr)
+{
+	struct i2c_rdwr_ioctl_data	rdwr;
+	struct i2c_msg			msgs[3];
+	
+	uint8_t block_buf = addr >> 8;
+	uint8_t addr_buf = addr & 0xFF;
+	uint8_t result_buf = 0;
+
+	rdwr.msgs = msgs;
+	rdwr.nmsgs = 0;
+
+	if(block_buf) {
+		msgs[0].addr = EDID_BLOCK_SEL;
+		msgs[0].flags = 0;
+		msgs[0].len = 1;
+		msgs[0].buf = (void*)&block_buf;
+		rdwr.nmsgs++;
+	}
+
+	msgs[rdwr.nmsgs].addr = EDID_BYTE_SEL;
+	msgs[rdwr.nmsgs].flags = 0;
+	msgs[rdwr.nmsgs].len = 1;
+	msgs[rdwr.nmsgs].buf = (void*)&addr_buf;
+	rdwr.nmsgs++;
+
+	msgs[rdwr.nmsgs].addr = EDID_BYTE_SEL;
+	msgs[rdwr.nmsgs].flags = I2C_M_RD;
+	msgs[rdwr.nmsgs].len = 1;
+	msgs[rdwr.nmsgs].buf = (void*)&result_buf;
+	rdwr.nmsgs++;
+
+	ioctl(hw->i2c, I2C_RDWR, &rdwr);
+	return result_buf;
+}
+
+static struct CEC_Physical_Address Get_Addr(struct CEC_Device* device)
+{
+	struct MSP430_Hardware* hw = (struct MSP430_Hardware*)device->hardware;
+	uint8_t nr_of_extensions = Read_DDC(hw, 126);
+
+	int i;
+
+	for(i=1; i <= nr_of_extensions; i++)
+	{
+		uint8_t tag = Read_DDC(hw, 128*i);
+		if(tag != 0x02) continue;
+
+		uint8_t data;
+		uint8_t end = Read_DDC(hw, 128*i + 0x02);
+
+		for(data = 0x04; data < end; data++)
+		{
+			uint8_t type = Read_DDC(hw, 128*i + data);
+			if((type >> 5) == 3) {
+				uint8_t byte[2];
+				byte[0] = Read_DDC(hw, 128*i + data + 4);
+				byte[1] = Read_DDC(hw, 128*i + data + 5);
+
+				struct CEC_Physical_Address result;
+				result.a = byte[0] & 0xF;
+				result.b = byte[0] >> 4;
+				result.c = byte[1] & 0xF;
+				result.d = byte[1] >> 4;
+				return result;
+			}
+			data += type & 0x1F;
+		}
+	}
+
+	struct CEC_Physical_Address result;
+	result.a = 0xF;
+	result.b = 0xF;
+	result.c = 0xF;
+	result.d = 0xF;
+	return result;
+}
+
 struct CEC_Hardware* MSP430_Open_Hardware(const char* device)
 {
 	struct MSP430_Hardware* result = malloc(sizeof(struct MSP430_Hardware));
@@ -125,6 +206,7 @@ struct CEC_Hardware* MSP430_Open_Hardware(const char* device)
 
 	result->cec.func_receive = Receive;
         result->cec.func_transmit = Transmit;
+	result->cec.func_get_addr = Get_Addr;
 
 	result->i2c = open(device, O_RDWR);