void vsprintf(ubyte *output_str, ubyte *fmt, va_list args)
{
// What follows is a fairly hairy looking scanner.
// error-checking is not my current priority, so be prepared for this to fail unless you drive it right.
ubyte *string_parameter = (ubyte *)kmalloc(128);
ubyte *param_pos = string_parameter;
sword len;
ubyte flags;
sbyte width;
ubyte precision;
ubyte done = 0;
uword index = 0;
sdword varag;
varag = va_start(args, fmt);
for(index=0;index < 512;index++)
{
*(output_str + index) = '\0';
}
index = 0;
while(*fmt)
{
done = 0;
if(*fmt != '%')
{
*(output_str + index) = *fmt++;
index++;
}
else
{
flags = 0;
while(!done)
{
*fmt++; // skip the escape %
switch(*fmt)
{
case '\'': flags |= GROUP; break;
case '-' : flags |= LEFT; break;
case '+' : flags |= SIGN; break;
case ' ' : flags |= SPACE; break;
case '#' : flags |= ALT; break;
case '0' : flags |= ZEROPAD; break;
default: done++; break;
}
}
// flags should now be set, next value is the optional field width
if(*fmt >= 0x30 && *fmt <= 0x39)
{
width = *fmt - 0x30;
*fmt++;
}
else
width = -1;
// TODO: there's a bunch of extra functionality in the printf standard that's not being
// included here. precision, conversion qualifiers, other.
// Now we see what value we're returning
switch(*fmt++)
{
case 'c': // character
string_parameter[0] = (ubyte)va_arg(args, udword);
len = 1;
string_parameter[1] = '\0';
break;
case 's': // String.
string_parameter = va_arg(args, ubyte *);
len = strlen(string_parameter);
break;
case 'd': // Integer (base 10)
varag = va_arg(args, sdword);
if(varag > 0 && flags & SIGN)
{
*(output_str + index) = '+';
index++;
}
else if(varag > 0 && flags & SPACE)
{
*(output_str + index) = ' ';
index++;
}
string_parameter = itoa(varag);
len = strlen(string_parameter);
break;
case 'o': // octal
string_parameter = octtoa(va_arg(args, udword));
len = strlen(string_parameter);
break;
case 'x': // lowercase hex
flags |= CASE;
case 'X': // uppercase hex
*(output_str + index) = '0';
index++;
*(output_str + index) = 'x';
index++;
string_parameter = hextoa(va_arg(args, udword));
if(flags & CASE)
{
string_parameter = strtolower(string_parameter);
}
len = strlen(string_parameter);
break;
case 'b': // binary
string_parameter = bintoa(va_arg(args, udword));
len = strlen(string_parameter);
break;
case '%': // pass the % right through.
string_parameter[0] = '%';
len = 1;
string_parameter[1] = '\0';
break;
default: // Does nothing at the moment (essentially silently ignores malformed strings.
break;
}
if(flags & ZEROPAD && !(flags & LEFT))
{
while(len < width--)
{
*(output_str + index) = '0';
index++;
}
}
// now we need to pad the field if necessary, then add the string parameter to it
if(!(flags & LEFT))
{
while(len < width--)
{
*(output_str + index) = ' ';
index++;
}
}
while(*string_parameter)
{
*(output_str + index) = *string_parameter++;
index++;
}
if(flags & LEFT)
{
while (len < width--)
{
*(output_str + index) = ' ';
index++;
}
}
// Done, clean up and continue.
len = 0;
string_parameter = param_pos;
}
}
*(output_str + index) = '\0';
// return output_str;
}
char *
print_ip6(ip6_addr * addr, char * str)
{
int i;
unshort * up;
char * cp;
unshort word;
int skip = 0; /* skipping zeros flag */
if(addr == NULL) /* trap NULL pointers */
return NULL;
up = (unshort*)addr;
cp = str;
for(i = 0; i < 8; i++) /* parse 8 16-bit words */
{
word = htons(*up);
up++;
/* If word has a zero value, see if we can skip it */
if(word == 0)
{
/* if we haven't already skipped a zero string... */
if(skip < 2)
{
/* if we aren't already skipping one, start */
if(!skip)
{
skip++;
if (i == 0)
*cp++ = ':';
}
continue;
}
}
else
{
if(skip == 1) /* If we were skipping zeros... */
{
skip++; /* ...stop now */
*cp++ = ':'; /* make an extra colon */
}
}
if(word & 0xF000)
*cp++ = hextoa(word >> 12);
if(word & 0xFF00)
*cp++ = hextoa((word & 0x0F00) >> 8);
if(word & 0xFFF0)
*cp++ = hextoa((word & 0x00F0) >> 4);
*cp++ = hextoa(word & 0x000F);
*cp++ = ':';
}
if(skip == 1) /* were we skipping training zeros? */
{
*cp++ = ':';
*cp = 0;
}
else
*--cp = 0; /* turn trailing colon into null */
return str;
}
int main(int argc, char *argv[])
{
int ret = 0, input = -1;
wpa_printf(MSG_DEBUG, "%s", "Hello rild");
while(1){
dumpMenu();
scanf("%d", &input);
printf("Input %d\n", input);
switch (input)
{
case 0:
{
char *pSimResult1 = "8944F433373A7603DE84C000";
char *pSimResult2 = "ERROR,rild is not setup up";
char *pAkaSuccess = "DB083F7D8D419277AE1C1080BB43519D0175C139187861DC01A31A10AC98E8D86AF2D92CA0CAC30B5C50E88A08B5F110E377A2E77D";
char *pAkaFailure = "DC0E8944F438944F438944F438944F43";
u8 i = 0xcd, ia;
char *a = "f4", ai[2];
u8 ii[2] = {0x44, 0xfb}, aaii[2];
char *aa = "ffee", iiaa[4];
wpa_printf(MSG_DEBUG, "FIXED TEST\n");
hextoa(&i, ai);
wpa_printf(MSG_DEBUG, "%x -> %c%c", i, ai[0], ai[1]);
atohex(a, &ia);
wpa_printf(MSG_DEBUG, "%s -> %x", a, ia);
hextostr(ii, sizeof(ii)/sizeof(ii[0]), iiaa);
wpa_printf(MSG_DEBUG, "%x%x -- > %c%c%c%c", ii[0], ii[1],
iiaa[0], iiaa[1], iiaa[2], iiaa[3]);
strtohex(aa, strlen(aa), aaii);
wpa_printf(MSG_DEBUG, "%s --> %x%x", aa,
aaii[0], aaii[1]);
uint8 *strParm = (uint8 *)pSimResult1, strLen = strlen(pSimResult1);
uint8 sres[SIM_SRES_LEN], kc[SIM_KC_LEN];
uint8 res[50];
size_t res_len = 0;
uint8 ck[50], ik[50], auts[50];
parseSimResult(strParm, strLen, sres, kc);
strParm = (uint8 *)pSimResult2, strLen = strlen(pSimResult2);
parseSimResult(strParm, strLen, sres, kc);
parseAkaResult(pAkaSuccess, strlen(pAkaSuccess),
res, &res_len, ck, ik, auts);
parseAkaResult(pAkaFailure, strlen(pAkaFailure),
res, &res_len, ck, ik, auts);
}
break;
case 1:
{
int sock = -1;
int i = 0;
char *simInput = "EAP_SIM,0,5a01d5224f14222c5554102a10df5896";
int slotId = 0;
uint8 sres[SIM_SRES_LEN], kc[SIM_KC_LEN];
uint8 rand[] = {0x5a, 0x01, 0xd5, 0x22, 0x4f, 0x14, 0x22, 0x2c, 0x55, 0x5, 0x4, 0x10, 0x2a, 0x10, 0xdf, 0x58, 0x96};
uint8 randLeon[3][16] = {{0x89, 0xab, 0xcb, 0xee, 0xf9, 0xab, 0xcd, 0xef,
0x89, 0xab, 0xcd, 0xef, 0x89, 0xab, 0xcd, 0xef},
{0x9a, 0xbc, 0xde, 0xf8, 0x9a, 0xbc, 0xde, 0xf8,
0x9a, 0xbc, 0xde, 0xf8, 0x9a, 0xbc, 0xde, 0xf8},
{0xab, 0xcd, 0xef, 0x89, 0xab, 0xcd, 0xef, 0x89,
0xab, 0xcd, 0xef, 0x89, 0xab, 0xcd, 0xef, 0x89}};
for(i = 0; i < 3; i++){
sock = connectToRild(&slotId);
eapSimSetParam(sock, 0, randLeon[i]);
eapSimQueryResult(sock, sres, kc);
disconnectRild(sock);
}
}
break;
case 2:
{
int sock = -1;
char *akaInput = "EAP_AKA,0, 8090a966d69c5dd3b5fb0ae975596961,d7ac9c65801a3412af8d47b4ce54ee1e";
int slotId = 0;
uint8 rand[] = {0x80, 0x90, 0xa9, 0x66, 0xd6, 0x9c, 0x5d, 0xd3,
0xb5, 0xfb, 0x0a, 0xe9, 0x75, 0x59, 0x69, 0x61};
uint8 autn[] = {0xd7,0xac,0x9c,0x65,0x80,0x1a,0x34,0x12,
0xaf,0x8d,0x47,0xb4,0xce,0x54,0xee,0x1e};
uint8 res[100], ik[100], ck[100], auts[100];
size_t res_len;
sock = connectToRild(&slotId);
eapAkaSetParam(sock, 0, rand, autn);
eapAkaQueryResult(sock, res, &res_len,
ik, ck, auts);
disconnectRild(sock);
}
break;
default:
printf("wrong input %d\n", input);
break;
}
}
return 0;
}
