1 files changed, 50 insertions, 6 deletions

diff --git a/reader_yhy.c b/reader_yhy.c
index 5174ef9..192e235 100644
--- a/reader_yhy.c
+++ b/reader_yhy.c
@@ -116,6 +116,7 @@ uint8_t read_ntag(nfc_tag_info_t* tag){
 	} while(n <= data[13] / 2);
 
 	//read dynamic data if present
+	tag->dynamic_max = tag->bytes_available - 8 /*4 byte magic, 4 byte length*/ - tag->static_length;
 	tag->dynamic_length = data[15] * tag->granularity;
 	if(tag->dynamic_length){
 		tag->dynamic_data = calloc(tag->dynamic_length, sizeof(uint8_t));
@@ -142,8 +143,47 @@ uint8_t read_ntag(nfc_tag_info_t* tag){
 	return FLAG_TAG_DATA_VALID;
 }
 
-void write_ntag(nfc_tag_info_t* tag){
-	//TODO
+void write_ntag(nfc_tag_info_t* tag, uint8_t flags){
+	size_t n = 0;
+	size_t static_offset = 6;
+	size_t static_blocks = (tag->static_length / tag->granularity) + ((tag->static_length % tag->granularity) ? 1 : 0);
+	size_t dynamic_offset = static_offset + static_blocks;
+	size_t dynamic_blocks = (tag->dynamic_length / tag->granularity) + ((tag->dynamic_length % tag->granularity) ? 1 : 0);
+	uint8_t data[16] = { 0 };
+
+	if(!static_blocks){
+		printf("Invalid tag data requested for write (static length is %ld), aborting\n", tag->static_length);
+		return;
+	}
+
+	//write directory
+	data[0] = static_offset;
+	data[1] = static_blocks;
+	data[2] = dynamic_offset;
+	data[3] = dynamic_blocks;
+	yhy_sync_write(reader_fd, 5, data);
+
+	if(flags & TAG_WRITE_FULL){
+		//write signature
+		data[0] = 0xCB;
+		data[1] = 'N';
+		data[2] = 'F';
+		data[3] = 'C';
+		yhy_sync_write(reader_fd, 4, data);
+
+		for(n = 0; n < static_blocks; n++){
+			memset(data, 0, sizeof(data));
+			memcpy(data, tag->static_data + n * tag->granularity, MIN(tag->static_length - n * tag->granularity, 4));
+			yhy_sync_write(reader_fd, static_offset + n, data);
+		}
+	}
+
+	//write dynamic data
+	for(n = 0; n < dynamic_blocks; n++){
+		memset(data, 0, sizeof(data));
+		memcpy(data, tag->dynamic_data + n * tag->granularity, MIN(tag->dynamic_length - n * tag->granularity, 4));
+		yhy_sync_write(reader_fd, dynamic_offset + n, data);
+	}
 }
 
 uint8_t read_mifare(nfc_tag_info_t* tag){
@@ -151,7 +191,7 @@ uint8_t read_mifare(nfc_tag_info_t* tag){
 	return FLAG_TAG_LOCKED;
 }
 
-void write_mifare(nfc_tag_info_t* tag){
+void write_mifare(nfc_tag_info_t* tag, uint8_t flags){
 	//TODO
 }
 
@@ -197,6 +237,7 @@ int scan(){
 			}
 		}
 
+		flags = TAG_WRITE_FULL;
 		switch(reader_tag_present(0, &card)){
 			case TAG_READ_REQUESTED:
 				//read card data
@@ -208,13 +249,16 @@ int scan(){
 				}
 				reader_tag_present(flags, &card);
 				break;
-			case TAG_WRITE_REQUESTED:
+			case TAG_WRITE_DYNAMIC:
+				flags = TAG_WRITE_DYNAMIC;
+				//fall through
+			case TAG_WRITE_FULL:
 				//write card data
 				if(card.type == tag_ntag){
-					write_ntag(&card);
+					write_ntag(&card, flags);
 				}
 				else if(card.type == tag_mifare1){
-					write_mifare(&card);
+					write_mifare(&card, flags);
 				}
 				break;
 		}