static u8 * pcapng_add_comments(struct wlantest *wt, u8 *pos)
{
size_t i;
u16 *len;
if (!wt->num_notes)
return pos;
*((u16 *) pos) = 1 /* opt_comment */;
pos += 2;
len = (u16 *) pos /* length to be filled in */;
pos += 2;
for (i = 0; i < wt->num_notes; i++) {
size_t nlen = os_strlen(wt->notes[i]);
if (i > 0)
*pos++ = '\n';
os_memcpy(pos, wt->notes[i], nlen);
pos += nlen;
}
*len = pos - (u8 *) len - 2;
pos += PAD32(*len);
*((u16 *) pos) = 0 /* opt_endofopt */;
pos += 2;
*((u16 *) pos) = 0;
pos += 2;
return pos;
}
void write_pcapng_write_read(struct wlantest *wt, int dlt,
struct pcap_pkthdr *hdr, const u8 *data)
{
struct pcapng_enhanced_packet *pkt;
u8 *pos;
u32 *block_len;
u64 timestamp;
size_t len, datalen = hdr->caplen;
u8 rtap[] = {
0x00 /* rev */,
0x00 /* pad */,
0x0a, 0x00, /* header len */
0x02, 0x00, 0x00, 0x00, /* present flags */
0x00, /* flags */
0x00 /* pad */
};
if (wt->assume_fcs)
rtap[8] |= 0x10;
if (!wt->pcapng)
return;
len = sizeof(*pkt) + hdr->len + 100 + notes_len(wt, 32) + sizeof(rtap);
pkt = os_zalloc(len);
if (pkt == NULL)
return;
pkt->block_type = PCAPNG_BLOCK_ENHANCED_PACKET;
pkt->interface_id = 0;
timestamp = 1000000 * hdr->ts.tv_sec + hdr->ts.tv_usec;
pkt->timestamp_high = timestamp >> 32;
pkt->timestamp_low = timestamp & 0xffffffff;
wt->write_pcapng_time_high = pkt->timestamp_high;
wt->write_pcapng_time_low = pkt->timestamp_low;
pkt->captured_len = hdr->caplen;
pkt->packet_len = hdr->len;
pos = (u8 *) (pkt + 1);
switch (dlt) {
case DLT_IEEE802_11_RADIO:
break;
case DLT_PRISM_HEADER:
/* remove prism header (could be kept ... lazy) */
pkt->captured_len -= WPA_GET_LE32(data + 4);
pkt->packet_len -= WPA_GET_LE32(data + 4);
datalen -= WPA_GET_LE32(data + 4);
data += WPA_GET_LE32(data + 4);
/* fall through */
case DLT_IEEE802_11:
pkt->captured_len += sizeof(rtap);
pkt->packet_len += sizeof(rtap);
os_memcpy(pos, &rtap, sizeof(rtap));
pos += sizeof(rtap);
break;
default:
return;
}
os_memcpy(pos, data, datalen);
pos += datalen + PAD32(pkt->captured_len);
pos = pcapng_add_comments(wt, pos);
block_len = (u32 *) pos;
pos += 4;
*block_len = pkt->block_total_len = pos - (u8 *) pkt;
fwrite(pkt, pos - (u8 *) pkt, 1, wt->pcapng);
os_free(pkt);
write_pcapng_decrypted(wt);
}
static int WriteBMP8(const char * pszFile, geBitmap *pBitmap)
{
geBitmap * pLock = NULL;
gePixelFormat Format;
geBitmap_Info BitmapInfo;
int nErrorCode = TPACKERROR_UNKNOWN; // Return code
BITMAPFILEHEADER BmpHeader; // bitmap file-header
MY_BITMAPINFO BmInfo;
uint32 nBytesPerPixel;
void * pPixelData;
uint8 * pOut = NULL;
int nNewStride = 0;
int nOldStride = 0;
int i;
HANDLE hFile = NULL;
DWORD nBytesWritten;
// uint8 PaletteData[768]; // palette data (see note below)
// Create the .BMP file.
hFile = CreateFile(pszFile,
GENERIC_READ | GENERIC_WRITE,
(DWORD) 0,
NULL,
CREATE_ALWAYS,
FILE_ATTRIBUTE_NORMAL,
(HANDLE) NULL);
if (hFile == INVALID_HANDLE_VALUE)
return TPACKERROR_CREATEFILE;
// get 8-bit palettized bitmap
Format = GE_PIXELFORMAT_8BIT;
if ( geBitmap_GetBits(pBitmap))
{
pLock = pBitmap;
}
else
{
if (! geBitmap_LockForRead(pBitmap, &pLock, 0, 0, Format, GE_FALSE,0) )
{
return FALSE;
}
}
geBitmap_GetInfo(pLock, &BitmapInfo, NULL);
if ( BitmapInfo.Format != Format )
{
nErrorCode = TPACKERROR_UNKNOWN;
goto ExitWriteBitmap;
}
// if (!geBitmap_Palette_Lock(BitmapInfo.Palette, (void **)&PaletteData, NULL ,NULL))
// goto ExitWriteBitmap;
// NOTE: For some reason the code below for getting the palette data doesn't work.
// Incorrect palette data is returned.
// Because of that I'm using the much slower geBitmap_Palette_GetEntryColor()
// method (below). This seems to produce the correct results.
// // Get the palette array
// if (!geBitmap_Palette_GetData(BitmapInfo.Palette, (void *)PaletteData, GE_PIXELFORMAT_8BIT, 255))
// goto ExitWriteBitmap;
// // Save the palette
// for (i = 0; i < 256; i++)
// {
// BmInfo.bmiColors[i].rgbRed = PaletteData[i];
// BmInfo.bmiColors[i].rgbGreen = PaletteData[i+1];
// BmInfo.bmiColors[i].rgbBlue = PaletteData[i+2];
// BmInfo.bmiColors[i].rgbReserved = 0;
// }
for (i = 0; i < 256; i++)
{
int r, g, b, a;
geBitmap_Palette_GetEntryColor(BitmapInfo.Palette, i, &r, &g, &b, &a);
BmInfo.bmiColors[i].rgbRed = (uint8)r;
BmInfo.bmiColors[i].rgbGreen = (uint8)g;
BmInfo.bmiColors[i].rgbBlue = (uint8)b;
BmInfo.bmiColors[i].rgbReserved = (uint8)0;
}
// geBitmap_Palette_UnLock(BitmapInfo.Palette);
nBytesPerPixel = gePixelFormat_BytesPerPel(Format);
pPixelData = geBitmap_GetBits(pLock);
// Build bitmap info
BmInfo.bmiHeader.biSize = sizeof(BITMAPINFOHEADER);
BmInfo.bmiHeader.biWidth = BitmapInfo.Width;
BmInfo.bmiHeader.biHeight = BitmapInfo.Height; // Bitmap are bottom-up
BmInfo.bmiHeader.biPlanes = 1;
BmInfo.bmiHeader.biBitCount = (WORD)8;
BmInfo.bmiHeader.biCompression = BI_RGB;
BmInfo.bmiHeader.biSizeImage = 0;
BmInfo.bmiHeader.biXPelsPerMeter = BmInfo.bmiHeader.biYPelsPerMeter = 0; // 10000;
if (BmInfo.bmiHeader.biBitCount< 24)
BmInfo.bmiHeader.biClrUsed = (1 << BmInfo.bmiHeader.biBitCount);
else
BmInfo.bmiHeader.biClrUsed = 0;
BmInfo.bmiHeader.biClrImportant = 0;
nNewStride = PAD32(BitmapInfo.Width * BmInfo.bmiHeader.biBitCount);
nOldStride = BitmapInfo.Width * nBytesPerPixel;
BmInfo.bmiHeader.biSizeImage = nNewStride * BitmapInfo.Height;
// Bitmap scanlines are padded to the nearest dword. If the pixel data from pBitmap
// is not a the correct width, we need to fix it.
// NOTE: The best solution is to write each scanline, That way we don't
// have to allocate a new pixel buffer.
if (nNewStride == nOldStride)
{
pOut = (uint8 *)pPixelData;
}
// Allocate new pixel buffer.
else
{
uint8 *pNew;
uint8 *pOld;
int y;
pOut = (uint8 *)geRam_Allocate(nNewStride * BitmapInfo.Height);
if (pOut == (uint8 *)0)
{
// Memory allocation error
nErrorCode = TPACKERROR_MEMORYALLOCATION;
goto ExitWriteBitmap;
}
pNew = (uint8 *)pOut;
pOld = (uint8 *)pPixelData;
// Copy old to new
for (y = 0; y < BitmapInfo.Height; y++)
{
memcpy(pNew, pOld, nOldStride);
// Next row
pOld += nOldStride;
pNew += nNewStride;
}
}
// Build the file header
BmpHeader.bfType = 0x4d42; // 0x42 = "B" 0x4d = "M"
// Compute the size of the entire file.
BmpHeader.bfSize = (DWORD)(sizeof(BITMAPFILEHEADER) +
BmInfo.bmiHeader.biSize +
(BmInfo.bmiHeader.biClrUsed * sizeof(RGBQUAD)) +
(nNewStride * BitmapInfo.Height));
BmpHeader.bfReserved1 = 0;
BmpHeader.bfReserved2 = 0;
// Compute the offset to the array of color indices.
BmpHeader.bfOffBits = (DWORD)sizeof(BITMAPFILEHEADER) +
BmInfo.bmiHeader.biSize +
(BmInfo.bmiHeader.biClrUsed * sizeof(RGBQUAD));
// Write the BMP file header
if (!WriteFile(hFile, (LPVOID)&BmpHeader, sizeof(BITMAPFILEHEADER), (LPDWORD)&nBytesWritten, (NULL)))
{
nErrorCode = TPACKERROR_WRITE;
goto ExitWriteBitmap;
}
// Write the Bitmap infor header and palette
if (!WriteFile(hFile, (LPVOID)&BmInfo, sizeof(MY_BITMAPINFO), (LPDWORD)&nBytesWritten, (NULL)))
{
nErrorCode = TPACKERROR_WRITE;
goto ExitWriteBitmap;
}
// Write the pixel data
if (!WriteFile(hFile, (LPVOID)pOut, nNewStride * BitmapInfo.Height, (LPDWORD)&nBytesWritten, (NULL)))
{
nErrorCode = TPACKERROR_WRITE;
goto ExitWriteBitmap;
}
CloseHandle(hFile);
hFile = NULL;
// Success!
nErrorCode = TPACKERROR_OK;
ExitWriteBitmap:
// Clean-up
//------------------------------------
// Make sure the file gets closed
if (hFile)
CloseHandle(hFile);
// If the temp pixel buffer was allocated, then free it
if (pOut && nNewStride != nOldStride)
geRam_Free(pOut);
// Unlock the geBitmap
if ( pLock != pBitmap )
{
geBitmap_UnLock (pLock);
}
return nErrorCode;
}