cleanup

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

2 files changed, 131 insertions, 74 deletions

diff --git a/pegoda/pegoda.c b/pegoda/pegoda.c
index ab41136..56f425e 100644
--- a/pegoda/pegoda.c
+++ b/pegoda/pegoda.c
@@ -42,6 +42,12 @@ rfid_hexdump(const void *data, unsigned int len)
 	return string;
 }
 
+struct pegoda_handle {
+	struct usb_dev_handle *handle;
+	unsigned char seq;
+	unsigned char snr[4];
+};
+
 
 struct usb_device *find_device(u_int16_t vendor, u_int16_t device)
 {
@@ -50,8 +56,6 @@ struct usb_device *find_device(u_int16_t vendor, u_int16_t device)
 	for (bus = usb_get_busses(); bus; bus = bus->next) {
 		struct usb_device *dev;
 		for (dev = bus->devices; dev; dev = dev->next) {
-			printf("vend 0x%x dev 0x%x\n",
-				dev->descriptor.idVendor, dev->descriptor.idProduct);
 			if (dev->descriptor.idVendor == vendor &&
 			    dev->descriptor.idProduct == device) {
 				return dev;
@@ -61,30 +65,29 @@ struct usb_device *find_device(u_int16_t vendor, u_int16_t device)
 	return NULL;
 }
 
-static unsigned char seq = 0x00;
-static struct usb_dev_handle *pegoda_handle;
-
-int pegoda_transcieve(u_int8_t cmd, unsigned char *tx, unsigned int tx_len,
+int pegoda_transcieve(struct pegoda_handle *ph,
+		      u_int8_t cmd, unsigned char *tx, unsigned int tx_len,
 		      unsigned char *rx, unsigned int *rx_len)
 {
 	unsigned char txbuf[256];
 	unsigned char rxbuf[256];
 	int rc;
 	unsigned int len_expected;
-	struct pegoda_cmd_hdr *hdr = txbuf;
+	struct pegoda_cmd_hdr *hdr = (struct pegoda_cmd_hdr *)txbuf;
 
-	hdr->seq = ++seq;
+	hdr->seq = ++(ph->seq);
 	hdr->cmd = cmd;
 	hdr->len = htons(tx_len);
 	memcpy(txbuf + sizeof(*hdr), tx, tx_len);
 
-	printf("tx [%u]: %s\n", tx_len+sizeof(*hdr), rfid_hexdump(txbuf, tx_len + sizeof(*hdr)));
-	rc = usb_bulk_write(pegoda_handle, 0x02, (char *)txbuf,
+	printf("tx [%u]: %s\n", tx_len+sizeof(*hdr),
+		rfid_hexdump(txbuf, tx_len + sizeof(*hdr)));
+	rc = usb_bulk_write(ph->handle, 0x02, (char *)txbuf,
 			    tx_len + sizeof(*hdr), 0);
 	if (rc < 0)
 		return rc;
 
-	rc = usb_bulk_read(pegoda_handle, 0x81, (char *)rxbuf, sizeof(rxbuf), 0);
+	rc = usb_bulk_read(ph->handle, 0x81, (char *)rxbuf, sizeof(rxbuf), 0);
 	if (rc <= 0)
 		return rc;
 
@@ -99,7 +102,7 @@ int pegoda_transcieve(u_int8_t cmd, unsigned char *tx, unsigned int tx_len,
 	if (len_expected > sizeof(rxbuf))
 		return -EIO;
 
-	rc = usb_bulk_read(pegoda_handle, 0x81, (char *)rxbuf, len_expected, 0);
+	rc = usb_bulk_read(ph->handle, 0x81, (char *)rxbuf, len_expected, 0);
 	if (rc <= 0)
 		return rc;
 	printf("rx [%u]: %s\n", rc, rfid_hexdump(rxbuf, rc));
@@ -110,9 +113,67 @@ int pegoda_transcieve(u_int8_t cmd, unsigned char *tx, unsigned int tx_len,
 	return 0;
 }
 
+struct pegoda_handle *pegoda_open(void)
+{
+	struct usb_device *pegoda;
+	unsigned char rbuf[16];
+	unsigned int rlen = sizeof(rbuf);
+	struct pegoda_handle *ph;
+
+	usb_init();
+	usb_find_busses();
+	usb_find_devices();
+
+	pegoda = find_device(USB_VENDOR_PHILIPS, USB_DEVICE_PEGODA);
+
+	if (!pegoda)
+		return NULL;
+
+	ph = malloc(sizeof(*ph));
+	if (!ph)
+		return NULL;
+	memset(ph, 0, sizeof(*ph));
+
+	printf("found pegoda, %u configurations\n",
+		pegoda->descriptor.bNumConfigurations);
+
+	printf("config 2 [nr %u] has %u interfaces\n",
+		pegoda->config[1].bConfigurationValue,
+		pegoda->config[1].bNumInterfaces);
+
+	printf("config 2 interface 0 has %u altsettings\n",
+		pegoda->config[1].interface[0].num_altsetting);
+
+	ph->handle = usb_open(pegoda);
+	if (!ph->handle) 
+		goto out_free;
+
+	if (usb_set_configuration(ph->handle, 2))
+		goto out_free;
+
+	printf("configuration 2 successfully set\n");
+
+	if (usb_claim_interface(ph->handle, 0))
+		goto out_free;
+
+	printf("interface 0 claimed\n");
+
+	if (usb_set_altinterface(ph->handle, 1))
+		goto out_free;
+
+	printf("alt setting 1 selected\n");
+
+	pegoda_transcieve(ph, PEGODA_CMD_PCD_CONFIG, NULL, 0, rbuf, &rlen);
+
+	return ph;
+out_free:
+	free(ph);
+	return NULL;
+}
+
 /* Transform crypto1 key from generic 6byte into rc632 specific 12byte */
 static int
-rc632_mifare_transform_key(const u_int8_t *key6, u_int8_t *key12)
+mifare_transform_key(const u_int8_t *key6, u_int8_t *key12)
 {
 	int i;
 	u_int8_t ln;
@@ -127,94 +188,91 @@ rc632_mifare_transform_key(const u_int8_t *key6, u_int8_t *key12)
 	return 0;
 }
 
-
-int main(int argc, char **argv)
+static int pegoda_auth_e2(struct pegoda_handle *ph,
+			  u_int8_t keynr, u_int8_t sector)
 {
-	struct usb_device *pegoda;
-	unsigned char buf[256];
-	unsigned char rbuf[256];
+	unsigned char buf[3];
+	unsigned char rbuf[16];
 	unsigned int rlen = sizeof(rbuf);
-	unsigned char snr[4];
 
-	usb_init();
-	usb_find_busses();
-	usb_find_devices();
+	buf[0] = 0x60;
+	buf[1] = keynr;		/* key number */
+	buf[2] = sector;	/* sector */
+	rlen = sizeof(rbuf);
+	pegoda_transcieve(ph, PEGODA_CMD_PICC_AUTH, buf, 3, rbuf, &rlen);
 
-	pegoda = find_device(USB_VENDOR_PHILIPS, USB_DEVICE_PEGODA);
+	/* FIXME: check response */
 
-	if (!pegoda)
-		exit(2);
+	return 0;
+}
 
-	printf("found pegoda, %u configurations\n",
-		pegoda->descriptor.bNumConfigurations);
+static int pegoda_auth_key(struct pegoda_handle *ph,
+			   u_int8_t sector, const unsigned char *key6)
+{
+	unsigned char buf[1+4+12+1];
+	unsigned char rbuf[16];
+	unsigned int rlen = sizeof(rbuf);
 
-	printf("config 2 [nr %u] has %u interfaces\n",
-		pegoda->config[1].bConfigurationValue,
-		pegoda->config[1].bNumInterfaces);
+	buf[0] = 0x60;
+	memcpy(buf+1, ph->snr, 4);
+	mifare_transform_key(key6, buf+5);
+	buf[17] = sector;
 
-	printf("config 2 interface 0 has %u altsettings\n",
-		pegoda->config[1].interface[0].num_altsetting);
+	pegoda_transcieve(ph, PEGODA_CMD_PICC_AUTH_KEY, buf, 18, rbuf, &rlen);
 
-	pegoda_handle = usb_open(pegoda);
-	if (!pegoda_handle)
-		exit(1);
+	/* FIXME: check response */
 
-	if (usb_set_configuration(pegoda_handle, 2))
-		exit(1);
+	return 0;
+}
 
-	printf("configuration 2 successfully set\n");
+static int pegoda_read16(struct pegoda_handle *ph,
+			 u_int8_t page, unsigned char *rx)
+{
+	unsigned int rlen = 24;
 
-	if (usb_claim_interface(pegoda_handle, 0))
-		exit(1);
+	return pegoda_transcieve(ph, PEGODA_CMD_PICC_READ,
+				 &page, 1, rx, &rlen);
+}
 
-	printf("interface 0 claimed\n");
+int main(int argc, char **argv)
+{
+	unsigned char buf[256];
+	unsigned char rbuf[256];
+	unsigned int rlen = sizeof(rbuf);
+	struct pegoda_handle *ph;
 
-	if (usb_set_altinterface(pegoda_handle, 1))
+	ph = pegoda_open();
+	if (!ph)
 		exit(1);
 
-	printf("alt setting 1 selected\n");
+	/* LED off */
+	buf[0] = 0x00;
+	rlen = sizeof(rbuf);
+	pegoda_transcieve(ph, PEGODA_CMD_SWITCH_LED, buf, 1, rbuf, &rlen);
 
-	pegoda_transcieve(PEGODA_CMD_PCD_CONFIG, NULL, 0, rbuf, &rlen);
+	/* anticollision */
 
 	buf[0] = 0x26;
 	rlen = sizeof(rbuf);
-	pegoda_transcieve(PEGODA_CMD_PICC_COMMON_REQUEST, buf, 1, rbuf, &rlen);
+	pegoda_transcieve(ph, PEGODA_CMD_PICC_COMMON_REQUEST, 
+			  buf, 1, rbuf, &rlen);
 
 	buf[0] = 0x93;
 	memset(buf+1, 0, 5);
 	rlen = sizeof(rbuf);
-	pegoda_transcieve(PEGODA_CMD_PICC_CASC_ANTICOLL, buf, 6, rbuf, &rlen);
+	pegoda_transcieve(ph, PEGODA_CMD_PICC_CASC_ANTICOLL, 
+			  buf, 6, rbuf, &rlen);
 
-	memcpy(snr, rbuf+3, 4);
+	memcpy(ph->snr, rbuf+3, 4);
 
 	buf[0] = 0x93;
-	memcpy(buf+1, snr, 4);
-	rlen = sizeof(rbuf);
-	pegoda_transcieve(PEGODA_CMD_PICC_CASC_SELECT, buf, 5, rbuf, &rlen);
-	
-	buf[0] = 0x60;
-#if 0
-	buf[1] = 0x00;	/* key number */
-	buf[2] = 0x00;	/* sector */
+	memcpy(buf+1, ph->snr, 4);
 	rlen = sizeof(rbuf);
-	pegoda_transcieve(PEGODA_CMD_PICC_AUTH, buf, 3, rbuf, &rlen);
-#else
-	memcpy(buf+1, snr, 4);
-	{ 
-		u_int8_t key6[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
-		//u_int8_t key6[6] = { 0xa1, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6 };
-		u_int8_t key12[12];
-
-		rc632_mifare_transform_key(key6, key12);
-
-		memcpy(buf+5, key12, 12);
-		buf[17] = 0x00; /* sector */
-	}
-	pegoda_transcieve(PEGODA_CMD_PICC_AUTH_KEY, buf, 18, rbuf, &rlen);
-#endif
-
-	buf[0] = 0x00; /* sector */
-	pegoda_transcieve(PEGODA_CMD_PICC_READ, buf, 1, rbuf, &rlen);
+	pegoda_transcieve(ph, PEGODA_CMD_PICC_CASC_SELECT, 
+			  buf, 5, rbuf, &rlen);
 
+	pegoda_auth_key(ph, 0, "\xff\xff\xff\xff\xff\xff");
+	pegoda_read16(ph, 0, rbuf);
+	
 	exit(0);
 }
diff --git a/pegoda/pegoda.h b/pegoda/pegoda.h
index b957899..236dc0a 100644
--- a/pegoda/pegoda.h
+++ b/pegoda/pegoda.h
@@ -79,7 +79,6 @@ enum {
 	PEGODA_CMD_PICC_ACTTIVATION		= 0x4a,
 	PEGODA_CMD_PCD_ENABLE_HIGH_BAUD_RATES	= 0x4b,
 
-
 	PEGODA_CMD_SWITCH_LED			= 0x60,
 	PEGODA_CMD_TEST_FLASH_NR		= 0x61,
 	PEGODA_CMD_START_DOWNLOAD		= 0x62,