aboutsummaryrefslogtreecommitdiffstats
path: root/lib/lufa/Projects/Webserver/Lib/uIPManagement.c
blob: 12f6c8f9e7f27dff837fe2e397f0a080280b02ce (plain)
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
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
/*
             LUFA Library
     Copyright (C) Dean Camera, 2017.

  dean [at] fourwalledcubicle [dot] com
           www.lufa-lib.org
*/

/*
  Copyright 2017  Dean Camera (dean [at] fourwalledcubicle [dot] com)

  Permission to use, copy, modify, distribute, and sell this
  software and its documentation for any purpose is hereby granted
  without fee, provided that the above copyright notice appear in
  all copies and that both that the copyright notice and this
  permission notice and warranty disclaimer appear in supporting
  documentation, and that the name of the author not be used in
  advertising or publicity pertaining to distribution of the
  software without specific, written prior permission.

  The author disclaims all warranties with regard to this
  software, including all implied warranties of merchantability
  and fitness.  In no event shall the author be liable for any
  special, indirect or consequential damages or any damages
  whatsoever resulting from loss of use, data or profits, whether
  in an action of contract, negligence or other tortious action,
  arising out of or in connection with the use or performance of
  this software.
*/

/** \file
 *
 *  uIP Management functions. This file contains the functions and globals needed to maintain the uIP
 *  stack once an RNDIS device has been attached to the system.
 */

#define  INCLUDE_FROM_UIPMANAGEMENT_C
#include "uIPManagement.h"

/** Connection timer, to retain the time elapsed since the last time the uIP connections were managed. */
static struct timer ConnectionTimer;

/** ARP timer, to retain the time elapsed since the ARP cache was last updated. */
static struct timer ARPTimer;

/** MAC address of the RNDIS device, when enumerated. */
struct uip_eth_addr MACAddress;


/** Configures the uIP stack ready for network traffic processing. */
void uIPManagement_Init(void)
{
	/* uIP Timing Initialization */
	clock_init();
	timer_set(&ConnectionTimer, CLOCK_SECOND / 2);
	timer_set(&ARPTimer, CLOCK_SECOND * 10);

	/* uIP Stack Initialization */
	uip_init();
	uip_arp_init();

	/* DHCP/Server IP Settings Initialization */
	if (USB_CurrentMode == USB_MODE_Device)
	{
		MACAddress.addr[0] = SERVER_MAC_ADDRESS[0];
		MACAddress.addr[1] = SERVER_MAC_ADDRESS[1];
		MACAddress.addr[2] = SERVER_MAC_ADDRESS[2];
		MACAddress.addr[3] = SERVER_MAC_ADDRESS[3];
		MACAddress.addr[4] = SERVER_MAC_ADDRESS[4];
		MACAddress.addr[5] = SERVER_MAC_ADDRESS[5];

		#if defined(ENABLE_DHCP_SERVER)
		DHCPServerApp_Init();
		#endif

		uip_ipaddr_t IPAddress, Netmask, GatewayIPAddress;
		uip_ipaddr(&IPAddress,        DEVICE_IP_ADDRESS[0], DEVICE_IP_ADDRESS[1], DEVICE_IP_ADDRESS[2], DEVICE_IP_ADDRESS[3]);
		uip_ipaddr(&Netmask,          DEVICE_NETMASK[0],    DEVICE_NETMASK[1],    DEVICE_NETMASK[2],    DEVICE_NETMASK[3]);
		uip_ipaddr(&GatewayIPAddress, DEVICE_GATEWAY[0],    DEVICE_GATEWAY[1],    DEVICE_GATEWAY[2],    DEVICE_GATEWAY[3]);
		uip_sethostaddr(&IPAddress);
		uip_setnetmask(&Netmask);
		uip_setdraddr(&GatewayIPAddress);
	}
	else
	{
		#if defined(ENABLE_DHCP_CLIENT)
		DHCPClientApp_Init();
		#else
		uip_ipaddr_t IPAddress, Netmask, GatewayIPAddress;
		uip_ipaddr(&IPAddress,        DEVICE_IP_ADDRESS[0], DEVICE_IP_ADDRESS[1], DEVICE_IP_ADDRESS[2], DEVICE_IP_ADDRESS[3]);
		uip_ipaddr(&Netmask,          DEVICE_NETMASK[0],    DEVICE_NETMASK[1],    DEVICE_NETMASK[2],    DEVICE_NETMASK[3]);
		uip_ipaddr(&GatewayIPAddress, DEVICE_GATEWAY[0],    DEVICE_GATEWAY[1],    DEVICE_GATEWAY[2],    DEVICE_GATEWAY[3]);
		uip_sethostaddr(&IPAddress);
		uip_setnetmask(&Netmask);
		uip_setdraddr(&GatewayIPAddress);
		#endif
	}

	/* Virtual Webserver Ethernet Address Configuration */
	uip_setethaddr(MACAddress);

	/* HTTP Webserver Initialization */
	HTTPServerApp_Init();

	/* TELNET Server Initialization */
	#if defined(ENABLE_TELNET_SERVER)
	TELNETServerApp_Init();
	#endif
}

/** uIP Management function. This function manages the uIP stack when called while an RNDIS device has been
 *  attached to the system.
 */
void uIPManagement_ManageNetwork(void)
{
	if (((USB_CurrentMode == USB_MODE_Host)   && (USB_HostState   == HOST_STATE_Configured)) ||
	    ((USB_CurrentMode == USB_MODE_Device) && (USB_DeviceState == DEVICE_STATE_Configured)))
	{
		uIPManagement_ProcessIncomingPacket();
		uIPManagement_ManageConnections();
	}
}

/** uIP TCP/IP network stack callback function for the processing of a given TCP connection. This routine dispatches
 *  to the appropriate TCP protocol application based on the connection's listen port number.
 */
void uIPManagement_TCPCallback(void)
{
	/* Call the correct TCP application based on the port number the connection is listening on */
	switch (uip_conn->lport)
	{
		case HTONS(HTTP_SERVER_PORT):
			HTTPServerApp_Callback();
			break;
		#if defined(ENABLE_TELNET_SERVER)
		case HTONS(TELNET_SERVER_PORT):
			TELNETServerApp_Callback();
			break;
		#endif
	}
}

/** uIP TCP/IP network stack callback function for the processing of a given UDP connection. This routine dispatches
 *  to the appropriate UDP protocol application based on the connection's listen port number.
 */
void uIPManagement_UDPCallback(void)
{
	/* Call the correct UDP application based on the port number the connection is listening on */
	switch (uip_udp_conn->lport)
	{
		#if defined(ENABLE_DHCP_CLIENT)
		case HTONS(DHCP_CLIENT_PORT):
			DHCPClientApp_Callback();
			break;
		#endif
		#if defined(ENABLE_DHCP_SERVER)
		case HTONS(DHCP_SERVER_PORT):
			DHCPServerApp_Callback();
			break;
		#endif
	}
}

/** Processes Incoming packets to the server from the connected RNDIS device, creating responses as needed. */
static void uIPManagement_ProcessIncomingPacket(void)
{
	/* Determine which USB mode the system is currently initialized in */
	if (USB_CurrentMode == USB_MODE_Device)
	{
		/* If no packet received, exit processing routine */
		if (!(RNDIS_Device_IsPacketReceived(&Ethernet_RNDIS_Interface_Device)))
		  return;

		LEDs_SetAllLEDs(LEDMASK_USB_BUSY);

		/* Read the Incoming packet straight into the UIP packet buffer */
		RNDIS_Device_ReadPacket(&Ethernet_RNDIS_Interface_Device, uip_buf, &uip_len);
	}
	else
	{
		/* If no packet received, exit processing routine */
		if (!(RNDIS_Host_IsPacketReceived(&Ethernet_RNDIS_Interface_Host)))
		  return;

		LEDs_SetAllLEDs(LEDMASK_USB_BUSY);

		/* Read the Incoming packet straight into the UIP packet buffer */
		RNDIS_Host_ReadPacket(&Ethernet_RNDIS_Interface_Host, uip_buf, &uip_len);
	}

	/* If the packet contains an Ethernet frame, process it */
	if (uip_len > 0)
	{
		switch (((struct uip_eth_hdr*)uip_buf)->type)
		{
			case HTONS(UIP_ETHTYPE_IP):
				/* Filter packet by MAC destination */
				uip_arp_ipin();

				/* Process Incoming packet */
				uip_input();

				/* If a response was generated, send it */
				if (uip_len > 0)
				{
					/* Add destination MAC to outgoing packet */
					uip_arp_out();

					uip_split_output();
				}

				break;
			case HTONS(UIP_ETHTYPE_ARP):
				/* Process ARP packet */
				uip_arp_arpin();

				/* If a response was generated, send it */
				if (uip_len > 0)
				  uip_split_output();

				break;
		}
	}

	LEDs_SetAllLEDs(LEDMASK_USB_READY);
}

/** Manages the currently open network connections, including TCP and (if enabled) UDP. */
static void uIPManagement_ManageConnections(void)
{
	/* Poll TCP connections for more data to send back to the host */
	for (uint8_t i = 0; i < UIP_CONNS; i++)
	{
		uip_poll_conn(&uip_conns[i]);

		/* If a response was generated, send it */
		if (uip_len > 0)
		{
			/* Add destination MAC to outgoing packet */
			uip_arp_out();

			/* Split and send the outgoing packet */
			uip_split_output();
		}
	}

	/* Manage open connections for timeouts */
	if (timer_expired(&ConnectionTimer))
	{
		timer_reset(&ConnectionTimer);

		LEDs_SetAllLEDs(LEDMASK_USB_BUSY);

		for (uint8_t i = 0; i < UIP_CONNS; i++)
		{
			/* Run periodic connection management for each TCP connection */
			uip_periodic(i);

			/* If a response was generated, send it */
			if (uip_len > 0)
			{
				/* Add destination MAC to outgoing packet */
				uip_arp_out();

				/* Split and send the outgoing packet */
				uip_split_output();
			}
		}

		#if defined(ENABLE_DHCP_CLIENT)
		for (uint8_t i = 0; i < UIP_UDP_CONNS; i++)
		{
			/* Run periodic connection management for each UDP connection */
			uip_udp_periodic(i);

			/* If a response was generated, send it */
			if (uip_len > 0)
			{
				/* Add destination MAC to outgoing packet */
				uip_arp_out();

				/* Split and send the outgoing packet */
				uip_split_output();
			}
		}
		#endif

		LEDs_SetAllLEDs(LEDMASK_USB_READY);
	}

	/* Manage ARP cache refreshing */
	if (timer_expired(&ARPTimer))
	{
		timer_reset(&ARPTimer);
		uip_arp_timer();
	}
}
84,52 +362,6 @@ struct bcm2835_codec_q_data { bool eos_buffer_in_use; /* debug only */ }; -enum { - V4L2_M2M_SRC = 0, - V4L2_M2M_DST = 1, -}; - -static inline struct bcm2835_codec_fmt_list *get_format_list(bool decode, - bool capture) -{ - return decode ^ capture ? &formats[ENCODED_LIST] : &formats[RAW_LIST]; -} - -static struct bcm2835_codec_fmt *get_default_format(bool decode, bool capture) -{ - return &get_format_list(decode, capture)->list[0]; -} - -static struct bcm2835_codec_fmt *find_format(struct v4l2_format *f, bool decode, - bool capture) -{ - struct bcm2835_codec_fmt *fmt; - unsigned int k; - struct bcm2835_codec_fmt_list *fmts = get_format_list(decode, capture); - - for (k = 0; k < fmts->num_entries; k++) { - fmt = &fmts->list[k]; - if (fmt->fourcc == f->fmt.pix.pixelformat) - break; - } - - /* - * Some compressed formats are only supported for decoding, not - * encoding. - */ - if (!decode && fmts->list[k].decode_only) - return NULL; - - /* Some pixel formats are only supported for encoding, not decoding. */ - if (decode && fmts->list[k].encode_only) - return NULL; - - if (k == fmts->num_entries) - return NULL; - - return &fmts->list[k]; -} - struct bcm2835_codec_dev { struct platform_device *pdev; @@ -342,6 +374,9 @@ struct bcm2835_codec_dev { /* allocated mmal instance and components */ bool decode; /* Is this instance a decoder? */ + /* The list of formats supported on input and output queues. */ + struct bcm2835_codec_fmt_list supported_fmts[2]; + struct vchiq_mmal_instance *instance; struct v4l2_m2m_dev *m2m_dev; @@ -374,8 +409,59 @@ struct bcm2835_codec_ctx { struct bcm2835_codec_driver { struct bcm2835_codec_dev *encode; struct bcm2835_codec_dev *decode; + struct bcm2835_codec_dev *isp; +}; + +enum { + V4L2_M2M_SRC = 0, + V4L2_M2M_DST = 1, }; +static const struct bcm2835_codec_fmt *get_fmt(u32 mmal_fmt) +{ + unsigned int i; + + for (i = 0; i < ARRAY_SIZE(supported_formats); i++) { + if (supported_formats[i].mmal_fmt == mmal_fmt) + return &supported_formats[i]; + } + return NULL; +} + +static inline +struct bcm2835_codec_fmt_list *get_format_list(struct bcm2835_codec_dev *dev, + bool capture) +{ + return &dev->supported_fmts[capture ? 1 : 0]; +} + +static +struct bcm2835_codec_fmt *get_default_format(struct bcm2835_codec_dev *dev, + bool capture) +{ + return &dev->supported_fmts[capture ? 1 : 0].list[0]; +} + +static struct bcm2835_codec_fmt *find_format(struct v4l2_format *f, + struct bcm2835_codec_dev *dev, + bool capture) +{ + struct bcm2835_codec_fmt *fmt; + unsigned int k; + struct bcm2835_codec_fmt_list *fmts = + &dev->supported_fmts[capture ? 1 : 0]; + + for (k = 0; k < fmts->num_entries; k++) { + fmt = &fmts->list[k]; + if (fmt->fourcc == f->fmt.pix.pixelformat) + break; + } + if (k == fmts->num_entries) + return NULL; + + return &fmts->list[k]; +} + static inline struct bcm2835_codec_ctx *file2ctx(struct file *file) { return container_of(file->private_data, struct bcm2835_codec_ctx, fh); @@ -456,7 +542,6 @@ static inline unsigned int get_bytesperl } static void setup_mmal_port_format(struct bcm2835_codec_ctx *ctx, - bool decode, struct bcm2835_codec_q_data *q_data, struct vchiq_mmal_port *port) { @@ -473,7 +558,7 @@ static void setup_mmal_port_format(struc port->es.video.frame_rate.den = 1; } else { /* Compressed format - leave resolution as 0 for decode */ - if (decode) { + if (ctx->dev->decode) { port->es.video.width = 0; port->es.video.height = 0; port->es.video.crop.width = 0; @@ -802,22 +887,15 @@ static int vidioc_querycap(struct file * return 0; } -static int enum_fmt(struct v4l2_fmtdesc *f, bool decode, bool capture) +static int enum_fmt(struct v4l2_fmtdesc *f, struct bcm2835_codec_ctx *ctx, + bool capture) { struct bcm2835_codec_fmt *fmt; - struct bcm2835_codec_fmt_list *fmts = get_format_list(decode, capture); + struct bcm2835_codec_fmt_list *fmts = + get_format_list(ctx->dev, capture); if (f->index < fmts->num_entries) { /* Format found */ - /* Check format isn't a decode only format when encoding */ - if (!decode && - fmts->list[f->index].decode_only) - return -EINVAL; - /* Check format isn't a decode only format when encoding */ - if (decode && - fmts->list[f->index].encode_only) - return -EINVAL; - fmt = &fmts->list[f->index]; f->pixelformat = fmt->fourcc; f->flags = fmt->flags; @@ -833,7 +911,7 @@ static int vidioc_enum_fmt_vid_cap(struc { struct bcm2835_codec_ctx *ctx = file2ctx(file); - return enum_fmt(f, ctx->dev->decode, true); + return enum_fmt(f, ctx, true); } static int vidioc_enum_fmt_vid_out(struct file *file, void *priv, @@ -841,7 +919,7 @@ static int vidioc_enum_fmt_vid_out(struc { struct bcm2835_codec_ctx *ctx = file2ctx(file); - return enum_fmt(f, ctx->dev->decode, false); + return enum_fmt(f, ctx, false); } static int vidioc_g_fmt(struct bcm2835_codec_ctx *ctx, struct v4l2_format *f) @@ -933,11 +1011,11 @@ static int vidioc_try_fmt_vid_cap(struct struct bcm2835_codec_fmt *fmt; struct bcm2835_codec_ctx *ctx = file2ctx(file); - fmt = find_format(f, ctx->dev->decode, true); + fmt = find_format(f, ctx->dev, true); if (!fmt) { - f->fmt.pix.pixelformat = get_default_format(ctx->dev->decode, + f->fmt.pix.pixelformat = get_default_format(ctx->dev, true)->fourcc; - fmt = find_format(f, ctx->dev->decode, true); + fmt = find_format(f, ctx->dev, true); } return vidioc_try_fmt(f, fmt); @@ -949,11 +1027,11 @@ static int vidioc_try_fmt_vid_out(struct struct bcm2835_codec_fmt *fmt; struct bcm2835_codec_ctx *ctx = file2ctx(file); - fmt = find_format(f, ctx->dev->decode, false); + fmt = find_format(f, ctx->dev, false); if (!fmt) { - f->fmt.pix.pixelformat = get_default_format(ctx->dev->decode, + f->fmt.pix.pixelformat = get_default_format(ctx->dev, false)->fourcc; - fmt = find_format(f, ctx->dev->decode, false); + fmt = find_format(f, ctx->dev, false); } if (!f->fmt.pix.colorspace) @@ -988,7 +1066,7 @@ static int vidioc_s_fmt(struct bcm2835_c return -EBUSY; } - q_data->fmt = find_format(f, ctx->dev->decode, + q_data->fmt = find_format(f, ctx->dev, f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE); q_data->crop_width = f->fmt.pix.width; q_data->height = f->fmt.pix.height; @@ -1041,7 +1119,7 @@ static int vidioc_s_fmt(struct bcm2835_c if (!port) return 0; - setup_mmal_port_format(ctx, ctx->dev->decode, q_data, port); + setup_mmal_port_format(ctx, q_data, port); ret = vchiq_mmal_port_set_format(ctx->dev->instance, port); if (ret) { v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed vchiq_mmal_port_set_format on port, ret %d\n", @@ -1064,8 +1142,7 @@ static int vidioc_s_fmt(struct bcm2835_c struct bcm2835_codec_q_data *q_data_dst = &ctx->q_data[V4L2_M2M_DST]; - setup_mmal_port_format(ctx, ctx->dev->decode, q_data_dst, - port_dst); + setup_mmal_port_format(ctx, q_data_dst, port_dst); ret = vchiq_mmal_port_set_format(ctx->dev->instance, port_dst); if (ret) { v4l2_err(&ctx->dev->v4l2_dev, "%s: Failed vchiq_mmal_port_set_format on output port, ret %d\n", @@ -1636,10 +1713,10 @@ static int bcm2835_codec_create_componen MMAL_PARAMETER_ZERO_COPY, &enable, sizeof(enable)); - setup_mmal_port_format(ctx, dev->decode, &ctx->q_data[V4L2_M2M_SRC], + setup_mmal_port_format(ctx, &ctx->q_data[V4L2_M2M_SRC], &ctx->component->input[0]); - setup_mmal_port_format(ctx, dev->decode, &ctx->q_data[V4L2_M2M_DST], + setup_mmal_port_format(ctx, &ctx->q_data[V4L2_M2M_DST], &ctx->component->output[0]); ret = vchiq_mmal_port_set_format(dev->instance, @@ -2025,8 +2102,8 @@ static int bcm2835_codec_open(struct fil goto open_unlock; } - ctx->q_data[V4L2_M2M_SRC].fmt = get_default_format(dev->decode, false); - ctx->q_data[V4L2_M2M_DST].fmt = get_default_format(dev->decode, true); + ctx->q_data[V4L2_M2M_SRC].fmt = get_default_format(dev, false); + ctx->q_data[V4L2_M2M_DST].fmt = get_default_format(dev, true); if (dev->decode) { /* * Input width and height are irrelevant as they will be defined @@ -2209,13 +2286,130 @@ static const struct v4l2_m2m_ops m2m_ops .job_abort = job_abort, }; +/* Size of the array to provide to the VPU when asking for the list of supported + * formats. + * The ISP component currently advertises 33 input formats, so add a small + * overhead on that. + */ +#define MAX_SUPPORTED_ENCODINGS 40 + +/* Populate dev->supported_fmts with the formats supported by those ports. */ +static int bcm2835_codec_get_supported_fmts(struct bcm2835_codec_dev *dev) +{ + struct bcm2835_codec_fmt *list; + struct vchiq_mmal_component *component; + u32 fourccs[MAX_SUPPORTED_ENCODINGS]; + u32 param_size = sizeof(fourccs); + unsigned int i, j, num_encodings; + int ret; + + ret = vchiq_mmal_component_init(dev->instance, + dev->decode ? + "ril.video_decode" : + "ril.video_encode", + &component); + if (ret < 0) { + v4l2_err(&dev->v4l2_dev, "%s: failed to create component\n", + __func__); + return -ENOMEM; + } + + ret = vchiq_mmal_port_parameter_get(dev->instance, + &component->input[0], + MMAL_PARAMETER_SUPPORTED_ENCODINGS, + &fourccs, + &param_size); + + if (ret) { + if (ret == MMAL_MSG_STATUS_ENOSPC) { + v4l2_err(&dev->v4l2_dev, "%s: port has more encoding than we provided space for. Some are dropped.\n", + __func__); + num_encodings = MAX_SUPPORTED_ENCODINGS; + } else { + v4l2_err(&dev->v4l2_dev, "%s: get_param ret %u.\n", + __func__, ret); + ret = -EINVAL; + goto destroy_component; + } + } else { + num_encodings = param_size / sizeof(u32); + } + + /* Assume at this stage that all encodings will be supported in V4L2. + * Any that aren't supported will waste a very small amount of memory. + */ + list = devm_kzalloc(&dev->pdev->dev, + sizeof(struct bcm2835_codec_fmt) * num_encodings, + GFP_KERNEL); + if (!list) { + ret = -ENOMEM; + goto destroy_component; + } + dev->supported_fmts[0].list = list; + + for (i = 0, j = 0; i < num_encodings; i++) { + const struct bcm2835_codec_fmt *fmt = get_fmt(fourccs[i]); + + if (fmt) { + list[j] = *fmt; + j++; + } + } + dev->supported_fmts[0].num_entries = j; + + param_size = sizeof(fourccs); + ret = vchiq_mmal_port_parameter_get(dev->instance, + &component->output[0], + MMAL_PARAMETER_SUPPORTED_ENCODINGS, + &fourccs, + &param_size); + + if (ret) { + if (ret == MMAL_MSG_STATUS_ENOSPC) { + v4l2_err(&dev->v4l2_dev, "%s: port has more encoding than we provided space for. Some are dropped.\n", + __func__); + num_encodings = MAX_SUPPORTED_ENCODINGS; + } else { + ret = -EINVAL; + goto destroy_component; + } + } else { + num_encodings = param_size / sizeof(u32); + } + /* Assume at this stage that all encodings will be supported in V4L2. */ + list = devm_kzalloc(&dev->pdev->dev, + sizeof(struct bcm2835_codec_fmt) * num_encodings, + GFP_KERNEL); + if (!list) { + ret = -ENOMEM; + goto destroy_component; + } + dev->supported_fmts[1].list = list; + + for (i = 0, j = 0; i < num_encodings; i++) { + const struct bcm2835_codec_fmt *fmt = get_fmt(fourccs[i]); + + if (fmt) { + list[j] = *fmt; + j++; + } + } + dev->supported_fmts[1].num_entries = j; + + ret = 0; + +destroy_component: + vchiq_mmal_component_finalise(dev->instance, component); + + return ret; +} + static int bcm2835_codec_create(struct platform_device *pdev, struct bcm2835_codec_dev **new_dev, bool decode) { struct bcm2835_codec_dev *dev; struct video_device *vfd; - struct vchiq_mmal_instance *instance = NULL; int video_nr; int ret; @@ -2227,10 +2421,18 @@ static int bcm2835_codec_create(struct p dev->decode = decode; - ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev); + ret = vchiq_mmal_init(&dev->instance); if (ret) return ret; + ret = bcm2835_codec_get_supported_fmts(dev); + if (ret) + goto vchiq_finalise; + + ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev); + if (ret) + goto vchiq_finalise; + atomic_set(&dev->num_inst, 0); mutex_init(&dev->dev_mutex); @@ -2270,12 +2472,7 @@ static int bcm2835_codec_create(struct p goto err_m2m; } - ret = vchiq_mmal_init(&instance); - if (ret < 0) - goto err_m2m; - dev->instance = instance; - - v4l2_info(&dev->v4l2_dev, "Loaded V4L2 %s codec\n", + v4l2_info(&dev->v4l2_dev, "Loaded V4L2 %s\n", dev->decode ? "decode" : "encode"); return 0; @@ -2284,7 +2481,8 @@ err_m2m: video_unregister_device(&dev->vfd); unreg_dev: v4l2_device_unregister(&dev->v4l2_dev); - +vchiq_finalise: + vchiq_mmal_finalise(dev->instance); return ret; } @@ -2297,6 +2495,7 @@ static int bcm2835_codec_destroy(struct v4l2_m2m_release(dev->m2m_dev); video_unregister_device(&dev->vfd); v4l2_device_unregister(&dev->v4l2_dev); + vchiq_mmal_finalise(dev->instance); return 0; }