Enhance I2C buffer usage

Assume that user will probably use more than one time I2C size.
write: avoid copy byte/byte in the buffer

Signed-off-by: Frederic.Pillon <[email protected]>

Author: fpistm

libraries/Wire/src/Wire.cpp

@@ -29,11 +29,13 @@ extern "C" {
 
 // Initialize Class Variables //////////////////////////////////////////////////
 uint8_t *TwoWire::rxBuffer = nullptr;
+uint8_t TwoWire::rxBufferAllocated = 0;
 uint8_t TwoWire::rxBufferIndex = 0;
 uint8_t TwoWire::rxBufferLength = 0;
 
 uint8_t TwoWire::txAddress = 0;
 uint8_t *TwoWire::txBuffer = nullptr;
+uint8_t TwoWire::txBufferAllocated = 0;
 uint8_t TwoWire::txBufferIndex = 0;
 uint8_t TwoWire::txBufferLength = 0;
 
@@ -66,11 +68,11 @@ void TwoWire::begin(uint8_t address)
 {
   rxBufferIndex = 0;
   rxBufferLength = 0;
-  rxBuffer = resetBuffer(rxBuffer);
+  resetRxBuffer();
 
   txBufferIndex = 0;
   txBufferLength = 0;
-  txBuffer = resetBuffer(txBuffer);
+  resetTxBuffer();
 
   transmitting = 0;
 
@@ -101,8 +103,10 @@ void TwoWire::end(void)
 {
   free(txBuffer);
   txBuffer = nullptr;
+  txBufferAllocated = 0;
   free(rxBuffer);
   rxBuffer = nullptr;
+  rxBufferAllocated = 0;
   i2c_deinit(&_i2c);
 }
 
@@ -115,7 +119,7 @@ uint8_t TwoWire::requestFrom(uint8_t address, uint8_t quantity, uint32_t iaddres
 {
   UNUSED(sendStop);
   if (master == true) {
-    rxBuffer = allocateBuffer(rxBuffer, quantity);
+    allocateRxBuffer(quantity);
     // error if no memory block available to allocate the buffer
     if(rxBuffer == nullptr){
       setWriteError();
@@ -225,9 +229,7 @@ uint8_t TwoWire::endTransmission(uint8_t sendStop)
     }
 
     //Reduce buffer size to free memory in case of large memory use
-    if(txBufferLength > BUFFER_LENGTH) {
-      txBuffer = resetBuffer(txBuffer);
-    }
+    resetTxBuffer();
 
     // reset tx buffer iterator vars
     txBufferIndex = 0;
@@ -255,7 +257,7 @@ size_t TwoWire::write(uint8_t data)
 {
   if(transmitting){
   // in master transmitter mode
-    txBuffer = allocateBuffer(txBuffer, txBufferLength + 1);
+    allocateTxBuffer(txBufferLength + 1);
     // error if no memory block available to allocate the buffer
     if(txBuffer == nullptr){
       setWriteError();
@@ -285,14 +287,22 @@ size_t TwoWire::write(uint8_t data)
   */
 size_t TwoWire::write(const uint8_t *data, size_t quantity)
 {
-  size_t nb = 0;
+  //size_t nb = 0;
 
   if(transmitting){
   // in master transmitter mode
-    for(size_t i = 0; i < quantity; ++i){
-      nb += write(data[i]);
+    allocateTxBuffer(txBufferLength + quantity);
+    // error if no memory block available to allocate the buffer
+    if(txBuffer == nullptr){
+      setWriteError();
+      return 0;
     }
-    return nb;
+    // put bytes in tx buffer
+	memcpy(&(txBuffer[txBufferIndex]), data, quantity);
+    txBufferIndex= txBufferIndex + quantity;
+    // update amount in buffer
+    txBufferLength = txBufferIndex;
+    return quantity;
   }else{
   // in slave send mode
     // reply to master
@@ -326,7 +336,7 @@ int TwoWire::read(void)
     /*  Reduce buffer size to free memory in case of large memory use when no more
         data available */
     if((rxBufferIndex == rxBufferLength) && (rxBufferLength > BUFFER_LENGTH)) {
-      rxBuffer = resetBuffer(rxBuffer);
+      resetRxBuffer();
     }
   }
 
@@ -351,10 +361,10 @@ void TwoWire::flush(void)
 {
   rxBufferIndex = 0;
   rxBufferLength = 0;
-  rxBuffer = resetBuffer(rxBuffer);
+  resetRxBuffer();
   txBufferIndex = 0;
   txBufferLength = 0;
-  txBuffer = resetBuffer(txBuffer);
+  resetTxBuffer();
 }
 
 // behind the scenes function that is called when data is received
@@ -417,29 +427,61 @@ void TwoWire::onRequest( void (*function)(void) )
   * @param  length: number of bytes to allocate
   * @retval pointer to the new buffer location
   */
-uint8_t *TwoWire::allocateBuffer(uint8_t *buffer, size_t length)
+void TwoWire::allocateRxBuffer(size_t length)
 {
-  // By default we allocate BUFFER_LENGTH bytes. It is the min size of the buffer.
-  if(length < BUFFER_LENGTH) {
-    length = BUFFER_LENGTH;
+  if(rxBufferAllocated < length) {
+    // By default we allocate BUFFER_LENGTH bytes. It is the min size of the buffer.
+    if(length < BUFFER_LENGTH) {
+      length = BUFFER_LENGTH;
+    }
+    rxBuffer = (uint8_t *)realloc(rxBuffer, length * sizeof(uint8_t));
+	rxBufferAllocated = length;
   }
+}
 
-  buffer = (uint8_t *)realloc(buffer, length * sizeof(uint8_t));
-  return buffer;
+void TwoWire::allocateTxBuffer(size_t length)
+{
+  if(txBufferAllocated < length) {
+    // By default we allocate BUFFER_LENGTH bytes. It is the min size of the buffer.
+    if(length < BUFFER_LENGTH) {
+      length = BUFFER_LENGTH;
+    }
+    txBuffer = (uint8_t *)realloc(txBuffer, length * sizeof(uint8_t));
+	txBufferAllocated = length;
+  }
 }
 
 /**
   * @brief  Reset the buffer. Reduce is size if greater than BUFFER_LENGTH.
   * @param  buffer: pointer to the allocated buffer
   * @retval pointer to the new buffer location
   */
-uint8_t *TwoWire::resetBuffer(uint8_t *buffer)
+void TwoWire::resetRxBuffer(void)
 {
-  buffer = (uint8_t *)realloc(buffer, BUFFER_LENGTH * sizeof(uint8_t));
-  if(buffer != nullptr) {
-    memset(buffer, 0, BUFFER_LENGTH);
+  if(rxBufferAllocated != BUFFER_LENGTH) {
+    rxBuffer = (uint8_t *)realloc(rxBuffer, BUFFER_LENGTH * sizeof(uint8_t));
+    if(rxBuffer != nullptr) {
+      memset(rxBuffer, 0, BUFFER_LENGTH);
+      rxBufferAllocated = BUFFER_LENGTH;
+    }
+    else {
+      rxBufferAllocated = 0;
+	}
+  }
+}
+
+void TwoWire::resetTxBuffer(void)
+{
+  if(txBufferAllocated != BUFFER_LENGTH) {
+    txBuffer = (uint8_t *)realloc(txBuffer, BUFFER_LENGTH * sizeof(uint8_t));
+    if(txBuffer != nullptr) {
+      memset(txBuffer, 0, BUFFER_LENGTH);
+      txBufferAllocated = BUFFER_LENGTH;
+    }
+    else {
+      txBufferAllocated = 0;
+	}
   }
-  return buffer;
 }
 
 // Preinstantiate Objects //////////////////////////////////////////////////////

libraries/Wire/src/Wire.h

@@ -37,11 +37,13 @@ class TwoWire : public Stream
 {
   private:
     static uint8_t *rxBuffer;
+    static uint8_t rxBufferAllocated;
     static uint8_t rxBufferIndex;
     static uint8_t rxBufferLength;
 
     static uint8_t txAddress;
     static uint8_t *txBuffer;
+    static uint8_t txBufferAllocated;
     static uint8_t txBufferIndex;
     static uint8_t txBufferLength;
 
@@ -56,8 +58,11 @@ class TwoWire : public Stream
     static void onRequestService(void);
     static void onReceiveService(uint8_t*, int);
 
-    uint8_t *allocateBuffer(uint8_t *buffer, size_t length);
-    uint8_t *resetBuffer(uint8_t *buffer);
+    void allocateRxBuffer(size_t length);
+    void allocateTxBuffer(size_t length);
+    void resetRxBuffer(void);
+    void resetTxBuffer(void);
+
 
   public:
     TwoWire();
@@ -73,7 +78,7 @@ class TwoWire : public Stream
     uint8_t endTransmission(uint8_t);
     uint8_t requestFrom(uint8_t, uint8_t);
     uint8_t requestFrom(uint8_t, uint8_t, uint8_t);
-	  uint8_t requestFrom(uint8_t, uint8_t, uint32_t, uint8_t, uint8_t);
+    uint8_t requestFrom(uint8_t, uint8_t, uint32_t, uint8_t, uint8_t);
     uint8_t requestFrom(int, int);
     uint8_t requestFrom(int, int, int);
     virtual size_t write(uint8_t);