net/cor/kpacket_parse.c

   1 /*
   2  *      Connection oriented routing
   3  *      Copyright (C) 2007-2008 Michael Blizek
   4  *
   5  *      This program is free software; you can redistribute it and/or
   6  *      modify it under the terms of the GNU General Public License
   7  *      as published by the Free Software Foundation; either version 2
   8  *      of the License, or (at your option) any later version.
   9  *
  10  *      This program is distributed in the hope that it will be useful,
  11  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  *      GNU General Public License for more details.
  14  *
  15  *      You should have received a copy of the GNU General Public License
  16  *      along with this program; if not, write to the Free Software
  17  *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
  18  *      02110-1301, USA.
  19  */
  20
  21 #include <asm/byteorder.h>
  22
  23 #include "cor.h"
  24
  25 /* not used, avoid compiler warning
  26  * static __u64 pull_u64(struct sk_buff *skb, int convbo)
  27 {
  28         char *ptr = cor_pull_skb(skb, 8);
  29
  30         __u64 ret = 0;
  31
  32         BUG_ON(0 == ptr);
  33
  34         ((char *)&ret)[0] = ptr[0];
  35         ((char *)&ret)[1] = ptr[1];
  36         ((char *)&ret)[2] = ptr[2];
  37         ((char *)&ret)[3] = ptr[3];
  38         ((char *)&ret)[4] = ptr[4];
  39         ((char *)&ret)[5] = ptr[5];
  40         ((char *)&ret)[6] = ptr[6];
  41         ((char *)&ret)[7] = ptr[7];
  42
  43         if (convbo)
  44                 return be64_to_cpu(ret);
  45         return ret;
  46 } */
  47
  48 static __u32 pull_u32(struct sk_buff *skb, int convbo)
  49 {
  50         char *ptr = cor_pull_skb(skb, 4);
  51
  52         __u32 ret = 0;
  53
  54         BUG_ON(0 == ptr);
  55
  56         ((char *)&ret)[0] = ptr[0];
  57         ((char *)&ret)[1] = ptr[1];
  58         ((char *)&ret)[2] = ptr[2];
  59         ((char *)&ret)[3] = ptr[3];
  60
  61         if (convbo)
  62                 return be32_to_cpu(ret);
  63         return ret;
  64 }
  65
  66 static __u16 pull_u16(struct sk_buff *skb, int convbo)
  67 {
  68         char *ptr = cor_pull_skb(skb, 2);
  69
  70         __u16 ret = 0;
  71
  72         BUG_ON(0 == ptr);
  73
  74         ((char *)&ret)[0] = ptr[0];
  75         ((char *)&ret)[1] = ptr[1];
  76
  77         if (convbo)
  78                 return be16_to_cpu(ret);
  79         return ret;
  80 }
  81
  82 /* not used, avoid compiler warning
  83  * static __u8 pull_u8(struct sk_buff *skb)
  84 {
  85         char *ptr = cor_pull_skb(skb, 1);
  86         BUG_ON(0 == ptr);
  87         return *ptr;
  88 } */
  89
  90 static void discard_speed(struct neighbor *nb, struct sk_buff *skb)
  91 {
  92         pull_u32(skb, 1); /* speed */
  93 }
  94
  95 #warning todo
  96 static void parse_speed(struct neighbor *nb, struct sk_buff *skb,
  97                 struct conn *rconn)
  98 {
  99         __u16 speed = pull_u32(skb, 1);
 100 }
 101
 102 static void discard_conn_success(struct neighbor *nb, struct sk_buff *skb)
 103 {
 104         __u32 conn_id = pull_u32(skb, 1);
 105         struct control_msg_out *cm = alloc_control_msg();
 106         if (cm != 0)
 107                 send_reset_conn(cm, nb, conn_id);
 108 }
 109
 110 static void parse_conn_success(struct neighbor *nb, struct sk_buff *skb,
 111                 struct conn *rconn)
 112 {
 113         struct conn *sconn = rconn->reversedir;
 114
 115         __u32 conn_id = pull_u32(skb, 1);
 116
 117         BUG_ON(sconn == 0);
 118
 119         if (unlikely(unlikely(sconn->targettype != TARGET_OUT) ||
 120                         unlikely(sconn->target.out.nb != nb) ||
 121                         unlikely(sconn->target.out.conn_id != 0))) {
 122                 struct control_msg_out *cm = alloc_control_msg();
 123                 if (likely(cm != 0))
 124                         send_reset_conn(cm, nb, conn_id);
 125                 return;
 126         }
 127
 128         mutex_lock(&(sconn->rcv_lock));
 129         sconn->target.out.conn_id = conn_id;
 130         flush_out(sconn);
 131         mutex_unlock(&(sconn->rcv_lock));
 132 }
 133
 134 static void parse_reset(struct neighbor *nb, struct sk_buff *skb,
 135                 struct conn *rconn)
 136 {
 137         atomic_cmpxchg(&(rconn->reversedir->isreset), 0, 1);
 138         reset_conn(rconn);
 139 }
 140
 141 static void conn_cmd(struct neighbor *nb, struct sk_buff *skb, __u8 code,
 142                 void (*parsefunc)(struct neighbor *nb, struct sk_buff *skb,
 143                 struct conn *rconn), void (*readdiscardfunc)(
 144                 struct neighbor *nb, struct sk_buff *skb))
 145 {
 146         __u32 conn_id = pull_u32(skb, 1);
 147         struct conn *rconn = get_conn(conn_id);
 148
 149         if (unlikely(rconn == 0))
 150                 goto err;
 151
 152         BUG_ON(SOURCE_IN != rconn->sourcetype);
 153         BUG_ON(0 == rconn->reversedir);
 154
 155         if (unlikely(rconn->source.in.nb != nb))
 156                 goto err;
 157
 158         parsefunc(nb, skb, rconn);
 159         if (0) {
 160 err:
 161                 readdiscardfunc(nb, skb);
 162         }
 163         if (rconn != 0)
 164                 kref_put(&(rconn->ref), free_conn);
 165 }
 166
 167 static void parse_conndata(struct neighbor *nb, struct sk_buff *skb)
 168 {
 169         __u32 conn_id = pull_u32(skb, 1);
 170         __u32 seqno = pull_u32(skb, 1);
 171         __u16 datalength = pull_u16(skb, 1);
 172         char *data = cor_pull_skb(skb, datalength);
 173
 174         BUG_ON(data == 0);
 175
 176         conn_rcv_buildskb(data, datalength, conn_id, seqno);
 177 }
 178
 179 static void parse_connect(struct neighbor *nb, struct sk_buff *skb)
 180 {
 181         struct conn *rconn;
 182         __u32 conn_id = pull_u32(skb, 1);
 183         struct control_msg_out *cm = alloc_control_msg();
 184
 185         if (unlikely(cm == 0))
 186                 return;
 187
 188         rconn = alloc_conn(GFP_KERNEL);
 189
 190         if (unlikely(rconn == 0))
 191                 goto err;
 192
 193         if (conn_init_out(rconn->reversedir, nb))
 194                 goto err;
 195
 196         rconn->reversedir->target.out.conn_id = conn_id;
 197
 198         send_connect_success(cm, nb, rconn->reversedir->target.out.conn_id,
 199                         rconn->source.in.conn_id);
 200
 201         if (0) {
 202 err:
 203                 send_reset_conn(cm, nb, conn_id);
 204         }
 205 }
 206
 207 static void kernel_packet2(struct neighbor *nb, struct sk_buff *skb,
 208                 __u32 seqno)
 209 {
 210         struct control_msg_out *cm = alloc_control_msg();
 211         int ack = 0;
 212
 213         if (unlikely(cm == 0))
 214                 return;
 215
 216         while (1) {
 217                 struct control_msg_out *cm_tmp;
 218                 __u32 cookie;
 219                 __u32 respdelay;
 220
 221                 __u32 conn_id;
 222                 __u32 seqno;
 223
 224                 __u8 *codeptr = cor_pull_skb(skb, 1);
 225                 __u8 code;
 226
 227                 if (codeptr == 0)
 228                         break;
 229
 230                 code = *codeptr;
 231
 232                 switch (code) {
 233                 case KP_PADDING:
 234                         break;
 235                 case KP_PING:
 236                         cookie = pull_u32(skb, 0);
 237                         cm_tmp = alloc_control_msg();
 238                         if (likely(cm != 0)) {
 239                                 send_pong(cm_tmp, nb, cookie);
 240                         }
 241                         break;
 242                 case KP_PONG:
 243                         cookie = pull_u32(skb, 0);
 244                         respdelay = pull_u32(skb, 1);
 245                         ping_resp(nb, cookie, respdelay);
 246                         ack = 1;
 247                         break;
 248                 case KP_ACK:
 249                         conn_id = pull_u32(skb, 1);
 250                         seqno = pull_u32(skb, 1);
 251                         ack_received(nb, conn_id, seqno);
 252                         if (conn_id != 0)
 253                                 ack = 1;
 254                         break;
 255                 case KP_SPEED:
 256                         conn_cmd(nb, skb, code, parse_speed, discard_speed);
 257                         ack = 1;
 258                         break;
 259                 case KP_CONNECT:
 260                         parse_connect(nb, skb);
 261                         ack = 1;
 262                         break;
 263                 case KP_CONNECT_SUCCESS:
 264                         conn_cmd(nb, skb, code, parse_conn_success,
 265                                         discard_conn_success);
 266                         ack = 1;
 267                         break;
 268                 case KP_CONN_DATA:
 269                         parse_conndata(nb, skb);
 270                         ack = 1;
 271                         break;
 272                 case KP_RESET_CONN:
 273                         conn_cmd(nb, skb, code, parse_reset, 0);
 274                         ack = 1;
 275                         break;
 276                 default:
 277                         BUG();
 278                 }
 279         }
 280
 281         if (ack)
 282                 send_ack(cm, nb, 0, seqno);
 283         else
 284                 free_control_msg(cm);
 285 }
 286
 287 void kernel_packet(struct neighbor *nb, struct sk_buff *skb, __u32 seqno)
 288 {
 289         struct sk_buff *skb2 = skb_clone(skb, __GFP_DMA | GFP_KERNEL);
 290
 291         while (1) {
 292                 __u8 *codeptr = cor_pull_skb(skb2, 1);
 293                 __u8 code;
 294
 295                 char *lengthptr;
 296                 __u32 length;
 297
 298                 if (codeptr == 0)
 299                         break;
 300                 code = *codeptr;
 301
 302                 switch (code) {
 303                 case KP_PADDING:
 304                         break;
 305                 case KP_PING:
 306                         if (cor_pull_skb(skb2, 4) == 0)
 307                                 goto discard;
 308                         break;
 309                 case KP_PONG:
 310                         if (cor_pull_skb(skb2, 8) == 0)
 311                                 goto discard;
 312                         break;
 313                 case KP_ACK:
 314                         if (cor_pull_skb(skb2, 8) == 0)
 315                                 goto discard;
 316                         break;
 317                 case KP_SPEED:
 318                         if (cor_pull_skb(skb2, 6) == 0)
 319                                 goto discard;
 320                         break;
 321                 case KP_CONNECT:
 322                         if (cor_pull_skb(skb2, 4) == 0)
 323                                 goto discard;
 324                         break;
 325                 case KP_CONNECT_SUCCESS:
 326                         if (cor_pull_skb(skb2, 8) == 0)
 327                                 goto discard;
 328                         break;
 329                 case KP_CONN_DATA:
 330                         if (cor_pull_skb(skb2, 8) == 0)
 331                                 goto discard;
 332                         lengthptr = cor_pull_skb(skb2, 2);
 333                         if (lengthptr == 0)
 334                                 goto discard;
 335                         length = ntohs(*((__u16 *)lengthptr));
 336                         if (cor_pull_skb(skb2, length) == 0)
 337                                 goto discard;
 338                         break;
 339                 case KP_RESET_CONN:
 340                         if (cor_pull_skb(skb2, 4) == 0)
 341                                 goto discard;
 342                         break;
 343                 default:
 344                         goto discard;
 345                 }
 346         }
 347         kfree_skb(skb2);
 348         kernel_packet2(nb, skb, seqno);
 349         kfree_skb(skb);
 350         return;
 351 discard:
 352         kfree_skb(skb2);
 353         kfree_skb(skb);
 354 }