void Packet::WriteS(WChar *pData)
{
int i = 0;
while (iPacketSize < sizeof(pPacket) - 2)
{
WriteH((Int16)pData[i]);
if (pData[i] == 0) break;
i++;
}
}
int SendToDestination(int netfd, int tapfd, unsigned char *buffer, int len, ClientConf *Client)
{
int cr_size = 0, dcr_size = 0;
unsigned char dcr_buffer[BUFSIZ], cr_buffer[BUFSIZ];
register int i = 0;
struct iphdr *header;
struct in_addr dst;
/* If Client == NULL, then the packet is a raw packet from the tun/tap device, so it must not be decrypted */
if (Client != NULL) {
if (len == 0)
return 0;
/* Decrypt the message to get the destination */
if (do_decrypt(&(Client->ctx), Client->PrivKey, Client->IV, buffer, len, dcr_buffer, &dcr_size) < 0) {
fprintf(stderr, "SendToDestination : do_decrypt\n");
return -1;
}
/* Empty packet. Drop it */
if (!*dcr_buffer)
return 0;
}
/* Get the header and the destination address */
header = (Client == NULL ? (struct iphdr *)buffer : (struct iphdr *)dcr_buffer);
dst.s_addr = header->daddr;
/* Get the destination client's information if we have it */
for (i = 0; i < CurrentClients; i++)
if (dst.s_addr == Clients[i].ns.data.sin_addr.s_addr)
Client = Clients + i;
if (Client != NULL) {
printf("SENT!\n");
/* Encrypt the message with the destination key */
if (do_encrypt(&(Client->ctx), Client->PrivKey, Client->IV, dcr_buffer, dcr_size, cr_buffer, &cr_size) < 0) {
fprintf(stderr, "SendToDestination : do_encrypt\n");
return -1;
}
}
else
return 0;
if (WriteH(netfd, (struct sockaddr *)&Client->ns.data, (Client != NULL) ? cr_buffer : dcr_buffer, (Client != NULL) ? cr_size : dcr_size) < 0) {
fprintf(stderr, "SendToDestination : WriteH\n");
return -1;
}
return 0;
}
/*!***********************************************************************
@Function SaveH
@Input pszFilename Filename to save to
@Description Save a header file (.H).
*************************************************************************/
EPVRTError CPVRTModelPOD::SaveH(const char * const pszFilename, const char * const pszExpOpt, const char * const pszHistory)
{
FILE *pFile;
bool bRet;
pFile = fopen(pszFilename, "wt+");
if(!pFile)
return PVR_FAIL;
bRet = WriteH(pFile, pszFilename, pszExpOpt, pszHistory, *this);
// Done
fclose(pFile);
return bRet ? PVR_SUCCESS : PVR_FAIL;
}
ClientConf *DoAuthenticateServer(int net_fd, struct sockaddr_in *from)
{
ClientConf *Client = NULL;
int dcr_size = 0, cr_size = 0, i = 0;
char ConnectionID[MED_BUF];
unsigned char cr_buffer[BUFSIZ], dcr_buffer[BUFSIZ], orig_buffer[BUFSIZ], signature[MED_BUF];
unsigned int siglen = 0;
memset(dcr_buffer, 0, sizeof(dcr_buffer));
memset(orig_buffer, 0, sizeof(orig_buffer));
/* Get the connection ID */
if ((i = ReadN(net_fd, (struct sockaddr *)from, (unsigned char *)ConnectionID, MED_BUF)) <= 0) {
fprintf(stderr, "DoAuthenticateServer : ReadN\n");
return NULL;
}
ConnectionID[i] = '\0';
/* Load the client setup */
if ((Client = ReadClientConf(SrvSetup.cfgfile, ConnectionID)) == NULL) {
fprintf(stderr, "DoAuthenticateServer : ReadClientConf\n");
return NULL;
}
if (LoadPublicKeyFromFile(Client->fpubkey, &(Client->pub), CLIENT_LOAD_SERVER_PUBLIC_KEYFILE_ERR)) {
fprintf(stderr, "DoAuthenticateServer : LoadPublicKeyFromFile\n");
return NULL;
}
/* Create a random message, sign it, encrypt it and send it with its signature */
if (CreateRandomMessage(orig_buffer, MIN_BUF) < 0) {
fprintf(stderr, "DoAuthenticateServer : CreateRandomMessage\n");
return NULL;
}
if (SignMessage(SrvSetup.priv, orig_buffer, MIN_BUF, signature, &siglen) < 0) {
fprintf(stderr, "DoAuthenticateServer : SignMesage\n");
return NULL;
}
if (EncryptMessageWithPublicKey(Client->pub, orig_buffer, MIN_BUF, cr_buffer, &cr_size) < 0) {
fprintf(stderr, "DoAuthenticateServer : EncryptMessageWithPublicKey\n");
return NULL;
}
if (WriteH(net_fd, (struct sockaddr *)from, cr_buffer, cr_size) < 0) {
fprintf(stderr, "DoAuthenticateServer : WriteH\n");
return NULL;
}
if (WriteH(net_fd, (struct sockaddr *)from, signature, siglen) <= 0) {
fprintf(stderr, "DoAuthenticateServer : WriteH\n");
return NULL;
}
/* Read the answer and decrypt it */
if ((cr_size = ReadN(net_fd, (struct sockaddr *)from, cr_buffer, BUFSIZ)) < 0) {
fprintf(stderr, "DoAuthenticateServer : ReadN\n");
return NULL;
}
else if (cr_size == 0) {
fprintf(stderr, "Authentication failed. Access denied!\n");
fprintf(stderr, "Disconnecting client\n");
return NULL;
}
if (DecryptMessageWithPrivateKey(SrvSetup.priv, cr_buffer, cr_size, dcr_buffer, &dcr_size)) {
fprintf(stderr, "DoAuthenticateServer : DecryptMessageWithPrivateKey\n");
return NULL;
}
/* Check if the message is ok */
if (memcmp(orig_buffer, dcr_buffer, MIN_BUF)) {
fprintf(stderr, "Access denied!\n");
return NULL;
}
/* Save the origin information in the client's structure */
memcpy(&(Client->ns.data), from, sizeof(struct sockaddr_in));
inet_aton(Client->ns.ip, &Client->ns.data.sin_addr);
/* Create a key, arrange a network setup for the client and send it over */
if (CreateRandomKey(Client->PrivKey, KEYSIZE)) {
fprintf(stderr, "DoAuthenticateServer : CreateRandomKey\n");
return NULL;
}
/*sprintf((char *)dcr_buffer, "key=%s,ip=%s,netmask=%s,broadcast=%s,mtu=%s", Client->PrivKey, Client->ns.ip, Client->ns.msk, Client->ns.brd, Client->ns.mtu);*/
memset(dcr_buffer, '\0', sizeof(dcr_buffer));
memcpy(dcr_buffer, "key=", 4);
memcpy(dcr_buffer + 4, Client->PrivKey, KEYSIZE);
sprintf((char *)dcr_buffer + 4 + KEYSIZE, ",ip=%s,netmask=%s,broadcast=%s", Client->ns.ip, Client->ns.msk, Client->ns.brd);
dcr_size = 4 + KEYSIZE + strlen(((char *)dcr_buffer) + 4 + KEYSIZE);
if (EncryptMessageWithPublicKey(Client->pub, dcr_buffer, dcr_size, cr_buffer, &cr_size) < 0) {
fprintf(stderr, "DoAuthenticateServer : EncryptMessageWithPublicKey\n");
return NULL;
}
if (WriteH(net_fd, (struct sockaddr *)from, cr_buffer, cr_size) < 0) {
fprintf(stderr, "DoAuthenticateServer : WriteH\n");
return NULL;
}
CryptoInit(&(Client->ctx), Client->PrivKey, &(Client->IV));
return Client;
}
