nfc.c

   1 #include "nfc.h"
   2
   3 #define CMD_BYTE 0x20; /* all cmnds start with this byte */
   4 #define ATS_BYTE 0x21; /* cmd to return the ats */
   5 #define PRS_BYTE 0x22; /* cmd to check for the presence of a icc */
   6
   7 char cmd[2]; /* commands */
   8 int scan_sock; /* socket to scan for devices */
   9 int com_sock; /* socket for communication */
  10 fd_set socks; /* needed for select call to check for closed connection */
  11 struct timeval timeout; /* needed for select */
  12 inquiry_info *devices = NULL; /* holds found device */
  13 int useL2CAP = 0;
  14
  15 //uint32_t svc_uuid_int32 = 0x1101; /* UUID of the service */
  16 //uint32_t svc_uuid_int[] = {0x0100, 0, 0, 0x34fb};
  17 uint32_t svc_uuid_int32 = 0x0100;
  18 //uint16_t svc_uuid_int16 = 0x1101;
  19
  20 const char *svc_name = "SPPServer1";
  21
  22 /* clean up */
  23 void closeSocket()
  24 {
  25         if (com_sock > 0)
  26         {
  27                 close(com_sock);
  28                 com_sock = -1;
  29         }
  30
  31         free(devices);
  32         if (scan_sock > 0)
  33         {
  34                 close(scan_sock);
  35         }
  36 }
  37
  38 /* scan for devices */
  39 RESPONSECODE getDevice()
  40 {
  41         RESPONSECODE rv;
  42
  43         int max_rsp, num_rsp;
  44         int adapter_id, len, flags;
  45         int i;
  46         char addr[19] = { 0 };
  47         char name[248] = { 0 };
  48
  49         adapter_id = hci_get_route(NULL);
  50         if (adapter_id < 0)
  51         {
  52                 fprintf(stderr, "couldn't get adapter: error code %d: %s\n",
  53                                 errno, strerror(errno));
  54                 return IFD_COMMUNICATION_ERROR;
  55         }
  56
  57         scan_sock = hci_open_dev(adapter_id);
  58         if (scan_sock < 0)
  59         {
  60                 Log1(PCSC_LOG_CRITICAL, "opening socket failed");
  61                 return IFD_COMMUNICATION_ERROR;
  62         }
  63
  64         len = 8;
  65         max_rsp = 255;
  66         flags = IREQ_CACHE_FLUSH;
  67         devices = (inquiry_info*) malloc(max_rsp * sizeof(inquiry_info));
  68
  69         // scan
  70         num_rsp = hci_inquiry(adapter_id, len, max_rsp, NULL, &devices, flags);
  71
  72         if (num_rsp < 0)
  73         {
  74                 Log1(PCSC_LOG_CRITICAL, "hci_inquiry failed");
  75                 return IFD_COMMUNICATION_ERROR;
  76         }
  77
  78         if (num_rsp == 0)
  79         {
  80                 Log1(PCSC_LOG_CRITICAL, "no devices found");
  81                 return IFD_COMMUNICATION_ERROR;
  82         }
  83
  84         // try to find the service on every device
  85         for (i = 0; i < num_rsp; ++i)
  86         {
  87                 ba2str(&(devices + i)->bdaddr, addr);
  88                 memset(name, 0, sizeof(name));
  89                 if (0 != hci_read_remote_name(scan_sock, &(devices + i)->bdaddr,
  90                                                                           sizeof(name), name, 0))
  91                 {
  92                         strcpy(name, "[unknown]");
  93                 }
  94                 printf("trying: %s %s\n", addr, name);
  95
  96                 // return first found
  97                 if (getService(devices->bdaddr) == IFD_SUCCESS)
  98                 {
  99                         return IFD_SUCCESS;
 100                 }
 101         }
 102         return IFD_COMMUNICATION_ERROR;
 103 }
 104
 105 /* entry point for practical use with bt-address as string */
 106 RESPONSECODE getServiceByName(char* strAddr)
 107 {
 108         bdaddr_t target;
 109         str2ba(strAddr, &target);
 110         return getService(target);
 111 }
 112
 113 /* connect to the service on the specified device */
 114 RESPONSECODE getService(bdaddr_t target)
 115 {
 116         // service discovery
 117         int status_sdp;
 118         uuid_t svc_uuid;
 119         sdp_list_t *response_list, *search_list, *attrid_list;
 120         sdp_session_t *session = 0;
 121         uint32_t range = 0x0000ffff;
 122         uint16_t port = 0;
 123
 124         // connect to the sdp server running on the remote machine
 125         session = sdp_connect(BDADDR_ANY, &target, 0);
 126
 127         if (session == NULL)
 128         {
 129                 fprintf(stderr, "couldn't get session: error code %d: %s\n",
 130                                 errno, strerror(errno));
 131                 return IFD_COMMUNICATION_ERROR;
 132         }
 133
 134         //sdp_uuid128_create(&svc_uuid, &svc_uuid_int);
 135         sdp_uuid32_create(&svc_uuid, svc_uuid_int32);
 136         search_list = sdp_list_append(0, &svc_uuid);
 137         attrid_list = sdp_list_append(0, &range);
 138
 139         // get a list of service records that have UUID 0x1101
 140         response_list = NULL;
 141         status_sdp = sdp_service_search_attr_req(session, search_list,
 142                                 SDP_ATTR_REQ_RANGE, attrid_list, &response_list);
 143
 144         if (status_sdp == 0)
 145         {
 146                 sdp_list_t *proto_list = NULL;
 147                 sdp_list_t *r = response_list;
 148
 149                 if (r == NULL)
 150                 {
 151                         Log1(PCSC_LOG_DEBUG, "no records matched");
 152                 }
 153
 154                 // go through each of the service records
 155                 for (; r; r = r->next)
 156                 {
 157                         sdp_record_t *rec = (sdp_record_t*) r->data;
 158
 159                         // get a list of the protocol sequences
 160                         if (sdp_get_access_protos(rec, &proto_list) == 0)
 161                         {
 162                                 sdp_data_t *d = sdp_data_get(rec, SDP_ATTR_SVCNAME_PRIMARY);
 163                                 if (d)
 164                                 {
 165                                         //printf("Service Name: %s\n", d->val.str);
 166
 167                                         if (strncmp(d->val.str, svc_name, 10) == 0)
 168                                         {
 169                                                 // get the RFCOMM port number
 170                                                 port = sdp_get_proto_port(proto_list, RFCOMM_UUID);
 171                                                 if (port == 0)
 172                                                 {
 173                                                         Log1(PCSC_LOG_DEBUG, "trying L2CAP");
 174
 175                                                         // get the L2CAP port number
 176                                                         port = sdp_get_proto_port(proto_list, L2CAP_UUID);
 177                                                         useL2CAP = 1;
 178                                                 }
 179                                                 sdp_list_free(proto_list, 0);
 180                                         }
 181                                 }
 182                         }
 183                         else
 184                         {
 185                                 fprintf(stderr, "couldn't get access protocol: error code %d: %s\n",
 186                                         errno, strerror(errno));
 187                         }
 188                         sdp_record_free(rec);
 189                 }
 190         }
 191         else
 192         {
 193                 fprintf(stderr, "couldn't connect to sdp: error code %d: %s\n",
 194                         errno, strerror(errno));
 195         }
 196
 197         sdp_list_free(response_list, 0);
 198         sdp_list_free(search_list, 0);
 199         sdp_list_free(attrid_list, 0);
 200         sdp_close(session);
 201
 202         if (port != 0)
 203         {
 204                 // socket connect
 205                 int status = -1;
 206
 207                 if (useL2CAP < 1)
 208                 {
 209                         Log2(PCSC_LOG_DEBUG, "found service running on RFCOMM port %d", port);
 210                         // allocate socket
 211                         com_sock = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
 212
 213                         struct sockaddr_rc addrs = { 0 };
 214
 215                         // set the connection parameters (who to connect to)
 216                         addrs.rc_family = AF_BLUETOOTH;
 217                         addrs.rc_channel = port;
 218                         addrs.rc_bdaddr = target;
 219
 220                         // connect to server
 221                         status = connect(com_sock, (struct sockaddr *)&addrs, sizeof(addrs));
 222                 }
 223                 else
 224                 {
 225                         Log2(PCSC_LOG_DEBUG, "found service running on L2CAP psm %d", port);
 226                         // allocate socket
 227                         com_sock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
 228
 229                         struct sockaddr_l2 addrs = { 0 };
 230
 231                         // set the connection parameters (who to connect to)
 232                         addrs.l2_family = AF_BLUETOOTH;
 233                         addrs.l2_psm = htobs(port);
 234                         addrs.l2_bdaddr = target;
 235
 236                         // connect to server
 237                         status = connect(com_sock, (struct sockaddr *)&addrs, sizeof(addrs));
 238
 239                         // set MTU
 240                         set_l2cap_mtu(com_sock, 257);
 241                 }
 242
 243                 // return success
 244                 if (0 == status)
 245                 {
 246                         return IFD_SUCCESS;
 247                 }
 248         }
 249         return IFD_COMMUNICATION_ERROR;
 250 }
 251
 252 int set_l2cap_mtu(int sock, uint16_t mtu)
 253 {
 254         struct l2cap_options opts;
 255         int optlen = sizeof(opts);
 256         int status = getsockopt(sock, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen);
 257
 258         if (status == 0)
 259         {
 260                 opts.omtu = opts.imtu = mtu;
 261                 status = setsockopt(sock, SOL_L2CAP, L2CAP_OPTIONS, &opts, optlen);
 262         }
 263
 264         return status;
 265 }
 266
 267 /* wrap data, transmit to the device and get answer */
 268 RESPONSECODE sendData(PUCHAR TxBuffer, DWORD TxLength,
 269                                  PUCHAR RxBuffer, PDWORD RxLength, int wait)
 270 {
 271         RESPONSECODE rv;
 272         if (com_sock < 0)
 273         {
 274                 return IFD_COMMUNICATION_ERROR;
 275         }
 276         char apdu[TxLength + 2];
 277         int i, j, response_length, n = 0;
 278         int n2 = 0;
 279
 280         // always use first 2 bytes byte as length of following data
 281         apdu[0] = TxLength >> 8 & 0xFF;
 282         apdu[1] = TxLength & 0xFF;
 283
 284         for (i = 0; i < TxLength; ++i)
 285         {
 286                 apdu[i + 2] = *(TxBuffer + i);
 287         }
 288
 289         //// check if write is possible
 290         //FD_ZERO(&socks);
 291         //FD_SET(com_sock, &socks);
 292         ////sock_max = s + 1;
 293         //n = select(com_sock + 1, (fd_set *) 0, &socks, (fd_set *) 0, &timeout);
 294         //if (n < 1)
 295         //{
 296         //      return IFD_COMMUNICATION_ERROR;
 297         //}
 298
 299     send(com_sock, apdu, TxLength + 2, 0);
 300
 301         Log2(PCSC_LOG_DEBUG, "wrote %d data bytes", TxLength);
 302         char buffer[65538];
 303
 304         n = 0;
 305         if (wait > 0)
 306         {
 307                 // use select to recognize broken connection
 308                 timeout.tv_sec = 1;
 309                 timeout.tv_usec = 0;
 310                 FD_ZERO(&socks);
 311                 FD_SET(com_sock, &socks);
 312                 n = select(com_sock + 1, &socks, (fd_set *) 0, (fd_set *) 0, &timeout);
 313
 314                 if (n < 0)
 315                 {
 316                         return IFD_COMMUNICATION_ERROR;
 317                 }
 318                 else if (n == 0 || !FD_ISSET(com_sock, &socks))
 319                 {
 320                         *RxLength = 1;
 321                         *RxBuffer = 0;
 322                     return IFD_SUCCESS;
 323                 }
 324                 else
 325                 {
 326                         n = recv(com_sock, buffer, sizeof(buffer), 0);
 327                 }
 328         }
 329         else
 330         {
 331                 n = recv(com_sock, buffer, sizeof(buffer), 0);
 332         }
 333
 334         if (n > 0)
 335         {
 336             if (n == 1)
 337             {
 338                     Log1(PCSC_LOG_DEBUG, "1 byte error response");
 339                     // handle errors
 340                     return IFD_COMMUNICATION_ERROR;
 341             }
 342
 343             Log2(PCSC_LOG_DEBUG, "got %d response bytes", n);
 344             // unwrap response
 345                 response_length = (int) (0xFF & buffer[0]) << 8;
 346                 response_length += (int) (0xFF & buffer[1]);
 347
 348             if (response_length != n - 2)
 349             {
 350                         *RxLength = response_length;
 351
 352                         i = 0;
 353                         n2 = n;
 354
 355                         for (j = 2; j < n2; ++i)
 356                         {
 357                                 *(RxBuffer + i) = buffer[j];
 358                                 ++j;
 359                         }
 360
 361                     // try to read more
 362                         while (n - 2 < response_length)
 363                         {
 364                                 n2 = 0;
 365
 366                                 //Log1(PCSC_LOG_DEBUG, "rereceive");
 367
 368                                 n2 = recv(com_sock, buffer, sizeof(buffer), 0);
 369
 370                                 if (n2 > 0)
 371                                 {
 372                                         //Log2(PCSC_LOG_DEBUG, "got %d additional response bytes", n2);
 373                                         n += n2;
 374
 375                                         for (j = 0; j < n2; ++i)
 376                                         {
 377                                                 *(RxBuffer + i) = buffer[j];
 378                                                 ++j;
 379                                         }
 380                                 }
 381                                 else
 382                                 {
 383                                         Log3(PCSC_LOG_ERROR, "response length wrong, is: %d  should be: %d",
 384                                                 n - 2, response_length);
 385                                 return IFD_COMMUNICATION_ERROR;
 386                                 }
 387                         }
 388
 389                     //Log3(PCSC_LOG_DEBUG, "concated response length is: %d  should be: %d",
 390                         //                      i, response_length);
 391
 392                         rv = IFD_SUCCESS;
 393             }
 394             else
 395             {
 396                     *RxLength = response_length;
 397                     for (i = 0; i < response_length; ++i)
 398                     {
 399                             *(RxBuffer + i) = buffer[2 + i];
 400                     }
 401                     //Log_Xxd(PCSC_LOG_INFO, "RAPDU: ", *RxBuffer, *RxLength);
 402                     rv = IFD_SUCCESS;
 403             }
 404         }
 405         else
 406         {
 407             Log1(PCSC_LOG_ERROR,
 408                          "no response, meaning eof, reader not usable anymore\n");
 409                 closeSocket();
 410             rv = IFD_COMMUNICATION_ERROR;
 411         }
 412         return rv;
 413 }
 414
 415 /* get the uid from the device and return it as an atr */
 416 void readUID(PDWORD Length, PUCHAR Value)
 417 {
 418         cmd[0] = CMD_BYTE;
 419         cmd[1] = ATS_BYTE;
 420         DWORD rxLength;
 421         int i ,j;
 422         char atr[MAX_ATR_SIZE - 4];
 423
 424         // send the ats cmd
 425         if (sendData(&cmd, sizeof(cmd), &atr, &rxLength, 0) == IFD_SUCCESS)
 426         {
 427                 // construct standart contactless ATR
 428                 *(Value + 0) = 0x3B; //TS direct convention
 429                 *(Value + 1) = 0x88; //T0 TD1 available, 8 historical bytes
 430                 *(Value + 2) = 0x80; //TD1 TD2 follows, protocol T0
 431                 *(Value + 3) = 0x01; //TD2 no Tx3, protocol T1
 432                 j = 4;
 433                 char crc = 0x88 ^ 0x80 ^ 0x01;
 434                 for (i = 0; i < rxLength; ++i)
 435                 {
 436                         *(Value + i + j) = atr[i];
 437                         crc ^= atr[i];
 438                 }
 439                 *(Value + i + j) = crc;
 440                 *Length = rxLength + j + 1;
 441         }
 442         else
 443         {
 444                 *Length = 0;
 445         }
 446 }
 447
 448 /* answer to the pcscd polling */
 449 int readPresence()
 450 {
 451         // construct presence cmd
 452         cmd[0] = CMD_BYTE;
 453         cmd[1] = PRS_BYTE;
 454     DWORD rxLength = 0;
 455         char response[1];
 456
 457         // send cmd
 458         if (sendData(&cmd, sizeof(cmd), &response, &rxLength, 1) == IFD_SUCCESS)
 459         {
 460                 // if icc present the response will be just 0x1
 461                 if (rxLength == 1)
 462                 {
 463                         return (int) response[0];
 464                 }
 465         }
 466         return 0;
 467 }