Index: trunk/modules/sdmmc/caba/source/include/sdmmc.h
===================================================================
--- trunk/modules/sdmmc/caba/source/include/sdmmc.h	(revision 552)
+++ trunk/modules/sdmmc/caba/source/include/sdmmc.h	(revision 555)
@@ -85,5 +85,5 @@
 
     void handle_sdmmc_cmd(uint8_t, uint32_t);
-    void handle_sdmmc_write();
+    void handle_sdmmc_write(uint8_t, uint32_t);
 
     //  Master FSM states
@@ -94,8 +94,7 @@
     S_RECEIVE_ARGS       = 3,
     S_RECEIVE_CRC        = 4,
-    S_RECEIVE_DATA       = 5,
-    S_RECEIVE_DATA_END   = 6,
+    S_RECEIVE_DATA_WAIT  = 5,
+    S_RECEIVE_DATA       = 6,
     S_SEND_DATA      	 = 7,
-    S_NOP		 = 8,
     };
 
Index: trunk/modules/sdmmc/caba/source/src/sdmmc.cpp
===================================================================
--- trunk/modules/sdmmc/caba/source/src/sdmmc.cpp	(revision 552)
+++ trunk/modules/sdmmc/caba/source/src/sdmmc.cpp	(revision 555)
@@ -1,3 +1,4 @@
-/* -*- c++ -*-
+/*
+ -*- c++ -*-
  *
  * SOCLIB_LGPL_HEADER_BEGIN
@@ -49,4 +50,12 @@
     } 
     if (p_spi_ss.read()) {
+	if (r_spi_fsm != S_IDLE) {
+		std::cerr << name() << " deselect but not idle, state "
+		<< std::dec << r_spi_fsm << " last cmd " << (int)r_command
+		<< " args " << std::hex << r_args << std::dec
+		<< " bitcount " << (int)r_spi_bitcount.read()
+		<< " idx " << m_data_idx << " len_snd " << m_datalen_snd
+		<< " len_rcv " << m_datalen_rcv << std::endl;
+	}
 	r_spi_fsm  = S_IDLE;
 	return;
@@ -73,5 +82,5 @@
 				r_spi_fsm = S_RECEIVE_ARGS_START;
 			} else {
-#ifdef SOCLIB_MODULE_DEBUG
+#ifdef SOCLIB_MODULE_DEBUG0
 				std::cout << name() << " S_RECEIVE_CMD " << std::hex << ((int)((r_command << 1) | p_spi_mosi) & 0xff) << std::endl;
 #endif
@@ -125,9 +134,9 @@
 				r_spi_fsm = S_SEND_DATA;
 				m_data_idx++;
-#ifdef SOCLIB_MODULE_DEBUG
+#ifdef SOCLIB_MODULE_DEBUG0
 		        std::cout << name() << " S_SEND_DATA " << std::dec << m_datalen_snd << " idx " << m_data_idx << " " << std::hex << (uint32_t)m_databuf[m_data_idx] << std::endl;
 #endif
 			} else if (m_datalen_rcv != 0) {
-				r_spi_fsm = S_RECEIVE_DATA;
+				r_spi_fsm = S_RECEIVE_DATA_WAIT;
 				r_spi_bitcount = 7;
 				m_data_idx = 0;
@@ -141,4 +150,52 @@
 	}
 	break;
+    case S_RECEIVE_DATA_WAIT:
+	if (p_spi_clk.read() == 1 && r_spi_clk.read() == 0) {
+	    uint8_t s_data;
+	    // rising edge
+	    s_data = (m_databuf[0] << 1) | p_spi_mosi;
+	    m_databuf[0] = s_data;
+	    r_spi_bitcount = r_spi_bitcount - 1;
+	    if (r_spi_bitcount == 0) {
+#ifdef SOCLIB_MODULE_DEBUG
+	std::cout << name() << " S_RECEIVE_DATA_WAIT " << std::dec << (int)s_data << std::endl;
+#endif
+		    r_spi_bitcount = 7;
+		    if (s_data == 0xfe) { // data start token
+			r_spi_fsm = S_RECEIVE_DATA;
+			m_data_idx = 1;
+		    } else {
+#ifdef SOCLIB_MODULE_DEBUG
+			std::cout << name() << " S_RECEIVE_DATA_WAIT " << std::hex << (int)s_data << std::endl;
+#endif
+			r_spi_fsm = S_RECEIVE_DATA_WAIT;
+		}
+	    }
+	}
+	break;
+	case S_RECEIVE_DATA:
+	    if (p_spi_clk.read() == 1 && r_spi_clk.read() == 0) {
+		// rising edge
+		m_databuf[m_data_idx] = (m_databuf[m_data_idx] << 1) | p_spi_mosi;
+		if (r_spi_bitcount == 0) {
+		    m_data_idx++;
+		    if (m_data_idx != m_datalen_rcv) {
+		    	r_spi_fsm = S_RECEIVE_DATA;
+		    	r_spi_bitcount = 7;
+		    } else {
+		        handle_sdmmc_write(r_command.read(), r_args.read());
+		        if (m_datalen_snd > 0) {
+		            r_spi_bitcount = 0; // SEND_DATA will reset it
+		            r_spi_fsm = S_SEND_DATA;
+		            m_data_idx = 0;
+		        } else {
+		            r_spi_fsm = S_IDLE;
+		        }
+		    }
+		} else {
+			r_spi_bitcount = r_spi_bitcount - 1;
+		}
+	    }
+	    break;
     }
 }  // end transition
@@ -177,5 +234,5 @@
 	cmd &= 0x3f;
 	if (m_acmd) {
-#ifdef SOCLIB_MODULE_DEBUG
+#ifdef SOCLIB_MODULE_DEBUG0
 	std::cout << name() << " new acmd " << std::dec << (int)cmd << " args " << std::hex << data << " crc " << (int)r_cmdcrc << std::endl;
 #endif
@@ -209,5 +266,5 @@
 	    }
 	} else {
-#ifdef SOCLIB_MODULE_DEBUG
+#ifdef SOCLIB_MODULE_DEBUG0
 	std::cout << name() << " new cmd " << std::dec << (int)cmd << " args " << std::hex << data << " crc " << (int)r_cmdcrc << std::endl;
 #endif
@@ -323,4 +380,19 @@
 		break;
 	      }
+	    case 24:
+	      {
+		// write data block
+		if (m_sdstate == SD_IDLE) {
+			// return illegal command
+			return;
+		}
+#ifdef SOCLIB_MODULE_DEBUG
+	std::cout << name() << " new cmd write " << std::dec << (int)cmd << " args " << std::hex << data << std::endl;
+#endif
+		m_databuf[0] = 0x0; // R1
+		m_datalen_snd = 1;
+		m_datalen_rcv = 512 + 2 + 1; // data + tocken + CRC
+		break;
+	      }
 	    case 55:
 		// app-specific command follow
@@ -346,6 +418,52 @@
 }
 
-void SdMMC::handle_sdmmc_write()
-{
+void SdMMC::handle_sdmmc_write(uint8_t cmd, uint32_t data)
+{
+	m_datalen_rcv = 0;
+	cmd &= 0x3f;
+#ifdef SOCLIB_MODULE_DEBUG
+	std::cout << name() << " cmd write " << std::dec << (int)cmd << " args " << std::hex << data << std::endl;
+#endif
+	switch(cmd) {
+	    case 24:
+	      {
+		int ret;
+		// write data block
+		assert(m_sdstate != SD_IDLE && "can't write in idle state");
+		if (data >= m_device_size) {
+			std::cerr << name() << " write: request " << data
+			    << " past end of file " << m_device_size << std::endl;
+			m_databuf[0] = 0xd; // write error
+			m_datalen_snd = 1;
+			return;
+		}
+		do {
+			if (lseek(m_fd, data, SEEK_SET) < 0) {
+				std::cerr << name() << " lseek: " <<
+				  strerror(errno) << std::endl;
+				m_databuf[0] = 0xd; // write error
+				m_datalen_snd = 1;
+				return;
+			}
+			ret = write(m_fd, &m_databuf[1], 512);
+		} while (ret < 0 && errno == EINTR);
+		if (ret < 0) {
+			std::cerr << name() << " write: " <<
+			  strerror(errno) << std::endl;
+			m_databuf[0] = 0xd; // write error
+			m_datalen_snd = 1;
+			return;
+		}
+		m_databuf[0] = 0x5; // write complete
+		m_databuf[1] = 0x0; // busy
+		m_datalen_snd = 2;
+		break;
+	      }
+	    default:
+		std::cerr << name() << " unkown write cmd " << std::dec <<
+		    (int)cmd << std::endl;
+		m_databuf[0] = 0xd; // write error;
+		m_datalen_snd = 1;
+	}
 	return;
 }
@@ -401,6 +519,6 @@
                 "S_RECEIVE_ARGS",
                 "S_RECEIVE_CRC",
+                "S_RECEIVE_DATA_START",
                 "S_RECEIVE_DATA",
-                "S_RECEIVE_DATA_END",
                 "S_SEND_DATA",
                 "S_NOP",