From 3ce07dceee19915e34d82868d26933c9aa94da24 Mon Sep 17 00:00:00 2001
From: Deepika <deepika.bhavnani@arm.com>
Date: Wed, 5 Jul 2017 10:25:41 -0500
Subject: [PATCH] Function to update transfer freq

---
 features/filesystem/sd/SDBlockDevice.cpp | 27 +++++++++++++++++++++---
 features/filesystem/sd/SDBlockDevice.h   |  9 ++++++++
 2 files changed, 33 insertions(+), 3 deletions(-)

diff --git a/features/filesystem/sd/SDBlockDevice.cpp b/features/filesystem/sd/SDBlockDevice.cpp
index 86225db959..ff02057cfb 100644
--- a/features/filesystem/sd/SDBlockDevice.cpp
+++ b/features/filesystem/sd/SDBlockDevice.cpp
@@ -239,7 +239,7 @@ SDBlockDevice::SDBlockDevice(PinName mosi, PinName miso, PinName sclk, PinName c
 
     // Set default to 100kHz for initialisation and 1MHz for data transfer
     _init_sck = 100000;
-    _transfer_sck = 25000000;
+    _transfer_sck = 1000000;
 
     // Only HC block size is supported.
     _block_size = BLOCK_SIZE_HC;
@@ -352,7 +352,7 @@ int SDBlockDevice::init()
     }
 
     // Set SCK for data transfer
-    _spi.frequency(_transfer_sck);
+    _freq();
     _lock.unlock();
     return BD_ERROR_OK;
 }
@@ -533,7 +533,6 @@ int SDBlockDevice::erase(bd_addr_t addr, bd_size_t size)
         return SD_BLOCK_DEVICE_ERROR_NO_INIT;
     }
     int status = BD_ERROR_OK;
-    uint8_t response = 0xFF;
 
     size -= _block_size;
     // SDSC Card (CCS=0) uses byte unit address
@@ -590,7 +589,29 @@ void SDBlockDevice::debug(bool dbg)
     _dbg = dbg;
 }
 
+int SDBlockDevice::set_freq(uint64_t freq)
+{
+    _lock.lock();
+    _transfer_sck = freq;
+    _freq();
+    _lock.unlock();
+    return 0;
+}
+
 // PRIVATE FUNCTIONS
+int SDBlockDevice::_freq(void)
+{
+    // Max frequency supported is 25MHZ
+    if (_transfer_sck <= 25000000) {
+        _spi.frequency(_transfer_sck);
+    }
+    else {  // TODO: Switch function to be implemented for higher frequency
+        _transfer_sck = 25000000;
+        _spi.frequency(_transfer_sck);
+    }
+    return 0;
+}
+
 uint8_t SDBlockDevice::_cmd_spi(SDBlockDevice::cmdSupported cmd, uint32_t arg) {
     uint8_t response;
     char cmdPacket[PACKET_SIZE];
diff --git a/features/filesystem/sd/SDBlockDevice.h b/features/filesystem/sd/SDBlockDevice.h
index aa7abe4be7..f9adcbf4dd 100644
--- a/features/filesystem/sd/SDBlockDevice.h
+++ b/features/filesystem/sd/SDBlockDevice.h
@@ -127,6 +127,14 @@ public:
      */
     virtual void debug(bool dbg);
 
+    /** Set the transfer frequency
+     *
+     *  @param         Transfer frequency
+     *  @note Max frequency supported is 25MHZ
+     */
+    virtual int set_freq(uint64_t freq);
+
+
 private:
     /* Commands : Listed below are commands supported
      * in SPI mode for SD card : Only Mandatory ones
@@ -204,6 +212,7 @@ private:
     int _read(uint8_t * buffer, uint32_t length);
     int _read_bytes(uint8_t * buffer, uint32_t length);
     uint8_t _write(const uint8_t *buffer,uint8_t token, uint32_t length);
+    int _freq(void);
 
     /* Chip Select and SPI mode select */
     DigitalOut _cs;