Skip to content

Instantly share code, notes, and snippets.

@pdumais

pdumais/bgp.c

Created May 3, 2017 19:04
Show Gist options
  • Save pdumais/c07ab443af2cedb371cff9b61d7c68d4 to your computer and use it in GitHub Desktop.
Save pdumais/c07ab443af2cedb371cff9b61d7c68d4 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
bgp.c
#include "bgp.h"
#include <stdlib.h>
#include <string.h>
#define FSM_IDLE 0
#define FSM_CONNECT 1
#define FSM_ACTIVE 2
#define FSM_OPENSENT 3
#define FSM_OPENCONFIRM 4
#define FSM_ESTABLISHED 5
#define ATTR_ORIGIN 1
#define ATTR_AS_PATH 2
#define ATTR_NEXT_HOP 3
#define ATTR_MULTI_EXIT_DISC 4
#define ATTR_LOCAL_REF 5
#define ATTR_ATOMIC_AGGREGATE 6
#define ATTR_AGGREGATOR 7
#define BGP_ID 0x01010101
#ifdef _DEBUG
#include <stdio.h>
#define DEBUG(fmt, ...) printf(fmt, ##__VA_ARGS__)
#else
#define DEBUG(fmt, ...)
#endif
const char* state_names[] = {"IDLE","CONNECT","ACTIVE","OPEN SENT","OPEN CONFIRM", "ESTABLISHED"};
const char* type_codes[] = {"ORIGIN","AS_PATH","NEXT_HOP","MULTI_EXIT_DISC","LOCAL_PREF","ATOMIC_AGGREGATE","AGGREGATOR"};
const char* messages[] = {"OPEN","UPDATE","NOTIFICATION","KEEPALIVE"};
typedef struct
{
u8 ip[4];
u8 size;
} bgp_prefix;
typedef struct
{
u16 size;
char data[8];
u8 type;
} bgp_attribute;
void send_open_message(bgp_context* ctx)
{
int n;
bgp_message_open msg;
msg.header.type = 1;
msg.header.length = __builtin_bswap16(sizeof(msg));
for (n=0;n<16;n++) msg.header.marker[n] = 0xFF;
msg.as = __builtin_bswap16(ctx->config.as);
msg.version = 4;
msg.hold_time = 0;
msg.id = __builtin_bswap32(BGP_ID);
msg.optional_length = 0;
ctx->config.send_function(ctx,(char*)&msg,sizeof(msg));
}
void send_keepalive_message(bgp_context* ctx)
{
int n;
bgp_header msg;
msg.type = 4;
msg.length = __builtin_bswap16(sizeof(msg));
for (n=0;n<16;n++) msg.marker[n] = 0xFF;
ctx->config.send_function(ctx,(char*)&msg,sizeof(msg));
}
u8 transition(bgp_context* ctx, u8 newstate)
{
u8 valid = 0;
if (ctx->state == newstate) return 0;
switch (newstate)
{
case FSM_IDLE:
{
valid = 1;
}
break;
case FSM_CONNECT:
{
if (ctx->state == FSM_IDLE) valid = 1;
if (ctx->state == FSM_ACTIVE) valid = 1;
}
break;
case FSM_OPENSENT:
{
if (ctx->state == FSM_CONNECT) valid = 1;
if (ctx->state == FSM_ACTIVE) valid = 1;
}
break;
case FSM_ACTIVE:
{
if (ctx->state == FSM_CONNECT) valid = 1;
}
break;
case FSM_OPENCONFIRM:
{
if (ctx->state == FSM_OPENSENT) valid = 1;
}
break;
case FSM_ESTABLISHED:
{
if (ctx->state == FSM_OPENCONFIRM) valid = 1;
}
}
if (valid != 0)
{
ctx->state = newstate;
DEBUG("=================> %s <=======================\r\n",state_names[newstate]);
switch (newstate)
{
case FSM_IDLE:
{
ctx->config.on_reset_function(ctx);
}
break;
case FSM_CONNECT:
{
ctx->config.accept_function(ctx);
}
break;
case FSM_OPENSENT:
{
send_open_message(ctx);
ctx->config.manage_function(ctx);
}
break;
case FSM_ACTIVE:
{
ctx->config.connect_function(ctx);
}
break;
case FSM_OPENCONFIRM:
{
}
break;
case FSM_ESTABLISHED:
{
send_keepalive_message(ctx);
}
break;
}
return 0;
}
else
{
DEBUG("Invalid transition from %s to %s\r\n",state_names[ctx->state],state_names[newstate]);
}
return 1;
}
bgp_context* bgp_create_context()
{
bgp_context* ctx = (bgp_context*)malloc(sizeof(bgp_context));
ctx->state = FSM_IDLE;
ctx->session_fd = 0;
return ctx;
}
void bgp_release_context(bgp_context *ctx)
{
ctx->config.on_reset_function(ctx);
free(ctx);
}
void bgp_on_session_closed(bgp_context* ctx)
{
if (ctx == 0) return;
transition(ctx,FSM_IDLE);
}
void bgp_start(bgp_context* ctx, bgp_config conf)
{
if (ctx == 0) return;
ctx->config = conf;
if (transition(ctx,FSM_CONNECT) != 0) return;
}
int bgp_on_accept_success(bgp_context* ctx, int s, char* ip)
{
if (ctx == 0) return;
if (transition(ctx,FSM_OPENSENT) != 0) return;
int strmatch = strcmp(ip,ctx->config.neighbour);
if (strmatch != 0)
{
DEBUG("[%s] != [%s]\r\n",ip,ctx->config.neighbour);
transition(ctx,FSM_ACTIVE);
return -1;
}
ctx->session_fd = s;
send_open_message(ctx);
ctx->config.manage_function(ctx);
return 0;
}
void bgp_on_accept_failed(bgp_context* ctx, u8 fatal)
{
if (ctx == 0) return;
if (fatal)
{
if (transition(ctx,FSM_IDLE) != 0);
return;
}
if (transition(ctx,FSM_ACTIVE) != 0) return;
}
void bgp_on_connect_success(bgp_context* ctx, int s)
{
if (ctx == 0) return;
ctx->session_fd = s;
if (transition(ctx,FSM_OPENSENT) != 0) return;
}
void bgp_on_connect_failed(bgp_context* ctx, u8 fatal)
{
if (ctx == 0) return;
if (fatal)
{
if (transition(ctx,FSM_IDLE) != 0);
return;
}
if (transition(ctx,FSM_CONNECT) != 0) return;
}
bgp_attribute* get_attribute(bgp_attribute p[], int count, u8 attr)
{
int i;
for (i=0;i<count;i++)
{
if (p[i].type == attr) return &p[i];
}
return 0;
}
int parse_attributes(char* data, u16 size, bgp_attribute p[], int max)
{
int i = 0;
int n = 0;
int e = 0;
while (i < size)
{
u8 flags = data[i];
u8 type = data[i+1];
u16 asize = 0;
if (flags & 0b1000) // extended length
{
asize = __builtin_bswap16(*((u16*)&data[i+2]));
i+=4;
}
else
{
asize = (u16)data[i+2];
i+=3;
}
if (asize > sizeof(p[e].data)) continue;
p[e].type = type;
p[e].size = asize;
for (n=0;n<asize;n++) p[e].data[n] = data[i+n];
i+=asize;
e++;
if (e>=max) break;
}
return e;
}
int parse_prefixes(char* data, u16 size, bgp_prefix p[], int max)
{
u16 i = 0;
u16 n = 0;
u8 e = 0;
while (i < size)
{
char dsize = data[i];
p[e].ip[0]=0; p[e].ip[1]=0; p[e].ip[2]=0; p[e].ip[3]=0;
for (n=0;n<(dsize>>3);n++) p[e].ip[n] = (u8)data[i+n+1];
p[e].size = dsize;
i+= (1+(dsize>>3));
e++;
if (e>=max) break;
}
return e;
}
void process_update(bgp_context* ctx, char* data, u16 size)
{
bgp_prefix p[32];
bgp_attribute attr[32];
u16 i;
u16 n;
u16 withdrawSize = __builtin_bswap16(((u16*)data)[0]);
char* withdraw = (char*)&data[2];
data += withdrawSize+2;
u16 tpaSize = __builtin_bswap16(((u16*)data)[0]);
char* tpa = (char*)&data[2];
data += tpaSize+2;
u16 nlriSize = size-(tpaSize+withdrawSize+2+2);
char* nlri = data;
DEBUG("UPDATE Message:\r\n");
// Parse Withdraws
int num = parse_prefixes(withdraw, withdrawSize, (bgp_prefix*)&p, 32);
for (i=0;i<num;i++)
{
ctx->config.on_route_removed_function(p[i].ip,p[i].size);
}
int attrCount = parse_attributes(tpa, tpaSize, (bgp_attribute*)&attr, 32);
u8 nexthop[4] = {0,0,0,0};
bgp_attribute* attribute = get_attribute((bgp_attribute*)&attr,attrCount,ATTR_NEXT_HOP);
if (attribute)
{
for (i=0;i<4;i++) nexthop[i]=attribute->data[i];
}
// parse NLRI
num = parse_prefixes(nlri, nlriSize, (bgp_prefix*)&p, 32);
for (i=0;i<num;i++)
{
ctx->config.on_route_added_function(p[i].ip,p[i].size,nexthop);
}
}
void bgp_on_message(bgp_context* ctx, bgp_header* h, u16 size)
{
DEBUG("received message %s\r\n", messages[h->type-1]);
if (h->type == 1)
{
bgp_message_open* msg = (bgp_message_open*)h;
if (transition(ctx,FSM_OPENCONFIRM) != 0) return;
}
else if (h->type == 2)
{
if (transition(ctx,FSM_ESTABLISHED) != 0) return;
char* data = (char*)h;
process_update(ctx,(char*)&data[sizeof(bgp_header)],__builtin_bswap16(h->length)-sizeof(bgp_header));
}
else if (h->type == 3)
{
bgp_message_notification* msg = (bgp_message_notification*)h;
DEBUG("Notification. Err=%i, SubErr=%i, data=%i\r\n",msg->err,msg->suberr,msg->data);
transition(ctx,FSM_IDLE);
}
else if (h->type == 4)
{
if (transition(ctx,FSM_ESTABLISHED) != 0) return;
}
}
Raw
bgp.h
typedef unsigned char u8;
typedef unsigned short u16;
typedef unsigned int u32;
struct _bgp_context;
typedef int (*accept_session)(struct _bgp_context *ctx);
typedef int (*connect_session)(struct _bgp_context *ctx);
typedef int (*manage_session)(struct _bgp_context *ctx);
typedef int (*send_message)(struct _bgp_context *ctx, char* data, int size);
typedef int (*on_route_added)(u8 net[4], u8 size, u8 hop[4]);
typedef int (*on_route_removed)(u8 net[4], u8 size);
typedef int (*on_reset)(struct _bgp_context *ctx);
typedef struct
{
char* neighbour;
u16 as;
accept_session accept_function;
connect_session connect_function;
manage_session manage_function;
send_message send_function;
on_route_added on_route_added_function;
on_route_removed on_route_removed_function;
on_reset on_reset_function;
} bgp_config;
struct _bgp_context
{
u8 state;
int session_fd;
bgp_config config;
};
typedef struct _bgp_context bgp_context;
typedef struct __attribute__((__packed__))
{
u8 marker[16];
u16 length;
u8 type;
} bgp_header;
typedef struct __attribute__((__packed__))
{
bgp_header header;
u8 version;
u16 as;
u16 hold_time;
u32 id;
u8 optional_length;
} bgp_message_open;
typedef struct __attribute__((__packed__))
{
bgp_header header;
u8 err;
u8 suberr;
u16 data;
} bgp_message_notification;
#ifdef __cplusplus
extern "C"{
#endif
bgp_context* bgp_create_context();
void bgp_release_context(bgp_context* ctx);
void bgp_start(bgp_context* ctx, bgp_config conf);
int bgp_on_accept_success(bgp_context* ctx, int s, char* ip);
void bgp_on_accept_failed(bgp_context* ctx, u8 fatal);
void bgp_on_connect_success(bgp_context* ctx, int s);
void bgp_on_connect_failed(bgp_context* ctx, u8 fatal);
void bgp_on_message(bgp_context* ctx, bgp_header* data, u16 size);
void bgp_on_session_closed(bgp_context* ctx);
#ifdef __cplusplus
}
#endif
Raw
main.cpp
#include <stdlib.h>
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include "bgp.h"
#include <fcntl.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <netdb.h>
#define ACCEPT_WAIT 10
#define NEIGHBOUR "192.168.1.111"
#define AS 65243
#define ACTION_CONNECT 1
#define ACTION_ACCEPT 2
#define ACTION_MANAGE 3
int action;
void debug_data(char* data, int size)
{
int i;
for (i = 0; i<size; i++) printf("%02X ",(u8)data[i]);
printf("\r\n");
}
int route_added(u8 net[4], u8 size, u8 hop[4])
{
printf("\tRoute: %i.%i.%i.%i/%i -> ",net[0],net[1],net[2],net[3],size);
printf("%i.%i.%i.%i\r\n",hop[0],hop[1],hop[2],hop[3]);
}
int route_removed(u8 net[4], u8 size)
{
printf("\tWithdraw: %i.%i.%i.%i/%i\r\n",net[0],net[1],net[2],net[3],size);
}
int manage_tcp_session(bgp_context* ctx)
{
printf("manage\r\n");
action = ACTION_MANAGE;
}
int reset_tcp_session(bgp_context* ctx)
{
if (ctx->session_fd != 0) close(ctx->session_fd);
}
int connect_tcp_session(bgp_context* ctx)
{
printf("connect\r\n");
action = ACTION_CONNECT;
}
int accept_tcp_session(bgp_context* ctx)
{
printf("accept\r\n");
action = ACTION_ACCEPT;
}
int send_tcp_message(bgp_context* ctx, char* data, int size)
{
send(ctx->session_fd,data,size,0);
}
int acceptsession()
{
struct timeval timeout;
timeout.tv_sec = ACCEPT_WAIT;
timeout.tv_usec = 0;
int s = socket(AF_INET,SOCK_STREAM,0);
int flags = fcntl(s,F_GETFL,0);
sockaddr_in sockadd;
sockadd.sin_family=AF_INET;
sockadd.sin_addr.s_addr=inet_addr("0.0.0.0");
sockadd.sin_port=htons(179);
char r = 1;
setsockopt(s,SOL_SOCKET,SO_REUSEADDR,&r,sizeof(r));
setsockopt (s, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout));
setsockopt (s, SOL_SOCKET, SO_SNDTIMEO, (char *)&timeout, sizeof(timeout));
if (bind(s,(struct sockaddr *)&sockadd,sizeof(sockadd))<0) return 0;
listen(s,10);
int client = accept(s,0,0);
printf("Connection, client = %i\r\n",client);
close(s);
timeout.tv_sec = 0;
timeout.tv_usec = 0;
setsockopt(client, SOL_SOCKET, SO_RCVTIMEO, (char *)&timeout, sizeof(timeout));
setsockopt(client, SOL_SOCKET, SO_SNDTIMEO, (char *)&timeout, sizeof(timeout));
return client;
}
int connectsession(const char* ip)
{
sockaddr_in sockadd;
int sock=socket(AF_INET,SOCK_STREAM,0);
memset(&sockadd,0,sizeof(sockadd));
struct hostent *he;
he = gethostbyname(ip);
sockadd.sin_family = AF_INET;
sockadd.sin_port = htons(179);
memcpy(&sockadd.sin_addr, he->h_addr_list[0], he->h_length);
if (connect(sock,(struct sockaddr *)&sockadd, sizeof(sockadd)) >= 0)
{
return sock;
}
return 0;
}
void get_message(bgp_context* ctx, char* data, int& size)
{
int r = 0;
int l;
while (r < sizeof(bgp_header))
{
l = recv(ctx->session_fd,(char*)&data[r],sizeof(bgp_header)-r,0);
if (l == 0)
{
printf("Connection closed\r\n");
size = 0;
return;
}
else if (l <0)
{
size = 0;
return;
}
r += l;
}
bgp_header* b = (bgp_header*)data;
size = __builtin_bswap16(b->length);
while (r < size)
{
l = recv(ctx->session_fd,(char*)&data[r],size-r,0);
if (l == 0)
{
printf("Connection closed\r\n");
size = 0;
return;
}
else if (l <0)
{
size = 0;
return;
}
r += l;
}
//printf("recv: %i\r\n",r);
//debug_data(data,r);
}
int main(int argc, char** argv)
{
char* data = (char*)malloc(2000);
bgp_config conf;
conf.neighbour = NEIGHBOUR;
conf.as = AS;
conf.accept_function = &accept_tcp_session;
conf.connect_function = &connect_tcp_session;
conf.manage_function = &manage_tcp_session;
conf.send_function = &send_tcp_message;
conf.on_route_added_function = &route_added;
conf.on_route_removed_function = &route_removed;
conf.on_reset_function = &reset_tcp_session;
bgp_context* ctx = bgp_create_context();
bgp_start(ctx, conf);
while (1)
{
int a = action;
action = 0;
if (a == ACTION_CONNECT)
{
int r = connectsession(ctx->config.neighbour);
if (r == 0)
{
bgp_on_connect_failed(ctx,0);
} else {
bgp_on_connect_success(ctx,r);
}
}
if (a == ACTION_ACCEPT)
{
int s = acceptsession();
if (s < 0)
{
printf("Error A: %i\r\n",errno);
bgp_on_accept_failed(ctx,0);
}
else if (s == 0)
{
printf("Error B: %i\r\n",errno);
bgp_on_accept_failed(ctx,0);
}
else
{
socklen_t len;
struct sockaddr_storage addr;
char ipstr[16];
for (int i = 0; i<16; i++) ipstr[i]=0;
len = sizeof addr;
getpeername(s, (struct sockaddr*)&addr, &len);
struct sockaddr_in *si = (struct sockaddr_in *)&addr;
inet_ntop(AF_INET, &si->sin_addr, ipstr, sizeof(ipstr));
if (bgp_on_accept_success(ctx,s,(char*)&ipstr) < 0)
{
close(s);
}
}
}
if (a == ACTION_MANAGE)
{
int r;
get_message(ctx, data, r);
if (r > 0)
{
bgp_on_message(ctx,(bgp_header*)data,r);
action = ACTION_MANAGE;
}
else
{
printf("Error C: %i\r\n",errno);
bgp_on_session_closed(ctx);
}
}
}
bgp_release_context(ctx);
free(data);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment