// Return the left most node in the tree
static stm_blk * _first_node_non_stm (ptst_t *ptst, stm_tx *tx, stm_blk *sb) {
stm_blk *pb;
node_t *p;
set_t * s = (set_t*) init_stm_blk__(ptst, MEMORY, sb);
pb = GET_ROOT(s);
if(pb != NULL) {
p = init_stm_blk__(ptst, MEMORY, pb);
while ( GET_LEFT(p) != NULL ) {
pb = GET_LEFT(p);
p = init_stm_blk__(ptst, MEMORY, pb);
}
}
return pb;
}//_first_node_non_stm
static stm_blk * _lookup(ptst_t *ptst, stm_tx *tx, stm_blk *sb, setkey_t k) {
stm_blk *pb;
node_t *p;
set_t * s = (set_t*) READ_OBJ(sb);
pb = GET_ROOT(s);
while ( pb != NULL ) {
p = READ_OBJ(pb);
//assert(GET_KEY(p)+100==GET_VALUE(p));
int cmp = k - GET_KEY(p);
if (cmp == 0) {
return pb;
}
pb = (cmp < 0) ? GET_LEFT(p) : GET_RIGHT(p);
}
return NULL;
}
static stm_blk * _insert(ptst_t *ptst, stm_tx *tx, stm_blk *sb, setkey_t k, setval_t v, stm_blk * newb) {
stm_blk *xb, *pb;
node_t *x, *p, *new;
int cmp = 0;
set_t * s = (set_t*) READ_OBJ(sb);
xb = GET_ROOT(s);
pb = NULL;
if(xb == NULL) {
//insert at the root
new = WRITE_OBJ(newb);
SET_KEY(new, k);
SET_COLOUR(new, BLACK);
SET_VALUE(new, v);
SET_LEFT(new, NULL);
SET_RIGHT(new, NULL);
SET_PARENT(new, NULL);
s = (set_t*) WRITE_OBJ(sb);
SET_ROOT(s, newb);
return NULL;
}
int rtl_net80211_setkey(struct net_device *dev, struct iw_request_info *info, union iwreq_data *wrqu, char *extra)
{
struct rtl8192cd_priv *priv = (struct rtl8192cd_priv *)dev->priv;
struct ieee80211req_key *wk = (struct ieee80211req_key *)wrqu->data.pointer;
struct wifi_mib *pmib = priv->pmib;
struct Dot11EncryptKey *pEncryptKey = NULL;
struct stat_info *pstat = NULL;
unsigned char MULTICAST_ADD[6]={0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
int cipher =0;
int retVal = 0;
int group_key = 0;
HAPD_MSG("rtl_net80211_setkey +++\n");
HAPD_MSG("keyid = %d, mac = 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x\n"
, wk->ik_keyix, wk->ik_macaddr[0], wk->ik_macaddr[1], wk->ik_macaddr[2],
wk->ik_macaddr[3], wk->ik_macaddr[4], wk->ik_macaddr[5]);
HAPD_MSG("type = 0x%x, flags = 0x%x, keylen = 0x%x \n"
, wk->ik_type, wk->ik_flags, wk->ik_keylen);
//check if the interface is down
if (!netif_running(priv->dev))
{
if(wk->ik_type == IEEE80211_CIPHER_WEP)
{
HAPD_MSG("set WEP Key in driver DOWN\n");
memcpy(&priv->pmib->dot11DefaultKeysTable.keytype[wk->ik_keyix].skey[0], wk->ik_keydata, wk->ik_keylen);
return 0;
}
HAPD_MSG("\nFail: interface not opened\n");
return 0;
}
if(!memcmp(wk->ik_macaddr, MULTICAST_ADD, 6))
group_key = 1;
if(wk->ik_type == IEEE80211_CIPHER_WEP)
{
if(priv->pmib->dot1180211AuthEntry.dot11PrivacyAlgrthm == _WEP_40_PRIVACY_)
cipher = (DOT11_ENC_WEP40);
else if(priv->pmib->dot1180211AuthEntry.dot11PrivacyAlgrthm == _WEP_104_PRIVACY_)
cipher = (DOT11_ENC_WEP104);
else
return -EINVAL;
}
else if(wk->ik_type == IEEE80211_CIPHER_TKIP)
cipher = (DOT11_ENC_TKIP);
else if(wk->ik_type == IEEE80211_CIPHER_AES_CCM)
cipher = (DOT11_ENC_CCMP);
else
{
HAPD_MSG("unknown encAlg !!!\n");
return -EINVAL;
}
//_Eric ?? if call DOT11_Process_Set_Key
//CamAddOneEntry(priv, wk->ik_macaddr, wk->ik_keyix, cipher, 0, wk->ik_keydata);
if(group_key)
{
int set_gkey_to_cam = 1;
HAPD_MSG("set group key !!\n");
#ifdef UNIVERSAL_REPEATER
if (IS_VXD_INTERFACE(priv))
set_gkey_to_cam = 0;
else {
if (IS_ROOT_INTERFACE(priv)) {
if (IS_DRV_OPEN(GET_VXD_PRIV(priv)))
set_gkey_to_cam = 0;
}
}
#endif
#ifdef MBSSID
if (GET_ROOT(priv)->pmib->miscEntry.vap_enable)
{
// No matter root or vap, don't set key to cam if vap is enabled.
set_gkey_to_cam = 0;
}
#endif
#ifdef CONFIG_RTK_MESH
//modify by Joule for SECURITY
if (dev == priv->mesh_dev)
{
pmib->dot11sKeysTable.dot11Privacy = cipher;
pEncryptKey = &pmib->dot11sKeysTable.dot11EncryptKey;
pmib->dot11sKeysTable.keyid = (UINT)wk->ik_keyix;
}
else
#endif
{
pmib->dot11GroupKeysTable.dot11Privacy = cipher;
pEncryptKey = &pmib->dot11GroupKeysTable.dot11EncryptKey;
pmib->dot11GroupKeysTable.keyid = (UINT)wk->ik_keyix;
}
switch(cipher)
{
case DOT11_ENC_TKIP:
set_ttkeylen(pEncryptKey, 16);
set_tmickeylen(pEncryptKey, 8);
set_tkip_key(pEncryptKey, wk->ik_keydata);
HAPD_MSG("going to set TKIP group key! id %X\n", (UINT)wk->ik_keyix);
if (!SWCRYPTO) {
if (set_gkey_to_cam)
{
retVal = CamDeleteOneEntry(priv, MULTICAST_ADD, wk->ik_keyix, 0);
if (retVal) {
priv->pshare->CamEntryOccupied--;
pmib->dot11GroupKeysTable.keyInCam = FALSE;
}
retVal = CamAddOneEntry(priv, MULTICAST_ADD, wk->ik_keyix, DOT11_ENC_TKIP<<2, 0, wk->ik_keydata);
if (retVal) {
priv->pshare->CamEntryOccupied++;
pmib->dot11GroupKeysTable.keyInCam = TRUE;
}
}
}
break;
case DOT11_ENC_WEP40:
set_ttkeylen(pEncryptKey, 5);
set_tmickeylen(pEncryptKey, 0);
set_wep40_key(pEncryptKey, wk->ik_keydata);
HAPD_MSG("going to set WEP40 group key!\n");
if (!SWCRYPTO) {
if (set_gkey_to_cam)
{
retVal = CamDeleteOneEntry(priv, MULTICAST_ADD, wk->ik_keyix, 0);
if (retVal) {
priv->pshare->CamEntryOccupied--;
pmib->dot11GroupKeysTable.keyInCam = FALSE;
}
retVal = CamAddOneEntry(priv, MULTICAST_ADD, wk->ik_keyix, DOT11_ENC_WEP40<<2, 0, wk->ik_keydata);
if (retVal) {
priv->pshare->CamEntryOccupied++;
pmib->dot11GroupKeysTable.keyInCam = TRUE;
}
}
}
break;
case DOT11_ENC_WEP104:
set_ttkeylen(pEncryptKey, 13);
set_tmickeylen(pEncryptKey, 0);
set_wep104_key(pEncryptKey, wk->ik_keydata);
HAPD_MSG("going to set WEP104 group key!\n");
if (!SWCRYPTO) {
if (set_gkey_to_cam)
{
retVal = CamDeleteOneEntry(priv, MULTICAST_ADD, wk->ik_keyix, 0);
if (retVal) {
priv->pshare->CamEntryOccupied--;
pmib->dot11GroupKeysTable.keyInCam = FALSE;
}
retVal = CamAddOneEntry(priv, MULTICAST_ADD, wk->ik_keyix, DOT11_ENC_WEP104<<2, 0, wk->ik_keydata);
if (retVal) {
priv->pshare->CamEntryOccupied++;
pmib->dot11GroupKeysTable.keyInCam = TRUE;
}
}
}
break;
case DOT11_ENC_CCMP:
set_ttkeylen(pEncryptKey, 16);
set_tmickeylen(pEncryptKey, 16);
set_aes_key(pEncryptKey, wk->ik_keydata);
HAPD_MSG("going to set CCMP-AES group key!\n");
#ifdef CONFIG_RTK_MESH
if (dev == priv->mesh_dev)
pmib->dot11sKeysTable.keyInCam = TRUE; // keyInCam means key in driver
else
#endif
if (!SWCRYPTO) {
if (set_gkey_to_cam)
{
retVal = CamDeleteOneEntry(priv, MULTICAST_ADD, wk->ik_keyix, 0);
if (retVal) {
priv->pshare->CamEntryOccupied--;
pmib->dot11GroupKeysTable.keyInCam = FALSE;
}
retVal = CamAddOneEntry(priv, MULTICAST_ADD, wk->ik_keyix, DOT11_ENC_CCMP<<2, 0, wk->ik_keydata);
if (retVal) {
priv->pshare->CamEntryOccupied++;
pmib->dot11GroupKeysTable.keyInCam = TRUE;
}
}
}
break;
case DOT11_ENC_NONE:
default:
HAPD_MSG("No group encryption key is set!\n");
set_ttkeylen(pEncryptKey, 0);
set_tmickeylen(pEncryptKey, 0);
break;
}
}
else
{
pstat = get_stainfo(priv, wk->ik_macaddr);
if (pstat == NULL) {
DEBUG_ERR("Set key failed, invalid mac address: %02x%02x%02x%02x%02x%02x\n",
wk->ik_macaddr[0], wk->ik_macaddr[1], wk->ik_macaddr[2], wk->ik_macaddr[3],
wk->ik_macaddr[4], wk->ik_macaddr[5]);
return (-1);
}
pstat->dot11KeyMapping.dot11Privacy = cipher;
pEncryptKey = &pstat->dot11KeyMapping.dot11EncryptKey;
pstat->keyid = wk->ik_keyix;
#if defined(__DRAYTEK_OS__) && defined(WDS)
if (pstat->state & WIFI_WDS)
priv->pmib->dot11WdsInfo.wdsPrivacy = cipher;
#endif
switch(cipher)
{
case DOT11_ENC_TKIP:
set_ttkeylen(pEncryptKey, 16);
set_tmickeylen(pEncryptKey, 8);
set_tkip_key(pEncryptKey, wk->ik_keydata);
HAPD_MSG("going to set TKIP Unicast key for sta %02X%02X%02X%02X%02X%02X, id=%d\n",
wk->ik_macaddr[0], wk->ik_macaddr[1], wk->ik_macaddr[2],
wk->ik_macaddr[3], wk->ik_macaddr[4], wk->ik_macaddr[5], pstat->keyid);
if (!SWCRYPTO) {
retVal = CamDeleteOneEntry(priv, wk->ik_macaddr, wk->ik_keyix, 0);
if (retVal) {
priv->pshare->CamEntryOccupied--;
if (pstat) pstat->dot11KeyMapping.keyInCam = FALSE;
}
retVal = CamAddOneEntry(priv, wk->ik_macaddr, wk->ik_keyix, DOT11_ENC_TKIP<<2, 0, wk->ik_keydata);
if (retVal) {
priv->pshare->CamEntryOccupied++;
if (pstat) pstat->dot11KeyMapping.keyInCam = TRUE;
}
else {
if (pstat->aggre_mthd != AGGRE_MTHD_NONE)
pstat->aggre_mthd = AGGRE_MTHD_NONE;
}
}
break;
case DOT11_ENC_WEP40:
set_ttkeylen(pEncryptKey, 5);
set_tmickeylen(pEncryptKey, 0);
set_wep40_key(pEncryptKey, wk->ik_keydata);
HAPD_MSG("going to set WEP40 unicast key for sta %02X%02X%02X%02X%02X%02X, id=%d\n",
wk->ik_macaddr[0], wk->ik_macaddr[1], wk->ik_macaddr[2],
wk->ik_macaddr[3], wk->ik_macaddr[4], wk->ik_macaddr[5], pstat->keyid);
if (!SWCRYPTO) {
retVal = CamDeleteOneEntry(priv, wk->ik_macaddr, wk->ik_keyix, 0);
if (retVal) {
priv->pshare->CamEntryOccupied--;
if (pstat) pstat->dot11KeyMapping.keyInCam = FALSE;
}
retVal = CamAddOneEntry(priv, wk->ik_macaddr, wk->ik_keyix, DOT11_ENC_WEP40<<2, 0, wk->ik_keydata);
if (retVal) {
priv->pshare->CamEntryOccupied++;
if (pstat) pstat->dot11KeyMapping.keyInCam = TRUE;
}
else {
if (pstat->aggre_mthd != AGGRE_MTHD_NONE)
pstat->aggre_mthd = AGGRE_MTHD_NONE;
}
}
break;
case DOT11_ENC_WEP104:
set_ttkeylen(pEncryptKey, 13);
set_tmickeylen(pEncryptKey, 0);
set_wep104_key(pEncryptKey, wk->ik_keydata);
HAPD_MSG("going to set WEP104 unicast key for sta %02X%02X%02X%02X%02X%02X, id=%d\n",
wk->ik_macaddr[0], wk->ik_macaddr[1], wk->ik_macaddr[2],
wk->ik_macaddr[3], wk->ik_macaddr[4], wk->ik_macaddr[5], pstat->keyid);
if (!SWCRYPTO) {
retVal = CamDeleteOneEntry(priv, wk->ik_macaddr, wk->ik_keyix, 0);
if (retVal) {
priv->pshare->CamEntryOccupied--;
if (pstat) pstat->dot11KeyMapping.keyInCam = FALSE;
}
retVal = CamAddOneEntry(priv, wk->ik_macaddr, wk->ik_keyix, DOT11_ENC_WEP104<<2, 0, wk->ik_keydata);
if (retVal) {
priv->pshare->CamEntryOccupied++;
if (pstat) pstat->dot11KeyMapping.keyInCam = TRUE;
}
else {
if (pstat->aggre_mthd != AGGRE_MTHD_NONE)
pstat->aggre_mthd = AGGRE_MTHD_NONE;
}
}
break;
case DOT11_ENC_CCMP:
set_ttkeylen(pEncryptKey, 16);
set_tmickeylen(pEncryptKey, 16);
set_aes_key(pEncryptKey, wk->ik_keydata);
HAPD_MSG("going to set CCMP-AES unicast key for sta %02X%02X%02X%02X%02X%02X, id=%d\n",
wk->ik_macaddr[0], wk->ik_macaddr[1], wk->ik_macaddr[2],
wk->ik_macaddr[3], wk->ik_macaddr[4], wk->ik_macaddr[5], pstat->keyid);
if (!SWCRYPTO) {
retVal = CamDeleteOneEntry(priv, wk->ik_macaddr, wk->ik_keyix, 0);
if (retVal) {
priv->pshare->CamEntryOccupied--;
if (pstat) pstat->dot11KeyMapping.keyInCam = FALSE;
}
retVal = CamAddOneEntry(priv, wk->ik_macaddr, wk->ik_keyix, DOT11_ENC_CCMP<<2, 0, wk->ik_keydata);
if (retVal) {
HAPD_MSG("CamAddOneEntry of CCMP OK\n");
priv->pshare->CamEntryOccupied++;
if (pstat) pstat->dot11KeyMapping.keyInCam = TRUE;
assign_aggre_mthod(priv, pstat);
}
else {
HAPD_MSG("CamAddOneEntry of CCMP FAIL\n");
if (pstat->aggre_mthd != AGGRE_MTHD_NONE)
pstat->aggre_mthd = AGGRE_MTHD_NONE;
}
}
break;
case DOT11_ENC_NONE:
default:
DEBUG_ERR("No pairewise encryption key is set!\n");
set_ttkeylen(pEncryptKey, 0);
set_tmickeylen(pEncryptKey, 0);
break;
}
}
HAPD_MSG("rtl_net80211_setkey ---\n");
return 0;
}
static void _fix_after_insertion(ptst_t *ptst, stm_tx *tx, stm_blk *sb, stm_blk *xb, node_t *x) {
stm_blk *pb, *gpb, *yb, *lub;
node_t *p, *gp, *y;
SET_COLOUR(x, RED);
// rebalance tree
set_t * s = (set_t*)READ_OBJ(sb);
while (xb != NULL && GET_PARENT(x) != NULL) {
pb = GET_PARENT(x);
p = READ_OBJ(pb);
if (IS_BLACK(p)) // case 2 - parent is black
break;
gpb = GET_PARENT(p);
gp = READ_OBJ(gpb);
lub = GET_LEFT(gp);
if (pb == lub) {
// parent is red, p=GET_LEFT(g)
yb = GET_RIGHT(gp); // y (uncle)
y = READ_OBJ(yb);
if (IS_RED(y)) {
// case 3 - parent is red, uncle is red (p = GET_LEFT(gp))
p = WRITE_OBJ(pb);
y = WRITE_OBJ(yb);
gp = WRITE_OBJ(gpb);
SET_COLOUR(p, BLACK);
SET_COLOUR(y, BLACK);
SET_COLOUR(gp, RED);
xb = gpb;
x = gp;
} else {
// parent is red, uncle is black (p = GET_LEFT(gp))
if ( xb == GET_RIGHT(p) ) {
// case 4 - parent is red, uncle is black, x = GET_RIGHT(p), p = GET_LEFT(gp)
xb = pb;
x = WRITE_OBJ(pb);
_left_rotate(ptst, tx, sb, xb, x);
pb=GET_PARENT(x);
}
// case 5 - parent is red, uncle is black, x = GET_LEFT(p), p = GET_LEFT(gp)
p = WRITE_OBJ(pb);
gpb = GET_PARENT(p);
gp = WRITE_OBJ(gpb);
SET_COLOUR(p, BLACK);
SET_COLOUR(gp, RED);
if (gp != NULL) {
_right_rotate(ptst, tx, sb, gpb, gp);
}
}
} else {
// parent is red, p = GET_RIGHT(gp)
yb = lub;
y = READ_OBJ(yb);
if (IS_RED(y)) {
// case 3 - parent is red, uncle is red (p = GET_RIGHT(gp))
p = WRITE_OBJ(pb);
y = WRITE_OBJ(yb);
gp = WRITE_OBJ(gpb);
SET_COLOUR(p, BLACK);
SET_COLOUR(y, BLACK);
SET_COLOUR(gp, RED);
xb = gpb;
x = gp;
} else {
// parent is red, uncle is black (p = GET_RIGHT(gp))
if ( xb == GET_LEFT(p) ) {
// case 4 - parent is red, uncle is black, x = GET_LEFT(p), p = GET_RIGHT(gp)
xb = pb;
x = WRITE_OBJ(pb);
_right_rotate(ptst, tx, sb, xb, x);
pb = GET_PARENT(x);
}
// case 5 - parent is red, uncle is black, x = GET_RIGHT(p), p = GET_RIGHT(gp)
p = WRITE_OBJ(pb);
gpb = GET_PARENT(p);
gp = WRITE_OBJ(gpb);
SET_COLOUR(p, BLACK);
SET_COLOUR(gp, RED);
if(gp != NULL) {
_left_rotate(ptst, tx, sb, gpb, gp);
}
}
}
}
s = (set_t*)READ_OBJ(sb);
stm_blk * rob = GET_ROOT(s);
node_t * ro = READ_OBJ(rob);
if (IS_RED(ro)) {
ro = WRITE_OBJ(rob);
SET_COLOUR(ro,BLACK);
}
}
static void _fix_after_deletion(ptst_t *ptst, stm_tx *tx, stm_blk *sb, stm_blk *xb, node_t *x) {
stm_blk *pb, *plb, *sibb, *siblb, *sibrb;
node_t *p, *sib, *sibl, *sibr;
set_t *s;
while (GET_PARENT(x)!=NULL && IS_BLACK(x)) {
pb = GET_PARENT(x);
p = WRITE_OBJ(pb);
plb = GET_LEFT(p);
if ( xb == plb ) {
sibb = GET_RIGHT(p);
sib = WRITE_OBJ(sibb);
if (IS_RED(sib)) {
SET_COLOUR(sib, BLACK);
SET_COLOUR(p, RED);
_left_rotate(ptst, tx, sb, pb, p);
pb = GET_PARENT(x);
p=WRITE_OBJ(pb);
sibb = GET_RIGHT(p);
sib = WRITE_OBJ(sibb);
}
siblb = GET_LEFT(sib);
sibl = READ_OBJ(siblb);
sibrb = GET_RIGHT(sib);
sibr = READ_OBJ(sibrb);
if (IS_BLACK(sibl) && IS_BLACK(sibr)) {
SET_COLOUR(sib, RED);
xb = GET_PARENT(x);
x = WRITE_OBJ(xb);
} else {
if (IS_BLACK(sibr)) {
sibl = WRITE_OBJ(siblb);
SET_COLOUR(sibl, BLACK);
SET_COLOUR(sib,RED);
_right_rotate(ptst, tx, sb, sibb, sib);
pb = GET_PARENT(x);
p = WRITE_OBJ(pb);
sibb = GET_RIGHT(p);
}
sib = WRITE_OBJ(sibb);
SET_COLOUR(sib, GET_COLOUR(p));
p = WRITE_OBJ(pb);
SET_COLOUR(p, BLACK);
sibrb = GET_RIGHT(sib);
sibr = WRITE_OBJ(sibrb);
SET_COLOUR(sibr, BLACK);
_left_rotate(ptst, tx, sb, pb, p);
s = (set_t*)READ_OBJ(sb);
xb = GET_ROOT(s);
x = WRITE_OBJ(xb);
break;
}
} else { // inverse
sibb = plb;
sib = WRITE_OBJ(sibb);
if (IS_RED(sib)) {
SET_COLOUR(sib, BLACK);
SET_COLOUR(p, RED);
_right_rotate(ptst, tx, sb, pb, p);
pb = GET_PARENT(x);
p=WRITE_OBJ(pb);
sibb = GET_LEFT(p);
sib = WRITE_OBJ(sibb);
}
siblb = GET_LEFT(sib);
sibl = READ_OBJ(siblb);
sibrb = GET_RIGHT(sib);
sibr = READ_OBJ(sibrb);
if (IS_BLACK(sibl) && IS_BLACK(sibr)) {
SET_COLOUR(sib, RED);
xb = GET_PARENT(x);
x = WRITE_OBJ(xb);
} else {
if (IS_BLACK(sibl)) {
sibr = WRITE_OBJ(sibrb);
SET_COLOUR(sibr, BLACK);
SET_COLOUR(sib, RED);
_left_rotate(ptst, tx, sb, sibb, sib);
pb = GET_PARENT(x);
p = WRITE_OBJ(pb);
sibb = GET_LEFT(p);
}
sib = WRITE_OBJ(sibb);
SET_COLOUR(sib, GET_COLOUR(p));
p = WRITE_OBJ(pb);
SET_COLOUR(p, BLACK);
siblb = GET_LEFT(sib);
sibl = WRITE_OBJ(siblb);
SET_COLOUR(sibl, BLACK);
_right_rotate(ptst, tx, sb, pb, p);
s = (set_t*)READ_OBJ(sb);
xb = GET_ROOT(s);
x = WRITE_OBJ(xb);
break;
}
}
}
if(IS_RED(x)) {
SET_COLOUR(x, BLACK);
}
}//fix_after_deletion
