-
Notifications
You must be signed in to change notification settings - Fork 3
/
helpers.c
158 lines (134 loc) · 4.4 KB
/
helpers.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
/* NVIDIA CORPORATION gave permission to FLIR Systems, Inc to modify this code
* and distribute it as part of the ADAS GMSL Kit.
* http://www.flir.com/
* October-2019
*/
#include <stdio.h>
#include <string.h>
#include "log_utils.h"
#include "misc_utils.h"
#include "helpers.h"
NvMediaStatus
CreateImageQueue(NvMediaDevice *device,
NvQueue **queue,
uint32_t queueSize,
uint32_t width,
uint32_t height,
NvMediaSurfaceType surfType,
NvMediaSurfAllocAttr *surfAllocAttrs,
uint32_t numSurfAllocAttrs)
{
uint32_t j = 0;
NvMediaImage *image = NULL;
NvMediaStatus status = NVMEDIA_STATUS_OK;
if (NvQueueCreate(queue,
queueSize,
sizeof(NvMediaImage *)) != NVMEDIA_STATUS_OK) {
LOG_ERR("%s: Failed to create image Queue \n", __func__);
return NVMEDIA_STATUS_OK;
}
for (j = 0; j < queueSize; j++) {
LOG_DBG("%s: NvMediaImageCreateNew\n", __func__);
image = NvMediaImageCreateNew(device, // device
surfType, // NvMediaSurfaceType type
surfAllocAttrs, // surf allocation attrs
numSurfAllocAttrs, // num attrs
0); // flags
if (!image) {
LOG_ERR("%s: NvMediaImageCreate failed for image %d",
__func__, j);
return NVMEDIA_STATUS_ERROR;
}
image->tag = *queue;
if (IsFailed(NvQueuePut(*queue,
(void *)&image,
NV_TIMEOUT_INFINITE))) {
LOG_ERR("%s: Pushing image to image queue failed\n", __func__);
return NVMEDIA_STATUS_ERROR;
}
}
return NVMEDIA_STATUS_OK;
}
NvMediaStatus
ImageToBytes(NvMediaImage *imgSrc,
uint8_t *dstBuffer,
uint8_t *telemetry,
uint32_t rawBytesPerPixel,
uint8_t multiplex)
{
uint8_t *pSrcBuff = NULL;
NvMediaImageSurfaceMap surfaceMap;
NvMediaStatus status;
uint32_t srcWidth, srcHeight, srcPitch;
if (NvMediaImageLock(imgSrc, NVMEDIA_IMAGE_ACCESS_WRITE, &surfaceMap) !=
NVMEDIA_STATUS_OK) {
LOG_ERR("%s: NvMediaImageLock failed\n", __func__);
return NVMEDIA_STATUS_ERROR;
}
srcWidth = surfaceMap.width;
srcHeight = surfaceMap.height;
srcPitch = srcWidth * rawBytesPerPixel;
if (!(pSrcBuff = malloc(srcPitch * srcHeight * sizeof(uint8_t)))) {
LOG_ERR("%s: Out of memory\n", __func__);
return NVMEDIA_STATUS_OUT_OF_MEMORY;
}
status = NvMediaImageGetBits(imgSrc, NULL, (void **)&pSrcBuff, &srcPitch);
NvMediaImageUnlock(imgSrc);
if(multiplex) {
// get telemetry
for (size_t i = 0; i < srcPitch; i+=2) {
telemetry[i/2] = pSrcBuff[i];
}
// get image (skip telemetry line)
for (size_t i = srcPitch; i < srcPitch * srcHeight; i+=2) {
dstBuffer[(i - srcPitch)/2] = pSrcBuff[i];
}
} else {
// get telemetry data
memcpy(telemetry, pSrcBuff, srcPitch * sizeof(uint8_t));
// skip the first row (telemetry line)
memcpy(dstBuffer, &pSrcBuff[srcPitch],
srcPitch * (srcHeight - 1) * sizeof(uint8_t));
}
// FILE *fp = fopen("img.out", "w");
// fwrite(dstBuffer, srcPitch * (srcHeight - 1), sizeof(uint8_t), fp);
// fclose(fp);
if(pSrcBuff)
free(pSrcBuff);
return NVMEDIA_STATUS_OK;
}
void
MsbToLsb32(uint32_t *dest, uint8_t *src) {
*dest = 0;
for (size_t i = 0; i < 4; i++) {
*dest += src[i] << (24 - (8 * i));
}
}
void
LsbToMsbArr(uint8_t *dest, uint32_t src) {
for (size_t i = 0; i < 4; i++) {
dest[i] = (src >> (24 - (8 * i))) & 0xFF;
}
}
void
SplitString(char **dest, char *src, char *delim) {
char *pch;
uint32_t i = 0;
pch = strtok(src, delim);
while(pch != NULL) {
dest[i] = pch;
pch = strtok(NULL, delim);
i++;
}
}
int
EndsWith(const char *str, const char *suffix)
{
if (!str || !suffix)
return 0;
size_t lenstr = strlen(str);
size_t lensuffix = strlen(suffix);
if (lensuffix > lenstr)
return 0;
return strncmp(str + lenstr - lensuffix, suffix, lensuffix) == 0;
}