Added __probe_for_device

3 files changed, 31 insertions, 38 deletions

diff --git a/shared-bindings/busdevice/I2CDevice.c b/shared-bindings/busdevice/I2CDevice.c
index 02dac6a95..fa5cb02e0 100644
--- a/shared-bindings/busdevice/I2CDevice.c
+++ b/shared-bindings/busdevice/I2CDevice.c
@@ -37,6 +37,7 @@
 #include "py/runtime.h"
 #include "supervisor/shared/translate.h"
 
+
 //| class I2CDevice:
 //|     """Two wire serial protocol"""
 //|
@@ -59,6 +60,10 @@ STATIC mp_obj_t busdevice_i2cdevice_make_new(const mp_obj_type_t *type, size_t n
     busio_i2c_obj_t* i2c = args[ARG_i2c].u_obj;
 
     common_hal_busdevice_i2cdevice_construct(self, i2c, args[ARG_device_address].u_int, args[ARG_probe].u_bool);
+    if (args[ARG_probe].u_bool == true) {
+        common_hal_busdevice_i2cdevice___probe_for_device(self);
+    }
+
     return (mp_obj_t)self;
 }
 
@@ -204,28 +209,31 @@ MP_DEFINE_CONST_FUN_OBJ_KW(busdevice_i2cdevice_write_then_readinto_obj, 3, busde
 
 
 STATIC mp_obj_t busdevice_i2cdevice___probe_for_device(mp_obj_t self_in) {
-    //busdevice_i2cdevice_obj_t *self = self_in;
+    busdevice_i2cdevice_obj_t *self = self_in;
+    common_hal_busdevice_i2cdevice___probe_for_device(self);
 
-    //common_hal_busdevice_i2cdevice_lock(self_in);
+/*    common_hal_busdevice_i2cdevice_lock(self);
 
-/*
-    uint8_t buffer;
+
+    //uint8_t buffer;
     mp_buffer_info_t bufinfo;
-    mp_get_buffer_raise(&buffer, &bufinfo, MP_BUFFER_WRITE);
+    //mp_obj_t bufobj = MP_OBJ_FROM_PTR(&buffer)
+    mp_obj_t buffer = mp_obj_new_bytearray_of_zeros(1);
+
+    mp_get_buffer_raise(buffer, &bufinfo, MP_BUFFER_WRITE);
 
     uint8_t status = common_hal_busdevice_i2cdevice_readinto(self_in, (uint8_t*)bufinfo.buf, 1);
     if (status != 0) {
         common_hal_busdevice_i2cdevice_unlock(self_in);
         mp_raise_ValueError_varg(translate("No I2C device at address: %x"), self->device_address);
     }
+    
+    common_hal_busdevice_i2cdevice_unlock(self);
 */
-    //common_hal_busdevice_i2cdevice_unlock(self_in);
-
     return mp_const_none;
 }
 MP_DEFINE_CONST_FUN_OBJ_1(busdevice_i2cdevice___probe_for_device_obj, busdevice_i2cdevice___probe_for_device);
 
-
 STATIC const mp_rom_map_elem_t busdevice_i2cdevice_locals_dict_table[] = {
     { MP_ROM_QSTR(MP_QSTR___enter__), MP_ROM_PTR(&busdevice_i2cdevice___enter___obj) },
     { MP_ROM_QSTR(MP_QSTR___exit__), MP_ROM_PTR(&busdevice_i2cdevice___exit___obj) },
diff --git a/shared-bindings/busdevice/I2CDevice.h b/shared-bindings/busdevice/I2CDevice.h
index bc85023d7..795905b32 100644
--- a/shared-bindings/busdevice/I2CDevice.h
+++ b/shared-bindings/busdevice/I2CDevice.h
@@ -45,14 +45,12 @@ extern const mp_obj_type_t busdevice_i2cdevice_type;
 
 // Initializes the hardware peripheral.
 extern void common_hal_busdevice_i2cdevice_construct(busdevice_i2cdevice_obj_t *self, busio_i2c_obj_t *i2c, uint8_t device_address, bool probe);
-extern void common_hal_busdevice_i2cdevice___enter__(busdevice_i2cdevice_obj_t *self);
-extern void common_hal_busdevice_i2cdevice___exit__(busdevice_i2cdevice_obj_t *self);
 extern uint8_t common_hal_busdevice_i2cdevice_readinto(busdevice_i2cdevice_obj_t *self, mp_obj_t buffer, size_t length);
 extern uint8_t common_hal_busdevice_i2cdevice_write(busdevice_i2cdevice_obj_t *self, mp_obj_t buffer, size_t length);
 extern uint8_t common_hal_busdevice_i2cdevice_write_then_readinto(busdevice_i2cdevice_obj_t *self, mp_obj_t out_buffer,
                                                                  mp_obj_t in_buffer, size_t out_length, size_t in_length);
-extern uint8_t common_hal_busdevice_i2cdevice___probe_for_device(busdevice_i2cdevice_obj_t *self);
 extern void common_hal_busdevice_i2cdevice_lock(busdevice_i2cdevice_obj_t *self);
 extern void common_hal_busdevice_i2cdevice_unlock(busdevice_i2cdevice_obj_t *self);
+extern void common_hal_busdevice_i2cdevice___probe_for_device(busdevice_i2cdevice_obj_t *self);
 
 #endif // MICROPY_INCLUDED_SHARED_BINDINGS_BUSDEVICE_I2CDEVICE_H
diff --git a/shared-module/busdevice/I2CDevice.c b/shared-module/busdevice/I2CDevice.c
index 30f4aad33..516132c84 100644
--- a/shared-module/busdevice/I2CDevice.c
+++ b/shared-module/busdevice/I2CDevice.c
@@ -28,15 +28,12 @@
 #include "shared-bindings/busio/I2C.h"
 #include "py/mperrno.h"
 #include "py/nlr.h"
+#include "py/runtime.h"
 
 void common_hal_busdevice_i2cdevice_construct(busdevice_i2cdevice_obj_t *self, busio_i2c_obj_t *i2c, uint8_t device_address, bool probe) {
     self->i2c = i2c;
     self->device_address = device_address;
     self->probe = probe;
-
-    if (self->probe == true) {
-        common_hal_busdevice_i2cdevice___probe_for_device(self);
-    }
 }
 
 void common_hal_busdevice_i2cdevice_lock(busdevice_i2cdevice_obj_t *self) {
@@ -73,29 +70,19 @@ uint8_t common_hal_busdevice_i2cdevice_write_then_readinto(busdevice_i2cdevice_o
     return status;
 }
 
-uint8_t common_hal_busdevice_i2cdevice___probe_for_device(busdevice_i2cdevice_obj_t *self) {
-
-
+void common_hal_busdevice_i2cdevice___probe_for_device(busdevice_i2cdevice_obj_t *self) {
+    common_hal_busdevice_i2cdevice_lock(self);
 
+    mp_buffer_info_t bufinfo;
+    mp_obj_t buffer = mp_obj_new_bytearray_of_zeros(1);
 
-    // write ""
+    mp_get_buffer_raise(buffer, &bufinfo, MP_BUFFER_WRITE);
 
-
-/*
-            while not self.i2c.try_lock():
-            pass
-        try:
-            self.i2c.writeto(self.device_address, b"")
-        except OSError:
-            # some OS's dont like writing an empty bytesting...
-            # Retry by reading a byte
-            try:
-                result = bytearray(1)
-                self.i2c.readfrom_into(self.device_address, result)
-            except OSError:
-                raise ValueError("No I2C device at address: %x" % self.device_address)
-        finally:
-            self.i2c.unlock()
-*/
-    return 0;
-}
+    uint8_t status = common_hal_busdevice_i2cdevice_readinto(self, (uint8_t*)bufinfo.buf, 1);
+    if (status != 0) {
+        common_hal_busdevice_i2cdevice_unlock(self);
+        mp_raise_ValueError_varg(translate("No I2C device at address: %x"), self->device_address);
+    }
+    
+    common_hal_busdevice_i2cdevice_unlock(self);
+}
\ No newline at end of file