rijndaelCtx *
rijndael_set_key(rijndaelCtx *ctx, const uint32_t *in_key, const uint32_t key_len,
int encrypt)
{
uint32_t i,
t,
u,
v,
w;
uint32_t *e_key = ctx->e_key;
uint32_t *d_key = ctx->d_key;
ctx->decrypt = !encrypt;
ctx->k_len = (key_len + 31) / 32;
e_key[0] = io_swap(in_key[0]);
e_key[1] = io_swap(in_key[1]);
e_key[2] = io_swap(in_key[2]);
e_key[3] = io_swap(in_key[3]);
switch (ctx->k_len)
{
case 4:
t = e_key[3];
for (i = 0; i < 10; ++i)
loop4(i);
break;
case 6:
e_key[4] = io_swap(in_key[4]);
t = e_key[5] = io_swap(in_key[5]);
for (i = 0; i < 8; ++i)
loop6(i);
break;
case 8:
e_key[4] = io_swap(in_key[4]);
e_key[5] = io_swap(in_key[5]);
e_key[6] = io_swap(in_key[6]);
t = e_key[7] = io_swap(in_key[7]);
for (i = 0; i < 7; ++i)
loop8(i);
break;
}
if (!encrypt)
{
d_key[0] = e_key[0];
d_key[1] = e_key[1];
d_key[2] = e_key[2];
d_key[3] = e_key[3];
for (i = 4; i < 4 * ctx->k_len + 24; ++i)
imix_col(d_key[i], e_key[i]);
}
return ctx;
}
void encrypt(const u4byte in_blk[4], u4byte out_blk[4])
{ u4byte a,b,c,d,p,q,r,s,u,v;
p = io_swap(in_blk[0]); q = io_swap(in_blk[1]);
r = io_swap(in_blk[2]); s = io_swap(in_blk[3]);
p ^= l_key[48]; q ^= l_key[49]; r ^= l_key[50]; s ^= l_key[51];
p *= l_key[52]; q *= l_key[53]; r *= l_key[54]; s *= l_key[55];
bp_fun(a, b, c, d, p, q, r, s);
f_fun(a, b, c, d, l_key);
f_fun(c, d, a, b, l_key + 4);
f_fun(a, b, c, d, l_key + 8);
f_fun(c, d, a, b, l_key + 12);
f_fun(a, b, c, d, l_key + 16);
f_fun(c, d, a, b, l_key + 20);
f_fun(a, b, c, d, l_key + 24);
f_fun(c, d, a, b, l_key + 28);
f_fun(a, b, c, d, l_key + 32);
f_fun(c, d, a, b, l_key + 36);
f_fun(a, b, c, d, l_key + 40);
f_fun(c, d, a, b, l_key + 44);
ibp_fun(p, q, r, s, a, b, c, d);
p *= l_key[68]; q *= l_key[69]; r *= l_key[70]; s *= l_key[71];
p ^= l_key[60]; q ^= l_key[61]; r ^= l_key[62]; s ^= l_key[63];
out_blk[0] = io_swap(p); out_blk[1] = io_swap(q);
out_blk[2] = io_swap(r); out_blk[3] = io_swap(s);
};
void decrypt (const u4byte in_blk[4], u4byte out_blk[4]) {
u4byte t, u, blk[4];
blk[0] = io_swap (in_blk[0]);
blk[1] = io_swap (in_blk[1]);
blk[2] = io_swap (in_blk[2]);
blk[3] = io_swap (in_blk[3]);
f_rnd (blk, 88);
f_rnd (blk, 80);
f_rnd (blk, 72);
f_rnd (blk, 64);
f_rnd (blk, 56);
f_rnd (blk, 48);
i_rnd (blk, 40);
i_rnd (blk, 32);
i_rnd (blk, 24);
i_rnd (blk, 16);
i_rnd (blk, 8);
i_rnd (blk, 0);
out_blk[0] = io_swap (blk[0]);
out_blk[1] = io_swap (blk[1]);
out_blk[2] = io_swap (blk[2]);
out_blk[3] = io_swap (blk[3]);
}
u4byte * set_key (const u4byte in_key[], const u4byte key_len) {
u4byte i, j, t, u, cm, cr, lk[8], tm[8], tr[8];
for (i = 0; i < key_len / 32; ++i)
lk[i] = io_swap (in_key[i]);
for (; i < 8; ++i)
lk[i] = 0;
cm = 0x5a827999;
cr = 19;
for (i = 0; i < 96; i += 8)
{
for (j = 0; j < 8; ++j)
{
tm[j] = cm;
cm += 0x6ed9eba1;
tr[j] = cr;
cr += 17;
}
k_rnd (lk, tr, tm);
for (j = 0; j < 8; ++j)
{
tm[j] = cm;
cm += 0x6ed9eba1;
tr[j] = cr;
cr += 17;
}
k_rnd (lk, tr, tm);
l_key[i + 0] = lk[0];
l_key[i + 1] = lk[2];
l_key[i + 2] = lk[4];
l_key[i + 3] = lk[6];
l_key[i + 4] = lk[7];
l_key[i + 5] = lk[5];
l_key[i + 6] = lk[3];
l_key[i + 7] = lk[1];
}
return l_key;
}
void
rijndael_decrypt(rijndaelCtx *ctx, const uint32_t *in_blk, uint32_t *out_blk)
{
uint32_t b0[4],
b1[4],
*kp;
uint32_t k_len = ctx->k_len;
uint32_t *e_key = ctx->e_key;
uint32_t *d_key = ctx->d_key;
b0[0] = io_swap(in_blk[0]) ^ e_key[4 * k_len + 24];
b0[1] = io_swap(in_blk[1]) ^ e_key[4 * k_len + 25];
b0[2] = io_swap(in_blk[2]) ^ e_key[4 * k_len + 26];
b0[3] = io_swap(in_blk[3]) ^ e_key[4 * k_len + 27];
kp = d_key + 4 * (k_len + 5);
if (k_len > 6)
{
i_nround(b1, b0, kp);
i_nround(b0, b1, kp);
}
if (k_len > 4)
{
i_nround(b1, b0, kp);
i_nround(b0, b1, kp);
}
i_nround(b1, b0, kp);
i_nround(b0, b1, kp);
i_nround(b1, b0, kp);
i_nround(b0, b1, kp);
i_nround(b1, b0, kp);
i_nround(b0, b1, kp);
i_nround(b1, b0, kp);
i_nround(b0, b1, kp);
i_nround(b1, b0, kp);
i_lround(b0, b1, kp);
out_blk[0] = io_swap(b0[0]);
out_blk[1] = io_swap(b0[1]);
out_blk[2] = io_swap(b0[2]);
out_blk[3] = io_swap(b0[3]);
}
void
rijndael_encrypt(rijndaelCtx *ctx, const uint32_t *in_blk, uint32_t *out_blk)
{
uint32_t k_len = ctx->k_len;
uint32_t *e_key = ctx->e_key;
uint32_t b0[4],
b1[4],
*kp;
b0[0] = io_swap(in_blk[0]) ^ e_key[0];
b0[1] = io_swap(in_blk[1]) ^ e_key[1];
b0[2] = io_swap(in_blk[2]) ^ e_key[2];
b0[3] = io_swap(in_blk[3]) ^ e_key[3];
kp = e_key + 4;
if (k_len > 6)
{
f_nround(b1, b0, kp);
f_nround(b0, b1, kp);
}
if (k_len > 4)
{
f_nround(b1, b0, kp);
f_nround(b0, b1, kp);
}
f_nround(b1, b0, kp);
f_nround(b0, b1, kp);
f_nround(b1, b0, kp);
f_nround(b0, b1, kp);
f_nround(b1, b0, kp);
f_nround(b0, b1, kp);
f_nround(b1, b0, kp);
f_nround(b0, b1, kp);
f_nround(b1, b0, kp);
f_lround(b0, b1, kp);
out_blk[0] = io_swap(b0[0]);
out_blk[1] = io_swap(b0[1]);
out_blk[2] = io_swap(b0[2]);
out_blk[3] = io_swap(b0[3]);
}
u4byte *set_key(const u4byte in_key[], const u4byte key_len)
{ u4byte lk[8], v[2], lout[8];
u4byte i, j, k, w;
if(!lb_init)
{
for(i = 0; i < 256; ++i)
{
l_box[0][i] = ((u4byte)(s_box[i]));
l_box[1][i] = ((u4byte)(s_box[i])) << 8;
l_box[2][i] = ((u4byte)(s_box[i])) << 16;
l_box[3][i] = ((u4byte)(s_box[i])) << 24;
}
lb_init = 1;
}
v[0] = bswap(v_0); v[1] = bswap(v_1);
lk[0] = io_swap(in_key[0]); lk[1] = io_swap(in_key[1]);
lk[2] = io_swap(in_key[2]); lk[3] = io_swap(in_key[3]);
lk[4] = io_swap(key_len > 128 ? in_key[4] : k2_0);
lk[5] = io_swap(key_len > 128 ? in_key[5] : k2_1);
lk[6] = io_swap(key_len > 192 ? in_key[6] : k3_0);
lk[7] = io_swap(key_len > 192 ? in_key[7] : k3_1);
g_fun(lk, lout, v);
for(i = 0; i < 8; ++i)
{
g_fun(lk, lout, v);
for(j = 0; j < 4; ++j)
{
// this is complex because of a byte swap in each 32 bit output word
k = 2 * (48 - 16 * j + 2 * (i / 2) - i % 2);
((u1byte*)l_key)[k + 3] = ((u1byte*)lout)[j];
((u1byte*)l_key)[k + 2] = ((u1byte*)lout)[j + 16];
((u1byte*)l_key)[k + 19] = ((u1byte*)lout)[j + 8];
((u1byte*)l_key)[k + 18] = ((u1byte*)lout)[j + 24];
((u1byte*)l_key)[k + 131] = ((u1byte*)lout)[j + 4];
((u1byte*)l_key)[k + 130] = ((u1byte*)lout)[j + 20];
((u1byte*)l_key)[k + 147] = ((u1byte*)lout)[j + 12];
((u1byte*)l_key)[k + 146] = ((u1byte*)lout)[j + 28];
}
}
for(i = 52; i < 60; ++i)
{
l_key[i] |= 1; l_key[i + 12] = mod_inv(l_key[i]);
}
for(i = 0; i < 48; i += 4)
{
bp2_fun(l_key[i], l_key[i + 1]);
}
return (u4byte*)&l_key;
};
int main (int argc, char **argv)
{
pwr_tStatus sts = 1;
io_tCtx io_ctx;
io_tCtx io_ctx_swap;
pwr_sClass_IOHandler *ihp;
int swap_io;
int close_io;
int init_io;
qcom_sQid qid = qcom_cNQid;
int tmo;
char mp[2000];
qcom_sGet get;
pwr_tTime now;
pwr_tTime next;
pwr_tTime after;
pwr_tDeltaTime cycle;
lst_sEntry *csup_lh;
int delay_action = 0;
pwr_sNode *nodep;
pwr_tBoolean old_emergency_break = 0;
if ( argc > 1) {
if ( strcmp( argv[1], "-m") == 0) {
io_methods_print();
exit(0);
}
if ( strcmp( argv[1], "-h") == 0) {
usage();
exit(0);
}
}
ihp = init(&qid, &csup_lh, &nodep);
plc_UtlWaitForPlc();
/* Prepare the swap context */
sts = io_init_swap(io_mProcess_IoComm, pwr_cNObjid, &io_ctx_swap, 1, ihp->CycleTimeBus);
for (close_io = swap_io = 0, init_io = 1;;) {
if (init_io) {
double f;
sts = io_init(io_mProcess_IoComm, pwr_cNObjid, &io_ctx, 1, ihp->CycleTimeBus);
if ( ODD(sts))
errh_SetStatus( PWR__SRUN);
#if defined(OS_ELN)
ker$clear_event( &sts, io_comm_terminate);
io_dioc_init();
io_dioc_start();
#endif
init_io = 0;
tmo = ihp->CycleTimeBus * 1000.;
f = floor(ihp->CycleTimeBus);
cycle.tv_sec = f;
cycle.tv_nsec = (ihp->CycleTimeBus - f) * 1.0e9;
cycle.tv_nsec++;
time_GetTimeMonotonic(&next);
time_Aadd(NULL, &next, &cycle);
}
get.maxSize = sizeof(mp);
get.data = mp;
qcom_Get(&sts,&qid, &get, tmo);
if (sts == QCOM__TMO || sts == QCOM__QEMPTY) {
if ( nodep->EmergBreakTrue && !old_emergency_break)
sts = io_swap(io_ctx_swap, io_eEvent_IoCommEmergencyBreak);
sts = io_read(io_ctx);
if (EVEN(sts)) {
ihp->IOReadWriteFlag = FALSE;
errh_Error("IO read, %m", sts);
}
sts = io_write(io_ctx);
if (EVEN(sts)) {
ihp->IOReadWriteFlag = FALSE;
errh_Error("IO write, %m", sts);
}
if ( nodep->EmergBreakTrue && !old_emergency_break)
sts = io_swap(io_ctx, io_eEvent_EmergencyBreak);
old_emergency_break = nodep->EmergBreakTrue;
if (swap_io)
{
sts = io_swap(io_ctx_swap, io_eEvent_IoCommSwap);
}
io_ScanSupLst( io_ctx->SupCtx);
time_GetTime(&now);
time_GetTimeMonotonic(&after);
next = after;
time_Aadd(NULL, &next, &cycle);
delay_action = csup_Exec(&sts, csup_lh, (pwr_tDeltaTime *) &next,
(pwr_tDeltaTime *) &after, &now);
if (delay_action == 2)
ihp->IOReadWriteFlag = FALSE;
aproc_TimeStamp(ihp->CycleTimeBus, 5);
} else {
ini_mEvent new_event;
qcom_sEvent *ep = (qcom_sEvent*) get.data;
new_event.m = ep->mask;
if (new_event.b.oldPlcStop && !swap_io) {
swap_io = 1;
close_io = 1;
errh_SetStatus(PWR__SRVRESTART);
} else if (new_event.b.swapDone && swap_io) {
swap_io = 0;
init_io = 1;
} else if (new_event.b.terminate) {
exit(0);
}
if (close_io) {
io_close(io_ctx);
#if defined(OS_ELN)
ker$signal( &sts, io_comm_terminate);
#endif
close_io = 0;
}
}
}
}
