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 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         struct pegoda_cmd_hdr *rxhdr = (struct pegoda_cmd_hdr *)rxbuf;
  78
  79         hdr->seq = ++(ph->seq);
  80         hdr->cmd = cmd;
  81         hdr->len = htons(tx_len);
  82         memcpy(txbuf + sizeof(*hdr), tx, tx_len);
  83
  84         printf("tx [%u]: %s\n", tx_len+sizeof(*hdr),
  85                 hexdump(txbuf, tx_len + sizeof(*hdr)));
  86         rc = usb_bulk_write(ph->handle, 0x02, (char *)txbuf,
  87                             tx_len + sizeof(*hdr), 0);
  88         if (rc < 0)
  89                 return rc;
  90
  91         rc = usb_bulk_read(ph->handle, 0x81, (char *)rxbuf, sizeof(rxbuf), 0);
  92         if (rc <= 0)
  93                 return rc;
  94
  95         if (rc != 2) {
  96                 fprintf(stderr, "unexpected: received %u bytes as length?\n");
  97                 return -EIO;
  98         }
  99         printf("len [%u]: %s\n", rc, hexdump(rxbuf, rc));
 100
 101         len_expected = rxbuf[0];
 102
 103         if (len_expected > sizeof(rxbuf))
 104                 return -EIO;
 105
 106         rc = usb_bulk_read(ph->handle, 0x81, (char *)rxbuf, len_expected, 0);
 107         if (rc <= 0)
 108                 return rc;
 109         printf("rx [%u]: %s\n", rc, hexdump(rxbuf, rc));
 110
 111         if (rc < 4)
 112                 return -EIO;
 113
 114         if (rxhdr->seq != hdr->seq)
 115                 return -EIO;
 116
 117         *rx_len = ntohs(rxhdr->len);
 118
 119         memcpy(rx, rxbuf+sizeof(*rxhdr), rc-sizeof(*rxhdr));
 120
 121         return 0;
 122 }
 123
 124 struct pegoda_handle *pegoda_open(void)
 125 {
 126         struct usb_device *pegoda;
 127         unsigned char rbuf[16];
 128         unsigned int rlen = sizeof(rbuf);
 129         struct pegoda_handle *ph;
 130
 131         usb_init();
 132         usb_find_busses();
 133         usb_find_devices();
 134
 135         pegoda = find_device(USB_VENDOR_PHILIPS, USB_DEVICE_PEGODA);
 136
 137         if (!pegoda)
 138                 return NULL;
 139
 140         ph = malloc(sizeof(*ph));
 141         if (!ph)
 142                 return NULL;
 143         memset(ph, 0, sizeof(*ph));
 144
 145         printf("found pegoda, %u configurations\n",
 146                 pegoda->descriptor.bNumConfigurations);
 147
 148         printf("config 2 [nr %u] has %u interfaces\n",
 149                 pegoda->config[1].bConfigurationValue,
 150                 pegoda->config[1].bNumInterfaces);
 151
 152         printf("config 2 interface 0 has %u altsettings\n",
 153                 pegoda->config[1].interface[0].num_altsetting);
 154
 155         ph->handle = usb_open(pegoda);
 156         if (!ph->handle)
 157                 goto out_free;
 158
 159         if (usb_set_configuration(ph->handle, 2))
 160                 goto out_free;
 161
 162         printf("configuration 2 successfully set\n");
 163
 164         if (usb_claim_interface(ph->handle, 0))
 165                 goto out_free;
 166
 167         printf("interface 0 claimed\n");
 168
 169         if (usb_set_altinterface(ph->handle, 1))
 170                 goto out_free;
 171
 172         printf("alt setting 1 selected\n");
 173
 174         pegoda_transcieve(ph, PEGODA_CMD_PCD_CONFIG, NULL, 0, rbuf, &rlen);
 175
 176         return ph;
 177 out_free:
 178         free(ph);
 179         return NULL;
 180 }
 181
 182 /* Transform crypto1 key from generic 6byte into rc632 specific 12byte */
 183 static int
 184 mifare_transform_key(const u_int8_t *key6, u_int8_t *key12)
 185 {
 186         int i;
 187         u_int8_t ln;
 188         u_int8_t hn;
 189
 190         for (i = 0; i < 6; i++) {
 191                 ln = key6[i] & 0x0f;
 192                 hn = key6[i] >> 4;
 193                 key12[i * 2 + 1] = (~ln << 4) | ln;
 194                 key12[i * 2] = (~hn << 4) | hn;
 195         }
 196         return 0;
 197 }
 198
 199 static int pegoda_auth_e2(struct pegoda_handle *ph,
 200                           u_int8_t keynr, u_int8_t sector)
 201 {
 202         unsigned char buf[3];
 203         unsigned char rbuf[16];
 204         unsigned int rlen = sizeof(rbuf);
 205
 206         buf[0] = 0x60;
 207         buf[1] = keynr;         /* key number */
 208         buf[2] = sector;        /* sector */
 209         rlen = sizeof(rbuf);
 210         pegoda_transcieve(ph, PEGODA_CMD_PICC_AUTH, buf, 3, rbuf, &rlen);
 211
 212         /* FIXME: check response */
 213
 214         return 0;
 215 }
 216
 217 static int pegoda_auth_key(struct pegoda_handle *ph,
 218                            u_int8_t sector, const unsigned char *key6)
 219 {
 220         unsigned char buf[1+4+12+1];
 221         unsigned char rbuf[16];
 222         unsigned int rlen = sizeof(rbuf);
 223
 224         buf[0] = 0x60;
 225         memcpy(buf+1, ph->snr, 4);
 226         mifare_transform_key(key6, buf+5);
 227         buf[17] = sector;
 228
 229         pegoda_transcieve(ph, PEGODA_CMD_PICC_AUTH_KEY, buf, 18, rbuf, &rlen);
 230
 231         /* FIXME: check response */
 232
 233         return 0;
 234 }
 235
 236 static int pegoda_read16(struct pegoda_handle *ph,
 237                          u_int8_t page, unsigned char *rx)
 238 {
 239         int rc;
 240         unsigned int rlen = 16;
 241
 242         rc = pegoda_transcieve(ph, PEGODA_CMD_PICC_READ,
 243                                 &page, 1, rx, &rlen);
 244         if (rlen != 16)
 245                 return -EIO;
 246
 247         return 0;
 248 }
 249
 250 int main(int argc, char **argv)
 251 {
 252         unsigned char buf[256];
 253         unsigned char rbuf[256];
 254         unsigned int rlen = sizeof(rbuf);
 255         struct pegoda_handle *ph;
 256
 257         ph = pegoda_open();
 258         if (!ph)
 259                 exit(1);
 260
 261         /* LED off */
 262         buf[0] = 0x00;
 263         rlen = sizeof(rbuf);
 264         pegoda_transcieve(ph, PEGODA_CMD_SWITCH_LED, buf, 1, rbuf, &rlen);
 265
 266         /* anticollision */
 267
 268         buf[0] = 0x26;
 269         rlen = sizeof(rbuf);
 270         pegoda_transcieve(ph, PEGODA_CMD_PICC_COMMON_REQUEST,
 271                           buf, 1, rbuf, &rlen);
 272
 273         buf[0] = 0x93;
 274         memset(buf+1, 0, 5);
 275         rlen = sizeof(rbuf);
 276         pegoda_transcieve(ph, PEGODA_CMD_PICC_CASC_ANTICOLL,
 277                           buf, 6, rbuf, &rlen);
 278
 279         memcpy(ph->snr, rbuf, 4);
 280
 281         buf[0] = 0x93;
 282         memcpy(buf+1, ph->snr, 4);
 283         rlen = sizeof(rbuf);
 284         pegoda_transcieve(ph, PEGODA_CMD_PICC_CASC_SELECT,
 285                           buf, 5, rbuf, &rlen);
 286
 287         pegoda_auth_key(ph, 0, "\xff\xff\xff\xff\xff\xff");
 288         pegoda_read16(ph, 0, rbuf);
 289         printf("read16 = %s\n", hexdump(rbuf, 16));
 290
 291         exit(0);
 292 }