quadrotor/sdcard.c

/* sdcard.c */

#include "spi.h"
#include "types.h"
#include "uart.h"
#include "timer.h"
#include "event.h"
#include "log.h"

#define spi_write_array(x) spi_write_bytes(x, sizeof(x)/sizeof(x[0]))

#define SDCARD_COMMAND_TIMEOUT 0xffff

char dummy_block[] = {0xff, 0xff, 0xff, 0xff, 0xff,
		      0xff, 0xff, 0xff, 0xff, 0xff};
char reset_command[] = {0x40, 0, 0, 0, 0, 0x95};

/* Voltage = 2.7-3.6V, check pattern = 0xaa, CRC matters for CMD8 */
char sdcard_cmd8[] = {0x48, 0, 0, 0x01, 0xaa, 0x87};

char sdcard_cmd55[] = {0x77, 0, 0, 0, 0, 0xff};

char sdcard_acmd41[] = {0x69, 0, 0, 0, 0, 0xff};
char sdcard_acmd41_hcs[] = {0x69, 0x40, 0, 0, 0, 0xff};

char sdcard_cmd58[] = {0x7a, 0, 0, 0, 0, 0xff};

/* 512 bytes block length */
char sdcard_cmd16[] = {0x50, 0, 0, 2, 0, 0xff};

/* Read CSD */
char sdcard_cmd9[] = {0x49, 0, 0, 0, 0, 0xff};


static bool high_capacity;

#ifdef SDCARD_BOUNDARY_128K
/* 128K */
#define SDCARD_BOUNDARY_MASK 0xff
#define SDCARD_BOUNDARY_SIZE 0x100
#else
/* 32K */
#define SDCARD_BOUNDARY_MASK 0x3f
#define SDCARD_BOUNDARY_SIZE 0x40
#endif

unsigned int sdcard_sector;
unsigned int sdcard_offset;
unsigned int sdcard_size; /* defined as number of sectors */

#define SDCARD_IDLE          0
#define SDCARD_WRITE_GAP     1
#define SDCARD_WRITING_BLOCK 2
#define SDCARD_STOPPING      3
#define SDCARD_ERROR         4

unsigned int sdcard_active;

static bool sdcard_command(char *command, unsigned int command_length,
		char *response, unsigned int response_length, bool wait_busy);

bool sdcard_write(unsigned int address, char *buffer, unsigned int length);
bool sdcard_read_csd(char *buffer);
void sdcard_prepare(void);

/* SD card (SPI mode initialisation)

power on

CMD0+
CMD8
if (no response from CMD8) {
	// legacy (MMC) card
	CMD58 (optional, read OCR)  - no or bad response = don't use card
	while (ACMD41(arg 0x00) & in_idle_state_mask)
	    ;
	done
} else {
	// SD card
	if (response from CMD8 was present but invalid
			(check pattern not matched))
		retry CMD8;
	CMD58 (optional, read OCR)
	while (ACMD41(arg HCS=1) & in_idle_state_mask)
	    ;
	CMD58 (Get CCS)
	if (CCS)
		done - high capacity SD card
	else
		done - standard SD card
}


*/

bool init_sdcard(void)
{
	char response[16];
	unsigned int i;

	unsigned int read_bl_len, c_size_mult, c_size;
	unsigned int block_len, mult, blocknr;

	putstr("Initialising SPI\r\n");

	init_spi();

	high_capacity = FALSE;

	putstr("Sending 80 clocks\r\n");

	spi_transaction_start();
	spi_write_array(dummy_block);
	spi_transaction_stop();

	putstr("Sending reset command\r\n");

	if (!sdcard_command(reset_command, sizeof(reset_command),
			response, 1, FALSE))
		return FALSE;

	putstr("Reset command successful. Checking response.\r\n");

	puthex(response[0]);

	putstr("\r\n");

	if (response[0] != 0x01)
		return FALSE;

	putstr("Sending CMD8\r\n");

	if (!sdcard_command(sdcard_cmd8, sizeof(sdcard_cmd8),
			response, 5, FALSE))
	{
		putstr("No response. Legacy device.\r\n");
		/* Legacy device */
		do {
			if (!sdcard_command(sdcard_cmd55, sizeof(sdcard_cmd55),
					response, 1, FALSE))
				return FALSE;
			if (response[0] != 0x01)
				return FALSE;
			if (!sdcard_command(sdcard_acmd41,
					sizeof(sdcard_acmd41),
					response, 1, FALSE))
				return FALSE;
		} while (response[0] & 1);
		putstr("ACMD41 gave us the right response.\r\n");
	} else {
		putstr("We got a response. Not a legacy device.\r\n");
		/* Not legacy device */
		for (i = 1; i < 4; i++) {
			if (response[i] != sdcard_cmd8[i]) {
				/* We should really retry here. Meh. */
				return FALSE;
			}
		}
		putstr("Response OK. Safe to continue.\r\n");
		do {
			if (!sdcard_command(sdcard_cmd55, sizeof(sdcard_cmd55),
					response, 1, FALSE))
				return FALSE;
			if (response[0] != 0x01)
				return FALSE;
			if (!sdcard_command(sdcard_acmd41_hcs,
					sizeof(sdcard_acmd41_hcs),
					response, 1, FALSE))
				return FALSE;
		} while (response[0] & 1);
		putstr("ACMD41 gave us the right response.\r\n");
		if (!sdcard_command(sdcard_cmd58, sizeof(sdcard_cmd58),
				response, 5, FALSE))
			return FALSE;
		putstr("OCR register retrieved.\r\n");
		if ((response[1] & 0x80) == 0)
			return FALSE;
		putstr("Chip isn't still powering up.\r\n");
		if (response[1] & 0x40) {
			putstr("We have a high capacity device.\r\n");
			high_capacity = TRUE;
		} else {
			putstr("We have a low capacity device.\r\n");
			high_capacity = FALSE;
		}
	}

	spi_speedup();

	/* Set block length to 512 */
	if (!sdcard_command(sdcard_cmd16, sizeof(sdcard_cmd16),
			response, 1, FALSE))
		return FALSE;

	putstr("Determining card size.\r\n");

	if (!sdcard_read_csd(response))
		return FALSE;

	putstr("Read CSD\r\n");

	switch ((response[0] & 0xc0) >> 6) {
	case 0:
		/* CSD Version 1.0 */
		read_bl_len = response[5] & 0x0f;
		c_size_mult = ((response[9] & 0x03) << 1) | (response[10] >> 7);
		c_size = ((response[6] & 0x03) << 10)  | (response[7] << 2) |
			(response[8] >> 6);

		block_len = 1<<read_bl_len;
		mult = 1<<(c_size_mult+2);
		blocknr = (c_size+1) * mult;
		sdcard_size = blocknr * block_len / 512;
		break;
	case 1:
		/* CSD Version 2.0 */
		c_size = ((response[7] & 0x3f) << 16) |
			(response[8] << 8) | response[9];
		sdcard_size = (c_size+1) * 1024;
		break;
	default:
		/* Unrecognised CSD version */
		putstr("Unrecognised CSD version\r\n");
		return FALSE;
	}

	putstr("SD initialisation sequence complete.\r\n");
	putstr("size = ");
	putint(sdcard_size / 2);
	putstr("KB\r\n");

	putstr("Initialising logging system.\r\n");
	sdcard_prepare();

	return TRUE;
}

static bool sdcard_command_innards(char *command, unsigned int command_length,
		char *response, unsigned int response_length, bool wait_busy)
{
	char byte;
	unsigned int i;

	spi_write_bytes(command, command_length);

	i = 0;

	do
	{
		byte = spi_read_byte();
		i++;
	} while (((byte & 0x80) != 0) && (i < SDCARD_COMMAND_TIMEOUT));

	if (byte & 0x80)
		return FALSE;

	if (response_length > 0)
		response[0] = byte;

	/* We need to store the response, plus read one extra byte for luck. */
	/* XXX not an extra byte for luck any more */
	for (i = 1; i < response_length; i++) {
		byte = spi_read_byte();
		response[i] = byte;
	}

	if (wait_busy) {
		do {
			byte = spi_read_byte();
		} while (byte == 0);

		spi_write_byte(0xff);
	}

	return TRUE;
}

static bool sdcard_check_data_response(void)
{
	char byte;
	unsigned int i;

	i = 0;

	do
	{
		byte = spi_read_byte();
		i++;
	} while (((byte & 0x11) != 0x01) && (i < SDCARD_COMMAND_TIMEOUT));

	if ((byte & 0x11) != 0x01)
		return FALSE;

	if ((byte & 0x0f) != 0x05) /* Data accepted */
		return FALSE;

	/* Read one more byte for luck */
	byte = spi_read_byte();

	return TRUE;
}

static bool sdcard_command(char *command, unsigned int command_length,
		char *response, unsigned int response_length, bool wait_busy)
{
	bool result;

	spi_transaction_start();

	result = sdcard_command_innards(command, command_length,
		response, response_length, wait_busy);

	spi_transaction_stop();

	return result;
}

static bool sdcard_read_block(char *buffer, unsigned int length)
{
	unsigned int i;
	unsigned int crc_hi;
	unsigned int crc_lo;
	unsigned int crc;

	while (1) {
		char byte = spi_read_byte();
		if (byte == 0xff)
			continue;
		if (byte == 0xfe)
			break;
		if ((byte & 0xf0) == 0)
			if (byte != 0)
				return FALSE;
	}

	/* We need to store the response, plus read one extra byte for luck. */
	for (i = 0; i < length; i++) {
		buffer[i] = spi_read_byte();
	}

	crc_hi = spi_read_byte();
	crc_lo = spi_read_byte();

	crc = (crc_hi << 8) + crc_lo;

	/* XXX check CRC and return FALSE if doesn't match */

	return TRUE;
}

bool sdcard_read(unsigned int address, char *buffer, unsigned int length)
{
	bool valid;
	char response;

	char cmd[6];

	if (!high_capacity)
		address = address * 512;

	cmd[0] = 0x51; /* CMD17 */
	cmd[1] = (address >> 24) & 0xff;
	cmd[2] = (address >> 16) & 0xff;
	cmd[3] = (address >>  8) & 0xff;
	cmd[4] = (address >>  0) & 0xff;
	cmd[5] = 0xff; /* dummy CRC */

	spi_transaction_start();

	if (!sdcard_command_innards(cmd, sizeof(cmd),
			&response, 1, FALSE)) {
		spi_transaction_stop();
		return FALSE;
	}

	if (response != 0) {
		spi_transaction_stop();
		return FALSE;
	}

	valid = sdcard_read_block(buffer, length);

	spi_transaction_stop();

	return valid;
}

bool sdcard_read_csd(char *buffer)
{
	bool valid;
	char response;

	spi_transaction_start();

	if (!sdcard_command_innards(sdcard_cmd9, sizeof(sdcard_cmd9),
			&response, 1, FALSE)) {
		spi_transaction_stop();
		return FALSE;
	}

	if (response != 0) {
		spi_transaction_stop();
		return FALSE;
	}

	valid = sdcard_read_block(buffer, 16);

	spi_transaction_stop();

	return valid;
}

bool sdcard_send_write_cmd(unsigned int address)
{
	char response;

	char cmd[6];

	if (!high_capacity)
		address = address * 512;

	cmd[0] = 0x59; /* CMD25 */
	cmd[1] = (address >> 24) & 0xff;
	cmd[2] = (address >> 16) & 0xff;
	cmd[3] = (address >>  8) & 0xff;
	cmd[4] = (address >>  0) & 0xff;
	cmd[5] = 0xff; /* dummy CRC */

	spi_transaction_start();

	if (!sdcard_command_innards(cmd, sizeof(cmd),
			&response, 1, FALSE)) {
		spi_transaction_stop();
		return FALSE;
	}

	if (response != 0) {
		spi_transaction_stop();
		return FALSE;
	}

	return TRUE;
}

static void sdcard_send_data_token(void)
{
	spi_write_byte(0xfc);
}

static void sdcard_send_stop_token(void)
{
	spi_write_byte(0xfd);
}

#define READ_UINT(b, i) ((b)[(i)] + ((b)[(i)+1] << 8) + \
		((b)[(i)+2] << 16) + ((b)[(i)+3] << 24))

#define WRITE_UINT(b, i, d) \
		do {						\
			(b)[(i)] = (d) & 0xff;			\
			(b)[(i)+1] = ((d) >> 8) & 0xff;		\
			(b)[(i)+2] = ((d) >> 16) & 0xff;	\
			(b)[(i)+3] = ((d) >> 24) & 0xff;	\
		} while (0)


/* We assume that the magic is to be found within this area. If not,
 * we will need to read a bigger area. If the typical record size grows
 * to more than a sector, for example, then we will need to read in multiple
 * sectors where this function is called.
 */
bool sdcard_scan_magic(char *buffer, unsigned int size, unsigned int generation)
{
	unsigned int i;

	for (i = 0; i < size - 8; i++) {
		if ((buffer[i] == (LOG_MAGIC & 0xff)) &&
		    (buffer[i+1] == ((LOG_MAGIC >> 8) & 0xff)) &&
		    (buffer[i+2] == ((LOG_MAGIC >> 16) & 0xff)) &&
		    (buffer[i+3] == ((LOG_MAGIC >> 24) & 0xff)) &&
		    (buffer[i+4] == ((generation >> 0) & 0xff)) &&
		    (buffer[i+5] == ((generation >> 8) & 0xff)) &&
		    (buffer[i+6] == ((generation >> 16) & 0xff)) &&
		    (buffer[i+7] == ((generation >> 24) & 0xff)))
			return TRUE;
	}

	return FALSE;
}

void sdcard_prepare(void)
{
	unsigned int magic;
	unsigned int start_sector;
	unsigned int count;

	if (!sdcard_read(0, log_buffer, 512))
		return;

	magic = READ_UINT(log_buffer, 0);

	if (magic != LOG_MAGIC) {
		unsigned int i;
		for (i = 0; i < 512; i++)
			log_buffer[i] = 0;
		WRITE_UINT(log_buffer, 0, LOG_MAGIC);
		start_sector = SDCARD_BOUNDARY_SIZE;
		log_generation = 0;
		putstr("Did not find header. Formatting.\r\n");
	} else {
		start_sector = READ_UINT(log_buffer, 4);
		log_generation = READ_UINT(log_buffer, 8);
		count = 0;
		putstr("Found header.\r\n");
		putstr("Last started at sector ");
		putint(start_sector);
		putstr(" with generation ");
		putint(log_generation);
		putstr("\r\n");
		while (1) {
			if (!sdcard_read(start_sector, log_buffer+512, 512))
				return;
			/* This needs to change if record length exceeds 512 */
			if (sdcard_scan_magic(log_buffer+512, 512,
					log_generation)) {
				start_sector += SDCARD_BOUNDARY_SIZE;
				if (start_sector >= sdcard_size)
					start_sector = SDCARD_BOUNDARY_SIZE;
			} else {
				break;
			}
			if (count++ > (sdcard_size / SDCARD_BOUNDARY_SIZE)) {
				start_sector = SDCARD_BOUNDARY_SIZE;
				break;
			}
		}
		log_generation++;
	}

	WRITE_UINT(log_buffer, 4, start_sector);
	WRITE_UINT(log_buffer, 8, log_generation);

	putstr("Starting at sector ");
	putint(start_sector);
	putstr(" with generation ");
	putint(log_generation);
	putstr("\r\n");

	if (!sdcard_write(0, log_buffer, 512))
		return;

	sdcard_sector = start_sector;
	sdcard_offset = 0;
	log_enabled = TRUE;
}


static bool sdcard_busy(void)
{
	return (spi_read_byte() != 0xff);
}

static void sdcard_send_dummy_crc(void)
{
	spi_write_byte(0xff);
	spi_write_byte(0xff);
}

void sdcard_poll(void)
{
	if (!log_enabled)
		return;
	if (LOG_BUFFER_EMPTY)
		return;
	log_mark_busy();
	if (sdcard_active == SDCARD_IDLE) {
		spi_transaction_start();
		if (sdcard_busy()) {
			spi_transaction_stop();
			log_mark_idle();
			return;
		}
		putch('C');
		if (sdcard_send_write_cmd(sdcard_sector))
			sdcard_active = SDCARD_WRITE_GAP;
		else {
			spi_transaction_stop();
			sdcard_active = SDCARD_ERROR;
		}
	}
	if (sdcard_active == SDCARD_WRITE_GAP) {
		if (sdcard_busy()) {
			log_mark_idle();
			return;
		}
		sdcard_send_data_token();
		sdcard_active = SDCARD_WRITING_BLOCK;
	}
	if (sdcard_active == SDCARD_WRITING_BLOCK) {
		unsigned int bytes_to_end_of_sector;
		unsigned int i;

		i = LOG_BUFFER_BYTES;
		bytes_to_end_of_sector = 512 - sdcard_offset;
		if (i > bytes_to_end_of_sector)
			i = bytes_to_end_of_sector;
		if (i > 32)
			i = 32;
		sdcard_offset += i;
		while (i--) {
			spi_write_byte(log_get_byte());
		}
		if (sdcard_offset >= 512) {
			sdcard_offset = 0;
			sdcard_sector++;
			sdcard_send_dummy_crc();
			putch('.');
			if (!sdcard_check_data_response()) {
				/* Set state to STOPPING instead? */
				/* How do we test this? */
				spi_transaction_stop();
				sdcard_active = SDCARD_ERROR;
				log_mark_idle();
				return;
			}
			sdcard_active = SDCARD_WRITE_GAP;
			if ((sdcard_sector & SDCARD_BOUNDARY_MASK) == 0) {
				putch('S');
				sdcard_active = SDCARD_STOPPING;
			}
		}
	}
	if (sdcard_active == SDCARD_STOPPING) {
		if (sdcard_busy()) {
			log_mark_idle();
			return;
		}
		sdcard_send_stop_token();
		spi_transaction_stop();
		sdcard_active = SDCARD_IDLE;
	}
	log_mark_idle();
}

bool sdcard_write(unsigned int address, char *buffer, unsigned int length)
{
	unsigned int i;

	spi_transaction_start();

	if (!sdcard_send_write_cmd(address)) {
		spi_transaction_stop();
		return FALSE;
	}

	sdcard_send_data_token();

	for (i = 0; i < length; i++) {
		spi_write_byte(buffer[i]);
	}

	sdcard_send_dummy_crc();
	if (!sdcard_check_data_response()) {
		spi_transaction_stop();
		return FALSE;
	}

	while (sdcard_busy()) ;

	sdcard_send_stop_token();

	while (sdcard_busy()) ;

	spi_transaction_stop();

	return TRUE;
}