/** Maintain red-black tree balance after inserting node x
*
*/
static void insert_fixup(rbtree_t *tree, rbnode_t *x)
{
/* check RED-BLACK properties */
while ((x != tree->root) && (x->parent->colour == RED)) {
/* we have a violation */
if (x->parent == x->parent->parent->left) {
rbnode_t *y = x->parent->parent->right;
if (y->colour == RED) {
/* uncle is RED */
x->parent->colour = BLACK;
y->colour = BLACK;
x->parent->parent->colour = RED;
x = x->parent->parent;
} else {
/* uncle is BLACK */
if (x == x->parent->right) {
/* make x a left child */
x = x->parent;
rotate_left(tree, x);
}
/* recolour and rotate */
x->parent->colour = BLACK;
x->parent->parent->colour = RED;
rotate_right(tree, x->parent->parent);
}
} else {
/* mirror image of above code */
rbnode_t *y = x->parent->parent->left;
if (y->colour == RED) {
/* uncle is RED */
x->parent->colour = BLACK;
y->colour = BLACK;
x->parent->parent->colour = RED;
x = x->parent->parent;
} else {
/* uncle is BLACK */
if (x == x->parent->left) {
x = x->parent;
rotate_right(tree, x);
}
x->parent->colour = BLACK;
x->parent->parent->colour = RED;
rotate_left(tree, x->parent->parent);
}
}
}
tree->root->colour = BLACK;
}
void balanced_pst::erase_fixup(node* x) {
while (x != root && x->color == BLACK) {
if (x == x->p->left) {
node* w = x->p->right;
if (w->color == RED) {
w->color = BLACK;
x->p->color = RED;
rotate_left(x->p);
w = x->p->right;
}
if (w->left->color == BLACK && w->right->color == BLACK) {
w->color = RED;
x = x->p;
} else {
if (w->right->color == BLACK) {
w->left->color = BLACK;
w->color = RED;
rotate_right(w);
w = x->p->right;
}
w->color = x->p->color;
x->p->color = BLACK;
w->right->color = BLACK;
rotate_left(x->p);
x = root;
}
} else {
node* w = x->p->left;
if (w->color == RED) {
w->color = BLACK;
x->p->color = RED;
rotate_right(x->p);
w = x->p->left;
}
if (w->right->color == BLACK && w->left->color == BLACK) {
w->color = RED;
x = x->p;
} else {
if (w->left->color == BLACK) {
w->right->color = BLACK;
w->color = RED;
rotate_left(w);
w = x->p->left;
}
w->color = x->p->color;
x->p->color = BLACK;
w->left->color = BLACK;
rotate_right(x->p);
x = root;
}
}
}
x->color = BLACK;
}
void make_balance_after_insert(ptr_rbnode node) {
//case 1 : root node
if (node->parent == NULL) {
node->color = BLACK;
return;
}
//case 2 : parent is black
if (node->parent->color == BLACK) {
return; //nothing to do
}
//now it is guaranteed that node has grandparent
//case 3 : p and u = red, g = black
ptr_rbnode g = get_grandparent(node), u = get_uncle(node), p = node->parent;
assert(g);
if (p->color == RED && u->color == RED && g->color == BLACK) {
p->color = BLACK; u->color = BLACK; g->color = RED;
make_balance_after_insert(g);
/* this recursive operation will take O(log N) in worst case and O(1) in average case
because the time complexity distribution would have a form of geometric distribution. */
return;
}
//case 4,5 : p = red, u = black, g = black; -> guaranteed
//if node, p and g is not on a line, rotate
//case 4 would be changed into case 5
assert(p->color == RED && u->color == BLACK && g->color == BLACK);
if (g->left == p && p->right == node) {
rotate_left(p);
node = node->left;
//for case 5
g = get_grandparent(node), u = get_uncle(node), p = node->parent;
}
else if (g->right == p && p->left == node) {
rotate_right(p);
// for case 5
g = get_grandparent(node), u = get_uncle(node), p = node->parent;
node = node->right;
}
g = get_grandparent(node), u = get_uncle(node), p = node->parent;
//case 5
assert((p->left == node && g->left == p)
|| (p->right == node && g->right == p));
p->color = BLACK;
g->color = RED;
if (p->left == node) {
rotate_right(g);
}
else {
rotate_left(g);
}
}
void delete_one_child(ptr_rbnode node) {
ptr_rbnode s = get_sibiling(node), p = node->parent;
if (p) {
if (s->color == RED) {
p->color = RED; s->color = BLACK;
if (p->left == node) {
rotate_left(p);
}
else {
rotate_right(p);
}
}
s = get_sibiling(node), p = node->parent;
if (s->color == BLACK && s->left->color == BLACK && s->right->color == BLACK) {
if (p->color == BLACK) {
s->color = RED;
delete_one_child(p);
}
else {
s->color = RED;
p->color = BLACK;
}
}
else {
if (node->parent->left == node && s->right->color == RED) {
s->color = p->color; p->color = BLACK; s->right->color = BLACK;
rotate_left(p);
}
else if (node->parent->left == node && s->left->color == RED) {
s->left->color = p->color; p->color = BLACK;
rotate_right(s);
rotate_left(s->parent->parent);
}
else if (node->parent->right == node && s->left->color == RED) {
s->color = p->color; p->color = BLACK; s->left->color = BLACK;
rotate_right(p);
}
else if (node->parent->right == node && s->right->color == RED) {
s->right->color = p->color; p->color = BLACK;
rotate_left(s);
rotate_right(s->parent->parent);
}
else
assert(0 && "balance error");
}
}
}
static void insert_fix_up(rbtree_t rb,struct rbnode *n)
{
while(n->parent->color == RED)
{
struct rbnode *parent = n->parent;
struct rbnode *grand_parent = parent->parent;
if(parent == grand_parent->left)
{
struct rbnode *ancle = grand_parent->right;
if(ancle->color == RED)
{
color_flip(grand_parent);
n = grand_parent;
}
else
{
if(n == parent->right)
{
n = parent;
rotate_left(rb,n);
}
n->parent->color = BLACK;
n->parent->parent->color = RED;
rotate_right(rb,n->parent->parent);
}
}
else
{
struct rbnode *ancle = grand_parent->left;
if(ancle->color == RED)
{
color_flip(grand_parent);
n = grand_parent;
}
else
{
if(n == parent->left)
{
n = parent;
rotate_right(rb,n);
}
n->parent->color = BLACK;
n->parent->parent->color = RED;
rotate_left(rb,n->parent->parent);
}
}
}
rb->root->color = BLACK;
}
void redblacktree::red_black_check(rbtnode* act){
//funkcja sprawdzajaca wlasciwosci czerwono-czarne
//uwaga hardcore
while( (act != this->getroot()) && (act->getfather()->getcolor() == 'r') ){
if(act->getfather() == act->getfather()->getfather()->getleft()){
rbtnode* uncle = act->getfather()->getfather()->getright();
if(uncle->getcolor() == 'r'){ // przypadek pierwszy : czerwony wujek
act->getfather()->setcolor('b');
uncle->setcolor('b');
act->getfather()->getfather()->setcolor('r');
act = act->getfather()->getfather();
continue;
}
if(act == act->getfather()->getright()){ //wujek czarny z prawej, dodany jest prawym synem
act = act->getfather();
rotate_left(act);
}
act->getfather()->setcolor('b'); //wujek czarny, dodany jest lewym synem
act->getfather()->getfather()->setcolor('r');
rotate_right(act->getfather()->getfather());
break;
}
/* Lustrzane przypadki */
else{
rbtnode* uncle = act->getfather()->getfather()->getleft();
if(uncle->getcolor() == 'r'){ //czerwony wujek, symetrycznie
act->getfather()->setcolor('b');
uncle->setcolor('b');
act->getfather()->getfather()->setcolor('r');
act = act->getfather()->getfather();
continue;
}
if(act == act->getfather()->getleft()){//czarny wujek z lewej, dodany lewym synem
act = act->getfather();
rotate_right(act);
}
act->getfather()->setcolor('b'); // czarny wujek, dodany prawyym synem
act->getfather()->getfather()->setcolor('r');
rotate_left(act->getfather()->getfather());
break;
}
}
this->getroot()->setcolor('b');
}
/*
* Balance the tree. Start balancing from the given node.
*/
void AVL_tree::balance(AVL_tree::Node *node) {
Node *tmp = NULL;
int diff = 0, diff2 = 0;;
while (node != NULL) {
diff = balance_index(node);
if (diff == 2) {
tmp = node->left_child;
diff2 = balance_index(tmp);
// Double rotation situation.
if (diff2 == -1) {
rotate_left(tmp->right_child);
}
rotate_right(node->left_child);
// node->parent is a new root of the subtree.
// Update heights to the root.
update_heights(node->parent);
break;
}
else if (diff == -2) {
tmp = node->right_child;
diff2 = balance_index(tmp);
// Double rotation situation.
if (diff2 == 1) {
rotate_right(tmp->left_child);
}
rotate_left(node->right_child);
// node->parent is a new root of the subtree.
// Update heights to the root.
update_heights(node->parent);
break;
}
// If subtree is balanced, just update node's height.
// Check parent subtree.
node->height = 1 + std::max(height(node->left_child),
height(node->right_child));
node = node->parent;
}
}
node* insert(node *root, int data){
if(root && root->data == data)
return root;
if(root == NULL){
root = (node *)malloc(sizeof(node ));
root->data = data;
root->h = 1;
root->left = NULL;
root->right = NULL;
return root;
}
printf("data: %d ", root->data);
if(root->data < data){
root->right = insert(root->right, data);
}
else{
root->left = insert(root->left, data);
}
// update the height
root->h = max(height(root->left), height(root->right)) + 1;
//check if the root node is unbalanced
// there can be four cases
int bf = balance_factor(root);
printf("bf is: %d\n", bf);
if(bf == 2){
if(balance_factor(root->left) == -1){
//left-right case
printf("left-right case..\n");
root->left = rotate_left(root->left);
}
// left-left case
printf("left-left case...\n");
return rotate_right(root);
}
else if(bf == -2){
if(balance_factor(root->right) == 1){
//the right-left case
printf("right-left case...\n");
root->right = rotate_right(root->right);
}
// right-right case
printf("right-right case...\n");
return rotate_left(root);
}
// compiler would not tell you anything if you forget
// to write this return statement
return root;
}
void restore_properties(Node *node)
{
if (parent(node) == nullptr) // Cenário A: node é a raiz
node->color = Node::BLACK;
else if (parent(node)->color == Node::BLACK) // Cenário B
return;
else if (uncle(node) and uncle(node)->color == Node::RED)
{
// Cenário C: pai e tio vermelhos
parent(node)->color = Node::BLACK;
uncle(node)->color = Node::BLACK;
grandparent(node)->color = Node::RED;
// Como o pai é vermelho, o avô não é nulo
restore_properties(grandparent(node));
} else
{
// Cenário D: pai vermelho, tio preto
auto C = node;
auto P = parent(node);
auto G = grandparent(node);
if (C == P->right and P == G->left)
{
rotate_left(G, P, C);
P = C;
} else if (node == P->left and P == G->right)
{
rotate_right(G, P, C);
P = C;
}
C = P;
P = G;
G = parent(P);
if (C == P->left)
rotate_right(G, P, C);
else
rotate_left(G, P, C);
// Corner case: após a rotação C é a nova raiz
if (G == nullptr)
root = C;
C->color = Node::BLACK;
P->color = Node::RED;
}
}
int main(){
int i1 = rotate_right(0x12345678, 4);
printf("%x\n", i1);
// NOTE speical case
i1 = rotate_right(0x12345678, 20);
printf("%x\n", i1);
i1 = rotate_right(0x12345678, 32);
printf("%x\n", i1);
i1 = rotate_right(0x12345678, 0);
printf("%x\n", i1);
return 0;
}
rbtree_node _rbtree_insert(rbtree_node node,int key,int val){
if(node == NULL){
return new_node(key,val,RED,NULL,NULL);
}
if(key < node->key)
node->left = _rbtree_insert(node->left,key,val);
else if(key > node->key){
node->right = _rbtree_insert(node->right,key,val);
}else node->val = val;
/*
* 1. 如果插入到右边,只需要变色.
* 2. 如果插入到左结点的左边,右旋,变成情况1.
* 3. 如果插入到左结点的右边,左旋,变成情况2.
* 根据递归的顺序,可以把这些操作统一,自底向上返回.
*/
/* 情况1:强制左倾 */
if( is_red(node->right) && !is_red(node->left) ){
node = rotate_left(node);
}
/* 情况2:调整平衡 */
if( is_red(node->left) && is_red(node->left->left) ){
node = rotate_right(node);
}
/* 情况3:分解4-node */
if( is_red(node->left) && is_red(node->right) ){
flip_colors(node);
}
return node;
}
/* 删除任意值辅助函数 */
rbtree_node _rbtree_delete(rbtree_node h,int key){
if(key < h->key){
if( !is_red(h->left) && !is_red(h->left->left))
h = move_red_left(h);
h->left = _rbtree_delete(h->left,key);
}else{
if( is_red(h->left) )
h = rotate_right(h);
if( key == h->key && h->right == NULL){
free(h);
return NULL;
}
if( !is_red(h->right) && !is_red(h->right->left) )
h = move_red_right(h);
if( key == h->key ){
//TODO:获得最小值
rbtree_node x = _rbtree_min(h->right);
h->key = x->key;
h->val = x->val;
h->right = _rbtree_delete_min(h->right);
}else{
h->right = _rbtree_delete(h->right,key);
}
}
}
void set_URFtoDLF(CubieCube *cubie, short idx) {
int x, j, k;
char corner6[] = { URF, UFL, ULB, UBR, DFR, DLF };
char other_corner[] = { DBL, DRB };
int b = idx % 720; // Permutation
int a = idx / 720; // Combination
for (j = 0; j < 8; j++) {
cubie->cp[j] = DRB; // Use DRB to invalidate all corners
}
for (j = 1, k; j < 6; j++) { // generate permutation from index b
k = b % (j + 1);
b /= j + 1;
while (k-- > 0) {
rotate_right(corner6, 0, j);
}
}
// generate combination and set corners
for (x = 5, j = DRB; j >= 0; j--) {
if (a - Cnk[j][x + 1] >= 0) {
cubie->cp[j] = corner6[x];
a -= Cnk[j][x-- + 1];
}
}
for (x = 0, j = URF; j <= DRB; j++) {
if (cubie->cp[j] == DRB) {
cubie->cp[j] = other_corner[x++];
}
}
}
void set_URtoDF(CubieCube *cubie, int idx) {
int x, e, j, k;
char edge6[] = { UR, UF, UL, UB, DR, DF };
char other_edge[] = { DL, DB, FR, FL, BL, BR };
int b = idx % 720; // Permutation
int a = idx / 720; // Combination
for (e = 0; e < 12; e++) {
cubie->ep[e] = BR;// Use BR to invalidate all edges
}
for (j = 1, k; j < 6; j++) { // generate permutation from index b
k = b % (j + 1);
b /= j + 1;
while (k-- > 0) {
rotate_right(edge6, 0, j);
}
}
// generate combination and set edges
for (x = 5, j = BR; j >= 0; j--) {
if (a - Cnk[j][x + 1] >= 0) {
cubie->ep[j] = edge6[x];
a -= Cnk[j][x-- + 1];
}
}
// set the remaining edges DL..BR
for (x = 0, j = UR; j <= BR; j++) {
if (cubie->ep[j] == BR) {
cubie->ep[j] = other_edge[x++];
}
}
}
void delete_case5(rb_node* n, rb_tree* tree) {
rb_node* s = sibling(n);
/* this if statement is trivial,
due to Case 2 (even though Case two changed the sibling to a sibling's child,
the sibling's child can't be red, since no red parent can have a red child). */
/* the following statements just force the red to be on the left of the left of the parent,
or right of the right, so case six will rotate correctly. */
if (s->color == BLACK) {
if ((n == n->parent->left) &&
(s->right->color == BLACK) &&
(s->left->color == RED)) { /* this last test is trivial too due to cases 2-4. */
s->color = RED;
s->left->color = BLACK;
rotate_right(s, tree);
} else if ((n == n->parent->right) &&
(s->left->color == BLACK) &&
(s->right->color == RED)) {/* this last test is trivial too due to cases 2-4. */
s->color = RED;
s->right->color = BLACK;
rotate_left(s, tree);
}
}
delete_case6(n, tree);
}
void pulse_fine_rotate_right(){
rotate_right(9);
current_pulse_delay = 140;
start_pulse_timer(40);
pulse_fn = pulse_fine_rotate_right;
is_pulsing = true;
}
void erase(Treap* &node, int key)
{
if(node->key > key)
{
erase(node->left, key);
balance(node);
}
else if(node->key < key)
{
erase(node->right, key);
balance(node);
}
else
{
if(node->left == nil && node->right == nil)
{
delete node;
node = nil;
return;
}
if(node->left->pri > node->right->pri)
{
rotate_left(node);
erase(node->right, key);
balance(node);
}
else
{
rotate_right(node);
erase(node->left, key);
balance(node);
}
}
}
void balance_tree(BinaryTree** tree){
BinaryTree** child;
if(balance_factor(*tree) == 2){
child = &(*tree)->left;
if(balance_factor(*child) == -1){
rotate_left(child);
}
rotate_right(tree);
}else if(balance_factor(*tree) == -2){
child = &(*tree)->right;
if(balance_factor(*child) == 1){
rotate_right(child);
}
rotate_left(tree);
}
}
static wtreeNode_t* purge_rc(wtreeRoot_t* root, wtreeNode_t* node) {
if(!root)
return NULL;
if(node->right) {
node->right = purge_rc(root, node->right);
node = merge_next(root, node);
}
if(node->left) {
node->left = purge_rc(root, node->left);
node = merge_prev(root, node);
}
if (node->size == node->base_size) {
if(node->left && (node->left->base_size != node->left->size)) {
node = rotate_right(node);
node->right = purge_rc(root, node->right);
} else if(node->right && (node->right->base_size != node->right->size)) {
node = rotate_left(node);
node->left = purge_rc(root, node->left);
} else if(!node->left && !node->right) {
if(root->adapter->onfree) {
if(root->adapter->onremoved) root->adapter->onremoved(node, root->ext_ctx, FALSE);
root->adapter->onfree(node->top - node->base_size, node->base_size,node, root->ext_ctx);
return NULL;
}
}
}
return node;
}
/* Returns bnode if it wasn't freed by merge, left sibling otherwise. */
static bnode_t*
rebalance_bnode(piojo_btree_t *tree, size_t pidx, bnode_t *bnode,
bnode_t *parent)
{
bnode_t *lsibling=NULL, *rsibling=NULL;
if (pidx > 0){
lsibling = parent->children[pidx - 1];
}
if (pidx < parent->ecnt){
rsibling = parent->children[pidx + 1];
}
if (lsibling != NULL && lsibling->ecnt >= tree->cmin){
PIOJO_ASSERT(bnode->ecnt > 0 && bnode->ecnt < tree->cmax - 1);
PIOJO_ASSERT(lsibling->ecnt > 1);
rotate_right(pidx - 1, lsibling, bnode, parent, tree);
}else if (rsibling != NULL && rsibling->ecnt >= tree->cmin){
PIOJO_ASSERT(bnode->ecnt > 0 && bnode->ecnt < tree->cmax - 1);
PIOJO_ASSERT(rsibling->ecnt > 1);
rotate_left(pidx, bnode, rsibling, parent, tree);
}else if (lsibling != NULL){
merge_bnodes(tree, pidx - 1, lsibling, bnode, parent);
return lsibling;
}else{
PIOJO_ASSERT(rsibling);
merge_bnodes(tree, pidx, bnode, rsibling, parent);
}
return bnode;
}
void manual_control(void){
uint8_t len = TWI_read(s);
if(len){
switch(s[0]){
case CMD_MANUAL:
switch(s[2]){
case LEFT:
manual_left();
break;
case RIGHT:
manual_right();
break;
case FORWARD:
manual_forward();
break;
case REVERSE:
manual_reverse();
break;
case ROTATE_LEFT:
rotate_left();
break;
case ROTATE_RIGHT:
rotate_right();
break;
case STOP:
manual_stop();
break;
}
break;
}
}
}
void set_FRtoBR(CubieCube *cubie, short idx) {
int x, e, j, k;
char slice_edge[] = { FR, FL, BL, BR };
char other_edge[] = { UR, UF, UL, UB, DR, DF, DL, DB };
int b = idx % 24; // Permutation
int a = idx / 24; // Combination
for (e = 0; e < 12; e++) {
cubie->ep[e] = DB; // Use UR to invalidate all edges
}
for (j = 1, k; j < 4; j++) { // generate permutation from index b
k = b % (j + 1);
b /= j + 1;
while (k-- > 0) {
rotate_right(slice_edge, 0, j);
}
}
// generate combination and set slice edges
for (x = 3, j = UR; j <= BR; j++) {
if (a - Cnk[11 - j][x + 1] >= 0) {
cubie->ep[j] = slice_edge[3 - x];
a -= Cnk[11 - j][x-- + 1];
}
}
// set the remaining edges UR..DB
for (x = 0, j = UR; j <= BR; j++) {
if (cubie->ep[j] == DB) {
cubie->ep[j] = other_edge[x++];
}
}
}
struct rbtree *rb_insert(struct rbtree *tree, struct rbtree *node)
{
node->left = node->right = NULL;
node->red = false;
if (!tree) {
node->red = true;
return node;
}
if (is_red(tree->left) && is_red(tree->right))
color_flip(tree);
if (node->key < tree->key)
tree->left = rb_insert(tree->left, node);
else
tree->right = rb_insert(tree->right, node);
if (is_red(tree->right))
tree = rotate_left(tree);
if (is_red(tree->left) && is_red(tree->left->left))
tree = rotate_right(tree);
return tree;
}
void preference()
{
if(d1[k]==1)
{
rotate_right();
}
else if (d1[k]==2)
{
move_forward_mm(COUNT_MM_40);
}
else if (d1[k]==3)
{
rotate_left();
}
else if (d1[k]==9)
{
PORTC=0x08;
_delay_ms(500);
PORTC=0x00;
rotate_left_end();
move_forward_mm(COUNT_MM_80_E);
move_forward_mm(COUNT_MM_80_E);
move_forward_mm(COUNT_MM_80_E);
motion_set(Stop);
tag=1;
}
}
void rotate_dir(unsigned char speed, unsigned char direction){
if (direction == DIR_LEFT){
rotate_left(speed);
} else {
rotate_right(speed);
}
}
static Dict_node * dsw_tree_to_vine (Dict_node *root)
{
Dict_node *vine_tail, *vine_head, *rest;
Dict_node vh;
vine_head = &vh;
vine_head->left = NULL;
vine_head->right = root;
vine_tail = vine_head;
rest = root;
while (NULL != rest)
{
/* If no left, we are done, do the right */
if (NULL == rest->left)
{
vine_tail = rest;
rest = rest->right;
}
/* eliminate the left subtree */
else
{
rest = rotate_right(rest);
vine_tail->right = rest;
}
}
return vh.right;
}
static gfc_bbt *
insert (gfc_bbt *new_bbt, gfc_bbt *t, compare_fn compare)
{
int c;
if (t == NULL)
return new_bbt;
c = (*compare) (new_bbt, t);
if (c < 0)
{
t->left = insert (new_bbt, t->left, compare);
if (t->priority < t->left->priority)
t = rotate_right (t);
}
else if (c > 0)
{
t->right = insert (new_bbt, t->right, compare);
if (t->priority < t->right->priority)
t = rotate_left (t);
}
else /* if (c == 0) */
gfc_internal_error("insert_bbt(): Duplicate key found!");
return t;
}
/* insert a key into the root of a tree */
node *
root_insert_node (node * run, int key)
{
if (!run) {
node *new_node = (node *) malloc (sizeof (node));
if (!new_node)
return NULL;
new_node->key = key;
new_node->left = NULL;
new_node->right = NULL;
return new_node;
}
if (key < run->key) {
run->left = root_insert_node (run->left, key);
run = rotate_right (run);
} else {
run->right = root_insert_node (run->right, key);
run = rotate_left (run);
}
return run;
}
/*
* Twofish Encryption
*/
void Twofish::encrypt_n(const byte in[], byte out[], size_t blocks) const
{
for(size_t i = 0; i != blocks; ++i)
{
u32bit A = load_le<u32bit>(in, 0) ^ RK[0];
u32bit B = load_le<u32bit>(in, 1) ^ RK[1];
u32bit C = load_le<u32bit>(in, 2) ^ RK[2];
u32bit D = load_le<u32bit>(in, 3) ^ RK[3];
for(size_t j = 0; j != 16; j += 2)
{
u32bit X, Y;
X = SB[ get_byte(3, A)] ^ SB[256+get_byte(2, A)] ^
SB[512+get_byte(1, A)] ^ SB[768+get_byte(0, A)];
Y = SB[ get_byte(0, B)] ^ SB[256+get_byte(3, B)] ^
SB[512+get_byte(2, B)] ^ SB[768+get_byte(1, B)];
X += Y;
Y += X + RK[2*j + 9];
X += RK[2*j + 8];
C = rotate_right(C ^ X, 1);
D = rotate_left(D, 1) ^ Y;
X = SB[ get_byte(3, C)] ^ SB[256+get_byte(2, C)] ^
SB[512+get_byte(1, C)] ^ SB[768+get_byte(0, C)];
Y = SB[ get_byte(0, D)] ^ SB[256+get_byte(3, D)] ^
SB[512+get_byte(2, D)] ^ SB[768+get_byte(1, D)];
X += Y;
Y += X + RK[2*j + 11];
X += RK[2*j + 10];
A = rotate_right(A ^ X, 1);
B = rotate_left(B, 1) ^ Y;
}
C ^= RK[4];
D ^= RK[5];
A ^= RK[6];
B ^= RK[7];
store_le(out, C, D, A, B);
in += BLOCK_SIZE;
out += BLOCK_SIZE;
}
}
/*
* Twofish Decryption
*/
void Twofish::decrypt_n(const byte in[], byte out[], size_t blocks) const
{
for(size_t i = 0; i != blocks; ++i)
{
u32bit A = load_le<u32bit>(in, 0) ^ m_RK[4];
u32bit B = load_le<u32bit>(in, 1) ^ m_RK[5];
u32bit C = load_le<u32bit>(in, 2) ^ m_RK[6];
u32bit D = load_le<u32bit>(in, 3) ^ m_RK[7];
for(size_t j = 0; j != 16; j += 2)
{
u32bit X, Y;
X = m_SB[ get_byte(3, A)] ^ m_SB[256+get_byte(2, A)] ^
m_SB[512+get_byte(1, A)] ^ m_SB[768+get_byte(0, A)];
Y = m_SB[ get_byte(0, B)] ^ m_SB[256+get_byte(3, B)] ^
m_SB[512+get_byte(2, B)] ^ m_SB[768+get_byte(1, B)];
X += Y;
Y += X + m_RK[39 - 2*j];
X += m_RK[38 - 2*j];
C = rotate_left(C, 1) ^ X;
D = rotate_right(D ^ Y, 1);
X = m_SB[ get_byte(3, C)] ^ m_SB[256+get_byte(2, C)] ^
m_SB[512+get_byte(1, C)] ^ m_SB[768+get_byte(0, C)];
Y = m_SB[ get_byte(0, D)] ^ m_SB[256+get_byte(3, D)] ^
m_SB[512+get_byte(2, D)] ^ m_SB[768+get_byte(1, D)];
X += Y;
Y += X + m_RK[37 - 2*j];
X += m_RK[36 - 2*j];
A = rotate_left(A, 1) ^ X;
B = rotate_right(B ^ Y, 1);
}
C ^= m_RK[0];
D ^= m_RK[1];
A ^= m_RK[2];
B ^= m_RK[3];
store_le(out, C, D, A, B);
in += BLOCK_SIZE;
out += BLOCK_SIZE;
}
}
