git.rot13.org Git - librfid/blob - pegoda/pegoda.c

   1 /*
   2  *  (C) 2005 by Harald Welte <laforge@gnumonks.org>
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License version 2
   6  *  as published by the Free Software Foundation
   7  *
   8  *  This program is distributed in the hope that it will be useful,
   9  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  *  GNU General Public License for more details.
  12  *
  13  *  You should have received a copy of the GNU General Public License
  14  *  along with this program; if not, write to the Free Software
  15  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  16  */
  17
  18
  19 #include <stdlib.h>
  20 #include <stdio.h>
  21 #include <unistd.h>
  22 #include <string.h>
  23 #include <errno.h>
  24
  25 #include <usb.h>
  26 #include "pegoda.h"
  27
  28 const char *
  29 rfid_hexdump(const void *data, unsigned int len)
  30 {
  31         static char string[1024];
  32         unsigned char *d = (unsigned char *) data;
  33         unsigned int i, left;
  34
  35         string[0] = '\0';
  36         left = sizeof(string);
  37         for (i = 0; len--; i += 3) {
  38                 if (i >= sizeof(string) -4)
  39                         break;
  40                 snprintf(string+i, 4, " %02x", *d++);
  41         }
  42         return string;
  43 }
  44
  45 struct pegoda_handle {
  46         struct usb_dev_handle *handle;
  47         unsigned char seq;
  48         unsigned char snr[4];
  49 };
  50
  51
  52 struct usb_device *find_device(u_int16_t vendor, u_int16_t device)
  53 {
  54         struct usb_bus *bus;
  55
  56         for (bus = usb_get_busses(); bus; bus = bus->next) {
  57                 struct usb_device *dev;
  58                 for (dev = bus->devices; dev; dev = dev->next) {
  59                         if (dev->descriptor.idVendor == vendor &&
  60                             dev->descriptor.idProduct == device) {
  61                                 return dev;
  62                         }
  63                 }
  64         }
  65         return NULL;
  66 }
  67
  68 int pegoda_transcieve(struct pegoda_handle *ph,
  69                       u_int8_t cmd, unsigned char *tx, unsigned int tx_len,
  70                       unsigned char *rx, unsigned int *rx_len)
  71 {
  72         unsigned char txbuf[256];
  73         unsigned char rxbuf[256];
  74         int rc;
  75         unsigned int len_expected;
  76         struct pegoda_cmd_hdr *hdr = (struct pegoda_cmd_hdr *)txbuf;
  77
  78         hdr->seq = ++(ph->seq);
  79         hdr->cmd = cmd;
  80         hdr->len = htons(tx_len);
  81         memcpy(txbuf + sizeof(*hdr), tx, tx_len);
  82
  83         printf("tx [%u]: %s\n", tx_len+sizeof(*hdr),
  84                 rfid_hexdump(txbuf, tx_len + sizeof(*hdr)));
  85         rc = usb_bulk_write(ph->handle, 0x02, (char *)txbuf,
  86                             tx_len + sizeof(*hdr), 0);
  87         if (rc < 0)
  88                 return rc;
  89
  90         rc = usb_bulk_read(ph->handle, 0x81, (char *)rxbuf, sizeof(rxbuf), 0);
  91         if (rc <= 0)
  92                 return rc;
  93
  94         if (rc != 2) {
  95                 fprintf(stderr, "unexpected: received %u bytes as length?\n");
  96                 return -EIO;
  97         }
  98         printf("len [%u]: %s\n", rc, rfid_hexdump(rxbuf, rc));
  99
 100         len_expected = rxbuf[0];
 101
 102         if (len_expected > sizeof(rxbuf))
 103                 return -EIO;
 104
 105         rc = usb_bulk_read(ph->handle, 0x81, (char *)rxbuf, len_expected, 0);
 106         if (rc <= 0)
 107                 return rc;
 108         printf("rx [%u]: %s\n", rc, rfid_hexdump(rxbuf, rc));
 109
 110         memcpy(rx, rxbuf+1, rc-1);
 111         *rx_len = rc - 1;
 112
 113         return 0;
 114 }
 115
 116 struct pegoda_handle *pegoda_open(void)
 117 {
 118         struct usb_device *pegoda;
 119         unsigned char rbuf[16];
 120         unsigned int rlen = sizeof(rbuf);
 121         struct pegoda_handle *ph;
 122
 123         usb_init();
 124         usb_find_busses();
 125         usb_find_devices();
 126
 127         pegoda = find_device(USB_VENDOR_PHILIPS, USB_DEVICE_PEGODA);
 128
 129         if (!pegoda)
 130                 return NULL;
 131
 132         ph = malloc(sizeof(*ph));
 133         if (!ph)
 134                 return NULL;
 135         memset(ph, 0, sizeof(*ph));
 136
 137         printf("found pegoda, %u configurations\n",
 138                 pegoda->descriptor.bNumConfigurations);
 139
 140         printf("config 2 [nr %u] has %u interfaces\n",
 141                 pegoda->config[1].bConfigurationValue,
 142                 pegoda->config[1].bNumInterfaces);
 143
 144         printf("config 2 interface 0 has %u altsettings\n",
 145                 pegoda->config[1].interface[0].num_altsetting);
 146
 147         ph->handle = usb_open(pegoda);
 148         if (!ph->handle)
 149                 goto out_free;
 150
 151         if (usb_set_configuration(ph->handle, 2))
 152                 goto out_free;
 153
 154         printf("configuration 2 successfully set\n");
 155
 156         if (usb_claim_interface(ph->handle, 0))
 157                 goto out_free;
 158
 159         printf("interface 0 claimed\n");
 160
 161         if (usb_set_altinterface(ph->handle, 1))
 162                 goto out_free;
 163
 164         printf("alt setting 1 selected\n");
 165
 166         pegoda_transcieve(ph, PEGODA_CMD_PCD_CONFIG, NULL, 0, rbuf, &rlen);
 167
 168         return ph;
 169 out_free:
 170         free(ph);
 171         return NULL;
 172 }
 173
 174 /* Transform crypto1 key from generic 6byte into rc632 specific 12byte */
 175 static int
 176 mifare_transform_key(const u_int8_t *key6, u_int8_t *key12)
 177 {
 178         int i;
 179         u_int8_t ln;
 180         u_int8_t hn;
 181
 182         for (i = 0; i < 6; i++) {
 183                 ln = key6[i] & 0x0f;
 184                 hn = key6[i] >> 4;
 185                 key12[i * 2 + 1] = (~ln << 4) | ln;
 186                 key12[i * 2] = (~hn << 4) | hn;
 187         }
 188         return 0;
 189 }
 190
 191 static int pegoda_auth_e2(struct pegoda_handle *ph,
 192                           u_int8_t keynr, u_int8_t sector)
 193 {
 194         unsigned char buf[3];
 195         unsigned char rbuf[16];
 196         unsigned int rlen = sizeof(rbuf);
 197
 198         buf[0] = 0x60;
 199         buf[1] = keynr;         /* key number */
 200         buf[2] = sector;        /* sector */
 201         rlen = sizeof(rbuf);
 202         pegoda_transcieve(ph, PEGODA_CMD_PICC_AUTH, buf, 3, rbuf, &rlen);
 203
 204         /* FIXME: check response */
 205
 206         return 0;
 207 }
 208
 209 static int pegoda_auth_key(struct pegoda_handle *ph,
 210                            u_int8_t sector, const unsigned char *key6)
 211 {
 212         unsigned char buf[1+4+12+1];
 213         unsigned char rbuf[16];
 214         unsigned int rlen = sizeof(rbuf);
 215
 216         buf[0] = 0x60;
 217         memcpy(buf+1, ph->snr, 4);
 218         mifare_transform_key(key6, buf+5);
 219         buf[17] = sector;
 220
 221         pegoda_transcieve(ph, PEGODA_CMD_PICC_AUTH_KEY, buf, 18, rbuf, &rlen);
 222
 223         /* FIXME: check response */
 224
 225         return 0;
 226 }
 227
 228 static int pegoda_read16(struct pegoda_handle *ph,
 229                          u_int8_t page, unsigned char *rx)
 230 {
 231         unsigned int rlen = 24;
 232
 233         return pegoda_transcieve(ph, PEGODA_CMD_PICC_READ,
 234                                  &page, 1, rx, &rlen);
 235 }
 236
 237 int main(int argc, char **argv)
 238 {
 239         unsigned char buf[256];
 240         unsigned char rbuf[256];
 241         unsigned int rlen = sizeof(rbuf);
 242         struct pegoda_handle *ph;
 243
 244         ph = pegoda_open();
 245         if (!ph)
 246                 exit(1);
 247
 248         /* LED off */
 249         buf[0] = 0x00;
 250         rlen = sizeof(rbuf);
 251         pegoda_transcieve(ph, PEGODA_CMD_SWITCH_LED, buf, 1, rbuf, &rlen);
 252
 253         /* anticollision */
 254
 255         buf[0] = 0x26;
 256         rlen = sizeof(rbuf);
 257         pegoda_transcieve(ph, PEGODA_CMD_PICC_COMMON_REQUEST,
 258                           buf, 1, rbuf, &rlen);
 259
 260         buf[0] = 0x93;
 261         memset(buf+1, 0, 5);
 262         rlen = sizeof(rbuf);
 263         pegoda_transcieve(ph, PEGODA_CMD_PICC_CASC_ANTICOLL,
 264                           buf, 6, rbuf, &rlen);
 265
 266         memcpy(ph->snr, rbuf+3, 4);
 267
 268         buf[0] = 0x93;
 269         memcpy(buf+1, ph->snr, 4);
 270         rlen = sizeof(rbuf);
 271         pegoda_transcieve(ph, PEGODA_CMD_PICC_CASC_SELECT,
 272                           buf, 5, rbuf, &rlen);
 273
 274         pegoda_auth_key(ph, 0, "\xff\xff\xff\xff\xff\xff");
 275         pegoda_read16(ph, 0, rbuf);
 276
 277         exit(0);
 278 }