sm_handler.c

Linux上的802.1x 的supplicant的实现。很多supplicant程序都是基于它开发的

  loopcnt = 0;

  while (1)
    {
      dwState = 0;
      dwProtocol = 0;
      dwAtrLen = MAX_ATR_SIZE;
      size = 50;

      mszReaders = (LPSTR)malloc(size);
      if (mszReaders == NULL) 
	{
	  debug_printf(DEBUG_NORMAL, "Error trying to allocate memory for "
		       "mszReaders!  (%s:%d)\n", __FUNCTION__, __LINE__);
	  return XEMALLOC;
	}

      bzero(mszReaders, 50);
      bzero(&pbAtr, MAX_ATR_SIZE);
      ret = SCardStatus(*card_hdl, mszReaders, &size, &dwState, &dwProtocol, 
			pbAtr, &dwAtrLen);
      if (ret != SCARD_S_SUCCESS)
	{
	  debug_printf(DEBUG_NORMAL, "Error getting smart card status! ");
	  print_sc_error(ret);
	  free(mszReaders);
	  mszReaders = NULL;
	  return -1;
	}

      // XXX We should pass these up to the GUI when we get that going!
      switch (dwState)
	{
	case SCARD_ABSENT:
	  debug_printf(DEBUG_NORMAL, "There is no card in the reader!\n");
	  break;

	case SCARD_PRESENT:
	  debug_printf(DEBUG_NORMAL, "The card needs to be moved to a position"
		       " that the reader can use!\n");
	  break;

	case SCARD_SWALLOWED:
	  debug_printf(DEBUG_NORMAL, "Card is ready, but not powered!\n");
	  break;
	  
	case SCARD_POWERED:
	  debug_printf(DEBUG_NORMAL, "Card is powered, but in an unknown "
		       "mode!\n");
	  break;

	default: 
	  free(mszReaders);
	  return XENONE;
	}

      free(mszReaders);
      mszReaders = NULL;

      if ((loopcnt >= waittime) && (waittime != 0))
	{
	  return -1;
	}
      
      sleep(1);
    }
}

int
hextoint(u8 x)
{
	x = toupper(x);
	if (x >= 'A' && x <= 'F')
		return x-'A'+10;
	else if (x >= '0' && x <= '9')
		return x-'0';
	fprintf(stderr, "bad input.\n");
	exit(1);
}


/* convert commands of format 'A00001' or 'A0 00 01' to binary form */
int  
strtohex(u8  *src, u8  *dest, int *blen)
{
  int i,len;
  char *p, *q;
  char buf[512];
  
  p  = src;
  q = buf;
  while (*p) {	  /* squeeze out any whitespace */
    if (!isspace(*p)) {
      *q++ = *p;
    }
    p++;
  }
  *q = '\0';
  src = buf;
  if ((len = strlen(src)) & 0x01) {	/* oops, odd number of nibbles */
    debug_printf(DEBUG_NORMAL,"strtohex: odd number of nibbles!\n");
    return -1;
  }
  len /= 2;
  for (i = 0; i < len; i++, src += 2)
    dest[i] = (hextoint(*src) << 4) | hextoint(*(src + 1));
  *blen = len;
  return 0;
}

int sm_check_response(uint8_t s1, uint8_t s2)
{
  uint8_t t;

  switch (s1)
    {
    case 0x67:
      debug_printf(DEBUG_NORMAL, "SIM : incorrect parameter P3\n");
      break;

    case 0x6B:
      debug_printf(DEBUG_NORMAL, "SIM : incorrect parameter P1 or P2\n");
      break;

    case 0x6D:
      debug_printf(DEBUG_NORMAL, "SIM : unknown instruction code given in the command\n");
      break;

    case 0x6E:
      debug_printf(DEBUG_NORMAL, "SIM : wrong instruction class given in the command\n");
      break;

    case 0x6F:
      debug_printf(DEBUG_NORMAL, "SIM : technical problem with no diagnostic gien\n");
      break;

    case 0x6C:
      debug_printf(DEBUG_EXCESSIVE, "SIM : Invalid length.  Should have been %d.\n", s2);
      break;

    case 0x92:
      if (s2 == 0x40)
	{
	  debug_printf(DEBUG_NORMAL, "SIM : memory problem\n");
	} 
      else 
	{
	  debug_printf(DEBUG_NORMAL, "SIM : command successful but after using an internal update retry routine %d time(s)\n", s2);
	}
      break;

    case 0x94:
      switch (s2)
	{
	case 0x00:
	  debug_printf(DEBUG_NORMAL, "SIM : no EF selected\n");
	  break;

	case 0x02:
	  debug_printf(DEBUG_NORMAL, "SIM : out of range (invalid address)\n");
	  break;

	case 0x04:
	  debug_printf(DEBUG_NORMAL, "SIM : file ID, or pattern, not found\n");
	  break;

	case 0x08:
	  debug_printf(DEBUG_NORMAL, "SIM : file is inconsistent with the command\n");
	  break;

	default:
	  return -1;
	  break;
	}
      break;

    case 0x98:
      switch (s2)
	{
	case 0x02:
	  debug_printf(DEBUG_NORMAL, "SIM : no CHV initialised\n");
	  break;

	case 0x04:
	  debug_printf(DEBUG_NORMAL, "SIM : access condition not fulfilled\n");
	  break;

	case 0x08:
	  debug_printf(DEBUG_NORMAL, "SIM : in contradiction with CHV status\n");
	  break;

	case 0x10:
	  debug_printf(DEBUG_NORMAL, "SIM : in contradiction with invalidation status\n");
	  break;

	case 0x40:
	  debug_printf(DEBUG_NORMAL, "SIM : unsuccessful CHV verification, no attempt left\n");
	  break;

	case 0x50:
	  debug_printf(DEBUG_NORMAL, "SIM : increase cannot be performed, max value reached\n");
	  break;

	default:
	  return -1;
	  break;
	}
      break;

    case 0x69:
      switch (s2)
	{
	case 0x82:
	  debug_printf(DEBUG_NORMAL, "SIM : Security status not satisfied\n");
	  break;

	case 0x85:
	  debug_printf(DEBUG_NORMAL, "SIM : Conditions of use not satisfied\n");
	  break;

	default:
	  return -1;
	  break;
	}

    case 0x6a:
      switch (s2)
	{
	case 0x88:
	  debug_printf(DEBUG_NORMAL, "SIM : reference data not found\n");
	  break;

	default:
	  if ((s2 & 0x80) == 0x80)
	    {
	      debug_printf(DEBUG_EXCESSIVE, "Invalid P1-P2 value.\n");
	    }
	  else
	    {
	      return -1;
	    }
	}
      break;

    case 0x63:
      switch (s2)
	{
	case 0x00:
	  debug_printf(DEBUG_NORMAL, "SIM : authentication failed\n");
	  break;

	case 0x01:
	  debug_printf(DEBUG_NORMAL, "SIM : synchronisation failure\n");
	  break;

	default:
	  if ((s2 & 0xc0) == 0xc0)
	    {
	      t = s2 - 0xc0;
	      debug_printf(DEBUG_NORMAL, "%d pin attempts remain.\n", t);
	    }
	  else
	    {
	      return -1;
	    }
	}
      break;

    default:
      return -1;
    }

  return 0;
}

/* card_io -
 *    send a command to the card
 *    if return code indicates a GET RESPONSE is needed,
 *    it is exceuted - depending on context (2G, 3G) with
 *    the appropriate class byte.
 *    the data and length is returned.
 */
int 
cardio(SCARDHANDLE *card_hdl, char *cmd, long reader_protocol, char mode2g, 
	LPBYTE outbuff, LPDWORD olen, char debug)
{
  static char getresponse[5]= {0xa0,0xc0,0x00,0x00,0x00 };
  int cmdlen, ret, p, i;
  u8 bcmd[MAXBUFF];
  SCARD_IO_REQUEST scir;

  strtohex(cmd, bcmd, &cmdlen);
  *olen = MAXBUFF;		/* hm... */
  memset(outbuff, 0, MAXBUFF);

  if ((ret = SCardTransmit(*card_hdl, reader_protocol == SCARD_PROTOCOL_T1 ? SCARD_PCI_T1 : SCARD_PCI_T0,
		      bcmd, cmdlen, &scir, (BYTE *) outbuff,olen)) != SCARD_S_SUCCESS) {
    debug_printf(DEBUG_NORMAL, "Error sending commands to the smart card! ");
    print_sc_error(ret);
    return ret;
  }
  
  if (*olen == 2) {
    switch ((u8) outbuff[0]) {
    case 0x61:			
    case 0x9f:
      if (outbuff[1] == 0) {	/* nothing returned */
	debug_printf(DEBUG_NORMAL, "Nothing was returned when something was "
		     "expected!\n");
	break;
      }
      getresponse[4] = outbuff[1]; /* cmd ok, set length for GET RESPONSE  */
      if (mode2g == 1)
	{
	  getresponse[0] = 0xa0; /* set class byte for card  */
	} else {
	  getresponse[0] = 0x00;
	}

      *olen = MAXBUFF;
      if ((ret = SCardTransmit(*card_hdl,
			  reader_protocol == SCARD_PROTOCOL_T1 ? SCARD_PCI_T1 : SCARD_PCI_T0,
			  getresponse, sizeof(getresponse), &scir,
			       (BYTE *)outbuff, olen)) != SCARD_S_SUCCESS) {
	debug_printf(DEBUG_NORMAL, "Error sending commands to the smart "
		     "card!  ");
	print_sc_error(ret);
	return ret;
      } 
    }
  } 

  //  debug_hex_printf(DEBUG_NORMAL, outbuff, *olen);
  //  printf("\n\n");

  if (*olen >= 2)
    {
      t_response *t = response;
      int found = 0;

      p = *olen - 2;

      if ((outbuff[p] != 0x90) && (outbuff[p+1] != 0x00))
	{
	  while (t->msk[0]) {
	    if ((t->rsp[0] == (t->msk[0] & outbuff[p])) && 
		(t->rsp[1] == (t->msk[1] & outbuff[p+1]))) {
	  
	      debug_printf(DEBUG_NORMAL, t->text, outbuff[p+1] & ~t->msk[1]);
	      found++;
	    }
	    break;
	  }
	  t++;

	  if (!found) {
	    if (sm_check_response(outbuff[p], outbuff[p+1]) != 0)
	      {
		debug_printf(DEBUG_NORMAL, "Sim Card Response : %2.2X %2.2X (unknown response)\n", outbuff[p], outbuff[p+1]);
	      }
	  } else {
	    debug_printf(DEBUG_NORMAL,"\n");
	  }
	}
    }
  return 0;
}


unsigned char
hinibble(unsigned char c)
{
  unsigned char k;

  k = (c >> 4) & 0x0f;
  if (k == 0x0f)
    return 0;
  else
    return (k + '0');
}

unsigned char
lonibble(unsigned char c)
{
  unsigned char k;

  k = c & 0x0f;
  if (k == 0x0f)
    return 0;
  else
    return (k + '0');
}

char *decode_imsi(unsigned char *imsibytes)
{
  unsigned char *imsi, *s;
  int i;

  imsi = (unsigned char *)malloc(20);
  if (imsi == NULL) 
    {
      debug_printf(DEBUG_NORMAL, "Error attempting to allocate temporary "
		   "memory for IMSI!\n");
      return NULL;
    }

  bzero(imsi, 20);
  s = imsi;

  *s++ = hinibble(imsibytes[0]);
  for (i=1; i<8;i++)
    {
      *s++ = lonibble(imsibytes[i]);
      *s++ = hinibble(imsibytes[i]);
    }
  *s = '\0';

  return imsi;
}

char *sm_handler_2g_imsi(SCARDHANDLE *card_hdl, char reader_mode, char *pin)
{
  long len;
  unsigned char buf[512], buf2[512], buf3[8];
  int i;