zeroplus: Use message logging helpers.
[sigrok:libsigrok.git] / hardware / zeroplus-logic-cube / analyzer.c
1 /*
2  * This file is part of the sigrok project.
3  *
4  * Copyright (C) 2010 Sven Peter <sven@fail0verflow.com>
5  * Copyright (C) 2010 Haxx Enterprises <bushing@gmail.com>
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or
9  * without modification, are permitted provided that the following
10  * conditions are met:
11  *
12  * * Redistributions of source code must retain the above copyright notice,
13  *   this list of conditions and the following disclaimer.
14  *
15  * * Redistributions in binary form must reproduce the above copyright notice,
16  *   this list of conditions and the following disclaimer in the documentation
17  *   and/or other materials provided with the distribution.
18  *
19  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
23  *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
29  *  THE POSSIBILITY OF SUCH DAMAGE.
30  */
31
32 #include <assert.h>
33 #include "libsigrok.h"
34 #include "libsigrok-internal.h"
35 #include "analyzer.h"
36 #include "gl_usb.h"
37 #include "protocol.h"
38
39 enum {
40         HARD_DATA_CHECK_SUM             = 0x00,
41         PASS_WORD,
42
43         DEV_ID0                         = 0x10,
44         DEV_ID1,
45
46         START_STATUS                    = 0x20,
47         DEV_STATUS                      = 0x21,
48         FREQUENCY_REG0                  = 0x30,
49         FREQUENCY_REG1,
50         FREQUENCY_REG2,
51         FREQUENCY_REG3,
52         FREQUENCY_REG4,
53         MEMORY_LENGTH,
54         CLOCK_SOURCE,
55
56         TRIGGER_STATUS0                 = 0x40,
57         TRIGGER_STATUS1,
58         TRIGGER_STATUS2,
59         TRIGGER_STATUS3,
60         TRIGGER_STATUS4,
61         TRIGGER_STATUS5,
62         TRIGGER_STATUS6,
63         TRIGGER_STATUS7,
64         TRIGGER_STATUS8,
65
66         TRIGGER_COUNT0                  = 0x50,
67         TRIGGER_COUNT1,
68
69         TRIGGER_LEVEL0                  = 0x55,
70         TRIGGER_LEVEL1,
71         TRIGGER_LEVEL2,
72         TRIGGER_LEVEL3,
73
74         RAMSIZE_TRIGGERBAR_ADDRESS0     = 0x60,
75         RAMSIZE_TRIGGERBAR_ADDRESS1,
76         RAMSIZE_TRIGGERBAR_ADDRESS2,
77         TRIGGERBAR_ADDRESS0,
78         TRIGGERBAR_ADDRESS1,
79         TRIGGERBAR_ADDRESS2,
80         DONT_CARE_TRIGGERBAR,
81
82         FILTER_ENABLE                   = 0x70,
83         FILTER_STATUS,
84
85         ENABLE_DELAY_TIME0              = 0x7a,
86         ENABLE_DELAY_TIME1,
87
88         ENABLE_INSERT_DATA0             = 0x80,
89         ENABLE_INSERT_DATA1,
90         ENABLE_INSERT_DATA2,
91         ENABLE_INSERT_DATA3,
92         COMPRESSION_TYPE0,
93         COMPRESSION_TYPE1,
94
95         TRIGGER_ADDRESS0                = 0x90,
96         TRIGGER_ADDRESS1,
97         TRIGGER_ADDRESS2,
98
99         NOW_ADDRESS0                    = 0x96,
100         NOW_ADDRESS1,
101         NOW_ADDRESS2,
102
103         STOP_ADDRESS0                   = 0x9b,
104         STOP_ADDRESS1,
105         STOP_ADDRESS2,
106
107         READ_RAM_STATUS                 = 0xa0,
108 };
109
110 static int g_trigger_status[9] = { 0 };
111 static int g_trigger_count = 1;
112 static int g_filter_status[8] = { 0 };
113 static int g_filter_enable = 0;
114
115 static int g_freq_value = 1;
116 static int g_freq_scale = FREQ_SCALE_MHZ;
117 static int g_memory_size = MEMORY_SIZE_8K;
118 static int g_ramsize_triggerbar_addr = 2 * 1024;
119 static int g_triggerbar_addr = 0;
120 static int g_compression = COMPRESSION_NONE;
121
122 /* Maybe unk specifies an "endpoint" or "register" of sorts. */
123 static int analyzer_write_status(libusb_device_handle *devh, unsigned char unk,
124                                  unsigned char flags)
125 {
126         assert(unk <= 3);
127         return gl_reg_write(devh, START_STATUS, unk << 6 | flags);
128 }
129
130 #if 0
131 static int __analyzer_set_freq(libusb_device_handle *devh, int freq, int scale)
132 {
133         int reg0 = 0, divisor = 0, reg2 = 0;
134
135         switch (scale) {
136         case FREQ_SCALE_MHZ: /* MHz */
137                 if (freq >= 100 && freq <= 200) {
138                         reg0 = freq * 0.1;
139                         divisor = 1;
140                         reg2 = 0;
141                         break;
142                 }
143                 if (freq >= 50 && freq < 100) {
144                         reg0 = freq * 0.2;
145                         divisor = 2;
146                         reg2 = 0;
147                         break;
148                 }
149                 if (freq >= 10 && freq < 50) {
150                         if (freq == 25) {
151                                 reg0 = 25;
152                                 divisor = 5;
153                                 reg2 = 1;
154                                 break;
155                         } else {
156                                 reg0 = freq * 0.5;
157                                 divisor = 5;
158                                 reg2 = 1;
159                                 break;
160                         }
161                 }
162                 if (freq >= 2 && freq < 10) {
163                         divisor = 5;
164                         reg0 = freq * 2;
165                         reg2 = 2;
166                         break;
167                 }
168                 if (freq == 1) {
169                         divisor = 5;
170                         reg2 = 16;
171                         reg0 = 5;
172                         break;
173                 }
174                 divisor = 5;
175                 reg0 = 5;
176                 reg2 = 64;
177                 break;
178         case FREQ_SCALE_HZ: /* Hz */
179                 if (freq >= 500 && freq < 1000) {
180                         reg0 = freq * 0.01;
181                         divisor = 10;
182                         reg2 = 64;
183                         break;
184                 }
185                 if (freq >= 300 && freq < 500) {
186                         reg0 = freq * 0.005 * 8;
187                         divisor = 5;
188                         reg2 = 67;
189                         break;
190                 }
191                 if (freq >= 100 && freq < 300) {
192                         reg0 = freq * 0.005 * 16;
193                         divisor = 5;
194                         reg2 = 68;
195                         break;
196                 }
197                 divisor = 5;
198                 reg0 = 5;
199                 reg2 = 64;
200                 break;
201         case FREQ_SCALE_KHZ: /* kHz */
202                 if (freq >= 500 && freq < 1000) {
203                         reg0 = freq * 0.01;
204                         divisor = 5;
205                         reg2 = 17;
206                         break;
207                 }
208                 if (freq >= 100 && freq < 500) {
209                         reg0 = freq * 0.05;
210                         divisor = 5;
211                         reg2 = 32;
212                         break;
213                 }
214                 if (freq >= 50 && freq < 100) {
215                         reg0 = freq * 0.1;
216                         divisor = 5;
217                         reg2 = 33;
218                         break;
219                 }
220                 if (freq >= 10 && freq < 50) {
221                         if (freq == 25) {
222                                 reg0 = 25;
223                                 divisor = 5;
224                                 reg2 = 49;
225                                 break;
226                         }
227                         reg0 = freq * 0.5;
228                         divisor = 5;
229                         reg2 = 48;
230                         break;
231                 }
232                 if (freq >= 2 && freq < 10) {
233                         divisor = 5;
234                         reg0 = freq * 2;
235                         reg2 = 50;
236                         break;
237                 }
238                 divisor = 5;
239                 reg0 = 5;
240                 reg2 = 64;
241                 break;
242         default:
243                 divisor = 5;
244                 reg0 = 5;
245                 reg2 = 64;
246                 break;
247         }
248
249         sr_dbg("Setting samplerate regs (freq=%d, scale=%d): "
250                "reg0: %d, reg1: %d, reg2: %d, reg3: %d.",
251                freq, scale, divisor, reg0, 0x02, reg2);
252
253         if (gl_reg_write(devh, FREQUENCY_REG0, divisor) < 0)
254                 return -1; /* Divisor maybe? */
255
256         if (gl_reg_write(devh, FREQUENCY_REG1, reg0) < 0)
257                 return -1; /* 10 / 0.2 */
258
259         if (gl_reg_write(devh, FREQUENCY_REG2, 0x02) < 0)
260                 return -1; /* Always 2 */
261
262         if (gl_reg_write(devh, FREQUENCY_REG4, reg2) < 0)
263                 return -1;
264
265         return 0;
266 }
267 #endif
268
269 /*
270  * It seems that ...
271  *      FREQUENCT_REG0 - division factor (?)
272  *      FREQUENCT_REG1 - multiplication factor (?)
273  *      FREQUENCT_REG4 - clock selection (?)
274  *
275  *      clock selection
276  *      0  10MHz  16   1MHz  32 100kHz  48  10kHz  64   1kHz
277  *      1   5MHz  17 500kHz  33  50kHz  49   5kHz  65  500Hz
278  *      2 2.5MHz   .          .         50 2.5kHz  66  250Hz
279  *      .          .          .          .         67  125Hz
280  *      .          .          .          .         68 62.5Hz
281  */
282 static int __analyzer_set_freq(libusb_device_handle *devh, int freq, int scale)
283 {
284         struct freq_factor {
285                 int freq;
286                 int scale;
287                 int sel;
288                 int div;
289                 int mul;
290         };
291
292         static const struct freq_factor f[] = {
293                 { 200, FREQ_SCALE_MHZ,  0,  1, 20 },
294                 { 150, FREQ_SCALE_MHZ,  0,  1, 15 },
295                 { 100, FREQ_SCALE_MHZ,  0,  1, 10 },
296                 {  80, FREQ_SCALE_MHZ,  0,  2, 16 },
297                 {  50, FREQ_SCALE_MHZ,  0,  2, 10 },
298                 {  25, FREQ_SCALE_MHZ,  1,  5, 25 },
299                 {  10, FREQ_SCALE_MHZ,  1,  5, 10 },
300                 {   1, FREQ_SCALE_MHZ, 16,  5,  5 },
301                 { 800, FREQ_SCALE_KHZ, 17,  5,  8 },
302                 { 400, FREQ_SCALE_KHZ, 32,  5, 20 },
303                 { 200, FREQ_SCALE_KHZ, 32,  5, 10 },
304                 { 100, FREQ_SCALE_KHZ, 32,  5,  5 },
305                 {  50, FREQ_SCALE_KHZ, 33,  5,  5 },
306                 {  25, FREQ_SCALE_KHZ, 49,  5, 25 },
307                 {   5, FREQ_SCALE_KHZ, 50,  5, 10 },
308                 {   1, FREQ_SCALE_KHZ, 64,  5,  5 },
309                 { 500, FREQ_SCALE_HZ,  64, 10,  5 },
310                 { 100, FREQ_SCALE_HZ,  68,  5,  8 },
311                 {   0, 0,              0,   0,  0 }
312         };
313
314         int i;
315
316         for (i = 0; f[i].freq; i++) {
317                 if (scale == f[i].scale && freq == f[i].freq)
318                         break;
319         }
320         if (!f[i].freq)
321                 return -1;
322
323         sr_dbg("Setting samplerate regs (freq=%d, scale=%d): "
324                "reg0: %d, reg1: %d, reg2: %d, reg3: %d.",
325                freq, scale, f[i].div, f[i].mul, 0x02, f[i].sel);
326
327         if (gl_reg_write(devh, FREQUENCY_REG0, f[i].div) < 0)
328                 return -1;
329
330         if (gl_reg_write(devh, FREQUENCY_REG1, f[i].mul) < 0)
331                 return -1;
332
333         if (gl_reg_write(devh, FREQUENCY_REG2, 0x02) < 0)
334                 return -1;
335
336         if (gl_reg_write(devh, FREQUENCY_REG4, f[i].sel) < 0)
337                 return -1;
338
339         return 0;
340 }
341
342 static void __analyzer_set_ramsize_trigger_address(libusb_device_handle *devh,
343                                                    unsigned int address)
344 {
345         gl_reg_write(devh, RAMSIZE_TRIGGERBAR_ADDRESS0, (address >> 0) & 0xFF);
346         gl_reg_write(devh, RAMSIZE_TRIGGERBAR_ADDRESS1, (address >> 8) & 0xFF);
347         gl_reg_write(devh, RAMSIZE_TRIGGERBAR_ADDRESS2, (address >> 16) & 0xFF);
348 }
349
350 static void __analyzer_set_triggerbar_address(libusb_device_handle *devh,
351                                               unsigned int address)
352 {
353         gl_reg_write(devh, TRIGGERBAR_ADDRESS0, (address >> 0) & 0xFF);
354         gl_reg_write(devh, TRIGGERBAR_ADDRESS1, (address >> 8) & 0xFF);
355         gl_reg_write(devh, TRIGGERBAR_ADDRESS2, (address >> 16) & 0xFF);
356 }
357
358 static void __analyzer_set_compression(libusb_device_handle *devh,
359                                        unsigned int type)
360 {
361         gl_reg_write(devh, COMPRESSION_TYPE0, (type >> 0) & 0xFF);
362         gl_reg_write(devh, COMPRESSION_TYPE1, (type >> 8) & 0xFF);
363 }
364
365 static void __analyzer_set_trigger_count(libusb_device_handle *devh,
366                                          unsigned int count)
367 {
368         gl_reg_write(devh, TRIGGER_COUNT0, (count >> 0) & 0xFF);
369         gl_reg_write(devh, TRIGGER_COUNT1, (count >> 8) & 0xFF);
370 }
371
372 static void analyzer_write_enable_insert_data(libusb_device_handle *devh)
373 {
374         gl_reg_write(devh, ENABLE_INSERT_DATA0, 0x12);
375         gl_reg_write(devh, ENABLE_INSERT_DATA1, 0x34);
376         gl_reg_write(devh, ENABLE_INSERT_DATA2, 0x56);
377         gl_reg_write(devh, ENABLE_INSERT_DATA3, 0x78);
378 }
379
380 static void analyzer_set_filter(libusb_device_handle *devh)
381 {
382         int i;
383         gl_reg_write(devh, FILTER_ENABLE, g_filter_enable);
384         for (i = 0; i < 8; i++)
385                 gl_reg_write(devh, FILTER_STATUS + i, g_filter_status[i]);
386 }
387
388 SR_PRIV void analyzer_reset(libusb_device_handle *devh)
389 {
390         analyzer_write_status(devh, 3, STATUS_FLAG_NONE);       // reset device
391         analyzer_write_status(devh, 3, STATUS_FLAG_RESET);      // reset device
392 }
393
394 SR_PRIV void analyzer_initialize(libusb_device_handle *devh)
395 {
396         analyzer_write_status(devh, 1, STATUS_FLAG_NONE);
397         analyzer_write_status(devh, 1, STATUS_FLAG_INIT);
398         analyzer_write_status(devh, 1, STATUS_FLAG_NONE);
399 }
400
401 SR_PRIV void analyzer_wait(libusb_device_handle *devh, int set, int unset)
402 {
403         int status;
404
405         while (1) {
406                 status = gl_reg_read(devh, DEV_STATUS);
407                 if ((status & set) && ((status & unset) == 0))
408                         return;
409         }
410 }
411
412 SR_PRIV void analyzer_read_start(libusb_device_handle *devh)
413 {
414         int i;
415
416         analyzer_write_status(devh, 3, STATUS_FLAG_20 | STATUS_FLAG_READ);
417
418         for (i = 0; i < 8; i++)
419                 (void)gl_reg_read(devh, READ_RAM_STATUS);
420 }
421
422 SR_PRIV int analyzer_read_data(libusb_device_handle *devh, void *buffer,
423                        unsigned int size)
424 {
425         return gl_read_bulk(devh, buffer, size);
426 }
427
428 SR_PRIV void analyzer_read_stop(libusb_device_handle *devh)
429 {
430         analyzer_write_status(devh, 3, STATUS_FLAG_20);
431         analyzer_write_status(devh, 3, STATUS_FLAG_NONE);
432 }
433
434 SR_PRIV void analyzer_start(libusb_device_handle *devh)
435 {
436         analyzer_write_status(devh, 1, STATUS_FLAG_NONE);
437         analyzer_write_status(devh, 1, STATUS_FLAG_INIT);
438         analyzer_write_status(devh, 1, STATUS_FLAG_NONE);
439         analyzer_write_status(devh, 1, STATUS_FLAG_GO);
440 }
441
442 SR_PRIV void analyzer_configure(libusb_device_handle *devh)
443 {
444         int i;
445
446         /* Write_Start_Status */
447         analyzer_write_status(devh, 1, STATUS_FLAG_RESET);
448         analyzer_write_status(devh, 1, STATUS_FLAG_NONE);
449
450         /* Start_Config_Outside_Device ? */
451         analyzer_write_status(devh, 1, STATUS_FLAG_INIT);
452         analyzer_write_status(devh, 1, STATUS_FLAG_NONE);
453
454         /* SetData_To_Frequence_Reg */
455         __analyzer_set_freq(devh, g_freq_value, g_freq_scale);
456
457         /* SetMemory_Length */
458         gl_reg_write(devh, MEMORY_LENGTH, g_memory_size);
459
460         /* Sele_Inside_Outside_Clock */
461         gl_reg_write(devh, CLOCK_SOURCE, 0x03);
462
463         /* Set_Trigger_Status */
464         for (i = 0; i < 9; i++)
465                 gl_reg_write(devh, TRIGGER_STATUS0 + i, g_trigger_status[i]);
466
467         __analyzer_set_trigger_count(devh, g_trigger_count);
468
469         /* Set_Trigger_Level */
470         gl_reg_write(devh, TRIGGER_LEVEL0, 0x31);
471         gl_reg_write(devh, TRIGGER_LEVEL1, 0x31);
472         gl_reg_write(devh, TRIGGER_LEVEL2, 0x31);
473         gl_reg_write(devh, TRIGGER_LEVEL3, 0x31);
474
475         /* Size of actual memory >> 2 */
476         __analyzer_set_ramsize_trigger_address(devh, g_ramsize_triggerbar_addr);
477         __analyzer_set_triggerbar_address(devh, g_triggerbar_addr);
478
479         /* Set_Dont_Care_TriggerBar */
480         if (g_triggerbar_addr)
481                 gl_reg_write(devh, DONT_CARE_TRIGGERBAR, 0x00);
482         else
483                 gl_reg_write(devh, DONT_CARE_TRIGGERBAR, 0x01);
484
485         /* Enable_Status */
486         analyzer_set_filter(devh);
487
488         /* Set_Enable_Delay_Time */
489         gl_reg_write(devh, 0x7a, 0x00);
490         gl_reg_write(devh, 0x7b, 0x00);
491         analyzer_write_enable_insert_data(devh);
492         __analyzer_set_compression(devh, g_compression);
493 }
494
495 SR_PRIV void analyzer_add_trigger(int channel, int type)
496 {
497         switch (type) {
498         case TRIGGER_HIGH:
499                 g_trigger_status[channel / 4] |= 1 << (channel % 4 * 2);
500                 break;
501         case TRIGGER_LOW:
502                 g_trigger_status[channel / 4] |= 2 << (channel % 4 * 2);
503                 break;
504 #if 0
505         case TRIGGER_POSEDGE:
506                 g_trigger_status[8] = 0x40 | channel;
507                 break;
508         case TRIGGER_NEGEDGE:
509                 g_trigger_status[8] = 0x80 | channel;
510                 break;
511         case TRIGGER_ANYEDGE:
512                 g_trigger_status[8] = 0xc0 | channel;
513                 break;
514 #endif
515         default:
516                 break;
517         }
518 }
519
520 SR_PRIV void analyzer_add_filter(int channel, int type)
521 {
522         int i;
523
524         if (type != FILTER_HIGH && type != FILTER_LOW)
525                 return;
526         if ((channel & 0xf) >= 8)
527                 return;
528
529         if (channel & CHANNEL_A)
530                 i = 0;
531         else if (channel & CHANNEL_B)
532                 i = 2;
533         else if (channel & CHANNEL_C)
534                 i = 4;
535         else if (channel & CHANNEL_D)
536                 i = 6;
537         else
538                 return;
539
540         if ((channel & 0xf) >= 4) {
541                 i++;
542                 channel -= 4;
543         }
544
545         g_filter_status[i] |=
546             1 << ((2 * channel) + (type == FILTER_LOW ? 1 : 0));
547
548         g_filter_enable = 1;
549 }
550
551 SR_PRIV void analyzer_set_trigger_count(int count)
552 {
553         g_trigger_count = count;
554 }
555
556 SR_PRIV void analyzer_set_freq(int freq, int scale)
557 {
558         g_freq_value = freq;
559         g_freq_scale = scale;
560 }
561
562 SR_PRIV void analyzer_set_memory_size(unsigned int size)
563 {
564         g_memory_size = size;
565 }
566
567 SR_PRIV void analyzer_set_ramsize_trigger_address(unsigned int address)
568 {
569         g_ramsize_triggerbar_addr = address;
570 }
571
572 SR_PRIV void analyzer_set_triggerbar_address(unsigned int address)
573 {
574         g_triggerbar_addr = address;
575 }
576
577 SR_PRIV unsigned int analyzer_read_status(libusb_device_handle *devh)
578 {
579         return gl_reg_read(devh, DEV_STATUS);
580 }
581
582 SR_PRIV unsigned int analyzer_read_id(libusb_device_handle *devh)
583 {
584         return gl_reg_read(devh, DEV_ID1) << 8 | gl_reg_read(devh, DEV_ID0);
585 }
586
587 SR_PRIV unsigned int analyzer_get_stop_address(libusb_device_handle *devh)
588 {
589         return gl_reg_read(devh, STOP_ADDRESS2) << 16 | gl_reg_read(devh,
590                         STOP_ADDRESS1) << 8 | gl_reg_read(devh, STOP_ADDRESS0);
591 }
592
593 SR_PRIV unsigned int analyzer_get_now_address(libusb_device_handle *devh)
594 {
595         return gl_reg_read(devh, NOW_ADDRESS2) << 16 | gl_reg_read(devh,
596                         NOW_ADDRESS1) << 8 | gl_reg_read(devh, NOW_ADDRESS0);
597 }
598
599 SR_PRIV unsigned int analyzer_get_trigger_address(libusb_device_handle *devh)
600 {
601         return gl_reg_read(devh, TRIGGER_ADDRESS2) << 16 | gl_reg_read(devh,
602                 TRIGGER_ADDRESS1) << 8 | gl_reg_read(devh, TRIGGER_ADDRESS0);
603 }
604
605 SR_PRIV void analyzer_set_compression(unsigned int type)
606 {
607         g_compression = type;
608 }
609
610 SR_PRIV void analyzer_wait_button(libusb_device_handle *devh)
611 {
612         analyzer_wait(devh, STATUS_BUTTON_PRESSED, 0);
613 }
614
615 SR_PRIV void analyzer_wait_data(libusb_device_handle *devh)
616 {
617         analyzer_wait(devh, STATUS_READY | 8, STATUS_BUSY);
618 }
619
620 SR_PRIV int analyzer_decompress(void *input, unsigned int input_len,
621                                 void *output, unsigned int output_len)
622 {
623         unsigned char *in = input;
624         unsigned char *out = output;
625         unsigned int A, B, C, count;
626         unsigned int written = 0;
627
628         while (input_len > 0) {
629                 A = *in++;
630                 B = *in++;
631                 C = *in++;
632                 count = (*in++) + 1;
633
634                 if (count > output_len)
635                         count = output_len;
636                 output_len -= count;
637                 written += count;
638
639                 while (count--) {
640                         *out++ = A;
641                         *out++ = B;
642                         *out++ = C;
643                         *out++ = 0; /* Channel D */
644                 }
645
646                 input_len -= 4;
647                 if (output_len == 0)
648                         break;
649         }
650
651         return written;
652 }