Add WRITE support for ISO15693 (Bjoern Riemer)

git-svn-id: https://svn.gnumonks.org/trunk/librfid@2106 e0336214-984f-0b4b-a45f-81c69e1f0ede

5 files changed, 341 insertions, 57 deletions

diff --git a/include/librfid/rfid_asic_rc632.h b/include/librfid/rfid_asic_rc632.h
index 3ca8469..1400756 100644
--- a/include/librfid/rfid_asic_rc632.h
+++ b/include/librfid/rfid_asic_rc632.h
@@ -30,6 +30,7 @@ struct rfid_asic_handle;
 struct iso14443a_atqa;
 struct iso14443a_anticol_cmd;
 struct iso15693_anticol_cmd;
+struct iso15693_anticol_resp;
 
 struct rfid_asic_rc632 {
 	struct {
diff --git a/include/librfid/rfid_layer2_iso15693.h b/include/librfid/rfid_layer2_iso15693.h
index 495eebb..b678893 100644
--- a/include/librfid/rfid_layer2_iso15693.h
+++ b/include/librfid/rfid_layer2_iso15693.h
@@ -128,6 +128,7 @@ enum iso15693_response_flags {
 enum iso15693_response_errors {
 	RFID_15693_ERR_NOTSUPP	= 0x01,
 	RFID_15693_ERR_INVALID	= 0x02, /* command not recognized */
+    RFID_15693_ERR_NOTSUPP_OPTION = 0x03, /* option not supported*/
 	RFID_15693_ERR_UNKNOWN	= 0x0f, /* unknown error */
 	RFID_15693_ERR_BLOCK_NA	= 0x10, /* block not available */
 	RFID_15693_ERR_BLOCK_LOCKED = 0x11,
diff --git a/src/rfid_asic_rc632.c b/src/rfid_asic_rc632.c
index 1c64558..28bd5a1 100644
--- a/src/rfid_asic_rc632.c
+++ b/src/rfid_asic_rc632.c
@@ -288,7 +288,9 @@ static int rc632_wait_idle_timer(struct rfid_asic_handle *handle)
 		DEBUGP_STATUS_FLAG(stat);
 		if (stat & RC632_STAT_ERR) {
 			u_int8_t err;
-			rc632_reg_read(handle, RC632_REG_ERROR_FLAG, &err);
+			ret = rc632_reg_read(handle, RC632_REG_ERROR_FLAG, &err);
+			if (ret < 0)
+				return ret;
 			DEBUGP_ERROR_FLAG(err);
 			if (err & (RC632_ERR_FLAG_COL_ERR |
 				   RC632_ERR_FLAG_PARITY_ERR |
@@ -355,7 +357,9 @@ rc632_wait_idle(struct rfid_asic_handle *handle, u_int64_t timeout)
 				ret = rc632_reg_read(handle, RC632_REG_INTERRUPT_RQ, &foo);
 				DEBUGP_INTERRUPT_FLAG("irq_rq",foo);
 				/* clear all interrupts */
-				rc632_clear_irqs(handle, 0xff);
+				ret = rc632_clear_irqs(handle, 0xff);
+				if (ret < 0)
+					return ret;
 			}
 		}
 		if (cmd == 0) {
diff --git a/src/rfid_layer2_iso15693.c b/src/rfid_layer2_iso15693.c
index 8f694cd..6f7ea2e 100644
--- a/src/rfid_layer2_iso15693.c
+++ b/src/rfid_layer2_iso15693.c
@@ -6,7 +6,7 @@
 
 /*
  *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License version 2 
+ *  it under the terms of the GNU General Public License version 2
  *  as published by the Free Software Foundation
  *
  *  This program is distributed in the hope that it will be useful,
@@ -18,6 +18,7 @@
  *  along with this program; if not, write to the Free Software
  *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA
  */
+//#define DEBUG_LIBRFID
 
 #include <stdlib.h>
 #include <unistd.h>
@@ -29,17 +30,42 @@
 #include <librfid/rfid_reader.h>
 #include <librfid/rfid_layer2_iso15693.h>
 
-struct iso15693_request_read {
-	struct iso15693_request req;
+/*struct iso15693_request_read {
+	struct iso15693_request head;
 	u_int64_t uid;
 	u_int8_t blocknum;
-} __attribute__ ((packed));
+} __attribute__ ((packed));*/
 
 struct iso15693_request_adressed {
 	struct iso15693_request head;
 	u_int64_t uid;
 } __attribute__ ((packed));
 
+struct iso15693_request_block_addressed {
+	struct iso15693_request head;
+	u_int64_t uid;
+	u_int8_t blocknum;
+	u_int8_t data[0];
+} __attribute__ ((packed));
+
+struct iso15693_request_block_selected {
+	struct iso15693_request head;
+	u_int8_t blocknum;
+	u_int8_t data[0];
+} __attribute__ ((packed));
+
+struct iso15693_err_resp {
+	struct iso15693_response head;
+	u_int8_t error;
+	unsigned char crc[2];
+} __attribute__ ((packed));
+
+struct iso15693_response_sec {
+	struct iso15693_response head;
+	u_int8_t block_sec;
+	u_int8_t data[];
+} __attribute__ ((packed));
+
 #define ISO15693_BLOCK_SIZE_MAX	(256/8)
 #define ISO15693_RESP_SIZE_MAX	(4+ISO15693_BLOCK_SIZE_MAX)
 
@@ -69,6 +95,8 @@ iso15693_get_response_error_name(u_int8_t error){
 			return "ERR_INVALID";
 		case RFID_15693_ERR_UNKNOWN: /* unknown error */
 			return "ERR_UNKNOWN";
+        case RFID_15693_ERR_NOTSUPP_OPTION:
+            return "ERR_NotSuppOpt";
 		case RFID_15693_ERR_BLOCK_NA: /* block not available */
 			return "ERR_BLOCK_N";
 		case RFID_15693_ERR_BLOCK_LOCKED:
@@ -111,49 +139,180 @@ iso15693_transceive_acf(struct rfid_layer2_handle *handle,
 	return rdr->iso15693.transceive_ac(handle->rh, acf, acf_len, resp, rx_len, bit_of_col);
 }
 
-#if 0
 
-static int
+int
 iso15693_read_block(struct rfid_layer2_handle *handle,
-		    u_int8_t blocknr, u_int32_t *data)
+		    u_int8_t blocknr, u_int32_t *data, unsigned int len, 
+		    unsigned char *block_sec_out)
 {
-	int rc;
-	struct iso15693_request_read req;
+	union {
+		struct iso15693_request_block_selected sel;
+		struct iso15693_request_block_addressed addr;
+	} tx_req;
+
+	int ret;
+	unsigned char *errstr;
+	unsigned int rx_len, tx_len,timeout;
 	u_int8_t resp[ISO15693_RESP_SIZE_MAX];
+	struct iso15693_err_resp *rx_err;
+	struct iso15693_response *rx_pkt;
+	struct iso15693_response_sec *rx_pkt_sec;
+
+	rx_pkt_sec = (struct iso15693_response *)&resp[0];
+	rx_pkt = (struct iso15693_response *)&resp[0];
+	rx_err = (struct iso15693_err_resp *)&resp[0];
+
+	memset(&tx_req,0,sizeof(tx_req));
+
+	rx_len = sizeof(resp);
+
+	tx_req.sel.head.command = ISO15693_CMD_READ_BLOCK_SINGLE;
 
-	req.req.flags = 0;
-	req.command = ISO15693_CMD_READ_BLOCK_SINGLE;
-	memcpy(&req.uid, handle->..., ISO15693_UID_LEN);
-	req.blocknum = blocknr;
+	if (handle->priv.iso15693.vicc_fast){
+		tx_req.sel.head.flags |= RFID_15693_F_RATE_HIGH;
+		timeout=iso15693_timing[ISO15693_T_FAST][ISO15693_T4];
+	}else
+		timeout=iso15693_timing[ISO15693_T_SLOW][ISO15693_T4];
 
-	/* FIXME: fill CRC if required */
+	if (handle->priv.iso15693.vicc_two_subc)
+		tx_req.sel.head.flags |= RFID_15693_F_SUBC_TWO;
+
+	if (block_sec_out!=NULL)
+		tx_req.sel.head.flags |= RFID_15693_F4_CUSTOM;
+
+	if (handle->priv.iso15693.state==RFID_15693_STATE_SELECTED) {
+		tx_len = sizeof(struct iso15693_request_block_selected);
+		tx_req.sel.blocknum = blocknr;
+		tx_req.sel.head.flags |= RFID_15693_F4_SELECTED;
+	} else {
+		tx_len = sizeof(struct iso15693_request_block_addressed);
+		memcpy(&tx_req.addr.uid, handle->uid, ISO15693_UID_LEN);
+		tx_req.addr.head.flags |= RFID_15693_F4_ADDRESS;
+		tx_req.addr.blocknum = blocknr;
+	}
+
+	//DEBUGP("sizeof: addr: %d sel:%d\n",sizeof(struct iso15693_request_read_addressed),sizeof(struct iso15693_request_read_selected));
+	DEBUGP("tx_len=%u", tx_len); DEBUGPC(" rx_len=%u\n",rx_len);
 
-	rc = iso15693_transceive(... &req, ...,  );
+	ret = iso15693_transceive(handle, RFID_15693_FRAME, (u_int8_t*)&tx_req,
+				  tx_len, resp, &rx_len, timeout, 0);
 
-	if (rc < 0)
-		return rc;
+	if (ret==-ETIMEDOUT)
+		errstr="(TIMEOUT)";
+	else if (ret==-EIO)
+		errstr="(EIO)";
+	else
+		errstr="";
+	DEBUGP("length: %d rx_len: %d ret: %d%s\n",len,rx_len,ret,errstr);
 
-	memcpy(data, resp+1, rc-1); /* FIXME rc-3 in case of CRC */
+	if (ret < 0)
+		return ret;
 
-	return rc-1;
+	if (rx_len > len+1)
+		return -1;
+	DEBUGP("error_flag: %d", rx_pkt->flags&RFID_15693_RF_ERROR);
+	if (rx_pkt->flags & RFID_15693_RF_ERROR) {
+		DEBUGPC(" -> error: %02x '%s'\n", rx_err->error,
+			iso15693_get_response_error_name(rx_err->error));
+		return -1;
+	} else if (block_sec_out != NULL) {
+		DEBUGPC(" block_sec_stat: 0x%02x\n",rx_pkt_sec->block_sec);
+		memcpy(data, rx_pkt_sec->data, rx_len-2);
+		return rx_len-2;
+	} else {
+		memcpy(data, rx_pkt->data, rx_len-1); /* FIXME rc-3 in case of CRC */
+		return rx_len-1;
+	}
 }
 
-static int
-iso15693_write_block()
+int
+iso15693_write_block(struct rfid_layer2_handle *handle,
+		     u_int8_t blocknr, u_int32_t *data, unsigned int len)
 {
-	struct iso16593_request_read *rreq;
-	u_int32_t buf[sizeof(req)+ISO15693_BLOCK_SIZE_MAX];
+    int ret;
+	unsigned char *errstr;
+	unsigned int rx_len, tx_len,timeout;
 
-	rreq = (struct iso15693_request_read *) req;
+	union{
+		struct iso15693_request_block_selected sel;
+		struct iso15693_request_block_addressed addr;
+        u_int32_t buf[sizeof(struct iso15693_request_block_addressed)+ISO15693_BLOCK_SIZE_MAX];
+	} tx_req;
 
-	rreq->req.flags = ;
-	rreq->req.command = ISO15693_CMD_WRITE_BLOCK_SINGLE;
-	memcpy(rreq->uid, handle->, ISO15693_UID_LEN);
-	rreq->blocknum = blocknr;
-	memcpy(rreq->);
+	u_int8_t resp[ISO15693_RESP_SIZE_MAX];
+	struct iso15693_response *rx_pkt;
+	struct iso15693_err_resp *rx_err;
+
+	rx_pkt = (struct iso15693_response *)&resp[0];
+	rx_err = (struct iso15693_err_resp *)&resp[0];
+	rx_len = sizeof(resp);
+
+	if (len > ISO15693_BLOCK_SIZE_MAX)
+		return -1;
+
+	//return -1;
+
+	memset(&tx_req,0,sizeof(tx_req));
+	tx_req.sel.head.command = ISO15693_CMD_WRITE_BLOCK_SINGLE;
+
+	if (handle->priv.iso15693.vicc_fast) {
+		tx_req.sel.head.flags |= RFID_15693_F_RATE_HIGH;
+		timeout = iso15693_timing[ISO15693_T_FAST][ISO15693_T4_WRITE];
+	} else
+		timeout = iso15693_timing[ISO15693_T_SLOW][ISO15693_T4_WRITE];
+
+	if (handle->priv.iso15693.vicc_two_subc)
+		tx_req.sel.head.flags |= RFID_15693_F_SUBC_TWO;
+
+	if (handle->priv.iso15693.state == RFID_15693_STATE_SELECTED) {
+		tx_len=sizeof(struct iso15693_request_block_selected)+len;
+		tx_req.sel.head.flags |= RFID_15693_F4_SELECTED;
+		tx_req.sel.blocknum = blocknr;
+		memcpy(&tx_req.sel.data,data,len);
+	} else {
+		memcpy(&tx_req.addr.uid, handle->uid, ISO15693_UID_LEN);
+		tx_len=sizeof(struct iso15693_request_block_addressed)+len;
+		tx_req.addr.head.flags |= RFID_15693_F4_ADDRESS;
+		tx_req.addr.blocknum = blocknr;
+		memcpy(&tx_req.addr.data,data,len);
+	}
+
+	//DEBUGP("sizeof: addr: %d sel:%d\n",sizeof(struct iso15693_request_read_addressed),sizeof(struct iso15693_request_read_selected));
+	DEBUGP("tx_len=%u", tx_len); DEBUGPC(" rx_len=%u\n",rx_len);
+
+	ret = iso15693_transceive(handle, RFID_15693_FRAME, (u_int8_t*)&tx_req,
+				  tx_len, resp, &rx_len, timeout, 0);
+
+	if (ret == -ETIMEDOUT)
+		errstr = "(TIMEOUT)";
+	else if (ret == -EIO)
+		errstr = "(EIO)";
+	else
+		errstr = "";
+	DEBUGP("length: %d rx_len: %d ret: %d%s\n",len,rx_len,ret,errstr);
+
+	if (ret < 0)
+		return ret;
+
+	if (rx_len > len+1)
+		return -1;
+	DEBUGP("error_flag: %d", rx_pkt->flags & RFID_15693_RF_ERROR);
+	if (rx_pkt->flags & RFID_15693_RF_ERROR) {
+		DEBUGPC(" -> error: %02x '%s'\n", rx_err->error,
+			iso15693_get_response_error_name(rx_err->error));
+		return -1;
+	} else {
+		//DEBUGPC(" block_sec_stat: 0x%02x\n",rx_pkt->data[0]);
+		//memcpy(data, rx_pkt->data, rx_len-1); /* FIXME rc-3 in case of CRC */
+		//return rx_len-1;
+        return 0;
+	}
 
 }
 
+
+#if 0
+
 static int
 iso15693_lock_block()
 {
@@ -178,13 +337,13 @@ iso15693_build_acf(u_int8_t *target, u_int8_t flags, u_int8_t afi,
 	req->data[i++] = mask_len;
 
 	mask_bytes = mask_len/8 + (mask_len%8)?1:0;
-	mask_p=&req->data[i];
+	mask_p = &req->data[i];
 
 	for (j = 0; j < mask_bytes; j++)
 		req->data[i++] = mask[j];
 
 	byte = 0xFF >> (8-mask_len%8);
-	req->data[i-1]&=byte;
+	req->data[i-1] &= byte;
 
 	DEBUGP("mask_len: %d mask_bytes: %d i: %d return: %d mask:%s\n",
 		mask_len,mask_bytes,i,i + sizeof(*req),rfid_hexdump(mask_p,mask_bytes));
@@ -203,9 +362,9 @@ iso15693_anticol(struct rfid_layer2_handle *handle)
 	} acf;
 
 	struct iso15693_anticol_resp resp;
-		
+
 	u_int8_t boc;
-#define MAX_SLOTS 16	
+#define MAX_SLOTS 16
 	int num_slots = MAX_SLOTS;
 
 	u_int8_t uuid_list[MAX_SLOTS][ISO15693_UID_LEN];
@@ -350,12 +509,12 @@ start_of_ac_loop:
 	return num_valid;
 }
 
-static int
+int
 iso15693_select(struct rfid_layer2_handle *l2h)
 {
 	struct iso15693_request_adressed tx_req;
 	int ret;
-	unsigned int rx_len, tx_len;
+	unsigned int rx_len, tx_len, timeout;
 
 	struct {
 		struct iso15693_response head;
@@ -364,27 +523,39 @@ iso15693_select(struct rfid_layer2_handle *l2h)
 	} rx_buf;
 	rx_len = sizeof(rx_buf);
 
+	if (l2h->priv.iso15693.vicc_fast) {
+		tx_req.head.flags |= RFID_15693_F_RATE_HIGH;
+		timeout = iso15693_timing[ISO15693_T_FAST][ISO15693_T4];
+	} else
+		timeout = iso15693_timing[ISO15693_T_SLOW][ISO15693_T4];
+
 	tx_req.head.command = ISO15693_CMD_SELECT;
 	tx_req.head.flags = RFID_15693_F4_ADDRESS;
+
 	if (l2h->priv.iso15693.vicc_fast)
 		tx_req.head.flags |= RFID_15693_F_RATE_HIGH;
 	if (l2h->priv.iso15693.vicc_two_subc)
 		tx_req.head.flags |= RFID_15693_F_SUBC_TWO;
+
 	memcpy(&tx_req.uid, l2h->uid, ISO15693_UID_LEN);
 	tx_len = sizeof(tx_req);
 
-	DEBUGP("tx_len=%u", tx_len); DEBUGPC(" rx_len=%u\n",rx_len);
+	DEBUGP("tx_len=%u, rx_len=%u\n", tx_len,rx_len);
 
-	DEBUGP("ret: %d%s, error_flag: %d", ret,(ret==-ETIMEDOUT)?"(TIMEOUT)":"",
-			rx_buf.head.flags&RFID_15693_RF_ERROR);
-	if (rx_buf.head.flags&RFID_15693_RF_ERROR){
+	ret = iso15693_transceive(l2h, RFID_15693_FRAME, (u_int8_t*)&tx_req,
+				  tx_len, (u_int8_t*)&rx_buf, &rx_len,timeout ,0);
+
+	DEBUGP("ret: %d%s, rx_len: %d, error_flag: %d", ret,
+		(ret==-ETIMEDOUT)?"(TIMEOUT)":"", rx_len,
+		rx_buf.head.flags&RFID_15693_RF_ERROR);
+	if (rx_buf.head.flags & RFID_15693_RF_ERROR) {
 		DEBUGPC(" -> error: %02x '%s'\n", rx_buf.error,
 			iso15693_get_response_error_name(rx_buf.error));
+		return -1;
+	} else {
+		DEBUGPC(" SELECTED\n");
 		l2h->priv.iso15693.state = RFID_15693_STATE_SELECTED;
 		return 0;
-	}else{
-		DEBUGPC("\n");
-		return -1;
 	}
 }
 
@@ -438,7 +609,7 @@ iso15693_getopt(struct rfid_layer2_handle *handle,
 
 	if (!optlen || !optval || *optlen < sizeof(unsigned int))
 		return -EINVAL;
-	
+
 	*optlen = sizeof(unsigned int);
 
 	switch (optname) {
@@ -495,7 +666,7 @@ iso15693_setopt(struct rfid_layer2_handle *handle, int optname,
 	        const void *optval, unsigned int optlen)
 {
 	unsigned int val;
-	
+
 	if (optlen < sizeof(u_int8_t) || !optval)
 		return -EINVAL;
 
diff --git a/utils/librfid-tool.c b/utils/librfid-tool.c
index c8b59ea..3651e5d 100644
--- a/utils/librfid-tool.c
+++ b/utils/librfid-tool.c
@@ -3,7 +3,7 @@
  * (C) 2005-2008 by Harald Welte <laforge@gnumonks.org>
  *
  *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License version 2 
+ *  it under the terms of the GNU General Public License version 2
  *  as published by the Free Software Foundation
  *
  *  This program is distributed in the hope that it will be useful,
@@ -132,7 +132,7 @@ iso7816_read_binary(unsigned char *buf, unsigned int *len)
 	unsigned char cmd[] = { 0x00, 0xb0, 0x00, 0x00, 0x00 };
 	unsigned char resp[256];
 	unsigned int rlen = sizeof(resp);
-	
+
 	int rv;
 
 	rv = rfid_protocol_transceive(ph, cmd, sizeof(cmd), resp, &rlen, 0, 0);
@@ -241,7 +241,7 @@ mifare_classic_dump(struct rfid_protocol_handle *ph)
 	unsigned int size_len = sizeof(size);
 	int sector, num_sectors;
 
-	if (rfid_protocol_getopt(ph, RFID_OPT_PROTO_SIZE, 
+	if (rfid_protocol_getopt(ph, RFID_OPT_PROTO_SIZE,
 				 &size, &size_len) == 0) {
 		printf("Size: %u bytes\n", size);
 	} else {
@@ -280,13 +280,96 @@ mifare_classic_dump(struct rfid_protocol_handle *ph)
 		if (rc < 0) {
 			printf("mifare auth error\n");
 			exit(1);
-		} else 
+		} else
 			printf("mifare auth succeeded!\n");
 
 		mifare_classic_read_sector(ph, sector);
 	}
 }
 
+void
+iso15693_write(struct rfid_reader_handle *rh,int layer2,int sector,
+                            unsigned char *data, unsigned int len)
+{
+    int rc;
+	unsigned char uid_buf[16];
+	unsigned int uid_len = sizeof(uid_buf);
+
+	if (rh->reader->l2_supported & (1 << layer2)) {
+		l2h = rfid_layer2_init(rh, layer2);
+		if (!l2h) {
+			printf("error during layer2(%d)_init (0=14a,1=14b,3=15)\n",layer2);
+			return;
+		}
+		printf("Layer2 init ok\n");
+		rc = rfid_layer2_open(l2h);
+        if (rc>0){
+			rfid_layer2_getopt(l2h, RFID_OPT_LAYER2_UID, &uid_buf, &uid_len);
+			printf("Layer 2 success (%s)[%d]: '%s'\n", rfid_layer2_name(l2h), uid_len, hexdump(uid_buf, uid_len));
+            rc = iso15693_write_block(l2h,sector,data,len);
+            printf("write>>rc: %d\n",rc);
+
+        }else {
+			printf("error during layer2_open\n");
+			return ;
+		}
+		rfid_layer2_close(l2h);
+		rfid_layer2_fini(l2h);
+    }
+}
+
+void iso15693_dump(struct rfid_reader_handle *rh,int layer2,int sector){
+	unsigned int size;
+	unsigned int size_len = sizeof(size);
+    unsigned char buf[1024];
+    int rc,i;
+	unsigned char uid_buf[16], block_sec;
+	unsigned int uid_len = sizeof(uid_buf);
+
+
+	if (rh->reader->l2_supported & (1 << layer2)) {
+		l2h = rfid_layer2_init(rh, layer2);
+		if (!l2h) {
+			printf("error during layer2(%d)_init (0=14a,1=14b,3=15)\n",layer2);
+			return;
+		}
+		printf("Layer2 init ok\n");
+		rc = rfid_layer2_open(l2h);
+        if (rc>0){
+			rfid_layer2_getopt(l2h, RFID_OPT_LAYER2_UID, &uid_buf, &uid_len);
+			printf("Layer 2 success (%s)[%d]: '%s'\n", rfid_layer2_name(l2h), uid_len, hexdump(uid_buf, uid_len));
+
+			if (sector < 0){
+				if (sector<=-3)
+					iso15693_select(l2h);
+				for(i=0;i<=255;i++){
+					rc = iso15693_read_block(l2h,i,buf,sizeof(buf),&block_sec);
+					if (rc>=0)
+						printf("block[%3d:%02x]sec:0x%0x data(%d): %s\n",i,i,block_sec,rc,rfid_hexdump(buf,rc));
+					else{
+						printf("no data(read_block(%d)>> %d)\n",i,rc);
+						if ((sector == -1)||(sector == -3))
+							break;
+					}
+				}
+			}else{
+				if (sector>255)
+					sector=255;
+				rc = iso15693_read_block(l2h,sector,buf,sizeof(buf));
+				if (rc>=0)
+					printf("block[%d]data(%d): %s\n",i,rc,rfid_hexdump(buf,rc));
+				else
+					printf("no data(read_block(%d)>> %d)\n",i,rc);
+			}
+		} else {
+			printf("error during layer2_open\n");
+			return ;
+		}
+		rfid_layer2_close(l2h);
+		rfid_layer2_fini(l2h);
+	}
+}
+
 static char *proto_names[] = {
 	[RFID_PROTOCOL_TCL] = "tcl",
 	[RFID_PROTOCOL_MIFARE_UL] = "mifare-ultralight",
@@ -363,7 +446,7 @@ static int do_scan(int first)
 	if (rc >= 3) {
 		printf("Protocol success (%s)\n", rfid_protocol_name(ph));
 
-		if (rfid_protocol_getopt(ph, RFID_OPT_PROTO_SIZE, 
+		if (rfid_protocol_getopt(ph, RFID_OPT_PROTO_SIZE,
 					 &size, &size_len) == 0)
 			printf("Size: %u bytes\n", size);
 		size_len = sizeof(size);
@@ -527,6 +610,8 @@ static struct option original_opts[] = {
 	{ "scan-loop", 0, 0, 'S' },
 	{ "dump", 0, 0, 'd' },
 	{ "enum", 0, 0, 'e' },
+	{ "read", 1, 0, 'r' },
+    { "write", 1, 0, 'w'},
 	{ "enum-loop", 1, 0, 'E' },
 	{0, 0, 0, 0}
 };
@@ -599,12 +684,14 @@ void register_module(struct rfidtool_module *me)
 static void help(void)
 {
 	printf( " -s	--scan		scan until first RFID tag is found\n"
-		" -S	--scan-loop	endless scanning loop\n" 
+		" -S	--scan-loop	endless scanning loop\n"
 		" -p	--protocol	{tcl,mifare-ultralight,mifare-classic,tagit,icode}\n"
 		" -l	--layer2	{iso14443a,iso14443b,iso15693,icode1}\n"
 		" -d	--dump		dump rc632 registers\n"
 		" -e	--enum		enumerate all tag's in field \n"
 		" -E	--enum-loop	<delay> (ms) enumerate endless\n"
+		" -r	--read		<secror> read iso15693 sector \n\t\t\t(-1:0-255 stop on error, -2: 0-255 no stop)\n"
+        " -w	--write		<sector> write to iso15693 sector data: 01:02:03:04\n"
 		" -h	--help\n");
 }
 
@@ -619,7 +706,7 @@ int main(int argc, char **argv)
 #else /*__MINGW32__*/
 	program_name = basename(argv[0]);
 #endif/*__MINGW32__*/
-	
+
 	printf("%s - (C) 2005-2008 by Harald Welte\n"
 	       "This program is Free Software and has "
 	       "ABSOLUTELY NO WARRANTY\n\n", program_name);
@@ -629,11 +716,31 @@ int main(int argc, char **argv)
 
 	while (1) {
 		int c, option_index = 0;
-		c = getopt_long(argc, argv, "hp:l:sSdeE:", opts, &option_index);
+		c = getopt_long(argc, argv, "hp:l:sSdeE:r:w:", opts, &option_index);
 		if (c == -1)
 			break;
 
 		switch (c) {
+        case 'w':
+            //hexread(key, optarg, strlen(optarg));
+            i = strtol(optarg, NULL, 10);
+			if (reader_init() < 0)
+				exit(1);
+            layer2 = RFID_LAYER2_ISO15693;
+            iso15693_write(rh,layer2,i,"\x1\x2\x3\x4",4);
+			rfid_reader_close(rh);
+			exit(0);
+            break;
+		case 'r':
+            i = strtol(optarg, NULL, 10);
+			if (reader_init() < 0)
+				exit(1);
+			//if (layer2 < 0)
+            layer2 = RFID_LAYER2_ISO15693;
+			iso15693_dump(rh,layer2,i);
+			rfid_reader_close(rh);
+			exit(0);
+			break;
 		case 'E':
 			i = strtol(optarg, NULL, 10);
 
@@ -677,7 +784,7 @@ int main(int argc, char **argv)
 		case 'p':
 			protocol = proto_by_name(optarg);
 			if (protocol < 0) {
-				fprintf(stderr, "unknown protocol `%s'\n", 
+				fprintf(stderr, "unknown protocol `%s'\n",
 					optarg);
 				exit(2);
 			}
@@ -715,7 +822,7 @@ int main(int argc, char **argv)
 		fprintf(stderr, "you have to specify --layer2\n");
 		exit(2);
 	}
-	
+
 	if (reader_init() < 0)
 		exit(1);
 
@@ -805,6 +912,6 @@ int main(int argc, char **argv)
 	}
 
 	rfid_reader_close(rh);
-	
+
 	exit(0);
 }