I2C slave: allow function wrapped callbacks

Signed-off-by: hitech95 <nicveronese@gmail.com>

Author: hitech95

libraries/Wire/src/Wire.cpp

@@ -437,6 +437,31 @@ void TwoWire::onRequestService(i2c_t *obj)
   }
 }
 
+#ifdef __cplusplus
+// sets function called on slave write
+void TwoWire::onReceive(cb_function_receive_t function)
+{
+  user_onReceive = function;
+}
+
+void TwoWire::onReceive(void (*function)(int))
+{
+  cb_function_receive_t _c = function;
+  onReceive(_c);
+}
+
+// sets function called on slave read
+void TwoWire::onRequest(cb_function_request_t function)
+{
+  user_onRequest = function;
+}
+
+void TwoWire::onRequest(void (*function)(void))
+{
+  cb_function_request_t _c = function;
+  onRequest(_c);
+}
+#else
 // sets function called on slave write
 void TwoWire::onReceive(void (*function)(int))
 {
@@ -448,6 +473,7 @@ void TwoWire::onRequest(void (*function)(void))
 {
   user_onRequest = function;
 }
+#endif
 
 /**
   * @brief  Allocate the Rx/Tx buffer to the requested length if needed

libraries/Wire/src/Wire.h

@@ -28,6 +28,10 @@ extern "C" {
 #include "utility/twi.h"
 }
 
+#ifdef __cplusplus
+  #include <functional>
+#endif
+
 // Minimal buffer length. Buffers length will be increased when needed,
 // but TX buffer is limited to a maximum to avoid too much stack consumption
 // Note: Buffer length and max buffer length are limited by uin16_t type
@@ -56,8 +60,13 @@ class TwoWire : public Stream {
     uint8_t ownAddress;
     i2c_t _i2c;
 
+#ifdef __cplusplus
+    std::function<void(int)> user_onReceive;
+    std::function<void(void)> user_onRequest;
+#else
     void (*user_onRequest)(void);
     void (*user_onReceive)(int);
+#endif
     static void onRequestService(i2c_t *);
     static void onReceiveService(i2c_t *);
 
@@ -113,6 +122,14 @@ class TwoWire : public Stream {
     void onReceive(void (*)(int));
     void onRequest(void (*)(void));
 
+#ifdef __cplusplus
+    typedef std::function<void(int)> cb_function_receive_t;
+    typedef std::function<void(void)> cb_function_request_t;
+
+    void onReceive(cb_function_receive_t callback);
+    void onRequest(cb_function_request_t callback);
+#endif
+
     inline size_t write(unsigned long n)
     {
       return write((uint8_t)n);